102 lines
2.8 KiB
C
102 lines
2.8 KiB
C
#include "objects.h"
|
|
#include <math.h>
|
|
#include <raymath.h>
|
|
|
|
#define min(a, b) (a>b?b:a);
|
|
#define max(a, b) (a>b?a:b);
|
|
|
|
object *newobj(Vector3 pos, double r, Color c, double rest, double m)
|
|
{
|
|
object *obj = (object*)malloc(sizeof(object));
|
|
obj->r = r;
|
|
obj->pos = pos;
|
|
obj->color = c;
|
|
obj->vel = (Vector3){0, 0, 0};
|
|
obj->restitution = rest;
|
|
obj->mass = m;
|
|
return obj;
|
|
}
|
|
|
|
Vector3 gravitation_force(object *obj1, object *obj2)
|
|
{
|
|
const double GRAVITY_FACTOR = 1;
|
|
Vector3 force;
|
|
double dist, mass, total_g;
|
|
mass = obj1->mass * obj2->mass;
|
|
force = Vector3Subtract(obj2->pos, obj1->pos);
|
|
|
|
dist = Vector3Length(force);
|
|
force = Vector3Normalize(force);
|
|
|
|
dist = max(dist, obj1->r+obj2->r);
|
|
|
|
total_g = GRAVITY_FACTOR * mass / (dist * dist);
|
|
force = Vector3Scale(force, total_g);
|
|
|
|
return force;
|
|
}
|
|
|
|
void integrate_g(object *obj1, object *obj2, Vector3 force)
|
|
{
|
|
Vector3 acc;
|
|
double f, a;
|
|
|
|
f = Vector3Length(force);
|
|
//acc = Vector3Scale(Vector3Subtract(obj1->pos, obj2->pos), f);
|
|
acc = force;
|
|
|
|
a = f / obj1->mass;
|
|
acc = Vector3Scale(Vector3Normalize(acc), a*GetFrameTime());
|
|
obj1->vel = Vector3Add(obj1->vel, acc);
|
|
|
|
a = f / obj2->mass;
|
|
acc = Vector3Scale(Vector3Normalize(acc), a*GetFrameTime());
|
|
obj2->vel = Vector3Add(obj2->vel, acc);
|
|
}
|
|
|
|
void resolve_collision(object *obj1, object *obj2)
|
|
{
|
|
Vector3 normal = Vector3Subtract(obj1->pos, obj2->pos);
|
|
float distsq = Vector3LengthSqr(normal);
|
|
float sumrad = obj1->r+obj2->r;
|
|
float mindistsq = sumrad*sumrad;
|
|
|
|
if (distsq < mindistsq) {
|
|
float dist = sqrtf(distsq);
|
|
|
|
if (dist == 0) {
|
|
normal = Vector3RotateByAxisAngle(Vector3One(), (Vector3){0, 1, 1}, rand()*360);
|
|
dist = sumrad;
|
|
} else {
|
|
normal = Vector3Scale(normal, 1.0f / dist);
|
|
}
|
|
|
|
float overlap = sumrad - dist;
|
|
Vector3 separation = Vector3Scale(normal, overlap / 2.0f);
|
|
|
|
obj1->pos = Vector3Subtract(obj1->pos, separation);
|
|
obj2->pos = Vector3Add(obj2->pos, separation);
|
|
|
|
} else {
|
|
normal = Vector3Normalize(normal);
|
|
}
|
|
|
|
float vel1_normal = Vector3DotProduct(obj1->vel, normal);
|
|
float vel2_normal = Vector3DotProduct(obj2->vel, normal);
|
|
|
|
if (vel1_normal - vel2_normal > 0) return;
|
|
|
|
float totalmass = obj1->mass + obj2->mass;
|
|
float vel1_normal_final = (obj1->mass * vel1_normal + obj2->mass * vel2_normal - obj2->mass * obj2->restitution * (vel1_normal - vel2_normal)) / totalmass;
|
|
float vel2_normal_final = (obj1->mass * vel1_normal + obj2->mass * vel2_normal - obj2->mass * obj1->restitution * (vel2_normal - vel1_normal)) / totalmass;
|
|
|
|
Vector3 vel1_tang = Vector3Subtract(obj1->vel, Vector3Scale(normal, vel1_normal));
|
|
Vector3 vel2_tang = Vector3Subtract(obj2->vel, Vector3Scale(normal, vel2_normal));
|
|
|
|
obj1->vel = Vector3Add(Vector3Scale(normal, vel1_normal_final), vel1_tang);
|
|
obj2->vel = Vector3Add(Vector3Scale(normal, vel2_normal_final), vel2_tang);
|
|
|
|
}
|
|
|
|
|