Merge branch 'master' of https://github.com/erwincoumans/bullet3
This commit is contained in:
Erwin Coumans
@@ -48,9 +48,9 @@ public:
|
||||
arraySizeZ(10),
|
||||
#else
|
||||
|
||||
arraySizeX(30),
|
||||
arraySizeY(30),
|
||||
arraySizeZ(30),
|
||||
arraySizeX(1),
|
||||
arraySizeY(10),
|
||||
arraySizeZ(1),
|
||||
#endif
|
||||
m_useConcaveMesh(false),
|
||||
gapX(16.3),
|
||||
|
||||
@@ -101,7 +101,7 @@ enum
|
||||
};
|
||||
|
||||
b3AlignedObjectArray<const char*> demoNames;
|
||||
int selectedDemo = 1;
|
||||
int selectedDemo = 0;
|
||||
GpuDemo::CreateFunc* allDemos[]=
|
||||
{
|
||||
//ConcaveCompound2Scene::MyCreateFunc,
|
||||
@@ -247,9 +247,21 @@ static void MyMouseButtonCallback(int button, int state, float x, float y)
|
||||
}
|
||||
|
||||
extern bool useShadowMap;
|
||||
|
||||
static bool wireframe=false;
|
||||
void MyKeyboardCallback(int key, int state)
|
||||
{
|
||||
if (key=='w' && state)
|
||||
{
|
||||
wireframe=!wireframe;
|
||||
if (wireframe)
|
||||
{
|
||||
glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
|
||||
} else
|
||||
{
|
||||
glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
|
||||
}
|
||||
}
|
||||
|
||||
if (key=='s' && state)
|
||||
{
|
||||
useShadowMap=!useShadowMap;
|
||||
|
||||
@@ -64,11 +64,23 @@ public:
|
||||
|
||||
class BasicDemo : public Bullet2RigidBodyDemo
|
||||
{
|
||||
SimpleOpenGL3App* m_glApp;
|
||||
|
||||
btRigidBody* m_pickedBody;
|
||||
btTypedConstraint* m_pickedConstraint;
|
||||
btVector3 m_oldPickingPos;
|
||||
btVector3 m_hitPos;
|
||||
btScalar m_oldPickingDist;
|
||||
|
||||
|
||||
|
||||
public:
|
||||
SimpleOpenGL3App* m_glApp;
|
||||
|
||||
|
||||
BasicDemo(SimpleOpenGL3App* app)
|
||||
:m_glApp(app)
|
||||
:m_glApp(app),
|
||||
m_pickedBody(0),
|
||||
m_pickedConstraint(0)
|
||||
{
|
||||
}
|
||||
virtual ~BasicDemo()
|
||||
@@ -141,7 +153,7 @@ public:
|
||||
curColor&=3;
|
||||
startTransform.setOrigin(btVector3(
|
||||
btScalar(2.0*i),
|
||||
btScalar(1+2.0*k),
|
||||
btScalar(20+2.0*k),
|
||||
btScalar(2.0*j)));
|
||||
|
||||
m_glApp->m_instancingRenderer->registerGraphicsInstance(cubeShapeId,startTransform.getOrigin(),startTransform.getRotation(),color,halfExtents);
|
||||
@@ -179,12 +191,215 @@ public:
|
||||
|
||||
m_glApp->m_instancingRenderer->renderScene();
|
||||
}
|
||||
|
||||
btVector3 getRayTo(int x,int y)
|
||||
{
|
||||
if (!m_glApp->m_instancingRenderer)
|
||||
{
|
||||
btAssert(0);
|
||||
return btVector3(0,0,0);
|
||||
}
|
||||
|
||||
float top = 1.f;
|
||||
float bottom = -1.f;
|
||||
float nearPlane = 1.f;
|
||||
float tanFov = (top-bottom)*0.5f / nearPlane;
|
||||
float fov = b3Scalar(2.0) * b3Atan(tanFov);
|
||||
|
||||
btVector3 camPos,camTarget;
|
||||
m_glApp->m_instancingRenderer->getCameraPosition(camPos);
|
||||
m_glApp->m_instancingRenderer->getCameraTargetPosition(camTarget);
|
||||
|
||||
btVector3 rayFrom = camPos;
|
||||
btVector3 rayForward = (camTarget-camPos);
|
||||
rayForward.normalize();
|
||||
float farPlane = 10000.f;
|
||||
rayForward*= farPlane;
|
||||
|
||||
btVector3 rightOffset;
|
||||
btVector3 m_cameraUp=btVector3(0,1,0);
|
||||
btVector3 vertical = m_cameraUp;
|
||||
|
||||
btVector3 hor;
|
||||
hor = rayForward.cross(vertical);
|
||||
hor.normalize();
|
||||
vertical = hor.cross(rayForward);
|
||||
vertical.normalize();
|
||||
|
||||
float tanfov = tanf(0.5f*fov);
|
||||
|
||||
|
||||
hor *= 2.f * farPlane * tanfov;
|
||||
vertical *= 2.f * farPlane * tanfov;
|
||||
|
||||
b3Scalar aspect;
|
||||
float width = m_glApp->m_instancingRenderer->getScreenWidth();
|
||||
float height = m_glApp->m_instancingRenderer->getScreenHeight();
|
||||
|
||||
aspect = width / height;
|
||||
|
||||
hor*=aspect;
|
||||
|
||||
|
||||
btVector3 rayToCenter = rayFrom + rayForward;
|
||||
btVector3 dHor = hor * 1.f/width;
|
||||
btVector3 dVert = vertical * 1.f/height;
|
||||
|
||||
|
||||
btVector3 rayTo = rayToCenter - 0.5f * hor + 0.5f * vertical;
|
||||
rayTo += btScalar(x) * dHor;
|
||||
rayTo -= btScalar(y) * dVert;
|
||||
return rayTo;
|
||||
}
|
||||
|
||||
|
||||
bool mouseMoveCallback(float x,float y)
|
||||
{
|
||||
// if (m_data->m_altPressed!=0 || m_data->m_controlPressed!=0)
|
||||
// return false;
|
||||
|
||||
if (m_pickedBody && m_pickedConstraint)
|
||||
{
|
||||
btPoint2PointConstraint* pickCon = static_cast<btPoint2PointConstraint*>(m_pickedConstraint);
|
||||
if (pickCon)
|
||||
{
|
||||
//keep it at the same picking distance
|
||||
btVector3 newRayTo = getRayTo(x,y);
|
||||
btVector3 rayFrom;
|
||||
btVector3 oldPivotInB = pickCon->getPivotInB();
|
||||
btVector3 newPivotB;
|
||||
m_glApp->m_instancingRenderer->getCameraPosition(rayFrom);
|
||||
btVector3 dir = newRayTo-rayFrom;
|
||||
dir.normalize();
|
||||
dir *= m_oldPickingDist;
|
||||
|
||||
newPivotB = rayFrom + dir;
|
||||
pickCon->setPivotB(newPivotB);
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
bool mouseButtonCallback(int button, int state, float x, float y)
|
||||
{
|
||||
|
||||
if (state==1)
|
||||
{
|
||||
if(button==0)// && (m_data->m_altPressed==0 && m_data->m_controlPressed==0))
|
||||
{
|
||||
btVector3 camPos;
|
||||
m_glApp->m_instancingRenderer->getCameraPosition(camPos);
|
||||
|
||||
btVector3 rayFrom = camPos;
|
||||
btVector3 rayTo = getRayTo(x,y);
|
||||
|
||||
btCollisionWorld::ClosestRayResultCallback rayCallback(rayFrom,rayTo);
|
||||
m_dynamicsWorld->rayTest(rayFrom,rayTo,rayCallback);
|
||||
if (rayCallback.hasHit())
|
||||
{
|
||||
|
||||
btVector3 pickPos = rayCallback.m_hitPointWorld;
|
||||
btRigidBody* body = (btRigidBody*)btRigidBody::upcast(rayCallback.m_collisionObject);
|
||||
if (body)
|
||||
{
|
||||
//other exclusions?
|
||||
if (!(body->isStaticObject() || body->isKinematicObject()))
|
||||
{
|
||||
m_pickedBody = body;
|
||||
m_pickedBody->setActivationState(DISABLE_DEACTIVATION);
|
||||
//printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ());
|
||||
btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos;
|
||||
btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body,localPivot);
|
||||
m_dynamicsWorld->addConstraint(p2p,true);
|
||||
m_pickedConstraint = p2p;
|
||||
btScalar mousePickClamping = 30.f;
|
||||
p2p->m_setting.m_impulseClamp = mousePickClamping;
|
||||
//very weak constraint for picking
|
||||
p2p->m_setting.m_tau = 0.001f;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// pickObject(pickPos, rayCallback.m_collisionObject);
|
||||
m_oldPickingPos = rayTo;
|
||||
m_hitPos = pickPos;
|
||||
m_oldPickingDist = (pickPos-rayFrom).length();
|
||||
// printf("hit !\n");
|
||||
//add p2p
|
||||
}
|
||||
|
||||
}
|
||||
} else
|
||||
{
|
||||
if (button==0)
|
||||
{
|
||||
if (m_pickedConstraint)
|
||||
{
|
||||
m_dynamicsWorld->removeConstraint(m_pickedConstraint);
|
||||
delete m_pickedConstraint;
|
||||
m_pickedConstraint=0;
|
||||
m_pickedBody = 0;
|
||||
}
|
||||
//remove p2p
|
||||
}
|
||||
}
|
||||
|
||||
//printf("button=%d, state=%d\n",button,state);
|
||||
return false;
|
||||
}
|
||||
|
||||
void stepSimulation()
|
||||
{
|
||||
m_dynamicsWorld->stepSimulation(1./60,0);
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
|
||||
BasicDemo* sDemo = 0;
|
||||
|
||||
static void MyMouseMoveCallback( float x, float y)
|
||||
{
|
||||
bool handled = false;
|
||||
if (sDemo)
|
||||
handled = sDemo->mouseMoveCallback(x,y);
|
||||
if (!handled)
|
||||
b3DefaultMouseMoveCallback(x,y);
|
||||
}
|
||||
static void MyMouseButtonCallback(int button, int state, float x, float y)
|
||||
{
|
||||
bool handled = false;
|
||||
//try picking first
|
||||
if (sDemo)
|
||||
handled = sDemo->mouseButtonCallback(button,state,x,y);
|
||||
|
||||
if (!handled)
|
||||
b3DefaultMouseButtonCallback(button,state,x,y);
|
||||
}
|
||||
|
||||
void MyKeyboardCallback(int key, int state)
|
||||
{
|
||||
|
||||
if (key==B3G_ESCAPE && sDemo->m_glApp->m_window)
|
||||
{
|
||||
sDemo->m_glApp->m_window->setRequestExit();
|
||||
}
|
||||
if (key=='w')
|
||||
{
|
||||
glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
|
||||
}
|
||||
if (key=='s')
|
||||
{
|
||||
glPolygonMode( GL_FRONT_AND_BACK, GL_FILL);
|
||||
}
|
||||
|
||||
// if (sDemo)
|
||||
// sDemo->keyboardCallback(key,state);
|
||||
|
||||
b3DefaultKeyboardCallback(key,state);
|
||||
}
|
||||
|
||||
|
||||
int main(int argc, char* argv[])
|
||||
{
|
||||
|
||||
@@ -201,8 +416,13 @@ int main(int argc, char* argv[])
|
||||
app->m_instancingRenderer->setCameraPitch(0);
|
||||
app->m_instancingRenderer->setCameraTargetPosition(b3MakeVector3(0,0,0));
|
||||
|
||||
app->m_window->setMouseMoveCallback(MyMouseMoveCallback);
|
||||
app->m_window->setMouseButtonCallback(MyMouseButtonCallback);
|
||||
app->m_window->setKeyboardCallback(MyKeyboardCallback);
|
||||
|
||||
BasicDemo* demo = new BasicDemo(app);
|
||||
demo->initPhysics();
|
||||
sDemo = demo;
|
||||
|
||||
GLint err = glGetError();
|
||||
assert(err==GL_NO_ERROR);
|
||||
|
||||
742
Demos3/bullet2/FeatherstoneMultiBodyDemo/main.cpp
Normal file
742
Demos3/bullet2/FeatherstoneMultiBodyDemo/main.cpp
Normal file
@@ -0,0 +1,742 @@
|
||||
|
||||
#define ARRAY_SIZE_X 5
|
||||
#define ARRAY_SIZE_Y 5
|
||||
#define ARRAY_SIZE_Z 5
|
||||
float scaling = 1.f;
|
||||
float friction = 1.;
|
||||
|
||||
|
||||
#include "OpenGLWindow/SimpleOpenGL3App.h"
|
||||
#include "Bullet3Common/b3Vector3.h"
|
||||
#include "assert.h"
|
||||
#include <stdio.h>
|
||||
|
||||
#include "btBulletDynamicsCommon.h"
|
||||
|
||||
#include "BulletDynamics/Featherstone/btMultiBody.h"
|
||||
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
|
||||
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
|
||||
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
|
||||
#include "BulletDynamics/Featherstone/btMultiBodyLink.h"
|
||||
#include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h"
|
||||
#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
|
||||
#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
|
||||
|
||||
static b3Vector4 colors[4] =
|
||||
{
|
||||
b3MakeVector4(1,0,0,1),
|
||||
b3MakeVector4(0,1,0,1),
|
||||
b3MakeVector4(0,1,1,1),
|
||||
b3MakeVector4(1,1,0,1),
|
||||
};
|
||||
|
||||
|
||||
struct btMultiBodySettings
|
||||
{
|
||||
btMultiBodySettings()
|
||||
{
|
||||
m_numLinks = 0;
|
||||
m_basePosition.setZero();
|
||||
m_isFixedBase = true;
|
||||
m_usePrismatic = false;
|
||||
m_canSleep = true;
|
||||
m_createConstraints = false;
|
||||
m_disableParentCollision = false;
|
||||
}
|
||||
int m_numLinks;
|
||||
btVector3 m_basePosition;
|
||||
bool m_isFixedBase;
|
||||
bool m_usePrismatic;
|
||||
bool m_canSleep;
|
||||
bool m_createConstraints;
|
||||
bool m_disableParentCollision;
|
||||
};
|
||||
|
||||
class Bullet2MultiBodyDemo
|
||||
{
|
||||
protected:
|
||||
btMultiBodyDynamicsWorld* m_dynamicsWorld;
|
||||
btCollisionDispatcher* m_dispatcher;
|
||||
btBroadphaseInterface* m_bp;
|
||||
btCollisionConfiguration* m_config;
|
||||
btMultiBodyConstraintSolver* m_solver;
|
||||
|
||||
public:
|
||||
Bullet2MultiBodyDemo()
|
||||
{
|
||||
m_config = 0;
|
||||
m_dispatcher = 0;
|
||||
m_bp = 0;
|
||||
m_solver = 0;
|
||||
m_dynamicsWorld = 0;
|
||||
}
|
||||
virtual void initPhysics()
|
||||
{
|
||||
m_config = new btDefaultCollisionConfiguration;
|
||||
m_dispatcher = new btCollisionDispatcher(m_config);
|
||||
m_bp = new btDbvtBroadphase();
|
||||
m_solver = new btMultiBodyConstraintSolver();
|
||||
m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_dispatcher,m_bp,m_solver,m_config);
|
||||
}
|
||||
virtual void exitPhysics()
|
||||
{
|
||||
delete m_dynamicsWorld;
|
||||
m_dynamicsWorld=0;
|
||||
delete m_solver;
|
||||
m_solver=0;
|
||||
delete m_bp;
|
||||
m_bp=0;
|
||||
delete m_dispatcher;
|
||||
m_dispatcher=0;
|
||||
delete m_config;
|
||||
m_config=0;
|
||||
}
|
||||
|
||||
virtual ~Bullet2MultiBodyDemo()
|
||||
{
|
||||
btAssert(m_config == 0);
|
||||
btAssert(m_dispatcher == 0);
|
||||
btAssert(m_bp == 0);
|
||||
btAssert(m_solver == 0);
|
||||
btAssert(m_dynamicsWorld == 0);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
class BasicDemo : public Bullet2MultiBodyDemo
|
||||
{
|
||||
SimpleOpenGL3App* m_glApp;
|
||||
|
||||
btRigidBody* m_pickedBody;
|
||||
btTypedConstraint* m_pickedConstraint;
|
||||
btVector3 m_oldPickingPos;
|
||||
btVector3 m_hitPos;
|
||||
btScalar m_oldPickingDist;
|
||||
|
||||
class btMultiBodyPoint2Point* m_pickingMultiBodyPoint2Point;
|
||||
btAlignedObjectArray<btMultiBodyLinkCollider*> m_linkColliders;
|
||||
|
||||
public:
|
||||
BasicDemo(SimpleOpenGL3App* app)
|
||||
:m_glApp(app),
|
||||
m_pickedBody(0),
|
||||
m_pickedConstraint(0),
|
||||
m_pickingMultiBodyPoint2Point(0)
|
||||
{
|
||||
}
|
||||
virtual ~BasicDemo()
|
||||
{
|
||||
}
|
||||
|
||||
btMultiBody* createFeatherstoneMultiBody(class btMultiBodyDynamicsWorld* world, const btMultiBodySettings& settings)
|
||||
{
|
||||
static int curColor=0;
|
||||
|
||||
|
||||
int cubeShapeId = m_glApp->registerCubeShape();
|
||||
|
||||
int n_links = settings.m_numLinks;
|
||||
float mass = 13.5*scaling;
|
||||
btVector3 inertia = btVector3 (91,344,253)*scaling*scaling;
|
||||
|
||||
|
||||
btMultiBody * bod = new btMultiBody(n_links, mass, inertia, settings.m_isFixedBase, settings.m_canSleep);
|
||||
// bod->setHasSelfCollision(false);
|
||||
|
||||
//btQuaternion orn(btVector3(0,0,1),-0.25*SIMD_HALF_PI);//0,0,0,1);
|
||||
btQuaternion orn(0,0,0,1);
|
||||
bod->setBasePos(settings.m_basePosition);
|
||||
bod->setWorldToBaseRot(orn);
|
||||
btVector3 vel(0,0,0);
|
||||
bod->setBaseVel(vel);
|
||||
|
||||
{
|
||||
|
||||
btVector3 joint_axis_hinge(1,0,0);
|
||||
btVector3 joint_axis_prismatic(0,0,1);
|
||||
btQuaternion parent_to_child = orn.inverse();
|
||||
btVector3 joint_axis_child_prismatic = quatRotate(parent_to_child ,joint_axis_prismatic);
|
||||
btVector3 joint_axis_child_hinge = quatRotate(parent_to_child , joint_axis_hinge);
|
||||
|
||||
int this_link_num = -1;
|
||||
int link_num_counter = 0;
|
||||
|
||||
|
||||
|
||||
btVector3 pos = btVector3 (0,0,9.0500002)*scaling;
|
||||
|
||||
btVector3 joint_axis_position = btVector3 (0,0,4.5250001)*scaling;
|
||||
|
||||
for (int i=0;i<n_links;i++)
|
||||
{
|
||||
float initial_joint_angle=0.3;
|
||||
if (i>0)
|
||||
initial_joint_angle = -0.06f;
|
||||
|
||||
const int child_link_num = link_num_counter++;
|
||||
|
||||
|
||||
|
||||
if (settings.m_usePrismatic)// && i==(n_links-1))
|
||||
{
|
||||
bod->setupPrismatic(child_link_num, mass, inertia, this_link_num,
|
||||
parent_to_child, joint_axis_child_prismatic, quatRotate(parent_to_child , pos),settings.m_disableParentCollision);
|
||||
|
||||
} else
|
||||
{
|
||||
bod->setupRevolute(child_link_num, mass, inertia, this_link_num,parent_to_child, joint_axis_child_hinge,
|
||||
joint_axis_position,quatRotate(parent_to_child , (pos - joint_axis_position)),settings.m_disableParentCollision);
|
||||
}
|
||||
bod->setJointPos(child_link_num, initial_joint_angle);
|
||||
this_link_num = i;
|
||||
|
||||
if (0)//!useGroundShape && i==4)
|
||||
{
|
||||
btVector3 pivotInAworld(0,20,46);
|
||||
btVector3 pivotInAlocal = bod->worldPosToLocal(i, pivotInAworld);
|
||||
btVector3 pivotInBworld = pivotInAworld;
|
||||
btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(bod,i,&btTypedConstraint::getFixedBody(),pivotInAlocal,pivotInBworld);
|
||||
world->addMultiBodyConstraint(p2p);
|
||||
}
|
||||
//add some constraint limit
|
||||
if (settings.m_usePrismatic)
|
||||
{
|
||||
// btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(bod,n_links-1,2,3);
|
||||
|
||||
if (settings.m_createConstraints)
|
||||
{
|
||||
btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(bod,i,-1,1);
|
||||
world->addMultiBodyConstraint(con);
|
||||
}
|
||||
|
||||
} else
|
||||
{
|
||||
if (settings.m_createConstraints)
|
||||
{
|
||||
if (1)
|
||||
{
|
||||
btMultiBodyJointMotor* con = new btMultiBodyJointMotor(bod,i,0,500000);
|
||||
world->addMultiBodyConstraint(con);
|
||||
}
|
||||
|
||||
btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(bod,i,-1,1);
|
||||
world->addMultiBodyConstraint(con);
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//add a collider for the base
|
||||
{
|
||||
|
||||
btAlignedObjectArray<btQuaternion> world_to_local;
|
||||
world_to_local.resize(n_links+1);
|
||||
|
||||
btAlignedObjectArray<btVector3> local_origin;
|
||||
local_origin.resize(n_links+1);
|
||||
world_to_local[0] = bod->getWorldToBaseRot();
|
||||
local_origin[0] = bod->getBasePos();
|
||||
//float halfExtents[3]={7.5,0.05,4.5};
|
||||
float halfExtents[3]={7.5,0.45,4.5};
|
||||
{
|
||||
|
||||
float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1};
|
||||
float quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()};
|
||||
|
||||
|
||||
if (1)
|
||||
{
|
||||
btCollisionShape* box = new btBoxShape(btVector3(halfExtents[0],halfExtents[1],halfExtents[2])*scaling);
|
||||
btRigidBody* body = new btRigidBody(mass,0,box,inertia);
|
||||
btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(bod,-1);
|
||||
|
||||
|
||||
|
||||
|
||||
body->setCollisionShape(box);
|
||||
col->setCollisionShape(box);
|
||||
|
||||
btTransform tr;
|
||||
tr.setIdentity();
|
||||
tr.setOrigin(local_origin[0]);
|
||||
tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
|
||||
body->setWorldTransform(tr);
|
||||
col->setWorldTransform(tr);
|
||||
|
||||
b3Vector4 color = colors[curColor++];
|
||||
curColor&=3;
|
||||
|
||||
int index = m_glApp->m_instancingRenderer->registerGraphicsInstance(cubeShapeId,tr.getOrigin(),tr.getRotation(),color,halfExtents);
|
||||
col->setUserIndex(index);
|
||||
|
||||
|
||||
|
||||
|
||||
world->addCollisionObject(col,short(btBroadphaseProxy::DefaultFilter),short(btBroadphaseProxy::AllFilter));
|
||||
col->setFriction(friction);
|
||||
bod->setBaseCollider(col);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
for (int i=0;i<bod->getNumLinks();i++)
|
||||
{
|
||||
const int parent = bod->getParent(i);
|
||||
world_to_local[i+1] = bod->getParentToLocalRot(i) * world_to_local[parent+1];
|
||||
local_origin[i+1] = local_origin[parent+1] + (quatRotate(world_to_local[i+1].inverse() , bod->getRVector(i)));
|
||||
}
|
||||
|
||||
|
||||
for (int i=0;i<bod->getNumLinks();i++)
|
||||
{
|
||||
|
||||
btVector3 posr = local_origin[i+1];
|
||||
float pos[4]={posr.x(),posr.y(),posr.z(),1};
|
||||
|
||||
float quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()};
|
||||
|
||||
btCollisionShape* box = new btBoxShape(btVector3(halfExtents[0],halfExtents[1],halfExtents[2])*scaling);
|
||||
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(bod,i);
|
||||
|
||||
col->setCollisionShape(box);
|
||||
btTransform tr;
|
||||
tr.setIdentity();
|
||||
tr.setOrigin(posr);
|
||||
tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
|
||||
col->setWorldTransform(tr);
|
||||
col->setFriction(friction);
|
||||
|
||||
|
||||
b3Vector4 color = colors[curColor++];
|
||||
curColor&=3;
|
||||
|
||||
int index = m_glApp->m_instancingRenderer->registerGraphicsInstance(cubeShapeId,tr.getOrigin(),tr.getRotation(),color,halfExtents);
|
||||
col->setUserIndex(index);
|
||||
|
||||
|
||||
|
||||
world->addCollisionObject(col,short(btBroadphaseProxy::DefaultFilter),short(btBroadphaseProxy::AllFilter));
|
||||
|
||||
bod->getLink(i).m_collider=col;
|
||||
//app->drawBox(halfExtents, pos,quat);
|
||||
}
|
||||
|
||||
}
|
||||
world->addMultiBody(bod);
|
||||
|
||||
return bod;
|
||||
}
|
||||
|
||||
void addColliders_testMultiDof(btMultiBody *pMultiBody, btMultiBodyDynamicsWorld *pWorld, const btVector3 &baseHalfExtents, const btVector3 &linkHalfExtents)
|
||||
{
|
||||
}
|
||||
void addBoxes_testMultiDof()
|
||||
{
|
||||
}
|
||||
|
||||
void initPhysics()
|
||||
{
|
||||
|
||||
Bullet2MultiBodyDemo::initPhysics();
|
||||
|
||||
//create ground
|
||||
int cubeShapeId = m_glApp->registerCubeShape();
|
||||
float pos[]={0,0,0};
|
||||
float orn[]={0,0,0,1};
|
||||
|
||||
|
||||
{
|
||||
float color[]={0.3,0.3,1,1};
|
||||
float halfExtents[]={50,50,50,1};
|
||||
btTransform groundTransform;
|
||||
groundTransform.setIdentity();
|
||||
groundTransform.setOrigin(btVector3(0,-50,0));
|
||||
btBoxShape* groundShape = new btBoxShape(btVector3(btScalar(halfExtents[0]),btScalar(halfExtents[1]),btScalar(halfExtents[2])));
|
||||
//We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
|
||||
{
|
||||
btScalar mass(0.);
|
||||
//rigidbody is dynamic if and only if mass is non zero, otherwise static
|
||||
bool isDynamic = (mass != 0.f);
|
||||
btVector3 localInertia(0,0,0);
|
||||
if (isDynamic)
|
||||
groundShape->calculateLocalInertia(mass,localInertia);
|
||||
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
|
||||
btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
|
||||
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia);
|
||||
btRigidBody* body = new btRigidBody(rbInfo);
|
||||
|
||||
int index = m_glApp->m_instancingRenderer->registerGraphicsInstance(cubeShapeId,groundTransform.getOrigin(),groundTransform.getRotation(),color,halfExtents);
|
||||
body ->setUserIndex(index);
|
||||
|
||||
//add the body to the dynamics world
|
||||
m_dynamicsWorld->addRigidBody(body);
|
||||
}
|
||||
}
|
||||
#if 0
|
||||
|
||||
{
|
||||
float halfExtents[]={1,1,1,1};
|
||||
|
||||
|
||||
|
||||
btTransform startTransform;
|
||||
startTransform.setIdentity();
|
||||
btScalar mass = 1.f;
|
||||
btVector3 localInertia;
|
||||
btBoxShape* colShape = new btBoxShape(btVector3(halfExtents[0],halfExtents[1],halfExtents[2]));
|
||||
colShape ->calculateLocalInertia(mass,localInertia);
|
||||
|
||||
for (int k=0;k<ARRAY_SIZE_Y;k++)
|
||||
{
|
||||
for (int i=0;i<ARRAY_SIZE_X;i++)
|
||||
{
|
||||
for(int j = 0;j<ARRAY_SIZE_Z;j++)
|
||||
{
|
||||
static int curColor=0;
|
||||
b3Vector4 color = colors[curColor];
|
||||
curColor++;
|
||||
startTransform.setOrigin(btVector3(
|
||||
btScalar(2.0*i),
|
||||
btScalar(20+2.0*k),
|
||||
btScalar(2.0*j)));
|
||||
|
||||
int index = m_glApp->m_instancingRenderer->registerGraphicsInstance(cubeShapeId,startTransform.getOrigin(),startTransform.getRotation(),color,halfExtents);
|
||||
|
||||
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
|
||||
btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
|
||||
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,colShape,localInertia);
|
||||
btRigidBody* body = new btRigidBody(rbInfo);
|
||||
body->setUserIndex(index);
|
||||
|
||||
m_dynamicsWorld->addRigidBody(body);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
btMultiBodySettings settings;
|
||||
settings.m_isFixedBase = false;
|
||||
settings.m_basePosition.setValue(0,10,0);
|
||||
settings.m_numLinks = 10;
|
||||
btMultiBody* mb = createFeatherstoneMultiBody(m_dynamicsWorld,settings);
|
||||
|
||||
m_glApp->m_instancingRenderer->writeTransforms();
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
void exitPhysics()
|
||||
{
|
||||
Bullet2MultiBodyDemo::exitPhysics();
|
||||
}
|
||||
void drawObjects()
|
||||
{
|
||||
//sync graphics -> physics world transforms
|
||||
{
|
||||
for (int i=0;i<m_dynamicsWorld->getNumCollisionObjects();i++)
|
||||
{
|
||||
btCollisionObject* col = m_dynamicsWorld->getCollisionObjectArray()[i];
|
||||
|
||||
btVector3 pos = col->getWorldTransform().getOrigin();
|
||||
btQuaternion orn = col->getWorldTransform().getRotation();
|
||||
int index = col->getUserIndex();
|
||||
m_glApp->m_instancingRenderer->writeSingleInstanceTransformToCPU(pos,orn,index);
|
||||
}
|
||||
m_glApp->m_instancingRenderer->writeTransforms();
|
||||
}
|
||||
|
||||
m_glApp->m_instancingRenderer->renderScene();
|
||||
}
|
||||
|
||||
btVector3 getRayTo(int x,int y)
|
||||
{
|
||||
if (!m_glApp->m_instancingRenderer)
|
||||
{
|
||||
btAssert(0);
|
||||
return btVector3(0,0,0);
|
||||
}
|
||||
|
||||
float top = 1.f;
|
||||
float bottom = -1.f;
|
||||
float nearPlane = 1.f;
|
||||
float tanFov = (top-bottom)*0.5f / nearPlane;
|
||||
float fov = b3Scalar(2.0) * b3Atan(tanFov);
|
||||
|
||||
btVector3 camPos,camTarget;
|
||||
m_glApp->m_instancingRenderer->getCameraPosition(camPos);
|
||||
m_glApp->m_instancingRenderer->getCameraTargetPosition(camTarget);
|
||||
|
||||
btVector3 rayFrom = camPos;
|
||||
btVector3 rayForward = (camTarget-camPos);
|
||||
rayForward.normalize();
|
||||
float farPlane = 10000.f;
|
||||
rayForward*= farPlane;
|
||||
|
||||
btVector3 rightOffset;
|
||||
btVector3 m_cameraUp=btVector3(0,1,0);
|
||||
btVector3 vertical = m_cameraUp;
|
||||
|
||||
btVector3 hor;
|
||||
hor = rayForward.cross(vertical);
|
||||
hor.normalize();
|
||||
vertical = hor.cross(rayForward);
|
||||
vertical.normalize();
|
||||
|
||||
float tanfov = tanf(0.5f*fov);
|
||||
|
||||
|
||||
hor *= 2.f * farPlane * tanfov;
|
||||
vertical *= 2.f * farPlane * tanfov;
|
||||
|
||||
b3Scalar aspect;
|
||||
float width = m_glApp->m_instancingRenderer->getScreenWidth();
|
||||
float height = m_glApp->m_instancingRenderer->getScreenHeight();
|
||||
|
||||
aspect = width / height;
|
||||
|
||||
hor*=aspect;
|
||||
|
||||
|
||||
btVector3 rayToCenter = rayFrom + rayForward;
|
||||
btVector3 dHor = hor * 1.f/width;
|
||||
btVector3 dVert = vertical * 1.f/height;
|
||||
|
||||
|
||||
btVector3 rayTo = rayToCenter - 0.5f * hor + 0.5f * vertical;
|
||||
rayTo += btScalar(x) * dHor;
|
||||
rayTo -= btScalar(y) * dVert;
|
||||
return rayTo;
|
||||
}
|
||||
|
||||
|
||||
bool mouseMoveCallback(float x,float y)
|
||||
{
|
||||
// if (m_data->m_altPressed!=0 || m_data->m_controlPressed!=0)
|
||||
// return false;
|
||||
|
||||
if (m_pickedBody && m_pickedConstraint)
|
||||
{
|
||||
btPoint2PointConstraint* pickCon = static_cast<btPoint2PointConstraint*>(m_pickedConstraint);
|
||||
if (pickCon)
|
||||
{
|
||||
//keep it at the same picking distance
|
||||
btVector3 newRayTo = getRayTo(x,y);
|
||||
btVector3 rayFrom;
|
||||
btVector3 oldPivotInB = pickCon->getPivotInB();
|
||||
btVector3 newPivotB;
|
||||
m_glApp->m_instancingRenderer->getCameraPosition(rayFrom);
|
||||
btVector3 dir = newRayTo-rayFrom;
|
||||
dir.normalize();
|
||||
dir *= m_oldPickingDist;
|
||||
|
||||
newPivotB = rayFrom + dir;
|
||||
pickCon->setPivotB(newPivotB);
|
||||
}
|
||||
}
|
||||
if (m_pickingMultiBodyPoint2Point)
|
||||
{
|
||||
//keep it at the same picking distance
|
||||
|
||||
btVector3 newRayTo = getRayTo(x,y);
|
||||
btVector3 rayFrom;
|
||||
btVector3 oldPivotInB = m_pickingMultiBodyPoint2Point->getPivotInB();
|
||||
btVector3 newPivotB;
|
||||
btVector3 camPos;
|
||||
m_glApp->m_instancingRenderer->getCameraPosition(camPos);
|
||||
rayFrom = camPos;
|
||||
btVector3 dir = newRayTo-rayFrom;
|
||||
dir.normalize();
|
||||
dir *= m_oldPickingDist;
|
||||
|
||||
newPivotB = rayFrom + dir;
|
||||
|
||||
m_pickingMultiBodyPoint2Point->setPivotInB(newPivotB);
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
bool mouseButtonCallback(int button, int state, float x, float y)
|
||||
{
|
||||
|
||||
if (state==1)
|
||||
{
|
||||
if(button==0)// && (m_data->m_altPressed==0 && m_data->m_controlPressed==0))
|
||||
{
|
||||
btVector3 camPos;
|
||||
m_glApp->m_instancingRenderer->getCameraPosition(camPos);
|
||||
|
||||
btVector3 rayFrom = camPos;
|
||||
btVector3 rayTo = getRayTo(x,y);
|
||||
|
||||
btCollisionWorld::ClosestRayResultCallback rayCallback(rayFrom,rayTo);
|
||||
m_dynamicsWorld->rayTest(rayFrom,rayTo,rayCallback);
|
||||
if (rayCallback.hasHit())
|
||||
{
|
||||
|
||||
btVector3 pickPos = rayCallback.m_hitPointWorld;
|
||||
btRigidBody* body = (btRigidBody*)btRigidBody::upcast(rayCallback.m_collisionObject);
|
||||
if (body)
|
||||
{
|
||||
//other exclusions?
|
||||
if (!(body->isStaticObject() || body->isKinematicObject()))
|
||||
{
|
||||
m_pickedBody = body;
|
||||
m_pickedBody->setActivationState(DISABLE_DEACTIVATION);
|
||||
//printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ());
|
||||
btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos;
|
||||
btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body,localPivot);
|
||||
m_dynamicsWorld->addConstraint(p2p,true);
|
||||
m_pickedConstraint = p2p;
|
||||
btScalar mousePickClamping = 30.f;
|
||||
p2p->m_setting.m_impulseClamp = mousePickClamping;
|
||||
//very weak constraint for picking
|
||||
p2p->m_setting.m_tau = 0.001f;
|
||||
}
|
||||
} else
|
||||
{
|
||||
btMultiBodyLinkCollider* multiCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(rayCallback.m_collisionObject);
|
||||
if (multiCol && multiCol->m_multiBody)
|
||||
{
|
||||
multiCol->m_multiBody->setCanSleep(false);
|
||||
|
||||
btVector3 pivotInA = multiCol->m_multiBody->worldPosToLocal(multiCol->m_link, pickPos);
|
||||
|
||||
btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(multiCol->m_multiBody,multiCol->m_link,0,pivotInA,pickPos);
|
||||
//if you add too much energy to the system, causing high angular velocities, simulation 'explodes'
|
||||
//see also http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=4&t=949
|
||||
//so we try to avoid it by clamping the maximum impulse (force) that the mouse pick can apply
|
||||
//it is not satisfying, hopefully we find a better solution (higher order integrator, using joint friction using a zero-velocity target motor with limited force etc?)
|
||||
|
||||
p2p->setMaxAppliedImpulse(20*scaling);
|
||||
|
||||
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_dynamicsWorld;
|
||||
world->addMultiBodyConstraint(p2p);
|
||||
m_pickingMultiBodyPoint2Point =p2p;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// pickObject(pickPos, rayCallback.m_collisionObject);
|
||||
m_oldPickingPos = rayTo;
|
||||
m_hitPos = pickPos;
|
||||
m_oldPickingDist = (pickPos-rayFrom).length();
|
||||
// printf("hit !\n");
|
||||
//add p2p
|
||||
}
|
||||
|
||||
}
|
||||
} else
|
||||
{
|
||||
if (button==0)
|
||||
{
|
||||
if (m_pickedConstraint)
|
||||
{
|
||||
m_dynamicsWorld->removeConstraint(m_pickedConstraint);
|
||||
delete m_pickedConstraint;
|
||||
m_pickedConstraint=0;
|
||||
m_pickedBody = 0;
|
||||
}
|
||||
|
||||
if (m_pickingMultiBodyPoint2Point)
|
||||
{
|
||||
m_pickingMultiBodyPoint2Point->getMultiBodyA()->setCanSleep(true);
|
||||
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_dynamicsWorld;
|
||||
world->removeMultiBodyConstraint(m_pickingMultiBodyPoint2Point);
|
||||
delete m_pickingMultiBodyPoint2Point;
|
||||
m_pickingMultiBodyPoint2Point = 0;
|
||||
}
|
||||
//remove p2p
|
||||
}
|
||||
}
|
||||
|
||||
//printf("button=%d, state=%d\n",button,state);
|
||||
return false;
|
||||
}
|
||||
|
||||
void stepSimulation()
|
||||
{
|
||||
m_dynamicsWorld->stepSimulation(1./60,0);
|
||||
// CProfileManager::dumpAll();
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
|
||||
BasicDemo* sDemo = 0;
|
||||
|
||||
static void MyMouseMoveCallback( float x, float y)
|
||||
{
|
||||
bool handled = false;
|
||||
if (sDemo)
|
||||
handled = sDemo->mouseMoveCallback(x,y);
|
||||
if (!handled)
|
||||
b3DefaultMouseMoveCallback(x,y);
|
||||
}
|
||||
static void MyMouseButtonCallback(int button, int state, float x, float y)
|
||||
{
|
||||
bool handled = false;
|
||||
//try picking first
|
||||
if (sDemo)
|
||||
handled = sDemo->mouseButtonCallback(button,state,x,y);
|
||||
|
||||
if (!handled)
|
||||
b3DefaultMouseButtonCallback(button,state,x,y);
|
||||
}
|
||||
|
||||
|
||||
int main(int argc, char* argv[])
|
||||
{
|
||||
|
||||
float dt = 1./120.f;
|
||||
#ifdef BT_DEBUG
|
||||
char* name = "Bullet 2 CPU FeatherstoneMultiBodyDemo (Debug build=SLOW)";
|
||||
#else
|
||||
char* name = "Bullet 2 CPU FeatherstoneMultiBodyDemo";
|
||||
#endif
|
||||
|
||||
|
||||
SimpleOpenGL3App* app = new SimpleOpenGL3App(name,1024,768);
|
||||
app->m_instancingRenderer->setCameraDistance(40);
|
||||
app->m_instancingRenderer->setCameraPitch(0);
|
||||
app->m_instancingRenderer->setCameraTargetPosition(b3MakeVector3(0,0,0));
|
||||
|
||||
app->m_window->setMouseMoveCallback(MyMouseMoveCallback);
|
||||
app->m_window->setMouseButtonCallback(MyMouseButtonCallback);
|
||||
|
||||
BasicDemo* demo = new BasicDemo(app);
|
||||
demo->initPhysics();
|
||||
sDemo = demo;
|
||||
|
||||
GLint err = glGetError();
|
||||
assert(err==GL_NO_ERROR);
|
||||
|
||||
do
|
||||
{
|
||||
GLint err = glGetError();
|
||||
assert(err==GL_NO_ERROR);
|
||||
app->m_instancingRenderer->init();
|
||||
app->m_instancingRenderer->updateCamera();
|
||||
|
||||
demo->stepSimulation();
|
||||
demo->drawObjects();
|
||||
app->drawGrid(10,0.01);
|
||||
char bla[1024];
|
||||
static int frameCount = 0;
|
||||
frameCount++;
|
||||
sprintf(bla,"Simulation frame %d", frameCount);
|
||||
|
||||
app->drawText(bla,10,10);
|
||||
app->swapBuffer();
|
||||
} while (!app->m_window->requestedExit());
|
||||
|
||||
|
||||
demo->exitPhysics();
|
||||
delete demo;
|
||||
|
||||
delete app;
|
||||
return 0;
|
||||
}
|
||||
38
Demos3/bullet2/FeatherstoneMultiBodyDemo/premake4.lua
Normal file
38
Demos3/bullet2/FeatherstoneMultiBodyDemo/premake4.lua
Normal file
@@ -0,0 +1,38 @@
|
||||
|
||||
project "App2_FeatherstoneMultiBodyDemo"
|
||||
|
||||
language "C++"
|
||||
|
||||
kind "ConsoleApp"
|
||||
targetdir "../../../bin"
|
||||
|
||||
includedirs {
|
||||
".",
|
||||
"../../../src",
|
||||
"../../../btgui"
|
||||
}
|
||||
|
||||
initOpenGL()
|
||||
initGlew()
|
||||
|
||||
links{"gwen", "BulletDynamics", "BulletCollision","LinearMath",
|
||||
"OpenGL_Window", "OpenGL_TrueTypeFont"
|
||||
}
|
||||
|
||||
files {
|
||||
"**.cpp",
|
||||
"**.h",
|
||||
"../../../src/Bullet3Common/**.cpp",
|
||||
"../../../src/Bullet3Common/**.h",
|
||||
"../../../btgui/Timing/b3Clock.cpp",
|
||||
"../../../btgui/Timing/b3Clock.h"
|
||||
|
||||
}
|
||||
|
||||
if os.is("Linux") then
|
||||
links ("X11")
|
||||
end
|
||||
|
||||
if os.is("MacOSX") then
|
||||
links{"Cocoa.framework"}
|
||||
end
|
||||
@@ -407,6 +407,8 @@ GLInstancingRenderer::~GLInstancingRenderer()
|
||||
|
||||
void GLInstancingRenderer::writeSingleInstanceTransformToCPU(const float* position, const float* orientation, int srcIndex)
|
||||
{
|
||||
b3Assert(srcIndex<m_data->m_totalNumInstances);
|
||||
b3Assert(srcIndex>=0);
|
||||
m_data->m_instance_positions_ptr[srcIndex*4+0]=position[0];
|
||||
m_data->m_instance_positions_ptr[srcIndex*4+1]=position[1];
|
||||
m_data->m_instance_positions_ptr[srcIndex*4+2]=position[2];
|
||||
@@ -605,7 +607,7 @@ int GLInstancingRenderer::registerGraphicsInstance(int shapeIndex, const float*
|
||||
b3Error("registerGraphicsInstance out of range, %d\n", maxElements);
|
||||
return -1;
|
||||
}
|
||||
return gfxObj->m_numGraphicsInstances;
|
||||
return index;//gfxObj->m_numGraphicsInstances;
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -115,6 +115,7 @@ include "../Demos3/SimpleOpenGL3"
|
||||
include "../src/BulletCollision"
|
||||
include "../src/LinearMath"
|
||||
include "../Demos3/bullet2/BasicDemo"
|
||||
include "../Demos3/bullet2/FeatherstoneMultiBodyDemo"
|
||||
|
||||
include "../src/Bullet3Dynamics"
|
||||
include "../src/Bullet3Common"
|
||||
|
||||
@@ -0,0 +1,20 @@
|
||||
|
||||
#ifndef B3_BVH_SUBTREE_INFO_DATA_H
|
||||
#define B3_BVH_SUBTREE_INFO_DATA_H
|
||||
|
||||
typedef struct b3BvhSubtreeInfoData b3BvhSubtreeInfoData_t;
|
||||
|
||||
struct b3BvhSubtreeInfoData
|
||||
{
|
||||
//12 bytes
|
||||
unsigned short int m_quantizedAabbMin[3];
|
||||
unsigned short int m_quantizedAabbMax[3];
|
||||
//4 bytes, points to the root of the subtree
|
||||
int m_rootNodeIndex;
|
||||
//4 bytes
|
||||
int m_subtreeSize;
|
||||
int m_padding[3];
|
||||
};
|
||||
|
||||
#endif //B3_BVH_SUBTREE_INFO_DATA_H
|
||||
|
||||
@@ -0,0 +1,126 @@
|
||||
|
||||
|
||||
#include "Bullet3Common/shared/b3Int4.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
|
||||
#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h"
|
||||
|
||||
|
||||
|
||||
// work-in-progress
|
||||
void b3BvhTraversal( __global const b3Int4* pairs,
|
||||
__global const b3RigidBodyData* rigidBodies,
|
||||
__global const b3Collidable* collidables,
|
||||
__global b3Aabb* aabbs,
|
||||
__global b3Int4* concavePairsOut,
|
||||
__global volatile int* numConcavePairsOut,
|
||||
__global const b3BvhSubtreeInfo* subtreeHeadersRoot,
|
||||
__global const b3QuantizedBvhNode* quantizedNodesRoot,
|
||||
__global const b3BvhInfo* bvhInfos,
|
||||
int numPairs,
|
||||
int maxNumConcavePairsCapacity,
|
||||
int id)
|
||||
{
|
||||
|
||||
int bodyIndexA = pairs[id].x;
|
||||
int bodyIndexB = pairs[id].y;
|
||||
int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;
|
||||
int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;
|
||||
|
||||
//once the broadphase avoids static-static pairs, we can remove this test
|
||||
if ((rigidBodies[bodyIndexA].m_invMass==0) &&(rigidBodies[bodyIndexB].m_invMass==0))
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if (collidables[collidableIndexA].m_shapeType!=SHAPE_CONCAVE_TRIMESH)
|
||||
return;
|
||||
|
||||
int shapeTypeB = collidables[collidableIndexB].m_shapeType;
|
||||
|
||||
if (shapeTypeB!=SHAPE_CONVEX_HULL &&
|
||||
shapeTypeB!=SHAPE_SPHERE &&
|
||||
shapeTypeB!=SHAPE_COMPOUND_OF_CONVEX_HULLS
|
||||
)
|
||||
return;
|
||||
|
||||
b3BvhInfo bvhInfo = bvhInfos[collidables[collidableIndexA].m_numChildShapes];
|
||||
|
||||
b3Float4 bvhAabbMin = bvhInfo.m_aabbMin;
|
||||
b3Float4 bvhAabbMax = bvhInfo.m_aabbMax;
|
||||
b3Float4 bvhQuantization = bvhInfo.m_quantization;
|
||||
int numSubtreeHeaders = bvhInfo.m_numSubTrees;
|
||||
__global const b3BvhSubtreeInfoData* subtreeHeaders = &subtreeHeadersRoot[bvhInfo.m_subTreeOffset];
|
||||
__global const b3QuantizedBvhNodeData* quantizedNodes = &quantizedNodesRoot[bvhInfo.m_nodeOffset];
|
||||
|
||||
|
||||
unsigned short int quantizedQueryAabbMin[3];
|
||||
unsigned short int quantizedQueryAabbMax[3];
|
||||
b3QuantizeWithClamp(quantizedQueryAabbMin,aabbs[bodyIndexB].m_minVec,false,bvhAabbMin, bvhAabbMax,bvhQuantization);
|
||||
b3QuantizeWithClamp(quantizedQueryAabbMax,aabbs[bodyIndexB].m_maxVec,true ,bvhAabbMin, bvhAabbMax,bvhQuantization);
|
||||
|
||||
for (int i=0;i<numSubtreeHeaders;i++)
|
||||
{
|
||||
b3BvhSubtreeInfoData subtree = subtreeHeaders[i];
|
||||
|
||||
int overlap = b3TestQuantizedAabbAgainstQuantizedAabbSlow(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax);
|
||||
if (overlap != 0)
|
||||
{
|
||||
int startNodeIndex = subtree.m_rootNodeIndex;
|
||||
int endNodeIndex = subtree.m_rootNodeIndex+subtree.m_subtreeSize;
|
||||
int curIndex = startNodeIndex;
|
||||
int escapeIndex;
|
||||
int isLeafNode;
|
||||
int aabbOverlap;
|
||||
while (curIndex < endNodeIndex)
|
||||
{
|
||||
b3QuantizedBvhNodeData rootNode = quantizedNodes[curIndex];
|
||||
aabbOverlap = b3TestQuantizedAabbAgainstQuantizedAabbSlow(quantizedQueryAabbMin,quantizedQueryAabbMax,rootNode.m_quantizedAabbMin,rootNode.m_quantizedAabbMax);
|
||||
isLeafNode = b3IsLeaf(&rootNode);
|
||||
if (aabbOverlap)
|
||||
{
|
||||
if (isLeafNode)
|
||||
{
|
||||
int triangleIndex = b3GetTriangleIndex(&rootNode);
|
||||
if (shapeTypeB==SHAPE_COMPOUND_OF_CONVEX_HULLS)
|
||||
{
|
||||
int numChildrenB = collidables[collidableIndexB].m_numChildShapes;
|
||||
int pairIdx = b3AtomicAdd (numConcavePairsOut,numChildrenB);
|
||||
for (int b=0;b<numChildrenB;b++)
|
||||
{
|
||||
if ((pairIdx+b)<maxNumConcavePairsCapacity)
|
||||
{
|
||||
int childShapeIndexB = collidables[collidableIndexB].m_shapeIndex+b;
|
||||
b3Int4 newPair = b3MakeInt4(bodyIndexA,bodyIndexB,triangleIndex,childShapeIndexB);
|
||||
concavePairsOut[pairIdx+b] = newPair;
|
||||
}
|
||||
}
|
||||
} else
|
||||
{
|
||||
int pairIdx = b3AtomicInc(numConcavePairsOut);
|
||||
if (pairIdx<maxNumConcavePairsCapacity)
|
||||
{
|
||||
b3Int4 newPair = b3MakeInt4(bodyIndexA,bodyIndexB,triangleIndex,0);
|
||||
concavePairsOut[pairIdx] = newPair;
|
||||
}
|
||||
}
|
||||
}
|
||||
curIndex++;
|
||||
} else
|
||||
{
|
||||
if (isLeafNode)
|
||||
{
|
||||
curIndex++;
|
||||
} else
|
||||
{
|
||||
escapeIndex = b3GetEscapeIndex(&rootNode);
|
||||
curIndex += escapeIndex;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,597 @@
|
||||
#ifndef B3_FIND_CONCAVE_SEPARATING_AXIS_H
|
||||
#define B3_FIND_CONCAVE_SEPARATING_AXIS_H
|
||||
|
||||
#define B3_TRIANGLE_NUM_CONVEX_FACES 5
|
||||
|
||||
|
||||
#include "Bullet3Common/shared/b3Int4.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3RigidBodyData.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
|
||||
#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3ConvexPolyhedronData.h"
|
||||
|
||||
|
||||
inline void b3Project(__global const b3ConvexPolyhedronData* hull, b3Float4ConstArg pos, b3QuatConstArg orn,
|
||||
const b3Float4* dir, __global const b3Float4* vertices, float* min, float* max)
|
||||
{
|
||||
min[0] = FLT_MAX;
|
||||
max[0] = -FLT_MAX;
|
||||
int numVerts = hull->m_numVertices;
|
||||
|
||||
const b3Float4 localDir = b3QuatRotate(b3QuatInverse(orn),*dir);
|
||||
float offset = b3Dot(pos,*dir);
|
||||
for(int i=0;i<numVerts;i++)
|
||||
{
|
||||
float dp = b3Dot(vertices[hull->m_vertexOffset+i],localDir);
|
||||
if(dp < min[0])
|
||||
min[0] = dp;
|
||||
if(dp > max[0])
|
||||
max[0] = dp;
|
||||
}
|
||||
if(min[0]>max[0])
|
||||
{
|
||||
float tmp = min[0];
|
||||
min[0] = max[0];
|
||||
max[0] = tmp;
|
||||
}
|
||||
min[0] += offset;
|
||||
max[0] += offset;
|
||||
}
|
||||
|
||||
|
||||
inline bool b3TestSepAxis(const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB,
|
||||
b3Float4ConstArg posA,b3QuatConstArg ornA,
|
||||
b3Float4ConstArg posB,b3QuatConstArg ornB,
|
||||
b3Float4* sep_axis, const b3Float4* verticesA, __global const b3Float4* verticesB,float* depth)
|
||||
{
|
||||
float Min0,Max0;
|
||||
float Min1,Max1;
|
||||
b3Project(hullA,posA,ornA,sep_axis,verticesA, &Min0, &Max0);
|
||||
b3Project(hullB,posB,ornB, sep_axis,verticesB, &Min1, &Max1);
|
||||
|
||||
if(Max0<Min1 || Max1<Min0)
|
||||
return false;
|
||||
|
||||
float d0 = Max0 - Min1;
|
||||
float d1 = Max1 - Min0;
|
||||
*depth = d0<d1 ? d0:d1;
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
bool b3FindSeparatingAxis( const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB,
|
||||
b3Float4ConstArg posA1,
|
||||
b3QuatConstArg ornA,
|
||||
b3Float4ConstArg posB1,
|
||||
b3QuatConstArg ornB,
|
||||
b3Float4ConstArg DeltaC2,
|
||||
|
||||
const b3Float4* verticesA,
|
||||
const b3Float4* uniqueEdgesA,
|
||||
const b3GpuFace* facesA,
|
||||
const int* indicesA,
|
||||
|
||||
__global const b3Float4* verticesB,
|
||||
__global const b3Float4* uniqueEdgesB,
|
||||
__global const b3GpuFace* facesB,
|
||||
__global const int* indicesB,
|
||||
b3Float4* sep,
|
||||
float* dmin)
|
||||
{
|
||||
|
||||
|
||||
b3Float4 posA = posA1;
|
||||
posA.w = 0.f;
|
||||
b3Float4 posB = posB1;
|
||||
posB.w = 0.f;
|
||||
int curPlaneTests=0;
|
||||
{
|
||||
int numFacesA = hullA->m_numFaces;
|
||||
// Test normals from hullA
|
||||
for(int i=0;i<numFacesA;i++)
|
||||
{
|
||||
const b3Float4 normal = facesA[hullA->m_faceOffset+i].m_plane;
|
||||
b3Float4 faceANormalWS = b3QuatRotate(ornA,normal);
|
||||
if (b3Dot(DeltaC2,faceANormalWS)<0)
|
||||
faceANormalWS*=-1.f;
|
||||
curPlaneTests++;
|
||||
float d;
|
||||
if(!b3TestSepAxis( hullA, hullB, posA,ornA,posB,ornB,&faceANormalWS, verticesA, verticesB,&d))
|
||||
return false;
|
||||
if(d<*dmin)
|
||||
{
|
||||
*dmin = d;
|
||||
*sep = faceANormalWS;
|
||||
}
|
||||
}
|
||||
}
|
||||
if((b3Dot(-DeltaC2,*sep))>0.0f)
|
||||
{
|
||||
*sep = -(*sep);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
bool b3FindSeparatingAxisEdgeEdge( const b3ConvexPolyhedronData* hullA, __global const b3ConvexPolyhedronData* hullB,
|
||||
b3Float4ConstArg posA1,
|
||||
b3QuatConstArg ornA,
|
||||
b3Float4ConstArg posB1,
|
||||
b3QuatConstArg ornB,
|
||||
b3Float4ConstArg DeltaC2,
|
||||
const b3Float4* verticesA,
|
||||
const b3Float4* uniqueEdgesA,
|
||||
const b3GpuFace* facesA,
|
||||
const int* indicesA,
|
||||
__global const b3Float4* verticesB,
|
||||
__global const b3Float4* uniqueEdgesB,
|
||||
__global const b3GpuFace* facesB,
|
||||
__global const int* indicesB,
|
||||
b3Float4* sep,
|
||||
float* dmin)
|
||||
{
|
||||
|
||||
|
||||
b3Float4 posA = posA1;
|
||||
posA.w = 0.f;
|
||||
b3Float4 posB = posB1;
|
||||
posB.w = 0.f;
|
||||
|
||||
int curPlaneTests=0;
|
||||
|
||||
int curEdgeEdge = 0;
|
||||
// Test edges
|
||||
for(int e0=0;e0<hullA->m_numUniqueEdges;e0++)
|
||||
{
|
||||
const b3Float4 edge0 = uniqueEdgesA[hullA->m_uniqueEdgesOffset+e0];
|
||||
b3Float4 edge0World = b3QuatRotate(ornA,edge0);
|
||||
|
||||
for(int e1=0;e1<hullB->m_numUniqueEdges;e1++)
|
||||
{
|
||||
const b3Float4 edge1 = uniqueEdgesB[hullB->m_uniqueEdgesOffset+e1];
|
||||
b3Float4 edge1World = b3QuatRotate(ornB,edge1);
|
||||
|
||||
|
||||
b3Float4 crossje = b3Cross(edge0World,edge1World);
|
||||
|
||||
curEdgeEdge++;
|
||||
if(!b3IsAlmostZero(crossje))
|
||||
{
|
||||
crossje = b3Normalized(crossje);
|
||||
if (b3Dot(DeltaC2,crossje)<0)
|
||||
crossje *= -1.f;
|
||||
|
||||
float dist;
|
||||
bool result = true;
|
||||
{
|
||||
float Min0,Max0;
|
||||
float Min1,Max1;
|
||||
b3Project(hullA,posA,ornA,&crossje,verticesA, &Min0, &Max0);
|
||||
b3Project(hullB,posB,ornB,&crossje,verticesB, &Min1, &Max1);
|
||||
|
||||
if(Max0<Min1 || Max1<Min0)
|
||||
result = false;
|
||||
|
||||
float d0 = Max0 - Min1;
|
||||
float d1 = Max1 - Min0;
|
||||
dist = d0<d1 ? d0:d1;
|
||||
result = true;
|
||||
|
||||
}
|
||||
|
||||
|
||||
if(dist<*dmin)
|
||||
{
|
||||
*dmin = dist;
|
||||
*sep = crossje;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
if((b3Dot(-DeltaC2,*sep))>0.0f)
|
||||
{
|
||||
*sep = -(*sep);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
|
||||
inline int b3FindClippingFaces(b3Float4ConstArg separatingNormal,
|
||||
__global const b3ConvexPolyhedronData_t* hullA, __global const b3ConvexPolyhedronData_t* hullB,
|
||||
b3Float4ConstArg posA, b3QuatConstArg ornA,b3Float4ConstArg posB, b3QuatConstArg ornB,
|
||||
__global b3Float4* worldVertsA1,
|
||||
__global b3Float4* worldNormalsA1,
|
||||
__global b3Float4* worldVertsB1,
|
||||
int capacityWorldVerts,
|
||||
const float minDist, float maxDist,
|
||||
__global const b3Float4* verticesA,
|
||||
__global const b3GpuFace_t* facesA,
|
||||
__global const int* indicesA,
|
||||
__global const b3Float4* verticesB,
|
||||
__global const b3GpuFace_t* facesB,
|
||||
__global const int* indicesB,
|
||||
|
||||
__global b3Int4* clippingFaces, int pairIndex)
|
||||
{
|
||||
int numContactsOut = 0;
|
||||
int numWorldVertsB1= 0;
|
||||
|
||||
|
||||
int closestFaceB=-1;
|
||||
float dmax = -FLT_MAX;
|
||||
|
||||
{
|
||||
for(int face=0;face<hullB->m_numFaces;face++)
|
||||
{
|
||||
const b3Float4 Normal = b3MakeFloat4(facesB[hullB->m_faceOffset+face].m_plane.x,
|
||||
facesB[hullB->m_faceOffset+face].m_plane.y, facesB[hullB->m_faceOffset+face].m_plane.z,0.f);
|
||||
const b3Float4 WorldNormal = b3QuatRotate(ornB, Normal);
|
||||
float d = b3Dot(WorldNormal,separatingNormal);
|
||||
if (d > dmax)
|
||||
{
|
||||
dmax = d;
|
||||
closestFaceB = face;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
const b3GpuFace_t polyB = facesB[hullB->m_faceOffset+closestFaceB];
|
||||
const int numVertices = polyB.m_numIndices;
|
||||
for(int e0=0;e0<numVertices;e0++)
|
||||
{
|
||||
const b3Float4 b = verticesB[hullB->m_vertexOffset+indicesB[polyB.m_indexOffset+e0]];
|
||||
worldVertsB1[pairIndex*capacityWorldVerts+numWorldVertsB1++] = b3TransformPoint(b,posB,ornB);
|
||||
}
|
||||
}
|
||||
|
||||
int closestFaceA=-1;
|
||||
{
|
||||
float dmin = FLT_MAX;
|
||||
for(int face=0;face<hullA->m_numFaces;face++)
|
||||
{
|
||||
const b3Float4 Normal = b3MakeFloat4(
|
||||
facesA[hullA->m_faceOffset+face].m_plane.x,
|
||||
facesA[hullA->m_faceOffset+face].m_plane.y,
|
||||
facesA[hullA->m_faceOffset+face].m_plane.z,
|
||||
0.f);
|
||||
const b3Float4 faceANormalWS = b3QuatRotate(ornA,Normal);
|
||||
|
||||
float d = b3Dot(faceANormalWS,separatingNormal);
|
||||
if (d < dmin)
|
||||
{
|
||||
dmin = d;
|
||||
closestFaceA = face;
|
||||
worldNormalsA1[pairIndex] = faceANormalWS;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int numVerticesA = facesA[hullA->m_faceOffset+closestFaceA].m_numIndices;
|
||||
for(int e0=0;e0<numVerticesA;e0++)
|
||||
{
|
||||
const b3Float4 a = verticesA[hullA->m_vertexOffset+indicesA[facesA[hullA->m_faceOffset+closestFaceA].m_indexOffset+e0]];
|
||||
worldVertsA1[pairIndex*capacityWorldVerts+e0] = b3TransformPoint(a, posA,ornA);
|
||||
}
|
||||
|
||||
clippingFaces[pairIndex].x = closestFaceA;
|
||||
clippingFaces[pairIndex].y = closestFaceB;
|
||||
clippingFaces[pairIndex].z = numVerticesA;
|
||||
clippingFaces[pairIndex].w = numWorldVertsB1;
|
||||
|
||||
|
||||
return numContactsOut;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
__kernel void b3FindConcaveSeparatingAxisKernel( __global b3Int4* concavePairs,
|
||||
__global const b3RigidBodyData* rigidBodies,
|
||||
__global const b3Collidable* collidables,
|
||||
__global const b3ConvexPolyhedronData* convexShapes,
|
||||
__global const b3Float4* vertices,
|
||||
__global const b3Float4* uniqueEdges,
|
||||
__global const b3GpuFace* faces,
|
||||
__global const int* indices,
|
||||
__global const b3GpuChildShape* gpuChildShapes,
|
||||
__global b3Aabb* aabbs,
|
||||
__global b3Float4* concaveSeparatingNormalsOut,
|
||||
__global b3Int4* clippingFacesOut,
|
||||
__global b3Vector3* worldVertsA1Out,
|
||||
__global b3Vector3* worldNormalsA1Out,
|
||||
__global b3Vector3* worldVertsB1Out,
|
||||
__global int* hasSeparatingNormals,
|
||||
int vertexFaceCapacity,
|
||||
int numConcavePairs,
|
||||
int pairIdx
|
||||
)
|
||||
{
|
||||
int i = pairIdx;
|
||||
/* int i = get_global_id(0);
|
||||
if (i>=numConcavePairs)
|
||||
return;
|
||||
int pairIdx = i;
|
||||
*/
|
||||
|
||||
int bodyIndexA = concavePairs[i].x;
|
||||
int bodyIndexB = concavePairs[i].y;
|
||||
|
||||
int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx;
|
||||
int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx;
|
||||
|
||||
int shapeIndexA = collidables[collidableIndexA].m_shapeIndex;
|
||||
int shapeIndexB = collidables[collidableIndexB].m_shapeIndex;
|
||||
|
||||
if (collidables[collidableIndexB].m_shapeType!=SHAPE_CONVEX_HULL&&
|
||||
collidables[collidableIndexB].m_shapeType!=SHAPE_COMPOUND_OF_CONVEX_HULLS)
|
||||
{
|
||||
concavePairs[pairIdx].w = -1;
|
||||
return;
|
||||
}
|
||||
|
||||
hasSeparatingNormals[i] = 0;
|
||||
|
||||
int numFacesA = convexShapes[shapeIndexA].m_numFaces;
|
||||
int numActualConcaveConvexTests = 0;
|
||||
|
||||
int f = concavePairs[i].z;
|
||||
|
||||
bool overlap = false;
|
||||
|
||||
b3ConvexPolyhedronData convexPolyhedronA;
|
||||
|
||||
//add 3 vertices of the triangle
|
||||
convexPolyhedronA.m_numVertices = 3;
|
||||
convexPolyhedronA.m_vertexOffset = 0;
|
||||
b3Float4 localCenter = b3MakeFloat4(0.f,0.f,0.f,0.f);
|
||||
|
||||
b3GpuFace face = faces[convexShapes[shapeIndexA].m_faceOffset+f];
|
||||
b3Float4 triMinAabb, triMaxAabb;
|
||||
b3Aabb triAabb;
|
||||
triAabb.m_minVec = b3MakeFloat4(1e30f,1e30f,1e30f,0.f);
|
||||
triAabb.m_maxVec = b3MakeFloat4(-1e30f,-1e30f,-1e30f,0.f);
|
||||
|
||||
b3Float4 verticesA[3];
|
||||
for (int i=0;i<3;i++)
|
||||
{
|
||||
int index = indices[face.m_indexOffset+i];
|
||||
b3Float4 vert = vertices[convexShapes[shapeIndexA].m_vertexOffset+index];
|
||||
verticesA[i] = vert;
|
||||
localCenter += vert;
|
||||
|
||||
triAabb.m_minVec = b3MinFloat4(triAabb.m_minVec,vert);
|
||||
triAabb.m_maxVec = b3MaxFloat4(triAabb.m_maxVec,vert);
|
||||
|
||||
}
|
||||
|
||||
overlap = true;
|
||||
overlap = (triAabb.m_minVec.x > aabbs[bodyIndexB].m_maxVec.x || triAabb.m_maxVec.x < aabbs[bodyIndexB].m_minVec.x) ? false : overlap;
|
||||
overlap = (triAabb.m_minVec.z > aabbs[bodyIndexB].m_maxVec.z || triAabb.m_maxVec.z < aabbs[bodyIndexB].m_minVec.z) ? false : overlap;
|
||||
overlap = (triAabb.m_minVec.y > aabbs[bodyIndexB].m_maxVec.y || triAabb.m_maxVec.y < aabbs[bodyIndexB].m_minVec.y) ? false : overlap;
|
||||
|
||||
if (overlap)
|
||||
{
|
||||
float dmin = FLT_MAX;
|
||||
int hasSeparatingAxis=5;
|
||||
b3Float4 sepAxis=b3MakeFloat4(1,2,3,4);
|
||||
|
||||
int localCC=0;
|
||||
numActualConcaveConvexTests++;
|
||||
|
||||
//a triangle has 3 unique edges
|
||||
convexPolyhedronA.m_numUniqueEdges = 3;
|
||||
convexPolyhedronA.m_uniqueEdgesOffset = 0;
|
||||
b3Float4 uniqueEdgesA[3];
|
||||
|
||||
uniqueEdgesA[0] = (verticesA[1]-verticesA[0]);
|
||||
uniqueEdgesA[1] = (verticesA[2]-verticesA[1]);
|
||||
uniqueEdgesA[2] = (verticesA[0]-verticesA[2]);
|
||||
|
||||
|
||||
convexPolyhedronA.m_faceOffset = 0;
|
||||
|
||||
b3Float4 normal = b3MakeFloat4(face.m_plane.x,face.m_plane.y,face.m_plane.z,0.f);
|
||||
|
||||
b3GpuFace facesA[B3_TRIANGLE_NUM_CONVEX_FACES];
|
||||
int indicesA[3+3+2+2+2];
|
||||
int curUsedIndices=0;
|
||||
int fidx=0;
|
||||
|
||||
//front size of triangle
|
||||
{
|
||||
facesA[fidx].m_indexOffset=curUsedIndices;
|
||||
indicesA[0] = 0;
|
||||
indicesA[1] = 1;
|
||||
indicesA[2] = 2;
|
||||
curUsedIndices+=3;
|
||||
float c = face.m_plane.w;
|
||||
facesA[fidx].m_plane.x = normal.x;
|
||||
facesA[fidx].m_plane.y = normal.y;
|
||||
facesA[fidx].m_plane.z = normal.z;
|
||||
facesA[fidx].m_plane.w = c;
|
||||
facesA[fidx].m_numIndices=3;
|
||||
}
|
||||
fidx++;
|
||||
//back size of triangle
|
||||
{
|
||||
facesA[fidx].m_indexOffset=curUsedIndices;
|
||||
indicesA[3]=2;
|
||||
indicesA[4]=1;
|
||||
indicesA[5]=0;
|
||||
curUsedIndices+=3;
|
||||
float c = b3Dot(normal,verticesA[0]);
|
||||
float c1 = -face.m_plane.w;
|
||||
facesA[fidx].m_plane.x = -normal.x;
|
||||
facesA[fidx].m_plane.y = -normal.y;
|
||||
facesA[fidx].m_plane.z = -normal.z;
|
||||
facesA[fidx].m_plane.w = c;
|
||||
facesA[fidx].m_numIndices=3;
|
||||
}
|
||||
fidx++;
|
||||
|
||||
bool addEdgePlanes = true;
|
||||
if (addEdgePlanes)
|
||||
{
|
||||
int numVertices=3;
|
||||
int prevVertex = numVertices-1;
|
||||
for (int i=0;i<numVertices;i++)
|
||||
{
|
||||
b3Float4 v0 = verticesA[i];
|
||||
b3Float4 v1 = verticesA[prevVertex];
|
||||
|
||||
b3Float4 edgeNormal = b3Normalized(b3Cross(normal,v1-v0));
|
||||
float c = -b3Dot(edgeNormal,v0);
|
||||
|
||||
facesA[fidx].m_numIndices = 2;
|
||||
facesA[fidx].m_indexOffset=curUsedIndices;
|
||||
indicesA[curUsedIndices++]=i;
|
||||
indicesA[curUsedIndices++]=prevVertex;
|
||||
|
||||
facesA[fidx].m_plane.x = edgeNormal.x;
|
||||
facesA[fidx].m_plane.y = edgeNormal.y;
|
||||
facesA[fidx].m_plane.z = edgeNormal.z;
|
||||
facesA[fidx].m_plane.w = c;
|
||||
fidx++;
|
||||
prevVertex = i;
|
||||
}
|
||||
}
|
||||
convexPolyhedronA.m_numFaces = B3_TRIANGLE_NUM_CONVEX_FACES;
|
||||
convexPolyhedronA.m_localCenter = localCenter*(1.f/3.f);
|
||||
|
||||
|
||||
b3Float4 posA = rigidBodies[bodyIndexA].m_pos;
|
||||
posA.w = 0.f;
|
||||
b3Float4 posB = rigidBodies[bodyIndexB].m_pos;
|
||||
posB.w = 0.f;
|
||||
|
||||
b3Quaternion ornA = rigidBodies[bodyIndexA].m_quat;
|
||||
b3Quaternion ornB =rigidBodies[bodyIndexB].m_quat;
|
||||
|
||||
|
||||
|
||||
|
||||
///////////////////
|
||||
///compound shape support
|
||||
|
||||
if (collidables[collidableIndexB].m_shapeType==SHAPE_COMPOUND_OF_CONVEX_HULLS)
|
||||
{
|
||||
int compoundChild = concavePairs[pairIdx].w;
|
||||
int childShapeIndexB = compoundChild;//collidables[collidableIndexB].m_shapeIndex+compoundChild;
|
||||
int childColIndexB = gpuChildShapes[childShapeIndexB].m_shapeIndex;
|
||||
b3Float4 childPosB = gpuChildShapes[childShapeIndexB].m_childPosition;
|
||||
b3Quaternion childOrnB = gpuChildShapes[childShapeIndexB].m_childOrientation;
|
||||
b3Float4 newPosB = b3TransformPoint(childPosB,posB,ornB);
|
||||
b3Quaternion newOrnB = b3QuatMul(ornB,childOrnB);
|
||||
posB = newPosB;
|
||||
ornB = newOrnB;
|
||||
shapeIndexB = collidables[childColIndexB].m_shapeIndex;
|
||||
}
|
||||
//////////////////
|
||||
|
||||
b3Float4 c0local = convexPolyhedronA.m_localCenter;
|
||||
b3Float4 c0 = b3TransformPoint(c0local, posA, ornA);
|
||||
b3Float4 c1local = convexShapes[shapeIndexB].m_localCenter;
|
||||
b3Float4 c1 = b3TransformPoint(c1local,posB,ornB);
|
||||
const b3Float4 DeltaC2 = c0 - c1;
|
||||
|
||||
|
||||
bool sepA = b3FindSeparatingAxis( &convexPolyhedronA, &convexShapes[shapeIndexB],
|
||||
posA,ornA,
|
||||
posB,ornB,
|
||||
DeltaC2,
|
||||
verticesA,uniqueEdgesA,facesA,indicesA,
|
||||
vertices,uniqueEdges,faces,indices,
|
||||
&sepAxis,&dmin);
|
||||
hasSeparatingAxis = 4;
|
||||
if (!sepA)
|
||||
{
|
||||
hasSeparatingAxis = 0;
|
||||
} else
|
||||
{
|
||||
bool sepB = b3FindSeparatingAxis( &convexShapes[shapeIndexB],&convexPolyhedronA,
|
||||
posB,ornB,
|
||||
posA,ornA,
|
||||
DeltaC2,
|
||||
vertices,uniqueEdges,faces,indices,
|
||||
verticesA,uniqueEdgesA,facesA,indicesA,
|
||||
&sepAxis,&dmin);
|
||||
|
||||
if (!sepB)
|
||||
{
|
||||
hasSeparatingAxis = 0;
|
||||
} else
|
||||
{
|
||||
bool sepEE = b3FindSeparatingAxisEdgeEdge( &convexPolyhedronA, &convexShapes[shapeIndexB],
|
||||
posA,ornA,
|
||||
posB,ornB,
|
||||
DeltaC2,
|
||||
verticesA,uniqueEdgesA,facesA,indicesA,
|
||||
vertices,uniqueEdges,faces,indices,
|
||||
&sepAxis,&dmin);
|
||||
|
||||
if (!sepEE)
|
||||
{
|
||||
hasSeparatingAxis = 0;
|
||||
} else
|
||||
{
|
||||
hasSeparatingAxis = 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (hasSeparatingAxis)
|
||||
{
|
||||
hasSeparatingNormals[i]=1;
|
||||
sepAxis.w = dmin;
|
||||
concaveSeparatingNormalsOut[pairIdx]=sepAxis;
|
||||
|
||||
//now compute clipping faces A and B, and world-space clipping vertices A and B...
|
||||
|
||||
float minDist = -1e30f;
|
||||
float maxDist = 0.02f;
|
||||
|
||||
b3FindClippingFaces(sepAxis,
|
||||
&convexPolyhedronA,
|
||||
&convexShapes[shapeIndexB],
|
||||
posA,ornA,
|
||||
posB,ornB,
|
||||
worldVertsA1Out,
|
||||
worldNormalsA1Out,
|
||||
worldVertsB1Out,
|
||||
vertexFaceCapacity,
|
||||
minDist, maxDist,
|
||||
verticesA,
|
||||
facesA,
|
||||
indicesA,
|
||||
|
||||
vertices,
|
||||
faces,
|
||||
indices,
|
||||
clippingFacesOut, pairIdx);
|
||||
|
||||
} else
|
||||
{
|
||||
//mark this pair as in-active
|
||||
concavePairs[pairIdx].w = -1;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
//mark this pair as in-active
|
||||
concavePairs[pairIdx].w = -1;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
#endif //B3_FIND_CONCAVE_SEPARATING_AXIS_H
|
||||
|
||||
@@ -0,0 +1,90 @@
|
||||
|
||||
|
||||
#ifndef B3_QUANTIZED_BVH_NODE_H
|
||||
#define B3_QUANTIZED_BVH_NODE_H
|
||||
|
||||
#include "Bullet3Common/shared/b3Float4.h"
|
||||
|
||||
#define B3_MAX_NUM_PARTS_IN_BITS 10
|
||||
|
||||
///b3QuantizedBvhNodeData is a compressed aabb node, 16 bytes.
|
||||
///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range).
|
||||
typedef struct b3QuantizedBvhNodeData b3QuantizedBvhNodeData_t;
|
||||
|
||||
struct b3QuantizedBvhNodeData
|
||||
{
|
||||
//12 bytes
|
||||
unsigned short int m_quantizedAabbMin[3];
|
||||
unsigned short int m_quantizedAabbMax[3];
|
||||
//4 bytes
|
||||
int m_escapeIndexOrTriangleIndex;
|
||||
};
|
||||
|
||||
inline int b3GetTriangleIndex(const b3QuantizedBvhNodeData* rootNode)
|
||||
{
|
||||
unsigned int x=0;
|
||||
unsigned int y = (~(x&0))<<(31-B3_MAX_NUM_PARTS_IN_BITS);
|
||||
// Get only the lower bits where the triangle index is stored
|
||||
return (rootNode->m_escapeIndexOrTriangleIndex&~(y));
|
||||
}
|
||||
|
||||
inline int b3IsLeaf(const b3QuantizedBvhNodeData* rootNode)
|
||||
{
|
||||
//skipindex is negative (internal node), triangleindex >=0 (leafnode)
|
||||
return (rootNode->m_escapeIndexOrTriangleIndex >= 0)? 1 : 0;
|
||||
}
|
||||
|
||||
inline int b3GetEscapeIndex(const b3QuantizedBvhNodeData* rootNode)
|
||||
{
|
||||
return -rootNode->m_escapeIndexOrTriangleIndex;
|
||||
}
|
||||
|
||||
inline void b3QuantizeWithClamp(unsigned short* out, b3Float4ConstArg point2,int isMax, b3Float4ConstArg bvhAabbMin, b3Float4ConstArg bvhAabbMax, b3Float4ConstArg bvhQuantization)
|
||||
{
|
||||
b3Float4 clampedPoint = b3MaxFloat4(point2,bvhAabbMin);
|
||||
clampedPoint = b3MinFloat4 (clampedPoint, bvhAabbMax);
|
||||
|
||||
b3Float4 v = (clampedPoint - bvhAabbMin) * bvhQuantization;
|
||||
if (isMax)
|
||||
{
|
||||
out[0] = (unsigned short) (((unsigned short)(v.x+1.f) | 1));
|
||||
out[1] = (unsigned short) (((unsigned short)(v.y+1.f) | 1));
|
||||
out[2] = (unsigned short) (((unsigned short)(v.z+1.f) | 1));
|
||||
} else
|
||||
{
|
||||
out[0] = (unsigned short) (((unsigned short)(v.x) & 0xfffe));
|
||||
out[1] = (unsigned short) (((unsigned short)(v.y) & 0xfffe));
|
||||
out[2] = (unsigned short) (((unsigned short)(v.z) & 0xfffe));
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
inline int b3TestQuantizedAabbAgainstQuantizedAabbSlow(
|
||||
const unsigned short int* aabbMin1,
|
||||
const unsigned short int* aabbMax1,
|
||||
const unsigned short int* aabbMin2,
|
||||
const unsigned short int* aabbMax2)
|
||||
{
|
||||
//int overlap = 1;
|
||||
if (aabbMin1[0] > aabbMax2[0])
|
||||
return 0;
|
||||
if (aabbMax1[0] < aabbMin2[0])
|
||||
return 0;
|
||||
if (aabbMin1[1] > aabbMax2[1])
|
||||
return 0;
|
||||
if (aabbMax1[1] < aabbMin2[1])
|
||||
return 0;
|
||||
if (aabbMin1[2] > aabbMax2[2])
|
||||
return 0;
|
||||
if (aabbMax1[2] < aabbMin2[2])
|
||||
return 0;
|
||||
return 1;
|
||||
//overlap = ((aabbMin1[0] > aabbMax2[0]) || (aabbMax1[0] < aabbMin2[0])) ? 0 : overlap;
|
||||
//overlap = ((aabbMin1[2] > aabbMax2[2]) || (aabbMax1[2] < aabbMin2[2])) ? 0 : overlap;
|
||||
//overlap = ((aabbMin1[1] > aabbMax2[1]) || (aabbMax1[1] < aabbMin2[1])) ? 0 : overlap;
|
||||
//return overlap;
|
||||
}
|
||||
|
||||
|
||||
#endif //B3_QUANTIZED_BVH_NODE_H
|
||||
@@ -10,12 +10,30 @@
|
||||
#define b3Dot3F4 b3Dot
|
||||
#define b3Cross3 b3Cross
|
||||
#define b3MakeFloat4 b3MakeVector3
|
||||
inline b3Vector3 b3Normalized(const b3Vector3& vec)
|
||||
{
|
||||
return vec.normalized();
|
||||
}
|
||||
|
||||
inline b3Float4 b3FastNormalized3(b3Float4ConstArg v)
|
||||
{
|
||||
return v.normalized();
|
||||
}
|
||||
|
||||
inline b3Float4 b3MaxFloat4 (const b3Float4& a, const b3Float4& b)
|
||||
{
|
||||
b3Float4 tmp = a;
|
||||
tmp.setMax(b);
|
||||
return tmp;
|
||||
}
|
||||
inline b3Float4 b3MinFloat4 (const b3Float4& a, const b3Float4& b)
|
||||
{
|
||||
b3Float4 tmp = a;
|
||||
tmp.setMin(b);
|
||||
return tmp;
|
||||
}
|
||||
|
||||
|
||||
|
||||
#else
|
||||
typedef float4 b3Float4;
|
||||
@@ -33,6 +51,11 @@
|
||||
float4 b1 = b3MakeFloat4(v1.xyz,0.f);
|
||||
return cross(a1, b1);
|
||||
}
|
||||
#define b3MinFloat4 min
|
||||
#define b3MaxFloat4 max
|
||||
|
||||
#define b3Normalized(a) normalize(a)
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
@@ -1,8 +1,11 @@
|
||||
#ifndef B3_INT4_H
|
||||
#define B3_INT4_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
|
||||
#include "Bullet3Common/b3Scalar.h"
|
||||
|
||||
|
||||
B3_ATTRIBUTE_ALIGNED16(struct) b3UnsignedInt4
|
||||
{
|
||||
B3_DECLARE_ALIGNED_ALLOCATOR();
|
||||
@@ -51,5 +54,15 @@ B3_FORCE_INLINE b3UnsignedInt4 b3MakeUnsignedInt4(unsigned int x, unsigned int y
|
||||
return v;
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
|
||||
#define b3UnsignedInt4 uint4
|
||||
#define b3Int4 int4
|
||||
#define b3MakeInt4 (int4)
|
||||
#define b3MakeUnsignedInt4 (uint4)
|
||||
|
||||
|
||||
#endif //__cplusplus
|
||||
|
||||
#endif //B3_INT4_H
|
||||
|
||||
@@ -8,9 +8,19 @@ struct MyTest
|
||||
|
||||
#ifdef __cplusplus
|
||||
#define b3AtomicInc(a) ((*a)++)
|
||||
|
||||
inline int b3AtomicAdd (volatile int *p, int val)
|
||||
{
|
||||
int oldValue = *p;
|
||||
int newValue = oldValue+val;
|
||||
*p = newValue;
|
||||
return oldValue;
|
||||
}
|
||||
|
||||
#define __global
|
||||
#else
|
||||
#define b3AtomicInc atomic_inc
|
||||
#define b3AtomicAdd atomic_add
|
||||
#define b3Fabs fabs
|
||||
#define b3Sqrt native_sqrt
|
||||
#define b3Sin native_sin
|
||||
|
||||
@@ -10,6 +10,10 @@
|
||||
|
||||
#define b3Quat b3Quaternion
|
||||
#define b3QuatConstArg const b3Quaternion&
|
||||
inline b3Quat b3QuatInverse(b3QuatConstArg orn)
|
||||
{
|
||||
return orn.inverse();
|
||||
}
|
||||
|
||||
inline b3Float4 b3TransformPoint(b3Float4ConstArg point, b3Float4ConstArg translation, b3QuatConstArg orientation)
|
||||
{
|
||||
|
||||
@@ -6,7 +6,7 @@
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/b3Config.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/b3CpuNarrowPhase.h"
|
||||
#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3CollidableData.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3Collidable.h"
|
||||
#include "Bullet3Common/b3Vector3.h"
|
||||
#include "Bullet3Dynamics/shared/b3ContactConstraint4.h"
|
||||
#include "Bullet3Dynamics/shared/b3Inertia.h"
|
||||
|
||||
@@ -13,6 +13,10 @@ subject to the following restrictions:
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
bool findSeparatingAxisOnGpu = true;
|
||||
|
||||
bool bvhTraversalKernelGPU = true;
|
||||
bool findConcaveSeparatingAxisKernelGPU = true;
|
||||
|
||||
///This file was written by Erwin Coumans
|
||||
///Separating axis rest based on work from Pierre Terdiman, see
|
||||
@@ -65,6 +69,11 @@ typedef b3AlignedObjectArray<b3Vector3> b3VertexArray;
|
||||
#endif
|
||||
|
||||
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhTraversal.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3FindConcaveSatAxis.h"
|
||||
|
||||
|
||||
|
||||
#define dot3F4 b3Dot
|
||||
|
||||
GpuSatCollision::GpuSatCollision(cl_context ctx,cl_device_id device, cl_command_queue q )
|
||||
@@ -76,6 +85,7 @@ m_totalContactsOut(m_context, m_queue),
|
||||
m_sepNormals(m_context, m_queue),
|
||||
m_hasSeparatingNormals(m_context, m_queue),
|
||||
m_concaveSepNormals(m_context, m_queue),
|
||||
m_concaveHasSeparatingNormals(m_context,m_queue),
|
||||
m_numConcavePairsOut(m_context, m_queue),
|
||||
m_gpuCompoundPairs(m_context, m_queue),
|
||||
m_gpuCompoundSepNormals(m_context, m_queue),
|
||||
@@ -1197,7 +1207,7 @@ int clipHullHullSingle(
|
||||
int numPoints = 0;
|
||||
|
||||
{
|
||||
B3_PROFILE("extractManifold");
|
||||
// B3_PROFILE("extractManifold");
|
||||
numPoints = extractManifold(contactsOut, numContactsOut, normalOnSurfaceB, &contactIdx);
|
||||
}
|
||||
|
||||
@@ -2723,6 +2733,9 @@ int computeContactConvexConvex2(
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>* pairs, int nPairs,
|
||||
const b3OpenCLArray<b3RigidBodyCL>* bodyBuf,
|
||||
b3OpenCLArray<b3Contact4>* contactOut, int& nContacts,
|
||||
@@ -2898,15 +2911,13 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
|
||||
hostCollidables[collidableIndexB].m_shapeType == SHAPE_CONVEX_HULL)
|
||||
{
|
||||
//printf("hostPairs[i].z=%d\n",hostPairs[i].z);
|
||||
int contactIndex = computeContactConvexConvex2(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,hostBodyBuf,
|
||||
hostCollidables,hostConvexData,hostVertices,hostUniqueEdges,hostIndices,hostFaces,hostContacts,nContacts,maxContactCapacity,oldHostContacts);
|
||||
//int contactIndex = computeContactConvexConvex(hostPairs,i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,hostBodyBuf,
|
||||
// hostCollidables,hostConvexData,hostVertices,hostUniqueEdges,hostIndices,hostFaces,hostContacts,nContacts,maxContactCapacity,
|
||||
// oldHostContacts);
|
||||
int contactIndex = 0;//computeContactConvexConvex2(i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,hostBodyBuf, hostCollidables,hostConvexData,hostVertices,hostUniqueEdges,hostIndices,hostFaces,hostContacts,nContacts,maxContactCapacity,oldHostContacts);
|
||||
//int contactIndex = computeContactConvexConvex(hostPairs,i,bodyIndexA,bodyIndexB,collidableIndexA,collidableIndexB,hostBodyBuf,hostCollidables,hostConvexData,hostVertices,hostUniqueEdges,hostIndices,hostFaces,hostContacts,nContacts,maxContactCapacity,oldHostContacts);
|
||||
|
||||
|
||||
if (contactIndex>=0)
|
||||
{
|
||||
// printf("convex convex contactIndex = %d\n",contactIndex);
|
||||
hostPairs[i].z = contactIndex;
|
||||
}
|
||||
// printf("plane-convex\n");
|
||||
@@ -2932,7 +2943,8 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
|
||||
contactOut->resize(0);
|
||||
}
|
||||
|
||||
return;
|
||||
m_totalContactsOut.copyFromHostPointer(&nContacts,1,0,true);
|
||||
|
||||
#else
|
||||
|
||||
{
|
||||
@@ -2979,7 +2991,7 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
|
||||
|
||||
int concaveCapacity=maxTriConvexPairCapacity;
|
||||
m_concaveSepNormals.resize(concaveCapacity);
|
||||
|
||||
m_concaveHasSeparatingNormals.resize(concaveCapacity);
|
||||
m_numConcavePairsOut.resize(0);
|
||||
m_numConcavePairsOut.push_back(0);
|
||||
|
||||
@@ -2996,7 +3008,6 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
|
||||
|
||||
int numCompoundPairs = 0;
|
||||
|
||||
bool findSeparatingAxisOnGpu = true;//false;
|
||||
int numConcavePairs =0;
|
||||
|
||||
{
|
||||
@@ -3029,84 +3040,7 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
|
||||
clFinish(m_queue);
|
||||
}
|
||||
|
||||
//now perform the tree query on GPU
|
||||
{
|
||||
|
||||
|
||||
|
||||
{
|
||||
|
||||
if (treeNodesGPU->size() && treeNodesGPU->size())
|
||||
{
|
||||
B3_PROFILE("m_bvhTraversalKernel");
|
||||
|
||||
|
||||
numConcavePairs = m_numConcavePairsOut.at(0);
|
||||
|
||||
b3LauncherCL launcher(m_queue, m_bvhTraversalKernel,"m_bvhTraversalKernel");
|
||||
launcher.setBuffer( pairs->getBufferCL());
|
||||
launcher.setBuffer( bodyBuf->getBufferCL());
|
||||
launcher.setBuffer( gpuCollidables.getBufferCL());
|
||||
launcher.setBuffer( clAabbsWorldSpace.getBufferCL());
|
||||
launcher.setBuffer( triangleConvexPairsOut.getBufferCL());
|
||||
launcher.setBuffer( m_numConcavePairsOut.getBufferCL());
|
||||
launcher.setBuffer( subTreesGPU->getBufferCL());
|
||||
launcher.setBuffer( treeNodesGPU->getBufferCL());
|
||||
launcher.setBuffer( bvhInfo->getBufferCL());
|
||||
|
||||
launcher.setConst( nPairs );
|
||||
launcher.setConst( maxTriConvexPairCapacity);
|
||||
int num = nPairs;
|
||||
launcher.launch1D( num);
|
||||
clFinish(m_queue);
|
||||
numConcavePairs = m_numConcavePairsOut.at(0);
|
||||
//printf("numConcavePairs=%d (max = %d\n",numConcavePairs,maxTriConvexPairCapacity);
|
||||
|
||||
if (numConcavePairs > maxTriConvexPairCapacity)
|
||||
{
|
||||
static int exceeded_maxTriConvexPairCapacity_count = 0;
|
||||
b3Error("Rxceeded %d times the maxTriConvexPairCapacity (found %d but max is %d)\n", exceeded_maxTriConvexPairCapacity_count++,
|
||||
numConcavePairs,maxTriConvexPairCapacity);
|
||||
numConcavePairs = maxTriConvexPairCapacity;
|
||||
}
|
||||
triangleConvexPairsOut.resize(numConcavePairs);
|
||||
if (numConcavePairs)
|
||||
{
|
||||
//now perform a SAT test for each triangle-convex element (stored in triangleConvexPairsOut)
|
||||
B3_PROFILE("findConcaveSeparatingAxisKernel");
|
||||
b3BufferInfoCL bInfo[] = {
|
||||
b3BufferInfoCL( triangleConvexPairsOut.getBufferCL() ),
|
||||
b3BufferInfoCL( bodyBuf->getBufferCL(),true),
|
||||
b3BufferInfoCL( gpuCollidables.getBufferCL(),true),
|
||||
b3BufferInfoCL( convexData.getBufferCL(),true),
|
||||
b3BufferInfoCL( gpuVertices.getBufferCL(),true),
|
||||
b3BufferInfoCL( gpuUniqueEdges.getBufferCL(),true),
|
||||
b3BufferInfoCL( gpuFaces.getBufferCL(),true),
|
||||
b3BufferInfoCL( gpuIndices.getBufferCL(),true),
|
||||
b3BufferInfoCL( gpuChildShapes.getBufferCL(),true),
|
||||
b3BufferInfoCL( clAabbsWorldSpace.getBufferCL(),true),
|
||||
b3BufferInfoCL( m_concaveSepNormals.getBufferCL())
|
||||
};
|
||||
|
||||
b3LauncherCL launcher(m_queue, m_findConcaveSeparatingAxisKernel,"m_findConcaveSeparatingAxisKernel");
|
||||
launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) );
|
||||
|
||||
launcher.setConst( numConcavePairs );
|
||||
|
||||
int num = numConcavePairs;
|
||||
launcher.launch1D( num);
|
||||
clFinish(m_queue);
|
||||
|
||||
// b3AlignedObjectArray<b3Vector3> cpuCompoundSepNormals;
|
||||
// m_concaveSepNormals.copyToHost(cpuCompoundSepNormals);
|
||||
// b3AlignedObjectArray<b3Int4> cpuConcavePairs;
|
||||
// triangleConvexPairsOut.copyToHost(cpuConcavePairs);
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
numCompoundPairs = m_numCompoundPairsOut.at(0);
|
||||
bool useGpuFindCompoundPairs=true;
|
||||
@@ -3325,8 +3259,252 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
|
||||
|
||||
|
||||
}
|
||||
int vertexFaceCapacity = 64;
|
||||
|
||||
|
||||
|
||||
{
|
||||
//now perform the tree query on GPU
|
||||
|
||||
|
||||
|
||||
|
||||
if (treeNodesGPU->size() && treeNodesGPU->size())
|
||||
{
|
||||
if (bvhTraversalKernelGPU)
|
||||
{
|
||||
|
||||
B3_PROFILE("m_bvhTraversalKernel");
|
||||
|
||||
|
||||
numConcavePairs = m_numConcavePairsOut.at(0);
|
||||
|
||||
b3LauncherCL launcher(m_queue, m_bvhTraversalKernel,"m_bvhTraversalKernel");
|
||||
launcher.setBuffer( pairs->getBufferCL());
|
||||
launcher.setBuffer( bodyBuf->getBufferCL());
|
||||
launcher.setBuffer( gpuCollidables.getBufferCL());
|
||||
launcher.setBuffer( clAabbsWorldSpace.getBufferCL());
|
||||
launcher.setBuffer( triangleConvexPairsOut.getBufferCL());
|
||||
launcher.setBuffer( m_numConcavePairsOut.getBufferCL());
|
||||
launcher.setBuffer( subTreesGPU->getBufferCL());
|
||||
launcher.setBuffer( treeNodesGPU->getBufferCL());
|
||||
launcher.setBuffer( bvhInfo->getBufferCL());
|
||||
|
||||
launcher.setConst( nPairs );
|
||||
launcher.setConst( maxTriConvexPairCapacity);
|
||||
int num = nPairs;
|
||||
launcher.launch1D( num);
|
||||
clFinish(m_queue);
|
||||
numConcavePairs = m_numConcavePairsOut.at(0);
|
||||
} else
|
||||
{
|
||||
b3AlignedObjectArray<b3Int4> hostPairs;
|
||||
pairs->copyToHost(hostPairs);
|
||||
b3AlignedObjectArray<b3RigidBodyCL> hostBodyBuf;
|
||||
bodyBuf->copyToHost(hostBodyBuf);
|
||||
b3AlignedObjectArray<b3Collidable> hostCollidables;
|
||||
gpuCollidables.copyToHost(hostCollidables);
|
||||
b3AlignedObjectArray<b3Aabb> hostAabbsWorldSpace;
|
||||
clAabbsWorldSpace.copyToHost(hostAabbsWorldSpace);
|
||||
|
||||
//int maxTriConvexPairCapacity,
|
||||
b3AlignedObjectArray<b3Int4> triangleConvexPairsOutHost;
|
||||
triangleConvexPairsOutHost.resize(maxTriConvexPairCapacity);
|
||||
|
||||
int numTriConvexPairsOutHost=0;
|
||||
numConcavePairs = 0;
|
||||
//m_numConcavePairsOut
|
||||
|
||||
b3AlignedObjectArray<b3QuantizedBvhNode> treeNodesCPU;
|
||||
treeNodesGPU->copyToHost(treeNodesCPU);
|
||||
b3AlignedObjectArray<b3BvhSubtreeInfo> subTreesCPU;
|
||||
subTreesGPU->copyToHost(subTreesCPU);
|
||||
b3AlignedObjectArray<b3BvhInfo> bvhInfoCPU;
|
||||
bvhInfo->copyToHost(bvhInfoCPU);
|
||||
//compute it...
|
||||
|
||||
volatile int hostNumConcavePairsOut=0;
|
||||
|
||||
//
|
||||
for (int i=0;i<nPairs;i++)
|
||||
{
|
||||
b3BvhTraversal( &hostPairs.at(0),
|
||||
&hostBodyBuf.at(0),
|
||||
&hostCollidables.at(0),
|
||||
&hostAabbsWorldSpace.at(0),
|
||||
&triangleConvexPairsOutHost.at(0),
|
||||
&hostNumConcavePairsOut,
|
||||
&subTreesCPU.at(0),
|
||||
&treeNodesCPU.at(0),
|
||||
&bvhInfoCPU.at(0),
|
||||
nPairs,
|
||||
maxTriConvexPairCapacity,
|
||||
i);
|
||||
}
|
||||
numConcavePairs = hostNumConcavePairsOut;
|
||||
|
||||
if (hostNumConcavePairsOut)
|
||||
{
|
||||
triangleConvexPairsOutHost.resize(hostNumConcavePairsOut);
|
||||
triangleConvexPairsOut.copyFromHost(triangleConvexPairsOutHost);
|
||||
}
|
||||
//
|
||||
|
||||
m_numConcavePairsOut.resize(0);
|
||||
m_numConcavePairsOut.push_back(numConcavePairs);
|
||||
}
|
||||
|
||||
//printf("numConcavePairs=%d (max = %d\n",numConcavePairs,maxTriConvexPairCapacity);
|
||||
|
||||
if (numConcavePairs > maxTriConvexPairCapacity)
|
||||
{
|
||||
static int exceeded_maxTriConvexPairCapacity_count = 0;
|
||||
b3Error("Exceeded the maxTriConvexPairCapacity (found %d but max is %d, it happened %d times)\n",
|
||||
numConcavePairs,maxTriConvexPairCapacity,exceeded_maxTriConvexPairCapacity_count++);
|
||||
numConcavePairs = maxTriConvexPairCapacity;
|
||||
}
|
||||
triangleConvexPairsOut.resize(numConcavePairs);
|
||||
|
||||
if (numConcavePairs)
|
||||
{
|
||||
|
||||
|
||||
|
||||
|
||||
clippingFacesOutGPU.resize(numConcavePairs);
|
||||
worldNormalsAGPU.resize(numConcavePairs);
|
||||
worldVertsA1GPU.resize(vertexFaceCapacity*numConcavePairs);
|
||||
worldVertsB1GPU.resize(vertexFaceCapacity*numConcavePairs);
|
||||
|
||||
|
||||
if (findConcaveSeparatingAxisKernelGPU)
|
||||
{
|
||||
|
||||
/*
|
||||
m_concaveHasSeparatingNormals.copyFromHost(concaveHasSeparatingNormalsCPU);
|
||||
clippingFacesOutGPU.copyFromHost(clippingFacesOutCPU);
|
||||
worldVertsA1GPU.copyFromHost(worldVertsA1CPU);
|
||||
worldNormalsAGPU.copyFromHost(worldNormalsACPU);
|
||||
worldVertsB1GPU.copyFromHost(worldVertsB1CPU);
|
||||
*/
|
||||
|
||||
//now perform a SAT test for each triangle-convex element (stored in triangleConvexPairsOut)
|
||||
B3_PROFILE("findConcaveSeparatingAxisKernel");
|
||||
b3BufferInfoCL bInfo[] = {
|
||||
b3BufferInfoCL( triangleConvexPairsOut.getBufferCL() ),
|
||||
b3BufferInfoCL( bodyBuf->getBufferCL(),true),
|
||||
b3BufferInfoCL( gpuCollidables.getBufferCL(),true),
|
||||
b3BufferInfoCL( convexData.getBufferCL(),true),
|
||||
b3BufferInfoCL( gpuVertices.getBufferCL(),true),
|
||||
b3BufferInfoCL( gpuUniqueEdges.getBufferCL(),true),
|
||||
b3BufferInfoCL( gpuFaces.getBufferCL(),true),
|
||||
b3BufferInfoCL( gpuIndices.getBufferCL(),true),
|
||||
b3BufferInfoCL( gpuChildShapes.getBufferCL(),true),
|
||||
b3BufferInfoCL( clAabbsWorldSpace.getBufferCL(),true),
|
||||
b3BufferInfoCL( m_concaveSepNormals.getBufferCL()),
|
||||
b3BufferInfoCL( m_concaveHasSeparatingNormals.getBufferCL()),
|
||||
b3BufferInfoCL( clippingFacesOutGPU.getBufferCL()),
|
||||
b3BufferInfoCL( worldVertsA1GPU.getBufferCL()),
|
||||
b3BufferInfoCL(worldNormalsAGPU.getBufferCL()),
|
||||
b3BufferInfoCL(worldVertsB1GPU.getBufferCL())
|
||||
};
|
||||
|
||||
b3LauncherCL launcher(m_queue, m_findConcaveSeparatingAxisKernel,"m_findConcaveSeparatingAxisKernel");
|
||||
launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) );
|
||||
launcher.setConst(vertexFaceCapacity);
|
||||
launcher.setConst( numConcavePairs );
|
||||
|
||||
int num = numConcavePairs;
|
||||
launcher.launch1D( num);
|
||||
clFinish(m_queue);
|
||||
} else
|
||||
{
|
||||
|
||||
b3AlignedObjectArray<b3Int4> clippingFacesOutCPU;
|
||||
b3AlignedObjectArray<b3Vector3> worldVertsA1CPU;
|
||||
b3AlignedObjectArray<b3Vector3> worldNormalsACPU;
|
||||
b3AlignedObjectArray<b3Vector3> worldVertsB1CPU;
|
||||
b3AlignedObjectArray<int>concaveHasSeparatingNormalsCPU;
|
||||
|
||||
b3AlignedObjectArray<b3Int4> triangleConvexPairsOutHost;
|
||||
triangleConvexPairsOut.copyToHost(triangleConvexPairsOutHost);
|
||||
//triangleConvexPairsOutHost.resize(maxTriConvexPairCapacity);
|
||||
b3AlignedObjectArray<b3RigidBodyCL> hostBodyBuf;
|
||||
bodyBuf->copyToHost(hostBodyBuf);
|
||||
b3AlignedObjectArray<b3Collidable> hostCollidables;
|
||||
gpuCollidables.copyToHost(hostCollidables);
|
||||
b3AlignedObjectArray<b3Aabb> hostAabbsWorldSpace;
|
||||
clAabbsWorldSpace.copyToHost(hostAabbsWorldSpace);
|
||||
|
||||
b3AlignedObjectArray<b3ConvexPolyhedronCL> hostConvexData;
|
||||
convexData.copyToHost(hostConvexData);
|
||||
|
||||
b3AlignedObjectArray<b3Vector3> hostVertices;
|
||||
gpuVertices.copyToHost(hostVertices);
|
||||
|
||||
b3AlignedObjectArray<b3Vector3> hostUniqueEdges;
|
||||
gpuUniqueEdges.copyToHost(hostUniqueEdges);
|
||||
b3AlignedObjectArray<b3GpuFace> hostFaces;
|
||||
gpuFaces.copyToHost(hostFaces);
|
||||
b3AlignedObjectArray<int> hostIndices;
|
||||
gpuIndices.copyToHost(hostIndices);
|
||||
b3AlignedObjectArray<b3GpuChildShape> cpuChildShapes;
|
||||
gpuChildShapes.copyToHost(cpuChildShapes);
|
||||
|
||||
|
||||
|
||||
b3AlignedObjectArray<b3Vector3> concaveSepNormalsHost;
|
||||
m_concaveSepNormals.copyToHost(concaveSepNormalsHost);
|
||||
concaveHasSeparatingNormalsCPU.resize(concaveSepNormalsHost.size());
|
||||
|
||||
b3GpuChildShape* childShapePointerCPU = 0;
|
||||
if (cpuChildShapes.size())
|
||||
childShapePointerCPU = &cpuChildShapes.at(0);
|
||||
|
||||
clippingFacesOutCPU.resize(clippingFacesOutGPU.size());
|
||||
worldVertsA1CPU.resize(worldVertsA1GPU.size());
|
||||
worldNormalsACPU.resize(worldNormalsAGPU.size());
|
||||
worldVertsB1CPU.resize(worldVertsB1GPU.size());
|
||||
|
||||
for (int i=0;i<numConcavePairs;i++)
|
||||
{
|
||||
b3FindConcaveSeparatingAxisKernel(&triangleConvexPairsOutHost.at(0),
|
||||
&hostBodyBuf.at(0),
|
||||
&hostCollidables.at(0),
|
||||
&hostConvexData.at(0), &hostVertices.at(0),&hostUniqueEdges.at(0),
|
||||
&hostFaces.at(0),&hostIndices.at(0),childShapePointerCPU,
|
||||
&hostAabbsWorldSpace.at(0),
|
||||
&concaveSepNormalsHost.at(0),
|
||||
&clippingFacesOutCPU.at(0),
|
||||
&worldVertsA1CPU.at(0),
|
||||
&worldNormalsACPU.at(0),
|
||||
&worldVertsB1CPU.at(0),
|
||||
&concaveHasSeparatingNormalsCPU.at(0),
|
||||
vertexFaceCapacity,
|
||||
numConcavePairs,i);
|
||||
};
|
||||
|
||||
m_concaveSepNormals.copyFromHost(concaveSepNormalsHost);
|
||||
m_concaveHasSeparatingNormals.copyFromHost(concaveHasSeparatingNormalsCPU);
|
||||
clippingFacesOutGPU.copyFromHost(clippingFacesOutCPU);
|
||||
worldVertsA1GPU.copyFromHost(worldVertsA1CPU);
|
||||
worldNormalsAGPU.copyFromHost(worldNormalsACPU);
|
||||
worldVertsB1GPU.copyFromHost(worldVertsB1CPU);
|
||||
|
||||
|
||||
|
||||
}
|
||||
// b3AlignedObjectArray<b3Vector3> cpuCompoundSepNormals;
|
||||
// m_concaveSepNormals.copyToHost(cpuCompoundSepNormals);
|
||||
// b3AlignedObjectArray<b3Int4> cpuConcavePairs;
|
||||
// triangleConvexPairsOut.copyToHost(cpuConcavePairs);
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
if (numConcavePairs)
|
||||
{
|
||||
@@ -3377,45 +3555,130 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
|
||||
|
||||
if (contactClippingOnGpu)
|
||||
{
|
||||
//B3_PROFILE("clipHullHullKernel");
|
||||
|
||||
|
||||
m_totalContactsOut.copyFromHostPointer(&nContacts,1,0,true);
|
||||
|
||||
//concave-convex contact clipping
|
||||
//B3_PROFILE("clipHullHullKernel");
|
||||
|
||||
bool breakupConcaveConvexKernel = false;
|
||||
|
||||
#ifdef __APPLE__
|
||||
//actually, some Apple OpenCL platform/device combinations work fine...
|
||||
breakupConcaveConvexKernel = true;
|
||||
#endif
|
||||
//concave-convex contact clipping
|
||||
if (numConcavePairs)
|
||||
{
|
||||
// printf("numConcavePairs = %d\n", numConcavePairs);
|
||||
// nContacts = m_totalContactsOut.at(0);
|
||||
// printf("nContacts before = %d\n", nContacts);
|
||||
|
||||
B3_PROFILE("clipHullHullConcaveConvexKernel");
|
||||
nContacts = m_totalContactsOut.at(0);
|
||||
b3BufferInfoCL bInfo[] = {
|
||||
b3BufferInfoCL( triangleConvexPairsOut.getBufferCL(), true ),
|
||||
b3BufferInfoCL( bodyBuf->getBufferCL(),true),
|
||||
b3BufferInfoCL( gpuCollidables.getBufferCL(),true),
|
||||
b3BufferInfoCL( convexData.getBufferCL(),true),
|
||||
b3BufferInfoCL( gpuVertices.getBufferCL(),true),
|
||||
b3BufferInfoCL( gpuUniqueEdges.getBufferCL(),true),
|
||||
b3BufferInfoCL( gpuFaces.getBufferCL(),true),
|
||||
b3BufferInfoCL( gpuIndices.getBufferCL(),true),
|
||||
b3BufferInfoCL( gpuChildShapes.getBufferCL(),true),
|
||||
b3BufferInfoCL( m_concaveSepNormals.getBufferCL()),
|
||||
b3BufferInfoCL( contactOut->getBufferCL()),
|
||||
b3BufferInfoCL( m_totalContactsOut.getBufferCL())
|
||||
};
|
||||
b3LauncherCL launcher(m_queue, m_clipHullHullConcaveConvexKernel,"m_clipHullHullConcaveConvexKernel");
|
||||
launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) );
|
||||
launcher.setConst( numConcavePairs );
|
||||
int num = numConcavePairs;
|
||||
launcher.launch1D( num);
|
||||
clFinish(m_queue);
|
||||
nContacts = m_totalContactsOut.at(0);
|
||||
contactOut->resize(nContacts);
|
||||
b3AlignedObjectArray<b3Contact4> cpuContacts;
|
||||
contactOut->copyToHost(cpuContacts);
|
||||
if (breakupConcaveConvexKernel)
|
||||
{
|
||||
|
||||
worldVertsB2GPU.resize(vertexFaceCapacity*numConcavePairs);
|
||||
|
||||
|
||||
//clipFacesAndFindContacts
|
||||
bool clipFacesAndFindContactsCPU = false;
|
||||
if (clipFacesAndFindContactsCPU)
|
||||
{
|
||||
} else
|
||||
{
|
||||
|
||||
if (1)
|
||||
{
|
||||
|
||||
|
||||
B3_PROFILE("clipFacesAndFindContacts");
|
||||
//nContacts = m_totalContactsOut.at(0);
|
||||
//int h = m_hasSeparatingNormals.at(0);
|
||||
//int4 p = clippingFacesOutGPU.at(0);
|
||||
b3BufferInfoCL bInfo[] = {
|
||||
b3BufferInfoCL( m_concaveSepNormals.getBufferCL()),
|
||||
b3BufferInfoCL( m_concaveHasSeparatingNormals.getBufferCL()),
|
||||
b3BufferInfoCL( contactOut->getBufferCL()),
|
||||
b3BufferInfoCL( clippingFacesOutGPU.getBufferCL()),
|
||||
b3BufferInfoCL( worldVertsA1GPU.getBufferCL()),
|
||||
b3BufferInfoCL( worldNormalsAGPU.getBufferCL()),
|
||||
b3BufferInfoCL( worldVertsB1GPU.getBufferCL()),
|
||||
b3BufferInfoCL( worldVertsB2GPU.getBufferCL()),
|
||||
b3BufferInfoCL( m_totalContactsOut.getBufferCL())
|
||||
};
|
||||
b3LauncherCL launcher(m_queue, m_clipFacesAndFindContacts,"m_clipFacesAndFindContacts");
|
||||
launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) );
|
||||
launcher.setConst(vertexFaceCapacity);
|
||||
|
||||
launcher.setConst( numConcavePairs );
|
||||
int debugMode = 0;
|
||||
launcher.setConst( debugMode);
|
||||
int num = numConcavePairs;
|
||||
launcher.launch1D( num);
|
||||
clFinish(m_queue);
|
||||
//int bla = m_totalContactsOut.at(0);
|
||||
}
|
||||
}
|
||||
//contactReduction
|
||||
{
|
||||
contactOut->reserve(nContacts+numConcavePairs);
|
||||
|
||||
{
|
||||
B3_PROFILE("newContactReductionKernel");
|
||||
b3BufferInfoCL bInfo[] =
|
||||
{
|
||||
b3BufferInfoCL( triangleConvexPairsOut.getBufferCL(), true ),
|
||||
b3BufferInfoCL( bodyBuf->getBufferCL(),true),
|
||||
b3BufferInfoCL( m_concaveSepNormals.getBufferCL()),
|
||||
b3BufferInfoCL( m_concaveHasSeparatingNormals.getBufferCL()),
|
||||
b3BufferInfoCL( contactOut->getBufferCL()),
|
||||
b3BufferInfoCL( clippingFacesOutGPU.getBufferCL()),
|
||||
b3BufferInfoCL( worldVertsB2GPU.getBufferCL()),
|
||||
b3BufferInfoCL( m_totalContactsOut.getBufferCL())
|
||||
};
|
||||
|
||||
b3LauncherCL launcher(m_queue, m_newContactReductionKernel,"m_newContactReductionKernel");
|
||||
launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) );
|
||||
launcher.setConst(vertexFaceCapacity);
|
||||
launcher.setConst( numConcavePairs );
|
||||
int num = numConcavePairs;
|
||||
|
||||
launcher.launch1D( num);
|
||||
}
|
||||
nContacts = m_totalContactsOut.at(0);
|
||||
contactOut->resize(nContacts);
|
||||
|
||||
}
|
||||
//re-use?
|
||||
|
||||
|
||||
} else
|
||||
{
|
||||
B3_PROFILE("clipHullHullConcaveConvexKernel");
|
||||
nContacts = m_totalContactsOut.at(0);
|
||||
b3BufferInfoCL bInfo[] = {
|
||||
b3BufferInfoCL( triangleConvexPairsOut.getBufferCL(), true ),
|
||||
b3BufferInfoCL( bodyBuf->getBufferCL(),true),
|
||||
b3BufferInfoCL( gpuCollidables.getBufferCL(),true),
|
||||
b3BufferInfoCL( convexData.getBufferCL(),true),
|
||||
b3BufferInfoCL( gpuVertices.getBufferCL(),true),
|
||||
b3BufferInfoCL( gpuUniqueEdges.getBufferCL(),true),
|
||||
b3BufferInfoCL( gpuFaces.getBufferCL(),true),
|
||||
b3BufferInfoCL( gpuIndices.getBufferCL(),true),
|
||||
b3BufferInfoCL( gpuChildShapes.getBufferCL(),true),
|
||||
b3BufferInfoCL( m_concaveSepNormals.getBufferCL()),
|
||||
b3BufferInfoCL( contactOut->getBufferCL()),
|
||||
b3BufferInfoCL( m_totalContactsOut.getBufferCL())
|
||||
};
|
||||
b3LauncherCL launcher(m_queue, m_clipHullHullConcaveConvexKernel,"m_clipHullHullConcaveConvexKernel");
|
||||
launcher.setBuffers( bInfo, sizeof(bInfo)/sizeof(b3BufferInfoCL) );
|
||||
launcher.setConst( numConcavePairs );
|
||||
int num = numConcavePairs;
|
||||
launcher.launch1D( num);
|
||||
clFinish(m_queue);
|
||||
nContacts = m_totalContactsOut.at(0);
|
||||
contactOut->resize(nContacts);
|
||||
b3AlignedObjectArray<b3Contact4> cpuContacts;
|
||||
contactOut->copyToHost(cpuContacts);
|
||||
}
|
||||
// printf("nContacts after = %d\n", nContacts);
|
||||
}
|
||||
|
||||
@@ -3436,24 +3699,12 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
|
||||
|
||||
|
||||
|
||||
int vertexFaceCapacity = 64;
|
||||
|
||||
|
||||
worldVertsB1GPU.resize(vertexFaceCapacity*nPairs);
|
||||
|
||||
|
||||
clippingFacesOutGPU.resize(nPairs);
|
||||
|
||||
|
||||
worldNormalsAGPU.resize(nPairs);
|
||||
|
||||
|
||||
worldVertsA1GPU.resize(vertexFaceCapacity*nPairs);
|
||||
|
||||
|
||||
worldVertsB2GPU.resize(vertexFaceCapacity*nPairs);
|
||||
|
||||
|
||||
|
||||
{
|
||||
B3_PROFILE("findClippingFacesKernel");
|
||||
@@ -3491,13 +3742,11 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
|
||||
///clip face B against face A, reduce contacts and append them to a global contact array
|
||||
if (1)
|
||||
{
|
||||
B3_PROFILE("clipFacesAndContactReductionKernel");
|
||||
B3_PROFILE("clipFacesAndFindContacts");
|
||||
//nContacts = m_totalContactsOut.at(0);
|
||||
//int h = m_hasSeparatingNormals.at(0);
|
||||
//int4 p = clippingFacesOutGPU.at(0);
|
||||
b3BufferInfoCL bInfo[] = {
|
||||
b3BufferInfoCL( pairs->getBufferCL(), true ),
|
||||
b3BufferInfoCL( bodyBuf->getBufferCL(),true),
|
||||
b3BufferInfoCL( m_sepNormals.getBufferCL()),
|
||||
b3BufferInfoCL( m_hasSeparatingNormals.getBufferCL()),
|
||||
b3BufferInfoCL( contactOut->getBufferCL()),
|
||||
@@ -3516,23 +3765,11 @@ void GpuSatCollision::computeConvexConvexContactsGPUSAT( b3OpenCLArray<b3Int4>*
|
||||
launcher.setConst( nPairs );
|
||||
int debugMode = 0;
|
||||
launcher.setConst( debugMode);
|
||||
|
||||
/*
|
||||
int serializationBytes = launcher.getSerializationBufferSize();
|
||||
unsigned char* buf = (unsigned char*)malloc(serializationBytes+1);
|
||||
int actualWritten = launcher.serializeArguments(buf,serializationBytes+1);
|
||||
FILE* f = fopen("clipFacesAndContactReductionKernel.bin","wb");
|
||||
fwrite(buf,actualWritten,1,f);
|
||||
fclose(f);
|
||||
free(buf);
|
||||
printf("serializationBytes=%d, actualWritten=%d\n",serializationBytes,actualWritten);
|
||||
*/
|
||||
|
||||
int num = nPairs;
|
||||
|
||||
launcher.launch1D( num);
|
||||
int num = nPairs;
|
||||
launcher.launch1D( num);
|
||||
clFinish(m_queue);
|
||||
{
|
||||
|
||||
{
|
||||
// nContacts = m_totalContactsOut.at(0);
|
||||
// printf("nContacts = %d\n",nContacts);
|
||||
|
||||
|
||||
@@ -52,6 +52,7 @@ struct GpuSatCollision
|
||||
b3OpenCLArray<b3Vector3> m_sepNormals;
|
||||
b3OpenCLArray<int> m_hasSeparatingNormals;
|
||||
b3OpenCLArray<b3Vector3> m_concaveSepNormals;
|
||||
b3OpenCLArray<int> m_concaveHasSeparatingNormals;
|
||||
b3OpenCLArray<int> m_numConcavePairsOut;
|
||||
b3OpenCLArray<b3CompoundOverlappingPair> m_gpuCompoundPairs;
|
||||
b3OpenCLArray<b3Vector3> m_gpuCompoundSepNormals;
|
||||
|
||||
@@ -41,6 +41,9 @@ class b3Serializer;
|
||||
#define b3QuantizedBvhDataName "b3QuantizedBvhFloatData"
|
||||
#endif
|
||||
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3QuantizedBvhNodeData.h"
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/shared/b3BvhSubtreeInfoData.h"
|
||||
|
||||
|
||||
|
||||
//http://msdn.microsoft.com/library/default.asp?url=/library/en-us/vclang/html/vclrf__m128.asp
|
||||
@@ -55,16 +58,10 @@ class b3Serializer;
|
||||
|
||||
///b3QuantizedBvhNode is a compressed aabb node, 16 bytes.
|
||||
///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range).
|
||||
B3_ATTRIBUTE_ALIGNED16 (struct) b3QuantizedBvhNode
|
||||
B3_ATTRIBUTE_ALIGNED16 (struct) b3QuantizedBvhNode : public b3QuantizedBvhNodeData
|
||||
{
|
||||
B3_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
//12 bytes
|
||||
unsigned short int m_quantizedAabbMin[3];
|
||||
unsigned short int m_quantizedAabbMax[3];
|
||||
//4 bytes
|
||||
int m_escapeIndexOrTriangleIndex;
|
||||
|
||||
bool isLeafNode() const
|
||||
{
|
||||
//skipindex is negative (internal node), triangleindex >=0 (leafnode)
|
||||
@@ -116,20 +113,11 @@ B3_ATTRIBUTE_ALIGNED16 (struct) b3OptimizedBvhNode
|
||||
|
||||
|
||||
///b3BvhSubtreeInfo provides info to gather a subtree of limited size
|
||||
B3_ATTRIBUTE_ALIGNED16(class) b3BvhSubtreeInfo
|
||||
B3_ATTRIBUTE_ALIGNED16(class) b3BvhSubtreeInfo : public b3BvhSubtreeInfoData
|
||||
{
|
||||
public:
|
||||
B3_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
//12 bytes
|
||||
unsigned short int m_quantizedAabbMin[3];
|
||||
unsigned short int m_quantizedAabbMax[3];
|
||||
//4 bytes, points to the root of the subtree
|
||||
int m_rootNodeIndex;
|
||||
//4 bytes
|
||||
int m_subtreeSize;
|
||||
int m_padding[3];
|
||||
|
||||
b3BvhSubtreeInfo()
|
||||
{
|
||||
//memset(&m_padding[0], 0, sizeof(m_padding));
|
||||
@@ -501,14 +489,6 @@ private:
|
||||
;
|
||||
|
||||
|
||||
struct b3BvhSubtreeInfoData
|
||||
{
|
||||
int m_rootNodeIndex;
|
||||
int m_subtreeSize;
|
||||
unsigned short m_quantizedAabbMin[3];
|
||||
unsigned short m_quantizedAabbMax[3];
|
||||
};
|
||||
|
||||
struct b3OptimizedBvhNodeFloatData
|
||||
{
|
||||
b3Vector3FloatData m_aabbMinOrg;
|
||||
@@ -530,12 +510,6 @@ struct b3OptimizedBvhNodeDoubleData
|
||||
};
|
||||
|
||||
|
||||
struct b3QuantizedBvhNodeData
|
||||
{
|
||||
unsigned short m_quantizedAabbMin[3];
|
||||
unsigned short m_quantizedAabbMax[3];
|
||||
int m_escapeIndexOrTriangleIndex;
|
||||
};
|
||||
|
||||
struct b3QuantizedBvhFloatData
|
||||
{
|
||||
|
||||
@@ -34,33 +34,6 @@ typedef struct
|
||||
|
||||
} b3BvhInfo;
|
||||
|
||||
/*
|
||||
bool isLeafNode() const
|
||||
{
|
||||
//skipindex is negative (internal node), triangleindex >=0 (leafnode)
|
||||
return (m_escapeIndexOrTriangleIndex >= 0);
|
||||
}
|
||||
int getEscapeIndex() const
|
||||
{
|
||||
btAssert(!isLeafNode());
|
||||
return -m_escapeIndexOrTriangleIndex;
|
||||
}
|
||||
int getTriangleIndex() const
|
||||
{
|
||||
btAssert(isLeafNode());
|
||||
unsigned int x=0;
|
||||
unsigned int y = (~(x&0))<<(31-MAX_NUM_PARTS_IN_BITS);
|
||||
// Get only the lower bits where the triangle index is stored
|
||||
return (m_escapeIndexOrTriangleIndex&~(y));
|
||||
}
|
||||
int getPartId() const
|
||||
{
|
||||
btAssert(isLeafNode());
|
||||
// Get only the highest bits where the part index is stored
|
||||
return (m_escapeIndexOrTriangleIndex>>(31-MAX_NUM_PARTS_IN_BITS));
|
||||
}
|
||||
*/
|
||||
|
||||
int getTriangleIndex(const btQuantizedBvhNode* rootNode)
|
||||
{
|
||||
unsigned int x=0;
|
||||
|
||||
@@ -29,32 +29,6 @@ static const char* bvhTraversalKernelCL= \
|
||||
" int m_nodeOffset;\n"
|
||||
" int m_subTreeOffset;\n"
|
||||
"} b3BvhInfo;\n"
|
||||
"/*\n"
|
||||
" bool isLeafNode() const\n"
|
||||
" {\n"
|
||||
" //skipindex is negative (internal node), triangleindex >=0 (leafnode)\n"
|
||||
" return (m_escapeIndexOrTriangleIndex >= 0);\n"
|
||||
" }\n"
|
||||
" int getEscapeIndex() const\n"
|
||||
" {\n"
|
||||
" btAssert(!isLeafNode());\n"
|
||||
" return -m_escapeIndexOrTriangleIndex;\n"
|
||||
" }\n"
|
||||
" int getTriangleIndex() const\n"
|
||||
" {\n"
|
||||
" btAssert(isLeafNode());\n"
|
||||
" unsigned int x=0;\n"
|
||||
" unsigned int y = (~(x&0))<<(31-MAX_NUM_PARTS_IN_BITS);\n"
|
||||
" // Get only the lower bits where the triangle index is stored\n"
|
||||
" return (m_escapeIndexOrTriangleIndex&~(y));\n"
|
||||
" }\n"
|
||||
" int getPartId() const\n"
|
||||
" {\n"
|
||||
" btAssert(isLeafNode());\n"
|
||||
" // Get only the highest bits where the part index is stored\n"
|
||||
" return (m_escapeIndexOrTriangleIndex>>(31-MAX_NUM_PARTS_IN_BITS));\n"
|
||||
" }\n"
|
||||
"*/\n"
|
||||
"int getTriangleIndex(const btQuantizedBvhNode* rootNode)\n"
|
||||
"{\n"
|
||||
" unsigned int x=0;\n"
|
||||
|
||||
@@ -13,6 +13,7 @@ static const char* primitiveContactsKernelsCL= \
|
||||
"#ifdef __cplusplus\n"
|
||||
"#else\n"
|
||||
"#define b3AtomicInc atomic_inc\n"
|
||||
"#define b3AtomicAdd atomic_add\n"
|
||||
"#define b3Fabs fabs\n"
|
||||
"#define b3Sqrt native_sqrt\n"
|
||||
"#define b3Sin native_sin\n"
|
||||
@@ -36,6 +37,9 @@ static const char* primitiveContactsKernelsCL= \
|
||||
" float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n"
|
||||
" return cross(a1, b1);\n"
|
||||
" }\n"
|
||||
" #define b3MinFloat4 min\n"
|
||||
" #define b3MaxFloat4 max\n"
|
||||
" #define b3Normalized(a) normalize(a)\n"
|
||||
"#endif \n"
|
||||
" \n"
|
||||
"inline bool b3IsAlmostZero(b3Float4ConstArg v)\n"
|
||||
|
||||
@@ -401,7 +401,7 @@ bool findSeparatingAxisLocalA( const ConvexPolyhedronCL* hullA, __global const C
|
||||
float4* sep,
|
||||
float* dmin)
|
||||
{
|
||||
int i = get_global_id(0);
|
||||
|
||||
|
||||
float4 posA = posA1;
|
||||
posA.w = 0.f;
|
||||
@@ -452,7 +452,7 @@ bool findSeparatingAxisLocalB( __global const ConvexPolyhedronCL* hullA, const
|
||||
float4* sep,
|
||||
float* dmin)
|
||||
{
|
||||
int i = get_global_id(0);
|
||||
|
||||
|
||||
float4 posA = posA1;
|
||||
posA.w = 0.f;
|
||||
@@ -505,7 +505,7 @@ bool findSeparatingAxisEdgeEdgeLocalA( const ConvexPolyhedronCL* hullA, __global
|
||||
float4* sep,
|
||||
float* dmin)
|
||||
{
|
||||
int i = get_global_id(0);
|
||||
|
||||
|
||||
float4 posA = posA1;
|
||||
posA.w = 0.f;
|
||||
@@ -607,7 +607,7 @@ bool findSeparatingAxis( __global const ConvexPolyhedronCL* hullA, __global cons
|
||||
float4* sep,
|
||||
float* dmin)
|
||||
{
|
||||
int i = get_global_id(0);
|
||||
|
||||
|
||||
float4 posA = posA1;
|
||||
posA.w = 0.f;
|
||||
@@ -666,7 +666,7 @@ bool findSeparatingAxisEdgeEdge( __global const ConvexPolyhedronCL* hullA, __glo
|
||||
float4* sep,
|
||||
float* dmin)
|
||||
{
|
||||
int i = get_global_id(0);
|
||||
|
||||
|
||||
float4 posA = posA1;
|
||||
posA.w = 0.f;
|
||||
@@ -1353,6 +1353,97 @@ __kernel void findSeparatingAxisKernel( __global const int4* pairs,
|
||||
|
||||
|
||||
|
||||
|
||||
int findClippingFaces(const float4 separatingNormal,
|
||||
const ConvexPolyhedronCL* hullA,
|
||||
__global const ConvexPolyhedronCL* hullB,
|
||||
const float4 posA, const Quaternion ornA,const float4 posB, const Quaternion ornB,
|
||||
__global float4* worldVertsA1,
|
||||
__global float4* worldNormalsA1,
|
||||
__global float4* worldVertsB1,
|
||||
int capacityWorldVerts,
|
||||
const float minDist, float maxDist,
|
||||
const float4* verticesA,
|
||||
const btGpuFace* facesA,
|
||||
const int* indicesA,
|
||||
__global const float4* verticesB,
|
||||
__global const btGpuFace* facesB,
|
||||
__global const int* indicesB,
|
||||
__global int4* clippingFaces, int pairIndex)
|
||||
{
|
||||
int numContactsOut = 0;
|
||||
int numWorldVertsB1= 0;
|
||||
|
||||
|
||||
int closestFaceB=-1;
|
||||
float dmax = -FLT_MAX;
|
||||
|
||||
{
|
||||
for(int face=0;face<hullB->m_numFaces;face++)
|
||||
{
|
||||
const float4 Normal = make_float4(facesB[hullB->m_faceOffset+face].m_plane.x,
|
||||
facesB[hullB->m_faceOffset+face].m_plane.y, facesB[hullB->m_faceOffset+face].m_plane.z,0.f);
|
||||
const float4 WorldNormal = qtRotate(ornB, Normal);
|
||||
float d = dot3F4(WorldNormal,separatingNormal);
|
||||
if (d > dmax)
|
||||
{
|
||||
dmax = d;
|
||||
closestFaceB = face;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
const btGpuFace polyB = facesB[hullB->m_faceOffset+closestFaceB];
|
||||
const int numVertices = polyB.m_numIndices;
|
||||
for(int e0=0;e0<numVertices;e0++)
|
||||
{
|
||||
const float4 b = verticesB[hullB->m_vertexOffset+indicesB[polyB.m_indexOffset+e0]];
|
||||
worldVertsB1[pairIndex*capacityWorldVerts+numWorldVertsB1++] = transform(&b,&posB,&ornB);
|
||||
}
|
||||
}
|
||||
|
||||
int closestFaceA=-1;
|
||||
{
|
||||
float dmin = FLT_MAX;
|
||||
for(int face=0;face<hullA->m_numFaces;face++)
|
||||
{
|
||||
const float4 Normal = make_float4(
|
||||
facesA[hullA->m_faceOffset+face].m_plane.x,
|
||||
facesA[hullA->m_faceOffset+face].m_plane.y,
|
||||
facesA[hullA->m_faceOffset+face].m_plane.z,
|
||||
0.f);
|
||||
const float4 faceANormalWS = qtRotate(ornA,Normal);
|
||||
|
||||
float d = dot3F4(faceANormalWS,separatingNormal);
|
||||
if (d < dmin)
|
||||
{
|
||||
dmin = d;
|
||||
closestFaceA = face;
|
||||
worldNormalsA1[pairIndex] = faceANormalWS;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int numVerticesA = facesA[hullA->m_faceOffset+closestFaceA].m_numIndices;
|
||||
for(int e0=0;e0<numVerticesA;e0++)
|
||||
{
|
||||
const float4 a = verticesA[hullA->m_vertexOffset+indicesA[facesA[hullA->m_faceOffset+closestFaceA].m_indexOffset+e0]];
|
||||
worldVertsA1[pairIndex*capacityWorldVerts+e0] = transform(&a, &posA,&ornA);
|
||||
}
|
||||
|
||||
clippingFaces[pairIndex].x = closestFaceA;
|
||||
clippingFaces[pairIndex].y = closestFaceB;
|
||||
clippingFaces[pairIndex].z = numVerticesA;
|
||||
clippingFaces[pairIndex].w = numWorldVertsB1;
|
||||
|
||||
|
||||
return numContactsOut;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
// work-in-progress
|
||||
__kernel void findConcaveSeparatingAxisKernel( __global int4* concavePairs,
|
||||
__global const BodyData* rigidBodies,
|
||||
@@ -1365,6 +1456,12 @@ __kernel void findConcaveSeparatingAxisKernel( __global int4* concavePairs,
|
||||
__global const btGpuChildShape* gpuChildShapes,
|
||||
__global btAabbCL* aabbs,
|
||||
__global float4* concaveSeparatingNormalsOut,
|
||||
__global int* concaveHasSeparatingNormals,
|
||||
__global int4* clippingFacesOut,
|
||||
__global float4* worldVertsA1GPU,
|
||||
__global float4* worldNormalsAGPU,
|
||||
__global float4* worldVertsB1GPU,
|
||||
int vertexFaceCapacity,
|
||||
int numConcavePairs
|
||||
)
|
||||
{
|
||||
@@ -1372,6 +1469,9 @@ __kernel void findConcaveSeparatingAxisKernel( __global int4* concavePairs,
|
||||
int i = get_global_id(0);
|
||||
if (i>=numConcavePairs)
|
||||
return;
|
||||
|
||||
concaveHasSeparatingNormals[i] = 0;
|
||||
|
||||
int pairIdx = i;
|
||||
|
||||
int bodyIndexA = concavePairs[i].x;
|
||||
@@ -1604,6 +1704,33 @@ __kernel void findConcaveSeparatingAxisKernel( __global int4* concavePairs,
|
||||
{
|
||||
sepAxis.w = dmin;
|
||||
concaveSeparatingNormalsOut[pairIdx]=sepAxis;
|
||||
concaveHasSeparatingNormals[i]=1;
|
||||
|
||||
|
||||
float minDist = -1e30f;
|
||||
float maxDist = 0.02f;
|
||||
|
||||
|
||||
|
||||
findClippingFaces(sepAxis,
|
||||
&convexPolyhedronA,
|
||||
&convexShapes[shapeIndexB],
|
||||
posA,ornA,
|
||||
posB,ornB,
|
||||
worldVertsA1GPU,
|
||||
worldNormalsAGPU,
|
||||
worldVertsB1GPU,
|
||||
vertexFaceCapacity,
|
||||
minDist, maxDist,
|
||||
verticesA,
|
||||
facesA,
|
||||
indicesA,
|
||||
vertices,
|
||||
faces,
|
||||
indices,
|
||||
clippingFacesOut, pairIdx);
|
||||
|
||||
|
||||
} else
|
||||
{
|
||||
//mark this pair as in-active
|
||||
|
||||
@@ -1669,9 +1669,7 @@ __kernel void findClippingFacesKernel( __global const int4* pairs,
|
||||
|
||||
|
||||
|
||||
__kernel void clipFacesAndFindContactsKernel( __global int4* pairs,
|
||||
__global const b3RigidBodyData_t* rigidBodies,
|
||||
__global const float4* separatingNormals,
|
||||
__kernel void clipFacesAndFindContactsKernel( __global const float4* separatingNormals,
|
||||
__global const int* hasSeparatingAxis,
|
||||
__global struct b3Contact4Data* globalContactsOut,
|
||||
__global int4* clippingFacesOut,
|
||||
@@ -1698,8 +1696,8 @@ __kernel void clipFacesAndFindContactsKernel( __global int4* pairs,
|
||||
if (hasSeparatingAxis[i])
|
||||
{
|
||||
|
||||
int bodyIndexA = pairs[i].x;
|
||||
int bodyIndexB = pairs[i].y;
|
||||
// int bodyIndexA = pairs[i].x;
|
||||
// int bodyIndexB = pairs[i].y;
|
||||
|
||||
int numLocalContactsOut = 0;
|
||||
|
||||
|
||||
@@ -40,6 +40,7 @@ static const char* satClipKernelsCL= \
|
||||
"#ifdef __cplusplus\n"
|
||||
"#else\n"
|
||||
"#define b3AtomicInc atomic_inc\n"
|
||||
"#define b3AtomicAdd atomic_add\n"
|
||||
"#define b3Fabs fabs\n"
|
||||
"#define b3Sqrt native_sqrt\n"
|
||||
"#define b3Sin native_sin\n"
|
||||
@@ -63,6 +64,9 @@ static const char* satClipKernelsCL= \
|
||||
" float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n"
|
||||
" return cross(a1, b1);\n"
|
||||
" }\n"
|
||||
" #define b3MinFloat4 min\n"
|
||||
" #define b3MaxFloat4 max\n"
|
||||
" #define b3Normalized(a) normalize(a)\n"
|
||||
"#endif \n"
|
||||
" \n"
|
||||
"inline bool b3IsAlmostZero(b3Float4ConstArg v)\n"
|
||||
@@ -1859,9 +1863,7 @@ static const char* satClipKernelsCL= \
|
||||
" }// if (i<numPairs)\n"
|
||||
" \n"
|
||||
"}\n"
|
||||
"__kernel void clipFacesAndFindContactsKernel( __global int4* pairs,\n"
|
||||
" __global const b3RigidBodyData_t* rigidBodies,\n"
|
||||
" __global const float4* separatingNormals,\n"
|
||||
"__kernel void clipFacesAndFindContactsKernel( __global const float4* separatingNormals,\n"
|
||||
" __global const int* hasSeparatingAxis,\n"
|
||||
" __global struct b3Contact4Data* globalContactsOut,\n"
|
||||
" __global int4* clippingFacesOut,\n"
|
||||
@@ -1888,8 +1890,8 @@ static const char* satClipKernelsCL= \
|
||||
" if (hasSeparatingAxis[i])\n"
|
||||
" {\n"
|
||||
" \n"
|
||||
" int bodyIndexA = pairs[i].x;\n"
|
||||
" int bodyIndexB = pairs[i].y;\n"
|
||||
"// int bodyIndexA = pairs[i].x;\n"
|
||||
" // int bodyIndexB = pairs[i].y;\n"
|
||||
" \n"
|
||||
" int numLocalContactsOut = 0;\n"
|
||||
" int capacityWorldVertsB2 = vertexFaceCapacity;\n"
|
||||
|
||||
@@ -154,6 +154,7 @@ static const char* satKernelsCL= \
|
||||
"#ifdef __cplusplus\n"
|
||||
"#else\n"
|
||||
"#define b3AtomicInc atomic_inc\n"
|
||||
"#define b3AtomicAdd atomic_add\n"
|
||||
"#define b3Fabs fabs\n"
|
||||
"#define b3Sqrt native_sqrt\n"
|
||||
"#define b3Sin native_sin\n"
|
||||
@@ -177,6 +178,9 @@ static const char* satKernelsCL= \
|
||||
" float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n"
|
||||
" return cross(a1, b1);\n"
|
||||
" }\n"
|
||||
" #define b3MinFloat4 min\n"
|
||||
" #define b3MaxFloat4 max\n"
|
||||
" #define b3Normalized(a) normalize(a)\n"
|
||||
"#endif \n"
|
||||
" \n"
|
||||
"inline bool b3IsAlmostZero(b3Float4ConstArg v)\n"
|
||||
@@ -633,7 +637,7 @@ static const char* satKernelsCL= \
|
||||
" float4* sep,\n"
|
||||
" float* dmin)\n"
|
||||
"{\n"
|
||||
" int i = get_global_id(0);\n"
|
||||
" \n"
|
||||
" float4 posA = posA1;\n"
|
||||
" posA.w = 0.f;\n"
|
||||
" float4 posB = posB1;\n"
|
||||
@@ -682,7 +686,6 @@ static const char* satKernelsCL= \
|
||||
" float4* sep,\n"
|
||||
" float* dmin)\n"
|
||||
"{\n"
|
||||
" int i = get_global_id(0);\n"
|
||||
" float4 posA = posA1;\n"
|
||||
" posA.w = 0.f;\n"
|
||||
" float4 posB = posB1;\n"
|
||||
@@ -731,7 +734,6 @@ static const char* satKernelsCL= \
|
||||
" float4* sep,\n"
|
||||
" float* dmin)\n"
|
||||
"{\n"
|
||||
" int i = get_global_id(0);\n"
|
||||
" float4 posA = posA1;\n"
|
||||
" posA.w = 0.f;\n"
|
||||
" float4 posB = posB1;\n"
|
||||
@@ -815,7 +817,7 @@ static const char* satKernelsCL= \
|
||||
" float4* sep,\n"
|
||||
" float* dmin)\n"
|
||||
"{\n"
|
||||
" int i = get_global_id(0);\n"
|
||||
" \n"
|
||||
" float4 posA = posA1;\n"
|
||||
" posA.w = 0.f;\n"
|
||||
" float4 posB = posB1;\n"
|
||||
@@ -866,7 +868,7 @@ static const char* satKernelsCL= \
|
||||
" float4* sep,\n"
|
||||
" float* dmin)\n"
|
||||
"{\n"
|
||||
" int i = get_global_id(0);\n"
|
||||
" \n"
|
||||
" float4 posA = posA1;\n"
|
||||
" posA.w = 0.f;\n"
|
||||
" float4 posB = posB1;\n"
|
||||
@@ -1470,6 +1472,92 @@ static const char* satKernelsCL= \
|
||||
" \n"
|
||||
" }\n"
|
||||
"}\n"
|
||||
"int findClippingFaces(const float4 separatingNormal,\n"
|
||||
" const ConvexPolyhedronCL* hullA, \n"
|
||||
" __global const ConvexPolyhedronCL* hullB,\n"
|
||||
" const float4 posA, const Quaternion ornA,const float4 posB, const Quaternion ornB,\n"
|
||||
" __global float4* worldVertsA1,\n"
|
||||
" __global float4* worldNormalsA1,\n"
|
||||
" __global float4* worldVertsB1,\n"
|
||||
" int capacityWorldVerts,\n"
|
||||
" const float minDist, float maxDist,\n"
|
||||
" const float4* verticesA,\n"
|
||||
" const btGpuFace* facesA,\n"
|
||||
" const int* indicesA,\n"
|
||||
" __global const float4* verticesB,\n"
|
||||
" __global const btGpuFace* facesB,\n"
|
||||
" __global const int* indicesB,\n"
|
||||
" __global int4* clippingFaces, int pairIndex)\n"
|
||||
"{\n"
|
||||
" int numContactsOut = 0;\n"
|
||||
" int numWorldVertsB1= 0;\n"
|
||||
" \n"
|
||||
" \n"
|
||||
" int closestFaceB=-1;\n"
|
||||
" float dmax = -FLT_MAX;\n"
|
||||
" \n"
|
||||
" {\n"
|
||||
" for(int face=0;face<hullB->m_numFaces;face++)\n"
|
||||
" {\n"
|
||||
" const float4 Normal = make_float4(facesB[hullB->m_faceOffset+face].m_plane.x,\n"
|
||||
" facesB[hullB->m_faceOffset+face].m_plane.y, facesB[hullB->m_faceOffset+face].m_plane.z,0.f);\n"
|
||||
" const float4 WorldNormal = qtRotate(ornB, Normal);\n"
|
||||
" float d = dot3F4(WorldNormal,separatingNormal);\n"
|
||||
" if (d > dmax)\n"
|
||||
" {\n"
|
||||
" dmax = d;\n"
|
||||
" closestFaceB = face;\n"
|
||||
" }\n"
|
||||
" }\n"
|
||||
" }\n"
|
||||
" \n"
|
||||
" {\n"
|
||||
" const btGpuFace polyB = facesB[hullB->m_faceOffset+closestFaceB];\n"
|
||||
" const int numVertices = polyB.m_numIndices;\n"
|
||||
" for(int e0=0;e0<numVertices;e0++)\n"
|
||||
" {\n"
|
||||
" const float4 b = verticesB[hullB->m_vertexOffset+indicesB[polyB.m_indexOffset+e0]];\n"
|
||||
" worldVertsB1[pairIndex*capacityWorldVerts+numWorldVertsB1++] = transform(&b,&posB,&ornB);\n"
|
||||
" }\n"
|
||||
" }\n"
|
||||
" \n"
|
||||
" int closestFaceA=-1;\n"
|
||||
" {\n"
|
||||
" float dmin = FLT_MAX;\n"
|
||||
" for(int face=0;face<hullA->m_numFaces;face++)\n"
|
||||
" {\n"
|
||||
" const float4 Normal = make_float4(\n"
|
||||
" facesA[hullA->m_faceOffset+face].m_plane.x,\n"
|
||||
" facesA[hullA->m_faceOffset+face].m_plane.y,\n"
|
||||
" facesA[hullA->m_faceOffset+face].m_plane.z,\n"
|
||||
" 0.f);\n"
|
||||
" const float4 faceANormalWS = qtRotate(ornA,Normal);\n"
|
||||
" \n"
|
||||
" float d = dot3F4(faceANormalWS,separatingNormal);\n"
|
||||
" if (d < dmin)\n"
|
||||
" {\n"
|
||||
" dmin = d;\n"
|
||||
" closestFaceA = face;\n"
|
||||
" worldNormalsA1[pairIndex] = faceANormalWS;\n"
|
||||
" }\n"
|
||||
" }\n"
|
||||
" }\n"
|
||||
" \n"
|
||||
" int numVerticesA = facesA[hullA->m_faceOffset+closestFaceA].m_numIndices;\n"
|
||||
" for(int e0=0;e0<numVerticesA;e0++)\n"
|
||||
" {\n"
|
||||
" const float4 a = verticesA[hullA->m_vertexOffset+indicesA[facesA[hullA->m_faceOffset+closestFaceA].m_indexOffset+e0]];\n"
|
||||
" worldVertsA1[pairIndex*capacityWorldVerts+e0] = transform(&a, &posA,&ornA);\n"
|
||||
" }\n"
|
||||
" \n"
|
||||
" clippingFaces[pairIndex].x = closestFaceA;\n"
|
||||
" clippingFaces[pairIndex].y = closestFaceB;\n"
|
||||
" clippingFaces[pairIndex].z = numVerticesA;\n"
|
||||
" clippingFaces[pairIndex].w = numWorldVertsB1;\n"
|
||||
" \n"
|
||||
" \n"
|
||||
" return numContactsOut;\n"
|
||||
"}\n"
|
||||
"// work-in-progress\n"
|
||||
"__kernel void findConcaveSeparatingAxisKernel( __global int4* concavePairs,\n"
|
||||
" __global const BodyData* rigidBodies,\n"
|
||||
@@ -1482,12 +1570,19 @@ static const char* satKernelsCL= \
|
||||
" __global const btGpuChildShape* gpuChildShapes,\n"
|
||||
" __global btAabbCL* aabbs,\n"
|
||||
" __global float4* concaveSeparatingNormalsOut,\n"
|
||||
" __global int* concaveHasSeparatingNormals,\n"
|
||||
" __global int4* clippingFacesOut,\n"
|
||||
" __global float4* worldVertsA1GPU,\n"
|
||||
" __global float4* worldNormalsAGPU,\n"
|
||||
" __global float4* worldVertsB1GPU,\n"
|
||||
" int vertexFaceCapacity,\n"
|
||||
" int numConcavePairs\n"
|
||||
" )\n"
|
||||
"{\n"
|
||||
" int i = get_global_id(0);\n"
|
||||
" if (i>=numConcavePairs)\n"
|
||||
" return;\n"
|
||||
" concaveHasSeparatingNormals[i] = 0;\n"
|
||||
" int pairIdx = i;\n"
|
||||
" int bodyIndexA = concavePairs[i].x;\n"
|
||||
" int bodyIndexB = concavePairs[i].y;\n"
|
||||
@@ -1691,6 +1786,27 @@ static const char* satKernelsCL= \
|
||||
" {\n"
|
||||
" sepAxis.w = dmin;\n"
|
||||
" concaveSeparatingNormalsOut[pairIdx]=sepAxis;\n"
|
||||
" concaveHasSeparatingNormals[i]=1;\n"
|
||||
" float minDist = -1e30f;\n"
|
||||
" float maxDist = 0.02f;\n"
|
||||
" \n"
|
||||
" findClippingFaces(sepAxis,\n"
|
||||
" &convexPolyhedronA,\n"
|
||||
" &convexShapes[shapeIndexB],\n"
|
||||
" posA,ornA,\n"
|
||||
" posB,ornB,\n"
|
||||
" worldVertsA1GPU,\n"
|
||||
" worldNormalsAGPU,\n"
|
||||
" worldVertsB1GPU,\n"
|
||||
" vertexFaceCapacity,\n"
|
||||
" minDist, maxDist,\n"
|
||||
" verticesA,\n"
|
||||
" facesA,\n"
|
||||
" indicesA,\n"
|
||||
" vertices,\n"
|
||||
" faces,\n"
|
||||
" indices,\n"
|
||||
" clippingFacesOut, pairIdx);\n"
|
||||
" } else\n"
|
||||
" { \n"
|
||||
" //mark this pair as in-active\n"
|
||||
|
||||
@@ -27,7 +27,7 @@ b3LauncherCL::~b3LauncherCL()
|
||||
if (gDebugLauncherCL)
|
||||
{
|
||||
static int counter = 0;
|
||||
printf("[%d] Finished launching OpenCL kernel %s [%d]\n", counter++,m_name);
|
||||
printf("[%d] Finished launching OpenCL kernel %s\n", counter++,m_name);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -124,7 +124,7 @@ b3Solver::b3Solver(cl_context ctx, cl_device_id device, cl_command_queue queue,
|
||||
|
||||
{
|
||||
|
||||
cl_program solveContactProg= b3OpenCLUtils::compileCLProgramFromString( ctx, device, 0, &pErrNum,additionalMacros, B3_SOLVER_CONTACT_KERNEL_PATH,false);
|
||||
cl_program solveContactProg= b3OpenCLUtils::compileCLProgramFromString( ctx, device, solveContactSource, &pErrNum,additionalMacros, B3_SOLVER_CONTACT_KERNEL_PATH);
|
||||
b3Assert(solveContactProg);
|
||||
|
||||
cl_program solveFrictionProg= b3OpenCLUtils::compileCLProgramFromString( ctx, device, solveFrictionSource, &pErrNum,additionalMacros, B3_SOLVER_FRICTION_KERNEL_PATH);
|
||||
|
||||
@@ -25,6 +25,7 @@ static const char* batchingKernelsCL= \
|
||||
"#ifdef __cplusplus\n"
|
||||
"#else\n"
|
||||
"#define b3AtomicInc atomic_inc\n"
|
||||
"#define b3AtomicAdd atomic_add\n"
|
||||
"#define b3Fabs fabs\n"
|
||||
"#define b3Sqrt native_sqrt\n"
|
||||
"#define b3Sin native_sin\n"
|
||||
@@ -48,6 +49,9 @@ static const char* batchingKernelsCL= \
|
||||
" float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n"
|
||||
" return cross(a1, b1);\n"
|
||||
" }\n"
|
||||
" #define b3MinFloat4 min\n"
|
||||
" #define b3MaxFloat4 max\n"
|
||||
" #define b3Normalized(a) normalize(a)\n"
|
||||
"#endif \n"
|
||||
" \n"
|
||||
"inline bool b3IsAlmostZero(b3Float4ConstArg v)\n"
|
||||
|
||||
@@ -25,6 +25,7 @@ static const char* batchingKernelsNewCL= \
|
||||
"#ifdef __cplusplus\n"
|
||||
"#else\n"
|
||||
"#define b3AtomicInc atomic_inc\n"
|
||||
"#define b3AtomicAdd atomic_add\n"
|
||||
"#define b3Fabs fabs\n"
|
||||
"#define b3Sqrt native_sqrt\n"
|
||||
"#define b3Sin native_sin\n"
|
||||
@@ -48,6 +49,9 @@ static const char* batchingKernelsNewCL= \
|
||||
" float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n"
|
||||
" return cross(a1, b1);\n"
|
||||
" }\n"
|
||||
" #define b3MinFloat4 min\n"
|
||||
" #define b3MaxFloat4 max\n"
|
||||
" #define b3Normalized(a) normalize(a)\n"
|
||||
"#endif \n"
|
||||
" \n"
|
||||
"inline bool b3IsAlmostZero(b3Float4ConstArg v)\n"
|
||||
|
||||
@@ -25,6 +25,7 @@ static const char* integrateKernelCL= \
|
||||
"#ifdef __cplusplus\n"
|
||||
"#else\n"
|
||||
"#define b3AtomicInc atomic_inc\n"
|
||||
"#define b3AtomicAdd atomic_add\n"
|
||||
"#define b3Fabs fabs\n"
|
||||
"#define b3Sqrt native_sqrt\n"
|
||||
"#define b3Sin native_sin\n"
|
||||
@@ -48,6 +49,9 @@ static const char* integrateKernelCL= \
|
||||
" float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n"
|
||||
" return cross(a1, b1);\n"
|
||||
" }\n"
|
||||
" #define b3MinFloat4 min\n"
|
||||
" #define b3MaxFloat4 max\n"
|
||||
" #define b3Normalized(a) normalize(a)\n"
|
||||
"#endif \n"
|
||||
" \n"
|
||||
"inline bool b3IsAlmostZero(b3Float4ConstArg v)\n"
|
||||
|
||||
@@ -25,6 +25,7 @@ static const char* solverSetupCL= \
|
||||
"#ifdef __cplusplus\n"
|
||||
"#else\n"
|
||||
"#define b3AtomicInc atomic_inc\n"
|
||||
"#define b3AtomicAdd atomic_add\n"
|
||||
"#define b3Fabs fabs\n"
|
||||
"#define b3Sqrt native_sqrt\n"
|
||||
"#define b3Sin native_sin\n"
|
||||
@@ -48,6 +49,9 @@ static const char* solverSetupCL= \
|
||||
" float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n"
|
||||
" return cross(a1, b1);\n"
|
||||
" }\n"
|
||||
" #define b3MinFloat4 min\n"
|
||||
" #define b3MaxFloat4 max\n"
|
||||
" #define b3Normalized(a) normalize(a)\n"
|
||||
"#endif \n"
|
||||
" \n"
|
||||
"inline bool b3IsAlmostZero(b3Float4ConstArg v)\n"
|
||||
|
||||
@@ -25,6 +25,7 @@ static const char* solverSetup2CL= \
|
||||
"#ifdef __cplusplus\n"
|
||||
"#else\n"
|
||||
"#define b3AtomicInc atomic_inc\n"
|
||||
"#define b3AtomicAdd atomic_add\n"
|
||||
"#define b3Fabs fabs\n"
|
||||
"#define b3Sqrt native_sqrt\n"
|
||||
"#define b3Sin native_sin\n"
|
||||
@@ -48,6 +49,9 @@ static const char* solverSetup2CL= \
|
||||
" float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n"
|
||||
" return cross(a1, b1);\n"
|
||||
" }\n"
|
||||
" #define b3MinFloat4 min\n"
|
||||
" #define b3MaxFloat4 max\n"
|
||||
" #define b3Normalized(a) normalize(a)\n"
|
||||
"#endif \n"
|
||||
" \n"
|
||||
"inline bool b3IsAlmostZero(b3Float4ConstArg v)\n"
|
||||
|
||||
@@ -25,6 +25,7 @@ static const char* solverUtilsCL= \
|
||||
"#ifdef __cplusplus\n"
|
||||
"#else\n"
|
||||
"#define b3AtomicInc atomic_inc\n"
|
||||
"#define b3AtomicAdd atomic_add\n"
|
||||
"#define b3Fabs fabs\n"
|
||||
"#define b3Sqrt native_sqrt\n"
|
||||
"#define b3Sin native_sin\n"
|
||||
@@ -48,6 +49,9 @@ static const char* solverUtilsCL= \
|
||||
" float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n"
|
||||
" return cross(a1, b1);\n"
|
||||
" }\n"
|
||||
" #define b3MinFloat4 min\n"
|
||||
" #define b3MaxFloat4 max\n"
|
||||
" #define b3Normalized(a) normalize(a)\n"
|
||||
"#endif \n"
|
||||
" \n"
|
||||
"inline bool b3IsAlmostZero(b3Float4ConstArg v)\n"
|
||||
|
||||
@@ -15,6 +15,7 @@ static const char* updateAabbsKernelCL= \
|
||||
"#ifdef __cplusplus\n"
|
||||
"#else\n"
|
||||
"#define b3AtomicInc atomic_inc\n"
|
||||
"#define b3AtomicAdd atomic_add\n"
|
||||
"#define b3Fabs fabs\n"
|
||||
"#define b3Sqrt native_sqrt\n"
|
||||
"#define b3Sin native_sin\n"
|
||||
@@ -38,6 +39,9 @@ static const char* updateAabbsKernelCL= \
|
||||
" float4 b1 = b3MakeFloat4(v1.xyz,0.f);\n"
|
||||
" return cross(a1, b1);\n"
|
||||
" }\n"
|
||||
" #define b3MinFloat4 min\n"
|
||||
" #define b3MaxFloat4 max\n"
|
||||
" #define b3Normalized(a) normalize(a)\n"
|
||||
"#endif \n"
|
||||
" \n"
|
||||
"inline bool b3IsAlmostZero(b3Float4ConstArg v)\n"
|
||||
|
||||
Reference in New Issue
Block a user