diff --git a/consts.h b/consts.h new file mode 100644 index 0000000..027031f --- /dev/null +++ b/consts.h @@ -0,0 +1,7 @@ +#ifndef CONSTS_H_ +#define CONSTS_H_ +#define TIMESTEP 1000000 +#define simx 2 + + +#endif // CONSTS_H_ diff --git a/cphysics.c b/cphysics.c index 550f08d..4fe1f84 100644 --- a/cphysics.c +++ b/cphysics.c @@ -1,4 +1,5 @@ #include "cphysics.h" +#include "objects.h" void gravity(universe_t* univ){ @@ -6,6 +7,3 @@ void gravity(universe_t* univ){ void magnetism(universe_t* univ){ } -void mechanics(universe_t* univ){ - -} diff --git a/cphysics.h b/cphysics.h index 80b6944..26c0e5d 100644 --- a/cphysics.h +++ b/cphysics.h @@ -3,5 +3,4 @@ #include "objects.h" void gravity(universe_t* univ); void magnetism(universe_t* univ); -void mechanics(universe_t* univ); #endif // CPHYSICS_H_ diff --git a/main.c b/main.c index 1654a10..da430b7 100644 --- a/main.c +++ b/main.c @@ -1,7 +1,5 @@ #define _POSIX_C_SOURCE 200809L -#include "objects.h" -#include "vec_util.h" #include #include #include @@ -11,9 +9,8 @@ #include #include #include - -static const long TIMESTEP = 1000000; -static const long simx = 2; +#include "consts.h" +#include "mechanics.h" int center_x_text(const char *text, int width, int size) { return (width - MeasureText(text, size)) / 2; @@ -23,82 +20,7 @@ static inline int center_y_text(int height, int size) { return (height - size) / 2; } -Vector2 body_collision(mass_body_t *a, mass_body_t *b) { - Vector2 diff = vec2_diff(&a->position, &b->position); - float len = vec2_len(diff); - if (len > a->radius + b->radius) { - return NULL_VEC2; - } - if (len == 0) { - return (Vector2){1, 0}; - } - Vector2 result = vec2_normalize(diff); - return result; -} -void collide(mass_body_t *a, mass_body_t *b, const Vector2 *normal, - float resitution) { - float inverse_a = 1.0f / a->mass; - float inverse_b = 1.0f / b->mass; - float inverse_total = inverse_a + inverse_b; - float overlap = - a->radius + b->radius - vec2_len(vec2_diff(&a->position, &b->position)); - Vector2 correction = vec2_scale(normal, overlap / inverse_total); - Vector2 a_move = vec2_scale(&correction, inverse_a); - Vector2 b_move = vec2_scale(&correction, inverse_b); - a->position = vec2_sum(&a->position, &a_move); - b->position = vec2_diff(&b->position, &b_move); - - Vector2 relative = vec2_diff(&b->speed, &a->speed); - float v_normal = vec2_dot(&relative, normal); - if (v_normal <= 0) { - return; - } - - float factor = -(1 + resitution); - float j = factor * v_normal; - - j /= inverse_total; - - Vector2 final = vec2_scale(normal, j); - - Vector2 a_diff = vec2_scale(&final, inverse_a); - a->speed = vec2_diff(&a->speed, &a_diff); - Vector2 b_diff = vec2_scale(&final, inverse_b); - b->speed = vec2_sum(&b->speed, &b_diff); -} - -void collide_rigid(mass_body_t *a, float restitution) { - float max_x = GetRenderWidth() / 2.0f - a->radius; - float max_y = GetRenderHeight() / 2.0f - a->radius; - - if (a->position.x > max_x) { - a->position.x = max_x; - if (a->speed.x > 0) - a->speed.x *= -restitution; - } else if (a->position.x < -max_x) { - a->position.x = -max_x; - if (a->speed.x < 0) - a->speed.x *= -restitution; - } - - if (a->position.y > max_y) { - a->position.y = max_y; - if (a->speed.y > 0) - a->speed.y *= -restitution; - } else if (a->position.y < -max_y) { - a->position.y = -max_y; - if (a->speed.y < 0) - a->speed.y *= -restitution; - } -} - -bool vec2_is_null(Vector2 v) { - if (v.x == 0 && v.y == 0) { - return true; - } - return false; -} long long timespec_diff_ns(struct timespec end, struct timespec start) { return (end.tv_sec - start.tv_sec) * 1000000000LL + @@ -107,30 +29,9 @@ long long timespec_diff_ns(struct timespec end, struct timespec start) { void *physics(void *args) { universe_t *uni = (universe_t *)args; - mass_body_t body_1; - mass_body_t body_2; - Vector2 collision; - Vector2 scaled_speed; struct timespec ot, ct; while (true) { - clock_gettime(CLOCK_MONOTONIC, &ct); - for (unsigned int i = 0; i < uni->num_bodies; i++) { - body_1 = uni->bodies[i]; - scaled_speed = vec2_scale(&body_1.speed, 1.0 / simx); - body_1.position = vec2_sum(&body_1.position, &scaled_speed); - if (i < uni->num_bodies - 1) { - for (unsigned int y = i + 1; y < uni->num_bodies; y++) { - body_2 = uni->bodies[y]; - collision = body_collision(&body_1, &body_2); - if (!vec2_is_null(collision)) { - collide(&body_1, &body_2, &collision, 1.0); - } - uni->bodies[y] = body_2; - } - } - collide_rigid(&body_1, 1.1); - uni->bodies[i] = body_1; - } + mechanics(uni); ot = ct; clock_gettime(CLOCK_MONOTONIC, &ct); long long elapsed = timespec_diff_ns(ct, ot); diff --git a/mechanics.c b/mechanics.c new file mode 100644 index 0000000..71ed866 --- /dev/null +++ b/mechanics.c @@ -0,0 +1,99 @@ +#include "mechanics.h" +#include "objects.h" +#include + + +Vector2 body_collision(mass_body_t *a, mass_body_t *b) { + Vector2 diff = vec2_diff(&a->position, &b->position); + float len = vec2_len(diff); + if (len > a->radius + b->radius) { + return NULL_VEC2; + } + if (len == 0) { + return (Vector2){1, 0}; + } + Vector2 result = vec2_normalize(diff); + return result; +} + +void collide(mass_body_t *a, mass_body_t *b, const Vector2 *normal, + float resitution) { + float inverse_a = 1.0f / a->mass; + float inverse_b = 1.0f / b->mass; + float inverse_total = inverse_a + inverse_b; + float overlap = + a->radius + b->radius - vec2_len(vec2_diff(&a->position, &b->position)); + Vector2 correction = vec2_scale(normal, overlap / inverse_total); + Vector2 a_move = vec2_scale(&correction, inverse_a); + Vector2 b_move = vec2_scale(&correction, inverse_b); + a->position = vec2_sum(&a->position, &a_move); + b->position = vec2_diff(&b->position, &b_move); + + Vector2 relative = vec2_diff(&b->speed, &a->speed); + float v_normal = vec2_dot(&relative, normal); + if (v_normal <= 0) { + return; + } + + float factor = -(1 + resitution); + float j = factor * v_normal; + + j /= inverse_total; + + Vector2 final = vec2_scale(normal, j); + + Vector2 a_diff = vec2_scale(&final, inverse_a); + a->speed = vec2_diff(&a->speed, &a_diff); + Vector2 b_diff = vec2_scale(&final, inverse_b); + b->speed = vec2_sum(&b->speed, &b_diff); +} + +void collide_rigid(mass_body_t *a, float restitution) { + float max_x = GetRenderWidth() / 2.0f - a->radius; + float max_y = GetRenderHeight() / 2.0f - a->radius; + + if (a->position.x > max_x) { + a->position.x = max_x; + if (a->speed.x > 0) + a->speed.x *= -restitution; + } else if (a->position.x < -max_x) { + a->position.x = -max_x; + if (a->speed.x < 0) + a->speed.x *= -restitution; + } + + if (a->position.y > max_y) { + a->position.y = max_y; + if (a->speed.y > 0) + a->speed.y *= -restitution; + } else if (a->position.y < -max_y) { + a->position.y = -max_y; + if (a->speed.y < 0) + a->speed.y *= -restitution; + } +} + +void mechanics(universe_t* uni){ + mass_body_t body_1; + mass_body_t body_2; + Vector2 collision; + Vector2 scaled_speed; + for (unsigned int i = 0; i < uni->num_bodies; i++) { + body_1 = uni->bodies[i]; + scaled_speed = vec2_scale(&body_1.speed, 1.0 / simx); + body_1.position = vec2_sum(&body_1.position, &scaled_speed); + printf("aaa"); + if (i < uni->num_bodies - 1) { + for (unsigned int y = i + 1; y < uni->num_bodies; y++) { + body_2 = uni->bodies[y]; + collision = body_collision(&body_1, &body_2); + if (!vec2_is_null(collision)) { + collide(&body_1, &body_2, &collision, 1.0); + } + uni->bodies[y] = body_2; + } + } + collide_rigid(&body_1, 1.1); + uni->bodies[i] = body_1; + } +} diff --git a/mechanics.h b/mechanics.h new file mode 100644 index 0000000..0ef083e --- /dev/null +++ b/mechanics.h @@ -0,0 +1,9 @@ +#pragma once +#ifndef MECHANICS_H_ +#define MECHANICS_H_ +#include "objects.h" +#include "consts.h" +void mechanics(universe_t* uni); + + +#endif // MECHANICS_H_ diff --git a/vec_util.c b/vec_util.c new file mode 100644 index 0000000..096988a --- /dev/null +++ b/vec_util.c @@ -0,0 +1,45 @@ +#include "vec_util.h" + + +bool vec2_is_null(Vector2 v) { + if (v.x == 0 && v.y == 0) { + return true; + } + return false; +} + +Vector2 transform_to_center(Vector2 v) { + int width = GetRenderWidth(); + int height = GetRenderHeight(); + Vector2 result = {v.x + width / 2.0, v.y + height / 2.0}; + return result; +} + +Vector2 vec2_sum(const Vector2 *a, const Vector2 *b) { + Vector2 result = {a->x + b->x, a->y + b->y}; + return result; +} +Vector2 vec2_diff(const Vector2 *a, const Vector2 *b) { + Vector2 result = {a->x - b->x, a->y - b->y}; + return result; +} + +float vec2_len(Vector2 v) { return sqrtf(v.x * v.x + v.y * v.y); } + +Vector2 vec2_normalize(const Vector2 v) { + float len = vec2_len(v); + if (len == 0) { + return NULL_VEC2; + } + Vector2 result = {v.x / len, v.y / len}; + return result; +} + +Vector2 vec2_scale(const Vector2 *normal, const float len) { + Vector2 result = {len * normal->x, len * normal->y}; + return result; +} + +float vec2_dot(const Vector2 *a, const Vector2 *b) { + return a->x * b->x + a->y * b->y; +} diff --git a/vec_util.h b/vec_util.h index 69ed0b9..57f3d16 100644 --- a/vec_util.h +++ b/vec_util.h @@ -1,43 +1,21 @@ +#pragma once #ifndef VEC_UTIL_H_ #define VEC_UTIL_H_ #include #include static const Vector2 NULL_VEC2 = {0, 0}; -Vector2 transform_to_center(Vector2 v) { - int width = GetRenderWidth(); - int height = GetRenderHeight(); - Vector2 result = {v.x + width / 2.0, v.y + height / 2.0}; - return result; -} +bool vec2_is_null(Vector2 v) ; -Vector2 vec2_sum(const Vector2 *a, const Vector2 *b) { - Vector2 result = {a->x + b->x, a->y + b->y}; - return result; -} -Vector2 vec2_diff(const Vector2 *a, const Vector2 *b) { - Vector2 result = {a->x - b->x, a->y - b->y}; - return result; -} +Vector2 transform_to_center(Vector2 v); +Vector2 vec2_sum(const Vector2 *a, const Vector2 *b); +float vec2_len(Vector2 v) ; -float vec2_len(Vector2 v) { return sqrtf(v.x * v.x + v.y * v.y); } +Vector2 vec2_normalize(const Vector2 v) ; -Vector2 vec2_normalize(const Vector2 v) { - float len = vec2_len(v); - if (len == 0) { - return NULL_VEC2; - } - Vector2 result = {v.x / len, v.y / len}; - return result; -} +Vector2 vec2_scale(const Vector2 *normal, const float len) ; -Vector2 vec2_scale(const Vector2 *normal, const float len) { - Vector2 result = {len * normal->x, len * normal->y}; - return result; -} - -float vec2_dot(const Vector2 *a, const Vector2 *b) { - return a->x * b->x + a->y * b->y; -} +float vec2_dot(const Vector2 *a, const Vector2 *b) ; +Vector2 vec2_diff(const Vector2 *a, const Vector2 *b) ; #endif // VEC_UTIL_H_