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 
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];
515  int flags = m_flags >> (i * BT_6DOF_FLAGS_AXIS_SHIFT2);
516  limot.m_stopCFM = (flags & BT_6DOF_FLAGS_CFM_STOP2) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];
517  limot.m_stopERP = (flags & BT_6DOF_FLAGS_ERP_STOP2) ? m_linearLimits.m_stopERP[i] : info->erp;
518  limot.m_motorCFM = (flags & BT_6DOF_FLAGS_CFM_MOTO2) ? m_linearLimits.m_motorCFM[i] : info->cfm[0];
519  limot.m_motorERP = (flags & BT_6DOF_FLAGS_ERP_MOTO2) ? m_linearLimits.m_motorERP[i] : info->erp;
520 
521  //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.
522  int indx1 = (i + 1) % 3;
523  int indx2 = (i + 2) % 3;
524  int rotAllowed = 1; // rotations around orthos to current axis (it is used only when one of the body is static)
525  #define D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION 1.0e-3
526  bool indx1Violated = m_angularLimits[indx1].m_currentLimit == 1 ||
527  m_angularLimits[indx1].m_currentLimit == 2 ||
530  bool indx2Violated = m_angularLimits[indx2].m_currentLimit == 1 ||
531  m_angularLimits[indx2].m_currentLimit == 2 ||
534  if( indx1Violated && indx2Violated )
535  {
536  rotAllowed = 0;
537  }
538  row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0, rotAllowed);
539 
540  }
541  }
542  return row;
543 }
544 
545 
546 
547 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)
548 {
549  int row = row_offset;
550 
551  //order of rotational constraint rows
552  int cIdx[] = {0, 1, 2};
553  switch(m_rotateOrder)
554  {
555  case RO_XYZ : cIdx[0] = 0; cIdx[1] = 1; cIdx[2] = 2; break;
556  case RO_XZY : cIdx[0] = 0; cIdx[1] = 2; cIdx[2] = 1; break;
557  case RO_YXZ : cIdx[0] = 1; cIdx[1] = 0; cIdx[2] = 2; break;
558  case RO_YZX : cIdx[0] = 1; cIdx[1] = 2; cIdx[2] = 0; break;
559  case RO_ZXY : cIdx[0] = 2; cIdx[1] = 0; cIdx[2] = 1; break;
560  case RO_ZYX : cIdx[0] = 2; cIdx[1] = 1; cIdx[2] = 0; break;
561  default : btAssert(false);
562  }
563 
564  for (int ii = 0; ii < 3 ; ii++ )
565  {
566  int i = cIdx[ii];
567  if(m_angularLimits[i].m_currentLimit || m_angularLimits[i].m_enableMotor || m_angularLimits[i].m_enableSpring)
568  {
569  btVector3 axis = getAxis(i);
570  int flags = m_flags >> ((i + 3) * BT_6DOF_FLAGS_AXIS_SHIFT2);
571  if(!(flags & BT_6DOF_FLAGS_CFM_STOP2))
572  {
573  m_angularLimits[i].m_stopCFM = info->cfm[0];
574  }
575  if(!(flags & BT_6DOF_FLAGS_ERP_STOP2))
576  {
577  m_angularLimits[i].m_stopERP = info->erp;
578  }
579  if(!(flags & BT_6DOF_FLAGS_CFM_MOTO2))
580  {
581  m_angularLimits[i].m_motorCFM = info->cfm[0];
582  }
583  if(!(flags & BT_6DOF_FLAGS_ERP_MOTO2))
584  {
585  m_angularLimits[i].m_motorERP = info->erp;
586  }
587  row += get_limit_motor_info2(&m_angularLimits[i],transA,transB,linVelA,linVelB,angVelA,angVelB, info,row,axis,1);
588  }
589  }
590 
591  return row;
592 }
593 
594 
596 {
597  m_frameInA = frameA;
598  m_frameInB = frameB;
599  buildJacobian();
601 }
602 
603 
605 {
608  for(int i = 0; i < 3; i++)
609  {
612  }
613 }
614 
615 void btGeneric6DofSpring2Constraint::calculateJacobi(btRotationalLimitMotor2 * limot, const btTransform& transA,const btTransform& transB, btConstraintInfo2 *info, int srow, btVector3& ax1, int rotational, int rotAllowed)
616 {
617  btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
618  btScalar *J2 = rotational ? info->m_J2angularAxis : info->m_J2linearAxis;
619 
620  J1[srow+0] = ax1[0];
621  J1[srow+1] = ax1[1];
622  J1[srow+2] = ax1[2];
623 
624  J2[srow+0] = -ax1[0];
625  J2[srow+1] = -ax1[1];
626  J2[srow+2] = -ax1[2];
627 
628  if(!rotational)
629  {
630  btVector3 tmpA, tmpB, relA, relB;
631  // get vector from bodyB to frameB in WCS
632  relB = m_calculatedTransformB.getOrigin() - transB.getOrigin();
633  // same for bodyA
634  relA = m_calculatedTransformA.getOrigin() - transA.getOrigin();
635  tmpA = relA.cross(ax1);
636  tmpB = relB.cross(ax1);
637  if(m_hasStaticBody && (!rotAllowed))
638  {
639  tmpA *= m_factA;
640  tmpB *= m_factB;
641  }
642  int i;
643  for (i=0; i<3; i++) info->m_J1angularAxis[srow+i] = tmpA[i];
644  for (i=0; i<3; i++) info->m_J2angularAxis[srow+i] = -tmpB[i];
645  }
646 }
647 
648 
650  btRotationalLimitMotor2 * limot,
651  const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,
652  btConstraintInfo2 *info, int row, btVector3& ax1, int rotational,int rotAllowed)
653 {
654  int count = 0;
655  int srow = row * info->rowskip;
656 
657  if (limot->m_currentLimit==4)
658  {
659  btScalar vel = rotational ? angVelA.dot(ax1) - angVelB.dot(ax1) : linVelA.dot(ax1) - linVelB.dot(ax1);
660 
661  calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
662  info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitError * (rotational ? -1 : 1);
663  if (rotational) {
664  if (info->m_constraintError[srow]-vel*limot->m_stopERP > 0) {
665  btScalar bounceerror = -limot->m_bounce* vel;
666  if (bounceerror > info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror;
667  }
668  } else {
669  if (info->m_constraintError[srow]-vel*limot->m_stopERP < 0) {
670  btScalar bounceerror = -limot->m_bounce* vel;
671  if (bounceerror < info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror;
672  }
673  }
674  info->m_lowerLimit[srow] = rotational ? 0 : -SIMD_INFINITY;
675  info->m_upperLimit[srow] = rotational ? SIMD_INFINITY : 0;
676  info->cfm[srow] = limot->m_stopCFM;
677  srow += info->rowskip;
678  ++count;
679 
680  calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
681  info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitErrorHi * (rotational ? -1 : 1);
682  if (rotational) {
683  if (info->m_constraintError[srow]-vel*limot->m_stopERP < 0) {
684  btScalar bounceerror = -limot->m_bounce* vel;
685  if (bounceerror < info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror;
686  }
687  } else {
688  if (info->m_constraintError[srow]-vel*limot->m_stopERP > 0) {
689  btScalar bounceerror = -limot->m_bounce* vel;
690  if (bounceerror > info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror;
691  }
692  }
693  info->m_lowerLimit[srow] = rotational ? -SIMD_INFINITY : 0;
694  info->m_upperLimit[srow] = rotational ? 0 : SIMD_INFINITY;
695  info->cfm[srow] = limot->m_stopCFM;
696  srow += info->rowskip;
697  ++count;
698  } else
699  if (limot->m_currentLimit==3)
700  {
701  calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
702  info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitError * (rotational ? -1 : 1);
703  info->m_lowerLimit[srow] = -SIMD_INFINITY;
704  info->m_upperLimit[srow] = SIMD_INFINITY;
705  info->cfm[srow] = limot->m_stopCFM;
706  srow += info->rowskip;
707  ++count;
708  }
709 
710  if (limot->m_enableMotor && !limot->m_servoMotor)
711  {
712  calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
713  btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;
714  btScalar mot_fact = getMotorFactor(limot->m_currentPosition,
715  limot->m_loLimit,
716  limot->m_hiLimit,
717  tag_vel,
718  info->fps * limot->m_motorERP);
719  info->m_constraintError[srow] = mot_fact * limot->m_targetVelocity;
720  info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
721  info->m_upperLimit[srow] = limot->m_maxMotorForce;
722  info->cfm[srow] = limot->m_motorCFM;
723  srow += info->rowskip;
724  ++count;
725  }
726 
727  if (limot->m_enableMotor && limot->m_servoMotor)
728  {
729  btScalar error = limot->m_currentPosition - limot->m_servoTarget;
730  calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
731  btScalar targetvelocity = error<0 ? -limot->m_targetVelocity : limot->m_targetVelocity;
732  btScalar tag_vel = -targetvelocity;
733  btScalar mot_fact;
734  if(error != 0)
735  {
736  btScalar lowLimit;
737  btScalar hiLimit;
738  if(limot->m_loLimit > limot->m_hiLimit)
739  {
740  lowLimit = error > 0 ? limot->m_servoTarget : -SIMD_INFINITY;
741  hiLimit = error < 0 ? limot->m_servoTarget : SIMD_INFINITY;
742  }
743  else
744  {
745  lowLimit = error > 0 && limot->m_servoTarget>limot->m_loLimit ? limot->m_servoTarget : limot->m_loLimit;
746  hiLimit = error < 0 && limot->m_servoTarget<limot->m_hiLimit ? limot->m_servoTarget : limot->m_hiLimit;
747  }
748  mot_fact = getMotorFactor(limot->m_currentPosition, lowLimit, hiLimit, tag_vel, info->fps * limot->m_motorERP);
749  }
750  else
751  {
752  mot_fact = 0;
753  }
754  info->m_constraintError[srow] = mot_fact * targetvelocity * (rotational ? -1 : 1);
755  info->m_lowerLimit[srow] = -limot->m_maxMotorForce;
756  info->m_upperLimit[srow] = limot->m_maxMotorForce;
757  info->cfm[srow] = limot->m_motorCFM;
758  srow += info->rowskip;
759  ++count;
760  }
761 
762  if (limot->m_enableSpring)
763  {
764  btScalar error = limot->m_currentPosition - limot->m_equilibriumPoint;
765  calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
766 
767  //btScalar cfm = 1.0 / ((1.0/info->fps)*limot->m_springStiffness+ limot->m_springDamping);
768  //if(cfm > 0.99999)
769  // cfm = 0.99999;
770  //btScalar erp = (1.0/info->fps)*limot->m_springStiffness / ((1.0/info->fps)*limot->m_springStiffness + limot->m_springDamping);
771  //info->m_constraintError[srow] = info->fps * erp * error * (rotational ? -1.0 : 1.0);
772  //info->m_lowerLimit[srow] = -SIMD_INFINITY;
773  //info->m_upperLimit[srow] = SIMD_INFINITY;
774 
775  btScalar dt = BT_ONE / info->fps;
776  btScalar kd = limot->m_springDamping;
777  btScalar ks = limot->m_springStiffness;
778  btScalar vel = rotational ? angVelA.dot(ax1) - angVelB.dot(ax1) : linVelA.dot(ax1) - linVelB.dot(ax1);
779 // btScalar erp = 0.1;
780  btScalar cfm = BT_ZERO;
781  btScalar mA = BT_ONE / m_rbA.getInvMass();
782  btScalar mB = BT_ONE / m_rbB.getInvMass();
783  btScalar m = mA > mB ? mB : mA;
784  btScalar angularfreq = sqrt(ks / m);
785 
786 
787  //limit stiffness (the spring should not be sampled faster that the quarter of its angular frequency)
788  if( 0.25 < angularfreq * dt)
789  {
790  ks = BT_ONE / dt / dt / btScalar(16.0) / m;
791  }
792  //avoid overdamping
793  if(kd * dt > m)
794  {
795  kd = m / dt;
796  }
797  btScalar fs = ks * error * dt;
798  btScalar fd = -kd * (vel) * (rotational ? -1 : 1) * dt;
799  btScalar f = (fs+fd);
800 
801  info->m_constraintError[srow] = (vel + f * (rotational ? -1 : 1)) ;
802 
803  btScalar minf = f < fd ? f : fd;
804  btScalar maxf = f < fd ? fd : f;
805  if(!rotational)
806  {
807  info->m_lowerLimit[srow] = minf > 0 ? 0 : minf;
808  info->m_upperLimit[srow] = maxf < 0 ? 0 : maxf;
809  }
810  else
811  {
812  info->m_lowerLimit[srow] = -maxf > 0 ? 0 : -maxf;
813  info->m_upperLimit[srow] = -minf < 0 ? 0 : -minf;
814  }
815 
816  info->cfm[srow] = cfm;
817  srow += info->rowskip;
818  ++count;
819  }
820 
821  return count;
822 }
823 
824 
825 //override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
826 //If no axis is provided, it uses the default axis for this constraint.
828 {
829  if((axis >= 0) && (axis < 3))
830  {
831  switch(num)
832  {
833  case BT_CONSTRAINT_STOP_ERP :
834  m_linearLimits.m_stopERP[axis] = value;
836  break;
837  case BT_CONSTRAINT_STOP_CFM :
838  m_linearLimits.m_stopCFM[axis] = value;
840  break;
841  case BT_CONSTRAINT_ERP :
842  m_linearLimits.m_motorERP[axis] = value;
844  break;
845  case BT_CONSTRAINT_CFM :
846  m_linearLimits.m_motorCFM[axis] = value;
848  break;
849  default :
851  }
852  }
853  else if((axis >=3) && (axis < 6))
854  {
855  switch(num)
856  {
857  case BT_CONSTRAINT_STOP_ERP :
858  m_angularLimits[axis - 3].m_stopERP = value;
860  break;
861  case BT_CONSTRAINT_STOP_CFM :
862  m_angularLimits[axis - 3].m_stopCFM = value;
864  break;
865  case BT_CONSTRAINT_ERP :
866  m_angularLimits[axis - 3].m_motorERP = value;
868  break;
869  case BT_CONSTRAINT_CFM :
870  m_angularLimits[axis - 3].m_motorCFM = value;
872  break;
873  default :
875  }
876  }
877  else
878  {
880  }
881 }
882 
883 //return the local value of parameter
885 {
886  btScalar retVal = 0;
887  if((axis >= 0) && (axis < 3))
888  {
889  switch(num)
890  {
891  case BT_CONSTRAINT_STOP_ERP :
893  retVal = m_linearLimits.m_stopERP[axis];
894  break;
895  case BT_CONSTRAINT_STOP_CFM :
896  btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
897  retVal = m_linearLimits.m_stopCFM[axis];
898  break;
899  case BT_CONSTRAINT_ERP :
900  btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
901  retVal = m_linearLimits.m_motorERP[axis];
902  break;
903  case BT_CONSTRAINT_CFM :
904  btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
905  retVal = m_linearLimits.m_motorCFM[axis];
906  break;
907  default :
909  }
910  }
911  else if((axis >=3) && (axis < 6))
912  {
913  switch(num)
914  {
915  case BT_CONSTRAINT_STOP_ERP :
917  retVal = m_angularLimits[axis - 3].m_stopERP;
918  break;
919  case BT_CONSTRAINT_STOP_CFM :
920  btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
921  retVal = m_angularLimits[axis - 3].m_stopCFM;
922  break;
923  case BT_CONSTRAINT_ERP :
924  btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
925  retVal = m_angularLimits[axis - 3].m_motorERP;
926  break;
927  case BT_CONSTRAINT_CFM :
928  btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_MOTO2 << (axis * BT_6DOF_FLAGS_AXIS_SHIFT2)));
929  retVal = m_angularLimits[axis - 3].m_motorCFM;
930  break;
931  default :
933  }
934  }
935  else
936  {
938  }
939  return retVal;
940 }
941 
942 
943 
945 {
946  btVector3 zAxis = axis1.normalized();
947  btVector3 yAxis = axis2.normalized();
948  btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
949 
950  btTransform frameInW;
951  frameInW.setIdentity();
952  frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0],
953  xAxis[1], yAxis[1], zAxis[1],
954  xAxis[2], yAxis[2], zAxis[2]);
955 
956  // now get constraint frame in local coordinate systems
959 
961 }
962 
964 {
965  btAssert((index >= 0) && (index < 6));
966  if (index<3)
967  m_linearLimits.m_bounce[index] = bounce;
968  else
969  m_angularLimits[index - 3].m_bounce = bounce;
970 }
971 
973 {
974  btAssert((index >= 0) && (index < 6));
975  if (index<3)
976  m_linearLimits.m_enableMotor[index] = onOff;
977  else
978  m_angularLimits[index - 3].m_enableMotor = onOff;
979 }
980 
981 void btGeneric6DofSpring2Constraint::setServo(int index, bool onOff)
982 {
983  btAssert((index >= 0) && (index < 6));
984  if (index<3)
985  m_linearLimits.m_servoMotor[index] = onOff;
986  else
987  m_angularLimits[index - 3].m_servoMotor = onOff;
988 }
989 
991 {
992  btAssert((index >= 0) && (index < 6));
993  if (index<3)
994  m_linearLimits.m_targetVelocity[index] = velocity;
995  else
996  m_angularLimits[index - 3].m_targetVelocity = velocity;
997 }
998 
1000 {
1001  btAssert((index >= 0) && (index < 6));
1002  if (index<3)
1003  m_linearLimits.m_servoTarget[index] = target;
1004  else
1005  m_angularLimits[index - 3].m_servoTarget = target;
1006 }
1007 
1009 {
1010  btAssert((index >= 0) && (index < 6));
1011  if (index<3)
1012  m_linearLimits.m_maxMotorForce[index] = force;
1013  else
1014  m_angularLimits[index - 3].m_maxMotorForce = force;
1015 }
1016 
1018 {
1019  btAssert((index >= 0) && (index < 6));
1020  if (index<3)
1021  m_linearLimits.m_enableSpring[index] = onOff;
1022  else
1023  m_angularLimits[index - 3] .m_enableSpring = onOff;
1024 }
1025 
1027 {
1028  btAssert((index >= 0) && (index < 6));
1029  if (index<3)
1030  m_linearLimits.m_springStiffness[index] = stiffness;
1031  else
1032  m_angularLimits[index - 3] .m_springStiffness = stiffness;
1033 }
1034 
1036 {
1037  btAssert((index >= 0) && (index < 6));
1038  if (index<3)
1039  m_linearLimits.m_springDamping[index] = damping;
1040  else
1041  m_angularLimits[index - 3] .m_springDamping = damping;
1042 }
1043 
1045 {
1047  int i;
1048  for( i = 0; i < 3; i++)
1050  for(i = 0; i < 3; i++)
1052 }
1053 
1055 {
1056  btAssert((index >= 0) && (index < 6));
1058  if (index<3)
1060  else
1062 }
1063 
1065 {
1066  btAssert((index >= 0) && (index < 6));
1067  if (index<3)
1068  m_linearLimits.m_equilibriumPoint[index] = val;
1069  else
1070  m_angularLimits[index - 3] .m_equilibriumPoint = val;
1071 }
1072 
1073 
1075 
1077 {
1078  //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
1079  if(m_loLimit > m_hiLimit) {
1080  m_currentLimit = 0;
1081  m_currentLimitError = btScalar(0.f);
1082  }
1083  else if(m_loLimit == m_hiLimit) {
1084  m_currentLimitError = test_value - m_loLimit;
1085  m_currentLimit = 3;
1086  } else {
1087  m_currentLimitError = test_value - m_loLimit;
1088  m_currentLimitErrorHi = test_value - m_hiLimit;
1089  m_currentLimit = 4;
1090  }
1091 }
1092 
1094 
1096 {
1097  btScalar loLimit = m_lowerLimit[limitIndex];
1098  btScalar hiLimit = m_upperLimit[limitIndex];
1099  if(loLimit > hiLimit) {
1100  m_currentLimitError[limitIndex] = 0;
1101  m_currentLimit[limitIndex] = 0;
1102  }
1103  else if(loLimit == hiLimit) {
1104  m_currentLimitError[limitIndex] = test_value - loLimit;
1105  m_currentLimit[limitIndex] = 3;
1106  } else {
1107  m_currentLimitError[limitIndex] = test_value - loLimit;
1108  m_currentLimitErrorHi[limitIndex] = test_value - hiLimit;
1109  m_currentLimit[limitIndex] = 4;
1110  }
1111 }
1112 
1113 
#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)
const btVector3 & getAngularVelocity() const
Definition: btRigidBody.h:362
#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)
virtual btScalar getParam(int num, int axis=-1) const
return the local value of parameter
void setDamping(int index, btScalar damping)
const btTransform & getCenterOfMassTransform() const
Definition: btRigidBody.h:356
#define BT_ONE
Definition: btScalar.h:496
const btRigidBody & getRigidBodyA() const
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)
int setLinearLimits(btConstraintInfo2 *info, int row, const btTransform &transA, const btTransform &transB, const btVector3 &linVelA, const btVector3 &linVelB, const btVector3 &angVelA, const btVector3 &angVelB)
const btRigidBody & getRigidBodyB() const
static bool matrixToEulerZYX(const btMatrix3x3 &mat, btVector3 &xyz)
#define SIMD_HALF_PI
Definition: btScalar.h:478
#define BT_ZERO
Definition: btScalar.h:497
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:297
btVector3 normalized() const
Return a normalized version of this vector.
Definition: btVector3.h:952
btVector3 getAxis(int axis_index) const
btVector3 getColumn(int i) const
Get a column of the matrix as a vector.
Definition: btMatrix3x3.h:134
#define SIMD_INFINITY
Definition: btScalar.h:495
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:117
const btVector3 & getLinearVelocity() const
Definition: btRigidBody.h:359
btScalar getInvMass() const
Definition: btRigidBody.h:270
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:377
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
btScalar btAtan2(btScalar x, btScalar y)
Definition: btScalar.h:468
void setStiffness(int index, btScalar stiffness)
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
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&#39;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)
static btRigidBody & getFixedBody()
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)
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&#39;t use them directly
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)
btScalar btAsin(btScalar x)
Definition: btScalar.h:460
void setFrames(const btTransform &frameA, const btTransform &frameB)
btMatrix3x3 inverse() const
Return the inverse of the matrix.
Definition: btMatrix3x3.h:1046
virtual void getInfo1(btConstraintInfo1 *info)
internal method used by the constraint solver, don&#39;t use them directly
#define btAssertConstrParams(_par)
btTransform inverse() const
Return the inverse of this transform.
Definition: btTransform.h:188
void setServoTarget(int index, btScalar target)
virtual void getInfo2(btConstraintInfo2 *info)
internal method used by the constraint solver, don&#39;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)