Bullet Collision Detection & Physics Library
btGeneric6DofSpring2Constraint.cpp
Go to the documentation of this file.
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 2014 May: btGeneric6DofSpring2Constraint is created from the original (2.82.2712) btGeneric6DofConstraint by Gabor Puhr and Tamas Umenhoffer
18 Pros:
19 - Much more accurate and stable in a lot of situation. (Especially when a sleeping chain of RBs connected with 6dof2 is pulled)
20 - Stable and accurate spring with minimal energy loss that works with all of the solvers. (latter is not true for the original 6dof spring)
21 - Servo motor functionality
22 - Much more accurate bouncing. 0 really means zero bouncing (not true for the original 6odf) and there is only a minimal energy loss when the value is 1 (because of the solvers' precision)
23 - Rotation order for the Euler system can be set. (One axis' freedom is still limited to pi/2)
24 
25 Cons:
26 - It is slower than the original 6dof. There is no exact ratio, but half speed is a good estimation. (with PGS)
27 - At bouncing the correct velocity is calculated, but not the correct position. (it is because of the solver can correct position or velocity, but not both.)
28 */
29 
32 
33 /*
34 2007-09-09
35 btGeneric6DofConstraint Refactored by Francisco Le?n
36 email: projectileman@yahoo.com
37 http://gimpact.sf.net
38 */
39 
40 
41 
45 #include <new>
46 
47 
48 
51  , m_frameInA(frameInA)
52  , m_frameInB(frameInB)
53  , m_rotateOrder(rotOrder)
54  , m_flags(0)
55 {
57 }
58 
59 
61  : btTypedConstraint(D6_SPRING_2_CONSTRAINT_TYPE, getFixedBody(), rbB)
62  , m_frameInB(frameInB)
63  , m_rotateOrder(rotOrder)
64  , m_flags(0)
65 {
69 }
70 
71 
73 {
74  int i = index%3;
75  int j = index/3;
76  return mat[i][j];
77 }
78 
79 // MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
80 
82 {
83  // rot = cy*cz -cy*sz sy
84  // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx
85  // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy
86 
87  btScalar fi = btGetMatrixElem(mat,2);
88  if (fi < btScalar(1.0f))
89  {
90  if (fi > btScalar(-1.0f))
91  {
92  xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8));
93  xyz[1] = btAsin(btGetMatrixElem(mat,2));
94  xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
95  return true;
96  }
97  else
98  {
99  // WARNING. Not unique. XA - ZA = -atan2(r10,r11)
100  xyz[0] = -btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
101  xyz[1] = -SIMD_HALF_PI;
102  xyz[2] = btScalar(0.0);
103  return false;
104  }
105  }
106  else
107  {
108  // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11)
109  xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
110  xyz[1] = SIMD_HALF_PI;
111  xyz[2] = 0.0;
112  }
113  return false;
114 }
115 
117 {
118  // rot = cy*cz -sz sy*cz
119  // cy*cx*sz+sx*sy cx*cz sy*cx*sz-cy*sx
120  // cy*sx*sz-cx*sy sx*cz sy*sx*sz+cx*cy
121 
122  btScalar fi = btGetMatrixElem(mat,1);
123  if (fi < btScalar(1.0f))
124  {
125  if (fi > btScalar(-1.0f))
126  {
127  xyz[0] = btAtan2(btGetMatrixElem(mat,7),btGetMatrixElem(mat,4));
128  xyz[1] = btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,0));
129  xyz[2] = btAsin(-btGetMatrixElem(mat,1));
130  return true;
131  }
132  else
133  {
134  xyz[0] = -btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,8));
135  xyz[1] = btScalar(0.0);
136  xyz[2] = SIMD_HALF_PI;
137  return false;
138  }
139  }
140  else
141  {
142  xyz[0] = btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,8));
143  xyz[1] = 0.0;
144  xyz[2] = -SIMD_HALF_PI;
145  }
146  return false;
147 }
148 
150 {
151  // rot = cy*cz+sy*sx*sz cz*sy*sx-cy*sz cx*sy
152  // cx*sz cx*cz -sx
153  // cy*sx*sz-cz*sy sy*sz+cy*cz*sx cy*cx
154 
155  btScalar fi = btGetMatrixElem(mat,5);
156  if (fi < btScalar(1.0f))
157  {
158  if (fi > btScalar(-1.0f))
159  {
160  xyz[0] = btAsin(-btGetMatrixElem(mat,5));
161  xyz[1] = btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,8));
162  xyz[2] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
163  return true;
164  }
165  else
166  {
167  xyz[0] = SIMD_HALF_PI;
168  xyz[1] = -btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
169  xyz[2] = btScalar(0.0);
170  return false;
171  }
172  }
173  else
174  {
175  xyz[0] = -SIMD_HALF_PI;
176  xyz[1] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
177  xyz[2] = 0.0;
178  }
179  return false;
180 }
181 
183 {
184  // rot = cy*cz sy*sx-cy*cx*sz cx*sy+cy*sz*sx
185  // sz cz*cx -cz*sx
186  // -cz*sy cy*sx+cx*sy*sz cy*cx-sy*sz*sx
187 
188  btScalar fi = btGetMatrixElem(mat,3);
189  if (fi < btScalar(1.0f))
190  {
191  if (fi > btScalar(-1.0f))
192  {
193  xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,4));
194  xyz[1] = btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,0));
195  xyz[2] = btAsin(btGetMatrixElem(mat,3));
196  return true;
197  }
198  else
199  {
200  xyz[0] = btScalar(0.0);
201  xyz[1] = -btAtan2(btGetMatrixElem(mat,7),btGetMatrixElem(mat,8));
202  xyz[2] = -SIMD_HALF_PI;
203  return false;
204  }
205  }
206  else
207  {
208  xyz[0] = btScalar(0.0);
209  xyz[1] = btAtan2(btGetMatrixElem(mat,7),btGetMatrixElem(mat,8));
210  xyz[2] = SIMD_HALF_PI;
211  }
212  return false;
213 }
214 
216 {
217  // rot = cz*cy-sz*sx*sy -cx*sz cz*sy+cy*sz*sx
218  // cy*sz+cz*sx*sy cz*cx sz*sy-cz*xy*sx
219  // -cx*sy sx cx*cy
220 
221  btScalar fi = btGetMatrixElem(mat,7);
222  if (fi < btScalar(1.0f))
223  {
224  if (fi > btScalar(-1.0f))
225  {
226  xyz[0] = btAsin(btGetMatrixElem(mat,7));
227  xyz[1] = btAtan2(-btGetMatrixElem(mat,6),btGetMatrixElem(mat,8));
228  xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,4));
229  return true;
230  }
231  else
232  {
233  xyz[0] = -SIMD_HALF_PI;
234  xyz[1] = btScalar(0.0);
235  xyz[2] = -btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,0));
236  return false;
237  }
238  }
239  else
240  {
241  xyz[0] = SIMD_HALF_PI;
242  xyz[1] = btScalar(0.0);
243  xyz[2] = btAtan2(btGetMatrixElem(mat,2),btGetMatrixElem(mat,0));
244  }
245  return false;
246 }
247 
249 {
250  // rot = cz*cy cz*sy*sx-cx*sz sz*sx+cz*cx*sy
251  // cy*sz cz*cx+sz*sy*sx cx*sz*sy-cz*sx
252  // -sy cy*sx cy*cx
253 
254  btScalar fi = btGetMatrixElem(mat,6);
255  if (fi < btScalar(1.0f))
256  {
257  if (fi > btScalar(-1.0f))
258  {
259  xyz[0] = btAtan2(btGetMatrixElem(mat,7), btGetMatrixElem(mat,8));
260  xyz[1] = btAsin(-btGetMatrixElem(mat,6));
261  xyz[2] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,0));
262  return true;
263  }
264  else
265  {
266  xyz[0] = btScalar(0.0);
267  xyz[1] = SIMD_HALF_PI;
268  xyz[2] = -btAtan2(btGetMatrixElem(mat,1),btGetMatrixElem(mat,2));
269  return false;
270  }
271  }
272  else
273  {
274  xyz[0] = btScalar(0.0);
275  xyz[1] = -SIMD_HALF_PI;
276  xyz[2] = btAtan2(-btGetMatrixElem(mat,1),-btGetMatrixElem(mat,2));
277  }
278  return false;
279 }
280 
282 {
284  switch (m_rotateOrder)
285  {
286  case RO_XYZ : matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff); break;
287  case RO_XZY : matrixToEulerXZY(relative_frame,m_calculatedAxisAngleDiff); break;
288  case RO_YXZ : matrixToEulerYXZ(relative_frame,m_calculatedAxisAngleDiff); break;
289  case RO_YZX : matrixToEulerYZX(relative_frame,m_calculatedAxisAngleDiff); break;
290  case RO_ZXY : matrixToEulerZXY(relative_frame,m_calculatedAxisAngleDiff); break;
291  case RO_ZYX : matrixToEulerZYX(relative_frame,m_calculatedAxisAngleDiff); break;
292  default : btAssert(false);
293  }
294  // in euler angle mode we do not actually constrain the angular velocity
295  // along the axes axis[0] and axis[2] (although we do use axis[1]) :
296  //
297  // to get constrain w2-w1 along ...not
298  // ------ --------------------- ------
299  // d(angle[0])/dt = 0 ax[1] x ax[2] ax[0]
300  // d(angle[1])/dt = 0 ax[1]
301  // d(angle[2])/dt = 0 ax[0] x ax[1] ax[2]
302  //
303  // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
304  // to prove the result for angle[0], write the expression for angle[0] from
305  // GetInfo1 then take the derivative. to prove this for angle[2] it is
306  // easier to take the euler rate expression for d(angle[2])/dt with respect
307  // to the components of w and set that to 0.
308  switch (m_rotateOrder)
309  {
310  case RO_XYZ :
311  {
312  //Is this the "line of nodes" calculation choosing planes YZ (B coordinate system) and xy (A coordinate system)? (http://en.wikipedia.org/wiki/Euler_angles)
313  //The two planes are non-homologous, so this is a Tait–Bryan angle formalism and not a proper Euler
314  //Extrinsic rotations are equal to the reversed order intrinsic rotations so the above xyz extrinsic rotations (axes are fixed) are the same as the zy'x" intrinsic rotations (axes are refreshed after each rotation)
315  //that is why xy and YZ planes are chosen (this will describe a zy'x" intrinsic rotation) (see the figure on the left at http://en.wikipedia.org/wiki/Euler_angles under Tait–Bryan angles)
316  // x' = Nperp = N.cross(axis2)
317  // y' = N = axis2.cross(axis0)
318  // z' = z
319  //
320  // x" = X
321  // y" = y'
322  // z" = ??
323  //in other words:
324  //first rotate around z
325  //second rotate around y'= z.cross(X)
326  //third rotate around x" = X
327  //Original XYZ extrinsic rotation order.
328  //Planes: xy and YZ normals: z, X. Plane intersection (N) is z.cross(X)
331  m_calculatedAxis[1] = axis2.cross(axis0);
332  m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
333  m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
334  break;
335  }
336  case RO_XZY :
337  {
338  //planes: xz,ZY normals: y, X
339  //first rotate around y
340  //second rotate around z'= y.cross(X)
341  //third rotate around x" = X
344  m_calculatedAxis[2] = axis0.cross(axis1);
345  m_calculatedAxis[0] = axis1.cross(m_calculatedAxis[2]);
346  m_calculatedAxis[1] = m_calculatedAxis[2].cross(axis0);
347  break;
348  }
349  case RO_YXZ :
350  {
351  //planes: yx,XZ normals: z, Y
352  //first rotate around z
353  //second rotate around x'= z.cross(Y)
354  //third rotate around y" = Y
357  m_calculatedAxis[0] = axis1.cross(axis2);
358  m_calculatedAxis[1] = axis2.cross(m_calculatedAxis[0]);
359  m_calculatedAxis[2] = m_calculatedAxis[0].cross(axis1);
360  break;
361  }
362  case RO_YZX :
363  {
364  //planes: yz,ZX normals: x, Y
365  //first rotate around x
366  //second rotate around z'= x.cross(Y)
367  //third rotate around y" = Y
370  m_calculatedAxis[2] = axis0.cross(axis1);
371  m_calculatedAxis[0] = axis1.cross(m_calculatedAxis[2]);
372  m_calculatedAxis[1] = m_calculatedAxis[2].cross(axis0);
373  break;
374  }
375  case RO_ZXY :
376  {
377  //planes: zx,XY normals: y, Z
378  //first rotate around y
379  //second rotate around x'= y.cross(Z)
380  //third rotate around z" = Z
383  m_calculatedAxis[0] = axis1.cross(axis2);
384  m_calculatedAxis[1] = axis2.cross(m_calculatedAxis[0]);
385  m_calculatedAxis[2] = m_calculatedAxis[0].cross(axis1);
386  break;
387  }
388  case RO_ZYX :
389  {
390  //planes: zy,YX normals: x, Z
391  //first rotate around x
392  //second rotate around y' = x.cross(Z)
393  //third rotate around z" = Z
396  m_calculatedAxis[1] = axis2.cross(axis0);
397  m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
398  m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
399  break;
400  }
401  default:
402  btAssert(false);
403  }
404 
408 
409 }
410 
412 {
414 }
415 
417 {
422 
425  m_hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
426  btScalar miS = miA + miB;
427  if(miS > btScalar(0.f))
428  {
429  m_factA = miB / miS;
430  }
431  else
432  {
433  m_factA = btScalar(0.5f);
434  }
435  m_factB = btScalar(1.0f) - m_factA;
436 }
437 
438 
440 {
441  btScalar angle = m_calculatedAxisAngleDiff[axis_index];
442  angle = btAdjustAngleToLimits(angle, m_angularLimits[axis_index].m_loLimit, m_angularLimits[axis_index].m_hiLimit);
443  m_angularLimits[axis_index].m_currentPosition = angle;
444  m_angularLimits[axis_index].testLimitValue(angle);
445 }
446 
447 
449 {
450  //prepare constraint
452  info->m_numConstraintRows = 0;
453  info->nub = 0;
454  int i;
455  //test linear limits
456  for(i = 0; i < 3; i++)
457  {
458  if (m_linearLimits.m_currentLimit[i]==4) info->m_numConstraintRows += 2;
459  else if (m_linearLimits.m_currentLimit[i]!=0) info->m_numConstraintRows += 1;
462  }
463  //test angular limits
464  for (i=0;i<3 ;i++ )
465  {
467  if (m_angularLimits[i].m_currentLimit==4) info->m_numConstraintRows += 2;
468  else if (m_angularLimits[i].m_currentLimit!=0) info->m_numConstraintRows += 1;
469  if (m_angularLimits[i].m_enableMotor ) info->m_numConstraintRows += 1;
470  if (m_angularLimits[i].m_enableSpring) info->m_numConstraintRows += 1;
471  }
472 }
473 
474 
476 {
477  const btTransform& transA = m_rbA.getCenterOfMassTransform();
478  const btTransform& transB = m_rbB.getCenterOfMassTransform();
479  const btVector3& linVelA = m_rbA.getLinearVelocity();
480  const btVector3& linVelB = m_rbB.getLinearVelocity();
481  const btVector3& angVelA = m_rbA.getAngularVelocity();
482  const btVector3& angVelB = m_rbB.getAngularVelocity();
483 
484  // for stability better to solve angular limits first
485  int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);
486  setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);
487 }
488 
489 
490 int btGeneric6DofSpring2Constraint::setLinearLimits(btConstraintInfo2* info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
491 {
492  //solve linear limits
494  for (int i=0;i<3 ;i++ )
495  {
497  { // re-use rotational motor code
498  limot.m_bounce = m_linearLimits.m_bounce[i];
517  int flags = m_flags >> (i * BT_6DOF_FLAGS_AXIS_SHIFT2);
518  limot.m_stopCFM = (flags & BT_6DOF_FLAGS_CFM_STOP2) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];
519  limot.m_stopERP = (flags & BT_6DOF_FLAGS_ERP_STOP2) ? m_linearLimits.m_stopERP[i] : info->erp;
520  limot.m_motorCFM = (flags & BT_6DOF_FLAGS_CFM_MOTO2) ? m_linearLimits.m_motorCFM[i] : info->cfm[0];
521  limot.m_motorERP = (flags & BT_6DOF_FLAGS_ERP_MOTO2) ? m_linearLimits.m_motorERP[i] : info->erp;
522 
523  //rotAllowed is a bit of a magic from the original 6dof. The calculation of it here is something that imitates the original behavior as much as possible.
524  int indx1 = (i + 1) % 3;
525  int indx2 = (i + 2) % 3;
526  int rotAllowed = 1; // rotations around orthos to current axis (it is used only when one of the body is static)
527  #define D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION 1.0e-3
528  bool indx1Violated = m_angularLimits[indx1].m_currentLimit == 1 ||
529  m_angularLimits[indx1].m_currentLimit == 2 ||
532  bool indx2Violated = m_angularLimits[indx2].m_currentLimit == 1 ||
533  m_angularLimits[indx2].m_currentLimit == 2 ||
536  if( indx1Violated && indx2Violated )
537  {
538  rotAllowed = 0;
539  }
540  row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0, rotAllowed);
541 
542  }
543  }
544  return row;
545 }
546 
547 
548 
549 int btGeneric6DofSpring2Constraint::setAngularLimits(btConstraintInfo2 *info, int row_offset, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)
550 {
551  int row = row_offset;
552 
553  //order of rotational constraint rows
554  int cIdx[] = {0, 1, 2};
555  switch(m_rotateOrder)
556  {
557  case RO_XYZ : cIdx[0] = 0; cIdx[1] = 1; cIdx[2] = 2; break;
558  case RO_XZY : cIdx[0] = 0; cIdx[1] = 2; cIdx[2] = 1; break;
559  case RO_YXZ : cIdx[0] = 1; cIdx[1] = 0; cIdx[2] = 2; break;
560  case RO_YZX : cIdx[0] = 1; cIdx[1] = 2; cIdx[2] = 0; break;
561  case RO_ZXY : cIdx[0] = 2; cIdx[1] = 0; cIdx[2] = 1; break;
562  case RO_ZYX : cIdx[0] = 2; cIdx[1] = 1; cIdx[2] = 0; break;
563  default : btAssert(false);
564  }
565 
566  for (int ii = 0; ii < 3 ; ii++ )
567  {
568  int i = cIdx[ii];
569  if(m_angularLimits[i].m_currentLimit || m_angularLimits[i].m_enableMotor || m_angularLimits[i].m_enableSpring)
570  {
571  btVector3 axis = getAxis(i);
572  int flags = m_flags >> ((i + 3) * BT_6DOF_FLAGS_AXIS_SHIFT2);
573  if(!(flags & BT_6DOF_FLAGS_CFM_STOP2))
574  {
575  m_angularLimits[i].m_stopCFM = info->cfm[0];
576  }
577  if(!(flags & BT_6DOF_FLAGS_ERP_STOP2))
578  {
579  m_angularLimits[i].m_stopERP = info->erp;
580  }
581  if(!(flags & BT_6DOF_FLAGS_CFM_MOTO2))
582  {
583  m_angularLimits[i].m_motorCFM = info->cfm[0];
584  }
585  if(!(flags & BT_6DOF_FLAGS_ERP_MOTO2))
586  {
587  m_angularLimits[i].m_motorERP = info->erp;
588  }
589  row += get_limit_motor_info2(&m_angularLimits[i],transA,transB,linVelA,linVelB,angVelA,angVelB, info,row,axis,1);
590  }
591  }
592 
593  return row;
594 }
595 
596 
598 {
599  m_frameInA = frameA;
600  m_frameInB = frameB;
601  buildJacobian();
603 }
604 
605 
607 {
610  for(int i = 0; i < 3; i++)
611  {
614  }
615 }
616 
617 void btGeneric6DofSpring2Constraint::calculateJacobi(btRotationalLimitMotor2 * limot, const btTransform& transA,const btTransform& transB, btConstraintInfo2 *info, int srow, btVector3& ax1, int rotational, int rotAllowed)
618 {
619  btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
620  btScalar *J2 = rotational ? info->m_J2angularAxis : info->m_J2linearAxis;
621 
622  J1[srow+0] = ax1[0];
623  J1[srow+1] = ax1[1];
624  J1[srow+2] = ax1[2];
625 
626  J2[srow+0] = -ax1[0];
627  J2[srow+1] = -ax1[1];
628  J2[srow+2] = -ax1[2];
629 
630  if(!rotational)
631  {
632  btVector3 tmpA, tmpB, relA, relB;
633  // get vector from bodyB to frameB in WCS
634  relB = m_calculatedTransformB.getOrigin() - transB.getOrigin();
635  // same for bodyA
636  relA = m_calculatedTransformA.getOrigin() - transA.getOrigin();
637  tmpA = relA.cross(ax1);
638  tmpB = relB.cross(ax1);
639  if(m_hasStaticBody && (!rotAllowed))
640  {
641  tmpA *= m_factA;
642  tmpB *= m_factB;
643  }
644  int i;
645  for (i=0; i<3; i++) info->m_J1angularAxis[srow+i] = tmpA[i];
646  for (i=0; i<3; i++) info->m_J2angularAxis[srow+i] = -tmpB[i];
647  }
648 }
649 
650 
652  btRotationalLimitMotor2 * limot,
653  const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
654  btConstraintInfo2 *info, int row, btVector3& ax1, int rotational,int rotAllowed)
655 {
656  int count = 0;
657  int srow = row * info->rowskip;
658 
659  if (limot->m_currentLimit==4)
660  {
661  btScalar vel = rotational ? angVelA.dot(ax1) - angVelB.dot(ax1) : linVelA.dot(ax1) - linVelB.dot(ax1);
662 
663  calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
664  info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitError * (rotational ? -1 : 1);
665  if (rotational) {
666  if (info->m_constraintError[srow]-vel*limot->m_stopERP > 0) {
667  btScalar bounceerror = -limot->m_bounce* vel;
668  if (bounceerror > info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror;
669  }
670  } else {
671  if (info->m_constraintError[srow]-vel*limot->m_stopERP < 0) {
672  btScalar bounceerror = -limot->m_bounce* vel;
673  if (bounceerror < info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror;
674  }
675  }
676  info->m_lowerLimit[srow] = rotational ? 0 : -SIMD_INFINITY;
677  info->m_upperLimit[srow] = rotational ? SIMD_INFINITY : 0;
678  info->cfm[srow] = limot->m_stopCFM;
679  srow += info->rowskip;
680  ++count;
681 
682  calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
683  info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitErrorHi * (rotational ? -1 : 1);
684  if (rotational) {
685  if (info->m_constraintError[srow]-vel*limot->m_stopERP < 0) {
686  btScalar bounceerror = -limot->m_bounce* vel;
687  if (bounceerror < info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror;
688  }
689  } else {
690  if (info->m_constraintError[srow]-vel*limot->m_stopERP > 0) {
691  btScalar bounceerror = -limot->m_bounce* vel;
692  if (bounceerror > info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror;
693  }
694  }
695  info->m_lowerLimit[srow] = rotational ? -SIMD_INFINITY : 0;
696  info->m_upperLimit[srow] = rotational ? 0 : SIMD_INFINITY;
697  info->cfm[srow] = limot->m_stopCFM;
698  srow += info->rowskip;
699  ++count;
700  } else
701  if (limot->m_currentLimit==3)
702  {
703  calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
704  info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitError * (rotational ? -1 : 1);
705  info->m_lowerLimit[srow] = -SIMD_INFINITY;
706  info->m_upperLimit[srow] = SIMD_INFINITY;
707  info->cfm[srow] = limot->m_stopCFM;
708  srow += info->rowskip;
709  ++count;
710  }
711 
712  if (limot->m_enableMotor && !limot->m_servoMotor)
713  {
714  calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
715  btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;
716  btScalar mot_fact = getMotorFactor(limot->m_currentPosition,
717  limot->m_loLimit,
718  limot->m_hiLimit,
719  tag_vel,
720  info->fps * limot->m_motorERP);
721  info->m_constraintError[srow] = mot_fact * limot->m_targetVelocity;
722  info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
723  info->m_upperLimit[srow] = limot->m_maxMotorForce;
724  info->cfm[srow] = limot->m_motorCFM;
725  srow += info->rowskip;
726  ++count;
727  }
728 
729  if (limot->m_enableMotor && limot->m_servoMotor)
730  {
731  btScalar error = limot->m_currentPosition - limot->m_servoTarget;
732  calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
733  btScalar targetvelocity = error<0 ? -limot->m_targetVelocity : limot->m_targetVelocity;
734  btScalar tag_vel = -targetvelocity;
735  btScalar mot_fact;
736  if(error != 0)
737  {
738  btScalar lowLimit;
739  btScalar hiLimit;
740  if(limot->m_loLimit > limot->m_hiLimit)
741  {
742  lowLimit = error > 0 ? limot->m_servoTarget : -SIMD_INFINITY;
743  hiLimit = error < 0 ? limot->m_servoTarget : SIMD_INFINITY;
744  }
745  else
746  {
747  lowLimit = error > 0 && limot->m_servoTarget>limot->m_loLimit ? limot->m_servoTarget : limot->m_loLimit;
748  hiLimit = error < 0 && limot->m_servoTarget<limot->m_hiLimit ? limot->m_servoTarget : limot->m_hiLimit;
749  }
750  mot_fact = getMotorFactor(limot->m_currentPosition, lowLimit, hiLimit, tag_vel, info->fps * limot->m_motorERP);
751  }
752  else
753  {
754  mot_fact = 0;
755  }
756  info->m_constraintError[srow] = mot_fact * targetvelocity * (rotational ? -1 : 1);
757  info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
758  info->m_upperLimit[srow] = limot->m_maxMotorForce;
759  info->cfm[srow] = limot->m_motorCFM;
760  srow += info->rowskip;
761  ++count;
762  }
763 
764  if (limot->m_enableSpring)
765  {
766  btScalar error = limot->m_currentPosition - limot->m_equilibriumPoint;
767  calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
768 
769  //btScalar cfm = 1.0 / ((1.0/info->fps)*limot->m_springStiffness+ limot->m_springDamping);
770  //if(cfm > 0.99999)
771  // cfm = 0.99999;
772  //btScalar erp = (1.0/info->fps)*limot->m_springStiffness / ((1.0/info->fps)*limot->m_springStiffness + limot->m_springDamping);
773  //info->m_constraintError[srow] = info->fps * erp * error * (rotational ? -1.0 : 1.0);
774  //info->m_lowerLimit[srow] = -SIMD_INFINITY;
775  //info->m_upperLimit[srow] = SIMD_INFINITY;
776 
777  btScalar dt = BT_ONE / info->fps;
778  btScalar kd = limot->m_springDamping;
779  btScalar ks = limot->m_springStiffness;
780  btScalar vel = rotational ? angVelA.dot(ax1) - angVelB.dot(ax1) : linVelA.dot(ax1) - linVelB.dot(ax1);
781 // btScalar erp = 0.1;
782  btScalar cfm = BT_ZERO;
783  btScalar mA = BT_ONE / m_rbA.getInvMass();
784  btScalar mB = BT_ONE / m_rbB.getInvMass();
785  btScalar m = mA > mB ? mB : mA;
786  btScalar angularfreq = sqrt(ks / m);
787 
788 
789  //limit stiffness (the spring should not be sampled faster that the quarter of its angular frequency)
790  if(limot->m_springStiffnessLimited && 0.25 < angularfreq * dt)
791  {
792  ks = BT_ONE / dt / dt / btScalar(16.0) * m;
793  }
794  //avoid damping that would blow up the spring
795  if(limot->m_springDampingLimited && kd * dt > m)
796  {
797  kd = m / dt;
798  }
799  btScalar fs = ks * error * dt;
800  btScalar fd = -kd * (vel) * (rotational ? -1 : 1) * dt;
801  btScalar f = (fs+fd);
802 
803  info->m_constraintError[srow] = (vel + f * (rotational ? -1 : 1)) ;
804 
805  btScalar minf = f < fd ? f : fd;
806  btScalar maxf = f < fd ? fd : f;
807  if(!rotational)
808  {
809  info->m_lowerLimit[srow] = minf > 0 ? 0 : minf;
810  info->m_upperLimit[srow] = maxf < 0 ? 0 : maxf;
811  }
812  else
813  {
814  info->m_lowerLimit[srow] = -maxf > 0 ? 0 : -maxf;
815  info->m_upperLimit[srow] = -minf < 0 ? 0 : -minf;
816  }
817 
818  info->cfm[srow] = cfm;
819  srow += info->rowskip;
820  ++count;
821  }
822 
823  return count;
824 }
825 
826 
827 //override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
828 //If no axis is provided, it uses the default axis for this constraint.
830 {
831  if((axis >= 0) && (axis < 3))
832  {
833  switch(num)
834  {
835  case BT_CONSTRAINT_STOP_ERP :
836  m_linearLimits.m_stopERP[axis] = value;
838  break;
839  case BT_CONSTRAINT_STOP_CFM :
840  m_linearLimits.m_stopCFM[axis] = value;
842  break;
843  case BT_CONSTRAINT_ERP :
844  m_linearLimits.m_motorERP[axis] = value;
846  break;
847  case BT_CONSTRAINT_CFM :
848  m_linearLimits.m_motorCFM[axis] = value;
850  break;
851  default :
853  }
854  }
855  else if((axis >=3) && (axis < 6))
856  {
857  switch(num)
858  {
859  case BT_CONSTRAINT_STOP_ERP :
860  m_angularLimits[axis - 3].m_stopERP = value;
862  break;
863  case BT_CONSTRAINT_STOP_CFM :
864  m_angularLimits[axis - 3].m_stopCFM = value;
866  break;
867  case BT_CONSTRAINT_ERP :
868  m_angularLimits[axis - 3].m_motorERP = value;
870  break;
871  case BT_CONSTRAINT_CFM :
872  m_angularLimits[axis - 3].m_motorCFM = value;
874  break;
875  default :
877  }
878  }
879  else
880  {
882  }
883 }
884 
885 //return the local value of parameter
887 {
888  btScalar retVal = 0;
889  if((axis >= 0) && (axis < 3))
890  {
891  switch(num)
892  {
893  case BT_CONSTRAINT_STOP_ERP :
895  retVal = m_linearLimits.m_stopERP[axis];
896  break;
897  case BT_CONSTRAINT_STOP_CFM :
898  btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
899  retVal = m_linearLimits.m_stopCFM[axis];
900  break;
901  case BT_CONSTRAINT_ERP :
902  btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
903  retVal = m_linearLimits.m_motorERP[axis];
904  break;
905  case BT_CONSTRAINT_CFM :
906  btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
907  retVal = m_linearLimits.m_motorCFM[axis];
908  break;
909  default :
911  }
912  }
913  else if((axis >=3) && (axis < 6))
914  {
915  switch(num)
916  {
917  case BT_CONSTRAINT_STOP_ERP :
919  retVal = m_angularLimits[axis - 3].m_stopERP;
920  break;
921  case BT_CONSTRAINT_STOP_CFM :
922  btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
923  retVal = m_angularLimits[axis - 3].m_stopCFM;
924  break;
925  case BT_CONSTRAINT_ERP :
926  btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
927  retVal = m_angularLimits[axis - 3].m_motorERP;
928  break;
929  case BT_CONSTRAINT_CFM :
930  btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
931  retVal = m_angularLimits[axis - 3].m_motorCFM;
932  break;
933  default :
935  }
936  }
937  else
938  {
940  }
941  return retVal;
942 }
943 
944 
945 
947 {
948  btVector3 zAxis = axis1.normalized();
949  btVector3 yAxis = axis2.normalized();
950  btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
951 
952  btTransform frameInW;
953  frameInW.setIdentity();
954  frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0],
955  xAxis[1], yAxis[1], zAxis[1],
956  xAxis[2], yAxis[2], zAxis[2]);
957 
958  // now get constraint frame in local coordinate systems
961 
963 }
964 
966 {
967  btAssert((index >= 0) && (index < 6));
968  if (index<3)
969  m_linearLimits.m_bounce[index] = bounce;
970  else
971  m_angularLimits[index - 3].m_bounce = bounce;
972 }
973 
975 {
976  btAssert((index >= 0) && (index < 6));
977  if (index<3)
978  m_linearLimits.m_enableMotor[index] = onOff;
979  else
980  m_angularLimits[index - 3].m_enableMotor = onOff;
981 }
982 
983 void btGeneric6DofSpring2Constraint::setServo(int index, bool onOff)
984 {
985  btAssert((index >= 0) && (index < 6));
986  if (index<3)
987  m_linearLimits.m_servoMotor[index] = onOff;
988  else
989  m_angularLimits[index - 3].m_servoMotor = onOff;
990 }
991 
993 {
994  btAssert((index >= 0) && (index < 6));
995  if (index<3)
996  m_linearLimits.m_targetVelocity[index] = velocity;
997  else
998  m_angularLimits[index - 3].m_targetVelocity = velocity;
999 }
1000 
1002 {
1003  btAssert((index >= 0) && (index < 6));
1004  if (index<3)
1005  m_linearLimits.m_servoTarget[index] = target;
1006  else
1007  m_angularLimits[index - 3].m_servoTarget = target;
1008 }
1009 
1011 {
1012  btAssert((index >= 0) && (index < 6));
1013  if (index<3)
1014  m_linearLimits.m_maxMotorForce[index] = force;
1015  else
1016  m_angularLimits[index - 3].m_maxMotorForce = force;
1017 }
1018 
1020 {
1021  btAssert((index >= 0) && (index < 6));
1022  if (index<3)
1023  m_linearLimits.m_enableSpring[index] = onOff;
1024  else
1025  m_angularLimits[index - 3] .m_enableSpring = onOff;
1026 }
1027 
1028 void btGeneric6DofSpring2Constraint::setStiffness(int index, btScalar stiffness, bool limitIfNeeded)
1029 {
1030  btAssert((index >= 0) && (index < 6));
1031  if (index<3) {
1032  m_linearLimits.m_springStiffness[index] = stiffness;
1033  m_linearLimits.m_springStiffnessLimited[index] = limitIfNeeded;
1034  } else {
1035  m_angularLimits[index - 3].m_springStiffness = stiffness;
1036  m_angularLimits[index - 3].m_springStiffnessLimited = limitIfNeeded;
1037  }
1038 }
1039 
1040 void btGeneric6DofSpring2Constraint::setDamping(int index, btScalar damping, bool limitIfNeeded)
1041 {
1042  btAssert((index >= 0) && (index < 6));
1043  if (index<3) {
1044  m_linearLimits.m_springDamping[index] = damping;
1045  m_linearLimits.m_springDampingLimited[index] = limitIfNeeded;
1046  } else {
1047  m_angularLimits[index - 3].m_springDamping = damping;
1048  m_angularLimits[index - 3].m_springDampingLimited = limitIfNeeded;
1049  }
1050 }
1051 
1053 {
1055  int i;
1056  for( i = 0; i < 3; i++)
1058  for(i = 0; i < 3; i++)
1060 }
1061 
1063 {
1064  btAssert((index >= 0) && (index < 6));
1066  if (index<3)
1068  else
1070 }
1071 
1073 {
1074  btAssert((index >= 0) && (index < 6));
1075  if (index<3)
1076  m_linearLimits.m_equilibriumPoint[index] = val;
1077  else
1078  m_angularLimits[index - 3] .m_equilibriumPoint = val;
1079 }
1080 
1081 
1083 
1085 {
1086  //we can't normalize the angles here because we would lost the sign that we use later, but it doesn't seem to be a problem
1087  if(m_loLimit > m_hiLimit) {
1088  m_currentLimit = 0;
1090  }
1091  else if(m_loLimit == m_hiLimit) {
1092  m_currentLimitError = test_value - m_loLimit;
1093  m_currentLimit = 3;
1094  } else {
1095  m_currentLimitError = test_value - m_loLimit;
1096  m_currentLimitErrorHi = test_value - m_hiLimit;
1097  m_currentLimit = 4;
1098  }
1099 }
1100 
1102 
1104 {
1105  btScalar loLimit = m_lowerLimit[limitIndex];
1106  btScalar hiLimit = m_upperLimit[limitIndex];
1107  if(loLimit > hiLimit) {
1108  m_currentLimitError[limitIndex] = 0;
1109  m_currentLimit[limitIndex] = 0;
1110  }
1111  else if(loLimit == hiLimit) {
1112  m_currentLimitError[limitIndex] = test_value - loLimit;
1113  m_currentLimit[limitIndex] = 3;
1114  } else {
1115  m_currentLimitError[limitIndex] = test_value - loLimit;
1116  m_currentLimitErrorHi[limitIndex] = test_value - hiLimit;
1117  m_currentLimit[limitIndex] = 4;
1118  }
1119 }
1120 
1121 
btMatrix3x3 inverse() const
Return the inverse of the matrix.
Definition: btMatrix3x3.h:1046
btScalar getInvMass() const
Definition: btRigidBody.h:270
#define SIMD_EPSILON
Definition: btScalar.h:494
static bool matrixToEulerXYZ(const btMatrix3x3 &mat, btVector3 &xyz)
void setTargetVelocity(int index, btScalar velocity)
static bool matrixToEulerZXY(const btMatrix3x3 &mat, btVector3 &xyz)
#define D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION
void setMaxMotorForce(int index, btScalar force)
#define BT_6DOF_FLAGS_AXIS_SHIFT2
void testLimitValue(int limitIndex, btScalar test_value)
static bool matrixToEulerYXZ(const btMatrix3x3 &mat, btVector3 &xyz)
int get_limit_motor_info2(btRotationalLimitMotor2 *limot, const btTransform &transA, const btTransform &transB, const btVector3 &linVelA, const btVector3 &linVelB, const btVector3 &angVelA, const btVector3 &angVelB, btConstraintInfo2 *info, int row, btVector3 &ax1, int rotational, int rotAllowed=false)
#define BT_ONE
Definition: btScalar.h:496
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:172
#define btAssert(x)
Definition: btScalar.h:113
static btScalar btGetMatrixElem(const btMatrix3x3 &mat, int index)
void setAxis(const btVector3 &axis1, const btVector3 &axis2)
virtual btScalar getParam(int num, int axis=-1) const
return the local value of parameter
int setLinearLimits(btConstraintInfo2 *info, int row, const btTransform &transA, const btTransform &transB, const btVector3 &linVelA, const btVector3 &linVelB, const btVector3 &angVelA, const btVector3 &angVelB)
btVector3 getAxis(int axis_index) const
btVector3 getColumn(int i) const
Get a column of the matrix as a vector.
Definition: btMatrix3x3.h:134
static bool matrixToEulerZYX(const btMatrix3x3 &mat, btVector3 &xyz)
#define SIMD_HALF_PI
Definition: btScalar.h:478
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
#define BT_ZERO
Definition: btScalar.h:497
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:297
#define SIMD_INFINITY
Definition: btScalar.h:495
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:117
const btTransform & getCenterOfMassTransform() const
Definition: btRigidBody.h:356
btScalar btAtan2(btScalar x, btScalar y)
Definition: btScalar.h:468
const btVector3 & getAngularVelocity() const
Definition: btRigidBody.h:362
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:377
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:112
void setValue(const btScalar &xx, const btScalar &xy, const btScalar &xz, const btScalar &yx, const btScalar &yy, const btScalar &yz, const btScalar &zx, const btScalar &zy, const btScalar &zz)
Set the values of the matrix explicitly (row major)
Definition: btMatrix3x3.h:198
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:62
btTransform inverse() const
Return the inverse of this transform.
Definition: btTransform.h:188
btGeneric6DofSpring2Constraint(btRigidBody &rbA, btRigidBody &rbB, const btTransform &frameInA, const btTransform &frameInB, RotateOrder rotOrder=RO_XYZ)
2009 March: btGeneric6DofConstraint refactored by Roman Ponomarev Added support for generic constrain...
virtual void setParam(int num, btScalar value, int axis=-1)
override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0...
virtual void buildJacobian()
internal method used by the constraint solver, don't use them directly
btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScalar angleLowerLimitInRadians, btScalar angleUpperLimitInRadians)
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
static bool matrixToEulerXZY(const btMatrix3x3 &mat, btVector3 &xyz)
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
void calculateJacobi(btRotationalLimitMotor2 *limot, const btTransform &transA, const btTransform &transB, btConstraintInfo2 *info, int srow, btVector3 &ax1, int rotational, int rotAllowed)
btVector3 normalized() const
Return a normalized version of this vector.
Definition: btVector3.h:952
TypedConstraint is the baseclass for Bullet constraints and vehicles.
btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact)
internal method used by the constraint solver, don't use them directly
const btRigidBody & getRigidBodyA() const
void setStiffness(int index, btScalar stiffness, bool limitIfNeeded=true)
static bool matrixToEulerYZX(const btMatrix3x3 &mat, btVector3 &xyz)
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:48
void testLimitValue(btScalar test_value)
const btVector3 & getLinearVelocity() const
Definition: btRigidBody.h:359
btScalar btAsin(btScalar x)
Definition: btScalar.h:460
void setFrames(const btTransform &frameA, const btTransform &frameB)
virtual void getInfo1(btConstraintInfo1 *info)
internal method used by the constraint solver, don't use them directly
#define btAssertConstrParams(_par)
void setDamping(int index, btScalar damping, bool limitIfNeeded=true)
const btRigidBody & getRigidBodyB() const
void setServoTarget(int index, btScalar target)
virtual void getInfo2(btConstraintInfo2 *info)
internal method used by the constraint solver, don't use them directly
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:278
int setAngularLimits(btConstraintInfo2 *info, int row_offset, const btTransform &transA, const btTransform &transB, const btVector3 &linVelA, const btVector3 &linVelB, const btVector3 &angVelA, const btVector3 &angVelB)
void setBounce(int index, btScalar bounce)