orientation_util.cc 5.0 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113
  1. // Copyright 2017 The Chromium Authors. All rights reserved.
  2. // Use of this source code is governed by a BSD-style license that can be
  3. // found in the LICENSE file.
  4. #include "services/device/generic_sensor/orientation_util.h"
  5. #include <cmath>
  6. #include "base/check_op.h"
  7. #include "base/numerics/math_constants.h"
  8. #include "services/device/generic_sensor/generic_sensor_consts.h"
  9. #include "ui/gfx/geometry/angle_conversions.h"
  10. namespace {
  11. // Returns orientation angles from a rotation matrix, such that the angles are
  12. // according to spec http://dev.w3.org/geo/api/spec-source-orientation.html}.
  13. //
  14. // It is assumed the rotation matrix transforms a 3D column vector from device
  15. // coordinate system to the world's coordinate system.
  16. //
  17. // In particular we compute the decomposition of a given rotation matrix r such
  18. // that
  19. // r = rz(alpha) * rx(beta) * ry(gamma)
  20. // where rz, rx and ry are rotation matrices around z, x and y axes in the world
  21. // coordinate reference frame respectively. The reference frame consists of
  22. // three orthogonal axes x, y, z where x points East, y points north and z
  23. // points upwards perpendicular to the ground plane. The computed angles alpha,
  24. // beta and gamma are in degrees and clockwise-positive when viewed along the
  25. // positive direction of the corresponding axis. Except for the special case
  26. // when the beta angle is +-pi/2 these angles uniquely define the orientation of
  27. // a mobile device in 3D space. The alpha-beta-gamma representation resembles
  28. // the yaw-pitch-roll convention used in vehicle dynamics, however it does not
  29. // exactly match it. One of the differences is that the 'pitch' angle beta is
  30. // allowed to be within [-pi, pi). A mobile device with pitch angle greater than
  31. // pi/2 could correspond to a user lying down and looking upward at the screen.
  32. //
  33. // r is a 9 element rotation matrix:
  34. // r[ 0] r[ 1] r[ 2]
  35. // r[ 3] r[ 4] r[ 5]
  36. // r[ 6] r[ 7] r[ 8]
  37. //
  38. // alpha_in_radians: rotation around the z axis, in [0, 2*pi)
  39. // beta_in_radians: rotation around the x axis, in [-pi, pi)
  40. // gamma_in_radians: rotation around the y axis, in [-pi/2, pi/2)
  41. void ComputeOrientationEulerAnglesInRadiansFromRotationMatrix(
  42. const std::vector<double>& r,
  43. double* alpha_in_radians,
  44. double* beta_in_radians,
  45. double* gamma_in_radians) {
  46. DCHECK_EQ(9u, r.size());
  47. // Since |r| contains double, directly compare it with 0 won't be accurate,
  48. // so here |device::kEpsilon| is used to check if r[8] and r[6] is close to
  49. // 0. And this needs to be done before checking if it is greater or less
  50. // than 0 since a number close to 0 can be either a positive or negative
  51. // number.
  52. if (std::abs(r[8]) < device::kEpsilon) { // r[8] == 0
  53. if (std::abs(r[6]) < device::kEpsilon) { // r[6] == 0, cos(beta) == 0
  54. // gimbal lock discontinuity
  55. *alpha_in_radians = std::atan2(r[3], r[0]);
  56. *beta_in_radians = (r[7] > 0) ? base::kPiDouble / 2
  57. : -base::kPiDouble / 2; // beta = +-pi/2
  58. *gamma_in_radians = 0; // gamma = 0
  59. } else if (r[6] > 0) { // cos(gamma) == 0, cos(beta) > 0
  60. *alpha_in_radians = std::atan2(-r[1], r[4]);
  61. *beta_in_radians = std::asin(r[7]); // beta [-pi/2, pi/2]
  62. *gamma_in_radians = -base::kPiDouble / 2; // gamma = -pi/2
  63. } else { // cos(gamma) == 0, cos(beta) < 0
  64. *alpha_in_radians = std::atan2(r[1], -r[4]);
  65. *beta_in_radians = -std::asin(r[7]);
  66. *beta_in_radians +=
  67. (*beta_in_radians >= 0)
  68. ? -base::kPiDouble
  69. : base::kPiDouble; // beta [-pi,-pi/2) U (pi/2,pi)
  70. *gamma_in_radians = -base::kPiDouble / 2; // gamma = -pi/2
  71. }
  72. } else if (r[8] > 0) { // cos(beta) > 0
  73. *alpha_in_radians = std::atan2(-r[1], r[4]);
  74. *beta_in_radians = std::asin(r[7]); // beta (-pi/2, pi/2)
  75. *gamma_in_radians = std::atan2(-r[6], r[8]); // gamma (-pi/2, pi/2)
  76. } else { // cos(beta) < 0
  77. *alpha_in_radians = std::atan2(r[1], -r[4]);
  78. *beta_in_radians = -std::asin(r[7]);
  79. *beta_in_radians += (*beta_in_radians >= 0)
  80. ? -base::kPiDouble
  81. : base::kPiDouble; // beta [-pi,-pi/2) U (pi/2,pi)
  82. *gamma_in_radians = std::atan2(r[6], -r[8]); // gamma (-pi/2, pi/2)
  83. }
  84. // alpha is in [-pi, pi], make sure it is in [0, 2*pi).
  85. if (*alpha_in_radians < 0)
  86. *alpha_in_radians += 2 * base::kPiDouble; // alpha [0, 2*pi)
  87. }
  88. } // namespace
  89. namespace device {
  90. void ComputeOrientationEulerAnglesFromRotationMatrix(
  91. const std::vector<double>& r,
  92. double* alpha_in_degrees,
  93. double* beta_in_degrees,
  94. double* gamma_in_degrees) {
  95. double alpha_in_radians, beta_in_radians, gamma_in_radians;
  96. ComputeOrientationEulerAnglesInRadiansFromRotationMatrix(
  97. r, &alpha_in_radians, &beta_in_radians, &gamma_in_radians);
  98. *alpha_in_degrees = gfx::RadToDeg(alpha_in_radians);
  99. *beta_in_degrees = gfx::RadToDeg(beta_in_radians);
  100. *gamma_in_degrees = gfx::RadToDeg(gamma_in_radians);
  101. }
  102. } // namespace device