C++ Quaternion类代码示例

Quaternion Quaternion::operator/(const Quaternion & rhs)const
{
	return *this*rhs.i();
}

void DumpBSpline(NiBSplineCompTransformInterpolatorRef spline, string target)
{
   float start = spline->GetStartTime();
   float stop = spline->GetStopTime();

   int nframes = int((stop-start) / FramesIncrement);

   Ref<NiBSplineData > data = spline->GetSplineData();
   Ref<NiBSplineBasisData > basis = spline->GetBasisData();
   int nctrl = basis->GetNumControlPoints();

   cout.setf(ios::fixed, ios::floatfield);
   cout << setprecision(3); // << setiosflags(ios_base::showpos);

   if (dumpControl)
   {
      vector<short > control = data->GetShortControlPoints();
      cout << "Control data for " << target << endl
           << "Control Pt: " << control.size() << endl;
      int j=0;
      cout << "0" << '\t';
      for (int i=0, n=control.size(); i<n; ++i, ++j) {
         //cout << float(control[i]) / float (32767) << '\t';
         cout << i << '\t' << control[i] << endl;
      }
      cout << endl;

      return;
   }

   {
      vector<Vector3> control = spline->GetTranslateControlData();
      if (!control.empty()) {

         float mult = spline->GetTranslateMultiplier();
         float bias = spline->GetTranslateBias();

         cout << "Translation data for " << target << endl
            << "Control Pt: " << control.size() << endl
            << "Mult: " << mult << endl
            << "Bias: " << bias << endl
            << endl ;

         for (int i=0, n=control.size(); i<n; ++i){
            Vector3 xyz = control[i];
            cout << i << "\t[" << xyz.x << ",\t" << xyz.y << ",\t" << xyz.z << "]" << endl;
         }

         cout << endl;

         int npoints = control.size() * 2 + 1;
         vector< Key<Vector3> > keys = spline->SampleTranslateKeys(npoints, 3);
         for (int i=0, n=keys.size(); i<n; ++i){
            Vector3 xyz = keys[i].data;
            cout << i << "\t[" << xyz.x << ",\t" << xyz.y << ",\t" << xyz.z << "]" << endl;
         }
         cout << endl;
      }
   }

   {
      vector<Quaternion> control = spline->GetQuatRotateControlData();
      if (!control.empty()) {

         float mult = spline->GetRotationMultiplier();
         float bias = spline->GetRotationBias();

         cout << "Quaternion rotation data for " << target << endl
              << "Control Pt: " << control.size() << endl
              << "Mult: " << mult << endl
              << "Bias: " << bias << endl
              << endl ;

         for (int i=0, n=control.size(), j=1; i<n; ++i, ++j){
            Quaternion q = control[i];
            Float3 ypr = q.AsEulerYawPitchRoll();
               cout << i << "\t<" << q.w << ",\t" << q.x << ",\t" << q.y << ",\t" << q.z << ">"
                  << "\t[" << TODEG(ypr[0]) << ",\t" << TODEG(ypr[1]) << ",\t" << TODEG(ypr[2]) << "]" 
                  << endl;
            if (j==nctrl)
            {
               cout << endl;
               j = 0;
            }
         }

         cout << endl;

         int npoints = control.size() * 2 + 1;
         vector< Key<Quaternion> > keys = spline->SampleQuatRotateKeys(npoints, 3);
         for (int i=0, n=keys.size(); i<n; ++i){
            Quaternion q = keys[i].data;
            Float3 ypr = q.AsEulerYawPitchRoll();
            cout << i << "\t<" << q.w << ",\t" << q.x << ",\t" << q.y << ",\t" << q.z << ">"
               << "\t[" << TODEG(ypr[0]) << ",\t" << TODEG(ypr[1]) << ",\t" << TODEG(ypr[2]) << "]" 
               << endl;
         }
         cout << endl;
      }
   }
//.........这里部分代码省略.........

bool IK::Solve(Vector3 targetPos)
{
	//http://mrl.nyu.edu/~perlin/gdc/ik/ik.java.html

	//Get nodes
	Node* hip = effector_->GetParent()->GetParent();
	Node* knee = effector_->GetParent();

	// Get current world position for the 3 joints of the IK chain
	Vector3 hipPos = hip->GetWorldPosition(); // Thigh pos (hip joint)
	Vector3 kneePos = knee->GetWorldPosition(); // Calf pos (knee joint)
	Vector3 effectorPos = effector_->GetWorldPosition(); // Foot pos (ankle joint)

	// Pre IK Direction vectors
	Vector3 thighDir = kneePos - hipPos; // Thigh direction
	Vector3 calfDir = effectorPos - kneePos; // Calf direction	

	// Vectors lengths
	float A = Vector3(thighDir).Length();//length of hip to knee
	float B = Vector3(calfDir).Length();//length of knee to foot
	Vector3 P = hip->WorldToLocal(targetPos);//target at origin
	Vector3 D = hip->WorldToLocal(kneePos);//pre solve knee at origin
	//float limbLength = length1 + length2;
	//float lengthH = targetDir.Length();

	//PERLINS STUFF
	//bool test = Perlin(A,B,C,D);
	//GetSubsystem<DebugHud>()->SetAppStats("ik:", String(test) );
	//------
	Vector3 R;
	DefineM(P,D);
	R = Rot(Minv,P);
	//FIND D
	float c = R.Length();
    float d = Max(0.0f, Min(A, (c + (A*A-B*B)/c) / 2.0f));//FindD(A,B,R.Length());
    //FIND E
    float e = sqrtf(A*A-d*d);//FindE(A,d);
    Vector3 S = Vector3(d,e,0.0f);
    Vector3 Q = Rot(Mfwd,S);

    //Convert Q back to world space
    Vector3 worldQ = effector_->GetParent()->GetParent()->LocalToWorld(Q);

    //Get angles
    Vector3 tdn = thighDir.Normalized();
    Vector3 ntdn = Vector3(worldQ-hipPos).Normalized();
    Vector3 cdn = calfDir.Normalized();
    Vector3 ncdn = Vector3(targetPos-worldQ).Normalized();

    //Vector3 hipAxis = tdn.CrossProduct(ntdn); 
    //float hipAngle = tdn.Angle(ntdn);
    //Vector3 kneeAxis = cdn.CrossProduct(ncdn);
    //float kneeAngle = cdn.Angle(ncdn);

    //GetSubsystem<DebugHud>()->SetAppStats("ik:", String(hipAngle)+":"+String(kneeAngle) );

    //knee->SetWorldRotation(knee->GetWorldRotation() * Quaternion(kneeAngle,kneeAxis) );
	//hip->SetWorldRotation(hip->GetWorldRotation() * Quaternion(hipAngle,hipAxis) );
	//do top level first, then get new angle for lower level, since it might mangle it
	bool success = d > 0.0f && d < A;

	if(success)
	{
		Quaternion hipRot = Quaternion(tdn,ntdn);
		hip->Rotate(hipRot,TS_WORLD );
		knee->Rotate(Quaternion(cdn,ncdn)*hipRot.Inverse(),TS_WORLD );
	}

    if(drawDebug_)
    {
	    DebugRenderer* dbg = effector_->GetScene()->GetComponent<DebugRenderer>();
	    
	    /*dbg->AddLine(hipPos,hipPos+tdn,Color(0.0f,1.0f,0.0f),false);
    	dbg->AddLine(hipPos,hipPos+ntdn,Color(0.0f,0.0f,1.0f),false);
	    dbg->AddLine(kneePos,kneePos+cdn,Color(0.0f,1.0f,0.0f),false);
    	dbg->AddLine(kneePos,kneePos+ncdn,Color(0.0f,0.0f,1.0f),false);

    	dbg->AddSphere(Sphere(effectorPos,0.2f),Color(0.0f,1.0f,0.0f),false);
    	dbg->AddSphere(Sphere(targetPos,0.2f),Color(0.0f,0.0f,1.0f),false);*/
	    //at origin
	    /*dbg->AddSphere(Sphere(Vector3(),0.2f),Color(0.0f,0.0f,0.0f),false);//origin
	    dbg->AddSphere(Sphere(D,0.2f),Color(0.1f,0.0f,0.0f),false);//old elbow
	    dbg->AddSphere(Sphere(P,0.2f),Color(0.0f,1.0f,0.0f),false);//target
	    dbg->AddLine(Vector3(),P,Color(0.1f,0.1f,0.1f),false);

	    //show solve at origin
	    dbg->AddSphere(Sphere(Q,0.2f),Color(1.0f,0.0f,0.0f),false);
		dbg->AddLine(Vector3(),Q,Color(1.0f,0.0f,0.0f),false);
		dbg->AddLine(Q,P,Color(1.0f,0.0f,0.0f),false);*/

		//show solve at position
		dbg->AddSphere(Sphere(worldQ,0.2f),Color(1.0f,0.0f,0.0f),false);
		dbg->AddSphere(Sphere(targetPos,0.2f),Color(0.0f,1.0f,0.0f),false);
		dbg->AddLine(hipPos,worldQ,Color(1.0f,0.0f,0.0f),false);
		dbg->AddLine(worldQ,targetPos,Color(1.0f,0.0f,0.0f),false);
	}

    return success;

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

/*!
  Moves the element from its current pose to the new pose specified by \a tr.
  This motion is performed in several steps such that the translation
  between each step does not exceed \a translStepSize and the angle of
  rotation does not exceed \a rotStepSize (expressed in radians).  The
  intermediate poses are determined using linear interpolation for the
  translation and spherical linear interpolation for the rotation.  If
  a collision is encountered during the motion, the point of first contact
  is determined and the element is left in that position.  This function
  returns false if a collision was encountered (or contacts prevented the motion)
  or true if no collisions were encountered and the move was completed.
*/
bool
WorldElement::moveTo(transf &newTran, double translStepSize, double rotStepSize)
{
  bool moveFinished = false;
  transf origTran, nextTran, motion;
  Quaternion nextRotation;
  vec3 nextTranslation;
  double percentComplete, moveIncrement, translationLength;
  double angle;
  vec3 axis;
  bool success;

  CollisionReport contactReport;

  //DBGP("moveTo called");

  origTran = getTran();
  /*
    std::cout << "WorldElement origTran: " << origTran.translation().x() << " " <<
    origTran.translation().y() << " " <<
    origTran.translation().z() << " " <<
    origTran.rotation().w << " " <<
    origTran.rotation().x << " " <<
    origTran.rotation().y << " " <<
    origTran.rotation().z << " " << "\n";
  */
  //calculate the difference
  translationLength = (newTran.translation() - origTran.translation()).len();
  nextRotation = newTran.rotation() * origTran.rotation().inverse();
  nextRotation.ToAngleAxis(angle, axis);

  moveIncrement = 1.0;
  if (translationLength != 0.0) {
    if (translStepSize == ONE_STEP) {
      moveIncrement = 1.0;
    }
    else {
      moveIncrement = MIN(moveIncrement, translStepSize / translationLength);
    }
  }
  if (angle != 0.0) {
    if (rotStepSize == ONE_STEP) {
      moveIncrement = MIN(moveIncrement, 1.0);
    }
    else {
      moveIncrement = MIN(moveIncrement, rotStepSize / angle);
    }
  }

  // check contacts
  nextTranslation = (1.0 - moveIncrement) * origTran.translation() + moveIncrement * newTran.translation();
  nextRotation = Quaternion::Slerp(moveIncrement, origTran.rotation(), newTran.rotation());
  nextTran = transf(nextRotation, nextTranslation);
  motion = nextTran * getTran().inverse();

  if (contactsPreventMotion(motion)) {
    DBGP("contacts prevent motion")
    return false;
  }

  percentComplete = 0.0;
  while (!moveFinished) {
    percentComplete += moveIncrement;
    if (percentComplete >= 1.0) {
      percentComplete = 1.0;
      moveFinished = true;
    }

    nextTranslation = (1.0 - percentComplete) * origTran.translation() + percentComplete * newTran.translation();
    nextRotation = Quaternion::Slerp(percentComplete, origTran.rotation(), newTran.rotation());
    nextTran = transf(nextRotation, nextTranslation);
    /*
      std::cout << "moveTo NextTran: " << nextTran.translation().x() << " " <<
      nextTran.translation().y() << " " <<
      nextTran.translation().z() << " " <<
      nextTran.rotation().w << " " <<
      nextTran.rotation().x << " " <<
      nextTran.rotation().y << " " <<
      nextTran.rotation().z << " " << "\n";
    */
    success = jumpTo(nextTran, &contactReport);
    if (!success || contactReport.size() != 0) {
      moveFinished = true;
    }
  }

  if (!success) {
    DBGA("JumpTo error, stopping execution. Object " << myName.latin1() << " in thread "
//.........这里部分代码省略.........

	void DefaultCameraManager::setOrientation(Quaternion ori)
	{
		mCameraPosNode->setOrientation(ori.toOgre());
	}

	void DefaultCameraManager::setPosition(Vector3 pos,Quaternion dir,Vector3 offset)
	{
		mCameraPosNode->setPosition(pos+dir.toOgre()*offset);
	}