Catcrafts/Crafter.Math
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions
Crafter.Math/interfaces/Crafter.Math-Intersection.cppm

224 lines
No EOL
8.9 KiB
C++
Executable file
Raw Blame History

/*
Crafter®.Math
Copyright (C) 2026 Catcrafts®
catcrafts.net
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License version 3.0 as published by the Free Software Foundation;
This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*/
export module Crafter.Math:Intersection;
import :Vector;
import :MatrixRowMajor;
import std;
namespace Crafter {
export template<typename T>
constexpr T IntersectionTestRayTriangle(Vector<T, 3, 0> vert0, Vector<T, 3, 0> vert1, Vector<T, 3, 0> vert2, Vector<T, 3, 0> rayOrigin, Vector<T, 3, 0> rayDir) {
Vector<T, 3, 0> edge1 = vert1 - vert0;
Vector<T, 3, 0> edge2 = vert2 - vert0;
Vector<T, 3, 0> h = Vector<T, 3, 0>::Cross(rayDir, edge2);
T determinant = Vector<T, 3, 0>::Dot(edge1, h);
if (determinant <= std::numeric_limits<T>::epsilon()) {
return std::numeric_limits<T>::max();
}
T inverse_determinant = T(1) / determinant;
Vector<T, 3, 0> origins_diff_vector = rayOrigin - vert0;
T u = Vector<T, 3, 0>::Dot(origins_diff_vector, h) * inverse_determinant;
if (u < 0.0 || u > 1.0)
{
return std::numeric_limits<T>::max();
}
Vector<T, 3, 0> q = Vector<T, 3, 0>::Cross(origins_diff_vector, edge1);
T v = inverse_determinant * Vector<T, 3, 0>::Dot(rayDir, q);
if (v < 0.0 || u + v > 1.0) {
return std::numeric_limits<T>::max();
}
return inverse_determinant * Vector<T, 3, 0>::Dot(edge2, q);
}
export template<typename T>
constexpr T IntersectionTestRaySphere(Vector<T, 3, 0> position, T radius, Vector<T, 3, 0> rayOrigin, Vector<T, 3, 0> rayDir) {
T a = Vector<T, 3, 0>::Dot(rayDir, rayDir);
T b = Vector<T, 3, 0>::Dot(rayDir, (T(2) * (rayOrigin - position)));
T c = Vector<T, 3, 0>::Dot(position, position) + Vector<T, 3, 0>::Dot(rayOrigin, rayOrigin) - T(2) * Vector<T, 3, 0>::Dot(rayOrigin, position) - radius * radius;
T d = b * b + (T(-4)) * a * c;
if (d < 0) {
return std::numeric_limits<T>::max();
}
d = std::sqrt(d);
T t = (T(-0.5)) * (b + d) / a;
if (t > T(0)) {
return t;
} else {
return std::numeric_limits<T>::max();
}
}
export template<typename T>
constexpr T IntersectionTestRayOrientedBox(Vector<T, 3, 0> boxPosition, Vector<T, 3, 0> boxSize, Vector<T, 4, 0> boxRotation, Vector<T, 3, 0> rayOrigin, Vector<T, 3, 0> rayDir) {
Vector<T, 4, 0> invRot(
-boxRotation.x,
-boxRotation.y,
-boxRotation.z,
boxRotation.w
);
Vector<T, 3, 0> localOrigin = Vector<T, 3, 0>::Rotate(rayOrigin - boxPosition, invRot);
Vector<T, 3, 0> localDir = Vector<T, 3, 0>::Rotate(rayDir, invRot);
Vector<T,3,0> halfExtents = boxSize * T(0.5);
T tMin = T(0);
T tMax = std::numeric_limits<T>::max();
for (std::uint32_t i = 0; i < 3; ++i)
{
if (std::abs(localDir.v[i]) < std::numeric_limits<T>::epsilon())
{
if (localOrigin.v[i] < -halfExtents.v[i] || localOrigin.v[i] > halfExtents.v[i]) {
return std::numeric_limits<T>::max();
}
}
else
{
T invD = T(1) / localDir.v[i];
T t1 = (-halfExtents.v[i] - localOrigin.v[i]) * invD;
T t2 = ( halfExtents.v[i] - localOrigin.v[i]) * invD;
if (t1 > t2) {
std::swap(t1, t2);
}
tMin = std::max(tMin, t1);
tMax = std::min(tMax, t2);
if (tMin > tMax) {
return std::numeric_limits<T>::max();
}
}
}
return (tMin >= T(0)) ? tMin : tMax;
}
export template<typename T>
std::vector<Vector<T, 3, 0>> getOBBCorners(Vector<T, 3, 0> size, MatrixRowMajor<T, 4, 3, 1> matrix) {
std::vector<Vector<T, 3, 0>> localCorners = {
Vector<T, 3, 0>(-size.x, -size.y, -size.z),
Vector<T, 3, 0>( size.x, -size.y, -size.z),
Vector<T, 3, 0>(-size.x, size.y, -size.z),
Vector<T, 3, 0>( size.x, size.y, -size.z),
Vector<T, 3, 0>(-size.x, -size.y, size.z),
Vector<T, 3, 0>( size.x, -size.y, size.z),
Vector<T, 3, 0>(-size.x, size.y, size.z),
Vector<T, 3, 0>( size.x, size.y, size.z)
};
std::vector<Vector<T, 3, 0>> worldCorners;
for (Vector<T, 3, 0> localCorner : localCorners) {
Vector<T, 3, 0> rotatedCorner = matrix * localCorner;
worldCorners.push_back(rotatedCorner);
}
return worldCorners;
}
export template<typename T>
constexpr bool IntersectionTestOrientedBoxOrientedBox(Vector<T, 3, 0> sizeA, MatrixRowMajor<T, 4, 3, 1> boxA, Vector<T, 3, 0> sizeB, MatrixRowMajor<T, 4, 3, 1> boxB) {
std::vector<Vector<T, 3, 0>> axes;
std::vector<Vector<T, 3, 0>> box1Corners = getOBBCorners(sizeA, boxA);
std::vector<Vector<T, 3, 0>> box2Corners = getOBBCorners(sizeB, boxB);
axes.push_back(Vector<T, 3, 0>(boxA.m[0][0], boxA.m[0][1], boxA.m[0][2]));
axes.push_back(Vector<T, 3, 0>(boxA.m[1][0], boxA.m[1][1], boxA.m[1][2]));
axes.push_back(Vector<T, 3, 0>(boxA.m[2][0], boxA.m[2][1], boxA.m[2][2]));
axes.push_back(Vector<T, 3, 0>(boxB.m[0][0], boxB.m[0][1], boxB.m[0][2]));
axes.push_back(Vector<T, 3, 0>(boxB.m[1][0], boxB.m[1][1], boxB.m[1][2]));
axes.push_back(Vector<T, 3, 0>(boxB.m[2][0], boxB.m[2][1], boxB.m[2][2]));
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
axes.push_back(Vector<T, 3, 0>::Normalize(Vector<T, 3, 0>::Cross(Vector<T, 3, 0>(boxA.m[i][0], boxA.m[i][1], boxA.m[i][2]), Vector<T, 3, 0>(boxB.m[j][0], boxB.m[j][1], boxB.m[j][2]))));
}
}
for (Vector<T, 3, 0> axis : axes) {
T min1 = Vector<T, 3, 0>::Dot(box1Corners[0], axis);
T max1 = min1;
for (Vector<T, 3, 0> corner : box1Corners) {
T projection = Vector<T, 3, 0>::Dot(corner, axis);
min1 = std::min(min1, projection);
max1 = std::max(max1, projection);
}
T min2 = Vector<T, 3, 0>::Dot(box2Corners[0], axis);
T max2 = min2;
for (Vector<T, 3, 0> corner : box2Corners) {
T projection = Vector<T, 3, 0>::Dot(corner, axis);
min2 = std::min(min2, projection);
max2 = std::max(max2, projection);
}
if (max1 < min2 || max2 < min1) {
return false;
}
}
return true;
}
export template<typename T>
constexpr bool IntersectionTestSphereOrientedBox(Vector<T, 3, 0> sphereCenter, T sphereRadius, Vector<T, 3, 0> boxSize, MatrixRowMajor<T, 4, 3, 1> boxMatrix) {
// Extract the OBB's axes (columns of the rotation matrix)
Vector<T, 3, 0> xAxis(boxMatrix.m[0][0], boxMatrix.m[0][1], boxMatrix.m[0][2]);
Vector<T, 3, 0> yAxis(boxMatrix.m[1][0], boxMatrix.m[1][1], boxMatrix.m[1][2]);
Vector<T, 3, 0> zAxis(boxMatrix.m[2][0], boxMatrix.m[2][1], boxMatrix.m[2][2]);
// Translate the sphere center into the OBB's local space
Vector<T, 3, 0> localCenter = Vector<T, 3, 0>(
Vector<T, 3, 0>::Dot(sphereCenter - Vector<T, 3, 0>(boxMatrix.m[3][0], boxMatrix.m[3][1], boxMatrix.m[3][2]), xAxis),
Vector<T, 3, 0>::Dot(sphereCenter - Vector<T, 3, 0>(boxMatrix.m[3][0], boxMatrix.m[3][1], boxMatrix.m[3][2]), yAxis),
Vector<T, 3, 0>::Dot(sphereCenter - Vector<T, 3, 0>(boxMatrix.m[3][0], boxMatrix.m[3][1], boxMatrix.m[3][2]), zAxis)
);
// Clamp the local center to the OBB's extents
Vector<T, 3, 0> closestPoint = Vector<T, 3, 0>(
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))
);
// Calculate the distance between the closest point and the local center
Vector<T, 3, 0> delta = localCenter - closestPoint;
T distanceSquared = Vector<T, 3, 0>::Dot(delta, delta);
// Check if the distance is less than or equal to the sphere's radius squared
return distanceSquared <= (sphereRadius * sphereRadius);
}
}
Reference in a new issue View git blame Copy permalink