void CacheCollision(CPhysics a, CPhysics b)
{
var intersection = IntersectionSystem.CheckIntersection(a.Collider, b.Collider);
if (intersection.Collided)
{
// Calculating some collision data right away, since it will be reused multiple times.
var manifold = intersection.GenerateManifold();
var i = _collisionsPool.Take();
_collisionsPool[i].A = a;
_collisionsPool[i].B = b;
_collisionsPool[i].Intersection = intersection;
_collisionsPool[i].Manifold = manifold;
_collisionsPool[i].InvMassSum = a.InverseMass + b.InverseMass;
_collisionsPool[i].ElasticityDirection = (1 + Math.Min(a.Elasticity, b.Elasticity)) * manifold.Direction
// Secret sauce that makes platformer stacking work. Should point in the gravity direction.
+ Vector2.Min(a.DirectionalElasticity, b.DirectionalElasticity) * manifold.Direction;
a.HadCollision = true;
b.HadCollision = true;
}
}
/// <summary>
/// Returns a list of all physics bodies which intersect with given
/// collider and are not the specified owner.
/// </summary>
public static List <CPhysics> GetAllCollisions(ICollider collider, CPhysics owner = null)
{
var topLeft = collider.Position - collider.HalfSize;
var bottomRight = collider.Position + collider.HalfSize;
var cells = Grid.GetFilledCellsInRange(topLeft, bottomRight);
var leaves = new List <QuadTreeNode>();
for (var i = 0; i < cells.Count; i += 1)
{
cells[i].GetLeavesInRange(leaves, topLeft, bottomRight);
}
var collisions = new List <CPhysics>();
for (var i = 0; i < leaves.Count; i += 1)
{
for (var k = 0; k < leaves[i].ItemsCount; k += 1)
{
var physics = leaves[i].GetItem(k);
if (physics != owner && IntersectionSystem.CheckIntersection(collider, physics.Collider).Collided)
{
collisions.Add(physics);
}
}
for (var k = 0; k < leaves[i].ImmovableItemsCount; k += 1)
{
var physics = leaves[i].GetImmovableItem(k);
if (physics != owner && IntersectionSystem.CheckIntersection(collider, physics.Collider).Collided)
{
collisions.Add(physics);
}
}
}
return(collisions);
}
IEnumerator ActivateLightsAuto()
{
while(m_active)
{
switch(m_intersectionSystem)
{
case IntersectionSystem.DUAL:
{
if(m_lightsAGreen)
{
for(int lIndex = 0; lIndex < m_lightsA.Count; lIndex++)
m_lightsA[lIndex].SetStatus( TrafficSystemTrafficLight.Status.GREEN );
for(int lIndex = 0; lIndex < m_lightsB.Count; lIndex++)
m_lightsB[lIndex].SetStatus( TrafficSystemTrafficLight.Status.RED );
if(m_lightsA.Count > 0)
yield return new WaitForSeconds(m_lightsA[0].m_greenDuration);
}
else
{
for(int lIndex = 0; lIndex < m_lightsA.Count; lIndex++)
m_lightsA[lIndex].SetStatus( TrafficSystemTrafficLight.Status.RED );
for(int lIndex = 0; lIndex < m_lightsB.Count; lIndex++)
m_lightsB[lIndex].SetStatus( TrafficSystemTrafficLight.Status.GREEN );
if(m_lightsB.Count > 0)
yield return new WaitForSeconds(m_lightsB[0].m_greenDuration);
}
if(m_lightsAGreen)
{
for(int lIndex = 0; lIndex < m_lightsA.Count; lIndex++)
m_lightsA[lIndex].SetStatus( TrafficSystemTrafficLight.Status.YELLOW );
}
else
{
for(int lIndex = 0; lIndex < m_lightsB.Count; lIndex++)
m_lightsB[lIndex].SetStatus( TrafficSystemTrafficLight.Status.YELLOW );
}
yield return new WaitForSeconds(m_yellowDuration);
if(m_lightsAGreen)
{
for(int lIndex = 0; lIndex < m_lightsA.Count; lIndex++)
m_lightsA[lIndex].SetStatus( TrafficSystemTrafficLight.Status.RED );
}
else
{
for(int lIndex = 0; lIndex < m_lightsB.Count; lIndex++)
m_lightsB[lIndex].SetStatus( TrafficSystemTrafficLight.Status.RED );
}
yield return new WaitForSeconds(m_redDuration);
m_lightsAGreen = !m_lightsAGreen;
if(m_priorityLightQueue.Count > 0)
{
m_intersectionSystem = IntersectionSystem.SINGLE;
for(int lIndex = 0; lIndex < m_priorityLightQueue.Count; lIndex++)
m_priorityLightQueue[lIndex].SetStatus( TrafficSystemTrafficLight.Status.RED, true );
m_priorityLightQueue[0].SetStatus( TrafficSystemTrafficLight.Status.GREEN, true );
yield return new WaitForSeconds(m_priorityLightQueue[0].m_greenDuration);
m_priorityLightQueue[0].SetStatus( TrafficSystemTrafficLight.Status.YELLOW, true );
yield return new WaitForSeconds(m_yellowDuration);
m_priorityLightQueue[0].SetStatus( TrafficSystemTrafficLight.Status.RED, true );
yield return new WaitForSeconds(m_redDuration);
m_priorityLightQueue.RemoveAt(0);
yield return null;
}
m_intersectionSystem = IntersectionSystem.DUAL;
}
break;
case IntersectionSystem.SINGLE:
{
if(m_lightIndex < m_lights.Count)
{
for(int lIndex = 0; lIndex < m_lights.Count; lIndex++)
m_lights[lIndex].SetStatus( TrafficSystemTrafficLight.Status.RED );
m_lights[m_lightIndex].SetStatus( TrafficSystemTrafficLight.Status.GREEN );
yield return new WaitForSeconds(m_lights[m_lightIndex].m_greenDuration);
m_lights[m_lightIndex].SetStatus( TrafficSystemTrafficLight.Status.YELLOW );
yield return new WaitForSeconds(m_yellowDuration);
m_lights[m_lightIndex].SetStatus( TrafficSystemTrafficLight.Status.RED );
yield return new WaitForSeconds(m_redDuration);
m_lightIndex++;
if(m_lightIndex >= m_lights.Count)
m_lightIndex = 0;
}
}
break;
}
yield return null;
}
yield return null;
}
IEnumerator ActivateLightsManually()
{
switch(m_intersectionSystem)
{
case IntersectionSystem.DUAL:
{
if(m_lightsAGreen)
{
for(int lIndex = 0; lIndex < m_lightsA.Count; lIndex++)
m_lightsA[lIndex].SetStatus( TrafficSystemTrafficLight.Status.GREEN );
for(int lIndex = 0; lIndex < m_lightsB.Count; lIndex++)
m_lightsB[lIndex].SetStatus( TrafficSystemTrafficLight.Status.RED );
if(m_lightsA.Count > 0)
yield return new WaitForSeconds(m_lightsA[0].m_greenDuration);
}
else
{
for(int lIndex = 0; lIndex < m_lightsA.Count; lIndex++)
m_lightsA[lIndex].SetStatus( TrafficSystemTrafficLight.Status.RED );
for(int lIndex = 0; lIndex < m_lightsB.Count; lIndex++)
m_lightsB[lIndex].SetStatus( TrafficSystemTrafficLight.Status.GREEN );
if(m_lightsB.Count > 0)
yield return new WaitForSeconds(m_lightsB[0].m_greenDuration);
}
if(m_lightsAGreen)
{
for(int lIndex = 0; lIndex < m_lightsA.Count; lIndex++)
m_lightsA[lIndex].SetStatus( TrafficSystemTrafficLight.Status.YELLOW );
}
else
{
for(int lIndex = 0; lIndex < m_lightsB.Count; lIndex++)
m_lightsB[lIndex].SetStatus( TrafficSystemTrafficLight.Status.YELLOW );
}
yield return new WaitForSeconds(m_yellowDuration);
if(m_lightsAGreen)
{
for(int lIndex = 0; lIndex < m_lightsA.Count; lIndex++)
m_lightsA[lIndex].SetStatus( TrafficSystemTrafficLight.Status.RED );
}
else
{
for(int lIndex = 0; lIndex < m_lightsB.Count; lIndex++)
m_lightsB[lIndex].SetStatus( TrafficSystemTrafficLight.Status.RED );
}
yield return new WaitForSeconds(m_redDuration);
m_lightsAGreen = !m_lightsAGreen;
// TODO: if you want to manually process the priority queue for vehicles turning you'll need to put a bool around the next if statement and toggle that on and off.
// Currently it will automatically let the cars turn left after the above code (both lanes turn red).
if(m_priorityLightQueue.Count > 0)
{
m_intersectionSystem = IntersectionSystem.SINGLE;
for(int lIndex = 0; lIndex < m_priorityLightQueue.Count; lIndex++)
m_priorityLightQueue[lIndex].SetStatus( TrafficSystemTrafficLight.Status.RED, true );
m_priorityLightQueue[0].SetStatus( TrafficSystemTrafficLight.Status.GREEN, true );
yield return new WaitForSeconds(m_priorityLightQueue[0].m_greenDuration);
m_priorityLightQueue[0].SetStatus( TrafficSystemTrafficLight.Status.YELLOW, true );
yield return new WaitForSeconds(m_yellowDuration);
m_priorityLightQueue[0].SetStatus( TrafficSystemTrafficLight.Status.RED, true );
yield return new WaitForSeconds(m_redDuration);
m_priorityLightQueue.RemoveAt(0);
yield return null;
}
m_intersectionSystem = IntersectionSystem.DUAL;
}
break;
case IntersectionSystem.SINGLE:
{
if(m_lightIndex < m_lights.Count)
{
for(int lIndex = 0; lIndex < m_lights.Count; lIndex++)
m_lights[lIndex].SetStatus( TrafficSystemTrafficLight.Status.RED );
m_lights[m_lightIndex].SetStatus( TrafficSystemTrafficLight.Status.GREEN );
yield return new WaitForSeconds(m_lights[m_lightIndex].m_greenDuration);
m_lights[m_lightIndex].SetStatus( TrafficSystemTrafficLight.Status.YELLOW );
yield return new WaitForSeconds(m_yellowDuration);
m_lights[m_lightIndex].SetStatus( TrafficSystemTrafficLight.Status.RED );
yield return new WaitForSeconds(m_redDuration);
m_lightIndex++;
if(m_lightIndex >= m_lights.Count)
m_lightIndex = 0;
}
}
break;
}
yield return null;
m_active = false;
}
IEnumerator ActivateLightsAuto()
{
while (m_active)
{
switch (m_intersectionSystem)
{
case IntersectionSystem.DUAL:
{
DontWalk();
m_walkSequence++;
if (m_lightsAGreen)
{
for (int lIndex = 0; lIndex < m_lightsA.Count; lIndex++)
{
m_lightsA[lIndex].SetStatus(TrafficSystemTrafficLight.Status.GREEN);
}
for (int lIndex = 0; lIndex < m_lightsB.Count; lIndex++)
{
m_lightsB[lIndex].SetStatus(TrafficSystemTrafficLight.Status.RED);
}
if (m_lightsA.Count > 0)
{
yield return(new WaitForSeconds(m_lightsA[0].m_greenDuration));
}
}
else
{
for (int lIndex = 0; lIndex < m_lightsA.Count; lIndex++)
{
m_lightsA[lIndex].SetStatus(TrafficSystemTrafficLight.Status.RED);
}
for (int lIndex = 0; lIndex < m_lightsB.Count; lIndex++)
{
m_lightsB[lIndex].SetStatus(TrafficSystemTrafficLight.Status.GREEN);
}
if (m_lightsB.Count > 0)
{
yield return(new WaitForSeconds(m_lightsB[0].m_greenDuration));
}
}
if (m_lightsAGreen)
{
for (int lIndex = 0; lIndex < m_lightsA.Count; lIndex++)
{
m_lightsA[lIndex].SetStatus(TrafficSystemTrafficLight.Status.YELLOW);
}
}
else
{
for (int lIndex = 0; lIndex < m_lightsB.Count; lIndex++)
{
m_lightsB[lIndex].SetStatus(TrafficSystemTrafficLight.Status.YELLOW);
}
}
yield return(new WaitForSeconds(m_yellowDuration));
if (m_lightsAGreen)
{
for (int lIndex = 0; lIndex < m_lightsA.Count; lIndex++)
{
m_lightsA[lIndex].SetStatus(TrafficSystemTrafficLight.Status.RED);
}
}
else
{
for (int lIndex = 0; lIndex < m_lightsB.Count; lIndex++)
{
m_lightsB[lIndex].SetStatus(TrafficSystemTrafficLight.Status.RED);
}
}
yield return(new WaitForSeconds(m_redDuration));
m_lightsAGreen = !m_lightsAGreen;
//yield return new WaitForSeconds(m_redDuration);
//m_lightsAGreen = !m_lightsAGreen;
if (m_priorityLightQueue.Count > 0 && m_walkSequence < m_walkSequenceMax)
{
m_walkSequence++;
m_intersectionSystem = IntersectionSystem.SINGLE;
for (int lIndex = 0; lIndex < m_priorityLightQueue.Count; lIndex++)
{
m_priorityLightQueue[lIndex].SetStatus(TrafficSystemTrafficLight.Status.RED, true);
}
m_priorityLightQueue[0].SetStatus(TrafficSystemTrafficLight.Status.GREEN, true);
yield return(new WaitForSeconds(m_priorityLightQueue[0].m_greenDuration));
m_priorityLightQueue[0].SetStatus(TrafficSystemTrafficLight.Status.YELLOW, true);
yield return(new WaitForSeconds(m_yellowDuration));
m_priorityLightQueue[0].SetStatus(TrafficSystemTrafficLight.Status.RED, true);
yield return(new WaitForSeconds(m_redDuration));
m_priorityLightQueue.RemoveAt(0);
yield return(null);
}
if (m_pedestrianNodes.Count > 0 && m_walkSequence >= m_walkSequenceMax)
{
// for(int lIndex = 0; lIndex < m_lights.Count; lIndex++)
// m_lights[lIndex].SetStatus( TrafficSystemTrafficLight.Status.RED );
m_walkSequence = 0;
Walk();
float walkDurationPercentage = m_walkDuration * m_dontWalkPercentage;
yield return(new WaitForSeconds(walkDurationPercentage));
DontWalk();
yield return(new WaitForSeconds(m_walkDuration - walkDurationPercentage));
}
m_intersectionSystem = IntersectionSystem.DUAL;
}
break;
case IntersectionSystem.SINGLE:
{
if (m_lightIndex < m_lights.Count)
{
DontWalk();
m_walkSequence++;
for (int lIndex = 0; lIndex < m_lights.Count; lIndex++)
{
m_lights[lIndex].SetStatus(TrafficSystemTrafficLight.Status.RED);
}
m_lights[m_lightIndex].SetStatus(TrafficSystemTrafficLight.Status.GREEN);
yield return(new WaitForSeconds(m_lights[m_lightIndex].m_greenDuration));
m_lights[m_lightIndex].SetStatus(TrafficSystemTrafficLight.Status.YELLOW);
yield return(new WaitForSeconds(m_yellowDuration));
m_lights[m_lightIndex].SetStatus(TrafficSystemTrafficLight.Status.RED);
yield return(new WaitForSeconds(m_redDuration));
if (m_pedestrianNodes.Count > 0 && m_walkSequence >= m_walkSequenceMax)
{
m_walkSequence = 0;
Walk();
float walkDurationPercentage = m_walkDuration * m_dontWalkPercentage;
yield return(new WaitForSeconds(walkDurationPercentage));
DontWalk();
yield return(new WaitForSeconds(m_walkDuration - walkDurationPercentage));
}
m_lightIndex++;
if (m_lightIndex >= m_lights.Count)
{
m_lightIndex = 0;
}
}
}
break;
}
yield return(null);
}
yield return(null);
}
IEnumerator ActivateLightsManually()
{
switch (m_intersectionSystem)
{
case IntersectionSystem.DUAL:
{
DontWalk();
m_walkSequence++;
if (m_lightsAGreen)
{
for (int lIndex = 0; lIndex < m_lightsA.Count; lIndex++)
{
m_lightsA[lIndex].SetStatus(TrafficSystemTrafficLight.Status.GREEN);
}
for (int lIndex = 0; lIndex < m_lightsB.Count; lIndex++)
{
m_lightsB[lIndex].SetStatus(TrafficSystemTrafficLight.Status.RED);
}
if (m_lightsA.Count > 0)
{
yield return(new WaitForSeconds(m_lightsA[0].m_greenDuration));
}
}
else
{
for (int lIndex = 0; lIndex < m_lightsA.Count; lIndex++)
{
m_lightsA[lIndex].SetStatus(TrafficSystemTrafficLight.Status.RED);
}
for (int lIndex = 0; lIndex < m_lightsB.Count; lIndex++)
{
m_lightsB[lIndex].SetStatus(TrafficSystemTrafficLight.Status.GREEN);
}
if (m_lightsB.Count > 0)
{
yield return(new WaitForSeconds(m_lightsB[0].m_greenDuration));
}
}
if (m_lightsAGreen)
{
for (int lIndex = 0; lIndex < m_lightsA.Count; lIndex++)
{
m_lightsA[lIndex].SetStatus(TrafficSystemTrafficLight.Status.YELLOW);
}
}
else
{
for (int lIndex = 0; lIndex < m_lightsB.Count; lIndex++)
{
m_lightsB[lIndex].SetStatus(TrafficSystemTrafficLight.Status.YELLOW);
}
}
yield return(new WaitForSeconds(m_yellowDuration));
float dontWalkRedDuration = m_redDuration * m_dontWalkPercentage;
if (m_lightsAGreen)
{
for (int lIndex = 0; lIndex < m_lightsA.Count; lIndex++)
{
m_lightsA[lIndex].SetStatus(TrafficSystemTrafficLight.Status.RED);
}
}
else
{
for (int lIndex = 0; lIndex < m_lightsB.Count; lIndex++)
{
m_lightsB[lIndex].SetStatus(TrafficSystemTrafficLight.Status.RED);
}
}
yield return(new WaitForSeconds(m_redDuration));
m_lightsAGreen = !m_lightsAGreen;
// TODO: if you want to manually process the priority queue for vehicles turning you'll need to put a bool around the next if statement and toggle that on and off.
// Currently it will automatically let the cars turn left after the above code (both lanes turn red).
if (m_priorityLightQueue.Count > 0 && m_walkSequence < m_walkSequenceMax)
{
m_walkSequence++;
m_intersectionSystem = IntersectionSystem.SINGLE;
for (int lIndex = 0; lIndex < m_priorityLightQueue.Count; lIndex++)
{
m_priorityLightQueue[lIndex].SetStatus(TrafficSystemTrafficLight.Status.RED, true);
}
m_priorityLightQueue[0].SetStatus(TrafficSystemTrafficLight.Status.GREEN, true);
yield return(new WaitForSeconds(m_priorityLightQueue[0].m_greenDuration));
m_priorityLightQueue[0].SetStatus(TrafficSystemTrafficLight.Status.YELLOW, true);
yield return(new WaitForSeconds(m_yellowDuration));
m_priorityLightQueue[0].SetStatus(TrafficSystemTrafficLight.Status.RED, true);
yield return(new WaitForSeconds(m_redDuration));
m_priorityLightQueue.RemoveAt(0);
yield return(null);
}
if (m_pedestrianNodes.Count > 0 && m_walkSequence >= m_walkSequenceMax)
{
m_walkSequence = 0;
Walk();
float walkDurationPercentage = m_walkDuration * m_dontWalkPercentage;
yield return(new WaitForSeconds(walkDurationPercentage));
DontWalk();
yield return(new WaitForSeconds(m_walkDuration - walkDurationPercentage));
}
m_intersectionSystem = IntersectionSystem.DUAL;
}
break;
case IntersectionSystem.SINGLE:
{
if (m_lightIndex < m_lights.Count)
{
DontWalk();
m_walkSequence++;
for (int lIndex = 0; lIndex < m_lights.Count; lIndex++)
{
m_lights[lIndex].SetStatus(TrafficSystemTrafficLight.Status.RED);
}
m_lights[m_lightIndex].SetStatus(TrafficSystemTrafficLight.Status.GREEN);
yield return(new WaitForSeconds(m_lights[m_lightIndex].m_greenDuration));
m_lights[m_lightIndex].SetStatus(TrafficSystemTrafficLight.Status.YELLOW);
yield return(new WaitForSeconds(m_yellowDuration));
m_lights[m_lightIndex].SetStatus(TrafficSystemTrafficLight.Status.RED);
yield return(new WaitForSeconds(m_redDuration));
if (m_pedestrianNodes.Count > 0 && m_walkSequence >= m_walkSequenceMax)
{
m_walkSequence = 0;
Walk();
float walkDurationPercentage = m_walkDuration * m_dontWalkPercentage;
yield return(new WaitForSeconds(walkDurationPercentage));
DontWalk();
yield return(new WaitForSeconds(m_walkDuration - walkDurationPercentage));
}
m_lightIndex++;
if (m_lightIndex >= m_lights.Count)
{
m_lightIndex = 0;
}
}
}
break;
}
yield return(null);
m_active = false;
}
public override void FixedUpdate(List <Component> components)
{
_iterations = 0;
_stopwatch.Start();
// Updating the grid.
Grid.Clear();
for (var i = 0; i < components.Count; i += 1)
{
var physics = (CPhysics)components[i];
Grid.Add(physics);
physics.HadCollision = false;
physics.Collisions.Clear();
}
// Updating the grid.
IntersectionSystem.Update();
_collisionsPool.ReturnAll();
// Getting all intersections and manifolds.
for (var i = 0; i < Grid.FilledCells.Count; i += 1)
{
CacheAllCollisions(Grid.FilledCells[i]);
}
// Getting all intersections and manifolds.
// Resolving collisions.
for (var i = 0; i < _resolveIterations; i += 1)
{
// Doing collision resolving multiple times over and over again
// improves sim results.
for (var k = 0; k < _collisionsPool.TakenObjectsCount; k += 1)
{
ResolveCollision(k);
}
}
// Resolving collisions.
// Correcting positions.
for (var i = 0; i < _collisionsPool.TakenObjectsCount; i += 1)
{
PositionalCorrection(i);
}
// Correcting positions.
/*
* // Adding info about collisions.
* for (var l = 0; l < _cachedCollisions.Count; l += 1)
* {
* var list = _cachedCollisions[l];
* for (var c = 0; c < list.Count; c += 1)
* {
* list[c].A.Collisions.Add(
* new Collision
* {
* Other = list[c].B,
* Manifold = list[c].Manifold
* }
* );
* var otherManifold = list[c].Manifold;
* otherManifold.Direction = -otherManifold.Direction;
* list[c].B.Collisions.Add(
* new Collision
* {
* Other = list[c].A,
* Manifold = otherManifold
* }
* );
* }
* }
* // Adding info about collisions.
*/
// Updating positions.
for (var i = 0; i < components.Count; i += 1)
{
var physics = (CPhysics)components[i];
physics.Collider.Position += TimeKeeper.Time(physics.Speed);
physics.PositionComponent.Position = physics.Collider.Position;
}
// Updating positions.
_stopwatch.Stop();
}
