Beispiel #1
0
        public override void SolveVelocityConstraints(SolverData data)
        {
            Vec2  vB = data.Velocities[IndexB].V;
            float wB = data.Velocities[IndexB].W;

            // Cdot = v + cross(w, r)
            Vec2 Cdot = Pool.PopVec2();

            Vec2.CrossToOutUnsafe(wB, RB, Cdot);
            Cdot.AddLocal(vB);

            Vec2 impulse = Pool.PopVec2();
            Vec2 temp    = Pool.PopVec2();

            temp.Set(m_impulse).MulLocal(m_gamma).AddLocal(m_C).AddLocal(Cdot).NegateLocal();
            Mat22.MulToOutUnsafe(m_mass, temp, impulse);

            Vec2 oldImpulse = temp;

            oldImpulse.Set(m_impulse);
            m_impulse.AddLocal(impulse);
            float maxImpulse = data.Step.Dt * m_maxForce;

            if (m_impulse.LengthSquared() > maxImpulse * maxImpulse)
            {
                m_impulse.MulLocal(maxImpulse / m_impulse.Length());
            }
            impulse.Set(m_impulse).SubLocal(oldImpulse);

            vB.X += InvMassB * m_impulse.X;
            vB.Y += InvMassB * m_impulse.Y;
            wB   += InvIB * Vec2.Cross(RB, impulse);

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

            Pool.PushVec2(3);
        }
Beispiel #2
0
        public void SolveVelocityConstraints()
        {
            for (int i = 0; i < Count; ++i)
            {
                ContactVelocityConstraint vc = VelocityConstraints[i];

                int indexA = vc.IndexA;
                int indexB = vc.IndexB;

                float mA         = vc.InvMassA;
                float mB         = vc.InvMassB;
                float iA         = vc.InvIA;
                float iB         = vc.InvIB;
                int   pointCount = vc.PointCount;

                Vec2  vA = Velocities[indexA].V;
                float wA = Velocities[indexA].W;
                Vec2  vB = Velocities[indexB].V;
                float wB = Velocities[indexB].W;
                //Debug.Assert(wA == 0);
                //Debug.Assert(wB == 0);

                Vec2 normal = vc.Normal;
                //Vec2.crossToOutUnsafe(normal, 1f, tangent);
                tangent.X = 1.0f * vc.Normal.Y;
                tangent.Y = (-1.0f) * vc.Normal.X;
                float friction = vc.Friction;

                Debug.Assert(pointCount == 1 || pointCount == 2);

                // Solve tangent constraints
                for (int j = 0; j < pointCount; ++j)
                {
                    ContactVelocityConstraint.VelocityConstraintPoint vcp = vc.Points[j];
                    //Vec2.crossToOutUnsafe(wA, vcp.rA, temp);
                    //Vec2.crossToOutUnsafe(wB, vcp.rB, dv);
                    //dv.addLocal(vB).subLocal(vA).subLocal(temp);
                    Vec2 a = vcp.RA;

                    dv.X = (-wB) * vcp.RB.Y + vB.X - vA.X + wA * a.Y;
                    dv.Y = wB * vcp.RB.X + vB.Y - vA.Y - wA * a.X;

                    // Compute tangent force
                    float vt     = dv.X * tangent.X + dv.Y * tangent.Y - vc.TangentSpeed;
                    float lambda = vcp.TangentMass * (-vt);

                    // Clamp the accumulated force
                    float maxFriction = friction * vcp.NormalImpulse;
                    float newImpulse  = MathUtils.Clamp(vcp.TangentImpulse + lambda, -maxFriction, maxFriction);
                    lambda             = newImpulse - vcp.TangentImpulse;
                    vcp.TangentImpulse = newImpulse;

                    // Apply contact impulse
                    // Vec2 P = lambda * tangent;

                    float Px = tangent.X * lambda;
                    float Py = tangent.Y * lambda;

                    // vA -= invMassA * P;
                    vA.X -= Px * mA;
                    vA.Y -= Py * mA;
                    wA   -= iA * (vcp.RA.X * Py - vcp.RA.Y * Px);

                    // vB += invMassB * P;
                    vB.X += Px * mB;
                    vB.Y += Py * mB;
                    wB   += iB * (vcp.RB.X * Py - vcp.RB.Y * Px);

                    //Console.WriteLine("tangent solve velocity (point "+j+") for " + indexA + " is " + vA.x + "," + vA.y + " rot " + wA);
                    //Console.WriteLine("tangent solve velocity (point "+j+") for " + indexB + " is " + vB.x + "," + vB.y + " rot " + wB);
                }

                // Solve normal constraints
                if (vc.PointCount == 1)
                {
                    ContactVelocityConstraint.VelocityConstraintPoint vcp = vc.Points[0];
                    Vec2 a1 = vcp.RA;

                    // Relative velocity at contact
                    //Vec2 dv = vB + Cross(wB, vcp.rB) - vA - Cross(wA, vcp.rA);

                    //Vec2.crossToOut(wA, vcp.rA, temp1);
                    //Vec2.crossToOut(wB, vcp.rB, dv);
                    //dv.addLocal(vB).subLocal(vA).subLocal(temp1);

                    dv.X = (-wB) * vcp.RB.Y + vB.X - vA.X + wA * a1.Y;
                    dv.Y = wB * vcp.RB.X + vB.Y - vA.Y - wA * a1.X;

                    // Compute normal impulse
                    float vn     = dv.X * normal.X + dv.Y * normal.Y;
                    float lambda = (-vcp.NormalMass) * (vn - vcp.VelocityBias);

                    // Clamp the accumulated impulse
                    float a          = vcp.NormalImpulse + lambda;
                    float newImpulse = (a > 0.0f ? a : 0.0f);
                    lambda = newImpulse - vcp.NormalImpulse;
                    //Debug.Assert(newImpulse == 0);
                    vcp.NormalImpulse = newImpulse;

                    // Apply contact impulse
                    float Px = normal.X * lambda;
                    float Py = normal.Y * lambda;

                    // vA -= invMassA * P;
                    vA.X -= Px * mA;
                    vA.Y -= Py * mA;
                    wA   -= iA * (vcp.RA.X * Py - vcp.RA.Y * Px);
                    //Debug.Assert(vA.x == 0);

                    // vB += invMassB * P;
                    vB.X += Px * mB;
                    vB.Y += Py * mB;
                    wB   += iB * (vcp.RB.X * Py - vcp.RB.Y * Px);
                    //Debug.Assert(vB.x == 0);
                }
                else
                {
                    // Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on
                    // Box2D_Lite).
                    // Build the mini LCP for this contact patch
                    //
                    // vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2
                    //
                    // A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n )
                    // b = vn_0 - velocityBias
                    //
                    // The system is solved using the "Total enumeration method" (s. Murty). The complementary
                    // constraint vn_i * x_i
                    // implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D
                    // contact problem the cases
                    // vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be
                    // tested. The first valid
                    // solution that satisfies the problem is chosen.
                    //
                    // In order to account of the accumulated impulse 'a' (because of the iterative nature of
                    // the solver which only requires
                    // that the accumulated impulse is clamped and not the incremental impulse) we change the
                    // impulse variable (x_i).
                    //
                    // Substitute:
                    //
                    // x = a + d
                    //
                    // a := old total impulse
                    // x := new total impulse
                    // d := incremental impulse
                    //
                    // For the current iteration we extend the formula for the incremental impulse
                    // to compute the new total impulse:
                    //
                    // vn = A * d + b
                    // = A * (x - a) + b
                    // = A * x + b - A * a
                    // = A * x + b'
                    // b' = b - A * a;

                    ContactVelocityConstraint.VelocityConstraintPoint cp1 = vc.Points[0];
                    ContactVelocityConstraint.VelocityConstraintPoint cp2 = vc.Points[1];
                    a.X = cp1.NormalImpulse;
                    a.Y = cp2.NormalImpulse;

                    Debug.Assert(a.X >= 0.0f && a.Y >= 0.0f);
                    // Relative velocity at contact
                    // Vec2 dv1 = vB + Cross(wB, cp1.rB) - vA - Cross(wA, cp1.rA);
                    dv1.X = (-wB) * cp1.RB.Y + vB.X - vA.X + wA * cp1.RA.Y;
                    dv1.Y = wB * cp1.RB.X + vB.Y - vA.Y - wA * cp1.RA.X;

                    // Vec2 dv2 = vB + Cross(wB, cp2.rB) - vA - Cross(wA, cp2.rA);
                    dv2.X = (-wB) * cp2.RB.Y + vB.X - vA.X + wA * cp2.RA.Y;
                    dv2.Y = wB * cp2.RB.X + vB.Y - vA.Y - wA * cp2.RA.X;

                    // Compute normal velocity
                    float vn1 = dv1.X * normal.X + dv1.Y * normal.Y;
                    float vn2 = dv2.X * normal.X + dv2.Y * normal.Y;

                    b.X = vn1 - cp1.VelocityBias;
                    b.Y = vn2 - cp2.VelocityBias;
                    //Console.WriteLine("b is " + b.x + "," + b.y);

                    // Compute b'
                    Mat22 R = vc.K;
                    b.X -= (R.Ex.X * a.X + R.Ey.X * a.Y);
                    b.Y -= (R.Ex.Y * a.X + R.Ey.Y * a.Y);
                    //Console.WriteLine("b' is " + b.x + "," + b.y);

                    // final float k_errorTol = 1e-3f;
                    // B2_NOT_USED(k_errorTol);
                    for (; ;)
                    {
                        //
                        // Case 1: vn = 0
                        //
                        // 0 = A * x' + b'
                        //
                        // Solve for x':
                        //
                        // x' = - inv(A) * b'
                        //
                        // Vec2 x = - Mul(c.normalMass, b);
                        Mat22.MulToOutUnsafe(vc.NormalMass, b, x);
                        x.MulLocal(-1);

                        if (x.X >= 0.0f && x.Y >= 0.0f)
                        {
                            //Console.WriteLine("case 1");
                            // Get the incremental impulse
                            // Vec2 d = x - a;
                            d.Set(x).SubLocal(a);

                            // Apply incremental impulse
                            // Vec2 P1 = d.x * normal;
                            // Vec2 P2 = d.y * normal;
                            P1.Set(normal).MulLocal(d.X);
                            P2.Set(normal).MulLocal(d.Y);

                            /*
                             * vA -= invMassA * (P1 + P2); wA -= invIA * (Cross(cp1.rA, P1) + Cross(cp2.rA, P2));
                             *
                             * vB += invMassB * (P1 + P2); wB += invIB * (Cross(cp1.rB, P1) + Cross(cp2.rB, P2));
                             */

                            temp1.Set(P1).AddLocal(P2);
                            temp2.Set(temp1).MulLocal(mA);
                            vA.SubLocal(temp2);
                            temp2.Set(temp1).MulLocal(mB);
                            vB.AddLocal(temp2);
                            //Debug.Assert(vA.x == 0);
                            //Debug.Assert(vB.x == 0);

                            wA -= iA * (Vec2.Cross(cp1.RA, P1) + Vec2.Cross(cp2.RA, P2));
                            wB += iB * (Vec2.Cross(cp1.RB, P1) + Vec2.Cross(cp2.RB, P2));

                            // Accumulate
                            cp1.NormalImpulse = x.X;
                            cp2.NormalImpulse = x.Y;

                            /*
                             * #if B2_DEBUG_SOLVER == 1 // Postconditions dv1 = vB + Cross(wB, cp1.rB) - vA -
                             * Cross(wA, cp1.rA); dv2 = vB + Cross(wB, cp2.rB) - vA - Cross(wA, cp2.rA);
                             *
                             * // Compute normal velocity vn1 = Dot(dv1, normal); vn2 = Dot(dv2, normal);
                             *
                             * Debug.Assert(Abs(vn1 - cp1.velocityBias) < k_errorTol); Debug.Assert(Abs(vn2 - cp2.velocityBias)
                             * < k_errorTol); #endif
                             */
                            if (DEBUG_SOLVER)
                            {
                                // Postconditions
                                Vec2 _dv1 = vB.Add(Vec2.Cross(wB, cp1.RB).SubLocal(vA).SubLocal(Vec2.Cross(wA, cp1.RA)));
                                Vec2 _dv2 = vB.Add(Vec2.Cross(wB, cp2.RB).SubLocal(vA).SubLocal(Vec2.Cross(wA, cp2.RA)));
                                // Compute normal velocity
                                vn1 = Vec2.Dot(_dv1, normal);
                                vn2 = Vec2.Dot(_dv2, normal);

                                Debug.Assert(MathUtils.Abs(vn1 - cp1.VelocityBias) < ERROR_TO_I);
                                Debug.Assert(MathUtils.Abs(vn2 - cp2.VelocityBias) < ERROR_TO_I);
                            }
                            break;
                        }

                        //
                        // Case 2: vn1 = 0 and x2 = 0
                        //
                        // 0 = a11 * x1' + a12 * 0 + b1'
                        // vn2 = a21 * x1' + a22 * 0 + '
                        //
                        x.X = (-cp1.NormalMass) * b.X;
                        x.Y = 0.0f;
                        vn1 = 0.0f;
                        vn2 = vc.K.Ex.Y * x.X + b.Y;

                        if (x.X >= 0.0f && vn2 >= 0.0f)
                        {
                            //Console.WriteLine("case 2");
                            // Get the incremental impulse
                            d.Set(x).SubLocal(a);

                            // Apply incremental impulse
                            // Vec2 P1 = d.x * normal;
                            // Vec2 P2 = d.y * normal;
                            P1.Set(normal).MulLocal(d.X);
                            P2.Set(normal).MulLocal(d.Y);

                            /*
                             * Vec2 P1 = d.x * normal; Vec2 P2 = d.y * normal; vA -= invMassA * (P1 + P2); wA -=
                             * invIA * (Cross(cp1.rA, P1) + Cross(cp2.rA, P2));
                             *
                             * vB += invMassB * (P1 + P2); wB += invIB * (Cross(cp1.rB, P1) + Cross(cp2.rB, P2));
                             */

                            temp1.Set(P1).AddLocal(P2);
                            temp2.Set(temp1).MulLocal(mA);
                            vA.SubLocal(temp2);
                            temp2.Set(temp1).MulLocal(mB);
                            vB.AddLocal(temp2);
                            //Debug.Assert(vA.x == 0);
                            //Debug.Assert(vB.x == 0);

                            wA -= iA * (Vec2.Cross(cp1.RA, P1) + Vec2.Cross(cp2.RA, P2));
                            wB += iB * (Vec2.Cross(cp1.RB, P1) + Vec2.Cross(cp2.RB, P2));


                            // Accumulate
                            //Debug.Assert(x.x == 0 && x.y == 0);
                            cp1.NormalImpulse = x.X;
                            cp2.NormalImpulse = x.Y;

                            /*
                             * #if B2_DEBUG_SOLVER == 1 // Postconditions dv1 = vB + Cross(wB, cp1.rB) - vA -
                             * Cross(wA, cp1.rA);
                             *
                             * // Compute normal velocity vn1 = Dot(dv1, normal);
                             *
                             * Debug.Assert(Abs(vn1 - cp1.velocityBias) < k_errorTol); #endif
                             */
                            if (DEBUG_SOLVER)
                            {
                                // Postconditions
                                Vec2 _dv1 = vB.Add(Vec2.Cross(wB, cp1.RB).SubLocal(vA).SubLocal(Vec2.Cross(wA, cp1.RA)));
                                // Compute normal velocity
                                vn1 = Vec2.Dot(_dv1, normal);

                                Debug.Assert(MathUtils.Abs(vn1 - cp1.VelocityBias) < ERROR_TO_I);
                            }
                            break;
                        }


                        //
                        // Case 3: wB = 0 and x1 = 0
                        //
                        // vn1 = a11 * 0 + a12 * x2' + b1'
                        // 0 = a21 * 0 + a22 * x2' + '
                        //
                        x.X = 0.0f;
                        x.Y = (-cp2.NormalMass) * b.Y;
                        vn1 = vc.K.Ey.X * x.Y + b.X;
                        vn2 = 0.0f;

                        if (x.Y >= 0.0f && vn1 >= 0.0f)
                        {
                            //Console.WriteLine("case 3");
                            // Resubstitute for the incremental impulse
                            d.Set(x).SubLocal(a);

                            // Apply incremental impulse

                            /*
                             * Vec2 P1 = d.x * normal; Vec2 P2 = d.y * normal; vA -= invMassA * (P1 + P2); wA -=
                             * invIA * (Cross(cp1.rA, P1) + Cross(cp2.rA, P2));
                             *
                             * vB += invMassB * (P1 + P2); wB += invIB * (Cross(cp1.rB, P1) + Cross(cp2.rB, P2));
                             */

                            P1.Set(normal).MulLocal(d.X);
                            P2.Set(normal).MulLocal(d.Y);

                            temp1.Set(P1).AddLocal(P2);
                            temp2.Set(temp1).MulLocal(mA);
                            vA.SubLocal(temp2);
                            temp2.Set(temp1).MulLocal(mB);
                            vB.AddLocal(temp2);
                            //Debug.Assert(vA.x == 0);
                            //Debug.Assert(vB.x == 0);

                            wA -= iA * (Vec2.Cross(cp1.RA, P1) + Vec2.Cross(cp2.RA, P2));
                            wB += iB * (Vec2.Cross(cp1.RB, P1) + Vec2.Cross(cp2.RB, P2));

                            // Accumulate
                            //Debug.Assert(x.x == 0 && x.y == 0);
                            cp1.NormalImpulse = x.X;
                            cp2.NormalImpulse = x.Y;

                            /*
                             * #if B2_DEBUG_SOLVER == 1 // Postconditions dv2 = vB + Cross(wB, cp2.rB) - vA -
                             * Cross(wA, cp2.rA);
                             *
                             * // Compute normal velocity vn2 = Dot(dv2, normal);
                             *
                             * Debug.Assert(Abs(vn2 - cp2.velocityBias) < k_errorTol); #endif
                             */
                            if (DEBUG_SOLVER)
                            {
                                // Postconditions
                                Vec2 _dv2 =
                                    vB.Add(Vec2.Cross(wB, cp2.RB).SubLocal(vA).SubLocal(Vec2.Cross(wA, cp2.RA)));
                                // Compute normal velocity
                                vn2 = Vec2.Dot(_dv2, normal);

                                Debug.Assert(MathUtils.Abs(vn2 - cp2.VelocityBias) < ERROR_TO_I);
                            }
                            break;
                        }

                        //
                        // Case 4: x1 = 0 and x2 = 0
                        //
                        // vn1 = b1
                        // vn2 = ;
                        x.X = 0.0f;
                        x.Y = 0.0f;
                        vn1 = b.X;
                        vn2 = b.Y;

                        if (vn1 >= 0.0f && vn2 >= 0.0f)
                        {
                            //Console.WriteLine("case 4");
                            // Resubstitute for the incremental impulse
                            d.Set(x).SubLocal(a);

                            // Apply incremental impulse

                            /*
                             * Vec2 P1 = d.x * normal; Vec2 P2 = d.y * normal; vA -= invMassA * (P1 + P2); wA -=
                             * invIA * (Cross(cp1.rA, P1) + Cross(cp2.rA, P2));
                             *
                             * vB += invMassB * (P1 + P2); wB += invIB * (Cross(cp1.rB, P1) + Cross(cp2.rB, P2));
                             */

                            P1.Set(normal).MulLocal(d.X);
                            P2.Set(normal).MulLocal(d.Y);

                            temp1.Set(P1).AddLocal(P2);
                            temp2.Set(temp1).MulLocal(mA);
                            vA.SubLocal(temp2);
                            temp2.Set(temp1).MulLocal(mB);
                            vB.AddLocal(temp2);
                            //Debug.Assert(vA.x == 0);
                            //Debug.Assert(vB.x == 0);

                            wA -= iA * (Vec2.Cross(cp1.RA, P1) + Vec2.Cross(cp2.RA, P2));
                            wB += iB * (Vec2.Cross(cp1.RB, P1) + Vec2.Cross(cp2.RB, P2));


                            // Accumulate
                            //Debug.Assert(x.x == 0 && x.y == 0);
                            cp1.NormalImpulse = x.X;
                            cp2.NormalImpulse = x.Y;

                            break;
                        }

                        // No solution, give up. This is hit sometimes, but it doesn't seem to matter.
                        break;
                    }
                }

                Velocities[indexA].V.Set(vA);
                Velocities[indexA].W = wA;
                Velocities[indexB].V.Set(vB);
                Velocities[indexB].W = wB;

                //Console.WriteLine("Ending velocity for " + indexA + " is " + vA.x + "," + vA.y + " rot " + wA);
                //Console.WriteLine("Ending velocity for " + indexB + " is " + vB.x + "," + vB.y + " rot " + wB);
            }
        }
Beispiel #3
0
        public override void SolveVelocityConstraints(SolverData data)
        {
            Vec2  vA = data.Velocities[IndexA].V;
            float wA = data.Velocities[IndexA].W;
            Vec2  vB = data.Velocities[IndexB].V;
            float wB = data.Velocities[IndexB].W;

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

            float h = data.Step.Dt;

            // Solve angular friction
            {
                float Cdot    = wB - wA;
                float impulse = (-AngularMass) * Cdot;

                float oldImpulse = m_angularImpulse;
                float maxImpulse = h * m_maxTorque;
                m_angularImpulse = MathUtils.Clamp(m_angularImpulse + impulse, -maxImpulse, maxImpulse);
                impulse          = m_angularImpulse - oldImpulse;

                wA -= iA * impulse;
                wB += iB * impulse;
            }

            // Solve linear friction
            {
                Vec2 Cdot = Pool.PopVec2();
                Vec2 temp = Pool.PopVec2();

                Vec2.CrossToOutUnsafe(wA, RA, temp);
                Vec2.CrossToOutUnsafe(wB, RB, Cdot);
                Cdot.AddLocal(vB).SubLocal(vA).SubLocal(temp);

                Vec2 impulse = Pool.PopVec2();
                Mat22.MulToOutUnsafe(LinearMass, Cdot, impulse);
                impulse.NegateLocal();


                Vec2 oldImpulse = Pool.PopVec2();
                oldImpulse.Set(m_linearImpulse);
                m_linearImpulse.AddLocal(impulse);

                float maxImpulse = h * m_maxForce;

                if (m_linearImpulse.LengthSquared() > maxImpulse * maxImpulse)
                {
                    m_linearImpulse.Normalize();
                    m_linearImpulse.MulLocal(maxImpulse);
                }

                impulse.Set(m_linearImpulse).SubLocal(oldImpulse);

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

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

            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.PushVec2(4);
        }