一. 主动旋转和被动旋转
- active rotation 主动旋转
站在坐标系的位置看旋转目标物：目标物主动发生旋转。
- passive rotation 被动旋转
站在旋转目标物的位置看坐标系： 坐标系发生旋转，相当于目标物在坐标系内的位置被动地发生了旋转。
主动旋转的旋转矩阵和被动旋转的旋转矩阵是互相为逆的。
二. 内旋和外旋
-
intrinsic rotations 动坐标系旋转，即坐标系原点固定于目标物内部，当目标物发生旋转，坐标系也跟着做刚性旋转。
-
extrinsic rotations 固定坐标系旋转，即坐标系位于外部参考点，当目标物旋转，坐标系不动。
欧拉角描述物体姿态时，必须先确定是基于内旋坐标系还是外旋坐标系。
三. 3D旋转库
使用python的朋友，可以直接安装pytransform3d库就可以实现任意3d姿态的变换，里面有各种欧拉角，旋转矩阵，四元数之间的相互转换。官方网址为：pytransform3d --- pytransform3d 3.4.0 documentation
如果使用c/c++，则可以从pytransform3d库中的提取关键部分，改写为c/c++，以供调用。下面就将这些欧拉角转换部分提取出来，以源码形式贴出来，供有需要的朋友使用(下面欧拉角是弧度制表示的，使用时需要注意)：
cpp
// transform3d.h
#ifndef __TRANSFORM_3D_H__
#define __TRANSFORM_3D_H__
//https://dfki-ric.github.io/pytransform3d/euler_angles.html
typedef float MATRIX_SO3_F[9];
typedef float MATRIX_SE3_F[16];
typedef float MATRIX_EULER_F[3];
typedef float MATRIX_QUATERNION_F[4];
typedef float VECTOR3F[3];
bool matrix_so3_inverse_f(const MATRIX_SO3_F r, MATRIX_SO3_F r_t);
bool matrix_se3_inverse_f(const MATRIX_SE3_F t, MATRIX_SE3_F t_inv);
void matrix_3x3_product_f(const MATRIX_SO3_F r1, const MATRIX_SO3_F r2, MATRIX_SO3_F r);
void matrix_4x4_product_f(const MATRIX_SE3_F se1, const MATRIX_SE3_F se2, MATRIX_SE3_F se);
bool active_matrix_from_angle(int basis, float angle, MATRIX_SO3_F so);
void matrix_so3_from_se3_f(const MATRIX_SE3_F se, MATRIX_SO3_F so, VECTOR3F xyz);
void matrix_se3_from_so3_f(const MATRIX_SO3_F so, const VECTOR3F xyz, MATRIX_SE3_F se);
bool general_intrinsic_euler_from_active_matrix(const MATRIX_SO3_F R, const VECTOR3F n1, const VECTOR3F n2, const VECTOR3F n3, bool proper_euler, MATRIX_EULER_F euler, bool strict_check = true);
bool euler_from_matrix(const MATRIX_SO3_F R, int i, int j, int k, MATRIX_EULER_F result, bool extrinsic, bool strict_check = true);
bool matrix_from_euler(const MATRIX_EULER_F e, int i, int j, int k, MATRIX_SO3_F result, bool extrinsic);
bool quaternion_from_angle(int basis, float angle, MATRIX_QUATERNION_F quat);
bool quaternion_from_euler(MATRIX_EULER_F e, int i, int j, int k, MATRIX_QUATERNION_F res, bool extrinsic);
bool euler_from_quaternion(const MATRIX_QUATERNION_F q, int i, int j, int k, MATRIX_EULER_F euler, bool extrinsic);
bool matrix_from_quaternion(const MATRIX_QUATERNION_F q, MATRIX_SO3_F R);
bool quaternion_from_matrix(const MATRIX_SO3_F R, MATRIX_QUATERNION_F res, bool strict_check = true);
bool active_matrix_from_intrinsic_euler_xzx(const MATRIX_EULER_F e, MATRIX_SO3_F R);
bool active_matrix_from_extrinsic_euler_xzx(const MATRIX_EULER_F e, MATRIX_SO3_F R);
bool active_matrix_from_intrinsic_euler_xyx(const MATRIX_EULER_F e, MATRIX_SO3_F R);
bool active_matrix_from_extrinsic_euler_xyx(const MATRIX_EULER_F e, MATRIX_SO3_F R);
bool active_matrix_from_intrinsic_euler_yxy(const MATRIX_EULER_F e, MATRIX_SO3_F R);
bool active_matrix_from_extrinsic_euler_yxy(const MATRIX_EULER_F e, MATRIX_SO3_F R);
bool active_matrix_from_intrinsic_euler_yzy(const MATRIX_EULER_F e, MATRIX_SO3_F R);
bool active_matrix_from_extrinsic_euler_yzy(const MATRIX_EULER_F e, MATRIX_SO3_F R);
bool active_matrix_from_intrinsic_euler_zyz(const MATRIX_EULER_F e, MATRIX_SO3_F R);
bool active_matrix_from_extrinsic_euler_zyz(const MATRIX_EULER_F e, MATRIX_SO3_F R);
bool active_matrix_from_intrinsic_euler_zxz(const MATRIX_EULER_F e, MATRIX_SO3_F R);
bool active_matrix_from_extrinsic_euler_zxz(const MATRIX_EULER_F e, MATRIX_SO3_F R);
bool active_matrix_from_intrinsic_euler_xzy(const MATRIX_EULER_F e, MATRIX_SO3_F R);
bool active_matrix_from_extrinsic_euler_xzy(const MATRIX_EULER_F e, MATRIX_SO3_F R);
bool active_matrix_from_intrinsic_euler_xyz(const MATRIX_EULER_F e, MATRIX_SO3_F R);
bool active_matrix_from_extrinsic_euler_xyz(const MATRIX_EULER_F e, MATRIX_SO3_F R);
bool active_matrix_from_intrinsic_euler_yxz(const MATRIX_EULER_F e, MATRIX_SO3_F R);
bool active_matrix_from_extrinsic_euler_yxz(const MATRIX_EULER_F e, MATRIX_SO3_F R);
bool active_matrix_from_intrinsic_euler_yzx(const MATRIX_EULER_F e, MATRIX_SO3_F R);
bool active_matrix_from_extrinsic_euler_yzx(const MATRIX_EULER_F e, MATRIX_SO3_F R);
bool active_matrix_from_intrinsic_euler_zyx(const MATRIX_EULER_F e, MATRIX_SO3_F R);
bool active_matrix_from_extrinsic_euler_zyx(const MATRIX_EULER_F e, MATRIX_SO3_F R);
bool active_matrix_from_intrinsic_euler_zxy(const MATRIX_EULER_F e, MATRIX_SO3_F R);
bool active_matrix_from_extrinsic_euler_zxy(const MATRIX_EULER_F e, MATRIX_SO3_F R);
bool active_matrix_from_extrinsic_roll_pitch_yaw(const VECTOR3F rpy, MATRIX_SO3_F R);
bool intrinsic_euler_xzx_from_active_matrix(const MATRIX_SO3_F R, MATRIX_EULER_F euler, bool strict_check = true);
bool extrinsic_euler_xzx_from_active_matrix(const MATRIX_SO3_F R, MATRIX_EULER_F euler, bool strict_check = true);
bool intrinsic_euler_xyx_from_active_matrix(const MATRIX_SO3_F R, MATRIX_EULER_F euler, bool strict_check = true);
bool extrinsic_euler_xyx_from_active_matrix(const MATRIX_SO3_F R, MATRIX_EULER_F euler, bool strict_check = true);
bool intrinsic_euler_yxy_from_active_matrix(const MATRIX_SO3_F R, MATRIX_EULER_F euler, bool strict_check = true);
bool extrinsic_euler_yxy_from_active_matrix(const MATRIX_SO3_F R, MATRIX_EULER_F euler, bool strict_check = true);
bool intrinsic_euler_yzy_from_active_matrix(const MATRIX_SO3_F R, MATRIX_EULER_F euler, bool strict_check = true);
bool extrinsic_euler_yzy_from_active_matrix(const MATRIX_SO3_F R, MATRIX_EULER_F euler, bool strict_check = true);
bool intrinsic_euler_zyz_from_active_matrix(const MATRIX_SO3_F R, MATRIX_EULER_F euler, bool strict_check = true);
bool extrinsic_euler_zyz_from_active_matrix(const MATRIX_SO3_F R, MATRIX_EULER_F euler, bool strict_check = true);
bool intrinsic_euler_zxz_from_active_matrix(const MATRIX_SO3_F R, MATRIX_EULER_F euler, bool strict_check = true);
bool extrinsic_euler_zxz_from_active_matrix(const MATRIX_SO3_F R, MATRIX_EULER_F euler, bool strict_check = true);
bool intrinsic_euler_xzy_from_active_matrix(const MATRIX_SO3_F R, MATRIX_EULER_F euler, bool strict_check = true);
bool extrinsic_euler_xzy_from_active_matrix(const MATRIX_SO3_F R, MATRIX_EULER_F euler, bool strict_check = true);
bool intrinsic_euler_xyz_from_active_matrix(const MATRIX_SO3_F R, MATRIX_EULER_F euler, bool strict_check = true);
bool extrinsic_euler_xyz_from_active_matrix(const MATRIX_SO3_F R, MATRIX_EULER_F euler, bool strict_check = true);
bool intrinsic_euler_yxz_from_active_matrix(const MATRIX_SO3_F R, MATRIX_EULER_F euler, bool strict_check = true);
bool extrinsic_euler_yxz_from_active_matrix(const MATRIX_SO3_F R, MATRIX_EULER_F euler, bool strict_check = true);
bool intrinsic_euler_yzx_from_active_matrix(const MATRIX_SO3_F R, MATRIX_EULER_F euler, bool strict_check = true);
bool extrinsic_euler_yzx_from_active_matrix(const MATRIX_SO3_F R, MATRIX_EULER_F euler, bool strict_check = true);
bool intrinsic_euler_zyx_from_active_matrix(const MATRIX_SO3_F R, MATRIX_EULER_F euler, bool strict_check = true);
bool extrinsic_euler_zyx_from_active_matrix(const MATRIX_SO3_F R, MATRIX_EULER_F euler, bool strict_check = true);
bool intrinsic_euler_zxy_from_active_matrix(const MATRIX_SO3_F R, MATRIX_EULER_F euler, bool strict_check = true);
bool extrinsic_euler_zxy_from_active_matrix(const MATRIX_SO3_F R, MATRIX_EULER_F euler, bool strict_check = true);
#endif
cpp
// transform3d.cpp
#include "transform3d.h"
#include <math.h>
#include <string.h>
#include <iostream>
//#define PI acos(-1)
#define PI 3.14159265358979323846
const VECTOR3F unitxyz[3] = { { 1.0f, 0.0f, 0.0f },
{ 0.0f, 1.0f, 0.0f },
{ 0.0f, 0.0f, 1.0f } };
void matrix_3x3_product_f(const MATRIX_SO3_F r1, const MATRIX_SO3_F r2, MATRIX_SO3_F r)
{
r[0] = r1[0] * r2[0] + r1[1] * r2[3] + r1[2] * r2[6];
r[1] = r1[0] * r2[1] + r1[1] * r2[4] + r1[2] * r2[7];
r[2] = r1[0] * r2[2] + r1[1] * r2[5] + r1[2] * r2[8];
r[3] = r1[3] * r2[0] + r1[4] * r2[3] + r1[5] * r2[6];
r[4] = r1[3] * r2[1] + r1[4] * r2[4] + r1[5] * r2[7];
r[5] = r1[3] * r2[2] + r1[4] * r2[5] + r1[5] * r2[8];
r[6] = r1[6] * r2[0] + r1[7] * r2[3] + r1[8] * r2[6];
r[7] = r1[6] * r2[1] + r1[7] * r2[4] + r1[8] * r2[7];
r[8] = r1[6] * r2[2] + r1[7] * r2[5] + r1[8] * r2[8];
return;
}
void matrix_4x4_product_f(const MATRIX_SE3_F se1, const MATRIX_SE3_F se2, MATRIX_SE3_F se)
{
se[0] = se1[0] * se2[0] + se1[1] * se2[4] + se1[2] * se2[8] + se1[3] * se2[12];
se[1] = se1[0] * se2[1] + se1[1] * se2[5] + se1[2] * se2[9] + se1[3] * se2[13];
se[2] = se1[0] * se2[2] + se1[1] * se2[6] + se1[2] * se2[10] + se1[3] * se2[14];
se[3] = se1[0] * se2[3] + se1[1] * se2[7] + se1[2] * se2[11] + se1[3] * se2[15];
se[4] = se1[4] * se2[0] + se1[5] * se2[4] + se1[6] * se2[8] + se1[7] * se2[12];
se[5] = se1[4] * se2[1] + se1[5] * se2[5] + se1[6] * se2[9] + se1[7] * se2[13];
se[6] = se1[4] * se2[2] + se1[5] * se2[6] + se1[6] * se2[10] + se1[7] * se2[14];
se[7] = se1[4] * se2[3] + se1[5] * se2[7] + se1[6] * se2[11] + se1[7] * se2[15];
se[8] = se1[8] * se2[0] + se1[9] * se2[4] + se1[10] * se2[8] + se1[11] * se2[12];
se[9] = se1[8] * se2[1] + se1[9] * se2[5] + se1[10] * se2[9] + se1[11] * se2[13];
se[10] = se1[8] * se2[2] + se1[9] * se2[6] + se1[10] * se2[10] + se1[11] * se2[14];
se[11] = se1[8] * se2[3] + se1[9] * se2[7] + se1[10] * se2[11] + se1[11] * se2[15];
se[12] = se1[12] * se2[0] + se1[13] * se2[4] + se1[14] * se2[8] + se1[15] * se2[12];
se[13] = se1[12] * se2[1] + se1[13] * se2[5] + se1[14] * se2[9] + se1[15] * se2[13];
se[14] = se1[12] * se2[2] + se1[13] * se2[6] + se1[14] * se2[10] + se1[15] * se2[14];
se[15] = se1[12] * se2[3] + se1[13] * se2[7] + se1[14] * se2[11] + se1[15] * se2[15];
return;
}
void matrix_3x3_transpose_f(const MATRIX_SO3_F r1, MATRIX_SO3_F r)
{
r[0] = r1[0];
r[1] = r1[3];
r[2] = r1[6];
r[3] = r1[1];
r[4] = r1[4];
r[5] = r1[7];
r[6] = r1[2];
r[7] = r1[5];
r[8] = r1[8];
return;
}
void matrix_3x1_cross_f(const MATRIX_EULER_F r1, const MATRIX_EULER_F r2, MATRIX_EULER_F r)
{
r[0] = r1[1] * r2[2] - r1[2] * r2[1];
r[1] = r1[2] * r2[0] - r1[0] * r2[2];
r[2] = r1[0] * r2[1] - r1[1] * r2[0];
return;
}
float matrix_3x3_determinants_f(const MATRIX_SO3_F r1)
{
float res = 0.0f;
res = r1[0] * r1[4] * r1[8] + r1[2] * r1[5] * r1[6] + r1[3] * r1[7] * r1[2];
res = res - r1[2] * r1[4] * r1[6] - r1[1] * r1[3] * r1[8] - r1[5] * r1[7] * r1[0];
return res;
}
void matrix_so3_from_se3_f(const MATRIX_SE3_F se, MATRIX_SO3_F so, VECTOR3F xyz)
{
so[0] = se[0];
so[1] = se[1];
so[2] = se[2];
xyz[0] = se[3];
so[3] = se[4];
so[4] = se[5];
so[5] = se[6];
xyz[1] = se[7];
so[6] = se[8];
so[7] = se[9];
so[8] = se[10];
xyz[2] = se[11];
return;
}
void matrix_se3_from_so3_f(const MATRIX_SO3_F so, const VECTOR3F xyz, MATRIX_SE3_F se)
{
se[0] = so[0];
se[1] = so[1];
se[2] = so[2];
se[3] = xyz[0];
se[4] = so[3];
se[5] = so[4];
se[6] = so[5];
se[7] = xyz[1];
se[8] = so[6];
se[9] = so[7];
se[10] = so[8];
se[11] = xyz[2];
se[12] = 0.0f;
se[13] = 0.0f;
se[14] = 0.0f;
se[15] = 1.0f;
return;
}
float norm_angle(float angle)
{
/*"""Normalize angle to (-pi, pi].
Parameters
----------
a : float or array - like, shape(n, )
Angle(s) in radians
Returns
------ -
a_norm : float or array - like, shape(n, )
Normalized angle(s) in radians
"""*/
float mod = 0.0f;
int exp = int((PI - angle) / (2.0f*PI));
if (exp < 0)
mod = 2.0f*PI*(1 - exp) - (PI - angle);
else if (exp == 0)
mod = (PI - angle);
else
mod = PI - angle - 2.0f*PI*exp;
return PI - mod;
}
bool check_matrix(const MATRIX_SO3_F R, bool strict_check, float tolerance = 1e-6)
{
MATRIX_SO3_F RT = {}, RRT = {};
matrix_3x3_transpose_f(R, RT);
matrix_3x3_product_f(R, RT, RRT);
bool ok = true;
ok = ok && abs(RRT[0] - 1.0f)<tolerance;
ok = ok && abs(RRT[4] - 1.0f)<tolerance;
ok = ok && abs(RRT[8] - 1.0f)<tolerance;
ok = ok && abs(RRT[1] - 0.0f)<tolerance;
ok = ok && abs(RRT[2] - 0.0f)<tolerance;
ok = ok && abs(RRT[3] - 0.0f)<tolerance;
ok = ok && abs(RRT[5] - 0.0f)<tolerance;
ok = ok && abs(RRT[6] - 0.0f)<tolerance;
ok = ok && abs(RRT[7] - 0.0f)<tolerance;
if (!ok)
{
if (strict_check)
return false;
//printf("Warning: Expected rotation matrix, but it failed the test for inversion by transposition.\n");
}
float R_det = matrix_3x3_determinants_f(R);
if (R_det < 0.0)
{
if (strict_check)
return false;
//printf("Warning: Expected rotation matrix, but it failed the test for the determinant.\n");
}
return true;
}
bool matrix_so3_inverse_f(const MATRIX_SO3_F r, MATRIX_SO3_F r_t)
{
bool ret = check_matrix(r, false);
if (!ret)
return false;
MATRIX_SO3_F r_t_tmp = {};
r_t_tmp[0] = r[0];
r_t_tmp[3] = r[1];
r_t_tmp[6] = r[2];
r_t_tmp[1] = r[3];
r_t_tmp[4] = r[4];
r_t_tmp[7] = r[5];
r_t_tmp[2] = r[6];
r_t_tmp[5] = r[7];
r_t_tmp[8] = r[8];
memcpy(r_t, r_t_tmp, sizeof(MATRIX_SO3_F));
return true;
}
bool matrix_se3_inverse_f(const MATRIX_SE3_F t, MATRIX_SE3_F t_inv)
{
MATRIX_SO3_F r = {};
MATRIX_SO3_F r_t = {};
VECTOR3F p = {};
matrix_so3_from_se3_f(t, r, p);
bool ret = matrix_so3_inverse_f(r, r_t);
if (!ret)
return false;
t_inv[0] = r_t[0];
t_inv[1] = r_t[1];
t_inv[2] = r_t[2];
t_inv[4] = r_t[3];
t_inv[5] = r_t[4];
t_inv[6] = r_t[5];
t_inv[8] = r_t[6];
t_inv[9] = r_t[7];
t_inv[10] = r_t[8];
t_inv[3] = -r_t[0] * p[0] - r_t[1] * p[1] - r_t[2] * p[2];
t_inv[7] = -r_t[3] * p[0] - r_t[4] * p[1] - r_t[5] * p[2];
t_inv[11] = -r_t[6] * p[0] - r_t[7] * p[1] - r_t[8] * p[2];
t_inv[12] = 0.0f;
t_inv[13] = 0.0f;
t_inv[14] = 0.0f;
t_inv[15] = 1.0f;
return true;
}
bool matrix_norm_vector_f(float vec[], int ndims)
{
if (ndims<1)
return false;
float square_sum = 0.0;
for (int i = 0; i<ndims; i++)
square_sum = square_sum + vec[i] * vec[i];
square_sum = sqrt(square_sum);
if (square_sum == 0.0f)
return false;
for (int j = 0; j<ndims; j++)
vec[j] = vec[j] / sqrt(square_sum);
return true;
}
bool active_matrix_from_angle(int basis, float angle, MATRIX_SO3_F so)
{
float c = cos(angle);
float s = sin(angle);
if (basis == 0)
{
so[0] = 1.0f;
so[1] = 0.0f;
so[2] = 0.0f;
so[3] = 0.0f;
so[4] = c;
so[5] = 0.0f - s;
so[6] = 0.0f;
so[7] = s;
so[8] = c;
}
else if (basis == 1)
{
so[0] = c;
so[1] = 0.0f;
so[2] = s;
so[3] = 0.0f;
so[4] = 1.0f;
so[5] = 0.0f;
so[6] = 0.0f - s;
so[7] = 0.0f;
so[8] = c;
}
else if (basis == 2)
{
so[0] = c;
so[1] = 0.0f - s;
so[2] = 0.0f;
so[3] = s;
so[4] = c;
so[5] = 0.0f;
so[6] = 0.0f;
so[7] = 0.0f;
so[8] = 1.0f;
}
else
{
return false;
}
return true;
}
bool matrix_from_euler(const MATRIX_EULER_F e, int i, int j, int k, MATRIX_SO3_F result, bool extrinsic)
{
if (i != 0 && i != 1 && i != 2)
return false;
if (j != 0 && j != 1 && j != 2)
return false;
if (k != 0 && k != 1 && k != 2)
return false;
float alpha = e[0], beta = e[1], gamma = e[2], tmp = e[0];
if (extrinsic == false)
{
i = i^k;
k = i^k;
i = i^k;
alpha = gamma;
gamma = tmp;
}
MATRIX_SO3_F so1 = {};
MATRIX_SO3_F so2 = {};
MATRIX_SO3_F so3 = {};
bool ret = active_matrix_from_angle(k, gamma, so1);
ret = ret && active_matrix_from_angle(j, beta, so2);
ret = ret && active_matrix_from_angle(i, alpha, so3);
if (ret)
{
MATRIX_SO3_F dot1 = {};
matrix_3x3_product_f(so1, so2, dot1);
matrix_3x3_product_f(dot1, so3, result);
return true;
}
return false;
}
bool general_intrinsic_euler_from_active_matrix(const MATRIX_SO3_F R, const VECTOR3F n1, const VECTOR3F n2, const VECTOR3F n3, bool proper_euler, MATRIX_EULER_F euler, bool strict_check)
{
bool ret = check_matrix(R, strict_check);
if (ret == false)
return ret;
MATRIX_EULER_F n1_cross_n2 = {};
matrix_3x1_cross_f(n1, n2, n1_cross_n2);
float lmbda = atan2(n1_cross_n2[0] * n3[0] + n1_cross_n2[1] * n3[1] + n1_cross_n2[2] * n3[2],
n1[0] * n3[0] + n1[1] * n3[1] + n1[2] * n3[2]);
float C[9] = { n2[0], n2[1], n2[2], n1_cross_n2[0], n1_cross_n2[1], n1_cross_n2[2], n1[0], n1[1], n1[2] };
float CT[9] = {};
matrix_3x3_transpose_f(C, CT);
float CR[9] = {};
matrix_3x3_product_f(C, R, CR);
float CRCT[9] = {};
matrix_3x3_product_f(CR, CT, CRCT);
MATRIX_SO3_F lambda_so = {}, lambda_so_T = {};
ret = active_matrix_from_angle(0, lmbda, lambda_so);
matrix_3x3_transpose_f(lambda_so, lambda_so_T);
float O[9] = {};
matrix_3x3_product_f(CRCT, lambda_so_T, O);
float max_22 = (O[8] <= 1.0f ? O[8] : 1.0f)>(0.0f - 1.0f) ? (O[8] <= 1.0f ? O[8] : 1.0f) : (0.0f - 1.0f);
float beta = lmbda + acos(max_22);
bool safe1 = abs(beta - lmbda) >= 1e-10;
bool safe2 = abs(beta - lmbda - PI) >= 1e-10;
float alpha = 0.0f, gamma = 0.0f;
if (safe1 && safe2)
{
alpha = atan2(O[2], 0.0f - O[5]);
gamma = atan2(O[6], O[7]);
bool valid_beta = false;
if (proper_euler)
{
if ((beta >= 0.0f && beta <= PI))
valid_beta = true;
else
valid_beta = false;
}
else
{
if ((0.0f - 0.5f*PI <= beta) && (beta <= 0.5f*PI))
valid_beta = true;
else
valid_beta = false;
}
if (valid_beta == false)
{
alpha += PI;
beta = 2.0f * lmbda - beta;
gamma -= PI;
}
}
else
{
gamma = 0.0f;
if (safe1 == false)
alpha = atan2(O[3] - O[1], O[0] + O[4]);
else
alpha = atan2(O[3] + O[1], O[0] - O[4]);
}
//euler_angles = norm_angle([alpha, beta, gamma]);
alpha = norm_angle(alpha);
beta = norm_angle(beta);
gamma = norm_angle(gamma);
euler[0] = alpha;
euler[1] = beta;
euler[2] = gamma;
return true;
}
bool euler_from_matrix(const MATRIX_SO3_F R, int i, int j, int k, MATRIX_EULER_F result, bool extrinsic, bool strict_check)
{
if (i != 0 && i != 1 && i != 2)
return false;
if (j != 0 && j != 1 && j != 2)
return false;
if (k != 0 && k != 1 && k != 2)
return false;
bool proper_euler = i == k;
if (extrinsic)
{
i = i^k;
k = i^k;
i = i^k;
}
MATRIX_EULER_F euler;
bool ret = general_intrinsic_euler_from_active_matrix(R, unitxyz[i], unitxyz[j], unitxyz[k], proper_euler, euler, strict_check);
if (!ret)
return false;
if (extrinsic)
{
float tmp = euler[0];
euler[0] = euler[2];
euler[2] = tmp;
}
result[0] = euler[0];
result[1] = euler[1];
result[2] = euler[2];
return true;
}
bool quaternion_from_angle(int basis, float angle, MATRIX_QUATERNION_F quat)
{
/*Compute quaternion from rotation about basis vector.
Parameters
----------
basis : int from [0, 1, 2]
The rotation axis (0: x, 1: y, 2: z)
angle : float
Rotation angle
Returns
-------
q : array, shape (4,)
Unit quaternion to represent rotation: (w, x, y, z)
Raises
------
ValueError
If basis is invalid
*/
float half_angle = 0.5f * angle;
float c = cos(half_angle);
float s = sin(half_angle);
if (basis == 0)
{
quat[0] = c;
quat[1] = s;
quat[2] = 0.0f;
quat[3] = 0.0f;
}
else if (basis == 1)
{
quat[0] = c;
quat[1] = 0.0f;
quat[2] = s;
quat[3] = 0.0f;
}
else if (basis == 2)
{
quat[0] = c;
quat[1] = 0.0f;
quat[2] = 0.0f;
quat[3] = s;
}
else
return false;
return true;
}
void concatenate_quaternions(const MATRIX_QUATERNION_F q0, const MATRIX_QUATERNION_F q1, MATRIX_QUATERNION_F res)
{
res[0] = q0[0] * q1[0] - (q0[1] * q1[1] + q0[2] * q1[2] + q0[3] * q1[3]);
MATRIX_EULER_F part1 = { q0[1], q0[2], q0[3] };
MATRIX_EULER_F part2 = { q1[1], q1[2], q1[3] };
MATRIX_EULER_F cross1 = {};
matrix_3x1_cross_f(part1, part2, cross1);
res[1] = q0[0] * q1[1] + q1[0] * q0[1] + cross1[0];
res[2] = q0[0] * q1[2] + q1[0] * q0[2] + cross1[1];
res[3] = q0[0] * q1[3] + q1[0] * q0[3] + cross1[2];
return;
}
bool quaternion_from_euler(MATRIX_EULER_F e, int i, int j, int k, MATRIX_QUATERNION_F res, bool extrinsic)
{
/*General conversion to quaternion from any Euler angles.
Parameters
----------
e : array-like, shape (3,)
Rotation angles in radians about the axes i, j, k in this order. The
first and last angle are normalized to [-pi, pi]. The middle angle is
normalized to either [0, pi] (proper Euler angles) or [-pi/2, pi/2]
(Cardan / Tait-Bryan angles).
i : int from [0, 1, 2]
The first rotation axis (0: x, 1: y, 2: z)
j : int from [0, 1, 2]
The second rotation axis (0: x, 1: y, 2: z)
k : int from [0, 1, 2]
The third rotation axis (0: x, 1: y, 2: z)
extrinsic : bool
Do we use extrinsic transformations? Intrinsic otherwise.
Returns
-------
q : array, shape (4,)
Unit quaternion to represent rotation: (w, x, y, z)
Raises
------
ValueError
If basis is invalid
*/
if (i != 0 && i != 1 && i != 2)
return false;
if (j != 0 && j != 1 && j != 2)
return false;
if (k != 0 && k != 1 && k != 2)
return false;
MATRIX_QUATERNION_F q0, q1, q2;
quaternion_from_angle(i, e[0], q0);
quaternion_from_angle(j, e[1], q1);
quaternion_from_angle(k, e[2], q2);
bool ret = true;
MATRIX_QUATERNION_F tmp = {};
if (extrinsic == false)
{
concatenate_quaternions(q0, q1, tmp);
concatenate_quaternions(tmp, q2, res);
}
else
{
concatenate_quaternions(q2, q1, tmp);
concatenate_quaternions(tmp, q0, res);
}
return ret;
}
bool euler_from_quaternion(const MATRIX_QUATERNION_F q, int i, int j, int k, MATRIX_EULER_F euler, bool extrinsic)
{
/*General method to extract any Euler angles from quaternions.
Parameters
----------
q : array-like, shape (4,)
Unit quaternion to represent rotation: (w, x, y, z)
i : int from [0, 1, 2]
The first rotation axis (0: x, 1: y, 2: z)
j : int from [0, 1, 2]
The second rotation axis (0: x, 1: y, 2: z)
k : int from [0, 1, 2]
The third rotation axis (0: x, 1: y, 2: z)
extrinsic : bool
Do we use extrinsic transformations? Intrinsic otherwise.
Returns
-------
euler_angles : array, shape (3,)
Extracted rotation angles in radians about the axes i, j, k in this
order. The first and last angle are normalized to [-pi, pi]. The middle
angle is normalized to either [0, pi] (proper Euler angles) or
[-pi/2, pi/2] (Cardan / Tait-Bryan angles).
Raises
------
ValueError
If basis is invalid
References
----------
Bernardes, Evandro; Viollet, Stephane: Quaternion to Euler angles
conversion: A direct, general and computationally efficient method,
https://doi.org/10.1371/journal.pone.0276302
*/
float vec_q[4] = { q[0], q[1], q[2], q[3] };
bool ret = matrix_norm_vector_f(vec_q, 4);
if (!ret)
return ret;
if (i != 0 && i != 1 && i != 2)
return false;
if (j != 0 && j != 1 && j != 2)
return false;
if (k != 0 && k != 1 && k != 2)
return false;
i += 1;
j += 1;
k += 1;
// The original algorithm assumes extrinsic convention. Hence, we swap
// the order of axes for intrinsic rotation.
if (!extrinsic)
{
i = i^k;
k = i^k;
i = i^k;
}
// Proper Euler angles rotate about the same axis in the first and last
// rotation. If this is not the case, they are called Cardan or Tait-Bryan
// angles and have to be handled differently.
bool proper_euler = i == k;
if (proper_euler)
{
k = 6 - i - j;
}
int sign = ((i - j) * (j - k) * (k - i)) / 2;
float a = vec_q[0];
float b = vec_q[i];
float c = vec_q[j];
float d = vec_q[k] * sign;
if (!proper_euler)
{
a = a - c;
b = b + d;
c = c + a;
d = d - b;
}
// Equation 34 is used instead of Equation 35 as atan2 it is numerically
// more accurate than acos.
float angle_j = 2.0f * atan2(sqrt(c*c + d*d), sqrt(a*a + b*b));
// Check for singularities
int singularity = 0;
if (abs(angle_j) <= 1e-7)
singularity = 1;
else if (abs(angle_j - PI) <= 1e-7)
singularity = 2;
else
singularity = 0;
// Equation 25
// (theta_1 + theta_3) / 2
float half_sum = atan2(b, a);
// (theta_1 - theta_3) / 2
float half_diff = atan2(d, c);
float angle_i = 0.0f, angle_k = 0.0f;
if (singularity == 0)//# no singularity
{
// Equation 32
angle_i = half_sum + half_diff;
angle_k = half_sum - half_diff;
}
else if (extrinsic) //# singularity
{
angle_k = 0.0;
if (singularity == 1)
angle_i = 2.0f * half_sum;
else
{
//assert singularity == 2;
if (singularity != 2)
return false;
angle_i = 2.0f * half_diff;
}
}
else //# intrinsic, singularity
{
angle_i = 0.0;
if (singularity == 1)
angle_k = 2.0f * half_sum;
else
{
if (singularity != 2)
return false;
angle_k = -2.0f * half_diff;
}
}
if (!proper_euler)
{
// Equation 43
angle_j -= PI / 2.0f;
// Equation 44
angle_i *= sign;
}
angle_k = norm_angle(angle_k);
angle_i = norm_angle(angle_i);
if (extrinsic)
{
euler[0] = angle_k;
euler[1] = angle_j;
euler[2] = angle_i;
}
else
{
euler[0] = angle_i;
euler[1] = angle_j;
euler[2] = angle_k;
}
return true;
}
bool matrix_from_quaternion(const MATRIX_QUATERNION_F q, MATRIX_SO3_F R)
{
/*"""Compute rotation matrix from quaternion.
This typically results in an active rotation matrix.
Parameters
----------
q : array-like, shape (4,)
Unit quaternion to represent rotation: (w, x, y, z)
Returns
-------
R : array-like, shape (3, 3)
Rotation matrix
"""*/
float quat_vec[4] = { q[0], q[1], q[2], q[3] };
matrix_norm_vector_f(quat_vec, 4);
float w = quat_vec[0], x = quat_vec[1], y = quat_vec[2], z = quat_vec[3];
float x2 = 2.0f * x * x;
float y2 = 2.0f * y * y;
float z2 = 2.0f * z * z;
float xy = 2.0f * x * y;
float xz = 2.0f * x * z;
float yz = 2.0f * y * z;
float xw = 2.0f * x * w;
float yw = 2.0f * y * w;
float zw = 2.0f * z * w;
R[0] = 1.0f - y2 - z2; R[1] = xy - zw; R[2] = xz + yw;
R[3] = xy + zw; R[4] = 1.0f - x2 - z2; R[5] = yz - xw;
R[6] = xz - yw; R[7] = yz + xw; R[8] = 1.0f - x2 - y2;
return true;
}
bool quaternion_from_matrix(const MATRIX_SO3_F R, MATRIX_QUATERNION_F res, bool strict_check)
{
/*"""Compute quaternion from rotation matrix.
We usually assume active rotations.
.. warning::
When computing a quaternion from the rotation matrix there is a sign
ambiguity: q and -q represent the same rotation.
Parameters
----------
R : array-like, shape (3, 3)
Rotation matrix
strict_check : bool, optional (default: True)
Raise a ValueError if the rotation matrix is not numerically close
enough to a real rotation matrix. Otherwise we print a warning.
Returns
-------
q : array-like, shape (4,)
Unit quaternion to represent rotation: (w, x, y, z)
"""*/
bool ret = check_matrix(R, strict_check);
if (!ret)
return false;
// Source:
// http://www.euclideanspace.com/maths/geometry/rotations/conversions
float trace = R[0] + R[4] + R[8];
float sqrt_trace = 0.0f;
if (trace > 0.0f)
{
sqrt_trace = sqrt(1.0f + trace);
res[0] = 0.5f * sqrt_trace;
res[1] = 0.5f / sqrt_trace * (R[7] - R[5]);
res[2] = 0.5f / sqrt_trace * (R[2] - R[6]);
res[3] = 0.5f / sqrt_trace * (R[3] - R[1]);
}
else
{
if (R[0] > R[4] && R[0] > R[8])
{
sqrt_trace = sqrt(1.0f + R[0] - R[4] - R[8]);
res[0] = 0.5f / sqrt_trace * (R[7] - R[5]);
res[1] = 0.5f * sqrt_trace;
res[2] = 0.5f / sqrt_trace * (R[3] + R[1]);
res[3] = 0.5f / sqrt_trace * (R[2] + R[6]);
}
else if (R[4] > R[8])
{
sqrt_trace = sqrt(1.0f + R[4] - R[0] - R[8]);
res[0] = 0.5f / sqrt_trace * (R[2] - R[6]);
res[1] = 0.5f / sqrt_trace * (R[3] + R[1]);
res[2] = 0.5f * sqrt_trace;
res[3] = 0.5f / sqrt_trace * (R[7] + R[5]);
}
else
{
sqrt_trace = sqrt(1.01f + R[8] - R[0] - R[4]);
res[0] = 0.5f / sqrt_trace * (R[3] - R[1]);
res[1] = 0.5f / sqrt_trace * (R[2] + R[6]);
res[2] = 0.5f / sqrt_trace * (R[7] + R[5]);
res[3] = 0.5f * sqrt_trace;
}
}
return true;
}
bool active_matrix_from_intrinsic_euler_xzx(const MATRIX_EULER_F e, MATRIX_SO3_F mat)
{
/*"""Compute active rotation matrix from intrinsic xzx Euler angles.
Parameters
----------
e : array-like, shape (3,)
Angles for rotation around x-, z'-, and x''-axes (intrinsic rotations)
Returns
-------
R : array, shape (3, 3)
Rotation matrix
"""
*/
//return active_matrix_from_euler_base(e, 0, 2, 0, mat, false);
return matrix_from_euler(e,0,2,0,mat,false);
}
bool active_matrix_from_extrinsic_euler_xzx(const MATRIX_EULER_F e, MATRIX_SO3_F mat)
{
/*
"""Compute active rotation matrix from extrinsic xzx Euler angles.
Parameters
----------
e : array-like, shape (3,)
Angles for rotation around x-, z-, and x-axes (extrinsic rotations)
Returns
-------
R : array, shape (3, 3)
Rotation matrix
"""
*/
//return active_matrix_from_euler_base(e, 0, 2, 0, mat, true);
return matrix_from_euler(e, 0, 2, 0, mat, true);
}
bool active_matrix_from_intrinsic_euler_xyx(const MATRIX_EULER_F e, MATRIX_SO3_F mat)
{
/*"""Compute active rotation matrix from intrinsic xyx Euler angles.
Parameters
----------
e : array-like, shape (3,)
Angles for rotation around x-, y'-, and x''-axes (intrinsic rotations)
Returns
-------
R : array, shape (3, 3)
Rotation matrix
"""*/
//return active_matrix_from_euler_base(e, 0, 1, 0, mat, false);
return matrix_from_euler(e, 0, 1, 0, mat, false);
}
bool active_matrix_from_extrinsic_euler_xyx(const MATRIX_EULER_F e, MATRIX_SO3_F mat)
{
/*"""Compute active rotation matrix from extrinsic xyx Euler angles.
Parameters
----------
e : array-like, shape (3,)
Angles for rotation around x-, y-, and x-axes (extrinsic rotations)
Returns
-------
R : array, shape (3, 3)
Rotation matrix
"""*/
//return active_matrix_from_euler_base(e, 0, 1, 0, mat, true);
return matrix_from_euler(e, 0, 1, 0, mat, true);
}
bool active_matrix_from_intrinsic_euler_yxy(const MATRIX_EULER_F e, MATRIX_SO3_F mat)
{
/*"""Compute active rotation matrix from intrinsic yxy Euler angles.
Parameters
----------
e : array-like, shape (3,)
Angles for rotation around y-, x'-, and y''-axes (intrinsic rotations)
Returns
-------
R : array, shape (3, 3)
Rotation matrix
"""*/
//return active_matrix_from_euler_base(e, 1, 0, 1, mat, false);
return matrix_from_euler(e, 1, 0, 1, mat, false);
}
bool active_matrix_from_extrinsic_euler_yxy(const MATRIX_EULER_F e, MATRIX_SO3_F mat)
{
/*"""Compute active rotation matrix from extrinsic yxy Euler angles.
Parameters
----------
e : array-like, shape (3,)
Angles for rotation around y-, x-, and y-axes (extrinsic rotations)
Returns
-------
R : array, shape (3, 3)
Rotation matrix
"""*/
//return active_matrix_from_euler_base(e, 1, 0, 1, mat, true);
return matrix_from_euler(e, 1, 0, 1, mat, true);
}
bool active_matrix_from_intrinsic_euler_yzy(const MATRIX_EULER_F e, MATRIX_SO3_F mat)
{
/*"""Compute active rotation matrix from intrinsic yzy Euler angles.
Parameters
----------
e : array-like, shape (3,)
Angles for rotation around y-, z'-, and y''-axes (intrinsic rotations)
Returns
-------
R : array, shape (3, 3)
Rotation matrix
"""*/
//return active_matrix_from_euler_base(e, 1, 2, 1, mat, false);
return matrix_from_euler(e, 1, 2, 1, mat, false);
}
bool active_matrix_from_extrinsic_euler_yzy(const MATRIX_EULER_F e, MATRIX_SO3_F mat)
{
/*"""Compute active rotation matrix from extrinsic yzy Euler angles.
Parameters
----------
e : array-like, shape (3,)
Angles for rotation around y-, z-, and y-axes (extrinsic rotations)
Returns
-------
R : array, shape (3, 3)
Rotation matrix
"""*/
//return active_matrix_from_euler_base(e, 1, 2, 1, mat, true);
return matrix_from_euler(e, 1, 2, 1, mat, true);
}
bool active_matrix_from_intrinsic_euler_zyz(const MATRIX_EULER_F e, MATRIX_SO3_F mat)
{
/*"""Compute active rotation matrix from intrinsic zyz Euler angles.
Parameters
----------
e : array-like, shape (3,)
Angles for rotation around z-, y'-, and z''-axes (intrinsic rotations)
Returns
-------
R : array, shape (3, 3)
Rotation matrix
"""*/
//return active_matrix_from_euler_base(e, 2, 1, 2, mat, false);
return matrix_from_euler(e, 2, 1, 2, mat, false);
}
bool active_matrix_from_extrinsic_euler_zyz(const MATRIX_EULER_F e, MATRIX_SO3_F mat)
{
/*"""Compute active rotation matrix from extrinsic zyz Euler angles.
.. warning::
This function was not implemented correctly in versions 1.3 and 1.4
as the order of the angles was reversed, which actually corresponds
to intrinsic rotations. This has been fixed in version 1.5.
Parameters
----------
e : array-like, shape (3,)
Angles for rotation around z-, y-, and z-axes (extrinsic rotations)
Returns
-------
R : array, shape (3, 3)
Rotation matrix
"""*/
//return active_matrix_from_euler_base(e, 2, 1, 2, mat, true);
return matrix_from_euler(e, 2, 1, 2, mat, true);
}
bool active_matrix_from_intrinsic_euler_zxz(const MATRIX_EULER_F e, MATRIX_SO3_F mat)
{
/*"""Compute active rotation matrix from intrinsic zxz Euler angles.
Parameters
----------
e : array-like, shape (3,)
Angles for rotation around z-, x'-, and z''-axes (intrinsic rotations)
Returns
-------
R : array, shape (3, 3)
Rotation matrix
"""*/
//return active_matrix_from_euler_base(e, 2, 0, 2, mat, false);
return matrix_from_euler(e, 2, 0, 2, mat, false);
}
bool active_matrix_from_extrinsic_euler_zxz(const MATRIX_EULER_F e, MATRIX_SO3_F mat)
{
/*"""Compute active rotation matrix from extrinsic zxz Euler angles.
.. warning::
This function was not implemented correctly in versions 1.3 and 1.4
as the order of the angles was reversed, which actually corresponds
to intrinsic rotations. This has been fixed in version 1.5.
Parameters
----------
e : array-like, shape (3,)
Angles for rotation around z-, x-, and z-axes (extrinsic rotations)
Returns
-------
R : array, shape (3, 3)
Rotation matrix
"""*/
//return active_matrix_from_euler_base(e, 2, 0, 2, mat, true);
return matrix_from_euler(e, 2, 0, 2, mat, true);
}
bool active_matrix_from_intrinsic_euler_xzy(const MATRIX_EULER_F e, MATRIX_SO3_F mat)
{
/*"""Compute active rotation matrix from intrinsic xzy Cardan angles.
Parameters
----------
e : array-like, shape (3,)
Angles for rotation around x-, z'-, and y''-axes (intrinsic rotations)
Returns
-------
R : array, shape (3, 3)
Rotation matrix
"""*/
//return active_matrix_from_euler_base(e, 0, 2, 1, mat, false);
return matrix_from_euler(e, 0, 2, 1, mat, false);
}
bool active_matrix_from_extrinsic_euler_xzy(const MATRIX_EULER_F e, MATRIX_SO3_F mat)
{
/*"""Compute active rotation matrix from extrinsic xzy Cardan angles.
Parameters
----------
e : array-like, shape (3,)
Angles for rotation around x-, z-, and y-axes (extrinsic rotations)
Returns
-------
R : array, shape (3, 3)
Rotation matrix
"""*/
//return active_matrix_from_euler_base(e, 1, 2, 0, mat, true);
return matrix_from_euler(e, 0, 2, 1, mat, true);
}
bool active_matrix_from_intrinsic_euler_xyz(const MATRIX_EULER_F e, MATRIX_SO3_F mat)
{
/*"""Compute active rotation matrix from intrinsic xyz Cardan angles.
Parameters
----------
e : array-like, shape (3,)
Angles for rotation around x-, y'-, and z''-axes (intrinsic rotations)
Returns
-------
R : array, shape (3, 3)
Rotation matrix
"""*/
//return active_matrix_from_euler_base(e, 0, 1, 2, mat, false);
return matrix_from_euler(e, 0, 1, 2, mat, false);
}
bool active_matrix_from_extrinsic_euler_xyz(const MATRIX_EULER_F e, MATRIX_SO3_F mat)
{
/*"""Compute active rotation matrix from extrinsic xyz Cardan angles.
Parameters
----------
e : array-like, shape (3,)
Angles for rotation around x-, y-, and z-axes (extrinsic rotations)
Returns
-------
R : array, shape (3, 3)
Rotation matrix
"""*/
//return active_matrix_from_euler_base(e, 2, 1, 0, mat, true);
return matrix_from_euler(e, 0, 1, 2, mat, true);
}
bool active_matrix_from_intrinsic_euler_yxz(const MATRIX_EULER_F e, MATRIX_SO3_F mat)
{
/*"""Compute active rotation matrix from intrinsic yxz Cardan angles.
Parameters
----------
e : array-like, shape (3,)
Angles for rotation around y-, x'-, and z''-axes (intrinsic rotations)
Returns
-------
R : array, shape (3, 3)
Rotation matrix
"""*/
//return active_matrix_from_euler_base(e, 1, 0, 2, mat, false);
return matrix_from_euler(e, 1, 0, 2, mat, false);
}
bool active_matrix_from_extrinsic_euler_yxz(const MATRIX_EULER_F e, MATRIX_SO3_F mat)
{
/*"""Compute active rotation matrix from extrinsic yxz Cardan angles.
Parameters
----------
e : array-like, shape (3,)
Angles for rotation around y-, x-, and z-axes (extrinsic rotations)
Returns
-------
R : array, shape (3, 3)
Rotation matrix
"""*/
//return active_matrix_from_euler_base(e, 2, 0, 1, mat, true);
return matrix_from_euler(e, 1, 0, 2, mat, true);
}
bool active_matrix_from_intrinsic_euler_yzx(const MATRIX_EULER_F e, MATRIX_SO3_F mat)
{
/*"""Compute active rotation matrix from intrinsic yzx Cardan angles.
Parameters
----------
e : array-like, shape (3,)
Angles for rotation around y-, z'-, and x''-axes (intrinsic rotations)
Returns
-------
R : array, shape (3, 3)
Rotation matrix
"""*/
//return active_matrix_from_euler_base(e, 1, 2, 0, mat, false);
return matrix_from_euler(e, 1, 2, 0, mat, false);
}
bool active_matrix_from_extrinsic_euler_yzx(const MATRIX_EULER_F e, MATRIX_SO3_F mat)
{
/*"""Compute active rotation matrix from extrinsic yzx Cardan angles.
Parameters
----------
e : array-like, shape (3,)
Angles for rotation around y-, z-, and x-axes (extrinsic rotations)
Returns
-------
R : array, shape (3, 3)
Rotation matrix
"""*/
//return active_matrix_from_euler_base(e, 0, 2, 1, mat, true);
return matrix_from_euler(e, 1, 2, 0, mat, true);
}
bool active_matrix_from_intrinsic_euler_zyx(const MATRIX_EULER_F e, MATRIX_SO3_F mat)
{
/*"""Compute active rotation matrix from intrinsic zyx Cardan angles.
Parameters
----------
e : array-like, shape (3,)
Angles for rotation around z-, y'-, and x''-axes (intrinsic rotations)
Returns
-------
R : array, shape (3, 3)
Rotation matrix
"""*/
//return active_matrix_from_euler_base(e, 2, 1, 0, mat, false);
return matrix_from_euler(e, 2, 1, 0, mat, false);
}
bool active_matrix_from_extrinsic_euler_zyx(const MATRIX_EULER_F e, MATRIX_SO3_F mat)
{
/*"""Compute active rotation matrix from extrinsic zyx Cardan angles.
Parameters
----------
e : array-like, shape (3,)
Angles for rotation around z-, y-, and x-axes (extrinsic rotations)
Returns
-------
R : array, shape (3, 3)
Rotation matrix
"""*/
//return active_matrix_from_euler_base(e, 0, 1, 2, mat, true);
return matrix_from_euler(e, 2, 1, 0, mat, true);
}
bool active_matrix_from_intrinsic_euler_zxy(const MATRIX_EULER_F e, MATRIX_SO3_F mat)
{
/*"""Compute active rotation matrix from intrinsic zxy Cardan angles.
Parameters
----------
e : array-like, shape (3,)
Angles for rotation around z-, x'-, and y''-axes (intrinsic rotations)
Returns
-------
R : array, shape (3, 3)
Rotation matrix
"""*/
//return active_matrix_from_euler_base(e, 2, 0, 1, mat, false);
return matrix_from_euler(e, 2, 0, 1, mat, false);
}
bool active_matrix_from_extrinsic_euler_zxy(const MATRIX_EULER_F e, MATRIX_SO3_F mat)
{
/*"""Compute active rotation matrix from extrinsic zxy Cardan angles.
Parameters
----------
e : array-like, shape (3,)
Angles for rotation around z-, x-, and y-axes (extrinsic rotations)
Returns
-------
R : array, shape (3, 3)
Rotation matrix
"""*/
//return active_matrix_from_euler_base(e, 1, 0, 2, mat, true);
return matrix_from_euler(e, 2, 0, 1, mat, true);
}
bool active_matrix_from_extrinsic_roll_pitch_yaw(const VECTOR3F rpy, MATRIX_SO3_F mat)
{
/*"""Compute active rotation matrix from extrinsic roll, pitch, and yaw.
Parameters
----------
rpy : array-like, shape (3,)
Angles for rotation around x- (roll), y- (pitch), and z-axes (yaw),
extrinsic rotations
Returns
-------
R : array-like, shape (3, 3)
Rotation matrix
"""*/
MATRIX_EULER_F e_rpy = { rpy[0], rpy[1], rpy[2] };
return active_matrix_from_extrinsic_euler_xyz(e_rpy, mat);
}
bool intrinsic_euler_xzx_from_active_matrix(const MATRIX_SO3_F R, MATRIX_EULER_F euler, bool strict_check)
{
/*"""Compute intrinsic xzx Euler angles from active rotation matrix.
Parameters
----------
R : array-like, shape (3, 3)
Rotation matrix
strict_check : bool, optional (default: True)
Raise a ValueError if the rotation matrix is not numerically close
enough to a real rotation matrix. Otherwise we print a warning.
Returns
-------
e : array, shape (3,)
Angles for rotation around x-, z'-, and x''-axes (intrinsic rotations)
"""*/
return general_intrinsic_euler_from_active_matrix(R, unitxyz[0], unitxyz[2], unitxyz[0], true, euler, strict_check);
}
bool extrinsic_euler_xzx_from_active_matrix(const MATRIX_SO3_F R, MATRIX_EULER_F euler, bool strict_check)
{
/*"""Compute extrinsic xzx Euler angles from active rotation matrix.
Parameters
----------
R : array-like, shape (3, 3)
Rotation matrix
strict_check : bool, optional (default: True)
Raise a ValueError if the rotation matrix is not numerically close
enough to a real rotation matrix. Otherwise we print a warning.
Returns
-------
e : array, shape (3,)
Angles for rotation around x-, z-, and x-axes (extrinsic rotations)
"""*/
bool ret = general_intrinsic_euler_from_active_matrix(R, unitxyz[0], unitxyz[2], unitxyz[0], true, euler, strict_check);
if (!ret)
return false;
float tmp = euler[0];
euler[0] = euler[2];
euler[2] = tmp;
return true;
}
bool intrinsic_euler_xyx_from_active_matrix(const MATRIX_SO3_F R, MATRIX_EULER_F euler, bool strict_check)
{
/*"""Compute intrinsic xyx Euler angles from active rotation matrix.
Parameters
----------
R : array-like, shape (3, 3)
Rotation matrix
strict_check : bool, optional (default: True)
Raise a ValueError if the rotation matrix is not numerically close
enough to a real rotation matrix. Otherwise we print a warning.
Returns
-------
e : array, shape (3,)
Angles for rotation around x-, y'-, and x''-axes (intrinsic rotations)
"""*/
return general_intrinsic_euler_from_active_matrix(R, unitxyz[0], unitxyz[1], unitxyz[0], true, euler, strict_check);
}
bool extrinsic_euler_xyx_from_active_matrix(const MATRIX_SO3_F R, MATRIX_EULER_F euler, bool strict_check)
{
/*"""Compute extrinsic xyx Euler angles from active rotation matrix.
Parameters
----------
R : array-like, shape (3, 3)
Rotation matrix
strict_check : bool, optional (default: True)
Raise a ValueError if the rotation matrix is not numerically close
enough to a real rotation matrix. Otherwise we print a warning.
Returns
-------
e : array, shape (3,)
Angles for rotation around x-, y-, and x-axes (extrinsic rotations)
"""*/
bool ret = general_intrinsic_euler_from_active_matrix(R, unitxyz[0], unitxyz[1], unitxyz[0], true, euler, strict_check);
if (!ret)
return false;
float tmp = euler[0];
euler[0] = euler[2];
euler[2] = tmp;
return true;
}
bool intrinsic_euler_yxy_from_active_matrix(const MATRIX_SO3_F R, MATRIX_EULER_F euler, bool strict_check)
{
/*"""Compute intrinsic yxy Euler angles from active rotation matrix.
Parameters
----------
R : array-like, shape (3, 3)
Rotation matrix
strict_check : bool, optional (default: True)
Raise a ValueError if the rotation matrix is not numerically close
enough to a real rotation matrix. Otherwise we print a warning.
Returns
-------
e : array, shape (3,)
Angles for rotation around y-, x'-, and y''-axes (intrinsic rotations)
"""*/
return general_intrinsic_euler_from_active_matrix(R, unitxyz[1], unitxyz[0], unitxyz[1], true, euler, strict_check);
}
bool extrinsic_euler_yxy_from_active_matrix(const MATRIX_SO3_F R, MATRIX_EULER_F euler, bool strict_check)
{
/*"""Compute extrinsic yxy Euler angles from active rotation matrix.
Parameters
----------
R : array-like, shape (3, 3)
Rotation matrix
strict_check : bool, optional (default: True)
Raise a ValueError if the rotation matrix is not numerically close
enough to a real rotation matrix. Otherwise we print a warning.
Returns
-------
e : array, shape (3,)
Angles for rotation around y-, x-, and y-axes (extrinsic rotations)
"""*/
bool ret = general_intrinsic_euler_from_active_matrix(R, unitxyz[1], unitxyz[0], unitxyz[1], true, euler, strict_check);
if (!ret)
return false;
float tmp = euler[0];
euler[0] = euler[2];
euler[2] = tmp;
return true;
}
bool intrinsic_euler_yzy_from_active_matrix(const MATRIX_SO3_F R, MATRIX_EULER_F euler, bool strict_check)
{
/*"""Compute intrinsic yzy Euler angles from active rotation matrix.
Parameters
----------
R : array-like, shape (3, 3)
Rotation matrix
strict_check : bool, optional (default: True)
Raise a ValueError if the rotation matrix is not numerically close
enough to a real rotation matrix. Otherwise we print a warning.
Returns
-------
e : array, shape (3,)
Angles for rotation around y-, z'-, and y''-axes (intrinsic rotations)
"""*/
return general_intrinsic_euler_from_active_matrix(R, unitxyz[1], unitxyz[2], unitxyz[1], true, euler, strict_check);
}
bool extrinsic_euler_yzy_from_active_matrix(const MATRIX_SO3_F R, MATRIX_EULER_F euler, bool strict_check)
{
/*"""Compute extrinsic yzy Euler angles from active rotation matrix.
Parameters
----------
R : array-like, shape (3, 3)
Rotation matrix
strict_check : bool, optional (default: True)
Raise a ValueError if the rotation matrix is not numerically close
enough to a real rotation matrix. Otherwise we print a warning.
Returns
-------
e : array, shape (3,)
Angles for rotation around y-, z-, and y-axes (extrinsic rotations)
"""*/
bool ret = general_intrinsic_euler_from_active_matrix(R, unitxyz[1], unitxyz[2], unitxyz[1], true, euler, strict_check);
if (!ret)
return false;
float tmp = euler[0];
euler[0] = euler[2];
euler[2] = tmp;
return true;
}
bool intrinsic_euler_zyz_from_active_matrix(const MATRIX_SO3_F R, MATRIX_EULER_F euler, bool strict_check)
{
/*"""Compute intrinsic zyz Euler angles from active rotation matrix.
Parameters
----------
R : array-like, shape (3, 3)
Rotation matrix
strict_check : bool, optional (default: True)
Raise a ValueError if the rotation matrix is not numerically close
enough to a real rotation matrix. Otherwise we print a warning.
Returns
-------
e : array, shape (3,)
Angles for rotation around z-, y'-, and z''-axes (intrinsic rotations)
"""*/
return general_intrinsic_euler_from_active_matrix(R, unitxyz[2], unitxyz[1], unitxyz[2], true, euler, strict_check);
}
bool extrinsic_euler_zyz_from_active_matrix(const MATRIX_SO3_F R, MATRIX_EULER_F euler, bool strict_check)
{
/*"""Compute extrinsic zyz Euler angles from active rotation matrix.
Parameters
----------
R : array-like, shape (3, 3)
Rotation matrix
strict_check : bool, optional (default: True)
Raise a ValueError if the rotation matrix is not numerically close
enough to a real rotation matrix. Otherwise we print a warning.
Returns
-------
e : array, shape (3,)
Angles for rotation around z-, y-, and z-axes (extrinsic rotations)
"""*/
bool ret = general_intrinsic_euler_from_active_matrix(R, unitxyz[2], unitxyz[1], unitxyz[2], true, euler, strict_check);
if (!ret)
return false;
float tmp = euler[0];
euler[0] = euler[2];
euler[2] = tmp;
return true;
}
bool intrinsic_euler_zxz_from_active_matrix(const MATRIX_SO3_F R, MATRIX_EULER_F euler, bool strict_check)
{
/*"""Compute intrinsic zxz Euler angles from active rotation matrix.
Parameters
----------
R : array-like, shape (3, 3)
Rotation matrix
strict_check : bool, optional (default: True)
Raise a ValueError if the rotation matrix is not numerically close
enough to a real rotation matrix. Otherwise we print a warning.
Returns
-------
e : array, shape (3,)
Angles for rotation around z-, x'-, and z''-axes (intrinsic rotations)
"""*/
return general_intrinsic_euler_from_active_matrix(R, unitxyz[2], unitxyz[0], unitxyz[2], true, euler, strict_check);
}
bool extrinsic_euler_zxz_from_active_matrix(const MATRIX_SO3_F R, MATRIX_EULER_F euler, bool strict_check)
{
/*"""Compute extrinsic zxz Euler angles from active rotation matrix.
Parameters
----------
R : array-like, shape (3, 3)
Rotation matrix
strict_check : bool, optional (default: True)
Raise a ValueError if the rotation matrix is not numerically close
enough to a real rotation matrix. Otherwise we print a warning.
Returns
-------
e : array, shape (3,)
Angles for rotation around z-, x-, and z-axes (extrinsic rotations)
"""*/
bool ret = general_intrinsic_euler_from_active_matrix(R, unitxyz[2], unitxyz[0], unitxyz[2], true, euler, strict_check);
if (!ret)
return false;
float tmp = euler[0];
euler[0] = euler[2];
euler[2] = tmp;
return true;
}
bool intrinsic_euler_xzy_from_active_matrix(const MATRIX_SO3_F R, MATRIX_EULER_F euler, bool strict_check)
{
/*"""Compute intrinsic xzy Cardan angles from active rotation matrix.
Parameters
----------
R : array-like, shape (3, 3)
Rotation matrix
strict_check : bool, optional (default: True)
Raise a ValueError if the rotation matrix is not numerically close
enough to a real rotation matrix. Otherwise we print a warning.
Returns
-------
e : array, shape (3,)
Angles for rotation around x-, z'-, and y''-axes (intrinsic rotations)
"""*/
return general_intrinsic_euler_from_active_matrix(R, unitxyz[0], unitxyz[2], unitxyz[1], false, euler, strict_check);
}
bool extrinsic_euler_xzy_from_active_matrix(const MATRIX_SO3_F R, MATRIX_EULER_F euler, bool strict_check)
{
/*"""Compute extrinsic xzy Cardan angles from active rotation matrix.
Parameters
----------
R : array-like, shape (3, 3)
Rotation matrix
strict_check : bool, optional (default: True)
Raise a ValueError if the rotation matrix is not numerically close
enough to a real rotation matrix. Otherwise we print a warning.
Returns
-------
e : array, shape (3,)
Angles for rotation around x-, z-, and y-axes (extrinsic rotations)
"""*/
bool ret = general_intrinsic_euler_from_active_matrix(R, unitxyz[1], unitxyz[2], unitxyz[0], false, euler, strict_check);
if (!ret)
return false;
float tmp = euler[0];
euler[0] = euler[2];
euler[2] = tmp;
return true;
}
bool intrinsic_euler_xyz_from_active_matrix(const MATRIX_SO3_F R, MATRIX_EULER_F euler, bool strict_check)
{
/*"""Compute intrinsic xyz Cardan angles from active rotation matrix.
Parameters
----------
R : array-like, shape (3, 3)
Rotation matrix
strict_check : bool, optional (default: True)
Raise a ValueError if the rotation matrix is not numerically close
enough to a real rotation matrix. Otherwise we print a warning.
Returns
-------
e : array, shape (3,)
Angles for rotation around x-, y'-, and z''-axes (intrinsic rotations)
"""*/
return general_intrinsic_euler_from_active_matrix(R, unitxyz[0], unitxyz[1], unitxyz[2], false, euler, strict_check);
}
bool extrinsic_euler_xyz_from_active_matrix(const MATRIX_SO3_F R, MATRIX_EULER_F euler, bool strict_check)
{
/*"""Compute extrinsic xyz Cardan angles from active rotation matrix.
Parameters
----------
R : array-like, shape (3, 3)
Rotation matrix
strict_check : bool, optional (default: True)
Raise a ValueError if the rotation matrix is not numerically close
enough to a real rotation matrix. Otherwise we print a warning.
Returns
-------
e : array, shape (3,)
Angles for rotation around x-, y-, and z-axes (extrinsic rotations)
"""*/
bool ret = general_intrinsic_euler_from_active_matrix(R, unitxyz[2], unitxyz[1], unitxyz[0], false, euler, strict_check);
if (!ret)
return false;
float tmp = euler[0];
euler[0] = euler[2];
euler[2] = tmp;
return true;
}
bool intrinsic_euler_yxz_from_active_matrix(const MATRIX_SO3_F R, MATRIX_EULER_F euler, bool strict_check)
{
/*"""Compute intrinsic yxz Cardan angles from active rotation matrix.
Parameters
----------
R : array-like, shape (3, 3)
Rotation matrix
strict_check : bool, optional (default: True)
Raise a ValueError if the rotation matrix is not numerically close
enough to a real rotation matrix. Otherwise we print a warning.
Returns
-------
e : array, shape (3,)
Angles for rotation around y-, x'-, and z''-axes (intrinsic rotations)
"""*/
return general_intrinsic_euler_from_active_matrix(R, unitxyz[1], unitxyz[0], unitxyz[2], false, euler, strict_check);
}
bool extrinsic_euler_yxz_from_active_matrix(const MATRIX_SO3_F R, MATRIX_EULER_F euler, bool strict_check)
{
/*"""Compute extrinsic yxz Cardan angles from active rotation matrix.
Parameters
----------
R : array-like, shape (3, 3)
Rotation matrix
strict_check : bool, optional (default: True)
Raise a ValueError if the rotation matrix is not numerically close
enough to a real rotation matrix. Otherwise we print a warning.
Returns
-------
e : array, shape (3,)
Angles for rotation around y-, x-, and z-axes (extrinsic rotations)
"""*/
bool ret = general_intrinsic_euler_from_active_matrix(R, unitxyz[2], unitxyz[0], unitxyz[1], false, euler, strict_check);
if (!ret)
return false;
float tmp = euler[0];
euler[0] = euler[2];
euler[2] = tmp;
return true;
}
bool intrinsic_euler_yzx_from_active_matrix(const MATRIX_SO3_F R, MATRIX_EULER_F euler, bool strict_check)
{
/*"""Compute intrinsic yzx Cardan angles from active rotation matrix.
Parameters
----------
R : array-like, shape (3, 3)
Rotation matrix
strict_check : bool, optional (default: True)
Raise a ValueError if the rotation matrix is not numerically close
enough to a real rotation matrix. Otherwise we print a warning.
Returns
-------
e : array, shape (3,)
Angles for rotation around y-, z'-, and x''-axes (intrinsic rotations)
"""*/
return general_intrinsic_euler_from_active_matrix(R, unitxyz[1], unitxyz[2], unitxyz[0], false, euler, strict_check);
}
bool extrinsic_euler_yzx_from_active_matrix(const MATRIX_SO3_F R, MATRIX_EULER_F euler, bool strict_check)
{
/*"""Compute extrinsic yzx Cardan angles from active rotation matrix.
Parameters
----------
R : array-like, shape (3, 3)
Rotation matrix
strict_check : bool, optional (default: True)
Raise a ValueError if the rotation matrix is not numerically close
enough to a real rotation matrix. Otherwise we print a warning.
Returns
-------
e : array, shape (3,)
Angles for rotation around y-, z-, and x-axes (extrinsic rotations)
"""*/
bool ret = general_intrinsic_euler_from_active_matrix(R, unitxyz[0], unitxyz[2], unitxyz[1], false, euler, strict_check);
if (!ret)
return false;
float tmp = euler[0];
euler[0] = euler[2];
euler[2] = tmp;
return true;
}
bool intrinsic_euler_zyx_from_active_matrix(const MATRIX_SO3_F R, MATRIX_EULER_F euler, bool strict_check)
{
/*"""Compute intrinsic zyx Cardan angles from active rotation matrix.
Parameters
----------
R : array-like, shape (3, 3)
Rotation matrix
strict_check : bool, optional (default: True)
Raise a ValueError if the rotation matrix is not numerically close
enough to a real rotation matrix. Otherwise we print a warning.
Returns
-------
e : array, shape (3,)
Angles for rotation around z-, y'-, and x''-axes (intrinsic rotations)
"""*/
return general_intrinsic_euler_from_active_matrix(R, unitxyz[2], unitxyz[1], unitxyz[0], false, euler, strict_check);
}
bool extrinsic_euler_zyx_from_active_matrix(const MATRIX_SO3_F R, MATRIX_EULER_F euler, bool strict_check)
{
/*"""Compute extrinsic zyx Cardan angles from active rotation matrix.
Parameters
----------
R : array-like, shape (3, 3)
Rotation matrix
strict_check : bool, optional (default: True)
Raise a ValueError if the rotation matrix is not numerically close
enough to a real rotation matrix. Otherwise we print a warning.
Returns
-------
e : array, shape (3,)
Angles for rotation around z-, y-, and x-axes (extrinsic rotations)
"""*/
bool ret = general_intrinsic_euler_from_active_matrix(R, unitxyz[0], unitxyz[1], unitxyz[2], false, euler, strict_check);
if (!ret)
return false;
float tmp = euler[0];
euler[0] = euler[2];
euler[2] = tmp;
return true;
}
bool intrinsic_euler_zxy_from_active_matrix(const MATRIX_SO3_F R, MATRIX_EULER_F euler, bool strict_check)
{
/*"""Compute intrinsic zxy Cardan angles from active rotation matrix.
Parameters
----------
R : array-like, shape (3, 3)
Rotation matrix
strict_check : bool, optional (default: True)
Raise a ValueError if the rotation matrix is not numerically close
enough to a real rotation matrix. Otherwise we print a warning.
Returns
-------
e : array, shape (3,)
Angles for rotation around z-, x'-, and y''-axes (intrinsic rotations)
"""*/
return general_intrinsic_euler_from_active_matrix(R, unitxyz[2], unitxyz[0], unitxyz[1], false, euler, strict_check);
}
bool extrinsic_euler_zxy_from_active_matrix(const MATRIX_SO3_F R, MATRIX_EULER_F euler, bool strict_check)
{
/*"""Compute extrinsic zxy Cardan angles from active rotation matrix.
Parameters
----------
R : array-like, shape (3, 3)
Rotation matrix
strict_check : bool, optional (default: True)
Raise a ValueError if the rotation matrix is not numerically close
enough to a real rotation matrix. Otherwise we print a warning.
Returns
-------
e : array, shape (3,)
Angles for rotation around z-, x-, and y-axes (extrinsic rotations)
"""*/
bool ret = general_intrinsic_euler_from_active_matrix(R, unitxyz[1], unitxyz[0], unitxyz[2], false, euler, strict_check);
if (!ret)
return false;
float tmp = euler[0];
euler[0] = euler[2];
euler[2] = tmp;
return true;
}