Add Makefile and tweak makefile

- Limit FABRIK iteration to 100
- Fill background to GREEN for greenscreen purposes
- Draw circles at joints to smooth out the inter-joints

Makefile

@@ -4,7 +4,7 @@ BUILD_DIR = build/
 OBJ = $(patsubst %.c,$(BUILD_DIR)%.o,$(SRC_FILES))
 DEP = $(OBJ:.o=.d)
 
-RAYLIB_DIR = $(HOME)/Documents/Coding/raylib/build/
+RAYLIB_DIR ?= $(HOME)/Documents/Coding/raylib/build/
 LIB_DIRS = -L$(RAYLIB_DIR)lib
 INCLUDE_DIRS = -I$(RAYLIB_DIR)include -I.
 LIBS = -lraylib -lGL -lm -lpthread -ldl -lrt -lX11

README.md

@@ -0,0 +1,15 @@
+# FABRIK in C
+Implementation of 2D FABRIK in C. Drawing and input handling are done with [raylib](https://www.raylib.com/).
+
+## Build instruction
+1. Install raylib
+2. Set the RAYLIB\_DIR in the Makefile
+3. `make`
+
+## Usage
+- LMB to set the root position
+- `f` to toggle mouse following
+- `o` and `p` to decrease and increase the length respectively
+
+## Disclaimer
+This is a toy program. Probably won't get any more serious than this.

joints.c

@@ -4,26 +4,27 @@
 #include <stdio.h>
 #include <string.h>
 
-Body* init_body(unsigned int N, int total_length, float root_x, float root_y){
+Body* init_body(int N, float link_length, float root_x, float root_y){
     Body *body = (Body *)malloc(sizeof(Body));
     body->N = N;
     body->total_length = 0;
 
-    float link_len = total_length * 1.0f / ((N - 1)*1.0f);
+    body->link_length = link_length;
     body->links_lengths = (float *)calloc(N - 1, sizeof(float));
-    body->total_length = link_len * (N - 1);
+    body->total_length = link_length * (N - 1);
-    body->current_length = link_len * (N - 1);
+    body->current_length = body->total_length;
+    body->target_length = link_length * (N - 1);
     body->target = (Vector2){root_x, body->total_length};
     body->final_target = (Vector2){root_x, body->total_length};
-    body->angle_limit = PI/6;
+    body->angle_limit = PI;
 
     body->root_pos = (Vector2){root_x, root_y};
 
     body->joints = (Joint *)calloc(N, sizeof(Joint));
     body->joints[0].pos = body->root_pos;
     for (int i = 1; i<N;++i){
-        body->links_lengths[i-1] = link_len;
+        body->links_lengths[i-1] = link_length;
-        body->joints[i].pos = (Vector2){root_x, root_y - link_len * i};
+        body->joints[i].pos = (Vector2){root_x, root_y - link_length * i};
     }
 
 #ifdef DEBUG
@@ -44,12 +45,31 @@ void set_body_target(Body* body, Vector2 new_target){
     body->final_target = new_target;
 }
 
-void set_current_length(Body* body, float new_length){
+void set_length_target(Body* body, float new_length){
+    body->target_length = Clamp(new_length, 0, body->total_length);
+}
 
+void _update_body_length(Body* body){
+    body->current_length = Lerp(body->current_length, body->target_length, 0.2);
+    body->N = (int)(body->current_length / body->link_length);
+
+    for (int i=0; i< body->N; ++i)
+        body->links_lengths[i] = body->link_length;
+
+    float remainder = body->current_length - body->N * body->link_length;
+    if (remainder >= 1) {
+        body->links_lengths[body->N] = remainder;
+        body->N += 1;
+    }
+    body->N += 1;
 }
 
 Vector2 _get_new_pos(Vector2 p1, Vector2 p2, float length){
     float dist = Vector2Distance(p1, p2);
+    // Ensure it's not zero. Due to integer math, this is possible
+    // Just set it to 1 and let it settle
+    dist = (dist == 0) ? 1 : dist; 
+
     float lambda = length / dist;
     return Vector2Add(
         Vector2Scale(p2,1-lambda),
@@ -76,20 +96,28 @@ Vector2 _limit_new_pos(Vector2 a, Vector2 b, Vector2 c, float length, float angl
 }
 
 void update_body(Body *body){
+    _update_body_length(body);
     body->target = Vector2Lerp(body->target, body->final_target, 0.2);
 
     // Check distance
     float dist = Vector2Distance(body->joints[0].pos, body->target);
-    if (dist >= body->total_length){
+    if (dist >= body->current_length){
-        for(unsigned int i = 1; i< body->N; ++i){
+        for(int i = 0; i< body->N-1; ++i){
-            body->joints[i].pos = _get_new_pos(body->target, body->joints[i-1].pos, body->links_lengths[i]);
+            body->joints[i+1].pos = _get_new_pos(
+                body->target,
+                body->joints[i].pos,
+                body->links_lengths[i]
+            );
         }
        return;
     }
 
+    // Perform FABRIK
     double error = 1;
+    int iterations = 0;
     Vector2 prev_pos; 
-    while (error > 1e-2){
+    while (error > 1e-2 && iterations < 100){
+        ++iterations;
         prev_pos = body->joints[body->N-1].pos;
 
         // Backward Pass
@@ -132,14 +160,18 @@ void update_body(Body *body){
 
 void draw_body(Body* body){
     DrawCircleV(body->joints[0].pos, 5, BLACK);
-    DrawLineV(body->joints[0].pos, body->joints[1].pos, BLACK);
+    DrawLineEx(body->joints[0].pos, body->joints[1].pos, body->N, BLACK);
     for(int i = 1; i < body->N-1; ++i){
-        DrawLineV(body->joints[i].pos, body->joints[i+1].pos, BLACK);
+        DrawLineEx(body->joints[i].pos, body->joints[i+1].pos, body->N - i, BLACK);
 #ifdef DEBUG
         DrawCircleV(body->joints[i].pos, 5, BLUE);
+#else
+        DrawCircleV(body->joints[i].pos,(int)((body->N - i) / 2), BLACK);
 #endif
     }
+#ifdef DEBUG
     DrawCircleV(body->target, 3, RED); 
+#endif
 }
 
 void free_body(Body *body){

joints.h

@@ -6,21 +6,24 @@ typedef struct _Joint {
 
 typedef struct _Body {
     Vector2 final_target;
+    int N;
+    float total_length;
+    float angle_limit;
     Vector2 target;
     Vector2 root_pos;
-    unsigned int N;
+    float link_length;
     float* links_lengths;
-    float total_length;
     float current_length;
-    float angle_limit;
+    float target_length;
     Joint* joints;
 } Body;
 
 
-Body* init_body(unsigned int N, int total_length, float root_x, float root_y);
+Body* init_body(int N, float link_length, float root_x, float root_y);
 void set_body_root(Body* body, Vector2 new_pos);
 void set_body_target(Body* body, Vector2 new_target);
-void set_link_length(Body* body, unsigned int idx, double length);
+void set_length_target(Body* body, float new_length);
+void _update_body_length(Body* body);
 void update_body(Body* body);
 void draw_body(Body* body);
 void free_body(Body* body);

main.c

@@ -6,12 +6,16 @@ int main(int argc, char** argv) {
     const unsigned int scrnHeight = 600;
 
     InitWindow(scrnWidth, scrnHeight, "FABRIK");
-    SetTargetFPS(30);
+    SetTargetFPS(60);
 
-    Body* body = init_body(15, 300, 400, 500);
+    Body* body = init_body(15, 20, 400, 500);
     bool follow_mode = false;
 
     while(!WindowShouldClose()){
+        if (IsKeyReleased(KEY_O))
+            set_length_target(body, body->current_length - 30);
+        if (IsKeyReleased(KEY_P))
+            set_length_target(body, body->current_length + 30);
         if (IsKeyReleased(KEY_F))
             follow_mode = !follow_mode;
 
@@ -22,7 +26,7 @@ int main(int argc, char** argv) {
 
         update_body(body);
         BeginDrawing();
-            ClearBackground(RAYWHITE);
+            ClearBackground(GREEN);
             draw_body(body);
         EndDrawing();
     }