1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org
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 2007-09-09
17 Refactored by Francisco Le?n
18 email: projectileman@yahoo.com
19 http://gimpact.sf.net
20 */
21
22 #include "btGeneric6DofConstraint.h"
23 #include "BulletDynamics/Dynamics/btRigidBody.h"
24 #include "LinearMath/btTransformUtil.h"
25 #include "LinearMath/btTransformUtil.h"
26 #include <new>
27
28 #define D6_USE_OBSOLETE_METHOD false
29 #define D6_USE_FRAME_OFFSET true
30
btGeneric6DofConstraint(btRigidBody & rbA,btRigidBody & rbB,const btTransform & frameInA,const btTransform & frameInB,bool useLinearReferenceFrameA)31 btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
32 : btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB), m_frameInA(frameInA), m_frameInB(frameInB), m_useLinearReferenceFrameA(useLinearReferenceFrameA), m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET), m_flags(0), m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)
33 {
34 calculateTransforms();
35 }
36
btGeneric6DofConstraint(btRigidBody & rbB,const btTransform & frameInB,bool useLinearReferenceFrameB)37 btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB)
38 : btTypedConstraint(D6_CONSTRAINT_TYPE, getFixedBody(), rbB),
39 m_frameInB(frameInB),
40 m_useLinearReferenceFrameA(useLinearReferenceFrameB),
41 m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),
42 m_flags(0),
43 m_useSolveConstraintObsolete(false)
44 {
45 ///not providing rigidbody A means implicitly using worldspace for body A
46 m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB;
47 calculateTransforms();
48 }
49
50 #define GENERIC_D6_DISABLE_WARMSTARTING 1
51
52 btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);
btGetMatrixElem(const btMatrix3x3 & mat,int index)53 btScalar btGetMatrixElem(const btMatrix3x3& mat, int index)
54 {
55 int i = index % 3;
56 int j = index / 3;
57 return mat[i][j];
58 }
59
60 ///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
61 bool matrixToEulerXYZ(const btMatrix3x3& mat, btVector3& xyz);
matrixToEulerXYZ(const btMatrix3x3 & mat,btVector3 & xyz)62 bool matrixToEulerXYZ(const btMatrix3x3& mat, btVector3& xyz)
63 {
64 // // rot = cy*cz -cy*sz sy
65 // // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx
66 // // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy
67 //
68
69 btScalar fi = btGetMatrixElem(mat, 2);
70 if (fi < btScalar(1.0f))
71 {
72 if (fi > btScalar(-1.0f))
73 {
74 xyz[0] = btAtan2(-btGetMatrixElem(mat, 5), btGetMatrixElem(mat, 8));
75 xyz[1] = btAsin(btGetMatrixElem(mat, 2));
76 xyz[2] = btAtan2(-btGetMatrixElem(mat, 1), btGetMatrixElem(mat, 0));
77 return true;
78 }
79 else
80 {
81 // WARNING. Not unique. XA - ZA = -atan2(r10,r11)
82 xyz[0] = -btAtan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4));
83 xyz[1] = -SIMD_HALF_PI;
84 xyz[2] = btScalar(0.0);
85 return false;
86 }
87 }
88 else
89 {
90 // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11)
91 xyz[0] = btAtan2(btGetMatrixElem(mat, 3), btGetMatrixElem(mat, 4));
92 xyz[1] = SIMD_HALF_PI;
93 xyz[2] = 0.0;
94 }
95 return false;
96 }
97
98 //////////////////////////// btRotationalLimitMotor ////////////////////////////////////
99
testLimitValue(btScalar test_value)100 int btRotationalLimitMotor::testLimitValue(btScalar test_value)
101 {
102 if (m_loLimit > m_hiLimit)
103 {
104 m_currentLimit = 0; //Free from violation
105 return 0;
106 }
107 if (test_value < m_loLimit)
108 {
109 m_currentLimit = 1; //low limit violation
110 m_currentLimitError = test_value - m_loLimit;
111 if (m_currentLimitError > SIMD_PI)
112 m_currentLimitError -= SIMD_2_PI;
113 else if (m_currentLimitError < -SIMD_PI)
114 m_currentLimitError += SIMD_2_PI;
115 return 1;
116 }
117 else if (test_value > m_hiLimit)
118 {
119 m_currentLimit = 2; //High limit violation
120 m_currentLimitError = test_value - m_hiLimit;
121 if (m_currentLimitError > SIMD_PI)
122 m_currentLimitError -= SIMD_2_PI;
123 else if (m_currentLimitError < -SIMD_PI)
124 m_currentLimitError += SIMD_2_PI;
125 return 2;
126 };
127
128 m_currentLimit = 0; //Free from violation
129 return 0;
130 }
131
solveAngularLimits(btScalar timeStep,btVector3 & axis,btScalar jacDiagABInv,btRigidBody * body0,btRigidBody * body1)132 btScalar btRotationalLimitMotor::solveAngularLimits(
133 btScalar timeStep, btVector3& axis, btScalar jacDiagABInv,
134 btRigidBody* body0, btRigidBody* body1)
135 {
136 if (needApplyTorques() == false) return 0.0f;
137
138 btScalar target_velocity = m_targetVelocity;
139 btScalar maxMotorForce = m_maxMotorForce;
140
141 //current error correction
142 if (m_currentLimit != 0)
143 {
144 target_velocity = -m_stopERP * m_currentLimitError / (timeStep);
145 maxMotorForce = m_maxLimitForce;
146 }
147
148 maxMotorForce *= timeStep;
149
150 // current velocity difference
151
152 btVector3 angVelA = body0->getAngularVelocity();
153 btVector3 angVelB = body1->getAngularVelocity();
154
155 btVector3 vel_diff;
156 vel_diff = angVelA - angVelB;
157
158 btScalar rel_vel = axis.dot(vel_diff);
159
160 // correction velocity
161 btScalar motor_relvel = m_limitSoftness * (target_velocity - m_damping * rel_vel);
162
163 if (motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON)
164 {
165 return 0.0f; //no need for applying force
166 }
167
168 // correction impulse
169 btScalar unclippedMotorImpulse = (1 + m_bounce) * motor_relvel * jacDiagABInv;
170
171 // clip correction impulse
172 btScalar clippedMotorImpulse;
173
174 ///@todo: should clip against accumulated impulse
175 if (unclippedMotorImpulse > 0.0f)
176 {
177 clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce ? maxMotorForce : unclippedMotorImpulse;
178 }
179 else
180 {
181 clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce : unclippedMotorImpulse;
182 }
183
184 // sort with accumulated impulses
185 btScalar lo = btScalar(-BT_LARGE_FLOAT);
186 btScalar hi = btScalar(BT_LARGE_FLOAT);
187
188 btScalar oldaccumImpulse = m_accumulatedImpulse;
189 btScalar sum = oldaccumImpulse + clippedMotorImpulse;
190 m_accumulatedImpulse = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
191
192 clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
193
194 btVector3 motorImp = clippedMotorImpulse * axis;
195
196 body0->applyTorqueImpulse(motorImp);
197 body1->applyTorqueImpulse(-motorImp);
198
199 return clippedMotorImpulse;
200 }
201
202 //////////////////////////// End btRotationalLimitMotor ////////////////////////////////////
203
204 //////////////////////////// btTranslationalLimitMotor ////////////////////////////////////
205
testLimitValue(int limitIndex,btScalar test_value)206 int btTranslationalLimitMotor::testLimitValue(int limitIndex, btScalar test_value)
207 {
208 btScalar loLimit = m_lowerLimit[limitIndex];
209 btScalar hiLimit = m_upperLimit[limitIndex];
210 if (loLimit > hiLimit)
211 {
212 m_currentLimit[limitIndex] = 0; //Free from violation
213 m_currentLimitError[limitIndex] = btScalar(0.f);
214 return 0;
215 }
216
217 if (test_value < loLimit)
218 {
219 m_currentLimit[limitIndex] = 2; //low limit violation
220 m_currentLimitError[limitIndex] = test_value - loLimit;
221 return 2;
222 }
223 else if (test_value > hiLimit)
224 {
225 m_currentLimit[limitIndex] = 1; //High limit violation
226 m_currentLimitError[limitIndex] = test_value - hiLimit;
227 return 1;
228 };
229
230 m_currentLimit[limitIndex] = 0; //Free from violation
231 m_currentLimitError[limitIndex] = btScalar(0.f);
232 return 0;
233 }
234
solveLinearAxis(btScalar timeStep,btScalar jacDiagABInv,btRigidBody & body1,const btVector3 & pointInA,btRigidBody & body2,const btVector3 & pointInB,int limit_index,const btVector3 & axis_normal_on_a,const btVector3 & anchorPos)235 btScalar btTranslationalLimitMotor::solveLinearAxis(
236 btScalar timeStep,
237 btScalar jacDiagABInv,
238 btRigidBody& body1, const btVector3& pointInA,
239 btRigidBody& body2, const btVector3& pointInB,
240 int limit_index,
241 const btVector3& axis_normal_on_a,
242 const btVector3& anchorPos)
243 {
244 ///find relative velocity
245 // btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition();
246 // btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition();
247 btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition();
248 btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition();
249
250 btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1);
251 btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2);
252 btVector3 vel = vel1 - vel2;
253
254 btScalar rel_vel = axis_normal_on_a.dot(vel);
255
256 /// apply displacement correction
257
258 //positional error (zeroth order error)
259 btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a);
260 btScalar lo = btScalar(-BT_LARGE_FLOAT);
261 btScalar hi = btScalar(BT_LARGE_FLOAT);
262
263 btScalar minLimit = m_lowerLimit[limit_index];
264 btScalar maxLimit = m_upperLimit[limit_index];
265
266 //handle the limits
267 if (minLimit < maxLimit)
268 {
269 {
270 if (depth > maxLimit)
271 {
272 depth -= maxLimit;
273 lo = btScalar(0.);
274 }
275 else
276 {
277 if (depth < minLimit)
278 {
279 depth -= minLimit;
280 hi = btScalar(0.);
281 }
282 else
283 {
284 return 0.0f;
285 }
286 }
287 }
288 }
289
290 btScalar normalImpulse = m_limitSoftness * (m_restitution * depth / timeStep - m_damping * rel_vel) * jacDiagABInv;
291
292 btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index];
293 btScalar sum = oldNormalImpulse + normalImpulse;
294 m_accumulatedImpulse[limit_index] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
295 normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
296
297 btVector3 impulse_vector = axis_normal_on_a * normalImpulse;
298 body1.applyImpulse(impulse_vector, rel_pos1);
299 body2.applyImpulse(-impulse_vector, rel_pos2);
300
301 return normalImpulse;
302 }
303
304 //////////////////////////// btTranslationalLimitMotor ////////////////////////////////////
305
calculateAngleInfo()306 void btGeneric6DofConstraint::calculateAngleInfo()
307 {
308 btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse() * m_calculatedTransformB.getBasis();
309 matrixToEulerXYZ(relative_frame, m_calculatedAxisAngleDiff);
310 // in euler angle mode we do not actually constrain the angular velocity
311 // along the axes axis[0] and axis[2] (although we do use axis[1]) :
312 //
313 // to get constrain w2-w1 along ...not
314 // ------ --------------------- ------
315 // d(angle[0])/dt = 0 ax[1] x ax[2] ax[0]
316 // d(angle[1])/dt = 0 ax[1]
317 // d(angle[2])/dt = 0 ax[0] x ax[1] ax[2]
318 //
319 // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
320 // to prove the result for angle[0], write the expression for angle[0] from
321 // GetInfo1 then take the derivative. to prove this for angle[2] it is
322 // easier to take the euler rate expression for d(angle[2])/dt with respect
323 // to the components of w and set that to 0.
324 btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0);
325 btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2);
326
327 m_calculatedAxis[1] = axis2.cross(axis0);
328 m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
329 m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
330
331 m_calculatedAxis[0].normalize();
332 m_calculatedAxis[1].normalize();
333 m_calculatedAxis[2].normalize();
334 }
335
calculateTransforms()336 void btGeneric6DofConstraint::calculateTransforms()
337 {
338 calculateTransforms(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
339 }
340
calculateTransforms(const btTransform & transA,const btTransform & transB)341 void btGeneric6DofConstraint::calculateTransforms(const btTransform& transA, const btTransform& transB)
342 {
343 m_calculatedTransformA = transA * m_frameInA;
344 m_calculatedTransformB = transB * m_frameInB;
345 calculateLinearInfo();
346 calculateAngleInfo();
347 if (m_useOffsetForConstraintFrame)
348 { // get weight factors depending on masses
349 btScalar miA = getRigidBodyA().getInvMass();
350 btScalar miB = getRigidBodyB().getInvMass();
351 m_hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
352 btScalar miS = miA + miB;
353 if (miS > btScalar(0.f))
354 {
355 m_factA = miB / miS;
356 }
357 else
358 {
359 m_factA = btScalar(0.5f);
360 }
361 m_factB = btScalar(1.0f) - m_factA;
362 }
363 }
364
buildLinearJacobian(btJacobianEntry & jacLinear,const btVector3 & normalWorld,const btVector3 & pivotAInW,const btVector3 & pivotBInW)365 void btGeneric6DofConstraint::buildLinearJacobian(
366 btJacobianEntry& jacLinear, const btVector3& normalWorld,
367 const btVector3& pivotAInW, const btVector3& pivotBInW)
368 {
369 new (&jacLinear) btJacobianEntry(
370 m_rbA.getCenterOfMassTransform().getBasis().transpose(),
371 m_rbB.getCenterOfMassTransform().getBasis().transpose(),
372 pivotAInW - m_rbA.getCenterOfMassPosition(),
373 pivotBInW - m_rbB.getCenterOfMassPosition(),
374 normalWorld,
375 m_rbA.getInvInertiaDiagLocal(),
376 m_rbA.getInvMass(),
377 m_rbB.getInvInertiaDiagLocal(),
378 m_rbB.getInvMass());
379 }
380
buildAngularJacobian(btJacobianEntry & jacAngular,const btVector3 & jointAxisW)381 void btGeneric6DofConstraint::buildAngularJacobian(
382 btJacobianEntry& jacAngular, const btVector3& jointAxisW)
383 {
384 new (&jacAngular) btJacobianEntry(jointAxisW,
385 m_rbA.getCenterOfMassTransform().getBasis().transpose(),
386 m_rbB.getCenterOfMassTransform().getBasis().transpose(),
387 m_rbA.getInvInertiaDiagLocal(),
388 m_rbB.getInvInertiaDiagLocal());
389 }
390
testAngularLimitMotor(int axis_index)391 bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index)
392 {
393 btScalar angle = m_calculatedAxisAngleDiff[axis_index];
394 angle = btAdjustAngleToLimits(angle, m_angularLimits[axis_index].m_loLimit, m_angularLimits[axis_index].m_hiLimit);
395 m_angularLimits[axis_index].m_currentPosition = angle;
396 //test limits
397 m_angularLimits[axis_index].testLimitValue(angle);
398 return m_angularLimits[axis_index].needApplyTorques();
399 }
400
buildJacobian()401 void btGeneric6DofConstraint::buildJacobian()
402 {
403 #ifndef __SPU__
404 if (m_useSolveConstraintObsolete)
405 {
406 // Clear accumulated impulses for the next simulation step
407 m_linearLimits.m_accumulatedImpulse.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
408 int i;
409 for (i = 0; i < 3; i++)
410 {
411 m_angularLimits[i].m_accumulatedImpulse = btScalar(0.);
412 }
413 //calculates transform
414 calculateTransforms(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
415
416 // const btVector3& pivotAInW = m_calculatedTransformA.getOrigin();
417 // const btVector3& pivotBInW = m_calculatedTransformB.getOrigin();
418 calcAnchorPos();
419 btVector3 pivotAInW = m_AnchorPos;
420 btVector3 pivotBInW = m_AnchorPos;
421
422 // not used here
423 // btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
424 // btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
425
426 btVector3 normalWorld;
427 //linear part
428 for (i = 0; i < 3; i++)
429 {
430 if (m_linearLimits.isLimited(i))
431 {
432 if (m_useLinearReferenceFrameA)
433 normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
434 else
435 normalWorld = m_calculatedTransformB.getBasis().getColumn(i);
436
437 buildLinearJacobian(
438 m_jacLinear[i], normalWorld,
439 pivotAInW, pivotBInW);
440 }
441 }
442
443 // angular part
444 for (i = 0; i < 3; i++)
445 {
446 //calculates error angle
447 if (testAngularLimitMotor(i))
448 {
449 normalWorld = this->getAxis(i);
450 // Create angular atom
451 buildAngularJacobian(m_jacAng[i], normalWorld);
452 }
453 }
454 }
455 #endif //__SPU__
456 }
457
getInfo1(btConstraintInfo1 * info)458 void btGeneric6DofConstraint::getInfo1(btConstraintInfo1* info)
459 {
460 if (m_useSolveConstraintObsolete)
461 {
462 info->m_numConstraintRows = 0;
463 info->nub = 0;
464 }
465 else
466 {
467 //prepare constraint
468 calculateTransforms(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
469 info->m_numConstraintRows = 0;
470 info->nub = 6;
471 int i;
472 //test linear limits
473 for (i = 0; i < 3; i++)
474 {
475 if (m_linearLimits.needApplyForce(i))
476 {
477 info->m_numConstraintRows++;
478 info->nub--;
479 }
480 }
481 //test angular limits
482 for (i = 0; i < 3; i++)
483 {
484 if (testAngularLimitMotor(i))
485 {
486 info->m_numConstraintRows++;
487 info->nub--;
488 }
489 }
490 }
491 }
492
getInfo1NonVirtual(btConstraintInfo1 * info)493 void btGeneric6DofConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
494 {
495 if (m_useSolveConstraintObsolete)
496 {
497 info->m_numConstraintRows = 0;
498 info->nub = 0;
499 }
500 else
501 {
502 //pre-allocate all 6
503 info->m_numConstraintRows = 6;
504 info->nub = 0;
505 }
506 }
507
getInfo2(btConstraintInfo2 * info)508 void btGeneric6DofConstraint::getInfo2(btConstraintInfo2* info)
509 {
510 btAssert(!m_useSolveConstraintObsolete);
511
512 const btTransform& transA = m_rbA.getCenterOfMassTransform();
513 const btTransform& transB = m_rbB.getCenterOfMassTransform();
514 const btVector3& linVelA = m_rbA.getLinearVelocity();
515 const btVector3& linVelB = m_rbB.getLinearVelocity();
516 const btVector3& angVelA = m_rbA.getAngularVelocity();
517 const btVector3& angVelB = m_rbB.getAngularVelocity();
518
519 if (m_useOffsetForConstraintFrame)
520 { // for stability better to solve angular limits first
521 int row = setAngularLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
522 setLinearLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
523 }
524 else
525 { // leave old version for compatibility
526 int row = setLinearLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
527 setAngularLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
528 }
529 }
530
getInfo2NonVirtual(btConstraintInfo2 * info,const btTransform & transA,const btTransform & transB,const btVector3 & linVelA,const btVector3 & linVelB,const btVector3 & angVelA,const btVector3 & angVelB)531 void btGeneric6DofConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB)
532 {
533 btAssert(!m_useSolveConstraintObsolete);
534 //prepare constraint
535 calculateTransforms(transA, transB);
536
537 int i;
538 for (i = 0; i < 3; i++)
539 {
540 testAngularLimitMotor(i);
541 }
542
543 if (m_useOffsetForConstraintFrame)
544 { // for stability better to solve angular limits first
545 int row = setAngularLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
546 setLinearLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
547 }
548 else
549 { // leave old version for compatibility
550 int row = setLinearLimits(info, 0, transA, transB, linVelA, linVelB, angVelA, angVelB);
551 setAngularLimits(info, row, transA, transB, linVelA, linVelB, angVelA, angVelB);
552 }
553 }
554
setLinearLimits(btConstraintInfo2 * info,int row,const btTransform & transA,const btTransform & transB,const btVector3 & linVelA,const btVector3 & linVelB,const btVector3 & angVelA,const btVector3 & angVelB)555 int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info, int row, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB)
556 {
557 // int row = 0;
558 //solve linear limits
559 btRotationalLimitMotor limot;
560 for (int i = 0; i < 3; i++)
561 {
562 if (m_linearLimits.needApplyForce(i))
563 { // re-use rotational motor code
564 limot.m_bounce = btScalar(0.f);
565 limot.m_currentLimit = m_linearLimits.m_currentLimit[i];
566 limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[i];
567 limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i];
568 limot.m_damping = m_linearLimits.m_damping;
569 limot.m_enableMotor = m_linearLimits.m_enableMotor[i];
570 limot.m_hiLimit = m_linearLimits.m_upperLimit[i];
571 limot.m_limitSoftness = m_linearLimits.m_limitSoftness;
572 limot.m_loLimit = m_linearLimits.m_lowerLimit[i];
573 limot.m_maxLimitForce = btScalar(0.f);
574 limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i];
575 limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i];
576 btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i);
577 int flags = m_flags >> (i * BT_6DOF_FLAGS_AXIS_SHIFT);
578 limot.m_normalCFM = (flags & BT_6DOF_FLAGS_CFM_NORM) ? m_linearLimits.m_normalCFM[i] : info->cfm[0];
579 limot.m_stopCFM = (flags & BT_6DOF_FLAGS_CFM_STOP) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];
580 limot.m_stopERP = (flags & BT_6DOF_FLAGS_ERP_STOP) ? m_linearLimits.m_stopERP[i] : info->erp;
581 if (m_useOffsetForConstraintFrame)
582 {
583 int indx1 = (i + 1) % 3;
584 int indx2 = (i + 2) % 3;
585 int rotAllowed = 1; // rotations around orthos to current axis
586 if (m_angularLimits[indx1].m_currentLimit && m_angularLimits[indx2].m_currentLimit)
587 {
588 rotAllowed = 0;
589 }
590 row += get_limit_motor_info2(&limot, transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 0, rotAllowed);
591 }
592 else
593 {
594 row += get_limit_motor_info2(&limot, transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 0);
595 }
596 }
597 }
598 return row;
599 }
600
setAngularLimits(btConstraintInfo2 * info,int row_offset,const btTransform & transA,const btTransform & transB,const btVector3 & linVelA,const btVector3 & linVelB,const btVector3 & angVelA,const btVector3 & angVelB)601 int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2* info, int row_offset, const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB)
602 {
603 btGeneric6DofConstraint* d6constraint = this;
604 int row = row_offset;
605 //solve angular limits
606 for (int i = 0; i < 3; i++)
607 {
608 if (d6constraint->getRotationalLimitMotor(i)->needApplyTorques())
609 {
610 btVector3 axis = d6constraint->getAxis(i);
611 int flags = m_flags >> ((i + 3) * BT_6DOF_FLAGS_AXIS_SHIFT);
612 if (!(flags & BT_6DOF_FLAGS_CFM_NORM))
613 {
614 m_angularLimits[i].m_normalCFM = info->cfm[0];
615 }
616 if (!(flags & BT_6DOF_FLAGS_CFM_STOP))
617 {
618 m_angularLimits[i].m_stopCFM = info->cfm[0];
619 }
620 if (!(flags & BT_6DOF_FLAGS_ERP_STOP))
621 {
622 m_angularLimits[i].m_stopERP = info->erp;
623 }
624 row += get_limit_motor_info2(d6constraint->getRotationalLimitMotor(i),
625 transA, transB, linVelA, linVelB, angVelA, angVelB, info, row, axis, 1);
626 }
627 }
628
629 return row;
630 }
631
updateRHS(btScalar timeStep)632 void btGeneric6DofConstraint::updateRHS(btScalar timeStep)
633 {
634 (void)timeStep;
635 }
636
setFrames(const btTransform & frameA,const btTransform & frameB)637 void btGeneric6DofConstraint::setFrames(const btTransform& frameA, const btTransform& frameB)
638 {
639 m_frameInA = frameA;
640 m_frameInB = frameB;
641 buildJacobian();
642 calculateTransforms();
643 }
644
getAxis(int axis_index) const645 btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const
646 {
647 return m_calculatedAxis[axis_index];
648 }
649
getRelativePivotPosition(int axisIndex) const650 btScalar btGeneric6DofConstraint::getRelativePivotPosition(int axisIndex) const
651 {
652 return m_calculatedLinearDiff[axisIndex];
653 }
654
getAngle(int axisIndex) const655 btScalar btGeneric6DofConstraint::getAngle(int axisIndex) const
656 {
657 return m_calculatedAxisAngleDiff[axisIndex];
658 }
659
calcAnchorPos(void)660 void btGeneric6DofConstraint::calcAnchorPos(void)
661 {
662 btScalar imA = m_rbA.getInvMass();
663 btScalar imB = m_rbB.getInvMass();
664 btScalar weight;
665 if (imB == btScalar(0.0))
666 {
667 weight = btScalar(1.0);
668 }
669 else
670 {
671 weight = imA / (imA + imB);
672 }
673 const btVector3& pA = m_calculatedTransformA.getOrigin();
674 const btVector3& pB = m_calculatedTransformB.getOrigin();
675 m_AnchorPos = pA * weight + pB * (btScalar(1.0) - weight);
676 return;
677 }
678
calculateLinearInfo()679 void btGeneric6DofConstraint::calculateLinearInfo()
680 {
681 m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin();
682 m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff;
683 for (int i = 0; i < 3; i++)
684 {
685 m_linearLimits.m_currentLinearDiff[i] = m_calculatedLinearDiff[i];
686 m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]);
687 }
688 }
689
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)690 int btGeneric6DofConstraint::get_limit_motor_info2(
691 btRotationalLimitMotor* limot,
692 const btTransform& transA, const btTransform& transB, const btVector3& linVelA, const btVector3& linVelB, const btVector3& angVelA, const btVector3& angVelB,
693 btConstraintInfo2* info, int row, btVector3& ax1, int rotational, int rotAllowed)
694 {
695 int srow = row * info->rowskip;
696 bool powered = limot->m_enableMotor;
697 int limit = limot->m_currentLimit;
698 if (powered || limit)
699 { // if the joint is powered, or has joint limits, add in the extra row
700 btScalar* J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
701 btScalar* J2 = rotational ? info->m_J2angularAxis : info->m_J2linearAxis;
702 J1[srow + 0] = ax1[0];
703 J1[srow + 1] = ax1[1];
704 J1[srow + 2] = ax1[2];
705
706 J2[srow + 0] = -ax1[0];
707 J2[srow + 1] = -ax1[1];
708 J2[srow + 2] = -ax1[2];
709
710 if ((!rotational))
711 {
712 if (m_useOffsetForConstraintFrame)
713 {
714 btVector3 tmpA, tmpB, relA, relB;
715 // get vector from bodyB to frameB in WCS
716 relB = m_calculatedTransformB.getOrigin() - transB.getOrigin();
717 // get its projection to constraint axis
718 btVector3 projB = ax1 * relB.dot(ax1);
719 // get vector directed from bodyB to constraint axis (and orthogonal to it)
720 btVector3 orthoB = relB - projB;
721 // same for bodyA
722 relA = m_calculatedTransformA.getOrigin() - transA.getOrigin();
723 btVector3 projA = ax1 * relA.dot(ax1);
724 btVector3 orthoA = relA - projA;
725 // get desired offset between frames A and B along constraint axis
726 btScalar desiredOffs = limot->m_currentPosition - limot->m_currentLimitError;
727 // desired vector from projection of center of bodyA to projection of center of bodyB to constraint axis
728 btVector3 totalDist = projA + ax1 * desiredOffs - projB;
729 // get offset vectors relA and relB
730 relA = orthoA + totalDist * m_factA;
731 relB = orthoB - totalDist * m_factB;
732 tmpA = relA.cross(ax1);
733 tmpB = relB.cross(ax1);
734 if (m_hasStaticBody && (!rotAllowed))
735 {
736 tmpA *= m_factA;
737 tmpB *= m_factB;
738 }
739 int i;
740 for (i = 0; i < 3; i++) info->m_J1angularAxis[srow + i] = tmpA[i];
741 for (i = 0; i < 3; i++) info->m_J2angularAxis[srow + i] = -tmpB[i];
742 }
743 else
744 {
745 btVector3 ltd; // Linear Torque Decoupling vector
746 btVector3 c = m_calculatedTransformB.getOrigin() - transA.getOrigin();
747 ltd = c.cross(ax1);
748 info->m_J1angularAxis[srow + 0] = ltd[0];
749 info->m_J1angularAxis[srow + 1] = ltd[1];
750 info->m_J1angularAxis[srow + 2] = ltd[2];
751
752 c = m_calculatedTransformB.getOrigin() - transB.getOrigin();
753 ltd = -c.cross(ax1);
754 info->m_J2angularAxis[srow + 0] = ltd[0];
755 info->m_J2angularAxis[srow + 1] = ltd[1];
756 info->m_J2angularAxis[srow + 2] = ltd[2];
757 }
758 }
759 // if we're limited low and high simultaneously, the joint motor is
760 // ineffective
761 if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = false;
762 info->m_constraintError[srow] = btScalar(0.f);
763 if (powered)
764 {
765 info->cfm[srow] = limot->m_normalCFM;
766 if (!limit)
767 {
768 btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;
769
770 btScalar mot_fact = getMotorFactor(limot->m_currentPosition,
771 limot->m_loLimit,
772 limot->m_hiLimit,
773 tag_vel,
774 info->fps * limot->m_stopERP);
775 info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity;
776 info->m_lowerLimit[srow] = -limot->m_maxMotorForce / info->fps;
777 info->m_upperLimit[srow] = limot->m_maxMotorForce / info->fps;
778 }
779 }
780 if (limit)
781 {
782 btScalar k = info->fps * limot->m_stopERP;
783 if (!rotational)
784 {
785 info->m_constraintError[srow] += k * limot->m_currentLimitError;
786 }
787 else
788 {
789 info->m_constraintError[srow] += -k * limot->m_currentLimitError;
790 }
791 info->cfm[srow] = limot->m_stopCFM;
792 if (limot->m_loLimit == limot->m_hiLimit)
793 { // limited low and high simultaneously
794 info->m_lowerLimit[srow] = -SIMD_INFINITY;
795 info->m_upperLimit[srow] = SIMD_INFINITY;
796 }
797 else
798 {
799 if (limit == 1)
800 {
801 info->m_lowerLimit[srow] = 0;
802 info->m_upperLimit[srow] = SIMD_INFINITY;
803 }
804 else
805 {
806 info->m_lowerLimit[srow] = -SIMD_INFINITY;
807 info->m_upperLimit[srow] = 0;
808 }
809 // deal with bounce
810 if (limot->m_bounce > 0)
811 {
812 // calculate joint velocity
813 btScalar vel;
814 if (rotational)
815 {
816 vel = angVelA.dot(ax1);
817 //make sure that if no body -> angVelB == zero vec
818 // if (body1)
819 vel -= angVelB.dot(ax1);
820 }
821 else
822 {
823 vel = linVelA.dot(ax1);
824 //make sure that if no body -> angVelB == zero vec
825 // if (body1)
826 vel -= linVelB.dot(ax1);
827 }
828 // only apply bounce if the velocity is incoming, and if the
829 // resulting c[] exceeds what we already have.
830 if (limit == 1)
831 {
832 if (vel < 0)
833 {
834 btScalar newc = -limot->m_bounce * vel;
835 if (newc > info->m_constraintError[srow])
836 info->m_constraintError[srow] = newc;
837 }
838 }
839 else
840 {
841 if (vel > 0)
842 {
843 btScalar newc = -limot->m_bounce * vel;
844 if (newc < info->m_constraintError[srow])
845 info->m_constraintError[srow] = newc;
846 }
847 }
848 }
849 }
850 }
851 return 1;
852 }
853 else
854 return 0;
855 }
856
857 ///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
858 ///If no axis is provided, it uses the default axis for this constraint.
setParam(int num,btScalar value,int axis)859 void btGeneric6DofConstraint::setParam(int num, btScalar value, int axis)
860 {
861 if ((axis >= 0) && (axis < 3))
862 {
863 switch (num)
864 {
865 case BT_CONSTRAINT_STOP_ERP:
866 m_linearLimits.m_stopERP[axis] = value;
867 m_flags |= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
868 break;
869 case BT_CONSTRAINT_STOP_CFM:
870 m_linearLimits.m_stopCFM[axis] = value;
871 m_flags |= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
872 break;
873 case BT_CONSTRAINT_CFM:
874 m_linearLimits.m_normalCFM[axis] = value;
875 m_flags |= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
876 break;
877 default:
878 btAssertConstrParams(0);
879 }
880 }
881 else if ((axis >= 3) && (axis < 6))
882 {
883 switch (num)
884 {
885 case BT_CONSTRAINT_STOP_ERP:
886 m_angularLimits[axis - 3].m_stopERP = value;
887 m_flags |= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
888 break;
889 case BT_CONSTRAINT_STOP_CFM:
890 m_angularLimits[axis - 3].m_stopCFM = value;
891 m_flags |= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
892 break;
893 case BT_CONSTRAINT_CFM:
894 m_angularLimits[axis - 3].m_normalCFM = value;
895 m_flags |= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);
896 break;
897 default:
898 btAssertConstrParams(0);
899 }
900 }
901 else
902 {
903 btAssertConstrParams(0);
904 }
905 }
906
907 ///return the local value of parameter
getParam(int num,int axis) const908 btScalar btGeneric6DofConstraint::getParam(int num, int axis) const
909 {
910 btScalar retVal = 0;
911 if ((axis >= 0) && (axis < 3))
912 {
913 switch (num)
914 {
915 case BT_CONSTRAINT_STOP_ERP:
916 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
917 retVal = m_linearLimits.m_stopERP[axis];
918 break;
919 case BT_CONSTRAINT_STOP_CFM:
920 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
921 retVal = m_linearLimits.m_stopCFM[axis];
922 break;
923 case BT_CONSTRAINT_CFM:
924 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
925 retVal = m_linearLimits.m_normalCFM[axis];
926 break;
927 default:
928 btAssertConstrParams(0);
929 }
930 }
931 else if ((axis >= 3) && (axis < 6))
932 {
933 switch (num)
934 {
935 case BT_CONSTRAINT_STOP_ERP:
936 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
937 retVal = m_angularLimits[axis - 3].m_stopERP;
938 break;
939 case BT_CONSTRAINT_STOP_CFM:
940 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
941 retVal = m_angularLimits[axis - 3].m_stopCFM;
942 break;
943 case BT_CONSTRAINT_CFM:
944 btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));
945 retVal = m_angularLimits[axis - 3].m_normalCFM;
946 break;
947 default:
948 btAssertConstrParams(0);
949 }
950 }
951 else
952 {
953 btAssertConstrParams(0);
954 }
955 return retVal;
956 }
957
setAxis(const btVector3 & axis1,const btVector3 & axis2)958 void btGeneric6DofConstraint::setAxis(const btVector3& axis1, const btVector3& axis2)
959 {
960 btVector3 zAxis = axis1.normalized();
961 btVector3 yAxis = axis2.normalized();
962 btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system
963
964 btTransform frameInW;
965 frameInW.setIdentity();
966 frameInW.getBasis().setValue(xAxis[0], yAxis[0], zAxis[0],
967 xAxis[1], yAxis[1], zAxis[1],
968 xAxis[2], yAxis[2], zAxis[2]);
969
970 // now get constraint frame in local coordinate systems
971 m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW;
972 m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW;
973
974 calculateTransforms();
975 }
976