public void Test(Random random, int innerIterations)
{
GenerateRandomNormalizedVector(random, out var v1);
GenerateRandomNormalizedVector(random, out var v2);
for (int i = 0; i < innerIterations; ++i)
{
Quaternion.GetQuaternionBetweenNormalizedVectors(v1, v2, out var v1ToV2);
Quaternion.GetQuaternionBetweenNormalizedVectors(v2, v1, out var v2ToV1);
#if DEBUG
Quaternion.ConcatenateWithoutOverlap(v1ToV2, v2ToV1, out var concatenated);
Quaternion.Transform(v1, v1ToV2, out var v1TransformedToV2);
Quaternion.Transform(v2, v2ToV1, out var v2TransformedToV1);
Quaternion.Transform(v1, concatenated, out var v1TransformedToV1);
var v1ToV2ErrorLength = (v1TransformedToV2 - v2).LengthSquared();
var v2ToV1ErrorLength = (v2TransformedToV1 - v1).LengthSquared();
var v1ToV1ErrorLength = (v1TransformedToV1 - v1).LengthSquared();
const float epsilon = 1e-6f;
Debug.Assert(
v1ToV2ErrorLength < epsilon &&
v2ToV1ErrorLength < epsilon &&
v1ToV1ErrorLength < epsilon);
#endif
}
}
public unsafe void ExtractLines(ref BallSocketPrestepData prestepBundle, int innerIndex, int setIndex, int *bodyIndices,
Bodies bodies, ref Vector3 tint, ref QuickList <LineInstance, Array <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.LocalOffsetA, innerIndex, out var localOffsetA);
Vector3Wide.ReadSlot(ref prestepBundle.LocalOffsetB, innerIndex, out var localOffsetB);
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;
var lineA = new LineInstance(poseA.Position, endA, packedColor, 0);
var lineB = new LineInstance(poseB.Position, endB, packedColor, 0);
lines.AddUnsafely(ref lineA);
lines.AddUnsafely(ref lineB);
var errorColor = new Vector3(1, 0, 0) * tint;
var packedErrorColor = Helpers.PackColor(errorColor);
var errorLine = new LineInstance(endA, endB, packedErrorColor, 0);
lines.AddUnsafely(ref errorLine);
}
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);
Quaternion.Transform(localOffsetA, poseA.Orientation, out var worldOffsetA);
Quaternion.Transform(localDirection, poseA.Orientation, out var worldDirection);
Quaternion.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 override void Update(Window window, Camera camera, Input input, float dt)
{
if (input.WasPushed(OpenTK.Input.Key.Z))
{
//INVEST TODAY FOR INCREDIBLE RETURNS DON'T MISS OUT LOOK AT THE COINS THERE ARE A LOT OF THEM AND THEY COULD BE YOURS
var origin = new Vector3(-30, 5, -30) + new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()) * new Vector3(60, 30, 60);
for (int i = 0; i < 128; ++i)
{
var direction = new Vector3(-1) + 2 * new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble());
var length = direction.Length();
if (length > 1e-7f)
{
direction /= length;
}
else
{
direction = new Vector3(0, 1, 0);
}
coinDescription.Pose.Position = origin + direction * 10 * (float)random.NextDouble();
coinDescription.Pose.Orientation = Quaternion.Normalize(new Quaternion(0.01f + (float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()));
coinDescription.Velocity.Linear = direction * (5 + 30 * (float)random.NextDouble());
Simulation.Bodies.Add(coinDescription);
}
}
base.Update(window, camera, input, dt);
}
public override void Initialize(ContentArchive content, Camera camera)
{
camera.Position = new Vector3(-13f, 6, -13f);
camera.Yaw = MathF.PI * 3f / 4;
camera.Pitch = MathF.PI * 0.05f;
Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)));
{
var shapeA = new Cylinder(0.5f, 1f);
var poseA = new RigidPose(new Vector3(0, 0, 0));
var shapeB = new Cylinder(1f, 2f);
//var positionB = new Vector3(-0.2570486f, 1.780561f, -1.033215f);
//var localOrientationBMatrix = new Matrix3x3
//{
// X = new Vector3(0.9756086f, 0.1946615f, 0.101463f),
// Y = new Vector3(-0.1539477f, 0.9362175f, -0.3159063f),
// Z = new Vector3(-0.1564862f, 0.2925809f, 0.9433496f)
//};
//var positionB = new Vector3(-1.437585f, 0.386236f, -1.124907f);
var positionB = new Vector3(-0.437585f, 0.386236f, -.124907f);
var localOrientationBMatrix = new Matrix3x3
{
X = new Vector3(-0.7615921f, 0.001486331f, -0.648055f),
Y = new Vector3(0.6341797f, 0.2075436f, -0.7448099f),
Z = new Vector3(-0.1333926f, -0.9782246f, -0.1590062f)
};
//var poseB = new RigidPose(new Vector3(-0.2570486f, 1.780561f, -1.033215f), Quaternion.CreateFromAxisAngle(Vector3.Normalize(new Vector3(1, 1, 1)), MathF.PI * 0.35f));
var poseB = new RigidPose(positionB, Quaternion.CreateFromRotationMatrix(localOrientationBMatrix));
basePosition = default;
shapeLines = MinkowskiShapeVisualizer.CreateLines <Cylinder, CylinderWide, CylinderSupportFinder, Cylinder, CylinderWide, CylinderSupportFinder>(
shapeA, shapeB, poseA, poseB, 65536,
0.01f, new Vector3(0.4f, 0.4f, 0),
0.1f, new Vector3(0, 1, 0), default, basePosition, BufferPool);
static void CreateBallSocket(ref RigidPose a, ref RigidPose b, out BallSocket description)
{
var midpoint = 0.5f * (a.Position + b.Position);
description.LocalOffsetA = Quaternion.Transform(midpoint - a.Position, Quaternion.Conjugate(a.Orientation));
description.LocalOffsetB = Quaternion.Transform(midpoint - b.Position, Quaternion.Conjugate(b.Orientation));
description.SpringSettings = new SpringSettings(15, 0.1f);
}
public static void RotateInertia(ref Triangular3x3 localInertiaTensor, ref Quaternion orientation, out Triangular3x3 rotatedInertiaTensor)
{
Matrix3x3.CreateFromQuaternion(ref orientation, out var orientationMatrix);
//I^-1 = RT * Ilocal^-1 * R
//NOTE: If you were willing to confuse users a little bit, the local inertia could be required to be diagonal.
//This would be totally fine for all the primitive types which happen to have diagonal inertias, but for more complex shapes (convex hulls, meshes),
//there would need to be a reorientation step. That could be confusing, and it's probably not worth it.
Triangular3x3.RotationSandwich(ref orientationMatrix, ref localInertiaTensor, out rotatedInertiaTensor);
}
void GenerateObjects(Random rnd)
{
UInt16 Id = 0;
float RndDeg()
{
return(MathHelper.ToRadians(rnd.Next(0, 360)));
}
int count = rnd.Next(30, 30);
for (int i = 0; i < count; i++)
{
Vector3 location = new Vector3
(
rnd.Next(-500, 500),
rnd.Next(-200, 200),
rnd.Next(-500, 500)
);
Quat rotation = Quat.CreateFromYawPitchRoll
(
RndDeg(),
RndDeg(),
RndDeg()
);
float scale;
if (rnd.Next(5) == 0)
{
scale = 10 + (float)(rnd.NextDouble() * 20);
}
else
{
scale = 1 + (float)(rnd.NextDouble() * 4);
}
int ObjectType = rnd.Next(2, 5);
Objects.Add(Guid.NewGuid(), new SpaceObject()
{
CollisionMesh = (ListOf_CollisionMesh)ObjectType,
Location = location,
Rotation = rotation,
Scale = new Vector3(scale),
// Scale = new Vector3(1),
Id = Id++
});
}
Objects.Add(Guid.NewGuid(), new SpaceObject()
{
CollisionMesh = ListOf_CollisionMesh.XQ6,
Location = new Vector3(0, 0, 1000),
Rotation = Quat.CreateFromYawPitchRoll(0, 0, MathHelper.ToRadians(45)),
Scale = Vector3.One,
Id = Id++
});
}
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);
}
}
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);
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);
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);
}
}
static void CreateBallSocket(ref RigidPose a, ref RigidPose b, out BallSocket description)
{
var midpoint = 0.5f * (a.Position + b.Position);
description.LocalOffsetA = Quaternion.Transform(midpoint - a.Position, Quaternion.Conjugate(a.Orientation));
description.LocalOffsetB = Quaternion.Transform(midpoint - b.Position, Quaternion.Conjugate(b.Orientation));
description.SpringSettings = new SpringSettings
{
NaturalFrequency = (float)(Math.PI * 0.5f * 60),
DampingRatio = 0.1f
};
}
void CreateRingPlatform(Vector3 position, Box ringBoxShape, BodyDescription bodyDescription, float radius)
{
var innerCircumference = (float)Math.PI * 2 * (radius - ringBoxShape.HalfLength);
var boxCount = (int)(0.95f * innerCircumference / ringBoxShape.Height);
float increment = MathHelper.TwoPi / boxCount;
for (int i = 0; i < boxCount; i++)
{
var angle = i * increment;
bodyDescription.Pose = new RigidPose(
position + new Vector3(-(float)Math.Cos(angle) * radius, ringBoxShape.HalfWidth, (float)Math.Sin(angle) * radius),
Quaternion.Concatenate(Quaternion.CreateFromAxisAngle(Vector3.UnitZ, (float)Math.PI * 0.5f), Quaternion.CreateFromAxisAngle(Vector3.UnitY, angle + (float)Math.PI * 0.5f)));
Simulation.Bodies.Add(bodyDescription);
}
}
public unsafe override void Initialize(ContentArchive content, Camera camera)
{
camera.Position = new Vector3(0, 9, -40);
camera.Yaw = MathHelper.Pi;
camera.Pitch = 0;
var filters = new BodyProperty <SubgroupCollisionFilter>();
Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks {
CollisionFilters = filters
}, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)));
int ragdollIndex = 0;
var spacing = new Vector3(1.7f, 1.8f, 0.5f);
int width = 4;
int height = 4;
int length = 44;
var origin = -0.5f * spacing * new Vector3(width - 1, 0, length - 1) + new Vector3(0, 5f, 0);
for (int i = 0; i < width; ++i)
{
for (int j = 0; j < height; ++j)
{
for (int k = 0; k < length; ++k)
{
RagdollDemo.AddRagdoll(origin + spacing * new Vector3(i, j, k), Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), MathHelper.Pi * 0.05f), ragdollIndex++, filters, Simulation);
}
}
}
var tubeCenter = new Vector3(0, 8, 0);
const int panelCount = 20;
const float tubeRadius = 6;
var panelShape = new Box((float)Math.PI * 2 * tubeRadius / panelCount, 1, 80);
var panelShapeIndex = Simulation.Shapes.Add(panelShape);
var builder = new CompoundBuilder(BufferPool, Simulation.Shapes, panelCount + 1);
for (int i = 0; i < panelCount; ++i)
{
var rotation = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, i * MathHelper.TwoPi / panelCount);
Quaternion.TransformUnitY(rotation, out var localUp);
var position = localUp * tubeRadius;
builder.AddForKinematic(panelShapeIndex, new RigidPose(position, rotation), 1);
}
builder.AddForKinematic(Simulation.Shapes.Add(new Box(1, 2, panelShape.Length)), new RigidPose(new Vector3(0, tubeRadius - 1, 0)), 0);
builder.BuildKinematicCompound(out var children);
var compound = new BigCompound(children, Simulation.Shapes, BufferPool);
Simulation.Bodies.Add(BodyDescription.CreateKinematic(tubeCenter, new BodyVelocity(default, new Vector3(0, 0, .25f)), new CollidableDescription(Simulation.Shapes.Add(compound), 0.1f), new BodyActivityDescription()));
static void GetUnitQuaternion(Random random, out Quaternion orientation)
{
//Not much cleverness involved here. This does not produce a uniform distribution over the the unit sphere.
float length;
do
{
orientation = new Quaternion(
(float)random.NextDouble() * 2 - 1,
(float)random.NextDouble() * 2 - 1,
(float)random.NextDouble() * 2 - 1,
(float)random.NextDouble() * 2 - 1);
length = orientation.Length();
} while (length < 1e-7f);
Unsafe.As <Quaternion, Vector4>(ref orientation) /= length;
}
public override void Initialize(ContentArchive content, Camera camera)
{
camera.Position = new Vector3(-13f, 6, -13f);
camera.Yaw = MathF.PI * 3f / 4;
camera.Pitch = MathF.PI * 0.05f;
Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)));
var shapeA = new Cylinder(0.5f, 0.5f);
var poseA = new RigidPose(new Vector3(0, 0, 0));
var shapeB = new Cylinder(0.5f, 0.5f);
var poseB = new RigidPose(new Vector3(0.5f, 0.5f, 0.5f), Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), MathF.PI * 0.5f));
basePosition = default;
shapeLines = MinkowskiShapeVisualizer.CreateLines <Cylinder, CylinderWide, CylinderSupportFinder, Cylinder, CylinderWide, CylinderSupportFinder>(
shapeA, shapeB, poseA, poseB, 65536,
0.01f, new Vector3(0.4f, 0.4f, 0),
0.1f, new Vector3(0, 1, 0), default, basePosition, BufferPool);
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);
}
public unsafe void ExtractLines(ref BallSocketMotorPrestepData 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.LocalOffsetB, out var localOffsetB);
Quaternion.Transform(localOffsetB, poseB.Orientation, out var worldOffsetB);
var anchor = 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, anchor, packedColor, 0);
lines.AllocateUnsafely() = new LineInstance(poseB.Position, anchor, packedColor, 0);
}
void CreateRingWall(Vector3 position, Box ringBoxShape, BodyDescription bodyDescription, int height, float radius)
{
var circumference = (float)Math.PI * 2 * radius;
var boxCountPerRing = (int)(0.9f * circumference / ringBoxShape.Length);
float increment = MathHelper.TwoPi / boxCountPerRing;
for (int ringIndex = 0; ringIndex < height; ringIndex++)
{
for (int i = 0; i < boxCountPerRing; i++)
{
var angle = ((ringIndex & 1) == 0 ? i + 0.5f : i) * increment;
bodyDescription.Pose = new RigidPose(
position + new Vector3(-(float)Math.Cos(angle) * radius, (ringIndex + 0.5f) * ringBoxShape.Height, (float)Math.Sin(angle) * radius),
Quaternion.CreateFromAxisAngle(Vector3.UnitY, angle));
Simulation.Bodies.Add(bodyDescription);
}
}
}
public override void Initialize(ContentArchive content, Camera camera)
{
camera.Yaw = 0;
camera.Pitch = 0;
camera.Position = new Vector3(0, 0, 5);
Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, 0, 0)));
var a = new Cylinder(1f, 1f);
var b = new Cylinder(1f, 1f);
var localOffsetB = new Vector3(.1f, 1.1f, 0.1f);
var localOrientationB = Quaternion.CreateFromAxisAngle(Vector3.Normalize(new Vector3(1, 1, 1)), MathHelper.Pi * 1.5f);
Simulation.Bodies.Add(BodyDescription.CreateConvexKinematic(new Vector3(), Simulation.Shapes, a));
Simulation.Bodies.Add(BodyDescription.CreateConvexKinematic(new RigidPose(localOffsetB, localOrientationB), Simulation.Shapes, b));
CylinderWide aWide = default, bWide = default;
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 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 void Test(Random random, int innerIterations)
{
Quaternion q;
q.X = (float)random.NextDouble() * 2 - 1;
q.Y = (float)random.NextDouble() * 2 - 1;
q.Z = (float)random.NextDouble() * 2 - 1;
q.W = (float)random.NextDouble() * 2 - 1;
Quaternion.Normalize(ref q);
for (int i = 0; i < innerIterations; ++i)
{
Matrix3x3.CreateFromQuaternion(ref q, out var r);
Quaternion.CreateFromRotationMatrix(ref r, out var qTest);
#if DEBUG
const float epsilon = 1e-6f;
var lengthX = r.X.Length();
var lengthY = r.Y.Length();
var lengthZ = r.Z.Length();
Debug.Assert(
Math.Abs(1 - lengthX) < epsilon &&
Math.Abs(1 - lengthY) < epsilon &&
Math.Abs(1 - lengthZ) < epsilon);
if (qTest.X * q.X < 0)
{
Quaternion.Negate(ref qTest, out qTest);
}
Debug.Assert(
Math.Abs(qTest.X - q.X) < epsilon &&
Math.Abs(qTest.Y - q.Y) < epsilon &&
Math.Abs(qTest.Z - q.Z) < epsilon &&
Math.Abs(qTest.W - q.W) < epsilon);
#endif
}
}
public override void ProcessBlock(float deltaTime, BlockAccessor block)
{
var entities = block.GetEntityData();
var colliders = block.GetComponentData <InternalColliderHandle>();
var pos = block.GetReadOnlyComponentData <Position>();
var rot = block.GetReadOnlyComponentData <Rotation>();
for (int i = 0; i < block.length; i++)
{
TypedIndex shapeIdx = colliders[i].shapeIdx;
Vector3 p = new Vector3(pos[i].value.x, pos[i].value.y, pos[i].value.z);
BepuUtilities.Quaternion r = new BepuUtilities.Quaternion(rot[i].value.x, rot[i].value.y, rot[i].value.z, rot[i].value.w);
int handle;
afterUpdateCommands.AddComponent(entities[i],
new InternalStaticBodyHandle()
{
staticBodyHandle = handle =
PhysicsSystem.Simulation.Statics.Add(
new StaticDescription()
{
Collidable = new CollidableDescription(
shapeIdx, 0.1f,
ContinuousDetectionSettings.Passive),
Pose = new RigidPose()
{
Position = p,
Orientation = r
}
}
)
});
PhysicsSystem.staticBodyEntityDictionary.Add(handle, entities[i]);
}
}
public unsafe override void Initialize(ContentArchive content, Camera camera)
{
camera.Position = new Vector3(-32f, 20.5f, 61f);
camera.Yaw = MathHelper.Pi * 0.3f;
camera.Pitch = MathHelper.Pi * -0.05f;
filters = new BodyProperty <SubgroupCollisionFilter>(BufferPool);
Simulation = Simulation.Create(BufferPool, new SubgroupFilteredCallbacks()
{
CollisionFilters = filters
}, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)));
Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(1500, 1, 1500)), 0.1f)));
Simulation.Statics.Add(new StaticDescription(new Vector3(0, 10, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(70, 20, 80)), 0.1f)));
Simulation.Statics.Add(new StaticDescription(new Vector3(0, 7.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(80, 15, 90)), 0.1f)));
Simulation.Statics.Add(new StaticDescription(new Vector3(0, 5, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(90, 10, 100)), 0.1f)));
Simulation.Statics.Add(new StaticDescription(new Vector3(0, 2.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(100, 5, 110)), 0.1f)));
//High fidelity simulation isn't super important on this one.
Simulation.Solver.IterationCount = 2;
DemoMeshHelper.LoadModel(content, BufferPool, "Content\\newt.obj", new Vector3(30), out var mesh);
Simulation.Statics.Add(new StaticDescription(new Vector3(0, 20, 0), Quaternion.CreateFromAxisAngle(Vector3.UnitY, 0), new CollidableDescription(Simulation.Shapes.Add(mesh), 0.1f)));
}
public unsafe override void Initialize(ContentArchive content, Camera camera)
{
camera.Position = new Vector3(-5f, 5.5f, 5f);
camera.Yaw = MathHelper.Pi / 4;
camera.Pitch = MathHelper.Pi * 0.15f;
var filters = new BodyProperty <DeformableCollisionFilter>();
Simulation = Simulation.Create(BufferPool, new DeformableCallbacks {
Filters = filters
}, new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)));
var meshContent = content.Load <MeshContent>("Content\\newt.obj");
float cellSize = 0.1f;
DumbTetrahedralizer.Tetrahedralize(meshContent.Triangles, cellSize, BufferPool,
out var vertices, out var vertexSpatialIndices, out var cellVertexIndices, out var tetrahedraVertexIndices);
var weldSpringiness = new SpringSettings(30f, 0);
var volumeSpringiness = new SpringSettings(30f, 1);
for (int i = 0; i < 5; ++i)
{
NewtDemo.CreateDeformable(Simulation, new Vector3(i * 3, 5 + i * 1.5f, 0), Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), MathF.PI * (i * 0.55f)), 1f, cellSize, weldSpringiness, volumeSpringiness, i, filters, ref vertices, ref vertexSpatialIndices, ref cellVertexIndices, ref tetrahedraVertexIndices);
}
BufferPool.Return(ref vertices);
vertexSpatialIndices.Dispose(BufferPool);
BufferPool.Return(ref cellVertexIndices);
BufferPool.Return(ref tetrahedraVertexIndices);
Simulation.Bodies.Add(BodyDescription.CreateConvexDynamic(new Vector3(0, 100, -.5f), 10, Simulation.Shapes, new Sphere(5)));
Simulation.Statics.Add(new StaticDescription(new Vector3(0, -0.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Box(1500, 1, 1500)), 0.1f)));
Simulation.Statics.Add(new StaticDescription(new Vector3(0, -1.5f, 0), new CollidableDescription(Simulation.Shapes.Add(new Sphere(3)), 0.1f)));
var bulletShape = new Sphere(0.5f);
bulletShape.ComputeInertia(.25f, out var bulletInertia);
bulletDescription = BodyDescription.CreateDynamic(RigidPose.Identity, bulletInertia, new CollidableDescription(Simulation.Shapes.Add(bulletShape), 1f), new BodyActivityDescription(0.01f));
DemoMeshHelper.LoadModel(content, BufferPool, "Content\\newt.obj", new Vector3(20), out var mesh);
Simulation.Statics.Add(new StaticDescription(new Vector3(200, 0.5f, 120), Quaternion.CreateFromAxisAngle(Vector3.UnitY, -3 * MathHelper.PiOver4), new CollidableDescription(Simulation.Shapes.Add(mesh), 0.1f)));
}
public unsafe override void Initialize(ContentArchive content, Camera camera)
{
camera.Position = new Vector3(20, 10, 20);
camera.Yaw = MathF.PI;
camera.Pitch = 0;
characters = new CharacterControllers(BufferPool);
Simulation = Simulation.Create(BufferPool, new CharacterNarrowphaseCallbacks(characters), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)));
CreateCharacter(new Vector3(0, 2, -4));
//Create a bunch of legos to hurt your feet on.
var random = new Random(5);
var origin = new Vector3(-3f, 0.5f, 0);
var spacing = new Vector3(0.5f, 0, -0.5f);
for (int i = 0; i < 12; ++i)
{
for (int j = 0; j < 12; ++j)
{
var position = origin + new Vector3(i, 0, j) * spacing;
var orientation = Quaternion.CreateFromAxisAngle(Vector3.Normalize(new Vector3(0.0001f) + new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble())), 10 * (float)random.NextDouble());
var shape = new Box(0.1f + 0.3f * (float)random.NextDouble(), 0.1f + 0.3f * (float)random.NextDouble(), 0.1f + 0.3f * (float)random.NextDouble());
var collidable = new CollidableDescription(Simulation.Shapes.Add(shape), 0.1f);
shape.ComputeInertia(1, out var inertia);
var choice = (i + j) % 3;
switch (choice)
{
case 0:
Simulation.Bodies.Add(BodyDescription.CreateDynamic(new RigidPose(position, orientation), inertia, collidable, new BodyActivityDescription(0.01f)));
break;
case 1:
Simulation.Bodies.Add(BodyDescription.CreateKinematic(new RigidPose(position, orientation), collidable, new BodyActivityDescription(0.01f)));
break;
case 2:
Simulation.Statics.Add(new StaticDescription(position, orientation, collidable));
break;
}
}
}
//Add some spinning fans to get slapped by.
var bladeDescription = BodyDescription.CreateConvexDynamic(new Vector3(), 3, Simulation.Shapes, new Box(10, 0.2f, 2));
var bladeBaseDescription = BodyDescription.CreateConvexKinematic(new Vector3(), Simulation.Shapes, new Box(0.2f, 1, 0.2f));
for (int i = 0; i < 3; ++i)
{
bladeBaseDescription.Pose.Position = new Vector3(-22, 1, i * 11);
bladeDescription.Pose.Position = new Vector3(-22, 1.7f, i * 11);
var baseHandle = Simulation.Bodies.Add(bladeBaseDescription);
var bladeHandle = Simulation.Bodies.Add(bladeDescription);
Simulation.Solver.Add(baseHandle, bladeHandle,
new Hinge
{
LocalHingeAxisA = Vector3.UnitY,
LocalHingeAxisB = Vector3.UnitY,
LocalOffsetA = new Vector3(0, 0.7f, 0),
LocalOffsetB = new Vector3(0, 0, 0),
SpringSettings = new SpringSettings(30, 1)
});
Simulation.Solver.Add(baseHandle, bladeHandle,
new AngularAxisMotor
{
LocalAxisA = Vector3.UnitY,
TargetVelocity = (i + 1) * (i + 1) * (i + 1) * (i + 1) * 0.2f,
Settings = new MotorSettings(5 * (i + 1), 0.0001f)
});
}
//Include a giant newt to test character-newt behavior and to ensure thematic consistency.
DemoMeshHelper.LoadModel(content, BufferPool, @"Content\newt.obj", new Vector3(15, 15, 15), out var newtMesh);
Simulation.Statics.Add(new StaticDescription(new Vector3(0, 0.5f, 0), new CollidableDescription(Simulation.Shapes.Add(newtMesh), 0.1f)));
//Give the newt a tongue, I guess.
var tongueBase = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(0, 8.4f, 24), default, default));
public unsafe override void Initialize(ContentArchive content, Camera camera)
{
camera.Position = new Vector3(-13f, 6, -13f);
camera.Yaw = MathHelper.Pi * 3f / 4;
camera.Pitch = MathHelper.Pi * 0.05f;
Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10, 0)));
using (var compoundBuilder = new CompoundBuilder(BufferPool, Simulation.Shapes, 8))
{
{
//Note that, in bepuphysics v2, there is no 'recentering' when constructing a shape. The pose you pass in for a child is exactly the pose that the compound will use,
//even if the 'true' center of mass isn't at the local origin.
//Instead, if recentering is desired, it should performed ahead of time. The CompoundBuilder can help with this.
//We'll construct this compound using shapes far from the origin, and then use the CompoundBuilder overload that recenters the children and outputs the computed center.
var capsuleChildShape = new Capsule(0.5f, 0.5f);
var capsuleLocalPose = new RigidPose {
Position = new Vector3(-0.5f, 4, 4), Orientation = Quaternion.Identity
};
var boxChildShape = new Box(0.5f, 1f, 1.5f);
var boxLocalPose = new RigidPose {
Position = new Vector3(0.5f, 4, 4), Orientation = Quaternion.Identity
};
//All allocations from the buffer pool used for the final compound shape will be disposed when the demo is disposed. Don't have to worry about leaks in these demos.
compoundBuilder.Add(capsuleChildShape, capsuleLocalPose, 1);
compoundBuilder.Add(boxChildShape, boxLocalPose, 1);
compoundBuilder.BuildDynamicCompound(out var compoundChildren, out var compoundInertia, out var compoundCenter);
compoundBuilder.Reset();
var compound = new Compound(compoundChildren);
var compoundDescription = new BodyDescription
{
Activity = new BodyActivityDescription {
SleepThreshold = 0.01f, MinimumTimestepCountUnderThreshold = 32
},
Collidable = new CollidableDescription
{
Shape = Simulation.Shapes.Add(compound),
SpeculativeMargin = 0.1f,
},
LocalInertia = compoundInertia,
Pose = new RigidPose {
Position = compoundCenter, Orientation = Quaternion.Identity
},
};
Simulation.Bodies.Add(compoundDescription);
}
//Build a stack of sphere grids to stress manifold reduction heuristics in a convex-ish situation.
{
var gridShape = new Sphere(0.5f);
const float gridSpacing = 1.5f;
const int gridWidth = 3;
var gridShapeIndex = Simulation.Shapes.Add(gridShape);
gridShape.ComputeInertia(1, out var gridBoxInertia);
float localPoseOffset = -0.5f * gridSpacing * (gridWidth - 1);
for (int i = 0; i < gridWidth; ++i)
{
for (int j = 0; j < gridWidth; ++j)
{
var localPose = new RigidPose
{
Orientation = Quaternion.Identity,
Position = new Vector3(localPoseOffset, 0, localPoseOffset) + new Vector3(gridSpacing) * new Vector3(i, 0, j)
};
compoundBuilder.Add(gridShapeIndex, localPose, gridBoxInertia.InverseInertiaTensor, 1);
}
}
compoundBuilder.BuildDynamicCompound(out var gridChildren, out var gridInertia, out var center);
compoundBuilder.Reset();
var gridCompound = new Compound(gridChildren);
var bodyDescription = new BodyDescription
{
Activity = new BodyActivityDescription {
SleepThreshold = 0.01f, MinimumTimestepCountUnderThreshold = 32
},
Collidable = new CollidableDescription
{
Shape = Simulation.Shapes.Add(gridCompound),
SpeculativeMargin = 0.1f,
},
LocalInertia = gridInertia,
Pose = new RigidPose {
Orientation = Quaternion.Identity
}
};
for (int i = 0; i < 4; ++i)
{
bodyDescription.Pose.Position = new Vector3(0, 2 + i * 3, 0);
//if (i == 0)
// gridDescription.LocalInertia = new BodyInertia();
//else
// gridDescription.LocalInertia = gridInertia;
Simulation.Bodies.Add(bodyDescription);
}
}
//Build a table and use it for a couple of different tests.
{
var legShape = new Box(0.2f, 1, 0.2f);
legShape.ComputeInertia(1f, out var legInverseInertia);
var legShapeIndex = Simulation.Shapes.Add(legShape);
var legPose0 = new RigidPose {
Position = new Vector3(-1.5f, 0, -1.5f), Orientation = Quaternion.Identity
};
var legPose1 = new RigidPose {
Position = new Vector3(-1.5f, 0, 1.5f), Orientation = Quaternion.Identity
};
var legPose2 = new RigidPose {
Position = new Vector3(1.5f, 0, -1.5f), Orientation = Quaternion.Identity
};
var legPose3 = new RigidPose {
Position = new Vector3(1.5f, 0, 1.5f), Orientation = Quaternion.Identity
};
compoundBuilder.Add(legShapeIndex, legPose0, legInverseInertia.InverseInertiaTensor, 1);
compoundBuilder.Add(legShapeIndex, legPose1, legInverseInertia.InverseInertiaTensor, 1);
compoundBuilder.Add(legShapeIndex, legPose2, legInverseInertia.InverseInertiaTensor, 1);
compoundBuilder.Add(legShapeIndex, legPose3, legInverseInertia.InverseInertiaTensor, 1);
var tableTopPose = new RigidPose {
Position = new Vector3(0, 0.6f, 0), Orientation = Quaternion.Identity
};
var tableTopShape = new Box(3.2f, 0.2f, 3.2f);
compoundBuilder.Add(tableTopShape, tableTopPose, 3);
compoundBuilder.BuildDynamicCompound(out var tableChildren, out var tableInertia, out var tableCenter);
compoundBuilder.Reset();
var table = new Compound(tableChildren);
var tableDescription = new BodyDescription
{
Activity = new BodyActivityDescription {
SleepThreshold = 0.01f, MinimumTimestepCountUnderThreshold = 32
},
Collidable = new CollidableDescription
{
Shape = Simulation.Shapes.Add(table),
SpeculativeMargin = 0.1f,
},
LocalInertia = tableInertia,
Pose = new RigidPose {
Orientation = Quaternion.Identity
}
};
//Stack some tables.
{
for (int i = 0; i < 10; ++i)
{
tableDescription.Pose.Position = new Vector3(10, 3 + i * 1.4f, 10);
Simulation.Bodies.Add(tableDescription);
}
}
{
for (int k = 0; k < 5; ++k)
{
tableDescription.Pose.Position = new Vector3(64 + k * 3, 6 + k * 1.4f, 32);
Simulation.Bodies.Add(tableDescription);
}
//for (int i = 0; i < 10; ++i)
//{
// for (int j = 0; j < 20; ++j)
// {
// for (int k = 0; k < 10; ++k)
// {
// tableDescription.Pose.Position = new Vector3(32 + i * 6, 6 + j * 1.4f, 16 + k * 6);
// Simulation.Bodies.Add(tableDescription);
// }
// }
//}
}
//Put a table on top of a sphere to stress out nonconvex reduction for divergent normals.
{
tableDescription.Pose.Position = new Vector3(10, 6, 0);
Simulation.Bodies.Add(tableDescription);
var sphereShape = new Sphere(3);
var sphereIndex = Simulation.Shapes.Add(sphereShape);
var sphereDescription = new StaticDescription
{
Collidable = new CollidableDescription
{
Shape = sphereIndex,
SpeculativeMargin = 0.1f,
},
Pose = new RigidPose {
Position = new Vector3(10, 2, 0), Orientation = Quaternion.Identity
}
};
Simulation.Statics.Add(sphereDescription);
}
//Put another table on the ground, but with a clamp-ish thing on it that generates opposing normals.
{
tableDescription.Pose.Position = new Vector3(10, 3, -10);
Simulation.Bodies.Add(tableDescription);
var clampPieceShape = new Box(2f, 0.1f, 0.3f);
clampPieceShape.ComputeInertia(1f, out var clampPieceInverseInertia);
var clampPieceShapeIndex = Simulation.Shapes.Add(clampPieceShape);
var clamp0 = new RigidPose {
Position = new Vector3(0, -0.2f, -1.1f), Orientation = Quaternion.Identity
};
var clamp1 = new RigidPose {
Position = new Vector3(0, 0.2f, -1.1f), Orientation = Quaternion.Identity
};
var clamp2 = new RigidPose {
Position = new Vector3(0, -0.2f, 0), Orientation = Quaternion.Identity
};
var clamp3 = new RigidPose {
Position = new Vector3(0, 0.2f, 0), Orientation = Quaternion.Identity
};
var clamp4 = new RigidPose {
Position = new Vector3(0, -0.2f, 1.1f), Orientation = Quaternion.Identity
};
var clamp5 = new RigidPose {
Position = new Vector3(0, 0.2f, 1.1f), Orientation = Quaternion.Identity
};
compoundBuilder.Add(clampPieceShapeIndex, clamp0, clampPieceInverseInertia.InverseInertiaTensor, 1);
compoundBuilder.Add(clampPieceShapeIndex, clamp1, clampPieceInverseInertia.InverseInertiaTensor, 1);
compoundBuilder.Add(clampPieceShapeIndex, clamp2, clampPieceInverseInertia.InverseInertiaTensor, 1);
compoundBuilder.Add(clampPieceShapeIndex, clamp3, clampPieceInverseInertia.InverseInertiaTensor, 1);
compoundBuilder.Add(clampPieceShapeIndex, clamp4, clampPieceInverseInertia.InverseInertiaTensor, 1);
compoundBuilder.Add(clampPieceShapeIndex, clamp5, clampPieceInverseInertia.InverseInertiaTensor, 1);
compoundBuilder.BuildDynamicCompound(out var clampChildren, out var clampInertia, out var clampCenter);
compoundBuilder.Reset();
var clamp = new Compound(clampChildren);
var clampDescription = new BodyDescription
{
Activity = new BodyActivityDescription {
SleepThreshold = 0.01f, MinimumTimestepCountUnderThreshold = 32
},
Collidable = new CollidableDescription
{
Shape = Simulation.Shapes.Add(clamp),
SpeculativeMargin = 0.1f,
},
LocalInertia = clampInertia,
Pose = new RigidPose {
Position = tableDescription.Pose.Position + new Vector3(2f, 0.3f, 0), Orientation = Quaternion.Identity
}
};
Simulation.Bodies.Add(clampDescription);
}
}
//Create a tree-accelerated big compound.
{
var random = new Random(5);
var treeCompoundBoxShape = new Box(0.5f, 1.5f, 1f);
var treeCompoundBoxShapeIndex = Simulation.Shapes.Add(treeCompoundBoxShape);
treeCompoundBoxShape.ComputeInertia(1, out var childInertia);
for (int i = 0; i < 128; ++i)
{
RigidPose localPose;
localPose.Position = new Vector3(12, 6, 12) * (0.5f * new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()) - Vector3.One);
float orientationLengthSquared;
do
{
localPose.Orientation = new Quaternion((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble());
}while ((orientationLengthSquared = localPose.Orientation.LengthSquared()) < 1e-9f);
Quaternion.Scale(localPose.Orientation, 1f / MathF.Sqrt(orientationLengthSquared), out localPose.Orientation);
//Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), MathF.PI, out localPose.Orientation);
compoundBuilder.Add(treeCompoundBoxShapeIndex, localPose, childInertia.InverseInertiaTensor, 1);
}
compoundBuilder.BuildDynamicCompound(out var children, out var inertia, out var center);
compoundBuilder.Reset();
var compound = new BigCompound(children, Simulation.Shapes, BufferPool);
//var compound = new Compound(children);
var compoundIndex = Simulation.Shapes.Add(compound);
for (int i = 0; i < 8; ++i)
{
Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 4 + 5 * i, 32), inertia, new CollidableDescription(compoundIndex, 0.1f), new BodyActivityDescription(0.01f)));
}
}
}
//Prevent stuff from falling into the infinite void.
{
var boxShape = new Box(256, 1, 256);
var groundShapeIndex = Simulation.Shapes.Add(boxShape);
var groundDescription = new StaticDescription
{
Collidable = new CollidableDescription
{
Shape = groundShapeIndex,
SpeculativeMargin = 0.1f,
},
Pose = new RigidPose {
Position = new Vector3(0, 0, 0), Orientation = Quaternion.Identity
}
};
Simulation.Statics.Add(groundDescription);
}
const int planeWidth = 48;
const int planeHeight = 48;
MeshDemo.CreateDeformedPlane(planeWidth, planeHeight,
(int x, int y) =>
{
Vector2 offsetFromCenter = new Vector2(x - planeWidth / 2, y - planeHeight / 2);
return(new Vector3(offsetFromCenter.X, MathF.Cos(x / 4f) * MathF.Sin(y / 4f) - 0.01f * offsetFromCenter.LengthSquared(), offsetFromCenter.Y));
}, new Vector3(2, 1, 2), BufferPool, out var planeMesh);
Simulation.Statics.Add(new StaticDescription(new Vector3(64, 4, 32), Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2),
new CollidableDescription(Simulation.Shapes.Add(planeMesh), 0.1f)));
}
//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);
}
public unsafe override void Initialize(ContentArchive content, Camera camera)
{
camera.Position = new Vector3(25, 4, 40);
camera.Yaw = 0;
Simulation = Simulation.Create(BufferPool, new TestCallbacks());
Simulation.PoseIntegrator.Gravity = new Vector3(0, -10, 0);
var shapeA = new Box(.75f, 1, .5f);
var shapeIndexA = Simulation.Shapes.Add(shapeA);
var collidableA = new CollidableDescription(shapeIndexA, 0.1f);
var shapeB = new Box(.75f, 1, .5f);
var shapeIndexB = Simulation.Shapes.Add(shapeB);
var collidableB = new CollidableDescription(shapeIndexB, 0.1f);
var activity = new BodyActivityDescription(0.01f);
shapeA.ComputeInertia(1, out var inertiaA);
shapeA.ComputeInertia(1, out var inertiaB);
var nextX = -10f;
{
var x = GetNextPosition(ref nextX);
var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity));
var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity));
Simulation.Solver.Add(a, b, new BallSocket {
LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1)
});
}
{
var x = GetNextPosition(ref nextX);
var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity));
var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity));
Simulation.Solver.Add(a, b, new BallSocket {
LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1)
});
Simulation.Solver.Add(a, b, new AngularHinge {
LocalHingeAxisA = new Vector3(0, 1, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1)
});
}
{
var x = GetNextPosition(ref nextX);
var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity));
var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity));
Simulation.Solver.Add(a, b, new Hinge
{
LocalOffsetA = new Vector3(0, 1, 0),
LocalHingeAxisA = new Vector3(0, 1, 0),
LocalOffsetB = new Vector3(0, -1, 0),
LocalHingeAxisB = new Vector3(0, 1, 0),
SpringSettings = new SpringSettings(30, 1)
});
}
{
var x = GetNextPosition(ref nextX);
var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity));
var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity));
Simulation.Solver.Add(a, b, new BallSocket {
LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1)
});
Simulation.Solver.Add(a, b, new AngularSwivelHinge {
LocalSwivelAxisA = new Vector3(1, 0, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1)
});
Simulation.Solver.Add(a, b, new SwingLimit {
AxisLocalA = new Vector3(0, 1, 0), AxisLocalB = new Vector3(0, 1, 0), MaximumSwingAngle = MathHelper.PiOver2, SpringSettings = new SpringSettings(30, 1)
});
}
{
var x = GetNextPosition(ref nextX);
var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity));
var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity));
Simulation.Solver.Add(a, b, new SwivelHinge
{
LocalOffsetA = new Vector3(0, 1, 0),
LocalSwivelAxisA = new Vector3(1, 0, 0),
LocalOffsetB = new Vector3(0, -1, 0),
LocalHingeAxisB = new Vector3(0, 1, 0),
SpringSettings = new SpringSettings(30, 1)
});
Simulation.Solver.Add(a, b, new SwingLimit {
AxisLocalA = new Vector3(0, 1, 0), AxisLocalB = new Vector3(0, 1, 0), MaximumSwingAngle = MathHelper.PiOver2, SpringSettings = new SpringSettings(30, 1)
});
}
{
var x = GetNextPosition(ref nextX);
var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity));
var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity));
Simulation.Solver.Add(a, b, new BallSocket {
LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1)
});
Simulation.Solver.Add(a, b, new TwistServo
{
LocalBasisA = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)),
LocalBasisB = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)),
TargetAngle = MathHelper.PiOver4,
SpringSettings = new SpringSettings(30, 1),
ServoSettings = new ServoSettings(float.MaxValue, 0, float.MaxValue)
});
}
{
var x = GetNextPosition(ref nextX);
var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity));
var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity));
Simulation.Solver.Add(a, b, new BallSocket {
LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1)
});
Simulation.Solver.Add(a, b, new TwistLimit
{
LocalBasisA = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)),
LocalBasisB = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)),
MinimumAngle = MathHelper.Pi * -0.5f,
MaximumAngle = MathHelper.Pi * 0.95f,
SpringSettings = new SpringSettings(30, 1),
});
Simulation.Solver.Add(a, b, new AngularHinge {
LocalHingeAxisA = new Vector3(0, 1, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1)
});
}
{
var x = GetNextPosition(ref nextX);
var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity));
var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity));
Simulation.Solver.Add(a, b, new BallSocket {
LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1)
});
Simulation.Solver.Add(a, b, new TwistMotor
{
LocalAxisA = new Vector3(0, 1, 0),
LocalAxisB = new Vector3(0, 1, 0),
TargetVelocity = MathHelper.Pi * 2,
Settings = new MotorSettings(float.MaxValue, 0.1f)
});
Simulation.Solver.Add(a, b, new AngularHinge {
LocalHingeAxisA = new Vector3(0, 1, 0), LocalHingeAxisB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1)
});
}
{
var x = GetNextPosition(ref nextX);
var a = Simulation.Bodies.Add(BodyDescription.CreateKinematic(new Vector3(x, 3, 0), collidableA, activity));
var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity));
Simulation.Solver.Add(a, b, new BallSocket {
LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1)
});
Simulation.Solver.Add(a, b, new AngularServo
{
TargetRelativeRotationLocalA = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), MathHelper.PiOver2),
ServoSettings = new ServoSettings(float.MaxValue, 0, 12f),
SpringSettings = new SpringSettings(30, 1)
});
}
{
var x = GetNextPosition(ref nextX);
var a = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity));
var b = Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity));
Simulation.Solver.Add(a, b, new BallSocket {
LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1)
});
Simulation.Solver.Add(a, b, new AngularMotor {
TargetVelocityLocalA = new Vector3(0, 1, 0), Settings = new MotorSettings(15, 0.0001f)
});
}
{
var x = GetNextPosition(ref nextX);
var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity);
var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 5, 0), inertiaB, collidableB, activity);
//aDescription.Velocity.Angular = new Vector3(0, 0, 5);
var a = Simulation.Bodies.Add(aDescription);
var b = Simulation.Bodies.Add(bDescription);
Simulation.Solver.Add(a, b, new Weld {
LocalOffset = new Vector3(0, 2, 0), LocalOrientation = Quaternion.Identity, SpringSettings = new SpringSettings(30, 1)
});
}
{
var x = GetNextPosition(ref nextX);
var sphere = new Sphere(0.125f);
//Treat each vertex as a point mass that cannot rotate.
var sphereInertia = new BodyInertia {
InverseMass = 1
};
var sphereCollidable = new CollidableDescription(Simulation.Shapes.Add(sphere), 0.1f);
var a = new Vector3(x, 3, 0);
var b = new Vector3(x, 4, 0);
var c = new Vector3(x, 3, 1);
var d = new Vector3(x + 1, 3, 0);
var aDescription = BodyDescription.CreateDynamic(a, sphereInertia, sphereCollidable, activity);
var bDescription = BodyDescription.CreateDynamic(b, sphereInertia, sphereCollidable, activity);
var cDescription = BodyDescription.CreateDynamic(c, sphereInertia, sphereCollidable, activity);
var dDescription = BodyDescription.CreateDynamic(d, sphereInertia, sphereCollidable, activity);
var aHandle = Simulation.Bodies.Add(aDescription);
var bHandle = Simulation.Bodies.Add(bDescription);
var cHandle = Simulation.Bodies.Add(cDescription);
var dHandle = Simulation.Bodies.Add(dDescription);
var distanceSpringiness = new SpringSettings(3f, 1);
Simulation.Solver.Add(aHandle, bHandle, new CenterDistanceConstraint(Vector3.Distance(a, b), distanceSpringiness));
Simulation.Solver.Add(aHandle, cHandle, new CenterDistanceConstraint(Vector3.Distance(a, c), distanceSpringiness));
Simulation.Solver.Add(aHandle, dHandle, new CenterDistanceConstraint(Vector3.Distance(a, d), distanceSpringiness));
Simulation.Solver.Add(bHandle, cHandle, new CenterDistanceConstraint(Vector3.Distance(b, c), distanceSpringiness));
Simulation.Solver.Add(bHandle, dHandle, new CenterDistanceConstraint(Vector3.Distance(b, d), distanceSpringiness));
Simulation.Solver.Add(cHandle, dHandle, new CenterDistanceConstraint(Vector3.Distance(c, d), distanceSpringiness));
Simulation.Solver.Add(aHandle, bHandle, cHandle, dHandle, new VolumeConstraint(a, b, c, d, new SpringSettings(30, 1)));
}
{
var x = GetNextPosition(ref nextX);
var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity);
var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity);
var a = Simulation.Bodies.Add(aDescription);
var b = Simulation.Bodies.Add(bDescription);
Simulation.Solver.Add(a, b, new DistanceServo(new Vector3(0, 0.55f, 0), new Vector3(0, -0.55f, 0), 1.9f, new SpringSettings(30, 1), ServoSettings.Default));
}
{
var x = GetNextPosition(ref nextX);
var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), inertiaA, collidableA, activity);
var bDescription = BodyDescription.CreateDynamic(new Vector3(x, 6, 0), inertiaB, collidableB, activity);
var a = Simulation.Bodies.Add(aDescription);
var b = Simulation.Bodies.Add(bDescription);
Simulation.Solver.Add(a, b, new DistanceLimit(new Vector3(0, 0.55f, 0), new Vector3(0, -0.55f, 0), 1f, 3, new SpringSettings(30, 1)));
}
{
var x = GetNextPosition(ref nextX);
var sphere = new Sphere(0.125f);
//Treat each vertex as a point mass that cannot rotate.
var sphereInertia = new BodyInertia {
InverseMass = 1
};
var sphereCollidable = new CollidableDescription(Simulation.Shapes.Add(sphere), 0.1f);
var a = new Vector3(x, 3, 0);
var b = new Vector3(x, 4, 0);
var c = new Vector3(x + 1, 3, 0);
var aDescription = BodyDescription.CreateDynamic(a, sphereInertia, sphereCollidable, activity);
var bDescription = BodyDescription.CreateDynamic(b, sphereInertia, sphereCollidable, activity);
var cDescription = BodyDescription.CreateDynamic(c, sphereInertia, sphereCollidable, activity);
var aHandle = Simulation.Bodies.Add(aDescription);
var bHandle = Simulation.Bodies.Add(bDescription);
var cHandle = Simulation.Bodies.Add(cDescription);
var distanceSpringiness = new SpringSettings(3f, 1);
Simulation.Solver.Add(aHandle, bHandle, new CenterDistanceConstraint(Vector3.Distance(a, b), distanceSpringiness));
Simulation.Solver.Add(aHandle, cHandle, new CenterDistanceConstraint(Vector3.Distance(a, c), distanceSpringiness));
Simulation.Solver.Add(bHandle, cHandle, new CenterDistanceConstraint(Vector3.Distance(b, c), distanceSpringiness));
Simulation.Solver.Add(aHandle, bHandle, cHandle, new AreaConstraint(a, b, c, new SpringSettings(30, 1)));
}
{
var x = GetNextPosition(ref nextX);
var aDescription = BodyDescription.CreateDynamic(new Vector3(x, 3, 0), default, collidableA, activity);
public unsafe override void Initialize(ContentArchive content, Camera camera)
{
camera.Position = new Vector3(0, 4, 15);
camera.Yaw = 0;
Simulation = Simulation.Create(BufferPool, new TestCallbacks());
Simulation.PoseIntegrator.Gravity = new Vector3(0, -10, 0);
var shapeA = new Box(.75f, 1, .5f);
var shapeB = new Box(.75f, 1, .5f);
shapeA.ComputeInertia(1, out var inertiaA);
shapeA.ComputeInertia(1, out var inertiaB);
{
var a = Simulation.Bodies.Add(new BodyDescription(new Vector3(-10, 3, 0), inertiaA, Simulation.Shapes.Add(shapeA), 0.1f, new BodyActivityDescription(0.01f)));
var b = Simulation.Bodies.Add(new BodyDescription(new Vector3(-10, 5, 0), inertiaB, Simulation.Shapes.Add(shapeB), 0.1f, new BodyActivityDescription(0.01f)));
Simulation.Solver.Add(a, b, new BallSocket {
LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1)
});
}
{
var a = Simulation.Bodies.Add(new BodyDescription(new Vector3(-7, 3, 0), new BodyInertia(), Simulation.Shapes.Add(shapeA), 0.1f, new BodyActivityDescription(0.01f)));
var b = Simulation.Bodies.Add(new BodyDescription(new Vector3(-7, 5, 0), inertiaB, Simulation.Shapes.Add(shapeB), 0.1f, new BodyActivityDescription(0.01f)));
Simulation.Solver.Add(a, b, new BallSocket {
LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1)
});
Simulation.Solver.Add(a, b, new AngularHinge {
HingeAxisLocalA = new Vector3(0, 1, 0), HingeAxisLocalB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1)
});
}
{
var a = Simulation.Bodies.Add(new BodyDescription(new Vector3(-4, 3, 0), new BodyInertia(), Simulation.Shapes.Add(shapeA), 0.1f, new BodyActivityDescription(0.01f)));
var b = Simulation.Bodies.Add(new BodyDescription(new Vector3(-4, 5, 0), inertiaB, Simulation.Shapes.Add(shapeB), 0.1f, new BodyActivityDescription(0.01f)));
Simulation.Solver.Add(a, b, new BallSocket {
LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1)
});
Simulation.Solver.Add(a, b, new AngularSwivelHinge {
SwivelAxisLocalA = new Vector3(0, 1, 0), HingeAxisLocalB = new Vector3(1, 0, 0), SpringSettings = new SpringSettings(30, 1)
});
Simulation.Solver.Add(a, b, new SwingLimit {
AxisLocalA = new Vector3(0, 1, 0), AxisLocalB = new Vector3(0, 1, 0), MaximumSwingAngle = MathHelper.PiOver2, SpringSettings = new SpringSettings(30, 1)
});
}
{
var a = Simulation.Bodies.Add(new BodyDescription(new Vector3(-1, 3, 0), new BodyInertia(), Simulation.Shapes.Add(shapeA), 0.1f, new BodyActivityDescription(0.01f)));
var b = Simulation.Bodies.Add(new BodyDescription(new Vector3(-1, 5, 0), inertiaB, Simulation.Shapes.Add(shapeB), 0.1f, new BodyActivityDescription(0.01f)));
Simulation.Solver.Add(a, b, new BallSocket {
LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1)
});
Simulation.Solver.Add(a, b, new TwistServo
{
LocalBasisA = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)),
LocalBasisB = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)),
TargetAngle = MathHelper.PiOver4,
SpringSettings = new SpringSettings(30, 1),
ServoSettings = new ServoSettings(float.MaxValue, 0, float.MaxValue)
});
}
{
var a = Simulation.Bodies.Add(new BodyDescription(new Vector3(2, 3, 0), new BodyInertia(), Simulation.Shapes.Add(shapeA), 0.1f, new BodyActivityDescription(0.01f)));
var b = Simulation.Bodies.Add(new BodyDescription(new Vector3(2, 5, 0), inertiaB, Simulation.Shapes.Add(shapeB), 0.1f, new BodyActivityDescription(0.01f)));
Simulation.Solver.Add(a, b, new BallSocket {
LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1)
});
Simulation.Solver.Add(a, b, new TwistLimit
{
LocalBasisA = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)),
LocalBasisB = RagdollDemo.CreateBasis(new Vector3(0, 1, 0), new Vector3(1, 0, 0)),
MinimumAngle = MathHelper.Pi * -0.5f,
MaximumAngle = MathHelper.Pi * 0.95f,
SpringSettings = new SpringSettings(30, 1),
});
Simulation.Solver.Add(a, b, new AngularHinge {
HingeAxisLocalA = new Vector3(0, 1, 0), HingeAxisLocalB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1)
});
}
{
var a = Simulation.Bodies.Add(new BodyDescription(new Vector3(5, 3, 0), new BodyInertia(), Simulation.Shapes.Add(shapeA), 0.1f, new BodyActivityDescription(0.01f)));
var b = Simulation.Bodies.Add(new BodyDescription(new Vector3(5, 5, 0), inertiaB, Simulation.Shapes.Add(shapeB), 0.1f, new BodyActivityDescription(0.01f)));
Simulation.Solver.Add(a, b, new BallSocket {
LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1)
});
Simulation.Solver.Add(a, b, new TwistMotor
{
LocalAxisA = new Vector3(0, 1, 0),
LocalAxisB = new Vector3(0, 1, 0),
TargetVelocity = MathHelper.Pi * 2,
Settings = new MotorSettings(float.MaxValue, 0.1f)
});
Simulation.Solver.Add(a, b, new AngularHinge {
HingeAxisLocalA = new Vector3(0, 1, 0), HingeAxisLocalB = new Vector3(0, 1, 0), SpringSettings = new SpringSettings(30, 1)
});
}
{
var a = Simulation.Bodies.Add(new BodyDescription(new Vector3(8, 3, 0), new BodyInertia(), Simulation.Shapes.Add(shapeA), 0.1f, new BodyActivityDescription(0.01f)));
var b = Simulation.Bodies.Add(new BodyDescription(new Vector3(8, 5, 0), inertiaB, Simulation.Shapes.Add(shapeB), 0.1f, new BodyActivityDescription(0.01f)));
Simulation.Solver.Add(a, b, new BallSocket {
LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1)
});
Simulation.Solver.Add(a, b, new AngularServo
{
TargetRelativeRotationLocalA = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), MathHelper.PiOver2),
ServoSettings = new ServoSettings(float.MaxValue, 0, 12f),
SpringSettings = new SpringSettings(30, 1)
});
}
{
var a = Simulation.Bodies.Add(new BodyDescription(new Vector3(11, 3, 0), inertiaA, Simulation.Shapes.Add(shapeA), 0.1f, new BodyActivityDescription(0.01f)));
var b = Simulation.Bodies.Add(new BodyDescription(new Vector3(11, 5, 0), inertiaB, Simulation.Shapes.Add(shapeB), 0.1f, new BodyActivityDescription(0.01f)));
Simulation.Solver.Add(a, b, new BallSocket {
LocalOffsetA = new Vector3(0, 1, 0), LocalOffsetB = new Vector3(0, -1, 0), SpringSettings = new SpringSettings(30, 1)
});
Simulation.Solver.Add(a, b, new AngularMotor {
TargetVelocityLocalA = new Vector3(0, 1, 0), Settings = new MotorSettings(15, 0.0001f)
});
}
Simulation.Statics.Add(new StaticDescription(new Vector3(), new CollidableDescription(Simulation.Shapes.Add(new Box(256, 1, 256)), 0.1f)));
}
