Beispiel #1
0
        public override void InitVelocityConstraints(SolverData data)
        {
            IndexA = BodyA.IslandIndex;
            IndexB = BodyB.IslandIndex;
            LocalCenterB.Set(BodyB.Sweep.LocalCenter);
            InvMassB = BodyB.InvMass;
            InvIB    = BodyB.InvI;

            Vec2  cB = data.Positions[IndexB].C;
            float aB = data.Positions[IndexB].A;
            Vec2  vB = data.Velocities[IndexB].V;
            float wB = data.Velocities[IndexB].W;

            Rot qB = Pool.PopRot();

            qB.Set(aB);

            float mass = BodyB.Mass;

            // Frequency
            float omega = 2.0f * MathUtils.PI * Frequency;

            // 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.
            float h = data.Step.Dt;

            Debug.Assert(d + h * k > Settings.EPSILON);
            m_gamma = h * (d + h * k);
            if (m_gamma != 0.0f)
            {
                m_gamma = 1.0f / m_gamma;
            }
            m_beta = h * k * m_gamma;

            Vec2 temp = Pool.PopVec2();

            // Compute the effective mass matrix.
            Rot.MulToOutUnsafe(qB, temp.Set(m_localAnchorB).SubLocal(LocalCenterB), RB);

            // 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]
            Mat22 K = Pool.PopMat22();

            K.Ex.X = InvMassB + InvIB * RB.Y * RB.Y + m_gamma;
            K.Ex.Y = (-InvIB) * RB.X * RB.Y;
            K.Ey.X = K.Ex.Y;
            K.Ey.Y = InvMassB + InvIB * RB.X * RB.X + m_gamma;

            K.InvertToOut(m_mass);

            m_C.Set(cB).AddLocal(RB).SubLocal(m_targetA);
            m_C.MulLocal(m_beta);

            // Cheat with some damping
            wB *= 0.98f;

            if (data.Step.WarmStarting)
            {
                m_impulse.MulLocal(data.Step.DtRatio);
                vB.X += InvMassB * m_impulse.X;
                vB.Y += InvMassB * m_impulse.Y;
                wB   += InvIB * Vec2.Cross(RB, m_impulse);
            }
            else
            {
                m_impulse.SetZero();
            }

            data.Velocities[IndexB].V.Set(vB);
            data.Velocities[IndexB].W = wB;

            Pool.PushVec2(1);
            Pool.PushMat22(1);
            Pool.PushRot(1);
        }
Beispiel #2
0
        /// <seealso cref="Joint.initVelocityConstraints(TimeStep)"></seealso>
        public override void InitVelocityConstraints(SolverData data)
        {
            IndexA = BodyA.IslandIndex;
            IndexB = BodyB.IslandIndex;
            LocalCenterA.Set(BodyA.Sweep.LocalCenter);
            LocalCenterB.Set(BodyB.Sweep.LocalCenter);
            InvMassA = BodyA.InvMass;
            InvMassB = BodyB.InvMass;
            InvIA    = BodyA.InvI;
            InvIB    = BodyB.InvI;

            float aA = data.Positions[IndexA].A;
            Vec2  vA = data.Velocities[IndexA].V;
            float wA = data.Velocities[IndexA].W;

            float aB = data.Positions[IndexB].A;
            Vec2  vB = data.Velocities[IndexB].V;
            float wB = data.Velocities[IndexB].W;


            Vec2 temp = Pool.PopVec2();
            Rot  qA   = Pool.PopRot();
            Rot  qB   = Pool.PopRot();

            qA.Set(aA);
            qB.Set(aB);

            // Compute the effective mass matrix.
            Rot.MulToOutUnsafe(qA, temp.Set(m_localAnchorA).SubLocal(LocalCenterA), RA);
            Rot.MulToOutUnsafe(qB, temp.Set(m_localAnchorB).SubLocal(LocalCenterB), RB);

            // J = [-I -r1_skew I r2_skew]
            // [ 0 -1 0 1]
            // r_skew = [-ry; rx]

            // Matlab
            // K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
            // [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
            // [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]

            float mA = InvMassA, mB = InvMassB;
            float iA = InvIA, iB = InvIB;

            Mat22 K = Pool.PopMat22();

            K.Ex.X = mA + mB + iA * RA.Y * RA.Y + iB * RB.Y * RB.Y;
            K.Ex.Y = (-iA) * RA.X * RA.Y - iB * RB.X * RB.Y;
            K.Ey.X = K.Ex.Y;
            K.Ey.Y = mA + mB + iA * RA.X * RA.X + iB * RB.X * RB.X;

            K.InvertToOut(LinearMass);

            AngularMass = iA + iB;
            if (AngularMass > 0.0f)
            {
                AngularMass = 1.0f / AngularMass;
            }

            if (data.Step.WarmStarting)
            {
                // Scale impulses to support a variable time step.
                m_linearImpulse.MulLocal(data.Step.DtRatio);
                m_angularImpulse *= data.Step.DtRatio;

                Vec2 P = Pool.PopVec2();
                P.Set(m_linearImpulse);

                temp.Set(P).MulLocal(mA);
                vA.SubLocal(temp);
                wA -= iA * (Vec2.Cross(RA, P) + m_angularImpulse);

                temp.Set(P).MulLocal(mB);
                vB.AddLocal(temp);
                wB += iB * (Vec2.Cross(RB, P) + m_angularImpulse);

                Pool.PushVec2(1);
            }
            else
            {
                m_linearImpulse.SetZero();
                m_angularImpulse = 0.0f;
            }
            data.Velocities[IndexA].V.Set(vA);
            if (data.Velocities[IndexA].W != wA)
            {
                Debug.Assert(data.Velocities[IndexA].W != wA);
            }
            data.Velocities[IndexA].W = wA;
            data.Velocities[IndexB].V.Set(vB);
            data.Velocities[IndexB].W = wB;

            Pool.PushRot(2);
            Pool.PushVec2(1);
            Pool.PushMat22(1);
        }