// Cache here per time step to reduce cache misses.
// final Vec2 m_localCenterA, m_localCenterB;
// float m_invMassA, m_invIA;
// float m_invMassB, m_invIB;
protected internal Joint(IWorldPool argWorldPool, JointDef def)
{
Debug.Assert(def.bodyA != def.bodyB);
pool = argWorldPool;
m_type = def.type;
m_prev = null;
m_next = null;
m_bodyA = def.bodyA;
m_bodyB = def.bodyB;
m_collideConnected = def.collideConnected;
m_islandFlag = false;
m_userData = def.userData;
m_index = 0;
m_edgeA = new JointEdge();
m_edgeA.joint = null;
m_edgeA.other = null;
m_edgeA.prev = null;
m_edgeA.next = null;
m_edgeB = new JointEdge();
m_edgeB.joint = null;
m_edgeB.other = null;
m_edgeB.prev = null;
m_edgeB.next = null;
// m_localCenterA = new Vec2();
// m_localCenterB = new Vec2();
}
/// <summary>
/// destroy a joint. This may cause the connected bodies to begin colliding.
/// </summary>
/// <warning>This function is locked during callbacks.</warning>
/// <param name="joint"></param>
public virtual void destroyJoint(Joint j)
{
Debug.Assert(Locked == false);
if (Locked)
{
return;
}
bool collideConnected = j.m_collideConnected;
// Remove from the doubly linked list.
if (j.m_prev != null)
{
j.m_prev.m_next = j.m_next;
}
if (j.m_next != null)
{
j.m_next.m_prev = j.m_prev;
}
if (j == m_jointList)
{
m_jointList = j.m_next;
}
// Disconnect from island graph.
Body bodyA = j.m_bodyA;
Body bodyB = j.m_bodyB;
// Wake up connected bodies.
bodyA.Awake = true;
bodyB.Awake = true;
// Remove from body 1.
if (j.m_edgeA.prev != null)
{
j.m_edgeA.prev.next = j.m_edgeA.next;
}
if (j.m_edgeA.next != null)
{
j.m_edgeA.next.prev = j.m_edgeA.prev;
}
if (j.m_edgeA == bodyA.m_jointList)
{
bodyA.m_jointList = j.m_edgeA.next;
}
j.m_edgeA.prev = null;
j.m_edgeA.next = null;
// Remove from body 2
if (j.m_edgeB.prev != null)
{
j.m_edgeB.prev.next = j.m_edgeB.next;
}
if (j.m_edgeB.next != null)
{
j.m_edgeB.next.prev = j.m_edgeB.prev;
}
if (j.m_edgeB == bodyB.m_jointList)
{
bodyB.m_jointList = j.m_edgeB.next;
}
j.m_edgeB.prev = null;
j.m_edgeB.next = null;
Joint.destroy(j);
Debug.Assert(m_jointCount > 0);
--m_jointCount;
// If the joint prevents collisions, then flag any contacts for filtering.
if (collideConnected == false)
{
ContactEdge edge = bodyB.ContactList;
while (edge != null)
{
if (edge.other == bodyA)
{
// Flag the contact for filtering at the next time step (where either
// body is awake).
edge.contact.flagForFiltering();
}
edge = edge.next;
}
}
}
/// <summary>
/// create a joint to constrain bodies together. No reference to the definition is retained. This
/// may cause the connected bodies to cease colliding.
/// </summary>
/// <warning>This function is locked during callbacks.</warning>
/// <param name="def"></param>
/// <returns></returns>
public virtual Joint createJoint(JointDef def)
{
Debug.Assert(Locked == false);
if (Locked)
{
return null;
}
Joint j = Joint.create(this, def);
// Connect to the world list.
j.m_prev = null;
j.m_next = m_jointList;
if (m_jointList != null)
{
m_jointList.m_prev = j;
}
m_jointList = j;
++m_jointCount;
// Connect to the bodies' doubly linked lists.
j.m_edgeA.joint = j;
j.m_edgeA.other = j.m_bodyB;
j.m_edgeA.prev = null;
j.m_edgeA.next = j.m_bodyA.m_jointList;
if (j.m_bodyA.m_jointList != null)
{
j.m_bodyA.m_jointList.prev = j.m_edgeA;
}
j.m_bodyA.m_jointList = j.m_edgeA;
j.m_edgeB.joint = j;
j.m_edgeB.other = j.m_bodyA;
j.m_edgeB.prev = null;
j.m_edgeB.next = j.m_bodyB.m_jointList;
if (j.m_bodyB.m_jointList != null)
{
j.m_bodyB.m_jointList.prev = j.m_edgeB;
}
j.m_bodyB.m_jointList = j.m_edgeB;
Body bodyA = def.bodyA;
Body bodyB = def.bodyB;
// If the joint prevents collisions, then flag any contacts for filtering.
if (def.collideConnected == false)
{
ContactEdge edge = bodyB.ContactList;
while (edge != null)
{
if (edge.other == bodyA)
{
// Flag the contact for filtering at the next time step (where either
// body is awake).
edge.contact.flagForFiltering();
}
edge = edge.next;
}
}
// Note: creating a joint doesn't wake the bodies.
return j;
}
/// <summary>
/// Construct a world object.
/// </summary>
/// <param name="gravity">the world gravity vector.</param>
/// <param name="doSleep">improve performance by not simulating inactive bodies.</param>
public World(Vec2 gravity, IWorldPool argPool)
{
contactStacks = new ContactRegister[ShapeTypesCount][];
for (int i = 0; i < ShapeTypesCount; i++)
{
contactStacks[i] = new ContactRegister[ShapeTypesCount];
}
pool = argPool;
m_destructionListener = null;
m_debugDraw = null;
m_bodyList = null;
m_jointList = null;
m_bodyCount = 0;
m_jointCount = 0;
m_warmStarting = true;
m_continuousPhysics = true;
m_subStepping = false;
m_stepComplete = true;
m_allowSleep = true;
m_gravity.set_Renamed(gravity);
m_flags = CLEAR_FORCES;
m_inv_dt0 = 0f;
m_contactManager = new ContactManager(this);
m_profile = new Profile();
initializeRegisters();
}
private void drawJoint(Joint joint)
{
Body bodyA = joint.BodyA;
Body bodyB = joint.BodyB;
Transform xf1 = bodyA.getTransform();
Transform xf2 = bodyB.getTransform();
Vec2 x1 = xf1.p;
Vec2 x2 = xf2.p;
Vec2 p1 = pool.popVec2();
Vec2 p2 = pool.popVec2();
joint.getAnchorA(p1);
joint.getAnchorB(p2);
color.set_Renamed(0.5f, 0.8f, 0.8f);
switch (joint.Type)
{
// TODO djm write after writing joints
case JointType.DISTANCE:
m_debugDraw.drawSegment(p1, p2, color);
break;
case JointType.PULLEY:
{
PulleyJoint pulley = (PulleyJoint)joint;
Vec2 s1 = pulley.GroundAnchorA;
Vec2 s2 = pulley.GroundAnchorB;
m_debugDraw.drawSegment(s1, p1, color);
m_debugDraw.drawSegment(s2, p2, color);
m_debugDraw.drawSegment(s1, s2, color);
}
break;
case JointType.CONSTANT_VOLUME:
case JointType.MOUSE:
// don't draw this
break;
default:
m_debugDraw.drawSegment(x1, p1, color);
m_debugDraw.drawSegment(p1, p2, color);
m_debugDraw.drawSegment(x2, p2, color);
break;
}
pool.pushVec2(2);
}
private void DrawJoint(Joint joint)
{
Body bodyA = joint.BodyA;
Body bodyB = joint.BodyB;
Transform xf1 = bodyA.GetTransform();
Transform xf2 = bodyB.GetTransform();
Vec2 x1 = xf1.P;
Vec2 x2 = xf2.P;
Vec2 p1 = Pool.PopVec2();
Vec2 p2 = Pool.PopVec2();
joint.GetAnchorA(p1);
joint.GetAnchorB(p2);
color.Set(0.5f, 0.8f, 0.8f);
switch (joint.Type)
{
// TODO djm write after writing joints
case JointType.Distance:
DebugDraw.DrawSegment(p1, p2, color);
break;
case JointType.Pulley:
{
PulleyJoint pulley = (PulleyJoint)joint;
Vec2 s1 = pulley.GroundAnchorA;
Vec2 s2 = pulley.GroundAnchorB;
DebugDraw.DrawSegment(s1, p1, color);
DebugDraw.DrawSegment(s2, p2, color);
DebugDraw.DrawSegment(s1, s2, color);
}
break;
case JointType.ConstantVolume:
case JointType.Mouse:
// don't draw this
break;
default:
DebugDraw.DrawSegment(x1, p1, color);
DebugDraw.DrawSegment(p1, p2, color);
DebugDraw.DrawSegment(x2, p2, color);
break;
}
Pool.PushVec2(2);
}
public void Add(Joint joint)
{
Debug.Assert(JointCount < JointCapacity);
Joints[JointCount++] = joint;
}
// Cache here per time step to reduce cache misses.
// final Vec2 m_localCenterA, m_localCenterB;
// float m_invMassA, m_invIA;
// float m_invMassB, m_invIB;
protected internal Joint(IWorldPool argWorldPool, JointDef def)
{
Debug.Assert(def.BodyA != def.BodyB);
Pool = argWorldPool;
Type = def.Type;
Prev = null;
Next = null;
BodyA = def.BodyA;
BodyB = def.BodyB;
CollideConnected = def.CollideConnected;
IslandFlag = false;
UserData = def.UserData;
Index = 0;
EdgeA = new JointEdge();
EdgeA.Joint = null;
EdgeA.Other = null;
EdgeA.Prev = null;
EdgeA.Next = null;
EdgeB = new JointEdge();
EdgeB.Joint = null;
EdgeB.Other = null;
EdgeB.Prev = null;
EdgeB.Next = null;
// m_localCenterA = new Vec2();
// m_localCenterB = new Vec2();
}
public static void Destroy(Joint joint)
{
joint.Destructor();
}
public virtual void add(Joint joint)
{
Debug.Assert(m_jointCount < m_jointCapacity);
m_joints[m_jointCount++] = joint;
}
