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 /*
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 int powered = 0;
560 if(getEnableAngularMotor())
561 {
562 powered = 1;
563 }
564 if(limit || powered)
565 {
566 nrow++;
567 srow = nrow * info->rowskip;
568 info->m_J1angularAxis[srow+0] = ax1[0];
569 info->m_J1angularAxis[srow+1] = ax1[1];
570 info->m_J1angularAxis[srow+2] = ax1[2];
571
572 info->m_J2angularAxis[srow+0] = -ax1[0];
573 info->m_J2angularAxis[srow+1] = -ax1[1];
574 info->m_J2angularAxis[srow+2] = -ax1[2];
575
576 btScalar lostop = getLowerLimit();
577 btScalar histop = getUpperLimit();
578 if(limit && (lostop == histop))
579 { // the joint motor is ineffective
580 powered = 0;
581 }
582 info->m_constraintError[srow] = btScalar(0.0f);
583 btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : normalErp;
584 if(powered)
585 {
586 if(m_flags & BT_HINGE_FLAGS_CFM_NORM)
587 {
588 info->cfm[srow] = m_normalCFM;
589 }
590 btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP);
591 info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
592 info->m_lowerLimit[srow] = - m_maxMotorImpulse;
593 info->m_upperLimit[srow] = m_maxMotorImpulse;
594 }
595 if(limit)
596 {
597 k = info->fps * currERP;
598 info->m_constraintError[srow] += k * limit_err;
599 if(m_flags & BT_HINGE_FLAGS_CFM_STOP)
600 {
601 info->cfm[srow] = m_stopCFM;
602 }
603 if(lostop == histop)
604 {
605 // limited low and high simultaneously
606 info->m_lowerLimit[srow] = -SIMD_INFINITY;
607 info->m_upperLimit[srow] = SIMD_INFINITY;
608 }
609 else if(limit == 1)
610 { // low limit
611 info->m_lowerLimit[srow] = 0;
612 info->m_upperLimit[srow] = SIMD_INFINITY;
613 }
614 else
615 { // high limit
616 info->m_lowerLimit[srow] = -SIMD_INFINITY;
617 info->m_upperLimit[srow] = 0;
618 }
619 // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
620 #ifdef _BT_USE_CENTER_LIMIT_
621 btScalar bounce = m_limit.getRelaxationFactor();
622 #else
623 btScalar bounce = m_relaxationFactor;
624 #endif
625 if(bounce > btScalar(0.0))
626 {
627 btScalar vel = angVelA.dot(ax1);
628 vel -= angVelB.dot(ax1);
629 // only apply bounce if the velocity is incoming, and if the
630 // resulting c[] exceeds what we already have.
631 if(limit == 1)
632 { // low limit
633 if(vel < 0)
634 {
635 btScalar newc = -bounce * vel;
636 if(newc > info->m_constraintError[srow])
637 {
638 info->m_constraintError[srow] = newc;
639 }
640 }
641 }
642 else
643 { // high limit - all those computations are reversed
644 if(vel > 0)
645 {
646 btScalar newc = -bounce * vel;
647 if(newc < info->m_constraintError[srow])
648 {
649 info->m_constraintError[srow] = newc;
650 }
651 }
652 }
653 }
654 #ifdef _BT_USE_CENTER_LIMIT_
655 info->m_constraintError[srow] *= m_limit.getBiasFactor();
656 #else
657 info->m_constraintError[srow] *= m_biasFactor;
658 #endif
659 } // if(limit)
660 } // if angular limit or powered
661 }
662
663
664 void btHingeConstraint::setFrames(const btTransform & frameA, const btTransform & frameB)
665 {
666 m_rbAFrame = frameA;
667 m_rbBFrame = frameB;
668 buildJacobian();
669 }
670
671
672 void btHingeConstraint::updateRHS(btScalar timeStep)
673 {
674 (void)timeStep;
675
676 }
677
678
679
680
681 btScalar btHingeConstraint::getHingeAngle()
682 {
683 return getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
684 }
685
686 btScalar btHingeConstraint::getHingeAngle(const btTransform& transA,const btTransform& transB)
687 {
688 const btVector3 refAxis0 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0);
689 const btVector3 refAxis1 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1);
690 const btVector3 swingAxis = transB.getBasis() * m_rbBFrame.getBasis().getColumn(1);
691 // btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
692 btScalar angle = btAtan2(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
693 return m_referenceSign * angle;
694 }
695
696
697
698 void btHingeConstraint::testLimit(const btTransform& transA,const btTransform& transB)
699 {
700 // Compute limit information
701 m_hingeAngle = getHingeAngle(transA,transB);
702 #ifdef _BT_USE_CENTER_LIMIT_
703 m_limit.test(m_hingeAngle);
704 #else
705 m_correction = btScalar(0.);
706 m_limitSign = btScalar(0.);
707 m_solveLimit = false;
708 if (m_lowerLimit <= m_upperLimit)
709 {
710 m_hingeAngle = btAdjustAngleToLimits(m_hingeAngle, m_lowerLimit, m_upperLimit);
711 if (m_hingeAngle <= m_lowerLimit)
712 {
713 m_correction = (m_lowerLimit - m_hingeAngle);
714 m_limitSign = 1.0f;
715 m_solveLimit = true;
716 }
717 else if (m_hingeAngle >= m_upperLimit)
718 {
719 m_correction = m_upperLimit - m_hingeAngle;
720 m_limitSign = -1.0f;
721 m_solveLimit = true;
722 }
723 }
724 #endif
725 return;
726 }
727
728
729 static btVector3 vHinge(0, 0, btScalar(1));
730
731 void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt)
732 {
733 // convert target from body to constraint space
734 btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * qAinB * m_rbAFrame.getRotation();
735 qConstraint.normalize();
736
737 // extract "pure" hinge component
738 btVector3 vNoHinge = quatRotate(qConstraint, vHinge); vNoHinge.normalize();
739 btQuaternion qNoHinge = shortestArcQuat(vHinge, vNoHinge);
740 btQuaternion qHinge = qNoHinge.inverse() * qConstraint;
741 qHinge.normalize();
742
743 // compute angular target, clamped to limits
744 btScalar targetAngle = qHinge.getAngle();
745 if (targetAngle > SIMD_PI) // long way around. flip quat and recalculate.
746 {
747 qHinge = -(qHinge);
748 targetAngle = qHinge.getAngle();
749 }
750 if (qHinge.getZ() < 0)
751 targetAngle = -targetAngle;
752
753 setMotorTarget(targetAngle, dt);
754 }
755
756 void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt)
757 {
758 #ifdef _BT_USE_CENTER_LIMIT_
759 m_limit.fit(targetAngle);
760 #else
761 if (m_lowerLimit < m_upperLimit)
762 {
763 if (targetAngle < m_lowerLimit)
764 targetAngle = m_lowerLimit;
765 else if (targetAngle > m_upperLimit)
766 targetAngle = m_upperLimit;
767 }
768 #endif
769 // compute angular velocity
770 btScalar curAngle = getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
771 btScalar dAngle = targetAngle - curAngle;
772 m_motorTargetVelocity = dAngle / dt;
773 }
774
775
776
777 void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
778 {
779 btAssert(!m_useSolveConstraintObsolete);
780 int i, s = info->rowskip;
781 // transforms in world space
782 btTransform trA = transA*m_rbAFrame;
783 btTransform trB = transB*m_rbBFrame;
784 // pivot point
785 // btVector3 pivotAInW = trA.getOrigin();
786 // btVector3 pivotBInW = trB.getOrigin();
787 #if 1
788 // difference between frames in WCS
789 btVector3 ofs = trB.getOrigin() - trA.getOrigin();
790 // now get weight factors depending on masses
791 btScalar miA = getRigidBodyA().getInvMass();
792 btScalar miB = getRigidBodyB().getInvMass();
793 bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
794 btScalar miS = miA + miB;
795 btScalar factA, factB;
796 if(miS > btScalar(0.f))
797 {
798 factA = miB / miS;
799 }
800 else
801 {
802 factA = btScalar(0.5f);
803 }
804 factB = btScalar(1.0f) - factA;
805 // get the desired direction of hinge axis
806 // as weighted sum of Z-orthos of frameA and frameB in WCS
807 btVector3 ax1A = trA.getBasis().getColumn(2);
808 btVector3 ax1B = trB.getBasis().getColumn(2);
809 btVector3 ax1 = ax1A * factA + ax1B * factB;
810 ax1.normalize();
811 // fill first 3 rows
812 // we want: velA + wA x relA == velB + wB x relB
813 btTransform bodyA_trans = transA;
814 btTransform bodyB_trans = transB;
815 int s0 = 0;
816 int s1 = s;
817 int s2 = s * 2;
818 int nrow = 2; // last filled row
819 btVector3 tmpA, tmpB, relA, relB, p, q;
820 // get vector from bodyB to frameB in WCS
821 relB = trB.getOrigin() - bodyB_trans.getOrigin();
822 // get its projection to hinge axis
823 btVector3 projB = ax1 * relB.dot(ax1);
824 // get vector directed from bodyB to hinge axis (and orthogonal to it)
825 btVector3 orthoB = relB - projB;
826 // same for bodyA
827 relA = trA.getOrigin() - bodyA_trans.getOrigin();
828 btVector3 projA = ax1 * relA.dot(ax1);
829 btVector3 orthoA = relA - projA;
830 btVector3 totalDist = projA - projB;
831 // get offset vectors relA and relB
832 relA = orthoA + totalDist * factA;
833 relB = orthoB - totalDist * factB;
834 // now choose average ortho to hinge axis
835 p = orthoB * factA + orthoA * factB;
836 btScalar len2 = p.length2();
837 if(len2 > SIMD_EPSILON)
838 {
839 p /= btSqrt(len2);
840 }
841 else
842 {
843 p = trA.getBasis().getColumn(1);
844 }
845 // make one more ortho
846 q = ax1.cross(p);
847 // fill three rows
848 tmpA = relA.cross(p);
849 tmpB = relB.cross(p);
850 for (i=0; i<3; i++) info->m_J1angularAxis[s0+i] = tmpA[i];
851 for (i=0; i<3; i++) info->m_J2angularAxis[s0+i] = -tmpB[i];
852 tmpA = relA.cross(q);
853 tmpB = relB.cross(q);
854 if(hasStaticBody && getSolveLimit())
855 { // to make constraint between static and dynamic objects more rigid
856 // remove wA (or wB) from equation if angular limit is hit
857 tmpB *= factB;
858 tmpA *= factA;
859 }
860 for (i=0; i<3; i++) info->m_J1angularAxis[s1+i] = tmpA[i];
861 for (i=0; i<3; i++) info->m_J2angularAxis[s1+i] = -tmpB[i];
862 tmpA = relA.cross(ax1);
863 tmpB = relB.cross(ax1);
864 if(hasStaticBody)
865 { // to make constraint between static and dynamic objects more rigid
866 // remove wA (or wB) from equation
867 tmpB *= factB;
868 tmpA *= factA;
869 }
870 for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i];
871 for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i];
872
873 btScalar normalErp = (m_flags & BT_HINGE_FLAGS_ERP_NORM)? m_normalERP : info->erp;
874 btScalar k = info->fps * normalErp;
875
876 if (!m_angularOnly)
877 {
878 for (i=0; i<3; i++) info->m_J1linearAxis[s0+i] = p[i];
879 for (i=0; i<3; i++) info->m_J1linearAxis[s1+i] = q[i];
880 for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = ax1[i];
881
882 for (i=0; i<3; i++) info->m_J2linearAxis[s0+i] = -p[i];
883 for (i=0; i<3; i++) info->m_J2linearAxis[s1+i] = -q[i];
884 for (i=0; i<3; i++) info->m_J2linearAxis[s2+i] = -ax1[i];
885
886 // compute three elements of right hand side
887
888 btScalar rhs = k * p.dot(ofs);
889 info->m_constraintError[s0] = rhs;
890 rhs = k * q.dot(ofs);
891 info->m_constraintError[s1] = rhs;
892 rhs = k * ax1.dot(ofs);
893 info->m_constraintError[s2] = rhs;
894 }
895 // the hinge axis should be the only unconstrained
896 // rotational axis, the angular velocity of the two bodies perpendicular to
897 // the hinge axis should be equal. thus the constraint equations are
898 // p*w1 - p*w2 = 0
899 // q*w1 - q*w2 = 0
900 // where p and q are unit vectors normal to the hinge axis, and w1 and w2
901 // are the angular velocity vectors of the two bodies.
902 int s3 = 3 * s;
903 int s4 = 4 * s;
904 info->m_J1angularAxis[s3 + 0] = p[0];
905 info->m_J1angularAxis[s3 + 1] = p[1];
906 info->m_J1angularAxis[s3 + 2] = p[2];
907 info->m_J1angularAxis[s4 + 0] = q[0];
908 info->m_J1angularAxis[s4 + 1] = q[1];
909 info->m_J1angularAxis[s4 + 2] = q[2];
910
911 info->m_J2angularAxis[s3 + 0] = -p[0];
912 info->m_J2angularAxis[s3 + 1] = -p[1];
913 info->m_J2angularAxis[s3 + 2] = -p[2];
914 info->m_J2angularAxis[s4 + 0] = -q[0];
915 info->m_J2angularAxis[s4 + 1] = -q[1];
916 info->m_J2angularAxis[s4 + 2] = -q[2];
917 // compute the right hand side of the constraint equation. set relative
918 // body velocities along p and q to bring the hinge back into alignment.
919 // if ax1A,ax1B are the unit length hinge axes as computed from bodyA and
920 // bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2).
921 // if "theta" is the angle between ax1 and ax2, we need an angular velocity
922 // along u to cover angle erp*theta in one step :
923 // |angular_velocity| = angle/time = erp*theta / stepsize
924 // = (erp*fps) * theta
925 // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
926 // = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
927 // ...as ax1 and ax2 are unit length. if theta is smallish,
928 // theta ~= sin(theta), so
929 // angular_velocity = (erp*fps) * (ax1 x ax2)
930 // ax1 x ax2 is in the plane space of ax1, so we project the angular
931 // velocity to p and q to find the right hand side.
932 k = info->fps * normalErp;//??
933
934 btVector3 u = ax1A.cross(ax1B);
935 info->m_constraintError[s3] = k * u.dot(p);
936 info->m_constraintError[s4] = k * u.dot(q);
937 #endif
938 // check angular limits
939 nrow = 4; // last filled row
940 int srow;
941 btScalar limit_err = btScalar(0.0);
942 int limit = 0;
943 if(getSolveLimit())
944 {
945 #ifdef _BT_USE_CENTER_LIMIT_
946 limit_err = m_limit.getCorrection() * m_referenceSign;
947 #else
948 limit_err = m_correction * m_referenceSign;
949 #endif
950 limit = (limit_err > btScalar(0.0)) ? 1 : 2;
951
952 }
953 // if the hinge has joint limits or motor, add in the extra row
954 int powered = 0;
955 if(getEnableAngularMotor())
956 {
957 powered = 1;
958 }
959 if(limit || powered)
960 {
961 nrow++;
962 srow = nrow * info->rowskip;
963 info->m_J1angularAxis[srow+0] = ax1[0];
964 info->m_J1angularAxis[srow+1] = ax1[1];
965 info->m_J1angularAxis[srow+2] = ax1[2];
966
967 info->m_J2angularAxis[srow+0] = -ax1[0];
968 info->m_J2angularAxis[srow+1] = -ax1[1];
969 info->m_J2angularAxis[srow+2] = -ax1[2];
970
971 btScalar lostop = getLowerLimit();
972 btScalar histop = getUpperLimit();
973 if(limit && (lostop == histop))
974 { // the joint motor is ineffective
975 powered = 0;
976 }
977 info->m_constraintError[srow] = btScalar(0.0f);
978 btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : normalErp;
979 if(powered)
980 {
981 if(m_flags & BT_HINGE_FLAGS_CFM_NORM)
982 {
983 info->cfm[srow] = m_normalCFM;
984 }
985 btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP);
986 info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
987 info->m_lowerLimit[srow] = - m_maxMotorImpulse;
988 info->m_upperLimit[srow] = m_maxMotorImpulse;
989 }
990 if(limit)
991 {
992 k = info->fps * currERP;
993 info->m_constraintError[srow] += k * limit_err;
994 if(m_flags & BT_HINGE_FLAGS_CFM_STOP)
995 {
996 info->cfm[srow] = m_stopCFM;
997 }
998 if(lostop == histop)
999 {
1000 // limited low and high simultaneously
1001 info->m_lowerLimit[srow] = -SIMD_INFINITY;
1002 info->m_upperLimit[srow] = SIMD_INFINITY;
1003 }
1004 else if(limit == 1)
1005 { // low limit
1006 info->m_lowerLimit[srow] = 0;
1007 info->m_upperLimit[srow] = SIMD_INFINITY;
1008 }
1009 else
1010 { // high limit
1011 info->m_lowerLimit[srow] = -SIMD_INFINITY;
1012 info->m_upperLimit[srow] = 0;
1013 }
1014 // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
1015 #ifdef _BT_USE_CENTER_LIMIT_
1016 btScalar bounce = m_limit.getRelaxationFactor();
1017 #else
1018 btScalar bounce = m_relaxationFactor;
1019 #endif
1020 if(bounce > btScalar(0.0))
1021 {
1022 btScalar vel = angVelA.dot(ax1);
1023 vel -= angVelB.dot(ax1);
1024 // only apply bounce if the velocity is incoming, and if the
1025 // resulting c[] exceeds what we already have.
1026 if(limit == 1)
1027 { // low limit
1028 if(vel < 0)
1029 {
1030 btScalar newc = -bounce * vel;
1031 if(newc > info->m_constraintError[srow])
1032 {
1033 info->m_constraintError[srow] = newc;
1034 }
1035 }
1036 }
1037 else
1038 { // high limit - all those computations are reversed
1039 if(vel > 0)
1040 {
1041 btScalar newc = -bounce * vel;
1042 if(newc < info->m_constraintError[srow])
1043 {
1044 info->m_constraintError[srow] = newc;
1045 }
1046 }
1047 }
1048 }
1049 #ifdef _BT_USE_CENTER_LIMIT_
1050 info->m_constraintError[srow] *= m_limit.getBiasFactor();
1051 #else
1052 info->m_constraintError[srow] *= m_biasFactor;
1053 #endif
1054 } // if(limit)
1055 } // if angular limit or powered
1056 }
1057
1058
1061 void btHingeConstraint::setParam(int num, btScalar value, int axis)
1062 {
1063 if((axis == -1) || (axis == 5))
1064 {
1065 switch(num)
1066 {
1067 case BT_CONSTRAINT_STOP_ERP :
1068 m_stopERP = value;
1069 m_flags |= BT_HINGE_FLAGS_ERP_STOP;
1070 break;
1071 case BT_CONSTRAINT_STOP_CFM :
1072 m_stopCFM = value;
1073 m_flags |= BT_HINGE_FLAGS_CFM_STOP;
1074 break;
1075 case BT_CONSTRAINT_CFM :
1076 m_normalCFM = value;
1077 m_flags |= BT_HINGE_FLAGS_CFM_NORM;
1078 break;
1079 case BT_CONSTRAINT_ERP:
1080 m_normalERP = value;
1081 m_flags |= BT_HINGE_FLAGS_ERP_NORM;
1082 break;
1083 default :
1084 btAssertConstrParams(0);
1085 }
1086 }
1087 else
1088 {
1089 btAssertConstrParams(0);
1090 }
1091 }
1092
1094 btScalar btHingeConstraint::getParam(int num, int axis) const
1095 {
1096 btScalar retVal = 0;
1097 if((axis == -1) || (axis == 5))
1098 {
1099 switch(num)
1100 {
1101 case BT_CONSTRAINT_STOP_ERP :
1102 btAssertConstrParams(m_flags & BT_HINGE_FLAGS_ERP_STOP);
1103 retVal = m_stopERP;
1104 break;
1105 case BT_CONSTRAINT_STOP_CFM :
1106 btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_STOP);
1107 retVal = m_stopCFM;
1108 break;
1109 case BT_CONSTRAINT_CFM :
1110 btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_NORM);
1111 retVal = m_normalCFM;
1112 break;
1113 case BT_CONSTRAINT_ERP:
1114 btAssertConstrParams(m_flags & BT_HINGE_FLAGS_ERP_NORM);
1115 retVal = m_normalERP;
1116 break;
1117 default :
1118 btAssertConstrParams(0);
1119 }
1120 }
1121 else
1122 {
1123 btAssertConstrParams(0);
1124 }
1125 return retVal;
1126 }
1127
1128
btTypedConstraint::btConstraintInfo2::m_constraintError
btScalar * m_constraintError
Definition: btTypedConstraint.h:142
btRigidBody::getInvMass
btScalar getInvMass() const
Definition: btRigidBody.h:270
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:494
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
btQuaternion::getAngle
btScalar getAngle() const
Return the angle of rotation represented by this quaternion.
Definition: btQuaternion.h:415
btTypedConstraint::btConstraintInfo2::m_J2angularAxis
btScalar * m_J2angularAxis
Definition: btTypedConstraint.h:134
btTypedConstraint::btConstraintInfo2::m_upperLimit
btScalar * m_upperLimit
Definition: btTypedConstraint.h:145
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:1061
btRigidBody::computeAngularImpulseDenominator
btScalar computeAngularImpulseDenominator(const btVector3 &axis) const
Definition: btRigidBody.h:412
btTypedConstraint::m_rbA
btRigidBody & m_rbA
Definition: btTypedConstraint.h:101
btTransform::getRotation
btQuaternion getRotation() const
Return a quaternion representing the rotation.
Definition: btTransform.h:122
btVector3::setValue
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:640
btJacobianEntry
Jacobian entry is an abstraction that allows to describe constraints it can be used in combination wi...
Definition: btJacobianEntry.h:30
btTypedConstraint::btConstraintInfo2
Definition: btTypedConstraint.h:125
btHingeAccumulatedAngleConstraint::getInfo1
virtual void getInfo1(btConstraintInfo1 *info)
internal method used by the constraint solver, don't use them directly
Definition: btHingeConstraint.cpp:343
btHingeConstraint::getInfo1
virtual void getInfo1(btConstraintInfo1 *info)
internal method used by the constraint solver, don't use them directly
Definition: btHingeConstraint.cpp:354
btHingeAccumulatedAngleConstraint::setAccumulatedHingeAngle
void setAccumulatedHingeAngle(btScalar accAngle)
Definition: btHingeConstraint.cpp:338
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:1272
btSqrt
btScalar btSqrt(btScalar y)
Definition: btScalar.h:418
btTypedConstraint::btConstraintInfo1::m_numConstraintRows
int m_numConstraintRows
Definition: btTypedConstraint.h:120
btAssert
#define btAssert(x)
Definition: btScalar.h:113
btTypedConstraint::btConstraintInfo2::cfm
btScalar * cfm
Definition: btTypedConstraint.h:142
btTypedConstraint::btConstraintInfo2::m_J1angularAxis
btScalar * m_J1angularAxis
Definition: btTypedConstraint.h:134
BT_CONSTRAINT_CFM
Definition: btTypedConstraint.h:56
btMatrix3x3::getColumn
btVector3 getColumn(int i) const
Get a column of the matrix as a vector.
Definition: btMatrix3x3.h:134
btHingeConstraint::getRigidBodyA
const btRigidBody & getRigidBodyA() const
Definition: btHingeConstraint.h:132
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::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
btVector3::normalize
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:297
quatRotate
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
Definition: btQuaternion.h:866
btVector3::getZ
const btScalar & getZ() const
Return the z value.
Definition: btVector3.h:565
BT_CONSTRAINT_STOP_ERP
Definition: btTypedConstraint.h:55
btHingeAccumulatedAngleConstraint::m_accumulatedAngle
btScalar m_accumulatedAngle
Definition: btHingeConstraint.h:344
vHinge
static btVector3 vHinge(0, 0, btScalar(1))
btShortestAngularDistance
static btScalar btShortestAngularDistance(btScalar accAngle, btScalar curAngle)
Definition: btHingeConstraint.cpp:311
SIMD_PI
#define SIMD_PI
Definition: btScalar.h:476
SIMD_INFINITY
#define SIMD_INFINITY
Definition: btScalar.h:495
BT_CONSTRAINT_STOP_CFM
Definition: btTypedConstraint.h:57
btTypedConstraint::btConstraintInfo2::m_J1linearAxis
btScalar * m_J1linearAxis
Definition: btTypedConstraint.h:134
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
btHingeConstraint::getUpperLimit
btScalar getUpperLimit() const
Definition: btHingeConstraint.h:239
shortestArcQuat
btQuaternion shortestArcQuat(const btVector3 &v0, const btVector3 &v1)
Definition: btQuaternion.h:880
_BT_USE_CENTER_LIMIT_
#define _BT_USE_CENTER_LIMIT_
Definition: btHingeConstraint.h:21
btRigidBody::getCenterOfMassTransform
const btTransform & getCenterOfMassTransform() const
Definition: btRigidBody.h:356
btQuaternion::normalize
btQuaternion & normalize()
Normalize the quaternion Such that x^2 + y^2 + z^2 +w^2 = 1.
Definition: btQuaternion.h:332
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
btHingeConstraint::updateRHS
void updateRHS(btScalar timeStep)
Definition: btHingeConstraint.cpp:672
btTypedConstraint::btConstraintInfo2::erp
btScalar erp
Definition: btTypedConstraint.h:128
btHingeConstraint::getInfo2InternalUsingFrameOffset
void getInfo2InternalUsingFrameOffset(btConstraintInfo2 *info, const btTransform &transA, const btTransform &transB, const btVector3 &angVelA, const btVector3 &angVelB)
Definition: btHingeConstraint.cpp:777
btAtan2
btScalar btAtan2(btScalar x, btScalar y)
Definition: btScalar.h:468
btAngularLimit::getBiasFactor
btScalar getBiasFactor() const
Returns limit's bias factor.
Definition: btTypedConstraint.h:495
btRigidBody::getAngularVelocity
const btVector3 & getAngularVelocity() const
Definition: btRigidBody.h:362
btVector3::cross
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:377
btVector3::getY
const btScalar & getY() const
Return the y value.
Definition: btVector3.h:563
btHingeConstraint::getParam
virtual btScalar getParam(int num, int axis=-1) const
return the local value of parameter
Definition: btHingeConstraint.cpp:1094
btTypedConstraint::btConstraintInfo2::m_lowerLimit
btScalar * m_lowerLimit
Definition: btTypedConstraint.h:145
btTransform::getBasis
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:112
btVector3::getX
const btScalar & getX() const
Return the x value.
Definition: btVector3.h:561
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::getLowerLimit
btScalar getLowerLimit() const
Definition: btHingeConstraint.h:230
btHingeConstraint::getInfo1NonVirtual
void getInfo1NonVirtual(btConstraintInfo1 *info)
Definition: btHingeConstraint.cpp:379
btHingeConstraint::getSolveLimit
int getSolveLimit()
Definition: btHingeConstraint.h:263
btRigidBody::getCenterOfMassPosition
const btVector3 & getCenterOfMassPosition() const
Definition: btRigidBody.h:351
btQuaternion::inverse
btQuaternion inverse() const
Return the inverse of this quaternion.
Definition: btQuaternion.h:446
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't use them directly
Definition: btHingeConstraint.cpp:215
btAdjustAngleToLimits
btScalar btAdjustAngleToLimits(btScalar angleInRadians, btScalar angleLowerLimitInRadians, btScalar angleUpperLimitInRadians)
Definition: btTypedConstraint.h:345
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
btVector3::length2
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:257
btNormalizeAngle
btScalar btNormalizeAngle(btScalar angleInRadians)
Definition: btScalar.h:724
btTypedConstraint::btConstraintInfo2::rowskip
int rowskip
Definition: btTypedConstraint.h:137
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:102
btHingeConstraint::getEnableAngularMotor
bool getEnableAngularMotor()
Definition: btHingeConstraint.h:285
btVector3::normalized
btVector3 normalized() const
Return a normalized version of this vector.
Definition: btVector3.h:952
btTypedConstraint::btConstraintInfo2::m_J2linearAxis
btScalar * m_J2linearAxis
Definition: btTypedConstraint.h:134
btAngularLimit::getRelaxationFactor
btScalar getRelaxationFactor() const
Returns limit's relaxation factor.
Definition: btTypedConstraint.h:501
btFmod
btScalar btFmod(btScalar x, btScalar y)
Definition: btScalar.h:472
btTypedConstraint
TypedConstraint is the baseclass for Bullet constraints and vehicles.
Definition: btTypedConstraint.h:77
btTypedConstraint::getMotorFactor
btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact)
internal method used by the constraint solver, don't use them directly
Definition: btTypedConstraint.cpp:60
btMatrix3x3::transpose
btMatrix3x3 transpose() const
Return the transpose of the matrix.
Definition: btMatrix3x3.h:1001
btTypedConstraint::btConstraintInfo1::nub
int nub
Definition: btTypedConstraint.h:120
btHingeConstraint::getInfo2
virtual void getInfo2(btConstraintInfo2 *info)
internal method used by the constraint solver, don'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:128
btHingeConstraint::m_angularOnly
bool m_angularOnly
Definition: btHingeConstraint.h:88
BT_CONSTRAINT_ERP
Definition: btTypedConstraint.h:54
btAngularLimit::fit
void fit(btScalar &angle) const
Checks given angle against limit.
Definition: btTypedConstraint.cpp:195
btTypedConstraint::m_appliedImpulse
btScalar m_appliedImpulse
Definition: btTypedConstraint.h:103
btNormalizeAnglePositive
static btScalar btNormalizeAnglePositive(btScalar angle)
Definition: btHingeConstraint.cpp:304
btRigidBody::getInvInertiaDiagLocal
const btVector3 & getInvInertiaDiagLocal() const
Definition: btRigidBody.h:294
btAngularLimit::getCorrection
btScalar getCorrection() const
Returns correction value evaluated when test() was invoked.
Definition: btTypedConstraint.h:507
btHingeConstraint::setFrames
void setFrames(const btTransform &frameA, const btTransform &frameB)
Definition: btHingeConstraint.cpp:664
btHingeConstraint::setMotorTarget
void setMotorTarget(const btQuaternion &qAinB, btScalar dt)
Definition: btHingeConstraint.cpp:731
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:48
HINGE_CONSTRAINT_TYPE
Definition: btTypedConstraint.h:39
btHingeConstraint::getHingeAngle
btScalar getHingeAngle()
The getHingeAngle gives the hinge angle in range [-PI,PI].
Definition: btHingeConstraint.cpp:681
btHingeConstraint::getRigidBodyB
const btRigidBody & getRigidBodyB() const
Definition: btHingeConstraint.h:136
btHingeConstraint::m_limit
btAngularLimit m_limit
Definition: btHingeConstraint.h:67
btAssertConstrParams
#define btAssertConstrParams(_par)
Definition: btTypedConstraint.h:61
btSolverBody.h
btTypedConstraint::btConstraintInfo1
Definition: btTypedConstraint.h:119
btHingeAccumulatedAngleConstraint::getAccumulatedHingeAngle
btScalar getAccumulatedHingeAngle()
Definition: btHingeConstraint.cpp:332
btHingeConstraint::testLimit
void testLimit(const btTransform &transA, const btTransform &transB)
Definition: btHingeConstraint.cpp:698
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
btQuadWord::getZ
const btScalar & getZ() const
Return the z value.
Definition: btQuadWord.h:106
btVector3::getSkewSymmetricMatrix
void getSkewSymmetricMatrix(btVector3 *v0, btVector3 *v1, btVector3 *v2) const
Definition: btVector3.h:648
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:278
btFabs
btScalar btFabs(btScalar x)
Definition: btScalar.h:449