Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/physics/src/bullet/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp @ 2056

Last change on this file since 2056 was 1963, checked in by rgrieder, 16 years ago

Added Bullet physics engine.
  • Property svn:eol-style set to native
File size: 14.2 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{
73        initParams();
74} // btSliderConstraint::btSliderConstraint()
75
76//-----------------------------------------------------------------------------
77
78btSliderConstraint::btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)
79        : btTypedConstraint(SLIDER_CONSTRAINT_TYPE, rbA, rbB)
80        , m_frameInA(frameInA)
81        , m_frameInB(frameInB),
82                m_useLinearReferenceFrameA(useLinearReferenceFrameA)
83{
84        initParams();
85} // btSliderConstraint::btSliderConstraint()
86
87//-----------------------------------------------------------------------------
88
89void btSliderConstraint::buildJacobian()
90{
91        if(m_useLinearReferenceFrameA)
92        {
93                buildJacobianInt(m_rbA, m_rbB, m_frameInA, m_frameInB);
94        }
95        else
96        {
97                buildJacobianInt(m_rbB, m_rbA, m_frameInB, m_frameInA);
98        }
99} // btSliderConstraint::buildJacobian()
100
101//-----------------------------------------------------------------------------
102
103void btSliderConstraint::buildJacobianInt(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB)
104{
105        //calculate transforms
106    m_calculatedTransformA = rbA.getCenterOfMassTransform() * frameInA;
107    m_calculatedTransformB = rbB.getCenterOfMassTransform() * frameInB;
108        m_realPivotAInW = m_calculatedTransformA.getOrigin();
109        m_realPivotBInW = m_calculatedTransformB.getOrigin();
110        m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X
111        m_delta = m_realPivotBInW - m_realPivotAInW;
112        m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis;
113        m_relPosA = m_projPivotInW - rbA.getCenterOfMassPosition();
114        m_relPosB = m_realPivotBInW - rbB.getCenterOfMassPosition();
115    btVector3 normalWorld;
116    int i;
117    //linear part
118    for(i = 0; i < 3; i++)
119    {
120                normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
121                new (&m_jacLin[i]) btJacobianEntry(
122                        rbA.getCenterOfMassTransform().getBasis().transpose(),
123                        rbB.getCenterOfMassTransform().getBasis().transpose(),
124                        m_relPosA,
125                        m_relPosB,
126                        normalWorld,
127                        rbA.getInvInertiaDiagLocal(),
128                        rbA.getInvMass(),
129                        rbB.getInvInertiaDiagLocal(),
130                        rbB.getInvMass()
131                        );
132                m_jacLinDiagABInv[i] = btScalar(1.) / m_jacLin[i].getDiagonal();
133                m_depth[i] = m_delta.dot(normalWorld);
134    }
135        testLinLimits();
136    // angular part
137    for(i = 0; i < 3; i++)
138    {
139                normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
140                new (&m_jacAng[i])      btJacobianEntry(
141                        normalWorld,
142            rbA.getCenterOfMassTransform().getBasis().transpose(),
143            rbB.getCenterOfMassTransform().getBasis().transpose(),
144            rbA.getInvInertiaDiagLocal(),
145            rbB.getInvInertiaDiagLocal()
146                        );
147        }
148        testAngLimits();
149        btVector3 axisA = m_calculatedTransformA.getBasis().getColumn(0);
150        m_kAngle = btScalar(1.0 )/ (rbA.computeAngularImpulseDenominator(axisA) + rbB.computeAngularImpulseDenominator(axisA));
151        // clear accumulator for motors
152        m_accumulatedLinMotorImpulse = btScalar(0.0);
153        m_accumulatedAngMotorImpulse = btScalar(0.0);
154} // btSliderConstraint::buildJacobianInt()
155
156//-----------------------------------------------------------------------------
157
158void btSliderConstraint::solveConstraint(btScalar timeStep)
159{
160    m_timeStep = timeStep;
161        if(m_useLinearReferenceFrameA)
162        {
163                solveConstraintInt(m_rbA, m_rbB);
164        }
165        else
166        {
167                solveConstraintInt(m_rbB, m_rbA);
168        }
169} // btSliderConstraint::solveConstraint()
170
171//-----------------------------------------------------------------------------
172
173void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btRigidBody& rbB)
174{
175    int i;
176    // linear
177    btVector3 velA = rbA.getVelocityInLocalPoint(m_relPosA);
178    btVector3 velB = rbB.getVelocityInLocalPoint(m_relPosB);
179    btVector3 vel = velA - velB;
180        for(i = 0; i < 3; i++)
181    {
182                const btVector3& normal = m_jacLin[i].m_linearJointAxis;
183                btScalar rel_vel = normal.dot(vel);
184                // calculate positional error
185                btScalar depth = m_depth[i];
186                // get parameters
187                btScalar softness = (i) ? m_softnessOrthoLin : (m_solveLinLim ? m_softnessLimLin : m_softnessDirLin);
188                btScalar restitution = (i) ? m_restitutionOrthoLin : (m_solveLinLim ? m_restitutionLimLin : m_restitutionDirLin);
189                btScalar damping = (i) ? m_dampingOrthoLin : (m_solveLinLim ? m_dampingLimLin : m_dampingDirLin);
190                // calcutate and apply impulse
191                btScalar normalImpulse = softness * (restitution * depth / m_timeStep - damping * rel_vel) * m_jacLinDiagABInv[i];
192                btVector3 impulse_vector = normal * normalImpulse;
193                rbA.applyImpulse( impulse_vector, m_relPosA);
194                rbB.applyImpulse(-impulse_vector, m_relPosB);
195                if(m_poweredLinMotor && (!i))
196                { // apply linear motor
197                        if(m_accumulatedLinMotorImpulse < m_maxLinMotorForce)
198                        {
199                                btScalar desiredMotorVel = m_targetLinMotorVelocity;
200                                btScalar motor_relvel = desiredMotorVel + rel_vel;
201                                normalImpulse = -motor_relvel * m_jacLinDiagABInv[i];
202                                // clamp accumulated impulse
203                                btScalar new_acc = m_accumulatedLinMotorImpulse + btFabs(normalImpulse);
204                                if(new_acc  > m_maxLinMotorForce)
205                                {
206                                        new_acc = m_maxLinMotorForce;
207                                }
208                                btScalar del = new_acc  - m_accumulatedLinMotorImpulse;
209                                if(normalImpulse < btScalar(0.0))
210                                {
211                                        normalImpulse = -del;
212                                }
213                                else
214                                {
215                                        normalImpulse = del;
216                                }
217                                m_accumulatedLinMotorImpulse = new_acc;
218                                // apply clamped impulse
219                                impulse_vector = normal * normalImpulse;
220                                rbA.applyImpulse( impulse_vector, m_relPosA);
221                                rbB.applyImpulse(-impulse_vector, m_relPosB);
222                        }
223                }
224    }
225        // angular
226        // get axes in world space
227        btVector3 axisA =  m_calculatedTransformA.getBasis().getColumn(0);
228        btVector3 axisB =  m_calculatedTransformB.getBasis().getColumn(0);
229
230        const btVector3& angVelA = rbA.getAngularVelocity();
231        const btVector3& angVelB = rbB.getAngularVelocity();
232
233        btVector3 angVelAroundAxisA = axisA * axisA.dot(angVelA);
234        btVector3 angVelAroundAxisB = axisB * axisB.dot(angVelB);
235
236        btVector3 angAorthog = angVelA - angVelAroundAxisA;
237        btVector3 angBorthog = angVelB - angVelAroundAxisB;
238        btVector3 velrelOrthog = angAorthog-angBorthog;
239        //solve orthogonal angular velocity correction
240        btScalar len = velrelOrthog.length();
241        if (len > btScalar(0.00001))
242        {
243                btVector3 normal = velrelOrthog.normalized();
244                btScalar denom = rbA.computeAngularImpulseDenominator(normal) + rbB.computeAngularImpulseDenominator(normal);
245                velrelOrthog *= (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
246        }
247        //solve angular positional correction
248        btVector3 angularError = axisA.cross(axisB) *(btScalar(1.)/m_timeStep);
249        btScalar len2 = angularError.length();
250        if (len2>btScalar(0.00001))
251        {
252                btVector3 normal2 = angularError.normalized();
253                btScalar denom2 = rbA.computeAngularImpulseDenominator(normal2) + rbB.computeAngularImpulseDenominator(normal2);
254                angularError *= (btScalar(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng;
255        }
256        // apply impulse
257        rbA.applyTorqueImpulse(-velrelOrthog+angularError);
258        rbB.applyTorqueImpulse(velrelOrthog-angularError);
259        btScalar impulseMag;
260        //solve angular limits
261        if(m_solveAngLim)
262        {
263                impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingLimAng + m_angDepth * m_restitutionLimAng / m_timeStep;
264                impulseMag *= m_kAngle * m_softnessLimAng;
265        }
266        else
267        {
268                impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingDirAng + m_angDepth * m_restitutionDirAng / m_timeStep;
269                impulseMag *= m_kAngle * m_softnessDirAng;
270        }
271        btVector3 impulse = axisA * impulseMag;
272        rbA.applyTorqueImpulse(impulse);
273        rbB.applyTorqueImpulse(-impulse);
274        //apply angular motor
275        if(m_poweredAngMotor) 
276        {
277                if(m_accumulatedAngMotorImpulse < m_maxAngMotorForce)
278                {
279                        btVector3 velrel = angVelAroundAxisA - angVelAroundAxisB;
280                        btScalar projRelVel = velrel.dot(axisA);
281
282                        btScalar desiredMotorVel = m_targetAngMotorVelocity;
283                        btScalar motor_relvel = desiredMotorVel - projRelVel;
284
285                        btScalar angImpulse = m_kAngle * motor_relvel;
286                        // clamp accumulated impulse
287                        btScalar new_acc = m_accumulatedAngMotorImpulse + btFabs(angImpulse);
288                        if(new_acc  > m_maxAngMotorForce)
289                        {
290                                new_acc = m_maxAngMotorForce;
291                        }
292                        btScalar del = new_acc  - m_accumulatedAngMotorImpulse;
293                        if(angImpulse < btScalar(0.0))
294                        {
295                                angImpulse = -del;
296                        }
297                        else
298                        {
299                                angImpulse = del;
300                        }
301                        m_accumulatedAngMotorImpulse = new_acc;
302                        // apply clamped impulse
303                        btVector3 motorImp = angImpulse * axisA;
304                        m_rbA.applyTorqueImpulse(motorImp);
305                        m_rbB.applyTorqueImpulse(-motorImp);
306                }
307        }
308} // btSliderConstraint::solveConstraint()
309
310//-----------------------------------------------------------------------------
311
312//-----------------------------------------------------------------------------
313
314void btSliderConstraint::calculateTransforms(void){
315        if(m_useLinearReferenceFrameA)
316        {
317                m_calculatedTransformA = m_rbA.getCenterOfMassTransform() * m_frameInA;
318                m_calculatedTransformB = m_rbB.getCenterOfMassTransform() * m_frameInB;
319        }
320        else
321        {
322                m_calculatedTransformA = m_rbB.getCenterOfMassTransform() * m_frameInB;
323                m_calculatedTransformB = m_rbA.getCenterOfMassTransform() * m_frameInA;
324        }
325        m_realPivotAInW = m_calculatedTransformA.getOrigin();
326        m_realPivotBInW = m_calculatedTransformB.getOrigin();
327        m_sliderAxis = m_calculatedTransformA.getBasis().getColumn(0); // along X
328        m_delta = m_realPivotBInW - m_realPivotAInW;
329        m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis;
330    btVector3 normalWorld;
331    int i;
332    //linear part
333    for(i = 0; i < 3; i++)
334    {
335                normalWorld = m_calculatedTransformA.getBasis().getColumn(i);
336                m_depth[i] = m_delta.dot(normalWorld);
337    }
338} // btSliderConstraint::calculateTransforms()
339 
340//-----------------------------------------------------------------------------
341
342void btSliderConstraint::testLinLimits(void)
343{
344        m_solveLinLim = false;
345        m_linPos = m_depth[0];
346        if(m_lowerLinLimit <= m_upperLinLimit)
347        {
348                if(m_depth[0] > m_upperLinLimit)
349                {
350                        m_depth[0] -= m_upperLinLimit;
351                        m_solveLinLim = true;
352                }
353                else if(m_depth[0] < m_lowerLinLimit)
354                {
355                        m_depth[0] -= m_lowerLinLimit;
356                        m_solveLinLim = true;
357                }
358                else
359                {
360                        m_depth[0] = btScalar(0.);
361                }
362        }
363        else
364        {
365                m_depth[0] = btScalar(0.);
366        }
367} // btSliderConstraint::testLinLimits()
368
369//-----------------------------------------------------------------------------
370 
371
372void btSliderConstraint::testAngLimits(void)
373{
374        m_angDepth = btScalar(0.);
375        m_solveAngLim = false;
376        if(m_lowerAngLimit <= m_upperAngLimit)
377        {
378                const btVector3 axisA0 = m_calculatedTransformA.getBasis().getColumn(1);
379                const btVector3 axisA1 = m_calculatedTransformA.getBasis().getColumn(2);
380                const btVector3 axisB0 = m_calculatedTransformB.getBasis().getColumn(1);
381                btScalar rot = btAtan2Fast(axisB0.dot(axisA1), axisB0.dot(axisA0)); 
382                if(rot < m_lowerAngLimit)
383                {
384                        m_angDepth = rot - m_lowerAngLimit;
385                        m_solveAngLim = true;
386                } 
387                else if(rot > m_upperAngLimit)
388                {
389                        m_angDepth = rot - m_upperAngLimit;
390                        m_solveAngLim = true;
391                }
392        }
393} // btSliderConstraint::testAngLimits()
394
395       
396//-----------------------------------------------------------------------------
397
398
399
400btVector3 btSliderConstraint::getAncorInA(void)
401{
402        btVector3 ancorInA;
403        ancorInA = m_realPivotAInW + (m_lowerLinLimit + m_upperLinLimit) * btScalar(0.5) * m_sliderAxis;
404        ancorInA = m_rbA.getCenterOfMassTransform().inverse() * ancorInA;
405        return ancorInA;
406} // btSliderConstraint::getAncorInA()
407
408//-----------------------------------------------------------------------------
409
410btVector3 btSliderConstraint::getAncorInB(void)
411{
412        btVector3 ancorInB;
413        ancorInB = m_frameInB.getOrigin();
414        return ancorInB;
415} // btSliderConstraint::getAncorInB();
Note: See TracBrowser for help on using the repository browser.