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