/// <summary>
/// Initializes a new instance of the <see cref="World"/> class.
/// </summary>
/// <param name="gravity">The gravity.</param>
public World(Vector2 gravity)
{
ContactManager = new ContactManager();
Gravity = gravity;
Flags = WorldFlags.ClearForces;
_queryAABBCallbackWrapper = QueryAABBCallbackWrapper;
_rayCastCallbackWrapper = RayCastCallbackWrapper;
Controllers = new List <Controller>();
BreakableBodyList = new List <BreakableBody>();
BodyList = new List <Body>(32);
JointList = new List <Joint>(32);
}
/// <summary>
/// Initializes a new instance of the <see cref="World"/> class.
/// </summary>
/// <param name="gravity">The gravity.</param>
public World(Vector2 gravity)
{
ContactManager = new ContactManager();
Gravity = gravity;
Flags = WorldFlags.ClearForces;
_queryAABBCallbackWrapper = QueryAABBCallbackWrapper;
_rayCastCallbackWrapper = RayCastCallbackWrapper;
Controllers = new List<Controller>();
BreakableBodyList = new List<BreakableBody>();
BodyList = new List<Body>(32);
JointList = new List<Joint>(32);
}
/// <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.
/// @param input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).
/// @param callback a callback class that is called for each proxy that is hit by the ray.
/// </summary>
/// <param name="callback"></param>
/// <param name="input"></param>
internal void RayCast(RayCastCallbackInternal callback, ref RayCastInput input)
{
Vector2 p1 = input.p1;
Vector2 p2 = input.p2;
Vector2 r = p2 - p1;
Debug.Assert(r.LengthSquared() > 0.0f);
r.Normalize();
// v is perpendicular to the segment.
Vector2 v = MathUtils.Cross(1.0f, r);
Vector2 abs_v = MathUtils.Abs(v);
// 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);
segmentAABB.lowerBound = Vector2.Min(p1, t);
segmentAABB.upperBound = Vector2.Max(p1, t);
}
int count = 0;
stack[count++] = _root;
while (count > 0)
{
int nodeId = stack[--count];
if (nodeId == NullNode)
{
continue;
}
DynamicTreeNode 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)
Vector2 c = node.aabb.GetCenter();
Vector2 h = node.aabb.GetExtents();
float separation = Math.Abs(Vector2.Dot(v, p1 - c)) - Vector2.Dot(abs_v, h);
if (separation > 0.0f)
{
continue;
}
if (node.IsLeaf())
{
RayCastInput subInput;
subInput.p1 = input.p1;
subInput.p2 = input.p2;
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);
segmentAABB.lowerBound = Vector2.Min(p1, t);
segmentAABB.upperBound = Vector2.Max(p1, t);
}
}
else
{
if (count < k_stackSize)
{
stack[count++] = node.child1;
}
if (count < k_stackSize)
{
stack[count++] = node.child2;
}
}
}
}
/// 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.
/// @param input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).
/// @param callback a callback class that is called for each proxy that is hit by the ray.
internal void RayCast(RayCastCallbackInternal callback, ref RayCastInput input)
{
Vector2 p1 = input.p1;
Vector2 p2 = input.p2;
Vector2 r = p2 - p1;
Debug.Assert(r.LengthSquared() > 0.0f);
r.Normalize();
// v is perpendicular to the segment.
Vector2 v = MathUtils.Cross(1.0f, r);
Vector2 abs_v = MathUtils.Abs(v);
// 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);
segmentAABB.lowerBound = Vector2.Min(p1, t);
segmentAABB.upperBound = Vector2.Max(p1, t);
}
int count = 0;
stack[count++] = _root;
while (count > 0)
{
int nodeId = stack[--count];
if (nodeId == NullNode)
{
continue;
}
DynamicTreeNode 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)
Vector2 c = node.aabb.GetCenter();
Vector2 h = node.aabb.GetExtents();
float separation = Math.Abs(Vector2.Dot(v, p1 - c)) - Vector2.Dot(abs_v, h);
if (separation > 0.0f)
{
continue;
}
if (node.IsLeaf())
{
RayCastInput subInput;
subInput.p1 = input.p1;
subInput.p2 = input.p2;
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);
segmentAABB.lowerBound = Vector2.Min(p1, t);
segmentAABB.upperBound = Vector2.Max(p1, t);
}
}
else
{
if (count < k_stackSize)
{
stack[count++] = node.child1;
}
if (count < k_stackSize)
{
stack[count++] = node.child2;
}
}
}
}
internal void RayCast(RayCastCallbackInternal callback, ref RayCastInput input)
{
_tree.RayCast(callback, ref input);
}
/// <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 public 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(RayCastCallbackInternal 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 v = MathUtils.Cross(1.0f, r);
Vector2 absV = MathUtils.Abs(v);
// 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);
segmentAABB.LowerBound = Vector2.Min(p1, t);
segmentAABB.UpperBound = Vector2.Max(p1, t);
}
_stack.Clear();
_stack.Push(_root);
while (_stack.Count > 0)
{
int nodeId = _stack.Pop();
if (nodeId == NullNode)
{
continue;
}
DynamicTreeNode 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)
Vector2 c = node.AABB.Center;
Vector2 h = node.AABB.Extents;
float separation = Math.Abs(Vector2.Dot(v, p1 - c)) - Vector2.Dot(absV, h);
if (separation > 0.0f)
{
continue;
}
if (node.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);
segmentAABB.LowerBound = Vector2.Min(p1, t);
segmentAABB.UpperBound = Vector2.Max(p1, t);
}
}
else
{
_stack.Push(node.Child1);
_stack.Push(node.Child2);
}
}
}
/// <summary>
/// construct a world object.
/// </summary>
/// <param name="gravity">the world gravity vector.</param>
/// <param name="doSleep">improve performance by not simulating inactive bodies.</param>
public World(Vector2 gravity, bool doSleep)
{
WarmStarting = true;
ContinuousPhysics = true;
_allowSleep = doSleep;
Gravity = gravity;
_flags = WorldFlags.ClearForces;
_queryAABBCallbackWrapper = QueryAABBCallbackWrapper;
_rayCastCallbackWrapper = RayCastCallbackWrapper;
}