angular-only hinge updated
[blender.git] / extern / bullet2 / src / BulletDynamics / ConstraintSolver / btGeneric6DofConstraint.cpp
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
4
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages 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 freely, 
9 subject to the following restrictions:
10
11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
15
16
17 #include "btGeneric6DofConstraint.h"
18 #include "BulletDynamics/Dynamics/btRigidBody.h"
19 #include "LinearMath/btTransformUtil.h"
20
21 static const btScalar kSign[] = { 1.0f, -1.0f, 1.0f };
22 static const int kAxisA[] = { 1, 0, 0 };
23 static const int kAxisB[] = { 2, 2, 1 };
24 #define GENERIC_D6_DISABLE_WARMSTARTING 1
25
26 btGeneric6DofConstraint::btGeneric6DofConstraint()
27 {
28 }
29
30 btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB)
31 : btTypedConstraint(rbA, rbB)
32 , m_frameInA(frameInA)
33 , m_frameInB(frameInB)
34 {
35         //free means upper < lower, 
36         //locked means upper == lower
37         //limited means upper > lower
38         //so start all locked
39         for (int i=0; i<6;++i)
40         {
41                 m_lowerLimit[i] = 0.0f;
42                 m_upperLimit[i] = 0.0f;
43                 m_accumulatedImpulse[i] = 0.0f;
44         }
45
46 }
47
48
49 void btGeneric6DofConstraint::buildJacobian()
50 {
51         btVector3       localNormalInA(0,0,0);
52
53         const btVector3& pivotInA = m_frameInA.getOrigin();
54         const btVector3& pivotInB = m_frameInB.getOrigin();
55
56         btVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_frameInA.getOrigin();
57         btVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_frameInB.getOrigin();
58
59         btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
60         btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
61
62         int i;
63         //linear part
64         for (i=0;i<3;i++)
65         {
66                 if (isLimited(i))
67                 {
68                         localNormalInA[i] = 1;
69                         btVector3 normalWorld = m_rbA.getCenterOfMassTransform().getBasis() * localNormalInA;
70
71                         
72                         // Create linear atom
73                         new (&m_jacLinear[i]) btJacobianEntry(
74                                 m_rbA.getCenterOfMassTransform().getBasis().transpose(),
75                                 m_rbB.getCenterOfMassTransform().getBasis().transpose(),
76                                 m_rbA.getCenterOfMassTransform()*pivotInA - m_rbA.getCenterOfMassPosition(),
77                                 m_rbB.getCenterOfMassTransform()*pivotInB - m_rbB.getCenterOfMassPosition(),
78                                 normalWorld,
79                                 m_rbA.getInvInertiaDiagLocal(),
80                                 m_rbA.getInvMass(),
81                                 m_rbB.getInvInertiaDiagLocal(),
82                                 m_rbB.getInvMass());
83
84                         //optionally disable warmstarting
85 #ifdef GENERIC_D6_DISABLE_WARMSTARTING
86                         m_accumulatedImpulse[i] = 0.f;
87 #endif //GENERIC_D6_DISABLE_WARMSTARTING
88
89                         // Apply accumulated impulse
90                         btVector3 impulse_vector = m_accumulatedImpulse[i] * normalWorld;
91
92                         m_rbA.applyImpulse( impulse_vector, rel_pos1);
93                         m_rbB.applyImpulse(-impulse_vector, rel_pos2);
94
95                         localNormalInA[i] = 0;
96                 }
97         }
98
99         // angular part
100         for (i=0;i<3;i++)
101         {
102                 if (isLimited(i+3))
103                 {
104                         btVector3 axisA = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn( kAxisA[i] );
105                         btVector3 axisB = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn( kAxisB[i] );
106
107                         // Dirk: This is IMO mathematically the correct way, but we should consider axisA and axisB being near parallel maybe
108                         btVector3 axis = kSign[i] * axisA.cross(axisB);
109
110                         // Create angular atom
111                         new (&m_jacAng[i])      btJacobianEntry(axis,
112                                 m_rbA.getCenterOfMassTransform().getBasis().transpose(),
113                                 m_rbB.getCenterOfMassTransform().getBasis().transpose(),
114                                 m_rbA.getInvInertiaDiagLocal(),
115                                 m_rbB.getInvInertiaDiagLocal());
116
117 #ifdef GENERIC_D6_DISABLE_WARMSTARTING
118                         m_accumulatedImpulse[i + 3] = 0.f;
119 #endif //GENERIC_D6_DISABLE_WARMSTARTING
120
121                         // Apply accumulated impulse
122                         btVector3 impulse_vector = m_accumulatedImpulse[i + 3] * axis;
123
124                         m_rbA.applyTorqueImpulse( impulse_vector);
125                         m_rbB.applyTorqueImpulse(-impulse_vector);
126                 }
127         }
128 }
129
130 float getMatrixElem(const btMatrix3x3& mat,int index)
131 {
132         int row = index%3;
133         int col = index / 3;
134         return mat[row][col];
135 }
136
137 ///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
138 bool    MatrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz)
139 {
140     // rot =  cy*cz          -cy*sz           sy
141     //        cz*sx*sy+cx*sz  cx*cz-sx*sy*sz -cy*sx
142     //       -cx*cz*sy+sx*sz  cz*sx+cx*sy*sz  cx*cy
143
144 ///     0..8
145
146          if (getMatrixElem(mat,2) < 1.0f)
147     {
148         if (getMatrixElem(mat,2) > -1.0f)
149         {
150             xyz[0] = btAtan2(-getMatrixElem(mat,5),getMatrixElem(mat,8));
151             xyz[1] = btAsin(getMatrixElem(mat,2));
152             xyz[2] = btAtan2(-getMatrixElem(mat,1),getMatrixElem(mat,0));
153             return true;
154         }
155         else
156         {
157             // WARNING.  Not unique.  XA - ZA = -atan2(r10,r11)
158             xyz[0] = -btAtan2(getMatrixElem(mat,3),getMatrixElem(mat,4));
159             xyz[1] = -SIMD_HALF_PI;
160             xyz[2] = 0.0f;
161             return false;
162         }
163     }
164     else
165     {
166         // WARNING.  Not unique.  XAngle + ZAngle = atan2(r10,r11)
167         xyz[0] = btAtan2(getMatrixElem(mat,3),getMatrixElem(mat,4));
168         xyz[1] = SIMD_HALF_PI;
169         xyz[2] = 0.0;
170         return false;
171     }
172          return false;
173 }
174
175
176 void    btGeneric6DofConstraint::solveConstraint(btScalar       timeStep)
177 {
178         btScalar tau = 0.1f;
179         btScalar damping = 1.0f;
180
181         btVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_frameInA.getOrigin();
182         btVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_frameInB.getOrigin();
183
184         btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
185         btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
186         
187         btVector3 localNormalInA(0,0,0);
188         int i;
189
190         // linear
191         for (i=0;i<3;i++)
192         {               
193                 if (isLimited(i))
194                 {
195                         btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
196                         btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
197                 
198                         localNormalInA.setValue(0,0,0);
199                         localNormalInA[i] = 1;
200                         btVector3 normalWorld = m_rbA.getCenterOfMassTransform().getBasis() * localNormalInA;
201
202                         btScalar jacDiagABInv = 1.f / m_jacLinear[i].getDiagonal();
203
204                         //velocity error (first order error)
205                         btScalar rel_vel = m_jacLinear[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, 
206                                                                                                                                         m_rbB.getLinearVelocity(),angvelB);
207                 
208                         //positional error (zeroth order error)
209                         btScalar depth = -(pivotAInW - pivotBInW).dot(normalWorld); 
210                         btScalar        lo = -1e30f;
211                         btScalar        hi = 1e30f;
212                 
213                         //handle the limits
214                         if (m_lowerLimit[i] < m_upperLimit[i])
215                         {
216                                 {
217                                         if (depth > m_upperLimit[i])
218                                         {
219                                                 depth -= m_upperLimit[i];
220                                                 lo = 0.f;
221                                         
222                                         } else
223                                         {
224                                                 if (depth < m_lowerLimit[i])
225                                                 {
226                                                         depth -= m_lowerLimit[i];
227                                                         hi = 0.f;
228                                                 } else
229                                                 {
230                                                         continue;
231                                                 }
232                                         }
233                                 }
234                         }
235
236                         btScalar normalImpulse= (tau*depth/timeStep - damping*rel_vel) * jacDiagABInv;
237                         float oldNormalImpulse = m_accumulatedImpulse[i];
238                         float sum = oldNormalImpulse + normalImpulse;
239                         m_accumulatedImpulse[i] = sum > hi ? 0.f : sum < lo ? 0.f : sum;
240                         normalImpulse = m_accumulatedImpulse[i] - oldNormalImpulse;
241
242                         btVector3 impulse_vector = normalWorld * normalImpulse;
243                         m_rbA.applyImpulse( impulse_vector, rel_pos1);
244                         m_rbB.applyImpulse(-impulse_vector, rel_pos2);
245                         
246                         localNormalInA[i] = 0;
247                 }
248         }
249
250         btVector3       axis;
251         btScalar        angle;
252         btTransform     frameAWorld = m_rbA.getCenterOfMassTransform() * m_frameInA;
253         btTransform     frameBWorld = m_rbB.getCenterOfMassTransform() * m_frameInB;
254
255         btTransformUtil::calculateDiffAxisAngle(frameAWorld,frameBWorld,axis,angle);
256         btQuaternion diff(axis,angle);
257         btMatrix3x3 diffMat (diff);
258         btVector3 xyz;
259         ///this is not perfect, we can first check which axis are limited, and choose a more appropriate order
260         MatrixToEulerXYZ(diffMat,xyz);
261
262         // angular
263         for (i=0;i<3;i++)
264         {
265                 if (isLimited(i+3))
266                 {
267                         btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
268                         btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
269                 
270                         btScalar jacDiagABInv = 1.f / m_jacAng[i].getDiagonal();
271                         
272                         //velocity error (first order error)
273                         btScalar rel_vel = m_jacAng[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, 
274                                                                                                                                                         m_rbB.getLinearVelocity(),angvelB);
275
276                         //positional error (zeroth order error)
277                         btVector3 axisA = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn( kAxisA[i] );
278                         btVector3 axisB = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn( kAxisB[i] );
279
280                         btScalar rel_pos = kSign[i] * axisA.dot(axisB);
281
282                         btScalar        lo = -1e30f;
283                         btScalar        hi = 1e30f;
284                 
285                         //handle the twist limit
286                         if (m_lowerLimit[i+3] < m_upperLimit[i+3])
287                         {
288                                 //clamp the values
289                                 btScalar loLimit =  m_upperLimit[i+3] > -3.1415 ? m_lowerLimit[i+3] : -1e30f;
290                                 btScalar hiLimit = m_upperLimit[i+3] < 3.1415 ? m_upperLimit[i+3] : 1e30f;
291
292                                 float projAngle  = -2.*xyz[i];
293                                 
294                                 if (projAngle < loLimit)
295                                 {
296                                         hi = 0.f;
297                                         rel_pos = (loLimit - projAngle);
298                                 } else
299                                 {
300                                         if (projAngle > hiLimit)
301                                         {
302                                                 lo = 0.f;
303                                                 rel_pos = (hiLimit - projAngle);
304                                         } else
305                                         {
306                                                 continue;
307                                         }
308                                 }
309                         }
310                 
311                         //impulse
312                         
313                         btScalar normalImpulse= -(tau*rel_pos/timeStep + damping*rel_vel) * jacDiagABInv;
314                         float oldNormalImpulse = m_accumulatedImpulse[i+3];
315                         float sum = oldNormalImpulse + normalImpulse;
316                         m_accumulatedImpulse[i+3] = sum > hi ? 0.f : sum < lo ? 0.f : sum;
317                         normalImpulse = m_accumulatedImpulse[i+3] - oldNormalImpulse;
318                         
319                         // Dirk: Not needed - we could actually project onto Jacobian entry here (same as above)
320                         btVector3 axis = kSign[i] * axisA.cross(axisB);
321                         btVector3 impulse_vector = axis * normalImpulse;
322
323                         m_rbA.applyTorqueImpulse( impulse_vector);
324                         m_rbB.applyTorqueImpulse(-impulse_vector);
325                 }
326         }
327 }
328
329 void    btGeneric6DofConstraint::updateRHS(btScalar     timeStep)
330 {
331
332 }
333
334 btScalar btGeneric6DofConstraint::computeAngle(int axis) const
335         {
336         btScalar angle;
337
338         switch (axis)
339                 {
340                 case 0:
341                         {
342                         btVector3 v1 = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn(1);
343                         btVector3 v2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(1);
344                         btVector3 w2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(2);
345
346                         btScalar s = v1.dot(w2);
347                         btScalar c = v1.dot(v2);
348
349                         angle = btAtan2( s, c );
350                         }
351                         break;
352
353                 case 1:
354                         {
355                         btVector3 w1 = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn(2);
356                         btVector3 w2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(2);
357                         btVector3 u2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(0);
358
359                         btScalar s = w1.dot(u2);
360                         btScalar c = w1.dot(w2);
361
362                         angle = btAtan2( s, c );
363                         }
364                         break;
365
366                 case 2:
367                         {
368                         btVector3 u1 = m_rbA.getCenterOfMassTransform().getBasis() * m_frameInA.getBasis().getColumn(0);
369                         btVector3 u2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(0);
370                         btVector3 v2 = m_rbB.getCenterOfMassTransform().getBasis() * m_frameInB.getBasis().getColumn(1);
371
372                         btScalar s = u1.dot(v2);
373                         btScalar c = u1.dot(u2);
374
375                         angle = btAtan2( s, c );
376                         }
377                         break;
378                   default: assert ( 0 ) ; break ;
379                 }
380
381         return angle;
382         }
383