// Cache here per time step to reduce cache misses.
// Vec2 m_localCenterA, m_localCenterB;
// float m_invMassA, m_invIA;
// float m_invMassB, m_invIB;
protected Joint(IWorldPool worldPool, JointDef def)
{
Debug.Assert(def.bodyA != def.bodyB);
pool = worldPool;
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_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();
}
public override long getTag(Joint argJoint)
{
if (argJoint == joint)
{
return JOINT_TAG;
}
return default(long);
}
public override long getTag(Joint argJoint)
{
if (argJoint == m_motorJoint)
{
return MOTOR_TAG;
}
return default(long);
}
public override void processJoint(Joint argJoint, long argTag)
{
if (argTag == MOTOR_TAG)
{
m_motorJoint = (RevoluteJoint) argJoint;
m_motorOn = m_motorJoint.isMotorEnabled();
}
}
public override void processJoint(Joint argJoint, long argTag)
{
if (argTag == JOINT_TAG)
{
joint = (RevoluteJoint) argJoint;
}
}
public override void initTest(bool deserialized)
{
if (deserialized)
{
return;
}
Body ground = null;
{
BodyDef bd = new BodyDef();
ground = getWorld().createBody(bd);
EdgeShape shape = new EdgeShape();
shape.set(new Vec2(-40.0f, 0.0f), new Vec2(40.0f, 0.0f));
ground.createFixture(shape, 0.0f);
}
{
PolygonShape shape = new PolygonShape();
shape.setAsBox(0.5f, 0.125f);
FixtureDef fd = new FixtureDef();
fd.shape = shape;
fd.density = 20.0f;
fd.friction = 0.2f;
fd.filter.categoryBits = 0x0001;
fd.filter.maskBits = 0xFFFF & ~0x0002;
RevoluteJointDef jd = new RevoluteJointDef();
jd.collideConnected = false;
int N = 10;
float y = 15.0f;
m_ropeDef = new RopeJointDef();
m_ropeDef.localAnchorA.set(0.0f, y);
Body prevBody = ground;
for (int i = 0; i < N; ++i)
{
BodyDef bd = new BodyDef();
bd.type = BodyType.DYNAMIC;
bd.position.set(0.5f + 1.0f*i, y);
if (i == N - 1)
{
shape.setAsBox(1.5f, 1.5f);
fd.density = 100.0f;
fd.filter.categoryBits = 0x0002;
bd.position.set(1.0f*i, y);
bd.angularDamping = 0.4f;
}
Body body = getWorld().createBody(bd);
body.createFixture(fd);
Vec2 anchor = new Vec2(i, y);
jd.initialize(prevBody, body, anchor);
getWorld().createJoint(jd);
prevBody = body;
}
m_ropeDef.localAnchorB.setZero();
float extraLength = 0.01f;
m_ropeDef.maxLength = N - 1.0f + extraLength;
m_ropeDef.bodyB = prevBody;
}
{
m_ropeDef.bodyA = ground;
m_rope = getWorld().createJoint(m_ropeDef);
}
}
public static void destroy(Joint joint)
{
joint.destructor();
}
public override void processJoint(Joint joint, long tag)
{
if (tag == JOINT_TAG)
{
m_joint = (PrismaticJoint) joint;
}
else
{
base.processJoint(joint, tag);
}
}
public override void processJoint(Joint joint, long tag)
{
if (tag == JOINT_TAG)
{
m_joint = (RevoluteJoint) joint;
isLeft = m_joint.getMotorSpeed() > 0;
}
else
{
base.processJoint(joint, tag);
}
}
public virtual void jointDestroyed(Joint joint)
{
}
public virtual long getTag(Joint joint)
{
return default(long);
}
/**
* destroy a joint. This may cause the connected bodies to begin colliding.
*
* @warning This function is locked during callbacks.
* @param joint
*/
public void destroyJoint(Joint j)
{
Debug.Assert(isLocked() == false);
if (isLocked())
{
return;
}
bool collideConnected = j.getCollideConnected();
// 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.getBodyA();
Body bodyB = j.getBodyB();
// Wake up connected bodies.
bodyA.setAwake(true);
bodyB.setAwake(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.getContactList();
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;
}
}
}
/**
* create a joint to constrain bodies together. No reference to the definition is retained. This
* may cause the connected bodies to cease colliding.
*
* @warning This function is locked during callbacks.
* @param def
* @return
*/
public Joint createJoint(JointDef def)
{
Debug.Assert(isLocked() == false);
if (isLocked())
{
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.getBodyB();
j.m_edgeA.prev = null;
j.m_edgeA.next = j.getBodyA().m_jointList;
if (j.getBodyA().m_jointList != null)
{
j.getBodyA().m_jointList.prev = j.m_edgeA;
}
j.getBodyA().m_jointList = j.m_edgeA;
j.m_edgeB.joint = j;
j.m_edgeB.other = j.getBodyA();
j.m_edgeB.prev = null;
j.m_edgeB.next = j.getBodyB().m_jointList;
if (j.getBodyB().m_jointList != null)
{
j.getBodyB().m_jointList.prev = j.m_edgeB;
}
j.getBodyB().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.getContactList();
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;
}
public World(Vec2 gravity, IWorldPool pool, BroadPhase broadPhase)
{
this.pool = pool;
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(gravity);
m_flags = CLEAR_FORCES;
m_inv_dt0 = 0f;
m_contactManager = new ContactManager(this, broadPhase);
m_profile = new Profile();
m_particleSystem = new ParticleSystem(this);
for (int i = 0; i < contactStacks.Length; i++)
{
contactStacks[i] = new ContactRegister[Enum.GetValues(typeof (ShapeType)).Length];
}
initializeRegisters();
}
private void drawJoint(Joint joint)
{
Body bodyA = joint.getBodyA();
Body bodyB = joint.getBodyB();
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(ref p1);
joint.getAnchorB(ref p2);
color.set(0.5f, 0.8f, 0.8f);
switch (joint.getType())
{
// 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.getGroundAnchorA();
Vec2 s2 = pulley.getGroundAnchorB();
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);
}
public override void jointDestroyed(Joint joint)
{
for (int i = 0; i < 8; ++i)
{
if (m_joints[i] == joint)
{
m_joints[i] = null;
break;
}
}
}
internal GearJoint(IWorldPool argWorldPool, GearJointDef def)
: base(argWorldPool, def)
{
m_joint1 = def.joint1;
m_joint2 = def.joint2;
m_typeA = m_joint1.getType();
m_typeB = m_joint2.getType();
Debug.Assert(m_typeA == JointType.REVOLUTE || m_typeA == JointType.PRISMATIC);
Debug.Assert(m_typeB == JointType.REVOLUTE || m_typeB == JointType.PRISMATIC);
float coordinateA, coordinateB;
// TODO_ERIN there might be some problem with the joint edges in Joint.
m_bodyC = m_joint1.getBodyA();
m_bodyA = m_joint1.getBodyB();
// Get geometry of joint1
Transform xfA = m_bodyA.m_xf;
float aA = m_bodyA.m_sweep.a;
Transform xfC = m_bodyC.m_xf;
float aC = m_bodyC.m_sweep.a;
if (m_typeA == JointType.REVOLUTE)
{
RevoluteJoint revolute = (RevoluteJoint) def.joint1;
m_localAnchorC.set(revolute.m_localAnchorA);
m_localAnchorA.set(revolute.m_localAnchorB);
m_referenceAngleA = revolute.m_referenceAngle;
m_localAxisC.setZero();
coordinateA = aA - aC - m_referenceAngleA;
}
else
{
Vec2 pA = pool.popVec2();
Vec2 temp = pool.popVec2();
PrismaticJoint prismatic = (PrismaticJoint) def.joint1;
m_localAnchorC.set(prismatic.m_localAnchorA);
m_localAnchorA.set(prismatic.m_localAnchorB);
m_referenceAngleA = prismatic.m_referenceAngle;
m_localAxisC.set(prismatic.m_localXAxisA);
Vec2 pC = m_localAnchorC;
Rot.mulToOutUnsafe(xfA.q, m_localAnchorA, ref temp);
temp.addLocal(xfA.p);
temp.subLocal(xfC.p);
Rot.mulTransUnsafe(xfC.q, temp, ref pA);
pA.subLocal(pC);
coordinateA = Vec2.dot(pA, m_localAxisC);
pool.pushVec2(2);
}
m_bodyD = m_joint2.getBodyA();
m_bodyB = m_joint2.getBodyB();
// Get geometry of joint2
Transform xfB = m_bodyB.m_xf;
float aB = m_bodyB.m_sweep.a;
Transform xfD = m_bodyD.m_xf;
float aD = m_bodyD.m_sweep.a;
if (m_typeB == JointType.REVOLUTE)
{
RevoluteJoint revolute = (RevoluteJoint) def.joint2;
m_localAnchorD.set(revolute.m_localAnchorA);
m_localAnchorB.set(revolute.m_localAnchorB);
m_referenceAngleB = revolute.m_referenceAngle;
m_localAxisD.setZero();
coordinateB = aB - aD - m_referenceAngleB;
}
else
{
Vec2 pB = pool.popVec2();
Vec2 temp = pool.popVec2();
PrismaticJoint prismatic = (PrismaticJoint) def.joint2;
m_localAnchorD.set(prismatic.m_localAnchorA);
m_localAnchorB.set(prismatic.m_localAnchorB);
m_referenceAngleB = prismatic.m_referenceAngle;
m_localAxisD.set(prismatic.m_localXAxisA);
Vec2 pD = m_localAnchorD;
Rot.mulToOutUnsafe(xfB.q, m_localAnchorB, ref temp);
temp.addLocal(xfB.p);
temp.subLocal(xfD.p);
Rot.mulTransUnsafe(xfD.q, temp, ref pB);
pB.subLocal(pD);
coordinateB = Vec2.dot(pB, m_localAxisD);
pool.pushVec2(2);
}
m_ratio = def.ratio;
m_constant = coordinateA + m_ratio*coordinateB;
m_impulse = 0.0f;
}
public virtual void processJoint(Joint joint, long tag)
{
}
public void add(Joint joint)
{
Debug.Assert(m_jointCount < m_jointCapacity);
m_joints[m_jointCount++] = joint;
}
public void sayGoodbye(Joint joint)
{
if (mouseJoint == joint)
{
mouseJoint = null;
}
else
{
jointDestroyed(joint);
}
}
public override long getTag(Joint joint)
{
if (joint == m_joint)
return JOINT_TAG;
return base.getTag(joint);
}
public override void processJoint(Joint joint, long tag)
{
if (tag == SPRING1_TAG)
{
m_spring1 = (WheelJoint) joint;
}
else if (tag == SPRING2_TAG)
{
m_spring2 = (WheelJoint) joint;
}
else
{
base.processJoint(joint, tag);
}
}
public override long getTag(Joint joint)
{
if (joint == m_spring1)
{
return SPRING1_TAG;
}
if (joint == m_spring2)
{
return SPRING2_TAG;
}
return base.getTag(joint);
}
public override void keyPressed(char keyChar, int keyCode)
{
switch (keyChar)
{
case 'j':
if (m_rope != null)
{
getWorld().destroyJoint(m_rope);
m_rope = null;
}
else
{
m_rope = getWorld().createJoint(m_ropeDef);
}
break;
}
}
