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

void PlayerCameraOgre::onRightButtonPressed()
{
	if (!mRightButtonPressedLastFrame)
		mMousePosLastFrame = mMouse->getPosition();

	mp::Vector2i diff = mMouse->getPosition() - mMousePosLastFrame;
	const mp::Vector3f &playerPos = mPlayer->model()->getPosition();
	Ogre::Vector3 pivotPoint(playerPos.getX(), playerPos.getY() + mPivotHeight, playerPos.getZ());
	float yaw = (float)diff.getX() * CAMERA_SPEED;
	float pitch = (float)-diff.getY() * CAMERA_SPEED;

	Ogre::Quaternion yawQuat;
	yawQuat.FromAngleAxis(Ogre::Radian(yaw), Ogre::Vector3::UNIT_Y);
	Ogre::Matrix3 yawMat;
	yawQuat.ToRotationMatrix(yawMat);

	Ogre::Vector3 pivotToPos = Ogre::Vector3(mRealPosition.getX(), mRealPosition.getY(), mRealPosition.getZ()) - pivotPoint;
	Ogre::Matrix4 pos(1, 0, 0, pivotToPos.x,
		0, 1, 0, pivotToPos.y,
		0, 0, 1, pivotToPos.z,
		0, 0, 0, 1);

	Ogre::Vector3 xz(pivotToPos.x, 0, pivotToPos.z);
	Ogre::Vector3 norm(-xz.z, 0, xz.x);
	Ogre::Quaternion pitchQuat;
	pitchQuat.FromAngleAxis(Ogre::Radian(pitch), norm);
	Ogre::Matrix3 pitchMat;
	pitchQuat.ToRotationMatrix(pitchMat);

	Ogre::Matrix4 toPivot(1, 0, 0, pivotPoint.x,
		0, 1, 0, pivotPoint.y,
		0, 0, 1, pivotPoint.z,
		0, 0, 0, 1);

	Ogre::Matrix4 newPosMat = pos * pitchMat * yawMat * toPivot;
	newPosMat = newPosMat.inverse();
	Ogre::Vector3 newPos = newPosMat.getTrans();
	mRealPosition.set(-newPos.x, -newPos.y, -newPos.z);
	setPosition(mRealPosition);
	lookAt(pivotPoint.x, pivotPoint.y, pivotPoint.z);

	adjustDistance();

	mMousePosLastFrame = mMouse->getPosition();
	mRightButtonPressedLastFrame = true;
}

    void
    BoxCenterMovable::getBoundingBoxCenter()
    {
        if(object.lock()->hasProperty("bounding box") && object.lock()->hasProperty("position"))
        {
            Ogre::AxisAlignedBox aabb = VariantCast<Ogre::AxisAlignedBox>(object.lock()->getProperty("bounding box"));
            /*Ogre::Vector3 position = VariantCast<Ogre::Vector3>(object.lock()->getProperty("position"));
            Ogre::Matrix4 matTrans;
            matTrans.makeTrans( position );
            aabb.transformAffine(matTrans);*/

			Ogre::Matrix4 transform = Ogre::Matrix4::IDENTITY;

			Ogre::Matrix3 rot3x3;
			if (object.lock()->hasProperty("orientation"))
			{
				Ogre::Quaternion orientation = VariantCast<Ogre::Quaternion>(object.lock()->getProperty("orientation"));
				orientation.ToRotationMatrix(rot3x3);
			}
			else
			{
				rot3x3 = Ogre::Matrix3::IDENTITY;
			}

			Ogre::Matrix3 scale3x3;
			if (object.lock()->hasProperty("scale"))
			{
				Ogre::Vector3 scale = VariantCast<Ogre::Vector3>(object.lock()->getProperty("scale"));
				scale3x3 = Ogre::Matrix3::ZERO;
				scale3x3[0][0] = scale.x;
				scale3x3[1][1] = scale.y;
				scale3x3[2][2] = scale.z;
			}
			else
			{
				scale3x3 = Ogre::Matrix3::IDENTITY;
			}

			transform = rot3x3 * scale3x3;

			if (object.lock()->hasProperty("position"))
			{
				Ogre::Vector3 position = VariantCast<Ogre::Vector3>(object.lock()->getProperty("position"));
				transform.setTrans(position);
			}
			aabb.transformAffine(transform);

            mCenterPosWC = aabb.getCenter();
        }
    }

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

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 Billboard::addTrajectoryPredictionMarkers()
{
  visualization_msgs::InteractiveMarker predictionMarker;
  predictionMarker = object_;
  predictionMarker.controls.clear();

  Ogre::Matrix3 *rotation = new Ogre::Matrix3();
  Ogre::Quaternion orientation;
  orientation.x = direction_.x;
  orientation.y = direction_.y;
  orientation.z = direction_.z;
  orientation.w = direction_.w;
  orientation.normalise();
  orientation.ToRotationMatrix(*rotation);
  Ogre::Vector3 position;
  position.x = velocity_;
  position.y = 0;
  position.z = 0;
  position = rotation->operator *(position);

  predictionMarker.pose.position.x = pose_.position.x;
  predictionMarker.pose.position.y = pose_.position.y;
  predictionMarker.pose.position.z = pose_.position.z;

  visualization_msgs::InteractiveMarkerControl predictionControl;
  predictionControl.name = "prediction_control";
  predictionControl.always_visible = true;
  predictionControl.orientation_mode = InteractiveMarkerControl::VIEW_FACING;
  predictionControl.interaction_mode = InteractiveMarkerControl::NONE;

  for (int i = 1; i < PREDICTIONS_COUNT + 1; i++)
  {
    std::stringstream name;
    name << name_ << "_prediction_" << i;
    predictionMarker.name = name.str();
    std::stringstream desc;
    desc << i << "s";
    predictionMarker.description = desc.str();
    predictionMarker.pose.position.x += position.x;
    predictionMarker.pose.position.y += position.y;
    predictionMarker.pose.position.z += position.z;

    predictionControl.markers.clear();
    predictionMarker.controls.clear();

    visualization_msgs::Marker sphere;
    sphere.type = visualization_msgs::Marker::SPHERE;
    sphere.color.g = 1;
    sphere.color.b = 1;
    sphere.color.a = 1;
    sphere.scale.x = PREDICTION_SPHERE_SIZE;
    sphere.scale.y = PREDICTION_SPHERE_SIZE;
    sphere.scale.z = PREDICTION_SPHERE_SIZE;

    predictionControl.markers.push_back(sphere);
    predictionMarker.controls.push_back(predictionControl);

    server_->insert(predictionMarker);
  }
  server_->applyChanges();
}