JoltPhysics/Jolt/Geometry/AABox4.h

// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
// SPDX-License-Identifier: MIT

#pragma once

#include <Jolt/Geometry/OrientedBox.h>

JPH_NAMESPACE_BEGIN

/// Helper functions that process 4 axis aligned boxes at the same time using SIMD
/// Test if 4 bounding boxes overlap with 1 bounding box, splat 1 box
JPH_INLINE UVec4 AABox4VsBox(const AABox &inBox1, Vec4Arg inBox2MinX, Vec4Arg inBox2MinY, Vec4Arg inBox2MinZ, Vec4Arg inBox2MaxX, Vec4Arg inBox2MaxY, Vec4Arg inBox2MaxZ)
{
	// Splat values of box 1
	Vec4 box1_minx = inBox1.mMin.SplatX();
	Vec4 box1_miny = inBox1.mMin.SplatY();
	Vec4 box1_minz = inBox1.mMin.SplatZ();
	Vec4 box1_maxx = inBox1.mMax.SplatX();
	Vec4 box1_maxy = inBox1.mMax.SplatY();
	Vec4 box1_maxz = inBox1.mMax.SplatZ();

	// Test separation over each axis
	UVec4 nooverlapx = UVec4::sOr(Vec4::sGreater(box1_minx, inBox2MaxX), Vec4::sGreater(inBox2MinX, box1_maxx));
	UVec4 nooverlapy = UVec4::sOr(Vec4::sGreater(box1_miny, inBox2MaxY), Vec4::sGreater(inBox2MinY, box1_maxy));
	UVec4 nooverlapz = UVec4::sOr(Vec4::sGreater(box1_minz, inBox2MaxZ), Vec4::sGreater(inBox2MinZ, box1_maxz));

	// Return overlap
	return UVec4::sNot(UVec4::sOr(UVec4::sOr(nooverlapx, nooverlapy), nooverlapz));
}

/// Scale 4 axis aligned boxes
JPH_INLINE void AABox4Scale(Vec3Arg inScale, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ, Vec4 &outBoundsMinX, Vec4 &outBoundsMinY, Vec4 &outBoundsMinZ, Vec4 &outBoundsMaxX, Vec4 &outBoundsMaxY, Vec4 &outBoundsMaxZ)
{
	Vec4 scale_x = inScale.SplatX();
	Vec4 scaled_min_x = scale_x * inBoxMinX;
	Vec4 scaled_max_x = scale_x * inBoxMaxX;
	outBoundsMinX = Vec4::sMin(scaled_min_x, scaled_max_x); // Negative scale can flip min and max
	outBoundsMaxX = Vec4::sMax(scaled_min_x, scaled_max_x);

	Vec4 scale_y = inScale.SplatY();
	Vec4 scaled_min_y = scale_y * inBoxMinY;
	Vec4 scaled_max_y = scale_y * inBoxMaxY;
	outBoundsMinY = Vec4::sMin(scaled_min_y, scaled_max_y);
	outBoundsMaxY = Vec4::sMax(scaled_min_y, scaled_max_y);

	Vec4 scale_z = inScale.SplatZ();
	Vec4 scaled_min_z = scale_z * inBoxMinZ;
	Vec4 scaled_max_z = scale_z * inBoxMaxZ;
	outBoundsMinZ = Vec4::sMin(scaled_min_z, scaled_max_z);
	outBoundsMaxZ = Vec4::sMax(scaled_min_z, scaled_max_z);
}

/// Enlarge 4 bounding boxes with extent (add to both sides)
JPH_INLINE void AABox4EnlargeWithExtent(Vec3Arg inExtent, Vec4 &ioBoundsMinX, Vec4 &ioBoundsMinY, Vec4 &ioBoundsMinZ, Vec4 &ioBoundsMaxX, Vec4 &ioBoundsMaxY, Vec4 &ioBoundsMaxZ)
{
	Vec4 extent_x = inExtent.SplatX();
	ioBoundsMinX -= extent_x;
	ioBoundsMaxX += extent_x;

	Vec4 extent_y = inExtent.SplatY();
	ioBoundsMinY -= extent_y;
	ioBoundsMaxY += extent_y;

	Vec4 extent_z = inExtent.SplatZ();
	ioBoundsMinZ -= extent_z;
	ioBoundsMaxZ += extent_z;
}

/// Test if 4 bounding boxes overlap with a point
JPH_INLINE UVec4 AABox4VsPoint(Vec3Arg inPoint, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ)
{
	// Splat point to 4 component vectors
	Vec4 point_x = Vec4(inPoint).SplatX();
	Vec4 point_y = Vec4(inPoint).SplatY();
	Vec4 point_z = Vec4(inPoint).SplatZ();

	// Test if point overlaps with box
	UVec4 overlapx = UVec4::sAnd(Vec4::sGreaterOrEqual(point_x, inBoxMinX), Vec4::sLessOrEqual(point_x, inBoxMaxX));
	UVec4 overlapy = UVec4::sAnd(Vec4::sGreaterOrEqual(point_y, inBoxMinY), Vec4::sLessOrEqual(point_y, inBoxMaxY));
	UVec4 overlapz = UVec4::sAnd(Vec4::sGreaterOrEqual(point_z, inBoxMinZ), Vec4::sLessOrEqual(point_z, inBoxMaxZ));

	// Test if all are overlapping
	return UVec4::sAnd(UVec4::sAnd(overlapx, overlapy), overlapz);
}

/// Test if 4 bounding boxes overlap with an oriented box
JPH_INLINE UVec4 AABox4VsBox(Mat44Arg inOrientation, Vec3Arg inHalfExtents, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ, float inEpsilon = 1.0e-6f)
{
	// Taken from: Real Time Collision Detection - Christer Ericson
	// Chapter 4.4.1, page 103-105.
	// Note that the code is swapped around: A is the aabox and B is the oriented box (this saves us from having to invert the orientation of the oriented box)

	// Compute translation vector t (the translation of B in the space of A)
	Vec4 t[3] { 
		inOrientation.GetTranslation().SplatX() - 0.5f * (inBoxMinX + inBoxMaxX), 
		inOrientation.GetTranslation().SplatY() - 0.5f * (inBoxMinY + inBoxMaxY), 
		inOrientation.GetTranslation().SplatZ() - 0.5f * (inBoxMinZ + inBoxMaxZ) };
	
	// Compute common subexpressions. Add in an epsilon term to
	// counteract arithmetic errors when two edges are parallel and
	// their cross product is (near) null (see text for details)
	Vec3 epsilon = Vec3::sReplicate(inEpsilon);
	Vec3 abs_r[3] { inOrientation.GetAxisX().Abs() + epsilon, inOrientation.GetAxisY().Abs() + epsilon, inOrientation.GetAxisZ().Abs() + epsilon };

	// Half extents for a
	Vec4 a_half_extents[3] { 
		0.5f * (inBoxMaxX - inBoxMinX), 
		0.5f * (inBoxMaxY - inBoxMinY), 
		0.5f * (inBoxMaxZ - inBoxMinZ) };

	// Half extents of b
	Vec4 b_half_extents_x = inHalfExtents.SplatX();
	Vec4 b_half_extents_y = inHalfExtents.SplatY();
	Vec4 b_half_extents_z = inHalfExtents.SplatZ();

	// Each component corresponds to 1 overlapping OBB vs ABB
	UVec4 overlaps = UVec4(0xffffffff, 0xffffffff, 0xffffffff, 0xffffffff);

	// Test axes L = A0, L = A1, L = A2
	Vec4 ra, rb;
	for (int i = 0; i < 3; i++) 
	{
		ra = a_half_extents[i];
		rb = b_half_extents_x * abs_r[0][i] + b_half_extents_y * abs_r[1][i] + b_half_extents_z * abs_r[2][i];
		overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual(t[i].Abs(), ra + rb));
	}

	// Test axes L = B0, L = B1, L = B2
	for (int i = 0; i < 3; i++) 
	{
		ra = a_half_extents[0] * abs_r[i][0] + a_half_extents[1] * abs_r[i][1] + a_half_extents[2] * abs_r[i][2];
		rb = Vec4::sReplicate(inHalfExtents[i]);
		overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[0] * inOrientation(0, i) + t[1] * inOrientation(1, i) + t[2] * inOrientation(2, i)).Abs(), ra + rb));
	}

	// Test axis L = A0 x B0
	ra = a_half_extents[1] * abs_r[0][2] + a_half_extents[2] * abs_r[0][1];
	rb = b_half_extents_y * abs_r[2][0] + b_half_extents_z * abs_r[1][0];
	overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[2] * inOrientation(1, 0) - t[1] * inOrientation(2, 0)).Abs(), ra + rb));

	// Test axis L = A0 x B1
	ra = a_half_extents[1] * abs_r[1][2] + a_half_extents[2] * abs_r[1][1];
	rb = b_half_extents_x * abs_r[2][0] + b_half_extents_z * abs_r[0][0];
	overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[2] * inOrientation(1, 1) - t[1] * inOrientation(2, 1)).Abs(), ra + rb));

	// Test axis L = A0 x B2
	ra = a_half_extents[1] * abs_r[2][2] + a_half_extents[2] * abs_r[2][1];
	rb = b_half_extents_x * abs_r[1][0] + b_half_extents_y * abs_r[0][0];
	overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[2] * inOrientation(1, 2) - t[1] * inOrientation(2, 2)).Abs(), ra + rb));

	// Test axis L = A1 x B0
	ra = a_half_extents[0] * abs_r[0][2] + a_half_extents[2] * abs_r[0][0];
	rb = b_half_extents_y * abs_r[2][1] + b_half_extents_z * abs_r[1][1];	
	overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[0] * inOrientation(2, 0) - t[2] * inOrientation(0, 0)).Abs(), ra + rb));

	// Test axis L = A1 x B1
	ra = a_half_extents[0] * abs_r[1][2] + a_half_extents[2] * abs_r[1][0];
	rb = b_half_extents_x * abs_r[2][1] + b_half_extents_z * abs_r[0][1];
	overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[0] * inOrientation(2, 1) - t[2] * inOrientation(0, 1)).Abs(), ra + rb));
	
	// Test axis L = A1 x B2
	ra = a_half_extents[0] * abs_r[2][2] + a_half_extents[2] * abs_r[2][0];
	rb = b_half_extents_x * abs_r[1][1] + b_half_extents_y * abs_r[0][1];
	overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[0] * inOrientation(2, 2) - t[2] * inOrientation(0, 2)).Abs(), ra + rb));
	
	// Test axis L = A2 x B0
	ra = a_half_extents[0] * abs_r[0][1] + a_half_extents[1] * abs_r[0][0];
	rb = b_half_extents_y * abs_r[2][2] + b_half_extents_z * abs_r[1][2];
	overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[1] * inOrientation(0, 0) - t[0] * inOrientation(1, 0)).Abs(), ra + rb));
	
	// Test axis L = A2 x B1
	ra = a_half_extents[0] * abs_r[1][1] + a_half_extents[1] * abs_r[1][0];
	rb = b_half_extents_x * abs_r[2][2] + b_half_extents_z * abs_r[0][2];
	overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[1] * inOrientation(0, 1) - t[0] * inOrientation(1, 1)).Abs(), ra + rb));
	
	// Test axis L = A2 x B2
	ra = a_half_extents[0] * abs_r[2][1] + a_half_extents[1] * abs_r[2][0];
	rb = b_half_extents_x * abs_r[1][2] + b_half_extents_y * abs_r[0][2];
	overlaps = UVec4::sAnd(overlaps, Vec4::sLessOrEqual((t[1] * inOrientation(0, 2) - t[0] * inOrientation(1, 2)).Abs(), ra + rb));
	
	// Return if the OBB vs AABBs are intersecting
	return overlaps;
}

/// Convenience function that tests 4 AABoxes vs OrientedBox
JPH_INLINE UVec4 AABox4VsBox(const OrientedBox &inBox, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ, float inEpsilon = 1.0e-6f)
{
	return AABox4VsBox(inBox.mOrientation, inBox.mHalfExtents, inBoxMinX, inBoxMinY, inBoxMinZ, inBoxMaxX, inBoxMaxY, inBoxMaxZ, inEpsilon);
}

/// Test 4 AABoxes vs a sphere
JPH_INLINE UVec4 AABox4VsSphere(Vec4Arg inCenterX, Vec4Arg inCenterY, Vec4Arg inCenterZ, Vec4Arg inRadiusSq, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ)
{
	// Get closest point on box
	Vec4 closest_x = Vec4::sMin(Vec4::sMax(inCenterX, inBoxMinX), inBoxMaxX);
	Vec4 closest_y = Vec4::sMin(Vec4::sMax(inCenterY, inBoxMinY), inBoxMaxY);
	Vec4 closest_z = Vec4::sMin(Vec4::sMax(inCenterZ, inBoxMinZ), inBoxMaxZ);

	// Test the distance from the center of the sphere to the box is smaller than the radius
	Vec4 distance_sq = Square(closest_x - inCenterX) + Square(closest_y - inCenterY) + Square(closest_z - inCenterZ);
	return Vec4::sLessOrEqual(distance_sq, inRadiusSq);
}

/// Test 4 AABoxes vs a sphere
JPH_INLINE UVec4 AABox4VsSphere(Vec3Arg inCenter, float inRadiusSq, Vec4Arg inBoxMinX, Vec4Arg inBoxMinY, Vec4Arg inBoxMinZ, Vec4Arg inBoxMaxX, Vec4Arg inBoxMaxY, Vec4Arg inBoxMaxZ)
{
	return AABox4VsSphere(inCenter.SplatX(), inCenter.SplatY(), inCenter.SplatZ(), Vec4::sReplicate(inRadiusSq), inBoxMinX, inBoxMinY, inBoxMinZ, inBoxMaxX, inBoxMaxY, inBoxMaxZ);
}

JPH_NAMESPACE_END