/// <summary>
/// Prepares the constraint for iterative processing in the current frame.
/// </summary>
public override void PreProcess()
{
RigidBody a = BodyA, b = BodyB;
// get offsets and mass in world coordinates
Vector3.Transform(ref _bodyPointA, ref a.World.Combined, out _worldOffsetA);
Vector3.Subtract(ref _worldOffsetA, ref a.World.Position, out _worldOffsetA);
Vector3.Transform(ref _bodyPointB, ref b.World.Combined, out _worldOffsetB);
Vector3.Subtract(ref _worldOffsetB, ref b.World.Position, out _worldOffsetB);
MassProperties.EffectiveMassMatrix(ref a.MassWorld, ref b.MassWorld, ref _worldOffsetA, ref _worldOffsetB, out _mass);
if (this.Manager.IsSolverWarmStarted)
{
Vector3 impulse;
Vector3.Multiply(ref _impulse, this.Manager.TimeStep, out impulse);
b.ApplyImpulse(ref impulse, ref _worldOffsetB);
Vector3.Negate(ref impulse, out impulse);
a.ApplyImpulse(ref impulse, ref _worldOffsetA);
}
else
{
_impulse = Vector3.Zero;
}
}
/// <summary>
/// Solve the constraint for position.
/// </summary>
/// <returns>Returns a value indicating whether the constraint has been satisfied.</returns>
public override bool ProcessPosition()
{
RigidBody a = BodyA, b = BodyB;
// get offsets and distance in world coordinates
Vector3 impulse;
Vector3.Transform(ref _bodyPointA, ref a.World.Combined, out _worldOffsetA);
Vector3.Transform(ref _bodyPointB, ref b.World.Combined, out _worldOffsetB);
Vector3.Subtract(ref _worldOffsetA, ref _worldOffsetB, out impulse);
Vector3.Subtract(ref _worldOffsetA, ref a.World.Position, out _worldOffsetA);
Vector3.Subtract(ref _worldOffsetB, ref b.World.Position, out _worldOffsetB);
float error = impulse.Length();
if (error <= this.Manager.LinearErrorTolerance)
{
return(true);
}
// need normalized direction to calculate effective mass
Vector3 n;
Vector3.Divide(ref impulse, error, out n);
float mass = MassProperties.EffectiveMass(ref a.MassWorld, ref b.MassWorld, ref _worldOffsetA, ref _worldOffsetB, ref n);
Vector3.Multiply(ref impulse, mass * this.Manager.PositionCorrectionFactor, out impulse);
// apply impulse
b.ApplyFlashImpulse(ref impulse, ref _worldOffsetB);
Vector3.Negate(ref impulse, out impulse);
a.ApplyFlashImpulse(ref impulse, ref _worldOffsetA);
return(false);
}
internal static float EffectiveMass(ref MassProperties a, ref MassProperties b, ref Vector3 offsetA, ref Vector3 offsetB, ref Vector3 normal)
{
Vector3 v;
float normalMass = 0f, f;
if (a.Mass < float.PositiveInfinity)
{
normalMass = a.MassInverse;
Vector3.Cross(ref offsetA, ref normal, out v);
Vector3.Transform(ref v, ref a.InertiaInverse, out v);
Vector3.Cross(ref v, ref offsetA, out v);
Vector3.Dot(ref normal, ref v, out f);
normalMass += f;
}
if (b.Mass < float.PositiveInfinity)
{
normalMass += b.MassInverse;
Vector3.Cross(ref offsetB, ref normal, out v);
Vector3.Transform(ref v, ref b.InertiaInverse, out v);
Vector3.Cross(ref v, ref offsetB, out v);
Vector3.Dot(ref normal, ref v, out f);
normalMass += f;
}
if (normalMass < Constants.Epsilon)
{
normalMass = Constants.Epsilon;
}
return(1f / normalMass);
}
/// <summary>
/// Prepares the constraint for iterative processing in the current frame.
/// </summary>
public override void PreProcess()
{
RigidBody a = BodyA, b = BodyB;
// get world points and normal
Vector3.Transform(ref _bodyPointA, ref a.World.Combined, out _worldOffsetA);
Vector3.Transform(ref _bodyPointB, ref b.World.Combined, out _worldOffsetB);
Vector3.Subtract(ref _worldOffsetA, ref _worldOffsetB, out _normal);
Vector3.Subtract(ref _worldOffsetA, ref a.World.Position, out _worldOffsetA);
Vector3.Subtract(ref _worldOffsetB, ref b.World.Position, out _worldOffsetB);
_distance = _normal.Length();
if (_distance < Constants.Epsilon)
{
_normal = Vector3.Zero;
}
else
{
Vector3.Divide(ref _normal, _distance, out _normal);
}
_mass = MassProperties.EffectiveMass(ref a.MassWorld, ref b.MassWorld, ref _worldOffsetA, ref _worldOffsetB, ref _normal);
// determine the constraint behavior for this frame
var prevState = _state;
if (Math.Abs(_maxDistance - _minDistance) < 2f * this.Manager.LinearErrorTolerance)
{
_state = LimitState.Equal;
}
else if (_distance <= _minDistance)
{
_state = LimitState.Min;
}
else if (_distance >= _maxDistance)
{
_state = LimitState.Max;
}
else
{
_state = LimitState.Between;
}
if (this.Manager.IsSolverWarmStarted && prevState == _state)
{
Vector3 impulse;
Vector3.Multiply(ref _impulse, this.Manager.TimeStep, out impulse);
b.ApplyImpulse(ref impulse, ref _worldOffsetB);
Vector3.Negate(ref impulse, out impulse);
a.ApplyImpulse(ref impulse, ref _worldOffsetA);
}
else
{
_impulse = Vector3.Zero;
}
_pImpulse = Vector3.Zero;
}
/// <summary>
/// Construct a new rigid body.
/// </summary>
public RigidBody()
{
Mass = MassProperties.Immovable;
Transform = World = Transform.Identity;
_isMovable = false;
_contacts = new List <Constraint>();
_constraints = new List <Constraint>();
_skin = new BodySkin();
_skin.Owner = this;
}
public RigidBodyModel(MassProperties mass, CompiledPart[] parts,
Material[] materials)
{
_mass = mass;
_parts = parts;
_materials = materials;
if (_parts.Length != _materials.Length)
{
throw new ArgumentException("The count of supplied mesh parts and materials do not match.");
}
}
internal static void Transform(ref MassProperties mass, ref Transform transform, out MassProperties output)
{
output.Mass = mass.Mass;
output.MassInverse = mass.MassInverse;
Matrix orientation, orientationInv;
Matrix.CreateFromQuaternion(ref transform.Orientation, out orientation);
Matrix.Transpose(ref orientation, out orientationInv);
Matrix.Multiply(ref orientationInv, ref mass.Inertia, out output.Inertia);
Matrix.Multiply(ref output.Inertia, ref orientation, out output.Inertia);
Matrix.Multiply(ref orientationInv, ref mass.InertiaInverse, out output.InertiaInverse);
Matrix.Multiply(ref output.InertiaInverse, ref orientation, out output.InertiaInverse);
}
internal static void EffectiveMassMatrix(ref MassProperties a, ref MassProperties b, ref Vector3 offsetA, ref Vector3 offsetB, out Matrix k)
{
Matrix ka = new Matrix(), kb = new Matrix();
if (a.Mass < float.PositiveInfinity)
{
a.InverseMassMatrix(ref offsetA, out ka);
}
if (b.Mass < float.PositiveInfinity)
{
b.InverseMassMatrix(ref offsetB, out kb);
}
Matrix.Add(ref ka, ref kb, out k);
k.M44 = 1f;
Matrix.Invert(ref k, out k);
}
private void UpdateWorld()
{
World.Invert(out WorldInverse);
MassProperties.Transform(ref Mass, ref World, out MassWorld);
Transform = World;
}
private void ComputeVitals()
{
RigidBody a = this.BodyA, b = this.BodyB;
// compute relative basis between the two objects in world position
Frame.Transform(ref _bodyBasisA, ref this.BodyA.World, out _worldBasisA);
Frame.Transform(ref _bodyBasisB, ref this.BodyB.World, out _worldBasisB);
Frame rel;
Frame.Subtract(ref _worldBasisA, ref _worldBasisB, out rel);
// compute current positions and angles
Vector3.Subtract(ref _worldBasisA.Origin, ref _worldBasisB.Origin, out _positions);
Matrix m;
_worldBasisB.ToMatrix(out m);
Matrix.Transpose(ref m, out m);
Vector3.Transform(ref _positions, ref m, out _positions);
rel.ComputeEulerAnglesXYZ(out _angles);
// borrowed from Bullet; construct the axes about which we actually restrict rotation
Vector3.Cross(ref _worldBasisB.Z, ref _worldBasisA.X, out _axisY);
Vector3.Cross(ref _axisY, ref _worldBasisB.Z, out _axisX);
Vector3.Cross(ref _worldBasisA.X, ref _axisY, out _axisZ);
_axisX.Normalize();
_axisY.Normalize();
_axisZ.Normalize();
// calculate effective inertia along each axis
Matrix inertia = new Matrix();
if (a.IsMovable)
{
Matrix.Add(ref inertia, ref a.MassWorld.InertiaInverse, out inertia);
}
if (b.IsMovable)
{
Matrix.Add(ref inertia, ref b.MassWorld.InertiaInverse, out inertia);
}
inertia.M44 = 1f;
Matrix.Invert(ref inertia, out inertia);
Vector3 d;
Vector3.Transform(ref _axisX, ref inertia, out d);
Vector3.Dot(ref d, ref _axisX, out _inertia.X);
Vector3.Transform(ref _axisY, ref inertia, out d);
Vector3.Dot(ref d, ref _axisY, out _inertia.Y);
Vector3.Transform(ref _axisZ, ref inertia, out d);
Vector3.Dot(ref d, ref _axisZ, out _inertia.Z);
// calculate effective mass along each axis of basis B
//float w = b.MassWorld.MassInverse / (a.MassWorld.MassInverse = b.MassWorld.MassInverse);
//Vector3.Multiply(ref _worldBasisA.Origin, 1f - w, out _anchorA);
//Vector3.Multiply(ref _worldBasisB.Origin, w, out _anchorB);
//Vector3.Add(ref _anchorA, ref _anchorB, out _anchorB);
//Vector3.Subtract(ref _anchorB, ref a.World.Position, out _anchorA);
//Vector3.Subtract(ref _anchorB, ref b.World.Position, out _anchorB);
Vector3.Subtract(ref _worldBasisA.Origin, ref a.World.Position, out _anchorA);
Vector3.Subtract(ref _worldBasisB.Origin, ref b.World.Position, out _anchorB);
_mass.X = MassProperties.EffectiveMass(ref a.MassWorld, ref b.MassWorld, ref _anchorA, ref _anchorB, ref _worldBasisB.X);
_mass.Y = MassProperties.EffectiveMass(ref a.MassWorld, ref b.MassWorld, ref _anchorA, ref _anchorB, ref _worldBasisB.Y);
_mass.Z = MassProperties.EffectiveMass(ref a.MassWorld, ref b.MassWorld, ref _anchorA, ref _anchorB, ref _worldBasisB.Z);
}
/// <summary>
/// Construct a new rigid body.
/// </summary>
public RigidBody()
{
Mass = MassProperties.Immovable;
Transform = World = Transform.Identity;
_isMovable = false;
_contacts = new List<Constraint>();
_constraints = new List<Constraint>();
_skin = new BodySkin();
_skin.Owner = this;
}
public override ModelContent Process(NodeContent input, ContentProcessorContext context)
{
var attributes = input.Children.ToDictionary(n => n.Name, n => n.OpaqueData);
var nodesToRemove = (from node in input.Children
where node.OpaqueData.GetAttribute(TYPE_ATTR_NAME, MeshType.Both) == MeshType.Physical
select node).ToArray();
ModelContent model = base.Process(input, context);
var parts = new List<CompiledPart>();
var materials = new List<Material>();
var mass = new MassProperties();
var centerOfMass = Vector3.Zero;
foreach (var mesh in model.Meshes)
{
MeshType type = MeshType.Both;
PhysicalShape shape = PhysicalShape.Mesh;
float elasticity = _defaultElasticity, roughness = _defaultRoughness, density = _defaultDensity;
if (attributes.ContainsKey(mesh.Name))
{
type = attributes[mesh.Name].GetAttribute(TYPE_ATTR_NAME, MeshType.Both);
if (type == MeshType.Visual) continue;
elasticity = attributes[mesh.Name].GetAttribute(ELASTICITY_ATTR_NAME, _defaultElasticity);
roughness = attributes[mesh.Name].GetAttribute(ROUGHNESS_ATTR_NAME, _defaultRoughness);
density = attributes[mesh.Name].GetAttribute(DENSITY_ATTR_NAME, _defaultDensity);
shape = attributes[mesh.Name].GetAttribute(SHAPE_ATTR_NAME, _defaultShape);
}
var meshCenterOfMass = Vector3.Zero;
var meshMass = MassProperties.Immovable;
CompiledPart meshPart = null;
if (mesh.MeshParts.Count < 1)
{
continue;
}
int[] indices = mesh.MeshParts[0].IndexBuffer.Skip(mesh.MeshParts[0].StartIndex).Take(mesh.MeshParts[0].PrimitiveCount * 3).ToArray();
Vector3[] vertices = MeshToVertexArray(context.TargetPlatform, mesh);
if (_windingOrder == WindingOrder.Clockwise)
{
ReverseWindingOrder(indices);
}
switch (shape)
{
case PhysicalShape.Mesh:
{
meshPart = new CompiledMesh(vertices, indices);
meshMass = MassProperties.Immovable;
meshCenterOfMass = GetMeshTranslation(mesh);
}
break;
case PhysicalShape.Polyhedron:
{
var hull = new ConvexHull3D(vertices);
meshPart = hull.ToPolyhedron();
meshMass = MassProperties.FromTriMesh(density, vertices, indices, out meshCenterOfMass);
}
break;
case PhysicalShape.Sphere:
{
Sphere s;
Sphere.Fit(vertices, out s);
meshPart = new CompiledSphere(s.Center, s.Radius);
meshMass = MassProperties.FromSphere(density, s.Center, s.Radius);
meshCenterOfMass = s.Center;
}
break;
case PhysicalShape.Capsule:
{
Capsule c;
Capsule.Fit(vertices, out c);
meshPart = new CompiledCapsule(c.P1, c.P2, c.Radius);
meshMass = MassProperties.FromCapsule(density, c.P1, c.P2, c.Radius, out meshCenterOfMass);
}
break;
}
parts.Add(meshPart);
materials.Add(new Material(elasticity, roughness));
Vector3.Multiply(ref meshCenterOfMass, meshMass.Mass, out meshCenterOfMass);
Vector3.Add(ref centerOfMass, ref meshCenterOfMass, out centerOfMass);
mass.Mass += meshMass.Mass;
meshMass.Inertia.M44 = 0f;
Matrix.Add(ref mass.Inertia, ref meshMass.Inertia, out mass.Inertia);
}
// compute mass properties
Vector3.Divide(ref centerOfMass, mass.Mass, out centerOfMass);
mass.Inertia.M44 = 1f;
MassProperties.TranslateInertiaTensor(ref mass.Inertia, -mass.Mass, centerOfMass, out mass.Inertia);
if (centerOfMass.Length() >= Constants.Epsilon)
{
var transform = Matrix.CreateTranslation(-centerOfMass.X, -centerOfMass.Y, -centerOfMass.Z);
foreach (var p in parts)
{
p.Transform(ref transform);
}
transform = model.Root.Transform;
transform.M41 -= centerOfMass.X;
transform.M42 -= centerOfMass.Y;
transform.M43 -= centerOfMass.Z;
model.Root.Transform = transform;
}
mass = new MassProperties(mass.Mass, mass.Inertia);
var rbm = new RigidBodyModel(mass, parts.ToArray(), materials.ToArray());
// remove non-visual nodes
if (nodesToRemove.Length > 0)
{
foreach (var node in nodesToRemove)
input.Children.Remove(node);
model = base.Process(input, context);
}
model.Tag = rbm;
return model;
}
internal static void EffectiveMassMatrix(ref MassProperties a, ref MassProperties b, ref Vector3 offsetA, ref Vector3 offsetB, out Matrix k)
{
Matrix ka = new Matrix(), kb = new Matrix();
if(a.Mass < float.PositiveInfinity) a.InverseMassMatrix(ref offsetA, out ka);
if(b.Mass < float.PositiveInfinity) b.InverseMassMatrix(ref offsetB, out kb);
Matrix.Add(ref ka, ref kb, out k);
k.M44 = 1f;
Matrix.Invert(ref k, out k);
}
internal static float EffectiveMass(ref MassProperties a, ref MassProperties b, ref Vector3 offsetA, ref Vector3 offsetB, ref Vector3 normal)
{
Vector3 v;
float normalMass = 0f, f;
if (a.Mass < float.PositiveInfinity)
{
normalMass = a.MassInverse;
Vector3.Cross(ref offsetA, ref normal, out v);
Vector3.Transform(ref v, ref a.InertiaInverse, out v);
Vector3.Cross(ref v, ref offsetA, out v);
Vector3.Dot(ref normal, ref v, out f);
normalMass += f;
}
if (b.Mass < float.PositiveInfinity)
{
normalMass += b.MassInverse;
Vector3.Cross(ref offsetB, ref normal, out v);
Vector3.Transform(ref v, ref b.InertiaInverse, out v);
Vector3.Cross(ref v, ref offsetB, out v);
Vector3.Dot(ref normal, ref v, out f);
normalMass += f;
}
if (normalMass < Constants.Epsilon)
normalMass = Constants.Epsilon;
return 1f / normalMass;
}
internal static void Transform(ref MassProperties mass, ref Transform transform, out MassProperties output)
{
output.Mass = mass.Mass;
output.MassInverse = mass.MassInverse;
Matrix orientation, orientationInv;
Matrix.CreateFromQuaternion(ref transform.Orientation, out orientation);
Matrix.Transpose(ref orientation, out orientationInv);
Matrix.Multiply(ref orientationInv, ref mass.Inertia, out output.Inertia);
Matrix.Multiply(ref output.Inertia, ref orientation, out output.Inertia);
Matrix.Multiply(ref orientationInv, ref mass.InertiaInverse, out output.InertiaInverse);
Matrix.Multiply(ref output.InertiaInverse, ref orientation, out output.InertiaInverse);
}
/// <summary>
/// Solve the constraint for position.
/// </summary>
/// <returns>Returns a value indicating whether the constraint has been satisfied.</returns>
public override bool ProcessPosition()
{
RigidBody a = BodyA, b = BodyB;
if (_state == LimitState.Between)
{
return(true);
}
// recalculate vitals
Vector3.Transform(ref _bodyPointA, ref a.World.Combined, out _worldOffsetA);
Vector3.Transform(ref _bodyPointB, ref b.World.Combined, out _worldOffsetB);
Vector3.Subtract(ref _worldOffsetA, ref _worldOffsetB, out _normal);
Vector3.Subtract(ref _worldOffsetA, ref a.World.Position, out _worldOffsetA);
Vector3.Subtract(ref _worldOffsetB, ref b.World.Position, out _worldOffsetB);
_distance = _normal.Length();
Vector3.Divide(ref _normal, _distance, out _normal);
_mass = MassProperties.EffectiveMass(ref a.MassWorld, ref b.MassWorld, ref _worldOffsetA, ref _worldOffsetB, ref _normal);
// the error depends on the current limit state
float error = 0f;
switch (_state)
{
case LimitState.Equal:
if (_distance > _maxDistance)
{
error = _distance - _maxDistance;
}
else if (_distance < _minDistance)
{
error = _distance - _minDistance;
}
break;
case LimitState.Min:
error = MathHelper.Min(_distance - _minDistance, 0f);
break;
case LimitState.Max:
error = MathHelper.Max(_distance - _maxDistance, 0f);
break;
}
if (Math.Abs(error) <= this.Manager.LinearErrorTolerance)
{
return(true);
}
// clamp impulse
Vector3 impulse, oldImpulse = _pImpulse;
Vector3.Multiply(ref _normal, error * _mass * this.Manager.PositionCorrectionFactor, out impulse);
Vector3.Add(ref _pImpulse, ref impulse, out _pImpulse);
float d;
Vector3.Dot(ref _normal, ref _pImpulse, out d);
if (_state == LimitState.Min && d > 0f || _state == LimitState.Max && d < 0f)
{
_pImpulse = Vector3.Zero;
}
Vector3.Subtract(ref _pImpulse, ref oldImpulse, out impulse);
// apply impulse
b.ApplyFlashImpulse(ref impulse, ref _worldOffsetB);
Vector3.Negate(ref impulse, out impulse);
a.ApplyFlashImpulse(ref impulse, ref _worldOffsetA);
return(false);
}
public void CreateScene(int sceneNumber)
{
_physics.Clear();
_markers.Clear();
Room room = new Room(this);
_physics.Add(room);
_physics.Gravity = new Vector3(0f, 0f, -9.8f);
Model cubeModel = this.Content.Load<Model>("models/small_cube");
Model obeliskModel = this.Content.Load<Model>("models/obelisk");
Model sphereModel = this.Content.Load<Model>("models/sphere");
Model capsuleModel = this.Content.Load<Model>("models/capsule");
Model torusModel = this.Content.Load<Model>("models/torus");
Model slabModel = this.Content.Load<Model>("models/slab");
Model triangleModel = this.Content.Load<Model>("models/triangle");
switch (sceneNumber)
{
case 1:
{
for (int i = 0; i < 12; i++)
{
var cube = new SolidThing(this, cubeModel);
cube.SetWorld(new Vector3(0f, 0f, 0.25f + 0.51f * i));
_physics.Add(cube);
}
}
break;
case 2:
{
for (int i = 0; i < 7; i++)
{
for (int j = 0; j < 7 - i; j++)
{
var cube = new SolidThing(this, cubeModel);
cube.SetWorld(new Vector3(0f, 0.501f * j + 0.25f * i, 0.5f + 0.55f * i));
_physics.Add(cube);
}
}
}
break;
case 3:
{
for (int i = 0; i < 6; i++)
{
for (int j = 0; j < 6 - i; j++)
{
var cube = new SolidThing(this, cubeModel);
cube.SetWorld(new Vector3(0f, 2.2f * j + 1f * i - 4.1f, 0.75f * i + 0.25f));
_physics.Add(cube);
}
}
for (int i = 0; i < 6; i++)
{
for (int j = 0; j < 5 - i; j++)
{
var plank = new SolidThing(this, obeliskModel);
plank.SetWorld(new Vector3(0f, 2.2f * j + 1f * i + 1f - 4f, 0.75f * i + 0.65f),
Quaternion.CreateFromAxisAngle(Vector3.UnitZ, MathHelper.ToRadians(90f)));
_physics.Add(plank);
}
}
}
break;
case 4:
{
int size = 9;
#if WINDOWS
#else
size = 4;
#endif
for (int i = 0; i < size; i++)
{
for (int j = 0; j < size - i; j++)
{
for (int k = 0; k < size - i; k++)
{
var sphere = new SolidThing(this, sphereModel);
sphere.SetWorld(new Vector3(
0.501f * j + 0.25f * i,
0.501f * k + 0.25f * i,
0.501f * i + 0.5f
), Quaternion.Identity);
_physics.Add(sphere);
}
}
}
}
break;
case 5:
{
var plank = new SolidThing(this, obeliskModel);
plank.SetWorld(new Vector3(0.0f, 0.0f, 4.0f),
Quaternion.CreateFromAxisAngle(Vector3.UnitY, MathHelper.ToRadians(15f)));
MassProperties immovableMassProperties = new MassProperties(float.PositiveInfinity, Matrix.Identity);
plank.MassProperties = immovableMassProperties;
_physics.Add(plank);
var sphere = new SolidThing(this, sphereModel);
sphere.SetWorld(new Vector3(-4.9f, 0.0f, 9.0f), Quaternion.Identity);
_physics.Add(sphere);
// int size = 9;
//#if WINDOWS
//#else
// size = 4;
//#endif
// var models = new Model[] { cubeModel, sphereModel };
// for (int i = 0; i < size; i++)
// {
// for (int j = 0; j < size - i; j++)
// {
// for (int k = 0; k < size - i; k++)
// {
// var sphere = new SolidThing(this, i % 2 == 0 ? sphereModel : cubeModel);
// sphere.SetWorld(new Vector3(
// 0.501f * j + 0.25f * i,
// 0.501f * k + 0.25f * i,
// 0.501f * i + 0.5f
// ), Quaternion.Identity);
// _physics.Add(sphere);
// }
// }
// }
}
break;
case 6:
{
int size = 9;
#if WINDOWS
#else
size = 4;
#endif
var models = new Model[] { cubeModel, sphereModel, capsuleModel };
for (int i = 0; i < size; i++)
{
for (int j = 0; j < size - i; j++)
{
for (int k = 0; k < size - i; k++)
{
var sphere = new SolidThing(this, models[_rand.Next(3)]);
sphere.SetWorld(new Vector3(
0.501f * j + 0.25f * i,
0.501f * k + 0.25f * i,
1f * i + 0.5f));
_physics.Add(sphere);
}
}
}
}
break;
case 7:
{
var o = new SolidThing(this, torusModel);
o.SetWorld(new Vector3(0f, 0f, 0.5f), Quaternion.CreateFromAxisAngle(Vector3.UnitY, -MathHelper.PiOver2));
_physics.Add(o);
o = new SolidThing(this, torusModel);
o.SetWorld(new Vector3(0f, 0f, 4f), Quaternion.CreateFromAxisAngle(Vector3.UnitY, -MathHelper.PiOver4));
_physics.Add(o);
o = new SolidThing(this, slabModel);
o.SetWorld(new Vector3(-4f, 4f, 2f));
_physics.Add(o);
o = new SolidThing(this, this.Content.Load<Model>("models/cone"));
o.SetWorld(new Vector3(-4f, -4f, 1f), Quaternion.CreateFromAxisAngle(Vector3.UnitZ, MathHelper.PiOver2));
_physics.Add(o);
o = new SolidThing(this, cubeModel);
o.SetWorld(new Vector3(-4f, 6.1f, 3f));
_physics.Add(o);
}
break;
case 8:
{
RigidBody oLast = null;
for (int i = 0; i < 10; i++)
{
var o = new SolidThing(this, capsuleModel);
o.SetWorld(new Vector3(0f, 0f, 9.5f - i));
_physics.Add(o);
if (i == 0)
{
var j = new PointConstraint(o, room, new Vector3(0f, 0f, 10f));
j.IsCollisionEnabled = false;
_physics.Add(j);
}
else
{
var j = new PointConstraint(oLast, o, new Vector3(0f, 0f, 10f - (float)i));
j.IsCollisionEnabled = false;
_physics.Add(j);
}
oLast = o;
}
var a = new SolidThing(this, cubeModel);
a.SetWorld(new Vector3(1f, 0f, 0.25f));
_physics.Add(a);
var b = new SolidThing(this, cubeModel);
b.SetWorld(new Vector3(1f, 0f, 0.75f));
_physics.Add(b);
var j2 = new RevoluteJoint(b, a, new Vector3(1.25f, 0f, 0.5f), Vector3.UnitY,
0f, MathHelper.PiOver2);
j2.IsCollisionEnabled = false;
_physics.Add(j2);
a = new SolidThing(this, cubeModel);
a.SetWorld(new Vector3(1f, 1f, 0.25f));
_physics.Add(a);
b = new SolidThing(this, cubeModel);
b.SetWorld(new Vector3(1f, 1f, 0.75f));
_physics.Add(b);
var j4 = new GenericConstraint(b, a, new Frame(new Vector3(1f, 1f, 0.5f)),
Axes.All, Vector3.Zero, new Vector3(0f, 0f, 0.5f),
Axes.All, Vector3.Zero, Vector3.Zero);
j4.IsCollisionEnabled = false;
_physics.Add(j4);
a = new SolidThing(this, cubeModel);
a.SetWorld(new Vector3(1f, 2f, 0.25f));
_physics.Add(a);
b = new SolidThing(this, cubeModel);
b.SetWorld(new Vector3(1f, 2f, 0.75f));
_physics.Add(b);
var j5 = new GenericConstraint(b, a, new Frame(new Vector3(1f, 2f, 0.5f)),
Axes.All, new Vector3(-0.125f, -0.125f, 0f), new Vector3(0.125f, 0.125f, 0f),
Axes.All, Vector3.Zero, Vector3.Zero);
j5.IsCollisionEnabled = false;
_physics.Add(j5);
a = new SolidThing(this, sphereModel);
a.SetWorld(new Vector3(2f, 0f, 2f));
_physics.Add(a);
b = new SolidThing(this, sphereModel);
b.SetWorld(new Vector3(2f, 0f, 1f));
_physics.Add(b);
var g1 = new SpringForce(a, b, Vector3.Zero, Vector3.Zero, 1f, 5f, 0.05f);
_physics.Add(g1);
var j3 = new WorldPointConstraint(a, new Vector3(2f, 0f, 2f));
_physics.Add(j3);
}
break;
case 9:
{
var a = new SolidThing(this, sphereModel);
a.Skin.Remove(a.Skin[0]);
a.Skin.Add(new SpherePart(new Sphere(Vector3.Zero, 0.25f)), new Material(0f, 0.5f));
a.SetWorld(7.0f, new Vector3(0f, 0f, 5f), Quaternion.Identity);
a.MassProperties = MassProperties.Immovable;
_physics.Add(a);
_physics.Add(new SingularityForce(new Vector3(0f, 0f, 5f), 1E12f));
_physics.Gravity = Vector3.Zero;
var b = new SolidThing(this, cubeModel);
b.SetWorld(new Vector3(0f, 0f, 8f),
Quaternion.CreateFromAxisAngle(Vector3.UnitX, MathHelper.PiOver4 / 2.0f) *
Quaternion.CreateFromAxisAngle(Vector3.UnitY, MathHelper.PiOver4)
);
_physics.Add(b);
}
break;
default:
break;
}
}
/// <summary>
/// Prepare the contact constraint for iterative processing within a single frame. Computes the desired target velocities
/// to attempt to prevent inter-penetration.
/// </summary>
public override void PreProcess()
{
if (this.IsCollisionSuppressed)
{
return;
}
RigidBody a = BodyA, b = BodyB;
var cached = b.Manager.ContactCache.Get(a, b);
bool isWarmStarted = cached != null && cached.Count == _count;
// calculate relative force applied during this frame
Vector3 va = Vector3.Zero, vb = Vector3.Zero;
float forceMag;
if (a.IsMovable)
{
Vector3.Multiply(ref a.Force, this.Manager.TimeStep * a.Mass.MassInverse, out va);
}
if (b.IsMovable)
{
Vector3.Multiply(ref b.Force, this.Manager.TimeStep * b.Mass.MassInverse, out vb);
}
Vector3.Add(ref va, ref vb, out va);
Vector3.Dot(ref _normal, ref va, out forceMag);
forceMag = MathHelper.Min(forceMag, 0f);
for (int i = 0; i < _count; i++)
{
var p = _points[i];
// calculate movement along normal and tangent
float normalDelta;
a.GetVelocityAtPoint(ref p.OffsetA, out va);
b.GetVelocityAtPoint(ref p.OffsetB, out vb);
Vector3.Subtract(ref va, ref vb, out va);
Vector3.Dot(ref _normal, ref va, out normalDelta);
float tangentDelta;
Vector3.Multiply(ref _normal, normalDelta, out p.Tangent);
Vector3.Subtract(ref va, ref p.Tangent, out p.Tangent);
Vector3.Dot(ref p.Tangent, ref va, out tangentDelta);
if (p.Tangent.LengthSquared() >= Constants.Epsilon)
{
p.Tangent.Normalize();
Vector3.Negate(ref p.Tangent, out p.Tangent);
}
else
{
p.Tangent = Vector3.Zero;
}
// calculate effective mass along tangent and normal
p.NormalMass = MassProperties.EffectiveMass(ref a.MassWorld, ref b.MassWorld, ref p.OffsetA, ref p.OffsetB, ref _normal);
p.TangentMass = p.Tangent != Vector3.Zero ?
MassProperties.EffectiveMass(ref a.MassWorld, ref b.MassWorld, ref p.OffsetA, ref p.OffsetB, ref p.Tangent) : 0f;
// calculate target velocity
float restitution = Math.Max(_restitution * -(normalDelta - forceMag), 0f);
float penetration = (p.Depth - this.Manager.LinearErrorTolerance);
if (restitution < this.Manager.MinRestitution)
{
if (penetration > 0f)
{
p.Target = penetration * this.Manager.PenetrationBias;
}
else
{
float scale = MathHelper.Clamp(-0.1f * penetration / this.Manager.LinearErrorTolerance, Constants.Epsilon, 1f);
p.Target = scale * (p.Depth - this.Manager.LinearErrorTolerance) * this.Manager.TimeStepInverse;
}
}
else
{
p.Target = Math.Max(Math.Max(penetration * this.Manager.PenetrationBias, 0f), restitution);
}
p.Impulse = 0f;
p.TangentImpulse = 0f;
p.PositionImpulse = 0f;
if (isWarmStarted)
{
Vector3 impulse, tangentImpulse;
// find the best cached point
var bestPoint = new CachedContactPoint();
float bestDistance = float.MaxValue;
for (int j = 0; j < cached.Points.Length; j++)
{
float d1, d2;
Vector3.DistanceSquared(ref cached.Points[j].OffsetA, ref p.OffsetA, out d1);
Vector3.DistanceSquared(ref cached.Points[j].OffsetB, ref p.OffsetB, out d2);
if (d1 + d2 < bestDistance)
{
bestDistance = d1 + d2;
bestPoint = cached.Points[j];
}
}
p.Impulse = bestPoint.NormalImpulse;
float tangentImpulseMag = MathHelper.Clamp(-tangentDelta * p.TangentMass, 0f, _friction * p.Impulse);
p.TangentImpulse = tangentImpulseMag * p.NormalMass;
if (Math.Abs(p.Impulse) >= Constants.Epsilon)
{
Vector3.Multiply(ref _normal, p.Impulse, out impulse);
Vector3.Multiply(ref p.Tangent, p.TangentImpulse, out tangentImpulse);
Vector3.Add(ref impulse, ref tangentImpulse, out impulse);
a.ApplyImpulse(ref impulse, ref p.OffsetA);
Vector3.Negate(ref impulse, out impulse);
b.ApplyImpulse(ref impulse, ref p.OffsetB);
}
}
_points[i] = p;
}
// calculate an averaged contact point for help with stabilization during position correction
if (_count > 2 && _count < _points.Length)
{
var ap = _points[_count];
ap.Depth = float.MaxValue;
for (int i = 0; i < _count; i++)
{
var p = _points[i];
float depth;
Vector3 pa, pb;
Vector3.Add(ref a.World.Position, ref p.OffsetA, out pa);
Vector3.Add(ref b.World.Position, ref p.OffsetB, out pb);
Vector3.Add(ref ap.OffsetA, ref pa, out ap.OffsetA);
Vector3.Add(ref ap.OffsetB, ref pb, out ap.OffsetB);
Vector3.Subtract(ref pb, ref pa, out pa);
Vector3.Dot(ref _normal, ref pa, out depth);
if (depth < ap.Depth)
{
ap.Depth = depth;
}
}
Vector3.Divide(ref ap.OffsetA, _count, out ap.OffsetA);
Vector3.Divide(ref ap.OffsetB, _count, out ap.OffsetB);
ap.NormalMass = MassProperties.EffectiveMass(ref a.MassWorld, ref b.MassWorld, ref ap.OffsetA, ref ap.OffsetB, ref _normal);
ap.PositionImpulse = 0f;
_points[_count] = ap;
}
}
