orientation_euler_angles_fusion_algorithm_using_quaternion.cc 3.7 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106
  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_euler_angles_fusion_algorithm_using_quaternion.h"
  5. #include <cmath>
  6. #include "base/check.h"
  7. #include "services/device/generic_sensor/orientation_util.h"
  8. #include "services/device/generic_sensor/platform_sensor_fusion.h"
  9. namespace device {
  10. namespace {
  11. // Helper function to convert a quaternion to a rotation matrix. x, y, z, w
  12. // are values of a quaternion representing the orientation of the device in
  13. // 3D space. Returns a 9 element rotation matrix:
  14. // r[ 0] r[ 1] r[ 2]
  15. // r[ 3] r[ 4] r[ 5]
  16. // r[ 6] r[ 7] r[ 8]
  17. std::vector<double> ComputeRotationMatrixFromQuaternion(double x,
  18. double y,
  19. double z,
  20. double w) {
  21. std::vector<double> r(9);
  22. double sq_x = 2 * x * x;
  23. double sq_y = 2 * y * y;
  24. double sq_z = 2 * z * z;
  25. double x_y = 2 * x * y;
  26. double z_w = 2 * z * w;
  27. double x_z = 2 * x * z;
  28. double y_w = 2 * y * w;
  29. double y_z = 2 * y * z;
  30. double x_w = 2 * x * w;
  31. r[0] = 1 - sq_y - sq_z;
  32. r[1] = x_y - z_w;
  33. r[2] = x_z + y_w;
  34. r[3] = x_y + z_w;
  35. r[4] = 1 - sq_x - sq_z;
  36. r[5] = y_z - x_w;
  37. r[6] = x_z - y_w;
  38. r[7] = y_z + x_w;
  39. r[8] = 1 - sq_x - sq_y;
  40. return r;
  41. }
  42. void ComputeEulerAnglesFromQuaternion(double x,
  43. double y,
  44. double z,
  45. double w,
  46. double* alpha_in_degrees,
  47. double* beta_in_degrees,
  48. double* gamma_in_degrees) {
  49. std::vector<double> rotation_matrix =
  50. ComputeRotationMatrixFromQuaternion(x, y, z, w);
  51. device::ComputeOrientationEulerAnglesFromRotationMatrix(
  52. rotation_matrix, alpha_in_degrees, beta_in_degrees, gamma_in_degrees);
  53. }
  54. constexpr mojom::SensorType GetEulerAngleFusedType(bool absolute) {
  55. return absolute ? mojom::SensorType::ABSOLUTE_ORIENTATION_EULER_ANGLES
  56. : mojom::SensorType::RELATIVE_ORIENTATION_EULER_ANGLES;
  57. }
  58. constexpr mojom::SensorType GetQuaternionSourceType(bool absolute) {
  59. return absolute ? mojom::SensorType::ABSOLUTE_ORIENTATION_QUATERNION
  60. : mojom::SensorType::RELATIVE_ORIENTATION_QUATERNION;
  61. }
  62. } // namespace
  63. OrientationEulerAnglesFusionAlgorithmUsingQuaternion::
  64. OrientationEulerAnglesFusionAlgorithmUsingQuaternion(bool absolute)
  65. : PlatformSensorFusionAlgorithm(GetEulerAngleFusedType(absolute),
  66. {GetQuaternionSourceType(absolute)}) {}
  67. OrientationEulerAnglesFusionAlgorithmUsingQuaternion::
  68. ~OrientationEulerAnglesFusionAlgorithmUsingQuaternion() = default;
  69. bool OrientationEulerAnglesFusionAlgorithmUsingQuaternion::GetFusedDataInternal(
  70. mojom::SensorType which_sensor_changed,
  71. SensorReading* fused_reading) {
  72. // Transform the *_ORIENTATION_QUATERNION values to
  73. // *_ORIENTATION_EULER_ANGLES.
  74. DCHECK(fusion_sensor_);
  75. SensorReading reading;
  76. if (!fusion_sensor_->GetSourceReading(which_sensor_changed, &reading))
  77. return false;
  78. ComputeEulerAnglesFromQuaternion(
  79. reading.orientation_quat.x, reading.orientation_quat.y,
  80. reading.orientation_quat.z, reading.orientation_quat.w,
  81. &fused_reading->orientation_euler.z.value() /* alpha_in_degrees */,
  82. &fused_reading->orientation_euler.x.value() /* beta_in_degrees */,
  83. &fused_reading->orientation_euler.y.value() /* gamma_in_degrees */);
  84. return true;
  85. }
  86. } // namespace device