public bool RayCast(out b2RayCastOutput castOutput, b2RayCastInput input, Transform transform, int childIndex)
{
Vector2D position = transform.Position + b2Mat22.Mul(transform.Rotation, m_p);
Vector2D s = input.p1 - position;
float b = Vector2D.Dot(s, s) - Radius * Radius;
castOutput.fraction = 0.0f;
castOutput.Normal = null;
// Solve quadratic equation
Vector2D r = input.p2 - input.p1;
float c = Vector2D.Dot(s, r);
float rr = Vector2D.Dot(r, r);
float sigma = c * c - rr * b;
if (sigma < 0.0f || rr < float.Epsilon)
{
return(false);
}
float a = -(float)(c + System.Math.Sqrt(sigma));
if (0.0f <= a && a <= input.maxFraction * rr)
{
a /= rr;
castOutput.fraction = a;
castOutput.Normal = s + r * a;
castOutput.Normal = castOutput.Normal.Normal();
return(true);
}
return(false);
}
private void RayCast()
{
m_rayActor = null;
b2RayCastInput input = m_rayCastInput;
// Ray cast against the dynamic tree.
m_tree.RayCast(this, input);
// Brute force ray cast.
Actor bruteActor = null;
b2RayCastOutput bruteOutput = new b2RayCastOutput();
for (int i = 0; i < e_actorCount; ++i)
{
if (m_actors[i].proxyId == b2_nullNode)
{
continue;
}
b2RayCastOutput output;
bool hit = m_actors[i].aabb.RayCast(out output, input);
if (hit)
{
bruteActor = m_actors[i];
bruteOutput = output;
input.maxFraction = output.fraction;
}
}
if (bruteActor != null)
{
Debug.Assert(bruteOutput.fraction == m_rayCastOutput.fraction);
}
}
/// Implement b2Shape.
// 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(b2RayCastOutput output, b2RayCastInput input, b2Transform transform, int childIndex)
{
b2Vec2 position = transform.p + Utils.b2Mul(transform.q, m_p);
b2Vec2 s = input.p1 - position;
float b = Utils.b2Dot(s, s) - m_radius * m_radius;
// Solve quadratic equation.
b2Vec2 r = input.p2 - input.p1;
float c = Utils.b2Dot(s, r);
float rr = Utils.b2Dot(r, r);
float sigma = c * c - rr * b;
// Check for negative discriminant and short segment.
if (sigma < 0.0f || rr < float.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;
output.normal = s + a * r;
output.normal.Normalize();
return(true);
}
return(false);
}
public bool RayCast(b2RayCastOutput output, b2RayCastInput input, int childIndex)
{
bool ret = Box2DPINVOKE.b2Fixture_RayCast(swigCPtr, b2RayCastOutput.getCPtr(output), b2RayCastInput.getCPtr(input), childIndex);
if (Box2DPINVOKE.SWIGPendingException.Pending)
{
throw Box2DPINVOKE.SWIGPendingException.Retrieve();
}
return(ret);
}
public override bool RayCast(b2RayCastOutput output, b2RayCastInput input, b2Transform transform, int childIndex)
{
bool ret = Box2DPINVOKE.b2CircleShape_RayCast(swigCPtr, b2RayCastOutput.getCPtr(output), b2RayCastInput.getCPtr(input), b2Transform.getCPtr(transform), childIndex);
if (Box2DPINVOKE.SWIGPendingException.Pending)
{
throw Box2DPINVOKE.SWIGPendingException.Retrieve();
}
return(ret);
}
public bool RayCast(b2RayCastOutput output, b2RayCastInput input)
{
bool ret = Box2dPINVOKE.b2AABB_RayCast(swigCPtr, b2RayCastOutput.getCPtr(output), b2RayCastInput.getCPtr(input));
if (Box2dPINVOKE.SWIGPendingException.Pending)
{
throw Box2dPINVOKE.SWIGPendingException.Retrieve();
}
return(ret);
}
/**
* @inheritDoc
*/
public override bool RayCast(b2RayCastOutput output, b2RayCastInput input, b2Transform transform)
{
//b2Vec2 position = transform.position + b2Mul(transform.R, m_p);
b2Mat22 tMat = transform.R;
float positionX = transform.position.x + (tMat.col1.x * m_p.x + tMat.col2.x * m_p.y);
float positionY = transform.position.y + (tMat.col1.y * m_p.x + tMat.col2.y * m_p.y);
//b2Vec2 s = input.p1 - position;
float sX = input.p1.x - positionX;
float sY = input.p1.y - positionY;
//float32 b = b2Dot(s, s) - m_radius * m_radius;
float b = (sX * sX + sY * sY) - m_radius * m_radius;
/*// Does the segment start inside the circle?
* if (b < 0.0)
* {
* output.fraction = 0;
* output.hit = e_startsInsideCollide;
* return;
* }*/
// Solve quadratic equation.
//b2Vec2 r = input.p2 - input.p1;
float rX = input.p2.x - input.p1.x;
float rY = input.p2.y - input.p1.y;
//float32 c = b2Dot(s, r);
float c = (sX * rX + sY * rY);
//float32 rr = b2Dot(r, r);
float rr = (rX * rX + rY * rY);
float sigma = c * c - rr * b;
// Check for negative discriminant and short segment.
if (sigma < 0.0 || rr < float.MinValue)
{
return(false);
}
// Find the point of intersection of the line with the circle.
float a = -(c + Mathf.Sqrt(sigma));
// Is the intersection point on the segment?
if (0.0f <= a && a <= input.maxFraction * rr)
{
a /= rr;
output.fraction = a;
// manual inline of: output.normal = s + a * r;
output.normal.x = sX + a * rX;
output.normal.y = sY + a * rY;
output.normal.Normalize();
return(true);
}
return(false);
}
// 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(out b2RayCastOutput output, b2RayCastInput input,
b2Transform transform, int childIndex)
{
output = b2RayCastOutput.Zero;
b2Vec2 tx = b2Math.b2Mul(transform.q, m_p);
// b2Vec2 position = transform.p + tx;
b2Vec2 position;
position.x = transform.p.x + tx.x;
position.y = transform.p.y + tx.y;
// b2Vec2 s = input.p1 - position;
b2Vec2 s;
s.x = input.p1.x - position.x;
s.y = input.p1.y - position.y;
float b = s.LengthSquared - m_radius * m_radius;
// Solve quadratic equation.
b2Vec2 r;
r.x = input.p2.x - input.p1.x;
r.y = input.p2.y - input.p1.y;
// b2Vec2 r = input.p2 - input.p1;
float c = s.x * r.x + s.y * r.y; // b2Math.b2Dot(ref s, ref r);
float rr = r.LengthSquared; // b2Math.b2Dot(r, r);
float sigma = c * c - rr * b;
// Check for negative discriminant and short segment.
if (sigma < 0.0f || rr < b2Settings.b2_epsilon)
{
return(false);
}
// Find the point of intersection of the line with the circle.
float a = -(c + b2Math.b2Sqrt(sigma));
// Is the intersection point on the segment?
if (0.0f <= a && a <= input.maxFraction * rr)
{
a /= rr;
output.fraction = a;
output.normal.x = s.x + a * r.x;
output.normal.y = s.y + a * r.y;
// output.normal = s + a * r;
output.normal.Normalize();
return(true);
}
return(false);
}
// 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(out b2RayCastOutput output, b2RayCastInput input,
b2Transform transform, int childIndex)
{
output = b2RayCastOutput.Zero;
b2Vec2 tx = b2Math.b2Mul(transform.q, m_p);
// b2Vec2 position = transform.p + tx;
b2Vec2 position;
position.x = transform.p.x + tx.x;
position.y = transform.p.y + tx.y;
// b2Vec2 s = input.p1 - position;
b2Vec2 s;
s.x = input.p1.x - position.x;
s.y = input.p1.y - position.y;
float b = s.LengthSquared - m_radius * m_radius;
// Solve quadratic equation.
b2Vec2 r;
r.x = input.p2.x - input.p1.x;
r.y = input.p2.y - input.p1.y;
// b2Vec2 r = input.p2 - input.p1;
float c = s.x * r.x + s.y * r.y; // b2Math.b2Dot(ref s, ref r);
float rr = r.LengthSquared; // b2Math.b2Dot(r, r);
float sigma = c * c - rr * b;
// Check for negative discriminant and short segment.
if (sigma < 0.0f || rr < b2Settings.b2_epsilon)
{
return false;
}
// Find the point of intersection of the line with the circle.
float a = -(c + b2Math.b2Sqrt(sigma));
// Is the intersection point on the segment?
if (0.0f <= a && a <= input.maxFraction * rr)
{
a /= rr;
output.fraction = a;
output.normal.x = s.x + a * r.x;
output.normal.y = s.y + a * r.y;
// output.normal = s + a * r;
output.normal.Normalize();
return true;
}
return false;
}
public float RayCastCallback(ref b2RayCastInput input, int proxyId)
{
Actor actor = (Actor)m_tree.GetUserData(proxyId);
b2RayCastOutput output = new b2RayCastOutput();
bool hit = actor.aabb.RayCast(out output, input);
if (hit)
{
m_rayCastOutput = output;
m_rayActor = actor;
m_rayActor.fraction = output.fraction;
return(output.fraction);
}
return(input.maxFraction);
}
/**
* @inheritDoc
*/
public override bool RayCast(b2RayCastOutput output, b2RayCastInput input, b2Transform transform)
{
b2Mat22 tMat;
float rX = input.p2.x - input.p1.x;
float rY = input.p2.y - input.p1.y;
//b2Vec2 v1 = b2Mul(transform, m_v1);
tMat = transform.R;
float v1X = transform.position.x + (tMat.col1.x * m_v1.x + tMat.col2.x * m_v1.y);
float v1Y = transform.position.y + (tMat.col1.y * m_v1.x + tMat.col2.y * m_v1.y);
//b2Vec2 n = b2Cross(d, 1.0);
float nX = transform.position.y + (tMat.col1.y * m_v2.x + tMat.col2.y * m_v2.y) - v1Y;
float nY = -(transform.position.x + (tMat.col1.x * m_v2.x + tMat.col2.x * m_v2.y) - v1X);
float k_slop = 100.0f * float.MinValue;
float denom = -(rX * nX + rY * nY);
// Cull back facing collision and ignore parallel segments.
if (denom > k_slop)
{
// Does the segment intersect the infinite line associated with this segment?
float bX = input.p1.x - v1X;
float bY = input.p1.y - v1Y;
float a = (bX * nX + bY * nY);
if (0.0f <= a && a <= input.maxFraction * denom)
{
float mu2 = -rX * bY + rY * bX;
// Does the segment intersect this segment?
if (-k_slop * denom <= mu2 && mu2 <= denom * (1.0f + k_slop))
{
a /= denom;
output.fraction = a;
float nLen = Mathf.Sqrt(nX * nX + nY * nY);
output.normal.x = nX / nLen;
output.normal.y = nY / nLen;
return(true);
}
}
}
return(false);
}
/// Implement b2Shape.
public override bool RayCast(b2RayCastOutput output, b2RayCastInput input, b2Transform xf, int childIndex)
{
Debug.Assert(childIndex < m_count);
b2EdgeShape edgeShape = new b2EdgeShape();
int i1 = childIndex;
int i2 = childIndex + 1;
if (i2 == m_count)
{
i2 = 0;
}
edgeShape.m_vertex1 = m_vertices[i1];
edgeShape.m_vertex2 = m_vertices[i2];
return(edgeShape.RayCast(output, input, xf, 0));
}
public override bool RayCast(out b2RayCastOutput output, b2RayCastInput input, ref b2Transform xf, int childIndex)
{
b2EdgeShape edgeShape = new b2EdgeShape();
output = b2RayCastOutput.Zero;
int i1 = childIndex;
int i2 = childIndex + 1;
if (i2 == Count)
{
i2 = 0;
}
edgeShape.Vertex1 = Vertices[i1];
edgeShape.Vertex2 = Vertices[i2];
b2RayCastOutput co = b2RayCastOutput.Zero;
bool b = edgeShape.RayCast(out co, input, ref xf, 0);
output = co;
return(b);
}
// Collision Detection in Interactive 3D Environments by Gino van den Bergen
// From Section 3.1.2
// x = s + a * r
// norm(x) = radius
public virtual bool RayCast(out b2RayCastOutput output, b2RayCastInput input,
b2Transform transform, int childIndex)
{
output = b2RayCastOutput.Zero;
b2Vec2 position = transform.p + b2Math.b2Mul(transform.q, m_p);
b2Vec2 s = input.p1 - position;
float b = b2Math.b2Dot(s, s) - m_radius * m_radius;
// Solve quadratic equation.
b2Vec2 r = input.p2 - input.p1;
float c = b2Math.b2Dot(s, r);
float rr = b2Math.b2Dot(r, r);
float sigma = c * c - rr * b;
// Check for negative discriminant and short segment.
if (sigma < 0.0f || rr < b2Settings.b2_epsilon)
{
return false;
}
// Find the point of intersection of the line with the circle.
float a = -(c + b2Math.b2Sqrt(sigma));
// Is the intersection point on the segment?
if (0.0f <= a && a <= input.maxFraction * rr)
{
a /= rr;
output.fraction = a;
output.normal = s + a * r;
output.normal.Normalize();
return true;
}
return false;
}
/**
* @inheritDoc
*/
public override bool RayCast(b2RayCastOutput output, b2RayCastInput input, b2Transform transform)
{
float lower = 0.0f;
float upper = input.maxFraction;
float tX;
float tY;
b2Mat22 tMat;
b2Vec2 tVec;
// Put the ray into the polygon's frame of reference. (AS3 Port Manual inlining follows)
//b2Vec2 p1 = b2MulT(transform.R, segment.p1 - transform.position);
tX = input.p1.x - transform.position.x;
tY = input.p1.y - transform.position.y;
tMat = transform.R;
float p1X = (tX * tMat.col1.x + tY * tMat.col1.y);
float p1Y = (tX * tMat.col2.x + tY * tMat.col2.y);
//b2Vec2 p2 = b2MulT(transform.R, segment.p2 - transform.position);
tX = input.p2.x - transform.position.x;
tY = input.p2.y - transform.position.y;
tMat = transform.R;
float p2X = (tX * tMat.col1.x + tY * tMat.col1.y);
float p2Y = (tX * tMat.col2.x + tY * tMat.col2.y);
//b2Vec2 d = p2 - p1;
float dX = p2X - p1X;
float dY = p2Y - p1Y;
int index = -1;
for (int i = 0; i < m_vertexCount; ++i)
{
// p = p1 + a * d
// dot(normal, p - v) = 0
// dot(normal, p1 - v) + a * dot(normal, d) = 0
//float32 numerator = b2Dot(m_normals[i], m_vertices[i] - p1);
tVec = m_vertices[i];
tX = tVec.x - p1X;
tY = tVec.y - p1Y;
tVec = m_normals[i];
float numerator = (tVec.x * tX + tVec.y * tY);
//float32 denominator = b2Dot(m_normals[i], d);
float denominator = (tVec.x * dX + tVec.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 - float.MinValue)
* {
* return false;
* }*/
//改
if (upper < lower)
{
return(false);
}
}
//b2Settings.b2Assert(0.0 <= lower && lower <= input.maxLambda);
if (index >= 0)
{
output.fraction = lower;
//output.normal = b2Mul(transform.R, m_normals[index]);
tMat = transform.R;
tVec = m_normals[index];
output.normal.x = (tMat.col1.x * tVec.x + tMat.col2.x * tVec.y);
output.normal.y = (tMat.col1.y * tVec.x + tMat.col2.y * tVec.y);
return(true);
}
return(false);
}
// p = p1 + t * d
// v = v1 + s * e
// p1 + t * d = v1 + s * e
// s * e - t * d = p1 - v1
public override bool RayCast(out b2RayCastOutput output, b2RayCastInput input,
b2Transform xf, int childIndex)
{
output = b2RayCastOutput.Zero;
// Put the ray into the edge's frame of reference.
b2Vec2 p1 = b2Math.b2MulT(xf.q, input.p1 - xf.p);
b2Vec2 p2 = b2Math.b2MulT(xf.q, input.p2 - xf.p);
b2Vec2 d = p2 - p1;
b2Vec2 v1 = m_vertex1;
b2Vec2 v2 = m_vertex2;
b2Vec2 e = v2 - v1;
b2Vec2 normal = b2Vec2.Zero; // new b2Vec2(e.y, -e.x);
normal.m_x = e.m_y;
normal.m_y = -e.m_x;
normal.Normalize();
// q = p1 + t * d
// dot(normal, q - v1) = 0
// dot(normal, p1 - v1) + t * dot(normal, d) = 0
b2Vec2 diff = v1 - p1;
float numerator = b2Math.b2Dot(ref normal, ref diff);
float denominator = b2Math.b2Dot(ref normal, ref d);
if (denominator == 0.0f)
{
return(false);
}
float t = numerator / denominator;
if (t < 0.0f || input.maxFraction < t)
{
return(false);
}
b2Vec2 q = p1 + t * d;
// q = v1 + s * r
// s = dot(q - v1, r) / dot(r, r)
b2Vec2 r = v2 - v1;
float rr = r.LengthSquared; // b2Math.b2Dot(r, r);
if (rr == 0.0f)
{
return(false);
}
diff = q - v1;
float s = b2Math.b2Dot(ref diff, ref 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(out b2RayCastOutput output, b2RayCastInput input, ref b2Transform xf, int childIndex)
{
output = b2RayCastOutput.Zero;
// Put the ray into the polygon's frame of reference.
b2Vec2 p1 = b2Math.b2MulT(xf.q, input.p1 - xf.p);
b2Vec2 p2 = b2Math.b2MulT(xf.q, input.p2 - xf.p);
b2Vec2 d = p2 - p1;
float lower = 0.0f, upper = input.maxFraction;
int index = -1;
for (int i = 0; i < m_vertexCount; ++i)
{
// p = p1 + a * d
// dot(normal, p - v) = 0
// dot(normal, p1 - v) + a * dot(normal, d) = 0
float numerator = b2Math.b2Dot(Normals[i], Vertices[i] - p1);
float denominator = b2Math.b2Dot(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 = b2Math.b2Mul(xf.q, Normals[index]);
return true;
}
return false;
}
internal static global::System.Runtime.InteropServices.HandleRef getCPtr(b2RayCastOutput obj)
{
return((obj == null) ? new global::System.Runtime.InteropServices.HandleRef(null, global::System.IntPtr.Zero) : obj.swigCPtr);
}
public virtual bool RayCast(out b2RayCastOutput output, b2RayCastInput input, int childIndex)
{
return(m_shape.RayCast(out output, input, m_body.Transform, childIndex));
}
public override bool RayCast(out b2RayCastOutput output, b2RayCastInput input,
b2Transform xf, int childIndex)
{
b2EdgeShape edgeShape = new b2EdgeShape();
output = b2RayCastOutput.Zero;
int i1 = childIndex;
int i2 = childIndex + 1;
if (i2 == m_count)
{
i2 = 0;
}
edgeShape.Vertex1 = m_vertices[i1];
edgeShape.Vertex2 = m_vertices[i2];
b2RayCastOutput co = b2RayCastOutput.Zero;
bool b = edgeShape.RayCast(out co, input, xf, 0);
output = co;
return (b);
}
/// Cast a ray against a child shape.
/// @param output the ray-cast results.
/// @param input the ray-cast input parameters.
/// @param transform the transform to be applied to the shape.
/// @param childIndex the child shape index
public abstract bool RayCast(out b2RayCastOutput output, b2RayCastInput input,
b2Transform transform, int childIndex);
/// Cast a ray against a child shape. /// @param output the ray-cast results. /// @param input the ray-cast input parameters. /// @param transform the transform to be applied to the shape. /// @param childIndex the child shape index public abstract bool RayCast(out b2RayCastOutput output, b2RayCastInput input, ref b2Transform transform, int childIndex);
/**
* Perform a ray cast against this shape.
* @param output the ray-cast results.
* @param input the ray-cast input parameters.
*/
public bool RayCast(b2RayCastOutput output, b2RayCastInput input)
{
return(m_shape.RayCast(output, input, m_body.GetTransform()));
}
// p = p1 + t * d
// v = v1 + s * e
// p1 + t * d = v1 + s * e
// s * e - t * d = p1 - v1
public override bool RayCast(out b2RayCastOutput output, b2RayCastInput input,
b2Transform xf, int childIndex)
{
output = b2RayCastOutput.Zero;
// Put the ray into the edge's frame of reference.
b2Vec2 p1 = b2Math.b2MulT(xf.q, input.p1 - xf.p);
b2Vec2 p2 = b2Math.b2MulT(xf.q, input.p2 - xf.p);
b2Vec2 d = p2 - p1;
b2Vec2 v1 = m_vertex1;
b2Vec2 v2 = m_vertex2;
b2Vec2 e = v2 - v1;
b2Vec2 normal = new b2Vec2(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 = b2Math.b2Dot(normal, v1 - p1);
float denominator = b2Math.b2Dot(normal, d);
if (denominator == 0.0f)
{
return false;
}
float t = numerator / denominator;
if (t < 0.0f || input.maxFraction < t)
{
return false;
}
b2Vec2 q = p1 + t * d;
// q = v1 + s * r
// s = dot(q - v1, r) / dot(r, r)
b2Vec2 r = v2 - v1;
float rr = b2Math.b2Dot(r, r);
if (rr == 0.0f)
{
return false;
}
float s = b2Math.b2Dot(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;
}
/// Cast a ray against this shape.
/// @param output the ray-cast results.
/// @param input the ray-cast input parameters.
public bool RayCast(b2RayCastOutput output, b2RayCastInput input, int childIndex)
{
return(m_shape.RayCast(output, input, m_body.GetTransform(), childIndex));
}
/**
* Cast a ray against this shape.
* @param output the ray-cast results.
* @param input the ray-cast input parameters.
* @param transform the transform to be applied to the shape.
*/
virtual public bool RayCast(b2RayCastOutput output, b2RayCastInput input, b2Transform transform)
{
return false;
}
// From Real-time Collision Detection, p179.
public bool RayCast(b2RayCastOutput output, b2RayCastInput input)
{
float tmin = -float.MaxValue;
float tmax = float.MaxValue;
b2Vec2 p = new b2Vec2(input.p1);
b2Vec2 d = input.p2 - input.p1;
b2Vec2 absD = Utils.b2Abs(d);
b2Vec2 normal = new b2Vec2();
for (int i = 0; i < 2; ++i)
{
if (absD[i] < float.Epsilon)
{
// Parallel.
if (p[i] < lowerBound[i] || upperBound[i] < p[i])
{
return(false);
}
}
else
{
float inv_d = 1.0f / d[i];
float t1 = (lowerBound[i] - p[i]) * inv_d;
float t2 = (upperBound[i] - p[i]) * inv_d;
// Sign of the normal vector.
float s = -1.0f;
if (t1 > t2)
{
Utils.b2Swap(ref t1, ref t2);
s = 1.0f;
}
// Push the min up
if (t1 > tmin)
{
normal.SetZero();
normal[i] = s;
tmin = t1;
}
// Pull the max down
tmax = Utils.b2Min(tmax, t2);
if (tmin > tmax)
{
return(false);
}
}
}
// Does the ray start inside the box?
// Does the ray intersect beyond the max fraction?
if (tmin < 0.0f || input.maxFraction < tmin)
{
return(false);
}
// Intersection.
output.fraction = tmin;
output.normal = normal;
return(true);
}
public override bool RayCast(out b2RayCastOutput output, b2RayCastInput input, ref b2Transform xf, int childIndex)
{
output = b2RayCastOutput.Zero;
// Put the ray into the polygon's frame of reference.
b2Vec2 p1 = b2Math.b2MulT(xf.q, input.p1 - xf.p);
b2Vec2 p2 = b2Math.b2MulT(xf.q, input.p2 - xf.p);
b2Vec2 d = p2 - p1;
float lower = 0.0f, upper = input.maxFraction;
int index = -1;
for (int i = 0; i < m_vertexCount; ++i)
{
// p = p1 + a * d
// dot(normal, p - v) = 0
// dot(normal, p1 - v) + a * dot(normal, d) = 0
float numerator = b2Math.b2Dot(Normals[i], Vertices[i] - p1);
float denominator = b2Math.b2Dot(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 = b2Math.b2Mul(xf.q, Normals[index]);
return(true);
}
return(false);
}
/// Implement b2Shape.
// p = p1 + t * d
// v = v1 + s * e
// p1 + t * d = v1 + s * e
// s * e - t * d = p1 - v1
public override bool RayCast(b2RayCastOutput output, b2RayCastInput input, b2Transform xf, int childIndex)
{
// Put the ray into the edge's frame of reference.
b2Vec2 p1 = Utils.b2MulT(xf.q, input.p1 - xf.p);
b2Vec2 p2 = Utils.b2MulT(xf.q, input.p2 - xf.p);
b2Vec2 d = p2 - p1;
b2Vec2 v1 = new b2Vec2(m_vertex1);
b2Vec2 v2 = new b2Vec2(m_vertex2);
b2Vec2 e = v2 - v1;
b2Vec2 normal = new b2Vec2(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 = Utils.b2Dot(normal, v1 - p1);
float denominator = Utils.b2Dot(normal, d);
if (denominator == 0.0f)
{
return(false);
}
float t = numerator / denominator;
if (t < 0.0f || input.maxFraction < t)
{
return(false);
}
b2Vec2 q = p1 + t * d;
// q = v1 + s * r
// s = dot(q - v1, r) / dot(r, r)
b2Vec2 r = v2 - v1;
float rr = Utils.b2Dot(r, r);
if (rr == 0.0f)
{
return(false);
}
float s = Utils.b2Dot(q - v1, r) / rr;
if (s < 0.0f || 1.0f < s)
{
return(false);
}
output.fraction = t;
if (numerator > 0.0f)
{
output.normal = -Utils.b2Mul(xf.q, normal);
}
else
{
output.normal = Utils.b2Mul(xf.q, normal);
}
return(true);
}
