00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 #ifndef B2_DYNAMIC_TREE_H
00020 #define B2_DYNAMIC_TREE_H
00021
00022 #include <Box2D/Collision/b2Collision.h>
00023
00025
00026 #define b2_nullNode (-1)
00027
00029 struct b2DynamicTreeNode
00030 {
00031 bool IsLeaf() const
00032 {
00033 return child1 == b2_nullNode;
00034 }
00035
00037 b2AABB aabb;
00038
00039
00040 void* userData;
00041
00042 union
00043 {
00044 int32 parent;
00045 int32 next;
00046 };
00047
00048 int32 child1;
00049 int32 child2;
00050 };
00051
00059 class b2DynamicTree
00060 {
00061 public:
00062
00064 b2DynamicTree();
00065
00067 ~b2DynamicTree();
00068
00070 int32 CreateProxy(const b2AABB& aabb, void* userData);
00071
00073 void DestroyProxy(int32 proxyId);
00074
00079 bool MoveProxy(int32 proxyId, const b2AABB& aabb1, const b2Vec2& displacement);
00080
00082 void Rebalance(int32 iterations);
00083
00086 void* GetUserData(int32 proxyId) const;
00087
00089 const b2AABB& GetFatAABB(int32 proxyId) const;
00090
00092 int32 ComputeHeight() const;
00093
00096 template <typename T>
00097 void Query(T* callback, const b2AABB& aabb) const;
00098
00106 template <typename T>
00107 void RayCast(T* callback, const b2RayCastInput& input) const;
00108
00109 private:
00110
00111 int32 AllocateNode();
00112 void FreeNode(int32 node);
00113
00114 void InsertLeaf(int32 node);
00115 void RemoveLeaf(int32 node);
00116
00117 int32 ComputeHeight(int32 nodeId) const;
00118
00119 int32 m_root;
00120
00121 b2DynamicTreeNode* m_nodes;
00122 int32 m_nodeCount;
00123 int32 m_nodeCapacity;
00124
00125 int32 m_freeList;
00126
00128 uint32 m_path;
00129
00130 int32 m_insertionCount;
00131 };
00132
00133 inline void* b2DynamicTree::GetUserData(int32 proxyId) const
00134 {
00135 b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
00136 return m_nodes[proxyId].userData;
00137 }
00138
00139 inline const b2AABB& b2DynamicTree::GetFatAABB(int32 proxyId) const
00140 {
00141 b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
00142 return m_nodes[proxyId].aabb;
00143 }
00144
00145 template <typename T>
00146 inline void b2DynamicTree::Query(T* callback, const b2AABB& aabb) const
00147 {
00148 const int32 k_stackSize = 128;
00149 int32 stack[k_stackSize];
00150
00151 int32 count = 0;
00152 stack[count++] = m_root;
00153
00154 while (count > 0)
00155 {
00156 int32 nodeId = stack[--count];
00157 if (nodeId == b2_nullNode)
00158 {
00159 continue;
00160 }
00161
00162 const b2DynamicTreeNode* node = m_nodes + nodeId;
00163
00164 if (b2TestOverlap(node->aabb, aabb))
00165 {
00166 if (node->IsLeaf())
00167 {
00168 bool proceed = callback->QueryCallback(nodeId);
00169 if (proceed == false)
00170 {
00171 return;
00172 }
00173 }
00174 else
00175 {
00176 if (count < k_stackSize)
00177 {
00178 stack[count++] = node->child1;
00179 }
00180
00181 if (count < k_stackSize)
00182 {
00183 stack[count++] = node->child2;
00184 }
00185 }
00186 }
00187 }
00188 }
00189
00190 template <typename T>
00191 inline void b2DynamicTree::RayCast(T* callback, const b2RayCastInput& input) const
00192 {
00193 b2Vec2 p1 = input.p1;
00194 b2Vec2 p2 = input.p2;
00195 b2Vec2 r = p2 - p1;
00196 b2Assert(r.LengthSquared() > 0.0f);
00197 r.Normalize();
00198
00199
00200 b2Vec2 v = b2Cross(1.0f, r);
00201 b2Vec2 abs_v = b2Abs(v);
00202
00203
00204
00205
00206 float32 maxFraction = input.maxFraction;
00207
00208
00209 b2AABB segmentAABB;
00210 {
00211 b2Vec2 t = p1 + maxFraction * (p2 - p1);
00212 segmentAABB.lowerBound = b2Min(p1, t);
00213 segmentAABB.upperBound = b2Max(p1, t);
00214 }
00215
00216 const int32 k_stackSize = 128;
00217 int32 stack[k_stackSize];
00218
00219 int32 count = 0;
00220 stack[count++] = m_root;
00221
00222 while (count > 0)
00223 {
00224 int32 nodeId = stack[--count];
00225 if (nodeId == b2_nullNode)
00226 {
00227 continue;
00228 }
00229
00230 const b2DynamicTreeNode* node = m_nodes + nodeId;
00231
00232 if (b2TestOverlap(node->aabb, segmentAABB) == false)
00233 {
00234 continue;
00235 }
00236
00237
00238
00239 b2Vec2 c = node->aabb.GetCenter();
00240 b2Vec2 h = node->aabb.GetExtents();
00241 float32 separation = b2Abs(b2Dot(v, p1 - c)) - b2Dot(abs_v, h);
00242 if (separation > 0.0f)
00243 {
00244 continue;
00245 }
00246
00247 if (node->IsLeaf())
00248 {
00249 b2RayCastInput subInput;
00250 subInput.p1 = input.p1;
00251 subInput.p2 = input.p2;
00252 subInput.maxFraction = maxFraction;
00253
00254 float32 value = callback->RayCastCallback(subInput, nodeId);
00255
00256 if (value == 0.0f)
00257 {
00258
00259 return;
00260 }
00261
00262 if (value > 0.0f)
00263 {
00264
00265 maxFraction = value;
00266 b2Vec2 t = p1 + maxFraction * (p2 - p1);
00267 segmentAABB.lowerBound = b2Min(p1, t);
00268 segmentAABB.upperBound = b2Max(p1, t);
00269 }
00270 }
00271 else
00272 {
00273 if (count < k_stackSize)
00274 {
00275 stack[count++] = node->child1;
00276 }
00277
00278 if (count < k_stackSize)
00279 {
00280 stack[count++] = node->child2;
00281 }
00282 }
00283 }
00284 }
00285
00286 #endif