C++ ogre::Matrix3类代码示例

void PlanningDisplay::calculateRobotPosition()
{
  if (!displaying_kinematic_path_message_)
  {
    return;
  }

  tf::Stamped<tf::Pose> pose(tf::Transform(tf::Quaternion(0, 0, 0, 1.0), tf::Vector3(0, 0, 0)), displaying_kinematic_path_message_->trajectory.joint_trajectory.header.stamp,
                             displaying_kinematic_path_message_->trajectory.joint_trajectory.header.frame_id);

  if (context_->getTFClient()->canTransform(fixed_frame_.toStdString(), displaying_kinematic_path_message_->trajectory.joint_trajectory.header.frame_id, displaying_kinematic_path_message_->trajectory.joint_trajectory.header.stamp))
  {
    try
    {
      context_->getTFClient()->transformPose(fixed_frame_.toStdString(), pose, pose);
    }
    catch (tf::TransformException& e)
    {
      ROS_ERROR( "Error transforming from frame '%s' to frame '%s'", pose.frame_id_.c_str(), fixed_frame_.toStdString().c_str() );
    }
  }

  Ogre::Vector3 position(pose.getOrigin().x(), pose.getOrigin().y(), pose.getOrigin().z());

  double yaw, pitch, roll;
  pose.getBasis().getEulerYPR(yaw, pitch, roll);
  Ogre::Matrix3 orientation;
  orientation.FromEulerAnglesYXZ(Ogre::Radian(yaw), Ogre::Radian(pitch), Ogre::Radian(roll));

  robot_->setPosition(position);
  robot_->setOrientation(orientation);
}

void PlanningDisplay::calculateRobotPosition()
{
  tf::Stamped<tf::Pose> pose( btTransform( btQuaternion( 0, 0, 0 ), btVector3( 0, 0, 0 ) ), ros::Time(), displaying_kinematic_path_message_.frame_id );

  if (tf_->canTransform(target_frame_, displaying_kinematic_path_message_.frame_id, ros::Time()))
  {
    try
    {
      tf_->transformPose( target_frame_, pose, pose );
    }
    catch(tf::TransformException& e)
    {
      ROS_ERROR( "Error transforming from frame '%s' to frame '%s'\n", pose.frame_id_.c_str(), target_frame_.c_str() );
    }
  }

  Ogre::Vector3 position( pose.getOrigin().x(), pose.getOrigin().y(), pose.getOrigin().z() );
  robotToOgre( position );

  btScalar yaw, pitch, roll;
  pose.getBasis().getEulerZYX( yaw, pitch, roll );
  Ogre::Matrix3 orientation;
  orientation.FromEulerAnglesYXZ( Ogre::Radian( yaw ), Ogre::Radian( pitch ), Ogre::Radian( roll ) );

  robot_->setPosition( position );
  robot_->setOrientation( orientation );
}

void RobotBase2DPoseDisplay::transformArrow( const std_msgs::RobotBase2DOdom& message, ogre_tools::Arrow* arrow )
{
  std::string frame_id = message.header.frame_id;
  if ( frame_id.empty() )
  {
    frame_id = fixed_frame_;
  }

  tf::Stamped<tf::Pose> pose( btTransform( btQuaternion( message.pos.th, 0.0f, 0.0f ), btVector3( message.pos.x, message.pos.y, 0.0f ) ),
                              message.header.stamp, message_.header.frame_id );

  try
  {
    tf_->transformPose( fixed_frame_, pose, pose );
  }
  catch(tf::TransformException& e)
  {
    ROS_ERROR( "Error transforming 2d base pose '%s' from frame '%s' to frame '%s'\n", name_.c_str(), message.header.frame_id.c_str(), fixed_frame_.c_str() );
  }

  btScalar yaw, pitch, roll;
  pose.getBasis().getEulerZYX( yaw, pitch, roll );
  Ogre::Matrix3 orient;
  orient.FromEulerAnglesZXY( Ogre::Radian( roll ), Ogre::Radian( pitch ), Ogre::Radian( yaw ) );
  arrow->setOrientation( orient );

  Ogre::Vector3 pos( pose.getOrigin().x(), pose.getOrigin().y(), pose.getOrigin().z() );
  robotToOgre( pos );
  arrow->setPosition( pos );
}

void OrbitOrientedViewController::updateCamera()
{
	float distance = distance_property_->getFloat();
	float yaw = yaw_property_->getFloat();
	float pitch = pitch_property_->getFloat();

	Ogre::Matrix3 rot;
	reference_orientation_.ToRotationMatrix(rot);
	Ogre::Radian rollTarget, pitchTarget, yawTarget;
	rot.ToEulerAnglesXYZ(yawTarget, pitchTarget, rollTarget);

	yaw += rollTarget.valueRadians();
	pitch += pitchTarget.valueRadians();

	Ogre::Vector3 focal_point = focal_point_property_->getVector();

	float x = distance * cos( yaw ) * cos( pitch ) + focal_point.x;
	float y = distance * sin( yaw ) * cos( pitch ) + focal_point.y;
	float z = distance *              sin( pitch ) + focal_point.z;

	Ogre::Vector3 pos( x, y, z );

	camera_->setPosition(pos);
	camera_->setFixedYawAxis(true, target_scene_node_->getOrientation() * Ogre::Vector3::UNIT_Z);
	camera_->setDirection(target_scene_node_->getOrientation() * (focal_point - pos));

	focal_shape_->setPosition( focal_point );
}

void OgrePointSpecification::setLocalOrientation(const Vector3& ori)
{
	Ogre::Matrix3 m;
	m.FromEulerAnglesXYZ(Ogre::Radian(ori.x),Ogre::Radian(ori.y),Ogre::Radian(ori.z));
	Ogre::Quaternion q(m);
	_node->setOrientation(q);
	_node->_update(true,true);
}

Ogre::Matrix3 Utility::convert_btMatrix3(const btMatrix3x3 &m)
{
	Ogre::Matrix3 mat;
	mat.SetColumn(0,Utility::convert_btVector3(m[0]));
	mat.SetColumn(1,Utility::convert_btVector3(m[1]));
	mat.SetColumn(2,Utility::convert_btVector3(m[2]));
	return mat;
}

Ogre::Quaternion LuaScriptUtilities::QuaternionFromRotationDegrees(
    Ogre::Real xRotation, Ogre::Real yRotation, Ogre::Real zRotation)
{
    Ogre::Matrix3 matrix;
    matrix.FromEulerAnglesXYZ(
        Ogre::Degree(xRotation), Ogre::Degree(yRotation), Ogre::Degree(zRotation));
    return Ogre::Quaternion(matrix);
}

void OgrePointSpecification::rotate(const Vector3& orientation)
{
	Ogre::Matrix3 m;
	m.FromEulerAnglesXYZ(Ogre::Radian(orientation.x),Ogre::Radian(orientation.y),Ogre::Radian(orientation.z));
	Ogre::Quaternion q(m);
	_node->rotate(q,Ogre::Node::TS_LOCAL);
	_node->_update(true,true);
}

VEHA::Vector3 OgrePointSpecification::getLocalOrientation() const
{
	Ogre::Quaternion q=_node->getOrientation();
	Ogre::Matrix3 m;
	q.ToRotationMatrix(m);
	Ogre::Radian x,y,z;
	m.ToEulerAnglesXYZ(x,y,z);

	return VEHA::Vector3((double)x.valueRadians(), (double)y.valueRadians(), (double)z.valueRadians());
}

void OgrePointSpecification::setGlobalOrientation(const Vector3& ori)
{
	Ogre::Matrix3 m;
	m.FromEulerAnglesXYZ(Ogre::Radian(ori.x),Ogre::Radian(ori.y),Ogre::Radian(ori.z));
	Ogre::Quaternion q(m);
	if(_parent)
		q=shared_dynamic_cast<OgrePointSpecification>(_parent)->_node->convertWorldToLocalOrientation(q);
	_node->setOrientation(q);
	_node->_update(true,true);
}

 //-------------------------------------------------------------------------
 void
 AFile::setFrameRotation( Ogre::TransformKeyFrame* key_frame
                         , const Ogre::Vector3& v ) const
 {
     Ogre::Quaternion rot;
     Ogre::Matrix3 mat;
     mat.FromEulerAnglesZXY( Ogre::Radian(Ogre::Degree( -v.y )), Ogre::Radian(Ogre::Degree( -v.x )), Ogre::Radian(Ogre::Degree( -v.z )) );
     rot.FromRotationMatrix( mat );
     key_frame->setRotation( rot );
 }

void
BuildTank::setBadGun(Ogre::Radian theta){
		Ogre::Quaternion gunOrientation = mTankGunNode->getOrientation() ;
		// convert orientation to a matrix
		Ogre::Matrix3 tGunMatrix3;
		gunOrientation.ToRotationMatrix( tGunMatrix3 );

		Ogre::Vector3 xBasis = Ogre::Vector3(Ogre::Math::Cos(theta),0, - Ogre::Math::Sin(theta));
		Ogre::Vector3 yBasis = Ogre::Vector3(0,1,0);
		Ogre::Vector3 zBasis = Ogre::Vector3(Ogre::Math::Sin(theta),0,Ogre::Math::Cos(theta));
		Ogre::Matrix3 rotate;
		rotate.FromAxes(xBasis, yBasis, zBasis);
		mGunOrientation = rotate * tGunMatrix3;

		mTankGunNode->setOrientation(Ogre::Quaternion(mGunOrientation));
}

void CameraSceneNode::SetLookAtPoint(Vec3 lap)
{
	//mOgreSceneNode->lookAt(lap, Ogre::Node::TS_WORLD);

	Vec3 z = -(lap - GetPosition()); z.normalise(); // z
	Vec3 x = z.crossProduct(Vec3::NEGATIVE_UNIT_Y); x.normalise(); // x
	Vec3 y = -x.crossProduct(z); y.normalise(); // y

	Ogre::Matrix3 R;
	R.FromAxes(x, y, z);

	Quat q; q.FromRotationMatrix(R);
	mOgreSceneNode->setOrientation(q);

	this->SetOrientation(mOgreSceneNode->getOrientation()*this->GetParent()->GetOrientation().Inverse());
}

Ogre::Vector3 LuaScriptUtilities::QuaternionToRotationDegrees(
    const Ogre::Quaternion& quaternion)
{
    Ogre::Vector3 angles;

    Ogre::Radian xAngle;
    Ogre::Radian yAngle;
    Ogre::Radian zAngle;

    Ogre::Matrix3 rotation;
    quaternion.ToRotationMatrix(rotation);
    rotation.ToEulerAnglesXYZ(xAngle, yAngle, zAngle);

    angles.x = xAngle.valueDegrees();
    angles.y = yAngle.valueDegrees();
    angles.z = zAngle.valueDegrees();

    return angles;
}

void SimplePlayerComponent::HandleEvent(std::shared_ptr<Event> e) {
    // do not react to any events if this component is disabled
    if(!IsEnabled())
        return;

    if(mMouseEnabled && e->GetType() == "DT_MOUSEEVENT") {
        std::shared_ptr<MouseEvent> m = std::dynamic_pointer_cast<MouseEvent>(e);
        if(m->GetAction() == MouseEvent::MOVED) {
            float factor = mMouseSensitivity * -0.01;
            float dx = m->GetMouseState().X.rel * factor;
            float dy = m->GetMouseState().Y.rel * factor * (mMouseYInversed ? -1 : 1);

            if(dx != 0 || dy != 0) {
                // watch out for da gimbal lock !!

                Ogre::Matrix3 orientMatrix;
                GetNode()->GetRotation().ToRotationMatrix(orientMatrix);

                Ogre::Radian yaw, pitch, roll;
                orientMatrix.ToEulerAnglesYXZ(yaw, pitch, roll);

                pitch += Ogre::Radian(dy);
                yaw += Ogre::Radian(dx);

                // do not let it look completely vertical, or the yaw will break
                if(pitch > Ogre::Degree(89.9))
                    pitch = Ogre::Degree(89.9);

                if(pitch < Ogre::Degree(-89.9))
                    pitch = Ogre::Degree(-89.9);

                orientMatrix.FromEulerAnglesYXZ(yaw, pitch, roll);

                Ogre::Quaternion rot;
                rot.FromRotationMatrix(orientMatrix);
                GetNode()->SetRotation(rot);
            }
        }
    }
}