orientation_quaternion_fusion_algorithm_using_euler_angles.cc 3.6 KB

12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394
  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_quaternion_fusion_algorithm_using_euler_angles.h"
  5. #include <cmath>
  6. #include "base/check.h"
  7. #include "services/device/generic_sensor/generic_sensor_consts.h"
  8. #include "services/device/generic_sensor/platform_sensor_fusion.h"
  9. #include "ui/gfx/geometry/angle_conversions.h"
  10. namespace device {
  11. namespace {
  12. void ComputeQuaternionFromEulerAngles(double alpha_in_degrees,
  13. double beta_in_degrees,
  14. double gamma_in_degrees,
  15. double* x,
  16. double* y,
  17. double* z,
  18. double* w) {
  19. if (std::isnan(alpha_in_degrees)) {
  20. // The RelativeOrientationEulerAnglesFusionAlgorithmUsingAccelerometer
  21. // algorithm cannot measure rotation around the z-axis because it only
  22. // measures the direction of Earth's gravitational field through the
  23. // accelerometer. It sets |alpha| to NaN to reflect that. There is no
  24. // analogue in the world of quaternions so we set |alpha| to 0 to choose
  25. // an arbitrary fixed orientation around the z-axis.
  26. alpha_in_degrees = 0.0;
  27. }
  28. double alpha_in_radians = gfx::DegToRad(alpha_in_degrees);
  29. double beta_in_radians = gfx::DegToRad(beta_in_degrees);
  30. double gamma_in_radians = gfx::DegToRad(gamma_in_degrees);
  31. double cx = std::cos(beta_in_radians / 2);
  32. double cy = std::cos(gamma_in_radians / 2);
  33. double cz = std::cos(alpha_in_radians / 2);
  34. double sx = std::sin(beta_in_radians / 2);
  35. double sy = std::sin(gamma_in_radians / 2);
  36. double sz = std::sin(alpha_in_radians / 2);
  37. *x = sx * cy * cz - cx * sy * sz;
  38. *y = cx * sy * cz + sx * cy * sz;
  39. *z = cx * cy * sz + sx * sy * cz;
  40. *w = cx * cy * cz - sx * sy * sz;
  41. }
  42. constexpr mojom::SensorType GetQuaternionFusedType(bool absolute) {
  43. return absolute ? mojom::SensorType::ABSOLUTE_ORIENTATION_QUATERNION
  44. : mojom::SensorType::RELATIVE_ORIENTATION_QUATERNION;
  45. }
  46. constexpr mojom::SensorType GetEulerAngleSourceType(bool absolute) {
  47. return absolute ? mojom::SensorType::ABSOLUTE_ORIENTATION_EULER_ANGLES
  48. : mojom::SensorType::RELATIVE_ORIENTATION_EULER_ANGLES;
  49. }
  50. } // namespace
  51. OrientationQuaternionFusionAlgorithmUsingEulerAngles::
  52. OrientationQuaternionFusionAlgorithmUsingEulerAngles(bool absolute)
  53. : PlatformSensorFusionAlgorithm(GetQuaternionFusedType(absolute),
  54. {GetEulerAngleSourceType(absolute)}) {}
  55. OrientationQuaternionFusionAlgorithmUsingEulerAngles::
  56. ~OrientationQuaternionFusionAlgorithmUsingEulerAngles() = default;
  57. bool OrientationQuaternionFusionAlgorithmUsingEulerAngles::GetFusedDataInternal(
  58. mojom::SensorType which_sensor_changed,
  59. SensorReading* fused_reading) {
  60. // Transform the *_ORIENTATION_EULER_ANGLES values to
  61. // *_ORIENTATION_QUATERNION.
  62. DCHECK(fusion_sensor_);
  63. SensorReading reading;
  64. if (!fusion_sensor_->GetSourceReading(which_sensor_changed, &reading))
  65. return false;
  66. ComputeQuaternionFromEulerAngles(
  67. reading.orientation_euler.z /* alpha_in_degrees */,
  68. reading.orientation_euler.x /* beta_in_degrees */,
  69. reading.orientation_euler.y /* gamma_in_degrees */,
  70. &fused_reading->orientation_quat.x.value(),
  71. &fused_reading->orientation_quat.y.value(),
  72. &fused_reading->orientation_quat.z.value(),
  73. &fused_reading->orientation_quat.w.value());
  74. return true;
  75. }
  76. } // namespace device