void SampleTransform(const IKGoal& goal,RigidTransform& T)
{
Assert(goal.posConstraint==IKGoal::PosFixed);
switch(goal.rotConstraint) {
case IKGoal::RotFixed:
goal.GetFixedGoalTransform(T);
break;
case IKGoal::RotTwoAxis:
FatalError("Can't have 2 axis terms rotation!");
break;
case IKGoal::RotAxis:
{
Real theta = Rand()*TwoPi;
goal.GetEdgeGoalTransform(theta,T);
}
break;
case IKGoal::RotNone:
{
QuaternionRotation q;
RandRotation(q);
q.getMatrix(T.R);
T.t = goal.endPosition - T.R*goal.localPosition;
}
break;
}
}
void SampleTransform(const IKGoal& goal,RigidTransform& T)
{
assert(goal.posConstraint == IKGoal::PosFixed);
if(goal.rotConstraint == IKGoal::RotFixed) {
goal.GetFixedGoalTransform(T);
}
else if (goal.rotConstraint == IKGoal::RotAxis) {
goal.GetEdgeGoalTransform(Rand(0,TwoPi),T);
}
else {
QuaternionRotation q=RandRotation();
q.getMatrix(T.R);
T.t = goal.endPosition - T.R*goal.localPosition;
}
}
