/// <summary>Constructs left matrix from the given quaternion.</summary> /// <param name="quaternion">The quaternion to use to construct the martix.</param> public Matrix3d(Quaterniond quaternion) { quaternion.Normalize(); double xx = quaternion.X * quaternion.X; double yy = quaternion.Y * quaternion.Y; double zz = quaternion.Z * quaternion.Z; double xy = quaternion.X * quaternion.Y; double xz = quaternion.X * quaternion.Z; double yz = quaternion.Y * quaternion.Z; double wx = quaternion.W * quaternion.X; double wy = quaternion.W * quaternion.Y; double wz = quaternion.W * quaternion.Z; R0C0 = 1 - 2 * (yy + zz); R0C1 = 2 * (xy - wz); R0C2 = 2 * (xz + wy); R1C0 = 2 * (xy + wz); R1C1 = 1 - 2 * (xx + zz); R1C2 = 2 * (yz - wx); R2C0 = 2 * (xz - wy); R2C1 = 2 * (yz + wx); R2C2 = 1 - 2 * (xx + yy); }
/// <summary>Constructs left matrix from the given quaternion.</summary> /// <param name="quaternion">The quaternion to use to construct the martix.</param> public Matrix3(Quaterniond quaternion) { quaternion.Normalize(); float xx = (float)(quaternion.X * quaternion.X); float yy = (float)(quaternion.Y * quaternion.Y); float zz = (float)(quaternion.Z * quaternion.Z); float xy = (float)(quaternion.X * quaternion.Y); float xz = (float)(quaternion.X * quaternion.Z); float yz = (float)(quaternion.Y * quaternion.Z); float wx = (float)(quaternion.W * quaternion.X); float wy = (float)(quaternion.W * quaternion.Y); float wz = (float)(quaternion.W * quaternion.Z); R0C0 = 1 - 2 * (yy + zz); R0C1 = 2 * (xy - wz); R0C2 = 2 * (xz + wy); R1C0 = 2 * (xy + wz); R1C1 = 1 - 2 * (xx + zz); R1C2 = 2 * (yz - wx); R2C0 = 2 * (xz - wy); R2C1 = 2 * (yz + wx); R2C2 = 1 - 2 * (xx + yy); }
/// <summary> /// Returns the rotation component of this instance. Quite slow. /// </summary> /// <param name="row_normalise">Whether the method should row-normalise (i.e. remove scale from) the Matrix. Pass false if you know it's already normalised.</param> public Quaterniond ExtractRotation(bool row_normalise = true) { var row0 = Row0; var row1 = Row1; var row2 = Row2; if (row_normalise) { row0 = row0.Normalized(); row1 = row1.Normalized(); row2 = row2.Normalized(); } // code below adapted from Blender Quaterniond q = new Quaterniond(); double trace = 0.25 * (row0[0] + row1[1] + row2[2] + 1.0); if (trace > 0) { double sq = Math.Sqrt(trace); q.W = sq; sq = 1.0 / (4.0 * sq); q.X = (row1[2] - row2[1]) * sq; q.Y = (row2[0] - row0[2]) * sq; q.Z = (row0[1] - row1[0]) * sq; } else if (row0[0] > row1[1] && row0[0] > row2[2]) { double sq = 2.0 * Math.Sqrt(1.0 + row0[0] - row1[1] - row2[2]); q.X = 0.25 * sq; sq = 1.0 / sq; q.W = (row2[1] - row1[2]) * sq; q.Y = (row1[0] + row0[1]) * sq; q.Z = (row2[0] + row0[2]) * sq; } else if (row1[1] > row2[2]) { double sq = 2.0 * Math.Sqrt(1.0 + row1[1] - row0[0] - row2[2]); q.Y = 0.25 * sq; sq = 1.0 / sq; q.W = (row2[0] - row0[2]) * sq; q.X = (row1[0] + row0[1]) * sq; q.Z = (row2[1] + row1[2]) * sq; } else { double sq = 2.0 * Math.Sqrt(1.0 + row2[2] - row0[0] - row1[1]); q.Z = 0.25 * sq; sq = 1.0 / sq; q.W = (row1[0] - row0[1]) * sq; q.X = (row2[0] + row0[2]) * sq; q.Y = (row2[1] + row1[2]) * sq; } q.Normalize(); return(q); }