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();
TSVector2 position = transform.p + MathUtils.Mul(transform.q, Position);
TSVector2 s = input.Point1 - position;
FP b = TSVector2.Dot(s, s) - _2radius;
// Solve quadratic equation.
TSVector2 r = input.Point2 - input.Point1;
FP c = TSVector2.Dot(s, r);
FP rr = TSVector2.Dot(r, r);
FP 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.
FP a = -(c + FP.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);
}
public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex)
{
output = default(RayCastOutput);
TSVector2 value = transform.p + MathUtils.Mul(transform.q, this.Position);
TSVector2 tSVector = input.Point1 - value;
FP y = TSVector2.Dot(tSVector, tSVector) - this._2radius;
TSVector2 tSVector2 = input.Point2 - input.Point1;
FP fP = TSVector2.Dot(tSVector, tSVector2);
FP fP2 = TSVector2.Dot(tSVector2, tSVector2);
FP x = fP * fP - fP2 * y;
bool flag = x < 0f || fP2 < Settings.Epsilon;
bool result;
if (flag)
{
result = false;
}
else
{
FP fP3 = -(fP + FP.Sqrt(x));
bool flag2 = 0f <= fP3 && fP3 <= input.MaxFraction * fP2;
if (flag2)
{
fP3 /= fP2;
output.Fraction = fP3;
output.Normal = tSVector + fP3 * tSVector2;
output.Normal.Normalize();
result = true;
}
else
{
result = false;
}
}
return(result);
}
public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex)
{
output = default(RayCastOutput);
TSVector2 value = MathUtils.MulT(transform.q, input.Point1 - transform.p);
TSVector2 value2 = MathUtils.MulT(transform.q, input.Point2 - transform.p);
TSVector2 value3 = value2 - value;
FP fP = 0f;
FP x = input.MaxFraction;
int num = -1;
int i = 0;
bool result;
while (i < this.Vertices.Count)
{
FP x2 = TSVector2.Dot(this.Normals[i], this.Vertices[i] - value);
FP fP2 = TSVector2.Dot(this.Normals[i], value3);
bool flag = fP2 == 0f;
if (flag)
{
bool flag2 = x2 < 0f;
if (flag2)
{
result = false;
return(result);
}
}
else
{
bool flag3 = fP2 < 0f && x2 < fP * fP2;
if (flag3)
{
fP = x2 / fP2;
num = i;
}
else
{
bool flag4 = fP2 > 0f && x2 < x * fP2;
if (flag4)
{
x = x2 / fP2;
}
}
}
bool flag5 = x < fP;
if (!flag5)
{
i++;
continue;
}
result = false;
return(result);
}
Debug.Assert(0f <= fP && fP <= input.MaxFraction);
bool flag6 = num >= 0;
if (flag6)
{
output.Fraction = fP;
output.Normal = MathUtils.Mul(transform.q, this.Normals[num]);
result = true;
return(result);
}
result = false;
return(result);
}
/// <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 bool RayCast(out RayCastOutput output, ref RayCastInput input, bool doInteriorCheck = true)
{
output = default(RayCastOutput);
FP fP = -Settings.MaxFP;
FP fP2 = Settings.MaxFP;
TSVector2 point = input.Point1;
TSVector2 tSVector = input.Point2 - input.Point1;
TSVector2 tSVector2 = MathUtils.Abs(tSVector);
TSVector2 zero = TSVector2.zero;
bool result;
for (int i = 0; i < 2; i++)
{
FP x = (i == 0) ? tSVector2.x : tSVector2.y;
FP fP3 = (i == 0) ? this.LowerBound.x : this.LowerBound.y;
FP x2 = (i == 0) ? this.UpperBound.x : this.UpperBound.y;
FP fP4 = (i == 0) ? point.x : point.y;
bool flag = x < Settings.Epsilon;
if (flag)
{
bool flag2 = fP4 < fP3 || x2 < fP4;
if (flag2)
{
result = false;
return(result);
}
}
else
{
FP y = (i == 0) ? tSVector.x : tSVector.y;
FP y2 = 1f / y;
FP fP5 = (fP3 - fP4) * y2;
FP fP6 = (x2 - fP4) * y2;
FP fP7 = -1f;
bool flag3 = fP5 > fP6;
if (flag3)
{
MathUtils.Swap <FP>(ref fP5, ref fP6);
fP7 = 1f;
}
bool flag4 = fP5 > fP;
if (flag4)
{
bool flag5 = i == 0;
if (flag5)
{
zero.x = fP7;
}
else
{
zero.y = fP7;
}
fP = fP5;
}
fP2 = TSMath.Min(fP2, fP6);
bool flag6 = fP > fP2;
if (flag6)
{
result = false;
return(result);
}
}
}
bool flag7 = doInteriorCheck && (fP < 0f || input.MaxFraction < fP);
if (flag7)
{
result = false;
return(result);
}
output.Fraction = fP;
output.Normal = zero;
result = true;
return(result);
}
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.
TSVector2 p1 = MathUtils.MulT(transform.q, input.Point1 - transform.p);
TSVector2 p2 = MathUtils.MulT(transform.q, input.Point2 - transform.p);
TSVector2 d = p2 - p1;
FP 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
FP numerator = TSVector2.Dot(Normals[i], Vertices[i] - p1);
FP denominator = TSVector2.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);
}
public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex)
{
output = default(RayCastOutput);
TSVector2 tSVector = MathUtils.MulT(transform.q, input.Point1 - transform.p);
TSVector2 value = MathUtils.MulT(transform.q, input.Point2 - transform.p);
TSVector2 tSVector2 = value - tSVector;
TSVector2 vertex = this._vertex1;
TSVector2 vertex2 = this._vertex2;
TSVector2 tSVector3 = vertex2 - vertex;
TSVector2 tSVector4 = new TSVector2(tSVector3.y, -tSVector3.x);
tSVector4.Normalize();
FP x = TSVector2.Dot(tSVector4, vertex - tSVector);
FP fP = TSVector2.Dot(tSVector4, tSVector2);
bool flag = fP == 0f;
bool result;
if (flag)
{
result = false;
}
else
{
FP fP2 = x / fP;
bool flag2 = fP2 < 0f || input.MaxFraction < fP2;
if (flag2)
{
result = false;
}
else
{
TSVector2 value2 = tSVector + fP2 * tSVector2;
TSVector2 tSVector5 = vertex2 - vertex;
FP fP3 = TSVector2.Dot(tSVector5, tSVector5);
bool flag3 = fP3 == 0f;
if (flag3)
{
result = false;
}
else
{
FP fP4 = TSVector2.Dot(value2 - vertex, tSVector5) / fP3;
bool flag4 = fP4 < 0f || 1f < fP4;
if (flag4)
{
result = false;
}
else
{
output.Fraction = fP2;
bool flag5 = x > 0f;
if (flag5)
{
output.Normal = -tSVector4;
}
else
{
output.Normal = tSVector4;
}
result = true;
}
}
}
}
return(result);
}
public abstract bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex);