/starLiGHT.Engine/starLiGHT.Physic2D/starLiGHT.Physic2D/Collision/BroadPhase.cs

#region Using directives

using System;

using Microsoft.Xna.Framework;

using System.Diagnostics;

#endregion



#region COPYRIGHT



/*

    Copyright (c) 2008, 2009, 2010 

        Roland Rosenkranz (Glatzemann@email.de)

*/



#endregion



#region LICENSE



/*

    This file is part of starLiGHT.Collision.



    starLiGHT.Collision is free software: you can redistribute it and/or modify

    it under the terms of the GNU Lesser General Public License as published by

    the Free Software Foundation, either version 3 of the License, or

    (at your option) any later version.



    starLiGHT.Collision is distributed in the hope that it will be useful,

    but WITHOUT ANY WARRANTY; without even the implied warranty of

    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

    GNU Lesser General Public License for more details.



    You should have received a copy of the GNU Lesser General Public License

    along with starLiGHT.Collision.  If not, see <http://www.gnu.org/licenses/>.



    

    ADDITIONAL (commercial) LICENSES for starLiGHT.Collision are available on request.

*/



#endregion



#region Box2D Copyright

/*

* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com

*

* This software is provided 'as-is', without any express or implied

* warranty.  In no event will the authors be held liable for any damages

* arising from the use of this software.

* Permission is granted to anyone to use this software for any purpose,

* including commercial applications, and to alter it and redistribute it

* freely, subject to the following restrictions:

* 1. The origin of this software must not be misrepresented; you must not

* claim that you wrote the original software. If you use this software

* in a product, an acknowledgment in the product documentation would be

* appreciated but is not required.

* 2. Altered source versions must be plainly marked as such, and must not be

* misrepresented as being the original software.

* 3. This notice may not be removed or altered from any source distribution.

*/

#endregion



#region Version Stuff

// **************[ starLiGHT.Engine SVN ]**********************

// * $Rev:: 1486                  $:  Revision of last commit *

// * $Author:: glatzemann         $:  Author of last commit   *

// * $Date:: 2010-10-08 15:55:47 #$:  Date of last commit     *

// ************************************************************



// **************[ Box2D-Repository Info ]*********************

// Header-File: http://code.google.com/p/box2d/source/browse/trunk/Box2D/Box2D/Collision/b2BroadPhase.h

// Revision   : r142

// Change-Date: 2011-02-03

//

// Source-File: http://code.google.com/p/box2d/source/browse/trunk/Box2D/Box2D/Collision/b2BroadPhase.cpp

// Revision   : r142

// Change-Date: 2011-02-03

//

// Status     : Finished!

#endregion



namespace starLiGHT.Physic.TwoD

{

    public struct Pair : IComparable<Pair>, IEquatable<Pair>

    {

        public Int32 proxyIdA;

        public Int32 proxyIdB;

        public Int32 next;



        public int CompareTo(Pair other)

        {

            if (proxyIdA < other.proxyIdA)

            {

                return -1;

            }

            else if (proxyIdA == other.proxyIdA)

            {

                if (proxyIdB < other.proxyIdB)

                {

                    return -1;

                }

                else if (proxyIdB == other.proxyIdB)

                {

                    return 0;

                }

            }



            return 1;

        }



        public static bool operator == (Pair me, Pair other)

        {

            return (me.proxyIdA == other.proxyIdB && me.proxyIdB == other.proxyIdB);

        }



        public static bool operator !=(Pair me, Pair other)

        {

            return (me.proxyIdA != other.proxyIdB || me.proxyIdB != other.proxyIdB);

        }



        public override bool Equals(object obj)

        {

            return this == (Pair)obj;

        }



        public override int GetHashCode()

        {

            return base.GetHashCode();

        }



        #region IEquatable<b2Pair> Member



        public bool Equals(Pair other)

        {

            return (proxyIdA == other.proxyIdA && proxyIdB == other.proxyIdB);

        }



        #endregion

    }



    /// <summary>

    /// The broad-phase is used for computing pairs and performing volume queries and ray casts. 

    /// This broad-phase does not persist pairs. Instead, this reports potentially new pairs.

    /// It is up to the client to consume the new pairs and to track subsequent overlap.

    /// </summary>

    public class BroadPhase : WorldQueryWrapper

    {

        internal static int e_nullProxy = -1;



        #region Member variables

        DynamicTree m_tree = new DynamicTree();

        Int32 m_proxyCount;

        Int32[] m_moveBuffer;

        Int32 m_moveCapacity;

        Int32 m_moveCount;

        Pair[] m_pairBuffer;

        Int32 m_pairCapacity;

        Int32 m_pairCount;

        Int32 m_queryProxyId;



        #endregion



        public BroadPhase()

        {

            m_proxyCount = 0;



            m_pairCapacity = 16;

            m_pairCount = 0;

            m_pairBuffer = new Pair[m_pairCapacity];



            m_moveCapacity = 16;

            m_moveCount = 0;

            m_moveBuffer = new Int32[m_moveCapacity];

        }



        ~BroadPhase()

        {

            m_moveBuffer = null;

            m_pairBuffer = null;

        }



        ///<summary>

        /// Create a proxy with an initial AABB. Pairs are not reported until UpdatePairs is called.

        ///</summary>

        ///<param name="aabb"></param>

        ///<param name="userData"></param>

        public Int32 CreateProxy(AABB aabb, object userData)

        {

            Int32 proxyId = m_tree.CreateProxy(aabb, userData);

            ++m_proxyCount;

            BufferMove(proxyId);

            return proxyId;

        }



        /// <summary>

        /// Destroy a proxy. It is up to the client to remove any pairs.

        /// </summary>

        /// <param name="proxyId"></param>

        public void DestroyProxy(Int32 proxyId)

        {

            UnBufferMove(proxyId);

            --m_proxyCount;

            m_tree.DestroyProxy(proxyId);

        }



        ///<summary>

        ///Call MoveProxy as many times as you like, then when you are done. 

        ///call UpdatePairs to finalize the proxy pairs (for your time step).

        ///</summary>

        ///<param name="aabb"></param>

        ///<param name="proxyId"></param>

        public void MoveProxy(Int32 proxyId, ref AABB aabb, Vector2 displacement)

        {

            bool buffer = m_tree.MoveProxy(proxyId, ref aabb, displacement);

            if (buffer)

            {

                BufferMove(proxyId);

            }

        }



        /// <summary>

        /// Call to trigger a re-processing of it's pairs on the next call to UpdatePairs.

        /// </summary>

        /// <param name="proxyId"></param>

        public void TouchProxy(int proxyId)

        {

            BufferMove(proxyId);

        }



        /// <summary>

        /// Get the fat AABB for a proxy.

        /// </summary>

        /// <param name="proxyId"></param>

        /// <returns></returns>

        public AABB GetFatAABB(Int32 proxyId)

        {

            return m_tree.GetFatAABB(proxyId);

        }



        /// <summary>

        /// Get user data from a proxy. Returns NULL if the id is invalid.

        /// </summary>

        /// <param name="proxyId"></param>

        /// <returns></returns>

        public Object GetUserData(Int32 proxyId)

        {

            return m_tree.GetUserData(proxyId);

        }



        /// <summary>

        /// Test overlap of fat AABBs.

        /// </summary>

        /// <param name="p1"></param>

        /// <param name="p2"></param>

        /// <returns></returns>

        public bool TestOverlap(Int32 proxyIdA, Int32 proxyIdB)

        {

            AABB aabbA = m_tree.GetFatAABB(proxyIdA);

            AABB aabbB = m_tree.GetFatAABB(proxyIdB);

            

            //return TestOverlap(aabbA, aabbB);

            // manual inline

            Vector2 d1 = aabbB.lowerBound - aabbA.upperBound;

            Vector2 d2 = aabbA.lowerBound - aabbB.upperBound;



            if (d1.X > 0.0f || d1.Y > 0.0f) return false;

            if (d2.X > 0.0f || d2.Y > 0.0f) return false;



            return true;

        }



        /// <summary>

        /// Get the number of proxies.

        /// </summary>

        /// <returns></returns>

        public Int32 GetProxyCount()

        {

            return m_proxyCount;

        }



        /// <summary>

        /// Update the pairs. This results in pair callbacks. This can only add pairs.

        /// </summary>

        /// <param name="callback"></param>

        public void UpdatePairs(ContactManager callback)

        {

            // Reset pair buffer 

            m_pairCount = 0;



            Int32 i;



            // Perform tree queries for all moving proxies. 

             for (i = 0; i < m_moveCount; ++i) 

             { 

                m_queryProxyId = m_moveBuffer[i]; 

                if (m_queryProxyId == e_nullProxy) 

                { 

                    continue; 

                } 



                // We have to query the tree with the fat AABB so that 

                // we don't fail to create a pair that may touch later. 

                AABB fatAABB = m_tree.GetFatAABB(m_queryProxyId); 



                // Query tree, create pairs and add them pair buffer. 

                m_tree.Query(this, ref fatAABB); 

             } 

      

             // Reset move buffer 

             m_moveCount = 0; 

      

             // Sort the pair buffer to expose duplicates. 

             Array.Sort(m_pairBuffer, 0, m_pairCount);

      

             // Send the pairs back to the client. 

             i = 0; 

             while (i < m_pairCount) 

             { 

                     Pair primaryPair = m_pairBuffer[i]; 

                     Object userDataA = m_tree.GetUserData(primaryPair.proxyIdA); 

                     Object userDataB = m_tree.GetUserData(primaryPair.proxyIdB);



                     callback.AddPair(userDataA, userDataB); 

                     ++i; 

      

                     // Skip any duplicate pairs. 

                     while (i < m_pairCount) 

                     { 

                             Pair pair = m_pairBuffer[i]; 

                             if (pair.proxyIdA != primaryPair.proxyIdA || pair.proxyIdB != primaryPair.proxyIdB) 

                             { 

                                     break; 

                             } 

                             ++i; 

                     } 

             } 

     

             // Try to keep the tree balanced.

             m_tree.Rebalance(4);

        }



        /// <summary>

        /// Query an AABB for overlapping proxies. The callback class is called for each proxy that overlaps the supplied AABB.

        /// </summary>

        /// <param name="callback"></param>

        /// <param name="aabb"></param>

        public void Query(WorldQueryWrapper callback, ref AABB aabb)

        {

            m_tree.Query(callback, ref aabb);

        }



        ///<summary>

        /// Ray-cast against the proxies in the tree. This relies on the callback         

        /// to perform a exact ray-cast in the case were the proxy contains a shape.         

        /// The callback also performs the any collision filtering. This has performance         

        /// roughly equal to k * log(n), where k is the number of collisions and n is the         

        /// number of proxies in the tree.         

        /// <param name="input">the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).</param>

        /// <param name="callback">a callback class that is called for each proxy that is hit by the ray. </param>

        /// </summary>

        internal void RayCast(RayCastCallbackInternal callback, ref RayCastInput input)

        {

            m_tree.RayCast(callback, ref input);

        }



        /// <summary>

        /// Compute the height of the embedded tree.

        /// </summary>

        /// <returns></returns>

        public Int32 ComputeHeight()

        {

            return m_tree.ComputeHeight();

        }



        void BufferMove(Int32 proxyId)

        {

            if (m_moveCount == m_moveCapacity)

            {

                Int32[] oldBuffer = m_moveBuffer;

                m_moveCapacity *= 2;

                m_moveBuffer = new Int32[m_moveCapacity];

                Array.Copy(oldBuffer, m_moveBuffer, m_moveCount);

            }



            m_moveBuffer[m_moveCount] = proxyId;

            ++m_moveCount;

        }



        void UnBufferMove(Int32 proxyId)

        {

            for (Int32 i = 0; i < m_moveCount; ++i)

            {

                if (m_moveBuffer[i] == proxyId)

                {

                    m_moveBuffer[i] = e_nullProxy;

                    return;

                }

            }

        }



        public override bool QueryCallback(Int32 proxyId)

        {

            // A proxy cannot form a pair with itself.

            if (proxyId == m_queryProxyId)

            {

                return true;

            }

            

            // Grow the pair buffer as needed.

            if (m_pairCount == m_pairCapacity)

            {

                Pair[] oldBuffer = m_pairBuffer;

                m_pairCapacity *= 2;

                m_pairBuffer = new Pair[m_pairCapacity];

                Array.Copy(oldBuffer, m_pairBuffer, m_pairCount);

                oldBuffer = null;

            }



            m_pairBuffer[m_pairCount].proxyIdA = System.Math.Min(proxyId, m_queryProxyId);

            m_pairBuffer[m_pairCount].proxyIdB = System.Math.Max(proxyId, m_queryProxyId);

            ++m_pairCount;

            

            return true; 

        }

    }

}