/// <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>
/// 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;
}
/// <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;
}
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 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;
}
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));
}
