103 lines
4 KiB
C
103 lines
4 KiB
C
#include "physics_entity.h"
|
|
#include "camera.h"
|
|
#include "rigidbody.h"
|
|
#include "shape.h"
|
|
#include "render.h"
|
|
#include "debug.h"
|
|
#include "program.h"
|
|
|
|
void physics_entity_debug_draw(PhysicsEntity self) {
|
|
RigidBody* body = self.tc->get_rigidbody(self.data);
|
|
Shape* shape = self.tc->get_shape(self.data);
|
|
Transform* transform = self.transformable->get_transform(self.data);
|
|
|
|
shape_draw(shape, *transform);
|
|
rigidbody_debug_draw_contacts(body);
|
|
|
|
Vector lhs = transform->position;
|
|
Vector rhs = vaddf(lhs, rigidbody_get_velocity(body));
|
|
lhs = camera_world_to_pixel_point(&g_camera, lhs);
|
|
rhs = camera_world_to_pixel_point(&g_camera, rhs);
|
|
SDL_SetRenderDrawColor(g_renderer, 0, 255, 0, 255);
|
|
SDL_RenderDrawLine(g_renderer, lhs.x, lhs.y, rhs.x, rhs.y);
|
|
|
|
rhs = camera_world_to_pixel_point(&g_camera, vaddf(transform->position, rigidbody_get_force(body)));
|
|
SDL_SetRenderDrawColor(g_renderer, 0, 255, 255, 255);
|
|
SDL_RenderDrawLine(g_renderer, lhs.x, lhs.y, rhs.x, rhs.y);
|
|
}
|
|
static inline
|
|
Vector _internal_calculate_contact_force(RigidBody* self, Contact* contact) {
|
|
Collision hit = contact->hit;
|
|
|
|
const Vector velocity = hit.velocity;
|
|
const Vector normal = hit.normal;
|
|
|
|
const float elasticity = 0.0;
|
|
const float damping = 0.0;
|
|
const float push = vdotf(normal, velocity);
|
|
|
|
return ZeroVector; //vsubf(vmulff(normal, elasticity * fmaxf(1.0, -push)), vmulff(normal, damping * fminf(0.0, push)));
|
|
}
|
|
|
|
static inline
|
|
int _internal_default_contact_solver(RigidBody* body, Contact* contact, Transform pre_solve) {
|
|
Collision hit = contact->hit;
|
|
Transform* trans = rigidbody_get_transform(body);
|
|
const Vector world_collision_point = vaddf(transform_point(&pre_solve, hit.point), hit.penetration_vector);
|
|
const float current_dot = vdotf(hit.normal, vsubf(transform_point(trans, hit.point), world_collision_point));
|
|
if(current_dot >= -0.0001)
|
|
return 1;
|
|
// the desired position is anywhere the overlapping vertex is further along the normal than the contact point
|
|
const Vector target = vaddf(trans->position, vmulff(hit.normal, -current_dot));
|
|
trans->position = vmovetowardsf(trans->position, target, 1.f);
|
|
return 0;
|
|
}
|
|
|
|
void physics_entity_apply_collision_forces(PhysicsEntity self, List* contacts) {
|
|
RigidBody* body = self.tc->get_rigidbody(self.data);
|
|
// apply collision impulse
|
|
list_foreach(Contact*, contact, contacts)
|
|
rigidbody_add_impulse(body, _internal_calculate_contact_force(body, contact), 1);
|
|
}
|
|
|
|
void physics_entity_solve_contacts(PhysicsEntity self, List* contacts) {
|
|
physics_entity_apply_collision_forces(self, contacts);
|
|
RigidBody* body = self.tc->get_rigidbody(self.data);
|
|
const Transform pre_solve = *rigidbody_get_transform(body);
|
|
// attempt to solve constraints
|
|
int done;
|
|
for(size_t iteration = 100; iteration != 0; --iteration) {
|
|
done = 1;
|
|
list_foreach(Contact*, contact, contacts) {
|
|
if(!_internal_default_contact_solver(body, contact, pre_solve))
|
|
done = 0;
|
|
}
|
|
if(done)
|
|
break;
|
|
if(iteration == 1)
|
|
LOG_WARNING("gave up on solving %zu contacts", contacts->len);
|
|
}
|
|
Vector dir = vnormalizedf(vsubf(rigidbody_get_transform(body)->position, pre_solve.position));
|
|
Vector vel = rigidbody_get_velocity(body);
|
|
float dot = vdotf(dir, vel);
|
|
if(dot < 0)
|
|
vel = vsubf(vel, vmulff(dir, dot));
|
|
rigidbody_set_velocity(body, vel);
|
|
}
|
|
|
|
void physics_entity_update(PhysicsEntity self) {
|
|
RigidBody* body = self.tc->get_rigidbody(self.data);
|
|
|
|
ASSERT_RETURN(!visnanf(rigidbody_get_velocity(body)),, "Velocity is NaN (0)");
|
|
|
|
List* contacts = rigidbody_get_contacts(body);
|
|
if(contacts->len > 0) {
|
|
self.tc->collision_solver(self.data, contacts);
|
|
list_foreach(Contact*, contact, contacts)
|
|
self.tc->on_collision(self.data, contact->hit);
|
|
}
|
|
rigidbody_collect_contacts(body);
|
|
|
|
ASSERT_RETURN(!visnanf(rigidbody_get_velocity(body)),, "Velocity is NaN (1)");
|
|
}
|