diff --git a/interfaces/Crafter.Math-Intersection.cppm b/interfaces/Crafter.Math-Intersection.cppm index 260526e..9462d11 100755 --- a/interfaces/Crafter.Math-Intersection.cppm +++ b/interfaces/Crafter.Math-Intersection.cppm @@ -194,44 +194,31 @@ namespace Crafter { } export template - constexpr bool IntersectionTestSphereOrientatedBox(Vector spherePos, T sphereRadius, Vector boxSize, MatrixRowMajor boxMat) { - Vector d = spherePos - Vector(boxMat.m[0][3], boxMat.m[1][3], boxMat.m[2][3]); + constexpr bool IntersectionTestSphereOrientedBox(Vector sphereCenter, T sphereRadius, Vector boxSize, MatrixRowMajor boxMatrix) { + // Extract the OBB's axes (columns of the rotation matrix) + Vector xAxis(boxMatrix.m[0][0], boxMatrix.m[0][1], boxMatrix.m[0][2]); + Vector yAxis(boxMatrix.m[1][0], boxMatrix.m[1][1], boxMatrix.m[1][2]); + Vector zAxis(boxMatrix.m[2][0], boxMatrix.m[2][1], boxMatrix.m[2][2]); - T distSq = T(0); + // Translate the sphere center into the OBB's local space + Vector localCenter = Vector( + Vector::Dot(sphereCenter - Vector(boxMatrix.m[3][0], boxMatrix.m[3][1], boxMatrix.m[3][2]), xAxis), + Vector::Dot(sphereCenter - Vector(boxMatrix.m[3][0], boxMatrix.m[3][1], boxMatrix.m[3][2]), yAxis), + Vector::Dot(sphereCenter - Vector(boxMatrix.m[3][0], boxMatrix.m[3][1], boxMatrix.m[3][2]), zAxis) + ); - for (std::uint32_t i = 0; i < 3; ++i) - { - Vector axis(boxMat.m[0][i], boxMat.m[1][i], boxMat.m[2][i]); - T dist = Vector::Dot(d, axis); - T excess = std::fabs(dist) - boxSize.v[i]; - excess = std::max(excess, T(0)); - distSq += excess * excess; - } + // Clamp the local center to the OBB's extents + Vector closestPoint = Vector( + std::max(-boxSize.x, std::min(localCenter.x, boxSize.x)), + std::max(-boxSize.y, std::min(localCenter.y, boxSize.y)), + std::max(-boxSize.z, std::min(localCenter.z, boxSize.z)) + ); - // Check sphere axes (world axes) - for (std::uint32_t i = 0; i < 3; ++i) { - Vector axis = Vector(0,0,0); - axis.v[i] = T(1); - T dist = Vector::Dot(d, axis); - T excess = std::abs(dist) - sphereRadius; - excess = std::max(excess, T(0)); - distSq += excess * excess; - } + // Calculate the distance between the closest point and the local center + Vector delta = localCenter - closestPoint; + T distanceSquared = Vector::Dot(delta, delta); - // Check cross product axes - for (std::uint32_t i = 0; i < 3; ++i) { - for (std::uint32_t j = 0; j < 3; ++j) { - Vector boxAxis(boxMat.m[0][i], boxMat.m[1][i], boxMat.m[2][i]); - Vector sphereAxis = Vector(0,0,0); - sphereAxis.v[j] = T(1); - Vector axis = Vector::Cross(boxAxis, sphereAxis); - T dist = Vector::Dot(d, axis); - T excess = std::abs(dist) - (boxSize.v[(i+1)%3] * sphereRadius + boxSize.v[(i+2)%3] * sphereRadius); - excess = std::max(excess, T(0)); - distSq += excess * excess; - } - } - - return distSq <= sphereRadius * sphereRadius; + // Check if the distance is less than or equal to the sphere's radius squared + return distanceSquared <= (sphereRadius * sphereRadius); } } \ No newline at end of file