/// <summary>
/// String representation of an object in reference coordinate system.
/// </summary>
public string ToString(Coord3d coord)
{
if (coord == null)
{
coord = Coord3d.GlobalCS;
}
Quaternion2 q = this.ConvertTo(coord);
return(string.Format("Quaternion2 -> ({0,10:g5}, {1,10:g5}, {2,10:g5}, {3,10:g5})", q.W, q.X, q.Y, q.Z));
}
/// <summary>
/// Determines whether two objects are equal.
/// </summary>
public override bool Equals(object obj)
{
if (obj == null || (!object.ReferenceEquals(this.GetType(), obj.GetType())))
{
return(false);
}
Quaternion2 q = (Quaternion2)obj;
// No check for absolute tolerance since this is unit Quaternion2
return((this - q).Norm <= GeometRi3D.Tolerance);
}
public Quaternion2 Subtract(Quaternion2 q)
{
if ((this._coord != q._coord))
{
q = q.ConvertTo(this._coord);
}
Quaternion2 m = new Quaternion2(this.Coord);
m.W = _w - q.W;
m.X = _x - q.X;
m.Y = _y - q.Y;
m.Z = _z - q.Z;
return(m);
}
public Quaternion2 Add(Quaternion2 q)
{
if ((this._coord != q._coord))
{
q = q.ConvertTo(this._coord);
}
Quaternion2 m = new Quaternion2(this.Coord);
m.W = _w + q.W;
m.X = _x + q.X;
m.Y = _y + q.Y;
m.Z = _z + q.Z;
return(m);
}
public Quaternion2 Mult(Quaternion2 q)
{
if ((this._coord != q._coord))
{
q = q.ConvertTo(this._coord);
}
Quaternion2 m = new Quaternion2(this.Coord);
m.W = W * q.W - X * q.X - Y * q.Y - Z * q.Z;
m.X = W * q.X + X * q.W + Y * q.Z - Z * q.Y;
m.Y = W * q.Y - X * q.Z + Y * q.W + Z * q.X;
m.Z = W * q.Z + X * q.Y - Y * q.X + Z * q.W;
return(m);
}
/// <summary>
/// Spherical linear interpolation of two rotations.
/// </summary>
/// <param name="q1">Initial rotation</param>
/// <param name="q2">Final rotation</param>
/// <param name="t">Interpolation parameter within range [0, 1]</param>
public static Quaternion2 SLERP(Quaternion2 q1, Quaternion2 q2, double t)
{
// Algorithm from https://en.wikipedia.org/wiki/Slerp
Quaternion2 qq1 = q1.Copy().Normalized;
Quaternion2 qq2 = q2.Copy().Normalized;
if (qq2.Coord != qq1.Coord)
{
qq2 = qq2.ConvertTo(qq1.Coord);
}
double dot = qq1.W * qq2.W + qq1.X * qq2.X + qq1.Y * qq2.Y + qq1.Z * qq2.Z;
const double threshold = 0.9995;
if (Abs(dot) > threshold)
{
// Using linear interpolation if two rotations are close
Quaternion2 res = qq1 + t * (qq2 - qq1);
res.Normalize();
return(res);
}
//Make sure to choose shortest path
if (dot < 0.0)
{
qq2 = -qq2;
dot = -dot;
}
double theta_0 = Acos(dot);
double theta = theta_0 * t;
qq2 = qq2 - qq1 * dot;
qq2.Normalize();
return(qq1 * Cos(theta) + qq2 * Sin(theta));
}
/// <summary>
/// Spherical linear interpolation of two rotations.
/// </summary>
/// <param name="r1">Initial rotation</param>
/// <param name="r2">Final rotation</param>
/// <param name="t">Interpolation parameter within range [0, 1]</param>
public static Rotation SLERP(Rotation r1, Rotation r2, double t)
{
return(new Rotation(Quaternion2.SLERP(r1.ToQuaternion, r2.ToQuaternion, t)));
}
/// <summary>
/// Initializes rotation using quaternion.
/// </summary>
/// <param name="q"></param>
public Rotation(Quaternion2 q)
{
_r = q.ToRotationMatrix();
_coord = q.Coord;
}
