bullet: Update to current svn, r2636
[blender.git] / extern / bullet2 / src / BulletDynamics / ConstraintSolver / btSolverBody.h
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 #ifndef BT_SOLVER_BODY_H
17 #define BT_SOLVER_BODY_H
18
19 class   btRigidBody;
20 #include "LinearMath/btVector3.h"
21 #include "LinearMath/btMatrix3x3.h"
22
23 #include "LinearMath/btAlignedAllocator.h"
24 #include "LinearMath/btTransformUtil.h"
25
26 ///Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later, and not double precision
27 #ifdef BT_USE_SSE
28 #define USE_SIMD 1
29 #endif //
30
31
32 #ifdef USE_SIMD
33
34 struct  btSimdScalar
35 {
36         SIMD_FORCE_INLINE       btSimdScalar()
37         {
38
39         }
40
41         SIMD_FORCE_INLINE       btSimdScalar(float      fl)
42         :m_vec128 (_mm_set1_ps(fl))
43         {
44         }
45
46         SIMD_FORCE_INLINE       btSimdScalar(__m128 v128)
47                 :m_vec128(v128)
48         {
49         }
50         union
51         {
52                 __m128          m_vec128;
53                 float           m_floats[4];
54                 int                     m_ints[4];
55                 btScalar        m_unusedPadding;
56         };
57         SIMD_FORCE_INLINE       __m128  get128()
58         {
59                 return m_vec128;
60         }
61
62         SIMD_FORCE_INLINE       const __m128    get128() const
63         {
64                 return m_vec128;
65         }
66
67         SIMD_FORCE_INLINE       void    set128(__m128 v128)
68         {
69                 m_vec128 = v128;
70         }
71
72         SIMD_FORCE_INLINE       operator       __m128()       
73         { 
74                 return m_vec128; 
75         }
76         SIMD_FORCE_INLINE       operator const __m128() const 
77         { 
78                 return m_vec128; 
79         }
80         
81         SIMD_FORCE_INLINE       operator float() const 
82         { 
83                 return m_floats[0]; 
84         }
85
86 };
87
88 ///@brief Return the elementwise product of two btSimdScalar
89 SIMD_FORCE_INLINE btSimdScalar 
90 operator*(const btSimdScalar& v1, const btSimdScalar& v2) 
91 {
92         return btSimdScalar(_mm_mul_ps(v1.get128(),v2.get128()));
93 }
94
95 ///@brief Return the elementwise product of two btSimdScalar
96 SIMD_FORCE_INLINE btSimdScalar 
97 operator+(const btSimdScalar& v1, const btSimdScalar& v2) 
98 {
99         return btSimdScalar(_mm_add_ps(v1.get128(),v2.get128()));
100 }
101
102
103 #else
104 #define btSimdScalar btScalar
105 #endif
106
107 ///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
108 ATTRIBUTE_ALIGNED16 (struct)    btSolverBody
109 {
110         BT_DECLARE_ALIGNED_ALLOCATOR();
111         btTransform             m_worldTransform;
112         btVector3               m_deltaLinearVelocity;
113         btVector3               m_deltaAngularVelocity;
114         btVector3               m_angularFactor;
115         btVector3               m_linearFactor;
116         btVector3               m_invMass;
117         btVector3               m_pushVelocity;
118         btVector3               m_turnVelocity;
119         btVector3               m_linearVelocity;
120         btVector3               m_angularVelocity;
121
122         btRigidBody*    m_originalBody;
123         void    setWorldTransform(const btTransform& worldTransform)
124         {
125                 m_worldTransform = worldTransform;
126         }
127
128         const btTransform& getWorldTransform() const
129         {
130                 return m_worldTransform;
131         }
132         
133         SIMD_FORCE_INLINE void  getVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
134         {
135                 if (m_originalBody)
136                         velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos);
137                 else
138                         velocity.setValue(0,0,0);
139         }
140
141         SIMD_FORCE_INLINE void  getAngularVelocity(btVector3& angVel) const
142         {
143                 if (m_originalBody)
144                         angVel =m_angularVelocity+m_deltaAngularVelocity;
145                 else
146                         angVel.setValue(0,0,0);
147         }
148
149
150         //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
151         SIMD_FORCE_INLINE void applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude)
152         {
153                 if (m_originalBody)
154                 {
155                         m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor;
156                         m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
157                 }
158         }
159
160         SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
161         {
162                 if (m_originalBody)
163                 {
164                         m_pushVelocity += linearComponent*impulseMagnitude*m_linearFactor;
165                         m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
166                 }
167         }
168
169
170
171         const btVector3& getDeltaLinearVelocity() const
172         {
173                 return m_deltaLinearVelocity;
174         }
175
176         const btVector3& getDeltaAngularVelocity() const
177         {
178                 return m_deltaAngularVelocity;
179         }
180
181         const btVector3& getPushVelocity() const 
182         {
183                 return m_pushVelocity;
184         }
185
186         const btVector3& getTurnVelocity() const 
187         {
188                 return m_turnVelocity;
189         }
190
191
192         ////////////////////////////////////////////////
193         ///some internal methods, don't use them
194                 
195         btVector3& internalGetDeltaLinearVelocity()
196         {
197                 return m_deltaLinearVelocity;
198         }
199
200         btVector3& internalGetDeltaAngularVelocity()
201         {
202                 return m_deltaAngularVelocity;
203         }
204
205         const btVector3& internalGetAngularFactor() const
206         {
207                 return m_angularFactor;
208         }
209
210         const btVector3& internalGetInvMass() const
211         {
212                 return m_invMass;
213         }
214
215         void internalSetInvMass(const btVector3& invMass)
216         {
217                 m_invMass = invMass;
218         }
219         
220         btVector3& internalGetPushVelocity()
221         {
222                 return m_pushVelocity;
223         }
224
225         btVector3& internalGetTurnVelocity()
226         {
227                 return m_turnVelocity;
228         }
229
230         SIMD_FORCE_INLINE void  internalGetVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
231         {
232                 velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos);
233         }
234
235         SIMD_FORCE_INLINE void  internalGetAngularVelocity(btVector3& angVel) const
236         {
237                 angVel = m_angularVelocity+m_deltaAngularVelocity;
238         }
239
240
241         //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
242         SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude)
243         {
244                 if (m_originalBody)
245                 {
246                         m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor;
247                         m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
248                 }
249         }
250                 
251         
252         
253
254         void    writebackVelocity()
255         {
256                 if (m_originalBody)
257                 {
258                         m_linearVelocity +=m_deltaLinearVelocity;
259                         m_angularVelocity += m_deltaAngularVelocity;
260                         
261                         //m_originalBody->setCompanionId(-1);
262                 }
263         }
264
265
266         void    writebackVelocityAndTransform(btScalar timeStep, btScalar splitImpulseTurnErp)
267         {
268         (void) timeStep;
269                 if (m_originalBody)
270                 {
271                         m_linearVelocity += m_deltaLinearVelocity;
272                         m_angularVelocity += m_deltaAngularVelocity;
273                         
274                         //correct the position/orientation based on push/turn recovery
275                         btTransform newTransform;
276                         if (m_pushVelocity[0]!=0.f || m_pushVelocity[1]!=0 || m_pushVelocity[2]!=0 || m_turnVelocity[0]!=0.f || m_turnVelocity[1]!=0 || m_turnVelocity[2]!=0)
277                         {
278                         //      btQuaternion orn = m_worldTransform.getRotation();
279                                 btTransformUtil::integrateTransform(m_worldTransform,m_pushVelocity,m_turnVelocity*splitImpulseTurnErp,timeStep,newTransform);
280                                 m_worldTransform = newTransform;
281                         }
282                         //m_worldTransform.setRotation(orn);
283                         //m_originalBody->setCompanionId(-1);
284                 }
285         }
286         
287
288
289 };
290
291 #endif //BT_SOLVER_BODY_H
292
293