Bullet Collision Detection & Physics Library: btGeneric6DofConstraint.cpp Source File

Bullet Collision Detection & Physics Library

- Main Page
- Namespaces
- Classes
- Files

- File List
- File Members

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