void quaternionMultiply(float a[4], float b[4], float out[4]){
float va[3], vb[3], crossAB[3];
float dotAB;
va[VEC3_X] = a[QUAT_X];
va[VEC3_Y] = a[QUAT_Y];
va[VEC3_Z] = a[QUAT_Z];
vb[VEC3_X] = b[QUAT_X];
vb[VEC3_Y] = b[QUAT_Y];
vb[VEC3_Z] = b[QUAT_Z];
dotAB = vector3DotProduct(va, vb);
vector3CrossProduct(va, vb, crossAB);
out[QUAT_W] = a[QUAT_W]*b[QUAT_W]-dotAB;
out[QUAT_X] = a[QUAT_W]*vb[VEC3_X]+b[QUAT_W]*va[VEC3_X]+crossAB[VEC3_X];
out[QUAT_Y] = a[QUAT_W]*vb[VEC3_Y]+b[QUAT_W]*va[VEC3_Y]+crossAB[VEC3_Y];
out[QUAT_Z] = a[QUAT_W]*vb[VEC3_Z]+b[QUAT_W]*va[VEC3_Z]+crossAB[VEC3_Z];
}
void quaternionMultiply(quaternion_t qa, quaternion_t qb, quaternion_t qd)
{
vector3d_t va;
vector3d_t vb;
float dotAB;
vector3d_t crossAB;
va[VEC3_X] = qa[QUAT_X];
va[VEC3_Y] = qa[QUAT_Y];
va[VEC3_Z] = qa[QUAT_Z];
vb[VEC3_X] = qb[QUAT_X];
vb[VEC3_Y] = qb[QUAT_Y];
vb[VEC3_Z] = qb[QUAT_Z];
vector3DotProduct(va, vb, &dotAB);
vector3CrossProduct(va, vb, crossAB);
qd[QUAT_W] = qa[QUAT_W] * qb[QUAT_W] - dotAB;
qd[QUAT_X] = qa[QUAT_W] * vb[VEC3_X] + qb[QUAT_W] * va[VEC3_X] + crossAB[VEC3_X];
qd[QUAT_Y] = qa[QUAT_W] * vb[VEC3_Y] + qb[QUAT_W] * va[VEC3_Y] + crossAB[VEC3_Y];
qd[QUAT_Z] = qa[QUAT_W] * vb[VEC3_Z] + qb[QUAT_W] * va[VEC3_Z] + crossAB[VEC3_Z];
}
