public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex)
{
// Collision Detection in Interactive 3D Environments by Gino van den Bergen
// From Section 3.1.2
// x = s + a * r
// norm(x) = radius
output = new RayCastOutput();
Vector2 position = transform.p + MathUtils.Mul(transform.q, Position);
Vector2 s = input.Point1 - position;
float b = Vector2.Dot(s, s) - _2radius;
// Solve quadratic equation.
Vector2 r = input.Point2 - input.Point1;
float c = Vector2.Dot(s, r);
float rr = Vector2.Dot(r, r);
float sigma = c * c - rr * b;
// Check for negative discriminant and short segment.
if (sigma < 0.0f || rr < Settings.Epsilon)
{
return false;
}
// Find the point of intersection of the line with the circle.
float a = -(c + (float)Math.Sqrt(sigma));
// Is the intersection point on the segment?
if (0.0f <= a && a <= input.MaxFraction * rr)
{
a /= rr;
output.Fraction = a;
//TODO: Check results here
output.Normal = s + a * r;
output.Normal.Normalize();
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);
}
}
}
public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex)
{
output = new RayCastOutput();
// Put the ray into the polygon's frame of reference.
Vector2 p1 = MathUtils.MulT(transform.q, input.Point1 - transform.p);
Vector2 p2 = MathUtils.MulT(transform.q, input.Point2 - transform.p);
Vector2 d = p2 - p1;
float lower = 0.0f, upper = input.MaxFraction;
int index = -1;
for (int i = 0; i < Vertices.Count; ++i)
{
// p = p1 + a * d
// dot(normal, p - v) = 0
// dot(normal, p1 - v) + a * dot(normal, d) = 0
float numerator = Vector2.Dot(Normals[i], Vertices[i] - p1);
float denominator = Vector2.Dot(Normals[i], d);
if (denominator == 0.0f)
{
if (numerator < 0.0f)
{
return false;
}
}
else
{
// Note: we want this predicate without division:
// lower < numerator / denominator, where denominator < 0
// Since denominator < 0, we have to flip the inequality:
// lower < numerator / denominator <==> denominator * lower > numerator.
if (denominator < 0.0f && numerator < lower * denominator)
{
// Increase lower.
// The segment enters this half-space.
lower = numerator / denominator;
index = i;
}
else if (denominator > 0.0f && numerator < upper * denominator)
{
// Decrease upper.
// The segment exits this half-space.
upper = numerator / denominator;
}
}
// The use of epsilon here causes the assert on lower to trip
// in some cases. Apparently the use of epsilon was to make edge
// shapes work, but now those are handled separately.
//if (upper < lower - b2_epsilon)
if (upper < lower)
{
return false;
}
}
Debug.Assert(0.0f <= lower && lower <= input.MaxFraction);
if (index >= 0)
{
output.Fraction = lower;
output.Normal = MathUtils.Mul(transform.q, Normals[index]);
return true;
}
return false;
}
/// <summary>
/// Cast a ray against a child shape.
/// </summary>
/// <param name="output">The ray-cast results.</param>
/// <param name="input">The ray-cast input parameters.</param>
/// <param name="transform">The transform to be applied to the shape.</param>
/// <param name="childIndex">The child shape index.</param>
/// <returns>True if the ray-cast hits the shape</returns>
public override bool RayCast(out RayCastOutput output, ref RayCastInput input,
ref Transform transform, int childIndex)
{
//Debug.Assert(childIndex < Vertices.Count);
int i1 = childIndex;
int i2 = childIndex + 1;
if (i2 == Vertices.Count)
{
i2 = 0;
}
_edgeShape.Vertex1 = Vertices[i1];
_edgeShape.Vertex2 = Vertices[i2];
return _edgeShape.RayCast(out output, ref input, ref transform, 0);
}
/// <summary>
/// Cast a ray against a child shape.
/// </summary>
/// <param name="output">The ray-cast results.</param>
/// <param name="input">The ray-cast input parameters.</param>
/// <param name="transform">The transform to be applied to the shape.</param>
/// <param name="childIndex">The child shape index.</param>
/// <returns>True if the ray-cast hits the shape</returns>
public abstract bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform,
int childIndex);
public override bool Raycast(RayCastOutput output, RayCastInput input, Transform xf, int childIndex)
{
Debug.Assert(childIndex < Count);
EdgeShape edgeShape = pool0;
int i1 = childIndex;
int i2 = childIndex + 1;
if (i2 == Count)
{
i2 = 0;
}
edgeShape.Vertex1.Set(Vertices[i1]);
edgeShape.Vertex2.Set(Vertices[i2]);
return edgeShape.Raycast(output, input, xf, 0);
}
public override bool rayCast( out RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex )
{
// p = p1 + t * d
// v = v1 + s * e
// p1 + t * d = v1 + s * e
// s * e - t * d = p1 - v1
output = new RayCastOutput();
// Put the ray into the edge's frame of reference.
var p1 = MathUtils.mulT( transform.q, input.Point1 - transform.p );
var p2 = MathUtils.mulT( transform.q, input.Point2 - transform.p );
var d = p2 - p1;
var v1 = _vertex1;
var v2 = _vertex2;
var e = v2 - v1;
var normal = new Vector2( e.Y, -e.X ); //TODO: Could possibly cache the normal.
normal.Normalize();
// q = p1 + t * d
// dot(normal, q - v1) = 0
// dot(normal, p1 - v1) + t * dot(normal, d) = 0
var numerator = Vector2.Dot( normal, v1 - p1 );
var denominator = Vector2.Dot( normal, d );
if( denominator == 0.0f )
return false;
float t = numerator / denominator;
if( t < 0.0f || input.MaxFraction < t )
return false;
var q = p1 + t * d;
// q = v1 + s * r
// s = dot(q - v1, r) / dot(r, r)
var r = v2 - v1;
var rr = Vector2.Dot( r, r );
if( rr == 0.0f )
return false;
float s = Vector2.Dot( q - v1, r ) / rr;
if( s < 0.0f || 1.0f < s )
return false;
output.Fraction = t;
if( numerator > 0.0f )
output.Normal = -normal;
else
output.Normal = normal;
return true;
}
/// <summary> /// Cast a ray against a child shape. /// </summary> /// <param name="argOutput">the ray-cast results.</param> /// <param name="argInput">the ray-cast input parameters.</param> /// <param name="argTransform">the transform to be applied to the shape.</param> /// <param name="argChildIndex">the child shape index</param> /// <returns>if hit</returns> public abstract bool raycast(RayCastOutput output, RayCastInput input, Transform transform, int childIndex);
// Collision Detection in Interactive 3D Environments by Gino van den Bergen
// From Section 3.1.2
// x = s + a * r
// norm(x) = radius
public override bool Raycast(RayCastOutput output, RayCastInput input, Transform transform, int childIndex)
{
Vec2 position = pool1;
Vec2 s = pool2;
Vec2 r = pool3;
Rot.MulToOutUnsafe(transform.Q, P, position);
position.AddLocal(transform.P);
s.Set(input.P1).SubLocal(position);
float b = Vec2.Dot(s, s) - Radius * Radius;
// Solve quadratic equation.
r.Set(input.P2).SubLocal(input.P1);
float c = Vec2.Dot(s, r);
float rr = Vec2.Dot(r, r);
float sigma = c * c - rr * b;
// Check for negative discriminant and short segment.
if (sigma < 0.0f || rr < Settings.EPSILON)
{
return false;
}
// Find the point of intersection of the line with the circle.
float a = -(c + MathUtils.Sqrt(sigma));
// Is the intersection point on the segment?
if (0.0f <= a && a <= input.MaxFraction * rr)
{
a /= rr;
output.Fraction = a;
output.Normal.Set(r).MulLocal(a);
output.Normal.AddLocal(s);
output.Normal.Normalize();
return true;
}
return false;
}
public void raycast(TreeRayCastCallback callback, RayCastInput input)
{
Vec2 p1 = input.p1;
Vec2 p2 = input.p2;
float p1x = p1.x, p2x = p2.x, p1y = p1.y, p2y = p2.y;
float vx, vy;
float rx, ry;
float absVx, absVy;
float cx, cy;
float hx, hy;
float tempx, tempy;
r.x = p2x - p1x;
r.y = p2y - p1y;
Debug.Assert((r.x*r.x + r.y*r.y) > 0f);
r.normalize();
rx = r.x;
ry = r.y;
// v is perpendicular to the segment.
vx = -1f*ry;
vy = 1f*rx;
absVx = MathUtils.abs(vx);
absVy = MathUtils.abs(vy);
// 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 segAABB = aabb;
// Vec2 t = p1 + maxFraction * (p2 - p1);
// before inline
// temp.set(p2).subLocal(p1).mulLocal(maxFraction).addLocal(p1);
// Vec2.minToOut(p1, temp, segAABB.lowerBound);
// Vec2.maxToOut(p1, temp, segAABB.upperBound);
tempx = (p2x - p1x)*maxFraction + p1x;
tempy = (p2y - p1y)*maxFraction + p1y;
segAABB.lowerBound.x = p1x < tempx ? p1x : tempx;
segAABB.lowerBound.y = p1y < tempy ? p1y : tempy;
segAABB.upperBound.x = p1x > tempx ? p1x : tempx;
segAABB.upperBound.y = p1y > tempy ? p1y : tempy;
// end inline
nodeStackIndex = 0;
nodeStack[nodeStackIndex++] = m_root;
while (nodeStackIndex > 0)
{
DynamicTreeNode node = nodeStack[--nodeStackIndex];
if (node == null)
{
continue;
}
AABB nodeAABB = node.aabb;
if (!AABB.testOverlap(nodeAABB, segAABB))
{
continue;
}
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
// node.aabb.getCenterToOut(c);
// node.aabb.getExtentsToOut(h);
cx = (nodeAABB.lowerBound.x + nodeAABB.upperBound.x)*.5f;
cy = (nodeAABB.lowerBound.y + nodeAABB.upperBound.y)*.5f;
hx = (nodeAABB.upperBound.x - nodeAABB.lowerBound.x)*.5f;
hy = (nodeAABB.upperBound.y - nodeAABB.lowerBound.y)*.5f;
tempx = p1x - cx;
tempy = p1y - cy;
float separation = MathUtils.abs(vx*tempx + vy*tempy) - (absVx*hx + absVy*hy);
if (separation > 0.0f)
{
continue;
}
if (node.child1 == null)
{
subInput.p1.x = p1x;
subInput.p1.y = p1y;
subInput.p2.x = p2x;
subInput.p2.y = p2y;
subInput.maxFraction = maxFraction;
float value = callback.raycastCallback(subInput, node.id);
if (value == 0.0f)
{
// The client has terminated the ray cast.
return;
}
if (value > 0.0f)
{
// Update segment bounding box.
maxFraction = value;
// temp.set(p2).subLocal(p1).mulLocal(maxFraction).addLocal(p1);
// Vec2.minToOut(p1, temp, segAABB.lowerBound);
// Vec2.maxToOut(p1, temp, segAABB.upperBound);
tempx = (p2x - p1x)*maxFraction + p1x;
tempy = (p2y - p1y)*maxFraction + p1y;
segAABB.lowerBound.x = p1x < tempx ? p1x : tempx;
segAABB.lowerBound.y = p1y < tempy ? p1y : tempy;
segAABB.upperBound.x = p1x > tempx ? p1x : tempx;
segAABB.upperBound.y = p1y > tempy ? p1y : tempy;
}
}
else
{
if (nodeStack.Length - nodeStackIndex - 2 <= 0)
{
DynamicTreeNode[] newBuffer = new DynamicTreeNode[nodeStack.Length*2];
Array.Copy(nodeStack, 0, newBuffer, 0, nodeStack.Length);
nodeStack = newBuffer;
}
nodeStack[nodeStackIndex++] = node.child1;
nodeStack[nodeStackIndex++] = node.child2;
}
}
}
/// <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, float> callback, ref RayCastInput input)
{
this._tree.RayCast(callback, ref input);
}
/// <summary> /// Cast a ray against a child shape. /// </summary> /// <param name="output">The ray-cast results.</param> /// <param name="input">The ray-cast input parameters.</param> /// <param name="transform">The transform to be applied to the shape.</param> /// <param name="childIndex">The child shape index.</param> /// <returns>True if the ray-cast hits the shape</returns> public abstract bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex);
/// <summary>
/// Cast a ray against this Shape.
/// </summary>
/// <param name="output">The ray-cast results.</param>
/// <param name="input">The ray-cast input parameters.</param>
/// <param name="childIndex">Index of the child.</param>
/// <returns></returns>
public bool RayCast(out RayCastOutput output, ref RayCastInput input, int childIndex)
{
return(Shape.RayCast(out output, ref input, ref Body.Xf, childIndex));
}
public PhysicsOutput RayCast(RayCastInput input)
{
output.EndIndex = 0;
PhysicsTree.RayCast(RayCastCallback, ref input);
return(output);
}
public override bool raycast(RayCastOutput output, RayCastInput input, Transform xf, int childIndex)
{
// Put the ray into the edge's frame of reference.
Vec2 p1 = pool0.set_Renamed(input.p1).subLocal(xf.p);
Rot.mulTrans(xf.q, p1, p1);
Vec2 p2 = pool1.set_Renamed(input.p2).subLocal(xf.p);
Rot.mulTrans(xf.q, p1, p1);
Vec2 d = p2.subLocal(p1); // we don't use p2 later
Vec2 v1 = m_vertex1;
Vec2 v2 = m_vertex2;
Vec2 normal = pool2.set_Renamed(v2).subLocal(v1);
normal.set_Renamed(normal.y, -normal.x);
normal.normalize();
// q = p1 + t * d
// dot(normal, q - v1) = 0
// dot(normal, p1 - v1) + t * dot(normal, d) = 0
pool3.set_Renamed(v1).subLocal(p1);
float numerator = Vec2.dot(normal, pool3);
float denominator = Vec2.dot(normal, d);
if (denominator == 0.0f)
{
return false;
}
float t = numerator / denominator;
if (t < 0.0f || 1.0f < t)
{
return false;
}
Vec2 q = pool3;
Vec2 r = pool4;
// Vec2 q = p1 + t * d;
q.set_Renamed(d).mulLocal(t).addLocal(p1);
// q = v1 + s * r
// s = dot(q - v1, r) / dot(r, r)
// Vec2 r = v2 - v1;
r.set_Renamed(v2).subLocal(v1);
float rr = Vec2.dot(r, r);
if (rr == 0.0f)
{
return false;
}
pool5.set_Renamed(q).subLocal(v1);
float s = Vec2.dot(pool5, r) / rr;
if (s < 0.0f || 1.0f < s)
{
return false;
}
output.fraction = t;
if (numerator > 0.0f)
{
// argOutput.normal = -normal;
output.normal.set_Renamed(normal).negateLocal();
}
else
{
// output.normal = normal;
output.normal.set_Renamed(normal);
}
return true;
}
public override bool Raycast(RayCastOutput output, RayCastInput input, Transform xf, int childIndex)
{
// Put the ray into the edge's frame of reference.
Vec2 p1 = pool0.Set(input.P1).SubLocal(xf.P);
Rot.MulTrans(xf.Q, p1, p1);
Vec2 p2 = pool1.Set(input.P2).SubLocal(xf.P);
Rot.MulTrans(xf.Q, p1, p1);
Vec2 d = p2.SubLocal(p1); // we don't use p2 later
Vec2 v1 = Vertex1;
Vec2 v2 = Vertex2;
Vec2 normal = pool2.Set(v2).SubLocal(v1);
normal.Set(normal.Y, -normal.X);
normal.Normalize();
// q = p1 + t * d
// dot(normal, q - v1) = 0
// dot(normal, p1 - v1) + t * dot(normal, d) = 0
pool3.Set(v1).SubLocal(p1);
float numerator = Vec2.Dot(normal, pool3);
float denominator = Vec2.Dot(normal, d);
if (denominator == 0.0f)
{
return false;
}
float t = numerator / denominator;
if (t < 0.0f || 1.0f < t)
{
return false;
}
Vec2 q = pool3;
Vec2 r = pool4;
// Vec2 q = p1 + t * d;
q.Set(d).MulLocal(t).AddLocal(p1);
// q = v1 + s * r
// s = dot(q - v1, r) / dot(r, r)
// Vec2 r = v2 - v1;
r.Set(v2).SubLocal(v1);
float rr = Vec2.Dot(r, r);
if (rr == 0.0f)
{
return false;
}
pool5.Set(q).SubLocal(v1);
float s = Vec2.Dot(pool5, r) / rr;
if (s < 0.0f || 1.0f < s)
{
return false;
}
output.Fraction = t;
if (numerator > 0.0f)
{
// argOutput.normal = -normal;
output.Normal.Set(normal).NegateLocal();
}
else
{
// output.normal = normal;
output.Normal.Set(normal);
}
return true;
}
public override bool raycast(RayCastOutput output, RayCastInput input, Transform xf, int childIndex)
{
Debug.Assert(childIndex < m_count);
EdgeShape edgeShape = pool0;
int i1 = childIndex;
int i2 = childIndex + 1;
if (i2 == m_count)
{
i2 = 0;
}
Vec2 v = m_vertices[i1];
edgeShape.m_vertex1.x = v.x;
edgeShape.m_vertex1.y = v.y;
Vec2 v1 = m_vertices[i2];
edgeShape.m_vertex2.x = v1.x;
edgeShape.m_vertex2.y = v1.y;
return edgeShape.raycast(output, input, xf, 0);
}
// Collision Detection in Interactive 3D Environments by Gino van den Bergen
// From Section 3.1.2
// x = s + a * r
// norm(x) = radius
public override bool raycast(RayCastOutput output, RayCastInput input, Transform transform, int childIndex)
{
Vec2 position = pool1;
Vec2 s = pool2;
Vec2 r = pool3;
Rot.mulToOutUnsafe(transform.q, m_p, position);
position.addLocal(transform.p);
s.set_Renamed(input.p1).subLocal(position);
float b = Vec2.dot(s, s) - m_radius * m_radius;
// Solve quadratic equation.
r.set_Renamed(input.p2).subLocal(input.p1);
float c = Vec2.dot(s, r);
float rr = Vec2.dot(r, r);
float sigma = c * c - rr * b;
// Check for negative discriminant and short segment.
if (sigma < 0.0f || rr < Settings.EPSILON)
{
return false;
}
// Find the point of intersection of the line with the circle.
float a = -(c + MathUtils.sqrt(sigma));
// Is the intersection point on the segment?
if (0.0f <= a && a <= input.maxFraction * rr)
{
a /= rr;
output.fraction = a;
output.normal.set_Renamed(r).mulLocal(a);
output.normal.addLocal(s);
output.normal.normalize();
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="input">the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).</param>
/// <param name="callback">a callback class that is called for each proxy that is hit by the ray.</param>
public void Raycast(ITreeRayCastCallback callback, RayCastInput input)
{
Vec2 p1 = input.P1;
Vec2 p2 = input.P2;
r.Set(p2).SubLocal(p1);
Debug.Assert(r.LengthSquared() > 0f);
r.Normalize();
// v is perpendicular to the segment.
Vec2.CrossToOutUnsafe(1f, r, v);
absV.Set(v).AbsLocal();
// 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 segAABB = aabb;
// Vec2 t = p1 + maxFraction * (p2 - p1);
temp.Set(p2).SubLocal(p1).MulLocal(maxFraction).AddLocal(p1);
Vec2.MinToOut(p1, temp, segAABB.LowerBound);
Vec2.MaxToOut(p1, temp, segAABB.UpperBound);
intStack.Push(m_root);
while (intStack.Count > 0)
{
int nodeId = intStack.Pop();
if (nodeId == TreeNode.NULL_NODE)
{
continue;
}
TreeNode node = m_nodes[nodeId];
if (!AABB.TestOverlap(node.AABB, segAABB))
{
continue;
}
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
node.AABB.GetCenterToOut(c);
node.AABB.GetExtentsToOut(h);
temp.Set(p1).SubLocal(c);
float separation = MathUtils.Abs(Vec2.Dot(v, temp)) - Vec2.Dot(absV, h);
if (separation > 0.0f)
{
continue;
}
if (node.Leaf)
{
subInput.P1.Set(input.P1);
subInput.P2.Set(input.P2);
subInput.MaxFraction = maxFraction;
float value = callback.RaycastCallback(subInput, nodeId);
if (value == 0.0f)
{
// The client has terminated the ray cast.
return;
}
if (value > 0.0f)
{
// Update segment bounding box.
maxFraction = value;
t.Set(p2).SubLocal(p1).MulLocal(maxFraction).AddLocal(p1);
Vec2.MinToOut(p1, t, segAABB.LowerBound);
Vec2.MaxToOut(p1, t, segAABB.UpperBound);
}
}
else
{
intStack.Push(node.Child1);
intStack.Push(node.Child2);
}
}
}
// Collision Detection in Interactive 3D Environments by Gino van den Bergen
// From Section 3.1.2
// x = s + a * r
// norm(x) = radius
public override bool raycast(RayCastOutput output, RayCastInput input, Transform transform,
int childIndex)
{
Vec2 inputp1 = input.p1;
Vec2 inputp2 = input.p2;
Rot tq = transform.q;
Vec2 tp = transform.p;
// Rot.mulToOutUnsafe(transform.q, m_p, position);
// position.addLocal(transform.p);
float positionx = tq.c*m_p.x - tq.s*m_p.y + tp.x;
float positiony = tq.s*m_p.x + tq.c*m_p.y + tp.y;
float sx = inputp1.x - positionx;
float sy = inputp1.y - positiony;
// float b = Vec2.dot(s, s) - m_radius * m_radius;
float b = sx*sx + sy*sy - m_radius*m_radius;
// Solve quadratic equation.
float rx = inputp2.x - inputp1.x;
float ry = inputp2.y - inputp1.y;
// float c = Vec2.dot(s, r);
// float rr = Vec2.dot(r, r);
float c = sx*rx + sy*ry;
float rr = rx*rx + ry*ry;
float sigma = c*c - rr*b;
// Check for negative discriminant and short segment.
if (sigma < 0.0f || rr < Settings.EPSILON)
{
return false;
}
// Find the point of intersection of the line with the circle.
float a = -(c + MathUtils.sqrt(sigma));
// Is the intersection point on the segment?
if (0.0f <= a && a <= input.maxFraction*rr)
{
a /= rr;
output.fraction = a;
output.normal.x = rx*a + sx;
output.normal.y = ry*a + sy;
output.normal.normalize();
return true;
}
return false;
}
public override bool raycast(RayCastOutput output, RayCastInput input, Transform xf, int childIndex)
{
Debug.Assert(childIndex < m_count);
EdgeShape edgeShape = pool0;
int i1 = childIndex;
int i2 = childIndex + 1;
if (i2 == m_count)
{
i2 = 0;
}
edgeShape.m_vertex1.set_Renamed(m_vertices[i1]);
edgeShape.m_vertex2.set_Renamed(m_vertices[i2]);
return edgeShape.raycast(output, input, xf, 0);
}
// p = p1 + t * d
// v = v1 + s * e
// p1 + t * d = v1 + s * e
// s * e - t * d = p1 - v1
public override bool raycast(RayCastOutput output, RayCastInput input, Transform xf, int childIndex)
{
float tempx, tempy;
Vec2 v1 = m_vertex1;
Vec2 v2 = m_vertex2;
Rot xfq = xf.q;
Vec2 xfp = xf.p;
// Put the ray into the edge's frame of reference.
// b2Vec2 p1 = b2MulT(xf.q, input.p1 - xf.p);
// b2Vec2 p2 = b2MulT(xf.q, input.p2 - xf.p);
tempx = input.p1.x - xfp.x;
tempy = input.p1.y - xfp.y;
float p1x = xfq.c*tempx + xfq.s*tempy;
float p1y = -xfq.s*tempx + xfq.c*tempy;
tempx = input.p2.x - xfp.x;
tempy = input.p2.y - xfp.y;
float p2x = xfq.c*tempx + xfq.s*tempy;
float p2y = -xfq.s*tempx + xfq.c*tempy;
float dx = p2x - p1x;
float dy = p2y - p1y;
// Vec2 normal = pool2.set(v2).subLocal(v1);
// normal.set(normal.y, -normal.x);
normal.x = v2.y - v1.y;
normal.y = v1.x - v2.x;
normal.normalize();
float normalx = normal.x;
float normaly = normal.y;
// q = p1 + t * d
// dot(normal, q - v1) = 0
// dot(normal, p1 - v1) + t * dot(normal, d) = 0
tempx = v1.x - p1x;
tempy = v1.y - p1y;
float numerator = normalx*tempx + normaly*tempy;
float denominator = normalx*dx + normaly*dy;
if (denominator == 0.0f)
{
return false;
}
float t = numerator/denominator;
if (t < 0.0f || 1.0f < t)
{
return false;
}
// Vec2 q = p1 + t * d;
float qx = p1x + t*dx;
float qy = p1y + t*dy;
// q = v1 + s * r
// s = dot(q - v1, r) / dot(r, r)
// Vec2 r = v2 - v1;
float rx = v2.x - v1.x;
float ry = v2.y - v1.y;
float rr = rx*rx + ry*ry;
if (rr == 0.0f)
{
return false;
}
tempx = qx - v1.x;
tempy = qy - v1.y;
// float s = Vec2.dot(pool5, r) / rr;
float s = (tempx*rx + tempy*ry)/rr;
if (s < 0.0f || 1.0f < s)
{
return false;
}
output.fraction = t;
if (numerator > 0.0f)
{
// output.normal = -b2Mul(xf.q, normal);
output.normal.x = -xfq.c*normal.x + xfq.s*normal.y;
output.normal.y = -xfq.s*normal.x - xfq.c*normal.y;
}
else
{
// output->normal = b2Mul(xf.q, normal);
output.normal.x = xfq.c*normal.x - xfq.s*normal.y;
output.normal.y = xfq.s*normal.x + xfq.c*normal.y;
}
return true;
}
/// <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="input">the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).</param>
/// <param name="callback">a callback class that is called for each proxy that is hit by the ray.</param>
public virtual void raycast(TreeRayCastCallback callback, RayCastInput input)
{
Vec2 p1 = input.p1;
Vec2 p2 = input.p2;
r.set_Renamed(p2).subLocal(p1);
Debug.Assert(r.lengthSquared() > 0f);
r.normalize();
// v is perpendicular to the segment.
Vec2.crossToOutUnsafe(1f, r, v);
absV.set_Renamed(v).absLocal();
// 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 segAABB = aabb;
// Vec2 t = p1 + maxFraction * (p2 - p1);
temp.set_Renamed(p2).subLocal(p1).mulLocal(maxFraction).addLocal(p1);
Vec2.minToOut(p1, temp, segAABB.lowerBound);
Vec2.maxToOut(p1, temp, segAABB.upperBound);
intStack.push(m_root);
while (intStack.Count > 0)
{
int nodeId = intStack.pop();
if (nodeId == TreeNode.NULL_NODE)
{
continue;
}
TreeNode node = m_nodes[nodeId];
if (!AABB.testOverlap(node.aabb, segAABB))
{
continue;
}
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
node.aabb.getCenterToOut(c);
node.aabb.getExtentsToOut(h);
temp.set_Renamed(p1).subLocal(c);
float separation = MathUtils.abs(Vec2.dot(v, temp)) - Vec2.dot(absV, h);
if (separation > 0.0f)
{
continue;
}
if (node.Leaf)
{
subInput.p1.set_Renamed(input.p1);
subInput.p2.set_Renamed(input.p2);
subInput.maxFraction = maxFraction;
float value_Renamed = callback.raycastCallback(subInput, nodeId);
if (value_Renamed == 0.0f)
{
// The client has terminated the ray cast.
return;
}
if (value_Renamed > 0.0f)
{
// Update segment bounding box.
maxFraction = value_Renamed;
t.set_Renamed(p2).subLocal(p1).mulLocal(maxFraction).addLocal(p1);
Vec2.minToOut(p1, t, segAABB.lowerBound);
Vec2.maxToOut(p1, t, segAABB.upperBound);
}
}
else
{
intStack.push(node.child1);
intStack.push(node.child2);
}
}
}
float RayCastCallback(ref RayCastInput input, int proxyid)
{
Actor actor = (Actor)_tree.GetUserData(proxyid);
RayCastOutput output;
bool hit = actor.aabb.RayCast(out output, ref input);
if (hit)
{
_rayCastOutput = output;
actor.fraction = output.fraction;
_rayActor = actor;
return output.fraction;
}
return input.maxFraction;
}
public override bool raycast(RayCastOutput output, RayCastInput input, Transform xf, int childIndex)
{
float xfqc = xf.q.c;
float xfqs = xf.q.s;
Vec2 xfp = xf.p;
float tempx, tempy;
// b2Vec2 p1 = b2MulT(xf.q, input.p1 - xf.p);
// b2Vec2 p2 = b2MulT(xf.q, input.p2 - xf.p);
tempx = input.p1.x - xfp.x;
tempy = input.p1.y - xfp.y;
float p1x = xfqc*tempx + xfqs*tempy;
float p1y = -xfqs*tempx + xfqc*tempy;
tempx = input.p2.x - xfp.x;
tempy = input.p2.y - xfp.y;
float p2x = xfqc*tempx + xfqs*tempy;
float p2y = -xfqs*tempx + xfqc*tempy;
float dx = p2x - p1x;
float dy = p2y - p1y;
float lower = 0, upper = input.maxFraction;
int index = -1;
for (int i = 0; i < m_count; ++i)
{
Vec2 normal = m_normals[i];
Vec2 vertex = m_vertices[i];
// p = p1 + a * d
// dot(normal, p - v) = 0
// dot(normal, p1 - v) + a * dot(normal, d) = 0
float tempxn = vertex.x - p1x;
float tempyn = vertex.y - p1y;
float numerator = normal.x*tempxn + normal.y*tempyn;
float denominator = normal.x*dx + normal.y*dy;
if (denominator == 0.0f)
{
if (numerator < 0.0f)
{
return false;
}
}
else
{
// Note: we want this predicate without division:
// lower < numerator / denominator, where denominator < 0
// Since denominator < 0, we have to flip the inequality:
// lower < numerator / denominator <==> denominator * lower >
// numerator.
if (denominator < 0.0f && numerator < lower*denominator)
{
// Increase lower.
// The segment enters this half-space.
lower = numerator/denominator;
index = i;
}
else if (denominator > 0.0f && numerator < upper*denominator)
{
// Decrease upper.
// The segment exits this half-space.
upper = numerator/denominator;
}
}
if (upper < lower)
{
return false;
}
}
Debug.Assert(0.0f <= lower && lower <= input.maxFraction);
if (index >= 0)
{
output.fraction = lower;
// normal = Mul(xf.R, m_normals[index]);
Vec2 normal = m_normals[index];
Vec2 outputNormal = output.normal;
outputNormal.x = xfqc*normal.x - xfqs*normal.y;
outputNormal.y = xfqs*normal.x + xfqc*normal.y;
output.normal = outputNormal;
return true;
}
return false;
}
/// <summary>
/// Cast a ray against a child shape.
/// </summary>
/// <param name="output">The ray-cast results.</param>
/// <param name="input">The ray-cast input parameters.</param>
/// <param name="transform">The transform to be applied to the shape.</param>
/// <param name="childIndex">The child shape index.</param>
/// <returns>True if the ray-cast hits the shape</returns>
public override bool RayCast(out RayCastOutput output, ref RayCastInput input,
ref Transform transform, int childIndex)
{
// p = p1 + t * d
// v = v1 + s * e
// p1 + t * d = v1 + s * e
// s * e - t * d = p1 - v1
output = new RayCastOutput();
// Put the ray into the edge's frame of reference.
Vector2 p1 = MathUtils.MultiplyT(ref transform.R, input.Point1 - transform.Position);
Vector2 p2 = MathUtils.MultiplyT(ref transform.R, input.Point2 - transform.Position);
Vector2 d = p2 - p1;
Vector2 v1 = _vertex1;
Vector2 v2 = _vertex2;
Vector2 e = v2 - v1;
Vector2 normal = new Vector2(e.Y, -e.X);
normal.Normalize();
// q = p1 + t * d
// dot(normal, q - v1) = 0
// dot(normal, p1 - v1) + t * dot(normal, d) = 0
float numerator = Vector2.Dot(normal, v1 - p1);
float denominator = Vector2.Dot(normal, d);
if (denominator == 0.0f)
{
return false;
}
float t = numerator / denominator;
if (t < 0.0f || 1.0f < t)
{
return false;
}
Vector2 q = p1 + t * d;
// q = v1 + s * r
// s = dot(q - v1, r) / dot(r, r)
Vector2 r = v2 - v1;
float rr = Vector2.Dot(r, r);
if (rr == 0.0f)
{
return false;
}
float s = Vector2.Dot(q - v1, r) / rr;
if (s < 0.0f || 1.0f < s)
{
return false;
}
output.Fraction = t;
if (numerator > 0.0f)
{
output.Normal = -normal;
}
else
{
output.Normal = normal;
}
return true;
}
public override bool Raycast(RayCastOutput output, RayCastInput input, Transform xf, int childIndex)
{
Vec2 p1 = pool1;
Vec2 p2 = pool2;
Vec2 d = pool3;
Vec2 temp = pool4;
p1.Set(input.P1).SubLocal(xf.P);
Rot.MulTrans(xf.Q, p1, p1);
p2.Set(input.P2).SubLocal(xf.P);
Rot.MulTrans(xf.Q, p2, p2);
d.Set(p2).SubLocal(p1);
// if (count == 2) {
// } else {
float lower = 0, upper = input.MaxFraction;
int index = -1;
for (int i = 0; i < VertexCount; ++i)
{
// p = p1 + a * d
// dot(normal, p - v) = 0
// dot(normal, p1 - v) + a * dot(normal, d) = 0
temp.Set(Vertices[i]).SubLocal(p1);
float numerator = Vec2.Dot(Normals[i], temp);
float denominator = Vec2.Dot(Normals[i], d);
if (denominator == 0.0f)
{
if (numerator < 0.0f)
{
return false;
}
}
else
{
// Note: we want this predicate without division:
// lower < numerator / denominator, where denominator < 0
// Since denominator < 0, we have to flip the inequality:
// lower < numerator / denominator <==> denominator * lower >
// numerator.
if (denominator < 0.0f && numerator < lower * denominator)
{
// Increase lower.
// The segment enters this half-space.
lower = numerator / denominator;
index = i;
}
else if (denominator > 0.0f && numerator < upper * denominator)
{
// Decrease upper.
// The segment exits this half-space.
upper = numerator / denominator;
}
}
if (upper < lower)
{
return false;
}
}
Debug.Assert(0.0f <= lower && lower <= input.MaxFraction);
if (index >= 0)
{
output.Fraction = lower;
Rot.MulToOutUnsafe(xf.Q, Normals[index], output.Normal);
// normal = Mul(xf.R, m_normals[index]);
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="input">the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).</param>
/// <param name="callback">a callback class that is called for each proxy that is hit by the ray.</param>
public void Raycast(ITreeRayCastCallback callback, RayCastInput input)
{
m_tree.Raycast(callback, input);
}
public void RayCast(Func <RayCastInput, int, float> callback, ref RayCastInput input)
{
this._quadTree.RayCast(TransformRayCallback(callback), ref input);
}
