Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/trunk_ogre_old/src/external/bullet/BulletDynamics/ConstraintSolver/btJacobianEntry.h @ 12373

Last change on this file since 12373 was 8393, checked in by rgrieder, 14 years ago

Updated Bullet from v2.77 to v2.78.
(I'm not going to make a branch for that since the update from 2.74 to 2.77 hasn't been tested that much either).

You will HAVE to do a complete RECOMPILE! I tested with MSVC and MinGW and they both threw linker errors at me.
  • Property svn:eol-style set to native
File size: 5.3 KB
Line 
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. This notice may not be removed or altered from any source distribution.
14*/
15
16#ifndef BT_JACOBIAN_ENTRY_H
17#define BT_JACOBIAN_ENTRY_H
18
19#include "LinearMath/btVector3.h"
20#include "BulletDynamics/Dynamics/btRigidBody.h"
21
22
23//notes:
24// Another memory optimization would be to store m_1MinvJt in the remaining 3 w components
25// which makes the btJacobianEntry memory layout 16 bytes
26// if you only are interested in angular part, just feed massInvA and massInvB zero
27
28/// Jacobian entry is an abstraction that allows to describe constraints
29/// it can be used in combination with a constraint solver
30/// Can be used to relate the effect of an impulse to the constraint error
31ATTRIBUTE_ALIGNED16(class) btJacobianEntry
32{
33public:
34        btJacobianEntry() {};
35        //constraint between two different rigidbodies
36        btJacobianEntry(
37                const btMatrix3x3& world2A,
38                const btMatrix3x3& world2B,
39                const btVector3& rel_pos1,const btVector3& rel_pos2,
40                const btVector3& jointAxis,
41                const btVector3& inertiaInvA, 
42                const btScalar massInvA,
43                const btVector3& inertiaInvB,
44                const btScalar massInvB)
45                :m_linearJointAxis(jointAxis)
46        {
47                m_aJ = world2A*(rel_pos1.cross(m_linearJointAxis));
48                m_bJ = world2B*(rel_pos2.cross(-m_linearJointAxis));
49                m_0MinvJt       = inertiaInvA * m_aJ;
50                m_1MinvJt = inertiaInvB * m_bJ;
51                m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ);
52
53                btAssert(m_Adiag > btScalar(0.0));
54        }
55
56        //angular constraint between two different rigidbodies
57        btJacobianEntry(const btVector3& jointAxis,
58                const btMatrix3x3& world2A,
59                const btMatrix3x3& world2B,
60                const btVector3& inertiaInvA,
61                const btVector3& inertiaInvB)
62                :m_linearJointAxis(btVector3(btScalar(0.),btScalar(0.),btScalar(0.)))
63        {
64                m_aJ= world2A*jointAxis;
65                m_bJ = world2B*-jointAxis;
66                m_0MinvJt       = inertiaInvA * m_aJ;
67                m_1MinvJt = inertiaInvB * m_bJ;
68                m_Adiag =  m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
69
70                btAssert(m_Adiag > btScalar(0.0));
71        }
72
73        //angular constraint between two different rigidbodies
74        btJacobianEntry(const btVector3& axisInA,
75                const btVector3& axisInB,
76                const btVector3& inertiaInvA,
77                const btVector3& inertiaInvB)
78                : m_linearJointAxis(btVector3(btScalar(0.),btScalar(0.),btScalar(0.)))
79                , m_aJ(axisInA)
80                , m_bJ(-axisInB)
81        {
82                m_0MinvJt       = inertiaInvA * m_aJ;
83                m_1MinvJt = inertiaInvB * m_bJ;
84                m_Adiag =  m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
85
86                btAssert(m_Adiag > btScalar(0.0));
87        }
88
89        //constraint on one rigidbody
90        btJacobianEntry(
91                const btMatrix3x3& world2A,
92                const btVector3& rel_pos1,const btVector3& rel_pos2,
93                const btVector3& jointAxis,
94                const btVector3& inertiaInvA, 
95                const btScalar massInvA)
96                :m_linearJointAxis(jointAxis)
97        {
98                m_aJ= world2A*(rel_pos1.cross(jointAxis));
99                m_bJ = world2A*(rel_pos2.cross(-jointAxis));
100                m_0MinvJt       = inertiaInvA * m_aJ;
101                m_1MinvJt = btVector3(btScalar(0.),btScalar(0.),btScalar(0.));
102                m_Adiag = massInvA + m_0MinvJt.dot(m_aJ);
103
104                btAssert(m_Adiag > btScalar(0.0));
105        }
106
107        btScalar        getDiagonal() const { return m_Adiag; }
108
109        // for two constraints on the same rigidbody (for example vehicle friction)
110        btScalar        getNonDiagonal(const btJacobianEntry& jacB, const btScalar massInvA) const
111        {
112                const btJacobianEntry& jacA = *this;
113                btScalar lin = massInvA * jacA.m_linearJointAxis.dot(jacB.m_linearJointAxis);
114                btScalar ang = jacA.m_0MinvJt.dot(jacB.m_aJ);
115                return lin + ang;
116        }
117
118       
119
120        // for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies)
121        btScalar        getNonDiagonal(const btJacobianEntry& jacB,const btScalar massInvA,const btScalar massInvB) const
122        {
123                const btJacobianEntry& jacA = *this;
124                btVector3 lin = jacA.m_linearJointAxis * jacB.m_linearJointAxis;
125                btVector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ;
126                btVector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ;
127                btVector3 lin0 = massInvA * lin ;
128                btVector3 lin1 = massInvB * lin;
129                btVector3 sum = ang0+ang1+lin0+lin1;
130                return sum[0]+sum[1]+sum[2];
131        }
132
133        btScalar getRelativeVelocity(const btVector3& linvelA,const btVector3& angvelA,const btVector3& linvelB,const btVector3& angvelB)
134        {
135                btVector3 linrel = linvelA - linvelB;
136                btVector3 angvela  = angvelA * m_aJ;
137                btVector3 angvelb  = angvelB * m_bJ;
138                linrel *= m_linearJointAxis;
139                angvela += angvelb;
140                angvela += linrel;
141                btScalar rel_vel2 = angvela[0]+angvela[1]+angvela[2];
142                return rel_vel2 + SIMD_EPSILON;
143        }
144//private:
145
146        btVector3       m_linearJointAxis;
147        btVector3       m_aJ;
148        btVector3       m_bJ;
149        btVector3       m_0MinvJt;
150        btVector3       m_1MinvJt;
151        //Optimization: can be stored in the w/last component of one of the vectors
152        btScalar        m_Adiag;
153
154};
155
156#endif //BT_JACOBIAN_ENTRY_H
Note: See TracBrowser for help on using the repository browser.