96 lines
2.8 KiB
C
96 lines
2.8 KiB
C
#include "mechanics.h"
|
|
#include "objects.h"
|
|
#include <stdio.h>
|
|
|
|
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() * 100 / 2.0f - a->radius;
|
|
float max_y = GetRenderHeight() * 100 / 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) {
|
|
// Advance every body to the same simulated time before checking contacts.
|
|
for (unsigned int i = 0; i < uni->num_bodies; i++) {
|
|
mass_body_t *body = &uni->bodies[i];
|
|
body->position = vec2_sum(body->position,
|
|
vec2_scale(body->speed, PHYSICS_TIMESTEP));
|
|
}
|
|
for (unsigned int i = 0; i < uni->num_bodies; i++) {
|
|
mass_body_t *body_1 = &uni->bodies[i];
|
|
for (unsigned int y = i + 1; y < uni->num_bodies; y++) {
|
|
mass_body_t *body_2 = &uni->bodies[y];
|
|
Vector2 collision = body_collision(body_1, body_2);
|
|
if (!vec2_is_null(collision)) {
|
|
collide(body_1, body_2, collision, 0.99);
|
|
}
|
|
}
|
|
if (WALL_COLLISIONS) {
|
|
collide_rigid(body_1, 1.0f);
|
|
}
|
|
}
|
|
}
|