physics library version update
[spacedolphin:spacedolphin.git] / lib / chipmunk / src / cpArbiter.c
1 /* Copyright (c) 2007 Scott Lembcke
2  * 
3  * Permission is hereby granted, free of charge, to any person obtaining a copy
4  * of this software and associated documentation files (the "Software"), to deal
5  * in the Software without restriction, including without limitation the rights
6  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
7  * copies of the Software, and to permit persons to whom the Software is
8  * furnished to do so, subject to the following conditions:
9  * 
10  * The above copyright notice and this permission notice shall be included in
11  * all copies or substantial portions of the Software.
12  * 
13  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
16  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
17  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
18  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
19  * SOFTWARE.
20  */
21
22 #include "chipmunk_private.h"
23 #include "constraints/util.h"
24
25 cpContact*
26 cpContactInit(cpContact *con, cpVect p, cpVect n, cpFloat dist, cpHashValue hash)
27 {
28         con->p = p;
29         con->n = n;
30         con->dist = dist;
31         
32         con->jnAcc = 0.0f;
33         con->jtAcc = 0.0f;
34         con->jBias = 0.0f;
35         
36         con->hash = hash;
37                 
38         return con;
39 }
40
41 // TODO make this generic so I can reuse it for constraints also.
42 static inline void
43 unthreadHelper(cpArbiter *arb, cpBody *body)
44 {
45         struct cpArbiterThread *thread = cpArbiterThreadForBody(arb, body);
46         cpArbiter *prev = thread->prev;
47         cpArbiter *next = thread->next;
48         
49         if(prev){
50                 cpArbiterThreadForBody(prev, body)->next = next;
51         } else if(body->arbiterList == arb) {
52                 // IFF prev is NULL and body->arbiterList == arb, is arb at the head of the list.
53                 // This function may be called for an arbiter that was never in a list.
54                 // In that case, we need to protect it from wiping out the body->arbiterList pointer.
55                 body->arbiterList = next;
56         }
57         
58         if(next) cpArbiterThreadForBody(next, body)->prev = prev;
59         
60         thread->prev = NULL;
61         thread->next = NULL;
62 }
63
64 void
65 cpArbiterUnthread(cpArbiter *arb)
66 {
67         unthreadHelper(arb, arb->body_a);
68         unthreadHelper(arb, arb->body_b);
69 }
70
71 cpBool cpArbiterIsFirstContact(const cpArbiter *arb)
72 {
73         return arb->CP_PRIVATE(state) == cpArbiterStateFirstColl;
74 }
75
76 int cpArbiterGetCount(const cpArbiter *arb)
77 {
78         // Return 0 contacts if we are in a separate callback.
79         return (arb->CP_PRIVATE(state) != cpArbiterStateCached ? arb->CP_PRIVATE(numContacts) : 0);
80 }
81
82 cpVect
83 cpArbiterGetNormal(const cpArbiter *arb, int i)
84 {
85         cpAssertHard(0 <= i && i < cpArbiterGetCount(arb), "Index error: The specified contact index is invalid for this arbiter");
86         
87         cpVect n = arb->contacts[i].n;
88         return arb->swappedColl ? cpvneg(n) : n;
89 }
90
91 cpVect
92 cpArbiterGetPoint(const cpArbiter *arb, int i)
93 {
94         cpAssertHard(0 <= i && i < cpArbiterGetCount(arb), "Index error: The specified contact index is invalid for this arbiter");
95         
96         return arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(p);
97 }
98
99 cpFloat
100 cpArbiterGetDepth(const cpArbiter *arb, int i)
101 {
102         cpAssertHard(0 <= i && i < cpArbiterGetCount(arb), "Index error: The specified contact index is invalid for this arbiter");
103         
104         return arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(dist);
105 }
106
107 cpContactPointSet
108 cpArbiterGetContactPointSet(const cpArbiter *arb)
109 {
110         cpContactPointSet set;
111         set.count = cpArbiterGetCount(arb);
112         
113         int i;
114         for(i=0; i<set.count; i++){
115                 set.points[i].point = arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(p);
116                 set.points[i].normal = arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(n);
117                 set.points[i].dist = arb->CP_PRIVATE(contacts)[i].CP_PRIVATE(dist);
118         }
119         
120         return set;
121 }
122
123
124 cpVect
125 cpArbiterTotalImpulse(const cpArbiter *arb)
126 {
127         cpContact *contacts = arb->contacts;
128         cpVect sum = cpvzero;
129         
130         for(int i=0, count=cpArbiterGetCount(arb); i<count; i++){
131                 cpContact *con = &contacts[i];
132                 sum = cpvadd(sum, cpvmult(con->n, con->jnAcc));
133         }
134         
135         return (arb->swappedColl ? sum : cpvneg(sum));
136 }
137
138 cpVect
139 cpArbiterTotalImpulseWithFriction(const cpArbiter *arb)
140 {
141         cpContact *contacts = arb->contacts;
142         cpVect sum = cpvzero;
143         
144         for(int i=0, count=cpArbiterGetCount(arb); i<count; i++){
145                 cpContact *con = &contacts[i];
146                 sum = cpvadd(sum, cpvrotate(con->n, cpv(con->jnAcc, con->jtAcc)));
147         }
148                 
149         return (arb->swappedColl ? sum : cpvneg(sum));
150 }
151
152 cpFloat
153 cpArbiterTotalKE(const cpArbiter *arb)
154 {
155         cpFloat eCoef = (1 - arb->e)/(1 + arb->e);
156         cpFloat sum = 0.0;
157         
158         cpContact *contacts = arb->contacts;
159         for(int i=0, count=cpArbiterGetCount(arb); i<count; i++){
160                 cpContact *con = &contacts[i];
161                 cpFloat jnAcc = con->jnAcc;
162                 cpFloat jtAcc = con->jtAcc;
163                 
164                 sum += eCoef*jnAcc*jnAcc/con->nMass + jtAcc*jtAcc/con->tMass;
165         }
166         
167         return sum;
168 }
169
170 //cpFloat
171 //cpContactsEstimateCrushingImpulse(cpContact *contacts, int numContacts)
172 //{
173 //      cpFloat fsum = 0.0f;
174 //      cpVect vsum = cpvzero;
175 //      
176 //      for(int i=0; i<numContacts; i++){
177 //              cpContact *con = &contacts[i];
178 //              cpVect j = cpvrotate(con->n, cpv(con->jnAcc, con->jtAcc));
179 //              
180 //              fsum += cpvlength(j);
181 //              vsum = cpvadd(vsum, j);
182 //      }
183 //      
184 //      cpFloat vmag = cpvlength(vsum);
185 //      return (1.0f - vmag/fsum);
186 //}
187
188 void
189 cpArbiterIgnore(cpArbiter *arb)
190 {
191         arb->state = cpArbiterStateIgnore;
192 }
193
194 cpArbiter*
195 cpArbiterInit(cpArbiter *arb, cpShape *a, cpShape *b)
196 {
197         arb->handler = NULL;
198         arb->swappedColl = cpFalse;
199         
200         arb->e = 0.0f;
201         arb->u = 0.0f;
202         arb->surface_vr = cpvzero;
203         
204         arb->numContacts = 0;
205         arb->contacts = NULL;
206         
207         arb->a = a; arb->body_a = a->body;
208         arb->b = b; arb->body_b = b->body;
209         
210         arb->thread_a.next = NULL;
211         arb->thread_b.next = NULL;
212         arb->thread_a.prev = NULL;
213         arb->thread_b.prev = NULL;
214         
215         arb->stamp = 0;
216         arb->state = cpArbiterStateFirstColl;
217         
218         arb->data = NULL;
219         
220         return arb;
221 }
222
223 void
224 cpArbiterUpdate(cpArbiter *arb, cpContact *contacts, int numContacts, cpCollisionHandler *handler, cpShape *a, cpShape *b)
225 {
226         // Arbiters without contact data may exist if a collision function rejected the collision.
227         if(arb->numContacts > 0){
228                 // Iterate over the possible pairs to look for hash value matches.
229                 for(int i=0; i<arb->numContacts; i++){
230                         cpContact *old = &arb->contacts[i];
231                         
232                         for(int j=0; j<numContacts; j++){
233                                 cpContact *new_contact = &contacts[j];
234                                 
235                                 // This could trigger false positives, but is fairly unlikely nor serious if it does.
236                                 if(new_contact->hash == old->hash){
237                                         // Copy the persistant contact information.
238                                         new_contact->jnAcc = old->jnAcc;
239                                         new_contact->jtAcc = old->jtAcc;
240                                 }
241                         }
242                 }
243         }
244         
245         arb->contacts = contacts;
246         arb->numContacts = numContacts;
247         
248         arb->handler = handler;
249         arb->swappedColl = (a->collision_type != handler->a);
250         
251         arb->e = a->e * b->e;
252         arb->u = a->u * b->u;
253         arb->surface_vr = cpvsub(a->surface_v, b->surface_v);
254         
255         // For collisions between two similar primitive types, the order could have been swapped.
256         arb->a = a; arb->body_a = a->body;
257         arb->b = b; arb->body_b = b->body;
258         
259         // mark it as new if it's been cached
260         if(arb->state == cpArbiterStateCached) arb->state = cpArbiterStateFirstColl;
261 }
262
263 void
264 cpArbiterPreStep(cpArbiter *arb, cpFloat dt, cpFloat slop, cpFloat bias)
265 {
266         cpBody *a = arb->body_a;
267         cpBody *b = arb->body_b;
268         
269         for(int i=0; i<arb->numContacts; i++){
270                 cpContact *con = &arb->contacts[i];
271                 
272                 // Calculate the offsets.
273                 con->r1 = cpvsub(con->p, a->p);
274                 con->r2 = cpvsub(con->p, b->p);
275                 
276                 // Calculate the mass normal and mass tangent.
277                 con->nMass = 1.0f/k_scalar(a, b, con->r1, con->r2, con->n);
278                 con->tMass = 1.0f/k_scalar(a, b, con->r1, con->r2, cpvperp(con->n));
279                                 
280                 // Calculate the target bias velocity.
281                 con->bias = -bias*cpfmin(0.0f, con->dist + slop)/dt;
282                 con->jBias = 0.0f;
283                 
284                 // Calculate the target bounce velocity.
285                 con->bounce = normal_relative_velocity(a, b, con->r1, con->r2, con->n)*arb->e;
286         }
287 }
288
289 void
290 cpArbiterApplyCachedImpulse(cpArbiter *arb, cpFloat dt_coef)
291 {
292         if(cpArbiterIsFirstContact(arb)) return;
293         
294         cpBody *a = arb->body_a;
295         cpBody *b = arb->body_b;
296         
297         for(int i=0; i<arb->numContacts; i++){
298                 cpContact *con = &arb->contacts[i];
299                 cpVect j = cpvrotate(con->n, cpv(con->jnAcc, con->jtAcc));
300                 apply_impulses(a, b, con->r1, con->r2, cpvmult(j, dt_coef));
301         }
302 }
303
304 // TODO is it worth splitting velocity/position correction?
305
306 void
307 cpArbiterApplyImpulse(cpArbiter *arb)
308 {
309         cpBody *a = arb->body_a;
310         cpBody *b = arb->body_b;
311         cpVect surface_vr = arb->surface_vr;
312         cpFloat friction = arb->u;
313
314         for(int i=0; i<arb->numContacts; i++){
315                 cpContact *con = &arb->contacts[i];
316                 cpFloat nMass = con->nMass;
317                 cpVect n = con->n;
318                 cpVect r1 = con->r1;
319                 cpVect r2 = con->r2;
320                 
321                 cpVect vb1 = cpvadd(a->v_bias, cpvmult(cpvperp(r1), a->w_bias));
322                 cpVect vb2 = cpvadd(b->v_bias, cpvmult(cpvperp(r2), b->w_bias));
323                 cpVect vr = relative_velocity(a, b, r1, r2);
324                 
325                 cpFloat vbn = cpvdot(cpvsub(vb2, vb1), n);
326                 cpFloat vrn = cpvdot(vr, n);
327                 cpFloat vrt = cpvdot(cpvadd(vr, surface_vr), cpvperp(n));
328                 
329                 cpFloat jbn = (con->bias - vbn)*nMass;
330                 cpFloat jbnOld = con->jBias;
331                 con->jBias = cpfmax(jbnOld + jbn, 0.0f);
332                 
333                 cpFloat jn = -(con->bounce + vrn)*nMass;
334                 cpFloat jnOld = con->jnAcc;
335                 con->jnAcc = cpfmax(jnOld + jn, 0.0f);
336                 
337                 cpFloat jtMax = friction*con->jnAcc;
338                 cpFloat jt = -vrt*con->tMass;
339                 cpFloat jtOld = con->jtAcc;
340                 con->jtAcc = cpfclamp(jtOld + jt, -jtMax, jtMax);
341                 
342                 apply_bias_impulses(a, b, r1, r2, cpvmult(n, con->jBias - jbnOld));
343                 apply_impulses(a, b, r1, r2, cpvrotate(n, cpv(con->jnAcc - jnOld, con->jtAcc - jtOld)));
344         }
345 }