void
bot_trans_set_from_velocities(BotTrans *dest, const double angular_rate[3],
const double velocity[3], double dt)
{
//see Frazolli notes, Aircraft Stability and Control, lecture 2, page 15
if (dt == 0) {
bot_trans_set_identity(dest);
return;
}
double identity_quat[4] = { 1, 0, 0, 0 };
double norm_angular_rate = bot_vector_magnitude_3d(angular_rate);
if (norm_angular_rate == 0) {
double delta[3];
memcpy(delta, velocity, 3 * sizeof(double));
bot_vector_scale_3d(delta, dt);
bot_trans_set_from_quat_trans(dest, identity_quat, delta);
return;
}
//"exponential of a twist: R=exp(skew(omega*t));
//rescale vel, omega, t so ||omega||=1
//trans = (I-R)*(omega \cross v) + (omega \dot v) *omega* t
// term2 term3
// R*(omega \cross v)
// term1
//rescale
double t = dt * norm_angular_rate;
double omega[3], vel[3];
memcpy(omega, angular_rate, 3 * sizeof(double));
memcpy(vel, velocity, 3 * sizeof(double));
bot_vector_scale_3d(omega, 1.0/norm_angular_rate);
bot_vector_scale_3d(vel, 1.0/norm_angular_rate);
//compute R (quat in our case)
bot_angle_axis_to_quat(t, omega, dest->rot_quat);
//cross and dot products
double omega_cross_vel[3];
bot_vector_cross_3d(omega, vel, omega_cross_vel);
double omega_dot_vel = bot_vector_dot_3d(omega, vel);
double term1[3];
double term2[3];
double term3[3];
//(I-R)*(omega \cross v) = term2
memcpy(term1, omega_cross_vel, 3 * sizeof(double));
bot_quat_rotate(dest->rot_quat, term1);
bot_vector_subtract_3d(omega_cross_vel, term1, term2);
//(omega \dot v) *omega* t
memcpy(term3, omega, 3 * sizeof(double));
bot_vector_scale_3d(term3, omega_dot_vel * t);
bot_vector_add_3d(term2, term3, dest->trans_vec);
}
void
bot_angle_axis_to_roll_pitch_yaw (double theta, const double axis[3],
double rpy[3])
{
double q[4];
bot_angle_axis_to_quat (theta, axis, q);
bot_quat_to_roll_pitch_yaw (q, rpy);
}
/* ================ general ============== */
int bot_param_get_quat(BotParam *param, const char *name, double quat[4])
{
char key[2048];
sprintf(key, "%s.quat", name);
if (bot_param_has_key(param, key)) {
int sz = bot_param_get_double_array(param, key, quat, 4);
assert(sz == 4);
return 0;
}
sprintf(key, "%s.rpy", name);
if (bot_param_has_key(param, key)) {
double rpy[3];
int sz = bot_param_get_double_array(param, key, rpy, 3);
assert(sz == 3);
for (int i = 0; i < 3; i++)
rpy[i] = bot_to_radians (rpy[i]);
bot_roll_pitch_yaw_to_quat(rpy, quat);
return 0;
}
sprintf(key, "%s.rodrigues", name);
if (bot_param_has_key(param, key)) {
double rod[3];
int sz = bot_param_get_double_array(param, key, rod, 3);
assert(sz == 3);
bot_rodrigues_to_quat(rod, quat);
return 0;
}
sprintf(key, "%s.angleaxis", name);
if (bot_param_has_key(param, key)) {
double aa[4];
int sz = bot_param_get_double_array(param, key, aa, 4);
assert(sz == 4);
bot_angle_axis_to_quat(aa[0], aa + 1, quat);
return 0;
}
return -1;
}
static void update_follow_target(BotViewHandler *vhandler, const double pos[3], const double quat[4])
{
BotDefaultViewHandler *dvh = (BotDefaultViewHandler*) vhandler->user;
if (dvh->have_last && (vhandler->follow_mode & BOT_FOLLOW_YAW)) {
// compute the vectors from the vehicle to the lookat and eye
// point and then project them given the new position of the car/
double v2eye[3];
bot_vector_subtract_3d(dvh->lastpos, dvh->eye, v2eye);
double v2look[3];
bot_vector_subtract_3d(dvh->lastpos, dvh->lookat, v2look);
double vxy[3] = { 1, 0, 0 };
bot_quat_rotate (quat, vxy);
vxy[2] = 0;
// where was the car pointing last time?
double oxy[3] = { 1, 0, 0 };
bot_quat_rotate (dvh->lastquat, oxy);
oxy[2] = 0;
double theta = bot_vector_angle_2d (oxy, vxy);
double zaxis[3] = { 0, 0, 1 };
double q[4];
bot_angle_axis_to_quat (theta, zaxis, q);
bot_quat_rotate(q, v2look);
bot_vector_subtract_3d(pos, v2look, dvh->lookat);
bot_quat_rotate(q, v2eye);
bot_vector_subtract_3d(pos, v2eye, dvh->eye);
bot_quat_rotate (q, dvh->up);
// the above algorithm "builds in" a BOT_FOLLOW_POS behavior.
} else if (dvh->have_last && (vhandler->follow_mode & BOT_FOLLOW_POS)) {
double dpos[3];
for (int i = 0; i < 3; i++)
dpos[i] = pos[i] - dvh->lastpos[i];
for (int i = 0; i < 3; i++) {
dvh->eye[i] += dpos[i];
dvh->lookat[i] += dpos[i];
}
} else {
// when the follow target moves, we want rotations to still behave
// correctly. Rotations use the lookat point as the center of rotation,
// so adjust the lookat point so that it is on the same plane as the
// follow target.
double look_dir[3];
bot_vector_subtract_3d(dvh->lookat, dvh->eye, look_dir);
bot_vector_normalize_3d(look_dir);
if (fabs(look_dir[2]) > 0.0001) {
double dist = (dvh->lastpos[2] - dvh->lookat[2]) / look_dir[2];
for (int i = 0; i < 3; i++)
dvh->lookat[i] += dist * look_dir[i];
}
}
look_at_changed(dvh);
dvh->have_last = 1;
memcpy(dvh->lastpos, pos, 3 * sizeof(double));
memcpy(dvh->lastquat, quat, 4 * sizeof(double));
}
static int mouse_motion (BotViewer *viewer, BotEventHandler *ehandler,
const double ray_start[3], const double ray_dir[3], const GdkEventMotion *event)
{
BotDefaultViewHandler *dvh = (BotDefaultViewHandler*) ehandler->user;
double dy = event->y - dvh->last_mouse_y;
double dx = event->x - dvh->last_mouse_x;
double look[3];
bot_vector_subtract_3d (dvh->lookat, dvh->eye, look);
if (event->state & GDK_BUTTON3_MASK) {
double xy_len = bot_vector_magnitude_2d (look);
double init_elevation = atan2 (look[2], xy_len);
double left[3];
bot_vector_cross_3d (dvh->up, look, left);
bot_vector_normalize_3d (left);
double delevation = -dy * 0.005;
if (delevation + init_elevation < -M_PI/2) {
delevation = -M_PI/2 - init_elevation;
}
if (delevation + init_elevation > M_PI/8) {
delevation = M_PI/8 - init_elevation;
}
double q[4];
bot_angle_axis_to_quat(-delevation, left, q);
double newlook[3];
memcpy (newlook, look, sizeof (newlook));
bot_quat_rotate (q, newlook);
double dazimuth = -dx * 0.01;
double zaxis[] = { 0, 0, 1 };
bot_angle_axis_to_quat (dazimuth, zaxis, q);
bot_quat_rotate (q, newlook);
bot_quat_rotate (q, left);
bot_vector_subtract_3d (dvh->lookat, newlook, dvh->eye);
bot_vector_cross_3d (newlook, left, dvh->up);
look_at_changed(dvh);
}
else if (event->state & GDK_BUTTON2_MASK) {
double init_eye_dist = bot_vector_magnitude_3d (look);
double ded = pow (10, dy * 0.01);
double eye_dist = init_eye_dist * ded;
if (eye_dist > EYE_MAX_DIST)
eye_dist = EYE_MAX_DIST;
else if (eye_dist < EYE_MIN_DIST)
eye_dist = EYE_MIN_DIST;
double le[3];
memcpy (le, look, sizeof (le));
bot_vector_normalize_3d (le);
bot_vector_scale_3d (le, eye_dist);
bot_vector_subtract_3d (dvh->lookat, le, dvh->eye);
look_at_changed(dvh);
}
else if (event->state & GDK_BUTTON1_MASK) {
double dx = event->x - dvh->last_mouse_x;
double dy = event->y - dvh->last_mouse_y;
window_space_pan(dvh, dvh->manipulation_point, dx, dy, 1);
}
dvh->last_mouse_x = event->x;
dvh->last_mouse_y = event->y;
bot_viewer_request_redraw(viewer);
return 1;
}
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;
}
