| // Copyright 2017 The Chromium Authors. All rights reserved. |
| // 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/orientation_util.h" |
| |
| #include <cmath> |
| |
| #include "base/logging.h" |
| #include "base/numerics/math_constants.h" |
| #include "services/device/generic_sensor/generic_sensor_consts.h" |
| #include "ui/gfx/geometry/angle_conversions.h" |
| |
| namespace { |
| |
| // Returns orientation angles from a rotation matrix, such that the angles are |
| // according to spec http://dev.w3.org/geo/api/spec-source-orientation.html}. |
| // |
| // It is assumed the rotation matrix transforms a 3D column vector from device |
| // coordinate system to the world's coordinate system. |
| // |
| // In particular we compute the decomposition of a given rotation matrix r such |
| // that |
| // r = rz(alpha) * rx(beta) * ry(gamma) |
| // where rz, rx and ry are rotation matrices around z, x and y axes in the world |
| // coordinate reference frame respectively. The reference frame consists of |
| // three orthogonal axes x, y, z where x points East, y points north and z |
| // points upwards perpendicular to the ground plane. The computed angles alpha, |
| // beta and gamma are in degrees and clockwise-positive when viewed along the |
| // positive direction of the corresponding axis. Except for the special case |
| // when the beta angle is +-pi/2 these angles uniquely define the orientation of |
| // a mobile device in 3D space. The alpha-beta-gamma representation resembles |
| // the yaw-pitch-roll convention used in vehicle dynamics, however it does not |
| // exactly match it. One of the differences is that the 'pitch' angle beta is |
| // allowed to be within [-pi, pi). A mobile device with pitch angle greater than |
| // pi/2 could correspond to a user lying down and looking upward at the screen. |
| // |
| // r is a 9 element rotation matrix: |
| // r[ 0] r[ 1] r[ 2] |
| // r[ 3] r[ 4] r[ 5] |
| // r[ 6] r[ 7] r[ 8] |
| // |
| // alpha_in_radians: rotation around the z axis, in [0, 2*pi) |
| // beta_in_radians: rotation around the x axis, in [-pi, pi) |
| // gamma_in_radians: rotation around the y axis, in [-pi/2, pi/2) |
| void ComputeOrientationEulerAnglesInRadiansFromRotationMatrix( |
| const std::vector<double>& r, |
| double* alpha_in_radians, |
| double* beta_in_radians, |
| double* gamma_in_radians) { |
| DCHECK_EQ(9u, r.size()); |
| |
| // Since |r| contains double, directly compare it with 0 won't be accurate, |
| // so here |device::kEpsilon| is used to check if r[8] and r[6] is close to |
| // 0. And this needs to be done before checking if it is greater or less |
| // than 0 since a number close to 0 can be either a positive or negative |
| // number. |
| if (std::abs(r[8]) < device::kEpsilon) { // r[8] == 0 |
| if (std::abs(r[6]) < device::kEpsilon) { // r[6] == 0, cos(beta) == 0 |
| // gimbal lock discontinuity |
| *alpha_in_radians = std::atan2(r[3], r[0]); |
| *beta_in_radians = (r[7] > 0) ? base::kPiDouble / 2 |
| : -base::kPiDouble / 2; // beta = +-pi/2 |
| *gamma_in_radians = 0; // gamma = 0 |
| } else if (r[6] > 0) { // cos(gamma) == 0, cos(beta) > 0 |
| *alpha_in_radians = std::atan2(-r[1], r[4]); |
| *beta_in_radians = std::asin(r[7]); // beta [-pi/2, pi/2] |
| *gamma_in_radians = -base::kPiDouble / 2; // gamma = -pi/2 |
| } else { // cos(gamma) == 0, cos(beta) < 0 |
| *alpha_in_radians = std::atan2(r[1], -r[4]); |
| *beta_in_radians = -std::asin(r[7]); |
| *beta_in_radians += |
| (*beta_in_radians >= 0) |
| ? -base::kPiDouble |
| : base::kPiDouble; // beta [-pi,-pi/2) U (pi/2,pi) |
| *gamma_in_radians = -base::kPiDouble / 2; // gamma = -pi/2 |
| } |
| } else if (r[8] > 0) { // cos(beta) > 0 |
| *alpha_in_radians = std::atan2(-r[1], r[4]); |
| *beta_in_radians = std::asin(r[7]); // beta (-pi/2, pi/2) |
| *gamma_in_radians = std::atan2(-r[6], r[8]); // gamma (-pi/2, pi/2) |
| } else { // cos(beta) < 0 |
| *alpha_in_radians = std::atan2(r[1], -r[4]); |
| *beta_in_radians = -std::asin(r[7]); |
| *beta_in_radians += (*beta_in_radians >= 0) |
| ? -base::kPiDouble |
| : base::kPiDouble; // beta [-pi,-pi/2) U (pi/2,pi) |
| *gamma_in_radians = std::atan2(r[6], -r[8]); // gamma (-pi/2, pi/2) |
| } |
| |
| // alpha is in [-pi, pi], make sure it is in [0, 2*pi). |
| if (*alpha_in_radians < 0) |
| *alpha_in_radians += 2 * base::kPiDouble; // alpha [0, 2*pi) |
| } |
| |
| } // namespace |
| |
| namespace device { |
| |
| void ComputeOrientationEulerAnglesFromRotationMatrix( |
| const std::vector<double>& r, |
| double* alpha_in_degrees, |
| double* beta_in_degrees, |
| double* gamma_in_degrees) { |
| double alpha_in_radians, beta_in_radians, gamma_in_radians; |
| ComputeOrientationEulerAnglesInRadiansFromRotationMatrix( |
| r, &alpha_in_radians, &beta_in_radians, &gamma_in_radians); |
| *alpha_in_degrees = gfx::RadToDeg(alpha_in_radians); |
| *beta_in_degrees = gfx::RadToDeg(beta_in_radians); |
| *gamma_in_degrees = gfx::RadToDeg(gamma_in_radians); |
| } |
| |
| } // namespace device |