static void DrawAabb(IDebugDraw* debugDrawer,const SimdVector3& from,const SimdVector3& to,const SimdVector3& color) { SimdVector3 halfExtents = (to-from)* 0.5f; SimdVector3 center = (to+from) *0.5f; int i,j; SimdVector3 edgecoord(1.f,1.f,1.f),pa,pb; for (i=0;i<4;i++) { for (j=0;j<3;j++) { pa = SimdVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1], edgecoord[2]*halfExtents[2]); pa+=center; int othercoord = j%3; edgecoord[othercoord]*=-1.f; pb = SimdVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1], edgecoord[2]*halfExtents[2]); pb+=center; debugDrawer->DrawLine(pa,pb,color); } edgecoord = SimdVector3(-1.f,-1.f,-1.f); if (i<3) edgecoord[i]*=-1.f; } }
void ColladaDemo::initPhysics(const char* filename) { m_cameraUp = SimdVector3(0,0,1); m_forwardAxis = 1; ///Setup a Physics Simulation Environment CollisionDispatcher* dispatcher = new CollisionDispatcher(); SimdVector3 worldAabbMin(-10000,-10000,-10000); SimdVector3 worldAabbMax(10000,10000,10000); OverlappingPairCache* broadphase = new AxisSweep3(worldAabbMin,worldAabbMax); //BroadphaseInterface* broadphase = new SimpleBroadphase(); m_physicsEnvironmentPtr = new CcdPhysicsEnvironment(dispatcher,broadphase); m_physicsEnvironmentPtr->setDeactivationTime(2.f); m_physicsEnvironmentPtr->setGravity(0,0,-10); m_physicsEnvironmentPtr->setDebugDrawer(&debugDrawer); MyColladaConverter* converter = new MyColladaConverter(this); bool result = converter->load(filename); if (result) { result = converter->convert(); } if (result) { gColladaConverter = converter; } else { gColladaConverter = 0; } }
//clear the simplex, remove all the vertices void VoronoiSimplexSolver::reset() { m_cachedValidClosest = false; m_numVertices = 0; m_needsUpdate = true; m_lastW = SimdVector3(1e30f,1e30f,1e30f); m_cachedBC.Reset(); }
void renderme() { float m[16]; int i; for (i=0;i<numObjects;i++) { SimdTransform transA; transA.setIdentity(); float pos[3]; float rot[4]; ms[i].getWorldPosition(pos[0],pos[1],pos[2]); ms[i].getWorldOrientation(rot[0],rot[1],rot[2],rot[3]); SimdQuaternion q(rot[0],rot[1],rot[2],rot[3]); transA.setRotation(q); SimdPoint3 dpos; dpos.setValue(pos[0],pos[1],pos[2]); transA.setOrigin( dpos ); transA.getOpenGLMatrix( m ); SimdVector3 wireColor(0.f,0.f,1.f); //wants deactivation ///color differently for active, sleeping, wantsdeactivation states if (physObjects[i]->GetRigidBody()->GetActivationState() == 1) //active { wireColor = SimdVector3 (1.f,0.f,0.f); } if (physObjects[i]->GetRigidBody()->GetActivationState() == 2) //ISLAND_SLEEPING { wireColor = SimdVector3 (0.f,1.f,0.f); } char extraDebug[125]; //sprintf(extraDebug,"islId, Body=%i , %i",physObjects[i]->GetRigidBody()->m_islandTag1,physObjects[i]->GetRigidBody()->m_debugBodyId); shapePtr[shapeIndex[i]]->SetExtraDebugInfo(extraDebug); GL_ShapeDrawer::DrawOpenGL(m,shapePtr[shapeIndex[i]],wireColor,getDebugMode()); } }
int main(int argc,char** argv) { raytracePicture = new RenderTexture(screenWidth,screenHeight); myBox.SetMargin(0.02f); myCone.SetMargin(0.2f); simplex.SetSimplexSolver(&simplexSolver); simplex.AddVertex(SimdPoint3(-1,0,-1)); simplex.AddVertex(SimdPoint3(1,0,-1)); simplex.AddVertex(SimdPoint3(0,0,1)); simplex.AddVertex(SimdPoint3(0,1,0)); /// convex hull of 5 spheres #define NUM_SPHERES 5 SimdVector3 inertiaHalfExtents(10.f,10.f,10.f); SimdVector3 positions[NUM_SPHERES] = { SimdVector3(-1.2f, -0.3f, 0.f), SimdVector3(0.8f, -0.3f, 0.f), SimdVector3(0.5f, 0.6f, 0.f), SimdVector3(-0.5f, 0.6f, 0.f), SimdVector3(0.f, 0.f, 0.f) }; SimdScalar radi[NUM_SPHERES] = { 0.35f,0.35f,0.45f,0.40f,0.40f }; MultiSphereShape multiSphereShape(inertiaHalfExtents,positions,radi,NUM_SPHERES); ConvexHullShape convexHullShape(positions,3); //choose shape shapePtr[0] = &myCone; shapePtr[1] =&simplex; shapePtr[2] =&convexHullShape; shapePtr[3] =&myMink;//myBox; simplex.SetMargin(0.3f); setCameraDistance(6.f); return glutmain(argc, argv,screenWidth,screenHeight,"Minkowski-Sum Raytracer Demo"); }
void LinearConvexCastDemo::initPhysics() { setCameraDistance(30.f); tr[0].setOrigin(SimdVector3(0,0,0)); tr[1].setOrigin(SimdVector3(0,10,0)); SimdMatrix3x3 basisA; basisA.setValue(0.99999958f,0.00022980258f,0.00090992288f, -0.00029313788f,0.99753088f,0.070228584f, -0.00089153741f,-0.070228823f,0.99753052f); SimdMatrix3x3 basisB; basisB.setValue(1.0000000f,4.4865553e-018f,-4.4410586e-017f, 4.4865495e-018f,0.97979438f,0.20000751f, 4.4410586e-017f,-0.20000751f,0.97979438f); tr[0].setBasis(basisA); tr[1].setBasis(basisB); SimdVector3 boxHalfExtentsA(0.2,4,4); SimdVector3 boxHalfExtentsB(6,6,6); BoxShape* boxA = new BoxShape(boxHalfExtentsA); /* BU_Simplex1to4 boxB; boxB.AddVertex(SimdPoint3(-5,0,-5)); boxB.AddVertex(SimdPoint3(5,0,-5)); boxB.AddVertex(SimdPoint3(0,0,5)); boxB.AddVertex(SimdPoint3(0,5,0)); */ BoxShape* boxB = new BoxShape(boxHalfExtentsB); shapePtr[0] = boxA; shapePtr[1] = boxB; shapePtr[0]->SetMargin(0.01f); shapePtr[1]->SetMargin(0.01f); SimdTransform tr; tr.setIdentity(); }
AxisSweep3::AxisSweep3(const SimdPoint3& worldAabbMin,const SimdPoint3& worldAabbMax, int maxHandles, int maxOverlaps) :OverlappingPairCache(maxOverlaps) { //assert(bounds.HasVolume()); // 1 handle is reserved as sentinel assert(maxHandles > 1 && maxHandles < 32767); // doesn't need this limit right now, but I may want to use unsigned short indexes into overlaps array assert(maxOverlaps > 0 && maxOverlaps < 65536); // init bounds m_worldAabbMin = worldAabbMin; m_worldAabbMax = worldAabbMax; SimdVector3 aabbSize = m_worldAabbMax - m_worldAabbMin; m_quantize = SimdVector3(65535.0f,65535.0f,65535.0f) / aabbSize; // allocate handles buffer and put all handles on free list m_pHandles = new Handle[maxHandles]; m_maxHandles = maxHandles; m_numHandles = 0; // handle 0 is reserved as the null index, and is also used as the sentinel m_firstFreeHandle = 1; { for (int i = m_firstFreeHandle; i < maxHandles; i++) m_pHandles[i].SetNextFree(i + 1); m_pHandles[maxHandles - 1].SetNextFree(0); } { // allocate edge buffers for (int i = 0; i < 3; i++) m_pEdges[i] = new Edge[maxHandles * 2]; } //removed overlap management // make boundary sentinels m_pHandles[0].m_clientObject = 0; for (int axis = 0; axis < 3; axis++) { m_pHandles[0].m_minEdges[axis] = 0; m_pHandles[0].m_maxEdges[axis] = 1; m_pEdges[axis][0].m_pos = 0; m_pEdges[axis][0].m_handle = 0; m_pEdges[axis][1].m_pos = 0xffff; m_pEdges[axis][1].m_handle = 0; } }
int main(int argc,char** argv) { clientResetScene(); SimdMatrix3x3 basisA; basisA.setIdentity(); SimdMatrix3x3 basisB; basisB.setIdentity(); objects[0].m_worldTransform.setBasis(basisA); objects[1].m_worldTransform.setBasis(basisB); SimdPoint3 points0[3]={SimdPoint3(1,0,0),SimdPoint3(0,1,0),SimdPoint3(0,0,1)}; SimdPoint3 points1[5]={SimdPoint3(1,0,0),SimdPoint3(0,1,0),SimdPoint3(0,0,1),SimdPoint3(0,0,-1),SimdPoint3(-1,-1,0)}; BoxShape boxA(SimdVector3(1,1,1)); BoxShape boxB(SimdVector3(0.5,0.5,0.5)); //ConvexHullShape hullA(points0,3); //hullA.setLocalScaling(SimdVector3(3,3,3)); //ConvexHullShape hullB(points1,4); //hullB.setLocalScaling(SimdVector3(4,4,4)); objects[0].m_collisionShape = &boxA;//&hullA; objects[1].m_collisionShape = &boxB;//&hullB; CollisionDispatcher dispatcher; //SimpleBroadphase broadphase; SimdVector3 worldAabbMin(-1000,-1000,-1000); SimdVector3 worldAabbMax(1000,1000,1000); AxisSweep3 broadphase(worldAabbMin,worldAabbMax); collisionWorld = new CollisionWorld(&dispatcher,&broadphase); collisionWorld->AddCollisionObject(&objects[0]); collisionWorld->AddCollisionObject(&objects[1]); return glutmain(argc, argv,screenWidth,screenHeight,"Collision Interface Demo"); }
int main(int argc,char** argv) { setCameraDistance(20.f); tr[0].setOrigin(SimdVector3(0.0013328250f,8.1363249f,7.0390840f)); tr[1].setOrigin(SimdVector3(0.00000000f,9.1262732f,2.0343180f)); //tr[0].setOrigin(SimdVector3(0,0,0)); //tr[1].setOrigin(SimdVector3(0,10,0)); SimdMatrix3x3 basisA; basisA.setValue(0.99999958f,0.00022980258f,0.00090992288f, -0.00029313788f,0.99753088f,0.070228584f, -0.00089153741f,-0.070228823f,0.99753052f); SimdMatrix3x3 basisB; basisB.setValue(1.0000000f,4.4865553e-018f,-4.4410586e-017f, 4.4865495e-018f,0.97979438f,0.20000751f, 4.4410586e-017f,-0.20000751f,0.97979438f); tr[0].setBasis(basisA); tr[1].setBasis(basisB); SimdVector3 boxHalfExtentsA(1.0000004768371582f,1.0000004768371582f,1.0000001192092896f); SimdVector3 boxHalfExtentsB(3.2836332321166992f,3.2836332321166992f,3.2836320400238037f); BoxShape boxA(boxHalfExtentsA); BoxShape boxB(boxHalfExtentsB); shapePtr[0] = &boxA; shapePtr[1] = &boxB; SimdTransform tr; tr.setIdentity(); return glutmain(argc, argv,screenWidth,screenHeight,"Collision Demo"); }
void Raytracer::initPhysics() { raytracePicture = new RenderTexture(screenWidth,screenHeight); myBox.SetMargin(0.02f); myCone.SetMargin(0.2f); simplex.SetSimplexSolver(&simplexSolver); simplex.AddVertex(SimdPoint3(-1,0,-1)); simplex.AddVertex(SimdPoint3(1,0,-1)); simplex.AddVertex(SimdPoint3(0,0,1)); simplex.AddVertex(SimdPoint3(0,1,0)); /// convex hull of 5 spheres #define NUM_SPHERES 5 SimdVector3 inertiaHalfExtents(10.f,10.f,10.f); SimdVector3 positions[NUM_SPHERES] = { SimdVector3(-1.2f, -0.3f, 0.f), SimdVector3(0.8f, -0.3f, 0.f), SimdVector3(0.5f, 0.6f, 0.f), SimdVector3(-0.5f, 0.6f, 0.f), SimdVector3(0.f, 0.f, 0.f) }; // MultiSphereShape* multiSphereShape = new MultiSphereShape(inertiaHalfExtents,positions,radi,NUM_SPHERES); ConvexHullShape* convexHullShape = new ConvexHullShape(positions,3); //choose shape shapePtr[0] = &myCone; shapePtr[1] =&simplex; shapePtr[2] =convexHullShape; shapePtr[3] =&myMink;//myBox;//multiSphereShape simplex.SetMargin(0.3f); }
bool createBoxShape( int shapeIndex ) { //#ifdef _DEBUG //static bool b = true; // //if ( b ) //{ // g_pConvexShapes[ shapeIndex ] = new BoxShape( SimdVector3( 1, 1, 1 ) ); // g_pConvexShapes[ shapeIndex ]->SetMargin( 0.05 ); // g_convexShapesTransform[ shapeIndex ].setIdentity(); // SimdMatrix3x3 basis( 0.99365157, 0.024418538, -0.10981932, // -0.025452739, 0.99964380, -0.0080251107, // 0.10958424, 0.010769366, 0.99391919 ); // g_convexShapesTransform[ shapeIndex ].setOrigin( SimdVector3( 4.4916530, -19.059078, -0.22695254 ) ); // g_convexShapesTransform[ shapeIndex ].setBasis( basis ); // b = false; //} //else //{ // g_pConvexShapes[ shapeIndex ] = new BoxShape( SimdVector3( 25, 10, 25 ) ); // g_pConvexShapes[ shapeIndex ]->SetMargin( 0.05 ); // //SimdMatrix3x3 basis( 0.658257, 0.675022, -0.333709, // // -0.333120, 0.658556, 0.675023, // // 0.675314, -0.333120, 0.658256 ); // g_convexShapesTransform[ shapeIndex ].setIdentity(); // g_convexShapesTransform[ shapeIndex ].setOrigin( SimdVector3( 0, -30, 0/*0.326090, -0.667531, 0.214331*/ ) ); // //g_convexShapesTransform[ shapeIndex ].setBasis( basis ); //} //#endif g_pConvexShapes[ shapeIndex ] = new BoxShape( SimdVector3( 1, 1, 1 ) ); g_pConvexShapes[ shapeIndex ]->SetMargin( 1e-1 ); g_convexShapesTransform[ shapeIndex ].setIdentity(); g_convexShapesTransform[ shapeIndex ].setOrigin( randomPosition( g_sceneVolumeMin, g_sceneVolumeMax ) ); return true; }
void CcdPhysicsEnvironment::setGravity(float x,float y,float z) { m_gravity = SimdVector3(x,y,z); std::vector<CcdPhysicsController*>::iterator i; //todo: review this gravity stuff for (i=m_controllers.begin(); !(i==m_controllers.end()); i++) { CcdPhysicsController* ctrl = (*i); ctrl->GetRigidBody()->setGravity(m_gravity); } }
PHY_IPhysicsController* CcdPhysicsEnvironment::CreateSphereController(float radius,const PHY__Vector3& position) { CcdConstructionInfo cinfo; cinfo.m_collisionShape = new SphereShape(radius); cinfo.m_MotionState = 0; cinfo.m_physicsEnv = this; cinfo.m_collisionFlags |= CollisionObject::noContactResponse; DefaultMotionState* motionState = new DefaultMotionState(); cinfo.m_MotionState = motionState; motionState->m_worldTransform.setIdentity(); motionState->m_worldTransform.setOrigin(SimdVector3(position[0],position[1],position[2])); CcdPhysicsController* sphereController = new CcdPhysicsController(cinfo); return sphereController; }
void clientKeyboard(unsigned char key, int x, int y) { if (key == '.') { shootBox(SimdVector3(0,0,0)); } if (key == '+') { bulletSpeed += 10.f; } if (key == '-') { bulletSpeed -= 10.f; } defaultKeyboard(key, x, y); }
virtual void AddConvexVerticesCollider(std::vector<SimdVector3>& vertices, bool isEntity, const SimdVector3& entityTargetLocation) { ///perhaps we can do something special with entities (isEntity) ///like adding a collision Triggering (as example) if (vertices.size() > 0) { bool isDynamic = false; float mass = 0.f; SimdTransform startTransform; //can use a shift startTransform.setIdentity(); startTransform.setOrigin(SimdVector3(0,0,-10.f)); //this create an internal copy of the vertices CollisionShape* shape = new ConvexHullShape(&vertices[0],vertices.size()); m_demoApp->LocalCreatePhysicsObject(isDynamic, mass, startTransform,shape); } }
CcdPhysicsEnvironment::CcdPhysicsEnvironment(Dispatcher* dispatcher,OverlappingPairCache* pairCache) :m_scalingPropagated(false), m_numIterations(10), m_numTimeSubSteps(1), m_ccdMode(0), m_solverType(-1), m_profileTimings(0), m_enableSatCollisionDetection(false) { for (int i=0;i<PHY_NUM_RESPONSE;i++) { m_triggerCallbacks[i] = 0; } //if (!dispatcher) // dispatcher = new CollisionDispatcher(); if(!pairCache) { //todo: calculate/let user specify this world sizes SimdVector3 worldMin(-10000,-10000,-10000); SimdVector3 worldMax(10000,10000,10000); pairCache = new AxisSweep3(worldMin,worldMax); //broadphase = new SimpleBroadphase(); } setSolverType(1);//issues with quickstep and memory allocations m_collisionWorld = new CollisionWorld(dispatcher,pairCache); m_debugDrawer = 0; m_gravity = SimdVector3(0.f,-10.f,0.f); m_islandManager = new SimulationIslandManager(); }
void PolyhedralConvexShape::CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia) { //not yet, return box inertia float margin = GetMargin(); SimdTransform ident; ident.setIdentity(); SimdVector3 aabbMin,aabbMax; GetAabb(ident,aabbMin,aabbMax); SimdVector3 halfExtents = (aabbMax-aabbMin)*0.5f; SimdScalar lx=2.f*(halfExtents.x()+margin); SimdScalar ly=2.f*(halfExtents.y()+margin); SimdScalar lz=2.f*(halfExtents.z()+margin); const SimdScalar x2 = lx*lx; const SimdScalar y2 = ly*ly; const SimdScalar z2 = lz*lz; const SimdScalar scaledmass = mass * 0.08333333f; inertia = scaledmass * (SimdVector3(y2+z2,x2+z2,x2+y2)); }
void SimulationIsland::UpdateAabbs(IDebugDraw* debugDrawer,BroadphaseInterface* scene,float timeStep) { std::vector<CcdPhysicsController*>::iterator i; // // update aabbs, only for moving objects (!) // for (i=m_controllers.begin(); !(i==m_controllers.end()); i++) { CcdPhysicsController* ctrl = (*i); RigidBody* body = ctrl->GetRigidBody(); SimdPoint3 minAabb,maxAabb; CollisionShape* shapeinterface = ctrl->GetCollisionShape(); shapeinterface->CalculateTemporalAabb(body->getCenterOfMassTransform(), body->getLinearVelocity(), //body->getAngularVelocity(), SimdVector3(0.f,0.f,0.f),//no angular effect for now //body->getAngularVelocity(), timeStep,minAabb,maxAabb); SimdVector3 manifoldExtraExtents(gContactBreakingTreshold,gContactBreakingTreshold,gContactBreakingTreshold); minAabb -= manifoldExtraExtents; maxAabb += manifoldExtraExtents; BroadphaseProxy* bp = body->m_broadphaseHandle; if (bp) { SimdVector3 color (1,1,0); class IDebugDraw* m_debugDrawer = 0; /* if (m_debugDrawer) { //draw aabb switch (body->GetActivationState()) { case ISLAND_SLEEPING: { color.setValue(1,1,1); break; } case WANTS_DEACTIVATION: { color.setValue(0,0,1); break; } case ACTIVE_TAG: { break; } case DISABLE_DEACTIVATION: { color.setValue(1,0,1); }; }; if (m_debugDrawer->GetDebugMode() & IDebugDraw::DBG_DrawAabb) { DrawAabb(m_debugDrawer,minAabb,maxAabb,color); } } */ if ( (maxAabb-minAabb).length2() < 1e12f) { scene->SetAabb(bp,minAabb,maxAabb); } else { //something went wrong, investigate //removeCcdPhysicsController(ctrl); body->SetActivationState(DISABLE_SIMULATION); static bool reportMe = true; if (reportMe) { reportMe = false; printf("Overflow in AABB, object removed from simulation \n"); printf("If you can reproduce this, please email [email protected]\n"); printf("Please include above information, your Platform, version of OS.\n"); printf("Thanks.\n"); } } } } }
BU_CollisionPair::BU_CollisionPair(const PolyhedralConvexShape* convexA,const PolyhedralConvexShape* convexB,SimdScalar tolerance) : m_convexA(convexA),m_convexB(convexB),m_screwing(SimdVector3(0,0,0),SimdVector3(0,0,0)), m_tolerance(tolerance) { }
void CcdPhysicsEnvironment::addCcdPhysicsController(CcdPhysicsController* ctrl) { RigidBody* body = ctrl->GetRigidBody(); //this m_userPointer is just used for triggers, see CallbackTriggers body->m_userPointer = ctrl; body->setGravity( m_gravity ); m_controllers.push_back(ctrl); m_collisionWorld->AddCollisionObject(body,ctrl->GetCollisionFilterGroup(),ctrl->GetCollisionFilterMask()); assert(body->m_broadphaseHandle); BroadphaseInterface* scene = GetBroadphase(); CollisionShape* shapeinterface = ctrl->GetCollisionShape(); assert(shapeinterface); const SimdTransform& t = ctrl->GetRigidBody()->getCenterOfMassTransform(); body->m_cachedInvertedWorldTransform = body->m_worldTransform.inverse(); SimdPoint3 minAabb,maxAabb; shapeinterface->GetAabb(t,minAabb,maxAabb); float timeStep = 0.02f; //extent it with the motion SimdVector3 linMotion = body->getLinearVelocity()*timeStep; float maxAabbx = maxAabb.getX(); float maxAabby = maxAabb.getY(); float maxAabbz = maxAabb.getZ(); float minAabbx = minAabb.getX(); float minAabby = minAabb.getY(); float minAabbz = minAabb.getZ(); if (linMotion.x() > 0.f) maxAabbx += linMotion.x(); else minAabbx += linMotion.x(); if (linMotion.y() > 0.f) maxAabby += linMotion.y(); else minAabby += linMotion.y(); if (linMotion.z() > 0.f) maxAabbz += linMotion.z(); else minAabbz += linMotion.z(); minAabb = SimdVector3(minAabbx,minAabby,minAabbz); maxAabb = SimdVector3(maxAabbx,maxAabby,maxAabbz); }
int main(int argc,char** argv) { int i; for (i=0;i<numObjects;i++) { if (i>0) { shapePtr[i] = prebuildShapePtr[1]; shapeIndex[i] = 1;//sphere } else { shapeIndex[i] = 0; shapePtr[i] = prebuildShapePtr[0]; } } ConvexDecomposition::WavefrontObj wo; char* filename = "file.obj"; tcount = wo.loadObj(filename); class MyConvexDecomposition : public ConvexDecomposition::ConvexDecompInterface { public: MyConvexDecomposition (FILE* outputFile) :mBaseCount(0), mHullCount(0), mOutputFile(outputFile) { } virtual void ConvexDecompResult(ConvexDecomposition::ConvexResult &result) { TriangleMesh* trimesh = new TriangleMesh(); SimdVector3 localScaling(6.f,6.f,6.f); //export data to .obj printf("ConvexResult\n"); if (mOutputFile) { fprintf(mOutputFile,"## Hull Piece %d with %d vertices and %d triangles.\r\n", mHullCount, result.mHullVcount, result.mHullTcount ); fprintf(mOutputFile,"usemtl Material%i\r\n",mBaseCount); fprintf(mOutputFile,"o Object%i\r\n",mBaseCount); for (unsigned int i=0; i<result.mHullVcount; i++) { const float *p = &result.mHullVertices[i*3]; fprintf(mOutputFile,"v %0.9f %0.9f %0.9f\r\n", p[0], p[1], p[2] ); } //calc centroid, to shift vertices around center of mass centroids[numObjects] = SimdVector3(0,0,0); if ( 1 ) { const unsigned int *src = result.mHullIndices; for (unsigned int i=0; i<result.mHullTcount; i++) { unsigned int index0 = *src++; unsigned int index1 = *src++; unsigned int index2 = *src++; SimdVector3 vertex0(result.mHullVertices[index0*3], result.mHullVertices[index0*3+1],result.mHullVertices[index0*3+2]); SimdVector3 vertex1(result.mHullVertices[index1*3], result.mHullVertices[index1*3+1],result.mHullVertices[index1*3+2]); SimdVector3 vertex2(result.mHullVertices[index2*3], result.mHullVertices[index2*3+1],result.mHullVertices[index2*3+2]); vertex0 *= localScaling; vertex1 *= localScaling; vertex2 *= localScaling; centroids[numObjects] += vertex0; centroids[numObjects]+= vertex1; centroids[numObjects]+= vertex2; } } centroids[numObjects] *= 1.f/(float(result.mHullTcount) * 3); if ( 1 ) { const unsigned int *src = result.mHullIndices; for (unsigned int i=0; i<result.mHullTcount; i++) { unsigned int index0 = *src++; unsigned int index1 = *src++; unsigned int index2 = *src++; SimdVector3 vertex0(result.mHullVertices[index0*3], result.mHullVertices[index0*3+1],result.mHullVertices[index0*3+2]); SimdVector3 vertex1(result.mHullVertices[index1*3], result.mHullVertices[index1*3+1],result.mHullVertices[index1*3+2]); SimdVector3 vertex2(result.mHullVertices[index2*3], result.mHullVertices[index2*3+1],result.mHullVertices[index2*3+2]); vertex0 *= localScaling; vertex1 *= localScaling; vertex2 *= localScaling; vertex0 -= centroids[numObjects]; vertex1 -= centroids[numObjects]; vertex2 -= centroids[numObjects]; trimesh->AddTriangle(vertex0,vertex1,vertex2); index0+=mBaseCount; index1+=mBaseCount; index2+=mBaseCount; fprintf(mOutputFile,"f %d %d %d\r\n", index0+1, index1+1, index2+1 ); } } shapeIndex[numObjects] = numObjects; shapePtr[numObjects++] = new ConvexTriangleMeshShape(trimesh); mBaseCount+=result.mHullVcount; // advance the 'base index' counter. } } int mBaseCount; int mHullCount; FILE* mOutputFile; }; if (tcount) { numObjects = 1; //always have the ground object first TriangleMesh* trimesh = new TriangleMesh(); SimdVector3 localScaling(6.f,6.f,6.f); for (int i=0;i<wo.mTriCount;i++) { int index0 = wo.mIndices[i*3]; int index1 = wo.mIndices[i*3+1]; int index2 = wo.mIndices[i*3+2]; SimdVector3 vertex0(wo.mVertices[index0*3], wo.mVertices[index0*3+1],wo.mVertices[index0*3+2]); SimdVector3 vertex1(wo.mVertices[index1*3], wo.mVertices[index1*3+1],wo.mVertices[index1*3+2]); SimdVector3 vertex2(wo.mVertices[index2*3], wo.mVertices[index2*3+1],wo.mVertices[index2*3+2]); vertex0 *= localScaling; vertex1 *= localScaling; vertex2 *= localScaling; trimesh->AddTriangle(vertex0,vertex1,vertex2); } shapePtr[numObjects++] = new ConvexTriangleMeshShape(trimesh); } if (tcount) { char outputFileName[512]; strcpy(outputFileName,filename); char *dot = strstr(outputFileName,"."); if ( dot ) *dot = 0; strcat(outputFileName,"_convex.obj"); FILE* outputFile = fopen(outputFileName,"wb"); unsigned int depth = 7; float cpercent = 5; float ppercent = 15; unsigned int maxv = 16; float skinWidth = 0.01; printf("WavefrontObj num triangles read %i",tcount); ConvexDecomposition::DecompDesc desc; desc.mVcount = wo.mVertexCount; desc.mVertices = wo.mVertices; desc.mTcount = wo.mTriCount; desc.mIndices = (unsigned int *)wo.mIndices; desc.mDepth = depth; desc.mCpercent = cpercent; desc.mPpercent = ppercent; desc.mMaxVertices = maxv; desc.mSkinWidth = skinWidth; MyConvexDecomposition convexDecomposition(outputFile); desc.mCallback = &convexDecomposition; //convexDecomposition.performConvexDecomposition(desc); ConvexBuilder cb(desc.mCallback); int ret = cb.process(desc); if (outputFile) fclose(outputFile); } CollisionDispatcher* dispatcher = new CollisionDispatcher(); SimdVector3 worldAabbMin(-10000,-10000,-10000); SimdVector3 worldAabbMax(10000,10000,10000); OverlappingPairCache* broadphase = new AxisSweep3(worldAabbMin,worldAabbMax); //OverlappingPairCache* broadphase = new SimpleBroadphase(); physicsEnvironmentPtr = new CcdPhysicsEnvironment(dispatcher,broadphase); physicsEnvironmentPtr->setDeactivationTime(2.f); physicsEnvironmentPtr->setGravity(0,-10,0); PHY_ShapeProps shapeProps; shapeProps.m_do_anisotropic = false; shapeProps.m_do_fh = false; shapeProps.m_do_rot_fh = false; shapeProps.m_friction_scaling[0] = 1.; shapeProps.m_friction_scaling[1] = 1.; shapeProps.m_friction_scaling[2] = 1.; shapeProps.m_inertia = 1.f; shapeProps.m_lin_drag = 0.2f; shapeProps.m_ang_drag = 0.1f; shapeProps.m_mass = 10.0f; PHY_MaterialProps materialProps; materialProps.m_friction = 10.5f; materialProps.m_restitution = 0.0f; CcdConstructionInfo ccdObjectCi; ccdObjectCi.m_friction = 0.5f; ccdObjectCi.m_linearDamping = shapeProps.m_lin_drag; ccdObjectCi.m_angularDamping = shapeProps.m_ang_drag; SimdTransform tr; tr.setIdentity(); for (i=0;i<numObjects;i++) { shapeProps.m_shape = shapePtr[shapeIndex[i]]; shapeProps.m_shape->SetMargin(0.05f); bool isDyna = i>0; //if (i==1) // isDyna=false; if (0)//i==1) { SimdQuaternion orn(0,0,0.1*SIMD_HALF_PI); ms[i].setWorldOrientation(orn.x(),orn.y(),orn.z(),orn[3]); } if (i>0) { switch (i) { case 1: { ms[i].setWorldPosition(0,10,0); //for testing, rotate the ground cube so the stack has to recover a bit break; } case 2: { ms[i].setWorldPosition(0,8,2); break; } default: ms[i].setWorldPosition(0,i*CUBE_HALF_EXTENTS*2 - CUBE_HALF_EXTENTS,0); } float quatIma0,quatIma1,quatIma2,quatReal; SimdQuaternion quat; SimdVector3 axis(0,0,1); SimdScalar angle=0.5f; quat.setRotation(axis,angle); ms[i].setWorldOrientation(quat.getX(),quat.getY(),quat.getZ(),quat[3]); } else { ms[i].setWorldPosition(0,-10+EXTRA_HEIGHT,0); } ccdObjectCi.m_MotionState = &ms[i]; ccdObjectCi.m_gravity = SimdVector3(0,0,0); ccdObjectCi.m_localInertiaTensor =SimdVector3(0,0,0); if (!isDyna) { shapeProps.m_mass = 0.f; ccdObjectCi.m_mass = shapeProps.m_mass; ccdObjectCi.m_collisionFlags = CollisionObject::isStatic; } else { shapeProps.m_mass = 1.f; ccdObjectCi.m_mass = shapeProps.m_mass; ccdObjectCi.m_collisionFlags = 0; } SimdVector3 localInertia; if (shapePtr[shapeIndex[i]]->GetShapeType() == EMPTY_SHAPE_PROXYTYPE) { //take inertia from first shape shapePtr[1]->CalculateLocalInertia(shapeProps.m_mass,localInertia); } else { shapePtr[shapeIndex[i]]->CalculateLocalInertia(shapeProps.m_mass,localInertia); } ccdObjectCi.m_localInertiaTensor = localInertia; ccdObjectCi.m_collisionShape = shapePtr[shapeIndex[i]]; physObjects[i]= new CcdPhysicsController( ccdObjectCi); // Only do CCD if motion in one timestep (1.f/60.f) exceeds CUBE_HALF_EXTENTS physObjects[i]->GetRigidBody()->m_ccdSquareMotionTreshold = CUBE_HALF_EXTENTS; //Experimental: better estimation of CCD Time of Impact: //physObjects[i]->GetRigidBody()->m_ccdSweptShereRadius = 0.5*CUBE_HALF_EXTENTS; physicsEnvironmentPtr->addCcdPhysicsController( physObjects[i]); if (i==1) { //physObjects[i]->SetAngularVelocity(0,0,-2,true); } physicsEnvironmentPtr->setDebugDrawer(&debugDrawer); } //create a constraint if (createConstraint) { //physObjects[i]->SetAngularVelocity(0,0,-2,true); int constraintId; float pivotX=CUBE_HALF_EXTENTS, pivotY=-CUBE_HALF_EXTENTS, pivotZ=CUBE_HALF_EXTENTS; float axisX=1,axisY=0,axisZ=0; HingeConstraint* hinge = 0; SimdVector3 pivotInA(CUBE_HALF_EXTENTS,-CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS); SimdVector3 pivotInB(-CUBE_HALF_EXTENTS,-CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS); SimdVector3 axisInA(0,1,0); SimdVector3 axisInB(0,-1,0); RigidBody* rb0 = physObjects[1]->GetRigidBody(); RigidBody* rb1 = physObjects[2]->GetRigidBody(); hinge = new HingeConstraint( *rb0, *rb1,pivotInA,pivotInB,axisInA,axisInB); physicsEnvironmentPtr->m_constraints.push_back(hinge); hinge->SetUserConstraintId(100); hinge->SetUserConstraintType(PHY_LINEHINGE_CONSTRAINT); } clientResetScene(); setCameraDistance(26.f); return glutmain(argc, argv,640,480,"Bullet Physics Demo. http://www.continuousphysics.com/Bullet/phpBB2/"); }
void LinearConvexCastDemo::displayCallback(void) { updateCamera(); glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glDisable(GL_LIGHTING); //GL_ShapeDrawer::DrawCoordSystem(); float m[16]; int i; for (i=0;i<numObjects;i++) { tr[i].getOpenGLMatrix( m ); GL_ShapeDrawer::DrawOpenGL(m,shapePtr[i],SimdVector3(1,1,1),getDebugMode()); } int shapeIndex = 1; SimdQuaternion orn; orn.setEuler(yaw,pitch,roll); tr[shapeIndex].setRotation(orn); if (m_stepping || m_singleStep) { m_singleStep = false; pitch += 0.005f; yaw += 0.01f; } SimdVector3 fromA(-25,11,0); SimdVector3 toA(15,11,0); SimdQuaternion ornFromA(0.f,0.f,0.f,1.f); SimdQuaternion ornToA(0.f,0.f,0.f,1.f); SimdTransform rayFromWorld(ornFromA,fromA); SimdTransform rayToWorld(ornToA,toA); tr[0] = rayFromWorld; if (drawLine) { glBegin(GL_LINES); glColor3f(0, 0, 1); glVertex3d(rayFromWorld.getOrigin().x(), rayFromWorld.getOrigin().y(),rayFromWorld.getOrigin().z()); glVertex3d(rayToWorld.getOrigin().x(),rayToWorld.getOrigin().y(),rayToWorld.getOrigin().z()); glEnd(); } //now perform a raycast on the shapes, in local (shape) space //choose one of the following lines for (i=1;i<numObjects;i++) { ContinuousConvexCollision convexCaster0(shapePtr[0],shapePtr[i],&gGjkSimplexSolver,0); GjkConvexCast convexCaster1(shapePtr[0],shapePtr[i],&gGjkSimplexSolver); //BU_CollisionPair (algebraic version) is currently broken, will look into this //BU_CollisionPair convexCaster2(shapePtr[0],shapePtr[i]); SubsimplexConvexCast convexCaster3(shapePtr[0],shapePtr[i],&gGjkSimplexSolver); gGjkSimplexSolver.reset(); ConvexCast::CastResult rayResult; if (convexCaster3.calcTimeOfImpact(rayFromWorld,rayToWorld,tr[i],tr[i],rayResult)) { glDisable(GL_DEPTH_TEST); SimdVector3 hitPoint; hitPoint.setInterpolate3(rayFromWorld.getOrigin(),rayToWorld.getOrigin(),rayResult.m_fraction); //draw the raycast result glBegin(GL_LINES); glColor3f(1, 1, 1); glVertex3d(rayFromWorld.getOrigin().x(), rayFromWorld.getOrigin().y(),rayFromWorld.getOrigin().z()); glVertex3d(hitPoint.x(),hitPoint.y(),hitPoint.z()); glEnd(); glEnable(GL_DEPTH_TEST); SimdTransform toTransWorld; toTransWorld = tr[0]; toTransWorld.setOrigin(hitPoint); toTransWorld.getOpenGLMatrix( m ); GL_ShapeDrawer::DrawOpenGL(m,shapePtr[0],SimdVector3(0,1,1),getDebugMode()); } } glFlush(); glutSwapBuffers(); }
virtual void ConvexDecompResult(ConvexDecomposition::ConvexResult &result) { TriangleMesh* trimesh = new TriangleMesh(); SimdVector3 localScaling(6.f,6.f,6.f); //export data to .obj printf("ConvexResult\n"); if (mOutputFile) { fprintf(mOutputFile,"## Hull Piece %d with %d vertices and %d triangles.\r\n", mHullCount, result.mHullVcount, result.mHullTcount ); fprintf(mOutputFile,"usemtl Material%i\r\n",mBaseCount); fprintf(mOutputFile,"o Object%i\r\n",mBaseCount); for (unsigned int i=0; i<result.mHullVcount; i++) { const float *p = &result.mHullVertices[i*3]; fprintf(mOutputFile,"v %0.9f %0.9f %0.9f\r\n", p[0], p[1], p[2] ); } //calc centroid, to shift vertices around center of mass centroids[numObjects] = SimdVector3(0,0,0); if ( 1 ) { const unsigned int *src = result.mHullIndices; for (unsigned int i=0; i<result.mHullTcount; i++) { unsigned int index0 = *src++; unsigned int index1 = *src++; unsigned int index2 = *src++; SimdVector3 vertex0(result.mHullVertices[index0*3], result.mHullVertices[index0*3+1],result.mHullVertices[index0*3+2]); SimdVector3 vertex1(result.mHullVertices[index1*3], result.mHullVertices[index1*3+1],result.mHullVertices[index1*3+2]); SimdVector3 vertex2(result.mHullVertices[index2*3], result.mHullVertices[index2*3+1],result.mHullVertices[index2*3+2]); vertex0 *= localScaling; vertex1 *= localScaling; vertex2 *= localScaling; centroids[numObjects] += vertex0; centroids[numObjects]+= vertex1; centroids[numObjects]+= vertex2; } } centroids[numObjects] *= 1.f/(float(result.mHullTcount) * 3); if ( 1 ) { const unsigned int *src = result.mHullIndices; for (unsigned int i=0; i<result.mHullTcount; i++) { unsigned int index0 = *src++; unsigned int index1 = *src++; unsigned int index2 = *src++; SimdVector3 vertex0(result.mHullVertices[index0*3], result.mHullVertices[index0*3+1],result.mHullVertices[index0*3+2]); SimdVector3 vertex1(result.mHullVertices[index1*3], result.mHullVertices[index1*3+1],result.mHullVertices[index1*3+2]); SimdVector3 vertex2(result.mHullVertices[index2*3], result.mHullVertices[index2*3+1],result.mHullVertices[index2*3+2]); vertex0 *= localScaling; vertex1 *= localScaling; vertex2 *= localScaling; vertex0 -= centroids[numObjects]; vertex1 -= centroids[numObjects]; vertex2 -= centroids[numObjects]; trimesh->AddTriangle(vertex0,vertex1,vertex2); index0+=mBaseCount; index1+=mBaseCount; index2+=mBaseCount; fprintf(mOutputFile,"f %d %d %d\r\n", index0+1, index1+1, index2+1 ); } } shapeIndex[numObjects] = numObjects; shapePtr[numObjects++] = new ConvexTriangleMeshShape(trimesh); mBaseCount+=result.mHullVcount; // advance the 'base index' counter. } }
void Raytracer::displayCallback() { updateCamera(); for (int i=0;i<numObjects;i++) { transforms[i].setIdentity(); SimdVector3 pos(-3.5f+i*2.5f,0.f,0.f); transforms[i].setOrigin( pos ); SimdQuaternion orn; if (i < 2) { orn.setEuler(yaw,pitch,roll); transforms[i].setRotation(orn); } } myMink.SetTransformA(SimdTransform(transforms[0].getRotation())); glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glDisable(GL_LIGHTING); if (once) { glGenTextures(1, &glTextureId); glBindTexture(GL_TEXTURE_2D,glTextureId ); once = 0; glPixelStorei(GL_UNPACK_ALIGNMENT, 1); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); } glDisable(GL_TEXTURE_2D); glDisable(GL_BLEND); #define RAYTRACER #ifdef RAYTRACER SimdVector4 rgba(1.f,0.f,0.f,0.5f); float top = 1.f; float bottom = -1.f; float nearPlane = 1.f; float tanFov = (top-bottom)*0.5f / nearPlane; float fov = 2.0 * atanf (tanFov); SimdVector3 rayFrom = getCameraPosition(); SimdVector3 rayForward = getCameraTargetPosition()-getCameraPosition(); rayForward.normalize(); float farPlane = 600.f; rayForward*= farPlane; SimdVector3 rightOffset; SimdVector3 vertical(0.f,1.f,0.f); SimdVector3 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; SimdVector3 rayToCenter = rayFrom + rayForward; SimdVector3 dHor = hor * 1.f/float(screenWidth); SimdVector3 dVert = vertical * 1.f/float(screenHeight); SimdTransform rayFromTrans; rayFromTrans.setIdentity(); rayFromTrans.setOrigin(rayFrom); SimdTransform rayFromLocal; SimdTransform rayToLocal; SphereShape pointShape(0.0f); ///clear texture for (int x=0;x<screenWidth;x++) { for (int y=0;y<screenHeight;y++) { SimdVector4 rgba(0.f,0.f,0.f,0.f); raytracePicture->SetPixel(x,y,rgba); } } ConvexCast::CastResult rayResult; SimdTransform rayToTrans; rayToTrans.setIdentity(); SimdVector3 rayTo; for (int x=0;x<screenWidth;x++) { for (int y=0;y<screenHeight;y++) { rayTo = rayToCenter - 0.5f * hor + 0.5f * vertical; rayTo += x * dHor; rayTo -= y * dVert; rayToTrans.setOrigin(rayTo); for (int s=0;s<numObjects;s++) { // rayFromLocal = transforms[s].inverse()* rayFromTrans; // rayToLocal = transforms[s].inverse()* rayToTrans; //choose the continuous collision detection method SubsimplexConvexCast convexCaster(&pointShape,shapePtr[s],&simplexSolver); //GjkConvexCast convexCaster(&pointShape,shapePtr[0],&simplexSolver); //ContinuousConvexCollision convexCaster(&pointShape,shapePtr[0],&simplexSolver,0); // BU_Simplex1to4 ptShape(SimdVector3(0,0,0));//algebraic needs features, doesnt use 'supporting vertex' // BU_CollisionPair convexCaster(&ptShape,shapePtr[0]); //reset previous result rayResult.m_fraction = 1.f; if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans,transforms[s],transforms[s],rayResult)) { //float fog = 1.f - 0.1f * rayResult.m_fraction; rayResult.m_normal.normalize(); SimdVector3 worldNormal; worldNormal = transforms[s].getBasis() *rayResult.m_normal; float light = worldNormal.dot(SimdVector3(0.4f,-1.f,-0.4f)); if (light < 0.2f) light = 0.2f; if (light > 1.f) light = 1.f; rgba = SimdVector4(light,light,light,1.f); raytracePicture->SetPixel(x,y,rgba); } else { //clear is already done //rgba = SimdVector4(0.f,0.f,0.f,0.f); //raytracePicture->SetPixel(x,y,rgba); } } } } #define TEST_PRINTF #ifdef TEST_PRINTF extern BMF_FontData BMF_font_helv10; raytracePicture->Printf("CCD RAYTRACER",&BMF_font_helv10); char buffer[256]; sprintf(buffer,"%d RAYS / Frame",screenWidth*screenHeight*numObjects); raytracePicture->Printf(buffer,&BMF_font_helv10,0,10); #endif //TEST_PRINTF glMatrixMode(GL_PROJECTION); glPushMatrix(); glLoadIdentity(); glFrustum(-1.0,1.0,-1.0,1.0,3,2020.0); glMatrixMode(GL_MODELVIEW); glPushMatrix(); glLoadIdentity(); // Reset The Modelview Matrix glTranslatef(0.0f,0.0f,-3.0f); // Move Into The Screen 5 Units glEnable(GL_TEXTURE_2D); glBindTexture(GL_TEXTURE_2D,glTextureId ); const unsigned char *ptr = raytracePicture->GetBuffer(); glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, raytracePicture->GetWidth(),raytracePicture->GetHeight(), 0, GL_RGBA, GL_UNSIGNED_BYTE, ptr); glEnable (GL_BLEND); glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); glColor4f (1,1,1,1); // alpha=0.5=half visible glBegin(GL_QUADS); glTexCoord2f(0.0f, 0.0f); glVertex2f(-1,1); glTexCoord2f(1.0f, 0.0f); glVertex2f(1,1); glTexCoord2f(1.0f, 1.0f); glVertex2f(1,-1); glTexCoord2f(0.0f, 1.0f); glVertex2f(-1,-1); glEnd(); glMatrixMode(GL_MODELVIEW); glPopMatrix(); glMatrixMode(GL_PROJECTION); glPopMatrix(); glMatrixMode(GL_MODELVIEW); #endif //RAYRACER glDisable(GL_TEXTURE_2D); glDisable(GL_DEPTH_TEST); GL_ShapeDrawer::DrawCoordSystem(); glPushMatrix(); /* /// normal opengl rendering float m[16]; int i; for (i=0;i<numObjects;i++) { transA.getOpenGLMatrix( m ); /// draw the simplex GL_ShapeDrawer::DrawOpenGL(m,shapePtr[i],SimdVector3(1,1,1)); /// calculate closest point from simplex to the origin, and draw this vector simplex.CalcClosest(m); } */ glPopMatrix(); pitch += 0.005f; yaw += 0.01f; glFlush(); glutSwapBuffers(); }
CcdPhysicsEnvironment* physicsEnvironmentPtr = 0; #define CUBE_HALF_EXTENTS 1 #define EXTRA_HEIGHT -20.f //GL_LineSegmentShape shapeE(SimdPoint3(-50,0,0), // SimdPoint3(50,0,0)); static const int numShapes = 4; CollisionShape* shapePtr[numShapes] = { ///Please don't make the box sizes larger then 1000: the collision detection will be inaccurate. ///See http://www.continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=346 new BoxShape (SimdVector3(450,10,450)), new BoxShape (SimdVector3(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS)), new SphereShape (CUBE_HALF_EXTENTS- 0.05f), //new ConeShape(CUBE_HALF_EXTENTS,2.f*CUBE_HALF_EXTENTS), //new BU_Simplex1to4(SimdPoint3(-1,-1,-1),SimdPoint3(1,-1,-1),SimdPoint3(-1,1,-1),SimdPoint3(0,0,1)), //new EmptyShape(), new BoxShape (SimdVector3(0.4,1,0.8)) }; ////////////////////////////////////
const int numObjects = 4; /// simplex contains the vertices, and some extra code to draw and debug GL_Simplex1to4 simplex; ConvexShape* shapePtr[maxNumObjects]; SimdTransform transforms[maxNumObjects]; RenderTexture* raytracePicture = 0; int screenWidth = 128; int screenHeight = 128; GLuint glTextureId; SphereShape mySphere(1); BoxShape myBox(SimdVector3(0.4f,0.4f,0.4f)); CylinderShape myCylinder(SimdVector3(0.3f,0.3f,0.3f)); ConeShape myCone(1,1); MinkowskiSumShape myMink(&myCylinder,&myBox); /// /// /// int main(int argc,char** argv) { Raytracer* raytraceDemo = new Raytracer(); raytraceDemo->initPhysics();
bool Solid3EpaPenetrationDepth::CalcPenDepth( SimplexSolverInterface& simplexSolver, ConvexShape* convexA,ConvexShape* convexB, const SimdTransform& transformA,const SimdTransform& transformB, SimdVector3& v, SimdPoint3& pa, SimdPoint3& pb) { int num_verts = simplexSolver.getSimplex(pBuf, qBuf, yBuf); switch (num_verts) { case 1: // Touching contact. Yes, we have a collision, // but no penetration. return false; case 2: { // We have a line segment inside the Minkowski sum containing the // origin. Blow it up by adding three additional support points. SimdVector3 dir = (yBuf[1] - yBuf[0]).normalized(); int axis = dir.furthestAxis(); static SimdScalar sin_60 = 0.8660254037f;//84438646763723170752941.22474487f;//13915890490986420373529;// SimdQuaternion rot(dir[0] * sin_60, dir[1] * sin_60, dir[2] * sin_60, SimdScalar(0.5)); SimdMatrix3x3 rot_mat(rot); SimdVector3 aux1 = dir.cross(SimdVector3(axis == 0, axis == 1, axis == 2)); SimdVector3 aux2 = rot_mat * aux1; SimdVector3 aux3 = rot_mat * aux2; pBuf[2] = transformA(convexA->LocalGetSupportingVertex(aux1*transformA.getBasis())); qBuf[2] = transformB(convexB->LocalGetSupportingVertex((-aux1)*transformB.getBasis())); yBuf[2] = pBuf[2] - qBuf[2]; pBuf[3] = transformA(convexA->LocalGetSupportingVertex(aux2*transformA.getBasis())); qBuf[3] = transformB(convexB->LocalGetSupportingVertex((-aux2)*transformB.getBasis())); yBuf[3] = pBuf[3] - qBuf[3]; pBuf[4] = transformA(convexA->LocalGetSupportingVertex(aux3*transformA.getBasis())); qBuf[4] = transformB(convexB->LocalGetSupportingVertex((-aux3)*transformB.getBasis())); yBuf[4] = pBuf[4] - qBuf[4]; if (originInTetrahedron(yBuf[0], yBuf[2], yBuf[3], yBuf[4])) { pBuf[1] = pBuf[4]; qBuf[1] = qBuf[4]; yBuf[1] = yBuf[4]; } else if (originInTetrahedron(yBuf[1], yBuf[2], yBuf[3], yBuf[4])) { pBuf[0] = pBuf[4]; qBuf[0] = qBuf[4]; yBuf[0] = yBuf[4]; } else { // Origin not in initial polytope return false; } num_verts = 4; break; } case 3: { // We have a triangle inside the Minkowski sum containing // the origin. First blow it up. SimdVector3 v1 = yBuf[1] - yBuf[0]; SimdVector3 v2 = yBuf[2] - yBuf[0]; SimdVector3 vv = v1.cross(v2); pBuf[3] = transformA(convexA->LocalGetSupportingVertex(vv*transformA.getBasis())); qBuf[3] = transformB(convexB->LocalGetSupportingVertex((-vv)*transformB.getBasis())); yBuf[3] = pBuf[3] - qBuf[3]; pBuf[4] = transformA(convexA->LocalGetSupportingVertex((-vv)*transformA.getBasis())); qBuf[4] = transformB(convexB->LocalGetSupportingVertex(vv*transformB.getBasis())); yBuf[4] = pBuf[4] - qBuf[4]; if (originInTetrahedron(yBuf[0], yBuf[1], yBuf[2], yBuf[4])) { pBuf[3] = pBuf[4]; qBuf[3] = qBuf[4]; yBuf[3] = yBuf[4]; } else if (!originInTetrahedron(yBuf[0], yBuf[1], yBuf[2], yBuf[3])) { // Origin not in initial polytope return false; } num_verts = 4; break; } } // We have a tetrahedron inside the Minkowski sum containing // the origin (if GJK did it's job right ;-) if (!originInTetrahedron(yBuf[0], yBuf[1], yBuf[2], yBuf[3])) { // assert(false); return false; } num_facets = 0; freeFacet = 0; ReplaceMeFacet *f0 = addFacet(0, 1, 2, SimdScalar(0.0), SIMD_INFINITY); ReplaceMeFacet *f1 = addFacet(0, 3, 1, SimdScalar(0.0), SIMD_INFINITY); ReplaceMeFacet *f2 = addFacet(0, 2, 3, SimdScalar(0.0), SIMD_INFINITY); ReplaceMeFacet *f3 = addFacet(1, 3, 2, SimdScalar(0.0), SIMD_INFINITY); if (!f0 || f0->getDist2() == SimdScalar(0.0) || !f1 || f1->getDist2() == SimdScalar(0.0) || !f2 || f2->getDist2() == SimdScalar(0.0) || !f3 || f3->getDist2() == SimdScalar(0.0)) { return false; } f0->link(0, f1, 2); f0->link(1, f3, 2); f0->link(2, f2, 0); f1->link(0, f2, 2); f1->link(1, f3, 0); f2->link(1, f3, 1); if (num_facets == 0) { return false; } // at least one facet on the heap. ReplaceMeEdgeBuffer edgeBuffer(20); ReplaceMeFacet *facet = 0; SimdScalar upper_bound2 = SIMD_INFINITY; do { facet = facetHeap[0]; std::pop_heap(&facetHeap[0], &facetHeap[num_facets], myFacetComp); --num_facets; if (!facet->isObsolete()) { assert(facet->getDist2() > SimdScalar(0.0)); if (num_verts == MaxSupportPoints) { #ifdef DEBUG std::cout << "Ouch, no convergence!!!" << std::endl; #endif ASSERT_MESSAGE(false,"Error: pendepth calc failed"); break; } pBuf[num_verts] = transformA(convexA->LocalGetSupportingVertex((facet->getClosest())*transformA.getBasis())); qBuf[num_verts] = transformB(convexB->LocalGetSupportingVertex((-facet->getClosest())*transformB.getBasis())); yBuf[num_verts] = pBuf[num_verts] - qBuf[num_verts]; int index = num_verts++; SimdScalar far_dist2 = yBuf[index].dot(facet->getClosest()); // Make sure the support mapping is OK. //assert(far_dist2 > SimdScalar(0.0)); // // this is to avoid problems with implicit-sphere-touching contact // if (far_dist2 < SimdScalar(0.0)) { return false; } GEN_set_min(upper_bound2, (far_dist2 * far_dist2) / facet->getDist2()); if (upper_bound2 <= ReplaceMeAccuracy::depth_tolerance * facet->getDist2() #define CHECK_NEW_SUPPORT #ifdef CHECK_NEW_SUPPORT || yBuf[index] == yBuf[(*facet)[0]] || yBuf[index] == yBuf[(*facet)[1]] || yBuf[index] == yBuf[(*facet)[2]] #endif ) { break; } // Compute the silhouette cast by the new vertex // Note that the new vertex is on the positive side // of the current facet, so the current facet is will // not be in the convex hull. Start local search // from this facet. facet->silhouette(yBuf[index], edgeBuffer); if (edgeBuffer.empty()) { return false; } ReplaceMeEdgeBuffer::const_iterator it = edgeBuffer.begin(); ReplaceMeFacet *firstFacet = addFacet((*it).getTarget(), (*it).getSource(), index, facet->getDist2(), upper_bound2); if (!firstFacet) { break; } firstFacet->link(0, (*it).getFacet(), (*it).getIndex()); ReplaceMeFacet *lastFacet = firstFacet; ++it; for (; it != edgeBuffer.end(); ++it) { ReplaceMeFacet *newFacet = addFacet((*it).getTarget(), (*it).getSource(), index, facet->getDist2(), upper_bound2); if (!newFacet) { break; } if (!newFacet->link(0, (*it).getFacet(), (*it).getIndex())) { break; } if (!newFacet->link(2, lastFacet, 1)) { break; } lastFacet = newFacet; } if (it != edgeBuffer.end()) { break; } firstFacet->link(2, lastFacet, 1); } } while (num_facets > 0 && facetHeap[0]->getDist2() <= upper_bound2); #ifdef DEBUG std::cout << "#facets left = " << num_facets << std::endl; #endif v = facet->getClosest(); pa = facet->getClosestPoint(pBuf); pb = facet->getClosestPoint(qBuf); return true; }
int main(int argc,char** argv) { CollisionDispatcher* dispatcher = new CollisionDispatcher(); SimdVector3 worldAabbMin(-10000,-10000,-10000); SimdVector3 worldAabbMax(10000,10000,10000); BroadphaseInterface* broadphase = new AxisSweep3(worldAabbMin,worldAabbMax); //BroadphaseInterface* broadphase = new SimpleBroadphase(); physicsEnvironmentPtr = new CcdPhysicsEnvironment(dispatcher,broadphase); physicsEnvironmentPtr->setDeactivationTime(2.f); physicsEnvironmentPtr->setGravity(0,-10,0); PHY_ShapeProps shapeProps; shapeProps.m_do_anisotropic = false; shapeProps.m_do_fh = false; shapeProps.m_do_rot_fh = false; shapeProps.m_friction_scaling[0] = 1.; shapeProps.m_friction_scaling[1] = 1.; shapeProps.m_friction_scaling[2] = 1.; shapeProps.m_inertia = 1.f; shapeProps.m_lin_drag = 0.2f; shapeProps.m_ang_drag = 0.1f; shapeProps.m_mass = 10.0f; PHY_MaterialProps materialProps; materialProps.m_friction = 10.5f; materialProps.m_restitution = 0.0f; CcdConstructionInfo ccdObjectCi; ccdObjectCi.m_friction = 0.5f; ccdObjectCi.m_linearDamping = shapeProps.m_lin_drag; ccdObjectCi.m_angularDamping = shapeProps.m_ang_drag; SimdTransform tr; tr.setIdentity(); int i; for (i=0;i<numObjects;i++) { if (i>0) { shapeIndex[i] = 1;//sphere } else shapeIndex[i] = 0; } for (i=0;i<numObjects;i++) { shapeProps.m_shape = shapePtr[shapeIndex[i]]; shapeProps.m_shape->SetMargin(0.05f); bool isDyna = i>0; //if (i==1) // isDyna=false; if (0)//i==1) { SimdQuaternion orn(0,0,0.1*SIMD_HALF_PI); ms[i].setWorldOrientation(orn.x(),orn.y(),orn.z(),orn[3]); } if (i>0) { switch (i) { case 1: { ms[i].setWorldPosition(0,10,0); //for testing, rotate the ground cube so the stack has to recover a bit break; } case 2: { ms[i].setWorldPosition(0,8,2); break; } default: ms[i].setWorldPosition(0,i*CUBE_HALF_EXTENTS*2 - CUBE_HALF_EXTENTS,0); } float quatIma0,quatIma1,quatIma2,quatReal; SimdQuaternion quat; SimdVector3 axis(0,0,1); SimdScalar angle=0.5f; quat.setRotation(axis,angle); ms[i].setWorldOrientation(quat.getX(),quat.getY(),quat.getZ(),quat[3]); } else { ms[i].setWorldPosition(0,-10+EXTRA_HEIGHT,0); } ccdObjectCi.m_MotionState = &ms[i]; ccdObjectCi.m_gravity = SimdVector3(0,0,0); ccdObjectCi.m_localInertiaTensor =SimdVector3(0,0,0); if (!isDyna) { shapeProps.m_mass = 0.f; ccdObjectCi.m_mass = shapeProps.m_mass; ccdObjectCi.m_collisionFlags = CollisionObject::isStatic; } else { shapeProps.m_mass = 1.f; ccdObjectCi.m_mass = shapeProps.m_mass; ccdObjectCi.m_collisionFlags = 0; } SimdVector3 localInertia; if (shapePtr[shapeIndex[i]]->GetShapeType() == EMPTY_SHAPE_PROXYTYPE) { //take inertia from first shape shapePtr[1]->CalculateLocalInertia(shapeProps.m_mass,localInertia); } else { shapePtr[shapeIndex[i]]->CalculateLocalInertia(shapeProps.m_mass,localInertia); } ccdObjectCi.m_localInertiaTensor = localInertia; ccdObjectCi.m_collisionShape = shapePtr[shapeIndex[i]]; physObjects[i]= new CcdPhysicsController( ccdObjectCi); // Only do CCD if motion in one timestep (1.f/60.f) exceeds CUBE_HALF_EXTENTS physObjects[i]->GetRigidBody()->m_ccdSquareMotionTreshold = CUBE_HALF_EXTENTS; //Experimental: better estimation of CCD Time of Impact: //physObjects[i]->GetRigidBody()->m_ccdSweptShereRadius = 0.5*CUBE_HALF_EXTENTS; physicsEnvironmentPtr->addCcdPhysicsController( physObjects[i]); if (i==1) { //physObjects[i]->SetAngularVelocity(0,0,-2,true); } physicsEnvironmentPtr->setDebugDrawer(&debugDrawer); } clientResetScene(); { //physObjects[i]->SetAngularVelocity(0,0,-2,true); int constraintId; float pivotX=CUBE_HALF_EXTENTS, pivotY=CUBE_HALF_EXTENTS, pivotZ=CUBE_HALF_EXTENTS; float axisX=0,axisY=1,axisZ=0; constraintId =physicsEnvironmentPtr->createConstraint( physObjects[1], //0, physObjects[2], ////PHY_POINT2POINT_CONSTRAINT, PHY_GENERIC_6DOF_CONSTRAINT,//can leave any of the 6 degree of freedom 'free' or 'locked' //PHY_LINEHINGE_CONSTRAINT, pivotX,pivotY,pivotZ, axisX,axisY,axisZ ); } setCameraDistance(26.f); return glutmain(argc, argv,640,480,"Bullet Physics Demo. http://www.continuousphysics.com/Bullet/phpBB2/"); }
SimdVector3 randomPosition( const SimdPoint3& minPoint, const SimdPoint3& maxPoint ) { return SimdVector3( randomFloat( minPoint.getX(), maxPoint.getX() ), randomFloat( minPoint.getY(), maxPoint.getY() ), randomFloat( minPoint.getZ(), maxPoint.getZ() ) ); }
//to be implemented by the demo void renderme() { debugDrawer.SetDebugMode(getDebugMode()); //render the hinge axis if (createConstraint) { SimdVector3 color(1,0,0); SimdVector3 dirLocal(0,1,0); SimdVector3 pivotInA(CUBE_HALF_EXTENTS,-CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS); SimdVector3 pivotInB(-CUBE_HALF_EXTENTS,-CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS); SimdVector3 from = physObjects[1]->GetRigidBody()->getCenterOfMassTransform()(pivotInA); SimdVector3 fromB = physObjects[2]->GetRigidBody()->getCenterOfMassTransform()(pivotInB); SimdVector3 dirWorldA = physObjects[1]->GetRigidBody()->getCenterOfMassTransform().getBasis() * dirLocal ; SimdVector3 dirWorldB = physObjects[2]->GetRigidBody()->getCenterOfMassTransform().getBasis() * dirLocal ; debugDrawer.DrawLine(from,from+dirWorldA,color); debugDrawer.DrawLine(fromB,fromB+dirWorldB,color); } float m[16]; int i; if (getDebugMode() & IDebugDraw::DBG_DisableBulletLCP) { //don't use Bullet, use quickstep physicsEnvironmentPtr->setSolverType(0); } else { //Bullet LCP solver physicsEnvironmentPtr->setSolverType(1); } if (getDebugMode() & IDebugDraw::DBG_EnableCCD) { physicsEnvironmentPtr->setCcdMode(3); } else { physicsEnvironmentPtr->setCcdMode(0); } bool isSatEnabled = (getDebugMode() & IDebugDraw::DBG_EnableSatComparison); physicsEnvironmentPtr->EnableSatCollisionDetection(isSatEnabled); #ifdef USE_HULL //some testing code for SAT if (isSatEnabled) { for (int s=0;s<numShapes;s++) { CollisionShape* shape = shapePtr[s]; if (shape->IsPolyhedral()) { PolyhedralConvexShape* polyhedron = static_cast<PolyhedralConvexShape*>(shape); if (!polyhedron->m_optionalHull) { //first convert vertices in 'Point3' format int numPoints = polyhedron->GetNumVertices(); Point3* points = new Point3[numPoints+1]; //first 4 points should not be co-planar, so add central point to satisfy MakeHull points[0] = Point3(0.f,0.f,0.f); SimdVector3 vertex; for (int p=0;p<numPoints;p++) { polyhedron->GetVertex(p,vertex); points[p+1] = Point3(vertex.getX(),vertex.getY(),vertex.getZ()); } Hull* hull = Hull::MakeHull(numPoints+1,points); polyhedron->m_optionalHull = hull; } } } } #endif //USE_HULL for (i=0;i<numObjects;i++) { SimdTransform transA; transA.setIdentity(); float pos[3]; float rot[4]; ms[i].getWorldPosition(pos[0],pos[1],pos[2]); ms[i].getWorldOrientation(rot[0],rot[1],rot[2],rot[3]); SimdQuaternion q(rot[0],rot[1],rot[2],rot[3]); transA.setRotation(q); SimdPoint3 dpos; dpos.setValue(pos[0],pos[1],pos[2]); transA.setOrigin( dpos ); transA.getOpenGLMatrix( m ); SimdVector3 wireColor(1.f,1.0f,0.5f); //wants deactivation if (i & 1) { wireColor = SimdVector3(0.f,0.0f,1.f); } ///color differently for active, sleeping, wantsdeactivation states if (physObjects[i]->GetRigidBody()->GetActivationState() == 1) //active { if (i & 1) { wireColor += SimdVector3 (1.f,0.f,0.f); } else { wireColor += SimdVector3 (.5f,0.f,0.f); } } if (physObjects[i]->GetRigidBody()->GetActivationState() == 2) //ISLAND_SLEEPING { if (i & 1) { wireColor += SimdVector3 (0.f,1.f, 0.f); } else { wireColor += SimdVector3 (0.f,0.5f,0.f); } } char extraDebug[125]; sprintf(extraDebug,"islId, Body=%i , %i",physObjects[i]->GetRigidBody()->m_islandTag1,physObjects[i]->GetRigidBody()->m_debugBodyId); physObjects[i]->GetRigidBody()->GetCollisionShape()->SetExtraDebugInfo(extraDebug); GL_ShapeDrawer::DrawOpenGL(m,physObjects[i]->GetRigidBody()->GetCollisionShape(),wireColor,getDebugMode()); ///this block is just experimental code to show some internal issues with replacing shapes on the fly. if (getDebugMode()!=0 && (i>0)) { if (physObjects[i]->GetRigidBody()->GetCollisionShape()->GetShapeType() == EMPTY_SHAPE_PROXYTYPE) { physObjects[i]->GetRigidBody()->SetCollisionShape(shapePtr[1]); //remove the persistent collision pairs that were created based on the previous shape BroadphaseProxy* bpproxy = physObjects[i]->GetRigidBody()->m_broadphaseHandle; physicsEnvironmentPtr->GetBroadphase()->CleanProxyFromPairs(bpproxy); SimdVector3 newinertia; SimdScalar newmass = 10.f; physObjects[i]->GetRigidBody()->GetCollisionShape()->CalculateLocalInertia(newmass,newinertia); physObjects[i]->GetRigidBody()->setMassProps(newmass,newinertia); physObjects[i]->GetRigidBody()->updateInertiaTensor(); } } } if (!(getDebugMode() & IDebugDraw::DBG_NoHelpText)) { float xOffset = 10.f; float yStart = 20.f; float yIncr = -2.f; char buf[124]; glColor3f(0, 0, 0); #ifdef USE_QUICKPROF if ( getDebugMode() & IDebugDraw::DBG_ProfileTimings) { static int counter = 0; counter++; std::map<std::string, hidden::ProfileBlock*>::iterator iter; for (iter = Profiler::mProfileBlocks.begin(); iter != Profiler::mProfileBlocks.end(); ++iter) { char blockTime[128]; sprintf(blockTime, "%s: %lf",&((*iter).first[0]),Profiler::getBlockTime((*iter).first, Profiler::BLOCK_CYCLE_SECONDS));//BLOCK_TOTAL_PERCENT)); glRasterPos3f(xOffset,yStart,0); BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),blockTime); yStart += yIncr; } } #endif //USE_QUICKPROF //profiling << Profiler::createStatsString(Profiler::BLOCK_TOTAL_PERCENT); //<< std::endl; glRasterPos3f(xOffset,yStart,0); sprintf(buf,"mouse to interact"); BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); yStart += yIncr; glRasterPos3f(xOffset,yStart,0); sprintf(buf,"space to reset"); BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); yStart += yIncr; glRasterPos3f(xOffset,yStart,0); sprintf(buf,"cursor keys and z,x to navigate"); BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); yStart += yIncr; glRasterPos3f(xOffset,yStart,0); sprintf(buf,"i to toggle simulation, s single step"); BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); yStart += yIncr; glRasterPos3f(xOffset,yStart,0); sprintf(buf,"q to quit"); BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); yStart += yIncr; glRasterPos3f(xOffset,yStart,0); sprintf(buf,"d to toggle deactivation"); BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); yStart += yIncr; glRasterPos3f(xOffset,yStart,0); sprintf(buf,"a to draw temporal AABBs"); BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); yStart += yIncr; glRasterPos3f(xOffset,yStart,0); sprintf(buf,"h to toggle help text"); BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); yStart += yIncr; bool useBulletLCP = !(getDebugMode() & IDebugDraw::DBG_DisableBulletLCP); bool useCCD = (getDebugMode() & IDebugDraw::DBG_EnableCCD); glRasterPos3f(xOffset,yStart,0); sprintf(buf,"m Bullet GJK = %i",!isSatEnabled); BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); yStart += yIncr; glRasterPos3f(xOffset,yStart,0); sprintf(buf,"n Bullet LCP = %i",useBulletLCP); BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); yStart += yIncr; glRasterPos3f(xOffset,yStart,0); sprintf(buf,"1 CCD mode (adhoc) = %i",useCCD); BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); yStart += yIncr; glRasterPos3f(xOffset,yStart,0); sprintf(buf,"+- shooting speed = %10.2f",bulletSpeed); BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf); yStart += yIncr; } }