← Back to branch summary

~siretart/ubuntu/utopic/blender/libav10

« back to all changes in this revision

Viewing changes to extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp

  • Committer: Package Import Robot
  • Author(s): Matteo F. Vescovi
  • Date: 2012-07-23 08:54:18 UTC
  • mfrom: (14.2.16 sid)
  • mto: (14.2.19 sid)
  • mto: This revision was merged to the branch mainline in revision 42.
  • Revision ID: package-import@ubuntu.com-20120723085418-9foz30v6afaf5ffs
Tags: 2.63a-2
http://bugs.debian.org/658075
* debian/: Cycles support added (Closes: #658075)
  For now, this top feature has been enabled only
  on [any-amd64 any-i386] architectures because
  of OpenImageIO failing on all others
* debian/: scripts installation path changed
  from /usr/lib to /usr/share:
  + debian/patches/: patchset re-worked for path changing
  + debian/control: "Breaks" field added on yafaray-exporter

Show diffs side-by-side

added added

removed removed

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