123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113 |
- // 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/check_op.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
|