Impl Sphere/Sphere and Sphere/AABB collision

TODO: Impl Sphere/Generic-Box collision

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_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;
+}

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

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';