private static void CreateConstraint(PhysicsWorld world, float3 up,
int hitRigidBodyIndex, ColliderKey hitColliderKey, float3 hitPosition, float3 hitSurfaceNormal,
float hitDistance,
float skinWidth, float maxSlopeCos, ref NativeList <SurfaceConstraintInfo> constraints)
{
CreateConstraintFromHit(world, hitRigidBodyIndex, hitColliderKey, hitPosition,
hitSurfaceNormal, hitDistance, skinWidth, out var constraint);
// Check if max slope plane is required
var verticalComponent = math.dot(constraint.Plane.Normal, up);
var shouldAddPlane = verticalComponent > KSimplexSolverEpsilon && verticalComponent < maxSlopeCos;
if (shouldAddPlane)
{
constraint.IsTooSteep = true;
CreateMaxSlopeConstraint(up, ref constraint, out var maxSlopeConstraint);
constraints.Add(maxSlopeConstraint);
}
// Prepare velocity to resolve penetration
ResolveConstraintPenetration(ref constraint);
// Add original constraint to the list
constraints.Add(constraint);
}
private static bool IsTransparent(BlobAssetReference <Collider> collider, ColliderKey key)
{
bool bIsTransparent = false;
unsafe
{
// Only Convex Colliders have Materials associated with them. So base on CollisionType
// we'll need to cast from the base Collider type, hence, we need the pointer.
var c = (Collider *)collider.GetUnsafePtr();
{
var cc = ((ConvexCollider *)c);
// We also need to check if our Collider is Composite (i.e. has children).
// If it is then we grab the actual leaf node hit by the ray.
// Checking if our collider is composite
if (c->CollisionType != CollisionType.Convex)
{
// If it is, get the leaf as a Convex Collider
c->GetLeaf(key, out ChildCollider child);
cc = (ConvexCollider *)child.Collider;
}
// Now we've definitely got a ConvexCollider so can check the Material.
bIsTransparent = (cc->Material.CustomTags & k_TransparentCustomTag) != 0;
}
}
return(bIsTransparent);
}
private static unsafe void ConvexComposite(
Context context, ColliderKey convexKeyA,
Collider *convexColliderA, Collider *compositeColliderB, MTransform worldFromA, MTransform worldFromB,
MotionExpansion expansion, bool flipped, ref BlockStream.Writer contactWriter)
{
// Calculate AABB of A in B
MTransform bFromWorld = Inverse(worldFromB);
MTransform bFromA = Mul(bFromWorld, worldFromA);
var transform = new RigidTransform(new quaternion(bFromA.Rotation), bFromA.Translation); // TODO: avoid this conversion to and back from float3x3
Aabb aabbAinB = expansion.ExpandAabb(convexColliderA->CalculateAabb(transform));
// Do the midphase query and build manifolds for any overlapping leaf colliders
var input = new OverlapAabbInput {
Aabb = aabbAinB, Filter = convexColliderA->Filter
};
var collector = new ConvexCompositeOverlapCollector(
context,
convexColliderA, convexKeyA, compositeColliderB,
worldFromA, worldFromB, expansion.MaxDistance, flipped,
contactWriter);
OverlapQueries.AabbCollider(input, compositeColliderB, ref collector);
// Keep updated writer state
contactWriter = collector.m_ContactWriter;
}
private ColliderGroup CreateColliderGroup(ColliderGrid grid, ColliderCell cell, ColliderKey key)
{
ColliderGroup colliderGroup = Facepunch.Pool.Get <ColliderGroup>();
colliderGroup.Initialize(grid, cell, key);
return(colliderGroup);
}
public void AddLeaf(ColliderKey key, ref ChildCollider leaf)
{
MTransform worldFromLeafA = Mul(m_WorldFromA, new MTransform(leaf.TransformFromChild));
ConvexComposite(
m_Context, m_KeyPath.GetLeafKey(key), leaf.Collider, m_CompositeColliderB,
worldFromLeafA, m_WorldFromB, m_Expansion, m_Flipped);
}
public void TransformNewHits(int oldNumHits, float oldFraction, Math.MTransform transform, uint numSubKeyBits, uint subKey)
{
m_lastColliderKey = ColliderKey.Empty;
m_lastColliderKey.PushSubKey(numSubKeyBits, subKey);
if (m_ClosestHit.Fraction < oldFraction)
{
m_ClosestHit.Transform(transform, numSubKeyBits, subKey);
}
}
private ColliderGroup CreateColliderGroup(
ColliderGrid grid,
ColliderCell cell,
ColliderKey key)
{
M0 m0 = Pool.Get <ColliderGroup>();
((ColliderGroup)m0).Initialize(grid, cell, key);
return((ColliderGroup)m0);
}
public ColliderKey SetSubKey(uint childSubKeyNumOfBits, uint childSubKey)
{
var parentColliderKey = ColliderKey;
parentColliderKey.PopSubKey(NumColliderKeyBits, out uint parentKey);
var colliderKey = new ColliderKey(childSubKeyNumOfBits, childSubKey);
colliderKey.PushSubKey(NumColliderKeyBits, parentKey);
return(colliderKey);
}
private void FallbackToPreviousHit()
{
MaxFraction = m_prevMaxFraction;
m_prevMaxFraction = 0;
m_ClosestHit = m_OldHit;
m_OldHit = default;
m_lastColliderKey = m_prevColliderKey;
m_prevColliderKey = ColliderKey.Empty;
NumHits = m_prevNumHits;
m_prevNumHits = 0;
}
public QueryContext(int physicsBodyIndex, Entity entity, PhysicsTransform localToWorldTransform)
{
PhysicsBodyIndex = physicsBodyIndex;
Entity = entity;
LocalToWorldTransform = localToWorldTransform;
ColliderKey = ColliderKey.Empty;
NumColliderKeyBits = 0;
m_IsInitialized = 1;
}
public ColliderGroup FindBatchGroup(ColliderBatch collider)
{
ColliderKey key = new ColliderKey(collider);
ColliderGroup colliderGroup;
if (!this.batches.TryGetValue(key, ref colliderGroup))
{
colliderGroup = this.CreateColliderGroup(this.grid, this, key);
this.batches.Add(key, colliderGroup);
}
return(colliderGroup);
}
public bool AddHit(T hit)
{
Assert.IsTrue(hit.Fraction <= MaxFraction);
m_prevMaxFraction = MaxFraction;
MaxFraction = hit.Fraction;
m_OldHit = m_ClosestHit;
m_ClosestHit = hit;
m_prevNumHits = NumHits;
NumHits = 1;
m_prevColliderKey = m_lastColliderKey;
m_lastColliderKey = ColliderKey.Empty;
return(true);
}
public SelfAndTriggerFilteringClosestHitCollector(PhysicsWorld world, int rbIndex, float maxFraction)
{
m_prevMaxFraction = 0;
MaxFraction = maxFraction;
m_OldHit = default;
m_ClosestHit = default;
m_prevNumHits = 0;
NumHits = 0;
m_selfRBIndex = rbIndex;
m_world = world;
m_lastColliderKey = ColliderKey.Empty;
m_prevColliderKey = ColliderKey.Empty;
}
/// <summary>
/// Given the root Collider of a hierarchy and a ColliderKey referencing a child in that hierarchy,
/// this function returns the ChildCollider requested.
/// </summary>
/// <param name="rootColliderPtr">A <see cref="Collider"/> at the root of a Collider hierarchy.</param>
/// <param name="childColliderKey">A <see cref="ColliderKey"/> referencing a child Collider somewhere in the hierarchy below rootColliderPtr.</param>
/// <param name="childCollider">A valid <see cref="ChildCollider"/> returned from the hierarchy, if found.</param>
/// <returns>Whether a specified ColliderKey was successfully found in the hierarchy.</returns>
public static unsafe bool TryGetChildInHierarchy(Collider *rootColliderPtr, ColliderKey childColliderKey, out ChildCollider childCollider)
{
//public static unsafe bool GetLeafCollider(Collider* root, RigidTransform rootTransform, ColliderKey key, out ChildCollider leaf)
childCollider = new ChildCollider(rootColliderPtr, RigidTransform.identity);
while (!childColliderKey.Equals(ColliderKey.Empty))
{
if (!childCollider.Collider->GetChild(ref childColliderKey, out ChildCollider child))
{
break;
}
childCollider = new ChildCollider(childCollider, child);
}
return(childCollider.Collider != null);
}
public ConvexCompositeOverlapCollector(
Context context,
Collider *convexCollider, ColliderKey convexColliderKey, Collider *compositeCollider,
MTransform worldFromA, MTransform worldFromB, float collisionTolerance, bool flipped)
{
m_Context = context;
m_ConvexColliderA = convexCollider;
m_ConvexColliderKey = convexColliderKey;
m_CompositeColliderB = compositeCollider;
m_CompositeColliderKeyPath = ColliderKeyPath.Empty;
m_WorldFromA = worldFromA;
m_WorldFromB = worldFromB;
m_CollisionTolerance = collisionTolerance;
m_Flipped = flipped;
}
private static SurfaceConstraintInfo CreateConstraint(
ColliderKey colliderKey, float3 hitPosition, float3 normal, float distance,
int priority, int rigidBodyIndex, bool touched, float3 velocity)
{
return(new SurfaceConstraintInfo
{
ColliderKey = colliderKey,
HitPosition = hitPosition,
Plane = new Plane(normal, distance),
Priority = priority,
RigidBodyIndex = rigidBodyIndex,
Touched = touched,
Velocity = velocity
});
}
public void EnterPool()
{
this.Invalidated = false;
this.NeedsRefresh = false;
this.Processing = false;
this.Preserving = false;
this.Colliders.Clear();
this.TempColliders.Clear();
this.Batches.Clear();
this.TempBatches.Clear();
this.TempInstances.Clear();
this.grid = null;
this.cell = null;
this.key = new ColliderKey();
}
public bool GetChild(ref ColliderKey key, out ChildCollider child)
{
if (key.PopSubKey(NumColliderKeyBits, out uint subKey))
{
var index = new int2(((int)subKey >> 1) & ((1 << (int)Terrain.NumXBits) - 1), (int)subKey >> ((int)Terrain.NumXBits + 1));
int triangle = (int)subKey & 1;
child = Terrain.GetTriangle(index, triangle, Filter, Material);
return(true);
}
else
{
child = new ChildCollider();
return(false);
}
}
/// <summary>
/// Given the root Collider of a hierarchy and a ColliderKey referencing a child in that hierarchy,
/// this function returns the ColliderKey referencing the parent Collider.
/// </summary>
/// <param name="rootColliderPtr">A <see cref="Collider"/> at the root of a Collider hierarchy.</param>
/// <param name="childColliderKey">A <see cref="ColliderKey"/> referencing a child Collider somewhere in the hierarchy below rootColliderPtr.</param>
/// <param name="parentColliderKey">A <see cref="ColliderKey"/> referencing the parent of the child Collider. Will be ColliderKey.Empty if the parameters where invalid.</param>
/// <returns>Whether the parent was successfully found in the hierarchy.</returns>
public static unsafe bool TryGetParentColliderKey(Collider *rootColliderPtr, ColliderKey childColliderKey, out ColliderKey parentColliderKey)
{
var childColliderPtr = rootColliderPtr;
var childColliderKeyNumBits = childColliderPtr->NumColliderKeyBits;
// Start with an Empty collider key and push sub keys onto it as we traverse down the compound hierarchy.
var parentColliderKeyPath = ColliderKeyPath.Empty;
// On the way down, the childColliderKey pops of sub keys and pushes them onto the parentColliderKeyPath
do
{
childColliderKey.PopSubKey(childColliderKeyNumBits, out var childIndex);
switch (childColliderPtr->Type)
{
case ColliderType.Compound:
// Get the next child down and loop again
parentColliderKeyPath.PushChildKey(new ColliderKeyPath(new ColliderKey(childColliderKeyNumBits, childIndex), childColliderKeyNumBits));
childColliderPtr = ((CompoundCollider *)childColliderPtr)->Children[(int)childIndex].Collider;
childColliderKeyNumBits = childColliderPtr->NumColliderKeyBits;
break;
case ColliderType.Mesh:
case ColliderType.Terrain:
// We've hit a Terrain or Mesh collider so there should only be PolygonColliders below this.
// At this point the childColliderKey should be Empty and childIndex should be the index of the polygon.
if (!childColliderKey.Equals(ColliderKey.Empty))
{
// We've reached the bottom without popping all the child keys.
// The given childColliderKey doesn't fit this hierarchy!
parentColliderKey = ColliderKey.Empty;
return(false);
}
break;
default:
// We've hit a Convex collider, so rootColliderPtr must not have been
// the root of a hierarchy in the first place and so there is no parent!
parentColliderKey = ColliderKey.Empty;
return(false);
}
}while (!childColliderKey.Equals(ColliderKey.Empty) &&
!childColliderPtr->CollisionType.Equals(CollisionType.Convex));
parentColliderKey = parentColliderKeyPath.Key;
// childColliderKey should be Empty at this point.
// However, if it isn't then we reached a leaf without finding the child collider!
return(childColliderKey.Equals(ColliderKey.Empty));
}
public static unsafe bool IsTrigger(Collider *c, ColliderKey key)
{
bool bIsTrigger = false;
{
var cc = ((ConvexCollider *)c);
if (cc->CollisionType != CollisionType.Convex)
{
c->GetLeaf(key, out ChildCollider child);
cc = (ConvexCollider *)child.Collider;
UnityEngine.Assertions.Assert.IsTrue(cc->CollisionType == CollisionType.Convex);
}
bIsTrigger = cc->Material.IsTrigger;
}
return(bIsTrigger);
}
public void GetLeaves <T>(ref T collector) where T : struct, ILeafColliderCollector
{
for (int x = 0; x < Terrain.Size.x - 1; x++)
{
for (int y = 0; y < Terrain.Size.y - 1; y++)
{
var index = new int2(x, y);
for (int iTriangle = 0; iTriangle < 2; iTriangle++)
{
ColliderKey key = Terrain.GetColliderKey(index, iTriangle);
ChildCollider leaf = Terrain.GetTriangle(index, iTriangle, Filter, Material);
collector.AddLeaf(key, ref leaf);
}
}
}
}
public static unsafe bool IsTrigger(BlobAssetReference <Collider> collider, ColliderKey key)
{
bool bIsTrigger = false;
var c = (Collider *)collider.GetUnsafePtr();
{
var cc = ((ConvexCollider *)c);
if (cc->CollisionType != CollisionType.Convex)
{
c->GetLeaf(key, out ChildCollider child);
cc = (ConvexCollider *)child.Collider;
Assert.IsTrue(cc->CollisionType == CollisionType.Convex);
}
bIsTrigger = cc->Material.IsTrigger;
}
return(bIsTrigger);
}
public void AabbLeaf <T>(OverlapAabbInput input, int primitiveKey, [NoAlias] ref T collector) where T : struct, IOverlapCollector
{
fixed(uint *keys = m_Keys)
{
keys[m_NumKeys++] = new ColliderKey(m_NumColliderKeyBits, (uint)(primitiveKey << 1)).Value;
Mesh.PrimitiveFlags flags = m_Mesh->GetPrimitiveFlags(primitiveKey);
if (Mesh.IsPrimitiveFlagSet(flags, Mesh.PrimitiveFlags.IsTrianglePair) &&
!Mesh.IsPrimitiveFlagSet(flags, Mesh.PrimitiveFlags.IsQuad))
{
keys[m_NumKeys++] = new ColliderKey(m_NumColliderKeyBits, (uint)(primitiveKey << 1) | 1).Value;
}
}
if (m_NumKeys > k_MaxKeys - 8)
{
Flush(ref collector);
}
}
public StatefulTriggerEvent(Entity entityA, Entity entityB, int bodyIndexA, int bodyIndexB,
ColliderKey colliderKeyA, ColliderKey colliderKeyB)
{
Entities = new EntityPair
{
EntityA = entityA,
EntityB = entityB
};
BodyIndices = new BodyIndexPair
{
BodyIndexA = bodyIndexA,
BodyIndexB = bodyIndexB
};
ColliderKeys = new ColliderKeyPair
{
ColliderKeyA = colliderKeyA,
ColliderKeyB = colliderKeyB
};
State = default;
}
public StatefulCollisionEvent(Entity entityA, Entity entityB, int bodyIndexA, int bodyIndexB,
ColliderKey colliderKeyA, ColliderKey colliderKeyB, float3 normal)
{
Entities = new EntityPair
{
EntityA = entityA,
EntityB = entityB
};
BodyIndices = new BodyIndexPair
{
BodyIndexA = bodyIndexA,
BodyIndexB = bodyIndexB
};
ColliderKeys = new ColliderKeyPair
{
ColliderKeyA = colliderKeyA,
ColliderKeyB = colliderKeyB
};
Normal = normal;
CollidingState = default;
CollisionDetails = default;
}
public static void CreateConstraintFromHit(PhysicsWorld world, ColliderKey key, int rigidbodyIndex, float3 position, float3 velocity, float3 normal, float distance, float deltaTime, out SurfaceConstraintInfo constraint)
{
bool dynamicBody = 0 <= rigidbodyIndex && rigidbodyIndex < world.NumDynamicBodies;
constraint = new SurfaceConstraintInfo
{
Plane = new Plane
{
Normal = normal,
Distance = distance
},
RigidBodyIndex = rigidbodyIndex,
ColliderKey = key,
HitPosition = position,
Velocity = dynamicBody ? world.MotionVelocities[rigidbodyIndex].LinearVelocity : velocity
};
if (distance < 0.0f)
{
constraint.Velocity = constraint.Velocity - constraint.Plane.Normal * distance;
}
}
private static void CreateConstraint(PhysicsWorld world, float3 up,
int hitRigidBodyIndex, ColliderKey hitColliderKey, float3 hitPosition, float3 hitSurfaceNormal, float hitDistance,
float skinWidth, float maxSlopeCos, ref NativeArray <SurfaceConstraintInfo> constraints, ref int numConstraints)
{
CreateConstraintFromHit(world, hitRigidBodyIndex, hitColliderKey, hitPosition,
hitSurfaceNormal, hitDistance, skinWidth, out SurfaceConstraintInfo constraint);
// Check if max slope plane is required
float verticalComponent = math.dot(constraint.Plane.Normal, up);
bool shouldAddPlane = verticalComponent > k_SimplexSolverEpsilon && verticalComponent < maxSlopeCos;
if (shouldAddPlane)
{
AddMaxSlopeConstraint(up, ref constraint, ref constraints, ref numConstraints);
}
// Prepare velocity to resolve penetration
ResolveConstraintPenetration(ref constraint);
// Add original constraint to the list
constraints[numConstraints++] = constraint;
}
private static void CreateConstraintFromHit(PhysicsWorld world, float3 gravity, float deltaTime, int rigidBodyIndex, ColliderKey colliderKey,
float3 hitPosition, float3 normal, float distance, bool zeroVelocity, out SurfaceConstraintInfo constraint)
{
bool bodyIsDynamic = 0 <= rigidBodyIndex && rigidBodyIndex < world.NumDynamicBodies;
constraint = new SurfaceConstraintInfo()
{
Plane = new Plane
{
Normal = normal,
Distance = distance,
},
RigidBodyIndex = rigidBodyIndex,
ColliderKey = colliderKey,
HitPosition = hitPosition,
Velocity = bodyIsDynamic && !zeroVelocity ?
world.MotionVelocities[rigidBodyIndex].LinearVelocity - world.MotionDatas[rigidBodyIndex].GravityFactor * gravity * deltaTime :
constraint.Velocity = float3.zero,
Priority = bodyIsDynamic ? 1 : 0
};
// Fix up the velocity to enable penetration recovery
if (distance < 0.0f)
{
float3 newVel = constraint.Velocity - constraint.Plane.Normal * distance;
constraint.Velocity = newVel;
}
}
private static void CreateConstraintFromHit(PhysicsWorld world, int rigidBodyIndex, ColliderKey colliderKey,
float3 hitPosition, float3 normal, float distance, float skinWidth, out SurfaceConstraintInfo constraint)
{
bool bodyIsDynamic = 0 <= rigidBodyIndex && rigidBodyIndex < world.NumDynamicBodies;
constraint = new SurfaceConstraintInfo()
{
Plane = new Unity.Physics.Plane
{
Normal = normal,
Distance = distance - skinWidth,
},
RigidBodyIndex = rigidBodyIndex,
ColliderKey = colliderKey,
HitPosition = hitPosition,
Velocity = bodyIsDynamic ?
world.GetLinearVelocity(rigidBodyIndex, hitPosition) :
float3.zero,
Priority = bodyIsDynamic ? 1 : 0
};
}
public bool GetLeaf(ColliderKey key, out ChildCollider leaf)
{
return(GetChild(ref key, out leaf)); // all children of TerrainCollider are leaves
}
private Collider2D CreateCollider( int scriptId, ColliderKey key )
{
Collider2D newCollider = null;
// Using bounding box shape as collider.
if ( key.colliderShape == ColliderShape.eBox )
{
// Update collider bounding box if available.
float[] box = new float[4];
if ( Internal.CalculateBoundingBox( scriptId, box ) )
{
BoxCollider2D boxCollider = gameObject.AddComponent<BoxCollider2D>();
boxCollider.enabled = true;
boxCollider.isTrigger = colliderTrigger;
#if UNITY_4_3 || UNITY_4_5 || UNITY_4_6
boxCollider.center = new Vector2( (box[0]+box[2]) * 0.5f, (box[1]+box[3]) * 0.5f );
#else // UNITY_5_0
boxCollider.offset = new Vector2( (box[0]+box[2]) * 0.5f, (box[1]+box[3]) * 0.5f );
#endif
boxCollider.size = new Vector2( box[2]-box[0], box[3]-box[1] );
colliderKeys.Add( key.ToString() );
colliderLinks.Add( boxCollider );
newCollider = boxCollider;
}
}
// Calculate convex hull shape as collider.
else if ( colliderShape == ColliderShape.eConvexHull )
{
IntPtr convexHullArrayRaw = IntPtr.Zero;
int convexHullSize = 0;
if ( Internal.CalculateConvexHull( scriptId, ref convexHullArrayRaw, ref convexHullSize ) )
{
float[] convexHullArray = new float[ convexHullSize * 2 ];
Marshal.Copy( convexHullArrayRaw,
convexHullArray,
0,
convexHullSize * 2 );
Internal.DeallocateConvexHull( convexHullArrayRaw );
PolygonCollider2D polygonCollider = gameObject.AddComponent<PolygonCollider2D>();
polygonCollider.enabled = true;
polygonCollider.isTrigger = colliderTrigger;
polygonCollider.pathCount = 1;
Vector2[] path = new Vector2[ convexHullSize ];
for( int idx=0 ; idx < convexHullSize ; ++idx )
{
path[idx] = new Vector2( convexHullArray[idx*2], convexHullArray[idx*2 + 1] );
}
polygonCollider.SetPath( 0, path );
colliderKeys.Add( key.ToString() );
colliderLinks.Add( polygonCollider );
newCollider = polygonCollider;
}
}
// Calculate tight polygons as collider.
else // if ( colliderShape == ColliderShape.ePolygons )
{
IntPtr polygonArrayRaw = IntPtr.Zero;
int polygonSize = 0;
IntPtr subPolygonArrayRaw = IntPtr.Zero;
int subPolygonSize = 0;
if ( Internal.CalculatePolygons( scriptId, ref polygonArrayRaw, ref polygonSize, ref subPolygonArrayRaw, ref subPolygonSize ) )
{
float[] polygonArray = new float[ polygonSize * 2 ];
Marshal.Copy( polygonArrayRaw,
polygonArray,
0,
polygonSize * 2 );
int[] subPolygonArray = new int[ subPolygonSize ];
Marshal.Copy( subPolygonArrayRaw,
subPolygonArray,
0,
subPolygonSize );
Internal.DeallocatePolygons( polygonArrayRaw, subPolygonArrayRaw );
PolygonCollider2D polygonCollider = gameObject.AddComponent<PolygonCollider2D>();
polygonCollider.enabled = true;
polygonCollider.isTrigger = colliderTrigger;
polygonCollider.pathCount = subPolygonSize;
int baseIndex = 0;
for ( int polygonIndex=0 ; polygonIndex < subPolygonSize ; ++polygonIndex )
{
int numberOfPoints = subPolygonArray[polygonIndex];
int lastIndex = baseIndex + numberOfPoints;
Vector2[] path = new Vector2[numberOfPoints];
for ( int idx=0 ; idx<numberOfPoints ; ++idx )
{
int ptIndex = baseIndex + idx;
path[idx] = new Vector2( polygonArray[ptIndex*2], polygonArray[ptIndex*2 + 1] );
}
polygonCollider.SetPath(polygonIndex, path);
baseIndex = lastIndex;
}
colliderKeys.Add( key.ToString() );
colliderLinks.Add( polygonCollider );
newCollider = polygonCollider;
}
}
return newCollider;
}
