ref IslandScaffoldTypeBatch GetOrCreateTypeBatch(int typeId, Solver solver, BufferPool <int> intPool)
{
ref var idMap = ref TypeIdToIndex[typeId];
//TODO: I wonder if people will abuse the dt-as-parameter to the point where we should make it a field instead, like it effectively was in v1.
/// <summary>
/// Performs one timestep of the given length.
/// </summary>
/// <remarks>
/// Be wary of variable timesteps. They can harm stability. Whenever possible, keep the timestep the same across multiple frames unless you have a specific reason not to.
/// </remarks>
/// <param name="dt">Duration of the time step in time.</param>
public void Timestep(float dt, IThreadDispatcher threadDispatcher = null)
{
ProfilerClear();
ProfilerStart(this);
//Note that the first behavior-affecting stage is actually the pose integrator. This is a shift from v1, where collision detection went first.
//This is a tradeoff:
//1) Any externally set velocities will be integrated without input from the solver. The v1-style external velocity control won't work as well-
//the user would instead have to change velocities after the pose integrator runs. This isn't perfect either, since the pose integrator is also responsible
//for updating the bounding boxes used for collision detection.
//2) By bundling bounding box calculation with pose integration, you avoid redundant pose and velocity memory accesses.
//3) Generated contact positions are in sync with the integrated poses.
//That's often helpful for gameplay purposes- you don't have to reinterpret contact data when creating graphical effects or positioning sound sources.
//TODO: This is something that is possibly worth exposing as one of the generic type parameters. Users could just choose the order arbitrarily.
//Or, since you're talking about something that happens once per frame instead of once per collision pair, just provide a simple callback.
//(Or maybe an enum even?)
//#1 is a difficult problem, though. There is no fully 'correct' place to change velocities. We might just have to bite the bullet and create a
//inertia tensor/bounding box update separate from pose integration. If the cache gets evicted in between (virtually guaranteed unless no stages run),
//this basically means an extra 100-200 microseconds per frame on a processor with ~20GBps bandwidth simulating 32768 bodies.
//Note that the reason why the pose integrator comes first instead of, say, the solver, is that the solver relies on world space inertias calculated by the pose integration.
//If the pose integrator doesn't run first, we either need
//1) complicated on demand updates of world inertia when objects are added or local inertias are changed or
//2) local->world inertia calculation before the solver.
ProfilerStart(PoseIntegrator);
PoseIntegrator.Update(dt, BufferPool, threadDispatcher);
ProfilerEnd(PoseIntegrator);
ProfilerStart(BroadPhase);
BroadPhase.Update(threadDispatcher);
ProfilerEnd(BroadPhase);
ProfilerStart(BroadPhaseOverlapFinder);
BroadPhaseOverlapFinder.DispatchOverlaps(threadDispatcher);
ProfilerEnd(BroadPhaseOverlapFinder);
ProfilerStart(NarrowPhase);
NarrowPhase.Flush(threadDispatcher, threadDispatcher != null && Deterministic);
ProfilerEnd(NarrowPhase);
ProfilerStart(Solver);
if (threadDispatcher == null)
{
Solver.Update(dt);
}
else
{
Solver.MultithreadedUpdate(threadDispatcher, BufferPool, dt);
}
ProfilerEnd(Solver);
//Note that constraint optimization should be performed after body optimization, since body optimization moves the bodies- and so affects the optimal constraint position.
//TODO: The order of these optimizer stages is performance relevant, even though they don't have any effect on correctness.
//You may want to try them in different locations to see how they impact cache residency.
ProfilerStart(BodyLayoutOptimizer);
if (threadDispatcher == null)
{
BodyLayoutOptimizer.IncrementalOptimize();
}
else
{
BodyLayoutOptimizer.IncrementalOptimize(BufferPool, threadDispatcher);
}
ProfilerEnd(BodyLayoutOptimizer);
ProfilerStart(ConstraintLayoutOptimizer);
ConstraintLayoutOptimizer.Update(BufferPool, threadDispatcher);
ProfilerEnd(ConstraintLayoutOptimizer);
ProfilerStart(SolverBatchCompressor);
SolverBatchCompressor.Compress(BufferPool, threadDispatcher, threadDispatcher != null && Deterministic);
ProfilerEnd(SolverBatchCompressor);
ProfilerEnd(this);
}
/// <summary>
/// Registers the set of constraints that are packaged in the engine.
/// </summary>
public static void RegisterDefaults(Solver solver, NarrowPhase narrowPhase)
{
solver.Register <BallSocket>();
solver.Register <AngularHinge>();
solver.Register <AngularSwivelHinge>();
solver.Register <SwingLimit>();
solver.Register <TwistServo>();
solver.Register <TwistLimit>();
solver.Register <TwistMotor>();
solver.Register <AngularServo>();
solver.Register <AngularMotor>();
solver.Register <Weld>();
solver.Register <VolumeConstraint>();
solver.Register <DistanceServo>();
solver.Register <DistanceLimit>();
solver.Register <CenterDistanceConstraint>();
solver.Register <AreaConstraint>();
solver.Register <PointOnLineServo>();
solver.Register <LinearAxisServo>();
solver.Register <LinearAxisMotor>();
solver.Register <LinearAxisLimit>();
solver.Register <AngularAxisMotor>();
solver.Register <OneBodyAngularServo>();
solver.Register <OneBodyAngularMotor>();
solver.Register <OneBodyLinearServo>();
solver.Register <OneBodyLinearMotor>();
solver.Register <SwivelHinge>();
solver.Register <Hinge>();
solver.Register <BallSocketMotor>();
solver.Register <BallSocketServo>();
solver.Register <Contact1OneBody>();
solver.Register <Contact2OneBody>();
solver.Register <Contact3OneBody>();
solver.Register <Contact4OneBody>();
solver.Register <Contact1>();
solver.Register <Contact2>();
solver.Register <Contact3>();
solver.Register <Contact4>();
solver.Register <Contact2NonconvexOneBody>();
solver.Register <Contact3NonconvexOneBody>();
solver.Register <Contact4NonconvexOneBody>();
//We may later use more contacts in the nonconvex manifold; for now we clamped it to 4.
solver.Register <Contact2Nonconvex>();
solver.Register <Contact3Nonconvex>();
solver.Register <Contact4Nonconvex>();
narrowPhase.RegisterContactConstraintAccessor(new NonconvexTwoBodyAccessor <Contact4Nonconvex, Contact4NonconvexPrestepData, Contact4NonconvexAccumulatedImpulses, ContactImpulses4, ConstraintCache4>());
narrowPhase.RegisterContactConstraintAccessor(new NonconvexTwoBodyAccessor <Contact3Nonconvex, Contact3NonconvexPrestepData, Contact3NonconvexAccumulatedImpulses, ContactImpulses3, ConstraintCache3>());
narrowPhase.RegisterContactConstraintAccessor(new NonconvexTwoBodyAccessor <Contact2Nonconvex, Contact2NonconvexPrestepData, Contact2NonconvexAccumulatedImpulses, ContactImpulses2, ConstraintCache2>());
narrowPhase.RegisterContactConstraintAccessor(new NonconvexOneBodyAccessor <Contact4NonconvexOneBody, Contact4NonconvexOneBodyPrestepData, Contact4NonconvexAccumulatedImpulses, ContactImpulses4, ConstraintCache4>());
narrowPhase.RegisterContactConstraintAccessor(new NonconvexOneBodyAccessor <Contact3NonconvexOneBody, Contact3NonconvexOneBodyPrestepData, Contact3NonconvexAccumulatedImpulses, ContactImpulses3, ConstraintCache3>());
narrowPhase.RegisterContactConstraintAccessor(new NonconvexOneBodyAccessor <Contact2NonconvexOneBody, Contact2NonconvexOneBodyPrestepData, Contact2NonconvexAccumulatedImpulses, ContactImpulses2, ConstraintCache2>());
narrowPhase.RegisterContactConstraintAccessor(new ConvexTwoBodyAccessor <Contact4, Contact4PrestepData, Contact4AccumulatedImpulses, ContactImpulses4, ConstraintCache4>());
narrowPhase.RegisterContactConstraintAccessor(new ConvexTwoBodyAccessor <Contact3, Contact3PrestepData, Contact3AccumulatedImpulses, ContactImpulses3, ConstraintCache3>());
narrowPhase.RegisterContactConstraintAccessor(new ConvexTwoBodyAccessor <Contact2, Contact2PrestepData, Contact2AccumulatedImpulses, ContactImpulses2, ConstraintCache2>());
narrowPhase.RegisterContactConstraintAccessor(new ConvexTwoBodyAccessor <Contact1, Contact1PrestepData, Contact1AccumulatedImpulses, ContactImpulses1, ConstraintCache1>());
narrowPhase.RegisterContactConstraintAccessor(new ConvexOneBodyAccessor <Contact4OneBody, Contact4OneBodyPrestepData, Contact4AccumulatedImpulses, ContactImpulses4, ConstraintCache4>());
narrowPhase.RegisterContactConstraintAccessor(new ConvexOneBodyAccessor <Contact3OneBody, Contact3OneBodyPrestepData, Contact3AccumulatedImpulses, ContactImpulses3, ConstraintCache3>());
narrowPhase.RegisterContactConstraintAccessor(new ConvexOneBodyAccessor <Contact2OneBody, Contact2OneBodyPrestepData, Contact2AccumulatedImpulses, ContactImpulses2, ConstraintCache2>());
narrowPhase.RegisterContactConstraintAccessor(new ConvexOneBodyAccessor <Contact1OneBody, Contact1OneBodyPrestepData, Contact1AccumulatedImpulses, ContactImpulses1, ConstraintCache1>());
}
public void Execute(Solver solver, int blockIndex)
{
ref var block = ref solver.context.ConstraintBlocks.Blocks[blockIndex];
/// <summary>
/// Registers the set of constraints that are packaged in the engine.
/// </summary>
public static void RegisterDefaults(Solver solver, NarrowPhase narrowPhase)
{
solver.Register <BallSocket>();
solver.Register <AngularHinge>();
solver.Register <AngularSwivelHinge>();
solver.Register <SwingLimit>();
solver.Register <TwistServo>();
solver.Register <TwistLimit>();
solver.Register <TwistMotor>();
solver.Register <AngularServo>();
solver.Register <AngularMotor>();
solver.Register <Weld>();
solver.Register <VolumeConstraint>();
solver.Register <DistanceServo>();
solver.Register <DistanceLimit>();
solver.Register <CenterDistanceConstraint>();
solver.Register <AreaConstraint>();
solver.Register <PointOnLineServo>();
solver.Register <LinearAxisServo>();
solver.Register <LinearAxisMotor>();
solver.Register <LinearAxisLimit>();
solver.Register <AngularAxisMotor>();
solver.Register <OneBodyAngularServo>();
solver.Register <OneBodyAngularMotor>();
solver.Register <OneBodyLinearServo>();
solver.Register <OneBodyLinearMotor>();
solver.Register <SwivelHinge>();
solver.Register <Hinge>();
solver.Register <Contact1OneBody>();
solver.Register <Contact2OneBody>();
solver.Register <Contact3OneBody>();
solver.Register <Contact4OneBody>();
solver.Register <Contact1>();
solver.Register <Contact2>();
solver.Register <Contact3>();
solver.Register <Contact4>();
solver.Register <Contact2NonconvexOneBody>();
solver.Register <Contact3NonconvexOneBody>();
solver.Register <Contact4NonconvexOneBody>();
solver.Register <Contact5NonconvexOneBody>();
solver.Register <Contact6NonconvexOneBody>();
solver.Register <Contact7NonconvexOneBody>();
solver.Register <Contact8NonconvexOneBody>();
solver.Register <Contact2Nonconvex>();
solver.Register <Contact3Nonconvex>();
solver.Register <Contact4Nonconvex>();
solver.Register <Contact5Nonconvex>();
solver.Register <Contact6Nonconvex>();
solver.Register <Contact7Nonconvex>();
solver.Register <Contact8Nonconvex>();
narrowPhase.RegisterContactConstraintAccessor(new NonconvexTwoBodyAccessor <Contact8Nonconvex, Contact8NonconvexAccumulatedImpulses, ContactImpulses8, ConstraintCache8>());
narrowPhase.RegisterContactConstraintAccessor(new NonconvexTwoBodyAccessor <Contact7Nonconvex, Contact7NonconvexAccumulatedImpulses, ContactImpulses7, ConstraintCache7>());
narrowPhase.RegisterContactConstraintAccessor(new NonconvexTwoBodyAccessor <Contact6Nonconvex, Contact6NonconvexAccumulatedImpulses, ContactImpulses6, ConstraintCache6>());
narrowPhase.RegisterContactConstraintAccessor(new NonconvexTwoBodyAccessor <Contact5Nonconvex, Contact5NonconvexAccumulatedImpulses, ContactImpulses5, ConstraintCache5>());
narrowPhase.RegisterContactConstraintAccessor(new NonconvexTwoBodyAccessor <Contact4Nonconvex, Contact4NonconvexAccumulatedImpulses, ContactImpulses4, ConstraintCache4>());
narrowPhase.RegisterContactConstraintAccessor(new NonconvexTwoBodyAccessor <Contact3Nonconvex, Contact3NonconvexAccumulatedImpulses, ContactImpulses3, ConstraintCache3>());
narrowPhase.RegisterContactConstraintAccessor(new NonconvexTwoBodyAccessor <Contact2Nonconvex, Contact2NonconvexAccumulatedImpulses, ContactImpulses2, ConstraintCache2>());
narrowPhase.RegisterContactConstraintAccessor(new NonconvexOneBodyAccessor <Contact8NonconvexOneBody, Contact8NonconvexAccumulatedImpulses, ContactImpulses8, ConstraintCache8>());
narrowPhase.RegisterContactConstraintAccessor(new NonconvexOneBodyAccessor <Contact7NonconvexOneBody, Contact7NonconvexAccumulatedImpulses, ContactImpulses7, ConstraintCache7>());
narrowPhase.RegisterContactConstraintAccessor(new NonconvexOneBodyAccessor <Contact6NonconvexOneBody, Contact6NonconvexAccumulatedImpulses, ContactImpulses6, ConstraintCache6>());
narrowPhase.RegisterContactConstraintAccessor(new NonconvexOneBodyAccessor <Contact5NonconvexOneBody, Contact5NonconvexAccumulatedImpulses, ContactImpulses5, ConstraintCache5>());
narrowPhase.RegisterContactConstraintAccessor(new NonconvexOneBodyAccessor <Contact4NonconvexOneBody, Contact4NonconvexAccumulatedImpulses, ContactImpulses4, ConstraintCache4>());
narrowPhase.RegisterContactConstraintAccessor(new NonconvexOneBodyAccessor <Contact3NonconvexOneBody, Contact3NonconvexAccumulatedImpulses, ContactImpulses3, ConstraintCache3>());
narrowPhase.RegisterContactConstraintAccessor(new NonconvexOneBodyAccessor <Contact2NonconvexOneBody, Contact2NonconvexAccumulatedImpulses, ContactImpulses2, ConstraintCache2>());
narrowPhase.RegisterContactConstraintAccessor(new ConvexTwoBodyAccessor <Contact4, Contact4AccumulatedImpulses, ContactImpulses4, ConstraintCache4>());
narrowPhase.RegisterContactConstraintAccessor(new ConvexTwoBodyAccessor <Contact3, Contact3AccumulatedImpulses, ContactImpulses3, ConstraintCache3>());
narrowPhase.RegisterContactConstraintAccessor(new ConvexTwoBodyAccessor <Contact2, Contact2AccumulatedImpulses, ContactImpulses2, ConstraintCache2>());
narrowPhase.RegisterContactConstraintAccessor(new ConvexTwoBodyAccessor <Contact1, Contact1AccumulatedImpulses, ContactImpulses1, ConstraintCache1>());
narrowPhase.RegisterContactConstraintAccessor(new ConvexOneBodyAccessor <Contact4OneBody, Contact4AccumulatedImpulses, ContactImpulses4, ConstraintCache4>());
narrowPhase.RegisterContactConstraintAccessor(new ConvexOneBodyAccessor <Contact3OneBody, Contact3AccumulatedImpulses, ContactImpulses3, ConstraintCache3>());
narrowPhase.RegisterContactConstraintAccessor(new ConvexOneBodyAccessor <Contact2OneBody, Contact2AccumulatedImpulses, ContactImpulses2, ConstraintCache2>());
narrowPhase.RegisterContactConstraintAccessor(new ConvexOneBodyAccessor <Contact1OneBody, Contact1AccumulatedImpulses, ContactImpulses1, ConstraintCache1>());
}
//There is some roughly-duplicate logic here, compared to the 'real' constraint batch.
public unsafe static bool TryAdd(void *batch, int constraintHandle, Solver solver, BufferPool pool)
{
ref var constraintLocation = ref solver.HandleToConstraint[constraintHandle];
