ebf58c344851075db13236decc2edfc2934da6c8
[blender.git] / extern / bullet / BulletDynamics / ConstraintSolver / JacobianEntry.h
1 /*
2  * Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/
3  *
4  * Permission to use, copy, modify, distribute and sell this software
5  * and its documentation for any purpose is hereby granted without fee,
6  * provided that the above copyright notice appear in all copies.
7  * Erwin Coumans makes no representations about the suitability 
8  * of this software for any purpose.  
9  * It is provided "as is" without express or implied warranty.
10 */
11 #ifndef JACOBIAN_ENTRY_H
12 #define JACOBIAN_ENTRY_H
13
14 #include "SimdVector3.h"
15 #include "Dynamics/RigidBody.h"
16
17
18 //notes:
19 // Another memory optimization would be to store m_MbJ in the remaining 3 w components
20 // which makes the JacobianEntry memory layout 16 bytes
21 // if you only are interested in angular part, just feed massInvA and massInvB zero
22
23 /// Jacobian entry is an abstraction that allows to describe constraints
24 /// it can be used in combination with a constraint solver
25 /// Can be used to relate the effect of an impulse to the constraint error
26 class JacobianEntry
27 {
28 public:
29         JacobianEntry() {};
30         //constraint between two different rigidbodies
31         JacobianEntry(
32                 const SimdMatrix3x3& world2A,
33                 const SimdMatrix3x3& world2B,
34                 const SimdVector3& rel_pos1,const SimdVector3& rel_pos2,
35                 const SimdVector3& normal,
36                 const SimdVector3& inertiaInvA, 
37                 const SimdScalar massInvA,
38                 const SimdVector3& inertiaInvB,
39                 const SimdScalar massInvB)
40                 :m_normalAxis(normal)
41         {
42                 m_aJ = world2A*(rel_pos1.cross(normal));
43                 m_bJ = world2B*(rel_pos2.cross(normal));
44                 m_MaJ   = inertiaInvA * m_aJ;
45                 m_MbJ = inertiaInvB * m_bJ;
46                 m_jacDiagAB = massInvA + m_MaJ.dot(m_aJ) + massInvB + m_MbJ.dot(m_bJ);
47         }
48
49                 //angular constraint between two different rigidbodies
50         JacobianEntry(const SimdVector3& normal,
51                 const SimdMatrix3x3& world2A,
52                 const SimdMatrix3x3& world2B,
53                 const SimdVector3& inertiaInvA,
54                 const SimdVector3& inertiaInvB)
55                 :m_normalAxis(normal)
56         {
57                 m_aJ= world2A*normal;
58                 m_bJ = world2B*-normal;
59                 m_MaJ   = inertiaInvA * m_aJ;
60                 m_MbJ = inertiaInvB * m_bJ;
61                 m_jacDiagAB =  m_MaJ.dot(m_aJ) + m_MbJ.dot(m_bJ);
62         }
63
64         //constraint on one rigidbody
65         JacobianEntry(
66                 const SimdMatrix3x3& world2A,
67                 const SimdVector3& rel_pos1,const SimdVector3& rel_pos2,
68                 const SimdVector3& normal,
69                 const SimdVector3& inertiaInvA, 
70                 const SimdScalar massInvA)
71                 :m_normalAxis(normal)
72         {
73                 m_aJ= world2A*(rel_pos1.cross(normal));
74                 m_bJ = world2A*(rel_pos2.cross(normal));
75                 m_MaJ   = inertiaInvA * m_aJ;
76                 m_MbJ = SimdVector3(0.f,0.f,0.f);
77                 m_jacDiagAB = massInvA + m_MaJ.dot(m_aJ);
78         }
79
80         SimdScalar      getDiagonal() const { return m_jacDiagAB; }
81
82         // for two constraints on the same rigidbody (for example vehicle friction)
83         SimdScalar      getNonDiagonal(const JacobianEntry& jacB, const SimdScalar massInvA) const
84         {
85                 const JacobianEntry& jacA = *this;
86                 SimdScalar lin = massInvA * jacA.m_normalAxis.dot(jacB.m_normalAxis);
87                 SimdScalar ang = jacA.m_MaJ.dot(jacB.m_aJ);
88                 return lin + ang;
89         }
90
91         
92
93         // for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies)
94         SimdScalar      getNonDiagonal(const JacobianEntry& jacB,const SimdScalar massInvA,const SimdScalar massInvB) const
95         {
96                 const JacobianEntry& jacA = *this;
97                 SimdVector3 lin = jacA.m_normalAxis * jacB.m_normalAxis;
98                 SimdVector3 ang0 = jacA.m_MaJ * jacB.m_aJ;
99                 SimdVector3 ang1 = jacA.m_MbJ * jacB.m_bJ;
100                 SimdVector3 lin0 = massInvA * lin ;
101                 SimdVector3 lin1 = massInvB * lin;
102                 SimdVector3 sum = ang0+ang1+lin0+lin1;
103                 return sum[0]+sum[1]+sum[2];
104         }
105
106         SimdScalar getRelativeVelocity(const SimdVector3& linvelA,const SimdVector3& angvelA,const SimdVector3& linvelB,const SimdVector3& angvelB)
107         {
108                 SimdVector3 linrel = linvelA - linvelB;
109                 SimdVector3 angvela  = angvelA * m_aJ;
110                 SimdVector3 angvelb  = angvelB * m_bJ;
111                 linrel *= m_normalAxis;
112                 angvela += angvelb;
113                 angvela += linrel;
114                 SimdScalar rel_vel2 = angvela[0]+angvela[1]+angvela[2];
115                 return rel_vel2 + SIMD_EPSILON;
116         }
117 //private:
118
119         SimdVector3     m_normalAxis;
120         SimdVector3     m_aJ;
121         SimdVector3     m_bJ;
122         SimdVector3     m_MaJ;
123         SimdVector3     m_MbJ;
124         //Optimization: can be stored in the w/last component of one of the vectors
125         SimdScalar      m_jacDiagAB;
126
127 };
128
129 #endif //JACOBIAN_ENTRY_H