1 // MIT License 2 3 // Copyright (c) 2019 Erin Catto 4 5 // Permission is hereby granted, free of charge, to any person obtaining a copy 6 // of this software and associated documentation files (the "Software"), to deal 7 // in the Software without restriction, including without limitation the rights 8 // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 // copies of the Software, and to permit persons to whom the Software is 10 // furnished to do so, subject to the following conditions: 11 12 // The above copyright notice and this permission notice shall be included in all 13 // copies or substantial portions of the Software. 14 15 // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 // SOFTWARE. 22 23 #include "test.h" 24 25 class DynamicTree : public Test 26 { 27 public: 28 29 enum 30 { 31 e_actorCount = 128 32 }; 33 DynamicTree()34 DynamicTree() 35 { 36 m_worldExtent = 15.0f; 37 m_proxyExtent = 0.5f; 38 39 srand(888); 40 41 for (int32 i = 0; i < e_actorCount; ++i) 42 { 43 Actor* actor = m_actors + i; 44 GetRandomAABB(&actor->aabb); 45 actor->proxyId = m_tree.CreateProxy(actor->aabb, actor); 46 } 47 48 m_stepCount = 0; 49 50 float h = m_worldExtent; 51 m_queryAABB.lowerBound.Set(-3.0f, -4.0f + h); 52 m_queryAABB.upperBound.Set(5.0f, 6.0f + h); 53 54 m_rayCastInput.p1.Set(-5.0, 5.0f + h); 55 m_rayCastInput.p2.Set(7.0f, -4.0f + h); 56 //m_rayCastInput.p1.Set(0.0f, 2.0f + h); 57 //m_rayCastInput.p2.Set(0.0f, -2.0f + h); 58 m_rayCastInput.maxFraction = 1.0f; 59 60 m_automated = false; 61 } 62 Create()63 static Test* Create() 64 { 65 return new DynamicTree; 66 } 67 Step(Settings & settings)68 void Step(Settings& settings) override 69 { 70 B2_NOT_USED(settings); 71 72 m_rayActor = NULL; 73 for (int32 i = 0; i < e_actorCount; ++i) 74 { 75 m_actors[i].fraction = 1.0f; 76 m_actors[i].overlap = false; 77 } 78 79 if (m_automated == true) 80 { 81 int32 actionCount = b2Max(1, e_actorCount >> 2); 82 83 for (int32 i = 0; i < actionCount; ++i) 84 { 85 Action(); 86 } 87 } 88 89 Query(); 90 RayCast(); 91 92 for (int32 i = 0; i < e_actorCount; ++i) 93 { 94 Actor* actor = m_actors + i; 95 if (actor->proxyId == b2_nullNode) 96 continue; 97 98 b2Color c(0.9f, 0.9f, 0.9f); 99 if (actor == m_rayActor && actor->overlap) 100 { 101 c.Set(0.9f, 0.6f, 0.6f); 102 } 103 else if (actor == m_rayActor) 104 { 105 c.Set(0.6f, 0.9f, 0.6f); 106 } 107 else if (actor->overlap) 108 { 109 c.Set(0.6f, 0.6f, 0.9f); 110 } 111 112 g_debugDraw.DrawAABB(&actor->aabb, c); 113 } 114 115 b2Color c(0.7f, 0.7f, 0.7f); 116 g_debugDraw.DrawAABB(&m_queryAABB, c); 117 118 g_debugDraw.DrawSegment(m_rayCastInput.p1, m_rayCastInput.p2, c); 119 120 b2Color c1(0.2f, 0.9f, 0.2f); 121 b2Color c2(0.9f, 0.2f, 0.2f); 122 g_debugDraw.DrawPoint(m_rayCastInput.p1, 6.0f, c1); 123 g_debugDraw.DrawPoint(m_rayCastInput.p2, 6.0f, c2); 124 125 if (m_rayActor) 126 { 127 b2Color cr(0.2f, 0.2f, 0.9f); 128 b2Vec2 p = m_rayCastInput.p1 + m_rayActor->fraction * (m_rayCastInput.p2 - m_rayCastInput.p1); 129 g_debugDraw.DrawPoint(p, 6.0f, cr); 130 } 131 132 { 133 int32 height = m_tree.GetHeight(); 134 g_debugDraw.DrawString(5, m_textLine, "dynamic tree height = %d", height); 135 m_textLine += m_textIncrement; 136 } 137 138 ++m_stepCount; 139 } 140 Keyboard(int key)141 void Keyboard(int key) override 142 { 143 switch (key) 144 { 145 case GLFW_KEY_A: 146 m_automated = !m_automated; 147 break; 148 149 case GLFW_KEY_C: 150 CreateProxy(); 151 break; 152 153 case GLFW_KEY_D: 154 DestroyProxy(); 155 break; 156 157 case GLFW_KEY_M: 158 MoveProxy(); 159 break; 160 } 161 } 162 QueryCallback(int32 proxyId)163 bool QueryCallback(int32 proxyId) 164 { 165 Actor* actor = (Actor*)m_tree.GetUserData(proxyId); 166 actor->overlap = b2TestOverlap(m_queryAABB, actor->aabb); 167 return true; 168 } 169 RayCastCallback(const b2RayCastInput & input,int32 proxyId)170 float RayCastCallback(const b2RayCastInput& input, int32 proxyId) 171 { 172 Actor* actor = (Actor*)m_tree.GetUserData(proxyId); 173 174 b2RayCastOutput output; 175 bool hit = actor->aabb.RayCast(&output, input); 176 177 if (hit) 178 { 179 m_rayCastOutput = output; 180 m_rayActor = actor; 181 m_rayActor->fraction = output.fraction; 182 return output.fraction; 183 } 184 185 return input.maxFraction; 186 } 187 188 private: 189 190 struct Actor 191 { 192 b2AABB aabb; 193 float fraction; 194 bool overlap; 195 int32 proxyId; 196 }; 197 GetRandomAABB(b2AABB * aabb)198 void GetRandomAABB(b2AABB* aabb) 199 { 200 b2Vec2 w; w.Set(2.0f * m_proxyExtent, 2.0f * m_proxyExtent); 201 //aabb->lowerBound.x = -m_proxyExtent; 202 //aabb->lowerBound.y = -m_proxyExtent + m_worldExtent; 203 aabb->lowerBound.x = RandomFloat(-m_worldExtent, m_worldExtent); 204 aabb->lowerBound.y = RandomFloat(0.0f, 2.0f * m_worldExtent); 205 aabb->upperBound = aabb->lowerBound + w; 206 } 207 MoveAABB(b2AABB * aabb)208 void MoveAABB(b2AABB* aabb) 209 { 210 b2Vec2 d; 211 d.x = RandomFloat(-0.5f, 0.5f); 212 d.y = RandomFloat(-0.5f, 0.5f); 213 //d.x = 2.0f; 214 //d.y = 0.0f; 215 aabb->lowerBound += d; 216 aabb->upperBound += d; 217 218 b2Vec2 c0 = 0.5f * (aabb->lowerBound + aabb->upperBound); 219 b2Vec2 min; min.Set(-m_worldExtent, 0.0f); 220 b2Vec2 max; max.Set(m_worldExtent, 2.0f * m_worldExtent); 221 b2Vec2 c = b2Clamp(c0, min, max); 222 223 aabb->lowerBound += c - c0; 224 aabb->upperBound += c - c0; 225 } 226 CreateProxy()227 void CreateProxy() 228 { 229 for (int32 i = 0; i < e_actorCount; ++i) 230 { 231 int32 j = rand() % e_actorCount; 232 Actor* actor = m_actors + j; 233 if (actor->proxyId == b2_nullNode) 234 { 235 GetRandomAABB(&actor->aabb); 236 actor->proxyId = m_tree.CreateProxy(actor->aabb, actor); 237 return; 238 } 239 } 240 } 241 DestroyProxy()242 void DestroyProxy() 243 { 244 for (int32 i = 0; i < e_actorCount; ++i) 245 { 246 int32 j = rand() % e_actorCount; 247 Actor* actor = m_actors + j; 248 if (actor->proxyId != b2_nullNode) 249 { 250 m_tree.DestroyProxy(actor->proxyId); 251 actor->proxyId = b2_nullNode; 252 return; 253 } 254 } 255 } 256 MoveProxy()257 void MoveProxy() 258 { 259 for (int32 i = 0; i < e_actorCount; ++i) 260 { 261 int32 j = rand() % e_actorCount; 262 Actor* actor = m_actors + j; 263 if (actor->proxyId == b2_nullNode) 264 { 265 continue; 266 } 267 268 b2AABB aabb0 = actor->aabb; 269 MoveAABB(&actor->aabb); 270 b2Vec2 displacement = actor->aabb.GetCenter() - aabb0.GetCenter(); 271 m_tree.MoveProxy(actor->proxyId, actor->aabb, displacement); 272 return; 273 } 274 } 275 Action()276 void Action() 277 { 278 int32 choice = rand() % 20; 279 280 switch (choice) 281 { 282 case 0: 283 CreateProxy(); 284 break; 285 286 case 1: 287 DestroyProxy(); 288 break; 289 290 default: 291 MoveProxy(); 292 } 293 } 294 Query()295 void Query() 296 { 297 m_tree.Query(this, m_queryAABB); 298 299 for (int32 i = 0; i < e_actorCount; ++i) 300 { 301 if (m_actors[i].proxyId == b2_nullNode) 302 { 303 continue; 304 } 305 306 bool overlap = b2TestOverlap(m_queryAABB, m_actors[i].aabb); 307 B2_NOT_USED(overlap); 308 b2Assert(overlap == m_actors[i].overlap); 309 } 310 } 311 RayCast()312 void RayCast() 313 { 314 m_rayActor = NULL; 315 316 b2RayCastInput input = m_rayCastInput; 317 318 // Ray cast against the dynamic tree. 319 m_tree.RayCast(this, input); 320 321 // Brute force ray cast. 322 Actor* bruteActor = NULL; 323 b2RayCastOutput bruteOutput; 324 for (int32 i = 0; i < e_actorCount; ++i) 325 { 326 if (m_actors[i].proxyId == b2_nullNode) 327 { 328 continue; 329 } 330 331 b2RayCastOutput output; 332 bool hit = m_actors[i].aabb.RayCast(&output, input); 333 if (hit) 334 { 335 bruteActor = m_actors + i; 336 bruteOutput = output; 337 input.maxFraction = output.fraction; 338 } 339 } 340 341 if (bruteActor != NULL) 342 { 343 b2Assert(bruteOutput.fraction == m_rayCastOutput.fraction); 344 } 345 } 346 347 float m_worldExtent; 348 float m_proxyExtent; 349 350 b2DynamicTree m_tree; 351 b2AABB m_queryAABB; 352 b2RayCastInput m_rayCastInput; 353 b2RayCastOutput m_rayCastOutput; 354 Actor* m_rayActor; 355 Actor m_actors[e_actorCount]; 356 int32 m_stepCount; 357 bool m_automated; 358 }; 359 360 static int testIndex = RegisterTest("Collision", "Dynamic Tree", DynamicTree::Create); 361