Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/SuperOrxoBros_HS18/src/external/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp @ 12177

Last change on this file since 12177 was 12177, checked in by siramesh, 5 years ago

Super Orxo Bros Final (Sidharth Ramesh, Nisa Balta, Jeff Ren)

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