System info & network
This commit is contained in:
336
code/physics/collision.cpp
Normal file
336
code/physics/collision.cpp
Normal file
@@ -0,0 +1,336 @@
|
||||
#include "collision.h"
|
||||
#include "../lib/ds.h"
|
||||
#include "../lib/geometry.h"
|
||||
#include <assert.h>
|
||||
|
||||
|
||||
void phy_collisions_broadphase(phy_world *world, phy_body_pair *pair_list, u32 max_pairs, u32 *num_pairs)
|
||||
{
|
||||
// Update AABBs
|
||||
for(u32 i = 0; i < world->bodies_count; i++)
|
||||
{
|
||||
phy_body *body = world->bodies + i;
|
||||
body->aabb = phy_aabb_from_body(body);
|
||||
}
|
||||
|
||||
|
||||
u32 pairs_count = 0;
|
||||
|
||||
for(u32 a = 0; a < world->bodies_count; a++)
|
||||
{
|
||||
phy_body *body_a = world->bodies + a;
|
||||
for(u32 b = a + 1; b < world->bodies_count; b++)
|
||||
{
|
||||
phy_body *body_b = world->bodies + b;
|
||||
|
||||
if(overlaps(body_a->aabb, body_b->aabb))
|
||||
{
|
||||
// Add to list of possible collisions.
|
||||
if(pairs_count < max_pairs)
|
||||
{
|
||||
pair_list[pairs_count] = phy_body_pair{
|
||||
.body_a = body_a,
|
||||
.body_b = body_b
|
||||
};
|
||||
}
|
||||
pairs_count++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if(num_pairs)
|
||||
*num_pairs = pairs_count;
|
||||
}
|
||||
|
||||
|
||||
// @Cleanup: put this in the right place
|
||||
// @Performance: this is probably not a good way of doing this
|
||||
void project_box_on_axis(phy_body *body, v3 axis, f32 *min, f32 *max)
|
||||
{
|
||||
v3 points[8];
|
||||
build_cube_vertices(points);
|
||||
|
||||
m4 transform = translation_v3(body->position) * rotation_v3(body->rotation) * scale_v3(body->box.dimensions);
|
||||
|
||||
*min = *max = project_point_on_vector(axis, body->position);
|
||||
for(int i = 0; i < 8; i++)
|
||||
{
|
||||
f32 projected = project_point_on_vector(axis, (transform * V4(points[i], 1)).xyz);
|
||||
*min = minimum(*min, projected);
|
||||
*max = maximum(*max, projected);
|
||||
}
|
||||
}
|
||||
|
||||
f32 axis_range_distance(f32 a_min, f32 a_max, f32 b_min, f32 b_max)
|
||||
{
|
||||
f32 a_range = a_max - a_min;
|
||||
f32 a_center = a_min + a_range / 2;
|
||||
f32 b_range = b_max - b_min;
|
||||
f32 b_center = b_min + b_range / 2;
|
||||
return fabs(a_center - b_center) - (a_range + b_range) / 2;
|
||||
}
|
||||
|
||||
bool sat_box_face_point_collision_test(phy_body *body_a, phy_body *body_b, v3 axis, f32 a_size_on_current_axis, f32 *depth, v3 *furthest_a, v3 *furthest_b)
|
||||
{
|
||||
f32 a_middle = project_point_on_vector(axis, body_a->position);
|
||||
f32 b_middle = project_point_on_vector(axis, body_b->position);
|
||||
f32 direction = (b_middle - a_middle < 0) ? -1 : 1;
|
||||
|
||||
f32 boundary = a_middle + direction * a_size_on_current_axis / 2;
|
||||
f32 dist = +INFINITY;
|
||||
v3 point;
|
||||
|
||||
v3 points[8]; build_cube_vertices(points);
|
||||
m4 transform = translation_v3(body_b->position) * rotation_v3(body_b->rotation) * scale_v3(body_b->box.dimensions);
|
||||
|
||||
for(int i = 0; i < 8; i++)
|
||||
{
|
||||
v3 p = (transform * V4(points[i], 1)).xyz;
|
||||
f32 projected = project_point_on_vector(axis, p);
|
||||
f32 curr_dist = direction * (projected - boundary);
|
||||
if(curr_dist < dist)
|
||||
{
|
||||
dist = curr_dist;
|
||||
point = p;
|
||||
}
|
||||
}
|
||||
|
||||
if(dist < 0) // Possible collision
|
||||
{
|
||||
v3 furthest_point_b = point;
|
||||
v3 furthest_point_a = point + -dist * -direction * axis;
|
||||
|
||||
*depth = -dist * direction;
|
||||
*furthest_a = furthest_point_a;
|
||||
*furthest_b = furthest_point_b;
|
||||
return true;
|
||||
}
|
||||
// else Separated on this axis. No collision
|
||||
return false;
|
||||
}
|
||||
|
||||
bool sat_box_collision_test(phy_body *body_a, phy_body *body_b, phy_collision *res_collision)
|
||||
{
|
||||
// Separating axis test
|
||||
// 3 + 3 = 6 face normals
|
||||
// 3 * 3 = 9 right angle to pair of edges
|
||||
v3 axes[15];
|
||||
m4 a_rotation = rotation_v3(body_a->rotation);
|
||||
axes[0] = extract_column(a_rotation, 0).xyz;
|
||||
axes[1] = extract_column(a_rotation, 1).xyz;
|
||||
axes[2] = extract_column(a_rotation, 2).xyz;
|
||||
m4 b_rotation = rotation_v3(body_b->rotation);
|
||||
axes[3] = -extract_column(b_rotation, 0).xyz;
|
||||
axes[4] = -extract_column(b_rotation, 1).xyz;
|
||||
axes[5] = -extract_column(b_rotation, 2).xyz;
|
||||
|
||||
for(int a = 0; a < 3; a++)
|
||||
for(int b = 0; b < 3; b++)
|
||||
{
|
||||
(axes+6)[3*a + b] = cross(axes[a], axes[b + 3]);
|
||||
}
|
||||
|
||||
phy_collision collision;
|
||||
collision.body_a = body_a;
|
||||
collision.body_b = body_b;
|
||||
collision.depth = +INFINITY;
|
||||
|
||||
// Face axes: body a
|
||||
for(int i = 0; i < 3; i++)
|
||||
{
|
||||
v3 axis = axes[i];
|
||||
f32 depth; v3 furthest_point_a, furthest_point_b;
|
||||
bool collides = sat_box_face_point_collision_test(body_a, body_b, axis, body_a->box.dimensions.E[i], &depth, &furthest_point_a, &furthest_point_b);
|
||||
|
||||
if(!collides)
|
||||
return false;
|
||||
|
||||
if(abs(depth) < abs(collision.depth))
|
||||
{
|
||||
collision.depth = depth;
|
||||
collision.furthest_point_a = furthest_point_a;
|
||||
collision.furthest_point_b = furthest_point_b;
|
||||
collision.best_separation_direction = axis;
|
||||
}
|
||||
}
|
||||
// Face axes: body b
|
||||
for(int i = 0; i < 3; i++)
|
||||
{
|
||||
v3 axis = axes[3 + i];
|
||||
f32 depth; v3 furthest_point_a, furthest_point_b;
|
||||
bool collides = sat_box_face_point_collision_test(body_b, body_a, axis, body_b->box.dimensions.E[i], &depth, &furthest_point_b, &furthest_point_a);
|
||||
|
||||
if(!collides)
|
||||
return false;
|
||||
|
||||
if(abs(depth) < abs(collision.depth))
|
||||
{
|
||||
collision.depth = depth;
|
||||
collision.furthest_point_a = furthest_point_a;
|
||||
collision.furthest_point_b = furthest_point_b;
|
||||
collision.best_separation_direction = -axis;
|
||||
}
|
||||
}
|
||||
|
||||
// @TODO: edge-edge
|
||||
|
||||
*res_collision = collision;
|
||||
return true;
|
||||
}
|
||||
|
||||
void phy_collisions_detection(phy_world *world, phy_body_pair *pair_list, u32 num_pairs, phy_collision *collision_list, u32 max_collisions, u32 *num_collisions)
|
||||
{
|
||||
u32 n_collisions = 0;
|
||||
for(u32 i = 0; i < num_pairs; i++)
|
||||
{
|
||||
phy_body *body_a = pair_list[i].body_a;
|
||||
phy_body *body_b = pair_list[i].body_b;
|
||||
|
||||
if(body_b->shape < body_a->shape)
|
||||
swap(body_b, body_a);
|
||||
|
||||
|
||||
switch(body_a->shape)
|
||||
{
|
||||
case PHY_SHAPE_SPHERE:
|
||||
{
|
||||
switch(body_b->shape)
|
||||
{
|
||||
case PHY_SHAPE_SPHERE:
|
||||
{
|
||||
f32 dist = distance(body_a->position, body_b->position);
|
||||
f32 radius_sum = body_a->sphere.radius + body_b->sphere.radius;
|
||||
if(dist <= radius_sum)
|
||||
{
|
||||
phy_collision collision;
|
||||
|
||||
collision.body_a = body_a;
|
||||
collision.body_b = body_b;
|
||||
collision.depth = radius_sum - dist;
|
||||
v3 direction = normalize(body_b->position - body_a->position);
|
||||
collision.furthest_point_a = body_a->position + (body_a->sphere.radius - collision.depth) * direction;
|
||||
collision.furthest_point_b = body_b->position + (body_b->sphere.radius - collision.depth) * -direction;
|
||||
collision.best_separation_direction = direction;
|
||||
|
||||
if(n_collisions < max_collisions)
|
||||
collision_list[n_collisions] = collision;
|
||||
n_collisions++;
|
||||
}
|
||||
} break;
|
||||
case PHY_SHAPE_BOX:
|
||||
{
|
||||
// Use box's cordinate space
|
||||
m4 box_b_coord_space_transform = rotation_v3(-body_b->rotation) * translation_v3(-body_b->position);
|
||||
|
||||
v4 sphere_center4 = box_b_coord_space_transform * V4(body_a->position, 1.0);
|
||||
v3 sphere_center = sphere_center4.xyz / sphere_center4.w;
|
||||
|
||||
Box box = {
|
||||
.min = -0.5*body_b->box.dimensions,
|
||||
.max = 0.5*body_b->box.dimensions
|
||||
};
|
||||
|
||||
v3 closest_point;
|
||||
f32 dist = box_SDF(box, sphere_center, &closest_point);
|
||||
|
||||
if(dist - body_a->sphere.radius <= 0)
|
||||
{
|
||||
phy_collision collision;
|
||||
|
||||
collision.body_a = body_a;
|
||||
collision.body_b = body_b;
|
||||
collision.depth = dist - body_a->sphere.radius;
|
||||
v3 direction = normalize(closest_point - body_a ->position);
|
||||
collision.furthest_point_a = body_a->position + (body_a->sphere.radius - collision.depth) * direction;
|
||||
collision.furthest_point_b = body_a->position + body_a->sphere.radius * direction;
|
||||
collision.best_separation_direction = direction;
|
||||
|
||||
if(n_collisions < max_collisions)
|
||||
collision_list[n_collisions] = collision;
|
||||
n_collisions++;
|
||||
}
|
||||
|
||||
} break;
|
||||
case PHY_SHAPE_MESH:
|
||||
{
|
||||
assert(false && "Unmanaged PHY_SHAPE pair");
|
||||
} break;
|
||||
default:
|
||||
{
|
||||
assert(false && "Unmanaged PHY_SHAPE pair");
|
||||
} break;
|
||||
}
|
||||
} break;
|
||||
case PHY_SHAPE_BOX:
|
||||
{
|
||||
switch(body_b->shape)
|
||||
{
|
||||
// case PHY_SHAPE_SPHERE: // Already managed
|
||||
case PHY_SHAPE_BOX:
|
||||
{
|
||||
phy_collision collision;
|
||||
bool collides = sat_box_collision_test(body_a, body_b, &collision);
|
||||
if(collides)
|
||||
{
|
||||
if(n_collisions < max_collisions)
|
||||
collision_list[n_collisions] = collision;
|
||||
n_collisions++;
|
||||
}
|
||||
} break;
|
||||
case PHY_SHAPE_MESH:
|
||||
{
|
||||
assert(false && "Unmanaged PHY_SHAPE pair");
|
||||
} break;
|
||||
default:
|
||||
{
|
||||
assert(false && "Unmanaged PHY_SHAPE pair");
|
||||
} break;
|
||||
}
|
||||
} break;
|
||||
case PHY_SHAPE_MESH:
|
||||
{
|
||||
switch(body_b->shape)
|
||||
{
|
||||
// case PHY_SHAPE_SPHERE: // Already managed
|
||||
// case PHY_SHAPE_BOX: // Already managed
|
||||
case PHY_SHAPE_MESH:
|
||||
{
|
||||
assert(false && "Unmanaged PHY_SHAPE pair");
|
||||
} break;
|
||||
default:
|
||||
{
|
||||
assert(false && "Unmanaged PHY_SHAPE pair");
|
||||
} break;
|
||||
}
|
||||
} break;
|
||||
default:
|
||||
{
|
||||
assert(false && "Unmanaged PHY_SHAPE");
|
||||
} break;
|
||||
}
|
||||
}
|
||||
*num_collisions = n_collisions;
|
||||
}
|
||||
|
||||
void phy_collisions_resolution(phy_world *world, phy_collision *collision_list, u32 num_collisions)
|
||||
{
|
||||
|
||||
for(u32 i = 0; i < num_collisions; i++)
|
||||
{
|
||||
phy_body *body_a = collision_list[i].body_a;
|
||||
phy_body *body_b = collision_list[i].body_b;
|
||||
v3 separation_direction = collision_list[i].best_separation_direction;
|
||||
f32 depth = collision_list[i].depth;
|
||||
|
||||
if(body_a->mass != 0) // Not a static body
|
||||
{
|
||||
body_a->position += -separation_direction * depth;//-normalize(body_a->velocity) * dot(normalize(body_a->velocity), separation_direction * depth);
|
||||
body_a->velocity = {0,0,0};//length(body_a->velocity) * separation_direction * body_a->bounciness;
|
||||
}
|
||||
if(body_b->mass != 0) // Not a static body
|
||||
{
|
||||
body_b->position += separation_direction * depth;//-normalize(body_b->velocity) * dot(normalize(body_b->velocity), separation_direction * depth);
|
||||
body_b->velocity = {0,0,0};//length(body_a->velocity) * -separation_direction * body_b->bounciness;
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user