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

Bullet Collision Detection & Physics Library

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