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

Bullet Collision Detection & Physics Library

- Main Page
- Namespaces
- Classes
- Files

- File List
- File Members

btConeTwistConstraint.cpp
Go to the documentation of this file.
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 btConeTwistConstraint is Copyright (c) 2007 Starbreeze Studios
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 Written by: Marcus Hennix
16 */
17
18
19 #include "btConeTwistConstraint.h"
20 #include "BulletDynamics/Dynamics/btRigidBody.h"
21 #include "LinearMath/btTransformUtil.h"
22 #include "LinearMath/btMinMax.h"
23 #include <new>
24
25
26
27 //#define CONETWIST_USE_OBSOLETE_SOLVER true
28 #define CONETWIST_USE_OBSOLETE_SOLVER false
29 #define CONETWIST_DEF_FIX_THRESH btScalar(.05f)
30
31
32 SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3& axis, const btMatrix3x3& invInertiaWorld)
33 {
34 btVector3 vec = axis * invInertiaWorld;
35 return axis.dot(vec);
36 }
37
38
39
40
41 btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB,
42 const btTransform& rbAFrame,const btTransform& rbBFrame)
43 :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
44 m_angularOnly(false),
45 m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
46 {
47 init();
48 }
49
50 btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame)
51 :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE,rbA),m_rbAFrame(rbAFrame),
52 m_angularOnly(false),
53 m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER)
54 {
55 m_rbBFrame = m_rbAFrame;
56 m_rbBFrame.setOrigin(btVector3(0., 0., 0.));
57 init();
58 }
59
60
61 void btConeTwistConstraint::init()
62 {
63 m_angularOnly = false;
64 m_solveTwistLimit = false;
65 m_solveSwingLimit = false;
66 m_bMotorEnabled = false;
67 m_maxMotorImpulse = btScalar(-1);
68
69 setLimit(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
70 m_damping = btScalar(0.01);
71 m_fixThresh = CONETWIST_DEF_FIX_THRESH;
72 m_flags = 0;
73 m_linCFM = btScalar(0.f);
74 m_linERP = btScalar(0.7f);
75 m_angCFM = btScalar(0.f);
76 }
77
78
79 void btConeTwistConstraint::getInfo1 (btConstraintInfo1* info)
80 {
81 if (m_useSolveConstraintObsolete)
82 {
83 info->m_numConstraintRows = 0;
84 info->nub = 0;
85 }
86 else
87 {
88 info->m_numConstraintRows = 3;
89 info->nub = 3;
90 calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
91 if(m_solveSwingLimit)
92 {
93 info->m_numConstraintRows++;
94 info->nub--;
95 if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
96 {
97 info->m_numConstraintRows++;
98 info->nub--;
99 }
100 }
101 if(m_solveTwistLimit)
102 {
103 info->m_numConstraintRows++;
104 info->nub--;
105 }
106 }
107 }
108
109 void btConeTwistConstraint::getInfo1NonVirtual (btConstraintInfo1* info)
110 {
111 //always reserve 6 rows: object transform is not available on SPU
112 info->m_numConstraintRows = 6;
113 info->nub = 0;
114
115 }
116
117
118 void btConeTwistConstraint::getInfo2 (btConstraintInfo2* info)
119 {
120 getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
121 }
122
123 void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB)
124 {
125 calcAngleInfo2(transA,transB,invInertiaWorldA,invInertiaWorldB);
126
127 btAssert(!m_useSolveConstraintObsolete);
128 // set jacobian
129 info->m_J1linearAxis[0] = 1;
130 info->m_J1linearAxis[info->rowskip+1] = 1;
131 info->m_J1linearAxis[2*info->rowskip+2] = 1;
132 btVector3 a1 = transA.getBasis() * m_rbAFrame.getOrigin();
133 {
134 btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
135 btVector3* angular1 = (btVector3*)(info->m_J1angularAxis+info->rowskip);
136 btVector3* angular2 = (btVector3*)(info->m_J1angularAxis+2*info->rowskip);
137 btVector3 a1neg = -a1;
138 a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
139 }
140 info->m_J2linearAxis[0] = -1;
141 info->m_J2linearAxis[info->rowskip+1] = -1;
142 info->m_J2linearAxis[2*info->rowskip+2] = -1;
143 btVector3 a2 = transB.getBasis() * m_rbBFrame.getOrigin();
144 {
145 btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
146 btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip);
147 btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip);
148 a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
149 }
150 // set right hand side
151 btScalar linERP = (m_flags & BT_CONETWIST_FLAGS_LIN_ERP) ? m_linERP : info->erp;
152 btScalar k = info->fps * linERP;
153 int j;
154 for (j=0; j<3; j++)
155 {
156 info->m_constraintError[j*info->rowskip] = k * (a2[j] + transB.getOrigin()[j] - a1[j] - transA.getOrigin()[j]);
157 info->m_lowerLimit[j*info->rowskip] = -SIMD_INFINITY;
158 info->m_upperLimit[j*info->rowskip] = SIMD_INFINITY;
159 if(m_flags & BT_CONETWIST_FLAGS_LIN_CFM)
160 {
161 info->cfm[j*info->rowskip] = m_linCFM;
162 }
163 }
164 int row = 3;
165 int srow = row * info->rowskip;
166 btVector3 ax1;
167 // angular limits
168 if(m_solveSwingLimit)
169 {
170 btScalar *J1 = info->m_J1angularAxis;
171 btScalar *J2 = info->m_J2angularAxis;
172 if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
173 {
174 btTransform trA = transA*m_rbAFrame;
175 btVector3 p = trA.getBasis().getColumn(1);
176 btVector3 q = trA.getBasis().getColumn(2);
177 int srow1 = srow + info->rowskip;
178 J1[srow+0] = p[0];
179 J1[srow+1] = p[1];
180 J1[srow+2] = p[2];
181 J1[srow1+0] = q[0];
182 J1[srow1+1] = q[1];
183 J1[srow1+2] = q[2];
184 J2[srow+0] = -p[0];
185 J2[srow+1] = -p[1];
186 J2[srow+2] = -p[2];
187 J2[srow1+0] = -q[0];
188 J2[srow1+1] = -q[1];
189 J2[srow1+2] = -q[2];
190 btScalar fact = info->fps * m_relaxationFactor;
191 info->m_constraintError[srow] = fact * m_swingAxis.dot(p);
192 info->m_constraintError[srow1] = fact * m_swingAxis.dot(q);
193 info->m_lowerLimit[srow] = -SIMD_INFINITY;
194 info->m_upperLimit[srow] = SIMD_INFINITY;
195 info->m_lowerLimit[srow1] = -SIMD_INFINITY;
196 info->m_upperLimit[srow1] = SIMD_INFINITY;
197 srow = srow1 + info->rowskip;
198 }
199 else
200 {
201 ax1 = m_swingAxis * m_relaxationFactor * m_relaxationFactor;
202 J1[srow+0] = ax1[0];
203 J1[srow+1] = ax1[1];
204 J1[srow+2] = ax1[2];
205 J2[srow+0] = -ax1[0];
206 J2[srow+1] = -ax1[1];
207 J2[srow+2] = -ax1[2];
208 btScalar k = info->fps * m_biasFactor;
209
210 info->m_constraintError[srow] = k * m_swingCorrection;
211 if(m_flags & BT_CONETWIST_FLAGS_ANG_CFM)
212 {
213 info->cfm[srow] = m_angCFM;
214 }
215 // m_swingCorrection is always positive or 0
216 info->m_lowerLimit[srow] = 0;
217 info->m_upperLimit[srow] = (m_bMotorEnabled && m_maxMotorImpulse >= 0.0f) ? m_maxMotorImpulse : SIMD_INFINITY;
218 srow += info->rowskip;
219 }
220 }
221 if(m_solveTwistLimit)
222 {
223 ax1 = m_twistAxis * m_relaxationFactor * m_relaxationFactor;
224 btScalar *J1 = info->m_J1angularAxis;
225 btScalar *J2 = info->m_J2angularAxis;
226 J1[srow+0] = ax1[0];
227 J1[srow+1] = ax1[1];
228 J1[srow+2] = ax1[2];
229 J2[srow+0] = -ax1[0];
230 J2[srow+1] = -ax1[1];
231 J2[srow+2] = -ax1[2];
232 btScalar k = info->fps * m_biasFactor;
233 info->m_constraintError[srow] = k * m_twistCorrection;
234 if(m_flags & BT_CONETWIST_FLAGS_ANG_CFM)
235 {
236 info->cfm[srow] = m_angCFM;
237 }
238 if(m_twistSpan > 0.0f)
239 {
240
241 if(m_twistCorrection > 0.0f)
242 {
243 info->m_lowerLimit[srow] = 0;
244 info->m_upperLimit[srow] = SIMD_INFINITY;
245 }
246 else
247 {
248 info->m_lowerLimit[srow] = -SIMD_INFINITY;
249 info->m_upperLimit[srow] = 0;
250 }
251 }
252 else
253 {
254 info->m_lowerLimit[srow] = -SIMD_INFINITY;
255 info->m_upperLimit[srow] = SIMD_INFINITY;
256 }
257 srow += info->rowskip;
258 }
259 }
260
261
262
263 void btConeTwistConstraint::buildJacobian()
264 {
265 if (m_useSolveConstraintObsolete)
266 {
267 m_appliedImpulse = btScalar(0.);
268 m_accTwistLimitImpulse = btScalar(0.);
269 m_accSwingLimitImpulse = btScalar(0.);
270 m_accMotorImpulse = btVector3(0.,0.,0.);
271
272 if (!m_angularOnly)
273 {
274 btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
275 btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
276 btVector3 relPos = pivotBInW - pivotAInW;
277
278 btVector3 normal[3];
279 if (relPos.length2() > SIMD_EPSILON)
280 {
281 normal[0] = relPos.normalized();
282 }
283 else
284 {
285 normal[0].setValue(btScalar(1.0),0,0);
286 }
287
288 btPlaneSpace1(normal[0], normal[1], normal[2]);
289
290 for (int i=0;i<3;i++)
291 {
292 new (&m_jac[i]) btJacobianEntry(
293 m_rbA.getCenterOfMassTransform().getBasis().transpose(),
294 m_rbB.getCenterOfMassTransform().getBasis().transpose(),
295 pivotAInW - m_rbA.getCenterOfMassPosition(),
296 pivotBInW - m_rbB.getCenterOfMassPosition(),
297 normal[i],
298 m_rbA.getInvInertiaDiagLocal(),
299 m_rbA.getInvMass(),
300 m_rbB.getInvInertiaDiagLocal(),
301 m_rbB.getInvMass());
302 }
303 }
304
305 calcAngleInfo2(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getInvInertiaTensorWorld(),m_rbB.getInvInertiaTensorWorld());
306 }
307 }
308
309
310
311 void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
312 {
313 #ifndef __SPU__
314 if (m_useSolveConstraintObsolete)
315 {
316 btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
317 btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
318
319 btScalar tau = btScalar(0.3);
320
321 //linear part
322 if (!m_angularOnly)
323 {
324 btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
325 btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
326
327 btVector3 vel1;
328 bodyA.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1);
329 btVector3 vel2;
330 bodyB.internalGetVelocityInLocalPointObsolete(rel_pos2,vel2);
331 btVector3 vel = vel1 - vel2;
332
333 for (int i=0;i<3;i++)
334 {
335 const btVector3& normal = m_jac[i].m_linearJointAxis;
336 btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
337
338 btScalar rel_vel;
339 rel_vel = normal.dot(vel);
340 //positional error (zeroth order error)
341 btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
342 btScalar impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv;
343 m_appliedImpulse += impulse;
344
345 btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
346 btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
347 bodyA.internalApplyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
348 bodyB.internalApplyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse);
349
350 }
351 }
352
353 // apply motor
354 if (m_bMotorEnabled)
355 {
356 // compute current and predicted transforms
357 btTransform trACur = m_rbA.getCenterOfMassTransform();
358 btTransform trBCur = m_rbB.getCenterOfMassTransform();
359 btVector3 omegaA; bodyA.internalGetAngularVelocity(omegaA);
360 btVector3 omegaB; bodyB.internalGetAngularVelocity(omegaB);
361 btTransform trAPred; trAPred.setIdentity();
362 btVector3 zerovec(0,0,0);
363 btTransformUtil::integrateTransform(
364 trACur, zerovec, omegaA, timeStep, trAPred);
365 btTransform trBPred; trBPred.setIdentity();
366 btTransformUtil::integrateTransform(
367 trBCur, zerovec, omegaB, timeStep, trBPred);
368
369 // compute desired transforms in world
370 btTransform trPose(m_qTarget);
371 btTransform trABDes = m_rbBFrame * trPose * m_rbAFrame.inverse();
372 btTransform trADes = trBPred * trABDes;
373 btTransform trBDes = trAPred * trABDes.inverse();
374
375 // compute desired omegas in world
376 btVector3 omegaADes, omegaBDes;
377
378 btTransformUtil::calculateVelocity(trACur, trADes, timeStep, zerovec, omegaADes);
379 btTransformUtil::calculateVelocity(trBCur, trBDes, timeStep, zerovec, omegaBDes);
380
381 // compute delta omegas
382 btVector3 dOmegaA = omegaADes - omegaA;
383 btVector3 dOmegaB = omegaBDes - omegaB;
384
385 // compute weighted avg axis of dOmega (weighting based on inertias)
386 btVector3 axisA, axisB;
387 btScalar kAxisAInv = 0, kAxisBInv = 0;
388
389 if (dOmegaA.length2() > SIMD_EPSILON)
390 {
391 axisA = dOmegaA.normalized();
392 kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(axisA);
393 }
394
395 if (dOmegaB.length2() > SIMD_EPSILON)
396 {
397 axisB = dOmegaB.normalized();
398 kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(axisB);
399 }
400
401 btVector3 avgAxis = kAxisAInv * axisA + kAxisBInv * axisB;
402
403 static bool bDoTorque = true;
404 if (bDoTorque && avgAxis.length2() > SIMD_EPSILON)
405 {
406 avgAxis.normalize();
407 kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(avgAxis);
408 kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(avgAxis);
409 btScalar kInvCombined = kAxisAInv + kAxisBInv;
410
411 btVector3 impulse = (kAxisAInv * dOmegaA - kAxisBInv * dOmegaB) /
412 (kInvCombined * kInvCombined);
413
414 if (m_maxMotorImpulse >= 0)
415 {
416 btScalar fMaxImpulse = m_maxMotorImpulse;
417 if (m_bNormalizedMotorStrength)
418 fMaxImpulse = fMaxImpulse/kAxisAInv;
419
420 btVector3 newUnclampedAccImpulse = m_accMotorImpulse + impulse;
421 btScalar newUnclampedMag = newUnclampedAccImpulse.length();
422 if (newUnclampedMag > fMaxImpulse)
423 {
424 newUnclampedAccImpulse.normalize();
425 newUnclampedAccImpulse *= fMaxImpulse;
426 impulse = newUnclampedAccImpulse - m_accMotorImpulse;
427 }
428 m_accMotorImpulse += impulse;
429 }
430
431 btScalar impulseMag = impulse.length();
432 btVector3 impulseAxis = impulse / impulseMag;
433
434 bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
435 bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
436
437 }
438 }
439 else if (m_damping > SIMD_EPSILON) // no motor: do a little damping
440 {
441 btVector3 angVelA; bodyA.internalGetAngularVelocity(angVelA);
442 btVector3 angVelB; bodyB.internalGetAngularVelocity(angVelB);
443 btVector3 relVel = angVelB - angVelA;
444 if (relVel.length2() > SIMD_EPSILON)
445 {
446 btVector3 relVelAxis = relVel.normalized();
447 btScalar m_kDamping = btScalar(1.) /
448 (getRigidBodyA().computeAngularImpulseDenominator(relVelAxis) +
449 getRigidBodyB().computeAngularImpulseDenominator(relVelAxis));
450 btVector3 impulse = m_damping * m_kDamping * relVel;
451
452 btScalar impulseMag = impulse.length();
453 btVector3 impulseAxis = impulse / impulseMag;
454 bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*impulseAxis, impulseMag);
455 bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*impulseAxis, -impulseMag);
456 }
457 }
458
459 // joint limits
460 {
462 btVector3 angVelA;
463 bodyA.internalGetAngularVelocity(angVelA);
464 btVector3 angVelB;
465 bodyB.internalGetAngularVelocity(angVelB);
466
467 // solve swing limit
468 if (m_solveSwingLimit)
469 {
470 btScalar amplitude = m_swingLimitRatio * m_swingCorrection*m_biasFactor/timeStep;
471 btScalar relSwingVel = (angVelB - angVelA).dot(m_swingAxis);
472 if (relSwingVel > 0)
473 amplitude += m_swingLimitRatio * relSwingVel * m_relaxationFactor;
474 btScalar impulseMag = amplitude * m_kSwing;
475
476 // Clamp the accumulated impulse
477 btScalar temp = m_accSwingLimitImpulse;
478 m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) );
479 impulseMag = m_accSwingLimitImpulse - temp;
480
481 btVector3 impulse = m_swingAxis * impulseMag;
482
483 // don't let cone response affect twist
484 // (this can happen since body A's twist doesn't match body B's AND we use an elliptical cone limit)
485 {
486 btVector3 impulseTwistCouple = impulse.dot(m_twistAxisA) * m_twistAxisA;
487 btVector3 impulseNoTwistCouple = impulse - impulseTwistCouple;
488 impulse = impulseNoTwistCouple;
489 }
490
491 impulseMag = impulse.length();
492 btVector3 noTwistSwingAxis = impulse / impulseMag;
493
494 bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*noTwistSwingAxis, impulseMag);
495 bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*noTwistSwingAxis, -impulseMag);
496 }
497
498
499 // solve twist limit
500 if (m_solveTwistLimit)
501 {
502 btScalar amplitude = m_twistLimitRatio * m_twistCorrection*m_biasFactor/timeStep;
503 btScalar relTwistVel = (angVelB - angVelA).dot( m_twistAxis );
504 if (relTwistVel > 0) // only damp when moving towards limit (m_twistAxis flipping is important)
505 amplitude += m_twistLimitRatio * relTwistVel * m_relaxationFactor;
506 btScalar impulseMag = amplitude * m_kTwist;
507
508 // Clamp the accumulated impulse
509 btScalar temp = m_accTwistLimitImpulse;
510 m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) );
511 impulseMag = m_accTwistLimitImpulse - temp;
512
513 // btVector3 impulse = m_twistAxis * impulseMag;
514
515 bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag);
516 bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_twistAxis,-impulseMag);
517 }
518 }
519 }
520 #else
521 btAssert(0);
522 #endif //__SPU__
523 }
524
525
526
527
528 void btConeTwistConstraint::updateRHS(btScalar timeStep)
529 {
530 (void)timeStep;
531
532 }
533
534
535 #ifndef __SPU__
536 void btConeTwistConstraint::calcAngleInfo()
537 {
538 m_swingCorrection = btScalar(0.);
539 m_twistLimitSign = btScalar(0.);
540 m_solveTwistLimit = false;
541 m_solveSwingLimit = false;
542
543 btVector3 b1Axis1(0,0,0),b1Axis2(0,0,0),b1Axis3(0,0,0);
544 btVector3 b2Axis1(0,0,0),b2Axis2(0,0,0);
545
546 b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0);
547 b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0);
548
549 btScalar swing1=btScalar(0.),swing2 = btScalar(0.);
550
551 btScalar swx=btScalar(0.),swy = btScalar(0.);
552 btScalar thresh = btScalar(10.);
553 btScalar fact;
554
555 // Get Frame into world space
556 if (m_swingSpan1 >= btScalar(0.05f))
557 {
558 b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1);
559 swx = b2Axis1.dot(b1Axis1);
560 swy = b2Axis1.dot(b1Axis2);
561 swing1 = btAtan2Fast(swy, swx);
562 fact = (swy*swy + swx*swx) * thresh * thresh;
563 fact = fact / (fact + btScalar(1.0));
564 swing1 *= fact;
565 }
566
567 if (m_swingSpan2 >= btScalar(0.05f))
568 {
569 b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2);
570 swx = b2Axis1.dot(b1Axis1);
571 swy = b2Axis1.dot(b1Axis3);
572 swing2 = btAtan2Fast(swy, swx);
573 fact = (swy*swy + swx*swx) * thresh * thresh;
574 fact = fact / (fact + btScalar(1.0));
575 swing2 *= fact;
576 }
577
578 btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1);
579 btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2);
580 btScalar EllipseAngle = btFabs(swing1*swing1)* RMaxAngle1Sq + btFabs(swing2*swing2) * RMaxAngle2Sq;
581
582 if (EllipseAngle > 1.0f)
583 {
584 m_swingCorrection = EllipseAngle-1.0f;
585 m_solveSwingLimit = true;
586 // Calculate necessary axis & factors
587 m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3));
588 m_swingAxis.normalize();
589 btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
590 m_swingAxis *= swingAxisSign;
591 }
592
593 // Twist limits
594 if (m_twistSpan >= btScalar(0.))
595 {
596 btVector3 b2Axis2 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(1);
597 btQuaternion rotationArc = shortestArcQuat(b2Axis1,b1Axis1);
598 btVector3 TwistRef = quatRotate(rotationArc,b2Axis2);
599 btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) );
600 m_twistAngle = twist;
601
602 // btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.);
603 btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? btScalar(1.0f) : btScalar(0.);
604 if (twist <= -m_twistSpan*lockedFreeFactor)
605 {
606 m_twistCorrection = -(twist + m_twistSpan);
607 m_solveTwistLimit = true;
608 m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
609 m_twistAxis.normalize();
610 m_twistAxis *= -1.0f;
611 }
612 else if (twist > m_twistSpan*lockedFreeFactor)
613 {
614 m_twistCorrection = (twist - m_twistSpan);
615 m_solveTwistLimit = true;
616 m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
617 m_twistAxis.normalize();
618 }
619 }
620 }
621 #endif //__SPU__
622
623 static btVector3 vTwist(1,0,0); // twist axis in constraint's space
624
625
626
627 void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTransform& transB, const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB)
628 {
629 m_swingCorrection = btScalar(0.);
630 m_twistLimitSign = btScalar(0.);
631 m_solveTwistLimit = false;
632 m_solveSwingLimit = false;
633 // compute rotation of A wrt B (in constraint space)
634 if (m_bMotorEnabled && (!m_useSolveConstraintObsolete))
635 { // it is assumed that setMotorTarget() was alredy called
636 // and motor target m_qTarget is within constraint limits
637 // TODO : split rotation to pure swing and pure twist
638 // compute desired transforms in world
639 btTransform trPose(m_qTarget);
640 btTransform trA = transA * m_rbAFrame;
641 btTransform trB = transB * m_rbBFrame;
642 btTransform trDeltaAB = trB * trPose * trA.inverse();
643 btQuaternion qDeltaAB = trDeltaAB.getRotation();
644 btVector3 swingAxis = btVector3(qDeltaAB.x(), qDeltaAB.y(), qDeltaAB.z());
645 float swingAxisLen2 = swingAxis.length2();
646 if(btFuzzyZero(swingAxisLen2))
647 {
648 return;
649 }
650 m_swingAxis = swingAxis;
651 m_swingAxis.normalize();
652 m_swingCorrection = qDeltaAB.getAngle();
653 if(!btFuzzyZero(m_swingCorrection))
654 {
655 m_solveSwingLimit = true;
656 }
657 return;
658 }
659
660
661 {
662 // compute rotation of A wrt B (in constraint space)
663 btQuaternion qA = transA.getRotation() * m_rbAFrame.getRotation();
664 btQuaternion qB = transB.getRotation() * m_rbBFrame.getRotation();
665 btQuaternion qAB = qB.inverse() * qA;
666 // split rotation into cone and twist
667 // (all this is done from B's perspective. Maybe I should be averaging axes...)
668 btVector3 vConeNoTwist = quatRotate(qAB, vTwist); vConeNoTwist.normalize();
669 btQuaternion qABCone = shortestArcQuat(vTwist, vConeNoTwist); qABCone.normalize();
670 btQuaternion qABTwist = qABCone.inverse() * qAB; qABTwist.normalize();
671
672 if (m_swingSpan1 >= m_fixThresh && m_swingSpan2 >= m_fixThresh)
673 {
674 btScalar swingAngle, swingLimit = 0; btVector3 swingAxis;
675 computeConeLimitInfo(qABCone, swingAngle, swingAxis, swingLimit);
676
677 if (swingAngle > swingLimit * m_limitSoftness)
678 {
679 m_solveSwingLimit = true;
680
681 // compute limit ratio: 0->1, where
682 // 0 == beginning of soft limit
683 // 1 == hard/real limit
684 m_swingLimitRatio = 1.f;
685 if (swingAngle < swingLimit && m_limitSoftness < 1.f - SIMD_EPSILON)
686 {
687 m_swingLimitRatio = (swingAngle - swingLimit * m_limitSoftness)/
688 (swingLimit - swingLimit * m_limitSoftness);
689 }
690
691 // swing correction tries to get back to soft limit
692 m_swingCorrection = swingAngle - (swingLimit * m_limitSoftness);
693
694 // adjustment of swing axis (based on ellipse normal)
695 adjustSwingAxisToUseEllipseNormal(swingAxis);
696
697 // Calculate necessary axis & factors
698 m_swingAxis = quatRotate(qB, -swingAxis);
699
700 m_twistAxisA.setValue(0,0,0);
701
702 m_kSwing = btScalar(1.) /
703 (computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldA) +
704 computeAngularImpulseDenominator(m_swingAxis,invInertiaWorldB));
705 }
706 }
707 else
708 {
709 // you haven't set any limits;
710 // or you're trying to set at least one of the swing limits too small. (if so, do you really want a conetwist constraint?)
711 // anyway, we have either hinge or fixed joint
712 btVector3 ivA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0);
713 btVector3 jvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1);
714 btVector3 kvA = transA.getBasis() * m_rbAFrame.getBasis().getColumn(2);
715 btVector3 ivB = transB.getBasis() * m_rbBFrame.getBasis().getColumn(0);
716 btVector3 target;
717 btScalar x = ivB.dot(ivA);
718 btScalar y = ivB.dot(jvA);
719 btScalar z = ivB.dot(kvA);
720 if((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
721 { // fixed. We'll need to add one more row to constraint
722 if((!btFuzzyZero(y)) || (!(btFuzzyZero(z))))
723 {
724 m_solveSwingLimit = true;
725 m_swingAxis = -ivB.cross(ivA);
726 }
727 }
728 else
729 {
730 if(m_swingSpan1 < m_fixThresh)
731 { // hinge around Y axis
732 // if(!(btFuzzyZero(y)))
733 if((!(btFuzzyZero(x))) || (!(btFuzzyZero(z))))
734 {
735 m_solveSwingLimit = true;
736 if(m_swingSpan2 >= m_fixThresh)
737 {
738 y = btScalar(0.f);
739 btScalar span2 = btAtan2(z, x);
740 if(span2 > m_swingSpan2)
741 {
742 x = btCos(m_swingSpan2);
743 z = btSin(m_swingSpan2);
744 }
745 else if(span2 < -m_swingSpan2)
746 {
747 x = btCos(m_swingSpan2);
748 z = -btSin(m_swingSpan2);
749 }
750 }
751 }
752 }
753 else
754 { // hinge around Z axis
755 // if(!btFuzzyZero(z))
756 if((!(btFuzzyZero(x))) || (!(btFuzzyZero(y))))
757 {
758 m_solveSwingLimit = true;
759 if(m_swingSpan1 >= m_fixThresh)
760 {
761 z = btScalar(0.f);
762 btScalar span1 = btAtan2(y, x);
763 if(span1 > m_swingSpan1)
764 {
765 x = btCos(m_swingSpan1);
766 y = btSin(m_swingSpan1);
767 }
768 else if(span1 < -m_swingSpan1)
769 {
770 x = btCos(m_swingSpan1);
771 y = -btSin(m_swingSpan1);
772 }
773 }
774 }
775 }
776 target[0] = x * ivA[0] + y * jvA[0] + z * kvA[0];
777 target[1] = x * ivA[1] + y * jvA[1] + z * kvA[1];
778 target[2] = x * ivA[2] + y * jvA[2] + z * kvA[2];
779 target.normalize();
780 m_swingAxis = -ivB.cross(target);
781 m_swingCorrection = m_swingAxis.length();
782
783 if (!btFuzzyZero(m_swingCorrection))
784 m_swingAxis.normalize();
785 }
786 }
787
788 if (m_twistSpan >= btScalar(0.f))
789 {
790 btVector3 twistAxis;
791 computeTwistLimitInfo(qABTwist, m_twistAngle, twistAxis);
792
793 if (m_twistAngle > m_twistSpan*m_limitSoftness)
794 {
795 m_solveTwistLimit = true;
796
797 m_twistLimitRatio = 1.f;
798 if (m_twistAngle < m_twistSpan && m_limitSoftness < 1.f - SIMD_EPSILON)
799 {
800 m_twistLimitRatio = (m_twistAngle - m_twistSpan * m_limitSoftness)/
801 (m_twistSpan - m_twistSpan * m_limitSoftness);
802 }
803
804 // twist correction tries to get back to soft limit
805 m_twistCorrection = m_twistAngle - (m_twistSpan * m_limitSoftness);
806
807 m_twistAxis = quatRotate(qB, -twistAxis);
808
809 m_kTwist = btScalar(1.) /
810 (computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldA) +
811 computeAngularImpulseDenominator(m_twistAxis,invInertiaWorldB));
812 }
813
814 if (m_solveSwingLimit)
815 m_twistAxisA = quatRotate(qA, -twistAxis);
816 }
817 else
818 {
819 m_twistAngle = btScalar(0.f);
820 }
821 }
822 }
823
824
825
826 // given a cone rotation in constraint space, (pre: twist must already be removed)
827 // this method computes its corresponding swing angle and axis.
828 // more interestingly, it computes the cone/swing limit (angle) for this cone "pose".
829 void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone,
830 btScalar& swingAngle, // out
831 btVector3& vSwingAxis, // out
832 btScalar& swingLimit) // out
833 {
834 swingAngle = qCone.getAngle();
835 if (swingAngle > SIMD_EPSILON)
836 {
837 vSwingAxis = btVector3(qCone.x(), qCone.y(), qCone.z());
838 vSwingAxis.normalize();
839 #if 0
840 // non-zero twist?! this should never happen.
841 btAssert(fabs(vSwingAxis.x()) <= SIMD_EPSILON));
842 #endif
843
844 // Compute limit for given swing. tricky:
845 // Given a swing axis, we're looking for the intersection with the bounding cone ellipse.
846 // (Since we're dealing with angles, this ellipse is embedded on the surface of a sphere.)
847
848 // For starters, compute the direction from center to surface of ellipse.
849 // This is just the perpendicular (ie. rotate 2D vector by PI/2) of the swing axis.
850 // (vSwingAxis is the cone rotation (in z,y); change vars and rotate to (x,y) coords.)
851 btScalar xEllipse = vSwingAxis.y();
852 btScalar yEllipse = -vSwingAxis.z();
853
854 // Now, we use the slope of the vector (using x/yEllipse) and find the length
855 // of the line that intersects the ellipse:
856 // x^2 y^2
857 // --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits)
858 // a^2 b^2
859 // Do the math and it should be clear.
860
861 swingLimit = m_swingSpan1; // if xEllipse == 0, we have a pure vSwingAxis.z rotation: just use swingspan1
862 if (fabs(xEllipse) > SIMD_EPSILON)
863 {
864 btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse);
865 btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
866 norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1);
867 btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
868 swingLimit = sqrt(swingLimit2);
869 }
870
871 // test!
872 /*swingLimit = m_swingSpan2;
873 if (fabs(vSwingAxis.z()) > SIMD_EPSILON)
874 {
875 btScalar mag_2 = m_swingSpan1*m_swingSpan1 + m_swingSpan2*m_swingSpan2;
876 btScalar sinphi = m_swingSpan2 / sqrt(mag_2);
877 btScalar phi = asin(sinphi);
878 btScalar theta = atan2(fabs(vSwingAxis.y()),fabs(vSwingAxis.z()));
879 btScalar alpha = 3.14159f - theta - phi;
880 btScalar sinalpha = sin(alpha);
881 swingLimit = m_swingSpan1 * sinphi/sinalpha;
882 }*/
883 }
884 else if (swingAngle < 0)
885 {
886 // this should never happen!
887 #if 0
888 btAssert(0);
889 #endif
890 }
891 }
892
893 btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const
894 {
895 // compute x/y in ellipse using cone angle (0 -> 2*PI along surface of cone)
896 btScalar xEllipse = btCos(fAngleInRadians);
897 btScalar yEllipse = btSin(fAngleInRadians);
898
899 // Use the slope of the vector (using x/yEllipse) and find the length
900 // of the line that intersects the ellipse:
901 // x^2 y^2
902 // --- + --- = 1, where a and b are semi-major axes 2 and 1 respectively (ie. the limits)
903 // a^2 b^2
904 // Do the math and it should be clear.
905
906 float swingLimit = m_swingSpan1; // if xEllipse == 0, just use axis b (1)
907 if (fabs(xEllipse) > SIMD_EPSILON)
908 {
909 btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse);
910 btScalar norm = 1 / (m_swingSpan2 * m_swingSpan2);
911 norm += surfaceSlope2 / (m_swingSpan1 * m_swingSpan1);
912 btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
913 swingLimit = sqrt(swingLimit2);
914 }
915
916 // convert into point in constraint space:
917 // note: twist is x-axis, swing 1 and 2 are along the z and y axes respectively
918 btVector3 vSwingAxis(0, xEllipse, -yEllipse);
919 btQuaternion qSwing(vSwingAxis, swingLimit);
920 btVector3 vPointInConstraintSpace(fLength,0,0);
921 return quatRotate(qSwing, vPointInConstraintSpace);
922 }
923
924 // given a twist rotation in constraint space, (pre: cone must already be removed)
925 // this method computes its corresponding angle and axis.
926 void btConeTwistConstraint::computeTwistLimitInfo(const btQuaternion& qTwist,
927 btScalar& twistAngle, // out
928 btVector3& vTwistAxis) // out
929 {
930 btQuaternion qMinTwist = qTwist;
931 twistAngle = qTwist.getAngle();
932
933 if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate.
934 {
935 qMinTwist = -(qTwist);
936 twistAngle = qMinTwist.getAngle();
937 }
938 if (twistAngle < 0)
939 {
940 // this should never happen
941 #if 0
942 btAssert(0);
943 #endif
944 }
945
946 vTwistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z());
947 if (twistAngle > SIMD_EPSILON)
948 vTwistAxis.normalize();
949 }
950
951
952 void btConeTwistConstraint::adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const
953 {
954 // the swing axis is computed as the "twist-free" cone rotation,
955 // but the cone limit is not circular, but elliptical (if swingspan1 != swingspan2).
956 // so, if we're outside the limits, the closest way back inside the cone isn't
957 // along the vector back to the center. better (and more stable) to use the ellipse normal.
958
959 // convert swing axis to direction from center to surface of ellipse
960 // (ie. rotate 2D vector by PI/2)
961 btScalar y = -vSwingAxis.z();
962 btScalar z = vSwingAxis.y();
963
964 // do the math...
965 if (fabs(z) > SIMD_EPSILON) // avoid division by 0. and we don't need an update if z == 0.
966 {
967 // compute gradient/normal of ellipse surface at current "point"
968 btScalar grad = y/z;
969 grad *= m_swingSpan2 / m_swingSpan1;
970
971 // adjust y/z to represent normal at point (instead of vector to point)
972 if (y > 0)
973 y = fabs(grad * z);
974 else
975 y = -fabs(grad * z);
976
977 // convert ellipse direction back to swing axis
978 vSwingAxis.setZ(-y);
979 vSwingAxis.setY( z);
980 vSwingAxis.normalize();
981 }
982 }
983
984
985
986 void btConeTwistConstraint::setMotorTarget(const btQuaternion &q)
987 {
988 //btTransform trACur = m_rbA.getCenterOfMassTransform();
989 //btTransform trBCur = m_rbB.getCenterOfMassTransform();
990 // btTransform trABCur = trBCur.inverse() * trACur;
991 // btQuaternion qABCur = trABCur.getRotation();
992 // btTransform trConstraintCur = (trBCur * m_rbBFrame).inverse() * (trACur * m_rbAFrame);
993 //btQuaternion qConstraintCur = trConstraintCur.getRotation();
994
995 btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * q * m_rbAFrame.getRotation();
996 setMotorTargetInConstraintSpace(qConstraint);
997 }
998
999
1000 void btConeTwistConstraint::setMotorTargetInConstraintSpace(const btQuaternion &q)
1001 {
1002 m_qTarget = q;
1003
1004 // clamp motor target to within limits
1005 {
1006 btScalar softness = 1.f;//m_limitSoftness;
1007
1008 // split into twist and cone
1009 btVector3 vTwisted = quatRotate(m_qTarget, vTwist);
1010 btQuaternion qTargetCone = shortestArcQuat(vTwist, vTwisted); qTargetCone.normalize();
1011 btQuaternion qTargetTwist = qTargetCone.inverse() * m_qTarget; qTargetTwist.normalize();
1012
1013 // clamp cone
1014 if (m_swingSpan1 >= btScalar(0.05f) && m_swingSpan2 >= btScalar(0.05f))
1015 {
1016 btScalar swingAngle, swingLimit; btVector3 swingAxis;
1017 computeConeLimitInfo(qTargetCone, swingAngle, swingAxis, swingLimit);
1018
1019 if (fabs(swingAngle) > SIMD_EPSILON)
1020 {
1021 if (swingAngle > swingLimit*softness)
1022 swingAngle = swingLimit*softness;
1023 else if (swingAngle < -swingLimit*softness)
1024 swingAngle = -swingLimit*softness;
1025 qTargetCone = btQuaternion(swingAxis, swingAngle);
1026 }
1027 }
1028
1029 // clamp twist
1030 if (m_twistSpan >= btScalar(0.05f))
1031 {
1032 btScalar twistAngle; btVector3 twistAxis;
1033 computeTwistLimitInfo(qTargetTwist, twistAngle, twistAxis);
1034
1035 if (fabs(twistAngle) > SIMD_EPSILON)
1036 {
1037 // eddy todo: limitSoftness used here???
1038 if (twistAngle > m_twistSpan*softness)
1039 twistAngle = m_twistSpan*softness;
1040 else if (twistAngle < -m_twistSpan*softness)
1041 twistAngle = -m_twistSpan*softness;
1042 qTargetTwist = btQuaternion(twistAxis, twistAngle);
1043 }
1044 }
1045
1046 m_qTarget = qTargetCone * qTargetTwist;
1047 }
1048 }
1049
1052 void btConeTwistConstraint::setParam(int num, btScalar value, int axis)
1053 {
1054 switch(num)
1055 {
1056 case BT_CONSTRAINT_ERP :
1057 case BT_CONSTRAINT_STOP_ERP :
1058 if((axis >= 0) && (axis < 3))
1059 {
1060 m_linERP = value;
1061 m_flags |= BT_CONETWIST_FLAGS_LIN_ERP;
1062 }
1063 else
1064 {
1065 m_biasFactor = value;
1066 }
1067 break;
1068 case BT_CONSTRAINT_CFM :
1069 case BT_CONSTRAINT_STOP_CFM :
1070 if((axis >= 0) && (axis < 3))
1071 {
1072 m_linCFM = value;
1073 m_flags |= BT_CONETWIST_FLAGS_LIN_CFM;
1074 }
1075 else
1076 {
1077 m_angCFM = value;
1078 m_flags |= BT_CONETWIST_FLAGS_ANG_CFM;
1079 }
1080 break;
1081 default:
1082 btAssertConstrParams(0);
1083 break;
1084 }
1085 }
1086
1088 btScalar btConeTwistConstraint::getParam(int num, int axis) const
1089 {
1090 btScalar retVal = 0;
1091 switch(num)
1092 {
1093 case BT_CONSTRAINT_ERP :
1094 case BT_CONSTRAINT_STOP_ERP :
1095 if((axis >= 0) && (axis < 3))
1096 {
1097 btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_ERP);
1098 retVal = m_linERP;
1099 }
1100 else if((axis >= 3) && (axis < 6))
1101 {
1102 retVal = m_biasFactor;
1103 }
1104 else
1105 {
1106 btAssertConstrParams(0);
1107 }
1108 break;
1109 case BT_CONSTRAINT_CFM :
1110 case BT_CONSTRAINT_STOP_CFM :
1111 if((axis >= 0) && (axis < 3))
1112 {
1113 btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_LIN_CFM);
1114 retVal = m_linCFM;
1115 }
1116 else if((axis >= 3) && (axis < 6))
1117 {
1118 btAssertConstrParams(m_flags & BT_CONETWIST_FLAGS_ANG_CFM);
1119 retVal = m_angCFM;
1120 }
1121 else
1122 {
1123 btAssertConstrParams(0);
1124 }
1125 break;
1126 default :
1127 btAssertConstrParams(0);
1128 }
1129 return retVal;
1130 }
1131
1132
1133 void btConeTwistConstraint::setFrames(const btTransform & frameA, const btTransform & frameB)
1134 {
1135 m_rbAFrame = frameA;
1136 m_rbBFrame = frameB;
1137 buildJacobian();
1138 //calculateTransforms();
1139 }
1140
1141
1142
1143
btTransform::setOrigin
void setOrigin(const btVector3 &origin)
Set the translational element.
Definition: btTransform.h:150
btTypedConstraint::btConstraintInfo2::m_constraintError
btScalar * m_constraintError
Definition: btTypedConstraint.h:143
btRigidBody::getInvMass
btScalar getInvMass() const
Definition: btRigidBody.h:270
btConeTwistConstraint::m_qTarget
btQuaternion m_qTarget
Definition: btConeTwistConstraint.h:113
btConeTwistConstraint::setLimit
void setLimit(int limitIndex, btScalar limitValue)
Definition: btConeTwistConstraint.h:179
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:494
btTransformUtil.h
btConeTwistConstraint::m_maxMotorImpulse
btScalar m_maxMotorImpulse
Definition: btConeTwistConstraint.h:114
computeAngularImpulseDenominator
btScalar computeAngularImpulseDenominator(const btVector3 &axis, const btMatrix3x3 &invInertiaWorld)
Definition: btConeTwistConstraint.cpp:32
btQuaternion::getAngle
btScalar getAngle() const
Return the angle of rotation represented by this quaternion.
Definition: btQuaternion.h:422
btConeTwistConstraint::buildJacobian
virtual void buildJacobian()
internal method used by the constraint solver, don&#39;t use them directly
Definition: btConeTwistConstraint.cpp:263
btTypedConstraint::btConstraintInfo2::m_J2angularAxis
btScalar * m_J2angularAxis
Definition: btTypedConstraint.h:135
btConeTwistConstraint::m_twistAxis
btVector3 m_twistAxis
Definition: btConeTwistConstraint.h:85
BT_LARGE_FLOAT
#define BT_LARGE_FLOAT
Definition: btScalar.h:280
btTypedConstraint::btConstraintInfo2::m_upperLimit
btScalar * m_upperLimit
Definition: btTypedConstraint.h:146
btConeTwistConstraint::setMotorTarget
void setMotorTarget(const btQuaternion &q)
Definition: btConeTwistConstraint.cpp:986
btConeTwistConstraint::m_kTwist
btScalar m_kTwist
Definition: btConeTwistConstraint.h:88
btConeTwistConstraint::m_swingCorrection
btScalar m_swingCorrection
Definition: btConeTwistConstraint.h:91
vTwist
static btVector3 vTwist(1, 0, 0)
btConeTwistConstraint::m_flags
int m_flags
Definition: btConeTwistConstraint.h:118
btRigidBody::computeAngularImpulseDenominator
btScalar computeAngularImpulseDenominator(const btVector3 &axis) const
Definition: btRigidBody.h:412
btTypedConstraint::m_rbA
btRigidBody & m_rbA
Definition: btTypedConstraint.h:102
btTransform::getRotation
btQuaternion getRotation() const
Return a quaternion representing the rotation.
Definition: btTransform.h:122
btVector3::setValue
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:640
btJacobianEntry
Jacobian entry is an abstraction that allows to describe constraints it can be used in combination wi...
Definition: btJacobianEntry.h:30
btTypedConstraint::btConstraintInfo2
Definition: btTypedConstraint.h:126
CONETWIST_DEF_FIX_THRESH
#define CONETWIST_DEF_FIX_THRESH
Definition: btConeTwistConstraint.cpp:29
BT_CONETWIST_FLAGS_ANG_CFM
Definition: btConeTwistConstraint.h:58
btSolverBody::internalApplyImpulse
void internalApplyImpulse(const btVector3 &linearComponent, const btVector3 &angularComponent, const btScalar impulseMagnitude)
Definition: btSolverBody.h:255
btSin
btScalar btSin(btScalar x)
Definition: btScalar.h:451
btConeTwistConstraint::getInfo1
virtual void getInfo1(btConstraintInfo1 *info)
internal method used by the constraint solver, don&#39;t use them directly
Definition: btConeTwistConstraint.cpp:79
btConeTwistConstraint::getRigidBodyB
const btRigidBody & getRigidBodyB() const
Definition: btConeTwistConstraint.h:164
CONETWIST_USE_OBSOLETE_SOLVER
#define CONETWIST_USE_OBSOLETE_SOLVER
Definition: btConeTwistConstraint.cpp:28
btVector3::setZ
void setZ(btScalar _z)
Set the z value.
Definition: btVector3.h:571
btQuadWord::z
const btScalar & z() const
Return the z value.
Definition: btQuadWord.h:120
btSolverBody::internalGetVelocityInLocalPointObsolete
void internalGetVelocityInLocalPointObsolete(const btVector3 &rel_pos, btVector3 &velocity) const
Definition: btSolverBody.h:243
btConeTwistConstraint::setParam
virtual void setParam(int num, btScalar value, int axis=-1)
override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0...
Definition: btConeTwistConstraint.cpp:1052
btConeTwistConstraint::m_angularOnly
bool m_angularOnly
Definition: btConeTwistConstraint.h:99
btPlaneSpace1
void btPlaneSpace1(const T &n, T &p, T &q)
Definition: btVector3.h:1272
btTransform::setIdentity
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:172
btTypedConstraint::btConstraintInfo1::m_numConstraintRows
int m_numConstraintRows
Definition: btTypedConstraint.h:121
btAssert
#define btAssert(x)
Definition: btScalar.h:113
btTypedConstraint::btConstraintInfo2::cfm
btScalar * cfm
Definition: btTypedConstraint.h:143
SIMD_FORCE_INLINE
#define SIMD_FORCE_INLINE
Definition: btScalar.h:63
btTypedConstraint::btConstraintInfo2::m_J1angularAxis
btScalar * m_J1angularAxis
Definition: btTypedConstraint.h:135
btQuadWord::y
const btScalar & y() const
Return the y value.
Definition: btQuadWord.h:118
BT_CONSTRAINT_CFM
Definition: btTypedConstraint.h:56
btMatrix3x3::getColumn
btVector3 getColumn(int i) const
Get a column of the matrix as a vector.
Definition: btMatrix3x3.h:134
btConeTwistConstraint::m_biasFactor
btScalar m_biasFactor
Definition: btConeTwistConstraint.h:73
btConeTwistConstraint::m_twistLimitRatio
btScalar m_twistLimitRatio
Definition: btConeTwistConstraint.h:107
btSolverBody::internalGetAngularVelocity
void internalGetAngularVelocity(btVector3 &angVel) const
Definition: btSolverBody.h:248
btConeTwistConstraint::init
void init()
Definition: btConeTwistConstraint.cpp:61
btRigidBody.h
btConeTwistConstraint::m_angCFM
btScalar m_angCFM
Definition: btConeTwistConstraint.h:121
btConeTwistConstraint::m_relaxationFactor
btScalar m_relaxationFactor
Definition: btConeTwistConstraint.h:74
btConeTwistConstraint::getParam
virtual btScalar getParam(int num, int axis=-1) const
return the local value of parameter
Definition: btConeTwistConstraint.cpp:1088
btConeTwistConstraint::m_solveTwistLimit
bool m_solveTwistLimit
Definition: btConeTwistConstraint.h:100
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
btConeTwistConstraint::setMotorTargetInConstraintSpace
void setMotorTargetInConstraintSpace(const btQuaternion &q)
Definition: btConeTwistConstraint.cpp:1000
btVector3::normalize
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
Definition: btVector3.h:297
btVector3::x
const btScalar & x() const
Return the x value.
Definition: btVector3.h:575
quatRotate
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
Definition: btQuaternion.h:884
BT_CONSTRAINT_STOP_ERP
Definition: btTypedConstraint.h:55
btConeTwistConstraint::updateRHS
void updateRHS(btScalar timeStep)
Definition: btConeTwistConstraint.cpp:528
btConeTwistConstraint::m_twistLimitSign
btScalar m_twistLimitSign
Definition: btConeTwistConstraint.h:90
SIMD_PI
#define SIMD_PI
Definition: btScalar.h:476
SIMD_INFINITY
#define SIMD_INFINITY
Definition: btScalar.h:495
btJacobianEntry::m_linearJointAxis
btVector3 m_linearJointAxis
Definition: btJacobianEntry.h:145
BT_CONSTRAINT_STOP_CFM
Definition: btTypedConstraint.h:57
btTypedConstraint::btConstraintInfo2::m_J1linearAxis
btScalar * m_J1linearAxis
Definition: btTypedConstraint.h:135
btTransform::getOrigin
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:117
btConeTwistConstraint::m_solveSwingLimit
bool m_solveSwingLimit
Definition: btConeTwistConstraint.h:101
shortestArcQuat
btQuaternion shortestArcQuat(const btVector3 &v0, const btVector3 &v1)
Definition: btQuaternion.h:898
btConeTwistConstraint::btConeTwistConstraint
btConeTwistConstraint(btRigidBody &rbA, btRigidBody &rbB, const btTransform &rbAFrame, const btTransform &rbBFrame)
Definition: btConeTwistConstraint.cpp:41
btConeTwistConstraint::solveConstraintObsolete
virtual void solveConstraintObsolete(btSolverBody &bodyA, btSolverBody &bodyB, btScalar timeStep)
internal method used by the constraint solver, don&#39;t use them directly
Definition: btConeTwistConstraint.cpp:311
btRigidBody::getCenterOfMassTransform
const btTransform & getCenterOfMassTransform() const
Definition: btRigidBody.h:356
btQuaternion::normalize
btQuaternion & normalize()
Normalize the quaternion Such that x^2 + y^2 + z^2 +w^2 = 1.
Definition: btQuaternion.h:339
btConeTwistConstraint::m_limitSoftness
btScalar m_limitSoftness
Definition: btConeTwistConstraint.h:72
btJacobianEntry::getDiagonal
btScalar getDiagonal() const
Definition: btJacobianEntry.h:106
btTypedConstraint::btConstraintInfo2::erp
btScalar erp
Definition: btTypedConstraint.h:129
btAtan2Fast
btScalar btAtan2Fast(btScalar y, btScalar x)
Definition: btScalar.h:502
btAtan2
btScalar btAtan2(btScalar x, btScalar y)
Definition: btScalar.h:468
btVector3::cross
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:377
btConeTwistConstraint::m_accMotorImpulse
btVector3 m_accMotorImpulse
Definition: btConeTwistConstraint.h:115
btTypedConstraint::btConstraintInfo2::m_lowerLimit
btScalar * m_lowerLimit
Definition: btTypedConstraint.h:146
btVector3::setY
void setY(btScalar _y)
Set the y value.
Definition: btVector3.h:569
btTransform::getBasis
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:112
btConeTwistConstraint::adjustSwingAxisToUseEllipseNormal
void adjustSwingAxisToUseEllipseNormal(btVector3 &vSwingAxis) const
Definition: btConeTwistConstraint.cpp:952
btConeTwistConstraint.h
btRigidBody::getCenterOfMassPosition
const btVector3 & getCenterOfMassPosition() const
Definition: btRigidBody.h:351
btQuaternion::inverse
btQuaternion inverse() const
Return the inverse of this quaternion.
Definition: btQuaternion.h:453
btRigidBody
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:62
btVector3::length
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:263
btConeTwistConstraint::m_useSolveConstraintObsolete
bool m_useSolveConstraintObsolete
Definition: btConeTwistConstraint.h:103
btConeTwistConstraint::getInfo2NonVirtual
void getInfo2NonVirtual(btConstraintInfo2 *info, const btTransform &transA, const btTransform &transB, const btMatrix3x3 &invInertiaWorldA, const btMatrix3x3 &invInertiaWorldB)
Definition: btConeTwistConstraint.cpp:123
btConeTwistConstraint::m_damping
btScalar m_damping
Definition: btConeTwistConstraint.h:76
btTransform::inverse
btTransform inverse() const
Return the inverse of this transform.
Definition: btTransform.h:188
btConeTwistConstraint::m_twistCorrection
btScalar m_twistCorrection
Definition: btConeTwistConstraint.h:92
btConeTwistConstraint::m_fixThresh
btScalar m_fixThresh
Definition: btConeTwistConstraint.h:82
btConeTwistConstraint::m_rbAFrame
btTransform m_rbAFrame
Definition: btConeTwistConstraint.h:69
btMinMax.h
btVector3::y
const btScalar & y() const
Return the y value.
Definition: btVector3.h:577
btConeTwistConstraint::m_bMotorEnabled
bool m_bMotorEnabled
Definition: btConeTwistConstraint.h:111
btConeTwistConstraint::m_swingSpan1
btScalar m_swingSpan1
Definition: btConeTwistConstraint.h:78
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
btTransformUtil::integrateTransform
static void integrateTransform(const btTransform &curTrans, const btVector3 &linvel, const btVector3 &angvel, btScalar timeStep, btTransform &predictedTransform)
Definition: btTransformUtil.h:43
btVector3::length2
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:257
btTypedConstraint::btConstraintInfo2::rowskip
int rowskip
Definition: btTypedConstraint.h:138
btConeTwistConstraint::m_linCFM
btScalar m_linCFM
Definition: btConeTwistConstraint.h:119
btConeTwistConstraint::m_kSwing
btScalar m_kSwing
Definition: btConeTwistConstraint.h:87
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
btTypedConstraint::m_rbB
btRigidBody & m_rbB
Definition: btTypedConstraint.h:103
btConeTwistConstraint::m_linERP
btScalar m_linERP
Definition: btConeTwistConstraint.h:120
BT_CONETWIST_FLAGS_LIN_CFM
Definition: btConeTwistConstraint.h:56
btSolverBody
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
Definition: btSolverBody.h:108
btVector3::normalized
btVector3 normalized() const
Return a normalized version of this vector.
Definition: btVector3.h:952
btConeTwistConstraint::m_swingAxis
btVector3 m_swingAxis
Definition: btConeTwistConstraint.h:84
btConeTwistConstraint::calcAngleInfo
void calcAngleInfo()
Definition: btConeTwistConstraint.cpp:536
btConeTwistConstraint::computeConeLimitInfo
void computeConeLimitInfo(const btQuaternion &qCone, btScalar &swingAngle, btVector3 &vSwingAxis, btScalar &swingLimit)
Definition: btConeTwistConstraint.cpp:829
btTypedConstraint::btConstraintInfo2::m_J2linearAxis
btScalar * m_J2linearAxis
Definition: btTypedConstraint.h:135
btConeTwistConstraint::m_rbBFrame
btTransform m_rbBFrame
Definition: btConeTwistConstraint.h:70
btConeTwistConstraint::m_jac
btJacobianEntry m_jac[3]
Definition: btConeTwistConstraint.h:67
CONETWIST_CONSTRAINT_TYPE
Definition: btTypedConstraint.h:40
btTypedConstraint
TypedConstraint is the baseclass for Bullet constraints and vehicles.
Definition: btTypedConstraint.h:78
btFuzzyZero
bool btFuzzyZero(btScalar x)
Definition: btScalar.h:518
btConeTwistConstraint::m_twistSpan
btScalar m_twistSpan
Definition: btConeTwistConstraint.h:80
btConeTwistConstraint::m_twistAngle
btScalar m_twistAngle
Definition: btConeTwistConstraint.h:94
btMatrix3x3::transpose
btMatrix3x3 transpose() const
Return the transpose of the matrix.
Definition: btMatrix3x3.h:1001
btTypedConstraint::btConstraintInfo1::nub
int nub
Definition: btTypedConstraint.h:121
btConeTwistConstraint::calcAngleInfo2
void calcAngleInfo2(const btTransform &transA, const btTransform &transB, const btMatrix3x3 &invInertiaWorldA, const btMatrix3x3 &invInertiaWorldB)
Definition: btConeTwistConstraint.cpp:627
btConeTwistConstraint::setFrames
virtual void setFrames(const btTransform &frameA, const btTransform &frameB)
Definition: btConeTwistConstraint.cpp:1133
btConeTwistConstraint::m_accSwingLimitImpulse
btScalar m_accSwingLimitImpulse
Definition: btConeTwistConstraint.h:96
btTypedConstraint::btConstraintInfo2::fps
btScalar fps
Definition: btTypedConstraint.h:129
btMax
const T & btMax(const T &a, const T &b)
Definition: btMinMax.h:29
btRigidBody::getInvInertiaTensorWorld
const btMatrix3x3 & getInvInertiaTensorWorld() const
Definition: btRigidBody.h:271
btConeTwistConstraint::getInfo1NonVirtual
void getInfo1NonVirtual(btConstraintInfo1 *info)
Definition: btConeTwistConstraint.cpp:109
BT_CONSTRAINT_ERP
Definition: btTypedConstraint.h:54
btConeTwistConstraint::m_twistAxisA
btVector3 m_twistAxisA
Definition: btConeTwistConstraint.h:108
btTypedConstraint::m_appliedImpulse
btScalar m_appliedImpulse
Definition: btTypedConstraint.h:104
btConeTwistConstraint::GetPointForAngle
btVector3 GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const
Definition: btConeTwistConstraint.cpp:893
btQuadWord::x
const btScalar & x() const
Return the x value.
Definition: btQuadWord.h:116
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:48
btRigidBody::getInvInertiaDiagLocal
const btVector3 & getInvInertiaDiagLocal() const
Definition: btRigidBody.h:294
dot
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:845
btConeTwistConstraint::getInfo2
virtual void getInfo2(btConstraintInfo2 *info)
internal method used by the constraint solver, don&#39;t use them directly
Definition: btConeTwistConstraint.cpp:118
btConeTwistConstraint::getRigidBodyA
const btRigidBody & getRigidBodyA() const
Definition: btConeTwistConstraint.h:160
BT_CONETWIST_FLAGS_LIN_ERP
Definition: btConeTwistConstraint.h:57
btConeTwistConstraint::m_swingLimitRatio
btScalar m_swingLimitRatio
Definition: btConeTwistConstraint.h:106
btConeTwistConstraint::m_accTwistLimitImpulse
btScalar m_accTwistLimitImpulse
Definition: btConeTwistConstraint.h:97
btQuaternion
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
Definition: btQuaternion.h:55
btAssertConstrParams
#define btAssertConstrParams(_par)
Definition: btTypedConstraint.h:61
btTypedConstraint::btConstraintInfo1
Definition: btTypedConstraint.h:120
btConeTwistConstraint::m_swingSpan2
btScalar m_swingSpan2
Definition: btConeTwistConstraint.h:79
btTransformUtil::calculateVelocity
static void calculateVelocity(const btTransform &transform0, const btTransform &transform1, btScalar timeStep, btVector3 &linVel, btVector3 &angVel)
Definition: btTransformUtil.h:112
btConeTwistConstraint::m_bNormalizedMotorStrength
bool m_bNormalizedMotorStrength
Definition: btConeTwistConstraint.h:112
btVector3::getSkewSymmetricMatrix
void getSkewSymmetricMatrix(btVector3 *v0, btVector3 *v1, btVector3 *v2) const
Definition: btVector3.h:648
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:278
btConeTwistConstraint::computeTwistLimitInfo
void computeTwistLimitInfo(const btQuaternion &qTwist, btScalar &twistAngle, btVector3 &vTwistAxis)
Definition: btConeTwistConstraint.cpp:926
btCos
btScalar btCos(btScalar x)
Definition: btScalar.h:450
btFabs
btScalar btFabs(btScalar x)
Definition: btScalar.h:449
btVector3::z
const btScalar & z() const
Return the z value.
Definition: btVector3.h:579