public void GenerateBounds()
{
if (Shape == ColliderType.Circle)
{
_radius = Radius;
}
else if (Shape == ColliderType.AABox)
{
_radius = FixedMath.Sqrt((HalfHeight * HalfHeight + HalfWidth * HalfWidth) >> FixedMath.SHIFT_AMOUNT);
}
else if (Shape == ColliderType.Polygon)
{
long BiggestSqrRadius = Vertices[0].SqrMagnitude();
for (int i = 1; i < Vertices.Length; i++)
{
long sqrRadius = Vertices[i].SqrMagnitude();
if (sqrRadius > BiggestSqrRadius)
{
BiggestSqrRadius = sqrRadius;
}
}
_radius = FixedMath.Sqrt(BiggestSqrRadius);
FastRadius = this.Radius * this.Radius;
}
}
/// <summary>
/// This vector's magnitude.
/// </summary>
public long Magnitude()
{
temp1 = (this.x * this.x + this.y * this.y);
if (temp1 == 0)
return 0;
temp1 >>= FixedMath.SHIFT_AMOUNT;
return FixedMath.Sqrt(temp1);
}
public long Distance(long otherX, long otherY)
{
temp1 = this.x - otherX;
temp1 *= temp1;
temp2 = this.y - otherY;
temp2 *= temp2;
return(FixedMath.Sqrt((temp1 + temp2) >> FixedMath.SHIFT_AMOUNT));
}
public void Normalize()
{
long magnitude = FixedMath.Sqrt(x.Mul(x) + y.Mul(y) + z.Mul(z));
x = x.Div(magnitude);
y = y.Div(magnitude);
z = z.Div(magnitude);
}
/// <summary>
/// This vector's magnitude.
/// </summary>
public long Magnitude()
{
temp1 = (this.x * this.x + this.y * this.y) >> FixedMath.SHIFT_AMOUNT;
if (temp1 == 0)
{
return(0);
}
return(FixedMath.Sqrt((this.x * this.x + this.y * this.y) >> FixedMath.SHIFT_AMOUNT));
}
public void CalculateAndExecuteBehaviors()
{
Move mover;
if (movers.Count >= MinGroupSize)
{
averageCollisionSize = 0;
groupPosition = Vector2d.zero;
for (int i = 0; i < movers.Count; i++)
{
mover = movers [i];
groupPosition += mover.Position;
averageCollisionSize += mover.CollisionSize;
}
groupPosition /= movers.Count;
averageCollisionSize /= movers.Count;
long biggestSqrDistance = 0;
for (int i = 0; i < movers.Count; i++)
{
long currentSqrDistance = movers [i].Position.SqrDistance(groupPosition.x, groupPosition.y);
if (currentSqrDistance > biggestSqrDistance)
{
long currentDistance = FixedMath.Sqrt(currentSqrDistance);
/*
* DistDif = currentDistance - Radius;
* if (DistDif > MaximumDistDif * MoversCount / 128) {
* ExecuteGroupIndividualMove ();
* return;
* }*/
biggestSqrDistance = currentSqrDistance;
radius = currentDistance;
}
}
if (radius == 0)
{
ExecuteGroupIndividualMove();
return;
}
long expectedSize = averageCollisionSize.Mul(averageCollisionSize).Mul(FixedMath.One * 2).Mul(movers.Count);
long groupSize = radius.Mul(radius);
if (groupSize > expectedSize || groupPosition.FastDistance(Destination.x, Destination.y) < (radius * radius))
{
ExecuteGroupIndividualMove();
return;
}
ExecuteGroupMove();
}
else
{
ExecuteIndividualMove();
}
}
public Vector3d Normalize()
{
long magnitude = FixedMath.Sqrt(x.Mul(x) + y.Mul(y) + z.Mul(z));
if (magnitude == 0)
{
return(Vector3d.zero);
}
return(new Vector3d(x.Div(magnitude), y.Div(magnitude), z.Div(magnitude)));
}
public static Vector3d Normalize(Vector3d v)
{
long magnitude = FixedMath.Sqrt(v.x.Mul(v.x) + v.y.Mul(v.y) + v.z.Mul(v.z));
if (magnitude == 0)
{
return(Vector3d.zero);
}
return(new Vector3d(v.x.Div(magnitude), v.y.Div(magnitude), v.z.Div(magnitude)));
}
Vector2d GetAdjustVector(Vector2d desiredVel)
{
var adjust = desiredVel - cachedBody._velocity;
var adjustFastMag = adjust.FastMagnitude();
//Cap acceleration vector magnitude
if (adjustFastMag > timescaledAcceleration * (timescaledAcceleration))
{
var mag = FixedMath.Sqrt(adjustFastMag >> FixedMath.SHIFT_AMOUNT);
adjust *= timescaledAcceleration.Div(mag);
}
return(adjust);
}
public static long Distance(Vector3d a, Vector3d b)
{
long tX = b.x - a.x;
tX *= tX;
tX >>= FixedMath.SHIFT_AMOUNT;
long tY = b.y - a.y;
tY *= tY;
tY >>= FixedMath.SHIFT_AMOUNT;
long tZ = b.z - a.z;
tZ *= tZ;
tZ >>= FixedMath.SHIFT_AMOUNT;
return(FixedMath.Sqrt(tX + tY + tZ));
}
public long Distance(Vector3d other)
{
long tX = other.x - x;
tX *= tX;
tX >>= FixedMath.SHIFT_AMOUNT;
long tY = other.y - y;
tY *= tY;
tY >>= FixedMath.SHIFT_AMOUNT;
long tZ = other.z - z;
tZ *= tZ;
tZ >>= FixedMath.SHIFT_AMOUNT;
return(FixedMath.Sqrt(tX + tY + tZ));
}
public static Vector2d GenerateRandomPointOnCircle(bool evenDistribution = false)
{
long angle = LSUtility.GetRandomOne().Mul(FixedMath.TwoPi);
long distance = LSUtility.GetRandomOne();
if (evenDistribution)
{
distance = FixedMath.Sqrt(distance);
}
Vector2d randomOffset = new Vector2d(
FixedMath.Trig.Cos(angle),
FixedMath.Trig.Sin(angle)
) * distance;
return(randomOffset);
}
public void ClampMagnitude(long min, long max)
{
long mag = this.FastMagnitude();
long fastMin;
long fastMax;
long normal;
//Check if normalization is needed and if so, set scale to normalize to
if (mag < (fastMin = min * min))
{
normal = fastMin;
}
else if (mag > (fastMax = max * max))
{
normal = fastMax;
}
else
{
return;
}
mag = FixedMath.Sqrt(mag >> FixedMath.SHIFT_AMOUNT);
if (mag.MoreThanEpsilon())
{
//convert from fast multiplied value
normal = FixedMath.Sqrt(normal >> FixedMath.SHIFT_AMOUNT);
//Shift unneeded as fixed fraction canceled through mul then div
x = x * normal / mag;
y = y * normal / mag;
}
else
{
x = 0;
y = 0;
}
}
public void DistributeCircle_Box(LSBody box, LSBody circle)
{
xMore = circle.Position.x > box.Position.x;
yMore = circle.Position.y > box.Position.y;
if (xMore)
{
PenetrationX = (circle.XMin - box.XMax);
if (PenetrationX > 0)
{
PenetrationX = 0;
}
}
else
{
PenetrationX = (circle.XMax - box.XMin);
if (PenetrationX < 0)
{
PenetrationX = 0;
}
}
if (yMore)
{
PenetrationY = (circle.YMin - box.YMax);
if (PenetrationY > 0)
{
PenetrationY = 0;
}
}
else
{
PenetrationY = (circle.YMax - box.YMin);
if (PenetrationY < 0)
{
PenetrationY = 0;
}
}
xAbs = PenetrationX < 0 ? -PenetrationX : PenetrationX;
yAbs = PenetrationY < 0 ? -PenetrationY : PenetrationY;
if (xAbs > yAbs)
{
PenetrationX = 0;
}
else
{
PenetrationY = 0;
}
//Resolving
if (Vector2d.Dot(PenetrationX, PenetrationY, circle.Velocity.x, circle.Velocity.y) < 0)
{
return;
}
circle.Position.x -= PenetrationX; //(PenetrationX * Multiplier) >> FixedMath.SHIFT_AMOUNT;
circle.Position.y -= PenetrationY; //(PenetrationY * Multiplier) >> FixedMath.SHIFT_AMOUNT;
if (LeCollisionType != CollisionType.Circle_AABox)
{
Mag = FixedMath.Sqrt((PenetrationX * PenetrationX + PenetrationY * PenetrationY) >> FixedMath.SHIFT_AMOUNT);
PenetrationX = (PenetrationX << FixedMath.SHIFT_AMOUNT) / Mag;
PenetrationY = (PenetrationY << FixedMath.SHIFT_AMOUNT) / Mag;
}
else
{
if (PenetrationX != 0)
{
PenetrationX = PenetrationX > 0 ? FixedMath.One : -FixedMath.One;
}
else if (PenetrationY != 0)
{
PenetrationY = PenetrationY > 0 ? FixedMath.One : -FixedMath.One;
}
}
circle.PositionChanged = true;
circle.BuildBounds();
}
internal void DistributeCollision()
{
if (!DoPhysics)
{
return;
}
switch (LeCollisionType)
{
case CollisionType.Circle_Circle:
dist = FixedMath.Sqrt(FastDistance >> FixedMath.SHIFT_AMOUNT);
depth = (Body1.Radius + Body2.Radius - dist);
if (depth <= 0)
{
return;
}
if (dist == 0)
{
dist = 1;
}
//Minimum vector to no longer be colliding
DistX = (DistX * depth / dist);
DistY = (DistY * depth / dist);
//Resolving collision
//Note: Immovable bodies don't check collision against each other so this case doesn't need to be considered
if (Body1.Immovable || (Body2.Immovable == false && Body1.Priority > Body2.Priority))
{
//Invert adjustment values when Body2 is being moved
DistX *= -1;
DistY *= -1;
DistributeCircle_CirclePriority(Body1, Body2);
}
else
{
if (OnlyAffectBody1)
{
return;
}
if (Body2.Immovable || Body2.Priority > Body1.Priority)
{
DistributeCircle_CirclePriority(Body2, Body1);
}
else
{
DistX /= 2;
DistY /= 2;
DistributeCircle(Body1);
DistX *= -1;
DistY *= -1;
DistributeCircle(Body2);
}
}
break;
case CollisionType.Circle_AABox:
if (Body1.Shape == ColliderType.AABox)
{
DistributeCircle_Box(Body1, Body2);
}
else
{
DistributeCircle_Box(Body2, Body1);
}
break;
case CollisionType.Circle_Polygon:
if (Body1.Shape == ColliderType.Circle)
{
this.DistributeCircle_Poly(Body1, Body2);
}
else
{
this.DistributeCircle_Poly(Body2, Body1);
}
break;
}
}
public long Magnitude()
{
long magnitude = FixedMath.Sqrt(x.Mul(x) + y.Mul(y) + z.Mul(z));
return(magnitude);
}
private void DistributeCollision()
{
if (!DoPhysics)
{
return;
}
switch (LeCollisionType)
{
case CollisionType.Circle_Circle:
DistX = Body1._position.x - Body2._position.x;
DistY = Body1._position.y - Body2._position.y;
dist = FixedMath.Sqrt((DistX * DistX + DistY * DistY) >> FixedMath.SHIFT_AMOUNT);
if (dist == 0)
{
//If objects are on the same position, give them push in random direction
const long randomMax = FixedMath.One / 32;
Body1._position.x += LSUtility.GetRandomLong(randomMax) - randomMax / 2;
Body1._position.y += LSUtility.GetRandomLong(randomMax) - randomMax / 2;
Body1.PositionChanged = true;
Body2._position.x += LSUtility.GetRandomLong(randomMax) - randomMax / 2;
Body2._position.y += LSUtility.GetRandomLong(randomMax) - randomMax / 2;
Body2.PositionChanged = true;
return;
}
depth = (Body1.Radius + Body2.Radius - dist);
if (depth <= 0)
{
return;
}
DistX = (DistX * depth / dist);
DistY = (DistY * depth / dist);
//Resolving collision
if (Body1.Immovable || (Body2.Immovable == false && Body1.Priority > Body2.Priority))
{
DistX *= -1;
DistY *= -1;
DistributeCircle_CirclePriority(Body1, Body2);
}
else if (Body2.Immovable || Body2.Priority > Body1.Priority)
{
DistributeCircle_CirclePriority(Body2, Body1);
}
else
{
DistX /= 2;
DistY /= 2;
DistributeCircle(Body1);
DistX *= -1;
DistY *= -1;
DistributeCircle(Body2);
}
break;
case CollisionType.Circle_AABox:
if (Body1.Shape == ColliderType.AABox)
{
DistributeCircle_Box(Body1, Body2);
}
else
{
DistributeCircle_Box(Body2, Body1);
}
break;
case CollisionType.Circle_Polygon:
if (Body1.Shape == ColliderType.Circle)
{
this.DistributeCircle_Poly(Body1, Body2);
}
else
{
this.DistributeCircle_Poly(Body2, Body1);
}
break;
}
}
private void DistributeCollision()
{
if (!DoPhysics)
{
return;
}
switch (LeCollisionType)
{
case CollisionType.Circle_Circle:
DistX = Body1._position.x - Body2._position.x;
DistY = Body1._position.y - Body2._position.y;
dist = FixedMath.Sqrt((DistX * DistX + DistY * DistY) >> FixedMath.SHIFT_AMOUNT);
if (dist == 0)
{
//If objects are on the same position, give them push in random direction
const long randomMax = FixedMath.One / 32;
Body1._position.x += LSUtility.GetRandomLong(randomMax) - randomMax / 2;
Body1._position.y += LSUtility.GetRandomLong(randomMax) - randomMax / 2;
Body1.PositionChanged = true;
Body2._position.x += LSUtility.GetRandomLong(randomMax) - randomMax / 2;
Body2._position.y += LSUtility.GetRandomLong(randomMax) - randomMax / 2;
Body2.PositionChanged = true;
return;
}
depth = (Body1.Radius + Body2.Radius - dist);
if (depth <= 0)
{
return;
}
DistX = (DistX * depth / dist) / 2L;
DistY = (DistY * depth / dist) / 2L;
//Switch, used to be const
bool applyVelocity = false;
//Resolving collision
//TODO: Less copy-paste code
if (Body1.Immovable || (Body2.Immovable == false && Body1.Priority > Body2.Priority))
{
DistX *= -1;
DistY *= -1;
if (Body1.Immovable || Body2.ImmovableCollisionDirection.EqualsZero())
{
Body2._position.x += DistX;
Body2._position.y += DistY;
Body2.PositionChanged = true;
Body2.ImmovableCollisionDirection = new Vector2d(DistX, DistY);
if (applyVelocity)
{
Body2._velocity.x += DistX;
Body2._velocity.y += DistY;
Body2.VelocityChanged = true;
}
}
else
{
//Only move if there isn't an immovable object in that direction
if (Body2.ImmovableCollisionDirection.x.Sign() != DistX.Sign())
{
Body1._position.x += DistX;
}
if (Body2.ImmovableCollisionDirection.y.Sign() != DistY.Sign())
{
Body1._position.y += DistY;
}
}
}
else if (Body2.Immovable || Body2.Priority > Body1.Priority)
{
if (Body2.Immovable || Body1.ImmovableCollisionDirection.EqualsZero())
{
Body2.ImmovableCollisionDirection = new Vector2d(DistX, DistY);
Body1._position.x += DistX;
Body1._position.y += DistY;
Body1.PositionChanged = true;
if (applyVelocity)
{
Body1._velocity.x += DistX;
Body1._velocity.y += DistY;
Body1.VelocityChanged = true;
}
}
else
{
//Only move if there isn't an immovable object in that direction
if (Body1.ImmovableCollisionDirection.x.Sign() != DistX.Sign())
{
Body2._position.x += DistX;
}
if (Body1.ImmovableCollisionDirection.y.Sign() != DistY.Sign())
{
Body2._position.y += DistY;
}
}
}
else
{
DistX /= 2;
DistY /= 2;
Body1._position.x += DistX;
Body1._position.y += DistY;
Body2._position.x -= DistX;
Body2._position.y -= DistY;
Body1.PositionChanged = true;
Body2.PositionChanged = true;
if (applyVelocity)
{
DistX /= 8;
DistY /= 8;
Body1._velocity.x += DistX;
Body1._velocity.y += DistY;
Body1.VelocityChanged = true;
Body2._velocity.x -= DistX;
Body2._velocity.y -= DistY;
Body2.VelocityChanged = true;
}
}
break;
case CollisionType.Circle_AABox:
if (Body1.Shape == ColliderType.AABox)
{
DistributeCircle_Box(Body1, Body2);
}
else
{
DistributeCircle_Box(Body2, Body1);
}
break;
case CollisionType.Circle_Polygon:
if (Body1.Shape == ColliderType.Circle)
{
this.DistributeCircle_Poly(Body1, Body2);
}
else
{
this.DistributeCircle_Poly(Body2, Body1);
}
break;
}
}
private void DistributeCollision()
{
if (Body1.OnContact != null)
{
Body1.OnContact(Body2);
}
if (Body2.OnContact != null)
{
Body2.OnContact(Body1);
}
if (DoPhysics && Body1.HasParent == false && Body2.HasParent == false)
{
switch (LeCollisionType)
{
case CollisionType.Circle_Circle:
DistX = Body1.Position.x - Body2.Position.x;
DistY = Body1.Position.y - Body2.Position.y;
dist = FixedMath.Sqrt((DistX * DistX + DistY * DistY) >> FixedMath.SHIFT_AMOUNT);
if (dist == 0)
{
Body1.Position.x += (LSUtility.GetRandom(1 << (FixedMath.SHIFT_AMOUNT - 2)) + 1);
Body1.Position.y -= (LSUtility.GetRandom(1 << (FixedMath.SHIFT_AMOUNT - 2)) + 1);
return;
}
depth = (Body1.Radius + Body2.Radius - dist);
if (depth < 0)
{
return;
}
DistX = (DistX * depth / dist);
DistY = (DistY * depth / dist);
//Resolving collision
if (physicsFavor == PhysicsFavor.Favor1)
{
Body2.Position.x -= DistX;
Body2.Position.y -= DistY;
Body2.PositionChanged = true;
}
else if (physicsFavor == PhysicsFavor.Favor2)
{
Body1.Position.x += DistX;
Body1.Position.y += DistY;
Body1.PositionChanged = true;
}
else
{
DistX /= 4;
DistY /= 4;
if (Body1.Velocity.Dot(Body2.Velocity.x, Body2.Velocity.y) <= 0)
{
Body1.Velocity.x += DistX; //FixedMath.Mul(DistX, Body1.VelocityMagnitude);
Body1.Velocity.y += DistY; //FixedMath.Mul(DistY, Body1.VelocityMagnitude);
Body1.VelocityChanged = true;
Body2.Velocity.x -= DistX; //FixedMath.Mul(DistX, Body2.VelocityMagnitude);
Body2.Velocity.y -= DistY; //FixedMath.Mul(DistY, Body2.VelocityMagnitude);
Body2.VelocityChanged = true;
Body1.Position.x += DistX;
Body1.Position.y += DistY;
Body2.Position.x -= DistX;
Body2.Position.y -= DistY;
}
else
{
Body1.Position.x += DistX;
Body1.Position.y += DistY;
Body2.Position.x -= DistX;
Body2.Position.y -= DistY;
}
Body1.PositionChanged = true;
Body2.PositionChanged = true;
}
break;
case CollisionType.Circle_AABox:
if (Body1.Shape == ColliderType.AABox)
{
DistributeCircle_Box(Body1, Body2);
}
else
{
DistributeCircle_Box(Body2, Body1);
}
break;
case CollisionType.Circle_Polygon:
break;
}
}
}
private bool InternalRaycast(Vector2d From, Vector2d To, int ExceptionID)
{
_Version++;
MadeContact = false;
Hits.FastClear();
const int StepSize = 1 << Partition.ShiftSize;
x0 = From.x;
y0 = From.y;
x1 = To.x;
y1 = To.y;
if (y1 > y0)
{
compare1 = y1 - y0;
}
else
{
compare1 = y0 - y1;
}
if (x1 > x0)
{
compare2 = x1 - x0;
}
else
{
compare2 = x0 - x1;
}
steep = compare1 > compare2;
if (steep)
{
t = x0; // swap x0 and y0
x0 = y0;
y0 = t;
t = x1; // swap x1 and y1
x1 = y1;
y1 = t;
}
if (x0 > x1)
{
t = x0; // swap x0 and x1
x0 = x1;
x1 = t;
t = y0; // swap y0 and y1
y0 = y1;
y1 = t;
}
dx = x1 - x0;
dy = (y1 - y0);
if (dy < 0)
{
dy = -dy;
}
error = dx / 2;
ystep = (y0 < y1) ? StepSize : -StepSize;
y = y0;
AxisX = From.x - To.x;
AxisY = From.y - To.y;
Mag = FixedMath.Sqrt((AxisX * AxisX + AxisY * AxisY) >> FixedMath.SHIFT_AMOUNT);
if (Mag == 0)
{
return(false);
}
AxisX = FixedMath.Div(AxisX, Mag);
AxisY = FixedMath.Div(AxisY, Mag);
AxisMin = Vector2d.Dot(AxisX, AxisY, From.x, From.y);
AxisMax = Vector2d.Dot(AxisX, AxisY, To.x, To.y);
if (AxisMin > AxisMax)
{
SwapValue = AxisMin;
AxisMin = AxisMax;
AxisMax = SwapValue;
}
PerpProj = Vector2d.Dot(-AxisY, AxisX, From.x, From.y);
XMin = From.x;
XMax = To.x;
if (XMin > XMax)
{
SwapValue = XMin;
XMin = XMax;
XMax = SwapValue;
}
YMin = From.y;
YMax = To.y;
if (YMin > YMax)
{
SwapValue = YMin;
YMin = YMax;
YMax = SwapValue;
}
x = x0;
while (true)
{
if (steep)
{
retX = (y - Partition.OffsetX) / StepSize;
retY = (x - Partition.OffsetY) / StepSize;
}
else
{
retX = (x - Partition.OffsetX) / StepSize;
retY = (y - Partition.OffsetY) / StepSize;
}
PartitionNode node = Partition.Nodes [retX * Partition.Count + retY];
if (node.Count > 0)
{
for (i = 0; i < node.Count; i++)
{
DidHit = false;
LSBody body = PhysicsManager.SimObjects [node [i]];
if (body.RaycastVersion != _Version && body.ID != ExceptionID)
{
body.RaycastVersion = _Version;
switch (body.Shape)
{
case ColliderType.Circle:
Projection = Vector2d.Dot(AxisX, AxisY, body.Position.x, body.Position.y);
TestMin = Projection - body.Radius;
TestMax = Projection + body.Radius;
if (TestMin < AxisMax)
{
if (TestMax > AxisMin)
{
Projection = Vector2d.Dot(-AxisY, AxisX, body.Position.x, body.Position.y);
TestMin = Projection - body.Radius;
TestMax = Projection + body.Radius;
if (PerpProj < TestMax && PerpProj > TestMin)
{
DidHit = true;
}
}
}
break;
case ColliderType.AABox:
if (AxisMin < body.XMax)
{
if (AxisMax > body.XMin)
{
if (PerpProj < body.YMax)
{
if (PerpProj > body.YMin)
{
DidHit = true;
}
}
}
}
break;
}
if (DidHit)
{
Hits.Add(body);
MadeContact = true;
break;
}
}
}
}
error = error - dy;
if (error < 0)
{
y += ystep;
error += dx;
}
if (x >= x1)
{
break;
}
x += StepSize;
}
return(MadeContact);
}
public bool Overlaps(FastList <Vector2d> outputIntersectionPoints)
{
outputIntersectionPoints.FastClear();
//Checks if this object overlaps the line formed by p1 and p2
switch (this.Shape)
{
case ColliderType.Circle:
{
bool overlaps = false;
//Check if the circle completely fits between the line
long projPos = this._position.Dot(cacheAxis.x, cacheAxis.y);
//Circle withing bounds?
if (projPos >= axisMin && projPos <= axisMax)
{
long projPerp = this._position.Dot(cacheAxisNormal.x, cacheAxisNormal.y);
long perpDif = (cacheProjPerp - projPerp);
long perpDist = perpDif.Abs();
if (perpDist <= _radius)
{
overlaps = true;
}
if (overlaps)
{
long sin = (perpDif);
long cos = FixedMath.Sqrt(_radius.Mul(_radius) - sin.Mul(sin));
if (cos == 0)
{
outputIntersectionPoints.Add((cacheAxis * projPos) + perpVector);
}
else
{
outputIntersectionPoints.Add(cacheAxis * (projPos - cos) + perpVector);
outputIntersectionPoints.Add(cacheAxis * (projPos + cos) + perpVector);
}
}
}
else
{
//If not, check distances to points
long p1Dist = _position.FastDistance(cacheP1.x, cacheP2.y);
if (p1Dist <= this.FastRadius)
{
outputIntersectionPoints.Add(cacheP1);
overlaps = true;
}
long p2Dist = _position.FastDistance(cacheP2.x, cacheP2.y);
if (p2Dist <= this.FastRadius)
{
outputIntersectionPoints.Add(cacheP2);
overlaps = true;
}
}
return(overlaps);
}
break;
case ColliderType.AABox:
{
}
break;
case ColliderType.Polygon:
{
bool intersected = false;
for (int i = 0; i < this.Vertices.Length; i++)
{
int edgeIndex = i;
Vector2d pivot = this.RealPoints [edgeIndex];
Vector2d edge = this.Edges [edgeIndex];
long proj1 = 0;
int nextIndex = edgeIndex + 1 < this.RealPoints.Length ? edgeIndex + 1 : 0;
Vector2d nextPoint = RealPoints [nextIndex];
long proj2 = (nextPoint - pivot).Dot(edge);
long min;
long max;
if (proj1 < proj2)
{
min = proj1;
max = proj2;
}
else
{
min = proj2;
max = proj1;
}
long lineProj1 = (cacheP1 - pivot).Dot(edge);
long lineProj2 = (cacheP2 - pivot).Dot(edge);
long lineMin;
long lineMax;
if (lineProj1 < lineProj2)
{
lineMin = lineProj1;
lineMax = lineProj2;
}
else
{
lineMin = lineProj2;
lineMax = lineProj1;
}
if (CollisionPair.CheckOverlap(min, max, lineMin, lineMax))
{
Vector2d edgeNorm = this.EdgeNorms [edgeIndex];
long normProj = 0;
long normLineProj1 = (cacheP1 - pivot).Dot(edgeNorm);
long normLineProj2 = (cacheP2 - pivot).Dot(edgeNorm);
long normLineMin;
long normLineMax;
if (normLineProj1 < normLineProj2)
{
normLineMin = normLineProj1;
normLineMax = normLineProj2;
}
else
{
normLineMin = normLineProj2;
normLineMax = normLineProj1;
}
if (normProj >= normLineMin && normProj <= normLineMax)
{
long revProj1 = pivot.Dot(LSBody.cacheAxisNormal);
long revProj2 = nextPoint.Dot(cacheAxisNormal);
long revMin;
long revMax;
if (revProj1 < revProj2)
{
revMin = revProj1;
revMax = revProj2;
}
else
{
revMin = revProj2;
revMax = revProj1;
}
if (LSBody.cacheProjPerp >= revMin && LSBody.cacheProjPerp <= revMax)
{
intersected = true;
if (LSBody.calculateIntersections)
{
long fraction = normLineProj1.Abs().Div(normLineMax - normLineMin);
long intersectionProj = FixedMath.Lerp(lineProj1, lineProj2, fraction);
outputIntersectionPoints.Add(edge * intersectionProj + pivot);
if (outputIntersectionPoints.Count == 2)
{
break;
}
}
}
}
}
}
return(intersected);
}
break;
}
return(false);
}
private void DistributeCollision()
{
if (Body1.OnContact.IsNotNull())
{
Body1.OnContact(Body2);
}
if (Body2.OnContact.IsNotNull())
{
Body2.OnContact(Body1);
}
if (Body1.IsTrigger || Body2.IsTrigger)
{
return;
}
switch (LeCollisionType)
{
case CollisionType.Circle_Circle:
DistX = Body1._position.x - Body2._position.x;
DistY = Body1._position.y - Body2._position.y;
dist = FixedMath.Sqrt((DistX * DistX + DistY * DistY) >> FixedMath.SHIFT_AMOUNT);
if (dist == 0)
{
const int randomMax = (int)((long)int.MaxValue % (FixedMath.One / 64));
Body1._position.x += LSUtility.GetRandom(randomMax) - randomMax / 2;
Body1._position.y += LSUtility.GetRandom(randomMax) - randomMax / 2;
Body1.PositionChanged = true;
Body2._position.x += LSUtility.GetRandom(randomMax) - randomMax / 2;
Body2._position.y += LSUtility.GetRandom(randomMax) - randomMax / 2;
Body2.PositionChanged = true;
return;
}
depth = (Body1.Radius + Body2.Radius - dist);
if (depth <= 0)
{
return;
}
DistX = (DistX * depth / dist) / 2L;
DistY = (DistY * depth / dist) / 2L;
const bool applyVelocity = false;
//Resolving collision
if (Body1.Immovable && Body1.isActiveAndEnabled || (Body2.Immovable == false && Body1.Priority > Body2.Priority))
{
Body2._position.x -= DistX;
Body2._position.y -= DistY;
Body2.PositionChanged = true;
if (applyVelocity)
{
Body2._velocity.x -= DistX;
Body2.VelocityChanged = true;
}
}
else if (Body2.Immovable || Body2.Priority > Body1.Priority)
{
Body1._position.x += DistX;
Body1._position.y += DistY;
Body1.PositionChanged = true;
if (applyVelocity)
{
Body1._velocity.x += DistX;
Body1._velocity.y += DistY;
Body1.VelocityChanged = true;
}
}
else
{
DistX /= 2;
DistY /= 2;
Body1._position.x += DistX;
Body1._position.y += DistY;
Body2._position.x -= DistX;
Body2._position.y -= DistY;
Body1.PositionChanged = true;
Body2.PositionChanged = true;
if (applyVelocity)
{
DistX /= 8;
DistY /= 8;
Body1._velocity.x += DistX;
Body1._velocity.y += DistY;
Body1.VelocityChanged = true;
Body2._velocity.x -= DistX;
Body2._velocity.y -= DistY;
Body2.VelocityChanged = true;
}
}
break;
case CollisionType.Circle_AABox:
if (Body1.Shape == ColliderType.AABox)
{
DistributeCircle_Box(Body1, Body2);
}
else
{
DistributeCircle_Box(Body2, Body1);
}
break;
case CollisionType.Circle_Polygon:
if (Body1.Shape == ColliderType.Circle)
{
this.DistributeCircle_Poly(Body1, Body2);
}
else
{
this.DistributeCircle_Poly(Body2, Body1);
}
break;
}
}
void BehaveWithTarget()
{
if (Target.IsActive == false || Target.SpawnVersion != targetVersion)
{
StopEngage();
BehaveWithNoTarget();
return;
}
Vector2d targetDirection = Target.Body.Position - cachedBody.Position;
long fastMag = targetDirection.FastMagnitude();
if (fastMag <= fastRangeToTarget)
{
if (!inRange)
{
if (CanMove)
{
cachedMove.StopMove();
}
}
Agent.SetState(AnimState.Engaging);
long mag = FixedMath.Sqrt(fastMag >> FixedMath.SHIFT_AMOUNT);
//cachedTurn.StartTurn(targetDirection / mag);
bool withinTurn = TrackAttackAngle == false ||
(fastMag != 0 &&
cachedBody.Rotation.Dot(targetDirection.x, targetDirection.y) > 0 &&
cachedBody.Rotation.Cross(targetDirection.x, targetDirection.y).Abs() <= AttackAngle);
bool needTurn = mag != 0 && !withinTurn;
if (needTurn)
{
if (CanTurn)
{
targetDirection /= mag;
cachedTurn.StartTurn(targetDirection);
}
}
else
{
if (attackCount <= 0)
{
attackCount = attackFrameCount;
Fire();
}
}
if (inRange == false)
{
inRange = true;
}
}
else
{
if (CanMove)
{
if (cachedMove.IsMoving == false)
{
cachedMove.StartMove(Target.Body.Position);
cachedBody.Priority = basePriority;
}
else
{
if (Target.Body.PositionChanged || inRange)
{
cachedMove.Destination = Target.Body.Position;
}
}
}
if (isAttackMoving || isFocused == false)
{
searchCount -= 1;
if (searchCount <= 0)
{
searchCount = SearchRate;
if (ScanAndEngage())
{
}
else
{
}
}
}
if (inRange == true)
{
inRange = false;
}
}
}
public void Initialize(LSBody b1, LSBody b2)
{
IsValid = true;
if (!IsValid)
{
return;
}
if (b1.ID < b2.ID)
{
Body1 = b1;
Body2 = b2;
}
else
{
Body1 = b2;
Body2 = b1;
}
_ranIndex = -1;
_isColliding = false;
DistX = 0;
DistY = 0;
PenetrationX = 0;
PenetrationY = 0;
FastCollideDistance = b1.Radius + b2.Radius;
FastCollideDistance *= FastCollideDistance;
LeCollisionType = CollisionType.None;
if (Body1.Shape == ColliderType.None || Body2.Shape == ColliderType.None)
{
}
else if (Body1.Shape == ColliderType.Circle)
{
if (Body2.Shape == ColliderType.Circle)
{
LeCollisionType = CollisionType.Circle_Circle;
}
else if (Body2.Shape == ColliderType.AABox)
{
LeCollisionType = CollisionType.Circle_AABox;
}
else if (Body2.Shape == ColliderType.Polygon)
{
LeCollisionType = CollisionType.Circle_Polygon;
}
}
else if (Body1.Shape == ColliderType.AABox)
{
if (Body2.Shape == ColliderType.Circle)
{
LeCollisionType = CollisionType.Circle_AABox;
}
else if (Body2.Shape == ColliderType.AABox)
{
LeCollisionType = CollisionType.AABox_AABox;
}
else if (Body2.Shape == ColliderType.Polygon)
{
LeCollisionType = CollisionType.AABox_Polygon;
}
}
else if (Body1.Shape == ColliderType.Polygon)
{
if (Body2.Shape == ColliderType.Circle)
{
LeCollisionType = CollisionType.Circle_Polygon;
}
else if (Body2.Shape == ColliderType.AABox)
{
LeCollisionType = CollisionType.AABox_Polygon;
}
else if (Body2.Shape == ColliderType.Polygon)
{
LeCollisionType = CollisionType.Polygon_Polygon;
}
}
DoPhysics = ((Body1.IsTrigger || Body2.IsTrigger) == false);
if (DoPhysics)
{
}
//TODO: Space out checks when culled
//TODO: The time between collision checks might cause goofy behavior
//Maybe use a distance or velocity heuristic for culling instead of time since last collision
//It wouldn't be able to replace partitions because of raycasts and fast-moving objects
//Let's see if this works well or if something better is needed.
if (Body1.PreventCulling || Body2.PreventCulling)
{
//Never cull
CullCounter = -1;
}
else
{
//Immediately check collision
CullCounter = 0;
//If collision distance is too large, don't cull based on distance
PreventDistanceCull = FastCollideDistance > PhysicsManager.CullFastDistanceMax;
LastCollidedFrame = LockstepManager.FrameCount;
FastDistanceOffset = FixedMath.Sqrt(FastCollideDistance >> FixedMath.SHIFT_AMOUNT) + FixedMath.One * 2;
FastDistanceOffset *= FastDistanceOffset;
}
Active = true;
_Version++;
}
/// <summary>
/// This vector's magnitude.
/// </summary>
public long Magnitude()
{
return(FixedMath.Sqrt((this.x * this.x + this.y * this.y) >> FixedMath.SHIFT_AMOUNT));
}
public static long Cos(long theta)
{
long sin = Sin(theta);
return(FixedMath.Sqrt(FixedMath.One - (sin.Mul(sin))));
}
public void DistributeCollision()
{
if (!Active)
{
return;
}
if (IsColliding)
{
if (Body1.OnContact != null)
{
Body1.OnContact(Body2);
}
if (Body2.OnContact != null)
{
Body2.OnContact(Body1);
}
if (DoPhysics)
{
switch (LeCollisionType)
{
case CollisionType.Circle_Circle:
DistX = Body1.Position.x - Body2.Position.x;
DistY = Body1.Position.y - Body2.Position.y;
dist = FixedMath.Sqrt((DistX * DistX + DistY * DistY) >> FixedMath.SHIFT_AMOUNT);
if (dist == 0)
{
Body1.Position.x += (LSUtility.GetRandom(1 << (FixedMath.SHIFT_AMOUNT - 2)) + 1);
Body1.Position.y -= (LSUtility.GetRandom(1 << (FixedMath.SHIFT_AMOUNT - 2)) + 1);
Body1.PositionChanged = true;
return;
}
depth = (Body1.Radius + Body2.Radius - dist);
if (depth < 0)
{
return;
}
DistX = (DistX * depth / dist);
DistY = (DistY * depth / dist);
//Resolving collision
if (Body1.Immovable)
{
Body2.Position.x -= DistX;
Body2.Position.y -= DistY;
Body2.PositionChanged = true;
}
else if (Body2.Immovable)
{
Body1.Position.x += DistX;
Body1.Position.y += DistY;
Body1.PositionChanged = true;
}
else
{
DistX /= 4;
DistY /= 4;
if (Body1.Velocity.Dot(Body2.Velocity.x, Body2.Velocity.y) < 0)
{
if (Body1.Mover != null)
{
Body1.Velocity.x += DistX;
Body1.Velocity.y += DistY;
Body1.VelocityChanged = true;
}
else
{
Body1.Position.x += DistX;
Body1.Position.y += DistY;
Body1.PositionChanged = true;
}
if (Body2.Mover != null)
{
Body2.Velocity.x -= DistX;
Body2.Velocity.y -= DistY;
Body2.VelocityChanged = true;
}
else
{
Body2.Position.x -= DistX;
Body2.Position.y -= DistY;
Body2.PositionChanged = true;
}
}
else
{
Body1.Position.x += DistX;
Body1.Position.y += DistY;
Body1.PositionChanged = true;
Body2.Position.x -= DistX;
Body2.Position.y -= DistY;
Body2.PositionChanged = true;
}
}
break;
case CollisionType.Circle_AABox:
if (Body1.Shape == ColliderType.AABox)
{
DistributeCircle_Box(Body1, Body2);
}
else
{
DistributeCircle_Box(Body2, Body1);
}
break;
case CollisionType.Circle_Polygon:
break;
}
}
}
}
private void DistributeCollision()
{
if (Body1.OnContact.IsNotNull())
{
Body1.OnContact(Body2);
}
if (Body2.OnContact.IsNotNull())
{
Body2.OnContact(Body1);
}
if (DoPhysics && Body1.HasParent == false && Body2.HasParent == false)
{
switch (LeCollisionType)
{
case CollisionType.Circle_Circle:
DistX = Body1.Position.x - Body2.Position.x;
DistY = Body1.Position.y - Body2.Position.y;
dist = FixedMath.Sqrt((DistX * DistX + DistY * DistY) >> FixedMath.SHIFT_AMOUNT);
if (dist == 0)
{
const int randomMax = 1000;
Body1.Position.x += LSUtility.GetRandom(randomMax);
Body1.Position.y += LSUtility.GetRandom(randomMax);
Body1.PositionChanged = true;
Body2.Position.x += LSUtility.GetRandom(randomMax);
Body2.Position.y += LSUtility.GetRandom(randomMax);
Body2.PositionChanged = true;
return;
}
depth = (Body1.Radius + Body2.Radius - dist);
if (depth <= 0)
{
return;
}
DistX = (DistX * depth / dist) / 2L;
DistY = (DistY * depth / dist) / 2L;
const bool applyVelocity = true;
//Resolving collision
if (Body1.Immovable || (Body2.Immovable == false && Body1.Priority > Body2.Priority))
{
Body2.Position.x -= DistX;
Body2.Position.y -= DistY;
Body2.PositionChanged = true;
if (applyVelocity)
{
Body2._velocity.x -= DistX;
Body2._velocity.y -= DistY;
Body2.VelocityChanged = true;
}
}
else if (Body2.Immovable || Body2.Priority > Body1.Priority)
{
Body1.Position.x += DistX;
Body1.Position.y += DistY;
Body1.PositionChanged = true;
if (applyVelocity)
{
Body1._velocity.x += DistX;
Body1._velocity.y += DistY;
Body1.VelocityChanged = true;
}
}
else
{
DistX /= 2;
DistY /= 2;
Body1.Position.x += DistX;
Body1.Position.y += DistY;
Body2.Position.x -= DistX;
Body2.Position.y -= DistY;
Body1.PositionChanged = true;
Body2.PositionChanged = true;
if (applyVelocity)
{
DistX /= 8;
DistY /= 8;
Body1._velocity.x += DistX;
Body1._velocity.y += DistY;
Body1.VelocityChanged = true;
Body2._velocity.x -= DistX;
Body2._velocity.y -= DistY;
Body2.VelocityChanged = true;
}
}
break;
case CollisionType.Circle_AABox:
if (Body1.Shape == ColliderType.AABox)
{
DistributeCircle_Box(Body1, Body2);
}
else
{
DistributeCircle_Box(Body2, Body1);
}
break;
case CollisionType.Circle_Polygon:
break;
}
}
}
public static long Distance(Vector2d a, Vector2d b)
{
return(FixedMath.Sqrt(SqrDistance(a, b)));
}
