示例#1
0
// Update =================================================================================
CStatus gStretchOp2_Update( CRef& in_ctxt )
{
	OperatorContext ctxt( in_ctxt );

	// User Datas ------------------------------------
	CValue::siPtrType pUserData = ctxt.GetUserData();
	OpUserData* pOpState = (OpUserData*)pUserData;

	if ( pOpState == NULL || pOpState->index >= 4)
	{
		// First time called
		pOpState = new OpUserData();
		ctxt.PutUserData( (CValue::siPtrType)pOpState );

		// Inputs ---------------------------------------
		KinematicState kRoot(ctxt.GetInputValue(0));
		KinematicState kCtrl(ctxt.GetInputValue(1));
		CTransformation tRoot(kRoot.GetTransform());
		CTransformation tCtrl(kCtrl.GetTransform());
		CVector3 vRoot = tRoot.GetTranslation();
		CVector3 vCtrl = tCtrl.GetTranslation();
		CMatrix4 mRoot = tRoot.GetMatrix4();
		CMatrix4 mRootNeg;
		mRootNeg.Invert(mRoot);

		double dRest0      = ctxt.GetParameterValue(L"rest0");
		double dRest1      = ctxt.GetParameterValue(L"rest1");
		double dPrefRot    = ctxt.GetParameterValue(L"prefrot");
		double dScale0     = ctxt.GetParameterValue(L"scale0");
		double dScale1     = ctxt.GetParameterValue(L"scale1");
		double dSoftness   = ctxt.GetParameterValue(L"soft");
		double dMaxStretch = ctxt.GetParameterValue(L"maxstretch");
		double dSlide      = ctxt.GetParameterValue(L"slide");
		double dReverse    = ctxt.GetParameterValue(L"reverse");

		// Distance with MaxStretch ---------------------
		double dRestLength = dRest0 * dScale0 + dRest1 * dScale1;
		CVector3 vDistance;
		vDistance.MulByMatrix4(vCtrl, mRootNeg);
		double dDistance = vDistance.GetLength();
		double dDistance2 = dDistance;
		if (dDistance > (dRestLength * dMaxStretch))
		{
			vDistance.NormalizeInPlace();
			vDistance.ScaleInPlace(dRestLength * dMaxStretch);
			dDistance = dRestLength * dMaxStretch;
		}

		// Adapt Softness value to chain length --------
		dSoftness *= dRestLength*.1;

		// Stretch and softness ------------------------
		/// We use the real distance from root to controler to calculate the softness
		/// This way we have softness working even when there is no stretch
		double dStretch = dDistance/dRestLength;
		if (dStretch < 1)
			dStretch = 1;
		double da = dRestLength - dSoftness;
		if (dSoftness > 0 && dDistance2 > da)
		{
			double newlen = dSoftness*(1.0 - exp(-(dDistance2 -da)/dSoftness)) + da;
			dStretch = dDistance / newlen;
		}

		double dLength0 = dRest0 * dStretch * dScale0;
		double dLength1 = dRest1 * dStretch * dScale1;

		// Reverse -------------------------------------
		double d = dDistance/(dLength0 + dLength1);

		double dScale;
		if (dReverse < 0.5)
			dScale = 1-(dReverse*2 * (1-d));
		else
			dScale = 1-((1-dReverse)*2 * (1-d));

		dLength0 *= dScale;
		dLength1 *= dScale;

		dPrefRot = -(dReverse-0.5) * 2 * dPrefRot;

		// Slide ---------------------------------------
		double dAdd;
		if (dSlide < .5)
			dAdd = (dLength0 * (dSlide*2)) - (dLength0);
		else
			dAdd = (dLength1 * (dSlide*2)) - (dLength1);

		dLength0 += dAdd;
		dLength1 -= dAdd;

		// Effector Position ----------------------------
		CTransformation t;
		vDistance.MulByMatrix4(vDistance, mRoot);
		t.SetTranslation(vDistance);

		pOpState->index = 0;
		pOpState->t = t;
		pOpState->dLength0 = dLength0;
		pOpState->dLength1 = dLength1;
		pOpState->dPrefRot = dPrefRot;
	}

	// Outputs -------------------------------------
	CRef outputPortRef=ctxt.GetOutputPort();
	OutputPort OutPort(outputPortRef);

	// Effector Transform
	if (OutPort.GetIndex() == 2)
	{
		KinematicState kOut = ctxt.GetOutputTarget();
		kOut.PutTransform(pOpState->t);
	}
	// Bone 0 Length
	else if (OutPort.GetIndex() == 3)
	{
		OutPort.PutValue(pOpState->dLength0);
	}
	// Bone 1 Length
	else if (OutPort.GetIndex() == 4)
	{
		OutPort.PutValue(pOpState->dLength1);
	}
	// Bone 1 PrefRot
	else if (OutPort.GetIndex() == 5)
	{
		OutPort.PutValue(pOpState->dPrefRot);
	}

	pOpState->index += 1;

	return CStatus::OK;
}
示例#2
0
///////////////////////////////////////////////////////////////
// METHODS
///////////////////////////////////////////////////////////////
// GetIKTransform =============================================
CTransformation GetIKTransform(s_GetIKTransform values, CString outportName)
{
   // prepare all variables
   CTransformation result;
   CVector3 bonePos,rootPos,effPos,upvPos,rootEff, xAxis,yAxis,zAxis, rollAxis;

   rootPos = values.root.GetTranslation();
   effPos = values.eff.GetTranslation();
   upvPos = values.upv.GetTranslation();
   rootEff.Sub(effPos,rootPos);
   rollAxis.Normalize(rootEff);

   CMatrix4 rootMatrix = values.root.GetMatrix4();
   CMatrix4 rootMatrixNeg;
   rootMatrixNeg.Invert(rootMatrix);

   double rootEffDistance = rootEff.GetLength();

   // init the scaling
   double global_scale = values.root.GetScaling().GetX();
   result.SetScaling(values.root.GetScaling());

   // Distance with MaxStretch ---------------------
   double restLength = values.lengthA * values.scaleA + values.lengthB * values.scaleB;
   CVector3 distanceVector;
   distanceVector.MulByMatrix4(effPos, rootMatrixNeg);
   double distance = distanceVector.GetLength();
   double distance2 = distance;
   if (distance > (restLength * (values.maxstretch)))
   {
      distanceVector.NormalizeInPlace();
      distanceVector.ScaleInPlace(restLength * (values.maxstretch) );
      distance = restLength * (values.maxstretch);
   }

   // Adapt Softness value to chain length --------
   values.softness = values.softness * restLength *.1;

   // Stretch and softness ------------------------
   // We use the real distance from root to controler to calculate the softness
   // This way we have softness working even when there is no stretch
   double stretch = max(1, distance / restLength);
   double da = restLength - values.softness;
   if (values.softness > 0 && distance2 > da)
   {
      double newlen = values.softness*(1.0 - exp(-(distance2 -da)/values.softness)) + da;
      stretch = distance / newlen;
   }

   values.lengthA = values.lengthA * stretch * values.scaleA * global_scale;
   values.lengthB = values.lengthB * stretch * values.scaleB * global_scale;

   // Reverse -------------------------------------
   double d = distance / (values.lengthA + values.lengthB);

   double reverse_scale;
   if (values.reverse < 0.5)
      reverse_scale = 1-(values.reverse*2 * (1-d));
   else
      reverse_scale = 1-((1-values.reverse)*2 * (1-d));

   values.lengthA *= reverse_scale;
   values.lengthB *= reverse_scale;

   bool invert = values.reverse > 0.5;

   // Slide ---------------------------------------
   double slide_add;
   if (values.slide < .5)
      slide_add = (values.lengthA * (values.slide * 2)) - (values.lengthA);
   else
      slide_add = (values.lengthB * (values.slide * 2)) - (values.lengthB);

   values.lengthA += slide_add;
   values.lengthB -= slide_add;

   // calculate the angle inside the triangle!
   double angleA = 0;
   double angleB = 0;

   // check if the divider is not null otherwise the result is nan
   // and the output disapear from xsi, that breaks constraints
   if((rootEffDistance < values.lengthA + values.lengthB) && (rootEffDistance > fabs(values.lengthA - values.lengthB) + 1E-6))
   {
      // use the law of cosine for lengthA
      double a = values.lengthA;
      double b = rootEffDistance;
      double c = values.lengthB;

         angleA = acos(min(1, (a * a + b * b - c * c ) / ( 2 * a * b)));

      // use the law of cosine for lengthB
      a = values.lengthB;
      b = values.lengthA;
      c = rootEffDistance;
      angleB = acos(min(1, (a * a + b * b - c * c ) / ( 2 * a * b)));

      // invert the angles if need be
      if(invert)
      {
         angleA = -angleA;
         angleB = -angleB;
      }
   }

   // start with the X and Z axis
   xAxis = rootEff;
   yAxis.LinearlyInterpolate(rootPos,effPos,.5);
   yAxis.Sub(upvPos,yAxis);
   yAxis = rotateVectorAlongAxis(yAxis, rollAxis, values.roll);
   zAxis.Cross(xAxis,yAxis);
   xAxis.NormalizeInPlace();
   zAxis.NormalizeInPlace();

   // switch depending on our mode
   if(outportName == "OutBoneA")  // Bone A
   {
      // check if we need to rotate the bone
      if(angleA != 0.0)
      {
         CRotation rot;
         rot.SetFromAxisAngle(zAxis,angleA);
         xAxis.MulByMatrix3InPlace(rot.GetMatrix());
      }

      if (values.negate)
         xAxis.NegateInPlace();

      // cross the yAxis and normalize
      yAxis.Cross(zAxis,xAxis);
      yAxis.NormalizeInPlace();

      // output the rotation
      result.SetRotationFromXYZAxes(xAxis,yAxis,zAxis);

      // set the scaling + the position
      result.SetSclX(values.lengthA);
      result.SetTranslation(rootPos);
   }
   else if(outportName == "OutBoneB")  // Bone B
   {
      // check if we need to rotate the bone
      if(angleA != 0.0)
      {
         CRotation rot;
         rot.SetFromAxisAngle(zAxis,angleA);
         xAxis.MulByMatrix3InPlace(rot.GetMatrix());
      }

      // calculate the position of the elbow!
      bonePos.Scale(values.lengthA,xAxis);
      bonePos.AddInPlace(rootPos);

      // check if we need to rotate the bone
      if(angleB != 0.0)
      {
         CRotation rot;
         rot.SetFromAxisAngle(zAxis,angleB-XSI::MATH::PI);
         xAxis.MulByMatrix3InPlace(rot.GetMatrix());
      }

      if (values.negate)
         xAxis.NegateInPlace();

      // cross the yAxis and normalize
      yAxis.Cross(zAxis,xAxis);
      yAxis.NormalizeInPlace();

      // output the rotation
      result.SetRotationFromXYZAxes(xAxis,yAxis,zAxis);

      // set the scaling + the position
      result.SetSclX(values.lengthB);
      result.SetTranslation(bonePos);
   }
   else if(outportName == "OutCenter")  // center
   {
      // check if we need to rotate the bone
      bonePos.Scale(values.lengthA,xAxis);
      if(angleA != 0.0)
      {
         CRotation rot;
         rot.SetFromAxisAngle(zAxis,angleA);
         bonePos.MulByMatrix3InPlace(rot.GetMatrix());
      }
      bonePos.AddInPlace(rootPos);

      // cross the yAxis and normalize
      yAxis.Sub(upvPos,bonePos);
      zAxis.Cross(xAxis,yAxis);
      zAxis.NormalizeInPlace();

      if (values.negate)
         xAxis.NegateInPlace();

      yAxis.Cross(zAxis,xAxis);
      yAxis.NormalizeInPlace();

      // output the rotation
      result.SetRotationFromXYZAxes(xAxis,yAxis,zAxis);

      // set the scaling + the position
      result.SetSclX(stretch * values.root.GetSclX());

      result.SetTranslation(bonePos);
   }
   else if(outportName == "OutCenterN")  // center normalized
   {
      // check if we need to rotate the bone
      if(angleA != 0.0)
      {
         CRotation rot;
         rot.SetFromAxisAngle(zAxis,angleA);
         xAxis.MulByMatrix3InPlace(rot.GetMatrix());
      }

      // calculate the position of the elbow!
      bonePos.Scale(values.lengthA,xAxis);
      bonePos.AddInPlace(rootPos);

      // check if we need to rotate the bone
      if(angleB != 0.0)
      {
         if(invert)
            angleB += XSI::MATH::PI * 2;
         CRotation rot;
         rot.SetFromAxisAngle(zAxis,angleB*.5-XSI::MATH::PI*.5);
         xAxis.MulByMatrix3InPlace(rot.GetMatrix());
      }      // cross the yAxis and normalize
      // yAxis.Sub(upvPos,bonePos); // this was flipping the centerN when the elbow/upv was aligned to root/eff
      zAxis.Cross(xAxis,yAxis);
      zAxis.NormalizeInPlace();

      if (values.negate)
         xAxis.NegateInPlace();

      yAxis.Cross(zAxis,xAxis);
      yAxis.NormalizeInPlace();

      // output the rotation
      result.SetRotationFromXYZAxes(xAxis,yAxis,zAxis);

      // set the scaling + the position
      // result.SetSclX(stretch * values.root.GetSclX());

      result.SetTranslation(bonePos);
   }
   else if(outportName == "OutEff")  // effector
   {
      // check if we need to rotate the bone
      effPos = rootPos;
      if(angleA != 0.0)
      {
         CRotation rot;
         rot.SetFromAxisAngle(zAxis,angleA);
         xAxis.MulByMatrix3InPlace(rot.GetMatrix());
      }

      // calculate the position of the elbow!
      bonePos.Scale(values.lengthA,xAxis);
      effPos.AddInPlace(bonePos);

      // check if we need to rotate the bone
      if(angleB != 0.0)
      {
         CRotation rot;
         rot.SetFromAxisAngle(zAxis,angleB-XSI::MATH::PI);
         xAxis.MulByMatrix3InPlace(rot.GetMatrix());
      }

      // calculate the position of the effector!
      bonePos.Scale(values.lengthB,xAxis);
      effPos.AddInPlace(bonePos);

      // cross the yAxis and normalize
      yAxis.Cross(zAxis,xAxis);
      yAxis.NormalizeInPlace();

      // output the rotation
      result = values.eff;
      result.SetTranslation(effPos);
   }


   CRotation r = result.GetRotation();
   CVector3 eulerAngles = r.GetXYZAngles();

   double rx = eulerAngles.GetX();
   double ry = eulerAngles.GetY();
   double rz = eulerAngles.GetZ();

   return result;
}
示例#3
0
// Update =============================================================================
CStatus gStretchOp2Multi_Update( CRef& in_ctxt )
{

	OperatorContext ctxt( in_ctxt );

		// User Datas ------------------------------------
		CValue::siPtrType pUserData = ctxt.GetUserData();
		OpUserData* pOpState = (OpUserData*)pUserData;

		if ( pOpState == NULL || pOpState->index >= 2)
		{
			// First time called
			pOpState = new OpUserData();
			ctxt.PutUserData( (CValue::siPtrType)pOpState );

			// Inputs ---------------------------------------
			KinematicState kRoot(ctxt.GetInputValue(0));
			KinematicState kCtrl(ctxt.GetInputValue(1));
			CTransformation tRoot(kRoot.GetTransform());
			CTransformation tCtrl(kCtrl.GetTransform());
			CVector3 vRoot = tRoot.GetTranslation();
			CVector3 vCtrl = tCtrl.GetTranslation();
			CMatrix4 mRoot = tRoot.GetMatrix4();
			CMatrix4 mRootNeg;
			mRootNeg.Invert(mRoot);

			double dRestLength = ctxt.GetParameterValue(L"restlength");
			double dScale      = ctxt.GetParameterValue(L"scale");
			double dSoftness   = ctxt.GetParameterValue(L"soft");
			double dMaxStretch = ctxt.GetParameterValue(L"maxstretch");

			// Distance with MaxStretch ---------------------
			dRestLength = dRestLength * dScale - .00001;
			CVector3 vDistance;
			vDistance.MulByMatrix4(vCtrl, mRootNeg);
			double dDistance = vDistance.GetLength();
			double dDistance2 = dDistance;
			if (dDistance > (dRestLength * dMaxStretch))
			{
				vDistance.NormalizeInPlace();
				vDistance.ScaleInPlace(dRestLength * dMaxStretch);
				dDistance = dRestLength * dMaxStretch;
			}

			Application app;
			app.LogMessage(L"dist : "+CString(dDistance));
			app.LogMessage(L"dist2 : "+CString(dDistance2));

			// Adapt Softness value to chain length --------
			dSoftness = dSoftness * dRestLength *.1;

			// Stretch and softness ------------------------
			/// We use the real distance from root to controler to calculate the softness
			/// This way we have softness working even when there is no stretch
			double dStretch = dDistance/dRestLength;
			if (dStretch < 1)
				dStretch = 1;
			double da = dRestLength - dSoftness;
			if (dSoftness > 0 && dDistance2 > da)
			{
				double newlen = dSoftness*(1.0 - exp(-(dDistance2 -da)/dSoftness)) + da;
				dStretch = dDistance / newlen;
			}

			double dScaleX = dStretch * dScale;
			app.LogMessage(L"scalex : "+CString(dScaleX));

			// Effector Position ----------------------------
			CTransformation t;
			vDistance.MulByMatrix4(vDistance, mRoot);
			t.SetTranslation(vDistance);

			pOpState->index = 0;
			pOpState->t = t;
			pOpState->dLength0 = dScaleX;
		}

	// Outputs -------------------------------------
	CRef outputPortRef=ctxt.GetOutputPort();
	OutputPort OutPort(outputPortRef);

	// Effector Transform
	if (OutPort.GetIndex() == 2)
	{
		KinematicState kOut = ctxt.GetOutputTarget();
		kOut.PutTransform(pOpState->t);
	}
	// Bone 0 Length
	else if (OutPort.GetIndex() == 3)
	{
		OutPort.PutValue(pOpState->dLength0);
	}

	pOpState->index += 1;

	return CStatus::OK;
}