99 lines
3.7 KiB
C
99 lines
3.7 KiB
C
#include "physics_entity.h"
|
|
#include "camera.h"
|
|
#include "rigidbody.h"
|
|
#include "shape.h"
|
|
#include "render.h"
|
|
#include "debug.h"
|
|
#include "collider.h"
|
|
|
|
void physics_entity_debug_draw(PhysicsEntity self) {
|
|
RigidBody* body = self.tc->get_rigidbody(self.data);
|
|
Transform* transform = rigidbody_get_transform(body);
|
|
|
|
list_foreach(Collider**, collider, rigidbody_get_colliders(body)) {
|
|
shape_draw(collider_get_shape(*collider), *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_RenderDrawLineF(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_RenderDrawLineF(g_renderer, lhs.x, lhs.y, rhs.x, rhs.y);
|
|
}
|
|
|
|
static inline
|
|
Vector _internal_calculate_contact_force(RigidBody* self, Contact* contact) {
|
|
const Vector velocity = contact->hit.velocity;
|
|
const Vector normal = contact->hit.normal;
|
|
const float push = -vdotf(normal, velocity);
|
|
const Vector current_velocity = rigidbody_get_velocity(self);
|
|
return vaddf(current_velocity, vmulff(normal, push * rigidbody_get_bounce(self)));
|
|
}
|
|
|
|
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_solve_contacts(PhysicsEntity self, List* 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 = 50; 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 * (1.0f + rigidbody_get_bounce(body))));
|
|
|
|
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) {
|
|
if (rigidbody_is_static(body) == 0) {
|
|
physics_entity_solve_contacts(self, 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)");
|
|
}
|