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);
}
public GridSettings()
{
Init(256, 256, FixedMath.Create(-128), FixedMath.Create(-128), true);
}
protected override void OnSimulate()
{
if (!CanMove)
{
return;
}
//TODO: Organize/split this function
if (IsMoving)
{
if (CanPathfind)
{
if (DoPathfind)
{
DoPathfind = false;
if (viableDestination)
{
if (Pathfinder.GetStartNode(cachedBody.Position, out currentNode))
{
if (currentNode.DoesEqual(this.destinationNode))
{
if (this.RepathTries >= 1)
{
this.Arrive();
}
}
else
{
if (straightPath)
{
if (Pathfinder.NeedsPath(currentNode, destinationNode, this.GridSize))
{
if (Pathfinder.FindPath(Destination, currentNode, destinationNode, myPath,
GridSize, GetNodeHash(destinationNode)))
{
hasPath = true;
pathIndex = 0;
}
else
{
if (IsFormationMoving)
{
StartMove(MyMovementGroup.Destination);
IsFormationMoving = false;
}
}
straightPath = false;
}
else
{
}
}
else
{
if (Pathfinder.NeedsPath(currentNode, destinationNode, this.GridSize))
{
if (Pathfinder.FindPath(Destination, currentNode, destinationNode, myPath,
GridSize, GetNodeHash(destinationNode)))
{
hasPath = true;
pathIndex = 0;
}
else
{
if (IsFormationMoving)
{
StartMove(MyMovementGroup.Destination);
IsFormationMoving = false;
}
}
}
else
{
straightPath = true;
}
}
}
}
else
{
}
}
else
{
hasPath = false;
if (IsFormationMoving)
{
StartMove(MyMovementGroup.Destination);
IsFormationMoving = false;
}
}
}
else
{
}
if (straightPath)
{
targetPos = Destination;
}
else if (hasPath)
{
if (pathIndex >= myPath.Count)
{
targetPos = this.Destination;
}
else
{
targetPos = myPath[pathIndex];
}
}
else
{
targetPos = Destination;
}
}
else
{
targetPos = Destination;
}
movementDirection = targetPos - cachedBody._position;
movementDirection.Normalize(out distance);
if (targetPos.x != lastTargetPos.x || targetPos.y != lastTargetPos.y)
{
lastTargetPos = targetPos;
targetDirection = movementDirection;
}
bool movingToWaypoint = (this.hasPath && this.pathIndex < myPath.Count - 1);
long stuckThreshold = timescaledAcceleration / LockstepManager.FrameRate;
long slowDistance = cachedBody.VelocityMagnitude.Div(timescaledDecceleration);
if (distance > slowDistance || movingToWaypoint)
{
desiredVelocity = (movementDirection);
if (CanTurn)
{
cachedTurn.StartTurnDirection(movementDirection);
}
}
else
{
if (distance < FixedMath.Mul(closingDistance, StopMultiplier))
{
Arrive();
//TODO: Don't skip this frame of slowing down
return;
}
if (distance > closingDistance)
{
if (CanTurn)
{
cachedTurn.StartTurnDirection(movementDirection);
}
}
if (distance <= slowDistance)
{
long closingSpeed = distance.Div(slowDistance);
if (CanTurn)
{
cachedTurn.StartTurnDirection(movementDirection);
}
desiredVelocity = movementDirection * closingSpeed;
decellerating = true;
//Reduce occurence of units preventing other units from reaching destination
stuckThreshold *= 4;
}
}
//If unit has not moved stuckThreshold in a frame, it's stuck
StuckTime++;
if (GetCanAutoStop())
{
if (Agent.Body.Position.FastDistance(AveragePosition) <= (stuckThreshold * stuckThreshold))
{
if (StuckTime > StuckTimeThreshold)
{
if (movingToWaypoint)
{
this.pathIndex++;
}
else
{
if (RepathTries < StuckRepathTries)
{
DoPathfind = true;
RepathTries++;
}
else
{
RepathTries = 0;
this.Arrive();
}
}
StuckTime = 0;
}
}
else
{
if (StuckTime > 0)
{
StuckTime -= 1;
}
RepathTries = 0;
}
}
if (movingToWaypoint)
{
if (
(
this.pathIndex >= 0 &&
distance < closingDistance &&
(movementDirection).Dot(waypointDirection) < 0
) ||
distance < FixedMath.Mul(closingDistance, FixedMath.Half))
{
this.pathIndex++;
}
}
desiredVelocity *= Speed;
cachedBody._velocity += GetAdjustVector(desiredVelocity);
cachedBody.VelocityChanged = true;
}
else
{
decellerating = true;
//Slowin' down
if (cachedBody.VelocityMagnitude > 0)
{
cachedBody.Velocity += GetAdjustVector(Vector2d.zero);
}
StoppedTime++;
}
decellerating = false;
AutoStopPauser--;
CollisionStopPauser--;
StopPauseLooker--;
AveragePosition = AveragePosition.Lerped(Agent.Body.Position, FixedMath.One / 2);
}
long FixedNumberField(Rect position, GUIContent label, long value)
{
return(FixedMath.Create(EditorGUI.DoubleField(position, label, value.ToDouble())));
}
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;
}
}
