// 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; } }