98 lines
2.8 KiB
C
98 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() * 10 / 2.0f - a->radius;
|
|
float max_y = GetRenderHeight() * 10 / 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);
|
|
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;
|
|
}
|
|
}
|