void SimpleOpenGL3App::registerGrid(int cells_x, int cells_z, float color0[4], float color1[4]) { b3Vector3 cubeExtents=b3MakeVector3(0.5,0.5,0.5); cubeExtents[m_data->m_upAxis] = 0; int cubeId = registerCubeShape(cubeExtents[0],cubeExtents[1],cubeExtents[2]); b3Quaternion orn(0,0,0,1); b3Vector3 center=b3MakeVector3(0,0,0,1); b3Vector3 scaling=b3MakeVector3(1,1,1,1); for ( int i = 0; i < cells_x; i++) { for (int j = 0; j < cells_z; j++) { float* color =0; if ((i + j) % 2 == 0) { color = (float*)color0; } else { color = (float*)color1; } if (this->m_data->m_upAxis==1) { center =b3MakeVector3((i + 0.5f) - cells_x * 0.5f, 0.f, (j + 0.5f) - cells_z * 0.5f); } else { center =b3MakeVector3((i + 0.5f) - cells_x * 0.5f, (j + 0.5f) - cells_z * 0.5f,0.f ); } m_instancingRenderer->registerGraphicsInstance(cubeId,center,orn,color,scaling); } } }
void GpuConvexPlaneScene::createStaticEnvironment(const ConstructionInfo& ci) { int strideInBytes = 9*sizeof(float); int numVertices = sizeof(cube_vertices)/strideInBytes; int numIndices = sizeof(cube_indices)/sizeof(int); //int shapeId = ci.m_instancingRenderer->registerShape(&cube_vertices[0],numVertices,cube_indices,numIndices); int shapeId = ci.m_instancingRenderer->registerShape(&cube_vertices[0],numVertices,cube_indices,numIndices); int group=1; int mask=1; int index=0; { b3Vector4 scaling=b3MakeVector4(400,400,400,1); int colIndex = m_data->m_np->registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling); b3Vector3 position=b3MakeVector3(0,-400,0); b3Quaternion orn(0,0,0,1); b3Vector4 color=b3MakeVector4(0,0,1,1); int id = ci.m_instancingRenderer->registerGraphicsInstance(shapeId,position,orn,color,scaling); int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(0.f,position,orn,colIndex,index,false); } }
virtual void initPhysics() { ///create some graphics proxy for the tracking target ///the endeffector tries to track it using Inverse Kinematics { int sphereId = m_app->registerGraphicsUnitSphereShape(SPHERE_LOD_MEDIUM); b3Quaternion orn(0, 0, 0, 1); b3Vector4 color = b3MakeVector4(1., 0.3, 0.3, 1); b3Vector3 scaling = b3MakeVector3(.02, .02, .02); m_targetSphereInstance = m_app->m_renderer->registerGraphicsInstance(sphereId, m_targetPos, orn, color, scaling); } m_app->m_renderer->writeTransforms(); int mode = eCONNECT_EXISTING_EXAMPLE_BROWSER; m_robotSim.setGuiHelper(m_guiHelper); bool connected = m_robotSim.connect(mode); // 0;//m_robotSim.connect(m_guiHelper); b3Printf("robotSim connected = %d",connected); { m_kukaIndex = m_robotSim.loadURDF("kuka_iiwa/model.urdf"); if (m_kukaIndex >=0) { int numJoints = m_robotSim.getNumJoints(m_kukaIndex); b3Printf("numJoints = %d",numJoints); for (int i=0;i<numJoints;i++) { b3JointInfo jointInfo; m_robotSim.getJointInfo(m_kukaIndex,i,&jointInfo); b3Printf("joint[%d].m_jointName=%s",i,jointInfo.m_jointName); } /* int wheelJointIndices[4]={2,3,6,7}; int wheelTargetVelocities[4]={-10,-10,-10,-10}; for (int i=0;i<4;i++) { b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); controlArgs.m_targetVelocity = wheelTargetVelocities[i]; controlArgs.m_maxTorqueValue = 1e30; m_robotSim.setJointMotorControl(m_kukaIndex,wheelJointIndices[i],controlArgs); } */ } { m_robotSim.loadURDF("plane.urdf"); m_robotSim.setGravity(b3MakeVector3(0,0,0)); } } }
void pl_rigidbody_SetOrientation(PlRigidBody* object, const PlQuaternion* orientation) { CAST_ASSERT(object,btRigidBody*,body); btQuaternion orn(orientation[0],orientation[1],orientation[2],orientation[3]); btTransform worldTrans = body->getWorldTransform(); worldTrans.setRotation(orn); body->setWorldTransform(worldTrans); }
void plSetOrientation(plRigidBodyHandle object, const plQuaternion orientation) { btRigidBody* body = reinterpret_cast< btRigidBody* >(object); btAssert(body); btQuaternion orn(orientation[0],orientation[1],orientation[2],orientation[3]); btTransform worldTrans = body->getWorldTransform(); worldTrans.setRotation(orn); body->setWorldTransform(worldTrans); }
virtual void renderScene() { m_robotSim.renderScene(); b3Quaternion orn(0, 0, 0, 1); m_app->m_renderer->writeSingleInstanceTransformToCPU(m_targetPos, orn, m_targetSphereInstance); m_app->m_renderer->writeTransforms(); //draw the end-effector target sphere //m_app->m_renderer->renderScene(); }
void writeTransforms() { glFlush(); char* bla = (char*)glMapBuffer( GL_ARRAY_BUFFER,GL_READ_WRITE);//GL_WRITE_ONLY float* positions = (float*)(bla+sizeof(cube_vertices)); float* orientations = (float*)(bla+sizeof(cube_vertices) + POSITION_BUFFER_SIZE); // positions[0]+=0.001f; static int offset=0; //offset++; static btVector3 axis(1,0,0); angle += 0.01f; int index=0; btQuaternion orn(axis,angle); for (int i=0;i<NUM_OBJECTS_X;i++) { for (int j=0;j<NUM_OBJECTS_Y;j++) { for (int k=0;k<NUM_OBJECTS_Z;k++) { //if (!((index+offset)%15)) { instance_positions_ptr[index*4+1]-=0.01f; positions[index*4]=instance_positions_ptr[index*4]; positions[index*4+1]=instance_positions_ptr[index*4+1]; positions[index*4+2]=instance_positions_ptr[index*4+2]; positions[index*4+3]=instance_positions_ptr[index*4+3]; orientations[index*4] = orn[0]; orientations[index*4+1] = orn[1]; orientations[index*4+2] = orn[2]; orientations[index*4+3] = orn[3]; } // memcpy((void*)&orientations[index*4],orn,sizeof(btQuaternion)); index++; } } } glUnmapBuffer( GL_ARRAY_BUFFER); //if this glFinish is removed, the animation is not always working/blocks //@todo: figure out why glFlush(); }
void mutex_lock(uint8_t r, uint8_t p) { //Serial.print("[Lock] r:"); //Serial.print(r); //Serial.print(" p:"); //Serial.println(p); entrance_intents[r][p] = true; while (orn(entrance_intents[r], p)) { if (turn[r] != 0) { entrance_intents[r][0] = false; while (turn[r] != 0) { // busy wait } entrance_intents[r][0] = true; } } }
void ConcaveSphereScene::createDynamicObjects(const ConstructionInfo& ci) { b3Vector4 colors[4] = { b3MakeVector4(1,0,0,1), b3MakeVector4(0,1,0,1), b3MakeVector4(0,1,1,1), b3MakeVector4(1,1,0,1), }; int index=0; int curColor = 0; float radius = 1; //int colIndex = m_data->m_np->registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling); int colIndex = m_data->m_np->registerSphereShape(radius);//>registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling); int prevGraphicsShapeIndex = registerGraphicsSphereShape(ci,radius,false); for (int i=0;i<ci.arraySizeX;i++) { for (int j=0;j<ci.arraySizeY;j++) { for (int k=0;k<ci.arraySizeZ;k++) { float mass = 1.f; b3Vector3 position=b3MakeVector3(-(ci.arraySizeX/2)*8+i*8,50+j*8,-(ci.arraySizeZ/2)*8+k*8); //b3Vector3 position(0,-41,0);//0,0,0);//i*radius*3,-41+j*radius*3,k*radius*3); b3Quaternion orn(0,0,0,1); b3Vector4 color = colors[curColor]; curColor++; curColor&=3; b3Vector4 scaling=b3MakeVector4(radius,radius,radius,1); int id = ci.m_instancingRenderer->registerGraphicsInstance(prevGraphicsShapeIndex,position,orn,color,scaling); int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(mass,position,orn,colIndex,index,false); index++; } } } }
RenderInstancingDemo(CommonGraphicsApp* app) :m_app(app), m_x(0), m_y(0), m_z(0) { m_app->setUpAxis(2); { b3Vector3 extents=b3MakeVector3(100,100,100); extents[m_app->getUpAxis()]=1; int xres = 20; int yres = 20; b3Vector4 color0=b3MakeVector4(0.1, 0.1, 0.1,1); b3Vector4 color1=b3MakeVector4(0.6, 0.6, 0.6,1); m_app->registerGrid(xres, yres, color0, color1); } { int boxId = m_app->registerCubeShape(0.1,0.1,0.1); for (int i=-numCubesX/2;i<numCubesX/2;i++) { for (int j = -numCubesY/2;j<numCubesY/2;j++) { b3Vector3 pos=b3MakeVector3(i,j,j); pos[app->getUpAxis()] = 1; b3Quaternion orn(0,0,0,1); b3Vector4 color=b3MakeVector4(0.3,0.3,0.3,1); b3Vector3 scaling=b3MakeVector3(1,1,1); int instanceId = m_app->m_renderer->registerGraphicsInstance(boxId,pos,orn,color,scaling); m_movingInstances.push_back(instanceId); } } } m_app->m_renderer->writeTransforms(); }
bool parseTransform(btTransform& tr, TiXmlElement* xml, ErrorLogger* logger) { tr.setIdentity(); { const char* xyz_str = xml->Attribute("xyz"); if (xyz_str) { parseVector3(tr.getOrigin(),std::string(xyz_str),logger); } } { const char* rpy_str = xml->Attribute("rpy"); if (rpy_str != NULL) { btVector3 rpy; if (parseVector3(rpy,std::string(rpy_str),logger)) { double phi, the, psi; double roll = rpy[0]; double pitch = rpy[1]; double yaw = rpy[2]; phi = roll / 2.0; the = pitch / 2.0; psi = yaw / 2.0; btQuaternion orn( sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi), cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi), cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi), cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi)); orn.normalize(); tr.setRotation(orn); } } } return true; }
btQuaternion getLuaQuaternionArg(lua_State* L, int index) { btQuaternion orn(0,0,0,1); int sz = luaL_returnlen(L, index); // get size of table { lua_rawgeti(L, index, 1); // push t[i] orn[0] = lua_tonumber(L,-1); lua_pop(L, 1); lua_rawgeti(L, index, 2); // push t[i] orn[1] = lua_tonumber(L,-1); lua_pop(L, 1); lua_rawgeti(L, index, 3); // push t[i] orn[2] = lua_tonumber(L,-1); lua_pop(L, 1); lua_rawgeti(L, index, 4); // push t[i] orn[3] = lua_tonumber(L,-1); lua_pop(L, 1); } return orn; }
virtual void processTriangle(btVector3* triangle, int partId, int triangleIndex) { //skip self-collisions if ((m_partIdA == partId) && (m_triangleIndexA == triangleIndex)) return; //skip duplicates (disabled for now) //if ((m_partIdA <= partId) && (m_triangleIndexA <= triangleIndex)) // return; //search for shared vertices and edges int numshared = 0; int sharedVertsA[3]={-1,-1,-1}; int sharedVertsB[3]={-1,-1,-1}; ///skip degenerate triangles btScalar crossBSqr = ((triangle[1]-triangle[0]).cross(triangle[2]-triangle[0])).length2(); if (crossBSqr < m_triangleInfoMap->m_equalVertexThreshold) return; btScalar crossASqr = ((m_triangleVerticesA[1]-m_triangleVerticesA[0]).cross(m_triangleVerticesA[2]-m_triangleVerticesA[0])).length2(); ///skip degenerate triangles if (crossASqr< m_triangleInfoMap->m_equalVertexThreshold) return; #if 0 printf("triangle A[0] = (%f,%f,%f)\ntriangle A[1] = (%f,%f,%f)\ntriangle A[2] = (%f,%f,%f)\n", m_triangleVerticesA[0].getX(),m_triangleVerticesA[0].getY(),m_triangleVerticesA[0].getZ(), m_triangleVerticesA[1].getX(),m_triangleVerticesA[1].getY(),m_triangleVerticesA[1].getZ(), m_triangleVerticesA[2].getX(),m_triangleVerticesA[2].getY(),m_triangleVerticesA[2].getZ()); printf("partId=%d, triangleIndex=%d\n",partId,triangleIndex); printf("triangle B[0] = (%f,%f,%f)\ntriangle B[1] = (%f,%f,%f)\ntriangle B[2] = (%f,%f,%f)\n", triangle[0].getX(),triangle[0].getY(),triangle[0].getZ(), triangle[1].getX(),triangle[1].getY(),triangle[1].getZ(), triangle[2].getX(),triangle[2].getY(),triangle[2].getZ()); #endif for (int i=0;i<3;i++) { for (int j=0;j<3;j++) { if ( (m_triangleVerticesA[i]-triangle[j]).length2() < m_triangleInfoMap->m_equalVertexThreshold) { sharedVertsA[numshared] = i; sharedVertsB[numshared] = j; numshared++; ///degenerate case if(numshared >= 3) return; } } ///degenerate case if(numshared >= 3) return; } switch (numshared) { case 0: { break; } case 1: { //shared vertex break; } case 2: { //shared edge //we need to make sure the edge is in the order V2V0 and not V0V2 so that the signs are correct if (sharedVertsA[0] == 0 && sharedVertsA[1] == 2) { sharedVertsA[0] = 2; sharedVertsA[1] = 0; int tmp = sharedVertsB[1]; sharedVertsB[1] = sharedVertsB[0]; sharedVertsB[0] = tmp; } int hash = btGetHash(m_partIdA,m_triangleIndexA); btTriangleInfo* info = m_triangleInfoMap->find(hash); if (!info) { btTriangleInfo tmp; m_triangleInfoMap->insert(hash,tmp); info = m_triangleInfoMap->find(hash); } int sumvertsA = sharedVertsA[0]+sharedVertsA[1]; int otherIndexA = 3-sumvertsA; btVector3 edge(m_triangleVerticesA[sharedVertsA[1]]-m_triangleVerticesA[sharedVertsA[0]]); btTriangleShape tA(m_triangleVerticesA[0],m_triangleVerticesA[1],m_triangleVerticesA[2]); int otherIndexB = 3-(sharedVertsB[0]+sharedVertsB[1]); btTriangleShape tB(triangle[sharedVertsB[1]],triangle[sharedVertsB[0]],triangle[otherIndexB]); //btTriangleShape tB(triangle[0],triangle[1],triangle[2]); btVector3 normalA; btVector3 normalB; tA.calcNormal(normalA); tB.calcNormal(normalB); edge.normalize(); btVector3 edgeCrossA = edge.cross(normalA).normalize(); { btVector3 tmp = m_triangleVerticesA[otherIndexA]-m_triangleVerticesA[sharedVertsA[0]]; if (edgeCrossA.dot(tmp) < 0) { edgeCrossA*=-1; } } btVector3 edgeCrossB = edge.cross(normalB).normalize(); { btVector3 tmp = triangle[otherIndexB]-triangle[sharedVertsB[0]]; if (edgeCrossB.dot(tmp) < 0) { edgeCrossB*=-1; } } btScalar angle2 = 0; btScalar ang4 = 0.f; btVector3 calculatedEdge = edgeCrossA.cross(edgeCrossB); btScalar len2 = calculatedEdge.length2(); btScalar correctedAngle(0); btVector3 calculatedNormalB = normalA; bool isConvex = false; if (len2<m_triangleInfoMap->m_planarEpsilon) { angle2 = 0.f; ang4 = 0.f; } else { calculatedEdge.normalize(); btVector3 calculatedNormalA = calculatedEdge.cross(edgeCrossA); calculatedNormalA.normalize(); angle2 = btGetAngle(calculatedNormalA,edgeCrossA,edgeCrossB); ang4 = SIMD_PI-angle2; btScalar dotA = normalA.dot(edgeCrossB); ///@todo: check if we need some epsilon, due to floating point imprecision isConvex = (dotA<0.); correctedAngle = isConvex ? ang4 : -ang4; btQuaternion orn2(calculatedEdge,-correctedAngle); calculatedNormalB = btMatrix3x3(orn2)*normalA; } //alternatively use //btVector3 calculatedNormalB2 = quatRotate(orn,normalA); switch (sumvertsA) { case 1: { btVector3 edge = m_triangleVerticesA[0]-m_triangleVerticesA[1]; btQuaternion orn(edge,-correctedAngle); btVector3 computedNormalB = quatRotate(orn,normalA); btScalar bla = computedNormalB.dot(normalB); if (bla<0) { computedNormalB*=-1; info->m_flags |= TRI_INFO_V0V1_SWAP_NORMALB; } #ifdef DEBUG_INTERNAL_EDGE if ((computedNormalB-normalB).length()>0.0001) { printf("warning: normals not identical\n"); } #endif//DEBUG_INTERNAL_EDGE info->m_edgeV0V1Angle = -correctedAngle; if (isConvex) info->m_flags |= TRI_INFO_V0V1_CONVEX; break; } case 2: { btVector3 edge = m_triangleVerticesA[2]-m_triangleVerticesA[0]; btQuaternion orn(edge,-correctedAngle); btVector3 computedNormalB = quatRotate(orn,normalA); if (computedNormalB.dot(normalB)<0) { computedNormalB*=-1; info->m_flags |= TRI_INFO_V2V0_SWAP_NORMALB; } #ifdef DEBUG_INTERNAL_EDGE if ((computedNormalB-normalB).length()>0.0001) { printf("warning: normals not identical\n"); } #endif //DEBUG_INTERNAL_EDGE info->m_edgeV2V0Angle = -correctedAngle; if (isConvex) info->m_flags |= TRI_INFO_V2V0_CONVEX; break; } case 3: { btVector3 edge = m_triangleVerticesA[1]-m_triangleVerticesA[2]; btQuaternion orn(edge,-correctedAngle); btVector3 computedNormalB = quatRotate(orn,normalA); if (computedNormalB.dot(normalB)<0) { info->m_flags |= TRI_INFO_V1V2_SWAP_NORMALB; computedNormalB*=-1; } #ifdef DEBUG_INTERNAL_EDGE if ((computedNormalB-normalB).length()>0.0001) { printf("warning: normals not identical\n"); } #endif //DEBUG_INTERNAL_EDGE info->m_edgeV1V2Angle = -correctedAngle; if (isConvex) info->m_flags |= TRI_INFO_V1V2_CONVEX; break; } } break; } default: { // printf("warning: duplicate triangle\n"); } } }
/// Changes a btManifoldPoint collision normal to the normal from the mesh. void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObject* colObj0,const btCollisionObject* colObj1, int partId0, int index0, int normalAdjustFlags) { //btAssert(colObj0->getCollisionShape()->getShapeType() == TRIANGLE_SHAPE_PROXYTYPE); if (colObj0->getCollisionShape()->getShapeType() != TRIANGLE_SHAPE_PROXYTYPE) return; btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*)colObj0->getRootCollisionShape(); btTriangleInfoMap* triangleInfoMapPtr = (btTriangleInfoMap*) trimesh->getTriangleInfoMap(); if (!triangleInfoMapPtr) return; int hash = btGetHash(partId0,index0); btTriangleInfo* info = triangleInfoMapPtr->find(hash); if (!info) return; btScalar frontFacing = (normalAdjustFlags & BT_TRIANGLE_CONVEX_BACKFACE_MODE)==0? 1.f : -1.f; const btTriangleShape* tri_shape = static_cast<const btTriangleShape*>(colObj0->getCollisionShape()); btVector3 v0,v1,v2; tri_shape->getVertex(0,v0); tri_shape->getVertex(1,v1); tri_shape->getVertex(2,v2); btVector3 center = (v0+v1+v2)*btScalar(1./3.); btVector3 red(1,0,0), green(0,1,0),blue(0,0,1),white(1,1,1),black(0,0,0); btVector3 tri_normal; tri_shape->calcNormal(tri_normal); //btScalar dot = tri_normal.dot(cp.m_normalWorldOnB); btVector3 nearest; btNearestPointInLineSegment(cp.m_localPointB,v0,v1,nearest); btVector3 contact = cp.m_localPointB; #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW const btTransform& tr = colObj0->getWorldTransform(); btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,red); #endif //BT_INTERNAL_EDGE_DEBUG_DRAW bool isNearEdge = false; int numConcaveEdgeHits = 0; int numConvexEdgeHits = 0; btVector3 localContactNormalOnB = colObj0->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB; localContactNormalOnB.normalize();//is this necessary? if ((info->m_edgeV0V1Angle)< SIMD_2_PI) { #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black); #endif btScalar len = (contact-nearest).length(); if(len<triangleInfoMapPtr->m_edgeDistanceThreshold) { btVector3 edge(v0-v1); isNearEdge = true; if (info->m_edgeV0V1Angle==btScalar(0)) { numConcaveEdgeHits++; } else { bool isEdgeConvex = (info->m_flags & TRI_INFO_V0V1_CONVEX); btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1); #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white); #endif //BT_INTERNAL_EDGE_DEBUG_DRAW btVector3 nA = swapFactor * tri_normal; btQuaternion orn(edge,info->m_edgeV0V1Angle); btVector3 computedNormalB = quatRotate(orn,tri_normal); if (info->m_flags & TRI_INFO_V0V1_SWAP_NORMALB) computedNormalB*=-1; btVector3 nB = swapFactor*computedNormalB; btScalar NdotA = localContactNormalOnB.dot(nA); btScalar NdotB = localContactNormalOnB.dot(nB); bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon); #ifdef DEBUG_INTERNAL_EDGE { btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red); } #endif //DEBUG_INTERNAL_EDGE if (backFacingNormal) { numConcaveEdgeHits++; } else { numConvexEdgeHits++; btVector3 clampedLocalNormal; bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB, info->m_edgeV0V1Angle,clampedLocalNormal); if (isClamped) { if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0)) { btVector3 newNormal = colObj0->getWorldTransform().getBasis() * clampedLocalNormal; // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB); cp.m_normalWorldOnB = newNormal; // Reproject collision point along normal. (what about cp.m_distance1?) cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; cp.m_localPointB = colObj0->getWorldTransform().invXform(cp.m_positionWorldOnB); } } } } } } btNearestPointInLineSegment(contact,v1,v2,nearest); #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,green); #endif //BT_INTERNAL_EDGE_DEBUG_DRAW if ((info->m_edgeV1V2Angle)< SIMD_2_PI) { #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black); #endif //BT_INTERNAL_EDGE_DEBUG_DRAW btScalar len = (contact-nearest).length(); if(len<triangleInfoMapPtr->m_edgeDistanceThreshold) { isNearEdge = true; #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW btDebugDrawLine(tr*nearest,tr*(nearest+tri_normal*10),white); #endif //BT_INTERNAL_EDGE_DEBUG_DRAW btVector3 edge(v1-v2); isNearEdge = true; if (info->m_edgeV1V2Angle == btScalar(0)) { numConcaveEdgeHits++; } else { bool isEdgeConvex = (info->m_flags & TRI_INFO_V1V2_CONVEX)!=0; btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1); #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white); #endif //BT_INTERNAL_EDGE_DEBUG_DRAW btVector3 nA = swapFactor * tri_normal; btQuaternion orn(edge,info->m_edgeV1V2Angle); btVector3 computedNormalB = quatRotate(orn,tri_normal); if (info->m_flags & TRI_INFO_V1V2_SWAP_NORMALB) computedNormalB*=-1; btVector3 nB = swapFactor*computedNormalB; #ifdef DEBUG_INTERNAL_EDGE { btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red); } #endif //DEBUG_INTERNAL_EDGE btScalar NdotA = localContactNormalOnB.dot(nA); btScalar NdotB = localContactNormalOnB.dot(nB); bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon); if (backFacingNormal) { numConcaveEdgeHits++; } else { numConvexEdgeHits++; btVector3 localContactNormalOnB = colObj0->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB; btVector3 clampedLocalNormal; bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB, info->m_edgeV1V2Angle,clampedLocalNormal); if (isClamped) { if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0)) { btVector3 newNormal = colObj0->getWorldTransform().getBasis() * clampedLocalNormal; // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB); cp.m_normalWorldOnB = newNormal; // Reproject collision point along normal. cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; cp.m_localPointB = colObj0->getWorldTransform().invXform(cp.m_positionWorldOnB); } } } } } } btNearestPointInLineSegment(contact,v2,v0,nearest); #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,blue); #endif //BT_INTERNAL_EDGE_DEBUG_DRAW if ((info->m_edgeV2V0Angle)< SIMD_2_PI) { #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black); #endif //BT_INTERNAL_EDGE_DEBUG_DRAW btScalar len = (contact-nearest).length(); if(len<triangleInfoMapPtr->m_edgeDistanceThreshold) { isNearEdge = true; #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW btDebugDrawLine(tr*nearest,tr*(nearest+tri_normal*10),white); #endif //BT_INTERNAL_EDGE_DEBUG_DRAW btVector3 edge(v2-v0); if (info->m_edgeV2V0Angle==btScalar(0)) { numConcaveEdgeHits++; } else { bool isEdgeConvex = (info->m_flags & TRI_INFO_V2V0_CONVEX)!=0; btScalar swapFactor = isEdgeConvex ? btScalar(1) : btScalar(-1); #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW btDebugDrawLine(tr*nearest,tr*(nearest+swapFactor*tri_normal*10),white); #endif //BT_INTERNAL_EDGE_DEBUG_DRAW btVector3 nA = swapFactor * tri_normal; btQuaternion orn(edge,info->m_edgeV2V0Angle); btVector3 computedNormalB = quatRotate(orn,tri_normal); if (info->m_flags & TRI_INFO_V2V0_SWAP_NORMALB) computedNormalB*=-1; btVector3 nB = swapFactor*computedNormalB; #ifdef DEBUG_INTERNAL_EDGE { btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+tr.getBasis()*(nB*20),red); } #endif //DEBUG_INTERNAL_EDGE btScalar NdotA = localContactNormalOnB.dot(nA); btScalar NdotB = localContactNormalOnB.dot(nB); bool backFacingNormal = (NdotA< triangleInfoMapPtr->m_convexEpsilon) && (NdotB<triangleInfoMapPtr->m_convexEpsilon); if (backFacingNormal) { numConcaveEdgeHits++; } else { numConvexEdgeHits++; // printf("hitting convex edge\n"); btVector3 localContactNormalOnB = colObj0->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB; btVector3 clampedLocalNormal; bool isClamped = btClampNormal(edge,swapFactor*tri_normal,localContactNormalOnB,info->m_edgeV2V0Angle,clampedLocalNormal); if (isClamped) { if (((normalAdjustFlags & BT_TRIANGLE_CONVEX_DOUBLE_SIDED)!=0) || (clampedLocalNormal.dot(frontFacing*tri_normal)>0)) { btVector3 newNormal = colObj0->getWorldTransform().getBasis() * clampedLocalNormal; // cp.m_distance1 = cp.m_distance1 * newNormal.dot(cp.m_normalWorldOnB); cp.m_normalWorldOnB = newNormal; // Reproject collision point along normal. cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; cp.m_localPointB = colObj0->getWorldTransform().invXform(cp.m_positionWorldOnB); } } } } } } #ifdef DEBUG_INTERNAL_EDGE { btVector3 color(0,1,1); btDebugDrawLine(cp.getPositionWorldOnB(),cp.getPositionWorldOnB()+cp.m_normalWorldOnB*10,color); } #endif //DEBUG_INTERNAL_EDGE if (isNearEdge) { if (numConcaveEdgeHits>0) { if ((normalAdjustFlags & BT_TRIANGLE_CONCAVE_DOUBLE_SIDED)!=0) { //fix tri_normal so it pointing the same direction as the current local contact normal if (tri_normal.dot(localContactNormalOnB) < 0) { tri_normal *= -1; } cp.m_normalWorldOnB = colObj0->getWorldTransform().getBasis()*tri_normal; } else { //modify the normal to be the triangle normal (or backfacing normal) cp.m_normalWorldOnB = colObj0->getWorldTransform().getBasis() *(tri_normal *frontFacing); } // Reproject collision point along normal. cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; cp.m_localPointB = colObj0->getWorldTransform().invXform(cp.m_positionWorldOnB); } } }
int main(int argc, char* argv[]) { { b3CommandLineArgs myArgs(argc, argv); app = new SimpleOpenGLApp("SimpleOpenGLApp", gWidth, gHeight); app->m_renderer->getActiveCamera()->setCameraDistance(13); app->m_renderer->getActiveCamera()->setCameraPitch(0); app->m_renderer->getActiveCamera()->setCameraTargetPosition(0, 0, 0); sOldKeyboardCB = app->m_window->getKeyboardCallback(); app->m_window->setKeyboardCallback(MyKeyboardCallback); sOldMouseMoveCB = app->m_window->getMouseMoveCallback(); app->m_window->setMouseMoveCallback(MyMouseMoveCallback); sOldMouseButtonCB = app->m_window->getMouseButtonCallback(); app->m_window->setMouseButtonCallback(MyMouseButtonCallback); sOldWheelCB = app->m_window->getWheelCallback(); app->m_window->setWheelCallback(MyWheelCallback); sOldResizeCB = app->m_window->getResizeCallback(); app->m_window->setResizeCallback(MyResizeCallback); myArgs.GetCmdLineArgument("mp4_file", gVideoFileName); if (gVideoFileName) app->dumpFramesToVideo(gVideoFileName); myArgs.GetCmdLineArgument("png_file", gPngFileName); char fileName[1024]; int textureWidth = 128; int textureHeight = 128; unsigned char* image = new unsigned char[textureWidth*textureHeight * 4]; int textureHandle = app->m_renderer->registerTexture(image, textureWidth, textureHeight); int cubeIndex = app->registerCubeShape(1, 1, 1); b3Vector3 pos = b3MakeVector3(0, 0, 0); b3Quaternion orn(0, 0, 0, 1); b3Vector3 color = b3MakeVector3(1, 0, 0); b3Vector3 scaling = b3MakeVector3 (1, 1, 1); app->m_renderer->registerGraphicsInstance(cubeIndex, pos, orn, color, scaling); app->m_renderer->writeTransforms(); do { static int frameCount = 0; frameCount++; if (gPngFileName) { printf("gPngFileName=%s\n", gPngFileName); sprintf(fileName, "%s%d.png", gPngFileName, frameCount++); app->dumpNextFrameToPng(fileName); } //update the texels of the texture using a simple pattern, animated using frame index for (int y = 0; y < textureHeight; ++y) { const int t = (y + frameCount) >> 4; unsigned char* pi = image + y*textureWidth * 3; for (int x = 0; x < textureWidth; ++x) { const int s = x >> 4; const unsigned char b = 180; unsigned char c = b + ((s + (t & 1)) & 1)*(255 - b); pi[0] = pi[1] = pi[2] = pi[3] = c; pi += 3; } } app->m_renderer->activateTexture(textureHandle); app->m_renderer->updateTexture(textureHandle, image); //float color[4] = { 255, 1, 1, 1 }; //app->m_primRenderer->drawTexturedRect(100, 200, gWidth / 2 - 50, gHeight / 2 - 50, color, 0, 0, 1, 1, true); app->m_renderer->init(); app->m_renderer->updateCamera(1); app->m_renderer->renderScene(); app->drawGrid(); char bla[1024]; sprintf(bla, "Simple test frame %d", frameCount); //app->drawText(bla, 10, 10); #ifdef B3_USE_IMGUI { bool show_test_window = true; bool show_another_window = false; ImVec4 clear_color = ImColor(114, 144, 154); // Start the frame ImGuiIO& io = ImGui::GetIO(); if (!g_FontTexture) ImGui_ImplBullet_CreateDeviceObjects(); io.DisplaySize = ImVec2((float)gWidth, (float)gHeight); io.DisplayFramebufferScale = ImVec2(gWidth > 0 ? ((float)1.) : 0, gHeight > 0 ? ((float)1.) : 0); io.DeltaTime = (float)(1.0f/60.0f); io.MousePos = ImVec2((float)gMouseX, (float)gMouseY); io.RenderDrawListsFn = ImGui_ImplBullet_RenderDrawLists; for (int i=0;i<3;i++) { io.MouseDown[i] = g_MousePressed[i]|g_MousePressed2[i]; g_MousePressed[i] = false; } io.MouseWheel = g_MouseWheel; ImGui::NewFrame(); ImGui::ShowTestWindow(); ImGui::ShowMetricsWindow(); #if 0 static float f = 0.0f; ImGui::Text("Hello, world!"); ImGui::SliderFloat("float", &f, 0.0f, 1.0f); ImGui::ColorEdit3("clear color", (float*)&clear_color); if (ImGui::Button("Test Window")) show_test_window ^= 1; if (ImGui::Button("Another Window")) show_another_window ^= 1; ImGui::Text("Application average %.3f ms/frame (%.1f FPS)", 1000.0f / ImGui::GetIO().Framerate, ImGui::GetIO().Framerate); #endif ImGui::Render(); } #endif //B3_USE_IMGUI app->swapBuffer(); } while (!app->m_window->requestedExit()); delete app; delete[] image; } return 0; }
void ConcaveDemo::initPhysics() { setTexturing(true); setShadows(false);//true); #define TRISIZE 10.f gContactAddedCallback = CustomMaterialCombinerCallback; #define USE_TRIMESH_SHAPE 1 #ifdef USE_TRIMESH_SHAPE int vertStride = sizeof(btVector3); int indexStride = 3*sizeof(int); const int totalTriangles = 2*(NUM_VERTS_X-1)*(NUM_VERTS_Y-1); gVertices = new btVector3[totalVerts]; gIndices = new int[totalTriangles*3]; int i; setVertexPositions(waveheight,0.f); int index=0; for ( i=0;i<NUM_VERTS_X-1;i++) { for (int j=0;j<NUM_VERTS_Y-1;j++) { gIndices[index++] = j*NUM_VERTS_X+i; gIndices[index++] = j*NUM_VERTS_X+i+1; gIndices[index++] = (j+1)*NUM_VERTS_X+i+1; gIndices[index++] = j*NUM_VERTS_X+i; gIndices[index++] = (j+1)*NUM_VERTS_X+i+1; gIndices[index++] = (j+1)*NUM_VERTS_X+i; } } m_indexVertexArrays = new btTriangleIndexVertexArray(totalTriangles, gIndices, indexStride, totalVerts,(btScalar*) &gVertices[0].x(),vertStride); bool useQuantizedAabbCompression = true; //comment out the next line to read the BVH from disk (first run the demo once to create the BVH) #ifdef SERIALIZE_TO_DISK btVector3 aabbMin(-1000,-1000,-1000),aabbMax(1000,1000,1000); trimeshShape = new btBvhTriangleMeshShape(m_indexVertexArrays,useQuantizedAabbCompression,aabbMin,aabbMax); m_collisionShapes.push_back(trimeshShape); int maxSerializeBufferSize = 1024*1024*5; btDefaultSerializer* serializer = new btDefaultSerializer(maxSerializeBufferSize); //serializer->setSerializationFlags(BT_SERIALIZE_NO_BVH);// or BT_SERIALIZE_NO_TRIANGLEINFOMAP serializer->startSerialization(); //registering a name is optional, it allows you to retrieve the shape by name //serializer->registerNameForPointer(trimeshShape,"mymesh"); #ifdef SERIALIZE_SHAPE trimeshShape->serializeSingleShape(serializer); #else trimeshShape->serializeSingleBvh(serializer); #endif serializer->finishSerialization(); FILE* f2 = fopen("myShape.bullet","wb"); fwrite(serializer->getBufferPointer(),serializer->getCurrentBufferSize(),1,f2); fclose(f2); #else btBulletWorldImporter import(0);//don't store info into the world if (import.loadFile("myShape.bullet")) { int numBvh = import.getNumBvhs(); if (numBvh) { btOptimizedBvh* bvh = import.getBvhByIndex(0); btVector3 aabbMin(-1000,-1000,-1000),aabbMax(1000,1000,1000); trimeshShape = new btBvhTriangleMeshShape(m_indexVertexArrays,useQuantizedAabbCompression,aabbMin,aabbMax,false); trimeshShape->setOptimizedBvh(bvh); //trimeshShape = new btBvhTriangleMeshShape(m_indexVertexArrays,useQuantizedAabbCompression,aabbMin,aabbMax); //trimeshShape->setOptimizedBvh(bvh); } int numShape = import.getNumCollisionShapes(); if (numShape) { trimeshShape = (btBvhTriangleMeshShape*)import.getCollisionShapeByIndex(0); //if you know the name, you can also try to get the shape by name: const char* meshName = import.getNameForPointer(trimeshShape); if (meshName) trimeshShape = (btBvhTriangleMeshShape*)import.getCollisionShapeByName(meshName); } } #endif btCollisionShape* groundShape = trimeshShape; #else btCollisionShape* groundShape = new btBoxShape(btVector3(50,3,50)); m_collisionShapes.push_back(groundShape); #endif //USE_TRIMESH_SHAPE m_collisionConfiguration = new btDefaultCollisionConfiguration(); #ifdef USE_PARALLEL_DISPATCHER #ifdef USE_WIN32_THREADING int maxNumOutstandingTasks = 4;//number of maximum outstanding tasks Win32ThreadSupport* threadSupport = new Win32ThreadSupport(Win32ThreadSupport::Win32ThreadConstructionInfo( "collision", processCollisionTask, createCollisionLocalStoreMemory, maxNumOutstandingTasks)); #else ///@todo show other platform threading ///Playstation 3 SPU (SPURS) version is available through PS3 Devnet ///Libspe2 SPU support will be available soon ///pthreads version ///you can hook it up to your custom task scheduler by deriving from btThreadSupportInterface #endif m_dispatcher = new SpuGatheringCollisionDispatcher(threadSupport,maxNumOutstandingTasks,m_collisionConfiguration); #else m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); #endif//USE_PARALLEL_DISPATCHER btVector3 worldMin(-1000,-1000,-1000); btVector3 worldMax(1000,1000,1000); m_broadphase = new btAxisSweep3(worldMin,worldMax); m_solver = new btSequentialImpulseConstraintSolver(); m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration); #ifdef USE_PARALLEL_DISPATCHER m_dynamicsWorld->getDispatchInfo().m_enableSPU=true; #endif //USE_PARALLEL_DISPATCHER float mass = 0.f; btTransform startTransform; startTransform.setIdentity(); startTransform.setOrigin(btVector3(0,-2,0)); #ifdef USE_BOX_SHAPE btCollisionShape* colShape = new btBoxShape(btVector3(1,1,1)); #else btCompoundShape* colShape = new btCompoundShape; btCollisionShape* cylinderShape = new btCylinderShapeX(btVector3(4,1,1)); btCollisionShape* boxShape = new btBoxShape(btVector3(4,1,1)); btTransform localTransform; localTransform.setIdentity(); colShape->addChildShape(localTransform,boxShape); btQuaternion orn(SIMD_HALF_PI,0,0); localTransform.setRotation(orn); colShape->addChildShape(localTransform,cylinderShape); #endif //USE_BOX_SHAPE m_collisionShapes.push_back(colShape); { for (int i=0;i<10;i++) { startTransform.setOrigin(btVector3(2,10+i*2,1)); localCreateRigidBody(1, startTransform,colShape); } } startTransform.setIdentity(); staticBody = localCreateRigidBody(mass, startTransform,groundShape); staticBody->setCollisionFlags(staticBody->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);//STATIC_OBJECT); //enable custom material callback staticBody->setCollisionFlags(staticBody->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK); }
btMultiBody* createFeatherstoneMultiBody(class btMultiBodyDynamicsWorld* world, const btMultiBodySettings& settings) { static int curColor=0; int cubeShapeId = m_glApp->registerCubeShape(); int n_links = settings.m_numLinks; float mass = 13.5*scaling; btVector3 inertia = btVector3 (91,344,253)*scaling*scaling; btMultiBody * bod = new btMultiBody(n_links, mass, inertia, settings.m_isFixedBase, settings.m_canSleep); // bod->setHasSelfCollision(false); //btQuaternion orn(btVector3(0,0,1),-0.25*SIMD_HALF_PI);//0,0,0,1); btQuaternion orn(0,0,0,1); bod->setBasePos(settings.m_basePosition); bod->setWorldToBaseRot(orn); btVector3 vel(0,0,0); bod->setBaseVel(vel); { btVector3 joint_axis_hinge(1,0,0); btVector3 joint_axis_prismatic(0,0,1); btQuaternion parent_to_child = orn.inverse(); btVector3 joint_axis_child_prismatic = quatRotate(parent_to_child ,joint_axis_prismatic); btVector3 joint_axis_child_hinge = quatRotate(parent_to_child , joint_axis_hinge); int this_link_num = -1; int link_num_counter = 0; btVector3 pos = btVector3 (0,0,9.0500002)*scaling; btVector3 joint_axis_position = btVector3 (0,0,4.5250001)*scaling; for (int i=0;i<n_links;i++) { float initial_joint_angle=0.3; if (i>0) initial_joint_angle = -0.06f; const int child_link_num = link_num_counter++; if (settings.m_usePrismatic)// && i==(n_links-1)) { bod->setupPrismatic(child_link_num, mass, inertia, this_link_num, parent_to_child, joint_axis_child_prismatic, quatRotate(parent_to_child , pos),settings.m_disableParentCollision); } else { bod->setupRevolute(child_link_num, mass, inertia, this_link_num,parent_to_child, joint_axis_child_hinge, joint_axis_position,quatRotate(parent_to_child , (pos - joint_axis_position)),settings.m_disableParentCollision); } bod->setJointPos(child_link_num, initial_joint_angle); this_link_num = i; if (0)//!useGroundShape && i==4) { btVector3 pivotInAworld(0,20,46); btVector3 pivotInAlocal = bod->worldPosToLocal(i, pivotInAworld); btVector3 pivotInBworld = pivotInAworld; btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(bod,i,&btTypedConstraint::getFixedBody(),pivotInAlocal,pivotInBworld); world->addMultiBodyConstraint(p2p); } //add some constraint limit if (settings.m_usePrismatic) { // btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(bod,n_links-1,2,3); if (settings.m_createConstraints) { btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(bod,i,-1,1); world->addMultiBodyConstraint(con); } } else { if (settings.m_createConstraints) { if (1) { btMultiBodyJointMotor* con = new btMultiBodyJointMotor(bod,i,0,500000); world->addMultiBodyConstraint(con); } btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(bod,i,-1,1); world->addMultiBodyConstraint(con); } } } } //add a collider for the base { btAlignedObjectArray<btQuaternion> world_to_local; world_to_local.resize(n_links+1); btAlignedObjectArray<btVector3> local_origin; local_origin.resize(n_links+1); world_to_local[0] = bod->getWorldToBaseRot(); local_origin[0] = bod->getBasePos(); //float halfExtents[3]={7.5,0.05,4.5}; float halfExtents[3]={7.5,0.45,4.5}; { float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1}; float quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()}; if (1) { btCollisionShape* box = new btBoxShape(btVector3(halfExtents[0],halfExtents[1],halfExtents[2])*scaling); btRigidBody* body = new btRigidBody(mass,0,box,inertia); btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(bod,-1); body->setCollisionShape(box); col->setCollisionShape(box); btTransform tr; tr.setIdentity(); tr.setOrigin(local_origin[0]); tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); body->setWorldTransform(tr); col->setWorldTransform(tr); b3Vector4 color = colors[curColor++]; curColor&=3; int index = m_glApp->m_instancingRenderer->registerGraphicsInstance(cubeShapeId,tr.getOrigin(),tr.getRotation(),color,halfExtents); col->setUserIndex(index); world->addCollisionObject(col,short(btBroadphaseProxy::DefaultFilter),short(btBroadphaseProxy::AllFilter)); col->setFriction(friction); bod->setBaseCollider(col); } } for (int i=0;i<bod->getNumLinks();i++) { const int parent = bod->getParent(i); world_to_local[i+1] = bod->getParentToLocalRot(i) * world_to_local[parent+1]; local_origin[i+1] = local_origin[parent+1] + (quatRotate(world_to_local[i+1].inverse() , bod->getRVector(i))); } for (int i=0;i<bod->getNumLinks();i++) { btVector3 posr = local_origin[i+1]; float pos[4]={posr.x(),posr.y(),posr.z(),1}; float quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()}; btCollisionShape* box = new btBoxShape(btVector3(halfExtents[0],halfExtents[1],halfExtents[2])*scaling); btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(bod,i); col->setCollisionShape(box); btTransform tr; tr.setIdentity(); tr.setOrigin(posr); tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); col->setWorldTransform(tr); col->setFriction(friction); b3Vector4 color = colors[curColor++]; curColor&=3; int index = m_glApp->m_instancingRenderer->registerGraphicsInstance(cubeShapeId,tr.getOrigin(),tr.getRotation(),color,halfExtents); col->setUserIndex(index); world->addCollisionObject(col,short(btBroadphaseProxy::DefaultFilter),short(btBroadphaseProxy::AllFilter)); bod->getLink(i).m_collider=col; //app->drawBox(halfExtents, pos,quat); } } world->addMultiBody(bod); return bod; }
void ConcaveScene::createDynamicObjects(const ConstructionInfo& ci) { int strideInBytes = 9*sizeof(float); int numVertices = sizeof(cube_vertices)/strideInBytes; int numIndices = sizeof(cube_indices)/sizeof(int); //int shapeId = ci.m_instancingRenderer->registerShape(&cube_vertices[0],numVertices,cube_indices,numIndices); int shapeId = ci.m_instancingRenderer->registerShape(&cube_vertices[0],numVertices,cube_indices,numIndices); int group=1; int mask=1; int index=0; if (1) { int curColor = 0; b3Vector4 colors[4] = { b3MakeVector4(1,1,1,1), b3MakeVector4(1,1,0.3,1), b3MakeVector4(0.3,1,1,1), b3MakeVector4(0.3,0.3,1,1), }; b3ConvexUtility* utilPtr = new b3ConvexUtility(); b3Vector4 scaling=b3MakeVector4(1,1,1,1); { b3AlignedObjectArray<b3Vector3> verts; unsigned char* vts = (unsigned char*) cube_vertices; for (int i=0;i<numVertices;i++) { float* vertex = (float*) &vts[i*strideInBytes]; verts.push_back(b3MakeVector3(vertex[0]*scaling[0],vertex[1]*scaling[1],vertex[2]*scaling[2])); } bool merge = true; if (numVertices) { utilPtr->initializePolyhedralFeatures(&verts[0],verts.size(),merge); } } // int colIndex = m_data->m_np->registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling); int colIndex=-1; if (ci.m_useInstancedCollisionShapes) colIndex = m_data->m_np->registerConvexHullShape(utilPtr); for (int i=0;i<ci.arraySizeX;i++) { for (int j=0;j<ci.arraySizeY;j++) { for (int k=0;k<ci.arraySizeZ;k++) { if (!ci.m_useInstancedCollisionShapes) colIndex = m_data->m_np->registerConvexHullShape(utilPtr); float mass = 1; //b3Vector3 position(-2*ci.gapX+i*ci.gapX,25+j*ci.gapY,-2*ci.gapZ+k*ci.gapZ); b3Vector3 position=b3MakeVector3(-(ci.arraySizeX/2)*CONCAVE_GAPX+i*CONCAVE_GAPX, 23+j*CONCAVE_GAPY, -(ci.arraySizeZ/2)*CONCAVE_GAPZ+k*CONCAVE_GAPZ); b3Quaternion orn(0,0,0,1); b3Vector4 color = colors[curColor]; curColor++; curColor&=3; int id = ci.m_instancingRenderer->registerGraphicsInstance(shapeId,position,orn,color,scaling); int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(mass,position,orn,colIndex,index,false); index++; } } } } }
void readNodeHierarchy(TiXmlElement* node,btHashMap<btHashString,int>& name2Shape, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances, const btMatrix4x4& parentTransMat) { const char* nodeName = node->Attribute("id"); printf("processing node %s\n", nodeName); btMatrix4x4 nodeTrans; nodeTrans.setIdentity(); ///todo(erwincoumans) we probably have to read the elements 'translate', 'scale', 'rotate' and 'matrix' in-order and accumulate them... { for (TiXmlElement* transElem = node->FirstChildElement("matrix");transElem;transElem=node->NextSiblingElement("matrix")) { if (transElem->GetText()) { btAlignedObjectArray<float> floatArray; TokenFloatArray adder(floatArray); tokenize(transElem->GetText(),adder); if (floatArray.size()==16) { btMatrix4x4 t(floatArray[0],floatArray[1],floatArray[2],floatArray[3], floatArray[4],floatArray[5],floatArray[6],floatArray[7], floatArray[8],floatArray[9],floatArray[10],floatArray[11], floatArray[12],floatArray[13],floatArray[14],floatArray[15]); nodeTrans = nodeTrans*t; } else { printf("Error: expected 16 elements in a <matrix> element, skipping\n"); } } } } { for (TiXmlElement* transElem = node->FirstChildElement("translate");transElem;transElem=node->NextSiblingElement("translate")) { if (transElem->GetText()) { btVector3 pos = getVector3FromXmlText(transElem->GetText()); //nodePos+= unitScaling*parentScaling*pos; btMatrix4x4 t; t.setPureTranslation(pos); nodeTrans = nodeTrans*t; } } } { for(TiXmlElement* scaleElem = node->FirstChildElement("scale"); scaleElem!= NULL; scaleElem= node->NextSiblingElement("scale")) { if (scaleElem->GetText()) { btVector3 scaling = getVector3FromXmlText(scaleElem->GetText()); btMatrix4x4 t; t.setPureScaling(scaling); nodeTrans = nodeTrans*t; } } } { for(TiXmlElement* rotateElem = node->FirstChildElement("rotate"); rotateElem!= NULL; rotateElem= node->NextSiblingElement("rotate")) { if (rotateElem->GetText()) { //accumulate orientation btVector4 rotate = getVector4FromXmlText(rotateElem->GetText()); btQuaternion orn(btVector3(rotate),btRadians(rotate[3]));//COLLADA DAE rotate is in degrees, convert to radians btMatrix4x4 t; t.setPureRotation(orn); nodeTrans = nodeTrans*t; } } } nodeTrans = parentTransMat*nodeTrans; for (TiXmlElement* instanceGeom = node->FirstChildElement("instance_geometry"); instanceGeom!=0; instanceGeom=instanceGeom->NextSiblingElement("instance_geometry")) { const char* geomUrl = instanceGeom->Attribute("url"); printf("node referring to geom %s\n", geomUrl); geomUrl++; int* shapeIndexPtr = name2Shape[geomUrl]; if (shapeIndexPtr) { int index = *shapeIndexPtr; printf("found geom with index %d\n", *shapeIndexPtr); ColladaGraphicsInstance& instance = visualShapeInstances.expand(); instance.m_shapeIndex = *shapeIndexPtr; instance.m_worldTransform = nodeTrans; } else { printf("geom not found\n"); } } for(TiXmlElement* childNode = node->FirstChildElement("node"); childNode!= NULL; childNode = childNode->NextSiblingElement("node")) { readNodeHierarchy(childNode,name2Shape,visualShapeInstances, nodeTrans); } }
CollisionTutorialBullet2(GUIHelperInterface* guiHelper, int tutorialIndex) :m_app(guiHelper->getAppInterface()), m_guiHelper(guiHelper), m_tutorialIndex(tutorialIndex), m_collisionSdkHandle(0), m_collisionWorldHandle(0), m_stage(0), m_counter(0), m_timeSeriesCanvas0(0) { gTotalPoints = 0; m_app->setUpAxis(1); m_app->m_renderer->enableBlend(true); switch (m_tutorialIndex) { case TUT_SPHERE_PLANE_RTB3: case TUT_SPHERE_PLANE_BULLET2: { if (m_tutorialIndex==TUT_SPHERE_PLANE_BULLET2) { m_collisionSdkHandle = plCreateBullet2CollisionSdk(); } else { #ifndef DISABLE_REAL_TIME_BULLET3_COLLISION_SDK m_collisionSdkHandle = plCreateRealTimeBullet3CollisionSdk(); #endif //DISABLE_REAL_TIME_BULLET3_COLLISION_SDK } if (m_collisionSdkHandle) { int maxNumObjsCapacity=1024; int maxNumShapesCapacity=1024; int maxNumPairsCapacity=16384; btAlignedObjectArray<plCollisionObjectHandle> colliders; m_collisionWorldHandle = plCreateCollisionWorld(m_collisionSdkHandle,maxNumObjsCapacity,maxNumShapesCapacity,maxNumPairsCapacity); //create objects, do query etc { float radius = 1.f; void* userPointer = 0; { for (int j=0;j<sNumCompounds;j++) { plCollisionShapeHandle compoundShape = plCreateCompoundShape(m_collisionSdkHandle,m_collisionWorldHandle); for (int i=0;i<sNumSpheres;i++) { btVector3 childPos(i*1.5,0,0); btQuaternion childOrn(0,0,0,1); btVector3 scaling(radius,radius,radius); plCollisionShapeHandle childShape = plCreateSphereShape(m_collisionSdkHandle, m_collisionWorldHandle,radius); plAddChildShape(m_collisionSdkHandle,m_collisionWorldHandle,compoundShape, childShape,childPos,childOrn); //m_guiHelper->createCollisionObjectGraphicsObject(colObj,color); } if (m_tutorialIndex==TUT_SPHERE_PLANE_BULLET2) { btCollisionShape* colShape = (btCollisionShape*) compoundShape; m_guiHelper->createCollisionShapeGraphicsObject(colShape); } else { } { btVector3 pos(j*sNumSpheres*1.5,-2.4,0); btQuaternion orn(0,0,0,1); plCollisionObjectHandle colObjHandle = plCreateCollisionObject(m_collisionSdkHandle,m_collisionWorldHandle,userPointer, -1,compoundShape,pos,orn); if (m_tutorialIndex==TUT_SPHERE_PLANE_BULLET2) { btCollisionObject* colObj = (btCollisionObject*) colObjHandle; btVector4 color=sColors[j&3]; m_guiHelper->createCollisionObjectGraphicsObject(colObj,color); colliders.push_back(colObjHandle); plAddCollisionObject(m_collisionSdkHandle, m_collisionWorldHandle,colObjHandle); } } } } } { plCollisionShapeHandle colShape = plCreatePlaneShape(m_collisionSdkHandle, m_collisionWorldHandle,0,1,0,-3.5); btVector3 pos(0,0,0); btQuaternion orn(0,0,0,1); void* userPointer = 0; plCollisionObjectHandle colObj = plCreateCollisionObject(m_collisionSdkHandle,m_collisionWorldHandle,userPointer, 0,colShape,pos,orn); colliders.push_back(colObj); plAddCollisionObject(m_collisionSdkHandle, m_collisionWorldHandle,colObj); } int numContacts = plCollide(m_collisionSdkHandle,m_collisionWorldHandle,colliders[0],colliders[1],pointsOut,sPointCapacity); printf("numContacts = %d\n", numContacts); void* myUserPtr = 0; plWorldCollide(m_collisionSdkHandle,m_collisionWorldHandle,myNearCallback, myUserPtr); printf("total points=%d\n",gTotalPoints); //plRemoveCollisionObject(m_collisionSdkHandle,m_collisionWorldHandle,colObj); //plDeleteCollisionObject(m_collisionSdkHandle,colObj); //plDeleteShape(m_collisionSdkHandle,colShape); } /* m_timeSeriesCanvas0 = new TimeSeriesCanvas(m_app->m_2dCanvasInterface,512,256,"Constant Velocity"); m_timeSeriesCanvas0 ->setupTimeSeries(2,60, 0); m_timeSeriesCanvas0->addDataSource("X position (m)", 255,0,0); m_timeSeriesCanvas0->addDataSource("X velocity (m/s)", 0,0,255); m_timeSeriesCanvas0->addDataSource("dX/dt (m/s)", 0,0,0); */ break; } default: { m_timeSeriesCanvas0 = new TimeSeriesCanvas(m_app->m_2dCanvasInterface,512,256,"Unknown"); m_timeSeriesCanvas0 ->setupTimeSeries(1,60, 0); } }; { int boxId = m_app->registerCubeShape(100,0.01,100); b3Vector3 pos = b3MakeVector3(0,-3.5,0); b3Quaternion orn(0,0,0,1); b3Vector4 color = b3MakeVector4(1,1,1,1); b3Vector3 scaling = b3MakeVector3(1,1,1); m_app->m_renderer->registerGraphicsInstance(boxId,pos,orn,color,scaling); } { int textureIndex = -1; if (1) { int width,height,n; const char* filename = "data/cube.png"; const unsigned char* image=0; const char* prefix[]={"./","../","../../","../../../","../../../../"}; int numprefix = sizeof(prefix)/sizeof(const char*); for (int i=0;!image && i<numprefix;i++) { char relativeFileName[1024]; sprintf(relativeFileName,"%s%s",prefix[i],filename); image = stbi_load(relativeFileName, &width, &height, &n, 3); } b3Assert(image); if (image) { textureIndex = m_app->m_renderer->registerTexture(image,width,height); } } } m_app->m_renderer->writeTransforms(); }
/* Create from TetGen .ele, .face, .node data */ void GpuTetraScene::createFromTetGenData(const char* ele, const char* node, const ConstructionInfo& ci) { b3Scalar scaling(10); b3AlignedObjectArray<b3Vector3> pos; int nnode=0; int ndims=0; int nattrb=0; int hasbounds=0; int result = sscanf(node,"%d %d %d %d",&nnode,&ndims,&nattrb,&hasbounds); result = sscanf(node,"%d %d %d %d",&nnode,&ndims,&nattrb,&hasbounds); node += nextLine(node); //b3AlignedObjectArray<b3Vector3> rigidBodyPositions; //b3AlignedObjectArray<int> rigidBodyIds; pos.resize(nnode); for(int i=0;i<pos.size();++i) { int index=0; //int bound=0; float x,y,z; sscanf(node,"%d %f %f %f",&index,&x,&y,&z); // sn>>index; // sn>>x;sn>>y;sn>>z; node += nextLine(node); //for(int j=0;j<nattrb;++j) // sn>>a; //if(hasbounds) // sn>>bound; pos[index].setX(b3Scalar(x)*scaling); pos[index].setY(b3Scalar(y)*scaling); pos[index].setZ(b3Scalar(z)*scaling); } if(ele&&ele[0]) { int ntetra=0; int ncorner=0; int neattrb=0; sscanf(ele,"%d %d %d",&ntetra,&ncorner,&neattrb); ele += nextLine(ele); //se>>ntetra;se>>ncorner;se>>neattrb; for(int i=0;i<ntetra;++i) { int index=0; int ni[4]; //se>>index; //se>>ni[0];se>>ni[1];se>>ni[2];se>>ni[3]; sscanf(ele,"%d %d %d %d %d",&index,&ni[0],&ni[1],&ni[2],&ni[3]); ele+=nextLine(ele); b3Vector3 average=b3MakeVector3(0,0,0); for (int v=0;v<4;v++) { average+=pos[ni[v]]; } average/=4; for (int v=0;v<4;v++) { b3Vector3 shiftedPos = pos[ni[v]]-average; mytetra_vertices[0+v*9] = shiftedPos.getX(); mytetra_vertices[1+v*9] = shiftedPos.getY(); mytetra_vertices[2+v*9] = shiftedPos.getZ(); } //todo: subtract average int strideInBytes = 9*sizeof(float); int numVertices = sizeof(mytetra_vertices)/strideInBytes; int numIndices = sizeof(mytetra_indices)/sizeof(int); int shapeId = ci.m_instancingRenderer->registerShape(&mytetra_vertices[0],numVertices,mytetra_indices,numIndices); int group=1; int mask=1; { b3Vector4 scaling=b3MakeVector4(1,1,1,1); int colIndex = m_data->m_np->registerConvexHullShape(&mytetra_vertices[0],strideInBytes,numVertices, scaling); b3Vector3 position=b3MakeVector3(0,150,0); // position+=average;//*1.2;//*2; position+=average*1.2;//*2; //rigidBodyPositions.push_back(position); b3Quaternion orn(0,0,0,1); static int curColor=0; b3Vector4 color = colors[curColor++]; curColor&=3; int id = ci.m_instancingRenderer->registerGraphicsInstance(shapeId,position,orn,color,scaling); int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(1.f,position,orn,colIndex,0,false); //rigidBodyIds.push_back(pid); } //for(int j=0;j<neattrb;++j) // se>>a; //psb->appendTetra(ni[0],ni[1],ni[2],ni[3]); } // printf("Nodes: %u\r\n",psb->m_nodes.size()); // printf("Links: %u\r\n",psb->m_links.size()); // printf("Faces: %u\r\n",psb->m_faces.size()); // printf("Tetras: %u\r\n",psb->m_tetras.size()); } m_data->m_rigidBodyPipeline->writeAllInstancesToGpu(); m_data->m_np->writeAllBodiesToGpu(); m_data->m_bp->writeAabbsToGpu(); m_data->m_rigidBodyPipeline->setupGpuAabbsFull(); m_data->m_bp->calculateOverlappingPairs(m_data->m_config.m_maxBroadphasePairs); int numPairs = m_data->m_bp->getNumOverlap(); cl_mem pairs = m_data->m_bp->getOverlappingPairBuffer(); b3OpenCLArray<b3Int2> clPairs(m_clData->m_clContext,m_clData->m_clQueue); clPairs.setFromOpenCLBuffer(pairs,numPairs); b3AlignedObjectArray<b3Int2> allPairs; clPairs.copyToHost(allPairs); for (int p=0;p<allPairs.size();p++) { b3Vector3 posA,posB; b3Quaternion ornA,ornB; int bodyIndexA = allPairs[p].x; int bodyIndexB = allPairs[p].y; m_data->m_np->getObjectTransformFromCpu(posA,ornA,bodyIndexA); m_data->m_np->getObjectTransformFromCpu(posB,ornB,bodyIndexB); b3Vector3 pivotWorld = (posA+posB)*0.5f; b3Transform transA,transB; transA.setIdentity(); transA.setOrigin(posA); transA.setRotation(ornA); transB.setIdentity(); transB.setOrigin(posB); transB.setRotation(ornB); b3Vector3 pivotInA = transA.inverse()*pivotWorld; b3Vector3 pivotInB = transB.inverse()*pivotWorld; b3Transform frameInA,frameInB; frameInA.setIdentity(); frameInB.setIdentity(); frameInA.setOrigin(pivotInA); frameInB.setOrigin(pivotInB); b3Quaternion relTargetAB = frameInA.getRotation()*frameInB.getRotation().inverse(); //c = new b3FixedConstraint(pid,prevBody,frameInA,frameInB); float breakingThreshold = 45;//37.f; //c->setBreakingImpulseThreshold(37.1); bool useGPU = true; if (useGPU) { int cid = m_data->m_rigidBodyPipeline->createFixedConstraint(bodyIndexA,bodyIndexB,pivotInA,pivotInB,relTargetAB,breakingThreshold); } else { b3FixedConstraint* c = new b3FixedConstraint(bodyIndexA,bodyIndexB,frameInA,frameInB); c->setBreakingImpulseThreshold(breakingThreshold); m_data->m_rigidBodyPipeline->addConstraint(c); } } printf("numPairs = %d\n",numPairs); }
void ConcaveCompoundScene::createDynamicObjects(const ConstructionInfo& ci) { int strideInBytes = 9*sizeof(float); int numVertices = sizeof(cube_vertices)/strideInBytes; int numIndices = sizeof(cube_indices)/sizeof(int); b3AlignedObjectArray<GLInstanceVertex> vertexArray; b3AlignedObjectArray<int> indexArray; //int shapeId = ci.m_instancingRenderer->registerShape(&cube_vertices[0],numVertices,cube_indices,numIndices); int group=1; int mask=1; int index=0; float scaling[4] = {1,1,1,1}; int colIndex = 0; GLInstanceVertex* cubeVerts = (GLInstanceVertex*)&cube_vertices[0]; int stride2 = sizeof(GLInstanceVertex); b3Assert(stride2 == strideInBytes); { int childColIndex = m_data->m_np->registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling); b3Vector3 childPositions[3] = { b3MakeVector3(0,-2,0), b3MakeVector3(0,0,0), b3MakeVector3(0,0,2) }; b3AlignedObjectArray<b3GpuChildShape> childShapes; int numChildShapes = 3; for (int i=0;i<numChildShapes;i++) { //for now, only support polyhedral child shapes b3GpuChildShape child; child.m_shapeIndex = childColIndex; b3Vector3 pos = childPositions[i]; b3Quaternion orn(0,0,0,1); for (int v=0;v<4;v++) { child.m_childPosition[v] = pos[v]; child.m_childOrientation[v] = orn[v]; } childShapes.push_back(child); b3Transform tr; tr.setIdentity(); tr.setOrigin(pos); tr.setRotation(orn); int baseIndex = vertexArray.size(); for (int j=0;j<numIndices;j++) indexArray.push_back(cube_indices[j]+baseIndex); //add transformed graphics vertices and indices for (int v=0;v<numVertices;v++) { GLInstanceVertex vert = cubeVerts[v]; b3Vector3 vertPos=b3MakeVector3(vert.xyzw[0],vert.xyzw[1],vert.xyzw[2]); b3Vector3 newPos = tr*vertPos; vert.xyzw[0] = newPos[0]; vert.xyzw[1] = newPos[1]; vert.xyzw[2] = newPos[2]; vert.xyzw[3] = 0.f; vertexArray.push_back(vert); } } colIndex= m_data->m_np->registerCompoundShape(&childShapes); } //int shapeId = ci.m_instancingRenderer->registerShape(&cube_vertices[0],numVertices,cube_indices,numIndices); int shapeId = ci.m_instancingRenderer->registerShape(&vertexArray[0].xyzw[0],vertexArray.size(),&indexArray[0],indexArray.size()); b3Vector4 colors[4] = { b3MakeVector4(1,0,0,1), b3MakeVector4(0,1,0,1), b3MakeVector4(0,0,1,1), b3MakeVector4(0,1,1,1), }; int curColor = 0; for (int i=0;i<ci.arraySizeX;i++) { for (int j=0;j<ci.arraySizeY;j++) { for (int k=0;k<ci.arraySizeZ;k++) { float mass = 1;//j==0? 0.f : 1.f; b3Vector3 position=b3MakeVector3((-ci.arraySizeX/2+i)*ci.gapX,50+j*ci.gapY,(-ci.arraySizeZ/2+k)*ci.gapZ); //b3Quaternion orn(0,0,0,1); b3Quaternion orn(b3MakeVector3(1,0,0),0.7); b3Vector4 color = colors[curColor]; curColor++; curColor&=3; b3Vector4 scaling=b3MakeVector4(1,1,1,1); int id = ci.m_instancingRenderer->registerGraphicsInstance(shapeId,position,orn,color,scaling); int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(mass,position,orn,colIndex,index,false); index++; } } } }
void ConcaveCompound2Scene::createDynamicObjects(const ConstructionInfo& ci) { char* fileName = "teddy2_VHACD_CHs.obj"; //char* fileName = "cube_offset.obj"; b3Vector3 shift=b3MakeVector3(0,0,0);//0,230,80);//150,-100,-120); b3Vector4 scaling=b3MakeVector4(1,1,1,1); const char* prefix[]={"./data/","../data/","../../data/","../../../data/","../../../../data/"}; int prefixIndex=-1; char relativeFileName[1024]; { int numPrefixes = sizeof(prefix)/sizeof(char*); for (int i=0;i<numPrefixes;i++) { sprintf(relativeFileName,"%s%s",prefix[i],fileName); FILE* f = 0; f = fopen(relativeFileName,"r"); if (f) { prefixIndex = i; fclose(f); break; } } } if (prefixIndex<0) return; std::vector<tinyobj::shape_t> shapes; std::string err = tinyobj::LoadObj(shapes, relativeFileName, prefix[prefixIndex]); if (shapes.size()>0) { int strideInBytes = 9*sizeof(float); b3AlignedObjectArray<GLInstanceVertex> vertexArray; b3AlignedObjectArray<int> indexArray; //int shapeId = ci.m_instancingRenderer->registerShape(&cube_vertices[0],numVertices,cube_indices,numIndices); int group=1; int mask=1; int index=0; int colIndex = 0; b3AlignedObjectArray<GLInstanceVertex> vertices; int stride2 = sizeof(GLInstanceVertex); b3Assert(stride2 == strideInBytes); { b3AlignedObjectArray<b3GpuChildShape> childShapes; int numChildShapes = shapes.size(); for (int i=0;i<numChildShapes;i++) // int i=4; { tinyobj::shape_t& shape = shapes[i]; int numVertices = shape.mesh.positions.size()/3; int numFaces = shape.mesh.indices.size()/3; //for now, only support polyhedral child shapes b3GpuChildShape child; b3Vector3 pos=b3MakeVector3(0,0,0); b3Quaternion orn(0,0,0,1); for (int v=0;v<4;v++) { child.m_childPosition[v] = pos[v]; child.m_childOrientation[v] = orn[v]; } b3Transform tr; tr.setIdentity(); tr.setOrigin(pos); tr.setRotation(orn); int baseIndex = vertexArray.size(); for (int f=0;f<numFaces;f++) { for (int i=0;i<3;i++) { indexArray.push_back(baseIndex+shape.mesh.indices[f*3+i]); } } b3Vector3 center=b3MakeVector3(0,0,0); b3AlignedObjectArray<GLInstanceVertex> tmpVertices; //add transformed graphics vertices and indices b3Vector3 myScaling=b3MakeVector3(50,50,50);//300,300,300); for (int v=0;v<numVertices;v++) { GLInstanceVertex vert; vert.uv[0] = 0.5f; vert.uv[1] = 0.5f; vert.normal[0]=0.f; vert.normal[1]=1.f; vert.normal[2]=0.f; b3Vector3 vertPos; vertPos[0] = shape.mesh.positions[v*3+0]*myScaling[0]; vertPos[1] = shape.mesh.positions[v*3+1]*myScaling[1]; vertPos[2] = shape.mesh.positions[v*3+2]*myScaling[2]; vertPos[3] =0.f; center+=vertPos; } center/=numVertices; for (int v=0;v<numVertices;v++) { GLInstanceVertex vert; vert.uv[0] = 0.5f; vert.uv[1] = 0.5f; vert.normal[0]=0.f; vert.normal[1]=1.f; vert.normal[2]=0.f; b3Vector3 vertPos; vertPos[0] = shape.mesh.positions[v*3+0]*myScaling[0]; vertPos[1] = shape.mesh.positions[v*3+1]*myScaling[1]; vertPos[2] = shape.mesh.positions[v*3+2]*myScaling[2]; vertPos[3] =0.f; // vertPos-=center; vert.xyzw[0] = vertPos[0]; vert.xyzw[1] = vertPos[1]; vert.xyzw[2] = vertPos[2]; tmpVertices.push_back(vert); b3Vector3 newPos = tr*vertPos; vert.xyzw[0] = newPos[0]; vert.xyzw[1] = newPos[1]; vert.xyzw[2] = newPos[2]; vert.xyzw[3] = 0.f; vertexArray.push_back(vert); } int childColIndex = m_data->m_np->registerConvexHullShape(&tmpVertices[0].xyzw[0],strideInBytes,numVertices, scaling); child.m_shapeIndex = childColIndex; childShapes.push_back(child); colIndex = childColIndex; } colIndex= m_data->m_np->registerCompoundShape(&childShapes); } //int shapeId = ci.m_instancingRenderer->registerShape(&cube_vertices[0],numVertices,cube_indices,numIndices); int shapeId = ci.m_instancingRenderer->registerShape(&vertexArray[0].xyzw[0],vertexArray.size(),&indexArray[0],indexArray.size()); b3Vector4 colors[4] = { b3MakeVector4(1,0,0,1), b3MakeVector4(0,1,0,1), b3MakeVector4(0,0,1,1), b3MakeVector4(0,1,1,1), }; int curColor = 0; for (int i=0;i<1;i++)//ci.arraySizeX;i++) { for (int j=0;j<4;j++) { // for (int k=0;k<ci.arraySizeZ;k++) int k=0; { float mass = 1;//j==0? 0.f : 1.f; //b3Vector3 position(i*10*ci.gapX,j*ci.gapY,k*10*ci.gapZ); b3Vector3 position=b3MakeVector3(i*10*ci.gapX,10+j*10*ci.gapY,k*10*ci.gapZ); // b3Quaternion orn(0,0,0,1); b3Quaternion orn(b3MakeVector3(0,0,1),1.8); b3Vector4 color = colors[curColor]; curColor++; curColor&=3; b3Vector4 scaling=b3MakeVector4(1,1,1,1); int id = ci.m_instancingRenderer->registerGraphicsInstance(shapeId,position,orn,color,scaling); int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(mass,position,orn,colIndex,index,false); index++; } } } } }
void RigidBodyDemo::initPhysics(const ConstructionInfo& ci) { m_instancingRenderer = ci.m_instancingRenderer; int x_dim=1; int y_dim=2; int z_dim=1; int aabbCapacity = x_dim*y_dim*z_dim+10; b3Config config; m_bp = new b3DynamicBvhBroadphase(aabbCapacity); m_np = new b3CpuNarrowPhase(config); m_rb = new b3CpuRigidBodyPipeline(m_np,m_bp, config); m_instancingRenderer ->setCameraDistance(100); float target[4]={0,0,0,0}; m_instancingRenderer->setCameraTargetPosition(target); int strideInBytes = 9*sizeof(float); int numVertices = sizeof(cube_vertices)/strideInBytes; int numIndices = sizeof(cube_indices)/sizeof(int); int shapeId = ci.m_instancingRenderer->registerShape(&cube_vertices[0],numVertices,cube_indices,numIndices); { static int curColor=0; b3Vector4 color = colors[curColor]; curColor++; curColor&=3; b3Vector3 position = b3MakeVector3(0,0,0);//((j+1)&1)+i*2.2,1+j*2.,((j+1)&1)+k*2.2); b3Quaternion orn(0,0,0,1); b3Vector4 scaling=b3MakeVector4(100,1,100,1); int id = ci.m_instancingRenderer->registerGraphicsInstance(shapeId,position,orn,color,scaling); float mass=0.f; int collidableIndex = m_np->registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling); int bodyIndex = m_rb->getNumBodies(); int userData=-1; int rbid = m_rb->registerPhysicsInstance(mass, position, orn, collidableIndex, userData); } ci.m_instancingRenderer->writeTransforms(); { b3Vector4 scaling=b3MakeVector4(1,1,1,1); int collidableIndex = m_np->registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling); for (int x=0;x<x_dim;x++) { //for (int y=-y_dim/2;y<y_dim/2;y++) for (int y=0;y<y_dim;y++) { for (int z=0;z<z_dim;z++) { static int curColor=0; b3Vector4 color = colors[curColor]; curColor++; curColor&=3; b3Vector3 position = b3MakeVector3(x*2,2+y*2,z*2); b3Quaternion orn(0,0,0,1); int id = ci.m_instancingRenderer->registerGraphicsInstance(shapeId,position,orn,color,scaling); float mass=1.f; int userData=-1; int bodyIndex = m_rb->getNumBodies(); int rbid = m_rb->registerPhysicsInstance(mass, position, orn, collidableIndex, userData); } } } } printf("num objects = %d\n",m_rb->getNumBodies()); }
void TestJointTorqueSetup::initPhysics() { int upAxis = 1; gJointFeedbackInWorldSpace = true; gJointFeedbackInJointFrame = true; m_guiHelper->setUpAxis(upAxis); btVector4 colors[4] = { btVector4(1,0,0,1), btVector4(0,1,0,1), btVector4(0,1,1,1), btVector4(1,1,0,1), }; int curColor = 0; this->createEmptyDynamicsWorld(); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); m_dynamicsWorld->getDebugDrawer()->setDebugMode( //btIDebugDraw::DBG_DrawConstraints +btIDebugDraw::DBG_DrawWireframe +btIDebugDraw::DBG_DrawContactPoints +btIDebugDraw::DBG_DrawAabb );//+btIDebugDraw::DBG_DrawConstraintLimits); //create a static ground object if (1) { btVector3 groundHalfExtents(1,1,0.2); groundHalfExtents[upAxis]=1.f; btBoxShape* box = new btBoxShape(groundHalfExtents); box->initializePolyhedralFeatures(); m_guiHelper->createCollisionShapeGraphicsObject(box); btTransform start; start.setIdentity(); btVector3 groundOrigin(-0.4f, 3.f, 0.f); groundOrigin[upAxis] -=.5; groundOrigin[2]-=0.6; start.setOrigin(groundOrigin); btQuaternion groundOrn(btVector3(0,1,0),0.25*SIMD_PI); // start.setRotation(groundOrn); btRigidBody* body = createRigidBody(0,start,box); body->setFriction(0); btVector4 color = colors[curColor]; curColor++; curColor&=3; m_guiHelper->createRigidBodyGraphicsObject(body,color); } { bool floating = false; bool damping = false; bool gyro = false; int numLinks = 2; bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals bool canSleep = false; bool selfCollide = false; btVector3 linkHalfExtents(0.05, 0.37, 0.1); btVector3 baseHalfExtents(0.05, 0.37, 0.1); btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f); //mbC->forceMultiDof(); //if !spherical, you can comment this line to check the 1DoF algorithm //init the base btVector3 baseInertiaDiag(0.f, 0.f, 0.f); float baseMass = 1.f; if(baseMass) { //btCollisionShape *shape = new btSphereShape(baseHalfExtents[0]);// btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2])); btCollisionShape *shape = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2])); shape->calculateLocalInertia(baseMass, baseInertiaDiag); delete shape; } bool isMultiDof = true; btMultiBody *pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep, isMultiDof); m_multiBody = pMultiBody; btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f); // baseOriQuat.setEulerZYX(-.25*SIMD_PI,0,-1.75*SIMD_PI); pMultiBody->setBasePos(basePosition); pMultiBody->setWorldToBaseRot(baseOriQuat); btVector3 vel(0, 0, 0); // pMultiBody->setBaseVel(vel); //init the links btVector3 hingeJointAxis(1, 0, 0); //y-axis assumed up btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] * 2.f, 0); //par body's COM to cur body's COM offset btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0); //cur body's COM to cur body's PIV offset btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset ////// btScalar q0 = 0.f * SIMD_PI/ 180.f; btQuaternion quat0(btVector3(0, 1, 0).normalized(), q0); quat0.normalize(); ///// for(int i = 0; i < numLinks; ++i) { float linkMass = 1.f; //if (i==3 || i==2) // linkMass= 1000; btVector3 linkInertiaDiag(0.f, 0.f, 0.f); btCollisionShape* shape = 0; if (i==0) { shape = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));// } else { shape = new btSphereShape(radius); } shape->calculateLocalInertia(linkMass, linkInertiaDiag); delete shape; if(!spherical) { //pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false); if (i==0) { pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false); } else { btVector3 parentComToCurrentCom(0, -radius * 2.f, 0); //par body's COM to cur body's COM offset btVector3 currentPivotToCurrentCom(0, -radius, 0); //cur body's COM to cur body's PIV offset btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset pMultiBody->setupFixed(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, false); } //pMultiBody->setupFixed(i,linkMass,linkInertiaDiag,i-1,btQuaternion(0,0,0,1),parentComToCurrentPivot,currentPivotToCurrentCom,false); } else { //pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false); pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, false); } } pMultiBody->finalizeMultiDof(); //for (int i=pMultiBody->getNumLinks()-1;i>=0;i--)// for (int i=0;i<pMultiBody->getNumLinks();i++) { btMultiBodyJointFeedback* fb = new btMultiBodyJointFeedback(); pMultiBody->getLink(i).m_jointFeedback = fb; m_jointFeedbacks.push_back(fb); //break; } btMultiBodyDynamicsWorld* world = m_dynamicsWorld; /// world->addMultiBody(pMultiBody); btMultiBody* mbC = pMultiBody; mbC->setCanSleep(canSleep); mbC->setHasSelfCollision(selfCollide); mbC->setUseGyroTerm(gyro); // if(!damping) { mbC->setLinearDamping(0.f); mbC->setAngularDamping(0.f); }else { mbC->setLinearDamping(0.1f); mbC->setAngularDamping(0.9f); } // m_dynamicsWorld->setGravity(btVector3(0,0,-10)); ////////////////////////////////////////////// if(0)//numLinks > 0) { btScalar q0 = 45.f * SIMD_PI/ 180.f; if(!spherical) if(mbC->isMultiDof()) mbC->setJointPosMultiDof(0, &q0); else mbC->setJointPos(0, q0); else { btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0); quat0.normalize(); mbC->setJointPosMultiDof(0, quat0); } } /// btAlignedObjectArray<btQuaternion> world_to_local; world_to_local.resize(pMultiBody->getNumLinks() + 1); btAlignedObjectArray<btVector3> local_origin; local_origin.resize(pMultiBody->getNumLinks() + 1); world_to_local[0] = pMultiBody->getWorldToBaseRot(); local_origin[0] = pMultiBody->getBasePos(); // double friction = 1; { // float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1}; // btScalar quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()}; if (1) { btCollisionShape* shape = new btBoxShape(btVector3(baseHalfExtents[0],baseHalfExtents[1],baseHalfExtents[2]));//new btSphereShape(baseHalfExtents[0]); m_guiHelper->createCollisionShapeGraphicsObject(shape); btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(pMultiBody, -1); col->setCollisionShape(shape); btTransform tr; tr.setIdentity(); //if we don't set the initial pose of the btCollisionObject, the simulator will do this //when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider tr.setOrigin(local_origin[0]); btQuaternion orn(btVector3(0,0,1),0.25*3.1415926538); tr.setRotation(orn); col->setWorldTransform(tr); bool isDynamic = (baseMass > 0 && floating); short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter); short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); world->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);//, 2,1+2); btVector3 color(0.0,0.0,0.5); m_guiHelper->createCollisionObjectGraphicsObject(col,color); // col->setFriction(friction); pMultiBody->setBaseCollider(col); } } for (int i=0; i < pMultiBody->getNumLinks(); ++i) { const int parent = pMultiBody->getParent(i); world_to_local[i+1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent+1]; local_origin[i+1] = local_origin[parent+1] + (quatRotate(world_to_local[i+1].inverse() , pMultiBody->getRVector(i))); } for (int i=0; i < pMultiBody->getNumLinks(); ++i) { btVector3 posr = local_origin[i+1]; // float pos[4]={posr.x(),posr.y(),posr.z(),1}; btScalar quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()}; btCollisionShape* shape =0; if (i==0) { shape = new btBoxShape(btVector3(linkHalfExtents[0],linkHalfExtents[1],linkHalfExtents[2]));//btSphereShape(linkHalfExtents[0]); } else { shape = new btSphereShape(radius); } m_guiHelper->createCollisionShapeGraphicsObject(shape); btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i); col->setCollisionShape(shape); btTransform tr; tr.setIdentity(); tr.setOrigin(posr); tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3])); col->setWorldTransform(tr); // col->setFriction(friction); bool isDynamic = 1;//(linkMass > 0); short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter); short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); //if (i==0||i>numLinks-2) { world->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);//,2,1+2); btVector4 color = colors[curColor]; curColor++; curColor&=3; m_guiHelper->createCollisionObjectGraphicsObject(col,color); pMultiBody->getLink(i).m_collider=col; } } } btSerializer* s = new btDefaultSerializer; m_dynamicsWorld->serialize(s); b3ResourcePath p; char resourcePath[1024]; if (p.findResourcePath("multibody.bullet",resourcePath,1024)) { FILE* f = fopen(resourcePath,"wb"); fwrite(s->getBufferPointer(),s->getCurrentBufferSize(),1,f); fclose(f); } }
void ConcaveScene::setupScene(const ConstructionInfo& ci) { if (1) { //char* fileName = "slopedPlane100.obj"; //char* fileName = "plane100.obj"; // char* fileName = "plane100.obj"; //char* fileName = "teddy.obj";//"plane.obj"; // char* fileName = "sponza_closed.obj";//"plane.obj"; //char* fileName = "leoTest1.obj"; char* fileName = "samurai_monastry.obj"; // char* fileName = "teddy2_VHACD_CHs.obj"; b3Vector3 shift1=b3MakeVector3(0,0,0);//0,230,80);//150,-100,-120); b3Vector4 scaling=b3MakeVector4(10,10,10,1); // createConcaveMesh(ci,"plane100.obj",shift1,scaling); //createConcaveMesh(ci,"plane100.obj",shift,scaling); // b3Vector3 shift2(0,0,0);//0,230,80);//150,-100,-120); // createConcaveMesh(ci,"teddy.obj",shift2,scaling); // b3Vector3 shift3(130,-150,-75);//0,230,80);//150,-100,-120); // createConcaveMesh(ci,"leoTest1.obj",shift3,scaling); createConcaveMesh(ci,fileName,shift1,scaling); } else { int strideInBytes = 9*sizeof(float); int numVertices = sizeof(cube_vertices)/strideInBytes; int numIndices = sizeof(cube_indices)/sizeof(int); int shapeId = ci.m_instancingRenderer->registerShape(&cube_vertices[0],numVertices,cube_indices,numIndices); int group=1; int mask=1; int index=0; { b3Vector4 scaling=b3MakeVector4(400,1.,400,1); int colIndex = m_data->m_np->registerConvexHullShape(&cube_vertices[0],strideInBytes,numVertices, scaling); b3Vector3 position=b3MakeVector3(0,-2,0); b3Quaternion orn(0,0,0,1); b3Vector4 color=b3MakeVector4(0,0,1,1); int id = ci.m_instancingRenderer->registerGraphicsInstance(shapeId,position,orn,color,scaling); int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(0.f,position,orn,colIndex,index,false); } } createDynamicObjects(ci); m_data->m_rigidBodyPipeline->writeAllInstancesToGpu(); float camPos[4]={0,0,0,0};//65.5,4.5,65.5,0}; //float camPos[4]={1,12.5,1.5,0}; m_instancingRenderer->setCameraPitch(45); m_instancingRenderer->setCameraTargetPosition(camPos); m_instancingRenderer->setCameraDistance(355); char msg[1024]; int numInstances = m_data->m_rigidBodyPipeline->getNumBodies(); sprintf(msg,"Num objects = %d",numInstances); if (ci.m_gui) ci.m_gui->setStatusBarMessage(msg,true); }
int GpuConvexScene::createDynamicsObjects2(const ConstructionInfo& ci, const float* vertices, int numVertices, const int* indices, int numIndices) { int strideInBytes = 9*sizeof(float); int shapeId = ci.m_instancingRenderer->registerShape(&vertices[0],numVertices,indices,numIndices); int group=1; int mask=1; int index=0; { int curColor = 0; float scaling[4] = {1,1,1,1}; int prevBody = -1; int insta = 0; b3ConvexUtility* utilPtr = new b3ConvexUtility(); { b3AlignedObjectArray<b3Vector3> verts; unsigned char* vts = (unsigned char*) vertices; for (int i=0;i<numVertices;i++) { float* vertex = (float*) &vts[i*strideInBytes]; verts.push_back(b3MakeVector3(vertex[0]*scaling[0],vertex[1]*scaling[1],vertex[2]*scaling[2])); } bool merge = true; if (numVertices) { utilPtr->initializePolyhedralFeatures(&verts[0],verts.size(),merge); } } int colIndex=-1; if (ci.m_useInstancedCollisionShapes) colIndex = m_data->m_np->registerConvexHullShape(utilPtr); //int colIndex = m_data->m_np->registerSphereShape(1); for (int i=0;i<ci.arraySizeX;i++) { //printf("%d of %d\n", i, ci.arraySizeX); for (int j=0;j<ci.arraySizeY;j++) { for (int k=0;k<ci.arraySizeZ;k++) { //int colIndex = m_data->m_np->registerConvexHullShape(&vertices[0],strideInBytes,numVertices, scaling); if (!ci.m_useInstancedCollisionShapes) colIndex = m_data->m_np->registerConvexHullShape(utilPtr); float mass = 1.f; if (j==0)//ci.arraySizeY-1) { //mass=0.f; } b3Vector3 position = b3MakeVector3(((j+1)&1)+i*2.2,1+j*2.,((j+1)&1)+k*2.2); //b3Vector3 position(i*2.2,10+j*1.9,k*2.2); //b3Vector3 position=b3MakeVector3(1,0.9,1); b3Quaternion orn(0,0,0,1); b3Vector4 color = colors[curColor]; curColor++; curColor&=3; b3Vector4 scalin=b3MakeVector4(1,1,1,1); int id = ci.m_instancingRenderer->registerGraphicsInstance(shapeId,position,orn,color,scaling); int pid = m_data->m_rigidBodyPipeline->registerPhysicsInstance(mass,position,orn,colIndex,index,false); if (prevBody>=0) { //b3Point2PointConstraint* p2p = new b3Point2PointConstraint(pid,prevBody,b3Vector3(0,-1.1,0),b3Vector3(0,1.1,0)); // m_data->m_rigidBodyPipeline->addConstraint(p2p);//,false); } prevBody = pid; index++; } } } delete utilPtr; } return index; }
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(); btQuaternion orn2 = m_collisionObjects[i]->getWorldTransform().getRotation(); b3Quaternion orn(orn2[0],orn2[1],orn2[2],orn2[3]); 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; }
Tutorial(GUIHelperInterface* guiHelper, int tutorialIndex) :m_app(guiHelper->getAppInterface()), m_guiHelper(guiHelper), m_tutorialIndex(tutorialIndex), m_stage(0), m_counter(0), m_timeSeriesCanvas0(0), m_timeSeriesCanvas1(0) { int numBodies = 1; m_app->setUpAxis(1); m_app->m_renderer->enableBlend(true); switch (m_tutorialIndex) { case TUT_VELOCITY: { numBodies=10; m_timeSeriesCanvas0 = new TimeSeriesCanvas(m_app->m_2dCanvasInterface,512,256,"Constant Velocity"); m_timeSeriesCanvas0 ->setupTimeSeries(2,60, 0); m_timeSeriesCanvas0->addDataSource("X position (m)", 255,0,0); m_timeSeriesCanvas0->addDataSource("X velocity (m/s)", 0,0,255); m_timeSeriesCanvas0->addDataSource("dX/dt (m/s)", 0,0,0); break; } case TUT_ACCELERATION: { numBodies=10; m_timeSeriesCanvas1 = new TimeSeriesCanvas(m_app->m_2dCanvasInterface,256,512,"Constant Acceleration"); m_timeSeriesCanvas1 ->setupTimeSeries(50,60, 0); m_timeSeriesCanvas1->addDataSource("Y position (m)", 255,0,0); m_timeSeriesCanvas1->addDataSource("Y velocity (m/s)", 0,0,255); m_timeSeriesCanvas1->addDataSource("dY/dt (m/s)", 0,0,0); break; } case TUT_COLLISION: { numBodies=2; m_timeSeriesCanvas1 = new TimeSeriesCanvas(m_app->m_2dCanvasInterface,512,200,"Distance"); m_timeSeriesCanvas1 ->setupTimeSeries(1.5,60, 0); m_timeSeriesCanvas1->addDataSource("distance", 255,0,0); break; } case TUT_SOLVE_CONTACT_CONSTRAINT: { numBodies=2; m_timeSeriesCanvas1 = new TimeSeriesCanvas(m_app->m_2dCanvasInterface,512,200,"Collision Impulse"); m_timeSeriesCanvas1 ->setupTimeSeries(1.5,60, 0); m_timeSeriesCanvas1->addDataSource("Distance", 0,0,255); m_timeSeriesCanvas1->addDataSource("Impulse magnutide", 255,0,0); { SliderParams slider("Restitution",&gRestitution); slider.m_minVal=0; slider.m_maxVal=1; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { SliderParams slider("Mass A",&gMassA); slider.m_minVal=0; slider.m_maxVal=100; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { SliderParams slider("Mass B",&gMassB); slider.m_minVal=0; slider.m_maxVal=100; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } break; } default: { m_timeSeriesCanvas0 = new TimeSeriesCanvas(m_app->m_2dCanvasInterface,512,256,"Unknown"); m_timeSeriesCanvas0 ->setupTimeSeries(1,60, 0); } }; if (m_tutorialIndex==TUT_VELOCITY) { int boxId = m_app->registerCubeShape(100,1,100); b3Vector3 pos = b3MakeVector3(0,-3.5,0); b3Quaternion orn(0,0,0,1); b3Vector4 color = b3MakeVector4(1,1,1,1); b3Vector3 scaling = b3MakeVector3(1,1,1); m_app->m_renderer->registerGraphicsInstance(boxId,pos,orn,color,scaling); } for (int i=0;i<numBodies;i++) { m_bodies.push_back(new LWRigidBody()); } for (int i=0;i<m_bodies.size();i++) { m_bodies[i]->m_worldPose.m_position.setValue((i/4)*5,3,(i&3)*5); } { int textureIndex = -1; if (1) { int width,height,n; const char* filename = "data/cube.png"; const unsigned char* image=0; const char* prefix[]={"./","../","../../","../../../","../../../../"}; int numprefix = sizeof(prefix)/sizeof(const char*); for (int i=0;!image && i<numprefix;i++) { char relativeFileName[1024]; sprintf(relativeFileName,"%s%s",prefix[i],filename); image = stbi_load(relativeFileName, &width, &height, &n, 0); } b3Assert(image); if (image) { textureIndex = m_app->m_renderer->registerTexture(image,width,height); } } // int boxId = m_app->registerCubeShape(1,1,1,textureIndex); int boxId = m_app->registerGraphicsUnitSphereShape(SPHERE_LOD_HIGH, textureIndex); b3Vector4 color = b3MakeVector4(1,1,1,0.8); b3Vector3 scaling = b3MakeVector3(SPHERE_RADIUS,SPHERE_RADIUS,SPHERE_RADIUS); for (int i=0;i<m_bodies.size();i++) { m_bodies[i]->m_collisionShape.m_sphere.m_radius = SPHERE_RADIUS; m_bodies[i]->m_collisionShape.m_type = LW_SPHERE_TYPE; m_bodies[i]->m_graphicsIndex = m_app->m_renderer->registerGraphicsInstance(boxId,m_bodies[i]->m_worldPose.m_position, m_bodies[i]->m_worldPose.m_orientation,color,scaling); m_app->m_renderer->writeSingleInstanceTransformToCPU(m_bodies[i]->m_worldPose.m_position, m_bodies[i]->m_worldPose.m_orientation, m_bodies[i]->m_graphicsIndex); } } if (m_tutorialIndex == TUT_SOLVE_CONTACT_CONSTRAINT) { m_bodies[0]->m_invMass = gMassA? 1./gMassA : 0; m_bodies[0]->m_collisionShape.m_sphere.computeLocalInertia(gMassA,m_bodies[0]->m_localInertia); m_bodies[1]->m_invMass =gMassB? 1./gMassB : 0; m_bodies[1]->m_collisionShape.m_sphere.computeLocalInertia(gMassB,m_bodies[1]->m_localInertia); if (gMassA) m_bodies[0]->m_linearVelocity.setValue(0,0,1); if (gMassB) m_bodies[1]->m_linearVelocity.setValue(0,0,-1); } m_app->m_renderer->writeTransforms(); }
void AllConstraintDemo::initPhysics() { m_guiHelper->setUpAxis(1); m_Time = 0; setupEmptyDynamicsWorld(); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); //btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(40.),btScalar(50.))); btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),40); m_collisionShapes.push_back(groundShape); btTransform groundTransform; groundTransform.setIdentity(); groundTransform.setOrigin(btVector3(0,-56,0)); btRigidBody* groundBody; groundBody= createRigidBody(0, groundTransform, groundShape); btCollisionShape* shape = new btBoxShape(btVector3(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS)); m_collisionShapes.push_back(shape); btTransform trans; trans.setIdentity(); trans.setOrigin(btVector3(0,20,0)); float mass = 1.f; #if ENABLE_ALL_DEMOS ///gear constraint demo #define THETA SIMD_PI/4.f #define L_1 (2 - tan(THETA)) #define L_2 (1 / cos(THETA)) #define RATIO L_2 / L_1 btRigidBody* bodyA=0; btRigidBody* bodyB=0; { btCollisionShape* cylA = new btCylinderShape(btVector3(0.2,0.25,0.2)); btCollisionShape* cylB = new btCylinderShape(btVector3(L_1,0.025,L_1)); btCompoundShape* cyl0 = new btCompoundShape(); cyl0->addChildShape(btTransform::getIdentity(),cylA); cyl0->addChildShape(btTransform::getIdentity(),cylB); btScalar mass = 6.28; btVector3 localInertia; cyl0->calculateLocalInertia(mass,localInertia); btRigidBody::btRigidBodyConstructionInfo ci(mass,0,cyl0,localInertia); ci.m_startWorldTransform.setOrigin(btVector3(-8,1,-8)); btRigidBody* body = new btRigidBody(ci);//1,0,cyl0,localInertia); m_dynamicsWorld->addRigidBody(body); body->setLinearFactor(btVector3(0,0,0)); body->setAngularFactor(btVector3(0,1,0)); bodyA = body; } { btCollisionShape* cylA = new btCylinderShape(btVector3(0.2,0.26,0.2)); btCollisionShape* cylB = new btCylinderShape(btVector3(L_2,0.025,L_2)); btCompoundShape* cyl0 = new btCompoundShape(); cyl0->addChildShape(btTransform::getIdentity(),cylA); cyl0->addChildShape(btTransform::getIdentity(),cylB); btScalar mass = 6.28; btVector3 localInertia; cyl0->calculateLocalInertia(mass,localInertia); btRigidBody::btRigidBodyConstructionInfo ci(mass,0,cyl0,localInertia); ci.m_startWorldTransform.setOrigin(btVector3(-10,2,-8)); btQuaternion orn(btVector3(0,0,1),-THETA); ci.m_startWorldTransform.setRotation(orn); btRigidBody* body = new btRigidBody(ci);//1,0,cyl0,localInertia); body->setLinearFactor(btVector3(0,0,0)); btHingeConstraint* hinge = new btHingeConstraint(*body,btVector3(0,0,0),btVector3(0,1,0),true); m_dynamicsWorld->addConstraint(hinge); bodyB= body; body->setAngularVelocity(btVector3(0,3,0)); m_dynamicsWorld->addRigidBody(body); } btVector3 axisA(0,1,0); btVector3 axisB(0,1,0); btQuaternion orn(btVector3(0,0,1),-THETA); btMatrix3x3 mat(orn); axisB = mat.getRow(1); btGearConstraint* gear = new btGearConstraint(*bodyA,*bodyB, axisA,axisB,RATIO); m_dynamicsWorld->addConstraint(gear,true); #endif #if ENABLE_ALL_DEMOS //point to point constraint with a breaking threshold { trans.setIdentity(); trans.setOrigin(btVector3(1,30,-5)); createRigidBody( mass,trans,shape); trans.setOrigin(btVector3(0,0,-5)); btRigidBody* body0 = createRigidBody( mass,trans,shape); trans.setOrigin(btVector3(2*CUBE_HALF_EXTENTS,20,0)); mass = 1.f; // btRigidBody* body1 = 0;//createRigidBody( mass,trans,shape); btVector3 pivotInA(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,0); btTypedConstraint* p2p = new btPoint2PointConstraint(*body0,pivotInA); m_dynamicsWorld->addConstraint(p2p); p2p ->setBreakingImpulseThreshold(10.2); p2p->setDbgDrawSize(btScalar(5.f)); } #endif #if ENABLE_ALL_DEMOS //point to point constraint (ball socket) { btRigidBody* body0 = createRigidBody( mass,trans,shape); trans.setOrigin(btVector3(2*CUBE_HALF_EXTENTS,20,0)); mass = 1.f; // btRigidBody* body1 = 0;//createRigidBody( mass,trans,shape); // btRigidBody* body1 = createRigidBody( 0.0,trans,0); //body1->setActivationState(DISABLE_DEACTIVATION); //body1->setDamping(0.3,0.3); btVector3 pivotInA(CUBE_HALF_EXTENTS,-CUBE_HALF_EXTENTS,-CUBE_HALF_EXTENTS); btVector3 axisInA(0,0,1); // btVector3 pivotInB = body1 ? body1->getCenterOfMassTransform().inverse()(body0->getCenterOfMassTransform()(pivotInA)) : pivotInA; // btVector3 axisInB = body1? // (body1->getCenterOfMassTransform().getBasis().inverse()*(body1->getCenterOfMassTransform().getBasis() * axisInA)) : body0->getCenterOfMassTransform().getBasis() * axisInA; #define P2P #ifdef P2P btTypedConstraint* p2p = new btPoint2PointConstraint(*body0,pivotInA); //btTypedConstraint* p2p = new btPoint2PointConstraint(*body0,*body1,pivotInA,pivotInB); //btTypedConstraint* hinge = new btHingeConstraint(*body0,*body1,pivotInA,pivotInB,axisInA,axisInB); m_dynamicsWorld->addConstraint(p2p); p2p->setDbgDrawSize(btScalar(5.f)); #else btHingeConstraint* hinge = new btHingeConstraint(*body0,pivotInA,axisInA); //use zero targetVelocity and a small maxMotorImpulse to simulate joint friction //float targetVelocity = 0.f; //float maxMotorImpulse = 0.01; float targetVelocity = 1.f; float maxMotorImpulse = 1.0f; hinge->enableAngularMotor(true,targetVelocity,maxMotorImpulse); m_dynamicsWorld->addConstraint(hinge); hinge->setDbgDrawSize(btScalar(5.f)); #endif //P2P } #endif #if ENABLE_ALL_DEMOS { btTransform trans; trans.setIdentity(); btVector3 worldPos(-20,0,30); trans.setOrigin(worldPos); btTransform frameInA, frameInB; frameInA = btTransform::getIdentity(); frameInB = btTransform::getIdentity(); btRigidBody* pRbA1 = createRigidBody(mass, trans, shape); // btRigidBody* pRbA1 = createRigidBody(0.f, trans, shape); pRbA1->setActivationState(DISABLE_DEACTIVATION); // add dynamic rigid body B1 worldPos.setValue(-30,0,30); trans.setOrigin(worldPos); btRigidBody* pRbB1 = createRigidBody(mass, trans, shape); // btRigidBody* pRbB1 = createRigidBody(0.f, trans, shape); pRbB1->setActivationState(DISABLE_DEACTIVATION); // create slider constraint between A1 and B1 and add it to world btSliderConstraint* spSlider1 = new btSliderConstraint(*pRbA1, *pRbB1, frameInA, frameInB, true); // spSlider1 = new btSliderConstraint(*pRbA1, *pRbB1, frameInA, frameInB, false); spSlider1->setLowerLinLimit(-15.0F); spSlider1->setUpperLinLimit(-5.0F); // spSlider1->setLowerLinLimit(5.0F); // spSlider1->setUpperLinLimit(15.0F); // spSlider1->setLowerLinLimit(-10.0F); // spSlider1->setUpperLinLimit(-10.0F); spSlider1->setLowerAngLimit(-SIMD_PI / 3.0F); spSlider1->setUpperAngLimit( SIMD_PI / 3.0F); m_dynamicsWorld->addConstraint(spSlider1, true); spSlider1->setDbgDrawSize(btScalar(5.f)); } #endif #if ENABLE_ALL_DEMOS //create a slider, using the generic D6 constraint { mass = 1.f; btVector3 sliderWorldPos(0,10,0); btVector3 sliderAxis(1,0,0); btScalar angle=0.f;//SIMD_RADS_PER_DEG * 10.f; btMatrix3x3 sliderOrientation(btQuaternion(sliderAxis ,angle)); trans.setIdentity(); trans.setOrigin(sliderWorldPos); //trans.setBasis(sliderOrientation); sliderTransform = trans; d6body0 = createRigidBody( mass,trans,shape); d6body0->setActivationState(DISABLE_DEACTIVATION); btRigidBody* fixedBody1 = createRigidBody(0,trans,0); m_dynamicsWorld->addRigidBody(fixedBody1); btTransform frameInA, frameInB; frameInA = btTransform::getIdentity(); frameInB = btTransform::getIdentity(); frameInA.setOrigin(btVector3(0., 5., 0.)); frameInB.setOrigin(btVector3(0., 5., 0.)); // bool useLinearReferenceFrameA = false;//use fixed frame B for linear llimits bool useLinearReferenceFrameA = true;//use fixed frame A for linear llimits spSlider6Dof = new btGeneric6DofConstraint(*fixedBody1, *d6body0,frameInA,frameInB,useLinearReferenceFrameA); spSlider6Dof->setLinearLowerLimit(lowerSliderLimit); spSlider6Dof->setLinearUpperLimit(hiSliderLimit); //range should be small, otherwise singularities will 'explode' the constraint // spSlider6Dof->setAngularLowerLimit(btVector3(-1.5,0,0)); // spSlider6Dof->setAngularUpperLimit(btVector3(1.5,0,0)); // spSlider6Dof->setAngularLowerLimit(btVector3(0,0,0)); // spSlider6Dof->setAngularUpperLimit(btVector3(0,0,0)); spSlider6Dof->setAngularLowerLimit(btVector3(-SIMD_PI,0,0)); spSlider6Dof->setAngularUpperLimit(btVector3(1.5,0,0)); spSlider6Dof->getTranslationalLimitMotor()->m_enableMotor[0] = true; spSlider6Dof->getTranslationalLimitMotor()->m_targetVelocity[0] = -5.0f; spSlider6Dof->getTranslationalLimitMotor()->m_maxMotorForce[0] = 6.0f; m_dynamicsWorld->addConstraint(spSlider6Dof); spSlider6Dof->setDbgDrawSize(btScalar(5.f)); } #endif #if ENABLE_ALL_DEMOS { // create a door using hinge constraint attached to the world btCollisionShape* pDoorShape = new btBoxShape(btVector3(2.0f, 5.0f, 0.2f)); m_collisionShapes.push_back(pDoorShape); btTransform doorTrans; doorTrans.setIdentity(); doorTrans.setOrigin(btVector3(-5.0f, -2.0f, 0.0f)); btRigidBody* pDoorBody = createRigidBody( 1.0, doorTrans, pDoorShape); pDoorBody->setActivationState(DISABLE_DEACTIVATION); const btVector3 btPivotA(10.f + 2.1f, -2.0f, 0.0f ); // right next to the door slightly outside btVector3 btAxisA( 0.0f, 1.0f, 0.0f ); // pointing upwards, aka Y-axis spDoorHinge = new btHingeConstraint( *pDoorBody, btPivotA, btAxisA ); // spDoorHinge->setLimit( 0.0f, SIMD_PI_2 ); // test problem values // spDoorHinge->setLimit( -SIMD_PI, SIMD_PI*0.8f); // spDoorHinge->setLimit( 1.f, -1.f); // spDoorHinge->setLimit( -SIMD_PI*0.8f, SIMD_PI); // spDoorHinge->setLimit( -SIMD_PI*0.8f, SIMD_PI, 0.9f, 0.3f, 0.0f); // spDoorHinge->setLimit( -SIMD_PI*0.8f, SIMD_PI, 0.9f, 0.01f, 0.0f); // "sticky limits" spDoorHinge->setLimit( -SIMD_PI * 0.25f, SIMD_PI * 0.25f ); // spDoorHinge->setLimit( 0.0f, 0.0f ); m_dynamicsWorld->addConstraint(spDoorHinge); spDoorHinge->setDbgDrawSize(btScalar(5.f)); //doorTrans.setOrigin(btVector3(-5.0f, 2.0f, 0.0f)); //btRigidBody* pDropBody = createRigidBody( 10.0, doorTrans, shape); } #endif #if ENABLE_ALL_DEMOS { // create a generic 6DOF constraint btTransform tr; tr.setIdentity(); tr.setOrigin(btVector3(btScalar(10.), btScalar(6.), btScalar(0.))); tr.getBasis().setEulerZYX(0,0,0); // btRigidBody* pBodyA = createRigidBody( mass, tr, shape); btRigidBody* pBodyA = createRigidBody( 0.0, tr, shape); // btRigidBody* pBodyA = createRigidBody( 0.0, tr, 0); pBodyA->setActivationState(DISABLE_DEACTIVATION); tr.setIdentity(); tr.setOrigin(btVector3(btScalar(0.), btScalar(6.), btScalar(0.))); tr.getBasis().setEulerZYX(0,0,0); btRigidBody* pBodyB = createRigidBody(mass, tr, shape); // btRigidBody* pBodyB = createRigidBody(0.f, tr, shape); pBodyB->setActivationState(DISABLE_DEACTIVATION); btTransform frameInA, frameInB; frameInA = btTransform::getIdentity(); frameInA.setOrigin(btVector3(btScalar(-5.), btScalar(0.), btScalar(0.))); frameInB = btTransform::getIdentity(); frameInB.setOrigin(btVector3(btScalar(5.), btScalar(0.), btScalar(0.))); btGeneric6DofConstraint* pGen6DOF = new btGeneric6DofConstraint(*pBodyA, *pBodyB, frameInA, frameInB, true); // btGeneric6DofConstraint* pGen6DOF = new btGeneric6DofConstraint(*pBodyA, *pBodyB, frameInA, frameInB, false); pGen6DOF->setLinearLowerLimit(btVector3(-10., -2., -1.)); pGen6DOF->setLinearUpperLimit(btVector3(10., 2., 1.)); // pGen6DOF->setLinearLowerLimit(btVector3(-10., 0., 0.)); // pGen6DOF->setLinearUpperLimit(btVector3(10., 0., 0.)); // pGen6DOF->setLinearLowerLimit(btVector3(0., 0., 0.)); // pGen6DOF->setLinearUpperLimit(btVector3(0., 0., 0.)); // pGen6DOF->getTranslationalLimitMotor()->m_enableMotor[0] = true; // pGen6DOF->getTranslationalLimitMotor()->m_targetVelocity[0] = 5.0f; // pGen6DOF->getTranslationalLimitMotor()->m_maxMotorForce[0] = 6.0f; // pGen6DOF->setAngularLowerLimit(btVector3(0., SIMD_HALF_PI*0.9, 0.)); // pGen6DOF->setAngularUpperLimit(btVector3(0., -SIMD_HALF_PI*0.9, 0.)); // pGen6DOF->setAngularLowerLimit(btVector3(0., 0., -SIMD_HALF_PI)); // pGen6DOF->setAngularUpperLimit(btVector3(0., 0., SIMD_HALF_PI)); pGen6DOF->setAngularLowerLimit(btVector3(-SIMD_HALF_PI * 0.5f, -0.75, -SIMD_HALF_PI * 0.8f)); pGen6DOF->setAngularUpperLimit(btVector3(SIMD_HALF_PI * 0.5f, 0.75, SIMD_HALF_PI * 0.8f)); // pGen6DOF->setAngularLowerLimit(btVector3(0.f, -0.75, SIMD_HALF_PI * 0.8f)); // pGen6DOF->setAngularUpperLimit(btVector3(0.f, 0.75, -SIMD_HALF_PI * 0.8f)); // pGen6DOF->setAngularLowerLimit(btVector3(0.f, -SIMD_HALF_PI * 0.8f, SIMD_HALF_PI * 1.98f)); // pGen6DOF->setAngularUpperLimit(btVector3(0.f, SIMD_HALF_PI * 0.8f, -SIMD_HALF_PI * 1.98f)); // pGen6DOF->setAngularLowerLimit(btVector3(-0.75,-0.5, -0.5)); // pGen6DOF->setAngularUpperLimit(btVector3(0.75,0.5, 0.5)); // pGen6DOF->setAngularLowerLimit(btVector3(-0.75,0., 0.)); // pGen6DOF->setAngularUpperLimit(btVector3(0.75,0., 0.)); // pGen6DOF->setAngularLowerLimit(btVector3(0., -0.7,0.)); // pGen6DOF->setAngularUpperLimit(btVector3(0., 0.7, 0.)); // pGen6DOF->setAngularLowerLimit(btVector3(-1., 0.,0.)); // pGen6DOF->setAngularUpperLimit(btVector3(1., 0., 0.)); m_dynamicsWorld->addConstraint(pGen6DOF, true); pGen6DOF->setDbgDrawSize(btScalar(5.f)); } #endif #if ENABLE_ALL_DEMOS { // create a ConeTwist constraint btTransform tr; tr.setIdentity(); tr.setOrigin(btVector3(btScalar(-10.), btScalar(5.), btScalar(0.))); tr.getBasis().setEulerZYX(0,0,0); btRigidBody* pBodyA = createRigidBody( 1.0, tr, shape); // btRigidBody* pBodyA = createRigidBody( 0.0, tr, shape); pBodyA->setActivationState(DISABLE_DEACTIVATION); tr.setIdentity(); tr.setOrigin(btVector3(btScalar(-10.), btScalar(-5.), btScalar(0.))); tr.getBasis().setEulerZYX(0,0,0); btRigidBody* pBodyB = createRigidBody(0.0, tr, shape); // btRigidBody* pBodyB = createRigidBody(1.0, tr, shape); btTransform frameInA, frameInB; frameInA = btTransform::getIdentity(); frameInA.getBasis().setEulerZYX(0, 0, SIMD_PI_2); frameInA.setOrigin(btVector3(btScalar(0.), btScalar(-5.), btScalar(0.))); frameInB = btTransform::getIdentity(); frameInB.getBasis().setEulerZYX(0,0, SIMD_PI_2); frameInB.setOrigin(btVector3(btScalar(0.), btScalar(5.), btScalar(0.))); m_ctc = new btConeTwistConstraint(*pBodyA, *pBodyB, frameInA, frameInB); // m_ctc->setLimit(btScalar(SIMD_PI_4), btScalar(SIMD_PI_4), btScalar(SIMD_PI) * 0.8f); // m_ctc->setLimit(btScalar(SIMD_PI_4*0.6f), btScalar(SIMD_PI_4), btScalar(SIMD_PI) * 0.8f, 1.0f); // soft limit == hard limit m_ctc->setLimit(btScalar(SIMD_PI_4*0.6f), btScalar(SIMD_PI_4), btScalar(SIMD_PI) * 0.8f, 0.5f); m_dynamicsWorld->addConstraint(m_ctc, true); m_ctc->setDbgDrawSize(btScalar(5.f)); // s_bTestConeTwistMotor = true; // use only with old solver for now s_bTestConeTwistMotor = false; } #endif #if ENABLE_ALL_DEMOS { // Hinge connected to the world, with motor (to hinge motor with new and old constraint solver) btTransform tr; tr.setIdentity(); tr.setOrigin(btVector3(btScalar(0.), btScalar(0.), btScalar(0.))); btRigidBody* pBody = createRigidBody( 1.0, tr, shape); pBody->setActivationState(DISABLE_DEACTIVATION); const btVector3 btPivotA( 10.0f, 0.0f, 0.0f ); btVector3 btAxisA( 0.0f, 0.0f, 1.0f ); btHingeConstraint* pHinge = new btHingeConstraint( *pBody, btPivotA, btAxisA ); // pHinge->enableAngularMotor(true, -1.0, 0.165); // use for the old solver pHinge->enableAngularMotor(true, -1.0f, 1.65f); // use for the new SIMD solver m_dynamicsWorld->addConstraint(pHinge); pHinge->setDbgDrawSize(btScalar(5.f)); } #endif #if ENABLE_ALL_DEMOS { // create a universal joint using generic 6DOF constraint // create two rigid bodies // static bodyA (parent) on top: btTransform tr; tr.setIdentity(); tr.setOrigin(btVector3(btScalar(20.), btScalar(4.), btScalar(0.))); btRigidBody* pBodyA = createRigidBody( 0.0, tr, shape); pBodyA->setActivationState(DISABLE_DEACTIVATION); // dynamic bodyB (child) below it : tr.setIdentity(); tr.setOrigin(btVector3(btScalar(20.), btScalar(0.), btScalar(0.))); btRigidBody* pBodyB = createRigidBody(1.0, tr, shape); pBodyB->setActivationState(DISABLE_DEACTIVATION); // add some (arbitrary) data to build constraint frames btVector3 parentAxis(1.f, 0.f, 0.f); btVector3 childAxis(0.f, 0.f, 1.f); btVector3 anchor(20.f, 2.f, 0.f); btUniversalConstraint* pUniv = new btUniversalConstraint(*pBodyA, *pBodyB, anchor, parentAxis, childAxis); pUniv->setLowerLimit(-SIMD_HALF_PI * 0.5f, -SIMD_HALF_PI * 0.5f); pUniv->setUpperLimit(SIMD_HALF_PI * 0.5f, SIMD_HALF_PI * 0.5f); // add constraint to world m_dynamicsWorld->addConstraint(pUniv, true); // draw constraint frames and limits for debugging pUniv->setDbgDrawSize(btScalar(5.f)); } #endif #if ENABLE_ALL_DEMOS { // create a generic 6DOF constraint with springs btTransform tr; tr.setIdentity(); tr.setOrigin(btVector3(btScalar(-20.), btScalar(16.), btScalar(0.))); tr.getBasis().setEulerZYX(0,0,0); btRigidBody* pBodyA = createRigidBody( 0.0, tr, shape); pBodyA->setActivationState(DISABLE_DEACTIVATION); tr.setIdentity(); tr.setOrigin(btVector3(btScalar(-10.), btScalar(16.), btScalar(0.))); tr.getBasis().setEulerZYX(0,0,0); btRigidBody* pBodyB = createRigidBody(1.0, tr, shape); pBodyB->setActivationState(DISABLE_DEACTIVATION); btTransform frameInA, frameInB; frameInA = btTransform::getIdentity(); frameInA.setOrigin(btVector3(btScalar(10.), btScalar(0.), btScalar(0.))); frameInB = btTransform::getIdentity(); frameInB.setOrigin(btVector3(btScalar(0.), btScalar(0.), btScalar(0.))); btGeneric6DofSpringConstraint* pGen6DOFSpring = new btGeneric6DofSpringConstraint(*pBodyA, *pBodyB, frameInA, frameInB, true); pGen6DOFSpring->setLinearUpperLimit(btVector3(5., 0., 0.)); pGen6DOFSpring->setLinearLowerLimit(btVector3(-5., 0., 0.)); pGen6DOFSpring->setAngularLowerLimit(btVector3(0.f, 0.f, -1.5f)); pGen6DOFSpring->setAngularUpperLimit(btVector3(0.f, 0.f, 1.5f)); m_dynamicsWorld->addConstraint(pGen6DOFSpring, true); pGen6DOFSpring->setDbgDrawSize(btScalar(5.f)); pGen6DOFSpring->enableSpring(0, true); pGen6DOFSpring->setStiffness(0, 39.478f); pGen6DOFSpring->setDamping(0, 0.5f); pGen6DOFSpring->enableSpring(5, true); pGen6DOFSpring->setStiffness(5, 39.478f); pGen6DOFSpring->setDamping(0, 0.3f); pGen6DOFSpring->setEquilibriumPoint(); } #endif #if ENABLE_ALL_DEMOS { // create a Hinge2 joint // create two rigid bodies // static bodyA (parent) on top: btTransform tr; tr.setIdentity(); tr.setOrigin(btVector3(btScalar(-20.), btScalar(4.), btScalar(0.))); btRigidBody* pBodyA = createRigidBody( 0.0, tr, shape); pBodyA->setActivationState(DISABLE_DEACTIVATION); // dynamic bodyB (child) below it : tr.setIdentity(); tr.setOrigin(btVector3(btScalar(-20.), btScalar(0.), btScalar(0.))); btRigidBody* pBodyB = createRigidBody(1.0, tr, shape); pBodyB->setActivationState(DISABLE_DEACTIVATION); // add some data to build constraint frames btVector3 parentAxis(0.f, 1.f, 0.f); btVector3 childAxis(1.f, 0.f, 0.f); btVector3 anchor(-20.f, 0.f, 0.f); btHinge2Constraint* pHinge2 = new btHinge2Constraint(*pBodyA, *pBodyB, anchor, parentAxis, childAxis); pHinge2->setLowerLimit(-SIMD_HALF_PI * 0.5f); pHinge2->setUpperLimit( SIMD_HALF_PI * 0.5f); // add constraint to world m_dynamicsWorld->addConstraint(pHinge2, true); // draw constraint frames and limits for debugging pHinge2->setDbgDrawSize(btScalar(5.f)); } #endif #if ENABLE_ALL_DEMOS { // create a Hinge joint between two dynamic bodies // create two rigid bodies // static bodyA (parent) on top: btTransform tr; tr.setIdentity(); tr.setOrigin(btVector3(btScalar(-20.), btScalar(-2.), btScalar(0.))); btRigidBody* pBodyA = createRigidBody( 1.0f, tr, shape); pBodyA->setActivationState(DISABLE_DEACTIVATION); // dynamic bodyB: tr.setIdentity(); tr.setOrigin(btVector3(btScalar(-30.), btScalar(-2.), btScalar(0.))); btRigidBody* pBodyB = createRigidBody(10.0, tr, shape); pBodyB->setActivationState(DISABLE_DEACTIVATION); // add some data to build constraint frames btVector3 axisA(0.f, 1.f, 0.f); btVector3 axisB(0.f, 1.f, 0.f); btVector3 pivotA(-5.f, 0.f, 0.f); btVector3 pivotB( 5.f, 0.f, 0.f); spHingeDynAB = new btHingeConstraint(*pBodyA, *pBodyB, pivotA, pivotB, axisA, axisB); spHingeDynAB->setLimit(-SIMD_HALF_PI * 0.5f, SIMD_HALF_PI * 0.5f); // add constraint to world m_dynamicsWorld->addConstraint(spHingeDynAB, true); // draw constraint frames and limits for debugging spHingeDynAB->setDbgDrawSize(btScalar(5.f)); } #endif #if ENABLE_ALL_DEMOS { // 6DOF connected to the world, with motor btTransform tr; tr.setIdentity(); tr.setOrigin(btVector3(btScalar(10.), btScalar(-15.), btScalar(0.))); btRigidBody* pBody = createRigidBody( 1.0, tr, shape); pBody->setActivationState(DISABLE_DEACTIVATION); btTransform frameB; frameB.setIdentity(); btGeneric6DofConstraint* pGen6Dof = new btGeneric6DofConstraint( *pBody, frameB, false ); m_dynamicsWorld->addConstraint(pGen6Dof); pGen6Dof->setDbgDrawSize(btScalar(5.f)); pGen6Dof->setAngularLowerLimit(btVector3(0,0,0)); pGen6Dof->setAngularUpperLimit(btVector3(0,0,0)); pGen6Dof->setLinearLowerLimit(btVector3(-10., 0, 0)); pGen6Dof->setLinearUpperLimit(btVector3(10., 0, 0)); pGen6Dof->getTranslationalLimitMotor()->m_enableMotor[0] = true; pGen6Dof->getTranslationalLimitMotor()->m_targetVelocity[0] = 5.0f; pGen6Dof->getTranslationalLimitMotor()->m_maxMotorForce[0] = 6.0f; } #endif m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); }