C++ Quaternion::getYaw方法代码示例

void Application::updateStats()
{
	static Ogre::String currFps = "Current FPS: ";
	static Ogre::String avgFps = "Average FPS: ";
	static Ogre::String bestFps = "Best FPS: ";
	static Ogre::String worstFps = "Worst FPS: ";
	static Ogre::String tris = "Triangle Count: ";
	static Ogre::String batches = "Batch Count: ";

	// update stats when necessary
	try {
		Ogre::OverlayElement* guiAvg = Ogre::OverlayManager::getSingleton().getOverlayElement("Core/AverageFps");
		Ogre::OverlayElement* guiCurr = Ogre::OverlayManager::getSingleton().getOverlayElement("Core/CurrFps");
		Ogre::OverlayElement* guiBest = Ogre::OverlayManager::getSingleton().getOverlayElement("Core/BestFps");
		Ogre::OverlayElement* guiWorst = Ogre::OverlayManager::getSingleton().getOverlayElement("Core/WorstFps");

		const Ogre::RenderTarget::FrameStats& stats = m_window->getStatistics();
		guiAvg->setCaption(avgFps + Ogre::StringConverter::toString(stats.avgFPS));
		guiCurr->setCaption(currFps + Ogre::StringConverter::toString(stats.lastFPS));
		guiBest->setCaption(bestFps + Ogre::StringConverter::toString(stats.bestFPS)
			+" "+Ogre::StringConverter::toString(stats.bestFrameTime)+" ms");
		guiWorst->setCaption(worstFps + Ogre::StringConverter::toString(stats.worstFPS)
			+" "+Ogre::StringConverter::toString(stats.worstFrameTime)+" ms");

		Ogre::OverlayElement* guiTris = Ogre::OverlayManager::getSingleton().getOverlayElement("Core/NumTris");
		guiTris->setCaption(tris + Ogre::StringConverter::toString(stats.triangleCount));

		Ogre::OverlayElement* guiBatches = Ogre::OverlayManager::getSingleton().getOverlayElement("Core/NumBatches");
		guiBatches->setCaption(batches + Ogre::StringConverter::toString(stats.batchCount));

		/////////////////////////
		
		Ogre::Vector3 camPos = m_camera->getPosition();
		Ogre::Quaternion camOri = m_camera->getOrientation();
		Ogre::Vector3 freddyPos = m_sceneNode->getPosition();
		Ogre::Quaternion freddyOri = m_sceneNode->getOrientation();
		
		Ogre::OverlayElement* guiDbg = Ogre::OverlayManager::getSingleton().getOverlayElement("Core/DebugText");
		guiDbg->setTop(0);

		Ogre::String message = 
			"Camera Position: x: "+Ogre::StringConverter::toString(camPos.x)+", y: "+Ogre::StringConverter::toString(camPos.y)+", z:"+Ogre::StringConverter::toString(camPos.z) + "\n" + 
			"Camera orientation: yaw: "+Ogre::StringConverter::toString(camOri.getYaw())+", pitch: "+Ogre::StringConverter::toString(camOri.getPitch())+", roll:"+Ogre::StringConverter::toString(camOri.getRoll()) + "\n" + 
			"Freddy position: x: "+Ogre::StringConverter::toString(freddyPos.x)+", y: "+Ogre::StringConverter::toString(freddyPos.y)+", z:"+Ogre::StringConverter::toString(freddyPos.z) + "\n" + 
			"Freddy orientation: yaw: "+Ogre::StringConverter::toString(freddyOri.getYaw())+", pitch: "+Ogre::StringConverter::toString(freddyOri.getPitch())+", roll:"+Ogre::StringConverter::toString(freddyOri.getRoll());
		
		guiDbg->setCaption(message);
		debugWindow->setText("This is a demo!");
	}
	catch(...) { /* ignore */ }
}

void  OgreWidget::mouseReleaseEvent(QMouseEvent * e)
{
  if (e->button() == Qt::MiddleButton || e->button() == Qt::LeftButton)
    {
      m_mouseButtonsPressed &= e->buttons();
      m_oldPos = QPoint(InvalidMousePoint);

      if (m_snapeToGrid)
        {
          Ogre::Vector3 pos = m_selectionManager.getPosition();
          int           gridSpace = DataManager::getSingleton()->getGridSpace();

          pos.x = qFloor((pos.x + gridSpace / 2.f) / gridSpace) * gridSpace;
          pos.y = qFloor((pos.y + gridSpace / 2.f) / gridSpace) * gridSpace;
          pos.z = qFloor((pos.z + gridSpace / 2.f) / gridSpace) * gridSpace;
          m_selectionManager.setPosition(pos.x, pos.y, pos.z);
          emit itemMoved();
        }
      if (m_snapeToAngle)
        {
          Ogre::Quaternion  orientation = m_selectionManager.getOrientation();
          int               snapAngle = DataManager::getSingleton()->getSnapAngle();

          if (m_constraintedX)
            {
              m_selectionManager.pitch(-orientation.getPitch());
              m_selectionManager.pitch(Ogre::Degree(qFloor((orientation.getPitch().valueDegrees() + snapAngle / 2.f) / snapAngle) * snapAngle));
            }
          else if (m_constraintedY)
            {
              m_selectionManager.yaw(-orientation.getYaw());
              m_selectionManager.yaw(Ogre::Degree(qFloor((orientation.getYaw().valueDegrees() + snapAngle / 2.f) / snapAngle) * snapAngle));
            }
          else if (m_constraintedZ)
            {
              m_selectionManager.roll(-orientation.getRoll());
              m_selectionManager.roll(Ogre::Degree(qFloor((orientation.getRoll().valueDegrees() + snapAngle / 2.f) / snapAngle) * snapAngle));
            }
          emit itemMoved();
        }
      update();
      e->accept();
    }
  else
    {
      e->ignore();
    }
}

void EnemyController::approach(void)
{
	EnemyPlane* enemy = static_cast<EnemyPlane*>(IDManager::getPointer(_enemyName, ACTOR));
	Ogre::Vector3 temp = FCKnowledge::getSingleton().getPlayerPosition() - enemy->getPosition();
	Ogre::Vector3 direction = temp * enemy->getAxis();
	//std::cout<<direction.x<<" "<<direction.y<<" "<<direction.z<<std::endl;
	if(direction.angleBetween(Ogre::Vector3::NEGATIVE_UNIT_Z) >= Ogre::Radian(Ogre::Degree(1)))
	{
		Ogre::Quaternion test = direction.getRotationTo(Ogre::Vector3::NEGATIVE_UNIT_Z);
		Ogre::Degree angle = enemy->getRotateLimit();
		
		double yawNum = test.getYaw().valueDegrees()/(angle*WORLD_UPDATE_INTERVAL).valueDegrees();
		yawNum = Ogre::Math::Clamp(yawNum, -1.0, 1.0);
		enemy->yaw(yawNum);

		double pitchNum = test.getPitch().valueDegrees()/(angle*WORLD_UPDATE_INTERVAL).valueDegrees();
		pitchNum = Ogre::Math::Clamp(pitchNum, -1.0, 1.0);
		enemy->pitch(pitchNum);
		
		double rollNum = test.getRoll().valueDegrees()/(angle*WORLD_UPDATE_INTERVAL).valueDegrees();
		rollNum = Ogre::Math::Clamp(rollNum, -1.0, 1.0);
		enemy->roll(rollNum);
		
	}
	else
	{
		enemy->yaw(0);
		enemy->pitch(0);
		enemy->roll(0);
	}
}

void ProjectileManager::createProjectile(const Ogre::Vector3& tankPosition, const Ogre::Quaternion& turretOrientation, 
										 const Ogre::Degree& angle, const float& velocity, const float& dmg){
	
	std::ostringstream oss;
	oss << "Projectile" << time(0) << projectiles.size() << counter++;										 
											 
	Ogre::ParticleSystem* particleSystem = mSceneMgr->createParticleSystem(oss.str(), "Examples/PurpleFountain");

	scaleBy(1.f, particleSystem);

	Ogre::SceneNode* parentParticleSn = mSceneMgr->getRootSceneNode()->createChildSceneNode();

	Ogre::SceneNode* particleSn = parentParticleSn->createChildSceneNode();

	Ogre::Vector3 start(-115.f, 10.f, 0.f);

	parentParticleSn->setPosition(tankPosition);
	particleSn->setPosition(start);

	parentParticleSn->yaw(turretOrientation.getYaw());

	particleSn->attachObject(particleSystem);

	particleSn->roll(Ogre::Degree(-90.f));

	particleSn->scale(Ogre::Vector3(0.1f));

	projectiles.insert(new Projectile(start, particleSn, angle, velocity, dmg));
}

void FPSViewController::setPropertiesFromCamera( Ogre::Camera* source_camera )
{
  Ogre::Quaternion quat = source_camera->getOrientation() * ROBOT_TO_CAMERA_ROTATION.Inverse();
  float yaw = quat.getRoll( false ).valueRadians(); // OGRE camera frame looks along -Z, so they call rotation around Z "roll".
  float pitch = quat.getYaw( false ).valueRadians(); // OGRE camera frame has +Y as "up", so they call rotation around Y "yaw".

  Ogre::Vector3 direction = quat * Ogre::Vector3::NEGATIVE_UNIT_Z;

  if ( direction.dotProduct( Ogre::Vector3::NEGATIVE_UNIT_Z ) < 0 )
  {
    if ( pitch > Ogre::Math::HALF_PI )
    {
      pitch -= Ogre::Math::PI;
    }
    else if ( pitch < -Ogre::Math::HALF_PI )
    {
      pitch += Ogre::Math::PI;
    }

    yaw = -yaw;

    if ( direction.dotProduct( Ogre::Vector3::UNIT_X ) < 0 )
    {
      yaw -= Ogre::Math::PI;
    }
    else
    {
      yaw += Ogre::Math::PI;
    }
  }

  pitch_property_->setFloat( pitch );
  yaw_property_->setFloat( mapAngleTo0_2Pi( yaw ));
  position_property_->setVector( source_camera->getPosition() );
}

    void PhysicsSystem::doPhysics(float dt, const std::vector<std::pair<std::string, Ogre::Vector3> >& actors)
    {
        //set the DebugRenderingMode. To disable it,set it to 0
        //eng->setDebugRenderingMode(1);

        //set the walkdirection to 0 (no movement) for every actor)
        for(std::map<std::string,OEngine::Physic::PhysicActor*>::iterator it = mEngine->PhysicActorMap.begin(); it != mEngine->PhysicActorMap.end();it++)
        {
            OEngine::Physic::PhysicActor* act = it->second;
            act->setWalkDirection(btVector3(0,0,0));
        }
		playerMove::playercmd& pm_ref = playerphysics->cmd;

        pm_ref.rightmove = 0;
        pm_ref.forwardmove = 0;
        pm_ref.upmove = 0;

		//playerphysics->ps.move_type = PM_NOCLIP;
        for (std::vector<std::pair<std::string, Ogre::Vector3> >::const_iterator iter (actors.begin());
            iter!=actors.end(); ++iter)
        {
            //dirty stuff to get the camera orientation. Must be changed!

            Ogre::SceneNode *sceneNode = mRender.getScene()->getSceneNode (iter->first);
            Ogre::Vector3 dir;
            Ogre::Node* yawNode = sceneNode->getChildIterator().getNext();
            Ogre::Node* pitchNode = yawNode->getChildIterator().getNext();
			Ogre::Quaternion yawQuat = yawNode->getOrientation();
            Ogre::Quaternion pitchQuat = pitchNode->getOrientation();



            playerphysics->ps.viewangles.x = pitchQuat.getPitch().valueDegrees();

			playerphysics->ps.viewangles.y = yawQuat.getYaw().valueDegrees() *-1 + 90;


                Ogre::Quaternion quat = yawNode->getOrientation();
                Ogre::Vector3 dir1(iter->second.x,iter->second.z,-iter->second.y);

				pm_ref.rightmove = -iter->second.x;
				pm_ref.forwardmove = -iter->second.y;
				pm_ref.upmove = iter->second.z;



            }





        mEngine->stepSimulation(dt);
    }

void Application::updateLogic( const float elapsedSeconds )
{
	const float TURN_DEGREES_PER_SECOND = 360;
	const float MOVEMENT_UNITS_PER_SECOND = 4;

	m_movingDirection = 0;

	if ( m_keyboard->isKeyDown( OIS::KC_LEFT ) )
	{
		m_sceneNode->yaw( Ogre::Degree( -TURN_DEGREES_PER_SECOND * elapsedSeconds ) );

		Ogre::Quaternion nodeOri = m_sceneNode->getOrientation();
		m_cameraController->addOrientation(nodeOri.getYaw().valueDegrees(), nodeOri.getPitch().valueDegrees());
	}

	if ( m_keyboard->isKeyDown( OIS::KC_RIGHT ) )
	{
		m_sceneNode->yaw( Ogre::Degree( -TURN_DEGREES_PER_SECOND * elapsedSeconds ) );
		
		Ogre::Quaternion nodeOri = m_sceneNode->getOrientation();
		m_cameraController->addOrientation(nodeOri.getYaw().valueDegrees(), nodeOri.getPitch().valueDegrees());
	}

	if ( m_keyboard->isKeyDown( OIS::KC_UP ) )
	{
		m_sceneNode->translate( Ogre::Vector3::UNIT_Z * MOVEMENT_UNITS_PER_SECOND * elapsedSeconds, Ogre::Node::TS_LOCAL );
		m_movingDirection++;

		Ogre::Vector3 nodePos = m_sceneNode->getPosition();
		m_cameraController->setLookAtPosition(nodePos.x, nodePos.y, nodePos.z);
	}

	if ( m_keyboard->isKeyDown( OIS::KC_DOWN ) )
	{
		m_sceneNode->translate( Ogre::Vector3::NEGATIVE_UNIT_Z * MOVEMENT_UNITS_PER_SECOND * elapsedSeconds, Ogre::Node::TS_LOCAL );
		m_movingDirection--;		
	
		Ogre::Vector3 nodePos = m_sceneNode->getPosition();
		m_cameraController->setLookAtPosition(nodePos.x, nodePos.y, nodePos.z);
	}
}

void CameraManager::setOrientation(Ogre::Quaternion ori){

	mNodeYaw = ori.getYaw();
	mNodePitch = ori.getPitch();
	mNodeRoll = ori.getRoll();

	if(mCameraMode == OCULUS)
		mOculusCamera->setOrientation(ori);
	else
		mSimpleCamera->setOrientation(ori);

}

inline void GUIRadarLayout::DrawPos(unsigned num, Ogre::Vector3 dst_pos, Ogre::Quaternion players_orientation)
{
	Ogre::Rectangle2D *row = Rows.Data[num];
	Ogre::Rectangle2D *obj = Objects.Data[num];
	//dst_pos -= CommonDeclarations::MaxDistForRadar/2;
    Ogre::Quaternion orientation(Ogre::Degree(30), Ogre::Vector3::UNIT_X);
	Ogre::Quaternion player_rotation(players_orientation.getYaw(), Ogre::Vector3::UNIT_Y);
	player_rotation = Ogre::Quaternion(players_orientation.getRoll(), Ogre::Vector3::UNIT_Z) * player_rotation;
	
	Ogre::Vector3 rel_pos = dst_pos, base_pos = rel_pos;

	base_pos.y=0;

	float scr_height = rel_pos.y*ScreenSize / CommonDeclarations::MaxDistForRadar;

	base_pos = orientation * players_orientation * base_pos;
	base_pos = (base_pos / CommonDeclarations::MaxDistForRadar)*ScreenSize;

	base_pos.x += 0.75;
	base_pos.y -= 0.75;

	if (scr_height==0)
		scr_height=0.01;

	float left=base_pos.x-0.0035, top=base_pos.y, right=base_pos.x+0.0035, bottom=base_pos.y+scr_height;    

	//	
	obj->setCorners(base_pos.x-0.01, base_pos.y+0.005, base_pos.x+0.01, base_pos.y-0.005);
	if (scr_height>0)
	{       
		left=base_pos.x-0.0025;
		top=base_pos.y+scr_height;
		right=base_pos.x+0.0025;
		bottom=base_pos.y;
		obj->setMaterial("Radar/ObjMaterialUp");
	} else 
		if (scr_height<0)
		{   
			left=base_pos.x-0.0025;
			top=base_pos.y;
			right=base_pos.x+0.0025;
			bottom=base_pos.y+scr_height;
			obj->setMaterial("Radar/ObjMaterialDn");
		}
		row->setCorners(left, top, right, bottom);	
	
	row->setVisible(true);
	obj->setVisible(true);
}

void game_tick( GameThreadSockets & gsockets, GameState & gs, SharedRenderState & srs, unsigned int now ) {
  Ogre::Quaternion o;
  uint8_t b;
  zstr_send( gsockets.zmq_input_req, "mouse_state" );
  char * mouse_state = zstr_recv( gsockets.zmq_input_req );
  parse_mouse_state( mouse_state, o, b );
  free( mouse_state );

  uint8_t w, a, s, d, spc, alt;
  zstr_send( gsockets.zmq_input_req, "kb_state" );
  char * kb_state = zstr_recv( gsockets.zmq_input_req );
  parse_kb_state( kb_state, w, a, s, d, spc, alt );
  free( kb_state );

  // yaw 0 looks towards -Z
  float yaw = o.getYaw().valueRadians();
  Ogre::Vector3 xp( cosf( yaw ), 0.0, -sinf( yaw ) );
  Ogre::Vector3 yp( -sinf( yaw ), 0.0, -cosf( yaw ) );

  float speed = 10.0f;
  if ( w ) {
    srs.position += speed * yp;
  }
  if ( s ) {
    srs.position -= speed * yp;
  }
  if ( a ) {
    srs.position -= speed * xp;
  }
  if ( d ) {
    srs.position += speed * xp;
  }
  if ( spc ) {
    srs.position[1] += speed;
  }
  if ( alt ) {
    srs.position[1] -= speed;
  }
}

// Returns volume for the right ear.
// If the sound is directly to the right, that channel gets 254 of intensity.
// If the sound is directly in front of us, each channel gets 254/2 of intensity.
int SoundManager::calcPanning(Ogre::Camera* mCamera, Ogre::Vector3 soundPosition) {
    Ogre::Vector3 camDirection = mCamera->getOrientation() * Ogre::Vector3::NEGATIVE_UNIT_Z;
    Ogre::Vector3 soundDir = soundPosition - mCamera->getPosition();
    soundDir.normalise();
    // Retrieve the angle made between the sound position and the camera's direction,
    // which should be pointing forwards.
    Ogre::Quaternion q = camDirection.getRotationTo(soundDir);
    Ogre::Radian radians = q.getYaw();

    // anything left of the camera returns positive degrees
    float degrees = radians.valueDegrees();

    int leftIntensity;
    int rightIntensity;

    // from 0 to -179 it's the right ear.
    if(degrees <= 0) {
      //std::cout << "degrees " << degrees << std::endl;
      degrees *= -1;
      if(degrees > 90)
          degrees = 90 - (degrees - 90); // if 91, we subtract 1 and go back to 89
      // Intensity can never be 255 because then the effect cancels out.
      int intensity = (degrees / 90.0) * 254/2 + 254/2;
      //std::cout << "intensity " << intensity << std::endl;
      rightIntensity = intensity;
      leftIntensity = 255 - (rightIntensity);
    }
    // from 0 to 179 it's the left ear.
    else {
      if(degrees > 90)
          degrees = 90 - (degrees - 90); 
      int intensity = (degrees / 90) * 254/2 + 254/2;
      //std::cout << "intensity " << intensity << std::endl;
      leftIntensity = intensity;
      rightIntensity = 254 - (leftIntensity);
    }
    return rightIntensity;
}

    void PhysicsSystem::doPhysics(float dt, const std::vector<std::pair<std::string, Ogre::Vector3> >& actors)
    {
        //set the DebugRenderingMode. To disable it,set it to 0
        //eng->setDebugRenderingMode(1);

        //set the walkdirection to 0 (no movement) for every actor)
        for(std::map<std::string,OEngine::Physic::PhysicActor*>::iterator it = mEngine->PhysicActorMap.begin(); it != mEngine->PhysicActorMap.end();it++)
        {
            OEngine::Physic::PhysicActor* act = it->second;
            act->setWalkDirection(btVector3(0,0,0));
        }
		playerMove::playercmd& pm_ref = playerphysics->cmd;

        pm_ref.rightmove = 0;
        pm_ref.forwardmove = 0;
        pm_ref.upmove = 0;

		//playerphysics->ps.move_type = PM_NOCLIP;
        for (std::vector<std::pair<std::string, Ogre::Vector3> >::const_iterator iter (actors.begin());
            iter!=actors.end(); ++iter)
        {
            OEngine::Physic::PhysicActor* act = mEngine->getCharacter(iter->first);
			//if(iter->first == "player")
			//	std::cout << "This is player\n";
            //dirty stuff to get the camera orientation. Must be changed!

            Ogre::SceneNode *sceneNode = mRender.getScene()->getSceneNode (iter->first);
            Ogre::Vector3 dir;
            Ogre::Node* yawNode = sceneNode->getChildIterator().getNext();
            Ogre::Node* pitchNode = yawNode->getChildIterator().getNext();
			Ogre::Quaternion yawQuat = yawNode->getOrientation();
            Ogre::Quaternion pitchQuat = pitchNode->getOrientation();

            // unused
            //Ogre::Quaternion both = yawQuat * pitchQuat;

            playerphysics->ps.viewangles.x = pitchQuat.getPitch().valueDegrees();
            playerphysics->ps.viewangles.z = 0;
			playerphysics->ps.viewangles.y = yawQuat.getYaw().valueDegrees() *-1 + 90;

            if(mFreeFly)
            {
                Ogre::Vector3 dir1(iter->second.x,iter->second.z,-iter->second.y);

				pm_ref.rightmove = -dir1.x;
				pm_ref.forwardmove = dir1.z;
				pm_ref.upmove = dir1.y;


				//std::cout << "Current angle" << yawQuat.getYaw().valueDegrees() - 90<< "\n";
				//playerphysics->ps.viewangles.x = pitchQuat.getPitch().valueDegrees();
				//std::cout << "Pitch: " << yawQuat.getPitch() << "Yaw:" << yawQuat.getYaw() << "Roll: " << yawQuat.getRoll() << "\n";
                dir = 0.07*(yawQuat*pitchQuat*dir1);
            }
            else
            {

                Ogre::Quaternion quat = yawNode->getOrientation();
                Ogre::Vector3 dir1(iter->second.x,iter->second.z,-iter->second.y);

				pm_ref.rightmove = -dir1.x;
				pm_ref.forwardmove = dir1.z;
				pm_ref.upmove = dir1.y;



                dir = 0.025*(quat*dir1);
            }


            //set the walk direction
            act->setWalkDirection(btVector3(dir.x,-dir.z,dir.y));
        }
        mEngine->stepSimulation(dt);
    }

  void
  CovarianceVisual::setMessage
  (const geometry_msgs::PoseWithCovariance& msg)
  {
    // Construct pose position and orientation.
    const geometry_msgs::Point& p = msg.pose.position;
    Ogre::Vector3 position (p.x, p.y, p.z);
    Ogre::Quaternion orientation
      (msg.pose.orientation.w,
       msg.pose.orientation.x,
       msg.pose.orientation.y,
       msg.pose.orientation.z);

    // Set position and orientation for axes scene node.
    if(!position.isNaN())
      axes_->setPosition (position);
    else
      ROS_WARN_STREAM_THROTTLE(1, "position contains NaN: " << position);
    if(!orientation.isNaN())
      axes_->setOrientation (orientation);
    else
      ROS_WARN_STREAM_THROTTLE(1, "orientation contains NaN: " << orientation);

    // check for NaN in covariance
    for (unsigned i = 0; i < 3; ++i) {
      if(isnan(msg.covariance[i])) {
        ROS_WARN_THROTTLE(1, "covariance contains NaN");
        return;
      }
    }

    // Compute eigen values and vectors for both shapes.
    std::pair<Eigen::Matrix3d, Eigen::Vector3d>
      positionEigenVectorsAndValues
      (computeEigenValuesAndVectors (msg, 0));
    std::pair<Eigen::Matrix3d, Eigen::Vector3d>
      orientationEigenVectorsAndValues
      (computeEigenValuesAndVectors (msg, 3));

    Ogre::Quaternion positionQuaternion
      (computeRotation (msg, positionEigenVectorsAndValues));
    Ogre::Quaternion orientationQuaternion
      (computeRotation (msg, orientationEigenVectorsAndValues));

    positionNode_->setOrientation (positionQuaternion);
    orientationNode_->setOrientation (orientationQuaternion);

    // Compute scaling.
    Ogre::Vector3 positionScaling
      (std::sqrt (positionEigenVectorsAndValues.second[0]),
       std::sqrt (positionEigenVectorsAndValues.second[1]),
       std::sqrt (positionEigenVectorsAndValues.second[2]));
    positionScaling *= scaleFactor_;

    Ogre::Vector3 orientationScaling
      (std::sqrt (orientationEigenVectorsAndValues.second[0]),
       std::sqrt (orientationEigenVectorsAndValues.second[1]),
       std::sqrt (orientationEigenVectorsAndValues.second[2]));
    orientationScaling *= scaleFactor_;

    // Set the scaling.
    if(!positionScaling.isNaN())
      positionNode_->setScale (positionScaling);
    else
      ROS_WARN_STREAM("positionScaling contains NaN: " << positionScaling);

    if(!orientationScaling.isNaN())
      orientationNode_->setScale (orientationScaling);
    else
      ROS_WARN_STREAM("orientationScaling contains NaN: " << orientationScaling);

    // Debugging.
    ROS_DEBUG_STREAM_THROTTLE
      (1.,
       "Position:\n"
       << position << "\n"
       << "Positional part 3x3 eigen values:\n"
       << positionEigenVectorsAndValues.second << "\n"
       << "Positional part 3x3 eigen vectors:\n"
       << positionEigenVectorsAndValues.first << "\n"
       << "Sphere orientation:\n"
       << positionQuaternion << "\n"
       << positionQuaternion.getRoll () << " "
       << positionQuaternion.getPitch () << " "
       << positionQuaternion.getYaw () << "\n"
       << "Sphere scaling:\n"
       << positionScaling << "\n"
       << "Rotational part 3x3 eigen values:\n"
       << orientationEigenVectorsAndValues.second << "\n"
       << "Rotational part 3x3 eigen vectors:\n"
       << orientationEigenVectorsAndValues.first << "\n"
       << "Cone orientation:\n"
       << orientationQuaternion << "\n"
       << orientationQuaternion.getRoll () << " "
       << orientationQuaternion.getPitch () << " "
       << orientationQuaternion.getYaw () << "\n"
       << "Cone scaling:\n"
       << orientationScaling
       );
  }

//----------------------------------------------------------------------------------------------------------------------------------
///  Init  dynamics
//----------------------------------------------------------------------------------------------------------------------------------
void CARDYNAMICS::Init(
	class SETTINGS* pSet1, class Scene* pScene1, class FluidsXml* pFluids1,
	COLLISION_WORLD & world,
	const MODEL & chassisModel, const MODEL & wheelModelFront, const MODEL & wheelModelRear,
	const MATHVECTOR<Dbl,3> & position, const QUATERNION<Dbl> & orientation)
{
	pSet = pSet1;  pScene = pScene1;  pFluids = pFluids1;
	this->world = &world;

	MATHVECTOR<Dbl,3> zero(0, 0, 0);
	body.SetPosition(position);
	body.SetOrientation(orientation);
	body.SetInitialForce(zero);
	body.SetInitialTorque(zero);
	cam_body.SetPosition(zero);
	cam_body.SetInitialForce(zero);

	if (sphere)
	{	Ogre::Quaternion q = Axes::toOgre(orientation);
		sphereYaw = PI_d - q.getYaw().valueRadians();
	}
	
	// init engine
	engine.SetInitialConditions();


	// init chassis
	btTransform tr;
	tr.setIdentity();

	AABB <float> box = chassisModel.GetAABB();
	for (int i = 0; i < 4; i++)
	{
		MATHVECTOR<float,3> wheelpos = GetLocalWheelPosition(WHEEL_POSITION(i), 0);

		const MODEL * wheelmodel = &wheelModelFront;
		if (i > 1) wheelmodel = &wheelModelRear;

		AABB <float> wheelaabb;
		float sidefactor = 1.0;
		if (i == 1 || i == 3) sidefactor = -1.0;

		wheelaabb.SetFromCorners(
			wheelpos - wheelmodel->GetAABB().GetSize() * 0.5 * sidefactor,
			wheelpos + wheelmodel->GetAABB().GetSize() * 0.5 * sidefactor);
		box.CombineWith(wheelaabb);
	}


	///  chassis shape  ---------------------------------------------------------
	const MATHVECTOR<Dbl,3> verticalMargin(0, 0, 0.3);
	btVector3 origin = ToBulletVector(box.GetCenter() + verticalMargin - center_of_mass);
	btVector3 size = ToBulletVector(box.GetSize() - verticalMargin);

	btCollisionShape* chassisShape;
	if (sphere)
	{	chassisShape = new btSphereShape(1.f);
		//chassisShape = new btBoxShape(btVector3(1.f,1.f,0.2f));
		chassisShape->setMargin(0.05f);  //!? doesnt work, bounces too much
	}else
	{	/// todo: all params in .car
		// y| length  x- width  z^ height
		btScalar w = size.getX()*0.2, r = size.getZ()*0.3, h = 0.45;

		///  spheres
		btScalar l0 = 0.f, w0 = 0.f, h0 = 0.f;
		if (coll_R > 0.f)  r = coll_R;  l0 = coll_Lofs;
		if (coll_W > 0.f)  w = coll_W;  w0 = coll_Wofs;
		if (coll_H > 0.f)  h = coll_H;	h0 = coll_Hofs;
		origin = btVector3(l0, w0, h0);

		btScalar r2 = r*0.6;
		btScalar l1 = coll_posLfront, l2 = coll_posLback, l1m = l1*0.5, l2m = l2*0.5;

		//LogO("Car shape dims:  r="+toStr(r)+"  w="+toStr(w)+"  h="+toStr(h)+"  h0="+toStr(h0));
		//LogO("Car offset:  x="+toStr(origin.x())+"  y="+toStr(origin.y())+"  z="+toStr(origin.z()));

		const int numSph = 14;  int i = 0;
		btScalar rad[numSph];  btVector3 pos[numSph];
		float ww = hover ? 0.2f : 1.f;  ///
		pos[i] = btVector3( l1 , -w*ww, -h);    	rad[i] = r2;  ++i;  // front
		pos[i] = btVector3( l1 ,  w*ww, -h);    	rad[i] = r2;  ++i;
		pos[i] = btVector3( l1m, -w*ww, -h);    	rad[i] = r;   ++i;  // front near
		pos[i] = btVector3( l1m,  w*ww, -h);    	rad[i] = r;   ++i;

		pos[i] = btVector3( l2m, -w,    -h);    	rad[i] = r;   ++i;  // rear near
		pos[i] = btVector3( l2m,  w,    -h);    	rad[i] = r;   ++i;
		pos[i] = btVector3( l2 , -w,    -h);    	rad[i] = r2;  ++i;  // rear
		pos[i] = btVector3( l2 ,  w,    -h);    	rad[i] = r2;  ++i;

		pos[i] = btVector3( 0.4, -w*0.8*ww, h*0.2);	rad[i] = r2;  ++i;  // top
		pos[i] = btVector3( 0.4,  w*0.8*ww, h*0.2);	rad[i] = r2;  ++i;
		pos[i] = btVector3(-0.3, -w*0.8*ww, h*0.4);	rad[i] = r2;  ++i;
		pos[i] = btVector3(-0.3,  w*0.8*ww, h*0.4);	rad[i] = r2;  ++i;
		pos[i] = btVector3(-1.1, -w*0.8*ww, h*0.2);	rad[i] = r2;  ++i;  // top rear
		pos[i] = btVector3(-1.1,  w*0.8*ww, h*0.2);	rad[i] = r2;  ++i;

//.........这里部分代码省略.........

/**
 * @brief Returns the rotation Vector
 *
 * @param mode
 */
Vector Transform::getRotation	()
{
	Ogre::Quaternion rot = m_4x4.extractQuaternion();
	return Vector(	rot.getYaw().valueRadians(), rot.getPitch().valueRadians(),	rot.getRoll().valueRadians() );
}