From cc4e67ace18114763841241f9ea45f55d1f01454 Mon Sep 17 00:00:00 2001 From: Stephen Seo Date: Fri, 26 Apr 2024 17:36:57 +0900 Subject: [PATCH] Impl Sphere/Sphere and Sphere/AABB collision TODO: Impl Sphere/Generic-Box collision --- src/sc_sacd.cpp | 166 ++++++++++++++++++++++++++++++++++++++++++++++++ src/sc_sacd.h | 24 +++++++ src/test.cpp | 91 ++++++++++++++++++++++++++ 3 files changed, 281 insertions(+) diff --git a/src/sc_sacd.cpp b/src/sc_sacd.cpp index f97bb3c..a5ab6b1 100644 --- a/src/sc_sacd.cpp +++ b/src/sc_sacd.cpp @@ -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_Get_Box_Normals( const SC_SACD_Generic_Box *box) { std::vector 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 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; +} diff --git a/src/sc_sacd.h b/src/sc_sacd.h index 4893046..38e1f0a 100644 --- a/src/sc_sacd.h +++ b/src/sc_sacd.h @@ -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 diff --git a/src/test.cpp b/src/test.cpp index ea09c49..1c191c6 100644 --- a/src/test.cpp +++ b/src/test.cpp @@ -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';