diff --git a/panda/src/linmath/lquaternion_src.I b/panda/src/linmath/lquaternion_src.I index 1f6c9689d3..e219b210b2 100644 --- a/panda/src/linmath/lquaternion_src.I +++ b/panda/src/linmath/lquaternion_src.I @@ -315,6 +315,32 @@ get_axis() const { return FLOATNAME(LVector3)(_v.data[1], _v.data[2], _v.data[3]); } +//////////////////////////////////////////////////////////////////// +// Function: LQuaternion::get_axis +// Access: Public +// Description: 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 { + return FLOATNAME(LVector3)( + _v.data[1], _v.data[2], _v.data[3]).normalize(); +} + +//////////////////////////////////////////////////////////////////// +// Function: LQuaternion::get_angle_rad +// Access: Public +// Description: 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. +//////////////////////////////////////////////////////////////////// +INLINE_LINMATH FLOATTYPE FLOATNAME(LQuaternion):: +get_angle_rad() const { + return acos(_v.data[0]) * 2.0; +} + //////////////////////////////////////////////////////////////////// // Function: LQuaternion::get_angle // Access: Public @@ -325,7 +351,34 @@ get_axis() const { //////////////////////////////////////////////////////////////////// INLINE_LINMATH FLOATTYPE FLOATNAME(LQuaternion):: get_angle() const { - return rad_2_deg(acos(_v.data[0]) * 2.0); + return rad_2_deg(get_angle_rad()); +} + +//////////////////////////////////////////////////////////////////// +// Function: LQuaternion::set_from_axis_angle_rad +// Access: Public +// Description: 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 * 0.5); + _v.data[0] = cos(angle_rad * 0.5); + _v.data[1] = axis[0] * sinHalfAngle; + _v.data[2] = axis[1] * sinHalfAngle; + _v.data[3] = axis[2] * sinHalfAngle; +} + +//////////////////////////////////////////////////////////////////// +// Function: LQuaternion::set_from_axis_angle_deg +// Access: Public +// Description: 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); } //////////////////////////////////////////////////////////////////// diff --git a/panda/src/linmath/lquaternion_src.h b/panda/src/linmath/lquaternion_src.h index 58af1b9c86..7c2b34d484 100644 --- a/panda/src/linmath/lquaternion_src.h +++ b/panda/src/linmath/lquaternion_src.h @@ -47,24 +47,30 @@ PUBLISHED: INLINE_LINMATH FLOATNAME(LQuaternion) operator - (const FLOATNAME(LQuaternion) &other) const; - INLINE_LINMATH FLOATTYPE angle_rad(const FLOATNAME(LQuaternion) &other) const; - INLINE_LINMATH FLOATTYPE angle_deg(const FLOATNAME(LQuaternion) &other) const; + INLINE_LINMATH FLOATTYPE angle_rad(const FLOATNAME( + LQuaternion) &other) const; + INLINE_LINMATH FLOATTYPE angle_deg(const FLOATNAME( + LQuaternion) &other) const; INLINE_LINMATH FLOATNAME(LQuaternion) operator * (FLOATTYPE scalar) const; INLINE_LINMATH FLOATNAME(LQuaternion) operator / (FLOATTYPE scalar) const; - INLINE_LINMATH FLOATNAME(LQuaternion) operator *(const FLOATNAME(LQuaternion) &) const; - INLINE_LINMATH FLOATNAME(LQuaternion)& operator *=(const FLOATNAME(LQuaternion) &); + INLINE_LINMATH FLOATNAME(LQuaternion) operator *( + const FLOATNAME(LQuaternion) &) const; + INLINE_LINMATH FLOATNAME(LQuaternion)& operator *=( + const FLOATNAME(LQuaternion) &); INLINE_LINMATH FLOATNAME(LMatrix3) operator *(const FLOATNAME(LMatrix3) &); INLINE_LINMATH FLOATNAME(LMatrix4) operator *(const FLOATNAME(LMatrix4) &); - INLINE_LINMATH bool almost_equal(const FLOATNAME(LQuaternion) &other) const; - INLINE_LINMATH bool almost_equal(const FLOATNAME(LQuaternion) &other, - FLOATTYPE threshold) const; - INLINE_LINMATH bool is_same_direction(const FLOATNAME(LQuaternion) &other) const; - INLINE_LINMATH bool almost_same_direction(const FLOATNAME(LQuaternion) &other, - FLOATTYPE threshold) const; + INLINE_LINMATH bool almost_equal( + const FLOATNAME(LQuaternion) &other) const; + INLINE_LINMATH bool almost_equal( + const FLOATNAME(LQuaternion) &other, FLOATTYPE threshold) const; + INLINE_LINMATH bool is_same_direction( + const FLOATNAME(LQuaternion) &other) const; + INLINE_LINMATH bool almost_same_direction( + const FLOATNAME(LQuaternion) &other, FLOATTYPE threshold) const; INLINE_LINMATH void output(ostream&) const; @@ -77,8 +83,15 @@ PUBLISHED: FLOATNAME(LVecBase3) get_hpr(CoordinateSystem cs = CS_default) const; INLINE_LINMATH FLOATNAME(LVector3) get_axis() const; + INLINE_LINMATH FLOATNAME(LVector3) get_axis_normalized() const; + INLINE_LINMATH FLOATTYPE get_angle_rad() const; INLINE_LINMATH FLOATTYPE get_angle() const; + INLINE_LINMATH void set_from_axis_angle_rad( + FLOATTYPE angle_rad, const FLOATNAME(LVector3) &axis); + INLINE_LINMATH void set_from_axis_angle( + FLOATTYPE angle_deg, const FLOATNAME(LVector3) &axis); + INLINE_LINMATH FLOATNAME(LVector3) get_up(CoordinateSystem cs = CS_default) const; INLINE_LINMATH FLOATNAME(LVector3) get_right(CoordinateSystem cs = CS_default) const; INLINE_LINMATH FLOATNAME(LVector3) get_forward(CoordinateSystem cs = CS_default) const; diff --git a/panda/src/physics/physicsObject.cxx b/panda/src/physics/physicsObject.cxx index d5466df45b..3dac36be8c 100644 --- a/panda/src/physics/physicsObject.cxx +++ b/panda/src/physics/physicsObject.cxx @@ -107,12 +107,9 @@ add_local_impact(const LPoint3f &offset_from_center_of_mass, const LVector3f &force) { nassertv(!offset_from_center_of_mass.is_nan()); nassertv(!force.is_nan()); - LRotationf torque; - LVector3f impulse; - torque = LRotationf::ident_quat(); // place holder - impulse = LVector3f::zero(); // place holder - add_torque(torque); - add_impulse(impulse); + add_impact( + _orientation.xform(offset_from_center_of_mass), + _orientation.xform(force)); } //////////////////////////////////////////////////////////////////// @@ -131,10 +128,14 @@ add_impact(const LPoint3f &offset_from_center_of_mass, const LVector3f &force) { nassertv(!offset_from_center_of_mass.is_nan()); nassertv(!force.is_nan()); + LVector3f a = -offset_from_center_of_mass; + LVector3f b = force; + a.normalize(); + b.normalize(); + float theta = a.dot(b); LRotationf torque; - LVector3f impulse; - torque = LRotationf::ident_quat(); // place holder - impulse = LVector3f::zero(); // place holder + torque.set_from_axis_angle((1.0f - theta) * a.length(), b.cross(a).normalize()); + LVector3f impulse = theta * force; add_torque(torque); add_impulse(impulse); }