git.maemo.org Git - blok/blob - Box2D/Source/Dynamics/b2Body.cpp

   1 /*
   2 * Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
   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 "b2Body.h"
  20 #include "b2World.h"
  21 #include "Joints/b2Joint.h"
  22 #include "../Collision/Shapes/b2Shape.h"
  23
  24 b2Body::b2Body(const b2BodyDef* bd, b2World* world)
  25 {
  26         b2Assert(world->m_lock == false);
  27
  28         m_flags = 0;
  29
  30         if (bd->isBullet)
  31         {
  32                 m_flags |= e_bulletFlag;
  33         }
  34         if (bd->fixedRotation)
  35         {
  36                 m_flags |= e_fixedRotationFlag;
  37         }
  38         if (bd->allowSleep)
  39         {
  40                 m_flags |= e_allowSleepFlag;
  41         }
  42         if (bd->isSleeping)
  43         {
  44                 m_flags |= e_sleepFlag;
  45         }
  46
  47         m_world = world;
  48
  49         m_xf.position = bd->position;
  50         m_xf.R.Set(bd->angle);
  51
  52         m_sweep.localCenter = bd->massData.center;
  53         m_sweep.t0 = 1.0f;
  54         m_sweep.a0 = m_sweep.a = bd->angle;
  55         m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
  56
  57         m_jointList = NULL;
  58         m_contactList = NULL;
  59         m_prev = NULL;
  60         m_next = NULL;
  61
  62         m_linearDamping = bd->linearDamping;
  63         m_angularDamping = bd->angularDamping;
  64
  65         m_force.Set(0.0f, 0.0f);
  66         m_torque = 0.0f;
  67
  68         m_linearVelocity.SetZero();
  69         m_angularVelocity = 0.0f;
  70
  71         m_sleepTime = 0.0f;
  72
  73         m_invMass = 0.0f;
  74         m_I = 0.0f;
  75         m_invI = 0.0f;
  76
  77         m_mass = bd->massData.mass;
  78
  79         if (m_mass > 0.0f)
  80         {
  81                 m_invMass = 1.0f / m_mass;
  82         }
  83
  84         if ((m_flags & b2Body::e_fixedRotationFlag) == 0)
  85         {
  86                 m_I = bd->massData.I;
  87         }
  88
  89         if (m_I > 0.0f)
  90         {
  91                 m_invI = 1.0f / m_I;
  92         }
  93
  94         if (m_invMass == 0.0f && m_invI == 0.0f)
  95         {
  96                 m_type = e_staticType;
  97         }
  98         else
  99         {
 100                 m_type = e_dynamicType;
 101         }
 102
 103         m_userData = bd->userData;
 104
 105         m_shapeList = NULL;
 106         m_shapeCount = 0;
 107 }
 108
 109 b2Body::~b2Body()
 110 {
 111         b2Assert(m_world->m_lock == false);
 112         // shapes and joints are destroyed in b2World::Destroy
 113 }
 114
 115 b2Shape* b2Body::CreateShape(b2ShapeDef* def)
 116 {
 117         b2Assert(m_world->m_lock == false);
 118         if (m_world->m_lock == true)
 119         {
 120                 return NULL;
 121         }
 122
 123         b2Shape* s = b2Shape::Create(def, &m_world->m_blockAllocator);
 124
 125         s->m_next = m_shapeList;
 126         m_shapeList = s;
 127         ++m_shapeCount;
 128
 129         s->m_body = this;
 130
 131         // Add the shape to the world's broad-phase.
 132         s->CreateProxy(m_world->m_broadPhase, m_xf);
 133
 134         // Compute the sweep radius for CCD.
 135         s->UpdateSweepRadius(m_sweep.localCenter);
 136
 137         return s;
 138 }
 139
 140 void b2Body::DestroyShape(b2Shape* s)
 141 {
 142         b2Assert(m_world->m_lock == false);
 143         if (m_world->m_lock == true)
 144         {
 145                 return;
 146         }
 147
 148         b2Assert(s->GetBody() == this);
 149         s->DestroyProxy(m_world->m_broadPhase);
 150
 151         b2Assert(m_shapeCount > 0);
 152         b2Shape** node = &m_shapeList;
 153         bool found = false;
 154         while (*node != NULL)
 155         {
 156                 if (*node == s)
 157                 {
 158                         *node = s->m_next;
 159                         found = true;
 160                         break;
 161                 }
 162
 163                 node = &(*node)->m_next;
 164         }
 165
 166         // You tried to remove a shape that is not attached to this body.
 167         b2Assert(found);
 168
 169         s->m_body = NULL;
 170         s->m_next = NULL;
 171
 172         --m_shapeCount;
 173
 174         b2Shape::Destroy(s, &m_world->m_blockAllocator);
 175 }
 176
 177 // TODO_ERIN adjust linear velocity and torque to account for movement of center.
 178 void b2Body::SetMass(const b2MassData* massData)
 179 {
 180         b2Assert(m_world->m_lock == false);
 181         if (m_world->m_lock == true)
 182         {
 183                 return;
 184         }
 185
 186         m_invMass = 0.0f;
 187         m_I = 0.0f;
 188         m_invI = 0.0f;
 189
 190         m_mass = massData->mass;
 191
 192         if (m_mass > 0.0f)
 193         {
 194                 m_invMass = 1.0f / m_mass;
 195         }
 196
 197         if ((m_flags & b2Body::e_fixedRotationFlag) == 0)
 198         {
 199                 m_I = massData->I;
 200         }
 201
 202         if (m_I > 0.0f)
 203         {
 204                 m_invI = 1.0f / m_I;
 205         }
 206
 207         // Move center of mass.
 208         m_sweep.localCenter = massData->center;
 209         m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
 210
 211         // Update the sweep radii of all child shapes.
 212         for (b2Shape* s = m_shapeList; s; s = s->m_next)
 213         {
 214                 s->UpdateSweepRadius(m_sweep.localCenter);
 215         }
 216
 217         int16 oldType = m_type;
 218         if (m_invMass == 0.0f && m_invI == 0.0f)
 219         {
 220                 m_type = e_staticType;
 221         }
 222         else
 223         {
 224                 m_type = e_dynamicType;
 225         }
 226
 227         // If the body type changed, we need to refilter the broad-phase proxies.
 228         if (oldType != m_type)
 229         {
 230                 for (b2Shape* s = m_shapeList; s; s = s->m_next)
 231                 {
 232                         s->RefilterProxy(m_world->m_broadPhase, m_xf);
 233                 }
 234         }
 235 }
 236
 237 // TODO_ERIN adjust linear velocity and torque to account for movement of center.
 238 void b2Body::SetMassFromShapes()
 239 {
 240         b2Assert(m_world->m_lock == false);
 241         if (m_world->m_lock == true)
 242         {
 243                 return;
 244         }
 245
 246         // Compute mass data from shapes. Each shape has its own density.
 247         m_mass = 0.0f;
 248         m_invMass = 0.0f;
 249         m_I = 0.0f;
 250         m_invI = 0.0f;
 251
 252         b2Vec2 center = b2Vec2_zero;
 253         for (b2Shape* s = m_shapeList; s; s = s->m_next)
 254         {
 255                 b2MassData massData;
 256                 s->ComputeMass(&massData);
 257                 m_mass += massData.mass;
 258                 center += massData.mass * massData.center;
 259                 m_I += massData.I;
 260         }
 261
 262         // Compute center of mass, and shift the origin to the COM.
 263         if (m_mass > 0.0f)
 264         {
 265                 m_invMass = 1.0f / m_mass;
 266                 center *= m_invMass;
 267         }
 268
 269         if (m_I > 0.0f && (m_flags & e_fixedRotationFlag) == 0)
 270         {
 271                 // Center the inertia about the center of mass.
 272                 m_I -= m_mass * b2Dot(center, center);
 273                 b2Assert(m_I > 0.0f);
 274                 m_invI = 1.0f / m_I;
 275         }
 276         else
 277         {
 278                 m_I = 0.0f;
 279                 m_invI = 0.0f;
 280         }
 281
 282         // Move center of mass.
 283         m_sweep.localCenter = center;
 284         m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
 285
 286         // Update the sweep radii of all child shapes.
 287         for (b2Shape* s = m_shapeList; s; s = s->m_next)
 288         {
 289                 s->UpdateSweepRadius(m_sweep.localCenter);
 290         }
 291
 292         int16 oldType = m_type;
 293         if (m_invMass == 0.0f && m_invI == 0.0f)
 294         {
 295                 m_type = e_staticType;
 296         }
 297         else
 298         {
 299                 m_type = e_dynamicType;
 300         }
 301
 302         // If the body type changed, we need to refilter the broad-phase proxies.
 303         if (oldType != m_type)
 304         {
 305                 for (b2Shape* s = m_shapeList; s; s = s->m_next)
 306                 {
 307                         s->RefilterProxy(m_world->m_broadPhase, m_xf);
 308                 }
 309         }
 310 }
 311
 312 bool b2Body::SetXForm(const b2Vec2& position, float32 angle)
 313 {
 314         b2Assert(m_world->m_lock == false);
 315         if (m_world->m_lock == true)
 316         {
 317                 return true;
 318         }
 319
 320         if (IsFrozen())
 321         {
 322                 return false;
 323         }
 324
 325         m_xf.R.Set(angle);
 326         m_xf.position = position;
 327
 328         m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
 329         m_sweep.a0 = m_sweep.a = angle;
 330
 331         bool freeze = false;
 332         for (b2Shape* s = m_shapeList; s; s = s->m_next)
 333         {
 334                 bool inRange = s->Synchronize(m_world->m_broadPhase, m_xf, m_xf);
 335
 336                 if (inRange == false)
 337                 {
 338                         freeze = true;
 339                         break;
 340                 }
 341         }
 342
 343         if (freeze == true)
 344         {
 345                 m_flags |= e_frozenFlag;
 346                 m_linearVelocity.SetZero();
 347                 m_angularVelocity = 0.0f;
 348                 for (b2Shape* s = m_shapeList; s; s = s->m_next)
 349                 {
 350                         s->DestroyProxy(m_world->m_broadPhase);
 351                 }
 352
 353                 // Failure
 354                 return false;
 355         }
 356
 357         // Success
 358         m_world->m_broadPhase->Commit();
 359         return true;
 360 }
 361
 362 bool b2Body::SynchronizeShapes()
 363 {
 364         b2XForm xf1;
 365         xf1.R.Set(m_sweep.a0);
 366         xf1.position = m_sweep.c0 - b2Mul(xf1.R, m_sweep.localCenter);
 367
 368         bool inRange = true;
 369         for (b2Shape* s = m_shapeList; s; s = s->m_next)
 370         {
 371                 inRange = s->Synchronize(m_world->m_broadPhase, xf1, m_xf);
 372                 if (inRange == false)
 373                 {
 374                         break;
 375                 }
 376         }
 377
 378         if (inRange == false)
 379         {
 380                 m_flags |= e_frozenFlag;
 381                 m_linearVelocity.SetZero();
 382                 m_angularVelocity = 0.0f;
 383                 for (b2Shape* s = m_shapeList; s; s = s->m_next)
 384                 {
 385                         s->DestroyProxy(m_world->m_broadPhase);
 386                 }
 387
 388                 // Failure
 389                 return false;
 390         }
 391
 392         // Success
 393         return true;
 394 }