Changeset 2908 for code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btSolverBody.h

Timestamp:

Apr 8, 2009, 12:58:47 AM (16 years ago)

Author:

dafrick

Message:

Reverted to revision 2906 (because I'm too stupid to merge correctly, 2nd try will follow shortly. )

Location:

code/branches/questsystem5

Files:

• . (modified) (1 prop)
• src/bullet/BulletDynamics/ConstraintSolver/btSolverBody.h (modified) (3 diffs)

code/branches/questsystem5/src/bullet/BulletDynamics/ConstraintSolver/btSolverBody.h

@@ -24 +24 @@
 #include "LinearMath/btTransformUtil.h"
 
-///Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later, and not double precision
-#ifdef BT_USE_SSE
-#define USE_SIMD 1
-#endif //
-
-
-#ifdef USE_SIMD
-
-struct  btSimdScalar
-{
-        SIMD_FORCE_INLINE       btSimdScalar()
-        {
-
-        }
-
-        SIMD_FORCE_INLINE       btSimdScalar(float      fl)
-        :m_vec128 (_mm_set1_ps(fl))
-        {
-        }
-
-        SIMD_FORCE_INLINE       btSimdScalar(__m128 v128)
-                :m_vec128(v128)
-        {
-        }
-        union
-        {
-                __m128          m_vec128;
-                float           m_floats[4];
-                int                     m_ints[4];
-                btScalar        m_unusedPadding;
-        };
-        SIMD_FORCE_INLINE       __m128  get128()
-        {
-                return m_vec128;
-        }
-
-        SIMD_FORCE_INLINE       const __m128    get128() const
-        {
-                return m_vec128;
-        }
-
-        SIMD_FORCE_INLINE       void    set128(__m128 v128)
-        {
-                m_vec128 = v128;
-        }
-
-        SIMD_FORCE_INLINE       operator       __m128()       
-        { 
-                return m_vec128; 
-        }
-        SIMD_FORCE_INLINE       operator const __m128() const 
-        { 
-                return m_vec128; 
-        }
-        
-        SIMD_FORCE_INLINE       operator float() const 
-        { 
-                return m_floats[0]; 
-        }
-
-};
-
-///@brief Return the elementwise product of two btSimdScalar
-SIMD_FORCE_INLINE btSimdScalar 
-operator*(const btSimdScalar& v1, const btSimdScalar& v2) 
-{
-        return btSimdScalar(_mm_mul_ps(v1.get128(),v2.get128()));
-}
-
-///@brief Return the elementwise product of two btSimdScalar
-SIMD_FORCE_INLINE btSimdScalar 
-operator+(const btSimdScalar& v1, const btSimdScalar& v2) 
-{
-        return btSimdScalar(_mm_add_ps(v1.get128(),v2.get128()));
-}
-
-
-#else
-#define btSimdScalar btScalar
-#endif
 
 ///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance.
@@ -109 +29 @@
 {
         BT_DECLARE_ALIGNED_ALLOCATOR();
-        btVector3               m_deltaLinearVelocity;
-        btVector3               m_deltaAngularVelocity;
-        btScalar                m_angularFactor;
-        btScalar                m_invMass;
-        btScalar                m_friction;
+        
+        btMatrix3x3             m_worldInvInertiaTensor;
+
+        btVector3               m_angularVelocity;
+        btVector3               m_linearVelocity;
+        btVector3               m_centerOfMassPosition;
+        btVector3               m_pushVelocity;
+        btVector3               m_turnVelocity;
+
+        float                   m_angularFactor;
+        float                   m_invMass;
+        float                   m_friction;
         btRigidBody*    m_originalBody;
-        btVector3               m_pushVelocity;
-        //btVector3             m_turnVelocity; 
-
         
-        SIMD_FORCE_INLINE void  getVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
+        
+        SIMD_FORCE_INLINE void  getVelocityInLocalPoint(const btVector3& rel_pos, btVector3& velocity ) const
         {
-                if (m_originalBody)
-                        velocity = m_originalBody->getLinearVelocity()+m_deltaLinearVelocity + (m_originalBody->getAngularVelocity()+m_deltaAngularVelocity).cross(rel_pos);
-                else
-                        velocity.setValue(0,0,0);
+                velocity = m_linearVelocity + m_angularVelocity.cross(rel_pos);
         }
 
-        SIMD_FORCE_INLINE void  getAngularVelocity(btVector3& angVel) const
+        //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
+        SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
         {
-                if (m_originalBody)
-                        angVel = m_originalBody->getAngularVelocity()+m_deltaAngularVelocity;
-                else
-                        angVel.setValue(0,0,0);
+                if (m_invMass)
+                {
+                        m_linearVelocity += linearComponent*impulseMagnitude;
+                        m_angularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
+                }
         }
-
-
-        //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
-        SIMD_FORCE_INLINE void applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude)
+        
+        SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
         {
-                //if (m_invMass)
+                if (m_invMass)
                 {
-                        m_deltaLinearVelocity += linearComponent*impulseMagnitude;
-                        m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
+                        m_pushVelocity += linearComponent*impulseMagnitude;
+                        m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
                 }
         }
 
-        
-/*
         
         void    writebackVelocity()
@@ -153 +73 @@
                 if (m_invMass)
                 {
-                        m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+ m_deltaLinearVelocity);
-                        m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity);
+                        m_originalBody->setLinearVelocity(m_linearVelocity);
+                        m_originalBody->setAngularVelocity(m_angularVelocity);
                         
                         //m_originalBody->setCompanionId(-1);
                 }
         }
-        */
+        
 
-        void    writebackVelocity(btScalar timeStep=0)
+        void    writebackVelocity(btScalar timeStep)
         {
                 if (m_invMass)
                 {
-                        m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+m_deltaLinearVelocity);
-                        m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity);
+                        m_originalBody->setLinearVelocity(m_linearVelocity);
+                        m_originalBody->setAngularVelocity(m_angularVelocity);
+                        
+                        //correct the position/orientation based on push/turn recovery
+                        btTransform newTransform;
+                        btTransformUtil::integrateTransform(m_originalBody->getWorldTransform(),m_pushVelocity,m_turnVelocity,timeStep,newTransform);
+                        m_originalBody->setWorldTransform(newTransform);
+                        
                         //m_originalBody->setCompanionId(-1);
                 }
         }
+
+        void    readVelocity()
+        {
+                if (m_invMass)
+                {
+                        m_linearVelocity = m_originalBody->getLinearVelocity();
+                        m_angularVelocity = m_originalBody->getAngularVelocity();
+                }
+        }
+