From 9ac405451cf3e41614b9570121ed3dde9b2c9af4 Mon Sep 17 00:00:00 2001 From: BeardedBread Date: Sat, 14 Aug 2021 23:07:52 +0800 Subject: [PATCH] First commit for FABRIK --- .gitignore | 4 ++ Makefile | 39 ++++++++++++++++++ joints.c | 119 +++++++++++++++++++++++++++++++++++++++++++++++++++++ joints.h | 27 ++++++++++++ main.c | 24 +++++++++++ 5 files changed, 213 insertions(+) create mode 100644 .gitignore create mode 100644 Makefile create mode 100644 joints.c create mode 100644 joints.h create mode 100644 main.c diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..b8dbb27 --- /dev/null +++ b/.gitignore @@ -0,0 +1,4 @@ +build/ +main +.ycm_extra_conf.py +*.swp diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..0cb13b3 --- /dev/null +++ b/Makefile @@ -0,0 +1,39 @@ +SRC_FILES = $(wildcard *.c) +EXE = main +BUILD_DIR = build/ +OBJ = $(patsubst %.c,$(BUILD_DIR)%.o,$(SRC_FILES)) +DEP = $(OBJ:.o=.d) + +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 + +CFLAGS = -MMD $(INCLUDE_DIRS) +LDFLAGS = $(LIB_DIRS) $(LIBS) + +all: CFLAGS += -O2 +all: $(EXE) + +debug: CFLAGS += -DDEBUG -O0 -ggdb +debug: $(EXE) + +$(EXE) : $(OBJ) + $(CC) -o $@ $^ $(LDFLAGS) + +$(BUILD_DIR)%.o : %.c + mkdir -p $(BUILD_DIR) + $(CC) $(CFLAGS) -o $@ -c $< + +-include $(DEP) + +.PHONY: cleanall +cleanall: clean cleandep + +.PHONY: clean +clean: + rm -f $(OBJ) $(EXE) + +.PHONY: cleandep +cleandep: + rm -f $(DEP) diff --git a/joints.c b/joints.c new file mode 100644 index 0000000..9fc1740 --- /dev/null +++ b/joints.c @@ -0,0 +1,119 @@ +#include "joints.h" +#include "raymath.h" +#include +#include + +Body* init_body(unsigned int N, int total_length, float root_x, float root_y){ + Body *body = (Body *)malloc(sizeof(Body)); + body->N = N; + body->total_length = 0; + + 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->target = (Vector2){root_x, body->total_length}; + body->final_target = (Vector2){root_x, body->total_length}; + + body->root_pos = (Vector2){root_x, root_y}; + body->root = (Joint *)malloc(sizeof(Joint)); + body->root->pos = (Vector2){root_x, root_y}; + body->root->parent = NULL; + + Joint* prev_joint = body->root; + for (int i = 1; ilinks_lengths[i-1] = link_len; + Joint* new_joint = (Joint *)malloc(sizeof(Joint)); + new_joint->pos = (Vector2){root_x, root_y - link_len * i}; + new_joint->parent = prev_joint; + new_joint->child = NULL; + prev_joint->child = new_joint; + prev_joint = new_joint; + if (i == N-1){ + body->end = new_joint; + } + } + +#ifdef DEBUG + Joint* current_joint = body->root; + for (Joint* jt = body->root;jt != NULL; jt = jt->child){ + printf("x %.2f, y %.2f\n", jt->pos.x, jt->pos.y); + } +#endif + return body; +} + +void set_body_target(Body* body, Vector2 new_target){ + body->final_target = new_target; +} + +Vector2 _get_new_pos(Vector2 p1, Vector2 p2, float length){ + float dist = Vector2Distance(p1, p2); + float lambda = length / dist; + return Vector2Add( + Vector2Scale(p2,1-lambda), + Vector2Scale(p1, lambda) + ); +} + +void update_body(Body *body){ + body->target = Vector2Lerp(body->target, body->final_target, 0.2); + + // Check distance + float dist = Vector2Distance(body->root->pos, body->target); + unsigned int i = 0; + 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; + } + return; + } + + double error = 1; + Vector2 prev_pos; + while (error > 1e-2){ + prev_pos = body->end->pos; + + // Backward Pass + body->end->pos = body->target; + i = body->N - 2; + for (Joint *jt = body->end->parent; jt != NULL; jt = jt->parent){ + jt->pos = _get_new_pos(jt->pos, jt->child->pos, body->links_lengths[i]); + --i; + } + + // Forward Pass + body->root->pos.x = body->root_pos.x; + body->root->pos.y = body->root_pos.y; + i = 0; + for (Joint *jt = body->root->child; jt != NULL; jt = jt->child){ + jt->pos = _get_new_pos(jt->pos, jt->parent->pos, body->links_lengths[i]); + ++i; + } + error = Vector2Distance(prev_pos, body->end->pos); + } +} + +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 + DrawCircleV(jt->pos, 5, BLACK); + } + DrawCircleV(body->target, 3, RED); +} + +void free_body(Body *body){ + Joint* current_joint = body->root; + Joint* next_joint = body->root; + while (current_joint != NULL){ + current_joint->parent = NULL; + next_joint = current_joint->child; + current_joint->child = NULL; + free(current_joint); + current_joint = next_joint; + } +} diff --git a/joints.h b/joints.h new file mode 100644 index 0000000..051c209 --- /dev/null +++ b/joints.h @@ -0,0 +1,27 @@ +#include "raylib.h" + +typedef struct _Joint { + Vector2 pos; + struct _Joint* parent; + struct _Joint* child; +} Joint; + +typedef struct _Body { + Vector2 final_target; + Vector2 target; + Vector2 root_pos; + unsigned int N; + float* links_lengths; + float total_length; + Joint* root; + Joint* end; +} Body; + + +Body* init_body(unsigned int N, int total_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 update_body(Body* body); +void draw_body(Body* body); +void free_body(Body* body); diff --git a/main.c b/main.c new file mode 100644 index 0000000..9c751a0 --- /dev/null +++ b/main.c @@ -0,0 +1,24 @@ +#include "joints.h" + +int main(int argc, char** argv) { + const unsigned int scrnWidth = 800; + const unsigned int scrnHeight = 600; + + InitWindow(scrnWidth, scrnHeight, "FABRIK"); + SetTargetFPS(60); + + Body* body = init_body(15, 300, 400, 500); + + while(!WindowShouldClose()){ + if (IsMouseButtonReleased(MOUSE_BUTTON_LEFT)){ + set_body_target(body, GetMousePosition()); + } + update_body(body); + BeginDrawing(); + ClearBackground(RAYWHITE); + draw_body(body); + EndDrawing(); + } + CloseWindow(); + free_body(body); +}