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