/// <summary>
/// Performs a collider cast along the specified ray.<para/>
///
/// Will return true if there was a collision and populate the provided <see cref="ColliderCastHit"/>.
/// </summary>
/// <param name="nearestHit"></param>
/// <param name="collider"></param>
/// <param name="from"></param>
/// <param name="to"></param>
/// <param name="collisionWorld"></param>
/// <param name="ignore"></param>
/// <param name="filter">Used to exclude collisions that do not match the filter.</param>
/// <param name="manager">Required if specifying a collision filter. Otherwise is unused.</param>
/// <param name="colliderData">Alternative to the EntityManager if used in a job.</param>
/// <returns></returns>
public unsafe static bool ColliderCast(
out ColliderCastHit nearestHit,
PhysicsCollider collider,
float3 from,
float3 to,
ref CollisionWorld collisionWorld,
Entity ignore,
CollisionFilter?filter = null,
EntityManager?manager = null,
ComponentDataFromEntity <PhysicsCollider>?colliderData = null,
Allocator allocator = Allocator.TempJob)
{
nearestHit = new ColliderCastHit();
NativeList <ColliderCastHit> allHits = ColliderCastAll(collider, from, to, ref collisionWorld, ignore, allocator);
if (filter.HasValue)
{
if (manager.HasValue)
{
TrimByFilter(ref allHits, manager.Value, filter.Value);
}
else if (colliderData.HasValue)
{
TrimByFilter(ref allHits, colliderData.Value, filter.Value);
}
}
GetSmallestFractional(ref allHits, out nearestHit);
allHits.Dispose();
return(true);
}
public static bool ColliderCast <T>(ref T target, ColliderCastInput input, out ColliderCastHit result) where T : struct, ICollidable
{
var collector = new ClosestHitCollector <ColliderCastHit>(sfloat.One);
if (target.CastCollider(input, ref collector))
{
result = collector.ClosestHit; // TODO: would be nice to avoid this copy
return(true);
}
result = new ColliderCastHit();
return(false);
}
public static bool ColliderCast <T>(ref T target, ColliderCastInput input, out ColliderCastHit result) where T : struct, IQueryable
{
var collector = new ClosestHitCollector <ColliderCastHit>(1.0f);
if (target.CastCollider(input, ref collector))
{
result = collector.ClosestHit;
return(true);
}
result = new ColliderCastHit();
return(false);
}
public unsafe void PhysicsBodyCastColliderTest()
{
var geometry = new BoxGeometry
{
Size = new float2(1f),
};
using (var collider = PhysicsBoxCollider.Create(geometry))
{
var physicsBody = new PhysicsBody(collider);
var queryInput = new ColliderCastInput()
{
Rotation = float2x2.identity
};
var closestHit = new ColliderCastHit();
var allHits = new NativeList <ColliderCastHit>(Allocator.Temp);
var circleGeometry = new CircleGeometry {
Radius = 0.5f
};
using (var circleBlob = PhysicsCircleCollider.Create(circleGeometry))
{
queryInput.Collider = circleBlob;
// OK case.
var startOK = new float2(-10f, -10f);
var endOK = new float2(10f, 10f);
queryInput.Start = startOK;
queryInput.End = endOK;
Assert.IsTrue(physicsBody.CastCollider(queryInput));
Assert.IsTrue(physicsBody.CastCollider(queryInput, out closestHit));
Assert.IsTrue(physicsBody.CastCollider(queryInput, ref allHits));
// Fail Case.
var startFail = new float2(-10f, -10f);
var endFail = new float2(10f, -10f);
queryInput.Start = startFail;
queryInput.End = endFail;
Assert.IsFalse(physicsBody.CastCollider(queryInput));
Assert.IsFalse(physicsBody.CastCollider(queryInput, out closestHit));
Assert.IsFalse(physicsBody.CastCollider(queryInput, ref allHits));
}
allHits.Dispose();
}
}
public unsafe void RigidBodyCastColliderTest()
{
Physics.RigidBody rigidbody = Unity.Physics.RigidBody.Zero;
const float size = 1.0f;
const float convexRadius = 0.0f;
const float sphereRadius = 1.0f;
var rayStartOK = new float3(-10, -10, -10);
var rayEndOK = new float3(10, 10, 10);
var rayStartFail = new float3(-10, 10, -10);
var rayEndFail = new float3(10, 10, 10);
rigidbody.Collider = (Collider *)BoxCollider.Create(new BoxGeometry
{
Center = float3.zero,
Orientation = quaternion.identity,
Size = size,
BevelRadius = convexRadius
}).GetUnsafePtr();
var colliderCastInput = new ColliderCastInput();
var closestHit = new ColliderCastHit();
var allHits = new NativeList <ColliderCastHit>(Allocator.Temp);
// OK case : Sphere hits the box collider
colliderCastInput.Start = rayStartOK;
colliderCastInput.End = rayEndOK;
colliderCastInput.Collider = (Collider *)SphereCollider.Create(
new SphereGeometry {
Center = float3.zero, Radius = sphereRadius
}
).GetUnsafePtr();
Assert.IsTrue(rigidbody.CastCollider(colliderCastInput));
Assert.IsTrue(rigidbody.CastCollider(colliderCastInput, out closestHit));
Assert.IsTrue(rigidbody.CastCollider(colliderCastInput, ref allHits));
// Fail case : wrong direction
colliderCastInput.Start = rayStartFail;
colliderCastInput.End = rayEndFail;
Assert.IsFalse(rigidbody.CastCollider(colliderCastInput));
Assert.IsFalse(rigidbody.CastCollider(colliderCastInput, out closestHit));
Assert.IsFalse(rigidbody.CastCollider(colliderCastInput, ref allHits));
}
public static unsafe Entity ColliderCast(float3 rayFrom, float3 rayTo, quaternion rotation, CollisionWorld collisionWorld, PhysicsCollider collider, PhysicsWorld physicsWorld)
{
ColliderCastInput input = new ColliderCastInput()
{
Collider = (Collider *)collider.Value.GetUnsafePtr(),
Orientation = rotation,
Start = rayFrom,
End = rayTo
};
ColliderCastHit hit = new ColliderCastHit();
if (collisionWorld.CastCollider(input, out hit))
{
return(physicsWorld.Bodies[hit.RigidBodyIndex].Entity);
}
return(Entity.Null);
}
public unsafe Entity SphereCast(float3 RayFrom, float3 RayTo, float radius)
{
var physicsWorldSystem = World.DefaultGameObjectInjectionWorld.GetExistingSystem <Unity.Physics.Systems.BuildPhysicsWorld>();
var collisionWorld = physicsWorldSystem.PhysicsWorld.CollisionWorld;
var filter = new CollisionFilter()
{
BelongsTo = ~0u,
CollidesWith = ~0u, // all 1s, so all layers, collide with everything
GroupIndex = 0
};
SphereGeometry sphereGeometry = new SphereGeometry()
{
Center = float3.zero, Radius = radius
};
BlobAssetReference <Unity.Physics.Collider> sphereCollider = Unity.Physics.SphereCollider.Create(sphereGeometry, filter);
ColliderCastInput input = new ColliderCastInput()
{
Collider = (Unity.Physics.Collider *)sphereCollider.GetUnsafePtr(),
Orientation = quaternion.identity,
Start = RayFrom,
End = RayTo
};
ColliderCastHit hit = new ColliderCastHit();
bool haveHit = collisionWorld.CastCollider(input, out hit);
if (haveHit)
{
// see hit.Position
// see hit.SurfaceNormal
Entity e = physicsWorldSystem.PhysicsWorld.Bodies[hit.RigidBodyIndex].Entity;
return(e);
}
sphereCollider.Dispose();
return(Entity.Null);
}
public unsafe void RigidBodyCastColliderTest()
{
Physics.RigidBody rigidbody = Unity.Physics.RigidBody.Zero;
const float size = 1.0f;
const float convexRadius = 0.0f;
const float sphereRadius = 1.0f;
var rayStartOK = new float3(-10, -10, -10);
var rayEndOK = new float3(10, 10, 10);
var rayStartFail = new float3(-10, 10, -10);
var rayEndFail = new float3(10, 10, 10);
rigidbody.Collider = (Collider *)BoxCollider.Create(float3.zero, quaternion.identity, new float3(size), convexRadius).GetUnsafePtr();
var colliderCastInput = new ColliderCastInput();
var closestHit = new ColliderCastHit();
var allHits = new NativeList <ColliderCastHit>(Allocator.Temp);
// OK case : Sphere hits the box collider
float3 rayDir = rayEndOK - rayStartOK;
colliderCastInput.Position = rayStartOK;
colliderCastInput.Direction = rayDir;
colliderCastInput.Collider = (Collider *)SphereCollider.Create(float3.zero, sphereRadius).GetUnsafePtr();
Assert.IsTrue(rigidbody.CastCollider(colliderCastInput));
Assert.IsTrue(rigidbody.CastCollider(colliderCastInput, out closestHit));
Assert.IsTrue(rigidbody.CastCollider(colliderCastInput, ref allHits));
// Fail case : wrong direction
rayDir = rayEndFail - rayStartFail;
colliderCastInput.Position = rayStartFail;
colliderCastInput.Direction = rayDir;
Assert.IsFalse(rigidbody.CastCollider(colliderCastInput));
Assert.IsFalse(rigidbody.CastCollider(colliderCastInput, out closestHit));
Assert.IsFalse(rigidbody.CastCollider(colliderCastInput, ref allHits));
}
public unsafe static ColliderCastHit SphereCast(CollisionWorld world, float radius, uint mask, float3 origin, float3 direction)
{
var sphereCollider = Unity.Physics.SphereCollider.Create(float3.zero, radius,
new CollisionFilter()
{
CategoryBits = mask, MaskBits = mask, GroupIndex = (int)mask
});
ColliderCastInput input = new ColliderCastInput()
{
Position = origin,
Orientation = quaternion.identity,
Direction = direction,
Collider = (Collider *)sphereCollider.GetUnsafePtr()
};
ColliderCastHit hit = new ColliderCastHit()
{
RigidBodyIndex = -1
};
world.CastCollider(input, out hit);
return(hit);
}
// Does collider casts and checks some properties of the results:
// - Closest hit returned from the all hits query has the same fraction as the hit returned from the closest hit query
// - Any hit and closest hit queries return a hit if and only if the all hits query does
// - Distance between the shapes at the hit fraction is zero
static unsafe void WorldColliderCastTest(ref Physics.PhysicsWorld world, ColliderCastInput input, ref NativeList <ColliderCastHit> hits, string failureMessage)
{
// Do an all-hits query
hits.Clear();
world.CastCollider(input, ref hits);
// Check each hit and find the earliest
float minFraction = float.MaxValue;
RigidTransform worldFromQuery = new RigidTransform(input.Orientation, input.Position);
for (int iHit = 0; iHit < hits.Length; iHit++)
{
ColliderCastHit hit = hits[iHit];
minFraction = math.min(minFraction, hit.Fraction);
CheckColliderCastHit(ref world, input, hit, failureMessage + ", hits[" + iHit + "]");
}
// Do a closest-hit query and check that the fraction matches
ColliderCastHit closestHit;
bool hasClosestHit = world.CastCollider(input, out closestHit);
if (hits.Length == 0)
{
Assert.IsFalse(hasClosestHit, failureMessage + ", closestHit: no matching result in hits");
}
else
{
Assert.AreEqual(closestHit.Fraction, minFraction, tolerance * math.length(input.Direction), failureMessage + ", closestHit: fraction does not match");
CheckColliderCastHit(ref world, input, closestHit, failureMessage + ", closestHit");
}
// Do an any-hit query and check that it is consistent with the others
bool hasAnyHit = world.CastCollider(input);
Assert.AreEqual(hasAnyHit, hasClosestHit, failureMessage + ": any hit result inconsistent with the others");
// TODO - this test can't catch false misses. We could do brute-force broadphase / midphase search to cover those.
}
public static unsafe void CollideAndIntegrate(
CharacterControllerStepInput stepInput, float characterMass, bool affectBodies, Collider *collider,
MaxHitsCollector <DistanceHit> distanceHitsCollector, ref NativeArray <ColliderCastHit> castHits, ref NativeArray <SurfaceConstraintInfo> constraints,
ref RigidTransform transform, ref float3 linearVelocity, ref BlockStream.Writer deferredImpulseWriter)
{
// Copy parameters
float deltaTime = stepInput.DeltaTime;
float3 gravity = stepInput.Gravity;
float3 up = stepInput.Up;
PhysicsWorld world = stepInput.World;
float remainingTime = deltaTime;
float3 lastDisplacement = linearVelocity * remainingTime;
float3 newPosition = transform.pos;
quaternion orientation = transform.rot;
float3 newVelocity = linearVelocity;
float maxSlopeCos = math.cos(stepInput.MaxSlope);
// Iterate over hits and create constraints from them
int numDistanceConstraints = 0;
for (int hitIndex = 0; hitIndex < distanceHitsCollector.NumHits; hitIndex++)
{
DistanceHit hit = distanceHitsCollector.AllHits[hitIndex];
CreateConstraintFromHit(world, gravity, deltaTime, hit.RigidBodyIndex, hit.ColliderKey, hit.Position,
hit.SurfaceNormal, hit.Distance, false, out SurfaceConstraintInfo constraint);
// Check if max slope plane is required
float verticalComponent = math.dot(constraint.Plane.Normal, up);
bool shouldAddPlane = verticalComponent > SimplexSolver.c_SimplexSolverEpsilon && verticalComponent < maxSlopeCos;
if (shouldAddPlane)
{
AddMaxSlopeConstraint(up, ref constraint, ref constraints, ref numDistanceConstraints);
}
// Add original constraint to the list
constraints[numDistanceConstraints++] = constraint;
}
const float timeEpsilon = 0.000001f;
for (int i = 0; i < stepInput.MaxIterations && remainingTime > timeEpsilon; i++)
{
int numConstraints = numDistanceConstraints;
float3 gravityMovement = gravity * remainingTime * remainingTime * 0.5f;
// Then do a collider cast (but not in first iteration)
if (i > 0)
{
float3 displacement = lastDisplacement + gravityMovement;
MaxHitsCollector <ColliderCastHit> collector = new MaxHitsCollector <ColliderCastHit>(1.0f, ref castHits);
ColliderCastInput input = new ColliderCastInput()
{
Collider = collider,
Orientation = orientation,
Start = newPosition,
End = newPosition + displacement,
};
world.CastCollider(input, ref collector);
// Iterate over hits and create constraints from them
for (int hitIndex = 0; hitIndex < collector.NumHits; hitIndex++)
{
ColliderCastHit hit = collector.AllHits[hitIndex];
bool found = false;
for (int distanceHitIndex = 0; distanceHitIndex < distanceHitsCollector.NumHits; distanceHitIndex++)
{
DistanceHit dHit = distanceHitsCollector.AllHits[distanceHitIndex];
if (dHit.RigidBodyIndex == hit.RigidBodyIndex &&
dHit.ColliderKey.Equals(hit.ColliderKey))
{
found = true;
break;
}
}
// Skip duplicate hits
if (!found)
{
CreateConstraintFromHit(world, gravity, deltaTime, hit.RigidBodyIndex, hit.ColliderKey, hit.Position, hit.SurfaceNormal,
hit.Fraction * math.length(lastDisplacement), false, out SurfaceConstraintInfo constraint);
// Check if max slope plane is required
float verticalComponent = math.dot(constraint.Plane.Normal, up);
bool shouldAddPlane = verticalComponent > SimplexSolver.c_SimplexSolverEpsilon && verticalComponent < maxSlopeCos;
if (shouldAddPlane)
{
AddMaxSlopeConstraint(up, ref constraint, ref constraints, ref numConstraints);
}
// Add original constraint to the list
constraints[numConstraints++] = constraint;
}
}
}
// Solve
float3 prevVelocity = newVelocity;
float3 prevPosition = newPosition;
SimplexSolver.Solve(world, remainingTime, up, numConstraints, ref constraints, ref newPosition, ref newVelocity, out float integratedTime);
// Apply impulses to hit bodies
if (affectBodies)
{
CalculateAndStoreDeferredImpulses(stepInput, characterMass, prevVelocity, numConstraints, ref constraints, ref deferredImpulseWriter);
}
float3 newDisplacement = newPosition - prevPosition;
// Check if we can walk to the position simplex solver has suggested
MaxHitsCollector <ColliderCastHit> newCollector = new MaxHitsCollector <ColliderCastHit>(1.0f, ref castHits);
int newContactIndex = -1;
// If simplex solver moved the character we need to re-cast to make sure it can move to new position
if (math.lengthsq(newDisplacement) > SimplexSolver.c_SimplexSolverEpsilon)
{
float3 displacement = newDisplacement + gravityMovement;
ColliderCastInput input = new ColliderCastInput()
{
Collider = collider,
Orientation = orientation,
Start = prevPosition,
End = prevPosition + displacement
};
world.CastCollider(input, ref newCollector);
for (int hitIndex = 0; hitIndex < newCollector.NumHits; hitIndex++)
{
ColliderCastHit hit = newCollector.AllHits[hitIndex];
bool found = false;
for (int constraintIndex = 0; constraintIndex < numConstraints; constraintIndex++)
{
SurfaceConstraintInfo constraint = constraints[constraintIndex];
if (constraint.RigidBodyIndex == hit.RigidBodyIndex &&
constraint.ColliderKey.Equals(hit.ColliderKey))
{
found = true;
break;
}
}
if (!found)
{
newContactIndex = hitIndex;
break;
}
}
}
// Move character along the newDisplacement direction until it reaches this new contact
if (newContactIndex >= 0)
{
ColliderCastHit newContact = newCollector.AllHits[newContactIndex];
Assert.IsTrue(newContact.Fraction >= 0.0f && newContact.Fraction <= 1.0f);
integratedTime *= newContact.Fraction;
newPosition = prevPosition + newDisplacement * newContact.Fraction;
}
remainingTime -= integratedTime;
// Remember last displacement for next iteration
lastDisplacement = newVelocity * remainingTime;
}
// Write back position and velocity
transform.pos = newPosition;
linearVelocity = newVelocity;
}
public bool CapsuleCast(float3 point1, float3 point2, float radius, float3 direction, float maxDistance, out ColliderCastHit hitInfo, CollisionFilter filter, QueryInteraction queryInteraction = QueryInteraction.Default) => QueryWrappers.CapsuleCast(ref this, point1, point2, radius, direction, maxDistance, out hitInfo, filter, queryInteraction);
public bool BoxCast(float3 center, quaternion orientation, float3 halfExtents, float3 direction, float maxDistance, out ColliderCastHit hitInfo, CollisionFilter filter, QueryInteraction queryInteraction = QueryInteraction.Default) => QueryWrappers.BoxCast(ref this, center, orientation, halfExtents, direction, maxDistance, out hitInfo, filter, queryInteraction);
public bool SphereCast(float3 origin, float radius, float3 direction, float maxDistance, out ColliderCastHit hitInfo, CollisionFilter filter, QueryInteraction queryInteraction = QueryInteraction.Default) => QueryWrappers.SphereCast(ref this, origin, radius, direction, maxDistance, out hitInfo, filter, queryInteraction);
static unsafe bool CastCollider(
Ray ray, ref float2x2 rotation,
ref DistanceProxy proxySource, ref DistanceProxy proxyTarget,
out ColliderCastHit hit)
{
hit = default;
var transformSource = new PhysicsTransform
{
Translation = ray.Origin,
Rotation = rotation
};
// Check we're not initially overlapped.
if ((proxySource.VertexCount < 3 || proxyTarget.VertexCount < 3) &&
OverlapQueries.OverlapConvexConvex(ref transformSource, ref proxySource, ref proxyTarget))
{
return(false);
}
// B = Source
// A = Target
var radiusSource = proxySource.ConvexRadius;
var radiusTarget = proxyTarget.ConvexRadius;
var totalRadius = radiusSource + radiusTarget;
var invRotation = math.inverse(rotation);
var sweepDirection = ray.Displacement;
var normal = float2.zero;
var lambda = 0.0f;
// Initialize the simplex.
var simplex = new Simplex();
simplex.Count = 0;
var vertices = &simplex.Vertex1;
// Get a support point in the inverse direction.
var indexTarget = proxyTarget.GetSupport(-sweepDirection);
var supportTarget = proxyTarget.Vertices[indexTarget];
var indexSource = proxySource.GetSupport(PhysicsMath.mul(invRotation, sweepDirection));
var supportSource = PhysicsMath.mul(transformSource, proxySource.Vertices[indexSource]);
var v = supportTarget - supportSource;
// Sigma is the target distance between polygons
var sigma = math.max(PhysicsSettings.Constants.MinimumConvexRadius, totalRadius - PhysicsSettings.Constants.MinimumConvexRadius);
const float tolerance = PhysicsSettings.Constants.LinearSlop * 0.5f;
var iteration = 0;
while (
iteration++ < PhysicsSettings.Constants.MaxGJKInterations &&
math.abs(math.length(v) - sigma) > tolerance
)
{
if (simplex.Count >= 3)
{
SafetyChecks.ThrowInvalidOperationException("ColliderCast Simplex must have less than 3 vertex.");
}
// Support in direction -supportV (Target - Source)
indexTarget = proxyTarget.GetSupport(-v);
supportTarget = proxyTarget.Vertices[indexTarget];
indexSource = proxySource.GetSupport(PhysicsMath.mul(invRotation, v));
supportSource = PhysicsMath.mul(transformSource, proxySource.Vertices[indexSource]);
var p = supportTarget - supportSource;
v = math.normalizesafe(v);
// Intersect ray with plane.
var vp = math.dot(v, p);
var vr = math.dot(v, sweepDirection);
if (vp - sigma > lambda * vr)
{
if (vr <= 0.0f)
{
return(false);
}
lambda = (vp - sigma) / vr;
if (lambda > 1.0f)
{
return(false);
}
normal = -v;
simplex.Count = 0;
}
// Reverse simplex since it works with B - A.
// Shift by lambda * r because we want the closest point to the current clip point.
// Note that the support point p is not shifted because we want the plane equation
// to be formed in un-shifted space.
var vertex = vertices + simplex.Count;
vertex->IndexA = indexSource;
vertex->SupportA = supportSource + lambda * sweepDirection;
vertex->IndexB = indexTarget;
vertex->SupportB = supportTarget;
vertex->W = vertex->SupportB - vertex->SupportA;
vertex->A = 1.0f;
simplex.Count += 1;
switch (simplex.Count)
{
case 1:
break;
case 2:
simplex.Solve2();
break;
case 3:
simplex.Solve3();
break;
default:
SafetyChecks.ThrowInvalidOperationException("Simplex has invalid count.");
return(default);
}
// If we have 3 points, then the origin is in the corresponding triangle.
if (simplex.Count == 3)
{
// Overlap.
return(false);
}
// Get search direction.
v = simplex.GetClosestPoint();
}
// Ensure we don't process an empty simplex.
if (simplex.Count == 0)
{
return(false);
}
// Prepare result.
var pointSource = float2.zero;
var pointTarget = float2.zero;
simplex.GetWitnessPoints(ref pointSource, ref pointTarget);
normal = math.normalizesafe(-v);
hit = new ColliderCastHit
{
Position = pointTarget + (normal * radiusTarget),
SurfaceNormal = normal,
Fraction = lambda
};
return(true);
}
public bool CastCollider(ColliderCastInput input, out ColliderCastHit closestHit) => QueryWrappers.ColliderCast(ref this, input, out closestHit);
public static bool SphereCast <T>(ref T target, float3 origin, float radius, float3 direction, float maxDistance, out ColliderCastHit hitInfo, CollisionFilter filter, QueryInteraction queryInteraction = QueryInteraction.Default)
where T : struct, ICollidable
{
var collector = new ClosestHitCollector <ColliderCastHit>(1.0f);
bool hasHit = target.SphereCastCustom(origin, radius, direction, maxDistance, ref collector, filter, queryInteraction);
hitInfo = collector.ClosestHit;
return(hasHit);
}
public static unsafe void CollideAndIntegrate(PhysicsWorld world, float deltaTime,
int maxIterations, float3 up, float3 gravity,
float characterMass, float tau, float damping, bool affectBodies, Collider *collider,
ref NativeArray <DistanceHit> distanceHits, ref NativeArray <ColliderCastHit> castHits, ref NativeArray <SurfaceConstraintInfo> constraints,
ref RigidTransform transform, ref float3 linearVelocity, ref BlockStream.Writer deferredImpulseWriter)
{
float remainingTime = deltaTime;
float3 lastDisplacement = linearVelocity * remainingTime;
float3 newPosition = transform.pos;
quaternion orientation = transform.rot;
float3 newVelocity = linearVelocity;
const float timeEpsilon = 0.000001f;
for (int i = 0; i < maxIterations && remainingTime > timeEpsilon; i++)
{
// First do distance query for penetration recovery
MaxHitsCollector <DistanceHit> distanceHitsCollector = new MaxHitsCollector <DistanceHit>(0.0f, ref distanceHits);
int numConstraints = 0;
{
ColliderDistanceInput input = new ColliderDistanceInput()
{
MaxDistance = 0.0f,
Transform = new RigidTransform
{
pos = newPosition,
rot = orientation,
},
Collider = collider
};
world.CalculateDistance(input, ref distanceHitsCollector);
// Iterate over hits and create constraints from them
for (int hitIndex = 0; hitIndex < distanceHitsCollector.NumHits; hitIndex++)
{
DistanceHit hit = distanceHitsCollector.AllHits[hitIndex];
CreateConstraintFromHit(world, gravity, deltaTime, hit.RigidBodyIndex, hit.ColliderKey, hit.Position,
hit.SurfaceNormal, hit.Distance, false, out SurfaceConstraintInfo constraint);
constraints[numConstraints++] = constraint;
}
}
// Then do a collider cast
{
float3 displacement = lastDisplacement - up * timeEpsilon;
float3 endPosition = newPosition + displacement;
MaxHitsCollector <ColliderCastHit> collector = new MaxHitsCollector <ColliderCastHit>(1.0f, ref castHits);
ColliderCastInput input = new ColliderCastInput()
{
Collider = collider,
Orientation = orientation,
Position = newPosition,
Direction = displacement
};
world.CastCollider(input, ref collector);
// Iterate over hits and create constraints from them
for (int hitIndex = 0; hitIndex < collector.NumHits; hitIndex++)
{
ColliderCastHit hit = collector.AllHits[hitIndex];
bool found = false;
for (int distanceHitIndex = 0; distanceHitIndex < distanceHitsCollector.NumHits; distanceHitIndex++)
{
DistanceHit dHit = distanceHitsCollector.AllHits[distanceHitIndex];
if (dHit.RigidBodyIndex == hit.RigidBodyIndex &&
dHit.ColliderKey.Equals(hit.ColliderKey))
{
found = true;
break;
}
}
// Skip duplicate hits
if (!found)
{
CreateConstraintFromHit(world, gravity, deltaTime, hit.RigidBodyIndex, hit.ColliderKey, hit.Position, hit.SurfaceNormal,
hit.Fraction * math.length(lastDisplacement), false, out SurfaceConstraintInfo constraint);
constraints[numConstraints++] = constraint;
}
}
}
// petarm.todo: Add max slope plane to avoid climbing the not allowed slopes
// Solve
float3 prevVelocity = newVelocity;
SimplexSolver.Solve(world, deltaTime, up, numConstraints, ref constraints, ref newPosition, ref newVelocity, out float integratedTime);
remainingTime -= integratedTime;
lastDisplacement = newVelocity * remainingTime;
// Apply impulses to hit bodies
if (affectBodies)
{
ResolveContacts(world, deltaTime, gravity, tau, damping, characterMass, prevVelocity, numConstraints, ref constraints, ref deferredImpulseWriter);
}
}
// Write back position and velocity
transform.pos = newPosition;
linearVelocity = newVelocity;
}
public static unsafe void SolveCollisionConstraints(PhysicsWorld world, float deltaTime, int maxIterations, float skinWidth, float maxSlope, Collider *collider, ref RigidTransform transform, ref float3 velocity, ref NativeArray <DistanceHit> distanceHits, ref NativeArray <ColliderCastHit> colliderHits, ref NativeArray <SurfaceConstraintInfo> surfaceConstraints)
{
float remainingTime = deltaTime;
float3 previousDisplacement = velocity * remainingTime;
float3 outPosition = transform.pos;
float3 outVelocity = velocity;
quaternion orientation = transform.rot;
const float timeEpsilon = 0.000001f;
for (int i = 0; i < maxIterations && remainingTime > timeEpsilon; i++)
{
MaxHitCollector <DistanceHit> distanceHitCollector = new MaxHitCollector <DistanceHit>(skinWidth, ref distanceHits);
int constraintCount = 0;
// Handle distance checks
{
ColliderDistanceInput input = new ColliderDistanceInput
{
Collider = collider,
MaxDistance = skinWidth,
Transform = new RigidTransform
{
pos = outPosition,
rot = orientation
}
};
world.CalculateDistance(input, ref distanceHitCollector);
for (int hitIndex = 0; hitIndex < distanceHitCollector.NumHits; hitIndex++)
{
DistanceHit hit = distanceHitCollector.AllHits[hitIndex];
CreateConstraintFromHit(world, hit.ColliderKey, hit.RigidBodyIndex, hit.Position, float3.zero, hit.SurfaceNormal, hit.Distance, deltaTime, out SurfaceConstraintInfo constraint);
CreateSlopeConstraint(math.up(), math.cos(maxSlope), ref constraint, ref surfaceConstraints, ref constraintCount);
surfaceConstraints[constraintCount++] = constraint;
}
}
// Handle Collider
{
float3 displacement = previousDisplacement;
MaxHitCollector <ColliderCastHit> colliderHitCollector = new MaxHitCollector <ColliderCastHit>(1.0f, ref colliderHits);
ColliderCastInput input = new ColliderCastInput
{
Collider = collider,
Position = outPosition,
Direction = velocity,
Orientation = orientation
};
world.CastCollider(input, ref colliderHitCollector);
for (int hitIndex = 0; hitIndex < colliderHitCollector.NumHits; hitIndex++)
{
ColliderCastHit hit = colliderHitCollector.AllHits[hitIndex];
bool duplicate = false;
for (int distanceHitIndex = 0; distanceHitIndex < distanceHitCollector.NumHits; distanceHitIndex++)
{
DistanceHit distanceHit = distanceHitCollector.AllHits[distanceHitIndex];
if (distanceHit.RigidBodyIndex == hit.RigidBodyIndex && distanceHit.ColliderKey.Equals(hit.ColliderKey))
{
duplicate = true;
break;
}
}
if (!duplicate)
{
CreateConstraintFromHit(world, hit.ColliderKey, hit.RigidBodyIndex, hit.Position, outVelocity, hit.SurfaceNormal, hit.Fraction * math.length(previousDisplacement), deltaTime, out SurfaceConstraintInfo constraint);
CreateSlopeConstraint(math.up(), math.cos(maxSlope), ref constraint, ref surfaceConstraints, ref constraintCount);
surfaceConstraints[constraintCount++] = constraint;
}
}
}
float3 previousPosition = outPosition;
float3 previousVelocity = outVelocity;
SimplexSolver.Solve(world, remainingTime, math.up(), constraintCount, ref surfaceConstraints, ref outPosition, ref outVelocity, out float integratedTime);
float3 currentDisplacement = outPosition - previousPosition;
MaxHitCollector <ColliderCastHit> displacementHitCollector = new MaxHitCollector <ColliderCastHit>(1.0f, ref colliderHits);
int displacementContactIndex = -1;
if (math.lengthsq(currentDisplacement) > SimplexSolver.c_SimplexSolverEpsilon)
{
ColliderCastInput input = new ColliderCastInput
{
Collider = collider,
Position = previousPosition,
Direction = currentDisplacement,
Orientation = orientation
};
world.CastCollider(input, ref displacementHitCollector);
for (int hitIndex = 0; hitIndex < distanceHitCollector.NumHits; hitIndex++)
{
ColliderCastHit hit = displacementHitCollector.AllHits[hitIndex];
bool duplicate = false;
for (int constrainIndex = 0; constrainIndex < constraintCount; constrainIndex++)
{
SurfaceConstraintInfo constraint = surfaceConstraints[constrainIndex];
if (constraint.RigidBodyIndex == hit.RigidBodyIndex && constraint.ColliderKey.Equals(hit.ColliderKey))
{
duplicate = true;
break;
}
if (!duplicate)
{
displacementContactIndex = hitIndex;
break;
}
}
}
if (displacementContactIndex >= 0)
{
ColliderCastHit newContact = displacementHitCollector.AllHits[displacementContactIndex];
float fraction = newContact.Fraction / math.length(currentDisplacement);
integratedTime *= fraction;
float3 displacement = currentDisplacement * fraction;
outPosition = previousPosition + displacement;
}
}
remainingTime -= integratedTime;
previousDisplacement = outVelocity * remainingTime;
}
transform.pos = outPosition;
velocity = outVelocity;
}
public static unsafe void CollideAndIntegrate(
CharacterControllerStepInput stepInput, float characterMass, bool affectBodies, Collider *collider,
ref NativeArray <ColliderCastHit> castHits, ref NativeArray <SurfaceConstraintInfo> constraints, int numConstraints,
ref RigidTransform transform, ref float3 linearVelocity, ref BlockStream.Writer deferredImpulseWriter)
{
// Copy parameters
float deltaTime = stepInput.DeltaTime;
float3 gravity = stepInput.Gravity;
float3 up = stepInput.Up;
PhysicsWorld world = stepInput.World;
float remainingTime = deltaTime;
float3 lastDisplacement = linearVelocity * remainingTime;
float3 newPosition = transform.pos;
quaternion orientation = transform.rot;
float3 newVelocity = linearVelocity;
float maxSlopeCos = math.cos(stepInput.MaxSlope);
const float timeEpsilon = 0.000001f;
for (int i = 0; i < stepInput.MaxIterations && remainingTime > timeEpsilon; i++)
{
float3 gravityMovement = gravity * remainingTime * remainingTime * 0.5f;
// Then do a collider cast (but not in first iteration)
if (i > 0)
{
int numCastConstraints = 0;
float3 displacement = lastDisplacement + gravityMovement;
MaxHitsCollector <ColliderCastHit> collector = new MaxHitsCollector <ColliderCastHit>(stepInput.RigidBodyIndex, 1.0f, ref castHits);
ColliderCastInput input = new ColliderCastInput()
{
Collider = collider,
Orientation = orientation,
Start = newPosition,
End = newPosition + displacement * (1.0f + stepInput.ContactTolerance),
};
world.CastCollider(input, ref collector);
// Iterate over hits and create constraints from them
for (int hitIndex = 0; hitIndex < collector.NumHits; hitIndex++)
{
ColliderCastHit hit = collector.AllHits[hitIndex];
CreateConstraint(stepInput.World, stepInput.Up,
hit.RigidBodyIndex, hit.ColliderKey, hit.Position, hit.SurfaceNormal, hit.Fraction * math.length(lastDisplacement),
stepInput.SkinWidth, maxSlopeCos, ref constraints, ref numCastConstraints);
}
numConstraints = numCastConstraints;
}
// Min delta time for solver to break
float minDeltaTime = 0.0f;
if (math.lengthsq(newVelocity) > k_SimplexSolverEpsilonSq)
{
// Min delta time to travel at least 1cm
minDeltaTime = 0.01f / math.length(newVelocity);
}
// Solve
float3 prevVelocity = newVelocity;
float3 prevPosition = newPosition;
SimplexSolver.Solve(world, remainingTime, minDeltaTime, up, numConstraints, ref constraints, ref newPosition, ref newVelocity, out float integratedTime);
// Apply impulses to hit bodies
if (affectBodies)
{
CalculateAndStoreDeferredImpulses(stepInput, characterMass, prevVelocity, numConstraints, ref constraints, ref deferredImpulseWriter);
}
float3 newDisplacement = newPosition - prevPosition;
// Check if we can walk to the position simplex solver has suggested
MaxHitsCollector <ColliderCastHit> newCollector = new MaxHitsCollector <ColliderCastHit>(stepInput.RigidBodyIndex, 1.0f, ref castHits);
int newContactIndex = -1;
// If simplex solver moved the character we need to re-cast to make sure it can move to new position
if (math.lengthsq(newDisplacement) > k_SimplexSolverEpsilon)
{
float3 displacement = newDisplacement + gravityMovement;
ColliderCastInput input = new ColliderCastInput()
{
Collider = collider,
Orientation = orientation,
Start = prevPosition,
End = prevPosition + displacement * (1.0f + stepInput.ContactTolerance)
};
world.CastCollider(input, ref newCollector);
float minFraction = float.MaxValue;
for (int hitIndex = 0; hitIndex < newCollector.NumHits; hitIndex++)
{
ColliderCastHit hit = newCollector.AllHits[hitIndex];
if (hit.Fraction < minFraction)
{
bool found = false;
for (int constraintIndex = 0; constraintIndex < numConstraints; constraintIndex++)
{
SurfaceConstraintInfo constraint = constraints[constraintIndex];
if (constraint.RigidBodyIndex == hit.RigidBodyIndex &&
constraint.ColliderKey.Equals(hit.ColliderKey))
{
found = true;
break;
}
}
if (!found)
{
minFraction = hit.Fraction;
newContactIndex = hitIndex;
}
}
}
}
// Move character along the newDisplacement direction until it reaches this new contact
if (newContactIndex >= 0)
{
ColliderCastHit newContact = newCollector.AllHits[newContactIndex];
Assert.IsTrue(newContact.Fraction >= 0.0f && newContact.Fraction <= 1.0f);
integratedTime *= newContact.Fraction;
newPosition = prevPosition + newDisplacement * newContact.Fraction;
}
remainingTime -= integratedTime;
// Remember last displacement for next iteration
lastDisplacement = newVelocity * remainingTime;
}
// Write back position and velocity
transform.pos = newPosition;
linearVelocity = newVelocity;
}
public static unsafe void CollideAndIntegrate(
CharacterControllerStepInput stepInput, float characterMass, bool affectBodies, Collider *collider,
ref RigidTransform transform, ref float3 linearVelocity, ref NativeStream.Writer defferredImpulseWriter,
ref NativeList <SurfaceConstraintInfo> constraints, ref NativeList <ColliderCastHit> castHits, ref NativeList <DistanceHit> distanceHits, out ColliderCastInput debugInput, out ColliderCastHit debugHit)
{
float deltaTime = stepInput.DeltaTime;
float3 up = stepInput.Up;
PhysicsWorld world = stepInput.World;
float remaingTIme = deltaTime;
float3 newPosition = transform.pos;
quaternion orientation = transform.rot;
float3 newVelocity = linearVelocity;
float maxSlopCos = math.cos(stepInput.MaxSlope);
debugInput = (default);
static unsafe void CheckColliderCastHit(ref Physics.PhysicsWorld world, ColliderCastInput input, ColliderCastHit hit, string failureMessage)
{
// Fetch the leaf collider and convert the shape cast result into a distance result at the hit transform
ChildCollider leaf;
MTransform queryFromWorld = Math.Inverse(new MTransform(input.Orientation, input.Position + input.Direction * hit.Fraction));
GetHitLeaf(ref world, hit.RigidBodyIndex, hit.ColliderKey, queryFromWorld, out leaf, out MTransform queryFromTarget);
DistanceQueries.Result result = new DistanceQueries.Result
{
PositionOnAinA = Math.Mul(queryFromWorld, hit.Position),
NormalInA = math.mul(queryFromWorld.Rotation, hit.SurfaceNormal),
Distance = 0.0f
};
// If the fraction is zero then the shapes should penetrate, otherwise they should have zero distance
if (hit.Fraction == 0.0f)
{
// Do a distance query to verify initial penetration
result.Distance = DistanceQueries.ConvexConvex(input.Collider, leaf.Collider, queryFromTarget).Distance;
Assert.Less(result.Distance, tolerance, failureMessage + ": zero fraction with positive distance");
}
// Verify the distance at the hit transform
ValidateDistanceResult(result, ref ((ConvexCollider *)input.Collider)->ConvexHull, ref ((ConvexCollider *)leaf.Collider)->ConvexHull, queryFromTarget, result.Distance, failureMessage);
}
static bool CastConvex(ref ColliderCastInput input, ref DistanceProxy proxyTarget, out ColliderCastHit hit)
{
DistanceProxy proxySource;
var inputColliderBlob = input.Collider;
switch (inputColliderBlob.Value.ColliderType)
{
case ColliderType.Box:
case ColliderType.Polygon:
case ColliderType.Capsule:
case ColliderType.Circle:
{
ref var convexHull = ref inputColliderBlob.GetColliderRef <ConvexCollider>().m_ConvexHull;
proxySource = new DistanceProxy(ref convexHull);
break;
}
public static unsafe void CheckSupport(
ref PhysicsWorld world, ref PhysicsCollider collider, CharacterControllerStepInput stepInput, RigidTransform transform,
out CharacterSupportState characterState, out float3 surfaceNormal, out float3 surfaceVelocity,
NativeList <StatefulCollisionEvent> collisionEvents = default)
{
surfaceNormal = float3.zero;
surfaceVelocity = float3.zero;
// Up direction must be normalized
Assert.IsTrue(Unity.Physics.Math.IsNormalized(stepInput.Up));
// Query the world
NativeList <ColliderCastHit> castHits = new NativeList <ColliderCastHit>(k_DefaultQueryHitsCapacity, Allocator.Temp);
CharacterControllerAllHitsCollector <ColliderCastHit> castHitsCollector = new CharacterControllerAllHitsCollector <ColliderCastHit>(
stepInput.RigidBodyIndex, 1.0f, ref castHits, world);
var maxDisplacement = -stepInput.ContactTolerance * stepInput.Up;
{
ColliderCastInput input = new ColliderCastInput()
{
Collider = collider.ColliderPtr,
Orientation = transform.rot,
Start = transform.pos,
End = transform.pos + maxDisplacement
};
world.CastCollider(input, ref castHitsCollector);
}
// If no hits, proclaim unsupported state
if (castHitsCollector.NumHits == 0)
{
characterState = CharacterSupportState.Unsupported;
return;
}
float maxSlopeCos = math.cos(stepInput.MaxSlope);
// Iterate over distance hits and create constraints from them
NativeList <SurfaceConstraintInfo> constraints = new NativeList <SurfaceConstraintInfo>(k_DefaultConstraintsCapacity, Allocator.Temp);
float maxDisplacementLength = math.length(maxDisplacement);
for (int i = 0; i < castHitsCollector.NumHits; i++)
{
ColliderCastHit hit = castHitsCollector.AllHits[i];
CreateConstraint(stepInput.World, stepInput.Up,
hit.RigidBodyIndex, hit.ColliderKey, hit.Position, hit.SurfaceNormal, hit.Fraction * maxDisplacementLength,
stepInput.SkinWidth, maxSlopeCos, ref constraints);
}
// Velocity for support checking
float3 initialVelocity = maxDisplacement / stepInput.DeltaTime;
// Solve downwards (don't use min delta time, try to solve full step)
float3 outVelocity = initialVelocity;
float3 outPosition = transform.pos;
SimplexSolver.Solve(stepInput.DeltaTime, stepInput.DeltaTime, stepInput.Up, stepInput.MaxMovementSpeed,
constraints, ref outPosition, ref outVelocity, out float integratedTime, false);
// Get info on surface
int numSupportingPlanes = 0;
{
for (int j = 0; j < constraints.Length; j++)
{
var constraint = constraints[j];
if (constraint.Touched && !constraint.IsTooSteep && !constraint.IsMaxSlope)
{
numSupportingPlanes++;
surfaceNormal += constraint.Plane.Normal;
surfaceVelocity += constraint.Velocity;
// Add supporting planes to collision events
if (collisionEvents.IsCreated)
{
var collisionEvent = new StatefulCollisionEvent(stepInput.World.Bodies[stepInput.RigidBodyIndex].Entity,
stepInput.World.Bodies[constraint.RigidBodyIndex].Entity, stepInput.RigidBodyIndex, constraint.RigidBodyIndex,
ColliderKey.Empty, constraint.ColliderKey, constraint.Plane.Normal);
collisionEvent.CollisionDetails = new StatefulCollisionEvent.Details(1, 0, constraint.HitPosition);
collisionEvents.Add(collisionEvent);
}
}
}
if (numSupportingPlanes > 0)
{
float invNumSupportingPlanes = 1.0f / numSupportingPlanes;
surfaceNormal *= invNumSupportingPlanes;
surfaceVelocity *= invNumSupportingPlanes;
surfaceNormal = math.normalize(surfaceNormal);
}
}
// Check support state
{
if (math.lengthsq(initialVelocity - outVelocity) < k_SimplexSolverEpsilonSq)
{
// If velocity hasn't changed significantly, declare unsupported state
characterState = CharacterSupportState.Unsupported;
}
else if (math.lengthsq(outVelocity) < k_SimplexSolverEpsilonSq && numSupportingPlanes > 0)
{
// If velocity is very small, declare supported state
characterState = CharacterSupportState.Supported;
}
else
{
// Check if sliding
outVelocity = math.normalize(outVelocity);
float slopeAngleSin = math.max(0.0f, math.dot(outVelocity, -stepInput.Up));
float slopeAngleCosSq = 1 - slopeAngleSin * slopeAngleSin;
if (slopeAngleCosSq <= maxSlopeCos * maxSlopeCos)
{
characterState = CharacterSupportState.Sliding;
}
else if (numSupportingPlanes > 0)
{
characterState = CharacterSupportState.Supported;
}
else
{
// If numSupportingPlanes is 0, surface normal is invalid, so state is unsupported
characterState = CharacterSupportState.Unsupported;
}
}
}
}
public static unsafe void CollideAndIntegrate(
CharacterControllerStepInput stepInput, float characterMass, bool affectBodies, Unity.Physics.Collider *collider,
ref RigidTransform transform, ref float3 linearVelocity, ref NativeStream.Writer deferredImpulseWriter,
NativeList <StatefulCollisionEvent> collisionEvents = default, NativeList <StatefulTriggerEvent> triggerEvents = default)
{
// Copy parameters
float deltaTime = stepInput.DeltaTime;
float3 up = stepInput.Up;
PhysicsWorld world = stepInput.World;
float remainingTime = deltaTime;
float3 newPosition = transform.pos;
quaternion orientation = transform.rot;
float3 newVelocity = linearVelocity;
float maxSlopeCos = math.cos(stepInput.MaxSlope);
const float timeEpsilon = 0.000001f;
for (int i = 0; i < stepInput.MaxIterations && remainingTime > timeEpsilon; i++)
{
NativeList <SurfaceConstraintInfo> constraints = new NativeList <SurfaceConstraintInfo>(k_DefaultConstraintsCapacity, Allocator.Temp);
// Do a collider cast
{
float3 displacement = newVelocity * remainingTime;
NativeList <ColliderCastHit> triggerHits = default;
if (triggerEvents.IsCreated)
{
triggerHits = new NativeList <ColliderCastHit>(k_DefaultQueryHitsCapacity / 4, Allocator.Temp);
}
NativeList <ColliderCastHit> castHits = new NativeList <ColliderCastHit>(k_DefaultQueryHitsCapacity, Allocator.Temp);
CharacterControllerAllHitsCollector <ColliderCastHit> collector = new CharacterControllerAllHitsCollector <ColliderCastHit>(
stepInput.RigidBodyIndex, 1.0f, ref castHits, world, triggerHits);
ColliderCastInput input = new ColliderCastInput()
{
Collider = collider,
Orientation = orientation,
Start = newPosition,
End = newPosition + displacement
};
world.CastCollider(input, ref collector);
// Iterate over hits and create constraints from them
for (int hitIndex = 0; hitIndex < collector.NumHits; hitIndex++)
{
ColliderCastHit hit = collector.AllHits[hitIndex];
CreateConstraint(stepInput.World, stepInput.Up,
hit.RigidBodyIndex, hit.ColliderKey, hit.Position, hit.SurfaceNormal, math.dot(-hit.SurfaceNormal, hit.Fraction * displacement),
stepInput.SkinWidth, maxSlopeCos, ref constraints);
}
// Update trigger events
if (triggerEvents.IsCreated)
{
UpdateTriggersSeen(stepInput, triggerHits, triggerEvents, collector.MinHitFraction);
}
}
// Then do a collider distance for penetration recovery,
// but only fix up penetrating hits
{
// Collider distance query
NativeList <DistanceHit> distanceHits = new NativeList <DistanceHit>(k_DefaultQueryHitsCapacity, Allocator.Temp);
CharacterControllerAllHitsCollector <DistanceHit> distanceHitsCollector = new CharacterControllerAllHitsCollector <DistanceHit>(
stepInput.RigidBodyIndex, stepInput.ContactTolerance, ref distanceHits, world);
{
ColliderDistanceInput input = new ColliderDistanceInput()
{
MaxDistance = stepInput.ContactTolerance,
Transform = transform,
Collider = collider
};
world.CalculateDistance(input, ref distanceHitsCollector);
}
// Iterate over penetrating hits and fix up distance and normal
int numConstraints = constraints.Length;
for (int hitIndex = 0; hitIndex < distanceHitsCollector.NumHits; hitIndex++)
{
DistanceHit hit = distanceHitsCollector.AllHits[hitIndex];
if (hit.Distance < stepInput.SkinWidth)
{
bool found = false;
// Iterate backwards to locate the original constraint before the max slope constraint
for (int constraintIndex = numConstraints - 1; constraintIndex >= 0; constraintIndex--)
{
SurfaceConstraintInfo constraint = constraints[constraintIndex];
if (constraint.RigidBodyIndex == hit.RigidBodyIndex &&
constraint.ColliderKey.Equals(hit.ColliderKey))
{
// Fix up the constraint (normal, distance)
{
// Create new constraint
CreateConstraintFromHit(world, hit.RigidBodyIndex, hit.ColliderKey,
hit.Position, hit.SurfaceNormal, hit.Distance,
stepInput.SkinWidth, out SurfaceConstraintInfo newConstraint);
// Resolve its penetration
ResolveConstraintPenetration(ref newConstraint);
// Write back
constraints[constraintIndex] = newConstraint;
}
found = true;
break;
}
}
// Add penetrating hit not caught by collider cast
if (!found)
{
CreateConstraint(stepInput.World, stepInput.Up,
hit.RigidBodyIndex, hit.ColliderKey, hit.Position, hit.SurfaceNormal, hit.Distance,
stepInput.SkinWidth, maxSlopeCos, ref constraints);
}
}
}
}
// Min delta time for solver to break
float minDeltaTime = 0.0f;
if (math.lengthsq(newVelocity) > k_SimplexSolverEpsilonSq)
{
// Min delta time to travel at least 1cm
minDeltaTime = 0.01f / math.length(newVelocity);
}
// Solve
float3 prevVelocity = newVelocity;
float3 prevPosition = newPosition;
SimplexSolver.Solve(remainingTime, minDeltaTime, up, stepInput.MaxMovementSpeed, constraints, ref newPosition, ref newVelocity, out float integratedTime);
// Apply impulses to hit bodies and store collision events
if (affectBodies || collisionEvents.IsCreated)
{
CalculateAndStoreDeferredImpulsesAndCollisionEvents(stepInput, affectBodies, characterMass,
prevVelocity, constraints, ref deferredImpulseWriter, collisionEvents);
}
// Calculate new displacement
float3 newDisplacement = newPosition - prevPosition;
// If simplex solver moved the character we need to re-cast to make sure it can move to new position
if (math.lengthsq(newDisplacement) > k_SimplexSolverEpsilon)
{
// Check if we can walk to the position simplex solver has suggested
var newCollector = new CharacterControllerClosestHitCollector <ColliderCastHit>(constraints, world, stepInput.RigidBodyIndex, 1.0f);
ColliderCastInput input = new ColliderCastInput()
{
Collider = collider,
Orientation = orientation,
Start = prevPosition,
End = prevPosition + newDisplacement
};
world.CastCollider(input, ref newCollector);
if (newCollector.NumHits > 0)
{
ColliderCastHit hit = newCollector.ClosestHit;
// Move character along the newDisplacement direction until it reaches this new contact
{
Assert.IsTrue(hit.Fraction >= 0.0f && hit.Fraction <= 1.0f);
integratedTime *= hit.Fraction;
newPosition = prevPosition + newDisplacement * hit.Fraction;
}
}
}
// Reduce remaining time
remainingTime -= integratedTime;
// Write back position so that the distance query will update results
transform.pos = newPosition;
}
// Write back final velocity
linearVelocity = newVelocity;
}
public static bool BoxCast <T>(ref T target, float3 center, quaternion orientation, float3 halfExtents, float3 direction, sfloat maxDistance, out ColliderCastHit hitInfo, CollisionFilter filter, QueryInteraction queryInteraction = QueryInteraction.Default)
where T : struct, ICollidable
{
var collector = new ClosestHitCollector <ColliderCastHit>(sfloat.One);
bool hasHit = target.BoxCastCustom(center, orientation, halfExtents, direction, maxDistance, ref collector, filter, queryInteraction);
hitInfo = collector.ClosestHit;
return(hasHit);
}
public static unsafe void CollideAndIntegrate(PhysicsWorld world, float deltaTime,
int maxIterations, float3 up, float3 gravity,
float characterMass, float tau, float damping, float maxSlope, bool affectBodies, Collider *collider,
ref NativeArray <DistanceHit> distanceHits, ref NativeArray <ColliderCastHit> castHits, ref NativeArray <SurfaceConstraintInfo> constraints,
ref RigidTransform transform, ref float3 linearVelocity, ref BlockStream.Writer deferredImpulseWriter)
{
float remainingTime = deltaTime;
float3 lastDisplacement = linearVelocity * remainingTime;
float3 newPosition = transform.pos;
quaternion orientation = transform.rot;
float3 newVelocity = linearVelocity;
float maxSlopeCos = math.cos(maxSlope);
const float timeEpsilon = 0.000001f;
for (int i = 0; i < maxIterations && remainingTime > timeEpsilon; i++)
{
// First do distance query for penetration recovery
MaxHitsCollector <DistanceHit> distanceHitsCollector = new MaxHitsCollector <DistanceHit>(0.0f, ref distanceHits);
int numConstraints = 0;
{
ColliderDistanceInput input = new ColliderDistanceInput()
{
MaxDistance = 0.0f,
Transform = new RigidTransform
{
pos = newPosition,
rot = orientation,
},
Collider = collider
};
world.CalculateDistance(input, ref distanceHitsCollector);
// Iterate over hits and create constraints from them
for (int hitIndex = 0; hitIndex < distanceHitsCollector.NumHits; hitIndex++)
{
DistanceHit hit = distanceHitsCollector.AllHits[hitIndex];
CreateConstraintFromHit(world, gravity, deltaTime, hit.RigidBodyIndex, hit.ColliderKey, hit.Position,
hit.SurfaceNormal, hit.Distance, false, out SurfaceConstraintInfo constraint);
// Potentially add a max slope constraint
AddMaxSlopeConstraint(up, maxSlopeCos, ref constraint, ref constraints, ref numConstraints);
// Add original constraint to the list
constraints[numConstraints++] = constraint;
}
}
float3 gravityMovement = gravity * remainingTime * remainingTime * 0.5f;
// Then do a collider cast
{
float3 displacement = lastDisplacement + gravityMovement;
float3 endPosition = newPosition + displacement;
MaxHitsCollector <ColliderCastHit> collector = new MaxHitsCollector <ColliderCastHit>(1.0f, ref castHits);
ColliderCastInput input = new ColliderCastInput()
{
Collider = collider,
Orientation = orientation,
Position = newPosition,
Direction = displacement
};
world.CastCollider(input, ref collector);
// Iterate over hits and create constraints from them
for (int hitIndex = 0; hitIndex < collector.NumHits; hitIndex++)
{
ColliderCastHit hit = collector.AllHits[hitIndex];
bool found = false;
for (int distanceHitIndex = 0; distanceHitIndex < distanceHitsCollector.NumHits; distanceHitIndex++)
{
DistanceHit dHit = distanceHitsCollector.AllHits[distanceHitIndex];
if (dHit.RigidBodyIndex == hit.RigidBodyIndex &&
dHit.ColliderKey.Equals(hit.ColliderKey))
{
found = true;
break;
}
}
// Skip duplicate hits
if (!found)
{
CreateConstraintFromHit(world, gravity, deltaTime, hit.RigidBodyIndex, hit.ColliderKey, hit.Position, hit.SurfaceNormal,
hit.Fraction * math.length(lastDisplacement), false, out SurfaceConstraintInfo constraint);
// Potentially add a max slope constraint
AddMaxSlopeConstraint(up, maxSlopeCos, ref constraint, ref constraints, ref numConstraints);
// Add original constraint to the list
constraints[numConstraints++] = constraint;
}
}
}
// Solve
float3 prevVelocity = newVelocity;
float3 prevPosition = newPosition;
SimplexSolver.Solve(world, remainingTime, up, numConstraints, ref constraints, ref newPosition, ref newVelocity, out float integratedTime);
// Apply impulses to hit bodies
if (affectBodies)
{
ResolveContacts(world, remainingTime, gravity, tau, damping, characterMass, prevVelocity, numConstraints, ref constraints, ref deferredImpulseWriter);
}
float3 newDisplacement = newPosition - prevPosition;
// Check if we can walk to the position simplex solver has suggested
MaxHitsCollector <ColliderCastHit> newCollector = new MaxHitsCollector <ColliderCastHit>(1.0f, ref castHits);
int newContactIndex = -1;
// If simplex solver moved the character we need to re-cast to make sure it can move to new position
if (math.lengthsq(newDisplacement) > SimplexSolver.c_SimplexSolverEpsilon)
{
float3 displacement = newDisplacement + gravityMovement;
float3 endPosition = prevPosition + displacement;
ColliderCastInput input = new ColliderCastInput()
{
Collider = collider,
Orientation = orientation,
Position = prevPosition,
Direction = displacement
};
world.CastCollider(input, ref newCollector);
for (int hitIndex = 0; hitIndex < newCollector.NumHits; hitIndex++)
{
ColliderCastHit hit = newCollector.AllHits[hitIndex];
bool found = false;
for (int constraintIndex = 0; constraintIndex < numConstraints; constraintIndex++)
{
SurfaceConstraintInfo constraint = constraints[constraintIndex];
if (constraint.RigidBodyIndex == hit.RigidBodyIndex &&
constraint.ColliderKey.Equals(hit.ColliderKey))
{
found = true;
break;
}
}
if (!found)
{
newContactIndex = hitIndex;
break;
}
}
}
// Move character along the newDisplacement direction until it reaches this new contact
if (newContactIndex >= 0)
{
ColliderCastHit newContact = newCollector.AllHits[newContactIndex];
float fraction = newContact.Fraction / math.length(newDisplacement);
integratedTime *= fraction;
float3 displacement = newDisplacement * fraction;
newPosition = prevPosition + displacement;
}
remainingTime -= integratedTime;
// Remember last displacement for next iteration
lastDisplacement = newVelocity * remainingTime;
}
// Write back position and velocity
transform.pos = newPosition;
linearVelocity = newVelocity;
}
public static bool CapsuleCast <T>(ref T target, float3 point1, float3 point2, sfloat radius, float3 direction, sfloat maxDistance, out ColliderCastHit hitInfo, CollisionFilter filter, QueryInteraction queryInteraction = QueryInteraction.Default)
where T : struct, ICollidable
{
var collector = new ClosestHitCollector <ColliderCastHit>(sfloat.One);
bool hasHit = target.CapsuleCastCustom(point1, point2, radius, direction, maxDistance, ref collector, filter, queryInteraction);
hitInfo = collector.ClosestHit;
return(hasHit);
}
private static unsafe bool ConvexConvex(ColliderCastInput input, Collider *target, float maxFraction, out ColliderCastHit hit)
{
hit = default;
// 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 >= 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.Entity = input.QueryContext.Entity;
return(true);
}
// 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 >= maxFraction)
{
// Exceeded the maximum fraction without a hit
return(false);
}
targetFromQuery.Translation = math.lerp(input.Start, input.End, fraction);
}
}
protected unsafe override JobHandle OnUpdate(JobHandle inputDeps)
{
var entityCount = m_CharacterControllerGroup.CalculateEntityCount();
if (entityCount == 0)
{
return(inputDeps);
}
var defferredImpulses = new NativeStream(entityCount, Allocator.TempJob);
var time = m_TimeSingletonQuery.GetSingleton <GlobalGameTime>().gameTime;
var physicWorld = m_BuildPhysicsWorld.PhysicsWorld;
var writer = defferredImpulses.AsWriter();
var constraints = new NativeList <SurfaceConstraintInfo>(Allocator.Temp);
var castHits = new NativeList <ColliderCastHit>(Allocator.Temp);
var distanceHits = new NativeList <DistanceHit>(Allocator.Temp);
var input = new ColliderCastInput();
var hit = new ColliderCastHit();
var hasHit = false;
var deltaTime = Time.DeltaTime;
Entities
.WithName("CharacterControllerStepSystem")
.ForEach((
ref CharacterControllerComponentData ccData,
ref CharacterControllerCollider ccCollider,
ref CharacterControllerMoveQuery moveQuery,
ref CharacterControllerMoveResult moveResult,
ref CharacterControllerVelocity velocity
) => {
var collider = (Collider *)ccCollider.Collider.GetUnsafePtr();
var stepInput = new CharacterControllerUtilities.CharacterControllerStepInput {
World = physicWorld,
//DeltaTime = time.tickDuration,
DeltaTime = deltaTime,
Gravity = new float3(0.0f, -9.8f, 0.0f),
MaxIterations = ccData.MaxIterations,
Tau = CharacterControllerUtilities.k_DefaultTau,
Damping = CharacterControllerUtilities.k_DefaultDamping,
SkinWidth = ccData.SkinWidth,
ContactTolerance = ccData.ContactTolearance,
MaxSlope = ccData.MaxSlope,
RigidBodyIndex = -1,
CurrentVelocity = velocity.WorldVelocity,
MaxMovementSpeed = ccData.MaxMovementSpeed,
FollowGroud = moveQuery.FollowGroud
};
var transform = new RigidTransform {
pos = moveQuery.StartPosition,
rot = quaternion.identity
};
CharacterControllerUtilities.CollideAndIntegrate(
stepInput,
ccData.CharacterMass,
ccData.AffectsPhysicsBodies > 0,
collider,
ref transform,
ref velocity.WorldVelocity,
ref writer,
ref constraints,
ref castHits,
ref distanceHits,
out input,
out hit);
moveResult.MoveResult = transform.pos;
})
.Run();
var applyJob = new ApplyDefferedImpulses()
{
DeferredImpulseReader = defferredImpulses.AsReader(),
PhysicsVelocityData = GetComponentDataFromEntity <PhysicsVelocity>(),
PhysicsMassData = GetComponentDataFromEntity <PhysicsMass>(),
TranslationData = GetComponentDataFromEntity <Translation>(),
RotationData = GetComponentDataFromEntity <Rotation>()
};
applyJob.Run();
CharacterControllerDebug.input = input;
CharacterControllerDebug.hit = hit;
defferredImpulses.Dispose();
constraints.Dispose();
castHits.Dispose();
distanceHits.Dispose();
return(inputDeps);
}
