public void UpdateSeparatingDistance(ref Matrix transA, ref Matrix transB)
{
Vector3 toPosA = transA.Translation;
Vector3 toPosB = transB.Translation;
Quaternion toOrnA = TransformUtil.GetRotation(ref transA);
Quaternion toOrnB = TransformUtil.GetRotation(ref transB);
if (m_separatingDistance > 0.0f)
{
Vector3 linVelA = Vector3.Zero;
Vector3 angVelA = Vector3.Zero;
Vector3 linVelB = Vector3.Zero;
Vector3 angVelB = Vector3.Zero;
TransformUtil.CalculateVelocityQuaternion(ref m_posA, ref toPosA, ref m_ornA, ref toOrnA, 1f, ref linVelA, ref angVelA);
TransformUtil.CalculateVelocityQuaternion(ref m_posB, ref toPosB, ref m_ornB, ref toOrnB, 1f, ref linVelB, ref angVelB);
float maxAngularProjectedVelocity = angVelA.Length() * m_boundingRadiusA + angVelB.Length() * m_boundingRadiusB;
Vector3 relLinVel = (linVelB - linVelA);
float relLinVelocLength = Vector3.Dot((linVelB - linVelA), m_separatingNormal);
if (relLinVelocLength < 0f)
{
relLinVelocLength = 0f;
}
float projectedMotion = maxAngularProjectedVelocity + relLinVelocLength;
m_separatingDistance -= projectedMotion;
}
m_posA = toPosA;
m_posB = toPosB;
m_ornA = toOrnA;
m_ornB = toOrnB;
}
void InitSeparatingDistance(ref Vector3 separatingVector, float separatingDistance, ref Matrix transA, ref Matrix transB)
{
m_separatingNormal = separatingVector;
m_separatingDistance = separatingDistance;
Vector3 toPosA = transA.Translation;
Vector3 toPosB = transB.Translation;
Quaternion toOrnA = TransformUtil.GetRotation(ref transA);
Quaternion toOrnB = TransformUtil.GetRotation(ref transB);
m_posA = toPosA;
m_posB = toPosB;
m_ornA = toOrnA;
m_ornB = toOrnB;
}
public static extern Handle CreateChildSceneNode( Handle self, [MarshalAs(UnmanagedType.LPStr)] String name, Vector3 translate, Quaternion rotate);
public static Quaternion QuatVectorMultiply(ref Quaternion q, ref Vector3 w)
{
return new Quaternion(q.W * w.X + q.Y * w.Z - q.Z * w.Y,
q.W * w.Y + q.Z * w.X - q.X * w.Z,
q.W * w.Z + q.X * w.Y - q.Y * w.X,
-q.X * w.X - q.Y * w.Y - q.Z * w.Z);
}
public static Vector3 QuatRotate(Quaternion rotation, Vector3 v)
{
return QuatRotate(ref rotation, ref v);
}
public static float QuatAngle(ref Quaternion quat)
{
return 2f * (float)System.Math.Acos(quat.W);
}
public static Quaternion QuaternionMultiply(ref Quaternion a, ref Quaternion b)
{
return a * b;
//return b * a;
}
public static Quaternion QuaternionInverse(ref Quaternion q)
{
return new Quaternion(-q.X, -q.Y, -q.Z, q.W);
}
public static Quaternion Inverse(Quaternion quaternion)
{
Inverse(ref quaternion, out quaternion);
return quaternion;
}
public static void Dot(ref Quaternion quaternion1, ref Quaternion quaternion2, out float result)
{
result = (quaternion1.X * quaternion2.X) + (quaternion1.Y * quaternion2.Y) +
(quaternion1.Z * quaternion2.Z) + (quaternion1.W * quaternion2.W);
}
public static float Dot(Quaternion quaternion1, Quaternion quaternion2)
{
float result;
Dot(ref quaternion1, ref quaternion2, out result);
return result;
}
public static void Conjugate(ref Quaternion value, out Quaternion result)
{
result = default(Quaternion);
result.X = -value.X;
result.Y = -value.Y;
result.Z = -value.Z;
result.W = value.W;
}
public static Quaternion Conjugate(Quaternion value)
{
Conjugate(ref value, out value);
return value;
}
public static void Concatenate(ref Quaternion value1, ref Quaternion value2, out Quaternion result)
{
Multiply(ref value1, ref value2, out result);
}
public static Quaternion Concatenate(Quaternion value1, Quaternion value2)
{
Quaternion result;
Concatenate(ref value1, ref value2, out result);
return result;
}
// Taken from Fabian Vikings post at : http://forums.xna.com/forums/p/4574/23763.aspx
public static Vector3 quaternionToEuler(ref Quaternion q)
{
Vector3 v = Vector3.Zero;
v.X = (float)System.Math.Atan2
(
2 * q.Y * q.W - 2 * q.X * q.Z,
1 - 2 * System.Math.Pow(q.Y, 2) - 2 * System.Math.Pow(q.Z, 2)
);
v.Z = (float)System.Math.Asin
(
2 * q.X * q.Y + 2 * q.Z * q.W
);
v.Y = (float)System.Math.Atan2
(
2 * q.X * q.W - 2 * q.Y * q.Z,
1 - 2 * System.Math.Pow(q.X, 2) - 2 * System.Math.Pow(q.Z, 2)
);
if (q.X * q.Y + q.Z * q.W == 0.5)
{
v.X = (float)(2 * System.Math.Atan2(q.X, q.W));
v.Y = 0;
}
else if (q.X * q.Y + q.Z * q.W == -0.5)
{
v.X = (float)(-2 * System.Math.Atan2(q.X, q.W));
v.Y = 0;
}
return v;
}
public static Quaternion QuaternionInverse(Quaternion q)
{
return QuaternionInverse(ref q);
}
public static void Inverse(ref Quaternion quaternion, out Quaternion result)
{
// http://www.ncsa.illinois.edu/~kindr/emtc/quaternions/quaternion.c++
Quaternion conj = new Quaternion(quaternion.X, quaternion.Y, quaternion.Z, quaternion.W);
conj.Conjugate();
result = conj * (1.0f / quaternion.LengthSquared());
}
public static Quaternion QuaternionMultiply(Quaternion a, Quaternion b)
{
return QuaternionMultiply(ref a, ref b);
}
public static void Lerp(ref Quaternion quaternion1, ref Quaternion quaternion2, float amount, out Quaternion result)
{
Quaternion q1;
Multiply(ref quaternion1, 1.0f - amount, out q1);
Quaternion q2;
Multiply(ref quaternion2, amount, out q2);
Quaternion q1q2;
Add(ref q1, ref q2, out q1q2);
Normalize(ref q1q2, out result);
}
public static void PrintQuaternion(TextWriter writer, Quaternion q)
{
writer.Write(String.Format("{{X:{0:0.00000000} Y:{1:0.00000000} Z:{2:0.00000000} W:{3:0.00000000}}}", q.X, q.Y, q.Z,q.W));
}
public static Quaternion Normalize(Quaternion quaternion)
{
Normalize(ref quaternion, out quaternion);
return quaternion;
}
public static Quaternion QuatFurthest(ref Quaternion input1,ref Quaternion input2)
{
Quaternion diff,sum;
diff = input1 - input2;
sum = input1 + input2;
if( Quaternion.Dot(diff,diff) > Quaternion.Dot(sum,sum))
{
return input2;
}
return (-input2);
}
public static void Normalize(ref Quaternion quaternion, out Quaternion result)
{
// TODO: SIMD optimization
Multiply(ref quaternion, 1.0f / quaternion.Length(), out result);
}
public static Vector3 QuatRotate(ref Quaternion rotation, ref Vector3 v)
{
Quaternion q = QuatVectorMultiply(ref rotation,ref v);
q *= QuaternionInverse(ref rotation);
return new Vector3(q.X,q.Y,q.Z);
}
public static Quaternion Slerp(Quaternion quaternion1, Quaternion quaternion2, float amount)
{
Quaternion result;
Slerp(ref quaternion1, ref quaternion2, amount, out result);
return result;
}
public static extern Handle CreateChildSceneNode( Handle self, Vector3 translate, Quaternion rotate);
public static void Slerp(ref Quaternion quaternion1, ref Quaternion quaternion2, float amount, out Quaternion result)
{
float dot;
Dot(ref quaternion1, ref quaternion2, out dot);
Quaternion q3;
if (dot < 0.0f) {
dot = -dot;
Negate(ref quaternion2, out q3);
} else {
q3 = quaternion2;
}
if (dot < 0.999999f) {
float angle = (float)System.Math.Acos(dot);
float sin1 = (float)System.Math.Sin(angle * (1.0f - amount));
float sin2 = (float)System.Math.Sin(angle * amount);
float sin3 = (float)System.Math.Sin(angle);
Quaternion q1;
Multiply(ref quaternion1, sin1, out q1);
Quaternion q2;
Multiply(ref q3, sin2, out q2);
Quaternion q4;
Add(ref q1, ref q2, out q4);
Divide(ref q4, sin3, out result);
} else {
Lerp(ref quaternion1, ref q3, amount, out result);
}
}
public static extern void SetOrientation( Handle self, Quaternion orientation);
public bool Equals(Quaternion other)
{
return other == this;
}
