inline void DebugPrint( const XSI::CString& str, const XSI::MATH::CQuaternion& q ) { Application app; wchar_t wszBuf[256]; XSI::MATH::CQuaternion q = tGParent.GetRotationQuaternion(); double x, y, z; q.GetXYZAnglesValues(x,y,z); swprintf( wszBuf, L"%s R(%f,%f,%f)]", (const wchar_t*)str, r2d(x),r2d(y),r2d(z) ); app.LogMessage( (const wchar_t*)wszBuf ); }
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" }
XSI::CStatus CAxisInterpOp::Update ( UpdateContext& ctx, OutputPort& output ) { Operator op(ctx.GetOperator()); /////////////////////////////////////////////////////////////// // get operator parameters /////////////////////////////////////////////////////////////// XSI::CString triggers(op.GetParameterValue(L"Triggers")); // triggers changed if ( m_csTriggers != triggers ) { m_csTriggers = triggers; Init( ctx, 0 ); } double boneperc = op.GetParameterValue(L"BoneDist"); /////////////////////////////////////////////////////////////// // get objects connected to input & output ports /////////////////////////////////////////////////////////////// InputPort rootboneport(op.GetPort(L"globalkineport",L"RootBoneGroup",0)); InputPort parentboneport(op.GetPort(L"globalkineport",L"ParentBoneGroup",0)); InputPort parentbonelenport(op.GetPort(L"bonelengthport",L"ParentBoneGroup",0)); InputPort childboneport(op.GetPort(L"globalkineport",L"ChildBoneGroup",0)); KinematicState gkRoot(rootboneport.GetValue()); KinematicState gkParent(parentboneport.GetValue()); double parentbonelen(parentbonelenport.GetValue()); KinematicState gkChild(childboneport.GetValue()); KinematicState gkHelper(output.GetValue()); // GET TRANSFORMATIONS OF ROOT, PARENT & CHILD CTransformation tGRoot = gkRoot.GetTransform(); CTransformation tGBone1 = gkParent.GetTransform(); CTransformation tGBone2 = gkChild.GetTransform(); #ifdef _DEBUG_UPDATE { Application app; wchar_t wszBuf[256]; XSI::MATH::CQuaternion q = tGBone1.GetRotationQuaternion(); double x, y, z; q.GetXYZAnglesValues(x,y,z); swprintf( wszBuf, L"parent R(%f,%f,%f)]", r2d(x),r2d(y),r2d(z) ); app.LogMessage( (const wchar_t*)wszBuf ); } #endif #ifdef _DEBUG_UPDATE { Application app; wchar_t wszBuf[256]; XSI::MATH::CQuaternion q = tGBone2.GetRotationQuaternion(); double x, y, z; q.GetXYZAnglesValues(x,y,z); swprintf( wszBuf, L"child R(%f,%f,%f)]", r2d(x),r2d(y),r2d(z) ); app.LogMessage( (const wchar_t*)wszBuf ); } #endif /////////////////////////////////////////////////////////////// // perform update function /////////////////////////////////////////////////////////////// // GET LOCAL TRANSFORM OF CHILD RELATIVE TO PARENT XSI::MATH::CMatrix3 mBone1( tGBone1.GetRotationMatrix3() ); XSI::MATH::CMatrix3 mBone2( tGBone2.GetRotationMatrix3() ); #ifdef _DEBUG_UPDATE { Application app; wchar_t wszBuf[256]; double m0, m1, m2, m3, m4, m5, m6, m7, m8; mBone1.Get(m0, m1, m2, m3, m4, m5, m6, m7, m8); swprintf( wszBuf, L"mBone1->\n%.2f,%.2f,%.2f\n%.2f,%.2f,%.2f\n%.2f,%.2f,%.2f", m0, m1, m2, m3, m4, m5, m6, m7, m8 ); app.LogMessage( (const wchar_t*)wszBuf ); } #endif #ifdef _DEBUG_UPDATE { Application app; wchar_t wszBuf[256]; double m0, m1, m2, m3, m4, m5, m6, m7, m8; mBone2.Get(m0, m1, m2, m3, m4, m5, m6, m7, m8); swprintf( wszBuf, L"mBone2->\n%.2f,%.2f,%.2f\n%.2f,%.2f,%.2f\n%.2f,%.2f,%.2f", m0, m1, m2, m3, m4, m5, m6, m7, m8 ); app.LogMessage( (const wchar_t*)wszBuf ); } #endif mBone1.TransposeInPlace(); #ifdef _DEBUG_UPDATE { Application app; wchar_t wszBuf[256]; double m0, m1, m2, m3, m4, m5, m6, m7, m8; mBone1.Get(m0, m1, m2, m3, m4, m5, m6, m7, m8); swprintf( wszBuf, L"mBone1.TransposeInPlace->\n%.2f,%.2f,%.2f\n%.2f,%.2f,%.2f\n%.2f,%.2f,%.2f", m0, m1, m2, m3, m4, m5, m6, m7, m8 ); app.LogMessage( (const wchar_t*)wszBuf ); } #endif XSI::MATH::CMatrix3 tmpMat3; for(int nR=0; nR<3; nR++) for(int nC=0; nC<3; nC++) { tmpMat3.SetValue(nR,nC , mBone2.GetValue(nR,0) * mBone1.GetValue(0,nC) + mBone2.GetValue(nR,1) * mBone1.GetValue(1,nC) + mBone2.GetValue(nR,2) * mBone1.GetValue(2,nC) ); } mBone2 = tmpMat3; // bug #90494 // mBone2.MulInPlace( mBone1 ); #ifdef _DEBUG_UPDATE { Application app; wchar_t wszBuf[256]; double m0, m1, m2, m3, m4, m5, m6, m7, m8; mBone2.Get(m0, m1, m2, m3, m4, m5, m6, m7, m8); swprintf( wszBuf, L"mBone2.MulInPlace( mBone1 )->\n%.2f,%.2f,%.2f\n%.2f,%.2f,%.2f\n%.2f,%.2f,%.2f", m0, m1, m2, m3, m4, m5, m6, m7, m8 ); app.LogMessage( (const wchar_t*)wszBuf ); } #endif // GET ORIENTATION OF BONE2 RELATIVE TO BONE1 AS A QUATERNION XSI::MATH::CQuaternion qBone2 = mBone2.GetQuaternion(); // MATCH QUATERNIONS XSI::MATH::CVector3 vBasePos( op.GetParameterValue(L"BasePoseX"), op.GetParameterValue(L"BasePoseY"), op.GetParameterValue(L"BasePoseZ") ); #ifdef _DEBUG_UPDATE { Application app; wchar_t wszBuf[256]; double x, y, z; qBone2.GetXYZAnglesValues(x,y,z); swprintf( wszBuf, L"child2parent R(%f,%f,%f)]", r2d(x),r2d(y),r2d(z) ); app.LogMessage( (const wchar_t*)wszBuf ); } #endif ComputeBaseOffset(vBasePos, boneperc, parentbonelen ); ComputeWeights( qBone2, m_cTriggers, m_aWeights, m_aTriggerOri, m_aTriggerTol ); // SUM TARGET ORIENTATIONS & POSITIONS XSI::MATH::CQuaternion qNewOri; SumTargets( qNewOri, vBasePos, m_cTriggers, m_aWeights, m_aTargetOri, m_aTargetPos ); #ifdef _DEBUG_UPDATE Application app; wchar_t wszBuf[256]; double x, y, z; qNewOri.GetXYZAnglesValues(x,y,z); swprintf( wszBuf, L"qNewOri->R(%f,%f,%f)]", r2d(x),r2d(y),r2d(z) ); app.LogMessage( (const wchar_t*)wszBuf ); #endif // UPDATE TRANSFORMATION XSI::MATH::CTransformation tNewPose; tNewPose.SetRotationFromQuaternion( qNewOri ); tNewPose.SetTranslation( vBasePos ); tNewPose.MulInPlace( tGRoot ); /////////////////////////////////////////////////////////////// // update output port /////////////////////////////////////////////////////////////// gkHelper.PutTransform( tNewPose ); return CStatus::OK; }