public void WriteContextualizedEffect(int entityIndex, ref NativeStream.Writer consumerWriter, Effect1 effect, Entity target)
{
consumerWriter.Write(new Effect1Context()
{
Target = target, Effect = effect
});
}
unsafe static void SolveSingleJoint(JointData *jointData, int numIterations, float timestep,
ref MotionVelocity velocityA, ref MotionVelocity velocityB, ref MotionData motionA, ref MotionData motionB, out NativeStream jacobiansOut)
{
var stepInput = new Solver.StepInput
{
IsLastIteration = false,
InvNumSolverIterations = 1.0f / numIterations,
Timestep = timestep,
InvTimestep = timestep > 0.0f ? 1.0f / timestep : 0.0f
};
// Build jacobians
jacobiansOut = new NativeStream(1, Allocator.Temp);
{
NativeStream.Writer jacobianWriter = jacobiansOut.AsWriter();
jacobianWriter.BeginForEachIndex(0);
Solver.BuildJointJacobian(jointData, new BodyIndexPair(), velocityA, velocityB, motionA, motionB, timestep, numIterations, ref jacobianWriter);
jacobianWriter.EndForEachIndex();
}
var eventWriter = new NativeStream.Writer(); // no events expected
// Solve the joint
for (int iIteration = 0; iIteration < numIterations; iIteration++)
{
stepInput.IsLastIteration = (iIteration == numIterations - 1);
NativeStream.Reader jacobianReader = jacobiansOut.AsReader();
var jacIterator = new JacobianIterator(jacobianReader, 0);
while (jacIterator.HasJacobiansLeft())
{
ref JacobianHeader header = ref jacIterator.ReadJacobianHeader();
header.Solve(ref velocityA, ref velocityB, stepInput, ref eventWriter, ref eventWriter);
}
}
internal static unsafe void ExecuteImpl(int2 pair, Tree dynamicTree, ref NativeStream.Writer pairWriter)
{
var bodyFilters = (CollisionFilter *)dynamicTree.BodyFilters.GetUnsafeReadOnlyPtr();
var bufferedPairs = new BodyPairWriter((NativeStream.Writer *)UnsafeUtility.AddressOf(ref pairWriter), bodyFilters, bodyFilters, 0, 0);
new BoundingVolumeHierarchy(dynamicTree.Nodes, dynamicTree.NodeFilters).SelfBvhOverlap(ref bufferedPairs, pair.x, pair.y);
bufferedPairs.Close();
}
private unsafe void WriteEvent(CollisionEventData collisionEvent, ref NativeStream.Writer collisionEventWriter)
{
int numContactPoints = collisionEvent.NumNarrowPhaseContactPoints;
int size = CollisionEventData.CalculateSize(numContactPoints);
collisionEventWriter.Write(size);
byte *eventPtr = collisionEventWriter.Allocate(size);
ref CollisionEventData eventRef = ref UnsafeUtility.AsRef <CollisionEventData>(eventPtr);
private unsafe void WriteEvent(LowLevel.CollisionEvent collisionEvent, ref NativeStream.Writer collisionEventWriter)
{
int numContactPoints = collisionEvent.NumNarrowPhaseContactPoints;
int size = UnsafeUtility.SizeOf <LowLevel.CollisionEvent>() + numContactPoints * UnsafeUtility.SizeOf <ContactPoint>();
collisionEventWriter.Write(size);
byte *eventPtr = collisionEventWriter.Allocate(size);
ref LowLevel.CollisionEvent eventRef = ref UnsafeUtilityEx.AsRef <LowLevel.CollisionEvent>(eventPtr);
// Iterates the provided dispatch pairs and creates contacts and based on them.
public static void CreateContacts(ref PhysicsWorld world, NativeArray <DispatchPairSequencer.DispatchPair> dispatchPairs, float timeStep,
ref NativeStream.Writer contactsWriter)
{
contactsWriter.BeginForEachIndex(0);
ParallelCreateContactsJob.ExecuteImpl(ref world, timeStep, dispatchPairs, 0, dispatchPairs.Length, ref contactsWriter);
contactsWriter.EndForEachIndex();
}
/// <summary>
/// Write the contextualized effect to it's corresponding consumer stream.
/// </summary>
/// <param name="entityIndex">The casting entity.</param>
/// <param name="consumerWriter">The corresponding effect consumer stream.</param>
/// <param name="effect">The effect to contextualize.</param>
public void WriteContextualizedEffect(int entityIndex, ref NativeStream.Writer consumerWriter, TestEmptyEffect effect, Entity target)
{
consumerWriter.Write(new TestEmptyEffectContext()
{
Target = target,
Effect = effect
// YOUR CODE : populate the effect context with additonal contextual data.
});
}
public static unsafe void WriteArray <T>([NoAlias] ref NativeStream.Writer nativeStream, [NoAlias] ref NativeArray <T> array, int length) where T : unmanaged
{
length = math.min(length, array.Length);
nativeStream.Write(length);
for (int i = 0; i < length; i++)
{
nativeStream.Write(array[i]);
}
}
public void ConusmeAllEffectOnTargetEntity()
{
// Arrange
Entity caster = _entityManager.CreateEntity();
Entity Target = _entityManager.CreateEntity();
_entityManager.AddComponentData(Target, new TestResource()
{
Value = 100
});
_world.WithSystem <TestEffectConsumerSystem>();
TestEffectConsumerSystem consumerSystem = _world.GetReference().GetExistingSystem <TestEffectConsumerSystem>();
NativeStream.Writer effectWriter = consumerSystem.CreateConsumerWriter(2);
effectWriter.BeginForEachIndex(0);
effectWriter.Write(new TestEffectContext()
{
Target = Target, Effect = new TestEffect()
{
Value = 10
}
});
effectWriter.Write(new TestEffectContext()
{
Target = Target, Effect = new TestEffect()
{
Value = 20
}
});
effectWriter.EndForEachIndex();
effectWriter.BeginForEachIndex(1);
effectWriter.Write(new TestEffectContext()
{
Target = Target, Effect = new TestEffect()
{
Value = 5
}
});
effectWriter.Write(new TestEffectContext()
{
Target = Target, Effect = new TestEffect()
{
Value = 15
}
});
effectWriter.EndForEachIndex();
// Act
_world.UpdateSystem <TestEffectConsumerSystem>();
_world.CompleteAllJobs();
// Assert
Assert.AreEqual(50, _entityManager.GetComponentData <TestResource>(Target).Value);
}
// Build Jacobians from the contacts and joints stored in the simulation context
public static void BuildJacobians(ref PhysicsWorld world,
float timeStep, float3 gravity, int numSolverIterations,
NativeArray <DispatchPairSequencer.DispatchPair> dispatchPairs,
ref NativeStream.Reader contactsReader, ref NativeStream.Writer jacobiansWriter)
{
contactsReader.BeginForEachIndex(0);
jacobiansWriter.BeginForEachIndex(0);
float invTimeStep = timeStep > 0.0f ? 1.0f / timeStep : 0.0f;
float gravityAcceleration = math.length(gravity);
BuildJacobians(ref world, timeStep, invTimeStep, gravityAcceleration, numSolverIterations,
new NativeSlice <DispatchPairSequencer.DispatchPair>(dispatchPairs, 0, dispatchPairs.Length), ref contactsReader, ref jacobiansWriter);
}
// Generic solve method that dispatches to specific ones
public void Solve(
ref JacobianHeader jacHeader, ref MotionVelocity velocityA, ref MotionVelocity velocityB,
Solver.StepInput stepInput, ref NativeStream.Writer collisionEventsWriter)
{
bool bothBodiesWithInfInertiaAndMass = math.all(velocityA.InverseInertiaAndMass == float4.zero) && math.all(velocityB.InverseInertiaAndMass == float4.zero);
if (bothBodiesWithInfInertiaAndMass)
{
SolveInfMassPair(ref jacHeader, velocityA, velocityB, stepInput, ref collisionEventsWriter);
}
else
{
SolveContact(ref jacHeader, ref velocityA, ref velocityB, stepInput, ref collisionEventsWriter);
}
}
// Generic solve method that dispatches to specific ones
public void Solve(
ref JacobianHeader jacHeader, ref MotionVelocity velocityA, ref MotionVelocity velocityB,
Solver.StepInput stepInput, ref NativeStream.Writer collisionEventsWriter)
{
bool bothBodiesWithInfInertiaAndMass = velocityA.HasInfiniteInertiaAndMass && velocityB.HasInfiniteInertiaAndMass;
if (bothBodiesWithInfInertiaAndMass)
{
SolveInfMassPair(ref jacHeader, velocityA, velocityB, stepInput, ref collisionEventsWriter);
}
else
{
SolveContact(ref jacHeader, ref velocityA, ref velocityB, stepInput, ref collisionEventsWriter);
}
}
// Solve the Jacobian
public void SolveContact(
ref JacobianHeader jacHeader, ref MotionVelocity velocityA, ref MotionVelocity velocityB,
Solver.StepInput stepInput, ref NativeStream.Writer collisionEventsWriter, bool enableFrictionVelocitiesHeuristic,
Solver.MotionStabilizationInput motionStabilizationSolverInputA, Solver.MotionStabilizationInput motionStabilizationSolverInputB)
{
// Copy velocity data
MotionVelocity tempVelocityA = velocityA;
MotionVelocity tempVelocityB = velocityB;
if (jacHeader.HasMassFactors)
{
MassFactors jacMod = jacHeader.AccessMassFactors();
tempVelocityA.InverseInertia *= jacMod.InverseInertiaFactorA;
tempVelocityA.InverseMass *= jacMod.InverseMassFactorA;
tempVelocityB.InverseInertia *= jacMod.InverseInertiaFactorB;
tempVelocityB.InverseMass *= jacMod.InverseMassFactorB;
}
// Solve normal impulses
float sumImpulses = 0.0f;
float totalAccumulatedImpulse = 0.0f;
bool forceCollisionEvent = false;
for (int j = 0; j < BaseJacobian.NumContacts; j++)
{
ref ContactJacAngAndVelToReachCp jacAngular = ref jacHeader.AccessAngularJacobian(j);
// Solve velocity so that predicted contact distance is greater than or equal to zero
float relativeVelocity = BaseContactJacobian.GetJacVelocity(BaseJacobian.Normal, jacAngular.Jac,
tempVelocityA.LinearVelocity, tempVelocityA.AngularVelocity, tempVelocityB.LinearVelocity, tempVelocityB.AngularVelocity);
float dv = jacAngular.VelToReachCp - relativeVelocity;
float impulse = dv * jacAngular.Jac.EffectiveMass;
float accumulatedImpulse = math.max(jacAngular.Jac.Impulse + impulse, 0.0f);
if (accumulatedImpulse != jacAngular.Jac.Impulse)
{
float deltaImpulse = accumulatedImpulse - jacAngular.Jac.Impulse;
ApplyImpulse(deltaImpulse, BaseJacobian.Normal, jacAngular.Jac, ref tempVelocityA, ref tempVelocityB,
motionStabilizationSolverInputA.InverseInertiaScale, motionStabilizationSolverInputB.InverseInertiaScale);
}
jacAngular.Jac.Impulse = accumulatedImpulse;
sumImpulses += accumulatedImpulse;
totalAccumulatedImpulse += jacAngular.Jac.Impulse;
// Force contact event even when no impulse is applied, but there is penetration.
forceCollisionEvent |= jacAngular.VelToReachCp > 0.0f;
}
/// <summary>
/// Write the contextualized effect to it's corresponding consumer stream.
/// </summary>
/// <param name="entityIndex">The casting entity.</param>
/// <param name="consumerWriter">The corresponding effect consumer stream.</param>
/// <param name="effect">The effect to contextualize.</param>
public void WriteContextualizedEffect(int entityIndex, ref NativeStream.Writer consumerWriter, Effect2 effect, Entity target)
{
float attackPower = 1;
if (_attackPowers.Length > entityIndex)
{
attackPower = _attackPowers[entityIndex].Value;
}
consumerWriter.Write(new Effect2Context()
{
AttackPower = attackPower,
Target = target,
Effect = effect
});
}
public void SolveTest()
{
var jacobian = new ContactJacobian();
var jacHeader = new JacobianHeader();
var velocityA = MotionVelocity.Zero;
var velocityB = MotionVelocity.Zero;
var stepInput = new Solver.StepInput();
var collisionEventsWriter = new NativeStream.Writer();
jacobian.SolveContact(ref jacHeader, ref velocityA, ref velocityB, stepInput, ref collisionEventsWriter);
Assert.AreEqual(new JacobianHeader(), jacHeader);
Assert.AreEqual(MotionVelocity.Zero, velocityA);
Assert.AreEqual(MotionVelocity.Zero, velocityB);
Assert.AreEqual(new NativeStream.Writer(), collisionEventsWriter);
}
// Generic solve method that dispatches to specific ones
public void Solve(
ref JacobianHeader jacHeader, ref MotionVelocity velocityA, ref MotionVelocity velocityB,
Solver.StepInput stepInput, ref NativeStream.Writer collisionEventsWriter, bool enableFrictionVelocitiesHeuristic,
Solver.MotionStabilizationInput motionStabilizationSolverInputA, Solver.MotionStabilizationInput motionStabilizationSolverInputB)
{
bool bothBodiesWithInfInertiaAndMass = velocityA.HasInfiniteInertiaAndMass && velocityB.HasInfiniteInertiaAndMass;
if (bothBodiesWithInfInertiaAndMass)
{
SolveInfMassPair(ref jacHeader, velocityA, velocityB, stepInput, ref collisionEventsWriter);
}
else
{
SolveContact(ref jacHeader, ref velocityA, ref velocityB, stepInput, ref collisionEventsWriter,
enableFrictionVelocitiesHeuristic, motionStabilizationSolverInputA, motionStabilizationSolverInputB);
}
}
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);
// Write all overlapping body pairs to the given streams,
// where at least one of the bodies is dynamic. The results are unsorted.
public void FindOverlaps(ref NativeStream.Writer dynamicVsDynamicPairsWriter, ref NativeStream.Writer staticVsDynamicPairsWriter)
{
// Dynamic-dynamic
{
dynamicVsDynamicPairsWriter.BeginForEachIndex(0);
DynamicVsDynamicFindOverlappingPairsJob.ExecuteImpl(
new int2(1, 1), m_DynamicTree, ref dynamicVsDynamicPairsWriter);
dynamicVsDynamicPairsWriter.EndForEachIndex();
}
// Static-dynamic
{
staticVsDynamicPairsWriter.BeginForEachIndex(0);
StaticVsDynamicFindOverlappingPairsJob.ExecuteImpl(
new int2(1, 1), m_StaticTree, m_DynamicTree, ref staticVsDynamicPairsWriter);
staticVsDynamicPairsWriter.EndForEachIndex();
}
}
// Solve the Jacobians stored in the simulation context
public static void SolveJacobians(ref NativeStream.Reader jacobiansReader, NativeSlice <MotionVelocity> motionVelocities, float timeStep, int numIterations,
ref NativeStream.Writer collisionEventsWriter, ref NativeStream.Writer triggerEventsWriter)
{
float invNumIterations = math.rcp(numIterations);
float invTimeStep = timeStep > 0.0f ? 1.0f / timeStep : 0.0f;
for (int solverIterationId = 0; solverIterationId < numIterations; solverIterationId++)
{
var stepInput = new StepInput
{
InvNumSolverIterations = invNumIterations,
IsLastIteration = solverIterationId == numIterations - 1,
Timestep = timeStep,
InvTimestep = invTimeStep
};
Solve(motionVelocities, ref jacobiansReader, ref collisionEventsWriter, ref triggerEventsWriter, 0, stepInput);
}
}
public DotsFrameTrace(Allocator allocator)
{
NativeStream = new NativeStream(1, allocator);
_writer = NativeStream.AsWriter();
_writer.BeginForEachIndex(0);
}
private static void CalculateAndStoreDeferredImpulses(
CharacterControllerStepInput stepInput, float characterMass, float3 linearVelocity,
ref NativeList <SurfaceConstraintInfo> constraints, ref NativeStream.Writer deferredImpulseWriter)
{
var world = stepInput.World;
for (var i = 0; i < constraints.Length; i++)
{
var constraint = constraints[i];
var rigidBodyIndex = constraint.RigidBodyIndex;
// FSLog.Info($"Store impulse:{rigidBodyIndex}");
if (rigidBodyIndex < 0 /* || rigidBodyIndex >= world.NumDynamicBodies*/)
{
// Invalid and static bodies should be skipped
continue;
}
var impulse = float3.zero;
var body = world.Bodies[rigidBodyIndex];
var pointRelVel = world.GetLinearVelocity(rigidBodyIndex, constraint.HitPosition);
pointRelVel -= linearVelocity;
var projectedVelocity = math.dot(pointRelVel, constraint.Plane.Normal);
// Required velocity change
var deltaVelocity = -projectedVelocity * stepInput.Damping;
var distance = constraint.Plane.Distance;
if (distance < 0.0f)
{
deltaVelocity += distance / stepInput.DeltaTime * stepInput.Tau;
}
// Calculate impulse
if (deltaVelocity < 0.0f)
{
// Impulse magnitude
float impulseMagnitude;
{
var objectMassInv = 0.5f;
if (rigidBodyIndex < world.NumDynamicBodies)
{
var mv = world.MotionVelocities[rigidBodyIndex];
objectMassInv = GetInvMassAtPoint(constraint.HitPosition,
constraint.Plane.Normal, body, mv);
if (System.Math.Abs(objectMassInv) < 0.0001)
{
objectMassInv = 0.5f;
}
// FSLog.Info($"objectMassInv:{objectMassInv}");
}
impulseMagnitude = deltaVelocity / objectMassInv;
}
impulse = impulseMagnitude * constraint.Plane.Normal;
}
// Add gravity
{
// Effect of gravity on character velocity in the normal direction
var charVelDown = stepInput.Gravity * stepInput.DeltaTime;
var relVelN = math.dot(charVelDown, constraint.Plane.Normal);
// Subtract separation velocity if separating contact
{
var isSeparatingContact = projectedVelocity < 0.0f;
var newRelVelN = relVelN - projectedVelocity;
relVelN = math.select(relVelN, newRelVelN, isSeparatingContact);
}
// If resulting velocity is negative, an impulse is applied to stop the character
// from falling into the body
{
var newImpulse = impulse;
newImpulse += relVelN * characterMass * constraint.Plane.Normal;
impulse = math.select(impulse, newImpulse, relVelN < 0.0f);
}
}
// Store impulse
deferredImpulseWriter.Write(
new DeferredCharacterControllerImpulse
{
Entity = body.Entity,
Impulse = impulse,
Point = constraint.HitPosition
});
}
}
// Write all overlapping body pairs to the given streams,
// where at least one of the bodies is dynamic. The results are unsorted.
public void FindOverlaps(ref NativeStream.Writer dynamicVsDynamicPairsWriter, ref NativeStream.Writer staticVsDynamicPairsWriter)
{
Broadphase.FindOverlaps(ref dynamicVsDynamicPairsWriter, ref staticVsDynamicPairsWriter);
}
private static unsafe void CalculateAndStoreDeferredImpulsesAndCollisionEvents(
CharacterControllerStepInput stepInput, bool affectBodies, float characterMass,
float3 linearVelocity, NativeList <SurfaceConstraintInfo> constraints, ref NativeStream.Writer deferredImpulseWriter,
NativeList <StatefulCollisionEvent> collisionEvents)
{
PhysicsWorld world = stepInput.World;
for (int i = 0; i < constraints.Length; i++)
{
SurfaceConstraintInfo constraint = constraints[i];
int rigidBodyIndex = constraint.RigidBodyIndex;
float3 impulse = float3.zero;
if (rigidBodyIndex < 0)
{
continue;
}
// Skip static bodies if needed to calculate impulse
if (affectBodies && (rigidBodyIndex < world.NumDynamicBodies))
{
RigidBody body = world.Bodies[rigidBodyIndex];
float3 pointRelVel = world.GetLinearVelocity(rigidBodyIndex, constraint.HitPosition);
pointRelVel -= linearVelocity;
float projectedVelocity = math.dot(pointRelVel, constraint.Plane.Normal);
// Required velocity change
float deltaVelocity = -projectedVelocity * stepInput.Damping;
float distance = constraint.Plane.Distance;
if (distance < 0.0f)
{
deltaVelocity += (distance / stepInput.DeltaTime) * stepInput.Tau;
}
// Calculate impulse
MotionVelocity mv = world.MotionVelocities[rigidBodyIndex];
if (deltaVelocity < 0.0f)
{
// Impulse magnitude
float impulseMagnitude = 0.0f;
{
float objectMassInv = GetInvMassAtPoint(constraint.HitPosition, constraint.Plane.Normal, body, mv);
impulseMagnitude = deltaVelocity / objectMassInv;
}
impulse = impulseMagnitude * constraint.Plane.Normal;
}
// Add gravity
{
// Effect of gravity on character velocity in the normal direction
float3 charVelDown = stepInput.Gravity * stepInput.DeltaTime;
float relVelN = math.dot(charVelDown, constraint.Plane.Normal);
// Subtract separation velocity if separating contact
{
bool isSeparatingContact = projectedVelocity < 0.0f;
float newRelVelN = relVelN - projectedVelocity;
relVelN = math.select(relVelN, newRelVelN, isSeparatingContact);
}
// If resulting velocity is negative, an impulse is applied to stop the character
// from falling into the body
{
float3 newImpulse = impulse;
newImpulse += relVelN * characterMass * constraint.Plane.Normal;
impulse = math.select(impulse, newImpulse, relVelN < 0.0f);
}
}
// Store impulse
deferredImpulseWriter.Write(
new DeferredCharacterControllerImpulse()
{
Entity = body.Entity,
Impulse = impulse,
Point = constraint.HitPosition
});
}
if (collisionEvents.IsCreated && constraint.Touched && !constraint.IsMaxSlope)
{
var collisionEvent = new StatefulCollisionEvent(world.Bodies[stepInput.RigidBodyIndex].Entity,
world.Bodies[rigidBodyIndex].Entity, stepInput.RigidBodyIndex, rigidBodyIndex, ColliderKey.Empty,
constraint.ColliderKey, constraint.Plane.Normal);
collisionEvent.CollisionDetails = new StatefulCollisionEvent.Details(
1, math.dot(impulse, collisionEvent.Normal), constraint.HitPosition);
// check if collision event exists for the same bodyID and colliderKey
// although this is a nested for, number of solved constraints shouldn't be high
// if the same constraint (same entities, rigidbody indices and collider keys)
// is solved in multiple solver iterations, pick the one from latest iteration
bool newEvent = true;
for (int j = 0; j < collisionEvents.Length; j++)
{
if (collisionEvents[j].CompareTo(collisionEvent) == 0)
{
collisionEvents[j] = collisionEvent;
newEvent = false;
break;
}
}
if (newEvent)
{
collisionEvents.Add(collisionEvent);
}
}
}
}
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;
}
// Write a set of contact manifolds for a pair of bodies to the given stream.
public static unsafe void BodyBody(RigidBody rigidBodyA, RigidBody rigidBodyB, MotionVelocity motionVelocityA, MotionVelocity motionVelocityB,
float collisionTolerance, float timeStep, BodyIndexPair pair, ref NativeStream.Writer contactWriter)
{
Collider *colliderA = rigidBodyA.Collider;
Collider *colliderB = rigidBodyB.Collider;
if (colliderA == null || colliderB == null || !CollisionFilter.IsCollisionEnabled(colliderA->Filter, colliderB->Filter))
{
return;
}
// Build combined motion expansion
MotionExpansion expansion;
{
MotionExpansion expansionA = motionVelocityA.CalculateExpansion(timeStep);
MotionExpansion expansionB = motionVelocityB.CalculateExpansion(timeStep);
expansion = new MotionExpansion
{
Linear = expansionA.Linear - expansionB.Linear,
Uniform = expansionA.Uniform + expansionB.Uniform + collisionTolerance
};
}
var context = new Context
{
BodyIndices = pair,
BodyCustomTags = new CustomTagsPair {
CustomTagsA = rigidBodyA.CustomTags, CustomTagsB = rigidBodyB.CustomTags
},
BodiesHaveInfiniteMass =
!math.any(motionVelocityA.InverseInertiaAndMass) &&
!math.any(motionVelocityB.InverseInertiaAndMass),
ContactWriter = (NativeStream.Writer *)UnsafeUtility.AddressOf(ref contactWriter)
};
var worldFromA = new MTransform(rigidBodyA.WorldFromBody);
var worldFromB = new MTransform(rigidBodyB.WorldFromBody);
// Dispatch to appropriate manifold generator
switch (colliderA->CollisionType)
{
case CollisionType.Convex:
switch (colliderB->CollisionType)
{
case CollisionType.Convex:
ConvexConvex(context, ColliderKeyPair.Empty, colliderA, colliderB, worldFromA, worldFromB, expansion.MaxDistance, false);
break;
case CollisionType.Composite:
ConvexComposite(context, ColliderKey.Empty, colliderA, colliderB, worldFromA, worldFromB, expansion, false);
break;
case CollisionType.Terrain:
ConvexTerrain(context, ColliderKeyPair.Empty, colliderA, colliderB, worldFromA, worldFromB, expansion.MaxDistance, false);
break;
}
break;
case CollisionType.Composite:
switch (colliderB->CollisionType)
{
case CollisionType.Convex:
CompositeConvex(context, colliderA, colliderB, worldFromA, worldFromB, expansion, false);
break;
case CollisionType.Composite:
CompositeComposite(context, colliderA, colliderB, worldFromA, worldFromB, expansion, false);
break;
case CollisionType.Terrain:
CompositeTerrain(context, colliderA, colliderB, worldFromA, worldFromB, expansion.MaxDistance, false);
break;
}
break;
case CollisionType.Terrain:
switch (colliderB->CollisionType)
{
case CollisionType.Convex:
TerrainConvex(context, ColliderKeyPair.Empty, colliderA, colliderB, worldFromA, worldFromB, expansion.MaxDistance, false);
break;
case CollisionType.Composite:
TerrainComposite(context, colliderA, colliderB, worldFromA, worldFromB, expansion.MaxDistance, false);
break;
case CollisionType.Terrain:
UnityEngine.Assertions.Assert.IsTrue(false);
break;
}
break;
}
}
public unsafe void OverlapTaskFilteringTest([Values(2, 10, 33, 100)] int elementCount)
{
elementCount *= 2;
int numNodes = elementCount + Constants.MaxNumTreeBranches;
var points = new NativeArray <PointAndIndex>(elementCount, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
var aabbs = new NativeArray <Aabb>(elementCount, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
var bodyFilters = new NativeArray <CollisionFilter>(elementCount, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
InitInputWithCopyArrays(points, aabbs, bodyFilters);
var nodes = new NativeArray <Node>(numNodes, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
Node *nodesPtr = (Node *)nodes.GetUnsafePtr();
var seenUnfiltered = new HashSet <BodyIndexPair>();
{
var bvhUnfiltered = new BoundingVolumeHierarchy(nodes);
bvhUnfiltered.Build(points, aabbs, out int numNodesOut);
bvhUnfiltered.CheckIntegrity();
EverythingWriter pairWriter = new EverythingWriter {
SeenPairs = seenUnfiltered
};
BoundingVolumeHierarchy.TreeOverlap(ref pairWriter, nodesPtr, nodesPtr);
}
var nodeFilters = new NativeArray <CollisionFilter>(numNodes, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
var bvhFiltered = new BoundingVolumeHierarchy(nodes, nodeFilters);
int numNodesFilteredTree;
bvhFiltered.Build(points, aabbs, out numNodesFilteredTree);
bvhFiltered.BuildCombinedCollisionFilter(bodyFilters, 0, numNodesFilteredTree - 1);
var filteredCollisionPairs = new NativeStream(1, Allocator.TempJob);
NativeStream.Writer filteredPairWriter = filteredCollisionPairs.AsWriter();
filteredPairWriter.BeginForEachIndex(0);
CollisionFilter *bodyFiltersPtr = (CollisionFilter *)bodyFilters.GetUnsafePtr();
var bufferedPairs = new Broadphase.BodyPairWriter(&filteredPairWriter, bodyFiltersPtr, bodyFiltersPtr, 0, 0);
CollisionFilter *nodeFiltersPtr = (CollisionFilter *)nodeFilters.GetUnsafePtr();
BoundingVolumeHierarchy.TreeOverlap(ref bufferedPairs, nodesPtr, nodesPtr, nodeFiltersPtr, nodeFiltersPtr);
bufferedPairs.Close();
filteredPairWriter.EndForEachIndex();
NativeStream.Reader filteredPairReader = filteredCollisionPairs.AsReader();
filteredPairReader.BeginForEachIndex(0);
// Check that every pair in our filtered set also appears in the unfiltered set
while (filteredPairReader.RemainingItemCount > 0)
{
var pair = filteredPairReader.Read <BodyIndexPair>();
Assert.IsTrue(seenUnfiltered.Contains(pair));
seenUnfiltered.Remove(pair); // Remove the pair
}
// Pairs were removed, so the only remaining ones should be filtered
foreach (BodyIndexPair pair in seenUnfiltered)
{
bool shouldCollide = CollisionFilter.IsCollisionEnabled(bodyFilters[pair.BodyIndexA], bodyFilters[pair.BodyIndexB]);
Assert.IsFalse(shouldCollide);
}
nodeFilters.Dispose();
nodes.Dispose();
bodyFilters.Dispose();
aabbs.Dispose();
points.Dispose();
filteredCollisionPairs.Dispose();
}
internal static unsafe void ExecuteImpl(int2 pair, Tree staticTree, Tree dynamicTree, ref NativeStream.Writer pairWriter)
{
var staticBvh = new BoundingVolumeHierarchy(staticTree.Nodes, staticTree.NodeFilters);
var dynamicBvh = new BoundingVolumeHierarchy(dynamicTree.Nodes, dynamicTree.NodeFilters);
var bodyPairWriter = new BodyPairWriter((NativeStream.Writer *)UnsafeUtility.AddressOf(ref pairWriter),
(CollisionFilter *)staticTree.BodyFilters.GetUnsafeReadOnlyPtr(), (CollisionFilter *)dynamicTree.BodyFilters.GetUnsafeReadOnlyPtr(),
dynamicTree.NumBodies, 0);
staticBvh.BvhOverlap(ref bodyPairWriter, dynamicBvh, pair.x, pair.y);
bodyPairWriter.Close();
}
internal static unsafe void ExecuteImpl(ref PhysicsWorld world, float timeStep,
NativeArray <DispatchPairSequencer.DispatchPair> dispatchPairs,
int dispatchPairReadOffset, int numPairsToRead, ref NativeStream.Writer contactWriter)
{
for (int i = 0; i < numPairsToRead; i++)
{
DispatchPairSequencer.DispatchPair dispatchPair = dispatchPairs[dispatchPairReadOffset + i];
// Invalid pairs can exist by being disabled by users
if (dispatchPair.IsValid)
{
if (dispatchPair.IsContact)
{
// Create contact manifolds for this pair of bodies
var pair = new BodyIndexPair
{
BodyIndexA = dispatchPair.BodyIndexA,
BodyIndexB = dispatchPair.BodyIndexB
};
RigidBody rigidBodyA = world.Bodies[pair.BodyIndexA];
RigidBody rigidBodyB = world.Bodies[pair.BodyIndexB];
MotionVelocity motionVelocityA = pair.BodyIndexA < world.MotionVelocities.Length ?
world.MotionVelocities[pair.BodyIndexA] : MotionVelocity.Zero;
MotionVelocity motionVelocityB = pair.BodyIndexB < world.MotionVelocities.Length ?
world.MotionVelocities[pair.BodyIndexB] : MotionVelocity.Zero;
ManifoldQueries.BodyBody(rigidBodyA, rigidBodyB, motionVelocityA, motionVelocityB,
world.CollisionWorld.CollisionTolerance, timeStep, pair, ref contactWriter);
}
}
}
}
private static unsafe void CalculateAndStoreDeferredImpulses(
CharacterControllerStepInput stepInput, float characterMass, float3 linearVelocity,
ref NativeList <SurfaceConstraintInfo> constraints, ref NativeStream.Writer deferredImpulseWriter)
{
PhysicsWorld world = stepInput.World;
for (int i = 0; i < constraints.Length; i++)
{
SurfaceConstraintInfo constraint = constraints[i];
int rigidBodyIndex = constraint.RigidBodyIndex;
if (rigidBodyIndex < 0 || rigidBodyIndex >= world.NumDynamicBodies)
{
// Invalid and static bodies should be skipped
continue;
}
RigidBody body = world.Bodies[rigidBodyIndex];
float3 pointRelVel = world.GetLinearVelocity(rigidBodyIndex, constraint.HitPosition);
pointRelVel -= linearVelocity;
float projectedVelocity = math.dot(pointRelVel, constraint.Plane.Normal);
// Required velocity change
float deltaVelocity = -projectedVelocity * stepInput.Damping;
float distance = constraint.Plane.Distance;
if (distance < 0.0f)
{
deltaVelocity += (distance / stepInput.DeltaTime) * stepInput.Tau;
}
// Calculate impulse
MotionVelocity mv = world.MotionVelocities[rigidBodyIndex];
float3 impulse = float3.zero;
if (deltaVelocity < 0.0f)
{
// Impulse magnitude
float impulseMagnitude = 0.0f;
{
float objectMassInv = GetInvMassAtPoint(constraint.HitPosition, constraint.Plane.Normal, body, mv);
impulseMagnitude = deltaVelocity / objectMassInv;
}
impulse = impulseMagnitude * constraint.Plane.Normal;
}
// Add gravity
{
// Effect of gravity on character velocity in the normal direction
float3 charVelDown = stepInput.Gravity * stepInput.DeltaTime;
float relVelN = math.dot(charVelDown, constraint.Plane.Normal);
// Subtract separation velocity if separating contact
{
bool isSeparatingContact = projectedVelocity < 0.0f;
float newRelVelN = relVelN - projectedVelocity;
relVelN = math.select(relVelN, newRelVelN, isSeparatingContact);
}
// If resulting velocity is negative, an impulse is applied to stop the character
// from falling into the body
{
float3 newImpulse = impulse;
newImpulse += relVelN * characterMass * constraint.Plane.Normal;
impulse = math.select(impulse, newImpulse, relVelN < 0.0f);
}
}
// Store impulse
deferredImpulseWriter.Write(
new DeferredCharacterControllerImpulse()
{
Entity = body.Entity,
Impulse = impulse,
Point = constraint.HitPosition
});
}
}
public Writer(ref GraphStream graphStream)
{
m_Writer = graphStream.NativeStream.AsWriter();
}
