1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
|
// Copyright 2020 The Chromium Authors
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.
#include "services/device/generic_sensor/platform_sensor_util.h"
#include <algorithm>
#include <cmath>
#include "base/notreached.h"
#include "services/device/public/cpp/generic_sensor/sensor_reading.h"
namespace device {
namespace {
// Check that each rounding multiple is positive number.
static_assert(kAccelerometerRoundingMultiple > 0.0,
"Rounding multiple must be positive.");
static_assert(kAlsRoundingMultiple > 0,
"Rounding multiple must be positive.");
static_assert(kGyroscopeRoundingMultiple > 0.0,
"Rounding multiple must be positive.");
static_assert(kOrientationEulerRoundingMultiple > 0.0,
"Rounding multiple must be positive.");
static_assert(kOrientationQuaternionRoundingMultiple > 0.0,
"Rounding multiple must be positive.");
static_assert(kMagnetometerRoundingMultiple > 0.0,
"Rounding multiple must be positive.");
// Check that threshold value is at least half of rounding multiple.
static_assert(kAlsSignificanceThreshold >= (kAlsRoundingMultiple / 2),
"Threshold must be at least half of rounding multiple.");
template <typename T>
T square(T x) {
return x * x;
}
} // namespace
double RoundToMultiple(double value, double multiple) {
double scaledValue = value / multiple;
if (value < 0.0) {
return -multiple * floor(-scaledValue + 0.5);
} else {
return multiple * floor(scaledValue + 0.5);
}
}
void RoundAccelerometerReading(SensorReadingXYZ* reading) {
reading->x = RoundToMultiple(reading->x, kAccelerometerRoundingMultiple);
reading->y = RoundToMultiple(reading->y, kAccelerometerRoundingMultiple);
reading->z = RoundToMultiple(reading->z, kAccelerometerRoundingMultiple);
}
void RoundGyroscopeReading(SensorReadingXYZ* reading) {
reading->x = RoundToMultiple(reading->x, kGyroscopeRoundingMultiple);
reading->y = RoundToMultiple(reading->y, kGyroscopeRoundingMultiple);
reading->z = RoundToMultiple(reading->z, kGyroscopeRoundingMultiple);
}
void RoundIlluminanceReading(SensorReadingSingle* reading) {
reading->value = RoundToMultiple(reading->value, kAlsRoundingMultiple);
}
void RoundOrientationQuaternionReading(SensorReadingQuat* reading) {
double original_angle_div_2 = std::acos(reading->w);
double rounded_angle_div_2 =
RoundToMultiple(original_angle_div_2 * 2.0,
kOrientationQuaternionRoundingMultiple) /
2.0;
if (rounded_angle_div_2 == 0.0) {
// If there's no rotation after rounding, return the identity quaternion.
reading->w = 1;
reading->x = reading->y = reading->z = 0;
return;
}
// After this, original_angle_div_2 will definitely not be too close to 0.
double sin_angle_div_2 = std::sin(original_angle_div_2);
double axis_x = reading->x / sin_angle_div_2;
double axis_y = reading->y / sin_angle_div_2;
double axis_z = reading->z / sin_angle_div_2;
// Convert from (x,y,z) vector to azimuth/elevation.
double xy_dist = std::sqrt(square(axis_x) + square(axis_y));
double azim = std::atan2(axis_x, axis_y);
double elev = std::atan2(axis_z, xy_dist);
azim = RoundToMultiple(azim, kOrientationQuaternionRoundingMultiple);
elev = RoundToMultiple(elev, kOrientationQuaternionRoundingMultiple);
// Convert back from azimuth/elevation to the (x,y,z) unit vector.
axis_x = std::sin(azim) * std::cos(elev);
axis_y = std::cos(azim) * std::cos(elev);
axis_z = std::sin(elev);
// Reconstruct Quaternion from (x,y,z,rotation).
sin_angle_div_2 = std::sin(rounded_angle_div_2);
reading->x = axis_x * sin_angle_div_2;
reading->y = axis_y * sin_angle_div_2;
reading->z = axis_z * sin_angle_div_2;
reading->w = std::cos(rounded_angle_div_2);
}
void RoundOrientationEulerReading(SensorReadingXYZ* reading) {
reading->x = RoundToMultiple(reading->x, kOrientationEulerRoundingMultiple);
reading->y = RoundToMultiple(reading->y, kOrientationEulerRoundingMultiple);
reading->z = RoundToMultiple(reading->z, kOrientationEulerRoundingMultiple);
}
void RoundMagnetometerReading(SensorReadingXYZ* reading) {
reading->x = RoundToMultiple(reading->x, kMagnetometerRoundingMultiple);
reading->y = RoundToMultiple(reading->y, kMagnetometerRoundingMultiple);
reading->z = RoundToMultiple(reading->z, kMagnetometerRoundingMultiple);
}
void RoundSensorReading(SensorReading* reading, mojom::SensorType sensor_type) {
switch (sensor_type) {
case mojom::SensorType::ACCELEROMETER:
case mojom::SensorType::GRAVITY:
case mojom::SensorType::LINEAR_ACCELERATION:
RoundAccelerometerReading(&reading->accel);
break;
case mojom::SensorType::GYROSCOPE:
RoundGyroscopeReading(&reading->gyro);
break;
case mojom::SensorType::ABSOLUTE_ORIENTATION_EULER_ANGLES:
case mojom::SensorType::RELATIVE_ORIENTATION_EULER_ANGLES:
RoundOrientationEulerReading(&reading->orientation_euler);
break;
case mojom::SensorType::ABSOLUTE_ORIENTATION_QUATERNION:
case mojom::SensorType::RELATIVE_ORIENTATION_QUATERNION:
RoundOrientationQuaternionReading(&reading->orientation_quat);
break;
case mojom::SensorType::AMBIENT_LIGHT:
RoundIlluminanceReading(&reading->als);
break;
case mojom::SensorType::MAGNETOMETER:
RoundMagnetometerReading(&reading->magn);
break;
}
}
} // namespace device
|
