10ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Ceres Solver - A fast non-linear least squares minimizer 20ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. 30ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// http://code.google.com/p/ceres-solver/ 40ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 50ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Redistribution and use in source and binary forms, with or without 60ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// modification, are permitted provided that the following conditions are met: 70ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 80ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Redistributions of source code must retain the above copyright notice, 90ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// this list of conditions and the following disclaimer. 100ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Redistributions in binary form must reproduce the above copyright notice, 110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// this list of conditions and the following disclaimer in the documentation 120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// and/or other materials provided with the distribution. 130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// * Neither the name of Google Inc. nor the names of its contributors may be 140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// used to endorse or promote products derived from this software without 150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// specific prior written permission. 160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// POSSIBILITY OF SUCH DAMAGE. 280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Author: keir@google.com (Keir Mierle) 300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// sameeragarwal@google.com (Sameer Agarwal) 310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Templated functions for manipulating rotations. The templated 330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// functions are useful when implementing functors for automatic 340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// differentiation. 350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// In the following, the Quaternions are laid out as 4-vectors, thus: 370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// q[0] scalar part. 390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// q[1] coefficient of i. 400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// q[2] coefficient of j. 410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// q[3] coefficient of k. 420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// where: i*i = j*j = k*k = -1 and i*j = k, j*k = i, k*i = j. 440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#ifndef CERES_PUBLIC_ROTATION_H_ 460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#define CERES_PUBLIC_ROTATION_H_ 470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <algorithm> 490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include <cmath> 500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#include "glog/logging.h" 510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongnamespace ceres { 530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Trivial wrapper to index linear arrays as matrices, given a fixed 551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// column and row stride. When an array "T* array" is wrapped by a 561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 571d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// (const) MatrixAdapter<T, row_stride, col_stride> M" 581d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// the expression M(i, j) is equivalent to 601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// arrary[i * row_stride + j * col_stride] 621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// 631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Conversion functions to and from rotation matrices accept 641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// MatrixAdapters to permit using row-major and column-major layouts, 651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// and rotation matrices embedded in larger matrices (such as a 3x4 661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// projection matrix). 671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingtemplate <typename T, int row_stride, int col_stride> 681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingstruct MatrixAdapter; 691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 701d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// Convenience functions to create a MatrixAdapter that treats the 711d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// array pointed to by "pointer" as a 3x3 (contiguous) column-major or 721d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling// row-major matrix. 731d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingtemplate <typename T> 741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingMatrixAdapter<T, 1, 3> ColumnMajorAdapter3x3(T* pointer); 751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingtemplate <typename T> 771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingMatrixAdapter<T, 3, 1> RowMajorAdapter3x3(T* pointer); 781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Convert a value in combined axis-angle representation to a quaternion. 800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// The value angle_axis is a triple whose norm is an angle in radians, 810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// and whose direction is aligned with the axis of rotation, 820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// and quaternion is a 4-tuple that will contain the resulting quaternion. 830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// The implementation may be used with auto-differentiation up to the first 840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// derivative, higher derivatives may have unexpected results near the origin. 850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate<typename T> 861d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingvoid AngleAxisToQuaternion(const T* angle_axis, T* quaternion); 870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Convert a quaternion to the equivalent combined axis-angle representation. 890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// The value quaternion must be a unit quaternion - it is not normalized first, 900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// and angle_axis will be filled with a value whose norm is the angle of 910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// rotation in radians, and whose direction is the axis of rotation. 920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// The implemention may be used with auto-differentiation up to the first 930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// derivative, higher derivatives may have unexpected results near the origin. 940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate<typename T> 951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingvoid QuaternionToAngleAxis(const T* quaternion, T* angle_axis); 960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Conversions between 3x3 rotation matrix (in column major order) and 980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// axis-angle rotation representations. Templated for use with 990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// autodifferentiation. 1000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate <typename T> 1011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingvoid RotationMatrixToAngleAxis(const T* R, T* angle_axis); 1021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingtemplate <typename T, int row_stride, int col_stride> 1041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingvoid RotationMatrixToAngleAxis( 1051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const MatrixAdapter<const T, row_stride, col_stride>& R, 1061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling T* angle_axis); 1071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate <typename T> 1091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingvoid AngleAxisToRotationMatrix(const T* angle_axis, T* R); 1101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingtemplate <typename T, int row_stride, int col_stride> 1121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingvoid AngleAxisToRotationMatrix( 1131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T* angle_axis, 1141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const MatrixAdapter<T, row_stride, col_stride>& R); 1150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Conversions between 3x3 rotation matrix (in row major order) and 1170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Euler angle (in degrees) rotation representations. 1180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 1190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// The {pitch,roll,yaw} Euler angles are rotations around the {x,y,z} 1200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// axes, respectively. They are applied in that same order, so the 1210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// total rotation R is Rz * Ry * Rx. 1220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate <typename T> 1230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid EulerAnglesToRotationMatrix(const T* euler, int row_stride, T* R); 1240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingtemplate <typename T, int row_stride, int col_stride> 1261d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingvoid EulerAnglesToRotationMatrix( 1271d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T* euler, 1281d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const MatrixAdapter<T, row_stride, col_stride>& R); 1291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Convert a 4-vector to a 3x3 scaled rotation matrix. 1310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 1320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// The choice of rotation is such that the quaternion [1 0 0 0] goes to an 1330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// identity matrix and for small a, b, c the quaternion [1 a b c] goes to 1340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// the matrix 1350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 1360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// [ 0 -c b ] 1370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// I + 2 [ c 0 -a ] + higher order terms 1380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// [ -b a 0 ] 1390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 1400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// which corresponds to a Rodrigues approximation, the last matrix being 1410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// the cross-product matrix of [a b c]. Together with the property that 1420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// R(q1 * q2) = R(q1) * R(q2) this uniquely defines the mapping from q to R. 1430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 1440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// The rotation matrix is row-major. 1450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 1460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// No normalization of the quaternion is performed, i.e. 1470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// R = ||q||^2 * Q, where Q is an orthonormal matrix 1480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// such that det(Q) = 1 and Q*Q' = I 1490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate <typename T> inline 1500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid QuaternionToScaledRotation(const T q[4], T R[3 * 3]); 1510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1521d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingtemplate <typename T, int row_stride, int col_stride> inline 1531d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingvoid QuaternionToScaledRotation( 1541d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T q[4], 1551d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const MatrixAdapter<T, row_stride, col_stride>& R); 1561d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Same as above except that the rotation matrix is normalized by the 1580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Frobenius norm, so that R * R' = I (and det(R) = 1). 1590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate <typename T> inline 1600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid QuaternionToRotation(const T q[4], T R[3 * 3]); 1610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1621d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingtemplate <typename T, int row_stride, int col_stride> inline 1631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingvoid QuaternionToRotation( 1641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T q[4], 1651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const MatrixAdapter<T, row_stride, col_stride>& R); 1661d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 1670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Rotates a point pt by a quaternion q: 1680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 1690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// result = R(q) * pt 1700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// 1710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// Assumes the quaternion is unit norm. This assumption allows us to 1720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// write the transform as (something)*pt + pt, as is clear from the 1730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// formula below. If you pass in a quaternion with |q|^2 = 2 then you 1740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// WILL NOT get back 2 times the result you get for a unit quaternion. 1750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate <typename T> inline 1760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid UnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]); 1770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// With this function you do not need to assume that q has unit norm. 1790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// It does assume that the norm is non-zero. 1800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate <typename T> inline 1810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid QuaternionRotatePoint(const T q[4], const T pt[3], T result[3]); 1820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// zw = z * w, where * is the Quaternion product between 4 vectors. 1840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate<typename T> inline 1850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid QuaternionProduct(const T z[4], const T w[4], T zw[4]); 1860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// xy = x cross y; 1880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate<typename T> inline 1890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid CrossProduct(const T x[3], const T y[3], T x_cross_y[3]); 1900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate<typename T> inline 1920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongT DotProduct(const T x[3], const T y[3]); 1930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// y = R(angle_axis) * x; 1950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate<typename T> inline 1960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]); 1970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 1980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// --- IMPLEMENTATION 1990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingtemplate<typename T, int row_stride, int col_stride> 2011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingstruct MatrixAdapter { 2021d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling T* pointer_; 2031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling explicit MatrixAdapter(T* pointer) 2041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling : pointer_(pointer) 2051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling {} 2061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling T& operator()(int r, int c) const { 2081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return pointer_[r * row_stride + c * col_stride]; 2091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 2101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling}; 2111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingtemplate <typename T> 2131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingMatrixAdapter<T, 1, 3> ColumnMajorAdapter3x3(T* pointer) { 2141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return MatrixAdapter<T, 1, 3>(pointer); 2151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 2161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingtemplate <typename T> 2181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha HaeberlingMatrixAdapter<T, 3, 1> RowMajorAdapter3x3(T* pointer) { 2191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling return MatrixAdapter<T, 3, 1>(pointer); 2201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 2211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 2220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate<typename T> 2230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Konginline void AngleAxisToQuaternion(const T* angle_axis, T* quaternion) { 2240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T& a0 = angle_axis[0]; 2250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T& a1 = angle_axis[1]; 2260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T& a2 = angle_axis[2]; 2270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T theta_squared = a0 * a0 + a1 * a1 + a2 * a2; 2280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // For points not at the origin, the full conversion is numerically stable. 2300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (theta_squared > T(0.0)) { 2310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T theta = sqrt(theta_squared); 2320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T half_theta = theta * T(0.5); 2330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T k = sin(half_theta) / theta; 2340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong quaternion[0] = cos(half_theta); 2350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong quaternion[1] = a0 * k; 2360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong quaternion[2] = a1 * k; 2370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong quaternion[3] = a2 * k; 2380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } else { 2390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // At the origin, sqrt() will produce NaN in the derivative since 2400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // the argument is zero. By approximating with a Taylor series, 2410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // and truncating at one term, the value and first derivatives will be 2420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // computed correctly when Jets are used. 2430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T k(0.5); 2440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong quaternion[0] = T(1.0); 2450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong quaternion[1] = a0 * k; 2460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong quaternion[2] = a1 * k; 2470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong quaternion[3] = a2 * k; 2480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate<typename T> 2520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Konginline void QuaternionToAngleAxis(const T* quaternion, T* angle_axis) { 2530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T& q1 = quaternion[1]; 2540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T& q2 = quaternion[2]; 2550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T& q3 = quaternion[3]; 2560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T sin_squared_theta = q1 * q1 + q2 * q2 + q3 * q3; 2570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // For quaternions representing non-zero rotation, the conversion 2590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // is numerically stable. 2600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (sin_squared_theta > T(0.0)) { 2610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T sin_theta = sqrt(sin_squared_theta); 2620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T& cos_theta = quaternion[0]; 2630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // If cos_theta is negative, theta is greater than pi/2, which 2650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // means that angle for the angle_axis vector which is 2 * theta 2660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // would be greater than pi. 2670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 2680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // While this will result in the correct rotation, it does not 2690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // result in a normalized angle-axis vector. 2700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 2710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // In that case we observe that 2 * theta ~ 2 * theta - 2 * pi, 2720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // which is equivalent saying 2730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 2740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // theta - pi = atan(sin(theta - pi), cos(theta - pi)) 2750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // = atan(-sin(theta), -cos(theta)) 2760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 2770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T two_theta = 2780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong T(2.0) * ((cos_theta < 0.0) 2790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ? atan2(-sin_theta, -cos_theta) 2800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong : atan2(sin_theta, cos_theta)); 2810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T k = two_theta / sin_theta; 2820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong angle_axis[0] = q1 * k; 2830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong angle_axis[1] = q2 * k; 2840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong angle_axis[2] = q3 * k; 2850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } else { 2860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // For zero rotation, sqrt() will produce NaN in the derivative since 2870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // the argument is zero. By approximating with a Taylor series, 2880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // and truncating at one term, the value and first derivatives will be 2890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // computed correctly when Jets are used. 2900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T k(2.0); 2910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong angle_axis[0] = q1 * k; 2920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong angle_axis[1] = q2 * k; 2930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong angle_axis[2] = q3 * k; 2940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 2950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 2960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 2970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// The conversion of a rotation matrix to the angle-axis form is 2980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// numerically problematic when then rotation angle is close to zero 2990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// or to Pi. The following implementation detects when these two cases 3000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// occurs and deals with them by taking code paths that are guaranteed 3010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// to not perform division by a small number. 3020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate <typename T> 3031d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlinginline void RotationMatrixToAngleAxis(const T* R, T* angle_axis) { 3041d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling RotationMatrixToAngleAxis(ColumnMajorAdapter3x3(R), angle_axis); 3051d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 3061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingtemplate <typename T, int row_stride, int col_stride> 3081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingvoid RotationMatrixToAngleAxis( 3091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const MatrixAdapter<const T, row_stride, col_stride>& R, 3101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling T* angle_axis) { 3110ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // x = k * 2 * sin(theta), where k is the axis of rotation. 3121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling angle_axis[0] = R(2, 1) - R(1, 2); 3131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling angle_axis[1] = R(0, 2) - R(2, 0); 3141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling angle_axis[2] = R(1, 0) - R(0, 1); 3150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong static const T kOne = T(1.0); 3170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong static const T kTwo = T(2.0); 3180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Since the right hand side may give numbers just above 1.0 or 3200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // below -1.0 leading to atan misbehaving, we threshold. 3211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling T costheta = std::min(std::max((R(0, 0) + R(1, 1) + R(2, 2) - kOne) / kTwo, 3220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong T(-1.0)), 3230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong kOne); 3240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // sqrt is guaranteed to give non-negative results, so we only 3260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // threshold above. 3270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong T sintheta = std::min(sqrt(angle_axis[0] * angle_axis[0] + 3280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong angle_axis[1] * angle_axis[1] + 3290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong angle_axis[2] * angle_axis[2]) / kTwo, 3300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong kOne); 3310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Use the arctan2 to get the right sign on theta 3330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T theta = atan2(sintheta, costheta); 3340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Case 1: sin(theta) is large enough, so dividing by it is not a 3360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // problem. We do not use abs here, because while jets.h imports 3370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // std::abs into the namespace, here in this file, abs resolves to 3380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // the int version of the function, which returns zero always. 3390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 3400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // We use a threshold much larger then the machine epsilon, because 3410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // if sin(theta) is small, not only do we risk overflow but even if 3420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // that does not occur, just dividing by a small number will result 3430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // in numerical garbage. So we play it safe. 3440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong static const double kThreshold = 1e-12; 3450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if ((sintheta > kThreshold) || (sintheta < -kThreshold)) { 3460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T r = theta / (kTwo * sintheta); 3470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < 3; ++i) { 3480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong angle_axis[i] *= r; 3490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 3500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return; 3510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 3520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Case 2: theta ~ 0, means sin(theta) ~ theta to a good 3540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // approximation. 3550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (costheta > 0.0) { 3560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T kHalf = T(0.5); 3570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < 3; ++i) { 3580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong angle_axis[i] *= kHalf; 3590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 3600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return; 3610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 3620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Case 3: theta ~ pi, this is the hard case. Since theta is large, 3640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // and sin(theta) is small. Dividing by theta by sin(theta) will 3650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // either give an overflow or worse still numerically meaningless 3660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // results. Thus we use an alternate more complicated formula 3670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // here. 3680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Since cos(theta) is negative, division by (1-cos(theta)) cannot 3700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // overflow. 3710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T inv_one_minus_costheta = kOne / (kOne - costheta); 3720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // We now compute the absolute value of coordinates of the axis 3740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // vector using the diagonal entries of R. To resolve the sign of 3750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // these entries, we compare the sign of angle_axis[i]*sin(theta) 3760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // with the sign of sin(theta). If they are the same, then 3770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // angle_axis[i] should be positive, otherwise negative. 3780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong for (int i = 0; i < 3; ++i) { 3791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling angle_axis[i] = theta * sqrt((R(i, i) - costheta) * inv_one_minus_costheta); 3800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong if (((sintheta < 0.0) && (angle_axis[i] > 0.0)) || 3810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong ((sintheta > 0.0) && (angle_axis[i] < 0.0))) { 3820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong angle_axis[i] = -angle_axis[i]; 3830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 3840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 3850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 3860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 3870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate <typename T> 3881d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlinginline void AngleAxisToRotationMatrix(const T* angle_axis, T* R) { 3891d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling AngleAxisToRotationMatrix(angle_axis, ColumnMajorAdapter3x3(R)); 3901d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 3911d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 3921d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingtemplate <typename T, int row_stride, int col_stride> 3931d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingvoid AngleAxisToRotationMatrix( 3941d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T* angle_axis, 3951d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const MatrixAdapter<T, row_stride, col_stride>& R) { 3960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong static const T kOne = T(1.0); 3970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T theta2 = DotProduct(angle_axis, angle_axis); 39879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (theta2 > T(std::numeric_limits<double>::epsilon())) { 3990ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // We want to be careful to only evaluate the square root if the 4000ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // norm of the angle_axis vector is greater than zero. Otherwise 4010ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // we get a division by zero. 4020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T theta = sqrt(theta2); 4030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T wx = angle_axis[0] / theta; 4040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T wy = angle_axis[1] / theta; 4050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T wz = angle_axis[2] / theta; 4060ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4070ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T costheta = cos(theta); 4080ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T sintheta = sin(theta); 4090ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling R(0, 0) = costheta + wx*wx*(kOne - costheta); 4111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling R(1, 0) = wz*sintheta + wx*wy*(kOne - costheta); 4121d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling R(2, 0) = -wy*sintheta + wx*wz*(kOne - costheta); 4131d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling R(0, 1) = wx*wy*(kOne - costheta) - wz*sintheta; 4141d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling R(1, 1) = costheta + wy*wy*(kOne - costheta); 4151d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling R(2, 1) = wx*sintheta + wy*wz*(kOne - costheta); 4161d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling R(0, 2) = wy*sintheta + wx*wz*(kOne - costheta); 4171d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling R(1, 2) = -wx*sintheta + wy*wz*(kOne - costheta); 4181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling R(2, 2) = costheta + wz*wz*(kOne - costheta); 4190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } else { 42079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Near zero, we switch to using the first order Taylor expansion. 4211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling R(0, 0) = kOne; 42279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez R(1, 0) = angle_axis[2]; 42379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez R(2, 0) = -angle_axis[1]; 42479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez R(0, 1) = -angle_axis[2]; 4251d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling R(1, 1) = kOne; 42679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez R(2, 1) = angle_axis[0]; 42779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez R(0, 2) = angle_axis[1]; 42879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez R(1, 2) = -angle_axis[0]; 4291d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling R(2, 2) = kOne; 4300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 4310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 4320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate <typename T> 4340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Konginline void EulerAnglesToRotationMatrix(const T* euler, 4351d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const int row_stride_parameter, 4360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong T* R) { 4371d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling CHECK_EQ(row_stride_parameter, 3); 4381d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling EulerAnglesToRotationMatrix(euler, RowMajorAdapter3x3(R)); 4391d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 4401d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4411d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingtemplate <typename T, int row_stride, int col_stride> 4421d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingvoid EulerAnglesToRotationMatrix( 4431d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T* euler, 4441d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const MatrixAdapter<T, row_stride, col_stride>& R) { 4450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const double kPi = 3.14159265358979323846; 4460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T degrees_to_radians(kPi / 180.0); 4470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T pitch(euler[0] * degrees_to_radians); 4490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T roll(euler[1] * degrees_to_radians); 4500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T yaw(euler[2] * degrees_to_radians); 4510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T c1 = cos(yaw); 4530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T s1 = sin(yaw); 4540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T c2 = cos(roll); 4550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T s2 = sin(roll); 4560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T c3 = cos(pitch); 4570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T s3 = sin(pitch); 4580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4591d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling R(0, 0) = c1*c2; 4601d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling R(0, 1) = -s1*c3 + c1*s2*s3; 4611d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling R(0, 2) = s1*s3 + c1*s2*c3; 4620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4631d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling R(1, 0) = s1*c2; 4641d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling R(1, 1) = c1*c3 + s1*s2*s3; 4651d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling R(1, 2) = -c1*s3 + s1*s2*c3; 4660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4671d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling R(2, 0) = -s2; 4681d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling R(2, 1) = c2*s3; 4691d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling R(2, 2) = c2*c3; 4700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 4710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate <typename T> inline 4730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid QuaternionToScaledRotation(const T q[4], T R[3 * 3]) { 4741d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling QuaternionToScaledRotation(q, RowMajorAdapter3x3(R)); 4751d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 4761d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 4771d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingtemplate <typename T, int row_stride, int col_stride> inline 4781d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingvoid QuaternionToScaledRotation( 4791d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const T q[4], 4801d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const MatrixAdapter<T, row_stride, col_stride>& R) { 4810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Make convenient names for elements of q. 4820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong T a = q[0]; 4830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong T b = q[1]; 4840ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong T c = q[2]; 4850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong T d = q[3]; 4860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // This is not to eliminate common sub-expression, but to 4870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // make the lines shorter so that they fit in 80 columns! 4880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong T aa = a * a; 4890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong T ab = a * b; 4900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong T ac = a * c; 4910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong T ad = a * d; 4920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong T bb = b * b; 4930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong T bc = b * c; 4940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong T bd = b * d; 4950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong T cc = c * c; 4960ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong T cd = c * d; 4970ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong T dd = d * d; 4980ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 4991d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling R(0, 0) = aa + bb - cc - dd; R(0, 1) = T(2) * (bc - ad); R(0, 2) = T(2) * (ac + bd); // NOLINT 5001d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling R(1, 0) = T(2) * (ad + bc); R(1, 1) = aa - bb + cc - dd; R(1, 2) = T(2) * (cd - ab); // NOLINT 5011d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling R(2, 0) = T(2) * (bd - ac); R(2, 1) = T(2) * (ab + cd); R(2, 2) = aa - bb - cc + dd; // NOLINT 5020ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 5030ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5040ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate <typename T> inline 5050ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid QuaternionToRotation(const T q[4], T R[3 * 3]) { 5061d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling QuaternionToRotation(q, RowMajorAdapter3x3(R)); 5071d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling} 5081d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling 5091d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingtemplate <typename T, int row_stride, int col_stride> inline 5101d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberlingvoid QuaternionToRotation(const T q[4], 5111d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling const MatrixAdapter<T, row_stride, col_stride>& R) { 5120ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong QuaternionToScaledRotation(q, R); 5130ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5140ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong T normalizer = q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]; 5150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong CHECK_NE(normalizer, T(0)); 5160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong normalizer = T(1) / normalizer; 5170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5181d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int i = 0; i < 3; ++i) { 5191d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling for (int j = 0; j < 3; ++j) { 5201d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling R(i, j) *= normalizer; 5211d2624a10e2c559f8ba9ef89eaa30832c0a83a96Sascha Haeberling } 5220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 5230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 5240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate <typename T> inline 5260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid UnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) { 5270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T t2 = q[0] * q[1]; 5280ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T t3 = q[0] * q[2]; 5290ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T t4 = q[0] * q[3]; 5300ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T t5 = -q[1] * q[1]; 5310ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T t6 = q[1] * q[2]; 5320ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T t7 = q[1] * q[3]; 5330ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T t8 = -q[2] * q[2]; 5340ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T t9 = q[2] * q[3]; 5350ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T t1 = -q[3] * q[3]; 5360ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong result[0] = T(2) * ((t8 + t1) * pt[0] + (t6 - t4) * pt[1] + (t3 + t7) * pt[2]) + pt[0]; // NOLINT 5370ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong result[1] = T(2) * ((t4 + t6) * pt[0] + (t5 + t1) * pt[1] + (t9 - t2) * pt[2]) + pt[1]; // NOLINT 5380ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong result[2] = T(2) * ((t7 - t3) * pt[0] + (t2 + t9) * pt[1] + (t5 + t8) * pt[2]) + pt[2]; // NOLINT 5390ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 5400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate <typename T> inline 5420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid QuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) { 5430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 'scale' is 1 / norm(q). 5440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T scale = T(1) / sqrt(q[0] * q[0] + 5450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong q[1] * q[1] + 5460ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong q[2] * q[2] + 5470ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong q[3] * q[3]); 5480ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5490ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Make unit-norm version of q. 5500ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T unit[4] = { 5510ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong scale * q[0], 5520ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong scale * q[1], 5530ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong scale * q[2], 5540ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong scale * q[3], 5550ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong }; 5560ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5570ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong UnitQuaternionRotatePoint(unit, pt, result); 5580ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 5590ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5600ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate<typename T> inline 5610ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid QuaternionProduct(const T z[4], const T w[4], T zw[4]) { 5620ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong zw[0] = z[0] * w[0] - z[1] * w[1] - z[2] * w[2] - z[3] * w[3]; 5630ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong zw[1] = z[0] * w[1] + z[1] * w[0] + z[2] * w[3] - z[3] * w[2]; 5640ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong zw[2] = z[0] * w[2] - z[1] * w[3] + z[2] * w[0] + z[3] * w[1]; 5650ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong zw[3] = z[0] * w[3] + z[1] * w[2] - z[2] * w[1] + z[3] * w[0]; 5660ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 5670ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5680ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong// xy = x cross y; 5690ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate<typename T> inline 5700ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid CrossProduct(const T x[3], const T y[3], T x_cross_y[3]) { 5710ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong x_cross_y[0] = x[1] * y[2] - x[2] * y[1]; 5720ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong x_cross_y[1] = x[2] * y[0] - x[0] * y[2]; 5730ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong x_cross_y[2] = x[0] * y[1] - x[1] * y[0]; 5740ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 5750ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5760ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate<typename T> inline 5770ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus KongT DotProduct(const T x[3], const T y[3]) { 5780ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong return (x[0] * y[0] + x[1] * y[1] + x[2] * y[2]); 5790ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 5800ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 5810ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongtemplate<typename T> inline 5820ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kongvoid AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]) { 5830ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T theta2 = DotProduct(angle_axis, angle_axis); 58479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez if (theta2 > T(std::numeric_limits<double>::epsilon())) { 5850ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Away from zero, use the rodriguez formula 5860ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 5870ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // result = pt costheta + 5880ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // (w x pt) * sintheta + 5890ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // w (w . pt) (1 - costheta) 5900ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 5910ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // We want to be careful to only evaluate the square root if the 5920ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // norm of the angle_axis vector is greater than zero. Otherwise 5930ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // we get a division by zero. 5940ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 5950ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong const T theta = sqrt(theta2); 59679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const T costheta = cos(theta); 59779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const T sintheta = sin(theta); 59879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const T theta_inverse = 1.0 / theta; 59979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 60079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const T w[3] = { angle_axis[0] * theta_inverse, 60179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez angle_axis[1] * theta_inverse, 60279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez angle_axis[2] * theta_inverse }; 60379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 60479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Explicitly inlined evaluation of the cross product for 60579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // performance reasons. 60679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const T w_cross_pt[3] = { w[1] * pt[2] - w[2] * pt[1], 60779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez w[2] * pt[0] - w[0] * pt[2], 60879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez w[0] * pt[1] - w[1] * pt[0] }; 60979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const T tmp = 61079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez (w[0] * pt[0] + w[1] * pt[1] + w[2] * pt[2]) * (T(1.0) - costheta); 61179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 61279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez result[0] = pt[0] * costheta + w_cross_pt[0] * sintheta + w[0] * tmp; 61379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez result[1] = pt[1] * costheta + w_cross_pt[1] * sintheta + w[1] * tmp; 61479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez result[2] = pt[2] * costheta + w_cross_pt[2] * sintheta + w[2] * tmp; 6150ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } else { 6160ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // Near zero, the first order Taylor approximation of the rotation 6170ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // matrix R corresponding to a vector w and angle w is 6180ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 6190ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // R = I + hat(w) * sin(theta) 6200ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 6210ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // But sintheta ~ theta and theta * w = angle_axis, which gives us 6220ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 6230ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // R = I + hat(w) 6240ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 6250ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // and actually performing multiplication with the point pt, gives us 6260ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // R * pt = pt + w x pt. 6270ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong // 62879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Switching to the Taylor expansion near zero provides meaningful 62979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // derivatives when evaluated using Jets. 63079397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // 63179397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // Explicitly inlined evaluation of the cross product for 63279397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez // performance reasons. 63379397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez const T w_cross_pt[3] = { angle_axis[1] * pt[2] - angle_axis[2] * pt[1], 63479397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez angle_axis[2] * pt[0] - angle_axis[0] * pt[2], 63579397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez angle_axis[0] * pt[1] - angle_axis[1] * pt[0] }; 63679397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez 63779397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez result[0] = pt[0] + w_cross_pt[0]; 63879397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez result[1] = pt[1] + w_cross_pt[1]; 63979397c21138f54fcff6ec067b44b847f1f7e0e98Carlos Hernandez result[2] = pt[2] + w_cross_pt[2]; 6400ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong } 6410ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} 6420ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 6430ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong} // namespace ceres 6440ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong 6450ae28bd5885b5daa526898fcf7c323dc2c3e1963Angus Kong#endif // CERES_PUBLIC_ROTATION_H_ 646