Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/trunk/src/external/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp @ 9967

Last change on this file since 9967 was 8351, checked in by rgrieder, 14 years ago

Merged kicklib2 branch back to trunk (includes former branches ois_update, mac_osx and kicklib).

Notes for updating

Linux:
You don't need an extra package for CEGUILua and Tolua, it's already shipped with CEGUI.
However you do need to make sure that the OgreRenderer is installed too with CEGUI 0.7 (may be a separate package).
Also, Orxonox now recognises if you install the CgProgramManager (a separate package available on newer Ubuntu on Debian systems).

Windows:
Download the new dependency packages versioned 6.0 and use these. If you have problems with that or if you don't like the in game console problem mentioned below, you can download the new 4.3 version of the packages (only available for Visual Studio 2005/2008).

Key new features:

  • *Support for Mac OS X*
  • Visual Studio 2010 support
  • Bullet library update to 2.77
  • OIS library update to 1.3
  • Support for CEGUI 0.7 —> Support for Arch Linux and even SuSE
  • Improved install target
  • Compiles now with GCC 4.6
  • Ogre Cg Shader plugin activated for Linux if available
  • And of course lots of bug fixes

There are also some regressions:

  • No support for CEGUI 0.5, Ogre 1.4 and boost 1.35 - 1.39 any more
  • In game console is not working in main menu for CEGUI 0.7
  • Tolua (just the C lib, not the application) and CEGUILua libraries are no longer in our repository. —> You will need to get these as well when compiling Orxonox
  • And of course lots of new bugs we don't yet know about
  • Property svn:eol-style set to native
File size: 24.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/*
17Added by Roman Ponomarev (rponom@gmail.com)
18April 04, 2008
19*/
20
21
22
23#include "btSliderConstraint.h"
24#include "BulletDynamics/Dynamics/btRigidBody.h"
25#include "LinearMath/btTransformUtil.h"
26#include <new>
27
28#define USE_OFFSET_FOR_CONSTANT_FRAME true
29
30void btSliderConstraint::initParams()
31{
32    m_lowerLinLimit = btScalar(1.0);
33    m_upperLinLimit = btScalar(-1.0);
34    m_lowerAngLimit = btScalar(0.);
35    m_upperAngLimit = btScalar(0.);
36        m_softnessDirLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
37        m_restitutionDirLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
38        m_dampingDirLin = btScalar(0.);
39        m_cfmDirLin = SLIDER_CONSTRAINT_DEF_CFM;
40        m_softnessDirAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
41        m_restitutionDirAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
42        m_dampingDirAng = btScalar(0.);
43        m_cfmDirAng = SLIDER_CONSTRAINT_DEF_CFM;
44        m_softnessOrthoLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
45        m_restitutionOrthoLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
46        m_dampingOrthoLin = SLIDER_CONSTRAINT_DEF_DAMPING;
47        m_cfmOrthoLin = SLIDER_CONSTRAINT_DEF_CFM;
48        m_softnessOrthoAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
49        m_restitutionOrthoAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
50        m_dampingOrthoAng = SLIDER_CONSTRAINT_DEF_DAMPING;
51        m_cfmOrthoAng = SLIDER_CONSTRAINT_DEF_CFM;
52        m_softnessLimLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
53        m_restitutionLimLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
54        m_dampingLimLin = SLIDER_CONSTRAINT_DEF_DAMPING;
55        m_cfmLimLin = SLIDER_CONSTRAINT_DEF_CFM;
56        m_softnessLimAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
57        m_restitutionLimAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
58        m_dampingLimAng = SLIDER_CONSTRAINT_DEF_DAMPING;
59        m_cfmLimAng = SLIDER_CONSTRAINT_DEF_CFM;
60
61        m_poweredLinMotor = false;
62    m_targetLinMotorVelocity = btScalar(0.);
63    m_maxLinMotorForce = btScalar(0.);
64        m_accumulatedLinMotorImpulse = btScalar(0.0);
65
66        m_poweredAngMotor = false;
67    m_targetAngMotorVelocity = btScalar(0.);
68    m_maxAngMotorForce = btScalar(0.);
69        m_accumulatedAngMotorImpulse = btScalar(0.0);
70
71        m_flags = 0;
72        m_flags = 0;
73
74        m_useOffsetForConstraintFrame = USE_OFFSET_FOR_CONSTANT_FRAME;
75
76        calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
77}
78
79
80
81
82
83btSliderConstraint::btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
84        : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, rbA, rbB),
85                m_useSolveConstraintObsolete(false),
86                m_frameInA(frameInA),
87        m_frameInB(frameInB),
88                m_useLinearReferenceFrameA(useLinearReferenceFrameA)
89{
90        initParams();
91}
92
93
94
95btSliderConstraint::btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameA)
96        : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, getFixedBody(), rbB),
97                m_useSolveConstraintObsolete(false),
98                m_frameInB(frameInB),
99                m_useLinearReferenceFrameA(useLinearReferenceFrameA)
100{
101        ///not providing rigidbody A means implicitly using worldspace for body A
102        m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB;
103//      m_frameInA.getOrigin() = m_rbA.getCenterOfMassTransform()(m_frameInA.getOrigin());
104
105        initParams();
106}
107
108
109
110
111
112
113void btSliderConstraint::getInfo1(btConstraintInfo1* info)
114{
115        if (m_useSolveConstraintObsolete)
116        {
117                info->m_numConstraintRows = 0;
118                info->nub = 0;
119        }
120        else
121        {
122                info->m_numConstraintRows = 4; // Fixed 2 linear + 2 angular
123                info->nub = 2; 
124                //prepare constraint
125                calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
126                testAngLimits();
127                testLinLimits();
128                if(getSolveLinLimit() || getPoweredLinMotor())
129                {
130                        info->m_numConstraintRows++; // limit 3rd linear as well
131                        info->nub--; 
132                }
133                if(getSolveAngLimit() || getPoweredAngMotor())
134                {
135                        info->m_numConstraintRows++; // limit 3rd angular as well
136                        info->nub--; 
137                }
138        }
139}
140
141void btSliderConstraint::getInfo1NonVirtual(btConstraintInfo1* info)
142{
143
144        info->m_numConstraintRows = 6; // Fixed 2 linear + 2 angular + 1 limit (even if not used)
145        info->nub = 0; 
146}
147
148void btSliderConstraint::getInfo2(btConstraintInfo2* info)
149{
150        getInfo2NonVirtual(info,m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform(), m_rbA.getLinearVelocity(),m_rbB.getLinearVelocity(), m_rbA.getInvMass(),m_rbB.getInvMass());
151}
152
153
154
155
156
157
158
159void btSliderConstraint::calculateTransforms(const btTransform& transA,const btTransform& transB)
160{
161        if(m_useLinearReferenceFrameA || (!m_useSolveConstraintObsolete))
162        {
163                m_calculatedTransformA = transA * m_frameInA;
164                m_calculatedTransformB = transB * m_frameInB;
165        }
166        else
167        {
168                m_calculatedTransformA = transB * m_frameInB;
169                m_calculatedTransformB = transA * m_frameInA;
170        }
171        m_realPivotAInW = m_calculatedTransformA.getOrigin();
172        m_realPivotBInW = m_calculatedTransformB.getOrigin();
173        m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X
174        if(m_useLinearReferenceFrameA || m_useSolveConstraintObsolete)
175        {
176                m_delta = m_realPivotBInW - m_realPivotAInW;
177        }
178        else
179        {
180                m_delta = m_realPivotAInW - m_realPivotBInW;
181        }
182        m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis;
183    btVector3 normalWorld;
184    int i;
185    //linear part
186    for(i = 0; i < 3; i++)
187    {
188                normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
189                m_depth[i] = m_delta.dot(normalWorld);
190    }
191}
192 
193
194
195void btSliderConstraint::testLinLimits(void)
196{
197        m_solveLinLim = false;
198        m_linPos = m_depth[0];
199        if(m_lowerLinLimit <= m_upperLinLimit)
200        {
201                if(m_depth[0] > m_upperLinLimit)
202                {
203                        m_depth[0] -= m_upperLinLimit;
204                        m_solveLinLim = true;
205                }
206                else if(m_depth[0] < m_lowerLinLimit)
207                {
208                        m_depth[0] -= m_lowerLinLimit;
209                        m_solveLinLim = true;
210                }
211                else
212                {
213                        m_depth[0] = btScalar(0.);
214                }
215        }
216        else
217        {
218                m_depth[0] = btScalar(0.);
219        }
220}
221
222
223
224void btSliderConstraint::testAngLimits(void)
225{
226        m_angDepth = btScalar(0.);
227        m_solveAngLim = false;
228        if(m_lowerAngLimit <= m_upperAngLimit)
229        {
230                const btVector3 axisA0 = m_calculatedTransformA.getBasis().getColumn(1);
231                const btVector3 axisA1 = m_calculatedTransformA.getBasis().getColumn(2);
232                const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1);
233//              btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0)); 
234                btScalar rot = btAtan2(axisB0.dot(axisA1), axisB0.dot(axisA0)); 
235                rot = btAdjustAngleToLimits(rot, m_lowerAngLimit, m_upperAngLimit);
236                m_angPos = rot;
237                if(rot < m_lowerAngLimit)
238                {
239                        m_angDepth = rot - m_lowerAngLimit;
240                        m_solveAngLim = true;
241                } 
242                else if(rot > m_upperAngLimit)
243                {
244                        m_angDepth = rot - m_upperAngLimit;
245                        m_solveAngLim = true;
246                }
247        }
248}
249
250btVector3 btSliderConstraint::getAncorInA(void)
251{
252        btVector3 ancorInA;
253        ancorInA = m_realPivotAInW + (m_lowerLinLimit + m_upperLinLimit) * btScalar(0.5) * m_sliderAxis;
254        ancorInA = m_rbA.getCenterOfMassTransform().inverse() * ancorInA;
255        return ancorInA;
256}
257
258
259
260btVector3 btSliderConstraint::getAncorInB(void)
261{
262        btVector3 ancorInB;
263        ancorInB = m_frameInB.getOrigin();
264        return ancorInB;
265}
266
267
268void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB, const btVector3& linVelA,const btVector3& linVelB, btScalar rbAinvMass,btScalar rbBinvMass  )
269{
270        const btTransform& trA = getCalculatedTransformA();
271        const btTransform& trB = getCalculatedTransformB();
272       
273        btAssert(!m_useSolveConstraintObsolete);
274        int i, s = info->rowskip;
275       
276        btScalar signFact = m_useLinearReferenceFrameA ? btScalar(1.0f) : btScalar(-1.0f);
277       
278        // difference between frames in WCS
279        btVector3 ofs = trB.getOrigin() - trA.getOrigin();
280        // now get weight factors depending on masses
281        btScalar miA = rbAinvMass;
282        btScalar miB = rbBinvMass;
283        bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
284        btScalar miS = miA + miB;
285        btScalar factA, factB;
286        if(miS > btScalar(0.f))
287        {
288                factA = miB / miS;
289        }
290        else 
291        {
292                factA = btScalar(0.5f);
293        }
294        factB = btScalar(1.0f) - factA;
295        btVector3 ax1, p, q;
296        btVector3 ax1A = trA.getBasis().getColumn(0);
297        btVector3 ax1B = trB.getBasis().getColumn(0);
298        if(m_useOffsetForConstraintFrame)
299        {
300                // get the desired direction of slider axis
301                // as weighted sum of X-orthos of frameA and frameB in WCS
302                ax1 = ax1A * factA + ax1B * factB;
303                ax1.normalize();
304                // construct two orthos to slider axis
305                btPlaneSpace1 (ax1, p, q);
306        }
307        else
308        { // old way - use frameA
309                ax1 = trA.getBasis().getColumn(0);
310                // get 2 orthos to slider axis (Y, Z)
311                p = trA.getBasis().getColumn(1);
312                q = trA.getBasis().getColumn(2);
313        }
314        // make rotations around these orthos equal
315        // the slider axis should be the only unconstrained
316        // rotational axis, the angular velocity of the two bodies perpendicular to
317        // the slider axis should be equal. thus the constraint equations are
318        //    p*w1 - p*w2 = 0
319        //    q*w1 - q*w2 = 0
320        // where p and q are unit vectors normal to the slider axis, and w1 and w2
321        // are the angular velocity vectors of the two bodies.
322        info->m_J1angularAxis[0] = p[0];
323        info->m_J1angularAxis[1] = p[1];
324        info->m_J1angularAxis[2] = p[2];
325        info->m_J1angularAxis[s+0] = q[0];
326        info->m_J1angularAxis[s+1] = q[1];
327        info->m_J1angularAxis[s+2] = q[2];
328
329        info->m_J2angularAxis[0] = -p[0];
330        info->m_J2angularAxis[1] = -p[1];
331        info->m_J2angularAxis[2] = -p[2];
332        info->m_J2angularAxis[s+0] = -q[0];
333        info->m_J2angularAxis[s+1] = -q[1];
334        info->m_J2angularAxis[s+2] = -q[2];
335        // compute the right hand side of the constraint equation. set relative
336        // body velocities along p and q to bring the slider back into alignment.
337        // if ax1A,ax1B are the unit length slider axes as computed from bodyA and
338        // bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2).
339        // if "theta" is the angle between ax1 and ax2, we need an angular velocity
340        // along u to cover angle erp*theta in one step :
341        //   |angular_velocity| = angle/time = erp*theta / stepsize
342        //                      = (erp*fps) * theta
343        //    angular_velocity  = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
344        //                      = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
345        // ...as ax1 and ax2 are unit length. if theta is smallish,
346        // theta ~= sin(theta), so
347        //    angular_velocity  = (erp*fps) * (ax1 x ax2)
348        // ax1 x ax2 is in the plane space of ax1, so we project the angular
349        // velocity to p and q to find the right hand side.
350//      btScalar k = info->fps * info->erp * getSoftnessOrthoAng();
351        btScalar currERP = (m_flags & BT_SLIDER_FLAGS_ERP_ORTANG) ? m_softnessOrthoAng : m_softnessOrthoAng * info->erp;
352        btScalar k = info->fps * currERP;
353
354        btVector3 u = ax1A.cross(ax1B);
355        info->m_constraintError[0] = k * u.dot(p);
356        info->m_constraintError[s] = k * u.dot(q);
357        if(m_flags & BT_SLIDER_FLAGS_CFM_ORTANG)
358        {
359                info->cfm[0] = m_cfmOrthoAng;
360                info->cfm[s] = m_cfmOrthoAng;
361        }
362
363        int nrow = 1; // last filled row
364        int srow;
365        btScalar limit_err;
366        int limit;
367        int powered;
368
369        // next two rows.
370        // we want: velA + wA x relA == velB + wB x relB ... but this would
371        // result in three equations, so we project along two orthos to the slider axis
372
373        btTransform bodyA_trans = transA;
374        btTransform bodyB_trans = transB;
375        nrow++;
376        int s2 = nrow * s;
377        nrow++;
378        int s3 = nrow * s;
379        btVector3 tmpA(0,0,0), tmpB(0,0,0), relA(0,0,0), relB(0,0,0), c(0,0,0);
380        if(m_useOffsetForConstraintFrame)
381        {
382                // get vector from bodyB to frameB in WCS
383                relB = trB.getOrigin() - bodyB_trans.getOrigin();
384                // get its projection to slider axis
385                btVector3 projB = ax1 * relB.dot(ax1);
386                // get vector directed from bodyB to slider axis (and orthogonal to it)
387                btVector3 orthoB = relB - projB;
388                // same for bodyA
389                relA = trA.getOrigin() - bodyA_trans.getOrigin();
390                btVector3 projA = ax1 * relA.dot(ax1);
391                btVector3 orthoA = relA - projA;
392                // get desired offset between frames A and B along slider axis
393                btScalar sliderOffs = m_linPos - m_depth[0];
394                // desired vector from projection of center of bodyA to projection of center of bodyB to slider axis
395                btVector3 totalDist = projA + ax1 * sliderOffs - projB;
396                // get offset vectors relA and relB
397                relA = orthoA + totalDist * factA;
398                relB = orthoB - totalDist * factB;
399                // now choose average ortho to slider axis
400                p = orthoB * factA + orthoA * factB;
401                btScalar len2 = p.length2();
402                if(len2 > SIMD_EPSILON)
403                {
404                        p /= btSqrt(len2);
405                }
406                else
407                {
408                        p = trA.getBasis().getColumn(1);
409                }
410                // make one more ortho
411                q = ax1.cross(p);
412                // fill two rows
413                tmpA = relA.cross(p);
414                tmpB = relB.cross(p);
415                for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i];
416                for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i];
417                tmpA = relA.cross(q);
418                tmpB = relB.cross(q);
419                if(hasStaticBody && getSolveAngLimit())
420                { // to make constraint between static and dynamic objects more rigid
421                        // remove wA (or wB) from equation if angular limit is hit
422                        tmpB *= factB;
423                        tmpA *= factA;
424                }
425                for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = tmpA[i];
426                for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = -tmpB[i];
427                for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i];
428                for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i];
429        }
430        else
431        {       // old way - maybe incorrect if bodies are not on the slider axis
432                // see discussion "Bug in slider constraint" http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=9&t=4024&start=0
433                c = bodyB_trans.getOrigin() - bodyA_trans.getOrigin();
434                btVector3 tmp = c.cross(p);
435                for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = factA*tmp[i];
436                for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = factB*tmp[i];
437                tmp = c.cross(q);
438                for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = factA*tmp[i];
439                for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = factB*tmp[i];
440
441                for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i];
442                for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i];
443        }
444        // compute two elements of right hand side
445
446        //      k = info->fps * info->erp * getSoftnessOrthoLin();
447        currERP = (m_flags & BT_SLIDER_FLAGS_ERP_ORTLIN) ? m_softnessOrthoLin : m_softnessOrthoLin * info->erp;
448        k = info->fps * currERP;
449
450        btScalar rhs = k * p.dot(ofs);
451        info->m_constraintError[s2] = rhs;
452        rhs = k * q.dot(ofs);
453        info->m_constraintError[s3] = rhs;
454        if(m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN)
455        {
456                info->cfm[s2] = m_cfmOrthoLin;
457                info->cfm[s3] = m_cfmOrthoLin;
458        }
459
460
461        // check linear limits
462        limit_err = btScalar(0.0);
463        limit = 0;
464        if(getSolveLinLimit())
465        {
466                limit_err = getLinDepth() *  signFact;
467                limit = (limit_err > btScalar(0.0)) ? 2 : 1;
468        }
469        powered = 0;
470        if(getPoweredLinMotor())
471        {
472                powered = 1;
473        }
474        // if the slider has joint limits or motor, add in the extra row
475        if (limit || powered) 
476        {
477                nrow++;
478                srow = nrow * info->rowskip;
479                info->m_J1linearAxis[srow+0] = ax1[0];
480                info->m_J1linearAxis[srow+1] = ax1[1];
481                info->m_J1linearAxis[srow+2] = ax1[2];
482                // linear torque decoupling step:
483                //
484                // we have to be careful that the linear constraint forces (+/- ax1) applied to the two bodies
485                // do not create a torque couple. in other words, the points that the
486                // constraint force is applied at must lie along the same ax1 axis.
487                // a torque couple will result in limited slider-jointed free
488                // bodies from gaining angular momentum.
489                if(m_useOffsetForConstraintFrame)
490                {
491                        // this is needed only when bodyA and bodyB are both dynamic.
492                        if(!hasStaticBody)
493                        {
494                                tmpA = relA.cross(ax1);
495                                tmpB = relB.cross(ax1);
496                                info->m_J1angularAxis[srow+0] = tmpA[0];
497                                info->m_J1angularAxis[srow+1] = tmpA[1];
498                                info->m_J1angularAxis[srow+2] = tmpA[2];
499                                info->m_J2angularAxis[srow+0] = -tmpB[0];
500                                info->m_J2angularAxis[srow+1] = -tmpB[1];
501                                info->m_J2angularAxis[srow+2] = -tmpB[2];
502                        }
503                }
504                else
505                { // The old way. May be incorrect if bodies are not on the slider axis
506                        btVector3 ltd;  // Linear Torque Decoupling vector (a torque)
507                        ltd = c.cross(ax1);
508                        info->m_J1angularAxis[srow+0] = factA*ltd[0];
509                        info->m_J1angularAxis[srow+1] = factA*ltd[1];
510                        info->m_J1angularAxis[srow+2] = factA*ltd[2];
511                        info->m_J2angularAxis[srow+0] = factB*ltd[0];
512                        info->m_J2angularAxis[srow+1] = factB*ltd[1];
513                        info->m_J2angularAxis[srow+2] = factB*ltd[2];
514                }
515                // right-hand part
516                btScalar lostop = getLowerLinLimit();
517                btScalar histop = getUpperLinLimit();
518                if(limit && (lostop == histop))
519                {  // the joint motor is ineffective
520                        powered = 0;
521                }
522                info->m_constraintError[srow] = 0.;
523                info->m_lowerLimit[srow] = 0.;
524                info->m_upperLimit[srow] = 0.;
525                currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMLIN) ? m_softnessLimLin : info->erp;
526                if(powered)
527                {
528                        if(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN)
529                        {
530                                info->cfm[srow] = m_cfmDirLin;
531                        }
532                        btScalar tag_vel = getTargetLinMotorVelocity();
533                        btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * currERP);
534                        info->m_constraintError[srow] -= signFact * mot_fact * getTargetLinMotorVelocity();
535                        info->m_lowerLimit[srow] += -getMaxLinMotorForce() * info->fps;
536                        info->m_upperLimit[srow] += getMaxLinMotorForce() * info->fps;
537                }
538                if(limit)
539                {
540                        k = info->fps * currERP;
541                        info->m_constraintError[srow] += k * limit_err;
542                        if(m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN)
543                        {
544                                info->cfm[srow] = m_cfmLimLin;
545                        }
546                        if(lostop == histop) 
547                        {       // limited low and high simultaneously
548                                info->m_lowerLimit[srow] = -SIMD_INFINITY;
549                                info->m_upperLimit[srow] = SIMD_INFINITY;
550                        }
551                        else if(limit == 1) 
552                        { // low limit
553                                info->m_lowerLimit[srow] = -SIMD_INFINITY;
554                                info->m_upperLimit[srow] = 0;
555                        }
556                        else 
557                        { // high limit
558                                info->m_lowerLimit[srow] = 0;
559                                info->m_upperLimit[srow] = SIMD_INFINITY;
560                        }
561                        // bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin) for that)
562                        btScalar bounce = btFabs(btScalar(1.0) - getDampingLimLin());
563                        if(bounce > btScalar(0.0))
564                        {
565                                btScalar vel = linVelA.dot(ax1);
566                                vel -= linVelB.dot(ax1);
567                                vel *= signFact;
568                                // only apply bounce if the velocity is incoming, and if the
569                                // resulting c[] exceeds what we already have.
570                                if(limit == 1)
571                                {       // low limit
572                                        if(vel < 0)
573                                        {
574                                                btScalar newc = -bounce * vel;
575                                                if (newc > info->m_constraintError[srow])
576                                                {
577                                                        info->m_constraintError[srow] = newc;
578                                                }
579                                        }
580                                }
581                                else
582                                { // high limit - all those computations are reversed
583                                        if(vel > 0)
584                                        {
585                                                btScalar newc = -bounce * vel;
586                                                if(newc < info->m_constraintError[srow]) 
587                                                {
588                                                        info->m_constraintError[srow] = newc;
589                                                }
590                                        }
591                                }
592                        }
593                        info->m_constraintError[srow] *= getSoftnessLimLin();
594                } // if(limit)
595        } // if linear limit
596        // check angular limits
597        limit_err = btScalar(0.0);
598        limit = 0;
599        if(getSolveAngLimit())
600        {
601                limit_err = getAngDepth();
602                limit = (limit_err > btScalar(0.0)) ? 1 : 2;
603        }
604        // if the slider has joint limits, add in the extra row
605        powered = 0;
606        if(getPoweredAngMotor())
607        {
608                powered = 1;
609        }
610        if(limit || powered) 
611        {
612                nrow++;
613                srow = nrow * info->rowskip;
614                info->m_J1angularAxis[srow+0] = ax1[0];
615                info->m_J1angularAxis[srow+1] = ax1[1];
616                info->m_J1angularAxis[srow+2] = ax1[2];
617
618                info->m_J2angularAxis[srow+0] = -ax1[0];
619                info->m_J2angularAxis[srow+1] = -ax1[1];
620                info->m_J2angularAxis[srow+2] = -ax1[2];
621
622                btScalar lostop = getLowerAngLimit();
623                btScalar histop = getUpperAngLimit();
624                if(limit && (lostop == histop))
625                {  // the joint motor is ineffective
626                        powered = 0;
627                }
628                currERP = (m_flags & BT_SLIDER_FLAGS_ERP_LIMANG) ? m_softnessLimAng : info->erp;
629                if(powered)
630                {
631                        if(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG)
632                        {
633                                info->cfm[srow] = m_cfmDirAng;
634                        }
635                        btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * currERP);
636                        info->m_constraintError[srow] = mot_fact * getTargetAngMotorVelocity();
637                        info->m_lowerLimit[srow] = -getMaxAngMotorForce() * info->fps;
638                        info->m_upperLimit[srow] = getMaxAngMotorForce() * info->fps;
639                }
640                if(limit)
641                {
642                        k = info->fps * currERP;
643                        info->m_constraintError[srow] += k * limit_err;
644                        if(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG)
645                        {
646                                info->cfm[srow] = m_cfmLimAng;
647                        }
648                        if(lostop == histop) 
649                        {
650                                // limited low and high simultaneously
651                                info->m_lowerLimit[srow] = -SIMD_INFINITY;
652                                info->m_upperLimit[srow] = SIMD_INFINITY;
653                        }
654                        else if(limit == 1) 
655                        { // low limit
656                                info->m_lowerLimit[srow] = 0;
657                                info->m_upperLimit[srow] = SIMD_INFINITY;
658                        }
659                        else 
660                        { // high limit
661                                info->m_lowerLimit[srow] = -SIMD_INFINITY;
662                                info->m_upperLimit[srow] = 0;
663                        }
664                        // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
665                        btScalar bounce = btFabs(btScalar(1.0) - getDampingLimAng());
666                        if(bounce > btScalar(0.0))
667                        {
668                                btScalar vel = m_rbA.getAngularVelocity().dot(ax1);
669                                vel -= m_rbB.getAngularVelocity().dot(ax1);
670                                // only apply bounce if the velocity is incoming, and if the
671                                // resulting c[] exceeds what we already have.
672                                if(limit == 1)
673                                {       // low limit
674                                        if(vel < 0)
675                                        {
676                                                btScalar newc = -bounce * vel;
677                                                if(newc > info->m_constraintError[srow])
678                                                {
679                                                        info->m_constraintError[srow] = newc;
680                                                }
681                                        }
682                                }
683                                else
684                                {       // high limit - all those computations are reversed
685                                        if(vel > 0)
686                                        {
687                                                btScalar newc = -bounce * vel;
688                                                if(newc < info->m_constraintError[srow])
689                                                {
690                                                        info->m_constraintError[srow] = newc;
691                                                }
692                                        }
693                                }
694                        }
695                        info->m_constraintError[srow] *= getSoftnessLimAng();
696                } // if(limit)
697        } // if angular limit or powered
698}
699
700
701///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).
702///If no axis is provided, it uses the default axis for this constraint.
703void btSliderConstraint::setParam(int num, btScalar value, int axis)
704{
705        switch(num)
706        {
707        case BT_CONSTRAINT_STOP_ERP :
708                if(axis < 1)
709                {
710                        m_softnessLimLin = value;
711                        m_flags |= BT_SLIDER_FLAGS_ERP_LIMLIN;
712                }
713                else if(axis < 3)
714                {
715                        m_softnessOrthoLin = value;
716                        m_flags |= BT_SLIDER_FLAGS_ERP_ORTLIN;
717                }
718                else if(axis == 3)
719                {
720                        m_softnessLimAng = value;
721                        m_flags |= BT_SLIDER_FLAGS_ERP_LIMANG;
722                }
723                else if(axis < 6)
724                {
725                        m_softnessOrthoAng = value;
726                        m_flags |= BT_SLIDER_FLAGS_ERP_ORTANG;
727                }
728                else
729                {
730                        btAssertConstrParams(0);
731                }
732                break;
733        case BT_CONSTRAINT_CFM :
734                if(axis < 1)
735                {
736                        m_cfmDirLin = value;
737                        m_flags |= BT_SLIDER_FLAGS_CFM_DIRLIN;
738                }
739                else if(axis == 3)
740                {
741                        m_cfmDirAng = value;
742                        m_flags |= BT_SLIDER_FLAGS_CFM_DIRANG;
743                }
744                else
745                {
746                        btAssertConstrParams(0);
747                }
748                break;
749        case BT_CONSTRAINT_STOP_CFM :
750                if(axis < 1)
751                {
752                        m_cfmLimLin = value;
753                        m_flags |= BT_SLIDER_FLAGS_CFM_LIMLIN;
754                }
755                else if(axis < 3)
756                {
757                        m_cfmOrthoLin = value;
758                        m_flags |= BT_SLIDER_FLAGS_CFM_ORTLIN;
759                }
760                else if(axis == 3)
761                {
762                        m_cfmLimAng = value;
763                        m_flags |= BT_SLIDER_FLAGS_CFM_LIMANG;
764                }
765                else if(axis < 6)
766                {
767                        m_cfmOrthoAng = value;
768                        m_flags |= BT_SLIDER_FLAGS_CFM_ORTANG;
769                }
770                else
771                {
772                        btAssertConstrParams(0);
773                }
774                break;
775        }
776}
777
778///return the local value of parameter
779btScalar btSliderConstraint::getParam(int num, int axis) const 
780{
781        btScalar retVal(SIMD_INFINITY);
782        switch(num)
783        {
784        case BT_CONSTRAINT_STOP_ERP :
785                if(axis < 1)
786                {
787                        btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_LIMLIN);
788                        retVal = m_softnessLimLin;
789                }
790                else if(axis < 3)
791                {
792                        btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_ORTLIN);
793                        retVal = m_softnessOrthoLin;
794                }
795                else if(axis == 3)
796                {
797                        btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_LIMANG);
798                        retVal = m_softnessLimAng;
799                }
800                else if(axis < 6)
801                {
802                        btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_ERP_ORTANG);
803                        retVal = m_softnessOrthoAng;
804                }
805                else
806                {
807                        btAssertConstrParams(0);
808                }
809                break;
810        case BT_CONSTRAINT_CFM :
811                if(axis < 1)
812                {
813                        btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_DIRLIN);
814                        retVal = m_cfmDirLin;
815                }
816                else if(axis == 3)
817                {
818                        btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_DIRANG);
819                        retVal = m_cfmDirAng;
820                }
821                else
822                {
823                        btAssertConstrParams(0);
824                }
825                break;
826        case BT_CONSTRAINT_STOP_CFM :
827                if(axis < 1)
828                {
829                        btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_LIMLIN);
830                        retVal = m_cfmLimLin;
831                }
832                else if(axis < 3)
833                {
834                        btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_ORTLIN);
835                        retVal = m_cfmOrthoLin;
836                }
837                else if(axis == 3)
838                {
839                        btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_LIMANG);
840                        retVal = m_cfmLimAng;
841                }
842                else if(axis < 6)
843                {
844                        btAssertConstrParams(m_flags & BT_SLIDER_FLAGS_CFM_ORTANG);
845                        retVal = m_cfmOrthoAng;
846                }
847                else
848                {
849                        btAssertConstrParams(0);
850                }
851                break;
852        }
853        return retVal;
854}
855
856
857
Note: See TracBrowser for help on using the repository browser.