From 95376cffab0512c5ced1066757173c7a8779d049 Mon Sep 17 00:00:00 2001
From: BeardedBread <randomsity1@gmail.com>
Date: Tue, 17 Aug 2021 20:57:33 +0800
Subject: [PATCH] Add controls to change target and root

Internal Changelog:
- Add follow mode of target
- Add LMB to set root
---
 joints.c | 26 ++++++++++++++++++++------
 joints.h |  1 +
 main.c   | 13 ++++++++++---
 3 files changed, 31 insertions(+), 9 deletions(-)

diff --git a/joints.c b/joints.c
index 0c80ab2..743b7c7 100644
--- a/joints.c
+++ b/joints.c
@@ -11,9 +11,10 @@ Body* init_body(unsigned int N, int total_length, float root_x, float root_y){
     body->links_lengths = (float *)calloc(N - 1, sizeof(float));
     float link_len = total_length * 1.0f / ((N - 1)*1.0f);
     body->total_length = link_len * (N - 1);
+    body->current_length = link_len * (N - 1);
     body->target = (Vector2){root_x, body->total_length};
     body->final_target = (Vector2){root_x, body->total_length};
-    body->angle_limit = PI;
+    body->angle_limit = PI/6;
 
     body->root_pos = (Vector2){root_x, root_y};
     body->root = (Joint *)malloc(sizeof(Joint));
@@ -43,10 +44,21 @@ Body* init_body(unsigned int N, int total_length, float root_x, float root_y){
     return body;
 }
 
+void set_body_root(Body* body, Vector2 new_pos){
+    Vector2 offset = Vector2Subtract(new_pos, body->root_pos);
+    set_body_target(body, Vector2Add(body->final_target, offset));
+
+    body->root_pos = new_pos;
+}
+
 void set_body_target(Body* body, Vector2 new_target){
     body->final_target = new_target;
 }
 
+void set_current_length(Body* body, float new_length){
+
+}
+
 Vector2 _get_new_pos(Vector2 p1, Vector2 p2, float length){
     float dist = Vector2Distance(p1, p2);
     float lambda = length / dist;
@@ -71,7 +83,7 @@ Vector2 _limit_new_pos(Vector2 a, Vector2 b, Vector2 c, float length, float angl
         mu *= -1;
 
     Vector2 v = Vector2Add(Vector2Scale(e_hat, lambda), Vector2Scale(f_hat, mu));
-    return Vector2Add(b, Vector2Scale(v, length / Vector2Length(v)));
+    return Vector2Add(b, v);
 }
 
 void update_body(Body *body){
@@ -80,7 +92,7 @@ void update_body(Body *body){
     // Check distance
     float dist = Vector2Distance(body->root->pos, body->target);
     unsigned int i = 0;
-    if (dist > body->total_length){
+    if (dist >= body->total_length){
         for (Joint *jt=body->root->child;jt != NULL; jt= jt->child){
             jt->pos = _get_new_pos(body->target, jt->parent->pos, body->links_lengths[i]);
             ++i;
@@ -128,10 +140,12 @@ void draw_body(Body* body){
     for (Joint* jt = body->root; jt != NULL; jt = jt->child){
         if (jt->child != NULL)
             DrawLineV(jt->pos, jt->child->pos, BLACK);
-        if (jt != body->root)
-            DrawCircleV(jt->pos, 5, BLUE);
-        else
+        if (jt == body->root)
             DrawCircleV(jt->pos, 5, BLACK);
+#ifdef DEBUG
+        else
+            DrawCircleV(jt->pos, 5, BLUE);
+#endif
     }
     DrawCircleV(body->target, 3, RED); 
 }
diff --git a/joints.h b/joints.h
index 41e4acb..58e6fbf 100644
--- a/joints.h
+++ b/joints.h
@@ -13,6 +13,7 @@ typedef struct _Body {
     unsigned int N;
     float* links_lengths;
     float total_length;
+    float current_length;
     float angle_limit;
     Joint* root;
     Joint* end;
diff --git a/main.c b/main.c
index 9c751a0..384476b 100644
--- a/main.c
+++ b/main.c
@@ -1,18 +1,25 @@
 #include "joints.h"
+#include "raylib.h"
 
 int main(int argc, char** argv) {
     const unsigned int scrnWidth = 800;
     const unsigned int scrnHeight = 600;
 
     InitWindow(scrnWidth, scrnHeight, "FABRIK");
-    SetTargetFPS(60);
+    SetTargetFPS(30);
 
     Body* body = init_body(15, 300, 400, 500);
+    bool follow_mode = false;
 
     while(!WindowShouldClose()){
-        if (IsMouseButtonReleased(MOUSE_BUTTON_LEFT)){
+        if (IsKeyReleased(KEY_F))
+            follow_mode = !follow_mode;
+
+        if (follow_mode)
             set_body_target(body, GetMousePosition());
-        }
+        if (IsMouseButtonReleased(MOUSE_BUTTON_LEFT))
+            set_body_root(body, GetMousePosition());
+
         update_body(body);
         BeginDrawing();
             ClearBackground(RAYWHITE);