/// <summary> /// When you take m * my result, you get the identity matrix /// </summary> public static MyMatrix3 Inverse(MyMatrix3 m) { double determinant = Determinant(m); if (determinant > -0.0005d && determinant < 0.0005d) { return(MyMatrix3.IdentityMatrix); } else { MyMatrix3 retVal = new MyMatrix3(); // The determinant is non-zero, so I can take the inverse (no divide by zero issues) retVal.M11 = (m.M22 * m.M33 - m.M32 * m.M23) / determinant; retVal.M21 = -(m.M21 * m.M33 - m.M23 * m.M31) / determinant; retVal.M31 = (m.M21 * m.M32 - m.M22 * m.M31) / determinant; retVal.M12 = -(m.M12 * m.M33 - m.M32 * m.M13) / determinant; retVal.M22 = (m.M11 * m.M33 - m.M13 * m.M31) / determinant; retVal.M32 = -(m.M11 * m.M32 - m.M12 * m.M31) / determinant; retVal.M13 = (m.M12 * m.M23 - m.M13 * m.M22) / determinant; retVal.M23 = -(m.M11 * m.M23 - m.M13 * m.M21) / determinant; retVal.M33 = (m.M11 * m.M22 - m.M21 * m.M12) / determinant; return(retVal); } }
/// <summary> /// I only made this public so I could hook a tester to it /// </summary> public static void OrthonormalizeOrientation(MyMatrix3 orientation) { // Do some crazy math (something about constraining 9 degrees of freedom of a matrix down to 3) MyVector x = new MyVector(orientation.M11, orientation.M21, orientation.M31); x.BecomeUnitVector(); MyVector y = new MyVector(orientation.M12, orientation.M22, orientation.M32); // just store a temp variable into y (until I calculate z) MyVector z = MyVector.Cross(x, y); z.BecomeUnitVector(); y = MyVector.Cross(z, x); y.BecomeUnitVector(); // Overwrite the matrix passed in orientation.M11 = x.X; orientation.M12 = y.X; orientation.M13 = z.X; orientation.M21 = x.Y; orientation.M22 = y.Y; orientation.M23 = z.Y; orientation.M31 = x.Z; orientation.M32 = y.Z; orientation.M33 = z.Z; }
public static MyMatrix3 Transpose(MyMatrix3 matrix) { MyMatrix3 retVal = matrix.Clone(); retVal.Transpose(); return(retVal); }
public static MyMatrix3 Add(MyMatrix3 a, MyMatrix3 b) { MyMatrix3 retVal = a.Clone(); retVal.Add(b); return(retVal); }
public static MyMatrix3 Multiply(MyMatrix3 matrix, double scalar) { MyMatrix3 retVal = matrix.Clone(); retVal.Multiply(scalar); return(retVal); }
/// <summary> /// Transforms a vector by a matrix. /// </summary> /// <remarks> /// I've seen this in other code as matrix * vector /// </remarks> public static MyVector Transform(MyMatrix3 matrix, MyVector vector) { return(new MyVector( (matrix.M11 * vector.X) + (matrix.M12 * vector.Y) + (matrix.M13 * vector.Z), (matrix.M21 * vector.X) + (matrix.M22 * vector.Y) + (matrix.M23 * vector.Z), (matrix.M31 * vector.X) + (matrix.M32 * vector.Y) + (matrix.M33 * vector.Z))); }
/// <summary> /// Every frame, angular velocity is wiped out and recalculated based on angular momentum. So this function actually /// sets angular momentum to produce the velocity passed in. /// </summary> /// <remarks> /// This function is the opposite of the calculation in ApplyTorque /// </remarks> public void SetAngularVelocity(MyVector angularVelocity) { // Figure out the world frame's inertia tensor // (Rotation * bodyInertialTensorInverse * Transposed Rotation) MyMatrix3 curRotation = base.RotationMatrix.Clone(); MyMatrix3 worldInertiaTensor = MyMatrix3.Multiply(MyMatrix3.Multiply(curRotation, _inertialTensorBody), MyMatrix3.Transpose(curRotation)); // Now store the angular momentum required to generate this velocity _angularMomentum.StoreNewValues(MyMatrix3.Multiply(worldInertiaTensor, angularVelocity)); }
/// <remarks> /// This function assumes that the quaternion has a magnitude of one. (it's optimized for that) /// /// I still have no clue what should be done with this matrix, but here it is /// /// I copied the code from the other overload, because I want absolute performance /// </remarks> public MyMatrix3 ToMatrix3FromUnitQuaternion() { MyMatrix3 retVal = new MyMatrix3(); // All these cached results are used at least twice, so I'm not being wastefull double xx = this.X * this.X; double yy = this.Y * this.Y; double zz = this.Z * this.Z; double xy = this.X * this.Y; double xz = this.X * this.Z; double yz = this.Y * this.Z; double wx = this.W * this.X; double wy = this.W * this.Y; double wz = this.W * this.Z; retVal.M11 = 1 - 2 * (yy + zz); retVal.M12 = 2 * (xy - wz); retVal.M13 = 2 * (xz + wy); retVal.M21 = 2 * (xy + wz); retVal.M22 = 1 - 2 * (xx + zz); retVal.M23 = 2 * (yz - wx); retVal.M31 = 2 * (xz - wy); retVal.M32 = 2 * (yz + wx); retVal.M33 = 1 - 2 * (xx + yy); #region Old /* * // Row 1 * retVal.M11 = 1 - ((2 * yy) - (2 * zz)); * retVal.M12 = (2 * xy) - (2 * wz); * retVal.M13 = (2 * xz) + (2 * wy); * * // Row 2 * retVal.M21 = (2 * xy) + (2 * wz); * retVal.M22 = 1 - ((2 * xx) - (2 * zz)); * retVal.M23 = (2 * yz) - (2 * wz); * * // Row 3 * retVal.M31 = (2 * xz) - (2 * wy); * retVal.M32 = (2 * yz) - (2 * wz); * retVal.M33 = 1 - ((2 * xx) - (2 * yy)); */ #endregion // Exit Function return(retVal); }
public void Add(MyMatrix3 matrix) { this.M11 += matrix.M11; this.M12 += matrix.M12; this.M13 += matrix.M13; this.M21 += matrix.M21; this.M22 += matrix.M22; this.M23 += matrix.M23; this.M31 += matrix.M31; this.M32 += matrix.M32; this.M33 += matrix.M33; }
private void ApplyTorque(double elapsedTime) { // Calculate the new angular momentum (current + (torque * time)) MyVector newMomentum = _internalTorque.Clone(); newMomentum.Multiply(elapsedTime); _angularMomentum.Add(newMomentum); // Figure out the inverse of the world frame's inertia tensor // (Rotation * bodyInertialTensorInverse * Transposed Rotation) MyMatrix3 curRotation = base.RotationMatrix.Clone(); MyMatrix3 inverseWorldInertiaTensor = MyMatrix3.Multiply(MyMatrix3.Multiply(curRotation, _inertialTensorBodyInverse), MyMatrix3.Transpose(curRotation)); // Now all that's left is to figure out the new angular velocity _angularVelocity.StoreNewValues(MyMatrix3.Multiply(inverseWorldInertiaTensor, _angularMomentum)); }
/// <summary> /// This multiplies the matrix by the vector, and returns the result as a vector (I think) /// </summary> /// <remarks> /// It's sort of a tossup where this function belongs. It should probably go in Utility, but nobody will think to look there. /// And I don't want to dirty up the vector class, it's already pretty full. /// </remarks> public static MyVector Multiply(MyMatrix3 matrix, MyVector vector) { MyVector retVal = new MyVector(); retVal.X = matrix.M11 * vector.X; retVal.X += matrix.M12 * vector.Y; retVal.X += matrix.M13 * vector.Z; retVal.Y = matrix.M21 * vector.X; retVal.Y += matrix.M22 * vector.Y; retVal.Y += matrix.M23 * vector.Z; retVal.Z = matrix.M31 * vector.X; retVal.Z += matrix.M32 * vector.Y; retVal.Z += matrix.M33 * vector.Z; return(retVal); }
public MyMatrix3 Clone() { MyMatrix3 retVal = new MyMatrix3(); retVal.M11 = this.M11; retVal.M12 = this.M12; retVal.M13 = this.M13; retVal.M21 = this.M21; retVal.M22 = this.M22; retVal.M23 = this.M23; retVal.M31 = this.M31; retVal.M32 = this.M32; retVal.M33 = this.M33; return(retVal); }
/// <remarks> /// The other author just called this star (probably for vector star) /// </remarks> private static MyMatrix3 SkewSymmetric(MyVector vector) { MyMatrix3 retVal = new MyMatrix3(); retVal.M11 = 0; retVal.M12 = vector.Z * -1; retVal.M13 = vector.Y; retVal.M21 = vector.Z; retVal.M22 = 0; retVal.M23 = vector.X * -1; retVal.M31 = vector.Y * -1; retVal.M32 = vector.X; retVal.M33 = 0; return(retVal); }
public static MyMatrix3 Multiply(MyMatrix3 a, MyMatrix3 b) { MyMatrix3 retVal = new MyMatrix3(); retVal.M11 = (a.M11 * b.M11) + (a.M12 * b.M21) + (a.M13 * b.M31); retVal.M12 = (a.M11 * b.M12) + (a.M12 * b.M22) + (a.M13 * b.M32); retVal.M13 = (a.M11 * b.M13) + (a.M12 * b.M23) + (a.M13 * b.M33); retVal.M21 = (a.M21 * b.M11) + (a.M22 * b.M21) + (a.M23 * b.M31); retVal.M22 = (a.M21 * b.M12) + (a.M22 * b.M22) + (a.M23 * b.M32); retVal.M23 = (a.M21 * b.M13) + (a.M22 * b.M23) + (a.M23 * b.M33); retVal.M31 = (a.M31 * b.M11) + (a.M32 * b.M21) + (a.M33 * b.M31); retVal.M32 = (a.M31 * b.M12) + (a.M32 * b.M22) + (a.M33 * b.M32); retVal.M33 = (a.M31 * b.M13) + (a.M32 * b.M23) + (a.M33 * b.M33); return(retVal); }
protected override void ResetInertiaTensorAndCenterOfMass() { double momentOfInertia = (2d / 5d) * base.Mass * (base.Radius * base.Radius); MyMatrix3 inertiaTensor = new MyMatrix3(); inertiaTensor.M11 = momentOfInertia; inertiaTensor.M12 = 0; inertiaTensor.M13 = 0; inertiaTensor.M21 = 0; inertiaTensor.M22 = momentOfInertia; inertiaTensor.M23 = 0; inertiaTensor.M31 = 0; inertiaTensor.M32 = 0; inertiaTensor.M33 = momentOfInertia; base.InertialTensorBody = inertiaTensor; // I will let the center of mass stay 0,0,0 }
public void FromRotationMatrix(MyMatrix3 matrix) { double trace = matrix.M11 + matrix.M22 + matrix.M33 + 1; double s; // I'm not sure what s should be called if (trace > 0d) { #region Instant Calculation s = Math.Sqrt(trace) * 2d; this.W = 0.25d * s; // the other had this .25/s this.X = (matrix.M32 - matrix.M23) / s; this.Y = (matrix.M13 - matrix.M31) / s; this.Z = (matrix.M21 - matrix.M12) / s; #endregion } else { // Find the major diagonal element with the greatest value if (matrix.M11 > matrix.M22 && matrix.M11 > matrix.M33) { #region Column 0 s = Math.Sqrt(1d + matrix.M11 - matrix.M22 - matrix.M33) * 2d; this.X = 0.25d * s; this.Y = (matrix.M21 + matrix.M12) / s; this.Z = (matrix.M13 + matrix.M31) / s; this.W = (matrix.M32 - matrix.M23) / s; /* * s = Math.Sqrt(1d + matrix.M11 - matrix.M22 - matrix.M33) * 2d; * * this.X = 0.5d / s; * this.Y = (matrix.M21 + matrix.M12) / s; * this.Z = (matrix.M32 + matrix.M13) / s; * this.W = (matrix.M32 + matrix.M23) / s; */ #endregion } else if (matrix.M22 > matrix.M11 && matrix.M22 > matrix.M33) { #region Column 1 s = Math.Sqrt(1d + matrix.M22 - matrix.M11 - matrix.M33) * 2d; this.X = (matrix.M21 + matrix.M12) / s; this.Y = 0.25d * s; this.Z = (matrix.M32 + matrix.M23) / s; this.W = (matrix.M13 - matrix.M31) / s; /* * s = Math.Sqrt(1d + matrix.M22 - matrix.M11 - matrix.M33) * 2d; * * this.X = (matrix.M21 + matrix.M12) / s; * this.Y = 0.5d / s; * this.Z = (matrix.M32 + matrix.M23) / s; * this.W = (matrix.M31 + matrix.M13) / s; */ #endregion } else { #region Column 2 s = Math.Sqrt(1d + matrix.M33 - matrix.M11 - matrix.M22) * 2d; this.X = (matrix.M13 + matrix.M31) / s; this.Y = (matrix.M32 + matrix.M23) / s; this.Z = 0.25d * s; this.W = (matrix.M21 - matrix.M12) / s; /* * s = Math.Sqrt(1d + matrix.M33 - matrix.M11 - matrix.M22) * 2d; * * this.X = (matrix.M31 + matrix.M13) / s; * this.Y = (matrix.M32 + matrix.M23) / s; * this.Z = 0.5d / s; * this.W = (matrix.M21 + matrix.M12) / s; */ #endregion } } // This makes me feel better this.BecomeUnitQuaternion(); #region Reading Material /* * * Q48. How do I convert a rotation matrix to a quaternion? * -------------------------------------------------------- * * A rotation may be converted back to a quaternion through the use of * the following algorithm: * * The process is performed in the following stages, which are as follows: * * Calculate the trace of the matrix T from the equation: * * 2 2 2 * T = 4 - 4x - 4y - 4z * * 2 2 2 * = 4( 1 -x - y - z ) * * = mat[0] + mat[5] + mat[10] + 1 * * * If the trace of the matrix is greater than zero, then * perform an "instant" calculation. * * S = 0.5 / sqrt(T) * * W = 0.25 / S * * X = ( mat[9] - mat[6] ) * S * * Y = ( mat[2] - mat[8] ) * S * * Z = ( mat[4] - mat[1] ) * S */ #endregion #region More Work /* * If the trace of the matrix is less than or equal to zero * then identify which major diagonal element has the greatest * value. * * Depending on this value, calculate the following: * * Column 0: * S = sqrt( 1.0 + mr[0] - mr[5] - mr[10] ) * 2; * * Qx = 0.5 / S; * Qy = (mr[1] + mr[4] ) / S; * Qz = (mr[2] + mr[8] ) / S; * Qw = (mr[6] + mr[9] ) / S; * * Column 1: * S = sqrt( 1.0 + mr[5] - mr[0] - mr[10] ) * 2; * * Qx = (mr[1] + mr[4] ) / S; * Qy = 0.5 / S; * Qz = (mr[6] + mr[9] ) / S; * Qw = (mr[2] + mr[8] ) / S; * * Column 2: * S = sqrt( 1.0 + mr[10] - mr[0] - mr[5] ) * 2; * * Qx = (mr[2] + mr[8] ) / S; * Qy = (mr[6] + mr[9] ) / S; * Qz = 0.5 / S; * Qw = (mr[1] + mr[4] ) / S; * * The quaternion is then defined as: * * Q = | Qx Qy Qz Qw | * */ #endregion }
/// <summary> /// Transforms a vector by a matrix. /// </summary> /// <remarks> /// I've seen this in other code as matrix * vector /// </remarks> public static MyVector Transform(MyMatrix3 matrix, MyVector vector) { return new MyVector( (matrix.M11 * vector.X) + (matrix.M12 * vector.Y) + (matrix.M13 * vector.Z), (matrix.M21 * vector.X) + (matrix.M22 * vector.Y) + (matrix.M23 * vector.Z), (matrix.M31 * vector.X) + (matrix.M32 * vector.Y) + (matrix.M33 * vector.Z)); }
/// <remarks> /// This function assumes that the quaternion has a magnitude of one. (it's optimized for that) /// /// I still have no clue what should be done with this matrix, but here it is /// /// I copied the code from the other overload, because I want absolute performance /// </remarks> public MyMatrix3 ToMatrix3FromUnitQuaternion() { MyMatrix3 retVal = new MyMatrix3(); // All these cached results are used at least twice, so I'm not being wastefull double xx = this.X * this.X; double yy = this.Y * this.Y; double zz = this.Z * this.Z; double xy = this.X * this.Y; double xz = this.X * this.Z; double yz = this.Y * this.Z; double wx = this.W * this.X; double wy = this.W * this.Y; double wz = this.W * this.Z; retVal.M11 = 1 - 2 * (yy + zz); retVal.M12 = 2 * (xy - wz); retVal.M13 = 2 * (xz + wy); retVal.M21 = 2 * (xy + wz); retVal.M22 = 1 - 2 * (xx + zz); retVal.M23 = 2 * (yz - wx); retVal.M31 = 2 * (xz - wy); retVal.M32 = 2 * (yz + wx); retVal.M33 = 1 - 2 * (xx + yy); #region Old /* // Row 1 retVal.M11 = 1 - ((2 * yy) - (2 * zz)); retVal.M12 = (2 * xy) - (2 * wz); retVal.M13 = (2 * xz) + (2 * wy); // Row 2 retVal.M21 = (2 * xy) + (2 * wz); retVal.M22 = 1 - ((2 * xx) - (2 * zz)); retVal.M23 = (2 * yz) - (2 * wz); // Row 3 retVal.M31 = (2 * xz) - (2 * wy); retVal.M32 = (2 * yz) - (2 * wz); retVal.M33 = 1 - ((2 * xx) - (2 * yy)); */ #endregion // Exit Function return retVal; }
public static MyMatrix3 Multiply(MyMatrix3 matrix, double scalar) { MyMatrix3 retVal = matrix.Clone(); retVal.Multiply(scalar); return retVal; }
/// <summary> /// This multiplies the matrix by the vector, and returns the result as a vector (I think) /// </summary> /// <remarks> /// It's sort of a tossup where this function belongs. It should probably go in Utility, but nobody will think to look there. /// And I don't want to dirty up the vector class, it's already pretty full. /// </remarks> public static MyVector Multiply(MyMatrix3 matrix, MyVector vector) { MyVector retVal = new MyVector(); retVal.X = matrix.M11 * vector.X; retVal.X += matrix.M12 * vector.Y; retVal.X += matrix.M13 * vector.Z; retVal.Y = matrix.M21 * vector.X; retVal.Y += matrix.M22 * vector.Y; retVal.Y += matrix.M23 * vector.Z; retVal.Z = matrix.M31 * vector.X; retVal.Z += matrix.M32 * vector.Y; retVal.Z += matrix.M33 * vector.Z; return retVal; }
public static MyMatrix3 Multiply(MyMatrix3 a, MyMatrix3 b) { MyMatrix3 retVal = new MyMatrix3(); retVal.M11 = (a.M11 * b.M11) + (a.M12 * b.M21) + (a.M13 * b.M31); retVal.M12 = (a.M11 * b.M12) + (a.M12 * b.M22) + (a.M13 * b.M32); retVal.M13 = (a.M11 * b.M13) + (a.M12 * b.M23) + (a.M13 * b.M33); retVal.M21 = (a.M21 * b.M11) + (a.M22 * b.M21) + (a.M23 * b.M31); retVal.M22 = (a.M21 * b.M12) + (a.M22 * b.M22) + (a.M23 * b.M32); retVal.M23 = (a.M21 * b.M13) + (a.M22 * b.M23) + (a.M23 * b.M33); retVal.M31 = (a.M31 * b.M11) + (a.M32 * b.M21) + (a.M33 * b.M31); retVal.M32 = (a.M31 * b.M12) + (a.M32 * b.M22) + (a.M33 * b.M32); retVal.M33 = (a.M31 * b.M13) + (a.M32 * b.M23) + (a.M33 * b.M33); return retVal; }
public static double Determinant(MyMatrix3 m) { return m.M11 * (m.M22 * m.M33 - m.M23 * m.M32) - m.M21 * (m.M12 * m.M33 - m.M13 * m.M32) + m.M31 * (m.M12 * m.M23 - m.M13 * m.M22); }
/// <summary> /// When you take m * my result, you get the identity matrix /// </summary> public static MyMatrix3 Inverse(MyMatrix3 m) { double determinant = Determinant(m); if (determinant > -0.0005d && determinant < 0.0005d) { return MyMatrix3.IdentityMatrix; } else { MyMatrix3 retVal = new MyMatrix3(); // The determinant is non-zero, so I can take the inverse (no divide by zero issues) retVal.M11 = (m.M22 * m.M33 - m.M32 * m.M23) / determinant; retVal.M21 = -(m.M21 * m.M33 - m.M23 * m.M31) / determinant; retVal.M31 = (m.M21 * m.M32 - m.M22 * m.M31) / determinant; retVal.M12 = -(m.M12 * m.M33 - m.M32 * m.M13) / determinant; retVal.M22 = (m.M11 * m.M33 - m.M13 * m.M31) / determinant; retVal.M32 = -(m.M11 * m.M32 - m.M12 * m.M31) / determinant; retVal.M13 = (m.M12 * m.M23 - m.M13 * m.M22) / determinant; retVal.M23 = -(m.M11 * m.M23 - m.M13 * m.M21) / determinant; retVal.M33 = (m.M11 * m.M22 - m.M21 * m.M12) / determinant; return retVal; } }
/// <summary> /// Impulse force to change the velocity at a certain point /// </summary> /// <remarks> /// R1 = Contact point on body 1 /// /// J = DeltaVelocity / [(1/m1 + 1/m2 + N dot (((R1 x N) * inverseTensor1) x R1) + N dot (((R2 x N) * inverseTensor2) x R2))] /// /// Denominator should never be negative, but due to floating point inaccuracies this seems to sometimes happen! /// </remarks> private double GetImpulseForceMagnitudeFromDeltaVelocity(double deltaVelocity, MyVector pointOfContact1, MyVector pointOfContact2, MyVector lineOfAction, double sumOf1OverMass, MyMatrix3 inverseTensor1, MyMatrix3 inverseTensor2) { MyVector R1crossNtimesJ1 = MyMatrix3.Multiply(inverseTensor1, MyVector.Cross(pointOfContact1, lineOfAction)); MyVector R2CrossNtimesJ2 = MyMatrix3.Multiply(inverseTensor2, MyVector.Cross(pointOfContact2, lineOfAction)); double denominator = sumOf1OverMass + MyVector.Dot(lineOfAction, MyVector.Cross(R1crossNtimesJ1, pointOfContact1)) + MyVector.Dot(lineOfAction, MyVector.Cross(R2CrossNtimesJ2, pointOfContact2)); return (deltaVelocity / denominator); }
private static double GetLeftRightThrusterMagnitude(MyMatrix3 inertialTensor) { // Create a standard sized solid ball, and use that as my baseline SolidBall standBall = new SolidBall(new MyVector(), new DoubleVector(1, 0, 0, 0, 1, 0), STANDARDRADIUS, UtilityCore.GetMassForRadius(STANDARDRADIUS, 1d)); double averageStand = GetLeftRightThrusterMagnitudeSprtGetAvg(standBall.InertialTensorBody); double averageShip = GetLeftRightThrusterMagnitudeSprtGetAvg(inertialTensor); return THRUSTER_FORCE * (Math.Sqrt(averageShip) / Math.Sqrt(averageStand)); // I need sqrt, because the tensor's don't grow linearly }
public void FromRotationMatrix(MyMatrix3 matrix) { double trace = matrix.M11 + matrix.M22 + matrix.M33 + 1; double s; // I'm not sure what s should be called if (trace > 0d) { #region Instant Calculation s = Math.Sqrt(trace) * 2d; this.W = 0.25d * s; // the other had this .25/s this.X = (matrix.M32 - matrix.M23) / s; this.Y = (matrix.M13 - matrix.M31) / s; this.Z = (matrix.M21 - matrix.M12) / s; #endregion } else { // Find the major diagonal element with the greatest value if (matrix.M11 > matrix.M22 && matrix.M11 > matrix.M33) { #region Column 0 s = Math.Sqrt(1d + matrix.M11 - matrix.M22 - matrix.M33) * 2d; this.X = 0.25d * s; this.Y = (matrix.M21 + matrix.M12) / s; this.Z = (matrix.M13 + matrix.M31) / s; this.W = (matrix.M32 - matrix.M23) / s; /* s = Math.Sqrt(1d + matrix.M11 - matrix.M22 - matrix.M33) * 2d; this.X = 0.5d / s; this.Y = (matrix.M21 + matrix.M12) / s; this.Z = (matrix.M32 + matrix.M13) / s; this.W = (matrix.M32 + matrix.M23) / s; */ #endregion } else if (matrix.M22 > matrix.M11 && matrix.M22 > matrix.M33) { #region Column 1 s = Math.Sqrt(1d + matrix.M22 - matrix.M11 - matrix.M33) * 2d; this.X = (matrix.M21 + matrix.M12) / s; this.Y = 0.25d * s; this.Z = (matrix.M32 + matrix.M23) / s; this.W = (matrix.M13 - matrix.M31) / s; /* s = Math.Sqrt(1d + matrix.M22 - matrix.M11 - matrix.M33) * 2d; this.X = (matrix.M21 + matrix.M12) / s; this.Y = 0.5d / s; this.Z = (matrix.M32 + matrix.M23) / s; this.W = (matrix.M31 + matrix.M13) / s; */ #endregion } else { #region Column 2 s = Math.Sqrt(1d + matrix.M33 - matrix.M11 - matrix.M22) * 2d; this.X = (matrix.M13 + matrix.M31) / s; this.Y = (matrix.M32 + matrix.M23) / s; this.Z = 0.25d * s; this.W = (matrix.M21 - matrix.M12) / s; /* s = Math.Sqrt(1d + matrix.M33 - matrix.M11 - matrix.M22) * 2d; this.X = (matrix.M31 + matrix.M13) / s; this.Y = (matrix.M32 + matrix.M23) / s; this.Z = 0.5d / s; this.W = (matrix.M21 + matrix.M12) / s; */ #endregion } } // This makes me feel better this.BecomeUnitQuaternion(); #region Reading Material /* Q48. How do I convert a rotation matrix to a quaternion? -------------------------------------------------------- A rotation may be converted back to a quaternion through the use of the following algorithm: The process is performed in the following stages, which are as follows: Calculate the trace of the matrix T from the equation: 2 2 2 T = 4 - 4x - 4y - 4z 2 2 2 = 4( 1 -x - y - z ) = mat[0] + mat[5] + mat[10] + 1 If the trace of the matrix is greater than zero, then perform an "instant" calculation. S = 0.5 / sqrt(T) W = 0.25 / S X = ( mat[9] - mat[6] ) * S Y = ( mat[2] - mat[8] ) * S Z = ( mat[4] - mat[1] ) * S */ #endregion #region More Work /* If the trace of the matrix is less than or equal to zero then identify which major diagonal element has the greatest value. Depending on this value, calculate the following: Column 0: S = sqrt( 1.0 + mr[0] - mr[5] - mr[10] ) * 2; Qx = 0.5 / S; Qy = (mr[1] + mr[4] ) / S; Qz = (mr[2] + mr[8] ) / S; Qw = (mr[6] + mr[9] ) / S; Column 1: S = sqrt( 1.0 + mr[5] - mr[0] - mr[10] ) * 2; Qx = (mr[1] + mr[4] ) / S; Qy = 0.5 / S; Qz = (mr[6] + mr[9] ) / S; Qw = (mr[2] + mr[8] ) / S; Column 2: S = sqrt( 1.0 + mr[10] - mr[0] - mr[5] ) * 2; Qx = (mr[2] + mr[8] ) / S; Qy = (mr[6] + mr[9] ) / S; Qz = 0.5 / S; Qw = (mr[1] + mr[4] ) / S; The quaternion is then defined as: Q = | Qx Qy Qz Qw | */ #endregion }
private static double GetLeftRightThrusterMagnitudeSprtGetAvg(MyMatrix3 inertialTensor) { double retVal = 0; // I don't include 2-2, because it represents the center, and doesn't really have an effect on spin? retVal += inertialTensor.M11; //retVal += inertialTensor.M12; //retVal += inertialTensor.M13; //retVal += inertialTensor.M21; //retVal += inertialTensor.M22; //retVal += inertialTensor.M23; //retVal += inertialTensor.M31; //retVal += inertialTensor.M32; retVal += inertialTensor.M33; return retVal / 2d; }
protected override void ResetInertiaTensorAndCenterOfMass() { if (_masses.Count == 0) { // Since I have no mass or structure, I'll use default values (nothing better whack me in this state, I'm sure // the result would be near infinite velocity) base.CenterOfMass.X = 0; base.CenterOfMass.Y = 0; base.CenterOfMass.Z = 0; base.InertialTensorBody = MyMatrix3.IdentityMatrix; base.Mass = NEARZEROMASS; // I don't want to call this.Mass, because it has extra checks return; } // Figure out the center of mass double totalMass; MyVector centerMass = GetCenterOfMass(out totalMass); // Get the locations of the point masses relative to the center of mass (instead of relative to base.Position) MyVector[] massLocations = GetRelativeMassPositions(centerMass); // this array lines up with _masses // Figure out the inertia tensor MyMatrix3 inertiaTensor = new MyMatrix3(); #region Calculate Tensor for (int massCntr = 0; massCntr < _masses.Count; massCntr++) { // M(Y^2 + Z^2) inertiaTensor.M11 += _masses[massCntr].Mass * ((massLocations[massCntr].Y * massLocations[massCntr].Y) + (massLocations[massCntr].Z * massLocations[massCntr].Z)); // M(X^2 + Z^2) inertiaTensor.M22 += _masses[massCntr].Mass * ((massLocations[massCntr].X * massLocations[massCntr].X) + (massLocations[massCntr].Z * massLocations[massCntr].Z)); // M(X^2 + Y^2) inertiaTensor.M33 += _masses[massCntr].Mass * ((massLocations[massCntr].X * massLocations[massCntr].X) + (massLocations[massCntr].Y * massLocations[massCntr].Y)); // MXY inertiaTensor.M21 += _masses[massCntr].Mass * massLocations[massCntr].X * massLocations[massCntr].Y; // MXZ inertiaTensor.M31 += _masses[massCntr].Mass * massLocations[massCntr].X * massLocations[massCntr].Z; // MYZ inertiaTensor.M32 += _masses[massCntr].Mass * massLocations[massCntr].Y * massLocations[massCntr].Z; } // Finish up the non diagnals (it's actually the negative sum for them, and the transpose elements have // the same value) inertiaTensor.M21 *= -1; inertiaTensor.M12 = inertiaTensor.M21; inertiaTensor.M31 *= -1; inertiaTensor.M13 = inertiaTensor.M31; inertiaTensor.M32 *= -1; inertiaTensor.M23 = inertiaTensor.M32; #endregion // Store the values base.CenterOfMass.StoreNewValues(centerMass); base.InertialTensorBody = inertiaTensor; base.Mass = totalMass; // I don't want to call this.Mass, because it has extra checks }
/// <remarks> /// The other author just called this star (probably for vector star) /// </remarks> private static MyMatrix3 SkewSymmetric(MyVector vector) { MyMatrix3 retVal = new MyMatrix3(); retVal.M11 = 0; retVal.M12 = vector.Z * -1; retVal.M13 = vector.Y; retVal.M21 = vector.Z; retVal.M22 = 0; retVal.M23 = vector.X * -1; retVal.M31 = vector.Y * -1; retVal.M32 = vector.X; retVal.M33 = 0; return retVal; }
public MyMatrix3 Clone() { MyMatrix3 retVal = new MyMatrix3(); retVal.M11 = this.M11; retVal.M12 = this.M12; retVal.M13 = this.M13; retVal.M21 = this.M21; retVal.M22 = this.M22; retVal.M23 = this.M23; retVal.M31 = this.M31; retVal.M32 = this.M32; retVal.M33 = this.M33; return retVal; }
public static MyMatrix3 Transpose(MyMatrix3 matrix) { MyMatrix3 retVal = matrix.Clone(); retVal.Transpose(); return retVal; }
public static MyMatrix3 Add(MyMatrix3 a, MyMatrix3 b) { MyMatrix3 retVal = a.Clone(); retVal.Add(b); return retVal; }
public static double Determinant(MyMatrix3 m) { return(m.M11 * (m.M22 * m.M33 - m.M23 * m.M32) - m.M21 * (m.M12 * m.M33 - m.M13 * m.M32) + m.M31 * (m.M12 * m.M23 - m.M13 * m.M22)); }