/// Interpolates the values at three 2D points to the
/// location of a third point.
static double interpolate(double p1X, double p1Y, double val1, double p2X,
double p2Y, double val2, double p3X, double p3Y,
double val3, double pointX, double pointY) {
// Fit a plane to three points, using their values as the
// third dimension.
q_vec_type p1, p2, p3;
q_vec_set(p1, p1X, p1Y, val1);
q_vec_set(p2, p2X, p2Y, val2);
q_vec_set(p3, p3X, p3Y, val3);
// The normalized cross product of the vectors from the first
// point to each of the other two is normal to this plane.
q_vec_type v1, v2;
q_vec_subtract(v1, p2, p1);
q_vec_subtract(v2, p3, p1);
q_vec_type ABC;
q_vec_cross_product(ABC, v1, v2);
if (q_vec_magnitude(ABC) == 0) {
// We can't get a normal, degenerate points, just return the first
// value.
return val1;
}
q_vec_normalize(ABC, ABC);
// Solve for the D associated with the plane by filling back
// in one of the points. This is done by taking the dot product
// of the ABC vector with the first point. We then solve for D.
// AX + BY + CZ + D = 0; D = -(AX + BY + CZ)
double D = -q_vec_dot_product(ABC, p1);
// Evaluate the plane equations at our input point, which will interpolate
// or extrapolate our values.
// We're solving for Z in this case, so we get
// CZ = -(AX + BY + D); Z = -(AX + BY + D)/C;
return -(ABC[0] * pointX + ABC[1] * pointY + D) / ABC[2];
}
void vrpn_IMU_SimpleCombiner::update_matrix_based_on_values(double time_interval)
{
//==================================================================
// Adjust the orientation based on the rotational velocity, which is
// measured in the rotated coordinate system. We need to rotate the
// difference vector back to the canonical orientation, apply the
// orientation change there, and then rotate back.
// Be sure to scale by the time value.
q_type forward, inverse;
q_copy(forward, d_quat);
q_invert(inverse, forward);
// Remember that Euler angles in Quatlib have rotation around Z in
// the first term. Compute the time-scaled delta transform in
// canonical space.
q_type delta;
q_from_euler(delta,
time_interval * d_rotational_vel.values[Q_Z],
time_interval * d_rotational_vel.values[Q_Y],
time_interval * d_rotational_vel.values[Q_X]);
// Bring the delta back into canonical space
q_type canonical;
q_mult(canonical, delta, inverse);
q_mult(canonical, forward, canonical);
q_mult(d_quat, canonical, d_quat);
//==================================================================
// To the extent that the acceleration vector's magnitude is equal
// to the expected gravity, rotate the orientation so that the vector
// points downward. This is measured in the rotated coordinate system,
// so we need to rotate back to canonical orientation and apply
// the difference there and then rotate back. The rate of rotation
// should be as specified in the gravity-rotation-rate parameter so
// we don't swing the head around too quickly but only slowly re-adjust.
double accel = sqrt(
d_acceleration.values[0] * d_acceleration.values[0] +
d_acceleration.values[1] * d_acceleration.values[1] +
d_acceleration.values[2] * d_acceleration.values[2] );
double diff = fabs(accel - 9.80665);
// Only adjust if we're close enough to the expected gravity
// @todo In a more complicated IMU tracker, base this on state and
// error estimates from a Kalman or other filter.
double scale = 1.0 - diff;
if (scale > 0) {
// Get a new forward and inverse matrix from the current, just-
// rotated matrix.
q_copy(forward, d_quat);
// Change how fast we adjust based on how close we are to the
// expected value of gravity. Then further scale this by the
// amount of time since the last estimate.
double gravity_scale = scale * d_gravity_restore_rate * time_interval;
// Rotate the gravity vector by the estimated transform.
// We expect this direction to match the global down (-Y) vector.
q_vec_type gravity_global;
q_vec_set(gravity_global, d_acceleration.values[0],
d_acceleration.values[1], d_acceleration.values[2]);
q_vec_normalize(gravity_global, gravity_global);
q_xform(gravity_global, forward, gravity_global);
//printf(" XXX Gravity: %lg, %lg, %lg\n", gravity_global[0], gravity_global[1], gravity_global[2]);
// Determine the rotation needed to take gravity and rotate
// it into the direction of -Y.
q_vec_type neg_y;
q_vec_set(neg_y, 0, -1, 0);
q_type rot;
q_from_two_vecs(rot, gravity_global, neg_y);
// Scale the rotation by the fraction of the orientation we
// should remove based on the time that has passed, how well our
// gravity vector matches expected, and the specified rate of
// correction.
static q_type identity = { 0, 0, 0, 1 };
q_type scaled_rot;
q_slerp(scaled_rot, identity, rot, gravity_scale);
//printf("XXX Scaling gravity vector by %lg\n", gravity_scale);
// Rotate by this angle.
q_mult(d_quat, scaled_rot, d_quat);
//==================================================================
// If we are getting compass data, and to the extent that the
// acceleration vector's magnitude is equal to the expected gravity,
// compute the cross product of the cross product to find the
// direction of north perpendicular to down. This is measured in
// the rotated coordinate system, so we need to rotate back to the
// canonical orientation and do the comparison there.
// The fraction of rotation should be as specified in the
// magnetometer-rotation-rate parameter so we don't swing the head
// around too quickly but only slowly re-adjust.
if (d_magnetometer.ana != NULL) {
// Get a new forward and inverse matrix from the current, just-
// rotated matrix.
q_copy(forward, d_quat);
// Find the North vector that is perpendicular to gravity by
// clearing its Y axis to zero and renormalizing.
q_vec_type magnetometer;
q_vec_set(magnetometer, d_magnetometer.values[0],
d_magnetometer.values[1], d_magnetometer.values[2]);
q_vec_type magnetometer_global;
q_xform(magnetometer_global, forward, magnetometer);
magnetometer_global[Q_Y] = 0;
q_vec_type north_global;
q_vec_normalize(north_global, magnetometer_global);
//printf(" XXX north_global: %lg, %lg, %lg\n", north_global[0], north_global[1], north_global[2]);
// Determine the rotation needed to take north and rotate it
// into the direction of negative Z.
q_vec_type neg_z;
q_vec_set(neg_z, 0, 0, -1);
q_from_two_vecs(rot, north_global, neg_z);
// Change how fast we adjust based on how close we are to the
// expected value of gravity. Then further scale this by the
// amount of time since the last estimate.
double north_rate = scale * d_north_restore_rate * time_interval;
// Scale the rotation by the fraction of the orientation we
// should remove based on the time that has passed, how well our
// gravity vector matches expected, and the specified rate of
// correction.
static q_type identity = { 0, 0, 0, 1 };
q_slerp(scaled_rot, identity, rot, north_rate);
// Rotate by this angle.
q_mult(d_quat, scaled_rot, d_quat);
}
}
//==================================================================
// Compute and store the velocity information
// This will be in the rotated coordinate system, so we need to
// rotate back to the identity orientation before storing.
// Convert from radians/second into a quaternion rotation as
// rotated in a hundredth of a second and set the rotational
// velocity dt to a hundredth of a second so that we don't
// risk wrapping.
// Remember that Euler angles in Quatlib have rotation around Z in
// the first term. Compute the time-scaled delta transform in
// canonical space.
q_from_euler(delta,
1e-2 * d_rotational_vel.values[Q_Z],
1e-2 * d_rotational_vel.values[Q_Y],
1e-2 * d_rotational_vel.values[Q_X]);
// Bring the delta back into canonical space
q_mult(canonical, delta, inverse);
q_mult(vel_quat, forward, canonical);
vel_quat_dt = 1e-2;
}
