/// <summary> Implement Shape.
/// </summary>
public override bool RayCast(out RayCastOutput output, ref RayCastInput input,
ref Transform transform, int childIndex)
{
Debug.Assert(childIndex < _count);
int i1 = childIndex;
int i2 = childIndex + 1;
if (i2 == _count)
{
i2 = 0;
}
s_edgeShape._vertex1 = _vertices[i1];
s_edgeShape._vertex2 = _vertices[i2];
return(s_edgeShape.RayCast(out output, ref input, ref transform, 0));
}
/// <summary>
/// Collision Detection in Interactive 3D Environments by Gino van den Bergen
/// From Section 3.1.2
/// x = s + a * r
/// norm(x) = radius
/// </summary>
/// <param name="output"></param>
/// <param name="input"></param>
/// <param name="transform"></param>
/// <returns></returns>
public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform)
{
output = new RayCastOutput();
Vector2 position = transform.Position + MathUtils.Multiply(ref transform.R, _p);
Vector2 s = input.p1 - position;
float b = Vector2.Dot(s, s) - _radius * _radius;
// Solve quadratic equation.
Vector2 r = input.p2 - input.p1;
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.b2_epsilon)
{
return(false);
}
// Find the point of intersection of the line with the circle.
float a = -(c + (float)Math.Sqrt((double)sigma));
// Is the intersection point on the segment?
if (0.0f <= a && a <= input.maxFraction * rr)
{
a /= rr;
output.fraction = a;
Vector2 norm = (s + a * r);
norm.Normalize();
output.normal = norm;
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 RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex)
{
output = new RayCastOutput();
Vector2 position = transform.Position + MathUtils.Multiply(ref transform.R, _p);
Vector2 s = input.p1 - position;
float b = Vector2.Dot(s, s) - _radius * _radius;
// Solve quadratic equation.
Vector2 r = input.p2 - input.p1;
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.b2_epsilon)
{
return false;
}
// Find the point of intersection of the line with the circle.
float a = -(c + (float)Math.Sqrt((double)sigma));
// Is the intersection point on the segment?
if (0.0f <= a && a <= input.maxFraction * rr)
{
a /= rr;
output.fraction = a;
Vector2 norm = (s + a * r);
norm.Normalize();
output.normal = norm;
return true;
}
return false;
}
/// <summary>
/// From Real-time Collision Detection, p179.
/// </summary>
/// <param name="output"></param>
/// <param name="input"></param>
/// <returns></returns>
public bool RayCast(out RayCastOutput output, ref RayCastInput input)
{
output = new RayCastOutput();
float tmin = -Settings.b2_maxFloat;
float tmax = Settings.b2_maxFloat;
Vector2 p = input.p1;
Vector2 d = input.p2 - input.p1;
Vector2 absD = MathUtils.Abs(d);
Vector2 normal = Vector2.Zero;
for (int i = 0; i < 2; ++i)
{
float absD_i = i == 0 ? absD.X : absD.Y;
float lowerBound_i = i == 0 ? lowerBound.X : lowerBound.Y;
float upperBound_i = i == 0 ? upperBound.X : upperBound.Y;
float p_i = i == 0 ? p.X : p.Y;
if (absD_i < Settings.b2_epsilon)
{
// Parallel.
if (p_i < lowerBound_i || upperBound_i < p_i)
{
return false;
}
}
else
{
float d_i = i == 0 ? d.X : d.Y;
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)
{
MathUtils.Swap<float>(ref t1, ref t2);
s = 1.0f;
}
// Push the min up
if (t1 > tmin)
{
if (i == 0)
{
normal.X = s;
}
else
{
normal.Y = s;
}
tmin = t1;
}
// Pull the max down
tmax = Math.Min(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 RayCastOutput output, ref RayCastInput input, ref Transform xf, int childIndex)
{
output = new RayCastOutput();
// Put the ray into the polygon's frame of reference.
Vector2 p1 = MathUtils.MultiplyT(ref xf.R, input.p1 - xf.Position);
Vector2 p2 = MathUtils.MultiplyT(ref xf.R, input.p2 - xf.Position);
Vector2 d = p2 - p1;
if (_vertexCount == 2)
{
Vector2 v1 = _vertices[0];
Vector2 v2 = _vertices[1];
Vector2 normal = _normals[0];
// 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);
}
else
{
float lower = 0.0f, 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
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.Multiply(ref xf.R, _normals[index]);
return(true);
}
}
return(false);
}
public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform xf)
{
output = new RayCastOutput();
// Put the ray into the polygon's frame of reference.
Vector2 p1 = MathUtils.MultiplyT(ref xf.R, input.p1 - xf.Position);
Vector2 p2 = MathUtils.MultiplyT(ref xf.R, input.p2 - xf.Position);
Vector2 d = p2 - p1;
if (_vertexCount == 2)
{
Vector2 v1 = _vertices[0];
Vector2 v2 = _vertices[1];
Vector2 normal = _normals[0];
// 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;
}
else
{
float lower = 0.0f, 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
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;
}
}
if (upper < lower - Settings.b2_epsilon)
{
return false;
}
}
Debug.Assert(0.0f <= lower && lower <= input.maxFraction);
if (index >= 0)
{
output.fraction = lower;
output.normal = MathUtils.Multiply(ref xf.R, _normals[index]);
return true;
}
}
return false;
}
/// Implement Shape.
///
// 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 RayCastOutput output, ref RayCastInput input,
ref Transform transform, int childIndex)
{
output = new RayCastOutput();
// Put the ray into the edge's frame of reference.
Vector2 p1 = MathUtils.MultiplyT(ref transform.R, input.p1 - transform.Position);
Vector2 p2 = MathUtils.MultiplyT(ref transform.R, input.p2 - 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);
}
/// <summary> /// Cast a ray against this shape. /// </summary> /// <param name="output">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> /// <returns></returns> public abstract bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform);
/// Cast a ray against this Shape.
/// @param output the ray-cast results.
/// @param input the ray-cast input parameters.
public bool RayCast(out RayCastOutput output, ref RayCastInput input, int childIndex)
{
Transform xf;
_body.GetTransform(out xf);
return _shape.RayCast(out output, ref input, ref xf, childIndex);
}
/// Implement Shape.
public override bool RayCast(out RayCastOutput output, ref RayCastInput input,
ref Transform transform, int childIndex)
{
Debug.Assert(childIndex < _count);
int i1 = childIndex;
int i2 = childIndex + 1;
if (i2 == _count)
{
i2 = 0;
}
s_edgeShape._vertex1 = _vertices[i1];
s_edgeShape._vertex2 = _vertices[i2];
return s_edgeShape.RayCast(out output, ref input, ref transform, 0);
}
/// Implement Shape.
///
// 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 RayCastOutput output, ref RayCastInput input,
ref Transform transform, int childIndex)
{
output = new RayCastOutput();
// Put the ray into the edge's frame of reference.
Vector2 p1 = MathUtils.MultiplyT(ref transform.R, input.p1 - transform.Position);
Vector2 p2 = MathUtils.MultiplyT(ref transform.R, input.p2 - 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;
}
/// 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 RayCastOutput output, ref RayCastInput input, ref Transform 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 RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex);
/// <summary>
/// From Real-time Collision Detection, p179.
/// </summary>
/// <param name="output"></param>
/// <param name="input"></param>
/// <returns></returns>
public bool RayCast(out RayCastOutput output, ref RayCastInput input)
{
output = new RayCastOutput();
float tmin = -Settings.b2_maxFloat;
float tmax = Settings.b2_maxFloat;
Vector2 p = input.p1;
Vector2 d = input.p2 - input.p1;
Vector2 absD = MathUtils.Abs(d);
Vector2 normal = Vector2.Zero;
for (int i = 0; i < 2; ++i)
{
float absD_i = i == 0 ? absD.X : absD.Y;
float lowerBound_i = i == 0 ? lowerBound.X : lowerBound.Y;
float upperBound_i = i == 0 ? upperBound.X : upperBound.Y;
float p_i = i == 0 ? p.X : p.Y;
if (absD_i < Settings.b2_epsilon)
{
// Parallel.
if (p_i < lowerBound_i || upperBound_i < p_i)
{
return(false);
}
}
else
{
float d_i = i == 0 ? d.X : d.Y;
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)
{
MathUtils.Swap <float>(ref t1, ref t2);
s = 1.0f;
}
// Push the min up
if (t1 > tmin)
{
if (i == 0)
{
normal.X = s;
}
else
{
normal.Y = s;
}
tmin = t1;
}
// Pull the max down
tmax = Math.Min(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);
}
/// <summary> /// Cast a ray against this shape. /// </summary> /// <param name="output">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> /// <returns></returns> public abstract bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform);