// Query the world to find any shapes intersecting a ray.
public void RayCast(QueryCallback cb, RaycastData rayCast)
{
SceneQueryRaycastWrapper wrapper = new SceneQueryRaycastWrapper();
wrapper.RayCast = rayCast;
wrapper.broadPhase = ContactManager.Broadphase;
wrapper.cb = cb;
ContactManager.Broadphase.Tree.Query(wrapper, rayCast);
}
public bool Raycast(Transform tx, RaycastData raycast)
{
Transform world = Transform.Mul(tx, local);
Vec3 d = Transform.MulT(world.rotation, raycast.dir);
Vec3 p = Transform.MulT(world, raycast.start);
double epsilon = 1e-8;
double tmin = 0;
double tmax = raycast.t;
// t = (e[ i ] - p.[ i ]) / d[ i ]
double t0;
double t1;
Vec3 n0 = new Vec3();
for (int i = 0; i < 3; ++i)
{
// Check for ray parallel to and outside of AABB
if (Math.Abs(d[i]) < epsilon)
{
// Detect separating axes
if (p[i] < -e[i] || p[i] > e[i])
{
return(false);
}
}
else
{
double d0 = 1 / d[i];
double s = Math.Sign(d[i]);
double ei = e[i] * s;
Vec3 n = new Vec3(0, 0, 0);
n[i] = -s;
t0 = -(ei + p[i]) * d0;
t1 = (ei - p[i]) * d0;
if (t0 > tmin)
{
n0 = n;
tmin = t0;
}
tmax = Math.Min(tmax, t1);
if (tmin > tmax)
{
return(false);
}
}
}
raycast.normal = Transform.Mul(world.rotation, n0);
raycast.toi = tmin;
return(true);
}
public void Query(ITreeCallback cb, RaycastData rayCast)
{
double k_epsilon = 1e-6;
int k_stackCapacity = 256;
int[] stack = new int[k_stackCapacity];
int sp = 1;
stack[0] = Root;
Vec3 p0 = rayCast.start;
Vec3 p1 = p0 + rayCast.dir * rayCast.t;
while (sp > 0)
{
// k_stackCapacity too small
Assert(sp < k_stackCapacity);
int id = stack[--sp];
if (id == Node.Null)
{
continue;
}
Node n = Nodes[id];
Vec3 e = n.aabb.max - n.aabb.min;
Vec3 d = p1 - p0;
Vec3 m = p0 + p1 - n.aabb.min - n.aabb.max;
double adx = Math.Abs(d.x);
if (Math.Abs(m.x) > e.x + adx)
{
continue;
}
double ady = Math.Abs(d.y);
if (Math.Abs(m.y) > e.y + ady)
{
continue;
}
double adz = Math.Abs(d.z);
if (Math.Abs(m.z) > e.z + adz)
{
continue;
}
adx += k_epsilon;
ady += k_epsilon;
adz += k_epsilon;
if (Math.Abs(m.y * d.z - m.z * d.y) > e.y * adz + e.z * ady)
{
continue;
}
if (Math.Abs(m.z * d.x - m.x * d.z) > e.x * adz + e.z * adx)
{
continue;
}
if (Math.Abs(m.x * d.y - m.y * d.x) > e.x * ady + e.y * adx)
{
continue;
}
if (n.IsLeaf())
{
if (!cb.TreeCallback(id))
{
return;
}
}
else
{
stack[sp++] = n.left;
stack[sp++] = n.right;
}
}
}
