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