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 // Since the body type changed, we need to flag contacts for filtering.
145 for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
146 {
147 f->Refilter();
148 }
149 }
150
CreateFixture(const b2FixtureDef * def)151 b2Fixture* b2Body::CreateFixture(const b2FixtureDef* def)
152 {
153 b2Assert(m_world->IsLocked() == false);
154 if (m_world->IsLocked() == true)
155 {
156 return NULL;
157 }
158
159 b2BlockAllocator* allocator = &m_world->m_blockAllocator;
160
161 void* memory = allocator->Allocate(sizeof(b2Fixture));
162 b2Fixture* fixture = new (memory) b2Fixture;
163 fixture->Create(allocator, this, def);
164
165 if (m_flags & e_activeFlag)
166 {
167 b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
168 fixture->CreateProxies(broadPhase, m_xf);
169 }
170
171 fixture->m_next = m_fixtureList;
172 m_fixtureList = fixture;
173 ++m_fixtureCount;
174
175 fixture->m_body = this;
176
177 // Adjust mass properties if needed.
178 if (fixture->m_density > 0.0f)
179 {
180 ResetMassData();
181 }
182
183 // Let the world know we have a new fixture. This will cause new contacts
184 // to be created at the beginning of the next time step.
185 m_world->m_flags |= b2World::e_newFixture;
186
187 return fixture;
188 }
189
CreateFixture(const b2Shape * shape,float32 density)190 b2Fixture* b2Body::CreateFixture(const b2Shape* shape, float32 density)
191 {
192 b2FixtureDef def;
193 def.shape = shape;
194 def.density = density;
195
196 return CreateFixture(&def);
197 }
198
DestroyFixture(b2Fixture * fixture)199 void b2Body::DestroyFixture(b2Fixture* fixture)
200 {
201 b2Assert(m_world->IsLocked() == false);
202 if (m_world->IsLocked() == true)
203 {
204 return;
205 }
206
207 b2Assert(fixture->m_body == this);
208
209 // Remove the fixture from this body's singly linked list.
210 b2Assert(m_fixtureCount > 0);
211 b2Fixture** node = &m_fixtureList;
212 bool found = false;
213 while (*node != NULL)
214 {
215 if (*node == fixture)
216 {
217 *node = fixture->m_next;
218 found = true;
219 break;
220 }
221
222 node = &(*node)->m_next;
223 }
224
225 // You tried to remove a shape that is not attached to this body.
226 b2Assert(found);
227
228 // Destroy any contacts associated with the fixture.
229 b2ContactEdge* edge = m_contactList;
230 while (edge)
231 {
232 b2Contact* c = edge->contact;
233 edge = edge->next;
234
235 b2Fixture* fixtureA = c->GetFixtureA();
236 b2Fixture* fixtureB = c->GetFixtureB();
237
238 if (fixture == fixtureA || fixture == fixtureB)
239 {
240 // This destroys the contact and removes it from
241 // this body's contact list.
242 m_world->m_contactManager.Destroy(c);
243 }
244 }
245
246 b2BlockAllocator* allocator = &m_world->m_blockAllocator;
247
248 if (m_flags & e_activeFlag)
249 {
250 b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
251 fixture->DestroyProxies(broadPhase);
252 }
253
254 fixture->Destroy(allocator);
255 fixture->m_body = NULL;
256 fixture->m_next = NULL;
257 fixture->~b2Fixture();
258 allocator->Free(fixture, sizeof(b2Fixture));
259
260 --m_fixtureCount;
261
262 // Reset the mass data.
263 ResetMassData();
264 }
265
ResetMassData()266 void b2Body::ResetMassData()
267 {
268 // Compute mass data from shapes. Each shape has its own density.
269 m_mass = 0.0f;
270 m_invMass = 0.0f;
271 m_I = 0.0f;
272 m_invI = 0.0f;
273 m_sweep.localCenter.SetZero();
274
275 // Static and kinematic bodies have zero mass.
276 if (m_type == b2_staticBody || m_type == b2_kinematicBody)
277 {
278 m_sweep.c0 = m_xf.p;
279 m_sweep.c = m_xf.p;
280 m_sweep.a0 = m_sweep.a;
281 return;
282 }
283
284 b2Assert(m_type == b2_dynamicBody);
285
286 // Accumulate mass over all fixtures.
287 b2Vec2 localCenter = b2Vec2_zero;
288 for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
289 {
290 if (f->m_density == 0.0f)
291 {
292 continue;
293 }
294
295 b2MassData massData;
296 f->GetMassData(&massData);
297 m_mass += massData.mass;
298 localCenter += massData.mass * massData.center;
299 m_I += massData.I;
300 }
301
302 // Compute center of mass.
303 if (m_mass > 0.0f)
304 {
305 m_invMass = 1.0f / m_mass;
306 localCenter *= m_invMass;
307 }
308 else
309 {
310 // Force all dynamic bodies to have a positive mass.
311 m_mass = 1.0f;
312 m_invMass = 1.0f;
313 }
314
315 if (m_I > 0.0f && (m_flags & e_fixedRotationFlag) == 0)
316 {
317 // Center the inertia about the center of mass.
318 m_I -= m_mass * b2Dot(localCenter, localCenter);
319 b2Assert(m_I > 0.0f);
320 m_invI = 1.0f / m_I;
321
322 }
323 else
324 {
325 m_I = 0.0f;
326 m_invI = 0.0f;
327 }
328
329 // Move center of mass.
330 b2Vec2 oldCenter = m_sweep.c;
331 m_sweep.localCenter = localCenter;
332 m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
333
334 // Update center of mass velocity.
335 m_linearVelocity += b2Cross(m_angularVelocity, m_sweep.c - oldCenter);
336 }
337
SetMassData(const b2MassData * massData)338 void b2Body::SetMassData(const b2MassData* massData)
339 {
340 b2Assert(m_world->IsLocked() == false);
341 if (m_world->IsLocked() == true)
342 {
343 return;
344 }
345
346 if (m_type != b2_dynamicBody)
347 {
348 return;
349 }
350
351 m_invMass = 0.0f;
352 m_I = 0.0f;
353 m_invI = 0.0f;
354
355 m_mass = massData->mass;
356 if (m_mass <= 0.0f)
357 {
358 m_mass = 1.0f;
359 }
360
361 m_invMass = 1.0f / m_mass;
362
363 if (massData->I > 0.0f && (m_flags & b2Body::e_fixedRotationFlag) == 0)
364 {
365 m_I = massData->I - m_mass * b2Dot(massData->center, massData->center);
366 b2Assert(m_I > 0.0f);
367 m_invI = 1.0f / m_I;
368 }
369
370 // Move center of mass.
371 b2Vec2 oldCenter = m_sweep.c;
372 m_sweep.localCenter = massData->center;
373 m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
374
375 // Update center of mass velocity.
376 m_linearVelocity += b2Cross(m_angularVelocity, m_sweep.c - oldCenter);
377 }
378
ShouldCollide(const b2Body * other) const379 bool b2Body::ShouldCollide(const b2Body* other) const
380 {
381 // At least one body should be dynamic.
382 if (m_type != b2_dynamicBody && other->m_type != b2_dynamicBody)
383 {
384 return false;
385 }
386
387 // Does a joint prevent collision?
388 for (b2JointEdge* jn = m_jointList; jn; jn = jn->next)
389 {
390 if (jn->other == other)
391 {
392 if (jn->joint->m_collideConnected == false)
393 {
394 return false;
395 }
396 }
397 }
398
399 return true;
400 }
401
SetTransform(const b2Vec2 & position,float32 angle)402 void b2Body::SetTransform(const b2Vec2& position, float32 angle)
403 {
404 b2Assert(m_world->IsLocked() == false);
405 if (m_world->IsLocked() == true)
406 {
407 return;
408 }
409
410 m_xf.q.Set(angle);
411 m_xf.p = position;
412
413 m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
414 m_sweep.a = angle;
415
416 m_sweep.c0 = m_sweep.c;
417 m_sweep.a0 = angle;
418
419 b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
420 for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
421 {
422 f->Synchronize(broadPhase, m_xf, m_xf);
423 }
424
425 m_world->m_contactManager.FindNewContacts();
426 }
427
SynchronizeFixtures()428 void b2Body::SynchronizeFixtures()
429 {
430 b2Transform xf1;
431 xf1.q.Set(m_sweep.a0);
432 xf1.p = m_sweep.c0 - b2Mul(xf1.q, m_sweep.localCenter);
433
434 b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
435 for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
436 {
437 f->Synchronize(broadPhase, xf1, m_xf);
438 }
439 }
440
SetActive(bool flag)441 void b2Body::SetActive(bool flag)
442 {
443 b2Assert(m_world->IsLocked() == false);
444
445 if (flag == IsActive())
446 {
447 return;
448 }
449
450 if (flag)
451 {
452 m_flags |= e_activeFlag;
453
454 // Create all proxies.
455 b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
456 for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
457 {
458 f->CreateProxies(broadPhase, m_xf);
459 }
460
461 // Contacts are created the next time step.
462 }
463 else
464 {
465 m_flags &= ~e_activeFlag;
466
467 // Destroy all proxies.
468 b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
469 for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
470 {
471 f->DestroyProxies(broadPhase);
472 }
473
474 // Destroy the attached contacts.
475 b2ContactEdge* ce = m_contactList;
476 while (ce)
477 {
478 b2ContactEdge* ce0 = ce;
479 ce = ce->next;
480 m_world->m_contactManager.Destroy(ce0->contact);
481 }
482 m_contactList = NULL;
483 }
484 }
485
Dump()486 void b2Body::Dump()
487 {
488 int32 bodyIndex = m_islandIndex;
489
490 b2Log("{\n");
491 b2Log(" b2BodyDef bd;\n");
492 b2Log(" bd.type = b2BodyType(%d);\n", m_type);
493 b2Log(" bd.position.Set(%.15lef, %.15lef);\n", m_xf.p.x, m_xf.p.y);
494 b2Log(" bd.angle = %.15lef;\n", m_sweep.a);
495 b2Log(" bd.linearVelocity.Set(%.15lef, %.15lef);\n", m_linearVelocity.x, m_linearVelocity.y);
496 b2Log(" bd.angularVelocity = %.15lef;\n", m_angularVelocity);
497 b2Log(" bd.linearDamping = %.15lef;\n", m_linearDamping);
498 b2Log(" bd.angularDamping = %.15lef;\n", m_angularDamping);
499 b2Log(" bd.allowSleep = bool(%d);\n", m_flags & e_autoSleepFlag);
500 b2Log(" bd.awake = bool(%d);\n", m_flags & e_awakeFlag);
501 b2Log(" bd.fixedRotation = bool(%d);\n", m_flags & e_fixedRotationFlag);
502 b2Log(" bd.bullet = bool(%d);\n", m_flags & e_bulletFlag);
503 b2Log(" bd.active = bool(%d);\n", m_flags & e_activeFlag);
504 b2Log(" bd.gravityScale = %.15lef;\n", m_gravityScale);
505 b2Log(" bodies[%d] = m_world->CreateBody(&bd);\n", m_islandIndex);
506 b2Log("\n");
507 for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
508 {
509 b2Log(" {\n");
510 f->Dump(bodyIndex);
511 b2Log(" }\n");
512 }
513 b2Log("}\n");
514 }