Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

more work on hashed pairmanager. growing doesn't work yet, so need to allocate enough room for the overlapping pairs in advance.

Browse Source 
boxbox reports contact point in B, rather then average point
box, cylinder use halfextents corrected for scaling and margin. made the margin in this halfextents explicit in the 'getHalfExtentsWithMargin' and 'getHalfExtentsWithoutMargin'
integrated changed for ODE quickstep solver
replaced inline with SIMD_FORCE_INLINE
some minor optimizations in the btSequentialImpulseConstraintSolver

added cone drawing (for X,Y,Z cones)
This commit is contained in:
ejcoumans
2007-10-12 02:52:28 +00:00
parent 1baa61bc8d
commit eff4fe8ec8
39 changed files with 1882 additions and 1336 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
Extras/quickstep/OdeConstraintSolver.cpp
View File

@@ -58,15 +58,6 @@ class BU_Joint;
//see below
//to bridge with ODE quickstep, we make a temp copy of the rigidbodies in each simultion island
// Remotion 10.10.07: we do not need thi any more!
//#define ODE_MAX_SOLVER_BODIES 16384
//#define ODE_MAX_SOLVER_JOINTS 65535
//static OdeSolverBody gSolverBodyArray[ODE_MAX_SOLVER_BODIES];
//static ContactJoint gJointArray[ODE_MAX_SOLVER_JOINTS];
//static OdeTypedJoint gTypedJointArray[ODE_MAX_SOLVER_JOINTS];
OdeConstraintSolver::OdeConstraintSolver():
m_cfm(0.f),//1e-5f),
@@ -84,7 +75,6 @@ btScalar OdeConstraintSolver::solveGroup(btCollisionObject** bodies,int numBulle
m_CurJoint = 0;
m_CurTypedJoint = 0;
// Remotion 10.10.07: to be sure just find max_contacts
int max_contacts = 0; /// should be 4 //Remotion
for (int j=0;j<numManifolds;j++){
btPersistentManifold* manifold = manifoldPtr[j];
@@ -138,7 +128,7 @@ btScalar OdeConstraintSolver::solveGroup(btCollisionObject** bodies,int numBulle
END_PROFILE("prepareConstraints");
BEGIN_PROFILE("solveConstraints");
SolveInternal1(m_cfm,m_erp,m_odeBodies,numBodies,m_joints,numJoints,infoGlobal);
m_SorLcpSolver.SolveInternal1(m_cfm,m_erp,m_odeBodies,numBodies,m_joints,numJoints,infoGlobal,stackAlloc); ///do
//write back resulting velocities
for (int i=0;i<numBodies;i++)
@@ -278,11 +268,8 @@ void OdeConstraintSolver::ConvertConstraint(btPersistentManifold* manifold,
{
///refresh contact points is not needed anymore, it has been moved into the processCollision detection part.
#ifdef FORCE_REFESH_CONTACT_MANIFOLDS
manifold->refreshContactPoints(((btRigidBody*)manifold->getBody0())->getCenterOfMassTransform(),
((btRigidBody*)manifold->getBody1())->getCenterOfMassTransform());
#endif //FORCE_REFESH_CONTACT_MANIFOLDS
int bodyId0 = _bodyId0,bodyId1 = _bodyId1;

15
Extras/quickstep/OdeConstraintSolver.h
View File

@@ -22,6 +22,7 @@ subject to the following restrictions:
#include "OdeContactJoint.h"
#include "OdeTypedJoint.h"
#include "OdeSolverBody.h"
#include "SorLcp.h"
class btRigidBody;
struct OdeSolverBody;
@@ -39,6 +40,8 @@ private:
float m_cfm;
float m_erp;
SorLcpSolver m_SorLcpSolver;
btAlignedObjectArray<OdeSolverBody*> m_odeBodies;
btAlignedObjectArray<BU_Joint*> m_joints;
@@ -47,6 +50,7 @@ private:
btAlignedObjectArray<OdeTypedJoint> m_TypedJointArray;
private:
int ConvertBody(btRigidBody* body,btAlignedObjectArray< OdeSolverBody*> &bodies,int& numBodies);
void ConvertConstraint(btPersistentManifold* manifold,
btAlignedObjectArray<BU_Joint*> &joints,int& numJoints,
@@ -80,8 +84,19 @@ public:
m_erp = erp;
}
///clear internal cached data and reset random seed
void reset()
{
m_SorLcpSolver.dRand2_seed = 0;
}
void setRandSeed(unsigned long seed)
{
m_SorLcpSolver.dRand2_seed = seed;
}
unsigned long getRandSeed() const
{
return m_SorLcpSolver.dRand2_seed;
}
};

133
Extras/quickstep/OdeMacros.h
View File

@@ -93,6 +93,7 @@ do { \
(A)[8] op dDOT1((B+8),(C)); \
(A)[9] op dDOT1((B+8),(C+4)); \
(A)[10] op dDOT1((B+8),(C+8));
#define dMULTIPLYOP0_333(A,op,B,C) \
(A)[0] op dDOT14((B),(C)); \
(A)[1] op dDOT14((B),(C+1)); \
@@ -119,52 +120,30 @@ do { \
#define dALLOCA16(n) \
((char*)dEFFICIENT_SIZE(((size_t)(alloca((n)+(EFFICIENT_ALIGNMENT-1))))))
/////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////
#ifdef DEBUG
#define ANSI_FTOL 1
extern "C" {
__declspec(naked) void _ftol2() {
__asm {
#if ANSI_FTOL
fnstcw WORD PTR [esp-2]
mov ax, WORD PTR [esp-2]
OR AX, 0C00h
mov WORD PTR [esp-4], ax
fldcw WORD PTR [esp-4]
fistp QWORD PTR [esp-12]
fldcw WORD PTR [esp-2]
mov eax, DWORD PTR [esp-12]
mov edx, DWORD PTR [esp-8]
#else
fistp DWORD PTR [esp-12]
mov eax, DWORD PTR [esp-12]
mov ecx, DWORD PTR [esp-8]
#endif
ret
}
}
}
#endif //DEBUG
#define ALLOCA dALLOCA16
//#define ALLOCA dALLOCA16
typedef const btScalar *dRealPtr;
typedef btScalar *dRealMutablePtr;
#define dRealArray(name,n) btScalar name[n];
#define dRealAllocaArray(name,n) btScalar *name = (btScalar*) ALLOCA ((n)*sizeof(btScalar));
//#define dRealArray(name,n) btScalar name[n];
//#define dRealAllocaArray(name,n) btScalar *name = (btScalar*) ALLOCA ((n)*sizeof(btScalar));
///////////////////////////////////////////////////////////////////////////////
//Remotion: 10.10.2007
#define ALLOCA(size) stackAlloc->allocate( dEFFICIENT_SIZE(size) );
//#define dRealAllocaArray(name,size) btScalar *name = (btScalar*) stackAlloc->allocate(dEFFICIENT_SIZE(size)*sizeof(btScalar));
#define dRealAllocaArray(name,size) btScalar *name = NULL; \
int memNeeded_##name = dEFFICIENT_SIZE(size)*sizeof(btScalar); \
if (memNeeded_##name < stackAlloc->getAvailableMemory()) name = (btScalar*) stackAlloc->allocate(memNeeded_##name); \
else{ btAssert(memNeeded_##name < stackAlloc->getAvailableMemory()); name = (btScalar*) alloca(memNeeded_##name); }
///////////////////////////////////////////////////////////////////////////////
#if 0
inline void dSetZero1 (btScalar *a, int n)
{
dAASSERT (a && n >= 0);
@@ -182,6 +161,76 @@ inline void dSetValue1 (btScalar *a, int n, btScalar value)
n--;
}
}
#else
/// This macros are for MSVC and XCode compilers. Remotion.
#if _MSC_VER //Visual Studio Win32, Win64
#include <xmmintrin.h> // SSE
#include <emmintrin.h> // SSE2
#include <intrin.h> // SSE3
#define __USE_SSE__
/*
#ifdef _WIN64
typedef unsigned __int64 size_t;
#else
typedef unsigned int size_t;
#endif
*/
#elif __GNUC__ // XCode GCC
#if defined(__ppc__) || defined(__ppc64__) // Mac PPC
///PPC or PPC64 Mac no SSE support
#elif defined(__i386__) // Intel Mac with SSE support
#include <xmmintrin.h> // SSE
#include <emmintrin.h> // SSE2
#include <pmmintrin.h> // SSE3
#define __USE_SSE__
#endif
#include <string.h>
#endif
//Remotion: 10.10.2007
//------------------------------------------------------------------------------
#define IS_ALIGNED_16(x) ((size_t(x)&15)==0)
//------------------------------------------------------------------------------
inline void dSetZero1 (btScalar *dest, int size)
{
dAASSERT (dest && size >= 0);
memset(dest, 0, size * sizeof(btScalar));
}
//------------------------------------------------------------------------------
inline void dSetValue1 (btScalar *dest, int size, btScalar val)
{
dAASSERT (dest && size >= 0);
int n_mod4 = size & 3;
int n4 = size - n_mod4;
#ifdef __USE_SSE__
if(IS_ALIGNED_16(dest)){
__m128 xmm0 = _mm_set_ps1(val);
for (int i=0; i<n4; i+=4)
{
_mm_store_ps(&dest[i],xmm0);
}
}else
#endif
{
for (int i=0; i<n4; i+=4) // Unrolled Loop
{
dest[i ] = val;
dest[i+1] = val;
dest[i+2] = val;
dest[i+3] = val;
}
}
for (int i=n4; i<size; i++){
dest[i] = val;
}
}
#endif
/////////////////////////////////////////////////////////////////////
#endif //USE_SOR_SOLVER

88
Extras/quickstep/SorLcp.cpp
View File

@@ -19,7 +19,6 @@
* LICENSE.TXT and LICENSE-BSD.TXT for more details. *
* *
*************************************************************************/
#include "SorLcp.h"
#include "OdeSolverBody.h"
@@ -49,10 +48,8 @@
#include "OdeJoint.h"
#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
////////////////////////////////////////////////////////////////////
//math stuff
#include "OdeMacros.h"
//***************************************************************************
@@ -72,14 +69,10 @@
#define RANDOMLY_REORDER_CONSTRAINTS 1
//***************************************************************************
// various common computations involving the matrix J
// compute iMJ = inv(M)*J'
static void compute_invM_JT (int m, dRealMutablePtr J, dRealMutablePtr iMJ, int *jb,
inline void compute_invM_JT (int m, dRealMutablePtr J, dRealMutablePtr iMJ, int *jb,
//OdeSolverBody* const *body,
const btAlignedObjectArray<OdeSolverBody*> &body,
dRealPtr invI)
@@ -183,8 +176,7 @@ static void multiply_invM_JT (int m, int nb, dRealMutablePtr iMJ, int *jb,
// compute out = J*in.
static void multiply_J (int m, dRealMutablePtr J, int *jb,
inline void multiply_J (int m, dRealMutablePtr J, int *jb,
dRealMutablePtr in, dRealMutablePtr out)
{
int i,j;
@@ -218,35 +210,25 @@ static void multiply_J (int m, dRealMutablePtr J, int *jb,
//
// b, lo and hi are modified on exit
struct IndexError {
//------------------------------------------------------------------------------
ATTRIBUTE_ALIGNED16(struct) IndexError {
btScalar error; // error to sort on
int findex;
int index; // row index
};
static unsigned long seed2 = 0;
unsigned long dRand2()
{
seed2 = (1664525L*seed2 + 1013904223L) & 0xffffffff;
return seed2;
}
int dRandInt2 (int n)
{
float a = float(n) / 4294967296.0f;
return (int) (float(dRand2()) * a);
}
static void SOR_LCP (int m, int nb, dRealMutablePtr J, int *jb,
//OdeSolverBody * const *body,
//------------------------------------------------------------------------------
void SorLcpSolver::SOR_LCP(int m, int nb, dRealMutablePtr J, int *jb,
const btAlignedObjectArray<OdeSolverBody*> &body,
dRealPtr invI, dRealMutablePtr lambda, dRealMutablePtr invMforce, dRealMutablePtr rhs,
dRealMutablePtr lo, dRealMutablePtr hi, dRealPtr cfm, int *findex,
int numiter,float overRelax)
int numiter,float overRelax,
btStackAlloc* stackAlloc
)
{
//btBlock* saBlock = stackAlloc->beginBlock();//Remo: 10.10.2007
AutoBlockSa asaBlock(stackAlloc);
const int num_iterations = numiter;
const float sor_w = overRelax; // SOR over-relaxation parameter
@@ -310,7 +292,9 @@ static void SOR_LCP (int m, int nb, dRealMutablePtr J, int *jb,
Ad[i] *= cfm[i];
// order to solve constraint rows in
IndexError *order = (IndexError*) alloca (m*sizeof(IndexError));
//IndexError *order = (IndexError*) alloca (m*sizeof(IndexError));
IndexError *order = (IndexError*) ALLOCA (m*sizeof(IndexError));
#ifndef REORDER_CONSTRAINTS
// make sure constraints with findex < 0 come first.
@@ -455,24 +439,20 @@ static void SOR_LCP (int m, int nb, dRealMutablePtr J, int *jb,
}
}
}
//stackAlloc->endBlock(saBlock);//Remo: 10.10.2007
}
/*
void SolveInternal1 (float global_cfm,
float global_erp,
OdeSolverBody* const *body, int nb,
BU_Joint **joint,
int nj,
const btContactSolverInfo& solverInfo)
*/
void SolveInternal1 (
//------------------------------------------------------------------------------
void SorLcpSolver::SolveInternal1 (
float global_cfm,
float global_erp,
const btAlignedObjectArray<OdeSolverBody*> &body, int nb,
btAlignedObjectArray<BU_Joint*> &joint,
int nj, const btContactSolverInfo& solverInfo)
int nj, const btContactSolverInfo& solverInfo,
btStackAlloc* stackAlloc)
{
//btBlock* saBlock = stackAlloc->beginBlock();//Remo: 10.10.2007
AutoBlockSa asaBlock(stackAlloc);
int numIter = solverInfo.m_numIterations;
float sOr = solverInfo.m_sor;
@@ -529,7 +509,8 @@ void SolveInternal1 (
// joints with m=0 are inactive and are removed from the joints array
// entirely, so that the code that follows does not consider them.
//@@@ do we really need to save all the info1's
BU_Joint::Info1 *info = (BU_Joint::Info1*) alloca (nj*sizeof(BU_Joint::Info1));
BU_Joint::Info1 *info = (BU_Joint::Info1*) ALLOCA (nj*sizeof(BU_Joint::Info1));
for (i=0, j=0; j<nj; j++) { // i=dest, j=src
joint[j]->GetInfo1 (info+i);
dIASSERT (info[i].m >= 0 && info[i].m <= 6 && info[i].nub >= 0 && info[i].nub <= info[i].m);
@@ -542,7 +523,7 @@ void SolveInternal1 (
// create the row offset array
int m = 0;
int *ofs = (int*) alloca (nj*sizeof(int));
int *ofs = (int*) ALLOCA (nj*sizeof(int));
for (i=0; i<nj; i++) {
ofs[i] = m;
m += info[i].m;
@@ -550,7 +531,7 @@ void SolveInternal1 (
// if there are constraints, compute the constraint force
dRealAllocaArray (J,m*12);
int *jb = (int*) alloca (m*2*sizeof(int));
int *jb = (int*) ALLOCA (m*2*sizeof(int));
if (m > 0) {
// create a constraint equation right hand side vector `c', a constraint
// force mixing vector `cfm', and LCP low and high bound vectors, and an
@@ -559,7 +540,9 @@ void SolveInternal1 (
dRealAllocaArray (cfm,m);
dRealAllocaArray (lo,m);
dRealAllocaArray (hi,m);
int *findex = (int*) alloca (m*sizeof(int));
int *findex = (int*) ALLOCA (m*sizeof(int));
dSetZero1 (c,m);
dSetValue1 (cfm,m,global_cfm);
dSetValue1 (lo,m,-dInfinity);
@@ -599,9 +582,6 @@ void SolveInternal1 (
if (Jinfo.c[0] > solverInfo.m_maxErrorReduction)
Jinfo.c[0] = solverInfo.m_maxErrorReduction;
// adjust returned findex values for global index numbering
for (j=0; j<info[i].m; j++) {
if (findex[ofs[i] + j] >= 0)
@@ -657,7 +637,8 @@ void SolveInternal1 (
// solve the LCP problem and get lambda and invM*constraint_force
dRealAllocaArray (cforce,nb*6);
SOR_LCP (m,nb,J,jb,body,invI,lambda,cforce,rhs,lo,hi,cfm,findex,numIter,sOr);
/// SOR_LCP
SOR_LCP (m,nb,J,jb,body,invI,lambda,cforce,rhs,lo,hi,cfm,findex,numIter,sOr,stackAlloc);
#ifdef WARM_STARTING
// save lambda for the next iteration
@@ -668,10 +649,8 @@ void SolveInternal1 (
}
#endif
// note that the SOR method overwrites rhs and J at this point, so
// they should not be used again.
// add stepsize * cforce to the body velocity
for (i=0; i<nb; i++) {
for (j=0; j<3; j++)
@@ -682,11 +661,8 @@ void SolveInternal1 (
}
}
// compute the velocity update:
// add stepsize * invM * fe to the body velocity
for (i=0; i<nb; i++) {
btScalar body_invMass = body[i]->m_invMass;
btVector3 linvel = body[i]->m_linearVelocity;
@@ -703,7 +679,7 @@ void SolveInternal1 (
dMULTIPLY0_331NEW(angvel,+=,invI + i*12,body[i]->m_tacc);
body[i]->m_angularVelocity = angvel;
}
//stackAlloc->endBlock(saBlock);//Remo: 10.10.2007
}

88
Extras/quickstep/SorLcp.h
View File

@@ -29,23 +29,85 @@ struct OdeSolverBody;
class BU_Joint;
#include "LinearMath/btScalar.h"
#include "LinearMath/btAlignedObjectArray.h"
#include "LinearMath/btStackAlloc.h"
struct btContactSolverInfo;
/*
void SolveInternal1 (float global_cfm,
float global_erp,
OdeSolverBody * const *body, int nb,
BU_Joint **_joint, int nj, const btContactSolverInfo& info);
*/
void SolveInternal1 (float global_cfm,
float global_erp,
const btAlignedObjectArray<OdeSolverBody*> &body, int nb,
btAlignedObjectArray<BU_Joint*> &joint,
int nj, const btContactSolverInfo& solverInfo);
int dRandInt2 (int n);
//=============================================================================
class SorLcpSolver //Remotion: 11.10.2007
{
public:
SorLcpSolver()
{
dRand2_seed = 0;
}
void SolveInternal1 (float global_cfm,
float global_erp,
const btAlignedObjectArray<OdeSolverBody*> &body, int nb,
btAlignedObjectArray<BU_Joint*> &joint,
int nj, const btContactSolverInfo& solverInfo,
btStackAlloc* stackAlloc
);
public: //data
unsigned long dRand2_seed;
protected: //typedef
typedef const btScalar *dRealPtr;
typedef btScalar *dRealMutablePtr;
protected: //members
//------------------------------------------------------------------------------
SIMD_FORCE_INLINE unsigned long dRand2()
{
dRand2_seed = (1664525L*dRand2_seed + 1013904223L) & 0xffffffff;
return dRand2_seed;
}
//------------------------------------------------------------------------------
SIMD_FORCE_INLINE int dRandInt2 (int n)
{
float a = float(n) / 4294967296.0f;
return (int) (float(dRand2()) * a);
}
//------------------------------------------------------------------------------
void SOR_LCP(int m, int nb, dRealMutablePtr J, int *jb,
const btAlignedObjectArray<OdeSolverBody*> &body,
dRealPtr invI, dRealMutablePtr lambda, dRealMutablePtr invMforce, dRealMutablePtr rhs,
dRealMutablePtr lo, dRealMutablePtr hi, dRealPtr cfm, int *findex,
int numiter,float overRelax,
btStackAlloc* stackAlloc
);
};
//=============================================================================
class AutoBlockSa //Remotion: 10.10.2007
{
btStackAlloc* stackAlloc;
btBlock* saBlock;
public:
AutoBlockSa(btStackAlloc* stackAlloc_)
{
stackAlloc = stackAlloc_;
saBlock = stackAlloc->beginBlock();
}
~AutoBlockSa()
{
stackAlloc->endBlock(saBlock);
}
//operator btBlock* () { return saBlock; }
};
// //Usage
//void function(btStackAlloc* stackAlloc)
//{
// AutoBlockSa(stackAlloc);
// ...
// if(...) return;
// return;
//}
//------------------------------------------------------------------------------
#endif //SOR_LCP_H