public override void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex)
{
FPVector2 v1 = MathUtils.Mul(ref transform, _vertex1);
FPVector2 v2 = MathUtils.Mul(ref transform, _vertex2);
FPVector2 lower = FPVector2.Min(v1, v2);
FPVector2 upper = FPVector2.Max(v1, v2);
FPVector2 r = new FPVector2(Radius, Radius);
aabb.LowerBound = lower - r;
aabb.UpperBound = upper + r;
}
/// <summary>
/// Given a transform, compute the associated axis aligned bounding box for a child shape.
/// </summary>
/// <param name="aabb">The aabb results.</param>
/// <param name="transform">The world transform of the shape.</param>
/// <param name="childIndex">The child shape index.</param>
public override void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex)
{
FPVector2 lower = MathUtils.Mul(ref transform, Vertices[0]);
FPVector2 upper = lower;
for (int i = 1; i < Vertices.Count; ++i)
{
FPVector2 v = MathUtils.Mul(ref transform, Vertices[i]);
lower = FPVector2.Min(lower, v);
upper = FPVector2.Max(upper, v);
}
FPVector2 r = new FPVector2(Radius, Radius);
aabb.LowerBound = lower - r;
aabb.UpperBound = upper + r;
}
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;
}
FPVector2 v1 = MathUtils.Mul(ref transform, Vertices[i1]);
FPVector2 v2 = MathUtils.Mul(ref transform, Vertices[i2]);
aabb.LowerBound = FPVector2.Min(v1, v2);
aabb.UpperBound = FPVector2.Max(v1, v2);
}
public static FPVector2 Clamp(FPVector2 a, FPVector2 low, FPVector2 high)
{
return(FPVector2.Max(low, FPVector2.Min(a, high)));
}
/// <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(Func<RayCastInput, int, FP> callback, ref RayCastInput input)
{
FPVector2 p1 = input.Point1;
FPVector2 p2 = input.Point2;
FPVector2 r = p2 - p1;
Debug.Assert(r.LengthSquared() > 0.0f);
r.Normalize();
// v is perpendicular to the segment.
FPVector2 absV = MathUtils.Abs(new FPVector2(-r.y, r.x)); //FPE: Inlined the 'v' variable
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
FP maxFraction = input.MaxFraction;
// Build a bounding box for the segment.
AABB segmentAABB = new AABB();
{
FPVector2 t = p1 + maxFraction * (p2 - p1);
FPVector2.Min(ref p1, ref t, out segmentAABB.LowerBound);
FPVector2.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 node.AABB, ref segmentAABB) == false)
{
continue;
}
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
FPVector2 c = node.AABB.Center;
FPVector2 h = node.AABB.Extents;
FP separation = FP.Abs(FPVector2.Dot(new FPVector2(-r.y, r.x), p1 - c)) - FPVector2.Dot(absV, h);
if (separation > 0.0f)
{
continue;
}
if (node.IsLeaf())
{
RayCastInput subInput;
subInput.Point1 = input.Point1;
subInput.Point2 = input.Point2;
subInput.MaxFraction = maxFraction;
FP value = callback(subInput, nodeId);
if (value == 0.0f)
{
// the client has terminated the raycast.
return;
}
if (value > 0.0f)
{
// Update segment bounding box.
maxFraction = value;
FPVector2 t = p1 + maxFraction * (p2 - p1);
segmentAABB.LowerBound = FPVector2.Min(p1, t);
segmentAABB.UpperBound = FPVector2.Max(p1, t);
}
}
else
{
_raycastStack.Push(node.Child1);
_raycastStack.Push(node.Child2);
}
}
}
