1 /*
2 * Copyright (c) 2006-2009 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 <Box2D/Dynamics/Contacts/b2Contact.h>
20 #include <Box2D/Dynamics/Contacts/b2CircleContact.h>
21 #include <Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h>
22 #include <Box2D/Dynamics/Contacts/b2PolygonContact.h>
23 #include <Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h>
24 #include <Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h>
25 #include <Box2D/Dynamics/Contacts/b2LoopAndCircleContact.h>
26 #include <Box2D/Dynamics/Contacts/b2LoopAndPolygonContact.h>
27 #include <Box2D/Dynamics/Contacts/b2ContactSolver.h>
28
29 #include <Box2D/Collision/b2TimeOfImpact.h>
30 #include <Box2D/Common/b2BlockAllocator.h>
31 #include <Box2D/Dynamics/b2Body.h>
32 #include <Box2D/Dynamics/b2World.h>
33
34 b2ContactRegister b2Contact::s_registers[b2Shape::e_typeCount][b2Shape::e_typeCount];
35 bool b2Contact::s_initialized = false;
36
InitializeRegisters()37 void b2Contact::InitializeRegisters()
38 {
39 AddType(b2CircleContact::Create, b2CircleContact::Destroy, b2Shape::e_circle, b2Shape::e_circle);
40 AddType(b2PolygonAndCircleContact::Create, b2PolygonAndCircleContact::Destroy, b2Shape::e_polygon, b2Shape::e_circle);
41 AddType(b2PolygonContact::Create, b2PolygonContact::Destroy, b2Shape::e_polygon, b2Shape::e_polygon);
42 AddType(b2EdgeAndCircleContact::Create, b2EdgeAndCircleContact::Destroy, b2Shape::e_edge, b2Shape::e_circle);
43 AddType(b2EdgeAndPolygonContact::Create, b2EdgeAndPolygonContact::Destroy, b2Shape::e_edge, b2Shape::e_polygon);
44 AddType(b2LoopAndCircleContact::Create, b2LoopAndCircleContact::Destroy, b2Shape::e_loop, b2Shape::e_circle);
45 AddType(b2LoopAndPolygonContact::Create, b2LoopAndPolygonContact::Destroy, b2Shape::e_loop, b2Shape::e_polygon);
46 }
47
AddType(b2ContactCreateFcn * createFcn,b2ContactDestroyFcn * destoryFcn,b2Shape::Type type1,b2Shape::Type type2)48 void b2Contact::AddType(b2ContactCreateFcn* createFcn, b2ContactDestroyFcn* destoryFcn,
49 b2Shape::Type type1, b2Shape::Type type2)
50 {
51 b2Assert(b2Shape::e_unknown < type1 && type1 < b2Shape::e_typeCount);
52 b2Assert(b2Shape::e_unknown < type2 && type2 < b2Shape::e_typeCount);
53
54 s_registers[type1][type2].createFcn = createFcn;
55 s_registers[type1][type2].destroyFcn = destoryFcn;
56 s_registers[type1][type2].primary = true;
57
58 if (type1 != type2)
59 {
60 s_registers[type2][type1].createFcn = createFcn;
61 s_registers[type2][type1].destroyFcn = destoryFcn;
62 s_registers[type2][type1].primary = false;
63 }
64 }
65
Create(b2Fixture * fixtureA,int32 indexA,b2Fixture * fixtureB,int32 indexB,b2BlockAllocator * allocator)66 b2Contact* b2Contact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator)
67 {
68 if (s_initialized == false)
69 {
70 InitializeRegisters();
71 s_initialized = true;
72 }
73
74 b2Shape::Type type1 = fixtureA->GetType();
75 b2Shape::Type type2 = fixtureB->GetType();
76
77 b2Assert(b2Shape::e_unknown < type1 && type1 < b2Shape::e_typeCount);
78 b2Assert(b2Shape::e_unknown < type2 && type2 < b2Shape::e_typeCount);
79
80 b2ContactCreateFcn* createFcn = s_registers[type1][type2].createFcn;
81 if (createFcn)
82 {
83 if (s_registers[type1][type2].primary)
84 {
85 return createFcn(fixtureA, indexA, fixtureB, indexB, allocator);
86 }
87 else
88 {
89 return createFcn(fixtureB, indexB, fixtureA, indexA, allocator);
90 }
91 }
92 else
93 {
94 return NULL;
95 }
96 }
97
Destroy(b2Contact * contact,b2BlockAllocator * allocator)98 void b2Contact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
99 {
100 b2Assert(s_initialized == true);
101
102 if (contact->m_manifold.pointCount > 0)
103 {
104 contact->GetFixtureA()->GetBody()->SetAwake(true);
105 contact->GetFixtureB()->GetBody()->SetAwake(true);
106 }
107
108 b2Shape::Type typeA = contact->GetFixtureA()->GetType();
109 b2Shape::Type typeB = contact->GetFixtureB()->GetType();
110
111 b2Assert(b2Shape::e_unknown < typeA && typeB < b2Shape::e_typeCount);
112 b2Assert(b2Shape::e_unknown < typeA && typeB < b2Shape::e_typeCount);
113
114 b2ContactDestroyFcn* destroyFcn = s_registers[typeA][typeB].destroyFcn;
115 destroyFcn(contact, allocator);
116 }
117
b2Contact(b2Fixture * fA,int32 indexA,b2Fixture * fB,int32 indexB)118 b2Contact::b2Contact(b2Fixture* fA, int32 indexA, b2Fixture* fB, int32 indexB)
119 {
120 m_flags = e_enabledFlag;
121
122 m_fixtureA = fA;
123 m_fixtureB = fB;
124
125 m_indexA = indexA;
126 m_indexB = indexB;
127
128 m_manifold.pointCount = 0;
129
130 m_prev = NULL;
131 m_next = NULL;
132
133 m_nodeA.contact = NULL;
134 m_nodeA.prev = NULL;
135 m_nodeA.next = NULL;
136 m_nodeA.other = NULL;
137
138 m_nodeB.contact = NULL;
139 m_nodeB.prev = NULL;
140 m_nodeB.next = NULL;
141 m_nodeB.other = NULL;
142
143 m_toiCount = 0;
144 }
145
146 // Update the contact manifold and touching status.
147 // Note: do not assume the fixture AABBs are overlapping or are valid.
Update(b2ContactListener * listener)148 void b2Contact::Update(b2ContactListener* listener)
149 {
150 b2Manifold oldManifold = m_manifold;
151
152 // Re-enable this contact.
153 m_flags |= e_enabledFlag;
154
155 bool touching = false;
156 bool wasTouching = (m_flags & e_touchingFlag) == e_touchingFlag;
157
158 bool sensorA = m_fixtureA->IsSensor();
159 bool sensorB = m_fixtureB->IsSensor();
160 bool sensor = sensorA || sensorB;
161
162 b2Body* bodyA = m_fixtureA->GetBody();
163 b2Body* bodyB = m_fixtureB->GetBody();
164 const b2Transform& xfA = bodyA->GetTransform();
165 const b2Transform& xfB = bodyB->GetTransform();
166
167 // Is this contact a sensor?
168 if (sensor)
169 {
170 const b2Shape* shapeA = m_fixtureA->GetShape();
171 const b2Shape* shapeB = m_fixtureB->GetShape();
172 touching = b2TestOverlap(shapeA, m_indexA, shapeB, m_indexB, xfA, xfB);
173
174 // Sensors don't generate manifolds.
175 m_manifold.pointCount = 0;
176 }
177 else
178 {
179 Evaluate(&m_manifold, xfA, xfB);
180 touching = m_manifold.pointCount > 0;
181
182 // Match old contact ids to new contact ids and copy the
183 // stored impulses to warm start the solver.
184 for (int32 i = 0; i < m_manifold.pointCount; ++i)
185 {
186 b2ManifoldPoint* mp2 = m_manifold.points + i;
187 mp2->normalImpulse = 0.0f;
188 mp2->tangentImpulse = 0.0f;
189 b2ContactID id2 = mp2->id;
190 bool found = false;
191
192 for (int32 j = 0; j < oldManifold.pointCount; ++j)
193 {
194 b2ManifoldPoint* mp1 = oldManifold.points + j;
195
196 if (mp1->id.key == id2.key)
197 {
198 mp2->normalImpulse = mp1->normalImpulse;
199 mp2->tangentImpulse = mp1->tangentImpulse;
200 found = true;
201 break;
202 }
203 }
204
205 if (found == false)
206 {
207 mp2->normalImpulse = 0.0f;
208 mp2->tangentImpulse = 0.0f;
209 }
210 }
211
212 if (touching != wasTouching)
213 {
214 bodyA->SetAwake(true);
215 bodyB->SetAwake(true);
216 }
217 }
218
219 if (touching)
220 {
221 m_flags |= e_touchingFlag;
222 }
223 else
224 {
225 m_flags &= ~e_touchingFlag;
226 }
227
228 if (wasTouching == false && touching == true && listener)
229 {
230 listener->BeginContact(this);
231 }
232
233 if (wasTouching == true && touching == false && listener)
234 {
235 listener->EndContact(this);
236 }
237
238 if (sensor == false && touching && listener)
239 {
240 listener->PreSolve(this, &oldManifold);
241 }
242 }
243