Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/trunk/src/external/bullet/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp @ 8393

Last change on this file since 8393 was 8393, checked in by rgrieder, 13 years ago

Updated Bullet from v2.77 to v2.78.
(I'm not going to make a branch for that since the update from 2.74 to 2.77 hasn't been tested that much either).

You will HAVE to do a complete RECOMPILE! I tested with MSVC and MinGW and they both threw linker errors at me.

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