Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/kicklib/src/external/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp @ 8633

Last change on this file since 8633 was 7983, checked in by rgrieder, 14 years ago

Updated Bullet Physics Engine from v2.74 to v2.77

  • Property svn:eol-style set to native
File size: 30.9 KB
RevLine 
[1963]1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
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*/
15
16
17#include "btHingeConstraint.h"
18#include "BulletDynamics/Dynamics/btRigidBody.h"
19#include "LinearMath/btTransformUtil.h"
20#include "LinearMath/btMinMax.h"
21#include <new>
[2882]22#include "btSolverBody.h"
[1963]23
24
[7983]25
26//#define HINGE_USE_OBSOLETE_SOLVER false
[2882]27#define HINGE_USE_OBSOLETE_SOLVER false
28
[7983]29#define HINGE_USE_FRAME_OFFSET true
[2882]30
[7983]31#ifndef __SPU__
[2882]32
[1963]33
[2882]34
[7983]35
36
[1963]37btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
[7983]38                                                                         const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA)
[1963]39                                                                         :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
40                                                                         m_angularOnly(false),
[2882]41                                                                         m_enableAngularMotor(false),
42                                                                         m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
[7983]43                                                                         m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
44                                                                         m_useReferenceFrameA(useReferenceFrameA),
45                                                                         m_flags(0)
[1963]46{
47        m_rbAFrame.getOrigin() = pivotInA;
48       
49        // since no frame is given, assume this to be zero angle and just pick rb transform axis
50        btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0);
51
52        btVector3 rbAxisA2;
53        btScalar projection = axisInA.dot(rbAxisA1);
54        if (projection >= 1.0f - SIMD_EPSILON) {
55                rbAxisA1 = -rbA.getCenterOfMassTransform().getBasis().getColumn(2);
56                rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
57        } else if (projection <= -1.0f + SIMD_EPSILON) {
58                rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(2);
59                rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);     
60        } else {
61                rbAxisA2 = axisInA.cross(rbAxisA1);
62                rbAxisA1 = rbAxisA2.cross(axisInA);
63        }
64
65        m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
66                                                                        rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
67                                                                        rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
68
69        btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
70        btVector3 rbAxisB1 =  quatRotate(rotationArc,rbAxisA1);
71        btVector3 rbAxisB2 =  axisInB.cross(rbAxisB1); 
72       
73        m_rbBFrame.getOrigin() = pivotInB;
[2882]74        m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
75                                                                        rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
76                                                                        rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
[1963]77       
78        //start with free
[7983]79        m_lowerLimit = btScalar(1.0f);
80        m_upperLimit = btScalar(-1.0f);
[1963]81        m_biasFactor = 0.3f;
82        m_relaxationFactor = 1.0f;
83        m_limitSoftness = 0.9f;
84        m_solveLimit = false;
[2882]85        m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
[1963]86}
87
88
[7983]89
90btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA)
[2882]91:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false), 
92m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
[7983]93m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
94m_useReferenceFrameA(useReferenceFrameA),
95m_flags(0)
[1963]96{
97
98        // since no frame is given, assume this to be zero angle and just pick rb transform axis
99        // fixed axis in worldspace
100        btVector3 rbAxisA1, rbAxisA2;
101        btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2);
102
103        m_rbAFrame.getOrigin() = pivotInA;
104        m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
105                                                                        rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
106                                                                        rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
107
[2882]108        btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * axisInA;
[1963]109
110        btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
111        btVector3 rbAxisB1 =  quatRotate(rotationArc,rbAxisA1);
112        btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
113
114
115        m_rbBFrame.getOrigin() = rbA.getCenterOfMassTransform()(pivotInA);
116        m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
117                                                                        rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
118                                                                        rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
119       
120        //start with free
[7983]121        m_lowerLimit = btScalar(1.0f);
122        m_upperLimit = btScalar(-1.0f);
[1963]123        m_biasFactor = 0.3f;
124        m_relaxationFactor = 1.0f;
125        m_limitSoftness = 0.9f;
126        m_solveLimit = false;
[2882]127        m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
[1963]128}
129
[2882]130
[7983]131
[1963]132btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, 
[2882]133                                                                     const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA)
[1963]134:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
135m_angularOnly(false),
[2882]136m_enableAngularMotor(false),
137m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
[7983]138m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
139m_useReferenceFrameA(useReferenceFrameA),
140m_flags(0)
[1963]141{
142        //start with free
[7983]143        m_lowerLimit = btScalar(1.0f);
144        m_upperLimit = btScalar(-1.0f);
[1963]145        m_biasFactor = 0.3f;
146        m_relaxationFactor = 1.0f;
147        m_limitSoftness = 0.9f;
148        m_solveLimit = false;
[2882]149        m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
[1963]150}                       
151
152
[7983]153
[2882]154btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame, bool useReferenceFrameA)
[1963]155:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame),
156m_angularOnly(false),
[2882]157m_enableAngularMotor(false),
158m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
[7983]159m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
160m_useReferenceFrameA(useReferenceFrameA),
161m_flags(0)
[1963]162{
163        ///not providing rigidbody B means implicitly using worldspace for body B
164
165        m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin());
166
167        //start with free
[7983]168        m_lowerLimit = btScalar(1.0f);
169        m_upperLimit = btScalar(-1.0f);
[1963]170        m_biasFactor = 0.3f;
171        m_relaxationFactor = 1.0f;
172        m_limitSoftness = 0.9f;
173        m_solveLimit = false;
[2882]174        m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
[1963]175}
176
[2882]177
[7983]178
[1963]179void    btHingeConstraint::buildJacobian()
180{
[2882]181        if (m_useSolveConstraintObsolete)
[1963]182        {
[2882]183                m_appliedImpulse = btScalar(0.);
[7983]184                m_accMotorImpulse = btScalar(0.);
[1963]185
[2882]186                if (!m_angularOnly)
[1963]187                {
[2882]188                        btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
189                        btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
190                        btVector3 relPos = pivotBInW - pivotAInW;
[1963]191
[2882]192                        btVector3 normal[3];
193                        if (relPos.length2() > SIMD_EPSILON)
194                        {
195                                normal[0] = relPos.normalized();
196                        }
197                        else
198                        {
199                                normal[0].setValue(btScalar(1.0),0,0);
200                        }
[1963]201
[2882]202                        btPlaneSpace1(normal[0], normal[1], normal[2]);
203
204                        for (int i=0;i<3;i++)
205                        {
206                                new (&m_jac[i]) btJacobianEntry(
[1963]207                                m_rbA.getCenterOfMassTransform().getBasis().transpose(),
208                                m_rbB.getCenterOfMassTransform().getBasis().transpose(),
209                                pivotAInW - m_rbA.getCenterOfMassPosition(),
210                                pivotBInW - m_rbB.getCenterOfMassPosition(),
211                                normal[i],
212                                m_rbA.getInvInertiaDiagLocal(),
213                                m_rbA.getInvMass(),
214                                m_rbB.getInvInertiaDiagLocal(),
215                                m_rbB.getInvMass());
[2882]216                        }
[1963]217                }
218
[2882]219                //calculate two perpendicular jointAxis, orthogonal to hingeAxis
220                //these two jointAxis require equal angular velocities for both bodies
[1963]221
[2882]222                //this is unused for now, it's a todo
223                btVector3 jointAxis0local;
224                btVector3 jointAxis1local;
[1963]225               
[2882]226                btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local);
[1963]227
[2882]228                btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local;
229                btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local;
230                btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
231                       
232                new (&m_jacAng[0])      btJacobianEntry(jointAxis0,
233                        m_rbA.getCenterOfMassTransform().getBasis().transpose(),
234                        m_rbB.getCenterOfMassTransform().getBasis().transpose(),
235                        m_rbA.getInvInertiaDiagLocal(),
236                        m_rbB.getInvInertiaDiagLocal());
[1963]237
[2882]238                new (&m_jacAng[1])      btJacobianEntry(jointAxis1,
239                        m_rbA.getCenterOfMassTransform().getBasis().transpose(),
240                        m_rbB.getCenterOfMassTransform().getBasis().transpose(),
241                        m_rbA.getInvInertiaDiagLocal(),
242                        m_rbB.getInvInertiaDiagLocal());
[1963]243
[2882]244                new (&m_jacAng[2])      btJacobianEntry(hingeAxisWorld,
245                        m_rbA.getCenterOfMassTransform().getBasis().transpose(),
246                        m_rbB.getCenterOfMassTransform().getBasis().transpose(),
247                        m_rbA.getInvInertiaDiagLocal(),
248                        m_rbB.getInvInertiaDiagLocal());
[1963]249
[2882]250                        // clear accumulator
251                        m_accLimitImpulse = btScalar(0.);
[1963]252
[2882]253                        // test angular limit
[7983]254                        testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
[1963]255
[2882]256                //Compute K = J*W*J' for hinge axis
257                btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
258                m_kHinge =   1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) +
259                                                         getRigidBodyB().computeAngularImpulseDenominator(axisA));
260
261        }
262}
263
264
[7983]265#endif //__SPU__
266
267
[2882]268void btHingeConstraint::getInfo1(btConstraintInfo1* info)
269{
270        if (m_useSolveConstraintObsolete)
[1963]271        {
[2882]272                info->m_numConstraintRows = 0;
273                info->nub = 0;
274        }
275        else
276        {
277                info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular
278                info->nub = 1; 
[7983]279                //always add the row, to avoid computation (data is not available yet)
[2882]280                //prepare constraint
[7983]281                testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
[2882]282                if(getSolveLimit() || getEnableAngularMotor())
[1963]283                {
[2882]284                        info->m_numConstraintRows++; // limit 3rd anguar as well
285                        info->nub--; 
[1963]286                }
[7983]287
[1963]288        }
[7983]289}
[1963]290
[7983]291void btHingeConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
292{
293        if (m_useSolveConstraintObsolete)
294        {
295                info->m_numConstraintRows = 0;
296                info->nub = 0;
297        }
298        else
299        {
300                //always add the 'limit' row, to avoid computation (data is not available yet)
301                info->m_numConstraintRows = 6; // Fixed 3 linear + 2 angular
302                info->nub = 0; 
303        }
304}
[1963]305
[2882]306void btHingeConstraint::getInfo2 (btConstraintInfo2* info)
307{
[7983]308        if(m_useOffsetForConstraintFrame)
309        {
310                getInfo2InternalUsingFrameOffset(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity());
311        }
312        else
313        {
314                getInfo2Internal(info, m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(),m_rbA.getAngularVelocity(),m_rbB.getAngularVelocity());
315        }
316}
317
318
319void    btHingeConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
320{
321        ///the regular (virtual) implementation getInfo2 already performs 'testLimit' during getInfo1, so we need to do it now
322        testLimit(transA,transB);
323
324        getInfo2Internal(info,transA,transB,angVelA,angVelB);
325}
326
327
328void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
329{
330
[2882]331        btAssert(!m_useSolveConstraintObsolete);
[7983]332        int i, skip = info->rowskip;
[2882]333        // transforms in world space
[7983]334        btTransform trA = transA*m_rbAFrame;
335        btTransform trB = transB*m_rbBFrame;
[2882]336        // pivot point
337        btVector3 pivotAInW = trA.getOrigin();
338        btVector3 pivotBInW = trB.getOrigin();
[7983]339#if 0
340        if (0)
341        {
342                for (i=0;i<6;i++)
343                {
344                        info->m_J1linearAxis[i*skip]=0;
345                        info->m_J1linearAxis[i*skip+1]=0;
346                        info->m_J1linearAxis[i*skip+2]=0;
347
348                        info->m_J1angularAxis[i*skip]=0;
349                        info->m_J1angularAxis[i*skip+1]=0;
350                        info->m_J1angularAxis[i*skip+2]=0;
351
352                        info->m_J2angularAxis[i*skip]=0;
353                        info->m_J2angularAxis[i*skip+1]=0;
354                        info->m_J2angularAxis[i*skip+2]=0;
355
356                        info->m_constraintError[i*skip]=0.f;
357                }
358        }
359#endif //#if 0
[2882]360        // linear (all fixed)
[7983]361
362        if (!m_angularOnly)
[2882]363        {
[7983]364                info->m_J1linearAxis[0] = 1;
365                info->m_J1linearAxis[skip + 1] = 1;
366                info->m_J1linearAxis[2 * skip + 2] = 1;
367        }       
368
369
370
371
372        btVector3 a1 = pivotAInW - transA.getOrigin();
373        {
[2882]374                btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
[7983]375                btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + skip);
376                btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * skip);
[2882]377                btVector3 a1neg = -a1;
378                a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
379        }
[7983]380        btVector3 a2 = pivotBInW - transB.getOrigin();
[2882]381        {
382                btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
[7983]383                btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + skip);
384                btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * skip);
[2882]385                a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
386        }
387        // linear RHS
388    btScalar k = info->fps * info->erp;
[7983]389        if (!m_angularOnly)
390        {
391                for(i = 0; i < 3; i++)
392                {
393                        info->m_constraintError[i * skip] = k * (pivotBInW[i] - pivotAInW[i]);
394                }
395        }
[2882]396        // make rotations around X and Y equal
397        // the hinge axis should be the only unconstrained
398        // rotational axis, the angular velocity of the two bodies perpendicular to
399        // the hinge axis should be equal. thus the constraint equations are
400        //    p*w1 - p*w2 = 0
401        //    q*w1 - q*w2 = 0
402        // where p and q are unit vectors normal to the hinge axis, and w1 and w2
403        // are the angular velocity vectors of the two bodies.
404        // get hinge axis (Z)
405        btVector3 ax1 = trA.getBasis().getColumn(2);
406        // get 2 orthos to hinge axis (X, Y)
407        btVector3 p = trA.getBasis().getColumn(0);
408        btVector3 q = trA.getBasis().getColumn(1);
409        // set the two hinge angular rows
410    int s3 = 3 * info->rowskip;
411    int s4 = 4 * info->rowskip;
412
413        info->m_J1angularAxis[s3 + 0] = p[0];
414        info->m_J1angularAxis[s3 + 1] = p[1];
415        info->m_J1angularAxis[s3 + 2] = p[2];
416        info->m_J1angularAxis[s4 + 0] = q[0];
417        info->m_J1angularAxis[s4 + 1] = q[1];
418        info->m_J1angularAxis[s4 + 2] = q[2];
419
420        info->m_J2angularAxis[s3 + 0] = -p[0];
421        info->m_J2angularAxis[s3 + 1] = -p[1];
422        info->m_J2angularAxis[s3 + 2] = -p[2];
423        info->m_J2angularAxis[s4 + 0] = -q[0];
424        info->m_J2angularAxis[s4 + 1] = -q[1];
425        info->m_J2angularAxis[s4 + 2] = -q[2];
426    // compute the right hand side of the constraint equation. set relative
427    // body velocities along p and q to bring the hinge back into alignment.
428    // if ax1,ax2 are the unit length hinge axes as computed from body1 and
429    // body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
430    // if `theta' is the angle between ax1 and ax2, we need an angular velocity
431    // along u to cover angle erp*theta in one step :
432    //   |angular_velocity| = angle/time = erp*theta / stepsize
433    //                      = (erp*fps) * theta
434    //    angular_velocity  = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
435    //                      = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
436    // ...as ax1 and ax2 are unit length. if theta is smallish,
437    // theta ~= sin(theta), so
438    //    angular_velocity  = (erp*fps) * (ax1 x ax2)
439    // ax1 x ax2 is in the plane space of ax1, so we project the angular
440    // velocity to p and q to find the right hand side.
441    btVector3 ax2 = trB.getBasis().getColumn(2);
442        btVector3 u = ax1.cross(ax2);
443        info->m_constraintError[s3] = k * u.dot(p);
444        info->m_constraintError[s4] = k * u.dot(q);
445        // check angular limits
446        int nrow = 4; // last filled row
447        int srow;
448        btScalar limit_err = btScalar(0.0);
449        int limit = 0;
450        if(getSolveLimit())
451        {
452                limit_err = m_correction * m_referenceSign;
453                limit = (limit_err > btScalar(0.0)) ? 1 : 2;
454        }
455        // if the hinge has joint limits or motor, add in the extra row
456        int powered = 0;
457        if(getEnableAngularMotor())
458        {
459                powered = 1;
460        }
461        if(limit || powered) 
462        {
463                nrow++;
464                srow = nrow * info->rowskip;
465                info->m_J1angularAxis[srow+0] = ax1[0];
466                info->m_J1angularAxis[srow+1] = ax1[1];
467                info->m_J1angularAxis[srow+2] = ax1[2];
468
469                info->m_J2angularAxis[srow+0] = -ax1[0];
470                info->m_J2angularAxis[srow+1] = -ax1[1];
471                info->m_J2angularAxis[srow+2] = -ax1[2];
472
473                btScalar lostop = getLowerLimit();
474                btScalar histop = getUpperLimit();
475                if(limit && (lostop == histop))
476                {  // the joint motor is ineffective
477                        powered = 0;
478                }
479                info->m_constraintError[srow] = btScalar(0.0f);
[7983]480                btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp;
[2882]481                if(powered)
482                {
[7983]483                        if(m_flags & BT_HINGE_FLAGS_CFM_NORM)
484                        {
485                                info->cfm[srow] = m_normalCFM;
486                        }
487                        btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP);
[2882]488                        info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
489                        info->m_lowerLimit[srow] = - m_maxMotorImpulse;
490                        info->m_upperLimit[srow] =   m_maxMotorImpulse;
491                }
492                if(limit)
493                {
[7983]494                        k = info->fps * currERP;
[2882]495                        info->m_constraintError[srow] += k * limit_err;
[7983]496                        if(m_flags & BT_HINGE_FLAGS_CFM_STOP)
497                        {
498                                info->cfm[srow] = m_stopCFM;
499                        }
[2882]500                        if(lostop == histop) 
501                        {
502                                // limited low and high simultaneously
503                                info->m_lowerLimit[srow] = -SIMD_INFINITY;
504                                info->m_upperLimit[srow] = SIMD_INFINITY;
505                        }
506                        else if(limit == 1) 
507                        { // low limit
508                                info->m_lowerLimit[srow] = 0;
509                                info->m_upperLimit[srow] = SIMD_INFINITY;
510                        }
511                        else 
512                        { // high limit
513                                info->m_lowerLimit[srow] = -SIMD_INFINITY;
514                                info->m_upperLimit[srow] = 0;
515                        }
516                        // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
517                        btScalar bounce = m_relaxationFactor;
518                        if(bounce > btScalar(0.0))
519                        {
[7983]520                                btScalar vel = angVelA.dot(ax1);
521                                vel -= angVelB.dot(ax1);
[2882]522                                // only apply bounce if the velocity is incoming, and if the
523                                // resulting c[] exceeds what we already have.
524                                if(limit == 1)
525                                {       // low limit
526                                        if(vel < 0)
527                                        {
528                                                btScalar newc = -bounce * vel;
529                                                if(newc > info->m_constraintError[srow])
530                                                {
531                                                        info->m_constraintError[srow] = newc;
532                                                }
533                                        }
534                                }
535                                else
536                                {       // high limit - all those computations are reversed
537                                        if(vel > 0)
538                                        {
539                                                btScalar newc = -bounce * vel;
540                                                if(newc < info->m_constraintError[srow])
541                                                {
542                                                        info->m_constraintError[srow] = newc;
543                                                }
544                                        }
545                                }
546                        }
547                        info->m_constraintError[srow] *= m_biasFactor;
548                } // if(limit)
549        } // if angular limit or powered
[1963]550}
551
[2882]552
[1963]553
554
555
556
557void    btHingeConstraint::updateRHS(btScalar   timeStep)
558{
559        (void)timeStep;
560
561}
562
[2882]563
[1963]564btScalar btHingeConstraint::getHingeAngle()
565{
[7983]566        return getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
567}
568
569btScalar btHingeConstraint::getHingeAngle(const btTransform& transA,const btTransform& transB)
570{
571        const btVector3 refAxis0  = transA.getBasis() * m_rbAFrame.getBasis().getColumn(0);
572        const btVector3 refAxis1  = transA.getBasis() * m_rbAFrame.getBasis().getColumn(1);
573        const btVector3 swingAxis = transB.getBasis() * m_rbBFrame.getBasis().getColumn(1);
574//      btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
575        btScalar angle = btAtan2(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
[2882]576        return m_referenceSign * angle;
[1963]577}
578
[2882]579
[7983]580#if 0
[2882]581void btHingeConstraint::testLimit()
582{
583        // Compute limit information
584        m_hingeAngle = getHingeAngle(); 
585        m_correction = btScalar(0.);
586        m_limitSign = btScalar(0.);
587        m_solveLimit = false;
588        if (m_lowerLimit <= m_upperLimit)
589        {
590                if (m_hingeAngle <= m_lowerLimit)
591                {
592                        m_correction = (m_lowerLimit - m_hingeAngle);
593                        m_limitSign = 1.0f;
594                        m_solveLimit = true;
595                }
596                else if (m_hingeAngle >= m_upperLimit)
597                {
598                        m_correction = m_upperLimit - m_hingeAngle;
599                        m_limitSign = -1.0f;
600                        m_solveLimit = true;
601                }
602        }
603        return;
[7983]604}
605#else
[2882]606
[7983]607
608void btHingeConstraint::testLimit(const btTransform& transA,const btTransform& transB)
609{
610        // Compute limit information
611        m_hingeAngle = getHingeAngle(transA,transB);
612        m_correction = btScalar(0.);
613        m_limitSign = btScalar(0.);
614        m_solveLimit = false;
615        if (m_lowerLimit <= m_upperLimit)
616        {
617                m_hingeAngle = btAdjustAngleToLimits(m_hingeAngle, m_lowerLimit, m_upperLimit);
618                if (m_hingeAngle <= m_lowerLimit)
619                {
620                        m_correction = (m_lowerLimit - m_hingeAngle);
621                        m_limitSign = 1.0f;
622                        m_solveLimit = true;
623                } 
624                else if (m_hingeAngle >= m_upperLimit)
625                {
626                        m_correction = m_upperLimit - m_hingeAngle;
627                        m_limitSign = -1.0f;
628                        m_solveLimit = true;
629                }
630        }
631        return;
632}
633#endif
634
635static btVector3 vHinge(0, 0, btScalar(1));
636
637void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt)
638{
639        // convert target from body to constraint space
640        btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * qAinB * m_rbAFrame.getRotation();
641        qConstraint.normalize();
642
643        // extract "pure" hinge component
644        btVector3 vNoHinge = quatRotate(qConstraint, vHinge); vNoHinge.normalize();
645        btQuaternion qNoHinge = shortestArcQuat(vHinge, vNoHinge);
646        btQuaternion qHinge = qNoHinge.inverse() * qConstraint;
647        qHinge.normalize();
648
649        // compute angular target, clamped to limits
650        btScalar targetAngle = qHinge.getAngle();
651        if (targetAngle > SIMD_PI) // long way around. flip quat and recalculate.
652        {
653                qHinge = operator-(qHinge);
654                targetAngle = qHinge.getAngle();
655        }
656        if (qHinge.getZ() < 0)
657                targetAngle = -targetAngle;
658
659        setMotorTarget(targetAngle, dt);
660}
661
662void btHingeConstraint::setMotorTarget(btScalar targetAngle, btScalar dt)
663{
664        if (m_lowerLimit < m_upperLimit)
665        {
666                if (targetAngle < m_lowerLimit)
667                        targetAngle = m_lowerLimit;
668                else if (targetAngle > m_upperLimit)
669                        targetAngle = m_upperLimit;
670        }
671
672        // compute angular velocity
673        btScalar curAngle  = getHingeAngle(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
674        btScalar dAngle = targetAngle - curAngle;
675        m_motorTargetVelocity = dAngle / dt;
676}
677
678
679
680void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
681{
682        btAssert(!m_useSolveConstraintObsolete);
683        int i, s = info->rowskip;
684        // transforms in world space
685        btTransform trA = transA*m_rbAFrame;
686        btTransform trB = transB*m_rbBFrame;
687        // pivot point
688        btVector3 pivotAInW = trA.getOrigin();
689        btVector3 pivotBInW = trB.getOrigin();
690#if 1
691        // difference between frames in WCS
692        btVector3 ofs = trB.getOrigin() - trA.getOrigin();
693        // now get weight factors depending on masses
694        btScalar miA = getRigidBodyA().getInvMass();
695        btScalar miB = getRigidBodyB().getInvMass();
696        bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
697        btScalar miS = miA + miB;
698        btScalar factA, factB;
699        if(miS > btScalar(0.f))
700        {
701                factA = miB / miS;
702        }
703        else 
704        {
705                factA = btScalar(0.5f);
706        }
707        factB = btScalar(1.0f) - factA;
708        // get the desired direction of hinge axis
709        // as weighted sum of Z-orthos of frameA and frameB in WCS
710        btVector3 ax1A = trA.getBasis().getColumn(2);
711        btVector3 ax1B = trB.getBasis().getColumn(2);
712        btVector3 ax1 = ax1A * factA + ax1B * factB;
713        ax1.normalize();
714        // fill first 3 rows
715        // we want: velA + wA x relA == velB + wB x relB
716        btTransform bodyA_trans = transA;
717        btTransform bodyB_trans = transB;
718        int s0 = 0;
719        int s1 = s;
720        int s2 = s * 2;
721        int nrow = 2; // last filled row
722        btVector3 tmpA, tmpB, relA, relB, p, q;
723        // get vector from bodyB to frameB in WCS
724        relB = trB.getOrigin() - bodyB_trans.getOrigin();
725        // get its projection to hinge axis
726        btVector3 projB = ax1 * relB.dot(ax1);
727        // get vector directed from bodyB to hinge axis (and orthogonal to it)
728        btVector3 orthoB = relB - projB;
729        // same for bodyA
730        relA = trA.getOrigin() - bodyA_trans.getOrigin();
731        btVector3 projA = ax1 * relA.dot(ax1);
732        btVector3 orthoA = relA - projA;
733        btVector3 totalDist = projA - projB;
734        // get offset vectors relA and relB
735        relA = orthoA + totalDist * factA;
736        relB = orthoB - totalDist * factB;
737        // now choose average ortho to hinge axis
738        p = orthoB * factA + orthoA * factB;
739        btScalar len2 = p.length2();
740        if(len2 > SIMD_EPSILON)
741        {
742                p /= btSqrt(len2);
743        }
744        else
745        {
746                p = trA.getBasis().getColumn(1);
747        }
748        // make one more ortho
749        q = ax1.cross(p);
750        // fill three rows
751        tmpA = relA.cross(p);
752        tmpB = relB.cross(p);
753    for (i=0; i<3; i++) info->m_J1angularAxis[s0+i] = tmpA[i];
754    for (i=0; i<3; i++) info->m_J2angularAxis[s0+i] = -tmpB[i];
755        tmpA = relA.cross(q);
756        tmpB = relB.cross(q);
757        if(hasStaticBody && getSolveLimit())
758        { // to make constraint between static and dynamic objects more rigid
759                // remove wA (or wB) from equation if angular limit is hit
760                tmpB *= factB;
761                tmpA *= factA;
762        }
763        for (i=0; i<3; i++) info->m_J1angularAxis[s1+i] = tmpA[i];
764    for (i=0; i<3; i++) info->m_J2angularAxis[s1+i] = -tmpB[i];
765        tmpA = relA.cross(ax1);
766        tmpB = relB.cross(ax1);
767        if(hasStaticBody)
768        { // to make constraint between static and dynamic objects more rigid
769                // remove wA (or wB) from equation
770                tmpB *= factB;
771                tmpA *= factA;
772        }
773        for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i];
774    for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i];
775
776        btScalar k = info->fps * info->erp;
777
778        if (!m_angularOnly)
779        {
780                for (i=0; i<3; i++) info->m_J1linearAxis[s0+i] = p[i];
781                for (i=0; i<3; i++) info->m_J1linearAxis[s1+i] = q[i];
782                for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = ax1[i];
783       
784        // compute three elements of right hand side
785       
786                btScalar rhs = k * p.dot(ofs);
787                info->m_constraintError[s0] = rhs;
788                rhs = k * q.dot(ofs);
789                info->m_constraintError[s1] = rhs;
790                rhs = k * ax1.dot(ofs);
791                info->m_constraintError[s2] = rhs;
792        }
793        // the hinge axis should be the only unconstrained
794        // rotational axis, the angular velocity of the two bodies perpendicular to
795        // the hinge axis should be equal. thus the constraint equations are
796        //    p*w1 - p*w2 = 0
797        //    q*w1 - q*w2 = 0
798        // where p and q are unit vectors normal to the hinge axis, and w1 and w2
799        // are the angular velocity vectors of the two bodies.
800        int s3 = 3 * s;
801        int s4 = 4 * s;
802        info->m_J1angularAxis[s3 + 0] = p[0];
803        info->m_J1angularAxis[s3 + 1] = p[1];
804        info->m_J1angularAxis[s3 + 2] = p[2];
805        info->m_J1angularAxis[s4 + 0] = q[0];
806        info->m_J1angularAxis[s4 + 1] = q[1];
807        info->m_J1angularAxis[s4 + 2] = q[2];
808
809        info->m_J2angularAxis[s3 + 0] = -p[0];
810        info->m_J2angularAxis[s3 + 1] = -p[1];
811        info->m_J2angularAxis[s3 + 2] = -p[2];
812        info->m_J2angularAxis[s4 + 0] = -q[0];
813        info->m_J2angularAxis[s4 + 1] = -q[1];
814        info->m_J2angularAxis[s4 + 2] = -q[2];
815        // compute the right hand side of the constraint equation. set relative
816        // body velocities along p and q to bring the hinge back into alignment.
817        // if ax1A,ax1B are the unit length hinge axes as computed from bodyA and
818        // bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2).
819        // if "theta" is the angle between ax1 and ax2, we need an angular velocity
820        // along u to cover angle erp*theta in one step :
821        //   |angular_velocity| = angle/time = erp*theta / stepsize
822        //                      = (erp*fps) * theta
823        //    angular_velocity  = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
824        //                      = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
825        // ...as ax1 and ax2 are unit length. if theta is smallish,
826        // theta ~= sin(theta), so
827        //    angular_velocity  = (erp*fps) * (ax1 x ax2)
828        // ax1 x ax2 is in the plane space of ax1, so we project the angular
829        // velocity to p and q to find the right hand side.
830        k = info->fps * info->erp;
831        btVector3 u = ax1A.cross(ax1B);
832        info->m_constraintError[s3] = k * u.dot(p);
833        info->m_constraintError[s4] = k * u.dot(q);
834#endif
835        // check angular limits
836        nrow = 4; // last filled row
837        int srow;
838        btScalar limit_err = btScalar(0.0);
839        int limit = 0;
840        if(getSolveLimit())
841        {
842                limit_err = m_correction * m_referenceSign;
843                limit = (limit_err > btScalar(0.0)) ? 1 : 2;
844        }
845        // if the hinge has joint limits or motor, add in the extra row
846        int powered = 0;
847        if(getEnableAngularMotor())
848        {
849                powered = 1;
850        }
851        if(limit || powered) 
852        {
853                nrow++;
854                srow = nrow * info->rowskip;
855                info->m_J1angularAxis[srow+0] = ax1[0];
856                info->m_J1angularAxis[srow+1] = ax1[1];
857                info->m_J1angularAxis[srow+2] = ax1[2];
858
859                info->m_J2angularAxis[srow+0] = -ax1[0];
860                info->m_J2angularAxis[srow+1] = -ax1[1];
861                info->m_J2angularAxis[srow+2] = -ax1[2];
862
863                btScalar lostop = getLowerLimit();
864                btScalar histop = getUpperLimit();
865                if(limit && (lostop == histop))
866                {  // the joint motor is ineffective
867                        powered = 0;
868                }
869                info->m_constraintError[srow] = btScalar(0.0f);
870                btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp;
871                if(powered)
872                {
873                        if(m_flags & BT_HINGE_FLAGS_CFM_NORM)
874                        {
875                                info->cfm[srow] = m_normalCFM;
876                        }
877                        btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP);
878                        info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
879                        info->m_lowerLimit[srow] = - m_maxMotorImpulse;
880                        info->m_upperLimit[srow] =   m_maxMotorImpulse;
881                }
882                if(limit)
883                {
884                        k = info->fps * currERP;
885                        info->m_constraintError[srow] += k * limit_err;
886                        if(m_flags & BT_HINGE_FLAGS_CFM_STOP)
887                        {
888                                info->cfm[srow] = m_stopCFM;
889                        }
890                        if(lostop == histop) 
891                        {
892                                // limited low and high simultaneously
893                                info->m_lowerLimit[srow] = -SIMD_INFINITY;
894                                info->m_upperLimit[srow] = SIMD_INFINITY;
895                        }
896                        else if(limit == 1) 
897                        { // low limit
898                                info->m_lowerLimit[srow] = 0;
899                                info->m_upperLimit[srow] = SIMD_INFINITY;
900                        }
901                        else 
902                        { // high limit
903                                info->m_lowerLimit[srow] = -SIMD_INFINITY;
904                                info->m_upperLimit[srow] = 0;
905                        }
906                        // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
907                        btScalar bounce = m_relaxationFactor;
908                        if(bounce > btScalar(0.0))
909                        {
910                                btScalar vel = angVelA.dot(ax1);
911                                vel -= angVelB.dot(ax1);
912                                // only apply bounce if the velocity is incoming, and if the
913                                // resulting c[] exceeds what we already have.
914                                if(limit == 1)
915                                {       // low limit
916                                        if(vel < 0)
917                                        {
918                                                btScalar newc = -bounce * vel;
919                                                if(newc > info->m_constraintError[srow])
920                                                {
921                                                        info->m_constraintError[srow] = newc;
922                                                }
923                                        }
924                                }
925                                else
926                                {       // high limit - all those computations are reversed
927                                        if(vel > 0)
928                                        {
929                                                btScalar newc = -bounce * vel;
930                                                if(newc < info->m_constraintError[srow])
931                                                {
932                                                        info->m_constraintError[srow] = newc;
933                                                }
934                                        }
935                                }
936                        }
937                        info->m_constraintError[srow] *= m_biasFactor;
938                } // if(limit)
939        } // if angular limit or powered
940}
941
942
943///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
944///If no axis is provided, it uses the default axis for this constraint.
945void btHingeConstraint::setParam(int num, btScalar value, int axis)
946{
947        if((axis == -1) || (axis == 5))
948        {
949                switch(num)
950                {       
951                        case BT_CONSTRAINT_STOP_ERP :
952                                m_stopERP = value;
953                                m_flags |= BT_HINGE_FLAGS_ERP_STOP;
954                                break;
955                        case BT_CONSTRAINT_STOP_CFM :
956                                m_stopCFM = value;
957                                m_flags |= BT_HINGE_FLAGS_CFM_STOP;
958                                break;
959                        case BT_CONSTRAINT_CFM :
960                                m_normalCFM = value;
961                                m_flags |= BT_HINGE_FLAGS_CFM_NORM;
962                                break;
963                        default : 
964                                btAssertConstrParams(0);
965                }
966        }
967        else
968        {
969                btAssertConstrParams(0);
970        }
971}
972
973///return the local value of parameter
974btScalar btHingeConstraint::getParam(int num, int axis) const 
975{
976        btScalar retVal = 0;
977        if((axis == -1) || (axis == 5))
978        {
979                switch(num)
980                {       
981                        case BT_CONSTRAINT_STOP_ERP :
982                                btAssertConstrParams(m_flags & BT_HINGE_FLAGS_ERP_STOP);
983                                retVal = m_stopERP;
984                                break;
985                        case BT_CONSTRAINT_STOP_CFM :
986                                btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_STOP);
987                                retVal = m_stopCFM;
988                                break;
989                        case BT_CONSTRAINT_CFM :
990                                btAssertConstrParams(m_flags & BT_HINGE_FLAGS_CFM_NORM);
991                                retVal = m_normalCFM;
992                                break;
993                        default : 
994                                btAssertConstrParams(0);
995                }
996        }
997        else
998        {
999                btAssertConstrParams(0);
1000        }
1001        return retVal;
1002}
1003
1004
Note: See TracBrowser for help on using the repository browser.