Particles
[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/btSimdMinMax.h"
23 #include <new>
24
25 btConeTwistConstraint::btConeTwistConstraint()
26 {
27 }
28
29
30 btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB, 
31                                                                                          const btTransform& rbAFrame,const btTransform& rbBFrame)
32                                                                                          :btTypedConstraint(rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame),
33                                                                                          m_angularOnly(false)
34 {
35         // flip axis for correct angles
36         m_rbBFrame.getBasis()[1][0] *= btScalar(-1.);
37         m_rbBFrame.getBasis()[1][1] *= btScalar(-1.);
38         m_rbBFrame.getBasis()[1][2] *= btScalar(-1.);
39
40         m_swingSpan1 = btScalar(1e30);
41         m_swingSpan2 = btScalar(1e30);
42         m_twistSpan  = btScalar(1e30);
43         m_biasFactor = 0.3f;
44         m_relaxationFactor = 1.0f;
45
46         m_solveTwistLimit = false;
47         m_solveSwingLimit = false;
48
49 }
50
51 btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame)
52                                                                                         :btTypedConstraint(rbA),m_rbAFrame(rbAFrame),
53                                                                                          m_angularOnly(false)
54 {
55         m_rbBFrame = m_rbAFrame;
56         
57         // flip axis for correct angles
58         m_rbBFrame.getBasis()[1][0] *= btScalar(-1.);
59         m_rbBFrame.getBasis()[1][1] *= btScalar(-1.);
60         m_rbBFrame.getBasis()[1][2] *= btScalar(-1.);
61
62         m_rbBFrame.getBasis()[2][0] *= btScalar(-1.);
63         m_rbBFrame.getBasis()[2][1] *= btScalar(-1.);
64         m_rbBFrame.getBasis()[2][2] *= btScalar(-1.);
65         
66         m_swingSpan1 = btScalar(1e30);
67         m_swingSpan2 = btScalar(1e30);
68         m_twistSpan  = btScalar(1e30);
69         m_biasFactor = 0.3f;
70         m_relaxationFactor = 1.0f;
71
72         m_solveTwistLimit = false;
73         m_solveSwingLimit = false;
74         
75 }                       
76
77 void    btConeTwistConstraint::buildJacobian()
78 {
79         m_appliedImpulse = btScalar(0.);
80
81         //set bias, sign, clear accumulator
82         m_swingCorrection = btScalar(0.);
83         m_twistLimitSign = btScalar(0.);
84         m_solveTwistLimit = false;
85         m_solveSwingLimit = false;
86         m_accTwistLimitImpulse = btScalar(0.);
87         m_accSwingLimitImpulse = btScalar(0.);
88
89         if (!m_angularOnly)
90         {
91                 btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
92                 btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
93                 btVector3 relPos = pivotBInW - pivotAInW;
94
95                 btVector3 normal[3];
96                 if (relPos.length2() > SIMD_EPSILON)
97                 {
98                         normal[0] = relPos.normalized();
99                 }
100                 else
101                 {
102                         normal[0].setValue(btScalar(1.0),0,0);
103                 }
104
105                 btPlaneSpace1(normal[0], normal[1], normal[2]);
106
107                 for (int i=0;i<3;i++)
108                 {
109                         new (&m_jac[i]) btJacobianEntry(
110                                 m_rbA.getCenterOfMassTransform().getBasis().transpose(),
111                                 m_rbB.getCenterOfMassTransform().getBasis().transpose(),
112                                 pivotAInW - m_rbA.getCenterOfMassPosition(),
113                                 pivotBInW - m_rbB.getCenterOfMassPosition(),
114                                 normal[i],
115                                 m_rbA.getInvInertiaDiagLocal(),
116                                 m_rbA.getInvMass(),
117                                 m_rbB.getInvInertiaDiagLocal(),
118                                 m_rbB.getInvMass());
119                 }
120         }
121
122         btVector3 b1Axis1,b1Axis2,b1Axis3;
123         btVector3 b2Axis1,b2Axis2;
124
125         b1Axis1 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(0);
126         b2Axis1 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(0);
127
128         btScalar swing1=btScalar(0.),swing2 = btScalar(0.);
129
130         // Get Frame into world space
131         if (m_swingSpan1 >= btScalar(0.05f))
132         {
133                 b1Axis2 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(1);
134                 swing1  = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) );
135         }
136
137         if (m_swingSpan2 >= btScalar(0.05f))
138         {
139                 b1Axis3 = getRigidBodyA().getCenterOfMassTransform().getBasis() * this->m_rbAFrame.getBasis().getColumn(2);                     
140                 swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) );
141         }
142
143         btScalar RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1);             
144         btScalar RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2);     
145         btScalar EllipseAngle = btFabs(swing1)* RMaxAngle1Sq + btFabs(swing2) * RMaxAngle2Sq;
146
147         if (EllipseAngle > 1.0f)
148         {
149                 m_swingCorrection = EllipseAngle-1.0f;
150                 m_solveSwingLimit = true;
151                 
152                 // Calculate necessary axis & factors
153                 m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3));
154                 m_swingAxis.normalize();
155
156                 btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
157                 m_swingAxis *= swingAxisSign;
158
159                 m_kSwing =  btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_swingAxis) +
160                         getRigidBodyB().computeAngularImpulseDenominator(m_swingAxis));
161
162         }
163
164         // Twist limits
165         if (m_twistSpan >= btScalar(0.))
166         {
167                 btVector3 b2Axis2 = getRigidBodyB().getCenterOfMassTransform().getBasis() * this->m_rbBFrame.getBasis().getColumn(1);
168                 btQuaternion rotationArc = shortestArcQuat(b2Axis1,b1Axis1);
169                 btVector3 TwistRef = quatRotate(rotationArc,b2Axis2); 
170                 btScalar twist = btAtan2Fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) );
171
172                 btScalar lockedFreeFactor = (m_twistSpan > btScalar(0.05f)) ? m_limitSoftness : btScalar(0.);
173                 if (twist <= -m_twistSpan*lockedFreeFactor)
174                 {
175                         m_twistCorrection = -(twist + m_twistSpan);
176                         m_solveTwistLimit = true;
177
178                         m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
179                         m_twistAxis.normalize();
180                         m_twistAxis *= -1.0f;
181
182                         m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
183                                 getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
184
185                 }       else
186                         if (twist >  m_twistSpan*lockedFreeFactor)
187                         {
188                                 m_twistCorrection = (twist - m_twistSpan);
189                                 m_solveTwistLimit = true;
190
191                                 m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
192                                 m_twistAxis.normalize();
193
194                                 m_kTwist = btScalar(1.) / (getRigidBodyA().computeAngularImpulseDenominator(m_twistAxis) +
195                                         getRigidBodyB().computeAngularImpulseDenominator(m_twistAxis));
196
197                         }
198         }
199 }
200
201 void    btConeTwistConstraint::solveConstraint(btScalar timeStep)
202 {
203
204         btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
205         btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();
206
207         btScalar tau = btScalar(0.3);
208         btScalar damping = btScalar(1.);
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, 0.0f );
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, 0.0f );
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 }