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