public static unsafe void AabbCollider <T>(OverlapAabbInput input, [NoAlias] Collider *collider, [NoAlias] ref T collector)
where T : struct, IOverlapCollector
{
if (!CollisionFilter.IsCollisionEnabled(input.Filter, collider->Filter))
{
return;
}
switch (collider->Type)
{
case ColliderType.Mesh:
AabbMesh(input, (MeshCollider *)collider, ref collector);
break;
case ColliderType.Compound:
AabbCompound(input, (CompoundCollider *)collider, ref collector);
break;
case ColliderType.Terrain:
AabbTerrain(input, (TerrainCollider *)collider, ref collector);
break;
default:
SafetyChecks.ThrowNotImplementedException();
return;
}
}
private static unsafe void CompositeComposite(
Context context,
Collider *compositeColliderA, Collider *compositeColliderB, MTransform worldFromA, MTransform worldFromB,
MotionExpansion expansion, bool flipped)
{
// Flip the order if necessary, so that A has fewer leaves than B
if (compositeColliderA->NumColliderKeyBits > compositeColliderB->NumColliderKeyBits)
{
Collider *c = compositeColliderA;
compositeColliderA = compositeColliderB;
compositeColliderB = c;
MTransform t = worldFromA;
worldFromA = worldFromB;
worldFromB = t;
expansion.Linear *= -1.0f;
flipped = !flipped;
}
var collector = new CompositeCompositeLeafCollector(
context,
compositeColliderA, compositeColliderB,
worldFromA, worldFromB, expansion, flipped);
compositeColliderA->GetLeaves(ref collector);
}
private static unsafe void DrawConvex(
Collider *collider,
ref PhysicsTransform worldTransform,
ref PhysicsDebugStreamSystem.Context outputStream,
Color colliderColor)
{
var convexCollider = (ConvexCollider *)collider;
var vertexCount = convexCollider->VertexCount;
var vertices = convexCollider->Vertices;
var convexRadius = convexCollider->m_ConvexHull.ConvexRadius;
switch (collider->ColliderType)
{
case ColliderType.Box:
case ColliderType.Polygon:
{
Assert.IsTrue(vertexCount >= 3, "ConvexCollider must have >= 3 vertices.");
outputStream.Polygon(vertices, vertexCount, worldTransform, colliderColor);
return;
}
case ColliderType.Circle:
{
Assert.AreEqual(1, vertexCount, "CircleCollider must have 1 vertex.");
var position = PhysicsMath.mul(worldTransform, vertices[0]);
outputStream.Circle(position, convexRadius, colliderColor);
return;
}
case ColliderType.Capsule:
{
Assert.AreEqual(2, vertexCount, "CapsuleCollider must have 2 vertices.");
var vertex0 = PhysicsMath.mul(worldTransform, vertices[0]);
var vertex1 = PhysicsMath.mul(worldTransform, vertices[1]);
var offset = PhysicsMath.perp(math.normalizesafe(vertex1 - vertex0)) * new float2(convexRadius);
// Side Edges.
{
outputStream.Line(vertex0 - offset, vertex1 - offset, colliderColor);
outputStream.Line(vertex0 + offset, vertex1 + offset, colliderColor);
}
// End Caps.
{
var startAngle = math.atan2(offset.y, offset.x);
var endAngle = startAngle + math.PI;
outputStream.Arc(vertex0, convexRadius, startAngle, endAngle, colliderColor);
outputStream.Arc(vertex1, convexRadius, startAngle + math.PI, endAngle + math.PI, colliderColor);
}
return;
}
default:
return;
}
}
static void PushoutFromCapsule(Collider *pCollider, ColliderEx *pColliderEx, ref Vector3 point)
{
var capsuleVec = pColliderEx->Direction;
var capsulePos = pColliderEx->Position;
var targetVec = point - capsulePos;
var radius = pCollider->Radius;
var distanceOnVec = Vector3.Dot(capsuleVec, targetVec);
if (distanceOnVec <= 0.0f)
{
PushoutFromSphere(capsulePos, radius, ref point);
}
else if (distanceOnVec >= pCollider->Height)
{
PushoutFromSphere(capsulePos + capsuleVec * distanceOnVec, radius, ref point);
}
else
{
var positionOnVec = capsulePos + (capsuleVec * distanceOnVec);
var pushoutVec = point - positionOnVec;
var distanceSquared = pushoutVec.sqrMagnitude;
if (distanceSquared > Epsilon && distanceSquared < radius * radius)
{
var distance = Mathf.Sqrt(distanceSquared);
point = positionOnVec + pushoutVec * radius / distance;
}
}
}
public unsafe void Execute <TCtx>(TCtx ctx, InputTriggerPort port) where TCtx : IGraphInstance
{
var entity = ctx.ReadEntity(Entity);
if (entity == Unity.Entities.Entity.Null)
{
return;
}
if (!ctx.EntityManager.HasComponent <PhysicsCollider>(entity))
{
return;
}
PhysicsCollider physicsCollider = ctx.EntityManager.GetComponentData <PhysicsCollider>(entity);
Collider * colliderPtr = (Collider *)physicsCollider.Value.GetUnsafePtr();
CollisionFilter newFilter = new CollisionFilter();
newFilter.BelongsTo = (uint)ctx.ReadInt(this.BelongsTo);
newFilter.CollidesWith = (uint)ctx.ReadInt(this.CollidesWith);
newFilter.GroupIndex = ctx.ReadInt(this.GroupIndex);
colliderPtr->Filter = newFilter;
ctx.Trigger(Output);
}
void IJobParallelFor.Execute(int index)
{
var pRW = pRWPoints + index;
if (IsEnableFloor)
{
if (pRW->Position.y <= FloorHeight)
{
pRW->Position.y = FloorHeight;
}
}
if (IsEnableCollider)
{
for (int i = 0; i < ColliderCount; ++i)
{
Collider * pCollider = pColliders + i;
ColliderEx *pColliderEx = pColliderExs + i;
if (pCollider->Height <= 0.0f)
{
PushoutFromSphere(pCollider, pColliderEx, ref pRW->Position);
}
else
{
PushoutFromCapsule(pCollider, pColliderEx, ref pRW->Position);
}
}
}
}
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;
}
//
// Object creation
//
private unsafe Entity CreateBody(float3 position, quaternion orientation, BlobAssetReference <Collider> collider,
float3 linearVelocity, float3 angularVelocity, float mass, bool isDynamic)
{
EntityManager entityManager = EntityManager;
Entity entity = entityManager.CreateEntity(new ComponentType[] { });
entityManager.AddComponentData(entity, new LocalToWorld {
});
entityManager.AddComponentData(entity, new Translation {
Value = position
});
entityManager.AddComponentData(entity, new Rotation {
Value = orientation
});
entityManager.AddComponentData(entity, new PhysicsCollider {
Value = collider
});
List <Unity.Physics.Authoring.DisplayBodyColliders.DrawComponent.DisplayResult> meshes = Unity.Physics.Authoring.DisplayBodyColliders.DrawComponent.BuildDebugDisplayMesh((Collider *)collider.GetUnsafePtr());
CombineInstance[] instances = new CombineInstance[meshes.Count];
for (int i = 0; i < meshes.Count; i++)
{
instances[i] = new CombineInstance
{
mesh = meshes[i].Mesh,
transform = Matrix4x4.TRS(meshes[i].Position, meshes[i].Orientation, meshes[i].Scale)
};
}
Mesh mesh = new Mesh();
mesh.CombineMeshes(instances);
entityManager.AddSharedComponentData(entity, new RenderMesh
{
mesh = mesh,
material = isDynamic ? dynamicMaterial : staticMaterial
});
if (isDynamic)
{
Collider *colliderPtr = (Collider *)collider.GetUnsafePtr();
entityManager.AddComponentData(entity, PhysicsMass.CreateDynamic(colliderPtr->MassProperties, mass));
float3 angularVelocityLocal = math.mul(math.inverse(colliderPtr->MassProperties.MassDistribution.Transform.rot), angularVelocity);
entityManager.AddComponentData(entity, new PhysicsVelocity()
{
Linear = linearVelocity,
Angular = angularVelocityLocal
});
entityManager.AddComponentData(entity, new PhysicsDamping()
{
Linear = 0.01f,
Angular = 0.05f
});
}
return(entity);
}
void PushoutFromCapsule(Collider *pCollider, ColliderEx *pColliderEx, ref Vector3 point)
{
var capsuleVec = pColliderEx->Direction;
var capsuleVecNormal = capsuleVec.normalized;
var capsulePos = pColliderEx->Position;
var targetVec = point - capsulePos;
var distanceOnVec = Vector3.Dot(capsuleVecNormal, targetVec);
if (distanceOnVec <= EPSILON)
{
PushoutFromSphere(capsulePos, pCollider->RadiusHead, ref point);
return;
}
else if (distanceOnVec >= pCollider->Height)
{
PushoutFromSphere(capsulePos + capsuleVec, pCollider->RadiusTail, ref point);
return;
}
else
{
var positionOnVec = capsulePos + (capsuleVecNormal * distanceOnVec);
var pushoutVec = point - positionOnVec;
var sqrPushoutDistance = pushoutVec.sqrMagnitude;
if (sqrPushoutDistance > EPSILON)
{
var Radius = pCollider->RadiusHead + (pCollider->RadiusTail - pCollider->RadiusHead) * distanceOnVec;
if (sqrPushoutDistance < Radius * Radius)
{
var pushoutDistance = Mathf.Sqrt(sqrPushoutDistance);
point = positionOnVec + pushoutVec * Radius / pushoutDistance;
return;
}
}
}
}
private static unsafe bool ConvexConvex(ColliderCastInput input, Collider *target, sfloat maxFraction, out ColliderCastHit hit)
{
hit = default;
// Get the current transform
MTransform targetFromQuery = new MTransform(input.Orientation, input.Start);
// Conservative advancement
sfloat tolerance = sfloat.FromRaw(0x3a83126f); // return if this close to a hit
sfloat keepDistance = sfloat.FromRaw(0x38d1b717); // avoid bad cases for GJK (penetration / exact hit)
int iterations = 10; // return after this many advances, regardless of accuracy
sfloat fraction = sfloat.Zero;
while (true)
{
if (fraction >= maxFraction)
{
// Exceeded the maximum fraction without a hit
return(false);
}
// Find the current distance
DistanceQueries.Result distanceResult = DistanceQueries.ConvexConvex(target, input.Collider, targetFromQuery);
// Check for a hit
if (distanceResult.Distance < tolerance || --iterations == 0)
{
targetFromQuery.Translation = input.Start;
hit.Position = Mul(input.QueryContext.WorldFromLocalTransform, distanceResult.PositionOnBinA);
hit.SurfaceNormal = math.mul(input.QueryContext.WorldFromLocalTransform.Rotation, -distanceResult.NormalInA);
hit.Fraction = fraction;
hit.RigidBodyIndex = input.QueryContext.RigidBodyIndex;
hit.ColliderKey = input.QueryContext.ColliderKey;
hit.Material = ((ConvexColliderHeader *)target)->Material;
hit.Entity = input.QueryContext.Entity;
return(true);
}
// Check for a miss
sfloat dot = math.dot(distanceResult.NormalInA, input.Ray.Displacement);
if (dot <= sfloat.Zero)
{
// Collider is moving away from the target, it will never hit
return(false);
}
// Advance
fraction += (distanceResult.Distance - keepDistance) / dot;
if (fraction >= maxFraction)
{
// Exceeded the maximum fraction without a hit
return(false);
}
targetFromQuery.Translation = math.lerp(input.Start, input.End, fraction);
}
}
public unsafe void Execute <TCtx>(TCtx ctx, InputTriggerPort port) where TCtx : IGraphInstance
{
var entity = ctx.ReadEntity(Entity);
if (entity == Unity.Entities.Entity.Null)
{
return;
}
if (!ctx.EntityManager.HasComponent <PhysicsCollider>(entity))
{
return;
}
PhysicsCollider physicsCollider = ctx.EntityManager.GetComponentData <PhysicsCollider>(entity);
Collider * colliderPtr = (Collider *)physicsCollider.Value.GetUnsafePtr();
if (!ctx.EntityManager.HasComponent <PhysicsGravityFactor>(entity))
{
ctx.EntityManager.AddComponent <PhysicsGravityFactor>(entity);
}
float gravityFactor = ctx.ReadFloat(GravityFactor);
ctx.EntityManager.SetComponentData(entity, new PhysicsGravityFactor()
{
Value = gravityFactor
});
if (!ctx.EntityManager.HasComponent <PhysicsMass>(entity))
{
ctx.EntityManager.AddComponent <PhysicsMass>(entity);
}
float mass = ctx.ReadFloat(Mass);
ctx.EntityManager.SetComponentData(entity, PhysicsMass.CreateDynamic(colliderPtr->MassProperties, mass));
if (!ctx.EntityManager.HasComponent <PhysicsVelocity>(entity))
{
ctx.EntityManager.AddComponent <PhysicsVelocity>(entity);
}
ctx.EntityManager.SetComponentData(entity, new PhysicsVelocity {
Linear = float3.zero, Angular = float3.zero
});
float drag = ctx.ReadFloat(Drag);
float angularDrag = ctx.ReadFloat(AngularDrag);
if (!ctx.EntityManager.HasComponent <PhysicsDamping>(entity))
{
ctx.EntityManager.AddComponent <PhysicsDamping>(entity);
}
ctx.EntityManager.SetComponentData(entity, new PhysicsDamping {
Linear = drag, Angular = angularDrag
});
ctx.Trigger(Output);
}
private static unsafe bool ConvexConvex <T>(ColliderCastInput input, Collider *target, ref T collector) where T : struct, ICollector <ColliderCastHit>
{
//Assert.IsTrue(target->CollisionType == CollisionType.Convex && input.Collider->CollisionType == CollisionType.Convex, "ColliderCast.ConvexConvex can only process convex colliders");
// Get the current transform
MTransform targetFromQuery = new MTransform(input.Orientation, input.Start);
// Conservative advancement
const float tolerance = 1e-3f; // return if this close to a hit
const float keepDistance = 1e-4f; // avoid bad cases for GJK (penetration / exact hit)
int iterations = 10; // return after this many advances, regardless of accuracy
float fraction = 0.0f;
while (true)
{
if (fraction >= collector.MaxFraction)
{
// Exceeded the maximum fraction without a hit
return(false);
}
// Find the current distance
DistanceQueries.Result distanceResult = DistanceQueries.ConvexConvex(target, input.Collider, targetFromQuery);
// Check for a hit
if (distanceResult.Distance < tolerance || --iterations == 0)
{
targetFromQuery.Translation = input.Start;
return(collector.AddHit(new ColliderCastHit
{
Position = distanceResult.PositionOnBinA,
SurfaceNormal = -distanceResult.NormalInA,
Fraction = fraction,
ColliderKey = ColliderKey.Empty,
RigidBodyIndex = -1
}));
}
// Check for a miss
float dot = math.dot(distanceResult.NormalInA, input.Ray.Displacement);
if (dot <= 0.0f)
{
// Collider is moving away from the target, it will never hit
return(false);
}
// Advance
fraction += (distanceResult.Distance - keepDistance) / dot;
if (fraction >= collector.MaxFraction)
{
// Exceeded the maximum fraction without a hit
return(false);
}
targetFromQuery.Translation = math.lerp(input.Start, input.End, fraction);
}
}
private static unsafe void CompositeConvex(
Context context,
Collider *compositeColliderA, Collider *convexColliderB, MTransform worldFromA, MTransform worldFromB,
MotionExpansion expansion, bool flipped)
{
// Flip the relevant inputs and call convex-vs-composite
expansion.Linear *= -1.0f;
ConvexComposite(context, ColliderKey.Empty,
convexColliderB, compositeColliderA, worldFromB, worldFromA, expansion, !flipped);
}
public static unsafe bool ColliderCollider <T>(ColliderCastInput input, Collider *target, ref T collector) where T : struct, ICollector <ColliderCastHit>
{
if (!CollisionFilter.IsCollisionEnabled(input.Collider->Filter, target->Filter))
{
return(false);
}
if (!input.QueryContext.IsInitialized)
{
input.QueryContext = QueryContext.DefaultContext;
}
switch (input.Collider->CollisionType)
{
case CollisionType.Convex:
switch (target->Type)
{
case ColliderType.Sphere:
case ColliderType.Capsule:
case ColliderType.Triangle:
case ColliderType.Quad:
case ColliderType.Box:
case ColliderType.Cylinder:
case ColliderType.Convex:
if (ConvexConvex(input, target, collector.MaxFraction, out ColliderCastHit hit))
{
return(collector.AddHit(hit));
}
return(false);
case ColliderType.Mesh:
return(ConvexMesh(input, (MeshCollider *)target, ref collector));
case ColliderType.Compound:
return(ConvexCompound(input, (CompoundCollider *)target, ref collector));
case ColliderType.Terrain:
return(ConvexTerrain(input, (TerrainCollider *)target, ref collector));
default:
SafetyChecks.ThrowNotImplementedException();
return(default);
}
case CollisionType.Composite:
case CollisionType.Terrain:
// no support for casting composite shapes
SafetyChecks.ThrowNotImplementedException();
return(default);
default:
SafetyChecks.ThrowNotImplementedException();
return(default);
}
}
public static unsafe bool ColliderCollider <T>(ColliderDistanceInput input, Collider *target, ref T collector) where T : struct, ICollector <DistanceHit>
{
if (!CollisionFilter.IsCollisionEnabled(input.Collider->Filter, target->Filter))
{
return(false);
}
switch (input.Collider->CollisionType)
{
case CollisionType.Convex:
switch (target->Type)
{
case ColliderType.Convex:
case ColliderType.Sphere:
case ColliderType.Capsule:
case ColliderType.Triangle:
case ColliderType.Quad:
case ColliderType.Box:
case ColliderType.Cylinder:
MTransform targetFromQuery = new MTransform(input.Transform);
Result result = ConvexConvex(target, input.Collider, targetFromQuery);
if (result.Distance < collector.MaxFraction)
{
collector.AddHit(new DistanceHit
{
Fraction = result.Distance,
SurfaceNormal = -result.NormalInA,
Position = result.PositionOnAinA,
ColliderKey = ColliderKey.Empty
});
return(true);
}
return(false);
case ColliderType.Mesh:
return(ConvexMesh(input, (MeshCollider *)target, ref collector));
case ColliderType.Compound:
return(ConvexCompound(input, (CompoundCollider *)target, ref collector));
default:
throw new NotImplementedException();
}
case CollisionType.Composite:
// no support for composite query shapes
throw new NotImplementedException();
default:
throw new NotImplementedException();
}
}
/// <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 CompositeCompositeLeafCollector(
Context context,
Collider *compositeColliderA, Collider *compositeColliderB,
MTransform worldFromA, MTransform worldFromB, MotionExpansion expansion, bool flipped)
{
m_Context = context;
m_CompositeColliderA = compositeColliderA;
m_CompositeColliderB = compositeColliderB;
m_WorldFromA = worldFromA;
m_WorldFromB = worldFromB;
m_Expansion = expansion;
m_Flipped = flipped;
m_KeyPath = ColliderKeyPath.Empty;
}
bool CollisionDetection(Collider *pCollider, ColliderEx *pColliderEx, Vector3 point1, Vector3 point2, out Vector3 pointOnLine, out Vector3 pointOnCollider, out float Radius)
{
if (pCollider->Height <= EPSILON)
{
var direction = point2 - point1;
var directionLength = direction.magnitude;
direction /= directionLength;
var toCenter = pColliderEx->Position - point1;
var dot = Vector3.Dot(direction, toCenter);
var pointOnDirection = direction * Mathf.Clamp(dot, 0.0f, directionLength);
pointOnCollider = pColliderEx->Position;
pointOnLine = pointOnDirection + point1;
Radius = pCollider->RadiusHead;
if ((pointOnCollider - pointOnLine).sqrMagnitude > pCollider->RadiusHead * pCollider->RadiusHead)
{
return(false);
}
return(true);
}
else
{
var capsuleDir = pColliderEx->Direction;
var capsulePos = pColliderEx->Position;
var pointDir = point2 - point1;
float t1, t2;
var sqrDistance = ComputeNearestPoints(capsulePos, capsuleDir, point1, pointDir, out t1, out t2, out pointOnCollider, out pointOnLine);
t1 = Mathf.Clamp01(t1);
Radius = pCollider->RadiusHead + (pCollider->RadiusTail - pCollider->RadiusHead) * t1;
if (sqrDistance > Radius * Radius)
{
pointOnCollider = Vector3.zero;
pointOnLine = Vector3.zero;
return(false);
}
t2 = Mathf.Clamp01(t2);
pointOnCollider = capsulePos + capsuleDir * t1;
pointOnLine = point1 + pointDir * t2;
return((pointOnCollider - pointOnLine).sqrMagnitude <= Radius * Radius);
}
}
internal static unsafe bool PointDistance <T>(PointDistanceInput input, Collider *collider, ref T collector) where T : struct, ICollector <DistanceHit>
{
if (!CollisionFilter.IsCollisionEnabled(input.Filter, collider->Filter))
{
return(false);
}
// Ensure the query context is initialized.
input.QueryContext.EnsureIsInitialized();
var proxySource = new DistanceProxy(1, &input.Position, 0f);
DistanceProxy proxyTarget;
switch (collider->ColliderType)
{
case ColliderType.Box:
case ColliderType.Polygon:
case ColliderType.Capsule:
case ColliderType.Circle:
{
var convexCollider = (ConvexCollider *)collider;
proxyTarget = new DistanceProxy(ref convexCollider->m_ConvexHull);
break;
}
case ColliderType.Compound:
return(PointDistanceCompound(input, (PhysicsCompoundCollider *)collider, ref collector));
default:
SafetyChecks.ThrowNotImplementedException();
return(default);
}
var hit = ColliderDistance(PhysicsTransform.Identity, ref proxySource, ref proxyTarget);
if (hit.Distance < collector.MaxFraction)
{
hit.PhysicsBodyIndex = input.QueryContext.PhysicsBodyIndex;
hit.ColliderKey = input.QueryContext.ColliderKey;
hit.Entity = input.QueryContext.Entity;
hit.PointA = PhysicsMath.mul(input.QueryContext.LocalToWorldTransform, hit.PointA);
hit.PointB = PhysicsMath.mul(input.QueryContext.LocalToWorldTransform, hit.PointB);
return(collector.AddHit(hit));
}
return(false);
}
public void Execute(ArchetypeChunk chunk, int chunkIndex, int firstEntityIndex)
{
NativeArray <KinematicMotor> chunkMotors = chunk.GetNativeArray(KinematicMotorType);
NativeArray <Movement> chunkMovements = chunk.GetNativeArray(MovementType);
NativeArray <PhysicsCollider> chunkColliders = chunk.GetNativeArray(PhysicsColliderType);
NativeArray <Translation> chunkTranslations = chunk.GetNativeArray(TranslationType);
NativeArray <Rotation> chunkRotations = chunk.GetNativeArray(RotationType);
for (int i = 0; i < chunk.Count; i++)
{
KinematicMotor motor = chunkMotors[i];
Movement movement = chunkMovements[i];
PhysicsCollider collider = chunkColliders[i];
Translation translation = chunkTranslations[i];
Rotation rotation = chunkRotations[i];
RigidTransform transform = new RigidTransform
{
pos = translation.Value,
rot = rotation.Value
};
float3 velocity = movement.Value;
unsafe
{
Collider *queryCollider;
{
Collider *colliderPtr = collider.ColliderPtr;
byte *copiedColliderMemory = stackalloc byte[colliderPtr->MemorySize];
queryCollider = (Collider *)(copiedColliderMemory);
UnsafeUtility.MemCpy(queryCollider, colliderPtr, colliderPtr->MemorySize);
queryCollider->Filter = CollisionFilter.Default;
}
KinematicMotorUtilities.SolveCollisionConstraints(World, DeltaTime, motor.MaxIterations, motor.SkinWidth, 360, queryCollider, ref transform, ref velocity, ref DistanceHits, ref ColliderCastHits, ref SurfaceConstraintInfos);
}
translation.Value = transform.pos;
movement.Value = velocity;
// Apply data back to chunk
{
chunkTranslations[i] = translation;
chunkMovements[i] = movement;
}
}
}
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;
}
/// <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 static unsafe bool ColliderCollider <T>(ColliderCastInput input, Collider *target, ref T collector) where T : struct, ICollector <ColliderCastHit>
{
if (!CollisionFilter.IsCollisionEnabled(input.Collider->Filter, target->Filter))
{
return(false);
}
switch (input.Collider->CollisionType)
{
case CollisionType.Convex:
switch (target->Type)
{
case ColliderType.Sphere:
case ColliderType.Capsule:
case ColliderType.Triangle:
case ColliderType.Quad:
case ColliderType.Box:
case ColliderType.Cylinder:
case ColliderType.Convex:
return(ConvexConvex(input, target, ref collector));
case ColliderType.Mesh:
return(ConvexMesh(input, (MeshCollider *)target, ref collector));
case ColliderType.Compound:
return(ConvexCompound(input, (CompoundCollider *)target, ref collector));
case ColliderType.Terrain:
return(ConvexTerrain(input, (TerrainCollider *)target, ref collector));
default:
throw new NotImplementedException();
}
case CollisionType.Composite:
case CollisionType.Terrain:
// no support for casting composite shapes
throw new NotImplementedException();
default:
throw new NotImplementedException();
}
}
public unsafe void Execute <TCtx>(TCtx ctx, InputTriggerPort port) where TCtx : IGraphInstance
{
var entity = ctx.ReadEntity(Entity);
if (entity == Unity.Entities.Entity.Null)
{
return;
}
if (!ctx.EntityManager.HasComponent <PhysicsCollider>(entity))
{
return;
}
PhysicsCollider physicsCollider = ctx.EntityManager.GetComponentData <PhysicsCollider>(entity);
Collider * colliderPtr = (Collider *)physicsCollider.Value.GetUnsafePtr();
colliderPtr->Filter = CollisionFilter.Default;
ctx.Trigger(Output);
}
public static unsafe void DrawCollider(
Collider *collider,
ref PhysicsTransform worldTransform,
ref PhysicsDebugStreamSystem.Context outputStream,
Color colliderColor)
{
switch (collider->CollisionType)
{
case CollisionType.Convex:
{
DrawConvex(collider, ref worldTransform, ref outputStream, colliderColor);
return;
}
case CollisionType.Composite:
{
DrawComposite(collider, ref worldTransform, ref outputStream, colliderColor);
return;
}
}
}
static bool HitTest(Collider *pCollider, ColliderEx *pColliderEx, Vector3 point1, Vector3 point2, out Vector3 pointOnLine, out Vector3 pointOnCollider)
{
if (pCollider->IsSphere)
{
var direction = point2 - point1;
var directionLength = direction.magnitude;
direction /= directionLength;
var toCenter = pColliderEx->Position - point1;
var dot = Vector3.Dot(direction, toCenter);
var pointOnDirection = direction * Mathf.Clamp(dot, 0.0f, directionLength);
pointOnCollider = pColliderEx->Position;
pointOnLine = pointOnDirection + point1;
return(!((pointOnCollider - pointOnLine).sqrMagnitude > pCollider->Radius * pCollider->Radius));
}
else
{
var capsuleDir = pColliderEx->Direction;
var capsulePos = pColliderEx->Position;
var pointDir = point2 - point1;
var sqrDistance = ComputeNearestPoints(capsulePos, capsuleDir, point1, pointDir, out float t1, out float t2, out pointOnCollider, out pointOnLine);
if (sqrDistance > pCollider->Radius * pCollider->Radius)
{
pointOnCollider = Vector3.zero;
pointOnLine = Vector3.zero;
return(false);
}
t1 = Mathf.Clamp01(t1);
t2 = Mathf.Clamp01(t2);
pointOnCollider = capsulePos + capsuleDir * t1;
pointOnLine = point1 + pointDir * t2;
return((pointOnCollider - pointOnLine).sqrMagnitude <= pCollider->Radius * pCollider->Radius);
}
}
public unsafe void Execute <TCtx>(TCtx ctx, InputTriggerPort port) where TCtx : IGraphInstance
{
var entity = ctx.ReadEntity(Entity);
if (entity == Unity.Entities.Entity.Null)
{
return;
}
if (!ctx.EntityManager.HasComponent <PhysicsMass>(entity) || !ctx.EntityManager.HasComponent <PhysicsCollider>(entity))
{
return;
}
PhysicsCollider physicsCollider = ctx.EntityManager.GetComponentData <PhysicsCollider>(entity);
float newMass = ctx.ReadFloat(Mass);
Collider *colliderPtr = (Collider *)physicsCollider.Value.GetUnsafePtr();
ctx.EntityManager.SetComponentData(entity, PhysicsMass.CreateDynamic(colliderPtr->MassProperties, newMass));
}
public unsafe void Execute <TCtx>(TCtx ctx, InputTriggerPort port) where TCtx : IGraphInstance
{
var entity = ctx.ReadEntity(Entity);
if (entity == Unity.Entities.Entity.Null)
{
return;
}
if (!ctx.EntityManager.HasComponent <PhysicsCollider>(entity))
{
return;
}
if (!ctx.EntityManager.HasComponent <PhysicsVelocity>(entity))
{
return;
}
PhysicsCollider physicsCollider = ctx.EntityManager.GetComponentData <PhysicsCollider>(entity);
Collider *colliderPtr = (Collider *)physicsCollider.Value.GetUnsafePtr();
float3 linearVelocity = ctx.ReadFloat3(Linear);
float3 angularVelocity = ctx.ReadFloat3(Angular);
// TODO: MBRIAU: Make sure to understand exactly what's going on here
// Calculate the angular velocity in local space from rotation and world angular velocity
float3 angularVelocityLocal = math.mul(math.inverse(colliderPtr->MassProperties.MassDistribution.Transform.rot), angularVelocity);
ctx.EntityManager.SetComponentData(entity, new PhysicsVelocity()
{
Linear = linearVelocity,
Angular = angularVelocityLocal
});
ctx.Trigger(Output);
}
private static unsafe void DrawComposite(
Collider *collider,
ref PhysicsTransform worldTransform,
ref PhysicsDebugStreamSystem.Context outputStream,
Color colliderColor)
{
switch (collider->ColliderType)
{
case ColliderType.Compound:
{
var compoundCollider = (PhysicsCompoundCollider *)collider;
var children = compoundCollider->Children;
for (var i = 0; i < children.Length; ++i)
{
ref PhysicsCompoundCollider.Child child = ref children[i];
var colliderWorldTransform = PhysicsMath.mul(worldTransform, child.CompoundFromChild);
DrawCollider(children[i].Collider, ref colliderWorldTransform, ref outputStream, colliderColor);
}
return;
}
}