108 lines
3.0 KiB
C++
108 lines
3.0 KiB
C++
#include "physics/collide.hpp"
|
|
|
|
namespace physics {
|
|
|
|
int collision_detector::sphere_and_sphere(const collision_sphere& a,
|
|
const collision_sphere& b,
|
|
collision_data& data)
|
|
{
|
|
if (data.ix >= data.length)
|
|
return 0;
|
|
|
|
vec3 a_position = col(a.transform, 3);
|
|
vec3 b_position = col(b.transform, 3);
|
|
|
|
vec3 mid = a_position - b_position;
|
|
float length = magnitude(mid);
|
|
|
|
if (length >= (a.radius + b.radius)) {
|
|
return 0;
|
|
}
|
|
|
|
vec3 normal = mid * (1.0f / length);
|
|
contact * contact = data.add_contact();
|
|
contact->contact_normal = normal;
|
|
contact->contact_point = a_position + mid * 0.5f;
|
|
contact->penetration = a.radius + b.radius - length;
|
|
contact->body[0] = a.body;
|
|
contact->body[1] = b.body;
|
|
contact->restitution = data.restitution;
|
|
contact->friction = data.friction;
|
|
|
|
return 1;
|
|
}
|
|
|
|
int collision_detector::sphere_and_halfspace(const collision_sphere& sphere,
|
|
const collision_plane& plane,
|
|
collision_data& data)
|
|
{
|
|
if (data.ix >= data.length)
|
|
return 0;
|
|
|
|
vec3 position = col(sphere.transform, 3);
|
|
float distance = dot(plane.normal, position) - sphere.radius - plane.distance;
|
|
|
|
if (distance >= 0)
|
|
return 0;
|
|
|
|
contact * contact = data.add_contact();
|
|
contact->contact_normal = plane.normal;
|
|
contact->penetration = -distance;
|
|
contact->contact_point = position - plane.normal * (distance + sphere.radius);
|
|
contact->body[0] = sphere.body;
|
|
contact->body[1] = nullptr;
|
|
contact->restitution = data.restitution;
|
|
contact->friction = data.friction;
|
|
|
|
return 1;
|
|
}
|
|
|
|
int collision_detector::sphere_and_plane(const collision_sphere& sphere,
|
|
const collision_plane& plane,
|
|
collision_data& data)
|
|
{
|
|
if (data.ix >= data.length)
|
|
return 0;
|
|
|
|
vec3 position = col(sphere.transform, 3);
|
|
|
|
float distance = dot(plane.normal, position) - plane.distance;
|
|
|
|
if (distance * distance > sphere.radius * sphere.radius) {
|
|
return 0;
|
|
}
|
|
|
|
vec3 normal;
|
|
float penetration;
|
|
if (distance < 0) {
|
|
normal = -plane.normal;
|
|
penetration = distance;
|
|
} else {
|
|
normal = plane.normal;
|
|
penetration = -distance;
|
|
}
|
|
penetration += sphere.radius;
|
|
|
|
contact * contact = data.add_contact();
|
|
contact->contact_normal = normal;
|
|
contact->penetration = penetration;
|
|
contact->contact_point = position - plane.normal * distance;
|
|
contact->body[0] = sphere.body;
|
|
contact->body[1] = nullptr;
|
|
contact->restitution = data.restitution;
|
|
contact->friction = data.friction;
|
|
|
|
return 1;
|
|
}
|
|
|
|
int collision_detector::box_and_halfspace(const collision_box& box,
|
|
const collision_plane& plane,
|
|
collision_data& data)
|
|
{
|
|
if (data.ix >= data.length)
|
|
return 0;
|
|
|
|
return 0;
|
|
}
|
|
}
|