b2Body.cpp
Go to the documentation of this file.
00001 /*
00002 * Copyright (c) 2006-2007 Erin Catto http://www.box2d.org
00003 *
00004 * This software is provided 'as-is', without any express or implied
00005 * warranty.  In no event will the authors be held liable for any damages
00006 * arising from the use of this software.
00007 * Permission is granted to anyone to use this software for any purpose,
00008 * including commercial applications, and to alter it and redistribute it
00009 * freely, subject to the following restrictions:
00010 * 1. The origin of this software must not be misrepresented; you must not
00011 * claim that you wrote the original software. If you use this software
00012 * in a product, an acknowledgment in the product documentation would be
00013 * appreciated but is not required.
00014 * 2. Altered source versions must be plainly marked as such, and must not be
00015 * misrepresented as being the original software.
00016 * 3. This notice may not be removed or altered from any source distribution.
00017 */
00018 
00019 #include <Box2D/Dynamics/b2Body.h>
00020 #include <Box2D/Dynamics/b2Fixture.h>
00021 #include <Box2D/Dynamics/b2World.h>
00022 #include <Box2D/Dynamics/Contacts/b2Contact.h>
00023 #include <Box2D/Dynamics/Joints/b2Joint.h>
00024 
00025 b2Body::b2Body(const b2BodyDef* bd, b2World* world)
00026 {
00027         b2Assert(bd->position.IsValid());
00028         b2Assert(bd->linearVelocity.IsValid());
00029         b2Assert(b2IsValid(bd->angle));
00030         b2Assert(b2IsValid(bd->angularVelocity));
00031         b2Assert(b2IsValid(bd->angularDamping) && bd->angularDamping >= 0.0f);
00032         b2Assert(b2IsValid(bd->linearDamping) && bd->linearDamping >= 0.0f);
00033 
00034         m_flags = 0;
00035 
00036         if (bd->bullet)
00037         {
00038                 m_flags |= e_bulletFlag;
00039         }
00040         if (bd->fixedRotation)
00041         {
00042                 m_flags |= e_fixedRotationFlag;
00043         }
00044         if (bd->allowSleep)
00045         {
00046                 m_flags |= e_autoSleepFlag;
00047         }
00048         if (bd->awake)
00049         {
00050                 m_flags |= e_awakeFlag;
00051         }
00052         if (bd->active)
00053         {
00054                 m_flags |= e_activeFlag;
00055         }
00056 
00057         m_world = world;
00058 
00059         m_xf.p = bd->position;
00060         m_xf.q.Set(bd->angle);
00061 
00062         m_sweep.localCenter.SetZero();
00063         m_sweep.c0 = m_xf.p;
00064         m_sweep.c = m_xf.p;
00065         m_sweep.a0 = bd->angle;
00066         m_sweep.a = bd->angle;
00067         m_sweep.alpha0 = 0.0f;
00068 
00069         m_jointList = NULL;
00070         m_contactList = NULL;
00071         m_prev = NULL;
00072         m_next = NULL;
00073 
00074         m_linearVelocity = bd->linearVelocity;
00075         m_angularVelocity = bd->angularVelocity;
00076 
00077         m_linearDamping = bd->linearDamping;
00078         m_angularDamping = bd->angularDamping;
00079         m_gravityScale = bd->gravityScale;
00080 
00081         m_force.SetZero();
00082         m_torque = 0.0f;
00083 
00084         m_sleepTime = 0.0f;
00085 
00086         m_type = bd->type;
00087 
00088         if (m_type == b2_dynamicBody)
00089         {
00090                 m_mass = 1.0f;
00091                 m_invMass = 1.0f;
00092         }
00093         else
00094         {
00095                 m_mass = 0.0f;
00096                 m_invMass = 0.0f;
00097         }
00098 
00099         m_I = 0.0f;
00100         m_invI = 0.0f;
00101 
00102         m_userData = bd->userData;
00103 
00104         m_fixtureList = NULL;
00105         m_fixtureCount = 0;
00106 }
00107 
00108 b2Body::~b2Body()
00109 {
00110         // shapes and joints are destroyed in b2World::Destroy
00111 }
00112 
00113 void b2Body::SetType(b2BodyType type)
00114 {
00115         b2Assert(m_world->IsLocked() == false);
00116         if (m_world->IsLocked() == true)
00117         {
00118                 return;
00119         }
00120 
00121         if (m_type == type)
00122         {
00123                 return;
00124         }
00125 
00126         m_type = type;
00127 
00128         ResetMassData();
00129 
00130         if (m_type == b2_staticBody)
00131         {
00132                 m_linearVelocity.SetZero();
00133                 m_angularVelocity = 0.0f;
00134                 m_sweep.a0 = m_sweep.a;
00135                 m_sweep.c0 = m_sweep.c;
00136                 SynchronizeFixtures();
00137         }
00138 
00139         SetAwake(true);
00140 
00141         m_force.SetZero();
00142         m_torque = 0.0f;
00143 
00144         // Delete the attached contacts.
00145         b2ContactEdge* ce = m_contactList;
00146         while (ce)
00147         {
00148                 b2ContactEdge* ce0 = ce;
00149                 ce = ce->next;
00150                 m_world->m_contactManager.Destroy(ce0->contact);
00151         }
00152         m_contactList = NULL;
00153 
00154         // Touch the proxies so that new contacts will be created (when appropriate)
00155         b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
00156         for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
00157         {
00158                 int32 proxyCount = f->m_proxyCount;
00159                 for (int32 i = 0; i < proxyCount; ++i)
00160                 {
00161                         broadPhase->TouchProxy(f->m_proxies[i].proxyId);
00162                 }
00163         }
00164 }
00165 
00166 b2Fixture* b2Body::CreateFixture(const b2FixtureDef* def)
00167 {
00168         b2Assert(m_world->IsLocked() == false);
00169         if (m_world->IsLocked() == true)
00170         {
00171                 return NULL;
00172         }
00173 
00174         b2BlockAllocator* allocator = &m_world->m_blockAllocator;
00175 
00176         void* memory = allocator->Allocate(sizeof(b2Fixture));
00177         b2Fixture* fixture = new (memory) b2Fixture;
00178         fixture->Create(allocator, this, def);
00179 
00180         if (m_flags & e_activeFlag)
00181         {
00182                 b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
00183                 fixture->CreateProxies(broadPhase, m_xf);
00184         }
00185 
00186         fixture->m_next = m_fixtureList;
00187         m_fixtureList = fixture;
00188         ++m_fixtureCount;
00189 
00190         fixture->m_body = this;
00191 
00192         // Adjust mass properties if needed.
00193         if (fixture->m_density > 0.0f)
00194         {
00195                 ResetMassData();
00196         }
00197 
00198         // Let the world know we have a new fixture. This will cause new contacts
00199         // to be created at the beginning of the next time step.
00200         m_world->m_flags |= b2World::e_newFixture;
00201 
00202         return fixture;
00203 }
00204 
00205 b2Fixture* b2Body::CreateFixture(const b2Shape* shape, float32 density)
00206 {
00207         b2FixtureDef def;
00208         def.shape = shape;
00209         def.density = density;
00210 
00211         return CreateFixture(&def);
00212 }
00213 
00214 void b2Body::DestroyFixture(b2Fixture* fixture)
00215 {
00216         b2Assert(m_world->IsLocked() == false);
00217         if (m_world->IsLocked() == true)
00218         {
00219                 return;
00220         }
00221 
00222         b2Assert(fixture->m_body == this);
00223 
00224         // Remove the fixture from this body's singly linked list.
00225         b2Assert(m_fixtureCount > 0);
00226         b2Fixture** node = &m_fixtureList;
00227         bool found = false;
00228         while (*node != NULL)
00229         {
00230                 if (*node == fixture)
00231                 {
00232                         *node = fixture->m_next;
00233                         found = true;
00234                         break;
00235                 }
00236 
00237                 node = &(*node)->m_next;
00238         }
00239 
00240         // You tried to remove a shape that is not attached to this body.
00241         b2Assert(found);
00242 
00243         // Destroy any contacts associated with the fixture.
00244         b2ContactEdge* edge = m_contactList;
00245         while (edge)
00246         {
00247                 b2Contact* c = edge->contact;
00248                 edge = edge->next;
00249 
00250                 b2Fixture* fixtureA = c->GetFixtureA();
00251                 b2Fixture* fixtureB = c->GetFixtureB();
00252 
00253                 if (fixture == fixtureA || fixture == fixtureB)
00254                 {
00255                         // This destroys the contact and removes it from
00256                         // this body's contact list.
00257                         m_world->m_contactManager.Destroy(c);
00258                 }
00259         }
00260 
00261         b2BlockAllocator* allocator = &m_world->m_blockAllocator;
00262 
00263         if (m_flags & e_activeFlag)
00264         {
00265                 b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
00266                 fixture->DestroyProxies(broadPhase);
00267         }
00268 
00269         fixture->Destroy(allocator);
00270         fixture->m_body = NULL;
00271         fixture->m_next = NULL;
00272         fixture->~b2Fixture();
00273         allocator->Free(fixture, sizeof(b2Fixture));
00274 
00275         --m_fixtureCount;
00276 
00277         // Reset the mass data.
00278         ResetMassData();
00279 }
00280 
00281 void b2Body::ResetMassData()
00282 {
00283         // Compute mass data from shapes. Each shape has its own density.
00284         m_mass = 0.0f;
00285         m_invMass = 0.0f;
00286         m_I = 0.0f;
00287         m_invI = 0.0f;
00288         m_sweep.localCenter.SetZero();
00289 
00290         // Static and kinematic bodies have zero mass.
00291         if (m_type == b2_staticBody || m_type == b2_kinematicBody)
00292         {
00293                 m_sweep.c0 = m_xf.p;
00294                 m_sweep.c = m_xf.p;
00295                 m_sweep.a0 = m_sweep.a;
00296                 return;
00297         }
00298 
00299         b2Assert(m_type == b2_dynamicBody);
00300 
00301         // Accumulate mass over all fixtures.
00302         b2Vec2 localCenter = b2Vec2_zero;
00303         for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
00304         {
00305                 if (f->m_density == 0.0f)
00306                 {
00307                         continue;
00308                 }
00309 
00310                 b2MassData massData;
00311                 f->GetMassData(&massData);
00312                 m_mass += massData.mass;
00313                 localCenter += massData.mass * massData.center;
00314                 m_I += massData.I;
00315         }
00316 
00317         // Compute center of mass.
00318         if (m_mass > 0.0f)
00319         {
00320                 m_invMass = 1.0f / m_mass;
00321                 localCenter *= m_invMass;
00322         }
00323         else
00324         {
00325                 // Force all dynamic bodies to have a positive mass.
00326                 m_mass = 1.0f;
00327                 m_invMass = 1.0f;
00328         }
00329 
00330         if (m_I > 0.0f && (m_flags & e_fixedRotationFlag) == 0)
00331         {
00332                 // Center the inertia about the center of mass.
00333                 m_I -= m_mass * b2Dot(localCenter, localCenter);
00334                 b2Assert(m_I > 0.0f);
00335                 m_invI = 1.0f / m_I;
00336 
00337         }
00338         else
00339         {
00340                 m_I = 0.0f;
00341                 m_invI = 0.0f;
00342         }
00343 
00344         // Move center of mass.
00345         b2Vec2 oldCenter = m_sweep.c;
00346         m_sweep.localCenter = localCenter;
00347         m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
00348 
00349         // Update center of mass velocity.
00350         m_linearVelocity += b2Cross(m_angularVelocity, m_sweep.c - oldCenter);
00351 }
00352 
00353 void b2Body::SetMassData(const b2MassData* massData)
00354 {
00355         b2Assert(m_world->IsLocked() == false);
00356         if (m_world->IsLocked() == true)
00357         {
00358                 return;
00359         }
00360 
00361         if (m_type != b2_dynamicBody)
00362         {
00363                 return;
00364         }
00365 
00366         m_invMass = 0.0f;
00367         m_I = 0.0f;
00368         m_invI = 0.0f;
00369 
00370         m_mass = massData->mass;
00371         if (m_mass <= 0.0f)
00372         {
00373                 m_mass = 1.0f;
00374         }
00375 
00376         m_invMass = 1.0f / m_mass;
00377 
00378         if (massData->I > 0.0f && (m_flags & b2Body::e_fixedRotationFlag) == 0)
00379         {
00380                 m_I = massData->I - m_mass * b2Dot(massData->center, massData->center);
00381                 b2Assert(m_I > 0.0f);
00382                 m_invI = 1.0f / m_I;
00383         }
00384 
00385         // Move center of mass.
00386         b2Vec2 oldCenter = m_sweep.c;
00387         m_sweep.localCenter =  massData->center;
00388         m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
00389 
00390         // Update center of mass velocity.
00391         m_linearVelocity += b2Cross(m_angularVelocity, m_sweep.c - oldCenter);
00392 }
00393 
00394 bool b2Body::ShouldCollide(const b2Body* other) const
00395 {
00396         // At least one body should be dynamic.
00397         if (m_type != b2_dynamicBody && other->m_type != b2_dynamicBody)
00398         {
00399                 return false;
00400         }
00401 
00402         // Does a joint prevent collision?
00403         for (b2JointEdge* jn = m_jointList; jn; jn = jn->next)
00404         {
00405                 if (jn->other == other)
00406                 {
00407                         if (jn->joint->m_collideConnected == false)
00408                         {
00409                                 return false;
00410                         }
00411                 }
00412         }
00413 
00414         return true;
00415 }
00416 
00417 void b2Body::SetTransform(const b2Vec2& position, float32 angle)
00418 {
00419         b2Assert(m_world->IsLocked() == false);
00420         if (m_world->IsLocked() == true)
00421         {
00422                 return;
00423         }
00424 
00425         m_xf.q.Set(angle);
00426         m_xf.p = position;
00427 
00428         m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
00429         m_sweep.a = angle;
00430 
00431         m_sweep.c0 = m_sweep.c;
00432         m_sweep.a0 = angle;
00433 
00434         b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
00435         for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
00436         {
00437                 f->Synchronize(broadPhase, m_xf, m_xf);
00438         }
00439 }
00440 
00441 void b2Body::SynchronizeFixtures()
00442 {
00443         b2Transform xf1;
00444         xf1.q.Set(m_sweep.a0);
00445         xf1.p = m_sweep.c0 - b2Mul(xf1.q, m_sweep.localCenter);
00446 
00447         b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
00448         for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
00449         {
00450                 f->Synchronize(broadPhase, xf1, m_xf);
00451         }
00452 }
00453 
00454 void b2Body::SetActive(bool flag)
00455 {
00456         b2Assert(m_world->IsLocked() == false);
00457 
00458         if (flag == IsActive())
00459         {
00460                 return;
00461         }
00462 
00463         if (flag)
00464         {
00465                 m_flags |= e_activeFlag;
00466 
00467                 // Create all proxies.
00468                 b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
00469                 for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
00470                 {
00471                         f->CreateProxies(broadPhase, m_xf);
00472                 }
00473 
00474                 // Contacts are created the next time step.
00475         }
00476         else
00477         {
00478                 m_flags &= ~e_activeFlag;
00479 
00480                 // Destroy all proxies.
00481                 b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
00482                 for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
00483                 {
00484                         f->DestroyProxies(broadPhase);
00485                 }
00486 
00487                 // Destroy the attached contacts.
00488                 b2ContactEdge* ce = m_contactList;
00489                 while (ce)
00490                 {
00491                         b2ContactEdge* ce0 = ce;
00492                         ce = ce->next;
00493                         m_world->m_contactManager.Destroy(ce0->contact);
00494                 }
00495                 m_contactList = NULL;
00496         }
00497 }
00498 
00499 void b2Body::SetFixedRotation(bool flag)
00500 {
00501         bool status = (m_flags & e_fixedRotationFlag) == e_fixedRotationFlag;
00502         if (status == flag)
00503         {
00504                 return;
00505         }
00506 
00507         if (flag)
00508         {
00509                 m_flags |= e_fixedRotationFlag;
00510         }
00511         else
00512         {
00513                 m_flags &= ~e_fixedRotationFlag;
00514         }
00515 
00516         m_angularVelocity = 0.0f;
00517 
00518         ResetMassData();
00519 }
00520 
00521 void b2Body::Dump()
00522 {
00523         int32 bodyIndex = m_islandIndex;
00524 
00525         b2Log("{\n");
00526         b2Log("  b2BodyDef bd;\n");
00527         b2Log("  bd.type = b2BodyType(%d);\n", m_type);
00528         b2Log("  bd.position.Set(%.15lef, %.15lef);\n", m_xf.p.x, m_xf.p.y);
00529         b2Log("  bd.angle = %.15lef;\n", m_sweep.a);
00530         b2Log("  bd.linearVelocity.Set(%.15lef, %.15lef);\n", m_linearVelocity.x, m_linearVelocity.y);
00531         b2Log("  bd.angularVelocity = %.15lef;\n", m_angularVelocity);
00532         b2Log("  bd.linearDamping = %.15lef;\n", m_linearDamping);
00533         b2Log("  bd.angularDamping = %.15lef;\n", m_angularDamping);
00534         b2Log("  bd.allowSleep = bool(%d);\n", m_flags & e_autoSleepFlag);
00535         b2Log("  bd.awake = bool(%d);\n", m_flags & e_awakeFlag);
00536         b2Log("  bd.fixedRotation = bool(%d);\n", m_flags & e_fixedRotationFlag);
00537         b2Log("  bd.bullet = bool(%d);\n", m_flags & e_bulletFlag);
00538         b2Log("  bd.active = bool(%d);\n", m_flags & e_activeFlag);
00539         b2Log("  bd.gravityScale = %.15lef;\n", m_gravityScale);
00540         b2Log("  bodies[%d] = m_world->CreateBody(&bd);\n", m_islandIndex);
00541         b2Log("\n");
00542         for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
00543         {
00544                 b2Log("  {\n");
00545                 f->Dump(bodyIndex);
00546                 b2Log("  }\n");
00547         }
00548         b2Log("}\n");
00549 }


mvsim
Author(s):
autogenerated on Thu Sep 7 2017 09:27:47