Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

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

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

Added Bullet physics engine.
  • Property svn:eol-style set to native
File size: 9.3 KB
Line 
1/*
2Bullet Continuous Collision Detection and Physics Library
3btConeTwistConstraint is Copyright (c) 2007 Starbreeze Studios
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
15Written by: Marcus Hennix
16*/
17
18
19#include "btConeTwistConstraint.h"
20#include "BulletDynamics/Dynamics/btRigidBody.h"
21#include "LinearMath/btTransformUtil.h"
22#include "LinearMath/btMinMax.h"
23#include <new>
24
25btConeTwistConstraint::btConeTwistConstraint()
26:btTypedConstraint(CONETWIST_CONSTRAINT_TYPE)
27{
28}
29
30
31btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB, 
32                                                                                         const btTransform& rbAFrame,const btTransform& rbBFrame)
33                                                                                         :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
34                                                                                         m_angularOnly(false)
35{
36        m_swingSpan1 = btScalar(1e30);
37        m_swingSpan2 = btScalar(1e30);
38        m_twistSpan  = btScalar(1e30);
39        m_biasFactor = 0.3f;
40        m_relaxationFactor = 1.0f;
41
42        m_solveTwistLimit = false;
43        m_solveSwingLimit = false;
44
45}
46
47btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame)
48                                                                                        :btTypedConstraint(CONETWIST_CONSTRAINT_TYPE,rbA),m_rbAFrame(rbAFrame),
49                                                                                         m_angularOnly(false)
50{
51        m_rbBFrame = m_rbAFrame;
52       
53        m_swingSpan1 = btScalar(1e30);
54        m_swingSpan2 = btScalar(1e30);
55        m_twistSpan  = btScalar(1e30);
56        m_biasFactor = 0.3f;
57        m_relaxationFactor = 1.0f;
58
59        m_solveTwistLimit = false;
60        m_solveSwingLimit = false;
61       
62}                       
63
64void    btConeTwistConstraint::buildJacobian()
65{
66        m_appliedImpulse = btScalar(0.);
67
68        //set bias, sign, clear accumulator
69        m_swingCorrection = btScalar(0.);
70        m_twistLimitSign = btScalar(0.);
71        m_solveTwistLimit = false;
72        m_solveSwingLimit = false;
73        m_accTwistLimitImpulse = btScalar(0.);
74        m_accSwingLimitImpulse = btScalar(0.);
75
76        if (!m_angularOnly)
77        {
78                btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
79                btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
80                btVector3 relPos = pivotBInW - pivotAInW;
81
82                btVector3 normal[3];
83                if (relPos.length2() > SIMD_EPSILON)
84                {
85                        normal[0] = relPos.normalized();
86                }
87                else
88                {
89                        normal[0].setValue(btScalar(1.0),0,0);
90                }
91
92                btPlaneSpace1(normal[0], normal[1], normal[2]);
93
94                for (int i=0;i<3;i++)
95                {
96                        new (&m_jac[i]) btJacobianEntry(
97                                m_rbA.getCenterOfMassTransform().getBasis().transpose(),
98                                m_rbB.getCenterOfMassTransform().getBasis().transpose(),
99                                pivotAInW - m_rbA.getCenterOfMassPosition(),
100                                pivotBInW - m_rbB.getCenterOfMassPosition(),
101                                normal[i],
102                                m_rbA.getInvInertiaDiagLocal(),
103                                m_rbA.getInvMass(),
104                                m_rbB.getInvInertiaDiagLocal(),
105                                m_rbB.getInvMass());
106                }
107        }
108
109        btVector3 b1Axis1,b1Axis2,b1Axis3;
110        btVector3 b2Axis1,b2Axis2;
111
112        b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0);
113        b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0);
114
115        btScalar swing1=btScalar(0.),swing2 = btScalar(0.);
116
117        btScalar swx=btScalar(0.),swy = btScalar(0.);
118        btScalar thresh = btScalar(10.);
119        btScalar fact;
120
121        // Get Frame into world space
122        if (m_swingSpan1 >= btScalar(0.05f))
123        {
124                b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1);
125//              swing1  = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) );
126                swx = b2Axis1.dot(b1Axis1);
127                swy = b2Axis1.dot(b1Axis2);
128                swing1  = btAtan2Fast(swy, swx);
129                fact = (swy*swy + swx*swx) * thresh * thresh;
130                fact = fact / (fact + btScalar(1.0));
131                swing1 *= fact; 
132
133        }
134
135        if (m_swingSpan2 >= btScalar(0.05f))
136        {
137                b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2);                     
138//              swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) );
139                swx = b2Axis1.dot(b1Axis1);
140                swy = b2Axis1.dot(b1Axis3);
141                swing2  = btAtan2Fast(swy, swx);
142                fact = (swy*swy + swx*swx) * thresh * thresh;
143                fact = fact / (fact + btScalar(1.0));
144                swing2 *= fact; 
145        }
146
147        btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1);             
148        btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2);     
149        btScalar EllipseAngle = btFabs(swing1*swing1)* RMaxAngle1Sq + btFabs(swing2*swing2) * RMaxAngle2Sq;
150
151        if (EllipseAngle > 1.0f)
152        {
153                m_swingCorrection = EllipseAngle-1.0f;
154                m_solveSwingLimit = true;
155               
156                // Calculate necessary axis & factors
157                m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3));
158                m_swingAxis.normalize();
159
160                btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
161                m_swingAxis *= swingAxisSign;
162
163                m_kSwing =  btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_swingAxis) +
164                        getRigidBodyB().computeAngularImpulseDenominator(m_swingAxis));
165
166        }
167
168        // Twist limits
169        if (m_twistSpan >= btScalar(0.))
170        {
171                btVector3 b2Axis2 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(1);
172                btQuaternion rotationArc = shortestArcQuat(b2Axis1,b1Axis1);
173                btVector3 TwistRef = quatRotate(rotationArc,b2Axis2); 
174                btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) );
175
176                btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.);
177                if (twist <= -m_twistSpan*lockedFreeFactor)
178                {
179                        m_twistCorrection = -(twist + m_twistSpan);
180                        m_solveTwistLimit = true;
181
182                        m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
183                        m_twistAxis.normalize();
184                        m_twistAxis *= -1.0f;
185
186                        m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
187                                getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
188
189                }       else
190                        if (twist >  m_twistSpan*lockedFreeFactor)
191                        {
192                                m_twistCorrection = (twist - m_twistSpan);
193                                m_solveTwistLimit = true;
194
195                                m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
196                                m_twistAxis.normalize();
197
198                                m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
199                                        getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
200
201                        }
202        }
203}
204
205void    btConeTwistConstraint::solveConstraint(btScalar timeStep)
206{
207
208        btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
209        btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
210
211        btScalar tau = btScalar(0.3);
212
213        //linear part
214        if (!m_angularOnly)
215        {
216                btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
217                btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
218
219                btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
220                btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
221                btVector3 vel = vel1 - vel2;
222
223                for (int i=0;i<3;i++)
224                {               
225                        const btVector3& normal = m_jac[i].m_linearJointAxis;
226                        btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();
227
228                        btScalar rel_vel;
229                        rel_vel = normal.dot(vel);
230                        //positional error (zeroth order error)
231                        btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
232                        btScalar impulse = depth*tau/timeStep  * jacDiagABInv -  rel_vel * jacDiagABInv;
233                        m_appliedImpulse += impulse;
234                        btVector3 impulse_vector = normal * impulse;
235                        m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
236                        m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
237                }
238        }
239       
240        {
241                ///solve angular part
242                const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
243                const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
244
245                // solve swing limit
246                if (m_solveSwingLimit)
247                {
248                        btScalar amplitude = ((angVelB - angVelA).dot( m_swingAxis )*m_relaxationFactor*m_relaxationFactor + m_swingCorrection*(btScalar(1.)/timeStep)*m_biasFactor);
249                        btScalar impulseMag = amplitude * m_kSwing;
250
251                        // Clamp the accumulated impulse
252                        btScalar temp = m_accSwingLimitImpulse;
253                        m_accSwingLimitImpulse = btMax(m_accSwingLimitImpulse + impulseMag, btScalar(0.0) );
254                        impulseMag = m_accSwingLimitImpulse - temp;
255
256                        btVector3 impulse = m_swingAxis * impulseMag;
257
258                        m_rbA.applyTorqueImpulse(impulse);
259                        m_rbB.applyTorqueImpulse(-impulse);
260
261                }
262
263                // solve twist limit
264                if (m_solveTwistLimit)
265                {
266                        btScalar amplitude = ((angVelB - angVelA).dot( m_twistAxis )*m_relaxationFactor*m_relaxationFactor + m_twistCorrection*(btScalar(1.)/timeStep)*m_biasFactor );
267                        btScalar impulseMag = amplitude * m_kTwist;
268
269                        // Clamp the accumulated impulse
270                        btScalar temp = m_accTwistLimitImpulse;
271                        m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) );
272                        impulseMag = m_accTwistLimitImpulse - temp;
273
274                        btVector3 impulse = m_twistAxis * impulseMag;
275
276                        m_rbA.applyTorqueImpulse(impulse);
277                        m_rbB.applyTorqueImpulse(-impulse);
278
279                }
280       
281        }
282
283}
284
285void    btConeTwistConstraint::updateRHS(btScalar       timeStep)
286{
287        (void)timeStep;
288
289}
Note: See TracBrowser for help on using the repository browser.