Ejemplo n.º 1
0
        /// <summary>
        ///     Solves the velocity constraints using the specified step
        /// </summary>
        /// <param name="step">The step</param>
        internal override void SolveVelocityConstraints(TimeStep step)
        {
            Body b1 = Body1;
            Body b2 = Body2;

            Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, LocalAnchor1 - b1.GetLocalCenter());
            Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().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 = Box2DXMath.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 = Box2DXMath.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 = Box2DXMath.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);
            }
        }
Ejemplo n.º 2
0
        internal override void SolveVelocityConstraints(TimeStep step)
        {
            Body b1 = _body1;
            Body b2 = _body2;

            Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
            Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().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 force    = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * _pulleyMass * Cdot;
                float oldForce = _force;
                _force = Box2DXMath.Max(0.0f, _force + force);
                force  = _force - oldForce;

                Vec2 P1 = -Settings.FORCE_SCALE(step.Dt) * force * _u1;
                Vec2 P2 = -Settings.FORCE_SCALE(step.Dt) * _ratio * force * _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 force    = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * _limitMass1 * Cdot;
                float oldForce = _limitForce1;
                _limitForce1 = Box2DXMath.Max(0.0f, _limitForce1 + force);
                force        = _limitForce1 - oldForce;

                Vec2 P1 = -Settings.FORCE_SCALE(step.Dt) * force * _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 force    = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * _limitMass2 * Cdot;
                float oldForce = _limitForce2;
                _limitForce2 = Box2DXMath.Max(0.0f, _limitForce2 + force);
                force        = _limitForce2 - oldForce;

                Vec2 P2 = -Settings.FORCE_SCALE(step.Dt) * force * _u2;
                b2._linearVelocity  += b2._invMass * P2;
                b2._angularVelocity += b2._invI * Vec2.Cross(r2, P2);
            }
        }
Ejemplo n.º 3
0
        public override void Step(Settings settings)
        {
            //B2_NOT_USED(settings);

            if (_automated == true)
            {
                int actionCount = Box2DXMath.Max(1, k_actorCount >> 2);

                for (int i = 0; i < actionCount; ++i)
                {
                    Action();
                }
            }

            _broadPhase.Commit();

            for (int i = 0; i < k_actorCount; ++i)
            {
                Actor actor = _actors[i];
                if (actor.proxyId == PairManager.NullProxy)
                {
                    continue;
                }

                Color c = new Color();
                switch (actor.overlapCount)
                {
                case 0:
                    c.R = 0.9f; c.G = 0.9f; c.B = 0.9f;
                    break;

                case 1:
                    c.R = 0.6f; c.G = 0.9f; c.B = 0.6f;
                    break;

                default:
                    c.R = 0.9f; c.G = 0.6f; c.B = 0.6f;
                    break;
                }

                OpenGLDebugDraw.DrawAABB(actor.aabb, c);
            }

            StringBuilder strBld = new StringBuilder();

            strBld.AppendFormat("overlaps = {0}, exact = {1}, diff = {2}",
                                new object[] { _overlapCount, _overlapCountExact, _overlapCount - _overlapCountExact });
            OpenGLDebugDraw.DrawString(5, 30, strBld.ToString());
            Validate();

            ++_stepCount;
        }
Ejemplo n.º 4
0
        /// <summary>
        ///     Describes whether this instance solve position constraints
        /// </summary>
        /// <param name="baumgarte">The baumgarte</param>
        /// <returns>The bool</returns>
        internal override bool SolvePositionConstraints(float baumgarte)
        {
            Body body1 = Body1;
            Body body2 = Body2;

            Vec2 groundAnchor1 = Ground.GetXForm().Position + GroundAnchor1;
            Vec2 groundAnchor2 = Ground.GetXForm().Position + GroundAnchor2;

            float linearError = 0.0f;

            if (State == LimitState.AtUpperLimit)
            {
                Vec2 mulR1 = Box2DXMath.Mul(body1.GetXForm().R, LocalAnchor1 - body1.GetLocalCenter());
                Vec2 mulR2 = Box2DXMath.Mul(body2.GetXForm().R, LocalAnchor2 - body2.GetLocalCenter());

                Vec2 body1SweepC = body1.Sweep.C + mulR1;
                Vec2 body2SweepC = body2.Sweep.C + mulR2;

                // Get the pulley axes.
                U1 = body1SweepC - groundAnchor1;
                U2 = body2SweepC - groundAnchor2;

                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;

                body1.Sweep.C += body1.InvMass * p1;
                body1.Sweep.A += body1.InvI * Vec2.Cross(mulR1, p1);
                body2.Sweep.C += body2.InvMass * p2;
                body2.Sweep.A += body2.InvI * Vec2.Cross(mulR2, p2);

                body1.SynchronizeTransform();
                body2.SynchronizeTransform();
            }

            if (LimitState1 == LimitState.AtUpperLimit)
            {
                Vec2 mulR1       = Box2DXMath.Mul(body1.GetXForm().R, LocalAnchor1 - body1.GetLocalCenter());
                Vec2 body1SweepC = body1.Sweep.C + mulR1;

                U1 = body1SweepC - groundAnchor1;
                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;
                body1.Sweep.C += body1.InvMass * p1;
                body1.Sweep.A += body1.InvI * Vec2.Cross(mulR1, p1);

                body1.SynchronizeTransform();
            }

            if (LimitState2 == LimitState.AtUpperLimit)
            {
                Vec2 mulR2       = Box2DXMath.Mul(body2.GetXForm().R, LocalAnchor2 - body2.GetLocalCenter());
                Vec2 body2SweepC = body2.Sweep.C + mulR2;

                U2 = body2SweepC - groundAnchor2;
                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;
                body2.Sweep.C += body2.InvMass * p2;
                body2.Sweep.A += body2.InvI * Vec2.Cross(mulR2, p2);

                body2.SynchronizeTransform();
            }

            return(linearError < Settings.LinearSlop);
        }
Ejemplo n.º 5
0
        /// <summary>
        ///     Describes whether this instance solve position constraints
        /// </summary>
        /// <param name="baumgarte">The baumgarte</param>
        /// <returns>The bool</returns>
        internal override bool SolvePositionConstraints(float baumgarte)
        {
            Body body1 = Body1;
            Body body2 = Body2;

            Vec2  body1SweepC = body1.Sweep.C;
            float body1SweepA = body1.Sweep.A;

            Vec2  body2SweepC = body2.Sweep.C;
            float body2SweepA = body2.Sweep.A;

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

            var mat22R1 = new Mat22(body1SweepA);
            var mat22R2 = new Mat22(body2SweepA);

            Vec2 r1       = Box2DXMath.Mul(mat22R1, LocalAnchor1 - LocalCenter1);
            Vec2 r2       = Box2DXMath.Mul(mat22R2, LocalAnchor2 - LocalCenter2);
            Vec2 distance = body2SweepC + r2 - body1SweepC - r1;

            if (IsLimitEnabled)
            {
                Axis = Box2DXMath.Mul(mat22R1, LocalXAxis1);

                a1 = Vec2.Cross(distance + r1, Axis);
                A2 = Vec2.Cross(r2, Axis);

                float translation = Vec2.Dot(Axis, distance);
                if (Box2DXMath.Abs(UpperLimit - LowerLimit) < 2.0f * Settings.LinearSlop)
                {
                    // Prevent large angular corrections
                    c2          = Box2DXMath.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
                    linearError = Box2DXMath.Abs(translation);
                    active      = true;
                }
                else if (translation <= LowerLimit)
                {
                    // Prevent large linear corrections and allow some slop.
                    c2 = Box2DXMath.Clamp(translation - LowerLimit + Settings.LinearSlop,
                                          -Settings.MaxLinearCorrection, 0.0f);
                    linearError = LowerLimit - translation;
                    active      = true;
                }
                else if (translation >= UpperLimit)
                {
                    // Prevent large linear corrections and allow some slop.
                    c2 = Box2DXMath.Clamp(translation - UpperLimit - Settings.LinearSlop, 0.0f,
                                          Settings.MaxLinearCorrection);
                    linearError = translation - UpperLimit;
                    active      = true;
                }
            }

            Perp = Box2DXMath.Mul(mat22R1, LocalYAxis1);

            s1 = Vec2.Cross(distance + r1, Perp);
            s2 = Vec2.Cross(r2, Perp);

            Vec3 impulse;
            Vec2 c1 = new Vec2();

            c1.X = Vec2.Dot(Perp, distance);
            c1.Y = body2SweepA - body1SweepA - refAngle;

            linearError  = Box2DXMath.Max(linearError, Box2DXMath.Abs(c1.X));
            angularError = Box2DXMath.Abs(c1.Y);

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

                Vec3 c = new Vec3();
                c.X = c1.X;
                c.Y = c1.Y;
                c.Z = c2;

                impulse = K.Solve33(-c);
            }
            else
            {
                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 k22 = i1 + i2;

                K.Col1.Set(k11, k12, 0.0f);
                K.Col2.Set(k12, k22, 0.0f);

                Vec2 impulse1 = K.Solve22(-c1);
                impulse.X = impulse1.X;
                impulse.Y = impulse1.Y;
                impulse.Z = 0.0f;
            }

            Vec2  p  = impulse.X * Perp + impulse.Z * Axis;
            float l1 = impulse.X * s1 + impulse.Y + impulse.Z * a1;
            float l2 = impulse.X * s2 + impulse.Y + impulse.Z * A2;

            body1SweepC -= InvMass1 * p;
            body1SweepA -= InvI1 * l1;
            body2SweepC += InvMass2 * p;
            body2SweepA += InvI2 * l2;

            // TODO_ERIN remove need for this.
            body1.Sweep.C = body1SweepC;
            body1.Sweep.A = body1SweepA;
            body2.Sweep.C = body2SweepC;
            body2.Sweep.A = body2SweepA;
            body1.SynchronizeTransform();
            body2.SynchronizeTransform();

            return(linearError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
        }
Ejemplo n.º 6
0
        /// <summary>
        ///     Solves the velocity constraints using the specified step
        /// </summary>
        /// <param name="step">The step</param>
        internal override void SolveVelocityConstraints(TimeStep step)
        {
            Body b1 = Body1;
            Body b2 = Body2;

            Vec2  v1 = b1.LinearVelocity;
            float w1 = b1.AngularVelocity;
            Vec2  v2 = b2.LinearVelocity;
            float w2 = b2.AngularVelocity;

            // Solve linear motor constraint.
            if (IsMotorEnabled && LimitState != LimitState.EqualLimits)
            {
                float cdot       = Vec2.Dot(Axis, v2 - v1) + A2 * w2 - a1 * w1;
                float impulse    = MotorMass * (motorSpeedx - cdot);
                float oldImpulse = MotorForce;
                float maxImpulse = step.Dt * MaxMotorForce;
                MotorForce = Box2DXMath.Clamp(MotorForce + impulse, -maxImpulse, maxImpulse);
                impulse    = MotorForce - oldImpulse;

                Vec2  p  = impulse * Axis;
                float l1 = impulse * a1;
                float l2 = impulse * A2;

                v1 -= InvMass1 * p;
                w1 -= InvI1 * l1;

                v2 += InvMass2 * p;
                w2 += InvI2 * l2;
            }

            Vec2 cdot1;

            cdot1.X = Vec2.Dot(Perp, v2 - v1) + s2 * w2 - s1 * w1;
            cdot1.Y = w2 - w1;

            if (IsLimitEnabled && LimitState != LimitState.InactiveLimit)
            {
                // Solve prismatic and limit constraint in block form.
                float cdot2;
                cdot2 = Vec2.Dot(Axis, v2 - v1) + A2 * w2 - a1 * w1;
                Vec3 cdot = new Vec3(cdot1.X, cdot1.Y, cdot2);

                Vec3 f1 = Impulse;
                Vec3 df = K.Solve33(-cdot);
                Impulse += df;

                if (LimitState == LimitState.AtLowerLimit)
                {
                    Impulse.Z = Box2DXMath.Max(Impulse.Z, 0.0f);
                }
                else if (LimitState == LimitState.AtUpperLimit)
                {
                    Impulse.Z = Box2DXMath.Min(Impulse.Z, 0.0f);
                }

                // f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
                Vec2 b   = -cdot1 - (Impulse.Z - f1.Z) * new Vec2(K.Col3.X, K.Col3.Y);
                Vec2 f2R = K.Solve22(b) + new Vec2(f1.X, f1.Y);
                Impulse.X = f2R.X;
                Impulse.Y = f2R.Y;

                df = Impulse - f1;

                Vec2  p  = df.X * Perp + df.Z * Axis;
                float l1 = df.X * s1 + df.Y + df.Z * a1;
                float l2 = df.X * s2 + df.Y + df.Z * A2;

                v1 -= InvMass1 * p;
                w1 -= InvI1 * l1;

                v2 += InvMass2 * p;
                w2 += InvI2 * l2;
            }
            else
            {
                // Limit is inactive, just solve the prismatic constraint in block form.
                Vec2 df = K.Solve22(-cdot1);
                Impulse.X += df.X;
                Impulse.Y += df.Y;

                Vec2  p  = df.X * Perp;
                float l1 = df.X * s1 + df.Y;
                float l2 = df.X * s2 + df.Y;

                v1 -= InvMass1 * p;
                w1 -= InvI1 * l1;

                v2 += InvMass2 * p;
                w2 += InvI2 * l2;
            }

            b1.LinearVelocity  = v1;
            b1.AngularVelocity = w1;
            b2.LinearVelocity  = v2;
            b2.AngularVelocity = w2;
        }
Ejemplo n.º 7
0
        internal override bool SolvePositionConstraints(float baumgarte)
        {
            Body b1 = _body1;
            Body b2 = _body2;

            Vec2 s1 = _ground.GetXForm().Position + _groundAnchor1;
            Vec2 s2 = _ground.GetXForm().Position + _groundAnchor2;

            float linearError = 0.0f;

            if (_state == LimitState.AtUpperLimit)
            {
                Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
                Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().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.GetXForm().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.GetXForm().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);
        }
Ejemplo n.º 8
0
        internal override bool SolvePositionConstraints()
        {
            Body b1 = _body1;
            Body b2 = _body2;

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

            Vec2 r1  = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
            Vec2 r2  = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
            Vec2 p1  = b1._sweep.C + r1;
            Vec2 p2  = b2._sweep.C + r2;
            Vec2 d   = p2 - p1;
            Vec2 ay1 = Box2DXMath.Mul(b1.GetXForm().R, _localYAxis1);

            // Solve linear (point-to-line) constraint.
            float linearC = Vec2.Dot(ay1, d);

            // Prevent overly large corrections.
            linearC = Box2DXMath.Clamp(linearC, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
            float linearImpulse = -_linearMass * linearC;

            b1._sweep.C += (invMass1 * linearImpulse) * _linearJacobian.Linear1;
            b1._sweep.A += invI1 * linearImpulse * _linearJacobian.Angular1;
            //b1->SynchronizeTransform(); // updated by angular constraint
            b2._sweep.C += (invMass2 * linearImpulse) * _linearJacobian.Linear2;
            b2._sweep.A += invI2 * linearImpulse * _linearJacobian.Angular2;
            //b2->SynchronizeTransform(); // updated by angular constraint

            float positionError = Box2DXMath.Abs(linearC);

            // Solve angular constraint.
            float angularC = b2._sweep.A - b1._sweep.A - _refAngle;

            // Prevent overly large corrections.
            angularC = Box2DXMath.Clamp(angularC, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection);
            float angularImpulse = -_angularMass * angularC;

            b1._sweep.A -= b1._invI * angularImpulse;
            b2._sweep.A += b2._invI * angularImpulse;

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

            float angularError = Box2DXMath.Abs(angularC);

            // Solve linear limit constraint.
            if (_enableLimit && _limitState != LimitState.InactiveLimit)
            {
                Vec2 r1_ = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
                Vec2 r2_ = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());
                Vec2 p1_ = b1._sweep.C + r1_;
                Vec2 p2_ = b2._sweep.C + r2_;
                Vec2 d_  = p2_ - p1_;
                Vec2 ax1 = Box2DXMath.Mul(b1.GetXForm().R, _localXAxis1);

                float translation  = Vec2.Dot(ax1, d_);
                float limitImpulse = 0.0f;

                if (_limitState == LimitState.EqualLimits)
                {
                    // Prevent large angular corrections
                    float limitC = Box2DXMath.Clamp(translation, -Settings.MaxLinearCorrection, Settings.MaxLinearCorrection);
                    limitImpulse  = -_motorMass * limitC;
                    positionError = Box2DXMath.Max(positionError, Box2DXMath.Abs(angularC));
                }
                else if (_limitState == LimitState.AtLowerLimit)
                {
                    float limitC = translation - _lowerTranslation;
                    positionError = Box2DXMath.Max(positionError, -limitC);

                    // Prevent large linear corrections and allow some slop.
                    limitC       = Box2DXMath.Clamp(limitC + Settings.LinearSlop, -Settings.MaxLinearCorrection, 0.0f);
                    limitImpulse = -_motorMass * limitC;
                    float oldLimitImpulse = _limitPositionImpulse;
                    _limitPositionImpulse = Box2DXMath.Max(_limitPositionImpulse + limitImpulse, 0.0f);
                    limitImpulse          = _limitPositionImpulse - oldLimitImpulse;
                }
                else if (_limitState == LimitState.AtUpperLimit)
                {
                    float limitC = translation - _upperTranslation;
                    positionError = Box2DXMath.Max(positionError, limitC);

                    // Prevent large linear corrections and allow some slop.
                    limitC       = Box2DXMath.Clamp(limitC - Settings.LinearSlop, 0.0f, Settings.MaxLinearCorrection);
                    limitImpulse = -_motorMass * limitC;
                    float oldLimitImpulse = _limitPositionImpulse;
                    _limitPositionImpulse = Box2DXMath.Min(_limitPositionImpulse + limitImpulse, 0.0f);
                    limitImpulse          = _limitPositionImpulse - oldLimitImpulse;
                }

                b1._sweep.C += (invMass1 * limitImpulse) * _motorJacobian.Linear1;
                b1._sweep.A += invI1 * limitImpulse * _motorJacobian.Angular1;
                b2._sweep.C += (invMass2 * limitImpulse) * _motorJacobian.Linear2;
                b2._sweep.A += invI2 * limitImpulse * _motorJacobian.Angular2;

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

            return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
        }
Ejemplo n.º 9
0
        internal override void SolveVelocityConstraints(TimeStep step)
        {
            Body b1 = _body1;
            Body b2 = _body2;

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

            // Solve linear constraint.
            float linearCdot = _linearJacobian.Compute(b1._linearVelocity, b1._angularVelocity, b2._linearVelocity, b2._angularVelocity);
            float force      = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * _linearMass * linearCdot;

            _force += force;

            float P = Settings.FORCE_SCALE(step.Dt) * force;

            b1._linearVelocity  += (invMass1 * P) * _linearJacobian.Linear1;
            b1._angularVelocity += invI1 * P * _linearJacobian.Angular1;

            b2._linearVelocity  += (invMass2 * P) * _linearJacobian.Linear2;
            b2._angularVelocity += invI2 * P * _linearJacobian.Angular2;

            // Solve angular constraint.
            float angularCdot = b2._angularVelocity - b1._angularVelocity;
            float torque      = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * _angularMass * angularCdot;

            _torque += torque;

            float L = Settings.FORCE_SCALE(step.Dt) * torque;

            b1._angularVelocity -= invI1 * L;
            b2._angularVelocity += invI2 * L;

            // Solve linear motor constraint.
            if (_enableMotor && _limitState != LimitState.EqualLimits)
            {
                float motorCdot     = _motorJacobian.Compute(b1._linearVelocity, b1._angularVelocity, b2._linearVelocity, b2._angularVelocity) - _motorSpeed;
                float motorForce    = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * _motorMass * motorCdot;
                float oldMotorForce = _motorForce;
                _motorForce = Box2DXMath.Clamp(_motorForce + motorForce, -_maxMotorForce, _maxMotorForce);
                motorForce  = _motorForce - oldMotorForce;

                float P_ = Settings.FORCE_SCALE(step.Dt) * motorForce;
                b1._linearVelocity  += (invMass1 * P_) * _motorJacobian.Linear1;
                b1._angularVelocity += invI1 * P_ * _motorJacobian.Angular1;

                b2._linearVelocity  += (invMass2 * P_) * _motorJacobian.Linear2;
                b2._angularVelocity += invI2 * P_ * _motorJacobian.Angular2;
            }

            // Solve linear limit constraint.
            if (_enableLimit && _limitState != LimitState.InactiveLimit)
            {
                float limitCdot  = _motorJacobian.Compute(b1._linearVelocity, b1._angularVelocity, b2._linearVelocity, b2._angularVelocity);
                float limitForce = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * _motorMass * limitCdot;

                if (_limitState == LimitState.EqualLimits)
                {
                    _limitForce += limitForce;
                }
                else if (_limitState == LimitState.AtLowerLimit)
                {
                    float oldLimitForce = _limitForce;
                    _limitForce = Box2DXMath.Max(_limitForce + limitForce, 0.0f);
                    limitForce  = _limitForce - oldLimitForce;
                }
                else if (_limitState == LimitState.AtUpperLimit)
                {
                    float oldLimitForce = _limitForce;
                    _limitForce = Box2DXMath.Min(_limitForce + limitForce, 0.0f);
                    limitForce  = _limitForce - oldLimitForce;
                }

                float P_ = Settings.FORCE_SCALE(step.Dt) * limitForce;

                b1._linearVelocity  += (invMass1 * P_) * _motorJacobian.Linear1;
                b1._angularVelocity += invI1 * P_ * _motorJacobian.Angular1;

                b2._linearVelocity  += (invMass2 * P_) * _motorJacobian.Linear2;
                b2._angularVelocity += invI2 * P_ * _motorJacobian.Angular2;
            }
        }
Ejemplo n.º 10
0
        internal override bool SolvePositionConstraints()
        {
            Body b1 = _body1;
            Body b2 = _body2;

            float positionError = 0.0f;

            // Solve point-to-point position error.
            Vec2 r1 = Box2DXMath.Mul(b1.GetXForm().R, _localAnchor1 - b1.GetLocalCenter());
            Vec2 r2 = Box2DXMath.Mul(b2.GetXForm().R, _localAnchor2 - b2.GetLocalCenter());

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

            positionError = ptpC.Length();

            // Prevent overly large corrections.
            //b2Vec2 dpMax(b2_maxLinearCorrection, b2_maxLinearCorrection);
            //ptpC = b2Clamp(ptpC, -dpMax, dpMax);

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

            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;
            Vec2  impulse = K.Solve(-ptpC);

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

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

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

            // Handle limits.
            float angularError = 0.0f;

            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 limitC = Box2DXMath.Clamp(angle, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection);
                    limitImpulse = -_motorMass * limitC;
                    angularError = Box2DXMath.Abs(limitC);
                }
                else if (_limitState == LimitState.AtLowerLimit)
                {
                    float limitC = angle - _lowerAngle;
                    angularError = Box2DXMath.Max(0.0f, -limitC);

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

                    // Prevent large angular corrections and allow some slop.
                    limitC       = Box2DXMath.Clamp(limitC - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection);
                    limitImpulse = -_motorMass * limitC;
                    float oldLimitImpulse = _limitPositionImpulse;
                    _limitPositionImpulse = Box2DXMath.Min(_limitPositionImpulse + limitImpulse, 0.0f);
                    limitImpulse          = _limitPositionImpulse - oldLimitImpulse;
                }

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

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

            return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop);
        }
Ejemplo n.º 11
0
        internal override void SolveVelocityConstraints(TimeStep step)
        {
            Body b1 = _body1;
            Body b2 = _body2;

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

            // Solve point-to-point constraint
            Vec2 pivotCdot = b2._linearVelocity + Vec2.Cross(b2._angularVelocity, r2) - b1._linearVelocity -
                             Vec2.Cross(b1._angularVelocity, r1);
            Vec2 pivotForce = -Settings.FORCE_INV_SCALE(step.Inv_Dt) * Box2DXMath.Mul(_pivotMass, pivotCdot);

#if B2_TOI_JOINTS
            if (step.WarmStarting)
            {
                _pivotForce += pivotForce;
                _lastWarmStartingPivotForce = _pivotForce;
            }
            else
            {
                _pivotForce = _lastWarmStartingPivotForce;
                //Do not update warm starting value!
            }
#else
            _pivotForce += pivotForce;
#endif

            Vec2 P = Settings.FORCE_SCALE(step.Dt) * pivotForce;
            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);

            if (_enableMotor && _limitState != LimitState.EqualLimits)
            {
                float motorCdot     = b2._angularVelocity - b1._angularVelocity - _motorSpeed;
                float motorForce    = -step.Inv_Dt * _motorMass * motorCdot;
                float oldMotorForce = _motorForce;
                _motorForce = Box2DXMath.Clamp(_motorForce + motorForce, -_maxMotorTorque, _maxMotorTorque);
                motorForce  = _motorForce - oldMotorForce;

                float P_ = step.Dt * motorForce;
                b1._angularVelocity -= b1._invI * P_;
                b2._angularVelocity += b2._invI * P_;
            }

            if (_enableLimit && _limitState != LimitState.InactiveLimit)
            {
                float limitCdot  = b2._angularVelocity - b1._angularVelocity;
                float limitForce = -step.Inv_Dt * _motorMass * limitCdot;

                if (_limitState == LimitState.EqualLimits)
                {
                    _limitForce += limitForce;
                }
                else if (_limitState == LimitState.AtLowerLimit)
                {
                    float oldLimitForce = _limitForce;
                    _limitForce = Box2DXMath.Max(_limitForce + limitForce, 0.0f);
                    limitForce  = _limitForce - oldLimitForce;
                }
                else if (_limitState == LimitState.AtUpperLimit)
                {
                    float oldLimitForce = _limitForce;
                    _limitForce = Box2DXMath.Min(_limitForce + limitForce, 0.0f);
                    limitForce  = _limitForce - oldLimitForce;
                }

                float P_ = step.Dt * limitForce;
                b1._angularVelocity -= b1._invI * P_;
                b2._angularVelocity += b2._invI * P_;
            }
        }