// I might not want to get the closest enemy, but a specific enemy.
// might use entity.id + entity.version of an entity and get position of entity
// if current target is destroyed, maybe seach for new target or destroy bullet
// add another option for auto mode
// start with auto and maybe can focus on specifics later
public void CheckForObstacle(int index)
{
CollisionFilter obstacleFilter = new CollisionFilter()
{
BelongsTo = ~0u,
CollidesWith = 16u + groupIndex,
GroupIndex = 0
};
PointDistanceInput obstacePointDistanceInput = new PointDistanceInput {
Position = cellSeparation[index],
MaxDistance = boidsData[index].obstacleAversionDistance,
Filter = obstacleFilter
};
if (physicsWorld.CalculateDistance(obstacePointDistanceInput, out DistanceHit obstaceHit))
{
cellObstaclePositions[index] = obstaceHit.Position;
cellObstacleDistance[index] = obstaceHit.Distance;
if (obstaceHit.Distance < settings.boidRadius)
{
Entity targetEntity = physicsWorld.CollisionWorld.Bodies[obstaceHit.RigidBodyIndex].Entity;
var hash = (int)math.hash(new int2(targetEntity.Index, targetEntity.Version));
damageDict.Add(hash, 1);
killTrigger[index] = 1;
}
}
else
{
cellObstacleDistance[index] = boidsData[index].obstacleAversionDistance + 1;
}
}
public void Execute()
{
if (CollectAllHits)
{
World.CalculateDistance(PointDistanceInput, ref DistanceHits);
}
else if (World.CalculateDistance(PointDistanceInput, out DistanceHit hit))
{
DistanceHits.Add(hit);
}
}
protected override JobHandle OnUpdate(JobHandle inputDeps)
{
PhysicsWorld physicsWorld = buildPhysicsWorld.PhysicsWorld;
inputDeps = JobHandle.CombineDependencies(inputDeps, buildPhysicsWorld.FinalJobHandle);
inputDeps = JobHandle.CombineDependencies(inputDeps, stepPhysicsWorld.FinalJobHandle);
var jobHandle = Entities.ForEach((ref DynamicBuffer <EntityBufferElement> adjacentEntities,
in Translation position, in BugComponent bug) =>
{
NativeList <DistanceHit> distanceHits = new NativeList <DistanceHit>(Allocator.Temp);
var pointDistanceInput = new PointDistanceInput
{
Position = position.Value,
MaxDistance = bug.Radius,
Filter = LayerFilter(Layer.Bug, Layer.Obstacle)
};
physicsWorld.CalculateDistance(pointDistanceInput, ref distanceHits);
adjacentEntities.Clear();
var adjacent = adjacentEntities.Reinterpret <Entity>();
for (int i = 0; i < distanceHits.Length; i++)
{
adjacent.Add(distanceHits[i].Entity);
}
distanceHits.Dispose();
})
public void Execute()
{
var colliderDistanceInput = new ColliderDistanceInput
{
Collider = Collider,
Transform = Transform,
MaxDistance = MaxDistance
};
if (CollectAllHits)
{
World.CalculateDistance(colliderDistanceInput, ref DistanceHits);
}
else if (World.CalculateDistance(colliderDistanceInput, out DistanceHit hit))
{
DistanceHits.Add(hit);
}
}
public void Execute()
{
var pointDistanceInput = new PointDistanceInput
{
Position = transData.Value,
MaxDistance = laserData.range,
Filter = CollisionFilter.Default
};
if (collectAll)
{
physWorld.CalculateDistance(pointDistanceInput, ref distanceHits);
}
else
{
if (physWorld.CalculateDistance(pointDistanceInput, out Unity.Physics.DistanceHit hit))
{
distanceHits.Add(hit);
}
}
}
public void Execute()
{
for (int i = 0; i < DamageEntities.Length; i++)
{
MaxHitsCollector <DistanceHit> collector = new MaxHitsCollector <DistanceHit>(10.0f, ref DistanceHits);
if (!DamageAreas[i].WasUsed)
{
CollisionFilter filter = CollisionFilter.Default;
filter.CollidesWith = DamageAreas[i].CollisionFilter;
pointDistanceInput.Position = Translations[i].Value;
pointDistanceInput.Filter = filter;
PhysicsWorld.CalculateDistance(pointDistanceInput, ref collector);
for (int j = 0; j < collector.NumHits; j++)
{
Entity hitEntity = PhysicsWorld.Bodies[DistanceHits[j].RigidBodyIndex].Entity;
if (DistanceHits[j].Fraction <= DamageAreas[i].Radius)
{
// deal damage
if (HealthsFromEntity.Exists(hitEntity))
{
Health h = HealthsFromEntity[hitEntity];
h.Value -= DamageAreas[i].Damage;
HealthsFromEntity[hitEntity] = h;
}
}
}
Entity damageEntity = DamageEntities[i];
if (DamageAreasFromEntity.Exists(damageEntity))
{
DamageArea d = DamageAreasFromEntity[damageEntity];
if (d.SingleUse)
{
d.WasUsed = true;
DamageAreasFromEntity[damageEntity] = d;
}
}
}
}
}
protected override void OnUpdate()
{
Entities.ForEach((ref Proximity proximity, ref Translation translation, ref PhysicsCollider collider) =>
{
ref PhysicsWorld physicsWorld = ref World.DefaultGameObjectInjectionWorld.GetExistingSystem <BuildPhysicsWorld>().PhysicsWorld;
if (collider.Value.IsCreated)
{
var pointDistanceInput = new PointDistanceInput
{
Position = translation.Value,
MaxDistance = proximity.maxDistance,
Filter = collider.Value.Value.Filter
};
// Assign DistanceHit data to proximiy component
physicsWorld.CalculateDistance(pointDistanceInput, out proximity.distanceHit);
}
}).WithoutBurst().Run();
public void Execute(ref Translation translation)
{
NativeList <DistanceHit> hits = new NativeList <DistanceHit>(Allocator.Temp);
CollisionFilter targetFilter = new CollisionFilter()
{
BelongsTo = ~0u,
CollidesWith = 8u,
GroupIndex = 0
};
var pointDistanceInput = new PointDistanceInput {
Position = translation.Value,
MaxDistance = 50,
Filter = targetFilter
};
physicsWorld.CalculateDistance(pointDistanceInput, ref hits);
hits.Dispose();
}
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 void Execute(ArchetypeChunk chunk, int chunkIndex, int firstEntityIndex)
{
NativeArray <CharacterController> chunkCharacterControllers = chunk.GetNativeArray(CharacterControllerType);
NativeArray <PhysicsCollider> chunkPhysicsColliders = chunk.GetNativeArray(PhysicsColliderType);
NativeArray <Translation> chunkTranslations = chunk.GetNativeArray(TranslationType);
NativeArray <Rotation> chunkRotations = chunk.GetNativeArray(RotationType);
for (int i = 0; i < chunk.Count; i++)
{
CharacterController controller = chunkCharacterControllers[i];
PhysicsCollider collider = chunkPhysicsColliders[i];
Translation translation = chunkTranslations[i];
Rotation rotation = chunkRotations[i];
RigidTransform transform = new RigidTransform
{
pos = translation.Value,
rot = rotation.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.MaxHitCollector <DistanceHit> distanceHitCollector = new KinematicMotorUtilities.MaxHitCollector <DistanceHit>(controller.GroundTollerance, ref DistanceHits);
{
ColliderDistanceInput input = new ColliderDistanceInput
{
MaxDistance = controller.GroundTollerance,
Transform = transform,
Collider = queryCollider
};
World.CalculateDistance(input, ref distanceHitCollector);
}
for (int hitIndex = 0; hitIndex < distanceHitCollector.NumHits; hitIndex++)
{
DistanceHit hit = distanceHitCollector.AllHits[hitIndex];
KinematicMotorUtilities.CreateConstraintFromHit(World, hit.ColliderKey, hit.RigidBodyIndex, hit.Position, float3.zero, hit.SurfaceNormal, hit.Distance, DeltaTime, out SurfaceConstraintInfo constraint);
SurfaceConstraintInfos[hitIndex] = constraint;
}
float3 outPosition = transform.pos;
float3 outVelocity = -math.up();
SimplexSolver.Solve(World, DeltaTime, math.up(), distanceHitCollector.NumHits, ref SurfaceConstraintInfos, ref outPosition, ref outVelocity, out float integratedTime);
if (distanceHitCollector.NumHits == 0)
{
controller.State = CharacterControllerState.NONE;
}
else
{
outVelocity = math.normalize(outVelocity);
float slopeAngleSin = math.dot(outVelocity, -math.up());
float slopeAngleCosSq = 1 - slopeAngleSin * slopeAngleSin;
float maxSlopeCos = math.cos(controller.MaxSlope);
controller.State = CharacterControllerState.GROUNDED;
}
}
// Apply data back to chunk
{
chunkCharacterControllers[i] = controller;
}
}
}
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 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;
}
protected override void OnUpdate()
{
EntityQuery eq = GetEntityQuery(typeof(Boid_ComponentData));
cellVsEntityPositions.Clear();
if (eq.CalculateEntityCount() > cellVsEntityPositions.Capacity)
{
cellVsEntityPositions.Capacity = eq.CalculateEntityCount();
}
unsafe
{
PhysicsWorld pw = bpw.PhysicsWorld;
Entities.ForEach((ref Boid_ComponentData bc, ref Translation trans, ref Rotation rot) =>
{
bc.obstacleAvoidance = float3.zero;
PhysicsWorld pWorld = pw;
float angle;
ColliderDistanceInput colliderDistanceInput = new ColliderDistanceInput
{
Collider = (Unity.Physics.Collider *)(bc.colliderCast.GetUnsafePtr()),
Transform = new RigidTransform(rot.Value, trans.Value),
MaxDistance = bc.boidManagerReference.Value.blobManagerArray[0].maxObstacleDistance
};
if (pWorld.CalculateDistance(colliderDistanceInput, out DistanceHit hit))
{
angle = math.acos(
math.dot(bc.velocity, (hit.Position - trans.Value))
/
(math.length(bc.velocity) * math.length(hit.Position - trans.Value))
);
if (math.abs(angle) <= bc.boidManagerReference.Value.blobManagerArray[0].fieldOfView)
{
bc.obstacleAvoidance = (trans.Value - hit.Position) / (math.distance(trans.Value, hit.Position));
bc.obstacleAvoidance = (math.length(bc.velocity) *
(angle / bc.boidManagerReference.Value.blobManagerArray[0].fieldOfView)) *
bc.obstacleAvoidance;
}
}
}).ScheduleParallel();
}
NativeMultiHashMap <int, Boid_ComponentData> .ParallelWriter cellVsEntityPositionsParallel = cellVsEntityPositions.AsParallelWriter();
Entities.ForEach((ref Boid_ComponentData bc, ref Translation trans) =>
{
Boid_ComponentData bcValues = new Boid_ComponentData();
bcValues = bc;
bcValues.currentPosition = trans.Value;
cellVsEntityPositionsParallel.Add(GetUniqueKeyForPosition(trans.Value, bc.boidManagerReference.Value.blobManagerArray[0].cellSize), bcValues);
}).ScheduleParallel();
float deltaTime = Time.DeltaTime;
NativeMultiHashMap <int, Boid_ComponentData> cellVsEntityPositionsForJob = cellVsEntityPositions;
Entities.WithBurst().WithReadOnly(cellVsEntityPositionsForJob).ForEach((ref Boid_ComponentData bc, ref Translation trans, ref Rotation rot) =>
{
int key = GetUniqueKeyForPosition(trans.Value, bc.boidManagerReference.Value.blobManagerArray[0].cellSize);
NativeMultiHashMapIterator <int> nmhKeyIterator;
Boid_ComponentData neighbour;
int total = 0;
float3 separation = float3.zero;
float3 alignment = float3.zero;
float3 coheshion = float3.zero;
float angle;
if (cellVsEntityPositionsForJob.TryGetFirstValue(key, out neighbour, out nmhKeyIterator))
{
do
{
if (!trans.Value.Equals(neighbour.currentPosition) &&
math.distance(trans.Value, neighbour.currentPosition) < bc.boidManagerReference.Value.blobManagerArray[0].perceptionRadius)
{
angle = math.acos(
math.dot(bc.velocity, (neighbour.currentPosition - trans.Value))
/
(math.length(bc.velocity) * math.length(neighbour.currentPosition - trans.Value))
);
if (math.abs(angle) <= bc.boidManagerReference.Value.blobManagerArray[0].fieldOfView)
{
if (total >= bc.boidManagerReference.Value.blobManagerArray[0].maxPercived)
{
break;
}
float3 distanceFromTo = trans.Value - neighbour.currentPosition;
separation += (distanceFromTo / math.distance(trans.Value, neighbour.currentPosition));
coheshion += neighbour.currentPosition;
alignment += neighbour.velocity;
total++;
bc.debug = angle;
}
}
} while (cellVsEntityPositionsForJob.TryGetNextValue(out neighbour, ref nmhKeyIterator));
if (total > 0)
{
coheshion = coheshion / total;
coheshion = coheshion - (trans.Value + bc.velocity);
coheshion = math.normalize(coheshion) * bc.boidManagerReference.Value.blobManagerArray[0].cohesionBias;
separation = separation / total;
separation = separation - bc.velocity;
separation = math.normalize(separation) * bc.boidManagerReference.Value.blobManagerArray[0].separationBias;
alignment = alignment / total;
alignment = alignment - bc.velocity;
alignment = math.normalize(alignment) * bc.boidManagerReference.Value.blobManagerArray[0].alignmentBias;
}
bc.acceleration += (coheshion + alignment + separation) + bc.obstacleAvoidance;
rot.Value = math.slerp(rot.Value, quaternion.LookRotation(math.normalize(bc.velocity), math.up()), deltaTime * math.length(bc.velocity));
bc.velocity = bc.velocity + bc.acceleration;
bc.velocity = math.normalize(bc.velocity) * bc.speed;
trans.Value = math.lerp(trans.Value, (trans.Value + bc.velocity), deltaTime * bc.boidManagerReference.Value.blobManagerArray[0].step);
bc.acceleration = math.normalize(bc.target - trans.Value) * bc.boidManagerReference.Value.blobManagerArray[0].targetBias;
}
}).ScheduleParallel();
}
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 unsafe void Execute(ArchetypeChunk chunk, int chunkIndex, int firstEntityIndex)
{
float3 up = math.up();
var chunkCCData = chunk.GetNativeArray(CharacterControllerComponentType);
var chunkCCInternalData = chunk.GetNativeArray(CharacterControllerInternalType);
var chunkPhysicsColliderData = chunk.GetNativeArray(PhysicsColliderType);
var chunkTranslationData = chunk.GetNativeArray(TranslationType);
var chunkRotationData = chunk.GetNativeArray(RotationType);
DeferredImpulseWriter.BeginForEachIndex(chunkIndex);
// Maximum number of hits character controller can store in world queries
const int maxQueryHits = 128;
var distanceHits = new NativeArray <DistanceHit>(maxQueryHits, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
var castHits = new NativeArray <ColliderCastHit>(maxQueryHits, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
var constraints = new NativeArray <SurfaceConstraintInfo>(4 * maxQueryHits, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
for (int i = 0; i < chunk.Count; i++)
{
var ccComponentData = chunkCCData[i];
var ccInternalData = chunkCCInternalData[i];
var collider = chunkPhysicsColliderData[i];
var position = chunkTranslationData[i];
var rotation = chunkRotationData[i];
// Collision filter must be valid
Assert.IsTrue(collider.ColliderPtr->Filter.IsValid);
// Character step input
CharacterControllerStepInput stepInput = new CharacterControllerStepInput
{
World = PhysicsWorld,
DeltaTime = DeltaTime,
Up = math.up(),
Gravity = ccComponentData.Gravity,
MaxIterations = ccComponentData.MaxIterations,
Tau = k_DefaultTau,
Damping = k_DefaultDamping,
SkinWidth = ccComponentData.SkinWidth,
ContactTolerance = ccComponentData.ContactTolerance,
MaxSlope = ccComponentData.MaxSlope,
RigidBodyIndex = PhysicsWorld.GetRigidBodyIndex(ccInternalData.Entity),
CurrentVelocity = ccInternalData.LinearVelocity
};
// Character transform
RigidTransform transform = new RigidTransform
{
pos = position.Value,
rot = rotation.Value
};
// "Broad phase" (used both for checking support and actual character collide and integrate).
MaxHitsCollector <DistanceHit> distanceHitsCollector = new MaxHitsCollector <DistanceHit>(
stepInput.RigidBodyIndex, ccComponentData.ContactTolerance, ref distanceHits);
{
ColliderDistanceInput input = new ColliderDistanceInput()
{
MaxDistance = ccComponentData.ContactTolerance,
Transform = transform,
Collider = collider.ColliderPtr
};
PhysicsWorld.CalculateDistance(input, ref distanceHitsCollector);
}
// Check support
CheckSupport(stepInput, transform, ccComponentData.MaxSlope, distanceHitsCollector,
ref constraints, out int numConstraints, out ccInternalData.SupportedState, out float3 surfaceNormal, out float3 surfaceVelocity);
// User input
float3 desiredVelocity = ccInternalData.LinearVelocity;
HandleUserInput(ccComponentData, stepInput.Up, surfaceVelocity, ref ccInternalData, ref desiredVelocity);
// Calculate actual velocity with respect to surface
if (ccInternalData.SupportedState == CharacterSupportState.Supported)
{
CalculateMovement(ccInternalData.CurrentRotationAngle, stepInput.Up, ccInternalData.IsJumping,
ccInternalData.LinearVelocity, desiredVelocity, surfaceNormal, surfaceVelocity, out ccInternalData.LinearVelocity);
}
else
{
ccInternalData.LinearVelocity = desiredVelocity;
}
// World collision + integrate
CollideAndIntegrate(stepInput, ccComponentData.CharacterMass, ccComponentData.AffectsPhysicsBodies > 0,
collider.ColliderPtr, ref castHits, ref constraints, numConstraints,
ref transform, ref ccInternalData.LinearVelocity, ref DeferredImpulseWriter);
// Write back and orientation integration
position.Value = transform.pos;
rotation.Value = quaternion.AxisAngle(up, ccInternalData.CurrentRotationAngle);
// Write back to chunk data
{
chunkCCInternalData[i] = ccInternalData;
chunkTranslationData[i] = position;
chunkRotationData[i] = rotation;
}
}
DeferredImpulseWriter.EndForEachIndex();
}
protected override void OnUpdate()
{
var ecb = es_ecb.CreateCommandBuffer();
var parallelECB = ecb.ToConcurrent();
PhysicsWorld pw = bpw.PhysicsWorld;
float deltaTime = Time.DeltaTime;
float3 directionToShoot = float3.zero;
unsafe
{
Entities.ForEach((ref ShooterComponentData scd, ref Translation trans, ref Rotation rot, in LocalToWorld ltw) =>
{
scd.elapsedTime += deltaTime;
ColliderDistanceInput colliderDistanceInput = new ColliderDistanceInput
{
Collider = (Unity.Physics.Collider *)(scd.colliderCast.GetUnsafePtr()),
Transform = new RigidTransform(rot.Value, trans.Value),
MaxDistance = 0.25f
};
if (pw.CalculateDistance(colliderDistanceInput, out DistanceHit hit))
{
directionToShoot = math.normalize(hit.Position - trans.Value);
rot.Value = math.slerp(rot.Value, quaternion.LookRotation(directionToShoot, math.up()), deltaTime * 25);
scd.projectileSpawnPosition = math.transform(ltw.Value, new float3(0, 0.3f, 0.7f));
if (scd.elapsedTime >= scd.firingInterval)
{
scd.elapsedTime = 0;
Entity projectile = ecb.Instantiate(scd.projectile);
ecb.SetComponent(projectile, new Translation {
Value = scd.projectileSpawnPosition
});
ecb.SetComponent(projectile, new Rotation {
Value = quaternion.LookRotation(ltw.Forward, math.up())
});
ecb.AddComponent <ProjectileFired>(projectile);
ecb.SetComponent(projectile, new ProjectileFired
{
elapsedTime = 0,
projectileSpeed = scd.projectileSpeed,
projectileLifeTime = scd.projectileLifeTime
});
}
}
}).Run();
}
Entities.ForEach((Entity e, int entityInQueryIndex, ref ProjectileFired pf, ref Translation trans, in LocalToWorld ltw) =>
{
pf.elapsedTime += deltaTime;
trans.Value += ltw.Forward * pf.projectileSpeed * deltaTime;
if (pf.elapsedTime > pf.projectileLifeTime)
{
parallelECB.DestroyEntity(entityInQueryIndex, e);
}
}).ScheduleParallel();
es_ecb.AddJobHandleForProducer(Dependency);
JobHandle jh = new ProjectileCollisionJob()
{
enemyGroup = GetComponentDataFromEntity <EnemyComponentData>(),
activeProjectileGroup = GetComponentDataFromEntity <ProjectileFired>(),
ecb = es_ecb_Job.CreateCommandBuffer()
}.Schedule(spw.Simulation, ref bpw.PhysicsWorld, Dependency);
es_ecb_Job.AddJobHandleForProducer(jh);
}
protected override void OnUpdate()
{
PhysicsWorld pw = bpw.PhysicsWorld;
float deltaTime = Time.DeltaTime;
var parallelECB = es_ecb.CreateCommandBuffer().ToConcurrent();
unsafe
{
Entities.ForEach((Entity e, int entityInQueryIndex, ref RangedWeaponParentData rwpd, ref Translation trans, ref Rotation rot, in LocalToWorld ltw) =>
{
rwpd.elapsedTime += deltaTime;
float3 directionToShoot = float3.zero;
ColliderDistanceInput colliderDistanceInput = new ColliderDistanceInput
{
Collider = (Unity.Physics.Collider *)(rwpd.colliderCast.GetUnsafePtr()),
Transform = new RigidTransform(rot.Value, trans.Value),
MaxDistance = 0.25f
};
if (pw.CalculateDistance(colliderDistanceInput, out DistanceHit hit))
{
rwpd.currentTargetDistance = math.length(hit.Position - trans.Value);
directionToShoot = math.normalize(hit.Position - trans.Value);
rot.Value = math.slerp(rot.Value, quaternion.LookRotation(directionToShoot, math.up()), deltaTime * 5);
if (rwpd.currentTargetDistance >= 1)
{
float height = rwpd.currentTargetDistance / 4;
float denom = math.sqrt((2 * height) / 9.8f);
rwpd.initialVelocity = new float3(0,
math.sqrt(2 * 9.8f * height),
rwpd.currentTargetDistance / (2 * denom));
rwpd.initialVelocity = math.mul(quaternion.LookRotation(directionToShoot, math.up()), rwpd.initialVelocity);
if (rwpd.elapsedTime >= rwpd.firingInterval)
{
rwpd.elapsedTime = 0;
Entity defEntity = parallelECB.Instantiate(entityInQueryIndex, rwpd.cannonBall);
float3 spawnPosition = math.transform(ltw.Value, new float3(0, 5.5f, 0));
parallelECB.SetComponent <Translation>(entityInQueryIndex, defEntity, new Translation {
Value = spawnPosition
});
parallelECB.AddComponent <CannonBallTag>(entityInQueryIndex, defEntity);
parallelECB.SetComponent <CannonBallTag>(entityInQueryIndex, defEntity, new CannonBallTag
{
initialVelocity = rwpd.initialVelocity,
cannonBallColliderCast = rwpd.cannonBallColliderCast
});
}
}
}
}).ScheduleParallel();
}
Entities.ForEach((ref CannonComponentData ccd, ref Rotation rot, ref Translation trans, in LocalToWorld ltw, in Parent p) =>
{
if (HasComponent <RangedWeaponParentData>(p.Value))
{
RangedWeaponParentData rwpd = GetComponent <RangedWeaponParentData>(p.Value);
if (rwpd.currentTargetDistance >= 1)
{
float3 localUpDirection = math.transform(math.inverse(ltw.Value), ltw.Up);
float angle = math.acos(math.dot(localUpDirection, rwpd.initialVelocity) / (math.length(localUpDirection) * (math.length(rwpd.initialVelocity))));
rot.Value = math.slerp(rot.Value, quaternion.Euler(angle, 0, 0), deltaTime * 5);
}
}
}).ScheduleParallel();
public void Execute(ArchetypeChunk batchInChunk, int batchIndex)
{
NativeArray <Translation> translations = batchInChunk.
GetNativeArray(translationTypeHandle);
NativeArray <MoveData> moveDatas = batchInChunk.
GetNativeArray(moveDataTypeHandle);
NativeArray <SensorData> sensorDatas = batchInChunk.
GetNativeArray(sensorDataTypeHandle);
for (int i = 0; i < batchInChunk.Count; i++)
{
int left = 0;
int right = 0;
int center = 0;
float3 leftPosition = GetDeltaVector(
sensorDatas[i].leftSensorDistance,
sensorDatas[i].leftSensorAngle);
float3 centerPosition = GetDeltaVector(
sensorDatas[i].centralSensorDistance,
sensorDatas[i].centralSensorAngle);
float3 rightPosition = GetDeltaVector(
sensorDatas[i].rightSensorDistance,
sensorDatas[i].rightSensorAngle);
NativeList <DistanceHit> distanceHits =
new NativeList <DistanceHit>(Allocator.Temp);
if (physicsWorld.CalculateDistance(new PointDistanceInput()
{
Filter = CollisionFilter.Default,
MaxDistance = sensorDatas[i].centralSensorSize,
Position = translations[i].Value + centerPosition
}, ref distanceHits))
{
center = distanceHits.Length;
}
if (physicsWorld.CalculateDistance(new PointDistanceInput()
{
Filter = CollisionFilter.Default,
MaxDistance = sensorDatas[i].leftSensorSize,
Position = translations[i].Value + leftPosition
}, ref distanceHits))
{
left = distanceHits.Length;
}
if (physicsWorld.CalculateDistance(new PointDistanceInput()
{
Filter = CollisionFilter.Default,
MaxDistance = sensorDatas[i].rightSensorSize,
Position = translations[i].Value + rightPosition
}, ref distanceHits))
{
right = distanceHits.Length;
}
distanceHits.Dispose();
float angle = DeltaTime * moveDatas[i].rotSpeed;
//Debug.Log($"Hits are {left}, {center}, {right}");
if (right > center && right > left)
{
//Debug.Log("Turn right ");
moveDatas[i] = new MoveData()
{
speed = moveDatas[i].speed,
rotSpeed = moveDatas[i].rotSpeed,
angle = moveDatas[i].angle - angle
};
}
else if (left > right && left > center)
{
//Debug.Log("Turn left");
moveDatas[i] = new MoveData()
{
speed = moveDatas[i].speed,
rotSpeed = moveDatas[i].rotSpeed,
angle = moveDatas[i].angle + angle
};
}
else if (left == right && left > center)
{
if (UnityEngine.Random.value > 0.5)
{
moveDatas[i] = new MoveData()
{
speed = moveDatas[i].speed,
rotSpeed = moveDatas[i].rotSpeed,
angle = moveDatas[i].angle - angle
};
}
else
{
moveDatas[i] = new MoveData()
{
speed = moveDatas[i].speed,
rotSpeed = moveDatas[i].rotSpeed,
angle = moveDatas[i].angle + angle
};
}
}
}
}
public static unsafe void CheckSupport(PhysicsWorld world, float deltaTime, RigidTransform transform,
float3 downwardsDirection, float maxSlope, float contactTolerance, Collider *collider, ref NativeArray <SurfaceConstraintInfo> constraints,
ref NativeArray <DistanceHit> checkSupportHits, out CharacterSupportState characterState)
{
// Downwards direction must be normalized
Assert.IsTrue(Math.IsNormalized(downwardsDirection));
// "Broad phase"
MaxHitsCollector <DistanceHit> collector = new MaxHitsCollector <DistanceHit>(contactTolerance, ref checkSupportHits);
{
ColliderDistanceInput input = new ColliderDistanceInput()
{
MaxDistance = contactTolerance,
Transform = transform,
Collider = collider
};
world.CalculateDistance(input, ref collector);
}
// Iterate over hits and create constraints from them
for (int i = 0; i < collector.NumHits; i++)
{
DistanceHit hit = collector.AllHits[i];
CreateConstraintFromHit(world, float3.zero, deltaTime, hit.RigidBodyIndex, hit.ColliderKey,
hit.Position, hit.SurfaceNormal, hit.Distance, true, out SurfaceConstraintInfo constraint);
constraints[i] = constraint;
}
// Solve downwards
float3 outVelocity = downwardsDirection;
float3 outPosition = transform.pos;
SimplexSolver.Solve(world, deltaTime, -downwardsDirection, collector.NumHits, ref constraints, ref outPosition, ref outVelocity, out float integratedTime);
// If no hits, proclaim unsupported state
if (collector.NumHits == 0)
{
characterState = CharacterSupportState.Unsupported;
}
else
{
if (math.lengthsq(downwardsDirection - outVelocity) < SimplexSolver.c_SimplexSolverEpsilon)
{
// If velocity hasn't changed significantly, declare unsupported state
characterState = CharacterSupportState.Unsupported;
}
else if (math.lengthsq(outVelocity) < SimplexSolver.c_SimplexSolverEpsilon)
{
// If velocity is very small, declare supported state
characterState = CharacterSupportState.Supported;
}
else
{
// Check if sliding or supported
outVelocity = math.normalize(outVelocity);
float slopeAngleSin = math.dot(outVelocity, downwardsDirection);
float slopeAngleCosSq = 1 - slopeAngleSin * slopeAngleSin;
float maxSlopeCosine = math.cos(maxSlope);
if (slopeAngleCosSq < maxSlopeCosine * maxSlopeCosine)
{
characterState = CharacterSupportState.Sliding;
}
else
{
characterState = CharacterSupportState.Supported;
}
}
}
}
public static unsafe void CheckSupport(
ref PhysicsWorld world, ref PhysicsCollider collider, CharacterControllerStepInput stepInput, RigidTransform transform, float maxSlope,
out CharacterSupportState characterState, out float3 surfaceNormal, out float3 surfaceVelocity)
{
surfaceNormal = float3.zero;
surfaceVelocity = float3.zero;
// Query the world
NativeList <DistanceHit> distanceHits = new NativeList <DistanceHit>(k_DefaultQueryHitsCapacity, Allocator.Temp);
SelfFilteringAllHitsCollector <DistanceHit> distanceHitsCollector = new SelfFilteringAllHitsCollector <DistanceHit>(
stepInput.RigidBodyIndex, stepInput.ContactTolerance, ref distanceHits);
{
ColliderDistanceInput input = new ColliderDistanceInput()
{
MaxDistance = stepInput.ContactTolerance,
Transform = transform,
Collider = collider.ColliderPtr
};
world.CalculateDistance(input, ref distanceHitsCollector);
}
// If no hits, proclaim unsupported state
if (distanceHitsCollector.NumHits == 0)
{
characterState = CharacterSupportState.Unsupported;
return;
}
// Downwards direction must be normalized
float3 downwardsDirection = -stepInput.Up;
Assert.IsTrue(Math.IsNormalized(downwardsDirection));
float maxSlopeCos = math.cos(maxSlope);
// Iterate over distance hits and create constraints from them
NativeList <SurfaceConstraintInfo> constraints = new NativeList <SurfaceConstraintInfo>(k_DefaultConstraintsCapacity, Allocator.Temp);
for (int i = 0; i < distanceHitsCollector.NumHits; i++)
{
DistanceHit hit = distanceHitsCollector.AllHits[i];
if (ColliderUtils.IsTrigger(world.Bodies[hit.RigidBodyIndex].Collider, hit.ColliderKey))
{
continue;
}
CreateConstraint(stepInput.World, stepInput.Up,
hit.RigidBodyIndex, hit.ColliderKey, hit.Position, hit.SurfaceNormal, hit.Distance,
stepInput.SkinWidth, maxSlopeCos, ref constraints);
}
float3 initialVelocity;
{
float velAlongDownwardsDir = math.dot(stepInput.CurrentVelocity, downwardsDirection);
bool velocityIsAlongDownwardsDirection = velAlongDownwardsDir > 0.0f;
if (velocityIsAlongDownwardsDirection)
{
float3 downwardsVelocity = velAlongDownwardsDir * downwardsDirection;
initialVelocity =
math.select(downwardsVelocity, downwardsDirection, math.abs(velAlongDownwardsDir) > 1.0f) +
stepInput.Gravity * stepInput.DeltaTime;
}
else
{
initialVelocity = downwardsDirection;
}
}
// Solve downwards (don't use min delta time, try to solve full step)
float3 outVelocity = initialVelocity;
float3 outPosition = transform.pos;
SimplexSolver.Solve(stepInput.World, 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)
{
numSupportingPlanes++;
surfaceNormal += constraint.Plane.Normal;
surfaceVelocity += constraint.Velocity;
}
}
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)
{
// If velocity is very small, declare supported state
characterState = CharacterSupportState.Supported;
}
else
{
// Check if sliding or supported
outVelocity = math.normalize(outVelocity);
float slopeAngleSin = math.max(0.0f, math.dot(outVelocity, downwardsDirection));
float slopeAngleCosSq = 1 - slopeAngleSin * slopeAngleSin;
if (slopeAngleCosSq < maxSlopeCos * maxSlopeCos)
{
characterState = CharacterSupportState.Sliding;
}
else
{
characterState = CharacterSupportState.Supported;
}
}
}
}
