Box2DX.Dynamics.LineJoint.GetJointSpeed C# (CSharp) Метод

        public float GetJointSpeed()
        {
            Body b1 = _body1;
            Body b2 = _body2;

            Vec2 r1 = Box2DX.Common.Math.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
            Vec2 r2 = Box2DX.Common.Math.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
            Vec2 p1 = b1._sweep.C + r1;
            Vec2 p2 = b2._sweep.C + r2;
            Vec2 d = p2 - p1;
            Vec2 axis = b1.GetWorldVector(_localXAxis1);

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

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