public float Distance(Vector2 p)
{
return(GeometryHelper.GetLineToPointDistance(StartPos, EndPos, p));
}
public GeometryHelper.LineIntersectionStates Intersects(LineSegment l2)
{
return(GeometryHelper.LinesIntersect(this, l2, true, true));
}
public bool Contains(Vector2 p)
{
return(GeometryHelper.IsPointOnLine(this, p) == GeometryHelper.PointOnLineStates.PointIsOnTheSegment);
}
public Vector2 GetIntersectionPoint(LineSegment l2)
{
return(GeometryHelper.GetIntersectionPoint(this, l2));
}
void UpdateBody(int i)
{
if (MaxUpdatePerBody < ++bodyUpdateCount[i])
{
return;
}
Vector2 a, v, p, r;
var body = Bodies[i];
//if (body.IsStatic) return;
a = body.Force * body.InverseMass -
(body.AttachToGravity ?
new Vector2(Gravity.X * body.GravityNormal.X, Gravity.Y * body.GravityNormal.Y) :
Vector2.Zero) + body.Acceleration;
v = a * Speed + body.Velocity;
Vector2 position, size;
body.Mesh.GetPositionAndSize(out position, out size);
v.X = MathHelper.Clamp(v.X, -body.MaxSpeed.X, body.MaxSpeed.X);
v.Y = MathHelper.Clamp(v.Y, -body.MaxSpeed.Y, body.MaxSpeed.Y);
r = v * Speed;
p = r + position;
body.Mesh.AutoTriangulate = true;
if (!body.Convex && body.Mesh.Triangles == null)
{
body.Mesh.Triangulate();
}
body.Mesh.Offset(r);
if (body.IsStatic)
{
return;
}
List <KeyValuePair <Polygon, Polygon> > polys = new List <KeyValuePair <Polygon, Polygon> >();
if (!body.IsFree && !body.IsStatic)
{
for (int j = 0; j < Bodies.Count; j++)
{
var item = Bodies[j];
if (i == j)
{
continue;
}
if (item.NotCollideWith.Contains(body) ||
body.NotCollideWith.Contains(item))
{
continue;
}
var push = Vector2.Zero;
polys.Clear();
if (item.Convex && body.Convex)
{
if (Polygon.Collide(body.Mesh, item.Mesh, out push))
{
polys.Add(new KeyValuePair <Polygon, Polygon>(body.Mesh, item.Mesh));
}
}
else
{
#region SAT With Triangles
if (body.Convex)
{
if (item.Mesh.Triangles == null)
{
item.Mesh.Triangulate();
}
foreach (var pTri in item.Mesh.Triangles)
{
var temppush = Vector2.Zero;
if (Polygon.Collide(body.Mesh, pTri, out temppush))
{
if (polys.Count == 0)
{
push = temppush;
}
polys.Add(new KeyValuePair <Polygon, Polygon>(body.Mesh, pTri));
}
}
}
else if (item.Convex)
{
foreach (var pTri in body.Mesh.Triangles)
{
var temppush = Vector2.Zero;
if (Polygon.Collide(pTri, item.Mesh, out temppush))
{
if (polys.Count == 0)
{
push = temppush;
}
polys.Add(new KeyValuePair <Polygon, Polygon>(pTri, item.Mesh));
}
}
}
else
{
foreach (var p1 in body.Mesh.Triangles)
{
if (item.Mesh.Triangles == null)
{
item.Mesh.Triangulate();
}
foreach (var p2 in item.Mesh.Triangles)
{
var temppush = Vector2.Zero;
if (Polygon.Collide(p1, p2, out temppush))
{
if (polys.Count == 0)
{
push = temppush;
}
polys.Add(new KeyValuePair <Polygon, Polygon>(p1, p2));
}
}
}
}
#endregion
}
for (int pi = 0; pi < polys.Count; pi++)
{
if (pi == 0 || Polygon.Collide(polys[pi].Key, polys[pi].Value, out push))
{
Vector2 pd = push;
pd.Normalize();
pd = push - AllowedPenetrationDepth * pd;
if (body.PreventSlippingOnSlopes)
{
if (Math.Abs(pd.X) < StickCoef)
{
pd.X = 0;
}
}
if (GeometryHelper.IsNaN(pd))
{
pd = Vector2.Zero;
}
/*
* EXPERIMENT:
* Move both bodies based on their weight
*/
else if (item.IsStatic)
{
if (!item.IsFree)
{
body.Mesh.Offset(pd);
}
if (body.Collide != null)
{
body.Collide(item.Entity, pd);
}
}
else
{
//Lerp between masses
var massSum = body.InverseMass + item.InverseMass;
var bodyW = body.InverseMass / massSum;
var itemW = bodyW - 1.0f;
var bodyPV = pd * bodyW;
var itemPV = pd * itemW;
if (!item.IsFree)
{
UpdateBody(j);
body.Mesh.Offset(bodyPV);
}
if (body.Collide != null)
{
body.Collide(item.Entity, bodyPV);
}
if (item.Collide != null)
{
item.Collide(body.Entity, itemPV);
}
}
}
}
}
}
var newP = body.Mesh.GetPosition();
var movedDistance = newP - p;
if (movedDistance.Length() > _bigNumber) //We don't want no weird teleports, do we?
{
body.Mesh.Offset(-movedDistance); //rollback
}
else
{
v += movedDistance / Speed;
}
body.Velocity = v * DampingCoef;
body.Acceleration = a;
/////////////////////
// Check for NaNs generated by bugs
/////////////////////
if (GeometryHelper.IsNaN(body.Velocity))
{
body.Velocity = Vector2.Zero;
}
if (GeometryHelper.IsNaN(body.Acceleration))
{
body.Acceleration = Vector2.Zero;
}
if (body.MaxSpeed.X >= 0 && Math.Abs(body.Velocity.X) > body.MaxSpeed.X)
{
body.Velocity.X = body.MaxSpeed.X * Math.Sign(body.Velocity.X);
}
if (body.MaxSpeed.Y >= 0 && Math.Abs(body.Velocity.Y) > body.MaxSpeed.Y)
{
body.Velocity.Y = body.MaxSpeed.Y * Math.Sign(body.Velocity.Y);
}
if (body.MaxAcceleration.X >= 0 && Math.Abs(body.Acceleration.X) > body.MaxAcceleration.X)
{
body.Acceleration.X = body.MaxAcceleration.X * Math.Sign(body.Acceleration.X);
}
if (body.MaxAcceleration.Y >= 0 && Math.Abs(body.Velocity.Y) > body.MaxAcceleration.Y)
{
body.Acceleration.Y = body.MaxAcceleration.Y * Math.Sign(body.Acceleration.Y);
}
body.Force = Vector2.Zero;
body.Acceleration = Vector2.Zero;
}
void bd_impact(IList <string> args)
{
ApplyImpact(GeometryHelper.String2Vector(args[1]));
}