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