Ejemplo n.º 1
0
        /// <summary>
        ///     Inits the velocity constraints using the specified step
        /// </summary>
        /// <param name="step">The step</param>
        internal override void InitVelocityConstraints(TimeStep step)
        {
            Body body2 = Body2;

            float body2Mass = body2.GetMass();

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

            // Damping coefficient
            float coefficient = 2.0f * body2Mass * DampingRatio * omega;

            // Spring stiffness
            float stiffness = body2Mass * (omega * omega);

            // magic formulas
            // gamma has units of inverse mass.
            // beta has units of inverse time.
            Box2DxDebug.Assert(coefficient + step.Dt * stiffness > Settings.FltEpsilon);
            Gamma = 1.0f / (step.Dt * (coefficient + step.Dt * stiffness));
            Beta  = step.Dt * stiffness * Gamma;

            // Compute the effective mass matrix.
            Vec2 effectiveMass = Math.Mul(body2.GetXForm().R, LocalAnchor - body2.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 = body2.InvMass;
            float invI    = body2.InvI;

            Mat22 k1 = new Mat22
            {
                Col1 = new Vec2(invMass, 0.0f),
                Col2 = new Vec2(0.0f, invMass)
            };

            Mat22 k2 = new Mat22
            {
                Col1 = new Vec2(invI * effectiveMass.Y * effectiveMass.Y, -invI * effectiveMass.X * effectiveMass.Y),
                Col2 = new Vec2(-invI * effectiveMass.X * effectiveMass.Y, invI * effectiveMass.X * effectiveMass.X)
            };

            Mat22 k = k1 + k2;

            k.Col1.X += Gamma;
            k.Col2.Y += Gamma;

            Mass = k.GetInverse();

            C = body2.Sweep.C + effectiveMass - Target;

            // Cheat with some damping
            body2.AngularVelocity *= 0.98f;

            // Warm starting.
            Impulse *= step.DtRatio;
            body2.LinearVelocity  += invMass * Impulse;
            body2.AngularVelocity += invI * Vec2.Cross(effectiveMass, Impulse);
        }
Ejemplo n.º 2
0
        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.
            Box2DNetDebug.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);
        }
        public void PreStep(float inv_dt)
        {
            // Pre-compute anchors, mass matrix, and bias.
            //计算相对质量
            Mat22 Rot1 = new Mat22(body1.rotation);
            Mat22 Rot2 = new Mat22(body2.rotation);

            r1 = Rot1 * localAnchor1;
            r2 = Rot2 * localAnchor2;

            //相对质量计算公式
            // deltaV = deltaV0 + K * impulse
            // invM = [(1/m1 + 1/m2) * I - 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]
            Mat22 K1 = new Mat22();

            K1.ex.x = body1.invMass + body2.invMass;
            K1.ey.x = 0.0f;
            K1.ex.y = 0.0f;
            K1.ey.y = body1.invMass + body2.invMass;

            Mat22 K2 = new Mat22();

            K2.ex.x = body1.invI * r1.y * r1.y;
            K2.ey.x = -body1.invI * r1.x * r1.y;
            K2.ex.y = -body1.invI * r1.x * r1.y;
            K2.ey.y = body1.invI * r1.x * r1.x;

            Mat22 K3 = new Mat22();

            K3.ex.x = body2.invI * r2.y * r2.y;
            K3.ey.x = -body2.invI * r2.x * r2.y;
            K3.ex.y = -body2.invI * r2.x * r2.y;
            K3.ey.y = body2.invI * r2.x * r2.x;

            Mat22 K = K1 + K2 + K3;

            K.ex.x += softness;
            K.ey.y += softness;

            M = K.GetInverse();

            Vector2f p1 = body1.position + r1;
            Vector2f p2 = body2.position + r2;
            Vector2f dp = p2 - p1;

            if (World.positionCorrection)
            {
                bias = -biasFactor * inv_dt * dp;
            }
            else
            {
                bias.Set(0, 0);
            }

            if (World.warmStarting)
            {
                // Apply accumulated impulse.
                body1.velocity        -= body1.invMass * P;
                body1.angularVelocity -= body1.invI * MyMath.Cross(r1, P);

                body2.velocity        += body2.invMass * P;
                body2.angularVelocity += body2.invI * MyMath.Cross(r2, P);
            }
            else
            {
                P.Set(0, 0);
            }
        }