Merging updated IK library source files

Source/ThirdParty/ik/include/ik/bst_vector.h

@@ -13,15 +13,15 @@
 
 C_HEADER_BEGIN
 
-struct bstv_hash_value_t
+typedef struct bstv_hash_value_t
 {
     uint32_t hash;
     void*    value;
-};
+} bstv_hash_value_t;
 
 struct bstv_t
 {
-    struct ordered_vector_t   vector;
+    ordered_vector_t   vector;
 };
 
 /*!
@@ -29,7 +29,7 @@ struct bstv_t
  * @return Returns the newly created bstv object. It must be freed with
  * bstv_destroy() when no longer required.
  */
-IK_PUBLIC_API struct bstv_t*
+IK_PUBLIC_API bstv_t*
 bstv_create(void);
 
 /*!
@@ -40,7 +40,7 @@ bstv_create(void);
  * @param[in] bstv The bstv object to initialise.
  */
 IK_PUBLIC_API void
-bstv_construct(struct bstv_t* bstv);
+bstv_construct(bstv_t* bstv);
 
 /*!
  * @brief Destroys an existing bstv object and FREEs the underlying memory.
@@ -48,7 +48,7 @@ bstv_construct(struct bstv_t* bstv);
  * @param[in] bstv The bstv object to destroy.
  */
 IK_PUBLIC_API void
-bstv_destroy(struct bstv_t* bstv);
+bstv_destroy(bstv_t* bstv);
 
 /*!
  * @brief Inserts an element into the bstv using a hashed key.
@@ -69,7 +69,7 @@ bstv_destroy(struct bstv_t* bstv);
  * existed (in which case nothing is inserted). Returns -1 on failure.
  */
 IK_PUBLIC_API int
-bstv_insert(struct bstv_t* bstv, uint32_t hash, void* value);
+bstv_insert(bstv_t* bstv, uint32_t hash, void* value);
 
 /*!
  * @brief Sets the value bstvped to the specified hash in the bstv.
@@ -79,7 +79,7 @@ bstv_insert(struct bstv_t* bstv, uint32_t hash, void* value);
  * @param[in] value The new value to set.
  */
 IK_PUBLIC_API void
-bstv_set(struct bstv_t* bstv, uint32_t hash, void* value);
+bstv_set(bstv_t* bstv, uint32_t hash, void* value);
 
 /*!
  * @brief Looks for an element in the bstv and returns it if found.
@@ -93,7 +93,7 @@ bstv_set(struct bstv_t* bstv, uint32_t hash, void* value);
  * hash exists, use bstv_key_exists() instead.
  */
 IK_PUBLIC_API void*
-bstv_find(const struct bstv_t* bstv, uint32_t hash);
+bstv_find(const bstv_t* bstv, uint32_t hash);
 
 /*!
  * @brief Looks for an element in the bstv and returns a pointer to the element
@@ -105,7 +105,7 @@ bstv_find(const struct bstv_t* bstv, uint32_t hash);
  * @param[in] hash The has to search for.
  */
 IK_PUBLIC_API void**
-bstv_find_ptr(const struct bstv_t* bstv, uint32_t hash);
+bstv_find_ptr(const bstv_t* bstv, uint32_t hash);
 
 /*!
  * @brief Finds the specified element in the bstv and returns its key.
@@ -116,7 +116,7 @@ bstv_find_ptr(const struct bstv_t* bstv, uint32_t hash);
  * otherwise.
  */
 IK_PUBLIC_API uint32_t
-bstv_find_element(const struct bstv_t* bstv, const void* value);
+bstv_find_element(const bstv_t* bstv, const void* value);
 
 /*!
  * @brief Gets any element from the bstv.
@@ -126,7 +126,7 @@ bstv_find_element(const struct bstv_t* bstv, const void* value);
  * @return Returns an element as a void pointer. Which element is random.
  */
 IK_PUBLIC_API void*
-bstv_get_any_element(const struct bstv_t* bstv);
+bstv_get_any_element(const bstv_t* bstv);
 
 /*!
  * @brief Returns 1 if the specified hash exists, 0 if otherwise.
@@ -135,7 +135,7 @@ bstv_get_any_element(const struct bstv_t* bstv);
  * @return 0 if the hash was found, -1 if the hash was not found.
  */
 IK_PUBLIC_API int
-bstv_hash_exists(struct bstv_t* bstv, uint32_t hash);
+bstv_hash_exists(bstv_t* bstv, uint32_t hash);
 
 /*!
  * @brief Returns a hash that does not yet exist in the bstv.
@@ -144,7 +144,7 @@ bstv_hash_exists(struct bstv_t* bstv, uint32_t hash);
  * @return Returns a hash that does not yet exist in the bstv.
  */
 IK_PUBLIC_API uint32_t
-bstv_find_unused_hash(struct bstv_t* bstv);
+bstv_find_unused_hash(bstv_t* bstv);
 
 /*!
  * @brief Erases an element from the bstv using a hash.
@@ -162,10 +162,10 @@ bstv_find_unused_hash(struct bstv_t* bstv);
  * bstv.
  */
 IK_PUBLIC_API void*
-bstv_erase(struct bstv_t* bstv, uint32_t hash);
+bstv_erase(bstv_t* bstv, uint32_t hash);
 
 IK_PUBLIC_API void*
-bstv_erase_element(struct bstv_t* bstv, void* value);
+bstv_erase_element(bstv_t* bstv, void* value);
 
 /*!
  * @brief Erases the entire bstv, including the underlying memory.
@@ -175,10 +175,10 @@ bstv_erase_element(struct bstv_t* bstv, void* value);
  * @param[in] bstv The bstv to clear.
  */
 IK_PUBLIC_API void
-bstv_clear(struct bstv_t* bstv);
+bstv_clear(bstv_t* bstv);
 
 IK_PUBLIC_API void
-bstv_clear_free(struct bstv_t* bstv);
+bstv_clear_free(bstv_t* bstv);
 
 /*!
  * @brief Returns the number of elements in the specified bstv.
@@ -201,8 +201,8 @@ bstv_clear_free(struct bstv_t* bstv);
     var_t* var_v;                                                                                        \
     for(i_##var_v = 0;                                                                                   \
         i_##var_v != bstv_count(bstv) &&                                                                 \
-            ((hash_v = ((struct bstv_hash_value_t*) (bstv)->vector.data)[i_##var_v].hash) || 1) &&       \
-            ((var_v  = (var_t*)((struct bstv_hash_value_t*)(bstv)->vector.data)[i_##var_v].value) || 1); \
+            ((hash_v = ((bstv_hash_value_t*) (bstv)->vector.data)[i_##var_v].hash) || 1) &&       \
+            ((var_v  = (var_t*)((bstv_hash_value_t*)(bstv)->vector.data)[i_##var_v].value) || 1); \
         ++i_##var_v) {
 
 /*!
@@ -218,7 +218,7 @@ bstv_clear_free(struct bstv_t* bstv);
  * @param[in] bstv A pointer to the bstv object currently being iterated.
  */
 #define BSTV_ERASE_CURRENT_ITEM_IN_FOR_LOOP(bstv, var_v) do { \
-    ordered_vector_erase_element(&(bstv)->vector, ((struct bstv_hash_value_t*)(bstv)->vector.data) + i_##var_v); \
+    ordered_vector_erase_element(&(bstv)->vector, ((bstv_hash_value_t*)(bstv)->vector.data) + i_##var_v); \
     --i_##var_v; } while(0)
 
 C_HEADER_END

Source/ThirdParty/ik/include/ik/chain_tree.h

@@ -0,0 +1,127 @@
+/*!
+ * @file chain.h
+ * @brief Manages synthesising the user specified tree of nodes (ik_node_t)
+ * into a structure more optimal for solvers.
+ *
+ *
+ */
+#ifndef IK_CHAIN_H
+#define IK_CHAIN_H
+
+#include "ik/config.h"
+#include "ik/ordered_vector.h"
+
+C_HEADER_BEGIN
+
+struct chain_t
+{
+    /* list of ik_node_t* references that belong to this chain */
+    ordered_vector_t nodes;
+    /* list of chain_t objects */
+    ordered_vector_t children;
+};
+
+struct chain_island_t
+{
+    chain_t       root_chain;
+
+    /*
+     * List of ik_node_t* objects. This list contains the leaf nodes of IK
+     * effectors, the children of which aren't part of the IK problem but need
+     * to be properly updated to match the new transform of the solved tree.
+     */
+    ordered_vector_t transform_dependent_nodes;
+};
+
+struct chain_tree_t
+{
+    ordered_vector_t islands; /* list of chain_island_t objects */
+};
+
+void
+chain_tree_construct(chain_tree_t* chain_trees);
+
+void
+chain_tree_destruct(chain_tree_t* chain_trees);
+
+void
+chain_island_construct(chain_island_t* chain_island);
+
+void
+chain_island_destruct(chain_island_t* chain_island);
+
+chain_t*
+chain_create(void);
+
+void
+chain_destroy(chain_t* chain);
+
+/*!
+ * @brief Initialises an allocated chain object.
+ */
+void
+chain_construct(chain_t* chain);
+
+/*!
+ * @brief Destroys and frees all members, but does not deallocate the chain
+ * object itself.
+ */
+void
+chain_destruct(chain_t* chain);
+
+
+/*!
+ * @brief Clears all children and nodes.
+ */
+void
+chain_clear_free(chain_t* chain);
+
+/*!
+ * @brief Breaks down the relevant nodes of the scene graph into a tree of
+ * chains. FABRIK can then more efficiently solve each chain individually.
+ *
+ * A "sub-base joint" is a node in the scene graph where at least two end
+ * effector nodes eventually join together. FABRIK only works on single
+ * chains of joints at a time. The end position of every sub-base joint is
+ * the average of the resulting multiple positions after running FABRIK on
+ * each chain. Said average position becomes the new target position for
+ * the next chain connected to it.
+ *
+ * This algorithm finds all sub-base joints and generates chains between
+ * base, sub-base joints, and end effectors. These chains are inserted into
+ * the chain tree.
+ */
+int
+rebuild_chain_tree(ik_solver_t* solver);
+
+void
+calculate_segment_lengths(chain_tree_t* chain_tree);
+
+/*!
+ * @brief Counts all of the chains in the tree, excluding the root chain.
+ */
+int
+count_chains_exclude_root(chain_tree_t* chain_tree);
+
+void
+calculate_global_rotations(chain_t* chain);
+
+#if IK_DOT_OUTPUT == ON
+/*!
+ * @brief Dumps the chain tree to DOT format.
+ * @param[in] root The root node of the user created tree. This is a parameter
+ * because the root chain does not necessarily hold the root node of the tree
+ * because the root node doesn't have to be part of the IK problem.
+ * @note Doesn't necessarily have to be the root node, it will dump the tree
+ * beginning at this node.
+ * @param[in] chain Usually the root chain. Doesn't necessarily have to be the
+ * root, in which case it will dump beginning at this chain.
+ * @param[in] file_name The name of the file to dump to.
+ */
+void
+dump_to_dot(ik_node_t* root, chain_tree_t* chain_tree, const char* file_name);
+#endif /* IK_DOT_OUTPUT */
+
+C_HEADER_END
+
+#endif /* IK_CHAIN_H */

Source/ThirdParty/ik/include/ik/config.h.in

@@ -12,7 +12,8 @@
 #   define OFF    0
 #   define ON     1
 
-#   define HAVE_STDINT_H @HAVE_STDINT_H@
+#   define IK_HAVE_STDINT_H @IK_HAVE_STDINT_H@
+#   define IK_RESTRICT @IK_RESTRICT@
 #   define ik_real @IK_REAL@
 #   define IK_MEMORY_DEBUGGING @IK_MEMORY_DEBUGGING@
 #   if IK_MEMORY_DEBUGGING == ON
@@ -25,6 +26,7 @@
      * --------------------------------------------------------------*/
 
 #   include "ik/export.h"
+#   include "ik/types.h"
 #   include <stddef.h>
 
 #   if HAVE_STDINT_H == ON

Source/ThirdParty/ik/include/ik/constraint.h

@@ -0,0 +1,58 @@
+#ifndef IK_CONSTRAINT_H
+#define IK_CONSTRAINT_H
+
+#include "ik/config.h"
+
+C_HEADER_BEGIN
+
+typedef void (*ik_constraint_apply_func)(ik_node_t*);
+
+typedef enum ik_constraint_type_e
+{
+    IK_CONSTRAINT_STIFF,
+    IK_CONSTRAINT_HINGE,
+    IK_CONSTRAINT_CONE
+} ik_constraint_type_e;
+
+struct ik_constraint_t
+{
+    ik_constraint_type_e type;
+    ik_constraint_apply_func apply;
+};
+
+/*!
+ * @brief Creates a new constraint object. It can be attached to any node in the
+ * tree using ik_node_attach_constraint().
+ */
+IK_PUBLIC_API ik_constraint_t*
+ik_constraint_create(ik_constraint_type_e constraint_type);
+
+/*!
+ * @brief Sets the type of constraint to enforce.
+ * @note The tree must be rebuilt only if you change to or from the "stiff"
+ * constraint (IK_CONSTRAINT_STIFF). Switching to any other constraint does not
+ * require a rebuild. The reason for this is because the stiff constraint
+ * causes the node to be excluded entirely from the chain tree, and determining
+ * this requires a rebuild.
+ */
+IK_PUBLIC_API void
+ik_constraint_set(ik_constraint_t* constraint, ik_constraint_type_e constraint_type);
+
+/*!
+ * @brief Allows the user to specify a custom callback function for enforcing
+ * a constraint.
+ */
+IK_PUBLIC_API void
+ik_constraint_set_custom(ik_constraint_t* constraint, ik_constraint_apply_func callback);
+
+/*!
+ * @brief Destroys and frees a constraint object. This should **NOT** be called
+ * on constraints that are attached to nodes. Use ik_node_destroy_constraint()
+ * instead.
+ */
+IK_PUBLIC_API void
+ik_constraint_destroy(ik_constraint_t* constraint);
+
+C_HEADER_END
+
+#endif /* IK_CONSTRAINT_H */

Source/ThirdParty/ik/include/ik/effector.h

@@ -7,9 +7,7 @@
 
 C_HEADER_BEGIN
 
-struct ik_node_t;
-
-enum effector_flags_e
+typedef enum effector_flags_e
 {
     /*!
      * @brief Causes intermediary weight values to rotate the target around the
@@ -17,10 +15,8 @@ enum effector_flags_e
      * appealing if the solved tree diverges a lot from the original tree
      * during weight transitions.
      */
-    EFFECTOR_WEIGHT_NLERP            = 0x01,
-
-    EFFECTOR_INHERIT_PARENT_ROTATION = 0x02
-};
+    EFFECTOR_WEIGHT_NLERP            = 0x01
+} effector_flgs_e;
 
 /*!
  * @brief Specifies how a chain of nodes should be solved. The effector can
@@ -83,14 +79,14 @@ struct ik_effector_t
  * @brief Creates a new effector object. It can be attached to any node in the
  * tree using ik_node_attach_effector().
  */
-IK_PUBLIC_API struct ik_effector_t*
+IK_PUBLIC_API ik_effector_t*
 ik_effector_create(void);
 
 /*!
  * @brief Constructs a previously allocated effector object.
  */
 IK_PUBLIC_API void
-ik_effector_construct(struct ik_effector_t* effector);
+ik_effector_construct(ik_effector_t* effector);
 
 /*!
  * @brief Destroys and frees an effector object. This should **NOT** be called
@@ -98,7 +94,7 @@ ik_effector_construct(struct ik_effector_t* effector);
  * instead.
  */
 IK_PUBLIC_API void
-ik_effector_destroy(struct ik_effector_t* effector);
+ik_effector_destroy(ik_effector_t* effector);
 
 C_HEADER_END
 

Source/ThirdParty/ik/include/ik/export.h.in

@@ -14,7 +14,7 @@
 #   define IK_EXPORT_H
 
     /* set @BUILD_TYPE@ to SHARED or STATIC */
-#   define IK_@IK_BUILD_TYPE@
+#   define IK_@IK_LIB_TYPE@
 
     /* --------------------------------------------------------------
      * define visibility macros

Source/ThirdParty/ik/include/ik/log.h

@@ -7,14 +7,14 @@ C_HEADER_BEGIN
 
 typedef void (*ik_log_cb_func)(const char*);
 
-enum ik_log_e
+typedef enum ik_log_e
 {
     IK_LOG_NONE,
     IK_LOG_STDOUT
-};
+} ik_log_e;
 
 IK_PUBLIC_API void
-ik_log_init(enum ik_log_e options);
+ik_log_init(ik_log_e options);
 
 IK_PUBLIC_API void
 ik_log_deinit(void);

Source/ThirdParty/ik/include/ik/node.h

@@ -8,8 +8,6 @@
 
 C_HEADER_BEGIN
 
-struct ik_effector_t;
-
 /*!
  * @brief Represents one node in the tree to be solved.
  */
@@ -28,8 +26,8 @@ struct ik_node_t
      * // A node in your scene graph
      * MyNode* node = GetMyNode();
      *
-     * struct ik_solver_t* solver = ik_solver_create(SOLVER_FABRIK);
-     * struct ik_node_t* ikNode = ik_node_create(node->GetID());
+     * ik_solver_t* solver = ik_solver_create(SOLVER_FABRIK);
+     * ik_node_t* ikNode = ik_node_create(node->GetID());
      * ikNode->user_data = node; // Store pointer to your own node object
      *
      * // ---- elsewhere ------
@@ -48,7 +46,7 @@ struct ik_node_t
      * be set and retrieved at any time.
      * @note The default value is (0, 0, 0).
      */
-    vec3_t position;
+    vec3_t initial_position;
 
     /*!
      * @brief The initial global rotation (in world space).
@@ -56,26 +54,28 @@ struct ik_node_t
      * angle computations enabled (SOLVER_CALCULATE_FINAL_ANGLES).
      * @note The default value is the identity quaternion.
      */
-    quat_t rotation;
-
-    /*!
-     * @brief Global identifier for this node. The identifier must be unique
-     * within the tree, but separate trees may re-use the same IDs again. The
-     * ID can later be used to retrieve nodes from the tree again.
-     */
-    uint32_t guid;
+    quat_t initial_rotation;
 
     /*!
      * @brief After the solver is executed, the solved global (world) position
      * is stored here and can be retrieved.
      */
-    vec3_t solved_position;
+    vec3_t position;
 
     /*!
      * @brief After the solver is executed, the solved global (world) rotation
      * is stored here and can be retrieved.
      */
-    quat_t solved_rotation;
+    quat_t rotation;
+
+    /*!
+     * @brief Global identifier for this node. The identifier must be unique
+     * within the tree, but separate trees may re-use the same IDs again. The
+     * ID can later be used to retrieve nodes from the tree again.
+     * @note Don't change this if this node has a parent. If you need to change
+     * the guid then unlink the node, change it, and re-add it as a child.
+     */
+    uint32_t guid;
 
     /*!
      * @brief The end effector object.
@@ -84,33 +84,38 @@ struct ik_node_t
      * node->effector->target_position or node->effector->target_rotation.
      * @note May be NULL.
      */
-    struct ik_effector_t* effector;
+    ik_effector_t* effector;
+
+    ik_constraint_t* constraint;
+
+    ik_real stiffness;
+    ik_real rotation_weight;
 
     /* Private data */
     ik_real segment_length;
-    struct ik_node_t* parent;
-    struct bstv_t children;
+    ik_node_t* parent;
+    bstv_t children;    /* ik_node_t objects */
 };
 
 /*!
  * @brief Creates a new node and returns it. Each node requires a tree-unique
  * ID, which can be used later to search for nodes in the tree.
  */
-IK_PUBLIC_API struct ik_node_t*
+IK_PUBLIC_API ik_node_t*
 ik_node_create(uint32_t guid);
 
 /*!
  * @brief Constructs an already allocated node.
  */
 IK_PUBLIC_API void
-ik_node_construct(struct ik_node_t* node, uint32_t guid);
+ik_node_construct(ik_node_t* node, uint32_t guid);
 
 /*!
  * @brief Destructs a node, destroying all children in the process, but does
  * not deallocate the node object itself.
  */
 IK_PUBLIC_API void
-ik_node_destruct(struct ik_node_t* node);
+ik_node_destruct(ik_node_t* node);
 
 /*!
  * @brief Destructs and frees the node, destroying all children in the process.
@@ -118,7 +123,7 @@ ik_node_destruct(struct ik_node_t* node);
  * @note You will need to rebuild the solver's tree before solving.
  */
 IK_PUBLIC_API void
-ik_node_destroy(struct ik_node_t* node);
+ik_node_destroy(ik_node_t* node);
 
 /*!
  * @brief Attaches a node as a child to another node. The parent node gains
@@ -126,7 +131,7 @@ ik_node_destroy(struct ik_node_t* node);
  * @note You will need to rebuild the solver's tree before solving.
  */
 IK_PUBLIC_API void
-ik_node_add_child(struct ik_node_t* node, struct ik_node_t* child);
+ik_node_add_child(ik_node_t* node, ik_node_t* child);
 
 /*!
  * @brief Unlinks a node from the tree, without destroying anything. All
@@ -135,7 +140,7 @@ ik_node_add_child(struct ik_node_t* node, struct ik_node_t* child);
  * @note You will need to rebuild the solver's tree before solving.
  */
 IK_PUBLIC_API void
-ik_node_unlink(struct ik_node_t* node);
+ik_node_unlink(ik_node_t* node);
 
 /*!
  * @brief Searches recursively for a node in a tree with the specified global
@@ -143,8 +148,8 @@ ik_node_unlink(struct ik_node_t* node);
  * @return Returns NULL if the node was not found, otherwise the node is
  * returned.
  */
-IK_PUBLIC_API struct ik_node_t*
-ik_node_find_child(struct ik_node_t* node, uint32_t guid);
+IK_PUBLIC_API ik_node_t*
+ik_node_find_child(ik_node_t* node, uint32_t guid);
 
 /*!
  * @brief Attaches an effector object to the node. The node gains ownership
@@ -153,22 +158,63 @@ ik_node_find_child(struct ik_node_t* node, uint32_t guid);
  * @note You will need to rebuild the solver's tree before solving.
  */
 IK_PUBLIC_API void
-ik_node_attach_effector(struct ik_node_t* node, struct ik_effector_t* effector);
+ik_node_attach_effector(ik_node_t* node, ik_effector_t* effector);
 
 /*!
- * @brief Removes and destroys the node's effector, if it exists. The attribute
+ * @brief Removes and destroys the node's effector, if it exists. The field
  * node->effector is set to NULL.
  * @note You will need to rebuild the solver's tree before solving.
  */
 IK_PUBLIC_API void
-ik_node_destroy_effector(struct ik_node_t* node);
+ik_node_destroy_effector(ik_node_t* node);
+
+/*!
+ * @brief The constraint is attached to the specified node, but applies to the
+ * parent of this node. In other words, if you wish to constraint the rotation
+ * of node A then you must attach said constraint to the **child** of node A.
+ *
+ * Constraints are a bit strange in how they are stored. They don't apply to
+ * single nodes, rather, they apply to entire segments (edges connecting nodes).
+ * This is not apparent in a single chain of nodes, but becomes apparent if you
+ * consider a tree structure.
+ *
+ *    A   C
+ *     \ /
+ *      B
+ *      |
+ *      D
+ *
+ * If you wanted to constraint the rotation of D, then you would add a
+ * constraint to node B. If you wanted to constraint the rotation of the
+ * segment B-A then you would add a constraint to node A.
+ *
+ * @param[in] node The child of the node you wish to constrain.
+ * @param[in] constraint The constraint object. The node gains ownership of
+ * the constraint and is responsible for its deallocation. If the node already
+ * owns a constraint, then it is first destroyed.
+ */
+IK_PUBLIC_API void
+ik_node_attach_constraint(ik_node_t* node, ik_constraint_t* constraint);
+
+/*!
+ * @brief Removes and destroys the node's constraint, if it exists. The field
+ * node->constraint is set to NULL.
+ */
+IK_PUBLIC_API void
+ik_node_destroy_constraint(ik_node_t* node);
 
 /*!
  * @brief Dumps all nodes recursively to DOT format. You can use graphviz (
  * or other compatible tools) to generate a graphic of the tree.
  */
 IK_PUBLIC_API void
-ik_node_dump_to_dot(struct ik_node_t* node, const char* file_name);
+ik_node_dump_to_dot(ik_node_t* node, const char* file_name);
+
+IK_PUBLIC_API void
+ik_node_global_to_local(ik_node_t* node);
+
+IK_PUBLIC_API void
+ik_node_local_to_global(ik_node_t* node);
 
 C_HEADER_END
 

Source/ThirdParty/ik/include/ik/ordered_vector.h

@@ -30,7 +30,7 @@ struct ordered_vector_t
  * the vector to store. Typically one would pass sizeof(my_data_type).
  * @return Returns the newly created vector object.
  */
-IK_PUBLIC_API struct ordered_vector_t*
+IK_PUBLIC_API ordered_vector_t*
 ordered_vector_create(const uint32_t element_size);
 
 /*!
@@ -42,7 +42,7 @@ ordered_vector_create(const uint32_t element_size);
  * want the vector to store. Typically one would pass sizeof(my_data_type).
  */
 IK_PUBLIC_API void
-ordered_vector_construct(struct ordered_vector_t* vector,
+ordered_vector_construct(ordered_vector_t* vector,
                            const uint32_t element_size);
 
 /*!
@@ -51,7 +51,7 @@ ordered_vector_construct(struct ordered_vector_t* vector,
  * @param[in] vector The vector to destroy.
  */
 IK_PUBLIC_API void
-ordered_vector_destroy(struct ordered_vector_t* vector);
+ordered_vector_destroy(ordered_vector_t* vector);
 
 /*!
  * @brief Erases all elements in a vector.
@@ -61,14 +61,14 @@ ordered_vector_destroy(struct ordered_vector_t* vector);
  * @param[in] vector The vector to clear.
  */
 IK_PUBLIC_API void
-ordered_vector_clear(struct ordered_vector_t* vector);
+ordered_vector_clear(ordered_vector_t* vector);
 
 /*!
  * @brief Erases all elements in a vector and frees their memory.
  * @param[in] vector The vector to clear.
  */
 IK_PUBLIC_API void
-ordered_vector_clear_free(struct ordered_vector_t* vector);
+ordered_vector_clear_free(ordered_vector_t* vector);
 
 /*!
  * @brief Sets the size of the vector to exactly the size specified. If the
@@ -80,7 +80,7 @@ ordered_vector_clear_free(struct ordered_vector_t* vector);
  * @return Returns -1 on failure, 0 on success.
  */
 IK_PUBLIC_API int
-ordered_vector_resize(struct ordered_vector_t* vector, uint32_t size);
+ordered_vector_resize(ordered_vector_t* vector, uint32_t size);
 
 /*!
  * @brief Gets the number of elements that have been inserted into the vector.
@@ -102,7 +102,7 @@ ordered_vector_resize(struct ordered_vector_t* vector, uint32_t size);
  * otherwise.
  */
 IK_PUBLIC_API int
-ordered_vector_push(struct ordered_vector_t* vector, void* data);
+ordered_vector_push(ordered_vector_t* vector, void* data);
 
 /*!
  * @brief Allocates space for a new element at the head of the vector, but does
@@ -116,14 +116,14 @@ ordered_vector_push(struct ordered_vector_t* vector, void* data);
  * warning and use with caution.
  */
 IK_PUBLIC_API void*
-ordered_vector_push_emplace(struct ordered_vector_t* vector);
+ordered_vector_push_emplace(ordered_vector_t* vector);
 
 /*!
  * @brief Copies the contents of another vector and pushes it into the vector.
  * @return Returns 0 if successful, -1 if otherwise.
  */
 IK_PUBLIC_API int
-ordered_vector_push_vector(struct ordered_vector_t* vector, struct ordered_vector_t* source_vector);
+ordered_vector_push_vector(ordered_vector_t* vector, ordered_vector_t* source_vector);
 
 /*!
  * @brief Removes an element from the back (end) of the vector.
@@ -136,7 +136,7 @@ ordered_vector_push_vector(struct ordered_vector_t* vector, struct ordered_vecto
  * If there are no elements to pop, NULL is returned.
  */
 IK_PUBLIC_API void*
-ordered_vector_pop(struct ordered_vector_t* vector);
+ordered_vector_pop(ordered_vector_t* vector);
 
 /*!
  * @brief Returns the very last element of the vector.
@@ -149,7 +149,7 @@ ordered_vector_pop(struct ordered_vector_t* vector);
  * If there are no elements in the vector, NULL is returned.
  */
 IK_PUBLIC_API void*
-ordered_vector_back(const struct ordered_vector_t* vector);
+ordered_vector_back(const ordered_vector_t* vector);
 
 /*!
  * @brief Allocates space for a new element at the specified index, but does
@@ -166,7 +166,7 @@ ordered_vector_back(const struct ordered_vector_t* vector);
  * @return A pointer to the emplaced element. See warning and use with caution.
  */
 IK_PUBLIC_API void*
-ordered_vector_insert_emplace(struct ordered_vector_t* vector, uint32_t index);
+ordered_vector_insert_emplace(ordered_vector_t* vector, uint32_t index);
 
 /*!
  * @brief Inserts (copies) a new element at the specified index.
@@ -181,7 +181,7 @@ ordered_vector_insert_emplace(struct ordered_vector_t* vector, uint32_t index);
  * created. If this is not the case then it could cause undefined behaviour.
  */
 IK_PUBLIC_API int
-ordered_vector_insert(struct ordered_vector_t* vector, uint32_t index, void* data);
+ordered_vector_insert(ordered_vector_t* vector, uint32_t index, void* data);
 
 /*!
  * @brief Erases the specified element from the vector.
@@ -191,7 +191,7 @@ ordered_vector_insert(struct ordered_vector_t* vector, uint32_t index, void* dat
  * ranges from **0** to **ordered_vector_count()-1**.
  */
 IK_PUBLIC_API void
-ordered_vector_erase_index(struct ordered_vector_t* vector, uint32_t index);
+ordered_vector_erase_index(ordered_vector_t* vector, uint32_t index);
 
 /*!
  * @brief Removes the element in the vector pointed to by **element**.
@@ -199,7 +199,7 @@ ordered_vector_erase_index(struct ordered_vector_t* vector, uint32_t index);
  * @param[in] element A pointer to an element within the vector.
  */
 IK_PUBLIC_API void
-ordered_vector_erase_element(struct ordered_vector_t* vector, void* element);
+ordered_vector_erase_element(ordered_vector_t* vector, void* element);
 
 /*!
  * @brief Gets a pointer to the specified element in the vector.
@@ -215,17 +215,17 @@ ordered_vector_erase_element(struct ordered_vector_t* vector, void* element);
  * returned.
  */
 IK_PUBLIC_API void*
-ordered_vector_get_element(struct ordered_vector_t*, uint32_t index);
+ordered_vector_get_element(ordered_vector_t*, uint32_t index);
 
 /*!
  * @brief Convenient macro for iterating a vector's elements.
  *
  * Example:
  * ```
- * ordered_vector_t* some_vector = (a vector containing elements of type "struct bar")
- * ORDERED_VECTOR_FOR_EACH(some_vector, struct bar, element)
+ * ordered_vector_t* some_vector = (a vector containing elements of type "bar")
+ * ORDERED_VECTOR_FOR_EACH(some_vector, bar, element)
  * {
- *     do_something_with(element);  ("element" is now of type "struct bar*")
+ *     do_something_with(element);  ("element" is now of type "bar*")
  * }
  * ```
  * @param[in] vector A pointer to the vector to iterate.
@@ -252,8 +252,8 @@ ordered_vector_get_element(struct ordered_vector_t*, uint32_t index);
  * @brief Convenient macro for iterating a range of a vector's elements.
  * @param[in] vector A pointer to the vector to iterate.
  * @param[in] var_type Should be the type of data stored in the vector. For
- * example, if your vector is storing ```struct type_t*``` objects then
- * var_type should equal ```struct type_t``` (without the pointer).
+ * example, if your vector is storing ```type_t*``` objects then
+ * var_type should equal ```type_t``` (without the pointer).
  * @param[in] var The name of a temporary variable you'd like to use within the
  * for loop to reference the current element.
  * @param[in] begin_index The index (starting at 0) of the first element to
@@ -277,9 +277,9 @@ ordered_vector_get_element(struct ordered_vector_t*, uint32_t index);
  * @warning Only call this while iterating.
  * Example:
  * ```
- * ORDERED_VECTOR_FOR_EACH(some_vector, struct bar, element)
+ * ORDERED_VECTOR_FOR_EACH(some_vector, bar, element)
  * {
- *     ORDERED_VECTOR_ERASE_IN_FOR_LOOP(some_vector, struct bar, element);
+ *     ORDERED_VECTOR_ERASE_IN_FOR_LOOP(some_vector, bar, element);
  * }
  * ```
  * @param[in] vector The vector to erase from.

Source/ThirdParty/ik/include/ik/solver.h

@@ -2,33 +2,31 @@
 #define IK_SOLVER_H
 
 #include "ik/config.h"
+#include "ik/chain_tree.h"
 #include "ik/ordered_vector.h"
-#include "ik/vec3.h"
 #include "ik/quat.h"
+#include "ik/vec3.h"
 
 C_HEADER_BEGIN
 
-struct ik_effector_t;
-struct ik_node_t;
-struct ik_solver_t;
+typedef void (*ik_solver_destruct_func)(ik_solver_t*);
+typedef int (*ik_solver_rebuild_data_func)(ik_solver_t*);
+typedef int (*ik_solver_solve_func)(ik_solver_t*);
 
-typedef void (*ik_solver_destroy_func)(struct ik_solver_t*);
-typedef int (*ik_solver_rebuild_data_func)(struct ik_solver_t*);
-typedef void (*ik_solver_recalculate_segment_lengths_func)(struct ik_solver_t*);
-typedef int (*ik_solver_solve_func)(struct ik_solver_t*);
+typedef void (*ik_solver_iterate_node_cb_func)(ik_node_t*);
 
-typedef void (*ik_solver_apply_constraint_cb_func)(struct ik_node_t*);
-typedef void (*ik_solver_apply_result_cb_func)(struct ik_node_t*);
-
-enum solver_algorithm_e
+typedef enum solver_algorithm_e
 {
-    SOLVER_FABRIK
+    SOLVER_FABRIK,
+    SOLVER_TWO_BONE,
+    SOLVER_ONE_BONE,
+    SOLVER_MSS_LAPLACE
     /* TODO Not implemented
     SOLVER_JACOBIAN_INVERSE,
     SOLVER_JACOBIAN_TRANSPOSE */
-};
+} solver_algorithm_e;
 
-enum solver_flags_e
+typedef enum solver_flags_e
 {
     /*!
      * @brief Causes the root node in the tree to be excluded from the list of
@@ -45,51 +43,29 @@ enum solver_flags_e
      */
     SOLVER_CALCULATE_FINAL_ROTATIONS      = 0x02,
 
-    /* (not yet implemented)
-     * Calculate node angles for each iteration, which may be useful in the
-     * solver->apply_constraint callback function.
-     */
-    SOLVER_CALCULATE_CONSTRAINT_ROTATIONS = 0x04,
-
-    SOLVER_CALCULATE_TARGET_ROTATIONS     = 0x08,
-
-    /*!
-     * @brief The solver will not reset the solved data to its initial state
-     * before solving. The result is a more "continuous" or "ongoing" solution
-     * to the tree, because it will use the previous solved tree as a bases for
-     * solving the next tree.
-     */
-    SOLVER_SKIP_RESET                     = 0x10,
+    SOLVER_ENABLE_CONSTRAINTS             = 0x04,
 
-    /*!
-     * @brief The solver will not call the solver->apply_result callback
-     * function after solving. The results are still calculated. This is useful
-     * if you wish to delay the point at which the solved data is applied. You
-     * can later call ik_solver_iterate_tree() to initiate calls to the
-     * callback function.
-     */
-    SOLVER_SKIP_APPLY                     = 0x20
-};
+    SOLVER_CALCULATE_TARGET_ROTATIONS     = 0x08
+} solver_flags_e;
 
 /*!
- * @brief This is a base struct for all solvers.
+ * @brief This is a base for all solvers.
  */
-#define SOLVER_DATA_HEAD                                             \
-    ik_solver_apply_constraint_cb_func apply_constraint;             \
-    ik_solver_apply_result_cb_func     apply_result;                 \
-                                                                     \
-    int32_t                            max_iterations;               \
-    float                              tolerance;                    \
-    uint8_t                            flags;                        \
-                                                                     \
-    /* Derived structure callbacks */                                \
-    ik_solver_destroy_func             destroy;                      \
-    ik_solver_rebuild_data_func        rebuild_data;                 \
-    ik_solver_recalculate_segment_lengths_func recalculate_segment_lengths; \
-    ik_solver_solve_func               solve;                        \
-                                                                     \
-    struct ordered_vector_t            effector_nodes_list;          \
-    struct ik_node_t*                  tree;
+#define SOLVER_DATA_HEAD                                              \
+    int32_t                             max_iterations;               \
+    float                               tolerance;                    \
+    uint8_t                             flags;                        \
+                                                                      \
+    /* Derived structure callbacks */                                 \
+    ik_solver_destruct_func             destruct;                     \
+    ik_solver_rebuild_data_func         rebuild_data;                 \
+    ik_solver_solve_func                solve;                        \
+                                                                      \
+    ordered_vector_t                    effector_nodes_list;          \
+    ik_node_t*                          tree;                         \
+    /* list of ik_chain_tree_t objects (allocated in-place) */        \
+    chain_tree_t                        chain_tree;
+
 struct ik_solver_t
 {
     SOLVER_DATA_HEAD
@@ -138,15 +114,15 @@ struct ik_solver_t
  * @param[in] algorithm The algorithm to use. Currently, only FABRIK is
  * supported.
  */
-IK_PUBLIC_API struct ik_solver_t*
-ik_solver_create(enum solver_algorithm_e algorithm);
+IK_PUBLIC_API ik_solver_t*
+ik_solver_create(solver_algorithm_e algorithm);
 
 /*!
  * @brief Destroys the solver and all nodes/effectors that are part of the
  * solver. Any pointers to tree nodes are invalid after this function returns.
  */
 IK_PUBLIC_API void
-ik_solver_destroy(struct ik_solver_t* solver);
+ik_solver_destroy(ik_solver_t* solver);
 
 /*!
  * @brief Sets the tree to solve. The solver takes ownership of the tree, so
@@ -155,7 +131,7 @@ ik_solver_destroy(struct ik_solver_t* solver);
  * solver already has a tree, then said tree will be destroyed.
  */
 IK_PUBLIC_API void
-ik_solver_set_tree(struct ik_solver_t* solver, struct ik_node_t* root);
+ik_solver_set_tree(ik_solver_t* solver, ik_node_t* root);
 
 /*!
  * @brief The solver releases any references to a previously set tree and
@@ -163,15 +139,15 @@ ik_solver_set_tree(struct ik_solver_t* solver, struct ik_node_t* root);
  * tree (e.g. solve or rebuild) will have no effect until a new tree is set.
  * @return If the solver has no tree then NULL is returned.
  */
-IK_PUBLIC_API struct ik_node_t*
-ik_solver_unlink_tree(struct ik_solver_t* solver);
+IK_PUBLIC_API ik_node_t*
+ik_solver_unlink_tree(ik_solver_t* solver);
 
 /*!
  * @brief The solver releases any references to a previously set tree and
  * destroys it.
  */
 IK_PUBLIC_API void
-ik_solver_destroy_tree(struct ik_solver_t* solver);
+ik_solver_destroy_tree(ik_solver_t* solver);
 
 /*!
  * @brief Causes the set tree to be processed into more optimal data structures
@@ -181,7 +157,7 @@ ik_solver_destroy_tree(struct ik_solver_t* solver);
  * you must call this again before calling the solver.
  */
 IK_PUBLIC_API int
-ik_solver_rebuild_data(struct ik_solver_t* solver);
+ik_solver_rebuild_data(ik_solver_t* solver);
 
 /*!
  * @brief Unusual, but if you have a tree with translational motions such that
@@ -191,7 +167,7 @@ ik_solver_rebuild_data(struct ik_solver_t* solver);
  * @note This function gets called by ik_solver_rebuild_data().
  */
 IK_PUBLIC_API void
-ik_solver_recalculate_segment_lengths(struct ik_solver_t* solver);
+ik_solver_recalculate_segment_lengths(ik_solver_t* solver);
 
 /*!
  * @brief Solves the IK problem. The node solutions will be provided via a
@@ -199,27 +175,22 @@ ik_solver_recalculate_segment_lengths(struct ik_solver_t* solver);
  * solver->apply_result.
  */
 IK_PUBLIC_API int
-ik_solver_solve(struct ik_solver_t* solver);
+ik_solver_solve(ik_solver_t* solver);
 
 /*!
- * @brief Iterates all nodes in the internal tree, breadth first, and calls the
- * solver->apply_result callback function for every node.
- *
- * This gets called automatically for you by ik_solver_solve() if
- * SOLVER_SKIP_APPLY is **not** set. This function could also be used to reset
- * your own scene graph to its initial state by reading the node->position and
- * node->rotation properties.
+ * @brief Iterates all nodes in the internal tree, breadth first, and passes
+ * each node to the specified callback function.
  */
 IK_PUBLIC_API void
-ik_solver_iterate_tree(struct ik_solver_t* solver);
+ik_solver_iterate_tree(ik_solver_t* solver,
+                       ik_solver_iterate_node_cb_func callback);
 
 /*!
  * @brief Sets the solved positions and rotations equal to the original
- * positions and rotations for every node in the tree. The solver will call
- * this automatically if SOLVER_SKIP_RESET is **not** set.
+ * positions and rotations for every node in the tree.
  */
 IK_PUBLIC_API void
-ik_solver_reset_solved_data(struct ik_solver_t* solver);
+ik_solver_reset_solved_data(ik_solver_t* solver);
 
 C_HEADER_END
 

Source/ThirdParty/ik/include/ik/solver_1bone.h

@@ -0,0 +1,29 @@
+#ifndef IK_SOLVER_1BONE_H
+#define IK_SOLVER_1BONE_H
+
+#include "ik/config.h"
+#include "ik/ordered_vector.h"
+#include "ik/solver.h"
+
+C_HEADER_BEGIN
+
+struct one_bone_t
+{
+    SOLVER_DATA_HEAD
+};
+
+int
+solver_1bone_construct(ik_solver_t* solver);
+
+void
+solver_1bone_destruct(ik_solver_t* solver);
+
+int
+solver_1bone_rebuild(ik_solver_t* solver);
+
+int
+solver_1bone_solve(ik_solver_t* solver);
+
+C_HEADER_END
+
+#endif /* IK_SOLVER_1BONE_H */

Source/ThirdParty/ik/include/ik/solver_2bone.h

@@ -0,0 +1,29 @@
+#ifndef IK_SOLVER_2BONE_H
+#define IK_SOLVER_2BONE_H
+
+#include "ik/config.h"
+#include "ik/ordered_vector.h"
+#include "ik/solver.h"
+
+C_HEADER_BEGIN
+
+struct two_bone_t
+{
+    SOLVER_DATA_HEAD
+};
+
+int
+solver_2bone_construct(ik_solver_t* solver);
+
+void
+solver_2bone_destruct(ik_solver_t* solver);
+
+int
+solver_2bone_rebuild(ik_solver_t* solver);
+
+int
+solver_2bone_solve(ik_solver_t* solver);
+
+C_HEADER_END
+
+#endif /* IK_SOLVER_2BONE_H */

Source/ThirdParty/ik/include/ik/solver_FABRIK.h

@@ -7,32 +7,19 @@
 
 C_HEADER_BEGIN
 
-struct chain_t
-{
-    struct ordered_vector_t nodes;    /* list of node_t* references */
-    struct ordered_vector_t children; /* list of chain_t objects */
-};
-
 struct fabrik_t
 {
     SOLVER_DATA_HEAD
-    struct chain_t* chain_tree;
 };
 
-struct ik_solver_t*
-solver_FABRIK_create(void);
-
-void
-solver_FABRIK_destroy(struct ik_solver_t* solver);
-
 int
-solver_FABRIK_rebuild_data(struct ik_solver_t* solver);
+solver_FABRIK_construct(ik_solver_t* solver);
 
 void
-solver_FABRIK_recalculate_segment_lengths(struct ik_solver_t* solver);
+solver_FABRIK_destruct(ik_solver_t* solver);
 
 int
-solver_FABRIK_solve(struct ik_solver_t* solver);
+solver_FABRIK_solve(ik_solver_t* solver);
 
 C_HEADER_END
 

Source/ThirdParty/ik/include/ik/solver_MSS.h

@@ -0,0 +1,20 @@
+#include "ik/config.h"
+#include "ik/solver.h"
+
+C_HEADER_BEGIN
+
+struct mss_t
+{
+    SOLVER_DATA_HEAD
+};
+
+int
+solver_MSS_construct(ik_solver_t* solver);
+
+void
+solver_MSS_destruct(ik_solver_t* solver);
+
+int
+solver_MSS_solve(ik_solver_t* solver);
+
+C_HEADER_END

Source/ThirdParty/ik/include/ik/types.h

@@ -0,0 +1,21 @@
+#ifndef IK_TYPES_H
+#define IK_TYPES_H
+
+/* IK specific types */
+typedef struct chain_t chain_t;
+typedef struct chain_island_t chain_island_t;
+typedef struct chain_tree_t chain_tree_t;
+typedef struct ik_constraint_t ik_constraint_t;
+typedef struct ik_effector_t ik_effector_t;
+typedef struct ik_node_t ik_node_t;
+typedef struct ik_solver_t ik_solver_t;
+typedef struct fabrik_t fabrik_t;
+typedef struct two_bone_t two_bone_t;
+typedef struct one_bone_t one_bone_t;
+typedef struct mss_t mss_t;
+
+/* containers */
+typedef struct ordered_vector_t ordered_vector_t;
+typedef struct bstv_t bstv_t;
+
+#endif /* IK_TYPES_H */

Source/ThirdParty/ik/include/ik/util.h

@@ -0,0 +1,13 @@
+#ifndef IK_UTIL_H
+#define IK_UTIL_H
+
+#include "ik/config.h"
+
+C_HEADER_BEGIN
+
+IK_PUBLIC_API void
+ik_calculate_rotation_weight_decays(chain_tree_t* chain_tree);
+
+C_HEADER_END
+
+#endif /* IK_UTIL_H */

Source/ThirdParty/ik/include/ik/vec3.h

@@ -45,6 +45,17 @@ vec3_dot(const ik_real* v1, const ik_real* v2);
 IK_PUBLIC_API void
 vec3_cross(ik_real* v1, const ik_real* v2);
 
+/*!
+ * @brief Calculates the angle between two vectors. If the angle is 0 or 180,
+ * the delta rotation is set to identity.
+ * @param[out] q A contiguous array of 4 ik_floats representing a quaternion.
+ * The result is written to this. Any previous data is overwritten.
+ * @param[in] v1 The first vector.
+ * @param[in] v2 The second vector.
+ */
+IK_PUBLIC_API void
+vec3_angle(ik_real* q, const ik_real* v1, const ik_real* v2);
+
 C_HEADER_END
 
 #endif /* VEC3_H */

Source/ThirdParty/ik/src/bst_vector.c

@@ -6,11 +6,11 @@
 const uint32_t BST_VECTOR_INVALID_HASH = (uint32_t)-1;
 
 /* ------------------------------------------------------------------------- */
-struct bstv_t*
+bstv_t*
 bstv_create(void)
 {
-    struct bstv_t* bstv;
-    if(!(bstv = (struct bstv_t*)MALLOC(sizeof *bstv)))
+    bstv_t* bstv;
+    if (!(bstv = (bstv_t*)MALLOC(sizeof *bstv)))
         return NULL;
     bstv_construct(bstv);
     return bstv;
@@ -18,15 +18,15 @@ bstv_create(void)
 
 /* ------------------------------------------------------------------------- */
 void
-bstv_construct(struct bstv_t* bstv)
+bstv_construct(bstv_t* bstv)
 {
     assert(bstv);
-    ordered_vector_construct(&bstv->vector, sizeof(struct bstv_hash_value_t));
+    ordered_vector_construct(&bstv->vector, sizeof(bstv_hash_value_t));
 }
 
 /* ------------------------------------------------------------------------- */
 void
-bstv_destroy(struct bstv_t* bstv)
+bstv_destroy(bstv_t* bstv)
 {
     assert(bstv);
     bstv_clear_free(bstv);
@@ -36,28 +36,28 @@ bstv_destroy(struct bstv_t* bstv)
 /* ------------------------------------------------------------------------- */
 /* algorithm taken from GNU GCC stdlibc++'s lower_bound function, line 2121 in stl_algo.h */
 /* https://gcc.gnu.org/onlinedocs/libstdc++/libstdc++-html-USERS-4.3/a02014.html */
-static struct bstv_hash_value_t*
-bstv_find_lower_bound(const struct bstv_t* bstv, uint32_t hash)
+static bstv_hash_value_t*
+bstv_find_lower_bound(const bstv_t* bstv, uint32_t hash)
 {
     uint32_t half;
-    struct bstv_hash_value_t* middle;
-    struct bstv_hash_value_t* data;
+    bstv_hash_value_t* middle;
+    bstv_hash_value_t* data;
     uint32_t len;
 
     assert(bstv);
 
-    data = (struct bstv_hash_value_t*)bstv->vector.data;
+    data = (bstv_hash_value_t*)bstv->vector.data;
     len = bstv->vector.count;
 
-    /* if the vector has no data, return NULL */
-    if(!len)
+    /* if (the vector has no data, return NULL */
+    if (!len)
         return NULL;
 
-    while(len > 0)
+    while (len > 0)
     {
         half = len >> 1;
         middle = data + half;
-        if(middle->hash < hash)
+        if (middle->hash < hash)
         {
             data = middle;
             ++data;
@@ -67,8 +67,8 @@ bstv_find_lower_bound(const struct bstv_t* bstv, uint32_t hash)
             len = half;
     }
 
-    /* if "data" is pointing outside of the valid elements in the vector, also return NULL */
-    if((intptr_t)data >= (intptr_t)bstv->vector.data + (intptr_t)bstv->vector.count * (intptr_t)bstv->vector.element_size)
+    /* if ("data" is pointing outside of the valid elements in the vector, also return NULL */
+    if ((intptr_t)data >= (intptr_t)bstv->vector.data + (intptr_t)bstv->vector.count * (intptr_t)bstv->vector.element_size)
         return NULL;
     else
         return data;
@@ -76,31 +76,31 @@ bstv_find_lower_bound(const struct bstv_t* bstv, uint32_t hash)
 
 /* ------------------------------------------------------------------------- */
 int
-bstv_insert(struct bstv_t* bstv, uint32_t hash, void* value)
+bstv_insert(bstv_t* bstv, uint32_t hash, void* value)
 {
-    struct bstv_hash_value_t* emplaced_data;
-    struct bstv_hash_value_t* lower_bound;
+    bstv_hash_value_t* emplaced_data;
+    bstv_hash_value_t* lower_bound;
 
     assert(bstv);
 
     /* don't insert reserved hashes */
-    if(hash == BST_VECTOR_INVALID_HASH)
+    if (hash == BST_VECTOR_INVALID_HASH)
         return -1;
 
     /* lookup location in bstv to insert */
     lower_bound = bstv_find_lower_bound(bstv, hash);
-    if(lower_bound && lower_bound->hash == hash)
+    if (lower_bound && lower_bound->hash == hash)
         return 1;
 
     /* either push back or insert, depending on whether there is already data
      * in the bstv */
-    if(!lower_bound)
-        emplaced_data = (struct bstv_hash_value_t*)ordered_vector_push_emplace(&bstv->vector);
+    if (!lower_bound)
+        emplaced_data = (bstv_hash_value_t*)ordered_vector_push_emplace(&bstv->vector);
     else
         emplaced_data = ordered_vector_insert_emplace(&bstv->vector,
-                          lower_bound - (struct bstv_hash_value_t*)bstv->vector.data);
+                          lower_bound - (bstv_hash_value_t*)bstv->vector.data);
 
-    if(!emplaced_data)
+    if (!emplaced_data)
         return -1;
 
     memset(emplaced_data, 0, sizeof *emplaced_data);
@@ -112,20 +112,20 @@ bstv_insert(struct bstv_t* bstv, uint32_t hash, void* value)
 
 /* ------------------------------------------------------------------------- */
 void
-bstv_set(struct bstv_t* bstv, uint32_t hash, void* value)
+bstv_set(bstv_t* bstv, uint32_t hash, void* value)
 {
-    struct bstv_hash_value_t* data;
+    bstv_hash_value_t* data;
 
     assert(bstv);
 
     data = bstv_find_lower_bound(bstv, hash);
-    if(data && data->hash == hash)
+    if (data && data->hash == hash)
         data->value = value;
 }
 
 /* ------------------------------------------------------------------------- */
 void*
-bstv_find(const struct bstv_t* bstv, uint32_t hash)
+bstv_find(const bstv_t* bstv, uint32_t hash)
 {
     void** result = bstv_find_ptr(bstv, hash);
     return result == NULL ? NULL : *result;
@@ -133,26 +133,26 @@ bstv_find(const struct bstv_t* bstv, uint32_t hash)
 
 /* ------------------------------------------------------------------------- */
 void**
-bstv_find_ptr(const struct bstv_t* bstv, uint32_t hash)
+bstv_find_ptr(const bstv_t* bstv, uint32_t hash)
 {
-    struct bstv_hash_value_t* data;
+    bstv_hash_value_t* data;
 
     assert(bstv);
 
     data = bstv_find_lower_bound(bstv, hash);
-    if(!data || data->hash != hash)
+    if (!data || data->hash != hash)
         return NULL;
     return &data->value;
 }
 
 /* ------------------------------------------------------------------------- */
 uint32_t
-bstv_find_element(const struct bstv_t* bstv, const void* value)
+bstv_find_element(const bstv_t* bstv, const void* value)
 {
     assert(bstv);
 
-    ORDERED_VECTOR_FOR_EACH(&bstv->vector, struct bstv_hash_value_t, kv)
-        if(kv->value == value)
+    ORDERED_VECTOR_FOR_EACH(&bstv->vector, bstv_hash_value_t, kv)
+        if (kv->value == value)
             return kv->hash;
     ORDERED_VECTOR_END_EACH
     return BST_VECTOR_INVALID_HASH;
@@ -160,40 +160,40 @@ bstv_find_element(const struct bstv_t* bstv, const void* value)
 
 /* ------------------------------------------------------------------------- */
 void*
-bstv_get_any_element(const struct bstv_t* bstv)
+bstv_get_any_element(const bstv_t* bstv)
 {
-    struct bstv_hash_value_t* kv;
+    bstv_hash_value_t* kv;
     assert(bstv);
-    kv = (struct bstv_hash_value_t*)ordered_vector_back(&bstv->vector);
-    if(kv)
+    kv = (bstv_hash_value_t*)ordered_vector_back(&bstv->vector);
+    if (kv)
         return kv->value;
     return NULL;
 }
 
 /* ------------------------------------------------------------------------- */
 int
-bstv_hash_exists(struct bstv_t* bstv, uint32_t hash)
+bstv_hash_exists(bstv_t* bstv, uint32_t hash)
 {
-    struct bstv_hash_value_t* data;
+    bstv_hash_value_t* data;
 
     assert(bstv);
 
     data = bstv_find_lower_bound(bstv, hash);
-    if(data && data->hash == hash)
+    if (data && data->hash == hash)
         return 0;
     return -1;
 }
 
 /* ------------------------------------------------------------------------- */
 uint32_t
-bstv_find_unused_hash(struct bstv_t* bstv)
+bstv_find_unused_hash(bstv_t* bstv)
 {
     uint32_t i = 0;
 
     assert(bstv);
 
     BSTV_FOR_EACH(bstv, void, key, value)
-        if(i != key)
+        if (i != key)
             break;
         ++i;
     BSTV_END_EACH
@@ -202,15 +202,15 @@ bstv_find_unused_hash(struct bstv_t* bstv)
 
 /* ------------------------------------------------------------------------- */
 void*
-bstv_erase(struct bstv_t* bstv, uint32_t hash)
+bstv_erase(bstv_t* bstv, uint32_t hash)
 {
     void* value;
-    struct bstv_hash_value_t* data;
+    bstv_hash_value_t* data;
 
     assert(bstv);
 
     data = bstv_find_lower_bound(bstv, hash);
-    if(!data || data->hash != hash)
+    if (!data || data->hash != hash)
         return NULL;
 
     value = data->value;
@@ -220,7 +220,7 @@ bstv_erase(struct bstv_t* bstv, uint32_t hash)
 
 /* ------------------------------------------------------------------------- */
 void*
-bstv_erase_element(struct bstv_t* bstv, void* value)
+bstv_erase_element(bstv_t* bstv, void* value)
 {
     void* data;
     uint32_t hash;
@@ -228,7 +228,7 @@ bstv_erase_element(struct bstv_t* bstv, void* value)
     assert(bstv);
 
     hash = bstv_find_element(bstv, value);
-    if(hash == BST_VECTOR_INVALID_HASH)
+    if (hash == BST_VECTOR_INVALID_HASH)
         return NULL;
 
     data = bstv_find_lower_bound(bstv, hash);
@@ -239,14 +239,14 @@ bstv_erase_element(struct bstv_t* bstv, void* value)
 
 /* ------------------------------------------------------------------------- */
 void
-bstv_clear(struct bstv_t* bstv)
+bstv_clear(bstv_t* bstv)
 {
     assert(bstv);
     ordered_vector_clear(&bstv->vector);
 }
 
 /* ------------------------------------------------------------------------- */
-void bstv_clear_free(struct bstv_t* bstv)
+void bstv_clear_free(bstv_t* bstv)
 {
     assert(bstv);
     ordered_vector_clear_free(&bstv->vector);

Source/ThirdParty/ik/src/chain_tree.c

@@ -0,0 +1,543 @@
+#include "ik/bst_vector.h"
+#include "ik/effector.h"
+#include "ik/log.h"
+#include "ik/memory.h"
+#include "ik/node.h"
+#include "ik/ordered_vector.h"
+#include "ik/solver.h"
+#include <assert.h>
+#include <stdio.h>
+
+enum node_marking_e
+{
+    MARK_NONE = 0,
+    MARK_SPLIT,
+    MARK_SECTION
+};
+
+/* ------------------------------------------------------------------------- */
+void
+chain_tree_construct(chain_tree_t* chain_tree)
+{
+    ordered_vector_construct(&chain_tree->islands, sizeof(chain_island_t));
+}
+
+/* ------------------------------------------------------------------------- */
+void
+chain_tree_destruct(chain_tree_t* chain_tree)
+{
+    ORDERED_VECTOR_FOR_EACH(&chain_tree->islands, chain_island_t, island)
+        chain_island_destruct(island);
+    ORDERED_VECTOR_END_EACH
+    ordered_vector_clear_free(&chain_tree->islands);
+}
+
+/* ------------------------------------------------------------------------- */
+void
+chain_island_construct(chain_island_t* chain_tree)
+{
+    chain_construct(&chain_tree->root_chain);
+    ordered_vector_construct(&chain_tree->transform_dependent_nodes, sizeof(ik_node_t*));
+}
+
+/* ------------------------------------------------------------------------- */
+void
+chain_island_destruct(chain_island_t* chain_tree)
+{
+    ordered_vector_clear_free(&chain_tree->transform_dependent_nodes);
+    chain_destruct(&chain_tree->root_chain);
+}
+
+/* ------------------------------------------------------------------------- */
+chain_t*
+chain_create(void)
+{
+    chain_t* chain = (chain_t*)MALLOC(sizeof *chain);
+    if (chain == NULL)
+    {
+        ik_log_message("Failed to allocate chain: out of memory");
+        return NULL;
+    }
+    chain_construct(chain);
+    return chain;
+}
+
+/* ------------------------------------------------------------------------- */
+void
+chain_destroy(chain_t* chain)
+{
+    chain_destruct(chain);
+    FREE(chain);
+}
+
+/* ------------------------------------------------------------------------- */
+void
+chain_construct(chain_t* chain)
+{
+    ordered_vector_construct(&chain->nodes, sizeof(ik_node_t*));
+    ordered_vector_construct(&chain->children, sizeof(chain_t));
+}
+
+/* ------------------------------------------------------------------------- */
+void
+chain_clear_free(chain_t* chain)
+{
+    chain_destruct(chain); /* does the same thing as de*/
+}
+
+/* ------------------------------------------------------------------------- */
+void
+chain_destruct(chain_t* chain)
+{
+    ORDERED_VECTOR_FOR_EACH(&chain->children, chain_t, child_chain)
+        chain_destruct(child_chain);
+    ORDERED_VECTOR_END_EACH
+    ordered_vector_clear_free(&chain->children);
+    ordered_vector_clear_free(&chain->nodes);
+}
+
+/* ------------------------------------------------------------------------- */
+static int
+count_chains_recursive(chain_t* chain)
+{
+    int counter = 1;
+    ORDERED_VECTOR_FOR_EACH(&chain->children, chain_t, child)
+        counter += count_chains_recursive(child);
+    ORDERED_VECTOR_END_EACH
+    return counter;
+}
+int
+count_chains_exclude_root(chain_tree_t* chain_tree)
+{
+    int counter = 1;
+    ORDERED_VECTOR_FOR_EACH(&chain_tree->islands, chain_island_t, island)
+        counter += count_chains_recursive(&island->root_chain);
+    ORDERED_VECTOR_END_EACH
+    return counter - 1; /* exclude root chain */
+}
+
+/* ------------------------------------------------------------------------- */
+static int
+mark_involved_nodes(ik_solver_t* solver, bstv_t* involved_nodes)
+{
+    /*
+     * Traverse the chain of parents starting at each effector node and ending
+     * at the root node of the tree and mark every node on the way. Each
+     * effector specifies a maximum chain length, which means it's possible
+     * that we won't hit the root node.
+     */
+    ordered_vector_t* effector_nodes_list = &solver->effector_nodes_list;
+    ORDERED_VECTOR_FOR_EACH(effector_nodes_list, ik_node_t*, p_effector_node)
+
+        /*
+         * Set up chain length counter. If the chain length is 0 then it is
+         * infinitely long. Set the counter to -1 in this case to skip the
+         * escape condition.
+         */
+        int chain_length_counter;
+        ik_node_t* node = *p_effector_node;
+        assert(node->effector != NULL);
+        chain_length_counter = node->effector->chain_length == 0 ? -1 : (int)node->effector->chain_length;
+
+        /*
+         * Mark nodes that are at the base of the chain differently, so the
+         * chains can be split correctly later. Section markings will overwrite
+         * break markings.
+         *
+         * Additionally, there is a special constraint (IK_CONSTRAINT_STIFF)
+         * that restricts all rotations of a node. If this constraint is
+         * imposed on a particular node, mark it differently as well so the
+         * surrounding nodes can be combined into a single bone properly later.
+         *
+         * NOTE: The node->constraint field specifies constraints for
+         * the *parent* node, not for the current node. However, we will be
+         * marking the *current* node, not the parent node.
+         */
+        for (; node != NULL; node = node->parent)
+        {
+            enum node_marking_e* current_marking;
+            enum node_marking_e marking = MARK_SECTION;
+            if (chain_length_counter == 0)
+                marking = MARK_SPLIT;
+
+            current_marking = (enum node_marking_e*)bstv_find_ptr(involved_nodes, node->guid);
+            if (current_marking == NULL)
+            {
+                if (bstv_insert(involved_nodes, node->guid, (void*)(intptr_t)marking) < 0)
+                {
+                    ik_log_message("Ran out of memory while marking involved nodes");
+                    return -1;
+                }
+            }
+            else
+            {
+                if (chain_length_counter != 0)
+                    *current_marking = marking;
+            }
+
+            if (chain_length_counter-- == 0)
+                break;
+        }
+    ORDERED_VECTOR_END_EACH
+
+    return 0;
+}
+
+/* ------------------------------------------------------------------------- */
+static int
+recursively_build_chain_tree(chain_tree_t* chain_tree,
+                             chain_t* chain_current,
+                             ik_node_t* node_base,
+                             ik_node_t* node_current,
+                             bstv_t* involved_nodes)
+{
+    int marked_children_count;
+    ik_node_t* child_node_base = node_base;
+    chain_t* child_chain = chain_current;
+
+    /* can remove the mark from the set to speed up future checks */
+    enum node_marking_e marking =
+        (enum node_marking_e)(intptr_t)bstv_erase(involved_nodes, node_current->guid);
+
+    switch(marking)
+    {
+        /*
+         * If this node was marked as the base of a chain then split the chain
+         * at this point by moving the pointer to the base node down the tree
+         * to the current node and set the current chain to NULL so a new
+         * island is created (this is necessary because all children of this
+         * node are necessarily part of an isolated tree).
+         */
+        case MARK_SPLIT:
+            child_node_base = node_current;
+            chain_current = NULL;
+            break;
+        /*
+         * If this node is not marked at all, cut off any previous chain but
+         * continue (fall through) as if a section was marked. It's possible
+         * that there are isolated chains somewhere further down the tree.
+         */
+        case MARK_NONE:
+            node_base = node_current;
+            /* falling through on purpose */
+
+        case MARK_SECTION:
+            /*
+             * If the current node has at least two children marked as sections
+             * or if (the current node is an effector node, but only if (the base
+             * node is not equal to this node (that is, we need to avoid chains
+             * that would have less than 2 nodes), then we must also split the
+             * chain at this point.
+             */
+            marked_children_count = 0;
+            BSTV_FOR_EACH(&node_current->children, ik_node_t, child_guid, child)
+                if ((enum node_marking_e)(intptr_t)bstv_find(involved_nodes, child_guid) == MARK_SECTION)
+                    if (++marked_children_count == 2)
+                        break;
+            BSTV_END_EACH
+            if ((marked_children_count == 2 || node_current->effector != NULL) && node_current != node_base)
+            {
+                ik_node_t* node;
+
+                if (chain_current == NULL)
+                {
+                    /*
+                     * If this is the first chain in the island, create and
+                     * initialise it in the chain tree.
+                     */
+                    chain_island_t* island = ordered_vector_push_emplace(&chain_tree->islands);
+                    if (island == NULL)
+                    {
+                        ik_log_message("Failed to create chain island: Ran out of memory");
+                        return -1;
+                    }
+                    chain_island_construct(island);
+                    child_chain = &island->root_chain;
+                }
+                else
+                {
+                    /*
+                     * This is not the first chain of the island, so create a
+                     * new child chain in the current chain and initialise it.
+                     */
+                    child_chain = ordered_vector_push_emplace(&chain_current->children);
+                    if (child_chain == NULL)
+                    {
+                        ik_log_message("Failed to create child chain: Ran out of memory");
+                        return -1;
+                    }
+                    chain_construct(child_chain);
+                }
+
+                /*
+                 * Add pointers to all nodes that are part of this chain into
+                 * the chain's list, starting with the end node.
+                 */
+                for (node = node_current; node != node_base; node = node->parent)
+                    ordered_vector_push(&child_chain->nodes, &node);
+                ordered_vector_push(&child_chain->nodes, &node_base);
+
+                /*
+                 * Update the base node to be this node so deeper chains are built back
+                 * to this node
+                 */
+                child_node_base = node_current;
+            }
+            break;
+    }
+
+    /* Recurse into children of the current node. */
+    BSTV_FOR_EACH(&node_current->children, ik_node_t, child_guid, child_node)
+        if (recursively_build_chain_tree(
+                chain_tree,
+                child_chain,
+                child_node_base,
+                child_node,
+                involved_nodes) < 0)
+            return -1;
+    BSTV_END_EACH
+
+    return 0;
+}
+
+/* ------------------------------------------------------------------------- */
+int
+rebuild_chain_tree(ik_solver_t* solver)
+{
+    bstv_t involved_nodes;
+    int involved_nodes_count;
+#if IK_DOT_OUTPUT == ON
+    char buffer[20];
+    static int file_name_counter = 0;
+#endif
+
+    /* Clear all existing chain trees */
+    ORDERED_VECTOR_FOR_EACH(&solver->chain_tree.islands, chain_island_t, island)
+        chain_island_destruct(island);
+    ORDERED_VECTOR_END_EACH
+    ordered_vector_clear_free(&solver->chain_tree.islands);
+
+    /*
+     * Build a set of all nodes that are in a direct path with all of the
+     * effectors.
+     */
+    bstv_construct(&involved_nodes);
+    if (mark_involved_nodes(solver, &involved_nodes) < 0)
+        goto mark_involved_nodes_failed;
+    involved_nodes_count = bstv_count(&involved_nodes);
+
+    recursively_build_chain_tree(&solver->chain_tree, NULL, solver->tree, solver->tree, &involved_nodes);
+
+    /* Pre-compute offsets for each node in the chain tree in relation to their
+     * parents *
+    calculate_segment_lengths(solver->chain_tree);*/
+
+    /* DEBUG: Save chain tree to DOT */
+#if IK_DOT_OUTPUT == ON
+    sprintf(buffer, "tree%d.dot", file_name_counter++);
+    dump_to_dot(solver->tree, solver->chain_tree, buffer);
+#endif
+
+    ik_log_message("There are %d effector(s) involving %d node(s). %d chain(s) were created",
+                   ordered_vector_count(&solver->effector_nodes_list),
+                   involved_nodes_count,
+                   count_chains_exclude_root(&solver->chain_tree));
+
+    bstv_clear_free(&involved_nodes);
+
+    return 0;
+
+    mark_involved_nodes_failed : bstv_clear_free(&involved_nodes);
+    return -1;
+}
+
+/* ------------------------------------------------------------------------- */
+void
+calculate_segment_lengths(chain_tree_t* chain_tree)
+{
+    /* TODO Fix this
+    int last_idx = ordered_vector_count(&chain->nodes) - 1;
+    while (last_idx-- > 0)
+    {
+        ik_node_t* child_node =
+            *(ik_node_t**)ordered_vector_get_element(&chain->nodes, last_idx + 0);
+        ik_node_t* parent_node =
+            *(ik_node_t**)ordered_vector_get_element(&chain->nodes, last_idx + 1);
+
+        vec3_t diff = child_node->initial_position;
+        vec3_sub_vec3(diff.f, parent_node->initial_position.f);
+        child_node->segment_length = vec3_length(diff.f);
+    }
+
+    ORDERED_VECTOR_FOR_EACH(&chain->children, chain_t, child)
+        calculate_segment_lengths(child);
+    ORDERED_VECTOR_END_EACH*/
+}
+
+/* ------------------------------------------------------------------------- */
+static void
+calculate_global_rotations_of_children(chain_t* chain)
+{
+    int average_count;
+    quat_t average_rotation = {{0, 0, 0, 0}};
+
+    /* Recurse into children chains */
+    average_count = 0;
+    ORDERED_VECTOR_FOR_EACH(&chain->children, chain_t, child)
+        quat_t rotation;
+        calculate_global_rotations(child);
+
+        /* Note: All chains that aren't the root chain *MUST* have at least two nodes */
+        assert(ordered_vector_count(&child->nodes) >= 2);
+        rotation = (*(ik_node_t**)
+                ordered_vector_get_element(&child->nodes,
+                    ordered_vector_count(&child->nodes) - 1))->rotation;
+
+        /*
+         * Averaging quaternions taken from here
+         * http://wiki.unity3d.com/index.php/Averaging_Quaternions_and_Vectors
+         */
+        quat_normalise_sign(rotation.f);
+        quat_add_quat(average_rotation.f, rotation.f);
+        ++average_count;
+    ORDERED_VECTOR_END_EACH
+
+    /*
+     * Assuming there was more than 1 child chain and assuming we aren't the
+     * root node, then the child chains we just iterated must share the same
+     * base node as our tip node. Average the accumulated quaternion and set
+     * this node's correct solved rotation.
+     */
+    if (average_count > 0 && ordered_vector_count(&chain->nodes) != 0)
+    {
+        quat_div_scalar(average_rotation.f, average_count);
+        quat_normalise(average_rotation.f);
+        (*(ik_node_t**)ordered_vector_get_element(&chain->nodes, 0))
+            ->rotation = average_rotation;
+    }
+}
+
+/* ------------------------------------------------------------------------- */
+static void
+calculate_delta_rotation_of_each_segment(chain_t* chain)
+{
+    int node_idx;
+
+    /*
+     * Calculate all of the delta angles of the joints. The resulting delta (!)
+     * angles will be written to node->rotation
+     */
+    node_idx = ordered_vector_count(&chain->nodes) - 1;
+    while (node_idx-- > 0)
+    {
+        ik_node_t* child_node  = *(ik_node_t**)ordered_vector_get_element(&chain->nodes, node_idx + 0);
+        ik_node_t* parent_node = *(ik_node_t**)ordered_vector_get_element(&chain->nodes, node_idx + 1);
+
+        /* calculate vectors for original and solved segments */
+        vec3_t segment_original = child_node->initial_position;
+        vec3_t segment_solved   = child_node->position;
+        vec3_sub_vec3(segment_original.f, parent_node->initial_position.f);
+        vec3_sub_vec3(segment_solved.f, parent_node->position.f);
+
+        vec3_angle(parent_node->rotation.f, segment_original.f, segment_solved.f);
+    }
+}
+
+/* ------------------------------------------------------------------------- */
+void
+calculate_global_rotations(chain_t* chain)
+{
+    int node_idx;
+
+    /*
+     * Calculates the "global" (world) angles of each joint and writes them to
+     * each node->solved_rotation slot.
+     *
+     * The angle between the original and solved segments are calculated using
+     * standard vector math (dot product). The axis of rotation is calculated
+     * with the cross product. From this data, a quaternion is constructed,
+     * describing this delta rotation. Finally, in order to make the rotations
+     * global instead of relative, the delta rotation is multiplied with
+     * node->rotation, which should be a quaternion describing the node's
+     * global rotation in the unsolved tree.
+     *
+     * The rotation of the base joint in the chain is returned so it can be
+     * averaged by parent chains.
+     */
+
+    calculate_global_rotations_of_children(chain);
+    calculate_delta_rotation_of_each_segment(chain);
+
+    /*
+     * At this point, all nodes have calculated their delta angles *except* for
+     * the end effector nodes, which remain untouched. It makes sense to copy
+     * the delta rotation of the parent node into the effector node by default.
+     */
+    node_idx = ordered_vector_count(&chain->nodes);
+    if (node_idx > 1)
+    {
+        ik_node_t* effector_node  = *(ik_node_t**)ordered_vector_get_element(&chain->nodes, 0);
+        ik_node_t* parent_node = *(ik_node_t**)ordered_vector_get_element(&chain->nodes, 1);
+        effector_node->rotation.q = parent_node->rotation.q;
+    }
+
+    /*
+     * Finally, apply initial global rotations to calculated delta rotations to
+     * obtain the solved global rotations.
+     */
+    ORDERED_VECTOR_FOR_EACH(&chain->nodes, ik_node_t*, pnode)
+        ik_node_t* node = *pnode;
+        quat_mul_quat(node->rotation.f, node->initial_rotation.f);
+    ORDERED_VECTOR_END_EACH
+}
+
+/* ------------------------------------------------------------------------- */
+#if IK_DOT_OUTPUT == ON
+static void
+dump_chain(ik_chain_t* chain, FILE* fp)
+{
+    int last_idx = ordered_vector_count(&chain->nodes) - 1;
+    if (last_idx > 0)
+    {
+        fprintf(fp, "    %d [shape=record];\n",
+            (*(ik_node_t**)ordered_vector_get_element(&chain->nodes, 0))->guid);
+        fprintf(fp, "    %d [shape=record];\n",
+            (*(ik_node_t**)ordered_vector_get_element(&chain->nodes, last_idx))->guid);
+    }
+
+    while (last_idx-- > 0)
+    {
+        fprintf(fp, "    %d -- %d [color=\"1.0 0.5 1.0\"];\n",
+            (*(ik_node_t**)ordered_vector_get_element(&chain->nodes, last_idx + 0))->guid,
+            (*(ik_node_t**)ordered_vector_get_element(&chain->nodes, last_idx + 1))->guid);
+    }
+
+    ORDERED_VECTOR_FOR_EACH(&chain->children, ik_chain_t, child)
+        dump_chain(child, fp);
+    ORDERED_VECTOR_END_EACH
+}
+static void
+dump_node(ik_node_t* node, FILE* fp)
+{
+    if (node->effector != NULL)
+        fprintf(fp, "    %d [color=\"0.6 0.5 1.0\"];\n", node->guid);
+    BSTV_FOR_EACH(&node->children, ik_node_t, guid, child)
+        fprintf(fp, "    %d -- %d;\n", node->guid, guid);
+        dump_node(child, fp);
+    BSTV_END_EACH
+}
+void
+dump_to_dot(ik_node_t* node, ik_chain_t* chain, const char* file_name)
+{
+    FILE* fp = fopen(file_name, "w");
+    if (fp == NULL)
+        return;
+
+    fprintf(fp, "graph chain_tree {\n");
+    dump_node(node, fp);
+    dump_chain(chain, fp);
+    fprintf(fp, "}\n");
+
+    fclose(fp);
+}
+#endif

Source/ThirdParty/ik/src/constraint.c

@@ -0,0 +1,94 @@
+#include "ik/constraint.h"
+#include "ik/log.h"
+#include "ik/memory.h"
+#include "ik/node.h"
+#include <string.h>
+#include <assert.h>
+
+static void
+apply_stiff(ik_node_t* node);
+static void
+apply_hinge(ik_node_t* node);
+static void
+apply_cone(ik_node_t* node);
+
+/* ------------------------------------------------------------------------- */
+ik_constraint_t*
+ik_constraint_create(ik_constraint_type_e constraint_type)
+{
+    ik_constraint_t* constraint = (ik_constraint_t*)MALLOC(sizeof *constraint);
+    if (constraint == NULL)
+    {
+        ik_log_message("Failed to allocate constraint: Out of memory");
+        return NULL;
+    }
+
+    memset(constraint, 0, sizeof *constraint);
+    ik_constraint_set(constraint, constraint_type);
+
+    return constraint;
+}
+
+/* ------------------------------------------------------------------------- */
+void
+ik_constraint_set(ik_constraint_t* constraint, ik_constraint_type_e constraint_type)
+{
+    switch (constraint_type)
+    {
+        case IK_CONSTRAINT_STIFF:
+            constraint->apply = apply_stiff;
+            break;
+
+        case IK_CONSTRAINT_HINGE:
+            constraint->apply = apply_hinge;
+            break;
+
+        case IK_CONSTRAINT_CONE:
+            constraint->apply = apply_cone;
+            break;
+    }
+    
+    constraint->type = constraint_type;
+}
+
+/* ------------------------------------------------------------------------- */
+void
+ik_constraint_set_custom(ik_constraint_t* constraint, ik_constraint_apply_func callback)
+{
+    constraint->apply = callback;
+}
+
+/* ------------------------------------------------------------------------- */
+void
+ik_constraint_destroy(ik_constraint_t* constraint)
+{
+    FREE(constraint);
+}
+
+/* ------------------------------------------------------------------------- */
+/* Constraint implementations (static) */
+/* ------------------------------------------------------------------------- */
+
+/* ------------------------------------------------------------------------- */
+static void
+apply_stiff(ik_node_t* node)
+{
+    /*
+     * The stiff constraint should never actually be reached, because joints
+     * that have a stiff constraint will be excluded from the chain tree
+     * entirely. This function exists solely to debug the chain tree.
+     */
+    assert(1);
+}
+
+/* ------------------------------------------------------------------------- */
+static void
+apply_hinge(ik_node_t* node)
+{
+}
+
+/* ------------------------------------------------------------------------- */
+static void
+apply_cone(ik_node_t* node)
+{
+}

Source/ThirdParty/ik/src/effector.c

@@ -4,11 +4,11 @@
 #include <string.h>
 
 /* ------------------------------------------------------------------------- */
-struct ik_effector_t*
+ik_effector_t*
 ik_effector_create(void)
 {
-    struct ik_effector_t* effector = (struct ik_effector_t*)MALLOC(sizeof *effector);
-    if(effector == NULL)
+    ik_effector_t* effector = (ik_effector_t*)MALLOC(sizeof *effector);
+    if (effector == NULL)
         return NULL;
 
     ik_effector_construct(effector);
@@ -17,7 +17,7 @@ ik_effector_create(void)
 
 /* ------------------------------------------------------------------------- */
 void
-ik_effector_construct(struct ik_effector_t* effector)
+ik_effector_construct(ik_effector_t* effector)
 {
     memset(effector, 0, sizeof *effector);
     quat_set_identity(effector->target_rotation.f);
@@ -28,17 +28,7 @@ ik_effector_construct(struct ik_effector_t* effector)
 
 /* ------------------------------------------------------------------------- */
 void
-ik_effector_destroy(struct ik_effector_t* effector)
+ik_effector_destroy(ik_effector_t* effector)
 {
     FREE(effector);
 }
-
-/* ------------------------------------------------------------------------- */
-void
-effector_attach(struct ik_effector_t* effector, struct ik_node_t* node)
-{
-    if(node->effector != NULL)
-        ik_effector_destroy(node->effector);
-
-    node->effector = effector;
-}

Source/ThirdParty/ik/src/log.c

@@ -5,11 +5,13 @@
 #include <string.h>
 #include <stdio.h>
 
-static struct log_t
+typedef struct log_t
 {
-    struct ordered_vector_t listeners; /* list of ik_log_cb_func */
-    struct ordered_vector_t message_buffer;
-}* g_log = NULL;
+    ordered_vector_t listeners; /* list of ik_log_cb_func */
+    ordered_vector_t message_buffer;
+} log_t;
+
+static log_t* g_log = NULL;
 
 static void log_stdout_callback(const char* msg)
 {
@@ -20,17 +22,17 @@ static void log_stdout_callback(const char* msg)
 void
 ik_log_init(enum ik_log_e options)
 {
-    if(g_log != NULL)
+    if (g_log != NULL)
         return;
 
-    g_log = (struct log_t*)MALLOC(sizeof *g_log);
-    if(g_log == NULL)
+    g_log = (log_t*)MALLOC(sizeof *g_log);
+    if (g_log == NULL)
         return;
 
     ordered_vector_construct(&g_log->listeners, sizeof(ik_log_cb_func));
     ordered_vector_construct(&g_log->message_buffer, sizeof(char));
 
-    if(options == IK_LOG_STDOUT)
+    if (options == IK_LOG_STDOUT)
         ik_log_register_listener(log_stdout_callback);
 }
 
@@ -38,7 +40,7 @@ ik_log_init(enum ik_log_e options)
 void
 ik_log_deinit(void)
 {
-    if(g_log == NULL)
+    if (g_log == NULL)
         return;
 
     ordered_vector_clear_free(&g_log->message_buffer);
@@ -51,7 +53,7 @@ ik_log_deinit(void)
 void
 ik_log_register_listener(ik_log_cb_func callback)
 {
-    if(g_log != NULL)
+    if (g_log != NULL)
         ordered_vector_push(&g_log->listeners, &callback);
 }
 
@@ -59,11 +61,11 @@ ik_log_register_listener(ik_log_cb_func callback)
 void
 ik_log_unregister_listener(ik_log_cb_func callback)
 {
-    if(g_log == NULL)
+    if (g_log == NULL)
         return;
 
     ORDERED_VECTOR_FOR_EACH(&g_log->listeners, ik_log_cb_func, registered_callback)
-        if(callback == *registered_callback)
+        if (callback == *registered_callback)
         {
             ordered_vector_erase_element(&g_log->listeners, registered_callback);
             return;
@@ -78,14 +80,14 @@ ik_log_message(const char* fmt, ...)
     va_list va;
     uintptr_t msg_len;
 
-    if(g_log == NULL)
+    if (g_log == NULL)
         return;
 
     va_start(va, fmt);
     msg_len = vsnprintf(NULL, 0, fmt, va);
     va_end(va);
 
-    if(ordered_vector_resize(&g_log->message_buffer, (msg_len + 1) * sizeof(char)) < 0)
+    if (ordered_vector_resize(&g_log->message_buffer, (msg_len + 1) * sizeof(char)) < 0)
         return;
     va_start(va, fmt);
     vsprintf((char*)g_log->message_buffer.data, fmt, va);

Source/ThirdParty/ik/src/memory.c

@@ -12,9 +12,9 @@
 static uintptr_t g_allocations = 0;
 static uintptr_t d_deg_allocations = 0;
 static uintptr_t g_ignore_bstv_malloc = 0;
-static struct bstv_t report;
+static bstv_t report;
 
-struct report_info_t
+typedef struct report_info_t
 {
     uintptr_t location;
     uintptr_t size;
@@ -22,7 +22,7 @@ struct report_info_t
     int backtrace_size;
     char** backtrace;
 #   endif
-};
+} report_info_t;
 
 /* ------------------------------------------------------------------------- */
 void
@@ -47,14 +47,14 @@ void*
 malloc_wrapper(intptr_t size)
 {
     void* p = NULL;
-    struct report_info_t* info = NULL;
+    report_info_t* info = NULL;
 
     /* breaking from this will clean up and return NULL */
-    for(;;)
+    for (;;)
     {
         /* allocate */
         p = malloc(size);
-        if(p)
+        if (p)
             ++g_allocations;
         else
             break;
@@ -63,11 +63,11 @@ malloc_wrapper(intptr_t size)
         * Record allocation info. Call to bstv may allocate memory,
         * so set flag to ignore the call to malloc() when inserting.
         */
-        if(!g_ignore_bstv_malloc)
+        if (!g_ignore_bstv_malloc)
         {
             g_ignore_bstv_malloc = 1;
-            info = (struct report_info_t*)malloc(sizeof(struct report_info_t));
-            if(!info)
+            info = (report_info_t*)malloc(sizeof(report_info_t));
+            if (!info)
             {
                 fprintf(stderr, "[memory] ERROR: malloc() for report_info_t failed"
                     " -- not enough memory.\n");
@@ -79,15 +79,15 @@ malloc_wrapper(intptr_t size)
             info->location = (uintptr_t)p;
             info->size = size;
 
-            /* if enabled, generate a backtrace so we know where memory leaks
+            /* if (enabled, generate a backtrace so we know where memory leaks
             * occurred */
 #   if IK_MEMORY_BACKTRACE == ON
-            if(!(info->backtrace = get_backtrace(&info->backtrace_size)))
+            if (!(info->backtrace = get_backtrace(&info->backtrace_size)))
                 fprintf(stderr, "[memory] WARNING: Failed to generate backtrace\n");
 #   endif
 
             /* insert into bstv */
-            if(bstv_insert(&report, (uintptr_t)p, info) == 1)
+            if (bstv_insert(&report, (uintptr_t)p, info) == 1)
             {
                 fprintf(stderr,
                 "[memory] WARNING: Hash collision occurred when inserting\n"
@@ -101,10 +101,10 @@ malloc_wrapper(intptr_t size)
                 {
                     char** bt;
                     int bt_size, i;
-                    if((bt = get_backtrace(&bt_size)))
+                    if ((bt = get_backtrace(&bt_size)))
                     {
                         printf("  backtrace to where malloc() was called:\n");
-                        for(i = 0; i < bt_size; ++i)
+                        for (i = 0; i < bt_size; ++i)
                             printf("      %s\n", bt[i]);
                         printf("  -----------------------------------------\n");
                         free(bt);
@@ -122,16 +122,16 @@ malloc_wrapper(intptr_t size)
     }
 
     /* failure */
-    if(p)
+    if (p)
     {
         free(p);
         --g_allocations;
     }
 
-    if(info)
+    if (info)
     {
 #   if IK_MEMORY_BACKTRACE == ON
-        if(info->backtrace)
+        if (info->backtrace)
             free(info->backtrace);
 #   endif
         free(info);
@@ -145,13 +145,13 @@ void
 free_wrapper(void* ptr)
 {
     /* find matching allocation and remove from bstv */
-    if(!g_ignore_bstv_malloc)
+    if (!g_ignore_bstv_malloc)
     {
-        struct report_info_t* info = (struct report_info_t*)bstv_erase(&report, (uintptr_t)ptr);
-        if(info)
+        report_info_t* info = (report_info_t*)bstv_erase(&report, (uintptr_t)ptr);
+        if (info)
         {
 #   if IK_MEMORY_BACKTRACE == ON
-            if(info->backtrace)
+            if (info->backtrace)
                 free(info->backtrace);
             else
                 fprintf(stderr, "[memory] WARNING: free(): Allocation didn't "
@@ -168,10 +168,10 @@ free_wrapper(void* ptr)
 #   endif
             fprintf(stderr, "  WARNING: Freeing something that was never allocated\n");
 #   if IK_MEMORY_BACKTRACE == ON
-            if((bt = get_backtrace(&bt_size)))
+            if ((bt = get_backtrace(&bt_size)))
             {
                 fprintf(stderr, "  backtrace to where free() was called:\n");
-                for(i = 0; i < bt_size; ++i)
+                for (i = 0; i < bt_size; ++i)
                     fprintf(stderr, "      %s\n", bt[i]);
                 fprintf(stderr, "  -----------------------------------------\n");
                 free(bt);
@@ -182,7 +182,7 @@ free_wrapper(void* ptr)
         }
     }
 
-    if(ptr)
+    if (ptr)
     {
         ++d_deg_allocations;
         free(ptr);
@@ -204,9 +204,9 @@ ik_memory_deinit(void)
     printf("=========================================\n");
 
     /* report details on any g_allocations that were not de-allocated */
-    if(report.vector.count != 0)
+    if (report.vector.count != 0)
     {
-        BSTV_FOR_EACH(&report, struct report_info_t, key, info)
+        BSTV_FOR_EACH(&report, report_info_t, key, info)
 
             printf("  un-freed memory at %p, size %p\n", (void*)info->location, (void*)info->size);
             mutated_string_and_hex_dump((void*)info->location, info->size);
@@ -215,7 +215,7 @@ ik_memory_deinit(void)
             printf("  Backtrace to where malloc() was called:\n");
             {
                 intptr_t i;
-                for(i = BACKTRACE_OMIT_COUNT; i < info->backtrace_size; ++i)
+                for (i = BACKTRACE_OMIT_COUNT; i < info->backtrace_size; ++i)
                     printf("      %s\n", info->backtrace[i]);
             }
             free(info->backtrace); /* this was allocated when malloc() was called */
@@ -257,7 +257,7 @@ mutated_string_and_hex_dump(void* data, intptr_t length_in_bytes)
     intptr_t i;
 
     /* allocate and copy data into new buffer */
-    if(!(dump = malloc(length_in_bytes + 1)))
+    if (!(dump = malloc(length_in_bytes + 1)))
     {
         fprintf(stderr, "[memory] WARNING: Failed to malloc() space for dump\n");
         return;
@@ -266,14 +266,14 @@ mutated_string_and_hex_dump(void* data, intptr_t length_in_bytes)
     dump[length_in_bytes] = '\0';
 
     /* mutate null terminators into dots */
-    for(i = 0; i != length_in_bytes; ++i)
-        if(dump[i] == '\0')
+    for (i = 0; i != length_in_bytes; ++i)
+        if (dump[i] == '\0')
             dump[i] = '.';
 
     /* dump */
     printf("  mutated string dump: %s\n", dump);
     printf("  hex dump: ");
-    for(i = 0; i != length_in_bytes; ++i)
+    for (i = 0; i != length_in_bytes; ++i)
         printf(" %02x", (unsigned char)dump[i]);
     printf("\n");
 

Source/ThirdParty/ik/src/node.c

@@ -1,3 +1,4 @@
+#include "ik/constraint.h"
 #include "ik/effector.h"
 #include "ik/log.h"
 #include "ik/memory.h"
@@ -7,11 +8,11 @@
 #include <stdio.h>
 
 /* ------------------------------------------------------------------------- */
-struct ik_node_t*
+ik_node_t*
 ik_node_create(uint32_t guid)
 {
-    struct ik_node_t* node = (struct ik_node_t*)MALLOC(sizeof *node);
-    if(node == NULL)
+    ik_node_t* node = (ik_node_t*)MALLOC(sizeof *node);
+    if (node == NULL)
         return NULL;
 
     ik_node_construct(node, guid);
@@ -20,39 +21,43 @@ ik_node_create(uint32_t guid)
 
 /* ------------------------------------------------------------------------- */
 void
-ik_node_construct(struct ik_node_t* node, uint32_t guid)
+ik_node_construct(ik_node_t* node, uint32_t guid)
 {
     memset(node, 0, sizeof *node);
     bstv_construct(&node->children);
+    quat_set_identity(node->initial_rotation.f);
     quat_set_identity(node->rotation.f);
-    quat_set_identity(node->solved_rotation.f);
     node->guid = guid;
 }
 
 /* ------------------------------------------------------------------------- */
 static void
-ik_node_destroy_recursive(struct ik_node_t* node);
+ik_node_destroy_recursive(ik_node_t* node);
 static void
-ik_node_destruct_recursive(struct ik_node_t* node)
+ik_node_destruct_recursive(ik_node_t* node)
 {
-    BSTV_FOR_EACH(&node->children, struct ik_node_t, guid, child)
+    BSTV_FOR_EACH(&node->children, ik_node_t, guid, child)
         ik_node_destroy_recursive(child);
     BSTV_END_EACH
 
-    if(node->effector)
+    if (node->effector)
         ik_effector_destroy(node->effector);
+    if (node->constraint)
+        ik_constraint_destroy(node->constraint);
 
     bstv_clear_free(&node->children);
 }
 void
-ik_node_destruct(struct ik_node_t* node)
+ik_node_destruct(ik_node_t* node)
 {
-    BSTV_FOR_EACH(&node->children, struct ik_node_t, guid, child)
+    BSTV_FOR_EACH(&node->children, ik_node_t, guid, child)
         ik_node_destroy_recursive(child);
     BSTV_END_EACH
 
-    if(node->effector)
+    if (node->effector)
         ik_effector_destroy(node->effector);
+    if (node->constraint)
+        ik_constraint_destroy(node->constraint);
 
     ik_node_unlink(node);
     bstv_clear_free(&node->children);
@@ -60,13 +65,13 @@ ik_node_destruct(struct ik_node_t* node)
 
 /* ------------------------------------------------------------------------- */
 static void
-ik_node_destroy_recursive(struct ik_node_t* node)
+ik_node_destroy_recursive(ik_node_t* node)
 {
     ik_node_destruct_recursive(node);
     FREE(node);
 }
 void
-ik_node_destroy(struct ik_node_t* node)
+ik_node_destroy(ik_node_t* node)
 {
     ik_node_destruct(node);
     FREE(node);
@@ -74,7 +79,7 @@ ik_node_destroy(struct ik_node_t* node)
 
 /* ------------------------------------------------------------------------- */
 void
-ik_node_add_child(struct ik_node_t* node, struct ik_node_t* child)
+ik_node_add_child(ik_node_t* node, ik_node_t* child)
 {
     child->parent = node;
     bstv_insert(&node->children, child->guid, child);
@@ -82,9 +87,9 @@ ik_node_add_child(struct ik_node_t* node, struct ik_node_t* child)
 
 /* ------------------------------------------------------------------------- */
 void
-ik_node_unlink(struct ik_node_t* node)
+ik_node_unlink(ik_node_t* node)
 {
-    if(node->parent == NULL)
+    if (node->parent == NULL)
         return;
 
     bstv_erase(&node->parent->children, node->guid);
@@ -92,19 +97,19 @@ ik_node_unlink(struct ik_node_t* node)
 }
 
 /* ------------------------------------------------------------------------- */
-struct ik_node_t*
-ik_node_find_child(struct ik_node_t* node, uint32_t guid)
+ik_node_t*
+ik_node_find_child(ik_node_t* node, uint32_t guid)
 {
-    struct ik_node_t* found = bstv_find(&node->children, guid);
-    if(found != NULL)
+    ik_node_t* found = bstv_find(&node->children, guid);
+    if (found != NULL)
         return found;
 
-    if(node->guid == guid)
+    if (node->guid == guid)
         return node;
 
-    BSTV_FOR_EACH(&node->children, struct ik_node_t, child_guid, child)
+    BSTV_FOR_EACH(&node->children, ik_node_t, child_guid, child)
         found = ik_node_find_child(child, guid);
-        if(found != NULL)
+        if (found != NULL)
             return found;
     BSTV_END_EACH
 
@@ -113,29 +118,52 @@ ik_node_find_child(struct ik_node_t* node, uint32_t guid)
 
 /* ------------------------------------------------------------------------- */
 void
-ik_node_attach_effector(struct ik_node_t* node, struct ik_effector_t* effector)
+ik_node_attach_effector(ik_node_t* node, ik_effector_t* effector)
 {
+    if (node->effector != NULL)
+        ik_effector_destroy(node->effector);
+
     node->effector = effector;
 }
 
 /* ------------------------------------------------------------------------- */
 void
-ik_node_destroy_effector(struct ik_node_t* node)
+ik_node_destroy_effector(ik_node_t* node)
 {
-    if(node->effector == NULL)
+    if (node->effector == NULL)
         return;
     ik_effector_destroy(node->effector);
     node->effector = NULL;
 }
 
+/* ------------------------------------------------------------------------- */
+void
+ik_node_attach_constraint(ik_node_t* node, ik_constraint_t* constraint)
+{
+    if (node->constraint != NULL)
+        ik_constraint_destroy(node->constraint);
+
+    node->constraint = constraint;
+}
+
+/* ------------------------------------------------------------------------- */
+void
+ik_node_destroy_constraint(ik_node_t* node)
+{
+    if (node->constraint == NULL)
+        return;
+    ik_constraint_destroy(node->constraint);
+    node->constraint = NULL;
+}
+
 /* ------------------------------------------------------------------------- */
 static void
-recursively_dump_dot(FILE* fp, struct ik_node_t* node)
+recursively_dump_dot(FILE* fp, ik_node_t* node)
 {
-    if(node->effector != NULL)
+    if (node->effector != NULL)
         fprintf(fp, "    %d [color=\"1.0 0.5 1.0\"];\n", node->guid);
 
-    BSTV_FOR_EACH(&node->children, struct ik_node_t, guid, child)
+    BSTV_FOR_EACH(&node->children, ik_node_t, guid, child)
         fprintf(fp, "    %d -- %d;\n", node->guid, guid);
         recursively_dump_dot(fp, child);
     BSTV_END_EACH
@@ -143,10 +171,10 @@ recursively_dump_dot(FILE* fp, struct ik_node_t* node)
 
 /* ------------------------------------------------------------------------- */
 void
-ik_node_dump_to_dot(struct ik_node_t* node, const char* file_name)
+ik_node_dump_to_dot(ik_node_t* node, const char* file_name)
 {
     FILE* fp = fopen(file_name, "w");
-    if(fp == NULL)
+    if (fp == NULL)
     {
         ik_log_message("Failed to open file %s", file_name);
         return;
@@ -158,3 +186,56 @@ ik_node_dump_to_dot(struct ik_node_t* node, const char* file_name)
 
     fclose(fp);
 }
+
+/* ------------------------------------------------------------------------- */
+static void
+local_to_global_recursive(ik_node_t* node, vec3_t acc_pos, quat_t acc_rot)
+{
+    vec3_t position;
+    quat_t rotation;
+
+    quat_rotate_vec(node->position.f, acc_rot.f);
+    position = node->position;
+    vec3_add_vec3(node->position.f, acc_pos.f);
+    vec3_add_vec3(acc_pos.f, position.f);
+
+    rotation = node->rotation;
+    quat_mul_quat(node->rotation.f, acc_rot.f);
+    quat_mul_quat(acc_rot.f, rotation.f);
+
+    BSTV_FOR_EACH(&node->children, ik_node_t, guid, child)
+        local_to_global_recursive(child, acc_pos, acc_rot);
+    BSTV_END_EACH
+}
+void
+ik_node_local_to_global(ik_node_t* node)
+{
+    vec3_t acc_pos = {{0, 0, 0}};
+    quat_t acc_rot = {{0, 0, 0, 1}};
+    local_to_global_recursive(node, acc_pos, acc_rot);
+}
+
+/* ------------------------------------------------------------------------- */
+static void
+global_to_local_recursive(ik_node_t* node, vec3_t acc_pos, quat_t acc_rot)
+{
+    quat_t inv_rotation = acc_rot;
+    quat_conj(inv_rotation.f);
+    quat_mul_quat(node->rotation.f, inv_rotation.f);
+    quat_mul_quat(acc_rot.f, node->rotation.f);
+
+    vec3_sub_vec3(node->position.f, acc_pos.f);
+    vec3_add_vec3(acc_pos.f, node->position.f);
+    quat_rotate_vec(node->position.f, inv_rotation.f);
+
+    BSTV_FOR_EACH(&node->children, ik_node_t, guid, child)
+        global_to_local_recursive(child, acc_pos, acc_rot);
+    BSTV_END_EACH
+}
+void
+ik_node_global_to_local(ik_node_t* node)
+{
+    vec3_t acc_pos = {{0, 0, 0}};
+    quat_t acc_rot = {{0, 0, 0, 1}};
+    global_to_local_recursive(node, acc_pos, acc_rot);
+}

Source/ThirdParty/ik/src/ordered_vector.c

@@ -13,7 +13,7 @@
  *
  * This implementation will expand the memory by a factor of 2 each time this
  * is called. All elements are copied into the new section of memory.
- * @param[in] insertion_index Set to -1 if no space should be made for element
+ * @param[in] insertion_index Set to -1 if (no space should be made for element
  * insertion. Otherwise this parameter specifies the index of the element to
  * "evade" when re-allocating all other elements.
  * @param[in] target_size If set to 0, target size is calculated automatically.
@@ -21,18 +21,18 @@
  * @note No checks are performed to make sure the target size is large enough.
  */
 static int
-ordered_vector_expand(struct ordered_vector_t *vector,
+ordered_vector_expand(ordered_vector_t *vector,
                       uintptr_t insertion_index,
                       uint32_t target_size);
 
 /* ----------------------------------------------------------------------------
  * Exported functions
  * ------------------------------------------------------------------------- */
-struct ordered_vector_t*
+ordered_vector_t*
 ordered_vector_create(const uint32_t element_size)
 {
-    struct ordered_vector_t* vector;
-    if(!(vector = (struct ordered_vector_t*)MALLOC(sizeof(struct ordered_vector_t))))
+    ordered_vector_t* vector;
+    if (!(vector = (ordered_vector_t*)MALLOC(sizeof(ordered_vector_t))))
         return NULL;
     ordered_vector_construct(vector, element_size);
     return vector;
@@ -40,16 +40,16 @@ ordered_vector_create(const uint32_t element_size)
 
 /* ------------------------------------------------------------------------- */
 void
-ordered_vector_construct(struct ordered_vector_t* vector, const uint32_t element_size)
+ordered_vector_construct(ordered_vector_t* vector, const uint32_t element_size)
 {
     assert(vector);
-    memset(vector, 0, sizeof(struct ordered_vector_t));
+    memset(vector, 0, sizeof(ordered_vector_t));
     vector->element_size = element_size;
 }
 
 /* ------------------------------------------------------------------------- */
 void
-ordered_vector_destroy(struct ordered_vector_t* vector)
+ordered_vector_destroy(ordered_vector_t* vector)
 {
     assert(vector);
     ordered_vector_clear_free(vector);
@@ -58,7 +58,7 @@ ordered_vector_destroy(struct ordered_vector_t* vector)
 
 /* ------------------------------------------------------------------------- */
 void
-ordered_vector_clear(struct ordered_vector_t* vector)
+ordered_vector_clear(ordered_vector_t* vector)
 {
     assert(vector);
     /*
@@ -70,11 +70,11 @@ ordered_vector_clear(struct ordered_vector_t* vector)
 
 /* ------------------------------------------------------------------------- */
 void
-ordered_vector_clear_free(struct ordered_vector_t* vector)
+ordered_vector_clear_free(ordered_vector_t* vector)
 {
     assert(vector);
 
-    if(vector->data)
+    if (vector->data)
         FREE(vector->data);
 
     vector->data = NULL;
@@ -84,13 +84,13 @@ ordered_vector_clear_free(struct ordered_vector_t* vector)
 
 /* ------------------------------------------------------------------------- */
 int
-ordered_vector_resize(struct ordered_vector_t* vector, uint32_t size)
+ordered_vector_resize(ordered_vector_t* vector, uint32_t size)
 {
     int result = 0;
 
     assert(vector);
 
-    if(vector->count < size)
+    if (vector->count < size)
         result = ordered_vector_expand(vector, -1, size);
     vector->count = size;
 
@@ -99,14 +99,14 @@ ordered_vector_resize(struct ordered_vector_t* vector, uint32_t size)
 
 /* ------------------------------------------------------------------------- */
 void*
-ordered_vector_push_emplace(struct ordered_vector_t* vector)
+ordered_vector_push_emplace(ordered_vector_t* vector)
 {
     void* data;
 
     assert(vector);
 
-    if(vector->count == vector->capacity)
-        if(ordered_vector_expand(vector, -1, 0) < 0)
+    if (vector->count == vector->capacity)
+        if (ordered_vector_expand(vector, -1, 0) < 0)
             return NULL;
     data = vector->data + (vector->element_size * vector->count);
     ++(vector->count);
@@ -115,7 +115,7 @@ ordered_vector_push_emplace(struct ordered_vector_t* vector)
 
 /* ------------------------------------------------------------------------- */
 int
-ordered_vector_push(struct ordered_vector_t* vector, void* data)
+ordered_vector_push(ordered_vector_t* vector, void* data)
 {
     void* emplaced;
 
@@ -123,7 +123,7 @@ ordered_vector_push(struct ordered_vector_t* vector, void* data)
     assert(data);
 
     emplaced = ordered_vector_push_emplace(vector);
-    if(!emplaced)
+    if (!emplaced)
         return -1;
     memcpy(emplaced, data, vector->element_size);
     return 0;
@@ -131,18 +131,18 @@ ordered_vector_push(struct ordered_vector_t* vector, void* data)
 
 /* ------------------------------------------------------------------------- */
 int
-ordered_vector_push_vector(struct ordered_vector_t* vector, struct ordered_vector_t* source_vector)
+ordered_vector_push_vector(ordered_vector_t* vector, ordered_vector_t* source_vector)
 {
     assert(vector);
     assert(source_vector);
 
     /* make sure element sizes are equal */
-    if(vector->element_size != source_vector->element_size)
+    if (vector->element_size != source_vector->element_size)
         return -1;
 
     /* make sure there's enough space in the target vector */
-    if(vector->count + source_vector->count > vector->capacity)
-        if(ordered_vector_expand(vector, -1, vector->count + source_vector->count) < 0)
+    if (vector->count + source_vector->count > vector->capacity)
+        if (ordered_vector_expand(vector, -1, vector->count + source_vector->count) < 0)
             return -1;
 
     /* copy data */
@@ -156,11 +156,11 @@ ordered_vector_push_vector(struct ordered_vector_t* vector, struct ordered_vecto
 
 /* ------------------------------------------------------------------------- */
 void*
-ordered_vector_pop(struct ordered_vector_t* vector)
+ordered_vector_pop(ordered_vector_t* vector)
 {
     assert(vector);
 
-    if(!vector->count)
+    if (!vector->count)
         return NULL;
 
     --(vector->count);
@@ -169,11 +169,11 @@ ordered_vector_pop(struct ordered_vector_t* vector)
 
 /* ------------------------------------------------------------------------- */
 void*
-ordered_vector_back(const struct ordered_vector_t* vector)
+ordered_vector_back(const ordered_vector_t* vector)
 {
     assert(vector);
 
-    if(!vector->count)
+    if (!vector->count)
         return NULL;
 
     return vector->data + (vector->element_size * (vector->count - 1));
@@ -181,7 +181,7 @@ ordered_vector_back(const struct ordered_vector_t* vector)
 
 /* ------------------------------------------------------------------------- */
 void*
-ordered_vector_insert_emplace(struct ordered_vector_t* vector, uint32_t index)
+ordered_vector_insert_emplace(ordered_vector_t* vector, uint32_t index)
 {
     uint32_t offset;
 
@@ -192,13 +192,13 @@ ordered_vector_insert_emplace(struct ordered_vector_t* vector, uint32_t index)
      * because it's possible the user will want to insert at the very end of
      * the vector.
      */
-    if(index > vector->count)
+    if (index > vector->count)
         return NULL;
 
     /* re-allocate? */
-    if(vector->count == vector->capacity)
+    if (vector->count == vector->capacity)
     {
-        if(ordered_vector_expand(vector, index, 0) < 0)
+        if (ordered_vector_expand(vector, index, 0) < 0)
             return NULL;
     }
     else
@@ -218,7 +218,7 @@ ordered_vector_insert_emplace(struct ordered_vector_t* vector, uint32_t index)
 
 /* ------------------------------------------------------------------------- */
 int
-ordered_vector_insert(struct ordered_vector_t* vector, uint32_t index, void* data)
+ordered_vector_insert(ordered_vector_t* vector, uint32_t index, void* data)
 {
     void* emplaced;
 
@@ -226,7 +226,7 @@ ordered_vector_insert(struct ordered_vector_t* vector, uint32_t index, void* dat
     assert(data);
 
     emplaced = ordered_vector_insert_emplace(vector, index);
-    if(!emplaced)
+    if (!emplaced)
         return -1;
     memcpy(emplaced, data, vector->element_size);
     return 0;
@@ -234,14 +234,14 @@ ordered_vector_insert(struct ordered_vector_t* vector, uint32_t index, void* dat
 
 /* ------------------------------------------------------------------------- */
 void
-ordered_vector_erase_index(struct ordered_vector_t* vector, uint32_t index)
+ordered_vector_erase_index(ordered_vector_t* vector, uint32_t index)
 {
     assert(vector);
 
-    if(index >= vector->count)
+    if (index >= vector->count)
         return;
 
-    if(index == vector->count - 1)
+    if (index == vector->count - 1)
         /* last element doesn't require memory shifting, just pop it */
         ordered_vector_pop(vector);
     else
@@ -258,7 +258,7 @@ ordered_vector_erase_index(struct ordered_vector_t* vector, uint32_t index)
 
 /* ------------------------------------------------------------------------- */
 void
-ordered_vector_erase_element(struct ordered_vector_t* vector, void* element)
+ordered_vector_erase_element(ordered_vector_t* vector, void* element)
 {
     uintptr_t last_element;
 
@@ -268,7 +268,7 @@ ordered_vector_erase_element(struct ordered_vector_t* vector, void* element)
     assert((uintptr_t)element >= (uintptr_t)vector->data);
     assert((uintptr_t)element <= (uintptr_t)last_element);
 
-    if(element != (void*)last_element)
+    if (element != (void*)last_element)
     {
         memmove(element,    /* target is to overwrite the element */
                 (void*)((uintptr_t)element + vector->element_size), /* read everything from next element */
@@ -279,11 +279,11 @@ ordered_vector_erase_element(struct ordered_vector_t* vector, void* element)
 
 /* ------------------------------------------------------------------------- */
 void*
-ordered_vector_get_element(struct ordered_vector_t* vector, uint32_t index)
+ordered_vector_get_element(ordered_vector_t* vector, uint32_t index)
 {
     assert(vector);
 
-    if(index >= vector->count)
+    if (index >= vector->count)
         return NULL;
     return vector->data + (vector->element_size * index);
 }
@@ -292,7 +292,7 @@ ordered_vector_get_element(struct ordered_vector_t* vector, uint32_t index)
  * Static functions
  * ------------------------------------------------------------------------- */
 static int
-ordered_vector_expand(struct ordered_vector_t *vector,
+ordered_vector_expand(ordered_vector_t *vector,
                       uintptr_t insertion_index,
                       uint32_t target_count)
 {
@@ -300,8 +300,8 @@ ordered_vector_expand(struct ordered_vector_t *vector,
     DATA_POINTER_TYPE* old_data;
     DATA_POINTER_TYPE* new_data;
 
-    /* expand by factor 2, or adopt target count if it is not 0 */
-    if(target_count)
+    /* expand by factor 2, or adopt target count if (it is not 0 */
+    if (target_count)
         new_count = target_count;
     else
         new_count = vector->capacity << 1;
@@ -310,11 +310,11 @@ ordered_vector_expand(struct ordered_vector_t *vector,
      * If vector hasn't allocated anything yet, just allocated the requested
      * amount of memory and return immediately.
      */
-    if(!vector->data)
+    if (!vector->data)
     {
         new_count = (new_count == 0 ? 2 : new_count);
         vector->data = MALLOC(new_count * vector->element_size);
-        if(!vector->data)
+        if (!vector->data)
             return -1;
         vector->capacity = new_count;
         return 0;
@@ -323,11 +323,11 @@ ordered_vector_expand(struct ordered_vector_t *vector,
     /* prepare for reallocating data */
     old_data = vector->data;
     new_data = (DATA_POINTER_TYPE*)MALLOC(new_count * vector->element_size);
-    if(!new_data)
+    if (!new_data)
         return -1;
 
-    /* if no insertion index is required, copy all data to new memory */
-    if(insertion_index == (uintptr_t)-1 || insertion_index >= new_count)
+    /* if (no insertion index is required, copy all data to new memory */
+    if (insertion_index == (uintptr_t)-1 || insertion_index >= new_count)
         memcpy(new_data, old_data, vector->count * vector->element_size);
 
     /* keep space for one element at the insertion index */

Source/ThirdParty/ik/src/quat.c

@@ -4,7 +4,7 @@
 
 /* ------------------------------------------------------------------------- */
 void
-quat_set_identity(ik_real* q)
+quat_set_identity(ik_real* IK_RESTRICT q)
 {
     memset(q, 0, sizeof(ik_real) * 3);
     q[3] = 1;
@@ -12,7 +12,7 @@ quat_set_identity(ik_real* q)
 
 /* ------------------------------------------------------------------------- */
 void
-quat_add_quat(ik_real* q1, const ik_real* q2)
+quat_add_quat(ik_real* IK_RESTRICT q1, const ik_real* IK_RESTRICT q2)
 {
     q1[0] += q2[0];
     q1[1] += q2[1];
@@ -22,14 +22,14 @@ quat_add_quat(ik_real* q1, const ik_real* q2)
 
 /* ------------------------------------------------------------------------- */
 ik_real
-quat_mag(const ik_real* q)
+quat_mag(const ik_real* IK_RESTRICT q)
 {
     return sqrt(q[3]*q[3] + q[2]*q[2] + q[1]*q[1] + q[0]*q[0]);
 }
 
 /* ------------------------------------------------------------------------- */
 void
-quat_conj(ik_real* q)
+quat_conj(ik_real* IK_RESTRICT q)
 {
     q[0] = -q[0];
     q[1] = -q[1];
@@ -38,7 +38,7 @@ quat_conj(ik_real* q)
 
 /* ------------------------------------------------------------------------- */
 void
-quat_invert_sign(ik_real* q)
+quat_invert_sign(ik_real* IK_RESTRICT q)
 {
     q[0] = -q[0];
     q[1] = -q[1];
@@ -49,10 +49,10 @@ quat_invert_sign(ik_real* q)
 
 /* ------------------------------------------------------------------------- */
 void
-quat_normalise(ik_real* q)
+quat_normalise(ik_real* IK_RESTRICT q)
 {
     ik_real mag = quat_mag(q);
-    if(mag != 0.0)
+    if (mag != 0.0)
         mag = 1.0 / mag;
     q[0] *= mag;
     q[1] *= mag;
@@ -61,8 +61,8 @@ quat_normalise(ik_real* q)
 }
 
 /* ------------------------------------------------------------------------- */
-void
-quat_mul_quat(ik_real* q1, const ik_real* q2)
+static void
+mul_quat_no_normalise(ik_real* IK_RESTRICT q1, const ik_real* IK_RESTRICT q2)
 {
     ik_real v1[3];
     ik_real v2[3];
@@ -75,13 +75,17 @@ quat_mul_quat(ik_real* q1, const ik_real* q2)
     vec3_cross(q1, q2);
     vec3_add_vec3(q1, v1);
     vec3_add_vec3(q1, v2);
-
+}
+void
+quat_mul_quat(ik_real* IK_RESTRICT q1, const ik_real* IK_RESTRICT q2)
+{
+    mul_quat_no_normalise(q1, q2);
     quat_normalise(q1);
 }
 
 /* ------------------------------------------------------------------------- */
 void
-quat_mul_scalar(ik_real* q, ik_real scalar)
+quat_mul_scalar(ik_real* IK_RESTRICT q, ik_real scalar)
 {
     q[0] *= scalar;
     q[1] *= scalar;
@@ -91,9 +95,9 @@ quat_mul_scalar(ik_real* q, ik_real scalar)
 
 /* ------------------------------------------------------------------------- */
 void
-quat_div_scalar(ik_real* q, ik_real scalar)
+quat_div_scalar(ik_real* IK_RESTRICT q, ik_real scalar)
 {
-    if(scalar == 0.0)
+    if (scalar == 0.0)
         quat_set_identity(q);
     else
     {
@@ -107,7 +111,7 @@ quat_div_scalar(ik_real* q, ik_real scalar)
 
 /* ------------------------------------------------------------------------- */
 ik_real
-quat_dot(ik_real* q1, const ik_real* q2)
+quat_dot(ik_real* IK_RESTRICT q1, const ik_real* IK_RESTRICT q2)
 {
     return q1[0] * q2[0] +
            q1[1] * q2[1] +
@@ -117,7 +121,7 @@ quat_dot(ik_real* q1, const ik_real* q2)
 
 /* ------------------------------------------------------------------------- */
 void
-quat_rotate_vec(ik_real* v, const ik_real* q)
+quat_rotate_vec(ik_real* IK_RESTRICT v, const ik_real* IK_RESTRICT q)
 {
     /* P' = RPR' */
     quat_t result;
@@ -131,17 +135,17 @@ quat_rotate_vec(ik_real* v, const ik_real* q)
     quat_conj(conj.f);
 
     result = *(quat_t*)q;
-    quat_mul_quat(result.f, point.f);
-    quat_mul_quat(result.f, conj.f);
+    mul_quat_no_normalise(result.f, point.f);
+    mul_quat_no_normalise(result.f, conj.f);
     memcpy(v, result.f, sizeof(ik_real) * 3);
 }
 
 /* ------------------------------------------------------------------------- */
 void
-quat_normalise_sign(ik_real* q1)
+quat_normalise_sign(ik_real* IK_RESTRICT q1)
 {
     quat_t unit = {{0, 0, 0, 1}};
     ik_real dot = quat_dot(q1, unit.f);
-    if(dot < 0.0)
+    if (dot < 0.0)
         quat_invert_sign(q1);
 }

Source/ThirdParty/ik/src/solver.c

@@ -1,66 +1,128 @@
+#include "ik/chain_tree.h"
 #include "ik/effector.h"
 #include "ik/log.h"
 #include "ik/memory.h"
 #include "ik/node.h"
 #include "ik/solver.h"
 #include "ik/solver_FABRIK.h"
+#include "ik/solver_2bone.h"
+#include "ik/solver_1bone.h"
+#include "ik/solver_MSS.h"
 #include "ik/solver_jacobian_inverse.h"
 #include "ik/solver_jacobian_transpose.h"
+#include <string.h>
 
 static int
-recursive_get_all_effector_nodes(struct ik_node_t* node, struct ordered_vector_t* effector_nodes_list);
+recursively_get_all_effector_nodes(ik_node_t* node, ordered_vector_t* effector_nodes_list);
 
 /* ------------------------------------------------------------------------- */
-struct ik_solver_t*
+ik_solver_t*
 ik_solver_create(enum solver_algorithm_e algorithm)
 {
-    struct ik_solver_t* solver = NULL;
+    uintptr_t solver_size = 0;
+    int (*solver_construct)(ik_solver_t*) = NULL;
+    ik_solver_t* solver = NULL;
 
-    switch(algorithm)
+    /*
+     * Determine the correct size and confunction, depending on the
+     * selected algorithm.
+     */
+    switch (algorithm)
     {
     case SOLVER_FABRIK:
-        solver = (struct ik_solver_t*)solver_FABRIK_create();
+        solver_size = sizeof(fabrik_t);
+        solver_construct = solver_FABRIK_construct;
+        break;
+
+    case SOLVER_TWO_BONE:
+        solver_size = sizeof(two_bone_t);
+        solver_construct = solver_2bone_construct;
+        break;
+
+    case SOLVER_ONE_BONE:
+        solver_size = sizeof(one_bone_t);
+        solver_construct = solver_1bone_construct;
+        break;
+        
+    case SOLVER_MSS_LAPLACE:
+        solver_size = sizeof(mss_t);
+        solver_construct = solver_MSS_construct;
         break;
 
-    /*case SOLVER_JACOBIAN_INVERSE:
+    /*
+    case SOLVER_JACOBIAN_INVERSE:
     case SOLVER_JACOBIAN_TRANSPOSE:
         break;*/
     }
 
-    if(solver == NULL)
-        return NULL;
+    if (solver_construct == NULL)
+    {
+        ik_log_message("Unknown algorithm \"%d\" was specified", algorithm);
+        goto alloc_solver_failed;
+    }
+
+    /*
+     * Allocate the solver, initialise to 0 and initialise the base fields
+     * before calling the construct() callback for the specific solver.
+     */
+    solver = (ik_solver_t*)MALLOC(solver_size);
+    if (solver == NULL)
+    {
+        ik_log_message("Failed to allocate solver: ran out of memory");
+        goto alloc_solver_failed;
+    }
+    memset(solver, 0, solver_size);
 
-    ordered_vector_construct(&solver->effector_nodes_list, sizeof(struct ik_node_t*));
+    ordered_vector_construct(&solver->effector_nodes_list, sizeof(ik_node_t*));
+    chain_tree_construct(&solver->chain_tree);
+
+    /* Now call derived construction */
+    if (solver_construct(solver) < 0)
+        goto construct_derived_solver_failed;
+
+    /* Derived destruct callback must be set */
+    if (solver->destruct == NULL)
+    {
+        ik_log_message("Derived solvers MUST implement the destruct() callback");
+        goto derived_didnt_implement_destruct;
+    }
 
     return solver;
+
+    derived_didnt_implement_destruct :
+    construct_derived_solver_failed  : FREE(solver);
+    alloc_solver_failed              : return NULL;
 }
 
 /* ------------------------------------------------------------------------- */
 void
-ik_solver_destroy(struct ik_solver_t* solver)
+ik_solver_destroy(ik_solver_t* solver)
 {
-    if(solver->tree)
+    solver->destruct(solver);
+
+    if (solver->tree)
         ik_node_destroy(solver->tree);
 
+    chain_tree_destruct(&solver->chain_tree);
     ordered_vector_clear_free(&solver->effector_nodes_list);
 
-    solver->destroy(solver);
+    FREE(solver);
 }
 
 /* ------------------------------------------------------------------------- */
 void
-ik_solver_set_tree(struct ik_solver_t* solver, struct ik_node_t* root)
+ik_solver_set_tree(ik_solver_t* solver, ik_node_t* root)
 {
     ik_solver_destroy_tree(solver);
     solver->tree = root;
 }
 
 /* ------------------------------------------------------------------------- */
-struct ik_node_t*
-ik_solver_unlink_tree(struct ik_solver_t* solver)
+ik_node_t*
+ik_solver_unlink_tree(ik_solver_t* solver)
 {
-    struct ik_node_t* root = solver->tree;
-    if(root == NULL)
+    ik_node_t* root = solver->tree;
+    if (root == NULL)
         return NULL;
     solver->tree = NULL;
 
@@ -75,20 +137,20 @@ ik_solver_unlink_tree(struct ik_solver_t* solver)
 
 /* ------------------------------------------------------------------------- */
 void
-ik_solver_destroy_tree(struct ik_solver_t* solver)
+ik_solver_destroy_tree(ik_solver_t* solver)
 {
-    struct ik_node_t* root;
-    if((root = ik_solver_unlink_tree(solver)) == NULL)
+    ik_node_t* root;
+    if ((root = ik_solver_unlink_tree(solver)) == NULL)
         return;
     ik_node_destroy(root);
 }
 
 /* ------------------------------------------------------------------------- */
 int
-ik_solver_rebuild_data(struct ik_solver_t* solver)
+ik_solver_rebuild_data(ik_solver_t* solver)
 {
     /* If the solver has no tree, then there's nothing to do */
-    if(solver->tree == NULL)
+    if (solver->tree == NULL)
     {
         ik_log_message("No tree to work with. Did you forget to set the tree with ik_solver_set_tree()?");
         return -1;
@@ -100,65 +162,76 @@ ik_solver_rebuild_data(struct ik_solver_t* solver)
      */
     ik_log_message("Rebuilding effector nodes list");
     ordered_vector_clear(&solver->effector_nodes_list);
-    if(recursive_get_all_effector_nodes(solver->tree,
-                                        &solver->effector_nodes_list) < 0)
+    if (recursively_get_all_effector_nodes(solver->tree, &solver->effector_nodes_list) < 0)
     {
         ik_log_message("Ran out of memory while building the effector nodes list");
         return -1;
     }
 
-    return solver->rebuild_data(solver);
+    /* now build the chain tree */
+    if (rebuild_chain_tree(solver) < 0)
+        return -1;
+
+    if (solver->rebuild_data != NULL)
+        return solver->rebuild_data(solver);
+
+    return 0;
 }
 
 /* ------------------------------------------------------------------------- */
 void
-ik_solver_recalculate_segment_lengths(struct ik_solver_t* solver)
+ik_solver_recalculate_segment_lengths(ik_solver_t* solver)
 {
-    solver->recalculate_segment_lengths(solver);
+    /* TODO: Implement again, take into consideration merged bones */
+    /*calculate_segment_lengths(solver->chain_tree);*/
 }
 
 /* ------------------------------------------------------------------------- */
 int
-ik_solver_solve(struct ik_solver_t* solver)
+ik_solver_solve(ik_solver_t* solver)
 {
     return solver->solve(solver);
 }
 
 /* ------------------------------------------------------------------------- */
 static void
-iterate_tree_recursive(struct ik_solver_t* solver, struct ik_node_t* node)
+iterate_tree_recursive(ik_node_t* node,
+                       ik_solver_iterate_node_cb_func callback)
 {
-    if(solver->apply_result)
-        solver->apply_result(node);
+    callback(node);
 
-    BSTV_FOR_EACH(&node->children, struct ik_node_t, guid, child)
-        iterate_tree_recursive(solver, child);
+    BSTV_FOR_EACH(&node->children, ik_node_t, guid, child)
+        iterate_tree_recursive(child, callback);
     BSTV_END_EACH
 }
 void
-ik_solver_iterate_tree(struct ik_solver_t* solver)
+ik_solver_iterate_tree(ik_solver_t* solver,
+                       ik_solver_iterate_node_cb_func callback)
 {
-    if(solver->tree == NULL)
+    if (solver->tree == NULL)
+    {
+        ik_log_message("Warning: Tried iterating the tree, but no tree was set");
         return;
+    }
 
-    iterate_tree_recursive(solver, solver->tree);
+    iterate_tree_recursive(solver->tree, callback);
 }
 
 /* ------------------------------------------------------------------------- */
 static void
-reset_solved_data_recursive(struct ik_node_t* node)
+reset_solved_data_recursive(ik_node_t* node)
 {
-    node->solved_position = node->position;
-    node->solved_rotation = node->rotation;
+    node->position = node->initial_position;
+    node->rotation = node->initial_rotation;
 
-    BSTV_FOR_EACH(&node->children, struct ik_node_t, guid, child)
+    BSTV_FOR_EACH(&node->children, ik_node_t, guid, child)
         reset_solved_data_recursive(child);
     BSTV_END_EACH
 }
 void
-ik_solver_reset_solved_data(struct ik_solver_t* solver)
+ik_solver_reset_solved_data(ik_solver_t* solver)
 {
-    if(solver->tree == NULL)
+    if (solver->tree == NULL)
         return;
 
     reset_solved_data_recursive(solver->tree);
@@ -166,14 +239,14 @@ ik_solver_reset_solved_data(struct ik_solver_t* solver)
 
 /* ------------------------------------------------------------------------- */
 static int
-recursive_get_all_effector_nodes(struct ik_node_t* node, struct ordered_vector_t* effector_nodes_list)
+recursively_get_all_effector_nodes(ik_node_t* node, ordered_vector_t* effector_nodes_list)
 {
-    if(node->effector != NULL)
-        if(ordered_vector_push(effector_nodes_list, &node) < 0)
+    if (node->effector != NULL)
+        if (ordered_vector_push(effector_nodes_list, &node) < 0)
             return -1;
 
-    BSTV_FOR_EACH(&node->children, struct ik_node_t, guid, child)
-        if(recursive_get_all_effector_nodes(child, effector_nodes_list) < 0)
+    BSTV_FOR_EACH(&node->children, ik_node_t, guid, child)
+        if (recursively_get_all_effector_nodes(child, effector_nodes_list) < 0)
             return -1;
     BSTV_END_EACH
 

Source/ThirdParty/ik/src/solver_1bone.c

@@ -0,0 +1,82 @@
+#include "ik/chain_tree.h"
+#include "ik/effector.h"
+#include "ik/log.h"
+#include "ik/node.h"
+#include "ik/solver_1bone.h"
+#include <assert.h>
+
+/* ------------------------------------------------------------------------- */
+int
+solver_1bone_construct(ik_solver_t* solver)
+{
+    one_bone_t* one_bone = (one_bone_t*)solver;
+
+    /* set up derived functions */
+    one_bone->destruct = solver_1bone_destruct;
+    one_bone->rebuild_data = solver_1bone_rebuild;
+    one_bone->solve = solver_1bone_solve;
+
+    return 0;
+}
+
+/* ------------------------------------------------------------------------- */
+void
+solver_1bone_destruct(ik_solver_t* solver)
+{
+}
+
+/* ------------------------------------------------------------------------- */
+int
+solver_1bone_rebuild(ik_solver_t* solver)
+{
+    /*
+     * We need to assert that there really are only chains of length 1 and no
+     * sub chains.
+     */
+    ORDERED_VECTOR_FOR_EACH(&solver->chain_tree.islands, chain_island_t, island)
+        if (ordered_vector_count(&island->root_chain.nodes) != 2) /* 2 nodes = 1 bone */
+        {
+            ik_log_message("ERROR: Your tree has chains that are longer than 1 bone. Are you sure you selected the correct solver algorithm?");
+            return -1;
+        }
+        if (ordered_vector_count(&island->root_chain.children) > 0)
+        {
+            ik_log_message("ERROR: Your tree has child chains. This solver does not support arbitrary trees. You will need to switch to another algorithm (e.g. FABRIK)");
+            return -1;
+        }
+    ORDERED_VECTOR_END_EACH
+
+    return 0;
+}
+
+/* ------------------------------------------------------------------------- */
+int
+solver_1bone_solve(ik_solver_t* solver)
+{
+    ORDERED_VECTOR_FOR_EACH(&solver->chain_tree.islands, chain_island_t, island)
+        ik_node_t* node_tip;
+        ik_node_t* node_base;
+        chain_t* root_chain = &island->root_chain;
+
+        assert(ordered_vector_count(&root_chain->nodes) > 1);
+        node_tip = *(ik_node_t**)ordered_vector_get_element(&root_chain->nodes, 0);
+        node_base = *(ik_node_t**)ordered_vector_get_element(&root_chain->nodes, 1);
+
+        assert(node_tip->effector != NULL);
+        node_tip->position = node_tip->effector->target_position;
+
+        vec3_sub_vec3(node_tip->position.f, node_base->position.f);
+        vec3_normalise(node_tip->position.f);
+        vec3_mul_scalar(node_tip->position.f, node_tip->segment_length);
+        vec3_add_vec3(node_tip->position.f, node_base->position.f);
+    ORDERED_VECTOR_END_EACH
+
+    if (solver->flags & SOLVER_CALCULATE_FINAL_ROTATIONS)
+    {
+        ORDERED_VECTOR_FOR_EACH(&solver->chain_tree.islands, chain_island_t, island)
+            calculate_global_rotations(&island->root_chain);
+        ORDERED_VECTOR_END_EACH
+    }
+
+    return 0;
+}

Source/ThirdParty/ik/src/solver_2bone.c

@@ -0,0 +1,144 @@
+#include "ik/effector.h"
+#include "ik/node.h"
+#include "ik/log.h"
+#include "ik/solver_2bone.h"
+#include <assert.h>
+#include <math.h>
+
+/* ------------------------------------------------------------------------- */
+int
+solver_2bone_construct(ik_solver_t* solver)
+{
+    two_bone_t* two_bone = (two_bone_t*)solver;
+
+    /* set up derived functions */
+    two_bone->destruct = solver_2bone_destruct;
+    two_bone->rebuild_data = solver_2bone_rebuild;
+    two_bone->solve = solver_2bone_solve;
+
+    return 0;
+}
+
+/* ------------------------------------------------------------------------- */
+void
+solver_2bone_destruct(ik_solver_t* solver)
+{
+}
+
+/* ------------------------------------------------------------------------- */
+int
+solver_2bone_rebuild(ik_solver_t* solver)
+{
+    /*
+     * We need to assert that there really are only chains of length 1 and no
+     * sub chains.
+     */
+    ORDERED_VECTOR_FOR_EACH(&solver->chain_tree.islands, chain_island_t, island)
+        if (ordered_vector_count(&island->root_chain.nodes) != 3) /* 3 nodes = 2 bones */
+        {
+            ik_log_message("ERROR: Your tree has chains that are longer or shorter than 2 bones. Are you sure you selected the correct solver algorithm?");
+            return -1;
+        }
+        if (ordered_vector_count(&island->root_chain.children) > 0)
+        {
+            ik_log_message("ERROR: Your tree has child chains. This solver does not support arbitrary trees. You will need to switch to another algorithm (e.g. FABRIK)");
+            return -1;
+        }
+    ORDERED_VECTOR_END_EACH
+
+    return 0;
+}
+
+/* ------------------------------------------------------------------------- */
+int
+solver_2bone_solve(ik_solver_t* solver)
+{
+    ORDERED_VECTOR_FOR_EACH(&solver->chain_tree.islands, chain_island_t, island)
+        ik_node_t* node_tip;
+        ik_node_t* node_mid;
+        ik_node_t* node_base;
+        vec3_t to_target;
+        ik_real a, b, c, aa, bb, cc;
+
+        assert(ordered_vector_count(&island->root_chain.nodes) > 2);
+        node_tip = *(ik_node_t**)ordered_vector_get_element(&island->root_chain.nodes, 0);
+        node_mid = *(ik_node_t**)ordered_vector_get_element(&island->root_chain.nodes, 1);
+        node_base = *(ik_node_t**)ordered_vector_get_element(&island->root_chain.nodes, 2);
+
+        assert(node_tip->effector != NULL);
+        to_target = node_tip->effector->target_position;
+        vec3_sub_vec3(to_target.f, node_base->position.f);
+
+        /*
+         * Form a triangle from the two segment lengths so we can calculate the
+         * angles. Here's some visual help.
+         *
+         *   target *--.__  a
+         *           \     --.___ (unknown position, needs solving)
+         *            \      _-
+         *           c \   _-
+         *              \-    b
+         *            base
+         *
+         */
+        a = node_mid->segment_length;
+        b = node_mid->segment_length;
+        aa = a*a;
+        bb = b*b;
+        cc = vec3_length_squared(to_target.f);
+        c = sqrt(cc);
+
+        /* check if in reach */
+        if (c < a + b)
+        {
+            /* Cosine law to get base angle (alpha) */
+            quat_t alpha_rotation;
+            ik_real alpha = acos((bb + cc - aa) / (2.0 * node_mid->segment_length * sqrt(cc)));
+            ik_real cos_a = cos(alpha * 0.5);
+            ik_real sin_a = sin(alpha * 0.5);
+
+            /* Cross product of both segment vectors defines axis of rotation */
+            alpha_rotation.vw.v = node_tip->position;
+            vec3_sub_vec3(alpha_rotation.f, node_mid->position.f);  /* top segment */
+            vec3_sub_vec3(node_mid->position.f, node_base->position.f);  /* bottom segment */
+            vec3_cross(alpha_rotation.f, node_mid->position.f);
+
+            /*
+             * Set up quaternion describing the rotation of alpha. Need to
+             * normalise vec3 component of quaternion so rotation is correct.
+             */
+            vec3_normalise(alpha_rotation.f);
+            vec3_mul_scalar(alpha_rotation.f, sin_a);
+            alpha_rotation.q.w = cos_a;
+
+            /* Rotate side c and scale to length of side b to get the unknown position */
+            node_mid->position = to_target;
+            vec3_normalise(node_mid->position.f);
+            vec3_mul_scalar(node_mid->position.f, node_mid->segment_length);
+            quat_rotate_vec(node_mid->position.f, alpha_rotation.f);
+            vec3_add_vec3(node_mid->position.f, node_base->position.f);
+
+            node_tip->position = node_tip->effector->target_position;
+        }
+        else
+        {
+            /* Just point both segments at target */
+            vec3_normalise(to_target.f);
+            node_mid->position = to_target;
+            node_tip->position = to_target;
+            vec3_mul_scalar(node_mid->position.f, node_mid->segment_length);
+            vec3_mul_scalar(node_tip->position.f, node_tip->segment_length);
+            vec3_add_vec3(node_mid->position.f, node_base->position.f);
+            vec3_add_vec3(node_tip->position.f, node_mid->position.f);
+        }
+    ORDERED_VECTOR_END_EACH
+
+    if (solver->flags & SOLVER_CALCULATE_FINAL_ROTATIONS)
+    {
+        ORDERED_VECTOR_FOR_EACH(&solver->chain_tree.islands, chain_island_t, island)
+            calculate_global_rotations(&island->root_chain);
+        ORDERED_VECTOR_END_EACH
+    }
+
+    return 0;
+}

Source/ThirdParty/ik/src/solver_FABRIK.c

@@ -1,4 +1,5 @@
 #include "ik/bst_vector.h"
+#include "ik/constraint.h"
 #include "ik/effector.h"
 #include "ik/log.h"
 #include "ik/memory.h"
@@ -9,203 +10,105 @@
 #include <stdio.h>
 #include <math.h>
 
-enum node_marking_e
+typedef struct position_direction_t
 {
-    MARK_NONE = 0,
-    MARK_SPLIT,
-    MARK_SECTION
-};
+    vec3_t position;
+    vec3_t direction;
+} position_direction_t;
 
-struct effector_data_t
+typedef struct transform_t
 {
-    vec3_t target_direction;
-    ik_real rotation_weight;
-    ik_real rotation_weight_decay;
-};
-
-static void
-chain_construct(struct chain_t* chain);
-static void
-chain_clear_free(struct chain_t* chain);
-static void
-chain_destruct(struct chain_t* chain);
-static void
-calculate_segment_lengths_recursive(struct chain_t* chain);
+    vec3_t position;
+    quat_t rotation;
+} transform_t;
 
 /* ------------------------------------------------------------------------- */
-static void
-chain_construct(struct chain_t* chain)
-{
-    ordered_vector_construct(&chain->nodes, sizeof(struct ik_node_t*));
-    ordered_vector_construct(&chain->children, sizeof(struct chain_t));
-}
-
-/* ------------------------------------------------------------------------- */
-static void
-chain_clear_free(struct chain_t* chain)
-{
-    ORDERED_VECTOR_FOR_EACH(&chain->children, struct chain_t, child_chain)
-        chain_destruct(child_chain);
-    ORDERED_VECTOR_END_EACH
-    ordered_vector_clear_free(&chain->children);
-    ordered_vector_clear_free(&chain->nodes);
-}
-
-/* ------------------------------------------------------------------------- */
-static void
-chain_destruct(struct chain_t* chain)
-{
-    chain_clear_free(chain);
-}
-
-/*!
- * @brief Breaks down the relevant nodes of the scene graph into a list of
- * chains. FABRIK can then more efficiently solve each chain individually.
- *
- * A "sub-base joint" is a node in the scene graph where at least two end
- * effector nodes eventually join together. FABRIK only works on single
- * chains of joints at a time. The end position of every sub-base joint is
- * the average of the resulting multiple positions after running FABRIK on
- * each chain. Said average position becomes the new target position for
- * the next chain connected to it.
- *
- * This algorithm finds all sub-base joints and generates chains between
- * base, sub-base joints, and end effectors. These chains are inserted into
- * the chain list. The order is such that iterating the list from the
- * beginning results in traversing the sub-base nodes breadth-last. This is
- * important.
- *
- * @note Effectors that are deactivated or invalid are ignored in this search.
- * So even though a node might share two effectors, if one of them is
- * deactivated, then the node is no longer considered a sub-base node.
- */
-static int
-rebuild_chain_tree(struct fabrik_t* solver);
-
-/* ------------------------------------------------------------------------- */
-struct ik_solver_t*
-solver_FABRIK_create(void)
+int
+solver_FABRIK_construct(ik_solver_t* solver)
 {
-    struct fabrik_t* solver;
-
-    ik_log_message("Creating FABRIK solver");
+    fabrik_t* fabrik = (fabrik_t*)solver;
 
-    solver = (struct fabrik_t*)MALLOC(sizeof *solver);
-    if(solver == NULL)
-    {
-        ik_log_message("Ran out of memory in solver_FABRIK_create()");
-        goto alloc_solver_filed;
-    }
-    memset(solver, 0, sizeof *solver);
+    /* set up derived functions */
+    fabrik->destruct = solver_FABRIK_destruct;
+    fabrik->solve = solver_FABRIK_solve;
 
-    solver->destroy = solver_FABRIK_destroy;
-    solver->rebuild_data = solver_FABRIK_rebuild_data;
-    solver->recalculate_segment_lengths = solver_FABRIK_recalculate_segment_lengths;
-    solver->solve = solver_FABRIK_solve;
+    /* typical default values */
+    fabrik->max_iterations = 20;
+    fabrik->tolerance = 1e-3;
 
-    solver->max_iterations = 20;
-    solver->tolerance = 1e-3;
-
-    solver->chain_tree = (struct chain_t*)MALLOC(sizeof(struct chain_t));
-    if(solver->chain_tree == NULL)
-    {
-        ik_log_message("Ran out of memory in solver_FABRIK_create()");
-        goto alloc_chain_tree_failed;
-    }
-    chain_construct(solver->chain_tree);
-
-    return (struct ik_solver_t*)solver;
-
-    alloc_chain_tree_failed : FREE(solver);
-    alloc_solver_filed      : return NULL;
+    return 0;
 }
 
 /* ------------------------------------------------------------------------- */
 void
-solver_FABRIK_destroy(struct ik_solver_t* solver)
-{
-    struct fabrik_t* fabrik;
-
-    ik_log_message("Destroying FABRIK solver");
-
-    fabrik = (struct fabrik_t*)solver;
-    chain_destruct(fabrik->chain_tree);
-    FREE(fabrik->chain_tree);
-    FREE(solver);
-}
-
-/* ------------------------------------------------------------------------- */
-int
-solver_FABRIK_rebuild_data(struct ik_solver_t* solver)
+solver_FABRIK_destruct(ik_solver_t* solver)
 {
-    return rebuild_chain_tree((struct fabrik_t*)solver);
 }
 
 /* ------------------------------------------------------------------------- */
 static void
-determine_target_data_from_effector(struct chain_t* chain, vec3_t* target_position)
+determine_target_data_from_effector(chain_t* chain, vec3_t* target_position)
 {
     /* Extract effector node and get its effector object */
-    struct ik_node_t* effector_node;
-    struct ik_effector_t* effector;
+    ik_node_t* effector_node;
+    ik_effector_t* effector;
     assert(ordered_vector_count(&chain->nodes) > 1);
-    effector_node = *(struct ik_node_t**)ordered_vector_get_element(&chain->nodes, 0);
+    effector_node = *(ik_node_t**)ordered_vector_get_element(&chain->nodes, 0);
     assert(effector_node->effector != NULL);
     effector = effector_node->effector;
 
     /* lerp using effector weight to get actual target position */
     *target_position = effector->target_position;
-    vec3_sub_vec3(target_position->f, effector_node->position.f);
+    vec3_sub_vec3(target_position->f, effector_node->initial_position.f);
     vec3_mul_scalar(target_position->f, effector->weight);
-    vec3_add_vec3(target_position->f, effector_node->position.f);
+    vec3_add_vec3(target_position->f, effector_node->initial_position.f);
 
     /* Fancy algorithm using nlerp, makes transitions look more natural */
-    if(effector->flags & EFFECTOR_WEIGHT_NLERP && effector->weight < 1.0)
+    if (effector->flags & EFFECTOR_WEIGHT_NLERP && effector->weight < 1.0)
     {
         ik_real distance_to_target;
         vec3_t base_to_effector;
         vec3_t base_to_target;
-        struct ik_node_t* base_node;
+        ik_node_t* base_node;
 
         /* Need distance from base node to target and base to effector node */
-        base_node = *(struct ik_node_t**)ordered_vector_get_element(&chain->nodes,
+        base_node = *(ik_node_t**)ordered_vector_get_element(&chain->nodes,
                 ordered_vector_count(&chain->nodes) - 1);
-        base_to_effector = effector_node->position;
+        base_to_effector = effector_node->initial_position;
         base_to_target = effector->target_position;
-        vec3_sub_vec3(base_to_effector.f, base_node->position.f);
-        vec3_sub_vec3(base_to_target.f, base_node->position.f);
+        vec3_sub_vec3(base_to_effector.f, base_node->initial_position.f);
+        vec3_sub_vec3(base_to_target.f, base_node->initial_position.f);
 
         /* The effective distance is a lerp between these two distances */
         distance_to_target = vec3_length(base_to_target.f) * effector->weight;
         distance_to_target += vec3_length(base_to_effector.f) * (1.0 - effector->weight);
 
         /* nlerp the target position by pinning it to the base node */
-        vec3_sub_vec3(target_position->f, base_node->position.f);
+        vec3_sub_vec3(target_position->f, base_node->initial_position.f);
         vec3_normalise(target_position->f);
         vec3_mul_scalar(target_position->f, distance_to_target);
-        vec3_add_vec3(target_position->f, base_node->position.f);
+        vec3_add_vec3(target_position->f, base_node->initial_position.f);
     }
 }
 
 /* ------------------------------------------------------------------------- */
-static vec3_t
-solve_chain_forwards_with_target_rotation(struct chain_t* chain, struct effector_data_t* effector_data)
+static position_direction_t
+solve_chain_forwards_with_target_rotation(chain_t* chain)
 {
     int node_count, node_idx;
     int average_count;
-    vec3_t target_position = {{0, 0, 0}};
+    position_direction_t target;
+
+    vec3_set_zero(target.position.f);
 
     /*
      * Target position is the average of all solved child chain base positions.
      */
     average_count = 0;
-    ORDERED_VECTOR_FOR_EACH(&chain->children, struct chain_t, child)
-        struct effector_data_t child_effector_data;
-        vec3_t child_base_position = solve_chain_forwards_with_target_rotation(child, &child_effector_data);
-        vec3_add_vec3(target_position.f, child_base_position.f);
-        vec3_add_vec3(effector_data->target_direction.f, child_effector_data.target_direction.f);
-        effector_data->rotation_weight += child_effector_data.rotation_weight;
-        effector_data->rotation_weight_decay += child_effector_data.rotation_weight_decay;
+    ORDERED_VECTOR_FOR_EACH(&chain->children, chain_t, child)
+        position_direction_t child_posdir = solve_chain_forwards_with_target_rotation(child);
+        vec3_add_vec3(target.position.f, child_posdir.position.f);
+        vec3_add_vec3(target.direction.f, child_posdir.direction.f);
         ++average_count;
     ORDERED_VECTOR_END_EACH
 
@@ -215,62 +118,56 @@ solve_chain_forwards_with_target_rotation(struct chain_t* chain, struct effector
      * position. Otherwise, average the data we've been accumulating from the
      * child chains.
      */
-    if(average_count == 0)
+    if (average_count == 0)
     {
-        struct ik_node_t* effector_node = *(struct ik_node_t**)ordered_vector_get_element(&chain->nodes, 0);
-        struct ik_effector_t* effector = effector_node->effector;
-        determine_target_data_from_effector(chain, &target_position);
-
-        effector_data->rotation_weight = effector->rotation_weight;
-        effector_data->rotation_weight_decay = effector->rotation_decay;
-        /* TODO This "global direction" could be made configurable if needed */
-        effector_data->target_direction.v.x = 0.0;
-        effector_data->target_direction.v.y = 0.0;
-        effector_data->target_direction.v.z = 1.0;
-        quat_rotate_vec(effector_data->target_direction.f, effector->target_rotation.f);
-
+        ik_node_t* effector_node = *(ik_node_t**)ordered_vector_get_element(&chain->nodes, 0);
+        ik_effector_t* effector = effector_node->effector;
+        determine_target_data_from_effector(chain, &target.position);
+
+        /* TODO This "global direction" could be made configurable if (needed */
+        target.direction.v.x = 0.0;
+        target.direction.v.y = 0.0;
+        target.direction.v.z = 1.0;
+        quat_rotate_vec(target.direction.f, effector->target_rotation.f);
     }
     else
     {
         ik_real div = 1.0 / average_count;
-        vec3_mul_scalar(target_position.f, div);
-        vec3_mul_scalar(effector_data->target_direction.f, div);
-        effector_data->rotation_weight *= div;
-        effector_data->rotation_weight_decay *= div;
+        vec3_mul_scalar(target.position.f, div);
+        vec3_mul_scalar(target.direction.f, div);
     }
 
     /*
      * Iterate through each segment and apply the FABRIK algorithm.
      */
     node_count = ordered_vector_count(&chain->nodes);
-    for(node_idx = 0; node_idx < node_count - 1; ++node_idx)
+    for (node_idx = 0; node_idx < node_count - 1; ++node_idx)
     {
-        struct ik_node_t* child_node  = *(struct ik_node_t**)ordered_vector_get_element(&chain->nodes, node_idx + 0);
-        struct ik_node_t* parent_node = *(struct ik_node_t**)ordered_vector_get_element(&chain->nodes, node_idx + 1);
+        ik_node_t* child_node  = *(ik_node_t**)ordered_vector_get_element(&chain->nodes, node_idx + 0);
+        ik_node_t* parent_node = *(ik_node_t**)ordered_vector_get_element(&chain->nodes, node_idx + 1);
 
         /* move node to target */
-        child_node->solved_position = target_position;
+        child_node->position = target.position;
 
         /* lerp direction vector and segment vector */
-        vec3_sub_vec3(target_position.f, effector_data->target_direction.f);
-        vec3_sub_vec3(target_position.f, parent_node->solved_position.f);
-        vec3_mul_scalar(target_position.f, effector_data->rotation_weight);
-        vec3_add_vec3(target_position.f, parent_node->solved_position.f);
+        vec3_sub_vec3(target.position.f, target.direction.f);
+        vec3_sub_vec3(target.position.f, parent_node->position.f);
+        vec3_mul_scalar(target.position.f, parent_node->rotation_weight);
+        vec3_add_vec3(target.position.f, parent_node->position.f);
+        vec3_sub_vec3(target.position.f, child_node->position.f);
 
-        vec3_sub_vec3(target_position.f, child_node->solved_position.f);
-        vec3_normalise(target_position.f);
-        vec3_mul_scalar(target_position.f, child_node->segment_length);
-        vec3_add_vec3(target_position.f, child_node->solved_position.f);
-
-        effector_data->rotation_weight *= effector_data->rotation_weight_decay;
+        /* point segment to previous node and set target position to its end */
+        vec3_normalise(target.position.f);
+        vec3_mul_scalar(target.position.f, child_node->segment_length);
+        vec3_add_vec3(target.position.f, child_node->position.f);
     }
 
-    return target_position;
+    return target;
 }
 
 /* ------------------------------------------------------------------------- */
-static vec3_t
-solve_chain_forwards(struct chain_t* chain)
+vec3_t
+solve_chain_forwards_with_constraints(chain_t* chain)
 {
     int node_count, node_idx;
     int average_count;
@@ -280,8 +177,8 @@ solve_chain_forwards(struct chain_t* chain)
      * Target position is the average of all solved child chain base positions.
      */
     average_count = 0;
-    ORDERED_VECTOR_FOR_EACH(&chain->children, struct chain_t, child)
-        vec3_t child_base_position = solve_chain_forwards(child);
+    ORDERED_VECTOR_FOR_EACH(&chain->children, chain_t, child)
+        vec3_t child_base_position = solve_chain_forwards_with_constraints(child);
         vec3_add_vec3(target_position.f, child_base_position.f);
         ++average_count;
     ORDERED_VECTOR_END_EACH
@@ -292,7 +189,7 @@ solve_chain_forwards(struct chain_t* chain)
      * position. Otherwise, average the data we've been accumulating from the
      * child chains.
      */
-    if(average_count == 0)
+    if (average_count == 0)
         determine_target_data_from_effector(chain, &target_position);
     else
         vec3_div_scalar(target_position.f, average_count);
@@ -301,514 +198,359 @@ solve_chain_forwards(struct chain_t* chain)
      * Iterate through each segment and apply the FABRIK algorithm.
      */
     node_count = ordered_vector_count(&chain->nodes);
-    for(node_idx = 0; node_idx < node_count - 1; ++node_idx)
+    for (node_idx = 0; node_idx < node_count - 1; ++node_idx)
     {
-        struct ik_node_t* child_node  = *(struct ik_node_t**)ordered_vector_get_element(&chain->nodes, node_idx + 0);
-        struct ik_node_t* parent_node = *(struct ik_node_t**)ordered_vector_get_element(&chain->nodes, node_idx + 1);
+        /*vec3_t segment_original, segment_current;*/
+
+        ik_node_t* child_node  = *(ik_node_t**)ordered_vector_get_element(&chain->nodes, node_idx + 0);
+        ik_node_t* parent_node = *(ik_node_t**)ordered_vector_get_element(&chain->nodes, node_idx + 1);
 
         /* move node to target */
-        child_node->solved_position = target_position;
+        child_node->position = target_position;
 
         /* point segment to previous node and set target position to its end */
-        vec3_sub_vec3(target_position.f, parent_node->solved_position.f); /* parent points to child */
+        vec3_sub_vec3(target_position.f, parent_node->position.f);        /* parent points to child */
         vec3_normalise(target_position.f);                                /* normalise */
         vec3_mul_scalar(target_position.f, -child_node->segment_length);  /* child points to parent */
-        vec3_add_vec3(target_position.f, child_node->solved_position.f);  /* attach to child -- this is the new target */
+        vec3_add_vec3(target_position.f, child_node->position.f);         /* attach to child -- this is the new target */
+
+        /* Calculate global rotation of parent node *
+        segment_original = child_node->initial_position;
+        segment_current  = child_node->position;
+        vec3_sub_vec3(segment_original.f, parent_node->initial_position.f);
+        vec3_sub_vec3(segment_current.f, target_position.f);
+        vec3_angle(parent_node->rotation.f, segment_original.f, segment_current.f);
+        quat_mul_quat(parent_node->rotation.f, parent_node->initial_rotation.f);
+
+        * Convert global transform to local *
+        inv_rotation = accumulated.rotation;
+        quat_conj(inv_rotation.f);
+        quat_mul_quat(parent_node->rotation.f, inv_rotation.f);
+        vec3_sub_vec3(parent_node->position.f, accumulated.position.f);
+        quat_rotate_vec(parent_node->position.f, inv_rotation.f);
+
+        if (child_node->constraint != NULL)
+            child_node->constraint->apply(parent_node);
+
+        * Accumulate local rotation and translation for deeper nodes *after*
+         * constraint was applied *
+        accumulated_previous = accumulated;
+        quat_mul_quat(accumulated.rotation.f, parent_node->rotation.f);
+        vec3_add_vec3(accumulated.position.f, parent_node->position.f);
+
+        * Convert local transform back to global *
+        quat_rotate_vec(parent_node->position.f, accumulated_previous.rotation.f);
+        vec3_add_vec3(parent_node->position.f, accumulated_previous.position.f);
+        quat_mul_quat(parent_node->rotation.f, accumulated_previous.rotation.f);
+
+        if (child_node->constraint != NULL)
+        {
+            * XXX combine this? *
+            inv_rotation = parent_node->initial_rotation;
+            quat_conj(inv_rotation.f);
+            quat_mul_quat(parent_node->rotation.f, inv_rotation.f);
+
+            target_position = parent_node->position;
+            quat_rotate_vec(segment_original.f, parent_node->rotation.f);
+            vec3_add_vec3(target_position.f, segment_original.f);
+        }*/
     }
 
     return target_position;
 }
 
 /* ------------------------------------------------------------------------- */
-void
-solve_chain_backwards(struct chain_t* chain, vec3_t target_position)
+vec3_t
+solve_chain_forwards(chain_t* chain)
 {
-    int node_idx = ordered_vector_count(&chain->nodes) - 1;
+    int node_count, node_idx;
+    int average_count;
+    vec3_t target_position = {{0, 0, 0}};
 
     /*
-     * The base node must be set to the target position before iterating.
+     * Target position is the average of all solved child chain base positions.
      */
-    if(node_idx > 1)
-    {
-        struct ik_node_t* base_node = *(struct ik_node_t**)ordered_vector_get_element(&chain->nodes, node_idx);
-        base_node->solved_position = target_position;
-    }
+    average_count = 0;
+    ORDERED_VECTOR_FOR_EACH(&chain->children, chain_t, child)
+        vec3_t child_base_position = solve_chain_forwards(child);
+        vec3_add_vec3(target_position.f, child_base_position.f);
+        ++average_count;
+    ORDERED_VECTOR_END_EACH
 
     /*
-     * Iterate through each segment the other way around and apply the FABRIK
-     * algorithm.
+     * If there are no child chains, then the first node in the chain must
+     * contain an effector. The target position is the effector's target
+     * position. Otherwise, average the data we've been accumulating from the
+     * child chains.
      */
-    while(node_idx-- > 0)
-    {
-        struct ik_node_t* child_node  = *(struct ik_node_t**)ordered_vector_get_element(&chain->nodes, node_idx + 0);
-        struct ik_node_t* parent_node = *(struct ik_node_t**)ordered_vector_get_element(&chain->nodes, node_idx + 1);
+    if (average_count == 0)
+        determine_target_data_from_effector(chain, &target_position);
+    else
+        vec3_div_scalar(target_position.f, average_count);
 
-        /* point segment to child node and set target position to its beginning */
-        vec3_sub_vec3(target_position.f, child_node->solved_position.f);  /* child points to parent */
-        vec3_normalise(target_position.f);                                /* normalise */
-        vec3_mul_scalar(target_position.f, -child_node->segment_length);  /* parent points to child */
-        vec3_add_vec3(target_position.f, parent_node->solved_position.f); /* attach to parent -- this is the new target */
+    /*
+     * Iterate through each segment and apply the FABRIK algorithm.
+     */
+    node_count = ordered_vector_count(&chain->nodes);
+    for (node_idx = 0; node_idx < node_count - 1; ++node_idx)
+    {
+        ik_node_t* child_node  = *(ik_node_t**)ordered_vector_get_element(&chain->nodes, node_idx + 0);
+        ik_node_t* parent_node = *(ik_node_t**)ordered_vector_get_element(&chain->nodes, node_idx + 1);
 
         /* move node to target */
-        child_node->solved_position = target_position;
+        child_node->position = target_position;
+
+        /* point segment to previous node and set target position to its end */
+        vec3_sub_vec3(target_position.f, parent_node->position.f);        /* parent points to child */
+        vec3_normalise(target_position.f);                                /* normalise */
+        vec3_mul_scalar(target_position.f, -child_node->segment_length);  /* child points to parent */
+        vec3_add_vec3(target_position.f, child_node->position.f);         /* attach to child -- this is the new target */
     }
 
-    ORDERED_VECTOR_FOR_EACH(&chain->children, struct chain_t, child)
-        solve_chain_backwards(child, target_position);
-    ORDERED_VECTOR_END_EACH
+    return target_position;
 }
 
 /* ------------------------------------------------------------------------- */
 static void
-calculate_global_rotations(struct chain_t* chain)
+solve_chain_backwards_with_constraints(chain_t* chain,
+                                       vec3_t target_position,
+                                       vec3_t accumulated_positions)
 {
-    /*
-     * Calculates the "global" (world) angles of each joint and writes them to
-     * each node->solved_rotation slot.
-     *
-     * The angle between the original and solved segments are calculated using
-     * standard vector math (dot product). The axis of rotation is calculated
-     * with the cross product. From this data, a quaternion is constructed,
-     * describing this delta rotation. Finally, in order to make the rotations
-     * global instead of relative, the delta rotation is multiplied with
-     * node->rotation, which should be a quaternion describing the node's
-     * global rotation in the unsolved tree.
-     *
-     * The rotation of the base joint in the chain is returned so it can be
-     * averaged by parent chains.
-     */
-
-    int node_idx;
-    int average_count;
-    quat_t average_rotation = {{0, 0, 0, 0}};
-
-    /* Recursive into children chains */
-    average_count = 0;
-    ORDERED_VECTOR_FOR_EACH(&chain->children, struct chain_t, child)
-        quat_t rotation;
-        calculate_global_rotations(child);
-
-        /* Note: All chains that aren't the root chain *MUST* have at least two nodes */
-        assert(ordered_vector_count(&child->nodes) >= 2);
-        rotation = (*(struct ik_node_t**)
-                ordered_vector_get_element(&child->nodes,
-                    ordered_vector_count(&child->nodes) - 1))->solved_rotation;
-
-        /*
-         * Averaging quaternions taken from here
-         * http://wiki.unity3d.com/index.php/Averaging_Quaternions_and_Vectors
-         */
-        quat_normalise_sign(rotation.f);
-        quat_add_quat(average_rotation.f, rotation.f);
-        ++average_count;
-    ORDERED_VECTOR_END_EACH
+    int node_idx = ordered_vector_count(&chain->nodes) - 1;
 
     /*
-     * Assuming there was more than 1 child chain and assuming we aren't the
-     * root node, then the child chains we just iterated must share the same
-     * base node as our tip node. Average the accumulated quaternion and set
-     * this node's correct solved rotation.
+     * The base node must be set to the target position before iterating.
      */
-    if(average_count > 0 && ordered_vector_count(&chain->nodes) != 0)
+    if (node_idx > 1)
     {
-        quat_div_scalar(average_rotation.f, average_count);
-        quat_normalise(average_rotation.f);
-        (*(struct ik_node_t**)ordered_vector_get_element(&chain->nodes, 0))
-            ->solved_rotation = average_rotation;
-    }
-
-    node_idx = ordered_vector_count(&chain->nodes) - 1;
-    while(node_idx-- > 0)
-    {
-        ik_real cos_a, sin_a, angle, denominator;
-        struct ik_node_t* child_node  = *(struct ik_node_t**)ordered_vector_get_element(&chain->nodes, node_idx + 0);
-        struct ik_node_t* parent_node = *(struct ik_node_t**)ordered_vector_get_element(&chain->nodes, node_idx + 1);
-
-        /* calculate vectors for original and solved segments */
-        vec3_t segment_original = child_node->position;
-        vec3_t segment_solved   = child_node->solved_position;
-        vec3_sub_vec3(segment_original.f, parent_node->position.f);
-        vec3_sub_vec3(segment_solved.f, parent_node->solved_position.f);
-
-        /*
-         * Calculate angle between original segment and solved segment. If the
-         * angle is 0 or 180, we don't do anything. The solved rotation is
-         * initially set to the original rotation.
-         */
-        denominator = 1.0 / vec3_length(segment_original.f) / vec3_length(segment_solved.f);
-        cos_a = vec3_dot(segment_original.f, segment_solved.f) * denominator;
-        if(cos_a >= -1.0 && cos_a <= 1.0)
-        {
-            /* calculate axis of rotation and write it to the quaternion's vector section */
-            parent_node->solved_rotation.vw.v = segment_original;
-            vec3_cross(parent_node->solved_rotation.vw.v.f, segment_solved.f);
-            vec3_normalise(parent_node->solved_rotation.f);
-
-            /* quaternion's vector needs to be weighted with sin_a */
-            angle = acos(cos_a);
-            cos_a = cos(angle * 0.5);
-            sin_a = sin(angle * 0.5);
-            vec3_mul_scalar(parent_node->solved_rotation.f, sin_a);
-            parent_node->solved_rotation.q.w = cos_a;
-
-            /*
-             * Apply initial global rotation to calculated delta rotation to
-             * obtain the solved global rotation.
-             */
-            quat_mul_quat(parent_node->solved_rotation.f, parent_node->rotation.f);
-        }
+        ik_node_t* base_node = *(ik_node_t**)ordered_vector_get_element(&chain->nodes, node_idx);
+        base_node->position = target_position;
     }
-}
-
-/* ------------------------------------------------------------------------- */
-int
-solver_FABRIK_solve(struct ik_solver_t* solver)
-{
-    struct fabrik_t* fabrik = (struct fabrik_t*)solver;
-    int iteration;
-    ik_real tolerance_squared = solver->tolerance * solver->tolerance;
-
-    if(!(solver->flags & SOLVER_SKIP_RESET))
-        ik_solver_reset_solved_data(solver);
 
-    iteration = solver->max_iterations;
-    while(iteration--)
+    /*
+     * Iterate through each segment the other way around and apply the FABRIK
+     * algorithm.
+     */
+    while (node_idx-- > 0)
     {
-        vec3_t root_position;
-        struct effector_data_t effector_data;
-
-        ORDERED_VECTOR_FOR_EACH(&fabrik->chain_tree->children, struct chain_t, chain)
-
-            assert(ordered_vector_count(&chain->nodes) > 1);
-            root_position = (*(struct ik_node_t**)ordered_vector_get_element(&chain->nodes,
-                    ordered_vector_count(&chain->nodes) - 1))->position;
+        ik_node_t* child_node  = *(ik_node_t**)ordered_vector_get_element(&chain->nodes, node_idx + 0);
+        ik_node_t* parent_node = *(ik_node_t**)ordered_vector_get_element(&chain->nodes, node_idx + 1);
 
-            if(solver->flags & SOLVER_CALCULATE_TARGET_ROTATIONS)
-                solve_chain_forwards_with_target_rotation(chain, &effector_data);
-            else
-                solve_chain_forwards(chain);
-
-            solve_chain_backwards(chain, root_position);
-        ORDERED_VECTOR_END_EACH
+        /* point segment to child node and set target position to its beginning */
+        vec3_sub_vec3(target_position.f, child_node->position.f);         /* child points to parent */
+        vec3_normalise(target_position.f);                                /* normalise */
+        vec3_mul_scalar(target_position.f, -child_node->segment_length);  /* parent points to child */
+        vec3_add_vec3(target_position.f, parent_node->position.f);        /* attach to parent -- this is the new target */
+
+        /* target_position is now where the position of child_node should be. */
+
+        /* Calculate delta rotation of parent node *
+        segment_original = child_node->initial_position;
+        segment_current  = target_position;
+        vec3_sub_vec3(segment_original.f, parent_node->initial_position.f);
+        vec3_sub_vec3(segment_current.f, parent_node->position.f);
+        vec3_angle(parent_node->rotation.f, segment_original.f, segment_current.f);
+
+        *
+         * Since the initial rotation is in local space temporarily (see
+         * solve() entry point on why), we now have the rotation in local space
+         *
+        quat_mul_quat(parent_node->rotation.f, parent_node->initial_rotation.f);
+
+        * Convert global translation to local *
+        inv_rotation = accumulated_positions.rotation;
+        quat_conj(inv_rotation.f);
+        vec3_sub_vec3(parent_node->position.f, accumulated_positions.position.f);
+        quat_rotate_vec(parent_node->position.f, inv_rotation.f);
+
+        if (child_node->constraint != NULL)
+            child_node->constraint->apply(parent_node);
+
+        * Accumulate local rotation and translation for deeper nodes *after*
+         * constraint was applied *
+        accumulated_previous = accumulated_positions;
+        vec3_add_vec3(accumulated_positions.position.f, parent_node->position.f);
+
+        * Convert local transform back to global *
+        quat_rotate_vec(parent_node->position.f, accumulated_previous.rotation.f);
+        vec3_add_vec3(parent_node->position.f, accumulated_previous.position.f);
+        quat_mul_quat(parent_node->rotation.f, accumulated_previous.rotation.f);
+
+        if (child_node->constraint != NULL)
+        {
+            * XXX combine this? *
+            inv_rotation = parent_node->initial_rotation;
+            quat_conj(inv_rotation.f);
+            quat_mul_quat(parent_node->rotation.f, inv_rotation.f);
 
-        ORDERED_VECTOR_FOR_EACH(&fabrik->effector_nodes_list, struct ik_node_t*, pnode)
-            vec3_t diff = (*pnode)->solved_position;
-            vec3_sub_vec3(diff.f, (*pnode)->effector->target_position.f);
-            if(vec3_length_squared(diff.f) > tolerance_squared)
-                goto continue_iterating;
-        ORDERED_VECTOR_END_EACH
+            target_position = parent_node->position;
+            quat_rotate_vec(segment_original.f, parent_node->rotation.f);
+            vec3_add_vec3(target_position.f, segment_original.f);
+        }*/
 
-        break;
-        continue_iterating:;
+        /* move node to target */
+        child_node->position = target_position;
     }
 
-    if(solver->flags & SOLVER_CALCULATE_FINAL_ROTATIONS)
-        calculate_global_rotations(fabrik->chain_tree);
-
-    if(!(solver->flags & SOLVER_SKIP_APPLY))
-        ik_solver_iterate_tree(solver);
-
-    return 0;
+    ORDERED_VECTOR_FOR_EACH(&chain->children, chain_t, child)
+        solve_chain_backwards_with_constraints(child, target_position, accumulated_positions);
+    ORDERED_VECTOR_END_EACH
 }
 
 /* ------------------------------------------------------------------------- */
 void
-solver_FABRIK_recalculate_segment_lengths(struct ik_solver_t* solver)
+solve_chain_backwards(chain_t* chain, vec3_t target_position)
 {
-    struct fabrik_t* fabrik = (struct fabrik_t*)solver;
-    calculate_segment_lengths_recursive(fabrik->chain_tree);
-}
-
-/* ------------------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
+    int node_idx = ordered_vector_count(&chain->nodes) - 1;
 
-/* ------------------------------------------------------------------------- */
-int
-mark_involved_nodes(struct fabrik_t* solver, struct bstv_t* involved_nodes)
-{
     /*
-     * Traverse the chain of parents starting at each effector node and ending
-     * at the root node of the tree and mark every node on the way. Each
-     * effector specifies a maximum chain length, which means it's possible
-     * that we won't hit the root node.
+     * The base node must be set to the target position before iterating.
      */
-    struct ordered_vector_t* effector_nodes_list = &solver->effector_nodes_list;
-    ORDERED_VECTOR_FOR_EACH(effector_nodes_list, struct ik_node_t*, p_effector_node)
-
-        /*
-         * Set up chain length counter. If the chain length is 0 then it is
-         * infinitely long. Set the counter to -1 in this case to skip the
-         * escape condition.
-         */
-        int chain_length_counter;
-        struct ik_node_t* node = *p_effector_node;
-        assert(node->effector != NULL);
-        chain_length_counter = node->effector->chain_length == 0 ? -1 : (int)node->effector->chain_length;
-
-        /*
-         * Mark nodes that are at the base of the chain differently, so the
-         * chains can be split correctly later. Section markings will overwrite
-         * break markings.
-         */
-        for(; node != NULL; node = node->parent)
-        {
-            enum node_marking_e* current_marking;
-            enum node_marking_e marking = MARK_SECTION;
-            if(chain_length_counter == 0)
-                marking = MARK_SPLIT;
-
-            current_marking = (enum node_marking_e*)bstv_find_ptr(involved_nodes, node->guid);
-            if(current_marking == NULL)
-            {
-                if(bstv_insert(involved_nodes, node->guid, (void*)(intptr_t)marking) < 0)
-                {
-                    ik_log_message("Ran out of memory while marking involved nodes");
-                    return -1;
-                }
-            }
-            else
-            {
-                if(chain_length_counter != 0)
-                    *current_marking = marking;
-            }
-
-            if(chain_length_counter-- == 0)
-                break;
-        }
-    ORDERED_VECTOR_END_EACH
-
-    return 0;
-}
+    if (node_idx > 1)
+    {
+        ik_node_t* base_node = *(ik_node_t**)ordered_vector_get_element(&chain->nodes, node_idx);
+        base_node->position = target_position;
+    }
 
-/* ------------------------------------------------------------------------- */
-static int
-recursively_build_chain_tree(struct chain_t* chain_current,
-                             struct ik_node_t* node_base,
-                             struct ik_node_t* node_current,
-                             struct bstv_t* involved_nodes)
-{
-    int marked_children_count;
-    struct ik_node_t* child_node_base = node_base;
-    struct chain_t* child_chain = chain_current;
+    /*
+     * Iterate through each segment the other way around and apply the FABRIK
+     * algorithm.
+     */
+    while (node_idx-- > 0)
+    {
+        ik_node_t* child_node  = *(ik_node_t**)ordered_vector_get_element(&chain->nodes, node_idx + 0);
+        ik_node_t* parent_node = *(ik_node_t**)ordered_vector_get_element(&chain->nodes, node_idx + 1);
 
-    /* can remove the mark from the set to speed up future checks */
-    enum node_marking_e marking =
-        (enum node_marking_e)(intptr_t)bstv_erase(involved_nodes, node_current->guid);
+        /* point segment to child node and set target position to its beginning */
+        vec3_sub_vec3(target_position.f, child_node->position.f);         /* child points to parent */
+        vec3_normalise(target_position.f);                                /* normalise */
+        vec3_mul_scalar(target_position.f, -child_node->segment_length);  /* parent points to child */
+        vec3_add_vec3(target_position.f, parent_node->position.f);        /* attach to parent -- this is the new target */
 
-    switch(marking)
-    {
-        /*
-         * If this node was marked as the base of a chain then split the chain at
-         * this point by moving the pointer to the base node down the tree to us.
-         */
-        case MARK_SPLIT:
-            child_node_base = node_current;
-            break;
-        /*
-         * If this node is not marked at all, cut off any previous chain but
-         * continue (fall through) as if a section was marked. It's possible
-         * that there are isolated chains somewhere further down the tree.
-         */
-        case MARK_NONE:
-            node_base = node_current;
-
-        case MARK_SECTION:
-            /*
-             * If the current node has at least two children marked as sections
-             * or if the current node is an effector node, but only if the base
-             * node is not equal to this node (that is, we need to avoid chains
-             * that would have less than 2 nodes), then we must also split the
-             * chain at this point.
-             */
-            marked_children_count = 0;
-            BSTV_FOR_EACH(&node_current->children, struct ik_node_t, child_guid, child)
-                if((enum node_marking_e)(intptr_t)bstv_find(involved_nodes, child_guid) == MARK_SECTION)
-                    if(++marked_children_count == 2)
-                        break;
-            BSTV_END_EACH
-            if((marked_children_count == 2 || node_current->effector != NULL) && node_current != node_base)
-            {
-                /*
-                 * Emplace a chain object into the current chain's vector of children
-                 * and initialise it.
-                 */
-                struct ik_node_t* node;
-                child_chain = ordered_vector_push_emplace(&chain_current->children);
-                if(child_chain == NULL)
-                    return -1;
-                chain_construct(child_chain);
-
-                /*
-                 * Add points to all nodes that are part of this chain into the chain's
-                 * list, starting with the end node.
-                 */
-                for(node = node_current; node != node_base; node = node->parent)
-                    ordered_vector_push(&child_chain->nodes, &node);
-                ordered_vector_push(&child_chain->nodes, &node_base);
-
-                /*
-                 * Update the base node to be this node so deeper chains are built back
-                 * to this node
-                 */
-                child_node_base = node_current;
-            }
-            break;
+        /* move node to target */
+        child_node->position = target_position;
     }
 
-    /* Recurse into children of the current node. */
-    BSTV_FOR_EACH(&node_current->children, struct ik_node_t, child_guid, child_node)
-        if(recursively_build_chain_tree(
-                child_chain,
-                child_node_base,
-                child_node,
-                involved_nodes) < 0)
-            return -1;
-    BSTV_END_EACH
-
-    return 0;
+    ORDERED_VECTOR_FOR_EACH(&chain->children, chain_t, child)
+        solve_chain_backwards(child, target_position);
+    ORDERED_VECTOR_END_EACH
 }
 
 /* ------------------------------------------------------------------------- */
 static void
-calculate_segment_lengths_recursive(struct chain_t* chain)
+initial_to_global_recursive(ik_node_t* node, quat_t acc_rot)
 {
-    int last_idx = ordered_vector_count(&chain->nodes) - 1;
-    while(last_idx-- > 0)
-    {
-        struct ik_node_t* child_node =
-            *(struct ik_node_t**)ordered_vector_get_element(&chain->nodes, last_idx + 0);
-        struct ik_node_t* parent_node =
-            *(struct ik_node_t**)ordered_vector_get_element(&chain->nodes, last_idx + 1);
-
-        vec3_t diff = child_node->position;
-        vec3_sub_vec3(diff.f, parent_node->position.f);
-        child_node->segment_length = vec3_length(diff.f);
-    }
+    quat_t rotation = node->initial_rotation;
+    quat_mul_quat(node->initial_rotation.f, acc_rot.f);
+    quat_mul_quat(acc_rot.f, rotation.f);
 
-    ORDERED_VECTOR_FOR_EACH(&chain->children, struct chain_t, child)
-        calculate_segment_lengths_recursive(child);
-    ORDERED_VECTOR_END_EACH
+    BSTV_FOR_EACH(&node->children, ik_node_t, guid, child)
+        initial_to_global_recursive(child, acc_rot);
+    BSTV_END_EACH
 }
-
-/* ------------------------------------------------------------------------- */
-static int
-count_chains(struct chain_t* chain)
+void
+initial_rotation_to_global(ik_node_t* node)
 {
-    int counter = 1;
-    ORDERED_VECTOR_FOR_EACH(&chain->children, struct chain_t, child)
-        counter += count_chains(child);
-    ORDERED_VECTOR_END_EACH
-    return counter;
+    quat_t acc_rot = {{0, 0, 0, 1}};
+    initial_to_global_recursive(node, acc_rot);
 }
 
 /* ------------------------------------------------------------------------- */
-#if IK_DOT_OUTPUT == ON
 static void
-dump_chain(struct chain_t* chain, FILE* fp)
+initial_to_local_recursive(ik_node_t* node, quat_t acc_rot)
 {
-    int last_idx = ordered_vector_count(&chain->nodes) - 1;
-    if(last_idx > 0)
-    {
-        fprintf(fp, "    %d [shape=record];\n",
-            (*(struct ik_node_t**)ordered_vector_get_element(&chain->nodes, 0))->guid);
-        fprintf(fp, "    %d [shape=record];\n",
-            (*(struct ik_node_t**)ordered_vector_get_element(&chain->nodes, last_idx))->guid);
-    }
-
-    while(last_idx-- > 0)
-    {
-        fprintf(fp, "    %d -- %d [color=\"1.0 0.5 1.0\"];\n",
-            (*(struct ik_node_t**)ordered_vector_get_element(&chain->nodes, last_idx + 0))->guid,
-            (*(struct ik_node_t**)ordered_vector_get_element(&chain->nodes, last_idx + 1))->guid);
-    }
+    quat_t inv_rotation = acc_rot;
+    quat_conj(inv_rotation.f);
+    quat_mul_quat(node->initial_rotation.f, inv_rotation.f);
+    quat_mul_quat(acc_rot.f, node->initial_rotation.f);
 
-    ORDERED_VECTOR_FOR_EACH(&chain->children, struct chain_t, child)
-        dump_chain(child, fp);
-    ORDERED_VECTOR_END_EACH
-}
-static void
-dump_node(struct ik_node_t* node, FILE* fp)
-{
-    if(node->effector != NULL)
-        fprintf(fp, "    %d [color=\"0.6 0.5 1.0\"];\n", node->guid);
-    BSTV_FOR_EACH(&node->children, struct ik_node_t, guid, child)
-        fprintf(fp, "    %d -- %d;\n", node->guid, guid);
-        dump_node(child, fp);
+    BSTV_FOR_EACH(&node->children, ik_node_t, guid, child)
+        initial_to_local_recursive(child, acc_rot);
     BSTV_END_EACH
 }
-static void
-dump_to_dot(struct ik_node_t* node, struct chain_t* chain, const char* file_name)
+void
+initial_rotation_to_local(ik_node_t* node)
 {
-    FILE* fp = fopen(file_name, "w");
-    if(fp == NULL)
-        return;
-
-    fprintf(fp, "graph chain_tree {\n");
-    dump_node(node, fp);
-    dump_chain(chain, fp);
-    fprintf(fp, "}\n");
-
-    fclose(fp);
+    quat_t acc_rot = {{0, 0, 0, 1}};
+    initial_to_local_recursive(node, acc_rot);
 }
-#endif
 
 /* ------------------------------------------------------------------------- */
-static int
-rebuild_chain_tree(struct fabrik_t* solver)
+int
+solver_FABRIK_solve(ik_solver_t* solver)
 {
-    struct bstv_t involved_nodes;
-    int involved_nodes_count;
-#if IK_DOT_OUTPUT == ON
-    char buffer[20];
-    static int file_name_counter = 0;
-#endif
+    int result = 0;
+    fabrik_t* fabrik = (fabrik_t*)solver;
+    int iteration = solver->max_iterations;
+    ik_real tolerance_squared = solver->tolerance * solver->tolerance;
 
     /*
-     * Build a set of all nodes that are in a direct path with all of the
-     * effectors.
+     * NOTE: Kind of a hack. Initial rotations are in local space during
+     * iteration.
+     *
+     * FABRIK works entirely in global space, so when constraints come into
+     * play, it is necessary to calculate joint angles and convert global
+     * positions into local positions. The constrained angles are then
+     * converted back again into global space.
+     *
+     * As you can imagine, this process is costly. We can actually cut down on
+     * a significant number of operations if the initial rotations are in local
+     * space. The algorithm doesn't need initial rotations, so this should have
+     * no side effects. We just need to make sure to convert the rotations back
+     * into global space after the algorithm has completed.
      */
-    bstv_construct(&involved_nodes);
-    if(mark_involved_nodes(solver, &involved_nodes) < 0)
-        goto mark_involved_nodes_failed;
-    involved_nodes_count = bstv_count(&involved_nodes);
+    if (solver->flags & SOLVER_ENABLE_CONSTRAINTS)
+        initial_rotation_to_local(solver->tree);
 
-    /*
-     * The user can choose to set the root node as a chain terminator (default)
-     * or choose to exclude the root node, in which case each immediate child
-     * of the tree is a chain terminator. In this case we need to build the
-     * chain tree for each child individually.
-     */
-    chain_clear_free(solver->chain_tree);
-    if(solver->flags & SOLVER_EXCLUDE_ROOT)
+    while (iteration-- > 0)
     {
-        BSTV_FOR_EACH(&solver->tree->children, struct ik_node_t, guid, child)
-            recursively_build_chain_tree(solver->chain_tree, child, child, &involved_nodes);
-        BSTV_END_EACH
-    }
-    else
-    {
-        recursively_build_chain_tree(solver->chain_tree, solver->tree, solver->tree, &involved_nodes);
-    }
+        vec3_t root_position;
 
-    /* Pre-compute offsets for each node in the chain tree in relation to their
-     * parents */
-    calculate_segment_lengths_recursive(solver->chain_tree);
+        /* Actual algorithm here */
+        ORDERED_VECTOR_FOR_EACH(&fabrik->chain_tree.islands, chain_island_t, island)
+            chain_t* root_chain = &island->root_chain;
 
-    /* DEBUG: Save chain tree to DOT */
-#if IK_DOT_OUTPUT == ON
-    sprintf(buffer, "tree%d.dot", file_name_counter++);
-    dump_to_dot(solver->tree, solver->chain_tree, buffer);
-#endif
+            /* The algorithm assumes chains have at least one bone. This should
+             * be asserted while building the chain trees, but it can't hurt
+             * to double check */
+            assert(ordered_vector_count(&root_chain->nodes) > 1);
 
-    ik_log_message("There are %d effector(s) involving %d node(s). %d chain(s) were created",
-                   ordered_vector_count(&solver->effector_nodes_list),
-                   involved_nodes_count,
-                   count_chains(solver->chain_tree) - 1); /* don't count root chain which always exists */
+            root_position = (*(ik_node_t**)ordered_vector_get_element(&root_chain->nodes,
+                    ordered_vector_count(&root_chain->nodes) - 1))->initial_position;
 
-    bstv_clear_free(&involved_nodes);
+            if (solver->flags & SOLVER_CALCULATE_TARGET_ROTATIONS)
+                solve_chain_forwards_with_target_rotation(root_chain);
+            else
+                solve_chain_forwards(root_chain);
 
-    return 0;
+            if (solver->flags & SOLVER_ENABLE_CONSTRAINTS)
+                solve_chain_backwards_with_constraints(root_chain, root_position, root_position);
+            else
+                solve_chain_backwards(root_chain, root_position);
+        ORDERED_VECTOR_END_EACH
+
+        /* Check if (all effectors are within range */
+        ORDERED_VECTOR_FOR_EACH(&fabrik->effector_nodes_list, ik_node_t*, pnode)
+            vec3_t diff = (*pnode)->position;
+            vec3_sub_vec3(diff.f, (*pnode)->effector->target_position.f);
+            if (vec3_length_squared(diff.f) > tolerance_squared)
+            {
+                result = 1; /* converged */
+                break;
+            }
+        ORDERED_VECTOR_END_EACH
+    }
+
+    /* Restore initial rotations to global space again. See above as to why. */
+    if (solver->flags & SOLVER_ENABLE_CONSTRAINTS)
+        initial_rotation_to_global(solver->tree);
+
+    if (solver->flags & SOLVER_CALCULATE_FINAL_ROTATIONS)
+    {
+        ORDERED_VECTOR_FOR_EACH(&fabrik->chain_tree.islands, chain_island_t, island)
+            calculate_global_rotations(&island->root_chain);
+        ORDERED_VECTOR_END_EACH
+    }
 
-    mark_involved_nodes_failed : bstv_clear_free(&involved_nodes);
-    return -1;
+    return result;
 }

Source/ThirdParty/ik/src/solver_MSS.c

@@ -0,0 +1,24 @@
+#include "ik/solver_MSS.h"
+
+/* ------------------------------------------------------------------------- */
+int
+solver_MSS_construct(ik_solver_t* solver)
+{
+    solver->destruct = solver_MSS_destruct;
+    solver->solve = solver_MSS_solve;
+    
+    return 0;
+}
+
+/* ------------------------------------------------------------------------- */
+void 
+solver_MSS_destruct(ik_solver_t* solver)
+{
+}
+
+/* ------------------------------------------------------------------------- */
+int 
+solver_MSS_solve(ik_solver_t* solver)
+{
+    return 0;
+}

Source/ThirdParty/ik/src/util.c

@@ -0,0 +1,98 @@
+#include "ik/chain_tree.h"
+#include "ik/effector.h"
+#include "ik/node.h"
+#include "ik/util.h"
+#include <assert.h>
+
+typedef struct effector_data_t
+{
+    ik_real rotation_weight;
+    ik_real rotation_weight_decay;
+} effector_data_t;
+
+/* ------------------------------------------------------------------------- */
+static effector_data_t
+calculate_rotation_weight_decays_recursive(chain_t* chain)
+{
+    int average_count;
+    int node_idx, node_count;
+    effector_data_t effector_data;
+
+    /*
+     * Find the rotation weight of this chain's last node by averaging the
+     * rotation weight of all child chain's first nodes.
+     *
+     * If there are no child chains, then the first node in the chain must
+     * contain an effector. Initialise the weight parameters from said
+     * effector.
+     *
+     * If there are child chains then average the effector data we've
+     * accumulated.
+     */
+    average_count = 0;
+    ORDERED_VECTOR_FOR_EACH(&chain->children, chain_t, child)
+        effector_data_t child_eff_data =
+                calculate_rotation_weight_decays_recursive(child);
+        effector_data.rotation_weight += child_eff_data.rotation_weight;
+        effector_data.rotation_weight_decay += child_eff_data.rotation_weight_decay;
+        ++average_count;
+    ORDERED_VECTOR_END_EACH
+
+    if (average_count == 0)
+    {
+        ik_node_t* effector_node = *(ik_node_t**)ordered_vector_get_element(&chain->nodes, 0);
+        ik_effector_t* effector = effector_node->effector;
+
+        effector_data.rotation_weight = effector->rotation_weight;
+        effector_data.rotation_weight_decay = effector->rotation_decay;
+    }
+    else
+    {
+        ik_real div = 1.0 / average_count;
+        effector_data.rotation_weight *= div;
+        effector_data.rotation_weight_decay *= div;
+    }
+
+    /*
+     * With the rotation weight of the last node calculated, we can now iterate
+     * the nodes in the chain and set each rotation weight, decaying a little
+     * bit every time. Note that we exclude the last node, because it will
+     * be handled by the parent chain. If there is no parent chain then the
+     * non-recursive caller of this function will set the rotation weight of
+     * the root node.
+     */
+    node_count = ordered_vector_count(&chain->nodes) - 1;
+    for (node_idx = 0; node_idx < node_count; ++node_idx)
+    {
+        ik_node_t* node = *(ik_node_t**)ordered_vector_get_element(&chain->nodes, node_idx);
+        node->rotation_weight = effector_data.rotation_weight;
+        effector_data.rotation_weight *= effector_data.rotation_weight_decay;
+    }
+
+    /* Rotation weight is now equal to that of this chain's base node */
+    return effector_data;
+}
+
+/* ------------------------------------------------------------------------- */
+void
+ik_calculate_rotation_weight_decays(chain_tree_t* chain_tree)
+{
+    /*
+     * The recursive version of this function does not set the rotation weight
+     * of the first node in every tree that gets passed to it, but it does
+     * return the rotation weight that *would have been set* (which gets used
+     * recursively to calculate the average rotation weight in the case of
+     * multiple child chains).
+     *
+     * For these reasons we iterate the chain islands and set the first node in
+     * each island to the returned rotation weight.
+     */
+    ORDERED_VECTOR_FOR_EACH(&chain_tree->islands, chain_island_t, island)
+        ik_node_t* root_node;
+        effector_data_t effector_data = calculate_rotation_weight_decays_recursive(&island->root_chain);
+        int last_idx = ordered_vector_count(&island->root_chain.nodes) - 1;
+        assert(last_idx > 0);
+        root_node = *(ik_node_t**)ordered_vector_get_element(&island->root_chain.nodes, last_idx);
+        root_node->rotation_weight = effector_data.rotation_weight;
+    ORDERED_VECTOR_END_EACH
+}

Source/ThirdParty/ik/src/vec3.c

@@ -1,17 +1,18 @@
 #include "ik/vec3.h"
+#include "ik/quat.h"
 #include <string.h>
 #include <math.h>
 
 /* ------------------------------------------------------------------------- */
 void
-vec3_set_zero(ik_real* v)
+vec3_set_zero(ik_real* IK_RESTRICT v)
 {
     memset(v, 0, sizeof *v);
 }
 
 /* ------------------------------------------------------------------------- */
 void
-vec3_add_vec3(ik_real* v1, const ik_real* v2)
+vec3_add_vec3(ik_real* IK_RESTRICT v1, const ik_real* IK_RESTRICT v2)
 {
     v1[0] += v2[0];
     v1[1] += v2[1];
@@ -20,7 +21,7 @@ vec3_add_vec3(ik_real* v1, const ik_real* v2)
 
 /* ------------------------------------------------------------------------- */
 void
-vec3_sub_vec3(ik_real* v1, const ik_real* v2)
+vec3_sub_vec3(ik_real* IK_RESTRICT v1, const ik_real* IK_RESTRICT v2)
 {
     v1[0] -= v2[0];
     v1[1] -= v2[1];
@@ -29,7 +30,7 @@ vec3_sub_vec3(ik_real* v1, const ik_real* v2)
 
 /* ------------------------------------------------------------------------- */
 void
-vec3_mul_scalar(ik_real* v, ik_real scalar)
+vec3_mul_scalar(ik_real* IK_RESTRICT v, ik_real scalar)
 {
     v[0] *= scalar;
     v[1] *= scalar;
@@ -38,7 +39,7 @@ vec3_mul_scalar(ik_real* v, ik_real scalar)
 
 /* ------------------------------------------------------------------------- */
 void
-vec3_div_scalar(ik_real* v, ik_real scalar)
+vec3_div_scalar(ik_real* IK_RESTRICT v, ik_real scalar)
 {
     v[0] /= scalar;
     v[1] /= scalar;
@@ -47,33 +48,39 @@ vec3_div_scalar(ik_real* v, ik_real scalar)
 
 /* ------------------------------------------------------------------------- */
 ik_real
-vec3_length_squared(const ik_real* v)
+vec3_length_squared(const ik_real* IK_RESTRICT v)
 {
     return vec3_dot(v, v);
 }
 
 /* ------------------------------------------------------------------------- */
 ik_real
-vec3_length(const ik_real* v)
+vec3_length(const ik_real* IK_RESTRICT v)
 {
     return sqrt(vec3_length_squared(v));
 }
 
 /* ------------------------------------------------------------------------- */
 void
-vec3_normalise(ik_real* v)
+vec3_normalise(ik_real* IK_RESTRICT v)
 {
     ik_real length = vec3_length(v);
-    if(length != 0.0)
+    if (length != 0.0)
+    {
         length = 1.0 / length;
-    v[0] *= length;
-    v[1] *= length;
-    v[2] *= length;
+        v[0] *= length;
+        v[1] *= length;
+        v[2] *= length;
+    }
+    else
+    {
+        v[0] = 1;
+    }
 }
 
 /* ------------------------------------------------------------------------- */
 ik_real
-vec3_dot(const ik_real* v1, const ik_real* v2)
+vec3_dot(const ik_real* IK_RESTRICT v1, const ik_real* IK_RESTRICT v2)
 {
     return v1[0] * v2[0] +
            v1[1] * v2[1] +
@@ -82,7 +89,7 @@ vec3_dot(const ik_real* v1, const ik_real* v2)
 
 /* ------------------------------------------------------------------------- */
 void
-vec3_cross(ik_real* v1, const ik_real* v2)
+vec3_cross(ik_real* IK_RESTRICT v1, const ik_real* IK_RESTRICT v2)
 {
     ik_real v1x = v1[1] * v2[2] - v2[1] * v1[2];
     ik_real v1z = v1[0] * v2[1] - v2[0] * v1[1];
@@ -90,3 +97,32 @@ vec3_cross(ik_real* v1, const ik_real* v2)
     v1[0] = v1x;
     v1[2] = v1z;
 }
+
+/* ------------------------------------------------------------------------- */
+void
+vec3_angle(ik_real* IK_RESTRICT q, const ik_real* IK_RESTRICT v1, const ik_real* IK_RESTRICT v2)
+{
+    ik_real cos_a, sin_a, angle, denominator;
+
+    denominator = 1.0 / vec3_length(v1) / vec3_length(v2);
+    cos_a = vec3_dot(v1, v2) * denominator;
+    if (cos_a >= -1.0 && cos_a <= 1.0)
+    {
+        /* calculate axis of rotation and write it to the quaternion's vector section */
+        memcpy(q, v1, sizeof(ik_real) * 3);
+        vec3_cross(q, v2);
+        vec3_normalise(q);
+
+        /* quaternion's vector needs to be weighted with sin_a */
+        angle = acos(cos_a);
+        cos_a = cos(angle * 0.5);
+        sin_a = sin(angle * 0.5);
+        vec3_mul_scalar(q, sin_a);
+        q[3] = cos_a; /* w component */
+    }
+    else
+    {
+        /* Important! otherwise garbage happens when applying initial rotations */
+        quat_set_identity(q);
+    }
+}