Beispiel #1
0
        public GearJoint(GearJointDef def) : base(def)
        {
            _jointA = def.JointA;
            _jointB = def.JointB;

            _typeA = _jointA.JointType;
            _typeB = _jointB.JointType;

            Debug.Assert(_typeA == JointType.Revolute || _typeA == JointType.Prismatic);
            Debug.Assert(_typeB == JointType.Revolute || _typeB == JointType.Prismatic);

            float coordinateA, coordinateB;

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

            _bodyC = _jointA.BodyA;
            _bodyA = _jointA.BodyB;

            // Body B on joint1 must be dynamic
            Debug.Assert(_bodyA._type == BodyType.Dynamic);

            // Get geometry of joint1
            Transform xfA = _bodyA._xf;
            float     aA  = _bodyA._sweep.A;
            Transform xfC = _bodyC._xf;
            float     aC  = _bodyC._sweep.A;

            if (_typeA == JointType.Revolute)
            {
                RevoluteJoint revolute = (RevoluteJoint)def.JointA;
                _localAnchorC    = revolute._localAnchorA;
                _localAnchorA    = revolute._localAnchorB;
                _referenceAngleA = revolute._referenceAngle;
                _localAxisC      = Vector2.Zero;

                coordinateA = aA - aC - _referenceAngleA;
            }
            else
            {
                PrismaticJoint prismatic = (PrismaticJoint)def.JointA;
                _localAnchorC    = prismatic._localAnchorA;
                _localAnchorA    = prismatic._localAnchorB;
                _referenceAngleA = prismatic._referenceAngle;
                _localAxisC      = prismatic._localXAxisA;

                Vector2 pC = _localAnchorC;
                Vector2 pA = MathUtils.MulT(xfC.q, MathUtils.Mul(xfA.q, _localAnchorA) + (xfA.p - xfC.p));
                coordinateA = MathUtils.Dot(pA - pC, _localAxisC);
            }

            _bodyD = _jointB.BodyA;
            _bodyB = _jointB.BodyB;

            // Body B on joint2 must be dynamic
            Debug.Assert(_bodyB._type == BodyType.Dynamic);

            // Get geometry of joint2
            Transform xfB = _bodyB._xf;
            float     aB  = _bodyB._sweep.A;
            Transform xfD = _bodyD._xf;
            float     aD  = _bodyD._sweep.A;

            if (_typeB == JointType.Revolute)
            {
                RevoluteJoint revolute = (RevoluteJoint)def.JointB;
                _localAnchorD    = revolute._localAnchorA;
                _localAnchorB    = revolute._localAnchorB;
                _referenceAngleB = revolute._referenceAngle;
                _localAxisD      = Vector2.Zero;

                coordinateB = aB - aD - _referenceAngleB;
            }
            else
            {
                PrismaticJoint prismatic = (PrismaticJoint)def.JointB;
                _localAnchorD    = prismatic._localAnchorA;
                _localAnchorB    = prismatic._localAnchorB;
                _referenceAngleB = prismatic._referenceAngle;
                _localAxisD      = prismatic._localXAxisA;

                Vector2 pD = _localAnchorD;
                Vector2 pB = MathUtils.MulT(xfD.q, MathUtils.Mul(xfB.q, _localAnchorB) + (xfB.p - xfD.p));
                coordinateB = MathUtils.Dot(pB - pD, _localAxisD);
            }

            _ratio = def.Ratio;

            _constant = coordinateA + _ratio * coordinateB;

            _impulse = 0.0f;
        }
Beispiel #2
0
        /// <summary>
        /// Requires two existing revolute or prismatic joints (any combination will work). The provided joints must
        /// attach a dynamic body to a static body.
        /// </summary>
        /// <param name="jointA">The first joint.</param>
        /// <param name="jointB">The second joint.</param>
        /// <param name="ratio">The ratio.</param>
        /// <param name="bodyA">The first body</param>
        /// <param name="bodyB">The second body</param>
        public GearJoint(Body bodyA, Body bodyB, Joint jointA, Joint jointB, float ratio = 1f)
        {
            JointType = JointType.Gear;
            BodyA     = bodyA;
            BodyB     = bodyB;
            JointA    = jointA;
            JointB    = jointB;
            Ratio     = ratio;

            _typeA = jointA.JointType;
            _typeB = jointB.JointType;

            Debug.Assert(_typeA == JointType.Revolute || _typeA == JointType.Prismatic || _typeA == JointType.FixedRevolute || _typeA == JointType.FixedPrismatic);
            Debug.Assert(_typeB == JointType.Revolute || _typeB == JointType.Prismatic || _typeB == JointType.FixedRevolute || _typeB == JointType.FixedPrismatic);

            float coordinateA, coordinateB;

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

            _bodyC = JointA.BodyA;
            _bodyA = JointA.BodyB;

            // Get geometry of joint1
            Transform xfA = _bodyA._xf;
            float     aA  = _bodyA._sweep.A;
            Transform xfC = _bodyC._xf;
            float     aC  = _bodyC._sweep.A;

            if (_typeA == JointType.Revolute)
            {
                RevoluteJoint revolute = (RevoluteJoint)jointA;
                _localAnchorC    = revolute.LocalAnchorA;
                _localAnchorA    = revolute.LocalAnchorB;
                _referenceAngleA = revolute.ReferenceAngle;
                _localAxisC      = Vector2.Zero;

                coordinateA = aA - aC - _referenceAngleA;
            }
            else
            {
                PrismaticJoint prismatic = (PrismaticJoint)jointA;
                _localAnchorC    = prismatic.LocalAnchorA;
                _localAnchorA    = prismatic.LocalAnchorB;
                _referenceAngleA = prismatic.ReferenceAngle;
                _localAxisC      = prismatic.LocalXAxis;

                Vector2 pC = _localAnchorC;
                Vector2 pA = MathUtils.MulT(xfC.q, MathUtils.Mul(xfA.q, _localAnchorA) + (xfA.p - xfC.p));
                coordinateA = Vector2.Dot(pA - pC, _localAxisC);
            }

            _bodyD = JointB.BodyA;
            _bodyB = JointB.BodyB;

            // Get geometry of joint2
            Transform xfB = _bodyB._xf;
            float     aB  = _bodyB._sweep.A;
            Transform xfD = _bodyD._xf;
            float     aD  = _bodyD._sweep.A;

            if (_typeB == JointType.Revolute)
            {
                RevoluteJoint revolute = (RevoluteJoint)jointB;
                _localAnchorD    = revolute.LocalAnchorA;
                _localAnchorB    = revolute.LocalAnchorB;
                _referenceAngleB = revolute.ReferenceAngle;
                _localAxisD      = Vector2.Zero;

                coordinateB = aB - aD - _referenceAngleB;
            }
            else
            {
                PrismaticJoint prismatic = (PrismaticJoint)jointB;
                _localAnchorD    = prismatic.LocalAnchorA;
                _localAnchorB    = prismatic.LocalAnchorB;
                _referenceAngleB = prismatic.ReferenceAngle;
                _localAxisD      = prismatic.LocalXAxis;

                Vector2 pD = _localAnchorD;
                Vector2 pB = MathUtils.MulT(xfD.q, MathUtils.Mul(xfB.q, _localAnchorB) + (xfB.p - xfD.p));
                coordinateB = Vector2.Dot(pB - pD, _localAxisD);
            }

            _ratio    = ratio;
            _constant = coordinateA + _ratio * coordinateB;
            _impulse  = 0.0f;
        }