Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/gamestate/src/external/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp @ 6815

Last change on this file since 6815 was 5781, checked in by rgrieder, 15 years ago

Reverted trunk again. We might want to find a way to delete these revisions again (x3n's changes are still available as diff in the commit mails).

  • Property svn:eol-style set to native
File size: 23.7 KB
Line 
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. This notice may not be removed or altered from any source distribution.
14*/
15
16
17#include "btHingeConstraint.h"
18#include "BulletDynamics/Dynamics/btRigidBody.h"
19#include "LinearMath/btTransformUtil.h"
20#include "LinearMath/btMinMax.h"
21#include <new>
22#include "btSolverBody.h"
23
24//-----------------------------------------------------------------------------
25
26#define HINGE_USE_OBSOLETE_SOLVER false
27
28//-----------------------------------------------------------------------------
29
30
31btHingeConstraint::btHingeConstraint()
32: btTypedConstraint (HINGE_CONSTRAINT_TYPE),
33m_enableAngularMotor(false),
34m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
35m_useReferenceFrameA(false)
36{
37        m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
38}
39
40//-----------------------------------------------------------------------------
41
42btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
43                                                                         btVector3& axisInA,btVector3& axisInB, bool useReferenceFrameA)
44                                                                         :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
45                                                                         m_angularOnly(false),
46                                                                         m_enableAngularMotor(false),
47                                                                         m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
48                                                                         m_useReferenceFrameA(useReferenceFrameA)
49{
50        m_rbAFrame.getOrigin() = pivotInA;
51       
52        // since no frame is given, assume this to be zero angle and just pick rb transform axis
53        btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0);
54
55        btVector3 rbAxisA2;
56        btScalar projection = axisInA.dot(rbAxisA1);
57        if (projection >= 1.0f - SIMD_EPSILON) {
58                rbAxisA1 = -rbA.getCenterOfMassTransform().getBasis().getColumn(2);
59                rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
60        } else if (projection <= -1.0f + SIMD_EPSILON) {
61                rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(2);
62                rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);     
63        } else {
64                rbAxisA2 = axisInA.cross(rbAxisA1);
65                rbAxisA1 = rbAxisA2.cross(axisInA);
66        }
67
68        m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
69                                                                        rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
70                                                                        rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
71
72        btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
73        btVector3 rbAxisB1 =  quatRotate(rotationArc,rbAxisA1);
74        btVector3 rbAxisB2 =  axisInB.cross(rbAxisB1); 
75       
76        m_rbBFrame.getOrigin() = pivotInB;
77        m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
78                                                                        rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
79                                                                        rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
80       
81        //start with free
82        m_lowerLimit = btScalar(1e30);
83        m_upperLimit = btScalar(-1e30);
84        m_biasFactor = 0.3f;
85        m_relaxationFactor = 1.0f;
86        m_limitSoftness = 0.9f;
87        m_solveLimit = false;
88        m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
89}
90
91//-----------------------------------------------------------------------------
92
93btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA, bool useReferenceFrameA)
94:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false), 
95m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
96m_useReferenceFrameA(useReferenceFrameA)
97{
98
99        // since no frame is given, assume this to be zero angle and just pick rb transform axis
100        // fixed axis in worldspace
101        btVector3 rbAxisA1, rbAxisA2;
102        btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2);
103
104        m_rbAFrame.getOrigin() = pivotInA;
105        m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
106                                                                        rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
107                                                                        rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
108
109        btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * axisInA;
110
111        btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
112        btVector3 rbAxisB1 =  quatRotate(rotationArc,rbAxisA1);
113        btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
114
115
116        m_rbBFrame.getOrigin() = rbA.getCenterOfMassTransform()(pivotInA);
117        m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
118                                                                        rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
119                                                                        rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
120       
121        //start with free
122        m_lowerLimit = btScalar(1e30);
123        m_upperLimit = btScalar(-1e30);
124        m_biasFactor = 0.3f;
125        m_relaxationFactor = 1.0f;
126        m_limitSoftness = 0.9f;
127        m_solveLimit = false;
128        m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
129}
130
131//-----------------------------------------------------------------------------
132
133btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, 
134                                                                     const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA)
135:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
136m_angularOnly(false),
137m_enableAngularMotor(false),
138m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
139m_useReferenceFrameA(useReferenceFrameA)
140{
141        //start with free
142        m_lowerLimit = btScalar(1e30);
143        m_upperLimit = btScalar(-1e30);
144        m_biasFactor = 0.3f;
145        m_relaxationFactor = 1.0f;
146        m_limitSoftness = 0.9f;
147        m_solveLimit = false;
148        m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
149}                       
150
151//-----------------------------------------------------------------------------
152
153btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame, bool useReferenceFrameA)
154:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame),
155m_angularOnly(false),
156m_enableAngularMotor(false),
157m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
158m_useReferenceFrameA(useReferenceFrameA)
159{
160        ///not providing rigidbody B means implicitly using worldspace for body B
161
162        m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin());
163
164        //start with free
165        m_lowerLimit = btScalar(1e30);
166        m_upperLimit = btScalar(-1e30); 
167        m_biasFactor = 0.3f;
168        m_relaxationFactor = 1.0f;
169        m_limitSoftness = 0.9f;
170        m_solveLimit = false;
171        m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
172}
173
174//-----------------------------------------------------------------------------
175
176void    btHingeConstraint::buildJacobian()
177{
178        if (m_useSolveConstraintObsolete)
179        {
180                m_appliedImpulse = btScalar(0.);
181
182                if (!m_angularOnly)
183                {
184                        btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
185                        btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
186                        btVector3 relPos = pivotBInW - pivotAInW;
187
188                        btVector3 normal[3];
189                        if (relPos.length2() > SIMD_EPSILON)
190                        {
191                                normal[0] = relPos.normalized();
192                        }
193                        else
194                        {
195                                normal[0].setValue(btScalar(1.0),0,0);
196                        }
197
198                        btPlaneSpace1(normal[0], normal[1], normal[2]);
199
200                        for (int i=0;i<3;i++)
201                        {
202                                new (&m_jac[i]) btJacobianEntry(
203                                m_rbA.getCenterOfMassTransform().getBasis().transpose(),
204                                m_rbB.getCenterOfMassTransform().getBasis().transpose(),
205                                pivotAInW - m_rbA.getCenterOfMassPosition(),
206                                pivotBInW - m_rbB.getCenterOfMassPosition(),
207                                normal[i],
208                                m_rbA.getInvInertiaDiagLocal(),
209                                m_rbA.getInvMass(),
210                                m_rbB.getInvInertiaDiagLocal(),
211                                m_rbB.getInvMass());
212                        }
213                }
214
215                //calculate two perpendicular jointAxis, orthogonal to hingeAxis
216                //these two jointAxis require equal angular velocities for both bodies
217
218                //this is unused for now, it's a todo
219                btVector3 jointAxis0local;
220                btVector3 jointAxis1local;
221               
222                btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local);
223
224                getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
225                btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local;
226                btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local;
227                btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
228                       
229                new (&m_jacAng[0])      btJacobianEntry(jointAxis0,
230                        m_rbA.getCenterOfMassTransform().getBasis().transpose(),
231                        m_rbB.getCenterOfMassTransform().getBasis().transpose(),
232                        m_rbA.getInvInertiaDiagLocal(),
233                        m_rbB.getInvInertiaDiagLocal());
234
235                new (&m_jacAng[1])      btJacobianEntry(jointAxis1,
236                        m_rbA.getCenterOfMassTransform().getBasis().transpose(),
237                        m_rbB.getCenterOfMassTransform().getBasis().transpose(),
238                        m_rbA.getInvInertiaDiagLocal(),
239                        m_rbB.getInvInertiaDiagLocal());
240
241                new (&m_jacAng[2])      btJacobianEntry(hingeAxisWorld,
242                        m_rbA.getCenterOfMassTransform().getBasis().transpose(),
243                        m_rbB.getCenterOfMassTransform().getBasis().transpose(),
244                        m_rbA.getInvInertiaDiagLocal(),
245                        m_rbB.getInvInertiaDiagLocal());
246
247                        // clear accumulator
248                        m_accLimitImpulse = btScalar(0.);
249
250                        // test angular limit
251                        testLimit();
252
253                //Compute K = J*W*J' for hinge axis
254                btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
255                m_kHinge =   1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) +
256                                                         getRigidBodyB().computeAngularImpulseDenominator(axisA));
257
258        }
259}
260
261//-----------------------------------------------------------------------------
262
263void btHingeConstraint::getInfo1(btConstraintInfo1* info)
264{
265        if (m_useSolveConstraintObsolete)
266        {
267                info->m_numConstraintRows = 0;
268                info->nub = 0;
269        }
270        else
271        {
272                info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular
273                info->nub = 1; 
274                //prepare constraint
275                testLimit();
276                if(getSolveLimit() || getEnableAngularMotor())
277                {
278                        info->m_numConstraintRows++; // limit 3rd anguar as well
279                        info->nub--; 
280                }
281        }
282} // btHingeConstraint::getInfo1 ()
283
284//-----------------------------------------------------------------------------
285
286void btHingeConstraint::getInfo2 (btConstraintInfo2* info)
287{
288        btAssert(!m_useSolveConstraintObsolete);
289        int i, s = info->rowskip;
290        // transforms in world space
291        btTransform trA = m_rbA.getCenterOfMassTransform()*m_rbAFrame;
292        btTransform trB = m_rbB.getCenterOfMassTransform()*m_rbBFrame;
293        // pivot point
294        btVector3 pivotAInW = trA.getOrigin();
295        btVector3 pivotBInW = trB.getOrigin();
296        // linear (all fixed)
297    info->m_J1linearAxis[0] = 1;
298    info->m_J1linearAxis[s + 1] = 1;
299    info->m_J1linearAxis[2 * s + 2] = 1;
300        btVector3 a1 = pivotAInW - m_rbA.getCenterOfMassTransform().getOrigin();
301        {
302                btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
303                btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + s);
304                btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * s);
305                btVector3 a1neg = -a1;
306                a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
307        }
308        btVector3 a2 = pivotBInW - m_rbB.getCenterOfMassTransform().getOrigin();
309        {
310                btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
311                btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + s);
312                btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * s);
313                a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
314        }
315        // linear RHS
316    btScalar k = info->fps * info->erp;
317        for(i = 0; i < 3; i++)
318    {
319        info->m_constraintError[i * s] = k * (pivotBInW[i] - pivotAInW[i]);
320    }
321        // make rotations around X and Y equal
322        // the hinge axis should be the only unconstrained
323        // rotational axis, the angular velocity of the two bodies perpendicular to
324        // the hinge axis should be equal. thus the constraint equations are
325        //    p*w1 - p*w2 = 0
326        //    q*w1 - q*w2 = 0
327        // where p and q are unit vectors normal to the hinge axis, and w1 and w2
328        // are the angular velocity vectors of the two bodies.
329        // get hinge axis (Z)
330        btVector3 ax1 = trA.getBasis().getColumn(2);
331        // get 2 orthos to hinge axis (X, Y)
332        btVector3 p = trA.getBasis().getColumn(0);
333        btVector3 q = trA.getBasis().getColumn(1);
334        // set the two hinge angular rows
335    int s3 = 3 * info->rowskip;
336    int s4 = 4 * info->rowskip;
337
338        info->m_J1angularAxis[s3 + 0] = p[0];
339        info->m_J1angularAxis[s3 + 1] = p[1];
340        info->m_J1angularAxis[s3 + 2] = p[2];
341        info->m_J1angularAxis[s4 + 0] = q[0];
342        info->m_J1angularAxis[s4 + 1] = q[1];
343        info->m_J1angularAxis[s4 + 2] = q[2];
344
345        info->m_J2angularAxis[s3 + 0] = -p[0];
346        info->m_J2angularAxis[s3 + 1] = -p[1];
347        info->m_J2angularAxis[s3 + 2] = -p[2];
348        info->m_J2angularAxis[s4 + 0] = -q[0];
349        info->m_J2angularAxis[s4 + 1] = -q[1];
350        info->m_J2angularAxis[s4 + 2] = -q[2];
351    // compute the right hand side of the constraint equation. set relative
352    // body velocities along p and q to bring the hinge back into alignment.
353    // if ax1,ax2 are the unit length hinge axes as computed from body1 and
354    // body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
355    // if `theta' is the angle between ax1 and ax2, we need an angular velocity
356    // along u to cover angle erp*theta in one step :
357    //   |angular_velocity| = angle/time = erp*theta / stepsize
358    //                      = (erp*fps) * theta
359    //    angular_velocity  = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
360    //                      = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
361    // ...as ax1 and ax2 are unit length. if theta is smallish,
362    // theta ~= sin(theta), so
363    //    angular_velocity  = (erp*fps) * (ax1 x ax2)
364    // ax1 x ax2 is in the plane space of ax1, so we project the angular
365    // velocity to p and q to find the right hand side.
366    btVector3 ax2 = trB.getBasis().getColumn(2);
367        btVector3 u = ax1.cross(ax2);
368        info->m_constraintError[s3] = k * u.dot(p);
369        info->m_constraintError[s4] = k * u.dot(q);
370        // check angular limits
371        int nrow = 4; // last filled row
372        int srow;
373        btScalar limit_err = btScalar(0.0);
374        int limit = 0;
375        if(getSolveLimit())
376        {
377                limit_err = m_correction * m_referenceSign;
378                limit = (limit_err > btScalar(0.0)) ? 1 : 2;
379        }
380        // if the hinge has joint limits or motor, add in the extra row
381        int powered = 0;
382        if(getEnableAngularMotor())
383        {
384                powered = 1;
385        }
386        if(limit || powered) 
387        {
388                nrow++;
389                srow = nrow * info->rowskip;
390                info->m_J1angularAxis[srow+0] = ax1[0];
391                info->m_J1angularAxis[srow+1] = ax1[1];
392                info->m_J1angularAxis[srow+2] = ax1[2];
393
394                info->m_J2angularAxis[srow+0] = -ax1[0];
395                info->m_J2angularAxis[srow+1] = -ax1[1];
396                info->m_J2angularAxis[srow+2] = -ax1[2];
397
398                btScalar lostop = getLowerLimit();
399                btScalar histop = getUpperLimit();
400                if(limit && (lostop == histop))
401                {  // the joint motor is ineffective
402                        powered = 0;
403                }
404                info->m_constraintError[srow] = btScalar(0.0f);
405                if(powered)
406                {
407            info->cfm[srow] = btScalar(0.0); 
408                        btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * info->erp);
409                        info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
410                        info->m_lowerLimit[srow] = - m_maxMotorImpulse;
411                        info->m_upperLimit[srow] =   m_maxMotorImpulse;
412                }
413                if(limit)
414                {
415                        k = info->fps * info->erp;
416                        info->m_constraintError[srow] += k * limit_err;
417                        info->cfm[srow] = btScalar(0.0);
418                        if(lostop == histop) 
419                        {
420                                // limited low and high simultaneously
421                                info->m_lowerLimit[srow] = -SIMD_INFINITY;
422                                info->m_upperLimit[srow] = SIMD_INFINITY;
423                        }
424                        else if(limit == 1) 
425                        { // low limit
426                                info->m_lowerLimit[srow] = 0;
427                                info->m_upperLimit[srow] = SIMD_INFINITY;
428                        }
429                        else 
430                        { // high limit
431                                info->m_lowerLimit[srow] = -SIMD_INFINITY;
432                                info->m_upperLimit[srow] = 0;
433                        }
434                        // bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
435                        btScalar bounce = m_relaxationFactor;
436                        if(bounce > btScalar(0.0))
437                        {
438                                btScalar vel = m_rbA.getAngularVelocity().dot(ax1);
439                                vel -= m_rbB.getAngularVelocity().dot(ax1);
440                                // only apply bounce if the velocity is incoming, and if the
441                                // resulting c[] exceeds what we already have.
442                                if(limit == 1)
443                                {       // low limit
444                                        if(vel < 0)
445                                        {
446                                                btScalar newc = -bounce * vel;
447                                                if(newc > info->m_constraintError[srow])
448                                                {
449                                                        info->m_constraintError[srow] = newc;
450                                                }
451                                        }
452                                }
453                                else
454                                {       // high limit - all those computations are reversed
455                                        if(vel > 0)
456                                        {
457                                                btScalar newc = -bounce * vel;
458                                                if(newc < info->m_constraintError[srow])
459                                                {
460                                                        info->m_constraintError[srow] = newc;
461                                                }
462                                        }
463                                }
464                        }
465                        info->m_constraintError[srow] *= m_biasFactor;
466                } // if(limit)
467        } // if angular limit or powered
468}
469
470//-----------------------------------------------------------------------------
471
472void    btHingeConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar     timeStep)
473{
474
475        ///for backwards compatibility during the transition to 'getInfo/getInfo2'
476        if (m_useSolveConstraintObsolete)
477        {
478
479                btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
480                btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
481
482                btScalar tau = btScalar(0.3);
483
484                //linear part
485                if (!m_angularOnly)
486                {
487                        btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
488                        btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
489
490                        btVector3 vel1,vel2;
491                        bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
492                        bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
493                        btVector3 vel = vel1 - vel2;
494
495                        for (int i=0;i<3;i++)
496                        {               
497                                const btVector3& normal = m_jac[i].m_linearJointAxis;
498                                btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
499
500                                btScalar rel_vel;
501                                rel_vel = normal.dot(vel);
502                                //positional error (zeroth order error)
503                                btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
504                                btScalar impulse = depth*tau/timeStep  * jacDiagABInv -  rel_vel * jacDiagABInv;
505                                m_appliedImpulse += impulse;
506                                btVector3 impulse_vector = normal * impulse;
507                                btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
508                                btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
509                                bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
510                                bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse);
511                        }
512                }
513
514               
515                {
516                        ///solve angular part
517
518                        // get axes in world space
519                        btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
520                        btVector3 axisB =  getRigidBodyB().getCenterOfMassTransform().getBasis() *  m_rbBFrame.getBasis().getColumn(2);
521
522                        btVector3 angVelA;
523                        bodyA.getAngularVelocity(angVelA);
524                        btVector3 angVelB;
525                        bodyB.getAngularVelocity(angVelB);
526
527                        btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA);
528                        btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB);
529
530                        btVector3 angAorthog = angVelA - angVelAroundHingeAxisA;
531                        btVector3 angBorthog = angVelB - angVelAroundHingeAxisB;
532                        btVector3 velrelOrthog = angAorthog-angBorthog;
533                        {
534                               
535
536                                //solve orthogonal angular velocity correction
537                                btScalar relaxation = btScalar(1.);
538                                btScalar len = velrelOrthog.length();
539                                if (len > btScalar(0.00001))
540                                {
541                                        btVector3 normal = velrelOrthog.normalized();
542                                        btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) +
543                                                getRigidBodyB().computeAngularImpulseDenominator(normal);
544                                        // scale for mass and relaxation
545                                        //velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor;
546
547                                        bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*velrelOrthog,-(btScalar(1.)/denom));
548                                        bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*velrelOrthog,(btScalar(1.)/denom));
549
550                                }
551
552                                //solve angular positional correction
553                                btVector3 angularError =  axisA.cross(axisB) *(btScalar(1.)/timeStep);
554                                btScalar len2 = angularError.length();
555                                if (len2>btScalar(0.00001))
556                                {
557                                        btVector3 normal2 = angularError.normalized();
558                                        btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) +
559                                                        getRigidBodyB().computeAngularImpulseDenominator(normal2);
560                                        //angularError *= (btScalar(1.)/denom2) * relaxation;
561                                       
562                                        bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*angularError,(btScalar(1.)/denom2));
563                                        bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*angularError,-(btScalar(1.)/denom2));
564
565                                }
566                               
567                               
568
569
570
571                                // solve limit
572                                if (m_solveLimit)
573                                {
574                                        btScalar amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (btScalar(1.)/timeStep)*m_biasFactor  ) * m_limitSign;
575
576                                        btScalar impulseMag = amplitude * m_kHinge;
577
578                                        // Clamp the accumulated impulse
579                                        btScalar temp = m_accLimitImpulse;
580                                        m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, btScalar(0) );
581                                        impulseMag = m_accLimitImpulse - temp;
582
583
584                                       
585                                        bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,impulseMag * m_limitSign);
586                                        bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-(impulseMag * m_limitSign));
587
588                                }
589                        }
590
591                        //apply motor
592                        if (m_enableAngularMotor) 
593                        {
594                                //todo: add limits too
595                                btVector3 angularLimit(0,0,0);
596
597                                btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB;
598                                btScalar projRelVel = velrel.dot(axisA);
599
600                                btScalar desiredMotorVel = m_motorTargetVelocity;
601                                btScalar motor_relvel = desiredMotorVel - projRelVel;
602
603                                btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;;
604                                //todo: should clip against accumulated impulse
605                                btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse;
606                                clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse;
607                                btVector3 motorImp = clippedMotorImpulse * axisA;
608                       
609                                bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*axisA,clippedMotorImpulse);
610                                bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*axisA,-clippedMotorImpulse);
611                               
612                        }
613                }
614        }
615
616}
617
618//-----------------------------------------------------------------------------
619
620void    btHingeConstraint::updateRHS(btScalar   timeStep)
621{
622        (void)timeStep;
623
624}
625
626//-----------------------------------------------------------------------------
627
628btScalar btHingeConstraint::getHingeAngle()
629{
630        const btVector3 refAxis0  = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0);
631        const btVector3 refAxis1  = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1);
632        const btVector3 swingAxis = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(1);
633        btScalar angle = btAtan2Fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
634        return m_referenceSign * angle;
635}
636
637//-----------------------------------------------------------------------------
638
639void btHingeConstraint::testLimit()
640{
641        // Compute limit information
642        m_hingeAngle = getHingeAngle(); 
643        m_correction = btScalar(0.);
644        m_limitSign = btScalar(0.);
645        m_solveLimit = false;
646        if (m_lowerLimit <= m_upperLimit)
647        {
648                if (m_hingeAngle <= m_lowerLimit)
649                {
650                        m_correction = (m_lowerLimit - m_hingeAngle);
651                        m_limitSign = 1.0f;
652                        m_solveLimit = true;
653                } 
654                else if (m_hingeAngle >= m_upperLimit)
655                {
656                        m_correction = m_upperLimit - m_hingeAngle;
657                        m_limitSign = -1.0f;
658                        m_solveLimit = true;
659                }
660        }
661        return;
662} // btHingeConstraint::testLimit()
663
664//-----------------------------------------------------------------------------
665//-----------------------------------------------------------------------------
666//-----------------------------------------------------------------------------
Note: See TracBrowser for help on using the repository browser.