public unsafe void ExtractLines(ref PointOnLineServoPrestepData prestepBundle, int setIndex, int *bodyIndices,
Bodies bodies, ref Vector3 tint, ref QuickList <LineInstance> lines)
{
//Could do bundles of constraints at a time, but eh.
var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]];
var poseB = bodies.Sets[setIndex].Poses[bodyIndices[1]];
Vector3Wide.ReadFirst(prestepBundle.LocalOffsetA, out var localOffsetA);
Vector3Wide.ReadFirst(prestepBundle.LocalOffsetB, out var localOffsetB);
Vector3Wide.ReadFirst(prestepBundle.LocalDirection, out var localDirection);
QuaternionEx.Transform(localOffsetA, poseA.Orientation, out var worldOffsetA);
QuaternionEx.Transform(localDirection, poseA.Orientation, out var worldDirection);
QuaternionEx.Transform(localOffsetB, poseB.Orientation, out var worldOffsetB);
var anchorA = poseA.Position + worldOffsetA;
var anchorB = poseB.Position + worldOffsetB;
var closestPointOnLine = Vector3.Dot(anchorB - anchorA, worldDirection) * worldDirection + anchorA;
var color = new Vector3(0.2f, 0.2f, 1f) * tint;
var packedColor = Helpers.PackColor(color);
var backgroundColor = new Vector3(0f, 0f, 1f) * tint;
lines.AllocateUnsafely() = new LineInstance(poseA.Position, anchorA, packedColor, 0);
lines.AllocateUnsafely() = new LineInstance(anchorA, closestPointOnLine, packedColor, 0);
lines.AllocateUnsafely() = new LineInstance(closestPointOnLine, anchorB, Helpers.PackColor(new Vector3(1, 0, 0) * tint), 0);
lines.AllocateUnsafely() = new LineInstance(anchorB, poseB.Position, packedColor, 0);
}
public unsafe void ExtractLines(ref CenterDistancePrestepData prestepBundle, int setIndex, int *bodyIndices,
Bodies bodies, ref Vector3 tint, ref QuickList <LineInstance> lines)
{
//Could do bundles of constraints at a time, but eh.
var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]];
var poseB = bodies.Sets[setIndex].Poses[bodyIndices[1]];
var targetDistance = GatherScatter.GetFirst(ref prestepBundle.TargetDistance);
var color = new Vector3(0.2f, 0.2f, 1f) * tint;
var packedColor = Helpers.PackColor(color);
var backgroundColor = new Vector3(0f, 0f, 1f) * tint;
//Draw a line from A to B. If the true distance is longer than the target distance, draw a red line to complete the gap.
//If the true distance is shorter than the target distance, draw an overshooting red line.
var offset = poseB.Position - poseA.Position;
var length = offset.Length();
var direction = length < 1e-9f ? new Vector3(1, 0, 0) : offset / length;
var errorColor = new Vector3(1, 0, 0) * tint;
var packedErrorColor = Helpers.PackColor(errorColor);
var packedDistanceColor = Helpers.PackColor(color * 0.5f);
var targetEnd = poseA.Position + direction * targetDistance;
if (length < targetDistance)
{
lines.AllocateUnsafely() = new LineInstance(poseA.Position, poseB.Position, packedDistanceColor, 0);
lines.AllocateUnsafely() = new LineInstance(poseB.Position, targetEnd, packedErrorColor, 0);
}
else
{
lines.AllocateUnsafely() = new LineInstance(poseA.Position, targetEnd, packedDistanceColor, 0);
lines.AllocateUnsafely() = new LineInstance(targetEnd, poseB.Position, packedErrorColor, 0);
}
}
public unsafe void ExtractLines(ref BallSocketPrestepData prestepBundle, int setIndex, int *bodyIndices,
Bodies bodies, ref Vector3 tint, ref QuickList <LineInstance> lines)
{
//Could do bundles of constraints at a time, but eh.
var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]];
var poseB = bodies.Sets[setIndex].Poses[bodyIndices[1]];
Vector3Wide.ReadFirst(prestepBundle.LocalOffsetA, out var localOffsetA);
Vector3Wide.ReadFirst(prestepBundle.LocalOffsetB, out var localOffsetB);
QuaternionEx.Transform(localOffsetA, poseA.Orientation, out var worldOffsetA);
QuaternionEx.Transform(localOffsetB, poseB.Orientation, out var worldOffsetB);
var endA = poseA.Position + worldOffsetA;
var endB = poseB.Position + worldOffsetB;
var color = new Vector3(0.2f, 0.2f, 1f) * tint;
var packedColor = Helpers.PackColor(color);
var backgroundColor = new Vector3(0f, 0f, 1f) * tint;
lines.AllocateUnsafely() = new LineInstance(poseA.Position, endA, packedColor, 0);
lines.AllocateUnsafely() = new LineInstance(poseB.Position, endB, packedColor, 0);
var errorColor = new Vector3(1, 0, 0) * tint;
var packedErrorColor = Helpers.PackColor(errorColor);
lines.AllocateUnsafely() = new LineInstance(endA, endB, packedErrorColor, 0);
}
public unsafe void ExtractLines(ref LinearAxisServoPrestepData prestepBundle, int setIndex, int *bodyIndices,
Bodies bodies, ref Vector3 tint, ref QuickList <LineInstance> lines)
{
//Could do bundles of constraints at a time, but eh.
var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]];
var poseB = bodies.Sets[setIndex].Poses[bodyIndices[1]];
Vector3Wide.ReadFirst(prestepBundle.LocalOffsetA, out var localOffsetA);
Vector3Wide.ReadFirst(prestepBundle.LocalOffsetB, out var localOffsetB);
Vector3Wide.ReadFirst(prestepBundle.LocalPlaneNormal, out var localPlaneNormal);
var targetOffset = GatherScatter.GetFirst(ref prestepBundle.TargetOffset);
Matrix3x3.CreateFromQuaternion(poseA.Orientation, out var orientationA);
Matrix3x3.Transform(localOffsetA, orientationA, out var worldOffsetA);
Matrix3x3.Transform(localPlaneNormal, orientationA, out var worldPlaneNormal);
Quaternion.Transform(localOffsetB, poseB.Orientation, out var worldOffsetB);
var anchorA = poseA.Position + worldOffsetA;
var anchorB = poseB.Position + worldOffsetB;
var planeOffset = Vector3.Dot(anchorB - anchorA, worldPlaneNormal);
var closestPointOnPlane = anchorB - planeOffset * worldPlaneNormal;
var packedColor = Helpers.PackColor(new Vector3(0.2f, 0.2f, 1f) * tint);
var packedBasisColor = Helpers.PackColor(new Vector3(0.2f, 0.6f, 1f) * tint);
var backgroundColor = new Vector3(0f, 0f, 1f) * tint;
lines.AllocateUnsafely() = new LineInstance(poseA.Position, anchorA, packedColor, 0);
ContactLines.BuildOrthnormalBasis(localPlaneNormal, out var localTX, out var localTY);
Matrix3x3.Transform(localTX, orientationA, out var tX);
Matrix3x3.Transform(localTY, orientationA, out var tY);
lines.AllocateUnsafely() = new LineInstance(anchorA - tX, anchorA + tX, packedBasisColor, 0);
lines.AllocateUnsafely() = new LineInstance(anchorA - tY, anchorA + tY, packedBasisColor, 0);
lines.AllocateUnsafely() = new LineInstance(anchorA, closestPointOnPlane, packedColor, 0);
lines.AllocateUnsafely() = new LineInstance(anchorB, poseB.Position, packedColor, 0);
if (targetOffset < 0)
{
targetOffset = -targetOffset;
planeOffset = -planeOffset;
worldPlaneNormal = -worldPlaneNormal;
}
var targetPoint = closestPointOnPlane + worldPlaneNormal * targetOffset;
var packedErrorColor = Helpers.PackColor(new Vector3(1, 0, 0) * tint);
if (planeOffset > targetOffset)
{
lines.AllocateUnsafely() = new LineInstance(closestPointOnPlane, targetPoint, packedColor, 0);
lines.AllocateUnsafely() = new LineInstance(targetPoint, anchorB, packedErrorColor, 0);
}
else
{
lines.AllocateUnsafely() = new LineInstance(closestPointOnPlane, anchorB, packedColor, 0);
lines.AllocateUnsafely() = new LineInstance(anchorB, targetPoint, packedErrorColor, 0);
}
}
void TryAddProjectileImpact(int projectileHandle, CollidableReference impactedCollidable)
{
bool lockTaken = false;
ProjectileLock.Enter(ref lockTaken);
try
{
//Note that we have to protect against redundant adds- a projectile might hit multiple things in the same frame. Wouldn't want it to explode multiple times.
for (int i = 0; i < ProjectileImpacts.Count; ++i)
{
ref var impact = ref ProjectileImpacts[i];
//If the projectile has already been handled, ignore it.
if (impact.ProjectileHandle == projectileHandle)
{
return;
}
}
//The exploding projectiles list should have been sized ahead of time to hold all projectiles, so no dynamic allocations should be required.
ref var newImpact = ref ProjectileImpacts.AllocateUnsafely();
newImpact.ProjectileHandle = projectileHandle;
if (impactedCollidable.Mobility != CollidableMobility.Static)
{
//The filter's group id is the tank's main body handle. We use that to find the tank (if this body is related to a tank at all).
ref var properties = ref Properties[impactedCollidable.Handle];
newImpact.ImpactedTankBodyHandle = properties.TankPart ? properties.Filter.GroupId : -1;
}
public VoxelGridCollidable(Vector3i coordinates, VoxelGrid space, BufferPool pool)
{
Coordinates = coordinates;
var data = space.Data;
Voxels = data;
var voxelList = new QuickList <Vector3>(data.XLength * data.YLength * data.ZLength, pool);
foreach (var voxel in data)
{
if (voxel.Item2.Exists)
{
voxelList.AllocateUnsafely() = voxel.Item1;
}
}
VoxelIndices = voxelList;
VoxelSize = Vector3.One * data.VoxelSize;
Tree = new Tree(pool, VoxelIndices.Count);
//Could stackalloc here, but the assumption is that there could be quite a few voxels.
//Quite possible to overflow the stack, so we instead resort to heap allocation.
pool.Take(VoxelIndices.Count, out Buffer <BoundingBox> bounds);
for (int i = 0; i < VoxelIndices.Count; ++i)
{
ref var voxel = ref VoxelIndices[i];
ref var voxelBounds = ref bounds[i];
public override void Update(Window window, Camera camera, Input input, float dt)
{
for (int iterationIndex = 0; iterationIndex < 100; ++iterationIndex)
{
var bodyIndicesToDeactivate = new QuickList <int>(Simulation.Bodies.ActiveSet.Count, BufferPool);
for (int i = 0; i < Simulation.Bodies.ActiveSet.Count; ++i)
{
bodyIndicesToDeactivate.AllocateUnsafely() = i;
}
Simulation.Sleeper.Sleep(ref bodyIndicesToDeactivate);
bodyIndicesToDeactivate.Dispose(BufferPool);
var setsToActivate = new QuickList <int>(Simulation.Bodies.Sets.Length, BufferPool);
for (int i = 1; i < Simulation.Bodies.Sets.Length; ++i)
{
if (Simulation.Bodies.Sets[i].Allocated)
{
setsToActivate.AllocateUnsafely() = i;
}
}
Simulation.Awakener.AwakenSets(ref setsToActivate);
setsToActivate.Dispose(BufferPool);
}
base.Update(window, camera, input, dt);
}
public unsafe void ExtractLines(ref OneBodyLinearServoPrestepData prestepBundle, int setIndex, int *bodyIndices,
Bodies bodies, ref Vector3 tint, ref QuickList <LineInstance> lines)
{
//Could do bundles of constraints at a time, but eh.
var pose = bodies.Sets[setIndex].Poses[*bodyIndices];
Vector3Wide.ReadFirst(prestepBundle.LocalOffset, out var localOffset);
Vector3Wide.ReadFirst(prestepBundle.Target, out var target);
Quaternion.Transform(localOffset, pose.Orientation, out var worldOffset);
var anchor = pose.Position + worldOffset;
var backgroundColor = new Vector3(0f, 0f, 1f) * tint;
lines.AllocateUnsafely() = new LineInstance(pose.Position, anchor, Helpers.PackColor(new Vector3(0.2f, 0.2f, 1f) * tint), 0);
lines.AllocateUnsafely() = new LineInstance(anchor, target, Helpers.PackColor(new Vector3(1, 0, 0) * tint), 0);
}
//List<DebugStep> debugSteps;
public override void Initialize(ContentArchive content, Camera camera)
{
camera.Position = new Vector3(0, -2.5f, 10);
camera.Yaw = 0;
camera.Pitch = 0;
Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0)));
const int pointCount = 128;
points = new QuickList <Vector3>(pointCount * 2, BufferPool);
//points.Allocate(BufferPool) = new Vector3(0, 0, 0);
//points.Allocate(BufferPool) = new Vector3(0, 0, 1);
//points.Allocate(BufferPool) = new Vector3(0, 1, 0);
//points.Allocate(BufferPool) = new Vector3(0, 1, 1);
//points.Allocate(BufferPool) = new Vector3(1, 0, 0);
//points.Allocate(BufferPool) = new Vector3(1, 0, 1);
//points.Allocate(BufferPool) = new Vector3(1, 1, 0);
//points.Allocate(BufferPool) = new Vector3(1, 1, 1);
var random = new Random(5);
for (int i = 0; i < pointCount; ++i)
{
points.AllocateUnsafely() = new Vector3(3 * (float)random.NextDouble(), 1 * (float)random.NextDouble(), 3 * (float)random.NextDouble());
//points.AllocateUnsafely() = new Vector3(0, 1, 0) + Vector3.Normalize(new Vector3((float)random.NextDouble() * 2 - 1, (float)random.NextDouble() * 2 - 1, (float)random.NextDouble() * 2 - 1)) * (float)random.NextDouble();
}
var pointsBuffer = points.Span.Slice(0, points.Count);
ConvexHullHelper.CreateShape(pointsBuffer, BufferPool, out _, out var hullShape);
const int iterationCount = 100;
var start = Stopwatch.GetTimestamp();
for (int i = 0; i < iterationCount; ++i)
{
ConvexHullHelper.CreateShape(pointsBuffer, BufferPool, out _, out var perfTestShape);
perfTestShape.Dispose(BufferPool);
}
var end = Stopwatch.GetTimestamp();
Console.WriteLine($"Hull computation time (us): {(end - start) * 1e6 / (iterationCount * Stopwatch.Frequency)}");
hullShape.ComputeInertia(1, out var inertia);
Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 0, 0), inertia, new CollidableDescription(Simulation.Shapes.Add(hullShape), 10.1f), new BodyActivityDescription(0.01f)));
Simulation.Statics.Add(new StaticDescription(new Vector3(-25, -5, 0), new CollidableDescription(Simulation.Shapes.Add(new Sphere(2)), 0.1f)));
Simulation.Statics.Add(new StaticDescription(new Vector3(-20, -5, 0), new CollidableDescription(Simulation.Shapes.Add(new Capsule(0.5f, 2)), 0.1f)));
Simulation.Statics.Add(new StaticDescription(new Vector3(-15, -5, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(2f, 2f, 2f)), 0.1f)));
Simulation.Statics.Add(new StaticDescription(new Vector3(-10, -5, 5), new CollidableDescription(Simulation.Shapes.Add(new Triangle {
A = new Vector3(0, 0, -10), B = new Vector3(5, 0, -10), C = new Vector3(0, 0, -5)
}), 0.1f)));
Simulation.Statics.Add(new StaticDescription(new Vector3(-5, -5, 0), new CollidableDescription(Simulation.Shapes.Add(new Cylinder(1, 1)), 0.1f)));
Simulation.Statics.Add(new StaticDescription(new Vector3(-5, -5, 5), new CollidableDescription(Simulation.Shapes.Add(new Cylinder(1, 1)), 0.1f)));
Simulation.Statics.Add(new StaticDescription(new Vector3(0, -5, 0), new CollidableDescription(Simulation.Shapes.Add(hullShape), 0.1f)));
}
public unsafe void ExtractLines(ref WeldPrestepData prestepBundle, int innerIndex, int setIndex, int *bodyIndices,
Bodies bodies, ref Vector3 tint, ref QuickList <LineInstance> lines)
{
//Could do bundles of constraints at a time, but eh.
var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]];
var poseB = bodies.Sets[setIndex].Poses[bodyIndices[1]];
Vector3Wide.ReadSlot(ref prestepBundle.LocalOffset, innerIndex, out var localOffset);
Quaternion.Transform(localOffset, poseA.Orientation, out var worldOffset);
var bTarget = poseA.Position + worldOffset;
var color = new Vector3(0.2f, 0.2f, 1f) * tint;
var packedColor = Helpers.PackColor(color);
var backgroundColor = new Vector3(0f, 0f, 1f) * tint;
lines.AllocateUnsafely() = new LineInstance(poseA.Position, bTarget, packedColor, 0);
var errorColor = new Vector3(1, 0, 0) * tint;
var packedErrorColor = Helpers.PackColor(errorColor);
lines.AllocateUnsafely() = new LineInstance(bTarget, poseB.Position, packedErrorColor, 0);
}
public unsafe override void Initialize(ContentArchive content, Camera camera)
{
camera.Position = new Vector3(25f, 1.5f, 15f);
camera.Yaw = 3 * MathHelper.Pi / 4;
camera.Pitch = 0;// MathHelper.Pi * 0.15f;
Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper());
var meshContent = content.Load <MeshContent>("Content\\newt.obj");
//This is actually a pretty good example of how *not* to make a convex hull shape.
//Generating it directly from a graphical data source tends to have way more surface complexity than needed,
//and it tends to have a lot of near-but-not-quite-coplanar surfaces which can make the contact manifold less stable.
//Prefer a simpler source with more distinct features, possibly created with an automated content-time tool.
var points = new QuickList <Vector3>(meshContent.Triangles.Length * 3, BufferPool);
for (int i = 0; i < meshContent.Triangles.Length; ++i)
{
ref var triangle = ref meshContent.Triangles[i];
//resisting the urge to just reinterpret the memory
points.AllocateUnsafely() = triangle.A * new Vector3(1, 1.5f, 1);
points.AllocateUnsafely() = triangle.B * new Vector3(1, 1.5f, 1);
points.AllocateUnsafely() = triangle.C * new Vector3(1, 1.5f, 1);
}
static void FillTrashBuffers(Simulation simulation, Random random)
{
var pool = simulation.BufferPool;
const int bufferCount = 50;
var bufferList = new QuickList <Buffer <int> >(bufferCount, pool);
for (int trashBufferIndex = 0; trashBufferIndex < bufferCount; ++trashBufferIndex)
{
//Pull a buffer from the pool, fill it with trash data, and return it.
ref var buffer = ref bufferList.AllocateUnsafely();
pool.TakeAtLeast(1 << random.Next(18), out buffer);
for (int k = 0; k < buffer.Length; ++k)
{
buffer[k] = random.Next(int.MinValue, int.MaxValue);
}
}
void TestConvexHullCreation()
{
var random = new Random(5);
for (int iterationIndex = 0; iterationIndex < 100000; ++iterationIndex)
{
const int pointCount = 32;
var points = new QuickList <Vector3>(pointCount, BufferPool);
for (int i = 0; i < pointCount; ++i)
{
points.AllocateUnsafely() = new Vector3(1 * (float)random.NextDouble(), 2 * (float)random.NextDouble(), 3 * (float)random.NextDouble());
}
var pointsBuffer = points.Span.Slice(0, points.Count);
CreateShape(pointsBuffer, BufferPool, out _, out var hullShape);
hullShape.Dispose(BufferPool);
}
}
public override void Initialize(ContentArchive content, Camera camera)
{
camera.Position = new Vector3(130, 50, 130);
camera.Yaw = -MathF.PI * 0.25f;
camera.Pitch = 0.4f;
characterControllers = new CharacterControllers(BufferPool);
Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characterControllers), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)));
DemoMeshHelper.LoadModel(content, BufferPool, @"Content\newt.obj", new Vector3(-10, 10, -10), out var newtMesh);
var newtShape = Simulation.Shapes.Add(newtMesh);
newts = new QuickList <SponsorNewt>(sponsors2.Count, BufferPool);
newtArenaMin = new Vector2(-100);
newtArenaMax = new Vector2(100);
random = new Random(6);
for (int i = 0; i < sponsors2.Count; ++i)
{
ref var newt = ref newts.AllocateUnsafely();
newt = new SponsorNewt(Simulation, newtShape, 0, newtArenaMin, newtArenaMax, random, i);
}
unsafe void BuildSortingTargets(ref QuickList <SortConstraintTarget> list, int typeIndex, int workerCount)
{
for (int i = 0; i < workerCount; ++i)
{
ref var workerList = ref overlapWorkers[i].PendingConstraints.pendingConstraintsByType[typeIndex];
if (workerList.Count > 0)
{
//This is doing redundant integer divides, but we lack type knowledge and it doesn't require any extra memory loading.
var entrySizeInBytes = workerList.ByteCount / workerList.Count;
int indexInBytes = 0;
for (int j = 0; j < workerList.Count; ++j)
{
ref var constraint = ref list.AllocateUnsafely();
constraint.WorkerIndex = i;
constraint.ByteIndexInCache = indexInBytes;
//Note two details:
//1) We rely on the layout of memory in the pending constraint add. If the collidable pair doesn't occupy the first 8 bytes, this breaks.
//2) We rely on the order of collidable pair references. The narrow phase should always guarantee a consistent order.
constraint.SortKey = *(ulong *)(workerList.Buffer.Memory + indexInBytes);
indexInBytes += entrySizeInBytes;
}
}
public override void Initialize(ContentArchive content, Camera camera)
{
camera.Position = new Vector3(130, 50, 130);
camera.Yaw = -MathF.PI * 0.25f;
camera.Pitch = 0.4f;
characterControllers = new CharacterControllers(BufferPool);
//Using a PositionLastTimestepper since we control the newts by velocity. Not as critical since they're kinematic and the position targets won't seek to cause undesired penetrations anyway,
//but it'll avoid integrating velocities into positions before the solver has a chance to intervene.
Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characterControllers), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionLastTimestepper());
DemoMeshHelper.LoadModel(content, BufferPool, @"Content\newt.obj", new Vector3(-10, 10, -10), out var newtMesh);
var newtShape = Simulation.Shapes.Add(newtMesh);
newts = new QuickList <SponsorNewt>(sponsors2.Count, BufferPool);
newtArenaMin = new Vector2(-100);
newtArenaMax = new Vector2(100);
random = new Random(6);
for (int i = 0; i < sponsors2.Count; ++i)
{
ref var newt = ref newts.AllocateUnsafely();
newt = new SponsorNewt(Simulation, newtShape, 0, newtArenaMin, newtArenaMax, random, i);
}
public void CreateJobs(int threadCount, ref QuickList <PreflushJob> jobs, BufferPool pool, int mappingCount)
{
if (mappingCount > 0)
{
if (threadCount > 1)
{
const int jobsPerThread = 2; //TODO: Empirical tune; probably just 1.
freshnessJobCount = Math.Min(threadCount * jobsPerThread, mappingCount);
var pairsPerJob = mappingCount / freshnessJobCount;
var remainder = mappingCount - pairsPerJob * freshnessJobCount;
int previousEnd = 0;
jobs.EnsureCapacity(jobs.Count + freshnessJobCount, pool);
int jobIndex = 0;
while (previousEnd < mappingCount)
{
ref var job = ref jobs.AllocateUnsafely();
job.Type = PreflushJobType.CheckFreshness;
job.Start = previousEnd;
//The end of every interval except the last one should be aligned on an 8 byte boundary.
var pairsInJob = jobIndex < remainder ? pairsPerJob + 1 : pairsPerJob;
previousEnd = ((previousEnd + pairsInJob + 7) >> 3) << 3;
if (previousEnd > mappingCount)
{
previousEnd = mappingCount;
}
job.End = previousEnd;
++jobIndex;
}
}
else
{
jobs.Add(new PreflushJob {
Type = PreflushJobType.CheckFreshness, Start = 0, End = mappingCount
}, pool);
}
}
unsafe void BuildSortingTargets <THandleCollector>(ref QuickList <SortConstraintTarget, Buffer <SortConstraintTarget> > list, int typeIndex, int workerCount) where THandleCollector : struct, ISortingHandleCollector
{
var handleCollector = default(THandleCollector);
for (int i = 0; i < workerCount; ++i)
{
ref var workerList = ref overlapWorkers[i].PendingConstraints.pendingConstraintsByType[typeIndex];
if (workerList.Count > 0)
{
//This is doing redundant integer divides, but we lack type knowledge and it doesn't require any extra memory loading.
var entrySizeInBytes = workerList.ByteCount / workerList.Count;
int indexInBytes = 0;
for (int j = 0; j < workerList.Count; ++j)
{
ref var constraint = ref list.AllocateUnsafely();
constraint.WorkerIndex = i;
constraint.ByteIndexInCache = indexInBytes;
//Note two details:
//1) We rely on the layout of memory in the pending constraint add. If the body handles don't occupy the first bytes, this breaks.
//2) We rely on the order of body handles. The narrow phase should always guarantee a consistent order.
constraint.Handles = handleCollector.GetHandles(workerList.Buffer.Memory + indexInBytes);
indexInBytes += entrySizeInBytes;
}
}
public unsafe void ExtractLines(ref DistanceServoPrestepData prestepBundle, int setIndex, int *bodyIndices,
Bodies bodies, ref Vector3 tint, ref QuickList <LineInstance> lines)
{
//Could do bundles of constraints at a time, but eh.
var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]];
var poseB = bodies.Sets[setIndex].Poses[bodyIndices[1]];
Vector3Wide.ReadFirst(prestepBundle.LocalOffsetA, out var localOffsetA);
Vector3Wide.ReadFirst(prestepBundle.LocalOffsetB, out var localOffsetB);
var targetDistance = GatherScatter.GetFirst(ref prestepBundle.TargetDistance);
Quaternion.Transform(localOffsetA, poseA.Orientation, out var worldOffsetA);
Quaternion.Transform(localOffsetB, poseB.Orientation, out var worldOffsetB);
var endA = poseA.Position + worldOffsetA;
var endB = poseB.Position + worldOffsetB;
var color = new Vector3(0.2f, 0.2f, 1f) * tint;
var packedColor = Helpers.PackColor(color);
var backgroundColor = new Vector3(0f, 0f, 1f) * tint;
lines.AllocateUnsafely() = new LineInstance(poseA.Position, endA, packedColor, 0);
lines.AllocateUnsafely() = new LineInstance(poseB.Position, endB, packedColor, 0);
//Draw a line from A to B. If the true distance is longer than the target distance, draw a red line to complete the gap.
//If the true distance is shorter than the target distance, draw an overshooting red line.
var offset = endB - endA;
var length = offset.Length();
var direction = length < 1e-9f ? new Vector3(1, 0, 0) : offset / length;
var errorColor = new Vector3(1, 0, 0) * tint;
var packedErrorColor = Helpers.PackColor(errorColor);
var packedDistanceColor = Helpers.PackColor(color * 0.5f);
var targetEnd = endA + direction * targetDistance;
if (length < targetDistance)
{
lines.AllocateUnsafely() = new LineInstance(endA, endB, packedDistanceColor, 0);
lines.AllocateUnsafely() = new LineInstance(endB, targetEnd, packedErrorColor, 0);
}
else
{
lines.AllocateUnsafely() = new LineInstance(endA, targetEnd, packedDistanceColor, 0);
lines.AllocateUnsafely() = new LineInstance(targetEnd, endB, packedErrorColor, 0);
}
}
public unsafe override void Initialize(ContentArchive content, Camera camera)
{
camera.Position = new Vector3(-20f, 13, -20f);
camera.Yaw = MathHelper.Pi * 3f / 4;
camera.Pitch = MathHelper.Pi * 0.1f;
Simulation = Simulation.Create(BufferPool, new NoCollisionCallbacks(), new DemoPoseIntegratorCallbacks(), new PositionFirstTimestepper());
var sphere = new Sphere(0.5f);
var shapeIndex = Simulation.Shapes.Add(sphere);
const int width = 16;
const int height = 16;
const int length = 16;
var spacing = new Vector3(2.01f);
var halfSpacing = spacing / 2;
float randomizationSubset = 0.9f;
var randomizationSpan = (spacing - new Vector3(1)) * randomizationSubset;
var randomizationBase = randomizationSpan * -0.5f;
var random = new Random(5);
for (int i = 0; i < width; ++i)
{
for (int j = 0; j < height; ++j)
{
for (int k = 0; k < length; ++k)
{
var r = new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble());
var location = spacing * (new Vector3(i, j, k) + new Vector3(-width, -height, -length) * 0.5f) + randomizationBase + r * randomizationSpan;
Quaternion orientation;
orientation.X = -1 + 2 * (float)random.NextDouble();
orientation.Y = -1 + 2 * (float)random.NextDouble();
orientation.Z = -1 + 2 * (float)random.NextDouble();
orientation.W = 0.01f + (float)random.NextDouble();
QuaternionEx.Normalize(ref orientation);
if ((i + j + k) % 2 == 1)
{
var bodyDescription = new BodyDescription
{
Activity = new BodyActivityDescription {
MinimumTimestepCountUnderThreshold = 32, SleepThreshold = -0.1f
},
Pose = new RigidPose
{
Orientation = orientation,
Position = location
},
Collidable = new CollidableDescription
{
Continuity = new ContinuousDetectionSettings {
Mode = ContinuousDetectionMode.Discrete
},
SpeculativeMargin = 0.1f,
Shape = shapeIndex
}
};
Simulation.Bodies.Add(bodyDescription);
}
else
{
var staticDescription = new StaticDescription
{
Pose = new RigidPose
{
Orientation = orientation,
Position = location
},
Collidable = new CollidableDescription
{
Continuity = new ContinuousDetectionSettings {
Mode = ContinuousDetectionMode.Discrete
},
SpeculativeMargin = 0.1f,
Shape = shapeIndex
}
};
Simulation.Statics.Add(staticDescription);
}
}
}
}
int boxCount = 16384;
var randomMin = new Vector3(width, height, length) * spacing * -0.5f;
var randomSpan = randomMin * -2;
queryBoxes = new QuickList <BoundingBox>(boxCount, BufferPool);
for (int i = 0; i < boxCount; ++i)
{
ref var box = ref queryBoxes.AllocateUnsafely();
var r = new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble());
var boxOrigin = randomMin + r * randomSpan;
var boxHalfSize = new Vector3(0.25f + 0.75f * (float)random.NextDouble());
box.Min = boxOrigin - boxHalfSize;
box.Max = boxOrigin + boxHalfSize;
}
public override void Initialize(ContentArchive content, Camera camera)
{
camera.Position = new Vector3(0, 5, 10);
camera.Yaw = 0;
camera.Pitch = 0;
bodyProperties = new CollidableProperty <TankDemoBodyProperties>();
//We assign velocities outside of the timestep to fire bullets, so using the PositionLastTimestepper avoids integrating those velocities into positions before the solver has a chance to intervene.
//We could have also modified velocities in the PositionFirstTimestepper's BeforeCollisionDetection callback, but it's just a little simpler to do this with very little cost.
Simulation = Simulation.Create(BufferPool, new TankCallbacks()
{
Properties = bodyProperties
}, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionLastTimestepper());
var builder = new CompoundBuilder(BufferPool, Simulation.Shapes, 2);
builder.Add(new Box(1.85f, 0.7f, 4.73f), RigidPose.Identity, 10);
builder.Add(new Box(1.85f, 0.6f, 2.5f), new RigidPose(new Vector3(0, 0.65f, -0.35f)), 0.5f);
builder.BuildDynamicCompound(out var children, out var bodyInertia, out _);
builder.Dispose();
var bodyShape = new Compound(children);
var bodyShapeIndex = Simulation.Shapes.Add(bodyShape);
var wheelShape = new Cylinder(0.4f, .18f);
wheelShape.ComputeInertia(0.25f, out var wheelInertia);
var wheelShapeIndex = Simulation.Shapes.Add(wheelShape);
var projectileShape = new Sphere(0.1f);
projectileShape.ComputeInertia(0.2f, out var projectileInertia);
var tankDescription = new TankDescription
{
Body = TankPartDescription.Create(10, new Box(4f, 1, 5), RigidPose.Identity, 0.5f, Simulation.Shapes),
Turret = TankPartDescription.Create(1, new Box(1.5f, 0.7f, 2f), new RigidPose(new Vector3(0, 0.85f, 0.4f)), 0.5f, Simulation.Shapes),
Barrel = TankPartDescription.Create(0.5f, new Box(0.2f, 0.2f, 3f), new RigidPose(new Vector3(0, 0.85f, 0.4f - 1f - 1.5f)), 0.5f, Simulation.Shapes),
TurretAnchor = new Vector3(0f, 0.5f, 0.4f),
BarrelAnchor = new Vector3(0, 0.5f + 0.35f, 0.4f - 1f),
TurretBasis = Quaternion.Identity,
TurretServo = new ServoSettings(1f, 0f, 40f),
TurretSpring = new SpringSettings(10f, 1f),
BarrelServo = new ServoSettings(1f, 0f, 40f),
BarrelSpring = new SpringSettings(10f, 1f),
ProjectileShape = Simulation.Shapes.Add(projectileShape),
ProjectileSpeed = 100f,
BarrelLocalProjectileSpawn = new Vector3(0, 0, -1.5f),
ProjectileInertia = projectileInertia,
LeftTreadOffset = new Vector3(-1.9f, 0f, 0),
RightTreadOffset = new Vector3(1.9f, 0f, 0),
SuspensionLength = 1f,
SuspensionSettings = new SpringSettings(2.5f, 1.5f),
WheelShape = wheelShapeIndex,
WheelInertia = wheelInertia,
WheelFriction = 2f,
TreadSpacing = 1f,
WheelCountPerTread = 5,
WheelOrientation = QuaternionEx.CreateFromAxisAngle(Vector3.UnitZ, MathF.PI * -0.5f),
};
playerController = new TankController(Tank.Create(Simulation, bodyProperties, BufferPool, new RigidPose(new Vector3(0, 10, 0), Quaternion.Identity), tankDescription), 20, 5, 2, 1, 3.5f);
const int planeWidth = 257;
const float terrainScale = 3;
const float inverseTerrainScale = 1f / terrainScale;
var terrainPosition = new Vector2(1 - planeWidth, 1 - planeWidth) * terrainScale * 0.5f;
random = new Random(5);
//Add some building-ish landmarks.
var landmarkMin = new Vector3(planeWidth * terrainScale * -0.45f, 0, planeWidth * terrainScale * -0.45f);
var landmarkMax = new Vector3(planeWidth * terrainScale * 0.45f, 0, planeWidth * terrainScale * 0.45f);
var landmarkSpan = landmarkMax - landmarkMin;
for (int j = 0; j < 25; ++j)
{
var buildingShape = new Box(10 + (float)random.NextDouble() * 10, 20 + (float)random.NextDouble() * 20, 10 + (float)random.NextDouble() * 10);
var position = landmarkMin + landmarkSpan * new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble());
Simulation.Statics.Add(new StaticDescription(
new Vector3(0, buildingShape.HalfHeight - 4f + GetHeightForPosition(position.X, position.Z, planeWidth, inverseTerrainScale, terrainPosition), 0) + position,
QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, (float)random.NextDouble() * MathF.PI),
new CollidableDescription(Simulation.Shapes.Add(buildingShape), 0.1f)));
}
DemoMeshHelper.CreateDeformedPlane(planeWidth, planeWidth,
(int vX, int vY) =>
{
var position2D = new Vector2(vX, vY) * terrainScale + terrainPosition;
return(new Vector3(position2D.X, GetHeightForPosition(position2D.X, position2D.Y, planeWidth, inverseTerrainScale, terrainPosition), position2D.Y));
}, new Vector3(1, 1, 1), BufferPool, out var planeMesh);
Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0, 0),
new CollidableDescription(Simulation.Shapes.Add(planeMesh), 0.1f)));
explosions = new QuickList <Explosion>(32, BufferPool);
//Create the AI tanks.
const int aiTankCount = 100;
aiTanks = new QuickList <AITank>(aiTankCount, BufferPool);
playAreaMin = new Vector2(landmarkMin.X, landmarkMin.Z);
playAreaMax = new Vector2(landmarkMax.X, landmarkMax.Z);
var playAreaSpan = playAreaMax - playAreaMin;
for (int i = 0; i < aiTankCount; ++i)
{
var horizontalPosition = playAreaMin + new Vector2((float)random.NextDouble(), (float)random.NextDouble()) * playAreaSpan;
aiTanks.AllocateUnsafely() = new AITank
{
Controller = new TankController(
Tank.Create(Simulation, bodyProperties, BufferPool, new RigidPose(
new Vector3(horizontalPosition.X, 10, horizontalPosition.Y),
QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), (float)random.NextDouble() * 0.1f)),
tankDescription), 20, 5, 2, 1, 3.5f),
HitPoints = 5
};
}
}
//List<DebugStep> debugSteps;
public override void Initialize(ContentArchive content, Camera camera)
{
camera.Position = new Vector3(0, -2.5f, 10);
camera.Yaw = 0;
camera.Pitch = 0;
Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)));
const int pointCount = 16;
points = new QuickList <Vector3>(pointCount * 2, BufferPool);
//points.Allocate(BufferPool) = new Vector3(0, 0, 0);
//points.Allocate(BufferPool) = new Vector3(0, 0, 1);
//points.Allocate(BufferPool) = new Vector3(0, 1, 0);
//points.Allocate(BufferPool) = new Vector3(0, 1, 1);
//points.Allocate(BufferPool) = new Vector3(1, 0, 0);
//points.Allocate(BufferPool) = new Vector3(1, 0, 1);
//points.Allocate(BufferPool) = new Vector3(1, 1, 0);
//points.Allocate(BufferPool) = new Vector3(1, 1, 1);
var random = new Random(5);
for (int i = 0; i < pointCount; ++i)
{
points.AllocateUnsafely() = new Vector3(3 * (float)random.NextDouble(), 1 * (float)random.NextDouble(), 3 * (float)random.NextDouble());
//points.AllocateUnsafely() = new Vector3(0, 1, 0) + Vector3.Normalize(new Vector3((float)random.NextDouble() * 2 - 1, (float)random.NextDouble() * 2 - 1, (float)random.NextDouble() * 2 - 1)) * (float)random.NextDouble();
}
var pointsBuffer = points.Span.Slice(0, points.Count);
CreateShape(pointsBuffer, BufferPool, out _, out var hullShape);
const int iterationCount = 100;
var start = Stopwatch.GetTimestamp();
for (int i = 0; i < iterationCount; ++i)
{
CreateShape(pointsBuffer, BufferPool, out _, out var perfTestShape);
perfTestShape.Dispose(BufferPool);
}
var end = Stopwatch.GetTimestamp();
Console.WriteLine($"Hull computation time (us): {(end - start) * 1e6 / (iterationCount * Stopwatch.Frequency)}");
var hullShapeIndex = Simulation.Shapes.Add(hullShape);
hullShape.ComputeInertia(1, out var inertia);
Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 0, 0), inertia, new CollidableDescription(hullShapeIndex, 10.1f), new BodyActivityDescription(0.01f)));
Simulation.Statics.Add(new StaticDescription(new Vector3(-25, -5, 0), new CollidableDescription(Simulation.Shapes.Add(new Sphere(2)), 0.1f)));
Simulation.Statics.Add(new StaticDescription(new Vector3(-20, -5, 0), new CollidableDescription(Simulation.Shapes.Add(new Capsule(0.5f, 2)), 0.1f)));
Simulation.Statics.Add(new StaticDescription(new Vector3(-15, -5, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(2f, 2f, 2f)), 0.1f)));
Simulation.Statics.Add(new StaticDescription(new Vector3(-10, -5, 5), new CollidableDescription(Simulation.Shapes.Add(new Triangle {
A = new Vector3(0, 0, -10), B = new Vector3(5, 0, -10), C = new Vector3(0, 0, -5)
}), 0.1f)));
Simulation.Statics.Add(new StaticDescription(new Vector3(-5, -5, 0), new CollidableDescription(Simulation.Shapes.Add(new Cylinder(1, 1)), 0.1f)));
Simulation.Statics.Add(new StaticDescription(new Vector3(-5, -5, 5), new CollidableDescription(Simulation.Shapes.Add(new Cylinder(1, 1)), 0.1f)));
Simulation.Statics.Add(new StaticDescription(new Vector3(0, -5, 0), new CollidableDescription(hullShapeIndex, 0.1f)));
var spacing = new Vector3(3f, 3f, 3);
int width = 16;
int height = 16;
int length = 16;
var origin = -0.5f * spacing * new Vector3(width, 0, length) + new Vector3(40, 0.2f, -40);
for (int i = 0; i < width; ++i)
{
for (int j = 0; j < height; ++j)
{
for (int k = 0; k < length; ++k)
{
Simulation.Bodies.Add(BodyDescription.CreateDynamic(
new RigidPose(origin + spacing * new Vector3(i, j, k), BepuUtilities.Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), MathHelper.Pi * 0.05f)),
inertia, new CollidableDescription(hullShapeIndex, 1f), new BodyActivityDescription(0.01f)));
}
}
}
Simulation.Statics.Add(new StaticDescription(new Vector3(0, -10, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(1000, 1, 1000)), 0.1f)));
}
public unsafe override void Initialize(ContentArchive content, Camera camera)
{
camera.Position = new Vector3(-20f, 13, -20f);
camera.Yaw = MathHelper.Pi * 3f / 4;
camera.Pitch = MathHelper.Pi * 0.1f;
//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 NoCollisionCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)), new PositionFirstTimestepper());
var sphere = new Sphere(0.5f);
var capsule = new Capsule(0, 0.5f);
var box = new Box(0.5f, 1.5f, 1f);
var cylinder = new Cylinder(0.5f, 1);
const int pointCount = 16;
var points = new QuickList <Vector3>(pointCount, BufferPool);
var random = new Random(5);
for (int i = 0; i < pointCount; ++i)
{
points.AllocateUnsafely() = new Vector3(1 * (float)random.NextDouble(), 1 * (float)random.NextDouble(), 1 * (float)random.NextDouble());
}
var hullShape = new ConvexHull(points, BufferPool, out _);
var sphereIndex = Simulation.Shapes.Add(sphere);
var capsuleIndex = Simulation.Shapes.Add(capsule);
var boxIndex = Simulation.Shapes.Add(box);
var cylinderIndex = Simulation.Shapes.Add(cylinder);
var hullIndex = Simulation.Shapes.Add(hullShape);
const int width = 16;
const int height = 16;
const int length = 16;
var spacing = new Vector3(2.01f);
var halfSpacing = spacing / 2;
float randomizationSubset = 0.9f;
var randomizationSpan = (spacing - new Vector3(1)) * randomizationSubset;
var randomizationBase = randomizationSpan * -0.5f;
for (int i = 0; i < width; ++i)
{
for (int j = 0; j < height; ++j)
{
for (int k = 0; k < length; ++k)
{
var r = new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble());
var location = spacing * (new Vector3(i, j, k) + new Vector3(-width, -height, -length) * 0.5f) + randomizationBase + r * randomizationSpan;
Quaternion orientation;
orientation.X = -1 + 2 * (float)random.NextDouble();
orientation.Y = -1 + 2 * (float)random.NextDouble();
orientation.Z = -1 + 2 * (float)random.NextDouble();
orientation.W = 0.01f + (float)random.NextDouble();
QuaternionEx.Normalize(ref orientation);
var shapeIndex = ((i + j + k) % 5) switch
{
0 => boxIndex,
1 => capsuleIndex,
2 => sphereIndex,
3 => cylinderIndex,
_ => hullIndex,
};
if ((i + j + k) % 2 == 1)
{
Simulation.Bodies.Add(BodyDescription.CreateKinematic(new RigidPose(location, orientation), new CollidableDescription(shapeIndex, 0.1f), new BodyActivityDescription(-0.1f)));
}
else
{
Simulation.Statics.Add(new StaticDescription(location, orientation, new CollidableDescription(shapeIndex, 0.1f)));
}
}
}
}
const int planeWidth = 128;
const int planeHeight = 128;
DemoMeshHelper.CreateDeformedPlane(planeWidth, planeHeight,
(int x, int y) =>
{
return(new Vector3(x - planeWidth / 2, 1 * MathF.Cos(x / 4f) * MathF.Sin(y / 4f), y - planeHeight / 2));
}, new Vector3(1, 3, 1), BufferPool, out var planeMesh);
Simulation.Statics.Add(new StaticDescription(
new Vector3(0, -10, 0), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 4),
new CollidableDescription(Simulation.Shapes.Add(planeMesh), 0.1f)));
int raySourceCount = 3;
raySources = new QuickList <QuickList <TestRay> >(raySourceCount, BufferPool);
raySources.Count = raySourceCount;
//Spew rays all over the place, starting inside the shape cube.
int randomRayCount = 1 << 14;
ref var randomRays = ref raySources[0];
public unsafe override void Initialize(ContentArchive content, Camera camera)
{
camera.Position = new Vector3(0, 10, 40);
camera.Yaw = 0;
camera.Pitch = 0;
Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)));
var box = new Box(2f, 2f, 2f);
var capsule = new Capsule(1f, 1f);
var sphere = new Sphere(1.5f);
box.ComputeInertia(1, out var boxInertia);
capsule.ComputeInertia(1, out var capsuleInertia);
sphere.ComputeInertia(1, out var sphereInertia);
var boxIndex = Simulation.Shapes.Add(box);
var capsuleIndex = Simulation.Shapes.Add(capsule);
var sphereIndex = Simulation.Shapes.Add(sphere);
const int width = 12;
const int height = 3;
const int length = 12;
for (int i = 0; i < width; ++i)
{
for (int j = 0; j < height; ++j)
{
for (int k = 0; k < length; ++k)
{
var location = new Vector3(5, 5, 5) * new Vector3(i, j, k) + new Vector3(-width * 2.5f, 2.5f, -length * 2.5f);
var bodyDescription = new BodyDescription
{
Activity = new BodyActivityDescription {
MinimumTimestepCountUnderThreshold = 32, SleepThreshold = 0.1f
},
Pose = new RigidPose
{
Orientation = Quaternion.Identity,
Position = location
},
Collidable = new CollidableDescription
{
Continuity = new ContinuousDetectionSettings {
Mode = ContinuousDetectionMode.Discrete
},
SpeculativeMargin = 0.1f
}
};
switch (j % 3)
{
case 0:
bodyDescription.Collidable.Shape = boxIndex;
bodyDescription.LocalInertia = boxInertia;
break;
case 1:
bodyDescription.Collidable.Shape = capsuleIndex;
bodyDescription.LocalInertia = capsuleInertia;
break;
case 2:
bodyDescription.Collidable.Shape = sphereIndex;
bodyDescription.LocalInertia = sphereInertia;
break;
}
Simulation.Bodies.Add(bodyDescription);
}
}
}
//Don't really want to regenerate a convex hull every frame; just cache one out.
const int pointCount = 32;
var points = new QuickList <Vector3>(pointCount, BufferPool);
var random = new Random(5);
for (int i = 0; i < pointCount; ++i)
{
points.AllocateUnsafely() = new Vector3((float)random.NextDouble() - 0.5f, (float)random.NextDouble() - 0.5f, (float)random.NextDouble() - 0.5f);
}
ConvexHullHelper.CreateShape(points.Span.Slice(0, points.Count), BufferPool, out _, out hull);
points.Dispose(BufferPool);
//var staticShapeIndex = Simulation.Shapes.Add(new Box(100, 1, 100));
//var staticDescription = new StaticDescription
//{
// Collidable = new CollidableDescription
// {
// Continuity = new ContinuousDetectionSettings { Mode = ContinuousDetectionMode.Discrete },
// Shape = Simulation.Shapes.Add(new Box(100, 1, 100)),
// SpeculativeMargin = 0.1f
// },
// Pose = new RigidPose { Position = new Vector3(0, -1, 0), Orientation = Quaternion.Identity }
//};
//Simulation.Statics.Add(staticDescription);
const int planeWidth = 64;
const int planeHeight = 64;
DemoMeshHelper.CreateDeformedPlane(planeWidth, planeHeight,
(int x, int y) =>
{
return(new Vector3(x, 1 * (float)Math.Cos(x / 4f) * (float)Math.Sin(y / 4f), y));
}, new Vector3(2, 3, 2), BufferPool, out var planeMesh);
Simulation.Statics.Add(new StaticDescription(new Vector3(-64, -10, -64), new CollidableDescription(Simulation.Shapes.Add(planeMesh), 0.1f)));
}
public unsafe override void Initialize(ContentArchive content, Camera camera)
{
camera.Position = new Vector3(-30, 10, -30);
//camera.Yaw = MathHelper.Pi ;
camera.Yaw = MathHelper.Pi * 3f / 4;
//camera.Pitch = MathHelper.PiOver2 * 0.999f;
Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)));
Simulation.Deterministic = true;
var sphere = new Sphere(1.5f);
var capsule = new Capsule(1f, 1f);
var box = new Box(1f, 3f, 2f);
var cylinder = new Cylinder(1.5f, 0.3f);
const int pointCount = 32;
var points = new QuickList <Vector3>(pointCount, BufferPool);
//points.Allocate(BufferPool) = new Vector3(0, 0, 0);
//points.Allocate(BufferPool) = new Vector3(0, 0, 1);
//points.Allocate(BufferPool) = new Vector3(0, 1, 0);
//points.Allocate(BufferPool) = new Vector3(0, 1, 1);
//points.Allocate(BufferPool) = new Vector3(1, 0, 0);
//points.Allocate(BufferPool) = new Vector3(1, 0, 1);
//points.Allocate(BufferPool) = new Vector3(1, 1, 0);
//points.Allocate(BufferPool) = new Vector3(1, 1, 1);
var random = new Random(5);
for (int i = 0; i < pointCount; ++i)
{
points.AllocateUnsafely() = new Vector3(3 * (float)random.NextDouble(), 1 * (float)random.NextDouble(), 3 * (float)random.NextDouble());
//points.AllocateUnsafely() = new Vector3(0, 1, 0) + Vector3.Normalize(new Vector3((float)random.NextDouble() * 2 - 1, (float)random.NextDouble() * 2 - 1, (float)random.NextDouble() * 2 - 1)) * (float)random.NextDouble();
}
var convexHull = new ConvexHull(points.Span.Slice(points.Count), BufferPool, out _);
box.ComputeInertia(1, out var boxInertia);
capsule.ComputeInertia(1, out var capsuleInertia);
sphere.ComputeInertia(1, out var sphereInertia);
cylinder.ComputeInertia(1, out var cylinderInertia);
convexHull.ComputeInertia(1, out var hullInertia);
var boxIndex = Simulation.Shapes.Add(box);
var capsuleIndex = Simulation.Shapes.Add(capsule);
var sphereIndex = Simulation.Shapes.Add(sphere);
var cylinderIndex = Simulation.Shapes.Add(cylinder);
var hullIndex = Simulation.Shapes.Add(convexHull);
const int width = 8;
const int height = 16;
const int length = 8;
var shapeCount = 0;
for (int i = 0; i < width; ++i)
{
for (int j = 0; j < height; ++j)
{
for (int k = 0; k < length; ++k)
{
var location = new Vector3(6, 3, 6) * new Vector3(i, j, k) + new Vector3(-width * 1.5f, 5.5f, -length * 1.5f);
var bodyDescription = new BodyDescription
{
Activity = new BodyActivityDescription(0.01f),
Pose = new RigidPose
{
Orientation = Quaternion.Identity,
Position = location
},
Collidable = new CollidableDescription
{
Continuity = new ContinuousDetectionSettings {
Mode = ContinuousDetectionMode.Discrete
},
SpeculativeMargin = 0.1f
}
};
var index = shapeCount++;
switch (2 + index % 5)
{
case 0:
bodyDescription.Collidable.Shape = sphereIndex;
bodyDescription.LocalInertia = sphereInertia;
break;
case 1:
bodyDescription.Collidable.Shape = capsuleIndex;
bodyDescription.LocalInertia = capsuleInertia;
break;
case 2:
bodyDescription.Collidable.Shape = boxIndex;
bodyDescription.LocalInertia = boxInertia;
break;
case 3:
bodyDescription.Collidable.Shape = cylinderIndex;
bodyDescription.LocalInertia = cylinderInertia;
break;
case 4:
bodyDescription.Collidable.Shape = hullIndex;
bodyDescription.LocalInertia = hullInertia;
break;
}
Simulation.Bodies.Add(bodyDescription);
}
}
}
DemoMeshHelper.CreateDeformedPlane(128, 128, (x, y) => new Vector3(x - 64, 2f * (float)(Math.Sin(x * 0.5f) * Math.Sin(y * 0.5f)), y - 64), new Vector3(4, 1, 4), BufferPool, out var mesh);
Simulation.Statics.Add(new StaticDescription(new Vector3(), new CollidableDescription(Simulation.Shapes.Add(mesh), 0.1f)));
}
public unsafe void ExtractLines(ref DistanceLimitPrestepData prestepBundle, int setIndex, int *bodyIndices,
Bodies bodies, ref Vector3 tint, ref QuickList <LineInstance> lines)
{
//Could do bundles of constraints at a time, but eh.
var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]];
var poseB = bodies.Sets[setIndex].Poses[bodyIndices[1]];
Vector3Wide.ReadFirst(prestepBundle.LocalOffsetA, out var localOffsetA);
Vector3Wide.ReadFirst(prestepBundle.LocalOffsetB, out var localOffsetB);
var minimumDistance = GatherScatter.GetFirst(ref prestepBundle.MinimumDistance);
var maximumDistance = GatherScatter.GetFirst(ref prestepBundle.MaximumDistance);
QuaternionEx.Transform(localOffsetA, poseA.Orientation, out var worldOffsetA);
QuaternionEx.Transform(localOffsetB, poseB.Orientation, out var worldOffsetB);
var endA = poseA.Position + worldOffsetA;
var endB = poseB.Position + worldOffsetB;
var color = new Vector3(0.2f, 0.2f, 1f) * tint;
var packedColor = Helpers.PackColor(color);
var backgroundColor = new Vector3(0f, 0f, 1f) * tint;
lines.AllocateUnsafely() = new LineInstance(poseA.Position, endA, packedColor, 0);
lines.AllocateUnsafely() = new LineInstance(poseB.Position, endB, packedColor, 0);
var offset = endB - endA;
var length = offset.Length();
var direction = length < 1e-9f ? new Vector3(1, 0, 0) : offset / length;
var errorColor = new Vector3(1, 0, 0) * tint;
var packedErrorColor = Helpers.PackColor(errorColor);
var packedFarColor = Helpers.PackColor(color * 0.5f);
var packedNearColor = Helpers.PackColor(color * 0.25f);
var minimumPoint = endA + direction * minimumDistance;
if (length >= minimumDistance && length <= maximumDistance)
{
//Create a darker bar to signify the minimum limit.
lines.AllocateUnsafely() = new LineInstance(endA, minimumPoint, packedNearColor, 0);
lines.AllocateUnsafely() = new LineInstance(minimumPoint, endB, packedFarColor, 0);
lines.AllocateUnsafely() = new LineInstance(new Vector3(float.MinValue), new Vector3(float.MinValue), 0, 0);
}
else if (length < minimumDistance)
{
//Too close; draw an error line extending beyond the connecting line.
lines.AllocateUnsafely() = new LineInstance(endA, endB, packedNearColor, 0);
lines.AllocateUnsafely() = new LineInstance(endB, endA + direction * minimumDistance, packedErrorColor, 0);
lines.AllocateUnsafely() = new LineInstance(new Vector3(float.MinValue), new Vector3(float.MinValue), 0, 0);
}
else
{
//Too far; draw an error line that extends from the desired endpoint to the current endpoint.
var targetEnd = endA + direction * maximumDistance;
lines.AllocateUnsafely() = new LineInstance(endA, minimumPoint, packedNearColor, 0);
lines.AllocateUnsafely() = new LineInstance(minimumPoint, targetEnd, packedFarColor, 0);
lines.AllocateUnsafely() = new LineInstance(targetEnd, endB, packedErrorColor, 0);
}
}