void CAxisInterpOp::ComputeWeights
( 
	XSI::MATH::CQuaternion& qbone2, 
	unsigned long size,			// number of triggers
	double* aweights, 
	double* atriggers, 
	double* atolerances 
)
{
	XSI::MATH::CQuaternion q2;

#ifdef _DEBUG_COMPUTEWEIGHTS
	Application app;
	wchar_t wszBuf[256]; 

	swprintf( wszBuf, L"ComputeWeights qbone2->[%f V(%f,%f,%f)]",  qbone2.GetW(), qbone2.GetX(), qbone2.GetY(), qbone2.GetZ() );
	app.LogMessage( (const wchar_t*)wszBuf ); 

	double x, y, z;
	
	qbone2.GetXYZAnglesValues(x,y,z);

	swprintf( wszBuf, L"ComputeWeights qbone2->R(%f,%f,%f)]",  r2d(x),r2d(y),r2d(z) );
	app.LogMessage( (const wchar_t*)wszBuf ); 
#endif

	// dim tsec : tsec = timer
	// fntrace "ComputeWeights("&j&") bone2 angle: " & fnstr(q2rot(qbone2))

	double sumw(0), weight(0), dot_product(0), tolerance(0);
	
	for ( unsigned long j=0,i=0; i < size*3; i=i+3,j=j+1 )
	{
		tolerance = d2r(atolerances[j]);
#ifdef _DEBUG_COMPUTEWEIGHTS
		swprintf( wszBuf, L"trigger(%d) ori: (%f,%f,%f)", j, atriggers[i], atriggers[i+1], atriggers[i+2] );
		app.LogMessage( (const wchar_t*)wszBuf ); 

		swprintf( wszBuf, L"trigger(%d) tolerance: %f", j, tolerance );
		app.LogMessage( (const wchar_t*)wszBuf ); 
#endif

		q2.SetFromXYZAnglesValues( d2r(atriggers[i]), d2r(atriggers[i+1]), d2r(atriggers[i+2]) );

		// compute dot product of quaternion
		dot_product = 
			qbone2.GetX() * q2.GetX() + 
			qbone2.GetY() * q2.GetY() + 
			qbone2.GetZ() * q2.GetZ() + 
			qbone2.GetW() * q2.GetW();
#ifdef _DEBUG_COMPUTEWEIGHTS
		swprintf( wszBuf, L"trigger(%d) dot: %f", j, dot_product );
		app.LogMessage( (const wchar_t*)wszBuf ); 
#endif

		if ( tolerance == 0 ) {
			weight = 0;
		} else {
#ifdef _DEBUG_COMPUTEWEIGHTS
//		swprintf( wszBuf, L"ComputeWeights(%d) _acos(%f) = %f", j, fabs(dot_product), _acos(fabs(dot_product)) );
//		app.LogMessage( (const wchar_t*)wszBuf ); 
		swprintf( wszBuf, L"trigger(%d) acos(%f) = %f", j, fabs(dot_product), acos(fabs(dot_product)) );
		app.LogMessage( (const wchar_t*)wszBuf ); 
#endif
			weight = 1.0 - (2.0 * acos(fabs(dot_product)) / tolerance);
#ifdef _DEBUG_COMPUTEWEIGHTS
		swprintf( wszBuf, L"trigger(%d) weight: %f", j, weight );
		app.LogMessage( (const wchar_t*)wszBuf ); 
#endif
			if ( weight < 0 ) {
				weight = 0;
			}
		}
#ifdef _DEBUG_COMPUTEWEIGHTS
		swprintf( wszBuf, L"trigger(%d) computed weight: %f", j, weight );
		app.LogMessage( (const wchar_t*)wszBuf ); 
#endif

		aweights[j] = weight;
		// fntrace "ComputeWeights("&j&") raw: " & aW(j)

		sumw=sumw+aweights[j];
	}

	// make sure sum of weights totals 1
	if ( sumw != 0 ) 
	{
		for ( unsigned long i=0; i < size; i++ )
		{
			if ( aweights[i] != 0 ) {
				aweights[i] = aweights[i] / sumw;
				//fntrace "ComputeWeights("&i&") normalized: " & aW(i)
			}  
#ifdef _DEBUG_COMPUTEWEIGHTS
			swprintf( wszBuf, L"trigger(%d) normalized = %f", i, aweights[i] );
			app.LogMessage( (const wchar_t*)wszBuf ); 
#endif
		}
	}
	//fntrace "AxisInterpOp::ComputeWeights: took " & timer-tsec & " seconds"

}
Пример #2
0
bool bulletSimulation::SetFrame(int in_Frame)
{
   // see if we have to reset the world
   if(in_Frame <= 0)
   {
      ResetWorld();
      return true;
   }

   // now let's skip if we already have that frame or the interval>5 - we don't want bullet to compute the whole sequence if you move to the last frame.
   if(mLastFrame >= in_Frame || (in_Frame-mLastFrame)>5)
   {
      return false;
   }

   mChangingFrame = true;

   // if this is not the first frame, let's update all rbd!
   for(btRigidBodyIt it = mRigidBodies.begin();it!=mRigidBodies.end();it++)
   {
      // skip corrupt bodies
      if(it->second->body==NULL)
         continue;
      if(it->first.secondary != -1)
         continue;

      btCollisionShape * shape = (btBoxShape*)it->second->body->getCollisionShape();
      if(shape != NULL)
      {
         // refresh the kine ptr
         it->second->kine.Set(it->second->kine.GetAsText());

         XSI::MATH::CTransformation xf = XSI::KinematicState(it->second->kine).GetTransform();//(float)in_Frame);
         shape->setLocalScaling(btVector3(xf.GetSclX(),xf.GetSclY(),xf.GetSclZ()));

         // skip non valid ops
         if(!it->second->op.IsValid())
            continue;

         // update the mass an inertia
         it->second->mass = XSI::CustomOperator(it->second->op).GetParameterValue(L"mass");
         btVector3 inertia(0,0,0);
         if(it->second->mass > 0.0f)
            shape->calculateLocalInertia(it->second->mass,inertia);
         it->second->body->setMassProps(it->second->mass,inertia);

         // if we are passive rigid body, let's update the transform
         if(it->second->mass == 0.0f)
         {
            btDefaultMotionState * motionState = (btDefaultMotionState*)it->second->body->getMotionState();
            if(motionState != NULL)
            {
               btTransform transform;
               transform.setOrigin(btVector3(xf.GetPosX(),xf.GetPosY(),xf.GetPosZ()));

               XSI::MATH::CQuaternion q = xf.GetRotationQuaternion();
               transform.setRotation(btQuaternion(q.GetX(),q.GetY(),q.GetZ(),q.GetW()));
               it->second->body->proceedToTransform( transform );
            }
         }
      }
   }

   // step to the given frame
   if(in_Frame > 0)
   {
      while(in_Frame > mLastFrame)
      {
         float dt=(1.0f/mFps)/(float)mSubSteps;
         //Step every subframe
         for (int i=0;i<mSubSteps;i++)
             mDynamicsWorld->stepSimulation(dt,0,1.0f/mFps);

         mLastFrame++;
      }
   }
   mLastFrame = in_Frame;

   mChangingFrame = false;
   return true;
}