Finalised support for compiling BULLET on linux.
[blender.git] / extern / bullet / BulletDynamics / ConstraintSolver / ContactConstraint.cpp
1 /*
2  * Copyright (c) 2005 Erwin Coumans http://www.erwincoumans.com
3  *
4  * Permission to use, copy, modify, distribute and sell this software
5  * and its documentation for any purpose is hereby granted without fee,
6  * provided that the above copyright notice appear in all copies.
7  * Erwin Coumans makes no representations about the suitability 
8  * of this software for any purpose.  
9  * It is provided "as is" without express or implied warranty.
10 */
11 #include "ContactConstraint.h"
12 #include "Dynamics/RigidBody.h"
13 #include "SimdVector3.h"
14 #include "JacobianEntry.h"
15 #include "ContactSolverInfo.h"
16 #include "GEN_MinMax.h"
17
18 #define ASSERT2 assert
19
20
21 static SimdScalar ContactThreshold = -10.0f;  
22
23 float useGlobalSettingContacts = false;//true;
24
25 SimdScalar contactDamping = 0.2f;
26 SimdScalar contactTau = .02f;//0.02f;//*0.02f;
27
28
29
30 SimdScalar restitutionCurve(SimdScalar rel_vel, SimdScalar restitution)
31 {
32         return 0.f;
33 //      return restitution * GEN_min(1.0f, rel_vel / ContactThreshold);
34 }
35
36 float MAX_FRICTION = 100.f;
37
38 SimdScalar      calculateCombinedFriction(RigidBody& body0,RigidBody& body1)
39 {
40         SimdScalar friction = body0.getFriction() * body1.getFriction();
41         if (friction < 0.f)
42                 friction = 0.f;
43         if (friction > MAX_FRICTION)
44                 friction = MAX_FRICTION;
45         return friction;
46
47 }
48
49 void    applyFrictionInContactPointOld(RigidBody& body1, const SimdVector3& pos1,
50                       RigidBody& body2, const SimdVector3& pos2,
51                 const SimdVector3& normal,float normalImpulse, 
52                 const ContactSolverInfo& solverInfo)
53 {
54
55
56         if (normalImpulse>0.f)
57         {
58                 SimdVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
59                 SimdVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
60
61                 SimdVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
62                 SimdVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
63                 SimdVector3 vel = vel1 - vel2;
64         
65                 SimdScalar rel_vel = normal.dot(vel);
66
67                 float combinedFriction = calculateCombinedFriction(body1,body2);
68
69 #define PER_CONTACT_FRICTION
70 #ifdef PER_CONTACT_FRICTION
71                 SimdVector3 lat_vel = vel - normal * rel_vel;
72                 SimdScalar lat_rel_vel = lat_vel.length();
73
74                 if (lat_rel_vel > SIMD_EPSILON)
75                 {
76                         lat_vel /= lat_rel_vel;
77                         SimdVector3 temp1 = body1.getInvInertiaTensorWorld() * rel_pos1.cross(lat_vel); 
78                         SimdVector3 temp2 = body2.getInvInertiaTensorWorld() * rel_pos2.cross(lat_vel); 
79                         SimdScalar frictionMaxImpulse = lat_rel_vel / 
80                                 (body1.getInvMass() + body2.getInvMass() + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
81                         SimdScalar frictionImpulse = (normalImpulse) * combinedFriction;
82                         GEN_set_min(frictionImpulse,frictionMaxImpulse );
83                         
84                         body1.applyImpulse(lat_vel * -frictionImpulse, rel_pos1);
85                         body2.applyImpulse(lat_vel * frictionImpulse, rel_pos2);
86                         
87                 }
88 #endif
89                 }
90 }
91
92
93 //bilateral constraint between two dynamic objects
94 void resolveSingleBilateral(RigidBody& body1, const SimdVector3& pos1,
95                       RigidBody& body2, const SimdVector3& pos2,
96                       SimdScalar depth, const SimdVector3& normal,SimdScalar& impulse ,float timeStep)
97 {
98         float normalLenSqr = normal.length2();
99         ASSERT2(fabs(normalLenSqr) < 1.1f);
100         if (normalLenSqr > 1.1f)
101         {
102                 impulse = 0.f;
103                 return;
104         }
105         SimdVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
106         SimdVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
107         //this jacobian entry could be re-used for all iterations
108         
109         SimdVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
110         SimdVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
111         SimdVector3 vel = vel1 - vel2;
112         
113         SimdScalar rel_vel;
114
115         /*
116         
117           JacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
118                 body2.getCenterOfMassTransform().getBasis().transpose(),
119                 rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(),
120                 body2.getInvInertiaDiagLocal(),body2.getInvMass());
121
122         SimdScalar jacDiagAB = jac.getDiagonal();
123         SimdScalar jacDiagABInv = 1.f / jacDiagAB;
124         
125           SimdScalar rel_vel = jac.getRelativeVelocity(
126                 body1.getLinearVelocity(),
127                 body1.getCenterOfMassTransform().getBasis().transpose() * body1.getAngularVelocity(),
128                 body2.getLinearVelocity(),
129                 body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity()); 
130         float a;
131         a=jacDiagABInv;
132
133         */
134         rel_vel = normal.dot(vel);
135         
136
137
138
139         /*int color = 255+255*256;
140
141         DrawRasterizerLine(pos1,pos1+normal,color);
142 */
143
144                 
145         SimdScalar massTerm = 1.f / (body1.getInvMass() + body2.getInvMass());
146
147         impulse = - contactDamping * rel_vel * massTerm;//jacDiagABInv;
148
149         //SimdScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
150         //impulse = velocityImpulse;
151
152 }
153
154
155
156
157
158 //velocity + friction
159 //response  between two dynamic objects with friction
160 float resolveSingleCollision(RigidBody& body1, const SimdVector3& pos1,
161                       RigidBody& body2, const SimdVector3& pos2,
162                       SimdScalar depth, const SimdVector3& normal, 
163                                                         const ContactSolverInfo& solverInfo
164                                           )
165 {
166         
167         float normalLenSqr = normal.length2();
168         ASSERT2(fabs(normalLenSqr) < 1.1f);
169         if (normalLenSqr > 1.1f)
170                 return 0.f;
171
172         SimdVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
173         SimdVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
174         //this jacobian entry could be re-used for all iterations
175         
176         SimdVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
177         SimdVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
178         SimdVector3 vel = vel1 - vel2;
179         SimdScalar rel_vel;
180         rel_vel = normal.dot(vel);
181         
182 //      if (rel_vel< 0.f)//-SIMD_EPSILON) 
183 //      {
184         float combinedRestitution = body1.getRestitution() * body2.getRestitution();
185
186         SimdScalar rest = restitutionCurve(rel_vel, combinedRestitution);
187
188         SimdScalar timeCorrection =  360.f*solverInfo.m_timeStep;
189         float damping = solverInfo.m_damping ;
190         float tau = solverInfo.m_tau;
191
192         if (useGlobalSettingContacts)
193         {
194                 damping = contactDamping;
195                 tau = contactTau;
196         } 
197
198         if (depth < 0.f)
199                 return 0.f;//bdepth = 0.f;
200
201         SimdScalar penetrationImpulse = (depth*tau*timeCorrection);// * massTerm;//jacDiagABInv
202         
203         SimdScalar velocityImpulse = -(1.0f + rest) * damping * rel_vel;
204
205         SimdScalar impulse = penetrationImpulse + velocityImpulse;
206         SimdVector3 temp1 = body1.getInvInertiaTensorWorld() * rel_pos1.cross(normal); 
207                 SimdVector3 temp2 = body2.getInvInertiaTensorWorld() * rel_pos2.cross(normal); 
208                 impulse /=
209                         (body1.getInvMass() + body2.getInvMass() + normal.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
210         
211         if (impulse > 0.f)
212         {
213         
214                 body1.applyImpulse(normal*(impulse), rel_pos1);
215                 body2.applyImpulse(-normal*(impulse), rel_pos2);
216         } else
217         {
218                 impulse = 0.f;
219         }
220
221         return impulse;//velocityImpulse;//impulse;
222         
223 }
224
225
226
227 //velocity + friction
228 //response  between two dynamic objects with friction
229 float resolveSingleCollisionWithFriction(
230
231                 RigidBody& body1, 
232                 const SimdVector3& pos1,
233         RigidBody& body2, 
234                 const SimdVector3& pos2,
235         SimdScalar depth, 
236                 const SimdVector3& normal, 
237
238                 const ContactSolverInfo& solverInfo
239                 )
240 {
241         float normalLenSqr = normal.length2();
242         ASSERT2(fabs(normalLenSqr) < 1.1f);
243         if (normalLenSqr > 1.1f)
244                 return 0.f;
245
246         SimdVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
247         SimdVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
248         //this jacobian entry could be re-used for all iterations
249         
250         JacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
251                 body2.getCenterOfMassTransform().getBasis().transpose(),
252                 rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(),
253                 body2.getInvInertiaDiagLocal(),body2.getInvMass());
254
255         SimdScalar jacDiagAB = jac.getDiagonal();
256         SimdScalar jacDiagABInv = 1.f / jacDiagAB;
257         SimdVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
258         SimdVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
259         SimdVector3 vel = vel1 - vel2;
260 SimdScalar rel_vel;
261         /*      rel_vel = jac.getRelativeVelocity(
262                 body1.getLinearVelocity(),
263                 body1.getTransform().getBasis().transpose() * body1.getAngularVelocity(),
264                 body2.getLinearVelocity(),
265                 body2.getTransform().getBasis().transpose() * body2.getAngularVelocity()); 
266 */      
267         rel_vel = normal.dot(vel);
268         
269 //      if (rel_vel< 0.f)//-SIMD_EPSILON) 
270 //      {
271         float combinedRestitution = body1.getRestitution() * body2.getRestitution();
272
273         SimdScalar rest = restitutionCurve(rel_vel, combinedRestitution);
274 //      SimdScalar massTerm = 1.f / (body1.getInvMass() + body2.getInvMass());
275
276         SimdScalar timeCorrection = 0.5f / solverInfo.m_timeStep ;
277
278         float damping = solverInfo.m_damping ;
279         float tau = solverInfo.m_tau;
280
281         if (useGlobalSettingContacts)
282         {
283                 damping = contactDamping;
284                 tau = contactTau;
285         } 
286         SimdScalar penetrationImpulse = (depth* tau *timeCorrection)  * jacDiagABInv;
287                 
288                 if (penetrationImpulse  < 0.f)
289                         penetrationImpulse  = 0.f;
290
291
292
293         SimdScalar velocityImpulse = -(1.0f + rest) * damping * rel_vel * jacDiagABInv;
294
295         SimdScalar friction_impulse = 0.f;
296
297         if (velocityImpulse <= 0.f)
298                 velocityImpulse = 0.f;
299
300 //      SimdScalar impulse = penetrationImpulse + velocityImpulse;
301         //if (impulse > 0.f)
302         {
303 //              SimdVector3 impulse_vector = normal * impulse;
304                 body1.applyImpulse(normal*(velocityImpulse+penetrationImpulse), rel_pos1);
305                 
306                 body2.applyImpulse(-normal*(velocityImpulse+penetrationImpulse), rel_pos2);
307                 
308                 //friction
309
310                 {
311                 
312                                 SimdVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
313         SimdVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
314         SimdVector3 vel = vel1 - vel2;
315         
316         rel_vel = normal.dot(vel);
317
318 #define PER_CONTACT_FRICTION
319 #ifdef PER_CONTACT_FRICTION
320                 SimdVector3 lat_vel = vel - normal * rel_vel;
321                 SimdScalar lat_rel_vel = lat_vel.length();
322
323                 float combinedFriction = calculateCombinedFriction(body1,body2);
324
325                 if (lat_rel_vel > SIMD_EPSILON)
326                 {
327                         lat_vel /= lat_rel_vel;
328                         SimdVector3 temp1 = body1.getInvInertiaTensorWorld() * rel_pos1.cross(lat_vel); 
329                         SimdVector3 temp2 = body2.getInvInertiaTensorWorld() * rel_pos2.cross(lat_vel); 
330                          friction_impulse = lat_rel_vel / 
331                                 (body1.getInvMass() + body2.getInvMass() + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
332                         SimdScalar normal_impulse = (penetrationImpulse+
333                                 velocityImpulse) * combinedFriction;
334                         GEN_set_min(friction_impulse, normal_impulse);
335                         
336                         body1.applyImpulse(lat_vel * -friction_impulse, rel_pos1);
337                         body2.applyImpulse(lat_vel * friction_impulse, rel_pos2);
338                         
339                 }
340 #endif
341                 }
342         } 
343         return velocityImpulse + friction_impulse;
344 }