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

Bullet Collision Detection & Physics Library

btGeneric6DofConstraint.cpp
Go to the documentation of this file.
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. This notice may not be removed or altered from any source distribution.
14*/
15/*
162007-09-09
17Refactored by Francisco Le?n
18email: projectileman@yahoo.com
19http://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
31btGeneric6DofConstraint::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
61bool 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:95
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::m_hasStaticBody
bool m_hasStaticBody
Definition btGeneric6DofConstraint.h:303
btGeneric6DofConstraint::m_frameInA
btTransform m_frameInA
the constraint space w.r.t body A
Definition btGeneric6DofConstraint.h:272
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]
3 orthogonal linear constraints
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::getRotationalLimitMotor
btRotationalLimitMotor * getRotationalLimitMotor(int index)
Retrieves the angular limit informacion.
Definition btGeneric6DofConstraint.h:478
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
Definition btGeneric6DofConstraint.h:284
btGeneric6DofConstraint::m_angularLimits
btRotationalLimitMotor m_angularLimits[3]
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::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::getVelocityInLocalPoint
btVector3 getVelocityInLocalPoint(const btVector3 &rel_pos) const
Definition btRigidBody.h:460
btRigidBody::getInvMass
btScalar getInvMass() const
Definition btRigidBody.h:263
btRigidBody::getCenterOfMassTransform
const btTransform & getCenterOfMassTransform() const
Definition btRigidBody.h:429
btRigidBody::applyImpulse
void applyImpulse(const btVector3 &impulse, const btVector3 &rel_pos)
Definition btRigidBody.h:335
btRigidBody::getAngularVelocity
const btVector3 & getAngularVelocity() const
Definition btRigidBody.h:437
btRigidBody::getCenterOfMassPosition
const btVector3 & getCenterOfMassPosition() const
Definition btRigidBody.h:423
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
joint limit
Definition btGeneric6DofConstraint.h:49
btRotationalLimitMotor::m_currentLimitError
btScalar m_currentLimitError
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::getBasis
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition btTransform.h:109
btTransform::setIdentity
void setIdentity()
Set this transformation to the identity.
Definition btTransform.h:167
btTransform::getOrigin
btVector3 & getOrigin()
Return the origin vector translation.
Definition btTransform.h:114
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_accumulatedImpulse
btVector3 m_accumulatedImpulse
Definition btGeneric6DofConstraint.h:136
btTranslationalLimitMotor::testLimitValue
int testLimitValue(int limitIndex, btScalar test_value)
Definition btGeneric6DofConstraint.cpp:206
btTranslationalLimitMotor::m_currentLimitError
btVector3 m_currentLimitError
Definition btGeneric6DofConstraint.h:149
btTranslationalLimitMotor::m_limitSoftness
btScalar m_limitSoftness
Softness for linear limit.
Definition btGeneric6DofConstraint.h:139
btTranslationalLimitMotor::m_lowerLimit
btVector3 m_lowerLimit
the constraint lower limits
Definition btGeneric6DofConstraint.h:134
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
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::getFixedBody
static btRigidBody & getFixedBody()
Definition btTypedConstraint.cpp:142
btTypedConstraint::getRigidBodyA
const btRigidBody & getRigidBodyA() const
Definition btTypedConstraint.h:214
btTypedConstraint::btTypedConstraint
btTypedConstraint(btTypedConstraintType type, btRigidBody &rbA)
Definition btTypedConstraint.cpp:22
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::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::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