Big work-in-progress refactoring of the constraint solver:
1) Add fast branchless SIMD support for constraint solver (Windows only until we get other contributions). See resolveSingleConstraintRowGenericSIMD in Bullet/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp resolveSingleConstraintRowGenericSIMD can be used for all constraints, including contact, point 2 point, hinge, generic etc. 2) During this refactoring, all constraints support the obsolete 'solveConstraintObsolete' while we add 'getInfo1' and 'getInfo2' support. This interface is almost identical interface to Open Dynamics Engine, to make it easier to port Dantzig LCP solver. 3) Some minor refactoring to reduce huge constructor overhead in math classes.
This commit is contained in:
erwin.coumans
@@ -23,193 +23,157 @@ class btRigidBody;
|
||||
#include "LinearMath/btAlignedAllocator.h"
|
||||
#include "LinearMath/btTransformUtil.h"
|
||||
|
||||
#ifndef _USE_JACOBIAN
|
||||
///Until we get other contributions, only use SIMD on Windows, when using Visual Studio 2008 or later, and not double precision
|
||||
#if (defined (WIN32) && (_MSC_VER) && _MSC_VER >= 1400) && (!defined (BT_USE_DOUBLE_PRECISION))
|
||||
#define USE_SIMD 1
|
||||
#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
|
||||
|
||||
#ifdef USE_SIMD
|
||||
#include <emmintrin.h>
|
||||
|
||||
struct btSimdScalar
|
||||
{
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
btMatrix3x3 m_worldInvInertiaTensor;
|
||||
|
||||
btVector3 m_angularVelocity;
|
||||
btVector3 m_linearVelocity;
|
||||
|
||||
btVector3 m_deltaLinearVelocity;
|
||||
btVector3 m_deltaAngularVelocity;
|
||||
|
||||
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 btSimdScalar()
|
||||
{
|
||||
velocity = m_linearVelocity + m_angularVelocity.cross(rel_pos);
|
||||
|
||||
}
|
||||
|
||||
//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,btScalar impulseMagnitude)
|
||||
SIMD_FORCE_INLINE btSimdScalar(float fl)
|
||||
:m_vec128 (_mm_set1_ps(fl))
|
||||
{
|
||||
//if (m_invMass)
|
||||
{
|
||||
m_deltaLinearVelocity += linearComponent*impulseMagnitude;
|
||||
m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
|
||||
m_linearVelocity += linearComponent*impulseMagnitude;
|
||||
m_angularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
|
||||
}
|
||||
}
|
||||
|
||||
SIMD_FORCE_INLINE btSimdScalar(__m128 v128)
|
||||
:m_vec128(v128)
|
||||
{
|
||||
}
|
||||
union
|
||||
{
|
||||
__m128 m_vec128;
|
||||
float m_floats[4];
|
||||
int m_ints[4];
|
||||
};
|
||||
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 void internalApplyPushImpulse(const btVector3& linearComponent, const btVector3& angularComponent,btScalar impulseMagnitude)
|
||||
{
|
||||
//if (m_invMass)
|
||||
{
|
||||
m_pushVelocity += linearComponent*impulseMagnitude;
|
||||
m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor);
|
||||
}
|
||||
SIMD_FORCE_INLINE operator float() const
|
||||
{
|
||||
return m_floats[0];
|
||||
}
|
||||
|
||||
|
||||
void writebackVelocity()
|
||||
{
|
||||
if (m_invMass)
|
||||
{
|
||||
m_originalBody->setLinearVelocity(m_linearVelocity);
|
||||
m_originalBody->setAngularVelocity(m_angularVelocity);
|
||||
|
||||
//m_originalBody->setCompanionId(-1);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void writebackVelocity(btScalar timeStep)
|
||||
{
|
||||
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->setCompanionId(-1);
|
||||
}
|
||||
}
|
||||
|
||||
void readVelocity()
|
||||
{
|
||||
if (m_invMass)
|
||||
{
|
||||
m_linearVelocity = m_originalBody->getLinearVelocity();
|
||||
m_angularVelocity = m_originalBody->getAngularVelocity();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
};
|
||||
|
||||
///@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();
|
||||
|
||||
btMatrix3x3 m_worldInvInertiaTensor;
|
||||
|
||||
btVector3 m_angularVelocity1;
|
||||
btVector3 m_linearVelocity1;
|
||||
|
||||
btVector3 m_deltaAngularVelocity;
|
||||
btVector3 m_deltaLinearVelocity;
|
||||
|
||||
btVector3 m_centerOfMassPosition;
|
||||
btVector3 m_pushVelocity;
|
||||
btVector3 m_turnVelocity;
|
||||
|
||||
float m_angularFactor;
|
||||
float m_invMass;
|
||||
float m_friction;
|
||||
btVector3 m_deltaAngularVelocity;
|
||||
btScalar m_angularFactor;
|
||||
btScalar m_invMass;
|
||||
btScalar m_friction;
|
||||
btRigidBody* m_originalBody;
|
||||
|
||||
|
||||
btVector3 m_pushVelocity;
|
||||
//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_linearVelocity1 + m_deltaLinearVelocity) + (m_angularVelocity1+m_deltaAngularVelocity).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)
|
||||
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 applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude)
|
||||
{
|
||||
//if (m_invMass)
|
||||
{
|
||||
m_deltaLinearVelocity += linearComponent*impulseMagnitude;
|
||||
m_deltaAngularVelocity += 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);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
|
||||
void writebackVelocity()
|
||||
{
|
||||
if (m_invMass)
|
||||
{
|
||||
m_originalBody->setLinearVelocity(m_linearVelocity1 + m_deltaLinearVelocity);
|
||||
m_originalBody->setAngularVelocity(m_angularVelocity1+m_deltaAngularVelocity);
|
||||
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_linearVelocity1 + m_deltaLinearVelocity);
|
||||
m_originalBody->setAngularVelocity(m_angularVelocity1+m_deltaAngularVelocity);
|
||||
|
||||
//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_linearVelocity1 = m_originalBody->getLinearVelocity();
|
||||
m_angularVelocity1 = m_originalBody->getAngularVelocity();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
};
|
||||
|
||||
#endif //USE_JAC
|
||||
#endif //BT_SOLVER_BODY_H
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user