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

Bullet Collision Detection & Physics Library

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