private float RayCastCallback(RayCastInput input, int proxyid)
{
Actor actor = _tree.GetUserData(proxyid);
RayCastOutput output;
bool hit = actor.AABB.RayCast(out output, ref input);
if (hit)
{
_rayCastOutput = output;
_rayActor = actor;
actor.Fraction = output.Fraction;
return(output.Fraction);
}
return(input.MaxFraction);
}
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));
}
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;
if (output.Normal != Vector2.Zero)
{
output.Normal = Vector2.Normalize(output.Normal);
}
return(true);
}
return(false);
}
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)
{
// 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;
}
private void RayCast()
{
_rayActor = null;
RayCastInput input = _rayCastInput;
// Ray cast against the dynamic tree.
_tree.RayCast(RayCastCallback, ref input);
// Brute force ray cast.
Actor bruteActor = null;
#if DEBUG
RayCastOutput bruteOutput = new RayCastOutput();
#endif
for (int i = 0; i < ActorCount; ++i)
{
if (_actors[i].ProxyId == -1)
{
continue;
}
RayCastOutput output;
bool hit = _actors[i].AABB.RayCast(out output, ref input);
if (hit)
{
bruteActor = _actors[i];
#if DEBUG
bruteOutput = output;
#endif
input.MaxFraction = output.Fraction;
}
}
if (bruteActor != null)
{
#if DEBUG
Debug.Assert(bruteOutput.Fraction == _rayCastOutput.Fraction);
#endif
}
}
/// <summary>
/// Describes whether ray cast circle
/// </summary>
/// <param name="pos">The pos</param>
/// <param name="radius">The radius</param>
/// <param name="input">The input</param>
/// <param name="transform">The transform</param>
/// <param name="output">The output</param>
/// <returns>The bool</returns>
public static bool RayCastCircle(ref Vector2 pos, float radius, ref RayCastInput input, ref Transform transform,
out RayCastOutput output)
{
// 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, pos);
Vector2 s = input.Point1 - position;
float b = Vector2.Dot(s, s) - radius * radius;
// 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 < MathConstants.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 = Vector2.Normalize(output.Normal);
return(true);
}
return(false);
}
private void RayCast()
{
_rayActor = null;
RayCastInput input = _rayCastInput;
// Ray cast against the dynamic tree.
_tree.RayCast(RayCastCallback, ref input);
// Brute force ray cast.
Actor bruteActor = null;
RayCastOutput bruteOutput = new RayCastOutput();
for (int i = 0; i < ActorCount; ++i)
{
if (_actors[i].ProxyId == -1)
{
continue;
}
bool hit = _actors[i].AABB.RayCast(ref input, out RayCastOutput output);
// 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 override bool raycast(RayCastOutput output, RayCastInput input, Transform xf, int childIndex)
{
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));
}
// for pooling
public override bool raycast(RayCastOutput output, RayCastInput input, Transform xf, int childIndex)
{
double 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;
double p1x = xfq.c * tempx + xfq.s * tempy;
double p1y = -xfq.s * tempx + xfq.c * tempy;
tempx = input.p2.x - xfp.x;
tempy = input.p2.y - xfp.y;
double p2x = xfq.c * tempx + xfq.s * tempy;
double p2y = -xfq.s * tempx + xfq.c * tempy;
double dx = p2x - p1x;
double 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();
double normalx = normal.x;
double 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;
double numerator = normalx * tempx + normaly * tempy;
double denominator = normalx * dx + normaly * dy;
if (denominator == 0.0d)
{
return(false);
}
double t = numerator / denominator;
if (t < 0.0d || 1.0d < t)
{
return(false);
}
// Vec2 q = p1 + t * d;
double qx = p1x + t * dx;
double qy = p1y + t * dy;
// q = v1 + s * r
// s = dot(q - v1, r) / dot(r, r)
// Vec2 r = v2 - v1;
double rx = v2.x - v1.x;
double ry = v2.y - v1.y;
double rr = rx * rx + ry * ry;
if (rr == 0.0d)
{
return(false);
}
tempx = qx - v1.x;
tempy = qy - v1.y;
// double s = Vec2.dot(pool5, r) / rr;
double s = (tempx * rx + tempy * ry) / rr;
if (s < 0.0d || 1.0d < s)
{
return(false);
}
output.fraction = t;
if (numerator > 0.0d)
{
// argOutput.normal = -normal;
output.normal.x = -normalx;
output.normal.y = -normaly;
}
else
{
// output.normal = normal;
output.normal.x = normalx;
output.normal.y = normaly;
}
return(true);
}
public override bool RayCast(ref RayCastInput input, ref Transform transform, int childIndex, out RayCastOutput output)
{
return(RayCastHelper.RayCastCircle(ref _position, Radius, ref input, ref transform, out output));
}
/// <summary>
/// Raycast against this AABB using the specified points and maxfraction (found in input)
/// </summary>
/// <param name="output">The results of the raycast.</param>
/// <param name="input">The parameters for the raycast.</param>
/// <returns>True if the ray intersects the AABB</returns>
public bool RayCast(out RayCastOutput output, ref RayCastInput input, bool doInteriorCheck = true)
{
// From Real-time Collision Detection, p179.
output = new RayCastOutput();
var tmin = -Settings.MaxFloat;
var tmax = Settings.MaxFloat;
var p = input.Point1;
var d = input.Point2 - input.Point1;
var absD = MathUtils.Abs(d);
var normal = Vector2.zero;
for (var i = 0; i < 2; ++i)
{
var absD_i = i == 0 ? absD.x : absD.y;
var lowerBound_i = i == 0 ? LowerBound.x : LowerBound.y;
var upperBound_i = i == 0 ? UpperBound.x : UpperBound.y;
var p_i = i == 0 ? p.x : p.y;
if (absD_i < Settings.Epsilon)
{
// Parallel.
if (p_i < lowerBound_i || upperBound_i < p_i)
{
return(false);
}
}
else
{
var d_i = i == 0 ? d.x : d.y;
var inv_d = 1.0f / d_i;
var t1 = (lowerBound_i - p_i) * inv_d;
var t2 = (upperBound_i - p_i) * inv_d;
// Sign of the normal vector.
var s = -1.0f;
if (t1 > t2)
{
MathUtils.Swap(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 = Mathf.Min(tmax, t2);
if (tmin > tmax)
{
return(false);
}
}
}
// Does the ray start inside the box?
// Does the ray intersect beyond the max fraction?
if (doInteriorCheck && (tmin < 0.0f || input.MaxFraction < tmin))
{
return(false);
}
// Intersection.
output.Fraction = tmin;
output.Normal = normal;
return(true);
}
public override bool RayCast(ref RayCastInput input, ref Transform transform, int childIndex, out RayCastOutput output)
{
Debug.Assert(childIndex < Vertices.Count);
int i1 = childIndex;
int i2 = childIndex + 1;
if (i2 == Vertices.Count)
{
i2 = 0;
}
Vector2 v1 = Vertices[i1];
Vector2 v2 = Vertices[i2];
return(RayCastHelper.RayCastEdge(ref v1, ref v2, ref input, ref transform, out output));
}
/// <summary>
/// Raycast against this AABB using the specified points and maxfraction (found in input)
/// </summary>
/// <param name="output">The results of the raycast.</param>
/// <param name="input">The parameters for the raycast.</param>
/// <returns>True if the ray intersects the AABB</returns>
public bool RayCast(out RayCastOutput output, ref RayCastInput input, bool doInteriorCheck = true)
{
// From Real-time Collision Detection, p179.
output = new RayCastOutput();
var tmin = -long.MaxValue;
var tmax = long.MaxValue;
var p = input.Point1;
var d = input.Point2 - input.Point1;
var absD = Vector2d.Abs(d);
var normal = new Vector2d(0, 0);
for (int i = 0; i < 2; ++i)
{
long absD_i = i == 0 ? absD.x : absD.y;
long lowerBound_i = i == 0 ? LowerBound.x : LowerBound.y;
long upperBound_i = i == 0 ? UpperBound.x : UpperBound.y;
long p_i = i == 0 ? p.x : p.y;
if (absD_i < 1)
{
// Parallel.
if (p_i < lowerBound_i || upperBound_i < p_i)
{
return(false);
}
}
else
{
long d_i = i == 0 ? d.x : d.y;
long inv_d = FixedMath.One.Div(d_i);
long t1 = (lowerBound_i - p_i).Mul(inv_d);
long t2 = (upperBound_i - p_i).Mul(inv_d);
// Sign of the normal vector.
long s = -FixedMath.One;
if (t1 > t2)
{
Utility.Swap(ref t1, ref t2);
s = FixedMath.One;
}
// 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 (doInteriorCheck && (tmin < 0 || 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 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>Raycast against this AABB using the specified points and maxfraction (found in input)</summary>
/// <param name="input">The parameters for the raycast.</param>
/// <param name="output">The results of the raycast.</param>
/// <param name="doInteriorCheck"></param>
/// <returns>True if the ray intersects the AABB</returns>
public bool RayCast(ref RayCastInput input, out RayCastOutput output, bool doInteriorCheck = true)
{
// From Real-time Collision Detection, p179.
output = new RayCastOutput();
float tmin = -MathConstants.MaxFloat;
float tmax = MathConstants.MaxFloat;
Vector2 p = input.Point1;
Vector2 d = input.Point2 - input.Point1;
Vector2 absD = MathUtils.Abs(d);
Vector2 normal = Vector2.Zero;
for (int i = 0; i < 2; ++i)
{
float absDI = i == 0 ? absD.X : absD.Y;
float lowerBoundI = i == 0 ? LowerBound.X : LowerBound.Y;
float upperBoundI = i == 0 ? UpperBound.X : UpperBound.Y;
float pI = i == 0 ? p.X : p.Y;
if (absDI < MathConstants.Epsilon)
{
// Parallel.
if (pI < lowerBoundI || upperBoundI < pI)
{
return(false);
}
}
else
{
float dI = i == 0 ? d.X : d.Y;
float invD = 1.0f / dI;
float t1 = (lowerBoundI - pI) * invD;
float t2 = (upperBoundI - pI) * invD;
// Sign of the normal vector.
float s = -1.0f;
if (t1 > t2)
{
MathUtils.Swap(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 (doInteriorCheck && (tmin < 0.0f || input.MaxFraction < tmin))
{
return(false);
}
// Intersection.
output.Fraction = tmin;
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;
}
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;
}
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);
}
// 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>
/// 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"></param>
public bool Raycast(RayCastOutput output, RayCastInput input, int childIndex)
{
return(Shape.Raycast(output, input, Body.Xf, childIndex));
}
public override bool RayCast(ref RayCastInput input, ref Transform transform, int childIndex, out RayCastOutput output)
{
return(RayCastHelper.RayCastEdge(ref _vertex1, ref _vertex2, ref input, ref transform, out output));
}
/// <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);
// 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 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);
}
/**
* Cast a ray against this shape.
*
* @param output the ray-cast results.
* @param input the ray-cast input parameters.
* @param output
* @param input
*/
public bool raycast(RayCastOutput output, RayCastInput input, int childIndex)
{
return(m_shape.raycast(output, input, m_body.m_xf, childIndex));
}
/// <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 static bool RayCastEdge(ref Vector2 start, ref Vector2 end, bool oneSided, ref RayCastInput input, ref Transform transform, out RayCastOutput output)
{
// 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.MulT(transform.q, input.Point1 - transform.p);
Vector2 p2 = MathUtils.MulT(transform.q, input.Point2 - transform.p);
Vector2 d = p2 - p1;
Vector2 v1 = start;
Vector2 v2 = end;
Vector2 e = v2 - v1;
// Normal points to the right, looking from v1 at v2
Vector2 normal = new Vector2(e.Y, -e.X); //Velcro 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
float numerator = Vector2.Dot(normal, v1 - p1);
if (oneSided && numerator > 0.0f)
{
return(false);
}
float denominator = Vector2.Dot(normal, d);
if (denominator == 0.0f)
{
return(false);
}
float t = numerator / denominator;
if (t < 0.0f || input.MaxFraction < 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 = -MathUtils.MulT(transform.q, normal);
}
else
{
output.Normal = MathUtils.MulT(transform.q, normal);
}
return(true);
}
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);
}
// 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;
}
/// <summary>
/// Raycast against this AABB using the specified points and maxfraction (found in input)
/// </summary>
/// <param name="output">The results of the raycast.</param>
/// <param name="input">The parameters for the raycast.</param>
/// <returns>True if the ray intersects the AABB</returns>
public bool RayCast(out RayCastOutput output, ref RayCastInput input, bool doInteriorCheck = true)
{
// From Real-time Collision Detection, p179.
output = new RayCastOutput();
GGame.Math.Fix64 tmin = -Settings.MaxFloat;
GGame.Math.Fix64 tmax = Settings.MaxFloat;
Vector2 p = input.Point1;
Vector2 d = input.Point2 - input.Point1;
Vector2 absD = MathUtils.Abs(d);
Vector2 normal = Vector2.Zero;
for (int i = 0; i < 2; ++i)
{
GGame.Math.Fix64 absD_i = i == 0 ? absD.X : absD.Y;
GGame.Math.Fix64 lowerBound_i = i == 0 ? LowerBound.X : LowerBound.Y;
GGame.Math.Fix64 upperBound_i = i == 0 ? UpperBound.X : UpperBound.Y;
GGame.Math.Fix64 p_i = i == 0 ? p.X : p.Y;
if (absD_i < Settings.Epsilon)
{
// Parallel.
if (p_i < lowerBound_i || upperBound_i < p_i)
{
return(false);
}
}
else
{
GGame.Math.Fix64 d_i = i == 0 ? d.X : d.Y;
GGame.Math.Fix64 inv_d = 1.0f / d_i;
GGame.Math.Fix64 t1 = (lowerBound_i - p_i) * inv_d;
GGame.Math.Fix64 t2 = (upperBound_i - p_i) * inv_d;
// Sign of the normal vector.
GGame.Math.Fix64 s = -1.0f;
if (t1 > t2)
{
MathUtils.Swap(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((float)tmax, (float)t2);
if (tmin > tmax)
{
return(false);
}
}
}
// Does the ray start inside the box?
// Does the ray intersect beyond the max fraction?
if (doInteriorCheck && (tmin < 0.0f || input.MaxFraction < tmin))
{
return(false);
}
// Intersection.
output.Fraction = tmin;
output.Normal = normal;
return(true);
}
/// <summary>
/// Cast a ray against this Fixture by passing the call through to the 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 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.MulT(transform.q, input.Point1 - transform.p);
Vector2 p2 = MathUtils.MulT(transform.q, input.Point2 - transform.p);
Vector2 d = p2 - p1;
Vector2 v1 = _vertex1;
Vector2 v2 = _vertex2;
Vector2 e = v2 - v1;
Vector2 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
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 || input.MaxFraction < 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 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)
{
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);
}
public override bool RayCast(ref RayCastInput input, ref Transform transform, int childIndex, out RayCastOutput output)
{
return(RayCastHelper.RayCastPolygon(_vertices, _normals, ref input, ref transform, out output));
}
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);
}
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)
{
// 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;
}
/// <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);
}
public override bool RayCast(out FarseerPhysics.Collision.RayCastOutput output, ref FarseerPhysics.Collision.RayCastInput input, ref FarseerPhysics.Common.Transform transform, int childIndex)
{
output = new RayCastOutput();
return false;
}
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;
}
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;
}
/// <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));
}
/// <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>if hit</returns> public abstract bool Raycast(RayCastOutput output, RayCastInput input, Transform transform, int childIndex);
public bool RayCast(out RayCastOutput output, RayCastInput input)
{
output = default;
var tmin = -Settings.MaxFloat;
var tmax = Settings.MaxFloat;
var p = input.P1;
var d = input.P2 - input.P1;
var absD = Vector2.Abs(d);
var normal = new Vector2();
{
if (absD.X < Settings.Epsilon)
{
// Parallel.
if (p.X < LowerBound.X || UpperBound.X < p.X)
{
return(false);
}
}
else
{
var invD = 1.0f / d.X;
var t1 = (LowerBound.X - p.X) * invD;
var t2 = (UpperBound.X - p.X) * invD;
// Sign of the normal vector.
var s = -1.0f;
if (t1 > t2)
{
MathUtils.Swap(ref t1, ref t2);
s = 1.0f;
}
// Push the min up
if (t1 > tmin)
{
normal.SetZero();
normal.X = s;
tmin = t1;
}
// Pull the max down
tmax = Math.Min(tmax, t2);
if (tmin > tmax)
{
return(false);
}
}
}
{
if (absD.Y < Settings.Epsilon)
{
// Parallel.
if (p.Y < LowerBound.Y || UpperBound.Y < p.Y)
{
return(false);
}
}
else
{
var invD = 1.0f / d.Y;
var t1 = (LowerBound.Y - p.Y) * invD;
var t2 = (UpperBound.Y - p.Y) * invD;
// Sign of the normal vector.
var s = -1.0f;
if (t1 > t2)
{
MathUtils.Swap(ref t1, ref t2);
s = 1.0f;
}
// Push the min up
if (t1 > tmin)
{
normal.SetZero();
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 = new RayCastOutput {
Fraction = tmin, 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);
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);
}
// 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;
}
