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 
43 #include <cmath>
44 #include <new>
45 
47  : btTypedConstraint(D6_SPRING_2_CONSTRAINT_TYPE, rbA, rbB), m_frameInA(frameInA), m_frameInB(frameInB), m_rotateOrder(rotOrder), m_flags(0)
48 {
50 }
51 
53  : btTypedConstraint(D6_SPRING_2_CONSTRAINT_TYPE, getFixedBody(), rbB), m_frameInB(frameInB), m_rotateOrder(rotOrder), m_flags(0)
54 {
58 }
59 
61 {
62  int i = index % 3;
63  int j = index / 3;
64  return mat[i][j];
65 }
66 
67 // MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
68 
70 {
71  // rot = cy*cz -cy*sz sy
72  // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx
73  // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy
74 
75  btScalar fi = btGetMatrixElem(mat, 2);
76  if (fi < btScalar(1.0f))
77  {
78  if (fi > btScalar(-1.0f))
79  {
80  xyz[0] = btAtan2(-btGetMatrixElem(mat, 5), btGetMatrixElem(mat, 8));
81  xyz[1] = btAsin(btGetMatrixElem(mat, 2));
82  xyz[2] = btAtan2(-btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 0));
83  return true;
84  }
85  else
86  {
87  // WARNING. Not unique. XA - ZA = -atan2(r10,r11)
88  xyz[0] = -btAtan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4));
89  xyz[1] = -SIMD_HALF_PI;
90  xyz[2] = btScalar(0.0);
91  return false;
92  }
93  }
94  else
95  {
96  // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11)
97  xyz[0] = btAtan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4));
98  xyz[1] = SIMD_HALF_PI;
99  xyz[2] = 0.0;
100  }
101  return false;
102 }
103 
105 {
106  // rot = cy*cz -sz sy*cz
107  // cy*cx*sz+sx*sy cx*cz sy*cx*sz-cy*sx
108  // cy*sx*sz-cx*sy sx*cz sy*sx*sz+cx*cy
109 
110  btScalar fi = btGetMatrixElem(mat, 1);
111  if (fi < btScalar(1.0f))
112  {
113  if (fi > btScalar(-1.0f))
114  {
115  xyz[0] = btAtan2(btGetMatrixElem(mat, 7), btGetMatrixElem(mat, 4));
116  xyz[1] = btAtan2(btGetMatrixElem(mat, 2), btGetMatrixElem(mat, 0));
117  xyz[2] = btAsin(-btGetMatrixElem(mat, 1));
118  return true;
119  }
120  else
121  {
122  xyz[0] = -btAtan2(-btGetMatrixElem(mat, 6), btGetMatrixElem(mat, 8));
123  xyz[1] = btScalar(0.0);
124  xyz[2] = SIMD_HALF_PI;
125  return false;
126  }
127  }
128  else
129  {
130  xyz[0] = btAtan2(-btGetMatrixElem(mat, 6), btGetMatrixElem(mat, 8));
131  xyz[1] = 0.0;
132  xyz[2] = -SIMD_HALF_PI;
133  }
134  return false;
135 }
136 
138 {
139  // rot = cy*cz+sy*sx*sz cz*sy*sx-cy*sz cx*sy
140  // cx*sz cx*cz -sx
141  // cy*sx*sz-cz*sy sy*sz+cy*cz*sx cy*cx
142 
143  btScalar fi = btGetMatrixElem(mat, 5);
144  if (fi < btScalar(1.0f))
145  {
146  if (fi > btScalar(-1.0f))
147  {
148  xyz[0] = btAsin(-btGetMatrixElem(mat, 5));
149  xyz[1] = btAtan2(btGetMatrixElem(mat, 2), btGetMatrixElem(mat, 8));
150  xyz[2] = btAtan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4));
151  return true;
152  }
153  else
154  {
155  xyz[0] = SIMD_HALF_PI;
156  xyz[1] = -btAtan2(-btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 0));
157  xyz[2] = btScalar(0.0);
158  return false;
159  }
160  }
161  else
162  {
163  xyz[0] = -SIMD_HALF_PI;
164  xyz[1] = btAtan2(-btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 0));
165  xyz[2] = 0.0;
166  }
167  return false;
168 }
169 
171 {
172  // rot = cy*cz sy*sx-cy*cx*sz cx*sy+cy*sz*sx
173  // sz cz*cx -cz*sx
174  // -cz*sy cy*sx+cx*sy*sz cy*cx-sy*sz*sx
175 
176  btScalar fi = btGetMatrixElem(mat, 3);
177  if (fi < btScalar(1.0f))
178  {
179  if (fi > btScalar(-1.0f))
180  {
181  xyz[0] = btAtan2(-btGetMatrixElem(mat, 5), btGetMatrixElem(mat, 4));
182  xyz[1] = btAtan2(-btGetMatrixElem(mat, 6), btGetMatrixElem(mat, 0));
183  xyz[2] = btAsin(btGetMatrixElem(mat, 3));
184  return true;
185  }
186  else
187  {
188  xyz[0] = btScalar(0.0);
189  xyz[1] = -btAtan2(btGetMatrixElem(mat, 7), btGetMatrixElem(mat, 8));
190  xyz[2] = -SIMD_HALF_PI;
191  return false;
192  }
193  }
194  else
195  {
196  xyz[0] = btScalar(0.0);
197  xyz[1] = btAtan2(btGetMatrixElem(mat, 7), btGetMatrixElem(mat, 8));
198  xyz[2] = SIMD_HALF_PI;
199  }
200  return false;
201 }
202 
204 {
205  // rot = cz*cy-sz*sx*sy -cx*sz cz*sy+cy*sz*sx
206  // cy*sz+cz*sx*sy cz*cx sz*sy-cz*xy*sx
207  // -cx*sy sx cx*cy
208 
209  btScalar fi = btGetMatrixElem(mat, 7);
210  if (fi < btScalar(1.0f))
211  {
212  if (fi > btScalar(-1.0f))
213  {
214  xyz[0] = btAsin(btGetMatrixElem(mat, 7));
215  xyz[1] = btAtan2(-btGetMatrixElem(mat, 6), btGetMatrixElem(mat, 8));
216  xyz[2] = btAtan2(-btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 4));
217  return true;
218  }
219  else
220  {
221  xyz[0] = -SIMD_HALF_PI;
222  xyz[1] = btScalar(0.0);
223  xyz[2] = -btAtan2(btGetMatrixElem(mat, 2), btGetMatrixElem(mat, 0));
224  return false;
225  }
226  }
227  else
228  {
229  xyz[0] = SIMD_HALF_PI;
230  xyz[1] = btScalar(0.0);
231  xyz[2] = btAtan2(btGetMatrixElem(mat, 2), btGetMatrixElem(mat, 0));
232  }
233  return false;
234 }
235 
237 {
238  // rot = cz*cy cz*sy*sx-cx*sz sz*sx+cz*cx*sy
239  // cy*sz cz*cx+sz*sy*sx cx*sz*sy-cz*sx
240  // -sy cy*sx cy*cx
241 
242  btScalar fi = btGetMatrixElem(mat, 6);
243  if (fi < btScalar(1.0f))
244  {
245  if (fi > btScalar(-1.0f))
246  {
247  xyz[0] = btAtan2(btGetMatrixElem(mat, 7), btGetMatrixElem(mat, 8));
248  xyz[1] = btAsin(-btGetMatrixElem(mat, 6));
249  xyz[2] = btAtan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 0));
250  return true;
251  }
252  else
253  {
254  xyz[0] = btScalar(0.0);
255  xyz[1] = SIMD_HALF_PI;
256  xyz[2] = -btAtan2(btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 2));
257  return false;
258  }
259  }
260  else
261  {
262  xyz[0] = btScalar(0.0);
263  xyz[1] = -SIMD_HALF_PI;
264  xyz[2] = btAtan2(-btGetMatrixElem(mat, 1), -btGetMatrixElem(mat, 2));
265  }
266  return false;
267 }
268 
270 {
272  switch (m_rotateOrder)
273  {
274  case RO_XYZ:
276  break;
277  case RO_XZY:
279  break;
280  case RO_YXZ:
282  break;
283  case RO_YZX:
285  break;
286  case RO_ZXY:
288  break;
289  case RO_ZYX:
291  break;
292  default:
293  btAssert(false);
294  }
295  // in euler angle mode we do not actually constrain the angular velocity
296  // along the axes axis[0] and axis[2] (although we do use axis[1]) :
297  //
298  // to get constrain w2-w1 along ...not
299  // ------ --------------------- ------
300  // d(angle[0])/dt = 0 ax[1] x ax[2] ax[0]
301  // d(angle[1])/dt = 0 ax[1]
302  // d(angle[2])/dt = 0 ax[0] x ax[1] ax[2]
303  //
304  // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
305  // to prove the result for angle[0], write the expression for angle[0] from
306  // GetInfo1 then take the derivative. to prove this for angle[2] it is
307  // easier to take the euler rate expression for d(angle[2])/dt with respect
308  // to the components of w and set that to 0.
309  switch (m_rotateOrder)
310  {
311  case RO_XYZ:
312  {
313  //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)
314  //The two planes are non-homologous, so this is a Tait Bryan angle formalism and not a proper Euler
315  //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)
316  //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)
317  // x' = Nperp = N.cross(axis2)
318  // y' = N = axis2.cross(axis0)
319  // z' = z
320  //
321  // x" = X
322  // y" = y'
323  // z" = ??
324  //in other words:
325  //first rotate around z
326  //second rotate around y'= z.cross(X)
327  //third rotate around x" = X
328  //Original XYZ extrinsic rotation order.
329  //Planes: xy and YZ normals: z, X. Plane intersection (N) is z.cross(X)
332  m_calculatedAxis[1] = axis2.cross(axis0);
333  m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
334  m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
335  break;
336  }
337  case RO_XZY:
338  {
339  //planes: xz,ZY normals: y, X
340  //first rotate around y
341  //second rotate around z'= y.cross(X)
342  //third rotate around x" = X
345  m_calculatedAxis[2] = axis0.cross(axis1);
346  m_calculatedAxis[0] = axis1.cross(m_calculatedAxis[2]);
347  m_calculatedAxis[1] = m_calculatedAxis[2].cross(axis0);
348  break;
349  }
350  case RO_YXZ:
351  {
352  //planes: yx,XZ normals: z, Y
353  //first rotate around z
354  //second rotate around x'= z.cross(Y)
355  //third rotate around y" = Y
358  m_calculatedAxis[0] = axis1.cross(axis2);
359  m_calculatedAxis[1] = axis2.cross(m_calculatedAxis[0]);
360  m_calculatedAxis[2] = m_calculatedAxis[0].cross(axis1);
361  break;
362  }
363  case RO_YZX:
364  {
365  //planes: yz,ZX normals: x, Y
366  //first rotate around x
367  //second rotate around z'= x.cross(Y)
368  //third rotate around y" = Y
371  m_calculatedAxis[2] = axis0.cross(axis1);
372  m_calculatedAxis[0] = axis1.cross(m_calculatedAxis[2]);
373  m_calculatedAxis[1] = m_calculatedAxis[2].cross(axis0);
374  break;
375  }
376  case RO_ZXY:
377  {
378  //planes: zx,XY normals: y, Z
379  //first rotate around y
380  //second rotate around x'= y.cross(Z)
381  //third rotate around z" = Z
384  m_calculatedAxis[0] = axis1.cross(axis2);
385  m_calculatedAxis[1] = axis2.cross(m_calculatedAxis[0]);
386  m_calculatedAxis[2] = m_calculatedAxis[0].cross(axis1);
387  break;
388  }
389  case RO_ZYX:
390  {
391  //planes: zy,YX normals: x, Z
392  //first rotate around x
393  //second rotate around y' = x.cross(Z)
394  //third rotate around z" = Z
397  m_calculatedAxis[1] = axis2.cross(axis0);
398  m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
399  m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
400  break;
401  }
402  default:
403  btAssert(false);
404  }
405 
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 
439 {
440  btScalar angle = m_calculatedAxisAngleDiff[axis_index];
441  angle = btAdjustAngleToLimits(angle, m_angularLimits[axis_index].m_loLimit, m_angularLimits[axis_index].m_hiLimit);
442  m_angularLimits[axis_index].m_currentPosition = angle;
443  m_angularLimits[axis_index].testLimitValue(angle);
444 }
445 
447 {
448  //prepare constraint
450  info->m_numConstraintRows = 0;
451  info->nub = 0;
452  int i;
453  //test linear limits
454  for (i = 0; i < 3; i++)
455  {
456  if (m_linearLimits.m_currentLimit[i] == 4)
457  info->m_numConstraintRows += 2;
458  else if (m_linearLimits.m_currentLimit[i] != 0)
459  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)
468  info->m_numConstraintRows += 2;
469  else if (m_angularLimits[i].m_currentLimit != 0)
470  info->m_numConstraintRows += 1;
471  if (m_angularLimits[i].m_enableMotor) info->m_numConstraintRows += 1;
472  if (m_angularLimits[i].m_enableSpring) info->m_numConstraintRows += 1;
473  }
474 }
475 
477 {
478  const btTransform& transA = m_rbA.getCenterOfMassTransform();
479  const btTransform& transB = m_rbB.getCenterOfMassTransform();
480  const btVector3& linVelA = m_rbA.getLinearVelocity();
481  const btVector3& linVelB = m_rbB.getLinearVelocity();
482  const btVector3& angVelA = m_rbA.getAngularVelocity();
483  const btVector3& angVelB = m_rbB.getAngularVelocity();
484 
485  // for stability better to solve angular limits first
486  int row = setAngularLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
487  setLinearLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
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  return row;
544 }
545 
546 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)
547 {
548  int row = row_offset;
549 
550  //order of rotational constraint rows
551  int cIdx[] = {0, 1, 2};
552  switch (m_rotateOrder)
553  {
554  case RO_XYZ:
555  cIdx[0] = 0;
556  cIdx[1] = 1;
557  cIdx[2] = 2;
558  break;
559  case RO_XZY:
560  cIdx[0] = 0;
561  cIdx[1] = 2;
562  cIdx[2] = 1;
563  break;
564  case RO_YXZ:
565  cIdx[0] = 1;
566  cIdx[1] = 0;
567  cIdx[2] = 2;
568  break;
569  case RO_YZX:
570  cIdx[0] = 1;
571  cIdx[1] = 2;
572  cIdx[2] = 0;
573  break;
574  case RO_ZXY:
575  cIdx[0] = 2;
576  cIdx[1] = 0;
577  cIdx[2] = 1;
578  break;
579  case RO_ZYX:
580  cIdx[0] = 2;
581  cIdx[1] = 1;
582  cIdx[2] = 0;
583  break;
584  default:
585  btAssert(false);
586  }
587 
588  for (int ii = 0; ii < 3; ii++)
589  {
590  int i = cIdx[ii];
591  if (m_angularLimits[i].m_currentLimit || m_angularLimits[i].m_enableMotor || m_angularLimits[i].m_enableSpring)
592  {
593  btVector3 axis = getAxis(i);
594  int flags = m_flags >> ((i + 3) * BT_6DOF_FLAGS_AXIS_SHIFT2);
595  if (!(flags & BT_6DOF_FLAGS_CFM_STOP2))
596  {
597  m_angularLimits[i].m_stopCFM = info->cfm[0];
598  }
599  if (!(flags & BT_6DOF_FLAGS_ERP_STOP2))
600  {
601  m_angularLimits[i].m_stopERP = info->erp;
602  }
603  if (!(flags & BT_6DOF_FLAGS_CFM_MOTO2))
604  {
605  m_angularLimits[i].m_motorCFM = info->cfm[0];
606  }
607  if (!(flags & BT_6DOF_FLAGS_ERP_MOTO2))
608  {
609  m_angularLimits[i].m_motorERP = info->erp;
610  }
611  row += get_limit_motor_info2(&m_angularLimits[i], transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 1);
612  }
613  }
614 
615  return row;
616 }
617 
619 {
620  m_frameInA = frameA;
621  m_frameInB = frameB;
622  buildJacobian();
624 }
625 
627 {
630  for (int i = 0; i < 3; i++)
631  {
634  }
635 }
636 
637 void btGeneric6DofSpring2Constraint::calculateJacobi(btRotationalLimitMotor2* limot, const btTransform& transA, const btTransform& transB, btConstraintInfo2* info, int srow, btVector3& ax1, int rotational, int rotAllowed)
638 {
639  btScalar* J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
640  btScalar* J2 = rotational ? info->m_J2angularAxis : info->m_J2linearAxis;
641 
642  J1[srow + 0] = ax1[0];
643  J1[srow + 1] = ax1[1];
644  J1[srow + 2] = ax1[2];
645 
646  J2[srow + 0] = -ax1[0];
647  J2[srow + 1] = -ax1[1];
648  J2[srow + 2] = -ax1[2];
649 
650  if (!rotational)
651  {
652  btVector3 tmpA, tmpB, relA, relB;
653  // get vector from bodyB to frameB in WCS
654  relB = m_calculatedTransformB.getOrigin() - transB.getOrigin();
655  // same for bodyA
656  relA = m_calculatedTransformA.getOrigin() - transA.getOrigin();
657  tmpA = relA.cross(ax1);
658  tmpB = relB.cross(ax1);
659  if (m_hasStaticBody && (!rotAllowed))
660  {
661  tmpA *= m_factA;
662  tmpB *= m_factB;
663  }
664  int i;
665  for (i = 0; i < 3; i++) info->m_J1angularAxis[srow + i] = tmpA[i];
666  for (i = 0; i < 3; i++) info->m_J2angularAxis[srow + i] = -tmpB[i];
667  }
668 }
669 
672  const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB,
673  btConstraintInfo2* info, int row, btVector3& ax1, int rotational, int rotAllowed)
674 {
675  int count = 0;
676  int srow = row * info->rowskip;
677 
678  if (limot->m_currentLimit == 4)
679  {
680  btScalar vel = rotational ? angVelA.dot(ax1) - angVelB.dot(ax1) : linVelA.dot(ax1) - linVelB.dot(ax1);
681 
682  calculateJacobi(limot, transA, transB, info, srow, ax1, rotational, rotAllowed);
683  info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitError * (rotational ? -1 : 1);
684  if (rotational)
685  {
686  if (info->m_constraintError[srow] - vel * limot->m_stopERP > 0)
687  {
688  btScalar bounceerror = -limot->m_bounce * vel;
689  if (bounceerror > info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror;
690  }
691  }
692  else
693  {
694  if (info->m_constraintError[srow] - vel * limot->m_stopERP < 0)
695  {
696  btScalar bounceerror = -limot->m_bounce * vel;
697  if (bounceerror < info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror;
698  }
699  }
700  info->m_lowerLimit[srow] = rotational ? 0 : -SIMD_INFINITY;
701  info->m_upperLimit[srow] = rotational ? SIMD_INFINITY : 0;
702  info->cfm[srow] = limot->m_stopCFM;
703  srow += info->rowskip;
704  ++count;
705 
706  calculateJacobi(limot, transA, transB, info, srow, ax1, rotational, rotAllowed);
707  info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitErrorHi * (rotational ? -1 : 1);
708  if (rotational)
709  {
710  if (info->m_constraintError[srow] - vel * limot->m_stopERP < 0)
711  {
712  btScalar bounceerror = -limot->m_bounce * vel;
713  if (bounceerror < info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror;
714  }
715  }
716  else
717  {
718  if (info->m_constraintError[srow] - vel * limot->m_stopERP > 0)
719  {
720  btScalar bounceerror = -limot->m_bounce * vel;
721  if (bounceerror > info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror;
722  }
723  }
724  info->m_lowerLimit[srow] = rotational ? -SIMD_INFINITY : 0;
725  info->m_upperLimit[srow] = rotational ? 0 : SIMD_INFINITY;
726  info->cfm[srow] = limot->m_stopCFM;
727  srow += info->rowskip;
728  ++count;
729  }
730  else if (limot->m_currentLimit == 3)
731  {
732  calculateJacobi(limot, transA, transB, info, srow, ax1, rotational, rotAllowed);
733  info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitError * (rotational ? -1 : 1);
734  info->m_lowerLimit[srow] = -SIMD_INFINITY;
735  info->m_upperLimit[srow] = SIMD_INFINITY;
736  info->cfm[srow] = limot->m_stopCFM;
737  srow += info->rowskip;
738  ++count;
739  }
740 
741  if (limot->m_enableMotor && !limot->m_servoMotor)
742  {
743  calculateJacobi(limot, transA, transB, info, srow, ax1, rotational, rotAllowed);
744  btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;
745  btScalar mot_fact = getMotorFactor(limot->m_currentPosition,
746  limot->m_loLimit,
747  limot->m_hiLimit,
748  tag_vel,
749  info->fps * limot->m_motorERP);
750  info->m_constraintError[srow] = mot_fact * limot->m_targetVelocity;
751  info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps;
752  info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps;
753  info->cfm[srow] = limot->m_motorCFM;
754  srow += info->rowskip;
755  ++count;
756  }
757 
758  if (limot->m_enableMotor && limot->m_servoMotor)
759  {
760  btScalar error = limot->m_currentPosition - limot->m_servoTarget;
761  btScalar curServoTarget = limot->m_servoTarget;
762  if (rotational)
763  {
764  if (error > SIMD_PI)
765  {
766  error -= SIMD_2_PI;
767  curServoTarget += SIMD_2_PI;
768  }
769  if (error < -SIMD_PI)
770  {
771  error += SIMD_2_PI;
772  curServoTarget -= SIMD_2_PI;
773  }
774  }
775 
776  calculateJacobi(limot, transA, transB, info, srow, ax1, rotational, rotAllowed);
777  btScalar targetvelocity = error < 0 ? -limot->m_targetVelocity : limot->m_targetVelocity;
778  btScalar tag_vel = -targetvelocity;
779  btScalar mot_fact;
780  if (error != 0)
781  {
782  btScalar lowLimit;
783  btScalar hiLimit;
784  if (limot->m_loLimit > limot->m_hiLimit)
785  {
786  lowLimit = error > 0 ? curServoTarget : -SIMD_INFINITY;
787  hiLimit = error < 0 ? curServoTarget : SIMD_INFINITY;
788  }
789  else
790  {
791  lowLimit = error > 0 && curServoTarget > limot->m_loLimit ? curServoTarget : limot->m_loLimit;
792  hiLimit = error < 0 && curServoTarget < limot->m_hiLimit ? curServoTarget : limot->m_hiLimit;
793  }
794  mot_fact = getMotorFactor(limot->m_currentPosition, lowLimit, hiLimit, tag_vel, info->fps * limot->m_motorERP);
795  }
796  else
797  {
798  mot_fact = 0;
799  }
800  info->m_constraintError[srow] = mot_fact * targetvelocity * (rotational ? -1 : 1);
801  info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps;
802  info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps;
803  info->cfm[srow] = limot->m_motorCFM;
804  srow += info->rowskip;
805  ++count;
806  }
807 
808  if (limot->m_enableSpring)
809  {
810  btScalar error = limot->m_currentPosition - limot->m_equilibriumPoint;
811  calculateJacobi(limot, transA, transB, info, srow, ax1, rotational, rotAllowed);
812 
813  //btScalar cfm = 1.0 / ((1.0/info->fps)*limot->m_springStiffness+ limot->m_springDamping);
814  //if(cfm > 0.99999)
815  // cfm = 0.99999;
816  //btScalar erp = (1.0/info->fps)*limot->m_springStiffness / ((1.0/info->fps)*limot->m_springStiffness + limot->m_springDamping);
817  //info->m_constraintError[srow] = info->fps * erp * error * (rotational ? -1.0 : 1.0);
818  //info->m_lowerLimit[srow] = -SIMD_INFINITY;
819  //info->m_upperLimit[srow] = SIMD_INFINITY;
820 
821  btScalar dt = BT_ONE / info->fps;
822  btScalar kd = limot->m_springDamping;
823  btScalar ks = limot->m_springStiffness;
824  btScalar vel;
825  if (rotational)
826  {
827  vel = angVelA.dot(ax1) - angVelB.dot(ax1);
828  }
829  else
830  {
831  btVector3 tanVelA = angVelA.cross(m_calculatedTransformA.getOrigin() - transA.getOrigin());
832  btVector3 tanVelB = angVelB.cross(m_calculatedTransformB.getOrigin() - transB.getOrigin());
833  vel = (linVelA + tanVelA).dot(ax1) - (linVelB + tanVelB).dot(ax1);
834  }
835  btScalar cfm = BT_ZERO;
836  btScalar mA = BT_ONE / m_rbA.getInvMass();
837  btScalar mB = BT_ONE / m_rbB.getInvMass();
838  if (rotational)
839  {
840  btScalar rrA = (m_calculatedTransformA.getOrigin() - transA.getOrigin()).length2();
841  btScalar rrB = (m_calculatedTransformB.getOrigin() - transB.getOrigin()).length2();
842  if (m_rbA.getInvMass()) mA = mA * rrA + 1 / (m_rbA.getInvInertiaTensorWorld() * ax1).length();
843  if (m_rbB.getInvMass()) mB = mB * rrB + 1 / (m_rbB.getInvInertiaTensorWorld() * ax1).length();
844  }
845  btScalar m;
846  if (m_rbA.getInvMass() == 0) m = mB; else
847  if (m_rbB.getInvMass() == 0) m = mA; else
848  m = mA*mB / (mA + mB);
849  btScalar angularfreq = btSqrt(ks / m);
850 
851  //limit stiffness (the spring should not be sampled faster that the quarter of its angular frequency)
852  if (limot->m_springStiffnessLimited && 0.25 < angularfreq * dt)
853  {
854  ks = BT_ONE / dt / dt / btScalar(16.0) * m;
855  }
856  //avoid damping that would blow up the spring
857  if (limot->m_springDampingLimited && kd * dt > m)
858  {
859  kd = m / dt;
860  }
861  btScalar fs = ks * error * dt;
862  btScalar fd = -kd * (vel) * (rotational ? -1 : 1) * dt;
863  btScalar f = (fs + fd);
864 
865  // after the spring force affecting the body(es) the new velocity will be
866  // vel + f / m * (rotational ? -1 : 1)
867  // so in theory this should be set here for m_constraintError
868  // (with m_constraintError we set a desired velocity for the affected body(es))
869  // however in practice any value is fine as long as it is greater than the "proper" velocity,
870  // because the m_lowerLimit and the m_upperLimit will determinate the strength of the final pulling force
871  // so it is much simpler (and more robust) just to simply use inf (with the proper sign)
872  // (Even with our best intent the "new" velocity is only an estimation. If we underestimate
873  // the "proper" velocity that will weaken the spring, however if we overestimate it, it doesn't
874  // matter, because the solver will limit it according the force limit)
875  // you may also wonder what if the current velocity (vel) so high that the pulling force will not change its direction (in this iteration)
876  // will we not request a velocity with the wrong direction ?
877  // and the answer is not, because in practice during the solving the current velocity is subtracted from the m_constraintError
878  // so the sign of the force that is really matters
879  info->m_constraintError[srow] = (rotational ? -1 : 1) * (f < 0 ? -SIMD_INFINITY : SIMD_INFINITY);
880 
881  btScalar minf = f < fd ? f : fd;
882  btScalar maxf = f < fd ? fd : f;
883  if (!rotational)
884  {
885  info->m_lowerLimit[srow] = minf > 0 ? 0 : minf;
886  info->m_upperLimit[srow] = maxf < 0 ? 0 : maxf;
887  }
888  else
889  {
890  info->m_lowerLimit[srow] = -maxf > 0 ? 0 : -maxf;
891  info->m_upperLimit[srow] = -minf < 0 ? 0 : -minf;
892  }
893 
894  info->cfm[srow] = cfm;
895  srow += info->rowskip;
896  ++count;
897  }
898 
899  return count;
900 }
901 
902 //override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
903 //If no axis is provided, it uses the default axis for this constraint.
905 {
906  if ((axis >= 0) && (axis < 3))
907  {
908  switch (num)
909  {
911  m_linearLimits.m_stopERP[axis] = value;
913  break;
915  m_linearLimits.m_stopCFM[axis] = value;
917  break;
918  case BT_CONSTRAINT_ERP:
919  m_linearLimits.m_motorERP[axis] = value;
921  break;
922  case BT_CONSTRAINT_CFM:
923  m_linearLimits.m_motorCFM[axis] = value;
925  break;
926  default:
928  }
929  }
930  else if ((axis >= 3) && (axis < 6))
931  {
932  switch (num)
933  {
935  m_angularLimits[axis - 3].m_stopERP = value;
937  break;
939  m_angularLimits[axis - 3].m_stopCFM = value;
941  break;
942  case BT_CONSTRAINT_ERP:
943  m_angularLimits[axis - 3].m_motorERP = value;
945  break;
946  case BT_CONSTRAINT_CFM:
947  m_angularLimits[axis - 3].m_motorCFM = value;
949  break;
950  default:
952  }
953  }
954  else
955  {
957  }
958 }
959 
960 //return the local value of parameter
962 {
963  btScalar retVal = 0;
964  if ((axis >= 0) && (axis < 3))
965  {
966  switch (num)
967  {
970  retVal = m_linearLimits.m_stopERP[axis];
971  break;
974  retVal = m_linearLimits.m_stopCFM[axis];
975  break;
976  case BT_CONSTRAINT_ERP:
978  retVal = m_linearLimits.m_motorERP[axis];
979  break;
980  case BT_CONSTRAINT_CFM:
982  retVal = m_linearLimits.m_motorCFM[axis];
983  break;
984  default:
986  }
987  }
988  else if ((axis >= 3) && (axis < 6))
989  {
990  switch (num)
991  {
994  retVal = m_angularLimits[axis - 3].m_stopERP;
995  break;
998  retVal = m_angularLimits[axis - 3].m_stopCFM;
999  break;
1000  case BT_CONSTRAINT_ERP:
1002  retVal = m_angularLimits[axis - 3].m_motorERP;
1003  break;
1004  case BT_CONSTRAINT_CFM:
1006  retVal = m_angularLimits[axis - 3].m_motorCFM;
1007  break;
1008  default:
1010  }
1011  }
1012  else
1013  {
1015  }
1016  return retVal;
1017 }
1018 
1020 {
1021  btVector3 zAxis = axis1.normalized();
1022  btVector3 yAxis = axis2.normalized();
1023  btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
1024 
1025  btTransform frameInW;
1026  frameInW.setIdentity();
1027  frameInW.getBasis().setValue(xAxis[0], yAxis[0], zAxis[0],
1028  xAxis[1], yAxis[1], zAxis[1],
1029  xAxis[2], yAxis[2], zAxis[2]);
1030 
1031  // now get constraint frame in local coordinate systems
1034 
1036 }
1037 
1039 {
1040  btAssert((index >= 0) && (index < 6));
1041  if (index < 3)
1042  m_linearLimits.m_bounce[index] = bounce;
1043  else
1044  m_angularLimits[index - 3].m_bounce = bounce;
1045 }
1046 
1048 {
1049  btAssert((index >= 0) && (index < 6));
1050  if (index < 3)
1051  m_linearLimits.m_enableMotor[index] = onOff;
1052  else
1053  m_angularLimits[index - 3].m_enableMotor = onOff;
1054 }
1055 
1056 void btGeneric6DofSpring2Constraint::setServo(int index, bool onOff)
1057 {
1058  btAssert((index >= 0) && (index < 6));
1059  if (index < 3)
1060  m_linearLimits.m_servoMotor[index] = onOff;
1061  else
1062  m_angularLimits[index - 3].m_servoMotor = onOff;
1063 }
1064 
1066 {
1067  btAssert((index >= 0) && (index < 6));
1068  if (index < 3)
1069  m_linearLimits.m_targetVelocity[index] = velocity;
1070  else
1071  m_angularLimits[index - 3].m_targetVelocity = velocity;
1072 }
1073 
1075 {
1076  btAssert((index >= 0) && (index < 6));
1077  if (index < 3)
1078  {
1079  m_linearLimits.m_servoTarget[index] = targetOrg;
1080  }
1081  else
1082  {
1083  //wrap between -PI and PI, see also
1084  //https://stackoverflow.com/questions/4633177/c-how-to-wrap-a-float-to-the-interval-pi-pi
1085 
1086  btScalar target = targetOrg + SIMD_PI;
1087  if (1)
1088  {
1089  btScalar m = target - SIMD_2_PI * std::floor(target / SIMD_2_PI);
1090  // handle boundary cases resulted from floating-point cut off:
1091  {
1092  if (m >= SIMD_2_PI)
1093  {
1094  target = 0;
1095  }
1096  else
1097  {
1098  if (m < 0)
1099  {
1100  if (SIMD_2_PI + m == SIMD_2_PI)
1101  target = 0;
1102  else
1103  target = SIMD_2_PI + m;
1104  }
1105  else
1106  {
1107  target = m;
1108  }
1109  }
1110  }
1111  target -= SIMD_PI;
1112  }
1113 
1114  m_angularLimits[index - 3].m_servoTarget = target;
1115  }
1116 }
1117 
1119 {
1120  btAssert((index >= 0) && (index < 6));
1121  if (index < 3)
1122  m_linearLimits.m_maxMotorForce[index] = force;
1123  else
1124  m_angularLimits[index - 3].m_maxMotorForce = force;
1125 }
1126 
1128 {
1129  btAssert((index >= 0) && (index < 6));
1130  if (index < 3)
1131  m_linearLimits.m_enableSpring[index] = onOff;
1132  else
1133  m_angularLimits[index - 3].m_enableSpring = onOff;
1134 }
1135 
1136 void btGeneric6DofSpring2Constraint::setStiffness(int index, btScalar stiffness, bool limitIfNeeded)
1137 {
1138  btAssert((index >= 0) && (index < 6));
1139  if (index < 3)
1140  {
1141  m_linearLimits.m_springStiffness[index] = stiffness;
1142  m_linearLimits.m_springStiffnessLimited[index] = limitIfNeeded;
1143  }
1144  else
1145  {
1146  m_angularLimits[index - 3].m_springStiffness = stiffness;
1147  m_angularLimits[index - 3].m_springStiffnessLimited = limitIfNeeded;
1148  }
1149 }
1150 
1151 void btGeneric6DofSpring2Constraint::setDamping(int index, btScalar damping, bool limitIfNeeded)
1152 {
1153  btAssert((index >= 0) && (index < 6));
1154  if (index < 3)
1155  {
1156  m_linearLimits.m_springDamping[index] = damping;
1157  m_linearLimits.m_springDampingLimited[index] = limitIfNeeded;
1158  }
1159  else
1160  {
1161  m_angularLimits[index - 3].m_springDamping = damping;
1162  m_angularLimits[index - 3].m_springDampingLimited = limitIfNeeded;
1163  }
1164 }
1165 
1167 {
1169  int i;
1170  for (i = 0; i < 3; i++)
1172  for (i = 0; i < 3; i++)
1173  m_angularLimits[i].m_equilibriumPoint = m_calculatedAxisAngleDiff[i];
1174 }
1175 
1177 {
1178  btAssert((index >= 0) && (index < 6));
1180  if (index < 3)
1182  else
1184 }
1185 
1187 {
1188  btAssert((index >= 0) && (index < 6));
1189  if (index < 3)
1190  m_linearLimits.m_equilibriumPoint[index] = val;
1191  else
1192  m_angularLimits[index - 3].m_equilibriumPoint = val;
1193 }
1194 
1196 
1198 {
1199  //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
1200  if (m_loLimit > m_hiLimit)
1201  {
1202  m_currentLimit = 0;
1204  }
1205  else if (m_loLimit == m_hiLimit)
1206  {
1207  m_currentLimitError = test_value - m_loLimit;
1208  m_currentLimit = 3;
1209  }
1210  else
1211  {
1212  m_currentLimitError = test_value - m_loLimit;
1213  m_currentLimitErrorHi = test_value - m_hiLimit;
1214  m_currentLimit = 4;
1215  }
1216 }
1217 
1219 
1221 {
1222  btScalar loLimit = m_lowerLimit[limitIndex];
1223  btScalar hiLimit = m_upperLimit[limitIndex];
1224  if (loLimit > hiLimit)
1225  {
1226  m_currentLimitError[limitIndex] = 0;
1227  m_currentLimit[limitIndex] = 0;
1228  }
1229  else if (loLimit == hiLimit)
1230  {
1231  m_currentLimitError[limitIndex] = test_value - loLimit;
1232  m_currentLimit[limitIndex] = 3;
1233  }
1234  else
1235  {
1236  m_currentLimitError[limitIndex] = test_value - loLimit;
1237  m_currentLimitErrorHi[limitIndex] = test_value - hiLimit;
1238  m_currentLimit[limitIndex] = 4;
1239  }
1240 }
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:543
btTranslationalLimitMotor2::m_springStiffnessLimited
bool m_springStiffnessLimited[3]
Definition: btGeneric6DofSpring2Constraint.h:178
BT_CONSTRAINT_CFM
Definition: btTypedConstraint.h:53
btTypedConstraint
TypedConstraint is the baseclass for Bullet constraints and vehicles.
Definition: btTypedConstraint.h:74
btTranslationalLimitMotor2::m_motorERP
btVector3 m_motorERP
Definition: btGeneric6DofSpring2Constraint.h:171
btGeneric6DofSpring2Constraint::calculateAngleInfo
void calculateAngleInfo()
Definition: btGeneric6DofSpring2Constraint.cpp:269
btRotationalLimitMotor2::m_servoMotor
bool m_servoMotor
Definition: btGeneric6DofSpring2Constraint.h:83
RO_XZY
Definition: btGeneric6DofSpring2Constraint.h:60
btRigidBody
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:59
RO_XYZ
Definition: btGeneric6DofSpring2Constraint.h:59
btGeneric6DofSpring2Constraint::m_factB
btScalar m_factB
Definition: btGeneric6DofSpring2Constraint.h:293
btAssertConstrParams
#define btAssertConstrParams(_par)
Definition: btTypedConstraint.h:58
btGeneric6DofSpring2Constraint::setParam
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...
Definition: btGeneric6DofSpring2Constraint.cpp:904
dot
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:888
btTransform::inverse
btTransform inverse() const
Return the inverse of this transform.
Definition: btTransform.h:182
btTranslationalLimitMotor2::m_currentLimitError
btVector3 m_currentLimitError
Definition: btGeneric6DofSpring2Constraint.h:185
btTranslationalLimitMotor2::m_targetVelocity
btVector3 m_targetVelocity
Definition: btGeneric6DofSpring2Constraint.h:182
btGeneric6DofSpring2Constraint::setBounce
void setBounce(int index, btScalar bounce)
Definition: btGeneric6DofSpring2Constraint.cpp:1038
SIMD_HALF_PI
#define SIMD_HALF_PI
Definition: btScalar.h:528
RO_YZX
Definition: btGeneric6DofSpring2Constraint.h:62
btRotationalLimitMotor2::m_equilibriumPoint
btScalar m_equilibriumPoint
Definition: btGeneric6DofSpring2Constraint.h:90
btRigidBody::getAngularVelocity
const btVector3 & getAngularVelocity() const
Definition: btRigidBody.h:402
RotateOrder
RotateOrder
Definition: btGeneric6DofSpring2Constraint.h:57
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
btGeneric6DofSpring2Constraint::setLinearLimits
int setLinearLimits(btConstraintInfo2 *info, int row, const btTransform &transA, const btTransform &transB, const btVector3 &linVelA, const btVector3 &linVelB, const btVector3 &angVelA, const btVector3 &angVelB)
Definition: btGeneric6DofSpring2Constraint.cpp:490
btTypedConstraint::getRigidBodyA
const btRigidBody & getRigidBodyA() const
Definition: btTypedConstraint.h:214
btMatrix3x3::inverse
btMatrix3x3 inverse() const
Return the inverse of the matrix.
Definition: btMatrix3x3.h:1077
btTranslationalLimitMotor2::m_enableSpring
bool m_enableSpring[3]
Definition: btGeneric6DofSpring2Constraint.h:175
btGeneric6DofSpring2Constraint::setMaxMotorForce
void setMaxMotorForce(int index, btScalar force)
Definition: btGeneric6DofSpring2Constraint.cpp:1118
btTranslationalLimitMotor2::m_currentLimitErrorHi
btVector3 m_currentLimitErrorHi
Definition: btGeneric6DofSpring2Constraint.h:186
btTranslationalLimitMotor2::m_currentLimit
int m_currentLimit[3]
Definition: btGeneric6DofSpring2Constraint.h:188
btGeneric6DofSpring2Constraint::m_calculatedAxis
btVector3 m_calculatedAxis[3]
Definition: btGeneric6DofSpring2Constraint.h:290
btTranslationalLimitMotor2::m_stopERP
btVector3 m_stopERP
Definition: btGeneric6DofSpring2Constraint.h:169
btVector3::cross
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:380
btRotationalLimitMotor2::m_currentPosition
btScalar m_currentPosition
Definition: btGeneric6DofSpring2Constraint.h:94
btGeneric6DofSpring2Constraint::matrixToEulerYXZ
static bool matrixToEulerYXZ(const btMatrix3x3 &mat, btVector3 &xyz)
Definition: btGeneric6DofSpring2Constraint.cpp:137
btRotationalLimitMotor2::m_currentLimitErrorHi
btScalar m_currentLimitErrorHi
Definition: btGeneric6DofSpring2Constraint.h:93
btGeneric6DofSpring2Constraint.h
btTypedConstraint::btConstraintInfo2::m_J2angularAxis
btScalar * m_J2angularAxis
Definition: btTypedConstraint.h:130
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:229
btTranslationalLimitMotor2::m_bounce
btVector3 m_bounce
Definition: btGeneric6DofSpring2Constraint.h:168
SIMD_PI
#define SIMD_PI
Definition: btScalar.h:526
btRigidBody.h
btGeneric6DofSpring2Constraint::setStiffness
void setStiffness(int index, btScalar stiffness, bool limitIfNeeded=true)
Definition: btGeneric6DofSpring2Constraint.cpp:1136
btGeneric6DofSpring2Constraint::getAxis
btVector3 getAxis(int axis_index) const
Definition: btGeneric6DofSpring2Constraint.h:346
btTypedConstraint::btConstraintInfo2::m_J2linearAxis
btScalar * m_J2linearAxis
Definition: btTypedConstraint.h:130
btTranslationalLimitMotor2::testLimitValue
void testLimitValue(int limitIndex, btScalar test_value)
Definition: btGeneric6DofSpring2Constraint.cpp:1220
btGeneric6DofSpring2Constraint::m_factA
btScalar m_factA
Definition: btGeneric6DofSpring2Constraint.h:292
btTranslationalLimitMotor2::m_servoTarget
btVector3 m_servoTarget
Definition: btGeneric6DofSpring2Constraint.h:176
btTypedConstraint::btConstraintInfo1::m_numConstraintRows
int m_numConstraintRows
Definition: btTypedConstraint.h:115
btTransformUtil.h
btRigidBody::getCenterOfMassTransform
const btTransform & getCenterOfMassTransform() const
Definition: btRigidBody.h:394
btRotationalLimitMotor2::m_targetVelocity
btScalar m_targetVelocity
Definition: btGeneric6DofSpring2Constraint.h:81
btGeneric6DofSpring2Constraint::m_calculatedLinearDiff
btVector3 m_calculatedLinearDiff
Definition: btGeneric6DofSpring2Constraint.h:291
btRotationalLimitMotor2::m_stopERP
btScalar m_stopERP
Definition: btGeneric6DofSpring2Constraint.h:76
btTransform::setIdentity
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:166
btGeneric6DofSpring2Constraint::testAngularLimitMotor
void testAngularLimitMotor(int axis_index)
Definition: btGeneric6DofSpring2Constraint.cpp:438
BT_6DOF_FLAGS_AXIS_SHIFT2
#define BT_6DOF_FLAGS_AXIS_SHIFT2
Definition: btGeneric6DofSpring2Constraint.h:269
btTypedConstraint::getRigidBodyB
const btRigidBody & getRigidBodyB() const
Definition: btTypedConstraint.h:218
btGeneric6DofSpring2Constraint::m_frameInB
btTransform m_frameInB
Definition: btGeneric6DofSpring2Constraint.h:276
btTranslationalLimitMotor2::m_motorCFM
btVector3 m_motorCFM
Definition: btGeneric6DofSpring2Constraint.h:172
BT_CONSTRAINT_STOP_ERP
Definition: btTypedConstraint.h:52
btGeneric6DofSpring2Constraint::calculateLinearInfo
void calculateLinearInfo()
Definition: btGeneric6DofSpring2Constraint.cpp:626
btGeneric6DofSpring2Constraint::getInfo1
virtual void getInfo1(btConstraintInfo1 *info)
internal method used by the constraint solver, don't use them directly
Definition: btGeneric6DofSpring2Constraint.cpp:446
btRotationalLimitMotor2::m_currentLimitError
btScalar m_currentLimitError
Definition: btGeneric6DofSpring2Constraint.h:92
btGeneric6DofSpring2Constraint::matrixToEulerZYX
static bool matrixToEulerZYX(const btMatrix3x3 &mat, btVector3 &xyz)
Definition: btGeneric6DofSpring2Constraint.cpp:236
BT_ONE
#define BT_ONE
Definition: btScalar.h:545
btAssert
#define btAssert(x)
Definition: btScalar.h:153
btGeneric6DofSpring2Constraint::m_linearLimits
btTranslationalLimitMotor2 m_linearLimits
Definition: btGeneric6DofSpring2Constraint.h:281
btGeneric6DofSpring2Constraint::enableSpring
void enableSpring(int index, bool onOff)
Definition: btGeneric6DofSpring2Constraint.cpp:1127
btTypedConstraint::btConstraintInfo2::erp
btScalar erp
Definition: btTypedConstraint.h:124
btTranslationalLimitMotor2::m_lowerLimit
btVector3 m_lowerLimit
Definition: btGeneric6DofSpring2Constraint.h:166
btGeneric6DofSpring2Constraint::setEquilibriumPoint
void setEquilibriumPoint()
Definition: btGeneric6DofSpring2Constraint.cpp:1166
btGeneric6DofSpring2Constraint::setAngularLimits
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)
Definition: btGeneric6DofSpring2Constraint.cpp:546
D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION
#define D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION
btTransform::getBasis
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:108
btTranslationalLimitMotor2::m_currentLinearDiff
btVector3 m_currentLinearDiff
Definition: btGeneric6DofSpring2Constraint.h:187
btRotationalLimitMotor2::m_servoTarget
btScalar m_servoTarget
Definition: btGeneric6DofSpring2Constraint.h:84
btRotationalLimitMotor2::m_springStiffnessLimited
bool m_springStiffnessLimited
Definition: btGeneric6DofSpring2Constraint.h:87
btGeneric6DofSpring2Constraint::m_calculatedTransformB
btTransform m_calculatedTransformB
Definition: btGeneric6DofSpring2Constraint.h:288
BT_6DOF_FLAGS_ERP_STOP2
Definition: btGeneric6DofSpring2Constraint.h:265
btRotationalLimitMotor2::m_springDampingLimited
bool m_springDampingLimited
Definition: btGeneric6DofSpring2Constraint.h:89
BT_6DOF_FLAGS_ERP_MOTO2
Definition: btGeneric6DofSpring2Constraint.h:267
btGeneric6DofSpring2Constraint::buildJacobian
virtual void buildJacobian()
internal method used by the constraint solver, don't use them directly
Definition: btGeneric6DofSpring2Constraint.h:321
btTranslationalLimitMotor2::m_springDamping
btVector3 m_springDamping
Definition: btGeneric6DofSpring2Constraint.h:179
btRotationalLimitMotor2::m_enableSpring
bool m_enableSpring
Definition: btGeneric6DofSpring2Constraint.h:85
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:46
btTypedConstraint::btConstraintInfo2::m_lowerLimit
btScalar * m_lowerLimit
Definition: btTypedConstraint.h:141
btGeneric6DofSpring2Constraint::btGeneric6DofSpring2Constraint
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...
Definition: btGeneric6DofSpring2Constraint.cpp:46
btTypedConstraint::btConstraintInfo2::m_constraintError
btScalar * m_constraintError
Definition: btTypedConstraint.h:138
BT_CONSTRAINT_STOP_CFM
Definition: btTypedConstraint.h:54
btRigidBody::getInvMass
btScalar getInvMass() const
Definition: btRigidBody.h:263
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:28
btGeneric6DofSpring2Constraint::m_hasStaticBody
bool m_hasStaticBody
Definition: btGeneric6DofSpring2Constraint.h:294
btMatrix3x3::getColumn
btVector3 getColumn(int i) const
Get a column of the matrix as a vector.
Definition: btMatrix3x3.h:140
SIMD_INFINITY
#define SIMD_INFINITY
Definition: btScalar.h:544
btGeneric6DofSpring2Constraint::matrixToEulerXYZ
static bool matrixToEulerXYZ(const btMatrix3x3 &mat, btVector3 &xyz)
Definition: btGeneric6DofSpring2Constraint.cpp:69
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:80
btRotationalLimitMotor2::m_maxMotorForce
btScalar m_maxMotorForce
Definition: btGeneric6DofSpring2Constraint.h:82
btTypedConstraint::btConstraintInfo1
Definition: btTypedConstraint.h:113
btGeneric6DofSpring2Constraint::matrixToEulerZXY
static bool matrixToEulerZXY(const btMatrix3x3 &mat, btVector3 &xyz)
Definition: btGeneric6DofSpring2Constraint.cpp:203
btGeneric6DofSpring2Constraint::m_rotateOrder
RotateOrder m_rotateOrder
Definition: btGeneric6DofSpring2Constraint.h:284
btGeneric6DofSpring2Constraint::calculateJacobi
void calculateJacobi(btRotationalLimitMotor2 *limot, const btTransform &transA, const btTransform &transB, btConstraintInfo2 *info, int srow, btVector3 &ax1, int rotational, int rotAllowed)
Definition: btGeneric6DofSpring2Constraint.cpp:637
btTransform::getOrigin
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:113
btRigidBody::getLinearVelocity
const btVector3 & getLinearVelocity() const
Definition: btRigidBody.h:398
D6_SPRING_2_CONSTRAINT_TYPE
Definition: btTypedConstraint.h:45
btGeneric6DofSpring2Constraint::m_flags
int m_flags
Definition: btGeneric6DofSpring2Constraint.h:295
btTranslationalLimitMotor2::m_upperLimit
btVector3 m_upperLimit
Definition: btGeneric6DofSpring2Constraint.h:167
btTypedConstraint::btConstraintInfo2::rowskip
int rowskip
Definition: btTypedConstraint.h:133
btGeneric6DofSpring2Constraint::setServo
void setServo(int index, bool onOff)
Definition: btGeneric6DofSpring2Constraint.cpp:1056
BT_6DOF_FLAGS_CFM_STOP2
Definition: btGeneric6DofSpring2Constraint.h:264
btTranslationalLimitMotor2::m_enableMotor
bool m_enableMotor[3]
Definition: btGeneric6DofSpring2Constraint.h:173
btTranslationalLimitMotor2::m_maxMotorForce
btVector3 m_maxMotorForce
Definition: btGeneric6DofSpring2Constraint.h:183
btTypedConstraint::btConstraintInfo1::nub
int nub
Definition: btTypedConstraint.h:115
btGeneric6DofSpring2Constraint::get_limit_motor_info2
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)
Definition: btGeneric6DofSpring2Constraint.cpp:670
btGeneric6DofSpring2Constraint::getParam
virtual btScalar getParam(int num, int axis=-1) const
return the local value of parameter
Definition: btGeneric6DofSpring2Constraint.cpp:961
btRotationalLimitMotor2::testLimitValue
void testLimitValue(btScalar test_value)
Definition: btGeneric6DofSpring2Constraint.cpp:1197
BT_CONSTRAINT_ERP
Definition: btTypedConstraint.h:51
btRotationalLimitMotor2::m_springDamping
btScalar m_springDamping
Definition: btGeneric6DofSpring2Constraint.h:88
btMatrix3x3::setValue
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:202
btRotationalLimitMotor2::m_bounce
btScalar m_bounce
Definition: btGeneric6DofSpring2Constraint.h:75
btRotationalLimitMotor2::m_motorCFM
btScalar m_motorCFM
Definition: btGeneric6DofSpring2Constraint.h:79
btGeneric6DofSpring2Constraint::setAxis
void setAxis(const btVector3 &axis1, const btVector3 &axis2)
Definition: btGeneric6DofSpring2Constraint.cpp:1019
btTranslationalLimitMotor2::m_springDampingLimited
bool m_springDampingLimited[3]
Definition: btGeneric6DofSpring2Constraint.h:180
btGeneric6DofSpring2Constraint::matrixToEulerXZY
static bool matrixToEulerXZY(const btMatrix3x3 &mat, btVector3 &xyz)
Definition: btGeneric6DofSpring2Constraint.cpp:104
btTypedConstraint::btConstraintInfo2::cfm
btScalar * cfm
Definition: btTypedConstraint.h:138
btRotationalLimitMotor2::m_currentLimit
int m_currentLimit
Definition: btGeneric6DofSpring2Constraint.h:95
RO_ZYX
Definition: btGeneric6DofSpring2Constraint.h:64
btRotationalLimitMotor2::m_motorERP
btScalar m_motorERP
Definition: btGeneric6DofSpring2Constraint.h:78
btTypedConstraint::btConstraintInfo2
Definition: btTypedConstraint.h:120
btRotationalLimitMotor2::m_hiLimit
btScalar m_hiLimit
Definition: btGeneric6DofSpring2Constraint.h:74
btAdjustAngleToLimits
btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScalar angleLowerLimitInRadians, btScalar angleUpperLimitInRadians)
Definition: btTypedConstraint.h:331
btGeneric6DofSpring2Constraint::m_frameInA
btTransform m_frameInA
Definition: btGeneric6DofSpring2Constraint.h:275
btRigidBody::getInvInertiaTensorWorld
const btMatrix3x3 & getInvInertiaTensorWorld() const
Definition: btRigidBody.h:265
btGeneric6DofSpring2Constraint::btGetMatrixElem
static btScalar btGetMatrixElem(const btMatrix3x3 &mat, int index)
Definition: btGeneric6DofSpring2Constraint.cpp:60
btGeneric6DofSpring2Constraint::setFrames
void setFrames(const btTransform &frameA, const btTransform &frameB)
Definition: btGeneric6DofSpring2Constraint.cpp:618
btTypedConstraint::m_rbB
btRigidBody & m_rbB
Definition: btTypedConstraint.h:98
btRotationalLimitMotor2::m_stopCFM
btScalar m_stopCFM
Definition: btGeneric6DofSpring2Constraint.h:77
btTranslationalLimitMotor2::m_stopCFM
btVector3 m_stopCFM
Definition: btGeneric6DofSpring2Constraint.h:170
btTypedConstraint::btConstraintInfo2::fps
btScalar fps
Definition: btTypedConstraint.h:124
btGeneric6DofSpring2Constraint::calculateTransforms
void calculateTransforms()
Definition: btGeneric6DofSpring2Constraint.cpp:411
btGeneric6DofSpring2Constraint::setDamping
void setDamping(int index, btScalar damping, bool limitIfNeeded=true)
Definition: btGeneric6DofSpring2Constraint.cpp:1151
btRotationalLimitMotor2
Definition: btGeneric6DofSpring2Constraint.h:67
btGeneric6DofSpring2Constraint::getInfo2
virtual void getInfo2(btConstraintInfo2 *info)
internal method used by the constraint solver, don't use them directly
Definition: btGeneric6DofSpring2Constraint.cpp:476
btTranslationalLimitMotor2::m_equilibriumPoint
btVector3 m_equilibriumPoint
Definition: btGeneric6DofSpring2Constraint.h:181
btAtan2
btScalar btAtan2(btScalar x, btScalar y)
Definition: btScalar.h:518
RO_YXZ
Definition: btGeneric6DofSpring2Constraint.h:61
btTranslationalLimitMotor2::m_springStiffness
btVector3 m_springStiffness
Definition: btGeneric6DofSpring2Constraint.h:177
RO_ZXY
Definition: btGeneric6DofSpring2Constraint.h:63
btGeneric6DofSpring2Constraint::setServoTarget
void setServoTarget(int index, btScalar target)
Definition: btGeneric6DofSpring2Constraint.cpp:1074
btTypedConstraint::getMotorFactor
btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact)
internal method used by the constraint solver, don't use them directly
Definition: btTypedConstraint.cpp:54
btTranslationalLimitMotor2::m_servoMotor
bool m_servoMotor[3]
Definition: btGeneric6DofSpring2Constraint.h:174
btTypedConstraint::btConstraintInfo2::m_J1angularAxis
btScalar * m_J1angularAxis
Definition: btTypedConstraint.h:130
btGeneric6DofSpring2Constraint::m_angularLimits
btRotationalLimitMotor2 m_angularLimits[3]
Definition: btGeneric6DofSpring2Constraint.h:282
BT_6DOF_FLAGS_CFM_MOTO2
Definition: btGeneric6DofSpring2Constraint.h:266
btGeneric6DofSpring2Constraint::enableMotor
void enableMotor(int index, bool onOff)
Definition: btGeneric6DofSpring2Constraint.cpp:1047
SIMD_2_PI
#define SIMD_2_PI
Definition: btScalar.h:527
btTypedConstraint::btConstraintInfo2::m_upperLimit
btScalar * m_upperLimit
Definition: btTypedConstraint.h:141
length
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
Definition: btQuaternion.h:895
btSqrt
btScalar btSqrt(btScalar y)
Definition: btScalar.h:466
btGeneric6DofSpring2Constraint::matrixToEulerYZX
static bool matrixToEulerYZX(const btMatrix3x3 &mat, btVector3 &xyz)
Definition: btGeneric6DofSpring2Constraint.cpp:170
btVector3::normalize
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:303
btRotationalLimitMotor2::m_enableMotor
bool m_enableMotor
Definition: btGeneric6DofSpring2Constraint.h:80
btVector3::normalized
btVector3 normalized() const
Return a normalized version of this vector.
Definition: btVector3.h:949
btTypedConstraint::m_rbA
btRigidBody & m_rbA
Definition: btTypedConstraint.h:97
BT_ZERO
#define BT_ZERO
Definition: btScalar.h:546
btTypedConstraint::btConstraintInfo2::m_J1linearAxis
btScalar * m_J1linearAxis
Definition: btTypedConstraint.h:130
btRotationalLimitMotor2::m_springStiffness
btScalar m_springStiffness
Definition: btGeneric6DofSpring2Constraint.h:86
btRotationalLimitMotor2::m_loLimit
btScalar m_loLimit
Definition: btGeneric6DofSpring2Constraint.h:73
btAsin
btScalar btAsin(btScalar x)
Definition: btScalar.h:509
btGeneric6DofSpring2Constraint::m_calculatedAxisAngleDiff
btVector3 m_calculatedAxisAngleDiff
Definition: btGeneric6DofSpring2Constraint.h:289
btGeneric6DofSpring2Constraint::m_calculatedTransformA
btTransform m_calculatedTransformA
Definition: btGeneric6DofSpring2Constraint.h:287
btGeneric6DofSpring2Constraint::setTargetVelocity
void setTargetVelocity(int index, btScalar velocity)
Definition: btGeneric6DofSpring2Constraint.cpp:1065