561 lines
14 KiB
Text
561 lines
14 KiB
Text
/**
|
|
* 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."
|
|
*
|
|
* @file lquaternion_src.I
|
|
* @author frang
|
|
* @date 2000-06-06
|
|
*/
|
|
|
|
/**
|
|
*
|
|
*/
|
|
INLINE_LINMATH FLOATNAME(LQuaternion)::
|
|
FLOATNAME(LQuaternion)() {
|
|
}
|
|
|
|
/**
|
|
*
|
|
*/
|
|
INLINE_LINMATH FLOATNAME(LQuaternion)::
|
|
FLOATNAME(LQuaternion)(const FLOATNAME(LVecBase4) ©) :
|
|
FLOATNAME(LVecBase4)(copy)
|
|
{
|
|
}
|
|
|
|
/**
|
|
*
|
|
*/
|
|
INLINE_LINMATH FLOATNAME(LQuaternion)::
|
|
FLOATNAME(LQuaternion)(FLOATTYPE r, const FLOATNAME(LVecBase3) ©) {
|
|
set(r, copy[0], copy[1], copy[2]);
|
|
}
|
|
|
|
/**
|
|
*
|
|
*/
|
|
INLINE_LINMATH FLOATNAME(LQuaternion)::
|
|
FLOATNAME(LQuaternion)(FLOATTYPE r, FLOATTYPE i, FLOATTYPE j, FLOATTYPE k) {
|
|
set(r, i, j, k);
|
|
}
|
|
|
|
/**
|
|
* Transforms a 3-d vector by the indicated rotation
|
|
*/
|
|
INLINE_LINMATH FLOATNAME(LVecBase3) FLOATNAME(LQuaternion)::
|
|
xform(const FLOATNAME(LVecBase3) &v) const {
|
|
FLOATNAME(LQuaternion) v_quat(0.0f, v[0], v[1], v[2]);
|
|
|
|
FLOATNAME(LQuaternion) conjugate(
|
|
_v(0), -_v(1), -_v(2), -_v(3));
|
|
v_quat = conjugate * v_quat * (*this);
|
|
|
|
return FLOATNAME(LVecBase3)(v_quat[1], v_quat[2], v_quat[3]);
|
|
}
|
|
|
|
/**
|
|
* Transforms a 4-d vector by the indicated rotation
|
|
*/
|
|
INLINE_LINMATH FLOATNAME(LVecBase4) FLOATNAME(LQuaternion)::
|
|
xform(const FLOATNAME(LVecBase4) &v) const {
|
|
FLOATNAME(LQuaternion) v_quat(v[0], v[1], v[2], v[3]);
|
|
|
|
FLOATNAME(LQuaternion) conjugate(
|
|
_v(0), -_v(1), -_v(2), -_v(3));
|
|
v_quat = conjugate * v_quat * (*this);
|
|
|
|
return FLOATNAME(LVecBase4)(v_quat);
|
|
}
|
|
|
|
/**
|
|
* actual multiply call (non virtual)
|
|
*/
|
|
INLINE_LINMATH FLOATNAME(LQuaternion) FLOATNAME(LQuaternion)::
|
|
multiply(const FLOATNAME(LQuaternion) &rhs) const {
|
|
FLOATTYPE r = (rhs._v(0) * _v(0)) - (rhs._v(1) * _v(1)) - (rhs._v(2) * _v(2)) - (rhs._v(3) * _v(3));
|
|
FLOATTYPE i = (rhs._v(1) * _v(0)) + (rhs._v(0) * _v(1)) - (rhs._v(3) * _v(2)) + (rhs._v(2) * _v(3));
|
|
FLOATTYPE j = (rhs._v(2) * _v(0)) + (rhs._v(3) * _v(1)) + (rhs._v(0) * _v(2)) - (rhs._v(1) * _v(3));
|
|
FLOATTYPE k = (rhs._v(3) * _v(0)) - (rhs._v(2) * _v(1)) + (rhs._v(1) * _v(2)) + (rhs._v(0) * _v(3));
|
|
|
|
return FLOATNAME(LQuaternion)(r, i, j, k);
|
|
}
|
|
|
|
/**
|
|
*
|
|
*/
|
|
INLINE_LINMATH FLOATNAME(LQuaternion) FLOATNAME(LQuaternion)::
|
|
operator - () const {
|
|
return FLOATNAME(LVecBase4)::operator - ();
|
|
}
|
|
|
|
/**
|
|
*
|
|
*/
|
|
INLINE_LINMATH FLOATNAME(LQuaternion) FLOATNAME(LQuaternion)::
|
|
operator + (const FLOATNAME(LQuaternion) &other) const {
|
|
return FLOATNAME(LVecBase4)::operator + (other);
|
|
}
|
|
|
|
/**
|
|
*
|
|
*/
|
|
INLINE_LINMATH FLOATNAME(LQuaternion) FLOATNAME(LQuaternion)::
|
|
operator - (const FLOATNAME(LQuaternion) &other) const {
|
|
return FLOATNAME(LVecBase4)::operator - (other);
|
|
}
|
|
|
|
/**
|
|
* Returns the angle between the orientation represented by this quaternion
|
|
* and the other one, expressed in radians.
|
|
*/
|
|
INLINE_LINMATH FLOATTYPE FLOATNAME(LQuaternion)::
|
|
angle_rad(const FLOATNAME(LQuaternion) &other) const {
|
|
return get_forward().angle_rad(other.get_forward());
|
|
}
|
|
|
|
/**
|
|
* Returns the angle between the orientation represented by this quaternion
|
|
* and the other one, expressed in degrees.
|
|
*/
|
|
INLINE_LINMATH FLOATTYPE FLOATNAME(LQuaternion)::
|
|
angle_deg(const FLOATNAME(LQuaternion) &other) const {
|
|
return rad_2_deg(angle_rad(other));
|
|
}
|
|
|
|
/**
|
|
*
|
|
*/
|
|
INLINE_LINMATH FLOATNAME(LQuaternion) FLOATNAME(LQuaternion)::
|
|
operator * (FLOATTYPE scalar) const {
|
|
return FLOATNAME(LVecBase4)::operator * (scalar);
|
|
}
|
|
|
|
/**
|
|
*
|
|
*/
|
|
INLINE_LINMATH FLOATNAME(LQuaternion) FLOATNAME(LQuaternion)::
|
|
operator / (FLOATTYPE scalar) const {
|
|
return FLOATNAME(LVecBase4)::operator / (scalar);
|
|
}
|
|
|
|
/**
|
|
*
|
|
*/
|
|
INLINE_LINMATH FLOATNAME(LQuaternion) FLOATNAME(LQuaternion)::
|
|
operator *(const FLOATNAME(LQuaternion)& c) const {
|
|
return multiply(c);
|
|
}
|
|
|
|
/**
|
|
*
|
|
*/
|
|
INLINE_LINMATH FLOATNAME(LQuaternion)& FLOATNAME(LQuaternion)::
|
|
operator *=(const FLOATNAME(LQuaternion)& c) {
|
|
(*this) = operator*(c);
|
|
return *this;
|
|
}
|
|
|
|
/**
|
|
* Quat * Matrix = matrix
|
|
*/
|
|
INLINE_LINMATH FLOATNAME(LMatrix3) FLOATNAME(LQuaternion)::
|
|
operator *(const FLOATNAME(LMatrix3) &m) {
|
|
FLOATNAME(LMatrix3) result;
|
|
extract_to_matrix(result);
|
|
return result * m;
|
|
}
|
|
|
|
/**
|
|
* Quat * Matrix = matrix
|
|
*/
|
|
INLINE_LINMATH FLOATNAME(LMatrix4) FLOATNAME(LQuaternion)::
|
|
operator *(const FLOATNAME(LMatrix4) &m) {
|
|
FLOATNAME(LMatrix3) m_upper_3 = m.get_upper_3();
|
|
FLOATNAME(LMatrix3) this_quat;
|
|
extract_to_matrix(this_quat);
|
|
|
|
FLOATNAME(LMatrix4) result;
|
|
result.set_upper_3(this_quat * m_upper_3);
|
|
result.set_row(3, m.get_row(3));
|
|
result.set_col(3, m.get_col(3));
|
|
|
|
return result;
|
|
}
|
|
|
|
/**
|
|
* Returns true if two quaternions are memberwise equal within a default
|
|
* tolerance based on the numeric type.
|
|
*/
|
|
INLINE_LINMATH bool FLOATNAME(LQuaternion)::
|
|
almost_equal(const FLOATNAME(LQuaternion) &other) const {
|
|
return almost_equal(other, NEARLY_ZERO(FLOATTYPE));
|
|
}
|
|
|
|
/**
|
|
* Returns true if two quaternions are memberwise equal within a specified
|
|
* tolerance.
|
|
*/
|
|
INLINE_LINMATH bool FLOATNAME(LQuaternion)::
|
|
almost_equal(const FLOATNAME(LQuaternion) &other,
|
|
FLOATTYPE threshold) const {
|
|
return (IS_THRESHOLD_EQUAL(_v(0), other._v(0), threshold) &&
|
|
IS_THRESHOLD_EQUAL(_v(1), other._v(1), threshold) &&
|
|
IS_THRESHOLD_EQUAL(_v(2), other._v(2), threshold) &&
|
|
IS_THRESHOLD_EQUAL(_v(3), other._v(3), threshold));
|
|
}
|
|
|
|
/**
|
|
* Returns true if two quaternions represent the same rotation within a
|
|
* default tolerance based on the numeric type.
|
|
*/
|
|
INLINE_LINMATH bool FLOATNAME(LQuaternion)::
|
|
is_same_direction(const FLOATNAME(LQuaternion) &other) const {
|
|
return almost_same_direction(other, NEARLY_ZERO(FLOATTYPE));
|
|
}
|
|
|
|
/**
|
|
* Returns true if two quaternions represent the same rotation within a
|
|
* specified tolerance.
|
|
*/
|
|
INLINE_LINMATH bool FLOATNAME(LQuaternion)::
|
|
almost_same_direction(const FLOATNAME(LQuaternion) &other,
|
|
FLOATTYPE threshold) const {
|
|
return ((*this) * invert(other)).is_almost_identity(threshold);
|
|
}
|
|
|
|
/**
|
|
*
|
|
*/
|
|
INLINE_LINMATH void FLOATNAME(LQuaternion)::
|
|
output(std::ostream& os) const {
|
|
os << MAYBE_ZERO(_v(0)) << " + "
|
|
<< MAYBE_ZERO(_v(1)) << "i + "
|
|
<< MAYBE_ZERO(_v(2)) << "j + "
|
|
<< MAYBE_ZERO(_v(3)) << "k";
|
|
}
|
|
|
|
/**
|
|
*
|
|
*/
|
|
INLINE_LINMATH void FLOATNAME(LQuaternion)::
|
|
set_from_matrix(const FLOATNAME(LMatrix4) &m) {
|
|
set_from_matrix(m.get_upper_3());
|
|
}
|
|
|
|
/**
|
|
* This, along with get_angle(), returns the rotation represented by the
|
|
* quaternion as an angle about an arbitrary axis. This returns the axis; it
|
|
* is not normalized.
|
|
*/
|
|
INLINE_LINMATH FLOATNAME(LVector3) FLOATNAME(LQuaternion)::
|
|
get_axis() const {
|
|
return FLOATNAME(LVector3)(_v(1), _v(2), _v(3));
|
|
}
|
|
|
|
/**
|
|
* This, along with get_angle(), returns the rotation represented by the
|
|
* quaternion as an angle about an arbitrary axis. This returns the
|
|
* normalized axis.
|
|
*/
|
|
INLINE_LINMATH FLOATNAME(LVector3) FLOATNAME(LQuaternion)::
|
|
get_axis_normalized() const {
|
|
FLOATNAME(LVector3) axis = get_axis();
|
|
axis.normalize();
|
|
return axis;
|
|
}
|
|
|
|
/**
|
|
* This, along with get_axis(), returns the rotation represented by the
|
|
* quaternion as an angle about an arbitrary axis. This returns the angle, in
|
|
* radians counterclockwise about the axis.
|
|
*
|
|
* It is necessary to ensure the quaternion has been normalized (for instance,
|
|
* with a call to normalize()) before calling this method.
|
|
*/
|
|
INLINE_LINMATH FLOATTYPE FLOATNAME(LQuaternion)::
|
|
get_angle_rad() const {
|
|
return acos(_v(0)) * FLOATCONST(2.0);
|
|
}
|
|
|
|
/**
|
|
* This, along with get_axis(), returns the rotation represented by the
|
|
* quaternion as an angle about an arbitrary axis. This returns the angle, in
|
|
* degrees counterclockwise about the axis.
|
|
*
|
|
* It is necessary to ensure the quaternion has been normalized (for instance,
|
|
* with a call to normalize()) before calling this method.
|
|
*/
|
|
INLINE_LINMATH FLOATTYPE FLOATNAME(LQuaternion)::
|
|
get_angle() const {
|
|
return rad_2_deg(get_angle_rad());
|
|
}
|
|
|
|
/**
|
|
* angle_rad is the angle about the axis in radians. axis must be normalized.
|
|
*/
|
|
INLINE_LINMATH void FLOATNAME(LQuaternion)::
|
|
set_from_axis_angle_rad(FLOATTYPE angle_rad, const FLOATNAME(LVector3) &axis) {
|
|
nassertv(IS_THRESHOLD_EQUAL(axis.length(), 1.0f, 0.001f));
|
|
FLOATTYPE sinHalfAngle = sin(angle_rad * FLOATCONST(0.5));
|
|
_v(0) = cos(angle_rad * FLOATCONST(0.5));
|
|
_v(1) = axis[0] * sinHalfAngle;
|
|
_v(2) = axis[1] * sinHalfAngle;
|
|
_v(3) = axis[2] * sinHalfAngle;
|
|
}
|
|
|
|
/**
|
|
* angle_deg is the angle about the axis in degrees. axis must be normalized.
|
|
*/
|
|
INLINE_LINMATH void FLOATNAME(LQuaternion)::
|
|
set_from_axis_angle(FLOATTYPE angle_deg, const FLOATNAME(LVector3) &axis) {
|
|
set_from_axis_angle_rad(deg_2_rad(angle_deg), axis);
|
|
}
|
|
|
|
/**
|
|
* Returns the orientation represented by this quaternion, expressed as an up
|
|
* vector.
|
|
*/
|
|
INLINE_LINMATH FLOATNAME(LVector3) FLOATNAME(LQuaternion)::
|
|
get_up(CoordinateSystem cs) const {
|
|
return xform(FLOATNAME(LVector3)::up(cs));
|
|
}
|
|
|
|
/**
|
|
* Returns the orientation represented by this quaternion, expressed as a
|
|
* right vector.
|
|
*/
|
|
INLINE_LINMATH FLOATNAME(LVector3) FLOATNAME(LQuaternion)::
|
|
get_right(CoordinateSystem cs) const {
|
|
return xform(FLOATNAME(LVector3)::right(cs));
|
|
}
|
|
|
|
/**
|
|
* Returns the orientation represented by this quaternion, expressed as a
|
|
* forward vector.
|
|
*/
|
|
INLINE_LINMATH FLOATNAME(LVector3) FLOATNAME(LQuaternion)::
|
|
get_forward(CoordinateSystem cs) const {
|
|
return xform(FLOATNAME(LVector3)::forward(cs));
|
|
}
|
|
|
|
|
|
/**
|
|
*
|
|
*/
|
|
INLINE_LINMATH FLOATTYPE FLOATNAME(LQuaternion)::
|
|
get_r() const {
|
|
return _v(0);
|
|
}
|
|
|
|
/**
|
|
*
|
|
*/
|
|
INLINE_LINMATH FLOATTYPE FLOATNAME(LQuaternion)::
|
|
get_i() const {
|
|
return _v(1);
|
|
}
|
|
|
|
/**
|
|
*
|
|
*/
|
|
INLINE_LINMATH FLOATTYPE FLOATNAME(LQuaternion)::
|
|
get_j() const {
|
|
return _v(2);
|
|
}
|
|
|
|
/**
|
|
*
|
|
*/
|
|
INLINE_LINMATH FLOATTYPE FLOATNAME(LQuaternion)::
|
|
get_k() const {
|
|
return _v(3);
|
|
}
|
|
|
|
/**
|
|
*
|
|
*/
|
|
INLINE_LINMATH void FLOATNAME(LQuaternion)::
|
|
set_r(FLOATTYPE r) {
|
|
_v(0) = r;
|
|
}
|
|
|
|
/**
|
|
*
|
|
*/
|
|
INLINE_LINMATH void FLOATNAME(LQuaternion)::
|
|
set_i(FLOATTYPE i) {
|
|
_v(1) = i;
|
|
}
|
|
|
|
/**
|
|
*
|
|
*/
|
|
INLINE_LINMATH void FLOATNAME(LQuaternion)::
|
|
set_j(FLOATTYPE j) {
|
|
_v(2) = j;
|
|
}
|
|
|
|
/**
|
|
*
|
|
*/
|
|
INLINE_LINMATH void FLOATNAME(LQuaternion)::
|
|
set_k(FLOATTYPE k) {
|
|
_v(3) = k;
|
|
}
|
|
|
|
/**
|
|
*
|
|
*/
|
|
INLINE_LINMATH bool FLOATNAME(LQuaternion)::
|
|
normalize() {
|
|
FLOATTYPE length_squared = (*this).dot(*this);
|
|
if (length_squared == (FLOATTYPE)0.0f) {
|
|
set(0.0f, 0.0f, 0.0f, 0.0f);
|
|
return false;
|
|
} else if (!IS_THRESHOLD_EQUAL(length_squared, 1.0f, NEARLY_ZERO(FLOATTYPE))) {
|
|
(*this) /= csqrt(length_squared);
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/**
|
|
* Returns the complex conjugate of this quat.
|
|
*/
|
|
INLINE_LINMATH FLOATNAME(LQuaternion) FLOATNAME(LQuaternion)::
|
|
conjugate() const {
|
|
return FLOATNAME(LQuaternion)(_v(0), -_v(1), -_v(2), -_v(3));
|
|
}
|
|
|
|
/**
|
|
* Computes the conjugate of the other quat, and stores the result in this
|
|
* quat. This is a fully general operation and makes no assumptions about the
|
|
* type of transform represented by the quat.
|
|
*
|
|
* The other quat must be a different object than this quat. However, if you
|
|
* need to get a conjugate of a quat in place, see conjugate_in_place.
|
|
*
|
|
* The return value is true if the quat was successfully inverted, false if
|
|
* there was a singularity.
|
|
*/
|
|
INLINE_LINMATH bool FLOATNAME(LQuaternion)::
|
|
conjugate_from(const FLOATNAME(LQuaternion) &other) {
|
|
set(other._v(0), -other._v(1), -other._v(2), -other._v(3));
|
|
return true;
|
|
}
|
|
|
|
/**
|
|
* Sets this to be the conjugate of the current quat. Returns true if the
|
|
* successful, false if the quat was singular.
|
|
*/
|
|
INLINE_LINMATH bool FLOATNAME(LQuaternion)::
|
|
conjugate_in_place() {
|
|
// _v(0) = _v(0);
|
|
_v(1) = -_v(1);
|
|
_v(2) = -_v(2);
|
|
_v(3) = -_v(3);
|
|
return true;
|
|
}
|
|
|
|
/**
|
|
* Computes the inverse of the other quat, and stores the result in this quat.
|
|
* This is a fully general operation and makes no assumptions about the type
|
|
* of transform represented by the quat.
|
|
*
|
|
* The other quat must be a different object than this quat. However, if you
|
|
* need to invert a quat in place, see invert_in_place.
|
|
*
|
|
* The return value is true if the quat was successfully inverted, false if
|
|
* there was a singularity.
|
|
*/
|
|
INLINE_LINMATH bool FLOATNAME(LQuaternion)::
|
|
invert_from(const FLOATNAME(LQuaternion) &other) {
|
|
set(-other._v(0), other._v(1), other._v(2), other._v(3));
|
|
return true;
|
|
}
|
|
|
|
/**
|
|
* Inverts the current quat. Returns true if the inverse is successful, false
|
|
* if the quat was singular.
|
|
*/
|
|
INLINE_LINMATH bool FLOATNAME(LQuaternion)::
|
|
invert_in_place() {
|
|
_v(0) = -_v(0);
|
|
return true;
|
|
}
|
|
|
|
/**
|
|
* Returns true if this quaternion represents the identity transformation: no
|
|
* rotation.
|
|
*/
|
|
INLINE_LINMATH bool FLOATNAME(LQuaternion)::
|
|
is_identity() const {
|
|
return is_almost_identity(NEARLY_ZERO(FLOATTYPE));
|
|
}
|
|
|
|
/**
|
|
* Returns true if this quaternion represents the identity transformation
|
|
* within a given tolerance.
|
|
*/
|
|
INLINE_LINMATH bool FLOATNAME(LQuaternion)::
|
|
is_almost_identity(FLOATTYPE tolerance) const {
|
|
return (IS_THRESHOLD_EQUAL(_v(0), -1.0f, tolerance) ||
|
|
IS_THRESHOLD_EQUAL(_v(0), 1.0f, tolerance));
|
|
}
|
|
|
|
/**
|
|
* Returns an identity quaternion.
|
|
*/
|
|
INLINE_LINMATH const FLOATNAME(LQuaternion) &FLOATNAME(LQuaternion)::
|
|
ident_quat() {
|
|
return _ident_quat;
|
|
}
|
|
|
|
/**
|
|
* Inverts the given quat and returns it.
|
|
*/
|
|
INLINE_LINMATH FLOATNAME(LQuaternion)
|
|
invert(const FLOATNAME(LQuaternion) &a) {
|
|
FLOATNAME(LQuaternion) result;
|
|
bool nonsingular = result.invert_from(a);
|
|
#ifndef NDEBUG
|
|
if (!nonsingular) {
|
|
nassert_raise("Attempt to compute inverse of singular quaternion!");
|
|
return FLOATNAME(LQuaternion)::ident_quat();
|
|
}
|
|
#endif
|
|
return result;
|
|
}
|
|
|
|
/**
|
|
*
|
|
*/
|
|
INLINE_LINMATH FLOATNAME(LMatrix3) operator *(const FLOATNAME(LMatrix3) &m,
|
|
const FLOATNAME(LQuaternion) &q) {
|
|
FLOATNAME(LMatrix3) q_matrix;
|
|
q.extract_to_matrix(q_matrix);
|
|
|
|
return m * q_matrix;
|
|
}
|
|
|
|
/**
|
|
*
|
|
*/
|
|
INLINE_LINMATH FLOATNAME(LMatrix4) operator *(const FLOATNAME(LMatrix4) &m,
|
|
const FLOATNAME(LQuaternion) &q) {
|
|
FLOATNAME(LMatrix4) q_matrix;
|
|
q.extract_to_matrix(q_matrix);
|
|
|
|
// preserve the homogeneous coords and the translate
|
|
FLOATNAME(LVector4) m_row3 = m.get_row(3);
|
|
FLOATNAME(LVector4) m_col3 = m.get_col(3);
|
|
|
|
q_matrix = m * q_matrix;
|
|
q_matrix.set_row(3, m_row3);
|
|
q_matrix.set_col(3, m_col3);
|
|
|
|
return q_matrix;
|
|
}
|