Sara/fencer
1
0
Fork 0
Code Issues Pull requests Actions Packages Projects Releases Wiki Activity

added custom collision solvers

Browse source
This commit is contained in:
Sara 2023-10-22 11:25:44 +02:00
parent 788c1970e2
commit f373fada26
10 changed files with 116 additions and 76 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

1
src/collision.c
View file

@ -2,6 +2,7 @@
#include "debug.h"
#include "player.h"
#include "vmath.h"
#include "rigidbody.h"
// =====================================================
// Shape overlap test using the separating axis theorem

52
src/physics_entity.c
View file

@ -3,6 +3,8 @@
#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);
@ -23,3 +25,53 @@ void physics_entity_debug_draw(PhysicsEntity self) {
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 = 4.0;
// const float damping = 1.0;//4.f;
return vmulff(normal, elasticity * -vdotf(normal, velocity));
}
static inline
void _internal_default_contact_solver(RigidBody* body, Contact* contact, Vector* solve_forces) {
Collision hit = contact->hit;
if(vdotf(hit.normal, rigidbody_get_velocity(body)) > 0.0)
return;
float normalmag = vmagnitudef(contact->hit.normal);
ASSERT_RETURN(fabsf(normalmag - 1.f) < 0.99999f,, "Normal of collision not one");
Vector force = _internal_calculate_contact_force(body, contact);
float dot = vdotf(vnormalizedf(*solve_forces), force);
if (veqf(*solve_forces, ZeroVector) || dot * dot > vsqrmagnitudef(*solve_forces) || dot <= 0.0f) {
*solve_forces = vaddf(*solve_forces, force);
rigidbody_add_impulse(body, force, 1);
}
ASSERT_RETURN(!visnanf(force),, "Force contains NaN (1)");
}
void default_contact_solver(PhysicsEntity self, List* contacts) {
Vector solve_forces = ZeroVector;
list_foreach(Contact, contact, contacts) {
_internal_default_contact_solver(self.tc->get_rigidbody(self.data), contact, &solve_forces);
}
}
void physics_entity_handle_contacts(PhysicsEntity self) {
RigidBody* body = self.tc->get_rigidbody(self.data);
ASSERT_RETURN(!visnanf(rigidbody_get_velocity(body)),, "Velocity is NaN (0)");
rigidbody_collect_contacts(body);
if(rigidbody_get_contacts(body)->len > 0)
self.tc->collision_solver(self.data, rigidbody_get_contacts(body));
ASSERT_RETURN(!visnanf(rigidbody_get_velocity(body)),, "Velocity is NaN (1)");
}

17
src/physics_entity.h
View file

@ -3,16 +3,17 @@
#include "vmath.h"
#include "typeclass_helpers.h"
#include "rigidbody.h"
#include "list.h"
#include "shape.h"
typedef struct Collision Collision;
typedef struct RigidBody RigidBody;
typedef struct {
RigidBody* (*const get_rigidbody)(void* self);
Shape* (*const get_shape)(void* self);
void(*const on_collision)(void* self, Collision collision);
void(*const collision_solver)(void* self, List* collisions);
} IPhysicsEntity;
typedef struct {
@ -22,17 +23,27 @@ typedef struct {
} PhysicsEntity;
extern void physics_entity_debug_draw(PhysicsEntity self);
extern void default_contact_solver(PhysicsEntity self, List* contacts);
extern void physics_entity_handle_contacts(PhysicsEntity self);
#define impl_PhysicsEntity_for(T, get_rigidbody_f, get_shape_f, on_collision_f)\
#define impl_PhysicsEntity_default_solver(__Name, T)\
void __Name(T* self, List* contacts) {\
default_contact_solver(T##_as_PhysicsEntity(self), contacts);\
}
#define impl_PhysicsEntity_for(T, get_rigidbody_f, get_shape_f, on_collision_f, collision_solver_f)\
static inline PhysicsEntity T##_as_PhysicsEntity(T* x) {\
TC_FN_TYPECHECK(Transformable, T##_as_Transformable, T*);\
TC_FN_TYPECHECK(RigidBody*, get_rigidbody_f, T*);\
TC_FN_TYPECHECK(Shape*, get_shape_f, T*);\
TC_FN_TYPECHECK(void, on_collision_f, T*, Collision);\
TC_FN_TYPECHECK(void, collision_solver_f, T*, List*);\
static IPhysicsEntity const tc = {\
.get_rigidbody = (RigidBody*(*const)(void*)) get_rigidbody_f,\
.get_shape = (Shape*(*const)(void*)) get_shape_f,\
.on_collision = (void(*const)(void*,Collision)) on_collision_f,\
.collision_solver = (void(*const)(void*,List*)) collision_solver_f,\
};\
Transformable transformable = T##_as_Transformable(x);\
return (PhysicsEntity){.data = x, .tc = &tc, .transformable = transformable.tc};\

5
src/physics_world.c
View file

@ -167,11 +167,10 @@ void _internal_physics_tilemap_collision() {
static
void _internal_physics_apply() {
PhysicsEntity* end = _world_bodies + _world_bodies_len;
RigidBody* body = NULL;
for(PhysicsEntity* entity = _world_bodies; entity < end; ++entity) {
body = entity->tc->get_rigidbody(entity->data);
rigidbody_apply_physics(body);
physics_entity_handle_contacts(*entity);
rigidbody_integrate_forces(entity->tc->get_rigidbody(entity->data));
}
}

22
src/player.c
View file

@ -1,6 +1,7 @@
#include "player.h"
#include "assets.h"
#include "debug.h"
#include "physics_entity.h"
#include "program.h"
#include "rigidbody.h"
#include "input.h"
@ -23,6 +24,27 @@ void player_spawn(Player* self, Vector at) {
rigidbody_get_transform(self->rigidbody)->position = at;
}
void player_collision_solver(Player* self, List* contacts) {
RigidBody* body = self->rigidbody;
Collision hit;
const Vector vel = vaddf(rigidbody_get_velocity(body), vmulff(rigidbody_get_force(body), delta_time()));
Vector avrg = ZeroVector;
int n = 0;
list_foreach(Contact, contact, contacts) {
hit = contact->hit;
Vector norm = vnormalizedf(hit.penetration_vector);
LOG_INFO("n %f %f", norm.x, norm.y);
float dot = fmaxf(0, vdotf(norm, hit.velocity));
avrg = vaddf(vmulff(avrg, (float)n), vsubf(vel, vmulff(norm, dot)));
++n;
avrg = vmulff(avrg, 1.0f / (float)n);
}
LOG_INFO("vel %f %f", vel.x, vel.y);
LOG_INFO("for %f %f", avrg.x, avrg.y);
// rigidbody_add_impulse(body, vinvf(avrg), 0);
default_contact_solver(Player_as_PhysicsEntity(self), contacts);
}
void player_start(Player* self) {
input_add_axis_action(SDL_SCANCODE_A, SDL_SCANCODE_D, &player_input_h);
input_add_axis_action(SDL_SCANCODE_S, SDL_SCANCODE_W, &player_input_v);

4
src/player.h
View file

@ -28,6 +28,7 @@ extern Transform* player_get_transform(Player* self);
extern Transform* player_get_rigidbody_transform(Player* self);
extern RigidBody* player_get_rigidbody(Player* self);
extern Shape* player_get_shape(Player* self);
extern void player_collision_solver(Player* self, List* contacts);
impl_Transformable_for(Player,
player_get_transform
@ -40,7 +41,8 @@ impl_SpriteEntity_for(Player,
impl_PhysicsEntity_for(Player,
player_get_rigidbody,
player_get_shape,
player_collision
player_collision,
player_collision_solver
)
impl_BehaviourEntity_for(Player,

72
src/rigidbody.c
View file

@ -5,12 +5,6 @@
#include "collision.h"
#include "transformable.h"
typedef struct {
Collision hit;
float warming;
int expiry;
} Contact;
struct RigidBody {
Transformable transformable;
@ -71,8 +65,7 @@ void rigidbody_add_contact(RigidBody* self, Collision hit) {
});
}
static inline
void _internal_rigidbody_collect_contacts(RigidBody* self) {
void rigidbody_collect_contacts(RigidBody* self) {
for(size_t i = 0; i < self->contacts.len; ++i) {
Contact* contact = list_at(&self->contacts, i);
@ -85,6 +78,10 @@ void _internal_rigidbody_collect_contacts(RigidBody* self) {
}
}
List* rigidbody_get_contacts(RigidBody* self) {
return &self->contacts;
}
static inline
void _internal_debug_draw_collision_edge(Vector left, Vector right, Vector normal) {
#if !NDEBUG
@ -100,57 +97,7 @@ void _internal_debug_draw_collision_edge(Vector left, Vector right, Vector norma
#endif
}
static inline
Vector _internal_calculate_contact_force(RigidBody* self, Contact* contact) {
Collision hit = contact->hit;
const float depth = -vdotf(hit.normal, hit.penetration_vector);
const float elasticity = 200.0f;
const float damping = 4.f;
const Vector velocity = hit.velocity;
const Vector normal = hit.normal;
return vmulff(normal, -depth * elasticity - damping * vdotf(normal, velocity));
}
static inline
void _internal_rigidbody_solve_contact(RigidBody* self, Contact* contact, Vector* solve_forces) {
Collision hit = contact->hit;
if(vdotf(hit.normal, self->linear_velocity) > 0.0)
return;
float normalmag = vmagnitudef(contact->hit.normal);
ASSERT_RETURN(fabsf(normalmag - 1.f) < 0.99999f,, "Normal of collision not one");
Vector force = _internal_calculate_contact_force(self, contact);
float dot = vdotf(vnormalizedf(*solve_forces), force);
if (veqf(*solve_forces, ZeroVector) || dot * dot > vsqrmagnitudef(*solve_forces) || dot <= 0.0f) {
*solve_forces = vaddf(*solve_forces, force);
rigidbody_add_impulse(self, force, 1);
}
ASSERT_RETURN(!visnanf(force),, "Force contains NaN (1)");
}
void rigidbody_solve_contacts(RigidBody* self, List* contacts) {
Vector solve_forces = ZeroVector;
list_foreach(Contact, contact, &self->contacts) {
_internal_rigidbody_solve_contact(self, contact, &solve_forces);
}
}
void rigidbody_handle_contacts(RigidBody* self) {
ASSERT_RETURN(!visnanf(self->linear_velocity),, "Velocity is NaN (0)");
_internal_rigidbody_collect_contacts(self);
rigidbody_solve_contacts(self, &self->contacts);
ASSERT_RETURN(!visnanf(self->linear_velocity),, "Velocity is NaN (1)");
}
static inline
void _internal_rigidbody_integrate_forces(RigidBody* self) {
void rigidbody_integrate_forces(RigidBody* self) {
const float dt = delta_time();
Vector position = self->internal_transform.position;
@ -172,11 +119,6 @@ void _internal_rigidbody_integrate_forces(RigidBody* self) {
self->next_linear_force = ZeroVector;
}
void rigidbody_apply_physics(RigidBody* self) {
rigidbody_handle_contacts(self);
_internal_rigidbody_integrate_forces(self);
}
float rigidbody_get_mass(const RigidBody* self) {
return self->mass;
}
@ -214,7 +156,7 @@ void rigidbody_set_velocity(RigidBody* self, Vector velocity) {
}
Vector rigidbody_get_force(RigidBody* self) {
return self->last_linear_force;
return self->next_linear_force;
}
void rigidbody_debug_draw_contacts(RigidBody* self) {

12
src/rigidbody.h
View file

@ -4,8 +4,14 @@
#include "shape.h"
#include "transformable.h"
#include "list.h"
#include "collision.h"
struct Collision;
typedef struct {
struct Collision hit;
float warming;
int expiry;
} Contact;
typedef struct RigidBody RigidBody;
typedef void (*CollisionHandlerFn)(void* obj, List* collisions);
@ -14,10 +20,10 @@ extern RigidBody* rigidbody_make(Transformable transform);
extern void rigidbody_destroy(RigidBody* self);
extern void rigidbody_add_contact(RigidBody* self, struct Collision hit);
extern void rigidbody_handle_contacts(RigidBody* self);
extern void rigidbody_solve_contacts(RigidBody* self, List* contacts);
extern void rigidbody_collect_contacts(RigidBody* self);
extern List* rigidbody_get_contacts(RigidBody* self);
extern void rigidbody_apply_physics(RigidBody* self);
extern void rigidbody_integrate_forces(RigidBody* self);
extern float rigidbody_get_mass(const RigidBody* self);
extern void rigidbody_set_mass(RigidBody* self, float mass);

2
src/tilemap.c
View file

@ -146,3 +146,5 @@ Vector* tile_instance_get_scale(TileInstance* self) {
float* tile_instance_get_rotation(TileInstance* self) {
return &self->transform.rotation;
}
impl_PhysicsEntity_default_solver(tile_instance_solve_contacts, TileInstance)

5
src/tilemap.h
View file

@ -29,6 +29,8 @@ extern Transform* tile_instance_get_transform(TileInstance* self);
extern Shape* tile_instance_get_shape(TileInstance* self);
extern void tile_instance_on_collision(TileInstance* self, Collision collision);
extern void tile_instance_solve_contacts(TileInstance* self, List* contacts);
impl_Transformable_for(Tilemap,
tilemap_get_transform
@ -40,7 +42,8 @@ impl_Transformable_for(TileInstance,
impl_PhysicsEntity_for(TileInstance,
tile_instance_get_rigidbody,
tile_instance_get_shape,
tile_instance_on_collision
tile_instance_on_collision,
tile_instance_solve_contacts
)
#endif // !_fencer_tilemap_h