diff --git a/panda/src/linmath/lquaternion_src.cxx b/panda/src/linmath/lquaternion_src.cxx index 7ffb7766a3..cd48827eea 100644 --- a/panda/src/linmath/lquaternion_src.cxx +++ b/panda/src/linmath/lquaternion_src.cxx @@ -26,6 +26,29 @@ pure_imaginary(const FLOATNAME(LVector3) &v) { return FLOATNAME(LQuaternion)(0, v[0], v[1], v[2]); } +/** + * Returns a new quaternion that represents this quaternion raised to the + * given power. + */ +FLOATNAME(LQuaternion) FLOATNAME(LQuaternion):: +__pow__(FLOATTYPE power) const { + if (IS_NEARLY_ZERO(power)) { + return FLOATNAME(LQuaternion)(1, 0, 0, 0); + } + + FLOATTYPE l = length(); + FLOATTYPE norm = _v(0) / l; + if (IS_NEARLY_EQUAL(cabs(norm), (FLOATTYPE)1)) { + return FLOATNAME(LQuaternion)(cpow(_v(0), power), 0, 0, 0); + } + + FLOATTYPE angle = acos(norm); + FLOATTYPE angle2 = angle * power; + FLOATTYPE mag = cpow(l, power - 1); + FLOATTYPE mult = mag * (sin(angle2) / sin(angle)); + return FLOATNAME(LQuaternion)(cos(angle2) * mag * l, _v(1) * mult, _v(2) * mult, _v(3) * mult); +} + /** * Based on the quat lib from VRPN. */ diff --git a/panda/src/linmath/lquaternion_src.h b/panda/src/linmath/lquaternion_src.h index 429b86a0cc..00ba2a934f 100644 --- a/panda/src/linmath/lquaternion_src.h +++ b/panda/src/linmath/lquaternion_src.h @@ -57,6 +57,8 @@ PUBLISHED: INLINE_LINMATH FLOATNAME(LMatrix3) operator *(const FLOATNAME(LMatrix3) &); INLINE_LINMATH FLOATNAME(LMatrix4) operator *(const FLOATNAME(LMatrix4) &); + FLOATNAME(LQuaternion) __pow__(FLOATTYPE) const; + INLINE_LINMATH bool almost_equal( const FLOATNAME(LQuaternion) &other) const; INLINE_LINMATH bool almost_equal(