diff --git a/Demos/SoftDemo/SoftDemo.cpp b/Demos/SoftDemo/SoftDemo.cpp index 5b814cdb1..1d78e5427 100644 --- a/Demos/SoftDemo/SoftDemo.cpp +++ b/Demos/SoftDemo/SoftDemo.cpp @@ -110,34 +110,34 @@ void SoftDemo::clientMoveAndDisplay() if (m_dynamicsWorld) { - if(m_drag) + if(m_drag) { - const int x=m_lastmousepos[0]; - const int y=m_lastmousepos[1]; - const btVector3 rayFrom=m_cameraPosition; - const btVector3 rayTo=getRayTo(x,y); - const btVector3 rayDir=(rayTo-rayFrom).normalized(); - const btVector3 N=(m_cameraTargetPosition-m_cameraPosition).normalized(); - const btScalar O=dot(m_impact,N); - const btScalar den=dot(N,rayDir); - if((den*den)>0) + const int x=m_lastmousepos[0]; + const int y=m_lastmousepos[1]; + const btVector3 rayFrom=m_cameraPosition; + const btVector3 rayTo=getRayTo(x,y); + const btVector3 rayDir=(rayTo-rayFrom).normalized(); + const btVector3 N=(m_cameraTargetPosition-m_cameraPosition).normalized(); + const btScalar O=dot(m_impact,N); + const btScalar den=dot(N,rayDir); + if((den*den)>0) { - const btScalar num=O-dot(N,rayFrom); - const btScalar hit=num/den; - if((hit>0)&&(hit<1500)) + const btScalar num=O-dot(N,rayFrom); + const btScalar hit=num/den; + if((hit>0)&&(hit<1500)) { - m_goal=rayFrom+rayDir*hit; + m_goal=rayFrom+rayDir*hit; } } - btVector3 delta=m_goal-m_node->m_x; - static const btScalar maxdrag=10; - if(delta.length2()>(maxdrag*maxdrag)) + btVector3 delta=m_goal-m_node->m_x; + static const btScalar maxdrag=10; + if(delta.length2()>(maxdrag*maxdrag)) { - delta=delta.normalized()*maxdrag; + delta=delta.normalized()*maxdrag; } - m_node->m_v+=delta/dt; + m_node->m_v+=delta/dt; } - + #define FIXED_STEP #ifdef FIXED_STEP m_dynamicsWorld->stepSimulation(dt=1.0f/60.f,0); @@ -171,13 +171,13 @@ void SoftDemo::clientMoveAndDisplay() if(m_drag) { - m_node->m_v*=0; + m_node->m_v*=0; } m_softBodyWorldInfo.m_sparsesdf.GarbageCollect(); //optional but useful: debug drawing - + } #ifdef USE_QUICKPROF @@ -229,13 +229,13 @@ void SoftDemo::displayCallback(void) { // struct ImplicitSphere : btSoftBody::ImplicitFn { -btVector3 center; -btScalar sqradius; - ImplicitSphere() {} - ImplicitSphere(const btVector3& c,btScalar r) : center(c),sqradius(r*r) {} -btScalar Eval(const btVector3& x) + btVector3 center; + btScalar sqradius; + ImplicitSphere() {} + ImplicitSphere(const btVector3& c,btScalar r) : center(c),sqradius(r*r) {} + btScalar Eval(const btVector3& x) { - return((x-center).length2()-sqradius); + return((x-center).length2()-sqradius); } }; @@ -280,7 +280,7 @@ static inline btVector3 Vector3Rand() static void Ctor_RbUpStack(SoftDemo* pdemo,int count) { float mass=10; - + btCompoundShape* cylinderCompound = new btCompoundShape; btCollisionShape* cylinderShape = new btCylinderShapeX(btVector3(4,1,1)); btCollisionShape* boxShape = new btBoxShape(btVector3(4,1,1)); @@ -289,14 +289,14 @@ static void Ctor_RbUpStack(SoftDemo* pdemo,int count) cylinderCompound->addChildShape(localTransform,boxShape); btQuaternion orn(SIMD_HALF_PI,0,0); localTransform.setRotation(orn); -// localTransform.setOrigin(btVector3(1,1,1)); + // localTransform.setOrigin(btVector3(1,1,1)); cylinderCompound->addChildShape(localTransform,cylinderShape); - + btCollisionShape* shape[]={ cylinderCompound, new btSphereShape(1.5), new btBoxShape(btVector3(1,1,1)) - }; + }; static const int nshapes=sizeof(shape)/sizeof(shape[0]); for(int i=0;im_softBodyWorldInfo,c,8); psb->generateBendingConstraints(2); pdemo->getSoftDynamicsWorld()->addSoftBody(psb); - + return(psb); } @@ -406,7 +406,7 @@ static void Init_Ropes(SoftDemo* pdemo) psb->m_materials[0]->m_kLST = 0.1+(i/(btScalar)(n-1))*0.9; psb->setTotalMass(20); pdemo->getSoftDynamicsWorld()->addSoftBody(psb); - + } } @@ -486,9 +486,9 @@ static void Init_Collide(SoftDemo* pdemo) { //TRACEDEMO struct Functor - { + { static btSoftBody* Create(SoftDemo* pdemo,const btVector3& x,const btVector3& a) - { + { btSoftBody* psb=btSoftBodyHelpers::CreateFromTriMesh(pdemo->m_softBodyWorldInfo,gVertices, &gIndices[0][0], NUM_TRIANGLES); @@ -503,12 +503,12 @@ static void Init_Collide(SoftDemo* pdemo) psb->setTotalMass(50,true); pdemo->getSoftDynamicsWorld()->addSoftBody(psb); return(psb); - } - }; - for(int i=0;i<3;++i) - { - Functor::Create(pdemo,btVector3(3*i,2,0),btVector3(SIMD_PI/2*(1-(i&1)),SIMD_PI/2*(i&1),0)); } + }; + for(int i=0;i<3;++i) + { + Functor::Create(pdemo,btVector3(3*i,2,0),btVector3(SIMD_PI/2*(1-(i&1)),SIMD_PI/2*(i&1),0)); + } pdemo->m_cutting=true; } @@ -519,9 +519,9 @@ static void Init_Collide2(SoftDemo* pdemo) { //TRACEDEMO struct Functor - { + { static btSoftBody* Create(SoftDemo* pdemo,const btVector3& x,const btVector3& a) - { + { btSoftBody* psb=btSoftBodyHelpers::CreateFromTriMesh(pdemo->m_softBodyWorldInfo,gVerticesBunny, &gIndicesBunny[0][0], BUNNY_NUM_TRIANGLES); @@ -540,12 +540,12 @@ static void Init_Collide2(SoftDemo* pdemo) psb->setTotalMass(100,true); pdemo->getSoftDynamicsWorld()->addSoftBody(psb); return(psb); - } - }; + } + }; for(int i=0;i<3;++i) - { + { Functor::Create(pdemo,btVector3(0,-1+5*i,0),btVector3(0,SIMD_PI/2*(i&1),0)); - } + } pdemo->m_cutting=true; } @@ -555,7 +555,7 @@ static void Init_Collide2(SoftDemo* pdemo) static void Init_Collide3(SoftDemo* pdemo) { //TRACEDEMO - { + { const btScalar s=8; btSoftBody* psb=btSoftBodyHelpers::CreatePatch( pdemo->m_softBodyWorldInfo,btVector3(-s,0,-s), btVector3(+s,0,-s), @@ -566,8 +566,8 @@ static void Init_Collide3(SoftDemo* pdemo) psb->m_cfg.collisions |= btSoftBody::fCollision::VF_SS; psb->setTotalMass(150); pdemo->getSoftDynamicsWorld()->addSoftBody(psb); - } - { + } + { const btScalar s=4; const btVector3 o=btVector3(5,10,0); btSoftBody* psb=btSoftBodyHelpers::CreatePatch( pdemo->m_softBodyWorldInfo, @@ -585,7 +585,7 @@ static void Init_Collide3(SoftDemo* pdemo) psb->setTotalMass(150); pdemo->getSoftDynamicsWorld()->addSoftBody(psb); pdemo->m_cutting=true; - } + } } // @@ -732,9 +732,9 @@ static void Init_Bending(SoftDemo* pdemo) //TRACEDEMO const btScalar s=4; const btVector3 x[]={ btVector3(-s,0,-s), - btVector3(+s,0,-s), - btVector3(+s,0,+s), - btVector3(-s,0,+s)}; + btVector3(+s,0,-s), + btVector3(+s,0,+s), + btVector3(-s,0,+s)}; const btScalar m[]={ 0,0,0,1}; btSoftBody* psb=new btSoftBody(&pdemo->m_softBodyWorldInfo,4,x,m); psb->appendLink(0,1); @@ -742,7 +742,7 @@ static void Init_Bending(SoftDemo* pdemo) psb->appendLink(2,3); psb->appendLink(3,0); psb->appendLink(0,2); - + pdemo->getSoftDynamicsWorld()->addSoftBody(psb); } @@ -867,9 +867,9 @@ static void Init_Cutting1(SoftDemo* pdemo) const btScalar h=2; const int r=16; const btVector3 p[]={ btVector3(+s,h,-s), - btVector3(-s,h,-s), - btVector3(+s,h,+s), - btVector3(-s,h,+s)}; + btVector3(-s,h,-s), + btVector3(+s,h,+s), + btVector3(-s,h,+s)}; btSoftBody* psb=btSoftBodyHelpers::CreatePatch(pdemo->m_softBodyWorldInfo,p[0],p[1],p[2],p[3],r,r,1+2+4+8,true); pdemo->getSoftDynamicsWorld()->addSoftBody(psb); psb->m_cfg.piterations=1; @@ -883,104 +883,104 @@ static void Init_Cutting1(SoftDemo* pdemo) // static void Ctor_Gear(SoftDemo* pdemo,const btVector3& pos,btScalar speed) { -btTransform startTransform; -startTransform.setIdentity(); -startTransform.setOrigin(pos); -btCompoundShape* shape=new btCompoundShape(); + btTransform startTransform; + startTransform.setIdentity(); + startTransform.setOrigin(pos); + btCompoundShape* shape=new btCompoundShape(); #if 1 -shape->addChildShape(btTransform(btQuaternion(0,0,0)),new btBoxShape(btVector3(5,1,6))); -shape->addChildShape(btTransform(btQuaternion(0,0,SIMD_HALF_PI)),new btBoxShape(btVector3(5,1,6))); + shape->addChildShape(btTransform(btQuaternion(0,0,0)),new btBoxShape(btVector3(5,1,6))); + shape->addChildShape(btTransform(btQuaternion(0,0,SIMD_HALF_PI)),new btBoxShape(btVector3(5,1,6))); #else -shape->addChildShape(btTransform(btQuaternion(0,0,0)),new btCylinderShapeZ(btVector3(5,1,7))); -shape->addChildShape(btTransform(btQuaternion(0,0,SIMD_HALF_PI)),new btBoxShape(btVector3(4,1,8))); + shape->addChildShape(btTransform(btQuaternion(0,0,0)),new btCylinderShapeZ(btVector3(5,1,7))); + shape->addChildShape(btTransform(btQuaternion(0,0,SIMD_HALF_PI)),new btBoxShape(btVector3(4,1,8))); #endif -btRigidBody* body=pdemo->localCreateRigidBody(10,startTransform,shape); -body->setFriction(1); -btDynamicsWorld* world=pdemo->getDynamicsWorld(); -btHingeConstraint* hinge=new btHingeConstraint(*body,btTransform::getIdentity()); -if(speed!=0) hinge->enableAngularMotor(true,speed,3); -world->addConstraint(hinge); + btRigidBody* body=pdemo->localCreateRigidBody(10,startTransform,shape); + body->setFriction(1); + btDynamicsWorld* world=pdemo->getDynamicsWorld(); + btHingeConstraint* hinge=new btHingeConstraint(*body,btTransform::getIdentity()); + if(speed!=0) hinge->enableAngularMotor(true,speed,3); + world->addConstraint(hinge); } // static btSoftBody* Ctor_ClusterBunny(SoftDemo* pdemo,const btVector3& x,const btVector3& a) { -btSoftBody* psb=btSoftBodyHelpers::CreateFromTriMesh(pdemo->m_softBodyWorldInfo,gVerticesBunny,&gIndicesBunny[0][0],BUNNY_NUM_TRIANGLES); -btSoftBody::Material* pm=psb->appendMaterial(); -pm->m_kLST = 1; -pm->m_flags -= btSoftBody::fMaterial::DebugDraw; -psb->generateBendingConstraints(2,pm); -psb->m_cfg.piterations = 2; -psb->m_cfg.kDF = 1; -psb->m_cfg.collisions = btSoftBody::fCollision::CL_SS+ - btSoftBody::fCollision::CL_RS; -psb->randomizeConstraints(); -btMatrix3x3 m; -m.setEulerZYX(a.x(),a.y(),a.z()); -psb->transform(btTransform(m,x)); -psb->scale(btVector3(8,8,8)); -psb->setTotalMass(150,true); -psb->generateClusters(1); -pdemo->getSoftDynamicsWorld()->addSoftBody(psb); -return(psb); + btSoftBody* psb=btSoftBodyHelpers::CreateFromTriMesh(pdemo->m_softBodyWorldInfo,gVerticesBunny,&gIndicesBunny[0][0],BUNNY_NUM_TRIANGLES); + btSoftBody::Material* pm=psb->appendMaterial(); + pm->m_kLST = 1; + pm->m_flags -= btSoftBody::fMaterial::DebugDraw; + psb->generateBendingConstraints(2,pm); + psb->m_cfg.piterations = 2; + psb->m_cfg.kDF = 1; + psb->m_cfg.collisions = btSoftBody::fCollision::CL_SS+ + btSoftBody::fCollision::CL_RS; + psb->randomizeConstraints(); + btMatrix3x3 m; + m.setEulerZYX(a.x(),a.y(),a.z()); + psb->transform(btTransform(m,x)); + psb->scale(btVector3(8,8,8)); + psb->setTotalMass(150,true); + psb->generateClusters(1); + pdemo->getSoftDynamicsWorld()->addSoftBody(psb); + return(psb); } // static btSoftBody* Ctor_ClusterTorus(SoftDemo* pdemo,const btVector3& x,const btVector3& a,const btVector3& s=btVector3(2,2,2)) { -btSoftBody* psb=btSoftBodyHelpers::CreateFromTriMesh(pdemo->m_softBodyWorldInfo,gVertices,&gIndices[0][0],NUM_TRIANGLES); -btSoftBody::Material* pm=psb->appendMaterial(); -pm->m_kLST = 1; -pm->m_flags -= btSoftBody::fMaterial::DebugDraw; -psb->generateBendingConstraints(2,pm); -psb->m_cfg.piterations = 2; -psb->m_cfg.collisions = btSoftBody::fCollision::CL_SS+ - btSoftBody::fCollision::CL_RS; -psb->randomizeConstraints(); -psb->scale(s); -psb->rotate(btQuaternion(a[0],a[1],a[2])); -psb->translate(x); -psb->setTotalMass(50,true); -psb->generateClusters(64); -pdemo->getSoftDynamicsWorld()->addSoftBody(psb); -return(psb); + btSoftBody* psb=btSoftBodyHelpers::CreateFromTriMesh(pdemo->m_softBodyWorldInfo,gVertices,&gIndices[0][0],NUM_TRIANGLES); + btSoftBody::Material* pm=psb->appendMaterial(); + pm->m_kLST = 1; + pm->m_flags -= btSoftBody::fMaterial::DebugDraw; + psb->generateBendingConstraints(2,pm); + psb->m_cfg.piterations = 2; + psb->m_cfg.collisions = btSoftBody::fCollision::CL_SS+ + btSoftBody::fCollision::CL_RS; + psb->randomizeConstraints(); + psb->scale(s); + psb->rotate(btQuaternion(a[0],a[1],a[2])); + psb->translate(x); + psb->setTotalMass(50,true); + psb->generateClusters(64); + pdemo->getSoftDynamicsWorld()->addSoftBody(psb); + return(psb); } // static struct MotorControl : btSoftBody::AJoint::IControl { - MotorControl() + MotorControl() { - goal=0; - maxtorque=0; + goal=0; + maxtorque=0; } -btScalar Speed(btSoftBody::AJoint*,btScalar current) + btScalar Speed(btSoftBody::AJoint*,btScalar current) { - return(current+btMin(maxtorque,btMax(-maxtorque,goal-current))); + return(current+btMin(maxtorque,btMax(-maxtorque,goal-current))); } -btScalar goal; -btScalar maxtorque; + btScalar goal; + btScalar maxtorque; } motorcontrol; // struct SteerControl : btSoftBody::AJoint::IControl { - SteerControl(btScalar s) + SteerControl(btScalar s) { - angle=0; - sign=s; + angle=0; + sign=s; } -void Prepare(btSoftBody::AJoint* joint) + void Prepare(btSoftBody::AJoint* joint) { - joint->m_refs[0][0]=btCos(angle*sign); - joint->m_refs[0][2]=btSin(angle*sign); + joint->m_refs[0][0]=btCos(angle*sign); + joint->m_refs[0][2]=btSin(angle*sign); } -btScalar Speed(btSoftBody::AJoint* joint,btScalar current) + btScalar Speed(btSoftBody::AJoint* joint,btScalar current) { - return(motorcontrol.Speed(joint,current)); + return(motorcontrol.Speed(joint,current)); } -btScalar angle; -btScalar sign; + btScalar angle; + btScalar sign; }; static SteerControl steercontrol_f(+1); @@ -989,259 +989,259 @@ static SteerControl steercontrol_r(-1); // static void Init_ClusterDeform(SoftDemo* pdemo) { -btSoftBody* psb=Ctor_ClusterTorus(pdemo,btVector3(0,0,0),btVector3(SIMD_PI/2,0,SIMD_HALF_PI)); -psb->generateClusters(8); -psb->m_cfg.kDF=1; + btSoftBody* psb=Ctor_ClusterTorus(pdemo,btVector3(0,0,0),btVector3(SIMD_PI/2,0,SIMD_HALF_PI)); + psb->generateClusters(8); + psb->m_cfg.kDF=1; } // static void Init_ClusterCollide1(SoftDemo* pdemo) { -const btScalar s=8; -btSoftBody* psb=btSoftBodyHelpers::CreatePatch( pdemo->m_softBodyWorldInfo,btVector3(-s,0,-s), - btVector3(+s,0,-s), - btVector3(-s,0,+s), - btVector3(+s,0,+s), - 31,31, - 1+2+4+8,true); -btSoftBody::Material* pm=psb->appendMaterial(); -pm->m_kLST = 0.4; -pm->m_flags -= btSoftBody::fMaterial::DebugDraw; -psb->m_cfg.kDF = 1; -psb->m_cfg.kSRHR_CL = 1; -psb->m_cfg.kSR_SPLT_CL = 0; -psb->m_cfg.collisions = btSoftBody::fCollision::CL_SS+ - btSoftBody::fCollision::CL_RS; -psb->generateBendingConstraints(2,pm); -psb->setTotalMass(50); -psb->generateClusters(64); -pdemo->getSoftDynamicsWorld()->addSoftBody(psb); + const btScalar s=8; + btSoftBody* psb=btSoftBodyHelpers::CreatePatch( pdemo->m_softBodyWorldInfo,btVector3(-s,0,-s), + btVector3(+s,0,-s), + btVector3(-s,0,+s), + btVector3(+s,0,+s), + 31,31, + 1+2+4+8,true); + btSoftBody::Material* pm=psb->appendMaterial(); + pm->m_kLST = 0.4; + pm->m_flags -= btSoftBody::fMaterial::DebugDraw; + psb->m_cfg.kDF = 1; + psb->m_cfg.kSRHR_CL = 1; + psb->m_cfg.kSR_SPLT_CL = 0; + psb->m_cfg.collisions = btSoftBody::fCollision::CL_SS+ + btSoftBody::fCollision::CL_RS; + psb->generateBendingConstraints(2,pm); + psb->setTotalMass(50); + psb->generateClusters(64); + pdemo->getSoftDynamicsWorld()->addSoftBody(psb); -Ctor_RbUpStack(pdemo,10); + Ctor_RbUpStack(pdemo,10); } // static void Init_ClusterCollide2(SoftDemo* pdemo) { -struct Functor + struct Functor { - static btSoftBody* Create(SoftDemo* pdemo,const btVector3& x,const btVector3& a) + static btSoftBody* Create(SoftDemo* pdemo,const btVector3& x,const btVector3& a) { - btSoftBody* psb=btSoftBodyHelpers::CreateFromTriMesh(pdemo->m_softBodyWorldInfo,gVertices, - &gIndices[0][0], - NUM_TRIANGLES); - btSoftBody::Material* pm=psb->appendMaterial(); - pm->m_flags -= btSoftBody::fMaterial::DebugDraw; - psb->generateBendingConstraints(2,pm); - psb->m_cfg.piterations=2; - psb->m_cfg.kDF =1; - psb->m_cfg.kSSHR_CL =1; - psb->m_cfg.kSS_SPLT_CL =0; - psb->m_cfg.kSKHR_CL =0.1f; - psb->m_cfg.kSK_SPLT_CL =1; - psb->m_cfg.collisions= btSoftBody::fCollision::CL_SS+ - btSoftBody::fCollision::CL_RS; - psb->randomizeConstraints(); - btMatrix3x3 m; - m.setEulerZYX(a.x(),a.y(),a.z()); - psb->transform(btTransform(m,x)); - psb->scale(btVector3(2,2,2)); - psb->setTotalMass(50,true); - psb->generateClusters(16); - pdemo->getSoftDynamicsWorld()->addSoftBody(psb); - return(psb); + btSoftBody* psb=btSoftBodyHelpers::CreateFromTriMesh(pdemo->m_softBodyWorldInfo,gVertices, + &gIndices[0][0], + NUM_TRIANGLES); + btSoftBody::Material* pm=psb->appendMaterial(); + pm->m_flags -= btSoftBody::fMaterial::DebugDraw; + psb->generateBendingConstraints(2,pm); + psb->m_cfg.piterations=2; + psb->m_cfg.kDF =1; + psb->m_cfg.kSSHR_CL =1; + psb->m_cfg.kSS_SPLT_CL =0; + psb->m_cfg.kSKHR_CL =0.1f; + psb->m_cfg.kSK_SPLT_CL =1; + psb->m_cfg.collisions= btSoftBody::fCollision::CL_SS+ + btSoftBody::fCollision::CL_RS; + psb->randomizeConstraints(); + btMatrix3x3 m; + m.setEulerZYX(a.x(),a.y(),a.z()); + psb->transform(btTransform(m,x)); + psb->scale(btVector3(2,2,2)); + psb->setTotalMass(50,true); + psb->generateClusters(16); + pdemo->getSoftDynamicsWorld()->addSoftBody(psb); + return(psb); } }; -for(int i=0;i<3;++i) + for(int i=0;i<3;++i) { - Functor::Create(pdemo,btVector3(3*i,2,0),btVector3(SIMD_PI/2*(1-(i&1)),SIMD_PI/2*(i&1),0)); + Functor::Create(pdemo,btVector3(3*i,2,0),btVector3(SIMD_PI/2*(1-(i&1)),SIMD_PI/2*(i&1),0)); } } // static void Init_ClusterSocket(SoftDemo* pdemo) { -btSoftBody* psb=Ctor_ClusterTorus(pdemo,btVector3(0,0,0),btVector3(SIMD_PI/2,0,SIMD_HALF_PI)); -btRigidBody* prb=Ctor_BigPlate(pdemo,50,8); -psb->m_cfg.kDF=1; -btSoftBody::LJoint::Specs lj; -lj.position = btVector3(0,5,0); -psb->appendLinearJoint(lj,prb); + btSoftBody* psb=Ctor_ClusterTorus(pdemo,btVector3(0,0,0),btVector3(SIMD_PI/2,0,SIMD_HALF_PI)); + btRigidBody* prb=Ctor_BigPlate(pdemo,50,8); + psb->m_cfg.kDF=1; + btSoftBody::LJoint::Specs lj; + lj.position = btVector3(0,5,0); + psb->appendLinearJoint(lj,prb); } // static void Init_ClusterHinge(SoftDemo* pdemo) { -btSoftBody* psb=Ctor_ClusterTorus(pdemo,btVector3(0,0,0),btVector3(SIMD_PI/2,0,SIMD_HALF_PI)); -btRigidBody* prb=Ctor_BigPlate(pdemo,50,8); -psb->m_cfg.kDF=1; -btSoftBody::AJoint::Specs aj; -aj.axis = btVector3(0,0,1); -psb->appendAngularJoint(aj,prb); + btSoftBody* psb=Ctor_ClusterTorus(pdemo,btVector3(0,0,0),btVector3(SIMD_PI/2,0,SIMD_HALF_PI)); + btRigidBody* prb=Ctor_BigPlate(pdemo,50,8); + psb->m_cfg.kDF=1; + btSoftBody::AJoint::Specs aj; + aj.axis = btVector3(0,0,1); + psb->appendAngularJoint(aj,prb); } // static void Init_ClusterCombine(SoftDemo* pdemo) { -const btVector3 sz(2,4,2); -btSoftBody* psb0=Ctor_ClusterTorus(pdemo,btVector3(0,8,0),btVector3(SIMD_PI/2,0,SIMD_HALF_PI),sz); -btSoftBody* psb1=Ctor_ClusterTorus(pdemo,btVector3(0,8,10),btVector3(SIMD_PI/2,0,SIMD_HALF_PI),sz); -btSoftBody* psbs[]={psb0,psb1}; -for(int j=0;j<2;++j) + const btVector3 sz(2,4,2); + btSoftBody* psb0=Ctor_ClusterTorus(pdemo,btVector3(0,8,0),btVector3(SIMD_PI/2,0,SIMD_HALF_PI),sz); + btSoftBody* psb1=Ctor_ClusterTorus(pdemo,btVector3(0,8,10),btVector3(SIMD_PI/2,0,SIMD_HALF_PI),sz); + btSoftBody* psbs[]={psb0,psb1}; + for(int j=0;j<2;++j) { - psbs[j]->m_cfg.kDF=1; - psbs[j]->m_cfg.kDP=0; - psbs[j]->m_cfg.piterations=1; - psbs[j]->m_clusters[0]->m_matching = 0.05; - psbs[j]->m_clusters[0]->m_ndamping = 0.05; + psbs[j]->m_cfg.kDF=1; + psbs[j]->m_cfg.kDP=0; + psbs[j]->m_cfg.piterations=1; + psbs[j]->m_clusters[0]->m_matching = 0.05; + psbs[j]->m_clusters[0]->m_ndamping = 0.05; } -btSoftBody::AJoint::Specs aj; -aj.axis = btVector3(0,0,1); -aj.icontrol = &motorcontrol; -psb0->appendAngularJoint(aj,psb1); + btSoftBody::AJoint::Specs aj; + aj.axis = btVector3(0,0,1); + aj.icontrol = &motorcontrol; + psb0->appendAngularJoint(aj,psb1); -btSoftBody::LJoint::Specs lj; -lj.position = btVector3(0,8,5); -psb0->appendLinearJoint(lj,psb1); + btSoftBody::LJoint::Specs lj; + lj.position = btVector3(0,8,5); + psb0->appendLinearJoint(lj,psb1); } // static void Init_ClusterCar(SoftDemo* pdemo) { pdemo->setAzi(270); -const btVector3 origin(100,80,0); -const btQuaternion orientation(-SIMD_PI/2,0,0); -const btScalar widthf=8; -const btScalar widthr=9; -const btScalar length=8; -const btScalar height=4; -const btVector3 wheels[]= { - btVector3(+widthf,-height,+length), // Front left - btVector3(-widthf,-height,+length), // Front right - btVector3(+widthr,-height,-length), // Rear left - btVector3(-widthr,-height,-length), // Rear right - }; -btSoftBody* pa=Ctor_ClusterBunny(pdemo,btVector3(0,0,0),btVector3(0,0,0)); -btSoftBody* pfl=Ctor_ClusterTorus(pdemo,wheels[0],btVector3(0,0,SIMD_HALF_PI),btVector3(2,4,2)); -btSoftBody* pfr=Ctor_ClusterTorus(pdemo,wheels[1],btVector3(0,0,SIMD_HALF_PI),btVector3(2,4,2)); -btSoftBody* prl=Ctor_ClusterTorus(pdemo,wheels[2],btVector3(0,0,SIMD_HALF_PI),btVector3(2,5,2)); -btSoftBody* prr=Ctor_ClusterTorus(pdemo,wheels[3],btVector3(0,0,SIMD_HALF_PI),btVector3(2,5,2)); + const btVector3 origin(100,80,0); + const btQuaternion orientation(-SIMD_PI/2,0,0); + const btScalar widthf=8; + const btScalar widthr=9; + const btScalar length=8; + const btScalar height=4; + const btVector3 wheels[]= { + btVector3(+widthf,-height,+length), // Front left + btVector3(-widthf,-height,+length), // Front right + btVector3(+widthr,-height,-length), // Rear left + btVector3(-widthr,-height,-length), // Rear right + }; + btSoftBody* pa=Ctor_ClusterBunny(pdemo,btVector3(0,0,0),btVector3(0,0,0)); + btSoftBody* pfl=Ctor_ClusterTorus(pdemo,wheels[0],btVector3(0,0,SIMD_HALF_PI),btVector3(2,4,2)); + btSoftBody* pfr=Ctor_ClusterTorus(pdemo,wheels[1],btVector3(0,0,SIMD_HALF_PI),btVector3(2,4,2)); + btSoftBody* prl=Ctor_ClusterTorus(pdemo,wheels[2],btVector3(0,0,SIMD_HALF_PI),btVector3(2,5,2)); + btSoftBody* prr=Ctor_ClusterTorus(pdemo,wheels[3],btVector3(0,0,SIMD_HALF_PI),btVector3(2,5,2)); -pfl->m_cfg.kDF = -pfr->m_cfg.kDF = -prl->m_cfg.kDF = -prr->m_cfg.kDF = 1; + pfl->m_cfg.kDF = + pfr->m_cfg.kDF = + prl->m_cfg.kDF = + prr->m_cfg.kDF = 1; -btSoftBody::LJoint::Specs lspecs; -lspecs.cfm = 1; -lspecs.erp = 1; -lspecs.position = btVector3(0,0,0); + btSoftBody::LJoint::Specs lspecs; + lspecs.cfm = 1; + lspecs.erp = 1; + lspecs.position = btVector3(0,0,0); -lspecs.position=wheels[0];pa->appendLinearJoint(lspecs,pfl); -lspecs.position=wheels[1];pa->appendLinearJoint(lspecs,pfr); -lspecs.position=wheels[2];pa->appendLinearJoint(lspecs,prl); -lspecs.position=wheels[3];pa->appendLinearJoint(lspecs,prr); + lspecs.position=wheels[0];pa->appendLinearJoint(lspecs,pfl); + lspecs.position=wheels[1];pa->appendLinearJoint(lspecs,pfr); + lspecs.position=wheels[2];pa->appendLinearJoint(lspecs,prl); + lspecs.position=wheels[3];pa->appendLinearJoint(lspecs,prr); -btSoftBody::AJoint::Specs aspecs; -aspecs.cfm = 1; -aspecs.erp = 1; -aspecs.axis = btVector3(1,0,0); + btSoftBody::AJoint::Specs aspecs; + aspecs.cfm = 1; + aspecs.erp = 1; + aspecs.axis = btVector3(1,0,0); -aspecs.icontrol = &steercontrol_f; -pa->appendAngularJoint(aspecs,pfl); -pa->appendAngularJoint(aspecs,pfr); + aspecs.icontrol = &steercontrol_f; + pa->appendAngularJoint(aspecs,pfl); + pa->appendAngularJoint(aspecs,pfr); -aspecs.icontrol = &motorcontrol; -pa->appendAngularJoint(aspecs,prl); -pa->appendAngularJoint(aspecs,prr); + aspecs.icontrol = &motorcontrol; + pa->appendAngularJoint(aspecs,prl); + pa->appendAngularJoint(aspecs,prr); -pa->rotate(orientation); -pfl->rotate(orientation); -pfr->rotate(orientation); -prl->rotate(orientation); -prr->rotate(orientation); -pa->translate(origin); -pfl->translate(origin); -pfr->translate(origin); -prl->translate(origin); -prr->translate(origin); -pfl->m_cfg.piterations = -pfr->m_cfg.piterations = -prl->m_cfg.piterations = -prr->m_cfg.piterations = 1; -pfl->m_clusters[0]->m_matching = -pfr->m_clusters[0]->m_matching = -prl->m_clusters[0]->m_matching = -prr->m_clusters[0]->m_matching = 0.05; -pfl->m_clusters[0]->m_ndamping = -pfr->m_clusters[0]->m_ndamping = -prl->m_clusters[0]->m_ndamping = -prr->m_clusters[0]->m_ndamping = 0.05; - -Ctor_LinearStair(pdemo,btVector3(0,-8,0),btVector3(3,2,40),0,20); -Ctor_RbUpStack(pdemo,50); -pdemo->m_autocam=true; + pa->rotate(orientation); + pfl->rotate(orientation); + pfr->rotate(orientation); + prl->rotate(orientation); + prr->rotate(orientation); + pa->translate(origin); + pfl->translate(origin); + pfr->translate(origin); + prl->translate(origin); + prr->translate(origin); + pfl->m_cfg.piterations = + pfr->m_cfg.piterations = + prl->m_cfg.piterations = + prr->m_cfg.piterations = 1; + pfl->m_clusters[0]->m_matching = + pfr->m_clusters[0]->m_matching = + prl->m_clusters[0]->m_matching = + prr->m_clusters[0]->m_matching = 0.05; + pfl->m_clusters[0]->m_ndamping = + pfr->m_clusters[0]->m_ndamping = + prl->m_clusters[0]->m_ndamping = + prr->m_clusters[0]->m_ndamping = 0.05; + + Ctor_LinearStair(pdemo,btVector3(0,-8,0),btVector3(3,2,40),0,20); + Ctor_RbUpStack(pdemo,50); + pdemo->m_autocam=true; } // static void Init_ClusterRobot(SoftDemo* pdemo) { -struct Functor + struct Functor { - static btSoftBody* CreateBall(SoftDemo* pdemo,const btVector3& pos) + static btSoftBody* CreateBall(SoftDemo* pdemo,const btVector3& pos) { - btSoftBody* psb=btSoftBodyHelpers::CreateEllipsoid(pdemo->m_softBodyWorldInfo,pos,btVector3(1,1,1)*3,512); - psb->m_materials[0]->m_kLST = 0.45; - psb->m_cfg.kVC = 20; - psb->setTotalMass(50,true); - psb->setPose(true,false); - psb->generateClusters(1); - pdemo->getSoftDynamicsWorld()->addSoftBody(psb); - return(psb); + btSoftBody* psb=btSoftBodyHelpers::CreateEllipsoid(pdemo->m_softBodyWorldInfo,pos,btVector3(1,1,1)*3,512); + psb->m_materials[0]->m_kLST = 0.45; + psb->m_cfg.kVC = 20; + psb->setTotalMass(50,true); + psb->setPose(true,false); + psb->generateClusters(1); + pdemo->getSoftDynamicsWorld()->addSoftBody(psb); + return(psb); } }; -const btVector3 base=btVector3(0,25,8); -btSoftBody* psb0=Functor::CreateBall(pdemo,base+btVector3(-8,0,0)); -btSoftBody* psb1=Functor::CreateBall(pdemo,base+btVector3(+8,0,0)); -btSoftBody* psb2=Functor::CreateBall(pdemo,base+btVector3(0,0,+8*btSqrt(2))); -const btVector3 ctr=(psb0->clusterCom(0)+psb1->clusterCom(0)+psb2->clusterCom(0))/3; -btCylinderShape* pshp=new btCylinderShape(btVector3(8,1,8)); -btRigidBody* prb=pdemo->localCreateRigidBody(50,btTransform(btQuaternion(0,0,0),ctr+btVector3(0,5,0)),pshp); -btSoftBody::LJoint::Specs ls; -ls.erp=0.5f; -ls.position=psb0->clusterCom(0);psb0->appendLinearJoint(ls,prb); -ls.position=psb1->clusterCom(0);psb1->appendLinearJoint(ls,prb); -ls.position=psb2->clusterCom(0);psb2->appendLinearJoint(ls,prb); + const btVector3 base=btVector3(0,25,8); + btSoftBody* psb0=Functor::CreateBall(pdemo,base+btVector3(-8,0,0)); + btSoftBody* psb1=Functor::CreateBall(pdemo,base+btVector3(+8,0,0)); + btSoftBody* psb2=Functor::CreateBall(pdemo,base+btVector3(0,0,+8*btSqrt(2))); + const btVector3 ctr=(psb0->clusterCom(0)+psb1->clusterCom(0)+psb2->clusterCom(0))/3; + btCylinderShape* pshp=new btCylinderShape(btVector3(8,1,8)); + btRigidBody* prb=pdemo->localCreateRigidBody(50,btTransform(btQuaternion(0,0,0),ctr+btVector3(0,5,0)),pshp); + btSoftBody::LJoint::Specs ls; + ls.erp=0.5f; + ls.position=psb0->clusterCom(0);psb0->appendLinearJoint(ls,prb); + ls.position=psb1->clusterCom(0);psb1->appendLinearJoint(ls,prb); + ls.position=psb2->clusterCom(0);psb2->appendLinearJoint(ls,prb); -btBoxShape* pbox=new btBoxShape(btVector3(20,1,40)); -btRigidBody* pgrn=pdemo->localCreateRigidBody(0,btTransform(btQuaternion(0,-SIMD_HALF_PI/2,0),btVector3(0,0,0)),pbox); + btBoxShape* pbox=new btBoxShape(btVector3(20,1,40)); + btRigidBody* pgrn=pdemo->localCreateRigidBody(0,btTransform(btQuaternion(0,-SIMD_HALF_PI/2,0),btVector3(0,0,0)),pbox); -pdemo->m_autocam=true; + pdemo->m_autocam=true; } // static void Init_ClusterStackSoft(SoftDemo* pdemo) { -for(int i=0;i<10;++i) + for(int i=0;i<10;++i) { - btSoftBody* psb=Ctor_ClusterTorus(pdemo,btVector3(0,-9+8.25*i,0),btVector3(0,0,0)); - psb->m_cfg.kDF=1; + btSoftBody* psb=Ctor_ClusterTorus(pdemo,btVector3(0,-9+8.25*i,0),btVector3(0,0,0)); + psb->m_cfg.kDF=1; } } // static void Init_ClusterStackMixed(SoftDemo* pdemo) { -for(int i=0;i<10;++i) + for(int i=0;i<10;++i) { - if((i+1)&1) + if((i+1)&1) { - Ctor_BigPlate(pdemo,50,-9+4.25*i); + Ctor_BigPlate(pdemo,50,-9+4.25*i); } else { - btSoftBody* psb=Ctor_ClusterTorus(pdemo,btVector3(0,-9+4.25*i,0),btVector3(0,0,0)); - psb->m_cfg.kDF=1; + btSoftBody* psb=Ctor_ClusterTorus(pdemo,btVector3(0,-9+4.25*i,0),btVector3(0,0,0)); + psb->m_cfg.kDF=1; } } } @@ -1261,11 +1261,11 @@ void SoftDemo::clientResetScene() delete body->getMotionState(); } while(m_dynamicsWorld->getNumConstraints()) - { + { btTypedConstraint* pc=m_dynamicsWorld->getConstraint(0); m_dynamicsWorld->removeConstraint(pc); delete pc; - } + } btSoftBody* softBody = btSoftBody::upcast(obj); if (softBody) { @@ -1276,7 +1276,7 @@ void SoftDemo::clientResetScene() } delete obj; } - + m_softBodyWorldInfo.m_sparsesdf.Reset(); /* Init */ void (*demofncs[])(SoftDemo*)= @@ -1311,7 +1311,7 @@ void SoftDemo::clientResetScene() Init_ClusterStackMixed, }; current_demo=current_demo%(sizeof(demofncs)/sizeof(demofncs[0])); - + m_softBodyWorldInfo.air_density = (btScalar)1.2; m_softBodyWorldInfo.water_density = 0; m_softBodyWorldInfo.water_offset = 0; @@ -1329,13 +1329,13 @@ void SoftDemo::clientResetScene() void SoftDemo::renderme() { btIDebugDraw* idraw=m_dynamicsWorld->getDebugDrawer(); - + m_dynamicsWorld->debugDrawWorld(); - + /* Bodies */ btVector3 ps(0,0,0); int nps=0; - + btSoftBodyArray& sbs=getSoftDynamicsWorld()->getSoftBodyArray(); for(int ib=0;ibrayTest(rayFrom,rayTo,results)) - { - *fraction=results.fraction; - } + { + *fraction=results.fraction; } - ++org;++fraction; } + ++org;++fraction; + } long ms=btMax(m_clock.getTimeMilliseconds(),1); long rayperseconds=(1000*(origins.size()*sbs.size()))/ms; printf("%d ms (%d rays/s)\r\n",ms,rayperseconds); - } + } /* Draw rays */ const btVector3 c[]={ origins[0], - origins[res-1], - origins[res*(res-1)], - origins[res*(res-1)+res-1]}; + origins[res-1], + origins[res*(res-1)], + origins[res*(res-1)+res-1]}; idraw->drawLine(c[0],c[1],btVector3(0,0,0)); idraw->drawLine(c[1],c[3],btVector3(0,0,0)); idraw->drawLine(c[3],c[2],btVector3(0,0,0)); idraw->drawLine(c[2],c[0],btVector3(0,0,0)); for(int i=0,ni=origins.size();idrawLine(org,org+dir*rayLength*fraction,btVector3(1,0,0)); - } - else - { - idraw->drawLine(org,org-dir*rayLength*0.1,btVector3(0,0,0)); - } } - #undef RES + else + { + idraw->drawLine(org,org-dir*rayLength*0.1,btVector3(0,0,0)); + } } +#undef RES + } /* Water level */ static const btVector3 axis[]={btVector3(1,0,0), btVector3(0,1,0), @@ -1448,7 +1448,7 @@ void SoftDemo::renderme() } // DemoApplication::renderme(); - + } void SoftDemo::setDrawClusters(bool drawClusters) @@ -1479,14 +1479,14 @@ void SoftDemo::keyboardCallback(unsigned char key, int x, int y) case 'c': getSoftDynamicsWorld()->setDrawFlags(getSoftDynamicsWorld()->getDrawFlags()^fDrawFlags::Clusters);break; case '`': { - btSoftBodyArray& sbs=getSoftDynamicsWorld()->getSoftBodyArray(); - for(int ib=0;ibgetSoftBodyArray(); + for(int ib=0;ibstaticSolve(128); + btSoftBody* psb=sbs[ib]; + psb->staticSolve(128); } } - break; + break; default: DemoApplication::keyboardCallback(key,x,y); } } @@ -1494,109 +1494,109 @@ void SoftDemo::keyboardCallback(unsigned char key, int x, int y) // void SoftDemo::mouseMotionFunc(int x,int y) { -if(m_node&&(m_results.fraction<1.f)) + if(m_node&&(m_results.fraction<1.f)) { - if(!m_drag) + if(!m_drag) { - #define SQ(_x_) (_x_)*(_x_) - if((SQ(x-m_lastmousepos[0])+SQ(y-m_lastmousepos[1]))>6) +#define SQ(_x_) (_x_)*(_x_) + if((SQ(x-m_lastmousepos[0])+SQ(y-m_lastmousepos[1]))>6) { - m_drag=true; + m_drag=true; } - #undef SQ +#undef SQ } - if(m_drag) + if(m_drag) { - m_lastmousepos[0] = x; - m_lastmousepos[1] = y; + m_lastmousepos[0] = x; + m_lastmousepos[1] = y; } } else { - DemoApplication::mouseMotionFunc(x,y); + DemoApplication::mouseMotionFunc(x,y); } } // void SoftDemo::mouseFunc(int button, int state, int x, int y) { -if(button==0) + if(button==0) { - switch(state) + switch(state) { case 0: { - m_results.fraction=1.f; - DemoApplication::mouseFunc(button,state,x,y); - if(!m_pickConstraint) + m_results.fraction=1.f; + DemoApplication::mouseFunc(button,state,x,y); + if(!m_pickConstraint) { - const btVector3 rayFrom=m_cameraPosition; - const btVector3 rayTo=getRayTo(x,y); - const btVector3 rayDir=(rayTo-rayFrom).normalized(); - btSoftBodyArray& sbs=getSoftDynamicsWorld()->getSoftBodyArray(); - for(int ib=0;ibgetSoftBodyArray(); + for(int ib=0;ibrayTest(rayFrom,rayTo,res)) + btSoftBody* psb=sbs[ib]; + btSoftBody::sRayCast res; + if(psb->rayTest(rayFrom,rayTo,res)) { - m_results=res; + m_results=res; } } - if(m_results.fraction<1.f) + if(m_results.fraction<1.f) { - m_impact = rayFrom+(rayTo-rayFrom)*m_results.fraction; - m_drag = false; - m_lastmousepos[0] = x; - m_lastmousepos[1] = y; - m_node = 0; - switch(m_results.feature) + m_impact = rayFrom+(rayTo-rayFrom)*m_results.fraction; + m_drag = false; + m_lastmousepos[0] = x; + m_lastmousepos[1] = y; + m_node = 0; + switch(m_results.feature) { case btSoftBody::eFeature::Face: { - btSoftBody::Face& f=m_results.body->m_faces[m_results.index]; - m_node=f.m_n[0]; - for(int i=1;i<3;++i) + btSoftBody::Face& f=m_results.body->m_faces[m_results.index]; + m_node=f.m_n[0]; + for(int i=1;i<3;++i) { - if( (m_node->m_x-m_impact).length2()> - (f.m_n[i]->m_x-m_impact).length2()) + if( (m_node->m_x-m_impact).length2()> + (f.m_n[i]->m_x-m_impact).length2()) { - m_node=f.m_n[i]; + m_node=f.m_n[i]; } } } - break; + break; } - if(m_node) m_goal=m_node->m_x; - return; + if(m_node) m_goal=m_node->m_x; + return; } } } - break; + break; case 1: - if((!m_drag)&&m_cutting&&(m_results.fraction<1.f)) + if((!m_drag)&&m_cutting&&(m_results.fraction<1.f)) { - ImplicitSphere isphere(m_impact,1); - printf("Mass before: %f\r\n",m_results.body->getTotalMass()); - m_results.body->refine(&isphere,0.0001,true); - printf("Mass after: %f\r\n",m_results.body->getTotalMass()); + ImplicitSphere isphere(m_impact,1); + printf("Mass before: %f\r\n",m_results.body->getTotalMass()); + m_results.body->refine(&isphere,0.0001,true); + printf("Mass after: %f\r\n",m_results.body->getTotalMass()); } - m_results.fraction=1.f; - m_drag=false; - DemoApplication::mouseFunc(button,state,x,y); - break; + m_results.fraction=1.f; + m_drag=false; + DemoApplication::mouseFunc(button,state,x,y); + break; } } else { - DemoApplication::mouseFunc(button,state,x,y); + DemoApplication::mouseFunc(button,state,x,y); } } void SoftDemo::initPhysics() { -///create concave ground mesh + ///create concave ground mesh //reset and disable motorcontrol at the start motorcontrol.goal = 0; @@ -1634,7 +1634,7 @@ void SoftDemo::initPhysics() int vertStride = sizeof(btVector3); int indexStride = 3*sizeof(int); - + int index=0; for ( i=0;iaddChildShape(localTransform,cylinderShape); - + m_collisionShapes.push_back(cylinderCompound); diff --git a/src/BulletCollision/BroadphaseCollision/btDbvt.cpp b/src/BulletCollision/BroadphaseCollision/btDbvt.cpp index 9bbd9b098..18f9c2e41 100644 --- a/src/BulletCollision/BroadphaseCollision/btDbvt.cpp +++ b/src/BulletCollision/BroadphaseCollision/btDbvt.cpp @@ -23,188 +23,188 @@ typedef btAlignedObjectArray tConstNodeArray; // struct btDbvtNodeEnumerator : btDbvt::ICollide { -tConstNodeArray nodes; -void Process(const btDbvtNode* n) { nodes.push_back(n); } + tConstNodeArray nodes; + void Process(const btDbvtNode* n) { nodes.push_back(n); } }; // static DBVT_INLINE int indexof(const btDbvtNode* node) { -return(node->parent->childs[1]==node); + return(node->parent->childs[1]==node); } // static DBVT_INLINE btDbvtVolume merge( const btDbvtVolume& a, - const btDbvtVolume& b) + const btDbvtVolume& b) { #if DBVT_MERGE_IMPL==DBVT_IMPL_SSE -DBVT_ALIGN char locals[sizeof(btDbvtAabbMm)]; -btDbvtVolume& res=*(btDbvtVolume*)locals; + DBVT_ALIGN char locals[sizeof(btDbvtAabbMm)]; + btDbvtVolume& res=*(btDbvtVolume*)locals; #else -btDbvtVolume res; + btDbvtVolume res; #endif -Merge(a,b,res); -return(res); + Merge(a,b,res); + return(res); } // volume+edge lengths static DBVT_INLINE btScalar size(const btDbvtVolume& a) { -const btVector3 edges=a.Lengths(); -return( edges.x()*edges.y()*edges.z()+ + const btVector3 edges=a.Lengths(); + return( edges.x()*edges.y()*edges.z()+ edges.x()+edges.y()+edges.z()); } // static void getmaxdepth(const btDbvtNode* node,int depth,int& maxdepth) { -if(node->isinternal()) + if(node->isinternal()) { - getmaxdepth(node->childs[0],depth+1,maxdepth); - getmaxdepth(node->childs[0],depth+1,maxdepth); + getmaxdepth(node->childs[0],depth+1,maxdepth); + getmaxdepth(node->childs[0],depth+1,maxdepth); } else maxdepth=btMax(maxdepth,depth); } // static DBVT_INLINE void deletenode( btDbvt* pdbvt, - btDbvtNode* node) + btDbvtNode* node) { -btAlignedFree(pdbvt->m_free); -pdbvt->m_free=node; + btAlignedFree(pdbvt->m_free); + pdbvt->m_free=node; } - + // static void recursedeletenode( btDbvt* pdbvt, - btDbvtNode* node) + btDbvtNode* node) { -if(!node->isleaf()) + if(!node->isleaf()) { - recursedeletenode(pdbvt,node->childs[0]); - recursedeletenode(pdbvt,node->childs[1]); + recursedeletenode(pdbvt,node->childs[0]); + recursedeletenode(pdbvt,node->childs[1]); } -if(node==pdbvt->m_root) pdbvt->m_root=0; -deletenode(pdbvt,node); + if(node==pdbvt->m_root) pdbvt->m_root=0; + deletenode(pdbvt,node); } // static DBVT_INLINE btDbvtNode* createnode( btDbvt* pdbvt, - btDbvtNode* parent, - void* data) + btDbvtNode* parent, + void* data) { -btDbvtNode* node; -if(pdbvt->m_free) + btDbvtNode* node; + if(pdbvt->m_free) { node=pdbvt->m_free;pdbvt->m_free=0; } else { node=new(btAlignedAlloc(sizeof(btDbvtNode),16)) btDbvtNode(); } -node->parent = parent; -node->data = data; -node->childs[1] = 0; -return(node); + node->parent = parent; + node->data = data; + node->childs[1] = 0; + return(node); } // static DBVT_INLINE btDbvtNode* createnode( btDbvt* pdbvt, - btDbvtNode* parent, - const btDbvtVolume& volume, - void* data) + btDbvtNode* parent, + const btDbvtVolume& volume, + void* data) { -btDbvtNode* node=createnode(pdbvt,parent,data); -node->volume=volume; -return(node); + btDbvtNode* node=createnode(pdbvt,parent,data); + node->volume=volume; + return(node); } // static DBVT_INLINE btDbvtNode* createnode( btDbvt* pdbvt, - btDbvtNode* parent, - const btDbvtVolume& volume0, - const btDbvtVolume& volume1, - void* data) + btDbvtNode* parent, + const btDbvtVolume& volume0, + const btDbvtVolume& volume1, + void* data) { -btDbvtNode* node=createnode(pdbvt,parent,data); -Merge(volume0,volume1,node->volume); -return(node); + btDbvtNode* node=createnode(pdbvt,parent,data); + Merge(volume0,volume1,node->volume); + return(node); } // static void insertleaf( btDbvt* pdbvt, - btDbvtNode* root, - btDbvtNode* leaf) + btDbvtNode* root, + btDbvtNode* leaf) { -if(!pdbvt->m_root) + if(!pdbvt->m_root) { - pdbvt->m_root = leaf; - leaf->parent = 0; + pdbvt->m_root = leaf; + leaf->parent = 0; } else { - if(!root->isleaf()) + if(!root->isleaf()) { - do { - root=root->childs[Select( leaf->volume, - root->childs[0]->volume, - root->childs[1]->volume)]; + do { + root=root->childs[Select( leaf->volume, + root->childs[0]->volume, + root->childs[1]->volume)]; } while(!root->isleaf()); } - btDbvtNode* prev=root->parent; - btDbvtNode* node=createnode(pdbvt,prev,leaf->volume,root->volume,0); - if(prev) + btDbvtNode* prev=root->parent; + btDbvtNode* node=createnode(pdbvt,prev,leaf->volume,root->volume,0); + if(prev) { - prev->childs[indexof(root)] = node; - node->childs[0] = root;root->parent=node; - node->childs[1] = leaf;leaf->parent=node; - do { - if(!prev->volume.Contain(node->volume)) - Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume); + prev->childs[indexof(root)] = node; + node->childs[0] = root;root->parent=node; + node->childs[1] = leaf;leaf->parent=node; + do { + if(!prev->volume.Contain(node->volume)) + Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume); else - break; - node=prev; + break; + node=prev; } while(0!=(prev=node->parent)); } else { - node->childs[0] = root;root->parent=node; - node->childs[1] = leaf;leaf->parent=node; - pdbvt->m_root = node; + node->childs[0] = root;root->parent=node; + node->childs[1] = leaf;leaf->parent=node; + pdbvt->m_root = node; } } } - + // static btDbvtNode* removeleaf( btDbvt* pdbvt, - btDbvtNode* leaf) + btDbvtNode* leaf) { -if(leaf==pdbvt->m_root) + if(leaf==pdbvt->m_root) { - pdbvt->m_root=0; - return(0); + pdbvt->m_root=0; + return(0); } else { - btDbvtNode* parent=leaf->parent; - btDbvtNode* prev=parent->parent; - btDbvtNode* sibling=parent->childs[1-indexof(leaf)]; - if(prev) + btDbvtNode* parent=leaf->parent; + btDbvtNode* prev=parent->parent; + btDbvtNode* sibling=parent->childs[1-indexof(leaf)]; + if(prev) { - prev->childs[indexof(parent)]=sibling; - sibling->parent=prev; - deletenode(pdbvt,parent); - while(prev) + prev->childs[indexof(parent)]=sibling; + sibling->parent=prev; + deletenode(pdbvt,parent); + while(prev) { - const btDbvtVolume pb=prev->volume; - Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume); - if(NotEqual(pb,prev->volume)) + const btDbvtVolume pb=prev->volume; + Merge(prev->childs[0]->volume,prev->childs[1]->volume,prev->volume); + if(NotEqual(pb,prev->volume)) { - prev=prev->parent; + prev=prev->parent; } else break; } - return(prev?prev:pdbvt->m_root); + return(prev?prev:pdbvt->m_root); } else { - pdbvt->m_root=sibling; - sibling->parent=0; - deletenode(pdbvt,parent); - return(pdbvt->m_root); + pdbvt->m_root=sibling; + sibling->parent=0; + deletenode(pdbvt,parent); + return(pdbvt->m_root); } } } @@ -215,33 +215,33 @@ static void fetchleaves(btDbvt* pdbvt, tNodeArray& leaves, int depth=-1) { -if(root->isinternal()&&depth) + if(root->isinternal()&&depth) { - fetchleaves(pdbvt,root->childs[0],leaves,depth-1); - fetchleaves(pdbvt,root->childs[1],leaves,depth-1); - deletenode(pdbvt,root); + fetchleaves(pdbvt,root->childs[0],leaves,depth-1); + fetchleaves(pdbvt,root->childs[1],leaves,depth-1); + deletenode(pdbvt,root); } else { - leaves.push_back(root); + leaves.push_back(root); } } // static void split( const tNodeArray& leaves, - tNodeArray& left, - tNodeArray& right, - const btVector3& org, - const btVector3& axis) + tNodeArray& left, + tNodeArray& right, + const btVector3& org, + const btVector3& axis) { -left.resize(0); -right.resize(0); -for(int i=0,ni=leaves.size();ivolume.Center()-org)<0) - left.push_back(leaves[i]); + if(dot(axis,leaves[i]->volume.Center()-org)<0) + left.push_back(leaves[i]); else - right.push_back(leaves[i]); + right.push_back(leaves[i]); } } @@ -249,49 +249,49 @@ for(int i=0,ni=leaves.size();ivolume; + DBVT_ALIGN char locals[sizeof(btDbvtVolume)]; + btDbvtVolume& volume=*(btDbvtVolume*)locals; + volume=leaves[0]->volume; #else -btDbvtVolume volume=leaves[0]->volume; + btDbvtVolume volume=leaves[0]->volume; #endif -for(int i=1,ni=leaves.size();ivolume,volume); + Merge(volume,leaves[i]->volume,volume); } -return(volume); + return(volume); } // static void bottomup( btDbvt* pdbvt, - tNodeArray& leaves) + tNodeArray& leaves) { -while(leaves.size()>1) + while(leaves.size()>1) { - btScalar minsize=SIMD_INFINITY; - int minidx[2]={-1,-1}; - for(int i=0;ivolume,leaves[j]->volume)); - if(szvolume,leaves[j]->volume)); + if(szvolume,n[1]->volume,0); - p->childs[0] = n[0]; - p->childs[1] = n[1]; - n[0]->parent = p; - n[1]->parent = p; - leaves[minidx[0]] = p; - leaves.swap(minidx[1],leaves.size()-1); - leaves.pop_back(); + btDbvtNode* n[] = {leaves[minidx[0]],leaves[minidx[1]]}; + btDbvtNode* p = createnode(pdbvt,0,n[0]->volume,n[1]->volume,0); + p->childs[0] = n[0]; + p->childs[1] = n[1]; + n[0]->parent = p; + n[1]->parent = p; + leaves[minidx[0]] = p; + leaves.swap(minidx[1],leaves.size()-1); + leaves.pop_back(); } } @@ -300,104 +300,104 @@ static btDbvtNode* topdown(btDbvt* pdbvt, tNodeArray& leaves, int bu_treshold) { -static const btVector3 axis[]={btVector3(1,0,0), - btVector3(0,1,0), - btVector3(0,0,1)}; -if(leaves.size()>1) + static const btVector3 axis[]={btVector3(1,0,0), + btVector3(0,1,0), + btVector3(0,0,1)}; + if(leaves.size()>1) { - if(leaves.size()>bu_treshold) + if(leaves.size()>bu_treshold) { - const btDbvtVolume vol=bounds(leaves); - const btVector3 org=vol.Center(); - tNodeArray sets[2]; - int bestaxis=-1; - int bestmidp=leaves.size(); - int splitcount[3][2]={{0,0},{0,0},{0,0}}; - int i; - for( i=0;ivolume.Center()-org; - for(int j=0;j<3;++j) + const btVector3 x=leaves[i]->volume.Center()-org; + for(int j=0;j<3;++j) { - ++splitcount[j][dot(x,axis[j])>0?1:0]; + ++splitcount[j][dot(x,axis[j])>0?1:0]; } } - for( i=0;i<3;++i) + for( i=0;i<3;++i) { - if((splitcount[i][0]>0)&&(splitcount[i][1]>0)) + if((splitcount[i][0]>0)&&(splitcount[i][1]>0)) { - const int midp=(int)btFabs(btScalar(splitcount[i][0]-splitcount[i][1])); - if(midp=0) + if(bestaxis>=0) { - sets[0].reserve(splitcount[bestaxis][0]); - sets[1].reserve(splitcount[bestaxis][1]); - split(leaves,sets[0],sets[1],org,axis[bestaxis]); + sets[0].reserve(splitcount[bestaxis][0]); + sets[1].reserve(splitcount[bestaxis][1]); + split(leaves,sets[0],sets[1],org,axis[bestaxis]); } else { - sets[0].reserve(leaves.size()/2+1); - sets[1].reserve(leaves.size()/2); - for(int i=0,ni=leaves.size();ichilds[0]=topdown(pdbvt,sets[0],bu_treshold); - node->childs[1]=topdown(pdbvt,sets[1],bu_treshold); - node->childs[0]->parent=node; - node->childs[1]->parent=node; - return(node); + btDbvtNode* node=createnode(pdbvt,0,vol,0); + node->childs[0]=topdown(pdbvt,sets[0],bu_treshold); + node->childs[1]=topdown(pdbvt,sets[1],bu_treshold); + node->childs[0]->parent=node; + node->childs[1]->parent=node; + return(node); } else { - bottomup(pdbvt,leaves); - return(leaves[0]); + bottomup(pdbvt,leaves); + return(leaves[0]); } } -return(leaves[0]); + return(leaves[0]); } // static DBVT_INLINE btDbvtNode* sort(btDbvtNode* n,btDbvtNode*& r) { -btDbvtNode* p=n->parent; -btAssert(n->isinternal()); -if(p>n) + btDbvtNode* p=n->parent; + btAssert(n->isinternal()); + if(p>n) { - const int i=indexof(n); - const int j=1-i; - btDbvtNode* s=p->childs[j]; - btDbvtNode* q=p->parent; - btAssert(n==p->childs[i]); - if(q) q->childs[indexof(p)]=n; else r=n; - s->parent=n; - p->parent=n; - n->parent=q; - p->childs[0]=n->childs[0]; - p->childs[1]=n->childs[1]; - n->childs[0]->parent=p; - n->childs[1]->parent=p; - n->childs[i]=p; - n->childs[j]=s; - btSwap(p->volume,n->volume); - return(p); + const int i=indexof(n); + const int j=1-i; + btDbvtNode* s=p->childs[j]; + btDbvtNode* q=p->parent; + btAssert(n==p->childs[i]); + if(q) q->childs[indexof(p)]=n; else r=n; + s->parent=n; + p->parent=n; + n->parent=q; + p->childs[0]=n->childs[0]; + p->childs[1]=n->childs[1]; + n->childs[0]->parent=p; + n->childs[1]->parent=p; + n->childs[i]=p; + n->childs[j]=s; + btSwap(p->volume,n->volume); + return(p); } -return(n); + return(n); } // static DBVT_INLINE btDbvtNode* walkup(btDbvtNode* n,int count) { -while(n&&(count--)) n=n->parent; -return(n); + while(n&&(count--)) n=n->parent; + return(n); } // @@ -405,70 +405,70 @@ return(n); // // - btDbvt::btDbvt() +btDbvt::btDbvt() { -m_root = 0; -m_free = 0; -m_lkhd = -1; -m_leaves = 0; -m_opath = 0; + m_root = 0; + m_free = 0; + m_lkhd = -1; + m_leaves = 0; + m_opath = 0; } // - btDbvt::~btDbvt() +btDbvt::~btDbvt() { -clear(); + clear(); } // void btDbvt::clear() { -if(m_root) recursedeletenode(this,m_root); -btAlignedFree(m_free); -m_free=0; + if(m_root) recursedeletenode(this,m_root); + btAlignedFree(m_free); + m_free=0; } // void btDbvt::optimizeBottomUp() { -if(m_root) + if(m_root) { - tNodeArray leaves; - leaves.reserve(m_leaves); - fetchleaves(this,m_root,leaves); - bottomup(this,leaves); - m_root=leaves[0]; + tNodeArray leaves; + leaves.reserve(m_leaves); + fetchleaves(this,m_root,leaves); + bottomup(this,leaves); + m_root=leaves[0]; } } // void btDbvt::optimizeTopDown(int bu_treshold) { -if(m_root) + if(m_root) { - tNodeArray leaves; - leaves.reserve(m_leaves); - fetchleaves(this,m_root,leaves); - m_root=topdown(this,leaves,bu_treshold); + tNodeArray leaves; + leaves.reserve(m_leaves); + fetchleaves(this,m_root,leaves); + m_root=topdown(this,leaves,bu_treshold); } } // void btDbvt::optimizeIncremental(int passes) { -if(passes<0) passes=m_leaves; -if(m_root&&(passes>0)) + if(passes<0) passes=m_leaves; + if(m_root&&(passes>0)) { - do { - btDbvtNode* node=m_root; - unsigned bit=0; - while(node->isinternal()) + do { + btDbvtNode* node=m_root; + unsigned bit=0; + while(node->isinternal()) { - node=sort(node,m_root)->childs[(m_opath>>bit)&1]; - bit=(bit+1)&(sizeof(unsigned)*8-1); + node=sort(node,m_root)->childs[(m_opath>>bit)&1]; + bit=(bit+1)&(sizeof(unsigned)*8-1); } - update(node); - ++m_opath; + update(node); + ++m_opath; } while(--passes); } } @@ -476,104 +476,104 @@ if(m_root&&(passes>0)) // btDbvtNode* btDbvt::insert(const btDbvtVolume& volume,void* data) { -btDbvtNode* leaf=createnode(this,0,volume,data); -insertleaf(this,m_root,leaf); -++m_leaves; -return(leaf); + btDbvtNode* leaf=createnode(this,0,volume,data); + insertleaf(this,m_root,leaf); + ++m_leaves; + return(leaf); } // void btDbvt::update(btDbvtNode* leaf,int lookahead) { -btDbvtNode* root=removeleaf(this,leaf); -if(root) + btDbvtNode* root=removeleaf(this,leaf); + if(root) { - if(lookahead>=0) + if(lookahead>=0) { - for(int i=0;(iparent;++i) + for(int i=0;(iparent;++i) { - root=root->parent; + root=root->parent; } } else root=m_root; } -insertleaf(this,root,leaf); + insertleaf(this,root,leaf); } // void btDbvt::update(btDbvtNode* leaf,const btDbvtVolume& volume) { -btDbvtNode* root=removeleaf(this,leaf); -if(root) + btDbvtNode* root=removeleaf(this,leaf); + if(root) { - if(m_lkhd>=0) + if(m_lkhd>=0) { - for(int i=0;(iparent;++i) + for(int i=0;(iparent;++i) { - root=root->parent; + root=root->parent; } } else root=m_root; } -leaf->volume=volume; -insertleaf(this,root,leaf); + leaf->volume=volume; + insertleaf(this,root,leaf); } // bool btDbvt::update(btDbvtNode* leaf,btDbvtVolume volume,const btVector3& velocity,btScalar margin) { -if(leaf->volume.Contain(volume)) return(false); -volume.Expand(btVector3(margin,margin,margin)); -volume.SignedExpand(velocity); -update(leaf,volume); -return(true); + if(leaf->volume.Contain(volume)) return(false); + volume.Expand(btVector3(margin,margin,margin)); + volume.SignedExpand(velocity); + update(leaf,volume); + return(true); } // bool btDbvt::update(btDbvtNode* leaf,btDbvtVolume volume,const btVector3& velocity) { -if(leaf->volume.Contain(volume)) return(false); -volume.SignedExpand(velocity); -update(leaf,volume); -return(true); + if(leaf->volume.Contain(volume)) return(false); + volume.SignedExpand(velocity); + update(leaf,volume); + return(true); } // bool btDbvt::update(btDbvtNode* leaf,btDbvtVolume volume,btScalar margin) { -if(leaf->volume.Contain(volume)) return(false); -volume.Expand(btVector3(margin,margin,margin)); -update(leaf,volume); -return(true); + if(leaf->volume.Contain(volume)) return(false); + volume.Expand(btVector3(margin,margin,margin)); + update(leaf,volume); + return(true); } // void btDbvt::remove(btDbvtNode* leaf) { -removeleaf(this,leaf); -deletenode(this,leaf); ---m_leaves; + removeleaf(this,leaf); + deletenode(this,leaf); + --m_leaves; } // void btDbvt::write(IWriter* iwriter) const { -btDbvtNodeEnumerator nodes; -nodes.nodes.reserve(m_leaves*2); -enumNodes(m_root,nodes); -iwriter->Prepare(m_root,nodes.nodes.size()); -for(int i=0;iPrepare(m_root,nodes.nodes.size()); + for(int i=0;iparent) p=nodes.nodes.findLinearSearch(n->parent); - if(n->isinternal()) + const btDbvtNode* n=nodes.nodes[i]; + int p=-1; + if(n->parent) p=nodes.nodes.findLinearSearch(n->parent); + if(n->isinternal()) { - const int c0=nodes.nodes.findLinearSearch(n->childs[0]); - const int c1=nodes.nodes.findLinearSearch(n->childs[1]); - iwriter->WriteNode(n,i,p,c0,c1); + const int c0=nodes.nodes.findLinearSearch(n->childs[0]); + const int c1=nodes.nodes.findLinearSearch(n->childs[1]); + iwriter->WriteNode(n,i,p,c0,c1); } else { - iwriter->WriteLeaf(n,i,p); + iwriter->WriteLeaf(n,i,p); } } } @@ -581,29 +581,29 @@ for(int i=0;i stack; - stack.reserve(m_leaves); - stack.push_back(sStkCLN(m_root,0)); - do { - const int i=stack.size()-1; - const sStkCLN e=stack[i]; - btDbvtNode* n=createnode(&dest,e.parent,e.node->volume,e.node->data); - stack.pop_back(); - if(e.parent!=0) - e.parent->childs[i&1]=n; + btAlignedObjectArray stack; + stack.reserve(m_leaves); + stack.push_back(sStkCLN(m_root,0)); + do { + const int i=stack.size()-1; + const sStkCLN e=stack[i]; + btDbvtNode* n=createnode(&dest,e.parent,e.node->volume,e.node->data); + stack.pop_back(); + if(e.parent!=0) + e.parent->childs[i&1]=n; else - dest.m_root=n; - if(e.node->isinternal()) + dest.m_root=n; + if(e.node->isinternal()) { - stack.push_back(sStkCLN(e.node->childs[0],n)); - stack.push_back(sStkCLN(e.node->childs[1],n)); + stack.push_back(sStkCLN(e.node->childs[0],n)); + stack.push_back(sStkCLN(e.node->childs[1],n)); } else { - iclone->CloneLeaf(n); + iclone->CloneLeaf(n); } } while(stack.size()>0); } @@ -612,31 +612,31 @@ if(m_root!=0) // int btDbvt::maxdepth(const btDbvtNode* node) { -int depth=0; -if(node) getmaxdepth(node,1,depth); -return(depth); + int depth=0; + if(node) getmaxdepth(node,1,depth); + return(depth); } // int btDbvt::countLeaves(const btDbvtNode* node) { -if(node->isinternal()) - return(countLeaves(node->childs[0])+countLeaves(node->childs[1])); + if(node->isinternal()) + return(countLeaves(node->childs[0])+countLeaves(node->childs[1])); else - return(1); + return(1); } // void btDbvt::extractLeaves(const btDbvtNode* node,btAlignedObjectArray& leaves) { -if(node->isinternal()) + if(node->isinternal()) { - extractLeaves(node->childs[0],leaves); - extractLeaves(node->childs[1],leaves); + extractLeaves(node->childs[0],leaves); + extractLeaves(node->childs[1],leaves); } else { - leaves.push_back(node); + leaves.push_back(node); } } @@ -657,12 +657,12 @@ q6600,2.4ghz /W3 /nologo /c /Wp64 /Zi /errorReport:prompt Benchmarking dbvt... - World scale: 100.000000 - Extents base: 1.000000 - Extents range: 4.000000 - Leaves: 8192 - sizeof(btDbvtVolume): 32 bytes - sizeof(btDbvtNode): 44 bytes +World scale: 100.000000 +Extents base: 1.000000 +Extents range: 4.000000 +Leaves: 8192 +sizeof(btDbvtVolume): 32 bytes +sizeof(btDbvtNode): 44 bytes [1] btDbvtVolume intersections: 3499 ms (-1%) [2] btDbvtVolume merges: 1934 ms (0%) [3] btDbvt::collideTT: 5485 ms (-21%) @@ -684,606 +684,606 @@ Benchmarking dbvt... struct btDbvtBenchmark { -struct NilPolicy : btDbvt::ICollide + struct NilPolicy : btDbvt::ICollide { - NilPolicy() : m_pcount(0),m_depth(-SIMD_INFINITY),m_checksort(true) {} - void Process(const btDbvtNode*,const btDbvtNode*) { ++m_pcount; } - void Process(const btDbvtNode*) { ++m_pcount; } - void Process(const btDbvtNode*,btScalar depth) + NilPolicy() : m_pcount(0),m_depth(-SIMD_INFINITY),m_checksort(true) {} + void Process(const btDbvtNode*,const btDbvtNode*) { ++m_pcount; } + void Process(const btDbvtNode*) { ++m_pcount; } + void Process(const btDbvtNode*,btScalar depth) { - ++m_pcount; - if(m_checksort) + ++m_pcount; + if(m_checksort) { if(depth>=m_depth) m_depth=depth; else printf("wrong depth: %f (should be >= %f)\r\n",depth,m_depth); } } - int m_pcount; - btScalar m_depth; - bool m_checksort; + int m_pcount; + btScalar m_depth; + bool m_checksort; }; -struct P14 : btDbvt::ICollide + struct P14 : btDbvt::ICollide { - struct Node + struct Node { - const btDbvtNode* leaf; - btScalar depth; + const btDbvtNode* leaf; + btScalar depth; }; - void Process(const btDbvtNode* leaf,btScalar depth) + void Process(const btDbvtNode* leaf,btScalar depth) { - Node n; - n.leaf = leaf; - n.depth = depth; + Node n; + n.leaf = leaf; + n.depth = depth; } - static int sortfnc(const Node& a,const Node& b) + static int sortfnc(const Node& a,const Node& b) { - if(a.depthb.depth) return(-1); - return(0); + if(a.depthb.depth) return(-1); + return(0); } - btAlignedObjectArray m_nodes; + btAlignedObjectArray m_nodes; }; -struct P15 : btDbvt::ICollide + struct P15 : btDbvt::ICollide { - struct Node + struct Node { - const btDbvtNode* leaf; - btScalar depth; + const btDbvtNode* leaf; + btScalar depth; }; - void Process(const btDbvtNode* leaf) + void Process(const btDbvtNode* leaf) { - Node n; - n.leaf = leaf; - n.depth = dot(leaf->volume.Center(),m_axis); + Node n; + n.leaf = leaf; + n.depth = dot(leaf->volume.Center(),m_axis); } - static int sortfnc(const Node& a,const Node& b) + static int sortfnc(const Node& a,const Node& b) { - if(a.depthb.depth) return(-1); - return(0); + if(a.depthb.depth) return(-1); + return(0); } - btAlignedObjectArray m_nodes; - btVector3 m_axis; + btAlignedObjectArray m_nodes; + btVector3 m_axis; }; -static btScalar RandUnit() + static btScalar RandUnit() { - return(rand()/(btScalar)RAND_MAX); + return(rand()/(btScalar)RAND_MAX); } -static btVector3 RandVector3() + static btVector3 RandVector3() { - return(btVector3(RandUnit(),RandUnit(),RandUnit())); + return(btVector3(RandUnit(),RandUnit(),RandUnit())); } -static btVector3 RandVector3(btScalar cs) + static btVector3 RandVector3(btScalar cs) { - return(RandVector3()*cs-btVector3(cs,cs,cs)/2); + return(RandVector3()*cs-btVector3(cs,cs,cs)/2); } -static btDbvtVolume RandVolume(btScalar cs,btScalar eb,btScalar es) + static btDbvtVolume RandVolume(btScalar cs,btScalar eb,btScalar es) { - return(btDbvtVolume::FromCE(RandVector3(cs),btVector3(eb,eb,eb)+RandVector3()*es)); + return(btDbvtVolume::FromCE(RandVector3(cs),btVector3(eb,eb,eb)+RandVector3()*es)); } -static btTransform RandTransform(btScalar cs) + static btTransform RandTransform(btScalar cs) { - btTransform t; - t.setOrigin(RandVector3(cs)); - t.setRotation(btQuaternion(RandUnit()*SIMD_PI*2,RandUnit()*SIMD_PI*2,RandUnit()*SIMD_PI*2).normalized()); - return(t); + btTransform t; + t.setOrigin(RandVector3(cs)); + t.setRotation(btQuaternion(RandUnit()*SIMD_PI*2,RandUnit()*SIMD_PI*2,RandUnit()*SIMD_PI*2).normalized()); + return(t); } -static void RandTree(btScalar cs,btScalar eb,btScalar es,int leaves,btDbvt& dbvt) + static void RandTree(btScalar cs,btScalar eb,btScalar es,int leaves,btDbvt& dbvt) { - dbvt.clear(); - for(int i=0;i volumes; - btAlignedObjectArray results; - volumes.resize(cfgLeaves); - results.resize(cfgLeaves); - for(int i=0;i volumes; + btAlignedObjectArray results; + volumes.resize(cfgLeaves); + results.resize(cfgLeaves); + for(int i=0;i volumes; - btAlignedObjectArray results; - volumes.resize(cfgLeaves); - results.resize(cfgLeaves); - for(int i=0;i volumes; + btAlignedObjectArray results; + volumes.resize(cfgLeaves); + results.resize(cfgLeaves); + for(int i=0;i transforms; - btDbvtBenchmark::NilPolicy policy; - transforms.resize(cfgBenchmark5_Iterations); - for(int i=0;i transforms; + btDbvtBenchmark::NilPolicy policy; + transforms.resize(cfgBenchmark5_Iterations); + for(int i=0;i transforms; - btDbvtBenchmark::NilPolicy policy; - transforms.resize(cfgBenchmark6_Iterations); - for(int i=0;i transforms; + btDbvtBenchmark::NilPolicy policy; + transforms.resize(cfgBenchmark6_Iterations); + for(int i=0;i rayorg; - btAlignedObjectArray raydir; - btDbvtBenchmark::NilPolicy policy; - rayorg.resize(cfgBenchmark7_Iterations); - raydir.resize(cfgBenchmark7_Iterations); - for(int i=0;i rayorg; + btAlignedObjectArray raydir; + btDbvtBenchmark::NilPolicy policy; + rayorg.resize(cfgBenchmark7_Iterations); + raydir.resize(cfgBenchmark7_Iterations); + for(int i=0;i leaves; - btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); - dbvt.optimizeTopDown(); - dbvt.extractLeaves(dbvt.m_root,leaves); - printf("[9] updates (teleport): "); - wallclock.reset(); - for(int i=0;i leaves; + btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + dbvt.optimizeTopDown(); + dbvt.extractLeaves(dbvt.m_root,leaves); + printf("[9] updates (teleport): "); + wallclock.reset(); + for(int i=0;i(leaves[rand()%cfgLeaves]), - btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale)); + dbvt.update(const_cast(leaves[rand()%cfgLeaves]), + btDbvtBenchmark::RandVolume(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale)); } } - const int time=(int)wallclock.getTimeMilliseconds(); - const int up=cfgBenchmark9_Passes*cfgBenchmark9_Iterations; - printf("%u ms (%i%%),(%u u/s)\r\n",time,(time-cfgBenchmark9_Reference)*100/time,up*1000/time); + const int time=(int)wallclock.getTimeMilliseconds(); + const int up=cfgBenchmark9_Passes*cfgBenchmark9_Iterations; + printf("%u ms (%i%%),(%u u/s)\r\n",time,(time-cfgBenchmark9_Reference)*100/time,up*1000/time); } -if(cfgBenchmark10_Enable) + if(cfgBenchmark10_Enable) {// Benchmark 10 - srand(380843); - btDbvt dbvt; - btAlignedObjectArray leaves; - btAlignedObjectArray vectors; - vectors.resize(cfgBenchmark10_Iterations); - for(int i=0;i leaves; + btAlignedObjectArray vectors; + vectors.resize(cfgBenchmark10_Iterations); + for(int i=0;i(leaves[rand()%cfgLeaves]); - btDbvtVolume v=btDbvtVolume::FromMM(l->volume.Mins()+d,l->volume.Maxs()+d); - dbvt.update(l,v); + const btVector3& d=vectors[j]; + btDbvtNode* l=const_cast(leaves[rand()%cfgLeaves]); + btDbvtVolume v=btDbvtVolume::FromMM(l->volume.Mins()+d,l->volume.Maxs()+d); + dbvt.update(l,v); } } - const int time=(int)wallclock.getTimeMilliseconds(); - const int up=cfgBenchmark10_Passes*cfgBenchmark10_Iterations; - printf("%u ms (%i%%),(%u u/s)\r\n",time,(time-cfgBenchmark10_Reference)*100/time,up*1000/time); + const int time=(int)wallclock.getTimeMilliseconds(); + const int up=cfgBenchmark10_Passes*cfgBenchmark10_Iterations; + printf("%u ms (%i%%),(%u u/s)\r\n",time,(time-cfgBenchmark10_Reference)*100/time,up*1000/time); } -if(cfgBenchmark11_Enable) + if(cfgBenchmark11_Enable) {// Benchmark 11 - srand(380843); - btDbvt dbvt; - btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); - dbvt.optimizeTopDown(); - printf("[11] optimize (incremental): "); - wallclock.reset(); - for(int i=0;i volumes; - btAlignedObjectArray results; - volumes.resize(cfgLeaves); - results.resize(cfgLeaves); - for(int i=0;i volumes; + btAlignedObjectArray results; + volumes.resize(cfgLeaves); + results.resize(cfgLeaves); + for(int i=0;i vectors; - btDbvtBenchmark::NilPolicy policy; - vectors.resize(cfgBenchmark13_Iterations); - for(int i=0;i vectors; + btDbvtBenchmark::NilPolicy policy; + vectors.resize(cfgBenchmark13_Iterations); + for(int i=0;i vectors; - btDbvtBenchmark::P14 policy; - vectors.resize(cfgBenchmark14_Iterations); - for(int i=0;i vectors; + btDbvtBenchmark::P14 policy; + vectors.resize(cfgBenchmark14_Iterations); + for(int i=0;i vectors; - btDbvtBenchmark::P15 policy; - vectors.resize(cfgBenchmark15_Iterations); - for(int i=0;i vectors; + btDbvtBenchmark::P15 policy; + vectors.resize(cfgBenchmark15_Iterations); + for(int i=0;i batch; - btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); - dbvt.optimizeTopDown(); - batch.reserve(cfgBenchmark16_BatchCount); - printf("[16] insert/remove batch(%u): ",cfgBenchmark16_BatchCount); - wallclock.reset(); - for(int i=0;i batch; + btDbvtBenchmark::RandTree(cfgVolumeCenterScale,cfgVolumeExentsBase,cfgVolumeExentsScale,cfgLeaves,dbvt); + dbvt.optimizeTopDown(); + batch.reserve(cfgBenchmark16_BatchCount); + printf("[16] insert/remove batch(%u): ",cfgBenchmark16_BatchCount); + wallclock.reset(); + for(int i=0;i volumes; - btAlignedObjectArray results; - btAlignedObjectArray indices; - volumes.resize(cfgLeaves); - results.resize(cfgLeaves); - indices.resize(cfgLeaves); - for(int i=0;i volumes; + btAlignedObjectArray results; + btAlignedObjectArray indices; + volumes.resize(cfgLeaves); + results.resize(cfgLeaves); + indices.resize(cfgLeaves); + for(int i=0;i= 1400) - #define DBVT_USE_TEMPLATE 1 - #else - #define DBVT_USE_TEMPLATE 0 +#if (defined (_MSC_VER) && _MSC_VER >= 1400) +#define DBVT_USE_TEMPLATE 1 +#else +#define DBVT_USE_TEMPLATE 0 #endif #else #define DBVT_USE_TEMPLATE 0 @@ -135,43 +135,43 @@ subject to the following restrictions: /* btDbvtAabbMm */ struct btDbvtAabbMm { -DBVT_INLINE btVector3 Center() const { return((mi+mx)/2); } -DBVT_INLINE btVector3 Lengths() const { return(mx-mi); } -DBVT_INLINE btVector3 Extents() const { return((mx-mi)/2); } -DBVT_INLINE const btVector3& Mins() const { return(mi); } -DBVT_INLINE const btVector3& Maxs() const { return(mx); } -static inline btDbvtAabbMm FromCE(const btVector3& c,const btVector3& e); -static inline btDbvtAabbMm FromCR(const btVector3& c,btScalar r); -static inline btDbvtAabbMm FromMM(const btVector3& mi,const btVector3& mx); -static inline btDbvtAabbMm FromPoints(const btVector3* pts,int n); -static inline btDbvtAabbMm FromPoints(const btVector3** ppts,int n); -DBVT_INLINE void Expand(const btVector3& e); -DBVT_INLINE void SignedExpand(const btVector3& e); -DBVT_INLINE bool Contain(const btDbvtAabbMm& a) const; -DBVT_INLINE int Classify(const btVector3& n,btScalar o,int s) const; -DBVT_INLINE btScalar ProjectMinimum(const btVector3& v,unsigned signs) const; -DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a, - const btDbvtAabbMm& b); -DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a, - const btDbvtAabbMm& b, - const btTransform& xform); -DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a, - const btVector3& b); + DBVT_INLINE btVector3 Center() const { return((mi+mx)/2); } + DBVT_INLINE btVector3 Lengths() const { return(mx-mi); } + DBVT_INLINE btVector3 Extents() const { return((mx-mi)/2); } + DBVT_INLINE const btVector3& Mins() const { return(mi); } + DBVT_INLINE const btVector3& Maxs() const { return(mx); } + static inline btDbvtAabbMm FromCE(const btVector3& c,const btVector3& e); + static inline btDbvtAabbMm FromCR(const btVector3& c,btScalar r); + static inline btDbvtAabbMm FromMM(const btVector3& mi,const btVector3& mx); + static inline btDbvtAabbMm FromPoints(const btVector3* pts,int n); + static inline btDbvtAabbMm FromPoints(const btVector3** ppts,int n); + DBVT_INLINE void Expand(const btVector3& e); + DBVT_INLINE void SignedExpand(const btVector3& e); + DBVT_INLINE bool Contain(const btDbvtAabbMm& a) const; + DBVT_INLINE int Classify(const btVector3& n,btScalar o,int s) const; + DBVT_INLINE btScalar ProjectMinimum(const btVector3& v,unsigned signs) const; + DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a, + const btDbvtAabbMm& b); + DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a, + const btDbvtAabbMm& b, + const btTransform& xform); + DBVT_INLINE friend bool Intersect( const btDbvtAabbMm& a, + const btVector3& b); -DBVT_INLINE friend btScalar Proximity( const btDbvtAabbMm& a, - const btDbvtAabbMm& b); -DBVT_INLINE friend int Select( const btDbvtAabbMm& o, - const btDbvtAabbMm& a, - const btDbvtAabbMm& b); -DBVT_INLINE friend void Merge( const btDbvtAabbMm& a, - const btDbvtAabbMm& b, - btDbvtAabbMm& r); -DBVT_INLINE friend bool NotEqual( const btDbvtAabbMm& a, - const btDbvtAabbMm& b); + DBVT_INLINE friend btScalar Proximity( const btDbvtAabbMm& a, + const btDbvtAabbMm& b); + DBVT_INLINE friend int Select( const btDbvtAabbMm& o, + const btDbvtAabbMm& a, + const btDbvtAabbMm& b); + DBVT_INLINE friend void Merge( const btDbvtAabbMm& a, + const btDbvtAabbMm& b, + btDbvtAabbMm& r); + DBVT_INLINE friend bool NotEqual( const btDbvtAabbMm& a, + const btDbvtAabbMm& b); private: -DBVT_INLINE void AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const; + DBVT_INLINE void AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const; private: -btVector3 mi,mx; + btVector3 mi,mx; }; // Types @@ -185,78 +185,78 @@ struct btDbvtNode DBVT_INLINE bool isleaf() const { return(childs[1]==0); } DBVT_INLINE bool isinternal() const { return(!isleaf()); } union { - btDbvtNode* childs[2]; - void* data; - int dataAsInt; - }; + btDbvtNode* childs[2]; + void* data; + int dataAsInt; + }; }; ///The btDbvt class implements a fast dynamic bounding volume tree based on axis aligned bounding boxes (aabb tree). ///This btDbvt is used for soft body collision detection and for the btDbvtBroadphase. It has a fast insert, remove and update of nodes. ///Unlike the btQuantizedBvh, nodes can be dynamically moved around, which allows for change in topology of the underlying data structure. struct btDbvt - { +{ /* Stack element */ struct sStkNN - { + { const btDbvtNode* a; const btDbvtNode* b; sStkNN() {} sStkNN(const btDbvtNode* na,const btDbvtNode* nb) : a(na),b(nb) {} - }; + }; struct sStkNP - { + { const btDbvtNode* node; int mask; sStkNP(const btDbvtNode* n,unsigned m) : node(n),mask(m) {} - }; + }; struct sStkNPS - { + { const btDbvtNode* node; int mask; btScalar value; sStkNPS() {} sStkNPS(const btDbvtNode* n,unsigned m,btScalar v) : node(n),mask(m),value(v) {} - }; + }; struct sStkCLN - { + { const btDbvtNode* node; btDbvtNode* parent; sStkCLN(const btDbvtNode* n,btDbvtNode* p) : node(n),parent(p) {} - }; + }; // Policies/Interfaces - + /* ICollide */ struct ICollide - { + { DBVT_VIRTUAL_DTOR(ICollide) - DBVT_VIRTUAL void Process(const btDbvtNode*,const btDbvtNode*) {} + DBVT_VIRTUAL void Process(const btDbvtNode*,const btDbvtNode*) {} DBVT_VIRTUAL void Process(const btDbvtNode*) {} DBVT_VIRTUAL void Process(const btDbvtNode* n,btScalar) { Process(n); } DBVT_VIRTUAL bool Descent(const btDbvtNode*) { return(true); } DBVT_VIRTUAL bool AllLeaves(const btDbvtNode*) { return(true); } - }; + }; /* IWriter */ struct IWriter - { + { virtual ~IWriter() {} virtual void Prepare(const btDbvtNode* root,int numnodes)=0; virtual void WriteNode(const btDbvtNode*,int index,int parent,int child0,int child1)=0; virtual void WriteLeaf(const btDbvtNode*,int index,int parent)=0; - }; + }; /* IClone */ struct IClone - { + { virtual ~IClone() {} virtual void CloneLeaf(btDbvtNode*) {} - }; - + }; + // Constants enum { - SIMPLE_STACKSIZE = 64, - DOUBLE_STACKSIZE = SIMPLE_STACKSIZE*2 - }; - + SIMPLE_STACKSIZE = 64, + DOUBLE_STACKSIZE = SIMPLE_STACKSIZE*2 + }; + // Fields btDbvtNode* m_root; btDbvtNode* m_free; @@ -264,8 +264,8 @@ struct btDbvt int m_leaves; unsigned m_opath; // Methods - btDbvt(); - ~btDbvt(); + btDbvt(); + ~btDbvt(); void clear(); bool empty() const { return(0==m_root); } void optimizeBottomUp(); @@ -283,85 +283,85 @@ struct btDbvt static int maxdepth(const btDbvtNode* node); static int countLeaves(const btDbvtNode* node); static void extractLeaves(const btDbvtNode* node,btAlignedObjectArray& leaves); - #if DBVT_ENABLE_BENCHMARK +#if DBVT_ENABLE_BENCHMARK static void benchmark(); - #else +#else static void benchmark(){} - #endif +#endif // DBVT_IPOLICY must support ICollide policy/interface DBVT_PREFIX - static void enumNodes( const btDbvtNode* root, - DBVT_IPOLICY); + static void enumNodes( const btDbvtNode* root, + DBVT_IPOLICY); DBVT_PREFIX - static void enumLeaves( const btDbvtNode* root, - DBVT_IPOLICY); + static void enumLeaves( const btDbvtNode* root, + DBVT_IPOLICY); DBVT_PREFIX - static void collideTT( const btDbvtNode* root0, - const btDbvtNode* root1, - DBVT_IPOLICY); + static void collideTT( const btDbvtNode* root0, + const btDbvtNode* root1, + DBVT_IPOLICY); DBVT_PREFIX - static void collideTT( const btDbvtNode* root0, - const btDbvtNode* root1, - const btTransform& xform, - DBVT_IPOLICY); + static void collideTT( const btDbvtNode* root0, + const btDbvtNode* root1, + const btTransform& xform, + DBVT_IPOLICY); DBVT_PREFIX - static void collideTT( const btDbvtNode* root0, - const btTransform& xform0, - const btDbvtNode* root1, - const btTransform& xform1, - DBVT_IPOLICY); + static void collideTT( const btDbvtNode* root0, + const btTransform& xform0, + const btDbvtNode* root1, + const btTransform& xform1, + DBVT_IPOLICY); DBVT_PREFIX - static void collideTV( const btDbvtNode* root, - const btDbvtVolume& volume, - DBVT_IPOLICY); + static void collideTV( const btDbvtNode* root, + const btDbvtVolume& volume, + DBVT_IPOLICY); DBVT_PREFIX - static void rayTest( const btDbvtNode* root, - const btVector3& rayFrom, - const btVector3& rayTo, - DBVT_IPOLICY); + static void rayTest( const btDbvtNode* root, + const btVector3& rayFrom, + const btVector3& rayTo, + DBVT_IPOLICY); DBVT_PREFIX - static void collideKDOP(const btDbvtNode* root, - const btVector3* normals, - const btScalar* offsets, - int count, - DBVT_IPOLICY); + static void collideKDOP(const btDbvtNode* root, + const btVector3* normals, + const btScalar* offsets, + int count, + DBVT_IPOLICY); DBVT_PREFIX - static void collideOCL( const btDbvtNode* root, - const btVector3* normals, - const btScalar* offsets, - const btVector3& sortaxis, - int count, - DBVT_IPOLICY, - bool fullsort=true); + static void collideOCL( const btDbvtNode* root, + const btVector3* normals, + const btScalar* offsets, + const btVector3& sortaxis, + int count, + DBVT_IPOLICY, + bool fullsort=true); DBVT_PREFIX - static void collideTU( const btDbvtNode* root, - DBVT_IPOLICY); + static void collideTU( const btDbvtNode* root, + DBVT_IPOLICY); // Helpers static DBVT_INLINE int nearest(const int* i,const btDbvt::sStkNPS* a,btScalar v,int l,int h) - { + { int m=0; while(l>1; if(a[i[m]].value>=v) l=m+1; else h=m; - } - return(h); } + return(h); + } static DBVT_INLINE int allocate( btAlignedObjectArray& ifree, - btAlignedObjectArray& stock, - const sStkNPS& value) - { + btAlignedObjectArray& stock, + const sStkNPS& value) + { int i; if(ifree.size()>0) - { i=ifree[ifree.size()-1];ifree.pop_back();stock[i]=value; } - else - { i=stock.size();stock.push_back(value); } + { i=ifree[ifree.size()-1];ifree.pop_back();stock[i]=value; } + else + { i=stock.size();stock.push_back(value); } return(i); - } + } // - private: - btDbvt(const btDbvt&) {} - }; +private: + btDbvt(const btDbvt&) {} +}; // // Inline's @@ -370,69 +370,69 @@ struct btDbvt // inline btDbvtAabbMm btDbvtAabbMm::FromCE(const btVector3& c,const btVector3& e) { -btDbvtAabbMm box; -box.mi=c-e;box.mx=c+e; -return(box); + btDbvtAabbMm box; + box.mi=c-e;box.mx=c+e; + return(box); } - + // inline btDbvtAabbMm btDbvtAabbMm::FromCR(const btVector3& c,btScalar r) { -return(FromCE(c,btVector3(r,r,r))); + return(FromCE(c,btVector3(r,r,r))); } - + // inline btDbvtAabbMm btDbvtAabbMm::FromMM(const btVector3& mi,const btVector3& mx) { -btDbvtAabbMm box; -box.mi=mi;box.mx=mx; -return(box); + btDbvtAabbMm box; + box.mi=mi;box.mx=mx; + return(box); } - + // inline btDbvtAabbMm btDbvtAabbMm::FromPoints(const btVector3* pts,int n) { -btDbvtAabbMm box; -box.mi=box.mx=pts[0]; -for(int i=1;i0) mx.setX(mx.x()+e[0]); else mi.setX(mi.x()+e[0]); -if(e.y()>0) mx.setY(mx.y()+e[1]); else mi.setY(mi.y()+e[1]); -if(e.z()>0) mx.setZ(mx.z()+e[2]); else mi.setZ(mi.z()+e[2]); + if(e.x()>0) mx.setX(mx.x()+e[0]); else mi.setX(mi.x()+e[0]); + if(e.y()>0) mx.setY(mx.y()+e[1]); else mi.setY(mi.y()+e[1]); + if(e.z()>0) mx.setZ(mx.z()+e[2]); else mi.setZ(mi.z()+e[2]); } - + // DBVT_INLINE bool btDbvtAabbMm::Contain(const btDbvtAabbMm& a) const { -return( (mi.x()<=a.mi.x())&& + return( (mi.x()<=a.mi.x())&& (mi.y()<=a.mi.y())&& (mi.z()<=a.mi.z())&& (mx.x()>=a.mx.x())&& @@ -443,64 +443,64 @@ return( (mi.x()<=a.mi.x())&& // DBVT_INLINE int btDbvtAabbMm::Classify(const btVector3& n,btScalar o,int s) const { -btVector3 pi,px; -switch(s) + btVector3 pi,px; + switch(s) { case (0+0+0): px=btVector3(mi.x(),mi.y(),mi.z()); - pi=btVector3(mx.x(),mx.y(),mx.z());break; + pi=btVector3(mx.x(),mx.y(),mx.z());break; case (1+0+0): px=btVector3(mx.x(),mi.y(),mi.z()); - pi=btVector3(mi.x(),mx.y(),mx.z());break; + pi=btVector3(mi.x(),mx.y(),mx.z());break; case (0+2+0): px=btVector3(mi.x(),mx.y(),mi.z()); - pi=btVector3(mx.x(),mi.y(),mx.z());break; + pi=btVector3(mx.x(),mi.y(),mx.z());break; case (1+2+0): px=btVector3(mx.x(),mx.y(),mi.z()); - pi=btVector3(mi.x(),mi.y(),mx.z());break; + pi=btVector3(mi.x(),mi.y(),mx.z());break; case (0+0+4): px=btVector3(mi.x(),mi.y(),mx.z()); - pi=btVector3(mx.x(),mx.y(),mi.z());break; + pi=btVector3(mx.x(),mx.y(),mi.z());break; case (1+0+4): px=btVector3(mx.x(),mi.y(),mx.z()); - pi=btVector3(mi.x(),mx.y(),mi.z());break; + pi=btVector3(mi.x(),mx.y(),mi.z());break; case (0+2+4): px=btVector3(mi.x(),mx.y(),mx.z()); - pi=btVector3(mx.x(),mi.y(),mi.z());break; + pi=btVector3(mx.x(),mi.y(),mi.z());break; case (1+2+4): px=btVector3(mx.x(),mx.y(),mx.z()); - pi=btVector3(mi.x(),mi.y(),mi.z());break; + pi=btVector3(mi.x(),mi.y(),mi.z());break; } -if((dot(n,px)+o)<0) return(-1); -if((dot(n,pi)+o)>=0) return(+1); -return(0); + if((dot(n,px)+o)<0) return(-1); + if((dot(n,pi)+o)>=0) return(+1); + return(0); } // DBVT_INLINE btScalar btDbvtAabbMm::ProjectMinimum(const btVector3& v,unsigned signs) const { -const btVector3* b[]={&mx,&mi}; -const btVector3 p( b[(signs>>0)&1]->x(), - b[(signs>>1)&1]->y(), - b[(signs>>2)&1]->z()); -return(dot(p,v)); + const btVector3* b[]={&mx,&mi}; + const btVector3 p( b[(signs>>0)&1]->x(), + b[(signs>>1)&1]->y(), + b[(signs>>2)&1]->z()); + return(dot(p,v)); } // DBVT_INLINE void btDbvtAabbMm::AddSpan(const btVector3& d,btScalar& smi,btScalar& smx) const { -for(int i=0;i<3;++i) + for(int i=0;i<3;++i) { - if(d[i]<0) + if(d[i]<0) { smi+=mx[i]*d[i];smx+=mi[i]*d[i]; } else { smi+=mi[i]*d[i];smx+=mx[i]*d[i]; } } } - + // DBVT_INLINE bool Intersect( const btDbvtAabbMm& a, - const btDbvtAabbMm& b) + const btDbvtAabbMm& b) { #if DBVT_INT0_IMPL == DBVT_IMPL_SSE -const __m128 rt(_mm_or_ps( _mm_cmplt_ps(_mm_load_ps(b.mx),_mm_load_ps(a.mi)), - _mm_cmplt_ps(_mm_load_ps(a.mx),_mm_load_ps(b.mi)))); -const __int32* pu((const __int32*)&rt); -return((pu[0]|pu[1]|pu[2])==0); + const __m128 rt(_mm_or_ps( _mm_cmplt_ps(_mm_load_ps(b.mx),_mm_load_ps(a.mi)), + _mm_cmplt_ps(_mm_load_ps(a.mx),_mm_load_ps(b.mi)))); + const __int32* pu((const __int32*)&rt); + return((pu[0]|pu[1]|pu[2])==0); #else -return( (a.mi.x()<=b.mx.x())&& + return( (a.mi.x()<=b.mx.x())&& (a.mx.x()>=b.mi.x())&& (a.mi.y()<=b.mx.y())&& (a.mx.y()>=b.mi.y())&& @@ -511,25 +511,25 @@ return( (a.mi.x()<=b.mx.x())&& // DBVT_INLINE bool Intersect( const btDbvtAabbMm& a, - const btDbvtAabbMm& b, - const btTransform& xform) + const btDbvtAabbMm& b, + const btTransform& xform) { -const btVector3 d0=xform*b.Center()-a.Center(); -const btVector3 d1=d0*xform.getBasis(); -btScalar s0[2]={0,0}; -btScalar s1[2]={dot(xform.getOrigin(),d0),s1[0]}; -a.AddSpan(d0,s0[0],s0[1]); -b.AddSpan(d1,s1[0],s1[1]); -if(s0[0]>(s1[1])) return(false); -if(s0[1]<(s1[0])) return(false); -return(true); + const btVector3 d0=xform*b.Center()-a.Center(); + const btVector3 d1=d0*xform.getBasis(); + btScalar s0[2]={0,0}; + btScalar s1[2]={dot(xform.getOrigin(),d0),s1[0]}; + a.AddSpan(d0,s0[0],s0[1]); + b.AddSpan(d1,s1[0],s1[1]); + if(s0[0]>(s1[1])) return(false); + if(s0[1]<(s1[0])) return(false); + return(true); } // DBVT_INLINE bool Intersect( const btDbvtAabbMm& a, - const btVector3& b) + const btVector3& b) { -return( (b.x()>=a.mi.x())&& + return( (b.x()>=a.mi.x())&& (b.y()>=a.mi.y())&& (b.z()>=a.mi.z())&& (b.x()<=a.mx.x())&& @@ -543,24 +543,24 @@ return( (b.x()>=a.mi.x())&& ////////////////////////////////////// - + // DBVT_INLINE btScalar Proximity( const btDbvtAabbMm& a, - const btDbvtAabbMm& b) + const btDbvtAabbMm& b) { -const btVector3 d=(a.mi+a.mx)-(b.mi+b.mx); -return(btFabs(d.x())+btFabs(d.y())+btFabs(d.z())); + const btVector3 d=(a.mi+a.mx)-(b.mi+b.mx); + return(btFabs(d.x())+btFabs(d.y())+btFabs(d.z())); } // DBVT_INLINE int Select( const btDbvtAabbMm& o, - const btDbvtAabbMm& a, - const btDbvtAabbMm& b) + const btDbvtAabbMm& a, + const btDbvtAabbMm& b) { #if DBVT_SELECT_IMPL == DBVT_IMPL_SSE -static DBVT_ALIGN const unsigned __int32 mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x7fffffff}; + static DBVT_ALIGN const unsigned __int32 mask[]={0x7fffffff,0x7fffffff,0x7fffffff,0x7fffffff}; // TODO: the intrinsic version is 11% slower - #if DBVT_USE_INTRINSIC_SSE +#if DBVT_USE_INTRINSIC_SSE __m128 omi(_mm_load_ps(o.mi)); omi=_mm_add_ps(omi,_mm_load_ps(o.mx)); __m128 ami(_mm_load_ps(a.mi)); @@ -578,70 +578,70 @@ static DBVT_ALIGN const unsigned __int32 mask[]={0x7fffffff,0x7fffffff,0x7ffffff bmi=_mm_add_ps(bmi,t1); bmi=_mm_add_ss(bmi,_mm_shuffle_ps(bmi,bmi,1)); return(_mm_cmple_ss(bmi,ami).m128_u32[0]&1); - #else +#else DBVT_ALIGN __int32 r[1]; __asm - { + { mov eax,o - mov ecx,a - mov edx,b - movaps xmm0,[eax] + mov ecx,a + mov edx,b + movaps xmm0,[eax] movaps xmm5,mask - addps xmm0,[eax+16] + addps xmm0,[eax+16] movaps xmm1,[ecx] movaps xmm2,[edx] addps xmm1,[ecx+16] addps xmm2,[edx+16] subps xmm1,xmm0 - subps xmm2,xmm0 - andps xmm1,xmm5 - andps xmm2,xmm5 - movhlps xmm3,xmm1 - movhlps xmm4,xmm2 - addps xmm1,xmm3 - addps xmm2,xmm4 - pshufd xmm3,xmm1,1 - pshufd xmm4,xmm2,1 - addss xmm1,xmm3 - addss xmm2,xmm4 - cmpless xmm2,xmm1 - movss r,xmm2 - } + subps xmm2,xmm0 + andps xmm1,xmm5 + andps xmm2,xmm5 + movhlps xmm3,xmm1 + movhlps xmm4,xmm2 + addps xmm1,xmm3 + addps xmm2,xmm4 + pshufd xmm3,xmm1,1 + pshufd xmm4,xmm2,1 + addss xmm1,xmm3 + addss xmm2,xmm4 + cmpless xmm2,xmm1 + movss r,xmm2 + } return(r[0]&1); - #endif +#endif #else -return(Proximity(o,a)b.mx[i]) r.mx[i]=a.mx[i]; else r.mx[i]=b.mx[i]; + if(a.mi[i]b.mx[i]) r.mx[i]=a.mx[i]; else r.mx[i]=b.mx[i]; } #endif } // DBVT_INLINE bool NotEqual( const btDbvtAabbMm& a, - const btDbvtAabbMm& b) + const btDbvtAabbMm& b) { -return( (a.mi.x()!=b.mi.x())|| + return( (a.mi.x()!=b.mi.x())|| (a.mi.y()!=b.mi.y())|| (a.mi.z()!=b.mi.z())|| (a.mx.x()!=b.mx.x())|| @@ -656,255 +656,255 @@ return( (a.mi.x()!=b.mi.x())|| // DBVT_PREFIX inline void btDbvt::enumNodes( const btDbvtNode* root, - DBVT_IPOLICY) + DBVT_IPOLICY) { -DBVT_CHECKTYPE -policy.Process(root); -if(root->isinternal()) + DBVT_CHECKTYPE + policy.Process(root); + if(root->isinternal()) { - enumNodes(root->childs[0],policy); - enumNodes(root->childs[1],policy); + enumNodes(root->childs[0],policy); + enumNodes(root->childs[1],policy); } } // DBVT_PREFIX inline void btDbvt::enumLeaves( const btDbvtNode* root, - DBVT_IPOLICY) + DBVT_IPOLICY) { -DBVT_CHECKTYPE -if(root->isinternal()) - { - enumLeaves(root->childs[0],policy); - enumLeaves(root->childs[1],policy); - } - else - { - policy.Process(root); - } + DBVT_CHECKTYPE + if(root->isinternal()) + { + enumLeaves(root->childs[0],policy); + enumLeaves(root->childs[1],policy); + } + else + { + policy.Process(root); + } } // DBVT_PREFIX inline void btDbvt::collideTT( const btDbvtNode* root0, - const btDbvtNode* root1, - DBVT_IPOLICY) + const btDbvtNode* root1, + DBVT_IPOLICY) { -DBVT_CHECKTYPE -if(root0&&root1) - { - btAlignedObjectArray stack; - int depth=1; - int treshold=DOUBLE_STACKSIZE-4; - stack.resize(DOUBLE_STACKSIZE); - stack[0]=sStkNN(root0,root1); - do { - sStkNN p=stack[--depth]; - if(depth>treshold) - { - stack.resize(stack.size()*2); - treshold=stack.size()-4; - } - if(p.a==p.b) - { - if(p.a->isinternal()) + DBVT_CHECKTYPE + if(root0&&root1) + { + btAlignedObjectArray stack; + int depth=1; + int treshold=DOUBLE_STACKSIZE-4; + stack.resize(DOUBLE_STACKSIZE); + stack[0]=sStkNN(root0,root1); + do { + sStkNN p=stack[--depth]; + if(depth>treshold) { - stack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]); - stack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]); - stack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]); + stack.resize(stack.size()*2); + treshold=stack.size()-4; } - } - else if(Intersect(p.a->volume,p.b->volume)) - { - if(p.a->isinternal()) + if(p.a==p.b) { - if(p.b->isinternal()) + if(p.a->isinternal()) { - stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]); - stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]); - stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]); - stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]); + stack[depth++]=sStkNN(p.a->childs[0],p.a->childs[0]); + stack[depth++]=sStkNN(p.a->childs[1],p.a->childs[1]); + stack[depth++]=sStkNN(p.a->childs[0],p.a->childs[1]); + } + } + else if(Intersect(p.a->volume,p.b->volume)) + { + if(p.a->isinternal()) + { + if(p.b->isinternal()) + { + stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]); + stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]); + stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]); + stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]); + } + else + { + stack[depth++]=sStkNN(p.a->childs[0],p.b); + stack[depth++]=sStkNN(p.a->childs[1],p.b); + } } else { - stack[depth++]=sStkNN(p.a->childs[0],p.b); - stack[depth++]=sStkNN(p.a->childs[1],p.b); + if(p.b->isinternal()) + { + stack[depth++]=sStkNN(p.a,p.b->childs[0]); + stack[depth++]=sStkNN(p.a,p.b->childs[1]); + } + else + { + policy.Process(p.a,p.b); + } } } - else - { - if(p.b->isinternal()) - { - stack[depth++]=sStkNN(p.a,p.b->childs[0]); - stack[depth++]=sStkNN(p.a,p.b->childs[1]); - } - else - { - policy.Process(p.a,p.b); - } - } - } - } while(depth); - } + } while(depth); + } } // DBVT_PREFIX inline void btDbvt::collideTT( const btDbvtNode* root0, - const btDbvtNode* root1, - const btTransform& xform, - DBVT_IPOLICY) + const btDbvtNode* root1, + const btTransform& xform, + DBVT_IPOLICY) { -DBVT_CHECKTYPE -if(root0&&root1) - { - btAlignedObjectArray stack; - int depth=1; - int treshold=DOUBLE_STACKSIZE-4; - stack.resize(DOUBLE_STACKSIZE); - stack[0]=sStkNN(root0,root1); - do { - sStkNN p=stack[--depth]; - if(Intersect(p.a->volume,p.b->volume,xform)) - { - if(depth>treshold) + DBVT_CHECKTYPE + if(root0&&root1) + { + btAlignedObjectArray stack; + int depth=1; + int treshold=DOUBLE_STACKSIZE-4; + stack.resize(DOUBLE_STACKSIZE); + stack[0]=sStkNN(root0,root1); + do { + sStkNN p=stack[--depth]; + if(Intersect(p.a->volume,p.b->volume,xform)) { - stack.resize(stack.size()*2); - treshold=stack.size()-4; - } - if(p.a->isinternal()) - { - if(p.b->isinternal()) - { - stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]); - stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]); - stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]); - stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]); + if(depth>treshold) + { + stack.resize(stack.size()*2); + treshold=stack.size()-4; + } + if(p.a->isinternal()) + { + if(p.b->isinternal()) + { + stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[0]); + stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[0]); + stack[depth++]=sStkNN(p.a->childs[0],p.b->childs[1]); + stack[depth++]=sStkNN(p.a->childs[1],p.b->childs[1]); + } + else + { + stack[depth++]=sStkNN(p.a->childs[0],p.b); + stack[depth++]=sStkNN(p.a->childs[1],p.b); + } } else { - stack[depth++]=sStkNN(p.a->childs[0],p.b); - stack[depth++]=sStkNN(p.a->childs[1],p.b); + if(p.b->isinternal()) + { + stack[depth++]=sStkNN(p.a,p.b->childs[0]); + stack[depth++]=sStkNN(p.a,p.b->childs[1]); + } + else + { + policy.Process(p.a,p.b); + } } } - else - { - if(p.b->isinternal()) - { - stack[depth++]=sStkNN(p.a,p.b->childs[0]); - stack[depth++]=sStkNN(p.a,p.b->childs[1]); - } - else - { - policy.Process(p.a,p.b); - } - } - } - } while(depth); - } + } while(depth); + } } // DBVT_PREFIX inline void btDbvt::collideTT( const btDbvtNode* root0, - const btTransform& xform0, - const btDbvtNode* root1, - const btTransform& xform1, - DBVT_IPOLICY) + const btTransform& xform0, + const btDbvtNode* root1, + const btTransform& xform1, + DBVT_IPOLICY) { -const btTransform xform=xform0.inverse()*xform1; -collideTT(root0,root1,xform,policy); + const btTransform xform=xform0.inverse()*xform1; + collideTT(root0,root1,xform,policy); } // DBVT_PREFIX inline void btDbvt::collideTV( const btDbvtNode* root, - const btDbvtVolume& vol, - DBVT_IPOLICY) + const btDbvtVolume& vol, + DBVT_IPOLICY) { -DBVT_CHECKTYPE -if(root) - { - ATTRIBUTE_ALIGNED16(btDbvtVolume) volume(vol); - btAlignedObjectArray stack; - stack.reserve(SIMPLE_STACKSIZE); - stack.push_back(root); - do { - const btDbvtNode* n=stack[stack.size()-1]; - stack.pop_back(); - if(Intersect(n->volume,volume)) - { - if(n->isinternal()) + DBVT_CHECKTYPE + if(root) + { + ATTRIBUTE_ALIGNED16(btDbvtVolume) volume(vol); + btAlignedObjectArray stack; + stack.reserve(SIMPLE_STACKSIZE); + stack.push_back(root); + do { + const btDbvtNode* n=stack[stack.size()-1]; + stack.pop_back(); + if(Intersect(n->volume,volume)) { - stack.push_back(n->childs[0]); - stack.push_back(n->childs[1]); + if(n->isinternal()) + { + stack.push_back(n->childs[0]); + stack.push_back(n->childs[1]); + } + else + { + policy.Process(n); + } } - else - { - policy.Process(n); - } - } - } while(stack.size()>0); - } + } while(stack.size()>0); + } } // DBVT_PREFIX inline void btDbvt::rayTest( const btDbvtNode* root, - const btVector3& rayFrom, - const btVector3& rayTo, - DBVT_IPOLICY) + const btVector3& rayFrom, + const btVector3& rayTo, + DBVT_IPOLICY) { -DBVT_CHECKTYPE - if(root) - { - btVector3 rayDir = (rayTo-rayFrom); - rayDir.normalize (); - - ///what about division by zero? --> just set rayDirection[i] to INF/1e30 - btVector3 rayDirectionInverse; - rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0]; - rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1]; - rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2]; - unsigned int signs[3] = { rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0}; - - - btVector3 resultNormal; + DBVT_CHECKTYPE + if(root) + { + btVector3 rayDir = (rayTo-rayFrom); + rayDir.normalize (); + + ///what about division by zero? --> just set rayDirection[i] to INF/1e30 + btVector3 rayDirectionInverse; + rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0]; + rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1]; + rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2]; + unsigned int signs[3] = { rayDirectionInverse[0] < 0.0, rayDirectionInverse[1] < 0.0, rayDirectionInverse[2] < 0.0}; - btAlignedObjectArray stack; - stack.reserve(SIMPLE_STACKSIZE); - stack.push_back(root); - do { - const btDbvtNode* node=stack[stack.size()-1]; - stack.pop_back(); + btVector3 resultNormal; - btVector3 bounds[2] = {node->volume.Mins(),node->volume.Maxs()}; - btScalar lambda_max = rayDir.dot(rayTo-rayFrom); - btScalar tmin=1.f,lambda_min=0.f; - bool result1 = btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max); + + btAlignedObjectArray stack; + stack.reserve(SIMPLE_STACKSIZE); + stack.push_back(root); + do { + const btDbvtNode* node=stack[stack.size()-1]; + stack.pop_back(); + + btVector3 bounds[2] = {node->volume.Mins(),node->volume.Maxs()}; + btScalar lambda_max = rayDir.dot(rayTo-rayFrom); + btScalar tmin=1.f,lambda_min=0.f; + bool result1 = btRayAabb2(rayFrom,rayDirectionInverse,signs,bounds,tmin,lambda_min,lambda_max); #ifdef COMPARE_BTRAY_AABB2 - btScalar param=1.f; - bool result2 = btRayAabb(rayFrom,rayTo,node->volume.Mins(),node->volume.Maxs(),param,resultNormal); - btAssert(result1 == result2); + btScalar param=1.f; + bool result2 = btRayAabb(rayFrom,rayTo,node->volume.Mins(),node->volume.Maxs(),param,resultNormal); + btAssert(result1 == result2); #endif //TEST_BTRAY_AABB2 - if(result1) - { - if(node->isinternal()) + if(result1) { - stack.push_back(node->childs[0]); - stack.push_back(node->childs[1]); + if(node->isinternal()) + { + stack.push_back(node->childs[0]); + stack.push_back(node->childs[1]); + } + else + { + policy.Process(node); + } } - else - { - policy.Process(node); - } - } - } while(stack.size()); - } + } while(stack.size()); + } } // @@ -915,174 +915,174 @@ inline void btDbvt::collideKDOP(const btDbvtNode* root, int count, DBVT_IPOLICY) { -DBVT_CHECKTYPE -if(root) - { - const int inside=(1< stack; - int signs[sizeof(unsigned)*8]; - btAssert(count=0)?1:0)+ + const int inside=(1< stack; + int signs[sizeof(unsigned)*8]; + btAssert(count=0)?1:0)+ ((normals[i].y()>=0)?2:0)+ ((normals[i].z()>=0)?4:0); - } - stack.reserve(SIMPLE_STACKSIZE); - stack.push_back(sStkNP(root,0)); - do { - sStkNP se=stack[stack.size()-1]; - bool out=false; - stack.pop_back(); - for(int i=0,j=1;(!out)&&(ivolume.Classify(normals[i],offsets[i],signs[i]); - switch(side) - { - case -1: out=true;break; - case +1: se.mask|=j;break; - } - } } - if(!out) - { - if((se.mask!=inside)&&(se.node->isinternal())) + stack.reserve(SIMPLE_STACKSIZE); + stack.push_back(sStkNP(root,0)); + do { + sStkNP se=stack[stack.size()-1]; + bool out=false; + stack.pop_back(); + for(int i=0,j=1;(!out)&&(ichilds[0],se.mask)); - stack.push_back(sStkNP(se.node->childs[1],se.mask)); - } - else - { - if(policy.AllLeaves(se.node)) enumLeaves(se.node,policy); - } - } - } while(stack.size()); - } -} - -// -DBVT_PREFIX -inline void btDbvt::collideOCL( const btDbvtNode* root, - const btVector3* normals, - const btScalar* offsets, - const btVector3& sortaxis, - int count, - DBVT_IPOLICY, - bool fsort) -{ -DBVT_CHECKTYPE -if(root) - { - const unsigned srtsgns=(sortaxis[0]>=0?1:0)+ - (sortaxis[1]>=0?2:0)+ - (sortaxis[2]>=0?4:0); - const int inside=(1< stock; - btAlignedObjectArray ifree; - btAlignedObjectArray stack; - int signs[sizeof(unsigned)*8]; - btAssert(count=0)?1:0)+ - ((normals[i].y()>=0)?2:0)+ - ((normals[i].z()>=0)?4:0); - } - stock.reserve(SIMPLE_STACKSIZE); - stack.reserve(SIMPLE_STACKSIZE); - ifree.reserve(SIMPLE_STACKSIZE); - stack.push_back(allocate(ifree,stock,sStkNPS(root,0,root->volume.ProjectMinimum(sortaxis,srtsgns)))); - do { - const int id=stack[stack.size()-1]; - sStkNPS se=stock[id]; - stack.pop_back();ifree.push_back(id); - if(se.mask!=inside) - { - bool out=false; - for(int i=0,j=1;(!out)&&(ivolume.Classify(normals[i],offsets[i],signs[i]); - switch(side) + const int side=se.node->volume.Classify(normals[i],offsets[i],signs[i]); + switch(side) { case -1: out=true;break; case +1: se.mask|=j;break; } } } - if(out) continue; - } - if(policy.Descent(se.node)) - { - if(se.node->isinternal()) + if(!out) { - const btDbvtNode* pns[]={ se.node->childs[0],se.node->childs[1]}; - sStkNPS nes[]={ sStkNPS(pns[0],se.mask,pns[0]->volume.ProjectMinimum(sortaxis,srtsgns)), - sStkNPS(pns[1],se.mask,pns[1]->volume.ProjectMinimum(sortaxis,srtsgns))}; - const int q=nes[0].value0)) + if((se.mask!=inside)&&(se.node->isinternal())) { - /* Insert 0 */ - j=nearest(&stack[0],&stock[0],nes[q].value,0,stack.size()); - stack.push_back(0); - #if DBVT_USE_MEMMOVE - memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1)); - #else - for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1]; - #endif - stack[j]=allocate(ifree,stock,nes[q]); - /* Insert 1 */ - j=nearest(&stack[0],&stock[0],nes[1-q].value,j,stack.size()); - stack.push_back(0); - #if DBVT_USE_MEMMOVE - memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1)); - #else - for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1]; - #endif - stack[j]=allocate(ifree,stock,nes[1-q]); + stack.push_back(sStkNP(se.node->childs[0],se.mask)); + stack.push_back(sStkNP(se.node->childs[1],se.mask)); } else { - stack.push_back(allocate(ifree,stock,nes[q])); - stack.push_back(allocate(ifree,stock,nes[1-q])); + if(policy.AllLeaves(se.node)) enumLeaves(se.node,policy); } } - else - { - policy.Process(se.node,se.value); - } + } while(stack.size()); + } +} + +// +DBVT_PREFIX +inline void btDbvt::collideOCL( const btDbvtNode* root, + const btVector3* normals, + const btScalar* offsets, + const btVector3& sortaxis, + int count, + DBVT_IPOLICY, + bool fsort) +{ + DBVT_CHECKTYPE + if(root) + { + const unsigned srtsgns=(sortaxis[0]>=0?1:0)+ + (sortaxis[1]>=0?2:0)+ + (sortaxis[2]>=0?4:0); + const int inside=(1< stock; + btAlignedObjectArray ifree; + btAlignedObjectArray stack; + int signs[sizeof(unsigned)*8]; + btAssert(count=0)?1:0)+ + ((normals[i].y()>=0)?2:0)+ + ((normals[i].z()>=0)?4:0); } - } while(stack.size()); - } + stock.reserve(SIMPLE_STACKSIZE); + stack.reserve(SIMPLE_STACKSIZE); + ifree.reserve(SIMPLE_STACKSIZE); + stack.push_back(allocate(ifree,stock,sStkNPS(root,0,root->volume.ProjectMinimum(sortaxis,srtsgns)))); + do { + const int id=stack[stack.size()-1]; + sStkNPS se=stock[id]; + stack.pop_back();ifree.push_back(id); + if(se.mask!=inside) + { + bool out=false; + for(int i=0,j=1;(!out)&&(ivolume.Classify(normals[i],offsets[i],signs[i]); + switch(side) + { + case -1: out=true;break; + case +1: se.mask|=j;break; + } + } + } + if(out) continue; + } + if(policy.Descent(se.node)) + { + if(se.node->isinternal()) + { + const btDbvtNode* pns[]={ se.node->childs[0],se.node->childs[1]}; + sStkNPS nes[]={ sStkNPS(pns[0],se.mask,pns[0]->volume.ProjectMinimum(sortaxis,srtsgns)), + sStkNPS(pns[1],se.mask,pns[1]->volume.ProjectMinimum(sortaxis,srtsgns))}; + const int q=nes[0].value0)) + { + /* Insert 0 */ + j=nearest(&stack[0],&stock[0],nes[q].value,0,stack.size()); + stack.push_back(0); +#if DBVT_USE_MEMMOVE + memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1)); +#else + for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1]; +#endif + stack[j]=allocate(ifree,stock,nes[q]); + /* Insert 1 */ + j=nearest(&stack[0],&stock[0],nes[1-q].value,j,stack.size()); + stack.push_back(0); +#if DBVT_USE_MEMMOVE + memmove(&stack[j+1],&stack[j],sizeof(int)*(stack.size()-j-1)); +#else + for(int k=stack.size()-1;k>j;--k) stack[k]=stack[k-1]; +#endif + stack[j]=allocate(ifree,stock,nes[1-q]); + } + else + { + stack.push_back(allocate(ifree,stock,nes[q])); + stack.push_back(allocate(ifree,stock,nes[1-q])); + } + } + else + { + policy.Process(se.node,se.value); + } + } + } while(stack.size()); + } } // DBVT_PREFIX inline void btDbvt::collideTU( const btDbvtNode* root, - DBVT_IPOLICY) + DBVT_IPOLICY) { -DBVT_CHECKTYPE -if(root) - { - btAlignedObjectArray stack; - stack.reserve(SIMPLE_STACKSIZE); - stack.push_back(root); - do { - const btDbvtNode* n=stack[stack.size()-1]; - stack.pop_back(); - if(policy.Descent(n)) - { - if(n->isinternal()) - { stack.push_back(n->childs[0]);stack.push_back(n->childs[1]); } - else - { policy.Process(n); } - } - } while(stack.size()>0); - } + DBVT_CHECKTYPE + if(root) + { + btAlignedObjectArray stack; + stack.reserve(SIMPLE_STACKSIZE); + stack.push_back(root); + do { + const btDbvtNode* n=stack[stack.size()-1]; + stack.pop_back(); + if(policy.Descent(n)) + { + if(n->isinternal()) + { stack.push_back(n->childs[0]);stack.push_back(n->childs[1]); } + else + { policy.Process(n); } + } + } while(stack.size()>0); + } } // diff --git a/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp b/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp index 5be3006f3..f0c9a51fe 100644 --- a/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp +++ b/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.cpp @@ -26,19 +26,19 @@ subject to the following restrictions: #if DBVT_BP_PROFILE struct ProfileScope - { +{ __forceinline ProfileScope(btClock& clock,unsigned long& value) : - m_clock(&clock),m_value(&value),m_base(clock.getTimeMicroseconds()) - { - } + m_clock(&clock),m_value(&value),m_base(clock.getTimeMicroseconds()) + { + } __forceinline ~ProfileScope() - { + { (*m_value)+=m_clock->getTimeMicroseconds()-m_base; - } + } btClock* m_clock; unsigned long* m_value; unsigned long m_base; - }; +}; #define SPC(_value_) ProfileScope spc_scope(m_clock,_value_) #else #define SPC(_value_) @@ -52,35 +52,35 @@ struct ProfileScope template static inline void listappend(T* item,T*& list) { -item->links[0]=0; -item->links[1]=list; -if(list) list->links[0]=item; -list=item; + item->links[0]=0; + item->links[1]=list; + if(list) list->links[0]=item; + list=item; } // template static inline void listremove(T* item,T*& list) { -if(item->links[0]) item->links[0]->links[1]=item->links[1]; else list=item->links[1]; -if(item->links[1]) item->links[1]->links[0]=item->links[0]; + if(item->links[0]) item->links[0]->links[1]=item->links[1]; else list=item->links[1]; + if(item->links[1]) item->links[1]->links[0]=item->links[0]; } // template static inline int listcount(T* root) { -int n=0; -while(root) { ++n;root=root->links[1]; } -return(n); + int n=0; + while(root) { ++n;root=root->links[1]; } + return(n); } // template static inline void clear(T& value) { -static const struct ZeroDummy : T {} zerodummy; -value=zerodummy; + static const struct ZeroDummy : T {} zerodummy; + value=zerodummy; } // @@ -90,25 +90,25 @@ value=zerodummy; /* Tree collider */ struct btDbvtTreeCollider : btDbvt::ICollide { -btDbvtBroadphase* pbp; -btDbvtProxy* proxy; - btDbvtTreeCollider(btDbvtBroadphase* p) : pbp(p) {} -void Process(const btDbvtNode* na,const btDbvtNode* nb) + btDbvtBroadphase* pbp; + btDbvtProxy* proxy; + btDbvtTreeCollider(btDbvtBroadphase* p) : pbp(p) {} + void Process(const btDbvtNode* na,const btDbvtNode* nb) { - if(na!=nb) + if(na!=nb) { - btDbvtProxy* pa=(btDbvtProxy*)na->data; - btDbvtProxy* pb=(btDbvtProxy*)nb->data; - #if DBVT_BP_SORTPAIRS - if(pa>pb) btSwap(pa,pb); - #endif - pbp->m_paircache->addOverlappingPair(pa,pb); - ++pbp->m_newpairs; + btDbvtProxy* pa=(btDbvtProxy*)na->data; + btDbvtProxy* pb=(btDbvtProxy*)nb->data; +#if DBVT_BP_SORTPAIRS + if(pa>pb) btSwap(pa,pb); +#endif + pbp->m_paircache->addOverlappingPair(pa,pb); + ++pbp->m_newpairs; } } -void Process(const btDbvtNode* n) + void Process(const btDbvtNode* n) { - Process(n,proxy->leaf); + Process(n,proxy->leaf); } }; @@ -119,88 +119,88 @@ void Process(const btDbvtNode* n) // btDbvtBroadphase::btDbvtBroadphase(btOverlappingPairCache* paircache) { -m_deferedcollide = false; -m_needcleanup = true; -m_releasepaircache = (paircache!=0)?false:true; -m_prediction = 1/(btScalar)2; -m_stageCurrent = 0; -m_fixedleft = 0; -m_fupdates = 1; -m_dupdates = 0; -m_cupdates = 10; -m_newpairs = 1; -m_updates_call = 0; -m_updates_done = 0; -m_updates_ratio = 0; -m_paircache = paircache? - paircache : - new(btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache(); -m_gid = 0; -m_pid = 0; -m_cid = 0; -for(int i=0;i<=STAGECOUNT;++i) + m_deferedcollide = false; + m_needcleanup = true; + m_releasepaircache = (paircache!=0)?false:true; + m_prediction = 1/(btScalar)2; + m_stageCurrent = 0; + m_fixedleft = 0; + m_fupdates = 1; + m_dupdates = 0; + m_cupdates = 10; + m_newpairs = 1; + m_updates_call = 0; + m_updates_done = 0; + m_updates_ratio = 0; + m_paircache = paircache? +paircache : + new(btAlignedAlloc(sizeof(btHashedOverlappingPairCache),16)) btHashedOverlappingPairCache(); + m_gid = 0; + m_pid = 0; + m_cid = 0; + for(int i=0;i<=STAGECOUNT;++i) { - m_stageRoots[i]=0; + m_stageRoots[i]=0; } #if DBVT_BP_PROFILE -clear(m_profiling); + clear(m_profiling); #endif } // btDbvtBroadphase::~btDbvtBroadphase() { -if(m_releasepaircache) -{ - m_paircache->~btOverlappingPairCache(); - btAlignedFree(m_paircache); -} + if(m_releasepaircache) + { + m_paircache->~btOverlappingPairCache(); + btAlignedFree(m_paircache); + } } // btBroadphaseProxy* btDbvtBroadphase::createProxy( const btVector3& aabbMin, - const btVector3& aabbMax, - int /*shapeType*/, - void* userPtr, - short int collisionFilterGroup, - short int collisionFilterMask, - btDispatcher* /*dispatcher*/, - void* /*multiSapProxy*/) + const btVector3& aabbMax, + int /*shapeType*/, + void* userPtr, + short int collisionFilterGroup, + short int collisionFilterMask, + btDispatcher* /*dispatcher*/, + void* /*multiSapProxy*/) { -btDbvtProxy* proxy=new(btAlignedAlloc(sizeof(btDbvtProxy),16)) btDbvtProxy( aabbMin,aabbMax,userPtr, - collisionFilterGroup, - collisionFilterMask); + btDbvtProxy* proxy=new(btAlignedAlloc(sizeof(btDbvtProxy),16)) btDbvtProxy( aabbMin,aabbMax,userPtr, + collisionFilterGroup, + collisionFilterMask); -btDbvtAabbMm aabb = btDbvtVolume::FromMM(aabbMin,aabbMax); + btDbvtAabbMm aabb = btDbvtVolume::FromMM(aabbMin,aabbMax); -//bproxy->aabb = btDbvtVolume::FromMM(aabbMin,aabbMax); -proxy->stage = m_stageCurrent; -proxy->m_uniqueId = ++m_gid; -proxy->leaf = m_sets[0].insert(aabb,proxy); -listappend(proxy,m_stageRoots[m_stageCurrent]); -if(!m_deferedcollide) + //bproxy->aabb = btDbvtVolume::FromMM(aabbMin,aabbMax); + proxy->stage = m_stageCurrent; + proxy->m_uniqueId = ++m_gid; + proxy->leaf = m_sets[0].insert(aabb,proxy); + listappend(proxy,m_stageRoots[m_stageCurrent]); + if(!m_deferedcollide) { - btDbvtTreeCollider collider(this); - collider.proxy=proxy; - btDbvt::collideTV(m_sets[0].m_root,aabb,collider); - btDbvt::collideTV(m_sets[1].m_root,aabb,collider); + btDbvtTreeCollider collider(this); + collider.proxy=proxy; + btDbvt::collideTV(m_sets[0].m_root,aabb,collider); + btDbvt::collideTV(m_sets[1].m_root,aabb,collider); } -return(proxy); + return(proxy); } // void btDbvtBroadphase::destroyProxy( btBroadphaseProxy* absproxy, - btDispatcher* dispatcher) + btDispatcher* dispatcher) { -btDbvtProxy* proxy=(btDbvtProxy*)absproxy; -if(proxy->stage==STAGECOUNT) - m_sets[1].remove(proxy->leaf); + btDbvtProxy* proxy=(btDbvtProxy*)absproxy; + if(proxy->stage==STAGECOUNT) + m_sets[1].remove(proxy->leaf); else - m_sets[0].remove(proxy->leaf); -listremove(proxy,m_stageRoots[proxy->stage]); -m_paircache->removeOverlappingPairsContainingProxy(proxy,dispatcher); -btAlignedFree(proxy); -m_needcleanup=true; + m_sets[0].remove(proxy->leaf); + listremove(proxy,m_stageRoots[proxy->stage]); + m_paircache->removeOverlappingPairsContainingProxy(proxy,dispatcher); + btAlignedFree(proxy); + m_needcleanup=true; } void btDbvtBroadphase::getAabb(btBroadphaseProxy* absproxy,btVector3& aabbMin, btVector3& aabbMax ) const @@ -230,79 +230,79 @@ void btDbvtBroadphase::rayTest(const btVector3& rayFrom,const btVector3& rayTo, BroadphaseRayTester callback(rayCallback); m_sets[0].rayTest( m_sets[0].m_root, - rayFrom, - rayTo, - callback); + rayFrom, + rayTo, + callback); m_sets[1].rayTest( m_sets[1].m_root, - rayFrom, - rayTo, - callback); + rayFrom, + rayTo, + callback); } // void btDbvtBroadphase::setAabb( btBroadphaseProxy* absproxy, - const btVector3& aabbMin, - const btVector3& aabbMax, - btDispatcher* /*dispatcher*/) + const btVector3& aabbMin, + const btVector3& aabbMax, + btDispatcher* /*dispatcher*/) { -btDbvtProxy* proxy=(btDbvtProxy*)absproxy; -ATTRIBUTE_ALIGNED16(btDbvtVolume) aabb=btDbvtVolume::FromMM(aabbMin,aabbMax); + btDbvtProxy* proxy=(btDbvtProxy*)absproxy; + ATTRIBUTE_ALIGNED16(btDbvtVolume) aabb=btDbvtVolume::FromMM(aabbMin,aabbMax); #if DBVT_BP_PREVENTFALSEUPDATE -if(NotEqual(aabb,proxy->leaf->volume)) + if(NotEqual(aabb,proxy->leaf->volume)) #endif { - bool docollide=false; - if(proxy->stage==STAGECOUNT) + bool docollide=false; + if(proxy->stage==STAGECOUNT) {/* fixed -> dynamic set */ - m_sets[1].remove(proxy->leaf); - proxy->leaf=m_sets[0].insert(aabb,proxy); - docollide=true; + m_sets[1].remove(proxy->leaf); + proxy->leaf=m_sets[0].insert(aabb,proxy); + docollide=true; } else {/* dynamic set */ - ++m_updates_call; - if(Intersect(proxy->leaf->volume,aabb)) + ++m_updates_call; + if(Intersect(proxy->leaf->volume,aabb)) {/* Moving */ - const btVector3 delta=aabbMin-proxy->m_aabbMin; - btVector3 velocity(((proxy->m_aabbMax-proxy->m_aabbMin)/2)*m_prediction); - if(delta[0]<0) velocity[0]=-velocity[0]; - if(delta[1]<0) velocity[1]=-velocity[1]; - if(delta[2]<0) velocity[2]=-velocity[2]; - if ( - #ifdef DBVT_BP_MARGIN - m_sets[0].update(proxy->leaf,aabb,velocity,DBVT_BP_MARGIN) - #else - m_sets[0].update(proxy->leaf,aabb,velocity) - #endif - ) + const btVector3 delta=aabbMin-proxy->m_aabbMin; + btVector3 velocity(((proxy->m_aabbMax-proxy->m_aabbMin)/2)*m_prediction); + if(delta[0]<0) velocity[0]=-velocity[0]; + if(delta[1]<0) velocity[1]=-velocity[1]; + if(delta[2]<0) velocity[2]=-velocity[2]; + if ( +#ifdef DBVT_BP_MARGIN + m_sets[0].update(proxy->leaf,aabb,velocity,DBVT_BP_MARGIN) +#else + m_sets[0].update(proxy->leaf,aabb,velocity) +#endif + ) { - ++m_updates_done; - docollide=true; + ++m_updates_done; + docollide=true; } } else {/* Teleporting */ - m_sets[0].update(proxy->leaf,aabb); - ++m_updates_done; - docollide=true; + m_sets[0].update(proxy->leaf,aabb); + ++m_updates_done; + docollide=true; } } - listremove(proxy,m_stageRoots[proxy->stage]); - proxy->m_aabbMin = aabbMin; - proxy->m_aabbMax = aabbMax; - proxy->stage = m_stageCurrent; - listappend(proxy,m_stageRoots[m_stageCurrent]); - if(docollide) + listremove(proxy,m_stageRoots[proxy->stage]); + proxy->m_aabbMin = aabbMin; + proxy->m_aabbMax = aabbMax; + proxy->stage = m_stageCurrent; + listappend(proxy,m_stageRoots[m_stageCurrent]); + if(docollide) { - m_needcleanup=true; - if(!m_deferedcollide) + m_needcleanup=true; + if(!m_deferedcollide) { - btDbvtTreeCollider collider(this); - btDbvt::collideTT(m_sets[1].m_root,proxy->leaf,collider); - btDbvt::collideTT(m_sets[0].m_root,proxy->leaf,collider); + btDbvtTreeCollider collider(this); + btDbvt::collideTT(m_sets[1].m_root,proxy->leaf,collider); + btDbvt::collideTT(m_sets[0].m_root,proxy->leaf,collider); } } } @@ -311,24 +311,24 @@ if(NotEqual(aabb,proxy->leaf->volume)) // void btDbvtBroadphase::calculateOverlappingPairs(btDispatcher* dispatcher) { -collide(dispatcher); + collide(dispatcher); #if DBVT_BP_PROFILE -if(0==(m_pid%DBVT_BP_PROFILING_RATE)) + if(0==(m_pid%DBVT_BP_PROFILING_RATE)) { - printf("fixed(%u) dynamics(%u) pairs(%u)\r\n",m_sets[1].m_leaves,m_sets[0].m_leaves,m_paircache->getNumOverlappingPairs()); - unsigned int total=m_profiling.m_total; - if(total<=0) total=1; - printf("ddcollide: %u%% (%uus)\r\n",(50+m_profiling.m_ddcollide*100)/total,m_profiling.m_ddcollide/DBVT_BP_PROFILING_RATE); - printf("fdcollide: %u%% (%uus)\r\n",(50+m_profiling.m_fdcollide*100)/total,m_profiling.m_fdcollide/DBVT_BP_PROFILING_RATE); - printf("cleanup: %u%% (%uus)\r\n",(50+m_profiling.m_cleanup*100)/total,m_profiling.m_cleanup/DBVT_BP_PROFILING_RATE); - printf("total: %uus\r\n",total/DBVT_BP_PROFILING_RATE); - const unsigned long sum=m_profiling.m_ddcollide+ - m_profiling.m_fdcollide+ - m_profiling.m_cleanup; - printf("leaked: %u%% (%uus)\r\n",100-((50+sum*100)/total),(total-sum)/DBVT_BP_PROFILING_RATE); - printf("job counts: %u%%\r\n",(m_profiling.m_jobcount*100)/((m_sets[0].m_leaves+m_sets[1].m_leaves)*DBVT_BP_PROFILING_RATE)); - clear(m_profiling); - m_clock.reset(); + printf("fixed(%u) dynamics(%u) pairs(%u)\r\n",m_sets[1].m_leaves,m_sets[0].m_leaves,m_paircache->getNumOverlappingPairs()); + unsigned int total=m_profiling.m_total; + if(total<=0) total=1; + printf("ddcollide: %u%% (%uus)\r\n",(50+m_profiling.m_ddcollide*100)/total,m_profiling.m_ddcollide/DBVT_BP_PROFILING_RATE); + printf("fdcollide: %u%% (%uus)\r\n",(50+m_profiling.m_fdcollide*100)/total,m_profiling.m_fdcollide/DBVT_BP_PROFILING_RATE); + printf("cleanup: %u%% (%uus)\r\n",(50+m_profiling.m_cleanup*100)/total,m_profiling.m_cleanup/DBVT_BP_PROFILING_RATE); + printf("total: %uus\r\n",total/DBVT_BP_PROFILING_RATE); + const unsigned long sum=m_profiling.m_ddcollide+ + m_profiling.m_fdcollide+ + m_profiling.m_cleanup; + printf("leaked: %u%% (%uus)\r\n",100-((50+sum*100)/total),(total-sum)/DBVT_BP_PROFILING_RATE); + printf("job counts: %u%%\r\n",(m_profiling.m_jobcount*100)/((m_sets[0].m_leaves+m_sets[1].m_leaves)*DBVT_BP_PROFILING_RATE)); + clear(m_profiling); + m_clock.reset(); } #endif } @@ -336,108 +336,108 @@ if(0==(m_pid%DBVT_BP_PROFILING_RATE)) // void btDbvtBroadphase::collide(btDispatcher* dispatcher) { -SPC(m_profiling.m_total); -/* optimize */ -m_sets[0].optimizeIncremental(1+(m_sets[0].m_leaves*m_dupdates)/100); -if(m_fixedleft) + SPC(m_profiling.m_total); + /* optimize */ + m_sets[0].optimizeIncremental(1+(m_sets[0].m_leaves*m_dupdates)/100); + if(m_fixedleft) { - const int count=1+(m_sets[1].m_leaves*m_fupdates)/100; - m_sets[1].optimizeIncremental(1+(m_sets[1].m_leaves*m_fupdates)/100); - m_fixedleft=btMax(0,m_fixedleft-count); + const int count=1+(m_sets[1].m_leaves*m_fupdates)/100; + m_sets[1].optimizeIncremental(1+(m_sets[1].m_leaves*m_fupdates)/100); + m_fixedleft=btMax(0,m_fixedleft-count); } -/* dynamic -> fixed set */ -m_stageCurrent=(m_stageCurrent+1)%STAGECOUNT; -btDbvtProxy* current=m_stageRoots[m_stageCurrent]; -if(current) + /* dynamic -> fixed set */ + m_stageCurrent=(m_stageCurrent+1)%STAGECOUNT; + btDbvtProxy* current=m_stageRoots[m_stageCurrent]; + if(current) { - btDbvtTreeCollider collider(this); - do { - btDbvtProxy* next=current->links[1]; - listremove(current,m_stageRoots[current->stage]); - listappend(current,m_stageRoots[STAGECOUNT]); - #if DBVT_BP_ACCURATESLEEPING - m_paircache->removeOverlappingPairsContainingProxy(current,dispatcher); - collider.proxy=current; - btDbvt::collideTV(m_sets[0].m_root,current->aabb,collider); - btDbvt::collideTV(m_sets[1].m_root,current->aabb,collider); - #endif - m_sets[0].remove(current->leaf); - ATTRIBUTE_ALIGNED16(btDbvtVolume) curAabb=btDbvtVolume::FromMM(current->m_aabbMin,current->m_aabbMax); - current->leaf = m_sets[1].insert(curAabb,current); - current->stage = STAGECOUNT; - current = next; + btDbvtTreeCollider collider(this); + do { + btDbvtProxy* next=current->links[1]; + listremove(current,m_stageRoots[current->stage]); + listappend(current,m_stageRoots[STAGECOUNT]); +#if DBVT_BP_ACCURATESLEEPING + m_paircache->removeOverlappingPairsContainingProxy(current,dispatcher); + collider.proxy=current; + btDbvt::collideTV(m_sets[0].m_root,current->aabb,collider); + btDbvt::collideTV(m_sets[1].m_root,current->aabb,collider); +#endif + m_sets[0].remove(current->leaf); + ATTRIBUTE_ALIGNED16(btDbvtVolume) curAabb=btDbvtVolume::FromMM(current->m_aabbMin,current->m_aabbMax); + current->leaf = m_sets[1].insert(curAabb,current); + current->stage = STAGECOUNT; + current = next; } while(current); - m_fixedleft=m_sets[1].m_leaves; - m_needcleanup=true; + m_fixedleft=m_sets[1].m_leaves; + m_needcleanup=true; } -/* collide dynamics */ + /* collide dynamics */ { - btDbvtTreeCollider collider(this); - if(m_deferedcollide) + btDbvtTreeCollider collider(this); + if(m_deferedcollide) { - SPC(m_profiling.m_fdcollide); - btDbvt::collideTT(m_sets[0].m_root,m_sets[1].m_root,collider); + SPC(m_profiling.m_fdcollide); + btDbvt::collideTT(m_sets[0].m_root,m_sets[1].m_root,collider); } - if(m_deferedcollide) + if(m_deferedcollide) { - SPC(m_profiling.m_ddcollide); - btDbvt::collideTT(m_sets[0].m_root,m_sets[0].m_root,collider); + SPC(m_profiling.m_ddcollide); + btDbvt::collideTT(m_sets[0].m_root,m_sets[0].m_root,collider); } } -/* clean up */ -if(m_needcleanup) + /* clean up */ + if(m_needcleanup) { - SPC(m_profiling.m_cleanup); - btBroadphasePairArray& pairs=m_paircache->getOverlappingPairArray(); - if(pairs.size()>0) + SPC(m_profiling.m_cleanup); + btBroadphasePairArray& pairs=m_paircache->getOverlappingPairArray(); + if(pairs.size()>0) { - const int ci=pairs.size(); - int ni=btMin(ci,btMax(m_newpairs,(ci*m_cupdates)/100)); - for(int i=0;i(m_newpairs,(ci*m_cupdates)/100)); + for(int i=0;ileaf->volume,pb->leaf->volume)) + btBroadphasePair& p=pairs[(m_cid+i)%ci]; + btDbvtProxy* pa=(btDbvtProxy*)p.m_pProxy0; + btDbvtProxy* pb=(btDbvtProxy*)p.m_pProxy1; + if(!Intersect(pa->leaf->volume,pb->leaf->volume)) { - #if DBVT_BP_SORTPAIRS - if(pa>pb) btSwap(pa,pb); - #endif - m_paircache->removeOverlappingPair(pa,pb,dispatcher); - --ni;--i; +#if DBVT_BP_SORTPAIRS + if(pa>pb) btSwap(pa,pb); +#endif + m_paircache->removeOverlappingPair(pa,pb,dispatcher); + --ni;--i; } } - if(pairs.size()>0) m_cid=(m_cid+ni)%pairs.size(); else m_cid=0; + if(pairs.size()>0) m_cid=(m_cid+ni)%pairs.size(); else m_cid=0; } } -++m_pid; -m_newpairs=1; -m_needcleanup=false; -if(m_updates_call>0) + ++m_pid; + m_newpairs=1; + m_needcleanup=false; + if(m_updates_call>0) { m_updates_ratio=m_updates_done/(btScalar)m_updates_call; } else { m_updates_ratio=0; } -m_updates_done/=2; -m_updates_call/=2; + m_updates_done/=2; + m_updates_call/=2; } // void btDbvtBroadphase::optimize() { -m_sets[0].optimizeTopDown(); -m_sets[1].optimizeTopDown(); + m_sets[0].optimizeTopDown(); + m_sets[1].optimizeTopDown(); } // btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache() { -return(m_paircache); + return(m_paircache); } // const btOverlappingPairCache* btDbvtBroadphase::getOverlappingPairCache() const { -return(m_paircache); + return(m_paircache); } // @@ -446,16 +446,16 @@ void btDbvtBroadphase::getBroadphaseAabb(btVector3& aabbMin,btVector3& aab ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds; -if(!m_sets[0].empty()) - if(!m_sets[1].empty()) Merge( m_sets[0].m_root->volume, - m_sets[1].m_root->volume,bounds); - else - bounds=m_sets[0].m_root->volume; -else if(!m_sets[1].empty()) bounds=m_sets[1].m_root->volume; - else - bounds=btDbvtVolume::FromCR(btVector3(0,0,0),0); -aabbMin=bounds.Mins(); -aabbMax=bounds.Maxs(); + if(!m_sets[0].empty()) + if(!m_sets[1].empty()) Merge( m_sets[0].m_root->volume, + m_sets[1].m_root->volume,bounds); + else + bounds=m_sets[0].m_root->volume; + else if(!m_sets[1].empty()) bounds=m_sets[1].m_root->volume; + else + bounds=btDbvtVolume::FromCR(btVector3(0,0,0),0); + aabbMin=bounds.Mins(); + aabbMax=bounds.Maxs(); } // @@ -466,9 +466,9 @@ void btDbvtBroadphase::printStats() #if DBVT_BP_ENABLE_BENCHMARK struct btBroadphaseBenchmark - { +{ struct Experiment - { + { const char* name; int object_count; int update_count; @@ -476,109 +476,109 @@ struct btBroadphaseBenchmark int iterations; btScalar speed; btScalar amplitude; - }; + }; struct Object - { + { btVector3 center; btVector3 extents; btBroadphaseProxy* proxy; btScalar time; void update(btScalar speed,btScalar amplitude,btBroadphaseInterface* pbi) - { + { time += speed; center[0] = btCos(time*(btScalar)2.17)*amplitude+ - btSin(time)*amplitude/2; + btSin(time)*amplitude/2; center[1] = btCos(time*(btScalar)1.38)*amplitude+ - btSin(time)*amplitude; + btSin(time)*amplitude; center[2] = btSin(time*(btScalar)0.777)*amplitude; pbi->setAabb(proxy,center-extents,center+extents,0); - } - }; + } + }; static int UnsignedRand(int range=RAND_MAX-1) { return(rand()%(range+1)); } static btScalar UnitRand() { return(UnsignedRand(16384)/(btScalar)16384); } static void OutputTime(const char* name,btClock& c,unsigned count=0) - { + { const unsigned long us=c.getTimeMicroseconds(); const unsigned long ms=(us+500)/1000; const btScalar sec=us/(btScalar)(1000*1000); if(count>0) printf("%s : %u us (%u ms), %.2f/s\r\n",name,us,ms,count/sec); - else + else printf("%s : %u us (%u ms)\r\n",name,us,ms); - } - }; + } +}; void btDbvtBroadphase::benchmark(btBroadphaseInterface* pbi) { -static const btBroadphaseBenchmark::Experiment experiments[]= + static const btBroadphaseBenchmark::Experiment experiments[]= { - {"1024o.10%",1024,10,0,8192,(btScalar)0.005,(btScalar)100}, - /*{"4096o.10%",4096,10,0,8192,(btScalar)0.005,(btScalar)100}, - {"8192o.10%",8192,10,0,8192,(btScalar)0.005,(btScalar)100},*/ + {"1024o.10%",1024,10,0,8192,(btScalar)0.005,(btScalar)100}, + /*{"4096o.10%",4096,10,0,8192,(btScalar)0.005,(btScalar)100}, + {"8192o.10%",8192,10,0,8192,(btScalar)0.005,(btScalar)100},*/ }; -static const int nexperiments=sizeof(experiments)/sizeof(experiments[0]); -btAlignedObjectArray objects; -btClock wallclock; -/* Begin */ -for(int iexp=0;iexp objects; + btClock wallclock; + /* Begin */ + for(int iexp=0;iexpcenter[0]=btBroadphaseBenchmark::UnitRand()*50; - po->center[1]=btBroadphaseBenchmark::UnitRand()*50; - po->center[2]=btBroadphaseBenchmark::UnitRand()*50; - po->extents[0]=btBroadphaseBenchmark::UnitRand()*2+2; - po->extents[1]=btBroadphaseBenchmark::UnitRand()*2+2; - po->extents[2]=btBroadphaseBenchmark::UnitRand()*2+2; - po->time=btBroadphaseBenchmark::UnitRand()*2000; - po->proxy=pbi->createProxy(po->center-po->extents,po->center+po->extents,0,po,1,1,0,0); - objects.push_back(po); + btBroadphaseBenchmark::Object* po=new btBroadphaseBenchmark::Object(); + po->center[0]=btBroadphaseBenchmark::UnitRand()*50; + po->center[1]=btBroadphaseBenchmark::UnitRand()*50; + po->center[2]=btBroadphaseBenchmark::UnitRand()*50; + po->extents[0]=btBroadphaseBenchmark::UnitRand()*2+2; + po->extents[1]=btBroadphaseBenchmark::UnitRand()*2+2; + po->extents[2]=btBroadphaseBenchmark::UnitRand()*2+2; + po->time=btBroadphaseBenchmark::UnitRand()*2000; + po->proxy=pbi->createProxy(po->center-po->extents,po->center+po->extents,0,po,1,1,0,0); + objects.push_back(po); } - btBroadphaseBenchmark::OutputTime("\tInitialization",wallclock); - /* First update */ - wallclock.reset(); - for(int i=0;iupdate(speed,amplitude,pbi); + objects[i]->update(speed,amplitude,pbi); } - btBroadphaseBenchmark::OutputTime("\tFirst update",wallclock); - /* Updates */ - wallclock.reset(); - for(int i=0;iupdate(speed,amplitude,pbi); + objects[j]->update(speed,amplitude,pbi); } - pbi->calculateOverlappingPairs(0); + pbi->calculateOverlappingPairs(0); } - btBroadphaseBenchmark::OutputTime("\tUpdate",wallclock,experiment.iterations); - /* Clean up */ - wallclock.reset(); - for(int i=0;idestroyProxy(objects[i]->proxy,0); - delete objects[i]; + pbi->destroyProxy(objects[i]->proxy,0); + delete objects[i]; } - objects.resize(0); - btBroadphaseBenchmark::OutputTime("\tRelease",wallclock); + objects.resize(0); + btBroadphaseBenchmark::OutputTime("\tRelease",wallclock); } } diff --git a/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h b/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h index f2b5f70eb..538488517 100644 --- a/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h +++ b/src/BulletCollision/BroadphaseCollision/btDbvtBroadphase.h @@ -31,8 +31,8 @@ subject to the following restrictions: #define DBVT_BP_MARGIN (btScalar)0.05 #if DBVT_BP_PROFILE - #define DBVT_BP_PROFILING_RATE 256 - #include "LinearMath/btQuickprof.h" +#define DBVT_BP_PROFILING_RATE 256 +#include "LinearMath/btQuickprof.h" #endif // @@ -40,16 +40,16 @@ subject to the following restrictions: // struct btDbvtProxy : btBroadphaseProxy { -/* Fields */ -//btDbvtAabbMm aabb; -btDbvtNode* leaf; -btDbvtProxy* links[2]; -int stage; -/* ctor */ -btDbvtProxy(const btVector3& aabbMin,const btVector3& aabbMax,void* userPtr,short int collisionFilterGroup, short int collisionFilterMask) : + /* Fields */ + //btDbvtAabbMm aabb; + btDbvtNode* leaf; + btDbvtProxy* links[2]; + int stage; + /* ctor */ + btDbvtProxy(const btVector3& aabbMin,const btVector3& aabbMax,void* userPtr,short int collisionFilterGroup, short int collisionFilterMask) : btBroadphaseProxy(aabbMin,aabbMax,userPtr,collisionFilterGroup,collisionFilterMask) { - links[0]=links[1]=0; + links[0]=links[1]=0; } }; @@ -60,60 +60,60 @@ typedef btAlignedObjectArray btDbvtProxyArray; ///This is a very fast broadphase, especially for very dynamic worlds where many objects are moving. Its insert/add and remove of objects is generally faster than the sweep and prune broadphases btAxisSweep3 and bt32BitAxisSweep3. struct btDbvtBroadphase : btBroadphaseInterface { -/* Config */ -enum { + /* Config */ + enum { DYNAMIC_SET = 0, /* Dynamic set index */ FIXED_SET = 1, /* Fixed set index */ STAGECOUNT = 2 /* Number of stages */ - }; -/* Fields */ -btDbvt m_sets[2]; // Dbvt sets -btDbvtProxy* m_stageRoots[STAGECOUNT+1]; // Stages list -btOverlappingPairCache* m_paircache; // Pair cache -btScalar m_prediction; // Velocity prediction -int m_stageCurrent; // Current stage -int m_fupdates; // % of fixed updates per frame -int m_dupdates; // % of dynamic updates per frame -int m_cupdates; // % of cleanup updates per frame -int m_newpairs; // Number of pairs created -int m_fixedleft; // Fixed optimization left -unsigned m_updates_call; // Number of updates call -unsigned m_updates_done; // Number of updates done -btScalar m_updates_ratio; // m_updates_done/m_updates_call -int m_pid; // Parse id -int m_cid; // Cleanup index -int m_gid; // Gen id -bool m_releasepaircache; // Release pair cache on delete -bool m_deferedcollide; // Defere dynamic/static collision to collide call -bool m_needcleanup; // Need to run cleanup? + }; + /* Fields */ + btDbvt m_sets[2]; // Dbvt sets + btDbvtProxy* m_stageRoots[STAGECOUNT+1]; // Stages list + btOverlappingPairCache* m_paircache; // Pair cache + btScalar m_prediction; // Velocity prediction + int m_stageCurrent; // Current stage + int m_fupdates; // % of fixed updates per frame + int m_dupdates; // % of dynamic updates per frame + int m_cupdates; // % of cleanup updates per frame + int m_newpairs; // Number of pairs created + int m_fixedleft; // Fixed optimization left + unsigned m_updates_call; // Number of updates call + unsigned m_updates_done; // Number of updates done + btScalar m_updates_ratio; // m_updates_done/m_updates_call + int m_pid; // Parse id + int m_cid; // Cleanup index + int m_gid; // Gen id + bool m_releasepaircache; // Release pair cache on delete + bool m_deferedcollide; // Defere dynamic/static collision to collide call + bool m_needcleanup; // Need to run cleanup? #if DBVT_BP_PROFILE -btClock m_clock; -struct { + btClock m_clock; + struct { unsigned long m_total; unsigned long m_ddcollide; unsigned long m_fdcollide; unsigned long m_cleanup; unsigned long m_jobcount; - } m_profiling; + } m_profiling; #endif -/* Methods */ -btDbvtBroadphase(btOverlappingPairCache* paircache=0); -~btDbvtBroadphase(); -void collide(btDispatcher* dispatcher); -void optimize(); -/* btBroadphaseInterface Implementation */ -btBroadphaseProxy* createProxy(const btVector3& aabbMin,const btVector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy); -void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher); -void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher); -virtual void rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback); + /* Methods */ + btDbvtBroadphase(btOverlappingPairCache* paircache=0); + ~btDbvtBroadphase(); + void collide(btDispatcher* dispatcher); + void optimize(); + /* btBroadphaseInterface Implementation */ + btBroadphaseProxy* createProxy(const btVector3& aabbMin,const btVector3& aabbMax,int shapeType,void* userPtr,short int collisionFilterGroup,short int collisionFilterMask,btDispatcher* dispatcher,void* multiSapProxy); + void destroyProxy(btBroadphaseProxy* proxy,btDispatcher* dispatcher); + void setAabb(btBroadphaseProxy* proxy,const btVector3& aabbMin,const btVector3& aabbMax,btDispatcher* dispatcher); + virtual void rayTest(const btVector3& rayFrom,const btVector3& rayTo, btBroadphaseRayCallback& rayCallback); -virtual void getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const; -void calculateOverlappingPairs(btDispatcher* dispatcher); -btOverlappingPairCache* getOverlappingPairCache(); -const btOverlappingPairCache* getOverlappingPairCache() const; -void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const; -void printStats(); -static void benchmark(btBroadphaseInterface*); + virtual void getAabb(btBroadphaseProxy* proxy,btVector3& aabbMin, btVector3& aabbMax ) const; + void calculateOverlappingPairs(btDispatcher* dispatcher); + btOverlappingPairCache* getOverlappingPairCache(); + const btOverlappingPairCache* getOverlappingPairCache() const; + void getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const; + void printStats(); + static void benchmark(btBroadphaseInterface*); }; #endif diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index ec9f0e2a7..1c9238a7d 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -150,98 +150,98 @@ bool btSoftBody::checkFace(int node0,int node1,int node2) const // btSoftBody::Material* btSoftBody::appendMaterial() { -Material* pm=new(btAlignedAlloc(sizeof(Material),16)) Material(); -if(m_materials.size()>0) - *pm=*m_materials[0]; + Material* pm=new(btAlignedAlloc(sizeof(Material),16)) Material(); + if(m_materials.size()>0) + *pm=*m_materials[0]; else - ZeroInitialize(*pm); -m_materials.push_back(pm); -return(pm); + ZeroInitialize(*pm); + m_materials.push_back(pm); + return(pm); } // void btSoftBody::appendNote( const char* text, - const btVector3& o, - const btVector4& c, - Node* n0, - Node* n1, - Node* n2, - Node* n3) + const btVector3& o, + const btVector4& c, + Node* n0, + Node* n1, + Node* n2, + Node* n3) { -Note n; -ZeroInitialize(n); -n.m_rank = 0; -n.m_text = text; -n.m_offset = o; -n.m_coords[0] = c.x(); -n.m_coords[1] = c.y(); -n.m_coords[2] = c.z(); -n.m_coords[3] = c.w(); -n.m_nodes[0] = n0;n.m_rank+=n0?1:0; -n.m_nodes[1] = n1;n.m_rank+=n1?1:0; -n.m_nodes[2] = n2;n.m_rank+=n2?1:0; -n.m_nodes[3] = n3;n.m_rank+=n3?1:0; -m_notes.push_back(n); + Note n; + ZeroInitialize(n); + n.m_rank = 0; + n.m_text = text; + n.m_offset = o; + n.m_coords[0] = c.x(); + n.m_coords[1] = c.y(); + n.m_coords[2] = c.z(); + n.m_coords[3] = c.w(); + n.m_nodes[0] = n0;n.m_rank+=n0?1:0; + n.m_nodes[1] = n1;n.m_rank+=n1?1:0; + n.m_nodes[2] = n2;n.m_rank+=n2?1:0; + n.m_nodes[3] = n3;n.m_rank+=n3?1:0; + m_notes.push_back(n); } // void btSoftBody::appendNote( const char* text, - const btVector3& o, - Node* feature) + const btVector3& o, + Node* feature) { -appendNote(text,o,btVector4(1,0,0,0),feature); + appendNote(text,o,btVector4(1,0,0,0),feature); } // void btSoftBody::appendNote( const char* text, - const btVector3& o, - Link* feature) + const btVector3& o, + Link* feature) { -static const btScalar w=1/(btScalar)2; -appendNote(text,o,btVector4(w,w,0,0), feature->m_n[0], - feature->m_n[1]); + static const btScalar w=1/(btScalar)2; + appendNote(text,o,btVector4(w,w,0,0), feature->m_n[0], + feature->m_n[1]); } - + // void btSoftBody::appendNote( const char* text, - const btVector3& o, - Face* feature) + const btVector3& o, + Face* feature) { -static const btScalar w=1/(btScalar)3; -appendNote(text,o,btVector4(w,w,w,0), feature->m_n[0], - feature->m_n[1], - feature->m_n[2]); + static const btScalar w=1/(btScalar)3; + appendNote(text,o,btVector4(w,w,w,0), feature->m_n[0], + feature->m_n[1], + feature->m_n[2]); } // void btSoftBody::appendNode( const btVector3& x,btScalar m) { -if(m_nodes.capacity()==m_nodes.size()) + if(m_nodes.capacity()==m_nodes.size()) { - pointersToIndices(); - m_nodes.reserve(m_nodes.size()*2+1); - indicesToPointers(); + pointersToIndices(); + m_nodes.reserve(m_nodes.size()*2+1); + indicesToPointers(); } -const btScalar margin=getCollisionShape()->getMargin(); -m_nodes.push_back(Node()); -Node& n=m_nodes[m_nodes.size()-1]; -ZeroInitialize(n); -n.m_x = x; -n.m_q = n.m_x; -n.m_im = m>0?1/m:0; -n.m_material = m_materials[0]; -n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x,margin),&n); + const btScalar margin=getCollisionShape()->getMargin(); + m_nodes.push_back(Node()); + Node& n=m_nodes[m_nodes.size()-1]; + ZeroInitialize(n); + n.m_x = x; + n.m_q = n.m_x; + n.m_im = m>0?1/m:0; + n.m_material = m_materials[0]; + n.m_leaf = m_ndbvt.insert(btDbvtVolume::FromCR(n.m_x,margin),&n); } // void btSoftBody::appendLink(int model,Material* mat) { -Link l; -if(model>=0) - l=m_links[model]; + Link l; + if(model>=0) + l=m_links[model]; else { ZeroInitialize(l);l.m_material=mat?mat:m_materials[0]; } -m_links.push_back(l); + m_links.push_back(l); } // @@ -273,12 +273,12 @@ void btSoftBody::appendLink( Node* node0, // void btSoftBody::appendFace(int model,Material* mat) { -Face f; -if(model>=0) + Face f; + if(model>=0) { f=m_faces[model]; } else { ZeroInitialize(f);f.m_material=mat?mat:m_materials[0]; } -m_faces.push_back(f); + m_faces.push_back(f); } // @@ -319,54 +319,54 @@ void btSoftBody::appendAnchor(int node,btRigidBody* body) // void btSoftBody::appendLinearJoint(const LJoint::Specs& specs,Cluster* body0,Body body1) { -LJoint* pj = new(btAlignedAlloc(sizeof(LJoint),16)) LJoint(); -pj->m_bodies[0] = body0; -pj->m_bodies[1] = body1; -pj->m_refs[0] = pj->m_bodies[0].xform().inverse()*specs.position; -pj->m_refs[1] = pj->m_bodies[1].xform().inverse()*specs.position; -pj->m_cfm = specs.cfm; -pj->m_erp = specs.erp; -pj->m_split = specs.split; -m_joints.push_back(pj); + LJoint* pj = new(btAlignedAlloc(sizeof(LJoint),16)) LJoint(); + pj->m_bodies[0] = body0; + pj->m_bodies[1] = body1; + pj->m_refs[0] = pj->m_bodies[0].xform().inverse()*specs.position; + pj->m_refs[1] = pj->m_bodies[1].xform().inverse()*specs.position; + pj->m_cfm = specs.cfm; + pj->m_erp = specs.erp; + pj->m_split = specs.split; + m_joints.push_back(pj); } // void btSoftBody::appendLinearJoint(const LJoint::Specs& specs,Body body) { -appendLinearJoint(specs,m_clusters[0],body); + appendLinearJoint(specs,m_clusters[0],body); } // void btSoftBody::appendLinearJoint(const LJoint::Specs& specs,btSoftBody* body) { -appendLinearJoint(specs,m_clusters[0],body->m_clusters[0]); + appendLinearJoint(specs,m_clusters[0],body->m_clusters[0]); } // void btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Cluster* body0,Body body1) { -AJoint* pj = new(btAlignedAlloc(sizeof(AJoint),16)) AJoint(); -pj->m_bodies[0] = body0; -pj->m_bodies[1] = body1; -pj->m_refs[0] = pj->m_bodies[0].xform().inverse().getBasis()*specs.axis; -pj->m_refs[1] = pj->m_bodies[1].xform().inverse().getBasis()*specs.axis; -pj->m_cfm = specs.cfm; -pj->m_erp = specs.erp; -pj->m_split = specs.split; -pj->m_icontrol = specs.icontrol; -m_joints.push_back(pj); + AJoint* pj = new(btAlignedAlloc(sizeof(AJoint),16)) AJoint(); + pj->m_bodies[0] = body0; + pj->m_bodies[1] = body1; + pj->m_refs[0] = pj->m_bodies[0].xform().inverse().getBasis()*specs.axis; + pj->m_refs[1] = pj->m_bodies[1].xform().inverse().getBasis()*specs.axis; + pj->m_cfm = specs.cfm; + pj->m_erp = specs.erp; + pj->m_split = specs.split; + pj->m_icontrol = specs.icontrol; + m_joints.push_back(pj); } // void btSoftBody::appendAngularJoint(const AJoint::Specs& specs,Body body) { -appendAngularJoint(specs,m_clusters[0],body); + appendAngularJoint(specs,m_clusters[0],body); } // void btSoftBody::appendAngularJoint(const AJoint::Specs& specs,btSoftBody* body) { -appendAngularJoint(specs,m_clusters[0],body->m_clusters[0]); + appendAngularJoint(specs,m_clusters[0],body->m_clusters[0]); } // @@ -455,8 +455,8 @@ void btSoftBody::setTotalMass(btScalar mass,bool fromfaces) { const Face& f=m_faces[i]; const btScalar twicearea=AreaOf( f.m_n[0]->m_x, - f.m_n[1]->m_x, - f.m_n[2]->m_x); + f.m_n[1]->m_x, + f.m_n[2]->m_x); for(int j=0;j<3;++j) { f.m_n[j]->m_im+=twicearea; @@ -502,19 +502,19 @@ void btSoftBody::transform(const btTransform& trs) // void btSoftBody::translate(const btVector3& trs) { -btTransform t; -t.setIdentity(); -t.setOrigin(trs); -transform(t); + btTransform t; + t.setIdentity(); + t.setOrigin(trs); + transform(t); } // void btSoftBody::rotate( const btQuaternion& rot) { -btTransform t; -t.setIdentity(); -t.setRotation(rot); -transform(t); + btTransform t; + t.setIdentity(); + t.setRotation(rot); + transform(t); } // @@ -553,8 +553,8 @@ void btSoftBody::setPose(bool bvolume,bool bframe) { Node& n=m_nodes[i]; m_pose.m_wgh[i]= n.m_im>0 ? - 1/(m_nodes[i].m_im*tmass) : - kmass/tmass; + 1/(m_nodes[i].m_im*tmass) : + kmass/tmass; } /* Pos */ const btVector3 com=evaluateCom(); @@ -569,16 +569,16 @@ void btSoftBody::setPose(bool bvolume,bool bframe) m_pose.m_scl.setIdentity(); /* Aqq */ m_pose.m_aqq[0] = - m_pose.m_aqq[1] = - m_pose.m_aqq[2] = btVector3(0,0,0); + m_pose.m_aqq[1] = + m_pose.m_aqq[2] = btVector3(0,0,0); for( i=0,ni=m_nodes.size();im_nodes.size();im_nodes.size();im_nodes[i]->m_x*cluster->m_masses[i]; + com+=cluster->m_nodes[i]->m_x*cluster->m_masses[i]; } -return(com*cluster->m_imass); + return(com*cluster->m_imass); } // btVector3 btSoftBody::clusterCom(int cluster) const { -return(clusterCom(m_clusters[cluster])); + return(clusterCom(m_clusters[cluster])); } // btVector3 btSoftBody::clusterVelocity(const Cluster* cluster,const btVector3& rpos) { -return(cluster->m_lv+cross(cluster->m_av,rpos)); + return(cluster->m_lv+cross(cluster->m_av,rpos)); } // void btSoftBody::clusterVImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse) { -const btVector3 li=cluster->m_imass*impulse; -const btVector3 ai=cluster->m_invwi*cross(rpos,impulse); -cluster->m_vimpulses[0]+=li;cluster->m_lv+=li; -cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai; -cluster->m_nvimpulses++; + const btVector3 li=cluster->m_imass*impulse; + const btVector3 ai=cluster->m_invwi*cross(rpos,impulse); + cluster->m_vimpulses[0]+=li;cluster->m_lv+=li; + cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai; + cluster->m_nvimpulses++; } // void btSoftBody::clusterDImpulse(Cluster* cluster,const btVector3& rpos,const btVector3& impulse) { -const btVector3 li=cluster->m_imass*impulse; -const btVector3 ai=cluster->m_invwi*cross(rpos,impulse); -cluster->m_dimpulses[0]+=li; -cluster->m_dimpulses[1]+=ai; -cluster->m_ndimpulses++; + const btVector3 li=cluster->m_imass*impulse; + const btVector3 ai=cluster->m_invwi*cross(rpos,impulse); + cluster->m_dimpulses[0]+=li; + cluster->m_dimpulses[1]+=ai; + cluster->m_ndimpulses++; } // void btSoftBody::clusterImpulse(Cluster* cluster,const btVector3& rpos,const Impulse& impulse) { -if(impulse.m_asVelocity) clusterVImpulse(cluster,rpos,impulse.m_velocity); -if(impulse.m_asDrift) clusterDImpulse(cluster,rpos,impulse.m_drift); + if(impulse.m_asVelocity) clusterVImpulse(cluster,rpos,impulse.m_velocity); + if(impulse.m_asDrift) clusterDImpulse(cluster,rpos,impulse.m_drift); } // void btSoftBody::clusterVAImpulse(Cluster* cluster,const btVector3& impulse) { -const btVector3 ai=cluster->m_invwi*impulse; -cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai; -cluster->m_nvimpulses++; + const btVector3 ai=cluster->m_invwi*impulse; + cluster->m_vimpulses[1]+=ai;cluster->m_av+=ai; + cluster->m_nvimpulses++; } // void btSoftBody::clusterDAImpulse(Cluster* cluster,const btVector3& impulse) { -const btVector3 ai=cluster->m_invwi*impulse; -cluster->m_dimpulses[1]+=ai; -cluster->m_ndimpulses++; + const btVector3 ai=cluster->m_invwi*impulse; + cluster->m_dimpulses[1]+=ai; + cluster->m_ndimpulses++; } // void btSoftBody::clusterAImpulse(Cluster* cluster,const Impulse& impulse) { -if(impulse.m_asVelocity) clusterVAImpulse(cluster,impulse.m_velocity); -if(impulse.m_asDrift) clusterDAImpulse(cluster,impulse.m_drift); + if(impulse.m_asVelocity) clusterVAImpulse(cluster,impulse.m_velocity); + if(impulse.m_asDrift) clusterDAImpulse(cluster,impulse.m_drift); } // void btSoftBody::clusterDCImpulse(Cluster* cluster,const btVector3& impulse) { -cluster->m_dimpulses[0]+=impulse*cluster->m_imass; -cluster->m_ndimpulses++; + cluster->m_dimpulses[0]+=impulse*cluster->m_imass; + cluster->m_ndimpulses++; } // @@ -753,9 +753,9 @@ int btSoftBody::generateBendingConstraints(int distance,Material* mat) // void btSoftBody::randomizeConstraints() { -unsigned long seed=243703; + unsigned long seed=243703; #define NEXTRAND (seed=(1664525L*seed+1013904223L)&0xffffffff) -int i,ni; + int i,ni; for(i=0,ni=m_links.size();im_leaf) m_cdbvt.remove(c->m_leaf); -c->~Cluster(); -btAlignedFree(c); -m_clusters.remove(c); + Cluster* c=m_clusters[index]; + if(c->m_leaf) m_cdbvt.remove(c->m_leaf); + c->~Cluster(); + btAlignedFree(c); + m_clusters.remove(c); } // void btSoftBody::releaseClusters() { -while(m_clusters.size()>0) releaseCluster(0); + while(m_clusters.size()>0) releaseCluster(0); } // int btSoftBody::generateClusters(int k,int maxiterations) { -int i; -releaseClusters(); -m_clusters.resize(btMin(k,m_nodes.size())); -for(i=0;im_collide= true; + m_clusters[i] = new(btAlignedAlloc(sizeof(Cluster),16)) Cluster(); + m_clusters[i]->m_collide= true; } -k=m_clusters.size(); -if(k>0) + k=m_clusters.size(); + if(k>0) { - /* Initialize */ - btAlignedObjectArray centers; - btVector3 cog(0,0,0); - int i; - for(i=0;i centers; + btVector3 cog(0,0,0); + int i; + for(i=0;im_nodes.push_back(&m_nodes[i]); + cog+=m_nodes[i].m_x; + m_clusters[(i*29873)%m_clusters.size()]->m_nodes.push_back(&m_nodes[i]); } - cog/=(btScalar)m_nodes.size(); - centers.resize(k,cog); - /* Iterate */ - const btScalar slope=16; - bool changed; - int iterations=0; - do { - const btScalar w=2-btMin(1,iterations/slope); - changed=false; - iterations++; - int i; + cog/=(btScalar)m_nodes.size(); + centers.resize(k,cog); + /* Iterate */ + const btScalar slope=16; + bool changed; + int iterations=0; + do { + const btScalar w=2-btMin(1,iterations/slope); + changed=false; + iterations++; + int i; - for(i=0;im_nodes.size();++j) + btVector3 c(0,0,0); + for(int j=0;jm_nodes.size();++j) { - c+=m_clusters[i]->m_nodes[j]->m_x; + c+=m_clusters[i]->m_nodes[j]->m_x; } - if(m_clusters[i]->m_nodes.size()) + if(m_clusters[i]->m_nodes.size()) { - c /= (btScalar)m_clusters[i]->m_nodes.size(); - c = centers[i]+(c-centers[i])*w; - changed |= ((c-centers[i]).length2()>SIMD_EPSILON); - centers[i] = c; - m_clusters[i]->m_nodes.resize(0); + c /= (btScalar)m_clusters[i]->m_nodes.size(); + c = centers[i]+(c-centers[i])*w; + changed |= ((c-centers[i]).length2()>SIMD_EPSILON); + centers[i] = c; + m_clusters[i]->m_nodes.resize(0); } } - for(i=0;im_nodes.push_back(&m_nodes[i]); + m_clusters[kbest]->m_nodes.push_back(&m_nodes[i]); } } while(changed&&(iterations cids; - cids.resize(m_nodes.size(),-1); - for(i=0;i cids; + cids.resize(m_nodes.size(),-1); + for(i=0;im_nodes.size();++j) + for(int j=0;jm_nodes.size();++j) { - cids[int(m_clusters[i]->m_nodes[j]-&m_nodes[0])]=i; + cids[int(m_clusters[i]->m_nodes[j]-&m_nodes[0])]=i; } } - for(i=0;im_nodes.findLinearSearch(&m_nodes[kid])==m_clusters[cid]->m_nodes.size()) + if(m_clusters[cid]->m_nodes.findLinearSearch(&m_nodes[kid])==m_clusters[cid]->m_nodes.size()) { - m_clusters[cid]->m_nodes.push_back(&m_nodes[kid]); + m_clusters[cid]->m_nodes.push_back(&m_nodes[kid]); } } } } } - /* Master */ - if(m_clusters.size()>1) + /* Master */ + if(m_clusters.size()>1) { - Cluster* pmaster=new(btAlignedAlloc(sizeof(Cluster),16)) Cluster(); - pmaster->m_collide = false; - pmaster->m_nodes.reserve(m_nodes.size()); - for(int i=0;im_nodes.push_back(&m_nodes[i]); - m_clusters.push_back(pmaster); - btSwap(m_clusters[0],m_clusters[m_clusters.size()-1]); + Cluster* pmaster=new(btAlignedAlloc(sizeof(Cluster),16)) Cluster(); + pmaster->m_collide = false; + pmaster->m_nodes.reserve(m_nodes.size()); + for(int i=0;im_nodes.push_back(&m_nodes[i]); + m_clusters.push_back(pmaster); + btSwap(m_clusters[0],m_clusters[m_clusters.size()-1]); } - /* Terminate */ - for(i=0;im_nodes.size()==0) + if(m_clusters[i]->m_nodes.size()==0) { - releaseCluster(i--); + releaseCluster(i--); } } - - initializeClusters(); - updateClusters(); - return(m_clusters.size()); + + initializeClusters(); + updateClusters(); + return(m_clusters.size()); } -return(0); + return(0); } // void btSoftBody::refine(ImplicitFn* ifn,btScalar accurary,bool cut) { -const Node* nbase = &m_nodes[0]; -int ncount = m_nodes.size(); -btSymMatrix edges(ncount,-2); -int newnodes=0; -int i,j,k,ni; + const Node* nbase = &m_nodes[0]; + int ncount = m_nodes.size(); + btSymMatrix edges(ncount,-2); + int newnodes=0; + int i,j,k,ni; -/* Filter out */ -for(i=0;iEval(l.m_n[0]->m_x),ifn->Eval(l.m_n[1]->m_x))) + if(!SameSign(ifn->Eval(l.m_n[0]->m_x),ifn->Eval(l.m_n[1]->m_x))) { - btSwap(m_links[i],m_links[m_links.size()-1]); - m_links.pop_back();--i; + btSwap(m_links[i],m_links[m_links.size()-1]); + m_links.pop_back();--i; } } } -/* Fill edges */ -for(i=0;i0) + Node& a=m_nodes[i]; + Node& b=m_nodes[j]; + const btScalar t=ImplicitSolve(ifn,a.m_x,b.m_x,accurary); + if(t>0) { - const btVector3 x=Lerp(a.m_x,b.m_x,t); - const btVector3 v=Lerp(a.m_v,b.m_v,t); - btScalar m=0; - if(a.m_im>0) + const btVector3 x=Lerp(a.m_x,b.m_x,t); + const btVector3 v=Lerp(a.m_v,b.m_v,t); + btScalar m=0; + if(a.m_im>0) { - if(b.m_im>0) + if(b.m_im>0) { - const btScalar ma=1/a.m_im; - const btScalar mb=1/b.m_im; - const btScalar mc=Lerp(ma,mb,t); - const btScalar f=(ma+mb)/(ma+mb+mc); - a.m_im=1/(ma*f); - b.m_im=1/(mb*f); - m=mc*f; + const btScalar ma=1/a.m_im; + const btScalar mb=1/b.m_im; + const btScalar mc=Lerp(ma,mb,t); + const btScalar f=(ma+mb)/(ma+mb+mc); + a.m_im=1/(ma*f); + b.m_im=1/(mb*f); + m=mc*f; } else { a.m_im/=0.5;m=1/a.m_im; } } else { - if(b.m_im>0) + if(b.m_im>0) { b.m_im/=0.5;m=1/b.m_im; } else - m=0; + m=0; } - appendNode(x,m); - edges(i,j)=m_nodes.size()-1; - m_nodes[edges(i,j)].m_v=v; - ++newnodes; + appendNode(x,m); + edges(i,j)=m_nodes.size()-1; + m_nodes[edges(i,j)].m_v=v; + ++newnodes; } } } } -nbase=&m_nodes[0]; -/* Refine links */ -for(i=0,ni=m_links.size();i0) - { - appendLink(i); - Link* pft[]={ &m_links[i], - &m_links[m_links.size()-1]}; - pft[0]->m_n[0]=&m_nodes[idx[0]]; - pft[0]->m_n[1]=&m_nodes[ni]; - pft[1]->m_n[0]=&m_nodes[ni]; - pft[1]->m_n[1]=&m_nodes[idx[1]]; - } - } - } -/* Refine faces */ -for(i=0;i0) - { - appendFace(i); - const int l=(k+1)%3; - Face* pft[]={ &m_faces[i], - &m_faces[m_faces.size()-1]}; - pft[0]->m_n[0]=&m_nodes[idx[l]]; - pft[0]->m_n[1]=&m_nodes[idx[j]]; - pft[0]->m_n[2]=&m_nodes[ni]; - pft[1]->m_n[0]=&m_nodes[ni]; - pft[1]->m_n[1]=&m_nodes[idx[k]]; - pft[1]->m_n[2]=&m_nodes[idx[l]]; - appendLink(ni,idx[l],pft[0]->m_material); - --i;break; - } - } - } - } -/* Cut */ -if(cut) - { - btAlignedObjectArray cnodes; - const int pcount=ncount; - int i; - ncount=m_nodes.size(); - cnodes.resize(ncount,0); - /* Nodes */ - for(i=0;i=pcount)||(btFabs(ifn->Eval(x))0) { m*=0.5;m_nodes[i].m_im/=0.5; } - appendNode(x,m); - cnodes[i]=m_nodes.size()-1; - m_nodes[cnodes[i]].m_v=v; - } - } nbase=&m_nodes[0]; - /* Links */ + /* Refine links */ for(i=0,ni=m_links.size();i0) { - appendLink(i); - todetach=m_links.size()-1; + appendLink(i); + Link* pft[]={ &m_links[i], + &m_links[m_links.size()-1]}; + pft[0]->m_n[0]=&m_nodes[idx[0]]; + pft[0]->m_n[1]=&m_nodes[ni]; + pft[1]->m_n[0]=&m_nodes[ni]; + pft[1]->m_n[1]=&m_nodes[idx[1]]; } - else + } + } + /* Refine faces */ + for(i=0;iEval(m_nodes[id[0]].m_x)Eval(m_nodes[id[1]].m_x)0) { - int cn=cnodes[int(l.m_n[j]-nbase)]; - if(cn) l.m_n[j]=&m_nodes[cn]; + appendFace(i); + const int l=(k+1)%3; + Face* pft[]={ &m_faces[i], + &m_faces[m_faces.size()-1]}; + pft[0]->m_n[0]=&m_nodes[idx[l]]; + pft[0]->m_n[1]=&m_nodes[idx[j]]; + pft[0]->m_n[2]=&m_nodes[ni]; + pft[1]->m_n[0]=&m_nodes[ni]; + pft[1]->m_n[1]=&m_nodes[idx[k]]; + pft[1]->m_n[2]=&m_nodes[idx[l]]; + appendLink(ni,idx[l],pft[0]->m_material); + --i;break; + } + } + } + } + /* Cut */ + if(cut) + { + btAlignedObjectArray cnodes; + const int pcount=ncount; + int i; + ncount=m_nodes.size(); + cnodes.resize(ncount,0); + /* Nodes */ + for(i=0;i=pcount)||(btFabs(ifn->Eval(x))0) { m*=0.5;m_nodes[i].m_im/=0.5; } + appendNode(x,m); + cnodes[i]=m_nodes.size()-1; + m_nodes[cnodes[i]].m_v=v; + } + } + nbase=&m_nodes[0]; + /* Links */ + for(i=0,ni=m_links.size();iEval(m_nodes[id[0]].m_x)Eval(m_nodes[id[1]].m_x)Eval(n[0]->m_x)Eval(n[1]->m_x)Eval(n[2]->m_x)Eval(n[0]->m_x)Eval(n[1]->m_x)Eval(n[2]->m_x) ranks; - btAlignedObjectArray todelete; - ranks.resize(nnodes,0); - for(i=0,ni=m_links.size();i ranks; + btAlignedObjectArray todelete; + ranks.resize(nnodes,0); + for(i=0,ni=m_links.size();i=0;--i) +#if 0 + for(i=nnodes-1;i>=0;--i) { - if(!ranks[i]) todelete.push_back(i); + if(!ranks[i]) todelete.push_back(i); } - if(todelete.size()) + if(todelete.size()) { - btAlignedObjectArray& map=ranks; - for(int i=0;i& map=ranks; + for(int i=0;im_v=v; -pn[1]->m_v=v; -for(i=0,ni=m_links.size();im_v=v; + pn[1]->m_v=v; + for(i=0,ni=m_links.size();im_n[1]=pn[mtch]; - pft[1]->m_n[0]=pn[1-mtch]; - done=true; + appendLink(i); + Link* pft[]={&m_links[i],&m_links[m_links.size()-1]}; + pft[0]->m_n[1]=pn[mtch]; + pft[1]->m_n[0]=pn[1-mtch]; + done=true; } } -for(i=0,ni=m_faces.size();im_n[l]=pn[mtch]; - pft[1]->m_n[k]=pn[1-mtch]; - appendLink(pn[0],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true); - appendLink(pn[1],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true); + appendFace(i); + Face* pft[]={&m_faces[i],&m_faces[m_faces.size()-1]}; + pft[0]->m_n[l]=pn[mtch]; + pft[1]->m_n[k]=pn[1-mtch]; + appendLink(pn[0],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true); + appendLink(pn[1],pft[0]->m_n[(l+1)%3],pft[0]->m_material,true); } } } -if(!done) + if(!done) { - m_ndbvt.remove(pn[0]->m_leaf); - m_ndbvt.remove(pn[1]->m_leaf); - m_nodes.pop_back(); - m_nodes.pop_back(); + m_ndbvt.remove(pn[0]->m_leaf); + m_ndbvt.remove(pn[1]->m_leaf); + m_nodes.pop_back(); + m_nodes.pop_back(); } -return(done); + return(done); } // bool btSoftBody::rayTest(const btVector3& rayFrom, const btVector3& rayTo, sRayCast& results) - { +{ if(m_faces.size()&&m_fdbvt.empty()) initializeFaceTree(); @@ -1240,26 +1240,26 @@ bool btSoftBody::rayTest(const btVector3& rayFrom, // void btSoftBody::setSolver(eSolverPresets::_ preset) { -m_cfg.m_vsequence.clear(); -m_cfg.m_psequence.clear(); -m_cfg.m_dsequence.clear(); -switch(preset) + m_cfg.m_vsequence.clear(); + m_cfg.m_psequence.clear(); + m_cfg.m_dsequence.clear(); + switch(preset) { case eSolverPresets::Positions: - m_cfg.m_psequence.push_back(ePSolver::Anchors); - m_cfg.m_psequence.push_back(ePSolver::RContacts); - m_cfg.m_psequence.push_back(ePSolver::SContacts); - m_cfg.m_psequence.push_back(ePSolver::Linear); - break; + m_cfg.m_psequence.push_back(ePSolver::Anchors); + m_cfg.m_psequence.push_back(ePSolver::RContacts); + m_cfg.m_psequence.push_back(ePSolver::SContacts); + m_cfg.m_psequence.push_back(ePSolver::Linear); + break; case eSolverPresets::Velocities: - m_cfg.m_vsequence.push_back(eVSolver::Linear); - - m_cfg.m_psequence.push_back(ePSolver::Anchors); - m_cfg.m_psequence.push_back(ePSolver::RContacts); - m_cfg.m_psequence.push_back(ePSolver::SContacts); - - m_cfg.m_dsequence.push_back(ePSolver::Linear); - break; + m_cfg.m_vsequence.push_back(eVSolver::Linear); + + m_cfg.m_psequence.push_back(ePSolver::Anchors); + m_cfg.m_psequence.push_back(ePSolver::RContacts); + m_cfg.m_psequence.push_back(ePSolver::SContacts); + + m_cfg.m_dsequence.push_back(ePSolver::Linear); + break; } } @@ -1275,11 +1275,11 @@ void btSoftBody::predictMotion(btScalar dt) updateConstants(); m_fdbvt.clear(); if(m_cfg.collisions&fCollision::VF_SS) - { + { initializeFaceTree(); - } + } } - + /* Prepare */ m_sst.sdt = dt*m_cfg.timescale; m_sst.isdt = 1/m_sst.sdt; @@ -1307,41 +1307,41 @@ void btSoftBody::predictMotion(btScalar dt) { Node& n=m_nodes[i]; m_ndbvt.update( n.m_leaf, - btDbvtVolume::FromCR(n.m_x,m_sst.radmrg), - n.m_v*m_sst.velmrg, - m_sst.updmrg); + btDbvtVolume::FromCR(n.m_x,m_sst.radmrg), + n.m_v*m_sst.velmrg, + m_sst.updmrg); } /* Faces */ if(!m_fdbvt.empty()) - { + { for(int i=0;im_v+ - f.m_n[1]->m_v+ - f.m_n[2]->m_v)/3; + f.m_n[1]->m_v+ + f.m_n[2]->m_v)/3; m_fdbvt.update( f.m_leaf, - VolumeOf(f,m_sst.radmrg), - v*m_sst.velmrg, - m_sst.updmrg); - } + VolumeOf(f,m_sst.radmrg), + v*m_sst.velmrg, + m_sst.updmrg); } + } /* Pose */ updatePose(); /* Match */ if(m_pose.m_bframe&&(m_cfg.kMT>0)) - { + { const btMatrix3x3 posetrs=m_pose.m_rot; for(int i=0,ni=m_nodes.size();i0) - { + { const btVector3 x=posetrs*m_pose.m_pos[i]+m_pose.m_com; n.m_x=Lerp(n.m_x,x,m_cfg.kMT); - } } } + } /* Clear contacts */ m_rcontacts.resize(0); m_scontacts.resize(0); @@ -1354,104 +1354,104 @@ void btSoftBody::predictMotion(btScalar dt) // void btSoftBody::solveConstraints() { -/* Apply clusters */ -applyClusters(false); -/* Prepare links */ + /* Apply clusters */ + applyClusters(false); + /* Prepare links */ -int i,ni; + int i,ni; -for(i=0,ni=m_links.size();im_q-l.m_n[0]->m_q; - l.m_c2 = 1/(l.m_c3.length2()*l.m_c0); + Link& l=m_links[i]; + l.m_c3 = l.m_n[1]->m_q-l.m_n[0]->m_q; + l.m_c2 = 1/(l.m_c3.length2()*l.m_c0); } -/* Prepare anchors */ -for(i=0,ni=m_anchors.size();igetWorldTransform().getBasis()*a.m_local; - a.m_c0 = ImpulseMatrix( m_sst.sdt, - a.m_node->m_im, - a.m_body->getInvMass(), - a.m_body->getInvInertiaTensorWorld(), - ra); - a.m_c1 = ra; - a.m_c2 = m_sst.sdt*a.m_node->m_im; - a.m_body->activate(); + Anchor& a=m_anchors[i]; + const btVector3 ra=a.m_body->getWorldTransform().getBasis()*a.m_local; + a.m_c0 = ImpulseMatrix( m_sst.sdt, + a.m_node->m_im, + a.m_body->getInvMass(), + a.m_body->getInvInertiaTensorWorld(), + ra); + a.m_c1 = ra; + a.m_c2 = m_sst.sdt*a.m_node->m_im; + a.m_body->activate(); } -/* Solve velocities */ -if(m_cfg.viterations>0) + /* Solve velocities */ + if(m_cfg.viterations>0) { - /* Solve */ - for(int isolve=0;isolve0) + /* Solve positions */ + if(m_cfg.piterations>0) { - for(int isolve=0;isolve0) + /* Solve drift */ + if(m_cfg.diterations>0) { - const btScalar vcf=m_cfg.kVCF*m_sst.isdt; - for(i=0,ni=m_nodes.size();i& bodies) { -const int nb=bodies.size(); -int iterations=0; -int i; + const int nb=bodies.size(); + int iterations=0; + int i; -for(i=0;im_cfg.citerations); + iterations=btMax(iterations,bodies[i]->m_cfg.citerations); } -for(i=0;iprepareClusters(iterations); + bodies[i]->prepareClusters(iterations); } -for(i=0;isolveClusters(sor); + bodies[j]->solveClusters(sor); } } -for(i=0;icleanupClusters(); + bodies[i]->cleanupClusters(); } } @@ -1499,7 +1499,7 @@ void btSoftBody::integrateMotion() } // - btSoftBody::RayFromToCaster::RayFromToCaster(const btVector3& rayFrom,const btVector3& rayTo,btScalar mxt) +btSoftBody::RayFromToCaster::RayFromToCaster(const btVector3& rayFrom,const btVector3& rayTo,btScalar mxt) { m_rayFrom = rayFrom; m_rayNormalizedDirection = (rayTo-rayFrom); @@ -1527,12 +1527,12 @@ void btSoftBody::RayFromToCaster::Process(const btDbvtNode* leaf) // btScalar btSoftBody::RayFromToCaster::rayFromToTriangle( const btVector3& rayFrom, - const btVector3& rayTo, - const btVector3& rayNormalizedDirection, - const btVector3& a, - const btVector3& b, - const btVector3& c, - btScalar maxt) + const btVector3& rayTo, + const btVector3& rayNormalizedDirection, + const btVector3& a, + const btVector3& b, + const btVector3& c, + btScalar maxt) { static const btScalar ceps=-SIMD_EPSILON*10; static const btScalar teps=SIMD_EPSILON*10; @@ -1568,9 +1568,9 @@ void btSoftBody::pointersToIndices() for(i=0,ni=m_nodes.size();idata=*(void**)&i; - } + } } for(i=0,ni=m_links.size();idata=*(void**)&i; - } + } } for(i=0,ni=m_anchors.size();idata=&m_nodes[i]; - } + } } for(i=0,ni=m_links.size();idata=&m_faces[i]; - } + } } for(i=0,ni=m_anchors.size();im_x, - f.m_n[1]->m_x, - f.m_n[2]->m_x, - mint); + f.m_n[0]->m_x, + f.m_n[1]->m_x, + f.m_n[2]->m_x, + mint); if(t>0) - { + { ++cnt; if(!bcountonly) - { + { feature=btSoftBody::eFeature::Face; index=i; mint=t; - } } } } - else - {/* Use dbvt */ + } + else + {/* Use dbvt */ RayFromToCaster collider(rayFrom,rayTo,mint); btDbvt::rayTest(m_fdbvt.m_root,rayFrom,rayTo,collider); if(collider.m_face) - { + { mint=collider.m_mint; feature=btSoftBody::eFeature::Face; index=(int)(collider.m_face-&m_faces[0]); cnt=1; - } } + } return(cnt); } // void btSoftBody::initializeFaceTree() { -m_fdbvt.clear(); -for(int i=0;igetCollisionShape(); const btTransform& wtr=prb->getInterpolationWorldTransform(); btScalar dst=m_worldInfo->m_sparsesdf.Evaluate( wtr.invXform(x), - shp, - nrm, - margin); + shp, + nrm, + margin); if(dst<0) { cti.m_body = prb; cti.m_normal = wtr.getBasis()*nrm; cti.m_offset = -dot( cti.m_normal, - x-cti.m_normal*dst); + x-cti.m_normal*dst); return(true); } return(false); @@ -1755,7 +1755,7 @@ void btSoftBody::updateNormals() { btSoftBody::Face& f=m_faces[i]; const btVector3 n=cross(f.m_n[1]->m_x-f.m_n[0]->m_x, - f.m_n[2]->m_x-f.m_n[0]->m_x); + f.m_n[2]->m_x-f.m_n[0]->m_x); f.m_normal=n.normalized(); f.m_n[0]->m_n+=n; f.m_n[1]->m_n+=n; @@ -1773,28 +1773,28 @@ void btSoftBody::updateNormals() void btSoftBody::updateBounds() { if(m_ndbvt.m_root) - { + { const btVector3& mins=m_ndbvt.m_root->volume.Mins(); const btVector3& maxs=m_ndbvt.m_root->volume.Maxs(); const btScalar csm=getCollisionShape()->getMargin(); const btVector3 mrg=btVector3( csm, - csm, - csm)*1; // ??? to investigate... + csm, + csm)*1; // ??? to investigate... m_bounds[0]=mins-mrg; m_bounds[1]=maxs+mrg; - if(0!=getBroadphaseHandle()) - { - m_worldInfo->m_broadphase->setAabb( getBroadphaseHandle(), - m_bounds[0], - m_bounds[1], - m_worldInfo->m_dispatcher); - } + if(0!=getBroadphaseHandle()) + { + m_worldInfo->m_broadphase->setAabb( getBroadphaseHandle(), + m_bounds[0], + m_bounds[1], + m_worldInfo->m_dispatcher); } - else - { + } + else + { m_bounds[0]= - m_bounds[1]=btVector3(0,0,0); - } + m_bounds[1]=btVector3(0,0,0); + } } @@ -1825,12 +1825,12 @@ void btSoftBody::updatePose() pose.m_rot=r; pose.m_scl=pose.m_aqq*r.transpose()*Apq; if(m_cfg.maxvolume>1) - { + { const btScalar idet=Clamp( 1/pose.m_scl.determinant(), - 1,m_cfg.maxvolume); + 1,m_cfg.maxvolume); pose.m_scl=Mul(pose.m_scl,idet); - } - + } + } } @@ -1885,53 +1885,53 @@ void btSoftBody::initializeClusters() { int i; -for( i=0;im_im>0?1/c.m_nodes[j]->m_im:0; - c.m_imass += c.m_masses[j]; + c.m_masses[j] = c.m_nodes[j]->m_im>0?1/c.m_nodes[j]->m_im:0; + c.m_imass += c.m_masses[j]; } - c.m_imass = 1/c.m_imass; - c.m_com = btSoftBody::clusterCom(&c); - c.m_lv = btVector3(0,0,0); - c.m_av = btVector3(0,0,0); - c.m_leaf = 0; - /* Inertia */ - btMatrix3x3& ii=c.m_locii; - ii[0]=ii[1]=ii[2]=btVector3(0,0,0); - { - int i,ni; + c.m_imass = 1/c.m_imass; + c.m_com = btSoftBody::clusterCom(&c); + c.m_lv = btVector3(0,0,0); + c.m_av = btVector3(0,0,0); + c.m_leaf = 0; + /* Inertia */ + btMatrix3x3& ii=c.m_locii; + ii[0]=ii[1]=ii[2]=btVector3(0,0,0); + { + int i,ni; - for(i=0,ni=c.m_nodes.size();im_x-c.m_com; - const btVector3 q=k*k; - const btScalar m=c.m_masses[i]; - ii[0][0] += m*(q[1]+q[2]); - ii[1][1] += m*(q[0]+q[2]); - ii[2][2] += m*(q[0]+q[1]); - ii[0][1] -= m*k[0]*k[1]; - ii[0][2] -= m*k[0]*k[2]; - ii[1][2] -= m*k[1]*k[2]; - } - } - ii[1][0]=ii[0][1]; - ii[2][0]=ii[0][2]; - ii[2][1]=ii[1][2]; - ii=ii.inverse(); - /* Frame */ - c.m_framexform.setIdentity(); - c.m_framexform.setOrigin(c.m_com); - c.m_framerefs.resize(c.m_nodes.size()); - { - int i; - for(i=0;im_x-c.m_com; + const btVector3 k=c.m_nodes[i]->m_x-c.m_com; + const btVector3 q=k*k; + const btScalar m=c.m_masses[i]; + ii[0][0] += m*(q[1]+q[2]); + ii[1][1] += m*(q[0]+q[2]); + ii[2][2] += m*(q[0]+q[1]); + ii[0][1] -= m*k[0]*k[1]; + ii[0][2] -= m*k[0]*k[2]; + ii[1][2] -= m*k[1]*k[2]; + } + } + ii[1][0]=ii[0][1]; + ii[2][0]=ii[0][2]; + ii[2][1]=ii[1][2]; + ii=ii.inverse(); + /* Frame */ + c.m_framexform.setIdentity(); + c.m_framexform.setOrigin(c.m_com); + c.m_framerefs.resize(c.m_nodes.size()); + { + int i; + for(i=0;im_x-c.m_com; } } } @@ -1940,49 +1940,49 @@ for( i=0;im_x-c.m_com; - const btVector3& b=c.m_framerefs[i]; - m[0]+=a[0]*b;m[1]+=a[1]*b;m[2]+=a[2]*b; + const btVector3 a=c.m_nodes[i]->m_x-c.m_com; + const btVector3& b=c.m_framerefs[i]; + m[0]+=a[0]*b;m[1]+=a[1]*b;m[2]+=a[2]*b; } - PolarDecompose(m,r,s); - c.m_framexform.setOrigin(c.m_com); - c.m_framexform.setBasis(r); - /* Inertia */ - #if 1/* Constant */ - c.m_invwi=c.m_framexform.getBasis()*c.m_locii*c.m_framexform.getBasis().transpose(); - #else - #if 0/* Sphere */ + PolarDecompose(m,r,s); + c.m_framexform.setOrigin(c.m_com); + c.m_framexform.setBasis(r); + /* Inertia */ +#if 1/* Constant */ + c.m_invwi=c.m_framexform.getBasis()*c.m_locii*c.m_framexform.getBasis().transpose(); +#else +#if 0/* Sphere */ const btScalar rk=(2*c.m_extents.length2())/(5*c.m_imass); const btVector3 inertia(rk,rk,rk); const btVector3 iin(btFabs(inertia[0])>SIMD_EPSILON?1/inertia[0]:0, - btFabs(inertia[1])>SIMD_EPSILON?1/inertia[1]:0, - btFabs(inertia[2])>SIMD_EPSILON?1/inertia[2]:0); - + btFabs(inertia[1])>SIMD_EPSILON?1/inertia[1]:0, + btFabs(inertia[2])>SIMD_EPSILON?1/inertia[2]:0); + c.m_invwi=c.m_xform.getBasis().scaled(iin)*c.m_xform.getBasis().transpose(); - #else/* Actual */ +#else/* Actual */ c.m_invwi[0]=c.m_invwi[1]=c.m_invwi[2]=btVector3(0,0,0); for(int i=0;im_x-c.m_com; const btVector3 q=k*k; const btScalar m=1/c.m_nodes[i]->m_im; @@ -1992,59 +1992,59 @@ for(i=0;im_v*c.m_masses[i]; - c.m_lv += v; - c.m_av += cross(c.m_nodes[i]->m_x-c.m_com,v); - } - } - c.m_lv=c.m_imass*c.m_lv*(1-c.m_ldamping); - c.m_av=c.m_invwi*c.m_av*(1-c.m_adamping); - c.m_vimpulses[0] = - c.m_vimpulses[1] = btVector3(0,0,0); - c.m_dimpulses[0] = - c.m_dimpulses[1] = btVector3(0,0,0); - c.m_nvimpulses = 0; - c.m_ndimpulses = 0; - /* Matching */ - if(c.m_matching>0) - { - for(int j=0;jm_v*c.m_masses[i]; + c.m_lv += v; + c.m_av += cross(c.m_nodes[i]->m_x-c.m_com,v); } } - /* Dbvt */ - if(c.m_collide) + c.m_lv=c.m_imass*c.m_lv*(1-c.m_ldamping); + c.m_av=c.m_invwi*c.m_av*(1-c.m_adamping); + c.m_vimpulses[0] = + c.m_vimpulses[1] = btVector3(0,0,0); + c.m_dimpulses[0] = + c.m_dimpulses[1] = btVector3(0,0,0); + c.m_nvimpulses = 0; + c.m_ndimpulses = 0; + /* Matching */ + if(c.m_matching>0) { - btVector3 mi=c.m_nodes[0]->m_x; - btVector3 mx=mi; - for(int j=1;jm_x); - mx.setMax(c.m_nodes[j]->m_x); + Node& n=*c.m_nodes[j]; + const btVector3 x=c.m_framexform*c.m_framerefs[j]; + n.m_x=Lerp(n.m_x,x,c.m_matching); + } + } + /* Dbvt */ + if(c.m_collide) + { + btVector3 mi=c.m_nodes[0]->m_x; + btVector3 mx=mi; + for(int j=1;jm_x); + mx.setMax(c.m_nodes[j]->m_x); } - const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(mi,mx); - if(c.m_leaf) - m_cdbvt.update(c.m_leaf,bounds,c.m_lv*m_sst.sdt*3,m_sst.radmrg); + const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(mi,mx); + if(c.m_leaf) + m_cdbvt.update(c.m_leaf,bounds,c.m_lv*m_sst.sdt*3,m_sst.radmrg); else - c.m_leaf=m_cdbvt.insert(bounds,&c); + c.m_leaf=m_cdbvt.insert(bounds,&c); } } } @@ -2053,13 +2053,13 @@ for(i=0;iTerminate(m_sst.sdt); - if(m_joints[i]->m_delete) + m_joints[i]->Terminate(m_sst.sdt); + if(m_joints[i]->m_delete) { - btAlignedFree(m_joints[i]); - m_joints.remove(m_joints[i--]); + btAlignedFree(m_joints[i]); + m_joints.remove(m_joints[i--]); } } } @@ -2067,9 +2067,9 @@ for(int i=0;iPrepare(m_sst.sdt,iterations); + m_joints[i]->Prepare(m_sst.sdt,iterations); } } @@ -2077,50 +2077,50 @@ for(int i=0;iSolve(m_sst.sdt,sor); + m_joints[i]->Solve(m_sst.sdt,sor); } } // void btSoftBody::applyClusters(bool drift) { -BT_PROFILE("ApplyClusters"); -const btScalar f0=m_sst.sdt; -const btScalar f1=f0/2; -btAlignedObjectArray deltas; -btAlignedObjectArray weights; -deltas.resize(m_nodes.size(),btVector3(0,0,0)); -weights.resize(m_nodes.size(),0); -int i; + BT_PROFILE("ApplyClusters"); + const btScalar f0=m_sst.sdt; + const btScalar f1=f0/2; + btAlignedObjectArray deltas; + btAlignedObjectArray weights; + deltas.resize(m_nodes.size(),btVector3(0,0,0)); + weights.resize(m_nodes.size(),0); + int i; -if(drift) + if(drift) { - for(i=0;im_x; - const btScalar q=c.m_masses[j]; - deltas[idx] += (v+cross(w,x-c.m_com))*q; - weights[idx] += q; + const int idx=int(c.m_nodes[j]-&m_nodes[0]); + const btVector3& x=c.m_nodes[j]->m_x; + const btScalar q=c.m_masses[j]; + deltas[idx] += (v+cross(w,x-c.m_com))*q; + weights[idx] += q; } } } @@ -2135,18 +2135,18 @@ void btSoftBody::dampClusters() { int i; -for(i=0;i0) + Cluster& c=*m_clusters[i]; + if(c.m_ndamping>0) { - for(int j=0;j0) + Node& n=*c.m_nodes[j]; + if(n.m_im>0) { - const btVector3 vx=c.m_lv+cross(c.m_av,c.m_nodes[j]->m_q-c.m_com); - n.m_v += c.m_ndamping*(vx-n.m_v); + const btVector3 vx=c.m_lv+cross(c.m_av,c.m_nodes[j]->m_q-c.m_com); + n.m_v += c.m_ndamping*(vx-n.m_v); } } } @@ -2156,148 +2156,148 @@ for(i=0;i0) + static const btScalar maxdrift=4; + Joint::Prepare(dt,iterations); + m_rpos[0] = m_bodies[0].xform()*m_refs[0]; + m_rpos[1] = m_bodies[1].xform()*m_refs[1]; + m_drift = Clamp(m_rpos[0]-m_rpos[1],maxdrift)*m_erp/dt; + m_rpos[0] -= m_bodies[0].xform().getOrigin(); + m_rpos[1] -= m_bodies[1].xform().getOrigin(); + m_massmatrix = ImpulseMatrix( m_bodies[0].invMass(),m_bodies[0].invWorldInertia(),m_rpos[0], + m_bodies[1].invMass(),m_bodies[1].invWorldInertia(),m_rpos[1]); + if(m_split>0) { - m_sdrift = m_massmatrix*(m_drift*m_split); - m_drift *= 1-m_split; + m_sdrift = m_massmatrix*(m_drift*m_split); + m_drift *= 1-m_split; } -m_drift /=(btScalar)iterations; + m_drift /=(btScalar)iterations; } // void btSoftBody::LJoint::Solve(btScalar dt,btScalar sor) { -const btVector3 va=m_bodies[0].velocity(m_rpos[0]); -const btVector3 vb=m_bodies[1].velocity(m_rpos[1]); -const btVector3 vr=va-vb; -btSoftBody::Impulse impulse; -impulse.m_asVelocity = 1; -impulse.m_velocity = m_massmatrix*(m_drift+vr*m_cfm)*sor; -m_bodies[0].applyImpulse(-impulse,m_rpos[0]); -m_bodies[1].applyImpulse( impulse,m_rpos[1]); + const btVector3 va=m_bodies[0].velocity(m_rpos[0]); + const btVector3 vb=m_bodies[1].velocity(m_rpos[1]); + const btVector3 vr=va-vb; + btSoftBody::Impulse impulse; + impulse.m_asVelocity = 1; + impulse.m_velocity = m_massmatrix*(m_drift+vr*m_cfm)*sor; + m_bodies[0].applyImpulse(-impulse,m_rpos[0]); + m_bodies[1].applyImpulse( impulse,m_rpos[1]); } // void btSoftBody::LJoint::Terminate(btScalar dt) { -if(m_split>0) + if(m_split>0) { - m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]); - m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]); + m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]); + m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]); } } // void btSoftBody::AJoint::Prepare(btScalar dt,int iterations) { -static const btScalar maxdrift=SIMD_PI/16; -m_icontrol->Prepare(this); -Joint::Prepare(dt,iterations); -m_axis[0] = m_bodies[0].xform().getBasis()*m_refs[0]; -m_axis[1] = m_bodies[1].xform().getBasis()*m_refs[1]; -m_drift = NormalizeAny(cross(m_axis[1],m_axis[0])); -m_drift *= btMin(maxdrift,btAcos(Clamp(dot(m_axis[0],m_axis[1]),-1,+1))); -m_drift *= m_erp/dt; -m_massmatrix= AngularImpulseMatrix(m_bodies[0].invWorldInertia(),m_bodies[1].invWorldInertia()); -if(m_split>0) + static const btScalar maxdrift=SIMD_PI/16; + m_icontrol->Prepare(this); + Joint::Prepare(dt,iterations); + m_axis[0] = m_bodies[0].xform().getBasis()*m_refs[0]; + m_axis[1] = m_bodies[1].xform().getBasis()*m_refs[1]; + m_drift = NormalizeAny(cross(m_axis[1],m_axis[0])); + m_drift *= btMin(maxdrift,btAcos(Clamp(dot(m_axis[0],m_axis[1]),-1,+1))); + m_drift *= m_erp/dt; + m_massmatrix= AngularImpulseMatrix(m_bodies[0].invWorldInertia(),m_bodies[1].invWorldInertia()); + if(m_split>0) { - m_sdrift = m_massmatrix*(m_drift*m_split); - m_drift *= 1-m_split; + m_sdrift = m_massmatrix*(m_drift*m_split); + m_drift *= 1-m_split; } -m_drift /=(btScalar)iterations; + m_drift /=(btScalar)iterations; } // void btSoftBody::AJoint::Solve(btScalar dt,btScalar sor) { -const btVector3 va=m_bodies[0].angularVelocity(); -const btVector3 vb=m_bodies[1].angularVelocity(); -const btVector3 vr=va-vb; -const btScalar sp=dot(vr,m_axis[0]); -const btVector3 vc=vr-m_axis[0]*m_icontrol->Speed(this,sp); -btSoftBody::Impulse impulse; -impulse.m_asVelocity = 1; -impulse.m_velocity = m_massmatrix*(m_drift+vc*m_cfm)*sor; -m_bodies[0].applyAImpulse(-impulse); -m_bodies[1].applyAImpulse( impulse); + const btVector3 va=m_bodies[0].angularVelocity(); + const btVector3 vb=m_bodies[1].angularVelocity(); + const btVector3 vr=va-vb; + const btScalar sp=dot(vr,m_axis[0]); + const btVector3 vc=vr-m_axis[0]*m_icontrol->Speed(this,sp); + btSoftBody::Impulse impulse; + impulse.m_asVelocity = 1; + impulse.m_velocity = m_massmatrix*(m_drift+vc*m_cfm)*sor; + m_bodies[0].applyAImpulse(-impulse); + m_bodies[1].applyAImpulse( impulse); } // void btSoftBody::AJoint::Terminate(btScalar dt) { -if(m_split>0) + if(m_split>0) { - m_bodies[0].applyDAImpulse(-m_sdrift); - m_bodies[1].applyDAImpulse( m_sdrift); + m_bodies[0].applyDAImpulse(-m_sdrift); + m_bodies[1].applyDAImpulse( m_sdrift); } } // void btSoftBody::CJoint::Prepare(btScalar dt,int iterations) { -Joint::Prepare(dt,iterations); -const bool dodrift=(m_life==0); -m_delete=(++m_life)>m_maxlife; -if(dodrift) + Joint::Prepare(dt,iterations); + const bool dodrift=(m_life==0); + m_delete=(++m_life)>m_maxlife; + if(dodrift) { - m_drift=m_drift*m_erp/dt; - if(m_split>0) + m_drift=m_drift*m_erp/dt; + if(m_split>0) { - m_sdrift = m_massmatrix*(m_drift*m_split); - m_drift *= 1-m_split; + m_sdrift = m_massmatrix*(m_drift*m_split); + m_drift *= 1-m_split; } - m_drift/=(btScalar)iterations; + m_drift/=(btScalar)iterations; } else { - m_drift=m_sdrift=btVector3(0,0,0); + m_drift=m_sdrift=btVector3(0,0,0); } } // void btSoftBody::CJoint::Solve(btScalar dt,btScalar sor) { -const btVector3 va=m_bodies[0].velocity(m_rpos[0]); -const btVector3 vb=m_bodies[1].velocity(m_rpos[1]); -const btVector3 vrel=va-vb; -const btScalar rvac=dot(vrel,m_normal); -btSoftBody::Impulse impulse; -impulse.m_asVelocity = 1; -impulse.m_velocity = m_drift; -if(rvac<0) + const btVector3 va=m_bodies[0].velocity(m_rpos[0]); + const btVector3 vb=m_bodies[1].velocity(m_rpos[1]); + const btVector3 vrel=va-vb; + const btScalar rvac=dot(vrel,m_normal); + btSoftBody::Impulse impulse; + impulse.m_asVelocity = 1; + impulse.m_velocity = m_drift; + if(rvac<0) { - const btVector3 iv=m_normal*rvac; - const btVector3 fv=vrel-iv; - impulse.m_velocity += iv+fv*m_friction; + const btVector3 iv=m_normal*rvac; + const btVector3 fv=vrel-iv; + impulse.m_velocity += iv+fv*m_friction; } -impulse.m_velocity=m_massmatrix*impulse.m_velocity*sor; -m_bodies[0].applyImpulse(-impulse,m_rpos[0]); -m_bodies[1].applyImpulse( impulse,m_rpos[1]); + impulse.m_velocity=m_massmatrix*impulse.m_velocity*sor; + m_bodies[0].applyImpulse(-impulse,m_rpos[0]); + m_bodies[1].applyImpulse( impulse,m_rpos[1]); } // void btSoftBody::CJoint::Terminate(btScalar dt) { -if(m_split>0) + if(m_split>0) { - m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]); - m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]); + m_bodies[0].applyDImpulse(-m_sdrift,m_rpos[0]); + m_bodies[1].applyDImpulse( m_sdrift,m_rpos[1]); } } @@ -2315,14 +2315,14 @@ void btSoftBody::applyForces() const bool as_pressure=kPR!=0; const bool as_volume=kVC>0; const bool as_aero= as_lift || - as_drag ; + as_drag ; const bool as_vaero= as_aero && - (m_cfg.aeromodel=btSoftBody::eAeroModel::F_TwoSided); + (m_cfg.aeromodel>=btSoftBody::eAeroModel::F_TwoSided); const bool use_medium= as_aero; const bool use_volume= as_pressure || - as_volume ; + as_volume ; btScalar volume=0; btScalar ivolumetp=0; btScalar dvolumetv=0; @@ -2470,32 +2470,32 @@ void btSoftBody::PSolve_RContacts(btSoftBody* psb,btScalar kst,btScalar ti) // void btSoftBody::PSolve_SContacts(btSoftBody* psb,btScalar,btScalar ti) { -for(int i=0,ni=psb->m_scontacts.size();im_scontacts.size();im_scontacts[i]; - const btVector3& nr=c.m_normal; - Node& n=*c.m_node; - Face& f=*c.m_face; - const btVector3 p=BaryEval( f.m_n[0]->m_x, - f.m_n[1]->m_x, - f.m_n[2]->m_x, - c.m_weights); - const btVector3 q=BaryEval( f.m_n[0]->m_q, - f.m_n[1]->m_q, - f.m_n[2]->m_q, - c.m_weights); - const btVector3 vr=(n.m_x-n.m_q)-(p-q); - btVector3 corr(0,0,0); - if(dot(vr,nr)<0) + const SContact& c=psb->m_scontacts[i]; + const btVector3& nr=c.m_normal; + Node& n=*c.m_node; + Face& f=*c.m_face; + const btVector3 p=BaryEval( f.m_n[0]->m_x, + f.m_n[1]->m_x, + f.m_n[2]->m_x, + c.m_weights); + const btVector3 q=BaryEval( f.m_n[0]->m_q, + f.m_n[1]->m_q, + f.m_n[2]->m_q, + c.m_weights); + const btVector3 vr=(n.m_x-n.m_q)-(p-q); + btVector3 corr(0,0,0); + if(dot(vr,nr)<0) { - const btScalar j=c.m_margin-(dot(nr,n.m_x)-dot(nr,p)); - corr+=c.m_normal*j; + const btScalar j=c.m_margin-(dot(nr,n.m_x)-dot(nr,p)); + corr+=c.m_normal*j; } - corr -= ProjectOnPlane(vr,nr)*c.m_friction; - n.m_x += corr*c.m_cfm[0]; - f.m_n[0]->m_x -= corr*(c.m_cfm[1]*c.m_weights.x()); - f.m_n[1]->m_x -= corr*(c.m_cfm[1]*c.m_weights.y()); - f.m_n[2]->m_x -= corr*(c.m_cfm[1]*c.m_weights.z()); + corr -= ProjectOnPlane(vr,nr)*c.m_friction; + n.m_x += corr*c.m_cfm[0]; + f.m_n[0]->m_x -= corr*(c.m_cfm[1]*c.m_weights.x()); + f.m_n[1]->m_x -= corr*(c.m_cfm[1]*c.m_weights.y()); + f.m_n[2]->m_x -= corr*(c.m_cfm[1]*c.m_weights.z()); } } @@ -2522,107 +2522,107 @@ void btSoftBody::PSolve_Links(btSoftBody* psb,btScalar kst,btScalar ti) // void btSoftBody::VSolve_Links(btSoftBody* psb,btScalar kst) { -for(int i=0,ni=psb->m_links.size();im_links.size();im_links[i]; - Node** n=l.m_n; - const btScalar j=-dot(l.m_c3,n[0]->m_v-n[1]->m_v)*l.m_c2*kst; - n[0]->m_v+= l.m_c3*(j*n[0]->m_im); - n[1]->m_v-= l.m_c3*(j*n[1]->m_im); + Link& l=psb->m_links[i]; + Node** n=l.m_n; + const btScalar j=-dot(l.m_c3,n[0]->m_v-n[1]->m_v)*l.m_c2*kst; + n[0]->m_v+= l.m_c3*(j*n[0]->m_im); + n[1]->m_v-= l.m_c3*(j*n[1]->m_im); } } // btSoftBody::psolver_t btSoftBody::getSolver(ePSolver::_ solver) { -switch(solver) + switch(solver) { case ePSolver::Anchors: return(&btSoftBody::PSolve_Anchors); case ePSolver::Linear: return(&btSoftBody::PSolve_Links); case ePSolver::RContacts: return(&btSoftBody::PSolve_RContacts); case ePSolver::SContacts: return(&btSoftBody::PSolve_SContacts); } -return(0); + return(0); } // btSoftBody::vsolver_t btSoftBody::getSolver(eVSolver::_ solver) { -switch(solver) + switch(solver) { case eVSolver::Linear: return(&btSoftBody::VSolve_Links); } -return(0); + return(0); } // void btSoftBody::defaultCollisionHandler(btCollisionObject* pco) { -switch(m_cfg.collisions&fCollision::RVSmask) + switch(m_cfg.collisions&fCollision::RVSmask) { case fCollision::SDF_RS: { - btSoftColliders::CollideSDF_RS docollide; - btRigidBody* prb=btRigidBody::upcast(pco); - const btTransform wtr=prb->getInterpolationWorldTransform(); - const btTransform ctr=prb->getWorldTransform(); - const btScalar timemargin=(wtr.getOrigin()-ctr.getOrigin()).length(); - const btScalar basemargin=getCollisionShape()->getMargin(); - btVector3 mins; - btVector3 maxs; - ATTRIBUTE_ALIGNED16(btDbvtVolume) volume; - pco->getCollisionShape()->getAabb( pco->getInterpolationWorldTransform(), - mins, - maxs); - volume=btDbvtVolume::FromMM(mins,maxs); - volume.Expand(btVector3(basemargin,basemargin,basemargin)); - docollide.psb = this; - docollide.prb = prb; - docollide.dynmargin = basemargin+timemargin; - docollide.stamargin = basemargin; - btDbvt::collideTV(m_ndbvt.m_root,volume,docollide); + btSoftColliders::CollideSDF_RS docollide; + btRigidBody* prb=btRigidBody::upcast(pco); + const btTransform wtr=prb->getInterpolationWorldTransform(); + const btTransform ctr=prb->getWorldTransform(); + const btScalar timemargin=(wtr.getOrigin()-ctr.getOrigin()).length(); + const btScalar basemargin=getCollisionShape()->getMargin(); + btVector3 mins; + btVector3 maxs; + ATTRIBUTE_ALIGNED16(btDbvtVolume) volume; + pco->getCollisionShape()->getAabb( pco->getInterpolationWorldTransform(), + mins, + maxs); + volume=btDbvtVolume::FromMM(mins,maxs); + volume.Expand(btVector3(basemargin,basemargin,basemargin)); + docollide.psb = this; + docollide.prb = prb; + docollide.dynmargin = basemargin+timemargin; + docollide.stamargin = basemargin; + btDbvt::collideTV(m_ndbvt.m_root,volume,docollide); } - break; + break; case fCollision::CL_RS: { - btSoftColliders::CollideCL_RS collider; - collider.Process(this,btRigidBody::upcast(pco)); + btSoftColliders::CollideCL_RS collider; + collider.Process(this,btRigidBody::upcast(pco)); } - break; + break; } } // void btSoftBody::defaultCollisionHandler(btSoftBody* psb) { -const int cf=m_cfg.collisions&psb->m_cfg.collisions; -switch(cf&fCollision::SVSmask) + const int cf=m_cfg.collisions&psb->m_cfg.collisions; + switch(cf&fCollision::SVSmask) { case fCollision::CL_SS: { - btSoftColliders::CollideCL_SS docollide; - docollide.Process(this,psb); + btSoftColliders::CollideCL_SS docollide; + docollide.Process(this,psb); } - break; + break; case fCollision::VF_SS: { - btSoftColliders::CollideVF_SS docollide; - /* common */ - docollide.mrg= getCollisionShape()->getMargin()+ - psb->getCollisionShape()->getMargin(); - /* psb0 nodes vs psb1 faces */ - docollide.psb[0]=this; - docollide.psb[1]=psb; - btDbvt::collideTT( docollide.psb[0]->m_ndbvt.m_root, - docollide.psb[1]->m_fdbvt.m_root, - docollide); - /* psb1 nodes vs psb0 faces */ - docollide.psb[0]=psb; - docollide.psb[1]=this; - btDbvt::collideTT( docollide.psb[0]->m_ndbvt.m_root, - docollide.psb[1]->m_fdbvt.m_root, - docollide); + btSoftColliders::CollideVF_SS docollide; + /* common */ + docollide.mrg= getCollisionShape()->getMargin()+ + psb->getCollisionShape()->getMargin(); + /* psb0 nodes vs psb1 faces */ + docollide.psb[0]=this; + docollide.psb[1]=psb; + btDbvt::collideTT( docollide.psb[0]->m_ndbvt.m_root, + docollide.psb[1]->m_fdbvt.m_root, + docollide); + /* psb1 nodes vs psb0 faces */ + docollide.psb[0]=psb; + docollide.psb[1]=this; + btDbvt::collideTT( docollide.psb[0]->m_ndbvt.m_root, + docollide.psb[1]->m_fdbvt.m_root, + docollide); } - break; + break; } } diff --git a/src/BulletSoftBody/btSoftBody.h b/src/BulletSoftBody/btSoftBody.h index fbb4ddca0..64c06bd4c 100644 --- a/src/BulletSoftBody/btSoftBody.h +++ b/src/BulletSoftBody/btSoftBody.h @@ -62,13 +62,13 @@ public: F_OneSided, ///Face normals are taken as it is END };}; - + ///eVSolver : velocities solvers struct eVSolver { enum _ { Linear, ///Linear solver END };}; - + ///ePSolver : positions solvers struct ePSolver { enum _ { Linear, ///Linear solver @@ -77,7 +77,7 @@ public: SContacts, ///Soft contacts solver END };}; - + ///eSolverPresets struct eSolverPresets { enum _ { Positions, @@ -85,7 +85,7 @@ public: Default = Positions, END };}; - + ///eFeature struct eFeature { enum _ { None, @@ -94,20 +94,20 @@ public: Face, END };}; - + typedef btAlignedObjectArray tVSolverArray; typedef btAlignedObjectArray tPSolverArray; - + // // Flags // - + ///fCollision struct fCollision { enum _ { RVSmask = 0x000f, ///Rigid versus soft mask SDF_RS = 0x0001, ///SDF based rigid vs soft CL_RS = 0x0002, ///Cluster vs convex rigid vs soft - + SVSmask = 0x00f0, ///Rigid versus soft mask VF_SS = 0x0010, ///Vertex vs face soft vs soft handling CL_SS = 0x0020, ///Cluster vs cluster soft vs soft handling @@ -115,7 +115,7 @@ public: Default = SDF_RS, END };}; - + ///fMaterial struct fMaterial { enum _ { DebugDraw = 0x0001, /// Enable debug draw @@ -123,11 +123,11 @@ public: Default = DebugDraw, END };}; - + // // API Types // - + /* sRayCast */ struct sRayCast { @@ -136,13 +136,13 @@ public: int index; /// feature index btScalar fraction; /// time of impact fraction (rayorg+(rayto-rayfrom)*fraction) }; - + /* ImplicitFn */ struct ImplicitFn { virtual btScalar Eval(const btVector3& x)=0; }; - + // // Internal types // @@ -180,7 +180,7 @@ public: btScalar m_kVST; // Volume stiffness coefficient [0,1] int m_flags; // Flags }; - + /* Feature */ struct Feature : Element { @@ -296,7 +296,7 @@ public: btScalar m_adamping; btScalar m_matching; bool m_collide; - Cluster() : m_leaf(0),m_ndamping(0),m_ldamping(0),m_adamping(0),m_matching(0) {} + Cluster() : m_leaf(0),m_ndamping(0),m_ldamping(0),m_adamping(0),m_matching(0) {} }; /* Impulse */ struct Impulse @@ -307,109 +307,109 @@ public: int m_asDrift:1; Impulse() : m_velocity(0,0,0),m_drift(0,0,0),m_asVelocity(0),m_asDrift(0) {} Impulse operator -() const - { + { Impulse i=*this; i.m_velocity=-i.m_velocity; i.m_drift=-i.m_drift; return(i); - } + } Impulse operator*(btScalar x) const - { + { Impulse i=*this; i.m_velocity*=x; i.m_drift*=x; return(i); - } + } }; /* Body */ struct Body { Cluster* m_soft; btRigidBody* m_rigid; - Body() : m_soft(0),m_rigid(0) {} - Body(Cluster* p) : m_soft(p),m_rigid(0) {} - Body(btRigidBody* p) : m_soft(0),m_rigid(p) {} + Body() : m_soft(0),m_rigid(0) {} + Body(Cluster* p) : m_soft(p),m_rigid(0) {} + Body(btRigidBody* p) : m_soft(0),m_rigid(p) {} void activate() const - { + { if(m_rigid) m_rigid->activate(); - } + } const btMatrix3x3& invWorldInertia() const - { + { static const btMatrix3x3 iwi(0,0,0,0,0,0,0,0,0); if(m_rigid) return(m_rigid->getInvInertiaTensorWorld()); if(m_soft) return(m_soft->m_invwi); return(iwi); - } + } btScalar invMass() const - { + { if(m_rigid) return(m_rigid->getInvMass()); if(m_soft) return(m_soft->m_imass); return(0); - } + } const btTransform& xform() const - { + { static const btTransform identity=btTransform::getIdentity(); if(m_rigid) return(m_rigid->getInterpolationWorldTransform()); if(m_soft) return(m_soft->m_framexform); return(identity); - } + } btVector3 linearVelocity() const - { + { if(m_rigid) return(m_rigid->getLinearVelocity()); if(m_soft) return(m_soft->m_lv); return(btVector3(0,0,0)); - } + } btVector3 angularVelocity(const btVector3& rpos) const - { + { if(m_rigid) return(cross(m_rigid->getAngularVelocity(),rpos)); if(m_soft) return(cross(m_soft->m_av,rpos)); return(btVector3(0,0,0)); - } + } btVector3 angularVelocity() const - { + { if(m_rigid) return(m_rigid->getAngularVelocity()); if(m_soft) return(m_soft->m_av); return(btVector3(0,0,0)); - } + } btVector3 velocity(const btVector3& rpos) const - { + { return(linearVelocity()+angularVelocity(rpos)); - } + } void applyVImpulse(const btVector3& impulse,const btVector3& rpos) const - { + { if(m_rigid) m_rigid->applyImpulse(impulse,rpos); if(m_soft) btSoftBody::clusterVImpulse(m_soft,rpos,impulse); - } + } void applyDImpulse(const btVector3& impulse,const btVector3& rpos) const - { + { if(m_rigid) m_rigid->applyImpulse(impulse,rpos); if(m_soft) btSoftBody::clusterDImpulse(m_soft,rpos,impulse); - } + } void applyImpulse(const Impulse& impulse,const btVector3& rpos) const - { + { if(impulse.m_asVelocity) applyVImpulse(impulse.m_velocity,rpos); if(impulse.m_asDrift) applyDImpulse(impulse.m_drift,rpos); - } + } void applyVAImpulse(const btVector3& impulse) const - { + { if(m_rigid) m_rigid->applyTorqueImpulse(impulse); if(m_soft) btSoftBody::clusterVAImpulse(m_soft,impulse); - } + } void applyDAImpulse(const btVector3& impulse) const - { + { if(m_rigid) m_rigid->applyTorqueImpulse(impulse); if(m_soft) btSoftBody::clusterDAImpulse(m_soft,impulse); - } + } void applyAImpulse(const Impulse& impulse) const - { + { if(impulse.m_asVelocity) applyVAImpulse(impulse.m_velocity); if(impulse.m_asDrift) applyDAImpulse(impulse.m_drift); - } + } void applyDCImpulse(const btVector3& impulse) const - { + { if(m_rigid) m_rigid->applyCentralImpulse(impulse); if(m_soft) btSoftBody::clusterDCImpulse(m_soft,impulse); - } + } }; /* Joint */ struct Joint @@ -420,12 +420,12 @@ public: Contact, };}; struct Specs - { - Specs() : erp(1),cfm(1),split(1) {} + { + Specs() : erp(1),cfm(1),split(1) {} btScalar erp; btScalar cfm; btScalar split; - }; + }; Body m_bodies[2]; btVector3 m_refs[2]; btScalar m_cfm; @@ -436,7 +436,7 @@ public: btMatrix3x3 m_massmatrix; bool m_delete; virtual ~Joint() {} - Joint() : m_delete(false) {} + Joint() : m_delete(false) {} virtual void Prepare(btScalar dt,int iterations); virtual void Solve(btScalar dt,btScalar sor)=0; virtual void Terminate(btScalar dt)=0; @@ -446,9 +446,9 @@ public: struct LJoint : Joint { struct Specs : Joint::Specs - { + { btVector3 position; - }; + }; btVector3 m_rpos[2]; void Prepare(btScalar dt,int iterations); void Solve(btScalar dt,btScalar sor); @@ -459,17 +459,17 @@ public: struct AJoint : Joint { struct IControl - { + { virtual void Prepare(AJoint*) {} virtual btScalar Speed(AJoint*,btScalar current) { return(current); } static IControl* Default() { static IControl def;return(&def); } - }; + }; struct Specs : Joint::Specs - { - Specs() : icontrol(IControl::Default()) {} + { + Specs() : icontrol(IControl::Default()) {} btVector3 axis; IControl* icontrol; - }; + }; btVector3 m_axis[2]; IControl* m_icontrol; void Prepare(btScalar dt,int iterations); @@ -534,24 +534,24 @@ public: }; /// RayFromToCaster takes a ray from, ray to (instead of direction!) struct RayFromToCaster : btDbvt::ICollide - { + { btVector3 m_rayFrom; btVector3 m_rayTo; btVector3 m_rayNormalizedDirection; btScalar m_mint; Face* m_face; int m_tests; - RayFromToCaster(const btVector3& rayFrom,const btVector3& rayTo,btScalar mxt); + RayFromToCaster(const btVector3& rayFrom,const btVector3& rayTo,btScalar mxt); void Process(const btDbvtNode* leaf); - + static inline btScalar rayFromToTriangle(const btVector3& rayFrom, - const btVector3& rayTo, - const btVector3& rayNormalizedDirection, - const btVector3& a, - const btVector3& b, - const btVector3& c, - btScalar maxt=SIMD_INFINITY); - }; + const btVector3& rayTo, + const btVector3& rayNormalizedDirection, + const btVector3& a, + const btVector3& b, + const btVector3& c, + btScalar maxt=SIMD_INFINITY); + }; // // Typedef's @@ -597,15 +597,15 @@ public: btDbvt m_fdbvt; // Faces tree btDbvt m_cdbvt; // Clusters tree tClusterArray m_clusters; // Clusters - + // // Api // - + /* ctor */ btSoftBody( btSoftBodyWorldInfo* worldInfo,int node_count, - const btVector3* x, - const btScalar* m); + const btVector3* x, + const btScalar* m); /* dtor */ virtual ~btSoftBody(); /* Check for existing link */ @@ -625,51 +625,51 @@ public: bool checkLink( int node0, int node1) const; bool checkLink( const Node* node0, - const Node* node1) const; + const Node* node1) const; /* Check for existring face */ bool checkFace( int node0, - int node1, - int node2) const; + int node1, + int node2) const; /* Append material */ Material* appendMaterial(); /* Append note */ void appendNote( const char* text, - const btVector3& o, - const btVector4& c=btVector4(1,0,0,0), - Node* n0=0, - Node* n1=0, - Node* n2=0, - Node* n3=0); + const btVector3& o, + const btVector4& c=btVector4(1,0,0,0), + Node* n0=0, + Node* n1=0, + Node* n2=0, + Node* n3=0); void appendNote( const char* text, - const btVector3& o, - Node* feature); + const btVector3& o, + Node* feature); void appendNote( const char* text, - const btVector3& o, - Link* feature); + const btVector3& o, + Link* feature); void appendNote( const char* text, - const btVector3& o, - Face* feature); + const btVector3& o, + Face* feature); /* Append node */ void appendNode( const btVector3& x,btScalar m); /* Append link */ void appendLink(int model=-1,Material* mat=0); void appendLink( int node0, - int node1, - Material* mat=0, - bool bcheckexist=false); + int node1, + Material* mat=0, + bool bcheckexist=false); void appendLink( Node* node0, - Node* node1, - Material* mat=0, - bool bcheckexist=false); + Node* node1, + Material* mat=0, + bool bcheckexist=false); /* Append face */ void appendFace(int model=-1,Material* mat=0); void appendFace( int node0, - int node1, - int node2, - Material* mat=0); + int node1, + int node2, + Material* mat=0); /* Append anchor */ void appendAnchor( int node, - btRigidBody* body); + btRigidBody* body); /* Append linear joint */ void appendLinearJoint(const LJoint::Specs& specs,Cluster* body0,Body body1); void appendLinearJoint(const LJoint::Specs& specs,Body body=Body()); @@ -682,7 +682,7 @@ public: void addForce( const btVector3& force); /* Add force (or gravity) to a node of the body */ void addForce( const btVector3& force, - int node); + int node); /* Add velocity to the entire body */ void addVelocity( const btVector3& velocity); @@ -691,17 +691,17 @@ public: /* Add velocity to a node of the body */ void addVelocity( const btVector3& velocity, - int node); + int node); /* Set mass */ void setMass( int node, - btScalar mass); + btScalar mass); /* Get mass */ btScalar getMass( int node) const; /* Get total mass */ btScalar getTotalMass() const; /* Set total mass (weighted by previous masses) */ void setTotalMass( btScalar mass, - bool fromfaces=false); + bool fromfaces=false); /* Set total density */ void setTotalDensity(btScalar density); /* Transform */ @@ -714,7 +714,7 @@ public: void scale( const btVector3& scl); /* Set current state as pose */ void setPose( bool bvolume, - bool bframe); + bool bframe); /* Return the volume */ btScalar getVolume() const; /* Cluster count */ @@ -734,7 +734,7 @@ public: static void clusterDCImpulse(Cluster* cluster,const btVector3& impulse); /* Generate bending constraints based on distance in the adjency graph */ int generateBendingConstraints( int distance, - Material* mat=0); + Material* mat=0); /* Randomize constraints to reduce solver bias */ void randomizeConstraints(); /* Release clusters */ @@ -747,11 +747,11 @@ public: /* CutLink */ bool cutLink(int node0,int node1,btScalar position); bool cutLink(const Node* node0,const Node* node1,btScalar position); - + ///Ray casting using rayFrom and rayTo in worldspace, (not direction!) bool rayTest(const btVector3& rayFrom, - const btVector3& rayTo, - sRayCast& results); + const btVector3& rayTo, + sRayCast& results); /* Solver presets */ void setSolver(eSolverPresets::_ preset); /* predictMotion */ @@ -769,11 +769,11 @@ public: /* defaultCollisionHandlers */ void defaultCollisionHandler(btCollisionObject* pco); void defaultCollisionHandler(btSoftBody* psb); - + // // Cast // - + static const btSoftBody* upcast(const btCollisionObject* colObj) { if (colObj->getInternalType()==CO_SOFT_BODY) @@ -803,7 +803,7 @@ public: void indicesToPointers(const int* map=0); int rayTest(const btVector3& rayFrom,const btVector3& rayTo, - btScalar& mint,eFeature::_& feature,int& index,bool bcountonly) const; + btScalar& mint,eFeature::_& feature,int& index,bool bcountonly) const; void initializeFaceTree(); btVector3 evaluateCom() const; bool checkContact(btRigidBody* prb,const btVector3& x,btScalar margin,btSoftBody::sCti& cti) const; @@ -826,7 +826,7 @@ public: static void VSolve_Links(btSoftBody* psb,btScalar kst); static psolver_t getSolver(ePSolver::_ solver); static vsolver_t getSolver(eVSolver::_ solver); - + }; diff --git a/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp b/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp index 4dd99a3df..d2cd74118 100644 --- a/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp +++ b/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp @@ -50,27 +50,27 @@ btSoftBodyConcaveCollisionAlgorithm::~btSoftBodyConcaveCollisionAlgorithm() btSoftBodyTriangleCallback::btSoftBodyTriangleCallback(btDispatcher* dispatcher,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped): - m_dispatcher(dispatcher), - m_dispatchInfoPtr(0) +m_dispatcher(dispatcher), +m_dispatchInfoPtr(0) { m_softBody = (btSoftBody*) (isSwapped? body1:body0); m_triBody = isSwapped? body0:body1; - - // - // create the manifold from the dispatcher 'manifold pool' - // -// m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBody,m_triBody); - clearCache(); + // + // create the manifold from the dispatcher 'manifold pool' + // + // m_manifoldPtr = m_dispatcher->getNewManifold(m_convexBody,m_triBody); + + clearCache(); } btSoftBodyTriangleCallback::~btSoftBodyTriangleCallback() { clearCache(); -// m_dispatcher->releaseManifold( m_manifoldPtr ); - + // m_dispatcher->releaseManifold( m_manifoldPtr ); + } - + void btSoftBodyTriangleCallback::clearCache() { @@ -88,13 +88,13 @@ void btSoftBodyTriangleCallback::clearCache() void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId, int triangleIndex) { - //just for debugging purposes + //just for debugging purposes //printf("triangle %d",m_triangleCount++); btCollisionObject* ob = static_cast(m_triBody); btCollisionAlgorithmConstructionInfo ci; ci.m_dispatcher1 = m_dispatcher; - ///debug drawing of the overlapping triangles + ///debug drawing of the overlapping triangles if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && m_dispatchInfoPtr->m_debugDraw->getDebugMode() > 0) { btVector3 color(255,255,0); @@ -107,7 +107,7 @@ void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId, btTriIndex triIndex(partId,triangleIndex,0); btHashKey triKey(triIndex.getUid()); - + btTriIndex* shapeIndex = m_shapeCache[triKey]; if (shapeIndex) { @@ -116,13 +116,13 @@ void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId, //copy over user pointers to temporary shape tm->setUserPointer(ob->getRootCollisionShape()->getUserPointer()); - + btCollisionShape* tmpShape = ob->getCollisionShape(); ob->internalSetTemporaryCollisionShape( tm ); - + btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_softBody,m_triBody,0);//m_manifoldPtr); - + colAlgo->processCollision(m_softBody,m_triBody,*m_dispatchInfoPtr,m_resultOut); colAlgo->~btCollisionAlgorithm(); ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo); @@ -133,56 +133,56 @@ void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId, //aabb filter is already applied! //btCollisionObject* colObj = static_cast(m_convexProxy->m_clientObject); - -// if (m_softBody->getCollisionShape()->getShapeType()== + + // if (m_softBody->getCollisionShape()->getShapeType()== { -// btVector3 other; + // btVector3 other; btVector3 normal = (triangle[1]-triangle[0]).cross(triangle[2]-triangle[0]); normal.normalize(); normal*= BT_SOFTBODY_TRIANGLE_EXTRUSION; -// other=(triangle[0]+triangle[1]+triangle[2])*0.333333f; -// other+=normal*22.f; + // other=(triangle[0]+triangle[1]+triangle[2])*0.333333f; + // other+=normal*22.f; btVector3 pts[6] = {triangle[0]+normal, triangle[1]+normal, - triangle[2]+normal, - triangle[0]-normal, - triangle[1]-normal, - triangle[2]-normal}; + triangle[2]+normal, + triangle[0]-normal, + triangle[1]-normal, + triangle[2]-normal}; btConvexHullShape* tm = new btConvexHullShape(&pts[0].getX(),6); -// btBU_Simplex1to4 tm(triangle[0],triangle[1],triangle[2],other); - + // btBU_Simplex1to4 tm(triangle[0],triangle[1],triangle[2],other); + //btTriangleShape tm(triangle[0],triangle[1],triangle[2]); - // tm.setMargin(m_collisionMarginTriangle); - + // tm.setMargin(m_collisionMarginTriangle); + //copy over user pointers to temporary shape tm->setUserPointer(ob->getRootCollisionShape()->getUserPointer()); - + btCollisionShape* tmpShape = ob->getCollisionShape(); ob->internalSetTemporaryCollisionShape( tm ); - + btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_softBody,m_triBody,0);//m_manifoldPtr); ///this should use the btDispatcher, so the actual registered algorithm is used // btConvexConvexAlgorithm cvxcvxalgo(m_manifoldPtr,ci,m_convexBody,m_triBody); //m_resultOut->setShapeIdentifiers(-1,-1,partId,triangleIndex); - // cvxcvxalgo.setShapeIdentifiers(-1,-1,partId,triangleIndex); -// cvxcvxalgo.processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut); + // cvxcvxalgo.setShapeIdentifiers(-1,-1,partId,triangleIndex); + // cvxcvxalgo.processCollision(m_convexBody,m_triBody,*m_dispatchInfoPtr,m_resultOut); colAlgo->processCollision(m_softBody,m_triBody,*m_dispatchInfoPtr,m_resultOut); colAlgo->~btCollisionAlgorithm(); ci.m_dispatcher1->freeCollisionAlgorithm(colAlgo); - - + + ob->internalSetTemporaryCollisionShape( tmpShape ); triIndex.m_childShape = tm; m_shapeCache.insert(triKey,triIndex); } - + } @@ -194,7 +194,7 @@ void btSoftBodyTriangleCallback::setTimeStepAndCounters(btScalar collisionMargin m_collisionMarginTriangle = collisionMarginTriangle+btScalar(BT_SOFTBODY_TRIANGLE_EXTRUSION); m_resultOut = resultOut; - + btVector3 aabbWorldSpaceMin,aabbWorldSpaceMax; m_softBody->getAabb(aabbWorldSpaceMin,aabbWorldSpaceMax); btVector3 halfExtents = (aabbWorldSpaceMax-aabbWorldSpaceMin)*btScalar(0.5); @@ -217,8 +217,8 @@ void btSoftBodyConcaveCollisionAlgorithm::clearCache() void btSoftBodyConcaveCollisionAlgorithm::processCollision (btCollisionObject* body0,btCollisionObject* body1,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut) { - - + + btCollisionObject* convexBody = m_isSwapped ? body1 : body0; btCollisionObject* triBody = m_isSwapped ? body0 : body1; @@ -228,26 +228,26 @@ void btSoftBodyConcaveCollisionAlgorithm::processCollision (btCollisionObject* b btCollisionObject* triOb = triBody; btConcaveShape* concaveShape = static_cast( triOb->getCollisionShape()); - - // if (convexBody->getCollisionShape()->isConvex()) + + // if (convexBody->getCollisionShape()->isConvex()) { btScalar collisionMarginTriangle = concaveShape->getMargin(); - -// resultOut->setPersistentManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr); + + // resultOut->setPersistentManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr); m_btSoftBodyTriangleCallback.setTimeStepAndCounters(collisionMarginTriangle,dispatchInfo,resultOut); //Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here. //m_dispatcher->clearManifold(m_btSoftBodyTriangleCallback.m_manifoldPtr); -// m_btSoftBodyTriangleCallback.m_manifoldPtr->setBodies(convexBody,triBody); + // m_btSoftBodyTriangleCallback.m_manifoldPtr->setBodies(convexBody,triBody); concaveShape->processAllTriangles( &m_btSoftBodyTriangleCallback,m_btSoftBodyTriangleCallback.getAabbMin(),m_btSoftBodyTriangleCallback.getAabbMax()); - - // resultOut->refreshContactPoints(); - + + // resultOut->refreshContactPoints(); + } - + } } @@ -287,7 +287,7 @@ btScalar btSoftBodyConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionO btScalar m_ccdSphereRadius; btScalar m_hitFraction; - + LocalTriangleSphereCastCallback(const btTransform& from,const btTransform& to,btScalar ccdSphereRadius,btScalar hitFraction) :m_ccdSphereFromTrans(from), @@ -296,8 +296,8 @@ btScalar btSoftBodyConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionO m_hitFraction(hitFraction) { } - - + + virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) { (void)partId; @@ -327,9 +327,9 @@ btScalar btSoftBodyConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionO }; - - + + if (triBody->getCollisionShape()->isConcave()) { btVector3 rayAabbMin = convexFromLocal.getOrigin(); @@ -349,12 +349,12 @@ btScalar btSoftBodyConcaveCollisionAlgorithm::calculateTimeOfImpact(btCollisionO btCollisionObject* concavebody = triBody; btConcaveShape* triangleMesh = (btConcaveShape*) concavebody->getCollisionShape(); - + if (triangleMesh) { triangleMesh->processAllTriangles(&raycastCallback,rayAabbMin,rayAabbMax); } - + if (raycastCallback.m_hitFraction < convexbody->getHitFraction()) diff --git a/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h b/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h index b9d507843..cddcf2cda 100644 --- a/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h +++ b/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h @@ -35,7 +35,7 @@ struct btTriIndex { int m_PartIdTriangleIndex; class btCollisionShape* m_childShape; - + btTriIndex(int partId,int triangleIndex,btCollisionShape* shape) { m_PartIdTriangleIndex = (partId<<(31-MAX_NUM_PARTS_IN_BITS)) | triangleIndex; @@ -75,11 +75,11 @@ class btSoftBodyTriangleCallback : public btTriangleCallback btScalar m_collisionMarginTriangle; btHashMap,btTriIndex> m_shapeCache; - + public: -int m_triangleCount; - -// btPersistentManifold* m_manifoldPtr; + int m_triangleCount; + + // btPersistentManifold* m_manifoldPtr; btSoftBodyTriangleCallback(btDispatcher* dispatcher,btCollisionObject* body0,btCollisionObject* body1,bool isSwapped); @@ -88,7 +88,7 @@ int m_triangleCount; virtual ~btSoftBodyTriangleCallback(); virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex); - + void clearCache(); SIMD_FORCE_INLINE const btVector3& getAabbMin() const diff --git a/src/BulletSoftBody/btSoftBodyHelpers.cpp b/src/BulletSoftBody/btSoftBodyHelpers.cpp index c8aa8df7d..eb86d4587 100644 --- a/src/BulletSoftBody/btSoftBodyHelpers.cpp +++ b/src/BulletSoftBody/btSoftBodyHelpers.cpp @@ -22,57 +22,57 @@ subject to the following restrictions: // static void drawVertex( btIDebugDraw* idraw, - const btVector3& x,btScalar s,const btVector3& c) - { - idraw->drawLine(x-btVector3(s,0,0),x+btVector3(s,0,0),c); - idraw->drawLine(x-btVector3(0,s,0),x+btVector3(0,s,0),c); - idraw->drawLine(x-btVector3(0,0,s),x+btVector3(0,0,s),c); - } + const btVector3& x,btScalar s,const btVector3& c) +{ + idraw->drawLine(x-btVector3(s,0,0),x+btVector3(s,0,0),c); + idraw->drawLine(x-btVector3(0,s,0),x+btVector3(0,s,0),c); + idraw->drawLine(x-btVector3(0,0,s),x+btVector3(0,0,s),c); +} // static void drawBox( btIDebugDraw* idraw, - const btVector3& mins, - const btVector3& maxs, - const btVector3& color) + const btVector3& mins, + const btVector3& maxs, + const btVector3& color) { -const btVector3 c[]={ btVector3(mins.x(),mins.y(),mins.z()), - btVector3(maxs.x(),mins.y(),mins.z()), - btVector3(maxs.x(),maxs.y(),mins.z()), - btVector3(mins.x(),maxs.y(),mins.z()), - btVector3(mins.x(),mins.y(),maxs.z()), - btVector3(maxs.x(),mins.y(),maxs.z()), - btVector3(maxs.x(),maxs.y(),maxs.z()), - btVector3(mins.x(),maxs.y(),maxs.z())}; -idraw->drawLine(c[0],c[1],color);idraw->drawLine(c[1],c[2],color); -idraw->drawLine(c[2],c[3],color);idraw->drawLine(c[3],c[0],color); -idraw->drawLine(c[4],c[5],color);idraw->drawLine(c[5],c[6],color); -idraw->drawLine(c[6],c[7],color);idraw->drawLine(c[7],c[4],color); -idraw->drawLine(c[0],c[4],color);idraw->drawLine(c[1],c[5],color); -idraw->drawLine(c[2],c[6],color);idraw->drawLine(c[3],c[7],color); + const btVector3 c[]={ btVector3(mins.x(),mins.y(),mins.z()), + btVector3(maxs.x(),mins.y(),mins.z()), + btVector3(maxs.x(),maxs.y(),mins.z()), + btVector3(mins.x(),maxs.y(),mins.z()), + btVector3(mins.x(),mins.y(),maxs.z()), + btVector3(maxs.x(),mins.y(),maxs.z()), + btVector3(maxs.x(),maxs.y(),maxs.z()), + btVector3(mins.x(),maxs.y(),maxs.z())}; + idraw->drawLine(c[0],c[1],color);idraw->drawLine(c[1],c[2],color); + idraw->drawLine(c[2],c[3],color);idraw->drawLine(c[3],c[0],color); + idraw->drawLine(c[4],c[5],color);idraw->drawLine(c[5],c[6],color); + idraw->drawLine(c[6],c[7],color);idraw->drawLine(c[7],c[4],color); + idraw->drawLine(c[0],c[4],color);idraw->drawLine(c[1],c[5],color); + idraw->drawLine(c[2],c[6],color);idraw->drawLine(c[3],c[7],color); } // static void drawTree( btIDebugDraw* idraw, - const btDbvtNode* node, - int depth, - const btVector3& ncolor, - const btVector3& lcolor, - int mindepth, - int maxdepth) + const btDbvtNode* node, + int depth, + const btVector3& ncolor, + const btVector3& lcolor, + int mindepth, + int maxdepth) { -if(node) + if(node) { - if(node->isinternal()&&((depthisinternal()&&((depthchilds[0],depth+1,ncolor,lcolor,mindepth,maxdepth); - drawTree(idraw,node->childs[1],depth+1,ncolor,lcolor,mindepth,maxdepth); + drawTree(idraw,node->childs[0],depth+1,ncolor,lcolor,mindepth,maxdepth); + drawTree(idraw,node->childs[1],depth+1,ncolor,lcolor,mindepth,maxdepth); } - if(depth>=mindepth) + if(depth>=mindepth) { - const btScalar scl=(btScalar)(node->isinternal()?1:1); - const btVector3 mi=node->volume.Center()-node->volume.Extents()*scl; - const btVector3 mx=node->volume.Center()+node->volume.Extents()*scl; - drawBox(idraw,mi,mx,node->isleaf()?lcolor:ncolor); + const btScalar scl=(btScalar)(node->isinternal()?1:1); + const btVector3 mi=node->volume.Center()-node->volume.Extents()*scl; + const btVector3 mx=node->volume.Center()+node->volume.Extents()*scl; + drawBox(idraw,mi,mx,node->isleaf()?lcolor:ncolor); } } } @@ -81,25 +81,25 @@ if(node) template static inline T sum(const btAlignedObjectArray& items) { -T v; -if(items.size()) + T v; + if(items.size()) { - v=items[0]; - for(int i=1,ni=items.size();i static inline void add(btAlignedObjectArray& items,const Q& value) { -for(int i=0,ni=items.size();i static inline void mul(btAlignedObjectArray& items,const Q& value) { -for(int i=0,ni=items.size();i static inline T average(const btAlignedObjectArray& items) { -const btScalar n=(btScalar)(items.size()>0?items.size():1); -return(sum(items)/n); + const btScalar n=(btScalar)(items.size()>0?items.size():1); + return(sum(items)/n); } // @@ -136,27 +136,27 @@ static inline btScalar tetravolume(const btVector3& x0, // #if 0 static btVector3 stresscolor(btScalar stress) - { +{ static const btVector3 spectrum[]= { btVector3(1,0,1), - btVector3(0,0,1), - btVector3(0,1,1), - btVector3(0,1,0), - btVector3(1,1,0), - btVector3(1,0,0), - btVector3(1,0,0)}; + btVector3(0,0,1), + btVector3(0,1,1), + btVector3(0,1,0), + btVector3(1,1,0), + btVector3(1,0,0), + btVector3(1,0,0)}; static const int ncolors=sizeof(spectrum)/sizeof(spectrum[0])-1; static const btScalar one=1; stress=btMax(0,btMin(1,stress))*ncolors; const int sel=(int)stress; const btScalar frc=stress-sel; return(spectrum[sel]+(spectrum[sel+1]-spectrum[sel])*frc); - } +} #endif // void btSoftBodyHelpers::Draw( btSoftBody* psb, - btIDebugDraw* idraw, - int drawflags) + btIDebugDraw* idraw, + int drawflags) { const btScalar scl=(btScalar)0.1; const btScalar nscl=scl*5; @@ -251,29 +251,29 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb, const btVector3 x[]={f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x}; const btVector3 c=(x[0]+x[1]+x[2])/3; idraw->drawTriangle((x[0]-c)*scl+c, - (x[1]-c)*scl+c, - (x[2]-c)*scl+c, - col,alp); + (x[1]-c)*scl+c, + (x[2]-c)*scl+c, + col,alp); } } /* Clusters */ if(0!=(drawflags&fDrawFlags::Clusters)) - { + { srand(1806); for(i=0;im_clusters.size();++i) - { + { if(psb->m_clusters[i]->m_collide) - { + { btVector3 color( rand()/(btScalar)RAND_MAX, - rand()/(btScalar)RAND_MAX, - rand()/(btScalar)RAND_MAX); + rand()/(btScalar)RAND_MAX, + rand()/(btScalar)RAND_MAX); color=color.normalized()*0.75; btAlignedObjectArray vertices; vertices.resize(psb->m_clusters[i]->m_nodes.size()); for(j=0,nj=vertices.size();jm_clusters[i]->m_nodes[j]->m_x; - } + } HullDesc hdsc(QF_TRIANGLES,vertices.size(),&vertices[0]); HullResult hres; HullLibrary hlib; @@ -284,32 +284,32 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb, mul(hres.m_OutputVertices,(btScalar)1); add(hres.m_OutputVertices,center); for(j=0;j<(int)hres.mNumFaces;++j) - { + { const int idx[]={hres.m_Indices[j*3+0],hres.m_Indices[j*3+1],hres.m_Indices[j*3+2]}; idraw->drawTriangle(hres.m_OutputVertices[idx[0]], - hres.m_OutputVertices[idx[1]], - hres.m_OutputVertices[idx[2]], - color,1); - } - hlib.ReleaseResult(hres); + hres.m_OutputVertices[idx[1]], + hres.m_OutputVertices[idx[2]], + color,1); } + hlib.ReleaseResult(hres); + } /* Velocities */ - #if 0 +#if 0 for(int j=0;jm_clusters[i].m_nodes.size();++j) - { + { const btSoftBody::Cluster& c=psb->m_clusters[i]; const btVector3 r=c.m_nodes[j]->m_x-c.m_com; const btVector3 v=c.m_lv+cross(c.m_av,r); idraw->drawLine(c.m_nodes[j]->m_x,c.m_nodes[j]->m_x+v,btVector3(1,0,0)); - } - #endif + } +#endif /* Frame */ btSoftBody::Cluster& c=*psb->m_clusters[i]; idraw->drawLine(c.m_com,c.m_framexform*btVector3(10,0,0),btVector3(1,0,0)); idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,10,0),btVector3(0,1,0)); idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,0,10),btVector3(0,0,1)); - } } + } /* Notes */ if(0!=(drawflags&fDrawFlags::Notes)) { @@ -318,9 +318,9 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb, const btSoftBody::Note& n=psb->m_notes[i]; btVector3 p=n.m_offset; for(int j=0;jm_x*n.m_coords[j]; - } + } idraw->draw3dText(p,n.m_text); } } @@ -333,33 +333,33 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb, /* Joints */ if(0!=(drawflags&fDrawFlags::Joints)) { - for(i=0;im_joints.size();++i) + for(i=0;im_joints.size();++i) { - const btSoftBody::Joint* pj=psb->m_joints[i]; - switch(pj->Type()) + const btSoftBody::Joint* pj=psb->m_joints[i]; + switch(pj->Type()) { case btSoftBody::Joint::eType::Linear: { - const btSoftBody::LJoint* pjl=(const btSoftBody::LJoint*)pj; - const btVector3 a0=pj->m_bodies[0].xform()*pjl->m_refs[0]; - const btVector3 a1=pj->m_bodies[1].xform()*pjl->m_refs[1]; - idraw->drawLine(pj->m_bodies[0].xform().getOrigin(),a0,btVector3(1,1,0)); - idraw->drawLine(pj->m_bodies[1].xform().getOrigin(),a1,btVector3(0,1,1)); - drawVertex(idraw,a0,0.25,btVector3(1,1,0)); - drawVertex(idraw,a1,0.25,btVector3(0,1,1)); + const btSoftBody::LJoint* pjl=(const btSoftBody::LJoint*)pj; + const btVector3 a0=pj->m_bodies[0].xform()*pjl->m_refs[0]; + const btVector3 a1=pj->m_bodies[1].xform()*pjl->m_refs[1]; + idraw->drawLine(pj->m_bodies[0].xform().getOrigin(),a0,btVector3(1,1,0)); + idraw->drawLine(pj->m_bodies[1].xform().getOrigin(),a1,btVector3(0,1,1)); + drawVertex(idraw,a0,0.25,btVector3(1,1,0)); + drawVertex(idraw,a1,0.25,btVector3(0,1,1)); } - break; + break; case btSoftBody::Joint::eType::Angular: { - const btSoftBody::AJoint* pja=(const btSoftBody::AJoint*)pj; - const btVector3 o0=pj->m_bodies[0].xform().getOrigin(); - const btVector3 o1=pj->m_bodies[1].xform().getOrigin(); - const btVector3 a0=pj->m_bodies[0].xform().getBasis()*pj->m_refs[0]; - const btVector3 a1=pj->m_bodies[1].xform().getBasis()*pj->m_refs[1]; - idraw->drawLine(o0,o0+a0*10,btVector3(1,1,0)); - idraw->drawLine(o0,o0+a1*10,btVector3(1,1,0)); - idraw->drawLine(o1,o1+a0*10,btVector3(0,1,1)); - idraw->drawLine(o1,o1+a1*10,btVector3(0,1,1)); + const btSoftBody::AJoint* pja=(const btSoftBody::AJoint*)pj; + const btVector3 o0=pj->m_bodies[0].xform().getOrigin(); + const btVector3 o1=pj->m_bodies[1].xform().getOrigin(); + const btVector3 a0=pj->m_bodies[0].xform().getBasis()*pj->m_refs[0]; + const btVector3 a1=pj->m_bodies[1].xform().getBasis()*pj->m_refs[1]; + idraw->drawLine(o0,o0+a0*10,btVector3(1,1,0)); + idraw->drawLine(o0,o0+a1*10,btVector3(1,1,0)); + idraw->drawLine(o1,o1+a0*10,btVector3(0,1,1)); + idraw->drawLine(o1,o1+a1*10,btVector3(0,1,1)); } } } @@ -368,10 +368,10 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb, // void btSoftBodyHelpers::DrawInfos( btSoftBody* psb, - btIDebugDraw* idraw, - bool masses, - bool areas, - bool /*stress*/) + btIDebugDraw* idraw, + bool masses, + bool areas, + bool /*stress*/) { for(int i=0;im_nodes.size();++i) { @@ -394,34 +394,34 @@ void btSoftBodyHelpers::DrawInfos( btSoftBody* psb, // void btSoftBodyHelpers::DrawNodeTree( btSoftBody* psb, - btIDebugDraw* idraw, - int mindepth, - int maxdepth) + btIDebugDraw* idraw, + int mindepth, + int maxdepth) { -drawTree(idraw,psb->m_ndbvt.m_root,0,btVector3(1,0,1),btVector3(1,1,1),mindepth,maxdepth); + drawTree(idraw,psb->m_ndbvt.m_root,0,btVector3(1,0,1),btVector3(1,1,1),mindepth,maxdepth); } // void btSoftBodyHelpers::DrawFaceTree( btSoftBody* psb, - btIDebugDraw* idraw, - int mindepth, - int maxdepth) + btIDebugDraw* idraw, + int mindepth, + int maxdepth) { -drawTree(idraw,psb->m_fdbvt.m_root,0,btVector3(0,1,0),btVector3(1,0,0),mindepth,maxdepth); + drawTree(idraw,psb->m_fdbvt.m_root,0,btVector3(0,1,0),btVector3(1,0,0),mindepth,maxdepth); } // void btSoftBodyHelpers::DrawClusterTree( btSoftBody* psb, - btIDebugDraw* idraw, - int mindepth, - int maxdepth) + btIDebugDraw* idraw, + int mindepth, + int maxdepth) { -drawTree(idraw,psb->m_cdbvt.m_root,0,btVector3(0,1,1),btVector3(1,0,0),mindepth,maxdepth); + drawTree(idraw,psb->m_cdbvt.m_root,0,btVector3(0,1,1),btVector3(1,0,0),mindepth,maxdepth); } // void btSoftBodyHelpers::DrawFrame( btSoftBody* psb, - btIDebugDraw* idraw) + btIDebugDraw* idraw) { if(psb->m_pose.m_bframe) { @@ -445,9 +445,9 @@ void btSoftBodyHelpers::DrawFrame( btSoftBody* psb, // btSoftBody* btSoftBodyHelpers::CreateRope( btSoftBodyWorldInfo& worldInfo, const btVector3& from, - const btVector3& to, - int res, - int fixeds) + const btVector3& to, + int res, + int fixeds) { /* Create nodes */ const int r=res+2; @@ -477,13 +477,13 @@ btSoftBody* btSoftBodyHelpers::CreateRope( btSoftBodyWorldInfo& worldInfo, cons // btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBodyWorldInfo& worldInfo,const btVector3& corner00, - const btVector3& corner10, - const btVector3& corner01, - const btVector3& corner11, - int resx, - int resy, - int fixeds, - bool gendiags) + const btVector3& corner10, + const btVector3& corner01, + const btVector3& corner11, + int resx, + int resy, + int fixeds, + bool gendiags) { #define IDX(_x_,_y_) ((_y_)*rx+(_x_)) /* Create nodes */ @@ -554,83 +554,83 @@ btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBodyWorldInfo& worldInfo,const // btSoftBody* btSoftBodyHelpers::CreatePatchUV(btSoftBodyWorldInfo& worldInfo, - const btVector3& corner00, - const btVector3& corner10, - const btVector3& corner01, - const btVector3& corner11, - int resx, - int resy, - int fixeds, - bool gendiags, - float* tex_coords) + const btVector3& corner00, + const btVector3& corner10, + const btVector3& corner01, + const btVector3& corner11, + int resx, + int resy, + int fixeds, + bool gendiags, + float* tex_coords) { -/* - * - * corners: - * - * [0][0] corner00 ------- corner01 [resx][0] - * | | - * | | - * [0][resy] corner10 -------- corner11 [resx][resy] - * - * - * - * - * - * - * "fixedgs" map: - * - * corner00 --> +1 - * corner01 --> +2 - * corner10 --> +4 - * corner11 --> +8 - * upper middle --> +16 - * left middle --> +32 - * right middle --> +64 - * lower middle --> +128 - * center --> +256 - * - * - * tex_coords size (resx-1)*(resy-1)*12 - * - * - * - * SINGLE QUAD INTERNALS - * - * 1) btSoftBody's nodes and links, - * diagonal link is optional ("gendiags") - * - * - * node00 ------ node01 - * | . - * | . - * | . - * | . - * | . - * node10 node11 - * - * - * - * 2) Faces: - * two triangles, - * UV Coordinates (hier example for single quad) - * - * (0,1) (0,1) (1,1) - * 1 |\ 3 \-----| 2 - * | \ \ | - * | \ \ | - * | \ \ | - * | \ \ | - * 2 |-----\ 3 \| 1 - * (0,0) (1,0) (1,0) - * - * - * - * - * - * - */ + /* + * + * corners: + * + * [0][0] corner00 ------- corner01 [resx][0] + * | | + * | | + * [0][resy] corner10 -------- corner11 [resx][resy] + * + * + * + * + * + * + * "fixedgs" map: + * + * corner00 --> +1 + * corner01 --> +2 + * corner10 --> +4 + * corner11 --> +8 + * upper middle --> +16 + * left middle --> +32 + * right middle --> +64 + * lower middle --> +128 + * center --> +256 + * + * + * tex_coords size (resx-1)*(resy-1)*12 + * + * + * + * SINGLE QUAD INTERNALS + * + * 1) btSoftBody's nodes and links, + * diagonal link is optional ("gendiags") + * + * + * node00 ------ node01 + * | . + * | . + * | . + * | . + * | . + * node10 node11 + * + * + * + * 2) Faces: + * two triangles, + * UV Coordinates (hier example for single quad) + * + * (0,1) (0,1) (1,1) + * 1 |\ 3 \-----| 2 + * | \ \ | + * | \ \ | + * | \ \ | + * | \ \ | + * 2 |-----\ 3 \| 1 + * (0,0) (1,0) (1,0) + * + * + * + * + * + * + */ #define IDX(_x_,_y_) ((_y_)*rx+(_x_)) /* Create nodes */ @@ -675,12 +675,12 @@ btSoftBody* btSoftBodyHelpers::CreatePatchUV(btSoftBodyWorldInfo& worldInfo, { const bool mdx=(ix+1)appendLink(node00,node01); if(mdy) psb->appendLink(node00,node10); if(mdx&&mdy) @@ -716,32 +716,32 @@ btSoftBody* btSoftBodyHelpers::CreatePatchUV(btSoftBodyWorldInfo& worldInfo, float btSoftBodyHelpers::CalculateUV(int resx,int resy,int ix,int iy,int id) { -/* - * - * - * node00 --- node01 - * | | - * node10 --- node11 - * - * - * ID map: - * - * node00 s --> 0 - * node00 t --> 1 - * - * node01 s --> 3 - * node01 t --> 1 - * - * node10 s --> 0 - * node10 t --> 2 - * - * node11 s --> 3 - * node11 t --> 2 - * - * - */ + /* + * + * + * node00 --- node01 + * | | + * node10 --- node11 + * + * + * ID map: + * + * node00 s --> 0 + * node00 t --> 1 + * + * node01 s --> 3 + * node01 t --> 1 + * + * node10 s --> 0 + * node10 t --> 2 + * + * node11 s --> 3 + * node11 t --> 2 + * + * + */ -float tc=0.0f; + float tc=0.0f; if (id == 0) { tc = (1.0f/((resx-1))*ix); } @@ -754,12 +754,12 @@ float tc=0.0f; else if (id==3) { tc = (1.0f/((resx-1))*(ix+1)); } -return tc; + return tc; } // btSoftBody* btSoftBodyHelpers::CreateEllipsoid(btSoftBodyWorldInfo& worldInfo,const btVector3& center, - const btVector3& radius, - int res) + const btVector3& radius, + int res) { struct Hammersley { @@ -790,8 +790,8 @@ btSoftBody* btSoftBodyHelpers::CreateEllipsoid(btSoftBodyWorldInfo& worldInfo,c // btSoftBody* btSoftBodyHelpers::CreateFromTriMesh(btSoftBodyWorldInfo& worldInfo,const btScalar* vertices, - const int* triangles, - int ntriangles) + const int* triangles, + int ntriangles) { int maxidx=0; int i,j,ni; @@ -832,7 +832,7 @@ btSoftBody* btSoftBodyHelpers::CreateFromTriMesh(btSoftBodyWorldInfo& worldInfo // btSoftBody* btSoftBodyHelpers::CreateFromConvexHull(btSoftBodyWorldInfo& worldInfo, const btVector3* vertices, - int nvertices) + int nvertices) { HullDesc hdsc(QF_TRIANGLES,nvertices,vertices); HullResult hres; diff --git a/src/BulletSoftBody/btSoftBodyHelpers.h b/src/BulletSoftBody/btSoftBodyHelpers.h index 317ea4d91..f835ca824 100644 --- a/src/BulletSoftBody/btSoftBodyHelpers.h +++ b/src/BulletSoftBody/btSoftBodyHelpers.h @@ -46,74 +46,74 @@ struct btSoftBodyHelpers { /* Draw body */ static void Draw( btSoftBody* psb, - btIDebugDraw* idraw, - int drawflags=fDrawFlags::Std); + btIDebugDraw* idraw, + int drawflags=fDrawFlags::Std); /* Draw body infos */ static void DrawInfos( btSoftBody* psb, - btIDebugDraw* idraw, - bool masses, - bool areas, - bool stress); + btIDebugDraw* idraw, + bool masses, + bool areas, + bool stress); /* Draw node tree */ static void DrawNodeTree( btSoftBody* psb, - btIDebugDraw* idraw, - int mindepth=0, - int maxdepth=-1); + btIDebugDraw* idraw, + int mindepth=0, + int maxdepth=-1); /* Draw face tree */ static void DrawFaceTree( btSoftBody* psb, - btIDebugDraw* idraw, - int mindepth=0, - int maxdepth=-1); + btIDebugDraw* idraw, + int mindepth=0, + int maxdepth=-1); /* Draw cluster tree */ static void DrawClusterTree(btSoftBody* psb, - btIDebugDraw* idraw, - int mindepth=0, - int maxdepth=-1); + btIDebugDraw* idraw, + int mindepth=0, + int maxdepth=-1); /* Draw rigid frame */ static void DrawFrame( btSoftBody* psb, - btIDebugDraw* idraw); + btIDebugDraw* idraw); /* Create a rope */ static btSoftBody* CreateRope( btSoftBodyWorldInfo& worldInfo, - const btVector3& from, - const btVector3& to, - int res, - int fixeds); + const btVector3& from, + const btVector3& to, + int res, + int fixeds); /* Create a patch */ static btSoftBody* CreatePatch(btSoftBodyWorldInfo& worldInfo, - const btVector3& corner00, - const btVector3& corner10, - const btVector3& corner01, - const btVector3& corner11, - int resx, - int resy, - int fixeds, - bool gendiags); + const btVector3& corner00, + const btVector3& corner10, + const btVector3& corner01, + const btVector3& corner11, + int resx, + int resy, + int fixeds, + bool gendiags); /* Create a patch with UV Texture Coordinates */ static btSoftBody* CreatePatchUV(btSoftBodyWorldInfo& worldInfo, - const btVector3& corner00, - const btVector3& corner10, - const btVector3& corner01, - const btVector3& corner11, - int resx, - int resy, - int fixeds, - bool gendiags, - float* tex_coords=0); + const btVector3& corner00, + const btVector3& corner10, + const btVector3& corner01, + const btVector3& corner11, + int resx, + int resy, + int fixeds, + bool gendiags, + float* tex_coords=0); static float CalculateUV(int resx,int resy,int ix,int iy,int id); /* Create an ellipsoid */ static btSoftBody* CreateEllipsoid(btSoftBodyWorldInfo& worldInfo, - const btVector3& center, - const btVector3& radius, - int res); + const btVector3& center, + const btVector3& radius, + int res); /* Create from trimesh */ static btSoftBody* CreateFromTriMesh( btSoftBodyWorldInfo& worldInfo, - const btScalar* vertices, - const int* triangles, - int ntriangles); + const btScalar* vertices, + const int* triangles, + int ntriangles); /* Create from convex-hull */ static btSoftBody* CreateFromConvexHull( btSoftBodyWorldInfo& worldInfo, - const btVector3* vertices, - int nvertices); + const btVector3* vertices, + int nvertices); }; #endif //SOFT_BODY_HELPERS_H diff --git a/src/BulletSoftBody/btSoftBodyInternals.h b/src/BulletSoftBody/btSoftBodyInternals.h index b307496ca..49ad177e9 100644 --- a/src/BulletSoftBody/btSoftBodyInternals.h +++ b/src/BulletSoftBody/btSoftBodyInternals.h @@ -31,14 +31,14 @@ subject to the following restrictions: template struct btSymMatrix { - btSymMatrix() : dim(0) {} - btSymMatrix(int n,const T& init=T()) { resize(n,init); } -void resize(int n,const T& init=T()) { dim=n;store.resize((n*(n+1))/2,init); } -int index(int c,int r) const { if(c>r) btSwap(c,r);btAssert(r store; -int dim; + btSymMatrix() : dim(0) {} + btSymMatrix(int n,const T& init=T()) { resize(n,init); } + void resize(int n,const T& init=T()) { dim=n;store.resize((n*(n+1))/2,init); } + int index(int c,int r) const { if(c>r) btSwap(c,r);btAssert(r store; + int dim; }; // @@ -48,7 +48,7 @@ class btSoftBodyCollisionShape : public btConcaveShape { public: btSoftBody* m_body; - + btSoftBodyCollisionShape(btSoftBody* backptr) { m_shapeType = SOFTBODY_SHAPE_PROXYTYPE; @@ -69,34 +69,34 @@ public: ///getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t. virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const { - /* t should be identity, but better be safe than...fast? */ - const btVector3 mins=m_body->m_bounds[0]; - const btVector3 maxs=m_body->m_bounds[1]; - const btVector3 crns[]={t*btVector3(mins.x(),mins.y(),mins.z()), - t*btVector3(maxs.x(),mins.y(),mins.z()), - t*btVector3(maxs.x(),maxs.y(),mins.z()), - t*btVector3(mins.x(),maxs.y(),mins.z()), - t*btVector3(mins.x(),mins.y(),maxs.z()), - t*btVector3(maxs.x(),mins.y(),maxs.z()), - t*btVector3(maxs.x(),maxs.y(),maxs.z()), - t*btVector3(mins.x(),maxs.y(),maxs.z())}; - aabbMin=aabbMax=crns[0]; - for(int i=1;i<8;++i) + /* t should be identity, but better be safe than...fast? */ + const btVector3 mins=m_body->m_bounds[0]; + const btVector3 maxs=m_body->m_bounds[1]; + const btVector3 crns[]={t*btVector3(mins.x(),mins.y(),mins.z()), + t*btVector3(maxs.x(),mins.y(),mins.z()), + t*btVector3(maxs.x(),maxs.y(),mins.z()), + t*btVector3(mins.x(),maxs.y(),mins.z()), + t*btVector3(mins.x(),mins.y(),maxs.z()), + t*btVector3(maxs.x(),mins.y(),maxs.z()), + t*btVector3(maxs.x(),maxs.y(),maxs.z()), + t*btVector3(mins.x(),maxs.y(),maxs.z())}; + aabbMin=aabbMax=crns[0]; + for(int i=1;i<8;++i) { - aabbMin.setMin(crns[i]); - aabbMax.setMax(crns[i]); + aabbMin.setMin(crns[i]); + aabbMax.setMax(crns[i]); } } - + virtual void setLocalScaling(const btVector3& /*scaling*/) { ///na } virtual const btVector3& getLocalScaling() const { - static const btVector3 dummy(1,1,1); - return dummy; + static const btVector3 dummy(1,1,1); + return dummy; } virtual void calculateLocalInertia(btScalar /*mass*/,btVector3& /*inertia*/) const { @@ -119,24 +119,24 @@ public: const btSoftBody::Cluster* m_cluster; btSoftClusterCollisionShape (const btSoftBody::Cluster* cluster) : m_cluster(cluster) { setMargin(0); } - - + + virtual btVector3 localGetSupportingVertex(const btVector3& vec) const - { + { btSoftBody::Node* const * n=&m_cluster->m_nodes[0]; btScalar d=dot(vec,n[0]->m_x); int j=0; for(int i=1,ni=m_cluster->m_nodes.size();im_x); if(k>d) { d=k;j=i; } - } + } return(n[j]->m_x); - } + } virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const - { + { return(localGetSupportingVertex(vec)); - } + } //notice that the vectors should be unit length virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const {} @@ -149,7 +149,7 @@ public: {} virtual int getShapeType() const { return SOFTBODY_SHAPE_PROXYTYPE; } - + //debugging virtual const char* getName()const {return "SOFTCLUSTER";} @@ -171,8 +171,8 @@ public: template static inline void ZeroInitialize(T& value) { -static const T zerodummy; -value=zerodummy; + static const T zerodummy; + value=zerodummy; } // template @@ -192,23 +192,23 @@ static inline T InvLerp(const T& a,const T& b,btScalar t) { return((b+a*t-b*t)/(a*b)); } // static inline btMatrix3x3 Lerp( const btMatrix3x3& a, - const btMatrix3x3& b, - btScalar t) + const btMatrix3x3& b, + btScalar t) { -btMatrix3x3 r; -r[0]=Lerp(a[0],b[0],t); -r[1]=Lerp(a[1],b[1],t); -r[2]=Lerp(a[2],b[2],t); -return(r); + btMatrix3x3 r; + r[0]=Lerp(a[0],b[0],t); + r[1]=Lerp(a[1],b[1],t); + r[2]=Lerp(a[2],b[2],t); + return(r); } // static inline btVector3 Clamp(const btVector3& v,btScalar maxlength) { -const btScalar sql=v.length2(); -if(sql>(maxlength*maxlength)) - return((v*maxlength)/btSqrt(sql)); + const btScalar sql=v.length2(); + if(sql>(maxlength*maxlength)) + return((v*maxlength)/btSqrt(sql)); else - return(v); + return(v); } // template @@ -233,8 +233,8 @@ static inline bool SameSign(const T& x,const T& y) // static inline btScalar ClusterMetric(const btVector3& x,const btVector3& y) { -const btVector3 d=x-y; -return(btFabs(d[0])+btFabs(d[1])+btFabs(d[2])); + const btVector3 d=x-y; + return(btFabs(d[0])+btFabs(d[1])+btFabs(d[2])); } // static inline btMatrix3x3 ScaleAlongAxis(const btVector3& a,btScalar s) @@ -271,7 +271,7 @@ static inline btMatrix3x3 Diagonal(btScalar x) } // static inline btMatrix3x3 Add(const btMatrix3x3& a, - const btMatrix3x3& b) + const btMatrix3x3& b) { btMatrix3x3 r; for(int i=0;i<3;++i) r[i]=a[i]+b[i]; @@ -279,7 +279,7 @@ static inline btMatrix3x3 Add(const btMatrix3x3& a, } // static inline btMatrix3x3 Sub(const btMatrix3x3& a, - const btMatrix3x3& b) + const btMatrix3x3& b) { btMatrix3x3 r; for(int i=0;i<3;++i) r[i]=a[i]-b[i]; @@ -287,7 +287,7 @@ static inline btMatrix3x3 Sub(const btMatrix3x3& a, } // static inline btMatrix3x3 Mul(const btMatrix3x3& a, - btScalar b) + btScalar b) { btMatrix3x3 r; for(int i=0;i<3;++i) r[i]=a[i]*b; @@ -296,9 +296,9 @@ static inline btMatrix3x3 Mul(const btMatrix3x3& a, // static inline void Orthogonalize(btMatrix3x3& m) { -m[2]=cross(m[0],m[1]).normalized(); -m[1]=cross(m[2],m[0]).normalized(); -m[0]=cross(m[1],m[2]).normalized(); + m[2]=cross(m[0],m[1]).normalized(); + m[1]=cross(m[2],m[0]).normalized(); + m[0]=cross(m[1],m[2]).normalized(); } // static inline btMatrix3x3 MassMatrix(btScalar im,const btMatrix3x3& iwi,const btVector3& r) @@ -309,90 +309,90 @@ static inline btMatrix3x3 MassMatrix(btScalar im,const btMatrix3x3& iwi,const bt // static inline btMatrix3x3 ImpulseMatrix( btScalar dt, - btScalar ima, - btScalar imb, - const btMatrix3x3& iwi, - const btVector3& r) + btScalar ima, + btScalar imb, + const btMatrix3x3& iwi, + const btVector3& r) { return(Diagonal(1/dt)*Add(Diagonal(ima),MassMatrix(imb,iwi,r)).inverse()); } // static inline btMatrix3x3 ImpulseMatrix( btScalar ima,const btMatrix3x3& iia,const btVector3& ra, - btScalar imb,const btMatrix3x3& iib,const btVector3& rb) + btScalar imb,const btMatrix3x3& iib,const btVector3& rb) { -return(Add(MassMatrix(ima,iia,ra),MassMatrix(imb,iib,rb)).inverse()); + return(Add(MassMatrix(ima,iia,ra),MassMatrix(imb,iib,rb)).inverse()); } // static inline btMatrix3x3 AngularImpulseMatrix( const btMatrix3x3& iia, - const btMatrix3x3& iib) + const btMatrix3x3& iib) { -return(Add(iia,iib).inverse()); + return(Add(iia,iib).inverse()); } // static inline btVector3 ProjectOnAxis( const btVector3& v, - const btVector3& a) + const btVector3& a) { return(a*dot(v,a)); } // static inline btVector3 ProjectOnPlane( const btVector3& v, - const btVector3& a) + const btVector3& a) { return(v-ProjectOnAxis(v,a)); } // static inline void ProjectOrigin( const btVector3& a, - const btVector3& b, - btVector3& prj, - btScalar& sqd) + const btVector3& b, + btVector3& prj, + btScalar& sqd) { -const btVector3 d=b-a; -const btScalar m2=d.length2(); -if(m2>SIMD_EPSILON) + const btVector3 d=b-a; + const btScalar m2=d.length2(); + if(m2>SIMD_EPSILON) { - const btScalar t=Clamp(-dot(a,d)/m2,0,1); - const btVector3 p=a+d*t; - const btScalar l2=p.length2(); - if(l2(-dot(a,d)/m2,0,1); + const btVector3 p=a+d*t; + const btScalar l2=p.length2(); + if(l2SIMD_EPSILON) + const btVector3& q=cross(b-a,c-a); + const btScalar m2=q.length2(); + if(m2>SIMD_EPSILON) { - const btVector3 n=q/btSqrt(m2); - const btScalar k=dot(a,n); - const btScalar k2=k*k; - if(k20)&& - (dot(cross(b-p,c-p),q)>0)&& - (dot(cross(c-p,a-p),q)>0)) + const btVector3 p=n*k; + if( (dot(cross(a-p,b-p),q)>0)&& + (dot(cross(b-p,c-p),q)>0)&& + (dot(cross(c-p,a-p),q)>0)) { - prj=p; - sqd=k2; + prj=p; + sqd=k2; } else { - ProjectOrigin(a,b,prj,sqd); - ProjectOrigin(b,c,prj,sqd); - ProjectOrigin(c,a,prj,sqd); + ProjectOrigin(a,b,prj,sqd); + ProjectOrigin(b,c,prj,sqd); + ProjectOrigin(c,a,prj,sqd); } } } @@ -401,53 +401,53 @@ if(m2>SIMD_EPSILON) // template static inline T BaryEval( const T& a, - const T& b, - const T& c, - const btVector3& coord) + const T& b, + const T& c, + const btVector3& coord) { return(a*coord.x()+b*coord.y()+c*coord.z()); } // static inline btVector3 BaryCoord( const btVector3& a, - const btVector3& b, - const btVector3& c, - const btVector3& p) + const btVector3& b, + const btVector3& c, + const btVector3& p) { -const btScalar w[]={ cross(a-p,b-p).length(), - cross(b-p,c-p).length(), - cross(c-p,a-p).length()}; -const btScalar isum=1/(w[0]+w[1]+w[2]); -return(btVector3(w[1]*isum,w[2]*isum,w[0]*isum)); + const btScalar w[]={ cross(a-p,b-p).length(), + cross(b-p,c-p).length(), + cross(c-p,a-p).length()}; + const btScalar isum=1/(w[0]+w[1]+w[2]); + return(btVector3(w[1]*isum,w[2]*isum,w[0]*isum)); } // static btScalar ImplicitSolve( btSoftBody::ImplicitFn* fn, - const btVector3& a, - const btVector3& b, - const btScalar accuracy, - const int maxiterations=256) + const btVector3& a, + const btVector3& b, + const btScalar accuracy, + const int maxiterations=256) { -btScalar span[2]={0,1}; -btScalar values[2]={fn->Eval(a),fn->Eval(b)}; -if(values[0]>values[1]) + btScalar span[2]={0,1}; + btScalar values[2]={fn->Eval(a),fn->Eval(b)}; + if(values[0]>values[1]) { - btSwap(span[0],span[1]); - btSwap(values[0],values[1]); + btSwap(span[0],span[1]); + btSwap(values[0],values[1]); } -if(values[0]>-accuracy) return(-1); -if(values[1]<+accuracy) return(-1); -for(int i=0;i-accuracy) return(-1); + if(values[1]<+accuracy) return(-1); + for(int i=0;iEval(Lerp(a,b,t)); - if((t<=0)||(t>=1)) break; - if(btFabs(v)Eval(Lerp(a,b,t)); + if((t<=0)||(t>=1)) break; + if(btFabs(v)m_x, - &f.m_n[1]->m_x, - &f.m_n[2]->m_x}; -btDbvtVolume vol=btDbvtVolume::FromPoints(pts,3); -vol.Expand(btVector3(margin,margin,margin)); -return(vol); + const btVector3* pts[]={ &f.m_n[0]->m_x, + &f.m_n[1]->m_x, + &f.m_n[2]->m_x}; + btDbvtVolume vol=btDbvtVolume::FromPoints(pts,3); + vol.Expand(btVector3(margin,margin,margin)); + return(vol); } // static inline btVector3 CenterOf( const btSoftBody::Face& f) { -return((f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3); + return((f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3); } // static inline btScalar AreaOf( const btVector3& x0, - const btVector3& x1, - const btVector3& x2) + const btVector3& x1, + const btVector3& x2) { const btVector3 a=x1-x0; const btVector3 b=x2-x0; @@ -492,9 +492,9 @@ static inline btScalar AreaOf( const btVector3& x0, // static inline btScalar VolumeOf( const btVector3& x0, - const btVector3& x1, - const btVector3& x2, - const btVector3& x3) + const btVector3& x1, + const btVector3& x2, + const btVector3& x3) { const btVector3 a=x1-x0; const btVector3 b=x2-x0; @@ -504,8 +504,8 @@ static inline btScalar VolumeOf( const btVector3& x0, // static void EvaluateMedium( const btSoftBodyWorldInfo* wfi, - const btVector3& x, - btSoftBody::sMedium& medium) + const btVector3& x, + btSoftBody::sMedium& medium) { medium.m_velocity = btVector3(0,0,0); medium.m_pressure = 0; @@ -523,8 +523,8 @@ static void EvaluateMedium( const btSoftBodyWorldInfo* wfi, // static inline void ApplyClampedForce( btSoftBody::Node& n, - const btVector3& f, - btScalar dt) + const btVector3& f, + btScalar dt) { const btScalar dtim=dt*n.m_im; if((f*dtim).length2()>n.m_v.length2()) @@ -539,13 +539,13 @@ static inline void ApplyClampedForce( btSoftBody::Node& n, // static inline int MatchEdge( const btSoftBody::Node* a, - const btSoftBody::Node* b, - const btSoftBody::Node* ma, - const btSoftBody::Node* mb) + const btSoftBody::Node* b, + const btSoftBody::Node* ma, + const btSoftBody::Node* mb) { -if((a==ma)&&(b==mb)) return(0); -if((a==mb)&&(b==ma)) return(1); -return(-1); + if((a==ma)&&(b==mb)) return(0); + if((a==mb)&&(b==ma)) return(1); + return(-1); } // @@ -555,56 +555,56 @@ return(-1); // struct btEigen { -static int system(btMatrix3x3& a,btMatrix3x3* vectors,btVector3* values=0) + static int system(btMatrix3x3& a,btMatrix3x3* vectors,btVector3* values=0) { - static const int maxiterations=16; - static const btScalar accuracy=(btScalar)0.0001; - btMatrix3x3& v=*vectors; - int iterations=0; - vectors->setIdentity(); - do { - int p=0,q=1; - if(btFabs(a[p][q])accuracy) + static const int maxiterations=16; + static const btScalar accuracy=(btScalar)0.0001; + btMatrix3x3& v=*vectors; + int iterations=0; + vectors->setIdentity(); + do { + int p=0,q=1; + if(btFabs(a[p][q])accuracy) { - const btScalar w=(a[q][q]-a[p][p])/(2*a[p][q]); - const btScalar z=btFabs(w); - const btScalar t=w/(z*(btSqrt(1+w*w)+z)); - if(t==t)/* [WARNING] let hope that one does not get thrown aways by some compilers... */ + const btScalar w=(a[q][q]-a[p][p])/(2*a[p][q]); + const btScalar z=btFabs(w); + const btScalar t=w/(z*(btSqrt(1+w*w)+z)); + if(t==t)/* [WARNING] let hope that one does not get thrown aways by some compilers... */ { - const btScalar c=1/btSqrt(t*t+1); - const btScalar s=c*t; - mulPQ(a,c,s,p,q); - mulTPQ(a,c,s,p,q); - mulPQ(v,c,s,p,q); + const btScalar c=1/btSqrt(t*t+1); + const btScalar s=c*t; + mulPQ(a,c,s,p,q); + mulTPQ(a,c,s,p,q); + mulPQ(v,c,s,p,q); } else break; } else break; } while((++iterations)data; - btSoftClusterCollisionShape cshape(cluster); - const btConvexShape* rshape=(const btConvexShape*)prb->getCollisionShape(); - btGjkEpaSolver2::sResults res; - if(btGjkEpaSolver2::SignedDistance( &cshape,btTransform::getIdentity(), - rshape,prb->getInterpolationWorldTransform(), - btVector3(1,0,0),res)) + btSoftBody::Cluster* cluster=(btSoftBody::Cluster*)leaf->data; + btSoftClusterCollisionShape cshape(cluster); + const btConvexShape* rshape=(const btConvexShape*)prb->getCollisionShape(); + btGjkEpaSolver2::sResults res; + if(btGjkEpaSolver2::SignedDistance( &cshape,btTransform::getIdentity(), + rshape,prb->getInterpolationWorldTransform(), + btVector3(1,0,0),res)) { - btSoftBody::CJoint joint; - if(SolveContact(res,cluster,prb,joint)) + btSoftBody::CJoint joint; + if(SolveContact(res,cluster,prb,joint)) { - btSoftBody::CJoint* pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint(); - *pj=joint;psb->m_joints.push_back(pj); - if(prb->isStaticOrKinematicObject()) + btSoftBody::CJoint* pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint(); + *pj=joint;psb->m_joints.push_back(pj); + if(prb->isStaticOrKinematicObject()) { - pj->m_erp *= psb->m_cfg.kSKHR_CL; - pj->m_split *= psb->m_cfg.kSK_SPLT_CL; + pj->m_erp *= psb->m_cfg.kSKHR_CL; + pj->m_split *= psb->m_cfg.kSK_SPLT_CL; } else { - pj->m_erp *= psb->m_cfg.kSRHR_CL; - pj->m_split *= psb->m_cfg.kSR_SPLT_CL; + pj->m_erp *= psb->m_cfg.kSRHR_CL; + pj->m_split *= psb->m_cfg.kSR_SPLT_CL; } } } } - void Process(btSoftBody* ps,btRigidBody* pr) + void Process(btSoftBody* ps,btRigidBody* pr) { - psb = ps; - prb = pr; - idt = ps->m_sst.isdt; - margin = ps->getCollisionShape()->getMargin()+ - pr->getCollisionShape()->getMargin(); - friction = btMin(psb->m_cfg.kDF,prb->getFriction()); - btVector3 mins; - btVector3 maxs; - - ATTRIBUTE_ALIGNED16(btDbvtVolume) volume; - pr->getCollisionShape()->getAabb(pr->getInterpolationWorldTransform(),mins,maxs); - volume=btDbvtVolume::FromMM(mins,maxs); - volume.Expand(btVector3(1,1,1)*margin); - btDbvt::collideTV(ps->m_cdbvt.m_root,volume,*this); + psb = ps; + prb = pr; + idt = ps->m_sst.isdt; + margin = ps->getCollisionShape()->getMargin()+ + pr->getCollisionShape()->getMargin(); + friction = btMin(psb->m_cfg.kDF,prb->getFriction()); + btVector3 mins; + btVector3 maxs; + + ATTRIBUTE_ALIGNED16(btDbvtVolume) volume; + pr->getCollisionShape()->getAabb(pr->getInterpolationWorldTransform(),mins,maxs); + volume=btDbvtVolume::FromMM(mins,maxs); + volume.Expand(btVector3(1,1,1)*margin); + btDbvt::collideTV(ps->m_cdbvt.m_root,volume,*this); } }; // @@ -760,36 +760,36 @@ struct btSoftColliders // struct CollideCL_SS : ClusterBase { - btSoftBody* bodies[2]; - void Process(const btDbvtNode* la,const btDbvtNode* lb) + btSoftBody* bodies[2]; + void Process(const btDbvtNode* la,const btDbvtNode* lb) { - btSoftBody::Cluster* cla=(btSoftBody::Cluster*)la->data; - btSoftBody::Cluster* clb=(btSoftBody::Cluster*)lb->data; - btSoftClusterCollisionShape csa(cla); - btSoftClusterCollisionShape csb(clb); - btGjkEpaSolver2::sResults res; - if(btGjkEpaSolver2::SignedDistance( &csa,btTransform::getIdentity(), - &csb,btTransform::getIdentity(), - cla->m_com-clb->m_com,res)) + btSoftBody::Cluster* cla=(btSoftBody::Cluster*)la->data; + btSoftBody::Cluster* clb=(btSoftBody::Cluster*)lb->data; + btSoftClusterCollisionShape csa(cla); + btSoftClusterCollisionShape csb(clb); + btGjkEpaSolver2::sResults res; + if(btGjkEpaSolver2::SignedDistance( &csa,btTransform::getIdentity(), + &csb,btTransform::getIdentity(), + cla->m_com-clb->m_com,res)) { - btSoftBody::CJoint joint; - if(SolveContact(res,cla,clb,joint)) + btSoftBody::CJoint joint; + if(SolveContact(res,cla,clb,joint)) { - btSoftBody::CJoint* pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint(); - *pj=joint;bodies[0]->m_joints.push_back(pj); - pj->m_erp *= btMax(bodies[0]->m_cfg.kSSHR_CL,bodies[1]->m_cfg.kSSHR_CL); - pj->m_split *= (bodies[0]->m_cfg.kSS_SPLT_CL+bodies[1]->m_cfg.kSS_SPLT_CL)/2; + btSoftBody::CJoint* pj=new(btAlignedAlloc(sizeof(btSoftBody::CJoint),16)) btSoftBody::CJoint(); + *pj=joint;bodies[0]->m_joints.push_back(pj); + pj->m_erp *= btMax(bodies[0]->m_cfg.kSSHR_CL,bodies[1]->m_cfg.kSSHR_CL); + pj->m_split *= (bodies[0]->m_cfg.kSS_SPLT_CL+bodies[1]->m_cfg.kSS_SPLT_CL)/2; } } } - void Process(btSoftBody* psa,btSoftBody* psb) + void Process(btSoftBody* psa,btSoftBody* psb) { - idt = psa->m_sst.isdt; - margin = (psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin())/2; - friction = btMin(psa->m_cfg.kDF,psb->m_cfg.kDF); - bodies[0] = psa; - bodies[1] = psb; - btDbvt::collideTT(psa->m_cdbvt.m_root,psb->m_cdbvt.m_root,*this); + idt = psa->m_sst.isdt; + margin = (psa->getCollisionShape()->getMargin()+psb->getCollisionShape()->getMargin())/2; + friction = btMin(psa->m_cfg.kDF,psb->m_cfg.kDF); + bodies[0] = psa; + bodies[1] = psb; + btDbvt::collideTT(psa->m_cdbvt.m_root,psb->m_cdbvt.m_root,*this); } }; // @@ -797,96 +797,96 @@ struct btSoftColliders // struct CollideSDF_RS : btDbvt::ICollide { - void Process(const btDbvtNode* leaf) + void Process(const btDbvtNode* leaf) { - btSoftBody::Node* node=(btSoftBody::Node*)leaf->data; - DoNode(*node); + btSoftBody::Node* node=(btSoftBody::Node*)leaf->data; + DoNode(*node); } - void DoNode(btSoftBody::Node& n) const + void DoNode(btSoftBody::Node& n) const { - const btScalar m=n.m_im>0?dynmargin:stamargin; - btSoftBody::RContact c; - if( (!n.m_battach)&& - psb->checkContact(prb,n.m_x,m,c.m_cti)) + const btScalar m=n.m_im>0?dynmargin:stamargin; + btSoftBody::RContact c; + if( (!n.m_battach)&& + psb->checkContact(prb,n.m_x,m,c.m_cti)) { - const btScalar ima=n.m_im; - const btScalar imb=prb->getInvMass(); - const btScalar ms=ima+imb; - if(ms>0) + const btScalar ima=n.m_im; + const btScalar imb=prb->getInvMass(); + const btScalar ms=ima+imb; + if(ms>0) { - const btTransform& wtr=prb->getInterpolationWorldTransform(); - const btMatrix3x3& iwi=prb->getInvInertiaTensorWorld(); - const btVector3 ra=n.m_x-wtr.getOrigin(); - const btVector3 va=prb->getVelocityInLocalPoint(ra)*psb->m_sst.sdt; - const btVector3 vb=n.m_x-n.m_q; - const btVector3 vr=vb-va; - const btScalar dn=dot(vr,c.m_cti.m_normal); - const btVector3 fv=vr-c.m_cti.m_normal*dn; - const btScalar fc=psb->m_cfg.kDF*prb->getFriction(); - c.m_node = &n; - c.m_c0 = ImpulseMatrix(psb->m_sst.sdt,ima,imb,iwi,ra); - c.m_c1 = ra; - c.m_c2 = ima*psb->m_sst.sdt; - c.m_c3 = fv.length2()<(btFabs(dn)*fc)?0:1-fc; - c.m_c4 = prb->isStaticOrKinematicObject()?psb->m_cfg.kKHR:psb->m_cfg.kCHR; - psb->m_rcontacts.push_back(c); - prb->activate(); + const btTransform& wtr=prb->getInterpolationWorldTransform(); + const btMatrix3x3& iwi=prb->getInvInertiaTensorWorld(); + const btVector3 ra=n.m_x-wtr.getOrigin(); + const btVector3 va=prb->getVelocityInLocalPoint(ra)*psb->m_sst.sdt; + const btVector3 vb=n.m_x-n.m_q; + const btVector3 vr=vb-va; + const btScalar dn=dot(vr,c.m_cti.m_normal); + const btVector3 fv=vr-c.m_cti.m_normal*dn; + const btScalar fc=psb->m_cfg.kDF*prb->getFriction(); + c.m_node = &n; + c.m_c0 = ImpulseMatrix(psb->m_sst.sdt,ima,imb,iwi,ra); + c.m_c1 = ra; + c.m_c2 = ima*psb->m_sst.sdt; + c.m_c3 = fv.length2()<(btFabs(dn)*fc)?0:1-fc; + c.m_c4 = prb->isStaticOrKinematicObject()?psb->m_cfg.kKHR:psb->m_cfg.kCHR; + psb->m_rcontacts.push_back(c); + prb->activate(); } } } - btSoftBody* psb; - btRigidBody* prb; - btScalar dynmargin; - btScalar stamargin; + btSoftBody* psb; + btRigidBody* prb; + btScalar dynmargin; + btScalar stamargin; }; // // CollideVF_SS // struct CollideVF_SS : btDbvt::ICollide { - void Process(const btDbvtNode* lnode, - const btDbvtNode* lface) + void Process(const btDbvtNode* lnode, + const btDbvtNode* lface) { - btSoftBody::Node* node=(btSoftBody::Node*)lnode->data; - btSoftBody::Face* face=(btSoftBody::Face*)lface->data; - btVector3 o=node->m_x; - btVector3 p; - btScalar d=SIMD_INFINITY; - ProjectOrigin( face->m_n[0]->m_x-o, - face->m_n[1]->m_x-o, - face->m_n[2]->m_x-o, - p,d); - const btScalar m=mrg+(o-node->m_q).length()*2; - if(d<(m*m)) + btSoftBody::Node* node=(btSoftBody::Node*)lnode->data; + btSoftBody::Face* face=(btSoftBody::Face*)lface->data; + btVector3 o=node->m_x; + btVector3 p; + btScalar d=SIMD_INFINITY; + ProjectOrigin( face->m_n[0]->m_x-o, + face->m_n[1]->m_x-o, + face->m_n[2]->m_x-o, + p,d); + const btScalar m=mrg+(o-node->m_q).length()*2; + if(d<(m*m)) { - const btSoftBody::Node* n[]={face->m_n[0],face->m_n[1],face->m_n[2]}; - const btVector3 w=BaryCoord(n[0]->m_x,n[1]->m_x,n[2]->m_x,p+o); - const btScalar ma=node->m_im; - btScalar mb=BaryEval(n[0]->m_im,n[1]->m_im,n[2]->m_im,w); - if( (n[0]->m_im<=0)|| - (n[1]->m_im<=0)|| - (n[2]->m_im<=0)) + const btSoftBody::Node* n[]={face->m_n[0],face->m_n[1],face->m_n[2]}; + const btVector3 w=BaryCoord(n[0]->m_x,n[1]->m_x,n[2]->m_x,p+o); + const btScalar ma=node->m_im; + btScalar mb=BaryEval(n[0]->m_im,n[1]->m_im,n[2]->m_im,w); + if( (n[0]->m_im<=0)|| + (n[1]->m_im<=0)|| + (n[2]->m_im<=0)) { - mb=0; + mb=0; } - const btScalar ms=ma+mb; - if(ms>0) + const btScalar ms=ma+mb; + if(ms>0) { - btSoftBody::SContact c; - c.m_normal = p/-btSqrt(d); - c.m_margin = m; - c.m_node = node; - c.m_face = face; - c.m_weights = w; - c.m_friction = btMax(psb[0]->m_cfg.kDF,psb[1]->m_cfg.kDF); - c.m_cfm[0] = ma/ms*psb[0]->m_cfg.kSHR; - c.m_cfm[1] = mb/ms*psb[1]->m_cfg.kSHR; - psb[0]->m_scontacts.push_back(c); + btSoftBody::SContact c; + c.m_normal = p/-btSqrt(d); + c.m_margin = m; + c.m_node = node; + c.m_face = face; + c.m_weights = w; + c.m_friction = btMax(psb[0]->m_cfg.kDF,psb[1]->m_cfg.kDF); + c.m_cfm[0] = ma/ms*psb[0]->m_cfg.kSHR; + c.m_cfm[1] = mb/ms*psb[1]->m_cfg.kSHR; + psb[0]->m_scontacts.push_back(c); } } } - btSoftBody* psb[2]; - btScalar mrg; + btSoftBody* psb[2]; + btScalar mrg; }; }; diff --git a/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp b/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp index da72f0957..bca968a58 100644 --- a/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp +++ b/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.cpp @@ -29,10 +29,10 @@ btSoftBodyRigidBodyCollisionConfiguration::btSoftBodyRigidBodyCollisionConfigura mem = btAlignedAlloc(sizeof(btSoftSoftCollisionAlgorithm::CreateFunc),16); m_softSoftCreateFunc = new(mem) btSoftSoftCollisionAlgorithm::CreateFunc; - + mem = btAlignedAlloc(sizeof(btSoftRigidCollisionAlgorithm::CreateFunc),16); m_softRigidConvexCreateFunc = new(mem) btSoftRigidCollisionAlgorithm::CreateFunc; - + mem = btAlignedAlloc(sizeof(btSoftRigidCollisionAlgorithm::CreateFunc),16); m_swappedSoftRigidConvexCreateFunc = new(mem) btSoftRigidCollisionAlgorithm::CreateFunc; m_swappedSoftRigidConvexCreateFunc->m_swapped=true; @@ -40,20 +40,20 @@ btSoftBodyRigidBodyCollisionConfiguration::btSoftBodyRigidBodyCollisionConfigura #ifdef ENABLE_SOFTBODY_CONCAVE_COLLISIONS mem = btAlignedAlloc(sizeof(btSoftBodyConcaveCollisionAlgorithm::CreateFunc),16); m_softRigidConcaveCreateFunc = new(mem) btSoftBodyConcaveCollisionAlgorithm::CreateFunc; - + mem = btAlignedAlloc(sizeof(btSoftBodyConcaveCollisionAlgorithm::CreateFunc),16); m_swappedSoftRigidConcaveCreateFunc = new(mem) btSoftBodyConcaveCollisionAlgorithm::SwappedCreateFunc; m_swappedSoftRigidConcaveCreateFunc->m_swapped=true; #endif //replace pool by a new one, with potential larger size - + if (m_ownsCollisionAlgorithmPool && m_collisionAlgorithmPool) { int curElemSize = m_collisionAlgorithmPool->getElementSize(); ///calculate maximum element size, big enough to fit any collision algorithm in the memory pool - - + + int maxSize0 = sizeof(btSoftSoftCollisionAlgorithm); int maxSize1 = sizeof(btSoftRigidCollisionAlgorithm); int maxSize2 = sizeof(btSoftBodyConcaveCollisionAlgorithm); @@ -92,7 +92,7 @@ btSoftBodyRigidBodyCollisionConfiguration::~btSoftBodyRigidBodyCollisionConfigur btAlignedFree( m_swappedSoftRigidConcaveCreateFunc); #endif } - + ///creation of soft-soft and soft-rigid, and otherwise fallback to base class implementation btCollisionAlgorithmCreateFunc* btSoftBodyRigidBodyCollisionConfiguration::getCollisionAlgorithmCreateFunc(int proxyType0,int proxyType1) { diff --git a/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h b/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h index b098604ed..17a8296d5 100644 --- a/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h +++ b/src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h @@ -32,7 +32,7 @@ class btSoftBodyRigidBodyCollisionConfiguration : public btDefaultCollisionConfi btCollisionAlgorithmCreateFunc* m_swappedSoftRigidConvexCreateFunc; btCollisionAlgorithmCreateFunc* m_softRigidConcaveCreateFunc; btCollisionAlgorithmCreateFunc* m_swappedSoftRigidConcaveCreateFunc; - + public: btSoftBodyRigidBodyCollisionConfiguration(const btDefaultCollisionConstructionInfo& constructionInfo = btDefaultCollisionConstructionInfo()); diff --git a/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp b/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp index f6279fc0e..9a3c9b2e0 100644 --- a/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp +++ b/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp @@ -35,13 +35,13 @@ m_isSwapped(isSwapped) btSoftRigidCollisionAlgorithm::~btSoftRigidCollisionAlgorithm() { - + //m_softBody->m_overlappingRigidBodies.remove(m_rigidCollisionObject); /*if (m_ownManifold) { - if (m_manifoldPtr) - m_dispatcher->releaseManifold(m_manifoldPtr); + if (m_manifoldPtr) + m_dispatcher->releaseManifold(m_manifoldPtr); } */ @@ -58,7 +58,7 @@ void btSoftRigidCollisionAlgorithm::processCollision (btCollisionObject* body0,b btSoftBody* softBody = m_isSwapped? (btSoftBody*)body1 : (btSoftBody*)body0; btCollisionObject* rigidCollisionObject = m_isSwapped? body0 : body1; - + softBody->defaultCollisionHandler(rigidCollisionObject); diff --git a/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.h b/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.h index 502e52dc6..e89570af3 100644 --- a/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.h +++ b/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.h @@ -28,15 +28,15 @@ class btSoftBody; /// btSoftRigidCollisionAlgorithm provides collision detection between btSoftBody and btRigidBody class btSoftRigidCollisionAlgorithm : public btCollisionAlgorithm { -// bool m_ownManifold; -// btPersistentManifold* m_manifoldPtr; + // bool m_ownManifold; + // btPersistentManifold* m_manifoldPtr; btSoftBody* m_softBody; btCollisionObject* m_rigidCollisionObject; ///for rigid versus soft (instead of soft versus rigid), we use this swapped boolean bool m_isSwapped; - + public: btSoftRigidCollisionAlgorithm(btPersistentManifold* mf,const btCollisionAlgorithmConstructionInfo& ci,btCollisionObject* col0,btCollisionObject* col1, bool isSwapped); @@ -52,7 +52,7 @@ public: //we don't add any manifolds } - + struct CreateFunc :public btCollisionAlgorithmCreateFunc { virtual btCollisionAlgorithm* CreateCollisionAlgorithm(btCollisionAlgorithmConstructionInfo& ci, btCollisionObject* body0,btCollisionObject* body1) diff --git a/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp b/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp index 0bda96eda..e8aaad036 100644 --- a/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp +++ b/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp @@ -28,17 +28,17 @@ subject to the following restrictions: btSoftRigidDynamicsWorld::btSoftRigidDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration) :btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration) { -m_drawFlags = fDrawFlags::Std; -m_drawNodeTree = true; -m_drawFaceTree = false; -m_drawClusterTree = false; -m_sbi.m_broadphase = pairCache; -m_sbi.m_dispatcher = dispatcher; -m_sbi.m_sparsesdf.Initialize(); -m_sbi.m_sparsesdf.Reset(); + m_drawFlags = fDrawFlags::Std; + m_drawNodeTree = true; + m_drawFaceTree = false; + m_drawClusterTree = false; + m_sbi.m_broadphase = pairCache; + m_sbi.m_dispatcher = dispatcher; + m_sbi.m_sparsesdf.Initialize(); + m_sbi.m_sparsesdf.Reset(); } - + btSoftRigidDynamicsWorld::~btSoftRigidDynamicsWorld() { @@ -55,7 +55,7 @@ void btSoftRigidDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) psb->predictMotion(timeStep); } } - + void btSoftRigidDynamicsWorld::internalSingleStepSimulation( btScalar timeStep) { btDiscreteDynamicsWorld::internalSingleStepSimulation( timeStep ); @@ -71,7 +71,7 @@ void btSoftRigidDynamicsWorld::internalSingleStepSimulation( btScalar timeStep) void btSoftRigidDynamicsWorld::updateSoftBodies() { BT_PROFILE("updateSoftBodies"); - + for ( int i=0;i btSoftBodyArray; class btSoftRigidDynamicsWorld : public btDiscreteDynamicsWorld { - + btSoftBodyArray m_softBodies; int m_drawFlags; bool m_drawNodeTree; @@ -32,9 +32,9 @@ class btSoftRigidDynamicsWorld : public btDiscreteDynamicsWorld btSoftBodyWorldInfo m_sbi; protected: - + virtual void predictUnconstraintMotion(btScalar timeStep); - + virtual void internalSingleStepSimulation( btScalar timeStep); void updateSoftBodies(); @@ -43,17 +43,17 @@ protected: public: - + btSoftRigidDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration); virtual ~btSoftRigidDynamicsWorld(); - + virtual void debugDrawWorld(); - + void addSoftBody(btSoftBody* body); void removeSoftBody(btSoftBody* body); - + int getDrawFlags() const { return(m_drawFlags); } void setDrawFlags(int f) { m_drawFlags=f; } @@ -66,7 +66,7 @@ public: return m_sbi; } - + btSoftBodyArray& getSoftBodyArray() { return m_softBodies; @@ -76,7 +76,7 @@ public: { return m_softBodies; } - + }; #endif //BT_SOFT_RIGID_DYNAMICS_WORLD_H diff --git a/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.h b/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.h index c60843c35..415c4b42a 100644 --- a/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.h +++ b/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.h @@ -33,7 +33,7 @@ class btSoftSoftCollisionAlgorithm : public btCollisionAlgorithm btSoftBody* m_softBody0; btSoftBody* m_softBody1; - + public: btSoftSoftCollisionAlgorithm(const btCollisionAlgorithmConstructionInfo& ci) : btCollisionAlgorithm(ci) {} diff --git a/src/BulletSoftBody/btSparseSDF.h b/src/BulletSoftBody/btSparseSDF.h index f02462504..77d4d7f4d 100644 --- a/src/BulletSoftBody/btSparseSDF.h +++ b/src/BulletSoftBody/btSparseSDF.h @@ -23,139 +23,139 @@ subject to the following restrictions: // Modified Paul Hsieh hash template unsigned int HsiehHash(const void* pdata) - { +{ const unsigned short* data=(const unsigned short*)pdata; unsigned hash=DWORDLEN<<2,tmp; for(int i=0;i>11; - } + } hash^=hash<<3;hash+=hash>>5; hash^=hash<<4;hash+=hash>>17; hash^=hash<<25;hash+=hash>>6; return(hash); - } +} template struct btSparseSdf - { +{ // // Inner types // struct IntFrac - { + { int b; int i; btScalar f; - }; + }; struct Cell - { + { btScalar d[CELLSIZE+1][CELLSIZE+1][CELLSIZE+1]; int c[3]; int puid; unsigned hash; btCollisionShape* pclient; Cell* next; - }; + }; // // Fields // - + btAlignedObjectArray cells; btScalar voxelsz; int puid; int ncells; int nprobes; int nqueries; - + // // Methods // - + // void Initialize(int hashsize=2383) - { + { cells.resize(hashsize,0); Reset(); - } + } // void Reset() - { + { for(int i=0,ni=cells.size();inext; delete pc; pc=pn; - } } + } voxelsz =0.25; puid =0; ncells =0; nprobes =1; nqueries =1; - } + } // void GarbageCollect(int lifetime=256) - { + { const int life=puid-lifetime; for(int i=0;inext; if(pc->puidnext=pn; else root=pn; delete pc;pc=pp;--ncells; - } - pp=pc;pc=pn; } + pp=pc;pc=pn; } + } //printf("GC[%d]: %d cells, PpQ: %f\r\n",puid,ncells,nprobes/(btScalar)nqueries); nqueries=1; nprobes=1; ++puid; /* TODO: Reset puid's when int range limit is reached */ - /* else setup a priority list... */ - } + /* else setup a priority list... */ + } // int RemoveReferences(btCollisionShape* pcs) - { + { int refcount=0; for(int i=0;inext; if(pc->pclient==pcs) - { + { if(pp) pp->next=pn; else root=pn; delete pc;pc=pp;++refcount; - } - pp=pc;pc=pn; } + pp=pc;pc=pn; } - return(refcount); } + return(refcount); + } // btScalar Evaluate( const btVector3& x, - btCollisionShape* shape, - btVector3& normal, - btScalar margin) - { + btCollisionShape* shape, + btVector3& normal, + btScalar margin) + { /* Lookup cell */ const btVector3 scx=x/voxelsz; const IntFrac ix=Decompose(scx.x()); @@ -166,19 +166,19 @@ struct btSparseSdf Cell* c=root; ++nqueries; while(c) - { + { ++nprobes; if( (c->hash==h) && (c->c[0]==ix.b) && (c->c[1]==iy.b) && (c->c[2]==iz.b) && (c->pclient==shape)) - { break; } - else - { c=c->next; } - } + { break; } + else + { c=c->next; } + } if(!c) - { + { ++nprobes; ++ncells; c=new Cell(); @@ -187,82 +187,82 @@ struct btSparseSdf c->hash=h; c->c[0]=ix.b;c->c[1]=iy.b;c->c[2]=iz.b; BuildCell(*c); - } + } c->puid=puid; /* Extract infos */ const int o[]={ ix.i,iy.i,iz.i}; const btScalar d[]={ c->d[o[0]+0][o[1]+0][o[2]+0], - c->d[o[0]+1][o[1]+0][o[2]+0], - c->d[o[0]+1][o[1]+1][o[2]+0], - c->d[o[0]+0][o[1]+1][o[2]+0], - c->d[o[0]+0][o[1]+0][o[2]+1], - c->d[o[0]+1][o[1]+0][o[2]+1], - c->d[o[0]+1][o[1]+1][o[2]+1], - c->d[o[0]+0][o[1]+1][o[2]+1]}; + c->d[o[0]+1][o[1]+0][o[2]+0], + c->d[o[0]+1][o[1]+1][o[2]+0], + c->d[o[0]+0][o[1]+1][o[2]+0], + c->d[o[0]+0][o[1]+0][o[2]+1], + c->d[o[0]+1][o[1]+0][o[2]+1], + c->d[o[0]+1][o[1]+1][o[2]+1], + c->d[o[0]+0][o[1]+1][o[2]+1]}; /* Normal */ - #if 1 +#if 1 const btScalar gx[]={ d[1]-d[0],d[2]-d[3], - d[5]-d[4],d[6]-d[7]}; + d[5]-d[4],d[6]-d[7]}; const btScalar gy[]={ d[3]-d[0],d[2]-d[1], - d[7]-d[4],d[6]-d[5]}; + d[7]-d[4],d[6]-d[5]}; const btScalar gz[]={ d[4]-d[0],d[5]-d[1], - d[7]-d[3],d[6]-d[2]}; + d[7]-d[3],d[6]-d[2]}; normal.setX(Lerp( Lerp(gx[0],gx[1],iy.f), - Lerp(gx[2],gx[3],iy.f),iz.f)); + Lerp(gx[2],gx[3],iy.f),iz.f)); normal.setY(Lerp( Lerp(gy[0],gy[1],ix.f), - Lerp(gy[2],gy[3],ix.f),iz.f)); + Lerp(gy[2],gy[3],ix.f),iz.f)); normal.setZ(Lerp( Lerp(gz[0],gz[1],ix.f), - Lerp(gz[2],gz[3],ix.f),iy.f)); + Lerp(gz[2],gz[3],ix.f),iy.f)); normal = normal.normalized(); - #else +#else normal = btVector3(d[1]-d[0],d[3]-d[0],d[4]-d[0]).normalized(); - #endif +#endif /* Distance */ const btScalar d0=Lerp(Lerp(d[0],d[1],ix.f), - Lerp(d[3],d[2],ix.f),iy.f); + Lerp(d[3],d[2],ix.f),iy.f); const btScalar d1=Lerp(Lerp(d[4],d[5],ix.f), - Lerp(d[7],d[6],ix.f),iy.f); + Lerp(d[7],d[6],ix.f),iy.f); return(Lerp(d0,d1,iz.f)-margin); - } + } // void BuildCell(Cell& c) - { + { const btVector3 org=btVector3( (btScalar)c.c[0], - (btScalar)c.c[1], - (btScalar)c.c[2]) * - CELLSIZE*voxelsz; + (btScalar)c.c[1], + (btScalar)c.c[2]) * + CELLSIZE*voxelsz; for(int k=0;k<=CELLSIZE;++k) - { + { const btScalar z=voxelsz*k+org.z(); for(int j=0;j<=CELLSIZE;++j) - { + { const btScalar y=voxelsz*j+org.y(); for(int i=0;i<=CELLSIZE;++i) - { + { const btScalar x=voxelsz*i+org.x(); c.d[i][j][k]=DistanceToShape( btVector3(x,y,z), - c.pclient); - } + c.pclient); } } } + } // static inline btScalar DistanceToShape(const btVector3& x, - btCollisionShape* shape) - { + btCollisionShape* shape) + { btTransform unit; unit.setIdentity(); if(shape->isConvex()) - { + { btGjkEpaSolver2::sResults res; btConvexShape* csh=static_cast(shape); return(btGjkEpaSolver2::SignedDistance(x,0,csh,unit,res)); - } - return(0); } + return(0); + } // static inline IntFrac Decompose(btScalar x) - { + { /* That one need a lot of improvements... */ /* Remove test, faster floor... */ IntFrac r; @@ -272,18 +272,18 @@ struct btSparseSdf const btScalar k=(x-r.b)*CELLSIZE; r.i=(int)k;r.f=k-r.i;r.b-=o; return(r); - } + } // static inline btScalar Lerp(btScalar a,btScalar b,btScalar t) - { + { return(a+(b-a)*t); - } + } + - // static inline unsigned int Hash(int x,int y,int z,btCollisionShape* shape) - { + { struct btS { int x,y,z; @@ -291,16 +291,16 @@ struct btSparseSdf }; btS myset; - + myset.x=x;myset.y=y;myset.z=z;myset.p=shape; const void* ptr = &myset; unsigned int result = HsiehHash (ptr); - + return result; - } + } }; - + #endif