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; } }