Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/netp6/src/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp @ 5739

Last change on this file since 5739 was 2882, checked in by rgrieder, 16 years ago

Update from Bullet 2.73 to 2.74.
  • Property svn:eol-style set to native
File size: 27.8 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//-----------------------------------------------------------------------------
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_softnessDirAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
40        m_restitutionDirAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
41        m_dampingDirAng = btScalar(0.);
42        m_softnessOrthoLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
43        m_restitutionOrthoLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
44        m_dampingOrthoLin = SLIDER_CONSTRAINT_DEF_DAMPING;
45        m_softnessOrthoAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
46        m_restitutionOrthoAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
47        m_dampingOrthoAng = SLIDER_CONSTRAINT_DEF_DAMPING;
48        m_softnessLimLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
49        m_restitutionLimLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
50        m_dampingLimLin = SLIDER_CONSTRAINT_DEF_DAMPING;
51        m_softnessLimAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
52        m_restitutionLimAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
53        m_dampingLimAng = SLIDER_CONSTRAINT_DEF_DAMPING;
54
55        m_poweredLinMotor = false;
56    m_targetLinMotorVelocity = btScalar(0.);
57    m_maxLinMotorForce = btScalar(0.);
58        m_accumulatedLinMotorImpulse = btScalar(0.0);
59
60        m_poweredAngMotor = false;
61    m_targetAngMotorVelocity = btScalar(0.);
62    m_maxAngMotorForce = btScalar(0.);
63        m_accumulatedAngMotorImpulse = btScalar(0.0);
64
65} // btSliderConstraint::initParams()
66
67//-----------------------------------------------------------------------------
68
69btSliderConstraint::btSliderConstraint()
70        :btTypedConstraint(SLIDER_CONSTRAINT_TYPE),
71                m_useLinearReferenceFrameA(true),
72                m_useSolveConstraintObsolete(false)
73//              m_useSolveConstraintObsolete(true)
74{
75        initParams();
76} // btSliderConstraint::btSliderConstraint()
77
78//-----------------------------------------------------------------------------
79
80btSliderConstraint::btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
81        : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, rbA, rbB)
82        , m_frameInA(frameInA)
83        , m_frameInB(frameInB),
84                m_useLinearReferenceFrameA(useLinearReferenceFrameA),
85                m_useSolveConstraintObsolete(false)
86//              m_useSolveConstraintObsolete(true)
87{
88        initParams();
89} // btSliderConstraint::btSliderConstraint()
90
91//-----------------------------------------------------------------------------
92
93void btSliderConstraint::buildJacobian()
94{
95        if (!m_useSolveConstraintObsolete) 
96        {
97                return;
98        }
99        if(m_useLinearReferenceFrameA)
100        {
101                buildJacobianInt(m_rbA, m_rbB, m_frameInA, m_frameInB);
102        }
103        else
104        {
105                buildJacobianInt(m_rbB, m_rbA, m_frameInB, m_frameInA);
106        }
107} // btSliderConstraint::buildJacobian()
108
109//-----------------------------------------------------------------------------
110
111void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB)
112{
113        //calculate transforms
114    m_calculatedTransformA = rbA.getCenterOfMassTransform() * frameInA;
115    m_calculatedTransformB = rbB.getCenterOfMassTransform() * frameInB;
116        m_realPivotAInW = m_calculatedTransformA.getOrigin();
117        m_realPivotBInW = m_calculatedTransformB.getOrigin();
118        m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X
119        m_delta = m_realPivotBInW - m_realPivotAInW;
120        m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis;
121        m_relPosA = m_projPivotInW - rbA.getCenterOfMassPosition();
122        m_relPosB = m_realPivotBInW - rbB.getCenterOfMassPosition();
123    btVector3 normalWorld;
124    int i;
125    //linear part
126    for(i = 0; i < 3; i++)
127    {
128                normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
129                new (&m_jacLin[i]) btJacobianEntry(
130                        rbA.getCenterOfMassTransform().getBasis().transpose(),
131                        rbB.getCenterOfMassTransform().getBasis().transpose(),
132                        m_relPosA,
133                        m_relPosB,
134                        normalWorld,
135                        rbA.getInvInertiaDiagLocal(),
136                        rbA.getInvMass(),
137                        rbB.getInvInertiaDiagLocal(),
138                        rbB.getInvMass()
139                        );
140                m_jacLinDiagABInv[i] = btScalar(1.) / m_jacLin[i].getDiagonal();
141                m_depth[i] = m_delta.dot(normalWorld);
142    }
143        testLinLimits();
144    // angular part
145    for(i = 0; i < 3; i++)
146    {
147                normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
148                new (&m_jacAng[i])      btJacobianEntry(
149                        normalWorld,
150            rbA.getCenterOfMassTransform().getBasis().transpose(),
151            rbB.getCenterOfMassTransform().getBasis().transpose(),
152            rbA.getInvInertiaDiagLocal(),
153            rbB.getInvInertiaDiagLocal()
154                        );
155        }
156        testAngLimits();
157        btVector3 axisA = m_calculatedTransformA.getBasis().getColumn(0);
158        m_kAngle = btScalar(1.0 )/ (rbA.computeAngularImpulseDenominator(axisA) + rbB.computeAngularImpulseDenominator(axisA));
159        // clear accumulator for motors
160        m_accumulatedLinMotorImpulse = btScalar(0.0);
161        m_accumulatedAngMotorImpulse = btScalar(0.0);
162} // btSliderConstraint::buildJacobianInt()
163
164//-----------------------------------------------------------------------------
165
166void btSliderConstraint::getInfo1(btConstraintInfo1* info)
167{
168        if (m_useSolveConstraintObsolete)
169        {
170                info->m_numConstraintRows = 0;
171                info->nub = 0;
172        }
173        else
174        {
175                info->m_numConstraintRows = 4; // Fixed 2 linear + 2 angular
176                info->nub = 2; 
177                //prepare constraint
178                calculateTransforms();
179                testLinLimits();
180                if(getSolveLinLimit() || getPoweredLinMotor())
181                {
182                        info->m_numConstraintRows++; // limit 3rd linear as well
183                        info->nub--; 
184                }
185                testAngLimits();
186                if(getSolveAngLimit() || getPoweredAngMotor())
187                {
188                        info->m_numConstraintRows++; // limit 3rd angular as well
189                        info->nub--; 
190                }
191        }
192} // btSliderConstraint::getInfo1()
193
194//-----------------------------------------------------------------------------
195
196void btSliderConstraint::getInfo2(btConstraintInfo2* info)
197{
198        btAssert(!m_useSolveConstraintObsolete);
199        int i, s = info->rowskip;
200        const btTransform& trA = getCalculatedTransformA();
201        const btTransform& trB = getCalculatedTransformB();
202        btScalar signFact = m_useLinearReferenceFrameA ? btScalar(1.0f) : btScalar(-1.0f);
203        // make rotations around Y and Z equal
204        // the slider axis should be the only unconstrained
205        // rotational axis, the angular velocity of the two bodies perpendicular to
206        // the slider axis should be equal. thus the constraint equations are
207        //    p*w1 - p*w2 = 0
208        //    q*w1 - q*w2 = 0
209        // where p and q are unit vectors normal to the slider axis, and w1 and w2
210        // are the angular velocity vectors of the two bodies.
211        // get slider axis (X)
212        btVector3 ax1 = trA.getBasis().getColumn(0);
213        // get 2 orthos to slider axis (Y, Z)
214        btVector3 p = trA.getBasis().getColumn(1);
215        btVector3 q = trA.getBasis().getColumn(2);
216        // set the two slider rows
217        info->m_J1angularAxis[0] = p[0];
218        info->m_J1angularAxis[1] = p[1];
219        info->m_J1angularAxis[2] = p[2];
220        info->m_J1angularAxis[s+0] = q[0];
221        info->m_J1angularAxis[s+1] = q[1];
222        info->m_J1angularAxis[s+2] = q[2];
223
224        info->m_J2angularAxis[0] = -p[0];
225        info->m_J2angularAxis[1] = -p[1];
226        info->m_J2angularAxis[2] = -p[2];
227        info->m_J2angularAxis[s+0] = -q[0];
228        info->m_J2angularAxis[s+1] = -q[1];
229        info->m_J2angularAxis[s+2] = -q[2];
230        // compute the right hand side of the constraint equation. set relative
231        // body velocities along p and q to bring the slider back into alignment.
232        // if ax1,ax2 are the unit length slider axes as computed from body1 and
233        // body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
234        // if "theta" is the angle between ax1 and ax2, we need an angular velocity
235        // along u to cover angle erp*theta in one step :
236        //   |angular_velocity| = angle/time = erp*theta / stepsize
237        //                      = (erp*fps) * theta
238        //    angular_velocity  = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
239        //                      = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
240        // ...as ax1 and ax2 are unit length. if theta is smallish,
241        // theta ~= sin(theta), so
242        //    angular_velocity  = (erp*fps) * (ax1 x ax2)
243        // ax1 x ax2 is in the plane space of ax1, so we project the angular
244        // velocity to p and q to find the right hand side.
245        btScalar k = info->fps * info->erp * getSoftnessOrthoAng();
246    btVector3 ax2 = trB.getBasis().getColumn(0);
247        btVector3 u = ax1.cross(ax2);
248        info->m_constraintError[0] = k * u.dot(p);
249        info->m_constraintError[s] = k * u.dot(q);
250        // pull out pos and R for both bodies. also get the connection
251        // vector c = pos2-pos1.
252        // next two rows. we want: vel2 = vel1 + w1 x c ... but this would
253        // result in three equations, so we project along the planespace vectors
254        // so that sliding along the slider axis is disregarded. for symmetry we
255        // also consider rotation around center of mass of two bodies (factA and factB).
256        btTransform bodyA_trans = m_rbA.getCenterOfMassTransform();
257        btTransform bodyB_trans = m_rbB.getCenterOfMassTransform();
258        int s2 = 2 * s, s3 = 3 * s;
259        btVector3 c;
260        btScalar miA = m_rbA.getInvMass();
261        btScalar miB = m_rbB.getInvMass();
262        btScalar miS = miA + miB;
263        btScalar factA, factB;
264        if(miS > btScalar(0.f))
265        {
266                factA = miB / miS;
267        }
268        else 
269        {
270                factA = btScalar(0.5f);
271        }
272        if(factA > 0.99f) factA = 0.99f;
273        if(factA < 0.01f) factA = 0.01f;
274        factB = btScalar(1.0f) - factA;
275        c = bodyB_trans.getOrigin() - bodyA_trans.getOrigin();
276        btVector3 tmp = c.cross(p);
277        for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = factA*tmp[i];
278        for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = factB*tmp[i];
279        tmp = c.cross(q);
280        for (i=0; i<3; i++) info->m_J1angularAxis[s3+i] = factA*tmp[i];
281        for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = factB*tmp[i];
282
283        for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i];
284        for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i];
285        // compute two elements of right hand side. we want to align the offset
286        // point (in body 2's frame) with the center of body 1.
287        btVector3 ofs; // offset point in global coordinates
288        ofs = trB.getOrigin() - trA.getOrigin();
289        k = info->fps * info->erp * getSoftnessOrthoLin();
290        info->m_constraintError[s2] = k * p.dot(ofs);
291        info->m_constraintError[s3] = k * q.dot(ofs);
292        int nrow = 3; // last filled row
293        int srow;
294        // check linear limits linear
295        btScalar limit_err = btScalar(0.0);
296        int limit = 0;
297        if(getSolveLinLimit())
298        {
299                limit_err = getLinDepth() *  signFact;
300                limit = (limit_err > btScalar(0.0)) ? 2 : 1;
301        }
302        int powered = 0;
303        if(getPoweredLinMotor())
304        {
305                powered = 1;
306        }
307        // if the slider has joint limits or motor, add in the extra row
308        if (limit || powered) 
309        {
310                nrow++;
311                srow = nrow * info->rowskip;
312                info->m_J1linearAxis[srow+0] = ax1[0];
313                info->m_J1linearAxis[srow+1] = ax1[1];
314                info->m_J1linearAxis[srow+2] = ax1[2];
315                // linear torque decoupling step:
316                //
317                // we have to be careful that the linear constraint forces (+/- ax1) applied to the two bodies
318                // do not create a torque couple. in other words, the points that the
319                // constraint force is applied at must lie along the same ax1 axis.
320                // a torque couple will result in limited slider-jointed free
321                // bodies from gaining angular momentum.
322                // the solution used here is to apply the constraint forces at the center of mass of the two bodies
323                btVector3 ltd;  // Linear Torque Decoupling vector (a torque)
324//              c = btScalar(0.5) * c;
325                ltd = c.cross(ax1);
326                info->m_J1angularAxis[srow+0] = factA*ltd[0];
327                info->m_J1angularAxis[srow+1] = factA*ltd[1];
328                info->m_J1angularAxis[srow+2] = factA*ltd[2];
329                info->m_J2angularAxis[srow+0] = factB*ltd[0];
330                info->m_J2angularAxis[srow+1] = factB*ltd[1];
331                info->m_J2angularAxis[srow+2] = factB*ltd[2];
332                // right-hand part
333                btScalar lostop = getLowerLinLimit();
334                btScalar histop = getUpperLinLimit();
335                if(limit && (lostop == histop))
336                {  // the joint motor is ineffective
337                        powered = 0;
338                }
339                info->m_constraintError[srow] = 0.;
340                info->m_lowerLimit[srow] = 0.;
341                info->m_upperLimit[srow] = 0.;
342                if(powered)
343                {
344            info->cfm[nrow] = btScalar(0.0); 
345                        btScalar tag_vel = getTargetLinMotorVelocity();
346                        btScalar mot_fact = getMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info->fps * info->erp);
347//                      info->m_constraintError[srow] += mot_fact * getTargetLinMotorVelocity();
348                        info->m_constraintError[srow] -= signFact * mot_fact * getTargetLinMotorVelocity();
349                        info->m_lowerLimit[srow] += -getMaxLinMotorForce() * info->fps;
350                        info->m_upperLimit[srow] += getMaxLinMotorForce() * info->fps;
351                }
352                if(limit)
353                {
354                        k = info->fps * info->erp;
355                        info->m_constraintError[srow] += k * limit_err;
356                        info->cfm[srow] = btScalar(0.0); // stop_cfm;
357                        if(lostop == histop) 
358                        {       // limited low and high simultaneously
359                                info->m_lowerLimit[srow] = -SIMD_INFINITY;
360                                info->m_upperLimit[srow] = SIMD_INFINITY;
361                        }
362                        else if(limit == 1) 
363                        { // low limit
364                                info->m_lowerLimit[srow] = -SIMD_INFINITY;
365                                info->m_upperLimit[srow] = 0;
366                        }
367                        else 
368                        { // high limit
369                                info->m_lowerLimit[srow] = 0;
370                                info->m_upperLimit[srow] = SIMD_INFINITY;
371                        }
372                        // bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin) for that)
373                        btScalar bounce = btFabs(btScalar(1.0) - getDampingLimLin());
374                        if(bounce > btScalar(0.0))
375                        {
376                                btScalar vel = m_rbA.getLinearVelocity().dot(ax1);
377                                vel -= m_rbB.getLinearVelocity().dot(ax1);
378                                vel *= signFact;
379                                // only apply bounce if the velocity is incoming, and if the
380                                // resulting c[] exceeds what we already have.
381                                if(limit == 1)
382                                {       // low limit
383                                        if(vel < 0)
384                                        {
385                                                btScalar newc = -bounce * vel;
386                                                if (newc > info->m_constraintError[srow])
387                                                {
388                                                        info->m_constraintError[srow] = newc;
389                                                }
390                                        }
391                                }
392                                else
393                                { // high limit - all those computations are reversed
394                                        if(vel > 0)
395                                        {
396                                                btScalar newc = -bounce * vel;
397                                                if(newc < info->m_constraintError[srow]) 
398                                                {
399                                                        info->m_constraintError[srow] = newc;
400                                                }
401                                        }
402                                }
403                        }
404                        info->m_constraintError[srow] *= getSoftnessLimLin();
405                } // if(limit)
406        } // if linear limit
407        // check angular limits
408        limit_err = btScalar(0.0);
409        limit = 0;
410        if(getSolveAngLimit())
411        {
412                limit_err = getAngDepth();
413                limit = (limit_err > btScalar(0.0)) ? 1 : 2;
414        }
415        // if the slider has joint limits, add in the extra row
416        powered = 0;
417        if(getPoweredAngMotor())
418        {
419                powered = 1;
420        }
421        if(limit || powered) 
422        {
423                nrow++;
424                srow = nrow * info->rowskip;
425                info->m_J1angularAxis[srow+0] = ax1[0];
426                info->m_J1angularAxis[srow+1] = ax1[1];
427                info->m_J1angularAxis[srow+2] = ax1[2];
428
429                info->m_J2angularAxis[srow+0] = -ax1[0];
430                info->m_J2angularAxis[srow+1] = -ax1[1];
431                info->m_J2angularAxis[srow+2] = -ax1[2];
432
433                btScalar lostop = getLowerAngLimit();
434                btScalar histop = getUpperAngLimit();
435                if(limit && (lostop == histop))
436                {  // the joint motor is ineffective
437                        powered = 0;
438                }
439                if(powered)
440                {
441            info->cfm[srow] = btScalar(0.0); 
442                        btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * info->erp);
443                        info->m_constraintError[srow] = mot_fact * getTargetAngMotorVelocity();
444                        info->m_lowerLimit[srow] = -getMaxAngMotorForce() * info->fps;
445                        info->m_upperLimit[srow] = getMaxAngMotorForce() * info->fps;
446                }
447                if(limit)
448                {
449                        k = info->fps * info->erp;
450                        info->m_constraintError[srow] += k * limit_err;
451                        info->cfm[srow] = btScalar(0.0); // stop_cfm;
452                        if(lostop == histop) 
453                        {
454                                // limited low and high simultaneously
455                                info->m_lowerLimit[srow] = -SIMD_INFINITY;
456                                info->m_upperLimit[srow] = SIMD_INFINITY;
457                        }
458                        else if(limit == 1) 
459                        { // low limit
460                                info->m_lowerLimit[srow] = 0;
461                                info->m_upperLimit[srow] = SIMD_INFINITY;
462                        }
463                        else 
464                        { // high limit
465                                info->m_lowerLimit[srow] = -SIMD_INFINITY;
466                                info->m_upperLimit[srow] = 0;
467                        }
468                        // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
469                        btScalar bounce = btFabs(btScalar(1.0) - getDampingLimAng());
470                        if(bounce > btScalar(0.0))
471                        {
472                                btScalar vel = m_rbA.getAngularVelocity().dot(ax1);
473                                vel -= m_rbB.getAngularVelocity().dot(ax1);
474                                // only apply bounce if the velocity is incoming, and if the
475                                // resulting c[] exceeds what we already have.
476                                if(limit == 1)
477                                {       // low limit
478                                        if(vel < 0)
479                                        {
480                                                btScalar newc = -bounce * vel;
481                                                if(newc > info->m_constraintError[srow])
482                                                {
483                                                        info->m_constraintError[srow] = newc;
484                                                }
485                                        }
486                                }
487                                else
488                                {       // high limit - all those computations are reversed
489                                        if(vel > 0)
490                                        {
491                                                btScalar newc = -bounce * vel;
492                                                if(newc < info->m_constraintError[srow])
493                                                {
494                                                        info->m_constraintError[srow] = newc;
495                                                }
496                                        }
497                                }
498                        }
499                        info->m_constraintError[srow] *= getSoftnessLimAng();
500                } // if(limit)
501        } // if angular limit or powered
502} // btSliderConstraint::getInfo2()
503
504//-----------------------------------------------------------------------------
505
506void btSliderConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep)
507{
508        if (m_useSolveConstraintObsolete)
509        {
510                m_timeStep = timeStep;
511                if(m_useLinearReferenceFrameA)
512                {
513                        solveConstraintInt(m_rbA,bodyA, m_rbB,bodyB);
514                }
515                else
516                {
517                        solveConstraintInt(m_rbB,bodyB, m_rbA,bodyA);
518                }
519        }
520} // btSliderConstraint::solveConstraint()
521
522//-----------------------------------------------------------------------------
523
524void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB)
525{
526    int i;
527    // linear
528    btVector3 velA;
529        bodyA.getVelocityInLocalPointObsolete(m_relPosA,velA);
530    btVector3 velB;
531        bodyB.getVelocityInLocalPointObsolete(m_relPosB,velB);
532    btVector3 vel = velA - velB;
533        for(i = 0; i < 3; i++)
534    {
535                const btVector3& normal = m_jacLin[i].m_linearJointAxis;
536                btScalar rel_vel = normal.dot(vel);
537                // calculate positional error
538                btScalar depth = m_depth[i];
539                // get parameters
540                btScalar softness = (i) ? m_softnessOrthoLin : (m_solveLinLim ? m_softnessLimLin : m_softnessDirLin);
541                btScalar restitution = (i) ? m_restitutionOrthoLin : (m_solveLinLim ? m_restitutionLimLin : m_restitutionDirLin);
542                btScalar damping = (i) ? m_dampingOrthoLin : (m_solveLinLim ? m_dampingLimLin : m_dampingDirLin);
543                // calcutate and apply impulse
544                btScalar normalImpulse = softness * (restitution * depth / m_timeStep - damping * rel_vel) * m_jacLinDiagABInv[i];
545                btVector3 impulse_vector = normal * normalImpulse;
546               
547                //rbA.applyImpulse( impulse_vector, m_relPosA);
548                //rbB.applyImpulse(-impulse_vector, m_relPosB);
549                {
550                        btVector3 ftorqueAxis1 = m_relPosA.cross(normal);
551                        btVector3 ftorqueAxis2 = m_relPosB.cross(normal);
552                        bodyA.applyImpulse(normal*rbA.getInvMass(), rbA.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
553                        bodyB.applyImpulse(normal*rbB.getInvMass(), rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);
554                }
555
556
557
558                if(m_poweredLinMotor && (!i))
559                { // apply linear motor
560                        if(m_accumulatedLinMotorImpulse < m_maxLinMotorForce)
561                        {
562                                btScalar desiredMotorVel = m_targetLinMotorVelocity;
563                                btScalar motor_relvel = desiredMotorVel + rel_vel;
564                                normalImpulse = -motor_relvel * m_jacLinDiagABInv[i];
565                                // clamp accumulated impulse
566                                btScalar new_acc = m_accumulatedLinMotorImpulse + btFabs(normalImpulse);
567                                if(new_acc  > m_maxLinMotorForce)
568                                {
569                                        new_acc = m_maxLinMotorForce;
570                                }
571                                btScalar del = new_acc  - m_accumulatedLinMotorImpulse;
572                                if(normalImpulse < btScalar(0.0))
573                                {
574                                        normalImpulse = -del;
575                                }
576                                else
577                                {
578                                        normalImpulse = del;
579                                }
580                                m_accumulatedLinMotorImpulse = new_acc;
581                                // apply clamped impulse
582                                impulse_vector = normal * normalImpulse;
583                                //rbA.applyImpulse( impulse_vector, m_relPosA);
584                                //rbB.applyImpulse(-impulse_vector, m_relPosB);
585
586                                {
587                                        btVector3 ftorqueAxis1 = m_relPosA.cross(normal);
588                                        btVector3 ftorqueAxis2 = m_relPosB.cross(normal);
589                                        bodyA.applyImpulse(normal*rbA.getInvMass(), rbA.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
590                                        bodyB.applyImpulse(normal*rbB.getInvMass(), rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);
591                                }
592
593
594
595                        }
596                }
597    }
598        // angular
599        // get axes in world space
600        btVector3 axisA =  m_calculatedTransformA.getBasis().getColumn(0);
601        btVector3 axisB =  m_calculatedTransformB.getBasis().getColumn(0);
602
603        btVector3 angVelA;
604        bodyA.getAngularVelocity(angVelA);
605        btVector3 angVelB;
606        bodyB.getAngularVelocity(angVelB);
607
608        btVector3 angVelAroundAxisA = axisA * axisA.dot(angVelA);
609        btVector3 angVelAroundAxisB = axisB * axisB.dot(angVelB);
610
611        btVector3 angAorthog = angVelA - angVelAroundAxisA;
612        btVector3 angBorthog = angVelB - angVelAroundAxisB;
613        btVector3 velrelOrthog = angAorthog-angBorthog;
614        //solve orthogonal angular velocity correction
615        btScalar len = velrelOrthog.length();
616        btScalar orthorImpulseMag = 0.f;
617
618        if (len > btScalar(0.00001))
619        {
620                btVector3 normal = velrelOrthog.normalized();
621                btScalar denom = rbA.computeAngularImpulseDenominator(normal) + rbB.computeAngularImpulseDenominator(normal);
622                //velrelOrthog *= (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
623                orthorImpulseMag = (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
624        }
625        //solve angular positional correction
626        btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/m_timeStep);
627        btVector3 angularAxis = angularError;
628        btScalar angularImpulseMag = 0;
629
630        btScalar len2 = angularError.length();
631        if (len2>btScalar(0.00001))
632        {
633                btVector3 normal2 = angularError.normalized();
634                btScalar denom2 = rbA.computeAngularImpulseDenominator(normal2) + rbB.computeAngularImpulseDenominator(normal2);
635                angularImpulseMag = (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng;
636                angularError *= angularImpulseMag;
637        }
638        // apply impulse
639        //rbA.applyTorqueImpulse(-velrelOrthog+angularError);
640        //rbB.applyTorqueImpulse(velrelOrthog-angularError);
641
642        bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*velrelOrthog,-orthorImpulseMag);
643        bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*velrelOrthog,orthorImpulseMag);
644        bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*angularAxis,angularImpulseMag);
645        bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*angularAxis,-angularImpulseMag);
646
647
648        btScalar impulseMag;
649        //solve angular limits
650        if(m_solveAngLim)
651        {
652                impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingLimAng + m_angDepth * m_restitutionLimAng / m_timeStep;
653                impulseMag *= m_kAngle * m_softnessLimAng;
654        }
655        else
656        {
657                impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingDirAng + m_angDepth * m_restitutionDirAng / m_timeStep;
658                impulseMag *= m_kAngle * m_softnessDirAng;
659        }
660        btVector3 impulse = axisA * impulseMag;
661        //rbA.applyTorqueImpulse(impulse);
662        //rbB.applyTorqueImpulse(-impulse);
663
664        bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*axisA,impulseMag);
665        bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*axisA,-impulseMag);
666
667
668
669        //apply angular motor
670        if(m_poweredAngMotor) 
671        {
672                if(m_accumulatedAngMotorImpulse < m_maxAngMotorForce)
673                {
674                        btVector3 velrel = angVelAroundAxisA - angVelAroundAxisB;
675                        btScalar projRelVel = velrel.dot(axisA);
676
677                        btScalar desiredMotorVel = m_targetAngMotorVelocity;
678                        btScalar motor_relvel = desiredMotorVel - projRelVel;
679
680                        btScalar angImpulse = m_kAngle * motor_relvel;
681                        // clamp accumulated impulse
682                        btScalar new_acc = m_accumulatedAngMotorImpulse + btFabs(angImpulse);
683                        if(new_acc  > m_maxAngMotorForce)
684                        {
685                                new_acc = m_maxAngMotorForce;
686                        }
687                        btScalar del = new_acc  - m_accumulatedAngMotorImpulse;
688                        if(angImpulse < btScalar(0.0))
689                        {
690                                angImpulse = -del;
691                        }
692                        else
693                        {
694                                angImpulse = del;
695                        }
696                        m_accumulatedAngMotorImpulse = new_acc;
697                        // apply clamped impulse
698                        btVector3 motorImp = angImpulse * axisA;
699                        //rbA.applyTorqueImpulse(motorImp);
700                        //rbB.applyTorqueImpulse(-motorImp);
701
702                        bodyA.applyImpulse(btVector3(0,0,0), rbA.getInvInertiaTensorWorld()*axisA,angImpulse);
703                        bodyB.applyImpulse(btVector3(0,0,0), rbB.getInvInertiaTensorWorld()*axisA,-angImpulse);
704                }
705        }
706} // btSliderConstraint::solveConstraint()
707
708//-----------------------------------------------------------------------------
709
710//-----------------------------------------------------------------------------
711
712void btSliderConstraint::calculateTransforms(void){
713        if(m_useLinearReferenceFrameA || (!m_useSolveConstraintObsolete))
714        {
715                m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA;
716                m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB;
717        }
718        else
719        {
720                m_calculatedTransformA = m_rbB.getCenterOfMassTransform() * m_frameInB;
721                m_calculatedTransformB = m_rbA.getCenterOfMassTransform() * m_frameInA;
722        }
723        m_realPivotAInW = m_calculatedTransformA.getOrigin();
724        m_realPivotBInW = m_calculatedTransformB.getOrigin();
725        m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X
726        if(m_useLinearReferenceFrameA || m_useSolveConstraintObsolete)
727        {
728                m_delta = m_realPivotBInW - m_realPivotAInW;
729        }
730        else
731        {
732                m_delta = m_realPivotAInW - m_realPivotBInW;
733        }
734        m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis;
735    btVector3 normalWorld;
736    int i;
737    //linear part
738    for(i = 0; i < 3; i++)
739    {
740                normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
741                m_depth[i] = m_delta.dot(normalWorld);
742    }
743} // btSliderConstraint::calculateTransforms()
744 
745//-----------------------------------------------------------------------------
746
747void btSliderConstraint::testLinLimits(void)
748{
749        m_solveLinLim = false;
750        m_linPos = m_depth[0];
751        if(m_lowerLinLimit <= m_upperLinLimit)
752        {
753                if(m_depth[0] > m_upperLinLimit)
754                {
755                        m_depth[0] -= m_upperLinLimit;
756                        m_solveLinLim = true;
757                }
758                else if(m_depth[0] < m_lowerLinLimit)
759                {
760                        m_depth[0] -= m_lowerLinLimit;
761                        m_solveLinLim = true;
762                }
763                else
764                {
765                        m_depth[0] = btScalar(0.);
766                }
767        }
768        else
769        {
770                m_depth[0] = btScalar(0.);
771        }
772} // btSliderConstraint::testLinLimits()
773
774//-----------------------------------------------------------------------------
775
776void btSliderConstraint::testAngLimits(void)
777{
778        m_angDepth = btScalar(0.);
779        m_solveAngLim = false;
780        if(m_lowerAngLimit <= m_upperAngLimit)
781        {
782                const btVector3 axisA0 = m_calculatedTransformA.getBasis().getColumn(1);
783                const btVector3 axisA1 = m_calculatedTransformA.getBasis().getColumn(2);
784                const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1);
785                btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0)); 
786                m_angPos = rot;
787                if(rot < m_lowerAngLimit)
788                {
789                        m_angDepth = rot - m_lowerAngLimit;
790                        m_solveAngLim = true;
791                } 
792                else if(rot > m_upperAngLimit)
793                {
794                        m_angDepth = rot - m_upperAngLimit;
795                        m_solveAngLim = true;
796                }
797        }
798} // btSliderConstraint::testAngLimits()
799       
800//-----------------------------------------------------------------------------
801
802btVector3 btSliderConstraint::getAncorInA(void)
803{
804        btVector3 ancorInA;
805        ancorInA = m_realPivotAInW + (m_lowerLinLimit + m_upperLinLimit) * btScalar(0.5) * m_sliderAxis;
806        ancorInA = m_rbA.getCenterOfMassTransform().inverse() * ancorInA;
807        return ancorInA;
808} // btSliderConstraint::getAncorInA()
809
810//-----------------------------------------------------------------------------
811
812btVector3 btSliderConstraint::getAncorInB(void)
813{
814        btVector3 ancorInB;
815        ancorInB = m_frameInB.getOrigin();
816        return ancorInB;
817} // btSliderConstraint::getAncorInB();
Note: See TracBrowser for help on using the repository browser.