public void ToRotationMatrix(Vector3d[] /*3*/ rotColumn)
{
Matrix3x3d mat = this.ToRotationMatrix();
rotColumn[0] = new Vector3d(mat[0, 0], mat[1, 0], mat[2, 0]);
rotColumn[1] = new Vector3d(mat[0, 1], mat[1, 1], mat[2, 1]);
rotColumn[2] = new Vector3d(mat[0, 2], mat[1, 2], mat[2, 2]);
}
public static Matrix4x4d RotateQuat(double x, double y, double z, double w)
{
Matrix3x3d m = Matrix3x3dUtils.RotateQuat(x, y, z, w);
return(new Matrix4x4d(
m.M00, m.M01, m.M02, 0,
m.M10, m.M11, m.M12, 0,
m.M20, m.M21, m.M22, 0,
0, 0, 0, 1));
}
/// this conversion uses NASA standard aeroplane conventions as described on page:
/// http://www.euclideanspace.com/maths/geometry/rotations/euler/index.htm
/// Coordinate System: right hand
/// Positive angle: right hand
/// Order of euler angles: heading first, then attitude, then bank
/// matrix row column ordering:
/// [m00 m01 m02 0]
/// [m10 m11 m12 0]
/// [m20 m21 m22 0]
/// [0 0 0 1]
public static Matrix4x4d RotateEuler(double heading, double attitude, double bank)
{
Matrix3x3d m = Matrix3x3dUtils.RotateEuler(heading, attitude, bank);
return(new Matrix4x4d(
m.M00, m.M01, m.M02, 0,
m.M10, m.M11, m.M12, 0,
m.M20, m.M21, m.M22, 0,
0, 0, 0, 1));
}
/// <summary>
/// <see cref="http://adndevblog.typepad.com/autocad/2012/08/remove-scaling-from-transformation-matrix.html" />
/// </summary>
public static Matrix3x3d RemoveScaling(Matrix3x3d m, double scalingValue = 1)
{
Vector2d v0 = new Vector2d(m.M00, m.M01).Unit.Mul(scalingValue);
Vector2d v1 = new Vector2d(m.M10, m.M11).Unit.Mul(scalingValue);
return(new Matrix3x3d(
new[, ]
{
{ v0.X, v0.Y, m.M02 },
{ v1.X, v1.Y, m.M12 },
{ m.M20, m.M21, m.M22 }
}));
}
public static Quaternion FromRotationMatrix(Matrix3x3d rot)
{
// Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
// article "Quaternion Calculus and Fast Animation".
int[] next = { 1, 2, 0 };
double[] mTuple = new double[4];
double trace = rot[0, 0] + rot[1, 1] + rot[2, 2];
if (trace > 0.0)
{
// |w| > 1/2, may as well choose w > 1/2
double root = Math.Sqrt(trace + 1.0); // 2w
double root2 = 0.5 / root; // 1/(4w)
mTuple[0] = 0.5 * root;
mTuple[1] = (rot[2, 1] - rot[1, 2]) * root2;
mTuple[2] = (rot[0, 2] - rot[2, 0]) * root2;
mTuple[3] = (rot[1, 0] - rot[0, 1]) * root2;
}
else
{
// |w| <= 1/2
int i = 0;
if (rot[1, 1] > rot[0, 0])
{
i = 1;
}
if (rot[2, 2] > rot[i, i])
{
i = 2;
}
int j = next[i];
int k = next[j];
double root = Math.Sqrt(rot[i, i] - rot[j, j] - rot[k, k] + 1.0);
double root2 = 0.5 / root;
int[] quat = new int[] { 1, 2, 3 };
mTuple[quat[i]] = 0.5 * root;
mTuple[0] = (rot[k, j] - rot[j, k]) * root2;
mTuple[quat[j]] = (rot[j, i] + rot[i, j]) * root2;
mTuple[quat[k]] = (rot[k, i] + rot[i, k]) * root2;
}
return(new Quaternion(mTuple));
}
/// <summary>
/// Crea una transformacion de un rectangulo.
/// <code><![CDATA[
/// * po2
/// / * pd2 * pd1
/// / \ /
/// / ==> \ /
/// * po0 \ /
/// \ * pd0
/// \
/// \
/// *
/// po1
/// ]]></code>
/// </summary>
/// <param name="po0">Punto origen LB.</param>
/// <param name="po1">Punto origen RB.</param>
/// <param name="po2">Punto origen LT.</param>
/// <param name="pd0">Punto destino LB.</param>
/// <param name="pd1">Punto destino RB.</param>
/// <param name="pd2">Punto destino LT.</param>
/// <returns>Transformacion.</returns>
public static Matrix3x3d TransformRectangle(Point2d po0, Point2d po1, Point2d po2,
Point2d pd0, Point2d pd1, Point2d pd2)
{
//RECTANGLE2 rec = new RECTANGLE2(0, 0, 1, 1);
////////////////////////////////////////////////////////////////////////////////////////
//MATRIX m = TransformRectangle(rec, pd0, pd1, pd2);
Matrix3x3d m;
{
double m00 = (pd1.X - pd0.X);
double m10 = (pd1.Y - pd0.Y);
double m01 = (pd2.X - pd0.X);
double m11 = (pd2.Y - pd0.Y);
m = new Matrix3x3d(
m00, m01, pd0.X,
m10, m11, pd0.Y,
0, 0, 1);
}
////////////////////////////////////////////////////////////////////////////////////////
//MATRIX aux = TransformRectangle(rec, po0, po1, po2);
Matrix3x3d aux;
{
double m00 = (po1.X - po0.X);
double m10 = (po1.Y - po0.Y);
double m01 = (po2.X - po0.X);
double m11 = (po2.Y - po0.Y);
aux = new Matrix3x3d(
m00, m01, po0.X,
m10, m11, po0.Y,
0, 0, 1);
}
aux.Inv();
////////////////////////////////////////////////////////////////////////////////////////
m.Mul(aux);
return(m);
}
// Rotation of a vector by a quaternion.
public Vector3d Rotate(Vector3d vec)
{
// Given a vector u = (x0,y0,z0) and a unit length quaternion
// q = <w,x,y,z>, the vector v = (x1,y1,z1) which represents the
// rotation of u by q is v = q*u*q^{-1} where * indicates quaternion
// multiplication and where u is treated as the quaternion <0,x0,y0,z0>.
// Note that q^{-1} = <w,-x,-y,-z>, so no real work is required to
// invert q. Now
//
// q*u*q^{-1} = q*<0,x0,y0,z0>*q^{-1}
// = q*(x0*i+y0*j+z0*k)*q^{-1}
// = x0*(q*i*q^{-1})+y0*(q*j*q^{-1})+z0*(q*k*q^{-1})
//
// As 3-vectors, q*i*q^{-1}, q*j*q^{-1}, and 2*k*q^{-1} are the columns
// of the rotation matrix computed in Quaternion<double>::ToRotationMatrix.
// The vector v is obtained as the product of that rotation matrix with
// vector u. As such, the quaternion representation of a rotation
// matrix requires less space than the matrix and more time to compute
// the rotated vector. Typical space-time tradeoff...
Matrix3x3d rot = this.ToRotationMatrix();
return(rot * vec);
}
/// <summary>
/// Constructor.
/// </summary>
public Matrix2x3d(Matrix3x3d mat)
{
Contract.Assert(mat.M20.EpsilonEquals(0) && mat.M21.EpsilonEquals(0) && mat.M22.EpsilonEquals(1));
this.Set(mat.M00, mat.M01, mat.M02,
mat.M10, mat.M11, mat.M12);
}
public Transform2Matrix(Matrix3x3d matrix)
{
this.Matrix = new Matrix2x3d(matrix.M00, matrix.M01, matrix.M02,
matrix.M10, matrix.M11, matrix.M12);
}
public override void GetMatrix(Matrix3x3d matrix)
{
matrix.Set(this.Matrix);
}
