public static Matrix3D GetMatrix(Quaternion q)
{
double n1 = 2 * q.Y * q.Y;
double n2 = 2 * q.Z * q.Z;
double n3 = 2 * q.X * q.X;
double n4 = 2 * q.X * q.Y;
double n5 = 2 * q.W * q.Z;
double n6 = 2 * q.X * q.Z;
double n7 = 2 * q.W * q.Y;
double n8 = 2 * q.Y * q.Z;
double n9 = 2 * q.W * q.X;
Matrix3D result = Matrix3D.Identity;
result.M11 = 1 - n1 - n2;
result.M12 = n4 + n5;
result.M13 = n6 - n7;
result.M21 = n4 - n5;
result.M22 = 1 - n3 - n2;
result.M23 = n8 + n9;
result.M31 = n6 + n7;
result.M32 = n8 - n9;
result.M33 = 1 - n3 - n1;
result.M44 = 1;
return result;
}
/// <summary>
/// Calculates the updated offsets for a given frame
/// </summary>
/// <param name="frame"></param>
public void SetFrame(double[] frame, Quaternion prevRotation)
{
int childCount = Children.Count;
Quaternion result = prevRotation;
/*Matrix3D matrix = QuaternionRotation3D.GetMatrix(result);
matrix.TransformTo(OriginalOffset, Offset);*/
result.RotateTo(OriginalOffset, Offset);
if (childCount != 0) {
Quaternion rotX = Quaternion.XRotation(frame[XRotationIndex]);
Quaternion rotY = Quaternion.YRotation(frame[YRotationIndex]);
Quaternion rotZ = Quaternion.ZRotation(frame[ZRotationIndex]);
/*if (Name == "RightKnee") {
rotX = Quaternion.Identity;
rotY = Quaternion.Identity;
rotZ = Quaternion.Identity;
}*/
result = prevRotation * rotZ * rotX * rotY;
}
/* if (Parent != null) {
Quaternion rotX = Quaternion.XRotation(frame[Parent.XRotationIndex]);
Quaternion rotY = Quaternion.YRotation(frame[Parent.YRotationIndex]);
Quaternion rotZ = Quaternion.ZRotation(frame[Parent.ZRotationIndex]);
result = prevRotation * rotZ * rotX * rotY;
}
else {
result = prevRotation;
}*/
if (Parent == null) { // if root node
StartPointWorld.X = frame[0] + Offset.X;
StartPointWorld.Y = frame[1] + Offset.Y;
StartPointWorld.Z = frame[2] + Offset.Z;
}
else
{
StartPointWorld.X = Parent.StartPointWorld.X + Offset.X;
StartPointWorld.Y = Parent.StartPointWorld.Y + Offset.Y;
StartPointWorld.Z = Parent.StartPointWorld.Z + Offset.Z;
}
for (int i = 0; i < childCount; i++)
{
Children[i].SetFrame(frame, result);
}
}
public static Quaternion ZRotation(double angleInRadians)
{
double halfAngleInRadians = (angleInRadians * 0.5);
Quaternion q = new Quaternion(0, 0, Math.Sin(halfAngleInRadians), Math.Cos(halfAngleInRadians));
q.value = Matrix3D.Identity;
q.valueIsDirty = true;
return q;
}
public static Quaternion Subtract(Quaternion left, Quaternion right)
{
return left - right;
}
public static Quaternion Slerp(Quaternion from, Quaternion to, double t, bool useShortestPath)
{
//For more information on how this works see:
//Chapter 10 in "Essential Mathermatics for Games & Interactive Applications"
//James M. Van Verth & Lars M. Bishop
//Slerp(p,q,t) = (sin((1-t)angle) * p + sin(t*angle) * q) / sin(angle)
//Interpolate along a sphere between the two quaternions
//Find the angle between the two axis
double fromNorm = from.Norm;
double toNorm = to.Norm;
from.Normalize();
to.Normalize();
double fromDotTo = from.X * to.X + from.Y * to.Y + from.Z * to.Z + from.W * to.W;
if (useShortestPath) {
if (fromDotTo < 0) {
//Need to negate one of the quaternions
to = new Quaternion(-to.X, -to.Y, -to.Z, -to.W);
fromDotTo *= -1;
}
}
if (fromDotTo < -1) {
fromDotTo = -1;
}
else if (fromDotTo > 1) {
fromDotTo = 1;
}
double angleInRadians = System.Math.Acos(fromDotTo);
double sinAngle = System.Math.Sin(angleInRadians);
//Be careful about division by zero
if (MathHelper.IsZero(sinAngle)) {
//Just lerp instead of slerp
return new Quaternion(from.X + (to.X - from.X) * t,
from.Y + (to.Y - from.Y) * t,
from.Z + (to.Z - from.Z) * t,
from.W + (to.W - from.W) * t);
}
else {
double sinFrom = System.Math.Sin((1.0 - t) * angleInRadians) / sinAngle;
double sinTo = System.Math.Sin(t * angleInRadians) / sinAngle;
//Scale from FromNorm to toNorm depending on t. When t==0 then
//scale == fromNorm, when t==1 scale == toNorm
double scale = 1;//fromNorm * System.Math.Pow((toNorm / fromNorm), t);
return new Quaternion(scale * (from.X * sinFrom + to.X * sinTo),
scale * (from.Y * sinFrom + to.Y * sinTo),
scale * (from.Z * sinFrom + to.Z * sinTo),
scale * (from.W * sinFrom + to.W * sinTo));
}
}
public static Quaternion Slerp(Quaternion from, Quaternion to, double t)
{
return Slerp(from, to, t, true);
}
public static Quaternion Multiply(Quaternion left, Quaternion right)
{
return left * right;
}
static Quaternion()
{
Quaternion.identity = new Quaternion(0, 0, 0, 1);
}
public static Quaternion Add(Quaternion left, Quaternion right)
{
return left + right;
}
public QuaternionRotation3D(Quaternion quaternion)
{
this.Quaternion = quaternion;
}