getOrientation function

List<double> getOrientation(
  1. List<double> R,
  2. List<double> values
)

Implementation

List<double> getOrientation(List<double> R, List<double> values) {
/*
         * 4x4 (length=16) case:
         *   /  R[ 0]   R[ 1]   R[ 2]   0  \
         *   |  R[ 4]   R[ 5]   R[ 6]   0  |
         *   |  R[ 8]   R[ 9]   R[10]   0  |
         *   \      0       0       0   1  /
         *
         * 3x3 (length=9) case:
         *   /  R[ 0]   R[ 1]   R[ 2]  \
         *   |  R[ 3]   R[ 4]   R[ 5]  |
         *   \  R[ 6]   R[ 7]   R[ 8]  /
         *
         */
  if (R.length == 9) {
    values[0] = atan2(R[1], R[4]);
    values[1] = asin(-R[7]);
    values[2] = atan2(-R[6], R[8]);
  } else {
    values[0] = atan2(R[1], R[5]);
    values[1] = asin(-R[9]);
    values[2] = atan2(-R[8], R[10]);
  }

  return values;
}