diff --git a/src/collision.c b/src/collision.c index e1ee83c..e630302 100644 --- a/src/collision.c +++ b/src/collision.c @@ -121,17 +121,15 @@ int collision_check(PhysicsEntity a, PhysicsEntity b, Collision* out_a, Collisio int collision_a_overlaps = _internal_collision_get_overlap(a, b, &collision_a); int collision_b_overlaps = _internal_collision_get_overlap(b, a, &collision_b); - if(!collision_a_overlaps && !collision_b_overlaps) { + if(!collision_a_overlaps && !collision_b_overlaps) return 0; - } - - if(vsqrmagnitudef(collision_a.separation_force) < vsqrmagnitudef(collision_b.separation_force)) { - collision_b = _internal_collision_invert(collision_a, a); - } else { + else if(!collision_a_overlaps && collision_b_overlaps) collision_a = _internal_collision_invert(collision_b, b); - } + else if(!collision_b_overlaps && collision_a_overlaps) + collision_b = _internal_collision_invert(collision_a, a); + *out_a = collision_a; *out_b = collision_b; - return 1; + return (collision_b_overlaps << 1) | collision_a_overlaps; } \ No newline at end of file diff --git a/src/fencer.c b/src/fencer.c index c720e1d..10e40ad 100644 --- a/src/fencer.c +++ b/src/fencer.c @@ -35,7 +35,7 @@ static void draw() { level_draw(level); sprite_entity_draw(Player_as_SpriteEntity(player)); - shape_draw(player->shape, player->transform); + physics_entity_debug_draw(Player_as_PhysicsEntity(player)); } int main(int argc, char* argv[]) { diff --git a/src/physics_entity.c b/src/physics_entity.c new file mode 100644 index 0000000..b845407 --- /dev/null +++ b/src/physics_entity.c @@ -0,0 +1,21 @@ +#include "physics_entity.h" +#include "camera.h" +#include "rigidbody.h" +#include "shape.h" +#include "render.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); +} diff --git a/src/physics_entity.h b/src/physics_entity.h index 0358165..e5626c8 100644 --- a/src/physics_entity.h +++ b/src/physics_entity.h @@ -21,6 +21,8 @@ typedef struct { ITransformable const* transformable; } PhysicsEntity; +extern void physics_entity_debug_draw(PhysicsEntity self); + #define impl_PhysicsEntity_for(T, get_rigidbody_f, get_shape_f, on_collision_f)\ static inline PhysicsEntity T##_as_PhysicsEntity(T* x) {\ TC_FN_TYPECHECK(Transformable, T##_as_Transformable, T*);\ diff --git a/src/player.c b/src/player.c index 8181670..7938a99 100644 --- a/src/player.c +++ b/src/player.c @@ -41,11 +41,13 @@ void player_start(Player* self) { float ex_w = 0.1f; float h = .75f; float r = 0.05f; + float rr = (r/3)*2; + self->shape = shape_new((Vector[]){ - {r-ex_w, 0.f}, {-ex_w, -r}, - {-ex_w, r-h}, {r-ex_w, -h}, - {ex_w-r, -h}, {ex_w, r-h}, - {ex_w, -r}, {ex_w-r, 0.f}, + {r-ex_w, 0.f}, {-ex_w, -rr}, + {-ex_w, rr-h}, {r-ex_w, -h}, + {ex_w-r, -h}, {ex_w, rr-h}, + {ex_w, -rr}, {ex_w-r, 0.f}, }, 8); physics_world_add_entity(Player_as_PhysicsEntity(self)); @@ -54,7 +56,7 @@ void player_start(Player* self) { void player_update(Player* self, float dt) { Vector velocity = rigidbody_get_velocity(self->rigidbody); Vector velocity_target = {directional.x, directional.y}; - rigidbody_accelerate(self->rigidbody, vmulff(vsubf(velocity_target, velocity), 200.f)); + rigidbody_accelerate(self->rigidbody, vmulff(vsubf(velocity_target, velocity), 20.f)); } void player_collision(Player* self, Collision hit) { diff --git a/src/rigidbody.c b/src/rigidbody.c index daa8fed..63a2315 100644 --- a/src/rigidbody.c +++ b/src/rigidbody.c @@ -69,11 +69,11 @@ static inline void _internal_rigidbody_collect_contacts(RigidBody* self) { for(size_t i = 0; i < self->contacts.len; ++i) { Contact* contact = list_at(&self->contacts, i); - --(contact->expiry); if(contact->expiry <= 0) { list_erase(&self->contacts, i); i--; } + --(contact->expiry); } } @@ -96,9 +96,9 @@ static inline Vector _internal_calculate_contact_force(Contact* contact) { Collision hit = contact->hit; const float warming = fminf(1.f, contact->warming); - const float d = 1.f; - const float k = 50.0 * warming; - const float b = 1.f; + const float d = 1.0f; + const float k = 1.0f * warming; + const float b = 1.0f; const Vector damping = vmulff(hit.normal, k * d); const Vector bounce = vprojectf(vmulff(hit.normal, -b), hit.velocity); @@ -109,8 +109,14 @@ Vector _internal_calculate_contact_force(Contact* contact) { 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; if (vsqrmagnitudef(contact->hit.separation_force) < 0.001f*0.001f) 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(contact); float dot = vdotf(vnormalizedf(*solve_forces), force); if (veqf(*solve_forces, ZeroVector) || dot * dot > vsqrmagnitudef(*solve_forces) || dot <= 0.0f) { @@ -124,21 +130,19 @@ void _internal_rigidbody_solve_contact(RigidBody* self, Contact* contact, Vector rigidbody_add_impulse(self, force); } - _internal_debug_draw_collision_edge(hit.edge_left, hit.edge_right, hit.normal); - ASSERT_RETURN(!visnanf(force), , "Force contains NaN (1)"); } void rigidbody_solve_contacts(RigidBody* self) { ASSERT_RETURN(!visnanf(self->linear_velocity),, "Velocity is NaN (0)"); + _internal_rigidbody_collect_contacts(self); + Vector solve_forces = ZeroVector; list_foreach(Contact, contact, &self->contacts) { _internal_rigidbody_solve_contact(self, contact, &solve_forces); } - _internal_rigidbody_collect_contacts(self); - ASSERT_RETURN(!visnanf(self->linear_velocity),, "Velocity is NaN (1)"); } @@ -146,7 +150,7 @@ void rigidbody_apply_physics(RigidBody* self) { Vector position = transformable_get_position(self->transformable); Vector velocity = vmulff(self->linear_velocity, delta_time()); - if(vsqrmagnitudef(velocity) > powf(0.00001f, 2)) { + if(vsqrmagnitudef(velocity) > powf(0.00000f, 2)) { transformable_set_position(self->transformable, vaddf(position, velocity)); } @@ -186,3 +190,9 @@ void rigidbody_set_velocity(RigidBody* self, Vector velocity) { self->linear_force = vaddf(self->linear_force, vsubf(velocity, self->linear_force)); self->linear_velocity = velocity; } + +void rigidbody_debug_draw_contacts(RigidBody* self) { + list_foreach(Contact, contact, &self->contacts) { + _internal_debug_draw_collision_edge(contact->hit.edge_left, contact->hit.edge_right, contact->hit.normal); + } +} diff --git a/src/rigidbody.h b/src/rigidbody.h index a4f29e2..4ddb725 100644 --- a/src/rigidbody.h +++ b/src/rigidbody.h @@ -10,24 +10,26 @@ struct Collision; typedef struct RigidBody RigidBody; // Referenced transform is stored but not owned by the rigidbody. -RigidBody* rigidbody_make(Transformable transform); -void rigidbody_destroy(RigidBody* self); +extern RigidBody* rigidbody_make(Transformable transform); +extern void rigidbody_destroy(RigidBody* self); -void rigidbody_add_contact(RigidBody* self, struct Collision hit); -void rigidbody_solve_contacts(RigidBody* self); +extern void rigidbody_add_contact(RigidBody* self, struct Collision hit); +extern void rigidbody_solve_contacts(RigidBody* self); -void rigidbody_apply_physics(RigidBody* self); +extern void rigidbody_apply_physics(RigidBody* self); -float rigidbody_get_mass(const RigidBody* self); -void rigidbody_set_mass(RigidBody* self, float mass); +extern float rigidbody_get_mass(const RigidBody* self); +extern void rigidbody_set_mass(RigidBody* self, float mass); -void rigidbody_add_impulse(RigidBody* self, Vector force); -void rigidbody_accelerate(RigidBody* self, Vector force); +extern void rigidbody_add_impulse(RigidBody* self, Vector force); +extern void rigidbody_accelerate(RigidBody* self, Vector force); -int rigidbody_is_static(const RigidBody* self); -void rigidbody_set_static(RigidBody* self, int is_static); +extern int rigidbody_is_static(const RigidBody* self); +extern void rigidbody_set_static(RigidBody* self, int is_static); -Vector rigidbody_get_velocity(const RigidBody* self); -void rigidbody_set_velocity(RigidBody* self, Vector velocity); +extern Vector rigidbody_get_velocity(const RigidBody* self); +extern void rigidbody_set_velocity(RigidBody* self, Vector velocity); + +extern void rigidbody_debug_draw_contacts(RigidBody* self); #endif // !_fencer_rigidbody_h