1 /*
2 * Copyright (c) 2006-2007 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 #include <Box2D/Dynamics/b2Body.h>
20 #include <Box2D/Dynamics/b2Fixture.h>
21 #include <Box2D/Dynamics/b2World.h>
22 #include <Box2D/Dynamics/Contacts/b2Contact.h>
23 #include <Box2D/Dynamics/Joints/b2Joint.h>
24
b2Body(const b2BodyDef * bd,b2World * world)25 b2Body::b2Body(const b2BodyDef* bd, b2World* world)
26 {
27 b2Assert(bd->position.IsValid());
28 b2Assert(bd->linearVelocity.IsValid());
29 b2Assert(b2IsValid(bd->angle));
30 b2Assert(b2IsValid(bd->angularVelocity));
31 b2Assert(b2IsValid(bd->angularDamping) && bd->angularDamping >= 0.0f);
32 b2Assert(b2IsValid(bd->linearDamping) && bd->linearDamping >= 0.0f);
33
34 m_flags = 0;
35
36 if (bd->bullet)
37 {
38 m_flags |= e_bulletFlag;
39 }
40 if (bd->fixedRotation)
41 {
42 m_flags |= e_fixedRotationFlag;
43 }
44 if (bd->allowSleep)
45 {
46 m_flags |= e_autoSleepFlag;
47 }
48 if (bd->awake)
49 {
50 m_flags |= e_awakeFlag;
51 }
52 if (bd->active)
53 {
54 m_flags |= e_activeFlag;
55 }
56
57 m_world = world;
58
59 m_xf.p = bd->position;
60 m_xf.q.Set(bd->angle);
61
62 m_sweep.localCenter.SetZero();
63 m_sweep.c0 = m_xf.p;
64 m_sweep.c = m_xf.p;
65 m_sweep.a0 = bd->angle;
66 m_sweep.a = bd->angle;
67 m_sweep.alpha0 = 0.0f;
68
69 m_jointList = NULL;
70 m_contactList = NULL;
71 m_prev = NULL;
72 m_next = NULL;
73
74 m_linearVelocity = bd->linearVelocity;
75 m_angularVelocity = bd->angularVelocity;
76
77 m_linearDamping = bd->linearDamping;
78 m_angularDamping = bd->angularDamping;
79 m_gravityScale = bd->gravityScale;
80
81 m_force.SetZero();
82 m_torque = 0.0f;
83
84 m_sleepTime = 0.0f;
85
86 m_type = bd->type;
87
88 if (m_type == b2_dynamicBody)
89 {
90 m_mass = 1.0f;
91 m_invMass = 1.0f;
92 }
93 else
94 {
95 m_mass = 0.0f;
96 m_invMass = 0.0f;
97 }
98
99 m_I = 0.0f;
100 m_invI = 0.0f;
101
102 m_userData = bd->userData;
103
104 m_fixtureList = NULL;
105 m_fixtureCount = 0;
106 }
107
~b2Body()108 b2Body::~b2Body()
109 {
110 // shapes and joints are destroyed in b2World::Destroy
111 }
112
SetType(b2BodyType type)113 void b2Body::SetType(b2BodyType type)
114 {
115 b2Assert(m_world->IsLocked() == false);
116 if (m_world->IsLocked() == true)
117 {
118 return;
119 }
120
121 if (m_type == type)
122 {
123 return;
124 }
125
126 m_type = type;
127
128 ResetMassData();
129
130 if (m_type == b2_staticBody)
131 {
132 m_linearVelocity.SetZero();
133 m_angularVelocity = 0.0f;
134 m_sweep.a0 = m_sweep.a;
135 m_sweep.c0 = m_sweep.c;
136 SynchronizeFixtures();
137 }
138
139 SetAwake(true);
140
141 m_force.SetZero();
142 m_torque = 0.0f;
143
144 // Delete the attached contacts.
145 b2ContactEdge* ce = m_contactList;
146 while (ce)
147 {
148 b2ContactEdge* ce0 = ce;
149 ce = ce->next;
150 m_world->m_contactManager.Destroy(ce0->contact);
151 }
152 m_contactList = NULL;
153
154 // Touch the proxies so that new contacts will be created (when appropriate)
155 b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
156 for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
157 {
158 int32 proxyCount = f->m_proxyCount;
159 for (int32 i = 0; i < proxyCount; ++i)
160 {
161 broadPhase->TouchProxy(f->m_proxies[i].proxyId);
162 }
163 }
164 }
165
CreateFixture(const b2FixtureDef * def)166 b2Fixture* b2Body::CreateFixture(const b2FixtureDef* def)
167 {
168 b2Assert(m_world->IsLocked() == false);
169 if (m_world->IsLocked() == true)
170 {
171 return NULL;
172 }
173
174 b2BlockAllocator* allocator = &m_world->m_blockAllocator;
175
176 void* memory = allocator->Allocate(sizeof(b2Fixture));
177 b2Fixture* fixture = new (memory) b2Fixture;
178 fixture->Create(allocator, this, def);
179
180 if (m_flags & e_activeFlag)
181 {
182 b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
183 fixture->CreateProxies(broadPhase, m_xf);
184 }
185
186 fixture->m_next = m_fixtureList;
187 m_fixtureList = fixture;
188 ++m_fixtureCount;
189
190 fixture->m_body = this;
191
192 // Adjust mass properties if needed.
193 if (fixture->m_density > 0.0f)
194 {
195 ResetMassData();
196 }
197
198 // Let the world know we have a new fixture. This will cause new contacts
199 // to be created at the beginning of the next time step.
200 m_world->m_flags |= b2World::e_newFixture;
201
202 return fixture;
203 }
204
CreateFixture(const b2Shape * shape,float32 density)205 b2Fixture* b2Body::CreateFixture(const b2Shape* shape, float32 density)
206 {
207 b2FixtureDef def;
208 def.shape = shape;
209 def.density = density;
210
211 return CreateFixture(&def);
212 }
213
DestroyFixture(b2Fixture * fixture)214 void b2Body::DestroyFixture(b2Fixture* fixture)
215 {
216 if (fixture == nullptr)
217 {
218 return;
219 }
220
221 b2Assert(m_world->IsLocked() == false);
222 if (m_world->IsLocked() == true)
223 {
224 return;
225 }
226
227 b2Assert(fixture->m_body == this);
228
229 // Remove the fixture from this body's singly linked list.
230 b2Assert(m_fixtureCount > 0);
231 b2Fixture** node = &m_fixtureList;
232 bool found = false;
233 while (*node != NULL)
234 {
235 if (*node == fixture)
236 {
237 *node = fixture->m_next;
238 found = true;
239 break;
240 }
241
242 node = &(*node)->m_next;
243 }
244
245 // You tried to remove a shape that is not attached to this body.
246 b2Assert(found);
247
248 // Destroy any contacts associated with the fixture.
249 b2ContactEdge* edge = m_contactList;
250 while (edge)
251 {
252 b2Contact* c = edge->contact;
253 edge = edge->next;
254
255 b2Fixture* fixtureA = c->GetFixtureA();
256 b2Fixture* fixtureB = c->GetFixtureB();
257
258 if (fixture == fixtureA || fixture == fixtureB)
259 {
260 // This destroys the contact and removes it from
261 // this body's contact list.
262 m_world->m_contactManager.Destroy(c);
263 }
264 }
265
266 b2BlockAllocator* allocator = &m_world->m_blockAllocator;
267
268 if (m_flags & e_activeFlag)
269 {
270 b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
271 fixture->DestroyProxies(broadPhase);
272 }
273
274 fixture->m_body = NULL;
275 fixture->m_next = NULL;
276 fixture->Destroy(allocator);
277 fixture->~b2Fixture();
278 allocator->Free(fixture, sizeof(b2Fixture));
279
280 --m_fixtureCount;
281
282 // Reset the mass data.
283 ResetMassData();
284 }
285
ResetMassData()286 void b2Body::ResetMassData()
287 {
288 // Compute mass data from shapes. Each shape has its own density.
289 m_mass = 0.0f;
290 m_invMass = 0.0f;
291 m_I = 0.0f;
292 m_invI = 0.0f;
293 m_sweep.localCenter.SetZero();
294
295 // Static and kinematic bodies have zero mass.
296 if (m_type == b2_staticBody || m_type == b2_kinematicBody)
297 {
298 m_sweep.c0 = m_xf.p;
299 m_sweep.c = m_xf.p;
300 m_sweep.a0 = m_sweep.a;
301 return;
302 }
303
304 b2Assert(m_type == b2_dynamicBody);
305
306 // Accumulate mass over all fixtures.
307 b2Vec2 localCenter = b2Vec2_zero;
308 for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
309 {
310 if (f->m_density == 0.0f)
311 {
312 continue;
313 }
314
315 b2MassData massData;
316 f->GetMassData(&massData);
317 m_mass += massData.mass;
318 localCenter += massData.mass * massData.center;
319 m_I += massData.I;
320 }
321
322 // Compute center of mass.
323 if (m_mass > 0.0f)
324 {
325 m_invMass = 1.0f / m_mass;
326 localCenter *= m_invMass;
327 }
328 else
329 {
330 // Force all dynamic bodies to have a positive mass.
331 m_mass = 1.0f;
332 m_invMass = 1.0f;
333 }
334
335 if (m_I > 0.0f && (m_flags & e_fixedRotationFlag) == 0)
336 {
337 // Center the inertia about the center of mass.
338 m_I -= m_mass * b2Dot(localCenter, localCenter);
339 b2Assert(m_I > 0.0f);
340 m_invI = 1.0f / m_I;
341
342 }
343 else
344 {
345 m_I = 0.0f;
346 m_invI = 0.0f;
347 }
348
349 // Move center of mass.
350 b2Vec2 oldCenter = m_sweep.c;
351 m_sweep.localCenter = localCenter;
352 m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
353
354 // Update center of mass velocity.
355 m_linearVelocity += b2Cross(m_angularVelocity, m_sweep.c - oldCenter);
356 }
357
SetMassData(const b2MassData * massData)358 void b2Body::SetMassData(const b2MassData* massData)
359 {
360 b2Assert(m_world->IsLocked() == false);
361 if (m_world->IsLocked() == true)
362 {
363 return;
364 }
365
366 if (m_type != b2_dynamicBody)
367 {
368 return;
369 }
370
371 m_invMass = 0.0f;
372 m_I = 0.0f;
373 m_invI = 0.0f;
374
375 m_mass = massData->mass;
376 if (m_mass <= 0.0f)
377 {
378 m_mass = 1.0f;
379 }
380
381 m_invMass = 1.0f / m_mass;
382
383 if (massData->I > 0.0f && (m_flags & b2Body::e_fixedRotationFlag) == 0)
384 {
385 m_I = massData->I - m_mass * b2Dot(massData->center, massData->center);
386 b2Assert(m_I > 0.0f);
387 m_invI = 1.0f / m_I;
388 }
389
390 // Move center of mass.
391 b2Vec2 oldCenter = m_sweep.c;
392 m_sweep.localCenter = massData->center;
393 m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
394
395 // Update center of mass velocity.
396 m_linearVelocity += b2Cross(m_angularVelocity, m_sweep.c - oldCenter);
397 }
398
ShouldCollide(const b2Body * other) const399 bool b2Body::ShouldCollide(const b2Body* other) const
400 {
401 // At least one body should be dynamic.
402 if (m_type != b2_dynamicBody && other->m_type != b2_dynamicBody)
403 {
404 return false;
405 }
406
407 // Does a joint prevent collision?
408 for (b2JointEdge* jn = m_jointList; jn; jn = jn->next)
409 {
410 if (jn->other == other)
411 {
412 if (jn->joint->m_collideConnected == false)
413 {
414 return false;
415 }
416 }
417 }
418
419 return true;
420 }
421
SetTransform(const b2Vec2 & position,float32 angle)422 void b2Body::SetTransform(const b2Vec2& position, float32 angle)
423 {
424 b2Assert(m_world->IsLocked() == false);
425 if (m_world->IsLocked() == true)
426 {
427 return;
428 }
429
430 m_xf.q.Set(angle);
431 m_xf.p = position;
432
433 m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
434 m_sweep.a = angle;
435
436 m_sweep.c0 = m_sweep.c;
437 m_sweep.a0 = angle;
438
439 b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
440 for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
441 {
442 f->Synchronize(broadPhase, m_xf, m_xf);
443 }
444 }
445
SynchronizeFixtures()446 void b2Body::SynchronizeFixtures()
447 {
448 b2Transform xf1;
449 xf1.q.Set(m_sweep.a0);
450 xf1.p = m_sweep.c0 - b2Mul(xf1.q, m_sweep.localCenter);
451
452 b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
453 for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
454 {
455 f->Synchronize(broadPhase, xf1, m_xf);
456 }
457 }
458
SetActive(bool flag)459 void b2Body::SetActive(bool flag)
460 {
461 b2Assert(m_world->IsLocked() == false);
462
463 if (flag == IsActive())
464 {
465 return;
466 }
467
468 if (flag)
469 {
470 m_flags |= e_activeFlag;
471
472 // Create all proxies.
473 b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
474 for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
475 {
476 f->CreateProxies(broadPhase, m_xf);
477 }
478
479 // Contacts are created the next time step.
480 }
481 else
482 {
483 m_flags &= ~e_activeFlag;
484
485 // Destroy all proxies.
486 b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
487 for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
488 {
489 f->DestroyProxies(broadPhase);
490 }
491
492 // Destroy the attached contacts.
493 b2ContactEdge* ce = m_contactList;
494 while (ce)
495 {
496 b2ContactEdge* ce0 = ce;
497 ce = ce->next;
498 m_world->m_contactManager.Destroy(ce0->contact);
499 }
500 m_contactList = NULL;
501 }
502 }
503
SetFixedRotation(bool flag)504 void b2Body::SetFixedRotation(bool flag)
505 {
506 bool status = (m_flags & e_fixedRotationFlag) == e_fixedRotationFlag;
507 if (status == flag)
508 {
509 return;
510 }
511
512 if (flag)
513 {
514 m_flags |= e_fixedRotationFlag;
515 }
516 else
517 {
518 m_flags &= ~e_fixedRotationFlag;
519 }
520
521 m_angularVelocity = 0.0f;
522
523 ResetMassData();
524 }
525
Dump()526 void b2Body::Dump()
527 {
528 int32 bodyIndex = m_islandIndex;
529
530 b2Log("{\n");
531 b2Log(" b2BodyDef bd;\n");
532 b2Log(" bd.type = b2BodyType(%d);\n", m_type);
533 b2Log(" bd.position.Set(%.15lef, %.15lef);\n", m_xf.p.x, m_xf.p.y);
534 b2Log(" bd.angle = %.15lef;\n", m_sweep.a);
535 b2Log(" bd.linearVelocity.Set(%.15lef, %.15lef);\n", m_linearVelocity.x, m_linearVelocity.y);
536 b2Log(" bd.angularVelocity = %.15lef;\n", m_angularVelocity);
537 b2Log(" bd.linearDamping = %.15lef;\n", m_linearDamping);
538 b2Log(" bd.angularDamping = %.15lef;\n", m_angularDamping);
539 b2Log(" bd.allowSleep = bool(%d);\n", m_flags & e_autoSleepFlag);
540 b2Log(" bd.awake = bool(%d);\n", m_flags & e_awakeFlag);
541 b2Log(" bd.fixedRotation = bool(%d);\n", m_flags & e_fixedRotationFlag);
542 b2Log(" bd.bullet = bool(%d);\n", m_flags & e_bulletFlag);
543 b2Log(" bd.active = bool(%d);\n", m_flags & e_activeFlag);
544 b2Log(" bd.gravityScale = %.15lef;\n", m_gravityScale);
545 b2Log(" bodies[%d] = m_world->CreateBody(&bd);\n", m_islandIndex);
546 b2Log("\n");
547 for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
548 {
549 b2Log(" {\n");
550 f->Dump(bodyIndex);
551 b2Log(" }\n");
552 }
553 b2Log("}\n");
554 }
555