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

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

//-------------------------------------------------------------------------------
bool PortalEditor::snapTpPortal(PortalEditor* dest,bool bAllowMove )
{
    //reposition & realign this portal (and its parent zone)
    //to connect with this portal.

    //Before snapping portals togther, we should check that the zone is
    //not already locked into position by another portal join.
    //However, even if this is the case, we can still join portals if
    //they are already in the correct position.

    //get current position data:
    Ogre::Quaternion qZone = mParentZone->getDerivedOrientation();
    Ogre::Quaternion qDest = dest->getDerivedOrientation();
    Ogre::Quaternion qPortal = this->getOrientation();
    Ogre::Vector3 vDest = dest->getDerivedPosition();
    Ogre::Vector3 vPortal = this->getDerivedPosition();


    const Ogre::Real DIST_EPSILON(0.01f);//fudge factor
    const Ogre::Radian ANG_EPSILON(0.01f);
    if(vPortal.distance(vDest)<DIST_EPSILON && qPortal.equals(qDest*Ogre::Quaternion(0,0,1,0),ANG_EPSILON))return true;
    if(!bAllowMove)return false;

    //orientation
    Ogre::Quaternion qNew = (qDest*Ogre::Quaternion(0,0,1,0))*qPortal.Inverse();
    mParentZone->setDerivedOrientation(qNew);

    //position
    Ogre::Vector3 vZone = mParentZone->getDerivedPosition();
    vPortal = this->getDerivedPosition();

    mParentZone->setDerivedPosition( (vDest - (vPortal-vZone)));

    return true;
}

void PhysicsRagDoll::setPositionOrientation(const Ogre::Vector3& pos, const Ogre::Quaternion &orient)
{
    Ogre::Vector3 oldPos = mNode->_getDerivedPosition();
    Ogre::Quaternion oldOri = mNode->_getDerivedOrientation();
    Ogre::Quaternion oldOriInv = oldOri.Inverse();

    for (RagBoneMapIterator it = mRagBonesMap.begin(); it != mRagBonesMap.end(); it++)
    {
        OgreNewt::Body* body = it->second->getBody();
        if (body)
        {
            Ogre::Vector3 boneOldPos;
            Ogre::Quaternion boneOldOri;
            body->getPositionOrientation(boneOldPos, boneOldOri);
            
            // get old position and orientation in local space
            Ogre::Vector3 boneOldLocalPos = oldOriInv*(boneOldPos - oldPos);
            Ogre::Quaternion boneOldLocalOri = oldOriInv*boneOldOri;

            // calculate and set new position in orientation
            body->setPositionOrientation(pos + orient*boneOldLocalPos, orient*boneOldLocalOri);
            body->unFreeze();
        }
    }
    mNode->setPosition(pos);
    mNode->setOrientation(orient);
}

void AppFrameListener::moveInGameCamera()
{
	//mCamNode->setPosition(Ogre::Vector3(100,200,100));
	////mCamNode->setOrientation(planeOri);
	//mCamNode->lookAt(Ogre::Vector3::ZERO, Ogre::Node::TS_WORLD, Ogre::Vector3::UNIT_Z);

	//return;


	Ogre::Vector3 planePos, planeDir, planeOmega;
	Ogre::Quaternion planeOri;

	mAirplane->getPositionOrientation(planePos,	planeOri);	
	mAirplane->getOmega(planeOmega);

	planeOmega = planeOri.Inverse() * planeOmega;	

	mCamNode->setPosition(planePos);
	mCamNode->setOrientation(planeOri);
	
	
	if( Ogre::Math::Abs(planeOmega.x) - Ogre::Math::Abs(mLastPlaneOmegaX) < 60*mTimeSinceLastFrame)
		mLastPlaneOmegaX = planeOmega.x;
	if( Ogre::Math::Abs(planeOmega.z) - Ogre::Math::Abs(mLastPlaneOmegaZ) < 60*mTimeSinceLastFrame)
		mLastPlaneOmegaZ = planeOmega.z;
	//if( planeVel.z > 0 && planeVel.z - mLastPlaneSpeed < 1)
	//	mLastPlaneSpeed = planeVel.z;

	
	
	if(mCamPosition != 0 || mCamLookBack)
	{
		mCamNode->pitch( Ogre::Radian(mLastPlaneOmegaX)*-0.1 );
		mCamNode->roll( Ogre::Radian(mLastPlaneOmegaZ)*-0.1);
	}
	if(mCamLookBack)
	{
		mCamNode->yaw(Ogre::Degree(180));
		//mCamNode->translate(Ogre::Vector3(0,5,-20), Ogre::Node::TS_LOCAL);
	}
	
	mCamNode->translate(mCamTranslation, Ogre::Node::TS_LOCAL);			


	//Launch a ray from airplane in it's direction, to get the first hit in order 
	//to find the correct position for the crosshair
	Ogre::Vector3 closestPos;
	Ogre::Matrix4 camMatrix;			
	bool enemyHit = false;
	enemyHit = mAirplane->getTargetHitPoint(closestPos);
	camMatrix = mCamera->getProjectionMatrix() * mCamera->getViewMatrix(true);			
	mHUDManager->setCrosshairVScroll((camMatrix*closestPos).y, enemyHit);	

	

}

	void CAnimatedEntity::setBoneWorldPosition(std::string boneName, Vector3 worldPos)
	{
		Ogre::Bone* bone = _entity->getSkeleton()->getBone(boneName);

		bone->setManuallyControlled(true);

		Ogre::Vector3 parentPos = _entityNode->_getDerivedPosition(); 
		Ogre::Vector3 parentQuatXbonePos = worldPos - parentPos;

		Ogre::Quaternion parentQuat = _entityNode->_getDerivedOrientation(); 
		Ogre::Vector3 bonePos = parentQuat.Inverse() * parentQuatXbonePos;
		Ogre::Vector3 inverseParentScale = 1.0 / _entityNode->_getDerivedScale();

		bone->_setDerivedPosition(bonePos * inverseParentScale);
	}

void ESKOgre::buildBone(unsigned short b, Ogre::Skeleton *ogre_skeleton, Ogre::Bone *parent_bone) {
	ESKBone *bone = bones[b];
	Ogre::Bone *mBone = ogre_skeleton->createBone(bone->name);
  if (parent_bone)
  {
    parent_bone->addChild(mBone);
  }

	Ogre::Matrix4 parent_matrix = Ogre::Matrix4::IDENTITY;

	if (bone->parent_index < bones.size()) {
		ESKBone *pbone = bones[bone->parent_index];

		parent_matrix = Ogre::Matrix4(pbone->transform_matrix[0], pbone->transform_matrix[4], pbone->transform_matrix[8], pbone->transform_matrix[12],
									  pbone->transform_matrix[1], pbone->transform_matrix[5], pbone->transform_matrix[9], pbone->transform_matrix[13],
									  pbone->transform_matrix[2], pbone->transform_matrix[6], pbone->transform_matrix[10], pbone->transform_matrix[14],
									  pbone->transform_matrix[3], pbone->transform_matrix[7], pbone->transform_matrix[11], pbone->transform_matrix[15]);
	}


	Ogre::Matrix4 transform_matrix(bone->transform_matrix[0], bone->transform_matrix[4], bone->transform_matrix[8], bone->transform_matrix[12],
								   bone->transform_matrix[1], bone->transform_matrix[5], bone->transform_matrix[9], bone->transform_matrix[13],
								   bone->transform_matrix[2], bone->transform_matrix[6], bone->transform_matrix[10], bone->transform_matrix[14],
								   bone->transform_matrix[3], bone->transform_matrix[7], bone->transform_matrix[11], bone->transform_matrix[15]);

	transform_matrix = transform_matrix * parent_matrix.inverse();

	Ogre::Vector3 mPos;
	Ogre::Vector3 mScale;
	Ogre::Quaternion mRot;
	transform_matrix.decomposition(mPos, mScale, mRot);

	LOG_DEBUG("Bone %d Setup for %s: %f %f %f %d\n", b, bone->name.c_str(), (float)mPos.x, (float)mPos.y, (float)mPos.z, bone->index_4);

	mBone->setPosition(mPos * -1);
	mBone->setScale(mScale);
	mBone->setOrientation(mRot.Inverse());
	mBone->setInitialState();

	mBone->setManuallyControlled(false);

	for (size_t i = 0; i < bones.size(); i++) {
		if (bones[i]->parent_index == b) {
			buildBone(i, ogre_skeleton, mBone);
		}
	}
}

void CameraDragRotate::_onDrag(const Point& pt)
{
    if (!mTerrainHitInfo.hitted)
        return;

    Ogre::Vector3 oldPosition = getCamera()->getDerivedPosition();
    Ogre::Quaternion oldOrientation = getCamera()->getDerivedOrientation();

    getCamera()->yaw(Ogre::Degree(-mDragDelta.x * getSceneManipulator()->getRotateFactor() / getViewport()->getActualWidth()));

    // 只有在不使用真实摄像机的情况下才允许上下旋转摄像机
    if ( !getSceneManipulator()->getUseRealCameraAngle() )
        getCamera()->pitch(Ogre::Degree(-mDragDelta.y * getSceneManipulator()->getRotateFactor() / getViewport()->getActualHeight()));

    Ogre::Quaternion newOrientation = getCamera()->getDerivedOrientation();
    Ogre::Vector3 newPosition = mTerrainHitInfo.hitPosition +
                                newOrientation * oldOrientation.Inverse() * (oldPosition - mTerrainHitInfo.hitPosition);

    getSceneManipulator()->setCameraPosition(newPosition);
}

void PhysicsRagDoll::_placementCallback(OgreNewt::Body* me, const Ogre::Quaternion& orient, const Ogre::Vector3& pos, int threadindex)
{
    Actor* parentActor = Ogre::any_cast<Actor*>(me->getUserData());
	PhysicsRagDoll* doll = parentActor->getPhysicalThing()->getRagDoll();
    PhysicsRagDoll::RagBoneMapIterator it = doll->mRagBonesMap.find(me);
    if (it == doll->mRagBonesMap.end())
    {
        LOG_ERROR(Logger::CORE, "Could not find ragdoll-bone in PhysicsRagDoll::_placementCallback!");
        return;
    }
	PhysicsRagDoll::RagBone* bone = it->second;

	// is this the root bone?
	if (!bone->getParent())
	{
		Ogre::Quaternion finalorient = (orient * bone->getOffsetOrient());
		Ogre::Vector3 finalpos = pos + (orient * bone->getOffsetPos());

		doll->mNode->setPosition(finalpos);
		doll->mNode->setOrientation(finalorient);
	}
	else
	{
		// standard bone, calculate the local orientation between it and it's parent.
		Ogre::Quaternion parentorient;
		Ogre::Vector3 parentpos;

        if (bone->getParent()->getBody())
    		bone->getParent()->getBody()->getPositionOrientation(parentpos, parentorient);
        else
            parentorient = bone->getParent()->getOgreBone()->getOrientation();


        Ogre::Quaternion localorient = orient;
        if (bone->getOgreBone()->getInheritOrientation())
            localorient = parentorient.Inverse() * localorient;

		bone->getOgreBone()->setOrientation(localorient);
	}
}

bool OgreNewtonFrameListener::frameStarted(const FrameEvent &evt)
{
	mKeyboard->capture();
	mMouse->capture();

	// ----------------------------------------
	// CAMERA CONTROLS
	// ----------------------------------------
	if ((mKeyboard->isKeyDown(OIS::KC_LSHIFT)) || (mKeyboard->isKeyDown(OIS::KC_RSHIFT)))
	{
		Vector3 trans, strafe, vec;
		Quaternion quat;

		quat = mCamera->getOrientation();

		vec = Vector3(0.0,0.0,-0.5);
		trans = quat * vec;

		vec = Vector3(0.5,0.0,0.0);
		strafe = quat * vec;

		mCamera->pitch( Degree(mMouse->getMouseState().Y.rel * -0.5) );
        mCamera->setFixedYawAxis(true);
		mCamera->yaw( Degree(mMouse->getMouseState().X.rel * -0.5) );

		if (mKeyboard->isKeyDown(OIS::KC_UP))
			mCamera->moveRelative(trans);

		if (mKeyboard->isKeyDown(OIS::KC_DOWN))
			mCamera->moveRelative(trans * -1.0);

		if (mKeyboard->isKeyDown(OIS::KC_LEFT))
			mCamera->moveRelative(strafe * -1.0);

		if (mKeyboard->isKeyDown(OIS::KC_RIGHT))
			mCamera->moveRelative(strafe);
	}


	// ----------------------------------------
	// DRAGGING!
	// ----------------------------------------

	if (!dragging)
	{

		//user pressing the left mouse button?
		if (mMouse->getMouseState().buttonDown(OIS::MB_Left))
		{
			// perform a raycast!
			// start at the camera, and go for 100 units in the Z direction.
			Ogre::Vector3 start, end;

			CEGUI::Point mouse = CEGUI::MouseCursor::getSingleton().getPosition();
			CEGUI::Renderer* rend = CEGUI::System::getSingleton().getRenderer();
	
			Ogre::Real mx,my;
			mx = mouse.d_x / rend->getWidth();
			my = mouse.d_y / rend->getHeight();
			Ogre::Ray camray = mCamera->getCameraToViewportRay(mx,my);

			start = camray.getOrigin();
			end = camray.getPoint( 100.0 );

			OgreNewt::BasicRaycast* ray = new OgreNewt::BasicRaycast( m_World, start, end, true );
			OgreNewt::BasicRaycast::BasicRaycastInfo info = ray->getFirstHit();

			if (info.mBody)
			{
				// a body was found.  first let's find the point we clicked, in local coordinates of the body.
				

				// while dragging, make sure the body can't fall asleep.
				info.mBody->unFreeze();
				//info.mBody->setAutoFreeze(0);

				Ogre::Vector3 bodpos;
				Ogre::Quaternion bodorient;

				info.mBody->getPositionOrientation( bodpos, bodorient );

				// info.mDistance is in the range [0,1].
				Ogre::Vector3 globalpt = camray.getPoint( 100.0 * info.mDistance );
				Ogre::Vector3 localpt = bodorient.Inverse() * (globalpt - bodpos);

				// now we need to save this point to apply the spring force, I'm using the userData of the bodies in this example.
                // (where is it used? probably not needed here...)
#ifndef OGRENEWT_NO_OGRE_ANY
                info.mBody->setUserData( Ogre::Any(this) );
#else
				info.mBody->setUserData( this );
#endif

				// now change the force callback from the standard one to the one that applies the spring (drag) force.
				// this is an example of binding a callback to a member of a specific class.  in previous versions of OgreNewt you were
				// required to use a static member function fr all callbacks... but now through the fantastic FastDelegate library, you can
				// now use callbacks that are members of specific classes.  to do this, use the syntax shown below.  the "this" is a pointer
				// to the specific class, and the 2nd parameter is a pointer to the function you want to use.  you can do this for all
				// body callbacks (ForceAndTorque, Transform, addBuoyancyPlane).
				info.mBody->setCustomForceAndTorqueCallback<OgreNewtonFrameListener>( &OgreNewtonFrameListener::dragCallback, this );
//.........这里部分代码省略.........

PhysicsRagDoll::RagBone* PhysicsRagDoll::_addAllBones(PhysicsRagDoll::RagBone* parent, TiXmlElement* boneElement, Actor* parentActor)
{
	// get the information for the bone represented by this element.
    Ogre::Vector3 dir = getAttributeValueAsVector3(boneElement, "dir");
	Ogre::Real length = getAttributeValueAsReal(boneElement, "length");
		
	Ogre::Vector3 size = getAttributeValueAsVector3(boneElement, "size");
	
	
	Ogre::String skeleton_bone = getAttributeValueAsStdString(boneElement, "skeleton_bone");
	Ogre::Bone* ogrebone = mSkeleton->getBone(skeleton_bone);


	Ogre::String shapestr = getAttributeValueAsStdString(boneElement, "shape");
	PhysicsRagDoll::RagBone::BoneShape shape = PhysicsRagDoll::RagBone::BS_BOX;

	if (shapestr=="box")
		shape = PhysicsRagDoll::RagBone::BS_BOX;
	
	if (shapestr=="capsule")
		shape = PhysicsRagDoll::RagBone::BS_CAPSULE;
	
	if (shapestr=="cylinder")
		shape = PhysicsRagDoll::RagBone::BS_CYLINDER;
	
	if (shapestr=="cone")
		shape = PhysicsRagDoll::RagBone::BS_CONE;
	
	if (shapestr=="ellipsoid")
		shape = PhysicsRagDoll::RagBone::BS_ELLIPSOID;

	if (shapestr=="hull")
		shape = PhysicsRagDoll::RagBone::BS_CONVEXHULL;

	Ogre::Real mass = getAttributeValueAsReal(boneElement, "mass");
	
	///////////////////////////////////////////////////////////////////////////////
	RagBone* me = _addBone(mWorld, parent, dir, shape, size, mass, ogrebone,  parentActor);
	///////////////////////////////////////////////////////////////////////////////

	// position the bone.
	Ogre::Quaternion boneorient = mNode->_getDerivedOrientation() * ogrebone->_getDerivedOrientation();
	Ogre::Vector3 bonepos;
	if (shape != PhysicsRagDoll::RagBone::BS_CONVEXHULL)
		bonepos = mNode->_getFullTransform() * ogrebone->_getDerivedPosition() + (boneorient * (dir * (length*0.5f)));
	else
		bonepos = mNode->_getFullTransform() * ogrebone->_getDerivedPosition();

    if (me->getBody())
    	me->getBody()->setPositionOrientation(bonepos, boneorient);

	// set offsets
	if (!parent)
	{
		Ogre::Quaternion offsetorient = (boneorient.Inverse()) * mNode->_getDerivedOrientation();
		Ogre::Vector3 offsetpos = boneorient.Inverse() * (mNode->_getDerivedPosition() - bonepos);
		me->setOffset(offsetorient, offsetpos);
	}


	// get the joint to connect this bone with it's parent.
	if (parent && me->getBody())
	{
		TiXmlElement* jointElement = getChildNamed(boneElement, "Joint");
		if (!jointElement)
		{
			// error!
            LOG_ERROR(Logger::CORE, " Joint not found while creating Ragdoll! ");
			return me;
		}

		Ogre::Vector3 jointpin = getAttributeValueAsVector3(jointElement, "pin");
		Ogre::String jointtypestr = getAttributeValueAsStdString(jointElement, "type");
		PhysicsRagDoll::JointType jointtype = PhysicsRagDoll::JT_BALLSOCKET;

		if (jointtypestr == "ballsocket")
			jointtype = PhysicsRagDoll::JT_BALLSOCKET;

		if (jointtypestr == "hinge")
			jointtype = PhysicsRagDoll::JT_HINGE;


		Ogre::Real limit1 = getAttributeValueAsReal(jointElement, "limit1");
		Ogre::Real limit2 = getAttributeValueAsReal(jointElement, "limit2");

		Ogre::Vector3 jpos = mNode->_getFullTransform() * ogrebone->_getDerivedPosition();
		Ogre::Vector3 jpin = (mNode->_getDerivedOrientation() * parent->getOgreBone()->_getDerivedOrientation()) * jointpin;

		_joinBones(jointtype, parent, me, jpos, jpin, limit1, limit2, mWorld);
	}
	
    LOG_MESSAGE(Logger::CORE, " added bone from '"+ogrebone->getName()+"'.");

	///////////////////////////////////////////////////////////////////////////////
	///////////////////////////////////////////////////////////////////////////////
	// add all children of this bone.
    for (TiXmlNode* cur = boneElement->FirstChild(); cur; cur = cur->NextSibling())
    {
        if (cur->Type() == TiXmlNode::ELEMENT && hasNodeName(cur, "Bone"))
        {
//.........这里部分代码省略.........