Experimental FABRIK implementation

direct/src/actor/Actor.py

@@ -1326,6 +1326,22 @@ class Actor(DirectObject, NodePath):
         if not anyGood:
             self.notify.warning("Cannot freeze joint %s" % (jointName))
 
+    def addEndEffector(self, node, partName, jointName, lodName="lodRoot"):
+        subpartDef = self.__subpartDict.get(partName, Actor.SubpartDef(partName))
+        trueName = subpartDef.truePartName
+        for bundleDict in self.__partBundleDict.values():
+            bundle = bundleDict[trueName].getBundle()
+            joint = bundle.findChild(jointName)
+            if node is None and joint and isinstance(joint, CharacterJoint):
+                node = self.attachNewNode(ModelNode(jointName))
+                mat = Mat4()
+                joint.getNetTransform(mat)
+                node.setMat(mat)
+
+            IKEffector(joint, node.node())
+
+        return node
+
     def releaseJoint(self, partName, jointName):
         """Undoes a previous call to controlJoint() or freezeJoint()
         and restores the named joint to its normal animation. """

panda/src/chan/config_chan.cxx

@@ -101,6 +101,11 @@ PRC_DESC("This specifies the priority assign to an asynchronous bind "
          "model loads).  A higher number here makes the animations "
          "load sooner."));
 
+ConfigVariableInt ik_max_iterations
+("ik-max-iterations", 8,
+ PRC_DESC("Set this to limit the number of iterations for the IK solver."));
+
+
 ConfigureFn(config_chan) {
   AnimBundle::init_type();
   AnimBundleNode::init_type();

panda/src/chan/config_chan.h

@@ -28,5 +28,6 @@ EXPCL_PANDA_CHAN extern ConfigVariableBool read_compressed_channels;
 EXPCL_PANDA_CHAN extern ConfigVariableBool interpolate_frames;
 EXPCL_PANDA_CHAN extern ConfigVariableBool restore_initial_pose;
 EXPCL_PANDA_CHAN extern ConfigVariableInt async_bind_priority;
+EXPCL_PANDA_CHAN extern ConfigVariableInt ik_max_iterations;
 
 #endif

panda/src/chan/partBundle.cxx

@@ -522,6 +522,45 @@ force_update() {
   return any_changed;
 }
 
+/**
+ * Recursively update this particular part and all of its descendents for the
+ * current frame.  This is not really public and is not intended to be called
+ * directly; it is called from the top of the tree by PartBundle::update().
+ *
+ * The return value is true if any part has changed, false otherwise.
+ */
+bool PartBundle::
+do_update(PartBundle *root, const CycleData *cdata, PartGroup *,
+          bool parent_changed, bool anim_changed, Thread *current_thread) {
+
+  bool any_changed = PartGroup::do_update(this, cdata, this, parent_changed, anim_changed, current_thread);
+
+  LPoint3 root_pos = ((CData *)cdata)->_root_xform.get_row3(3);
+  if (r_init_ik(root_pos)) {
+    int num_iterations = 0;
+    int max_iterations = ik_max_iterations;
+    PN_stdfloat error;
+    do {
+      LPoint3 root_pos_copy(root_pos);
+      r_reverse_ik(root_pos_copy);
+      error = (root_pos_copy - root_pos).length_squared();
+
+      r_forward_ik(root_pos);
+      ++num_iterations;
+    }
+    while (!IS_NEARLY_ZERO(error) && num_iterations < max_iterations);
+
+    if (chan_cat.is_debug()) {
+      chan_cat.debug()
+        << "IK solver ran for " << num_iterations << " iterations, error="
+        << error << "\n";
+    }
+
+    r_apply_ik(((CData *)cdata)->_root_xform);
+  }
+
+  return any_changed;
+}
 
 /**
  * Called by the AnimControl whenever it starts an animation.  This is just a

panda/src/chan/partBundle.h

@@ -146,6 +146,10 @@ PUBLISHED:
   bool force_update();
 
 public:
+  virtual bool do_update(PartBundle *root, const CycleData *root_cdata,
+                         PartGroup *parent, bool parent_changed,
+                         bool anim_changed, Thread *current_thread);
+
   // The following functions aren't really part of the public interface;
   // they're just public so we don't have to declare a bunch of friends.
   virtual void control_activated(AnimControl *control);

panda/src/chan/partGroup.cxx

@@ -507,6 +507,70 @@ determine_effective_channels(const CycleData *root_cdata) {
   }
 }
 
+/**
+ * Recursively initializes the joint for IK, calculating the current net
+ * position and lengths.  Returns true if there were any effectors under this
+ * node, false otherwise.
+ */
+bool PartGroup::
+r_init_ik(const LPoint3 &parent_pos) {
+  bool has_any = false;
+  for (PartGroup *child : _children) {
+    has_any = child->r_init_ik(parent_pos) || has_any;
+  }
+  return has_any;
+}
+
+/**
+ * Executes a forward IK pass on the given points (which are set up by
+ * r_setup_ik_points).
+ */
+void PartGroup::
+r_forward_ik(const LPoint3 &parent_pos) {
+  for (PartGroup *child : _children) {
+    child->r_forward_ik(parent_pos);
+  }
+}
+
+/**
+ * Executes a reverse IK pass on the given points (which are set up by
+ * r_setup_ik_points).  Returns true if there were any effectors under this
+ * joint, in which case the new position of this joint is stored in out_pos.
+ */
+bool PartGroup::
+r_reverse_ik(LPoint3 &parent_pos) {
+  LPoint3 effective_pos(0);
+  int num_effectors = 0;
+
+  for (PartGroup *child : _children) {
+    LPoint3 desired_pos = parent_pos;
+    if (!child->r_reverse_ik(desired_pos)) {
+      // No effectors under this joint, so it's not of interest to the reverse
+      // pass.
+      continue;
+    }
+
+    effective_pos += desired_pos;
+    ++num_effectors;
+  }
+
+  if (num_effectors == 0) {
+    return false;
+  }
+
+  parent_pos = effective_pos * (1.0 / num_effectors);
+  return true;
+}
+
+/**
+ *
+ */
+void PartGroup::
+r_apply_ik(const LMatrix4 &parent_net_transform) {
+  for (PartGroup *child : _children) {
+    child->r_apply_ik(parent_net_transform);
+  }
+}
 
 /**
  * Writes a brief description of all of the group's descendants.

panda/src/chan/partGroup.h

@@ -100,6 +100,11 @@ public:
   virtual void do_xform(const LMatrix4 &mat, const LMatrix4 &inv_mat);
   virtual void determine_effective_channels(const CycleData *root_cdata);
 
+  virtual bool r_init_ik(const LPoint3 &parent_pos);
+  virtual void r_forward_ik(const LPoint3 &parent_pos);
+  virtual bool r_reverse_ik(LPoint3 &out_pos);
+  virtual void r_apply_ik(const LMatrix4 &parent_net_transform);
+
 protected:
   void write_descendants(std::ostream &out, int indent_level) const;
   void write_descendants_with_value(std::ostream &out, int indent_level) const;

panda/src/char/CMakeLists.txt

@@ -6,6 +6,7 @@ set(P3CHAR_HEADERS
   characterSlider.h
   characterVertexSlider.I characterVertexSlider.h
   config_char.h
+  ikEffector.I ikEffector.h
   jointVertexTransform.I jointVertexTransform.h
 )
 
@@ -16,6 +17,7 @@ set(P3CHAR_SOURCES
   characterSlider.cxx
   characterVertexSlider.cxx
   config_char.cxx
+  ikEffector.cxx
   jointVertexTransform.cxx
 )
 

panda/src/char/characterJoint.cxx

@@ -182,7 +182,189 @@ do_xform(const LMatrix4 &mat, const LMatrix4 &inv_mat) {
   MovingPartMatrix::do_xform(mat, inv_mat);
 }
 
+/**
+ * Recursively initializes the joint for IK, calculating the current net
+ * position and lengths.  Returns true if there were any effectors under this
+ * node, false otherwise.
+ */
+bool CharacterJoint::
+r_init_ik(const LPoint3 &parent_pos) {
+  _ik_pos = _net_transform.get_row3(3);
+  _ik_length = (_ik_pos - parent_pos).length();
+
+  if (PartGroup::r_init_ik(_ik_pos)) {
+    _ik_weight = 1;
+    return true;
+  } else {
+    _ik_weight = 0;
+    return false;
+  }
+}
+
+/**
+ * Executes a forward IK pass on the given points (which are set up by
+ * r_setup_ik_points).
+ */
+void CharacterJoint::
+r_forward_ik(const LPoint3 &parent_pos) {
+  LVector3 delta = _ik_pos - parent_pos;
+  PN_stdfloat dist = delta.length();
+
+  if (!IS_NEARLY_ZERO(dist)) {
+    LVector3 dir = delta / dist;
+
+    _ik_pos = parent_pos + dir * _ik_length;
+  }
+  else {
+    // It has length 0, so we just inherit the parent position.
+    _ik_pos = parent_pos;
+  }
+
+  for (PartGroup *child : _children) {
+    child->r_forward_ik(_ik_pos);
+  }
+}
+
+/**
+ * Executes a reverse IK pass on the given points (which are set up by
+ * r_setup_ik_points).  Returns true if there were any effectors under this
+ * joint, in which case the new position of this joint is stored in out_pos.
+ */
+bool CharacterJoint::
+r_reverse_ik(LPoint3 &parent_pos) {
+  LPoint3 effective_pos(0);
+  int num_effectors = 0;
+
+  for (PartGroup *child : _children) {
+    LPoint3 desired_pos = _ik_pos;
+    if (!child->r_reverse_ik(desired_pos)) {
+      // No effectors under this joint, so it's not of interest to the reverse
+      // pass.
+      continue;
+    }
+
+    effective_pos += desired_pos;
+    ++num_effectors;
+  }
+
+  if (num_effectors == 0) {
+    return false;
+  }
+
+  _ik_pos = effective_pos * (1.0 / num_effectors);
+
+  PN_stdfloat base_length = _ik_length;
+  if (IS_NEARLY_ZERO(base_length)) {
+    parent_pos = _ik_pos;
+  } else {
+    LVector3 delta = _ik_pos - parent_pos;
+    PN_stdfloat length = delta.length();
+    LVector3 dir = delta / length;
+    parent_pos = _ik_pos - dir * base_length;
+  }
+
+  return true;
+}
+
+/**
+ * Recursively applies the IK position changes computed by r_forward_ik and
+ * r_reverse_ik onto _net_transform.
+ */
+void CharacterJoint::
+r_apply_ik(const LMatrix4 &parent_net_transform) {
+  _net_transform = _value * parent_net_transform;
+
+  // We can only apply one rotation to this joint, so when there are multiple
+  // children, we have a problem.  What we do is figure out which children have
+  // effectors under them (ik_weight > 0), and rotate to their average.
+  LVector3 cur_vec(0, 0, 0);
+  LVector3 new_vec(0, 0, 0);
+  PN_stdfloat total_weight = 0;
+
+  LMatrix4 inverse_xform;
+  inverse_xform.invert_from(_net_transform);
+
+  for (PartGroup *child : _children) {
+    if (!child->is_character_joint()) {
+      continue;
+    }
+
+    CharacterJoint *child_joint = (CharacterJoint *)child;
+    //if (child_joint->_ik_weight == 0) {
+    //  continue;
+    //}
+
+    // Get current position of joint relative to parent
+    LPoint3 cur_pos = child_joint->_value.get_row3(3);
+    cur_vec += cur_pos.normalized();
+
+    // Get desired position relative to parent
+    LPoint3 child_pos = child_joint->_ik_pos;
+    LPoint3 rel_pos = inverse_xform.xform_point(child_pos);
+    new_vec += rel_pos.normalized();
+
+    total_weight += 1;
+  }
 
+  if (total_weight > 0) {
+    PN_stdfloat factor = 1.0 / total_weight;
+    cur_vec *= factor;
+    new_vec *= factor;
+
+    LVector3 w = cur_vec.cross(new_vec);
+    if (w.length_squared() != 0) {
+      w.normalize();
+      PN_stdfloat angle = new_vec.signed_angle_deg(cur_vec, w);
+      if (!IS_NEARLY_ZERO(angle)) {
+        LMatrix3 r = LMatrix3::rotate_mat(-angle, w);
+
+        _value.set_upper_3(r * _value.get_upper_3());
+      }
+    }
+    else if (cur_vec.dot(new_vec) < 0) {
+      // Exactly antiparallel.  We have an infinite number of axes around which we
+      // could rotate to get the desired matrix.  Can we pick one arbitrarily?
+
+      w = new_vec.cross(LVector3(1, 0, 0));
+      if (IS_NEARLY_ZERO(w.length_squared())) {
+        w = new_vec.cross(LVector3(0, 1, 0));
+      }
+
+      LMatrix3 r = LMatrix3::rotate_mat(-180, w);
+
+      _value.set_upper_3(r * _value.get_upper_3());
+    }
+  }
+
+  //TODO: don't duplicate all the logic from update_internals.
+  _net_transform = _value * parent_net_transform;
+
+  Thread *current_thread = Thread::get_current_thread();
+  if (!_net_transform_nodes.empty()) {
+    CPT(TransformState) t = TransformState::make_mat(_net_transform);
+
+    NodeList::iterator ai;
+    for (ai = _net_transform_nodes.begin();
+         ai != _net_transform_nodes.end();
+         ++ai) {
+      PandaNode *node = *ai;
+      node->set_transform(t, current_thread);
+    }
+  }
+
+  _skinning_matrix = _initial_net_transform_inverse * _net_transform;
+
+  // Also tell our related JointVertexTransforms that we've changed their
+  // underlying matrix.
+  VertexTransforms::iterator vti;
+  for (vti = _vertex_transforms.begin(); vti != _vertex_transforms.end(); ++vti) {
+    (*vti)->mark_modified(current_thread);
+  }
+
+  for (PartGroup *child : _children) {
+    child->r_apply_ik(_net_transform);
+  }
+}
 
 /**
  * Adds the indicated node to the list of nodes that will be updated each

panda/src/char/characterJoint.h

@@ -49,6 +49,11 @@ public:
                                 Thread *current_thread);
   virtual void do_xform(const LMatrix4 &mat, const LMatrix4 &inv_mat);
 
+  bool r_init_ik(const LPoint3 &parent_pos);
+  void r_forward_ik(const LPoint3 &parent_pos);
+  bool r_reverse_ik(LPoint3 &out_pos);
+  void r_apply_ik(const LMatrix4 &parent_net_transform);
+
 PUBLISHED:
   bool add_net_transform(PandaNode *node);
   bool remove_net_transform(PandaNode *node);
@@ -114,6 +119,11 @@ public:
   // animated position.
   LMatrix4 _skinning_matrix;
 
+  // Used by IK calculations.
+  LPoint3 _ik_pos;
+  PN_stdfloat _ik_length;
+  PN_stdfloat _ik_weight;
+
 public:
   virtual TypeHandle get_type() const {
     return get_class_type();

panda/src/char/config_char.cxx

@@ -18,6 +18,7 @@
 #include "characterJointEffect.h"
 #include "characterSlider.h"
 #include "characterVertexSlider.h"
+#include "ikEffector.h"
 #include "jointVertexTransform.h"
 #include "dconfig.h"
 
@@ -61,6 +62,7 @@ init_libchar() {
   CharacterJointEffect::init_type();
   CharacterSlider::init_type();
   CharacterVertexSlider::init_type();
+  IKEffector::init_type();
   JointVertexTransform::init_type();
 
   // Registration of writeable object's creation functions with BamReader's

panda/src/char/ikEffector.I

@@ -0,0 +1,20 @@
+/**
+ * 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 ikEffector.I
+ * @author rdb
+ * @date 2020-11-16
+ */
+
+/**
+ *
+ */
+INLINE IKEffector::
+IKEffector(const IKEffector &copy) : _node(copy._node) {
+
+}

panda/src/char/ikEffector.cxx

@@ -0,0 +1,112 @@
+/**
+ * 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 ikEffector.cxx
+ * @author rdb
+ * @date 2020-11-16
+ */
+
+#include "ikEffector.h"
+#include "pandaNode.h"
+
+TypeHandle IKEffector::_type_handle;
+
+/**
+ * Creates a new IKEffector tracking the given node.
+ */
+IKEffector::
+IKEffector(PartGroup *parent, PandaNode *node) :
+  PartGroup(parent, ""),
+  _node(node) {
+}
+
+/**
+ * Recursively initializes the joint for IK, calculating the current net
+ * position and lengths.  Returns true if there were any effectors under this
+ * node, false otherwise.
+ */
+bool IKEffector::
+r_init_ik(const LPoint3 &parent_pos) {
+  _ik_pos = _node->get_transform()->get_pos();
+  _length = (_ik_pos - parent_pos).length();
+  return true;
+}
+
+/**
+ * Executes a forward IK pass on the given points (which are set up by
+ * r_setup_ik_points).
+ */
+void IKEffector::
+r_forward_ik(const LPoint3 &parent_pos) {
+  // Nothing to do here.  End effectors only take effect in the reverse pass.
+}
+
+/**
+ * Executes a reverse IK pass on the given points (which are set up by
+ * r_setup_ik_points).  Returns true if there were any effectors under this
+ * joint, in which case the new position of this joint is stored in out_pos.
+ */
+bool IKEffector::
+r_reverse_ik(LPoint3 &out_pos) {
+  out_pos = _ik_pos;
+  return true;
+}
+
+/**
+ * Function to write the important information in the particular object to a
+ * Datagram
+ */
+void IKEffector::
+write_datagram(BamWriter *manager, Datagram &me) {
+  PartGroup::write_datagram(manager, me);
+}
+
+/**
+ * Takes in a vector of pointers to TypedWritable objects that correspond to
+ * all the requests for pointers that this object made to BamReader.
+ */
+int IKEffector::
+complete_pointers(TypedWritable **p_list, BamReader* manager) {
+  int pi = PartGroup::complete_pointers(p_list, manager);
+
+  _node = DCAST(PandaNode, p_list[pi++]);
+
+  return pi;
+}
+
+/**
+ * Function that reads out of the datagram (or asks manager to read) all of
+ * the data that is needed to re-create this object and stores it in the
+ * appropiate place
+ */
+void IKEffector::
+fillin(DatagramIterator &scan, BamReader *manager) {
+  PartGroup::fillin(scan, manager);
+}
+
+/**
+ * Factory method to generate a IKEffector object
+ */
+/*TypedWritable* IKEffector::
+make_IKEffector(const FactoryParams &params) {
+  IKEffector *me = new IKEffector;
+  DatagramIterator scan;
+  BamReader *manager;
+
+  parse_params(params, scan, manager);
+  me->fillin(scan, manager);
+  return me;
+}*/
+
+/**
+ * Factory method to generate a IKEffector object
+ */
+/*void IKEffector::
+register_with_read_factory() {
+  BamReader::get_factory()->register_factory(get_class_type(), make_IKEffector);
+}*/

panda/src/char/ikEffector.h

@@ -0,0 +1,70 @@
+/**
+ * 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 ikEffector.h
+ * @author rdb
+ * @date 2020-11-16
+ */
+
+#ifndef IKEFFECTOR_H
+#define IKEFFECTOR_H
+
+#include "pandabase.h"
+
+#include "partGroup.h"
+
+/**
+ * This object is placed at the end of a chain of joints in order to pull it
+ * towards a particular position using Inverse Kinematics.
+ */
+class EXPCL_PANDA_CHAR IKEffector : public PartGroup {
+protected:
+  INLINE IKEffector(const IKEffector &copy);
+
+PUBLISHED:
+  explicit IKEffector(PartGroup *parent, PandaNode *node);
+
+  bool r_init_ik(const LPoint3 &parent_pos);
+  void r_forward_ik(const LPoint3 &parent_pos);
+  bool r_reverse_ik(LPoint3 &out_pos);
+
+protected:
+  IKEffector();
+
+  PT(PandaNode) _node;
+  LPoint3 _ik_pos;
+  PN_stdfloat _length;
+
+public:
+  virtual void write_datagram(BamWriter *manager, Datagram &dg);
+  virtual int complete_pointers(TypedWritable **plist, BamReader *manager);
+
+protected:
+  void fillin(DatagramIterator &scan, BamReader *manager);
+
+public:
+  virtual TypeHandle get_type() const {
+    return get_class_type();
+  }
+  virtual TypeHandle force_init_type() {init_type(); return get_class_type();}
+  static TypeHandle get_class_type() {
+    return _type_handle;
+  }
+  static void init_type() {
+    PartGroup::init_type();
+    register_type(_type_handle, "IKEffector",
+                  PartGroup::get_class_type());
+  }
+
+private:
+  static TypeHandle _type_handle;
+};
+
+#include "ikEffector.I"
+
+#endif

panda/src/char/p3char_composite2.cxx

@@ -1,5 +1,5 @@
 #include "characterJointEffect.cxx"
 #include "characterSlider.cxx"
 #include "characterVertexSlider.cxx"
+#include "ikEffector.cxx"
 #include "jointVertexTransform.cxx"
-