void sol_body_e(float e[4], const struct s_vary *vary, const struct v_body *bp, float dt) { if (bp->mj >= 0) { const struct v_move *mp = vary->mv + bp->mj; const struct b_path *pp = vary->base->pv + mp->pi; const struct b_path *pq = vary->base->pv + pp->pi; if (pp->fl & P_ORIENTED || pq->fl & P_ORIENTED) { float s; if (vary->pv[mp->pi].f) s = (mp->t + dt) / pp->t; else s = mp->t / pp->t; q_slerp(e, pp->e, pq->e, pp->s ? erp(s) : s); return; } } e[0] = 1.0f; e[1] = 0.0f; e[2] = 0.0f; e[3] = 0.0f; }
void sm_frame(sm_t *sm,int from,int to,float frame) { int i,frame0,frame1; if(from == -1) from = 0; if(to == -1) to = sm->num_frame; frame0 = (int)frame; frame -= frame0; frame0 += from; if(frame0 >= to) frame0 = ((frame0 - from) % (to - from)) + from; frame1 = frame0 + 1; if(frame1 >= to) frame1 = from; for(i = 0; i < sm->num_bone; i++) { float m0[16],m1[16]; float xyz0[3],xyz1[3],xyz[3],rot[4]; v_scale(sm->frame[frame0][i].xyz,1.0 - frame,xyz0); v_scale(sm->frame[frame1][i].xyz,frame,xyz1); v_add(xyz0,xyz1,xyz); q_slerp(sm->frame[frame0][i].rot,sm->frame[frame1][i].rot,frame,rot); m_translate(xyz,m0); q_to_matrix(rot,m1); m_multiply(m0,m1,sm->bone[i].matrix); } for(i = 0; i < sm->num_surface; i++) { int j; sm_surface_t *s = sm->surface[i]; for(j = 0; j < s->num_vertex; j++) { int k; v_set(0,0,0,s->vertex[j].xyz); v_set(0,0,0,s->vertex[j].normal); for(k = 0; k < s->vertex[j].num_weight; k++) { float v[3]; sm_weight_t *w = &s->vertex[j].weight[k]; v_transform(w->xyz,sm->bone[w->bone].matrix,v); v_scale(v,w->weight,v); v_add(s->vertex[j].xyz,v,s->vertex[j].xyz); v_transform_normal(w->normal,sm->bone[w->bone].matrix,v); v_scale(v,w->weight,v); v_add(s->vertex[j].normal,v,s->vertex[j].normal); } } } }
void VRPNTrackerInstance::getAccReport(vrpn_TRACKERACCCB* cpy, timeval* ts, int in_sensor) { TrackerAccList::iterator it; vrpn_TRACKERACCCB* last = NULL; for ( it = acc.begin(); it != acc.end(); it++ ) { vrpn_TRACKERACCCB* curr = *it; if (curr->sensor == in_sensor) { if (ts == NULL) { *cpy = *curr; return; } else if (vrpn_TimevalGreater(*ts,curr->msg_time)) { if (last) { double val = vrpn_TimevalMsecs(vrpn_TimevalDiff(*ts, curr->msg_time))/vrpn_TimevalMsecs(vrpn_TimevalDiff(last->msg_time, curr->msg_time)); cpy->acc[0] = curr->acc[0] + val*(last->acc[0] - curr->acc[0]); cpy->acc[1] = curr->acc[1] + val*(last->acc[1] - curr->acc[1]); cpy->acc[2] = curr->acc[2] + val*(last->acc[2] - curr->acc[2]); q_slerp(cpy->acc_quat, curr->acc_quat, last->acc_quat, val); cpy->acc_quat_dt = curr->acc_quat_dt + val*(last->acc_quat_dt - curr->acc_quat_dt); return; } else { *cpy = *curr; return; } } else last = curr; } } }
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; }
void vrpn_Tracker_DeadReckoning_Rotation::sendNewPrediction(vrpn_int32 sensor) { //======================================================================== // Figure out which rotation state we're supposed to use. if (sensor >= d_numSensors) { send_text_message(vrpn_TEXT_WARNING) << "sendNewPrediction: Asked for sensor " << sensor << " but I only have " << d_numSensors << "sensors. Discarding."; return; } vrpn_Tracker_DeadReckoning_Rotation::RotationState &state = d_rotationStates[sensor]; //======================================================================== // If we haven't had a tracker report yet, nothing to send. if (state.d_lastReportTime.tv_sec == 0) { return; } //======================================================================== // If we don't have permission to estimate velocity and haven't gotten it // either, then we just pass along the report. if (!state.d_receivedAngularVelocityReport && !d_estimateVelocity) { report_pose(sensor, state.d_lastReportTime, state.d_lastPosition, state.d_lastOrientation); return; } //======================================================================== // Estimate the future orientation based on the current angular velocity // estimate and the last reported orientation. Predict it into the future // the amount we've been asked to. // Start with the previous orientation. q_type newOrientation; q_copy(newOrientation, state.d_lastOrientation); // Rotate it by the amount to rotate once for every integral multiple // of the rotation time we've been asked to go. double remaining = d_predictionTime; while (remaining > state.d_rotationInterval) { q_mult(newOrientation, state.d_rotationAmount, newOrientation); remaining -= state.d_rotationInterval; } // Then rotate it by the remaining fractional amount. double fractionTime = remaining / state.d_rotationInterval; q_type identity = { 0, 0, 0, 1 }; q_type fractionRotation; q_slerp(fractionRotation, identity, state.d_rotationAmount, fractionTime); q_mult(newOrientation, fractionRotation, newOrientation); //======================================================================== // Find out the future time for which we will be predicting by adding the // prediction interval to our last report time. struct timeval future_time; struct timeval delta; delta.tv_sec = static_cast<unsigned long>(d_predictionTime); double remainder = d_predictionTime - delta.tv_sec; delta.tv_usec = static_cast<unsigned long>(remainder * 1e6); future_time = vrpn_TimevalSum(delta, state.d_lastReportTime); //======================================================================== // Pack our predicted tracker report for this future time. // Use the position portion of the report unaltered. if (0 != report_pose(sensor, future_time, state.d_lastPosition, newOrientation)) { fprintf(stderr, "vrpn_Tracker_DeadReckoning_Rotation::sendNewPrediction(): Can't report pose\n"); } }