Ejemplo n.º 1
0
        void CheckCollisions()
        {
            // Check for any collisions
            for (int i = 0; i < m_rigidBodies.Count; i++)
            {
                RigidBody_c b1 = m_rigidBodies[i];

                for (int j = i + 1; j < m_rigidBodies.Count; j++)
                {
                    RigidBody_c b2 = m_rigidBodies[j];

                    float cullingRadius = b1.maxRadius + b2.maxRadius;

                    if ((b1.body.x - b2.body.x).LengthSquared() > cullingRadius * cullingRadius)
                    {
                        continue;
                    }

                    //if (b1.body.inv_m==0.0f && b1.body.inv_m==0.0f) continue;

                    Vector3 n       = Vector3.Zero;
                    Vector3 p1      = Vector3.Zero;
                    Vector3 p2      = Vector3.Zero;
                    bool    haveHit =
                        Intersection_c.HasIntersection(b1.collideModel, b1.body.q, b1.body.x - MyMath.Rotate(b1.body.q, b1.body.com),
                                                       b2.collideModel, b2.body.q, b2.body.x - MyMath.Rotate(b2.body.q, b2.body.com),
                                                       out n,
                                                       out p1,
                                                       out p2);
                    if (haveHit)
                    {
                        Arbiter_c arb = ArbiterContainer_c.FindArbiter(ref b1, ref b2);

                        // Find the support points
                        Vector3 s1 = Collision.TransformSupportVert(b1.collideModel, b1.body.q, b1.body.x - MyMath.Rotate(b1.body.q, b1.body.com), -n);
                        Vector3 s2 = Collision.TransformSupportVert(b2.collideModel, b2.body.q, b2.body.x - MyMath.Rotate(b2.body.q, b2.body.com), n);

                        Vector3 pp1 = (s1 - p1) * Vector3.Dot(n, n) + p1;
                        Vector3 pp2 = (s2 - p2) * Vector3.Dot(n, n) + p2;

                        arb.AddContact(pp1, pp2, n);
                    }
                }
            }
        }
Ejemplo n.º 2
0
        void Update(float dt)
        {
            Contact_c.gTimeStamp++;


            //float linDrag = 0.99f;
            //float angDrag = 0.98f;

            /*
             * //*****Integrate******
             * for (int i=0; i < m_rigidBodies.Count; i++)
             * {
             *      Body_c b = m_rigidBodies[i].body;
             *
             *      b.x += b.v * dt;
             *      Debug_c.Valid(b.x);
             *
             *      b.v += new Vector3(0, -400.8f, 0) * dt * b.m;
             *      Debug_c.Valid(b.v);
             *
             *      Quaternion temp = MyMath.Mult(new Quaternion(b.omega.X, b.omega.Y, b.omega.Z, 0), b.q) * 0.5f;
             *      b.q = b.q + temp * dt;
             *      b.q.Normalize();
             *
             *      b.v			*= linDrag;
             *      b.omega		*= angDrag;
             *      Debug_c.Valid(b.omega);
             * }
             */


                        #if USE_IMPULSES
            m_prevSteps[m_curStep].Clear();
            for (int i = 0; i < m_rigidBodies.Count; i++)
            {
                m_prevSteps[m_curStep].Add(new Body_c(m_rigidBodies[i].body));
            }
            m_curStep = (m_curStep + 1) % m_prevSteps.Count();


            // Process all collisions
            //if (false)
            {
                for (int i = 0; i < m_rigidBodies.Count; i++)
                {
                    Body_c b = m_rigidBodies[i].body;
                    b.StoreState();
                }
                for (int i = 0; i < m_rigidBodies.Count; i++)
                {
                    Body_c b = m_rigidBodies[i].body;
                    b.UpdateVel(dt);
                    b.UpdatePos(dt);
                }

                CheckCollisions();
                ArbiterContainer_c.SortInYDirection();

                for (int i = 0; i < m_rigidBodies.Count; i++)
                {
                    Body_c b = m_rigidBodies[i].body;
                    b.RestoreState();
                }

                for (int iteration = 0; iteration < 4; iteration++)
                {
                    for (int k = 0; k < ArbiterContainer_c.arbiterArray.Count; k++)
                    {
                        for (int a = 0; a < ArbiterContainer_c.arbiterArray[k].arbiter.contacts.Count; a++)
                        {
                            Contact_c contact = ArbiterContainer_c.arbiterArray[k].arbiter.contacts[a];


                            bool zapIt = false;

                            if (contact.Distance() > 0.01f && Contact_c.gTimeStamp != contact.timeStamp)
                            {
                                zapIt = true;
                            }

                            if (zapIt)
                            {
                                ArbiterContainer_c.arbiterArray[k].arbiter.contacts.RemoveAt(a);
                                a--;
                                continue;
                            }

                            contact.constraint.GenerateImpulse(0.9f);
                        }
                    }
                }
            }

            // Update Velocity
            for (int i = 0; i < m_rigidBodies.Count; i++)
            {
                Body_c b = m_rigidBodies[i].body;
                b.UpdateVel(dt);
            }



            // Process Contacts
            //if (false)
            {
                for (int i = 0; i < m_rigidBodies.Count; i++)
                {
                    Body_c b = m_rigidBodies[i].body;
                    b.StoreState();
                }

                for (int i = 0; i < m_rigidBodies.Count; i++)
                {
                    Body_c b = m_rigidBodies[i].body;
                    b.UpdatePos(dt);
                }

                CheckCollisions();
                ArbiterContainer_c.SortInYDirection();

                for (int i = 0; i < m_rigidBodies.Count; i++)
                {
                    Body_c b = m_rigidBodies[i].body;
                    b.RestoreState();
                }


                // For the shock propogation - should do a sort on order
                // but since this is our test code, I know I've added the rigidbodies
                // to the list in the order from bottom to top

                //if (false)
                for (int iteration = 0; iteration < 90; iteration++)
                {
                    for (int i = 0; i < m_rigidBodies.Count; i++)
                    {
                        RigidBody_c b1 = m_rigidBodies[i];

                        b1.body.inv_m = 0.0f;

                        //int j = i+1;
                        //if (j>m_rigidBodies.Count-1) continue;

                        for (int j = i + 1; j < m_rigidBodies.Count; j++)
                        {
                            RigidBody_c b2 = m_rigidBodies[j];

                            float cullingRadius = b1.maxRadius + b2.maxRadius;

                            if ((b1.body.x - b2.body.x).LengthSquared() > cullingRadius * cullingRadius)
                            {
                                continue;
                            }

                            b1.body.StoreState();
                            b2.body.StoreState();

                            b1.body.UpdatePos(dt);
                            b2.body.UpdatePos(dt);

                            /*
                             * for (int mm=0; mm < m_rigidBodies.Count; mm++)
                             * {
                             *      Body_c b = m_rigidBodies[mm].body;
                             *      b.StoreState();
                             * }
                             * for (int mm=0; mm < m_rigidBodies.Count; mm++)
                             * {
                             *      Body_c b = m_rigidBodies[mm].body;
                             *      b.UpdatePos(dt);
                             * }
                             */

                            Vector3 n       = Vector3.Zero;
                            Vector3 p1      = Vector3.Zero;
                            Vector3 p2      = Vector3.Zero;
                            bool    haveHit =
                                Intersection_c.HasIntersection(b1.collideModel, b1.body.q, b1.body.x - MyMath.Rotate(b1.body.q, b1.body.com),
                                                               b2.collideModel, b2.body.q, b2.body.x - MyMath.Rotate(b2.body.q, b2.body.com),
                                                               out n,
                                                               out p1,
                                                               out p2);

                            /*
                             * for (int mm=0; mm < m_rigidBodies.Count; mm++)
                             * {
                             *      Body_c b = m_rigidBodies[mm].body;
                             *      b.RestoreState();
                             * }
                             */

                            b1.body.RestoreState();
                            b2.body.RestoreState();

                            if (haveHit)
                            {
                                Contact_c c = new Contact_c(b1, b2, p1, p2, n);
                                c.constraint.GenerateImpulse(0.0f);
                            }
                        }
                    }

                    for (int i = 0; i < m_rigidBodies.Count; i++)
                    {
                        m_rigidBodies[i].body.inv_m = m_rigidBodies[i].body.inv_m_back;
                    }
                }



                int numContactSteps = 2;
                if (false)
                {
                    for (int step = 0; step < numContactSteps; step++)
                    {
                        for (int iteration = 0; iteration < 5; iteration++)
                        {
                            for (int k = 0; k < ArbiterContainer_c.arbiterArray.Count; k++)
                            {
                                for (int a = 0; a < ArbiterContainer_c.arbiterArray[k].arbiter.contacts.Count; a++)
                                {
                                    /*
                                     * for (int i=0; i < m_rigidBodies.Count; i++)
                                     * {
                                     *      Body_c b = m_rigidBodies[i].body;
                                     *      b.StoreState();
                                     * }
                                     * for (int i=0; i < m_rigidBodies.Count; i++)
                                     * {
                                     *      Body_c b = m_rigidBodies[i].body;
                                     *      b.UpdatePos(dt);
                                     * }
                                     * CheckCollisions();
                                     * for (int i=0; i < m_rigidBodies.Count; i++)
                                     * {
                                     *      Body_c b = m_rigidBodies[i].body;
                                     *      b.RestoreState();
                                     * }
                                     */

                                    Contact_c contact = ArbiterContainer_c.arbiterArray[k].arbiter.contacts[a];

                                    bool zapIt = false;

                                    if (contact.Distance() > 0.01f && Contact_c.gTimeStamp != contact.timeStamp)
                                    {
                                        zapIt = true;
                                    }

                                    if (zapIt)
                                    {
                                        ArbiterContainer_c.arbiterArray[k].arbiter.contacts.RemoveAt(a);
                                        a--;
                                        continue;
                                    }

                                    float ee = (numContactSteps - step - 1) * -1.0f / (float)numContactSteps;

                                    //if ( Math.Abs(contact.timeStamp - Contact_c.gTimeStamp) > 2 ) ee = -0.8f;

                                    //ee = 0.0f;

                                    contact.constraint.GenerateImpulse(ee);
                                }
                            }
                        }
                    }
                }


                // Shock propogation
                if (false)
                {
                    for (int iteration = 0; iteration < 5; iteration++)
                    {
                        for (int k = 0; k < ArbiterContainer_c.arbiterArray.Count; k++)
                        {
                            for (int a = 0; a < ArbiterContainer_c.arbiterArray[k].arbiter.contacts.Count; a++)
                            {
                                Contact_c contact = ArbiterContainer_c.arbiterArray[k].arbiter.contacts[a];

                                if (ArbiterContainer_c.arbiterArray[k].arbiter.contacts.Count > 0)
                                {
                                    ArbiterContainer_c.arbiterArray[k].arbiter.contacts[0].b1.body.inv_m = 0.0f;
                                }


                                bool zapIt = false;

                                if (contact.Distance() > 0.1f && Contact_c.gTimeStamp != contact.timeStamp)
                                {
                                    zapIt = true;
                                }

                                //if ( Math.Abs(contact.timeStamp - Contact_c.gTimeStamp) > 30 ) zapIt = true;

                                if (zapIt)
                                {
                                    ArbiterContainer_c.arbiterArray[k].arbiter.contacts.RemoveAt(a);
                                    a--;
                                    continue;
                                }

                                contact.constraint.GenerateImpulse(0.0f);
                            }
                        }

                        for (int i = 0; i < m_rigidBodies.Count; i++)
                        {
                            m_rigidBodies[i].body.inv_m = m_rigidBodies[i].body.inv_m_back;
                        }
                    }
                }
            }

            // Update Positions
            for (int i = 0; i < m_rigidBodies.Count; i++)
            {
                Body_c b = m_rigidBodies[i].body;
                b.UpdatePos(dt);

                //b.v			*= linDrag;
                b.omega *= 0.95f;                         //angDrag;
                Debug_c.Valid(b.omega);
            }
                        #endif //USE_IMPULSES



            // Resolve momentum exchanges
                        #if !USE_IMPULSES
            for (int i = 0; i < m_rigidBodies.Count; i++)
            {
                Body_c b = m_rigidBodies[i].body;
                b.UpdateVel(dt);
                b.UpdatePos(dt);
            }

            CheckCollisions();
            ArbiterContainer_c.SortInYDirection();

            for (int iteration = 0; iteration < 10; iteration++)
            {
                for (int k = 0; k < ArbiterContainer_c.arbiterArray.Count; k++)
                {
                    for (int a = 0; a < ArbiterContainer_c.arbiterArray[k].arbiter.contacts.Count; a++)
                    {
                        Contact_c contact = ArbiterContainer_c.arbiterArray[k].arbiter.contacts[a];


                        bool zapIt = false;

                        if (contact.Distance() > 0.1f && Contact_c.gTimeStamp != contact.timeStamp)
                        {
                            zapIt = true;
                        }

                        if (zapIt)
                        {
                            ArbiterContainer_c.arbiterArray[k].arbiter.contacts.RemoveAt(a);
                            a--;
                            continue;
                        }

                        if (iteration == 0)
                        {
                            contact.constraint.PrepareForIteration();
                        }
                        else
                        {
                            contact.constraint.Iterate();
                        }
                    }
                }
            }
                        #endif
        }