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);
|
void set(const FLOATNAME(LMatrix3) &m);
|
||||||
INLINE void set(const FLOATNAME(LMatrix4) &m);
|
INLINE void set(const FLOATNAME(LMatrix4) &m);
|
||||||
|
|
||||||
INLINE void extract_to_matrix(FLOATNAME(LMatrix3) &m) const;
|
void extract_to_matrix(FLOATNAME(LMatrix3) &m) const;
|
||||||
INLINE void extract_to_matrix(FLOATNAME(LMatrix4) &m) const;
|
void extract_to_matrix(FLOATNAME(LMatrix4) &m) const;
|
||||||
|
|
||||||
INLINE void set_hpr(const FLOATNAME(LVecBase3) &hpr);
|
void set_hpr(const FLOATNAME(LVecBase3) &hpr);
|
||||||
INLINE FLOATNAME(LVecBase3) get_hpr() const;
|
FLOATNAME(LVecBase3) get_hpr() const;
|
||||||
|
|
||||||
INLINE FLOATTYPE1 get_r(void) const;
|
INLINE FLOATTYPE1 get_r(void) const;
|
||||||
INLINE FLOATTYPE1 get_i(void) const;
|
INLINE FLOATTYPE1 get_i(void) const;
|
||||||
@ -407,209 +407,13 @@ set(const FLOATNAME(LMatrix4) &m) {
|
|||||||
set(m.get_upper_3());
|
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)
|
// Function: operator *(Matrix3, Quat)
|
||||||
// Access: public
|
// Access: public
|
||||||
// Description:
|
// Description:
|
||||||
////////////////////////////////////////////////////////////////////
|
////////////////////////////////////////////////////////////////////
|
||||||
|
|
||||||
FLOATNAME(LMatrix3) operator *(const FLOATNAME(LMatrix3) &m,
|
INLINE FLOATNAME(LMatrix3) operator *(const FLOATNAME(LMatrix3) &m,
|
||||||
const FLOATNAME(LQuaternionBase) &q) {
|
const FLOATNAME(LQuaternionBase) &q) {
|
||||||
FLOATNAME(LMatrix3) q_matrix;
|
FLOATNAME(LMatrix3) q_matrix;
|
||||||
q.extract_to_matrix(q_matrix);
|
q.extract_to_matrix(q_matrix);
|
||||||
@ -623,7 +427,7 @@ FLOATNAME(LMatrix3) operator *(const FLOATNAME(LMatrix3) &m,
|
|||||||
// Description:
|
// Description:
|
||||||
////////////////////////////////////////////////////////////////////
|
////////////////////////////////////////////////////////////////////
|
||||||
|
|
||||||
FLOATNAME(LMatrix4) operator *(const FLOATNAME(LMatrix4) &m,
|
INLINE FLOATNAME(LMatrix4) operator *(const FLOATNAME(LMatrix4) &m,
|
||||||
const FLOATNAME(LQuaternionBase) &q) {
|
const FLOATNAME(LQuaternionBase) &q) {
|
||||||
FLOATNAME(LMatrix4) q_matrix;
|
FLOATNAME(LMatrix4) q_matrix;
|
||||||
q.extract_to_matrix(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]);
|
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
|
// Function: FLOATNAME(LQuaternionBase)::ident_quat
|
||||||
// Access: public
|
// Access: public
|
||||||
|
|||||||
Loading…
x
Reference in New Issue
Block a user