예제 #1
0
        public static b2Rot Create()
        {
            var rot = new b2Rot();

            rot.SetIdentity();
            return(rot);
        }
예제 #2
0
        public static b2Vec2 b2Mul(b2Rot q, b2Vec2 v)
        {
            b2Vec2 b;

            b.x = q.c * v.x - q.s * v.y;
            b.y = q.s * v.x + q.c * v.y;
            return(b);
        }
예제 #3
0
 public static b2Vec2 b2MulT(b2Rot q, b2Vec2 v)
 {
     float x = q.c * v.x + q.s * v.y;
     float y =  -q.s * v.x + q.c * v.y;
     b2Vec2 b = b2Vec2.Zero;
     b.Set(x, y);
     return b;
 }
예제 #4
0
        public static b2Vec2 b2MulT(ref b2Rot q, ref b2Vec2 v)
        {
            b2Vec2 b;

            b.x = q.c * v.x + q.s * v.y;
            b.y = -q.s * v.x + q.c * v.y;
            return(b);
        }
예제 #5
0
 public static b2Rot b2MulT(b2Rot q, b2Rot r)
 {
     // [ qc qs] * [rc -rs] = [qc*rc+qs*rs -qc*rs+qs*rc]
     // [-qs qc]   [rs  rc]   [-qs*rc+qc*rs qs*rs+qc*rc]
     // s = qc * rs - qs * rc
     // c = qc * rc + qs * rs
     b2Rot qr = b2Rot.Create();
     qr.s = q.c * r.s - q.s * r.c;
     qr.c = q.c * r.c + q.s * r.s;
     return qr;
 }
예제 #6
0
        public static b2Rot b2Mul(b2Rot q, b2Rot r)
        {
            // [qc -qs] * [rc -rs] = [qc*rc-qs*rs -qc*rs-qs*rc]
            // [qs  qc]   [rs  rc]   [qs*rc+qc*rs -qs*rs+qc*rc]
            // s = qs * rc + qc * rs
            // c = qc * rc - qs * rs
            b2Rot qr;

            qr.s = q.s * r.c + q.c * r.s;
            qr.c = q.c * r.c - q.s * r.s;
            return(qr);
        }
예제 #7
0
        public override bool SolvePositionConstraints(b2SolverData data)
        {
            b2Vec2 cA = m_bodyA.InternalPosition.c;
            float aA = m_bodyA.InternalPosition.a;
            b2Vec2 cB = m_bodyB.InternalPosition.c;
            float aB = m_bodyB.InternalPosition.a;

            b2Rot qA = new b2Rot(aA);
            b2Rot qB = new b2Rot(aB);

            b2Vec2 rA = b2Math.b2Mul(qA, m_localAnchorA - m_localCenterA);
            b2Vec2 rB = b2Math.b2Mul(qB, m_localAnchorB - m_localCenterB);

            // Get the pulley axes.
            b2Vec2 uA = cA + rA - m_groundAnchorA;
            b2Vec2 uB = cB + rB - m_groundAnchorB;

            float lengthA = uA.Length;
            float lengthB = uB.Length;

            if (lengthA > 10.0f * b2Settings.b2_linearSlop)
            {
                uA *= 1.0f / lengthA;
            }
            else
            {
                uA.SetZero();
            }

            if (lengthB > 10.0f * b2Settings.b2_linearSlop)
            {
                uB *= 1.0f / lengthB;
            }
            else
            {
                uB.SetZero();
            }

            // Compute effective mass.
            float ruA = b2Math.b2Cross(rA, uA);
            float ruB = b2Math.b2Cross(rB, uB);

            float mA = m_invMassA + m_invIA * ruA * ruA;
            float mB = m_invMassB + m_invIB * ruB * ruB;

            float mass = mA + m_ratio * m_ratio * mB;

            if (mass > 0.0f)
            {
                mass = 1.0f / mass;
            }

            float C = m_constant - lengthA - m_ratio * lengthB;
            float linearError = b2Math.b2Abs(C);

            float impulse = -mass * C;

            b2Vec2 PA = -impulse * uA;
            b2Vec2 PB = -m_ratio * impulse * uB;

            cA += m_invMassA * PA;
            aA += m_invIA * b2Math.b2Cross(rA, PA);
            cB += m_invMassB * PB;
            aB += m_invIB * b2Math.b2Cross(rB, PB);

            m_bodyA.InternalPosition.c = cA;
            m_bodyA.InternalPosition.a = aA;
            m_bodyB.InternalPosition.c = cB;
            m_bodyB.InternalPosition.a = aB;

            return linearError < b2Settings.b2_linearSlop;
        }
예제 #8
0
 /// The default ructor does nothing.
 /*public b2Transform()
 {
     p = new b2Vec2();
     q = new b2Rot();
 }*/
 /// Initialize using a position vector and a rotation.
 b2Transform(b2Vec2 position, b2Rot rotation)
 {
     p = position;
     q = rotation;
 }
예제 #9
0
        /// The default ructor does nothing.

        /*public b2Transform()
         * {
         *  p = new b2Vec2();
         *  q = new b2Rot();
         * }*/

        /// Initialize using a position vector and a rotation.
        b2Transform(b2Vec2 position, b2Rot rotation)
        {
            p = position;
            q = rotation;
        }
예제 #10
0
        public override bool SolvePositionConstraints(b2SolverData data)
        {
            b2Vec2 cA = data.positions[m_indexA].c;
            float aA = data.positions[m_indexA].a;
            b2Vec2 cB = data.positions[m_indexB].c;
            float aB = data.positions[m_indexB].a;
            b2Vec2 cC = data.positions[m_indexC].c;
            float aC = data.positions[m_indexC].a;
            b2Vec2 cD = data.positions[m_indexD].c;
            float aD = data.positions[m_indexD].a;

            b2Rot qA = new b2Rot(aA);
            b2Rot qB = new b2Rot(aB);
            b2Rot qC = new b2Rot(aC);
            b2Rot qD = new b2Rot(aD);

            float linearError = 0.0f;

            float coordinateA, coordinateB;

            b2Vec2 JvAC = b2Vec2.Zero, JvBD = b2Vec2.Zero;
            float JwA, JwB, JwC, JwD;
            float mass = 0.0f;

            if (m_typeA == b2JointType.e_revoluteJoint)
            {
                JvAC.SetZero();
                JwA = 1.0f;
                JwC = 1.0f;
                mass += m_iA + m_iC;

                coordinateA = aA - aC - m_referenceAngleA;
            }
            else
            {
                b2Vec2 u = b2Math.b2Mul(qC, m_localAxisC);
                b2Vec2 rC = b2Math.b2Mul(qC, m_localAnchorC - m_lcC);
                b2Vec2 rA = b2Math.b2Mul(qA, m_localAnchorA - m_lcA);
                JvAC = u;
                JwC = b2Math.b2Cross(rC, u);
                JwA = b2Math.b2Cross(rA, u);
                mass += m_mC + m_mA + m_iC * JwC * JwC + m_iA * JwA * JwA;

                b2Vec2 pC = m_localAnchorC - m_lcC;
                b2Vec2 pA = b2Math.b2MulT(qC, rA + (cA - cC));
                coordinateA = b2Math.b2Dot(pA - pC, m_localAxisC);
            }

            if (m_typeB == b2JointType.e_revoluteJoint)
            {
                JvBD.SetZero();
                JwB = m_ratio;
                JwD = m_ratio;
                mass += m_ratio * m_ratio * (m_iB + m_iD);

                coordinateB = aB - aD - m_referenceAngleB;
            }
            else
            {
                b2Vec2 u = b2Math.b2Mul(qD, m_localAxisD);
                b2Vec2 rD = b2Math.b2Mul(qD, m_localAnchorD - m_lcD);
                b2Vec2 rB = b2Math.b2Mul(qB, m_localAnchorB - m_lcB);
                JvBD = m_ratio * u;
                JwD = m_ratio * b2Math.b2Cross(rD, u);
                JwB = m_ratio * b2Math.b2Cross(rB, u);
                mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * JwD * JwD + m_iB * JwB * JwB;

                b2Vec2 pD = m_localAnchorD - m_lcD;
                b2Vec2 pB = b2Math.b2MulT(qD, rB + (cB - cD));
                coordinateB = b2Math.b2Dot(pB - pD, m_localAxisD);
            }

            float C = (coordinateA + m_ratio * coordinateB) - m_constant;

            float impulse = 0.0f;
            if (mass > 0.0f)
            {
                impulse = -C / mass;
            }

            cA += m_mA * impulse * JvAC;
            aA += m_iA * impulse * JwA;
            cB += m_mB * impulse * JvBD;
            aB += m_iB * impulse * JwB;
            cC -= m_mC * impulse * JvAC;
            aC -= m_iC * impulse * JwC;
            cD -= m_mD * impulse * JvBD;
            aD -= m_iD * impulse * JwD;

            data.positions[m_indexA].c = cA;
            data.positions[m_indexA].a = aA;
            data.positions[m_indexB].c = cB;
            data.positions[m_indexB].a = aB;
            data.positions[m_indexC].c = cC;
            data.positions[m_indexC].a = aC;
            data.positions[m_indexD].c = cD;
            data.positions[m_indexD].a = aD;

            // TODO_ERIN not implemented
            return linearError < b2Settings.b2_linearSlop;
        }
예제 #11
0
        public override void InitVelocityConstraints(b2SolverData data)
        {
            m_indexA = m_bodyA.IslandIndex;
            m_indexB = m_bodyB.IslandIndex;
            m_localCenterA = m_bodyA.Sweep.localCenter;
            m_localCenterB = m_bodyB.Sweep.localCenter;
            m_invMassA = m_bodyA.InvertedMass;
            m_invMassB = m_bodyB.InvertedMass;
            m_invIA = m_bodyA.InvertedI;
            m_invIB = m_bodyB.InvertedI;

            b2Vec2 cA = data.positions[m_indexA].c;
            float aA = data.positions[m_indexA].a;
            b2Vec2 vA = data.velocities[m_indexA].v;
            float wA = data.velocities[m_indexA].w;

            b2Vec2 cB = data.positions[m_indexB].c;
            float aB = data.positions[m_indexB].a;
            b2Vec2 vB = data.velocities[m_indexB].v;
            float wB = data.velocities[m_indexB].w;

            b2Rot qA = new b2Rot(aA);
            b2Rot qB = new b2Rot(aB);

            m_rA = b2Math.b2Mul(qA, m_localAnchorA - m_localCenterA);
            m_rB = b2Math.b2Mul(qB, m_localAnchorB - m_localCenterB);

            // 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 = m_invMassA, mB = m_invMassB;
            float iA = m_invIA, iB = m_invIB;

            bool fixedRotation = (iA + iB == 0.0f);
            b2Vec3 ex = new b2Vec3();
            b2Vec3 ey = new b2Vec3();
            b2Vec3 ez = new b2Vec3();
            ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB;
            ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB;
            ez.x = -m_rA.y * iA - m_rB.y * iB;
            ex.y = ey.x;
            ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB;
            ez.y = m_rA.x * iA + m_rB.x * iB;
            ex.z = ez.x;
            ey.z = ez.y;
            ez.z = iA + iB;
            m_mass = new b2Mat33(ex, ey, ez);

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

            if (m_enableMotor == false || fixedRotation)
            {
                m_motorImpulse = 0.0f;
            }

            if (m_enableLimit && fixedRotation == false)
            {
                float jointAngle = aB - aA - m_referenceAngle;
                if (b2Math.b2Abs(m_upperAngle - m_lowerAngle) < 2.0f * b2Settings.b2_angularSlop)
                {
                    m_limitState = b2LimitState.e_equalLimits;
                }
                else if (jointAngle <= m_lowerAngle)
                {
                    if (m_limitState != b2LimitState.e_atLowerLimit)
                    {
                        m_impulse.z = 0.0f;
                    }
                    m_limitState = b2LimitState.e_atLowerLimit;
                }
                else if (jointAngle >= m_upperAngle)
                {
                    if (m_limitState != b2LimitState.e_atUpperLimit)
                    {
                        m_impulse.z = 0.0f;
                    }
                    m_limitState = b2LimitState.e_atUpperLimit;
                }
                else
                {
                    m_limitState = b2LimitState.e_inactiveLimit;
                    m_impulse.z = 0.0f;
                }
            }
            else
            {
                m_limitState = b2LimitState.e_inactiveLimit;
            }

            if (data.step.warmStarting)
            {
                // Scale impulses to support a variable time step.
                m_impulse *= data.step.dtRatio;
                m_motorImpulse *= data.step.dtRatio;

                b2Vec2 P = new b2Vec2(m_impulse.x, m_impulse.y);

                vA -= mA * P;
                wA -= iA * (b2Math.b2Cross(m_rA, P) + m_motorImpulse + m_impulse.z);

                vB += mB * P;
                wB += iB * (b2Math.b2Cross(m_rB, P) + m_motorImpulse + m_impulse.z);
            }
            else
            {
                m_impulse.SetZero();
                m_motorImpulse = 0.0f;
            }

            data.velocities[m_indexA].v = vA;
            data.velocities[m_indexA].w = wA;
            data.velocities[m_indexB].v = vB;
            data.velocities[m_indexB].w = wB;
        }
예제 #12
0
        public override void InitVelocityConstraints(b2SolverData data)
        {
            m_indexA = m_bodyA.IslandIndex;
            m_indexB = m_bodyB.IslandIndex;
            m_localCenterA = m_bodyA.Sweep.localCenter;
            m_localCenterB = m_bodyB.Sweep.localCenter;
            m_invMassA = m_bodyA.InvertedMass;
            m_invMassB = m_bodyB.InvertedMass;
            m_invIA = m_bodyA.InvertedI;
            m_invIB = m_bodyB.InvertedI;

            b2Vec2 cA = m_bodyA.InternalPosition.c;
            float aA = m_bodyA.InternalPosition.a;
            b2Vec2 vA = m_bodyA.InternalVelocity.v;
            float wA = m_bodyA.InternalVelocity.w;

            b2Vec2 cB = m_bodyB.InternalPosition.c;
            float aB = m_bodyB.InternalPosition.a;
            b2Vec2 vB = m_bodyB.InternalVelocity.v;
            float wB = m_bodyB.InternalVelocity.w;

            b2Rot qA = new b2Rot(aA);
            b2Rot qB = new b2Rot(aB);

            m_rA = b2Math.b2Mul(qA, m_localAnchorA - m_localCenterA);
            m_rB = b2Math.b2Mul(qB, m_localAnchorB - m_localCenterB);

            // 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 = m_invMassA, mB = m_invMassB;
            float iA = m_invIA, iB = m_invIB;

            b2Vec3 ex = new b2Vec3();
            b2Vec3 ey = new b2Vec3();
            b2Vec3 ez = new b2Vec3();
            ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB;
            ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB;
            ez.x = -m_rA.y * iA - m_rB.y * iB;
            ex.y = ey.x;
            ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB;
            ez.y = m_rA.x * iA + m_rB.x * iB;
            ex.z = ez.x;
            ey.z = ez.y;
            ez.z = iA + iB;
            b2Mat33 K = new b2Mat33(ex, ey, ez);

            if (m_frequencyHz > 0.0f)
            {
                m_mass = K.GetInverse22(m_mass);

                float invM = iA + iB;
                float m = invM > 0.0f ? 1.0f / invM : 0.0f;

                float C = aB - aA - m_referenceAngle;

                // Frequency
                float omega = 2.0f * b2Settings.b2_pi * m_frequencyHz;

                // Damping coefficient
                float d = 2.0f * m * m_dampingRatio * omega;

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

                // magic formulas
                float h = data.step.dt;
                m_gamma = h * (d + h * k);
                m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f;
                m_bias = C * h * k * m_gamma;

                invM += m_gamma;
                m_mass.ezz = invM != 0.0f ? 1.0f / invM : 0.0f;
            }
            else
            {
                m_mass = K.GetSymInverse33(m_mass);
                m_gamma = 0.0f;
                m_bias = 0.0f;
            }

            if (data.step.warmStarting)
            {
                // Scale impulses to support a variable time step.
                m_impulse *= data.step.dtRatio;

                b2Vec2 P = new b2Vec2(m_impulse.x, m_impulse.y);

                vA -= mA * P;
                wA -= iA * (b2Math.b2Cross(m_rA, P) + m_impulse.z);

                vB += mB * P;
                wB += iB * (b2Math.b2Cross(m_rB, P) + m_impulse.z);
            }
            else
            {
                m_impulse.SetZero();
            }

            m_bodyA.InternalVelocity.v = vA;
            m_bodyA.InternalVelocity.w = wA;
            m_bodyB.InternalVelocity.v = vB;
            m_bodyB.InternalVelocity.w = wB;
        }
예제 #13
0
        public override void InitVelocityConstraints(b2SolverData data)
        {
            m_indexA = m_bodyA.IslandIndex;
            m_indexB = m_bodyB.IslandIndex;
            m_localCenterA = m_bodyA.Sweep.localCenter;
            m_localCenterB = m_bodyB.Sweep.localCenter;
            m_invMassA = m_bodyA.InvertedMass;
            m_invMassB = m_bodyB.InvertedMass;
            m_invIA = m_bodyA.InvertedI;
            m_invIB = m_bodyB.InvertedI;

            b2Vec2 cA = m_bodyA.InternalPosition.c;
            float aA = m_bodyA.InternalPosition.a;
            b2Vec2 vA = m_bodyA.InternalVelocity.v;
            float wA = m_bodyA.InternalVelocity.w;

            b2Vec2 cB = m_bodyB.InternalPosition.c;
            float aB = m_bodyB.InternalPosition.a;
            b2Vec2 vB = m_bodyB.InternalVelocity.v;
            float wB = m_bodyB.InternalVelocity.w;

            b2Rot qA = new b2Rot(aA);
            b2Rot qB = new b2Rot(aB);

            m_rA = b2Math.b2Mul(qA, m_localAnchorA - m_localCenterA);
            m_rB = b2Math.b2Mul(qB, m_localAnchorB - m_localCenterB);
            m_u = cB + m_rB - cA - m_rA;

            m_length = m_u.Length;

            float C = m_length - m_maxLength;
            if (C > 0.0f)
            {
                m_state = b2LimitState.e_atUpperLimit;
            }
            else
            {
                m_state = b2LimitState.e_inactiveLimit;
            }

            if (m_length > b2Settings.b2_linearSlop)
            {
                m_u *= 1.0f / m_length;
            }
            else
            {
                m_u.SetZero();
                m_mass = 0.0f;
                m_impulse = 0.0f;
                return;
            }

            // Compute effective mass.
            float crA = b2Math.b2Cross(m_rA, m_u);
            float crB = b2Math.b2Cross(m_rB, m_u);
            float invMass = m_invMassA + m_invIA * crA * crA + m_invMassB + m_invIB * crB * crB;

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

            if (data.step.warmStarting)
            {
                // Scale the impulse to support a variable time step.
                m_impulse *= data.step.dtRatio;

                b2Vec2 P = m_impulse * m_u;
                vA -= m_invMassA * P;
                wA -= m_invIA * b2Math.b2Cross(m_rA, P);
                vB += m_invMassB * P;
                wB += m_invIB * b2Math.b2Cross(m_rB, P);
            }
            else
            {
                m_impulse = 0.0f;
            }

            m_bodyA.InternalVelocity.v = vA;
            m_bodyA.InternalVelocity.w = wA;
            m_bodyB.InternalVelocity.v = vB;
            m_bodyB.InternalVelocity.w = wB;
        }
예제 #14
0
        public override bool SolvePositionConstraints(b2SolverData data)
        {
            b2Vec2 cA = data.positions[m_indexA].c;
            float aA = data.positions[m_indexA].a;
            b2Vec2 cB = data.positions[m_indexB].c;
            float aB = data.positions[m_indexB].a;

            b2Rot qA = new b2Rot(aA);
            b2Rot qB = new b2Rot(aB);

            float mA = InvertedMassA, mB = InvertedMassB;
            float iA = InvertedIA, iB = InvertedIB;

            // Compute fresh Jacobians
            b2Vec2 rA = b2Math.b2Mul(qA, m_localAnchorA - m_localCenterA);
            b2Vec2 rB = b2Math.b2Mul(qB, m_localAnchorB - m_localCenterB);
            b2Vec2 d = cB + rB - cA - rA;

            b2Vec2 axis = b2Math.b2Mul(qA, m_localXAxisA);
            float a1 = b2Math.b2Cross(d + rA, axis);
            float a2 = b2Math.b2Cross(rB, axis);
            b2Vec2 perp = b2Math.b2Mul(qA, m_localYAxisA);

            float s1 = b2Math.b2Cross(d + rA, perp);
            float s2 = b2Math.b2Cross(rB, perp);

            b2Vec3 impulse;
            b2Vec2 C1 = new b2Vec2();
            C1.x = b2Math.b2Dot(perp, d);
            C1.y = aB - aA - m_referenceAngle;

            float linearError = b2Math.b2Abs(C1.x);
            float angularError = b2Math.b2Abs(C1.y);

            bool active = false;
            float C2 = 0.0f;
            if (m_enableLimit)
            {
                float translation = b2Math.b2Dot(axis, d);
                if (b2Math.b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2Settings.b2_linearSlop)
                {
                    // Prevent large angular corrections
                    C2 = b2Math.b2Clamp(translation, -b2Settings.b2_maxLinearCorrection, b2Settings.b2_maxLinearCorrection);
                    linearError = Math.Max(linearError, b2Math.b2Abs(translation));
                    active = true;
                }
                else if (translation <= m_lowerTranslation)
                {
                    // Prevent large linear corrections and allow some slop.
                    C2 = b2Math.b2Clamp(translation - m_lowerTranslation + b2Settings.b2_linearSlop, -b2Settings.b2_maxLinearCorrection, 0.0f);
                    linearError = Math.Max(linearError, m_lowerTranslation - translation);
                    active = true;
                }
                else if (translation >= m_upperTranslation)
                {
                    // Prevent large linear corrections and allow some slop.
                    C2 = b2Math.b2Clamp(translation - m_upperTranslation - b2Settings.b2_linearSlop, 0.0f, b2Settings.b2_maxLinearCorrection);
                    linearError = Math.Max(linearError, translation - m_upperTranslation);
                    active = true;
                }
            }

            if (active)
            {
                float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
                float k12 = iA * s1 + iB * s2;
                float k13 = iA * s1 * a1 + iB * s2 * a2;
                float k22 = iA + iB;
                if (k22 == 0.0f)
                {
                    // For fixed rotation
                    k22 = 1.0f;
                }
                float k23 = iA * a1 + iB * a2;
                float k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2;

                b2Mat33 K = new b2Mat33(
                    new b2Vec3(k11, k12, k13),
                    new b2Vec3(k12, k22, k23),
                    new b2Vec3(k13, k23, k33));

                b2Vec3 C = new b2Vec3(C1.x, C1.y, C2);

                impulse = K.Solve33(-C);
            }
            else
            {
                float k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
                float k12 = iA * s1 + iB * s2;
                float k22 = iA + iB;
                if (k22 == 0.0f)
                {
                    k22 = 1.0f;
                }

                b2Mat22 K = new b2Mat22();
                K.ex.Set(k11, k12);
                K.ey.Set(k12, k22);

                b2Vec2 impulse1 = K.Solve(-C1);
                impulse = new b2Vec3();
                impulse.x = impulse1.x;
                impulse.y = impulse1.y;
                impulse.z = 0.0f;
            }

            b2Vec2 P = impulse.x * perp + impulse.z * axis;
            float LA = impulse.x * s1 + impulse.y + impulse.z * a1;
            float LB = impulse.x * s2 + impulse.y + impulse.z * a2;

            cA -= mA * P;
            aA -= iA * LA;
            cB += mB * P;
            aB += iB * LB;

            data.positions[m_indexA].c = cA;
            data.positions[m_indexA].a = aA;
            data.positions[m_indexB].c = cB;
            data.positions[m_indexB].a = aB;

            return linearError <= b2Settings.b2_linearSlop && angularError <= b2Settings.b2_angularSlop;
        }
예제 #15
0
        public override void InitVelocityConstraints(b2SolverData data)
        {
            m_indexB = m_bodyB.IslandIndex;
            m_localCenterB = m_bodyB.Sweep.localCenter;
            m_invMassB = m_bodyB.InvertedMass;
            m_invIB = m_bodyB.InvertedI;

            b2Vec2 cB = data.positions[m_indexB].c;
            float aB = data.positions[m_indexB].a;
            b2Vec2 vB = data.velocities[m_indexB].v;
            float wB = data.velocities[m_indexB].w;

            b2Rot qB = new b2Rot(aB);

            float mass = m_bodyB.Mass;

            // Frequency
            float omega = 2.0f * b2Settings.b2_pi * m_frequencyHz;

            // Damping coefficient
            float d = 2.0f * mass * m_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 > b2Settings.b2_epsilon);
            m_gamma = h * (d + h * k);
            if (m_gamma != 0.0f)
            {
                m_gamma = 1.0f / m_gamma;
            }
            m_beta = h * k * m_gamma;

            // Compute the effective mass matrix.
            m_rB = b2Math.b2Mul(qB, m_localAnchorB - m_localCenterB);

            // 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]
            b2Mat22 K = new b2Mat22();
            K.exx = m_invMassB + m_invIB * m_rB.y * m_rB.y + m_gamma;
            K.exy = -m_invIB * m_rB.x * m_rB.y;
            K.eyx = K.ex.y;
            K.eyy = m_invMassB + m_invIB * m_rB.x * m_rB.x + m_gamma;

            m_mass = K.GetInverse();

            m_C = cB + m_rB - m_targetA;
            m_C *= m_beta;

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

            if (data.step.warmStarting)
            {
                m_impulse *= data.step.dtRatio;
                vB += m_invMassB * m_impulse;
                wB += m_invIB * b2Math.b2Cross(m_rB, m_impulse);
            }
            else
            {
                m_impulse.SetZero();
            }

            data.velocities[m_indexB].v = vB;
            data.velocities[m_indexB].w = wB;
        }
예제 #16
0
        public override void InitVelocityConstraints(b2SolverData data)
        {
            m_indexA = m_bodyA.IslandIndex;
            m_indexB = m_bodyB.IslandIndex;
            m_localCenterA = m_bodyA.Sweep.localCenter;
            m_localCenterB = m_bodyB.Sweep.localCenter;
            InvertedMassA = m_bodyA.InvertedMass;
            InvertedMassB = m_bodyB.InvertedMass;
            InvertedIA = m_bodyA.InvertedI;
            InvertedIB = m_bodyB.InvertedI;

            b2Vec2 cA = data.positions[m_indexA].c;
            float aA = data.positions[m_indexA].a;
            b2Vec2 vA = data.velocities[m_indexA].v;
            float wA = data.velocities[m_indexA].w;

            b2Vec2 cB = data.positions[m_indexB].c;
            float aB = data.positions[m_indexB].a;
            b2Vec2 vB = data.velocities[m_indexB].v;
            float wB = data.velocities[m_indexB].w;

            b2Rot qA = new b2Rot(aA);
            b2Rot qB = new b2Rot(aB);

            // Compute the effective masses.
            b2Vec2 rA = b2Math.b2Mul(qA, m_localAnchorA - m_localCenterA);
            b2Vec2 rB = b2Math.b2Mul(qB, m_localAnchorB - m_localCenterB);
            b2Vec2 d = (cB - cA) + rB - rA;

            float mA = InvertedMassA, mB = InvertedMassB;
            float iA = InvertedIA, iB = InvertedIB;

            // Compute motor Jacobian and effective mass.
            {
                m_axis = b2Math.b2Mul(qA, m_localXAxisA);
                m_a1 = b2Math.b2Cross(d + rA, m_axis);
                m_a2 = b2Math.b2Cross(rB, m_axis);

                m_motorMass = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2;
                if (m_motorMass > 0.0f)
                {
                    m_motorMass = 1.0f / m_motorMass;
                }
            }

            // Prismatic constraint.
            {
                m_perp = b2Math.b2Mul(qA, m_localYAxisA);

                m_s1 = b2Math.b2Cross(d + rA, m_perp);
                m_s2 = b2Math.b2Cross(rB, m_perp);

                float k11 = mA + mB + iA * m_s1 * m_s1 + iB * m_s2 * m_s2;
                float k12 = iA * m_s1 + iB * m_s2;
                float k13 = iA * m_s1 * m_a1 + iB * m_s2 * m_a2;
                float k22 = iA + iB;
                if (k22 == 0.0f)
                {
                    // For bodies with fixed rotation.
                    k22 = 1.0f;
                }
                float k23 = iA * m_a1 + iB * m_a2;
                float k33 = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2;

                m_K.ex.Set(k11, k12, k13);
                m_K.ey.Set(k12, k22, k23);
                m_K.ez.Set(k13, k23, k33);
            }

            // Compute motor and limit terms.
            if (m_enableLimit)
            {
                float jointTranslation = b2Math.b2Dot(m_axis, d);
                if (b2Math.b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2Settings.b2_linearSlop)
                {
                    m_limitState = b2LimitState.e_equalLimits;
                }
                else if (jointTranslation <= m_lowerTranslation)
                {
                    if (m_limitState != b2LimitState.e_atLowerLimit)
                    {
                        m_limitState = b2LimitState.e_atLowerLimit;
                        m_impulse.z = 0.0f;
                    }
                }
                else if (jointTranslation >= m_upperTranslation)
                {
                    if (m_limitState != b2LimitState.e_atUpperLimit)
                    {
                        m_limitState = b2LimitState.e_atUpperLimit;
                        m_impulse.z = 0.0f;
                    }
                }
                else
                {
                    m_limitState = b2LimitState.e_inactiveLimit;
                    m_impulse.z = 0.0f;
                }
            }
            else
            {
                m_limitState = b2LimitState.e_inactiveLimit;
                m_impulse.z = 0.0f;
            }

            if (m_enableMotor == false)
            {
                m_motorImpulse = 0.0f;
            }

            if (data.step.warmStarting)
            {
                // Account for variable time step.
                m_impulse *= data.step.dtRatio;
                m_motorImpulse *= data.step.dtRatio;

                b2Vec2 P = m_impulse.x * m_perp + (m_motorImpulse + m_impulse.z) * m_axis;
                float LA = m_impulse.x * m_s1 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a1;
                float LB = m_impulse.x * m_s2 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a2;

                vA -= mA * P;
                wA -= iA * LA;

                vB += mB * P;
                wB += iB * LB;
            }
            else
            {
                m_impulse.SetZero();
                m_motorImpulse = 0.0f;
            }

            data.velocities[m_indexA].v = vA;
            data.velocities[m_indexA].w = wA;
            data.velocities[m_indexB].v = vB;
            data.velocities[m_indexB].w = wB;
        }
예제 #17
0
        public override bool SolvePositionConstraints(b2SolverData data)
        {
            b2Vec2 cA = data.positions[m_indexA].c;
            float aA = data.positions[m_indexA].a;
            b2Vec2 cB = data.positions[m_indexB].c;
            float aB = data.positions[m_indexB].a;

            b2Rot qA = new b2Rot(aA);
            b2Rot qB = new b2Rot(aB);

            b2Vec2 rA = b2Math.b2Mul(qA, m_localAnchorA - m_localCenterA);
            b2Vec2 rB = b2Math.b2Mul(qB, m_localAnchorB - m_localCenterB);
            b2Vec2 d = (cB - cA) + rB - rA;

            b2Vec2 ay = b2Math.b2Mul(qA, m_localYAxisA);

            float sAy = b2Math.b2Cross(d + rA, ay);
            float sBy = b2Math.b2Cross(rB, ay);

            float C = b2Math.b2Dot(d, ay);

            float k = m_invMassA + m_invMassB + m_invIA * m_sAy * m_sAy + m_invIB * m_sBy * m_sBy;

            float impulse;
            if (k != 0.0f)
            {
                impulse = -C / k;
            }
            else
            {
                impulse = 0.0f;
            }

            b2Vec2 P = impulse * ay;
            float LA = impulse * sAy;
            float LB = impulse * sBy;

            cA -= m_invMassA * P;
            aA -= m_invIA * LA;
            cB += m_invMassB * P;
            aB += m_invIB * LB;

            data.positions[m_indexA].c = cA;
            data.positions[m_indexA].a = aA;
            data.positions[m_indexB].c = cB;
            data.positions[m_indexB].a = aB;

            return b2Math.b2Abs(C) <= b2Settings.b2_linearSlop;
        }
예제 #18
0
        public override void InitVelocityConstraints(b2SolverData data)
        {
            m_indexA = m_bodyA.IslandIndex;
            m_indexB = m_bodyB.IslandIndex;
            m_localCenterA = m_bodyA.Sweep.localCenter;
            m_localCenterB = m_bodyB.Sweep.localCenter;
            m_invMassA = m_bodyA.InvertedMass;
            m_invMassB = m_bodyB.InvertedMass;
            m_invIA = m_bodyA.InvertedI;
            m_invIB = m_bodyB.InvertedI;

            float mA = m_invMassA, mB = m_invMassB;
            float iA = m_invIA, iB = m_invIB;

            b2Vec2 cA = data.positions[m_indexA].c;
            float aA = data.positions[m_indexA].a;
            b2Vec2 vA = data.velocities[m_indexA].v;
            float wA = data.velocities[m_indexA].w;

            b2Vec2 cB = data.positions[m_indexB].c;
            float aB = data.positions[m_indexB].a;
            b2Vec2 vB = data.velocities[m_indexB].v;
            float wB = data.velocities[m_indexB].w;

            b2Rot qA = new b2Rot(aA);
            b2Rot qB = new b2Rot(aB);

            // Compute the effective masses.
            b2Vec2 rA = b2Math.b2Mul(qA, m_localAnchorA - m_localCenterA);
            b2Vec2 rB = b2Math.b2Mul(qB, m_localAnchorB - m_localCenterB);
            b2Vec2 d = cB + rB - cA - rA;

            // Point to line raint
            {
                m_ay = b2Math.b2Mul(qA, m_localYAxisA);
                m_sAy = b2Math.b2Cross(d + rA, m_ay);
                m_sBy = b2Math.b2Cross(rB, m_ay);

                m_mass = mA + mB + iA * m_sAy * m_sAy + iB * m_sBy * m_sBy;

                if (m_mass > 0.0f)
                {
                    m_mass = 1.0f / m_mass;
                }
            }

            // Spring raint
            m_springMass = 0.0f;
            m_bias = 0.0f;
            m_gamma = 0.0f;
            if (m_frequencyHz > 0.0f)
            {
                m_ax = b2Math.b2Mul(qA, m_localXAxisA);
                m_sAx = b2Math.b2Cross(d + rA, m_ax);
                m_sBx = b2Math.b2Cross(rB, m_ax);

                float invMass = mA + mB + iA * m_sAx * m_sAx + iB * m_sBx * m_sBx;

                if (invMass > 0.0f)
                {
                    m_springMass = 1.0f / invMass;

                    float C = b2Math.b2Dot(d, m_ax);

                    // Frequency
                    float omega = 2.0f * (float)Math.PI * m_frequencyHz;

                    // Damping coefficient
                    float dx = 2.0f * m_springMass * m_dampingRatio * omega;

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

                    // magic formulas
                    float h = data.step.dt;
                    m_gamma = h * (dx + h * k);
                    if (m_gamma > 0.0f)
                    {
                        m_gamma = 1.0f / m_gamma;
                    }

                    m_bias = C * h * k * m_gamma;

                    m_springMass = invMass + m_gamma;
                    if (m_springMass > 0.0f)
                    {
                        m_springMass = 1.0f / m_springMass;
                    }
                }
            }
            else
            {
                m_springImpulse = 0.0f;
            }

            // Rotational motor
            if (m_enableMotor)
            {
                m_motorMass = iA + iB;
                if (m_motorMass > 0.0f)
                {
                    m_motorMass = 1.0f / m_motorMass;
                }
            }
            else
            {
                m_motorMass = 0.0f;
                m_motorImpulse = 0.0f;
            }

            if (data.step.warmStarting)
            {
                // Account for variable time step.
                m_impulse *= data.step.dtRatio;
                m_springImpulse *= data.step.dtRatio;
                m_motorImpulse *= data.step.dtRatio;

                b2Vec2 P = m_impulse * m_ay + m_springImpulse * m_ax;
                float LA = m_impulse * m_sAy + m_springImpulse * m_sAx + m_motorImpulse;
                float LB = m_impulse * m_sBy + m_springImpulse * m_sBx + m_motorImpulse;

                vA -= m_invMassA * P;
                wA -= m_invIA * LA;

                vB += m_invMassB * P;
                wB += m_invIB * LB;
            }
            else
            {
                m_impulse = 0.0f;
                m_springImpulse = 0.0f;
                m_motorImpulse = 0.0f;
            }

            data.velocities[m_indexA].v = vA;
            data.velocities[m_indexA].w = wA;
            data.velocities[m_indexB].v = vB;
            data.velocities[m_indexB].w = wB;
        }
예제 #19
0
 /// Inverse rotate a vector
 public static b2Vec2 b2MulT(b2Rot q, b2Vec2 v)
 {
     return(new b2Vec2(q.c * v.x + q.s * v.y, -q.s * v.x + q.c * v.y));
 }
예제 #20
0
        public override void InitVelocityConstraints(b2SolverData data)
        {
            m_indexA = m_bodyA.IslandIndex;
            m_indexB = m_bodyB.IslandIndex;
            m_localCenterA = m_bodyA.Sweep.localCenter;
            m_localCenterB = m_bodyB.Sweep.localCenter;
            m_invMassA = m_bodyA.InvertedMass;
            m_invMassB = m_bodyB.InvertedMass;
            m_invIA = m_bodyA.InvertedI;
            m_invIB = m_bodyB.InvertedI;

            b2Vec2 cA = m_bodyA.InternalPosition.c;
            float aA = m_bodyA.InternalPosition.a;
            b2Vec2 vA = m_bodyA.InternalVelocity.v;
            float wA = m_bodyA.InternalVelocity.w;

            b2Vec2 cB = m_bodyB.InternalPosition.c;
            float aB = m_bodyB.InternalPosition.a;
            b2Vec2 vB = m_bodyB.InternalVelocity.v;
            float wB = m_bodyB.InternalVelocity.w;

            b2Rot qA = new b2Rot(aA);
            b2Rot qB = new b2Rot(aB);

            m_rA = b2Math.b2Mul(qA, m_localAnchorA - m_localCenterA);
            m_rB = b2Math.b2Mul(qB, m_localAnchorB - m_localCenterB);

            // Get the pulley axes.
            m_uA = cA + m_rA - m_groundAnchorA;
            m_uB = cB + m_rB - m_groundAnchorB;

            float lengthA = m_uA.Length;
            float lengthB = m_uB.Length;

            if (lengthA > 10.0f * b2Settings.b2_linearSlop)
            {
                m_uA *= 1.0f / lengthA;
            }
            else
            {
                m_uA.SetZero();
            }

            if (lengthB > 10.0f * b2Settings.b2_linearSlop)
            {
                m_uB *= 1.0f / lengthB;
            }
            else
            {
                m_uB.SetZero();
            }

            // Compute effective mass.
            float ruA = b2Math.b2Cross(m_rA, m_uA);
            float ruB = b2Math.b2Cross(m_rB, m_uB);

            float mA = m_invMassA + m_invIA * ruA * ruA;
            float mB = m_invMassB + m_invIB * ruB * ruB;

            m_mass = mA + m_ratio * m_ratio * mB;

            if (m_mass > 0.0f)
            {
                m_mass = 1.0f / m_mass;
            }

            if (data.step.warmStarting)
            {
                // Scale impulses to support variable time steps.
                m_impulse *= data.step.dtRatio;

                // Warm starting.
                b2Vec2 PA = -(m_impulse) * m_uA;
                b2Vec2 PB = (-m_ratio * m_impulse) * m_uB;

                vA += m_invMassA * PA;
                wA += m_invIA * b2Math.b2Cross(m_rA, PA);
                vB += m_invMassB * PB;
                wB += m_invIB * b2Math.b2Cross(m_rB, PB);
            }
            else
            {
                m_impulse = 0.0f;
            }

            m_bodyA.InternalVelocity.v = vA;
            m_bodyA.InternalVelocity.w = wA;
            m_bodyB.InternalVelocity.v = vB;
            m_bodyB.InternalVelocity.w = wB;
        }
예제 #21
0
        /// <summary>
        /// Actualizamos las imagenes de los cuerpos con las fisicas
        /// </summary>
        public virtual void OnSetImagePositionsFromPhysicsBodies()
        {
            foreach (var imgInfo in m_imageInfos)
            {
                if (imgInfo != null)
                {
                    var pos = imgInfo.Center;
                    float angle = -imgInfo.Angle;
                    if (imgInfo.Body != null)
                    {
                        //need to rotate image local center by body angle
                        b2Vec2 localPos = new b2Vec2(pos.x, pos.y);
                        b2Rot rot = new b2Rot(imgInfo.Body.Angle);

                        localPos = CCPointExHelper.b2Mul(rot, localPos) + imgInfo.Body.Position;

                        pos.x = localPos.x;
                        pos.y = localPos.y;
                        angle += -imgInfo.Body.Angle;

                    }

                    imgInfo.Sprite.Rotation = CCMathHelper.ToDegrees(angle);
                    imgInfo.Sprite.Position = new CCPoint(pos.x, pos.y);

                }
            }
        }
예제 #22
0
        public override bool SolvePositionConstraints(b2SolverData data)
{
    b2Vec2 cA = m_bodyA.InternalPosition.c;
    float aA = m_bodyA.InternalPosition.a;
    b2Vec2 cB = m_bodyB.InternalPosition.c;
    float aB = m_bodyB.InternalPosition.a;

    b2Rot qA = new b2Rot(aA);
            b2Rot qB = new b2Rot(aB);

    b2Vec2 rA = b2Math.b2Mul(qA, m_localAnchorA - m_localCenterA);
    b2Vec2 rB = b2Math.b2Mul(qB, m_localAnchorB - m_localCenterB);
    b2Vec2 u = cB + rB - cA - rA;

    float length = u.Normalize();
    float C = length - m_maxLength;

    C = b2Math.b2Clamp(C, 0.0f, b2Settings.b2_maxLinearCorrection);

    float impulse = -m_mass * C;
    b2Vec2 P = impulse * u;

    cA -= m_invMassA * P;
    aA -= m_invIA * b2Math.b2Cross(rA, P);
    cB += m_invMassB * P;
    aB += m_invIB * b2Math.b2Cross(rB, P);

    m_bodyA.InternalPosition.c = cA;
    m_bodyA.InternalPosition.a = aA;
    m_bodyB.InternalPosition.c = cB;
    m_bodyB.InternalPosition.a = aB;

    return length - m_maxLength < b2Settings.b2_linearSlop;
}
예제 #23
0
 public static b2Rot Create()
 {
     var rot = new b2Rot();
     rot.SetIdentity();
     return rot;
 }
예제 #24
0
        public override bool SolvePositionConstraints(b2SolverData data)
        {
            b2Vec2 cA = m_bodyA.InternalPosition.c;
            float aA = m_bodyA.InternalPosition.a;
            b2Vec2 cB = m_bodyB.InternalPosition.c;
            float aB = m_bodyB.InternalPosition.a;

            b2Rot qA = new b2Rot(aA);
            b2Rot qB = new b2Rot(aB);

            float mA = m_invMassA, mB = m_invMassB;
            float iA = m_invIA, iB = m_invIB;

            b2Vec2 rA = b2Math.b2Mul(qA, m_localAnchorA - m_localCenterA);
            b2Vec2 rB = b2Math.b2Mul(qB, m_localAnchorB - m_localCenterB);

            float positionError, angularError;

            b2Vec3 ex = new b2Vec3();
            b2Vec3 ey = new b2Vec3();
            b2Vec3 ez = new b2Vec3();
            ex.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;
            ey.x = -rA.y * rA.x * iA - rB.y * rB.x * iB;
            ez.x = -rA.y * iA - rB.y * iB;
            ex.y = ey.x;
            ey.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;
            ez.y = rA.x * iA + rB.x * iB;
            ex.z = ez.x;
            ey.z = ez.y;
            ez.z = iA + iB;
            b2Mat33 K = new b2Mat33(ex, ey, ez);

            if (m_frequencyHz > 0.0f)
            {
                b2Vec2 C1 = cB + rB - cA - rA;

                positionError = C1.Length;
                angularError = 0.0f;

                b2Vec2 P = -K.Solve22(C1);

                cA -= mA * P;
                aA -= iA * b2Math.b2Cross(rA, P);

                cB += mB * P;
                aB += iB * b2Math.b2Cross(rB, P);
            }
            else
            {
                b2Vec2 C1 = cB + rB - cA - rA;
                float C2 = aB - aA - m_referenceAngle;

                positionError = C1.Length;
                angularError = b2Math.b2Abs(C2);

                b2Vec3 C = new b2Vec3(C1.x, C1.y, C2);

                b2Vec3 impulse = -K.Solve33(C);
                b2Vec2 P = new b2Vec2(impulse.x, impulse.y);

                cA -= mA * P;
                aA -= iA * (b2Math.b2Cross(rA, P) + impulse.z);

                cB += mB * P;
                aB += iB * (b2Math.b2Cross(rB, P) + impulse.z);
            }

            m_bodyA.InternalPosition.c = cA;
            m_bodyA.InternalPosition.a = aA;
            m_bodyB.InternalPosition.c = cB;
            m_bodyB.InternalPosition.a = aB;

            return positionError <= b2Settings.b2_linearSlop && angularError <= b2Settings.b2_angularSlop;
        }
예제 #25
0
        public override bool SolvePositionConstraints(b2SolverData data)
        {
            if (m_frequencyHz > 0.0f)
            {
                // There is no position correction for soft distance constraints.
                return true;
            }

            b2Vec2 cA = m_bodyA.InternalPosition.c;
            float aA = m_bodyA.InternalPosition.a;
            b2Vec2 cB = m_bodyB.InternalPosition.c;
            float aB = m_bodyB.InternalPosition.a;

            b2Rot qA = new b2Rot(aA);
            b2Rot qB = new b2Rot(aB);

            b2Vec2 rA = b2Math.b2Mul(qA, m_localAnchorA - m_localCenterA);
            b2Vec2 rB = b2Math.b2Mul(qB, m_localAnchorB - m_localCenterB);
            b2Vec2 u = cB + rB - cA - rA;

            float length = u.Normalize();
            float C = length - m_length;
            C = b2Math.b2Clamp(C, -b2Settings.b2_maxLinearCorrection, b2Settings.b2_maxLinearCorrection);

            float impulse = -m_mass * C;
            b2Vec2 P = impulse * u;

            cA -= m_invMassA * P;
            aA -= m_invIA * b2Math.b2Cross(ref rA, ref P);
            cB += m_invMassB * P;
            aB += m_invIB * b2Math.b2Cross(ref rB, ref P);

            m_bodyA.InternalPosition.c = cA;
            m_bodyA.InternalPosition.a = aA;
            m_bodyB.InternalPosition.c = cB;
            m_bodyB.InternalPosition.a = aB;

            return b2Math.b2Abs(C) < b2Settings.b2_linearSlop;
        }
예제 #26
0
        public override void InitVelocityConstraints(b2SolverData data)
        {
            m_indexA = m_bodyA.IslandIndex;
            m_indexB = m_bodyB.IslandIndex;
            m_localCenterA = m_bodyA.Sweep.localCenter;
            m_localCenterB = m_bodyB.Sweep.localCenter;
            m_invMassA = m_bodyA.InvertedMass;
            m_invMassB = m_bodyB.InvertedMass;
            m_invIA = m_bodyA.InvertedI;
            m_invIB = m_bodyB.InvertedI;

            float aA = m_bodyA.InternalPosition.a;
            b2Vec2 vA = m_bodyA.InternalVelocity.v;
            float wA = m_bodyA.InternalVelocity.w;

            float aB = m_bodyB.InternalPosition.a;
            b2Vec2 vB = m_bodyB.InternalVelocity.v;
            float wB = m_bodyB.InternalVelocity.w;

            b2Rot qA = new b2Rot(aA);
            b2Rot qB = new b2Rot(aB);

            // Compute the effective mass matrix.
            m_rA = b2Math.b2Mul(qA, m_localAnchorA - m_localCenterA);
            m_rB = b2Math.b2Mul(qB, m_localAnchorB - m_localCenterB);

            // 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 = m_invMassA, mB = m_invMassB;
            float iA = m_invIA, iB = m_invIB;

            b2Mat22 K = new b2Mat22();
            K.exx = mA + mB + iA * m_rA.y * m_rA.y + iB * m_rB.y * m_rB.y;
            K.exy = -iA * m_rA.x * m_rA.y - iB * m_rB.x * m_rB.y;
            K.eyx = K.ex.y;
            K.eyy = mA + mB + iA * m_rA.x * m_rA.x + iB * m_rB.x * m_rB.x;

            m_linearMass = K.GetInverse();

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

            if (data.step.warmStarting)
            {
                // Scale impulses to support a variable time step.
                m_linearImpulse *= data.step.dtRatio;
                m_angularImpulse *= data.step.dtRatio;

                b2Vec2 P = new b2Vec2(m_linearImpulse.x, m_linearImpulse.y);
                vA -= mA * P;
                wA -= iA * (b2Math.b2Cross(m_rA, P) + m_angularImpulse);
                vB += mB * P;
                wB += iB * (b2Math.b2Cross(m_rB, P) + m_angularImpulse);
            }
            else
            {
                m_linearImpulse.SetZero();
                m_angularImpulse = 0.0f;
            }

            m_bodyA.InternalVelocity.v = vA;
            m_bodyA.InternalVelocity.w = wA;
            m_bodyB.InternalVelocity.v = vB;
            m_bodyB.InternalVelocity.w = wB;
        }
예제 #27
0
        public override void InitVelocityConstraints(b2SolverData data)
        {
            m_indexA = m_bodyA.IslandIndex;
            m_indexB = m_bodyB.IslandIndex;
            m_localCenterA = m_bodyA.Sweep.localCenter;
            m_localCenterB = m_bodyB.Sweep.localCenter;
            m_invMassA = m_bodyA.InvertedMass;
            m_invMassB = m_bodyB.InvertedMass;
            m_invIA = m_bodyA.InvertedI;
            m_invIB = m_bodyB.InvertedI;

            b2Vec2 cA = m_bodyA.InternalPosition.c;
            float aA = m_bodyA.InternalPosition.a;
            b2Vec2 vA = m_bodyA.InternalVelocity.v;
            float wA = m_bodyA.InternalVelocity.w;

            b2Vec2 cB = m_bodyB.InternalPosition.c;
            float aB = m_bodyB.InternalPosition.a;
            b2Vec2 vB = m_bodyB.InternalVelocity.v;
            float wB = m_bodyB.InternalVelocity.w;

            b2Rot qA = new b2Rot(aA);
            b2Rot qB = new b2Rot(aB);

            m_rA = b2Math.b2Mul(qA, m_localAnchorA - m_localCenterA);
            m_rB = b2Math.b2Mul(qB, m_localAnchorB - m_localCenterB);
            m_u = cB + m_rB - cA - m_rA;

            // Handle singularity.
            float length = m_u.Length;
            if (length > b2Settings.b2_linearSlop)
            {
                m_u *= 1.0f / length;
            }
            else
            {
                m_u.Set(0.0f, 0.0f);
            }

            float crAu = b2Math.b2Cross(ref m_rA, ref m_u);
            float crBu = b2Math.b2Cross(ref m_rB, ref m_u);
            float invMass = m_invMassA + m_invIA * crAu * crAu + m_invMassB + m_invIB * crBu * crBu;

            // Compute the effective mass matrix.
            m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;

            if (m_frequencyHz > 0.0f)
            {
                float C = length - m_length;

                // Frequency
                float omega = 2.0f * (float)Math.PI * m_frequencyHz;

                // Damping coefficient
                float d = 2.0f * m_mass * m_dampingRatio * omega;

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

                // magic formulas
                float h = data.step.dt;
                m_gamma = h * (d + h * k);
                m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f;
                m_bias = C * h * k * m_gamma;

                invMass += m_gamma;
                m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
            }
            else
            {
                m_gamma = 0.0f;
                m_bias = 0.0f;
            }

            if (data.step.warmStarting)
            {
                // Scale the impulse to support a variable time step.
                m_impulse *= data.step.dtRatio;

                b2Vec2 P = m_impulse * m_u;
                vA -= m_invMassA * P;
                wA -= m_invIA * b2Math.b2Cross(ref m_rA, ref P);
                vB += m_invMassB * P;
                wB += m_invIB * b2Math.b2Cross(ref m_rB, ref P);
            }
            else
            {
                m_impulse = 0.0f;
            }

            m_bodyA.InternalVelocity.v = vA;
            m_bodyA.InternalVelocity.w = wA;
            m_bodyB.InternalVelocity.v = vB;
            m_bodyB.InternalVelocity.w = wB;
        }
예제 #28
0
        public override bool SolvePositionConstraints(b2SolverData data)
        {
            b2Vec2 cA = data.positions[m_indexA].c;
            float aA = data.positions[m_indexA].a;
            b2Vec2 cB = data.positions[m_indexB].c;
            float aB = data.positions[m_indexB].a;

            b2Rot qA = new b2Rot(aA);
            b2Rot qB = new b2Rot(aB);

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

            bool fixedRotation = (m_invIA + m_invIB == 0.0f);

            // Solve angular limit constraint.
            if (m_enableLimit && m_limitState != b2LimitState.e_inactiveLimit && fixedRotation == false)
            {
                float angle = aB - aA - m_referenceAngle;
                float limitImpulse = 0.0f;

                if (m_limitState == b2LimitState.e_equalLimits)
                {
                    // Prevent large angular corrections
                    float C = b2Math.b2Clamp(angle - m_lowerAngle, -b2Settings.b2_maxAngularCorrection, b2Settings.b2_maxAngularCorrection);
                    limitImpulse = -m_motorMass * C;
                    angularError = b2Math.b2Abs(C);
                }
                else if (m_limitState == b2LimitState.e_atLowerLimit)
                {
                    float C = angle - m_lowerAngle;
                    angularError = -C;

                    // Prevent large angular corrections and allow some slop.
                    C = b2Math.b2Clamp(C + b2Settings.b2_angularSlop, -b2Settings.b2_maxAngularCorrection, 0.0f);
                    limitImpulse = -m_motorMass * C;
                }
                else if (m_limitState == b2LimitState.e_atUpperLimit)
                {
                    float C = angle - m_upperAngle;
                    angularError = C;

                    // Prevent large angular corrections and allow some slop.
                    C = b2Math.b2Clamp(C - b2Settings.b2_angularSlop, 0.0f, b2Settings.b2_maxAngularCorrection);
                    limitImpulse = -m_motorMass * C;
                }

                aA -= m_invIA * limitImpulse;
                aB += m_invIB * limitImpulse;
            }

            // Solve point-to-point constraint.
            {
                qA.Set(aA);
                qB.Set(aB);
                b2Vec2 rA = b2Math.b2Mul(qA, m_localAnchorA - m_localCenterA);
                b2Vec2 rB = b2Math.b2Mul(qB, m_localAnchorB - m_localCenterB);

                b2Vec2 C = cB + rB - cA - rA;
                positionError = C.Length();

                float mA = m_invMassA, mB = m_invMassB;
                float iA = m_invIA, iB = m_invIB;

                b2Mat22 K = new b2Mat22();
                K.exx = mA + mB + iA * rA.y * rA.y + iB * rB.y * rB.y;
                K.exy = -iA * rA.x * rA.y - iB * rB.x * rB.y;
                K.eyx = K.ex.y;
                K.eyy = mA + mB + iA * rA.x * rA.x + iB * rB.x * rB.x;

                b2Vec2 impulse = -K.Solve(C);

                cA -= mA * impulse;
                aA -= iA * b2Math.b2Cross(rA, impulse);

                cB += mB * impulse;
                aB += iB * b2Math.b2Cross(rB, impulse);
            }

            data.positions[m_indexA].c = cA;
            data.positions[m_indexA].a = aA;
            data.positions[m_indexB].c = cB;
            data.positions[m_indexB].a = aB;

            return positionError <= b2Settings.b2_linearSlop && angularError <= b2Settings.b2_angularSlop;
        }
예제 #29
0
        public override void InitVelocityConstraints(b2SolverData data)
        {
            m_indexA = m_bodyA.IslandIndex;
            m_indexB = m_bodyB.IslandIndex;
            m_indexC = m_bodyC.IslandIndex;
            m_indexD = m_bodyD.IslandIndex;
            m_lcA = m_bodyA.Sweep.localCenter;
            m_lcB = m_bodyB.Sweep.localCenter;
            m_lcC = m_bodyC.Sweep.localCenter;
            m_lcD = m_bodyD.Sweep.localCenter;
            m_mA = m_bodyA.InvertedMass;
            m_mB = m_bodyB.InvertedMass;
            m_mC = m_bodyC.InvertedMass;
            m_mD = m_bodyD.InvertedMass;
            m_iA = m_bodyA.InvertedI;
            m_iB = m_bodyB.InvertedI;
            m_iC = m_bodyC.InvertedI;
            m_iD = m_bodyD.InvertedI;

            b2Vec2 cA = data.positions[m_indexA].c;
            float aA = data.positions[m_indexA].a;
            b2Vec2 vA = data.velocities[m_indexA].v;
            float wA = data.velocities[m_indexA].w;

            b2Vec2 cB = data.positions[m_indexB].c;
            float aB = data.positions[m_indexB].a;
            b2Vec2 vB = data.velocities[m_indexB].v;
            float wB = data.velocities[m_indexB].w;

            b2Vec2 cC = data.positions[m_indexC].c;
            float aC = data.positions[m_indexC].a;
            b2Vec2 vC = data.velocities[m_indexC].v;
            float wC = data.velocities[m_indexC].w;

            b2Vec2 cD = data.positions[m_indexD].c;
            float aD = data.positions[m_indexD].a;
            b2Vec2 vD = data.velocities[m_indexD].v;
            float wD = data.velocities[m_indexD].w;

            b2Rot qA = new b2Rot(aA);
            b2Rot qB = new b2Rot(aB);
            b2Rot qC = new b2Rot(aC);
            b2Rot qD = new b2Rot(aD);

            m_mass = 0.0f;

            if (m_typeA == b2JointType.e_revoluteJoint)
            {
                m_JvAC.SetZero();
                m_JwA = 1.0f;
                m_JwC = 1.0f;
                m_mass += m_iA + m_iC;
            }
            else
            {
                b2Vec2 u = b2Math.b2Mul(qC, m_localAxisC);
                b2Vec2 rC = b2Math.b2Mul(qC, m_localAnchorC - m_lcC);
                b2Vec2 rA = b2Math.b2Mul(qA, m_localAnchorA - m_lcA);
                m_JvAC = u;
                m_JwC = b2Math.b2Cross(rC, u);
                m_JwA = b2Math.b2Cross(rA, u);
                m_mass += m_mC + m_mA + m_iC * m_JwC * m_JwC + m_iA * m_JwA * m_JwA;
            }

            if (m_typeB == b2JointType.e_revoluteJoint)
            {
                m_JvBD.SetZero();
                m_JwB = m_ratio;
                m_JwD = m_ratio;
                m_mass += m_ratio * m_ratio * (m_iB + m_iD);
            }
            else
            {
                b2Vec2 u = b2Math.b2Mul(qD, m_localAxisD);
                b2Vec2 rD = b2Math.b2Mul(qD, m_localAnchorD - m_lcD);
                b2Vec2 rB = b2Math.b2Mul(qB, m_localAnchorB - m_lcB);
                m_JvBD = m_ratio * u;
                m_JwD = m_ratio * b2Math.b2Cross(rD, u);
                m_JwB = m_ratio * b2Math.b2Cross(rB, u);
                m_mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * m_JwD * m_JwD + m_iB * m_JwB * m_JwB;
            }

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

            if (data.step.warmStarting)
            {
                vA += (m_mA * m_impulse) * m_JvAC;
                wA += m_iA * m_impulse * m_JwA;
                vB += (m_mB * m_impulse) * m_JvBD;
                wB += m_iB * m_impulse * m_JwB;
                vC -= (m_mC * m_impulse) * m_JvAC;
                wC -= m_iC * m_impulse * m_JwC;
                vD -= (m_mD * m_impulse) * m_JvBD;
                wD -= m_iD * m_impulse * m_JwD;
            }
            else
            {
                m_impulse = 0.0f;
            }

            data.velocities[m_indexA].v = vA;
            data.velocities[m_indexA].w = wA;
            data.velocities[m_indexB].v = vB;
            data.velocities[m_indexB].w = wB;
            data.velocities[m_indexC].v = vC;
            data.velocities[m_indexC].w = wC;
            data.velocities[m_indexD].v = vD;
            data.velocities[m_indexD].w = wD;
        }