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_J2linearAxis[i*skip]=0;
373  info->m_J2linearAxis[i*skip+1]=0;
374  info->m_J2linearAxis[i*skip+2]=0;
375 
376  info->m_J2angularAxis[i*skip]=0;
377  info->m_J2angularAxis[i*skip+1]=0;
378  info->m_J2angularAxis[i*skip+2]=0;
379 
380  info->m_constraintError[i*skip]=0.f;
381  }
382  }
383 #endif //#if 0
384  // linear (all fixed)
385 
386  if (!m_angularOnly)
387  {
388  info->m_J1linearAxis[0] = 1;
389  info->m_J1linearAxis[skip + 1] = 1;
390  info->m_J1linearAxis[2 * skip + 2] = 1;
391 
392  info->m_J2linearAxis[0] = -1;
393  info->m_J2linearAxis[skip + 1] = -1;
394  info->m_J2linearAxis[2 * skip + 2] = -1;
395  }
396 
397 
398 
399 
400  btVector3 a1 = pivotAInW - transA.getOrigin();
401  {
402  btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
403  btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + skip);
404  btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * skip);
405  btVector3 a1neg = -a1;
406  a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
407  }
408  btVector3 a2 = pivotBInW - transB.getOrigin();
409  {
410  btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
411  btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + skip);
412  btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * skip);
413  a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
414  }
415  // linear RHS
416  btScalar k = info->fps * info->erp;
417  if (!m_angularOnly)
418  {
419  for(i = 0; i < 3; i++)
420  {
421  info->m_constraintError[i * skip] = k * (pivotBInW[i] - pivotAInW[i]);
422  }
423  }
424  // make rotations around X and Y equal
425  // the hinge axis should be the only unconstrained
426  // rotational axis, the angular velocity of the two bodies perpendicular to
427  // the hinge axis should be equal. thus the constraint equations are
428  // p*w1 - p*w2 = 0
429  // q*w1 - q*w2 = 0
430  // where p and q are unit vectors normal to the hinge axis, and w1 and w2
431  // are the angular velocity vectors of the two bodies.
432  // get hinge axis (Z)
433  btVector3 ax1 = trA.getBasis().getColumn(2);
434  // get 2 orthos to hinge axis (X, Y)
435  btVector3 p = trA.getBasis().getColumn(0);
436  btVector3 q = trA.getBasis().getColumn(1);
437  // set the two hinge angular rows
438  int s3 = 3 * info->rowskip;
439  int s4 = 4 * info->rowskip;
440 
441  info->m_J1angularAxis[s3 + 0] = p[0];
442  info->m_J1angularAxis[s3 + 1] = p[1];
443  info->m_J1angularAxis[s3 + 2] = p[2];
444  info->m_J1angularAxis[s4 + 0] = q[0];
445  info->m_J1angularAxis[s4 + 1] = q[1];
446  info->m_J1angularAxis[s4 + 2] = q[2];
447 
448  info->m_J2angularAxis[s3 + 0] = -p[0];
449  info->m_J2angularAxis[s3 + 1] = -p[1];
450  info->m_J2angularAxis[s3 + 2] = -p[2];
451  info->m_J2angularAxis[s4 + 0] = -q[0];
452  info->m_J2angularAxis[s4 + 1] = -q[1];
453  info->m_J2angularAxis[s4 + 2] = -q[2];
454  // compute the right hand side of the constraint equation. set relative
455  // body velocities along p and q to bring the hinge back into alignment.
456  // if ax1,ax2 are the unit length hinge axes as computed from body1 and
457  // body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
458  // if `theta' is the angle between ax1 and ax2, we need an angular velocity
459  // along u to cover angle erp*theta in one step :
460  // |angular_velocity| = angle/time = erp*theta / stepsize
461  // = (erp*fps) * theta
462  // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
463  // = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
464  // ...as ax1 and ax2 are unit length. if theta is smallish,
465  // theta ~= sin(theta), so
466  // angular_velocity = (erp*fps) * (ax1 x ax2)
467  // ax1 x ax2 is in the plane space of ax1, so we project the angular
468  // velocity to p and q to find the right hand side.
469  btVector3 ax2 = trB.getBasis().getColumn(2);
470  btVector3 u = ax1.cross(ax2);
471  info->m_constraintError[s3] = k * u.dot(p);
472  info->m_constraintError[s4] = k * u.dot(q);
473  // check angular limits
474  int nrow = 4; // last filled row
475  int srow;
476  btScalar limit_err = btScalar(0.0);
477  int limit = 0;
478  if(getSolveLimit())
479  {
480 #ifdef _BT_USE_CENTER_LIMIT_
481  limit_err = m_limit.getCorrection() * m_referenceSign;
482 #else
483  limit_err = m_correction * m_referenceSign;
484 #endif
485  limit = (limit_err > btScalar(0.0)) ? 1 : 2;
486 
487  }
488  // if the hinge has joint limits or motor, add in the extra row
489  int powered = 0;
490  if(getEnableAngularMotor())
491  {
492  powered = 1;
493  }
494  if(limit || powered)
495  {
496  nrow++;
497  srow = nrow * info->rowskip;
498  info->m_J1angularAxis[srow+0] = ax1[0];
499  info->m_J1angularAxis[srow+1] = ax1[1];
500  info->m_J1angularAxis[srow+2] = ax1[2];
501 
502  info->m_J2angularAxis[srow+0] = -ax1[0];
503  info->m_J2angularAxis[srow+1] = -ax1[1];
504  info->m_J2angularAxis[srow+2] = -ax1[2];
505 
506  btScalar lostop = getLowerLimit();
507  btScalar histop = getUpperLimit();
508  if(limit && (lostop == histop))
509  { // the joint motor is ineffective
510  powered = 0;
511  }
512  info->m_constraintError[srow] = btScalar(0.0f);
513  btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp;
514  if(powered)
515  {
516  if(m_flags & BT_HINGE_FLAGS_CFM_NORM)
517  {
518  info->cfm[srow] = m_normalCFM;
519  }
520  btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP);
521  info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
522  info->m_lowerLimit[srow] = - m_maxMotorImpulse;
523  info->m_upperLimit[srow] = m_maxMotorImpulse;
524  }
525  if(limit)
526  {
527  k = info->fps * currERP;
528  info->m_constraintError[srow] += k * limit_err;
529  if(m_flags & BT_HINGE_FLAGS_CFM_STOP)
530  {
531  info->cfm[srow] = m_stopCFM;
532  }
533  if(lostop == histop)
534  {
535  // limited low and high simultaneously
536  info->m_lowerLimit[srow] = -SIMD_INFINITY;
537  info->m_upperLimit[srow] = SIMD_INFINITY;
538  }
539  else if(limit == 1)
540  { // low limit
541  info->m_lowerLimit[srow] = 0;
542  info->m_upperLimit[srow] = SIMD_INFINITY;
543  }
544  else
545  { // high limit
546  info->m_lowerLimit[srow] = -SIMD_INFINITY;
547  info->m_upperLimit[srow] = 0;
548  }
549  // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
550 #ifdef _BT_USE_CENTER_LIMIT_
551  btScalar bounce = m_limit.getRelaxationFactor();
552 #else
553  btScalar bounce = m_relaxationFactor;
554 #endif
555  if(bounce > btScalar(0.0))
556  {
557  btScalar vel = angVelA.dot(ax1);
558  vel -= angVelB.dot(ax1);
559  // only apply bounce if the velocity is incoming, and if the
560  // resulting c[] exceeds what we already have.
561  if(limit == 1)
562  { // low limit
563  if(vel < 0)
564  {
565  btScalar newc = -bounce * vel;
566  if(newc > info->m_constraintError[srow])
567  {
568  info->m_constraintError[srow] = newc;
569  }
570  }
571  }
572  else
573  { // high limit - all those computations are reversed
574  if(vel > 0)
575  {
576  btScalar newc = -bounce * vel;
577  if(newc < info->m_constraintError[srow])
578  {
579  info->m_constraintError[srow] = newc;
580  }
581  }
582  }
583  }
584 #ifdef _BT_USE_CENTER_LIMIT_
585  info->m_constraintError[srow] *= m_limit.getBiasFactor();
586 #else
587  info->m_constraintError[srow] *= m_biasFactor;
588 #endif
589  } // if(limit)
590  } // if angular limit or powered
591 }
592 
593 
594 void btHingeConstraint::setFrames(const btTransform & frameA, const btTransform & frameB)
595 {
596  m_rbAFrame = frameA;
597  m_rbBFrame = frameB;
598  buildJacobian();
599 }
600 
601 
602 void btHingeConstraint::updateRHS(btScalar timeStep)
603 {
604  (void)timeStep;
605 
606 }
607 
608 
609 btScalar btHingeConstraint::getHingeAngle()
610 {
611  return getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
612 }
613 
614 btScalar btHingeConstraint::getHingeAngle(const btTransform& transA,const btTransform& transB)
615 {
616  const btVector3 refAxis0 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0);
617  const btVector3 refAxis1 = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1);
618  const btVector3 swingAxis = transB.getBasis() * m_rbBFrame.getBasis().getColumn(1);
619 // btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
620  btScalar angle = btAtan2(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
621  return m_referenceSign * angle;
622 }
623 
624 
625 
626 void btHingeConstraint::testLimit(const btTransform& transA,const btTransform& transB)
627 {
628  // Compute limit information
629  m_hingeAngle = getHingeAngle(transA,transB);
630 #ifdef _BT_USE_CENTER_LIMIT_
631  m_limit.test(m_hingeAngle);
632 #else
633  m_correction = btScalar(0.);
634  m_limitSign = btScalar(0.);
635  m_solveLimit = false;
636  if (m_lowerLimit <= m_upperLimit)
637  {
638  m_hingeAngle = btAdjustAngleToLimits(m_hingeAngle, m_lowerLimit, m_upperLimit);
639  if (m_hingeAngle <= m_lowerLimit)
640  {
641  m_correction = (m_lowerLimit - m_hingeAngle);
642  m_limitSign = 1.0f;
643  m_solveLimit = true;
644  }
645  else if (m_hingeAngle >= m_upperLimit)
646  {
647  m_correction = m_upperLimit - m_hingeAngle;
648  m_limitSign = -1.0f;
649  m_solveLimit = true;
650  }
651  }
652 #endif
653  return;
654 }
655 
656 
657 static btVector3 vHinge(0, 0, btScalar(1));
658 
659 void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt)
660 {
661  // convert target from body to constraint space
662  btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * qAinB * m_rbAFrame.getRotation();
663  qConstraint.normalize();
664 
665  // extract "pure" hinge component
666  btVector3 vNoHinge = quatRotate(qConstraint, vHinge); vNoHinge.normalize();
667  btQuaternion qNoHinge = shortestArcQuat(vHinge, vNoHinge);
668  btQuaternion qHinge = qNoHinge.inverse() * qConstraint;
669  qHinge.normalize();
670 
671  // compute angular target, clamped to limits
672  btScalar targetAngle = qHinge.getAngle();
673  if (targetAngle > SIMD_PI) // long way around. flip quat and recalculate.
674  {
675  qHinge = -(qHinge);
676  targetAngle = qHinge.getAngle();
677  }
678  if (qHinge.getZ() < 0)
679  targetAngle = -targetAngle;
680 
681  setMotorTarget(targetAngle, dt);
682 }
683 
684 void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt)
685 {
686 #ifdef _BT_USE_CENTER_LIMIT_
687  m_limit.fit(targetAngle);
688 #else
689  if (m_lowerLimit < m_upperLimit)
690  {
691  if (targetAngle < m_lowerLimit)
692  targetAngle = m_lowerLimit;
693  else if (targetAngle > m_upperLimit)
694  targetAngle = m_upperLimit;
695  }
696 #endif
697  // compute angular velocity
698  btScalar curAngle = getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
699  btScalar dAngle = targetAngle - curAngle;
700  m_motorTargetVelocity = dAngle / dt;
701 }
702 
703 
704 
705 void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
706 {
707  btAssert(!m_useSolveConstraintObsolete);
708  int i, s = info->rowskip;
709  // transforms in world space
710  btTransform trA = transA*m_rbAFrame;
711  btTransform trB = transB*m_rbBFrame;
712  // pivot point
713 // btVector3 pivotAInW = trA.getOrigin();
714 // btVector3 pivotBInW = trB.getOrigin();
715 #if 1
716  // difference between frames in WCS
717  btVector3 ofs = trB.getOrigin() - trA.getOrigin();
718  // now get weight factors depending on masses
719  btScalar miA = getRigidBodyA().getInvMass();
720  btScalar miB = getRigidBodyB().getInvMass();
721  bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
722  btScalar miS = miA + miB;
723  btScalar factA, factB;
724  if(miS > btScalar(0.f))
725  {
726  factA = miB / miS;
727  }
728  else
729  {
730  factA = btScalar(0.5f);
731  }
732  factB = btScalar(1.0f) - factA;
733  // get the desired direction of hinge axis
734  // as weighted sum of Z-orthos of frameA and frameB in WCS
735  btVector3 ax1A = trA.getBasis().getColumn(2);
736  btVector3 ax1B = trB.getBasis().getColumn(2);
737  btVector3 ax1 = ax1A * factA + ax1B * factB;
738  ax1.normalize();
739  // fill first 3 rows
740  // we want: velA + wA x relA == velB + wB x relB
741  btTransform bodyA_trans = transA;
742  btTransform bodyB_trans = transB;
743  int s0 = 0;
744  int s1 = s;
745  int s2 = s * 2;
746  int nrow = 2; // last filled row
747  btVector3 tmpA, tmpB, relA, relB, p, q;
748  // get vector from bodyB to frameB in WCS
749  relB = trB.getOrigin() - bodyB_trans.getOrigin();
750  // get its projection to hinge axis
751  btVector3 projB = ax1 * relB.dot(ax1);
752  // get vector directed from bodyB to hinge axis (and orthogonal to it)
753  btVector3 orthoB = relB - projB;
754  // same for bodyA
755  relA = trA.getOrigin() - bodyA_trans.getOrigin();
756  btVector3 projA = ax1 * relA.dot(ax1);
757  btVector3 orthoA = relA - projA;
758  btVector3 totalDist = projA - projB;
759  // get offset vectors relA and relB
760  relA = orthoA + totalDist * factA;
761  relB = orthoB - totalDist * factB;
762  // now choose average ortho to hinge axis
763  p = orthoB * factA + orthoA * factB;
764  btScalar len2 = p.length2();
765  if(len2 > SIMD_EPSILON)
766  {
767  p /= btSqrt(len2);
768  }
769  else
770  {
771  p = trA.getBasis().getColumn(1);
772  }
773  // make one more ortho
774  q = ax1.cross(p);
775  // fill three rows
776  tmpA = relA.cross(p);
777  tmpB = relB.cross(p);
778  for (i=0; i<3; i++) info->m_J1angularAxis[s0+i] = tmpA[i];
779  for (i=0; i<3; i++) info->m_J2angularAxis[s0+i] = -tmpB[i];
780  tmpA = relA.cross(q);
781  tmpB = relB.cross(q);
782  if(hasStaticBody && getSolveLimit())
783  { // to make constraint between static and dynamic objects more rigid
784  // remove wA (or wB) from equation if angular limit is hit
785  tmpB *= factB;
786  tmpA *= factA;
787  }
788  for (i=0; i<3; i++) info->m_J1angularAxis[s1+i] = tmpA[i];
789  for (i=0; i<3; i++) info->m_J2angularAxis[s1+i] = -tmpB[i];
790  tmpA = relA.cross(ax1);
791  tmpB = relB.cross(ax1);
792  if(hasStaticBody)
793  { // to make constraint between static and dynamic objects more rigid
794  // remove wA (or wB) from equation
795  tmpB *= factB;
796  tmpA *= factA;
797  }
798  for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i];
799  for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i];
800 
801  btScalar k = info->fps * info->erp;
802 
803  if (!m_angularOnly)
804  {
805  for (i=0; i<3; i++) info->m_J1linearAxis[s0+i] = p[i];
806  for (i=0; i<3; i++) info->m_J1linearAxis[s1+i] = q[i];
807  for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = ax1[i];
808 
809  for (i=0; i<3; i++) info->m_J2linearAxis[s0+i] = -p[i];
810  for (i=0; i<3; i++) info->m_J2linearAxis[s1+i] = -q[i];
811  for (i=0; i<3; i++) info->m_J2linearAxis[s2+i] = -ax1[i];
812 
813  // compute three elements of right hand side
814 
815  btScalar rhs = k * p.dot(ofs);
816  info->m_constraintError[s0] = rhs;
817  rhs = k * q.dot(ofs);
818  info->m_constraintError[s1] = rhs;
819  rhs = k * ax1.dot(ofs);
820  info->m_constraintError[s2] = rhs;
821  }
822  // the hinge axis should be the only unconstrained
823  // rotational axis, the angular velocity of the two bodies perpendicular to
824  // the hinge axis should be equal. thus the constraint equations are
825  // p*w1 - p*w2 = 0
826  // q*w1 - q*w2 = 0
827  // where p and q are unit vectors normal to the hinge axis, and w1 and w2
828  // are the angular velocity vectors of the two bodies.
829  int s3 = 3 * s;
830  int s4 = 4 * s;
831  info->m_J1angularAxis[s3 + 0] = p[0];
832  info->m_J1angularAxis[s3 + 1] = p[1];
833  info->m_J1angularAxis[s3 + 2] = p[2];
834  info->m_J1angularAxis[s4 + 0] = q[0];
835  info->m_J1angularAxis[s4 + 1] = q[1];
836  info->m_J1angularAxis[s4 + 2] = q[2];
837 
838  info->m_J2angularAxis[s3 + 0] = -p[0];
839  info->m_J2angularAxis[s3 + 1] = -p[1];
840  info->m_J2angularAxis[s3 + 2] = -p[2];
841  info->m_J2angularAxis[s4 + 0] = -q[0];
842  info->m_J2angularAxis[s4 + 1] = -q[1];
843  info->m_J2angularAxis[s4 + 2] = -q[2];
844  // compute the right hand side of the constraint equation. set relative
845  // body velocities along p and q to bring the hinge back into alignment.
846  // if ax1A,ax1B are the unit length hinge axes as computed from bodyA and
847  // bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2).
848  // if "theta" is the angle between ax1 and ax2, we need an angular velocity
849  // along u to cover angle erp*theta in one step :
850  // |angular_velocity| = angle/time = erp*theta / stepsize
851  // = (erp*fps) * theta
852  // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
853  // = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
854  // ...as ax1 and ax2 are unit length. if theta is smallish,
855  // theta ~= sin(theta), so
856  // angular_velocity = (erp*fps) * (ax1 x ax2)
857  // ax1 x ax2 is in the plane space of ax1, so we project the angular
858  // velocity to p and q to find the right hand side.
859  k = info->fps * info->erp;
860  btVector3 u = ax1A.cross(ax1B);
861  info->m_constraintError[s3] = k * u.dot(p);
862  info->m_constraintError[s4] = k * u.dot(q);
863 #endif
864  // check angular limits
865  nrow = 4; // last filled row
866  int srow;
867  btScalar limit_err = btScalar(0.0);
868  int limit = 0;
869  if(getSolveLimit())
870  {
871 #ifdef _BT_USE_CENTER_LIMIT_
872  limit_err = m_limit.getCorrection() * m_referenceSign;
873 #else
874  limit_err = m_correction * m_referenceSign;
875 #endif
876  limit = (limit_err > btScalar(0.0)) ? 1 : 2;
877 
878  }
879  // if the hinge has joint limits or motor, add in the extra row
880  int powered = 0;
881  if(getEnableAngularMotor())
882  {
883  powered = 1;
884  }
885  if(limit || powered)
886  {
887  nrow++;
888  srow = nrow * info->rowskip;
889  info->m_J1angularAxis[srow+0] = ax1[0];
890  info->m_J1angularAxis[srow+1] = ax1[1];
891  info->m_J1angularAxis[srow+2] = ax1[2];
892 
893  info->m_J2angularAxis[srow+0] = -ax1[0];
894  info->m_J2angularAxis[srow+1] = -ax1[1];
895  info->m_J2angularAxis[srow+2] = -ax1[2];
896 
897  btScalar lostop = getLowerLimit();
898  btScalar histop = getUpperLimit();
899  if(limit && (lostop == histop))
900  { // the joint motor is ineffective
901  powered = 0;
902  }
903  info->m_constraintError[srow] = btScalar(0.0f);
904  btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp;
905  if(powered)
906  {
907  if(m_flags & BT_HINGE_FLAGS_CFM_NORM)
908  {
909  info->cfm[srow] = m_normalCFM;
910  }
911  btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP);
912  info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
913  info->m_lowerLimit[srow] = - m_maxMotorImpulse;
914  info->m_upperLimit[srow] = m_maxMotorImpulse;
915  }
916  if(limit)
917  {
918  k = info->fps * currERP;
919  info->m_constraintError[srow] += k * limit_err;
920  if(m_flags & BT_HINGE_FLAGS_CFM_STOP)
921  {
922  info->cfm[srow] = m_stopCFM;
923  }
924  if(lostop == histop)
925  {
926  // limited low and high simultaneously
927  info->m_lowerLimit[srow] = -SIMD_INFINITY;
928  info->m_upperLimit[srow] = SIMD_INFINITY;
929  }
930  else if(limit == 1)
931  { // low limit
932  info->m_lowerLimit[srow] = 0;
933  info->m_upperLimit[srow] = SIMD_INFINITY;
934  }
935  else
936  { // high limit
937  info->m_lowerLimit[srow] = -SIMD_INFINITY;
938  info->m_upperLimit[srow] = 0;
939  }
940  // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
941 #ifdef _BT_USE_CENTER_LIMIT_
942  btScalar bounce = m_limit.getRelaxationFactor();
943 #else
944  btScalar bounce = m_relaxationFactor;
945 #endif
946  if(bounce > btScalar(0.0))
947  {
948  btScalar vel = angVelA.dot(ax1);
949  vel -= angVelB.dot(ax1);
950  // only apply bounce if the velocity is incoming, and if the
951  // resulting c[] exceeds what we already have.
952  if(limit == 1)
953  { // low limit
954  if(vel < 0)
955  {
956  btScalar newc = -bounce * vel;
957  if(newc > info->m_constraintError[srow])
958  {
959  info->m_constraintError[srow] = newc;
960  }
961  }
962  }
963  else
964  { // high limit - all those computations are reversed
965  if(vel > 0)
966  {
967  btScalar newc = -bounce * vel;
968  if(newc < info->m_constraintError[srow])
969  {
970  info->m_constraintError[srow] = newc;
971  }
972  }
973  }
974  }
975 #ifdef _BT_USE_CENTER_LIMIT_
976  info->m_constraintError[srow] *= m_limit.getBiasFactor();
977 #else
978  info->m_constraintError[srow] *= m_biasFactor;
979 #endif
980  } // if(limit)
981  } // if angular limit or powered
982 }
983 
984 
987 void btHingeConstraint::setParam(int num, btScalar value, int axis)
988 {
989  if((axis == -1) || (axis == 5))
990  {
991  switch(num)
992  {
993  case BT_CONSTRAINT_STOP_ERP :
994  m_stopERP = value;
995  m_flags |= BT_HINGE_FLAGS_ERP_STOP;
996  break;
997  case BT_CONSTRAINT_STOP_CFM :
998  m_stopCFM = value;
999  m_flags |= BT_HINGE_FLAGS_CFM_STOP;
1000  break;
1001  case BT_CONSTRAINT_CFM :
1002  m_normalCFM = value;
1003  m_flags |= BT_HINGE_FLAGS_CFM_NORM;
1004  break;
1005  default :
1006  btAssertConstrParams(0);
1007  }
1008  }
1009  else
1010  {
1011  btAssertConstrParams(0);
1012  }
1013 }
1014 
1016 btScalar btHingeConstraint::getParam(int num, int axis) const
1017 {
1018  btScalar retVal = 0;
1019  if((axis == -1) || (axis == 5))
1020  {
1021  switch(num)
1022  {
1023  case BT_CONSTRAINT_STOP_ERP :
1024  btAssertConstrParams(m_flags & BT_HINGE_FLAGS_ERP_STOP);
1025  retVal = m_stopERP;
1026  break;
1027  case BT_CONSTRAINT_STOP_CFM :
1028  btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_STOP);
1029  retVal = m_stopCFM;
1030  break;
1031  case BT_CONSTRAINT_CFM :
1032  btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_NORM);
1033  retVal = m_normalCFM;
1034  break;
1035  default :
1036  btAssertConstrParams(0);
1037  }
1038  }
1039  else
1040  {
1041  btAssertConstrParams(0);
1042  }
1043  return retVal;
1044 }
1045 
1046 
Generated on Wed Apr 9 2014 02:38:23 for Bullet Collision Detection & Physics Library by   doxygen 1.8.1.2