/// Multiply matrix with matrix
Matrix3 Matrix3::operator*(const Matrix3 &m) const {
Matrix3 prod;
for (int c=0;c<3;c++) {
for (int r=0;r<3;r++) {
prod.set(c,r,
get(c,0)*m.get(0,r) +
get(c,1)*m.get(1,r) +
get(c,2)*m.get(2,r));
}
}
return prod;
}
void IKObjective::getRotation(double out[9]) const
{
if(goal.rotConstraint == IKGoal::RotFixed) {
Matrix3 R;
goal.GetFixedGoalRotation(R);
R.get(out);
}
else {
PyException("getRotation called on non-fixed rotation");
}
}