1 /*
2 * Copyright (c) 2009 Erin Catto http://www.box2d.org
3 *
4 * This software is provided 'as-is', without any express or implied
5 * warranty.  In no event will the authors be held liable for any damages
6 * arising from the use of this software.
7 * Permission is granted to anyone to use this software for any purpose,
8 * including commercial applications, and to alter it and redistribute it
9 * freely, subject to the following restrictions:
10 * 1. The origin of this software must not be misrepresented; you must not
11 * claim that you wrote the original software. If you use this software
12 * in a product, an acknowledgment in the product documentation would be
13 * appreciated but is not required.
14 * 2. Altered source versions must be plainly marked as such, and must not be
15 * misrepresented as being the original software.
16 * 3. This notice may not be removed or altered from any source distribution.
17 */
18 
19 #ifndef DYNAMIC_TREE_TEST_H
20 #define DYNAMIC_TREE_TEST_H
21 
22 class DynamicTreeTest : public Test
23 {
24 public:
25 
26 	enum
27 	{
28 		e_actorCount = 128
29 	};
30 
DynamicTreeTest()31 	DynamicTreeTest()
32 	{
33 		m_worldExtent = 15.0f;
34 		m_proxyExtent = 0.5f;
35 
36 		srand(888);
37 
38 		for (int32 i = 0; i < e_actorCount; ++i)
39 		{
40 			Actor* actor = m_actors + i;
41 			GetRandomAABB(&actor->aabb);
42 			actor->proxyId = m_tree.CreateProxy(actor->aabb, actor);
43 		}
44 
45 		m_stepCount = 0;
46 
47 		float32 h = m_worldExtent;
48 		m_queryAABB.lowerBound.Set(-3.0f, -4.0f + h);
49 		m_queryAABB.upperBound.Set(5.0f, 6.0f + h);
50 
51 		m_rayCastInput.p1.Set(-5.0, 5.0f + h);
52 		m_rayCastInput.p2.Set(7.0f, -4.0f + h);
53 		//m_rayCastInput.p1.Set(0.0f, 2.0f + h);
54 		//m_rayCastInput.p2.Set(0.0f, -2.0f + h);
55 		m_rayCastInput.maxFraction = 1.0f;
56 
57 		m_automated = false;
58 	}
59 
Create()60 	static Test* Create()
61 	{
62 		return new DynamicTreeTest;
63 	}
64 
Step(Settings * settings)65 	void Step(Settings* settings)
66 	{
67 		B2_NOT_USED(settings);
68 
69 		m_rayActor = NULL;
70 		for (int32 i = 0; i < e_actorCount; ++i)
71 		{
72 			m_actors[i].fraction = 1.0f;
73 			m_actors[i].overlap = false;
74 		}
75 
76 		if (m_automated == true)
77 		{
78 			int32 actionCount = b2Max(1, e_actorCount >> 2);
79 
80 			for (int32 i = 0; i < actionCount; ++i)
81 			{
82 				Action();
83 			}
84 		}
85 
86 		Query();
87 		RayCast();
88 
89 		for (int32 i = 0; i < e_actorCount; ++i)
90 		{
91 			Actor* actor = m_actors + i;
92 			if (actor->proxyId == b2_nullNode)
93 				continue;
94 
95 			b2Color c(0.9f, 0.9f, 0.9f);
96 			if (actor == m_rayActor && actor->overlap)
97 			{
98 				c.Set(0.9f, 0.6f, 0.6f);
99 			}
100 			else if (actor == m_rayActor)
101 			{
102 				c.Set(0.6f, 0.9f, 0.6f);
103 			}
104 			else if (actor->overlap)
105 			{
106 				c.Set(0.6f, 0.6f, 0.9f);
107 			}
108 
109 			g_debugDraw.DrawAABB(&actor->aabb, c);
110 		}
111 
112 		b2Color c(0.7f, 0.7f, 0.7f);
113 		g_debugDraw.DrawAABB(&m_queryAABB, c);
114 
115 		g_debugDraw.DrawSegment(m_rayCastInput.p1, m_rayCastInput.p2, c);
116 
117 		b2Color c1(0.2f, 0.9f, 0.2f);
118 		b2Color c2(0.9f, 0.2f, 0.2f);
119 		g_debugDraw.DrawPoint(m_rayCastInput.p1, 6.0f, c1);
120 		g_debugDraw.DrawPoint(m_rayCastInput.p2, 6.0f, c2);
121 
122 		if (m_rayActor)
123 		{
124 			b2Color cr(0.2f, 0.2f, 0.9f);
125 			b2Vec2 p = m_rayCastInput.p1 + m_rayActor->fraction * (m_rayCastInput.p2 - m_rayCastInput.p1);
126 			g_debugDraw.DrawPoint(p, 6.0f, cr);
127 		}
128 
129 		{
130 			int32 height = m_tree.GetHeight();
131 			g_debugDraw.DrawString(5, m_textLine, "dynamic tree height = %d", height);
132 			m_textLine += DRAW_STRING_NEW_LINE;
133 		}
134 
135 		++m_stepCount;
136 	}
137 
Keyboard(int key)138 	void Keyboard(int key)
139 	{
140 		switch (key)
141 		{
142 		case GLFW_KEY_A:
143 			m_automated = !m_automated;
144 			break;
145 
146 		case GLFW_KEY_C:
147 			CreateProxy();
148 			break;
149 
150 		case GLFW_KEY_D:
151 			DestroyProxy();
152 			break;
153 
154 		case GLFW_KEY_M:
155 			MoveProxy();
156 			break;
157 		}
158 	}
159 
QueryCallback(int32 proxyId)160 	bool QueryCallback(int32 proxyId)
161 	{
162 		Actor* actor = (Actor*)m_tree.GetUserData(proxyId);
163 		actor->overlap = b2TestOverlap(m_queryAABB, actor->aabb);
164 		return true;
165 	}
166 
RayCastCallback(const b2RayCastInput & input,int32 proxyId)167 	float32 RayCastCallback(const b2RayCastInput& input, int32 proxyId)
168 	{
169 		Actor* actor = (Actor*)m_tree.GetUserData(proxyId);
170 
171 		b2RayCastOutput output;
172 		bool hit = actor->aabb.RayCast(&output, input);
173 
174 		if (hit)
175 		{
176 			m_rayCastOutput = output;
177 			m_rayActor = actor;
178 			m_rayActor->fraction = output.fraction;
179 			return output.fraction;
180 		}
181 
182 		return input.maxFraction;
183 	}
184 
185 private:
186 
187 	struct Actor
188 	{
189 		b2AABB aabb;
190 		float32 fraction;
191 		bool overlap;
192 		int32 proxyId;
193 	};
194 
GetRandomAABB(b2AABB * aabb)195 	void GetRandomAABB(b2AABB* aabb)
196 	{
197 		b2Vec2 w; w.Set(2.0f * m_proxyExtent, 2.0f * m_proxyExtent);
198 		//aabb->lowerBound.x = -m_proxyExtent;
199 		//aabb->lowerBound.y = -m_proxyExtent + m_worldExtent;
200 		aabb->lowerBound.x = RandomFloat(-m_worldExtent, m_worldExtent);
201 		aabb->lowerBound.y = RandomFloat(0.0f, 2.0f * m_worldExtent);
202 		aabb->upperBound = aabb->lowerBound + w;
203 	}
204 
MoveAABB(b2AABB * aabb)205 	void MoveAABB(b2AABB* aabb)
206 	{
207 		b2Vec2 d;
208 		d.x = RandomFloat(-0.5f, 0.5f);
209 		d.y = RandomFloat(-0.5f, 0.5f);
210 		//d.x = 2.0f;
211 		//d.y = 0.0f;
212 		aabb->lowerBound += d;
213 		aabb->upperBound += d;
214 
215 		b2Vec2 c0 = 0.5f * (aabb->lowerBound + aabb->upperBound);
216 		b2Vec2 min; min.Set(-m_worldExtent, 0.0f);
217 		b2Vec2 max; max.Set(m_worldExtent, 2.0f * m_worldExtent);
218 		b2Vec2 c = b2Clamp(c0, min, max);
219 
220 		aabb->lowerBound += c - c0;
221 		aabb->upperBound += c - c0;
222 	}
223 
CreateProxy()224 	void CreateProxy()
225 	{
226 		for (int32 i = 0; i < e_actorCount; ++i)
227 		{
228 			int32 j = rand() % e_actorCount;
229 			Actor* actor = m_actors + j;
230 			if (actor->proxyId == b2_nullNode)
231 			{
232 				GetRandomAABB(&actor->aabb);
233 				actor->proxyId = m_tree.CreateProxy(actor->aabb, actor);
234 				return;
235 			}
236 		}
237 	}
238 
DestroyProxy()239 	void DestroyProxy()
240 	{
241 		for (int32 i = 0; i < e_actorCount; ++i)
242 		{
243 			int32 j = rand() % e_actorCount;
244 			Actor* actor = m_actors + j;
245 			if (actor->proxyId != b2_nullNode)
246 			{
247 				m_tree.DestroyProxy(actor->proxyId);
248 				actor->proxyId = b2_nullNode;
249 				return;
250 			}
251 		}
252 	}
253 
MoveProxy()254 	void MoveProxy()
255 	{
256 		for (int32 i = 0; i < e_actorCount; ++i)
257 		{
258 			int32 j = rand() % e_actorCount;
259 			Actor* actor = m_actors + j;
260 			if (actor->proxyId == b2_nullNode)
261 			{
262 				continue;
263 			}
264 
265 			b2AABB aabb0 = actor->aabb;
266 			MoveAABB(&actor->aabb);
267 			b2Vec2 displacement = actor->aabb.GetCenter() - aabb0.GetCenter();
268 			m_tree.MoveProxy(actor->proxyId, actor->aabb, displacement);
269 			return;
270 		}
271 	}
272 
Action()273 	void Action()
274 	{
275 		int32 choice = rand() % 20;
276 
277 		switch (choice)
278 		{
279 		case 0:
280 			CreateProxy();
281 			break;
282 
283 		case 1:
284 			DestroyProxy();
285 			break;
286 
287 		default:
288 			MoveProxy();
289 		}
290 	}
291 
Query()292 	void Query()
293 	{
294 		m_tree.Query(this, m_queryAABB);
295 
296 		for (int32 i = 0; i < e_actorCount; ++i)
297 		{
298 			if (m_actors[i].proxyId == b2_nullNode)
299 			{
300 				continue;
301 			}
302 
303 			bool overlap = b2TestOverlap(m_queryAABB, m_actors[i].aabb);
304 			B2_NOT_USED(overlap);
305 			b2Assert(overlap == m_actors[i].overlap);
306 		}
307 	}
308 
RayCast()309 	void RayCast()
310 	{
311 		m_rayActor = NULL;
312 
313 		b2RayCastInput input = m_rayCastInput;
314 
315 		// Ray cast against the dynamic tree.
316 		m_tree.RayCast(this, input);
317 
318 		// Brute force ray cast.
319 		Actor* bruteActor = NULL;
320 		b2RayCastOutput bruteOutput;
321 		for (int32 i = 0; i < e_actorCount; ++i)
322 		{
323 			if (m_actors[i].proxyId == b2_nullNode)
324 			{
325 				continue;
326 			}
327 
328 			b2RayCastOutput output;
329 			bool hit = m_actors[i].aabb.RayCast(&output, input);
330 			if (hit)
331 			{
332 				bruteActor = m_actors + i;
333 				bruteOutput = output;
334 				input.maxFraction = output.fraction;
335 			}
336 		}
337 
338 		if (bruteActor != NULL)
339 		{
340 			b2Assert(bruteOutput.fraction == m_rayCastOutput.fraction);
341 		}
342 	}
343 
344 	float32 m_worldExtent;
345 	float32 m_proxyExtent;
346 
347 	b2DynamicTree m_tree;
348 	b2AABB m_queryAABB;
349 	b2RayCastInput m_rayCastInput;
350 	b2RayCastOutput m_rayCastOutput;
351 	Actor* m_rayActor;
352 	Actor m_actors[e_actorCount];
353 	int32 m_stepCount;
354 	bool m_automated;
355 };
356 
357 #endif
358