/**
* 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;
}
