RomanNum3ral
/
open_toontown_panda3d
mirror of https://github.com/open-toontown/panda3d.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
open_toontown_panda3d/panda/src/linmath/compose_matrix_src.cxx

701 lines
21 KiB
C++
Raw Blame History

// Filename: compose_matrix_src.cxx
// Created by: drose (27Jan99)
//
////////////////////////////////////////////////////////////////////
//
// PANDA 3D SOFTWARE
// Copyright (c) 2001, Disney Enterprises, Inc. All rights reserved
//
// All use of this software is subject to the terms of the Panda 3d
// Software license. You should have received a copy of this license
// along with this source code; you will also find a current copy of
// the license at http://www.panda3d.org/license.txt .
//
// To contact the maintainers of this program write to
// panda3d@yahoogroups.com .
//
////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////
// Function: compose_matrix
// Description: Computes the 3x3 matrix from scale and rotation.
////////////////////////////////////////////////////////////////////
void
compose_matrix(FLOATNAME(LMatrix3) &mat,
const FLOATNAME(LVecBase3) &scale,
const FLOATNAME(LVecBase3) &hpr,
CoordinateSystem cs) {
// temp_hpr_fix blocks use the correct way. need to keep other way as default until
// legacy tools are fixed to work with correct way
if (temp_hpr_fix) {
mat.scale_multiply(scale,
FLOATNAME(LMatrix3)::rotate_mat_normaxis(hpr[2], FLOATNAME(LVector3)::forward(cs), cs) *
FLOATNAME(LMatrix3)::rotate_mat_normaxis(hpr[1], FLOATNAME(LVector3)::right(cs), cs) *
FLOATNAME(LMatrix3)::rotate_mat_normaxis(hpr[0], FLOATNAME(LVector3)::up(cs), cs));
} else {
mat.scale_multiply(scale,
FLOATNAME(LMatrix3)::rotate_mat_normaxis(hpr[1], FLOATNAME(LVector3)::right(cs), cs) *
FLOATNAME(LMatrix3)::rotate_mat_normaxis(hpr[0], FLOATNAME(LVector3)::up(cs), cs) *
FLOATNAME(LMatrix3)::rotate_mat_normaxis(hpr[2], FLOATNAME(LVector3)::back(cs), cs));
}
}
////////////////////////////////////////////////////////////////////
// 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.
////////////////////////////////////////////////////////////////////
static void
unwind_yup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) {
typedef FLOATNAME(LMatrix3) Matrix;
if (temp_hpr_fix) {
// Extract the axes from the matrix.
FLOATNAME(LVector3) x, y, z;
mat.get_row(x,0);
mat.get_row(y,1);
mat.get_row(z,2);
// Project Z into the XZ plane.
FLOATNAME(LVector2) xz(z[0], z[2]);
xz = normalize(xz);
// Compute the rotation about the +Y (up) axis. This is yaw, or
// "heading".
FLOATTYPE heading = rad_2_deg(((FLOATTYPE)atan2(xz[0], xz[1])));
// Unwind the heading, and continue.
Matrix rot_y;
rot_y = Matrix::rotate_mat_normaxis(-heading, FLOATNAME(LVector3)(0.0f, 1.0f, 0.0f),
CS_yup_right);
x = x * rot_y;
y = y * rot_y;
z = z * rot_y;
// Project the rotated Z into the YZ plane.
FLOATNAME(LVector2) yz(z[1], z[2]);
yz = normalize(yz);
// Compute the rotation about the +X (right) axis. This is pitch.
FLOATTYPE pitch = rad_2_deg((FLOATTYPE)(-atan2(yz[0], yz[1])));
// Unwind the pitch.
Matrix rot_x;
rot_x = Matrix::rotate_mat_normaxis(-pitch, FLOATNAME(LVector3)(1.0f, 0.0f, 0.0f),
CS_yup_right);
x = x * rot_x;
y = y * rot_x;
z = z * rot_x;
// Project the rotated X onto the XY plane.
FLOATNAME(LVector2) xy(x[0], x[1]);
xy = normalize(xy);
// Compute the rotation about the +Z (back) axis. This is roll.
FLOATTYPE roll = -rad_2_deg(((FLOATTYPE)atan2(xy[1], xy[0])));
// Unwind the roll from the axes, and continue.
Matrix rot_z;
rot_z = Matrix::rotate_mat_normaxis(-roll, FLOATNAME(LVector3)(0.0f, 0.0f, 1.0f),
CS_yup_right);
x = x * rot_z;
y = y * rot_z;
z = z * rot_z;
// 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;
} else {
// Extract the axes from the matrix.
FLOATNAME(LVector3) x, y, z;
mat.get_row(x,0);
mat.get_row(y,1);
mat.get_row(z,2);
// Project X onto the XY plane.
FLOATNAME(LVector2) xy(x[0], x[1]);
xy = normalize(xy);
// Compute the rotation about the +Z (back) axis. This is roll.
FLOATTYPE roll = rad_2_deg(((FLOATTYPE)atan2(xy[1], xy[0])));
// Unwind the roll from the axes, and continue.
Matrix rot_z;
rot_z = Matrix::rotate_mat_normaxis(-roll, FLOATNAME(LVector3)(0.0f, 0.0f, 1.0f),
CS_yup_right);
x = x * rot_z;
y = y * rot_z;
z = z * rot_z;
// Project the rotated X into the XZ plane.
FLOATNAME(LVector2) xz(x[0], x[2]);
xz = normalize(xz);
// Compute the rotation about the +Y (up) axis. This is yaw, or
// "heading".
FLOATTYPE heading = rad_2_deg(((FLOATTYPE)-atan2(xz[1], xz[0])));
// Unwind the heading, and continue.
Matrix rot_y;
rot_y = Matrix::rotate_mat_normaxis(-heading, FLOATNAME(LVector3)(0.0f, 1.0f, 0.0f),
CS_yup_right);
x = x * rot_y;
y = y * rot_y;
z = z * rot_y;
// Project the rotated Z into the YZ plane.
FLOATNAME(LVector2) yz(z[1], z[2]);
yz = normalize(yz);
// Compute the rotation about the +X (right) axis. This is pitch.
FLOATTYPE pitch = rad_2_deg(((FLOATTYPE)-atan2(yz[0], yz[1])));
// Unwind the pitch.
Matrix rot_x;
rot_x = Matrix::rotate_mat_normaxis(-pitch, FLOATNAME(LVector3)(1.0f, 0.0f, 0.0f),
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.
////////////////////////////////////////////////////////////////////
static void
unwind_yup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr,
FLOATTYPE roll) {
if (temp_hpr_fix) {
unwind_yup_rotation(mat, hpr);
return;
}
typedef FLOATNAME(LMatrix3) Matrix;
// Extract the axes from the matrix.
FLOATNAME(LVector3) x, y, z;
mat.get_row(x,0);
mat.get_row(y,1);
mat.get_row(z,2);
// Unwind the roll from the axes, and continue.
Matrix rot_z;
rot_z = Matrix::rotate_mat_normaxis(-roll, FLOATNAME(LVector3)(0.0f, 0.0f, 1.0f),
CS_yup_right);
x = x * rot_z;
y = y * rot_z;
z = z * rot_z;
// Project the rotated X into the XZ plane.
FLOATNAME(LVector2) xz(x[0], x[2]);
xz = normalize(xz);
// Compute the rotation about the +Y (up) axis. This is yaw, or
// "heading".
FLOATTYPE heading = rad_2_deg(((FLOATTYPE)-atan2(xz[1], xz[0])));
// Unwind the heading, and continue.
Matrix rot_y;
rot_y = Matrix::rotate_mat_normaxis(-heading, FLOATNAME(LVector3)(0.0f, 1.0f, 0.0f),
CS_yup_right);
x = x * rot_y;
y = y * rot_y;
z = z * rot_y;
// Project the rotated Z into the YZ plane.
FLOATNAME(LVector2) yz(z[1], z[2]);
yz = normalize(yz);
// Compute the rotation about the +X (right) axis. This is pitch.
FLOATTYPE pitch = rad_2_deg(((FLOATTYPE)-atan2(yz[0], yz[1])));
// Unwind the pitch.
Matrix rot_x;
rot_x = Matrix::rotate_mat_normaxis(-pitch, FLOATNAME(LVector3)(1.0f, 0.0f, 0.0f),
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.
////////////////////////////////////////////////////////////////////
static void
unwind_zup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr) {
if (temp_hpr_fix) {
typedef FLOATNAME(LMatrix3) Matrix;
// Extract the axes from the matrix.
FLOATNAME(LVector3) x, y, z;
mat.get_row(x,0);
mat.get_row(y,1);
mat.get_row(z,2);
// Project Y into the XY plane.
FLOATNAME(LVector2) xy(y[0], y[1]);
xy = normalize(xy);
// Compute the rotation about the +Z (up) axis. This is yaw, or
// "heading".
FLOATTYPE heading = -rad_2_deg(((FLOATTYPE)atan2(xy[0], xy[1])));
// Unwind the heading, and continue.
Matrix rot_z;
rot_z = Matrix::rotate_mat_normaxis(-heading, FLOATNAME(LVector3)(0.0f, 0.0f, 1.0f),
CS_zup_right);
x = x * rot_z;
y = y * rot_z;
z = z * rot_z;
// Project the rotated Y into the YZ plane.
FLOATNAME(LVector2) yz(y[1], y[2]);
yz = normalize(yz);
// Compute the rotation about the +X (right) axis. This is pitch.
FLOATTYPE pitch = rad_2_deg(((FLOATTYPE)atan2(yz[1], yz[0])));
// Unwind the pitch.
Matrix rot_x;
rot_x = Matrix::rotate_mat_normaxis(-pitch, FLOATNAME(LVector3)(1.0f, 0.0f, 0.0f),
CS_zup_right);
x = x * rot_x;
y = y * rot_x;
z = z * rot_x;
// Project X into the XZ plane.
FLOATNAME(LVector2) xz(x[0], x[2]);
xz = normalize(xz);
// Compute the rotation about the -Y (back) axis. This is roll.
FLOATTYPE roll = -rad_2_deg(((FLOATTYPE)atan2(xz[1], xz[0])));
// Unwind the roll from the axes, and continue.
Matrix rot_y;
rot_y = Matrix::rotate_mat_normaxis(-roll, FLOATNAME(LVector3)(0.0f, 1.0f, 0.0f),
CS_zup_right);
x = x * rot_y;
y = y * rot_y;
z = z * rot_y;
// 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;
} else {
typedef FLOATNAME(LMatrix3) Matrix;
// Extract the axes from the matrix.
FLOATNAME(LVector3) x, y, z;
mat.get_row(x,0);
mat.get_row(y,1);
mat.get_row(z,2);
// Project X into the XZ plane.
FLOATNAME(LVector2) xz(x[0], x[2]);
xz = normalize(xz);
// Compute the rotation about the -Y (back) axis. This is roll.
FLOATTYPE roll = rad_2_deg(((FLOATTYPE)atan2(xz[1], xz[0])));
if (y[1] < 0.0f) {
if (roll < 0.0f) {
roll += 180.0;
} else {
roll -= 180.0;
}
}
// Unwind the roll from the axes, and continue.
Matrix rot_y;
rot_y = Matrix::rotate_mat_normaxis(roll, FLOATNAME(LVector3)(0.0f, 1.0f, 0.0f),
CS_zup_right);
x = x * rot_y;
y = y * rot_y;
z = z * rot_y;
// Project the rotated X into the XY plane.
FLOATNAME(LVector2) xy(x[0], x[1]);
xy = normalize(xy);
// Compute the rotation about the +Z (up) axis. This is yaw, or
// "heading".
FLOATTYPE heading = rad_2_deg(((FLOATTYPE)atan2(xy[1], xy[0])));
// Unwind the heading, and continue.
Matrix rot_z;
rot_z = Matrix::rotate_mat_normaxis(-heading, FLOATNAME(LVector3)(0.0f, 0.0f, 1.0f),
CS_zup_right);
x = x * rot_z;
y = y * rot_z;
z = z * rot_z;
// Project the rotated Y into the YZ plane.
FLOATNAME(LVector2) yz(y[1], y[2]);
yz = normalize(yz);
// Compute the rotation about the +X (right) axis. This is pitch.
FLOATTYPE pitch = rad_2_deg(((FLOATTYPE)atan2(yz[1], yz[0])));
// Unwind the pitch.
Matrix rot_x;
rot_x = Matrix::rotate_mat_normaxis(-pitch, FLOATNAME(LVector3)(1.0f, 0.0f, 0.0f),
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.
////////////////////////////////////////////////////////////////////
static void
unwind_zup_rotation(FLOATNAME(LMatrix3) &mat, FLOATNAME(LVecBase3) &hpr,
FLOATTYPE roll) {
if (temp_hpr_fix) {
unwind_zup_rotation(mat, hpr);
return;
}
typedef FLOATNAME(LMatrix3) Matrix;
// Extract the axes from the matrix.
FLOATNAME(LVector3) x, y, z;
mat.get_row(x,0);
mat.get_row(y,1);
mat.get_row(z,2);
// Unwind the roll from the axes, and continue.
Matrix rot_y;
rot_y = Matrix::rotate_mat_normaxis(roll, FLOATNAME(LVector3)(0.0f, 1.0f, 0.0f),
CS_zup_right);
x = x * rot_y;
y = y * rot_y;
z = z * rot_y;
// Project the rotated X into the XY plane.
FLOATNAME(LVector2) xy(x[0], x[1]);
xy = normalize(xy);
// Compute the rotation about the +Z (up) axis. This is yaw, or
// "heading".
FLOATTYPE heading = rad_2_deg(((FLOATTYPE)atan2(xy[1], xy[0])));
// Unwind the heading, and continue.
Matrix rot_z;
rot_z = Matrix::rotate_mat_normaxis(-heading, FLOATNAME(LVector3)(0.0f, 0.0f, 1.0f),
CS_zup_right);
x = x * rot_z;
y = y * rot_z;
z = z * rot_z;
// Project the rotated Y into the YZ plane.
FLOATNAME(LVector2) yz(y[1], y[2]);
yz = normalize(yz);
// Compute the rotation about the +X (right) axis. This is pitch.
FLOATTYPE pitch = rad_2_deg(((FLOATTYPE)atan2(yz[1], yz[0])));
// Unwind the pitch.
Matrix rot_x;
rot_x = Matrix::rotate_mat_normaxis(-pitch, FLOATNAME(LVector3)(1.0f, 0.0f, 0.0f),
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.
////////////////////////////////////////////////////////////////////
bool
decompose_matrix(const FLOATNAME(LMatrix3) &mat,
FLOATNAME(LVecBase3) &scale,
FLOATNAME(LVecBase3) &hpr,
CoordinateSystem cs) {
if (cs == CS_default) {
cs = default_coordinate_system;
}
// Extract the rotation and scale, according to the coordinate
// system of choice.
bool bMatHasNoShear,bIsLeftHandedMat;
FLOATNAME(LMatrix3) new_mat(mat);
switch (cs) {
case CS_zup_right:
{
unwind_zup_rotation(new_mat, hpr);
bIsLeftHandedMat = false;
}
break;
case CS_yup_right:
{
unwind_yup_rotation(new_mat, hpr);
bIsLeftHandedMat = false;
}
break;
case CS_zup_left:
{
new_mat._m.m._02 = -new_mat._m.m._02;
new_mat._m.m._12 = -new_mat._m.m._12;
new_mat._m.m._20 = -new_mat._m.m._20;
new_mat._m.m._21 = -new_mat._m.m._21;
/*
FLOATNAME(LMatrix3) 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(new_mat, hpr);
bIsLeftHandedMat = true;
}
break;
case CS_yup_left:
{
new_mat._m.m._02 = -new_mat._m.m._02;
new_mat._m.m._12 = -new_mat._m.m._12;
new_mat._m.m._20 = -new_mat._m.m._20;
new_mat._m.m._21 = -new_mat._m.m._21;
/*
FLOATNAME(LMatrix3) 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(new_mat, hpr);
bIsLeftHandedMat = true;
}
break;
default:
linmath_cat.error()
<< "Unexpected coordinate system: " << (int)cs << "\n";
return false;
}
scale[2] = new_mat._m.m._22;
if(bIsLeftHandedMat) {
scale[0] = -new_mat._m.m._00;
scale[1] = -new_mat._m.m._11;
} else {
scale[0] = new_mat._m.m._00;
scale[1] = new_mat._m.m._11;
}
bMatHasNoShear =
(fabs(new_mat(0, 1)) + fabs(new_mat(0, 2)) +
fabs(new_mat(1, 0)) + fabs(new_mat(1, 2)) +
fabs(new_mat(2, 0)) + fabs(new_mat(2, 1))) < 0.0001;
return bMatHasNoShear;
}
////////////////////////////////////////////////////////////////////
// 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.
////////////////////////////////////////////////////////////////////
bool
decompose_matrix(const FLOATNAME(LMatrix3) &mat,
FLOATNAME(LVecBase3) &scale,
FLOATNAME(LVecBase3) &hpr,
FLOATTYPE roll,
CoordinateSystem cs) {
if (cs == CS_default) {
cs = default_coordinate_system;
}
// Extract the rotation and scale, according to the coordinate
// system of choice.
bool bMatHasNoShear,bIsLeftHandedMat;
FLOATNAME(LMatrix3) new_mat(mat);
switch (cs) {
case CS_zup_right:
{
unwind_zup_rotation(new_mat, hpr, roll);
bIsLeftHandedMat = false;
}
break;
case CS_yup_right:
{
unwind_yup_rotation(new_mat, hpr, roll);
bIsLeftHandedMat = false;
}
break;
case CS_zup_left:
{
new_mat._m.m._02 = -new_mat._m.m._02;
new_mat._m.m._12 = -new_mat._m.m._12;
new_mat._m.m._20 = -new_mat._m.m._20;
new_mat._m.m._21 = -new_mat._m.m._21;
/*
FLOATNAME(LMatrix3) 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(new_mat, hpr, roll);
bIsLeftHandedMat = true;
}
break;
case CS_yup_left:
{
new_mat._m.m._02 = -new_mat._m.m._02;
new_mat._m.m._12 = -new_mat._m.m._12;
new_mat._m.m._20 = -new_mat._m.m._20;
new_mat._m.m._21 = -new_mat._m.m._21;
/*
FLOATNAME(LMatrix3) 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(new_mat, hpr, roll);
bIsLeftHandedMat = true;
}
break;
default:
linmath_cat.error()
<< "Unexpected coordinate system: " << (int)cs << "\n";
return false;
}
scale[2] = new_mat._m.m._22;
if(bIsLeftHandedMat) {
scale[0] = -new_mat._m.m._00;
scale[1] = -new_mat._m.m._11;
} else {
scale[0] = new_mat._m.m._00;
scale[1] = new_mat._m.m._11;
}
bMatHasNoShear =
(fabs(new_mat(0, 1)) + fabs(new_mat(0, 2)) +
fabs(new_mat(1, 0)) + fabs(new_mat(1, 2)) +
fabs(new_mat(2, 0)) + fabs(new_mat(2, 1))) < 0.0001;
return bMatHasNoShear;
}
Reference in New Issue View Git Blame Copy Permalink