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