public override void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex)
{
Debug.Assert(childIndex < Vertices.Count);
int i1 = childIndex;
int i2 = childIndex + 1;
if (i2 == Vertices.Count)
{
i2 = 0;
}
Vector2 v1 = Transform.Multiply(Vertices[i1], ref transform);
Vector2 v2 = Transform.Multiply(Vertices[i2], ref transform);
Vector2.Min(ref v1, ref v2, out aabb.LowerBound);
Vector2.Max(ref v1, ref v2, out aabb.UpperBound);
}
/// <summary>
/// tests if ray intersects AABB
/// </summary>
/// <param name="aabb"></param>
/// <returns></returns>
public static bool RayCastAABB(AABB aabb, Vector2 p1, Vector2 p2)
{
AABB segmentAABB = new AABB();
{
Vector2.Min(ref p1, ref p2, out segmentAABB.LowerBound);
Vector2.Max(ref p1, ref p2, out segmentAABB.UpperBound);
}
if (!AABB.TestOverlap(ref aabb, ref segmentAABB))
{
return(false);
}
Vector2 rayDir = p2 - p1;
Vector2 rayPos = p1;
Vector2 norm = new Vector2(-rayDir.Y, rayDir.X); //normal to ray
if (norm.Length() == 0.0f)
{
return(true); //if ray is just a point, return true (iff point is within aabb, as tested earlier)
}
norm.Normalize();
float dPos = Vector2.Dot(rayPos, norm);
var verts = aabb.Vertices;
float d0 = Vector2.Dot(verts[0], norm) - dPos;
for (int i = 1; i < 4; i++)
{
float d = Vector2.Dot(verts[i], norm) - dPos;
if (Math.Sign(d) != Math.Sign(d0))
{
//return true if the ray splits the vertices (ie: sign of dot products with normal are not all same)
return(true);
}
}
return(false);
}
/// <summary>
/// Ray-cast against the proxies in the tree. This relies on the callback
/// to perform a exact ray-cast in the case were the proxy contains a Shape.
/// The callback also performs the any collision filtering. This has performance
/// roughly equal to k * log(n), where k is the number of collisions and n is the
/// number of proxies in the tree.
/// </summary>
/// <param name="callback">A callback class that is called for each proxy that is hit by the ray.</param>
/// <param name="input">The ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).</param>
public void RayCast(BroadPhaseRayCastCallback callback, ref RayCastInput input)
{
Vector2 p1 = input.Point1;
Vector2 p2 = input.Point2;
Vector2 r = p2 - p1;
Debug.Assert(r.LengthSquared() > 0.0f);
r.Normalize();
// v is perpendicular to the segment.
Vector2 absV = MathUtils.Abs(new Vector2(-r.Y, r.X)); //FPE: Inlined the 'v' variable
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
float maxFraction = input.MaxFraction;
// Build a bounding box for the segment.
AABB segmentAABB = new AABB();
{
Vector2 t = p1 + maxFraction * (p2 - p1);
Vector2.Min(ref p1, ref t, out segmentAABB.LowerBound);
Vector2.Max(ref p1, ref t, out segmentAABB.UpperBound);
}
_raycastStack.Clear();
_raycastStack.Push(_root);
while (_raycastStack.Count > 0)
{
int nodeId = _raycastStack.Pop();
if (nodeId == NullNode)
{
continue;
}
//TreeNode<T>* node = &_nodes[nodeId];
if (AABB.TestOverlap(ref _nodes[nodeId].AABB, ref segmentAABB) == false)
{
continue;
}
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
Vector2 c = _nodes[nodeId].AABB.Center;
Vector2 h = _nodes[nodeId].AABB.Extents;
float separation = Math.Abs(Vector2.Dot(new Vector2(-r.Y, r.X), p1 - c)) - Vector2.Dot(absV, h);
if (separation > 0.0f)
{
continue;
}
if (_nodes[nodeId].IsLeaf())
{
RayCastInput subInput;
subInput.Point1 = input.Point1;
subInput.Point2 = input.Point2;
subInput.MaxFraction = maxFraction;
float value = callback(ref subInput, nodeId);
if (value == 0.0f)
{
// the client has terminated the raycast.
return;
}
if (value > 0.0f)
{
// Update segment bounding box.
maxFraction = value;
Vector2 t = p1 + maxFraction * (p2 - p1);
Vector2.Min(ref p1, ref t, out segmentAABB.LowerBound);
Vector2.Max(ref p1, ref t, out segmentAABB.UpperBound);
}
}
else
{
_raycastStack.Push(_nodes[nodeId].Child1);
_raycastStack.Push(_nodes[nodeId].Child2);
}
}
}
