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

Bullet Collision Detection & Physics Library

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 #define D6_USE_OBSOLETE_METHOD false
29 #define D6_USE_FRAME_OFFSET true
30
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
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 {
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);
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
61 bool 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
99
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
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
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
203
205
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
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 {
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
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
305
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
336 void btGeneric6DofConstraint::calculateTransforms()
337 {
338 calculateTransforms(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
339 }
340
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
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
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
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
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
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
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
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
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
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
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
632 void btGeneric6DofConstraint::updateRHS(btScalar timeStep)
633 {
634 (void)timeStep;
635 }
636
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
645 btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const
646 {
647 return m_calculatedAxis[axis_index];
648 }
649
650 btScalar btGeneric6DofConstraint::getRelativePivotPosition(int axisIndex) const
651 {
652 return m_calculatedLinearDiff[axisIndex];
653 }
654
655 btScalar btGeneric6DofConstraint::getAngle(int axisIndex) const
656 {
657 return m_calculatedAxisAngleDiff[axisIndex];
658 }
659
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
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
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
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
908 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
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 }
btGetMatrixElem
btScalar btGetMatrixElem(const btMatrix3x3 &mat, int index)
Definition: btGeneric6DofConstraint.cpp:53
D6_USE_FRAME_OFFSET
#define D6_USE_FRAME_OFFSET
Definition: btGeneric6DofConstraint.cpp:29
D6_USE_OBSOLETE_METHOD
#define D6_USE_OBSOLETE_METHOD
Definition: btGeneric6DofConstraint.cpp:28
matrixToEulerXYZ
bool matrixToEulerXYZ(const btMatrix3x3 &mat, btVector3 &xyz)
MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3....
Definition: btGeneric6DofConstraint.cpp:62
btGeneric6DofConstraint.h
BT_6DOF_FLAGS_ERP_STOP
@ BT_6DOF_FLAGS_ERP_STOP
Definition: btGeneric6DofConstraint.h:226
BT_6DOF_FLAGS_CFM_STOP
@ BT_6DOF_FLAGS_CFM_STOP
Definition: btGeneric6DofConstraint.h:225
BT_6DOF_FLAGS_CFM_NORM
@ BT_6DOF_FLAGS_CFM_NORM
Definition: btGeneric6DofConstraint.h:224
BT_6DOF_FLAGS_AXIS_SHIFT
#define BT_6DOF_FLAGS_AXIS_SHIFT
Definition: btGeneric6DofConstraint.h:228
dot
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:888
btRigidBody.h
SIMD_PI
#define SIMD_PI
Definition: btScalar.h:526
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
BT_LARGE_FLOAT
#define BT_LARGE_FLOAT
Definition: btScalar.h:316
btAtan2
btScalar btAtan2(btScalar x, btScalar y)
Definition: btScalar.h:518
SIMD_INFINITY
#define SIMD_INFINITY
Definition: btScalar.h:544
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:543
btAsin
btScalar btAsin(btScalar x)
Definition: btScalar.h:509
SIMD_HALF_PI
#define SIMD_HALF_PI
Definition: btScalar.h:528
SIMD_2_PI
#define SIMD_2_PI
Definition: btScalar.h:527
btAssert
#define btAssert(x)
Definition: btScalar.h:153
sum
static T sum(const btAlignedObjectArray< T > &items)
Definition: btSoftBodyHelpers.cpp:94
btTransformUtil.h
btAssertConstrParams
#define btAssertConstrParams(_par)
Definition: btTypedConstraint.h:58
btAdjustAngleToLimits
btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScalar angleLowerLimitInRadians, btScalar angleUpperLimitInRadians)
Definition: btTypedConstraint.h:331
BT_CONSTRAINT_CFM
@ BT_CONSTRAINT_CFM
Definition: btTypedConstraint.h:53
BT_CONSTRAINT_STOP_CFM
@ BT_CONSTRAINT_STOP_CFM
Definition: btTypedConstraint.h:54
BT_CONSTRAINT_STOP_ERP
@ BT_CONSTRAINT_STOP_ERP
Definition: btTypedConstraint.h:52
D6_CONSTRAINT_TYPE
@ D6_CONSTRAINT_TYPE
Definition: btTypedConstraint.h:39
btGeneric6DofConstraint
btGeneric6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis locatio...
Definition: btGeneric6DofConstraint.h:268
btGeneric6DofConstraint::m_hasStaticBody
bool m_hasStaticBody
Definition: btGeneric6DofConstraint.h:303
btGeneric6DofConstraint::m_frameInA
btTransform m_frameInA
relative_frames
Definition: btGeneric6DofConstraint.h:272
btGeneric6DofConstraint::getRotationalLimitMotor
btRotationalLimitMotor * getRotationalLimitMotor(int index)
Retrieves the angular limit informacion.
Definition: btGeneric6DofConstraint.h:478
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:531
btGeneric6DofConstraint::calcAnchorPos
virtual void calcAnchorPos(void)
Definition: btGeneric6DofConstraint.cpp:660
btGeneric6DofConstraint::getInfo2
virtual void getInfo2(btConstraintInfo2 *info)
internal method used by the constraint solver, don't use them directly
Definition: btGeneric6DofConstraint.cpp:508
btGeneric6DofConstraint::buildLinearJacobian
void buildLinearJacobian(btJacobianEntry &jacLinear, const btVector3 &normalWorld, const btVector3 &pivotAInW, const btVector3 &pivotBInW)
Definition: btGeneric6DofConstraint.cpp:365
btGeneric6DofConstraint::m_calculatedTransformA
btTransform m_calculatedTransformA
Definition: btGeneric6DofConstraint.h:296
btGeneric6DofConstraint::m_calculatedLinearDiff
btVector3 m_calculatedLinearDiff
Definition: btGeneric6DofConstraint.h:300
btGeneric6DofConstraint::m_jacLinear
btJacobianEntry m_jacLinear[3]
Jacobians.
Definition: btGeneric6DofConstraint.h:278
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:555
btGeneric6DofConstraint::calculateLinearInfo
void calculateLinearInfo()
Definition: btGeneric6DofConstraint.cpp:679
btGeneric6DofConstraint::m_calculatedAxis
btVector3 m_calculatedAxis[3]
Definition: btGeneric6DofConstraint.h:299
btGeneric6DofConstraint::btGeneric6DofConstraint
btGeneric6DofConstraint(btRigidBody &rbA, btRigidBody &rbB, const btTransform &frameInA, const btTransform &frameInB, bool useLinearReferenceFrameA)
Definition: btGeneric6DofConstraint.cpp:31
btGeneric6DofConstraint::m_flags
int m_flags
Definition: btGeneric6DofConstraint.h:310
btGeneric6DofConstraint::m_jacAng
btJacobianEntry m_jacAng[3]
3 orthogonal angular constraints
Definition: btGeneric6DofConstraint.h:279
btGeneric6DofConstraint::m_factB
btScalar m_factB
Definition: btGeneric6DofConstraint.h:302
btGeneric6DofConstraint::getRelativePivotPosition
btScalar getRelativePivotPosition(int axis_index) const
Get the relative position of the constraint pivot.
Definition: btGeneric6DofConstraint.cpp:650
btGeneric6DofConstraint::getInfo1
virtual void getInfo1(btConstraintInfo1 *info)
internal method used by the constraint solver, don't use them directly
Definition: btGeneric6DofConstraint.cpp:458
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:859
btGeneric6DofConstraint::m_frameInB
btTransform m_frameInB
the constraint space w.r.t body B
Definition: btGeneric6DofConstraint.h:273
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:690
btGeneric6DofConstraint::updateRHS
void updateRHS(btScalar timeStep)
Definition: btGeneric6DofConstraint.cpp:632
btGeneric6DofConstraint::buildJacobian
virtual void buildJacobian()
performs Jacobian calculation, and also calculates angle differences and axis
Definition: btGeneric6DofConstraint.cpp:401
btGeneric6DofConstraint::m_useOffsetForConstraintFrame
bool m_useOffsetForConstraintFrame
Definition: btGeneric6DofConstraint.h:308
btGeneric6DofConstraint::calculateTransforms
void calculateTransforms()
Definition: btGeneric6DofConstraint.cpp:336
btGeneric6DofConstraint::buildAngularJacobian
void buildAngularJacobian(btJacobianEntry &jacAngular, const btVector3 &jointAxisW)
Definition: btGeneric6DofConstraint.cpp:381
btGeneric6DofConstraint::m_useLinearReferenceFrameA
bool m_useLinearReferenceFrameA
Definition: btGeneric6DofConstraint.h:307
btGeneric6DofConstraint::m_AnchorPos
btVector3 m_AnchorPos
Definition: btGeneric6DofConstraint.h:305
btGeneric6DofConstraint::calculateAngleInfo
void calculateAngleInfo()
calcs the euler angles between the two bodies.
Definition: btGeneric6DofConstraint.cpp:306
btGeneric6DofConstraint::getAxis
btVector3 getAxis(int axis_index) const
Get the rotation axis in global coordinates.
Definition: btGeneric6DofConstraint.cpp:645
btGeneric6DofConstraint::m_useSolveConstraintObsolete
bool m_useSolveConstraintObsolete
for backwards compatibility during the transition to 'getInfo/getInfo2'
Definition: btGeneric6DofConstraint.h:341
btGeneric6DofConstraint::testAngularLimitMotor
bool testAngularLimitMotor(int axis_index)
Test angular limit.
Definition: btGeneric6DofConstraint.cpp:391
btGeneric6DofConstraint::getAngle
btScalar getAngle(int axis_index) const
Get the relative Euler angle.
Definition: btGeneric6DofConstraint.cpp:655
btGeneric6DofConstraint::m_factA
btScalar m_factA
Definition: btGeneric6DofConstraint.h:301
btGeneric6DofConstraint::setAxis
void setAxis(const btVector3 &axis1, const btVector3 &axis2)
Definition: btGeneric6DofConstraint.cpp:958
btGeneric6DofConstraint::getParam
virtual btScalar getParam(int num, int axis=-1) const
return the local value of parameter
Definition: btGeneric6DofConstraint.cpp:908
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:601
btGeneric6DofConstraint::m_linearLimits
btTranslationalLimitMotor m_linearLimits
Linear_Limit_parameters.
Definition: btGeneric6DofConstraint.h:284
btGeneric6DofConstraint::m_angularLimits
btRotationalLimitMotor m_angularLimits[3]
hinge_parameters
Definition: btGeneric6DofConstraint.h:289
btGeneric6DofConstraint::m_calculatedAxisAngleDiff
btVector3 m_calculatedAxisAngleDiff
Definition: btGeneric6DofConstraint.h:298
btGeneric6DofConstraint::setFrames
void setFrames(const btTransform &frameA, const btTransform &frameB)
Definition: btGeneric6DofConstraint.cpp:637
btGeneric6DofConstraint::m_calculatedTransformB
btTransform m_calculatedTransformB
Definition: btGeneric6DofConstraint.h:297
btGeneric6DofConstraint::getInfo1NonVirtual
void getInfo1NonVirtual(btConstraintInfo1 *info)
Definition: btGeneric6DofConstraint.cpp:493
btJacobianEntry
Jacobian entry is an abstraction that allows to describe constraints it can be used in combination wi...
Definition: btJacobianEntry.h:31
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:50
btMatrix3x3::inverse
btMatrix3x3 inverse() const
Return the inverse of the matrix.
Definition: btMatrix3x3.h:1093
btMatrix3x3::transpose
btMatrix3x3 transpose() const
Return the transpose of the matrix.
Definition: btMatrix3x3.h:1049
btMatrix3x3::getColumn
btVector3 getColumn(int i) const
Get a column of the matrix as a vector.
Definition: btMatrix3x3.h:142
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:204
btRigidBody
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:60
btRigidBody::applyTorqueImpulse
void applyTorqueImpulse(const btVector3 &torque)
Definition: btRigidBody.h:327
btRigidBody::getAngularVelocity
const btVector3 & getAngularVelocity() const
Definition: btRigidBody.h:437
btRigidBody::getVelocityInLocalPoint
btVector3 getVelocityInLocalPoint(const btVector3 &rel_pos) const
Definition: btRigidBody.h:460
btRigidBody::getInvMass
btScalar getInvMass() const
Definition: btRigidBody.h:263
btRigidBody::applyImpulse
void applyImpulse(const btVector3 &impulse, const btVector3 &rel_pos)
Definition: btRigidBody.h:335
btRigidBody::getCenterOfMassTransform
const btTransform & getCenterOfMassTransform() const
Definition: btRigidBody.h:429
btRigidBody::getLinearVelocity
const btVector3 & getLinearVelocity() const
Definition: btRigidBody.h:433
btRigidBody::getCenterOfMassPosition
const btVector3 & getCenterOfMassPosition() const
Definition: btRigidBody.h:423
btRigidBody::getInvInertiaDiagLocal
const btVector3 & getInvInertiaDiagLocal() const
Definition: btRigidBody.h:289
btRotationalLimitMotor
Rotation Limit structure for generic joints.
Definition: btGeneric6DofConstraint.h:45
btRotationalLimitMotor::m_currentPosition
btScalar m_currentPosition
How much is violated this limit.
Definition: btGeneric6DofConstraint.h:67
btRotationalLimitMotor::m_targetVelocity
btScalar m_targetVelocity
target motor velocity
Definition: btGeneric6DofConstraint.h:51
btRotationalLimitMotor::m_hiLimit
btScalar m_hiLimit
joint limit
Definition: btGeneric6DofConstraint.h:50
btRotationalLimitMotor::m_normalCFM
btScalar m_normalCFM
Relaxation factor.
Definition: btGeneric6DofConstraint.h:56
btRotationalLimitMotor::m_maxMotorForce
btScalar m_maxMotorForce
max force on motor
Definition: btGeneric6DofConstraint.h:52
btRotationalLimitMotor::m_damping
btScalar m_damping
Damping.
Definition: btGeneric6DofConstraint.h:54
btRotationalLimitMotor::m_bounce
btScalar m_bounce
restitution factor
Definition: btGeneric6DofConstraint.h:59
btRotationalLimitMotor::m_loLimit
btScalar m_loLimit
limit_parameters
Definition: btGeneric6DofConstraint.h:49
btRotationalLimitMotor::m_currentLimitError
btScalar m_currentLimitError
temp_variables
Definition: btGeneric6DofConstraint.h:66
btRotationalLimitMotor::m_limitSoftness
btScalar m_limitSoftness
Definition: btGeneric6DofConstraint.h:55
btRotationalLimitMotor::m_stopERP
btScalar m_stopERP
Error tolerance factor when joint is at limit.
Definition: btGeneric6DofConstraint.h:57
btRotationalLimitMotor::m_enableMotor
bool m_enableMotor
Definition: btGeneric6DofConstraint.h:60
btRotationalLimitMotor::needApplyTorques
bool needApplyTorques() const
Need apply correction.
Definition: btGeneric6DofConstraint.h:115
btRotationalLimitMotor::m_accumulatedImpulse
btScalar m_accumulatedImpulse
Definition: btGeneric6DofConstraint.h:69
btRotationalLimitMotor::testLimitValue
int testLimitValue(btScalar test_value)
calculates error
Definition: btGeneric6DofConstraint.cpp:100
btRotationalLimitMotor::m_maxLimitForce
btScalar m_maxLimitForce
max force on limit
Definition: btGeneric6DofConstraint.h:53
btRotationalLimitMotor::m_currentLimit
int m_currentLimit
current value of angle
Definition: btGeneric6DofConstraint.h:68
btRotationalLimitMotor::solveAngularLimits
btScalar solveAngularLimits(btScalar timeStep, btVector3 &axis, btScalar jacDiagABInv, btRigidBody *body0, btRigidBody *body1)
apply the correction impulses for two bodies
Definition: btGeneric6DofConstraint.cpp:132
btRotationalLimitMotor::m_stopCFM
btScalar m_stopCFM
Constraint force mixing factor when joint is at limit.
Definition: btGeneric6DofConstraint.h:58
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
btTransform::inverse
btTransform inverse() const
Return the inverse of this transform.
Definition: btTransform.h:182
btTransform::getBasis
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:108
btTransform::getOrigin
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:113
btTransform::setIdentity
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:166
btTranslationalLimitMotor::m_stopERP
btVector3 m_stopERP
Error tolerance factor when joint is at limit.
Definition: btGeneric6DofConstraint.h:143
btTranslationalLimitMotor::m_currentLimit
int m_currentLimit[3]
Current relative offset of constraint frames.
Definition: btGeneric6DofConstraint.h:151
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:235
btTranslationalLimitMotor::m_stopCFM
btVector3 m_stopCFM
Constraint force mixing factor when joint is at limit.
Definition: btGeneric6DofConstraint.h:144
btTranslationalLimitMotor::m_maxMotorForce
btVector3 m_maxMotorForce
max force on motor
Definition: btGeneric6DofConstraint.h:148
btTranslationalLimitMotor::m_accumulatedImpulse
btVector3 m_accumulatedImpulse
Definition: btGeneric6DofConstraint.h:136
btTranslationalLimitMotor::testLimitValue
int testLimitValue(int limitIndex, btScalar test_value)
Definition: btGeneric6DofConstraint.cpp:206
btTranslationalLimitMotor::m_currentLinearDiff
btVector3 m_currentLinearDiff
How much is violated this limit.
Definition: btGeneric6DofConstraint.h:150
btTranslationalLimitMotor::m_normalCFM
btVector3 m_normalCFM
Bounce parameter for linear limit.
Definition: btGeneric6DofConstraint.h:142
btTranslationalLimitMotor::needApplyForce
bool needApplyForce(int limitIndex) const
Definition: btGeneric6DofConstraint.h:205
btTranslationalLimitMotor::m_currentLimitError
btVector3 m_currentLimitError
Definition: btGeneric6DofConstraint.h:149
btTranslationalLimitMotor::m_limitSoftness
btScalar m_limitSoftness
Linear_Limit_parameters.
Definition: btGeneric6DofConstraint.h:139
btTranslationalLimitMotor::m_targetVelocity
btVector3 m_targetVelocity
target motor velocity
Definition: btGeneric6DofConstraint.h:147
btTranslationalLimitMotor::m_lowerLimit
btVector3 m_lowerLimit
the constraint lower limits
Definition: btGeneric6DofConstraint.h:134
btTranslationalLimitMotor::isLimited
bool isLimited(int limitIndex) const
Test limit.
Definition: btGeneric6DofConstraint.h:201
btTranslationalLimitMotor::m_restitution
btScalar m_restitution
Definition: btGeneric6DofConstraint.h:141
btTranslationalLimitMotor::m_damping
btScalar m_damping
Damping for linear limit.
Definition: btGeneric6DofConstraint.h:140
btTranslationalLimitMotor::m_upperLimit
btVector3 m_upperLimit
the constraint upper limits
Definition: btGeneric6DofConstraint.h:135
btTranslationalLimitMotor::m_enableMotor
bool m_enableMotor[3]
Definition: btGeneric6DofConstraint.h:146
btTypedConstraint
TypedConstraint is the baseclass for Bullet constraints and vehicles.
Definition: btTypedConstraint.h:76
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:54
btTypedConstraint::getRigidBodyA
const btRigidBody & getRigidBodyA() const
Definition: btTypedConstraint.h:214
btTypedConstraint::m_rbA
btRigidBody & m_rbA
Definition: btTypedConstraint.h:97
btTypedConstraint::getRigidBodyB
const btRigidBody & getRigidBodyB() const
Definition: btTypedConstraint.h:218
btTypedConstraint::m_rbB
btRigidBody & m_rbB
Definition: btTypedConstraint.h:98
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
btVector3::normalize
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:303
btVector3::cross
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:380
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:229
btVector3::setValue
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:640
btVector3::normalized
btVector3 normalized() const
Return a normalized version of this vector.
Definition: btVector3.h:949
btTypedConstraint::btConstraintInfo1
Definition: btTypedConstraint.h:114
btTypedConstraint::btConstraintInfo1::m_numConstraintRows
int m_numConstraintRows
Definition: btTypedConstraint.h:115
btTypedConstraint::btConstraintInfo1::nub
int nub
Definition: btTypedConstraint.h:115
btTypedConstraint::btConstraintInfo2
Definition: btTypedConstraint.h:121
btTypedConstraint::btConstraintInfo2::m_J2angularAxis
btScalar * m_J2angularAxis
Definition: btTypedConstraint.h:130
btTypedConstraint::btConstraintInfo2::m_J1linearAxis
btScalar * m_J1linearAxis
Definition: btTypedConstraint.h:130
btTypedConstraint::btConstraintInfo2::erp
btScalar erp
Definition: btTypedConstraint.h:124
btTypedConstraint::btConstraintInfo2::m_upperLimit
btScalar * m_upperLimit
Definition: btTypedConstraint.h:141
btTypedConstraint::btConstraintInfo2::m_lowerLimit
btScalar * m_lowerLimit
Definition: btTypedConstraint.h:141
btTypedConstraint::btConstraintInfo2::cfm
btScalar * cfm
Definition: btTypedConstraint.h:138
btTypedConstraint::btConstraintInfo2::m_J2linearAxis
btScalar * m_J2linearAxis
Definition: btTypedConstraint.h:130
btTypedConstraint::btConstraintInfo2::fps
btScalar fps
Definition: btTypedConstraint.h:124
btTypedConstraint::btConstraintInfo2::m_J1angularAxis
btScalar * m_J1angularAxis
Definition: btTypedConstraint.h:130
btTypedConstraint::btConstraintInfo2::m_constraintError
btScalar * m_constraintError
Definition: btTypedConstraint.h:138
btTypedConstraint::btConstraintInfo2::rowskip
int rowskip
Definition: btTypedConstraint.h:133