move files to correct location (case sensitive issue)
w
This commit is contained in:
erwincoumans
687
Demos3/BasicGpuDemo/b3GpuDynamicsWorld.cpp
Normal file
687
Demos3/BasicGpuDemo/b3GpuDynamicsWorld.cpp
Normal file
@@ -0,0 +1,687 @@
|
||||
#include "b3GpuDynamicsWorld.h"
|
||||
#include "BulletDynamics/Dynamics/btRigidBody.h"
|
||||
|
||||
//#include "../../../opencl/gpu_rigidbody_pipeline2/CLPhysicsDemo.h"
|
||||
//#include "../../../opencl/gpu_rigidbody_pipeline/b3GpuNarrowPhaseAndSolver.h"
|
||||
#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
|
||||
#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
|
||||
#include "BulletCollision/CollisionShapes/btCompoundShape.h"
|
||||
#include "BulletCollision/CollisionShapes/btSphereShape.h"
|
||||
#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
|
||||
#include "BulletCollision/CollisionShapes/btConvexHullShape.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h"
|
||||
#include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h"
|
||||
|
||||
#include "LinearMath/btQuickprof.h"
|
||||
#include "Bullet3OpenCL/BroadphaseCollision/b3GpuSapBroadphase.h"
|
||||
#include "Bullet3OpenCL/RigidBody/b3GpuNarrowPhase.h"
|
||||
#include "Bullet3OpenCL/RigidBody/b3GpuRigidBodyPipeline.h"
|
||||
#include "Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.h"
|
||||
#include "Bullet3Dynamics/ConstraintSolver/b3Generic6DofConstraint.h"
|
||||
|
||||
#include "Bullet3Collision/NarrowPhaseCollision/b3RigidBodyCL.h"
|
||||
#include "Bullet3Common/b3Logging.h"
|
||||
|
||||
|
||||
#ifdef _WIN32
|
||||
#include <windows.h>
|
||||
#endif
|
||||
|
||||
|
||||
#if (BT_BULLET_VERSION >= 282)
|
||||
#define BT_USE_BODY_UPDATE_REVISION
|
||||
#endif
|
||||
|
||||
|
||||
b3GpuDynamicsWorld::b3GpuDynamicsWorld(class b3GpuSapBroadphase* bp,class b3GpuNarrowPhase* np, class b3GpuRigidBodyPipeline* rigidBodyPipeline)
|
||||
:btDynamicsWorld(0,0,0),
|
||||
m_gravity(0,-10,0),
|
||||
m_cpuGpuSync(true),
|
||||
m_bp(bp),
|
||||
m_np(np),
|
||||
m_rigidBodyPipeline(rigidBodyPipeline),
|
||||
m_localTime(0.f),
|
||||
m_staticBody(0)
|
||||
{
|
||||
btConvexHullShape* nullShape = new btConvexHullShape();
|
||||
m_staticBody = new btRigidBody(0,0,nullShape);
|
||||
addRigidBody(m_staticBody,0,0);
|
||||
}
|
||||
|
||||
b3GpuDynamicsWorld::~b3GpuDynamicsWorld()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
int b3GpuDynamicsWorld::stepSimulation( btScalar timeStepUnused, int maxSubStepsUnused, btScalar fixedTimeStep)
|
||||
{
|
||||
///Don't use more than 1 simulation step, it destroys the performance having to copy the data between CPU and GPU multiple times per frame
|
||||
///Please use the CPU version in btDiscreteDynamicsWorld if you don't like this
|
||||
|
||||
|
||||
#ifndef BT_NO_PROFILE
|
||||
CProfileManager::Reset();
|
||||
#endif //BT_NO_PROFILE
|
||||
|
||||
|
||||
BT_PROFILE("stepSimulation");
|
||||
|
||||
{
|
||||
BT_PROFILE("sync constraints CPU");
|
||||
//todo: determine what data has changed, or perform copy on GPU?
|
||||
for (int i=0;i<m_constraints.size();i++)
|
||||
{
|
||||
btTypedConstraint* constraint = m_constraints[i];
|
||||
b3TypedConstraint* c = (b3TypedConstraint*) constraint->getUserConstraintPtr();
|
||||
if (c)
|
||||
{
|
||||
switch (constraint->getConstraintType())
|
||||
{
|
||||
case POINT2POINT_CONSTRAINT_TYPE:
|
||||
{
|
||||
btPoint2PointConstraint* p2 = (btPoint2PointConstraint*) constraint;
|
||||
b3Point2PointConstraint* p3 = (b3Point2PointConstraint*) c;
|
||||
p3->setPivotA((const b3Vector3&)p2->getPivotInA());
|
||||
p3->setPivotB((const b3Vector3&)p2->getPivotInB());
|
||||
p3->m_setting.m_damping = p2->m_setting.m_damping;
|
||||
p3->m_setting.m_impulseClamp = p2->m_setting.m_impulseClamp;
|
||||
p3->m_setting.m_tau = p2->m_setting.m_tau;
|
||||
|
||||
break;
|
||||
};
|
||||
|
||||
case D6_CONSTRAINT_TYPE:
|
||||
{
|
||||
btGeneric6DofConstraint* dof2 = (btGeneric6DofConstraint*) constraint;
|
||||
b3Generic6DofConstraint* dof3 = (b3Generic6DofConstraint*) c;
|
||||
const b3RigidBodyCL* bodiesCL = m_np->getBodiesCpu();
|
||||
|
||||
b3Transform frameInA = (b3Transform&) dof2->getFrameOffsetA();
|
||||
b3Transform frameInB = (b3Transform&) dof2->getFrameOffsetB();
|
||||
dof3->setFrames(frameInA,frameInB,bodiesCL);
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
{
|
||||
}
|
||||
};
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// detect any change (very simple)
|
||||
{
|
||||
BT_PROFILE("body update revision detection (CPU)");
|
||||
#ifdef BT_USE_BODY_UPDATE_REVISION
|
||||
b3Assert(m_bodyUpdateRevisions.size() == m_collisionObjects.size());
|
||||
b3Assert(m_np->getNumRigidBodies() == m_bodyUpdateRevisions.size());
|
||||
#endif //BT_USE_BODY_UPDATE_REVISION
|
||||
|
||||
|
||||
for (int i=0;i<this->m_collisionObjects.size();i++)
|
||||
{
|
||||
if (i>=m_np->getNumRigidBodies())
|
||||
{
|
||||
b3Error("bodiesCL out-of-range\n");
|
||||
continue;
|
||||
}
|
||||
|
||||
#ifdef BT_USE_BODY_UPDATE_REVISION
|
||||
if (m_bodyUpdateRevisions[i] != m_collisionObjects[i]->getUpdateRevisionInternal())
|
||||
#endif//BT_USE_BODY_UPDATE_REVISION
|
||||
{
|
||||
m_cpuGpuSync = true;
|
||||
|
||||
#ifdef BT_USE_BODY_UPDATE_REVISION
|
||||
m_bodyUpdateRevisions[i] = m_collisionObjects[i]->getUpdateRevisionInternal();
|
||||
#endif
|
||||
|
||||
|
||||
btRigidBody* body = btRigidBody::upcast(m_collisionObjects[i]);
|
||||
if (body)
|
||||
{
|
||||
b3Vector3 pos = (const b3Vector3&)m_collisionObjects[i]->getWorldTransform().getOrigin();
|
||||
b3Quaternion orn = (const b3Quaternion&)m_collisionObjects[i]->getWorldTransform().getRotation();
|
||||
body->integrateVelocities(fixedTimeStep);
|
||||
m_np->setObjectTransformCpu(&pos[0],&orn[0],i);
|
||||
b3Vector3 linVel = (const b3Vector3&)body->getLinearVelocity();
|
||||
b3Vector3 angVel = (const b3Vector3&)body->getAngularVelocity();
|
||||
m_np->setObjectVelocityCpu(&linVel[0],&angVel[0],i);
|
||||
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
if (m_cpuGpuSync)
|
||||
{
|
||||
BT_PROFILE("cpuGpuSync");
|
||||
m_cpuGpuSync = false;
|
||||
m_np->writeAllBodiesToGpu();
|
||||
m_bp->writeAabbsToGpu();
|
||||
m_rigidBodyPipeline->writeAllInstancesToGpu();
|
||||
}
|
||||
|
||||
|
||||
|
||||
//internalSingleStepSimulation(fixedTimeStep);
|
||||
// dispatch preTick callback
|
||||
if(0 != m_internalPreTickCallback)
|
||||
{
|
||||
BT_PROFILE("internalPreTickCallback");
|
||||
(*m_internalPreTickCallback)(this, fixedTimeStep);
|
||||
}
|
||||
|
||||
{
|
||||
BT_PROFILE("m_rigidBodyPipeline->stepSimulation");
|
||||
m_rigidBodyPipeline->stepSimulation(fixedTimeStep);
|
||||
}
|
||||
|
||||
{
|
||||
BT_PROFILE("readbackBodiesToCpu");
|
||||
//now copy info back to rigid bodies....
|
||||
m_np->readbackAllBodiesToCpu();
|
||||
}
|
||||
|
||||
{
|
||||
BT_PROFILE("scatter transforms into rigidbody (CPU)");
|
||||
|
||||
const b3RigidBodyCL* bodiesCL = m_np->getBodiesCpu();
|
||||
|
||||
for (int i=0;i<this->m_collisionObjects.size();i++)
|
||||
{
|
||||
btVector3 pos;
|
||||
btQuaternion orn;
|
||||
if (m_np->getObjectTransformFromCpu(&pos[0],&orn[0],i))
|
||||
{
|
||||
btTransform newTrans;
|
||||
newTrans.setOrigin(pos);
|
||||
newTrans.setRotation(orn);
|
||||
|
||||
btCollisionObject* colObj = this->m_collisionObjects[i];
|
||||
colObj->setWorldTransform(newTrans);
|
||||
|
||||
btRigidBody* body = btRigidBody::upcast(m_collisionObjects[i]);
|
||||
if (body)
|
||||
{
|
||||
body->setLinearVelocity((btVector3&)bodiesCL[i].m_linVel);
|
||||
body->setAngularVelocity((btVector3&)bodiesCL[i].m_angVel);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
#ifdef BT_USE_BODY_UPDATE_REVISION
|
||||
//ignore this revision update
|
||||
m_bodyUpdateRevisions[i] = m_collisionObjects[i]->getUpdateRevisionInternal();
|
||||
#endif //BT_USE_BODY_UPDATE_REVISION
|
||||
|
||||
}
|
||||
|
||||
{
|
||||
BT_PROFILE("synchronizeMotionStates");
|
||||
synchronizeMotionStates();
|
||||
}
|
||||
}
|
||||
|
||||
clearForces();
|
||||
|
||||
|
||||
#ifndef B3_NO_PROFILE
|
||||
CProfileManager::Increment_Frame_Counter();
|
||||
#endif //B3_NO_PROFILE
|
||||
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
|
||||
void b3GpuDynamicsWorld::clearForces()
|
||||
{
|
||||
///@todo: iterate over awake simulation islands!
|
||||
for ( int i=0;i<m_collisionObjects.size();i++)
|
||||
{
|
||||
btRigidBody* body = btRigidBody::upcast(m_collisionObjects[i]);
|
||||
//need to check if next line is ok
|
||||
//it might break backward compatibility (people applying forces on sleeping objects get never cleared and accumulate on wake-up
|
||||
if (body)
|
||||
body->clearForces();
|
||||
}
|
||||
}
|
||||
|
||||
void b3GpuDynamicsWorld::setGravity(const btVector3& gravity)
|
||||
{
|
||||
m_gravity = gravity;
|
||||
m_rigidBodyPipeline->setGravity(gravity);
|
||||
}
|
||||
|
||||
int b3GpuDynamicsWorld::findOrRegisterCollisionShape(const btCollisionShape* colShape)
|
||||
{
|
||||
int index = m_uniqueShapes.findLinearSearch(colShape);
|
||||
if (index==m_uniqueShapes.size())
|
||||
{
|
||||
if (colShape->isPolyhedral())
|
||||
{
|
||||
m_uniqueShapes.push_back(colShape);
|
||||
|
||||
btPolyhedralConvexShape* convex = (btPolyhedralConvexShape*)colShape;
|
||||
int numVertices=convex->getNumVertices();
|
||||
|
||||
int strideInBytes=sizeof(btVector3);
|
||||
btAlignedObjectArray<btVector3> tmpVertices;
|
||||
tmpVertices.resize(numVertices);
|
||||
for (int i=0;i<numVertices;i++)
|
||||
convex->getVertex(i,tmpVertices[i]);
|
||||
const float scaling[4]={1,1,1,1};
|
||||
//bool noHeightField=true;
|
||||
|
||||
//int gpuShapeIndex = m_np->registerConvexHullShape(&tmpVertices[0].getX(), strideInBytes, numVertices, scaling);
|
||||
const float* verts = numVertices? &tmpVertices[0].getX() : 0;
|
||||
|
||||
int gpuShapeIndex = m_np->registerConvexHullShape(verts,strideInBytes, numVertices, scaling);
|
||||
m_uniqueShapeMapping.push_back(gpuShapeIndex);
|
||||
} else
|
||||
{
|
||||
if (colShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
|
||||
{
|
||||
m_uniqueShapes.push_back(colShape);
|
||||
|
||||
btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*) colShape;
|
||||
btStridingMeshInterface* meshInterface = trimesh->getMeshInterface();
|
||||
b3AlignedObjectArray<b3Vector3> vertices;
|
||||
b3AlignedObjectArray<int> indices;
|
||||
|
||||
btVector3 trimeshScaling(1,1,1);
|
||||
for (int partId=0;partId<meshInterface->getNumSubParts();partId++)
|
||||
{
|
||||
|
||||
const unsigned char *vertexbase = 0;
|
||||
int numverts = 0;
|
||||
PHY_ScalarType type = PHY_INTEGER;
|
||||
int stride = 0;
|
||||
const unsigned char *indexbase = 0;
|
||||
int indexstride = 0;
|
||||
int numfaces = 0;
|
||||
PHY_ScalarType indicestype = PHY_INTEGER;
|
||||
//PHY_ScalarType indexType=0;
|
||||
|
||||
b3Vector3 triangleVerts[3];
|
||||
meshInterface->getLockedReadOnlyVertexIndexBase(&vertexbase,numverts, type,stride,&indexbase,indexstride,numfaces,indicestype,partId);
|
||||
btVector3 aabbMin,aabbMax;
|
||||
|
||||
for (int triangleIndex = 0 ; triangleIndex < numfaces;triangleIndex++)
|
||||
{
|
||||
unsigned int* gfxbase = (unsigned int*)(indexbase+triangleIndex*indexstride);
|
||||
|
||||
for (int j=2;j>=0;j--)
|
||||
{
|
||||
|
||||
int graphicsindex = indicestype==PHY_SHORT?((unsigned short*)gfxbase)[j]:gfxbase[j];
|
||||
if (type == PHY_FLOAT)
|
||||
{
|
||||
float* graphicsbase = (float*)(vertexbase+graphicsindex*stride);
|
||||
triangleVerts[j] = b3Vector3(
|
||||
graphicsbase[0]*trimeshScaling.getX(),
|
||||
graphicsbase[1]*trimeshScaling.getY(),
|
||||
graphicsbase[2]*trimeshScaling.getZ());
|
||||
}
|
||||
else
|
||||
{
|
||||
double* graphicsbase = (double*)(vertexbase+graphicsindex*stride);
|
||||
triangleVerts[j] = b3Vector3( btScalar(graphicsbase[0]*trimeshScaling.getX()),
|
||||
btScalar(graphicsbase[1]*trimeshScaling.getY()),
|
||||
btScalar(graphicsbase[2]*trimeshScaling.getZ()));
|
||||
}
|
||||
}
|
||||
vertices.push_back(triangleVerts[0]);
|
||||
vertices.push_back(triangleVerts[1]);
|
||||
vertices.push_back(triangleVerts[2]);
|
||||
indices.push_back(indices.size());
|
||||
indices.push_back(indices.size());
|
||||
indices.push_back(indices.size());
|
||||
}
|
||||
}
|
||||
//GraphicsShape* gfxShape = 0;//b3BulletDataExtractor::createGraphicsShapeFromWavefrontObj(objData);
|
||||
|
||||
//GraphicsShape* gfxShape = b3BulletDataExtractor::createGraphicsShapeFromConvexHull(&sUnitSpherePoints[0],MY_UNITSPHERE_POINTS);
|
||||
float meshScaling[4] = {1,1,1,1};
|
||||
//int shapeIndex = renderer.registerShape(gfxShape->m_vertices,gfxShape->m_numvertices,gfxShape->m_indices,gfxShape->m_numIndices);
|
||||
//float groundPos[4] = {0,0,0,0};
|
||||
|
||||
//renderer.registerGraphicsInstance(shapeIndex,groundPos,rotOrn,color,meshScaling);
|
||||
if (vertices.size() && indices.size())
|
||||
{
|
||||
int gpuShapeIndex = m_np->registerConcaveMesh(&vertices,&indices, meshScaling);
|
||||
m_uniqueShapeMapping.push_back(gpuShapeIndex);
|
||||
} else
|
||||
{
|
||||
printf("Error: no vertices in mesh in b3GpuDynamicsWorld::addRigidBody\n");
|
||||
index = -1;
|
||||
b3Assert(0);
|
||||
}
|
||||
|
||||
|
||||
} else
|
||||
{
|
||||
if (colShape->getShapeType()==COMPOUND_SHAPE_PROXYTYPE)
|
||||
{
|
||||
|
||||
btCompoundShape* compound = (btCompoundShape*) colShape;
|
||||
b3AlignedObjectArray<b3GpuChildShape> childShapes;
|
||||
|
||||
for (int i=0;i<compound->getNumChildShapes();i++)
|
||||
{
|
||||
//for now, only support polyhedral child shapes
|
||||
b3Assert(compound->getChildShape(i)->isPolyhedral());
|
||||
b3GpuChildShape child;
|
||||
child.m_shapeIndex = findOrRegisterCollisionShape(compound->getChildShape(i));
|
||||
btVector3 pos = compound->getChildTransform(i).getOrigin();
|
||||
btQuaternion orn = compound->getChildTransform(i).getRotation();
|
||||
for (int v=0;v<4;v++)
|
||||
{
|
||||
child.m_childPosition[v] = pos[v];
|
||||
child.m_childOrientation[v] = orn[v];
|
||||
}
|
||||
childShapes.push_back(child);
|
||||
}
|
||||
index = m_uniqueShapes.size();
|
||||
m_uniqueShapes.push_back(colShape);
|
||||
|
||||
int gpuShapeIndex = m_np->registerCompoundShape(&childShapes);
|
||||
m_uniqueShapeMapping.push_back(gpuShapeIndex);
|
||||
|
||||
|
||||
|
||||
|
||||
/*printf("Error: unsupported compound type (%d) in b3GpuDynamicsWorld::addRigidBody\n",colShape->getShapeType());
|
||||
index = -1;
|
||||
b3Assert(0);
|
||||
*/
|
||||
} else
|
||||
{
|
||||
if (colShape->getShapeType()==SPHERE_SHAPE_PROXYTYPE)
|
||||
{
|
||||
m_uniqueShapes.push_back(colShape);
|
||||
btSphereShape* sphere = (btSphereShape*)colShape;
|
||||
|
||||
int gpuShapeIndex = m_np->registerSphereShape(sphere->getRadius());
|
||||
m_uniqueShapeMapping.push_back(gpuShapeIndex);
|
||||
} else
|
||||
{
|
||||
if (colShape->getShapeType()==STATIC_PLANE_PROXYTYPE)
|
||||
{
|
||||
m_uniqueShapes.push_back(colShape);
|
||||
btStaticPlaneShape* plane = (btStaticPlaneShape*)colShape;
|
||||
|
||||
int gpuShapeIndex = m_np->registerPlaneShape((b3Vector3&)plane->getPlaneNormal(),plane->getPlaneConstant());
|
||||
m_uniqueShapeMapping.push_back(gpuShapeIndex);
|
||||
} else
|
||||
{
|
||||
printf("Error: unsupported shape type (%d) in b3GpuDynamicsWorld::addRigidBody\n",colShape->getShapeType());
|
||||
index = -1;
|
||||
b3Assert(0);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
return index;
|
||||
}
|
||||
|
||||
void b3GpuDynamicsWorld::addRigidBody(btRigidBody* body)
|
||||
{
|
||||
m_cpuGpuSync = true;
|
||||
//body->setMotionState(0);
|
||||
|
||||
|
||||
int index = findOrRegisterCollisionShape(body->getCollisionShape());
|
||||
|
||||
if (index>=0)
|
||||
{
|
||||
int gpuShapeIndex= m_uniqueShapeMapping[index];
|
||||
float mass = body->getInvMass() ? 1.f/body->getInvMass() : 0.f;
|
||||
btVector3 pos = body->getWorldTransform().getOrigin();
|
||||
btQuaternion orn = body->getWorldTransform().getRotation();
|
||||
|
||||
int bodyIndex = m_rigidBodyPipeline->registerPhysicsInstance(mass,&pos.getX(),&orn.getX(),gpuShapeIndex,m_collisionObjects.size(),false);
|
||||
|
||||
body->setUserIndex(bodyIndex);
|
||||
|
||||
m_collisionObjects.push_back(body);
|
||||
m_bodyUpdateRevisions.push_back(-1);
|
||||
}
|
||||
}
|
||||
|
||||
void b3GpuDynamicsWorld::removeRigidBody(btRigidBody* colObj)
|
||||
{
|
||||
m_cpuGpuSync = true;
|
||||
btDynamicsWorld::removeCollisionObject(colObj);
|
||||
m_bodyUpdateRevisions.resize(this->m_collisionObjects.size());
|
||||
for (int i=0;i<m_bodyUpdateRevisions.size();i++)
|
||||
{
|
||||
m_bodyUpdateRevisions[i] = -1;
|
||||
}
|
||||
|
||||
int bodyIndex = colObj->getUserIndex();
|
||||
|
||||
if (getNumCollisionObjects()==0)
|
||||
{
|
||||
m_uniqueShapes.resize(0);
|
||||
m_uniqueShapeMapping.resize(0);
|
||||
m_np->reset();
|
||||
m_bp->reset();
|
||||
m_rigidBodyPipeline->reset();
|
||||
#ifdef BT_USE_BODY_UPDATE_REVISION
|
||||
m_bodyUpdateRevisions.resize(0);
|
||||
#endif //BT_USE_BODY_UPDATE_REVISION
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
void b3GpuDynamicsWorld::removeCollisionObject(btCollisionObject* colObj)
|
||||
{
|
||||
m_cpuGpuSync = true;
|
||||
btDynamicsWorld::removeCollisionObject(colObj);
|
||||
m_bodyUpdateRevisions.resize(this->m_collisionObjects.size());
|
||||
for (int i=0;i<m_bodyUpdateRevisions.size();i++)
|
||||
{
|
||||
m_bodyUpdateRevisions[i] = -1;
|
||||
}
|
||||
if (getNumCollisionObjects()==0)
|
||||
{
|
||||
m_uniqueShapes.resize(0);
|
||||
m_uniqueShapeMapping.resize(0);
|
||||
m_np->reset();
|
||||
m_bp->reset();
|
||||
m_rigidBodyPipeline->reset();
|
||||
#ifdef BT_USE_BODY_UPDATE_REVISION
|
||||
m_bodyUpdateRevisions.resize(0);
|
||||
#endif
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void b3GpuDynamicsWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
|
||||
{
|
||||
b3AlignedObjectArray<b3RayInfo> rays;
|
||||
b3RayInfo ray;
|
||||
ray.m_from = (const b3Vector3&)rayFromWorld;
|
||||
ray.m_to = (const b3Vector3&)rayToWorld;
|
||||
rays.push_back(ray);
|
||||
|
||||
b3AlignedObjectArray<b3RayHit> hitResults;
|
||||
b3RayHit hit;
|
||||
hit.m_hitFraction = 1.f;
|
||||
|
||||
hitResults.push_back(hit);
|
||||
|
||||
m_rigidBodyPipeline->castRays(rays,hitResults);
|
||||
b3Printf("hit = %f\n", hitResults[0].m_hitFraction);
|
||||
if (hitResults[0].m_hitFraction<1.f)
|
||||
{
|
||||
b3Assert(hitResults[0].m_hitResult0 >=0);
|
||||
b3Assert(hitResults[0].m_hitResult0 < m_collisionObjects.size());
|
||||
b3Vector3 hitNormalLocal = hitResults[0].m_hitNormal;
|
||||
btCollisionObject* colObj = m_collisionObjects[hitResults[0].m_hitResult0];
|
||||
LocalRayResult rayResult(colObj,0,(btVector3&)hitNormalLocal,hitResults[0].m_hitFraction);
|
||||
rayResult.m_hitFraction = hitResults[0].m_hitFraction;
|
||||
|
||||
resultCallback.addSingleResult(rayResult,true);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
void b3GpuDynamicsWorld::synchronizeMotionStates()
|
||||
{
|
||||
//iterate over all collision objects
|
||||
for ( int i=0;i<m_collisionObjects.size();i++)
|
||||
{
|
||||
btCollisionObject* colObj = m_collisionObjects[i];
|
||||
btRigidBody* body = btRigidBody::upcast(colObj);
|
||||
if (body)
|
||||
synchronizeSingleMotionState(body);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void b3GpuDynamicsWorld::synchronizeSingleMotionState(btRigidBody* body)
|
||||
{
|
||||
btAssert(body);
|
||||
|
||||
if (body->getMotionState() && !body->isStaticOrKinematicObject())
|
||||
{
|
||||
//we need to call the update at least once, even for sleeping objects
|
||||
//otherwise the 'graphics' transform never updates properly
|
||||
const btTransform& centerOfMassWorldTrans = body->getWorldTransform();
|
||||
body->getMotionState()->setWorldTransform(centerOfMassWorldTrans);
|
||||
}
|
||||
}
|
||||
|
||||
void b3GpuDynamicsWorld::debugDrawWorld()
|
||||
{
|
||||
BT_PROFILE("debugDrawWorld");
|
||||
|
||||
btCollisionWorld::debugDrawWorld();
|
||||
}
|
||||
|
||||
void b3GpuDynamicsWorld::addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies)
|
||||
{
|
||||
constraint->setUserConstraintPtr(0);
|
||||
|
||||
m_constraints.push_back(constraint);
|
||||
|
||||
switch (constraint->getConstraintType())
|
||||
{
|
||||
case POINT2POINT_CONSTRAINT_TYPE:
|
||||
{
|
||||
btPoint2PointConstraint* p = (btPoint2PointConstraint*) constraint;
|
||||
int rbA = p->getRigidBodyA().getUserIndex();
|
||||
int rbB = p->getRigidBodyB().getUserIndex();
|
||||
btVector3 pivotInB = p->getPivotInB();
|
||||
if (rbB<=0)
|
||||
{
|
||||
pivotInB = p->getRigidBodyA().getWorldTransform()*p->getPivotInA();
|
||||
rbB = m_staticBody->getUserIndex();
|
||||
}
|
||||
if (rbA>=0 && rbB>=0)
|
||||
{
|
||||
b3Point2PointConstraint* p2p = new b3Point2PointConstraint(rbA,rbB, (const b3Vector3&)p->getPivotInA(),(const b3Vector3&)pivotInB);
|
||||
p2p->setBreakingImpulseThreshold(p->getBreakingImpulseThreshold());
|
||||
constraint->setUserConstraintPtr(p2p);
|
||||
m_rigidBodyPipeline->addConstraint(p2p);
|
||||
} else
|
||||
{
|
||||
b3Error("invalid body index in addConstraint,b3Point2PointConstraint\n");
|
||||
}
|
||||
break;
|
||||
}
|
||||
case D6_CONSTRAINT_TYPE:
|
||||
{
|
||||
btGeneric6DofConstraint* dof2 = (btGeneric6DofConstraint*) constraint;
|
||||
const b3RigidBodyCL* bodiesCL = m_np->getBodiesCpu();
|
||||
|
||||
int rbA = dof2->getRigidBodyA().getUserIndex();
|
||||
int rbB = dof2->getRigidBodyB().getUserIndex();
|
||||
|
||||
btTransform frameInA = dof2->getFrameOffsetB();
|
||||
btTransform frameInB = dof2->getFrameOffsetB();
|
||||
|
||||
if (rbA<=0)
|
||||
{
|
||||
frameInA = dof2->getRigidBodyB().getWorldTransform()*dof2->getFrameOffsetB();
|
||||
rbA = m_staticBody->getUserIndex();
|
||||
}
|
||||
|
||||
if (rbB<=0)
|
||||
{
|
||||
frameInB = dof2->getRigidBodyA().getWorldTransform()*dof2->getFrameOffsetA();
|
||||
rbB = m_staticBody->getUserIndex();
|
||||
}
|
||||
if (rbA>=0 && rbB>=0)
|
||||
{
|
||||
b3Generic6DofConstraint* dof3 = new b3Generic6DofConstraint(rbA,rbB,(b3Transform&)frameInA,(b3Transform&)frameInB,false,bodiesCL);//(();//(rbA,rbB, (const b3Vector3&)p->getPivotInA(),(const b3Vector3&)pivotInB);
|
||||
{
|
||||
btVector3 limit(0,0,0);
|
||||
dof2->getLinearLowerLimit(limit);
|
||||
dof3->setLinearLowerLimit((const b3Vector3&)limit);
|
||||
|
||||
dof2->setLinearUpperLimit(limit);
|
||||
dof3->setLinearUpperLimit((const b3Vector3&)limit);
|
||||
|
||||
dof2->setAngularLowerLimit(limit);
|
||||
dof3->setAngularLowerLimit((const b3Vector3&)limit);
|
||||
|
||||
dof2->setAngularUpperLimit(limit);
|
||||
dof3->setAngularUpperLimit((const b3Vector3&)limit);
|
||||
/* for (int i=0;i<6;i++)
|
||||
{
|
||||
dof3->setParam(BT_CONSTRAINT_STOP_CFM,dof2->getParam(BT_CONSTRAINT_STOP_CFM,i),i);
|
||||
dof3->setParam(BT_CONSTRAINT_STOP_ERP,dof2->getParam(BT_CONSTRAINT_STOP_ERP,i),i);
|
||||
}
|
||||
*/
|
||||
dof3->setBreakingImpulseThreshold(dof2->getBreakingImpulseThreshold());
|
||||
}
|
||||
// p2p->setBreakingImpulseThreshold(p->getBreakingImpulseThreshold());
|
||||
constraint->setUserConstraintPtr(dof3);
|
||||
m_rigidBodyPipeline->addConstraint(dof3);
|
||||
} else
|
||||
{
|
||||
b3Error("invalid body index in addConstraint, btGeneric6DofConstraint.\n");
|
||||
}
|
||||
|
||||
// b3Generic6DofConstraint
|
||||
break;
|
||||
}
|
||||
default:
|
||||
b3Warning("Warning: b3GpuDynamicsWorld::addConstraint with unsupported constraint type\n");
|
||||
};
|
||||
|
||||
|
||||
}
|
||||
|
||||
void b3GpuDynamicsWorld::removeConstraint(btTypedConstraint* constraint)
|
||||
{
|
||||
b3TypedConstraint* c = (b3TypedConstraint*) constraint->getUserConstraintPtr();
|
||||
|
||||
if (c)
|
||||
{
|
||||
this->m_rigidBodyPipeline->removeConstraint(c);
|
||||
delete c;
|
||||
}
|
||||
m_constraints.remove(constraint);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user