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