Warning, file /education/gcompris/external/qml-box2d/Box2D/Dynamics/b2Body.cpp was not indexed or was modified since last indexation (in which case cross-reference links may be missing, inaccurate or erroneous).
0001 /* 0002 * Copyright (c) 2006-2007 Erin Catto http://www.box2d.org 0003 * 0004 * This software is provided 'as-is', without any express or implied 0005 * warranty. In no event will the authors be held liable for any damages 0006 * arising from the use of this software. 0007 * Permission is granted to anyone to use this software for any purpose, 0008 * including commercial applications, and to alter it and redistribute it 0009 * freely, subject to the following restrictions: 0010 * 1. The origin of this software must not be misrepresented; you must not 0011 * claim that you wrote the original software. If you use this software 0012 * in a product, an acknowledgment in the product documentation would be 0013 * appreciated but is not required. 0014 * 2. Altered source versions must be plainly marked as such, and must not be 0015 * misrepresented as being the original software. 0016 * 3. This notice may not be removed or altered from any source distribution. 0017 */ 0018 0019 #include <Box2D/Dynamics/b2Body.h> 0020 #include <Box2D/Dynamics/b2Fixture.h> 0021 #include <Box2D/Dynamics/b2World.h> 0022 #include <Box2D/Dynamics/Contacts/b2Contact.h> 0023 #include <Box2D/Dynamics/Joints/b2Joint.h> 0024 0025 b2Body::b2Body(const b2BodyDef* bd, b2World* world) 0026 { 0027 b2Assert(bd->position.IsValid()); 0028 b2Assert(bd->linearVelocity.IsValid()); 0029 b2Assert(b2IsValid(bd->angle)); 0030 b2Assert(b2IsValid(bd->angularVelocity)); 0031 b2Assert(b2IsValid(bd->angularDamping) && bd->angularDamping >= 0.0f); 0032 b2Assert(b2IsValid(bd->linearDamping) && bd->linearDamping >= 0.0f); 0033 0034 m_flags = 0; 0035 0036 if (bd->bullet) 0037 { 0038 m_flags |= e_bulletFlag; 0039 } 0040 if (bd->fixedRotation) 0041 { 0042 m_flags |= e_fixedRotationFlag; 0043 } 0044 if (bd->allowSleep) 0045 { 0046 m_flags |= e_autoSleepFlag; 0047 } 0048 if (bd->awake) 0049 { 0050 m_flags |= e_awakeFlag; 0051 } 0052 if (bd->active) 0053 { 0054 m_flags |= e_activeFlag; 0055 } 0056 0057 m_world = world; 0058 0059 m_xf.p = bd->position; 0060 m_xf.q.Set(bd->angle); 0061 0062 m_sweep.localCenter.SetZero(); 0063 m_sweep.c0 = m_xf.p; 0064 m_sweep.c = m_xf.p; 0065 m_sweep.a0 = bd->angle; 0066 m_sweep.a = bd->angle; 0067 m_sweep.alpha0 = 0.0f; 0068 0069 m_jointList = NULL; 0070 m_contactList = NULL; 0071 m_prev = NULL; 0072 m_next = NULL; 0073 0074 m_linearVelocity = bd->linearVelocity; 0075 m_angularVelocity = bd->angularVelocity; 0076 0077 m_linearDamping = bd->linearDamping; 0078 m_angularDamping = bd->angularDamping; 0079 m_gravityScale = bd->gravityScale; 0080 0081 m_force.SetZero(); 0082 m_torque = 0.0f; 0083 0084 m_sleepTime = 0.0f; 0085 0086 m_type = bd->type; 0087 0088 if (m_type == b2_dynamicBody) 0089 { 0090 m_mass = 1.0f; 0091 m_invMass = 1.0f; 0092 } 0093 else 0094 { 0095 m_mass = 0.0f; 0096 m_invMass = 0.0f; 0097 } 0098 0099 m_I = 0.0f; 0100 m_invI = 0.0f; 0101 0102 m_userData = bd->userData; 0103 0104 m_fixtureList = NULL; 0105 m_fixtureCount = 0; 0106 } 0107 0108 b2Body::~b2Body() 0109 { 0110 // shapes and joints are destroyed in b2World::Destroy 0111 } 0112 0113 void b2Body::SetType(b2BodyType type) 0114 { 0115 b2Assert(m_world->IsLocked() == false); 0116 if (m_world->IsLocked() == true) 0117 { 0118 return; 0119 } 0120 0121 if (m_type == type) 0122 { 0123 return; 0124 } 0125 0126 m_type = type; 0127 0128 ResetMassData(); 0129 0130 if (m_type == b2_staticBody) 0131 { 0132 m_linearVelocity.SetZero(); 0133 m_angularVelocity = 0.0f; 0134 m_sweep.a0 = m_sweep.a; 0135 m_sweep.c0 = m_sweep.c; 0136 SynchronizeFixtures(); 0137 } 0138 0139 SetAwake(true); 0140 0141 m_force.SetZero(); 0142 m_torque = 0.0f; 0143 0144 // Delete the attached contacts. 0145 b2ContactEdge* ce = m_contactList; 0146 while (ce) 0147 { 0148 b2ContactEdge* ce0 = ce; 0149 ce = ce->next; 0150 m_world->m_contactManager.Destroy(ce0->contact); 0151 } 0152 m_contactList = NULL; 0153 0154 // Touch the proxies so that new contacts will be created (when appropriate) 0155 b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; 0156 for (b2Fixture* f = m_fixtureList; f; f = f->m_next) 0157 { 0158 int32 proxyCount = f->m_proxyCount; 0159 for (int32 i = 0; i < proxyCount; ++i) 0160 { 0161 broadPhase->TouchProxy(f->m_proxies[i].proxyId); 0162 } 0163 } 0164 } 0165 0166 b2Fixture* b2Body::CreateFixture(const b2FixtureDef* def) 0167 { 0168 b2Assert(m_world->IsLocked() == false); 0169 if (m_world->IsLocked() == true) 0170 { 0171 return NULL; 0172 } 0173 0174 b2BlockAllocator* allocator = &m_world->m_blockAllocator; 0175 0176 void* memory = allocator->Allocate(sizeof(b2Fixture)); 0177 b2Fixture* fixture = new (memory) b2Fixture; 0178 fixture->Create(allocator, this, def); 0179 0180 if (m_flags & e_activeFlag) 0181 { 0182 b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; 0183 fixture->CreateProxies(broadPhase, m_xf); 0184 } 0185 0186 fixture->m_next = m_fixtureList; 0187 m_fixtureList = fixture; 0188 ++m_fixtureCount; 0189 0190 fixture->m_body = this; 0191 0192 // Adjust mass properties if needed. 0193 if (fixture->m_density > 0.0f) 0194 { 0195 ResetMassData(); 0196 } 0197 0198 // Let the world know we have a new fixture. This will cause new contacts 0199 // to be created at the beginning of the next time step. 0200 m_world->m_flags |= b2World::e_newFixture; 0201 0202 return fixture; 0203 } 0204 0205 b2Fixture* b2Body::CreateFixture(const b2Shape* shape, float32 density) 0206 { 0207 b2FixtureDef def; 0208 def.shape = shape; 0209 def.density = density; 0210 0211 return CreateFixture(&def); 0212 } 0213 0214 void b2Body::DestroyFixture(b2Fixture* fixture) 0215 { 0216 b2Assert(m_world->IsLocked() == false); 0217 if (m_world->IsLocked() == true) 0218 { 0219 return; 0220 } 0221 0222 b2Assert(fixture->m_body == this); 0223 0224 // Remove the fixture from this body's singly linked list. 0225 b2Assert(m_fixtureCount > 0); 0226 b2Fixture** node = &m_fixtureList; 0227 bool found = false; 0228 while (*node != NULL) 0229 { 0230 if (*node == fixture) 0231 { 0232 *node = fixture->m_next; 0233 found = true; 0234 break; 0235 } 0236 0237 node = &(*node)->m_next; 0238 } 0239 0240 // You tried to remove a shape that is not attached to this body. 0241 b2Assert(found); 0242 0243 // Destroy any contacts associated with the fixture. 0244 b2ContactEdge* edge = m_contactList; 0245 while (edge) 0246 { 0247 b2Contact* c = edge->contact; 0248 edge = edge->next; 0249 0250 b2Fixture* fixtureA = c->GetFixtureA(); 0251 b2Fixture* fixtureB = c->GetFixtureB(); 0252 0253 if (fixture == fixtureA || fixture == fixtureB) 0254 { 0255 // This destroys the contact and removes it from 0256 // this body's contact list. 0257 m_world->m_contactManager.Destroy(c); 0258 } 0259 } 0260 0261 b2BlockAllocator* allocator = &m_world->m_blockAllocator; 0262 0263 if (m_flags & e_activeFlag) 0264 { 0265 b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; 0266 fixture->DestroyProxies(broadPhase); 0267 } 0268 0269 fixture->Destroy(allocator); 0270 fixture->m_body = NULL; 0271 fixture->m_next = NULL; 0272 fixture->~b2Fixture(); 0273 allocator->Free(fixture, sizeof(b2Fixture)); 0274 0275 --m_fixtureCount; 0276 0277 // Reset the mass data. 0278 ResetMassData(); 0279 } 0280 0281 void b2Body::ResetMassData() 0282 { 0283 // Compute mass data from shapes. Each shape has its own density. 0284 m_mass = 0.0f; 0285 m_invMass = 0.0f; 0286 m_I = 0.0f; 0287 m_invI = 0.0f; 0288 m_sweep.localCenter.SetZero(); 0289 0290 // Static and kinematic bodies have zero mass. 0291 if (m_type == b2_staticBody || m_type == b2_kinematicBody) 0292 { 0293 m_sweep.c0 = m_xf.p; 0294 m_sweep.c = m_xf.p; 0295 m_sweep.a0 = m_sweep.a; 0296 return; 0297 } 0298 0299 b2Assert(m_type == b2_dynamicBody); 0300 0301 // Accumulate mass over all fixtures. 0302 b2Vec2 localCenter = b2Vec2_zero; 0303 for (b2Fixture* f = m_fixtureList; f; f = f->m_next) 0304 { 0305 if (f->m_density == 0.0f) 0306 { 0307 continue; 0308 } 0309 0310 b2MassData massData; 0311 f->GetMassData(&massData); 0312 m_mass += massData.mass; 0313 localCenter += massData.mass * massData.center; 0314 m_I += massData.I; 0315 } 0316 0317 // Compute center of mass. 0318 if (m_mass > 0.0f) 0319 { 0320 m_invMass = 1.0f / m_mass; 0321 localCenter *= m_invMass; 0322 } 0323 else 0324 { 0325 // Force all dynamic bodies to have a positive mass. 0326 m_mass = 1.0f; 0327 m_invMass = 1.0f; 0328 } 0329 0330 if (m_I > 0.0f && (m_flags & e_fixedRotationFlag) == 0) 0331 { 0332 // Center the inertia about the center of mass. 0333 m_I -= m_mass * b2Dot(localCenter, localCenter); 0334 b2Assert(m_I > 0.0f); 0335 m_invI = 1.0f / m_I; 0336 0337 } 0338 else 0339 { 0340 m_I = 0.0f; 0341 m_invI = 0.0f; 0342 } 0343 0344 // Move center of mass. 0345 b2Vec2 oldCenter = m_sweep.c; 0346 m_sweep.localCenter = localCenter; 0347 m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter); 0348 0349 // Update center of mass velocity. 0350 m_linearVelocity += b2Cross(m_angularVelocity, m_sweep.c - oldCenter); 0351 } 0352 0353 void b2Body::SetMassData(const b2MassData* massData) 0354 { 0355 b2Assert(m_world->IsLocked() == false); 0356 if (m_world->IsLocked() == true) 0357 { 0358 return; 0359 } 0360 0361 if (m_type != b2_dynamicBody) 0362 { 0363 return; 0364 } 0365 0366 m_invMass = 0.0f; 0367 m_I = 0.0f; 0368 m_invI = 0.0f; 0369 0370 m_mass = massData->mass; 0371 if (m_mass <= 0.0f) 0372 { 0373 m_mass = 1.0f; 0374 } 0375 0376 m_invMass = 1.0f / m_mass; 0377 0378 if (massData->I > 0.0f && (m_flags & b2Body::e_fixedRotationFlag) == 0) 0379 { 0380 m_I = massData->I - m_mass * b2Dot(massData->center, massData->center); 0381 b2Assert(m_I > 0.0f); 0382 m_invI = 1.0f / m_I; 0383 } 0384 0385 // Move center of mass. 0386 b2Vec2 oldCenter = m_sweep.c; 0387 m_sweep.localCenter = massData->center; 0388 m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter); 0389 0390 // Update center of mass velocity. 0391 m_linearVelocity += b2Cross(m_angularVelocity, m_sweep.c - oldCenter); 0392 } 0393 0394 bool b2Body::ShouldCollide(const b2Body* other) const 0395 { 0396 // At least one body should be dynamic. 0397 if (m_type != b2_dynamicBody && other->m_type != b2_dynamicBody) 0398 { 0399 return false; 0400 } 0401 0402 // Does a joint prevent collision? 0403 for (b2JointEdge* jn = m_jointList; jn; jn = jn->next) 0404 { 0405 if (jn->other == other) 0406 { 0407 if (jn->joint->m_collideConnected == false) 0408 { 0409 return false; 0410 } 0411 } 0412 } 0413 0414 return true; 0415 } 0416 0417 void b2Body::SetTransform(const b2Vec2& position, float32 angle) 0418 { 0419 b2Assert(m_world->IsLocked() == false); 0420 if (m_world->IsLocked() == true) 0421 { 0422 return; 0423 } 0424 0425 m_xf.q.Set(angle); 0426 m_xf.p = position; 0427 0428 m_sweep.c = b2Mul(m_xf, m_sweep.localCenter); 0429 m_sweep.a = angle; 0430 0431 m_sweep.c0 = m_sweep.c; 0432 m_sweep.a0 = angle; 0433 0434 b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; 0435 for (b2Fixture* f = m_fixtureList; f; f = f->m_next) 0436 { 0437 f->Synchronize(broadPhase, m_xf, m_xf); 0438 } 0439 } 0440 0441 void b2Body::SynchronizeFixtures() 0442 { 0443 b2Transform xf1; 0444 xf1.q.Set(m_sweep.a0); 0445 xf1.p = m_sweep.c0 - b2Mul(xf1.q, m_sweep.localCenter); 0446 0447 b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; 0448 for (b2Fixture* f = m_fixtureList; f; f = f->m_next) 0449 { 0450 f->Synchronize(broadPhase, xf1, m_xf); 0451 } 0452 } 0453 0454 void b2Body::SetActive(bool flag) 0455 { 0456 b2Assert(m_world->IsLocked() == false); 0457 0458 if (flag == IsActive()) 0459 { 0460 return; 0461 } 0462 0463 if (flag) 0464 { 0465 m_flags |= e_activeFlag; 0466 0467 // Create all proxies. 0468 b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; 0469 for (b2Fixture* f = m_fixtureList; f; f = f->m_next) 0470 { 0471 f->CreateProxies(broadPhase, m_xf); 0472 } 0473 0474 // Contacts are created the next time step. 0475 } 0476 else 0477 { 0478 m_flags &= ~e_activeFlag; 0479 0480 // Destroy all proxies. 0481 b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase; 0482 for (b2Fixture* f = m_fixtureList; f; f = f->m_next) 0483 { 0484 f->DestroyProxies(broadPhase); 0485 } 0486 0487 // Destroy the attached contacts. 0488 b2ContactEdge* ce = m_contactList; 0489 while (ce) 0490 { 0491 b2ContactEdge* ce0 = ce; 0492 ce = ce->next; 0493 m_world->m_contactManager.Destroy(ce0->contact); 0494 } 0495 m_contactList = NULL; 0496 } 0497 } 0498 0499 void b2Body::SetFixedRotation(bool flag) 0500 { 0501 bool status = (m_flags & e_fixedRotationFlag) == e_fixedRotationFlag; 0502 if (status == flag) 0503 { 0504 return; 0505 } 0506 0507 if (flag) 0508 { 0509 m_flags |= e_fixedRotationFlag; 0510 } 0511 else 0512 { 0513 m_flags &= ~e_fixedRotationFlag; 0514 } 0515 0516 m_angularVelocity = 0.0f; 0517 0518 ResetMassData(); 0519 } 0520 0521 void b2Body::Dump() 0522 { 0523 int32 bodyIndex = m_islandIndex; 0524 0525 b2Log("{\n"); 0526 b2Log(" b2BodyDef bd;\n"); 0527 b2Log(" bd.type = b2BodyType(%d);\n", m_type); 0528 b2Log(" bd.position.Set(%.15lef, %.15lef);\n", m_xf.p.x, m_xf.p.y); 0529 b2Log(" bd.angle = %.15lef;\n", m_sweep.a); 0530 b2Log(" bd.linearVelocity.Set(%.15lef, %.15lef);\n", m_linearVelocity.x, m_linearVelocity.y); 0531 b2Log(" bd.angularVelocity = %.15lef;\n", m_angularVelocity); 0532 b2Log(" bd.linearDamping = %.15lef;\n", m_linearDamping); 0533 b2Log(" bd.angularDamping = %.15lef;\n", m_angularDamping); 0534 b2Log(" bd.allowSleep = bool(%d);\n", m_flags & e_autoSleepFlag); 0535 b2Log(" bd.awake = bool(%d);\n", m_flags & e_awakeFlag); 0536 b2Log(" bd.fixedRotation = bool(%d);\n", m_flags & e_fixedRotationFlag); 0537 b2Log(" bd.bullet = bool(%d);\n", m_flags & e_bulletFlag); 0538 b2Log(" bd.active = bool(%d);\n", m_flags & e_activeFlag); 0539 b2Log(" bd.gravityScale = %.15lef;\n", m_gravityScale); 0540 b2Log(" bodies[%d] = m_world->CreateBody(&bd);\n", m_islandIndex); 0541 b2Log("\n"); 0542 for (b2Fixture* f = m_fixtureList; f; f = f->m_next) 0543 { 0544 b2Log(" {\n"); 0545 f->Dump(bodyIndex); 0546 b2Log(" }\n"); 0547 } 0548 b2Log("}\n"); 0549 }