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