mirror of
https://github.com/panda3d/panda3d.git
synced 2025-10-03 10:22:45 -04:00
*** empty log message ***
This commit is contained in:
David Rose
parent
cf178c1e18
commit
bde4891e3f
@ -40,11 +40,11 @@ PUBLISHED:
|
||||
void set(const FLOATNAME(LMatrix3) &m);
|
||||
INLINE void set(const FLOATNAME(LMatrix4) &m);
|
||||
|
||||
INLINE void extract_to_matrix(FLOATNAME(LMatrix3) &m) const;
|
||||
INLINE void extract_to_matrix(FLOATNAME(LMatrix4) &m) const;
|
||||
void extract_to_matrix(FLOATNAME(LMatrix3) &m) const;
|
||||
void extract_to_matrix(FLOATNAME(LMatrix4) &m) const;
|
||||
|
||||
INLINE void set_hpr(const FLOATNAME(LVecBase3) &hpr);
|
||||
INLINE FLOATNAME(LVecBase3) get_hpr() const;
|
||||
void set_hpr(const FLOATNAME(LVecBase3) &hpr);
|
||||
FLOATNAME(LVecBase3) get_hpr() const;
|
||||
|
||||
INLINE FLOATTYPE1 get_r(void) const;
|
||||
INLINE FLOATTYPE1 get_i(void) const;
|
||||
@ -407,209 +407,13 @@ set(const FLOATNAME(LMatrix4) &m) {
|
||||
set(m.get_upper_3());
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////
|
||||
// Function: extract (LMatrix3)
|
||||
// Access: public
|
||||
// Description: Do-While Jones paper from cary.
|
||||
////////////////////////////////////////////////////////////////////
|
||||
|
||||
INLINE void FLOATNAME(LQuaternionBase)::
|
||||
extract_to_matrix(FLOATNAME(LMatrix3) &m) const {
|
||||
FLOATTYPE1 N = (_r * _r) + (_i * _i) + (_j * _j) + (_k * _k);
|
||||
FLOATTYPE1 s = (N == 0.) ? 0. : (2. / N);
|
||||
FLOATTYPE1 xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz;
|
||||
|
||||
xs = _i * s; ys = _j * s; zs = _k * s;
|
||||
wx = _r * xs; wy = _r * ys; wz = _r * zs;
|
||||
xx = _i * xs; xy = _i * ys; xz = _i * zs;
|
||||
yy = _j * ys; yz = _j * zs; zz = _k * zs;
|
||||
|
||||
m = FLOATNAME(LMatrix3)((1. - (yy + zz)), (xy - wz), (xz + wy),
|
||||
(xy + wz), (1. - (xx + zz)), (yz - wx),
|
||||
(xz - wy), (yz + wx), (1. - (xx + yy)));
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////
|
||||
// Function: extract (LMatrix4)
|
||||
// Access: public
|
||||
// Description:
|
||||
////////////////////////////////////////////////////////////////////
|
||||
|
||||
INLINE void FLOATNAME(LQuaternionBase)::
|
||||
extract_to_matrix(FLOATNAME(LMatrix4) &m) const {
|
||||
FLOATTYPE1 N = (_r * _r) + (_i * _i) + (_j * _j) + (_k * _k);
|
||||
FLOATTYPE1 s = (N == 0.) ? 0. : (2. / N);
|
||||
FLOATTYPE1 xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz;
|
||||
|
||||
xs = _i * s; ys = _j * s; zs = _k * s;
|
||||
wx = _r * xs; wy = _r * ys; wz = _r * zs;
|
||||
xx = _i * xs; xy = _i * ys; xz = _i * zs;
|
||||
yy = _j * ys; yz = _j * zs; zz = _k * zs;
|
||||
|
||||
m = FLOATNAME(LMatrix4)((1. - (yy + zz)), (xy - wz), (xz + wy), 0.,
|
||||
(xy + wz), (1. - (xx + zz)), (yz - wx), 0.,
|
||||
(xz - wy), (yz + wx), (1. - (xx + yy)), 0.,
|
||||
0., 0., 0., 1.);
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////
|
||||
// Function: 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)
|
||||
////////////////////////////////////////////////////////////////////
|
||||
|
||||
INLINE void FLOATNAME(LQuaternionBase)::
|
||||
set_hpr(const FLOATNAME(LVecBase3) &hpr) {
|
||||
FLOATNAME(LQuaternionBase) quat_h, quat_p, quat_r;
|
||||
|
||||
FLOATNAME(LVector3) v = FLOATNAME(LVector3)::up();
|
||||
FLOATTYPE1 a = deg_2_rad(hpr[0] * 0.5);
|
||||
FLOATTYPE1 s,c;
|
||||
|
||||
csincos(a,&s,&c);
|
||||
quat_h.set(c, v[0] * s, v[1] * s, v[2] * s);
|
||||
v = FLOATNAME(LVector3)::right();
|
||||
a = deg_2_rad(hpr[1] * 0.5);
|
||||
csincos(a,&s,&c);
|
||||
s = csin(a);
|
||||
quat_p.set(c, v[0] * s, v[1] * s, v[2] * s);
|
||||
v = FLOATNAME(LVector3)::forward();
|
||||
a = deg_2_rad(hpr[2] * 0.5);
|
||||
csincos(a,&s,&c);
|
||||
quat_r.set(c, v[0] * s, v[1] * s, v[2] * s);
|
||||
|
||||
(*this) = quat_h * quat_p * quat_r;
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////
|
||||
// Function: get_hpr
|
||||
// Access: public
|
||||
// Description: Extracts the equivalent Euler angles from the unit
|
||||
// quaternion.
|
||||
////////////////////////////////////////////////////////////////////
|
||||
|
||||
INLINE FLOATNAME(LVecBase3) FLOATNAME(LQuaternionBase)::
|
||||
get_hpr() const {
|
||||
FLOATTYPE1 heading, pitch, roll;
|
||||
FLOATTYPE1 N = (_r * _r) + (_i * _i) + (_j * _j) + (_k * _k);
|
||||
FLOATTYPE1 s = (N == 0.) ? 0. : (2. / N);
|
||||
FLOATTYPE1 xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz, c1, c2, c3, c4;
|
||||
FLOATTYPE1 cr, sr, cp, sp, ch, sh;
|
||||
|
||||
xs = _i * s; ys = _j * s; zs = _k * s;
|
||||
wx = _r * xs; wy = _r * ys; wz = _r * zs;
|
||||
xx = _i * xs; xy = _i * ys; xz = _i * zs;
|
||||
yy = _j * ys; yz = _j * zs; zz = _k * zs;
|
||||
c1 = xz - wy;
|
||||
c2 = 1. - (xx + yy);
|
||||
c3 = 1. - (yy + zz);
|
||||
c4 = xy + wz;
|
||||
|
||||
if (c1 == 0.) { // (roll = 0 or 180) or (pitch = +/- 90
|
||||
if (c2 >= 0.) {
|
||||
roll = 0.;
|
||||
ch = c3;
|
||||
sh = c4;
|
||||
cp = c2;
|
||||
} else {
|
||||
roll = 180.;
|
||||
ch = -c3;
|
||||
sh = -c4;
|
||||
cp = -c2;
|
||||
}
|
||||
} else {
|
||||
// this should work all the time, but the above saves some trig operations
|
||||
roll = catan2(-c1, c2);
|
||||
csincos(roll,&sr,&cr);
|
||||
roll = rad_2_deg(roll);
|
||||
ch = (cr * c3) + (sr * (xz + wy));
|
||||
sh = (cr * c4) + (sr * (yz - wx));
|
||||
cp = (cr * c2) - (sr * c1);
|
||||
}
|
||||
sp = yz + wx;
|
||||
heading = rad_2_deg(catan2(sh, ch));
|
||||
pitch = rad_2_deg(catan2(sp, cp));
|
||||
|
||||
return FLOATNAME(LVecBase3)(heading, pitch, roll);
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////
|
||||
// Function: set
|
||||
// Access: public
|
||||
// Description: Do-While Jones.
|
||||
////////////////////////////////////////////////////////////////////
|
||||
|
||||
INLINE void FLOATNAME(LQuaternionBase)::
|
||||
set(const FLOATNAME(LMatrix3) &m) {
|
||||
FLOATTYPE1 m00 = m.get_cell(0, 0);
|
||||
FLOATTYPE1 m01 = m.get_cell(0, 1);
|
||||
FLOATTYPE1 m02 = m.get_cell(0, 2);
|
||||
FLOATTYPE1 m10 = m.get_cell(1, 0);
|
||||
FLOATTYPE1 m11 = m.get_cell(1, 1);
|
||||
FLOATTYPE1 m12 = m.get_cell(1, 2);
|
||||
FLOATTYPE1 m20 = m.get_cell(2, 0);
|
||||
FLOATTYPE1 m21 = m.get_cell(2, 1);
|
||||
FLOATTYPE1 m22 = m.get_cell(2, 2);
|
||||
|
||||
FLOATTYPE1 T = m00 + m11 + m22 + 1.;
|
||||
|
||||
if (T > 0.) {
|
||||
// the easy case
|
||||
FLOATTYPE1 S = 0.5 / csqrt(T);
|
||||
_r = 0.25 / S;
|
||||
_i = (m21 - m12) * S;
|
||||
_j = (m02 - m20) * S;
|
||||
_k = (m10 - m01) * S;
|
||||
} else {
|
||||
// figure out which column to take as root
|
||||
int c = 0;
|
||||
if (cabs(m00) > cabs(m11)) {
|
||||
if (cabs(m00) > cabs(m22))
|
||||
c = 0;
|
||||
else
|
||||
c = 2;
|
||||
} else if (cabs(m11) > cabs(m22))
|
||||
c = 1;
|
||||
else
|
||||
c = 2;
|
||||
|
||||
FLOATTYPE1 S;
|
||||
|
||||
switch (c) {
|
||||
case 0:
|
||||
S = csqrt(1. + m00 - m11 - m22) * 2.;
|
||||
_r = (m12 + m21) / S;
|
||||
_i = 0.5 / S;
|
||||
_j = (m01 + m10) / S;
|
||||
_k = (m02 + m20) / S;
|
||||
break;
|
||||
case 1:
|
||||
S = csqrt(1. + m11 - m00 - m22) * 2.;
|
||||
_r = (m02 + m20) / S;
|
||||
_i = (m01 + m10) / S;
|
||||
_j = 0.5 / S;
|
||||
_k = (m12 + m21) / S;
|
||||
break;
|
||||
case 2:
|
||||
S = csqrt(1. + m22 - m00 - m11) * 2.;
|
||||
_r = (m01 + m10) / S;
|
||||
_i = (m02 + m20) / S;
|
||||
_j = (m12 + m21) / S;
|
||||
_k = 0.5 / S;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////
|
||||
// Function: operator *(Matrix3, Quat)
|
||||
// Access: public
|
||||
// Description:
|
||||
////////////////////////////////////////////////////////////////////
|
||||
|
||||
FLOATNAME(LMatrix3) operator *(const FLOATNAME(LMatrix3) &m,
|
||||
INLINE FLOATNAME(LMatrix3) operator *(const FLOATNAME(LMatrix3) &m,
|
||||
const FLOATNAME(LQuaternionBase) &q) {
|
||||
FLOATNAME(LMatrix3) q_matrix;
|
||||
q.extract_to_matrix(q_matrix);
|
||||
@ -623,7 +427,7 @@ FLOATNAME(LMatrix3) operator *(const FLOATNAME(LMatrix3) &m,
|
||||
// Description:
|
||||
////////////////////////////////////////////////////////////////////
|
||||
|
||||
FLOATNAME(LMatrix4) operator *(const FLOATNAME(LMatrix4) &m,
|
||||
INLINE FLOATNAME(LMatrix4) operator *(const FLOATNAME(LMatrix4) &m,
|
||||
const FLOATNAME(LQuaternionBase) &q) {
|
||||
FLOATNAME(LMatrix4) q_matrix;
|
||||
q.extract_to_matrix(q_matrix);
|
||||
|
||||
@ -22,6 +22,202 @@ pure_imaginary(const FLOATNAME(LVector3) &v) {
|
||||
return FLOATNAME(LQuaternionBase)(0, v[0], v[1], v[2]);
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////
|
||||
// Function: extract (LMatrix3)
|
||||
// Access: public
|
||||
// Description: Do-While Jones paper from cary.
|
||||
////////////////////////////////////////////////////////////////////
|
||||
|
||||
void FLOATNAME(LQuaternionBase)::
|
||||
extract_to_matrix(FLOATNAME(LMatrix3) &m) const {
|
||||
FLOATTYPE1 N = (_r * _r) + (_i * _i) + (_j * _j) + (_k * _k);
|
||||
FLOATTYPE1 s = (N == 0.) ? 0. : (2. / N);
|
||||
FLOATTYPE1 xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz;
|
||||
|
||||
xs = _i * s; ys = _j * s; zs = _k * s;
|
||||
wx = _r * xs; wy = _r * ys; wz = _r * zs;
|
||||
xx = _i * xs; xy = _i * ys; xz = _i * zs;
|
||||
yy = _j * ys; yz = _j * zs; zz = _k * zs;
|
||||
|
||||
m = FLOATNAME(LMatrix3)((1. - (yy + zz)), (xy - wz), (xz + wy),
|
||||
(xy + wz), (1. - (xx + zz)), (yz - wx),
|
||||
(xz - wy), (yz + wx), (1. - (xx + yy)));
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////
|
||||
// Function: extract (LMatrix4)
|
||||
// Access: public
|
||||
// Description:
|
||||
////////////////////////////////////////////////////////////////////
|
||||
|
||||
void FLOATNAME(LQuaternionBase)::
|
||||
extract_to_matrix(FLOATNAME(LMatrix4) &m) const {
|
||||
FLOATTYPE1 N = (_r * _r) + (_i * _i) + (_j * _j) + (_k * _k);
|
||||
FLOATTYPE1 s = (N == 0.) ? 0. : (2. / N);
|
||||
FLOATTYPE1 xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz;
|
||||
|
||||
xs = _i * s; ys = _j * s; zs = _k * s;
|
||||
wx = _r * xs; wy = _r * ys; wz = _r * zs;
|
||||
xx = _i * xs; xy = _i * ys; xz = _i * zs;
|
||||
yy = _j * ys; yz = _j * zs; zz = _k * zs;
|
||||
|
||||
m = FLOATNAME(LMatrix4)((1. - (yy + zz)), (xy - wz), (xz + wy), 0.,
|
||||
(xy + wz), (1. - (xx + zz)), (yz - wx), 0.,
|
||||
(xz - wy), (yz + wx), (1. - (xx + yy)), 0.,
|
||||
0., 0., 0., 1.);
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////
|
||||
// Function: 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(LQuaternionBase)::
|
||||
set_hpr(const FLOATNAME(LVecBase3) &hpr) {
|
||||
FLOATNAME(LQuaternionBase) quat_h, quat_p, quat_r;
|
||||
|
||||
FLOATNAME(LVector3) v = FLOATNAME(LVector3)::up();
|
||||
FLOATTYPE1 a = deg_2_rad(hpr[0] * 0.5);
|
||||
FLOATTYPE1 s,c;
|
||||
|
||||
csincos(a,&s,&c);
|
||||
quat_h.set(c, v[0] * s, v[1] * s, v[2] * s);
|
||||
v = FLOATNAME(LVector3)::right();
|
||||
a = deg_2_rad(hpr[1] * 0.5);
|
||||
csincos(a,&s,&c);
|
||||
s = csin(a);
|
||||
quat_p.set(c, v[0] * s, v[1] * s, v[2] * s);
|
||||
v = FLOATNAME(LVector3)::forward();
|
||||
a = deg_2_rad(hpr[2] * 0.5);
|
||||
csincos(a,&s,&c);
|
||||
quat_r.set(c, v[0] * s, v[1] * s, v[2] * s);
|
||||
|
||||
(*this) = quat_h * quat_p * quat_r;
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////
|
||||
// Function: get_hpr
|
||||
// Access: public
|
||||
// Description: Extracts the equivalent Euler angles from the unit
|
||||
// quaternion.
|
||||
////////////////////////////////////////////////////////////////////
|
||||
|
||||
FLOATNAME(LVecBase3) FLOATNAME(LQuaternionBase)::
|
||||
get_hpr() const {
|
||||
FLOATTYPE1 heading, pitch, roll;
|
||||
FLOATTYPE1 N = (_r * _r) + (_i * _i) + (_j * _j) + (_k * _k);
|
||||
FLOATTYPE1 s = (N == 0.) ? 0. : (2. / N);
|
||||
FLOATTYPE1 xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz, c1, c2, c3, c4;
|
||||
FLOATTYPE1 cr, sr, cp, sp, ch, sh;
|
||||
|
||||
xs = _i * s; ys = _j * s; zs = _k * s;
|
||||
wx = _r * xs; wy = _r * ys; wz = _r * zs;
|
||||
xx = _i * xs; xy = _i * ys; xz = _i * zs;
|
||||
yy = _j * ys; yz = _j * zs; zz = _k * zs;
|
||||
c1 = xz - wy;
|
||||
c2 = 1. - (xx + yy);
|
||||
c3 = 1. - (yy + zz);
|
||||
c4 = xy + wz;
|
||||
|
||||
if (c1 == 0.) { // (roll = 0 or 180) or (pitch = +/- 90
|
||||
if (c2 >= 0.) {
|
||||
roll = 0.;
|
||||
ch = c3;
|
||||
sh = c4;
|
||||
cp = c2;
|
||||
} else {
|
||||
roll = 180.;
|
||||
ch = -c3;
|
||||
sh = -c4;
|
||||
cp = -c2;
|
||||
}
|
||||
} else {
|
||||
// this should work all the time, but the above saves some trig operations
|
||||
roll = catan2(-c1, c2);
|
||||
csincos(roll,&sr,&cr);
|
||||
roll = rad_2_deg(roll);
|
||||
ch = (cr * c3) + (sr * (xz + wy));
|
||||
sh = (cr * c4) + (sr * (yz - wx));
|
||||
cp = (cr * c2) - (sr * c1);
|
||||
}
|
||||
sp = yz + wx;
|
||||
heading = rad_2_deg(catan2(sh, ch));
|
||||
pitch = rad_2_deg(catan2(sp, cp));
|
||||
|
||||
return FLOATNAME(LVecBase3)(heading, pitch, roll);
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////
|
||||
// Function: set
|
||||
// Access: public
|
||||
// Description: Do-While Jones.
|
||||
////////////////////////////////////////////////////////////////////
|
||||
|
||||
void FLOATNAME(LQuaternionBase)::
|
||||
set(const FLOATNAME(LMatrix3) &m) {
|
||||
FLOATTYPE1 m00 = m.get_cell(0, 0);
|
||||
FLOATTYPE1 m01 = m.get_cell(0, 1);
|
||||
FLOATTYPE1 m02 = m.get_cell(0, 2);
|
||||
FLOATTYPE1 m10 = m.get_cell(1, 0);
|
||||
FLOATTYPE1 m11 = m.get_cell(1, 1);
|
||||
FLOATTYPE1 m12 = m.get_cell(1, 2);
|
||||
FLOATTYPE1 m20 = m.get_cell(2, 0);
|
||||
FLOATTYPE1 m21 = m.get_cell(2, 1);
|
||||
FLOATTYPE1 m22 = m.get_cell(2, 2);
|
||||
|
||||
FLOATTYPE1 T = m00 + m11 + m22 + 1.;
|
||||
|
||||
if (T > 0.) {
|
||||
// the easy case
|
||||
FLOATTYPE1 S = 0.5 / csqrt(T);
|
||||
_r = 0.25 / S;
|
||||
_i = (m21 - m12) * S;
|
||||
_j = (m02 - m20) * S;
|
||||
_k = (m10 - m01) * S;
|
||||
} else {
|
||||
// figure out which column to take as root
|
||||
int c = 0;
|
||||
if (cabs(m00) > cabs(m11)) {
|
||||
if (cabs(m00) > cabs(m22))
|
||||
c = 0;
|
||||
else
|
||||
c = 2;
|
||||
} else if (cabs(m11) > cabs(m22))
|
||||
c = 1;
|
||||
else
|
||||
c = 2;
|
||||
|
||||
FLOATTYPE1 S;
|
||||
|
||||
switch (c) {
|
||||
case 0:
|
||||
S = csqrt(1. + m00 - m11 - m22) * 2.;
|
||||
_r = (m12 + m21) / S;
|
||||
_i = 0.5 / S;
|
||||
_j = (m01 + m10) / S;
|
||||
_k = (m02 + m20) / S;
|
||||
break;
|
||||
case 1:
|
||||
S = csqrt(1. + m11 - m00 - m22) * 2.;
|
||||
_r = (m02 + m20) / S;
|
||||
_i = (m01 + m10) / S;
|
||||
_j = 0.5 / S;
|
||||
_k = (m12 + m21) / S;
|
||||
break;
|
||||
case 2:
|
||||
S = csqrt(1. + m22 - m00 - m11) * 2.;
|
||||
_r = (m01 + m10) / S;
|
||||
_i = (m02 + m20) / S;
|
||||
_j = (m12 + m21) / S;
|
||||
_k = 0.5 / S;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////
|
||||
// Function: FLOATNAME(LQuaternionBase)::ident_quat
|
||||
// Access: public
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user