text: /*
  * 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 "b2Body.h"
  #include "b2World.h"
  #include "Joints/b2Joint.h"
  #include "../Collision/Shapes/b2Shape.h"
  
  b2Body::b2Body(const b2BodyDef* bd, b2World* world)
  {
  	b2Assert(world->m_lock == false);
  
  	m_flags = 0;
  
  	if (bd->isBullet)
  	{
  		m_flags |= e_bulletFlag;
  	}
  	if (bd->fixedRotation)
  	{
  		m_flags |= e_fixedRotationFlag;
  	}
  	if (bd->allowSleep)
  	{
  		m_flags |= e_allowSleepFlag;
  	}
  	if (bd->isSleeping)
  	{
  		m_flags |= e_sleepFlag;
  	}
  
  	m_world = world;
  
  	m_xf.position = bd->position;
  	m_xf.R.Set(bd->angle);
  
  	m_sweep.localCenter = bd->massData.center;
  	m_sweep.t0 = 1.0f;
  	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_linearDamping = bd->linearDamping;
  	m_angularDamping = bd->angularDamping;
  
  	m_force.Set(0.0f, 0.0f);
  	m_torque = 0.0f;
  
  	m_linearVelocity.SetZero();
  	m_angularVelocity = 0.0f;
  
  	m_sleepTime = 0.0f;
  
  	m_invMass = 0.0f;
  	m_I = 0.0f;
  	m_invI = 0.0f;
  
  	m_mass = bd->massData.mass;
  
  	if (m_mass > 0.0f)
  	{
  		m_invMass = 1.0f / m_mass;
  	}
  
  	if ((m_flags & b2Body::e_fixedRotationFlag) == 0)
  	{
  		m_I = bd->massData.I;
  	}
  	
  	if (m_I > 0.0f)
  	{
  		m_invI = 1.0f / m_I;
  	}
  
  	if (m_invMass == 0.0f && m_invI == 0.0f)
  	{
  		m_type = e_staticType;
  	}
  	else
  	{
  		m_type = e_dynamicType;
  	}
  
  	m_userData = bd->userData;
  
  	m_shapeList = NULL;
  	m_shapeCount = 0;
  }
  
  b2Body::~b2Body()
  {
  	b2Assert(m_world->m_lock == false);
  	// shapes and joints are destroyed in b2World::Destroy
  }
  
  b2Shape* b2Body::CreateShape(b2ShapeDef* def)
  {
  	b2Assert(m_world->m_lock == false);
  	if (m_world->m_lock == true)
  	{
  		return NULL;
  	}
  
  	b2Shape* s = b2Shape::Create(def, &m_world->m_blockAllocator);
  
  	s->m_next = m_shapeList;
  	m_shapeList = s;
  	++m_shapeCount;
  
  	s->m_body = this;
  
  	// Add the shape to the world's broad-phase.
  	s->CreateProxy(m_world->m_broadPhase, m_xf);
  
  	// Compute the sweep radius for CCD.
  	s->UpdateSweepRadius(m_sweep.localCenter);
  
  	return s;
  }
  
  void b2Body::DestroyShape(b2Shape* s)
  {
  	b2Assert(m_world->m_lock == false);
  	if (m_world->m_lock == true)
  	{
  		return;
  	}
  
  	b2Assert(s->GetBody() == this);
  	s->DestroyProxy(m_world->m_broadPhase);
  
  	b2Assert(m_shapeCount > 0);
  	b2Shape** node = &m_shapeList;
  	bool found = false;
  	while (*node != NULL)
  	{
  		if (*node == s)
  		{
  			*node = s->m_next;
  			found = true;
  			break;
  		}
  
  		node = &(*node)->m_next;
  	}
  
  	// You tried to remove a shape that is not attached to this body.
  	b2Assert(found);
  
  	s->m_body = NULL;
  	s->m_next = NULL;
  
  	--m_shapeCount;
  
  	b2Shape::Destroy(s, &m_world->m_blockAllocator);
  }
  
  // TODO_ERIN adjust linear velocity and torque to account for movement of center.
  void b2Body::SetMass(const b2MassData* massData)
  {
  	b2Assert(m_world->m_lock == false);
  	if (m_world->m_lock == true)
  	{
  		return;
  	}
  
  	m_invMass = 0.0f;
  	m_I = 0.0f;
  	m_invI = 0.0f;
  
  	m_mass = massData->mass;
  
  	if (m_mass > 0.0f)
  	{
  		m_invMass = 1.0f / m_mass;
  	}
  
  	if ((m_flags & b2Body::e_fixedRotationFlag) == 0)
  	{
  		m_I = massData->I;
  	}
  
  	if (m_I > 0.0f)
  	{
  		m_invI = 1.0f / m_I;
  	}
  
  	// Move center of mass.
  	m_sweep.localCenter = massData->center;
  	m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
  
  	// Update the sweep radii of all child shapes.
  	for (b2Shape* s = m_shapeList; s; s = s->m_next)
  	{
  		s->UpdateSweepRadius(m_sweep.localCenter);
  	}
  
  	int16 oldType = m_type;
  	if (m_invMass == 0.0f && m_invI == 0.0f)
  	{
  		m_type = e_staticType;
  	}
  	else
  	{
  		m_type = e_dynamicType;
  	}
  
  	// If the body type changed, we need to refilter the broad-phase proxies.
  	if (oldType != m_type)
  	{
  		for (b2Shape* s = m_shapeList; s; s = s->m_next)
  		{
  			s->RefilterProxy(m_world->m_broadPhase, m_xf);
  		}
  	}
  }
  
  // TODO_ERIN adjust linear velocity and torque to account for movement of center.
  void b2Body::SetMassFromShapes()
  {
  	b2Assert(m_world->m_lock == false);
  	if (m_world->m_lock == true)
  	{
  		return;
  	}
  
  	// 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;
  
  	b2Vec2 center = b2Vec2_zero;
  	for (b2Shape* s = m_shapeList; s; s = s->m_next)
  	{
  		b2MassData massData;
  		s->ComputeMass(&massData);
  		m_mass += massData.mass;
  		center += massData.mass * massData.center;
  		m_I += massData.I;
  	}
  
  	// Compute center of mass, and shift the origin to the COM.
  	if (m_mass > 0.0f)
  	{
  		m_invMass = 1.0f / m_mass;
  		center *= m_invMass;
  	}
  
  	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.
  	m_sweep.localCenter = center;
  	m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
  
  	// Update the sweep radii of all child shapes.
  	for (b2Shape* s = m_shapeList; s; s = s->m_next)
  	{
  		s->UpdateSweepRadius(m_sweep.localCenter);
  	}
  
  	int16 oldType = m_type;
  	if (m_invMass == 0.0f && m_invI == 0.0f)
  	{
  		m_type = e_staticType;
  	}
  	else
  	{
  		m_type = e_dynamicType;
  	}
  
  	// If the body type changed, we need to refilter the broad-phase proxies.
  	if (oldType != m_type)
  	{
  		for (b2Shape* s = m_shapeList; s; s = s->m_next)
  		{
  			s->RefilterProxy(m_world->m_broadPhase, m_xf);
  		}
  	}
  }
  
  bool b2Body::SetXForm(const b2Vec2& position, float32 angle)
  {
  	b2Assert(m_world->m_lock == false);
  	if (m_world->m_lock == true)
  	{
  		return true;
  	}
  
  	if (IsFrozen())
  	{
  		return false;
  	}
  
  	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;
  
  	bool freeze = false;
  	for (b2Shape* s = m_shapeList; s; s = s->m_next)
  	{
  		bool inRange = s->Synchronize(m_world->m_broadPhase, m_xf, m_xf);
  
  		if (inRange == false)
  		{
  			freeze = true;
  			break;
  		}
  	}
  
  	if (freeze == true)
  	{
  		m_flags |= e_frozenFlag;
  		m_linearVelocity.SetZero();
  		m_angularVelocity = 0.0f;
  		for (b2Shape* s = m_shapeList; s; s = s->m_next)
  		{
  			s->DestroyProxy(m_world->m_broadPhase);
  		}
  
  		// Failure
  		return false;
  	}
  
  	// Success
  	m_world->m_broadPhase->Commit();
  	return true;
  }
  
  bool b2Body::SynchronizeShapes()
  {
  	b2XForm xf1;
  	xf1.R.Set(m_sweep.a0);
  	xf1.position = m_sweep.c0 - b2Mul(xf1.R, m_sweep.localCenter);
  
  	bool inRange = true;
  	for (b2Shape* s = m_shapeList; s; s = s->m_next)
  	{
  		inRange = s->Synchronize(m_world->m_broadPhase, xf1, m_xf);
  		if (inRange == false)
  		{
  			break;
  		}
  	}
  
  	if (inRange == false)
  	{
  		m_flags |= e_frozenFlag;
  		m_linearVelocity.SetZero();
  		m_angularVelocity = 0.0f;
  		for (b2Shape* s = m_shapeList; s; s = s->m_next)
  		{
  			s->DestroyProxy(m_world->m_broadPhase);
  		}
  
  		// Failure
  		return false;
  	}
  
  	// Success
  	return true;
  }
  