// Solve the Jacobian
        public void Solve(ref MotionVelocity velocityA, ref MotionVelocity velocityB, float timestep, float invTimestep)
        {
            // Predict the motions' transforms at the end of the step
            MTransform futureWorldFromA;
            MTransform futureWorldFromB;

            {
                quaternion dqA = Integrator.IntegrateAngularVelocity(velocityA.AngularVelocity, timestep);
                quaternion dqB = Integrator.IntegrateAngularVelocity(velocityB.AngularVelocity, timestep);
                quaternion futureOrientationA = math.normalize(math.mul(WorldFromA.rot, dqA));
                quaternion futureOrientationB = math.normalize(math.mul(WorldFromB.rot, dqB));
                futureWorldFromA = new MTransform(futureOrientationA, WorldFromA.pos + velocityA.LinearVelocity * timestep);
                futureWorldFromB = new MTransform(futureOrientationB, WorldFromB.pos + velocityB.LinearVelocity * timestep);
            }

            // Calculate the angulars
            CalculateAngulars(PivotAinA, futureWorldFromA.Rotation, out float3 angA0, out float3 angA1, out float3 angA2);
            CalculateAngulars(PivotBinB, futureWorldFromB.Rotation, out float3 angB0, out float3 angB1, out float3 angB2);

            // Calculate effective mass
            float3 EffectiveMassDiag, EffectiveMassOffDiag;
            {
                // Calculate the inverse effective mass matrix
                float3 invEffectiveMassDiag = new float3(
                    JacobianUtilities.CalculateInvEffectiveMassDiag(angA0, velocityA.InverseInertia, velocityA.InverseMass,
                                                                    angB0, velocityB.InverseInertia, velocityB.InverseMass),
                    JacobianUtilities.CalculateInvEffectiveMassDiag(angA1, velocityA.InverseInertia, velocityA.InverseMass,
                                                                    angB1, velocityB.InverseInertia, velocityB.InverseMass),
                    JacobianUtilities.CalculateInvEffectiveMassDiag(angA2, velocityA.InverseInertia, velocityA.InverseMass,
                                                                    angB2, velocityB.InverseInertia, velocityB.InverseMass));

                float3 invEffectiveMassOffDiag = new float3(
                    JacobianUtilities.CalculateInvEffectiveMassOffDiag(angA0, angA1, velocityA.InverseInertia, angB0, angB1, velocityB.InverseInertia),
                    JacobianUtilities.CalculateInvEffectiveMassOffDiag(angA0, angA2, velocityA.InverseInertia, angB0, angB2, velocityB.InverseInertia),
                    JacobianUtilities.CalculateInvEffectiveMassOffDiag(angA1, angA2, velocityA.InverseInertia, angB1, angB2, velocityB.InverseInertia));

                // Invert to get the effective mass matrix
                JacobianUtilities.InvertSymmetricMatrix(invEffectiveMassDiag, invEffectiveMassOffDiag, out EffectiveMassDiag, out EffectiveMassOffDiag);
            }

            // Predict error at the end of the step and calculate the impulse to correct it
            float3 impulse;

            {
                // Find the difference between the future distance and the limit range, then apply tau and damping
                float futureDistanceError = CalculateError(futureWorldFromA, futureWorldFromB, out float3 futureDirection);
                float solveDistanceError  = JacobianUtilities.CalculateCorrection(futureDistanceError, InitialError, Tau, Damping);

                // Calculate the impulse to correct the error
                float3   solveError    = solveDistanceError * futureDirection;
                float3x3 effectiveMass = JacobianUtilities.BuildSymmetricMatrix(EffectiveMassDiag, EffectiveMassOffDiag);
                impulse = math.mul(effectiveMass, solveError) * invTimestep;
            }

            // Apply the impulse
            ApplyImpulse(impulse, angA0, angA1, angA2, ref velocityA);
            ApplyImpulse(-impulse, angB0, angB1, angB2, ref velocityB);
        }
Exemple #2
0
        // Schedule all the jobs for the simulation step.
        // Enqueued callbacks can choose to inject additional jobs at defined sync points.
        public unsafe void ScheduleStepJobs(SimulationStepInput input, JobHandle inputDeps)
        {
            if (input.TimeStep < 0)
            {
                throw new ArgumentOutOfRangeException();
            }
            if (input.ThreadCountHint <= 0)
            {
                throw new ArgumentOutOfRangeException();
            }
            if (input.NumSolverIterations <= 0)
            {
                throw new ArgumentOutOfRangeException();
            }

            // Dispose event streams from previous frame
            JobHandle handle = DisposeEventStreams(inputDeps);

            // Allocate storage for input velocities
            m_Storage.InputVelocityCount = input.World.NumDynamicBodies;

            m_Context = new Context
            {
                TimeStep        = input.TimeStep,
                InputVelocities = m_Storage.InputVelocities
            };

            if (input.World.NumDynamicBodies == 0)
            {
                // No need to do anything, since nothing can move
                FinalSimulationJobHandle = handle;
                FinalJobHandle           = handle;
                return;
            }

            SimulationCallbacks callbacks = input.Callbacks ?? new SimulationCallbacks();

            // Find all body pairs that overlap in the broadphase
            handle = input.World.CollisionWorld.Broadphase.ScheduleFindOverlapsJobs(
                out BlockStream dynamicVsDynamicBodyPairs, out BlockStream dynamicVsStaticBodyPairs, ref m_Context, handle);
            var postOverlapsHandle = handle;

            // Sort all overlapping and jointed body pairs into phases
            handle = m_Scheduler.ScheduleCreatePhasedDispatchPairsJob(
                ref input.World, ref dynamicVsDynamicBodyPairs, ref dynamicVsStaticBodyPairs, ref m_Context, handle);

            // Apply gravity and copy input velocities at this point (in parallel with the scheduler, but before the callbacks)
            var applyGravityAndCopyInputVelocitiesHandle = Solver.ScheduleApplyGravityAndCopyInputVelocitiesJob(
                ref input.World.DynamicsWorld, m_Storage.InputVelocities, input.TimeStep * input.Gravity, postOverlapsHandle);

            handle = JobHandle.CombineDependencies(handle, applyGravityAndCopyInputVelocitiesHandle);
            handle = callbacks.Execute(SimulationCallbacks.Phase.PostCreateDispatchPairs, this, ref input.World, handle);

            // Create contact points & joint Jacobians
            handle = NarrowPhase.ScheduleProcessBodyPairsJobs(ref input.World, input.TimeStep, input.NumSolverIterations, ref m_Context, handle);
            handle = callbacks.Execute(SimulationCallbacks.Phase.PostCreateContacts, this, ref input.World, handle);

            // Create contact Jacobians
            handle = Solver.ScheduleBuildContactJacobiansJobs(ref input.World.DynamicsWorld, input.TimeStep, math.length(input.Gravity), ref m_Context, handle);
            handle = callbacks.Execute(SimulationCallbacks.Phase.PostCreateContactJacobians, this, ref input.World, handle);

            // Solve all Jacobians
            handle = Solver.ScheduleSolveJacobiansJobs(ref input.World.DynamicsWorld, input.TimeStep, input.NumSolverIterations, ref m_Context, handle);
            handle = callbacks.Execute(SimulationCallbacks.Phase.PostSolveJacobians, this, ref input.World, handle);

            // Integrate motions
            handle = Integrator.ScheduleIntegrateJobs(ref input.World.DynamicsWorld, input.TimeStep, handle);

            // Synchronize the collision world
            if (input.SynchronizeCollisionWorld)
            {
                handle = input.World.CollisionWorld.ScheduleUpdateDynamicLayer(ref input.World, input.TimeStep, input.Gravity, input.ThreadCountHint, handle);  // TODO: timeStep = 0?
            }

            // Return the final simulation handle
            FinalSimulationJobHandle = handle;

            // Return the final handle, which includes disposing temporary arrays
            JobHandle *deps = stackalloc JobHandle[11]
            {
                FinalSimulationJobHandle,
                m_Context.DisposeOverlapPairs0,
                m_Context.DisposeOverlapPairs1,
                m_Context.DisposeBroadphasePairs0,
                m_Context.DisposeBroadphasePairs1,
                m_Context.DisposeContacts,
                m_Context.DisposeJacobians,
                m_Context.DisposeJointJacobians,
                m_Context.DisposeSolverSchedulerData,
                m_Context.DisposeProcessBodyPairs,
                m_Context.DisposePhasedDispatchPairs
            };

            FinalJobHandle = JobHandleUnsafeUtility.CombineDependencies(deps, 11);
        }
Exemple #3
0
        // Schedule all the jobs for the simulation step.
        // Enqueued callbacks can choose to inject additional jobs at defined sync points.
        // threadCountHint defines which simulation type will be called:
        //     - threadCountHint > 0 will result in default multithreaded simulation
        //     - threadCountHint <=0 will result in a very small number of jobs (1 per physics step phase) that are scheduled sequentially
        // Behavior doesn't change regardless of the threadCountHint provided.
        public unsafe SimulationJobHandles ScheduleStepJobs(SimulationStepInput input, SimulationCallbacks callbacksIn, JobHandle inputDeps, int threadCountHint = 0)
        {
            if (input.TimeStep < 0)
            {
                throw new ArgumentOutOfRangeException();
            }
            if (input.NumSolverIterations <= 0)
            {
                throw new ArgumentOutOfRangeException();
            }

            bool singleThreadedSim = (threadCountHint <= 0);

            // Dispose and reallocate input velocity buffer, if dynamic body count has increased.
            // Dispose previous collision and trigger event data streams.
            // New event streams are reallocated later when the work item count is known.
            JobHandle handle = SimulationContext.ScheduleReset(ref input.World, inputDeps, false);

            SimulationContext.TimeStep = input.TimeStep;

            StepContext = new StepContext();

            if (input.World.NumDynamicBodies == 0)
            {
                // No need to do anything, since nothing can move
                m_StepHandles = new SimulationJobHandles(handle);
                return(m_StepHandles);
            }

            SimulationCallbacks callbacks = callbacksIn ?? new SimulationCallbacks();

            // Find all body pairs that overlap in the broadphase
            var handles = input.World.CollisionWorld.ScheduleFindOverlapsJobs(
                out NativeStream dynamicVsDynamicBodyPairs, out NativeStream dynamicVsStaticBodyPairs, handle, threadCountHint);

            handle = handles.FinalExecutionHandle;
            var disposeHandle1     = handles.FinalDisposeHandle;
            var postOverlapsHandle = handle;

            // Sort all overlapping and jointed body pairs into phases
            handles = m_Scheduler.ScheduleCreatePhasedDispatchPairsJob(
                ref input.World, ref dynamicVsDynamicBodyPairs, ref dynamicVsStaticBodyPairs, handle,
                ref StepContext.PhasedDispatchPairs, out StepContext.SolverSchedulerInfo, threadCountHint);
            handle = handles.FinalExecutionHandle;
            var disposeHandle2 = handles.FinalDisposeHandle;

            // Apply gravity and copy input velocities at this point (in parallel with the scheduler, but before the callbacks)
            var applyGravityAndCopyInputVelocitiesHandle = Solver.ScheduleApplyGravityAndCopyInputVelocitiesJob(
                ref input.World.DynamicsWorld, SimulationContext.InputVelocities, input.TimeStep * input.Gravity, singleThreadedSim ? handle : postOverlapsHandle, threadCountHint);

            handle = JobHandle.CombineDependencies(handle, applyGravityAndCopyInputVelocitiesHandle);
            handle = callbacks.Execute(SimulationCallbacks.Phase.PostCreateDispatchPairs, this, ref input.World, handle);

            // Create contact points & joint Jacobians
            handles = NarrowPhase.ScheduleCreateContactsJobs(ref input.World, input.TimeStep,
                                                             ref StepContext.Contacts, ref StepContext.Jacobians, ref StepContext.PhasedDispatchPairs, handle,
                                                             ref StepContext.SolverSchedulerInfo, threadCountHint);
            handle = handles.FinalExecutionHandle;
            var disposeHandle3 = handles.FinalDisposeHandle;

            handle = callbacks.Execute(SimulationCallbacks.Phase.PostCreateContacts, this, ref input.World, handle);

            // Create contact Jacobians
            handles = Solver.ScheduleBuildJacobiansJobs(ref input.World, input.TimeStep, input.Gravity, input.NumSolverIterations,
                                                        handle, ref StepContext.PhasedDispatchPairs, ref StepContext.SolverSchedulerInfo,
                                                        ref StepContext.Contacts, ref StepContext.Jacobians, threadCountHint);
            handle = handles.FinalExecutionHandle;
            var disposeHandle4 = handles.FinalDisposeHandle;

            handle = callbacks.Execute(SimulationCallbacks.Phase.PostCreateContactJacobians, this, ref input.World, handle);

            // Solve all Jacobians
            handles = Solver.ScheduleSolveJacobiansJobs(ref input.World.DynamicsWorld, input.TimeStep, input.NumSolverIterations,
                                                        ref StepContext.Jacobians, ref SimulationContext.CollisionEventDataStream, ref SimulationContext.TriggerEventDataStream,
                                                        ref StepContext.SolverSchedulerInfo, handle, threadCountHint);
            handle = handles.FinalExecutionHandle;
            var disposeHandle5 = handles.FinalDisposeHandle;

            handle = callbacks.Execute(SimulationCallbacks.Phase.PostSolveJacobians, this, ref input.World, handle);

            // Integrate motions
            handle = Integrator.ScheduleIntegrateJobs(ref input.World.DynamicsWorld, input.TimeStep, handle, threadCountHint);

            // Synchronize the collision world
            if (input.SynchronizeCollisionWorld)
            {
                handle = input.World.CollisionWorld.ScheduleUpdateDynamicTree(ref input.World, input.TimeStep, input.Gravity, handle, threadCountHint);  // TODO: timeStep = 0?
            }

            // Return the final simulation handle
            m_StepHandles.FinalExecutionHandle = handle;

            // Different dispose logic for single threaded simulation compared to "standard" threading (multi threaded)
            if (singleThreadedSim)
            {
                handle = dynamicVsDynamicBodyPairs.Dispose(handle);
                handle = dynamicVsStaticBodyPairs.Dispose(handle);
                handle = StepContext.PhasedDispatchPairs.Dispose(handle);
                handle = StepContext.Contacts.Dispose(handle);
                handle = StepContext.Jacobians.Dispose(handle);
                handle = StepContext.SolverSchedulerInfo.ScheduleDisposeJob(handle);

                m_StepHandles.FinalDisposeHandle = handle;
            }
            else
            {
                // Return the final handle, which includes disposing temporary arrays
                JobHandle *deps = stackalloc JobHandle[5]
                {
                    disposeHandle1,
                    disposeHandle2,
                    disposeHandle3,
                    disposeHandle4,
                    disposeHandle5
                };
                m_StepHandles.FinalDisposeHandle = JobHandleUnsafeUtility.CombineDependencies(deps, 5);
            }

            return(m_StepHandles);
        }
Exemple #4
0
        // Steps the simulation immediately on a single thread without spawning any jobs.
        public static void StepImmediate(SimulationStepInput input, ref SimulationContext simulationContext)
        {
            if (input.TimeStep < 0)
            {
                throw new ArgumentOutOfRangeException();
            }
            if (input.NumSolverIterations <= 0)
            {
                throw new ArgumentOutOfRangeException();
            }

            if (input.World.NumDynamicBodies == 0)
            {
                // No need to do anything, since nothing can move
                return;
            }

            // Inform the context of the timeStep
            simulationContext.TimeStep = input.TimeStep;

            // Find all body pairs that overlap in the broadphase
            var dynamicVsDynamicBodyPairs = new NativeStream(1, Allocator.Temp);
            var dynamicVsStaticBodyPairs  = new NativeStream(1, Allocator.Temp);
            {
                var dynamicVsDynamicBodyPairsWriter = dynamicVsDynamicBodyPairs.AsWriter();
                var dynamicVsStaticBodyPairsWriter  = dynamicVsStaticBodyPairs.AsWriter();
                input.World.CollisionWorld.FindOverlaps(ref dynamicVsDynamicBodyPairsWriter, ref dynamicVsStaticBodyPairsWriter);
            }

            // Create dispatch pairs
            var dispatchPairs = new NativeList <DispatchPairSequencer.DispatchPair>(Allocator.Temp);

            DispatchPairSequencer.CreateDispatchPairs(ref dynamicVsDynamicBodyPairs, ref dynamicVsStaticBodyPairs,
                                                      input.World.NumDynamicBodies, input.World.Joints, ref dispatchPairs);

            // Apply gravity and copy input velocities
            Solver.ApplyGravityAndCopyInputVelocities(input.World.DynamicsWorld.MotionDatas, input.World.DynamicsWorld.MotionVelocities,
                                                      simulationContext.InputVelocities, input.TimeStep * input.Gravity);

            // Narrow phase
            var contacts = new NativeStream(1, Allocator.Temp);
            {
                var contactsWriter = contacts.AsWriter();
                NarrowPhase.CreateContacts(ref input.World, dispatchPairs.AsArray(), input.TimeStep, ref contactsWriter);
            }

            // Build Jacobians
            var jacobians = new NativeStream(1, Allocator.Temp);

            {
                var contactsReader  = contacts.AsReader();
                var jacobiansWriter = jacobians.AsWriter();
                Solver.BuildJacobians(ref input.World, input.TimeStep, input.Gravity, input.NumSolverIterations,
                                      dispatchPairs.AsArray(), ref contactsReader, ref jacobiansWriter);
            }

            // Solve Jacobians
            {
                var jacobiansReader       = jacobians.AsReader();
                var collisionEventsWriter = simulationContext.CollisionEventDataStream.AsWriter();
                var triggerEventsWriter   = simulationContext.TriggerEventDataStream.AsWriter();
                Solver.SolveJacobians(ref jacobiansReader, input.World.DynamicsWorld.MotionVelocities, input.TimeStep, input.NumSolverIterations,
                                      ref collisionEventsWriter, ref triggerEventsWriter);
            }

            // Integrate motions
            Integrator.Integrate(input.World.DynamicsWorld.MotionDatas, input.World.DynamicsWorld.MotionVelocities, input.TimeStep);

            // Synchronize the collision world if asked for
            if (input.SynchronizeCollisionWorld)
            {
                input.World.CollisionWorld.UpdateDynamicTree(ref input.World, input.TimeStep, input.Gravity);
            }
        }
Exemple #5
0
        // Schedule all the jobs for the simulation step.
        // Enqueued callbacks can choose to inject additional jobs at defined sync points.
        public void ScheduleStepJobs(SimulationStepInput input, JobHandle inputDeps, out JobHandle finalSimulationJobHandle, out JobHandle finalJobHandle)
        {
            // Dispose event streams from previous frame
            DisposeEventStreams();

            m_Context = new Context();

            if (input.World.NumDynamicBodies == 0)
            {
                // No need to do anything, since nothing can move
                finalSimulationJobHandle = new JobHandle();
                finalJobHandle           = new JobHandle();
                return;
            }

            SimulationCallbacks callbacks = input.Callbacks ?? new SimulationCallbacks();
            JobHandle           handle    = inputDeps;

            // We need to make sure that broadphase tree building is done before we schedule FindOverlapsJobs.
            handle.Complete();

            // Find all body pairs that overlap in the broadphase
            handle = input.World.CollisionWorld.Broadphase.ScheduleFindOverlapsJobs(
                out BlockStream dynamicVsDynamicBroadphasePairsStream, out BlockStream staticVsDynamicBroadphasePairsStream, handle);
            handle.Complete();  // Need to know the total number of pairs before continuing

            // Create phased dispatch pairs for all interacting body pairs
            handle = m_Scheduler.ScheduleCreatePhasedDispatchPairsJob(
                ref input.World, ref dynamicVsDynamicBroadphasePairsStream, ref staticVsDynamicBroadphasePairsStream, ref m_Context, handle);
            handle.Complete();  // Need to know the total number of work items before continuing
            handle = callbacks.Execute(SimulationCallbacks.Phase.PostCreateDispatchPairs, this, handle);
            m_Context.CreateBodyPairsHandle = handle;

            // Create contact points & joint Jacobians
            handle = NarrowPhase.ScheduleProcessBodyPairsJobs(ref input.World, input.TimeStep, input.NumSolverIterations, ref m_Context, handle);
            handle = callbacks.Execute(SimulationCallbacks.Phase.PostCreateContacts, this, handle);
            m_Context.CreateContactsHandle = handle;

            // Create contact Jacobians
            handle = Solver.ScheduleBuildContactJacobiansJobs(ref input.World.DynamicsWorld, input.TimeStep, ref m_Context, handle);
            handle = callbacks.Execute(SimulationCallbacks.Phase.PostCreateContactJacobians, this, handle);
            m_Context.CreateContactJacobiansHandle = handle;

            // Solve all Jacobians
            int numIterations = input.NumSolverIterations > 0 ? input.NumSolverIterations : 4;

            handle = Solver.ScheduleSolveJacobiansJobs(ref input.World.DynamicsWorld, input.TimeStep, input.Gravity, numIterations, ref m_Context, handle);
            handle = callbacks.Execute(SimulationCallbacks.Phase.PostSolveJacobians, this, handle);
            m_Context.SolveContactJacobiansHandle = handle;

            // Integration motions
            handle = Integrator.ScheduleIntegrateJobs(ref input.World.DynamicsWorld, input.TimeStep, input.Gravity, handle);
            handle = callbacks.Execute(SimulationCallbacks.Phase.PostIntegrateMotions, this, handle);
            m_Context.IntegrateMotionsHandle = handle;

            // Synchronize the collision world
            if (input.SynchronizeCollisionWorld)
            {
                handle = input.World.CollisionWorld.ScheduleUpdateDynamicLayer(ref input.World, input.TimeStep, input.ThreadCountHint, handle);  // TODO: timeStep = 0?
            }

            // Return the final simulation handle
            finalSimulationJobHandle = handle;

            // Return the final handle, which includes disposing temporary arrays
            finalJobHandle = JobHandle.CombineDependencies(finalSimulationJobHandle, m_Context.DisposeBroadphasePairs, m_Context.DisposeContacts);
            finalJobHandle = JobHandle.CombineDependencies(finalJobHandle, m_Context.DisposeJacobians, m_Context.DisposeJointJacobians);
            finalJobHandle = JobHandle.CombineDependencies(finalJobHandle, m_Context.DisposeSolverSchedulerData);
        }