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 <new>
44 
46  : btTypedConstraint(D6_SPRING_2_CONSTRAINT_TYPE, rbA, rbB), m_frameInA(frameInA), m_frameInB(frameInB), m_rotateOrder(rotOrder), m_flags(0)
47 {
49 }
50 
52  : btTypedConstraint(D6_SPRING_2_CONSTRAINT_TYPE, getFixedBody(), rbB), m_frameInB(frameInB), m_rotateOrder(rotOrder), m_flags(0)
53 {
57 }
58 
60 {
61  int i = index % 3;
62  int j = index / 3;
63  return mat[i][j];
64 }
65 
66 // MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
67 
69 {
70  // rot = cy*cz -cy*sz sy
71  // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx
72  // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy
73 
74  btScalar fi = btGetMatrixElem(mat, 2);
75  if (fi < btScalar(1.0f))
76  {
77  if (fi > btScalar(-1.0f))
78  {
79  xyz[0] = btAtan2(-btGetMatrixElem(mat, 5), btGetMatrixElem(mat, 8));
80  xyz[1] = btAsin(btGetMatrixElem(mat, 2));
81  xyz[2] = btAtan2(-btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 0));
82  return true;
83  }
84  else
85  {
86  // WARNING. Not unique. XA - ZA = -atan2(r10,r11)
87  xyz[0] = -btAtan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4));
88  xyz[1] = -SIMD_HALF_PI;
89  xyz[2] = btScalar(0.0);
90  return false;
91  }
92  }
93  else
94  {
95  // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11)
96  xyz[0] = btAtan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4));
97  xyz[1] = SIMD_HALF_PI;
98  xyz[2] = 0.0;
99  }
100  return false;
101 }
102 
104 {
105  // rot = cy*cz -sz sy*cz
106  // cy*cx*sz+sx*sy cx*cz sy*cx*sz-cy*sx
107  // cy*sx*sz-cx*sy sx*cz sy*sx*sz+cx*cy
108 
109  btScalar fi = btGetMatrixElem(mat, 1);
110  if (fi < btScalar(1.0f))
111  {
112  if (fi > btScalar(-1.0f))
113  {
114  xyz[0] = btAtan2(btGetMatrixElem(mat, 7), btGetMatrixElem(mat, 4));
115  xyz[1] = btAtan2(btGetMatrixElem(mat, 2), btGetMatrixElem(mat, 0));
116  xyz[2] = btAsin(-btGetMatrixElem(mat, 1));
117  return true;
118  }
119  else
120  {
121  xyz[0] = -btAtan2(-btGetMatrixElem(mat, 6), btGetMatrixElem(mat, 8));
122  xyz[1] = btScalar(0.0);
123  xyz[2] = SIMD_HALF_PI;
124  return false;
125  }
126  }
127  else
128  {
129  xyz[0] = btAtan2(-btGetMatrixElem(mat, 6), btGetMatrixElem(mat, 8));
130  xyz[1] = 0.0;
131  xyz[2] = -SIMD_HALF_PI;
132  }
133  return false;
134 }
135 
137 {
138  // rot = cy*cz+sy*sx*sz cz*sy*sx-cy*sz cx*sy
139  // cx*sz cx*cz -sx
140  // cy*sx*sz-cz*sy sy*sz+cy*cz*sx cy*cx
141 
142  btScalar fi = btGetMatrixElem(mat, 5);
143  if (fi < btScalar(1.0f))
144  {
145  if (fi > btScalar(-1.0f))
146  {
147  xyz[0] = btAsin(-btGetMatrixElem(mat, 5));
148  xyz[1] = btAtan2(btGetMatrixElem(mat, 2), btGetMatrixElem(mat, 8));
149  xyz[2] = btAtan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4));
150  return true;
151  }
152  else
153  {
154  xyz[0] = SIMD_HALF_PI;
155  xyz[1] = -btAtan2(-btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 0));
156  xyz[2] = btScalar(0.0);
157  return false;
158  }
159  }
160  else
161  {
162  xyz[0] = -SIMD_HALF_PI;
163  xyz[1] = btAtan2(-btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 0));
164  xyz[2] = 0.0;
165  }
166  return false;
167 }
168 
170 {
171  // rot = cy*cz sy*sx-cy*cx*sz cx*sy+cy*sz*sx
172  // sz cz*cx -cz*sx
173  // -cz*sy cy*sx+cx*sy*sz cy*cx-sy*sz*sx
174 
175  btScalar fi = btGetMatrixElem(mat, 3);
176  if (fi < btScalar(1.0f))
177  {
178  if (fi > btScalar(-1.0f))
179  {
180  xyz[0] = btAtan2(-btGetMatrixElem(mat, 5), btGetMatrixElem(mat, 4));
181  xyz[1] = btAtan2(-btGetMatrixElem(mat, 6), btGetMatrixElem(mat, 0));
182  xyz[2] = btAsin(btGetMatrixElem(mat, 3));
183  return true;
184  }
185  else
186  {
187  xyz[0] = btScalar(0.0);
188  xyz[1] = -btAtan2(btGetMatrixElem(mat, 7), btGetMatrixElem(mat, 8));
189  xyz[2] = -SIMD_HALF_PI;
190  return false;
191  }
192  }
193  else
194  {
195  xyz[0] = btScalar(0.0);
196  xyz[1] = btAtan2(btGetMatrixElem(mat, 7), btGetMatrixElem(mat, 8));
197  xyz[2] = SIMD_HALF_PI;
198  }
199  return false;
200 }
201 
203 {
204  // rot = cz*cy-sz*sx*sy -cx*sz cz*sy+cy*sz*sx
205  // cy*sz+cz*sx*sy cz*cx sz*sy-cz*xy*sx
206  // -cx*sy sx cx*cy
207 
208  btScalar fi = btGetMatrixElem(mat, 7);
209  if (fi < btScalar(1.0f))
210  {
211  if (fi > btScalar(-1.0f))
212  {
213  xyz[0] = btAsin(btGetMatrixElem(mat, 7));
214  xyz[1] = btAtan2(-btGetMatrixElem(mat, 6), btGetMatrixElem(mat, 8));
215  xyz[2] = btAtan2(-btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 4));
216  return true;
217  }
218  else
219  {
220  xyz[0] = -SIMD_HALF_PI;
221  xyz[1] = btScalar(0.0);
222  xyz[2] = -btAtan2(btGetMatrixElem(mat, 2), btGetMatrixElem(mat, 0));
223  return false;
224  }
225  }
226  else
227  {
228  xyz[0] = SIMD_HALF_PI;
229  xyz[1] = btScalar(0.0);
230  xyz[2] = btAtan2(btGetMatrixElem(mat, 2), btGetMatrixElem(mat, 0));
231  }
232  return false;
233 }
234 
236 {
237  // rot = cz*cy cz*sy*sx-cx*sz sz*sx+cz*cx*sy
238  // cy*sz cz*cx+sz*sy*sx cx*sz*sy-cz*sx
239  // -sy cy*sx cy*cx
240 
241  btScalar fi = btGetMatrixElem(mat, 6);
242  if (fi < btScalar(1.0f))
243  {
244  if (fi > btScalar(-1.0f))
245  {
246  xyz[0] = btAtan2(btGetMatrixElem(mat, 7), btGetMatrixElem(mat, 8));
247  xyz[1] = btAsin(-btGetMatrixElem(mat, 6));
248  xyz[2] = btAtan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 0));
249  return true;
250  }
251  else
252  {
253  xyz[0] = btScalar(0.0);
254  xyz[1] = SIMD_HALF_PI;
255  xyz[2] = -btAtan2(btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 2));
256  return false;
257  }
258  }
259  else
260  {
261  xyz[0] = btScalar(0.0);
262  xyz[1] = -SIMD_HALF_PI;
263  xyz[2] = btAtan2(-btGetMatrixElem(mat, 1), -btGetMatrixElem(mat, 2));
264  }
265  return false;
266 }
267 
269 {
271  switch (m_rotateOrder)
272  {
273  case RO_XYZ:
275  break;
276  case RO_XZY:
278  break;
279  case RO_YXZ:
281  break;
282  case RO_YZX:
284  break;
285  case RO_ZXY:
287  break;
288  case RO_ZYX:
290  break;
291  default:
292  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 
411 {
413 }
414 
416 {
421 
424  m_hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
425  btScalar miS = miA + miB;
426  if (miS > btScalar(0.f))
427  {
428  m_factA = miB / miS;
429  }
430  else
431  {
432  m_factA = btScalar(0.5f);
433  }
434  m_factB = btScalar(1.0f) - m_factA;
435 }
436 
438 {
439  btScalar angle = m_calculatedAxisAngleDiff[axis_index];
440  angle = btAdjustAngleToLimits(angle, m_angularLimits[axis_index].m_loLimit, m_angularLimits[axis_index].m_hiLimit);
441  m_angularLimits[axis_index].m_currentPosition = angle;
442  m_angularLimits[axis_index].testLimitValue(angle);
443 }
444 
446 {
447  //prepare constraint
449  info->m_numConstraintRows = 0;
450  info->nub = 0;
451  int i;
452  //test linear limits
453  for (i = 0; i < 3; i++)
454  {
455  if (m_linearLimits.m_currentLimit[i] == 4)
456  info->m_numConstraintRows += 2;
457  else if (m_linearLimits.m_currentLimit[i] != 0)
458  info->m_numConstraintRows += 1;
461  }
462  //test angular limits
463  for (i = 0; i < 3; i++)
464  {
466  if (m_angularLimits[i].m_currentLimit == 4)
467  info->m_numConstraintRows += 2;
468  else if (m_angularLimits[i].m_currentLimit != 0)
469  info->m_numConstraintRows += 1;
470  if (m_angularLimits[i].m_enableMotor) info->m_numConstraintRows += 1;
471  if (m_angularLimits[i].m_enableSpring) info->m_numConstraintRows += 1;
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 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)
490 {
491  //solve linear limits
493  for (int i = 0; i < 3; i++)
494  {
496  { // re-use rotational motor code
497  limot.m_bounce = m_linearLimits.m_bounce[i];
516  int flags = m_flags >> (i * BT_6DOF_FLAGS_AXIS_SHIFT2);
517  limot.m_stopCFM = (flags & BT_6DOF_FLAGS_CFM_STOP2) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];
518  limot.m_stopERP = (flags & BT_6DOF_FLAGS_ERP_STOP2) ? m_linearLimits.m_stopERP[i] : info->erp;
519  limot.m_motorCFM = (flags & BT_6DOF_FLAGS_CFM_MOTO2) ? m_linearLimits.m_motorCFM[i] : info->cfm[0];
520  limot.m_motorERP = (flags & BT_6DOF_FLAGS_ERP_MOTO2) ? m_linearLimits.m_motorERP[i] : info->erp;
521 
522  //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.
523  int indx1 = (i + 1) % 3;
524  int indx2 = (i + 2) % 3;
525  int rotAllowed = 1; // rotations around orthos to current axis (it is used only when one of the body is static)
526 #define D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION 1.0e-3
527  bool indx1Violated = m_angularLimits[indx1].m_currentLimit == 1 ||
528  m_angularLimits[indx1].m_currentLimit == 2 ||
531  bool indx2Violated = m_angularLimits[indx2].m_currentLimit == 1 ||
532  m_angularLimits[indx2].m_currentLimit == 2 ||
535  if (indx1Violated && indx2Violated)
536  {
537  rotAllowed = 0;
538  }
539  row += get_limit_motor_info2(&limot, transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 0, rotAllowed);
540  }
541  }
542  return row;
543 }
544 
545 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)
546 {
547  int row = row_offset;
548 
549  //order of rotational constraint rows
550  int cIdx[] = {0, 1, 2};
551  switch (m_rotateOrder)
552  {
553  case RO_XYZ:
554  cIdx[0] = 0;
555  cIdx[1] = 1;
556  cIdx[2] = 2;
557  break;
558  case RO_XZY:
559  cIdx[0] = 0;
560  cIdx[1] = 2;
561  cIdx[2] = 1;
562  break;
563  case RO_YXZ:
564  cIdx[0] = 1;
565  cIdx[1] = 0;
566  cIdx[2] = 2;
567  break;
568  case RO_YZX:
569  cIdx[0] = 1;
570  cIdx[1] = 2;
571  cIdx[2] = 0;
572  break;
573  case RO_ZXY:
574  cIdx[0] = 2;
575  cIdx[1] = 0;
576  cIdx[2] = 1;
577  break;
578  case RO_ZYX:
579  cIdx[0] = 2;
580  cIdx[1] = 1;
581  cIdx[2] = 0;
582  break;
583  default:
584  btAssert(false);
585  }
586 
587  for (int ii = 0; ii < 3; ii++)
588  {
589  int i = cIdx[ii];
590  if (m_angularLimits[i].m_currentLimit || m_angularLimits[i].m_enableMotor || m_angularLimits[i].m_enableSpring)
591  {
592  btVector3 axis = getAxis(i);
593  int flags = m_flags >> ((i + 3) * BT_6DOF_FLAGS_AXIS_SHIFT2);
594  if (!(flags & BT_6DOF_FLAGS_CFM_STOP2))
595  {
596  m_angularLimits[i].m_stopCFM = info->cfm[0];
597  }
598  if (!(flags & BT_6DOF_FLAGS_ERP_STOP2))
599  {
600  m_angularLimits[i].m_stopERP = info->erp;
601  }
602  if (!(flags & BT_6DOF_FLAGS_CFM_MOTO2))
603  {
604  m_angularLimits[i].m_motorCFM = info->cfm[0];
605  }
606  if (!(flags & BT_6DOF_FLAGS_ERP_MOTO2))
607  {
608  m_angularLimits[i].m_motorERP = info->erp;
609  }
610  row += get_limit_motor_info2(&m_angularLimits[i], transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 1);
611  }
612  }
613 
614  return row;
615 }
616 
618 {
619  m_frameInA = frameA;
620  m_frameInB = frameB;
621  buildJacobian();
623 }
624 
626 {
629  for (int i = 0; i < 3; i++)
630  {
633  }
634 }
635 
636 void btGeneric6DofSpring2Constraint::calculateJacobi(btRotationalLimitMotor2* limot, const btTransform& transA, const btTransform& transB, btConstraintInfo2* info, int srow, btVector3& ax1, int rotational, int rotAllowed)
637 {
638  btScalar* J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
639  btScalar* J2 = rotational ? info->m_J2angularAxis : info->m_J2linearAxis;
640 
641  J1[srow + 0] = ax1[0];
642  J1[srow + 1] = ax1[1];
643  J1[srow + 2] = ax1[2];
644 
645  J2[srow + 0] = -ax1[0];
646  J2[srow + 1] = -ax1[1];
647  J2[srow + 2] = -ax1[2];
648 
649  if (!rotational)
650  {
651  btVector3 tmpA, tmpB, relA, relB;
652  // get vector from bodyB to frameB in WCS
653  relB = m_calculatedTransformB.getOrigin() - transB.getOrigin();
654  // same for bodyA
655  relA = m_calculatedTransformA.getOrigin() - transA.getOrigin();
656  tmpA = relA.cross(ax1);
657  tmpB = relB.cross(ax1);
658  if (m_hasStaticBody && (!rotAllowed))
659  {
660  tmpA *= m_factA;
661  tmpB *= m_factB;
662  }
663  int i;
664  for (i = 0; i < 3; i++) info->m_J1angularAxis[srow + i] = tmpA[i];
665  for (i = 0; i < 3; i++) info->m_J2angularAxis[srow + i] = -tmpB[i];
666  }
667 }
668 
671  const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB,
672  btConstraintInfo2* info, int row, btVector3& ax1, int rotational, int rotAllowed)
673 {
674  int count = 0;
675  int srow = row * info->rowskip;
676 
677  if (limot->m_currentLimit == 4)
678  {
679  btScalar vel = rotational ? angVelA.dot(ax1) - angVelB.dot(ax1) : linVelA.dot(ax1) - linVelB.dot(ax1);
680 
681  calculateJacobi(limot, transA, transB, info, srow, ax1, rotational, rotAllowed);
682  info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitError * (rotational ? -1 : 1);
683  if (rotational)
684  {
685  if (info->m_constraintError[srow] - vel * limot->m_stopERP > 0)
686  {
687  btScalar bounceerror = -limot->m_bounce * vel;
688  if (bounceerror > info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror;
689  }
690  }
691  else
692  {
693  if (info->m_constraintError[srow] - vel * limot->m_stopERP < 0)
694  {
695  btScalar bounceerror = -limot->m_bounce * vel;
696  if (bounceerror < info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror;
697  }
698  }
699  info->m_lowerLimit[srow] = rotational ? 0 : -SIMD_INFINITY;
700  info->m_upperLimit[srow] = rotational ? SIMD_INFINITY : 0;
701  info->cfm[srow] = limot->m_stopCFM;
702  srow += info->rowskip;
703  ++count;
704 
705  calculateJacobi(limot, transA, transB, info, srow, ax1, rotational, rotAllowed);
706  info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitErrorHi * (rotational ? -1 : 1);
707  if (rotational)
708  {
709  if (info->m_constraintError[srow] - vel * limot->m_stopERP < 0)
710  {
711  btScalar bounceerror = -limot->m_bounce * vel;
712  if (bounceerror < info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror;
713  }
714  }
715  else
716  {
717  if (info->m_constraintError[srow] - vel * limot->m_stopERP > 0)
718  {
719  btScalar bounceerror = -limot->m_bounce * vel;
720  if (bounceerror > info->m_constraintError[srow]) info->m_constraintError[srow] = bounceerror;
721  }
722  }
723  info->m_lowerLimit[srow] = rotational ? -SIMD_INFINITY : 0;
724  info->m_upperLimit[srow] = rotational ? 0 : SIMD_INFINITY;
725  info->cfm[srow] = limot->m_stopCFM;
726  srow += info->rowskip;
727  ++count;
728  }
729  else if (limot->m_currentLimit == 3)
730  {
731  calculateJacobi(limot, transA, transB, info, srow, ax1, rotational, rotAllowed);
732  info->m_constraintError[srow] = info->fps * limot->m_stopERP * limot->m_currentLimitError * (rotational ? -1 : 1);
733  info->m_lowerLimit[srow] = -SIMD_INFINITY;
734  info->m_upperLimit[srow] = SIMD_INFINITY;
735  info->cfm[srow] = limot->m_stopCFM;
736  srow += info->rowskip;
737  ++count;
738  }
739 
740  if (limot->m_enableMotor && !limot->m_servoMotor)
741  {
742  calculateJacobi(limot, transA, transB, info, srow, ax1, rotational, rotAllowed);
743  btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;
744  btScalar mot_fact = getMotorFactor(limot->m_currentPosition,
745  limot->m_loLimit,
746  limot->m_hiLimit,
747  tag_vel,
748  info->fps * limot->m_motorERP);
749  info->m_constraintError[srow] = mot_fact * limot->m_targetVelocity;
750  info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps;
751  info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps;
752  info->cfm[srow] = limot->m_motorCFM;
753  srow += info->rowskip;
754  ++count;
755  }
756 
757  if (limot->m_enableMotor && limot->m_servoMotor)
758  {
759  btScalar error = limot->m_currentPosition - limot->m_servoTarget;
760  btScalar curServoTarget = limot->m_servoTarget;
761  if (rotational)
762  {
763  if (error > SIMD_PI)
764  {
765  error -= SIMD_2_PI;
766  curServoTarget += SIMD_2_PI;
767  }
768  if (error < -SIMD_PI)
769  {
770  error += SIMD_2_PI;
771  curServoTarget -= SIMD_2_PI;
772  }
773  }
774 
775  calculateJacobi(limot, transA, transB, info, srow, ax1, rotational, rotAllowed);
776  btScalar targetvelocity = error < 0 ? -limot->m_targetVelocity : limot->m_targetVelocity;
777  btScalar tag_vel = -targetvelocity;
778  btScalar mot_fact;
779  if (error != 0)
780  {
781  btScalar lowLimit;
782  btScalar hiLimit;
783  if (limot->m_loLimit > limot->m_hiLimit)
784  {
785  lowLimit = error > 0 ? curServoTarget : -SIMD_INFINITY;
786  hiLimit = error < 0 ? curServoTarget : SIMD_INFINITY;
787  }
788  else
789  {
790  lowLimit = error > 0 && curServoTarget > limot->m_loLimit ? curServoTarget : limot->m_loLimit;
791  hiLimit = error < 0 && curServoTarget < limot->m_hiLimit ? curServoTarget : limot->m_hiLimit;
792  }
793  mot_fact = getMotorFactor(limot->m_currentPosition, lowLimit, hiLimit, tag_vel, info->fps * limot->m_motorERP);
794  }
795  else
796  {
797  mot_fact = 0;
798  }
799  info->m_constraintError[srow] = mot_fact * targetvelocity * (rotational ? -1 : 1);
800  info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps;
801  info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps;
802  info->cfm[srow] = limot->m_motorCFM;
803  srow += info->rowskip;
804  ++count;
805  }
806 
807  if (limot->m_enableSpring)
808  {
809  btScalar error = limot->m_currentPosition - limot->m_equilibriumPoint;
810  calculateJacobi(limot, transA, transB, info, srow, ax1, rotational, rotAllowed);
811 
812  //btScalar cfm = 1.0 / ((1.0/info->fps)*limot->m_springStiffness+ limot->m_springDamping);
813  //if(cfm > 0.99999)
814  // cfm = 0.99999;
815  //btScalar erp = (1.0/info->fps)*limot->m_springStiffness / ((1.0/info->fps)*limot->m_springStiffness + limot->m_springDamping);
816  //info->m_constraintError[srow] = info->fps * erp * error * (rotational ? -1.0 : 1.0);
817  //info->m_lowerLimit[srow] = -SIMD_INFINITY;
818  //info->m_upperLimit[srow] = SIMD_INFINITY;
819 
820  btScalar dt = BT_ONE / info->fps;
821  btScalar kd = limot->m_springDamping;
822  btScalar ks = limot->m_springStiffness;
823  btScalar vel;
824  if (rotational)
825  {
826  vel = angVelA.dot(ax1) - angVelB.dot(ax1);
827  }
828  else
829  {
830  btVector3 tanVelA = angVelA.cross(m_calculatedTransformA.getOrigin() - transA.getOrigin());
831  btVector3 tanVelB = angVelB.cross(m_calculatedTransformB.getOrigin() - transB.getOrigin());
832  vel = (linVelA + tanVelA).dot(ax1) - (linVelB + tanVelB).dot(ax1);
833  }
834  btScalar cfm = BT_ZERO;
835  btScalar mA = BT_ONE / m_rbA.getInvMass();
836  btScalar mB = BT_ONE / m_rbB.getInvMass();
837  if (rotational)
838  {
839  btScalar rrA = (m_calculatedTransformA.getOrigin() - transA.getOrigin()).length2();
840  btScalar rrB = (m_calculatedTransformB.getOrigin() - transB.getOrigin()).length2();
841  if (m_rbA.getInvMass()) mA = mA * rrA + 1 / (m_rbA.getInvInertiaTensorWorld() * ax1).length();
842  if (m_rbB.getInvMass()) mB = mB * rrB + 1 / (m_rbB.getInvInertiaTensorWorld() * ax1).length();
843  }
844  btScalar m;
845  if (m_rbA.getInvMass() == 0) m = mB; else
846  if (m_rbB.getInvMass() == 0) m = mA; else
847  m = mA*mB / (mA + mB);
848  btScalar angularfreq = sqrt(ks / m);
849 
850  //limit stiffness (the spring should not be sampled faster that the quarter of its angular frequency)
851  if (limot->m_springStiffnessLimited && 0.25 < angularfreq * dt)
852  {
853  ks = BT_ONE / dt / dt / btScalar(16.0) * m;
854  }
855  //avoid damping that would blow up the spring
856  if (limot->m_springDampingLimited && kd * dt > m)
857  {
858  kd = m / dt;
859  }
860  btScalar fs = ks * error * dt;
861  btScalar fd = -kd * (vel) * (rotational ? -1 : 1) * dt;
862  btScalar f = (fs + fd);
863 
864  // after the spring force affecting the body(es) the new velocity will be
865  // vel + f / m * (rotational ? -1 : 1)
866  // so in theory this should be set here for m_constraintError
867  // (with m_constraintError we set a desired velocity for the affected body(es))
868  // however in practice any value is fine as long as it is greater then the "proper" velocity,
869  // because the m_lowerLimit and the m_upperLimit will determinate the strength of the final pulling force
870  // so it is much simpler (and more robust) just to simply use inf (with the proper sign)
871  // (Even with our best intent the "new" velocity is only an estimation. If we underestimate
872  // the "proper" velocity that will weaken the spring, however if we overestimate it, it doesn't
873  // matter, because the solver will limit it according the force limit)
874  // you may also wonder what if the current velocity (vel) so high that the pulling force will not change its direction (in this iteration)
875  // will we not request a velocity with the wrong direction ?
876  // and the answer is not, because in practice during the solving the current velocity is subtracted from the m_constraintError
877  // so the sign of the force that is really matters
878  info->m_constraintError[srow] = (rotational ? -1 : 1) * (f < 0 ? -SIMD_INFINITY : SIMD_INFINITY);
879 
880  btScalar minf = f < fd ? f : fd;
881  btScalar maxf = f < fd ? fd : f;
882  if (!rotational)
883  {
884  info->m_lowerLimit[srow] = minf > 0 ? 0 : minf;
885  info->m_upperLimit[srow] = maxf < 0 ? 0 : maxf;
886  }
887  else
888  {
889  info->m_lowerLimit[srow] = -maxf > 0 ? 0 : -maxf;
890  info->m_upperLimit[srow] = -minf < 0 ? 0 : -minf;
891  }
892 
893  info->cfm[srow] = cfm;
894  srow += info->rowskip;
895  ++count;
896  }
897 
898  return count;
899 }
900 
901 //override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
902 //If no axis is provided, it uses the default axis for this constraint.
904 {
905  if ((axis >= 0) && (axis < 3))
906  {
907  switch (num)
908  {
910  m_linearLimits.m_stopERP[axis] = value;
912  break;
914  m_linearLimits.m_stopCFM[axis] = value;
916  break;
917  case BT_CONSTRAINT_ERP:
918  m_linearLimits.m_motorERP[axis] = value;
920  break;
921  case BT_CONSTRAINT_CFM:
922  m_linearLimits.m_motorCFM[axis] = value;
924  break;
925  default:
927  }
928  }
929  else if ((axis >= 3) && (axis < 6))
930  {
931  switch (num)
932  {
934  m_angularLimits[axis - 3].m_stopERP = value;
936  break;
938  m_angularLimits[axis - 3].m_stopCFM = value;
940  break;
941  case BT_CONSTRAINT_ERP:
942  m_angularLimits[axis - 3].m_motorERP = value;
944  break;
945  case BT_CONSTRAINT_CFM:
946  m_angularLimits[axis - 3].m_motorCFM = value;
948  break;
949  default:
951  }
952  }
953  else
954  {
956  }
957 }
958 
959 //return the local value of parameter
961 {
962  btScalar retVal = 0;
963  if ((axis >= 0) && (axis < 3))
964  {
965  switch (num)
966  {
969  retVal = m_linearLimits.m_stopERP[axis];
970  break;
973  retVal = m_linearLimits.m_stopCFM[axis];
974  break;
975  case BT_CONSTRAINT_ERP:
977  retVal = m_linearLimits.m_motorERP[axis];
978  break;
979  case BT_CONSTRAINT_CFM:
981  retVal = m_linearLimits.m_motorCFM[axis];
982  break;
983  default:
985  }
986  }
987  else if ((axis >= 3) && (axis < 6))
988  {
989  switch (num)
990  {
993  retVal = m_angularLimits[axis - 3].m_stopERP;
994  break;
997  retVal = m_angularLimits[axis - 3].m_stopCFM;
998  break;
999  case BT_CONSTRAINT_ERP:
1001  retVal = m_angularLimits[axis - 3].m_motorERP;
1002  break;
1003  case BT_CONSTRAINT_CFM:
1005  retVal = m_angularLimits[axis - 3].m_motorCFM;
1006  break;
1007  default:
1009  }
1010  }
1011  else
1012  {
1014  }
1015  return retVal;
1016 }
1017 
1019 {
1020  btVector3 zAxis = axis1.normalized();
1021  btVector3 yAxis = axis2.normalized();
1022  btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
1023 
1024  btTransform frameInW;
1025  frameInW.setIdentity();
1026  frameInW.getBasis().setValue(xAxis[0], yAxis[0], zAxis[0],
1027  xAxis[1], yAxis[1], zAxis[1],
1028  xAxis[2], yAxis[2], zAxis[2]);
1029 
1030  // now get constraint frame in local coordinate systems
1033 
1035 }
1036 
1038 {
1039  btAssert((index >= 0) && (index < 6));
1040  if (index < 3)
1041  m_linearLimits.m_bounce[index] = bounce;
1042  else
1043  m_angularLimits[index - 3].m_bounce = bounce;
1044 }
1045 
1047 {
1048  btAssert((index >= 0) && (index < 6));
1049  if (index < 3)
1050  m_linearLimits.m_enableMotor[index] = onOff;
1051  else
1052  m_angularLimits[index - 3].m_enableMotor = onOff;
1053 }
1054 
1055 void btGeneric6DofSpring2Constraint::setServo(int index, bool onOff)
1056 {
1057  btAssert((index >= 0) && (index < 6));
1058  if (index < 3)
1059  m_linearLimits.m_servoMotor[index] = onOff;
1060  else
1061  m_angularLimits[index - 3].m_servoMotor = onOff;
1062 }
1063 
1065 {
1066  btAssert((index >= 0) && (index < 6));
1067  if (index < 3)
1068  m_linearLimits.m_targetVelocity[index] = velocity;
1069  else
1070  m_angularLimits[index - 3].m_targetVelocity = velocity;
1071 }
1072 
1074 {
1075  btAssert((index >= 0) && (index < 6));
1076  if (index < 3)
1077  {
1078  m_linearLimits.m_servoTarget[index] = targetOrg;
1079  }
1080  else
1081  {
1082  //wrap between -PI and PI, see also
1083  //https://stackoverflow.com/questions/4633177/c-how-to-wrap-a-float-to-the-interval-pi-pi
1084 
1085  btScalar target = targetOrg + SIMD_PI;
1086  if (1)
1087  {
1088  btScalar m = target - SIMD_2_PI * floor(target / SIMD_2_PI);
1089  // handle boundary cases resulted from floating-point cut off:
1090  {
1091  if (m >= SIMD_2_PI)
1092  {
1093  target = 0;
1094  }
1095  else
1096  {
1097  if (m < 0)
1098  {
1099  if (SIMD_2_PI + m == SIMD_2_PI)
1100  target = 0;
1101  else
1102  target = SIMD_2_PI + m;
1103  }
1104  else
1105  {
1106  target = m;
1107  }
1108  }
1109  }
1110  target -= SIMD_PI;
1111  }
1112 
1113  m_angularLimits[index - 3].m_servoTarget = target;
1114  }
1115 }
1116 
1118 {
1119  btAssert((index >= 0) && (index < 6));
1120  if (index < 3)
1121  m_linearLimits.m_maxMotorForce[index] = force;
1122  else
1123  m_angularLimits[index - 3].m_maxMotorForce = force;
1124 }
1125 
1127 {
1128  btAssert((index >= 0) && (index < 6));
1129  if (index < 3)
1130  m_linearLimits.m_enableSpring[index] = onOff;
1131  else
1132  m_angularLimits[index - 3].m_enableSpring = onOff;
1133 }
1134 
1135 void btGeneric6DofSpring2Constraint::setStiffness(int index, btScalar stiffness, bool limitIfNeeded)
1136 {
1137  btAssert((index >= 0) && (index < 6));
1138  if (index < 3)
1139  {
1140  m_linearLimits.m_springStiffness[index] = stiffness;
1141  m_linearLimits.m_springStiffnessLimited[index] = limitIfNeeded;
1142  }
1143  else
1144  {
1145  m_angularLimits[index - 3].m_springStiffness = stiffness;
1146  m_angularLimits[index - 3].m_springStiffnessLimited = limitIfNeeded;
1147  }
1148 }
1149 
1150 void btGeneric6DofSpring2Constraint::setDamping(int index, btScalar damping, bool limitIfNeeded)
1151 {
1152  btAssert((index >= 0) && (index < 6));
1153  if (index < 3)
1154  {
1155  m_linearLimits.m_springDamping[index] = damping;
1156  m_linearLimits.m_springDampingLimited[index] = limitIfNeeded;
1157  }
1158  else
1159  {
1160  m_angularLimits[index - 3].m_springDamping = damping;
1161  m_angularLimits[index - 3].m_springDampingLimited = limitIfNeeded;
1162  }
1163 }
1164 
1166 {
1168  int i;
1169  for (i = 0; i < 3; i++)
1171  for (i = 0; i < 3; i++)
1172  m_angularLimits[i].m_equilibriumPoint = m_calculatedAxisAngleDiff[i];
1173 }
1174 
1176 {
1177  btAssert((index >= 0) && (index < 6));
1179  if (index < 3)
1181  else
1183 }
1184 
1186 {
1187  btAssert((index >= 0) && (index < 6));
1188  if (index < 3)
1189  m_linearLimits.m_equilibriumPoint[index] = val;
1190  else
1191  m_angularLimits[index - 3].m_equilibriumPoint = val;
1192 }
1193 
1195 
1197 {
1198  //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
1199  if (m_loLimit > m_hiLimit)
1200  {
1201  m_currentLimit = 0;
1203  }
1204  else if (m_loLimit == m_hiLimit)
1205  {
1206  m_currentLimitError = test_value - m_loLimit;
1207  m_currentLimit = 3;
1208  }
1209  else
1210  {
1211  m_currentLimitError = test_value - m_loLimit;
1212  m_currentLimitErrorHi = test_value - m_hiLimit;
1213  m_currentLimit = 4;
1214  }
1215 }
1216 
1218 
1220 {
1221  btScalar loLimit = m_lowerLimit[limitIndex];
1222  btScalar hiLimit = m_upperLimit[limitIndex];
1223  if (loLimit > hiLimit)
1224  {
1225  m_currentLimitError[limitIndex] = 0;
1226  m_currentLimit[limitIndex] = 0;
1227  }
1228  else if (loLimit == hiLimit)
1229  {
1230  m_currentLimitError[limitIndex] = test_value - loLimit;
1231  m_currentLimit[limitIndex] = 3;
1232  }
1233  else
1234  {
1235  m_currentLimitError[limitIndex] = test_value - loLimit;
1236  m_currentLimitErrorHi[limitIndex] = test_value - hiLimit;
1237  m_currentLimit[limitIndex] = 4;
1238  }
1239 }
#define SIMD_EPSILON
Definition: btScalar.h:523
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
Definition: btQuaternion.h:895
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:357
#define D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION
void setMaxMotorForce(int index, btScalar force)
#define BT_6DOF_FLAGS_AXIS_SHIFT2
btRigidBody & m_rbA
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
const btTransform & getCenterOfMassTransform() const
Definition: btRigidBody.h:349
#define BT_ONE
Definition: btScalar.h:525
const btRigidBody & getRigidBodyA() const
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:166
#define btAssert(x)
Definition: btScalar.h:133
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:508
#define BT_ZERO
Definition: btScalar.h:526
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:303
btVector3 normalized() const
Return a normalized version of this vector.
Definition: btVector3.h:949
btVector3 getAxis(int axis_index) const
btVector3 getColumn(int i) const
Get a column of the matrix as a vector.
Definition: btMatrix3x3.h:133
#define SIMD_PI
Definition: btScalar.h:506
#define SIMD_INFINITY
Definition: btScalar.h:524
#define SIMD_2_PI
Definition: btScalar.h:507
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:113
const btVector3 & getLinearVelocity() const
Definition: btRigidBody.h:353
btScalar getInvMass() const
Definition: btRigidBody.h:261
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:380
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:229
btScalar btAtan2(btScalar x, btScalar y)
Definition: btScalar.h:498
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:108
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:195
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:59
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:80
static bool matrixToEulerXZY(const btMatrix3x3 &mat, btVector3 &xyz)
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:28
void calculateJacobi(btRotationalLimitMotor2 *limot, const btTransform &transA, const btTransform &transB, btConstraintInfo2 *info, int srow, btVector3 &ax1, int rotational, int rotAllowed)
btRigidBody & m_rbB
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
void setStiffness(int index, btScalar stiffness, bool limitIfNeeded=true)
static bool matrixToEulerYZX(const btMatrix3x3 &mat, btVector3 &xyz)
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:46
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:888
void testLimitValue(btScalar test_value)
const btMatrix3x3 & getInvInertiaTensorWorld() const
Definition: btRigidBody.h:262
btScalar btAsin(btScalar x)
Definition: btScalar.h:489
void setFrames(const btTransform &frameA, const btTransform &frameB)
btMatrix3x3 inverse() const
Return the inverse of the matrix.
Definition: btMatrix3x3.h:1070
virtual void getInfo1(btConstraintInfo1 *info)
internal method used by the constraint solver, don&#39;t use them directly
#define btAssertConstrParams(_par)
void setDamping(int index, btScalar damping, bool limitIfNeeded=true)
btTransform inverse() const
Return the inverse of this transform.
Definition: btTransform.h:182
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:294
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)