Ejemplo n.º 1
0
        public void ShutDown()
        {
            for (int i = 0; i < Contacts.Count; ++i)
            {
                ContactConstraintState c  = Contacts[i];
                ContactConstraint      cc = Island.Contacts[i];

                for (int j = 0; j < c.contactCount; ++j)
                {
                    Contact      oc = cc.manifold.contacts[j];
                    ContactState cs = c.contacts[j];
                    oc.normalImpulse    = cs.normalImpulse;
                    oc.tangentImpulse   = cs.tangentImpulse;
                    oc.bitangentImpulse = cs.bitangentImpulse;
                }
            }
        }
Ejemplo n.º 2
0
        public void Clear()
        {
            Bodies.Clear();
            Velocities.Clear();
            Contacts.Clear();

            foreach (var state in ContactStates)
            {
                for (int i = 0; i < state.contactCount; i++)
                {
                    ContactState.Free(state.contacts[i]);
                }
                ContactConstraintState.Free(state);
                // Array.Clear(state.contacts, 0, state.contactCount);
            }

            ContactStates.Clear();
        }
Ejemplo n.º 3
0
        public void Initialize()
        {
            for (int i = 0; i < Contacts.Count; ++i)
            {
                ContactConstraint cc = Contacts[i];

                ContactConstraintState c = ContactStates[i];

                c.centerA          = cc.bodyA.WorldCenter;
                c.centerB          = cc.bodyB.WorldCenter;
                c.iA               = cc.bodyA.InvInertiaWorld;
                c.iB               = cc.bodyB.InvInertiaWorld;
                c.mA               = cc.bodyA.InvMass;
                c.mB               = cc.bodyB.InvMass;
                c.restitution      = cc.restitution;
                c.friction         = cc.friction;
                c.indexA           = cc.bodyA.IslandIndex;
                c.indexB           = cc.bodyB.IslandIndex;
                c.normal           = cc.manifold.normal;
                c.tangentVectors   = cc.manifold.tangentVectors;
                c.bitangentVectors = cc.manifold.bitangentVectors;
                c.contactCount     = cc.manifold.contactCount;

                for (int j = 0; j < c.contactCount; ++j)
                {
                    ContactState s  = c.contacts[j] = ContactState.Allocate();
                    Contact      cp = cc.manifold.contacts[j];
                    s.ra               = cp.position - c.centerA;
                    s.rb               = cp.position - c.centerB;
                    s.penetration      = cp.penetration;
                    s.normalImpulse    = cp.normalImpulse;
                    s.tangentImpulse   = cp.tangentImpulse;
                    s.bitangentImpulse = cp.bitangentImpulse;
                }
            }
        }
Ejemplo n.º 4
0
 static public void Free(ContactState instance)
 {
     heap.Push(instance);
 }
Ejemplo n.º 5
0
        public void Solve()
        {
            for (int i = 0; i < Contacts.Count; ++i)
            {
                ContactConstraintState cs = Contacts[i];

                Vec3 vA = Velocities[cs.indexA].v;
                Vec3 wA = Velocities[cs.indexA].w;
                Vec3 vB = Velocities[cs.indexB].v;
                Vec3 wB = Velocities[cs.indexB].w;

                for (int j = 0; j < cs.contactCount; ++j)
                {
                    ContactState c = cs.contacts[j];

                    // relative velocity at contact
                    Vec3 dv = vB + Vec3.Cross(wB, c.rb) - vA - Vec3.Cross(wA, c.ra);

                    // Friction
                    if (EnableFriction)
                    {
                        {
                            double lambda = -Vec3.Dot(dv, cs.tangentVectors) * c.tangentMass;

                            // Calculate frictional impulse
                            double maxLambda = cs.friction * c.normalImpulse;

                            // Clamp frictional impulse
                            double oldPT = c.tangentImpulse;
                            c.tangentImpulse = Clamp(-maxLambda, maxLambda, oldPT + lambda);
                            lambda           = c.tangentImpulse - oldPT;

                            // Apply friction impulse
                            Vec3 impulse = cs.tangentVectors * lambda;
                            vA -= impulse * cs.mA;
                            wA -= cs.iA * Vec3.Cross(c.ra, impulse);

                            vB += impulse * cs.mB;
                            wB += cs.iB * Vec3.Cross(c.rb, impulse);
                        }
                        {
                            double lambda = -Vec3.Dot(dv, cs.bitangentVectors) * c.bitangentMass;

                            // Calculate frictional impulse
                            double maxLambda = cs.friction * c.normalImpulse;

                            // Clamp frictional impulse
                            double oldPT = c.bitangentImpulse;
                            c.bitangentImpulse = Clamp(-maxLambda, maxLambda, oldPT + lambda);
                            lambda             = c.bitangentImpulse - oldPT;

                            // Apply friction impulse
                            Vec3 impulse = cs.bitangentVectors * lambda;
                            vA -= impulse * cs.mA;
                            wA -= cs.iA * Vec3.Cross(c.ra, impulse);

                            vB += impulse * cs.mB;
                            wB += cs.iB * Vec3.Cross(c.rb, impulse);
                        }
                    }

                    // Normal
                    {
                        dv = vB + Vec3.Cross(wB, c.rb) - vA - Vec3.Cross(wA, c.ra);

                        // Normal impulse
                        double vn = Vec3.Dot(dv, cs.normal);

                        // Factor in positional bias to calculate impulse scalar j
                        double lambda = c.normalMass * (-vn + c.bias);

                        // Clamp impulse
                        double tempPN = c.normalImpulse;
                        c.normalImpulse = Math.Max(tempPN + lambda, 0);
                        lambda          = c.normalImpulse - tempPN;

                        // Apply impulse
                        Vec3 impulse = cs.normal * lambda;
                        vA -= impulse * cs.mA;
                        wA -= cs.iA * Vec3.Cross(c.ra, impulse);

                        vB += impulse * cs.mB;
                        wB += cs.iB * Vec3.Cross(c.rb, impulse);
                    }
                }

                Velocities[cs.indexA] = new VelocityState {
                    v = vA, w = wA
                };
                Velocities[cs.indexB] = new VelocityState {
                    v = vB, w = wB
                };
            }
        }
Ejemplo n.º 6
0
        public void PreSolve(double dt)
        {
            for (int i = 0; i < Contacts.Count; ++i)
            {
                ContactConstraintState cs = Contacts[i];

                Vec3 vA = Velocities[cs.indexA].v;
                Vec3 wA = Velocities[cs.indexA].w;
                Vec3 vB = Velocities[cs.indexB].v;
                Vec3 wB = Velocities[cs.indexB].w;

                for (int j = 0; j < cs.contactCount; ++j)
                {
                    ContactState c = cs.contacts[j];

                    // Precalculate JM^-1JT for contact and friction constraints
                    Vec3   raCn = Vec3.Cross(c.ra, cs.normal);
                    Vec3   rbCn = Vec3.Cross(c.rb, cs.normal);
                    double nm   = cs.mA + cs.mB;

                    nm          += Vec3.Dot(raCn, cs.iA * raCn) + Vec3.Dot(rbCn, cs.iB * rbCn);
                    c.normalMass = Invert(nm);


                    {
                        Vec3 raCt = Vec3.Cross(cs.tangentVectors, c.ra);
                        Vec3 rbCt = Vec3.Cross(cs.tangentVectors, c.rb);
                        var  tm   = nm + Vec3.Dot(raCt, cs.iA * raCt) + Vec3.Dot(rbCt, cs.iB * rbCt);
                        c.tangentMass = Invert(tm);
                    }
                    {
                        Vec3 raCt = Vec3.Cross(cs.bitangentVectors, c.ra);
                        Vec3 rbCt = Vec3.Cross(cs.bitangentVectors, c.rb);
                        var  tm   = nm + Vec3.Dot(raCt, cs.iA * raCt) + Vec3.Dot(rbCt, cs.iB * rbCt);
                        c.bitangentMass = Invert(tm);
                    }

                    // Precalculate bias factor
                    c.bias = -Q3_BAUMGARTE * (1 / dt) * Math.Min(0, c.penetration + Q3_PENETRATION_SLOP);

                    // Warm start contact
                    Vec3 P = cs.normal * c.normalImpulse;

                    if (EnableFriction)
                    {
                        P += cs.tangentVectors * c.tangentImpulse;
                        P += cs.bitangentVectors * c.bitangentImpulse;
                    }

                    vA -= P * cs.mA;
                    wA -= cs.iA * Vec3.Cross(c.ra, P);

                    vB += P * cs.mB;
                    wB += cs.iB * Vec3.Cross(c.rb, P);

                    // Add in restitution bias
                    double dv = Vec3.Dot(vB + Vec3.Cross(wB, c.rb) - vA - Vec3.Cross(wA, c.ra), cs.normal);

                    if (dv < -1)
                    {
                        c.bias += -(cs.restitution) * dv;
                    }
                }

                Velocities[cs.indexA] = new VelocityState {
                    v = vA, w = wA
                };
                Velocities[cs.indexB] = new VelocityState {
                    v = vB, w = wB
                };
            }
        }