// function compute the quaternion product conjg(qA) * qB
public static fquaternion qconjgAxB(fquaternion qA, fquaternion qB)
{
fquaternion qProd;
qProd.q0 = qA.q0 * qB.q0 + qA.q1 * qB.q1 + qA.q2 * qB.q2 + qA.q3 * qB.q3;
qProd.q1 = qA.q0 * qB.q1 - qA.q1 * qB.q0 - qA.q2 * qB.q3 + qA.q3 * qB.q2;
qProd.q2 = qA.q0 * qB.q2 + qA.q1 * qB.q3 - qA.q2 * qB.q0 - qA.q3 * qB.q1;
qProd.q3 = qA.q0 * qB.q3 - qA.q1 * qB.q2 + qA.q2 * qB.q1 - qA.q3 * qB.q0;
return(qProd);
}
// function compute the quaternion product conjg(qA) * qB
public static fquaternion qconjgAxB(fquaternion qA, fquaternion qB)
{
fquaternion qProd;
qProd.q0 = qA.q0 * qB.q0 + qA.q1 * qB.q1 + qA.q2 * qB.q2 + qA.q3 * qB.q3;
qProd.q1 = qA.q0 * qB.q1 - qA.q1 * qB.q0 - qA.q2 * qB.q3 + qA.q3 * qB.q2;
qProd.q2 = qA.q0 * qB.q2 + qA.q1 * qB.q3 - qA.q2 * qB.q0 - qA.q3 * qB.q1;
qProd.q3 = qA.q0 * qB.q3 - qA.q1 * qB.q2 + qA.q2 * qB.q1 - qA.q3 * qB.q0;
return qProd;
}
// function computes the rotation matrix from an orientation quaternion
public static void fRotationMatrixFromQuaternion(ref double[,] R, ref fquaternion q)
{
double f2q;
double f2q0q0, f2q0q1, f2q0q2, f2q0q3;
double f2q1q1, f2q1q2, f2q1q3;
double f2q2q2, f2q2q3;
double f2q3q3;
// calculate products
f2q = 2.0F * q.q0;
f2q0q0 = f2q * q.q0;
f2q0q1 = f2q * q.q1;
f2q0q2 = f2q * q.q2;
f2q0q3 = f2q * q.q3;
f2q = 2.0F * q.q1;
f2q1q1 = f2q * q.q1;
f2q1q2 = f2q * q.q2;
f2q1q3 = f2q * q.q3;
f2q = 2.0F * q.q2;
f2q2q2 = f2q * q.q2;
f2q2q3 = f2q * q.q3;
f2q3q3 = 2.0F * q.q3 * q.q3;
// calculate the rotation matrix assuming the quaternion is normalized
R[X, X] = f2q0q0 + f2q1q1 - 1.0F;
R[X, Y] = f2q1q2 + f2q0q3;
R[X, Z] = f2q1q3 - f2q0q2;
R[Y, X] = f2q1q2 - f2q0q3;
R[Y, Y] = f2q0q0 + f2q2q2 - 1.0F;
R[Y, Z] = f2q2q3 + f2q0q1;
R[Z, X] = f2q1q3 + f2q0q2;
R[Z, Y] = f2q2q3 - f2q0q1;
R[Z, Z] = f2q0q0 + f2q3q3 - 1.0F;
return;
}
// function computes the rotation matrix from an orientation quaternion
public static void fRotationMatrixFromQuaternion(ref double[,] R, ref fquaternion q)
{
double f2q;
double f2q0q0, f2q0q1, f2q0q2, f2q0q3;
double f2q1q1, f2q1q2, f2q1q3;
double f2q2q2, f2q2q3;
double f2q3q3;
// calculate products
f2q = 2.0F * q.q0;
f2q0q0 = f2q * q.q0;
f2q0q1 = f2q * q.q1;
f2q0q2 = f2q * q.q2;
f2q0q3 = f2q * q.q3;
f2q = 2.0F * q.q1;
f2q1q1 = f2q * q.q1;
f2q1q2 = f2q * q.q2;
f2q1q3 = f2q * q.q3;
f2q = 2.0F * q.q2;
f2q2q2 = f2q * q.q2;
f2q2q3 = f2q * q.q3;
f2q3q3 = 2.0F * q.q3 * q.q3;
// calculate the rotation matrix assuming the quaternion is normalized
R[X, X] = f2q0q0 + f2q1q1 - 1.0F;
R[X, Y] = f2q1q2 + f2q0q3;
R[X, Z] = f2q1q3 - f2q0q2;
R[Y, X] = f2q1q2 - f2q0q3;
R[Y, Y] = f2q0q0 + f2q2q2 - 1.0F;
R[Y, Z] = f2q2q3 + f2q0q1;
R[Z, X] = f2q1q3 + f2q0q2;
R[Z, Y] = f2q2q3 - f2q0q1;
R[Z, Z] = f2q0q0 + f2q3q3 - 1.0F;
return;
}
