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(); }