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

//-------------------------------------------------------------------------------------
void AxisLines::initAxis(Ogre::String boneName, Ogre::Entity* entity, Ogre::SceneManager* mSceneManager)
{

	if(isXVisible)	/* red */
	{
		xLine = new DynamicLines(Ogre::RenderOperation::OT_LINE_LIST);
		entity->attachObjectToBone(boneName, xLine);
		xLine->setMaterial(color1);
	}

	if(isYVisible) /* green */
	{
		yLine = new DynamicLines(Ogre::RenderOperation::OT_LINE_LIST);
		entity->attachObjectToBone(boneName, yLine);
		yLine->setMaterial(color2);
	}

	if(isZVisible) /* blue */
	{
		zLine = new DynamicLines(Ogre::RenderOperation::OT_LINE_LIST);
		entity->attachObjectToBone(boneName, zLine);
		zLine->setMaterial(color3);
	}

	Ogre::Bone* bone = entity->getSkeleton()->getBone(boneName);
	Ogre::Quaternion q = bone->getOrientation();
	this->updateLines(q.xAxis(), q.yAxis(), q.zAxis());
}

	bool Framework::frameRenderingQueued(const Ogre::FrameEvent& evt)
	{
		mTrayMgr->frameRenderingQueued(evt);

		if (!mTrayMgr->isDialogVisible())
		{
			mCameraMan->frameRenderingQueued(evt);   // if dialog isn't up, then update the camera

			if (mDetailsPanel->isVisible())   // if details panel is visible, then update its contents
			{
				mDetailsPanel->setParamValue(0, Ogre::StringConverter::toString(mCamera->getDerivedPosition().x));
				mDetailsPanel->setParamValue(1, Ogre::StringConverter::toString(mCamera->getDerivedPosition().y));
				mDetailsPanel->setParamValue(2, Ogre::StringConverter::toString(mCamera->getDerivedPosition().z));
				mDetailsPanel->setParamValue(4, Ogre::StringConverter::toString(mCamera->getDerivedOrientation().w));
				mDetailsPanel->setParamValue(5, Ogre::StringConverter::toString(mCamera->getDerivedOrientation().x));
				mDetailsPanel->setParamValue(6, Ogre::StringConverter::toString(mCamera->getDerivedOrientation().y));
				mDetailsPanel->setParamValue(7, Ogre::StringConverter::toString(mCamera->getDerivedOrientation().z));

#ifdef USE_RTSHADER_SYSTEM
				mDetailsPanel->setParamValue(14, Ogre::StringConverter::toString(mShaderGenerator->getVertexShaderCount()));
				mDetailsPanel->setParamValue(15, Ogre::StringConverter::toString(mShaderGenerator->getFragmentShaderCount()));		
#endif

				Ogre::Quaternion q = mCamera->getDerivedOrientation();
				mDetailsPanel->setParamValue(16,  Ogre::StringConverter::toString(q.xAxis() ) );
				mDetailsPanel->setParamValue(17,  Ogre::StringConverter::toString(q.yAxis() ));
				mDetailsPanel->setParamValue(18,  Ogre::StringConverter::toString(q.zAxis() ));															

			}	

		}

		return true;
	}

void Vehicle::jet(NxReal stiff)
{
	Ogre::Quaternion quat = UtilityFunc::NxQuat_ToOgre_Quat(mActor->getGlobalOrientationQuat());
	NxVec3 nxv3			  = UtilityFunc::OgreVec3_To_NxVec3(quat.zAxis());

	nxv3.normalize();
	mActor->addForce(nxv3 * stiff, NX_IMPULSE);
}

        void SoundListener::updateListener()
        {                           
          Ogre::Vector3 position = this->getPosition().Meter() ;
          alListener3f(AL_POSITION, (float)position.x, (float)position.y, (float)position.z);
          Ogre::Quaternion orientation = this->getOrientation().getQuaternion();

          ALfloat openal_orientation[6] ;
          openal_orientation[0] = orientation.zAxis().x ;
          openal_orientation[1] = orientation.zAxis().y ;
          openal_orientation[2] = orientation.zAxis().z ;
          openal_orientation[3] = orientation.yAxis().x ;
          openal_orientation[4] = orientation.yAxis().y ;
          openal_orientation[5] = orientation.yAxis().z ;
          
          alListenerfv(AL_ORIENTATION, openal_orientation) ; 
                      
          Ogre::Vector3 speed = this->getSpeed().MeterPerSecond();
          alListener3f(AL_VELOCITY, (float)speed.x, (float)speed.y, (float)speed.z) ;
          alListenerf(AL_GAIN,getGain()) ;
        }

/*/////////////////////////////////////////////////////////////////*/
void OgreOggListener::setOrientation(const Ogre::Quaternion& q) {
  Ogre::Vector3 vDirection = q.zAxis();
  Ogre::Vector3 vUp = q.yAxis();

  mOrientation[0] = -vDirection.x;
  mOrientation[1] = -vDirection.y;
  mOrientation[2] = -vDirection.z;
  mOrientation[3] = vUp.x;
  mOrientation[4] = vUp.y;
  mOrientation[5] = vUp.z;
  alListenerfv(AL_ORIENTATION, mOrientation);
}

 QColor SparseOccupancyGridArrayDisplay::axisColor(const Ogre::Quaternion& q,
                                                   const Ogre::Vector3& p)
 {
   Ogre::Vector3 zaxis = q.zAxis();
   Ogre::Vector3 reference = p.normalisedCopy();
   double dot = zaxis.dotProduct(reference);
   if (dot < -1) {
     dot = -1.0;
   }
   else if (dot > 1) {
     dot = 1.0;
   }
   double scale = (dot + 1) / 2.0;
   return gridColor(scale);
 }

void srs_ui_but::CButProjection::cameraInfoCB(const sensor_msgs::CameraInfo::ConstPtr &cam_info)
{
	//   std::cerr << "Camera callback. Frame id: " << cam_info->header.frame_id << std::endl;

	boost::mutex::scoped_lock( m_cameraInfoLock );

    // Get camera info
    ROS_DEBUG("OctMapPlugin: Set camera info: %d x %d\n", cam_info->height, cam_info->width);
    if( !m_camera_model.fromCameraInfo(*cam_info) )
        return;

    m_camera_size = m_camera_model.fullResolution();

    Ogre::Vector3 position;
    Ogre::Quaternion orientation;
    vis_manager_->getFrameManager()->getTransform(cam_info->header, position, orientation);
  //  const cv::Mat_<double> pm( m_camera_model.projectionMatrix() );

    // convert vision (Z-forward) frame to ogre frame (Z-out)
    orientation = orientation * Ogre::Quaternion(Ogre::Degree(180), Ogre::Vector3::UNIT_X);

    // Get z-axis
    Ogre::Vector3 normal( orientation.zAxis() );
    normal.normalise();

    // Compute camera plane equation
    Ogre::Vector4 equation( normal.x, normal.y, normal.z, -normal.dotProduct( position) );


    float width = cam_info->width;
    float height = cam_info->height;

    // Possibly malformed camera info...
    if( width == 0.0 || height == 0.0 )
        return;

    double fx = cam_info->P[0];
    double fy = cam_info->P[5];
/*
     Ogre::Radian fovy( 2.0*atan(height / (2.0 * fy)) );

    if( fovy != fovy)
    	return; // NAN

    double aspect_ratio = width / height;

    if( aspect_ratio != aspect_ratio )
    	return; //NaN
*/
    // Add the camera's translation relative to the left camera (from P[3]);
    // Tx = -1*(P[3] / P[0])
    double tx = -1.0 * (cam_info->P[3] / fx);
    Ogre::Vector3 right = orientation * Ogre::Vector3::UNIT_X;
    position = position + (right * tx);

//    std::cerr << right * tx << std::endl;

    double ty = -1 * (cam_info->P[7] / fy);
    Ogre::Vector3 down = orientation * Ogre::Vector3::UNIT_Y;
    position = position + (down * ty);

    if( !rviz::validateFloats( position ))
    {
    	return;
    }


    if( m_projectionData != 0 )
    {
		m_projectionData->setProjectorPosition( position );
		m_projectionData->setProjectorOrientation( orientation );

		// f.setFOVy( fovX );
//		m_projectionData->setFOVy( fovy );
//		m_projectionData->setAspectRatio( aspect_ratio );

		m_projectionData->setCameraModel( *cam_info );
		m_projectionData->setProjectorEquation( equation );
		m_projectionData->updateMatrices();
    }
}

//-------------------------------------------------------------------------------------
void ControllableCharacter::transformBone(Ogre::String boneName, NuiManager::NuiJointIndex jointIdx)
{
    int state = 0;
    state = (int)controller->getJointStatus(jointIdx);

    if(state == 2)
    {
        Ogre::Bone* bone = skeleton->getBone(boneName);
        Ogre::Quaternion qI = bone->getInitialOrientation();
        Ogre::Quaternion newQ = jointCalc->getSkeletonJointOrientation(jointIdx);

        bone->resetOrientation();
        newQ = bone->convertWorldToLocalOrientation(newQ);
        bone->setOrientation(newQ * qI);

        Ogre::Quaternion resQ = bone->getOrientation();
        if(showBoneOrientationAxes) axisLines[jointIdx]->updateLines(resQ.xAxis(), resQ.yAxis(), resQ.zAxis());		// debug
    }
}