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