public void GenerateBounds()
{
if (Shape == ColliderType.Circle)
{
_radius = Radius;
}
else if (Shape == ColliderType.AABox)
{
_radius = FixedMath.Sqrt((HalfLength * HalfLength + 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;
}
}
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);
}
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();
}
}
/// <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(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);
}
Vector2d GetAdjustVector(Vector2d desiredVel)
{
var adjust = desiredVel - cachedBody._velocity;
var adjustFastMag = adjust.FastMagnitude();
//Cap acceleration vector magnitude
float accel = timescaledAcceleration;
if (decellerating)
{
accel = timescaledDecceleration;
}
if (adjustFastMag > timescaledAcceleration * (timescaledAcceleration))
{
var mag = FixedMath.Sqrt(adjustFastMag >> FixedMath.SHIFT_AMOUNT);
adjust *= timescaledAcceleration.Div(mag);
}
return(adjust);
}
public static long Cos(long theta)
{
long sin = Sin(theta);
return(FixedMath.Sqrt(FixedMath.One - (sin.Mul(sin))));
}
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++;
}
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;
}
}
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);
}
