diff options
Diffstat (limited to 'extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h')
| -rw-r--r-- | extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h | 160 |
1 files changed, 113 insertions, 47 deletions
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h index b3f0c9d7444..98c9876ae46 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h @@ -23,86 +23,152 @@ class btRigidBody; #include "LinearMath/btAlignedAllocator.h" #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 // -///btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance. + +#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. ATTRIBUTE_ALIGNED16 (struct) btSolverBody { BT_DECLARE_ALIGNED_ALLOCATOR(); - - btVector3 m_angularVelocity; - float m_angularFactor; - float m_invMass; - float m_friction; + btVector3 m_deltaLinearVelocity; + btVector3 m_deltaAngularVelocity; + btScalar m_angularFactor; + btScalar m_invMass; + btScalar m_friction; btRigidBody* m_originalBody; - btVector3 m_linearVelocity; - btVector3 m_centerOfMassPosition; - btVector3 m_pushVelocity; - btVector3 m_turnVelocity; - + //btVector3 m_turnVelocity; + - 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); } - //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 getAngularVelocity(btVector3& angVel) const { - if (m_invMass) - { - m_linearVelocity += linearComponent*impulseMagnitude; - m_angularVelocity += angularComponent*(impulseMagnitude*m_angularFactor); - } + if (m_originalBody) + angVel = m_originalBody->getAngularVelocity()+m_deltaAngularVelocity; + else + angVel.setValue(0,0,0); } - - SIMD_FORCE_INLINE void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude) + + + //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) { - if (m_invMass) + //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() { 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(); - } - } - |