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; }