blob: f18a64c2226d4f411f4412f2c2e1442b97497a2e [file] [log] [blame]
// 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