void OculusWindow::start_frame() { GlfwWindow::start_frame(); auto ftiming = ovr_GetPredictedDisplayTime(hmd_session_, 0); ovrTrackingState hmdState = ovr_GetTrackingState(hmd_session_, ftiming, true); ovrEyeRenderDesc eyeRenderDesc[2]; ovrVector3f hmdToEyeViewOffset[2]; eyeRenderDesc[0] = ovr_GetRenderDesc(hmd_session_, ovrEye_Left, hmd_desc_.DefaultEyeFov[0]); eyeRenderDesc[1] = ovr_GetRenderDesc(hmd_session_, ovrEye_Right, hmd_desc_.DefaultEyeFov[1]); hmdToEyeViewOffset[0] = eyeRenderDesc[0].HmdToEyeOffset; hmdToEyeViewOffset[1] = eyeRenderDesc[1].HmdToEyeOffset; ovr_CalcEyePoses(hmdState.HeadPose.ThePose, hmdToEyeViewOffset, color_layer_.RenderPose); if (hmdState.StatusFlags & (ovrStatus_OrientationTracked | ovrStatus_PositionTracked)) { auto pose = hmdState.HeadPose.ThePose; scm::math::quat<double> rot_quat(pose.Orientation.w, pose.Orientation.x, pose.Orientation.y, pose.Orientation.z); hmd_sensor_orientation_ = scm::math::make_translation((double)pose.Position.x, (double)pose.Position.y, (double)pose.Position.z) * rot_quat.to_matrix(); } }
RobotInterface::Status RobotStatePublisher::RobotUpdate(){ for(int ii =0;ii<3;ii++){ for( int jj = 0;jj<3;jj++){ rot_Eigen(ii,jj) = rot_MathLib(ii,jj); } } //cout<<rot_Eigen<<endl; //rot_MathLib.Print(); Eigen::Quaternion<double> rot_quat(rot_Eigen); //cout<<rot_quat.w()<<endl; //cout<<rot_quat.toRotationMatrix()<<endl; for(int i=0;i<mRobot->GetDOFCount();i++){ jointStateMsg.position[i] = mSensorsGroup.GetJointAngles()(i); jointStateMsg.velocity[i] = mSensorsGroup.GetJointVelocities()(i); jointStateMsg.effort[i] = mSensorsGroup.GetJointTorques()(i); } jointStateMsg.header.stamp = ros::Time::now(); poseStampedMsg.pose.position.x = currEEPos(0); poseStampedMsg.pose.position.y = currEEPos(1); poseStampedMsg.pose.position.z = currEEPos(2); poseStampedMsg.pose.orientation.w = rot_quat.w(); poseStampedMsg.pose.orientation.x = rot_quat.x(); poseStampedMsg.pose.orientation.y = rot_quat.y(); poseStampedMsg.pose.orientation.z = rot_quat.z(); poseStampedMsg.header.stamp = ros::Time::now(); jointStatePublisher.publish(jointStateMsg); posePublisher.publish(poseStampedMsg); // New message for Cart FT ftMsg.header.stamp = ros::Time::now(); ftMsg.wrench.force.x = eeFT[0]; ftMsg.wrench.force.y = eeFT[1]; ftMsg.wrench.force.z = eeFT[2]; ftMsg.wrench.torque.x = eeFT[3]; ftMsg.wrench.torque.y = eeFT[4]; ftMsg.wrench.torque.z = eeFT[5]; // New message for Cart Stiffness stiffMsg.header.stamp = ros::Time::now(); stiffMsg.twist.linear.x = eeStiff[0]; stiffMsg.twist.linear.y = eeStiff[1]; stiffMsg.twist.linear.z = eeStiff[2]; stiffMsg.twist.angular.x = eeStiff[3]; stiffMsg.twist.angular.y = eeStiff[4]; stiffMsg.twist.angular.z = eeStiff[5]; ftPublisher.publish(ftMsg); stiffPublisher.publish(stiffMsg); return STATUS_OK; }
void PhysActor_PhysX::Rotate(vec4f quat) { if (impl && impl->physActor) { PxTransform trans = impl->physActor->getGlobalPose(); PxQuat rot_quat(quat.w, PxVec3(quat.x, quat.y, quat.z)); rot_quat = rot_quat.getNormalized(); trans.q *= rot_quat; trans.q = trans.q.getNormalized(); impl->physActor->setGlobalPose(trans); } }
/*! Updates camera and sets the view matrix \pre plyr != NULL, plyr has been initialized with position & velocity info., plyr->view.mode has been set \arg \c plyr pointer to player data \arg \c dt time step size \return none \author jfpatry \date Created: 2000-08-26 \date Modified: 2000-08-26 */ void update_view(Player& plyr, const float dt) { ppogl::Vec3d view_pt; ppogl::Vec3d view_dir; ppogl::Vec3d vel_cpy = plyr.vel; const float speed = vel_cpy.normalize(); const float time_constant_mult = 1.0 / MIN( 1.0, MAX( 0.0, ( speed - NO_INTERPOLATION_SPEED ) / ( BASELINE_INTERPOLATION_SPEED - NO_INTERPOLATION_SPEED ))); ppogl::Vec3d up_dir = ppogl::Vec3d( 0, 1, 0 ); ppogl::Vec3d vel_dir = plyr.vel; vel_dir.normalize(); const float course_angle = Course::getAngle(); switch(plyr.view.mode){ case BEHIND: { /* Camera-on-a-string mode */ /* Construct vector from player to camera */ ppogl::Vec3d view_vec(0, sin( ANGLES_TO_RADIANS( course_angle - CAMERA_ANGLE_ABOVE_SLOPE + PLAYER_ANGLE_IN_CAMERA ) ), cos( ANGLES_TO_RADIANS( course_angle - CAMERA_ANGLE_ABOVE_SLOPE + PLAYER_ANGLE_IN_CAMERA ) ) ); view_vec = CAMERA_DISTANCE*view_vec; ppogl::Vec3d y_vec(0.0, 1.0, 0.0); ppogl::Vec3d mz_vec(0.0, 0.0, -1.0); ppogl::Vec3d vel_proj = projectIntoPlane( y_vec, vel_dir ); vel_proj.normalize(); /* Rotate view_vec so that it places the camera behind player */ pp::Quat rot_quat(mz_vec, vel_proj); view_vec = rot_quat.rotate(view_vec); /* Construct view point */ view_pt = plyr.pos - view_vec; /* Make sure view point is above terrain */ float ycoord = find_y_coord( view_pt ); if ( view_pt.y() < ycoord + MIN_CAMERA_HEIGHT ) { view_pt.y() = ycoord + MIN_CAMERA_HEIGHT; } /* Interpolate view point */ if ( plyr.view.initialized ) { /* Interpolate twice to get a second-order filter */ for (int i=0; i<2; i++) { view_pt = interpolate_view_pos( plyr.pos, plyr.pos, MAX_CAMERA_PITCH, plyr.view.pos, view_pt, CAMERA_DISTANCE, dt, BEHIND_ORBIT_TIME_CONSTANT * time_constant_mult ); } } /* Make sure interpolated view point is above terrain */ ycoord = find_y_coord( view_pt ); if ( view_pt.y() < ycoord + ABSOLUTE_MIN_CAMERA_HEIGHT ) { view_pt.y() = ycoord + ABSOLUTE_MIN_CAMERA_HEIGHT; } /* Construct view direction */ view_vec = view_pt - plyr.pos; ppogl::Vec3d axis = y_vec^view_vec; axis.normalize(); pp::Matrix rot_mat; rot_mat.makeRotationAboutVector( axis, PLAYER_ANGLE_IN_CAMERA ); view_dir = -1.0*rot_mat.transformVector( view_vec ); /* Interpolate orientation of camera */ if ( plyr.view.initialized ) { /* Interpolate twice to get a second-order filter */ for (int i=0; i<2; i++) { interpolate_view_frame( plyr.view.up, plyr.view.dir, up_dir, view_dir, dt, BEHIND_ORIENT_TIME_CONSTANT ); up_dir = ppogl::Vec3d( 0.0, 1.0, 0.0 ); } } break; } case FOLLOW: { /* Camera follows player (above and behind) */ up_dir = ppogl::Vec3d( 0, 1, 0 ); /* Construct vector from player to camera */ ppogl::Vec3d view_vec( 0, sin( ANGLES_TO_RADIANS( course_angle - CAMERA_ANGLE_ABOVE_SLOPE + PLAYER_ANGLE_IN_CAMERA ) ), cos( ANGLES_TO_RADIANS( course_angle - CAMERA_ANGLE_ABOVE_SLOPE + PLAYER_ANGLE_IN_CAMERA ) ) ); view_vec = CAMERA_DISTANCE*view_vec; ppogl::Vec3d y_vec(0.0, 1.0, 0.0); ppogl::Vec3d mz_vec(0.0, 0.0, -1.0); ppogl::Vec3d vel_proj = projectIntoPlane( y_vec, vel_dir ); vel_proj.normalize(); /* Rotate view_vec so that it places the camera behind player */ pp::Quat rot_quat(mz_vec, vel_proj); view_vec = rot_quat.rotate( view_vec ); /* Construct view point */ view_pt = plyr.pos + view_vec; /* Make sure view point is above terrain */ float ycoord = find_y_coord( view_pt ); if ( view_pt.y() < ycoord + MIN_CAMERA_HEIGHT ) { view_pt.y() = ycoord + MIN_CAMERA_HEIGHT; } /* Interpolate view point */ if ( plyr.view.initialized ) { /* Interpolate twice to get a second-order filter */ for (int i=0; i<2; i++ ) { view_pt = interpolate_view_pos( plyr.view.plyr_pos, plyr.pos, MAX_CAMERA_PITCH, plyr.view.pos, view_pt, CAMERA_DISTANCE, dt, FOLLOW_ORBIT_TIME_CONSTANT * time_constant_mult ); } } /* Make sure interpolate view point is above terrain */ ycoord = find_y_coord( view_pt ); if ( view_pt.y() < ycoord + ABSOLUTE_MIN_CAMERA_HEIGHT ) { view_pt.y() = ycoord + ABSOLUTE_MIN_CAMERA_HEIGHT; } /* Construct view direction */ view_vec = view_pt - plyr.pos; ppogl::Vec3d axis = y_vec^view_vec; axis.normalize(); pp::Matrix rot_mat; rot_mat.makeRotationAboutVector( axis, PLAYER_ANGLE_IN_CAMERA ); view_dir = -1.0*rot_mat.transformVector( view_vec ); /* Interpolate orientation of camera */ if ( plyr.view.initialized ) { /* Interpolate twice to get a second-order filter */ for (int i=0; i<2; i++ ) { interpolate_view_frame( plyr.view.up, plyr.view.dir, up_dir, view_dir, dt, FOLLOW_ORIENT_TIME_CONSTANT ); up_dir = ppogl::Vec3d( 0.0, 1.0, 0.0 ); } } break; } case ABOVE: { /* Camera always uphill of player */ up_dir = ppogl::Vec3d( 0, 1, 0 ); /* Construct vector from player to camera */ ppogl::Vec3d view_vec( 0, sin( ANGLES_TO_RADIANS( course_angle - CAMERA_ANGLE_ABOVE_SLOPE+ PLAYER_ANGLE_IN_CAMERA ) ), cos( ANGLES_TO_RADIANS( course_angle - CAMERA_ANGLE_ABOVE_SLOPE+ PLAYER_ANGLE_IN_CAMERA ) ) ); view_vec = CAMERA_DISTANCE*view_vec; /* Construct view point */ view_pt = plyr.pos + view_vec; /* Make sure view point is above terrain */ float ycoord = find_y_coord( view_pt ); if ( view_pt.y() < ycoord + MIN_CAMERA_HEIGHT ) { view_pt.y() = ycoord + MIN_CAMERA_HEIGHT; } /* Construct view direction */ view_vec = view_pt - plyr.pos; pp::Matrix rot_mat; rot_mat.makeRotation( PLAYER_ANGLE_IN_CAMERA, ppogl::AXIS_X ); view_dir = -1.0*rot_mat.transformVector( view_vec ); break; } default: PP_NOT_REACHED(); } /* Create view matrix */ plyr.view.pos = view_pt; plyr.view.dir = view_dir; plyr.view.up = up_dir; plyr.view.plyr_pos = plyr.pos; plyr.view.initialized = true; }