private void checkBox(Aabb box)
{
IEnumerable <LineSeg> segs = Gen <LineSeg>(delegate() { return(LineSeg.FromEndpoints(PointInside(box), PointOutside(box))); });
foreach (LineSeg ls in Take(100, segs))
{
Assert.IsTrue(box.Intersects(ls));
LineSeg outline = new LineSeg(ls.End, ls.Dir);
Assert.IsFalse(box.Intersects(outline));
LineSeg swapped = LineSeg.FromEndpoints(ls.End, ls.Start);
Assert.IsTrue(box.Intersects(swapped));
// neither endpoint is inside but the line def. crosses
LineSeg crossing = new LineSeg(ls.Start - ls.Dir, ls.Dir * 2.0);
Assert.IsTrue(box.Intersects(crossing));
// Shift it outside of the box by moving it sideways a min amt...
double shift_length = Math.Sqrt(box.W * box.W + box.H * box.H);
Vector shift = shift_length * crossing.Dir.Normalize().Perp;
LineSeg out1 = new LineSeg(crossing.Start + shift,
crossing.Dir);
Assert.IsFalse(box.Intersects(out1));
}
}
public bool Query(Aabb bounds, QueryCallbcak callback = null)
{
m_stack.Clear();
m_stack.Push(m_root);
bool touchedAnyBounds = false;
while (m_stack.Count > 0)
{
int index = m_stack.Pop();
if (index == Null)
{
continue;
}
Aabb tightBounds = m_nodes[index].Bounds;
tightBounds.Expand(-FatBoundsRadius);
if (!Aabb.Intersects(bounds, tightBounds))
{
continue;
}
if (m_nodes[index].IsLeaf)
{
touchedAnyBounds = true;
bool proceed =
callback != null
? callback(m_nodes[index].UserData)
: true;
if (!proceed)
{
return(true);
}
}
else
{
m_stack.Push(m_nodes[index].ChildA);
m_stack.Push(m_nodes[index].ChildB);
}
}
return(touchedAnyBounds);
}
public bool RayCast(Vector3 from, Vector3 to, RayCastCallback callback = null)
{
Vector3 r = to - from;
r.Normalize();
float maxFraction = 1.0f;
// v is perpendicular to the segment.
Vector3 v = VectorUtil.FindOrthogonal(r).normalized;
Vector3 absV = VectorUtil.Abs(v);
// build a bounding box for the segment.
Aabb rayBounds = Aabb.Empty;
rayBounds.Include(from);
rayBounds.Include(to);
m_stack.Clear();
m_stack.Push(m_root);
bool hitAnyBounds = false;
while (m_stack.Count > 0)
{
int index = m_stack.Pop();
if (index == Null)
{
continue;
}
if (!Aabb.Intersects(m_nodes[index].Bounds, rayBounds))
{
continue;
}
// Separating axis for segment (Gino, p80).
// |dot(v, a - c)| > dot(|v|, h)
Vector3 c = m_nodes[index].Bounds.Center;
Vector3 h = m_nodes[index].Bounds.HalfExtents;
float separation = Mathf.Abs(Vector3.Dot(v, from - c)) - Vector3.Dot(absV, h);
if (separation > 0.0f)
{
continue;
}
if (m_nodes[index].IsLeaf)
{
Aabb tightBounds = m_nodes[index].Bounds;
tightBounds.Expand(-FatBoundsRadius);
float t = tightBounds.RayCast(from, to, maxFraction);
if (t < 0.0f)
{
continue;
}
hitAnyBounds = true;
float newMaxFraction =
callback != null
? callback(from, to, m_nodes[index].UserData)
: maxFraction;
if (newMaxFraction >= 0.0f)
{
// Update segment bounding box.
maxFraction = newMaxFraction;
Vector3 newTo = from + maxFraction * (to - from);
rayBounds.Min = VectorUtil.Min(from, newTo);
rayBounds.Max = VectorUtil.Max(from, newTo);
}
}
else
{
m_stack.Push(m_nodes[index].ChildA);
m_stack.Push(m_nodes[index].ChildB);
}
}
return(hitAnyBounds);
}
public bool Intersects(LineSeg l)
{
return(m_bound.Intersects(l));
}