/** * updateCollisions */ void PhysicsManagerImpl::updateCollisions() { // Should eventually be done using abstraction with // "engine" std::list<rigidbody*>::iterator it; if(!collisions.empty()) { for(it=collisions.begin();it!=collisions.end();++it) { #ifdef USE_IRRLICHT_ENGINE rigidbody * obj = *it; if(obj == 0) { collisions.erase(it); it--; continue; } scenenode * node = static_cast<scenenode*>((*it)->getUserPointer()); if(node) { // Bullet specific code // Position btVector3 p = obj->getCenterOfMassPosition(); node->setPosition(irr::core::vector3df((f32)p[0], (f32)p[1], (f32)p[2])); // Rotation btVector3 e(quatToEuler(obj->getOrientation())); node->setRotation(irr::core::vector3df(e[0], e[1], e[2])); } else { colRemovals.push(*it); } #endif } } // Removal queue queueProcess(); }
myQuat_t getPosition(mpu9250_t dev) { imuData_t imuData; if (getIMUData(dev, &imuData)) { if (! validIMUData(imuData)) { puts("invalid IMU data"); if (failureCount++ > 20) { puts("Error in IMU, restarting!"); identifyYourself("IMU failure"); if (! initialiseIMU(&dev)) { puts("Could not initialise IMU! dying..."); exit(1); } } return currentQ; } failureCount = 0; /************************************************************************ * Get Gyro data that records current angular velocity and create an * estimated quaternion representation of orientation using this * velocity, the time interval between the last calculation and the * previous orientation: * change in position = velocity * time, * new position = old position + change in position) ************************************************************************/ // get orientation as deduced by adding gyro measured rotational // velocity (times time interval) to previous orientation. double gx1 = (double)(imuData.gyro.x_axis); double gy1 = (double)(imuData.gyro.y_axis); double gz1 = (double)(imuData.gyro.z_axis); double omega[3] = { gx1, gy1, gz1 }; // this will be in degrees/sec //double omegaMagnitude = fabs(vecLength(omega)); uint32_t dt = imuData.ts - lastIMUData.ts; // dt in microseconds lastIMUData = imuData; myQuat_t gRot = makeQuatFromAngularVelocityTime(omega, dt/1000000.0); myQuat_t gyroDeducedQ = quatMultiply(currentQ, gRot); quatNormalise(&gyroDeducedQ); if (! useGyroscopes) { makeIdentityQuat(&gyroDeducedQ); } if (! (useAccelerometers | useMagnetometers)) { // return now with just the gyro data currentQ = gyroDeducedQ; lastIMUData = imuData; return currentQ; } // get orientation from the gyro as Roll Pitch Yaw (ie Euler angles) double gyroDeducedYPR[3]; quatToEuler(gyroDeducedQ, gyroDeducedYPR); double currentYPR[3]; quatToEuler(currentQ, currentYPR); // Calculate a roll/pich/yaw from the accelerometers and magnetometers. /************************************************************************ * Get Accelerometer data that records gravity direction and create a * 'measured' Quaternion representation of orientation (will only be * pitch and roll, not yaw) ************************************************************************/ // get gravity direction from accelerometer double ax1 = imuData.accel.x_axis / 1024.0; double ay1 = imuData.accel.y_axis / 1024.0; double az1 = imuData.accel.z_axis / 1024.0; double downSensor[3] = { ax1, ay1, az1 }; if (fabs(vecLength(downSensor) - 0.98) > 0.5) { // excessive accelerometer reading, bail out //printf("too much accel: %f ", fabs(vecLength(downSensor))); //dumpVec(downSensor); currentQ = gyroDeducedQ; lastIMUData = imuData; return currentQ; } vecNormalise(downSensor); // The accelerometers can only measure pitch and roll (in the world reference // frame), calculate the pitch and roll values to account for // accelerometer readings, make yaw = 0. double ypr[3]; double downX = downSensor[X_AXIS]; double downY = downSensor[Y_AXIS]; double downZ = downSensor[Z_AXIS]; // add a little X into Z when calculating roll to compensate for // situations when pitching near vertical as Y, Z will be near 0 and // the results will be unstable double fiddledDownZ = sqrt(downZ*downZ + 0.001*downX*downX); if (downZ <= 0) // compensate for loss of sign when squaring Z fiddledDownZ *= -1.0; ypr[ROLL] = atan2(downY, fiddledDownZ); ypr[PITCH] = -atan2(downX, sqrt(downY*downY + downZ*downZ)); ypr[YAW] = 0; // yaw == 0, accelerometer cannot measure yaw if (! useAccelerometers) { // if no accelerometers, use roll and pitch values from gyroscope // derived orientation ypr[PITCH] = gyroDeducedYPR[PITCH]; ypr[ROLL] = gyroDeducedYPR[ROLL]; ypr[YAW] = gyroDeducedYPR[YAW]; } else if (vecLength(downSensor) == 0) { puts("weightless?"); // something wrong! - weightless?? ypr[PITCH] = gyroDeducedYPR[PITCH]; ypr[ROLL] = gyroDeducedYPR[ROLL]; ypr[YAW] = gyroDeducedYPR[YAW]; } if (! useMagnetometers) { ypr[YAW] = gyroDeducedYPR[YAW]; } myQuat_t accelQ = eulerToQuat(ypr); myQuat_t invAccelQ = quatConjugate(accelQ); /************************************************************************ * Get Magnetometer data that records north direction and create a * 'measured' Quaternion representation of orientation (will only be * yaw) ************************************************************************/ // get magnetometer indication of North double mx1 = (double)(imuData.mag.x_axis - magHardCorrection[X_AXIS]); double my1 = (double)(imuData.mag.y_axis - magHardCorrection[Y_AXIS]); double mz1 = (double)(imuData.mag.z_axis - magHardCorrection[Z_AXIS]); mx1 *= magSoftCorrection[X_AXIS]; my1 *= magSoftCorrection[Y_AXIS]; mz1 *= magSoftCorrection[Z_AXIS]; // NB MPU9250 has different XYZ axis for magnetometers than for gyros // and accelerometers so juggle x,y,z here... double magV[3] = { my1, mx1, -mz1 }; vecNormalise(magV); // make quaternion representing mag readings myQuat_t magQ = quatFromValues(0, magV[X_AXIS], magV[Y_AXIS], magV[Z_AXIS]); if (vecLength(magV) == 0) { // something wrong! - no magnetic field?? //puts("not on earth?"); magQ = quatFromValues(1, 0, 0, 0); } // the magnetometers can only measure yaw (in the world reference // frame), adjust the yaw of the roll/pitch/yaw values calculated // earlier to account for the magnetometer readings. if (useMagnetometers) { // pitch and roll the mag direction using accelerometer info to // create a quaternion that represents the IMU as if it was // horizontal (so we can get pure yaw) myQuat_t horizQ = quatMultiply(accelQ, magQ); horizQ = quatMultiply(horizQ, invAccelQ); // calculate pure yaw ypr[YAW] = -atan2(horizQ.y, horizQ.x); } // measuredQ is the orientation as measured using the // accelerometer/magnetometer readings myQuat_t measuredQ = eulerToQuat(ypr); // check measured and deduced orientations are not wildly different (eg // due to a 360d alias flip) and adjust if problem... measuredQ = adjustForCongruence(measuredQ, gyroDeducedQ); /************************************************************************* * The gyro estimated orientation will be smooth and precise over short * time intervals but small errors will accumulate causing it to * drift over time. The 'measured' orientation as obtained from the * magnetometer and accelerometer will be jumpy but will always average * around the correct orientation. * * Take the mag and accel 'measured' orientation and the gyro * 'estimated' orientation and create a new 'current' orientation that * is the estimated orientation corrected slightly towards the measured * orientation. That way we get the smooth precision of the gyros but * eliminate the accumulation of drift errors. ************************************************************************/ if (useGyroscopes) { // the new orientation is the gyro deduced position corrected // towards the accel/mag measured position using interpolation // between quaternions. currentQ = slerp(gyroDeducedQ, measuredQ, 0.02); } else { currentQ = measuredQ; } if (! isQuatValid(currentQ)) { puts("error in quaternion, reseting orientation..."); displayData(imuData); dumpQuat(currentQ); makeIdentityQuat(¤tQ); } } return currentQ; }
geometry_msgs::PoseStamped PSMoveTemplateController::updatePose(geometry_msgs::PoseStamped object_pose, MoveServerPacket *move_server_packet, float normalized_scale) { geometry_msgs::PoseStamped transformed_pose; ///////////////////////// //Calculate scale QVector3D cam(camera_position_); QVector3D obj(object_pose.pose.position.x,object_pose.pose.position.y,object_pose.pose.position.z); double distance = (cam - obj).length(); //ROS_ERROR("distance: %f vfov: %f center distance: %f", distance, camera_update_.vfov, cos(camera_update_.vfov) * distance); double scale = (cos(camera_update_.vfov) * distance) * 1.0 / 5.0; // distance=5m is scale 1.0 float rotation_scale = normalized_scale;// * scale; float position_scale = 0.01 * normalized_scale * scale; // move position reported in mm, so scale it down (right now it's 10x real-world scale) ///////////////////////// //Translation- move relative to camera for direction but still translate in world space // create a vector representing the offset to be applied to position. QVector3D position_offset((move_server_packet->state[0].handle_pos[0] - old_move_position_.x()) * position_scale, (move_server_packet->state[0].handle_pos[1] - old_move_position_.y()) * position_scale, (move_server_packet->state[0].handle_pos[2] - old_move_position_.z()) * position_scale); //rotate the offset to be relative to camera orientation, can now be applied to position to move relative to camera QVector3D rotated_position_offset = camera_orientation_.rotatedVector(position_offset); //Update x, y, and z values // NEED TO RESTORE THESE FOR POSITION CONTROL if(first_time_) { transformed_pose.pose.position.x = object_pose.pose.position.x; transformed_pose.pose.position.y = object_pose.pose.position.y; transformed_pose.pose.position.z = object_pose.pose.position.z; } else { transformed_pose.pose.position.x = object_pose.pose.position.x;// + rotated_position_offset.x(); transformed_pose.pose.position.y = object_pose.pose.position.y;// + rotated_position_offset.y(); transformed_pose.pose.position.z = object_pose.pose.position.z;// + rotated_position_offset.z(); } ///////////////////////// //Rotation // calculate the relative rotation from the old move orientation, so we only have the difference QQuaternion move_orientation(move_server_packet->state[0].quat[3],move_server_packet->state[0].quat[0],move_server_packet->state[0].quat[1],move_server_packet->state[0].quat[2]); QQuaternion move_orientation_offset = old_move_orientation_.conjugate() * move_orientation; // object orientation QQuaternion object_orientation(object_pose.pose.orientation.w,object_pose.pose.orientation.x,object_pose.pose.orientation.y,object_pose.pose.orientation.z); camera_orientation_.normalize(); move_orientation_offset.normalize(); object_orientation.normalize(); printf("DEBUG\n"); //printf(" cam: %.3f, %.3f, %.3f, %.3f\n",camera_orientation_.scalar(),camera_orientation_.x(),camera_orientation_.y(),camera_orientation_.z()); //printf(" move1: %.3f, %.3f, %.3f, %.3f\n",move_orientation_offset.scalar(),move_orientation_offset.x(),move_orientation_offset.y(),move_orientation_offset.z()); //printf(" obj: %.3f, %.3f, %.3f, %.3f\n",object_orientation.scalar(),object_orientation.x(),object_orientation.y(),object_orientation.z()); float x,y,z; quatToEuler(camera_orientation_,y,z,x); printf(" cam: %.3f, %.3f, %.3f\n",x,y,z); quatToEuler(move_orientation_offset,y,z,x); printf(" move1: %.3f, %.3f, %.3f\n",x,y,z); quatToEuler(object_orientation,y,z,x); printf(" obj: %.3f, %.3f, %.3f\n",x,y,z); if(first_time_) { // now we need to apply the orientation offset relative to the camera view // QQuaternion camera_T_move = camera_orientation_ * move_orientation; // QQuaternion object_T_camera = last_object_T_move_.conjugate() * camera_orientation_; // QQuaternion object_T_move = object_T_camera * camera_T_move; // camera_T_object = camera_orientation_.conjugate() * object_orientation; degrees_ = 0; } // QQuaternion rotated_orientation = move_orientation * last_object_T_move_.conjugate(); /////// // old stuff - not quite right, doesn't take starting pose of move into consideration //based on http://www.arcsynthesis.org/gltut/Positioning/Tut08%20Camera%20Relative%20Orientation.html //new_rotation_offset = camera_orientation^-1 * (rotation_offset * camera_orientation) //QQuaternion rotated_orientation_offset = camera_orientation_ * move_orientation_offset * camera_orientation_.conjugate() * object_orientation; QQuaternion rotated_orientation_offset = camera_orientation_ * QQuaternion::fromAxisAndAngle(QVector3D(1,0,0),1) * camera_orientation_.conjugate() * object_orientation; // making transform constant rotated_orientation_offset = camera_orientation_ * move_orientation * camera_orientation_.conjugate(); quatToEuler(rotated_orientation_offset,y,z,x); printf(" off: %.3f, %.3f, %.3f\n",x,y,z); /////// // based on matrices // calculate camera x axis // QVector3D camera_x_axis = camera_orientation_.rotatedVector(QVector3D(1,0,0)); // move_orientation_offset = QQuaternion::fromAxisAndAngle(camera_x_axis,degrees_++);//camera_orientation_ * move_orientation_offset; // QQuaternion camera_T_new_object = move_orientation_offset * camera_T_object; // QQuaternion rotated_orientation_offset = camera_orientation_ * camera_T_new_object; /////// // based on vectors //Gets the world vector space for cameras vectors QVector3D camera_x = camera_orientation_.rotatedVector(QVector3D(1,0,0)); QVector3D camera_y = camera_orientation_.rotatedVector(QVector3D(0,1,0)); QVector3D camera_z = camera_orientation_.rotatedVector(QVector3D(0,0,1)); printf(" camera X: %.3f, %.3f, %.3f\n",camera_x.x(),camera_x.y(),camera_x.z()); printf(" camera Y: %.3f, %.3f, %.3f\n",camera_y.x(),camera_y.y(),camera_y.z()); printf(" camera Z: %.3f, %.3f, %.3f\n",camera_z.x(),camera_z.y(),camera_z.z()); QVector3D move_x = move_orientation.rotatedVector(QVector3D(1,0,0)); QVector3D move_y = move_orientation.rotatedVector(QVector3D(0,1,0)); QVector3D move_z = move_orientation.rotatedVector(QVector3D(0,0,1)); printf(" move X: %.3f, %.3f, %.3f\n",move_x.x(),move_x.y(),move_x.z()); printf(" move Y: %.3f, %.3f, %.3f\n",move_y.x(),move_y.y(),move_y.z()); printf(" move Z: %.3f, %.3f, %.3f\n",move_z.x(),move_z.y(),move_z.z()); // //Turns relative vectors from world to objects local space // QVector3D object_relative_y = object_orientation.conjugate().rotatedVector(camera_y); // QVector3D object_relative_x = object_orientation.conjugate().rotatedVector(camera_x); // QVector3D object_relative_z = object_orientation.conjugate().rotatedVector(camera_z); // QQuaternion rotated_orientation_offset = QQuaternion::fromAxisAndAngle(object_relative_z,1) // * QQuaternion::fromAxisAndAngle(object_relative_y,0) // * QQuaternion::fromAxisAndAngle(object_relative_x,0); rotated_orientation_offset.normalize(); // DEBUG // ROS_ERROR("DEBUG"); // float x,y,z; // quatToEuler(object_orientation,y,x,z); // ROS_ERROR(" obj: %.3f, %.3f, %.3f",x,y,z); // quatToEuler(move_orientation_offset,y,x,z); // ROS_ERROR(" off: %.3f, %.3f, %.3f",x,y,z); // quatToEuler(rotated_orientation_offset,y,x,z); // ROS_ERROR(" rot: %.3f, %.3f, %.3f",x,y,z); // DEBUG QQuaternion transformed_orientation; //if(first_time_) // transformed_orientation = object_orientation; //else transformed_orientation = rotated_orientation_offset; transformed_pose.pose.orientation.w = transformed_orientation.scalar(); transformed_pose.pose.orientation.x = transformed_orientation.x(); transformed_pose.pose.orientation.y = transformed_orientation.y(); transformed_pose.pose.orientation.z = transformed_orientation.z(); object_pose_.pose.orientation.w = transformed_orientation.scalar(); object_pose_.pose.orientation.x = transformed_orientation.x(); object_pose_.pose.orientation.y = transformed_orientation.y(); object_pose_.pose.orientation.z = transformed_orientation.z(); ///////////////////////// //Header transformed_pose.header = object_pose.header; transformed_pose.header.stamp = ros::Time::now(); first_time_ = false; return transformed_pose; }
static void export_alembic_camera(AlembicArchive &archive, const RenderedBuffer & renderedBuffer, bool isUseEuler) { static const int cameraKey = 0xFFFFFF; Alembic::AbcGeom::OObject topObj(*archive.archive, Alembic::AbcGeom::kTop); Alembic::AbcGeom::OXform xform; if (archive.xform_map.find(cameraKey) != archive.xform_map.end()) { xform = archive.xform_map[cameraKey]; } else { xform = Alembic::AbcGeom::OXform(topObj, "camera_xform", archive.timesampling); archive.xform_map[cameraKey] = xform; Alembic::AbcGeom::OXformSchema &xformSchema = xform.getSchema(); archive.xform_schema_map[cameraKey] = xformSchema; } // set camera transform { Alembic::AbcGeom::OXformSchema &xformSchema = archive.xform_schema_map[cameraKey]; xformSchema.setTimeSampling(archive.timesampling); Alembic::AbcGeom::XformSample xformSample; D3DXMATRIX convertMat( 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 1); D3DXMATRIX convertedWordInv; ::D3DXMatrixMultiply(&convertedWordInv, &renderedBuffer.world_inv, &convertMat); D3DXVECTOR3 eye; { D3DXVECTOR3 v; UMGetCameraEye(&v); d3d_vector3_transform(eye, v,convertedWordInv); } D3DXVECTOR3 at; { D3DXVECTOR3 v; UMGetCameraAt(&v); d3d_vector3_transform(at, v, convertedWordInv); } D3DXVECTOR3 up; { D3DXVECTOR3 v; UMGetCameraUp(&v); d3d_vector3_dir_transform(up, v, convertedWordInv); ::D3DXVec3Normalize(&up, &up); } Imath::V3d trans((double)eye.x, (double)eye.y, (double)(eye.z)); xformSample.setTranslation(trans); D3DXMATRIX view; ::D3DXMatrixLookAtLH(&view, &eye, &at, &up); Imath::M44d rot( -view.m[0][0], view.m[0][1], view.m[0][2], 0, -view.m[1][0], view.m[1][1], view.m[1][2], 0, view.m[2][0], -view.m[2][1], -view.m[2][2], 0, 0, 0, 0, 1); Imath::Quatd quat = Imath::extractQuat(rot); quat.normalize(); if (isUseEuler) { Imath::V3d imeuler; quatToEuler(imeuler, quat); //UMMat44d umrot( // -view.m[0][0], view.m[0][1], view.m[0][2], 0, // -view.m[1][0], view.m[1][1], view.m[1][2], 0, // view.m[2][0], -view.m[2][1], -view.m[2][2], 0, // 0, 0, 0, 1); //UMVec3d umeuler = umbase::um_matrix_to_euler_xyz(umrot.transposed()); xformSample.setXRotation(umbase::um_to_degree(imeuler.y)); xformSample.setYRotation(umbase::um_to_degree(imeuler.x)); xformSample.setZRotation(-umbase::um_to_degree(imeuler.z)); } else { xformSample.setRotation(quat.axis(), umbase::um_to_degree(quat.angle())); } xformSchema.set(xformSample); } Alembic::AbcGeom::OCamera camera; if (archive.camera_map.find(cameraKey) != archive.camera_map.end()) { camera = archive.camera_map[cameraKey]; } else { camera = Alembic::AbcGeom::OCamera(xform, "camera", archive.timesampling); archive.camera_map[cameraKey] = camera; Alembic::AbcGeom::OCameraSchema &cameraSchema = camera.getSchema(); archive.camera_schema_map[cameraKey] = cameraSchema; } Alembic::AbcGeom::OCameraSchema &cameraSchema = archive.camera_schema_map[cameraKey]; cameraSchema.setTimeSampling(archive.timesampling); Alembic::AbcGeom::CameraSample sample; D3DXVECTOR4 v; UMGetCameraFovLH(&v); sample.setNearClippingPlane(v.z); sample.setFarClippingPlane(v.w); double fovy = v.x; double aspect = v.y; double fovx = 2.0 * atan(tan(fovy / 2.0)*(aspect)); double w = BridgeParameter::instance().frame_width / 10.0; double h = BridgeParameter::instance().frame_height / 10.0; double focalLength = w / (2.0 * tan(fovx / 2.0)); sample.setHorizontalAperture(w / 10.0); sample.setVerticalAperture(h / 10.0); sample.setFocalLength(focalLength); cameraSchema.set(sample); }