private void RunQueries()
{
DistanceHits.Clear();
pWorld = World.DefaultGameObjectInjectionWorld.GetExistingSystem <BuildPhysicsWorld>().PhysicsWorld;
unsafe
{
float3 origin = transform.position;
ColliderDistanceInput = new ColliderDistanceInput
{
Collider = (Unity.Physics.Collider *)(col.GetUnsafePtr()),
Transform = new RigidTransform(transform.rotation, origin),
MaxDistance = 10.0f
};
new ColliderDistanceJob
{
Transform = ColliderDistanceInput.Transform,
MaxDistance = ColliderDistanceInput.MaxDistance,
Collider = ColliderDistanceInput.Collider,
DistanceHits = DistanceHits,
CollectAllHits = false,
World = pWorld,
}.Schedule().Complete();
}
Debug.Log("DistanceHits>> " + DistanceHits.Length);
}
public void Execute(Entity e, int index, ref PhysicsCollider collider, ref Translation translation, ref Rotation rotation)
{
ColliderDistanceInput distanceInput = new ColliderDistanceInput
{
Collider = collider.ColliderPtr,
MaxDistance = .01f,
Transform = new RigidTransform(rotation.Value, translation.Value),
};
NativeList <DistanceHit> hits = new NativeList <DistanceHit>(Allocator.Temp);
var collisionWorld = physicsWorld.CollisionWorld;
if (collisionWorld.CalculateDistance(distanceInput, ref hits))
{
foreach (var hit in hits)
{
var entity = collisionWorld.Bodies[hit.RigidBodyIndex].Entity;
if (entity != e)
{
if (consumedObjectIndex.HasComponent(entity))
{
var consumedEntity = commandBuffer.CreateEntity(index);
var consumedObject = consumedObjectIndex[entity];
commandBuffer.AddComponent(index, consumedEntity, consumedObject);
commandBuffer.AddComponent(index, consumedEntity, new ConsumedTag());
commandBuffer.DestroyEntity(index, entity);
}
}
}
}
hits.Dispose();
}
private JobHandle UpdateWithCollider2ColliderQuery(JobHandle inputDeps)
{
var physicsWorldSystem = World.DefaultGameObjectInjectionWorld.GetExistingSystem <BuildPhysicsWorld>();
var collisionWorld = physicsWorldSystem.PhysicsWorld.CollisionWorld;
// this is probably redundant but just for safety lets include all previous physics systems
inputDeps = JobHandle.CombineDependencies(inputDeps, finalPhysicsSystem.FinalJobHandle);
inputDeps = JobHandle.CombineDependencies(inputDeps, buildPhysicsSystem.FinalJobHandle);
inputDeps = JobHandle.CombineDependencies(inputDeps, stepPhysicsSystem.FinalJobHandle);
inputDeps = Entities.ForEach((Entity entity, ref EntitiesAroundCountCmp around, in Translation translation, in PhysicsCollider collider) =>
{
around.count = 0;
float3 offset = new float3(around.range, 1, around.range);
OverlapAabbInput input = new OverlapAabbInput()
{
Aabb = new Aabb()
{
Min = translation.Value - offset,
Max = translation.Value + offset,
},
Filter = CollisionFilter.Default
};
NativeList <int> bodyIndices = new NativeList <int>(Allocator.Temp);
// OverlapAabb is really nice and fast, all expected colliders are returned
if (collisionWorld.OverlapAabb(input, ref bodyIndices))
{
for (int i = 0; i < bodyIndices.Length; ++i)
{
var body = collisionWorld.Bodies[bodyIndices[i]];
// why this returns true for colliders in AABB instead of actual distance?
var colliderDistanceInput = new ColliderDistanceInput()
{
Collider = collider.ColliderPtr,
Transform = RigidTransform.identity,
MaxDistance = around.range
};
// why this always returns false?
var pointDistanceInput = new PointDistanceInput()
{
Filter = CollisionFilter.Default,
MaxDistance = around.range,
Position = translation.Value
};
if (body.CalculateDistance(pointDistanceInput))
{
++around.count;
}
}
}
bodyIndices.Dispose();
})
public unsafe static void ColliderRange(CollisionWorld world, float radius, Collider coll, RigidTransform trans, ref NativeList <DistanceHit> hits)
{
ColliderDistanceInput input = new ColliderDistanceInput()
{
MaxDistance = radius,
Collider = &coll,
Transform = trans
};
world.CalculateDistance(input, ref hits);
}
protected override unsafe void OnUpdate()
{
var physicsWorld = m_BuildPhysicsWorldSystem.PhysicsWorld;
Dependency = Entities.WithAll <ServerEntity>()
.ForEach((Entity entity,
ref FlyingPredictedState flyingState,
in TransformPredictedState transformState,
in OwnerPredictedState ownerState,
in PhysicsCollider collider) =>
{
if (!flyingState.IsFlying)
{
return;
}
var selfRigidBodyIndex = physicsWorld.GetRigidBodyIndex(entity);
// Query the world
var distanceHits =
new NativeList <DistanceHit>(8, Allocator.Temp);
var distanceHitsCollector =
new CharacterMoveUtilities.SelfFilteringAllHitsCollector <DistanceHit>(
selfRigidBodyIndex, 0.1f, ref distanceHits);
var input = new ColliderDistanceInput
{
MaxDistance = 0.1f,
Transform = new RigidTransform()
{
pos = transformState.Position,
rot = transformState.Rotation
},
Collider = collider.ColliderPtr
};
physicsWorld.CalculateDistance(input, ref distanceHitsCollector);
if (distanceHitsCollector.NumHits == 0)
{
return;
}
for (var i = 0; i < distanceHitsCollector.AllHits.Length; i++)
{
var hit = distanceHitsCollector.AllHits[i];
var e = physicsWorld.Bodies[hit.RigidBodyIndex].Entity;
// FSLog.Info($"flyingState trigger entity:{e}");
if (e == ownerState.PreOwner)
{
return;
}
}
flyingState.IsFlying = false;
// FSLog.Info($"flyingState.IsFlying = false,entity:{entity}");
}).ScheduleParallel(Dependency);
public static bool OverlapCapsuleCustom <T, C>(ref T target, float3 point1, float3 point2, sfloat radius, ref C collector, CollisionFilter filter, QueryInteraction queryInteraction = QueryInteraction.Default)
where T : struct, ICollidable
where C : struct, ICollector <DistanceHit>
{
Assert.IsTrue(collector.MaxFraction == sfloat.Zero);
CapsuleCollider collider = default;
float3 center = (point1 + point2) / 2;
CapsuleGeometry geometry = new CapsuleGeometry
{
Radius = radius,
Vertex0 = point1 - center,
Vertex1 = point2 - center
};
collider.Initialize(geometry, filter, Material.Default);
ColliderDistanceInput input;
unsafe
{
input = new ColliderDistanceInput
{
Collider = (Collider *)UnsafeUtility.AddressOf(ref collider),
MaxDistance = sfloat.Zero,
Transform = new RigidTransform
{
pos = center,
rot = quaternion.identity
}
};
}
if (queryInteraction == QueryInteraction.Default)
{
return(target.CalculateDistance(input, ref collector));
}
else
{
unsafe
{
var interactionCollector = new QueryInteractionCollector <DistanceHit, C>
{
Collector = collector,
};
bool returnValue = target.CalculateDistance(input, ref interactionCollector);
collector = interactionCollector.Collector;
return(returnValue);
}
}
}
public static bool OverlapBoxCustom <T, C>(ref T target, float3 center, quaternion orientation, float3 halfExtents, ref C collector, CollisionFilter filter, QueryInteraction queryInteraction = QueryInteraction.Default)
where T : struct, ICollidable
where C : struct, ICollector <DistanceHit>
{
Assert.IsTrue(collector.MaxFraction == sfloat.Zero);
BoxCollider collider = default;
BoxGeometry geometry = new BoxGeometry
{
BevelRadius = sfloat.Zero,
Center = float3.zero,
Size = halfExtents * 2,
Orientation = quaternion.identity
};
collider.Initialize(geometry, filter, Material.Default);
ColliderDistanceInput input;
unsafe
{
input = new ColliderDistanceInput
{
Collider = (Collider *)UnsafeUtility.AddressOf(ref collider),
MaxDistance = sfloat.Zero,
Transform = new RigidTransform
{
pos = center,
rot = orientation
}
};
}
if (queryInteraction == QueryInteraction.Default)
{
return(target.CalculateDistance(input, ref collector));
}
else
{
unsafe
{
var interactionCollector = new QueryInteractionCollector <DistanceHit, C>
{
Collector = collector,
};
bool returnValue = target.CalculateDistance(input, ref interactionCollector);
collector = interactionCollector.Collector;
return(returnValue);
}
}
}
public unsafe bool DistanceLeaf <T>(ColliderDistanceInput input, int rigidBodyIndex, ref T collector)
where T : struct, ICollector <DistanceHit>
{
rigidBodyIndex += BaseRigidBodyIndex;
input.QueryContext.IsInitialized = true;
input.QueryContext.RigidBodyIndex = rigidBodyIndex;
RigidBody body = m_Bodies[rigidBodyIndex];
return(body.CalculateDistance(input, ref collector));
}
public static bool CalculateDistance <T>(ref T target, ColliderDistanceInput input, out DistanceHit result) where T : struct, ICollidable
{
var collector = new ClosestHitCollector <DistanceHit>(input.MaxDistance);
if (target.CalculateDistance(input, ref collector))
{
result = collector.ClosestHit; // TODO: would be nice to avoid this copy
return(true);
}
result = new DistanceHit();
return(false);
}
public unsafe void RigidBodyCalculateDistanceTest()
{
const float size = 1.0f;
const float convexRadius = 0.0f;
const float sphereRadius = 1.0f;
var queryPos = new float3(-10, -10, -10);
BlobAssetReference <Collider> boxCollider = BoxCollider.Create(new BoxGeometry
{
Center = float3.zero,
Orientation = quaternion.identity,
Size = size,
BevelRadius = convexRadius
});
BlobAssetReference <Collider> sphereCollider = SphereCollider.Create(new SphereGeometry
{
Center = float3.zero,
Radius = sphereRadius
});
var rigidBody = new Physics.RigidBody
{
WorldFromBody = RigidTransform.identity,
Collider = boxCollider
};
var colliderDistanceInput = new ColliderDistanceInput
{
Collider = (Collider *)sphereCollider.GetUnsafePtr(),
Transform = new RigidTransform(quaternion.identity, queryPos)
};
var closestHit = new DistanceHit();
var allHits = new NativeList <DistanceHit>(Allocator.Temp);
// OK case : with enough max distance
colliderDistanceInput.MaxDistance = 10000.0f;
Assert.IsTrue(rigidBody.CalculateDistance(colliderDistanceInput));
Assert.IsTrue(rigidBody.CalculateDistance(colliderDistanceInput, out closestHit));
Assert.IsTrue(rigidBody.CalculateDistance(colliderDistanceInput, ref allHits));
// Fail case : not enough max distance
colliderDistanceInput.MaxDistance = 1;
Assert.IsFalse(rigidBody.CalculateDistance(colliderDistanceInput));
Assert.IsFalse(rigidBody.CalculateDistance(colliderDistanceInput, out closestHit));
Assert.IsFalse(rigidBody.CalculateDistance(colliderDistanceInput, ref allHits));
boxCollider.Dispose();
sphereCollider.Dispose();
}
public bool CalculateDistance <T>(ColliderDistanceInput input, ref T collector) where T : struct, ICollector <DistanceHit>
{
// Transform the input into body space
MTransform worldFromBody = new MTransform(WorldFromBody);
MTransform bodyFromWorld = Inverse(worldFromBody);
input.Transform = new RigidTransform(
math.mul(math.inverse(WorldFromBody.rot), input.Transform.rot),
Mul(bodyFromWorld, input.Transform.pos));
SetQueryContextParameters(ref input.QueryContext, ref worldFromBody);
return(Collider.IsCreated && Collider.Value.CalculateDistance(input, ref collector));
}
public unsafe bool CalculateDistance <T>(ColliderDistanceInput input, NativeArray <RigidBody> rigidBodies, ref T collector)
where T : struct, ICollector <DistanceHit>
{
Assert.IsTrue(input.Collider != null);
if (input.Collider->Filter.IsEmpty)
{
return(false);
}
var leafProcessor = new BvhLeafProcessor(rigidBodies);
leafProcessor.BaseRigidBodyIndex = m_DynamicTree.NumBodies;
bool hasHit = m_StaticTree.BoundingVolumeHierarchy.Distance(input, ref leafProcessor, ref collector);
leafProcessor.BaseRigidBodyIndex = 0;
hasHit |= m_DynamicTree.BoundingVolumeHierarchy.Distance(input, ref leafProcessor, ref collector);
return(hasHit);
}
/// <summary>
/// Returns a list of all colliders within the specified distance of the provided collider, as a list of <see cref="DistanceHit"/>.<para/>
///
/// Can be used in conjunction with <see cref="ColliderCastAll"/> to get the penetration depths of any colliders (using the distance of collisions).<para/>
///
/// The caller must dispose of the returned list.
/// </summary>
/// <param name="collider"></param>
/// <param name="maxDistance"></param>
/// <param name="transform"></param>
/// <param name="collisionWorld"></param>
/// <returns></returns>
public unsafe static NativeList <DistanceHit> ColliderDistanceAll(PhysicsCollider collider, float maxDistance, RigidTransform transform, ref CollisionWorld collisionWorld, Entity ignore, Allocator allocator = Allocator.TempJob)
{
ColliderDistanceInput input = new ColliderDistanceInput()
{
Collider = collider.ColliderPtr,
MaxDistance = maxDistance,
Transform = transform
};
NativeList <DistanceHit> allDistances = new NativeList <DistanceHit>(allocator);
if (collisionWorld.CalculateDistance(input, ref allDistances))
{
TrimByEntity(ref allDistances, ignore);
}
return(allDistances);
}
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 unsafe void PhysicsBodyCalculateDistanceTest()
{
var geometry = new BoxGeometry
{
Size = new float2(1f),
};
using (var collider = PhysicsBoxCollider.Create(geometry))
{
var physicsBody = new PhysicsBody(collider);
var circleGeometry = new CircleGeometry {
Radius = 1f
};
using (var circleBlob = PhysicsCircleCollider.Create(circleGeometry))
{
var queryInput = new ColliderDistanceInput
{
Collider = circleBlob,
Transform = new PhysicsTransform(new float2(-10f))
};
var closestHit = new DistanceHit();
var allHits = new NativeList <DistanceHit>(Allocator.Temp);
// OK case : with enough max distance
queryInput.MaxDistance = 10000.0f;
Assert.IsTrue(physicsBody.CalculateDistance(queryInput));
Assert.IsTrue(physicsBody.CalculateDistance(queryInput, out closestHit));
Assert.IsTrue(physicsBody.CalculateDistance(queryInput, ref allHits));
// Fail case : not enough max distance
queryInput.MaxDistance = 1f;
Assert.IsFalse(physicsBody.CalculateDistance(queryInput));
Assert.IsFalse(physicsBody.CalculateDistance(queryInput, out closestHit));
Assert.IsFalse(physicsBody.CalculateDistance(queryInput, ref allHits));
allHits.Dispose();
}
}
}
protected override void OnUpdate()
{
EntityManager.DestroyEntity(GetSingletonEntity <UpdateOnce>());
Entities.WithAny <PowerupTag>().ForEach((ref Translation position) => {
var collider = new PhysicsCollider
{
Value = Unity.Physics.SphereCollider.Create(
new SphereGeometry
{
Center = float3.zero,
Radius = 0.1f
},
new CollisionFilter
{
BelongsTo = 1 << 5,
CollidesWith = 1 << 4,
GroupIndex = 0
}
)
};
var distanceQueryInput = new ColliderDistanceInput
{
Collider = collider.ColliderPtr,
Transform = new RigidTransform
{
pos = position.Value,
rot = quaternion.identity
},
MaxDistance = 20
};
World.GetExistingSystem <BuildPhysicsWorld>().PhysicsWorld.CalculateDistance(distanceQueryInput, out DistanceHit hit);
position = new Translation {
Value = hit.Position + hit.SurfaceNormal * 3.5f
};
});
}
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 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()
{
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();
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);
}
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 bool CalculateDistance <T>(ColliderDistanceInput input, ref T collector) where T : struct, ICollector <DistanceHit>
{
return(CollisionWorld.CalculateDistance(input, ref collector));
}
public bool CalculateDistance <T>(ColliderDistanceInput input, ref T collector) where T : struct, ICollector <DistanceHit>
{
return(Broadphase.CalculateDistance(input, m_Bodies, ref collector));
}
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;
}
}
}
}
public static bool CalculateDistance <T>(ref T target, ColliderDistanceInput input, ref NativeList <DistanceHit> allHits) where T : struct, ICollidable
{
var collector = new AllHitsCollector <DistanceHit>(input.MaxDistance, ref allHits);
return(target.CalculateDistance(input, ref collector));
}
// Collider distance public bool CalculateDistance(ColliderDistanceInput input) => QueryWrappers.CalculateDistance(ref this, input);
public bool CalculateDistance(ColliderDistanceInput input, out DistanceHit closestHit) => QueryWrappers.CalculateDistance(ref this, input, out closestHit);
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 bool CalculateDistance <T>(ref T target, ColliderDistanceInput input) where T : struct, ICollidable
{
var collector = new AnyHitCollector <DistanceHit>(input.MaxDistance);
return(target.CalculateDistance(input, ref collector));
}
public bool CalculateDistance(ColliderDistanceInput input, ref NativeList <DistanceHit> allHits) => QueryWrappers.CalculateDistance(ref this, input, ref allHits);
