void vrpn_Tracker_WiimoteHead::_update_gravity_moving_avg() {
// Moving average of last three gravity vectors
/// @todo replace/supplement gravity moving average with Kalman filter
q_vec_type movingAvg = Q_NULL_VECTOR;
q_vec_copy(movingAvg, d_vGrav);
q_vec_add(movingAvg, movingAvg, d_vGravPenultimate);
q_vec_add(movingAvg, movingAvg, d_vGravAntepenultimate);
q_vec_scale(movingAvg, 0.33333, movingAvg);
// reset gravity transform
MAKE_IDENTITY_QUAT(d_gravityXform.quat);
MAKE_NULL_VEC(d_gravityXform.xyz);
q_vec_type regulargravity = Q_NULL_VECTOR;
regulargravity[2] = 1;
q_from_two_vecs(d_gravityXform.quat, movingAvg, regulargravity);
d_gravDirty = false;
}
void
q_xyz_quat_compose(q_xyz_quat_type *C_from_A_ptr,
q_xyz_quat_type *C_from_B_ptr,
q_xyz_quat_type *B_from_A_ptr)
{
q_vec_type rotated_BA_vec;
/* rotate local xlate into global */
q_xform(rotated_BA_vec, C_from_B_ptr->quat, B_from_A_ptr->xyz);
/* now add the xformed local vec to the unchanged global vec */
q_vec_add(C_from_A_ptr->xyz, C_from_B_ptr->xyz, rotated_BA_vec);
/* compose the rotations */
/* CA_rotate = CB_rotate . BA_rotate */
q_mult(C_from_A_ptr->quat, C_from_B_ptr->quat, B_from_A_ptr->quat);
q_normalize(C_from_A_ptr->quat, C_from_A_ptr->quat);
} /* qp_xyz_quat_compose */
