예제 #1
0
 internal static global::System.Runtime.InteropServices.HandleRef getCPtr(b2GearJointDef obj)
 {
     return((obj == null) ? new global::System.Runtime.InteropServices.HandleRef(null, global::System.IntPtr.Zero) : obj.swigCPtr);
 }
예제 #2
0
        public Gears()
        {
            b2Body ground = null;
            {
                b2BodyDef bd = new b2BodyDef();
                ground = m_world.CreateBody(bd);

                b2EdgeShape shape = new b2EdgeShape();
                shape.Set(new b2Vec2(50.0f, 0.0f), new b2Vec2(-50.0f, 0.0f));
                ground.CreateFixture(shape, 0.0f);
            }

            // Gears co
            {
                b2CircleShape circle1 = new b2CircleShape();
                circle1.Radius = 1.0f;

                b2PolygonShape box = new b2PolygonShape();
                box.SetAsBox(0.5f, 5.0f);

                b2CircleShape circle2 = new b2CircleShape();
                circle2.Radius = 2.0f;

                b2BodyDef bd1 = new b2BodyDef();
                bd1.type = b2BodyType.b2_staticBody;
                bd1.position.Set(10.0f, 9.0f);
                b2Body body1 = m_world.CreateBody(bd1);
                body1.CreateFixture(circle1, 0.0f);

                b2BodyDef bd2 = new b2BodyDef();
                bd2.type = b2BodyType.b2_dynamicBody;
                bd2.position.Set(10.0f, 8.0f);
                b2Body body2 = m_world.CreateBody(bd2);
                body2.CreateFixture(box, 5.0f);

                b2BodyDef bd3 = new b2BodyDef();
                bd3.type = b2BodyType.b2_dynamicBody;
                bd3.position.Set(10.0f, 6.0f);
                b2Body body3 = m_world.CreateBody(bd3);
                body3.CreateFixture(circle2, 5.0f);

                b2RevoluteJointDef jd1 = new b2RevoluteJointDef();
                jd1.Initialize(body2, body1, bd1.position);
                b2Joint joint1 = m_world.CreateJoint(jd1);

                b2RevoluteJointDef jd2 = new b2RevoluteJointDef();
                jd2.Initialize(body2, body3, bd3.position);
                b2Joint joint2 = m_world.CreateJoint(jd2);

                b2GearJointDef jd4 = new b2GearJointDef();
                jd4.BodyA  = body1;
                jd4.BodyB  = body3;
                jd4.joint1 = joint1;
                jd4.joint2 = joint2;
                jd4.ratio  = circle2.Radius / circle1.Radius;
                m_world.CreateJoint(jd4);
            }

            {
                b2CircleShape circle1 = new b2CircleShape();
                circle1.Radius = 1.0f;

                b2CircleShape circle2 = new b2CircleShape();
                circle2.Radius = 2.0f;

                b2PolygonShape box = new b2PolygonShape();
                box.SetAsBox(0.5f, 5.0f);

                b2BodyDef bd1 = new b2BodyDef();
                bd1.type = b2BodyType.b2_dynamicBody;
                bd1.position.Set(-3.0f, 12.0f);
                b2Body body1 = m_world.CreateBody(bd1);
                body1.CreateFixture(circle1, 5.0f);

                b2RevoluteJointDef jd1 = new b2RevoluteJointDef();
                jd1.BodyA          = ground;
                jd1.BodyB          = body1;
                jd1.localAnchorA   = ground.GetLocalPoint(bd1.position);
                jd1.localAnchorB   = body1.GetLocalPoint(bd1.position);
                jd1.referenceAngle = body1.Angle - ground.Angle;
                m_joint1           = (b2RevoluteJoint)m_world.CreateJoint(jd1);

                b2BodyDef bd2 = new b2BodyDef();
                bd2.type = b2BodyType.b2_dynamicBody;
                bd2.position.Set(0.0f, 12.0f);
                b2Body body2 = m_world.CreateBody(bd2);
                body2.CreateFixture(circle2, 5.0f);

                b2RevoluteJointDef jd2 = new b2RevoluteJointDef();
                jd2.Initialize(ground, body2, bd2.position);
                m_joint2 = (b2RevoluteJoint)m_world.CreateJoint(jd2);

                b2BodyDef bd3 = new b2BodyDef();
                bd3.type = b2BodyType.b2_dynamicBody;
                bd3.position.Set(2.5f, 12.0f);
                b2Body body3 = m_world.CreateBody(bd3);
                body3.CreateFixture(box, 5.0f);

                b2PrismaticJointDef jd3 = new b2PrismaticJointDef();
                jd3.Initialize(ground, body3, bd3.position, new b2Vec2(0.0f, 1.0f));
                jd3.lowerTranslation = -5.0f;
                jd3.upperTranslation = 5.0f;
                jd3.enableLimit      = true;

                m_joint3 = (b2PrismaticJoint)m_world.CreateJoint(jd3);

                b2GearJointDef jd4 = new b2GearJointDef();
                jd4.BodyA  = body1;
                jd4.BodyB  = body2;
                jd4.joint1 = m_joint1;
                jd4.joint2 = m_joint2;
                jd4.ratio  = circle2.Radius / circle1.Radius;
                m_joint4   = (b2GearJoint)m_world.CreateJoint(jd4);

                b2GearJointDef jd5 = new b2GearJointDef();
                jd5.BodyA  = body2;
                jd5.BodyB  = body3;
                jd5.joint1 = m_joint2;
                jd5.joint2 = m_joint3;
                jd5.ratio  = -1.0f / circle2.Radius;
                m_joint5   = (b2GearJoint)m_world.CreateJoint(jd5);
            }
        }
예제 #3
0
    //	friend class b2Joint;

    // Gear Joint:
    // C0 = (coordinate1 + ratio * coordinate2)_initial
    // C = (coordinate1 + ratio * coordinate2) - C0 = 0
    // J = [J1 ratio * J2]
    // K = J * invM * JT
    //   = J1 * invM1 * J1T + ratio * ratio * J2 * invM2 * J2T
    //
    // Revolute:
    // coordinate = rotation
    // Cdot = angularVelocity
    // J = [0 0 1]
    // K = J * invM * JT = invI
    //
    // Prismatic:
    // coordinate = dot(p - pg, ug)
    // Cdot = dot(v + cross(w, r), ug)
    // J = [ug cross(r, ug)]
    // K = J * invM * JT = invMass + invI * cross(r, ug)^2

    internal b2GearJoint(b2GearJointDef def) : base(def)
    {
        m_joint1 = def.joint1;
        m_joint2 = def.joint2;

        m_typeA = m_joint1.GetType();
        m_typeB = m_joint2.GetType();

        Debug.Assert(m_typeA == b2JointType.e_revoluteJoint || m_typeA == b2JointType.e_prismaticJoint);
        Debug.Assert(m_typeB == b2JointType.e_revoluteJoint || m_typeB == b2JointType.e_prismaticJoint);

        float coordinateA;
        float coordinateB;

        // TODO_ERIN there might be some problem with the joint edges in b2Joint.

        m_bodyC = m_joint1.GetBodyA();
        m_bodyA = m_joint1.GetBodyB();

        // Get geometry of joint1
        b2Transform xfA = new b2Transform(m_bodyA.m_xf);
        float       aA  = m_bodyA.m_sweep.a;
        b2Transform xfC = new b2Transform(m_bodyC.m_xf);
        float       aC  = m_bodyC.m_sweep.a;

        if (m_typeA == b2JointType.e_revoluteJoint)
        {
            b2RevoluteJoint revolute = (b2RevoluteJoint)def.joint1;
            m_localAnchorC    = revolute.m_localAnchorA;
            m_localAnchorA    = revolute.m_localAnchorB;
            m_referenceAngleA = revolute.m_referenceAngle;
            m_localAxisC.SetZero();

            coordinateA = aA - aC - m_referenceAngleA;
        }
        else
        {
            b2PrismaticJoint prismatic = (b2PrismaticJoint)def.joint1;


            m_localAnchorC = prismatic.m_localAnchorA;


            m_localAnchorA    = prismatic.m_localAnchorB;
            m_referenceAngleA = prismatic.m_referenceAngle;


            m_localAxisC = prismatic.m_localXAxisA;



            b2Vec2 pC = new b2Vec2(m_localAnchorC);
            b2Vec2 pA = Utils.b2MulT(xfC.q, Utils.b2Mul(xfA.q, m_localAnchorA) + (xfA.p - xfC.p));
            coordinateA = Utils.b2Dot(pA - pC, m_localAxisC);
        }

        m_bodyD = m_joint2.GetBodyA();
        m_bodyB = m_joint2.GetBodyB();

        // Get geometry of joint2


        b2Transform xfB = new b2Transform(m_bodyB.m_xf);
        float       aB  = m_bodyB.m_sweep.a;


        b2Transform xfD = new b2Transform(m_bodyD.m_xf);
        float       aD  = m_bodyD.m_sweep.a;

        if (m_typeB == b2JointType.e_revoluteJoint)
        {
            b2RevoluteJoint revolute = (b2RevoluteJoint)def.joint2;


            m_localAnchorD = revolute.m_localAnchorA;


            m_localAnchorB    = revolute.m_localAnchorB;
            m_referenceAngleB = revolute.m_referenceAngle;
            m_localAxisD.SetZero();

            coordinateB = aB - aD - m_referenceAngleB;
        }
        else
        {
            b2PrismaticJoint prismatic = (b2PrismaticJoint)def.joint2;


            m_localAnchorD = prismatic.m_localAnchorA;


            m_localAnchorB    = prismatic.m_localAnchorB;
            m_referenceAngleB = prismatic.m_referenceAngle;


            m_localAxisD = prismatic.m_localXAxisA;



            b2Vec2 pD = new b2Vec2(m_localAnchorD);
            b2Vec2 pB = Utils.b2MulT(xfD.q, Utils.b2Mul(xfB.q, m_localAnchorB) + (xfB.p - xfD.p));
            coordinateB = Utils.b2Dot(pB - pD, m_localAxisD);
        }

        m_ratio = def.ratio;

        m_constant = coordinateA + m_ratio * coordinateB;

        m_impulse = 0.0f;
    }