From 74369d88fed66d9ac45452f98fd4aa9dc632b091 Mon Sep 17 00:00:00 2001
From: Sara <sara@saragerretsen.nl>
Date: Tue, 17 Oct 2023 17:17:16 +0200
Subject: [PATCH] more debugging for collision sovler
---
src/collision.c | 14 ++++++--------
src/fencer.c | 2 +-
src/physics_entity.c | 21 +++++++++++++++++++++
src/physics_entity.h | 2 ++
src/player.c | 12 +++++++-----
src/rigidbody.c | 28 +++++++++++++++++++---------
src/rigidbody.h | 28 +++++++++++++++-------------
7 files changed, 71 insertions(+), 36 deletions(-)
create mode 100644 src/physics_entity.c
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