/starLiGHT.Engine/starLiGHT.Physic2D/starLiGHT.Physic2D/Collision/BroadPhase.cs
C# | 423 lines | 230 code | 58 blank | 135 comment | 42 complexity | 96a5f90ee45ea70b3784cc987c560b24 MD5 | raw file
Possible License(s): LGPL-3.0, GPL-3.0
- #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;
- }
- }
- }