4154dbf2caa5f51d2d0c58be97be220c5329f90d
[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
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(m_axisInA),
31 m_axisInB(m_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(m_axisInA),
40 //fixed axis in woeldspace
41 m_axisInB(rbA.getCenterOfMassTransform() * m_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
70         SimdVector3 jointAxis0(0,0,1);
71         SimdVector3 jointAxis1(0,1,0);
72         
73         new (&m_jacAng[0])      JacobianEntry(jointAxis0,
74                 m_rbA.getCenterOfMassTransform().getBasis().transpose(),
75                 m_rbB.getCenterOfMassTransform().getBasis().transpose(),
76                 m_rbA.getInvInertiaDiagLocal(),
77                 m_rbB.getInvInertiaDiagLocal());
78
79         new (&m_jacAng[1])      JacobianEntry(jointAxis1,
80                 m_rbA.getCenterOfMassTransform().getBasis().transpose(),
81                 m_rbB.getCenterOfMassTransform().getBasis().transpose(),
82                 m_rbA.getInvInertiaDiagLocal(),
83                 m_rbB.getInvInertiaDiagLocal());
84
85
86 }
87
88 void    HingeConstraint::SolveConstraint(SimdScalar     timeStep)
89 {
90         SimdVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
91         SimdVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;
92
93
94         SimdVector3 normal(0,0,0);
95         SimdScalar tau = 0.3f;
96         SimdScalar damping = 1.f;
97
98 //      SimdVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
99 //      SimdVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();
100
101         for (int i=0;i<3;i++)
102         {               
103                 normal[i] = 1;
104                 SimdScalar jacDiagABInv = 1.f / m_jac[i].getDiagonal();
105
106                 SimdVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
107                 SimdVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
108                 //this jacobian entry could be re-used for all iterations
109                 
110                 SimdVector3 vel1 = m_rbA.getVelocityInLocalPoint(rel_pos1);
111                 SimdVector3 vel2 = m_rbB.getVelocityInLocalPoint(rel_pos2);
112                 SimdVector3 vel = vel1 - vel2;
113                 
114                 SimdScalar rel_vel;
115                 rel_vel = normal.dot(vel);
116
117         /*
118                 //velocity error (first order error)
119                 SimdScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
120                                                                                                                 m_rbB.getLinearVelocity(),angvelB);
121         */
122         
123                 //positional error (zeroth order error)
124                 SimdScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
125                 
126                 SimdScalar impulse = depth*tau/timeStep  * jacDiagABInv -  damping * rel_vel * jacDiagABInv * damping;
127
128                 SimdVector3 impulse_vector = normal * impulse;
129                 m_rbA.applyImpulse(impulse_vector, pivotAInW - m_rbA.getCenterOfMassPosition());
130                 m_rbB.applyImpulse(-impulse_vector, pivotBInW - m_rbB.getCenterOfMassPosition());
131                 
132                 normal[i] = 0;
133         }
134 //todo: do angular part
135
136 }
137
138 void    HingeConstraint::UpdateRHS(SimdScalar   timeStep)
139 {
140
141 }
142