Пример #1
0
double getRotationMagnitude(const Mat3 & R2) {
  const Mat3 R1 = Mat3::Identity();
  double cos_theta = (R1.array() * R2.array()).sum() / 3.0;
  cos_theta = clamp(cos_theta, -1.0, 1.0);
  return std::acos(cos_theta);
}