mirror of
https://github.com/panda3d/panda3d.git
synced 2025-10-09 21:33:58 -04:00
709 lines
20 KiB
C++
709 lines
20 KiB
C++
// Filename: compose_matrix.cxx
|
|
// Created by: drose (27Jan99)
|
|
//
|
|
////////////////////////////////////////////////////////////////////
|
|
|
|
#include "compose_matrix.h"
|
|
#include "deg_2_rad.h"
|
|
#include "config_linmath.h"
|
|
|
|
#include <math.h>
|
|
|
|
////////////////////////////////////////////////////////////////////
|
|
// Function: compose_matrix
|
|
// Description: Computes the 3x3 matrix from scale and rotation.
|
|
////////////////////////////////////////////////////////////////////
|
|
template<class NumType>
|
|
INLINE void
|
|
_compose_matrix(LMatrix3<NumType> &mat,
|
|
const LVecBase3<NumType> &scale,
|
|
const LVecBase3<NumType> &hpr,
|
|
CoordinateSystem cs) {
|
|
mat =
|
|
LMatrix3<NumType>::scale_mat(scale) *
|
|
LMatrix3<NumType>::rotate_mat(hpr[1], LVector3<NumType>::right(cs), cs) *
|
|
LMatrix3<NumType>::rotate_mat(hpr[0], LVector3<NumType>::up(cs), cs) *
|
|
LMatrix3<NumType>::rotate_mat(hpr[2], LVector3<NumType>::back(cs), cs);
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////
|
|
// Function: compose_matrix
|
|
// Description: Computes the 4x4 matrix according to scale, rotation,
|
|
// and translation.
|
|
////////////////////////////////////////////////////////////////////
|
|
template<class NumType>
|
|
INLINE void
|
|
_compose_matrix(LMatrix4<NumType> &mat,
|
|
const LVecBase3<NumType> &scale,
|
|
const LVecBase3<NumType> &hpr,
|
|
const LVecBase3<NumType> &translate,
|
|
CoordinateSystem cs) {
|
|
LMatrix3<NumType> upper3;
|
|
_compose_matrix(upper3, scale, hpr, cs);
|
|
mat = LMatrix4<NumType>(upper3, translate);
|
|
}
|
|
|
|
|
|
|
|
////////////////////////////////////////////////////////////////////
|
|
// Function: unwind_yup_rotation
|
|
// Description: Extracts the rotation about the x, y, and z axes from
|
|
// the given hpr & scale matrix. Adjusts the matrix
|
|
// to eliminate the rotation.
|
|
//
|
|
// This function assumes the matrix is stored in a
|
|
// right-handed Y-up coordinate system.
|
|
////////////////////////////////////////////////////////////////////
|
|
template<class NumType>
|
|
static void
|
|
unwind_yup_rotation(LMatrix3<NumType> &mat, LVecBase3<NumType> &hpr) {
|
|
typedef LMatrix3<NumType> Matrix;
|
|
|
|
// Extract the axes from the matrix.
|
|
LVector3<NumType> x, y, z;
|
|
x = mat.get_row(0);
|
|
y = mat.get_row(1);
|
|
z = mat.get_row(2);
|
|
|
|
// Project X onto the XY plane.
|
|
LVector2<NumType> xy(x[0], x[1]);
|
|
xy = normalize(xy);
|
|
|
|
// Compute the rotation about the +Z (back) axis. This is roll.
|
|
NumType roll = rad_2_deg(atan2(xy[1], xy[0]));
|
|
|
|
// Unwind the roll from the axes, and continue.
|
|
Matrix rot_z;
|
|
rot_z = Matrix::rotate_mat(-roll, LVector3<NumType>(0.0, 0.0, 1.0),
|
|
CS_yup_right);
|
|
|
|
x = x * rot_z;
|
|
y = y * rot_z;
|
|
z = z * rot_z;
|
|
|
|
// Project the rotated X into the XZ plane.
|
|
LVector2<NumType> xz(x[0], x[2]);
|
|
xz = normalize(xz);
|
|
|
|
// Compute the rotation about the +Y (up) axis. This is yaw, or
|
|
// "heading".
|
|
NumType heading = rad_2_deg(-atan2(xz[1], xz[0]));
|
|
|
|
// Unwind the heading, and continue.
|
|
Matrix rot_y;
|
|
rot_y = Matrix::rotate_mat(-heading, LVector3<NumType>(0.0, 1.0, 0.0),
|
|
CS_yup_right);
|
|
|
|
x = x * rot_y;
|
|
y = y * rot_y;
|
|
z = z * rot_y;
|
|
|
|
// Project the rotated Z into the YZ plane.
|
|
LVector2<NumType> yz(z[1], z[2]);
|
|
yz = normalize(yz);
|
|
|
|
// Compute the rotation about the +X (right) axis. This is pitch.
|
|
NumType pitch = rad_2_deg(-atan2(yz[0], yz[1]));
|
|
|
|
// Unwind the pitch.
|
|
Matrix rot_x;
|
|
rot_x = Matrix::rotate_mat(-pitch, LVector3<NumType>(1.0, 0.0, 0.0),
|
|
CS_yup_right);
|
|
|
|
x = x * rot_x;
|
|
y = y * rot_x;
|
|
z = z * rot_x;
|
|
|
|
// Reset the matrix to reflect the unwinding.
|
|
mat.set_row(0, x);
|
|
mat.set_row(1, y);
|
|
mat.set_row(2, z);
|
|
|
|
// Return the three rotation components.
|
|
hpr[0] = heading;
|
|
hpr[1] = pitch;
|
|
hpr[2] = roll;
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////
|
|
// Function: unwind_yup_rotation
|
|
// Description: Extracts the rotation about the x, y, and z axes from
|
|
// the given hpr & scale matrix, given the indicated
|
|
// roll amount as a hint. Adjusts the matrix to
|
|
// eliminate the rotation.
|
|
//
|
|
// This function assumes the matrix is stored in a
|
|
// right-handed Y-up coordinate system.
|
|
////////////////////////////////////////////////////////////////////
|
|
template<class NumType>
|
|
static void
|
|
unwind_yup_rotation(LMatrix3<NumType> &mat, LVecBase3<NumType> &hpr,
|
|
NumType roll) {
|
|
typedef LMatrix3<NumType> Matrix;
|
|
|
|
// Extract the axes from the matrix.
|
|
LVector3<NumType> x, y, z;
|
|
x = mat.get_row(0);
|
|
y = mat.get_row(1);
|
|
z = mat.get_row(2);
|
|
|
|
// Unwind the roll from the axes, and continue.
|
|
Matrix rot_z;
|
|
rot_z = Matrix::rotate_mat(-roll, LVector3<NumType>(0.0, 0.0, 1.0),
|
|
CS_yup_right);
|
|
|
|
x = x * rot_z;
|
|
y = y * rot_z;
|
|
z = z * rot_z;
|
|
|
|
// Project the rotated X into the XZ plane.
|
|
LVector2<NumType> xz(x[0], x[2]);
|
|
xz = normalize(xz);
|
|
|
|
// Compute the rotation about the +Y (up) axis. This is yaw, or
|
|
// "heading".
|
|
NumType heading = rad_2_deg(-atan2(xz[1], xz[0]));
|
|
|
|
// Unwind the heading, and continue.
|
|
Matrix rot_y;
|
|
rot_y = Matrix::rotate_mat(-heading, LVector3<NumType>(0.0, 1.0, 0.0),
|
|
CS_yup_right);
|
|
|
|
x = x * rot_y;
|
|
y = y * rot_y;
|
|
z = z * rot_y;
|
|
|
|
// Project the rotated Z into the YZ plane.
|
|
LVector2<NumType> yz(z[1], z[2]);
|
|
yz = normalize(yz);
|
|
|
|
// Compute the rotation about the +X (right) axis. This is pitch.
|
|
NumType pitch = rad_2_deg(-atan2(yz[0], yz[1]));
|
|
|
|
// Unwind the pitch.
|
|
Matrix rot_x;
|
|
rot_x = Matrix::rotate_mat(-pitch, LVector3<NumType>(1.0, 0.0, 0.0),
|
|
CS_yup_right);
|
|
|
|
x = x * rot_x;
|
|
y = y * rot_x;
|
|
z = z * rot_x;
|
|
|
|
// Reset the matrix to reflect the unwinding.
|
|
mat.set_row(0, x);
|
|
mat.set_row(1, y);
|
|
mat.set_row(2, z);
|
|
|
|
// Return the three rotation components.
|
|
hpr[0] = heading;
|
|
hpr[1] = pitch;
|
|
hpr[2] = roll;
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////
|
|
// Function: unwind_zup_rotation
|
|
// Description: Extracts the rotation about the x, y, and z axes from
|
|
// the given hpr & scale matrix. Adjusts the matrix
|
|
// to eliminate the rotation.
|
|
//
|
|
// This function assumes the matrix is stored in a
|
|
// right-handed Z-up coordinate system.
|
|
////////////////////////////////////////////////////////////////////
|
|
template<class NumType>
|
|
static void
|
|
unwind_zup_rotation(LMatrix3<NumType> &mat, LVecBase3<NumType> &hpr) {
|
|
typedef LMatrix3<NumType> Matrix;
|
|
|
|
// Extract the axes from the matrix.
|
|
LVector3<NumType> x, y, z;
|
|
x = mat.get_row(0);
|
|
y = mat.get_row(1);
|
|
z = mat.get_row(2);
|
|
|
|
|
|
// Project X into the XZ plane.
|
|
LVector2<NumType> xz(x[0], x[2]);
|
|
xz = normalize(xz);
|
|
|
|
// Compute the rotation about the -Y (back) axis. This is roll.
|
|
NumType roll = rad_2_deg(atan2(xz[1], xz[0]));
|
|
|
|
if (y[1] < 0.0) {
|
|
if (roll < 0.0) {
|
|
roll += 180.0;
|
|
} else {
|
|
roll -= 180.0;
|
|
}
|
|
}
|
|
|
|
// Unwind the roll from the axes, and continue.
|
|
Matrix rot_y;
|
|
rot_y = Matrix::rotate_mat(roll, LVector3<NumType>(0.0, 1.0, 0.0),
|
|
CS_zup_right);
|
|
|
|
x = x * rot_y;
|
|
y = y * rot_y;
|
|
z = z * rot_y;
|
|
|
|
// Project the rotated X into the XY plane.
|
|
LVector2<NumType> xy(x[0], x[1]);
|
|
xy = normalize(xy);
|
|
|
|
// Compute the rotation about the +Z (up) axis. This is yaw, or
|
|
// "heading".
|
|
NumType heading = rad_2_deg(atan2(xy[1], xy[0]));
|
|
|
|
// Unwind the heading, and continue.
|
|
Matrix rot_z;
|
|
rot_z = Matrix::rotate_mat(-heading, LVector3<NumType>(0.0, 0.0, 1.0),
|
|
CS_zup_right);
|
|
|
|
x = x * rot_z;
|
|
y = y * rot_z;
|
|
z = z * rot_z;
|
|
|
|
// Project the rotated Y into the YZ plane.
|
|
LVector2<NumType> yz(y[1], y[2]);
|
|
yz = normalize(yz);
|
|
|
|
// Compute the rotation about the +X (right) axis. This is pitch.
|
|
NumType pitch = rad_2_deg(atan2(yz[1], yz[0]));
|
|
|
|
// Unwind the pitch.
|
|
Matrix rot_x;
|
|
rot_x = Matrix::rotate_mat(-pitch, LVector3<NumType>(1.0, 0.0, 0.0),
|
|
CS_zup_right);
|
|
|
|
x = x * rot_x;
|
|
y = y * rot_x;
|
|
z = z * rot_x;
|
|
|
|
// Reset the matrix to reflect the unwinding.
|
|
mat.set_row(0, x);
|
|
mat.set_row(1, y);
|
|
mat.set_row(2, z);
|
|
|
|
// Return the three rotation components.
|
|
hpr[0] = heading;
|
|
hpr[1] = pitch;
|
|
hpr[2] = roll;
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////
|
|
// Function: unwind_zup_rotation
|
|
// Description: Extracts the rotation about the x, y, and z axes from
|
|
// the given hpr & scale matrix, given the indicated
|
|
// roll amount as a hint. Adjusts the matrix to
|
|
// eliminate the rotation.
|
|
//
|
|
// This function assumes the matrix is stored in a
|
|
// right-handed Z-up coordinate system.
|
|
////////////////////////////////////////////////////////////////////
|
|
template<class NumType>
|
|
static void
|
|
unwind_zup_rotation(LMatrix3<NumType> &mat, LVecBase3<NumType> &hpr,
|
|
NumType roll) {
|
|
typedef LMatrix3<NumType> Matrix;
|
|
|
|
// Extract the axes from the matrix.
|
|
LVector3<NumType> x, y, z;
|
|
x = mat.get_row(0);
|
|
y = mat.get_row(1);
|
|
z = mat.get_row(2);
|
|
|
|
// Unwind the roll from the axes, and continue.
|
|
Matrix rot_y;
|
|
rot_y = Matrix::rotate_mat(roll, LVector3<NumType>(0.0, 1.0, 0.0),
|
|
CS_zup_right);
|
|
|
|
x = x * rot_y;
|
|
y = y * rot_y;
|
|
z = z * rot_y;
|
|
|
|
// Project the rotated X into the XY plane.
|
|
LVector2<NumType> xy(x[0], x[1]);
|
|
xy = normalize(xy);
|
|
|
|
// Compute the rotation about the +Z (up) axis. This is yaw, or
|
|
// "heading".
|
|
NumType heading = rad_2_deg(atan2(xy[1], xy[0]));
|
|
|
|
// Unwind the heading, and continue.
|
|
Matrix rot_z;
|
|
rot_z = Matrix::rotate_mat(-heading, LVector3<NumType>(0.0, 0.0, 1.0),
|
|
CS_zup_right);
|
|
|
|
x = x * rot_z;
|
|
y = y * rot_z;
|
|
z = z * rot_z;
|
|
|
|
// Project the rotated Y into the YZ plane.
|
|
LVector2<NumType> yz(y[1], y[2]);
|
|
yz = normalize(yz);
|
|
|
|
// Compute the rotation about the +X (right) axis. This is pitch.
|
|
NumType pitch = rad_2_deg(atan2(yz[1], yz[0]));
|
|
|
|
// Unwind the pitch.
|
|
Matrix rot_x;
|
|
rot_x = Matrix::rotate_mat(-pitch, LVector3<NumType>(1.0, 0.0, 0.0),
|
|
CS_zup_right);
|
|
|
|
x = x * rot_x;
|
|
y = y * rot_x;
|
|
z = z * rot_x;
|
|
|
|
// Reset the matrix to reflect the unwinding.
|
|
mat.set_row(0, x);
|
|
mat.set_row(1, y);
|
|
mat.set_row(2, z);
|
|
|
|
// Return the three rotation components.
|
|
hpr[0] = heading;
|
|
hpr[1] = pitch;
|
|
hpr[2] = roll;
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////
|
|
// Function: decompose_matrix
|
|
// Description: Extracts out the components of a 3x3 rotation matrix.
|
|
// Returns true if the scale and hpr completely describe
|
|
// the matrix, or false if there is also a shear
|
|
// component or if the matrix is not affine.
|
|
////////////////////////////////////////////////////////////////////
|
|
template<class NumType>
|
|
static bool
|
|
_decompose_matrix(const LMatrix3<NumType> &mat,
|
|
LVecBase3<NumType> &scale,
|
|
LVecBase3<NumType> &hpr,
|
|
CoordinateSystem cs) {
|
|
if (cs == CS_default) {
|
|
cs = default_coordinate_system;
|
|
}
|
|
|
|
// Extract the rotation and scale, according to the coordinate
|
|
// system of choice.
|
|
bool shear;
|
|
|
|
switch (cs) {
|
|
case CS_zup_right:
|
|
{
|
|
LMatrix3<NumType> rm(mat);
|
|
unwind_zup_rotation(rm, hpr);
|
|
scale[0] = rm(0, 0);
|
|
scale[1] = rm(1, 1);
|
|
scale[2] = rm(2, 2);
|
|
shear =
|
|
(fabs(rm(0, 1)) + fabs(rm(0, 2)) +
|
|
fabs(rm(1, 0)) + fabs(rm(1, 2)) +
|
|
fabs(rm(2, 0)) + fabs(rm(2, 1))) >= 0.0001;
|
|
}
|
|
break;
|
|
|
|
case CS_yup_right:
|
|
{
|
|
LMatrix3<NumType> rm(mat);
|
|
unwind_yup_rotation(rm, hpr);
|
|
scale[0] = rm(0, 0);
|
|
scale[1] = rm(1, 1);
|
|
scale[2] = rm(2, 2);
|
|
shear =
|
|
(fabs(rm(0, 1)) + fabs(rm(0, 2)) +
|
|
fabs(rm(1, 0)) + fabs(rm(1, 2)) +
|
|
fabs(rm(2, 0)) + fabs(rm(2, 1))) >= 0.0001;
|
|
}
|
|
break;
|
|
|
|
case CS_zup_left:
|
|
{
|
|
LMatrix3<NumType> lm(mat(0, 0), mat(0, 1), -mat(0, 2),
|
|
mat(1, 0), mat(1, 1), -mat(1, 2),
|
|
-mat(2, 0), -mat(2, 1), mat(2, 2));
|
|
unwind_zup_rotation(lm, hpr);
|
|
scale[0] = -lm(0, 0);
|
|
scale[1] = -lm(1, 1);
|
|
scale[2] = lm(2, 2);
|
|
shear =
|
|
(fabs(lm(0, 1)) + fabs(lm(0, 2)) +
|
|
fabs(lm(1, 0)) + fabs(lm(1, 2)) +
|
|
fabs(lm(2, 0)) + fabs(lm(2, 1))) >= 0.0001;
|
|
}
|
|
break;
|
|
|
|
case CS_yup_left:
|
|
{
|
|
LMatrix3<NumType> lm(mat(0, 0), mat(0, 1), -mat(0, 2),
|
|
mat(1, 0), mat(1, 1), -mat(1, 2),
|
|
-mat(2, 0), -mat(2, 1), mat(2, 2));
|
|
unwind_yup_rotation(lm, hpr);
|
|
scale[0] = -lm(0, 0);
|
|
scale[1] = -lm(1, 1);
|
|
scale[2] = lm(2, 2);
|
|
shear =
|
|
(fabs(lm(0, 1)) + fabs(lm(0, 2)) +
|
|
fabs(lm(1, 0)) + fabs(lm(1, 2)) +
|
|
fabs(lm(2, 0)) + fabs(lm(2, 1))) >= 0.0001;
|
|
}
|
|
break;
|
|
|
|
default:
|
|
linmath_cat.error()
|
|
<< "Unexpected coordinate system: " << (int)cs << "\n";
|
|
return false;
|
|
}
|
|
|
|
return !shear;
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////
|
|
// Function: decompose_matrix
|
|
// Description: Extracts out the components of a 3x3 rotation matrix.
|
|
// Returns true if the scale and hpr completely describe
|
|
// the matrix, or false if there is also a shear
|
|
// component or if the matrix is not affine.
|
|
//
|
|
// This flavor of the function accepts an expected roll
|
|
// amount. This amount will be used as the roll
|
|
// component, rather than attempting to determine roll
|
|
// by examining the matrix; this helps alleviate roll
|
|
// instability due to roundoff errors or gimbal lock.
|
|
////////////////////////////////////////////////////////////////////
|
|
template<class NumType>
|
|
static bool
|
|
_decompose_matrix(const LMatrix3<NumType> &mat,
|
|
LVecBase3<NumType> &scale,
|
|
LVecBase3<NumType> &hpr,
|
|
NumType roll,
|
|
CoordinateSystem cs) {
|
|
if (cs == CS_default) {
|
|
cs = default_coordinate_system;
|
|
}
|
|
|
|
// Extract the rotation and scale, according to the coordinate
|
|
// system of choice.
|
|
bool shear;
|
|
|
|
switch (cs) {
|
|
case CS_zup_right:
|
|
{
|
|
LMatrix3<NumType> rm(mat);
|
|
unwind_zup_rotation(rm, hpr, roll);
|
|
scale[0] = rm(0, 0);
|
|
scale[1] = rm(1, 1);
|
|
scale[2] = rm(2, 2);
|
|
shear =
|
|
(fabs(rm(0, 1)) + fabs(rm(0, 2)) +
|
|
fabs(rm(1, 0)) + fabs(rm(1, 2)) +
|
|
fabs(rm(2, 0)) + fabs(rm(2, 1))) >= 0.0001;
|
|
}
|
|
break;
|
|
|
|
case CS_yup_right:
|
|
{
|
|
LMatrix3<NumType> rm(mat);
|
|
unwind_yup_rotation(rm, hpr, roll);
|
|
scale[0] = rm(0, 0);
|
|
scale[1] = rm(1, 1);
|
|
scale[2] = rm(2, 2);
|
|
shear =
|
|
(fabs(rm(0, 1)) + fabs(rm(0, 2)) +
|
|
fabs(rm(1, 0)) + fabs(rm(1, 2)) +
|
|
fabs(rm(2, 0)) + fabs(rm(2, 1))) >= 0.0001;
|
|
}
|
|
break;
|
|
|
|
case CS_zup_left:
|
|
{
|
|
LMatrix3<NumType> lm(mat(0, 0), mat(0, 1), -mat(0, 2),
|
|
mat(1, 0), mat(1, 1), -mat(1, 2),
|
|
-mat(2, 0), -mat(2, 1), mat(2, 2));
|
|
unwind_zup_rotation(lm, hpr, roll);
|
|
scale[0] = -lm(0, 0);
|
|
scale[1] = -lm(1, 1);
|
|
scale[2] = lm(2, 2);
|
|
shear =
|
|
(fabs(lm(0, 1)) + fabs(lm(0, 2)) +
|
|
fabs(lm(1, 0)) + fabs(lm(1, 2)) +
|
|
fabs(lm(2, 0)) + fabs(lm(2, 1))) >= 0.0001;
|
|
}
|
|
break;
|
|
|
|
case CS_yup_left:
|
|
{
|
|
LMatrix3<NumType> lm(mat(0, 0), mat(0, 1), -mat(0, 2),
|
|
mat(1, 0), mat(1, 1), -mat(1, 2),
|
|
-mat(2, 0), -mat(2, 1), mat(2, 2));
|
|
unwind_yup_rotation(lm, hpr, roll);
|
|
scale[0] = -lm(0, 0);
|
|
scale[1] = -lm(1, 1);
|
|
scale[2] = lm(2, 2);
|
|
shear =
|
|
(fabs(lm(0, 1)) + fabs(lm(0, 2)) +
|
|
fabs(lm(1, 0)) + fabs(lm(1, 2)) +
|
|
fabs(lm(2, 0)) + fabs(lm(2, 1))) >= 0.0001;
|
|
}
|
|
break;
|
|
|
|
default:
|
|
linmath_cat.error()
|
|
<< "Unexpected coordinate system: " << (int)cs << "\n";
|
|
return false;
|
|
}
|
|
|
|
return !shear;
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////
|
|
// Function: decompose_matrix
|
|
// Description: Extracts out the components of an affine matrix.
|
|
// Returns true if the scale, hpr, translate
|
|
// completely describe the matrix, or false if there is
|
|
// also a shear component or if the matrix is not
|
|
// affine.
|
|
////////////////////////////////////////////////////////////////////
|
|
template<class NumType>
|
|
INLINE bool
|
|
_decompose_matrix(const LMatrix4<NumType> &mat,
|
|
LVecBase3<NumType> &scale,
|
|
LVecBase3<NumType> &hpr,
|
|
LVecBase3<NumType> &translate,
|
|
CoordinateSystem cs) {
|
|
// Get the translation first.
|
|
translate = mat.get_row3(3);
|
|
return _decompose_matrix(mat.get_upper_3(), scale, hpr, cs);
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////
|
|
// Function: decompose_matrix
|
|
// Description: Extracts out the components of an affine matrix.
|
|
// Returns true if the scale, hpr, translate
|
|
// completely describe the matrix, or false if there is
|
|
// also a shear component or if the matrix is not
|
|
// affine.
|
|
//
|
|
// This flavor of the function accepts an expected roll
|
|
// amount. This amount will be used as the roll
|
|
// component, rather than attempting to determine roll
|
|
// by examining the matrix; this helps alleviate roll
|
|
// instability due to roundoff errors or gimbal lock.
|
|
////////////////////////////////////////////////////////////////////
|
|
template<class NumType>
|
|
INLINE bool
|
|
_decompose_matrix(const LMatrix4<NumType> &mat,
|
|
LVecBase3<NumType> &scale,
|
|
LVecBase3<NumType> &hpr,
|
|
LVecBase3<NumType> &translate,
|
|
NumType roll,
|
|
CoordinateSystem cs) {
|
|
// Get the translation first.
|
|
translate = mat.get_row3(3);
|
|
return _decompose_matrix(mat.get_upper_3(), scale, hpr, roll, cs);
|
|
}
|
|
|
|
void
|
|
compose_matrix(LMatrix3f &mat,
|
|
const LVecBase3f &scale,
|
|
const LVecBase3f &hpr,
|
|
CoordinateSystem cs) {
|
|
_compose_matrix(mat, scale, hpr, cs);
|
|
}
|
|
|
|
bool
|
|
decompose_matrix(const LMatrix3f &mat,
|
|
LVecBase3f &scale,
|
|
LVecBase3f &hpr,
|
|
CoordinateSystem cs) {
|
|
return _decompose_matrix(mat, scale, hpr, cs);
|
|
}
|
|
|
|
bool
|
|
decompose_matrix(const LMatrix3f &mat,
|
|
LVecBase3f &scale,
|
|
LVecBase3f &hpr,
|
|
float roll,
|
|
CoordinateSystem cs) {
|
|
return _decompose_matrix(mat, scale, hpr, roll, cs);
|
|
}
|
|
|
|
void
|
|
compose_matrix(LMatrix3d &mat,
|
|
const LVecBase3d &scale,
|
|
const LVecBase3d &hpr,
|
|
CoordinateSystem cs) {
|
|
_compose_matrix(mat, scale, hpr, cs);
|
|
}
|
|
|
|
bool
|
|
decompose_matrix(const LMatrix3d &mat,
|
|
LVecBase3d &scale,
|
|
LVecBase3d &hpr,
|
|
CoordinateSystem cs) {
|
|
return _decompose_matrix(mat, scale, hpr, cs);
|
|
}
|
|
|
|
bool
|
|
decompose_matrix(const LMatrix3d &mat,
|
|
LVecBase3d &scale,
|
|
LVecBase3d &hpr,
|
|
double roll,
|
|
CoordinateSystem cs) {
|
|
return _decompose_matrix(mat, scale, hpr, roll, cs);
|
|
}
|
|
|
|
void
|
|
compose_matrix(LMatrix4f &mat,
|
|
const LVecBase3f &scale,
|
|
const LVecBase3f &hpr,
|
|
const LVecBase3f &translate,
|
|
CoordinateSystem cs) {
|
|
_compose_matrix(mat, scale, hpr, translate, cs);
|
|
}
|
|
|
|
bool
|
|
decompose_matrix(const LMatrix4f &mat,
|
|
LVecBase3f &scale,
|
|
LVecBase3f &hpr,
|
|
LVecBase3f &translate,
|
|
CoordinateSystem cs) {
|
|
return _decompose_matrix(mat, scale, hpr, translate, cs);
|
|
}
|
|
|
|
bool
|
|
decompose_matrix(const LMatrix4f &mat,
|
|
LVecBase3f &scale,
|
|
LVecBase3f &hpr,
|
|
LVecBase3f &translate,
|
|
float roll,
|
|
CoordinateSystem cs) {
|
|
return _decompose_matrix(mat, scale, hpr, translate, roll, cs);
|
|
}
|
|
|
|
void
|
|
compose_matrix(LMatrix4d &mat,
|
|
const LVecBase3d &scale,
|
|
const LVecBase3d &hpr,
|
|
const LVecBase3d &translate,
|
|
CoordinateSystem cs) {
|
|
_compose_matrix(mat, scale, hpr, translate, cs);
|
|
}
|
|
|
|
bool
|
|
decompose_matrix(const LMatrix4d &mat,
|
|
LVecBase3d &scale,
|
|
LVecBase3d &hpr,
|
|
LVecBase3d &translate,
|
|
CoordinateSystem cs) {
|
|
return _decompose_matrix(mat, scale, hpr, translate, cs);
|
|
}
|
|
|
|
bool
|
|
decompose_matrix(const LMatrix4d &mat,
|
|
LVecBase3d &scale,
|
|
LVecBase3d &hpr,
|
|
LVecBase3d &translate,
|
|
double roll,
|
|
CoordinateSystem cs) {
|
|
return _decompose_matrix(mat, scale, hpr, translate, roll, cs);
|
|
}
|
|
|