e3603ccec67929dfb6a29dca4823c56e05b87fce
[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.9f;
26 SimdScalar contactTau = .2f;//0.02f;//*0.02f;
27
28
29
30 SimdScalar restitutionCurve(SimdScalar rel_vel, SimdScalar restitution)
31 {
32         return restitution;
33 //      return restitution * GEN_min(1.0f, rel_vel / ContactThreshold);
34 }
35
36 void    applyFrictionInContactPointOld(RigidBody& body1, const SimdVector3& pos1,
37                       RigidBody& body2, const SimdVector3& pos2,
38                 const SimdVector3& normal,float normalImpulse, 
39                 const ContactSolverInfo& solverInfo)
40 {
41
42
43         if (normalImpulse>0.f)
44         {
45                 SimdVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
46                 SimdVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
47
48                 SimdVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
49                 SimdVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
50                 SimdVector3 vel = vel1 - vel2;
51         
52                 SimdScalar rel_vel = normal.dot(vel);
53
54 #define PER_CONTACT_FRICTION
55 #ifdef PER_CONTACT_FRICTION
56                 SimdVector3 lat_vel = vel - normal * rel_vel;
57                 SimdScalar lat_rel_vel = lat_vel.length();
58
59                 if (lat_rel_vel > SIMD_EPSILON)
60                 {
61                         lat_vel /= lat_rel_vel;
62                         SimdVector3 temp1 = body1.getInvInertiaTensorWorld() * rel_pos1.cross(lat_vel); 
63                         SimdVector3 temp2 = body2.getInvInertiaTensorWorld() * rel_pos2.cross(lat_vel); 
64                         SimdScalar frictionMaxImpulse = lat_rel_vel / 
65                                 (body1.getInvMass() + body2.getInvMass() + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
66                         SimdScalar frictionImpulse = (normalImpulse) * solverInfo.m_friction;
67                         GEN_set_min(frictionImpulse,frictionMaxImpulse );
68                         
69                         body1.applyImpulse(lat_vel * -frictionImpulse, rel_pos1);
70                         body2.applyImpulse(lat_vel * frictionImpulse, rel_pos2);
71                         
72                 }
73 #endif
74                 }
75 }
76
77
78 //bilateral constraint between two dynamic objects
79 void resolveSingleBilateral(RigidBody& body1, const SimdVector3& pos1,
80                       RigidBody& body2, const SimdVector3& pos2,
81                       SimdScalar depth, const SimdVector3& normal,SimdScalar& impulse ,float timeStep)
82 {
83         float normalLenSqr = normal.length2();
84         ASSERT2(fabs(normalLenSqr) < 1.1f);
85         if (normalLenSqr > 1.1f)
86         {
87                 impulse = 0.f;
88                 return;
89         }
90         SimdVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
91         SimdVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
92         //this jacobian entry could be re-used for all iterations
93         
94         SimdVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
95         SimdVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
96         SimdVector3 vel = vel1 - vel2;
97         
98         SimdScalar rel_vel;
99
100         /*
101         
102           JacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
103                 body2.getCenterOfMassTransform().getBasis().transpose(),
104                 rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(),
105                 body2.getInvInertiaDiagLocal(),body2.getInvMass());
106
107         SimdScalar jacDiagAB = jac.getDiagonal();
108         SimdScalar jacDiagABInv = 1.f / jacDiagAB;
109         
110           SimdScalar rel_vel = jac.getRelativeVelocity(
111                 body1.getLinearVelocity(),
112                 body1.getCenterOfMassTransform().getBasis().transpose() * body1.getAngularVelocity(),
113                 body2.getLinearVelocity(),
114                 body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity()); 
115         float a;
116         a=jacDiagABInv;
117
118         */
119         rel_vel = normal.dot(vel);
120         
121
122
123
124         /*int color = 255+255*256;
125
126         DrawRasterizerLine(pos1,pos1+normal,color);
127 */
128
129                 
130         SimdScalar massTerm = 1.f / (body1.getInvMass() + body2.getInvMass());
131
132         float contactDamping = 0.9f;
133         impulse = - contactDamping * rel_vel * massTerm;//jacDiagABInv;
134
135         //SimdScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
136         //impulse = velocityImpulse;
137
138 }
139
140
141
142
143
144 //velocity + friction
145 //response  between two dynamic objects with friction
146 float resolveSingleCollision(RigidBody& body1, const SimdVector3& pos1,
147                       RigidBody& body2, const SimdVector3& pos2,
148                       SimdScalar depth, const SimdVector3& normal, 
149                                                         const ContactSolverInfo& solverInfo
150                                           )
151 {
152         
153         float normalLenSqr = normal.length2();
154         ASSERT2(fabs(normalLenSqr) < 1.1f);
155         if (normalLenSqr > 1.1f)
156                 return 0.f;
157
158         SimdVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
159         SimdVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
160         //this jacobian entry could be re-used for all iterations
161         
162         SimdVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
163         SimdVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
164         SimdVector3 vel = vel1 - vel2;
165         SimdScalar rel_vel;
166         rel_vel = normal.dot(vel);
167         
168 //      if (rel_vel< 0.f)//-SIMD_EPSILON) 
169 //      {
170         SimdScalar rest = restitutionCurve(rel_vel, solverInfo.m_restitution);
171
172         SimdScalar timeCorrection =  1.f;///timeStep;
173         float damping = solverInfo.m_damping ;
174         float tau = solverInfo.m_tau;
175
176         if (useGlobalSettingContacts)
177         {
178                 damping = contactDamping;
179                 tau = contactTau;
180         } 
181
182         if (depth < 0.f)
183                 return 0.f;//bdepth = 0.f;
184
185         SimdScalar penetrationImpulse = (depth*tau*timeCorrection);// * massTerm;//jacDiagABInv
186         
187         SimdScalar velocityImpulse = -(1.0f + rest) * damping * rel_vel;
188
189         SimdScalar impulse = penetrationImpulse + velocityImpulse;
190         SimdVector3 temp1 = body1.getInvInertiaTensorWorld() * rel_pos1.cross(normal); 
191                 SimdVector3 temp2 = body2.getInvInertiaTensorWorld() * rel_pos2.cross(normal); 
192                 impulse /=
193                         (body1.getInvMass() + body2.getInvMass() + normal.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
194         
195         if (impulse > 0.f)
196         {
197         
198                 body1.applyImpulse(normal*(impulse), rel_pos1);
199                 body2.applyImpulse(-normal*(impulse), rel_pos2);
200         } else
201         {
202                 impulse = 0.f;
203         }
204
205         return impulse;//velocityImpulse;//impulse;
206         
207 }
208
209
210
211 //velocity + friction
212 //response  between two dynamic objects with friction
213 float resolveSingleCollisionWithFriction(
214
215                 RigidBody& body1, 
216                 const SimdVector3& pos1,
217         RigidBody& body2, 
218                 const SimdVector3& pos2,
219         SimdScalar depth, 
220                 const SimdVector3& normal, 
221
222                 const ContactSolverInfo& solverInfo
223                 )
224 {
225         float normalLenSqr = normal.length2();
226         ASSERT2(fabs(normalLenSqr) < 1.1f);
227         if (normalLenSqr > 1.1f)
228                 return 0.f;
229
230         SimdVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
231         SimdVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
232         //this jacobian entry could be re-used for all iterations
233         
234         JacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
235                 body2.getCenterOfMassTransform().getBasis().transpose(),
236                 rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(),
237                 body2.getInvInertiaDiagLocal(),body2.getInvMass());
238
239         SimdScalar jacDiagAB = jac.getDiagonal();
240         SimdScalar jacDiagABInv = 1.f / jacDiagAB;
241         SimdVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
242         SimdVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
243         SimdVector3 vel = vel1 - vel2;
244 SimdScalar rel_vel;
245         /*      rel_vel = jac.getRelativeVelocity(
246                 body1.getLinearVelocity(),
247                 body1.getTransform().getBasis().transpose() * body1.getAngularVelocity(),
248                 body2.getLinearVelocity(),
249                 body2.getTransform().getBasis().transpose() * body2.getAngularVelocity()); 
250 */      
251         rel_vel = normal.dot(vel);
252         
253 //      if (rel_vel< 0.f)//-SIMD_EPSILON) 
254 //      {
255         SimdScalar rest = restitutionCurve(rel_vel, solverInfo.m_restitution);
256 //      SimdScalar massTerm = 1.f / (body1.getInvMass() + body2.getInvMass());
257
258         SimdScalar timeCorrection = 0.5f / solverInfo.m_timeStep ;
259
260         float damping = solverInfo.m_damping ;
261         float tau = solverInfo.m_tau;
262
263         if (useGlobalSettingContacts)
264         {
265                 damping = contactDamping;
266                 tau = contactTau;
267         } 
268         SimdScalar penetrationImpulse = (depth* tau *timeCorrection)  * jacDiagABInv;
269                 
270                 if (penetrationImpulse  < 0.f)
271                         penetrationImpulse  = 0.f;
272
273
274
275         SimdScalar velocityImpulse = -(1.0f + rest) * damping * rel_vel * jacDiagABInv;
276
277         SimdScalar friction_impulse = 0.f;
278
279         if (velocityImpulse <= 0.f)
280                 velocityImpulse = 0.f;
281
282 //      SimdScalar impulse = penetrationImpulse + velocityImpulse;
283         //if (impulse > 0.f)
284         {
285 //              SimdVector3 impulse_vector = normal * impulse;
286                 body1.applyImpulse(normal*(velocityImpulse+penetrationImpulse), rel_pos1);
287                 
288                 body2.applyImpulse(-normal*(velocityImpulse+penetrationImpulse), rel_pos2);
289                 
290                 //friction
291
292                 {
293                 
294                                 SimdVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
295         SimdVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
296         SimdVector3 vel = vel1 - vel2;
297         
298         rel_vel = normal.dot(vel);
299
300 #define PER_CONTACT_FRICTION
301 #ifdef PER_CONTACT_FRICTION
302                 SimdVector3 lat_vel = vel - normal * rel_vel;
303                 SimdScalar lat_rel_vel = lat_vel.length();
304
305                 if (lat_rel_vel > SIMD_EPSILON)
306                 {
307                         lat_vel /= lat_rel_vel;
308                         SimdVector3 temp1 = body1.getInvInertiaTensorWorld() * rel_pos1.cross(lat_vel); 
309                         SimdVector3 temp2 = body2.getInvInertiaTensorWorld() * rel_pos2.cross(lat_vel); 
310                          friction_impulse = lat_rel_vel / 
311                                 (body1.getInvMass() + body2.getInvMass() + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
312                         SimdScalar normal_impulse = (penetrationImpulse+
313                                 velocityImpulse) * solverInfo.m_friction;
314                         GEN_set_min(friction_impulse, normal_impulse);
315                         
316                         body1.applyImpulse(lat_vel * -friction_impulse, rel_pos1);
317                         body2.applyImpulse(lat_vel * friction_impulse, rel_pos2);
318                         
319                 }
320 #endif
321                 }
322         } 
323         return velocityImpulse + friction_impulse;
324 }