Changeset 2882 for code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btSolverBody.h

Timestamp:

Mar 31, 2009, 8:05:51 PM (15 years ago)

Author:

rgrieder

Message:

Update from Bullet 2.73 to 2.74.

File:

• code/trunk/src/bullet/BulletDynamics/ConstraintSolver/btSolverBody.h (modified) (3 diffs)

code/trunk/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.
@@ -29 +109 @@
 {
         BT_DECLARE_ALIGNED_ALLOCATOR();
+        btVector3               m_deltaLinearVelocity;
+        btVector3               m_deltaAngularVelocity;
+        btScalar                m_angularFactor;
+        btScalar                m_invMass;
+        btScalar                m_friction;
+        btRigidBody*    m_originalBody;
+        btVector3               m_pushVelocity;
+        //btVector3             m_turnVelocity; 
+
         
-        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;
-        
-        
-        SIMD_FORCE_INLINE void  getVelocityInLocalPoint(const btVector3& rel_pos, btVector3& velocity ) const
+        SIMD_FORCE_INLINE void  getVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const
         {
-                velocity = m_linearVelocity + m_angularVelocity.cross(rel_pos);
+                if (m_originalBody)
+                        velocity = m_originalBody->getLinearVelocity()+m_deltaLinearVelocity + (m_originalBody->getAngularVelocity()+m_deltaAngularVelocity).cross(rel_pos);
+                else
+                        velocity.setValue(0,0,0);
         }
 
+        SIMD_FORCE_INLINE void  getAngularVelocity(btVector3& angVel) const
+        {
+                if (m_originalBody)
+                        angVel = m_originalBody->getAngularVelocity()+m_deltaAngularVelocity;
+                else
+                        angVel.setValue(0,0,0);
+        }
+
+
         //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)
+        SIMD_FORCE_INLINE void applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude)
         {
-                if (m_invMass)
+                //if (m_invMass)
                 {
-                        m_linearVelocity += linearComponent*impulseMagnitude;
-                        m_angularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
-                }
-        }
-        
-        SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
-        {
-                if (m_invMass)
-                {
-                        m_pushVelocity += linearComponent*impulseMagnitude;
-                        m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
+                        m_deltaLinearVelocity += linearComponent*impulseMagnitude;
+                        m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
                 }
         }
 
+        
+/*
         
         void    writebackVelocity()
@@ -73 +153 @@
                 if (m_invMass)
                 {
-                        m_originalBody->setLinearVelocity(m_linearVelocity);
-                        m_originalBody->setAngularVelocity(m_angularVelocity);
+                        m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+ m_deltaLinearVelocity);
+                        m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity);
                         
                         //m_originalBody->setCompanionId(-1);
                 }
         }
-        
+        */
 
-        void    writebackVelocity(btScalar timeStep)
+        void    writebackVelocity(btScalar timeStep=0)
         {
                 if (m_invMass)
                 {
-                        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->setLinearVelocity(m_originalBody->getLinearVelocity()+m_deltaLinearVelocity);
+                        m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity);
                         //m_originalBody->setCompanionId(-1);
                 }
         }
-
-        void    readVelocity()
-        {
-                if (m_invMass)
-                {
-                        m_linearVelocity = m_originalBody->getLinearVelocity();
-                        m_angularVelocity = m_originalBody->getAngularVelocity();
-                }
-        }
-