add_impact

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);
 }
 
 ////////////////////////////////////////////////////////////////////

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;

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);
 }