public float Dot(Quaternionf q2)
{
return(x * q2.x + y * q2.y + z * q2.z + w * q2.w);
}
public static Quaternionf Inverse(Quaternionf q)
{
return(q.Inverse());
}
public static Frame3f Interpolate(Frame3f f1, Frame3f f2, float alpha)
{
return(new Frame3f(
Vector3F.Lerp(f1.origin, f2.origin, alpha),
Quaternionf.Slerp(f1.rotation, f2.rotation, alpha)));
}
public Frame3f(Vector3F origin, Quaternionf orientation)
{
rotation = orientation;
this.origin = origin;
}
public void Rotate(Quaternionf q)
{
Debug.Assert(rotation.w != 0); // catch un-initialized quaternions
rotation = q * rotation;
}
public Frame3f(Vector3D origin, Vector3D setZ)
{
rotation = Quaternionf.FromTo(Vector3F.AxisZ, (Vector3F)setZ);
this.origin = (Vector3F)origin;
}
///<summary> Map quaternion *into* local coordinates of Frame </summary>
public Quaternionf ToFrame(Quaternionf q)
{
return(Quaternionf.Inverse(this.rotation) * q);
}
public static Quaternionf Slerp(Quaternionf p, Quaternionf q, float t)
{
return(new Quaternionf(p, q, t));
}
///<summary> Map point *into* local coordinates of Frame </summary>
public Vector3D ToFrameP(Vector3D v)
{
v = v - this.origin;
v = Quaternionf.Inverse(this.rotation) * v;
return(v);
}
///<summary> Map vector *into* local coordinates of Frame </summary>
public Vector3D ToFrameV(Vector3D v)
{
return(Quaternionf.Inverse(this.rotation) * v);
}
public Frame3f(Vector3D origin)
{
rotation = Quaternionf.Identity;
this.origin = (Vector3F)origin;
}
/// <summary>
/// this rotates the frame around its own axes, rather than around the world axes,
/// which is what Rotate() does. So, RotateAroundAxis(AxisAngleD(Z,180)) is equivalent
/// to Rotate(AxisAngleD(My_AxisZ,180)).
/// </summary>
public void RotateAroundAxes(Quaternionf q)
{
rotation = rotation * q;
}
public Frame3f Rotated(Quaternionf q)
{
Debug.Assert(rotation.w != 0);
return(new Frame3f(this.origin, q * this.rotation));
}
public Quaternionf(Quaternionf q2)
{
x = q2.x; y = q2.y; z = q2.z; w = q2.w;
}
/// <summary> Map quaternion *from* local frame coordinates into "world" coordinates </summary>
public Quaternionf FromFrame(Quaternionf q)
{
return(this.rotation * q);
}
public static Quaternionf FromToConstrained(Vector3F vFrom, Vector3F vTo, Vector3F vAround)
{
float fAngle = MathUtil.PlaneAngleSignedD(vFrom, vTo, vAround);
return(Quaternionf.AxisAngleD(vAround, fAngle));
}
public Frame3f(Vector3F origin, Vector3F setZ)
{
rotation = Quaternionf.FromTo(Vector3F.AxisZ, setZ);
this.origin = origin;
}
public Quaternionf(Quaternionf p, Quaternionf q, float t)
{
x = y = z = 0; w = 1;
SetToSlerp(p, q, t);
}
public Frame3f(Frame3f copy)
{
this.rotation = copy.rotation;
this.origin = copy.origin;
}
