237 lines
No EOL
9 KiB
C++
Executable file
237 lines
No EOL
9 KiB
C++
Executable file
/*
|
|
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 IntersectionTestSphereOrientatedBox(Vector<T, 3, 0> spherePos, T sphereRadius, Vector<T, 3, 0> boxSize, MatrixRowMajor<T, 4, 3, 1> boxMat) {
|
|
Vector<T, 3, 0> d = spherePos - Vector<T, 3, 0>(boxMat.m[0][3], boxMat.m[1][3], boxMat.m[2][3]);
|
|
|
|
T distSq = T(0);
|
|
|
|
for (std::uint32_t i = 0; i < 3; ++i)
|
|
{
|
|
Vector<T, 3, 0> axis(boxMat.m[0][i], boxMat.m[1][i], boxMat.m[2][i]);
|
|
T dist = Vector<T, 3, 0>::Dot(d, axis);
|
|
T excess = std::fabs(dist) - boxSize.v[i];
|
|
excess = std::max(excess, T(0));
|
|
distSq += excess * excess;
|
|
}
|
|
|
|
// Check sphere axes (world axes)
|
|
for (std::uint32_t i = 0; i < 3; ++i) {
|
|
Vector<T, 3, 0> axis = Vector<T, 3, 0>(0,0,0);
|
|
axis.v[i] = T(1);
|
|
T dist = Vector<T, 3, 0>::Dot(d, axis);
|
|
T excess = std::abs(dist) - sphereRadius;
|
|
excess = std::max(excess, T(0));
|
|
distSq += excess * excess;
|
|
}
|
|
|
|
// Check cross product axes
|
|
for (std::uint32_t i = 0; i < 3; ++i) {
|
|
for (std::uint32_t j = 0; j < 3; ++j) {
|
|
Vector<T, 3, 0> boxAxis(boxMat.m[0][i], boxMat.m[1][i], boxMat.m[2][i]);
|
|
Vector<T, 3, 0> sphereAxis = Vector<T, 3, 0>::Zero();
|
|
sphereAxis.v[j] = T(1);
|
|
Vector<T, 3, 0> axis = Vector<T, 3, 0>::Cross(boxAxis, sphereAxis);
|
|
T dist = Vector<T, 3, 0>::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;
|
|
}
|
|
} |