106 lines
3.0 KiB
C++
106 lines
3.0 KiB
C++
#include "physics/particle_contact.hpp"
|
|
|
|
namespace physics {
|
|
|
|
void particle_contact::resolve(float duration)
|
|
{
|
|
resolve_velocity(duration);
|
|
resolve_interpenetration(duration);
|
|
}
|
|
|
|
float particle_contact::calculate_separating_velocity() const
|
|
{
|
|
vec3 relative_velocity = particle[0]->velocity;
|
|
if (particle[1])
|
|
relative_velocity -= particle[1]->velocity;
|
|
|
|
return dot(relative_velocity, contact_normal);
|
|
}
|
|
|
|
void particle_contact::resolve_velocity(float duration)
|
|
{
|
|
float separating_velocity = calculate_separating_velocity();
|
|
|
|
if (separating_velocity > 0) {
|
|
// no impulse required
|
|
return;
|
|
}
|
|
|
|
float new_separating_velocity = -separating_velocity * restitution;
|
|
|
|
// velocity build-up due to acceleration only
|
|
vec3 acceleration_caused_velocity = particle[0]->acceleration;
|
|
if (particle[1]) acceleration_caused_velocity -= particle[1]->acceleration;
|
|
float acceleration_caused_separating_velocity = dot(acceleration_caused_velocity, contact_normal) * duration;
|
|
|
|
// remove acceleration velocity from separating velocity
|
|
if (acceleration_caused_separating_velocity < 0) {
|
|
new_separating_velocity += restitution * acceleration_caused_separating_velocity;
|
|
|
|
if (new_separating_velocity < 0) new_separating_velocity = 0;
|
|
}
|
|
|
|
float delta_velocity = new_separating_velocity - separating_velocity;
|
|
|
|
float total_inverse_mass = get_total_inverse_mass();
|
|
|
|
if (total_inverse_mass <= 0)
|
|
return;
|
|
|
|
float impulse = delta_velocity / total_inverse_mass;
|
|
|
|
vec3 impulse_per_inverse_mass = contact_normal * impulse;
|
|
|
|
particle[0]->velocity
|
|
= particle[0]->velocity
|
|
+ impulse_per_inverse_mass * particle[0]->inverse_mass;
|
|
|
|
if (particle[1]) {
|
|
particle[1]->velocity
|
|
= particle[1]->velocity
|
|
+ impulse_per_inverse_mass * -particle[1]->inverse_mass;
|
|
}
|
|
}
|
|
|
|
void particle_contact::resolve_interpenetration(float duration)
|
|
{
|
|
if (penetration <= 0) return;
|
|
|
|
float total_inverse_mass = get_total_inverse_mass();
|
|
|
|
if (total_inverse_mass <= 0) return;
|
|
|
|
vec3 move_per_inverse_mass = contact_normal * (penetration / total_inverse_mass);
|
|
|
|
particle[0]->position += move_per_inverse_mass * particle[0]->inverse_mass;
|
|
if (particle[1]) {
|
|
particle[1]->position += move_per_inverse_mass * -particle[1]->inverse_mass;
|
|
}
|
|
}
|
|
|
|
void particle_contact_resolver::resolve_contacts(particle_contact * contacts,
|
|
int contacts_length,
|
|
float duration)
|
|
{
|
|
iterations = 0;
|
|
|
|
while (iterations < max_iterations) {
|
|
float max = 0;
|
|
int max_index = -1;
|
|
for (int i = 0; i < contacts_length; i++) {
|
|
float separating_velocity = contacts[i].calculate_separating_velocity();
|
|
if (separating_velocity < max) {
|
|
max = separating_velocity;
|
|
max_index = i;
|
|
}
|
|
}
|
|
|
|
if (max_index == -1) break;
|
|
|
|
contacts[max_index].resolve(duration);
|
|
|
|
iterations += 1;
|
|
}
|
|
}
|
|
}
|