matrix operations
This commit is contained in:
parent
48e3b8e26c
commit
ad5ba21b4d
6 changed files with 1433 additions and 613 deletions
|
|
@ -18,207 +18,432 @@ Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
|||
*/
|
||||
|
||||
export module Crafter.Math:Intersection;
|
||||
import :Vector;
|
||||
import :VectorF32;
|
||||
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;
|
||||
// All intersection tests are batched over four primitives at a time so they
|
||||
// feed the VectorF32<3,1>::Dot / Cross / Length / Normalize four-pair
|
||||
// overloads directly. The single-primitive case is just "pass the same
|
||||
// primitive four times and read lane 0" - there is no single-vector
|
||||
// fast-path because the SIMD pipelines want full lanes.
|
||||
|
||||
Vector<T, 3, 0> h = Vector<T, 3, 0>::Cross(rayDir, edge2);
|
||||
T determinant = Vector<T, 3, 0>::Dot(edge1, h);
|
||||
// Möller-Trumbore against four triangles sharing one ray. Returns ray
|
||||
// parameter t per triangle, or float max where the ray misses.
|
||||
export inline VectorF32<1, 4> IntersectionTestRayTriangle(
|
||||
VectorF32<3, 1> rayOrigin, VectorF32<3, 1> rayDir,
|
||||
VectorF32<3, 1> aV0, VectorF32<3, 1> aV1, VectorF32<3, 1> aV2,
|
||||
VectorF32<3, 1> bV0, VectorF32<3, 1> bV1, VectorF32<3, 1> bV2,
|
||||
VectorF32<3, 1> cV0, VectorF32<3, 1> cV1, VectorF32<3, 1> cV2,
|
||||
VectorF32<3, 1> dV0, VectorF32<3, 1> dV1, VectorF32<3, 1> dV2
|
||||
) {
|
||||
VectorF32<3, 1> aE1 = aV1 - aV0;
|
||||
VectorF32<3, 1> aE2 = aV2 - aV0;
|
||||
VectorF32<3, 1> bE1 = bV1 - bV0;
|
||||
VectorF32<3, 1> bE2 = bV2 - bV0;
|
||||
VectorF32<3, 1> cE1 = cV1 - cV0;
|
||||
VectorF32<3, 1> cE2 = cV2 - cV0;
|
||||
VectorF32<3, 1> dE1 = dV1 - dV0;
|
||||
VectorF32<3, 1> dE2 = dV2 - dV0;
|
||||
|
||||
if (determinant <= std::numeric_limits<T>::epsilon()) {
|
||||
return std::numeric_limits<T>::max();
|
||||
VectorF32<3, 1> aH = VectorF32<3, 1>::Cross(rayDir, aE2);
|
||||
VectorF32<3, 1> bH = VectorF32<3, 1>::Cross(rayDir, bE2);
|
||||
VectorF32<3, 1> cH = VectorF32<3, 1>::Cross(rayDir, cE2);
|
||||
VectorF32<3, 1> dH = VectorF32<3, 1>::Cross(rayDir, dE2);
|
||||
|
||||
VectorF32<3, 1> aS = rayOrigin - aV0;
|
||||
VectorF32<3, 1> bS = rayOrigin - bV0;
|
||||
VectorF32<3, 1> cS = rayOrigin - cV0;
|
||||
VectorF32<3, 1> dS = rayOrigin - dV0;
|
||||
|
||||
VectorF32<3, 1> aQ = VectorF32<3, 1>::Cross(aS, aE1);
|
||||
VectorF32<3, 1> bQ = VectorF32<3, 1>::Cross(bS, bE1);
|
||||
VectorF32<3, 1> cQ = VectorF32<3, 1>::Cross(cS, cE1);
|
||||
VectorF32<3, 1> dQ = VectorF32<3, 1>::Cross(dS, dE1);
|
||||
|
||||
// Four 3-component dots packed into one __m128 per call.
|
||||
VectorF32<1, 4> det = VectorF32<3, 1>::Dot(
|
||||
aE1, aH, bE1, bH, cE1, cH, dE1, dH);
|
||||
VectorF32<1, 4> uNum = VectorF32<3, 1>::Dot(
|
||||
aS, aH, bS, bH, cS, cH, dS, dH);
|
||||
VectorF32<1, 4> vNum = VectorF32<3, 1>::Dot(
|
||||
rayDir, aQ, rayDir, bQ, rayDir, cQ, rayDir, dQ);
|
||||
VectorF32<1, 4> tNum = VectorF32<3, 1>::Dot(
|
||||
aE2, aQ, bE2, bQ, cE2, cQ, dE2, dQ);
|
||||
|
||||
std::array<float, 4> detArr = det.template Store<float>();
|
||||
std::array<float, 4> uArr = uNum.template Store<float>();
|
||||
std::array<float, 4> vArr = vNum.template Store<float>();
|
||||
std::array<float, 4> tArr = tNum.template Store<float>();
|
||||
|
||||
constexpr float eps = std::numeric_limits<float>::epsilon();
|
||||
constexpr float maxF = std::numeric_limits<float>::max();
|
||||
alignas(16) std::array<float, 4> out{};
|
||||
for (std::uint8_t i = 0; i < 4; ++i) {
|
||||
float d = detArr[i];
|
||||
if (d <= eps) { out[i] = maxF; continue; }
|
||||
float invD = 1.0f / d;
|
||||
float u = uArr[i] * invD;
|
||||
if (u < 0.0f || u > 1.0f) { out[i] = maxF; continue; }
|
||||
float v = vArr[i] * invD;
|
||||
if (v < 0.0f || u + v > 1.0f) { out[i] = maxF; continue; }
|
||||
out[i] = tArr[i] * invD;
|
||||
}
|
||||
|
||||
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);
|
||||
return VectorF32<1, 4>(out.data());
|
||||
}
|
||||
|
||||
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;
|
||||
// One ray against four spheres. radii must hold {rA, rB, rC, rD} in lanes
|
||||
// 0..3.
|
||||
export inline VectorF32<1, 4> IntersectionTestRaySphere(
|
||||
VectorF32<3, 1> rayOrigin, VectorF32<3, 1> rayDir,
|
||||
VectorF32<3, 1> posA, VectorF32<3, 1> posB,
|
||||
VectorF32<3, 1> posC, VectorF32<3, 1> posD,
|
||||
VectorF32<1, 4> radii
|
||||
) {
|
||||
VectorF32<3, 1> sA = rayOrigin - posA;
|
||||
VectorF32<3, 1> sB = rayOrigin - posB;
|
||||
VectorF32<3, 1> sC = rayOrigin - posC;
|
||||
VectorF32<3, 1> sD = rayOrigin - posD;
|
||||
|
||||
if (d < 0) {
|
||||
return std::numeric_limits<T>::max();
|
||||
}
|
||||
// dirDotS_i = rayDir · (rayOrigin - pos_i)
|
||||
VectorF32<1, 4> dirDotS = VectorF32<3, 1>::Dot(
|
||||
rayDir, sA, rayDir, sB, rayDir, sC, rayDir, sD);
|
||||
// sqDist_i = |rayOrigin - pos_i|² (a.k.a. LengthSq of the s vectors)
|
||||
VectorF32<1, 4> sqDist = VectorF32<3, 1>::LengthSq(sA, sB, sC, sD);
|
||||
// aScalar = rayDir · rayDir, broadcast across four lanes.
|
||||
VectorF32<1, 4> aScalar = VectorF32<3, 1>::LengthSq(
|
||||
rayDir, rayDir, rayDir, rayDir);
|
||||
|
||||
d = std::sqrt(d);
|
||||
VectorF32<1, 4> two(2.0f);
|
||||
VectorF32<1, 4> four(4.0f);
|
||||
VectorF32<1, 4> b = two * dirDotS;
|
||||
VectorF32<1, 4> c = sqDist - radii * radii;
|
||||
// discriminant = b² - 4·a·c
|
||||
VectorF32<1, 4> disc = b * b - four * aScalar * c;
|
||||
|
||||
T t = (T(-0.5)) * (b + d) / a;
|
||||
if (t > T(0)) {
|
||||
return t;
|
||||
} else {
|
||||
return std::numeric_limits<T>::max();
|
||||
std::array<float, 4> discArr = disc.template Store<float>();
|
||||
std::array<float, 4> bArr = b.template Store<float>();
|
||||
std::array<float, 4> aArr = aScalar.template Store<float>();
|
||||
|
||||
constexpr float maxF = std::numeric_limits<float>::max();
|
||||
alignas(16) std::array<float, 4> out{};
|
||||
for (std::uint8_t i = 0; i < 4; ++i) {
|
||||
float d = discArr[i];
|
||||
if (d < 0.0f) { out[i] = maxF; continue; }
|
||||
float sqrtD = std::sqrt(d);
|
||||
float t = -0.5f * (bArr[i] + sqrtD) / aArr[i];
|
||||
out[i] = (t > 0.0f) ? t : maxF;
|
||||
}
|
||||
return VectorF32<1, 4>(out.data());
|
||||
}
|
||||
|
||||
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
|
||||
);
|
||||
// One ray against four OBBs. Each box is described by world-space position,
|
||||
// half-extent vector (per-axis sizes), and a unit quaternion rotation.
|
||||
export inline VectorF32<1, 4> IntersectionTestRayOrientedBox(
|
||||
VectorF32<3, 1> rayOrigin, VectorF32<3, 1> rayDir,
|
||||
VectorF32<3, 1> posA, VectorF32<3, 1> sizeA, VectorF32<4, 1> rotA,
|
||||
VectorF32<3, 1> posB, VectorF32<3, 1> sizeB, VectorF32<4, 1> rotB,
|
||||
VectorF32<3, 1> posC, VectorF32<3, 1> sizeC, VectorF32<4, 1> rotC,
|
||||
VectorF32<3, 1> posD, VectorF32<3, 1> sizeD, VectorF32<4, 1> rotD
|
||||
) {
|
||||
// Conjugate quaternion: negate xyz, keep w. Negate<{true,true,true,false}>
|
||||
// is constant-folded into a single XOR with a mask vector.
|
||||
VectorF32<4, 1> invRotA = rotA.template Negate<{{true, true, true, false}}>();
|
||||
VectorF32<4, 1> invRotB = rotB.template Negate<{{true, true, true, false}}>();
|
||||
VectorF32<4, 1> invRotC = rotC.template Negate<{{true, true, true, false}}>();
|
||||
VectorF32<4, 1> invRotD = rotD.template Negate<{{true, true, true, false}}>();
|
||||
|
||||
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);
|
||||
VectorF32<3, 1> localOriginA = VectorF32<3, 1>::Rotate(rayOrigin - posA, invRotA);
|
||||
VectorF32<3, 1> localOriginB = VectorF32<3, 1>::Rotate(rayOrigin - posB, invRotB);
|
||||
VectorF32<3, 1> localOriginC = VectorF32<3, 1>::Rotate(rayOrigin - posC, invRotC);
|
||||
VectorF32<3, 1> localOriginD = VectorF32<3, 1>::Rotate(rayOrigin - posD, invRotD);
|
||||
|
||||
Vector<T,3,0> halfExtents = boxSize * T(0.5);
|
||||
VectorF32<3, 1> localDirA = VectorF32<3, 1>::Rotate(rayDir, invRotA);
|
||||
VectorF32<3, 1> localDirB = VectorF32<3, 1>::Rotate(rayDir, invRotB);
|
||||
VectorF32<3, 1> localDirC = VectorF32<3, 1>::Rotate(rayDir, invRotC);
|
||||
VectorF32<3, 1> localDirD = VectorF32<3, 1>::Rotate(rayDir, invRotD);
|
||||
|
||||
T tMin = T(0);
|
||||
T tMax = std::numeric_limits<T>::max();
|
||||
VectorF32<3, 1> halfA = sizeA * 0.5f;
|
||||
VectorF32<3, 1> halfB = sizeB * 0.5f;
|
||||
VectorF32<3, 1> halfC = sizeC * 0.5f;
|
||||
VectorF32<3, 1> halfD = sizeD * 0.5f;
|
||||
|
||||
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::array<std::array<float, 4>, 4> origLanes{
|
||||
localOriginA.template Store<float>(),
|
||||
localOriginB.template Store<float>(),
|
||||
localOriginC.template Store<float>(),
|
||||
localOriginD.template Store<float>(),
|
||||
};
|
||||
std::array<std::array<float, 4>, 4> dirLanes{
|
||||
localDirA.template Store<float>(),
|
||||
localDirB.template Store<float>(),
|
||||
localDirC.template Store<float>(),
|
||||
localDirD.template Store<float>(),
|
||||
};
|
||||
std::array<std::array<float, 4>, 4> halfLanes{
|
||||
halfA.template Store<float>(),
|
||||
halfB.template Store<float>(),
|
||||
halfC.template Store<float>(),
|
||||
halfD.template Store<float>(),
|
||||
};
|
||||
|
||||
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;
|
||||
constexpr float eps = std::numeric_limits<float>::epsilon();
|
||||
constexpr float maxF = std::numeric_limits<float>::max();
|
||||
alignas(16) std::array<float, 4> out{};
|
||||
for (std::uint8_t b = 0; b < 4; ++b) {
|
||||
float tMin = 0.0f;
|
||||
float tMax = maxF;
|
||||
bool miss = false;
|
||||
for (std::uint8_t i = 0; i < 3; ++i) {
|
||||
float d = dirLanes[b][i];
|
||||
float o = origLanes[b][i];
|
||||
float h = halfLanes[b][i];
|
||||
if (std::abs(d) < eps) {
|
||||
if (o < -h || o > h) { miss = true; break; }
|
||||
} else {
|
||||
float invD = 1.0f / d;
|
||||
float t1 = (-h - o) * invD;
|
||||
float t2 = ( h - o) * invD;
|
||||
if (t1 > t2) std::swap(t1, t2);
|
||||
tMin = std::max(tMin, t1);
|
||||
tMax = std::min(tMax, t2);
|
||||
if (tMin > tMax) { miss = true; break; }
|
||||
}
|
||||
}
|
||||
out[b] = miss ? maxF : (tMin >= 0.0f ? tMin : tMax);
|
||||
}
|
||||
return VectorF32<1, 4>(out.data());
|
||||
}
|
||||
|
||||
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;
|
||||
// One sphere against four OBBs. boxMatrix encodes rotation in m[r][0..2]
|
||||
// and translation in m[r][3].
|
||||
export inline VectorF32<1, 4> IntersectionTestSphereOrientedBox(
|
||||
VectorF32<3, 1> sphereCenter, VectorF32<1, 4> radii,
|
||||
VectorF32<3, 1> sizeA, MatrixRowMajor<float, 4, 3, 1> boxA,
|
||||
VectorF32<3, 1> sizeB, MatrixRowMajor<float, 4, 3, 1> boxB,
|
||||
VectorF32<3, 1> sizeC, MatrixRowMajor<float, 4, 3, 1> boxC,
|
||||
VectorF32<3, 1> sizeD, MatrixRowMajor<float, 4, 3, 1> boxD
|
||||
) {
|
||||
auto perBox = [&](MatrixRowMajor<float, 4, 3, 1> const& m,
|
||||
VectorF32<3, 1> const& size,
|
||||
VectorF32<3, 1>& xAxis,
|
||||
VectorF32<3, 1>& yAxis,
|
||||
VectorF32<3, 1>& zAxis,
|
||||
VectorF32<3, 1>& delta) {
|
||||
// Existing semantics: the OBB axes are read from the rows of the
|
||||
// upper 3x3 block, and the translation column is gathered from the
|
||||
// w lane of each row.
|
||||
std::array<float, 4> r0 = m.rows[0].template Store<float>();
|
||||
std::array<float, 4> r1 = m.rows[1].template Store<float>();
|
||||
std::array<float, 4> r2 = m.rows[2].template Store<float>();
|
||||
alignas(16) float xBuf[4] = { r0[0], r0[1], r0[2], 0.0f };
|
||||
alignas(16) float yBuf[4] = { r1[0], r1[1], r1[2], 0.0f };
|
||||
alignas(16) float zBuf[4] = { r2[0], r2[1], r2[2], 0.0f };
|
||||
alignas(16) float oBuf[4] = { r0[3], r1[3], r2[3], 0.0f };
|
||||
xAxis = VectorF32<3, 1>(xBuf);
|
||||
yAxis = VectorF32<3, 1>(yBuf);
|
||||
zAxis = VectorF32<3, 1>(zBuf);
|
||||
VectorF32<3, 1> origin(oBuf);
|
||||
delta = sphereCenter - origin;
|
||||
(void)size;
|
||||
};
|
||||
|
||||
std::vector<Vector<T, 3, 0>> box1Corners = getOBBCorners(sizeA, boxA);
|
||||
std::vector<Vector<T, 3, 0>> box2Corners = getOBBCorners(sizeB, boxB);
|
||||
VectorF32<3, 1> xA, yA, zA, dA;
|
||||
VectorF32<3, 1> xB, yB, zB, dB;
|
||||
VectorF32<3, 1> xC, yC, zC, dC;
|
||||
VectorF32<3, 1> xD, yD, zD, dD;
|
||||
perBox(boxA, sizeA, xA, yA, zA, dA);
|
||||
perBox(boxB, sizeB, xB, yB, zB, dB);
|
||||
perBox(boxC, sizeC, xC, yC, zC, dC);
|
||||
perBox(boxD, sizeD, xD, yD, zD, dD);
|
||||
|
||||
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]))));
|
||||
// Local sphere center per box: project delta onto each box axis. We
|
||||
// produce {lx, ly, lz, lx, ly, lz, lx, ly, lz, lx, ly, lz} as three
|
||||
// packed 4-wide Dot results (one Dot per axis).
|
||||
VectorF32<1, 4> locX = VectorF32<3, 1>::Dot(
|
||||
dA, xA, dB, xB, dC, xC, dD, xD);
|
||||
VectorF32<1, 4> locY = VectorF32<3, 1>::Dot(
|
||||
dA, yA, dB, yB, dC, yC, dD, yD);
|
||||
VectorF32<1, 4> locZ = VectorF32<3, 1>::Dot(
|
||||
dA, zA, dB, zB, dC, zC, dD, zD);
|
||||
|
||||
std::array<float, 4> lxArr = locX.template Store<float>();
|
||||
std::array<float, 4> lyArr = locY.template Store<float>();
|
||||
std::array<float, 4> lzArr = locZ.template Store<float>();
|
||||
std::array<float, 4> rArr = radii.template Store<float>();
|
||||
std::array<std::array<float, 4>, 4> sizeLanes{
|
||||
sizeA.template Store<float>(),
|
||||
sizeB.template Store<float>(),
|
||||
sizeC.template Store<float>(),
|
||||
sizeD.template Store<float>(),
|
||||
};
|
||||
|
||||
alignas(16) std::array<float, 4> out{};
|
||||
for (std::uint8_t i = 0; i < 4; ++i) {
|
||||
float lx = lxArr[i], ly = lyArr[i], lz = lzArr[i];
|
||||
float sx = sizeLanes[i][0], sy = sizeLanes[i][1], sz = sizeLanes[i][2];
|
||||
float cx = std::clamp(lx, -sx, sx);
|
||||
float cy = std::clamp(ly, -sy, sy);
|
||||
float cz = std::clamp(lz, -sz, sz);
|
||||
float dx = lx - cx, dy = ly - cy, dz = lz - cz;
|
||||
float distSq = dx * dx + dy * dy + dz * dz;
|
||||
float r = rArr[i];
|
||||
// Returns 0.0 on hit, max on miss - keeps a consistent
|
||||
// "t-like" output signature with the other intersection tests.
|
||||
out[i] = (distSq <= r * r) ? 0.0f : std::numeric_limits<float>::max();
|
||||
}
|
||||
return VectorF32<1, 4>(out.data());
|
||||
}
|
||||
|
||||
// Eight local corners of a unit OBB transformed by `matrix`. Uses one
|
||||
// batched 4-pair Dot per result row (x, y, z), reproducing the corners in
|
||||
// two groups of four.
|
||||
export inline std::array<VectorF32<3, 1>, 8> GetOBBCorners(
|
||||
VectorF32<3, 1> size, MatrixRowMajor<float, 4, 3, 1> matrix
|
||||
) {
|
||||
std::array<float, 4> sz = size.template Store<float>();
|
||||
const float sx = sz[0], sy = sz[1], sz_ = sz[2];
|
||||
|
||||
VectorF32<4, 1> mx = matrix.rows[0];
|
||||
VectorF32<4, 1> my = matrix.rows[1];
|
||||
VectorF32<4, 1> mz = matrix.rows[2];
|
||||
|
||||
// Eight homogeneous corner vectors with w=1 so the translation column
|
||||
// of `matrix` participates in the dot product.
|
||||
alignas(16) float c0[4] = { -sx, -sy, -sz_, 1.0f };
|
||||
alignas(16) float c1[4] = { sx, -sy, -sz_, 1.0f };
|
||||
alignas(16) float c2[4] = { -sx, sy, -sz_, 1.0f };
|
||||
alignas(16) float c3[4] = { sx, sy, -sz_, 1.0f };
|
||||
alignas(16) float c4[4] = { -sx, -sy, sz_, 1.0f };
|
||||
alignas(16) float c5[4] = { sx, -sy, sz_, 1.0f };
|
||||
alignas(16) float c6[4] = { -sx, sy, sz_, 1.0f };
|
||||
alignas(16) float c7[4] = { sx, sy, sz_, 1.0f };
|
||||
VectorF32<4, 1> v0(c0), v1(c1), v2(c2), v3(c3);
|
||||
VectorF32<4, 1> v4(c4), v5(c5), v6(c6), v7(c7);
|
||||
|
||||
// First four corners (0..3): batch x, y, z dots.
|
||||
VectorF32<1, 4> xLo = VectorF32<4, 1>::Dot(mx, v0, mx, v1, mx, v2, mx, v3);
|
||||
VectorF32<1, 4> yLo = VectorF32<4, 1>::Dot(my, v0, my, v1, my, v2, my, v3);
|
||||
VectorF32<1, 4> zLo = VectorF32<4, 1>::Dot(mz, v0, mz, v1, mz, v2, mz, v3);
|
||||
// Second four corners (4..7).
|
||||
VectorF32<1, 4> xHi = VectorF32<4, 1>::Dot(mx, v4, mx, v5, mx, v6, mx, v7);
|
||||
VectorF32<1, 4> yHi = VectorF32<4, 1>::Dot(my, v4, my, v5, my, v6, my, v7);
|
||||
VectorF32<1, 4> zHi = VectorF32<4, 1>::Dot(mz, v4, mz, v5, mz, v6, mz, v7);
|
||||
|
||||
std::array<float, 4> xLoA = xLo.template Store<float>();
|
||||
std::array<float, 4> yLoA = yLo.template Store<float>();
|
||||
std::array<float, 4> zLoA = zLo.template Store<float>();
|
||||
std::array<float, 4> xHiA = xHi.template Store<float>();
|
||||
std::array<float, 4> yHiA = yHi.template Store<float>();
|
||||
std::array<float, 4> zHiA = zHi.template Store<float>();
|
||||
|
||||
std::array<VectorF32<3, 1>, 8> result;
|
||||
for (std::uint8_t i = 0; i < 4; ++i) {
|
||||
alignas(16) float buf[4] = { xLoA[i], yLoA[i], zLoA[i], 0.0f };
|
||||
result[i] = VectorF32<3, 1>(buf);
|
||||
}
|
||||
for (std::uint8_t i = 0; i < 4; ++i) {
|
||||
alignas(16) float buf[4] = { xHiA[i], yHiA[i], zHiA[i], 0.0f };
|
||||
result[4 + i] = VectorF32<3, 1>(buf);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
// SAT against fifteen separating axes (3 box-A, 3 box-B, 9 cross products).
|
||||
// We compute every corner projection with batched 4-pair Dots: each axis
|
||||
// projects four corners per call, two calls per axis covers the 8 corners.
|
||||
export inline bool IntersectionTestOrientedBoxOrientedBox(
|
||||
VectorF32<3, 1> sizeA, MatrixRowMajor<float, 4, 3, 1> boxA,
|
||||
VectorF32<3, 1> sizeB, MatrixRowMajor<float, 4, 3, 1> boxB
|
||||
) {
|
||||
std::array<VectorF32<3, 1>, 8> cornersA = GetOBBCorners(sizeA, boxA);
|
||||
std::array<VectorF32<3, 1>, 8> cornersB = GetOBBCorners(sizeB, boxB);
|
||||
|
||||
// Axes are the upper-3 lanes of each matrix row (same convention as
|
||||
// SphereOrientedBox). ExtractLo<3> just retypes the SIMD register; the
|
||||
// 4th lane is ignored by the Len=3 ops below.
|
||||
std::array<VectorF32<3, 1>, 3> axesA = {
|
||||
boxA.rows[0].template ExtractLo<3>(),
|
||||
boxA.rows[1].template ExtractLo<3>(),
|
||||
boxA.rows[2].template ExtractLo<3>(),
|
||||
};
|
||||
std::array<VectorF32<3, 1>, 3> axesB = {
|
||||
boxB.rows[0].template ExtractLo<3>(),
|
||||
boxB.rows[1].template ExtractLo<3>(),
|
||||
boxB.rows[2].template ExtractLo<3>(),
|
||||
};
|
||||
|
||||
std::array<VectorF32<3, 1>, 15> axes{};
|
||||
axes[0] = axesA[0]; axes[1] = axesA[1]; axes[2] = axesA[2];
|
||||
axes[3] = axesB[0]; axes[4] = axesB[1]; axes[5] = axesB[2];
|
||||
// Normalize all nine cross axes together with a single batched
|
||||
// Normalize call (Packing=3 not in the API, so two calls of four +
|
||||
// one of one would be needed; for now just normalize in two batches
|
||||
// of four and the trailing one inline).
|
||||
std::array<VectorF32<3, 1>, 9> crossAxes{};
|
||||
std::uint8_t k = 0;
|
||||
for (std::uint8_t i = 0; i < 3; ++i) {
|
||||
for (std::uint8_t j = 0; j < 3; ++j) {
|
||||
crossAxes[k++] = VectorF32<3, 1>::Cross(axesA[i], axesB[j]);
|
||||
}
|
||||
}
|
||||
auto norm0 = VectorF32<3, 1>::Normalize(crossAxes[0], crossAxes[1], crossAxes[2], crossAxes[3]);
|
||||
auto norm1 = VectorF32<3, 1>::Normalize(crossAxes[4], crossAxes[5], crossAxes[6], crossAxes[7]);
|
||||
auto norm2 = VectorF32<3, 1>::Normalize(crossAxes[8], crossAxes[8], crossAxes[8], crossAxes[8]);
|
||||
axes[6] = std::get<0>(norm0);
|
||||
axes[7] = std::get<1>(norm0);
|
||||
axes[8] = std::get<2>(norm0);
|
||||
axes[9] = std::get<3>(norm0);
|
||||
axes[10] = std::get<0>(norm1);
|
||||
axes[11] = std::get<1>(norm1);
|
||||
axes[12] = std::get<2>(norm1);
|
||||
axes[13] = std::get<3>(norm1);
|
||||
axes[14] = std::get<0>(norm2);
|
||||
|
||||
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);
|
||||
for (std::uint8_t axisIdx = 0; axisIdx < 15; ++axisIdx) {
|
||||
VectorF32<3, 1> axis = axes[axisIdx];
|
||||
// Project all 8 corners of each box onto `axis` using two batched
|
||||
// 4-pair Dot calls (lo and hi corners).
|
||||
VectorF32<1, 4> projA_lo = VectorF32<3, 1>::Dot(
|
||||
cornersA[0], axis, cornersA[1], axis,
|
||||
cornersA[2], axis, cornersA[3], axis);
|
||||
VectorF32<1, 4> projA_hi = VectorF32<3, 1>::Dot(
|
||||
cornersA[4], axis, cornersA[5], axis,
|
||||
cornersA[6], axis, cornersA[7], axis);
|
||||
VectorF32<1, 4> projB_lo = VectorF32<3, 1>::Dot(
|
||||
cornersB[0], axis, cornersB[1], axis,
|
||||
cornersB[2], axis, cornersB[3], axis);
|
||||
VectorF32<1, 4> projB_hi = VectorF32<3, 1>::Dot(
|
||||
cornersB[4], axis, cornersB[5], axis,
|
||||
cornersB[6], axis, cornersB[7], axis);
|
||||
|
||||
std::array<float, 4> aLo = projA_lo.template Store<float>();
|
||||
std::array<float, 4> aHi = projA_hi.template Store<float>();
|
||||
std::array<float, 4> bLo = projB_lo.template Store<float>();
|
||||
std::array<float, 4> bHi = projB_hi.template Store<float>();
|
||||
|
||||
float minA = aLo[0], maxA = aLo[0];
|
||||
for (std::uint8_t i = 1; i < 4; ++i) {
|
||||
minA = std::min(minA, aLo[i]);
|
||||
maxA = std::max(maxA, aLo[i]);
|
||||
}
|
||||
for (std::uint8_t i = 0; i < 4; ++i) {
|
||||
minA = std::min(minA, aHi[i]);
|
||||
maxA = std::max(maxA, aHi[i]);
|
||||
}
|
||||
float minB = bLo[0], maxB = bLo[0];
|
||||
for (std::uint8_t i = 1; i < 4; ++i) {
|
||||
minB = std::min(minB, bLo[i]);
|
||||
maxB = std::max(maxB, bLo[i]);
|
||||
}
|
||||
for (std::uint8_t i = 0; i < 4; ++i) {
|
||||
minB = std::min(minB, bHi[i]);
|
||||
maxB = std::max(maxB, bHi[i]);
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
if (maxA < minB || maxB < minA) 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[0][3], boxMatrix.m[1][3], boxMatrix.m[2][3]), xAxis),
|
||||
Vector<T, 3, 0>::Dot(sphereCenter - Vector<T, 3, 0>(boxMatrix.m[0][3], boxMatrix.m[1][3], boxMatrix.m[2][3]), yAxis),
|
||||
Vector<T, 3, 0>::Dot(sphereCenter - Vector<T, 3, 0>(boxMatrix.m[0][3], boxMatrix.m[1][3], boxMatrix.m[2][3]), 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue