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 #include "settings.h"
25 #include <stdio.h>
26
SayGoodbye(b2Joint * joint)27 void DestructionListener::SayGoodbye(b2Joint* joint)
28 {
29 if (test->m_mouseJoint == joint)
30 {
31 test->m_mouseJoint = NULL;
32 }
33 else
34 {
35 test->JointDestroyed(joint);
36 }
37 }
38
Test()39 Test::Test()
40 {
41 b2Vec2 gravity;
42 gravity.Set(0.0f, -10.0f);
43 m_world = new b2World(gravity);
44 m_bomb = NULL;
45 m_textLine = 30;
46 m_textIncrement = 13;
47 m_mouseJoint = NULL;
48 m_pointCount = 0;
49
50 m_destructionListener.test = this;
51 m_world->SetDestructionListener(&m_destructionListener);
52 m_world->SetContactListener(this);
53 m_world->SetDebugDraw(&g_debugDraw);
54
55 m_bombSpawning = false;
56
57 m_stepCount = 0;
58
59 b2BodyDef bodyDef;
60 m_groundBody = m_world->CreateBody(&bodyDef);
61
62 memset(&m_maxProfile, 0, sizeof(b2Profile));
63 memset(&m_totalProfile, 0, sizeof(b2Profile));
64 }
65
~Test()66 Test::~Test()
67 {
68 // By deleting the world, we delete the bomb, mouse joint, etc.
69 delete m_world;
70 m_world = NULL;
71 }
72
PreSolve(b2Contact * contact,const b2Manifold * oldManifold)73 void Test::PreSolve(b2Contact* contact, const b2Manifold* oldManifold)
74 {
75 const b2Manifold* manifold = contact->GetManifold();
76
77 if (manifold->pointCount == 0)
78 {
79 return;
80 }
81
82 b2Fixture* fixtureA = contact->GetFixtureA();
83 b2Fixture* fixtureB = contact->GetFixtureB();
84
85 b2PointState state1[b2_maxManifoldPoints], state2[b2_maxManifoldPoints];
86 b2GetPointStates(state1, state2, oldManifold, manifold);
87
88 b2WorldManifold worldManifold;
89 contact->GetWorldManifold(&worldManifold);
90
91 for (int32 i = 0; i < manifold->pointCount && m_pointCount < k_maxContactPoints; ++i)
92 {
93 ContactPoint* cp = m_points + m_pointCount;
94 cp->fixtureA = fixtureA;
95 cp->fixtureB = fixtureB;
96 cp->position = worldManifold.points[i];
97 cp->normal = worldManifold.normal;
98 cp->state = state2[i];
99 cp->normalImpulse = manifold->points[i].normalImpulse;
100 cp->tangentImpulse = manifold->points[i].tangentImpulse;
101 cp->separation = worldManifold.separations[i];
102 ++m_pointCount;
103 }
104 }
105
DrawTitle(const char * string)106 void Test::DrawTitle(const char *string)
107 {
108 g_debugDraw.DrawString(5, 5, string);
109 m_textLine = int32(26.0f);
110 }
111
112 class QueryCallback : public b2QueryCallback
113 {
114 public:
QueryCallback(const b2Vec2 & point)115 QueryCallback(const b2Vec2& point)
116 {
117 m_point = point;
118 m_fixture = NULL;
119 }
120
ReportFixture(b2Fixture * fixture)121 bool ReportFixture(b2Fixture* fixture) override
122 {
123 b2Body* body = fixture->GetBody();
124 if (body->GetType() == b2_dynamicBody)
125 {
126 bool inside = fixture->TestPoint(m_point);
127 if (inside)
128 {
129 m_fixture = fixture;
130
131 // We are done, terminate the query.
132 return false;
133 }
134 }
135
136 // Continue the query.
137 return true;
138 }
139
140 b2Vec2 m_point;
141 b2Fixture* m_fixture;
142 };
143
MouseDown(const b2Vec2 & p)144 void Test::MouseDown(const b2Vec2& p)
145 {
146 m_mouseWorld = p;
147
148 if (m_mouseJoint != NULL)
149 {
150 return;
151 }
152
153 // Make a small box.
154 b2AABB aabb;
155 b2Vec2 d;
156 d.Set(0.001f, 0.001f);
157 aabb.lowerBound = p - d;
158 aabb.upperBound = p + d;
159
160 // Query the world for overlapping shapes.
161 QueryCallback callback(p);
162 m_world->QueryAABB(&callback, aabb);
163
164 if (callback.m_fixture)
165 {
166 float frequencyHz = 5.0f;
167 float dampingRatio = 0.7f;
168
169 b2Body* body = callback.m_fixture->GetBody();
170 b2MouseJointDef jd;
171 jd.bodyA = m_groundBody;
172 jd.bodyB = body;
173 jd.target = p;
174 jd.maxForce = 1000.0f * body->GetMass();
175 b2LinearStiffness(jd.stiffness, jd.damping, frequencyHz, dampingRatio, jd.bodyA, jd.bodyB);
176
177 m_mouseJoint = (b2MouseJoint*)m_world->CreateJoint(&jd);
178 body->SetAwake(true);
179 }
180 }
181
SpawnBomb(const b2Vec2 & worldPt)182 void Test::SpawnBomb(const b2Vec2& worldPt)
183 {
184 m_bombSpawnPoint = worldPt;
185 m_bombSpawning = true;
186 }
187
CompleteBombSpawn(const b2Vec2 & p)188 void Test::CompleteBombSpawn(const b2Vec2& p)
189 {
190 if (m_bombSpawning == false)
191 {
192 return;
193 }
194
195 const float multiplier = 30.0f;
196 b2Vec2 vel = m_bombSpawnPoint - p;
197 vel *= multiplier;
198 LaunchBomb(m_bombSpawnPoint,vel);
199 m_bombSpawning = false;
200 }
201
ShiftMouseDown(const b2Vec2 & p)202 void Test::ShiftMouseDown(const b2Vec2& p)
203 {
204 m_mouseWorld = p;
205
206 if (m_mouseJoint != NULL)
207 {
208 return;
209 }
210
211 SpawnBomb(p);
212 }
213
MouseUp(const b2Vec2 & p)214 void Test::MouseUp(const b2Vec2& p)
215 {
216 if (m_mouseJoint)
217 {
218 m_world->DestroyJoint(m_mouseJoint);
219 m_mouseJoint = NULL;
220 }
221
222 if (m_bombSpawning)
223 {
224 CompleteBombSpawn(p);
225 }
226 }
227
MouseMove(const b2Vec2 & p)228 void Test::MouseMove(const b2Vec2& p)
229 {
230 m_mouseWorld = p;
231
232 if (m_mouseJoint)
233 {
234 m_mouseJoint->SetTarget(p);
235 }
236 }
237
LaunchBomb()238 void Test::LaunchBomb()
239 {
240 b2Vec2 p(RandomFloat(-15.0f, 15.0f), 30.0f);
241 b2Vec2 v = -5.0f * p;
242 LaunchBomb(p, v);
243 }
244
LaunchBomb(const b2Vec2 & position,const b2Vec2 & velocity)245 void Test::LaunchBomb(const b2Vec2& position, const b2Vec2& velocity)
246 {
247 if (m_bomb)
248 {
249 m_world->DestroyBody(m_bomb);
250 m_bomb = NULL;
251 }
252
253 b2BodyDef bd;
254 bd.type = b2_dynamicBody;
255 bd.position = position;
256 bd.bullet = true;
257 m_bomb = m_world->CreateBody(&bd);
258 m_bomb->SetLinearVelocity(velocity);
259
260 b2CircleShape circle;
261 circle.m_radius = 0.3f;
262
263 b2FixtureDef fd;
264 fd.shape = &circle;
265 fd.density = 20.0f;
266 fd.restitution = 0.0f;
267
268 b2Vec2 minV = position - b2Vec2(0.3f,0.3f);
269 b2Vec2 maxV = position + b2Vec2(0.3f,0.3f);
270
271 b2AABB aabb;
272 aabb.lowerBound = minV;
273 aabb.upperBound = maxV;
274
275 m_bomb->CreateFixture(&fd);
276 }
277
Step(Settings & settings)278 void Test::Step(Settings& settings)
279 {
280 float timeStep = settings.m_hertz > 0.0f ? 1.0f / settings.m_hertz : float(0.0f);
281
282 if (settings.m_pause)
283 {
284 if (settings.m_singleStep)
285 {
286 settings.m_singleStep = 0;
287 }
288 else
289 {
290 timeStep = 0.0f;
291 }
292
293 g_debugDraw.DrawString(5, m_textLine, "****PAUSED****");
294 m_textLine += m_textIncrement;
295 }
296
297 uint32 flags = 0;
298 flags += settings.m_drawShapes * b2Draw::e_shapeBit;
299 flags += settings.m_drawJoints * b2Draw::e_jointBit;
300 flags += settings.m_drawAABBs * b2Draw::e_aabbBit;
301 flags += settings.m_drawCOMs * b2Draw::e_centerOfMassBit;
302 g_debugDraw.SetFlags(flags);
303
304 m_world->SetAllowSleeping(settings.m_enableSleep);
305 m_world->SetWarmStarting(settings.m_enableWarmStarting);
306 m_world->SetContinuousPhysics(settings.m_enableContinuous);
307 m_world->SetSubStepping(settings.m_enableSubStepping);
308
309 m_pointCount = 0;
310
311 m_world->Step(timeStep, settings.m_velocityIterations, settings.m_positionIterations);
312
313 m_world->DebugDraw();
314 g_debugDraw.Flush();
315
316 if (timeStep > 0.0f)
317 {
318 ++m_stepCount;
319 }
320
321 if (settings.m_drawStats)
322 {
323 int32 bodyCount = m_world->GetBodyCount();
324 int32 contactCount = m_world->GetContactCount();
325 int32 jointCount = m_world->GetJointCount();
326 g_debugDraw.DrawString(5, m_textLine, "bodies/contacts/joints = %d/%d/%d", bodyCount, contactCount, jointCount);
327 m_textLine += m_textIncrement;
328
329 int32 proxyCount = m_world->GetProxyCount();
330 int32 height = m_world->GetTreeHeight();
331 int32 balance = m_world->GetTreeBalance();
332 float quality = m_world->GetTreeQuality();
333 g_debugDraw.DrawString(5, m_textLine, "proxies/height/balance/quality = %d/%d/%d/%g", proxyCount, height, balance, quality);
334 m_textLine += m_textIncrement;
335 }
336
337 // Track maximum profile times
338 {
339 const b2Profile& p = m_world->GetProfile();
340 m_maxProfile.step = b2Max(m_maxProfile.step, p.step);
341 m_maxProfile.collide = b2Max(m_maxProfile.collide, p.collide);
342 m_maxProfile.solve = b2Max(m_maxProfile.solve, p.solve);
343 m_maxProfile.solveInit = b2Max(m_maxProfile.solveInit, p.solveInit);
344 m_maxProfile.solveVelocity = b2Max(m_maxProfile.solveVelocity, p.solveVelocity);
345 m_maxProfile.solvePosition = b2Max(m_maxProfile.solvePosition, p.solvePosition);
346 m_maxProfile.solveTOI = b2Max(m_maxProfile.solveTOI, p.solveTOI);
347 m_maxProfile.broadphase = b2Max(m_maxProfile.broadphase, p.broadphase);
348
349 m_totalProfile.step += p.step;
350 m_totalProfile.collide += p.collide;
351 m_totalProfile.solve += p.solve;
352 m_totalProfile.solveInit += p.solveInit;
353 m_totalProfile.solveVelocity += p.solveVelocity;
354 m_totalProfile.solvePosition += p.solvePosition;
355 m_totalProfile.solveTOI += p.solveTOI;
356 m_totalProfile.broadphase += p.broadphase;
357 }
358
359 if (settings.m_drawProfile)
360 {
361 const b2Profile& p = m_world->GetProfile();
362
363 b2Profile aveProfile;
364 memset(&aveProfile, 0, sizeof(b2Profile));
365 if (m_stepCount > 0)
366 {
367 float scale = 1.0f / m_stepCount;
368 aveProfile.step = scale * m_totalProfile.step;
369 aveProfile.collide = scale * m_totalProfile.collide;
370 aveProfile.solve = scale * m_totalProfile.solve;
371 aveProfile.solveInit = scale * m_totalProfile.solveInit;
372 aveProfile.solveVelocity = scale * m_totalProfile.solveVelocity;
373 aveProfile.solvePosition = scale * m_totalProfile.solvePosition;
374 aveProfile.solveTOI = scale * m_totalProfile.solveTOI;
375 aveProfile.broadphase = scale * m_totalProfile.broadphase;
376 }
377
378 g_debugDraw.DrawString(5, m_textLine, "step [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.step, aveProfile.step, m_maxProfile.step);
379 m_textLine += m_textIncrement;
380 g_debugDraw.DrawString(5, m_textLine, "collide [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.collide, aveProfile.collide, m_maxProfile.collide);
381 m_textLine += m_textIncrement;
382 g_debugDraw.DrawString(5, m_textLine, "solve [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.solve, aveProfile.solve, m_maxProfile.solve);
383 m_textLine += m_textIncrement;
384 g_debugDraw.DrawString(5, m_textLine, "solve init [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.solveInit, aveProfile.solveInit, m_maxProfile.solveInit);
385 m_textLine += m_textIncrement;
386 g_debugDraw.DrawString(5, m_textLine, "solve velocity [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.solveVelocity, aveProfile.solveVelocity, m_maxProfile.solveVelocity);
387 m_textLine += m_textIncrement;
388 g_debugDraw.DrawString(5, m_textLine, "solve position [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.solvePosition, aveProfile.solvePosition, m_maxProfile.solvePosition);
389 m_textLine += m_textIncrement;
390 g_debugDraw.DrawString(5, m_textLine, "solveTOI [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.solveTOI, aveProfile.solveTOI, m_maxProfile.solveTOI);
391 m_textLine += m_textIncrement;
392 g_debugDraw.DrawString(5, m_textLine, "broad-phase [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.broadphase, aveProfile.broadphase, m_maxProfile.broadphase);
393 m_textLine += m_textIncrement;
394 }
395
396 if (m_bombSpawning)
397 {
398 b2Color c;
399 c.Set(0.0f, 0.0f, 1.0f);
400 g_debugDraw.DrawPoint(m_bombSpawnPoint, 4.0f, c);
401
402 c.Set(0.8f, 0.8f, 0.8f);
403 g_debugDraw.DrawSegment(m_mouseWorld, m_bombSpawnPoint, c);
404 }
405
406 if (settings.m_drawContactPoints)
407 {
408 const float k_impulseScale = 0.1f;
409 const float k_axisScale = 0.3f;
410
411 for (int32 i = 0; i < m_pointCount; ++i)
412 {
413 ContactPoint* point = m_points + i;
414
415 if (point->state == b2_addState)
416 {
417 // Add
418 g_debugDraw.DrawPoint(point->position, 10.0f, b2Color(0.3f, 0.95f, 0.3f));
419 }
420 else if (point->state == b2_persistState)
421 {
422 // Persist
423 g_debugDraw.DrawPoint(point->position, 5.0f, b2Color(0.3f, 0.3f, 0.95f));
424 }
425
426 if (settings.m_drawContactNormals == 1)
427 {
428 b2Vec2 p1 = point->position;
429 b2Vec2 p2 = p1 + k_axisScale * point->normal;
430 g_debugDraw.DrawSegment(p1, p2, b2Color(0.9f, 0.9f, 0.9f));
431 }
432 else if (settings.m_drawContactImpulse == 1)
433 {
434 b2Vec2 p1 = point->position;
435 b2Vec2 p2 = p1 + k_impulseScale * point->normalImpulse * point->normal;
436 g_debugDraw.DrawSegment(p1, p2, b2Color(0.9f, 0.9f, 0.3f));
437 }
438
439 if (settings.m_drawFrictionImpulse == 1)
440 {
441 b2Vec2 tangent = b2Cross(point->normal, 1.0f);
442 b2Vec2 p1 = point->position;
443 b2Vec2 p2 = p1 + k_impulseScale * point->tangentImpulse * tangent;
444 g_debugDraw.DrawSegment(p1, p2, b2Color(0.9f, 0.9f, 0.3f));
445 }
446 }
447 }
448 }
449
ShiftOrigin(const b2Vec2 & newOrigin)450 void Test::ShiftOrigin(const b2Vec2& newOrigin)
451 {
452 m_world->ShiftOrigin(newOrigin);
453 }
454
455 TestEntry g_testEntries[MAX_TESTS] = { {nullptr} };
456 int g_testCount = 0;
457
RegisterTest(const char * category,const char * name,TestCreateFcn * fcn)458 int RegisterTest(const char* category, const char* name, TestCreateFcn* fcn)
459 {
460 int index = g_testCount;
461 if (index < MAX_TESTS)
462 {
463 g_testEntries[index] = { category, name, fcn };
464 ++g_testCount;
465 return index;
466 }
467
468 return -1;
469 }
470