void IKGoal::Transform(const RigidTransform& T)
{
endPosition = T*endPosition;
direction = T.R*direction;
if(rotConstraint == IKGoal::RotFixed) {
MomentRotation m; m.set(endRotation);
Matrix3 R;
m.getMatrix(R);
R = T.R*R;
Assert(IsFinite(R));
m.setMatrix(R);
endRotation = m;
}
else if(rotConstraint == IKGoal::RotAxis) {
endRotation = T.R*endRotation;
}
}
void EvalIKGoalDeriv(const IKGoal& g,const RigidTransform& T,const Vector3& dw,const Vector3& dv,Vector& derr)
{
Vector3 pv=dv + cross(dw,g.endPosition-T.t);
if(g.posConstraint == IKGoal::PosFixed) {
pv.get(derr(0),derr(1),derr(2));
}
else if(g.posConstraint == IKGoal::PosLinear) {
Vector3 xb,yb;
GetCanonicalBasis(g.direction,xb,yb);
derr(0) = dot(pv,xb);
derr(1) = dot(pv,yb);
}
else if(g.posConstraint == IKGoal::PosPlanar) {
derr(0) = dot(pv,g.direction);
}
int m=IKGoal::NumDims(g.posConstraint);
if(g.rotConstraint==IKGoal::RotFixed) {
Matrix3 TgoalInv,Trel;
MomentRotation endinv;
endinv.setNegative(g.endRotation);
endinv.getMatrix(TgoalInv);
Trel.mul(T.R,TgoalInv);
Vector3 dr;
MomentDerivative(Trel,dw,dr);
dr.get(derr(0),derr(1),derr(2));
}
else if(g.rotConstraint==IKGoal::RotAxis) {
Vector3 x,y;
GetCanonicalBasis(g.endRotation,x,y);
Vector3 curAxis;
T.R.mul(g.localAxis,curAxis);
derr(m) = dot(cross(dw,curAxis),x);
derr(m+1) = dot(cross(dw,curAxis),y);
}
else if(g.rotConstraint==IKGoal::RotNone) { }
else {
FatalError("EvalIKGoalDeriv(): Invalid number of rotation terms");
}
}