public static Quaternion operator *(Quaternion q, Quaternion multiplier)
{
Quaternion rv = new Quaternion();
rv.X = q.X * multiplier.X - q.Y * multiplier.Y - q.Z * multiplier.Z - q.W * multiplier.W;
rv.Y = q.X * multiplier.Y + q.Y * multiplier.X + q.Z * multiplier.W - q.W * multiplier.Z;
rv.Z = q.X * multiplier.Z + q.Z * multiplier.X + q.W * multiplier.Y - q.Y * multiplier.W;
rv.W = q.X * multiplier.W + q.W * multiplier.X + q.Y * multiplier.Z - q.Z * multiplier.Y;
return rv;
}
public static Quaternion Slerp(float t, Quaternion q1, Quaternion q2)
{
float cosTheta = Dot(q1, q2);
if (cosTheta > 0.9995f)
{
return Normalize((1.0f - t) * q1 + t * q2);
}
else
{
float theta = MathHelper.Acos(MathHelper.Clamp(cosTheta, -1.0f, 1.0f));
float thetap = theta * t;
Quaternion qperp = Normalize(q2 - q1 * cosTheta);
return q1 * MathHelper.Cos(thetap) + qperp * MathHelper.Sin(thetap);
}
}
public static Matrix3 SetOrientation(Quaternion q, bool diff)
{
float[] data = new float[9];
data[0] = 1.0f - (2.0f * q.Z * q.Z + 2.0f * q.W * q.W);
data[1] = 2.0f * q.Y * q.Z + 2.0f * q.W * q.X;
data[2] = 2.0f * q.Y * q.W - 2.0f * q.Z * q.X;
data[3] = 2.0f * q.Y * q.Z - 2.0f * q.W * q.X;
data[4] = 1.0f - (2.0f * q.Y * q.Y + 2.0f * q.W * q.W);
data[5] = 2.0f * q.Z * q.W + 2.0f * q.Y * q.X;
data[6] = 2.0f * q.Y * q.W + 2.0f * q.Z * q.X;
data[7] = 2.0f * q.Z * q.W - 2.0f * q.Y * q.X;
data[8] = 1.0f - (2.0f * q.Y * q.Y + 2.0f * q.Z * q.Z);
return new Matrix3(data);
}
public void SetOrientation(Quaternion q)
{
data[0] = 1 - (2 * q.Z * q.Z + 2 * q.W * q.W);
data[1] = 2 * q.Y * q.Z + 2 * q.W * q.X;
data[2] = 2 * q.Y * q.W - 2 * q.Z * q.X;
data[3] = 2 * q.Y * q.Z - 2 * q.W * q.X;
data[4] = 1 - (2 * q.Y * q.Y + 2 * q.W * q.W);
data[5] = 2 * q.Z * q.W + 2 * q.Y * q.X;
data[6] = 2 * q.Y * q.W + 2 * q.Z * q.X;
data[7] = 2 * q.Z * q.W - 2 * q.Y * q.X;
data[8] = 1 - (2 * q.Y * q.Y + 2 * q.Z * q.Z);
}
static Matrix4 _calculateTransformMatrix(Vector3 position, Quaternion orientation)
{
Matrix4 transformMatrix = Matrix4.Identity;
transformMatrix[0] = 1.0f - 2.0f * orientation.Z * orientation.Z - 2.0f * orientation.W * orientation.W;
transformMatrix[1] = 2.0f * orientation.Y * orientation.Z - 2.0f * orientation.X * orientation.W;
transformMatrix[2] = 2.0f * orientation.Y * orientation.W + 2.0f * orientation.X * orientation.Z;
transformMatrix[3] = position.X;
transformMatrix[4] = 2.0f * orientation.Y * orientation.Z + 2.0f * orientation.X * orientation.W;
transformMatrix[5] = 1.0f - 2.0f * orientation.Y * orientation.Y - 2.0f * orientation.W * orientation.W;
transformMatrix[6] = 2.0f * orientation.Z * orientation.W - 2.0f * orientation.X * orientation.Y;
transformMatrix[7] = position.Y;
transformMatrix[8] = 2.0f * orientation.Y * orientation.W - 2.0f * orientation.X * orientation.Z;
transformMatrix[9] = 2.0f * orientation.Z * orientation.W + 2.0f * orientation.X * orientation.Y;
transformMatrix[10] = 1.0f - 2.0f * orientation.Y * orientation.Y - 2.0f * orientation.Z * orientation.Z;
transformMatrix[11] = position.Z;
return transformMatrix;
}
static Matrix3 _transformInertiaTensor(Quaternion q, Matrix3 iitBody, Matrix4 rotmat)
{
float t4 = rotmat[0] * iitBody[0] + rotmat[1] * iitBody[3] + rotmat[2] * iitBody[6];
float t9 = rotmat[0] * iitBody[1] + rotmat[1] * iitBody[4] + rotmat[2] * iitBody[7];
float t14 = rotmat[0] * iitBody[2] + rotmat[1] * iitBody[5] + rotmat[2] * iitBody[8];
float t28 = rotmat[4] * iitBody[0] + rotmat[5] * iitBody[3] + rotmat[6] * iitBody[6];
float t33 = rotmat[4] * iitBody[1] + rotmat[5] * iitBody[4] + rotmat[6] * iitBody[7];
float t38 = rotmat[4] * iitBody[2] + rotmat[5] * iitBody[5] + rotmat[6] * iitBody[8];
float t52 = rotmat[8] * iitBody[0] + rotmat[9] * iitBody[3] + rotmat[10] * iitBody[6];
float t57 = rotmat[8] * iitBody[1] + rotmat[9] * iitBody[4] + rotmat[10] * iitBody[7];
float t62 = rotmat[8] * iitBody[2] + rotmat[9] * iitBody[5] + rotmat[10] * iitBody[8];
Matrix3 iitWorld = Matrix3.Identity;
iitWorld[0] = t4 * rotmat[0] + t9 * rotmat[1] + t14 * rotmat[2];
iitWorld[1] = t4 * rotmat[4] + t9 * rotmat[5] + t14 * rotmat[6];
iitWorld[2] = t4 * rotmat[8] + t9 * rotmat[9] + t14 * rotmat[10];
iitWorld[3] = t28 * rotmat[0] + t33 * rotmat[1] + t38 * rotmat[2];
iitWorld[4] = t28 * rotmat[4] + t33 * rotmat[5] + t38 * rotmat[6];
iitWorld[5] = t28 * rotmat[8] + t33 * rotmat[9] + t38 * rotmat[10];
iitWorld[6] = t52 * rotmat[0] + t57 * rotmat[1] + t62 * rotmat[2];
iitWorld[7] = t52 * rotmat[4] + t57 * rotmat[5] + t62 * rotmat[6];
iitWorld[8] = t52 * rotmat[8] + t57 * rotmat[9] + t62 * rotmat[10];
return iitWorld;
}
public void SetOrientation(Quaternion quat)
{
Orientation = quat;
Orientation.Normalize();
}
static internal void _calculateTransformMatrix(out Matrix4 transformMatrix, Vector3 position, Quaternion orientation)
{
transformMatrix = Matrix4.Identity;
transformMatrix[0] = 1.0f - 2.0f * orientation.Z * orientation.Z -
2.0f * orientation.W * orientation.W;
transformMatrix[1] = 2.0f * orientation.Y * orientation.Z -
2.0f * orientation.X * orientation.W;
transformMatrix[2] = 2.0f * orientation.Y * orientation.W +
2.0f * orientation.X * orientation.Z;
transformMatrix[3] = position.X;
transformMatrix[4] = 2.0f * orientation.Y * orientation.Z +
2.0f * orientation.X * orientation.W;
transformMatrix[5] = 1.0f - 2.0f * orientation.Y * orientation.Y -
2.0f * orientation.W * orientation.W;
transformMatrix[6] = 2.0f * orientation.Z * orientation.W -
2.0f * orientation.X * orientation.Y;
transformMatrix[7] = position.Y;
transformMatrix[8] = 2.0f * orientation.Y * orientation.W -
2.0f * orientation.X * orientation.Z;
transformMatrix[9] = 2.0f * orientation.Z * orientation.W +
2.0f * orientation.X * orientation.Y;
transformMatrix[10] = 1.0f - 2.0f * orientation.Y * orientation.Y -
2.0f * orientation.Z * orientation.Z;
transformMatrix[11] = position.Z;
}
public static float Dot(Quaternion q1, Quaternion q2)
{
return q1.X * q2.X + q1.Y * q2.Y + q1.Z * q2.Z + q1.W * q2.W;
}
public static Quaternion Normalize(Quaternion quat)
{
Quaternion q = quat;
q.Normalize();
return q;
}
public void Assignthis(Quaternion quat)
{
X = quat.X;
Y = quat.Y;
Z = quat.Z;
W = quat.W;
}
public void AddScaledVector(Vector3 vector, float scale)
{
Quaternion q = new Quaternion(0, vector.X * scale, vector.Y * scale, vector.Z * scale);
q *= this;
X += q.X * (0.5f);
Y += q.Y * (0.5f);
Z += q.Z * (0.5f);
W += q.W * (0.5f);
}
public void RotateByVector(Vector3 vector)
{
Quaternion q = new Quaternion(0, vector.X, vector.Y, vector.Z);
Assignthis(this * q);
}
public static Matrix4 SetOrientationAndPosition(Quaternion q, Vector3 pos)
{
float[] data = new float[12];
data[0] = 1 - (2 * q.Z * q.Z + 2 * q.W * q.W);
data[1] = 2 * q.Y * q.Z + 2 * q.W * q.X;
data[2] = 2 * q.Y * q.W - 2 * q.Z * q.X;
data[3] = pos.X;
data[4] = 2 * q.Y * q.Z - 2 * q.W * q.X;
data[5] = 1 - (2 * q.Y * q.Y + 2 * q.W * q.W);
data[6] = 2 * q.Z * q.W + 2 * q.Y * q.X;
data[7] = pos.Y;
data[8] = 2 * q.Y * q.W + 2 * q.Z * q.X;
data[9] = 2 * q.Z * q.W - 2 * q.Y * q.X;
data[10] = 1 - (2 * q.Y * q.Y + 2 * q.Z * q.Z);
data[11] = pos.Z;
return new Matrix4(data);
}
public void SetOrientationAndPos(Quaternion q, Vector3 pos)
{
data[0] = 1 - (2 * q.Z * q.Z + 2 * q.W * q.W);
data[1] = 2 * q.Y * q.Z + 2 * q.W * q.X;
data[2] = 2 * q.Y * q.W - 2 * q.Z * q.X;
data[3] = pos.X;
data[4] = 2 * q.Y * q.Z - 2 * q.W * q.X;
data[5] = 1 - (2 * q.Y * q.Y + 2 * q.W * q.W);
data[6] = 2 * q.Z * q.W + 2 * q.Y * q.X;
data[7] = pos.Y;
data[8] = 2 * q.Y * q.W + 2 * q.Z * q.X;
data[9] = 2 * q.Z * q.W - 2 * q.Y * q.X;
data[10] = 1 - (2 * q.Y * q.Y + 2 * q.Z * q.Z);
data[11] = pos.Z;
}
