public static IndexedMatrix CreateFromQuaternion(ref IndexedQuaternion q)
{
IndexedMatrix i = new IndexedMatrix();
i._basis.SetRotation(ref q);
return(i);
}
public IndexedVector3 QuatRotate(IndexedQuaternion rotation, IndexedVector3 v)
{
IndexedQuaternion q = rotation * v;
q *= rotation.Inverse();
return(new IndexedVector3(q.X, q.Y, q.Z));
}
public IndexedBasisMatrix(ref IndexedQuaternion q)
{
float d = q.LengthSquared();
Debug.Assert(d != 0.0f);
float s = 2.0f / d;
float xs = q.X * s, ys = q.Y * s, zs = q.Z * s;
float wx = q.W * xs, wy = q.W * ys, wz = q.W * zs;
float xx = q.X * xs, xy = q.X * ys, xz = q.X * zs;
float yy = q.Y * ys, yz = q.Y * zs, zz = q.Z * zs;
_el0 = new IndexedVector3(1.0f - (yy + zz), xy - wz, xz + wy);
_el1 = new IndexedVector3(xy + wz, 1.0f - (xx + zz), yz - wx);
_el2 = new IndexedVector3(xz - wy, yz + wx, 1.0f - (xx + yy));
}
public void SetRotation(ref IndexedQuaternion q)
{
float d = q.LengthSquared();
Debug.Assert(d != 0.0f);
float s = 2.0f / d;
float xs = q.X * s, ys = q.Y * s, zs = q.Z * s;
float wx = q.W * xs, wy = q.W * ys, wz = q.W * zs;
float xx = q.X * xs, xy = q.X * ys, xz = q.X * zs;
float yy = q.Y * ys, yz = q.Y * zs, zz = q.Z * zs;
SetValue(1.0f - (yy + zz), xy - wz, xz + wy,
xy + wz, 1.0f - (xx + zz), yz - wx,
xz - wy, yz + wx, 1.0f - (xx + yy));
}
public void SetRotation(ref IndexedQuaternion q)
{
_basis.SetRotation(ref q);
}
public static float Dot(IndexedQuaternion q, IndexedQuaternion q2)
{
return(q.X * q2.X + q.Y * q2.Y + q.Z * q2.Z + q.W * q2.W);
}
public float Dot(IndexedQuaternion q)
{
return(X * q.X + Y * q.Y + Z * q.Z + W * q.W);
}
public static IndexedQuaternion Inverse(IndexedQuaternion q)
{
return(new IndexedQuaternion(-q.X, -q.Y, -q.Z, q.W));
}