Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/map/src/bullet/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp @ 3090

Last change on this file since 3090 was 2662, checked in by rgrieder, 16 years ago

Merged presentation branch back to trunk.
  • Property svn:eol-style set to native
File size: 13.9 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
23
24btHingeConstraint::btHingeConstraint()
25: btTypedConstraint (HINGE_CONSTRAINT_TYPE),
26m_enableAngularMotor(false)
27{
28}
29
30btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
31                                                                         btVector3& axisInA,btVector3& axisInB)
32                                                                         :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
33                                                                         m_angularOnly(false),
34                                                                         m_enableAngularMotor(false)
35{
36        m_rbAFrame.getOrigin() = pivotInA;
37       
38        // since no frame is given, assume this to be zero angle and just pick rb transform axis
39        btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0);
40
41        btVector3 rbAxisA2;
42        btScalar projection = axisInA.dot(rbAxisA1);
43        if (projection >= 1.0f - SIMD_EPSILON) {
44                rbAxisA1 = -rbA.getCenterOfMassTransform().getBasis().getColumn(2);
45                rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
46        } else if (projection <= -1.0f + SIMD_EPSILON) {
47                rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(2);
48                rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);     
49        } else {
50                rbAxisA2 = axisInA.cross(rbAxisA1);
51                rbAxisA1 = rbAxisA2.cross(axisInA);
52        }
53
54        m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
55                                                                        rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
56                                                                        rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
57
58        btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
59        btVector3 rbAxisB1 =  quatRotate(rotationArc,rbAxisA1);
60        btVector3 rbAxisB2 =  axisInB.cross(rbAxisB1); 
61       
62        m_rbBFrame.getOrigin() = pivotInB;
63        m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),-axisInB.getX(),
64                                                                        rbAxisB1.getY(),rbAxisB2.getY(),-axisInB.getY(),
65                                                                        rbAxisB1.getZ(),rbAxisB2.getZ(),-axisInB.getZ() );
66       
67        //start with free
68        m_lowerLimit = btScalar(1e30);
69        m_upperLimit = btScalar(-1e30);
70        m_biasFactor = 0.3f;
71        m_relaxationFactor = 1.0f;
72        m_limitSoftness = 0.9f;
73        m_solveLimit = false;
74
75}
76
77
78
79btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA)
80:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false)
81{
82
83        // since no frame is given, assume this to be zero angle and just pick rb transform axis
84        // fixed axis in worldspace
85        btVector3 rbAxisA1, rbAxisA2;
86        btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2);
87
88        m_rbAFrame.getOrigin() = pivotInA;
89        m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
90                                                                        rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
91                                                                        rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
92
93        btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * -axisInA;
94
95        btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
96        btVector3 rbAxisB1 =  quatRotate(rotationArc,rbAxisA1);
97        btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
98
99
100        m_rbBFrame.getOrigin() = rbA.getCenterOfMassTransform()(pivotInA);
101        m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
102                                                                        rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
103                                                                        rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
104       
105        //start with free
106        m_lowerLimit = btScalar(1e30);
107        m_upperLimit = btScalar(-1e30);
108        m_biasFactor = 0.3f;
109        m_relaxationFactor = 1.0f;
110        m_limitSoftness = 0.9f;
111        m_solveLimit = false;
112}
113
114btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, 
115                                                                     const btTransform& rbAFrame, const btTransform& rbBFrame)
116:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
117m_angularOnly(false),
118m_enableAngularMotor(false)
119{
120        // flip axis
121        m_rbBFrame.getBasis()[0][2] *= btScalar(-1.);
122        m_rbBFrame.getBasis()[1][2] *= btScalar(-1.);
123        m_rbBFrame.getBasis()[2][2] *= btScalar(-1.);
124
125        //start with free
126        m_lowerLimit = btScalar(1e30);
127        m_upperLimit = btScalar(-1e30);
128        m_biasFactor = 0.3f;
129        m_relaxationFactor = 1.0f;
130        m_limitSoftness = 0.9f;
131        m_solveLimit = false;
132}                       
133
134
135
136btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame)
137:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame),
138m_angularOnly(false),
139m_enableAngularMotor(false)
140{
141        ///not providing rigidbody B means implicitly using worldspace for body B
142
143        // flip axis
144        m_rbBFrame.getBasis()[0][2] *= btScalar(-1.);
145        m_rbBFrame.getBasis()[1][2] *= btScalar(-1.);
146        m_rbBFrame.getBasis()[2][2] *= btScalar(-1.);
147
148        m_rbBFrame.getOrigin() = m_rbA.getCenterOfMassTransform()(m_rbAFrame.getOrigin());
149
150        //start with free
151        m_lowerLimit = btScalar(1e30);
152        m_upperLimit = btScalar(-1e30); 
153        m_biasFactor = 0.3f;
154        m_relaxationFactor = 1.0f;
155        m_limitSoftness = 0.9f;
156        m_solveLimit = false;
157}
158
159void    btHingeConstraint::buildJacobian()
160{
161        m_appliedImpulse = btScalar(0.);
162
163        if (!m_angularOnly)
164        {
165                btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
166                btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
167                btVector3 relPos = pivotBInW - pivotAInW;
168
169                btVector3 normal[3];
170                if (relPos.length2() > SIMD_EPSILON)
171                {
172                        normal[0] = relPos.normalized();
173                }
174                else
175                {
176                        normal[0].setValue(btScalar(1.0),0,0);
177                }
178
179                btPlaneSpace1(normal[0], normal[1], normal[2]);
180
181                for (int i=0;i<3;i++)
182                {
183                        new (&m_jac[i]) btJacobianEntry(
184                                m_rbA.getCenterOfMassTransform().getBasis().transpose(),
185                                m_rbB.getCenterOfMassTransform().getBasis().transpose(),
186                                pivotAInW - m_rbA.getCenterOfMassPosition(),
187                                pivotBInW - m_rbB.getCenterOfMassPosition(),
188                                normal[i],
189                                m_rbA.getInvInertiaDiagLocal(),
190                                m_rbA.getInvMass(),
191                                m_rbB.getInvInertiaDiagLocal(),
192                                m_rbB.getInvMass());
193                }
194        }
195
196        //calculate two perpendicular jointAxis, orthogonal to hingeAxis
197        //these two jointAxis require equal angular velocities for both bodies
198
199        //this is unused for now, it's a todo
200        btVector3 jointAxis0local;
201        btVector3 jointAxis1local;
202       
203        btPlaneSpace1(m_rbAFrame.getBasis().getColumn(2),jointAxis0local,jointAxis1local);
204
205        getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
206        btVector3 jointAxis0 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis0local;
207        btVector3 jointAxis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * jointAxis1local;
208        btVector3 hingeAxisWorld = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(2);
209               
210        new (&m_jacAng[0])      btJacobianEntry(jointAxis0,
211                m_rbA.getCenterOfMassTransform().getBasis().transpose(),
212                m_rbB.getCenterOfMassTransform().getBasis().transpose(),
213                m_rbA.getInvInertiaDiagLocal(),
214                m_rbB.getInvInertiaDiagLocal());
215
216        new (&m_jacAng[1])      btJacobianEntry(jointAxis1,
217                m_rbA.getCenterOfMassTransform().getBasis().transpose(),
218                m_rbB.getCenterOfMassTransform().getBasis().transpose(),
219                m_rbA.getInvInertiaDiagLocal(),
220                m_rbB.getInvInertiaDiagLocal());
221
222        new (&m_jacAng[2])      btJacobianEntry(hingeAxisWorld,
223                m_rbA.getCenterOfMassTransform().getBasis().transpose(),
224                m_rbB.getCenterOfMassTransform().getBasis().transpose(),
225                m_rbA.getInvInertiaDiagLocal(),
226                m_rbB.getInvInertiaDiagLocal());
227
228
229        // Compute limit information
230        btScalar hingeAngle = getHingeAngle(); 
231
232        //set bias, sign, clear accumulator
233        m_correction = btScalar(0.);
234        m_limitSign = btScalar(0.);
235        m_solveLimit = false;
236        m_accLimitImpulse = btScalar(0.);
237
238//      if (m_lowerLimit < m_upperLimit)
239        if (m_lowerLimit <= m_upperLimit)
240        {
241//              if (hingeAngle <= m_lowerLimit*m_limitSoftness)
242                if (hingeAngle <= m_lowerLimit)
243                {
244                        m_correction = (m_lowerLimit - hingeAngle);
245                        m_limitSign = 1.0f;
246                        m_solveLimit = true;
247                } 
248//              else if (hingeAngle >= m_upperLimit*m_limitSoftness)
249                else if (hingeAngle >= m_upperLimit)
250                {
251                        m_correction = m_upperLimit - hingeAngle;
252                        m_limitSign = -1.0f;
253                        m_solveLimit = true;
254                }
255        }
256
257        //Compute K = J*W*J' for hinge axis
258        btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
259        m_kHinge =   1.0f / (getRigidBodyA().computeAngularImpulseDenominator(axisA) +
260                                     getRigidBodyB().computeAngularImpulseDenominator(axisA));
261
262}
263
264void    btHingeConstraint::solveConstraint(btScalar     timeStep)
265{
266
267        btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
268        btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
269
270        btScalar tau = btScalar(0.3);
271
272        //linear part
273        if (!m_angularOnly)
274        {
275                btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
276                btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
277
278                btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
279                btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
280                btVector3 vel = vel1 - vel2;
281
282                for (int i=0;i<3;i++)
283                {               
284                        const btVector3& normal = m_jac[i].m_linearJointAxis;
285                        btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
286
287                        btScalar rel_vel;
288                        rel_vel = normal.dot(vel);
289                        //positional error (zeroth order error)
290                        btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
291                        btScalar impulse = depth*tau/timeStep  * jacDiagABInv -  rel_vel * jacDiagABInv;
292                        m_appliedImpulse += impulse;
293                        btVector3 impulse_vector = normal * impulse;
294                        m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
295                        m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
296                }
297        }
298
299       
300        {
301                ///solve angular part
302
303                // get axes in world space
304                btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
305                btVector3 axisB =  getRigidBodyB().getCenterOfMassTransform().getBasis() *  m_rbBFrame.getBasis().getColumn(2);
306
307                const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
308                const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
309
310                btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA);
311                btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB);
312
313                btVector3 angAorthog = angVelA - angVelAroundHingeAxisA;
314                btVector3 angBorthog = angVelB - angVelAroundHingeAxisB;
315                btVector3 velrelOrthog = angAorthog-angBorthog;
316                {
317                        //solve orthogonal angular velocity correction
318                        btScalar relaxation = btScalar(1.);
319                        btScalar len = velrelOrthog.length();
320                        if (len > btScalar(0.00001))
321                        {
322                                btVector3 normal = velrelOrthog.normalized();
323                                btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) +
324                                        getRigidBodyB().computeAngularImpulseDenominator(normal);
325                                // scale for mass and relaxation
326                                velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor;
327                        }
328
329                        //solve angular positional correction
330                        btVector3 angularError = -axisA.cross(axisB) *(btScalar(1.)/timeStep);
331                        btScalar len2 = angularError.length();
332                        if (len2>btScalar(0.00001))
333                        {
334                                btVector3 normal2 = angularError.normalized();
335                                btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) +
336                                                getRigidBodyB().computeAngularImpulseDenominator(normal2);
337                                angularError *= (btScalar(1.)/denom2) * relaxation;
338                        }
339
340                        m_rbA.applyTorqueImpulse(-velrelOrthog+angularError);
341                        m_rbB.applyTorqueImpulse(velrelOrthog-angularError);
342
343                        // solve limit
344                        if (m_solveLimit)
345                        {
346                                btScalar amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (btScalar(1.)/timeStep)*m_biasFactor  ) * m_limitSign;
347
348                                btScalar impulseMag = amplitude * m_kHinge;
349
350                                // Clamp the accumulated impulse
351                                btScalar temp = m_accLimitImpulse;
352                                m_accLimitImpulse = btMax(m_accLimitImpulse + impulseMag, btScalar(0) );
353                                impulseMag = m_accLimitImpulse - temp;
354
355
356                                btVector3 impulse = axisA * impulseMag * m_limitSign;
357                                m_rbA.applyTorqueImpulse(impulse);
358                                m_rbB.applyTorqueImpulse(-impulse);
359                        }
360                }
361
362                //apply motor
363                if (m_enableAngularMotor) 
364                {
365                        //todo: add limits too
366                        btVector3 angularLimit(0,0,0);
367
368                        btVector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB;
369                        btScalar projRelVel = velrel.dot(axisA);
370
371                        btScalar desiredMotorVel = m_motorTargetVelocity;
372                        btScalar motor_relvel = desiredMotorVel - projRelVel;
373
374                        btScalar unclippedMotorImpulse = m_kHinge * motor_relvel;;
375                        //todo: should clip against accumulated impulse
376                        btScalar clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse;
377                        clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse;
378                        btVector3 motorImp = clippedMotorImpulse * axisA;
379
380                        m_rbA.applyTorqueImpulse(motorImp+angularLimit);
381                        m_rbB.applyTorqueImpulse(-motorImp-angularLimit);
382                       
383                }
384        }
385
386}
387
388void    btHingeConstraint::updateRHS(btScalar   timeStep)
389{
390        (void)timeStep;
391
392}
393
394btScalar btHingeConstraint::getHingeAngle()
395{
396        const btVector3 refAxis0  = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(0);
397        const btVector3 refAxis1  = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_rbAFrame.getBasis().getColumn(1);
398        const btVector3 swingAxis = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_rbBFrame.getBasis().getColumn(1);
399
400        return btAtan2Fast( swingAxis.dot(refAxis0), swingAxis.dot(refAxis1)  );
401}
402
Note: See TracBrowser for help on using the repository browser.