/** * bot_quat_rotate_rev: * @rot: Unit quaternion that specifies the rotation. * @v: 3-vector that is rotated according to the quaternion @rot * and modified in place with the result. * * Rotates a vector @v from one coordinate system to another as * specified by a unit quaternion @rot, but performs the rotation * in the reverse direction of bot_quat_rotate(). */ void bot_quat_rotate_rev (const double rot[4], double v[3]) { double a[4], b[4], c[4]; b[0] = 0; memcpy (b+1, v, 3 * sizeof (double)); bot_quat_mult (a, b, rot); b[0] = rot[0]; b[1] = -rot[1]; b[2] = -rot[2]; b[3] = -rot[3]; bot_quat_mult (c, b, a); memcpy (v, c+1, 3 * sizeof (double)); }
void bot_trans_apply_trans(BotTrans *dest, const BotTrans * src) { bot_quat_rotate(src->rot_quat, dest->trans_vec); double qtmp[4]; bot_quat_mult(qtmp, src->rot_quat, dest->rot_quat); memcpy(dest->rot_quat, qtmp, 4*sizeof(double)); dest->trans_vec[0] += src->trans_vec[0]; dest->trans_vec[1] += src->trans_vec[1]; dest->trans_vec[2] += src->trans_vec[2]; }
int bot_quaternion_test() { #define FAIL_TEST { fprintf(stderr, "bot_quaternion_test failed at line %d\n", \ __LINE__); return 0; } fprintf(stderr, "running quaternion test\n"); double theta = 0; double rvec[] = { 0, 0, 1 }; double q[4]; double rpy[3]; double roll, pitch, yaw; bot_angle_axis_to_quat(theta, rvec, q); if (! qeq (q, 1, 0, 0, 0)) FAIL_TEST; bot_quat_to_roll_pitch_yaw (q, rpy); roll = rpy[0]; pitch = rpy[1]; yaw = rpy[2]; if (! rpyeq (roll,pitch,yaw, 0,0,0)) FAIL_TEST; // quat_from_angle_axis theta = M_PI; bot_angle_axis_to_quat(theta, rvec, q); fprintf(stderr,"<%.3f, %.3f, %.3f, %.3f>\n", q[0], q[1], q[2], q[3]); if (! qeq (q, 0, 0, 0, 1)) FAIL_TEST; // bot_quat_to_angle_axis bot_quat_to_angle_axis (q, &theta, rvec); if (!feq (theta, M_PI)) FAIL_TEST; if (!feq(rvec[0], 0) || !feq(rvec[1], 0) || !feq(rvec[2], 1)) FAIL_TEST; bot_quat_to_roll_pitch_yaw (q, rpy); if (! rpyeq (roll,pitch,yaw, 0,0,M_PI)) FAIL_TEST; double q2[4]; double q3[4]; double axis1[] = { 0, 1, 0 }; double axis2[] = { 0, 0, 1 }; bot_angle_axis_to_quat (M_PI/2, axis1, q); bot_angle_axis_to_quat (M_PI/2, axis2, q); bot_quat_mult (q3, q, q2); rvec[0] = 0; rvec[1] = 0; rvec[2] = 1; bot_quat_rotate (q, rvec); fprintf(stderr, "by q: [ %.2f, %.2f, %.2f ]\n", rvec[0], rvec[1], rvec[2]); rvec[0] = 0; rvec[1] = 0; rvec[2] = 1; bot_quat_rotate (q2, rvec); fprintf(stderr, "by q2: [ %.2f, %.2f, %.2f ]\n", rvec[0], rvec[1], rvec[2]); rvec[0] = 0; rvec[1] = 0; rvec[2] = 1; bot_quat_rotate (q3, rvec); fprintf(stderr, "by q*q2: [ %.2f, %.2f, %.2f ]\n", rvec[0], rvec[1], rvec[2]); rvec[0] = 0; rvec[1] = 0; rvec[2] = 1; bot_quat_mult (q3, q2, q); bot_quat_rotate (q3, rvec); fprintf(stderr, "by q2*q: [ %.2f, %.2f, %.2f ]\n", rvec[0], rvec[1], rvec[2]); // TODO #undef FAIL_TEST fprintf(stderr, "bot_quaternion_test complete\n"); return 1; }