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];
}
Esempio n. 2
0
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];
}