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