mirror of
https://github.com/panda3d/panda3d.git
synced 2025-10-04 02:42:49 -04:00
332 lines
10 KiB
C++
332 lines
10 KiB
C++
// Filename: lquaternion_src.cxx
|
|
// Created by:
|
|
//
|
|
////////////////////////////////////////////////////////////////////
|
|
//
|
|
// PANDA 3D SOFTWARE
|
|
// Copyright (c) Carnegie Mellon University. All rights reserved.
|
|
//
|
|
// All use of this software is subject to the terms of the revised BSD
|
|
// license. You should have received a copy of this license along
|
|
// with this source code in a file named "LICENSE."
|
|
//
|
|
////////////////////////////////////////////////////////////////////
|
|
|
|
#include "config_linmath.h"
|
|
#include "lmatrix.h"
|
|
#include "luse.h"
|
|
|
|
TypeHandle FLOATNAME(LQuaternion)::_type_handle;
|
|
|
|
const FLOATNAME(LQuaternion) FLOATNAME(LQuaternion)::_ident_quat =
|
|
FLOATNAME(LQuaternion)(1.0f, 0.0f, 0.0f, 0.0f);
|
|
|
|
////////////////////////////////////////////////////////////////////
|
|
// Function: LQuaternion::pure_imaginary_quat
|
|
// Access: public
|
|
// Description:
|
|
////////////////////////////////////////////////////////////////////
|
|
FLOATNAME(LQuaternion) FLOATNAME(LQuaternion)::
|
|
pure_imaginary(const FLOATNAME(LVector3) &v) {
|
|
return FLOATNAME(LQuaternion)(0, v[0], v[1], v[2]);
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////
|
|
// Function: LQuaternion::extract_to_matrix (LMatrix3)
|
|
// Access: Public
|
|
// Description: Based on the quat lib from VRPN.
|
|
////////////////////////////////////////////////////////////////////
|
|
void FLOATNAME(LQuaternion)::
|
|
extract_to_matrix(FLOATNAME(LMatrix3) &m) const {
|
|
FLOATTYPE N = this->dot(*this);
|
|
FLOATTYPE s = (N == 0.0f) ? 0.0f : (2.0f / N);
|
|
FLOATTYPE xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz;
|
|
|
|
xs = _v(1) * s; ys = _v(2) * s; zs = _v(3) * s;
|
|
wx = _v(0) * xs; wy = _v(0) * ys; wz = _v(0) * zs;
|
|
xx = _v(1) * xs; xy = _v(1) * ys; xz = _v(1) * zs;
|
|
yy = _v(2) * ys; yz = _v(2) * zs; zz = _v(3) * zs;
|
|
|
|
m.set((1.0f - (yy + zz)), (xy + wz), (xz - wy),
|
|
(xy - wz), (1.0f - (xx + zz)), (yz + wx),
|
|
(xz + wy), (yz - wx), (1.0f - (xx + yy)));
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////
|
|
// Function: LQuaternion::extract_to_matrix (LMatrix4)
|
|
// Access: Public
|
|
// Description: Based on the quat lib from VRPN.
|
|
////////////////////////////////////////////////////////////////////
|
|
void FLOATNAME(LQuaternion)::
|
|
extract_to_matrix(FLOATNAME(LMatrix4) &m) const {
|
|
FLOATTYPE N = this->dot(*this);
|
|
FLOATTYPE s = (N == 0.0f) ? 0.0f : (2.0f / N);
|
|
FLOATTYPE xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz;
|
|
|
|
xs = _v(1) * s; ys = _v(2) * s; zs = _v(3) * s;
|
|
wx = _v(0) * xs; wy = _v(0) * ys; wz = _v(0) * zs;
|
|
xx = _v(1) * xs; xy = _v(1) * ys; xz = _v(1) * zs;
|
|
yy = _v(2) * ys; yz = _v(2) * zs; zz = _v(3) * zs;
|
|
|
|
m.set((1.0f - (yy + zz)), (xy + wz), (xz - wy), 0.0f,
|
|
(xy - wz), (1.0f - (xx + zz)), (yz + wx), 0.0f,
|
|
(xz + wy), (yz - wx), (1.0f - (xx + yy)), 0.0f,
|
|
0.0f, 0.0f, 0.0f, 1.0f);
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////
|
|
// Function: LQuaternion::set_hpr
|
|
// Access: public
|
|
// Description: Sets the quaternion as the unit quaternion that
|
|
// is equivalent to these Euler angles.
|
|
// (from Real-time Rendering, p.49)
|
|
////////////////////////////////////////////////////////////////////
|
|
void FLOATNAME(LQuaternion)::
|
|
set_hpr(const FLOATNAME(LVecBase3) &hpr, CoordinateSystem cs) {
|
|
FLOATNAME(LQuaternion) quat_h, quat_p, quat_r;
|
|
|
|
FLOATNAME(LVector3) v;
|
|
FLOATTYPE a, s, c;
|
|
|
|
v = FLOATNAME(LVector3)::up(cs);
|
|
a = deg_2_rad(hpr[0] * 0.5f);
|
|
csincos(a, &s, &c);
|
|
quat_h.set(c, v[0] * s, v[1] * s, v[2] * s);
|
|
v = FLOATNAME(LVector3)::right(cs);
|
|
a = deg_2_rad(hpr[1] * 0.5f);
|
|
csincos(a, &s, &c);
|
|
s = csin(a);
|
|
quat_p.set(c, v[0] * s, v[1] * s, v[2] * s);
|
|
v = FLOATNAME(LVector3)::forward(cs);
|
|
a = deg_2_rad(hpr[2] * 0.5f);
|
|
csincos(a, &s, &c);
|
|
quat_r.set(c, v[0] * s, v[1] * s, v[2] * s);
|
|
|
|
if (is_right_handed(cs)) {
|
|
(*this) = quat_r * quat_p * quat_h;
|
|
} else {
|
|
(*this) = invert(quat_h * quat_p * quat_r);
|
|
}
|
|
|
|
if (!temp_hpr_fix) {
|
|
// Compute the old, broken hpr.
|
|
(*this) = quat_p * quat_h * invert(quat_r);
|
|
}
|
|
|
|
#ifndef NDEBUG
|
|
if (paranoid_hpr_quat) {
|
|
FLOATNAME(LMatrix3) mat;
|
|
compose_matrix(mat, FLOATNAME(LVecBase3)(1.0f, 1.0f, 1.0f), hpr, cs);
|
|
FLOATNAME(LQuaternion) compare;
|
|
compare.set_from_matrix(mat);
|
|
if (!compare.almost_equal(*this) && !compare.almost_equal(-(*this))) {
|
|
linmath_cat.warning()
|
|
<< "hpr-to-quat of " << hpr << " computed " << *this
|
|
<< " instead of " << compare << "\n";
|
|
(*this) = compare;
|
|
}
|
|
}
|
|
#endif // NDEBUG
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////
|
|
// Function: LQuaternion::get_hpr
|
|
// Access: public
|
|
// Description: Extracts the equivalent Euler angles from the unit
|
|
// quaternion.
|
|
////////////////////////////////////////////////////////////////////
|
|
FLOATNAME(LVecBase3) FLOATNAME(LQuaternion)::
|
|
get_hpr(CoordinateSystem cs) const {
|
|
if (!temp_hpr_fix) {
|
|
// With the old, broken hpr code in place, just go through the
|
|
// existing matrix decomposition code. Not particularly speedy,
|
|
// but I don't want to bother with working out how to do it
|
|
// directly for code that hopefully won't need to last much
|
|
// longer.
|
|
FLOATNAME(LMatrix3) mat;
|
|
extract_to_matrix(mat);
|
|
FLOATNAME(LVecBase3) scale, hpr;
|
|
decompose_matrix(mat, scale, hpr, cs);
|
|
return hpr;
|
|
}
|
|
|
|
if (cs == CS_default) {
|
|
cs = get_default_coordinate_system();
|
|
}
|
|
|
|
FLOATNAME(LVecBase3) hpr;
|
|
|
|
if (cs == CS_zup_right) {
|
|
FLOATTYPE N =
|
|
(_v(0) * _v(0)) +
|
|
(_v(1) * _v(1)) +
|
|
(_v(2) * _v(2)) +
|
|
(_v(3) * _v(3));
|
|
FLOATTYPE s = (N == 0.0f) ? 0.0f : (2.0f / N);
|
|
FLOATTYPE xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz, c1, c2, c3, c4;
|
|
FLOATTYPE cr, sr, cp, sp, ch, sh;
|
|
|
|
xs = _v(1) * s; ys = _v(2) * s; zs = _v(3) * s;
|
|
wx = _v(0) * xs; wy = _v(0) * ys; wz = _v(0) * zs;
|
|
xx = _v(1) * xs; xy = _v(1) * ys; xz = _v(1) * zs;
|
|
yy = _v(2) * ys; yz = _v(2) * zs; zz = _v(3) * zs;
|
|
c1 = xz - wy;
|
|
c2 = 1.0f - (xx + yy);
|
|
c3 = 1.0f - (yy + zz);
|
|
c4 = xy + wz;
|
|
|
|
if (c1 == 0.0f) { // (roll = 0 or 180) or (pitch = +/- 90)
|
|
if (c2 >= 0.0f) {
|
|
hpr[2] = 0.0f;
|
|
ch = c3;
|
|
sh = c4;
|
|
cp = c2;
|
|
} else {
|
|
hpr[2] = 180.0f;
|
|
ch = -c3;
|
|
sh = -c4;
|
|
cp = -c2;
|
|
}
|
|
} else {
|
|
// this should work all the time, but the above saves some trig operations
|
|
FLOATTYPE roll = catan2(-c1, c2);
|
|
csincos(roll, &sr, &cr);
|
|
hpr[2] = rad_2_deg(roll);
|
|
ch = (cr * c3) + (sr * (xz + wy));
|
|
sh = (cr * c4) + (sr * (yz - wx));
|
|
cp = (cr * c2) - (sr * c1);
|
|
}
|
|
sp = yz + wx;
|
|
hpr[0] = rad_2_deg(catan2(sh, ch));
|
|
hpr[1] = rad_2_deg(catan2(sp, cp));
|
|
|
|
} else {
|
|
// The code above implements quat-to-hpr for CS_zup_right only.
|
|
// For other coordinate systems, someone is welcome to extend the
|
|
// implementation; I'm going to choose the lazy path till then.
|
|
FLOATNAME(LMatrix3) mat;
|
|
extract_to_matrix(mat);
|
|
FLOATNAME(LVecBase3) scale;
|
|
decompose_matrix(mat, scale, hpr, cs);
|
|
return hpr;
|
|
}
|
|
|
|
|
|
#ifndef NDEBUG
|
|
if (paranoid_hpr_quat) {
|
|
FLOATNAME(LMatrix3) mat;
|
|
extract_to_matrix(mat);
|
|
FLOATNAME(LVecBase3) scale, compare_hpr;
|
|
decompose_matrix(mat, scale, compare_hpr, cs);
|
|
if (!compare_hpr.almost_equal(hpr)) {
|
|
linmath_cat.warning()
|
|
<< "quat-to-hpr of " << *this << " computed " << hpr << " instead of "
|
|
<< compare_hpr << "\n";
|
|
hpr = compare_hpr;
|
|
}
|
|
}
|
|
#endif // NDEBUG
|
|
|
|
return hpr;
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////
|
|
// Function: LQuaternion::set_from_matrix
|
|
// Access: public
|
|
// Description: Sets the quaternion according to the rotation
|
|
// represented by the matrix. Originally we tried an
|
|
// algorithm presented by Do-While Jones, but that
|
|
// turned out to be broken. This is based on the quat
|
|
// lib from UNC.
|
|
////////////////////////////////////////////////////////////////////
|
|
void FLOATNAME(LQuaternion)::
|
|
set_from_matrix(const FLOATNAME(LMatrix3) &m) {
|
|
FLOATTYPE m00, m01, m02, m10, m11, m12, m20, m21, m22;
|
|
|
|
m00 = m(0, 0);
|
|
m10 = m(1, 0);
|
|
m20 = m(2, 0);
|
|
m01 = m(0, 1);
|
|
m11 = m(1, 1);
|
|
m21 = m(2, 1);
|
|
m02 = m(0, 2);
|
|
m12 = m(1, 2);
|
|
m22 = m(2, 2);
|
|
|
|
FLOATTYPE trace = m00 + m11 + m22;
|
|
|
|
if (trace > 0.0f) {
|
|
// The easy case.
|
|
FLOATTYPE S = csqrt(trace + 1.0f);
|
|
_v(0) = S * 0.5f;
|
|
S = 0.5f / S;
|
|
_v(1) = (m12 - m21) * S;
|
|
_v(2) = (m20 - m02) * S;
|
|
_v(3) = (m01 - m10) * S;
|
|
|
|
} else {
|
|
// The harder case. First, figure out which column to take as
|
|
// root. This will be the column with the largest value.
|
|
|
|
// It is tempting to try to compare the absolute values of the
|
|
// diagonal values in the code below, instead of their normal,
|
|
// signed values. Don't do it. We are actually maximizing the
|
|
// value of S, which must always be positive, and is therefore
|
|
// based on the diagonal whose actual value--not absolute
|
|
// value--is greater than those of the other two.
|
|
|
|
// We already know that m00 + m11 + m22 <= 0 (because we are here
|
|
// in the harder case).
|
|
|
|
if (m00 > m11 && m00 > m22) {
|
|
// m00 is larger than m11 and m22.
|
|
FLOATTYPE S = 1.0f + m00 - (m11 + m22);
|
|
nassertv(S > 0.0f);
|
|
S = csqrt(S);
|
|
_v(1) = S * 0.5f;
|
|
S = 0.5f / S;
|
|
_v(2) = (m01 + m10) * S;
|
|
_v(3) = (m02 + m20) * S;
|
|
_v(0) = (m12 - m21) * S;
|
|
|
|
} else if (m11 > m22) {
|
|
// m11 is larger than m00 and m22.
|
|
FLOATTYPE S = 1.0f + m11 - (m22 + m00);
|
|
nassertv(S > 0.0f);
|
|
S = csqrt(S);
|
|
_v(2) = S * 0.5f;
|
|
S = 0.5f / S;
|
|
_v(3) = (m12 + m21) * S;
|
|
_v(1) = (m10 + m01) * S;
|
|
_v(0) = (m20 - m02) * S;
|
|
|
|
} else {
|
|
// m22 is larger than m00 and m11.
|
|
FLOATTYPE S = 1.0f + m22 - (m00 + m11);
|
|
nassertv(S > 0.0f);
|
|
S = csqrt(S);
|
|
_v(3) = S * 0.5f;
|
|
S = 0.5f / S;
|
|
_v(1) = (m20 + m02) * S;
|
|
_v(2) = (m21 + m12) * S;
|
|
_v(0) = (m01 - m10) * S;
|
|
}
|
|
}
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////
|
|
// Function: LQuaternion::init_type
|
|
// Access: public
|
|
// Description:
|
|
////////////////////////////////////////////////////////////////////
|
|
void FLOATNAME(LQuaternion)::
|
|
init_type() {
|
|
if (_type_handle == TypeHandle::none()) {
|
|
FLOATNAME(LVecBase4)::init_type();
|
|
string name = "LQuaternion";
|
|
name += FLOATTOKEN;
|
|
register_type(_type_handle, name,
|
|
FLOATNAME(LVecBase4)::get_class_type());
|
|
}
|
|
}
|