Bullet Collision Detection & Physics Library
btSliderConstraint.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 Added by Roman Ponomarev (rponom@gmail.com)
18 April 04, 2008
19 */
20 
21 
22 
23 #include "btSliderConstraint.h"
24 #include "BulletDynamics/Dynamics/btRigidBody.h"
25 #include "LinearMath/btTransformUtil.h"
26 #include <new>
27 
28 #define USE_OFFSET_FOR_CONSTANT_FRAME true
29 
30 void btSliderConstraint::initParams()
31 {
32  m_lowerLinLimit = btScalar(1.0);
33  m_upperLinLimit = btScalar(-1.0);
34  m_lowerAngLimit = btScalar(0.);
35  m_upperAngLimit = btScalar(0.);
36  m_softnessDirLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
37  m_restitutionDirLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
38  m_dampingDirLin = btScalar(0.);
39  m_cfmDirLin = SLIDER_CONSTRAINT_DEF_CFM;
40  m_softnessDirAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
41  m_restitutionDirAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
42  m_dampingDirAng = btScalar(0.);
43  m_cfmDirAng = SLIDER_CONSTRAINT_DEF_CFM;
44  m_softnessOrthoLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
45  m_restitutionOrthoLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
46  m_dampingOrthoLin = SLIDER_CONSTRAINT_DEF_DAMPING;
47  m_cfmOrthoLin = SLIDER_CONSTRAINT_DEF_CFM;
48  m_softnessOrthoAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
49  m_restitutionOrthoAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
50  m_dampingOrthoAng = SLIDER_CONSTRAINT_DEF_DAMPING;
51  m_cfmOrthoAng = SLIDER_CONSTRAINT_DEF_CFM;
52  m_softnessLimLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
53  m_restitutionLimLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
54  m_dampingLimLin = SLIDER_CONSTRAINT_DEF_DAMPING;
55  m_cfmLimLin = SLIDER_CONSTRAINT_DEF_CFM;
56  m_softnessLimAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
57  m_restitutionLimAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
58  m_dampingLimAng = SLIDER_CONSTRAINT_DEF_DAMPING;
59  m_cfmLimAng = SLIDER_CONSTRAINT_DEF_CFM;
60 
61  m_poweredLinMotor = false;
62  m_targetLinMotorVelocity = btScalar(0.);
63  m_maxLinMotorForce = btScalar(0.);
64  m_accumulatedLinMotorImpulse = btScalar(0.0);
65 
66  m_poweredAngMotor = false;
67  m_targetAngMotorVelocity = btScalar(0.);
68  m_maxAngMotorForce = btScalar(0.);
69  m_accumulatedAngMotorImpulse = btScalar(0.0);
70 
71  m_flags = 0;
72  m_flags = 0;
73 
74  m_useOffsetForConstraintFrame = USE_OFFSET_FOR_CONSTANT_FRAME;
75 
76  calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
77 }
78 
79 
80 
81 
82 
83 btSliderConstraint::btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
84  : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, rbA, rbB),
85  m_useSolveConstraintObsolete(false),
86  m_frameInA(frameInA),
87  m_frameInB(frameInB),
88  m_useLinearReferenceFrameA(useLinearReferenceFrameA)
89 {
90  initParams();
91 }
92 
93 
94 
95 btSliderConstraint::btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameA)
96  : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, getFixedBody(), rbB),
97  m_useSolveConstraintObsolete(false),
98  m_frameInB(frameInB),
99  m_useLinearReferenceFrameA(useLinearReferenceFrameA)
100 {
102  m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB;
103 // m_frameInA.getOrigin() = m_rbA.getCenterOfMassTransform()(m_frameInA.getOrigin());
104 
105  initParams();
106 }
107 
108 
109 
110 
111 
112 
113 void btSliderConstraint::getInfo1(btConstraintInfo1* info)
114 {
115  if (m_useSolveConstraintObsolete)
116  {
117  info->m_numConstraintRows = 0;
118  info->nub = 0;
119  }
120  else
121  {
122  info->m_numConstraintRows = 4; // Fixed 2 linear + 2 angular
123  info->nub = 2;
124  //prepare constraint
125  calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
126  testAngLimits();
127  testLinLimits();
128  if(getSolveLinLimit() || getPoweredLinMotor())
129  {
130  info->m_numConstraintRows++; // limit 3rd linear as well
131  info->nub--;
132  }
133  if(getSolveAngLimit() || getPoweredAngMotor())
134  {
135  info->m_numConstraintRows++; // limit 3rd angular as well
136  info->nub--;
137  }
138  }
139 }
140 
141 void btSliderConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
142 {
143 
144  info->m_numConstraintRows = 6; // Fixed 2 linear + 2 angular + 1 limit (even if not used)
145  info->nub = 0;
146 }
147 
148 void btSliderConstraint::getInfo2(btConstraintInfo2* info)
149 {
150  getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(), m_rbA.getLinearVelocity(),m_rbB.getLinearVelocity(), m_rbA.getInvMass(),m_rbB.getInvMass());
151 }
152 
153 
154 
155 
156 
157 
158 
159 void btSliderConstraint::calculateTransforms(const btTransform& transA,const btTransform& transB)
160 {
161  if(m_useLinearReferenceFrameA || (!m_useSolveConstraintObsolete))
162  {
163  m_calculatedTransformA = transA * m_frameInA;
164  m_calculatedTransformB = transB * m_frameInB;
165  }
166  else
167  {
168  m_calculatedTransformA = transB * m_frameInB;
169  m_calculatedTransformB = transA * m_frameInA;
170  }
171  m_realPivotAInW = m_calculatedTransformA.getOrigin();
172  m_realPivotBInW = m_calculatedTransformB.getOrigin();
173  m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X
174  if(m_useLinearReferenceFrameA || m_useSolveConstraintObsolete)
175  {
176  m_delta = m_realPivotBInW - m_realPivotAInW;
177  }
178  else
179  {
180  m_delta = m_realPivotAInW - m_realPivotBInW;
181  }
182  m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis;
183  btVector3 normalWorld;
184  int i;
185  //linear part
186  for(i = 0; i < 3; i++)
187  {
188  normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
189  m_depth[i] = m_delta.dot(normalWorld);
190  }
191 }
192 
193 
194 
195 void btSliderConstraint::testLinLimits(void)
196 {
197  m_solveLinLim = false;
198  m_linPos = m_depth[0];
199  if(m_lowerLinLimit <= m_upperLinLimit)
200  {
201  if(m_depth[0] > m_upperLinLimit)
202  {
203  m_depth[0] -= m_upperLinLimit;
204  m_solveLinLim = true;
205  }
206  else if(m_depth[0] < m_lowerLinLimit)
207  {
208  m_depth[0] -= m_lowerLinLimit;
209  m_solveLinLim = true;
210  }
211  else
212  {
213  m_depth[0] = btScalar(0.);
214  }
215  }
216  else
217  {
218  m_depth[0] = btScalar(0.);
219  }
220 }
221 
222 
223 
224 void btSliderConstraint::testAngLimits(void)
225 {
226  m_angDepth = btScalar(0.);
227  m_solveAngLim = false;
228  if(m_lowerAngLimit <= m_upperAngLimit)
229  {
230  const btVector3 axisA0 = m_calculatedTransformA.getBasis().getColumn(1);
231  const btVector3 axisA1 = m_calculatedTransformA.getBasis().getColumn(2);
232  const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1);
233 // btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0));
234  btScalar rot = btAtan2(axisB0.dot(axisA1), axisB0.dot(axisA0));
235  rot = btAdjustAngleToLimits(rot, m_lowerAngLimit, m_upperAngLimit);
236  m_angPos = rot;
237  if(rot < m_lowerAngLimit)
238  {
239  m_angDepth = rot - m_lowerAngLimit;
240  m_solveAngLim = true;
241  }
242  else if(rot > m_upperAngLimit)
243  {
244  m_angDepth = rot - m_upperAngLimit;
245  m_solveAngLim = true;
246  }
247  }
248 }
249 
250 btVector3 btSliderConstraint::getAncorInA(void)
251 {
252  btVector3 ancorInA;
253  ancorInA = m_realPivotAInW + (m_lowerLinLimit + m_upperLinLimit) * btScalar(0.5) * m_sliderAxis;
254  ancorInA = m_rbA.getCenterOfMassTransform().inverse() * ancorInA;
255  return ancorInA;
256 }
257 
258 
259 
260 btVector3 btSliderConstraint::getAncorInB(void)
261 {
262  btVector3 ancorInB;
263  ancorInB = m_frameInB.getOrigin();
264  return ancorInB;
265 }
266 
267 
268 void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB, const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass )
269 {
270  const btTransform& trA = getCalculatedTransformA();
271  const btTransform& trB = getCalculatedTransformB();
272 
273  btAssert(!m_useSolveConstraintObsolete);
274  int i, s = info->rowskip;
275 
276  btScalar signFact = m_useLinearReferenceFrameA ? btScalar(1.0f) : btScalar(-1.0f);
277 
278  // difference between frames in WCS
279  btVector3 ofs = trB.getOrigin() - trA.getOrigin();
280  // now get weight factors depending on masses
281  btScalar miA = rbAinvMass;
282  btScalar miB = rbBinvMass;
283  bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
284  btScalar miS = miA + miB;
285  btScalar factA, factB;
286  if(miS > btScalar(0.f))
287  {
288  factA = miB / miS;
289  }
290  else
291  {
292  factA = btScalar(0.5f);
293  }
294  factB = btScalar(1.0f) - factA;
295  btVector3 ax1, p, q;
296  btVector3 ax1A = trA.getBasis().getColumn(0);
297  btVector3 ax1B = trB.getBasis().getColumn(0);
298  if(m_useOffsetForConstraintFrame)
299  {
300  // get the desired direction of slider axis
301  // as weighted sum of X-orthos of frameA and frameB in WCS
302  ax1 = ax1A * factA + ax1B * factB;
303  ax1.normalize();
304  // construct two orthos to slider axis
305  btPlaneSpace1 (ax1, p, q);
306  }
307  else
308  { // old way - use frameA
309  ax1 = trA.getBasis().getColumn(0);
310  // get 2 orthos to slider axis (Y, Z)
311  p = trA.getBasis().getColumn(1);
312  q = trA.getBasis().getColumn(2);
313  }
314  // make rotations around these orthos equal
315  // the slider axis should be the only unconstrained
316  // rotational axis, the angular velocity of the two bodies perpendicular to
317  // the slider axis should be equal. thus the constraint equations are
318  // p*w1 - p*w2 = 0
319  // q*w1 - q*w2 = 0
320  // where p and q are unit vectors normal to the slider axis, and w1 and w2
321  // are the angular velocity vectors of the two bodies.
322  info->m_J1angularAxis[0] = p[0];
323  info->m_J1angularAxis[1] = p[1];
324  info->m_J1angularAxis[2] = p[2];
325  info->m_J1angularAxis[s+0] = q[0];
326  info->m_J1angularAxis[s+1] = q[1];
327  info->m_J1angularAxis[s+2] = q[2];
328 
329  info->m_J2angularAxis[0] = -p[0];
330  info->m_J2angularAxis[1] = -p[1];
331  info->m_J2angularAxis[2] = -p[2];
332  info->m_J2angularAxis[s+0] = -q[0];
333  info->m_J2angularAxis[s+1] = -q[1];
334  info->m_J2angularAxis[s+2] = -q[2];
335  // compute the right hand side of the constraint equation. set relative
336  // body velocities along p and q to bring the slider back into alignment.
337  // if ax1A,ax1B are the unit length slider axes as computed from bodyA and
338  // bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2).
339  // if "theta" is the angle between ax1 and ax2, we need an angular velocity
340  // along u to cover angle erp*theta in one step :
341  // |angular_velocity| = angle/time = erp*theta / stepsize
342  // = (erp*fps) * theta
343  // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
344  // = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
345  // ...as ax1 and ax2 are unit length. if theta is smallish,
346  // theta ~= sin(theta), so
347  // angular_velocity = (erp*fps) * (ax1 x ax2)
348  // ax1 x ax2 is in the plane space of ax1, so we project the angular
349  // velocity to p and q to find the right hand side.
350 // btScalar k = info->fps * info->erp * getSoftnessOrthoAng();
351  btScalar currERP = (m_flags & BT_SLIDER_FLAGS_ERP_ORTANG) ? m_softnessOrthoAng : m_softnessOrthoAng * info->erp;
352  btScalar k = info->fps * currERP;
353 
354  btVector3 u = ax1A.cross(ax1B);
355  info->m_constraintError[0] = k * u.dot(p);
356  info->m_constraintError[s] = k * u.dot(q);
357  if(m_flags & BT_SLIDER_FLAGS_CFM_ORTANG)
358  {
359  info->cfm[0] = m_cfmOrthoAng;
360  info->cfm[s] = m_cfmOrthoAng;
361  }
362 
363  int nrow = 1; // last filled row
364  int srow;
365  btScalar limit_err;
366  int limit;
367  int powered;
368 
369  // next two rows.
370  // we want: velA + wA x relA == velB + wB x relB ... but this would
371  // result in three equations, so we project along two orthos to the slider axis
372 
373  btTransform bodyA_trans = transA;
374  btTransform bodyB_trans = transB;
375  nrow++;
376  int s2 = nrow * s;
377  nrow++;
378  int s3 = nrow * s;
379  btVector3 tmpA(0,0,0), tmpB(0,0,0), relA(0,0,0), relB(0,0,0), c(0,0,0);
380  if(m_useOffsetForConstraintFrame)
381  {
382  // get vector from bodyB to frameB in WCS
383  relB = trB.getOrigin() - bodyB_trans.getOrigin();
384  // get its projection to slider axis
385  btVector3 projB = ax1 * relB.dot(ax1);
386  // get vector directed from bodyB to slider axis (and orthogonal to it)
387  btVector3 orthoB = relB - projB;
388  // same for bodyA
389  relA = trA.getOrigin() - bodyA_trans.getOrigin();
390  btVector3 projA = ax1 * relA.dot(ax1);
391  btVector3 orthoA = relA - projA;
392  // get desired offset between frames A and B along slider axis
393  btScalar sliderOffs = m_linPos - m_depth[0];
394  // desired vector from projection of center of bodyA to projection of center of bodyB to slider axis
395  btVector3 totalDist = projA + ax1 * sliderOffs - projB;
396  // get offset vectors relA and relB
397  relA = orthoA + totalDist * factA;
398  relB = orthoB - totalDist * factB;
399  // now choose average ortho to slider axis
400  p = orthoB * factA + orthoA * factB;
401  btScalar len2 = p.length2();
402  if(len2 > SIMD_EPSILON)
403  {
404  p /= btSqrt(len2);
405  }
406  else
407  {
408  p = trA.getBasis().getColumn(1);
409  }
410  // make one more ortho
411  q = ax1.cross(p);
412  // fill two rows
413  tmpA = relA.cross(p);
414  tmpB = relB.cross(p);
415  for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i];
416  for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i];
417  tmpA = relA.cross(q);
418  tmpB = relB.cross(q);
419  if(hasStaticBody && getSolveAngLimit())
420  { // to make constraint between static and dynamic objects more rigid
421  // remove wA (or wB) from equation if angular limit is hit
422  tmpB *= factB;
423  tmpA *= factA;
424  }
425  for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = tmpA[i];
426  for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = -tmpB[i];
427  for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i];
428  for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i];
429  }
430  else
431  { // old way - maybe incorrect if bodies are not on the slider axis
432  // see discussion "Bug in slider constraint" http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=9&t=4024&start=0
433  c = bodyB_trans.getOrigin() - bodyA_trans.getOrigin();
434  btVector3 tmp = c.cross(p);
435  for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = factA*tmp[i];
436  for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = factB*tmp[i];
437  tmp = c.cross(q);
438  for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = factA*tmp[i];
439  for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = factB*tmp[i];
440 
441  for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i];
442  for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i];
443  }
444  // compute two elements of right hand side
445 
446  // k = info->fps * info->erp * getSoftnessOrthoLin();
447  currERP = (m_flags & BT_SLIDER_FLAGS_ERP_ORTLIN) ? m_softnessOrthoLin : m_softnessOrthoLin * info->erp;
448  k = info->fps * currERP;
449 
450  btScalar rhs = k * p.dot(ofs);
451  info->m_constraintError[s2] = rhs;
452  rhs = k * q.dot(ofs);
453  info->m_constraintError[s3] = rhs;
454  if(m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN)
455  {
456  info->cfm[s2] = m_cfmOrthoLin;
457  info->cfm[s3] = m_cfmOrthoLin;
458  }
459 
460 
461  // check linear limits
462  limit_err = btScalar(0.0);
463  limit = 0;
464  if(getSolveLinLimit())
465  {
466  limit_err = getLinDepth() * signFact;
467  limit = (limit_err > btScalar(0.0)) ? 2 : 1;
468  }
469  powered = 0;
470  if(getPoweredLinMotor())
471  {
472  powered = 1;
473  }
474  // if the slider has joint limits or motor, add in the extra row
475  if (limit || powered)
476  {
477  nrow++;
478  srow = nrow * info->rowskip;
479  info->m_J1linearAxis[srow+0] = ax1[0];
480  info->m_J1linearAxis[srow+1] = ax1[1];
481  info->m_J1linearAxis[srow+2] = ax1[2];
482  // linear torque decoupling step:
483  //
484  // we have to be careful that the linear constraint forces (+/- ax1) applied to the two bodies
485  // do not create a torque couple. in other words, the points that the
486  // constraint force is applied at must lie along the same ax1 axis.
487  // a torque couple will result in limited slider-jointed free
488  // bodies from gaining angular momentum.
489  if(m_useOffsetForConstraintFrame)
490  {
491  // this is needed only when bodyA and bodyB are both dynamic.
492  if(!hasStaticBody)
493  {
494  tmpA = relA.cross(ax1);
495  tmpB = relB.cross(ax1);
496  info->m_J1angularAxis[srow+0] = tmpA[0];
497  info->m_J1angularAxis[srow+1] = tmpA[1];
498  info->m_J1angularAxis[srow+2] = tmpA[2];
499  info->m_J2angularAxis[srow+0] = -tmpB[0];
500  info->m_J2angularAxis[srow+1] = -tmpB[1];
501  info->m_J2angularAxis[srow+2] = -tmpB[2];
502  }
503  }
504  else
505  { // The old way. May be incorrect if bodies are not on the slider axis
506  btVector3 ltd; // Linear Torque Decoupling vector (a torque)
507  ltd = c.cross(ax1);
508  info->m_J1angularAxis[srow+0] = factA*ltd[0];
509  info->m_J1angularAxis[srow+1] = factA*ltd[1];
510  info->m_J1angularAxis[srow+2] = factA*ltd[2];
511  info->m_J2angularAxis[srow+0] = factB*ltd[0];
512  info->m_J2angularAxis[srow+1] = factB*ltd[1];
513  info->m_J2angularAxis[srow+2] = factB*ltd[2];
514  }
515  // right-hand part
516  btScalar lostop = getLowerLinLimit();
517  btScalar histop = getUpperLinLimit();
518  if(limit && (lostop == histop))
519  { // the joint motor is ineffective
520  powered = 0;
521  }
522  info->m_constraintError[srow] = 0.;
523  info->m_lowerLimit[srow] = 0.;
524  info->m_upperLimit[srow] = 0.;
525  currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMLIN) ? m_softnessLimLin : info->erp;
526  if(powered)
527  {
528  if(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN)
529  {
530  info->cfm[srow] = m_cfmDirLin;
531  }
532  btScalar tag_vel = getTargetLinMotorVelocity();
533  btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * currERP);
534  info->m_constraintError[srow] -= signFact * mot_fact * getTargetLinMotorVelocity();
535  info->m_lowerLimit[srow] += -getMaxLinMotorForce() * info->fps;
536  info->m_upperLimit[srow] += getMaxLinMotorForce() * info->fps;
537  }
538  if(limit)
539  {
540  k = info->fps * currERP;
541  info->m_constraintError[srow] += k * limit_err;
542  if(m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN)
543  {
544  info->cfm[srow] = m_cfmLimLin;
545  }
546  if(lostop == histop)
547  { // limited low and high simultaneously
548  info->m_lowerLimit[srow] = -SIMD_INFINITY;
549  info->m_upperLimit[srow] = SIMD_INFINITY;
550  }
551  else if(limit == 1)
552  { // low limit
553  info->m_lowerLimit[srow] = -SIMD_INFINITY;
554  info->m_upperLimit[srow] = 0;
555  }
556  else
557  { // high limit
558  info->m_lowerLimit[srow] = 0;
559  info->m_upperLimit[srow] = SIMD_INFINITY;
560  }
561  // bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin) for that)
562  btScalar bounce = btFabs(btScalar(1.0) - getDampingLimLin());
563  if(bounce > btScalar(0.0))
564  {
565  btScalar vel = linVelA.dot(ax1);
566  vel -= linVelB.dot(ax1);
567  vel *= signFact;
568  // only apply bounce if the velocity is incoming, and if the
569  // resulting c[] exceeds what we already have.
570  if(limit == 1)
571  { // low limit
572  if(vel < 0)
573  {
574  btScalar newc = -bounce * vel;
575  if (newc > info->m_constraintError[srow])
576  {
577  info->m_constraintError[srow] = newc;
578  }
579  }
580  }
581  else
582  { // high limit - all those computations are reversed
583  if(vel > 0)
584  {
585  btScalar newc = -bounce * vel;
586  if(newc < info->m_constraintError[srow])
587  {
588  info->m_constraintError[srow] = newc;
589  }
590  }
591  }
592  }
593  info->m_constraintError[srow] *= getSoftnessLimLin();
594  } // if(limit)
595  } // if linear limit
596  // check angular limits
597  limit_err = btScalar(0.0);
598  limit = 0;
599  if(getSolveAngLimit())
600  {
601  limit_err = getAngDepth();
602  limit = (limit_err > btScalar(0.0)) ? 1 : 2;
603  }
604  // if the slider has joint limits, add in the extra row
605  powered = 0;
606  if(getPoweredAngMotor())
607  {
608  powered = 1;
609  }
610  if(limit || powered)
611  {
612  nrow++;
613  srow = nrow * info->rowskip;
614  info->m_J1angularAxis[srow+0] = ax1[0];
615  info->m_J1angularAxis[srow+1] = ax1[1];
616  info->m_J1angularAxis[srow+2] = ax1[2];
617 
618  info->m_J2angularAxis[srow+0] = -ax1[0];
619  info->m_J2angularAxis[srow+1] = -ax1[1];
620  info->m_J2angularAxis[srow+2] = -ax1[2];
621 
622  btScalar lostop = getLowerAngLimit();
623  btScalar histop = getUpperAngLimit();
624  if(limit && (lostop == histop))
625  { // the joint motor is ineffective
626  powered = 0;
627  }
628  currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMANG) ? m_softnessLimAng : info->erp;
629  if(powered)
630  {
631  if(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG)
632  {
633  info->cfm[srow] = m_cfmDirAng;
634  }
635  btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * currERP);
636  info->m_constraintError[srow] = mot_fact * getTargetAngMotorVelocity();
637  info->m_lowerLimit[srow] = -getMaxAngMotorForce() * info->fps;
638  info->m_upperLimit[srow] = getMaxAngMotorForce() * info->fps;
639  }
640  if(limit)
641  {
642  k = info->fps * currERP;
643  info->m_constraintError[srow] += k * limit_err;
644  if(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG)
645  {
646  info->cfm[srow] = m_cfmLimAng;
647  }
648  if(lostop == histop)
649  {
650  // limited low and high simultaneously
651  info->m_lowerLimit[srow] = -SIMD_INFINITY;
652  info->m_upperLimit[srow] = SIMD_INFINITY;
653  }
654  else if(limit == 1)
655  { // low limit
656  info->m_lowerLimit[srow] = 0;
657  info->m_upperLimit[srow] = SIMD_INFINITY;
658  }
659  else
660  { // high limit
661  info->m_lowerLimit[srow] = -SIMD_INFINITY;
662  info->m_upperLimit[srow] = 0;
663  }
664  // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
665  btScalar bounce = btFabs(btScalar(1.0) - getDampingLimAng());
666  if(bounce > btScalar(0.0))
667  {
668  btScalar vel = m_rbA.getAngularVelocity().dot(ax1);
669  vel -= m_rbB.getAngularVelocity().dot(ax1);
670  // only apply bounce if the velocity is incoming, and if the
671  // resulting c[] exceeds what we already have.
672  if(limit == 1)
673  { // low limit
674  if(vel < 0)
675  {
676  btScalar newc = -bounce * vel;
677  if(newc > info->m_constraintError[srow])
678  {
679  info->m_constraintError[srow] = newc;
680  }
681  }
682  }
683  else
684  { // high limit - all those computations are reversed
685  if(vel > 0)
686  {
687  btScalar newc = -bounce * vel;
688  if(newc < info->m_constraintError[srow])
689  {
690  info->m_constraintError[srow] = newc;
691  }
692  }
693  }
694  }
695  info->m_constraintError[srow] *= getSoftnessLimAng();
696  } // if(limit)
697  } // if angular limit or powered
698 }
699 
700 
703 void btSliderConstraint::setParam(int num, btScalar value, int axis)
704 {
705  switch(num)
706  {
707  case BT_CONSTRAINT_STOP_ERP :
708  if(axis < 1)
709  {
710  m_softnessLimLin = value;
711  m_flags |= BT_SLIDER_FLAGS_ERP_LIMLIN;
712  }
713  else if(axis < 3)
714  {
715  m_softnessOrthoLin = value;
716  m_flags |= BT_SLIDER_FLAGS_ERP_ORTLIN;
717  }
718  else if(axis == 3)
719  {
720  m_softnessLimAng = value;
721  m_flags |= BT_SLIDER_FLAGS_ERP_LIMANG;
722  }
723  else if(axis < 6)
724  {
725  m_softnessOrthoAng = value;
726  m_flags |= BT_SLIDER_FLAGS_ERP_ORTANG;
727  }
728  else
729  {
730  btAssertConstrParams(0);
731  }
732  break;
733  case BT_CONSTRAINT_CFM :
734  if(axis < 1)
735  {
736  m_cfmDirLin = value;
737  m_flags |= BT_SLIDER_FLAGS_CFM_DIRLIN;
738  }
739  else if(axis == 3)
740  {
741  m_cfmDirAng = value;
742  m_flags |= BT_SLIDER_FLAGS_CFM_DIRANG;
743  }
744  else
745  {
746  btAssertConstrParams(0);
747  }
748  break;
749  case BT_CONSTRAINT_STOP_CFM :
750  if(axis < 1)
751  {
752  m_cfmLimLin = value;
753  m_flags |= BT_SLIDER_FLAGS_CFM_LIMLIN;
754  }
755  else if(axis < 3)
756  {
757  m_cfmOrthoLin = value;
758  m_flags |= BT_SLIDER_FLAGS_CFM_ORTLIN;
759  }
760  else if(axis == 3)
761  {
762  m_cfmLimAng = value;
763  m_flags |= BT_SLIDER_FLAGS_CFM_LIMANG;
764  }
765  else if(axis < 6)
766  {
767  m_cfmOrthoAng = value;
768  m_flags |= BT_SLIDER_FLAGS_CFM_ORTANG;
769  }
770  else
771  {
772  btAssertConstrParams(0);
773  }
774  break;
775  }
776 }
777 
779 btScalar btSliderConstraint::getParam(int num, int axis) const
780 {
781  btScalar retVal(SIMD_INFINITY);
782  switch(num)
783  {
784  case BT_CONSTRAINT_STOP_ERP :
785  if(axis < 1)
786  {
787  btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_LIMLIN);
788  retVal = m_softnessLimLin;
789  }
790  else if(axis < 3)
791  {
792  btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_ORTLIN);
793  retVal = m_softnessOrthoLin;
794  }
795  else if(axis == 3)
796  {
797  btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_LIMANG);
798  retVal = m_softnessLimAng;
799  }
800  else if(axis < 6)
801  {
802  btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_ORTANG);
803  retVal = m_softnessOrthoAng;
804  }
805  else
806  {
807  btAssertConstrParams(0);
808  }
809  break;
810  case BT_CONSTRAINT_CFM :
811  if(axis < 1)
812  {
813  btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN);
814  retVal = m_cfmDirLin;
815  }
816  else if(axis == 3)
817  {
818  btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG);
819  retVal = m_cfmDirAng;
820  }
821  else
822  {
823  btAssertConstrParams(0);
824  }
825  break;
826  case BT_CONSTRAINT_STOP_CFM :
827  if(axis < 1)
828  {
829  btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN);
830  retVal = m_cfmLimLin;
831  }
832  else if(axis < 3)
833  {
834  btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN);
835  retVal = m_cfmOrthoLin;
836  }
837  else if(axis == 3)
838  {
839  btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG);
840  retVal = m_cfmLimAng;
841  }
842  else if(axis < 6)
843  {
844  btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_ORTANG);
845  retVal = m_cfmOrthoAng;
846  }
847  else
848  {
849  btAssertConstrParams(0);
850  }
851  break;
852  }
853  return retVal;
854 }
855 
856 
857 
Generated on Thu Oct 3 2013 18:25:07 for Bullet Collision Detection & Physics Library by   doxygen 1.8.4