Added refactored Bullet 2.x library. Important: these files are not part of the Blend...
[blender.git] / extern / bullet2 / src / BulletDynamics / ConstraintSolver / btHingeConstraint.cpp
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
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
16
17 #include "btHingeConstraint.h"
18 #include "BulletDynamics/Dynamics/btRigidBody.h"
19 #include "LinearMath/btTransformUtil.h"
20
21
22 btHingeConstraint::btHingeConstraint()
23 {
24 }
25
26 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
27                                                                  btVector3& axisInA,btVector3& axisInB)
28 :btTypedConstraint(rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
29 m_axisInA(axisInA),
30 m_axisInB(-axisInB),
31 m_angularOnly(false)
32 {
33
34 }
35
36
37 btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,btVector3& axisInA)
38 :btTypedConstraint(rbA),m_pivotInA(pivotInA),m_pivotInB(rbA.getCenterOfMassTransform()(pivotInA)),
39 m_axisInA(axisInA),
40 //fixed axis in worldspace
41 m_axisInB(rbA.getCenterOfMassTransform().getBasis() * -axisInA),
42 m_angularOnly(false)
43 {
44         
45 }
46
47 void    btHingeConstraint::buildJacobian()
48 {
49         m_appliedImpulse = 0.f;
50
51         btVector3       normal(0,0,0);
52
53         if (!m_angularOnly)
54         {
55                 for (int i=0;i<3;i++)
56                 {
57                         normal[i] = 1;
58                         new (&m_jac[i]) btJacobianEntry(
59                                 m_rbA.getCenterOfMassTransform().getBasis().transpose(),
60                                 m_rbB.getCenterOfMassTransform().getBasis().transpose(),
61                                 m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(),
62                                 m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(),
63                                 normal,
64                                 m_rbA.getInvInertiaDiagLocal(),
65                                 m_rbA.getInvMass(),
66                                 m_rbB.getInvInertiaDiagLocal(),
67                                 m_rbB.getInvMass());
68                         normal[i] = 0;
69                 }
70         }
71
72         //calculate two perpendicular jointAxis, orthogonal to hingeAxis
73         //these two jointAxis require equal angular velocities for both bodies
74
75         //this is unused for now, it's a todo
76         btVector3 axisWorldA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
77         btVector3 jointAxis0;
78         btVector3 jointAxis1;
79         btPlaneSpace1(axisWorldA,jointAxis0,jointAxis1);
80         
81         new (&m_jacAng[0])      btJacobianEntry(jointAxis0,
82                 m_rbA.getCenterOfMassTransform().getBasis().transpose(),
83                 m_rbB.getCenterOfMassTransform().getBasis().transpose(),
84                 m_rbA.getInvInertiaDiagLocal(),
85                 m_rbB.getInvInertiaDiagLocal());
86
87         new (&m_jacAng[1])      btJacobianEntry(jointAxis1,
88                 m_rbA.getCenterOfMassTransform().getBasis().transpose(),
89                 m_rbB.getCenterOfMassTransform().getBasis().transpose(),
90                 m_rbA.getInvInertiaDiagLocal(),
91                 m_rbB.getInvInertiaDiagLocal());
92
93
94 }
95
96 void    btHingeConstraint::solveConstraint(btScalar     timeStep)
97 {
98 //#define NEW_IMPLEMENTATION
99
100 #ifdef NEW_IMPLEMENTATION
101         btScalar tau = 0.3f;
102         btScalar damping = 1.f;
103
104         btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
105         btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
106
107         // Dirk: Don't we need to update this after each applied impulse
108         btVector3 angvelA; // = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
109     btVector3 angvelB; // = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
110
111
112         if (!m_angularOnly)
113         {
114                 btVector3 normal(0,0,0);
115
116                 for (int i=0;i<3;i++)
117                 {               
118                         normal[i] = 1;
119                         btScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal();
120
121                         btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
122                         btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
123                         
124                         btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
125                         btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
126                         btVector3 vel = vel1 - vel2;
127
128                         // Dirk: Get new angular velocity since it changed after applying an impulse
129                         angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
130                         angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
131         
132                         //velocity error (first order error)
133                         btScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, 
134                                                                                                                                         m_rbB.getLinearVelocity(),angvelB);
135                 
136                         //positional error (zeroth order error)
137                         btScalar depth = -(pivotAInW - pivotBInW).dot(normal); 
138                         
139                         btScalar impulse = tau*depth/timeStep * jacDiagABInv -  damping * rel_vel * jacDiagABInv;
140
141                         btVector3 impulse_vector = normal * impulse;
142                         m_rbA.applyImpulse( impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
143                         m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
144                         
145                         normal[i] = 0;
146                 }
147         }
148
149         ///solve angular part
150
151         // get axes in world space
152         btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
153         btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_axisInB;
154
155         // constraint axes in world space
156         btVector3 jointAxis0;
157         btVector3 jointAxis1;
158         btPlaneSpace1(axisA,jointAxis0,jointAxis1);
159
160
161         // Dirk: Get new angular velocity since it changed after applying an impulse
162         angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
163     angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
164         
165         btScalar jacDiagABInv0 = 1.f / m_jacAng[0].getDiagonal();
166         btScalar rel_vel0 = m_jacAng[0].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, 
167                                                                                                                                         m_rbB.getLinearVelocity(),angvelB);
168         float tau1 = tau;//0.f;
169
170         btScalar impulse0 = (tau1 * axisB.dot(jointAxis1) / timeStep - damping * rel_vel0) * jacDiagABInv0;
171         btVector3 angular_impulse0 = jointAxis0 * impulse0;
172
173         m_rbA.applyTorqueImpulse( angular_impulse0);
174         m_rbB.applyTorqueImpulse(-angular_impulse0);
175
176
177
178         // Dirk: Get new angular velocity since it changed after applying an impulse    
179         angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
180     angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
181
182         btScalar jacDiagABInv1 = 1.f / m_jacAng[1].getDiagonal();
183         btScalar rel_vel1 = m_jacAng[1].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA, 
184                                                                                                                                         m_rbB.getLinearVelocity(),angvelB);;
185
186         btScalar impulse1 = -(tau1 * axisB.dot(jointAxis0) / timeStep + damping * rel_vel1) * jacDiagABInv1;
187         btVector3 angular_impulse1 = jointAxis1 * impulse1;
188
189         m_rbA.applyTorqueImpulse( angular_impulse1);
190         m_rbB.applyTorqueImpulse(-angular_impulse1);
191
192 #else
193
194
195         btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
196         btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
197
198         btVector3 normal(0,0,0);
199         btScalar tau = 0.3f;
200         btScalar damping = 1.f;
201
202 //linear part
203         {
204                 for (int i=0;i<3;i++)
205                 {               
206                         normal[i] = 1;
207                         btScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal();
208
209                         btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
210                         btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
211                         
212                         btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
213                         btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
214                         btVector3 vel = vel1 - vel2;
215                         btScalar rel_vel;
216                         rel_vel = normal.dot(vel);
217                         //positional error (zeroth order error)
218                         btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
219                         btScalar impulse = depth*tau/timeStep  * jacDiagABInv -  damping * rel_vel * jacDiagABInv * damping;
220                         m_appliedImpulse += impulse;
221                         btVector3 impulse_vector = normal * impulse;
222                         m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
223                         m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
224                         
225                         normal[i] = 0;
226                 }
227         }
228
229         ///solve angular part
230
231         // get axes in world space
232         btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
233         btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_axisInB;
234
235         const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
236         const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
237         btVector3 angA = angVelA - axisA * axisA.dot(angVelA);
238         btVector3 angB = angVelB - axisB * axisB.dot(angVelB);
239         btVector3 velrel = angA-angB;
240
241         //solve angular velocity correction
242         float relaxation = 1.f;
243         float len = velrel.length();
244         if (len > 0.00001f)
245         {
246                 btVector3 normal = velrel.normalized();
247                 float denom = getRigidBodyA().computeAngularImpulseDenominator(normal) +
248                         getRigidBodyB().computeAngularImpulseDenominator(normal);
249                 // scale for mass and relaxation
250                 velrel *= (1.f/denom) * 0.9;
251         }
252
253         //solve angular positional correction
254         btVector3 angularError = -axisA.cross(axisB) *(1.f/timeStep);
255         float len2 = angularError.length();
256         if (len2>0.00001f)
257         {
258                 btVector3 normal2 = angularError.normalized();
259                 float denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) +
260                                 getRigidBodyB().computeAngularImpulseDenominator(normal2);
261                 angularError *= (1.f/denom2) * relaxation;
262         }
263
264         m_rbA.applyTorqueImpulse(-velrel+angularError);
265         m_rbB.applyTorqueImpulse(velrel-angularError);
266
267 #endif
268
269 }
270
271 void    btHingeConstraint::updateRHS(btScalar   timeStep)
272 {
273
274 }
275