Root/
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 | using System; using System.Collections.Generic; using FarseerPhysics; using FarseerPhysics.Collision; using FarseerPhysics.Dynamics; using Microsoft.Xna.Framework; public class QuadTreeBroadPhase : IBroadPhase { private const int TreeUpdateThresh = 10000; private int _currID; private Dictionary< int , Element<FixtureProxy>> _idRegister; private List<Element<FixtureProxy>> _moveBuffer; private List<Pair> _pairBuffer; private QuadTree<FixtureProxy> _quadTree; private int _treeMoveNum; /// <summary> /// Creates a new quad tree broadphase with the specified span. /// </summary> /// <param name="span">the maximum span of the tree (world size)</param> public QuadTreeBroadPhase(AABB span) { _quadTree = new QuadTree<FixtureProxy>(span, 5, 10); _idRegister = new Dictionary< int , Element<FixtureProxy>>(); _moveBuffer = new List<Element<FixtureProxy>>(); _pairBuffer = new List<Pair>(); } #region IBroadPhase Members ///<summary> /// The number of proxies ///</summary> public int ProxyCount { get { return _idRegister.Count; } } public void GetFatAABB( int proxyID, out AABB aabb) { if (_idRegister.ContainsKey(proxyID)) aabb = _idRegister[proxyID].Span; else throw new KeyNotFoundException( "proxyID not found in register" ); } public void UpdatePairs(BroadphaseDelegate callback) { _pairBuffer.Clear(); foreach (Element<FixtureProxy> qtnode in _moveBuffer) { // Query tree, create pairs and add them pair buffer. Query(proxyID => PairBufferQueryCallback(proxyID, qtnode.Value.ProxyId), ref qtnode.Span); } _moveBuffer.Clear(); // Sort the pair buffer to expose duplicates. _pairBuffer.Sort(); // Send the pairs back to the client. int i = 0; while (i < _pairBuffer.Count) { Pair primaryPair = _pairBuffer[i]; FixtureProxy userDataA = GetProxy(primaryPair.ProxyIdA); FixtureProxy userDataB = GetProxy(primaryPair.ProxyIdB); callback( ref userDataA, ref userDataB); ++i; // Skip any duplicate pairs. while (i < _pairBuffer.Count && _pairBuffer[i].ProxyIdA == primaryPair.ProxyIdA && _pairBuffer[i].ProxyIdB == primaryPair.ProxyIdB) ++i; } } /// <summary> /// Test overlap of fat AABBs. /// </summary> /// <param name="proxyIdA">The proxy id A.</param> /// <param name="proxyIdB">The proxy id B.</param> /// <returns></returns> public bool TestOverlap( int proxyIdA, int proxyIdB) { AABB aabb1; AABB aabb2; GetFatAABB(proxyIdA, out aabb1); GetFatAABB(proxyIdB, out aabb2); return AABB.TestOverlap( ref aabb1, ref aabb2); } public int AddProxy( ref FixtureProxy proxy) { int proxyID = _currID++; proxy.ProxyId = proxyID; AABB aabb = Fatten( ref proxy.AABB); Element<FixtureProxy> qtnode = new Element<FixtureProxy>(proxy, aabb); _idRegister.Add(proxyID, qtnode); _quadTree.AddNode(qtnode); return proxyID; } public void RemoveProxy( int proxyId) { if (_idRegister.ContainsKey(proxyId)) { Element<FixtureProxy> qtnode = _idRegister[proxyId]; UnbufferMove(qtnode); _idRegister.Remove(proxyId); _quadTree.RemoveNode(qtnode); } else throw new KeyNotFoundException( "proxyID not found in register" ); } public void MoveProxy( int proxyId, ref AABB aabb, Vector2 displacement) { AABB fatAABB; GetFatAABB(proxyId, out fatAABB); //exit if movement is within fat aabb if (fatAABB.Contains( ref aabb)) return ; // Extend AABB. AABB b = aabb; Vector2 r = new Vector2(Settings.AABBExtension, Settings.AABBExtension); b.LowerBound = b.LowerBound - r; b.UpperBound = b.UpperBound + r; // Predict AABB displacement. Vector2 d = Settings.AABBMultiplier * displacement; if (d.X < 0.0f) b.LowerBound.X += d.X; else b.UpperBound.X += d.X; if (d.Y < 0.0f) b.LowerBound.Y += d.Y; else b.UpperBound.Y += d.Y; Element<FixtureProxy> qtnode = _idRegister[proxyId]; qtnode.Value.AABB = b; //not neccesary for QTree, but might be accessed externally qtnode.Span = b; ReinsertNode(qtnode); BufferMove(qtnode); } public FixtureProxy GetProxy( int proxyId) { if (_idRegister.ContainsKey(proxyId)) return _idRegister[proxyId].Value; else throw new KeyNotFoundException( "proxyID not found in register" ); } public void TouchProxy( int proxyId) { if (_idRegister.ContainsKey(proxyId)) BufferMove(_idRegister[proxyId]); else throw new KeyNotFoundException( "proxyID not found in register" ); } public void Query(Func< int , bool > callback, ref AABB query) { _quadTree.QueryAABB(TransformPredicate(callback), ref query); } public void RayCast(Func<RayCastInput, int , float > callback, ref RayCastInput input) { _quadTree.RayCast(TransformRayCallback(callback), ref input); } #endregion private AABB Fatten( ref AABB aabb) { Vector2 r = new Vector2(Settings.AABBExtension, Settings.AABBExtension); return new AABB(aabb.LowerBound - r, aabb.UpperBound + r); } private Func<Element<FixtureProxy>, bool > TransformPredicate(Func< int , bool > idPredicate) { Func<Element<FixtureProxy>, bool > qtPred = qtnode => idPredicate(qtnode.Value.ProxyId); return qtPred; } private Func<RayCastInput, Element<FixtureProxy>, float > TransformRayCallback( Func<RayCastInput, int , float > callback) { Func<RayCastInput, Element<FixtureProxy>, float > newCallback = (input, qtnode) => callback(input, qtnode.Value.ProxyId); return newCallback; } private bool PairBufferQueryCallback( int proxyID, int baseID) { // A proxy cannot form a pair with itself. if (proxyID == baseID) return true ; Pair p = new Pair(); p.ProxyIdA = Math.Min(proxyID, baseID); p.ProxyIdB = Math.Max(proxyID, baseID); _pairBuffer.Add(p); return true ; } private void ReconstructTree() { //this is faster than _quadTree.Reconstruct(), since the quadtree method runs a recusive query to find all nodes. _quadTree.Clear(); foreach (Element<FixtureProxy> elem in _idRegister.Values) _quadTree.AddNode(elem); } private void ReinsertNode(Element<FixtureProxy> qtnode) { _quadTree.RemoveNode(qtnode); _quadTree.AddNode(qtnode); if (++_treeMoveNum > TreeUpdateThresh) { ReconstructTree(); _treeMoveNum = 0; } } private void BufferMove(Element<FixtureProxy> proxy) { _moveBuffer.Add(proxy); } private void UnbufferMove(Element<FixtureProxy> proxy) { _moveBuffer.Remove(proxy); } } |
Source at commit 98094c69513a created 12 years 7 months ago. By Nathan Adams, + * - Adding an extension to determine what side the objects collided on |
---|