public void Rotate(Quaternionf q)
{
Debug.Assert(rotation.w != 0); // catch un-initialized quaternions
rotation = q * rotation;
}
/// <summary> Map quaternion *from* local frame coordinates into "world" coordinates </summary>
public Quaternionf FromFrame(ref Quaternionf q)
{
return(this.rotation * q);
}
public Frame3f(Vector3f origin, Quaternionf orientation)
{
rotation = orientation;
this.origin = origin;
}
public Frame3f(Vector3d origin)
{
rotation = Quaternionf.Identity;
this.origin = (Vector3f)origin;
}
public Frame3f(Vector3d origin, Vector3d setZ)
{
rotation = Quaternionf.FromTo(Vector3f.AxisZ, (Vector3f)setZ);
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 void AlignAxis(int nAxis, Vector3f vTo)
{
Quaternionf rot = Quaternionf.FromTo(GetAxis(nAxis), vTo);
Rotate(rot);
}
public Vector3f ToFrameP(Vector3f v)
{
v = v - this.origin;
v = Quaternionf.Inverse(this.rotation) * v;
return(v);
}
public Vector3f ToFrameV(Vector3f v)
{
return(Quaternionf.Inverse(this.rotation) * v);
}
/// <summary> Map quaternion *from* local frame coordinates into "world" coordinates </summary>
public Quaternionf FromFrame(Quaternionf q)
{
return this.rotation * q;
}
public Frame3f(Vector3f origin, Vector3f x, Vector3f y, Vector3f z)
{
this.origin = origin;
var m = new Matrix3f(x, y, z, false);
this.rotation = m.ToQuaternion();
}
///<summary> Map quaternion *into* local coordinates of Frame </summary>
public Quaternionf ToFrame(Quaternionf q)
{
return Quaternionf.Inverse(this.rotation) * q;
}
public void RotateAround(Vector3f point, Quaternionf q)
{
Vector3f dv = q * (origin - point);
rotation = q * rotation;
origin = point + dv;
}
public Frame3f Rotated(Quaternionf q)
{
Debug.Assert(rotation.w != 0);
return(new Frame3f(this.origin, q * this.rotation));
}
public void Rotate(Quaternionf q)
{
rotation = q * rotation;
}
// for checking things...
public static void print_compare(string prefix, Quaternion q1, g3.Quaternionf q2)
{
double dErr = Math.Abs(q1.x - q2.x) + Math.Abs(q1.y - q2.y) + Math.Abs(q1.z - q2.z) + Math.Abs(q1.w - q2.w);
DebugUtil.Log(2, "{0} {1} {2} err {3}", prefix, q1.ToString("F8"), q2.ToString(), dErr);
}
public Frame3f Rotated(Quaternionf q)
{
return(new Frame3f(this.origin, q * this.rotation));
}
public float Dot(Quaternionf q2)
{
return(x * q2.x + y * q2.y + z * q2.z + w * q2.w);
}
public Frame3f RotatedAround(Vector3f point, Quaternionf q)
{
Vector3f dv = q * (this.origin - point);
return(new Frame3f(point + dv, q * this.rotation));
}
public static Quaternionf Inverse(Quaternionf q)
{
return(q.Inverse());
}
// M.Baske
public void AlignAxis(Vector3f axis, Vector3f vTo)
{
Quaternionf rot = Quaternionf.FromTo(axis, vTo);
Rotate(rot);
}
public Quaternionf(Quaternionf q2)
{
x = q2.x; y = q2.y; z = q2.z; w = q2.w;
}
public Frame3f(Vector3f origin, Vector3f setZ)
{
rotation = Quaternionf.FromTo(Vector3f.AxisZ, setZ);
this.origin = origin;
}
public static Quaternionf Slerp(Quaternionf p, Quaternionf q, float t)
{
return(new Quaternionf(p, q, t));
}
///<summary> Map quaternion *into* local coordinates of Frame </summary>
public Quaternionf ToFrame(ref Quaternionf q)
{
return(Quaternionf.Inverse(this.rotation) * q);
}
public Quaternionf(Quaternionf p, Quaternionf q, float t)
{
x = y = z = 0; w = 1;
SetToSlerp(p, q, t);
}
/// <summary>
/// Interpolate between two frames - Lerp for origin, Slerp for rotation
/// </summary>
public static Frame3f Interpolate(Frame3f f1, Frame3f f2, float t)
{
return(new Frame3f(
Vector3f.Lerp(f1.origin, f2.origin, t),
Quaternionf.Slerp(f1.rotation, f2.rotation, t)));
}
public Frame3f(Frame3f copy)
{
this.rotation = copy.rotation;
this.origin = copy.origin;
}
public static Frame3f Rotate(Frame3f f, Vector3d origin, Quaternionf rotation)
{
f.Rotate(rotation);
f.Origin = (Vector3f)Rotate(f.Origin, origin, rotation);
return(f);
}
public static Quaternionf FromToConstrained(Vector3f vFrom, Vector3f vTo, Vector3f vAround)
{
float fAngle = MathUtil.PlaneAngleSignedD(vFrom, vTo, vAround);
return(Quaternionf.AxisAngleD(vAround, fAngle));
}
