Пример #1
0
        /**
           * 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;
                }
            }
        }
Пример #2
0
        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;
        }
Пример #3
0
        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);
        }