/// <summary>
/// Convert from float matrix to double matrix.
/// </summary>
/// <param name="m">Float matrix.</param>
/// <returns>Double matrix.</returns>
public static DMatrix4x4 FromMatrix4x4(Matrix4x4 m)
{
DMatrix4x4 dm = new DMatrix4x4((double)m.m00, (double)m.m01, (double)m.m02, (double)m.m03,
(double)m.m10, (double)m.m11, (double)m.m12, (double)m.m13,
(double)m.m20, (double)m.m21, (double)m.m22, (double)m.m23,
(double)m.m30, (double)m.m31, (double)m.m32, (double)m.m33);
return(dm);
}
/// <summary>
/// Create a translation and rotation matrix.
/// </summary>
/// <param name="translation">Translation as 3 doubles in a DVector3 struct.</param>
/// <param name="orientation">Orientation as 4 doubles in a DVector4 struct.</param>
/// <returns>Double matrix.</returns>
public static DMatrix4x4 TR(DVector3 translation, DVector4 orientation)
{
double[] dTranslation = new double[3];
double[] dOrientation = new double[4];
dTranslation[0] = translation.x;
dTranslation[1] = translation.y;
dTranslation[2] = translation.z;
dOrientation[0] = orientation.x;
dOrientation[1] = orientation.y;
dOrientation[2] = orientation.z;
dOrientation[3] = orientation.w;
return(DMatrix4x4.TR(dTranslation, dOrientation));
}
/// <summary>
/// Retrive translation vector from matrix.
/// </summary>
/// <param name="translation">Translation in vector double array, x, y, z.</param>
/// <param name="orientation">Orientation in quaternion double array, x, y, z, w.</param>
/// <returns>Double matrix.</returns>
private static DMatrix4x4 TR(double[] translation, double[] orientation)
{
DMatrix4x4 dmat = new DMatrix4x4();
double sqw = orientation[3] * orientation[3];
double sqx = orientation[0] * orientation[0];
double sqy = orientation[1] * orientation[1];
double sqz = orientation[2] * orientation[2];
// invs (inverse square length) is only required if quaternion is not already normalised
double invs = 1 / (sqx + sqy + sqz + sqw);
dmat.m00 = (sqx - sqy - sqz + sqw) * invs;
dmat.m11 = (-sqx + sqy - sqz + sqw) * invs;
dmat.m22 = (-sqx - sqy + sqz + sqw) * invs;
double tmp1 = orientation[0] * orientation[1];
double tmp2 = orientation[2] * orientation[3];
dmat.m10 = 2.0 * (tmp1 + tmp2) * invs;
dmat.m01 = 2.0 * (tmp1 - tmp2) * invs;
tmp1 = orientation[0] * orientation[2];
tmp2 = orientation[1] * orientation[3];
dmat.m20 = 2.0 * (tmp1 - tmp2) * invs;
dmat.m02 = 2.0 * (tmp1 + tmp2) * invs;
tmp1 = orientation[1] * orientation[2];
tmp2 = orientation[0] * orientation[3];
dmat.m21 = 2.0 * (tmp1 + tmp2) * invs;
dmat.m12 = 2.0 * (tmp1 - tmp2) * invs;
dmat.m03 = translation[0];
dmat.m13 = translation[1];
dmat.m23 = translation[2];
dmat.m33 = 1.0;
return(dmat);
}
