public BodyDef()
{
// Set all initial positions/velocties to zero
Vec3.Identity(ref axis);
angle = 0;
Vec3.Identity(ref position);
Vec3.Identity(ref linearVelocity);
Vec3.Identity(ref angularVelocity);
// Usually a gravity scale of 1 is the best
gravityScale = 1;
// Common default values
bodyType = eStaticBody;
layers = 0x000000001;
userData = null;
allowSleep = true;
awake = true;
active = true;
lockAxisX = false;
lockAxisY = false;
lockAxisZ = false;
linearDamping = 0;
angularDamping = 0.1;
}
public void SetToSleep()
{
Flags &= ~eAwake;
SleepTime = 0;
Vec3.Identity(ref LinearVelocity);
Vec3.Identity(ref AngularVelocity);
Vec3.Identity(ref Force);
Vec3.Identity(ref Torque);
}
// Run the simulation forward in time by dt (fixed timestep). Variable
// timestep is not supported.
public void Step(double Dt)
{
if (NewBox)
{
ContactManager.Broadphase.UpdatePairs();
NewBox = false;
}
ContactManager.TestCollisions();
foreach (var body in Bodies)
{
body.Flags &= ~BodyFlags.eIsland;
}
Island.AllowSleep = AllowSleep;
Island.EnableFriction = EnableFriction;
Island.Dt = Dt;
Island.Gravity = Gravity;
Island.Iterations = Iterations;
// Build each active Island and then solve each built Island
// int stackSize = Bodies.Count;
foreach (var seed in Bodies)
{
// Seed cannot be apart of an Island already
if ((seed.Flags & BodyFlags.eIsland) > 0)
{
continue;
}
// Seed must be awake
if ((seed.Flags & BodyFlags.eAwake) == 0)
{
continue;
}
// Seed cannot be a static body in order to keep islands
// as small as possible
if ((seed.Flags & BodyFlags.eStatic) > 0)
{
continue;
}
int stackCount = 0;
stack[stackCount++] = seed;
Island.Clear();
// Mark seed as apart of Island
seed.Flags |= BodyFlags.eIsland;
// Perform DFS on constraint graph
while (stackCount > 0)
{
// Decrement stack to implement iterative backtracking
Body body = stack[--stackCount];
Island.Add(body);
// Awaken all bodies connected to the Island
body.SetToAwake();
// Do not search across static bodies to keep Island
// formations as small as possible, however the static
// body itself should be apart of the Island in order
// to properly represent a full contact
if ((body.Flags & BodyFlags.eStatic) > 0)
{
continue;
}
// Search all contacts connected to this body
foreach (var edge in body.ContactList)
{
ContactConstraint contact = edge.constraint;
// Skip contacts that have been added to an Island already
if ((contact.Flags & ContactFlags.eIsland) > 0)
{
continue;
}
// Can safely skip this contact if it didn't actually collide with anything
if ((contact.Flags & ContactFlags.eColliding) == 0)
{
continue;
}
// Skip sensors
if (contact.A.sensor || contact.B.sensor)
{
continue;
}
// Mark Island flag and add to Island
contact.Flags |= ContactFlags.eIsland;
Island.Add(contact);
// Attempt to add the other body in the contact to the Island
// to simulate contact awakening propogation
Body other = edge.other;
if ((other.Flags & BodyFlags.eIsland) > 0)
{
continue;
}
Assert(stackCount < 256);
stack[stackCount++] = other;
other.Flags |= BodyFlags.eIsland;
}
}
Assert(Island.Bodies.Count != 0);
Island.Initialize();
Island.Solve();
// Reset all static Island flags
// This allows static bodies to participate in other Island formations
foreach (var body in Island.Bodies)
{
if ((body.Flags & BodyFlags.eStatic) > 0)
{
body.Flags &= ~BodyFlags.eIsland;
}
}
}
// Update the broadphase AABBs
foreach (var body in Bodies)
{
if ((body.Flags & BodyFlags.eStatic) > 0)
{
continue;
}
body.SynchronizeProxies();
}
// Look for new contacts
ContactManager.FindNewContacts();
// Clear all forces
foreach (var body in Bodies)
{
Vec3.Identity(ref body.Force);
Vec3.Identity(ref body.Torque);
}
}
void CalculateMassData()
{
Mat3 inertia = Mat3.Diagonal(0);
InvInertiaModel = Mat3.Diagonal(0);
InvInertiaWorld = Mat3.Diagonal(0);
InvMass = 0;
Mass = 0;
double mass = 0;
if ((Flags & eStatic) > 0 || (Flags & eKinematic) > 0)
{
Vec3.Identity(ref LocalCenter);
WorldCenter = Tx.position;
return;
}
Vec3 lc = new Vec3();
Vec3.Identity(ref lc);
foreach (var box in Boxes)
{
if (box.density == 0)
{
continue;
}
MassData md;
box.ComputeMass(out md);
mass += md.mass;
inertia += md.inertia;
lc += md.center * md.mass;
}
if (mass > 0)
{
Mass = mass;
InvMass = 1 / mass;
lc *= InvMass;
inertia -= (Mat3.Identity * Vec3.Dot(lc, lc) - Mat3.OuterProduct(lc, lc)) * mass;
InvInertiaModel = Mat3.Inverse(inertia);
if ((Flags & eLockAxisX) > 0)
{
Vec3.Identity(ref InvInertiaModel.ex);
}
if ((Flags & eLockAxisY) > 0)
{
Vec3.Identity(ref InvInertiaModel.ey);
}
if ((Flags & eLockAxisZ) > 0)
{
Vec3.Identity(ref InvInertiaModel.ez);
}
}
else
{
// Force all dynamic bodies to have some mass
InvMass = 1;
InvInertiaModel = Mat3.Diagonal(0);
InvInertiaWorld = Mat3.Diagonal(0);
}
LocalCenter = lc;
WorldCenter = Transform.Mul(Tx, lc);
}
public Body(BodyDef def, Scene scene)
{
LinearVelocity = def.linearVelocity;
AngularVelocity = def.angularVelocity;
Vec3.Identity(ref Force);
Vec3.Identity(ref Torque);
Q.Set(Vec3.Normalize(def.axis), def.angle);
Tx.rotation = Q.ToMat3();
Tx.position = def.position;
SleepTime = 0;
GravityScale = def.gravityScale;
Layers = def.layers;
UserData = def.userData;
Scene = scene;
Flags = 0;
LinearDamping = def.linearDamping;
AngularDamping = def.angularDamping;
if (def.bodyType == eDynamicBody)
{
Flags |= eDynamic;
}
else
{
if (def.bodyType == eStaticBody)
{
Flags |= eStatic;
Vec3.Identity(ref LinearVelocity);
Vec3.Identity(ref AngularVelocity);
Vec3.Identity(ref Force);
Vec3.Identity(ref Torque);
}
else if (def.bodyType == eKinematicBody)
{
Flags |= eKinematic;
}
}
if (def.allowSleep)
{
Flags |= eAllowSleep;
}
if (def.awake)
{
Flags |= eAwake;
}
if (def.active)
{
Flags |= eActive;
}
if (def.lockAxisX)
{
Flags |= eLockAxisX;
}
if (def.lockAxisY)
{
Flags |= eLockAxisY;
}
if (def.lockAxisZ)
{
Flags |= eLockAxisZ;
}
Boxes = new List <Box>();
ContactList = new List <ContactEdge>();
}