added hinge constraint support to Bullet physics
[blender.git] / extern / bullet / BulletDynamics / ConstraintSolver / HingeConstraint.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 "HingeConstraint.h"
18 #include "Dynamics/RigidBody.h"
19 #include "Dynamics/MassProps.h"
20 #include "SimdTransformUtil.h"
21
22
23 HingeConstraint::HingeConstraint()
24 {
25 }
26
27 HingeConstraint::HingeConstraint(RigidBody& rbA,RigidBody& rbB, const SimdVector3& pivotInA,const SimdVector3& pivotInB,
28                                                                  SimdVector3& axisInA,SimdVector3& axisInB)
29 :TypedConstraint(rbA,rbB),m_pivotInA(pivotInA),m_pivotInB(pivotInB),
30 m_axisInA(axisInA),
31 m_axisInB(axisInB)
32 {
33
34 }
35
36
37 HingeConstraint::HingeConstraint(RigidBody& rbA,const SimdVector3& pivotInA,SimdVector3& axisInA)
38 :TypedConstraint(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 {
43         
44 }
45
46 void    HingeConstraint::BuildJacobian()
47 {
48         SimdVector3     normal(0,0,0);
49
50         for (int i=0;i<3;i++)
51         {
52                 normal[i] = 1;
53                 new (&m_jac[i]) JacobianEntry(
54                         m_rbA.getCenterOfMassTransform().getBasis().transpose(),
55                         m_rbB.getCenterOfMassTransform().getBasis().transpose(),
56                         m_rbA.getCenterOfMassTransform()*m_pivotInA - m_rbA.getCenterOfMassPosition(),
57                         m_rbB.getCenterOfMassTransform()*m_pivotInB - m_rbB.getCenterOfMassPosition(),
58                         normal,
59                         m_rbA.getInvInertiaDiagLocal(),
60                         m_rbA.getInvMass(),
61                         m_rbB.getInvInertiaDiagLocal(),
62                         m_rbB.getInvMass());
63                 normal[i] = 0;
64         }
65
66         //calculate two perpendicular jointAxis, orthogonal to hingeAxis
67         //these two jointAxis require equal angular velocities for both bodies
68
69         //this is ununsed for now, it's a todo
70         SimdVector3 axisWorldA = GetRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
71         SimdVector3 jointAxis0;
72         SimdVector3 jointAxis1;
73         SimdPlaneSpace1(axisWorldA,jointAxis0,jointAxis1);
74         
75         new (&m_jacAng[0])      JacobianEntry(jointAxis0,
76                 m_rbA.getCenterOfMassTransform().getBasis().transpose(),
77                 m_rbB.getCenterOfMassTransform().getBasis().transpose(),
78                 m_rbA.getInvInertiaDiagLocal(),
79                 m_rbB.getInvInertiaDiagLocal());
80
81         new (&m_jacAng[1])      JacobianEntry(jointAxis1,
82                 m_rbA.getCenterOfMassTransform().getBasis().transpose(),
83                 m_rbB.getCenterOfMassTransform().getBasis().transpose(),
84                 m_rbA.getInvInertiaDiagLocal(),
85                 m_rbB.getInvInertiaDiagLocal());
86
87
88 }
89
90 void    HingeConstraint::SolveConstraint(SimdScalar     timeStep)
91 {
92
93         SimdVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
94         SimdVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
95
96         SimdVector3 normal(0,0,0);
97         SimdScalar tau = 0.3f;
98         SimdScalar damping = 1.f;
99
100         for (int i=0;i<3;i++)
101         {               
102                 normal[i] = 1;
103                 SimdScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal();
104
105                 SimdVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
106                 SimdVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
107                 
108                 SimdVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
109                 SimdVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
110                 SimdVector3 vel = vel1 - vel2;
111                 SimdScalar rel_vel;
112                 rel_vel = normal.dot(vel);
113                 //positional error (zeroth order error)
114                 SimdScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
115                 SimdScalar impulse = depth*tau/timeStep  * jacDiagABInv -  damping * rel_vel * jacDiagABInv * damping;
116
117                 SimdVector3 impulse_vector = normal * impulse;
118                 m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
119                 m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
120                 
121                 normal[i] = 0;
122         }
123
124         ///solve angular part
125
126         // get axes in world space
127         SimdVector3 axisA = GetRigidBodyA().getCenterOfMassTransform().getBasis() * m_axisInA;
128         SimdVector3 axisB = GetRigidBodyB().getCenterOfMassTransform().getBasis() * m_axisInB;
129
130         const SimdVector3& angVelA = GetRigidBodyA().getAngularVelocity();
131         const SimdVector3& angVelB = GetRigidBodyB().getAngularVelocity();
132         SimdVector3 angA = angVelA - axisA * axisA.dot(angVelA);
133         SimdVector3 angB = angVelB - axisB * axisB.dot(angVelB);
134         SimdVector3 velrel = angA-angB;
135
136         //solve angular velocity correction
137         float relaxation = 1.f;
138         float len = velrel.length();
139         if (len > 0.00001f)
140         {
141                 SimdVector3 normal = velrel.normalized();
142                 float denom = GetRigidBodyA().ComputeAngularImpulseDenominator(normal) +
143                         GetRigidBodyB().ComputeAngularImpulseDenominator(normal);
144                 // scale for mass and relaxation
145                 velrel *= (1.f/denom) * 0.9;
146         }
147
148         //solve angular positional correction
149         SimdVector3 angularError = -axisA.cross(axisB) *(1.f/timeStep);
150         float len2 = angularError.length();
151         if (len2>0.00001f)
152         {
153                 SimdVector3 normal2 = angularError.normalized();
154                 float denom2 = GetRigidBodyA().ComputeAngularImpulseDenominator(normal2) +
155                                 GetRigidBodyB().ComputeAngularImpulseDenominator(normal2);
156                 angularError *= (1.f/denom2) * relaxation;
157         }
158
159         m_rbA.applyTorqueImpulse(-velrel+angularError);
160         m_rbB.applyTorqueImpulse(velrel-angularError);
161
162 }
163
164 void    HingeConstraint::UpdateRHS(SimdScalar   timeStep)
165 {
166
167 }
168