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;
}
