public override void Step(TimeStep step)
        {
            float timestep = step.Dt;

            if (timestep <= Settings.FLT_EPSILON)
            {
                return;
            }
            if (timestep > MaxTimestep && MaxTimestep > 0)
            {
                timestep = MaxTimestep;
            }
            for (ControllerEdge i = _bodyList; i != null; i = i.nextBody)
            {
                Body body = i.body;
                if (body.IsSleeping())
                {
                    continue;
                }

                Vector2 damping = body.GetWorldVector(T.Multiply(body.GetLocalVector(body.GetLinearVelocity())));
                body.SetLinearVelocity(body.GetLinearVelocity() + timestep * damping);
            }
        }
        internal override void SolveVelocityConstraints(TimeStep step)
        {
            Body b = _body2;

            Vector2 r = b.GetTransform().TransformDirection(_localAnchor - b.GetLocalCenter());

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

            Vector2 oldImpulse = _impulse;

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

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

            b._linearVelocity  += b._invMass * impulse;
            b._angularVelocity += b._invI * r.Cross(impulse);
        }
        internal override bool SolvePositionConstraints(float baumgarte)
        {
            Body b1 = _body1;
            Body b2 = _body2;

            Vector2 c1 = b1._sweep.C;
            float   a1 = b1._sweep.A;

            Vector2 c2 = b2._sweep.C;
            float   a2 = b2._sweep.A;

            // Solve linear limit constraint.
            float linearError = 0.0f, angularError = 0.0f;
            bool  active = false;
            float C2     = 0.0f;

            Mat22 R1 = new Mat22(a1), R2 = new Mat22(a2);

            Vector2 r1 = R1.Multiply(_localAnchor1 - _localCenter1);
            Vector2 r2 = R2.Multiply(_localAnchor2 - _localCenter2);
            Vector2 d  = c2 + r2 - c1 - r1;

            if (_enableLimit)
            {
                _axis = R1.Multiply(_localXAxis1);

                _a1 = (d + r1).Cross(_axis);
                _a2 = r2.Cross(_axis);

                float translation = Vector2.Dot(_axis, d);
                if (System.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
                {
                    // Prevent large angular corrections
                    C2          = Box2DNet.Common.Math.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
                    linearError = Box2DNet.Common.Math.Abs(translation);
                    active      = true;
                }
                else if (translation <= _lowerTranslation)
                {
                    // Prevent large linear corrections and allow some slop.
                    C2          = Box2DNet.Common.Math.Clamp(translation - _lowerTranslation + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
                    linearError = _lowerTranslation - translation;
                    active      = true;
                }
                else if (translation >= _upperTranslation)
                {
                    // Prevent large linear corrections and allow some slop.
                    C2          = Box2DNet.Common.Math.Clamp(translation - _upperTranslation - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection);
                    linearError = translation - _upperTranslation;
                    active      = true;
                }
            }

            _perp = R1.Multiply(_localYAxis1);

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

            Vector2 impulse;
            float   C1;

            C1 = Vector2.Dot(_perp, d);

            linearError  = Box2DNet.Common.Math.Max(linearError, Box2DNet.Common.Math.Abs(C1));
            angularError = 0.0f;

            if (active)
            {
                float m1 = _invMass1, m2 = _invMass2;
                float i1 = _invI1, i2 = _invI2;

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

                _K.Col1 = new Vector2(k11, k12);
                _K.Col2 = new Vector2(k12, k22);

                Vector2 C = new Vector2();
                C.X = C1;
                C.Y = C2;

                impulse = _K.Solve(-C);
            }
            else
            {
                float m1 = _invMass1, m2 = _invMass2;
                float i1 = _invI1, i2 = _invI2;

                float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;

                float impulse1 = (-C1) / k11;
                impulse.X = impulse1;
                impulse.Y = 0.0f;
            }

            Vector2 P  = impulse.X * _perp + impulse.Y * _axis;
            float   L1 = impulse.X * _s1 + impulse.Y * _a1;
            float   L2 = impulse.X * _s2 + impulse.Y * _a2;

            c1 -= _invMass1 * P;
            a1 -= _invI1 * L1;
            c2 += _invMass2 * P;
            a2 += _invI2 * L2;

            // TODO_ERIN remove need for this.
            b1._sweep.C = c1;
            b1._sweep.A = a1;
            b2._sweep.C = c2;
            b2._sweep.A = a2;
            b1.SynchronizeTransform();
            b2.SynchronizeTransform();

            return(linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
        }