int vrpn_Tracker_DeadReckoning_Rotation::test(void) { // Construct a loopback connection to be used by all of our objects. vrpn_Connection *c = vrpn_create_server_connection("loopback:"); // Create a tracker server to be the initator and a dead-reckoning // rotation tracker to use it as a base; have it predict 1 second // into the future. vrpn_Tracker_Server *t0 = new vrpn_Tracker_Server("Tracker0", c, 2); vrpn_Tracker_DeadReckoning_Rotation *t1 = new vrpn_Tracker_DeadReckoning_Rotation("Tracker1", c, "*Tracker0", 2, 1); // Create a remote tracker to listen to t1 and set up its callbacks for // position and velocity reports. They will fill in the static structures // listed above with whatever values they receive. vrpn_Tracker_Remote *tr = new vrpn_Tracker_Remote("Tracker1", c); tr->register_change_handler(&poseResponse, handle_test_tracker_report); tr->register_change_handler(&velResponse, handle_test_tracker_velocity_report); // Set up the values in the pose and velocity responses with incorrect values // so that things will fail if the class does not perform as expected. q_vec_set(poseResponse.pos, 1, 1, 1); q_make(poseResponse.quat, 0, 0, 1, 0); poseResponse.time.tv_sec = 0; poseResponse.time.tv_usec = 0; poseResponse.sensor = -1; // Send a pose report from sensors 0 and 1 on the original tracker that places // them at (1,1,1) and with the identity rotation. We should get a response for // each of them so we should end up with the sensor-1 response matching what we // sent, with no change in position or orientation. q_vec_type pos = { 1, 1, 1 }; q_type quat = { 0, 0, 0, 1 }; struct timeval firstSent; vrpn_gettimeofday(&firstSent, NULL); t0->report_pose(0, firstSent, pos, quat); t0->report_pose(1, firstSent, pos, quat); t0->mainloop(); t1->mainloop(); c->mainloop(); tr->mainloop(); if ( (poseResponse.time.tv_sec != firstSent.tv_sec + 1) || (poseResponse.sensor != 1) || (q_vec_distance(poseResponse.pos, pos) > 1e-10) || (poseResponse.quat[Q_W] != 1) ) { std::cerr << "vrpn_Tracker_DeadReckoning_Rotation::test(): Got unexpected" << " initial response: pos (" << poseResponse.pos[Q_X] << ", " << poseResponse.pos[Q_Y] << ", " << poseResponse.pos[Q_Z] << "), quat (" << poseResponse.quat[Q_X] << ", " << poseResponse.quat[Q_Y] << ", " << poseResponse.quat[Q_Z] << ", " << poseResponse.quat[Q_W] << ")" << " from sensor " << poseResponse.sensor << " at time " << poseResponse.time.tv_sec << ":" << poseResponse.time.tv_usec << std::endl; delete tr; delete t1; delete t0; c->removeReference(); return 1; } // Send a second tracker report for sensor 0 coming 0.4 seconds later that has // translated to position (2,1,1) and rotated by 0.4 * 90 degrees around Z. // This should cause a prediction for one second later than this new pose // message that has rotated by very close to 1.4 * 90 degrees. q_vec_type pos2 = { 2, 1, 1 }; q_type quat2; double angle2 = 0.4 * 90 * M_PI / 180.0; q_from_axis_angle(quat2, 0, 0, 1, angle2); struct timeval p4Second = { 0, 400000 }; struct timeval firstPlusP4 = vrpn_TimevalSum(firstSent, p4Second); t0->report_pose(0, firstPlusP4, pos2, quat2); t0->mainloop(); t1->mainloop(); c->mainloop(); tr->mainloop(); double x, y, z, angle; q_to_axis_angle(&x, &y, &z, &angle, poseResponse.quat); if ((poseResponse.time.tv_sec != firstPlusP4.tv_sec + 1) || (poseResponse.sensor != 0) || (q_vec_distance(poseResponse.pos, pos2) > 1e-10) || !isClose(x, 0) || !isClose(y, 0) || !isClose(z, 1) || !isClose(angle, 1.4 * 90 * M_PI / 180.0) ) { std::cerr << "vrpn_Tracker_DeadReckoning_Rotation::test(): Got unexpected" << " predicted pose response: pos (" << poseResponse.pos[Q_X] << ", " << poseResponse.pos[Q_Y] << ", " << poseResponse.pos[Q_Z] << "), quat (" << poseResponse.quat[Q_X] << ", " << poseResponse.quat[Q_Y] << ", " << poseResponse.quat[Q_Z] << ", " << poseResponse.quat[Q_W] << ")" << " from sensor " << poseResponse.sensor << std::endl; delete tr; delete t1; delete t0; c->removeReference(); return 2; } // Send a velocity report for sensor 1 that has has translation by (1,0,0) // and rotating by 0.4 * 90 degrees per 0.4 of a second around Z. // This should cause a prediction for one second later than the first // report that has rotated by very close to 90 degrees. The translation // should be ignored, so the position should be the original position. q_vec_type vel = { 0, 0, 0 }; t0->report_pose_velocity(1, firstPlusP4, vel, quat2, 0.4); t0->mainloop(); t1->mainloop(); c->mainloop(); tr->mainloop(); q_to_axis_angle(&x, &y, &z, &angle, poseResponse.quat); if ((poseResponse.time.tv_sec != firstSent.tv_sec + 1) || (poseResponse.sensor != 1) || (q_vec_distance(poseResponse.pos, pos) > 1e-10) || !isClose(x, 0) || !isClose(y, 0) || !isClose(z, 1) || !isClose(angle, 90 * M_PI / 180.0) ) { std::cerr << "vrpn_Tracker_DeadReckoning_Rotation::test(): Got unexpected" << " predicted velocity response: pos (" << poseResponse.pos[Q_X] << ", " << poseResponse.pos[Q_Y] << ", " << poseResponse.pos[Q_Z] << "), quat (" << poseResponse.quat[Q_X] << ", " << poseResponse.quat[Q_Y] << ", " << poseResponse.quat[Q_Z] << ", " << poseResponse.quat[Q_W] << ")" << " from sensor " << poseResponse.sensor << std::endl; delete tr; delete t1; delete t0; c->removeReference(); return 3; } // To test the behavior of the prediction code when we're moving around more // than one axis, and when we're starting from a non-identity orientation, // set sensor 1 to be rotated 180 degrees around X. Then send a velocity // report that will produce a rotation of 180 degrees around Z. The result // should match a prediction of 180 degrees around Y (plus or minus 180, plus // or minus Y axis are all equivalent). struct timeval oneSecond = { 1, 0 }; struct timeval firstPlusOne = vrpn_TimevalSum(firstSent, oneSecond); q_type quat3; q_from_axis_angle(quat3, 1, 0, 0, M_PI); t0->report_pose(1, firstPlusOne, pos, quat3); q_type quat4; q_from_axis_angle(quat4, 0, 0, 1, M_PI); t0->report_pose_velocity(1, firstPlusOne, vel, quat4, 1.0); t0->mainloop(); t1->mainloop(); c->mainloop(); tr->mainloop(); q_to_axis_angle(&x, &y, &z, &angle, poseResponse.quat); if ((poseResponse.time.tv_sec != firstPlusOne.tv_sec + 1) || (poseResponse.sensor != 1) || (q_vec_distance(poseResponse.pos, pos) > 1e-10) || !isClose(x, 0) || !isClose(fabs(y), 1) || !isClose(z, 0) || !isClose(fabs(angle), M_PI) ) { std::cerr << "vrpn_Tracker_DeadReckoning_Rotation::test(): Got unexpected" << " predicted pose + velocity response: pos (" << poseResponse.pos[Q_X] << ", " << poseResponse.pos[Q_Y] << ", " << poseResponse.pos[Q_Z] << "), quat (" << poseResponse.quat[Q_X] << ", " << poseResponse.quat[Q_Y] << ", " << poseResponse.quat[Q_Z] << ", " << poseResponse.quat[Q_W] << ")" << " from sensor " << poseResponse.sensor << std::endl; delete tr; delete t1; delete t0; c->removeReference(); return 4; } // To test the behavior of the prediction code when we're moving around more // than one axis, and when we're starting from a non-identity orientation, // set sensor 0 to start out at identity. Then in one second it will be // rotated 180 degrees around X. Then in another second it will be rotated // additionally by 90 degrees around Z; the prediction should be another // 90 degrees around Z, which should turn out to compose to +/-180 degrees // around +/- Y, as in the velocity case above. // To make this work, we send a sequence of three poses, one second apart, // starting at the original time. We do this on sensor 0, which has never // had a velocity report, so that it will be using the pose-only prediction. struct timeval firstPlusTwo = vrpn_TimevalSum(firstPlusOne, oneSecond); q_from_axis_angle(quat3, 1, 0, 0, M_PI); t0->report_pose(0, firstSent, pos, quat); t0->report_pose(0, firstPlusOne, pos, quat3); q_from_axis_angle(quat4, 0, 0, 1, M_PI / 2); q_type quat5; q_mult(quat5, quat4, quat3); t0->report_pose(0, firstPlusTwo, pos, quat5); t0->mainloop(); t1->mainloop(); c->mainloop(); tr->mainloop(); q_to_axis_angle(&x, &y, &z, &angle, poseResponse.quat); if ((poseResponse.time.tv_sec != firstPlusTwo.tv_sec + 1) || (poseResponse.sensor != 0) || (q_vec_distance(poseResponse.pos, pos) > 1e-10) || !isClose(x, 0) || !isClose(fabs(y), 1.0) || !isClose(z, 0) || !isClose(fabs(angle), M_PI) ) { std::cerr << "vrpn_Tracker_DeadReckoning_Rotation::test(): Got unexpected" << " predicted pose + pose response: pos (" << poseResponse.pos[Q_X] << ", " << poseResponse.pos[Q_Y] << ", " << poseResponse.pos[Q_Z] << "), quat (" << poseResponse.quat[Q_X] << ", " << poseResponse.quat[Q_Y] << ", " << poseResponse.quat[Q_Z] << ", " << poseResponse.quat[Q_W] << ")" << " from sensor " << poseResponse.sensor << "; axis = (" << x << ", " << y << ", " << z << "), angle = " << angle << std::endl; delete tr; delete t1; delete t0; c->removeReference(); return 5; } // Done; delete our objects and return 0 to indicate that // everything worked. delete tr; delete t1; delete t0; c->removeReference(); return 0; }
void dSolverNode::dumpRigidBodyArray(MObject &node) { // std::cout << "dSolverNode::dumpRigidBodyArray" << std::endl; MFnDagNode fnDagNode(node); rigidBodyArrayNode *rbaNode = static_cast<rigidBodyArrayNode*>(fnDagNode.userNode()); MPlug plgFiles(node, rigidBodyArrayNode::ia_fioFiles); // MPlug plgPositionAttribute(node, rigidBodyArrayNode::ia_fioPositionAttribute); // MPlug plgRotationAttribute(node, rigidBodyArrayNode::ia_fioRotationAttribute); MString mstr; plgFiles.getValue(mstr); std::string expr(mstr.asChar()); std::string base_path, extension; if(!expandFileExpression(expr, base_path, extension)) { std::cout << "dSolverNode::dumpRigidBodyArray: syntax error in file expression: " << std::endl << expr << std::endl; return; } if(extension != "pdb") { std::cout << "dSolverNode::dumpRigidBodyArray: only pdb files are supported" << std::endl; return; } std::string file_name = base_path + "." + extension; std::vector<rigid_body_t::pointer>& rbs = rbaNode->rigid_bodies(); pdb_io_t pdb_io; pdb_io.m_num_particles = rbs.size(); pdb_io.m_time = m_prevTime.value(); pdb_io.m_attributes.resize(3); //position pdb_io.m_attributes[0].m_num_elements = 1; pdb_io.m_attributes[0].m_name = ATTR_POSITION; pdb_io.m_attributes[0].m_type = pdb_io_t::kVector; pdb_io.m_attributes[0].m_vector_data.resize(rbs.size()); //rotation angle (in degrees) pdb_io.m_attributes[1].m_num_elements = 1; pdb_io.m_attributes[1].m_name = ATTR_IN_RANGLE; pdb_io.m_attributes[1].m_type = pdb_io_t::kReal; pdb_io.m_attributes[1].m_real_data.resize(rbs.size()); //rotation vector pdb_io.m_attributes[2].m_num_elements = 1; pdb_io.m_attributes[2].m_name = ATTR_IN_RAXIS; pdb_io.m_attributes[2].m_type = pdb_io_t::kVector; pdb_io.m_attributes[2].m_vector_data.resize(rbs.size()); for(size_t i = 0; i < rbs.size(); ++i) { vec3f pos; quatf rot; rbs[i]->get_transform(pos, rot); pdb_io.m_attributes[0].m_vector_data[i].x = pos[0]; pdb_io.m_attributes[0].m_vector_data[i].y = pos[1]; pdb_io.m_attributes[0].m_vector_data[i].z = pos[2]; // vec3f axis; float angle; q_to_axis_angle(rot, axis, angle); pdb_io.m_attributes[1].m_real_data[i] = rad2deg(angle); pdb_io.m_attributes[2].m_vector_data[i].x = axis[0]; pdb_io.m_attributes[2].m_vector_data[i].y = axis[1]; pdb_io.m_attributes[2].m_vector_data[i].z = axis[2]; } std::ofstream out(file_name.c_str()); pdb_io.write(out); }