public static void Test()
{
var threadDispatcher = new SimpleThreadDispatcher(Environment.ProcessorCount);
const int flagCount = 65536;
const int jobsPerThread = 4;
var jobCount = threadDispatcher.ThreadCount * jobsPerThread;
jobs = new Job[jobCount];
var previousEnd = 0;
var baseJobIndexCount = flagCount / jobCount;
var remainder = flagCount - baseJobIndexCount * jobCount;
for (int i = 0; i < jobCount; ++i)
{
var jobIndexCount = i < remainder ? baseJobIndexCount + 1 : baseJobIndexCount;
jobs[i] = new Job {
Start = previousEnd, End = previousEnd += jobIndexCount
};
}
var pool = new BufferPool();
pool.SpecializeFor <int>().Take(flagCount, out accessIndices);
for (int i = 0; i < flagCount; ++i)
{
accessIndices[i] = i;
}
var random = new Random(5);
for (int i = 0; i < flagCount - 1; ++i)
{
ref var a = ref accessIndices[i];
ref var b = ref accessIndices[random.Next(i + 1, flagCount)];
public override void Initialize()
{
BufferPool = new BufferPool();
//Generally, shoving as many threads as possible into the simulation won't produce the best results on systems with multiple logical cores per physical core.
//Environment.ProcessorCount reports logical core count only, so we'll use a simple heuristic here- it'll leave one or two logical cores idle.
//For the common Intel quad core with hyperthreading, this'll use six logical cores and leave two logical cores free to be used for other stuff.
//This is by no means perfect. To maximize performance, you'll need to profile your simulation and target hardware.
//Note that issues can be magnified on older operating systems like Windows 7 if all logical cores are given work.
//Generally, the more memory bandwidth you have relative to CPU compute throughput, and the more collision detection heavy the simulation is relative to solving,
//the more benefit you get out of SMT/hyperthreading.
//For example, if you're using the 64 core quad memory channel AMD 3990x on a scene composed of thousands of ragdolls,
//there won't be enough memory bandwidth to even feed half the physical cores. Using all 128 logical cores would just add overhead.
//It may be worth using something like hwloc to extract extra information to reason about.
var targetThreadCount = Math.Max(1,
Environment.ProcessorCount > 4 ? Environment.ProcessorCount - 2 : Environment.ProcessorCount - 1);
ThreadDispatcher = new SimpleThreadDispatcher(targetThreadCount);
var size = GraphicsDevice.Viewport.Bounds.Size;
size.X /= 2;
size.Y /= 2;
Camera = new FreeCamera(GraphicsDevice.Viewport.AspectRatio, new Vector3(0, 40, 200), size);
base.Initialize();
}
private void LoadPhysics()
{
//Physics
BufferPool = new BufferPool();
Radii = new List <float>();
SphereHandles = new List <BodyHandle>();
var targetThreadCount = Math.Max(1,
Environment.ProcessorCount > 4 ? Environment.ProcessorCount - 2 : Environment.ProcessorCount - 1);
ThreadDispatcher = new SimpleThreadDispatcher(targetThreadCount);
Simulation = Simulation.Create(BufferPool, new NarrowPhaseCallbacks(),
new PoseIntegratorCallbacks(new NumericVector3(0, -500, 0)), new PositionFirstTimestepper());
// Simulation.Statics.Add(new StaticDescription(new NumericVector3(0, 0, 0),
//new CollidableDescription(Simulation.Shapes.Add(new Box(2000, 100, 5000)), 1)));
// for (int i=0;i<MatrixWorld.Count;i++)
// {
// Matrix world = MatrixWorld[i];
// Simulation.Statics.Add(new StaticDescription(new NumericVector3(world.Translation.X, world.Translation.Y, world.Translation.Z),
// new CollidableDescription(Simulation.Shapes.Add(new Box(200, 100, 500)), 1)));
// }
//Simulation.Statics.Add(new StaticDescription(new NumericVector3(0, -20, 0),
// new CollidableDescription(Simulation.Shapes.Add(new Box(200, 100, 500)), 1)));
//Simulation.Statics.Add(new StaticDescription(new NumericVector3(9, 28, 200), new CollidableDescription(Simulation.Shapes.Add(new Sphere(2f)),1)));
//Simulation.Statics.Add(new StaticDescription(new ))
/* for (int i = 0; i < MatrixWorld.Count(); i++)
* {
* Simulation.Statics.Add(new StaticDescription(MatrixWorld[i].))
* Floor.Draw(MatrixWorld[i], Camera.View, Camera.Projection);
* }*/
//Simulation.Statics.Add(new StaticDescription(new NumericVector3(0, -20, 0), new CollidableDescription(Simulation.Shapes.Add(
// new Box(2000, 100, 2000)), 1)));
//Esfera
SpheresWorld = new List <Matrix>();
var radius = 5f;
var sphereShape = new Sphere(radius);
var position = Vector3Utils.toNumeric(PlayerInitialPosition);
var bodyDescription = BodyDescription.CreateConvexDynamic(position, 1 / radius * radius * radius,
Simulation.Shapes, sphereShape);
var bodyHandle = Simulation.Bodies.Add(bodyDescription);
SphereHandles.Add(bodyHandle);
Radii.Add(radius);
}
static void Subtest <TBodyBuilder, TConstraintBuilder, TConstraint>(TBodyBuilder bodyBuilder, TConstraintBuilder constraintBuilder,
int width, int height, int length, int frameCount, IThreadDispatcher initializationThreadPool, StreamWriter writer)
where TBodyBuilder : IBodyBuilder where TConstraintBuilder : IConstraintBuilder where TConstraint : IConstraintDescription <TConstraint>
{
const int testsPerVariant = 8;
WriteLine(writer, $"{width}x{height}x{length} lattice, {frameCount} frames:");
var timings = new SimulationTimeSamples[Environment.ProcessorCount];
//for (int threadCount = 1; threadCount <= 1; ++threadCount)
for (int threadCount = 1; threadCount <= Environment.ProcessorCount; ++threadCount)
{
var threadPool = new SimpleThreadDispatcher(threadCount);
SimulationTimeSamples bestTimings = null;
double bestTime = double.MaxValue;
for (int i = 0; i < testsPerVariant; ++i)
{
var candidateTimings = Solve <TBodyBuilder, TConstraintBuilder, TConstraint>(bodyBuilder, constraintBuilder, width, height, length,
frameCount, threadCount, initializationThreadPool, threadPool);
var totalStats = candidateTimings.Simulation.ComputeStats();
WriteLine(writer,
$"{i} AVE: {Math.Round(1e3 * totalStats.Average, 2)}, " +
$"MIN: {Math.Round(1e3 * totalStats.Min, 2)}, " +
$"MAX: {Math.Round(1e3 * totalStats.Max, 2)}, " +
$"STD DEV: {Math.Round(1e3 * totalStats.StdDev, 3)}, ");
if (totalStats.Total < bestTime)
{
bestTime = totalStats.Total;
bestTimings = candidateTimings;
}
}
WriteLine(writer, $"{threadCount}T: " +
$"{Math.Round(bestTime * 1e3, 2)}, " +
$"BOPT: {Math.Round(bestTimings.BodyOptimizer.ComputeStats().Total * 1e3, 2)}, " +
$"COPT: {Math.Round(bestTimings.ConstraintOptimizer.ComputeStats().Total * 1e3, 2)}, " +
$"BCMP: {Math.Round(bestTimings.BatchCompressor.ComputeStats().Total * 1e3, 2)}, " +
$"POIN: {Math.Round(bestTimings.PoseIntegrator.ComputeStats().Total * 1e3, 2)}, " +
$"SOLV: {Math.Round(bestTimings.Solver.ComputeStats().Total * 1e3, 2)}");
timings[threadCount - 1] = bestTimings;
threadPool.Dispose();
}
int fastestIndex = 0;
for (int i = 1; i < timings.Length; ++i)
{
if (timings[i].Simulation.ComputeStats().Total < timings[fastestIndex].Simulation.ComputeStats().Total)
{
fastestIndex = i;
}
}
WriteLine(writer, $"Scaling: {timings[0].Simulation.ComputeStats().Total / timings[fastestIndex].Simulation.ComputeStats().Total}");
}
public void Init()
{
bufferPool = new BufferPool();
var collider = new BodyProperty <Collider>();
simulation = Simulation.Create(bufferPool, new CarCallbacks {
Collider = collider
}, new DemoPoseIntegratorCallbacks(new System.Numerics.Vector3(0, -10, 0)));
Simulation.Statics.Add(new StaticDescription(new System.Numerics.Vector3(0f, 0f, 0f), new CollidableDescription(Simulation.Shapes.Add(new Box(30, 1, 30)), 0.04f)));
threadDispatcher = new SimpleThreadDispatcher(Environment.ProcessorCount);
(new Thread(PoolThread)).Start();
}
public static void Test()
{
const int iterations = 100000000;
{
int accumulator = 0;
var start = Stopwatch.GetTimestamp();
for (int i = 0; i < iterations; ++i)
{
Interlocked.Increment(ref accumulator);
}
var end = Stopwatch.GetTimestamp();
Console.WriteLine($"Uncontested 1T increment (ns): {1e9 * (end - start) / (iterations * Stopwatch.Frequency)}");
}
var threadPool = new SimpleThreadDispatcher(Environment.ProcessorCount);
{
var start = Stopwatch.GetTimestamp();
threadPool.DispatchWorkers(workerIndex =>
{
while (true)
{
if (Interlocked.Increment(ref contestedAccumulator) >= iterations)
{
break;
}
}
});
var end = Stopwatch.GetTimestamp();
Console.WriteLine($"Contested {threadPool.ThreadCount}T increment (ns): {1e9 * (end - start) / (iterations * Stopwatch.Frequency)}");
}
{
uncontestedAccumulators = new int[Environment.ProcessorCount * 32];
var start = Stopwatch.GetTimestamp();
threadPool.DispatchWorkers(workerIndex =>
{
var offset = workerIndex * 32;
var exit = iterations / threadPool.ThreadCount;
while (true)
{
if (Interlocked.Increment(ref uncontestedAccumulators[offset]) >= exit)
{
break;
}
}
});
var end = Stopwatch.GetTimestamp();
Console.WriteLine($"Uncontested {threadPool.ThreadCount}T increment (ns): {1e9 * (end - start) / (iterations * Stopwatch.Frequency)}");
}
}
protected void InitializePhysics()
{
if (isInitialized)
{
return;
}
SetAddedBodies(new List <PhysicBodyData>());
SetPhysicUpdates(new List <BodyUpdateData>());
SetAddedStaticBodies(new List <PhysicBodyData>());
SetStaticBodiesData(new List <BodyUpdateData>());
SetDynamicBodiesToRemoveID(new List <int>());
SetStaticBodiesToRemoveID(new List <int>());
isInitialized = true;
BufferPool = new BufferPool();
ThreadDispatcher = new SimpleThreadDispatcher(Environment.ProcessorCount);
Initialize();
}
public void Execute(ref QuickList <BoundingBox> boxes, SimpleThreadDispatcher dispatcher)
{
CacheBlaster.Blast();
JobIndex = -1;
IntersectionCount = 0;
var start = Stopwatch.GetTimestamp();
if (dispatcher != null)
{
dispatcher.DispatchWorkers(internalWorker);
}
else
{
internalWorker(0);
}
var stop = Stopwatch.GetTimestamp();
Timings.Add((stop - start) / (double)Stopwatch.Frequency);
}
public static void Init()
{
//The buffer pool is a source of raw memory blobs for the engine to use.
pool = new BufferPool();
//Note that you can also control the order of internal stage execution using a different ITimestepper implementation.
//For the purposes of this demo, we just use the default by passing in nothing (which happens to be PositionFirstTimestepper at the time of writing).
sim = Simulation.Create(pool, new NarrowPhaseCallbacks(), new PoseIntegratorCallbacks(new Vector3(0, -10, 0)));
physicsBodies = new List <RigidBody>();
thread = new SimpleThreadDispatcher(Environment.ProcessorCount);
//Drop a ball on a big static box.
var sphere = new Box(10, 1, 10);
sphere.ComputeInertia(1, out var sphereInertia);
sim.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 5, 0), sphereInertia,
new CollidableDescription(sim.Shapes.Add(sphere), 0.1f), new BodyActivityDescription(0.01f)));
sim.Statics.Add(new StaticDescription(new Vector3(0, -1, 0), new CollidableDescription(sim.Shapes.Add(new Box(50, 1, 50)), 0.1f)));
}
public static void Test()
{
var memoryStream = new MemoryStream();
var writer = new StreamWriter(memoryStream);
var initializationThreadPool = new SimpleThreadDispatcher(Environment.ProcessorCount);
var bodyBuilder = new RegularGridWithKinematicBaseBuilder(new Vector3(1), new Vector3());
var constraintBuilder = new ContactManifoldConstraintBuilder();
//Subtest<RegularGridWithKinematicBaseBuilder, ContactManifoldConstraintBuilder, ContactManifold4Constraint>(bodyBuilder, constraintBuilder, 32, 32, 32, 8, initializationThreadPool, writer);
//Subtest<RegularGridWithKinematicBaseBuilder, ContactManifoldConstraintBuilder, ContactManifold4Constraint>(bodyBuilder, constraintBuilder, 26, 26, 26, 12, initializationThreadPool, writer);
//Subtest<RegularGridWithKinematicBaseBuilder, ContactManifoldConstraintBuilder, ContactManifold4Constraint>(bodyBuilder, constraintBuilder, 20, 20, 20, 20, initializationThreadPool, writer);
//Subtest<RegularGridWithKinematicBaseBuilder, ContactManifoldConstraintBuilder, ContactManifold4Constraint>(bodyBuilder, constraintBuilder, 16, 16, 16, 30, initializationThreadPool, writer);
//Subtest<RegularGridWithKinematicBaseBuilder, ContactManifoldConstraintBuilder, ContactManifold4Constraint>(bodyBuilder, constraintBuilder, 13, 13, 13, 45, initializationThreadPool, writer);
Subtest <RegularGridWithKinematicBaseBuilder, ContactManifoldConstraintBuilder, Contact4>(bodyBuilder, constraintBuilder, 10, 10, 10, 700, initializationThreadPool, writer);
//var bodyBuilder = new RegularGridWithKinematicBaseBuilder(new Vector3(1), new Vector3());
//var constraintBuilder = new BallSocketConstraintBuilder();
//Subtest<RegularGridWithKinematicBaseBuilder, BallSocketConstraintBuilder, BallSocket>(bodyBuilder, constraintBuilder, 32, 32, 32, 8, initializationThreadPool, writer);
//Subtest<RegularGridWithKinematicBaseBuilder, BallSocketConstraintBuilder, BallSocket>(bodyBuilder, constraintBuilder, 26, 26, 26, 12, initializationThreadPool, writer);
//Subtest<RegularGridWithKinematicBaseBuilder, BallSocketConstraintBuilder, BallSocket>(bodyBuilder, constraintBuilder, 20, 20, 20, 20, initializationThreadPool, writer);
//Subtest<RegularGridWithKinematicBaseBuilder, BallSocketConstraintBuilder, BallSocket>(bodyBuilder, constraintBuilder, 16, 16, 16, 30, initializationThreadPool, writer);
//Subtest<RegularGridWithKinematicBaseBuilder, BallSocketConstraintBuilder, BallSocket>(bodyBuilder, constraintBuilder, 13, 13, 13, 45, initializationThreadPool, writer);
//Subtest<RegularGridWithKinematicBaseBuilder, BallSocketConstraintBuilder, BallSocket>(bodyBuilder, constraintBuilder, 10, 10, 10, 70, initializationThreadPool, writer);
initializationThreadPool.Dispose();
writer.Flush();
var path = "log.txt";
using (var stream = File.OpenWrite(path))
{
Console.WriteLine($"Writing results to path: {Path.GetFullPath(path)}");
var bytes = memoryStream.ToArray();
stream.Write(bytes, 0, bytes.Length);
}
Console.ReadKey();
}
public static void Test()
{
//const int bodyCount = 8;
//SimulationSetup.BuildStackOfBodiesOnGround(bodyCount, false, true, out var bodies, out var solver, out var graph, out var bodyHandles, out var constraintHandles);
SimulationSetup.BuildLattice(
new RegularGridWithKinematicBaseBuilder(new Vector3(1), new Vector3()),
new ContactManifoldConstraintBuilder(),
32, 32, 32, out var simulation, out var bodyHandles, out var constraintHandles);
SimulationScrambling.ScrambleBodies(simulation);
SimulationScrambling.ScrambleConstraints(simulation.Solver);
SimulationScrambling.ScrambleBodyConstraintLists(simulation);
SimulationScrambling.AddRemoveChurn <Contact4>(simulation, 100000, bodyHandles, constraintHandles);
var threadDispatcher = new SimpleThreadDispatcher(8);
//var threadDispatcher = new NotQuiteAThreadDispatcher(8);
simulation.ConstraintLayoutOptimizer.OptimizationFraction = 0.01f;
int constraintOptimizationIterations = 8192;
simulation.ConstraintLayoutOptimizer.Update(simulation.BufferPool, threadDispatcher);//prejit
var timer = Stopwatch.StartNew();
for (int i = 0; i < constraintOptimizationIterations; ++i)
{
simulation.ConstraintLayoutOptimizer.Update(simulation.BufferPool, threadDispatcher);
}
timer.Stop();
Console.WriteLine($"Finished constraint optimizations, time (ms): {timer.Elapsed.TotalMilliseconds}" +
$", per iteration (us): {timer.Elapsed.TotalSeconds * 1e6 / constraintOptimizationIterations}");
//threadDispatcher.Dispose();
simulation.BufferPool.Clear();
}
public void Reset(CameraInitialState camera)
{
Models.Clear();
Textures.Clear();
camera.ModelView = Matrix4x4.CreateLookAt(new Vector3(-10, 5, 12) * 2, new Vector3(-5, 5, 0), new Vector3(0, 1, 0));
camera.FieldOfView = 40;
camera.Aperture = 0.0f;
camera.FocusDistance = 10.0f;
camera.ControlSpeed = 5.0f;
camera.GammaCorrection = true;
camera.SkyColor1 = new Vector4(.6f, .7f, .9f, 1);
camera.SkyColor2 = new Vector4(.6f, .7f, .9f, 1);
// Initialize physics
_bufferPool = new BufferPool();
var targetThreadCount = Math.Max(1, Environment.ProcessorCount > 4 ? Environment.ProcessorCount - 2 : Environment.ProcessorCount - 1);
_threadDispatcher = new SimpleThreadDispatcher(targetThreadCount);
_simulation = Simulation.Create(_bufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -9.8f, 0)), new PositionFirstTimestepper());
var random = new Random(42);
// Add a floor
const float groundDim = 400;
Textures.Add(Texture.LoadTexture(@"./assets/textures/laminate.jpg"));
var floor = Model.CreateGroundRect(new Vector3(0, 0, 0), groundDim, groundDim, Material.Dielectric(5.5f, Textures.Count - 1), groundDim / 16f);
Models.Add(floor);
_simulation.Statics.Add(new StaticDescription(new Vector3(0, -.5f, 0), new CollidableDescription(_simulation.Shapes.Add(new Box(groundDim, 1, groundDim)), 0.1f)));
// Create pyramid
const float boxDim = 1;
const float marbleRadius = boxDim / 2f;
var boxShape = new Box(boxDim, boxDim, boxDim);
boxShape.ComputeInertia(.1f, out var boxInertia);
var boxIndex = _simulation.Shapes.Add(boxShape);
var marbleShape = new BepuPhysics.Collidables.Sphere(marbleRadius);
var boxTemplate = Model.CreateBox(new Vector3(-(boxDim / 2)), new Vector3(boxDim / 2), default);
const int pyramidSize = 20;
const float offset = -(pyramidSize / 2f) * boxDim;
for (int y = 0; ; y++)
{
int startIndex = y;
int endIndex = pyramidSize - y;
if (startIndex >= endIndex)
{
break;
}
for (int x = startIndex; x < endIndex; x++)
{
for (int z = startIndex; z < endIndex; z++)
{
var box = boxTemplate.Clone();
box.SetMaterial(Material.Lambertian(new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()) / 1f));
box.Transform = Matrix4x4.CreateTranslation(new Vector3(x * boxDim + offset, y * boxDim + (boxDim / 2), z * boxDim + offset));
Models.Add(box);
var quat = box.Transform.ToQuaternion();
var bodyHandle = _simulation.Bodies.Add(
BodyDescription.CreateDynamic(
new RigidPose {
Position = box.Transform.Translation, Orientation = new Quaternion(quat.X, quat.Y, quat.Z, quat.W)
},
boxInertia,
new CollidableDescription(boxIndex, 0.1f),
new BodyActivityDescription(0.01f)));
_bodyHandleToModelIndex[bodyHandle] = Models.Count - 1;
// Put some marbles on top of the pyramid
if ((x == startIndex || x == endIndex - 1) ||
(z == startIndex || z == endIndex - 1))
{
Models.Add(Model.CreateSphere(default, marbleRadius, Material.Metallic(
public override void InitializePhysics(Simulation simulation, BufferPool bufferPool, SimpleThreadDispatcher threadDispatcher)
{
/*var boxShape = new Box(0.5f, 0.5f, 2.5f);
* boxShape.ComputeInertia(1, out var boxLocalInertia);
* var boxDescription = new BodyDescription
* {
* Activity = new BodyActivityDescription { MinimumTimestepCountUnderThreshold = 32, SleepThreshold = -0.01f },
* LocalInertia = boxLocalInertia,
* Pose = new RigidPose
* {
* Orientation = BepuUtilities.Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 0),
* Position = new Vector3(1, -0.5f, 0)
* },
* Collidable = new CollidableDescription { SpeculativeMargin = 50.1f, Shape = simulation.Shapes.Add(boxShape) }
* };
* simulation.Bodies.Add(boxDescription);
*
* simulation.Statics.Add(new StaticDescription(new Vector3(0, -3, 0), new CollidableDescription { SpeculativeMargin = 0.1f, Shape = simulation.Shapes.Add(new Box(4, 1, 4)) }));*/
var staticShape = new Box(300, 1, 300);
var staticShapeIndex = simulation.Shapes.Add(staticShape);
var staticDescription = new StaticDescription
{
Collidable = new CollidableDescription
{
Continuity = new ContinuousDetectionSettings {
Mode = ContinuousDetectionMode.Discrete
},
Shape = staticShapeIndex,
SpeculativeMargin = 0.1f
},
Pose = new RigidPose
{
Position = new Vector3(0, -0.5f, 0),
Orientation = BepuUtilities.Quaternion.Identity
}
};
simulation.Statics.Add(staticDescription);
}
/// <inheritdoc />
protected override void LoadContent()
{
Random = new Random(5);
SpriteFont = Game.Content.Load <SpriteFont>(ContentFolderSpriteFonts + "Arial");
//The buffer pool is a source of raw memory blobs for the engine to use.
BufferPool = new BufferPool();
Radii = new List <float>();
Camera = new SimpleCamera(GraphicsDevice.Viewport.AspectRatio, new Vector3(40, 60, 150), 55, 0.4f);
var boxTexture = Game.Content.Load <Texture2D>(ContentFolderTextures + "wood/caja-madera-3");
Box = new BoxPrimitive(GraphicsDevice, Vector3.One * 10, boxTexture);
Sphere = new SpherePrimitive(GraphicsDevice);
SphereHandles = new List <BodyHandle>();
BoxHandles = new List <BodyHandle>();
var targetThreadCount = Math.Max(1,
Environment.ProcessorCount > 4 ? Environment.ProcessorCount - 2 : Environment.ProcessorCount - 1);
ThreadDispatcher = new SimpleThreadDispatcher(targetThreadCount);
// This are meshes/model/primitives collections to render
// The PositionFirstTimestepper is the simplest timestepping mode, but since it integrates velocity into position at the start of the frame, directly modified velocities outside of the timestep
// will be integrated before collision detection or the solver has a chance to intervene. That's fine in this demo. Other built-in options include the PositionLastTimestepper and the SubsteppingTimestepper.
// Note that the timestepper also has callbacks that you can use for executing logic between processing stages, like BeforeCollisionDetection.
Simulation = Simulation.Create(BufferPool, new NarrowPhaseCallbacks(),
new PoseIntegratorCallbacks(new NumericVector3(0, -100, 0)), new PositionFirstTimestepper());
// Creates a floor
var floorTexture = Game.Content.Load <Texture2D>(ContentFolderTextures + "floor/adoquin-2");
//Floor = new QuadPrimitive(GraphicsDevice);
TilingEffect = Game.Content.Load <Effect>(ContentFolderEffects + "TextureTiling");
TilingEffect.Parameters["Texture"].SetValue(floorTexture);
TilingEffect.Parameters["Tiling"].SetValue(Vector2.One * 50f);
FloorWorld = Matrix.CreateScale(400f) * Matrix.CreateTranslation(new Vector3(75, 0, -150));
Simulation.Statics.Add(new StaticDescription(new NumericVector3(0, -0.5f, 0),
new CollidableDescription(Simulation.Shapes.Add(new Box(2000, 1, 2000)), 0.1f)));
BoxesWorld = new List <Matrix>();
SpheresWorld = new List <Matrix>();
// Single Box
var radius = 10;
for (var j = 0; j < 5; j++)
{
for (var i = 0; i < 20; i++)
{
var boxShape = new Box(radius, radius, radius);
boxShape.ComputeInertia(0.4f, out var boxInertia);
var boxIndex = Simulation.Shapes.Add(boxShape);
var position = new NumericVector3(-30 + i * 10 + 1, j * 10 + 1, -40);
var bodyDescription = BodyDescription.CreateDynamic(position, boxInertia,
new CollidableDescription(boxIndex, 0.1f), new BodyActivityDescription(0.01f));
var bodyHandle = Simulation.Bodies.Add(bodyDescription);
BoxHandles.Add(bodyHandle);
}
}
base.LoadContent();
}
public static void Test()
{
const int width = 32;
const int height = 32;
const int length = 32;
SimulationSetup.BuildLattice(
new RegularGridWithKinematicBaseBuilder(new Vector3(1), new Vector3()),
new BallSocketConstraintBuilder(),
width, height, length, out var simulation, out var bodyHandles, out var constraintHandles);
var threadDispatcher = new SimpleThreadDispatcher(8);
const float inverseDt = 60f;
const float dt = 1 / inverseDt;
const int iterationCount = 8;
const int frameCount = 128;
simulation.Solver.IterationCount = iterationCount;
simulation.PoseIntegrator.Gravity = new Vector3(0, -10, 0);
var samples = new SimulationTimeSamples(frameCount);
for (int frameIndex = 0; frameIndex < frameCount; ++frameIndex)
{
var energyBefore = TestHelpers.GetBodyEnergyHeuristic(simulation.Bodies);
//simulation.Timestep(dt);
simulation.Timestep(dt, threadDispatcher);
samples.RecordFrame(simulation);
var energyAfter = TestHelpers.GetBodyEnergyHeuristic(simulation.Bodies);
int sampledBodyIndex = width;
var samplePose = simulation.Bodies.ActiveSet.Poses[sampledBodyIndex];
var sampleVelocity = simulation.Bodies.ActiveSet.Velocities[sampledBodyIndex];
//for (int i =0; i < simulation.Bodies.BodyCount; ++i)
//{
// simulation.Bodies.GetPose(simulation.Bodies.IndexToHandle[i], out var pose);
// simulation.Bodies.GetVelocity(simulation.Bodies.IndexToHandle[i], out var velocity);
// Console.WriteLine($"Sample {i} position: {pose.Position}, velocity: {velocity.Linear}");
//}
Console.WriteLine($"Sample {sampledBodyIndex} position: {samplePose.Position}, velocity: {sampleVelocity.Linear}");
Console.WriteLine($"Body energy {frameIndex}: {energyAfter}, delta: {energyAfter - energyBefore}");
}
var multiplier = 1e3 / frameCount;
Console.WriteLine($"Simulation time (ms): {multiplier * samples.Simulation.ComputeStats().Total}");
Console.WriteLine($"Body opt time (ms): {multiplier * samples.BodyOptimizer.ComputeStats().Total}");
Console.WriteLine($"Constraint opt time (ms): {multiplier * samples.ConstraintOptimizer.ComputeStats().Total}");
Console.WriteLine($"Batch compress time (ms): {multiplier * samples.BatchCompressor.ComputeStats().Total}");
Console.WriteLine($"Pose integrate time (ms): {multiplier * samples.PoseIntegrator.ComputeStats().Total}");
Console.WriteLine($"Solve time (ms): {multiplier * samples.Solver.ComputeStats().Total}");
threadDispatcher.Dispose();
simulation.BufferPool.Clear();
}
//Constructor which sets up the whole game world scene
public GameWorld(GameLoop Loop, ContentArchive Content)
{
//Store references from the GameLoop class
ApplicationWindow = Loop.Window;
UserInput = Loop.Input;
//Assign the camera reference
ObservationCamera.SetCamera(Loop);
this.Content = Content;
TimeSamples = new SimulationTimeSamples(512, Loop.Pool);
UserControls = Controls.Default;
//Load font from the content archive
var FontContent = Content.Load <FontContent>(@"Carlito-Regular.ttf");
UIFont = new Font(Loop.Surface.Device, Loop.Surface.Context, FontContent);
//Position the camera
ObservationCamera.PositionCamera(new Vector3(2.7f, 6.48f, 9.76f));
ObservationCamera.FaceCamera(0.269f, 0.15899f);
//Setup character controller and world simulation
BufferPool = new BufferPool();
ThreadDispatcher = new SimpleThreadDispatcher(Environment.ProcessorCount);
World = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(new CharacterControllers(BufferPool)), new ScenePoseIntegratorCallbacks(new Vector3(0, -10, 0)));
//Initialize the mesh loader
MeshManager.Initialize(Content, BufferPool);
//Load in the numbers display the direction of the X/Z axes
AxisMarkers.LoadModels();
AxisMarkers.AddModels(World);
//Load in the PVP arena
ArenaMeshHandle = MeshManager.LoadMesh(@"Arena.obj", new Vector3(1));
//Define its starting position and location
Vector3 ArenaPosition = new Vector3(29.82f, 3.62f, 0.94f);
Quaternion ArenaRotation = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), Trig.DegreesToRadians(180));
//Use those to add it into the world
MeshManager.AddStatic(World, ArenaMeshHandle, ArenaPosition, ArenaRotation);
//Place the jumping cubes
World.Statics.Add(new StaticDescription(new Vector3(1.299f, 2.3f, -31.24f), new CollidableDescription(World.Shapes.Add(new Box(5, 5, 5)), 0.1f)));
World.Statics.Add(new StaticDescription(new Vector3(-26.56f, 2.3f, -35.2f), new CollidableDescription(World.Shapes.Add(new Box(5, 5, 5)), 0.1f)));
World.Statics.Add(new StaticDescription(new Vector3(-33.12f, 2.3f, -21.65f), new CollidableDescription(World.Shapes.Add(new Box(5, 5, 5)), 0.1f)));
World.Statics.Add(new StaticDescription(new Vector3(-26.05f, 2.3f, -9.38f), new CollidableDescription(World.Shapes.Add(new Box(5, 5, 5)), 0.1f)));
World.Statics.Add(new StaticDescription(new Vector3(-10.31f, 2.3f, -5.62f), new CollidableDescription(World.Shapes.Add(new Box(5, 5, 5)), 0.1f)));
World.Statics.Add(new StaticDescription(new Vector3(1.94f, 2.3f, -15.88f), new CollidableDescription(World.Shapes.Add(new Box(5, 5, 5)), 0.1f)));
World.Statics.Add(new StaticDescription(new Vector3(-11.3f, 2.3f, -37.44f), new CollidableDescription(World.Shapes.Add(new Box(5, 5, 5)), 0.1f)));
//Place a ground plane to walk on
World.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(World.Shapes.Add(new Box(100, 1, 100)), 0.1f)));
//Setup the command executor
CommandInputField.Initialize();
//Make sure the window size is correct relative to the current resolution
OnResize(ApplicationWindow.Resolution);
}
protected Demo()
{
BufferPool = new BufferPool();
ThreadDispatcher = new SimpleThreadDispatcher(Environment.ProcessorCount);
}
public static void Test()
{
void VerifySort <TSpan>(TSpan keys) where TSpan : ISpan <int>
{
for (int i = 1; i < keys.Length; ++i)
{
Debug.Assert(keys[i] >= keys[i - 1]);
}
}
const int elementCount = 4096;
const int elementExclusiveUpperBound = 32768;// 1 << 5;
var bufferPool = new BufferPool();
var threadDispatcher = new SimpleThreadDispatcher(8);
for (int iteration = 0; iteration < 4; ++iteration)
{
GC.Collect(3, GCCollectionMode.Forced, true);
var keys = new Array <int>(new int[elementCount]);
var indexMap = new Array <int>(new int[elementCount]);
Random random = new Random(5);
for (int i = 0; i < elementCount; ++i)
{
indexMap[i] = i;
//keys[i] = i / (elementCount / elementExclusiveUpperBound);
//keys[i] = i % elementExclusiveUpperBound;
//keys[i] = i;
keys[i] = random.Next(elementExclusiveUpperBound);
}
var keys2 = new Array <int>(new int[elementCount]);
var indexMap2 = new Array <int>(new int[elementCount]);
var keys3 = new Array <int>(new int[elementCount]);
var indexMap3 = new Array <int>(new int[elementCount]);
var keys4 = new Array <int>(new int[elementCount]);
var indexMap4 = new Array <int>(new int[elementCount]);
keys.CopyTo(0, ref keys2, 0, elementCount);
keys.CopyTo(0, ref keys3, 0, elementCount);
keys.CopyTo(0, ref keys4, 0, elementCount);
indexMap.CopyTo(0, ref indexMap2, 0, elementCount);
indexMap.CopyTo(0, ref indexMap3, 0, elementCount);
indexMap.CopyTo(0, ref indexMap4, 0, elementCount);
var timer = Stopwatch.StartNew();
var keysScratch = new int[elementCount];
var valuesScratch = new int[elementCount];
var bucketCounts = new int[1024];
for (int t = 0; t < 16; ++t)
{
//keys2.CopyTo(0, ref keys, 0, elementCount);
//unsafe
//{
// var comparer = new Comparer();
// fixed (int* keysPointer = keys.Memory)
// fixed (int* valuesPointer = indexMap.Memory)
// {
// var keysBuffer = new Buffer<int>(keysPointer, keys.Length);
// var valuesBuffer = new Buffer<int>(valuesPointer, indexMap.Length);
// timer.Restart();
// QuickSort.Sort(ref keysBuffer[0], ref valuesBuffer[0], 0, elementCount - 1, ref comparer);
// }
//}
////QuickSort.Sort2(ref keys[0], ref indexMap[0], 0, elementCount - 1, ref comparer);
//timer.Stop();
//VerifySort(keys);
//Console.WriteLine($"QuickSort time (ms): {timer.Elapsed.TotalSeconds * 1e3}");
//keys.CopyTo(0, ref keys2, 0, elementCount);
//timer.Restart();
//Array.Sort(keys2.Memory, indexMap2.Memory, 0, elementCount);
//timer.Stop();
//VerifySort(keys2);
//Console.WriteLine($"Array.Sort time (ms): {timer.Elapsed.TotalSeconds * 1e3}");
keys.CopyTo(0, ref keys3, 0, elementCount);
timer.Restart();
Array.Clear(bucketCounts, 0, bucketCounts.Length);
LSBRadixSort.SortU16(ref keys3[0], ref indexMap3[0], ref keysScratch[0], ref valuesScratch[0], ref bucketCounts[0], elementCount);
timer.Stop();
VerifySort(keys3);
Console.WriteLine($"{t} LSBRadixSort time (ms): {timer.Elapsed.TotalSeconds * 1e3}");
//keys.CopyTo(0, ref keys4, 0, elementCount);
//var originalIndices = new int[256];
//timer.Restart();
////MSBRadixSort.SortU32(ref keys4[0], ref indexMap4[0], ref bucketCounts[0], ref originalIndices[0], elementCount, 24);
//MSBRadixSort.SortU32(ref keys4[0], ref indexMap4[0], elementCount, SpanHelper.GetContainingPowerOf2(elementExclusiveUpperBound));
//timer.Stop();
//VerifySort(keys4);
//Console.WriteLine($"{t} MSBRadixSort time (ms): {timer.Elapsed.TotalSeconds * 1e3}");
}
}
}
public static void Test()
{
var pool = new BufferPool();
var tree = new Tree(pool, 128);
const int leafCountAlongXAxis = 11;
const int leafCountAlongYAxis = 13;
const int leafCountAlongZAxis = 15;
var leafCount = leafCountAlongXAxis * leafCountAlongYAxis * leafCountAlongZAxis;
pool.Take <BoundingBox>(leafCount, out var leafBounds);
const float boundsSpan = 2;
const float spanRange = 2;
const float boundsSpacing = 3;
var random = new Random(5);
for (int i = 0; i < leafCountAlongXAxis; ++i)
{
for (int j = 0; j < leafCountAlongYAxis; ++j)
{
for (int k = 0; k < leafCountAlongZAxis; ++k)
{
var index = leafCountAlongXAxis * leafCountAlongYAxis * k + leafCountAlongXAxis * j + i;
leafBounds[index].Min = new Vector3(i, j, k) * boundsSpacing;
leafBounds[index].Max = leafBounds[index].Min + new Vector3(boundsSpan) +
spanRange * new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble());
}
}
}
var prebuiltCount = Math.Max(leafCount / 2, 1);
tree.SweepBuild(pool, leafBounds.Slice(0, prebuiltCount));
tree.Validate();
for (int i = prebuiltCount; i < leafCount; ++i)
{
tree.Add(ref leafBounds[i], pool);
}
tree.Validate();
pool.Take <int>(leafCount, out var handleToLeafIndex);
pool.Take <int>(leafCount, out var leafIndexToHandle);
for (int i = 0; i < leafCount; ++i)
{
handleToLeafIndex[i] = i;
leafIndexToHandle[i] = i;
}
const int iterations = 100000;
const int maximumChangesPerIteration = 20;
var threadDispatcher = new SimpleThreadDispatcher(Environment.ProcessorCount);
var refineContext = new Tree.RefitAndRefineMultithreadedContext();
var selfTestContext = new Tree.MultithreadedSelfTest <OverlapHandler>(pool);
var overlapHandlers = new OverlapHandler[threadDispatcher.ThreadCount];
Action <int> pairTestAction = selfTestContext.PairTest;
QuickList <int, Buffer <int> > .Create(pool.SpecializeFor <int>(), leafCount, out var removedLeafHandles);
for (int i = 0; i < iterations; ++i)
{
var changeCount = random.Next(maximumChangesPerIteration);
for (int j = 0; j <= changeCount; ++j)
{
var addedFraction = tree.LeafCount / (float)leafCount;
if (random.NextDouble() < addedFraction)
{
//Remove a leaf.
var leafIndexToRemove = random.Next(tree.LeafCount);
var handleToRemove = leafIndexToHandle[leafIndexToRemove];
var movedLeafIndex = tree.RemoveAt(leafIndexToRemove);
if (movedLeafIndex >= 0)
{
var movedHandle = leafIndexToHandle[movedLeafIndex];
handleToLeafIndex[movedHandle] = leafIndexToRemove;
leafIndexToHandle[leafIndexToRemove] = movedHandle;
leafIndexToHandle[movedLeafIndex] = -1;
}
else
{
//The removed leaf was the last one. This leaf index is no longer associated with any existing leaf.
leafIndexToHandle[leafIndexToRemove] = -1;
}
handleToLeafIndex[handleToRemove] = -1;
removedLeafHandles.AddUnsafely(handleToRemove);
tree.Validate();
}
else
{
//Add a leaf.
var indexInRemovedList = random.Next(removedLeafHandles.Count);
var handleToAdd = removedLeafHandles[indexInRemovedList];
removedLeafHandles.FastRemoveAt(indexInRemovedList);
var leafIndex = tree.Add(ref leafBounds[handleToAdd], pool);
leafIndexToHandle[leafIndex] = handleToAdd;
handleToLeafIndex[handleToAdd] = leafIndex;
tree.Validate();
}
}
tree.Refit();
tree.Validate();
tree.RefitAndRefine(pool, i);
tree.Validate();
var handler = new OverlapHandler();
tree.GetSelfOverlaps(ref handler);
tree.Validate();
refineContext.RefitAndRefine(ref tree, pool, threadDispatcher, i);
tree.Validate();
for (int k = 0; k < threadDispatcher.ThreadCount; ++k)
{
overlapHandlers[k] = new OverlapHandler();
}
selfTestContext.PrepareJobs(ref tree, overlapHandlers, threadDispatcher.ThreadCount);
threadDispatcher.DispatchWorkers(pairTestAction);
selfTestContext.CompleteSelfTest();
tree.Validate();
if (i % 50 == 0)
{
Console.WriteLine($"Cost: {tree.MeasureCostMetric()}");
Console.WriteLine($"Cache Quality: {tree.MeasureCacheQuality()}");
Console.WriteLine($"Overlap Count: {handler.OverlapCount}");
}
}
threadDispatcher.Dispose();
pool.Clear();
}
public virtual void InitializePhysics(Simulation simulation, BufferPool bufferPool, SimpleThreadDispatcher threadDispatcher)
{
}
public static void Test()
{
const int elementCount = 65536;
const int elementExclusiveUpperBound = 1 << 16;
var bufferPool = new BufferPool();
var threadDispatcher = new SimpleThreadDispatcher(8);
bufferPool.Take <int>(elementCount, out var keys);
bufferPool.Take <int>(elementCount, out var indexMap);
bufferPool.Take <int>(elementCount, out var keys2);
bufferPool.Take <int>(elementCount, out var indexMap2);
bufferPool.Take <int>(elementCount, out var keys3);
bufferPool.Take <int>(elementCount, out var indexMap3);
bufferPool.Take <int>(elementCount, out var keys4);
bufferPool.Take <int>(elementCount, out var indexMap4);
Random random = new Random(5);
for (int iteration = 0; iteration < 4; ++iteration)
{
for (int i = 0; i < elementCount; ++i)
{
indexMap[i] = i;
//keys[i] = i / (elementCount / elementExclusiveUpperBound);
//keys[i] = i % elementExclusiveUpperBound;
//keys[i] = i;
keys[i] = random.Next(elementExclusiveUpperBound);
}
keys.CopyTo(0, ref keys2, 0, elementCount);
keys.CopyTo(0, ref keys3, 0, elementCount);
keys.CopyTo(0, ref keys4, 0, elementCount);
indexMap.CopyTo(0, ref indexMap2, 0, elementCount);
indexMap.CopyTo(0, ref indexMap3, 0, elementCount);
indexMap.CopyTo(0, ref indexMap4, 0, elementCount);
var timer = Stopwatch.StartNew();
var keysScratch = new int[elementCount];
var valuesScratch = new int[elementCount];
var bucketCounts = new int[1024];
for (int t = 0; t < 16; ++t)
{
var comparer = new Comparer();
timer.Restart();
QuickSort.Sort(ref keys[0], ref indexMap[0], 0, elementCount - 1, ref comparer);
//QuickSort.Sort2(ref keys[0], ref indexMap[0], 0, elementCount - 1, ref comparer);
timer.Stop();
VerifySort(ref keys);
Console.WriteLine($"QuickSort time (ms): {timer.Elapsed.TotalSeconds * 1e3}");
//timer.Restart();
//Array.Sort(keys2.Memory, indexMap2.Memory, 0, elementCount);
//timer.Stop();
//VerifySort(ref keys2);
//Console.WriteLine($"Array.Sort time (ms): {timer.Elapsed.TotalSeconds * 1e3}");
timer.Restart();
Array.Clear(bucketCounts, 0, bucketCounts.Length);
LSBRadixSort.SortU16(ref keys3[0], ref indexMap3[0], ref keysScratch[0], ref valuesScratch[0], ref bucketCounts[0], elementCount);
timer.Stop();
VerifySort(ref keys3);
Console.WriteLine($"{t} LSBRadixSort time (ms): {timer.Elapsed.TotalSeconds * 1e3}");
var originalIndices = new int[256];
timer.Restart();
//MSBRadixSort.SortU32(ref keys4[0], ref indexMap4[0], ref bucketCounts[0], ref originalIndices[0], elementCount, 24);
MSBRadixSort.SortU32(ref keys4[0], ref indexMap4[0], elementCount, SpanHelper.GetContainingPowerOf2(elementExclusiveUpperBound));
timer.Stop();
VerifySort(ref keys4);
Console.WriteLine($"{t} MSBRadixSort time (ms): {timer.Elapsed.TotalSeconds * 1e3}");
}
}
bufferPool.Clear();
}
public override void InitializePhysics(Simulation simulation, BufferPool bufferPool, SimpleThreadDispatcher threadDispatcher)
{
foreach (var o in Objects)
{
o.InitializePhysics(simulation, bufferPool, threadDispatcher);
}
}
