Minor fixes in Bullet/constraint solving
[blender-staging.git] / extern / bullet2 / src / BulletDynamics / ConstraintSolver / btSliderConstraint.cpp
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
4
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
10
11 1. 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.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
15
16 /*
17 Added by Roman Ponomarev (rponom@gmail.com)
18 April 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
30 void 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 }
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 }
77
78
79
80 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         ,
96         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
102 //      m_frameInA.getOrigin() = m_rbA.getCenterOfMassTransform()(m_frameInA.getOrigin());
103
104         initParams();
105 }
106
107
108
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
180
181
182 void btSliderConstraint::getInfo1(btConstraintInfo1* info)
183 {
184         if (m_useSolveConstraintObsolete)
185         {
186                 info->m_numConstraintRows = 0;
187                 info->nub = 0;
188         }
189         else
190         {
191                 info->m_numConstraintRows = 4; // Fixed 2 linear + 2 angular
192                 info->nub = 2; 
193                 //prepare constraint
194                 calculateTransforms();
195                 testLinLimits();
196                 if(getSolveLinLimit() || getPoweredLinMotor())
197                 {
198                         info->m_numConstraintRows++; // limit 3rd linear as well
199                         info->nub--; 
200                 }
201                 testAngLimits();
202                 if(getSolveAngLimit() || getPoweredAngMotor())
203                 {
204                         info->m_numConstraintRows++; // limit 3rd angular as well
205                         info->nub--; 
206                 }
207         }
208 }
209
210
211
212 void btSliderConstraint::getInfo2(btConstraintInfo2* info)
213 {
214         btAssert(!m_useSolveConstraintObsolete);
215         int i, s = info->rowskip;
216         const btTransform& trA = getCalculatedTransformA();
217         const btTransform& trB = getCalculatedTransformB();
218         btScalar signFact = m_useLinearReferenceFrameA ? btScalar(1.0f) : btScalar(-1.0f);
219         // make rotations around Y and Z equal
220         // the slider axis should be the only unconstrained
221         // rotational axis, the angular velocity of the two bodies perpendicular to
222         // the slider axis should be equal. thus the constraint equations are
223         //    p*w1 - p*w2 = 0
224         //    q*w1 - q*w2 = 0
225         // where p and q are unit vectors normal to the slider axis, and w1 and w2
226         // 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         info->m_J1angularAxis[0] = p[0];
234         info->m_J1angularAxis[1] = p[1];
235         info->m_J1angularAxis[2] = p[2];
236         info->m_J1angularAxis[s+0] = q[0];
237         info->m_J1angularAxis[s+1] = q[1];
238         info->m_J1angularAxis[s+2] = q[2];
239
240         info->m_J2angularAxis[0] = -p[0];
241         info->m_J2angularAxis[1] = -p[1];
242         info->m_J2angularAxis[2] = -p[2];
243         info->m_J2angularAxis[s+0] = -q[0];
244         info->m_J2angularAxis[s+1] = -q[1];
245         info->m_J2angularAxis[s+2] = -q[2];
246         // compute the right hand side of the constraint equation. set relative
247         // 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).
250         // if "theta" is the angle between ax1 and ax2, we need an angular velocity
251         // along u to cover angle erp*theta in one step :
252         //   |angular_velocity| = angle/time = erp*theta / stepsize
253         //                      = (erp*fps) * theta
254         //    angular_velocity  = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
255         //                      = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
256         // ...as ax1 and ax2 are unit length. if theta is smallish,
257         // theta ~= sin(theta), so
258         //    angular_velocity  = (erp*fps) * (ax1 x ax2)
259         // ax1 x ax2 is in the plane space of ax1, so we project the angular
260         // 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);
264         info->m_constraintError[0] = k * u.dot(p);
265         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];
298
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
309         int srow;
310         // check linear limits linear
311         btScalar limit_err = btScalar(0.0);
312         int limit = 0;
313         if(getSolveLinLimit())
314         {
315                 limit_err = getLinDepth() *  signFact;
316                 limit = (limit_err > btScalar(0.0)) ? 2 : 1;
317         }
318         int powered = 0;
319         if(getPoweredLinMotor())
320         {
321                 powered = 1;
322         }
323         // if the slider has joint limits or motor, add in the extra row
324         if (limit || powered) 
325         {
326                 nrow++;
327                 srow = nrow * info->rowskip;
328                 info->m_J1linearAxis[srow+0] = ax1[0];
329                 info->m_J1linearAxis[srow+1] = ax1[1];
330                 info->m_J1linearAxis[srow+2] = ax1[2];
331                 // linear torque decoupling step:
332                 //
333                 // we have to be careful that the linear constraint forces (+/- ax1) applied to the two bodies
334                 // do not create a torque couple. in other words, the points that the
335                 // constraint force is applied at must lie along the same ax1 axis.
336                 // a torque couple will result in limited slider-jointed free
337                 // 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];
348                 // right-hand part
349                 btScalar lostop = getLowerLinLimit();
350                 btScalar histop = getUpperLinLimit();
351                 if(limit && (lostop == histop))
352                 {  // the joint motor is ineffective
353                         powered = 0;
354                 }
355                 info->m_constraintError[srow] = 0.;
356                 info->m_lowerLimit[srow] = 0.;
357                 info->m_upperLimit[srow] = 0.;
358                 if(powered)
359                 {
360             info->cfm[nrow] = btScalar(0.0); 
361                         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();
364                         info->m_constraintError[srow] -= signFact * mot_fact * getTargetLinMotorVelocity();
365                         info->m_lowerLimit[srow] += -getMaxLinMotorForce() * info->fps;
366                         info->m_upperLimit[srow] += getMaxLinMotorForce() * info->fps;
367                 }
368                 if(limit)
369                 {
370                         k = info->fps * info->erp;
371                         info->m_constraintError[srow] += k * limit_err;
372                         info->cfm[srow] = btScalar(0.0); // stop_cfm;
373                         if(lostop == histop) 
374                         {       // limited low and high simultaneously
375                                 info->m_lowerLimit[srow] = -SIMD_INFINITY;
376                                 info->m_upperLimit[srow] = SIMD_INFINITY;
377                         }
378                         else if(limit == 1) 
379                         { // low limit
380                                 info->m_lowerLimit[srow] = -SIMD_INFINITY;
381                                 info->m_upperLimit[srow] = 0;
382                         }
383                         else 
384                         { // high limit
385                                 info->m_lowerLimit[srow] = 0;
386                                 info->m_upperLimit[srow] = SIMD_INFINITY;
387                         }
388                         // bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin) for that)
389                         btScalar bounce = btFabs(btScalar(1.0) - getDampingLimLin());
390                         if(bounce > btScalar(0.0))
391                         {
392                                 btScalar vel = m_rbA.getLinearVelocity().dot(ax1);
393                                 vel -= m_rbB.getLinearVelocity().dot(ax1);
394                                 vel *= signFact;
395                                 // only apply bounce if the velocity is incoming, and if the
396                                 // resulting c[] exceeds what we already have.
397                                 if(limit == 1)
398                                 {       // low limit
399                                         if(vel < 0)
400                                         {
401                                                 btScalar newc = -bounce * vel;
402                                                 if (newc > info->m_constraintError[srow])
403                                                 {
404                                                         info->m_constraintError[srow] = newc;
405                                                 }
406                                         }
407                                 }
408                                 else
409                                 { // high limit - all those computations are reversed
410                                         if(vel > 0)
411                                         {
412                                                 btScalar newc = -bounce * vel;
413                                                 if(newc < info->m_constraintError[srow]) 
414                                                 {
415                                                         info->m_constraintError[srow] = newc;
416                                                 }
417                                         }
418                                 }
419                         }
420                         info->m_constraintError[srow] *= getSoftnessLimLin();
421                 } // if(limit)
422         } // if linear limit
423         // check angular limits
424         limit_err = btScalar(0.0);
425         limit = 0;
426         if(getSolveAngLimit())
427         {
428                 limit_err = getAngDepth();
429                 limit = (limit_err > btScalar(0.0)) ? 1 : 2;
430         }
431         // if the slider has joint limits, add in the extra row
432         powered = 0;
433         if(getPoweredAngMotor())
434         {
435                 powered = 1;
436         }
437         if(limit || powered) 
438         {
439                 nrow++;
440                 srow = nrow * info->rowskip;
441                 info->m_J1angularAxis[srow+0] = ax1[0];
442                 info->m_J1angularAxis[srow+1] = ax1[1];
443                 info->m_J1angularAxis[srow+2] = ax1[2];
444
445                 info->m_J2angularAxis[srow+0] = -ax1[0];
446                 info->m_J2angularAxis[srow+1] = -ax1[1];
447                 info->m_J2angularAxis[srow+2] = -ax1[2];
448
449                 btScalar lostop = getLowerAngLimit();
450                 btScalar histop = getUpperAngLimit();
451                 if(limit && (lostop == histop))
452                 {  // the joint motor is ineffective
453                         powered = 0;
454                 }
455                 if(powered)
456                 {
457             info->cfm[srow] = btScalar(0.0); 
458                         btScalar mot_fact = getMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, getTargetAngMotorVelocity(), info->fps * info->erp);
459                         info->m_constraintError[srow] = mot_fact * getTargetAngMotorVelocity();
460                         info->m_lowerLimit[srow] = -getMaxAngMotorForce() * info->fps;
461                         info->m_upperLimit[srow] = getMaxAngMotorForce() * info->fps;
462                 }
463                 if(limit)
464                 {
465                         k = info->fps * info->erp;
466                         info->m_constraintError[srow] += k * limit_err;
467                         info->cfm[srow] = btScalar(0.0); // stop_cfm;
468                         if(lostop == histop) 
469                         {
470                                 // limited low and high simultaneously
471                                 info->m_lowerLimit[srow] = -SIMD_INFINITY;
472                                 info->m_upperLimit[srow] = SIMD_INFINITY;
473                         }
474                         else if(limit == 1) 
475                         { // low limit
476                                 info->m_lowerLimit[srow] = 0;
477                                 info->m_upperLimit[srow] = SIMD_INFINITY;
478                         }
479                         else 
480                         { // high limit
481                                 info->m_lowerLimit[srow] = -SIMD_INFINITY;
482                                 info->m_upperLimit[srow] = 0;
483                         }
484                         // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
485                         btScalar bounce = btFabs(btScalar(1.0) - getDampingLimAng());
486                         if(bounce > btScalar(0.0))
487                         {
488                                 btScalar vel = m_rbA.getAngularVelocity().dot(ax1);
489                                 vel -= m_rbB.getAngularVelocity().dot(ax1);
490                                 // only apply bounce if the velocity is incoming, and if the
491                                 // resulting c[] exceeds what we already have.
492                                 if(limit == 1)
493                                 {       // low limit
494                                         if(vel < 0)
495                                         {
496                                                 btScalar newc = -bounce * vel;
497                                                 if(newc > info->m_constraintError[srow])
498                                                 {
499                                                         info->m_constraintError[srow] = newc;
500                                                 }
501                                         }
502                                 }
503                                 else
504                                 {       // high limit - all those computations are reversed
505                                         if(vel > 0)
506                                         {
507                                                 btScalar newc = -bounce * vel;
508                                                 if(newc < info->m_constraintError[srow])
509                                                 {
510                                                         info->m_constraintError[srow] = newc;
511                                                 }
512                                         }
513                                 }
514                         }
515                         info->m_constraintError[srow] *= getSoftnessLimAng();
516                 } // if(limit)
517         } // if angular limit or powered
518 }
519
520
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 }