GetTransform() public method

Get the body Transform for the body's origin.
public GetTransform ( ) : Box2DX.Common.Transform
return Box2DX.Common.Transform
示例#1
0
        internal override void SolveVelocityConstraints(TimeStep step)
        {
            //B2_NOT_USED(step);

            Body b1 = _body1;
            Body b2 = _body2;

            Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter());
            Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter());

            // Cdot = dot(u, v + cross(w, r))
            Vector2 v1      = b1._linearVelocity + r1.CrossScalarPreMultiply(b1._angularVelocity);
            Vector2 v2      = b2._linearVelocity + r2.CrossScalarPreMultiply(b2._angularVelocity);
            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 * r1.Cross(P);
            b2._linearVelocity  += b2._invMass * P;
            b2._angularVelocity += b2._invI * r2.Cross(P);
        }
示例#2
0
        internal override void SolveVelocityConstraints(TimeStep step)
        {
            //B2_NOT_USED(step);

            Body b1 = _bodyA;
            Body b2 = _bodyB;

            Vec2 r1 = Math.Mul(b1.GetTransform().R, _localAnchor1 - b1.GetLocalCenter());
            Vec2 r2 = Math.Mul(b2.GetTransform().R, _localAnchor2 - b2.GetLocalCenter());

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

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

            _impulse += impulse;

            Vec2 P = impulse * _u;

            b1._linearVelocity  -= b1._invMass * P;
            b1._angularVelocity -= b1._invI * Vec2.Cross(r1, P);
            b2._linearVelocity  += b2._invMass * P;
            b2._angularVelocity += b2._invI * Vec2.Cross(r2, P);
        }
        internal override void InitVelocityConstraints(TimeStep step)
        {
            Body b = _body2;

            float mass = b.GetMass();

            // Frequency
            float omega = 2.0f * Settings.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.
            Box2DXDebug.Assert(d + step.Dt * k > Settings.FLT_EPSILON);
            _gamma = 1.0f / (step.Dt * (d + step.Dt * k));
            _beta  = step.Dt * k * _gamma;

            // Compute the effective mass matrix.
            Vector2 r = b.GetTransform().TransformDirection(_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();

            K1.Col1.X = invMass; K1.Col2.X = 0.0f;
            K1.Col1.Y = 0.0f; K1.Col2.Y = invMass;

            Mat22 K2 = new Mat22();

            K2.Col1.X = invI * r.Y * r.Y; K2.Col2.X = -invI * r.X * r.Y;
            K2.Col1.Y = -invI * r.X * r.Y; K2.Col2.Y = invI * r.X * r.X;

            Mat22 K = K1 + K2;

            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 * r.Cross(_impulse);
        }
示例#4
0
        internal override void InitVelocityConstraints(TimeStep step)
        {
            Body g1 = _ground1;
            Body g2 = _ground2;
            Body b1 = _body1;
            Body b2 = _body2;

            float K = 0.0f;

            _J.SetZero();

            if (_revolute1 != null)
            {
                _J.Angular1 = -1.0f;
                K          += b1._invI;
            }
            else
            {
                Vector2 ug   = g1.GetTransform().TransformDirection(_prismatic1._localXAxis1);
                Vector2 r    = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter());
                float   crug = r.Cross(ug);
                _J.Linear1  = -ug;
                _J.Angular1 = -crug;
                K          += b1._invMass + b1._invI * crug * crug;
            }

            if (_revolute2 != null)
            {
                _J.Angular2 = -_ratio;
                K          += _ratio * _ratio * b2._invI;
            }
            else
            {
                Vector2 ug   = g2.GetTransform().TransformDirection(_prismatic2._localXAxis1);
                Vector2 r    = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter());
                float   crug = r.Cross(ug);
                _J.Linear2  = -_ratio * ug;
                _J.Angular2 = -_ratio * crug;
                K          += _ratio * _ratio * (b2._invMass + b2._invI * crug * crug);
            }

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

            if (step.WarmStarting)
            {
                // Warm starting.
                b1._linearVelocity  += b1._invMass * _impulse * _J.Linear1;
                b1._angularVelocity += b1._invI * _impulse * _J.Angular1;
                b2._linearVelocity  += b2._invMass * _impulse * _J.Linear2;
                b2._angularVelocity += b2._invI * _impulse * _J.Angular2;
            }
            else
            {
                _impulse = 0.0f;
            }
        }
示例#5
0
        internal override void SolveVelocityConstraints(TimeStep step)
        {
            Body b1 = _bodyA;
            Body b2 = _bodyB;

            Vec2 r1 = Box2DXMath.Mul(b1.GetTransform().R, _localAnchor1 - b1.GetLocalCenter());
            Vec2 r2 = Box2DXMath.Mul(b2.GetTransform().R, _localAnchor2 - b2.GetLocalCenter());

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

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

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

            if (_limitState1 == LimitState.AtUpperLimit)
            {
                Vec2 v1 = b1._linearVelocity + Vec2.Cross(b1._angularVelocity, r1);

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

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

            if (_limitState2 == LimitState.AtUpperLimit)
            {
                Vec2 v2 = b2._linearVelocity + Vec2.Cross(b2._angularVelocity, r2);

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

                Vec2 P2 = -impulse * _u2;
                b2._linearVelocity  += b2._invMass * P2;
                b2._angularVelocity += b2._invI * Vec2.Cross(r2, P2);
            }
        }
示例#6
0
        public void Evaluate()
        {
            Body bodyA = _fixtureA.Body;
            Body bodyB = _fixtureB.Body;

            Box2DXDebug.Assert(CollideShapeFunction != null);

            CollideShapeFunction(ref _manifold, _fixtureA.Shape, bodyA.GetTransform(), _fixtureB.Shape, bodyB.GetTransform());
        }
示例#7
0
        internal override void InitVelocityConstraints(TimeStep step)
        {
            Body g1 = _ground1;
            Body g2 = _ground2;
            Body b1 = _bodyA;
            Body b2 = _bodyB;

            float K = 0.0f;

            _J.SetZero();

            if (_revolute1 != null)
            {
                _J.Angular1 = -1.0f;
                K          += b1._invI;
            }
            else
            {
                Vec2  ug   = Math.Mul(g1.GetTransform().R, _prismatic1._localXAxis1);
                Vec2  r    = Math.Mul(b1.GetTransform().R, _localAnchor1 - b1.GetLocalCenter());
                float crug = Vec2.Cross(r, ug);
                _J.Linear1  = -ug;
                _J.Angular1 = -crug;
                K          += b1._invMass + b1._invI * crug * crug;
            }

            if (_revolute2 != null)
            {
                _J.Angular2 = -_ratio;
                K          += _ratio * _ratio * b2._invI;
            }
            else
            {
                Vec2  ug   = Math.Mul(g2.GetTransform().R, _prismatic2._localXAxis1);
                Vec2  r    = Math.Mul(b2.GetTransform().R, _localAnchor2 - b2.GetLocalCenter());
                float crug = Vec2.Cross(r, ug);
                _J.Linear2  = -_ratio * ug;
                _J.Angular2 = -_ratio * crug;
                K          += _ratio * _ratio * (b2._invMass + b2._invI * crug * crug);
            }

            // Compute effective mass.
            _mass = K > 0.0f ? 1.0f / K : 0.0f;

            if (step.WarmStarting)
            {
                // Warm starting.
                b1._linearVelocity  += b1._invMass * _impulse * _J.Linear1;
                b1._angularVelocity += b1._invI * _impulse * _J.Angular1;
                b2._linearVelocity  += b2._invMass * _impulse * _J.Linear2;
                b2._angularVelocity += b2._invI * _impulse * _J.Angular2;
            }
            else
            {
                _impulse = 0.0f;
            }
        }
示例#8
0
        public override void Evaluate()
        {
            Body bodyA = _fixtureA.GetBody();
            Body bodyB = _fixtureB.GetBody();

            Collision.Collision.CollideCircles(out Manifold,
                                               (CircleShape)_fixtureA.GetShape(), bodyA.GetTransform(),
                                               (CircleShape)_fixtureB.GetShape(), bodyB.GetTransform());
        }
示例#9
0
        /// <summary>
        /// Get the world manifold.
        /// </summary>
        public void GetWorldManifold(out WorldManifold worldManifold)
        {
            worldManifold = new WorldManifold();

            Body  bodyA  = _fixtureA.Body;
            Body  bodyB  = _fixtureB.Body;
            Shape shapeA = _fixtureA.Shape;
            Shape shapeB = _fixtureB.Shape;

            worldManifold.Initialize(_manifold, bodyA.GetTransform(), shapeA._radius, bodyB.GetTransform(), shapeB._radius);
        }
示例#10
0
        public PulleyJoint(PulleyJointDef def)
            : base(def)
        {
            _ground        = _body1.GetWorld().GetGroundBody();
            _groundAnchor1 = def.GroundAnchor1 - _ground.GetTransform().position;
            _groundAnchor2 = def.GroundAnchor2 - _ground.GetTransform().position;
            _localAnchor1  = def.LocalAnchor1;
            _localAnchor2  = def.LocalAnchor2;

            Box2DXDebug.Assert(def.Ratio != 0.0f);
            _ratio = def.Ratio;

            _constant = def.Length1 + _ratio * def.Length2;

            _maxLength1 = Common.Math.Min(def.MaxLength1, _constant - _ratio * PulleyJoint.MinPulleyLength);
            _maxLength2 = Common.Math.Min(def.MaxLength2, (_constant - PulleyJoint.MinPulleyLength) / _ratio);

            _impulse       = 0.0f;
            _limitImpulse1 = 0.0f;
            _limitImpulse2 = 0.0f;
        }
示例#11
0
文件: World.cs 项目: vb0067/LGame
        private void DrawJoint(Joint joint)
        {
            Body      b1  = joint.GetBody1();
            Body      b2  = joint.GetBody2();
            Transform xf1 = b1.GetTransform();
            Transform xf2 = b2.GetTransform();
            Vec2      x1  = xf1.Position;
            Vec2      x2  = xf2.Position;
            Vec2      p1  = joint.Anchor1;
            Vec2      p2  = joint.Anchor2;

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

            switch (joint.GetType())
            {
            case JointType.DistanceJoint:
                _debugDraw.DrawSegment(p1, p2, color);
                break;

            case JointType.PulleyJoint:
            {
                PulleyJoint pulley = (PulleyJoint)joint;
                Vec2        s1     = pulley.GroundAnchor1;
                Vec2        s2     = pulley.GroundAnchor2;
                _debugDraw.DrawSegment(s1, p1, color);
                _debugDraw.DrawSegment(s2, p2, color);
                _debugDraw.DrawSegment(s1, s2, color);
            }
            break;

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

            default:
                _debugDraw.DrawSegment(x1, p1, color);
                _debugDraw.DrawSegment(p1, p2, color);
                _debugDraw.DrawSegment(x2, p2, color);
                break;
            }
        }
示例#12
0
        /// <summary>
        /// Get the current joint translation speed, usually in meters per second.
        /// </summary>
        public float GetJointSpeed()
        {
            Body b1 = _body1;
            Body b2 = _body2;

            Vector2 r1   = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter());
            Vector2 r2   = b2.GetTransform().TransformDirection(_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, axis.CrossScalarPreMultiply(w1)) + Vector2.Dot(axis, v2 + r2.CrossScalarPreMultiply(w2) - v1 - r1.CrossScalarPreMultiply(w1));

            return(speed);
        }
示例#13
0
        /// <summary>
        /// Get the current joint translation speed, usually in meters per second.
        /// </summary>
        public float GetJointSpeed()
        {
            Body b1 = _bodyA;
            Body b2 = _bodyB;

            Vec2 r1   = Box2DX.Common.Math.Mul(b1.GetTransform().R, _localAnchor1 - b1.GetLocalCenter());
            Vec2 r2   = Box2DX.Common.Math.Mul(b2.GetTransform().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);
        }
示例#14
0
        internal override bool SolvePositionConstraints(float baumgarte)
        {
            if (_frequencyHz > 0.0f)
            {
                //There is no possition correction for soft distace constraint.
                return(true);
            }

            Body b1 = _body1;
            Body b2 = _body2;

            Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter());
            Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter());

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

            float length = d.magnitude;

            d.Normalize();
            float C = length - _length;

            C = Mathf.Clamp(C, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);

            float impulse = -_mass * C;

            _u = d;
            Vector2 P = impulse * _u;

            b1._sweep.C -= b1._invMass * P;
            b1._sweep.A -= b1._invI * r1.Cross(P);
            b2._sweep.C += b2._invMass * P;
            b2._sweep.A += b2._invI * r2.Cross(P);

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

            return(System.Math.Abs(C) < Settings.LinearSlop);
        }
示例#15
0
        internal override void SolveVelocityConstraints(TimeStep step)
        {
            Body b = _bodyB;

            Vec2 r = Common.Math.Mul(b.GetTransform().R, _localAnchor - b.GetLocalCenter());

            // Cdot = v + cross(w, r)
            Vec2 Cdot    = b._linearVelocity + Vec2.Cross(b._angularVelocity, r);
            Vec2 impulse = Box2DX.Common.Math.Mul(_mass, -(Cdot + _beta * _C + _gamma * _impulse));

            Vec2 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 * Vec2.Cross(r, impulse);
        }
        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);
        }
示例#17
0
 /// <summary>
 /// Compute the volume and centroid of this fixture intersected with a half plane.
 /// </summary>
 /// <param name="normal">Normal the surface normal.</param>
 /// <param name="offset">Offset the surface offset along normal.</param>
 /// <param name="c">Returns the centroid.</param>
 /// <returns>The total volume less than offset along normal.</returns>
 public float ComputeSubmergedArea(Vector2 normal, float offset, out Vector2 c)
 {
     return(_shape.ComputeSubmergedArea(normal, offset, _body.GetTransform(), out c));
 }
示例#18
0
        internal override void InitVelocityConstraints(TimeStep step)
        {
            Body b1 = _bodyA;
            Body b2 = _bodyB;

            // Compute the effective mass matrix.
            Vec2 r1 = Math.Mul(b1.GetTransform().R, _localAnchor1 - b1.GetLocalCenter());
            Vec2 r2 = Math.Mul(b2.GetTransform().R, _localAnchor2 - b2.GetLocalCenter());

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

            // Handle singularity.
            float length = _u.Length();

            if (length > Settings.LinearSlop)
            {
                _u *= 1.0f / length;
            }
            else
            {
                _u.Set(0.0f, 0.0f);
            }

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

            _mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;

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

                // Frequency
                float omega = 2.0f * Settings.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 inpulse to support a variable timestep.
                _impulse *= step.DtRatio;

                Vec2 P = _impulse * _u;
                b1._linearVelocity  -= b1._invMass * P;
                b1._angularVelocity -= b1._invI * Vec2.Cross(r1, P);
                b2._linearVelocity  += b2._invMass * P;
                b2._angularVelocity += b2._invI * Vec2.Cross(r2, P);
            }
            else
            {
                _impulse = 0.0f;
            }
        }
示例#19
0
文件: World.cs 项目: vb0067/LGame
        public void DrawDebugData()
        {
            if (_debugDraw == null)
            {
                return;
            }

            DebugDraw.DrawFlags flags = _debugDraw.Flags;

            if ((flags & DebugDraw.DrawFlags.Shape) != 0)
            {
                for (Body b = _bodyList; b != null; b = b.GetNext())
                {
                    Transform xf = b.GetTransform();
                    for (Fixture f = b.GetFixtureList(); f != null; f = f.GetNext())
                    {
                        if (b.IsStatic())
                        {
                            DrawShape(f, xf, new Color(0.5f, 0.9f, 0.5f));
                        }
                        else if (b.IsSleeping())
                        {
                            DrawShape(f, xf, new Color(0.5f, 0.5f, 0.9f));
                        }
                        else
                        {
                            DrawShape(f, xf, new Color(0.9f, 0.9f, 0.9f));
                        }
                    }
                }
            }

            if ((flags & DebugDraw.DrawFlags.Joint) != 0)
            {
                for (Joint j = _jointList; j != null; j = j.GetNext())
                {
                    if (j.GetType() != JointType.MouseJoint)
                    {
                        DrawJoint(j);
                    }
                }
            }

            if ((flags & DebugDraw.DrawFlags.Pair) != 0)
            {
                // TODO_ERIN
            }

            if ((flags & DebugDraw.DrawFlags.Aabb) != 0)
            {
                Color      color = new Color(0.9f, 0.3f, 0.9f);
                BroadPhase bp    = _contactManager._broadPhase;

                for (Body b = _bodyList; b != null; b = b.GetNext())
                {
                    for (Fixture f = b.GetFixtureList(); f != null; f = f.GetNext())
                    {
                        AABB   aabb = bp.GetFatAABB(f.ProxyId);
                        Vec2[] vs   = new Vec2[4];
                        vs[0].Set(aabb.LowerBound.X, aabb.LowerBound.Y);
                        vs[1].Set(aabb.UpperBound.X, aabb.LowerBound.Y);
                        vs[2].Set(aabb.UpperBound.X, aabb.UpperBound.Y);
                        vs[3].Set(aabb.LowerBound.X, aabb.UpperBound.Y);

                        _debugDraw.DrawPolygon(vs, 4, color);
                    }
                }
            }

            if ((flags & DebugDraw.DrawFlags.CenterOfMass) != 0)
            {
                for (Body b = _bodyList; b != null; b = b.GetNext())
                {
                    Transform xf = b.GetTransform();
                    xf.Position = b.GetWorldCenter();
                    _debugDraw.DrawXForm(xf);
                }
            }
        }
        public PulleyJoint(PulleyJointDef def)
            : base(def)
        {
            _ground = _body1.GetWorld().GetGroundBody();
            _groundAnchor1 = def.GroundAnchor1 - _ground.GetTransform().position;
            _groundAnchor2 = def.GroundAnchor2 - _ground.GetTransform().position;
            _localAnchor1 = def.LocalAnchor1;
            _localAnchor2 = def.LocalAnchor2;

            Box2DXDebug.Assert(def.Ratio != 0.0f);
            _ratio = def.Ratio;

            _constant = def.Length1 + _ratio * def.Length2;

            _maxLength1 = Common.Math.Min(def.MaxLength1, _constant - _ratio * PulleyJoint.MinPulleyLength);
            _maxLength2 = Common.Math.Min(def.MaxLength2, (_constant - PulleyJoint.MinPulleyLength) / _ratio);

            _impulse = 0.0f;
            _limitImpulse1 = 0.0f;
            _limitImpulse2 = 0.0f;
        }
示例#21
0
文件: Fixture.cs 项目: vb0067/LGame
 /// Test a point for containment in this fixture. This only works for convex shapes.
 /// @param xf the shape world transform.
 /// @param p a point in world coordinates.
 public bool TestPoint(Vec2 p)
 {
     return(Shape.TestPoint(Body.GetTransform(), p));
 }
示例#22
0
        internal override void InitVelocityConstraints(TimeStep step)
        {
            Body b1 = _body1;
            Body b2 = _body2;

            // Compute the effective mass matrix.
            Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter());
            Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter());

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

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

            if (length > Settings.LinearSlop)
            {
                _u *= 1.0f / length;
            }
            else
            {
                _u = Vector2.zero;
            }

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

            Box2DXDebug.Assert(invMass > Settings.FLT_EPSILON);
            _mass = 1.0f / invMass;

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

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

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

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

                // magic formulas
                _gamma = 1.0f / (step.Dt * (d + step.Dt * k));
                _bias  = C * step.Dt * k * _gamma;

                _mass = 1.0f / (invMass + _gamma);
            }

            if (step.WarmStarting)
            {
                //Scale the inpulse to support a variable timestep.
                _impulse *= step.DtRatio;
                Vector2 P = _impulse * _u;
                b1._linearVelocity  -= b1._invMass * P;
                b1._angularVelocity -= b1._invI * r1.Cross(P);
                b2._linearVelocity  += b2._invMass * P;
                b2._angularVelocity += b2._invI * r2.Cross(P);
            }
            else
            {
                _impulse = 0.0f;
            }
        }
示例#23
0
        internal override void InitVelocityConstraints(TimeStep step)
        {
            Body b1 = _body1;
            Body b2 = _body2;

            _localCenter1 = b1.GetLocalCenter();
            _localCenter2 = b2.GetLocalCenter();

            Transform xf1 = b1.GetTransform();
            Transform xf2 = b2.GetTransform();

            // Compute the effective masses.
            Vector2 r1 = xf1.TransformDirection(_localAnchor1 - _localCenter1);
            Vector2 r2 = xf2.TransformDirection(_localAnchor2 - _localCenter2);
            Vector2 d  = b2._sweep.C + r2 - b1._sweep.C - r1;

            _invMass1 = b1._invMass;
            _invI1    = b1._invI;
            _invMass2 = b2._invMass;
            _invI2    = b2._invI;

            // Compute motor Jacobian and effective mass.
            {
                _axis = xf1.TransformDirection(_localXAxis1);
                _a1   = (d + r1).Cross(_axis);
                _a2   = r2.Cross(_axis);

                _motorMass = _invMass1 + _invMass2 + _invI1 * _a1 * _a1 + _invI2 * _a2 * _a2;
                Box2DXDebug.Assert(_motorMass > Settings.FLT_EPSILON);
                _motorMass = 1.0f / _motorMass;
            }

            // Prismatic constraint.
            {
                _perp = xf1.TransformDirection(_localYAxis1);

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

                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);
            }

            // Compute motor and limit terms.
            if (_enableLimit)
            {
                float jointTranslation = Vector2.Dot(_axis, d);
                if (Box2DX.Common.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
                {
                    _limitState = LimitState.EqualLimits;
                }
                else if (jointTranslation <= _lowerTranslation)
                {
                    if (_limitState != LimitState.AtLowerLimit)
                    {
                        _limitState = LimitState.AtLowerLimit;
                        _impulse.Y  = 0.0f;
                    }
                }
                else if (jointTranslation >= _upperTranslation)
                {
                    if (_limitState != LimitState.AtUpperLimit)
                    {
                        _limitState = LimitState.AtUpperLimit;
                        _impulse.Y  = 0.0f;
                    }
                }
                else
                {
                    _limitState = LimitState.InactiveLimit;
                    _impulse.Y  = 0.0f;
                }
            }
            else
            {
                _limitState = LimitState.InactiveLimit;
            }

            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.Y) * _axis;
                float   L1 = _impulse.X * _s1 + (_motorImpulse + _impulse.Y) * _a1;
                float   L2 = _impulse.X * _s2 + (_motorImpulse + _impulse.Y) * _a2;

                b1._linearVelocity  -= _invMass1 * P;
                b1._angularVelocity -= _invI1 * L1;

                b2._linearVelocity  += _invMass2 * P;
                b2._angularVelocity += _invI2 * L2;
            }
            else
            {
                _impulse      = Vector2.Zero;
                _motorImpulse = 0.0f;
            }
        }
示例#24
0
        internal override void InitVelocityConstraints(TimeStep step)
        {
            Body b1 = _bodyA;
            Body b2 = _bodyB;

            // You cannot create a prismatic joint between bodies that
            // both have fixed rotation.
            Box2DXDebug.Assert(b1._invI > 0.0f || b2._invI > 0.0f);

            _localCenter1 = b1.GetLocalCenter();
            _localCenter2 = b2.GetLocalCenter();

            Transform xf1 = b1.GetTransform();
            Transform xf2 = b2.GetTransform();

            // Compute the effective masses.
            Vec2 r1 = Box2DX.Common.Math.Mul(xf1.R, _localAnchor1 - _localCenter1);
            Vec2 r2 = Box2DX.Common.Math.Mul(xf2.R, _localAnchor2 - _localCenter2);
            Vec2 d  = b2._sweep.C + r2 - b1._sweep.C - r1;

            _invMass1 = b1._invMass;
            _invI1    = b1._invI;
            _invMass2 = b2._invMass;
            _invI2    = b2._invI;

            // Compute motor Jacobian and effective mass.
            {
                _axis = Math.Mul(xf1.R, _localXAxis1);
                _a1   = Vec2.Cross(d + r1, _axis);
                _a2   = Vec2.Cross(r2, _axis);

                _motorMass = _invMass1 + _invMass2 + _invI1 * _a1 * _a1 + _invI2 * _a2 * _a2;
                if (_motorMass > Settings.FLT_EPSILON)
                {
                    _motorMass = 1.0f / _motorMass;
                }
            }

            // Prismatic constraint.
            {
                _perp = Math.Mul(xf1.R, _localYAxis1);

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

                float m1 = _invMass1, m2 = _invMass2;
                float i1 = _invI1, i2 = _invI2;

                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.Set(k11, k12, k13);
                _K.Col2.Set(k12, k22, k23);
                _K.Col3.Set(k13, k23, k33);
            }

            // Compute motor and limit terms.
            if (_enableLimit)
            {
                float jointTranslation = Vec2.Dot(_axis, d);
                if (Box2DX.Common.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
                {
                    _limitState = LimitState.EqualLimits;
                }
                else if (jointTranslation <= _lowerTranslation)
                {
                    if (_limitState != LimitState.AtLowerLimit)
                    {
                        _limitState = LimitState.AtLowerLimit;
                        _impulse.Z  = 0.0f;
                    }
                }
                else if (jointTranslation >= _upperTranslation)
                {
                    if (_limitState != LimitState.AtUpperLimit)
                    {
                        _limitState = LimitState.AtUpperLimit;
                        _impulse.Z  = 0.0f;
                    }
                }
                else
                {
                    _limitState = LimitState.InactiveLimit;
                    _impulse.Z  = 0.0f;
                }
            }
            else
            {
                _limitState = LimitState.InactiveLimit;
            }

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

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

                Vec2  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  -= _invMass1 * P;
                b1._angularVelocity -= _invI1 * L1;

                b2._linearVelocity  += _invMass2 * P;
                b2._angularVelocity += _invI2 * L2;
            }
            else
            {
                _impulse.SetZero();
                _motorImpulse = 0.0f;
            }
        }
        internal override void SolveVelocityConstraints(TimeStep step)
        {
            Body b1 = _body1;
            Body b2 = _body2;

            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.EqualLimits)
            {
                float Cdot       = w2 - w1 - _motorSpeed;
                float impulse    = _motorMass * (-Cdot);
                float oldImpulse = _motorImpulse;
                float maxImpulse = step.Dt * _maxMotorTorque;
                _motorImpulse = Box2DX.Common.Math.Clamp(_motorImpulse + impulse, -maxImpulse, maxImpulse);
                impulse       = _motorImpulse - oldImpulse;

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

            //Solve limit constraint.
            if (_enableLimit && _limitState != LimitState.InactiveLimit)
            {
                Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter());
                Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter());

                // Solve point-to-point constraint
                Vector2 Cdot1 = v2 + r2.CrossScalarPreMultiply(w2) - v1 - r1.CrossScalarPreMultiply(w1);
                float   Cdot2 = w2 - w1;
                Vector3 Cdot  = new Vector3(Cdot1.X, Cdot1.Y, Cdot2);

                Vector3 impulse = _mass.Solve33(-Cdot);

                if (_limitState == LimitState.EqualLimits)
                {
                    _impulse += impulse;
                }
                else if (_limitState == LimitState.AtLowerLimit)
                {
                    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.AtUpperLimit)
                {
                    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 = impulse.ToVector2();

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

                v2 += m2 * P;
                w2 += i2 * (r2.Cross(P) + impulse.Z);
            }
            else
            {
                Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter());
                Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter());

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

                _impulse.X += impulse.X;
                _impulse.Y += impulse.Y;

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

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

            b1._linearVelocity  = v1;
            b1._angularVelocity = w1;
            b2._linearVelocity  = v2;
            b2._angularVelocity = w2;
        }
示例#26
0
        internal override void InitVelocityConstraints(TimeStep step)
        {
            Body b1 = _bodyA;
            Body b2 = _bodyB;

            Vec2 r1 = Box2DXMath.Mul(b1.GetTransform().R, _localAnchor1 - b1.GetLocalCenter());
            Vec2 r2 = Box2DXMath.Mul(b2.GetTransform().R, _localAnchor2 - b2.GetLocalCenter());

            Vec2 p1 = b1._sweep.C + r1;
            Vec2 p2 = b2._sweep.C + r2;

            Vec2 s1 = _groundAnchor1;
            Vec2 s2 = _groundAnchor2;

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

            float length1 = _u1.Length();
            float length2 = _u2.Length();

            if (length1 > Settings.LinearSlop)
            {
                _u1 *= 1.0f / length1;
            }
            else
            {
                _u1.SetZero();
            }

            if (length2 > Settings.LinearSlop)
            {
                _u2 *= 1.0f / length2;
            }
            else
            {
                _u2.SetZero();
            }

            float C = _constant - length1 - _ratio * length2;

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

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

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

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

            _limitMass1 = b1._invMass + b1._invI * cr1u1 * cr1u1;
            _limitMass2 = b2._invMass + b2._invI * cr2u2 * cr2u2;
            _pulleyMass = _limitMass1 + _ratio * _ratio * _limitMass2;
            Box2DXDebug.Assert(_limitMass1 > Settings.FLT_EPSILON);
            Box2DXDebug.Assert(_limitMass2 > Settings.FLT_EPSILON);
            Box2DXDebug.Assert(_pulleyMass > Settings.FLT_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.
                Vec2 P1 = -(_impulse + _limitImpulse1) * _u1;
                Vec2 P2 = (-_ratio * _impulse - _limitImpulse2) * _u2;
                b1._linearVelocity  += b1._invMass * P1;
                b1._angularVelocity += b1._invI * Vec2.Cross(r1, P1);
                b2._linearVelocity  += b2._invMass * P2;
                b2._angularVelocity += b2._invI * Vec2.Cross(r2, P2);
            }
            else
            {
                _impulse       = 0.0f;
                _limitImpulse1 = 0.0f;
                _limitImpulse2 = 0.0f;
            }
        }
        internal override void InitVelocityConstraints(TimeStep step)
        {
            Body b1 = _body1;
            Body b2 = _body2;

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

            // Compute the effective mass matrix.
            Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter());
            Vector2 r2 = b2.GetTransform().TransformDirection(_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.Y * i2;
            _mass.Col1.Z = _mass.Col3.X;
            _mass.Col2.Z = _mass.Col3.Y;
            _mass.Col3.Z = i1 + i2;

            _motorMass = 1.0f / (i1 + i2);

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

            if (_enableLimit)
            {
                float jointAngle = b2._sweep.A - b1._sweep.A - _referenceAngle;
                if (Box2DXMath.Abs(_upperAngle - _lowerAngle) < 2.0f * Settings.AngularSlop)
                {
                    _limitState = LimitState.EqualLimits;
                }
                else if (jointAngle <= _lowerAngle)
                {
                    if (_limitState != LimitState.AtLowerLimit)
                    {
                        _impulse.Z = 0.0f;
                    }
                    _limitState = LimitState.AtLowerLimit;
                }
                else if (jointAngle >= _upperAngle)
                {
                    if (_limitState != LimitState.AtUpperLimit)
                    {
                        _impulse.Z = 0.0f;
                    }
                    _limitState = LimitState.AtUpperLimit;
                }
                else
                {
                    _limitState = LimitState.InactiveLimit;
                    _impulse.Z  = 0.0f;
                }
            }
            else
            {
                _limitState = LimitState.InactiveLimit;
            }

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

                Vector2 P = _impulse.ToVector2();

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

                b2._linearVelocity  += m2 * P;
                b2._angularVelocity += i2 * (r2.Cross(P) + _motorImpulse + _impulse.Z);
            }
            else
            {
                _impulse      = Vector3.Zero;
                _motorImpulse = 0.0f;
            }
        }
示例#28
0
        internal override bool SolvePositionConstraints(float baumgarte)
        {
            Body b1 = _bodyA;
            Body b2 = _bodyB;

            Vec2 s1 = _groundAnchor1;
            Vec2 s2 = _groundAnchor2;

            float linearError = 0.0f;

            if (_state == LimitState.AtUpperLimit)
            {
                Vec2 r1 = Box2DXMath.Mul(b1.GetTransform().R, _localAnchor1 - b1.GetLocalCenter());
                Vec2 r2 = Box2DXMath.Mul(b2.GetTransform().R, _localAnchor2 - b2.GetLocalCenter());

                Vec2 p1 = b1._sweep.C + r1;
                Vec2 p2 = b2._sweep.C + r2;

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

                float length1 = _u1.Length();
                float length2 = _u2.Length();

                if (length1 > Settings.LinearSlop)
                {
                    _u1 *= 1.0f / length1;
                }
                else
                {
                    _u1.SetZero();
                }

                if (length2 > Settings.LinearSlop)
                {
                    _u2 *= 1.0f / length2;
                }
                else
                {
                    _u2.SetZero();
                }

                float C = _constant - length1 - _ratio * length2;
                linearError = Box2DXMath.Max(linearError, -C);

                C = Box2DXMath.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
                float impulse = -_pulleyMass * C;

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

                b1._sweep.C += b1._invMass * P1;
                b1._sweep.A += b1._invI * Vec2.Cross(r1, P1);
                b2._sweep.C += b2._invMass * P2;
                b2._sweep.A += b2._invI * Vec2.Cross(r2, P2);

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

            if (_limitState1 == LimitState.AtUpperLimit)
            {
                Vec2 r1 = Box2DXMath.Mul(b1.GetTransform().R, _localAnchor1 - b1.GetLocalCenter());
                Vec2 p1 = b1._sweep.C + r1;

                _u1 = p1 - s1;
                float length1 = _u1.Length();

                if (length1 > Settings.LinearSlop)
                {
                    _u1 *= 1.0f / length1;
                }
                else
                {
                    _u1.SetZero();
                }

                float C = _maxLength1 - length1;
                linearError = Box2DXMath.Max(linearError, -C);
                C           = Box2DXMath.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
                float impulse = -_limitMass1 * C;

                Vec2 P1 = -impulse * _u1;
                b1._sweep.C += b1._invMass * P1;
                b1._sweep.A += b1._invI * Vec2.Cross(r1, P1);

                b1.SynchronizeTransform();
            }

            if (_limitState2 == LimitState.AtUpperLimit)
            {
                Vec2 r2 = Box2DXMath.Mul(b2.GetTransform().R, _localAnchor2 - b2.GetLocalCenter());
                Vec2 p2 = b2._sweep.C + r2;

                _u2 = p2 - s2;
                float length2 = _u2.Length();

                if (length2 > Settings.LinearSlop)
                {
                    _u2 *= 1.0f / length2;
                }
                else
                {
                    _u2.SetZero();
                }

                float C = _maxLength2 - length2;
                linearError = Box2DXMath.Max(linearError, -C);
                C           = Box2DXMath.Clamp(C + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
                float impulse = -_limitMass2 * C;

                Vec2 P2 = -impulse * _u2;
                b2._sweep.C += b2._invMass * P2;
                b2._sweep.A += b2._invI * Vec2.Cross(r2, P2);

                b2.SynchronizeTransform();
            }

            return(linearError < Settings.LinearSlop);
        }
        internal override bool SolvePositionConstraints(float baumgarte)
        {
            // TODO_ERIN block solve with limit.

            Body b1 = _body1;
            Body b2 = _body2;

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

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

                if (_limitState == LimitState.EqualLimits)
                {
                    // Prevent large angular corrections
                    float C = Box2DX.Common.Math.Clamp(angle, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection);
                    limitImpulse = -_motorMass * C;
                    angularError = Box2DXMath.Abs(C);
                }
                else if (_limitState == LimitState.AtLowerLimit)
                {
                    float C = angle - _lowerAngle;
                    angularError = -C;

                    // Prevent large angular corrections and allow some slop.
                    C            = Box2DX.Common.Math.Clamp(C + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f);
                    limitImpulse = -_motorMass * C;
                }
                else if (_limitState == LimitState.AtUpperLimit)
                {
                    float C = angle - _upperAngle;
                    angularError = C;

                    // Prevent large angular corrections and allow some slop.
                    C            = Box2DX.Common.Math.Clamp(C - Settings.AngularSlop, 0.0f, Settings.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.
            {
                Vector2 r1 = b1.GetTransform().TransformDirection(_localAnchor1 - b1.GetLocalCenter());
                Vector2 r2 = b2.GetTransform().TransformDirection(_localAnchor2 - b2.GetLocalCenter());

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

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

                // Handle large detachment.
                float k_allowedStretch = 10.0f * Settings.LinearSlop;
                if (C.LengthSquared > k_allowedStretch * k_allowedStretch)
                {
                    // Use a particle solution (no rotation).
                    Vector2 u = C; u.Normalize();
                    float   k = invMass1 + invMass2;
                    Box2DXDebug.Assert(k > Settings.FLT_EPSILON);
                    float   m       = 1.0f / k;
                    Vector2 impulse = m * (-C);
                    float   k_beta  = 0.5f;
                    b1._sweep.C -= k_beta * invMass1 * impulse;
                    b2._sweep.C += k_beta * invMass2 * impulse;

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

                Mat22 K1 = new Mat22();
                K1.Col1.X = invMass1 + invMass2; K1.Col2.X = 0.0f;
                K1.Col1.Y = 0.0f; K1.Col2.Y = invMass1 + invMass2;

                Mat22 K2 = new Mat22();
                K2.Col1.X = invI1 * r1.Y * r1.Y; K2.Col2.X = -invI1 * r1.X * r1.Y;
                K2.Col1.Y = -invI1 * r1.X * r1.Y; K2.Col2.Y = invI1 * r1.X * r1.X;

                Mat22 K3 = new Mat22();
                K3.Col1.X = invI2 * r2.Y * r2.Y; K3.Col2.X = -invI2 * r2.X * r2.Y;
                K3.Col1.Y = -invI2 * r2.X * r2.Y; K3.Col2.Y = invI2 * r2.X * r2.X;

                Mat22   K        = K1 + K2 + K3;
                Vector2 impulse_ = K.Solve(-C);

                b1._sweep.C -= b1._invMass * impulse_;
                b1._sweep.A -= b1._invI * r1.Cross(impulse_);

                b2._sweep.C += b2._invMass * impulse_;
                b2._sweep.A += b2._invI * r2.Cross(impulse_);

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

            return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
        }
示例#30
0
        public ContactSolver(TimeStep step, Contact[] contacts, int contactCount)
        {
            Step = step;

            ConstraintCount = contactCount;
            Constraints     = new ContactConstraint[ConstraintCount];

            for (int i = 0; i < ConstraintCount; ++i)
            {
                Contact contact = contacts[i];

                Fixture  fixtureA = contact.GetFixtureA();
                Fixture  fixtureB = contact.GetFixtureB();
                Shape    shapeA   = fixtureA.GetShape();
                Shape    shapeB   = fixtureB.GetShape();
                float    radiusA  = shapeA._radius;
                float    radiusB  = shapeB._radius;
                Body     bodyA    = fixtureA.GetBody();
                Body     bodyB    = fixtureB.GetBody();
                Manifold manifold = contact.GetManifold();

                float friction    = Settings.MixFriction(fixtureA.GetFriction(), fixtureB.GetFriction());
                float restitution = Settings.MixRestitution(fixtureA.GetRestitution(), fixtureB.GetRestitution());

                Vec2  vA = bodyA._linearVelocity;
                Vec2  vB = bodyB._linearVelocity;
                float wA = bodyA._angularVelocity;
                float wB = bodyB._angularVelocity;

                Box2DXDebug.Assert(manifold.PointCount > 0);

                WorldManifold worldManifold = new WorldManifold();
                worldManifold.Initialize(manifold, bodyA.GetTransform(), radiusA, bodyB.GetTransform(), radiusB);

                ContactConstraint cc = new ContactConstraint();
                Constraints[i] = cc;
                cc.BodyA       = bodyA;
                cc.BodyB       = bodyB;
                cc.Manifold    = manifold;
                cc.Normal      = worldManifold.Normal;
                cc.PointCount  = manifold.PointCount;
                cc.Friction    = friction;
                cc.Restitution = restitution;

                cc.LocalPlaneNormal = manifold.LocalPlaneNormal;
                cc.LocalPoint       = manifold.LocalPoint;
                cc.Radius           = radiusA + radiusB;
                cc.Type             = manifold.Type;

                for (int j = 0; j < cc.PointCount; ++j)
                {
                    ManifoldPoint          cp  = manifold.Points[j];
                    ContactConstraintPoint ccp = cc.Points[j];

                    ccp.NormalImpulse  = cp.NormalImpulse;
                    ccp.TangentImpulse = cp.TangentImpulse;

                    ccp.LocalPoint = cp.LocalPoint;

                    ccp.RA = worldManifold.Points[j] - bodyA._sweep.C;
                    ccp.RB = worldManifold.Points[j] - bodyB._sweep.C;

                    float rnA = Vec2.Cross(ccp.RA, cc.Normal);
                    float rnB = Vec2.Cross(ccp.RB, cc.Normal);
                    rnA *= rnA;
                    rnB *= rnB;

                    float kNormal = bodyA._invMass + bodyB._invMass + bodyA._invI * rnA + bodyB._invI * rnB;

                    Box2DXDebug.Assert(kNormal > Settings.FLT_EPSILON);
                    ccp.NormalMass = 1.0f / kNormal;

                    float kEqualized = bodyA._mass * bodyA._invMass + bodyB._mass * bodyB._invMass;
                    kEqualized += bodyA._mass * bodyA._invI * rnA + bodyB._mass * bodyB._invI * rnB;

                    Box2DXDebug.Assert(kEqualized > Settings.FLT_EPSILON);
                    ccp.EqualizedMass = 1.0f / kEqualized;

                    Vec2 tangent = Vec2.Cross(cc.Normal, 1.0f);

                    float rtA = Vec2.Cross(ccp.RA, tangent);
                    float rtB = Vec2.Cross(ccp.RB, tangent);
                    rtA *= rtA;
                    rtB *= rtB;

                    float kTangent = bodyA._invMass + bodyB._invMass + bodyA._invI * rtA + bodyB._invI * rtB;

                    Box2DXDebug.Assert(kTangent > Settings.FLT_EPSILON);
                    ccp.TangentMass = 1.0f / kTangent;

                    // Setup a velocity bias for restitution.
                    ccp.VelocityBias = 0.0f;
                    float vRel = Vec2.Dot(cc.Normal, vB + Vec2.Cross(wB, ccp.RB) - vA - Vec2.Cross(wA, ccp.RA));
                    if (vRel < -Settings.VelocityThreshold)
                    {
                        ccp.VelocityBias = -cc.Restitution * vRel;
                    }
                }

                // If we have two points, then prepare the block solver.
                if (cc.PointCount == 2)
                {
                    ContactConstraintPoint ccp1 = cc.Points[0];
                    ContactConstraintPoint ccp2 = cc.Points[1];

                    float invMassA = bodyA._invMass;
                    float invIA    = bodyA._invI;
                    float invMassB = bodyB._invMass;
                    float invIB    = bodyB._invI;

                    float rn1A = Vec2.Cross(ccp1.RA, cc.Normal);
                    float rn1B = Vec2.Cross(ccp1.RB, cc.Normal);
                    float rn2A = Vec2.Cross(ccp2.RA, cc.Normal);
                    float rn2B = Vec2.Cross(ccp2.RB, cc.Normal);

                    float k11 = invMassA + invMassB + invIA * rn1A * rn1A + invIB * rn1B * rn1B;
                    float k22 = invMassA + invMassB + invIA * rn2A * rn2A + invIB * rn2B * rn2B;
                    float k12 = invMassA + invMassB + invIA * rn1A * rn2A + invIB * rn1B * rn2B;

                    // Ensure a reasonable condition number.
                    const float k_maxConditionNumber = 100.0f;
                    if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12))
                    {
                        // K is safe to invert.
                        cc.K.Col1.Set(k11, k12);
                        cc.K.Col2.Set(k12, k22);
                        cc.NormalMass = cc.K.Invert();
                    }
                    else
                    {
                        // The constraints are redundant, just use one.
                        // TODO_ERIN use deepest?
                        cc.PointCount = 1;
                    }
                }
            }
        }