Updated to Box2D 2.3.0
[qml-box2d:qml-box2d-folibis.git] / Box2D / Dynamics / b2ContactManager.cpp
1 /*
2 * Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
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 <Box2D/Dynamics/b2ContactManager.h>
20 #include <Box2D/Dynamics/b2Body.h>
21 #include <Box2D/Dynamics/b2Fixture.h>
22 #include <Box2D/Dynamics/b2WorldCallbacks.h>
23 #include <Box2D/Dynamics/Contacts/b2Contact.h>
24
25 b2ContactFilter b2_defaultFilter;
26 b2ContactListener b2_defaultListener;
27
28 b2ContactManager::b2ContactManager()
29 {
30         m_contactList = NULL;
31         m_contactCount = 0;
32         m_contactFilter = &b2_defaultFilter;
33         m_contactListener = &b2_defaultListener;
34         m_allocator = NULL;
35 }
36
37 void b2ContactManager::Destroy(b2Contact* c)
38 {
39         b2Fixture* fixtureA = c->GetFixtureA();
40         b2Fixture* fixtureB = c->GetFixtureB();
41         b2Body* bodyA = fixtureA->GetBody();
42         b2Body* bodyB = fixtureB->GetBody();
43
44         if (m_contactListener && c->IsTouching())
45         {
46                 m_contactListener->EndContact(c);
47         }
48
49         // Remove from the world.
50         if (c->m_prev)
51         {
52                 c->m_prev->m_next = c->m_next;
53         }
54
55         if (c->m_next)
56         {
57                 c->m_next->m_prev = c->m_prev;
58         }
59
60         if (c == m_contactList)
61         {
62                 m_contactList = c->m_next;
63         }
64
65         // Remove from body 1
66         if (c->m_nodeA.prev)
67         {
68                 c->m_nodeA.prev->next = c->m_nodeA.next;
69         }
70
71         if (c->m_nodeA.next)
72         {
73                 c->m_nodeA.next->prev = c->m_nodeA.prev;
74         }
75
76         if (&c->m_nodeA == bodyA->m_contactList)
77         {
78                 bodyA->m_contactList = c->m_nodeA.next;
79         }
80
81         // Remove from body 2
82         if (c->m_nodeB.prev)
83         {
84                 c->m_nodeB.prev->next = c->m_nodeB.next;
85         }
86
87         if (c->m_nodeB.next)
88         {
89                 c->m_nodeB.next->prev = c->m_nodeB.prev;
90         }
91
92         if (&c->m_nodeB == bodyB->m_contactList)
93         {
94                 bodyB->m_contactList = c->m_nodeB.next;
95         }
96
97         // Call the factory.
98         b2Contact::Destroy(c, m_allocator);
99         --m_contactCount;
100 }
101
102 // This is the top level collision call for the time step. Here
103 // all the narrow phase collision is processed for the world
104 // contact list.
105 void b2ContactManager::Collide()
106 {
107         // Update awake contacts.
108         b2Contact* c = m_contactList;
109         while (c)
110         {
111                 b2Fixture* fixtureA = c->GetFixtureA();
112                 b2Fixture* fixtureB = c->GetFixtureB();
113                 int32 indexA = c->GetChildIndexA();
114                 int32 indexB = c->GetChildIndexB();
115                 b2Body* bodyA = fixtureA->GetBody();
116                 b2Body* bodyB = fixtureB->GetBody();
117                  
118                 // Is this contact flagged for filtering?
119                 if (c->m_flags & b2Contact::e_filterFlag)
120                 {
121                         // Should these bodies collide?
122                         if (bodyB->ShouldCollide(bodyA) == false)
123                         {
124                                 b2Contact* cNuke = c;
125                                 c = cNuke->GetNext();
126                                 Destroy(cNuke);
127                                 continue;
128                         }
129
130                         // Check user filtering.
131                         if (m_contactFilter && m_contactFilter->ShouldCollide(fixtureA, fixtureB) == false)
132                         {
133                                 b2Contact* cNuke = c;
134                                 c = cNuke->GetNext();
135                                 Destroy(cNuke);
136                                 continue;
137                         }
138
139                         // Clear the filtering flag.
140                         c->m_flags &= ~b2Contact::e_filterFlag;
141                 }
142
143                 bool activeA = bodyA->IsAwake() && bodyA->m_type != b2_staticBody;
144                 bool activeB = bodyB->IsAwake() && bodyB->m_type != b2_staticBody;
145
146                 // At least one body must be awake and it must be dynamic or kinematic.
147                 if (activeA == false && activeB == false)
148                 {
149                         c = c->GetNext();
150                         continue;
151                 }
152
153                 int32 proxyIdA = fixtureA->m_proxies[indexA].proxyId;
154                 int32 proxyIdB = fixtureB->m_proxies[indexB].proxyId;
155                 bool overlap = m_broadPhase.TestOverlap(proxyIdA, proxyIdB);
156
157                 // Here we destroy contacts that cease to overlap in the broad-phase.
158                 if (overlap == false)
159                 {
160                         b2Contact* cNuke = c;
161                         c = cNuke->GetNext();
162                         Destroy(cNuke);
163                         continue;
164                 }
165
166                 // The contact persists.
167                 c->Update(m_contactListener);
168                 c = c->GetNext();
169         }
170 }
171
172 void b2ContactManager::FindNewContacts()
173 {
174         m_broadPhase.UpdatePairs(this);
175 }
176
177 void b2ContactManager::AddPair(void* proxyUserDataA, void* proxyUserDataB)
178 {
179         b2FixtureProxy* proxyA = (b2FixtureProxy*)proxyUserDataA;
180         b2FixtureProxy* proxyB = (b2FixtureProxy*)proxyUserDataB;
181
182         b2Fixture* fixtureA = proxyA->fixture;
183         b2Fixture* fixtureB = proxyB->fixture;
184
185         int32 indexA = proxyA->childIndex;
186         int32 indexB = proxyB->childIndex;
187
188         b2Body* bodyA = fixtureA->GetBody();
189         b2Body* bodyB = fixtureB->GetBody();
190
191         // Are the fixtures on the same body?
192         if (bodyA == bodyB)
193         {
194                 return;
195         }
196
197         // TODO_ERIN use a hash table to remove a potential bottleneck when both
198         // bodies have a lot of contacts.
199         // Does a contact already exist?
200         b2ContactEdge* edge = bodyB->GetContactList();
201         while (edge)
202         {
203                 if (edge->other == bodyA)
204                 {
205                         b2Fixture* fA = edge->contact->GetFixtureA();
206                         b2Fixture* fB = edge->contact->GetFixtureB();
207                         int32 iA = edge->contact->GetChildIndexA();
208                         int32 iB = edge->contact->GetChildIndexB();
209
210                         if (fA == fixtureA && fB == fixtureB && iA == indexA && iB == indexB)
211                         {
212                                 // A contact already exists.
213                                 return;
214                         }
215
216                         if (fA == fixtureB && fB == fixtureA && iA == indexB && iB == indexA)
217                         {
218                                 // A contact already exists.
219                                 return;
220                         }
221                 }
222
223                 edge = edge->next;
224         }
225
226         // Does a joint override collision? Is at least one body dynamic?
227         if (bodyB->ShouldCollide(bodyA) == false)
228         {
229                 return;
230         }
231
232         // Check user filtering.
233         if (m_contactFilter && m_contactFilter->ShouldCollide(fixtureA, fixtureB) == false)
234         {
235                 return;
236         }
237
238         // Call the factory.
239         b2Contact* c = b2Contact::Create(fixtureA, indexA, fixtureB, indexB, m_allocator);
240         if (c == NULL)
241         {
242                 return;
243         }
244
245         // Contact creation may swap fixtures.
246         fixtureA = c->GetFixtureA();
247         fixtureB = c->GetFixtureB();
248         indexA = c->GetChildIndexA();
249         indexB = c->GetChildIndexB();
250         bodyA = fixtureA->GetBody();
251         bodyB = fixtureB->GetBody();
252
253         // Insert into the world.
254         c->m_prev = NULL;
255         c->m_next = m_contactList;
256         if (m_contactList != NULL)
257         {
258                 m_contactList->m_prev = c;
259         }
260         m_contactList = c;
261
262         // Connect to island graph.
263
264         // Connect to body A
265         c->m_nodeA.contact = c;
266         c->m_nodeA.other = bodyB;
267
268         c->m_nodeA.prev = NULL;
269         c->m_nodeA.next = bodyA->m_contactList;
270         if (bodyA->m_contactList != NULL)
271         {
272                 bodyA->m_contactList->prev = &c->m_nodeA;
273         }
274         bodyA->m_contactList = &c->m_nodeA;
275
276         // Connect to body B
277         c->m_nodeB.contact = c;
278         c->m_nodeB.other = bodyA;
279
280         c->m_nodeB.prev = NULL;
281         c->m_nodeB.next = bodyB->m_contactList;
282         if (bodyB->m_contactList != NULL)
283         {
284                 bodyB->m_contactList->prev = &c->m_nodeB;
285         }
286         bodyB->m_contactList = &c->m_nodeB;
287
288         // Wake up the bodies
289         if (fixtureA->IsSensor() == false && fixtureB->IsSensor() == false)
290         {
291                 bodyA->SetAwake(true);
292                 bodyB->SetAwake(true);
293         }
294
295         ++m_contactCount;
296 }