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

Bullet Collision Detection & Physics Library

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