//*************************************************************************************** // 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(); }