btSolverBody.h

Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 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.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
00014 */
00015 
00016 #ifndef BT_SOLVER_BODY_H
00017 #define BT_SOLVER_BODY_H
00018 
00019 class   btRigidBody;
00020 #include "LinearMath/btVector3.h"
00021 #include "LinearMath/btMatrix3x3.h"
00022 
00023 #include "LinearMath/btAlignedAllocator.h"
00024 #include "LinearMath/btTransformUtil.h"
00025 
00027 #ifdef BT_USE_SSE
00028 #define USE_SIMD 1
00029 #endif //
00030 
00031 
00032 #ifdef USE_SIMD
00033 
00034 struct  btSimdScalar
00035 {
00036         SIMD_FORCE_INLINE       btSimdScalar()
00037         {
00038 
00039         }
00040 
00041         SIMD_FORCE_INLINE       btSimdScalar(float      fl)
00042         :m_vec128 (_mm_set1_ps(fl))
00043         {
00044         }
00045 
00046         SIMD_FORCE_INLINE       btSimdScalar(__m128 v128)
00047                 :m_vec128(v128)
00048         {
00049         }
00050         union
00051         {
00052                 __m128          m_vec128;
00053                 float           m_floats[4];
00054                 int                     m_ints[4];
00055                 btScalar        m_unusedPadding;
00056         };
00057         SIMD_FORCE_INLINE       __m128  get128()
00058         {
00059                 return m_vec128;
00060         }
00061 
00062         SIMD_FORCE_INLINE       const __m128    get128() const
00063         {
00064                 return m_vec128;
00065         }
00066 
00067         SIMD_FORCE_INLINE       void    set128(__m128 v128)
00068         {
00069                 m_vec128 = v128;
00070         }
00071 
00072         SIMD_FORCE_INLINE       operator       __m128()       
00073         { 
00074                 return m_vec128; 
00075         }
00076         SIMD_FORCE_INLINE       operator const __m128() const 
00077         { 
00078                 return m_vec128; 
00079         }
00080         
00081         SIMD_FORCE_INLINE       operator float() const 
00082         { 
00083                 return m_floats[0]; 
00084         }
00085 
00086 };
00087 
00089 SIMD_FORCE_INLINE btSimdScalar 
00090 operator*(const btSimdScalar& v1, const btSimdScalar& v2) 
00091 {
00092         return btSimdScalar(_mm_mul_ps(v1.get128(),v2.get128()));
00093 }
00094 
00096 SIMD_FORCE_INLINE btSimdScalar 
00097 operator+(const btSimdScalar& v1, const btSimdScalar& v2) 
00098 {
00099         return btSimdScalar(_mm_add_ps(v1.get128(),v2.get128()));
00100 }
00101 
00102 
00103 #else
00104 #define btSimdScalar btScalar
00105 #endif
00106 
00108 ATTRIBUTE_ALIGNED64 (struct)    btSolverBody
00109 {
00110         BT_DECLARE_ALIGNED_ALLOCATOR();
00111         btTransform             m_worldTransform;
00112         btVector3               m_deltaLinearVelocity;
00113         btVector3               m_deltaAngularVelocity;
00114         btVector3               m_angularFactor;
00115         btVector3               m_linearFactor;
00116         btVector3               m_invMass;
00117         btVector3               m_pushVelocity;
00118         btVector3               m_turnVelocity;
00119         btVector3               m_linearVelocity;
00120         btVector3               m_angularVelocity;
00121 
00122         btRigidBody*    m_originalBody;
00123         void    setWorldTransform(const btTransform& worldTransform)
00124         {
00125                 m_worldTransform = worldTransform;
00126         }
00127 
00128         const btTransform& getWorldTransform() const
00129         {
00130                 return m_worldTransform;
00131         }
00132         
00133         SIMD_FORCE_INLINE void  getVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
00134         {
00135                 if (m_originalBody)
00136                         velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos);
00137                 else
00138                         velocity.setValue(0,0,0);
00139         }
00140 
00141         SIMD_FORCE_INLINE void  getAngularVelocity(btVector3& angVel) const
00142         {
00143                 if (m_originalBody)
00144                         angVel =m_angularVelocity+m_deltaAngularVelocity;
00145                 else
00146                         angVel.setValue(0,0,0);
00147         }
00148 
00149 
00150         //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
00151         SIMD_FORCE_INLINE void applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude)
00152         {
00153                 if (m_originalBody)
00154                 {
00155                         m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor;
00156                         m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
00157                 }
00158         }
00159 
00160         SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
00161         {
00162                 if (m_originalBody)
00163                 {
00164                         m_pushVelocity += linearComponent*impulseMagnitude*m_linearFactor;
00165                         m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
00166                 }
00167         }
00168 
00169 
00170 
00171         const btVector3& getDeltaLinearVelocity() const
00172         {
00173                 return m_deltaLinearVelocity;
00174         }
00175 
00176         const btVector3& getDeltaAngularVelocity() const
00177         {
00178                 return m_deltaAngularVelocity;
00179         }
00180 
00181         const btVector3& getPushVelocity() const 
00182         {
00183                 return m_pushVelocity;
00184         }
00185 
00186         const btVector3& getTurnVelocity() const 
00187         {
00188                 return m_turnVelocity;
00189         }
00190 
00191 
00194                 
00195         btVector3& internalGetDeltaLinearVelocity()
00196         {
00197                 return m_deltaLinearVelocity;
00198         }
00199 
00200         btVector3& internalGetDeltaAngularVelocity()
00201         {
00202                 return m_deltaAngularVelocity;
00203         }
00204 
00205         const btVector3& internalGetAngularFactor() const
00206         {
00207                 return m_angularFactor;
00208         }
00209 
00210         const btVector3& internalGetInvMass() const
00211         {
00212                 return m_invMass;
00213         }
00214 
00215         void internalSetInvMass(const btVector3& invMass)
00216         {
00217                 m_invMass = invMass;
00218         }
00219         
00220         btVector3& internalGetPushVelocity()
00221         {
00222                 return m_pushVelocity;
00223         }
00224 
00225         btVector3& internalGetTurnVelocity()
00226         {
00227                 return m_turnVelocity;
00228         }
00229 
00230         SIMD_FORCE_INLINE void  internalGetVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
00231         {
00232                 velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos);
00233         }
00234 
00235         SIMD_FORCE_INLINE void  internalGetAngularVelocity(btVector3& angVel) const
00236         {
00237                 angVel = m_angularVelocity+m_deltaAngularVelocity;
00238         }
00239 
00240 
00241         //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
00242         SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude)
00243         {
00244                 if (m_originalBody)
00245                 {
00246                         m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor;
00247                         m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
00248                 }
00249         }
00250                 
00251         
00252         
00253 
00254         void    writebackVelocity()
00255         {
00256                 if (m_originalBody)
00257                 {
00258                         m_linearVelocity +=m_deltaLinearVelocity;
00259                         m_angularVelocity += m_deltaAngularVelocity;
00260                         
00261                         //m_originalBody->setCompanionId(-1);
00262                 }
00263         }
00264 
00265 
00266         void    writebackVelocityAndTransform(btScalar timeStep, btScalar splitImpulseTurnErp)
00267         {
00268         (void) timeStep;
00269                 if (m_originalBody)
00270                 {
00271                         m_linearVelocity += m_deltaLinearVelocity;
00272                         m_angularVelocity += m_deltaAngularVelocity;
00273                         
00274                         //correct the position/orientation based on push/turn recovery
00275                         btTransform newTransform;
00276                         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)
00277                         {
00278                         //      btQuaternion orn = m_worldTransform.getRotation();
00279                                 btTransformUtil::integrateTransform(m_worldTransform,m_pushVelocity,m_turnVelocity*splitImpulseTurnErp,timeStep,newTransform);
00280                                 m_worldTransform = newTransform;
00281                         }
00282                         //m_worldTransform.setRotation(orn);
00283                         //m_originalBody->setCompanionId(-1);
00284                 }
00285         }
00286         
00287 
00288 
00289 };
00290 
00291 #endif //BT_SOLVER_BODY_H
00292 
00293