internal TFRaycastHit2D Raycast(ITreeRaycastCallback callback, FixVec2 pointA, FixVec2 pointB, TFLayerMask mask)
{
TFRaycastHit2D hit = new TFRaycastHit2D();
FixVec2 r = pointB - pointA;
if (r.GetMagnitudeSquared() <= Fix.zero)
{
return(hit);
}
r.Normalize();
// v is perpendicular to the segment.
FixVec2 v = FixVec2.Cross(Fix.one, r);
FixVec2 abs_v = FixVec2.Abs(v);
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
Fix maxFraction = Fix.one;
// Build a bounding box for the segment.
AABB segmentAABB = new AABB();
FixVec2 t = pointA + maxFraction * (pointB - pointA);
segmentAABB.min = FixVec2.Min(pointA, t);
segmentAABB.max = FixVec2.Max(pointA, t);
Stack <int> stack = new Stack <int>();
stack.Push(rootIndex);
List <TFRaycastOutput> hitNodes = new List <TFRaycastOutput>(2);
while (stack.Count > 0)
{
var nodeId = stack.Pop();
if (nodeId == nullNode)
{
continue;
}
var node = nodes[nodeId];
if (!node.aabb.Overlaps(segmentAABB))
{
continue;
}
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
var c = node.aabb.GetCenter();
var h = node.aabb.GetExtents();
var separation = FixMath.Abs(FixVec2.Dot(v, pointA - c)) - FixVec2.Dot(abs_v, h);
if (separation > Fix.zero)
{
continue;
}
if (node.IsLeaf())
{
// If value is >= 0, then we hit the node.
TFRaycastHit2D rHit;
Fix value = callback.RayCastCallback(pointA, pointB, maxFraction, nodeId, out rHit, mask);
if (value == Fix.zero)
{
// The client has terminated the ray cast.
if (rHit)
{
// We actually hit the node, add it to the list.
hitNodes.Add(new TFRaycastOutput(nodeId, rHit));
}
break;
}
if (value == maxFraction)
{
if (rHit)
{
// We actually hit the node, add it to the list.
hitNodes.Add(new TFRaycastOutput(nodeId, rHit));
}
}
else if (value > Fix.zero)
{
if (rHit)
{
// We actually hit the node, add it to the list.
hitNodes.Add(new TFRaycastOutput(nodeId, rHit));
}
// Update segment bounding box.
maxFraction = value;
FixVec2 g = pointA + maxFraction * (pointB - pointA);
segmentAABB.min = FixVec2.Min(pointA, g);
segmentAABB.max = FixVec2.Max(pointA, g);
}
}
else
{
stack.Push(node.leftChildIndex);
stack.Push(node.rightChildIndex);
}
}
// Decide which node was the closest to the starting point.
Fix closestNode = maxFraction;
for (int i = 0; i < hitNodes.Count; i++)
{
if (hitNodes[i].hit.fraction < closestNode)
{
closestNode = hitNodes[i].hit.fraction;
hit = hitNodes[i].hit;
}
}
return(hit);
}
internal void Raycast(ITreeRaycastCallback callback, FixVec2 pointA, FixVec2 pointB)
{
FixVec2 r = pointB - pointA;
if (r.GetMagnitudeSquared() <= Fix.zero)
{
return;
}
r.Normalize();
// v is perpendicular to the segment.
FixVec2 v = FixVec2.Cross(Fix.one, r);
FixVec2 abs_v = FixVec2.Abs(v);
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
Fix maxFraction = Fix.one;
// Build a bounding box for the segment.
AABB segmentAABB = new AABB();
FixVec2 t = pointA + maxFraction * (pointB - pointA);
segmentAABB.min = FixVec2.Min(pointA, t);
segmentAABB.max = FixVec2.Max(pointA, t);
Stack <int> stack = new Stack <int>();
stack.Push(rootIndex);
while (stack.Count > 0)
{
var nodeId = stack.Pop();
if (nodeId == nullNode)
{
continue;
}
var node = nodes[nodeId];
if (!node.aabb.Overlaps(segmentAABB))
{
continue;
}
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
var c = node.aabb.GetCenter();
var h = node.aabb.GetExtents();
var separation = FixMath.Abs(FixVec2.Dot(v, pointA - c)) - FixVec2.Dot(abs_v, h);
if (separation > Fix.zero)
{
continue;
}
if (node.IsLeaf())
{
Fix value = callback.RayCastCallback(pointA, pointB, maxFraction, nodeId);
if (value == Fix.zero)
{
// The client has terminated the ray cast.
return;
}
if (value > Fix.zero)
{
// Update segment bounding box.
maxFraction = value;
FixVec2 g = pointA + maxFraction * (pointB - pointA);
segmentAABB.min = FixVec2.Min(pointA, g);
segmentAABB.max = FixVec2.Max(pointA, g);
}
}
else
{
stack.Push(node.leftChildIndex);
stack.Push(node.rightChildIndex);
}
}
}
