stephenseo/3d_collision_helpers
1
0
Fork 0
Code Issues Pull requests Actions Packages Projects Releases Wiki Activity

Impl Sphere/Sphere and Sphere/AABB collision

Browse source 
TODO: Impl Sphere/Generic-Box collision
This commit is contained in:
Stephen Seo 2024-04-26 17:36:57 +09:00
parent 849818e6f5
commit cc4e67ace1
3 changed files with 281 additions and 0 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

166
src/sc_sacd.cpp
View file

@ -9,10 +9,24 @@
// Private Helpers BEGIN
// =============================================================================
constexpr float INV_SQRT2 = 0.70710678118654752440F;
SC_SACD_Vec3 operator+(const SC_SACD_Vec3 &a, const SC_SACD_Vec3 &b) {
return SC_SACD_Vec3{a.x + b.x, a.y + b.y, a.z + b.z};
}
SC_SACD_Vec3 operator-(const SC_SACD_Vec3 &a, const SC_SACD_Vec3 &b) {
return SC_SACD_Vec3{a.x - b.x, a.y - b.y, a.z - b.z};
}
SC_SACD_Vec3 operator*(const SC_SACD_Vec3 &a, float scalar) {
return SC_SACD_Vec3{a.x * scalar, a.y * scalar, a.z * scalar};
}
SC_SACD_Vec3 operator/(const SC_SACD_Vec3 &a, float scalar) {
return SC_SACD_Vec3{a.x / scalar, a.y / scalar, a.z / scalar};
}
std::vector<SC_SACD_Vec3> SC_SACD_Get_Box_Normals(
const SC_SACD_Generic_Box *box) {
std::vector<SC_SACD_Vec3> normals;
@ -184,6 +198,141 @@ int SC_SACD_AABB_Generic_Box_Collision(const SC_SACD_AABB_Box *a,
return SC_SACD_Generic_Box_Collision(&a_conv, b);
}
int SC_SACD_Sphere_Collision(const SC_SACD_Sphere *a, const SC_SACD_Sphere *b) {
SC_SACD_Vec3 vec{a->x - b->x, a->y - b->y, a->z - b->z};
return (a->radius + b->radius) > std::sqrt(SC_SACD_Dot_Product(vec, vec)) ? 1
: 0;
}
int SC_SACD_Sphere_AABB_Box_Collision(const SC_SACD_Sphere *sphere,
const SC_SACD_AABB_Box *box) {
std::vector<SC_SACD_Vec3> dirs{// yz-plane
{0.0F, INV_SQRT2, INV_SQRT2},
// xz-plane
{INV_SQRT2, 0.0F, INV_SQRT2},
// xy-plane
{INV_SQRT2, INV_SQRT2, 0.0F}};
SC_SACD_Vec3 pos{box->x, box->y, box->z};
SC_SACD_Vec3 sphere_pos{sphere->x, sphere->y, sphere->z};
for (unsigned int i = 0; i < 3; ++i) {
SC_SACD_Vec3 side_pos = pos;
// Side 1 of 2.
// Get point on side.
switch (i) {
case 0: // yz-plane
side_pos.x += box->width / 2.0F;
break;
case 1: // xz-plane
side_pos.y += box->height / 2.0F;
break;
case 2: // xy-plane
side_pos.z += box->depth / 2.0F;
break;
}
// Calculate closest point to "side".
SC_SACD_Vec3 closest_point =
SC_SACD_Closest_Point_Dir_Normalized(&side_pos, &dirs[i], &sphere_pos);
// Calculate diff between closest and sphere.
SC_SACD_Vec3 point_diff = closest_point - sphere_pos;
float magnitude = std::sqrt(SC_SACD_Dot_Product(point_diff, point_diff));
if (magnitude < sphere->radius) {
// Check if point is on side.
switch (i) {
case 0: // yz-plane
if (closest_point.y > side_pos.y - box->height / 2.0F &&
closest_point.y < side_pos.y + box->height / 2.0F &&
closest_point.z > side_pos.z - box->depth / 2.0F &&
closest_point.z < side_pos.z + box->depth / 2.0F) {
return 1;
}
break;
case 1: // xz-plane
if (closest_point.x > side_pos.x - box->width / 2.0F &&
closest_point.x < side_pos.x + box->width / 2.0F &&
closest_point.z > side_pos.z - box->depth / 2.0F &&
closest_point.z < side_pos.z + box->depth / 2.0F) {
return 1;
}
break;
case 2: // xy-plane
if (closest_point.x > side_pos.x - box->width / 2.0F &&
closest_point.x < side_pos.x + box->width / 2.0F &&
closest_point.y > side_pos.y - box->height / 2.0F &&
closest_point.y < side_pos.y + box->height / 2.0F) {
return 1;
}
break;
}
}
// Side 2 of 2.
// Get point on side.
switch (i) {
case 0: // yz-plane
side_pos.x -= box->width / 2.0F;
break;
case 1: // xz-plane
side_pos.y -= box->height / 2.0F;
break;
case 2: // xy-plane
side_pos.z -= box->depth / 2.0F;
break;
}
// Calculate closest point to "side".
closest_point =
SC_SACD_Closest_Point_Dir_Normalized(&side_pos, &dirs[i], &sphere_pos);
// Calculate diff between closest and sphere.
point_diff = closest_point - sphere_pos;
magnitude = std::sqrt(SC_SACD_Dot_Product(point_diff, point_diff));
if (magnitude < sphere->radius) {
// Check if point is on side.
switch (i) {
case 0: // yz-plane
if (closest_point.y > side_pos.y - box->height / 2.0F &&
closest_point.y < side_pos.y + box->height / 2.0F &&
closest_point.z > side_pos.z - box->depth / 2.0F &&
closest_point.z < side_pos.z + box->depth / 2.0F) {
return 1;
}
break;
case 1: // xz-plane
if (closest_point.x > side_pos.x - box->width / 2.0F &&
closest_point.x < side_pos.x + box->width / 2.0F &&
closest_point.z > side_pos.z - box->depth / 2.0F &&
closest_point.z < side_pos.z + box->depth / 2.0F) {
return 1;
}
break;
case 2: // xy-plane
if (closest_point.x > side_pos.x - box->width / 2.0F &&
closest_point.x < side_pos.x + box->width / 2.0F &&
closest_point.y > side_pos.y - box->height / 2.0F &&
closest_point.y < side_pos.y + box->height / 2.0F) {
return 1;
}
break;
}
}
}
return 0;
}
int SC_SACD_Sphere_Box_Collision(const SC_SACD_Sphere *sphere,
const SC_SACD_Generic_Box *box) {
// TODO
return 0;
}
float SC_SACD_Dot_Product(const SC_SACD_Vec3 a, const SC_SACD_Vec3 b) {
return a.x * b.x + a.y * b.y + a.z * b.z;
}
@ -263,3 +412,20 @@ SC_SACD_Vec3 SC_SACD_Vec3_Rotate(const SC_SACD_Vec3 vec, float x_axis,
return SC_SACD_Mat3_Vec3_Mult(&mat, result);
}
SC_SACD_Vec3 SC_SACD_Closest_Point_Dir_Normalized(const SC_SACD_Vec3 *pos,
const SC_SACD_Vec3 *dir,
const SC_SACD_Vec3 *point) {
float alpha =
SC_SACD_Dot_Product(*dir, *point) - SC_SACD_Dot_Product(*dir, *pos);
return *pos + *dir * alpha;
}
SC_SACD_Vec3 SC_SACD_Closest_Point(const SC_SACD_Vec3 *pos,
const SC_SACD_Vec3 *dir,
const SC_SACD_Vec3 *point) {
float alpha =
(SC_SACD_Dot_Product(*dir, *point) - SC_SACD_Dot_Product(*dir, *pos)) /
SC_SACD_Dot_Product(*dir, *dir);
return *pos + *dir * alpha;
}

24
src/sc_sacd.h
View file

@ -60,6 +60,13 @@ typedef struct SC_SACD_EXPORT SC_SACD_Generic_Box {
float z_radians;
} SC_SACD_Generic_Box;
typedef struct SC_SACD_EXPORT SC_SACD_Sphere {
float x;
float y;
float z;
float radius;
} SC_SACD_Sphere;
/// Returns non-zero if there is collision.
SC_SACD_EXPORT int SC_SACD_AABB_Box_Collision(const SC_SACD_AABB_Box *a,
const SC_SACD_AABB_Box *b);
@ -72,6 +79,15 @@ SC_SACD_EXPORT int SC_SACD_Generic_Box_Collision(const SC_SACD_Generic_Box *a,
SC_SACD_EXPORT int SC_SACD_AABB_Generic_Box_Collision(
const SC_SACD_AABB_Box *a, const SC_SACD_Generic_Box *b);
SC_SACD_EXPORT int SC_SACD_Sphere_Collision(const SC_SACD_Sphere *a,
const SC_SACD_Sphere *b);
SC_SACD_EXPORT int SC_SACD_Sphere_AABB_Box_Collision(
const SC_SACD_Sphere *sphere, const SC_SACD_AABB_Box *box);
SC_SACD_EXPORT int SC_SACD_Sphere_Box_Collision(const SC_SACD_Sphere *sphere,
const SC_SACD_Generic_Box *box);
SC_SACD_EXPORT float SC_SACD_Dot_Product(const SC_SACD_Vec3 a,
const SC_SACD_Vec3 b);
@ -85,6 +101,14 @@ SC_SACD_EXPORT SC_SACD_Vec3 SC_SACD_Vec3_Rotate(const SC_SACD_Vec3 vec,
float x_axis, float y_axis,
float z_axis);
SC_SACD_EXPORT SC_SACD_Vec3 SC_SACD_Closest_Point_Dir_Normalized(
const SC_SACD_Vec3 *pos, const SC_SACD_Vec3 *dir,
const SC_SACD_Vec3 *point);
SC_SACD_EXPORT SC_SACD_Vec3 SC_SACD_Closest_Point(const SC_SACD_Vec3 *pos,
const SC_SACD_Vec3 *dir,
const SC_SACD_Vec3 *point);
#ifdef __cplusplus
}
#endif

91
src/test.cpp
View file

@ -131,6 +131,97 @@ int main() {
CHECK_TRUE(SC_SACD_Generic_Box_Collision(&a, &b));
}
// Test Sphere/Sphere collision check.
{
SC_SACD_Sphere a{0.0F, 0.0F, 0.0F, 1.0F};
SC_SACD_Sphere b{0.0F, 0.0F, 0.0F, 1.0F};
CHECK_TRUE(SC_SACD_Sphere_Collision(&a, &b));
a.x = 10.0F;
a.y = 10.0F;
a.z = 10.0F;
b.x = 10.0F;
b.y = 10.5F;
b.z = 10.0F;
CHECK_TRUE(SC_SACD_Sphere_Collision(&a, &b));
b.y = 12.0F;
b.z = 12.0F;
CHECK_FALSE(SC_SACD_Sphere_Collision(&a, &b));
b.y = 11.0F;
b.z = 11.0F;
CHECK_TRUE(SC_SACD_Sphere_Collision(&a, &b));
}
// Test Sphere/AABB collision check.
{
SC_SACD_Sphere sphere{0.0F, 0.0F, 0.0F, 1.0F};
SC_SACD_AABB_Box box{0.0F, 0.0F, 0.0F, 2.0F, 2.0F, 2.0F};
CHECK_TRUE(SC_SACD_Sphere_AABB_Box_Collision(&sphere, &box));
sphere.x = 2.1F;
CHECK_FALSE(SC_SACD_Sphere_AABB_Box_Collision(&sphere, &box));
sphere.x = -2.1F;
CHECK_FALSE(SC_SACD_Sphere_AABB_Box_Collision(&sphere, &box));
sphere.x = 0.0F;
sphere.y = 2.1F;
CHECK_FALSE(SC_SACD_Sphere_AABB_Box_Collision(&sphere, &box));
sphere.y = -2.1F;
CHECK_FALSE(SC_SACD_Sphere_AABB_Box_Collision(&sphere, &box));
sphere.y = 0.0F;
sphere.z = 2.1F;
CHECK_FALSE(SC_SACD_Sphere_AABB_Box_Collision(&sphere, &box));
sphere.z = -2.1F;
CHECK_FALSE(SC_SACD_Sphere_AABB_Box_Collision(&sphere, &box));
sphere.z = 0.0F;
sphere.x = 1.0F;
sphere.y = 1.0F;
CHECK_TRUE(SC_SACD_Sphere_AABB_Box_Collision(&sphere, &box));
sphere.x = -1.0F;
sphere.y = -1.0F;
CHECK_TRUE(SC_SACD_Sphere_AABB_Box_Collision(&sphere, &box));
sphere.x = 2.0F;
sphere.y = 2.0F;
CHECK_FALSE(SC_SACD_Sphere_AABB_Box_Collision(&sphere, &box));
sphere.x = -2.0F;
sphere.y = -2.0F;
CHECK_FALSE(SC_SACD_Sphere_AABB_Box_Collision(&sphere, &box));
sphere.x = 1.0F;
sphere.y = 0.0F;
sphere.z = 1.0F;
CHECK_TRUE(SC_SACD_Sphere_AABB_Box_Collision(&sphere, &box));
sphere.x = -1.0F;
sphere.z = -1.0F;
CHECK_TRUE(SC_SACD_Sphere_AABB_Box_Collision(&sphere, &box));
sphere.x = 2.0F;
sphere.z = 2.0F;
CHECK_FALSE(SC_SACD_Sphere_AABB_Box_Collision(&sphere, &box));
sphere.x = -2.0F;
sphere.z = -2.0F;
CHECK_FALSE(SC_SACD_Sphere_AABB_Box_Collision(&sphere, &box));
sphere.x = 1.0F;
sphere.z = 1.5F;
CHECK_TRUE(SC_SACD_Sphere_AABB_Box_Collision(&sphere, &box));
sphere.x = 1.0F;
sphere.z = 1.8F;
CHECK_FALSE(SC_SACD_Sphere_AABB_Box_Collision(&sphere, &box));
}
std::cout << "Checks checked: " << checks_checked << '\n'
<< "Checks passed: " << checks_passed << '\n';