/// <summary>
/// Test if this RotationMatrix is approximately equal to another.
/// </summary>
/// <param name="other"></param>
/// <returns></returns>
public bool IsSimilar(RotationMatrix other)
{
return(Math.Abs(this.R[0] - other.R[0]) < EPSILON2 &&
Math.Abs(this.R[1] - other.R[1]) < EPSILON2 &&
Math.Abs(this.R[2] - other.R[2]) < EPSILON2 &&
Math.Abs(this.R[3] - other.R[3]) < EPSILON2 &&
Math.Abs(this.R[4] - other.R[4]) < EPSILON2 &&
Math.Abs(this.R[5] - other.R[5]) < EPSILON2 &&
Math.Abs(this.R[6] - other.R[6]) < EPSILON2 &&
Math.Abs(this.R[7] - other.R[7]) < EPSILON2 &&
Math.Abs(this.R[8] - other.R[8]) < EPSILON2);
}
/// <summary>
/// Is this
/// </summary>
/// <returns></returns>
public bool IsOrthogonal()
{
// Orthogonal matrices satisfy that:
// Q * Qt = Qt * Q = I;
// (the matrix multiplied by its transpose yields the identity matrix)
// As a consequence, it also holds that the transpose of an orthogonal matrix equals its inverse:
// Qt = Q^-1
RotationMatrix t = new RotationMatrix(this);
t.Transpose();
RotationMatrix ident = RotationMatrix.Multiply(this, t);
return(ident.IsIdentity() && Math.Abs(this.Determinant() - 1) < EPSILON2);
}
/// <summary>
/// Multiplies two rotation matrices.
/// </summary>
/// <param name="m1"></param>
/// <param name="m2"></param>
/// <returns></returns>
public static RotationMatrix Multiply(RotationMatrix m1, RotationMatrix m2)
{
RotationMatrix m = new RotationMatrix();
m.R[0] = m1.R[0] * m2.R[0] + m1.R[1] * m2.R[3] + m1.R[2] * m2.R[6];
m.R[1] = m1.R[0] * m2.R[1] + m1.R[1] * m2.R[4] + m1.R[2] * m2.R[7];
m.R[2] = m1.R[0] * m2.R[2] + m1.R[1] * m2.R[5] + m1.R[2] * m2.R[8];
m.R[3] = m1.R[3] * m2.R[0] + m1.R[4] * m2.R[3] + m1.R[5] * m2.R[6];
m.R[4] = m1.R[3] * m2.R[1] + m1.R[4] * m2.R[4] + m1.R[5] * m2.R[7];
m.R[5] = m1.R[3] * m2.R[2] + m1.R[4] * m2.R[5] + m1.R[5] * m2.R[8];
m.R[6] = m1.R[6] * m2.R[0] + m1.R[7] * m2.R[3] + m1.R[8] * m2.R[6];
m.R[7] = m1.R[6] * m2.R[1] + m1.R[7] * m2.R[4] + m1.R[8] * m2.R[7];
m.R[8] = m1.R[6] * m2.R[2] + m1.R[7] * m2.R[5] + m1.R[8] * m2.R[8];
return(m);
}
/// <summary>
/// Returns a Rotation Matrix representation of this Axis Angle.
/// Please note that rotation matrices represent rotations in orthonormalized coordinates,
/// so that additional Axis Angle information such as overturns (angles outside [0, 180])
/// will get lost, and rotation axis might be flipped.
/// If this Axis Angle represents no effective rotation, the identity matrix will be returned.
/// </summary>
/// <returns></returns>
public RotationMatrix ToRotationMatrix()
{
// Some sanity: if this AA represents no rotation, return identity matrix
if (this.IsZero())
{
return(new RotationMatrix());
}
// Based on http://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToMatrix/index.htm
// This conversion assumes the rotation vector is normalized.
double ang = this.Angle * TO_RADS;
double c = Math.Cos(ang);
double s = Math.Sin(ang);
double t = 1 - c;
RotationMatrix m = new RotationMatrix();
m.m00 = c + t * this.Axis.X * this.Axis.X;
m.m11 = c + t * this.Axis.Y * this.Axis.Y;
m.m22 = c + t * this.Axis.Z * this.Axis.Z;
double t1 = t * this.Axis.X * this.Axis.Y;
double t2 = s * this.Axis.Z;
m.m10 = t1 + t2;
m.m01 = t1 - t2;
t1 = t * this.Axis.X * this.Axis.Z;
t2 = s * this.Axis.Y;
m.m20 = t1 - t2;
m.m02 = t1 + t2;
t1 = t * this.Axis.Y * this.Axis.Z;
t2 = s * this.Axis.X;
m.m21 = t1 + t2;
m.m12 = t1 - t2;
return(m);
}
/// <summary>
/// Create a Rotation object representing no rotation.
/// </summary>
public Orientation()
{
this.Q = new Quaternion();
this.RM = new RotationMatrix();
}
/// <summary>
/// Create an Orientation object from a Quaternion representation.
/// </summary>
/// <param name="q"></param>
/// <returns></returns>
internal Orientation(Quaternion q)
{
this.Q = new Quaternion(q);
this.RM = this.Q.ToRotationMatrix();
}
/// <summary>
/// Create a new Orientation object from the main X and Y axes.
/// This constructor will create the best-fit orthogonal coordinate system
/// respecting the direction of the X vector and the plane formed with the Y vector.
/// The Z vector will be normal to this planes, and all vectors will be unitized.
/// </summary>
/// <param name="x0"></param>
/// <param name="x1"></param>
/// <param name="x2"></param>
/// <param name="y0"></param>
/// <param name="y1"></param>
/// <param name="y2"></param>
public Orientation(double x0, double x1, double x2, double y0, double y1, double y2)
{
this.RM = new RotationMatrix(x0, x1, x2, y0, y1, y2);
this.Q = this.RM.ToQuaternion();
}
/// <summary>
/// Create a 3x3 Rotation Matrix as a shallow copy of another.
/// </summary>
/// <param name="rotationMatrix"></param>
public RotationMatrix(RotationMatrix rotationMatrix)
{
this.Initialize(rotationMatrix.R[0], rotationMatrix.R[1], rotationMatrix.R[2],
rotationMatrix.R[3], rotationMatrix.R[4], rotationMatrix.R[5],
rotationMatrix.R[6], rotationMatrix.R[7], rotationMatrix.R[8], false); // let's assume the RotationMatrix was already orthogonal...
}
