public DistanceConstraint(LinearDynamic a, LinearDynamic b)
{
A = a;
B = b;
distance = (a.Position - b.Position).Length();
}
public DistanceConstraint(LinearDynamic a, LinearDynamic b)
{
A = a;
B = b;
++a.ConstraintCount;
++b.ConstraintCount;
distance = (a.Position - b.Position).Length();
}
private void ApplyImpulse(LinearDynamic dynamic, float impulseToApply)
{
Vector3 worldSpaceImpulse;
if (A == dynamic)
{
Vector3.Multiply(ref jacobian, impulseToApply, out worldSpaceImpulse);
}
else
{
Vector3.Multiply(ref jacobian, -impulseToApply, out worldSpaceImpulse);
}
dynamic.ApplyImpulse(ref worldSpaceImpulse);
}
public override void ApplyImpulse(LinearDynamic dynamic)
{
ApplyImpulse(dynamic, impulse);
}
public override void ApplyAccumulatedImpulse(LinearDynamic dynamic)
{
ApplyImpulse(dynamic, accumulatedImpulse);
}
private void ApplyImpulse(LinearDynamic dynamic, float impulseToApply)
{
Vector3 worldSpaceImpulse;
if (A == dynamic)
{
Vector3.Multiply(ref jacobian, impulseToApply, out worldSpaceImpulse);
}
else
{
Vector3.Multiply(ref jacobian, -impulseToApply, out worldSpaceImpulse);
}
dynamic.ApplyImpulse(ref worldSpaceImpulse);
}
public override void ApplyAccumulatedImpulse(LinearDynamic dynamic)
{
ApplyAccumulatedImpulses();
}
public override void ApplyImpulse(LinearDynamic dynamic)
{
ApplyImpulses();
}
public PlaneCollisionConstraint(LinearDynamic dynamic, Plane plane)
{
Dynamic = dynamic;
++dynamic.ConstraintCount;
this.plane = plane;
}
public void Add(LinearDynamic dynamic)
{
dynamics.Add(dynamic);
}
public abstract void ApplyImpulse(LinearDynamic dynamic);
private void BuildSimulation(Vector3 offset, Simulator simulator)
{
//Create a lattice of dynamic objects.
int width = 30;
int height = 10;
int length = 30;
float spacing = 3;
var dynamics = new LinearDynamic[width, height, length];
for (int widthIndex = 0; widthIndex < width; ++widthIndex)
{
for (int heightIndex = 0; heightIndex < height; ++heightIndex)
{
for (int lengthIndex = 0; lengthIndex < length; ++lengthIndex)
{
var dynamic = new LinearDynamic(10)
{
Position = offset + new Vector3(spacing * widthIndex, spacing * heightIndex + 10, spacing * lengthIndex)
};
dynamics[widthIndex, heightIndex, lengthIndex] = dynamic;
simulator.Add(dynamic);
}
}
}
Vector3 normal = new Vector3(0, 1, 0);
Plane plane = new Plane(normal, -Vector3.Dot(offset, normal));
//Create a bunch of connections between the dynamic objects.
for (int widthIndex = 0; widthIndex < width; ++widthIndex)
{
for (int heightIndex = 0; heightIndex < height; ++heightIndex)
{
for (int lengthIndex = 0; lengthIndex < length; ++lengthIndex)
{
//Create a connection with the dynamics at +x, +y, +z, +x+y+z, -x+y+z, +x-y+z, +x+y-z
//+x
if (widthIndex + 1 < width)
{
simulator.Add(new DistanceConstraint(dynamics[widthIndex, heightIndex, lengthIndex], dynamics[widthIndex + 1, heightIndex, lengthIndex]));
}
//+y
if (heightIndex + 1 < height)
{
simulator.Add(new DistanceConstraint(dynamics[widthIndex, heightIndex, lengthIndex], dynamics[widthIndex, heightIndex + 1, lengthIndex]));
}
//+z
if (lengthIndex + 1 < length)
{
simulator.Add(new DistanceConstraint(dynamics[widthIndex, heightIndex, lengthIndex], dynamics[widthIndex, heightIndex, lengthIndex + 1]));
}
//+x+y+z
if (widthIndex + 1 < width && heightIndex + 1 < height && lengthIndex + 1 < length)
{
simulator.Add(new DistanceConstraint(dynamics[widthIndex, heightIndex, lengthIndex], dynamics[widthIndex + 1, heightIndex + 1, lengthIndex + 1]));
}
//-x+y+z
if (widthIndex - 1 >= 0 && heightIndex + 1 < height && lengthIndex + 1 < length)
{
simulator.Add(new DistanceConstraint(dynamics[widthIndex, heightIndex, lengthIndex], dynamics[widthIndex - 1, heightIndex + 1, lengthIndex + 1]));
}
//+x-y+z
if (widthIndex + 1 < width && heightIndex - 1 >= 0 && lengthIndex + 1 < length)
{
simulator.Add(new DistanceConstraint(dynamics[widthIndex, heightIndex, lengthIndex], dynamics[widthIndex + 1, heightIndex - 1, lengthIndex + 1]));
}
//+x+y-z
if (widthIndex + 1 < width && heightIndex + 1 < height && lengthIndex - 1 >= 0)
{
simulator.Add(new DistanceConstraint(dynamics[widthIndex, heightIndex, lengthIndex], dynamics[widthIndex + 1, heightIndex + 1, lengthIndex - 1]));
}
//Create a plane constraint to stop the object from falling.
simulator.Add(new PlaneCollisionConstraint(dynamics[widthIndex, heightIndex, lengthIndex], plane));
}
}
}
}
public DynamicVisualizer(LinearDynamic dynamic, ModelDrawer modelDrawer)
{
Dynamic = dynamic;
DisplayCollidable = new DisplayEntityCollidable(modelDrawer, new ConvexCollidable <BoxShape>(new BoxShape(0.5f, 0.5f, 0.5f)));
modelDrawer.Add(DisplayCollidable);
}
public PlaneCollisionConstraint(LinearDynamic dynamic, Plane plane)
{
Dynamic = dynamic;
++dynamic.ConstraintCount;
this.plane = plane;
}
public void Add(LinearDynamic dynamic)
{
dynamics.Add(dynamic);
}
public abstract void ApplyAccumulatedImpulse(LinearDynamic dynamic);
public abstract void ApplyImpulse(LinearDynamic dynamic);
public abstract void ApplyAccumulatedImpulse(LinearDynamic dynamic);
