public bool4 Raycast(NativeBVHTree.Ray ray, float3 invD)
{
float4 lx = Lx - new float4(ray.origin.x);
float4 hx = Hx - new float4(ray.origin.x);
float4 nearXt = lx * new float4(invD.x);
float4 farXt = hx * new float4(invD.x);
float4 ly = Ly - new float4(ray.origin.y);
float4 hy = Hy - new float4(ray.origin.y);
float4 nearYt = ly * new float4(invD.y);
float4 farYt = hy * new float4(invD.y);
float4 lz = Lz - new float4(ray.origin.z);
float4 hz = Hz - new float4(ray.origin.z);
float4 nearZt = lz * new float4(invD.z);
float4 farZt = hz * new float4(invD.z);
float4 nearX = math.min(nearXt, farXt);
float4 farX = math.max(nearXt, farXt);
float4 nearY = math.min(nearYt, farYt);
float4 farY = math.max(nearYt, farYt);
float4 nearZ = math.min(nearZt, farZt);
float4 farZ = math.max(nearZt, farZt);
float4 nearMax = math.max(math.max(math.max(nearX, nearY), nearZ), float4.zero);
float4 farMin = math.min(math.min(farX, farY), farZ);
return((nearMax <= farMin) & (lx <= hx));
}
/// <summary>
/// Per: Christer Ericson - Real-Time Collision Detection (p. 179)
/// </summary>
public bool RayQuery(NativeBVHTree.Ray ray)
{
var m = ray.origin - center;
var c = math.dot(m, m) - radius * radius;
// If there is definitely at least one real root, there must be an intersection
if (c <= 0.0f)
{
return(true);
}
var b = math.dot(m, ray.direction);
// Early exit if ray origin outside sphere and ray pointing away from sphere
if (b > 0.0f)
{
return(false);
}
var disc = b * b - c;
// A negative discriminant corresponds to ray missing sphere
if (disc < 0.0f)
{
return(false);
}
// Now ray must hit sphere
return(true);
}
public bool RayQuery(NativeBVHTree.Ray ray)
{
fixed(Collider *target = &this)
{
switch (type)
{
case Type.Box:
return(((BoxCollider *)target->data)->RayQuery(ray));
case Type.Sphere:
return(((SphereCollider *)target->data)->RayQuery(ray));
#if ENABLE_UNITY_COLLECTIONS_CHECKS
default:
throw new ArgumentOutOfRangeException();
#endif
}
}
}
public bool RayQuery(NativeBVHTree.Ray ray)
{
var invD = 1 / ray.direction;
return(IntersectionUtils.DoesOverlap(LowerBound, UpperBound, ref ray, invD));
}
public IEnumerator Test()
{
int amount = 3000;
Random.InitState(0);
var go = new GameObject("debug");
go.AddComponent <NativeBVHDebugDrawer>();
var world = new BVHTreeWorld(amount, Allocator.Persistent);
var movingObjects = new List <MovingObject>();
NativeBVHDebugDrawer.LastTree = world.tree;
NativeList <Collider> colliders = new NativeList <Collider>(amount, Allocator.TempJob);
for (int i = 0; i < amount / 2; i++)
{
var center = new float3(UnityEngine.Random.Range(0, 0), UnityEngine.Random.Range(0, 0), UnityEngine.Random.Range(0, 0));
var size = new float3(UnityEngine.Random.Range(1, 3), UnityEngine.Random.Range(1, 3), UnityEngine.Random.Range(1, 3));
colliders.Add(BoxCollider.Create(center, size));
}
for (int i = 0; i < amount / 2; i++)
{
var center = new float3(UnityEngine.Random.Range(0, 0), UnityEngine.Random.Range(0, 0), UnityEngine.Random.Range(0, 0));
colliders.Add(SphereCollider.Create(center, UnityEngine.Random.Range(10, 15)));
}
new BVHTreeWorld.InsertJob {
Tree = world.tree,
Bodies = new NativeList <BVHTreeWorld.Body>(0, Allocator.TempJob),
Colliders = new NativeList <Collider>(0, Allocator.TempJob),
}.Run();
new BVHTreeWorld.InsertJob {
Tree = world.tree,
Bodies = world.bodies,
Colliders = colliders,
}.Run();
var until = Time.time + 30;
// Prep raycast
var rayResult = new NativeList <int>(64, Allocator.TempJob);
var rayVisited = new NativeList <int>(64, Allocator.TempJob);
var ray = new NativeBVHTree.Ray {
origin = new float3(-30, -30, 0),
direction = math.normalize(new float3(5, 5, 5)),
minDistance = 0,
maxDistance = 500
};
// Job
var job = new RaycastJob {
Tree = world.tree,
Ray = ray,
Results = rayResult,
};
for (var i = 0; i < world.bodies.Length; i++)
{
movingObjects.Add(new MovingObject {
transform = RigidTransform.identity,
velocity = new float3(Random.Range(-1f, 1f), Random.Range(-1f, 1f), Random.Range(-1f, 1f))
});
}
while (Time.time < until)
{
// Update
Profiler.BeginSample("BVH - World.Update");
world.Update();
Profiler.EndSample();
Profiler.BeginSample("BVH - Raycast");
job.Run();
Profiler.EndSample();
yield return(null);
for (var i = 0; i < world.bodies.Length; i++)
{
// Update position
var movingObject = movingObjects[i];
movingObject.transform.pos += movingObject.velocity * Time.deltaTime * 2;
movingObjects[i] = movingObject;
// And update world
world.UpdateTransform(i, movingObject.transform);
}
// Debug
NativeBVHDebugDrawer.LastTree = world.tree;
NativeBVHDebugDrawer.LastTreeHits = rayResult.ToArray();
NativeBVHDebugDrawer.LastTreeRayVisited = new bool[world.tree.DebugGetTotalNodesLength() + 1];
foreach (var i in rayVisited)
{
NativeBVHDebugDrawer.LastTreeRayVisited[i] = true;
}
NativeBVHDebugDrawer.LastRay = ray;
}
}
public static bool DoesOverlap(ref AABB3D box, ref NativeBVHTree.Ray ray, float3 invD)
{
return(IntersectTest(box.LowerBound, box.UpperBound, ray.origin, ray.minDistance, ray.maxDistance, invD, out _));
}
public static bool DoesOverlap(float3 boxMin, float3 boxMax, ref NativeBVHTree.Ray ray, float3 invD)
{
return(IntersectTest(boxMin, boxMax, ray.origin, ray.minDistance, ray.maxDistance, invD, out _));
}