File indexing completed on 2025-08-03 03:50:00

 /*
 * Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
 *
 * This software is provided 'as-is', without any express or implied
 * warranty.  In no event will the authors be held liable for any damages
 * arising from the use of this software.
 * Permission is granted to anyone to use this software for any purpose,
 * including commercial applications, and to alter it and redistribute it
 * freely, subject to the following restrictions:
 * 1. The origin of this software must not be misrepresented; you must not
 * claim that you wrote the original software. If you use this software
 * in a product, an acknowledgment in the product documentation would be
 * appreciated but is not required.
 * 2. Altered source versions must be plainly marked as such, and must not be
 * misrepresented as being the original software.
 * 3. This notice may not be removed or altered from any source distribution.
 */
 
 #include <Box2D/Dynamics/b2Body.h>
 #include <Box2D/Dynamics/b2Fixture.h>
 #include <Box2D/Dynamics/b2World.h>
 #include <Box2D/Dynamics/Contacts/b2Contact.h>
 #include <Box2D/Dynamics/Joints/b2Joint.h>
 
 b2Body::b2Body(const b2BodyDef* bd, b2World* world)
 {
     b2Assert(bd->position.IsValid());
     b2Assert(bd->linearVelocity.IsValid());
     b2Assert(b2IsValid(bd->angle));
     b2Assert(b2IsValid(bd->angularVelocity));
     b2Assert(b2IsValid(bd->inertiaScale) && bd->inertiaScale >= 0.0f);
     b2Assert(b2IsValid(bd->angularDamping) && bd->angularDamping >= 0.0f);
     b2Assert(b2IsValid(bd->linearDamping) && bd->linearDamping >= 0.0f);
 
     m_flags = 0;
 
     if (bd->bullet)
     {
         m_flags |= e_bulletFlag;
     }
     if (bd->fixedRotation)
     {
         m_flags |= e_fixedRotationFlag;
     }
     if (bd->allowSleep)
     {
         m_flags |= e_autoSleepFlag;
     }
     if (bd->awake)
     {
         m_flags |= e_awakeFlag;
     }
     if (bd->active)
     {
         m_flags |= e_activeFlag;
     }
 
     m_world = world;
 
     m_xf.position = bd->position;
     m_xf.R.Set(bd->angle);
 
     m_sweep.localCenter.SetZero();
     m_sweep.a0 = m_sweep.a = bd->angle;
     m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
 
     m_jointList = NULL;
     m_contactList = NULL;
     m_prev = NULL;
     m_next = NULL;
 
     m_linearVelocity = bd->linearVelocity;
     m_angularVelocity = bd->angularVelocity;
 
     m_linearDamping = bd->linearDamping;
     m_angularDamping = bd->angularDamping;
 
     m_force.SetZero();
     m_torque = 0.0f;
 
     m_sleepTime = 0.0f;
 
     m_type = bd->type;
 
     if (m_type == b2_dynamicBody)
     {
         m_mass = 1.0f;
         m_invMass = 1.0f;
     }
     else
     {
         m_mass = 0.0f;
         m_invMass = 0.0f;
     }
 
     m_I = 0.0f;
     m_invI = 0.0f;
 
     m_userData = bd->userData;
 
     m_fixtureList = NULL;
     m_fixtureCount = 0;
 }
 
 b2Body::~b2Body()
 {
     // shapes and joints are destroyed in b2World::Destroy
 }
 
 void b2Body::SetType(b2BodyType type)
 {
     if (m_type == type)
     {
         return;
     }
 
     m_type = type;
 
     ResetMassData();
 
     if (m_type == b2_staticBody)
     {
         m_linearVelocity.SetZero();
         m_angularVelocity = 0.0f;
     }
 
     SetAwake(true);
 
     m_force.SetZero();
     m_torque = 0.0f;
 
     // Since the body type changed, we need to flag contacts for filtering.
     for (b2ContactEdge* ce = m_contactList; ce; ce = ce->next)
     {
         ce->contact->FlagForFiltering();
     }
 }
 
 b2Fixture* b2Body::CreateFixture(const b2FixtureDef* def)
 {
     b2Assert(m_world->IsLocked() == false);
     if (m_world->IsLocked() == true)
     {
         return NULL;
     }
 
     b2BlockAllocator* allocator = &m_world->m_blockAllocator;
 
     void* memory = allocator->Allocate(sizeof(b2Fixture));
     b2Fixture* fixture = new (memory) b2Fixture;
     fixture->Create(allocator, this, def);
 
     if (m_flags & e_activeFlag)
     {
         b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
         fixture->CreateProxies(broadPhase, m_xf);
     }
 
     fixture->m_next = m_fixtureList;
     m_fixtureList = fixture;
     ++m_fixtureCount;
 
     fixture->m_body = this;
 
     // Adjust mass properties if needed.
     if (fixture->m_density > 0.0f)
     {
         ResetMassData();
     }
 
     // Let the world know we have a new fixture. This will cause new contacts
     // to be created at the beginning of the next time step.
     m_world->m_flags |= b2World::e_newFixture;
 
     return fixture;
 }
 
 b2Fixture* b2Body::CreateFixture(const b2Shape* shape, qreal density)
 {
     b2FixtureDef def;
     def.shape = shape;
     def.density = density;
 
     return CreateFixture(&def);
 }
 
 void b2Body::DestroyFixture(b2Fixture* fixture)
 {
     b2Assert(m_world->IsLocked() == false);
     if (m_world->IsLocked() == true)
     {
         return;
     }
 
     b2Assert(fixture->m_body == this);
 
     // Remove the fixture from this body's singly linked list.
     b2Assert(m_fixtureCount > 0);
     b2Fixture** node = &m_fixtureList;
     bool found = false;
     while (*node != NULL)
     {
         if (*node == fixture)
         {
             *node = fixture->m_next;
             found = true;
             break;
         }
 
         node = &(*node)->m_next;
     }
 
     // You tried to remove a shape that is not attached to this body.
     b2Assert(found);
 
     // Destroy any contacts associated with the fixture.
     b2ContactEdge* edge = m_contactList;
     while (edge)
     {
         b2Contact* c = edge->contact;
         edge = edge->next;
 
         b2Fixture* fixtureA = c->GetFixtureA();
         b2Fixture* fixtureB = c->GetFixtureB();
 
         if (fixture == fixtureA || fixture == fixtureB)
         {
             // This destroys the contact and removes it from
             // this body's contact list.
             m_world->m_contactManager.Destroy(c);
         }
     }
 
     b2BlockAllocator* allocator = &m_world->m_blockAllocator;
 
     if (m_flags & e_activeFlag)
     {
         b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
         fixture->DestroyProxies(broadPhase);
     }
 
     fixture->Destroy(allocator);
     fixture->m_body = NULL;
     fixture->m_next = NULL;
     fixture->~b2Fixture();
     allocator->Free(fixture, sizeof(b2Fixture));
 
     --m_fixtureCount;
 
     // Reset the mass data.
     ResetMassData();
 }
 
 void b2Body::ResetMassData()
 {
     // Compute mass data from shapes. Each shape has its own density.
     m_mass = 0.0f;
     m_invMass = 0.0f;
     m_I = 0.0f;
     m_invI = 0.0f;
     m_sweep.localCenter.SetZero();
 
     // Static and kinematic bodies have zero mass.
     if (m_type == b2_staticBody || m_type == b2_kinematicBody)
     {
         m_sweep.c0 = m_sweep.c = m_xf.position;
         return;
     }
 
     b2Assert(m_type == b2_dynamicBody);
 
     // Accumulate mass over all fixtures.
     b2Vec2 center = b2Vec2_zero;
     for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
     {
         if (f->m_density == 0.0f)
         {
             continue;
         }
 
         b2MassData massData;
         f->GetMassData(&massData);
         m_mass += massData.mass;
         center += massData.mass * massData.center;
         m_I += massData.I;
     }
 
     // Compute center of mass.
     if (m_mass > 0.0f)
     {
         m_invMass = 1.0f / m_mass;
         center *= m_invMass;
     }
     else
     {
         // Force all dynamic bodies to have a positive mass.
         m_mass = 1.0f;
         m_invMass = 1.0f;
     }
 
     if (m_I > 0.0f && (m_flags & e_fixedRotationFlag) == 0)
     {
         // Center the inertia about the center of mass.
         m_I -= m_mass * b2Dot(center, center);
         b2Assert(m_I > 0.0f);
         m_invI = 1.0f / m_I;
 
     }
     else
     {
         m_I = 0.0f;
         m_invI = 0.0f;
     }
 
     // Move center of mass.
     b2Vec2 oldCenter = m_sweep.c;
     m_sweep.localCenter = center;
     m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
 
     // Update center of mass velocity.
     m_linearVelocity += b2Cross(m_angularVelocity, m_sweep.c - oldCenter);
 }
 
 void b2Body::SetMassData(const b2MassData* massData)
 {
     b2Assert(m_world->IsLocked() == false);
     if (m_world->IsLocked() == true)
     {
         return;
     }
 
     if (m_type != b2_dynamicBody)
     {
         return;
     }
 
     m_invMass = 0.0f;
     m_I = 0.0f;
     m_invI = 0.0f;
 
     m_mass = massData->mass;
     if (m_mass <= 0.0f)
     {
         m_mass = 1.0f;
     }
 
     m_invMass = 1.0f / m_mass;
 
     if (massData->I > 0.0f && (m_flags & b2Body::e_fixedRotationFlag) == 0)
     {
         m_I = massData->I - m_mass * b2Dot(massData->center, massData->center);
         b2Assert(m_I > 0.0f);
         m_invI = 1.0f / m_I;
     }
 
     // Move center of mass.
     b2Vec2 oldCenter = m_sweep.c;
     m_sweep.localCenter = massData->center;
     m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
 
     // Update center of mass velocity.
     m_linearVelocity += b2Cross(m_angularVelocity, m_sweep.c - oldCenter);
 }
 
 bool b2Body::ShouldCollide(const b2Body* other) const
 {
     // At least one body should be dynamic.
     if (m_type != b2_dynamicBody && other->m_type != b2_dynamicBody)
     {
         return false;
     }
 
     // Does a joint prevent collision?
     for (b2JointEdge* jn = m_jointList; jn; jn = jn->next)
     {
         if (jn->other == other)
         {
             if (jn->joint->m_collideConnected == false)
             {
                 return false;
             }
         }
     }
 
     return true;
 }
 
 void b2Body::SetTransform(const b2Vec2& position, qreal angle)
 {
     b2Assert(m_world->IsLocked() == false);
     if (m_world->IsLocked() == true)
     {
         return;
     }
 
     m_xf.R.Set(angle);
     m_xf.position = position;
 
     m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
     m_sweep.a0 = m_sweep.a = angle;
 
     b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
     for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
     {
         f->Synchronize(broadPhase, m_xf, m_xf);
     }
 
     m_world->m_contactManager.FindNewContacts();
 }
 
 void b2Body::SynchronizeFixtures()
 {
     b2Transform xf1;
     xf1.R.Set(m_sweep.a0);
     xf1.position = m_sweep.c0 - b2Mul(xf1.R, m_sweep.localCenter);
 
     b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
     for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
     {
         f->Synchronize(broadPhase, xf1, m_xf);
     }
 }
 
 void b2Body::SetActive(bool flag)
 {
     b2Assert(m_world->IsLocked() == false);
 
     if (flag == IsActive())
     {
         return;
     }
 
     if (flag)
     {
         m_flags |= e_activeFlag;
 
         // Create all proxies.
         b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
         for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
         {
             f->CreateProxies(broadPhase, m_xf);
         }
 
         // Contacts are created the next time step.
     }
     else
     {
         m_flags &= ~e_activeFlag;
 
         // Destroy all proxies.
         b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
         for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
         {
             f->DestroyProxies(broadPhase);
         }
 
         // Destroy the attached contacts.
         b2ContactEdge* ce = m_contactList;
         while (ce)
         {
             b2ContactEdge* ce0 = ce;
             ce = ce->next;
             m_world->m_contactManager.Destroy(ce0->contact);
         }
         m_contactList = NULL;
     }
 }