Kowalski/Physics/Dynamics/World.cs

using System.Collections;
using System.Collections.Generic;
using Kowalski.Mathematics;
namespace Kowalski.Physics
{
public sealed class World
{
private readonly ICollisionDetection m_CollisionDetection;
private KVec3 m_Gravity;
private List<Body> m_BodyList;
private uint m_CurrentBodyId;
private uint m_CurrentShapeId;
private bool m_InsideTimeStep;
private List<Deletion> m_DeletionList;
private ICollisionFilter m_CollisionFilter;
private ICollisionListener m_CollisionListener;
private List<CollisionManifold> m_CollisionList;
private List<CollisionManifold> m_TempCollisionList;
private List<BroadPhaseData> m_BroadPhaseDataList;
private List<Body> m_BroadPhaseActiveList;
// Used in overlap test to adapt to collision module and reduce GC
private SphereShape m_SphereHelper;
private BoxShape m_BoxHelper;
public World(KVec3 gravity, DimensionMode mode = DimensionMode.XYZ)
{
m_CollisionDetection = CollisionDetection.Create(mode);
m_Gravity = gravity;
m_BodyList = new List<Body>();
m_CurrentBodyId = 0;
m_CurrentShapeId = 0;
m_InsideTimeStep = false;
m_DeletionList = new List<Deletion>();
m_CollisionFilter = DefaultCollisionFilter.Singleton;
m_CollisionListener = null;
m_CollisionList = new List<CollisionManifold>();
m_TempCollisionList = new List<CollisionManifold>();
m_BroadPhaseDataList = new List<BroadPhaseData>();
m_BroadPhaseActiveList = new List<Body>();
m_SphereHelper = new SphereShape(KMath.One);
m_BoxHelper = new BoxShape(KVec3.One);
}
public KVec3 Gravity
{
get => m_Gravity;
set => m_Gravity = value;
}
/// <summary>
/// Note: This will change body id and ids of its shapes
/// </summary>
public void AddBody(Body body)
{
if (null == body)
{
throw new System.ArgumentNullException(nameof(body));
}
var broadPhase = body.SetWorld(this);
body.RefreshBounds();
m_BodyList.Add(body);
m_BroadPhaseDataList.Add(broadPhase.Item1);
m_BroadPhaseDataList.Add(broadPhase.Item2);
}
/// <summary>
/// Note: This will reset body id and ids of its shapes
/// </summary>
public void RemoveBody(Body body)
{
if (!m_BodyList.Contains(body))
{
return;
}
if (m_InsideTimeStep)
{
foreach (var deletion in m_DeletionList)
{
if (deletion.Body == body)
{
// Ignore duplicate deletion
return;
}
}
m_DeletionList.Add(new Deletion() { Body = body });
}
else
{
DoRemoveBody(body);
}
}
private void DoRemoveBody(Body body)
{
if (m_BodyList.Remove(body))
{
uint id = body.Id;
for (int i = 0; i < m_CollisionList.Count; ++i)
{
var collision = m_CollisionList[i];
if (collision.BodyA.Id == id || collision.BodyB.Id == id)
{
m_CollisionList.RemoveAt(i);
i--;
m_CollisionListener?.EndShapeCollision(collision);
}
}
m_BroadPhaseDataList.RemoveAll(data => data.Body.Id == id);
body.SetWorld(null);
}
}
internal void RemoveShape(Shape shape)
{
if (m_InsideTimeStep)
{
foreach (var deletion in m_DeletionList)
{
if (deletion.Shape == shape)
{
// Ignore duplicate deletion
return;
}
}
m_DeletionList.Add(new Deletion() { Shape = shape });
}
else
{
DoRemoveShape(shape);
}
}
private void DoRemoveShape(Shape shape)
{
uint id = shape.Id;
for (int i = 0; i < m_CollisionList.Count; ++i)
{
var collision = m_CollisionList[i];
if (collision.ShapeA.Id == id || collision.ShapeB.Id == id)
{
m_CollisionList.RemoveAt(i);
i--;
m_CollisionListener?.EndShapeCollision(collision);
}
}
shape.Parent.DoRemoveShape(shape);
}
internal uint GenerateBodyId()
{
m_CurrentBodyId++;
return m_CurrentBodyId;
}
internal uint GenerateShapeId()
{
m_CurrentShapeId++;
return m_CurrentShapeId;
}
public void SetCollisionFilter(ICollisionFilter filter)
{
m_CollisionFilter = null != filter ? filter : DefaultCollisionFilter.Singleton;
}
public void SetCollisionListener(ICollisionListener listener)
{
m_CollisionListener = listener;
}
/// <summary>
/// The ray extends from origin to origin + direction * maxFraction
/// </summary>
public int Raycast(KVec3 origin, KVec3 direction, Real maxFraction, List<RaycastOutput> results,
uint maskBits = uint.MaxValue)
{
int hitCount = 0;
foreach (var body in m_BodyList)
{
if (!body.Bounds.Raycast(origin, direction, maxFraction))
{
continue;
}
Transformation xf = body.Transform;
foreach (var shape in body.GetShapeList())
{
if ((shape.LayerBits & maskBits) == 0)
{
continue;
}
if (shape.Raycast(xf, origin, direction, maxFraction, out var output))
{
results.Add(output);
hitCount++;
}
}
}
return hitCount;
}
public int OverlapSphere(KVec3 position, Real radius, List<Shape> results, uint maskBits = uint.MaxValue)
{
m_SphereHelper.Position = position;
m_SphereHelper.Radius = radius;
return OverlapShape(m_SphereHelper, results, maskBits);
}
public int OverlapBox(KVec3 center, KVec3 size, KQuat rotation, List<Shape> results,
uint maskBits = uint.MaxValue)
{
m_BoxHelper.Position = center;
m_BoxHelper.Rotation = rotation;
m_BoxHelper.Size = size;
return OverlapShape(m_BoxHelper, results, maskBits);
}
public void Step(Real dt)
{
if (dt <= KMath.Zero)
{
return;
}
try
{
m_InsideTimeStep = true;
Integrate(dt);
Solve();
}
finally
{
m_InsideTimeStep = false;
foreach (var deletion in m_DeletionList)
{
deletion.Execute(this);
}
m_DeletionList.Clear();
}
}
/// <summary>
/// Integrate velocities and positions
/// </summary>
private void Integrate(Real dt)
{
KVec3 dv = m_Gravity * dt;
foreach (var body in m_BodyList)
{
if (body.IsDynamic)
{
if (body.UseGravity)
{
body.Velocity += dv;
}
body.Velocity = body.Velocity / (KMath.One + dt * body.Drag);
body.Position += body.Velocity * dt;
body.VelocityBeforeCollision = body.Velocity;
}
// Refresh every body including static one
// Because body can be changed outside physics engine
body.RefreshBounds();
}
}
/// <summary>
/// Resolve collision
/// </summary>
private void Solve()
{
int n;
// Mark all collisions (generated in previous steps) as ended
m_TempCollisionList.Clear();
n = m_CollisionList.Count;
for (int i = 0; i < n; ++i)
{
var manifold = m_CollisionList[i];
manifold.m_State = CollisionManifold.EndState;
m_TempCollisionList.Add(manifold);
}
m_CollisionList.Clear();
BroadPhase(m_TempCollisionList);
foreach (var item in m_TempCollisionList)
{
var collision = item;
collision.VelocityA = collision.BodyA.VelocityBeforeCollision;
collision.VelocityB = collision.BodyB.VelocityBeforeCollision;
switch (collision.m_State)
{
case CollisionManifold.BeginState:
m_CollisionList.Add(collision);
m_CollisionListener?.BeginShapeCollision(collision);
break;
case CollisionManifold.StayState:
m_CollisionList.Add(collision);
break;
case CollisionManifold.EndState:
m_CollisionListener?.EndShapeCollision(collision);
break;
default:
throw new System.Exception($"Invalid collision state: {collision.m_State}");
}
}
}
private void BroadPhase(List<CollisionManifold> collisionList)
{
// Insertion sort is better than List.Sort() on almost sorted list
// Insertion sort also provides stability
BroadPhaseData.InsertionSort(m_BroadPhaseDataList);
m_BroadPhaseActiveList.Clear();
int n = m_BroadPhaseDataList.Count;
for (int i = 0; i < n; ++i)
{
var data = m_BroadPhaseDataList[i];
if (data.Begin)
{
Body currentBody = data.Body;
bool currentDynamic = currentBody.IsDynamic;
foreach (var activeBody in m_BroadPhaseActiveList)
{
Body bodyA, bodyB;
if (activeBody.IsDynamic)
{
bodyA = activeBody;
bodyB = currentBody;
}
else if (currentDynamic)
{
bodyA = currentBody;
bodyB = activeBody;
}
else
{
// No collision between static bodies
continue;
}
ResolveBodyCollision(collisionList, bodyA, bodyB, m_CollisionFilter, m_CollisionDetection);
}
m_BroadPhaseActiveList.Add(data.Body);
}
else
{
m_BroadPhaseActiveList.Remove(data.Body);
}
}
}
/// <summary>
/// Body A must be dynamic
/// </summary>
private static void ResolveBodyCollision(List<CollisionManifold> collisionList, Body bodyA, Body bodyB,
ICollisionFilter filter, ICollisionDetection detection)
{
if (!Aabb.Intersect(bodyA.Bounds, bodyB.Bounds))
{
return;
}
Transformation xfA = bodyA.Transform;
Transformation xfB = bodyB.Transform;
bool dirtyA = false;
bool dirtyB = false;
ContactManifold manifold;
foreach (var shapeA in bodyA.GetShapeList())
{
bool isSensorA = shapeA.IsSensor;
foreach (var shapeB in bodyB.GetShapeList())
{
CollisionType collisionType = filter.GetShapeCollisionType(shapeA, shapeB);
if (collisionType == CollisionType.None)
{
continue;
}
detection.CollideShapes(shapeA, xfA, shapeB, xfB, out manifold);
if (manifold.IsTouching)
{
// Make sure normal is the direction of separation
if (manifold.Separation < KMath.Zero)
{
manifold.Normal = -manifold.Normal;
manifold.Separation = -manifold.Separation;
}
// Allow overlap slightly and ignore fake collision (and bouncing)
if (manifold.Separation > KMath.Epsilon)
{
// Sensor does not produce collision response
if (!isSensorA && !shapeB.IsSensor && collisionType == CollisionType.Full)
{
// Move the whole body
if (bodyB.IsDynamic)
{
KVec3 halfSV = manifold.GetScaledSeparationVector(KMath.Half);
xfA.Position += halfSV;
xfB.Position -= halfSV;
dirtyA = true;
dirtyB = true;
}
else
{
xfA.Position += manifold.GetSeparationVector();
dirtyA = true;
}
ApplyBouncing(bodyA, shapeA, bodyB, shapeB, manifold.Normal);
}
UpdateCollision(collisionList, bodyA, shapeA, bodyB, shapeB, manifold.Normal);
UpdateCollision(collisionList, bodyB, shapeB, bodyA, shapeA, -manifold.Normal);
}
}
}
}
// Save the final position
if (dirtyA)
{
bodyA.Position = xfA.Position;
bodyA.RefreshBounds();
}
if (dirtyB)
{
bodyB.Position = xfB.Position;
bodyB.RefreshBounds();
}
}
private static void UpdateCollision(List<CollisionManifold> collisionList,
Body bodyA, Shape shapeA, Body bodyB, Shape shapeB, KVec3 normal)
{
CollisionManifold collision;
collision.BodyA = bodyA;
collision.BodyB = bodyB;
collision.ShapeA = shapeA;
collision.ShapeB = shapeB;
collision.Normal = normal;
collision.m_State = CollisionManifold.InitialState;
collision.VelocityA = KVec3.Zero;
collision.VelocityB = KVec3.Zero;
// TODO: Optimize insertion speed (consider using red-black tree)
int index = collisionList.BinarySearch(collision, CollisionManifold.ShapeComparer.Default);
if (index < 0)
{
// New collision between two shapes
collision.m_State = CollisionManifold.BeginState;
collisionList.Insert(~index, collision);
}
else
{
CollisionManifold prev = collisionList[index];
// Record first collision in current step only
if (prev.m_State == CollisionManifold.EndState)
{
collision.m_State = CollisionManifold.StayState;
collisionList[index] = collision;
}
}
}
/// <summary>
/// Changes body velocity only (Body A must be dynamic)
/// </summary>
private static void ApplyBouncing(Body bodyA, Shape shapeA, Body bodyB, Shape shapeB, KVec3 normal)
{
Real t = KMath.Max(shapeA.Bounciness, shapeB.Bounciness);
KVec3 v10 = bodyA.Velocity;
KVec3 v20 = bodyB.Velocity;
// Apply bouncing only when there is one dynamic body moving along opposite of normal
// Note: The normal for body B is the opposite of body A
if (KVec3.Dot(v10, normal) >= KMath.Zero &&
(!bodyB.IsDynamic || KVec3.Dot(v20, normal) <= KMath.Zero))
{
return;
}
if (bodyB.IsDynamic)
{
Real m1 = bodyA.Mass;
Real m2 = bodyB.Mass;
Real invMass = KMath.One / (m1 + m2);
KVec3 v1n = KVec3.Project(v10, normal);
KVec3 v2n = KVec3.Project(v20, normal);
KVec3 v1p = v10 - v1n;
KVec3 v2p = v20 - v2n;
// Bouncing happens on normal direction only
bodyA.Velocity = (v1n * (m1 - t * m2) + v2n * ((KMath.One + t) * m2)) * invMass + v1p;
bodyB.Velocity = (v2n * (m2 - t * m1) + v1n * ((KMath.One + t) * m1)) * invMass + v2p;
}
else
{
// Body B has infinite mass
KVec3 v1n = KVec3.Project(v10, normal);
KVec3 v1p = v10 - v1n;
bodyA.Velocity = v1p - v1n * t;
}
}
private int OverlapShape(Shape shapeA, List<Shape> results, uint maskBits)
{
int hitCount = 0;
Transformation xfA = Transformation.Default;
Aabb tmp = shapeA.CalculateBounds(shapeA.Transform);
foreach (var bodyB in m_BodyList)
{
if (!Aabb.Intersect(tmp, bodyB.Bounds))
{
continue;
}
Transformation xfB = bodyB.Transform;
foreach (var shapeB in bodyB.GetShapeList())
{
if ((shapeB.LayerBits & maskBits) == 0)
{
continue;
}
m_CollisionDetection.CollideShapes(shapeA, xfA, shapeB, xfB, out var manifold);
if (manifold.IsTouching)
{
results.Add(shapeB);
hitCount++;
}
}
}
return hitCount;
}
private struct Deletion
{
public Body Body;
public Shape Shape;
public void Execute(World world)
{
if (Body != null)
{
world.DoRemoveBody(Body);
}
if (Shape != null)
{
world.DoRemoveShape(Shape);
}
}
}
}
}