Upgrade to latest Bullet trunk, that is in sync with Blender/extern/bullet2. (except...
[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         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
47 btConeTwistConstraint::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
64 void    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
205 void    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
285 void    btConeTwistConstraint::updateRHS(btScalar       timeStep)
286 {
287         (void)timeStep;
288
289 }