public bool TestSegment(out float lambda, out Vec2 normal, Segment segment, float maxLambda)
{
lambda = 0f;
normal = default(Vec2);
Vec2 p = segment.P1;
Vec2 a = segment.P2 - p;
Vec2 a2 = this.P2 - this.P1;
Vec2 vec = Vec2.Cross(a2, 1f);
float num = 100f * Settings.FLT_EPSILON;
float num2 = -Vec2.Dot(a, vec);
bool result;
if (num2 > num)
{
Vec2 a3 = p - this.P1;
float num3 = Vec2.Dot(a3, vec);
if (0f <= num3 && num3 <= maxLambda * num2)
{
float num4 = -a.X * a3.Y + a.Y * a3.X;
if (-num * num2 <= num4 && num4 <= num2 * (1f + num))
{
num3 /= num2;
vec.Normalize();
lambda = num3;
normal = vec;
result = true;
return result;
}
}
}
result = false;
return result;
}
// 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 SegmentCollide TestSegment(XForm transform, out float lambda, out Vec2 normal, Segment segment, float maxLambda)
{
lambda = 0f;
normal = Vec2.Zero;
Vec2 position = transform.Position + Common.Math.Mul(transform.R, _localPosition);
Vec2 s = segment.P1 - position;
float b = Vec2.Dot(s, s) - _radius * _radius;
// Does the segment start inside the circle?
if (b < 0.0f)
{
lambda = 0f;
return SegmentCollide.StartInsideCollide;
}
// Solve quadratic equation.
Vec2 r = segment.P2 - segment.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 < Common.Settings.FLT_EPSILON)
{
return SegmentCollide.MissCollide;
}
// Find the point of intersection of the line with the circle.
float a = -(c + Common.Math.Sqrt(sigma));
// Is the intersection point on the segment?
if (0.0f <= a && a <= maxLambda * rr)
{
a /= rr;
lambda = a;
normal = s + a * r;
normal.Normalize();
return SegmentCollide.HitCollide;
}
return SegmentCollide.MissCollide;
}
public override SegmentCollide TestSegment(XForm transform, out float lambda, out Vec2 normal, Segment segment, float maxLambda)
{
lambda = 0f;
normal = Vec2.Zero;
Vec2 v = transform.Position + Box2DX.Common.Math.Mul(transform.R, this._localPosition);
Vec2 vec = segment.P1 - v;
float num = Vec2.Dot(vec, vec) - this._radius * this._radius;
SegmentCollide result;
if (num < 0f)
{
lambda = 0f;
result = SegmentCollide.StartInsideCollide;
}
else
{
Vec2 vec2 = segment.P2 - segment.P1;
float num2 = Vec2.Dot(vec, vec2);
float num3 = Vec2.Dot(vec2, vec2);
float num4 = num2 * num2 - num3 * num;
if (num4 < 0f || num3 < Settings.FLT_EPSILON)
{
result = SegmentCollide.MissCollide;
}
else
{
float num5 = -(num2 + Box2DX.Common.Math.Sqrt(num4));
if (0f <= num5 && num5 <= maxLambda * num3)
{
num5 /= num3;
lambda = num5;
normal = vec + num5 * vec2;
normal.Normalize();
result = SegmentCollide.HitCollide;
}
else
{
result = SegmentCollide.MissCollide;
}
}
}
return result;
}
public override SegmentCollide TestSegment(XForm transform, out float lambda, out Vec2 normal, Segment segment, float maxLambda)
{
Vec2 r = segment.P2 - segment.P1;
Vec2 v1 = Common.Math.Mul(transform, _v1);
Vec2 d = Common.Math.Mul(transform, _v2) - v1;
Vec2 n = Vec2.Cross(d, 1.0f);
float k_slop = 100.0f * Common.Settings.FLT_EPSILON;
float denom = -Vec2.Dot(r, n);
// Cull back facing collision and ignore parallel segments.
if (denom > k_slop)
{
// Does the segment intersect the infinite line associated with this segment?
Vec2 b = segment.P1 - v1;
float a = Vec2.Dot(b, n);
if (0.0f <= a && a <= maxLambda * denom)
{
float mu2 = -r.X * b.Y + r.Y * b.X;
// Does the segment intersect this segment?
if (-k_slop * denom <= mu2 && mu2 <= denom * (1.0f + k_slop))
{
a /= denom;
n.Normalize();
lambda = a;
normal = n;
return SegmentCollide.HitCollide;
}
}
}
lambda = 0;
normal = new Vec2();
return SegmentCollide.MissCollide;
}
/// <summary>
/// Performs a raycast as with Raycast, finding the first intersecting shape.
/// </summary>
/// <param name="segment">Defines the begin and end point of the ray cast, from p1 to p2.
/// Use Segment.Extend to create (semi-)infinite rays.</param>
/// <param name="lambda">Returns the hit fraction. You can use this to compute the contact point
/// p = (1 - lambda) * segment.p1 + lambda * segment.p2.</param>
/// <param name="normal">Returns the normal at the contact point. If there is no intersection, the normal is not set.</param>
/// <param name="solidShapes">Determines if shapes that the ray starts in are counted as hits.</param>
/// <param name="userData"></param>
/// <returns>Returns the colliding shape shape, or null if not found.</returns>
public Shape RaycastOne(Segment segment, out float lambda, out Vec2 normal, bool solidShapes, object userData)
{
lambda = 0;
normal = new Vec2(0,0);
int maxCount = 1;
Shape[] shape = new Shape[maxCount];
int count = Raycast(segment, shape, maxCount, solidShapes, userData);
if (count == 0)
return null;
Box2DXDebug.Assert(count == 1);
//Redundantly do TestSegment a second time, as the previous one's results are inaccessible
XForm xf = shape[0].GetBody().GetXForm();
shape[0].TestSegment(xf, out lambda, out normal, segment, 1);
//We already know it returns true
return shape[0];
}
/// <summary>
/// Query the world for all shapes that intersect a given segment. You provide a shap
/// pointer buffer of specified size. The number of shapes found is returned, and the buffer
/// is filled in order of intersection.
/// </summary>
/// <param name="segment">Defines the begin and end point of the ray cast, from p1 to p2.
/// Use Segment.Extend to create (semi-)infinite rays.</param>
/// <param name="shapes">A user allocated shape pointer array of size maxCount (or greater).</param>
/// <param name="maxCount">The capacity of the shapes array.</param>
/// <param name="solidShapes">Determines if shapes that the ray starts in are counted as hits.</param>
/// <param name="userData">Passed through the worlds contact filter, with method RayCollide. This can be used to filter valid shapes.</param>
/// <returns>The number of shapes found</returns>
public int Raycast(Segment segment, Shape[] shapes, int maxCount, bool solidShapes, object userData)
{
#warning "PTR"
_raycastSegment = segment;
_raycastUserData = userData;
_raycastSolidShape = solidShapes;
object[] results = new object[maxCount];
int count = _broadPhase.QuerySegment(segment, results, maxCount, RaycastSortKey);
for (int i = 0; i < count; ++i)
{
shapes[i] = (Shape)results[i];
}
results = null;
return count;
}
public override SegmentCollide TestSegment(XForm transform, out float lambda, out Vec2 normal, Segment segment, float maxLambda)
{
lambda = 0f;
normal = Vec2.Zero;
Vec2 v = transform.Position + Box2DX.Common.Math.Mul(transform.R, this._localPosition);
Vec2 vec = segment.P1 - v;
float num = Vec2.Dot(vec, vec) - this._radius * this._radius;
SegmentCollide result;
if (num < 0f)
{
lambda = 0f;
result = SegmentCollide.StartInsideCollide;
}
else
{
Vec2 vec2 = segment.P2 - segment.P1;
float num2 = Vec2.Dot(vec, vec2);
float num3 = Vec2.Dot(vec2, vec2);
float num4 = num2 * num2 - num3 * num;
if (num4 < 0f || num3 < Settings.FLT_EPSILON)
{
result = SegmentCollide.MissCollide;
}
else
{
float num5 = -(num2 + Box2DX.Common.Math.Sqrt(num4));
if (0f <= num5 && num5 <= maxLambda * num3)
{
num5 /= num3;
lambda = num5;
normal = vec + num5 * vec2;
normal.Normalize();
result = SegmentCollide.HitCollide;
}
else
{
result = SegmentCollide.MissCollide;
}
}
}
return(result);
}
// Collision Detection in Interactive 3D Environments by Gino van den Bergen
// From Section 3.4.1
// x = mu1 * p1 + mu2 * p2
// mu1 + mu2 = 1 && mu1 >= 0 && mu2 >= 0
// mu1 = 1 - mu2;
// x = (1 - mu2) * p1 + mu2 * p2
// = p1 + mu2 * (p2 - p1)
// x = s + a * r (s := start, r := end - start)
// s + a * r = p1 + mu2 * d (d := p2 - p1)
// -a * r + mu2 * d = b (b := s - p1)
// [-r d] * [a; mu2] = b
// Cramer's rule:
// denom = det[-r d]
// a = det[b d] / denom
// mu2 = det[-r b] / denom
/// <summary>
/// Ray cast against this segment with another segment.
/// </summary>
/// <param name="lambda"></param>
/// <param name="normal"></param>
/// <param name="segment"></param>
/// <param name="maxLambda"></param>
/// <returns></returns>
public bool TestSegment(out float lambda, out Vec2 normal, Segment segment, float maxLambda)
{
lambda = 0f;
normal = new Vec2();
Vec2 s = segment.P1;
Vec2 r = segment.P2 - s;
Vec2 d = P2 - P1;
Vec2 n = Vec2.Cross(d, 1.0f);
float k_slop = 100.0f * Settings.FLT_EPSILON;
float denom = -Vec2.Dot(r, n);
// Cull back facing collision and ignore parallel segments.
if (denom > k_slop)
{
// Does the segment intersect the infinite line associated with this segment?
Vec2 b = s - P1;
float a = Vec2.Dot(b, n);
if (0.0f <= a && a <= maxLambda * denom)
{
float mu2 = -r.X * b.Y + r.Y * b.X;
// Does the segment intersect this segment?
if (-k_slop * denom <= mu2 && mu2 <= denom * (1.0f + k_slop))
{
a /= denom;
n.Normalize();
lambda = a;
normal = n;
return true;
}
}
}
return false;
}
public abstract SegmentCollide TestSegment(XForm xf, out float lambda, out Vec2 normal, Segment segment, float maxLambda);
public int Raycast(Segment segment, Shape[] shapes, int maxCount, bool solidShapes, object userData)
{
this._raycastSegment = segment;
this._raycastUserData = userData;
this._raycastSolidShape = solidShapes;
object[] array = new object[maxCount];
int num = this._broadPhase.QuerySegment(segment, array, maxCount, new SortKeyFunc(World.RaycastSortKey));
for (int i = 0; i < num; i++)
{
shapes[i] = (Shape)array[i];
}
return num;
}
public override SegmentCollide TestSegment(XForm xf, out float lambda, out Vec2 normal, Segment segment, float maxLambda)
{
lambda = 0f;
normal = Vec2.Zero;
float num = 0f;
float num2 = maxLambda;
Vec2 v = Box2DX.Common.Math.MulT(xf.R, segment.P1 - xf.Position);
Vec2 v2 = Box2DX.Common.Math.MulT(xf.R, segment.P2 - xf.Position);
Vec2 b = v2 - v;
int num3 = -1;
int i = 0;
SegmentCollide result;
while (i < this._vertexCount)
{
float num4 = Vec2.Dot(this._normals[i], this._vertices[i] - v);
float num5 = Vec2.Dot(this._normals[i], b);
if (num5 == 0f)
{
if (num4 < 0f)
{
result = SegmentCollide.MissCollide;
return result;
}
}
else
{
if (num5 < 0f && num4 < num * num5)
{
num = num4 / num5;
num3 = i;
}
else
{
if (num5 > 0f && num4 < num2 * num5)
{
num2 = num4 / num5;
}
}
}
if (num2 >= num)
{
i++;
continue;
}
result = SegmentCollide.MissCollide;
return result;
}
Box2DXDebug.Assert(0f <= num && num <= maxLambda);
if (num3 >= 0)
{
lambda = num;
normal = Box2DX.Common.Math.Mul(xf.R, this._normals[num3]);
result = SegmentCollide.HitCollide;
return result;
}
lambda = 0f;
result = SegmentCollide.StartInsideCollide;
return result;
}
public void Draw(GraphicsHelper gh)
{
var world = parent_car.body.GetWorld();
var s = new Box2DX.Collision.Segment();
var pen = new Pen(System.Drawing.Color.Gray, 1 / gh.Scale);
for (int i = 0; i < p.num_rays; i++)
{
GetSegment(i, ref s);
float lambda = data[i] / p.dist;
float b0 = 0.3f, b = System.Math.Max(b0, 1 - lambda);
byte b1 = (byte)((1 - b) * 255);
Vec2 p1 = s.P1, p2 = (1 - lambda) * s.P1 + lambda * s.P2;
pen.Color = System.Drawing.Color.FromArgb(b1, b1, b1);
gh.G.DrawLine(pen, p1.X, p1.Y, p2.X, p2.Y);
}
}
public void Simulate()
{
var world = parent_car.body.GetWorld();
var s = new Box2DX.Collision.Segment();
for (int i = 0; i < p.num_rays; i++)
{
GetSegment(i, ref s);
float lambda;
Vec2 normal;
var x = world.RaycastOne(s, out lambda, out normal, false, null);
if (x == null) lambda = 1;
data[i] = lambda * p.dist;
}
}
public override SegmentCollide TestSegment(Transform xf, out float lambda, out Vector2 normal, Segment segment, float maxLambda)
{
Vector2 r = segment.P2 - segment.P1;
Vector2 v1 = xf.TransformPoint(_v1);
Vector2 d = ((Vector2)xf.TransformPoint(_v2)) - v1;
Vector2 n = d.CrossScalarPostMultiply(1.0f);
float k_slop = 100.0f * Common.Settings.FLT_EPSILON;
float denom = -Vector2.Dot(r, n);
// Cull back facing collision and ignore parallel segments.
if (denom > k_slop)
{
// Does the segment intersect the infinite line associated with this segment?
Vector2 b = segment.P1 - v1;
float a = Vector2.Dot(b, n);
if (0.0f <= a && a <= maxLambda * denom)
{
float mu2 = -r.x * b.y + r.y * b.x;
// Does the segment intersect this segment?
if (-k_slop * denom <= mu2 && mu2 <= denom * (1.0f + k_slop))
{
a /= denom;
n.Normalize();
lambda = a;
normal = n;
return SegmentCollide.HitCollide;
}
}
}
lambda = 0;
normal = new Vector2();
return SegmentCollide.MissCollide;
}
public void Update(float mX, float mY, Vector2 offset, int levelWidth)
{
if (amDrawing || amErasing)
{
//animation object
myGameTime++;
sourceRect = new Rectangle(xFrame * spriteWidth, yFrame * spriteHeight, spriteWidth, spriteHeight);
animateTimer += myGameTime;
if (animateTimer > animateInterval)
{
xFrame++;
if (xFrame > numFrames - 1)
{
xFrame = 0;
}
myGameTime = 0;
animateTimer = 0;
}
}
//Console.WriteLine("{0} {1}", mX, mY);
//mx and my are screen coords but have been divided by scale so they are in "gamecoords"
//therefore mouseX and mouseY are in screencoords
mouseX = mX * SCALE;
mouseY = mY * SCALE;
int guyScreenPos = 0;
if (dude.Position.X * CASSWorld.SCALE <= GameEngine.SCREEN_WIDTH / 2)
guyScreenPos = (int)(dude.getWorld().getScreenCoords(dude.Position).X);
else if (dude.Position.X * CASSWorld.SCALE >= levelWidth - GameEngine.SCREEN_WIDTH / 2)
{
guyScreenPos = (int)(dude.getWorld().getScreenCoords(dude.Position).X); ;
}
else
{
guyScreenPos = GameEngine.SCREEN_WIDTH / 2;
}
original = new Vector2(dude.Position.X * SCALE, dude.Position.Y * SCALE);
end = new Vector2((mX + dude.Position.X) * SCALE - guyScreenPos, mY * SCALE);
//end = new Vector2(mX*SCALE, mY * SCALE);
//Vector2 gunpos = original; //+ adjustment;
Box2DX.Collision.Segment myseg = new Segment();
//myseg.P1 = new Box2DX.Common.Vec2(original.X / SCALE, original.Y / SCALE);
myseg.P1 = new Box2DX.Common.Vec2(dude.Position.X, dude.Position.Y);
myseg.P2 = new Box2DX.Common.Vec2(end.X / SCALE, end.Y / SCALE);
lambda = 1;
Box2DX.Common.Vec2 normal;
//Console.WriteLine("{0}", interference[0]);
for (int i = 0; i < 2; i++)
{
interference[i] = null;
}
//interference = world.RaycastOne(myseg, out lambda, out normal, false, null);
//Console.WriteLine("hi");
int numShapes = world.Raycast(myseg, interference, 3, false, null);
//Console.WriteLine( "{0}", interference[0]);
if (interference[0] == null)
{
canIDraw = true;
canIErase = true;
endpoint = new Vector2(mouseX, mouseY);
}
else if (interference[0].IsSensor)
{
if (interference[1] == null)
{
canIDraw = true;
canIErase = true;
endpoint = new Vector2(mouseX, mouseY);
}
else if (interference[1].IsSensor)
{
if (interference[2] == null)
{
canIDraw = true;
canIErase = true;
endpoint = new Vector2(mouseX, mouseY);
}
else
{
AABB aabb = new AABB();
aabb.LowerBound = Common.Utils.Convert(dude.getWorld().getGameCoords(new Vector2(mouseX, mouseY)) - new Vector2(0.1f));
aabb.UpperBound = Common.Utils.Convert(dude.getWorld().getGameCoords(new Vector2(mouseX, mouseY)) + new Vector2(0.1f));
Shape[] shapes = new Shape[1];
int nHit = world.Query(aabb, shapes, 1);
if (nHit > 0)
{
Body body1 = shapes[0].GetBody();
PhysicsObject po1 = (PhysicsObject)body1.GetUserData();
Body body2 = interference[2].GetBody();
PhysicsObject po2 = (PhysicsObject)body2.GetUserData();
if ((po1 is PaintedObject) && body1.Equals(body2))
{
canIErase = true;
canIDraw = false;
}
else
{
canIErase = false;
canIDraw = false;
}
}
else
{
canIErase = false;
canIDraw = false;
}
myseg.P1 = new Box2DX.Common.Vec2(dude.Position.X, dude.Position.Y + .7f);
Box2DX.Collision.Shape intersect = world.RaycastOne(myseg, out lambda, out normal, false, null);
Box2DX.Common.Vec2 p = (((1 - lambda) * (myseg.P1)) + (lambda * (myseg.P2)));
endpoint = dude.getWorld().getScreenCoords(Common.Utils.Convert(p));
}
}
else
{
AABB aabb = new AABB();
aabb.LowerBound = Common.Utils.Convert(dude.getWorld().getGameCoords(new Vector2(mouseX, mouseY)) - new Vector2(0.1f));
aabb.UpperBound = Common.Utils.Convert(dude.getWorld().getGameCoords(new Vector2(mouseX, mouseY)) + new Vector2(0.1f));
Shape[] shapes = new Shape[1];
int nHit = world.Query(aabb, shapes, 1);
if (nHit > 0)
{
Body body1 = shapes[0].GetBody();
PhysicsObject po1 = (PhysicsObject)body1.GetUserData();
Body body2 = interference[1].GetBody();
PhysicsObject po2 = (PhysicsObject)body2.GetUserData();
if ((po1 is PaintedObject) && body1.Equals(body2))
{
canIErase = true;
canIDraw = false;
}
else
{
canIErase = false;
canIDraw = false;
}
}
else
{
canIErase = false;
canIDraw = false;
}
myseg.P1 = new Box2DX.Common.Vec2(dude.Position.X, dude.Position.Y + .6f);
Box2DX.Collision.Shape intersect = world.RaycastOne(myseg, out lambda, out normal, false, null);
Box2DX.Common.Vec2 p = (((1 - lambda) * (myseg.P1)) + (lambda * (myseg.P2)));
endpoint = dude.getWorld().getScreenCoords(Common.Utils.Convert(p));
}
}
else
{
AABB aabb = new AABB();
aabb.LowerBound = Common.Utils.Convert(dude.getWorld().getGameCoords(new Vector2(mouseX, mouseY)) - new Vector2(0.1f));
aabb.UpperBound = Common.Utils.Convert(dude.getWorld().getGameCoords(new Vector2(mouseX, mouseY)) + new Vector2(0.1f));
Shape[] shapes = new Shape[1];
int nHit = world.Query(aabb, shapes, 1);
if (nHit > 0)
{
Body body1 = shapes[0].GetBody();
PhysicsObject po1 = (PhysicsObject)body1.GetUserData();
Body body2 = interference[0].GetBody();
PhysicsObject po2 = (PhysicsObject)body2.GetUserData();
if ((po1 is PaintedObject) && body1.Equals(body2))
{
canIErase = true;
canIDraw = false;
}
else
{
canIErase = false;
canIDraw = false;
}
}
else
{
canIDraw = false;
canIErase = false;
}
Box2DX.Collision.Shape intersect = world.RaycastOne(myseg, out lambda, out normal, false, null);
Box2DX.Common.Vec2 p = (((1 - lambda) * (myseg.P1)) + (lambda * (myseg.P2)));
endpoint = dude.getWorld().getScreenCoords(Common.Utils.Convert(p));
}
//original animation stuff
//startpoint = original + adjustment + offset;
startpoint = original + offset;
//endpoint = new Vector2(mouseX, mouseY);
// move the start point to the end of his gun
Vector2 cursorDirection = (endpoint - startpoint);
cursorDirection.Normalize();
startpoint = startpoint + (Constants.HALF_GUN * cursorDirection);
}
int QuerySegment(Segment segment, object[] userData, int maxCount, SortKeyFunc sortKey)
{
float maxLambda = 1;
float dx = (segment.P2.X - segment.P1.X) * _quantizationFactor.X;
float dy = (segment.P2.Y - segment.P1.Y) * _quantizationFactor.Y;
int sx = dx < -Settings.FLT_EPSILON ? -1 : (dx > Settings.FLT_EPSILON ? 1 : 0);
int sy = dy < -Settings.FLT_EPSILON ? -1 : (dy > Settings.FLT_EPSILON ? 1 : 0);
Box2DXDebug.Assert(sx != 0 || sy != 0);
float p1x = (segment.P1.X - _worldAABB.LowerBound.X) * _quantizationFactor.X;
float p1y = (segment.P1.Y - _worldAABB.LowerBound.Y) * _quantizationFactor.Y;
#if ALLOWUNSAFE
ushort* startValues = stackalloc ushort[2];
ushort* startValues2 = stackalloc ushort[2];
#else
ushort[] startValues = new ushort[2];
ushort[] startValues2 = new ushort[2];
#endif
int xIndex;
int yIndex;
ushort proxyId;
Proxy proxy;
// TODO_ERIN implement fast float to ushort conversion.
startValues[0] = (ushort)((ushort)(p1x) & (BROADPHASE_MAX - 1));
startValues2[0] = (ushort)((ushort)(p1x) | 1);
startValues[1] = (ushort)((ushort)(p1y) & (BROADPHASE_MAX - 1));
startValues2[1] = (ushort)((ushort)(p1y) | 1);
//First deal with all the proxies that contain segment.p1
int lowerIndex;
int upperIndex;
Query(out lowerIndex, out upperIndex, startValues[0], startValues2[0], _bounds[0], 2 * _proxyCount, 0);
if (sx >= 0) xIndex = upperIndex - 1;
else xIndex = lowerIndex;
Query(out lowerIndex, out upperIndex, startValues[1], startValues2[1], _bounds[1], 2 * _proxyCount, 1);
if (sy >= 0) yIndex = upperIndex - 1;
else yIndex = lowerIndex;
//If we are using sortKey, then sort what we have so far, filtering negative keys
if (sortKey != null)
{
//Fill keys
for (int j = 0; j < _queryResultCount; j++)
{
_querySortKeys[j] = sortKey(_proxyPool[_queryResults[j]].UserData);
}
//Bubble sort keys
//Sorting negative values to the top, so we can easily remove them
int i = 0;
while (i < _queryResultCount - 1)
{
float a = _querySortKeys[i];
float b = _querySortKeys[i + 1];
if ((a < 0) ? (b >= 0) : (a > b && b >= 0))
{
_querySortKeys[i + 1] = a;
_querySortKeys[i] = b;
ushort tempValue = _queryResults[i + 1];
_queryResults[i + 1] = _queryResults[i];
_queryResults[i] = tempValue;
i--;
if (i == -1) i = 1;
}
else
{
i++;
}
}
//Skim off negative values
while (_queryResultCount > 0 && _querySortKeys[_queryResultCount - 1] < 0)
_queryResultCount--;
}
//Now work through the rest of the segment
for (; ; )
{
float xProgress = 0;
float yProgress = 0;
if (xIndex < 0 || xIndex >= _proxyCount * 2)
break;
if (yIndex < 0 || yIndex >= _proxyCount * 2)
break;
if (sx != 0)
{
//Move on to the next bound
if (sx > 0)
{
xIndex++;
if (xIndex == _proxyCount * 2)
break;
}
else
{
xIndex--;
if (xIndex < 0)
break;
}
xProgress = (_bounds[0][xIndex].Value - p1x) / dx;
}
if (sy != 0)
{
//Move on to the next bound
if (sy > 0)
{
yIndex++;
if (yIndex == _proxyCount * 2)
break;
}
else
{
yIndex--;
if (yIndex < 0)
break;
}
yProgress = (_bounds[1][yIndex].Value - p1y) / dy;
}
for (; ; )
{
if (sy == 0 || (sx != 0 && xProgress < yProgress))
{
if (xProgress > maxLambda)
break;
//Check that we are entering a proxy, not leaving
if (sx > 0 ? _bounds[0][xIndex].IsLower : _bounds[0][xIndex].IsUpper)
{
//Check the other axis of the proxy
proxyId = _bounds[0][xIndex].ProxyId;
proxy = _proxyPool[proxyId];
if (sy >= 0)
{
if (proxy.LowerBounds[1] <= yIndex - 1 && proxy.UpperBounds[1] >= yIndex)
{
//Add the proxy
if (sortKey != null)
{
AddProxyResult(proxyId, proxy, maxCount, sortKey);
}
else
{
_queryResults[_queryResultCount] = proxyId;
++_queryResultCount;
}
}
}
else
{
if (proxy.LowerBounds[1] <= yIndex && proxy.UpperBounds[1] >= yIndex + 1)
{
//Add the proxy
if (sortKey != null)
{
AddProxyResult(proxyId, proxy, maxCount, sortKey);
}
else
{
_queryResults[_queryResultCount] = proxyId;
++_queryResultCount;
}
}
}
}
//Early out
if (sortKey != null && _queryResultCount == maxCount && _queryResultCount > 0 && xProgress > _querySortKeys[_queryResultCount - 1])
break;
//Move on to the next bound
if (sx > 0)
{
xIndex++;
if (xIndex == _proxyCount * 2)
break;
}
else
{
xIndex--;
if (xIndex < 0)
break;
}
xProgress = (_bounds[0][xIndex].Value - p1x) / dx;
}
else
{
if (yProgress > maxLambda)
break;
//Check that we are entering a proxy, not leaving
if (sy > 0 ? _bounds[1][yIndex].IsLower : _bounds[1][yIndex].IsUpper)
{
//Check the other axis of the proxy
proxyId = _bounds[1][yIndex].ProxyId;
proxy = _proxyPool[proxyId];
if (sx >= 0)
{
if (proxy.LowerBounds[0] <= xIndex - 1 && proxy.UpperBounds[0] >= xIndex)
{
//Add the proxy
if (sortKey != null)
{
AddProxyResult(proxyId, proxy, maxCount, sortKey);
}
else
{
_queryResults[_queryResultCount] = proxyId;
++_queryResultCount;
}
}
}
else
{
if (proxy.LowerBounds[0] <= xIndex && proxy.UpperBounds[0] >= xIndex + 1)
{
//Add the proxy
if (sortKey != null)
{
AddProxyResult(proxyId, proxy, maxCount, sortKey);
}
else
{
_queryResults[_queryResultCount] = proxyId;
++_queryResultCount;
}
}
}
}
//Early out
if (sortKey != null && _queryResultCount == maxCount && _queryResultCount > 0 && yProgress > _querySortKeys[_queryResultCount - 1])
break;
//Move on to the next bound
if (sy > 0)
{
yIndex++;
if (yIndex == _proxyCount * 2)
break;
}
else
{
yIndex--;
if (yIndex < 0)
break;
}
yProgress = (_bounds[1][yIndex].Value - p1y) / dy;
}
}
break;
}
int count = 0;
for (int i = 0; i < _queryResultCount && count < maxCount; ++i, ++count)
{
Box2DXDebug.Assert(_queryResults[i] < Settings.MaxProxies);
Proxy proxy_ = _proxyPool[_queryResults[i]];
Box2DXDebug.Assert(proxy_.IsValid);
userData[i] = proxy_.UserData;
}
// Prepare for next query.
_queryResultCount = 0;
IncrementTimeStamp();
return count;
}
public Shape RaycastOne(Segment segment, out float lambda, out Vec2 normal, bool solidShapes, object userData)
{
lambda = 0f;
normal = new Vec2(0f, 0f);
int num = 1;
Shape[] array = new Shape[num];
int num2 = this.Raycast(segment, array, num, solidShapes, userData);
Shape result;
if (num2 == 0)
{
result = null;
}
else
{
Box2DXDebug.Assert(num2 == 1);
XForm xForm = array[0].GetBody().GetXForm();
array[0].TestSegment(xForm, out lambda, out normal, segment, 1f);
result = array[0];
}
return result;
}
/// <summary>
/// Performs a raycast as with Raycast, finding the first intersecting shape.
/// </summary>
/// <param name="segment">Defines the begin and end point of the ray cast, from p1 to p2.
/// Use Segment.Extend to create (semi-)infinite rays.</param>
/// <param name="lambda">Returns the hit fraction. You can use this to compute the contact point
/// p = (1 - lambda) * segment.p1 + lambda * segment.p2.</param>
/// <param name="normal">Returns the normal at the contact point. If there is no intersection, the normal is not set.</param>
/// <param name="solidShapes">Determines if shapes that the ray starts in are counted as hits.</param>
/// <param name="userData"></param>
/// <returns>Returns the colliding shape shape, or null if not found.</returns>
public Fixture RaycastOne(Segment segment, out float lambda, out Vector2 normal, bool solidShapes, object userData)
{
int maxCount = 1;
Fixture[] fixture;
lambda = 0.0f;
normal = new Vector2();
int count = Raycast(segment, out fixture, maxCount, solidShapes, userData);
if (count == 0)
return null;
Box2DXDebug.Assert(count == 1);
//Redundantly do TestSegment a second time, as the previous one's results are inaccessible
fixture[0].TestSegment(out lambda, out normal, segment, 1);
//We already know it returns true
return fixture[0];
}
public unsafe int QuerySegment(Segment segment, object[] userData, int maxCount, SortKeyFunc sortKey)
{
float num = 1f;
float num2 = (segment.P2.X - segment.P1.X) * this._quantizationFactor.X;
float num3 = (segment.P2.Y - segment.P1.Y) * this._quantizationFactor.Y;
int num4 = (num2 < -Settings.FLT_EPSILON) ? -1 : ((num2 > Settings.FLT_EPSILON) ? 1 : 0);
int num5 = (num3 < -Settings.FLT_EPSILON) ? -1 : ((num3 > Settings.FLT_EPSILON) ? 1 : 0);
Box2DXDebug.Assert(num4 != 0 || num5 != 0);
float num6 = (segment.P1.X - this._worldAABB.LowerBound.X) * this._quantizationFactor.X;
float num7 = (segment.P1.Y - this._worldAABB.LowerBound.Y) * this._quantizationFactor.Y;
ushort* ptr = stackalloc ushort[2];
ushort* ptr2 = stackalloc ushort[2];
*ptr = (ushort)((ushort)num6 & BroadPhase.BROADPHASE_MAX - 1);
*ptr2 = (ushort)((ushort)num6 | 1);
ptr[2 / 2] = (ushort)((ushort)num7 & BroadPhase.BROADPHASE_MAX - 1);
ptr2[2 / 2] = (ushort)((ushort)num7 | 1);
int num8;
int num9;
this.Query(out num8, out num9, *ptr, *ptr2, this._bounds[0], 2 * this._proxyCount, 0);
int num10;
if (num4 >= 0)
{
num10 = num9 - 1;
}
else
{
num10 = num8;
}
this.Query(out num8, out num9, ptr[2 / 2], *(ptr2 + 2 / 2), this._bounds[1], 2 * this._proxyCount, 1);
int num11;
if (num5 >= 0)
{
num11 = num9 - 1;
}
else
{
num11 = num8;
}
int j;
if (sortKey != null)
{
for (int i = 0; i < this._queryResultCount; i++)
{
this._querySortKeys[i] = sortKey(this._proxyPool[(int)this._queryResults[i]].UserData);
}
j = 0;
while (j < this._queryResultCount - 1)
{
float num12 = this._querySortKeys[j];
float num13 = this._querySortKeys[j + 1];
if (((num12 < 0f) ? ((num13 >= 0f) ? 1 : 0) : ((num12 <= num13) ? 0 : ((num13 >= 0f) ? 1 : 0))) != 0)
{
this._querySortKeys[j + 1] = num12;
this._querySortKeys[j] = num13;
ushort num14 = this._queryResults[j + 1];
this._queryResults[j + 1] = this._queryResults[j];
this._queryResults[j] = num14;
j--;
if (j == -1)
{
j = 1;
}
}
else
{
j++;
}
}
while (this._queryResultCount > 0 && this._querySortKeys[this._queryResultCount - 1] < 0f)
{
this._queryResultCount--;
}
}
float num15 = 0f;
float num16 = 0f;
if (num10 >= 0 && num10 < this._proxyCount * 2)
{
if (num11 >= 0 && num11 < this._proxyCount * 2)
{
if (num4 != 0)
{
if (num4 > 0)
{
num10++;
if (num10 == this._proxyCount * 2)
{
goto IL_829;
}
}
else
{
num10--;
if (num10 < 0)
{
goto IL_829;
}
}
num15 = ((float)this._bounds[0][num10].Value - num6) / num2;
}
if (num5 != 0)
{
if (num5 > 0)
{
num11++;
if (num11 == this._proxyCount * 2)
{
goto IL_829;
}
}
else
{
num11--;
if (num11 < 0)
{
goto IL_829;
}
}
num16 = ((float)this._bounds[1][num11].Value - num7) / num3;
}
while (true)
{
if (num5 == 0 || (num4 != 0 && num15 < num16))
{
if (num15 > num)
{
break;
}
if ((num4 > 0) ? this._bounds[0][num10].IsLower : this._bounds[0][num10].IsUpper)
{
ushort proxyId = this._bounds[0][num10].ProxyId;
Proxy proxy = this._proxyPool[(int)proxyId];
if (num5 >= 0)
{
if ((int)proxy.LowerBounds[1] <= num11 - 1 && (int)proxy.UpperBounds[1] >= num11)
{
if (sortKey != null)
{
this.AddProxyResult(proxyId, proxy, maxCount, sortKey);
}
else
{
this._queryResults[this._queryResultCount] = proxyId;
this._queryResultCount++;
}
}
}
else
{
if ((int)proxy.LowerBounds[1] <= num11 && (int)proxy.UpperBounds[1] >= num11 + 1)
{
if (sortKey != null)
{
this.AddProxyResult(proxyId, proxy, maxCount, sortKey);
}
else
{
this._queryResults[this._queryResultCount] = proxyId;
this._queryResultCount++;
}
}
}
}
if (sortKey != null && this._queryResultCount == maxCount && this._queryResultCount > 0 && num15 > this._querySortKeys[this._queryResultCount - 1])
{
break;
}
if (num4 > 0)
{
num10++;
if (num10 == this._proxyCount * 2)
{
break;
}
}
else
{
num10--;
if (num10 < 0)
{
break;
}
}
num15 = ((float)this._bounds[0][num10].Value - num6) / num2;
}
else
{
if (num16 > num)
{
break;
}
if ((num5 > 0) ? this._bounds[1][num11].IsLower : this._bounds[1][num11].IsUpper)
{
ushort proxyId = this._bounds[1][num11].ProxyId;
Proxy proxy = this._proxyPool[(int)proxyId];
if (num4 >= 0)
{
if ((int)proxy.LowerBounds[0] <= num10 - 1 && (int)proxy.UpperBounds[0] >= num10)
{
if (sortKey != null)
{
this.AddProxyResult(proxyId, proxy, maxCount, sortKey);
}
else
{
this._queryResults[this._queryResultCount] = proxyId;
this._queryResultCount++;
}
}
}
else
{
if ((int)proxy.LowerBounds[0] <= num10 && (int)proxy.UpperBounds[0] >= num10 + 1)
{
if (sortKey != null)
{
this.AddProxyResult(proxyId, proxy, maxCount, sortKey);
}
else
{
this._queryResults[this._queryResultCount] = proxyId;
this._queryResultCount++;
}
}
}
}
if (sortKey != null && this._queryResultCount == maxCount && this._queryResultCount > 0 && num16 > this._querySortKeys[this._queryResultCount - 1])
{
break;
}
if (num5 > 0)
{
num11++;
if (num11 == this._proxyCount * 2)
{
break;
}
}
else
{
num11--;
if (num11 < 0)
{
break;
}
}
num16 = ((float)this._bounds[1][num11].Value - num7) / num3;
}
}
}
}
IL_829:
int num17 = 0;
j = 0;
while (j < this._queryResultCount && num17 < maxCount)
{
Box2DXDebug.Assert((int)this._queryResults[j] < Settings.MaxProxies);
Proxy proxy2 = this._proxyPool[(int)this._queryResults[j]];
Box2DXDebug.Assert(proxy2.IsValid);
userData[j] = proxy2.UserData;
j++;
num17++;
}
this._queryResultCount = 0;
this.IncrementTimeStamp();
return num17;
}
/// <summary>
/// Perform a ray cast against this shape.
/// </summary>
/// <param name="lambda">Returns the hit fraction. You can use this to compute the contact point
/// p = (1 - lambda) * segment.p1 + lambda * segment.p2.</param>
/// <param name="normal">Returns the normal at the contact point. If there is no intersection, the normal
/// is not set.</param>
/// <param name="segment">Defines the begin and end point of the ray cast.</param>
/// <param name="maxLambda">A number typically in the range [0,1].</param>
public SegmentCollide TestSegment(out float lambda, out Vec2 normal, Segment segment, float maxLambda)
{
return _shape.TestSegment(_body.GetXForm(), out lambda, out normal, segment, maxLambda);
}
public override SegmentCollide TestSegment(XForm xf, out float lambda, out Vec2 normal, Segment segment, float maxLambda)
{
lambda = 0f;
normal = Vec2.Zero;
float lower = 0.0f, upper = maxLambda;
Vec2 p1 = Common.Math.MulT(xf.R, segment.P1 - xf.Position);
Vec2 p2 = Common.Math.MulT(xf.R, segment.P2 - xf.Position);
Vec2 d = p2 - p1;
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 = Vec2.Dot(_normals[i], _vertices[i] - p1);
float denominator = Vec2.Dot(_normals[i], d);
if (denominator == 0.0f)
{
if (numerator < 0.0f)
{
return SegmentCollide.MissCollide;
}
}
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 SegmentCollide.MissCollide;
}
}
Box2DXDebug.Assert(0.0f <= lower && lower <= maxLambda);
if (index >= 0)
{
lambda = lower;
normal = Common.Math.Mul(xf.R, _normals[index]);
return SegmentCollide.HitCollide;
}
lambda = 0f;
return SegmentCollide.StartInsideCollide;
}
int QuerySegment(Segment segment, object[] userData, int maxCount, SortKeyFunc sortKey)
{
float maxLambda = 1;
float dx = (segment.P2.X - segment.P1.X) * _quantizationFactor.X;
float dy = (segment.P2.Y - segment.P1.Y) * _quantizationFactor.Y;
int sx = dx < -Settings.FLT_EPSILON ? -1 : (dx > Settings.FLT_EPSILON ? 1 : 0);
int sy = dy < -Settings.FLT_EPSILON ? -1 : (dy > Settings.FLT_EPSILON ? 1 : 0);
Box2DXDebug.Assert(sx != 0 || sy != 0);
float p1x = (segment.P1.X - _worldAABB.LowerBound.X) * _quantizationFactor.X;
float p1y = (segment.P1.Y - _worldAABB.LowerBound.Y) * _quantizationFactor.Y;
#if ALLOWUNSAFE
ushort *startValues = stackalloc ushort[2];
ushort *startValues2 = stackalloc ushort[2];
#else
ushort[] startValues = new ushort[2];
ushort[] startValues2 = new ushort[2];
#endif
int xIndex;
int yIndex;
ushort proxyId;
Proxy proxy;
// TODO_ERIN implement fast float to ushort conversion.
startValues[0] = (ushort)((ushort)(p1x) & (BROADPHASE_MAX - 1));
startValues2[0] = (ushort)((ushort)(p1x) | 1);
startValues[1] = (ushort)((ushort)(p1y) & (BROADPHASE_MAX - 1));
startValues2[1] = (ushort)((ushort)(p1y) | 1);
//First deal with all the proxies that contain segment.p1
int lowerIndex;
int upperIndex;
Query(out lowerIndex, out upperIndex, startValues[0], startValues2[0], _bounds[0], 2 * _proxyCount, 0);
if (sx >= 0)
{
xIndex = upperIndex - 1;
}
else
{
xIndex = lowerIndex;
}
Query(out lowerIndex, out upperIndex, startValues[1], startValues2[1], _bounds[1], 2 * _proxyCount, 1);
if (sy >= 0)
{
yIndex = upperIndex - 1;
}
else
{
yIndex = lowerIndex;
}
//If we are using sortKey, then sort what we have so far, filtering negative keys
if (sortKey != null)
{
//Fill keys
for (int j = 0; j < _queryResultCount; j++)
{
_querySortKeys[j] = sortKey(_proxyPool[_queryResults[j]].UserData);
}
//Bubble sort keys
//Sorting negative values to the top, so we can easily remove them
int i = 0;
while (i < _queryResultCount - 1)
{
float a = _querySortKeys[i];
float b = _querySortKeys[i + 1];
if ((a < 0) ? (b >= 0) : (a > b && b >= 0))
{
_querySortKeys[i + 1] = a;
_querySortKeys[i] = b;
ushort tempValue = _queryResults[i + 1];
_queryResults[i + 1] = _queryResults[i];
_queryResults[i] = tempValue;
i--;
if (i == -1)
{
i = 1;
}
}
else
{
i++;
}
}
//Skim off negative values
while (_queryResultCount > 0 && _querySortKeys[_queryResultCount - 1] < 0)
{
_queryResultCount--;
}
}
//Now work through the rest of the segment
for (; ;)
{
float xProgress = 0;
float yProgress = 0;
if (xIndex < 0 || xIndex >= _proxyCount * 2)
{
break;
}
if (yIndex < 0 || yIndex >= _proxyCount * 2)
{
break;
}
if (sx != 0)
{
//Move on to the next bound
if (sx > 0)
{
xIndex++;
if (xIndex == _proxyCount * 2)
{
break;
}
}
else
{
xIndex--;
if (xIndex < 0)
{
break;
}
}
xProgress = (_bounds[0][xIndex].Value - p1x) / dx;
}
if (sy != 0)
{
//Move on to the next bound
if (sy > 0)
{
yIndex++;
if (yIndex == _proxyCount * 2)
{
break;
}
}
else
{
yIndex--;
if (yIndex < 0)
{
break;
}
}
yProgress = (_bounds[1][yIndex].Value - p1y) / dy;
}
for (; ;)
{
if (sy == 0 || (sx != 0 && xProgress < yProgress))
{
if (xProgress > maxLambda)
{
break;
}
//Check that we are entering a proxy, not leaving
if (sx > 0 ? _bounds[0][xIndex].IsLower : _bounds[0][xIndex].IsUpper)
{
//Check the other axis of the proxy
proxyId = _bounds[0][xIndex].ProxyId;
proxy = _proxyPool[proxyId];
if (sy >= 0)
{
if (proxy.LowerBounds[1] <= yIndex - 1 && proxy.UpperBounds[1] >= yIndex)
{
//Add the proxy
if (sortKey != null)
{
AddProxyResult(proxyId, proxy, maxCount, sortKey);
}
else
{
_queryResults[_queryResultCount] = proxyId;
++_queryResultCount;
}
}
}
else
{
if (proxy.LowerBounds[1] <= yIndex && proxy.UpperBounds[1] >= yIndex + 1)
{
//Add the proxy
if (sortKey != null)
{
AddProxyResult(proxyId, proxy, maxCount, sortKey);
}
else
{
_queryResults[_queryResultCount] = proxyId;
++_queryResultCount;
}
}
}
}
//Early out
if (sortKey != null && _queryResultCount == maxCount && _queryResultCount > 0 && xProgress > _querySortKeys[_queryResultCount - 1])
{
break;
}
//Move on to the next bound
if (sx > 0)
{
xIndex++;
if (xIndex == _proxyCount * 2)
{
break;
}
}
else
{
xIndex--;
if (xIndex < 0)
{
break;
}
}
xProgress = (_bounds[0][xIndex].Value - p1x) / dx;
}
else
{
if (yProgress > maxLambda)
{
break;
}
//Check that we are entering a proxy, not leaving
if (sy > 0 ? _bounds[1][yIndex].IsLower : _bounds[1][yIndex].IsUpper)
{
//Check the other axis of the proxy
proxyId = _bounds[1][yIndex].ProxyId;
proxy = _proxyPool[proxyId];
if (sx >= 0)
{
if (proxy.LowerBounds[0] <= xIndex - 1 && proxy.UpperBounds[0] >= xIndex)
{
//Add the proxy
if (sortKey != null)
{
AddProxyResult(proxyId, proxy, maxCount, sortKey);
}
else
{
_queryResults[_queryResultCount] = proxyId;
++_queryResultCount;
}
}
}
else
{
if (proxy.LowerBounds[0] <= xIndex && proxy.UpperBounds[0] >= xIndex + 1)
{
//Add the proxy
if (sortKey != null)
{
AddProxyResult(proxyId, proxy, maxCount, sortKey);
}
else
{
_queryResults[_queryResultCount] = proxyId;
++_queryResultCount;
}
}
}
}
//Early out
if (sortKey != null && _queryResultCount == maxCount && _queryResultCount > 0 && yProgress > _querySortKeys[_queryResultCount - 1])
{
break;
}
//Move on to the next bound
if (sy > 0)
{
yIndex++;
if (yIndex == _proxyCount * 2)
{
break;
}
}
else
{
yIndex--;
if (yIndex < 0)
{
break;
}
}
yProgress = (_bounds[1][yIndex].Value - p1y) / dy;
}
}
break;
}
int count = 0;
for (int i = 0; i < _queryResultCount && count < maxCount; ++i, ++count)
{
Box2DXDebug.Assert(_queryResults[i] < Settings.MaxProxies);
Proxy proxy_ = _proxyPool[_queryResults[i]];
Box2DXDebug.Assert(proxy_.IsValid);
userData[i] = proxy_.UserData;
}
// Prepare for next query.
_queryResultCount = 0;
IncrementTimeStamp();
return(count);
}
