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

Bullet Collision Detection & Physics Library

- Main Page
- Namespaces
- Classes
- Files

- File List
- File Members

btHingeConstraint.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
16#include "btHingeConstraint.h"
17#include "BulletDynamics/Dynamics/btRigidBody.h"
18#include "LinearMath/btTransformUtil.h"
19#include "LinearMath/btMinMax.h"
20#include <new>
21#include "btSolverBody.h"
22
23//#define HINGE_USE_OBSOLETE_SOLVER false
24#define HINGE_USE_OBSOLETE_SOLVER false
25
26#define HINGE_USE_FRAME_OFFSET true
27
28#ifndef __SPU__
29
30btHingeConstraint::btHingeConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& pivotInA, const btVector3& pivotInB,
31 const btVector3& axisInA, const btVector3& axisInB, bool useReferenceFrameA)
32 : btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA, rbB),
33#ifdef _BT_USE_CENTER_LIMIT_
34 m_limit(),
35#endif
36 m_angularOnly(false),
37 m_enableAngularMotor(false),
38 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
39 m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
40 m_useReferenceFrameA(useReferenceFrameA),
41 m_flags(0),
42 m_normalCFM(0),
43 m_normalERP(0),
44 m_stopCFM(0),
45 m_stopERP(0)
46{
47 m_rbAFrame.getOrigin() = pivotInA;
48
49 // since no frame is given, assume this to be zero angle and just pick rb transform axis
50 btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0);
51
52 btVector3 rbAxisA2;
53 btScalar projection = axisInA.dot(rbAxisA1);
54 if (projection >= 1.0f - SIMD_EPSILON)
55 {
56 rbAxisA1 = -rbA.getCenterOfMassTransform().getBasis().getColumn(2);
57 rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
58 }
59 else if (projection <= -1.0f + SIMD_EPSILON)
60 {
61 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(2);
62 rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
63 }
64 else
65 {
66 rbAxisA2 = axisInA.cross(rbAxisA1);
67 rbAxisA1 = rbAxisA2.cross(axisInA);
68 }
69
70 m_rbAFrame.getBasis().setValue(rbAxisA1.getX(), rbAxisA2.getX(), axisInA.getX(),
71 rbAxisA1.getY(), rbAxisA2.getY(), axisInA.getY(),
72 rbAxisA1.getZ(), rbAxisA2.getZ(), axisInA.getZ());
73
74 btQuaternion rotationArc = shortestArcQuat(axisInA, axisInB);
75 btVector3 rbAxisB1 = quatRotate(rotationArc, rbAxisA1);
76 btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
77
78 m_rbBFrame.getOrigin() = pivotInB;
79 m_rbBFrame.getBasis().setValue(rbAxisB1.getX(), rbAxisB2.getX(), axisInB.getX(),
80 rbAxisB1.getY(), rbAxisB2.getY(), axisInB.getY(),
81 rbAxisB1.getZ(), rbAxisB2.getZ(), axisInB.getZ());
82
83#ifndef _BT_USE_CENTER_LIMIT_
84 //start with free
85 m_lowerLimit = btScalar(1.0f);
86 m_upperLimit = btScalar(-1.0f);
87 m_biasFactor = 0.3f;
88 m_relaxationFactor = 1.0f;
89 m_limitSoftness = 0.9f;
90 m_solveLimit = false;
91#endif
92 m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
93}
94
95 btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btVector3& pivotInA, const btVector3& axisInA, bool useReferenceFrameA)
96 : btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),
97#ifdef _BT_USE_CENTER_LIMIT_
98 m_limit(),
99#endif
100 m_angularOnly(false),
101 m_enableAngularMotor(false),
102 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
103 m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
104 m_useReferenceFrameA(useReferenceFrameA),
105 m_flags(0),
106 m_normalCFM(0),
107 m_normalERP(0),
108 m_stopCFM(0),
109 m_stopERP(0)
110{
111 // since no frame is given, assume this to be zero angle and just pick rb transform axis
112 // fixed axis in worldspace
113 btVector3 rbAxisA1, rbAxisA2;
114 btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2);
115
116 m_rbAFrame.getOrigin() = pivotInA;
117 m_rbAFrame.getBasis().setValue(rbAxisA1.getX(), rbAxisA2.getX(), axisInA.getX(),
118 rbAxisA1.getY(), rbAxisA2.getY(), axisInA.getY(),
119 rbAxisA1.getZ(), rbAxisA2.getZ(), axisInA.getZ());
120
121 btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * axisInA;
122
123 btQuaternion rotationArc = shortestArcQuat(axisInA, axisInB);
124 btVector3 rbAxisB1 = quatRotate(rotationArc, rbAxisA1);
125 btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
126
127 m_rbBFrame.getOrigin() = rbA.getCenterOfMassTransform()(pivotInA);
128 m_rbBFrame.getBasis().setValue(rbAxisB1.getX(), rbAxisB2.getX(), axisInB.getX(),
129 rbAxisB1.getY(), rbAxisB2.getY(), axisInB.getY(),
130 rbAxisB1.getZ(), rbAxisB2.getZ(), axisInB.getZ());
131
132#ifndef _BT_USE_CENTER_LIMIT_
133 //start with free
134 m_lowerLimit = btScalar(1.0f);
135 m_upperLimit = btScalar(-1.0f);
136 m_biasFactor = 0.3f;
137 m_relaxationFactor = 1.0f;
138 m_limitSoftness = 0.9f;
139 m_solveLimit = false;
140#endif
141 m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
142}
143
144 btHingeConstraint::btHingeConstraint(btRigidBody& rbA, btRigidBody& rbB,
145 const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA)
146 : btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA, rbB), m_rbAFrame(rbAFrame), m_rbBFrame(rbBFrame),
147#ifdef _BT_USE_CENTER_LIMIT_
148 m_limit(),
149#endif
150 m_angularOnly(false),
151 m_enableAngularMotor(false),
152 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
153 m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
154 m_useReferenceFrameA(useReferenceFrameA),
155 m_flags(0),
156 m_normalCFM(0),
157 m_normalERP(0),
158 m_stopCFM(0),
159 m_stopERP(0)
160{
161#ifndef _BT_USE_CENTER_LIMIT_
162 //start with free
163 m_lowerLimit = btScalar(1.0f);
164 m_upperLimit = btScalar(-1.0f);
165 m_biasFactor = 0.3f;
166 m_relaxationFactor = 1.0f;
167 m_limitSoftness = 0.9f;
168 m_solveLimit = false;
169#endif
170 m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
171}
172
173 btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame, bool useReferenceFrameA)
174 : btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_rbAFrame(rbAFrame), m_rbBFrame(rbAFrame),
175#ifdef _BT_USE_CENTER_LIMIT_
176 m_limit(),
177#endif
178 m_angularOnly(false),
179 m_enableAngularMotor(false),
180 m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
181 m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
182 m_useReferenceFrameA(useReferenceFrameA),
183 m_flags(0),
184 m_normalCFM(0),
185 m_normalERP(0),
186 m_stopCFM(0),
187 m_stopERP(0)
188{
190
191 m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin());
192#ifndef _BT_USE_CENTER_LIMIT_
193 //start with free
194 m_lowerLimit = btScalar(1.0f);
195 m_upperLimit = btScalar(-1.0f);
196 m_biasFactor = 0.3f;
197 m_relaxationFactor = 1.0f;
198 m_limitSoftness = 0.9f;
199 m_solveLimit = false;
200#endif
201 m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
202}
203
204 void btHingeConstraint::buildJacobian()
205{
206 if (m_useSolveConstraintObsolete)
207 {
208 m_appliedImpulse = btScalar(0.);
209 m_accMotorImpulse = btScalar(0.);
210
211 if (!m_angularOnly)
212 {
213 btVector3 pivotAInW = m_rbA.getCenterOfMassTransform() * m_rbAFrame.getOrigin();
214 btVector3 pivotBInW = m_rbB.getCenterOfMassTransform() * m_rbBFrame.getOrigin();
215 btVector3 relPos = pivotBInW - pivotAInW;
216
217 btVector3 normal[3];
218 if (relPos.length2() > SIMD_EPSILON)
219 {
220 normal[0] = relPos.normalized();
221 }
222 else
223 {
224 normal[0].setValue(btScalar(1.0), 0, 0);
225 }
226
227 btPlaneSpace1(normal[0], normal[1], normal[2]);
228
229 for (int i = 0; i < 3; i++)
230 {
231 new (&m_jac[i]) btJacobianEntry(
232 m_rbA.getCenterOfMassTransform().getBasis().transpose(),
233 m_rbB.getCenterOfMassTransform().getBasis().transpose(),
234 pivotAInW - m_rbA.getCenterOfMassPosition(),
235 pivotBInW - m_rbB.getCenterOfMassPosition(),
236 normal[i],
237 m_rbA.getInvInertiaDiagLocal(),
238 m_rbA.getInvMass(),
239 m_rbB.getInvInertiaDiagLocal(),
240 m_rbB.getInvMass());
241 }
242 }
243
244 //calculate two perpendicular jointAxis, orthogonal to hingeAxis
245 //these two jointAxis require equal angular velocities for both bodies
246
247 //this is unused for now, it's a todo
248 btVector3 jointAxis0local;
249 btVector3 jointAxis1local;
250
251 btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2), jointAxis0local, jointAxis1local);
252
253 btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local;
254 btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local;
255 btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
256
257 new (&m_jacAng[0]) btJacobianEntry(jointAxis0,
258 m_rbA.getCenterOfMassTransform().getBasis().transpose(),
259 m_rbB.getCenterOfMassTransform().getBasis().transpose(),
260 m_rbA.getInvInertiaDiagLocal(),
261 m_rbB.getInvInertiaDiagLocal());
262
263 new (&m_jacAng[1]) btJacobianEntry(jointAxis1,
264 m_rbA.getCenterOfMassTransform().getBasis().transpose(),
265 m_rbB.getCenterOfMassTransform().getBasis().transpose(),
266 m_rbA.getInvInertiaDiagLocal(),
267 m_rbB.getInvInertiaDiagLocal());
268
269 new (&m_jacAng[2]) btJacobianEntry(hingeAxisWorld,
270 m_rbA.getCenterOfMassTransform().getBasis().transpose(),
271 m_rbB.getCenterOfMassTransform().getBasis().transpose(),
272 m_rbA.getInvInertiaDiagLocal(),
273 m_rbB.getInvInertiaDiagLocal());
274
275 // clear accumulator
276 m_accLimitImpulse = btScalar(0.);
277
278 // test angular limit
279 testLimit(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
280
281 //Compute K = J*W*J' for hinge axis
282 btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
283 m_kHinge = 1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) +
284 getRigidBodyB().computeAngularImpulseDenominator(axisA));
285 }
286}
287
288#endif //__SPU__
289
290 static inline btScalar btNormalizeAnglePositive(btScalar angle)
291{
292 return btFmod(btFmod(angle, btScalar(2.0 * SIMD_PI)) + btScalar(2.0 * SIMD_PI), btScalar(2.0 * SIMD_PI));
293}
294
295 static btScalar btShortestAngularDistance(btScalar accAngle, btScalar curAngle)
296{
297 btScalar result = btNormalizeAngle(btNormalizeAnglePositive(btNormalizeAnglePositive(curAngle) -
298 btNormalizeAnglePositive(accAngle)));
299 return result;
300}
301
302 static btScalar btShortestAngleUpdate(btScalar accAngle, btScalar curAngle)
303{
304 btScalar tol(0.3);
305 btScalar result = btShortestAngularDistance(accAngle, curAngle);
306
307 if (btFabs(result) > tol)
308 return curAngle;
309 else
310 return accAngle + result;
311
312 return curAngle;
313}
314
315 btScalar btHingeAccumulatedAngleConstraint::getAccumulatedHingeAngle()
316{
317 btScalar hingeAngle = getHingeAngle();
318 m_accumulatedAngle = btShortestAngleUpdate(m_accumulatedAngle, hingeAngle);
319 return m_accumulatedAngle;
320}
321 void btHingeAccumulatedAngleConstraint::setAccumulatedHingeAngle(btScalar accAngle)
322{
323 m_accumulatedAngle = accAngle;
324}
325
326 void btHingeAccumulatedAngleConstraint::getInfo1(btConstraintInfo1* info)
327{
328 //update m_accumulatedAngle
329 btScalar curHingeAngle = getHingeAngle();
330 m_accumulatedAngle = btShortestAngleUpdate(m_accumulatedAngle, curHingeAngle);
331
332 btHingeConstraint::getInfo1(info);
333}
334
335 void btHingeConstraint::getInfo1(btConstraintInfo1* info)
336{
337 if (m_useSolveConstraintObsolete)
338 {
339 info->m_numConstraintRows = 0;
340 info->nub = 0;
341 }
342 else
343 {
344 info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular
345 info->nub = 1;
346 //always add the row, to avoid computation (data is not available yet)
347 //prepare constraint
348 testLimit(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
349 if (getSolveLimit() || getEnableAngularMotor())
350 {
351 info->m_numConstraintRows++; // limit 3rd anguar as well
352 info->nub--;
353 }
354 }
355}
356
357 void btHingeConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
358{
359 if (m_useSolveConstraintObsolete)
360 {
361 info->m_numConstraintRows = 0;
362 info->nub = 0;
363 }
364 else
365 {
366 //always add the 'limit' row, to avoid computation (data is not available yet)
367 info->m_numConstraintRows = 6; // Fixed 3 linear + 2 angular
368 info->nub = 0;
369 }
370}
371
372 void btHingeConstraint::getInfo2(btConstraintInfo2* info)
373{
374 if (m_useOffsetForConstraintFrame)
375 {
376 getInfo2InternalUsingFrameOffset(info, m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform(), m_rbA.getAngularVelocity(), m_rbB.getAngularVelocity());
377 }
378 else
379 {
380 getInfo2Internal(info, m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform(), m_rbA.getAngularVelocity(), m_rbB.getAngularVelocity());
381 }
382}
383
384 void btHingeConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB, const btVector3& angVelA, const btVector3& angVelB)
385{
387 testLimit(transA, transB);
388
389 getInfo2Internal(info, transA, transB, angVelA, angVelB);
390}
391
392 void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB, const btVector3& angVelA, const btVector3& angVelB)
393{
394 btAssert(!m_useSolveConstraintObsolete);
395 int i, skip = info->rowskip;
396 // transforms in world space
397 btTransform trA = transA * m_rbAFrame;
398 btTransform trB = transB * m_rbBFrame;
399 // pivot point
400 btVector3 pivotAInW = trA.getOrigin();
401 btVector3 pivotBInW = trB.getOrigin();
402#if 0
403 if (0)
404 {
405 for (i=0;i<6;i++)
406 {
407 info->m_J1linearAxis[i*skip]=0;
408 info->m_J1linearAxis[i*skip+1]=0;
409 info->m_J1linearAxis[i*skip+2]=0;
410
411 info->m_J1angularAxis[i*skip]=0;
412 info->m_J1angularAxis[i*skip+1]=0;
413 info->m_J1angularAxis[i*skip+2]=0;
414
415 info->m_J2linearAxis[i*skip]=0;
416 info->m_J2linearAxis[i*skip+1]=0;
417 info->m_J2linearAxis[i*skip+2]=0;
418
419 info->m_J2angularAxis[i*skip]=0;
420 info->m_J2angularAxis[i*skip+1]=0;
421 info->m_J2angularAxis[i*skip+2]=0;
422
423 info->m_constraintError[i*skip]=0.f;
424 }
425 }
426#endif //#if 0
427 // linear (all fixed)
428
429 if (!m_angularOnly)
430 {
431 info->m_J1linearAxis[0] = 1;
432 info->m_J1linearAxis[skip + 1] = 1;
433 info->m_J1linearAxis[2 * skip + 2] = 1;
434
435 info->m_J2linearAxis[0] = -1;
436 info->m_J2linearAxis[skip + 1] = -1;
437 info->m_J2linearAxis[2 * skip + 2] = -1;
438 }
439
440 btVector3 a1 = pivotAInW - transA.getOrigin();
441 {
442 btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
443 btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + skip);
444 btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * skip);
445 btVector3 a1neg = -a1;
446 a1neg.getSkewSymmetricMatrix(angular0, angular1, angular2);
447 }
448 btVector3 a2 = pivotBInW - transB.getOrigin();
449 {
450 btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
451 btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + skip);
452 btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * skip);
453 a2.getSkewSymmetricMatrix(angular0, angular1, angular2);
454 }
455 // linear RHS
456 btScalar normalErp = (m_flags & BT_HINGE_FLAGS_ERP_NORM) ? m_normalERP : info->erp;
457
458 btScalar k = info->fps * normalErp;
459 if (!m_angularOnly)
460 {
461 for (i = 0; i < 3; i++)
462 {
463 info->m_constraintError[i * skip] = k * (pivotBInW[i] - pivotAInW[i]);
464 }
465 }
466 // make rotations around X and Y equal
467 // the hinge axis should be the only unconstrained
468 // rotational axis, the angular velocity of the two bodies perpendicular to
469 // the hinge axis should be equal. thus the constraint equations are
470 // p*w1 - p*w2 = 0
471 // q*w1 - q*w2 = 0
472 // where p and q are unit vectors normal to the hinge axis, and w1 and w2
473 // are the angular velocity vectors of the two bodies.
474 // get hinge axis (Z)
475 btVector3 ax1 = trA.getBasis().getColumn(2);
476 // get 2 orthos to hinge axis (X, Y)
477 btVector3 p = trA.getBasis().getColumn(0);
478 btVector3 q = trA.getBasis().getColumn(1);
479 // set the two hinge angular rows
480 int s3 = 3 * info->rowskip;
481 int s4 = 4 * info->rowskip;
482
483 info->m_J1angularAxis[s3 + 0] = p[0];
484 info->m_J1angularAxis[s3 + 1] = p[1];
485 info->m_J1angularAxis[s3 + 2] = p[2];
486 info->m_J1angularAxis[s4 + 0] = q[0];
487 info->m_J1angularAxis[s4 + 1] = q[1];
488 info->m_J1angularAxis[s4 + 2] = q[2];
489
490 info->m_J2angularAxis[s3 + 0] = -p[0];
491 info->m_J2angularAxis[s3 + 1] = -p[1];
492 info->m_J2angularAxis[s3 + 2] = -p[2];
493 info->m_J2angularAxis[s4 + 0] = -q[0];
494 info->m_J2angularAxis[s4 + 1] = -q[1];
495 info->m_J2angularAxis[s4 + 2] = -q[2];
496 // compute the right hand side of the constraint equation. set relative
497 // body velocities along p and q to bring the hinge back into alignment.
498 // if ax1,ax2 are the unit length hinge axes as computed from body1 and
499 // body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
500 // if `theta' is the angle between ax1 and ax2, we need an angular velocity
501 // along u to cover angle erp*theta in one step :
502 // |angular_velocity| = angle/time = erp*theta / stepsize
503 // = (erp*fps) * theta
504 // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
505 // = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
506 // ...as ax1 and ax2 are unit length. if theta is smallish,
507 // theta ~= sin(theta), so
508 // angular_velocity = (erp*fps) * (ax1 x ax2)
509 // ax1 x ax2 is in the plane space of ax1, so we project the angular
510 // velocity to p and q to find the right hand side.
511 btVector3 ax2 = trB.getBasis().getColumn(2);
512 btVector3 u = ax1.cross(ax2);
513 info->m_constraintError[s3] = k * u.dot(p);
514 info->m_constraintError[s4] = k * u.dot(q);
515 // check angular limits
516 int nrow = 4; // last filled row
517 int srow;
518 btScalar limit_err = btScalar(0.0);
519 int limit = 0;
520 if (getSolveLimit())
521 {
522#ifdef _BT_USE_CENTER_LIMIT_
523 limit_err = m_limit.getCorrection() * m_referenceSign;
524#else
525 limit_err = m_correction * m_referenceSign;
526#endif
527 limit = (limit_err > btScalar(0.0)) ? 1 : 2;
528 }
529 // if the hinge has joint limits or motor, add in the extra row
530 bool powered = getEnableAngularMotor();
531 if (limit || powered)
532 {
533 nrow++;
534 srow = nrow * info->rowskip;
535 info->m_J1angularAxis[srow + 0] = ax1[0];
536 info->m_J1angularAxis[srow + 1] = ax1[1];
537 info->m_J1angularAxis[srow + 2] = ax1[2];
538
539 info->m_J2angularAxis[srow + 0] = -ax1[0];
540 info->m_J2angularAxis[srow + 1] = -ax1[1];
541 info->m_J2angularAxis[srow + 2] = -ax1[2];
542
543 btScalar lostop = getLowerLimit();
544 btScalar histop = getUpperLimit();
545 if (limit && (lostop == histop))
546 { // the joint motor is ineffective
547 powered = false;
548 }
549 info->m_constraintError[srow] = btScalar(0.0f);
550 btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : normalErp;
551 if (powered)
552 {
553 if (m_flags & BT_HINGE_FLAGS_CFM_NORM)
554 {
555 info->cfm[srow] = m_normalCFM;
556 }
557 btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP);
558 info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
559 info->m_lowerLimit[srow] = -m_maxMotorImpulse;
560 info->m_upperLimit[srow] = m_maxMotorImpulse;
561 }
562 if (limit)
563 {
564 k = info->fps * currERP;
565 info->m_constraintError[srow] += k * limit_err;
566 if (m_flags & BT_HINGE_FLAGS_CFM_STOP)
567 {
568 info->cfm[srow] = m_stopCFM;
569 }
570 if (lostop == histop)
571 {
572 // limited low and high simultaneously
573 info->m_lowerLimit[srow] = -SIMD_INFINITY;
574 info->m_upperLimit[srow] = SIMD_INFINITY;
575 }
576 else if (limit == 1)
577 { // low limit
578 info->m_lowerLimit[srow] = 0;
579 info->m_upperLimit[srow] = SIMD_INFINITY;
580 }
581 else
582 { // high limit
583 info->m_lowerLimit[srow] = -SIMD_INFINITY;
584 info->m_upperLimit[srow] = 0;
585 }
586 // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
587#ifdef _BT_USE_CENTER_LIMIT_
588 btScalar bounce = m_limit.getRelaxationFactor();
589#else
590 btScalar bounce = m_relaxationFactor;
591#endif
592 if (bounce > btScalar(0.0))
593 {
594 btScalar vel = angVelA.dot(ax1);
595 vel -= angVelB.dot(ax1);
596 // only apply bounce if the velocity is incoming, and if the
597 // resulting c[] exceeds what we already have.
598 if (limit == 1)
599 { // low limit
600 if (vel < 0)
601 {
602 btScalar newc = -bounce * vel;
603 if (newc > info->m_constraintError[srow])
604 {
605 info->m_constraintError[srow] = newc;
606 }
607 }
608 }
609 else
610 { // high limit - all those computations are reversed
611 if (vel > 0)
612 {
613 btScalar newc = -bounce * vel;
614 if (newc < info->m_constraintError[srow])
615 {
616 info->m_constraintError[srow] = newc;
617 }
618 }
619 }
620 }
621#ifdef _BT_USE_CENTER_LIMIT_
622 info->m_constraintError[srow] *= m_limit.getBiasFactor();
623#else
624 info->m_constraintError[srow] *= m_biasFactor;
625#endif
626 } // if(limit)
627 } // if angular limit or powered
628}
629
630 void btHingeConstraint::setFrames(const btTransform& frameA, const btTransform& frameB)
631{
632 m_rbAFrame = frameA;
633 m_rbBFrame = frameB;
634 buildJacobian();
635}
636
637 void btHingeConstraint::updateRHS(btScalar timeStep)
638{
639 (void)timeStep;
640}
641
642 btScalar btHingeConstraint::getHingeAngle()
643{
644 return getHingeAngle(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
645}
646
647 btScalar btHingeConstraint::getHingeAngle(const btTransform& transA, const btTransform& transB)
648{
649 const btVector3 refAxis0 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0);
650 const btVector3 refAxis1 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1);
651 const btVector3 swingAxis = transB.getBasis() * m_rbBFrame.getBasis().getColumn(1);
652 // btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
653 btScalar angle = btAtan2(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
654 return m_referenceSign * angle;
655}
656
657 void btHingeConstraint::testLimit(const btTransform& transA, const btTransform& transB)
658{
659 // Compute limit information
660 m_hingeAngle = getHingeAngle(transA, transB);
661#ifdef _BT_USE_CENTER_LIMIT_
662 m_limit.test(m_hingeAngle);
663#else
664 m_correction = btScalar(0.);
665 m_limitSign = btScalar(0.);
666 m_solveLimit = false;
667 if (m_lowerLimit <= m_upperLimit)
668 {
669 m_hingeAngle = btAdjustAngleToLimits(m_hingeAngle, m_lowerLimit, m_upperLimit);
670 if (m_hingeAngle <= m_lowerLimit)
671 {
672 m_correction = (m_lowerLimit - m_hingeAngle);
673 m_limitSign = 1.0f;
674 m_solveLimit = true;
675 }
676 else if (m_hingeAngle >= m_upperLimit)
677 {
678 m_correction = m_upperLimit - m_hingeAngle;
679 m_limitSign = -1.0f;
680 m_solveLimit = true;
681 }
682 }
683#endif
684 return;
685}
686
687 static btVector3 vHinge(0, 0, btScalar(1));
688
689 void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt)
690{
691 // convert target from body to constraint space
692 btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * qAinB * m_rbAFrame.getRotation();
693 qConstraint.normalize();
694
695 // extract "pure" hinge component
696 btVector3 vNoHinge = quatRotate(qConstraint, vHinge);
697 vNoHinge.normalize();
698 btQuaternion qNoHinge = shortestArcQuat(vHinge, vNoHinge);
699 btQuaternion qHinge = qNoHinge.inverse() * qConstraint;
700 qHinge.normalize();
701
702 // compute angular target, clamped to limits
703 btScalar targetAngle = qHinge.getAngle();
704 if (targetAngle > SIMD_PI) // long way around. flip quat and recalculate.
705 {
706 qHinge = -(qHinge);
707 targetAngle = qHinge.getAngle();
708 }
709 if (qHinge.getZ() < 0)
710 targetAngle = -targetAngle;
711
712 setMotorTarget(targetAngle, dt);
713}
714
715 void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt)
716{
717#ifdef _BT_USE_CENTER_LIMIT_
718 m_limit.fit(targetAngle);
719#else
720 if (m_lowerLimit < m_upperLimit)
721 {
722 if (targetAngle < m_lowerLimit)
723 targetAngle = m_lowerLimit;
724 else if (targetAngle > m_upperLimit)
725 targetAngle = m_upperLimit;
726 }
727#endif
728 // compute angular velocity
729 btScalar curAngle = getHingeAngle(m_rbA.getCenterOfMassTransform(), m_rbB.getCenterOfMassTransform());
730 btScalar dAngle = targetAngle - curAngle;
731 m_motorTargetVelocity = dAngle / dt;
732}
733
734 void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info, const btTransform& transA, const btTransform& transB, const btVector3& angVelA, const btVector3& angVelB)
735{
736 btAssert(!m_useSolveConstraintObsolete);
737 int i, s = info->rowskip;
738 // transforms in world space
739 btTransform trA = transA * m_rbAFrame;
740 btTransform trB = transB * m_rbBFrame;
741 // pivot point
742// btVector3 pivotAInW = trA.getOrigin();
743// btVector3 pivotBInW = trB.getOrigin();
744#if 1
745 // difference between frames in WCS
746 btVector3 ofs = trB.getOrigin() - trA.getOrigin();
747 // now get weight factors depending on masses
748 btScalar miA = getRigidBodyA().getInvMass();
749 btScalar miB = getRigidBodyB().getInvMass();
750 bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
751 btScalar miS = miA + miB;
752 btScalar factA, factB;
753 if (miS > btScalar(0.f))
754 {
755 factA = miB / miS;
756 }
757 else
758 {
759 factA = btScalar(0.5f);
760 }
761 factB = btScalar(1.0f) - factA;
762 // get the desired direction of hinge axis
763 // as weighted sum of Z-orthos of frameA and frameB in WCS
764 btVector3 ax1A = trA.getBasis().getColumn(2);
765 btVector3 ax1B = trB.getBasis().getColumn(2);
766 btVector3 ax1 = ax1A * factA + ax1B * factB;
767 if (ax1.length2()<SIMD_EPSILON)
768 {
769 factA=0.f;
770 factB=1.f;
771 ax1 = ax1A * factA + ax1B * factB;
772 }
773 ax1.normalize();
774 // fill first 3 rows
775 // we want: velA + wA x relA == velB + wB x relB
776 btTransform bodyA_trans = transA;
777 btTransform bodyB_trans = transB;
778 int s0 = 0;
779 int s1 = s;
780 int s2 = s * 2;
781 int nrow = 2; // last filled row
782 btVector3 tmpA, tmpB, relA, relB, p, q;
783 // get vector from bodyB to frameB in WCS
784 relB = trB.getOrigin() - bodyB_trans.getOrigin();
785 // get its projection to hinge axis
786 btVector3 projB = ax1 * relB.dot(ax1);
787 // get vector directed from bodyB to hinge axis (and orthogonal to it)
788 btVector3 orthoB = relB - projB;
789 // same for bodyA
790 relA = trA.getOrigin() - bodyA_trans.getOrigin();
791 btVector3 projA = ax1 * relA.dot(ax1);
792 btVector3 orthoA = relA - projA;
793 btVector3 totalDist = projA - projB;
794 // get offset vectors relA and relB
795 relA = orthoA + totalDist * factA;
796 relB = orthoB - totalDist * factB;
797 // now choose average ortho to hinge axis
798 p = orthoB * factA + orthoA * factB;
799 btScalar len2 = p.length2();
800 if (len2 > SIMD_EPSILON)
801 {
802 p /= btSqrt(len2);
803 }
804 else
805 {
806 p = trA.getBasis().getColumn(1);
807 }
808 // make one more ortho
809 q = ax1.cross(p);
810 // fill three rows
811 tmpA = relA.cross(p);
812 tmpB = relB.cross(p);
813 for (i = 0; i < 3; i++) info->m_J1angularAxis[s0 + i] = tmpA[i];
814 for (i = 0; i < 3; i++) info->m_J2angularAxis[s0 + i] = -tmpB[i];
815 tmpA = relA.cross(q);
816 tmpB = relB.cross(q);
817 if (hasStaticBody && getSolveLimit())
818 { // to make constraint between static and dynamic objects more rigid
819 // remove wA (or wB) from equation if angular limit is hit
820 tmpB *= factB;
821 tmpA *= factA;
822 }
823 for (i = 0; i < 3; i++) info->m_J1angularAxis[s1 + i] = tmpA[i];
824 for (i = 0; i < 3; i++) info->m_J2angularAxis[s1 + i] = -tmpB[i];
825 tmpA = relA.cross(ax1);
826 tmpB = relB.cross(ax1);
827 if (hasStaticBody)
828 { // to make constraint between static and dynamic objects more rigid
829 // remove wA (or wB) from equation
830 tmpB *= factB;
831 tmpA *= factA;
832 }
833 for (i = 0; i < 3; i++) info->m_J1angularAxis[s2 + i] = tmpA[i];
834 for (i = 0; i < 3; i++) info->m_J2angularAxis[s2 + i] = -tmpB[i];
835
836 btScalar normalErp = (m_flags & BT_HINGE_FLAGS_ERP_NORM) ? m_normalERP : info->erp;
837 btScalar k = info->fps * normalErp;
838
839 if (!m_angularOnly)
840 {
841 for (i = 0; i < 3; i++) info->m_J1linearAxis[s0 + i] = p[i];
842 for (i = 0; i < 3; i++) info->m_J1linearAxis[s1 + i] = q[i];
843 for (i = 0; i < 3; i++) info->m_J1linearAxis[s2 + i] = ax1[i];
844
845 for (i = 0; i < 3; i++) info->m_J2linearAxis[s0 + i] = -p[i];
846 for (i = 0; i < 3; i++) info->m_J2linearAxis[s1 + i] = -q[i];
847 for (i = 0; i < 3; i++) info->m_J2linearAxis[s2 + i] = -ax1[i];
848
849 // compute three elements of right hand side
850
851 btScalar rhs = k * p.dot(ofs);
852 info->m_constraintError[s0] = rhs;
853 rhs = k * q.dot(ofs);
854 info->m_constraintError[s1] = rhs;
855 rhs = k * ax1.dot(ofs);
856 info->m_constraintError[s2] = rhs;
857 }
858 // the hinge axis should be the only unconstrained
859 // rotational axis, the angular velocity of the two bodies perpendicular to
860 // the hinge axis should be equal. thus the constraint equations are
861 // p*w1 - p*w2 = 0
862 // q*w1 - q*w2 = 0
863 // where p and q are unit vectors normal to the hinge axis, and w1 and w2
864 // are the angular velocity vectors of the two bodies.
865 int s3 = 3 * s;
866 int s4 = 4 * s;
867 info->m_J1angularAxis[s3 + 0] = p[0];
868 info->m_J1angularAxis[s3 + 1] = p[1];
869 info->m_J1angularAxis[s3 + 2] = p[2];
870 info->m_J1angularAxis[s4 + 0] = q[0];
871 info->m_J1angularAxis[s4 + 1] = q[1];
872 info->m_J1angularAxis[s4 + 2] = q[2];
873
874 info->m_J2angularAxis[s3 + 0] = -p[0];
875 info->m_J2angularAxis[s3 + 1] = -p[1];
876 info->m_J2angularAxis[s3 + 2] = -p[2];
877 info->m_J2angularAxis[s4 + 0] = -q[0];
878 info->m_J2angularAxis[s4 + 1] = -q[1];
879 info->m_J2angularAxis[s4 + 2] = -q[2];
880 // compute the right hand side of the constraint equation. set relative
881 // body velocities along p and q to bring the hinge back into alignment.
882 // if ax1A,ax1B are the unit length hinge axes as computed from bodyA and
883 // bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2).
884 // if "theta" is the angle between ax1 and ax2, we need an angular velocity
885 // along u to cover angle erp*theta in one step :
886 // |angular_velocity| = angle/time = erp*theta / stepsize
887 // = (erp*fps) * theta
888 // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
889 // = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
890 // ...as ax1 and ax2 are unit length. if theta is smallish,
891 // theta ~= sin(theta), so
892 // angular_velocity = (erp*fps) * (ax1 x ax2)
893 // ax1 x ax2 is in the plane space of ax1, so we project the angular
894 // velocity to p and q to find the right hand side.
895 k = info->fps * normalErp; //??
896
897 btVector3 u = ax1A.cross(ax1B);
898 info->m_constraintError[s3] = k * u.dot(p);
899 info->m_constraintError[s4] = k * u.dot(q);
900#endif
901 // check angular limits
902 nrow = 4; // last filled row
903 int srow;
904 btScalar limit_err = btScalar(0.0);
905 int limit = 0;
906 if (getSolveLimit())
907 {
908#ifdef _BT_USE_CENTER_LIMIT_
909 limit_err = m_limit.getCorrection() * m_referenceSign;
910#else
911 limit_err = m_correction * m_referenceSign;
912#endif
913 limit = (limit_err > btScalar(0.0)) ? 1 : 2;
914 }
915 // if the hinge has joint limits or motor, add in the extra row
916 bool powered = getEnableAngularMotor();
917 if (limit || powered)
918 {
919 nrow++;
920 srow = nrow * info->rowskip;
921 info->m_J1angularAxis[srow + 0] = ax1[0];
922 info->m_J1angularAxis[srow + 1] = ax1[1];
923 info->m_J1angularAxis[srow + 2] = ax1[2];
924
925 info->m_J2angularAxis[srow + 0] = -ax1[0];
926 info->m_J2angularAxis[srow + 1] = -ax1[1];
927 info->m_J2angularAxis[srow + 2] = -ax1[2];
928
929 btScalar lostop = getLowerLimit();
930 btScalar histop = getUpperLimit();
931 if (limit && (lostop == histop))
932 { // the joint motor is ineffective
933 powered = false;
934 }
935 info->m_constraintError[srow] = btScalar(0.0f);
936 btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : normalErp;
937 if (powered)
938 {
939 if (m_flags & BT_HINGE_FLAGS_CFM_NORM)
940 {
941 info->cfm[srow] = m_normalCFM;
942 }
943 btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP);
944 info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
945 info->m_lowerLimit[srow] = -m_maxMotorImpulse;
946 info->m_upperLimit[srow] = m_maxMotorImpulse;
947 }
948 if (limit)
949 {
950 k = info->fps * currERP;
951 info->m_constraintError[srow] += k * limit_err;
952 if (m_flags & BT_HINGE_FLAGS_CFM_STOP)
953 {
954 info->cfm[srow] = m_stopCFM;
955 }
956 if (lostop == histop)
957 {
958 // limited low and high simultaneously
959 info->m_lowerLimit[srow] = -SIMD_INFINITY;
960 info->m_upperLimit[srow] = SIMD_INFINITY;
961 }
962 else if (limit == 1)
963 { // low limit
964 info->m_lowerLimit[srow] = 0;
965 info->m_upperLimit[srow] = SIMD_INFINITY;
966 }
967 else
968 { // high limit
969 info->m_lowerLimit[srow] = -SIMD_INFINITY;
970 info->m_upperLimit[srow] = 0;
971 }
972 // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
973#ifdef _BT_USE_CENTER_LIMIT_
974 btScalar bounce = m_limit.getRelaxationFactor();
975#else
976 btScalar bounce = m_relaxationFactor;
977#endif
978 if (bounce > btScalar(0.0))
979 {
980 btScalar vel = angVelA.dot(ax1);
981 vel -= angVelB.dot(ax1);
982 // only apply bounce if the velocity is incoming, and if the
983 // resulting c[] exceeds what we already have.
984 if (limit == 1)
985 { // low limit
986 if (vel < 0)
987 {
988 btScalar newc = -bounce * vel;
989 if (newc > info->m_constraintError[srow])
990 {
991 info->m_constraintError[srow] = newc;
992 }
993 }
994 }
995 else
996 { // high limit - all those computations are reversed
997 if (vel > 0)
998 {
999 btScalar newc = -bounce * vel;
1000 if (newc < info->m_constraintError[srow])
1001 {
1002 info->m_constraintError[srow] = newc;
1003 }
1004 }
1005 }
1006 }
1007#ifdef _BT_USE_CENTER_LIMIT_
1008 info->m_constraintError[srow] *= m_limit.getBiasFactor();
1009#else
1010 info->m_constraintError[srow] *= m_biasFactor;
1011#endif
1012 } // if(limit)
1013 } // if angular limit or powered
1014}
1015
1018 void btHingeConstraint::setParam(int num, btScalar value, int axis)
1019{
1020 if ((axis == -1) || (axis == 5))
1021 {
1022 switch (num)
1023 {
1024 case BT_CONSTRAINT_STOP_ERP:
1025 m_stopERP = value;
1026 m_flags |= BT_HINGE_FLAGS_ERP_STOP;
1027 break;
1028 case BT_CONSTRAINT_STOP_CFM:
1029 m_stopCFM = value;
1030 m_flags |= BT_HINGE_FLAGS_CFM_STOP;
1031 break;
1032 case BT_CONSTRAINT_CFM:
1033 m_normalCFM = value;
1034 m_flags |= BT_HINGE_FLAGS_CFM_NORM;
1035 break;
1036 case BT_CONSTRAINT_ERP:
1037 m_normalERP = value;
1038 m_flags |= BT_HINGE_FLAGS_ERP_NORM;
1039 break;
1040 default:
1041 btAssertConstrParams(0);
1042 }
1043 }
1044 else
1045 {
1046 btAssertConstrParams(0);
1047 }
1048}
1049
1051 btScalar btHingeConstraint::getParam(int num, int axis) const
1052{
1053 btScalar retVal = 0;
1054 if ((axis == -1) || (axis == 5))
1055 {
1056 switch (num)
1057 {
1058 case BT_CONSTRAINT_STOP_ERP:
1059 btAssertConstrParams(m_flags & BT_HINGE_FLAGS_ERP_STOP);
1060 retVal = m_stopERP;
1061 break;
1062 case BT_CONSTRAINT_STOP_CFM:
1063 btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_STOP);
1064 retVal = m_stopCFM;
1065 break;
1066 case BT_CONSTRAINT_CFM:
1067 btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_NORM);
1068 retVal = m_normalCFM;
1069 break;
1070 case BT_CONSTRAINT_ERP:
1071 btAssertConstrParams(m_flags & BT_HINGE_FLAGS_ERP_NORM);
1072 retVal = m_normalERP;
1073 break;
1074 default:
1075 btAssertConstrParams(0);
1076 }
1077 }
1078 else
1079 {
1080 btAssertConstrParams(0);
1081 }
1082 return retVal;
1083}
HINGE_USE_OBSOLETE_SOLVER
#define HINGE_USE_OBSOLETE_SOLVER
Definition: btHingeConstraint.cpp:24
btShortestAngularDistance
static btScalar btShortestAngularDistance(btScalar accAngle, btScalar curAngle)
Definition: btHingeConstraint.cpp:295
btNormalizeAnglePositive
static btScalar btNormalizeAnglePositive(btScalar angle)
Definition: btHingeConstraint.cpp:290
vHinge
static btVector3 vHinge(0, 0, btScalar(1))
btShortestAngleUpdate
static btScalar btShortestAngleUpdate(btScalar accAngle, btScalar curAngle)
Definition: btHingeConstraint.cpp:302
HINGE_USE_FRAME_OFFSET
#define HINGE_USE_FRAME_OFFSET
Definition: btHingeConstraint.cpp:26
btHingeConstraint.h
BT_HINGE_FLAGS_CFM_STOP
@ BT_HINGE_FLAGS_CFM_STOP
Definition: btHingeConstraint.h:39
BT_HINGE_FLAGS_CFM_NORM
@ BT_HINGE_FLAGS_CFM_NORM
Definition: btHingeConstraint.h:41
BT_HINGE_FLAGS_ERP_NORM
@ BT_HINGE_FLAGS_ERP_NORM
Definition: btHingeConstraint.h:42
BT_HINGE_FLAGS_ERP_STOP
@ BT_HINGE_FLAGS_ERP_STOP
Definition: btHingeConstraint.h:40
_BT_USE_CENTER_LIMIT_
#define _BT_USE_CENTER_LIMIT_
Definition: btHingeConstraint.h:21
btMinMax.h
shortestArcQuat
btQuaternion shortestArcQuat(const btVector3 &v0, const btVector3 &v1)
Definition: btQuaternion.h:940
quatRotate
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
Definition: btQuaternion.h:926
btRigidBody.h
btNormalizeAngle
btScalar btNormalizeAngle(btScalar angleInRadians)
Definition: btScalar.h:781
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
btSqrt
btScalar btSqrt(btScalar y)
Definition: btScalar.h:466
btAtan2
btScalar btAtan2(btScalar x, btScalar y)
Definition: btScalar.h:518
btFabs
btScalar btFabs(btScalar x)
Definition: btScalar.h:497
SIMD_INFINITY
#define SIMD_INFINITY
Definition: btScalar.h:544
btFmod
btScalar btFmod(btScalar x, btScalar y)
Definition: btScalar.h:522
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:543
btAssert
#define btAssert(x)
Definition: btScalar.h:153
btSolverBody.h
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_ERP
@ BT_CONSTRAINT_ERP
Definition: btTypedConstraint.h:51
BT_CONSTRAINT_STOP_CFM
@ BT_CONSTRAINT_STOP_CFM
Definition: btTypedConstraint.h:54
BT_CONSTRAINT_STOP_ERP
@ BT_CONSTRAINT_STOP_ERP
Definition: btTypedConstraint.h:52
HINGE_CONSTRAINT_TYPE
@ HINGE_CONSTRAINT_TYPE
Definition: btTypedConstraint.h:37
btPlaneSpace1
void btPlaneSpace1(const T &n, T &p, T &q)
Definition: btVector3.h:1251
btAngularLimit::getBiasFactor
btScalar getBiasFactor() const
Returns limit's bias factor.
Definition: btTypedConstraint.h:485
btAngularLimit::getCorrection
btScalar getCorrection() const
Returns correction value evaluated when test() was invoked.
Definition: btTypedConstraint.h:497
btAngularLimit::test
void test(const btScalar angle)
Checks conastaint angle against limit.
Definition: btTypedConstraint.cpp:158
btAngularLimit::fit
void fit(btScalar &angle) const
Checks given angle against limit.
Definition: btTypedConstraint.cpp:187
btAngularLimit::getRelaxationFactor
btScalar getRelaxationFactor() const
Returns limit's relaxation factor.
Definition: btTypedConstraint.h:491
btHingeAccumulatedAngleConstraint::setAccumulatedHingeAngle
void setAccumulatedHingeAngle(btScalar accAngle)
Definition: btHingeConstraint.cpp:321
btHingeAccumulatedAngleConstraint::m_accumulatedAngle
btScalar m_accumulatedAngle
Definition: btHingeConstraint.h:364
btHingeAccumulatedAngleConstraint::getAccumulatedHingeAngle
btScalar getAccumulatedHingeAngle()
Definition: btHingeConstraint.cpp:315
btHingeAccumulatedAngleConstraint::getInfo1
virtual void getInfo1(btConstraintInfo1 *info)
internal method used by the constraint solver, don't use them directly
Definition: btHingeConstraint.cpp:326
btHingeConstraint::m_stopERP
btScalar m_stopERP
Definition: btHingeConstraint.h:95
btHingeConstraint::getRigidBodyB
const btRigidBody & getRigidBodyB() const
Definition: btHingeConstraint.h:127
btHingeConstraint::m_jacAng
btJacobianEntry m_jacAng[3]
Definition: btHingeConstraint.h:54
btHingeConstraint::getHingeAngle
btScalar getHingeAngle()
The getHingeAngle gives the hinge angle in range [-PI,PI].
Definition: btHingeConstraint.cpp:642
btHingeConstraint::m_maxMotorImpulse
btScalar m_maxMotorImpulse
Definition: btHingeConstraint.h:60
btHingeConstraint::setMotorTarget
void setMotorTarget(const btQuaternion &qAinB, btScalar dt)
Definition: btHingeConstraint.cpp:689
btHingeConstraint::m_accLimitImpulse
btScalar m_accLimitImpulse
Definition: btHingeConstraint.h:79
btHingeConstraint::m_rbBFrame
btTransform m_rbBFrame
Definition: btHingeConstraint.h:57
btHingeConstraint::getInfo2NonVirtual
void getInfo2NonVirtual(btConstraintInfo2 *info, const btTransform &transA, const btTransform &transB, const btVector3 &angVelA, const btVector3 &angVelB)
Definition: btHingeConstraint.cpp:384
btHingeConstraint::getInfo2
virtual void getInfo2(btConstraintInfo2 *info)
internal method used by the constraint solver, don't use them directly
Definition: btHingeConstraint.cpp:372
btHingeConstraint::getUpperLimit
btScalar getUpperLimit() const
Definition: btHingeConstraint.h:257
btHingeConstraint::setFrames
void setFrames(const btTransform &frameA, const btTransform &frameB)
Definition: btHingeConstraint.cpp:630
btHingeConstraint::m_accMotorImpulse
btScalar m_accMotorImpulse
Definition: btHingeConstraint.h:89
btHingeConstraint::getLowerLimit
btScalar getLowerLimit() const
Definition: btHingeConstraint.h:248
btHingeConstraint::m_jac
btJacobianEntry m_jac[3]
Definition: btHingeConstraint.h:53
btHingeConstraint::btHingeConstraint
btHingeConstraint(btRigidBody &rbA, btRigidBody &rbB, const btVector3 &pivotInA, const btVector3 &pivotInB, const btVector3 &axisInA, const btVector3 &axisInB, bool useReferenceFrameA=false)
Definition: btHingeConstraint.cpp:30
btHingeConstraint::m_useSolveConstraintObsolete
bool m_useSolveConstraintObsolete
Definition: btHingeConstraint.h:85
btHingeConstraint::m_useOffsetForConstraintFrame
bool m_useOffsetForConstraintFrame
Definition: btHingeConstraint.h:86
btHingeConstraint::m_flags
int m_flags
Definition: btHingeConstraint.h:91
btHingeConstraint::getSolveLimit
int getSolveLimit()
Definition: btHingeConstraint.h:279
btHingeConstraint::updateRHS
void updateRHS(btScalar timeStep)
Definition: btHingeConstraint.cpp:637
btHingeConstraint::m_kHinge
btScalar m_kHinge
Definition: btHingeConstraint.h:77
btHingeConstraint::getParam
virtual btScalar getParam(int num, int axis=-1) const
return the local value of parameter
Definition: btHingeConstraint.cpp:1051
btHingeConstraint::m_stopCFM
btScalar m_stopCFM
Definition: btHingeConstraint.h:94
btHingeConstraint::buildJacobian
virtual void buildJacobian()
internal method used by the constraint solver, don't use them directly
Definition: btHingeConstraint.cpp:204
btHingeConstraint::m_useReferenceFrameA
bool m_useReferenceFrameA
Definition: btHingeConstraint.h:87
btHingeConstraint::m_referenceSign
btScalar m_referenceSign
Definition: btHingeConstraint.h:81
btHingeConstraint::m_limit
btAngularLimit m_limit
Definition: btHingeConstraint.h:63
btHingeConstraint::getInfo1
virtual void getInfo1(btConstraintInfo1 *info)
internal method used by the constraint solver, don't use them directly
Definition: btHingeConstraint.cpp:335
btHingeConstraint::m_motorTargetVelocity
btScalar m_motorTargetVelocity
Definition: btHingeConstraint.h:59
btHingeConstraint::m_normalERP
btScalar m_normalERP
Definition: btHingeConstraint.h:93
btHingeConstraint::m_angularOnly
bool m_angularOnly
Definition: btHingeConstraint.h:83
btHingeConstraint::getInfo2Internal
void getInfo2Internal(btConstraintInfo2 *info, const btTransform &transA, const btTransform &transB, const btVector3 &angVelA, const btVector3 &angVelB)
Definition: btHingeConstraint.cpp:392
btHingeConstraint::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: btHingeConstraint.cpp:1018
btHingeConstraint::getInfo2InternalUsingFrameOffset
void getInfo2InternalUsingFrameOffset(btConstraintInfo2 *info, const btTransform &transA, const btTransform &transB, const btVector3 &angVelA, const btVector3 &angVelB)
Definition: btHingeConstraint.cpp:734
btHingeConstraint::m_hingeAngle
btScalar m_hingeAngle
Definition: btHingeConstraint.h:80
btHingeConstraint::testLimit
void testLimit(const btTransform &transA, const btTransform &transB)
Definition: btHingeConstraint.cpp:657
btHingeConstraint::getRigidBodyA
const btRigidBody & getRigidBodyA() const
Definition: btHingeConstraint.h:123
btHingeConstraint::m_normalCFM
btScalar m_normalCFM
Definition: btHingeConstraint.h:92
btHingeConstraint::getEnableAngularMotor
bool getEnableAngularMotor()
Definition: btHingeConstraint.h:301
btHingeConstraint::getInfo1NonVirtual
void getInfo1NonVirtual(btConstraintInfo1 *info)
Definition: btHingeConstraint.cpp:357
btHingeConstraint::m_rbAFrame
btTransform m_rbAFrame
Definition: btHingeConstraint.h:56
btJacobianEntry
Jacobian entry is an abstraction that allows to describe constraints it can be used in combination wi...
Definition: btJacobianEntry.h:31
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
btQuadWord::getZ
const btScalar & getZ() const
Return the z value.
Definition: btQuadWord.h:103
btQuaternion
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
Definition: btQuaternion.h:50
btQuaternion::getAngle
btScalar getAngle() const
Return the angle [0, 2Pi] of rotation represented by this quaternion.
Definition: btQuaternion.h:468
btQuaternion::inverse
btQuaternion inverse() const
Return the inverse of this quaternion.
Definition: btQuaternion.h:497
btQuaternion::normalize
btQuaternion & normalize()
Normalize the quaternion Such that x^2 + y^2 + z^2 +w^2 = 1.
Definition: btQuaternion.h:385
btRigidBody
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:60
btRigidBody::computeAngularImpulseDenominator
btScalar computeAngularImpulseDenominator(const btVector3 &axis) const
Definition: btRigidBody.h:493
btRigidBody::getInvMass
btScalar getInvMass() const
Definition: btRigidBody.h:263
btRigidBody::getInvInertiaDiagLocal
const btVector3 & getInvInertiaDiagLocal() const
Definition: btRigidBody.h:289
btRigidBody::getCenterOfMassTransform
const btTransform & getCenterOfMassTransform() const
Definition: btRigidBody.h:429
btRigidBody::getAngularVelocity
const btVector3 & getAngularVelocity() const
Definition: btRigidBody.h:437
btRigidBody::getCenterOfMassPosition
const btVector3 & getCenterOfMassPosition() const
Definition: btRigidBody.h:423
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::getRotation
btQuaternion getRotation() const
Return a quaternion representing the rotation.
Definition: btTransform.h:119
btTransform::getOrigin
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:114
btTypedConstraint
TypedConstraint is the baseclass for Bullet constraints and vehicles.
Definition: btTypedConstraint.h:76
btTypedConstraint::m_appliedImpulse
btScalar m_appliedImpulse
Definition: btTypedConstraint.h:99
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::m_rbA
btRigidBody & m_rbA
Definition: btTypedConstraint.h:97
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::getZ
const btScalar & getZ() const
Return the z value.
Definition: btVector3.h:565
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::getSkewSymmetricMatrix
void getSkewSymmetricMatrix(btVector3 *v0, btVector3 *v1, btVector3 *v2) const
Definition: btVector3.h:648
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
btVector3::length2
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:251
btVector3::getY
const btScalar & getY() const
Return the y value.
Definition: btVector3.h:563
btVector3::normalize
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:303
btVector3::getX
const btScalar & getX() const
Return the x value.
Definition: btVector3.h:561
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