Impl Sphere/Sphere and Sphere/AABB collision
TODO: Impl Sphere/Generic-Box collision
This commit is contained in:
parent
6f641f13be
commit
919d931b4e
3 changed files with 281 additions and 0 deletions
166
src/sc_sacd.cpp
166
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;
|
||||
}
|
||||
|
|
|
@ -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
91
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';
|
||||
|
||||
|
|
Loading…
Reference in a new issue