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

implemented velocity verlet integration

Browse source
This commit is contained in:
Sara 2023-10-18 22:51:56 +02:00
parent 2b1014e603
commit 37b1fc3228
6 changed files with 75 additions and 29 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

8
src/collision.c
View file

@ -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);

2
src/collision.h
View file

@ -12,7 +12,7 @@ typedef struct Collision {
Vector normal;
Vector velocity;
Vector separation_force;
Vector penetration_vector;
Vector edge_left;
Vector edge_right;

3
src/physics_world.c
View file

@ -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);
}
}

3
src/player.c
View file

@ -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) {

83
src/rigidbody.c
View file

@ -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;
}

5
src/rigidbody.h
View file

@ -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