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
25b2Body::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
108b2Body::~b2Body()
109{
110 // shapes and joints are destroyed in b2World::Destroy
111}
112
113void 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
166b2Fixture* 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
205b2Fixture* 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
214void b2Body::DestroyFixture(b2Fixture* fixture)
215{
216 b2Assert(m_world->IsLocked() == false);
217 if (m_world->IsLocked() == true)
218 {
219 return;
220 }
221
222 b2Assert(fixture->m_body == this);
223
224 // Remove the fixture from this body's singly linked list.
225 b2Assert(m_fixtureCount > 0);
226 b2Fixture** node = &m_fixtureList;
227 bool found = false;
228 while (*node != NULL)
229 {
230 if (*node == fixture)
231 {
232 *node = fixture->m_next;
233 found = true;
234 break;
235 }
236
237 node = &(*node)->m_next;
238 }
239
240 // You tried to remove a shape that is not attached to this body.
241 b2Assert(found);
242
243 // Destroy any contacts associated with the fixture.
244 b2ContactEdge* edge = m_contactList;
245 while (edge)
246 {
247 b2Contact* c = edge->contact;
248 edge = edge->next;
249
250 b2Fixture* fixtureA = c->GetFixtureA();
251 b2Fixture* fixtureB = c->GetFixtureB();
252
253 if (fixture == fixtureA || fixture == fixtureB)
254 {
255 // This destroys the contact and removes it from
256 // this body's contact list.
257 m_world->m_contactManager.Destroy(c);
258 }
259 }
260
261 b2BlockAllocator* allocator = &m_world->m_blockAllocator;
262
263 if (m_flags & e_activeFlag)
264 {
265 b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
266 fixture->DestroyProxies(broadPhase);
267 }
268
269 fixture->Destroy(allocator);
270 fixture->m_body = NULL;
271 fixture->m_next = NULL;
272 fixture->~b2Fixture();
273 allocator->Free(fixture, sizeof(b2Fixture));
274
275 --m_fixtureCount;
276
277 // Reset the mass data.
278 ResetMassData();
279}
280
281void b2Body::ResetMassData()
282{
283 // Compute mass data from shapes. Each shape has its own density.
284 m_mass = 0.0f;
285 m_invMass = 0.0f;
286 m_I = 0.0f;
287 m_invI = 0.0f;
288 m_sweep.localCenter.SetZero();
289
290 // Static and kinematic bodies have zero mass.
291 if (m_type == b2_staticBody || m_type == b2_kinematicBody)
292 {
293 m_sweep.c0 = m_xf.p;
294 m_sweep.c = m_xf.p;
295 m_sweep.a0 = m_sweep.a;
296 return;
297 }
298
299 b2Assert(m_type == b2_dynamicBody);
300
301 // Accumulate mass over all fixtures.
302 b2Vec2 localCenter = b2Vec2_zero;
303 for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
304 {
305 if (f->m_density == 0.0f)
306 {
307 continue;
308 }
309
310 b2MassData massData;
311 f->GetMassData(&massData);
312 m_mass += massData.mass;
313 localCenter += massData.mass * massData.center;
314 m_I += massData.I;
315 }
316
317 // Compute center of mass.
318 if (m_mass > 0.0f)
319 {
320 m_invMass = 1.0f / m_mass;
321 localCenter *= m_invMass;
322 }
323 else
324 {
325 // Force all dynamic bodies to have a positive mass.
326 m_mass = 1.0f;
327 m_invMass = 1.0f;
328 }
329
330 if (m_I > 0.0f && (m_flags & e_fixedRotationFlag) == 0)
331 {
332 // Center the inertia about the center of mass.
333 m_I -= m_mass * b2Dot(localCenter, localCenter);
334 b2Assert(m_I > 0.0f);
335 m_invI = 1.0f / m_I;
336
337 }
338 else
339 {
340 m_I = 0.0f;
341 m_invI = 0.0f;
342 }
343
344 // Move center of mass.
345 b2Vec2 oldCenter = m_sweep.c;
346 m_sweep.localCenter = localCenter;
347 m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
348
349 // Update center of mass velocity.
350 m_linearVelocity += b2Cross(m_angularVelocity, m_sweep.c - oldCenter);
351}
352
353void b2Body::SetMassData(const b2MassData* massData)
354{
355 b2Assert(m_world->IsLocked() == false);
356 if (m_world->IsLocked() == true)
357 {
358 return;
359 }
360
361 if (m_type != b2_dynamicBody)
362 {
363 return;
364 }
365
366 m_invMass = 0.0f;
367 m_I = 0.0f;
368 m_invI = 0.0f;
369
370 m_mass = massData->mass;
371 if (m_mass <= 0.0f)
372 {
373 m_mass = 1.0f;
374 }
375
376 m_invMass = 1.0f / m_mass;
377
378 if (massData->I > 0.0f && (m_flags & b2Body::e_fixedRotationFlag) == 0)
379 {
380 m_I = massData->I - m_mass * b2Dot(massData->center, massData->center);
381 b2Assert(m_I > 0.0f);
382 m_invI = 1.0f / m_I;
383 }
384
385 // Move center of mass.
386 b2Vec2 oldCenter = m_sweep.c;
387 m_sweep.localCenter = massData->center;
388 m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
389
390 // Update center of mass velocity.
391 m_linearVelocity += b2Cross(m_angularVelocity, m_sweep.c - oldCenter);
392}
393
394bool b2Body::ShouldCollide(const b2Body* other) const
395{
396 // At least one body should be dynamic.
397 if (m_type != b2_dynamicBody && other->m_type != b2_dynamicBody)
398 {
399 return false;
400 }
401
402 // Does a joint prevent collision?
403 for (b2JointEdge* jn = m_jointList; jn; jn = jn->next)
404 {
405 if (jn->other == other)
406 {
407 if (jn->joint->m_collideConnected == false)
408 {
409 return false;
410 }
411 }
412 }
413
414 return true;
415}
416
417void b2Body::SetTransform(const b2Vec2& position, float32 angle)
418{
419 b2Assert(m_world->IsLocked() == false);
420 if (m_world->IsLocked() == true)
421 {
422 return;
423 }
424
425 m_xf.q.Set(angle);
426 m_xf.p = position;
427
428 m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
429 m_sweep.a = angle;
430
431 m_sweep.c0 = m_sweep.c;
432 m_sweep.a0 = angle;
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, m_xf, m_xf);
438 }
439}
440
441void b2Body::SynchronizeFixtures()
442{
443 b2Transform xf1;
444 xf1.q.Set(m_sweep.a0);
445 xf1.p = m_sweep.c0 - b2Mul(xf1.q, m_sweep.localCenter);
446
447 b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
448 for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
449 {
450 f->Synchronize(broadPhase, xf1, m_xf);
451 }
452}
453
454void b2Body::SetActive(bool flag)
455{
456 b2Assert(m_world->IsLocked() == false);
457
458 if (flag == IsActive())
459 {
460 return;
461 }
462
463 if (flag)
464 {
465 m_flags |= e_activeFlag;
466
467 // Create 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->CreateProxies(broadPhase, m_xf);
472 }
473
474 // Contacts are created the next time step.
475 }
476 else
477 {
478 m_flags &= ~e_activeFlag;
479
480 // Destroy all proxies.
481 b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
482 for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
483 {
484 f->DestroyProxies(broadPhase);
485 }
486
487 // Destroy the attached contacts.
488 b2ContactEdge* ce = m_contactList;
489 while (ce)
490 {
491 b2ContactEdge* ce0 = ce;
492 ce = ce->next;
493 m_world->m_contactManager.Destroy(ce0->contact);
494 }
495 m_contactList = NULL;
496 }
497}
498
499void b2Body::SetFixedRotation(bool flag)
500{
501 bool status = (m_flags & e_fixedRotationFlag) == e_fixedRotationFlag;
502 if (status == flag)
503 {
504 return;
505 }
506
507 if (flag)
508 {
509 m_flags |= e_fixedRotationFlag;
510 }
511 else
512 {
513 m_flags &= ~e_fixedRotationFlag;
514 }
515
516 m_angularVelocity = 0.0f;
517
518 ResetMassData();
519}
520
521void b2Body::Dump()
522{
523 int32 bodyIndex = m_islandIndex;
524
525 b2Log("{\n");
526 b2Log(" b2BodyDef bd;\n");
527 b2Log(" bd.type = b2BodyType(%d);\n", m_type);
528 b2Log(" bd.position.Set(%.15lef, %.15lef);\n", m_xf.p.x, m_xf.p.y);
529 b2Log(" bd.angle = %.15lef;\n", m_sweep.a);
530 b2Log(" bd.linearVelocity.Set(%.15lef, %.15lef);\n", m_linearVelocity.x, m_linearVelocity.y);
531 b2Log(" bd.angularVelocity = %.15lef;\n", m_angularVelocity);
532 b2Log(" bd.linearDamping = %.15lef;\n", m_linearDamping);
533 b2Log(" bd.angularDamping = %.15lef;\n", m_angularDamping);
534 b2Log(" bd.allowSleep = bool(%d);\n", m_flags & e_autoSleepFlag);
535 b2Log(" bd.awake = bool(%d);\n", m_flags & e_awakeFlag);
536 b2Log(" bd.fixedRotation = bool(%d);\n", m_flags & e_fixedRotationFlag);
537 b2Log(" bd.bullet = bool(%d);\n", m_flags & e_bulletFlag);
538 b2Log(" bd.active = bool(%d);\n", m_flags & e_activeFlag);
539 b2Log(" bd.gravityScale = %.15lef;\n", m_gravityScale);
540 b2Log(" bodies[%d] = m_world->CreateBody(&bd);\n", m_islandIndex);
541 b2Log("\n");
542 for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
543 {
544 b2Log(" {\n");
545 f->Dump(bodyIndex);
546 b2Log(" }\n");
547 }
548 b2Log("}\n");
549}
550