angular-only hinge updated
[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         if (!m_angularOnly)
204         {
205                 for (int i=0;i<3;i++)
206                 {               
207                         normal[i] = 1;
208                         btScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal();
209
210                         btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
211                         btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
212                         
213                         btVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
214                         btVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
215                         btVector3 vel = vel1 - vel2;
216                         btScalar rel_vel;
217                         rel_vel = normal.dot(vel);
218                         //positional error (zeroth order error)
219                         btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
220                         btScalar impulse = depth*tau/timeStep  * jacDiagABInv -  damping * rel_vel * jacDiagABInv * damping;
221                         m_appliedImpulse += impulse;
222                         btVector3 impulse_vector = normal * impulse;
223                         m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
224                         m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
225                         
226                         normal[i] = 0;
227                 }
228         }
229
230         
231         {
232                 ///solve angular part
233
234                 // get axes in world space
235                 btVector3 axisA = getRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
236                 btVector3 axisB = getRigidBodyB().getCenterOfMassTransform().getBasis() * m_axisInB;
237
238                 const btVector3& angVelA = getRigidBodyA().getAngularVelocity();
239                 const btVector3& angVelB = getRigidBodyB().getAngularVelocity();
240                 btVector3 angA = angVelA - axisA * axisA.dot(angVelA);
241                 btVector3 angB = angVelB - axisB * axisB.dot(angVelB);
242                 btVector3 velrel = angA-angB;
243
244                 //solve angular velocity correction
245                 float relaxation = 1.f;
246                 float len = velrel.length();
247                 if (len > 0.00001f)
248                 {
249                         btVector3 normal = velrel.normalized();
250                         float denom = getRigidBodyA().computeAngularImpulseDenominator(normal) +
251                                 getRigidBodyB().computeAngularImpulseDenominator(normal);
252                         // scale for mass and relaxation
253                         velrel *= (1.f/denom) * 0.9;
254                 }
255
256                 //solve angular positional correction
257                 btVector3 angularError = -axisA.cross(axisB) *(1.f/timeStep);
258                 float len2 = angularError.length();
259                 if (len2>0.00001f)
260                 {
261                         btVector3 normal2 = angularError.normalized();
262                         float denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) +
263                                         getRigidBodyB().computeAngularImpulseDenominator(normal2);
264                         angularError *= (1.f/denom2) * relaxation;
265                 }
266
267                 m_rbA.applyTorqueImpulse(-velrel+angularError);
268                 m_rbB.applyTorqueImpulse(velrel-angularError);
269         }
270 #endif
271
272 }
273
274 void    btHingeConstraint::updateRHS(btScalar   timeStep)
275 {
276
277 }
278