/// <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>
/// This function returns a vector that is rotated by the opposite of me
/// </summary>
public MyVector GetRotatedVectorReverse(MyVector vector, bool isQuatNormalized)
{
if (!isQuatNormalized)
{
// I'm not normalized, clone myself and normalize it
MyQuaternion myUnitClone = new MyQuaternion(this.X, this.Y, this.Z, this.W);
myUnitClone.BecomeUnitQuaternion();
return(myUnitClone.GetRotatedVectorReverse(vector, true));
}
MyVector qvec = new MyVector(this.X, this.Y, this.Z);
//Vector uv = qvec.Cross(vector);
MyVector uv = MyVector.Cross(qvec, vector);
//Vector uuv = qvec.Cross(uv);
MyVector uuv = MyVector.Cross(qvec, uv);
//uv *= (2.0f * quat.w);
uv.Multiply(this.W * -2d);
//uuv *= 2.0f;
uuv.Multiply(2d);
//return vector + uv + uuv;
MyVector retVal = vector.Clone();
retVal.Add(uv);
retVal.Add(uuv);
return(retVal);
}
private static void SplitForceIntoTranslationAndTorque(out MyVector translationForce, out MyVector torque, MyVector centerOfMass, MyVector offset, MyVector force)
{
// The offset passed in is relative to position. I need it to be relative to the center of mass
MyVector trueOffset = offset - centerOfMass;
// Torque is how much of the force is applied perpendicular to the radius
torque = MyVector.Cross(trueOffset, force);
// I'm still not convinced this is totally right, but none of the articles I've read seem to do anything
// different
translationForce = force.Clone();
}
public static MyVector Multiply(MyQuaternion quat, MyVector vector)
{
// nVidia SDK implementation
MyVector uv, uuv;
MyVector qvec = new MyVector(quat.X, quat.Y, quat.Z);
uv = MyVector.Cross(qvec, vector);
uuv = MyVector.Cross(qvec, uv);
uv.Multiply(2.0d * quat.W);
uuv.Multiply(2.0d);
// Add them together
return(vector + uv + uuv);
// get the rotation matrix of the Quaternion and multiply it times the vector
//return quat.ToRotationMatrix() * vector;
}
/// <summary>
/// This function takes in a destination double vector, and I will tell you how much you need to rotate me in order for me to end up
/// along that destination double vector.
/// </summary>
/// <remarks>
/// This function is a mutated copy of MyVector.GetAngleBetweenVectors. It is almost identical, but slightly more complex :)
///
/// If I am already aligned with the vector passed in, then I will return an arbitrary orthoganal, and an angle of zero.
/// </remarks>
/// <param name="destination">This is the double vector you want me to align myself with</param>
public MyQuaternion GetAngleAroundAxis(DoubleVector destination)
{
#region Standard
// Get the angle
double rotationRadians = MyVector.GetAngleBetweenVectors(this.Standard, destination.Standard);
if (Double.IsNaN(rotationRadians))
{
rotationRadians = 0d;
}
// I need to pull the cross product from me to the vector passed in
MyVector rotationAxis = MyVector.Cross(this.Standard, destination.Standard);
// If the cross product is zero, then there are two possibilities. The vectors point in the same direction, or opposite directions.
if (rotationAxis.IsNearZero)
{
// If I am here, then the angle will either be 0 or PI.
if (Utility3D.IsNearZero(rotationRadians))
{
// The vectors sit on top of each other. I will set the orthoganal to an arbitrary value, and return zero for the radians
rotationAxis.X = 1d;
rotationAxis.Y = 0d;
rotationAxis.Z = 0d;
rotationRadians = 0d;
}
else
{
// The vectors are pointing directly away from each other, because this is a double vector, I must rotate along an axis that
// is orthogonal to my standard and orth
rotationAxis = this.Orth; //MyVector.Cross(this.Standard, this.Orth);
}
}
MyQuaternion quatStandard = new MyQuaternion(rotationAxis, rotationRadians);
//return quatStandard;
#endregion
// I only need to rotate the orth, because I already know where the standard will be
MyVector rotatedOrth = quatStandard.GetRotatedVector(this.Orth, true);
#region Orthogonal
// Grab the angle
rotationRadians = MyVector.GetAngleBetweenVectors(rotatedOrth, destination.Orth);
if (Double.IsNaN(rotationRadians))
{
rotationRadians = 0d;
}
// Since I've rotated the standards onto each other, the rotation axis of the orth is the standard (asumming it was truely orthogonal
// to begin with)
rotationAxis = destination.Standard.Clone();
MyQuaternion quatOrth = new MyQuaternion(rotationAxis, rotationRadians);
#endregion
// Exit Function
//return MyQuaternion.Multiply(quatOrth, quatStandard);
return(MyQuaternion.Multiply(quatStandard, quatOrth));
}
public MyVector GetClosestPointOnTriangle(MyVector point)
{
MyVector ab = this.Vertex2 - this.Vertex1;
MyVector ac = this.Vertex3 - this.Vertex1;
MyVector bc = this.Vertex3 - this.Vertex2;
MyVector ap = point - this.Vertex1;
MyVector bp = point - this.Vertex2;
MyVector cp = point - this.Vertex3;
// Compute parametric position s for projection P' of P on AB,
// P' = A + s*AB, s = snom/(snom+sdenom)
double snom = MyVector.Dot(ap, ab);
double sdenom = MyVector.Dot(bp, this.Vertex1 - this.Vertex2);
// Compute parametric position t for projection P' of P on AC,
// P' = A + t*AC, s = tnom/(tnom+tdenom)
double tnom = MyVector.Dot(ap, ac);
double tdenom = MyVector.Dot(cp, this.Vertex1 - this.Vertex3);
if (snom <= 0d && tnom <= 0d)
{
return(this.Vertex1); // Vertex region early out
}
// Compute parametric position u for projection P' of P on BC,
// P' = B + u*BC, u = unom/(unom+udenom)
double unom = MyVector.Dot(bp, bc);
double udenom = MyVector.Dot(cp, this.Vertex2 - this.Vertex3);
if (sdenom <= 0d && unom <= 0d)
{
return(this.Vertex2); // Vertex region early out
}
if (tdenom <= 0d && udenom <= 0d)
{
return(this.Vertex3); // Vertex region early out
}
MyVector pa = this.Vertex1 - point;
MyVector pb = this.Vertex2 - point;
// P is outside (or on) AB if the triple scalar product [N PA PB] <= 0
MyVector n = MyVector.Cross(ab, ac);
double vc = MyVector.Dot(n, MyVector.Cross(pa, pb));
// If P outside AB and within feature region of AB,
// return projection of P onto AB
if (vc <= 0d && snom >= 0d && sdenom >= 0d)
{
return(this.Vertex1 + snom / (snom + sdenom) * ab);
}
MyVector pc = this.Vertex3 - point;
// P is outside (or on) BC if the triple scalar product [N PB PC] <= 0
double va = MyVector.Dot(n, MyVector.Cross(pb, pc));
// If P outside BC and within feature region of BC,
// return projection of P onto BC
if (va <= 0d && unom >= 0d && udenom >= 0d)
{
return(this.Vertex2 + unom / (unom + udenom) * bc);
}
// P is outside (or on) CA if the triple scalar product [N PC PA] <= 0
double vb = MyVector.Dot(n, MyVector.Cross(pc, pa));
// If P outside CA and within feature region of CA,
// return projection of P onto CA
if (vb <= 0d && tnom >= 0d && tdenom >= 0d)
{
return(this.Vertex1 + tnom / (tnom + tdenom) * ac);
}
// P must project inside face region. Compute Q using barycentric coordinates
double u = va / (va + vb + vc);
double v = vb / (va + vb + vc);
double w = 1d - u - v; // = vc / (va + vb + vc)
return(u * this.Vertex1 + v * this.Vertex2 + w * this.Vertex3);
}