git.maemo.org Git - blok/blob - Box2D/Source/Dynamics/b2ContactManager.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 "b2ContactManager.h"
  20 #include "b2World.h"
  21 #include "b2Body.h"
  22
  23 // This is a callback from the broadphase when two AABB proxies begin
  24 // to overlap. We create a b2Contact to manage the narrow phase.
  25 void* b2ContactManager::PairAdded(void* proxyUserData1, void* proxyUserData2)
  26 {
  27         b2Shape* shape1 = (b2Shape*)proxyUserData1;
  28         b2Shape* shape2 = (b2Shape*)proxyUserData2;
  29
  30         b2Body* body1 = shape1->GetBody();
  31         b2Body* body2 = shape2->GetBody();
  32
  33         if (body1->IsStatic() && body2->IsStatic())
  34         {
  35                 return &m_nullContact;
  36         }
  37
  38         if (shape1->GetBody() == shape2->GetBody())
  39         {
  40                 return &m_nullContact;
  41         }
  42
  43         if (body2->IsConnected(body1))
  44         {
  45                 return &m_nullContact;
  46         }
  47
  48         if (m_world->m_contactFilter != NULL && m_world->m_contactFilter->ShouldCollide(shape1, shape2) == false)
  49         {
  50                 return &m_nullContact;
  51         }
  52
  53         // Call the factory.
  54         b2Contact* c = b2Contact::Create(shape1, shape2, &m_world->m_blockAllocator);
  55
  56         if (c == NULL)
  57         {
  58                 return &m_nullContact;
  59         }
  60
  61         // Contact creation may swap shapes.
  62         shape1 = c->GetShape1();
  63         shape2 = c->GetShape2();
  64         body1 = shape1->GetBody();
  65         body2 = shape2->GetBody();
  66
  67         // Insert into the world.
  68         c->m_prev = NULL;
  69         c->m_next = m_world->m_contactList;
  70         if (m_world->m_contactList != NULL)
  71         {
  72                 m_world->m_contactList->m_prev = c;
  73         }
  74         m_world->m_contactList = c;
  75
  76         // Connect to island graph.
  77
  78         // Connect to body 1
  79         c->m_node1.contact = c;
  80         c->m_node1.other = body2;
  81
  82         c->m_node1.prev = NULL;
  83         c->m_node1.next = body1->m_contactList;
  84         if (body1->m_contactList != NULL)
  85         {
  86                 body1->m_contactList->prev = &c->m_node1;
  87         }
  88         body1->m_contactList = &c->m_node1;
  89
  90         // Connect to body 2
  91         c->m_node2.contact = c;
  92         c->m_node2.other = body1;
  93
  94         c->m_node2.prev = NULL;
  95         c->m_node2.next = body2->m_contactList;
  96         if (body2->m_contactList != NULL)
  97         {
  98                 body2->m_contactList->prev = &c->m_node2;
  99         }
 100         body2->m_contactList = &c->m_node2;
 101
 102         ++m_world->m_contactCount;
 103         return c;
 104 }
 105
 106 // This is a callback from the broadphase when two AABB proxies cease
 107 // to overlap. We retire the b2Contact.
 108 void b2ContactManager::PairRemoved(void* proxyUserData1, void* proxyUserData2, void* pairUserData)
 109 {
 110         B2_NOT_USED(proxyUserData1);
 111         B2_NOT_USED(proxyUserData2);
 112
 113         if (pairUserData == NULL)
 114         {
 115                 return;
 116         }
 117
 118         b2Contact* c = (b2Contact*)pairUserData;
 119         if (c == &m_nullContact)
 120         {
 121                 return;
 122         }
 123
 124         // An attached body is being destroyed, we must destroy this contact
 125         // immediately to avoid orphaned shape pointers.
 126         Destroy(c);
 127 }
 128
 129 void b2ContactManager::Destroy(b2Contact* c)
 130 {
 131         b2Shape* shape1 = c->GetShape1();
 132         b2Shape* shape2 = c->GetShape2();
 133
 134         // Inform the user that this contact is ending.
 135         int32 manifoldCount = c->GetManifoldCount();
 136         if (manifoldCount > 0 && m_world->m_contactListener)
 137         {
 138                 b2Body* b1 = shape1->GetBody();
 139                 b2Body* b2 = shape2->GetBody();
 140
 141                 b2Manifold* manifolds = c->GetManifolds();
 142                 b2ContactPoint cp;
 143                 cp.shape1 = c->GetShape1();
 144                 cp.shape2 = c->GetShape2();
 145                 cp.friction = c->m_friction;
 146                 cp.restitution = c->m_restitution;
 147
 148                 for (int32 i = 0; i < manifoldCount; ++i)
 149                 {
 150                         b2Manifold* manifold = manifolds + i;
 151                         cp.normal = manifold->normal;
 152
 153                         for (int32 j = 0; j < manifold->pointCount; ++j)
 154                         {
 155                                 b2ManifoldPoint* mp = manifold->points + j;
 156                                 cp.position = b1->GetWorldPoint(mp->localPoint1);
 157                                 b2Vec2 v1 = b1->GetLinearVelocityFromLocalPoint(mp->localPoint1);
 158                                 b2Vec2 v2 = b2->GetLinearVelocityFromLocalPoint(mp->localPoint2);
 159                                 cp.velocity = v2 - v1;
 160                                 cp.separation = mp->separation;
 161                                 cp.id = mp->id;
 162                                 m_world->m_contactListener->Remove(&cp);
 163                         }
 164                 }
 165         }
 166
 167         // Remove from the world.
 168         if (c->m_prev)
 169         {
 170                 c->m_prev->m_next = c->m_next;
 171         }
 172
 173         if (c->m_next)
 174         {
 175                 c->m_next->m_prev = c->m_prev;
 176         }
 177
 178         if (c == m_world->m_contactList)
 179         {
 180                 m_world->m_contactList = c->m_next;
 181         }
 182
 183         b2Body* body1 = shape1->GetBody();
 184         b2Body* body2 = shape2->GetBody();
 185
 186         // Remove from body 1
 187         if (c->m_node1.prev)
 188         {
 189                 c->m_node1.prev->next = c->m_node1.next;
 190         }
 191
 192         if (c->m_node1.next)
 193         {
 194                 c->m_node1.next->prev = c->m_node1.prev;
 195         }
 196
 197         if (&c->m_node1 == body1->m_contactList)
 198         {
 199                 body1->m_contactList = c->m_node1.next;
 200         }
 201
 202         // Remove from body 2
 203         if (c->m_node2.prev)
 204         {
 205                 c->m_node2.prev->next = c->m_node2.next;
 206         }
 207
 208         if (c->m_node2.next)
 209         {
 210                 c->m_node2.next->prev = c->m_node2.prev;
 211         }
 212
 213         if (&c->m_node2 == body2->m_contactList)
 214         {
 215                 body2->m_contactList = c->m_node2.next;
 216         }
 217
 218         // Call the factory.
 219         b2Contact::Destroy(c, &m_world->m_blockAllocator);
 220         --m_world->m_contactCount;
 221 }
 222
 223 // This is the top level collision call for the time step. Here
 224 // all the narrow phase collision is processed for the world
 225 // contact list.
 226 void b2ContactManager::Collide()
 227 {
 228         // Update awake contacts.
 229         for (b2Contact* c = m_world->m_contactList; c; c = c->GetNext())
 230         {
 231                 b2Body* body1 = c->GetShape1()->GetBody();
 232                 b2Body* body2 = c->GetShape2()->GetBody();
 233                 if (body1->IsSleeping() && body2->IsSleeping())
 234                 {
 235                         continue;
 236                 }
 237
 238                 c->Update(m_world->m_contactListener);
 239         }
 240 }