Blender  V3.3
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 
48 {
50 }
51 
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 {
271  btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse() * m_calculatedTransformB.getBasis();
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)
330  btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0);
331  btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2);
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
343  btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0);
344  btVector3 axis1 = m_calculatedTransformA.getBasis().getColumn(1);
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
356  btVector3 axis1 = m_calculatedTransformB.getBasis().getColumn(1);
357  btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2);
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
369  btVector3 axis0 = m_calculatedTransformA.getBasis().getColumn(0);
370  btVector3 axis1 = m_calculatedTransformB.getBasis().getColumn(1);
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
382  btVector3 axis1 = m_calculatedTransformA.getBasis().getColumn(1);
383  btVector3 axis2 = m_calculatedTransformB.getBasis().getColumn(2);
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
395  btVector3 axis0 = m_calculatedTransformA.getBasis().getColumn(0);
396  btVector3 axis2 = m_calculatedTransformB.getBasis().getColumn(2);
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 
406  m_calculatedAxis[0].normalize();
407  m_calculatedAxis[1].normalize();
408  m_calculatedAxis[2].normalize();
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 {
441  angle = btAdjustAngleToLimits(angle, m_angularLimits[axis_index].m_loLimit, m_angularLimits[axis_index].m_hiLimit);
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;
461  if (m_linearLimits.m_enableSpring[i]) 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  {
496  if (m_linearLimits.m_currentLimit[i] || m_linearLimits.m_enableMotor[i] || m_linearLimits.m_enableSpring[i])
497  { // re-use rotational motor code
498  limot.m_bounce = m_linearLimits.m_bounce[i];
502  limot.m_currentLimitErrorHi = m_linearLimits.m_currentLimitErrorHi[i];
504  limot.m_servoMotor = m_linearLimits.m_servoMotor[i];
505  limot.m_servoTarget = m_linearLimits.m_servoTarget[i];
506  limot.m_enableSpring = m_linearLimits.m_enableSpring[i];
507  limot.m_springStiffness = m_linearLimits.m_springStiffness[i];
508  limot.m_springStiffnessLimited = m_linearLimits.m_springStiffnessLimited[i];
509  limot.m_springDamping = m_linearLimits.m_springDamping[i];
510  limot.m_springDampingLimited = m_linearLimits.m_springDampingLimited[i];
511  limot.m_equilibriumPoint = m_linearLimits.m_equilibriumPoint[i];
516  btVector3 axis = m_calculatedTransformA.getBasis().getColumn(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 
618 void btGeneric6DofSpring2Constraint::setFrames(const btTransform& frameA, const btTransform& frameB)
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  {
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
880  info->m_constraintError[srow] = (rotational ? -1 : 1) * (f < 0 ? -SIMD_INFINITY : SIMD_INFINITY);
881  else
882  info->m_constraintError[srow] = vel + f / m * (rotational ? -1 : 1);
883 
884  btScalar minf = f < fd ? f : fd;
885  btScalar maxf = f < fd ? fd : f;
886  if (!rotational)
887  {
888  info->m_lowerLimit[srow] = minf > 0 ? 0 : minf;
889  info->m_upperLimit[srow] = maxf < 0 ? 0 : maxf;
890  }
891  else
892  {
893  info->m_lowerLimit[srow] = -maxf > 0 ? 0 : -maxf;
894  info->m_upperLimit[srow] = -minf < 0 ? 0 : -minf;
895  }
896 
897  info->cfm[srow] = cfm;
898  srow += info->rowskip;
899  ++count;
900  }
901 
902  return count;
903 }
904 
905 //override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
906 //If no axis is provided, it uses the default axis for this constraint.
907 void btGeneric6DofSpring2Constraint::setParam(int num, btScalar value, int axis)
908 {
909  if ((axis >= 0) && (axis < 3))
910  {
911  switch (num)
912  {
914  m_linearLimits.m_stopERP[axis] = value;
916  break;
918  m_linearLimits.m_stopCFM[axis] = value;
920  break;
921  case BT_CONSTRAINT_ERP:
922  m_linearLimits.m_motorERP[axis] = value;
924  break;
925  case BT_CONSTRAINT_CFM:
926  m_linearLimits.m_motorCFM[axis] = value;
928  break;
929  default:
931  }
932  }
933  else if ((axis >= 3) && (axis < 6))
934  {
935  switch (num)
936  {
938  m_angularLimits[axis - 3].m_stopERP = value;
940  break;
942  m_angularLimits[axis - 3].m_stopCFM = value;
944  break;
945  case BT_CONSTRAINT_ERP:
946  m_angularLimits[axis - 3].m_motorERP = value;
948  break;
949  case BT_CONSTRAINT_CFM:
950  m_angularLimits[axis - 3].m_motorCFM = value;
952  break;
953  default:
955  }
956  }
957  else
958  {
960  }
961 }
962 
963 //return the local value of parameter
964 btScalar btGeneric6DofSpring2Constraint::getParam(int num, int axis) const
965 {
966  btScalar retVal = 0;
967  if ((axis >= 0) && (axis < 3))
968  {
969  switch (num)
970  {
973  retVal = m_linearLimits.m_stopERP[axis];
974  break;
977  retVal = m_linearLimits.m_stopCFM[axis];
978  break;
979  case BT_CONSTRAINT_ERP:
981  retVal = m_linearLimits.m_motorERP[axis];
982  break;
983  case BT_CONSTRAINT_CFM:
985  retVal = m_linearLimits.m_motorCFM[axis];
986  break;
987  default:
989  }
990  }
991  else if ((axis >= 3) && (axis < 6))
992  {
993  switch (num)
994  {
997  retVal = m_angularLimits[axis - 3].m_stopERP;
998  break;
1001  retVal = m_angularLimits[axis - 3].m_stopCFM;
1002  break;
1003  case BT_CONSTRAINT_ERP:
1005  retVal = m_angularLimits[axis - 3].m_motorERP;
1006  break;
1007  case BT_CONSTRAINT_CFM:
1009  retVal = m_angularLimits[axis - 3].m_motorCFM;
1010  break;
1011  default:
1013  }
1014  }
1015  else
1016  {
1018  }
1019  return retVal;
1020 }
1021 
1022 void btGeneric6DofSpring2Constraint::setAxis(const btVector3& axis1, const btVector3& axis2)
1023 {
1024  btVector3 zAxis = axis1.normalized();
1025  btVector3 yAxis = axis2.normalized();
1026  btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
1027 
1028  btTransform frameInW;
1029  frameInW.setIdentity();
1030  frameInW.getBasis().setValue(xAxis[0], yAxis[0], zAxis[0],
1031  xAxis[1], yAxis[1], zAxis[1],
1032  xAxis[2], yAxis[2], zAxis[2]);
1033 
1034  // now get constraint frame in local coordinate systems
1035  m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW;
1036  m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW;
1037 
1039 }
1040 
1042 {
1043  btAssert((index >= 0) && (index < 6));
1044  if (index < 3)
1045  m_linearLimits.m_bounce[index] = bounce;
1046  else
1047  m_angularLimits[index - 3].m_bounce = bounce;
1048 }
1049 
1051 {
1052  btAssert((index >= 0) && (index < 6));
1053  if (index < 3)
1054  m_linearLimits.m_enableMotor[index] = onOff;
1055  else
1056  m_angularLimits[index - 3].m_enableMotor = onOff;
1057 }
1058 
1059 void btGeneric6DofSpring2Constraint::setServo(int index, bool onOff)
1060 {
1061  btAssert((index >= 0) && (index < 6));
1062  if (index < 3)
1063  m_linearLimits.m_servoMotor[index] = onOff;
1064  else
1065  m_angularLimits[index - 3].m_servoMotor = onOff;
1066 }
1067 
1069 {
1070  btAssert((index >= 0) && (index < 6));
1071  if (index < 3)
1072  m_linearLimits.m_targetVelocity[index] = velocity;
1073  else
1074  m_angularLimits[index - 3].m_targetVelocity = velocity;
1075 }
1076 
1078 {
1079  btAssert((index >= 0) && (index < 6));
1080  if (index < 3)
1081  {
1082  m_linearLimits.m_servoTarget[index] = targetOrg;
1083  }
1084  else
1085  {
1086  //wrap between -PI and PI, see also
1087  //https://stackoverflow.com/questions/4633177/c-how-to-wrap-a-float-to-the-interval-pi-pi
1088 
1089  btScalar target = targetOrg + SIMD_PI;
1090  if (1)
1091  {
1092  btScalar m = target - SIMD_2_PI * std::floor(target / SIMD_2_PI);
1093  // handle boundary cases resulted from floating-point cut off:
1094  {
1095  if (m >= SIMD_2_PI)
1096  {
1097  target = 0;
1098  }
1099  else
1100  {
1101  if (m < 0)
1102  {
1103  if (SIMD_2_PI + m == SIMD_2_PI)
1104  target = 0;
1105  else
1106  target = SIMD_2_PI + m;
1107  }
1108  else
1109  {
1110  target = m;
1111  }
1112  }
1113  }
1114  target -= SIMD_PI;
1115  }
1116 
1117  m_angularLimits[index - 3].m_servoTarget = target;
1118  }
1119 }
1120 
1122 {
1123  btAssert((index >= 0) && (index < 6));
1124  if (index < 3)
1125  m_linearLimits.m_maxMotorForce[index] = force;
1126  else
1127  m_angularLimits[index - 3].m_maxMotorForce = force;
1128 }
1129 
1131 {
1132  btAssert((index >= 0) && (index < 6));
1133  if (index < 3)
1134  m_linearLimits.m_enableSpring[index] = onOff;
1135  else
1136  m_angularLimits[index - 3].m_enableSpring = onOff;
1137 }
1138 
1139 void btGeneric6DofSpring2Constraint::setStiffness(int index, btScalar stiffness, bool limitIfNeeded)
1140 {
1141  btAssert((index >= 0) && (index < 6));
1142  if (index < 3)
1143  {
1144  m_linearLimits.m_springStiffness[index] = stiffness;
1145  m_linearLimits.m_springStiffnessLimited[index] = limitIfNeeded;
1146  }
1147  else
1148  {
1149  m_angularLimits[index - 3].m_springStiffness = stiffness;
1150  m_angularLimits[index - 3].m_springStiffnessLimited = limitIfNeeded;
1151  }
1152 }
1153 
1154 void btGeneric6DofSpring2Constraint::setDamping(int index, btScalar damping, bool limitIfNeeded)
1155 {
1156  btAssert((index >= 0) && (index < 6));
1157  if (index < 3)
1158  {
1159  m_linearLimits.m_springDamping[index] = damping;
1160  m_linearLimits.m_springDampingLimited[index] = limitIfNeeded;
1161  }
1162  else
1163  {
1164  m_angularLimits[index - 3].m_springDamping = damping;
1165  m_angularLimits[index - 3].m_springDampingLimited = limitIfNeeded;
1166  }
1167 }
1168 
1170 {
1172  int i;
1173  for (i = 0; i < 3; i++)
1174  m_linearLimits.m_equilibriumPoint[i] = m_calculatedLinearDiff[i];
1175  for (i = 0; i < 3; i++)
1177 }
1178 
1180 {
1181  btAssert((index >= 0) && (index < 6));
1183  if (index < 3)
1184  m_linearLimits.m_equilibriumPoint[index] = m_calculatedLinearDiff[index];
1185  else
1186  m_angularLimits[index - 3].m_equilibriumPoint = m_calculatedAxisAngleDiff[index - 3];
1187 }
1188 
1190 {
1191  btAssert((index >= 0) && (index < 6));
1192  if (index < 3)
1193  m_linearLimits.m_equilibriumPoint[index] = val;
1194  else
1195  m_angularLimits[index - 3].m_equilibriumPoint = val;
1196 }
1197 
1199 
1201 {
1202  //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
1203  if (m_loLimit > m_hiLimit)
1204  {
1205  m_currentLimit = 0;
1207  }
1208  else if (m_loLimit == m_hiLimit)
1209  {
1210  m_currentLimitError = test_value - m_loLimit;
1211  m_currentLimit = 3;
1212  }
1213  else
1214  {
1215  m_currentLimitError = test_value - m_loLimit;
1216  m_currentLimitErrorHi = test_value - m_hiLimit;
1217  m_currentLimit = 4;
1218  }
1219 }
1220 
1222 
1224 {
1225  btScalar loLimit = m_lowerLimit[limitIndex];
1226  btScalar hiLimit = m_upperLimit[limitIndex];
1227  if (loLimit > hiLimit)
1228  {
1229  m_currentLimitError[limitIndex] = 0;
1230  m_currentLimit[limitIndex] = 0;
1231  }
1232  else if (loLimit == hiLimit)
1233  {
1234  m_currentLimitError[limitIndex] = test_value - loLimit;
1235  m_currentLimit[limitIndex] = 3;
1236  }
1237  else
1238  {
1239  m_currentLimitError[limitIndex] = test_value - loLimit;
1240  m_currentLimitErrorHi[limitIndex] = test_value - hiLimit;
1241  m_currentLimit[limitIndex] = 4;
1242  }
1243 }
virtual void getInfo2(btConstraintInfo2 *info)
virtual void buildJacobian()
internal method used by the constraint solver, don't use them directly
virtual void setParam(int num, btScalar value, int axis=-1)
void setDamping(btScalar damping)
virtual void setFrames(const btTransform &frameA, const btTransform &frameB)
virtual void getInfo1(btConstraintInfo1 *info)
virtual btScalar getParam(int num, int axis=-1) const
return the local value of parameter
const btRigidBody & getRigidBodyA() const
void enableMotor(bool b)
const btRigidBody & getRigidBodyB() const
int m_flags
btFixedConstraint btRigidBody & rbB
btFixedConstraint btRigidBody const btTransform & frameInA
btFixedConstraint btRigidBody const btTransform const btTransform & frameInB
bool matrixToEulerXYZ(const btMatrix3x3 &mat, btVector3 &xyz)
MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3....
btScalar btGetMatrixElem(const btMatrix3x3 &mat, int index)
btTransform m_calculatedTransformB
btScalar m_factA
btScalar m_factB
btVector3 m_calculatedAxis[3]
void calculateLinearInfo()
btVector3 m_calculatedLinearDiff
btRotationalLimitMotor m_angularLimits[3]
int setLinearLimits(btConstraintInfo2 *info, int row, const btTransform &transA, const btTransform &transB, const btVector3 &linVelA, const btVector3 &linVelB, const btVector3 &angVelA, const btVector3 &angVelB)
void setAxis(const btVector3 &axis1, const btVector3 &axis2)
int get_limit_motor_info2(btRotationalLimitMotor *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)
btTranslationalLimitMotor m_linearLimits
btTransform m_frameInB
void calculateAngleInfo()
calcs the euler angles between the two bodies.
void calculateTransforms(const btTransform &transA, const btTransform &transB)
Calcs global transform of the offsets.
btVector3 m_calculatedAxisAngleDiff
btVector3 getAxis(int axis_index) const
Get the rotation axis in global coordinates.
bool testAngularLimitMotor(int axis_index)
Test angular limit.
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)
btTransform m_calculatedTransformA
bool m_hasStaticBody
#define D6_LIMIT_ERROR_THRESHOLD_FOR_ROTATION
static bool matrixToEulerYZX(const btMatrix3x3 &mat, btVector3 &xyz)
void setStiffness(int index, btScalar stiffness, bool limitIfNeeded=true)
static bool matrixToEulerYXZ(const btMatrix3x3 &mat, btVector3 &xyz)
#define BT_6DOF_FLAGS_AXIS_SHIFT2
void enableSpring(int index, bool onOff)
void setBounce(int index, btScalar bounce)
static bool matrixToEulerZXY(const btMatrix3x3 &mat, btVector3 &xyz)
void calculateJacobi(btRotationalLimitMotor2 *limot, const btTransform &transA, const btTransform &transB, btConstraintInfo2 *info, int srow, btVector3 &ax1, int rotational, int rotAllowed)
void setServoTarget(int index, btScalar target)
void setMaxMotorForce(int index, btScalar force)
static bool matrixToEulerZYX(const btMatrix3x3 &mat, btVector3 &xyz)
static bool matrixToEulerXZY(const btMatrix3x3 &mat, btVector3 &xyz)
void setTargetVelocity(int index, btScalar velocity)
void setServo(int index, bool onOff)
btGeneric6DofSpring2Constraint(btRigidBody &rbA, btRigidBody &rbB, const btTransform &frameInA, const btTransform &frameInB, RotateOrder rotOrder=RO_XYZ)
@ BT_6DOF_FLAGS_USE_INFINITE_ERROR
RotateOrder m_rotateOrder
btScalar m_equilibriumPoint[6]
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:50
#define SIMD_PI
Definition: btScalar.h:526
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
#define BT_ZERO
Definition: btScalar.h:546
#define BT_ONE
Definition: btScalar.h:545
SIMD_FORCE_INLINE btScalar btAsin(btScalar x)
Definition: btScalar.h:509
SIMD_FORCE_INLINE btScalar btSqrt(btScalar y)
Definition: btScalar.h:466
#define SIMD_INFINITY
Definition: btScalar.h:544
SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y)
Definition: btScalar.h:518
#define SIMD_EPSILON
Definition: btScalar.h:543
#define SIMD_HALF_PI
Definition: btScalar.h:528
#define SIMD_2_PI
Definition: btScalar.h:527
#define btAssert(x)
Definition: btScalar.h:295
btTransform m_frameInA
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact)
internal method used by the constraint solver, don't use them directly
btRigidBody & m_rbA
SIMD_FORCE_INLINE btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScalar angleLowerLimitInRadians, btScalar angleUpperLimitInRadians)
#define btAssertConstrParams(_par)
btTypedConstraint(btTypedConstraintType type, btRigidBody &rbA)
btRigidBody & m_rbB
static btRigidBody & getFixedBody()
@ BT_CONSTRAINT_CFM
@ BT_CONSTRAINT_ERP
@ BT_CONSTRAINT_STOP_CFM
@ BT_CONSTRAINT_STOP_ERP
@ D6_SPRING_2_CONSTRAINT_TYPE
SIMD_FORCE_INLINE btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:251
SIMD_FORCE_INLINE btScalar angle(const btVector3 &v) const
Return the angle between this and another vector.
Definition: btVector3.h:356
btVector3
btVector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-by...
Definition: btVector3.h:82
const btVector3 & getAngularVelocity() const
Definition: btRigidBody.h:437
btScalar getInvMass() const
Definition: btRigidBody.h:263
const btTransform & getCenterOfMassTransform() const
Definition: btRigidBody.h:429
const btVector3 & getLinearVelocity() const
Definition: btRigidBody.h:433
const btMatrix3x3 & getInvInertiaTensorWorld() const
Definition: btRigidBody.h:265
void testLimitValue(btScalar test_value)
btScalar m_currentPosition
How much is violated this limit.
btScalar m_targetVelocity
target motor velocity
btScalar m_maxMotorForce
max force on motor
btScalar m_bounce
restitution factor
btScalar m_stopERP
Error tolerance factor when joint is at limit.
int testLimitValue(btScalar test_value)
calculates error
int m_currentLimit
current value of angle
btScalar m_stopCFM
Constraint force mixing factor when joint is at limit.
void testLimitValue(int limitIndex, btScalar test_value)
btVector3 m_stopERP
Error tolerance factor when joint is at limit.
int m_currentLimit[3]
Current relative offset of constraint frames.
btVector3 m_maxMotorForce
max force on motor
int testLimitValue(int limitIndex, btScalar test_value)
btVector3 m_currentLinearDiff
How much is violated this limit.
btVector3 m_targetVelocity
target motor velocity
btVector3 m_lowerLimit
the constraint lower limits
btVector3 m_upperLimit
the constraint upper limits
int count
static void error(const char *str)
Definition: meshlaplacian.c:51
T dot(const vec_base< T, Size > &a, const vec_base< T, Size > &b)
T length(const vec_base< T, Size > &a)
T floor(const T &a)
btScalar * m_J2angularAxis
btScalar * m_J1angularAxis
btScalar * m_constraintError