platform_sensor_util.cc 5.1 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148
  1. // Copyright 2020 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/platform_sensor_util.h"
  5. #include <cmath>
  6. #include "base/notreached.h"
  7. #include "base/ranges/algorithm.h"
  8. #include "services/device/public/cpp/generic_sensor/sensor_reading.h"
  9. namespace device {
  10. namespace {
  11. // Check that each rounding multiple is positive number.
  12. static_assert(kAccelerometerRoundingMultiple > 0.0,
  13. "Rounding multiple must be positive.");
  14. static_assert(kAlsRoundingMultiple > 0,
  15. "Rounding multiple must be positive.");
  16. static_assert(kGyroscopeRoundingMultiple > 0.0,
  17. "Rounding multiple must be positive.");
  18. static_assert(kOrientationEulerRoundingMultiple > 0.0,
  19. "Rounding multiple must be positive.");
  20. static_assert(kOrientationQuaternionRoundingMultiple > 0.0,
  21. "Rounding multiple must be positive.");
  22. // Check that threshold value is at least half of rounding multiple.
  23. static_assert(kAlsSignificanceThreshold >= (kAlsRoundingMultiple / 2),
  24. "Threshold must be at least half of rounding multiple.");
  25. template <typename T>
  26. T square(T x) {
  27. return x * x;
  28. }
  29. } // namespace
  30. double RoundToMultiple(double value, double multiple) {
  31. double scaledValue = value / multiple;
  32. if (value < 0.0) {
  33. return -multiple * floor(-scaledValue + 0.5);
  34. } else {
  35. return multiple * floor(scaledValue + 0.5);
  36. }
  37. }
  38. void RoundAccelerometerReading(SensorReadingXYZ* reading) {
  39. reading->x = RoundToMultiple(reading->x, kAccelerometerRoundingMultiple);
  40. reading->y = RoundToMultiple(reading->y, kAccelerometerRoundingMultiple);
  41. reading->z = RoundToMultiple(reading->z, kAccelerometerRoundingMultiple);
  42. }
  43. void RoundGyroscopeReading(SensorReadingXYZ* reading) {
  44. reading->x = RoundToMultiple(reading->x, kGyroscopeRoundingMultiple);
  45. reading->y = RoundToMultiple(reading->y, kGyroscopeRoundingMultiple);
  46. reading->z = RoundToMultiple(reading->z, kGyroscopeRoundingMultiple);
  47. }
  48. void RoundIlluminanceReading(SensorReadingSingle* reading) {
  49. reading->value = RoundToMultiple(reading->value, kAlsRoundingMultiple);
  50. }
  51. void RoundOrientationQuaternionReading(SensorReadingQuat* reading) {
  52. double original_angle_div_2 = std::acos(reading->w);
  53. double rounded_angle_div_2 =
  54. RoundToMultiple(original_angle_div_2 * 2.0,
  55. kOrientationQuaternionRoundingMultiple) /
  56. 2.0;
  57. if (rounded_angle_div_2 == 0.0) {
  58. // If there's no rotation after rounding, return the identity quaternion.
  59. reading->w = 1;
  60. reading->x = reading->y = reading->z = 0;
  61. return;
  62. }
  63. // After this, original_angle_div_2 will definitely not be too close to 0.
  64. double sin_angle_div_2 = std::sin(original_angle_div_2);
  65. double axis_x = reading->x / sin_angle_div_2;
  66. double axis_y = reading->y / sin_angle_div_2;
  67. double axis_z = reading->z / sin_angle_div_2;
  68. // Convert from (x,y,z) vector to azimuth/elevation.
  69. double xy_dist = std::sqrt(square(axis_x) + square(axis_y));
  70. double azim = std::atan2(axis_x, axis_y);
  71. double elev = std::atan2(axis_z, xy_dist);
  72. azim = RoundToMultiple(azim, kOrientationQuaternionRoundingMultiple);
  73. elev = RoundToMultiple(elev, kOrientationQuaternionRoundingMultiple);
  74. // Convert back from azimuth/elevation to the (x,y,z) unit vector.
  75. axis_x = std::sin(azim) * std::cos(elev);
  76. axis_y = std::cos(azim) * std::cos(elev);
  77. axis_z = std::sin(elev);
  78. // Reconstruct Quaternion from (x,y,z,rotation).
  79. sin_angle_div_2 = std::sin(rounded_angle_div_2);
  80. reading->x = axis_x * sin_angle_div_2;
  81. reading->y = axis_y * sin_angle_div_2;
  82. reading->z = axis_z * sin_angle_div_2;
  83. reading->w = std::cos(rounded_angle_div_2);
  84. }
  85. void RoundOrientationEulerReading(SensorReadingXYZ* reading) {
  86. reading->x = RoundToMultiple(reading->x, kOrientationEulerRoundingMultiple);
  87. reading->y = RoundToMultiple(reading->y, kOrientationEulerRoundingMultiple);
  88. reading->z = RoundToMultiple(reading->z, kOrientationEulerRoundingMultiple);
  89. }
  90. void RoundSensorReading(SensorReading* reading, mojom::SensorType sensor_type) {
  91. switch (sensor_type) {
  92. case mojom::SensorType::ACCELEROMETER:
  93. case mojom::SensorType::GRAVITY:
  94. case mojom::SensorType::LINEAR_ACCELERATION:
  95. RoundAccelerometerReading(&reading->accel);
  96. break;
  97. case mojom::SensorType::GYROSCOPE:
  98. RoundGyroscopeReading(&reading->gyro);
  99. break;
  100. case mojom::SensorType::ABSOLUTE_ORIENTATION_EULER_ANGLES:
  101. case mojom::SensorType::RELATIVE_ORIENTATION_EULER_ANGLES:
  102. RoundOrientationEulerReading(&reading->orientation_euler);
  103. break;
  104. case mojom::SensorType::ABSOLUTE_ORIENTATION_QUATERNION:
  105. case mojom::SensorType::RELATIVE_ORIENTATION_QUATERNION:
  106. RoundOrientationQuaternionReading(&reading->orientation_quat);
  107. break;
  108. case mojom::SensorType::AMBIENT_LIGHT:
  109. RoundIlluminanceReading(&reading->als);
  110. break;
  111. case mojom::SensorType::MAGNETOMETER:
  112. case mojom::SensorType::PRESSURE:
  113. case mojom::SensorType::PROXIMITY:
  114. break;
  115. }
  116. }
  117. } // namespace device