Fixed several bugs: python refcounting related and Bullet related (basic add/remove...
[blender.git] / extern / bullet / BulletDynamics / ConstraintSolver / ContactConstraint.cpp
1 /*
2  * Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/
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 //some values to find stable penalty method tresholds
21 float MAX_FRICTION = 100.f;
22 static SimdScalar ContactThreshold = -10.0f;  
23 float useGlobalSettingContacts = false;//true;
24 SimdScalar contactDamping = 0.2f;
25 SimdScalar contactTau = .02f;//0.02f;//*0.02f;
26
27
28 SimdScalar restitutionCurve(SimdScalar rel_vel, SimdScalar restitution)
29 {
30         return 0.f;
31 //      return restitution * GEN_min(1.0f, rel_vel / ContactThreshold);
32 }
33
34
35 SimdScalar      calculateCombinedFriction(RigidBody& body0,RigidBody& body1)
36 {
37         SimdScalar friction = body0.getFriction() * body1.getFriction();
38         if (friction < 0.f)
39                 friction = 0.f;
40         if (friction > MAX_FRICTION)
41                 friction = MAX_FRICTION;
42         return friction;
43
44 }
45
46
47 //bilateral constraint between two dynamic objects
48 void resolveSingleBilateral(RigidBody& body1, const SimdVector3& pos1,
49                       RigidBody& body2, const SimdVector3& pos2,
50                       SimdScalar distance, const SimdVector3& normal,SimdScalar& impulse ,float timeStep)
51 {
52         float normalLenSqr = normal.length2();
53         ASSERT2(fabs(normalLenSqr) < 1.1f);
54         if (normalLenSqr > 1.1f)
55         {
56                 impulse = 0.f;
57                 return;
58         }
59         SimdVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
60         SimdVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
61         //this jacobian entry could be re-used for all iterations
62         
63         SimdVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
64         SimdVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
65         SimdVector3 vel = vel1 - vel2;
66         
67
68           JacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
69                 body2.getCenterOfMassTransform().getBasis().transpose(),
70                 rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(),
71                 body2.getInvInertiaDiagLocal(),body2.getInvMass());
72
73         SimdScalar jacDiagAB = jac.getDiagonal();
74         SimdScalar jacDiagABInv = 1.f / jacDiagAB;
75         
76           SimdScalar rel_vel = jac.getRelativeVelocity(
77                 body1.getLinearVelocity(),
78                 body1.getCenterOfMassTransform().getBasis().transpose() * body1.getAngularVelocity(),
79                 body2.getLinearVelocity(),
80                 body2.getCenterOfMassTransform().getBasis().transpose() * body2.getAngularVelocity()); 
81         float a;
82         a=jacDiagABInv;
83
84
85         rel_vel = normal.dot(vel);
86                 
87
88 #ifdef ONLY_USE_LINEAR_MASS
89         SimdScalar massTerm = 1.f / (body1.getInvMass() + body2.getInvMass());
90         impulse = - contactDamping * rel_vel * massTerm;
91 #else   
92         SimdScalar velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
93         impulse = velocityImpulse;
94 #endif
95 }
96
97
98
99
100 //velocity + friction
101 //response  between two dynamic objects with friction
102 float resolveSingleCollisionWithFriction(
103
104                 RigidBody& body1, 
105                 const SimdVector3& pos1,
106         RigidBody& body2, 
107                 const SimdVector3& pos2,
108         SimdScalar distance, 
109                 const SimdVector3& normal, 
110
111                 const ContactSolverInfo& solverInfo
112                 )
113 {
114         float normalLenSqr = normal.length2();
115         ASSERT2(fabs(normalLenSqr) < 1.1f);
116         if (normalLenSqr > 1.1f)
117                 return 0.f;
118
119         SimdVector3 rel_pos1 = pos1 - body1.getCenterOfMassPosition(); 
120         SimdVector3 rel_pos2 = pos2 - body2.getCenterOfMassPosition();
121         //this jacobian entry could be re-used for all iterations
122         
123         JacobianEntry jac(body1.getCenterOfMassTransform().getBasis().transpose(),
124                 body2.getCenterOfMassTransform().getBasis().transpose(),
125                 rel_pos1,rel_pos2,normal,body1.getInvInertiaDiagLocal(),body1.getInvMass(),
126                 body2.getInvInertiaDiagLocal(),body2.getInvMass());
127
128         SimdScalar jacDiagAB = jac.getDiagonal();
129         SimdScalar jacDiagABInv = 1.f / jacDiagAB;
130         SimdVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
131         SimdVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
132         SimdVector3 vel = vel1 - vel2;
133 SimdScalar rel_vel;
134         /*      rel_vel = jac.getRelativeVelocity(
135                 body1.getLinearVelocity(),
136                 body1.getTransform().getBasis().transpose() * body1.getAngularVelocity(),
137                 body2.getLinearVelocity(),
138                 body2.getTransform().getBasis().transpose() * body2.getAngularVelocity()); 
139 */      
140         rel_vel = normal.dot(vel);
141         
142         float combinedRestitution = body1.getRestitution() * body2.getRestitution();
143
144         SimdScalar restitution = restitutionCurve(rel_vel, combinedRestitution);
145
146         SimdScalar Kfps = 1.f / solverInfo.m_timeStep ;
147
148         float damping = solverInfo.m_damping ;
149         float Kerp = solverInfo.m_tau;
150         
151         if (useGlobalSettingContacts)
152         {
153                 damping = contactDamping;
154                 Kerp = contactTau;
155         } 
156
157         float Kcor = Kerp *Kfps;
158
159
160         SimdScalar positionalError = Kcor *-distance;
161         //jacDiagABInv;
162         SimdScalar velocityError = -(1.0f + restitution) * damping * rel_vel;
163
164         SimdScalar penetrationImpulse = positionalError * jacDiagABInv;
165
166         SimdScalar      velocityImpulse = velocityError * jacDiagABInv;
167
168         SimdScalar friction_impulse = 0.f;
169
170         SimdScalar totalimpulse = penetrationImpulse+velocityImpulse;
171
172         if (totalimpulse > 0.f)
173         {
174 //              SimdVector3 impulse_vector = normal * impulse;
175                 body1.applyImpulse(normal*(velocityImpulse+penetrationImpulse), rel_pos1);
176                 
177                 body2.applyImpulse(-normal*(velocityImpulse+penetrationImpulse), rel_pos2);
178                 
179                 //friction
180
181                 {
182                 
183                 SimdVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
184                 SimdVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
185                 SimdVector3 vel = vel1 - vel2;
186                 
187                 rel_vel = normal.dot(vel);
188
189 #define PER_CONTACT_FRICTION
190 #ifdef PER_CONTACT_FRICTION
191                 SimdVector3 lat_vel = vel - normal * rel_vel;
192                 SimdScalar lat_rel_vel = lat_vel.length();
193
194                 float combinedFriction = calculateCombinedFriction(body1,body2);
195
196                 if (lat_rel_vel > SIMD_EPSILON)
197                 {
198                         lat_vel /= lat_rel_vel;
199                         SimdVector3 temp1 = body1.getInvInertiaTensorWorld() * rel_pos1.cross(lat_vel); 
200                         SimdVector3 temp2 = body2.getInvInertiaTensorWorld() * rel_pos2.cross(lat_vel); 
201                          friction_impulse = lat_rel_vel / 
202                                 (body1.getInvMass() + body2.getInvMass() + lat_vel.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
203                         SimdScalar normal_impulse = (penetrationImpulse+
204                                 velocityImpulse) * combinedFriction;
205                         GEN_set_min(friction_impulse, normal_impulse);
206                         
207                         body1.applyImpulse(lat_vel * -friction_impulse, rel_pos1);
208                         body2.applyImpulse(lat_vel * friction_impulse, rel_pos2);
209                         
210                 }
211 #endif
212                 }
213         } 
214         return velocityImpulse + friction_impulse;
215 }