//***************************************************************************************
// function : SumTargets
// description: multiply all the weights by the set of new orientations and positions
// and sum the result to give the new orientation and position.
//
// aweights : array of weights
// qrot : resulting summed rotation quaternion
// vpos : resulting summed position vector3
// atargetori : array of target orientations
// atargetpos : array of target positions
//***************************************************************************************
void CAxisInterpOp::SumTargets( XSI::MATH::CQuaternion& qrot, XSI::MATH::CVector3& vpos,
unsigned long size, double* aweights, double* atargetori, double* atargetpos )
{
double w;
XSI::MATH::CRotation r1;
XSI::MATH::CQuaternion q1;
XSI::MATH::CQuaternion q2;
XSI::MATH::CVector3 v1;
qrot.Set(0,0,0,0);
for ( unsigned long i=0; i < size; i++ )
{
w = aweights[i];
if ( w != 0 ) {
q1.SetFromXYZAnglesValues( d2r(atargetori[(i*3)+0]), d2r(atargetori[(i*3)+1]), d2r(atargetori[(i*3)+2]) );
// w.qi
q2.Set( q1.GetW() * w, q1.GetX() * w, q1.GetY() * w, q1.GetZ() * w );
qrot.AddInPlace( q2 );
// w.vi
v1.Set( atargetpos[(i*3)+0], atargetpos[(i*3)+1], atargetpos[(i*3)+2] );
v1.ScaleInPlace( w );
vpos.AddInPlace( v1 );
}
}
qrot.Normalize();
}
