Esempio n. 1
0
        /// Get the current joint translation speed, usually in meters per second.
        public float GetJointSpeed()
        {
            Body      b1 = _bodyA;
            Body      b2 = _bodyB;
            Transform xf1, xf2;

            b1.GetTransform(out xf1);
            b2.GetTransform(out xf2);

            Vector2 r1   = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
            Vector2 r2   = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());
            Vector2 p1   = b1._sweep.c + r1;
            Vector2 p2   = b2._sweep.c + r2;
            Vector2 d    = p2 - p1;
            Vector2 axis = b1.GetWorldVector(_localxAxis1);

            Vector2 v1 = b1._linearVelocity;
            Vector2 v2 = b2._linearVelocity;
            float   w1 = b1._angularVelocity;
            float   w2 = b2._angularVelocity;

            float speed = Vector2.Dot(d, MathUtils.Cross(w1, axis)) + Vector2.Dot(axis, v2 + MathUtils.Cross(w2, r2) - v1 - MathUtils.Cross(w1, r1));

            return(speed);
        }
Esempio n. 2
0
        internal override void SolveVelocityConstraints(ref TimeStep step)
        {
            Body b = _bodyB;

            Transform xf1;

            b.GetTransform(out xf1);

            Vector2 r = MathUtils.Multiply(ref xf1.R, _localAnchor - b.GetLocalCenter());

            // Cdot = v + cross(w, r)
            Vector2 Cdot    = b._linearVelocity + MathUtils.Cross(b._angularVelocity, r);
            Vector2 impulse = MathUtils.Multiply(ref _mass, -(Cdot + _beta * _C + _gamma * _impulse));

            Vector2 oldImpulse = _impulse;

            _impulse += impulse;
            float maxImpulse = step.dt * _maxForce;

            if (_impulse.sqrMagnitude > maxImpulse * maxImpulse)
            {
                _impulse *= maxImpulse / _impulse.magnitude;
            }
            impulse = _impulse - oldImpulse;

            b._linearVelocity  += b._invMass * impulse;
            b._angularVelocity += b._invI * MathUtils.Cross(r, impulse);
        }
Esempio n. 3
0
        internal override bool SolvePositionConstraints(float baumgarte)
        {
            Body bA = _bodyA;
            Body bB = _bodyB;

            float mA = bA._invMass, mB = bB._invMass;
            float iA = bA._invI, iB = bB._invI;

            Transform xfA;
            Transform xfB;

            bA.GetTransform(out xfA);
            bB.GetTransform(out xfB);

            Vector2 rA = MathUtils.Multiply(ref xfA.R, _localAnchorA - bA.GetLocalCenter());
            Vector2 rB = MathUtils.Multiply(ref xfB.R, _localAnchorB - bB.GetLocalCenter());

            Vector2 C1 = bB._sweep.c + rB - bA._sweep.c - rA;
            float   C2 = bB._sweep.a - bA._sweep.a - _referenceAngle;

            // Handle large detachment.
            const float k_allowedStretch = 10.0f * Settings.b2_linearSlop;
            float       positionError    = C1.magnitude;
            float       angularError     = Math.Abs(C2);

            if (positionError > k_allowedStretch)
            {
                iA *= 1.0f;
                iB *= 1.0f;
            }

            _mass.col1.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;
            _mass.col2.x = -rA.y * rA.x * iA - rB.y * rB.x * iB;
            _mass.col3.x = -rA.y * iA - rB.y * iB;
            _mass.col1.y = _mass.col2.x;
            _mass.col2.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;
            _mass.col3.y = rA.x * iA + rB.x * iB;
            _mass.col1.z = _mass.col3.x;
            _mass.col2.z = _mass.col3.y;
            _mass.col3.z = iA + iB;

            Vector3 C = new Vector3(C1.x, C1.y, C2);

            Vector3 impulse = _mass.Solve33(-C);

            Vector2 P = new Vector2(impulse.x, impulse.y);

            bA._sweep.c -= mA * P;
            bA._sweep.a -= iA * (MathUtils.Cross(rA, P) + impulse.z);

            bB._sweep.c += mB * P;
            bB._sweep.a += iB * (MathUtils.Cross(rB, P) + impulse.z);

            bA.SynchronizeTransform();
            bB.SynchronizeTransform();

            return(positionError <= Settings.b2_linearSlop && angularError <= Settings.b2_angularSlop);
        }
Esempio n. 4
0
        internal override void InitVelocityConstraints(ref TimeStep step)
        {
            Body bA = _bodyA;
            Body bB = _bodyB;

            Transform xfA, xfB;

            bA.GetTransform(out xfA);
            bB.GetTransform(out xfB);

            // Compute the effective mass matrix.
            Vector2 rA = MathUtils.Multiply(ref xfA.R, _localAnchorA - bA.GetLocalCenter());
            Vector2 rB = MathUtils.Multiply(ref xfB.R, _localAnchorB - bB.GetLocalCenter());

            // J = [-I -r1_skew I r2_skew]
            //     [ 0       -1 0       1]
            // r_skew = [-ry; rx]

            // Matlab
            // K = [ mA+r1y^2*iA+mB+r2y^2*iB,  -r1y*iA*r1x-r2y*iB*r2x,          -r1y*iA-r2y*iB]
            //     [  -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB,           r1x*iA+r2x*iB]
            //     [          -r1y*iA-r2y*iB,           r1x*iA+r2x*iB,                   iA+iB]

            float mA = bA._invMass, mB = bB._invMass;
            float iA = bA._invI, iB = bB._invI;

            _mass.col1.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;
            _mass.col2.x = -rA.y * rA.x * iA - rB.y * rB.x * iB;
            _mass.col3.x = -rA.y * iA - rB.y * iB;
            _mass.col1.y = _mass.col2.x;
            _mass.col2.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;
            _mass.col3.y = rA.x * iA + rB.x * iB;
            _mass.col1.z = _mass.col3.x;
            _mass.col2.z = _mass.col3.y;
            _mass.col3.z = iA + iB;

            if (step.warmStarting)
            {
                // Scale impulses to support a variable time step.
                _impulse *= step.dtRatio;

                Vector2 P = new Vector2(_impulse.x, _impulse.y);

                bA._linearVelocity  -= mA * P;
                bA._angularVelocity -= iA * (MathUtils.Cross(rA, P) + _impulse.z);

                bB._linearVelocity  += mB * P;
                bB._angularVelocity += iB * (MathUtils.Cross(rB, P) + _impulse.z);
            }
            else
            {
                _impulse = Vector3.zero;
            }
        }
Esempio n. 5
0
        internal override bool SolvePositionConstraints(float baumgarte)
        {
            if (_frequencyHz > 0.0f)
            {
                // There is no position correction for soft distance constraints.
                return(true);
            }

            Body b1 = _bodyA;
            Body b2 = _bodyB;

            Transform xf1, xf2;

            b1.GetTransform(out xf1);
            b2.GetTransform(out xf2);

            Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
            Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());

            Vector2 d = b2._sweep.c + r2 - b1._sweep.c - r1;

            float length = d.magnitude;

            if (length < _length)
            {
                return(true);
            }

            if (length == 0.0f)
            {
                return(true);
            }

            d /= length;
            float C = length - _length;

            C = MathUtils.Clamp(C, -Settings.b2_maxLinearCorrection, Settings.b2_maxLinearCorrection);

            float impulse = -_mass * C;

            _u = d;
            Vector2 P = impulse * _u;

            b1._sweep.c -= b1._invMass * P;
            b1._sweep.a -= b1._invI * MathUtils.Cross(r1, P);
            b2._sweep.c += b2._invMass * P;
            b2._sweep.a += b2._invI * MathUtils.Cross(r2, P);

            b1.SynchronizeTransform();
            b2.SynchronizeTransform();

            return(Math.Abs(C) < Settings.b2_linearSlop);
        }
Esempio n. 6
0
        /// Get the world manifold.
        public void GetWorldManifold(out WorldManifold worldManifold)
        {
            Body  bodyA  = _fixtureA.GetBody();
            Body  bodyB  = _fixtureB.GetBody();
            Shape shapeA = _fixtureA.GetShape();
            Shape shapeB = _fixtureB.GetShape();

            Transform xfA, xfB;

            bodyA.GetTransform(out xfA);
            bodyB.GetTransform(out xfB);

            worldManifold = new WorldManifold(ref _manifold, ref xfA, shapeA._radius, ref xfB, shapeB._radius);
        }
Esempio n. 7
0
        internal override void SolveVelocityConstraints(ref TimeStep step)
        {
            Body bA = _bodyA;
            Body bB = _bodyB;

            Vector2 vA = bA._linearVelocity;
            float   wA = bA._angularVelocity;
            Vector2 vB = bB._linearVelocity;
            float   wB = bB._angularVelocity;

            float mA = bA._invMass, mB = bB._invMass;
            float iA = bA._invI, iB = bB._invI;

            Transform xfA, xfB;

            bA.GetTransform(out xfA);
            bB.GetTransform(out xfB);

            Vector2 rA = MathUtils.Multiply(ref xfA.R, _localAnchorA - bA.GetLocalCenter());
            Vector2 rB = MathUtils.Multiply(ref xfB.R, _localAnchorB - bB.GetLocalCenter());

            //  Solve point-to-point constraint
            Vector2 Cdot1 = vB + MathUtils.Cross(wB, rB) - vA - MathUtils.Cross(wA, rA);
            float   Cdot2 = wB - wA;
            Vector3 Cdot  = new Vector3(Cdot1.x, Cdot1.y, Cdot2);

            Vector3 impulse = _mass.Solve33(-Cdot);

            _impulse += impulse;

            Vector2 P = new Vector2(impulse.x, impulse.y);

            vA -= mA * P;
            wA -= iA * (MathUtils.Cross(rA, P) + impulse.z);

            vB += mB * P;
            wB += iB * (MathUtils.Cross(rB, P) + impulse.z);

            bA._linearVelocity  = vA;
            bA._angularVelocity = wA;
            bB._linearVelocity  = vB;
            bB._angularVelocity = wB;
        }
Esempio n. 8
0
        void DrawJoint(Joint joint)
        {
            Body      b1 = joint.GetBodyA();
            Body      b2 = joint.GetBodyB();
            Transform xf1, xf2;

            b1.GetTransform(out xf1);
            b2.GetTransform(out xf2);
            Vector2 x1 = xf1.Position;
            Vector2 x2 = xf2.Position;
            Vector2 p1 = joint.GetAnchorA();
            Vector2 p2 = joint.GetAnchorB();

            Color color = new Color(0.5f, 0.8f, 0.8f);

            switch (joint.JointType)
            {
            case JointType.Distance:
                DebugDraw.DrawSegment(p1, p2, color);
                break;

            case JointType.Pulley:
            {
                PulleyJoint pulley = (PulleyJoint)joint;
                Vector2     s1     = pulley.GetGroundAnchorA();
                Vector2     s2     = pulley.GetGroundAnchorB();
                DebugDraw.DrawSegment(s1, p1, color);
                DebugDraw.DrawSegment(s2, p2, color);
                DebugDraw.DrawSegment(s1, s2, color);
            }
            break;

            case JointType.Mouse:
                // don't draw this
                break;

            default:
                DebugDraw.DrawSegment(x1, p1, color);
                DebugDraw.DrawSegment(p1, p2, color);
                DebugDraw.DrawSegment(x2, p2, color);
                break;
            }
        }
Esempio n. 9
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    = joint.GetAnchorA();
            Vec2      p2    = joint.GetAnchorB();

            Color color = Color.FromArgb(128, 200, 200);

            switch (joint.GetJointType())
            {
            case JointType.e_distanceJoint:
                m_debugDraw.DrawSegment(p1, p2, color);
                break;

            case JointType.e_pulleyJoint:
            {
                throw new NotImplementedException();
                //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.e_mouseJoint:
                // 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;
            }
        }
Esempio n. 10
0
        internal override void SolveVelocityConstraints(ref TimeStep step)
        {
            Body b1 = _bodyA;
            Body b2 = _bodyB;

            Transform xf1, xf2;

            b1.GetTransform(out xf1);
            b2.GetTransform(out xf2);

            Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
            Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());

            Vector2 d = b2._sweep.c + r2 - b1._sweep.c - r1;

            float length = d.magnitude;

            if (length < _length)
            {
                return;
            }

            // Cdot = dot(u, v + cross(w, r))
            Vector2 v1   = b1._linearVelocity + MathUtils.Cross(b1._angularVelocity, r1);
            Vector2 v2   = b2._linearVelocity + MathUtils.Cross(b2._angularVelocity, r2);
            float   Cdot = Vector2.Dot(_u, v2 - v1);

            float impulse = -_mass * (Cdot + _bias + _gamma * _impulse);

            _impulse += impulse;

            Vector2 P = impulse * _u;

            b1._linearVelocity  -= b1._invMass * P;
            b1._angularVelocity -= b1._invI * MathUtils.Cross(r1, P);
            b2._linearVelocity  += b2._invMass * P;
            b2._angularVelocity += b2._invI * MathUtils.Cross(r2, P);
        }
Esempio n. 11
0
        internal override void SolveVelocityConstraints(ref TimeStep step)
        {
            Body b1 = _bodyA;
            Body b2 = _bodyB;

            Vector2 v1 = b1._linearVelocity;
            float   w1 = b1._angularVelocity;
            Vector2 v2 = b2._linearVelocity;
            float   w2 = b2._angularVelocity;

            float m1 = b1._invMass, m2 = b2._invMass;
            float i1 = b1._invI, i2 = b2._invI;

            // Solve motor constraint.
            if (_enableMotor && _limitState != LimitState.Equal)
            {
                float Cdot       = w2 - w1 - _motorSpeed;
                float impulse    = _motorMass * (-Cdot);
                float oldImpulse = _motorImpulse;
                float maxImpulse = step.dt * _maxMotorTorque;
                _motorImpulse = MathUtils.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
                impulse       = _motorImpulse - oldImpulse;

                w1 -= i1 * impulse;
                w2 += i2 * impulse;
            }

            // Solve limit constraint.
            if (_enableLimit && _limitState != LimitState.Inactive)
            {
                Transform xf1, xf2;
                b1.GetTransform(out xf1);
                b2.GetTransform(out xf2);

                Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
                Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());

                // Solve point-to-point constraint
                Vector2 Cdot1 = v2 + MathUtils.Cross(w2, r2) - v1 - MathUtils.Cross(w1, r1);
                float   Cdot2 = w2 - w1;
                Vector3 Cdot  = new Vector3(Cdot1.x, Cdot1.y, Cdot2);

                Vector3 impulse = _mass.Solve33(-Cdot);

                if (_limitState == LimitState.Equal)
                {
                    _impulse += impulse;
                }
                else if (_limitState == LimitState.AtLower)
                {
                    float newImpulse = _impulse.z + impulse.z;
                    if (newImpulse < 0.0f)
                    {
                        Vector2 reduced = _mass.Solve22(-Cdot1);
                        impulse.x   = reduced.x;
                        impulse.y   = reduced.y;
                        impulse.z   = -_impulse.z;
                        _impulse.x += reduced.x;
                        _impulse.y += reduced.y;
                        _impulse.z  = 0.0f;
                    }
                }
                else if (_limitState == LimitState.AtUpper)
                {
                    float newImpulse = _impulse.z + impulse.z;
                    if (newImpulse > 0.0f)
                    {
                        Vector2 reduced = _mass.Solve22(-Cdot1);
                        impulse.x   = reduced.x;
                        impulse.y   = reduced.y;
                        impulse.z   = -_impulse.z;
                        _impulse.x += reduced.x;
                        _impulse.y += reduced.y;
                        _impulse.z  = 0.0f;
                    }
                }

                Vector2 P = new Vector2(impulse.x, impulse.y);

                v1 -= m1 * P;
                w1 -= i1 * (MathUtils.Cross(r1, P) + impulse.z);

                v2 += m2 * P;
                w2 += i2 * (MathUtils.Cross(r2, P) + impulse.z);
            }
            else
            {
                Transform xf1, xf2;
                b1.GetTransform(out xf1);
                b2.GetTransform(out xf2);

                Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
                Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());

                // Solve point-to-point constraint
                Vector2 Cdot    = v2 + MathUtils.Cross(w2, r2) - v1 - MathUtils.Cross(w1, r1);
                Vector2 impulse = _mass.Solve22(-Cdot);

                _impulse.x += impulse.x;
                _impulse.y += impulse.y;

                v1 -= m1 * impulse;
                w1 -= i1 * MathUtils.Cross(r1, impulse);

                v2 += m2 * impulse;
                w2 += i2 * MathUtils.Cross(r2, impulse);
            }

            b1._linearVelocity  = v1;
            b1._angularVelocity = w1;
            b2._linearVelocity  = v2;
            b2._angularVelocity = w2;
        }
Esempio n. 12
0
        internal override void InitVelocityConstraints(ref TimeStep step)
        {
            Body b1 = _bodyA;
            Body b2 = _bodyB;

            if (_enableMotor || _enableLimit)
            {
                // you cannot create a rotation limit between bodies that
                // both have fixed rotation.
                //Debug.Assert(b1._invI > 0.0f || b2._invI > 0.0f);
            }

            // Compute the effective mass matrix.
            Transform xf1, xf2;

            b1.GetTransform(out xf1);
            b2.GetTransform(out xf2);

            Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
            Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());

            // J = [-I -r1_skew I r2_skew]
            //     [ 0       -1 0       1]
            // r_skew = [-ry; rx]

            // Matlab
            // K = [ m1+r1y^2*i1+m2+r2y^2*i2,  -r1y*i1*r1x-r2y*i2*r2x,          -r1y*i1-r2y*i2]
            //     [  -r1y*i1*r1x-r2y*i2*r2x, m1+r1x^2*i1+m2+r2x^2*i2,           r1x*i1+r2x*i2]
            //     [          -r1y*i1-r2y*i2,           r1x*i1+r2x*i2,                   i1+i2]

            float m1 = b1._invMass, m2 = b2._invMass;
            float i1 = b1._invI, i2 = b2._invI;

            _mass.col1.x = m1 + m2 + r1.y * r1.y * i1 + r2.y * r2.y * i2;
            _mass.col2.x = -r1.y * r1.x * i1 - r2.y * r2.x * i2;
            _mass.col3.x = -r1.y * i1 - r2.y * i2;
            _mass.col1.y = _mass.col2.x;
            _mass.col2.y = m1 + m2 + r1.x * r1.x * i1 + r2.x * r2.x * i2;
            _mass.col3.y = r1.x * i1 + r2.x * i2;
            _mass.col1.z = _mass.col3.x;
            _mass.col2.z = _mass.col3.y;
            _mass.col3.z = i1 + i2;

            _motorMass = i1 + i2;
            if (_motorMass > 0.0f)
            {
                _motorMass = 1.0f / _motorMass;
            }

            if (_enableMotor == false)
            {
                _motorImpulse = 0.0f;
            }

            if (_enableLimit)
            {
                float jointAngle = b2._sweep.a - b1._sweep.a - _referenceAngle;
                if (Math.Abs(_upperAngle - _lowerAngle) < 2.0f * Settings.b2_angularSlop)
                {
                    _limitState = LimitState.Equal;
                }
                else if (jointAngle <= _lowerAngle)
                {
                    if (_limitState != LimitState.AtLower)
                    {
                        _impulse.z = 0.0f;
                    }
                    _limitState = LimitState.AtLower;
                }
                else if (jointAngle >= _upperAngle)
                {
                    if (_limitState != LimitState.AtUpper)
                    {
                        _impulse.z = 0.0f;
                    }
                    _limitState = LimitState.AtUpper;
                }
                else
                {
                    _limitState = LimitState.Inactive;
                    _impulse.z  = 0.0f;
                }
            }
            else
            {
                _limitState = LimitState.Inactive;
            }

            if (step.warmStarting)
            {
                // Scale impulses to support a variable time step.
                _impulse      *= step.dtRatio;
                _motorImpulse *= step.dtRatio;

                Vector2 P = new Vector2(_impulse.x, _impulse.y);

                b1._linearVelocity  -= m1 * P;
                b1._angularVelocity -= i1 * (MathUtils.Cross(r1, P) + _motorImpulse + _impulse.z);

                b2._linearVelocity  += m2 * P;
                b2._angularVelocity += i2 * (MathUtils.Cross(r2, P) + _motorImpulse + _impulse.z);
            }
            else
            {
                _impulse      = Vector3.zero;
                _motorImpulse = 0.0f;
            }
        }
Esempio n. 13
0
        internal override bool SolvePositionConstraints(float baumgarte)
        {
            Body b1 = _bodyA;
            Body b2 = _bodyB;

            Vector2 s1 = _groundAnchor1;
            Vector2 s2 = _groundAnchor2;

            float linearError = 0.0f;

            if (_state == LimitState.AtUpper)
            {
                Transform xf1, xf2;
                b1.GetTransform(out xf1);
                b2.GetTransform(out xf2);

                Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
                Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());

                Vector2 p1 = b1._sweep.c + r1;
                Vector2 p2 = b2._sweep.c + r2;

                // Get the pulley axes.
                _u1 = p1 - s1;
                _u2 = p2 - s2;

                float length1 = _u1.magnitude;
                float length2 = _u2.magnitude;

                if (length1 > Settings.b2_linearSlop)
                {
                    _u1 *= 1.0f / length1;
                }
                else
                {
                    _u1 = Vector2.zero;
                }

                if (length2 > Settings.b2_linearSlop)
                {
                    _u2 *= 1.0f / length2;
                }
                else
                {
                    _u2 = Vector2.zero;
                }

                float C = _ant - length1 - _ratio * length2;
                linearError = Math.Max(linearError, -C);

                C = MathUtils.Clamp(C + Settings.b2_linearSlop, -Settings.b2_maxLinearCorrection, 0.0f);
                float impulse = -_pulleyMass * C;

                Vector2 P1 = -impulse * _u1;
                Vector2 P2 = -_ratio * impulse * _u2;

                b1._sweep.c += b1._invMass * P1;
                b1._sweep.a += b1._invI * MathUtils.Cross(r1, P1);
                b2._sweep.c += b2._invMass * P2;
                b2._sweep.a += b2._invI * MathUtils.Cross(r2, P2);

                b1.SynchronizeTransform();
                b2.SynchronizeTransform();
            }

            if (_limitState1 == LimitState.AtUpper)
            {
                Transform xf1;
                b1.GetTransform(out xf1);

                Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
                Vector2 p1 = b1._sweep.c + r1;

                _u1 = p1 - s1;
                float length1 = _u1.magnitude;

                if (length1 > Settings.b2_linearSlop)
                {
                    _u1 *= 1.0f / length1;
                }
                else
                {
                    _u1 = Vector2.zero;
                }

                float C = _maxLength1 - length1;
                linearError = Math.Max(linearError, -C);
                C           = MathUtils.Clamp(C + Settings.b2_linearSlop, -Settings.b2_maxLinearCorrection, 0.0f);
                float impulse = -_limitMass1 * C;

                Vector2 P1 = -impulse * _u1;
                b1._sweep.c += b1._invMass * P1;
                b1._sweep.a += b1._invI * MathUtils.Cross(r1, P1);

                b1.SynchronizeTransform();
            }

            if (_limitState2 == LimitState.AtUpper)
            {
                Transform xf2;
                b2.GetTransform(out xf2);

                Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());
                Vector2 p2 = b2._sweep.c + r2;

                _u2 = p2 - s2;
                float length2 = _u2.magnitude;

                if (length2 > Settings.b2_linearSlop)
                {
                    _u2 *= 1.0f / length2;
                }
                else
                {
                    _u2 = Vector2.zero;
                }

                float C = _maxLength2 - length2;
                linearError = Math.Max(linearError, -C);
                C           = MathUtils.Clamp(C + Settings.b2_linearSlop, -Settings.b2_maxLinearCorrection, 0.0f);
                float impulse = -_limitMass2 * C;

                Vector2 P2 = -impulse * _u2;
                b2._sweep.c += b2._invMass * P2;
                b2._sweep.a += b2._invI * MathUtils.Cross(r2, P2);

                b2.SynchronizeTransform();
            }

            return(linearError < Settings.b2_linearSlop);
        }
Esempio n. 14
0
        internal override void SolveVelocityConstraints(ref TimeStep step)
        {
            Body b1 = _bodyA;
            Body b2 = _bodyB;

            Transform xf1, xf2;

            b1.GetTransform(out xf1);
            b2.GetTransform(out xf2);

            Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
            Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());

            if (_state == LimitState.AtUpper)
            {
                Vector2 v1 = b1._linearVelocity + MathUtils.Cross(b1._angularVelocity, r1);
                Vector2 v2 = b2._linearVelocity + MathUtils.Cross(b2._angularVelocity, r2);

                float Cdot       = -Vector2.Dot(_u1, v1) - _ratio * Vector2.Dot(_u2, v2);
                float impulse    = _pulleyMass * (-Cdot);
                float oldImpulse = _impulse;
                _impulse = Math.Max(0.0f, _impulse + impulse);
                impulse  = _impulse - oldImpulse;

                Vector2 P1 = -impulse * _u1;
                Vector2 P2 = -_ratio * impulse * _u2;
                b1._linearVelocity  += b1._invMass * P1;
                b1._angularVelocity += b1._invI * MathUtils.Cross(r1, P1);
                b2._linearVelocity  += b2._invMass * P2;
                b2._angularVelocity += b2._invI * MathUtils.Cross(r2, P2);
            }

            if (_limitState1 == LimitState.AtUpper)
            {
                Vector2 v1 = b1._linearVelocity + MathUtils.Cross(b1._angularVelocity, r1);

                float Cdot       = -Vector2.Dot(_u1, v1);
                float impulse    = -_limitMass1 * Cdot;
                float oldImpulse = _limitImpulse1;
                _limitImpulse1 = Math.Max(0.0f, _limitImpulse1 + impulse);
                impulse        = _limitImpulse1 - oldImpulse;

                Vector2 P1 = -impulse * _u1;
                b1._linearVelocity  += b1._invMass * P1;
                b1._angularVelocity += b1._invI * MathUtils.Cross(r1, P1);
            }

            if (_limitState2 == LimitState.AtUpper)
            {
                Vector2 v2 = b2._linearVelocity + MathUtils.Cross(b2._angularVelocity, r2);

                float Cdot       = -Vector2.Dot(_u2, v2);
                float impulse    = -_limitMass2 * Cdot;
                float oldImpulse = _limitImpulse2;
                _limitImpulse2 = Math.Max(0.0f, _limitImpulse2 + impulse);
                impulse        = _limitImpulse2 - oldImpulse;

                Vector2 P2 = -impulse * _u2;
                b2._linearVelocity  += b2._invMass * P2;
                b2._angularVelocity += b2._invI * MathUtils.Cross(r2, P2);
            }
        }
Esempio n. 15
0
        internal override void InitVelocityConstraints(ref TimeStep step)
        {
            Body b1 = _bodyA;
            Body b2 = _bodyB;

            Transform xf1, xf2;

            b1.GetTransform(out xf1);
            b2.GetTransform(out xf2);

            Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
            Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());

            Vector2 p1 = b1._sweep.c + r1;
            Vector2 p2 = b2._sweep.c + r2;

            Vector2 s1 = _groundAnchor1;
            Vector2 s2 = _groundAnchor2;

            // Get the pulley axes.
            _u1 = p1 - s1;
            _u2 = p2 - s2;

            float length1 = _u1.magnitude;
            float length2 = _u2.magnitude;

            if (length1 > Settings.b2_linearSlop)
            {
                _u1 *= 1.0f / length1;
            }
            else
            {
                _u1 = Vector2.zero;
            }

            if (length2 > Settings.b2_linearSlop)
            {
                _u2 *= 1.0f / length2;
            }
            else
            {
                _u2 = Vector2.zero;
            }

            float C = _ant - length1 - _ratio * length2;

            if (C > 0.0f)
            {
                _state   = LimitState.Inactive;
                _impulse = 0.0f;
            }
            else
            {
                _state = LimitState.AtUpper;
            }

            if (length1 < _maxLength1)
            {
                _limitState1   = LimitState.Inactive;
                _limitImpulse1 = 0.0f;
            }
            else
            {
                _limitState1 = LimitState.AtUpper;
            }

            if (length2 < _maxLength2)
            {
                _limitState2   = LimitState.Inactive;
                _limitImpulse2 = 0.0f;
            }
            else
            {
                _limitState2 = LimitState.AtUpper;
            }

            // Compute effective mass.
            float cr1u1 = MathUtils.Cross(r1, _u1);
            float cr2u2 = MathUtils.Cross(r2, _u2);

            _limitMass1 = b1._invMass + b1._invI * cr1u1 * cr1u1;
            _limitMass2 = b2._invMass + b2._invI * cr2u2 * cr2u2;
            _pulleyMass = _limitMass1 + _ratio * _ratio * _limitMass2;
            //Debug.Assert(_limitMass1 > Settings.b2_epsilon);
            //Debug.Assert(_limitMass2 > Settings.b2_epsilon);
            //Debug.Assert(_pulleyMass > Settings.b2_epsilon);
            _limitMass1 = 1.0f / _limitMass1;
            _limitMass2 = 1.0f / _limitMass2;
            _pulleyMass = 1.0f / _pulleyMass;

            if (step.warmStarting)
            {
                // Scale impulses to support variable time steps.
                _impulse       *= step.dtRatio;
                _limitImpulse1 *= step.dtRatio;
                _limitImpulse2 *= step.dtRatio;

                // Warm starting.
                Vector2 P1 = -(_impulse + _limitImpulse1) * _u1;
                Vector2 P2 = (-_ratio * _impulse - _limitImpulse2) * _u2;
                b1._linearVelocity  += b1._invMass * P1;
                b1._angularVelocity += b1._invI * MathUtils.Cross(r1, P1);
                b2._linearVelocity  += b2._invMass * P2;
                b2._angularVelocity += b2._invI * MathUtils.Cross(r2, P2);
            }
            else
            {
                _impulse       = 0.0f;
                _limitImpulse1 = 0.0f;
                _limitImpulse2 = 0.0f;
            }
        }
Esempio n. 16
0
        // Update the contact manifold and touching status.
        // Note: do not assume the fixture AABBs are overlapping or are valid.
        internal void Update(ContactListener listener)
        {
            Manifold oldManifold = m_manifold;

            // Re-enable this contact.
            m_flags |= ContactFlags.e_enabledFlag;

            bool touching    = false;
            bool wasTouching = (m_flags & ContactFlags.e_touchingFlag) == ContactFlags.e_touchingFlag;

            bool sensorA = m_fixtureA.IsSensor;
            bool sensorB = m_fixtureB.IsSensor;
            bool sensor  = sensorA || sensorB;

            Body      bodyA = m_fixtureA.GetBody();
            Body      bodyB = m_fixtureB.GetBody();
            Transform xfA   = bodyA.GetTransform();
            Transform xfB   = bodyB.GetTransform();

            // Is this contact a sensor?
            if (sensor)
            {
                Shape shapeA = m_fixtureA.GetShape();
                Shape shapeB = m_fixtureB.GetShape();
                touching = Collision.TestOverlap(shapeA, m_indexA, shapeB, m_indexB, xfA, xfB);

                // Sensors don't generate manifolds.
                m_manifold.points.Clear();
            }
            else
            {
                Evaluate(out m_manifold, xfA, xfB);
                touching = m_manifold.points.Count() > 0;

                // Match old contact ids to new contact ids and copy the
                // stored impulses to warm start the solver.
                for (int i = 0; i < m_manifold.points.Count(); ++i)
                {
                    ManifoldPoint mp2 = m_manifold.points[i];
                    mp2.normalImpulse  = 0.0f;
                    mp2.tangentImpulse = 0.0f;
                    ContactID id2 = mp2.id;

                    for (int j = 0; j < oldManifold.points.Count(); ++j)
                    {
                        ManifoldPoint mp1 = oldManifold.points[j];

                        if (mp1.id.key == id2.key)
                        {
                            mp2.normalImpulse  = mp1.normalImpulse;
                            mp2.tangentImpulse = mp1.tangentImpulse;
                            break;
                        }
                    }
                }

                if (touching != wasTouching)
                {
                    bodyA.SetAwake(true);
                    bodyB.SetAwake(true);
                }
            }

            if (touching)
            {
                m_flags |= ContactFlags.e_touchingFlag;
            }
            else
            {
                m_flags &= ~ContactFlags.e_touchingFlag;
            }

            if (wasTouching == false && touching == true && listener != null)
            {
                listener.BeginContact(this);
            }

            if (wasTouching == true && touching == false && listener != null)
            {
                listener.EndContact(this);
            }

            if (sensor == false && touching && listener != null)
            {
                listener.PreSolve(this, oldManifold);
            }
        }
Esempio n. 17
0
        internal override void InitVelocityConstraints(ref TimeStep step)
        {
            Body b1 = _bodyA;
            Body b2 = _bodyB;

            Transform xf1, xf2;

            b1.GetTransform(out xf1);
            b2.GetTransform(out xf2);

            // Compute the effective mass matrix.
            Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
            Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());

            _u = b2._sweep.c + r2 - b1._sweep.c - r1;

            // Handle singularity.
            float length = _u.magnitude;

            if (length > Settings.b2_linearSlop)
            {
                _u *= 1.0f / length;
            }
            else
            {
                _u = new Vector2(0.0f, 0.0f);
            }

            float cr1u    = MathUtils.Cross(r1, _u);
            float cr2u    = MathUtils.Cross(r2, _u);
            float invMass = b1._invMass + b1._invI * cr1u * cr1u + b2._invMass + b2._invI * cr2u * cr2u;

            //Debug.Assert(invMass > Settings.b2_epsilon);
            _mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;

            if (_frequencyHz > 0.0f)
            {
                float C = length - _length;

                // Frequency
                float omega = 2.0f * Settings.b2_pi * _frequencyHz;

                // Damping coefficient
                float d = 2.0f * _mass * _dampingRatio * omega;

                // Spring stiffness
                float k = _mass * omega * omega;

                // magic formulas
                _gamma = step.dt * (d + step.dt * k);
                _gamma = _gamma != 0.0f ? 1.0f / _gamma : 0.0f;
                _bias  = C * step.dt * k * _gamma;

                _mass = invMass + _gamma;
                _mass = _mass != 0.0f ? 1.0f / _mass : 0.0f;
            }

            if (step.warmStarting)
            {
                // Scale the impulse to support a variable time step.
                _impulse *= step.dtRatio;

                Vector2 P = _impulse * _u;
                b1._linearVelocity  -= b1._invMass * P;
                b1._angularVelocity -= b1._invI * MathUtils.Cross(r1, P);
                b2._linearVelocity  += b2._invMass * P;
                b2._angularVelocity += b2._invI * MathUtils.Cross(r2, P);
            }
            else
            {
                _impulse = 0.0f;
            }
        }
Esempio n. 18
0
        // Update the contact manifold and touching status.
        // Note: do not assume the fixture AABBs are overlapping or are valid.
        internal void Update(IContactListener listener)
        {
            Manifold oldManifold = _manifold;

            // Re-enable this contact.
            _flags |= ContactFlags.Enabled;

            bool touching    = false;
            bool wasTouching = (_flags & ContactFlags.Touching) == ContactFlags.Touching;

            bool sensorA = _fixtureA.IsSensor();
            bool sensorB = _fixtureB.IsSensor();
            bool sensor  = sensorA || sensorB;

            Body      bodyA = _fixtureA.GetBody();
            Body      bodyB = _fixtureB.GetBody();
            Transform xfA; bodyA.GetTransform(out xfA);
            Transform xfB; bodyB.GetTransform(out xfB);

            // Is this contact a sensor?
            if (sensor)
            {
                Shape shapeA = _fixtureA.GetShape();
                Shape shapeB = _fixtureB.GetShape();
                touching = AABB.TestOverlap(shapeA, _indexA, shapeB, _indexB, ref xfA, ref xfB);

                // Sensors don't generate manifolds.
                _manifold._pointCount = 0;
            }
            else
            {
                Evaluate(ref _manifold, ref xfA, ref xfB);
                touching = _manifold._pointCount > 0;

                // Match old contact ids to new contact ids and copy the
                // stored impulses to warm start the solver.
                for (int i = 0; i < _manifold._pointCount; ++i)
                {
                    ManifoldPoint mp2 = _manifold._points[i];
                    mp2.NormalImpulse  = 0.0f;
                    mp2.TangentImpulse = 0.0f;
                    ContactID id2   = mp2.Id;
                    bool      found = false;

                    for (int j = 0; j < oldManifold._pointCount; ++j)
                    {
                        ManifoldPoint mp1 = oldManifold._points[j];

                        if (mp1.Id.Key == id2.Key)
                        {
                            mp2.NormalImpulse  = mp1.NormalImpulse;
                            mp2.TangentImpulse = mp1.TangentImpulse;
                            found = true;
                            break;
                        }
                    }
                    if (found == false)
                    {
                        mp2.NormalImpulse  = 0.0f;
                        mp2.TangentImpulse = 0.0f;
                    }

                    _manifold._points[i] = mp2;
                }

                if (touching != wasTouching)
                {
                    bodyA.SetAwake(true);
                    bodyB.SetAwake(true);
                }
            }

            if (touching)
            {
                _flags |= ContactFlags.Touching;
            }
            else
            {
                _flags &= ~ContactFlags.Touching;
            }

            if (wasTouching == false && touching == true && null != listener)
            {
                listener.BeginContact(this);
            }

            if (wasTouching == true && touching == false && null != listener)
            {
                listener.EndContact(this);
            }

            if (sensor == false && null != listener)
            {
                listener.PreSolve(this, ref oldManifold);
            }
        }
Esempio n. 19
0
        /// Call this to draw shapes and other debug draw data.
        public void DrawDebugData()
        {
            if (DebugDraw == null)
            {
                return;
            }

            DebugDrawFlags flags = DebugDraw.Flags;

            if ((flags & DebugDrawFlags.Shape) == DebugDrawFlags.Shape)
            {
                for (Body b = _bodyList; b != null; b = b.GetNext())
                {
                    Transform xf;
                    b.GetTransform(out xf);
                    for (Fixture f = b.GetFixtureList(); f != null; f = f.GetNext())
                    {
                        if (b.IsActive() == false)
                        {
                            DrawShape(f, xf, new Color(0.5f, 0.5f, 0.3f));
                        }
                        else if (b.GetType() == BodyType.Static)
                        {
                            DrawShape(f, xf, new Color(0.5f, 0.9f, 0.5f));
                        }
                        else if (b.GetType() == BodyType.Kinematic)
                        {
                            DrawShape(f, xf, new Color(0.5f, 0.5f, 0.9f));
                        }
                        else if (b.IsAwake() == false)
                        {
                            DrawShape(f, xf, new Color(0.6f, 0.6f, 0.6f));
                        }
                        else
                        {
                            DrawShape(f, xf, new Color(0.9f, 0.7f, 0.7f));
                        }
                    }
                }
            }

            if ((flags & DebugDrawFlags.Joint) == DebugDrawFlags.Joint)
            {
                for (Joint j = _jointList; j != null; j = j.GetNext())
                {
                    DrawJoint(j);
                }
            }

            if ((flags & DebugDrawFlags.Pair) == DebugDrawFlags.Pair)
            {
                Color color = new Color(0.3f, 0.9f, 0.9f);
                for (Contact c = _contactManager._contactList; c != null; c = c.GetNext())
                {
                    /*
                     * Fixture fixtureA = c.GetFixtureA();
                     * Fixture fixtureB = c.GetFixtureB();
                     *
                     * AABB aabbA;
                     * AABB aabbB;
                     * fixtureA.GetAABB(out aabbA);
                     * fixtureB.GetAABB(out aabbB);
                     *
                     * Vector2 cA = aabbA.GetCenter();
                     * Vector2 cB = aabbB.GetCenter();
                     *
                     * DebugDraw.DrawSegment(cA, cB, color);
                     */
                }
            }

            if ((flags & DebugDrawFlags.AABB) == DebugDrawFlags.AABB)
            {
                Color      color = new Color(0.9f, 0.3f, 0.9f);
                BroadPhase bp    = _contactManager._broadPhase;

                for (Body b = _bodyList; b != null; b = b.GetNext())
                {
                    if (b.IsActive() == false)
                    {
                        continue;
                    }

                    for (Fixture f = b.GetFixtureList(); f != null; f = f.GetNext())
                    {
                        for (int i = 0; i < f._proxyCount; ++i)
                        {
                            FixtureProxy proxy = f._proxies[i];
                            AABB         aabb;
                            bp.GetFatAABB(proxy.proxyId, out aabb);
                            FixedArray8 <Vector2> vs = new FixedArray8 <Vector2>();
                            vs[0] = new Vector2(aabb.lowerBound.x, aabb.lowerBound.y);
                            vs[1] = new Vector2(aabb.upperBound.x, aabb.lowerBound.y);
                            vs[2] = new Vector2(aabb.upperBound.x, aabb.upperBound.y);
                            vs[3] = new Vector2(aabb.lowerBound.x, aabb.upperBound.y);

                            DebugDraw.DrawPolygon(ref vs, 4, color);
                        }
                    }
                }
            }

            if ((flags & DebugDrawFlags.CenterOfMass) == DebugDrawFlags.CenterOfMass)
            {
                for (Body b = _bodyList; b != null; b = b.GetNext())
                {
                    Transform xf;
                    b.GetTransform(out xf);
                    xf.Position = b.GetWorldCenter();
                    DebugDraw.DrawTransform(ref xf);
                }
            }
        }
Esempio n. 20
0
        internal override bool SolvePositionConstraints(float baumgarte)
        {
            // TODO_ERIN block solve with limit. COME ON ERIN

            Body b1 = _bodyA;
            Body b2 = _bodyB;

            float angularError  = 0.0f;
            float positionError = 0.0f;

            // Solve angular limit constraint.
            if (_enableLimit && _limitState != LimitState.Inactive)
            {
                float angle        = b2._sweep.a - b1._sweep.a - _referenceAngle;
                float limitImpulse = 0.0f;

                if (_limitState == LimitState.Equal)
                {
                    // Prevent large angular corrections
                    float C = MathUtils.Clamp(angle - _lowerAngle, -Settings.b2_maxAngularCorrection, Settings.b2_maxAngularCorrection);
                    limitImpulse = -_motorMass * C;
                    angularError = Math.Abs(C);
                }
                else if (_limitState == LimitState.AtLower)
                {
                    float C = angle - _lowerAngle;
                    angularError = -C;

                    // Prevent large angular corrections and allow some slop.
                    C            = MathUtils.Clamp(C + Settings.b2_angularSlop, -Settings.b2_maxAngularCorrection, 0.0f);
                    limitImpulse = -_motorMass * C;
                }
                else if (_limitState == LimitState.AtUpper)
                {
                    float C = angle - _upperAngle;
                    angularError = C;

                    // Prevent large angular corrections and allow some slop.
                    C            = MathUtils.Clamp(C - Settings.b2_angularSlop, 0.0f, Settings.b2_maxAngularCorrection);
                    limitImpulse = -_motorMass * C;
                }

                b1._sweep.a -= b1._invI * limitImpulse;
                b2._sweep.a += b2._invI * limitImpulse;

                b1.SynchronizeTransform();
                b2.SynchronizeTransform();
            }

            // Solve point-to-point constraint.
            {
                Transform xf1, xf2;
                b1.GetTransform(out xf1);
                b2.GetTransform(out xf2);

                Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
                Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());

                Vector2 C = b2._sweep.c + r2 - b1._sweep.c - r1;
                positionError = C.magnitude;

                float invMass1 = b1._invMass, invMass2 = b2._invMass;
                float invI1 = b1._invI, invI2 = b2._invI;

                // Handle large detachment.
                const float k_allowedStretch = 10.0f * Settings.b2_linearSlop;
                if (C.sqrMagnitude > k_allowedStretch * k_allowedStretch)
                {
                    // Use a particle solution (no rotation).
                    Vector2 u = C; u.Normalize();
                    float   k = invMass1 + invMass2;
                    //Debug.Assert(k > Settings.b2_epsilon);
                    float       m        = 1.0f / k;
                    Vector2     impulse2 = m * (-C);
                    const float k_beta   = 0.5f;
                    b1._sweep.c -= k_beta * invMass1 * impulse2;
                    b2._sweep.c += k_beta * invMass2 * impulse2;

                    C = b2._sweep.c + r2 - b1._sweep.c - r1;
                }

                Mat22 K1 = new Mat22(new Vector2(invMass1 + invMass2, 0.0f), new Vector2(0.0f, invMass1 + invMass2));
                Mat22 K2 = new Mat22(new Vector2(invI1 * r1.y * r1.y, -invI1 * r1.x * r1.y), new Vector2(-invI1 * r1.x * r1.y, invI1 * r1.x * r1.x));
                Mat22 K3 = new Mat22(new Vector2(invI2 * r2.y * r2.y, -invI2 * r2.x * r2.y), new Vector2(-invI2 * r2.x * r2.y, invI2 * r2.x * r2.x));

                Mat22 Ka;
                Mat22 K;
                Mat22.Add(ref K1, ref K2, out Ka);
                Mat22.Add(ref Ka, ref K3, out K);

                Vector2 impulse = K.Solve(-C);

                b1._sweep.c -= b1._invMass * impulse;
                b1._sweep.a -= b1._invI * MathUtils.Cross(r1, impulse);

                b2._sweep.c += b2._invMass * impulse;
                b2._sweep.a += b2._invI * MathUtils.Cross(r2, impulse);

                b1.SynchronizeTransform();
                b2.SynchronizeTransform();
            }

            return(positionError <= Settings.b2_linearSlop && angularError <= Settings.b2_angularSlop);
        }
Esempio n. 21
0
        internal override void InitVelocityConstraints(ref TimeStep step)
        {
            Body g1 = _ground1;
            Body g2 = _ground2;
            Body b1 = _bodyA;
            Body b2 = _bodyB;

            float K = 0.0f;

            _J.Setzero();

            if (_revolute1 != null)
            {
                _J.angularA = -1.0f;
                K          += b1._invI;
            }
            else
            {
                Transform xf1, xfg1;
                b1.GetTransform(out xf1);
                g1.GetTransform(out xfg1);

                Vector2 ug   = MathUtils.Multiply(ref xfg1.R, _prismatic1._localxAxis1);
                Vector2 r    = MathUtils.Multiply(ref xf1.R, _localAnchor1 - b1.GetLocalCenter());
                float   crug = MathUtils.Cross(r, ug);
                _J.linearA  = -ug;
                _J.angularA = -crug;
                K          += b1._invMass + b1._invI * crug * crug;
            }

            if (_revolute2 != null)
            {
                _J.angularB = -_ratio;
                K          += _ratio * _ratio * b2._invI;
            }
            else
            {
                Transform xfg1, xf2;
                g1.GetTransform(out xfg1);
                b2.GetTransform(out xf2);

                Vector2 ug   = MathUtils.Multiply(ref xfg1.R, _prismatic2._localxAxis1);
                Vector2 r    = MathUtils.Multiply(ref xf2.R, _localAnchor2 - b2.GetLocalCenter());
                float   crug = MathUtils.Cross(r, ug);
                _J.linearB  = -_ratio * ug;
                _J.angularB = -_ratio * crug;
                K          += _ratio * _ratio * (b2._invMass + b2._invI * crug * crug);
            }

            // Compute effective mass.
            //Debug.Assert(K > 0.0f);
            _mass = K > 0.0f ? 1.0f / K : 0.0f;

            if (step.warmStarting)
            {
                // Warm starting.
                b1._linearVelocity  += b1._invMass * _impulse * _J.linearA;
                b1._angularVelocity += b1._invI * _impulse * _J.angularA;
                b2._linearVelocity  += b2._invMass * _impulse * _J.linearB;
                b2._angularVelocity += b2._invI * _impulse * _J.angularB;
            }
            else
            {
                _impulse = 0.0f;
            }
        }
Esempio n. 22
0
        internal override void SolveVelocityConstraints(ref TimeStep step)
        {
            Body bA = _bodyA;
            Body bB = _bodyB;

            Vector2 vA = bA._linearVelocity;
            float   wA = bA._angularVelocity;
            Vector2 vB = bB._linearVelocity;
            float   wB = bB._angularVelocity;

            float mA = bA._invMass, mB = bB._invMass;
            float iA = bA._invI, iB = bB._invI;

            Transform xfA, xfB;

            bA.GetTransform(out xfA);
            bB.GetTransform(out xfB);

            Vector2 rA = MathUtils.Multiply(ref xfA.R, _localAnchor1 - bA.GetLocalCenter());
            Vector2 rB = MathUtils.Multiply(ref xfB.R, _localAnchor2 - bB.GetLocalCenter());

            // Solve angular friction
            {
                float Cdot    = wB - wA;
                float impulse = -_angularMass * Cdot;

                float oldImpulse = _angularImpulse;
                float maxImpulse = step.dt * _maxTorque;
                _angularImpulse = MathUtils.Clamp(_angularImpulse + impulse, -maxImpulse, maxImpulse);
                impulse         = _angularImpulse - oldImpulse;

                wA -= iA * impulse;
                wB += iB * impulse;
            }

            // Solve linear friction
            {
                Vector2 Cdot = vB + MathUtils.Cross(wB, rB) - vA - MathUtils.Cross(wA, rA);

                Vector2 impulse    = -MathUtils.Multiply(ref _linearMass, Cdot);
                Vector2 oldImpulse = _linearImpulse;
                _linearImpulse += impulse;

                float maxImpulse = step.dt * _maxForce;

                if (_linearImpulse.sqrMagnitude > maxImpulse * maxImpulse)
                {
                    _linearImpulse.Normalize();
                    _linearImpulse *= maxImpulse;
                }

                impulse = _linearImpulse - oldImpulse;

                vA -= mA * impulse;
                wA -= iA * MathUtils.Cross(rA, impulse);

                vB += mB * impulse;
                wB += iB * MathUtils.Cross(rB, impulse);
            }

            bA._linearVelocity  = vA;
            bA._angularVelocity = wA;
            bB._linearVelocity  = vB;
            bB._angularVelocity = wB;
        }
Esempio n. 23
0
 /// Test a point for containment in this fixture.
 /// @param p a point in world coordinates.
 public bool TestPoint(Vec2 p)
 {
     return(m_shape.TestPoint(m_body.GetTransform(), p));
 }
Esempio n. 24
0
        internal override void InitVelocityConstraints(ref TimeStep step)
        {
            Body b = _bodyB;

            float mass = b.GetMass();

            // Frequency
            float omega = 2.0f * Settings.b2_pi * _frequencyHz;

            // Damping coefficient
            float d = 2.0f * mass * _dampingRatio * omega;

            // Spring stiffness
            float k = mass * (omega * omega);

            // magic formulas
            // gamma has units of inverse mass.
            // beta has units of inverse time.
            //Debug.Assert(d + step.dt * k > Settings.b2_epsilon);

            _gamma = step.dt * (d + step.dt * k);
            if (_gamma != 0.0f)
            {
                _gamma = 1.0f / _gamma;
            }

            _beta = step.dt * k * _gamma;

            // Compute the effective mass matrix.
            Transform xf1;

            b.GetTransform(out xf1);
            Vector2 r = MathUtils.Multiply(ref xf1.R, _localAnchor - b.GetLocalCenter());

            // K    = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
            //      = [1/m1+1/m2     0    ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y]
            //        [    0     1/m1+1/m2]           [-r1.x*r1.y r1.x*r1.x]           [-r1.x*r1.y r1.x*r1.x]
            float invMass = b._invMass;
            float invI    = b._invI;

            Mat22 K1 = new Mat22(new Vector2(invMass, 0.0f), new Vector2(0.0f, invMass));
            Mat22 K2 = new Mat22(new Vector2(invI * r.y * r.y, -invI * r.x * r.y), new Vector2(-invI * r.x * r.y, invI * r.x * r.x));

            Mat22 K;

            Mat22.Add(ref K1, ref K2, out K);

            K.col1.x += _gamma;
            K.col2.y += _gamma;

            _mass = K.GetInverse();

            _C = b._sweep.c + r - _target;

            // Cheat with some damping
            b._angularVelocity *= 0.98f;

            // Warm starting.
            _impulse           *= step.dtRatio;
            b._linearVelocity  += invMass * _impulse;
            b._angularVelocity += invI * MathUtils.Cross(r, _impulse);
        }
Esempio n. 25
0
        internal override void InitVelocityConstraints(ref TimeStep step)
        {
            Body b1 = _bodyA;
            Body b2 = _bodyB;

            _localCenterA = b1.GetLocalCenter();
            _localCenterB = b2.GetLocalCenter();

            Transform xf1, xf2;

            b1.GetTransform(out xf1);
            b2.GetTransform(out xf2);

            // Compute the effective masses.
            Vector2 r1 = MathUtils.Multiply(ref xf1.R, _localAnchor1 - _localCenterA);
            Vector2 r2 = MathUtils.Multiply(ref xf2.R, _localAnchor2 - _localCenterB);
            Vector2 d  = b2._sweep.c + r2 - b1._sweep.c - r1;

            _invMassA = b1._invMass;
            _invIA    = b1._invI;
            _invMassB = b2._invMass;
            _invIB    = b2._invI;

            // Compute motor Jacobian and effective mass.
            {
                _axis = MathUtils.Multiply(ref xf1.R, _localxAxis1);
                _a1   = MathUtils.Cross(d + r1, _axis);
                _a2   = MathUtils.Cross(r2, _axis);

                _motorMass = _invMassA + _invMassB + _invIA * _a1 * _a1 + _invIB * _a2 * _a2;

                if (_motorMass > Settings.b2_epsilon)
                {
                    _motorMass = 1.0f / _motorMass;
                }
            }

            // Prismatic constraint.
            {
                _perp = MathUtils.Multiply(ref xf1.R, _localyAxis1);

                _s1 = MathUtils.Cross(d + r1, _perp);
                _s2 = MathUtils.Cross(r2, _perp);

                float m1 = _invMassA, m2 = _invMassB;
                float i1 = _invIA, i2 = _invIB;

                float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
                float k12 = i1 * _s1 + i2 * _s2;
                float k13 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
                float k22 = i1 + i2;
                float k23 = i1 * _a1 + i2 * _a2;
                float k33 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;

                _K.col1 = new Vector3(k11, k12, k13);
                _K.col2 = new Vector3(k12, k22, k23);
                _K.col3 = new Vector3(k13, k23, k33);
            }

            // Compute motor and limit terms.
            if (_enableLimit)
            {
                float jointTranslation = Vector2.Dot(_axis, d);
                if (Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.b2_linearSlop)
                {
                    _limitState = LimitState.Equal;
                }
                else if (jointTranslation <= _lowerTranslation)
                {
                    if (_limitState != LimitState.AtLower)
                    {
                        _limitState = LimitState.AtLower;
                        _impulse.z  = 0.0f;
                    }
                }
                else if (jointTranslation >= _upperTranslation)
                {
                    if (_limitState != LimitState.AtUpper)
                    {
                        _limitState = LimitState.AtUpper;
                        _impulse.z  = 0.0f;
                    }
                }
                else
                {
                    _limitState = LimitState.Inactive;
                    _impulse.z  = 0.0f;
                }
            }
            else
            {
                _limitState = LimitState.Inactive;
            }

            if (_enableMotor == false)
            {
                _motorImpulse = 0.0f;
            }

            if (step.warmStarting)
            {
                // Account for variable time step.
                _impulse      *= step.dtRatio;
                _motorImpulse *= step.dtRatio;

                Vector2 P  = _impulse.x * _perp + (_motorImpulse + _impulse.z) * _axis;
                float   L1 = _impulse.x * _s1 + _impulse.y + (_motorImpulse + _impulse.z) * _a1;
                float   L2 = _impulse.x * _s2 + _impulse.y + (_motorImpulse + _impulse.z) * _a2;

                b1._linearVelocity  -= _invMassA * P;
                b1._angularVelocity -= _invIA * L1;

                b2._linearVelocity  += _invMassB * P;
                b2._angularVelocity += _invIB * L2;
            }
            else
            {
                _impulse      = Vector3.zero;
                _motorImpulse = 0.0f;
            }
        }
Esempio n. 26
0
        internal override void InitVelocityConstraints(ref TimeStep step)
        {
            Body bA = _bodyA;
            Body bB = _bodyB;

            Transform xfA, xfB;

            bA.GetTransform(out xfA);
            bB.GetTransform(out xfB);

            // Compute the effective mass matrix.
            Vector2 rA = MathUtils.Multiply(ref xfA.R, _localAnchor1 - bA.GetLocalCenter());
            Vector2 rB = MathUtils.Multiply(ref xfB.R, _localAnchor2 - bB.GetLocalCenter());

            // J = [-I -r1_skew I r2_skew]
            //     [ 0       -1 0       1]
            // r_skew = [-ry; rx]

            // Matlab
            // K = [ mA+r1y^2*iA+mB+r2y^2*iB,  -r1y*iA*r1x-r2y*iB*r2x,          -r1y*iA-r2y*iB]
            //     [  -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB,           r1x*iA+r2x*iB]
            //     [          -r1y*iA-r2y*iB,           r1x*iA+r2x*iB,                   iA+iB]

            float mA = bA._invMass, mB = bB._invMass;
            float iA = bA._invI, iB = bB._invI;

            Mat22 K1 = new Mat22();

            K1.col1.x = mA + mB;    K1.col2.x = 0.0f;
            K1.col1.y = 0.0f;               K1.col2.y = mA + mB;

            Mat22 K2 = new Mat22();

            K2.col1.x = iA * rA.y * rA.y;  K2.col2.x = -iA * rA.x * rA.y;
            K2.col1.y = -iA * rA.x * rA.y;  K2.col2.y = iA * rA.x * rA.x;

            Mat22 K3 = new Mat22();

            K3.col1.x = iB * rB.y * rB.y;  K3.col2.x = -iB * rB.x * rB.y;
            K3.col1.y = -iB * rB.x * rB.y;  K3.col2.y = iB * rB.x * rB.x;

            Mat22 K12;

            Mat22.Add(ref K1, ref K2, out K12);
            Mat22 K;

            Mat22.Add(ref K12, ref K3, out K);
            _linearMass = K.GetInverse();

            _angularMass = iA + iB;
            if (_angularMass > 0.0f)
            {
                _angularMass = 1.0f / _angularMass;
            }

            if (step.warmStarting)
            {
                // Scale impulses to support a variable time step.
                _linearImpulse  *= step.dtRatio;
                _angularImpulse *= step.dtRatio;

                Vector2 P = new Vector2(_linearImpulse.x, _linearImpulse.y);

                bA._linearVelocity  -= mA * P;
                bA._angularVelocity -= iA * (MathUtils.Cross(rA, P) + _angularImpulse);

                bB._linearVelocity  += mB * P;
                bB._angularVelocity += iB * (MathUtils.Cross(rB, P) + _angularImpulse);
            }
            else
            {
                _linearImpulse  = Vector2.zero;
                _angularImpulse = 0.0f;
            }
        }