void SyncWheels() { int numWheels = GetNumWheels(); int i; for (i=0;i<numWheels;i++) { WheelInfo& info = m_vehicle->GetWheelInfo(i); PHY_IMotionState* motionState = (PHY_IMotionState*)info.m_clientInfo ; m_vehicle->UpdateWheelTransform(i); SimdTransform trans = m_vehicle->GetWheelTransformWS(i); SimdQuaternion orn = trans.getRotation(); const SimdVector3& pos = trans.getOrigin(); motionState->setWorldOrientation(orn.x(),orn.y(),orn.z(),orn[3]); motionState->setWorldPosition(pos.x(),pos.y(),pos.z()); } }
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/"); }
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(); }
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(); }
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/"); }
bool BU_CollisionPair::calcTimeOfImpact( const SimdTransform& fromA, const SimdTransform& toA, const SimdTransform& fromB, const SimdTransform& toB, CastResult& result) { SimdVector3 linvelA,angvelA; SimdVector3 linvelB,angvelB; SimdTransformUtil::CalculateVelocity(fromA,toA,1.f,linvelA,angvelA); SimdTransformUtil::CalculateVelocity(fromB,toB,1.f,linvelB,angvelB); SimdVector3 linearMotionA = toA.getOrigin() - fromA.getOrigin(); SimdQuaternion angularMotionA(0,0,0,1.f); SimdVector3 linearMotionB = toB.getOrigin() - fromB.getOrigin(); SimdQuaternion angularMotionB(0,0,0,1); result.m_fraction = 1.f; SimdTransform impactTransA; SimdTransform impactTransB; int index=0; SimdScalar toiUnscaled=result.m_fraction; const SimdScalar toiUnscaledLimit = result.m_fraction; SimdTransform a2w; a2w = fromA; SimdTransform b2w = fromB; /* debugging code { const int numvertsB = m_convexB->GetNumVertices(); for (int v=0;v<numvertsB;v++) { SimdPoint3 pt; m_convexB->GetVertex(v,pt); pt = b2w * pt; char buf[1000]; if (pt.y() < 0.) { sprintf(buf,"PRE ERROR (%d) %.20E %.20E %.20E!!!!!!!!!\n",v,pt.x(),pt.y(),pt.z()); if (debugFile) fwrite(buf,1,strlen(buf),debugFile); } else { sprintf(buf,"PRE %d = %.20E,%.20E,%.20E\n",v,pt.x(),pt.y(),pt.z()); if (debugFile) fwrite(buf,1,strlen(buf),debugFile); } } } */ SimdTransform b2wp = b2w; b2wp.setOrigin(b2w.getOrigin() + linearMotionB); b2wp.setRotation( b2w.getRotation() + angularMotionB); impactTransB = b2wp; SimdTransform a2wp; a2wp.setOrigin(a2w.getOrigin()+ linearMotionA); a2wp.setRotation(a2w.getRotation()+angularMotionA); impactTransA = a2wp; SimdTransform a2winv; a2winv = a2w.inverse(); SimdTransform b2wpinv; b2wpinv = b2wp.inverse(); SimdTransform b2winv; b2winv = b2w.inverse(); SimdTransform a2wpinv; a2wpinv = a2wp.inverse(); //Redon's version with concatenated transforms SimdTransform relative; relative = b2w * b2wpinv * a2wp * a2winv; //relative = a2winv * a2wp * b2wpinv * b2w; SimdQuaternion qrel; relative.getBasis().getRotation(qrel); SimdVector3 linvel = relative.getOrigin(); if (linvel.length() < SCREWEPSILON) { linvel.setValue(0.,0.,0.); } SimdVector3 angvel; angvel[0] = 2.f * SimdAsin (qrel[0]); angvel[1] = 2.f * SimdAsin (qrel[1]); angvel[2] = 2.f * SimdAsin (qrel[2]); if (angvel.length() < SCREWEPSILON) { angvel.setValue(0.f,0.f,0.f); } //Redon's version with concatenated transforms m_screwing = BU_Screwing(linvel,angvel); SimdTransform w2s; m_screwing.LocalMatrix(w2s); SimdTransform s2w; s2w = w2s.inverse(); //impactTransA = a2w; //impactTransB = b2w; bool hit = false; if (SimdFuzzyZero(m_screwing.GetS()) && SimdFuzzyZero(m_screwing.GetW())) { //W = 0 , S = 0 , no collision //toi = 0; /* { const int numvertsB = m_convexB->GetNumVertices(); for (int v=0;v<numvertsB;v++) { SimdPoint3 pt; m_convexB->GetVertex(v,pt); pt = impactTransB * pt; char buf[1000]; if (pt.y() < 0.) { sprintf(buf,"EARLY POST ERROR (%d) %.20E,%.20E,%.20E!!!!!!!!!\n",v,pt.x(),pt.y(),pt.z()); if (debugFile) fwrite(buf,1,strlen(buf),debugFile); } else { sprintf(buf,"EARLY POST %d = %.20E,%.20E,%.20E\n",v,pt.x(),pt.y(),pt.z()); if (debugFile) fwrite(buf,1,strlen(buf),debugFile); } } } */ return false;//don't continue moving within epsilon } #define EDGEEDGE #ifdef EDGEEDGE BU_EdgeEdge edgeEdge; //for all edged in A check agains all edges in B for (int ea = 0;ea < m_convexA->GetNumEdges();ea++) { SimdPoint3 pA0,pA1; m_convexA->GetEdge(ea,pA0,pA1); pA0= a2w * pA0;//in world space pA0 = w2s * pA0;//in screwing space pA1= a2w * pA1;//in world space pA1 = w2s * pA1;//in screwing space int numedgesB = m_convexB->GetNumEdges(); for (int eb = 0; eb < numedgesB;eb++) { { SimdPoint3 pB0,pB1; m_convexB->GetEdge(eb,pB0,pB1); pB0= b2w * pB0;//in world space pB0 = w2s * pB0;//in screwing space pB1= b2w * pB1;//in world space pB1 = w2s * pB1;//in screwing space SimdScalar lambda,mu; toiUnscaled = 1.; SimdVector3 edgeDirA(pA1-pA0); SimdVector3 edgeDirB(pB1-pB0); if (edgeEdge.GetTimeOfImpact(m_screwing,pA0,edgeDirA,pB0,edgeDirB,toiUnscaled,lambda,mu)) { //printf("edgeedge potential hit\n"); if (toiUnscaled>=0) { if (toiUnscaled < toiUnscaledLimit) { //inside check is already done by checking the mu and gamma ! SimdPoint3 vtx = pA0+lambda * (pA1-pA0); SimdPoint3 hitpt = m_screwing.InBetweenPosition(vtx,toiUnscaled); SimdPoint3 hitptWorld = s2w * hitpt; { if (toiUnscaled < result.m_fraction) result.m_fraction = toiUnscaled; hit = true; SimdVector3 hitNormal = edgeDirB.cross(edgeDirA); hitNormal = m_screwing.InBetweenVector(hitNormal,toiUnscaled); hitNormal.normalize(); //an approximated normal can be calculated by taking the cross product of both edges //take care of the sign ! SimdVector3 hitNormalWorld = s2w.getBasis() * hitNormal ; SimdScalar dist = m_screwing.GetU().dot(hitNormalWorld); if (dist > 0) hitNormalWorld *= -1; //todo: this is the wrong point, because b2winv is still at begin of motion // not at time-of-impact location! //bhitpt = b2winv * hitptWorld; // m_manifold.SetContactPoint(BUM_FeatureEdgeEdge,index,ea,eb,hitptWorld,hitNormalWorld); } } } } } index++; } }; #endif //EDGEEDGE #define VERTEXFACE #ifdef VERTEXFACE // for all vertices in A, for each face in B,do vertex-face { const int numvertsA = m_convexA->GetNumVertices(); for (int v=0;v<numvertsA;v++) //int v=3; { SimdPoint3 vtx; m_convexA->GetVertex(v,vtx); vtx = a2w * vtx;//in world space vtx = w2s * vtx;//in screwing space const int numplanesB = m_convexB->GetNumPlanes(); for (int p = 0 ; p < numplanesB; p++) //int p=2; { { SimdVector3 planeNorm; SimdPoint3 planeSupport; m_convexB->GetPlane(planeNorm,planeSupport,p); planeSupport = b2w * planeSupport;//transform to world space SimdVector3 planeNormWorld = b2w.getBasis() * planeNorm; planeSupport = w2s * planeSupport ; //transform to screwing space planeNorm = w2s.getBasis() * planeNormWorld; planeNorm.normalize(); SimdScalar d = planeSupport.dot(planeNorm); SimdVector4 planeEq(planeNorm[0],planeNorm[1],planeNorm[2],d); BU_VertexPoly vtxApolyB; toiUnscaled = 1.; if ((p==2) && (v==6)) { // printf("%f toiUnscaled\n",toiUnscaled); } if (vtxApolyB.GetTimeOfImpact(m_screwing,vtx,planeEq,toiUnscaled,false)) { if (toiUnscaled >= 0. ) { //not only collect the first point, get every contactpoint, later we have to check the //manifold properly! if (toiUnscaled <= toiUnscaledLimit) { // printf("toiUnscaled %f\n",toiUnscaled ); SimdPoint3 hitpt = m_screwing.InBetweenPosition(vtx,toiUnscaled); SimdVector3 hitNormal = m_screwing.InBetweenVector(planeNorm ,toiUnscaled); SimdVector3 hitNormalWorld = s2w.getBasis() * hitNormal ; SimdPoint3 hitptWorld = s2w * hitpt; hitpt = b2winv * hitptWorld; //vertex has to be 'within' the facet's boundary if (m_convexB->IsInside(hitpt,m_tolerance)) { // m_manifold.SetContactPoint(BUM_FeatureVertexFace, index,v,p,hitptWorld,hitNormalWorld); if (toiUnscaled < result.m_fraction) result.m_fraction= toiUnscaled; hit = true; } } } } } index++; } } } // // for all vertices in B, for each face in A,do vertex-face //copy and pasted from all verts A -> all planes B so potential typos! //todo: make this into one method with a kind of 'swapped' logic // { const int numvertsB = m_convexB->GetNumVertices(); for (int v=0;v<numvertsB;v++) //int v=0; { SimdPoint3 vtx; m_convexB->GetVertex(v,vtx); vtx = b2w * vtx;//in world space /* char buf[1000]; if (vtx.y() < 0.) { sprintf(buf,"ERROR !!!!!!!!!\n",v,vtx.x(),vtx.y(),vtx.z()); if (debugFile) fwrite(buf,1,strlen(buf),debugFile); } sprintf(buf,"vertexWorld(%d) = (%.20E,%.20E,%.20E)\n",v,vtx.x(),vtx.y(),vtx.z()); if (debugFile) fwrite(buf,1,strlen(buf),debugFile); */ vtx = w2s * vtx;//in screwing space const int numplanesA = m_convexA->GetNumPlanes(); for (int p = 0 ; p < numplanesA; p++) //int p=2; { { SimdVector3 planeNorm; SimdPoint3 planeSupport; m_convexA->GetPlane(planeNorm,planeSupport,p); planeSupport = a2w * planeSupport;//transform to world space SimdVector3 planeNormWorld = a2w.getBasis() * planeNorm; planeSupport = w2s * planeSupport ; //transform to screwing space planeNorm = w2s.getBasis() * planeNormWorld; planeNorm.normalize(); SimdScalar d = planeSupport.dot(planeNorm); SimdVector4 planeEq(planeNorm[0],planeNorm[1],planeNorm[2],d); BU_VertexPoly vtxBpolyA; toiUnscaled = 1.; if (vtxBpolyA.GetTimeOfImpact(m_screwing,vtx,planeEq,toiUnscaled,true)) { if (toiUnscaled>=0.) { if (toiUnscaled < toiUnscaledLimit) { SimdPoint3 hitpt = m_screwing.InBetweenPosition( vtx , -toiUnscaled); SimdVector3 hitNormal = m_screwing.InBetweenVector(-planeNorm ,-toiUnscaled); //SimdScalar len = hitNormal.length()-1; //assert( SimdFuzzyZero(len) ); SimdVector3 hitNormalWorld = s2w.getBasis() * hitNormal ; SimdPoint3 hitptWorld = s2w * hitpt; hitpt = a2winv * hitptWorld; //vertex has to be 'within' the facet's boundary if (m_convexA->IsInside(hitpt,m_tolerance)) { // m_manifold.SetContactPoint(BUM_FeatureFaceVertex,index,p,v,hitptWorld,hitNormalWorld); if (toiUnscaled <result.m_fraction) result.m_fraction = toiUnscaled; hit = true; } } } } } } index++; } } #endif// VERTEXFACE //the manifold now consists of all points/normals generated by feature-pairs that have a time-of-impact within this frame //in addition there are contact points from previous frames //we have to cleanup the manifold, using an additional epsilon/tolerance //as long as the distance from the contactpoint (in worldspace) to both objects is within this epsilon we keep the point //else throw it away if (hit) { //try to avoid numerical drift on close contact if (result.m_fraction < 0.00001) { // printf("toiUnscaledMin< 0.00001\n"); impactTransA = a2w; impactTransB = b2w; } else { //SimdScalar vel = linearMotionB.length(); //todo: check this margin result.m_fraction *= 0.99f; //move B to new position impactTransB.setOrigin(b2w.getOrigin()+ result.m_fraction*linearMotionB); SimdQuaternion ornB = b2w.getRotation()+angularMotionB*result.m_fraction; ornB.normalize(); impactTransB.setRotation(ornB); //now transform A SimdTransform a2s,a2b; a2s.mult( w2s , a2w); a2s= m_screwing.InBetweenTransform(a2s,result.m_fraction); a2s.multInverseLeft(w2s,a2s); a2b.multInverseLeft(b2w, a2s); //transform by motion B impactTransA.mult(impactTransB, a2b); //normalize rotation SimdQuaternion orn; impactTransA.getBasis().getRotation(orn); orn.normalize(); impactTransA.setBasis(SimdMatrix3x3(orn)); } } /* { const int numvertsB = m_convexB->GetNumVertices(); for (int v=0;v<numvertsB;v++) { SimdPoint3 pt; m_convexB->GetVertex(v,pt); pt = impactTransB * pt; char buf[1000]; if (pt.y() < 0.) { sprintf(buf,"POST ERROR (%d) %.20E,%.20E,%.20E!!!!!!!!!\n",v,pt.x(),pt.y(),pt.z()); if (debugFile) fwrite(buf,1,strlen(buf),debugFile); } else { sprintf(buf,"POST %d = %.20E,%.20E,%.20E\n",v,pt.x(),pt.y(),pt.z()); if (debugFile) fwrite(buf,1,strlen(buf),debugFile); } } } */ return hit; }
void clientDisplay(void) { glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glDisable(GL_LIGHTING); //GL_ShapeDrawer::DrawCoordSystem(); float m[16]; int i; GjkPairDetector convexConvex(shapePtr[0],shapePtr[1],&sGjkSimplexSolver,0); SimdVector3 seperatingAxis(0.00000000f,0.059727669f,0.29259586f); convexConvex.SetCachedSeperatingAxis(seperatingAxis); PointCollector gjkOutput; GjkPairDetector::ClosestPointInput input; input.m_transformA = tr[0]; input.m_transformB = tr[1]; convexConvex.GetClosestPoints(input ,gjkOutput,0); if (gjkOutput.m_hasResult) { SimdVector3 endPt = gjkOutput.m_pointInWorld + gjkOutput.m_normalOnBInWorld*gjkOutput.m_distance; glBegin(GL_LINES); glColor3f(1, 0, 0); glVertex3d(gjkOutput.m_pointInWorld.x(), gjkOutput.m_pointInWorld.y(),gjkOutput.m_pointInWorld.z()); glVertex3d(endPt.x(),endPt.y(),endPt.z()); //glVertex3d(gjkOutputm_pointInWorld.x(), gjkOutputm_pointInWorld.y(),gjkOutputm_pointInWorld.z()); //glVertex3d(gjkOutputm_pointInWorld.x(), gjkOutputm_pointInWorld.y(),gjkOutputm_pointInWorld.z()); glEnd(); } for (i=0; i<numObjects; i++) { tr[i].getOpenGLMatrix( m ); GL_ShapeDrawer::DrawOpenGL(m,shapePtr[i],SimdVector3(1,1,1),getDebugMode()); } simplex.SetSimplexSolver(&sGjkSimplexSolver); SimdPoint3 ybuf[4],pbuf[4],qbuf[4]; int numpoints = sGjkSimplexSolver.getSimplex(pbuf,qbuf,ybuf); simplex.Reset(); for (i=0; i<numpoints; i++) simplex.AddVertex(ybuf[i]); SimdTransform ident; ident.setIdentity(); ident.getOpenGLMatrix(m); GL_ShapeDrawer::DrawOpenGL(m,&simplex,SimdVector3(1,1,1),getDebugMode()); SimdQuaternion orn; orn.setEuler(yaw,pitch,roll); tr[0].setRotation(orn); // pitch += 0.005f; // yaw += 0.01f; glFlush(); glutSwapBuffers(); }
void clientDisplay(void) { glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glDisable(GL_LIGHTING); if (collisionWorld) collisionWorld->PerformDiscreteCollisionDetection(); ///one way to draw all the contact points is iterating over contact manifolds / points: int i; int numManifolds = collisionWorld->GetDispatcher()->GetNumManifolds(); for (i=0;i<numManifolds;i++) { PersistentManifold* contactManifold = collisionWorld->GetDispatcher()->GetManifoldByIndexInternal(i); CollisionObject* obA = static_cast<CollisionObject*>(contactManifold->GetBody0()); CollisionObject* obB = static_cast<CollisionObject*>(contactManifold->GetBody1()); contactManifold->RefreshContactPoints(obA->m_worldTransform,obB->m_worldTransform); int numContacts = contactManifold->GetNumContacts(); for (int j=0;j<numContacts;j++) { ManifoldPoint& pt = contactManifold->GetContactPoint(j); glBegin(GL_LINES); glColor3f(1, 0, 1); SimdVector3 ptA = pt.GetPositionWorldOnA(); SimdVector3 ptB = pt.GetPositionWorldOnB(); glVertex3d(ptA.x(),ptA.y(),ptA.z()); glVertex3d(ptB.x(),ptB.y(),ptB.z()); glEnd(); } } //GL_ShapeDrawer::DrawCoordSystem(); float m[16]; for (i=0;i<numObjects;i++) { objects[i].m_worldTransform.getOpenGLMatrix( m ); GL_ShapeDrawer::DrawOpenGL(m,objects[i].m_collisionShape,SimdVector3(1,1,1),getDebugMode()); } SimdQuaternion orn; orn.setEuler(yaw,pitch,roll); objects[1].m_worldTransform.setOrigin(objects[1].m_worldTransform.getOrigin()+SimdVector3(0,-0.01,0)); //objects[0].m_worldTransform.setRotation(orn); pitch += 0.005f; yaw += 0.01f; glFlush(); glutSwapBuffers(); }