diff --git a/src/collision.c b/src/collision.c
index 35d9396..15bd2d9 100644
--- a/src/collision.c
+++ b/src/collision.c
@@ -79,7 +79,7 @@ int _internal_collision_get_overlap(PhysicsEntity self, PhysicsEntity other, Col
}
if(sqr_mag == 0) {
- out->separation_force = InfinityVector;
+ out->penetration_vector = InfinityVector;
return 0;
}
}
@@ -96,7 +96,7 @@ int _internal_collision_get_overlap(PhysicsEntity self, PhysicsEntity other, Col
.normal = vnormalizedf(shortest_escape),
.velocity = velocity,
- .separation_force = shortest_escape,
+ .penetration_vector = shortest_escape,
.edge_left = shape_get_point_transformed(self_shape, shortest_escape_edge, *self_transform),
.edge_right = shape_get_point_transformed(self_shape, (1 + shortest_escape_edge) % self_point_count, *self_transform),
@@ -112,7 +112,7 @@ Collision _internal_collision_invert(Collision collision_a, PhysicsEntity a) {
.point = collision_a.point,
.normal = vinvf(collision_a.normal),
.velocity = vinvf(collision_a.velocity),
- .separation_force = vinvf(collision_a.separation_force),
+ .penetration_vector = vinvf(collision_a.penetration_vector),
.edge_left = collision_a.edge_left,
.edge_right = collision_a.edge_right,
};
@@ -126,7 +126,7 @@ int collision_check(PhysicsEntity a, PhysicsEntity b, Collision* out_a, Collisio
if(!collision_a_overlaps || !collision_b_overlaps)
return 0;
- if(vsqrmagnitudef(collision_a.separation_force) < vsqrmagnitudef(collision_b.separation_force))
+ if(vsqrmagnitudef(collision_a.penetration_vector) < vsqrmagnitudef(collision_b.penetration_vector))
collision_b = _internal_collision_invert(collision_a, a);
else
collision_a = _internal_collision_invert(collision_b, b);
diff --git a/src/collision.h b/src/collision.h
index 9d639c0..bc295eb 100644
--- a/src/collision.h
+++ b/src/collision.h
@@ -12,7 +12,7 @@ typedef struct Collision {
Vector normal;
Vector velocity;
- Vector separation_force;
+ Vector penetration_vector;
Vector edge_left;
Vector edge_right;
diff --git a/src/physics_world.c b/src/physics_world.c
index 13ff1ef..53a9676 100644
--- a/src/physics_world.c
+++ b/src/physics_world.c
@@ -125,7 +125,7 @@ void _internal_physics_narrow_collision() {
for(PhysicsEntity* left = _world_bodies; left < end; ++left) {
for(PhysicsEntity* right = _world_bodies; right < half_end; ++right) {
- if(left == right) continue;
+ if(left->data == right->data) continue;
if(collision_check(*left, *right, &collision_left, &collision_right)) {
left->tc->on_collision(left->data, collision_left);
@@ -171,7 +171,6 @@ void _internal_physics_apply() {
for(PhysicsEntity* entity = _world_bodies; entity < end; ++entity) {
body = entity->tc->get_rigidbody(entity->data);
- rigidbody_solve_contacts(body);
rigidbody_apply_physics(body);
}
}
diff --git a/src/player.c b/src/player.c
index 1a61e8d..c585999 100644
--- a/src/player.c
+++ b/src/player.c
@@ -55,8 +55,9 @@ 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};
+ Vector velocity_target = {directional.x, velocity.y};
rigidbody_accelerate(self->rigidbody, vmulff(vsubf(velocity_target, velocity), 20.f));
+ rigidbody_accelerate(self->rigidbody, (Vector){0.0f, 20.f});
}
void player_collision(Player* self, Collision hit) {
diff --git a/src/rigidbody.c b/src/rigidbody.c
index f272e19..15b4222 100644
--- a/src/rigidbody.c
+++ b/src/rigidbody.c
@@ -3,6 +3,7 @@
#include "debug.h"
#include "program.h"
#include "collision.h"
+#include "transformable.h"
typedef struct {
Collision hit;
@@ -15,8 +16,10 @@ struct RigidBody {
float mass;
- Vector linear_force;
+ Vector last_linear_force;
+ Vector next_linear_force;
Vector linear_velocity;
+ Transform internal_transform;
int is_static;
@@ -29,7 +32,13 @@ RigidBody* rigidbody_make(Transformable transform) {
*self = (RigidBody){
.transformable = transform,
.mass = 1.0f,
+
.linear_velocity = ZeroVector,
+ .next_linear_force = ZeroVector,
+ .last_linear_force = ZeroVector,
+
+ .internal_transform = *transform.tc->get_transform(transform.data),
+
.is_static = 0,
.contacts = list_from_type(Contact),
};
@@ -47,6 +56,7 @@ void rigidbody_add_contact(RigidBody* self, Collision hit) {
list_foreach(Contact, contact, &self->contacts) {
if(contact->hit.other.data == hit.other.data) {
++contact->expiry;
+ hit.velocity = contact->hit.velocity;
contact->hit = hit;
contact->warming += delta_time();
return;
@@ -74,6 +84,7 @@ void _internal_rigidbody_collect_contacts(RigidBody* self) {
if(vdotf(vnormalizedf(self->linear_velocity), contact->hit.normal) >= -0.01f || contact->expiry <= -1) {
list_erase(&self->contacts, i);
i--;
+ continue;
}
}
--(contact->expiry);
@@ -96,21 +107,27 @@ void _internal_debug_draw_collision_edge(Vector left, Vector right, Vector norma
}
static inline
-Vector _internal_calculate_contact_force(Contact* contact) {
+Vector _internal_calculate_contact_force(RigidBody* self, Contact* contact) {
Collision hit = contact->hit;
- if(vsqrmagnitudef(hit.separation_force) < powf(0.01f, 2))
+ if(vsqrmagnitudef(hit.penetration_vector) < powf(0.01f, 2))
return ZeroVector;
-
- const float warming = fminf(1.f, contact->warming * 100.f);
+
+
+ const float warming = fminf(1.f, contact->warming * 1000.f);
const float d = 1.0f;
- const float k = 0.001f;
- const float b = 1.0f * warming;
+ const float k = 0.3f;
+ const float b = 0.3f;
const Vector damping = vmulff(hit.normal, k * d);
- const Vector bounce = vprojectf(vmulff(hit.normal, -b), hit.velocity);
+ const Vector bounce = vmulff(hit.normal, b * vdotf(hit.normal, hit.velocity));
- return vsubf(damping, bounce);
+ float dot = -vdotf(contact->hit.normal, vaddf(self->next_linear_force, self->linear_velocity));
+ if(dot < 0)
+ dot = 0;
+ Vector counter_force = vmulff(contact->hit.normal, dot * vsqrmagnitudef(hit.penetration_vector));
+
+ return vaddf(vmulff(vsubf(damping, bounce), warming), counter_force);
}
static inline
@@ -118,20 +135,18 @@ void _internal_rigidbody_solve_contact(RigidBody* self, Contact* contact, Vector
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);
+ 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) {
#if !NDEBUG
LOG_INFO("warming: %f", contact->warming);
LOG_INFO("force: %f %f", force.x, force.y);
LOG_INFO("dot: %f", dot);
- LOG_INFO("mag: %f", vmagnitudef(self->linear_force));
+ LOG_INFO("mag: %f", vmagnitudef(self->next_linear_force));
#endif
*solve_forces = vaddf(*solve_forces, force);
rigidbody_add_impulse(self, force);
@@ -153,11 +168,33 @@ void rigidbody_solve_contacts(RigidBody* self) {
ASSERT_RETURN(!visnanf(self->linear_velocity),, "Velocity is NaN (1)");
}
+static inline
+void _internal_rigidbody_integrate_forces(RigidBody* self) {
+ const float dt = delta_time();
+
+ Vector position = self->internal_transform.position;
+ Vector velocity = self->linear_velocity;
+ Vector acceleration = vmulff(self->last_linear_force, 0.5f * dt*dt);
+
+ position = vaddf(position, vmulff(velocity, dt));
+ position = vaddf(position, acceleration);
+
+ velocity = vaddf(velocity, acceleration);
+ acceleration = vmulff(self->next_linear_force, dt/2);
+ velocity = vaddf(velocity, acceleration);
+
+ self->linear_velocity = velocity;
+ self->internal_transform.position = position;
+ if(vsqrdistf(position, transformable_get_position(self->transformable)) > 0.001f)
+ transformable_set_position(self->transformable, position);
+
+ self->last_linear_force = self->next_linear_force;
+ self->next_linear_force = ZeroVector;
+}
+
void rigidbody_apply_physics(RigidBody* self) {
- Vector position = transformable_get_position(self->transformable);
- Vector velocity = vmulff(self->linear_velocity, delta_time());
- transformable_set_position(self->transformable, vaddf(position, velocity));
- self->linear_force = ZeroVector;
+ rigidbody_solve_contacts(self);
+ _internal_rigidbody_integrate_forces(self);
}
float rigidbody_get_mass(const RigidBody* self) {
@@ -169,12 +206,12 @@ void rigidbody_set_mass(RigidBody* self, float mass) {
}
void rigidbody_add_impulse(RigidBody* self, Vector force) {
- self->linear_force = vaddf(self->linear_force, force);
- self->linear_velocity = vaddf(self->linear_velocity, force);
+ rigidbody_accelerate(self, vmulff(force, 1.0f/delta_time()));
}
void rigidbody_accelerate(RigidBody* self, Vector force) {
- rigidbody_add_impulse(self, vmulff(force, delta_time()));
+ if(vsqrmagnitudef(force) > powf(0.01f, 2))
+ self->next_linear_force = vaddf(self->next_linear_force, force);
}
int rigidbody_is_static(const RigidBody* self) {
@@ -190,7 +227,7 @@ Vector rigidbody_get_velocity(const RigidBody* self) {
}
void rigidbody_set_velocity(RigidBody* self, Vector velocity) {
- self->linear_force = vaddf(self->linear_force, vsubf(velocity, self->linear_force));
+ self->next_linear_force = vaddf(self->next_linear_force, vsubf(velocity, self->next_linear_force));
self->linear_velocity = velocity;
}
@@ -199,3 +236,7 @@ void rigidbody_debug_draw_contacts(RigidBody* self) {
_internal_debug_draw_collision_edge(contact->hit.edge_left, contact->hit.edge_right, contact->hit.normal);
}
}
+
+Transform* rigidbody_get_transform(RigidBody* self) {
+ return &self->internal_transform;
+}
diff --git a/src/rigidbody.h b/src/rigidbody.h
index 4ddb725..0d3f636 100644
--- a/src/rigidbody.h
+++ b/src/rigidbody.h
@@ -31,5 +31,10 @@ 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);
+extern Transform* rigidbody_get_transform(RigidBody* self);
+
+impl_Transformable_for(RigidBody,
+ rigidbody_get_transform
+)
#endif // !_fencer_rigidbody_h