void SimpleOpenGL2Renderer::getCameraViewMatrix(float viewMat[16]) const { b3Assert(0); }
void SimpleOpenGL2Renderer::getCameraProjectionMatrix(float projMat[16]) const { b3Assert(0); }
void GpuRaytraceScene::renderScene2() { // GpuBoxPlaneScene::renderScene(); // return; B3_PROFILE("raytrace"); //raytrace into the texels { B3_PROFILE("update camera"); m_instancingRenderer->updateCamera(); } //generate primary rays { B3_PROFILE("readbackAllBodiesToCpu"); m_data->m_np->readbackAllBodiesToCpu(); } { B3_PROFILE("Generate primary rays"); float top = 1.f; float bottom = -1.f; float nearPlane = 1.f; float farPlane = 1000.f; float tanFov = (top-bottom)*0.5f / nearPlane; float screenWidth = m_instancingRenderer->getScreenWidth(); float screenHeight = m_instancingRenderer->getScreenHeight(); float fov = 2. * atanf (tanFov); float aspect = screenWidth / screenHeight; b3Vector3 rayFrom, camTarget; m_instancingRenderer->getCameraPosition(rayFrom); m_instancingRenderer->getCameraTargetPosition(camTarget); b3Vector3 rayForward = camTarget-rayFrom; rayForward.normalize(); rayForward*= farPlane; b3Vector3 rightOffset; b3Vector3 vertical=b3MakeVector3(0.f,1.f,0.f); b3Vector3 hor; hor = rayForward.cross(vertical); hor.normalize(); vertical = hor.cross(rayForward); vertical.normalize(); float tanfov = tanf(0.5f*fov); hor *= aspect*2.f * farPlane * tanfov; vertical *= 2.f * farPlane * tanfov; b3Vector3 rayToCenter = rayFrom + rayForward; float texWidth = m_raytraceData->textureWidth; float texHeight = m_raytraceData->textureHeight; float widthFactor = (screenWidth/texWidth); float heightFactor = (screenHeight/texHeight); //should be screenwidth/height b3Vector3 dHor = hor * 1./float(screenWidth); b3Vector3 dVert = vertical * 1./float(screenHeight); b3Transform rayFromTrans; rayFromTrans.setIdentity(); rayFromTrans.setOrigin(rayFrom); b3Transform rayFromLocal; b3Transform rayToLocal; //create primary rays primaryRays.resize(m_raytraceData->textureWidth*m_raytraceData->textureHeight); b3Vector3 rayTo; b3RayInfo ray; { for (int x=0;x<m_raytraceData->textureWidth;x++) { for (int y=0;y<m_raytraceData->textureHeight;y++) { rayTo = rayToCenter - 0.5f * hor + 0.5f * vertical; rayTo += x * dHor*widthFactor; rayTo -= y * dVert*heightFactor; ray.m_from = rayFrom; ray.m_to = rayTo; primaryRays[x+m_raytraceData->textureWidth*y] = ray; } } } } b3AlignedObjectArray<b3RayHit> hits; { B3_PROFILE("hits.resize"); hits.resize(primaryRays.size()); } if (1) { B3_PROFILE("init hits"); for (int i=0;i<hits.size();i++) { hits[i].m_hitFraction = 1.f; hits[i].m_hitResult2 = -1; } } b3Vector3 lightPos=b3MakeVector3(1000,1000,100); { B3_PROFILE("cast primary rays"); //m_raycaster->castRaysHost(primaryRays, hits, this->m_data->m_np->getNumRigidBodies(), m_data->m_np->getBodiesCpu(), m_data->m_np->getNumCollidablesGpu(), m_data->m_np->getCollidablesCpu(),m_data->m_np->getInternalData()); m_raycaster->castRays(primaryRays, hits, this->m_data->m_np->getNumRigidBodies(), m_data->m_np->getBodiesCpu(), m_data->m_np->getNumCollidablesGpu(), m_data->m_np->getCollidablesCpu(), m_data->m_np->getInternalData()); } b3AlignedObjectArray<b3RayInfo> shadowRays; { B3_PROFILE("shadowRays.resize"); shadowRays.resize(primaryRays.size()); } b3AlignedObjectArray<b3RayHit> shadowHits; { B3_PROFILE("shadowHits.resize"); shadowHits.resize(hits.size()); } { B3_PROFILE("init shadow rays"); for (int i=0;i<hits.size();i++) { if(hits[i].m_hitFraction<1.f) { //hits[i].m_hitPoint.setInterpolate3(primaryRays[i].m_from,primaryRays[i].m_to,hits[i].m_hitFraction); //b3Vector3 shift = (lightPos-hits[i].m_hitPoint).normalize()*0.001f; shadowRays[i].m_from = hits[i].m_hitPoint; shadowRays[i].m_to = lightPos; shadowHits[i].m_hitFraction=1.f; shadowHits[i].m_hitBody = hits[i].m_hitBody; } else { shadowRays[i].m_from.setValue(0,0,0); shadowRays[i].m_to.setValue(0,0,0); shadowHits[i].m_hitFraction=1.f; shadowHits[i].m_hitResult2 = -2; } } } { B3_PROFILE("cast shadow rays"); //m_raycaster->castRaysHost(primaryRays, hits, this->m_data->m_np->getNumRigidBodies(), m_data->m_np->getBodiesCpu(), m_data->m_np->getNumCollidablesGpu(), m_data->m_np->getCollidablesCpu()); m_raycaster->castRays(shadowRays, shadowHits, this->m_data->m_np->getNumRigidBodies(), m_data->m_np->getBodiesCpu(), m_data->m_np->getNumCollidablesGpu(), m_data->m_np->getCollidablesCpu(), m_data->m_np->getInternalData()); } { B3_PROFILE("write texels"); for (int i=0;i<shadowHits.size();i++) { bool hit = hits[i].m_hitFraction < 1.f; if (hit) { float dotje = hits[i].m_hitNormal.dot(lightPos); if (dotje>0.f) { if (shadowHits[i].m_hitFraction<1.f) { dotje = -1.f; } } if (dotje>0.f) { m_raytraceData->m_texels[(i)*3+0] = 128+128.f*hits[i].m_hitNormal.x; m_raytraceData->m_texels[(i)*3+1] = 128+128.f*hits[i].m_hitNormal.y; m_raytraceData->m_texels[(i)*3+2] = 128+128.f*hits[i].m_hitNormal.z; if (hits[i].m_hitBody==0) { m_raytraceData->m_texels[(i)*3+0] = 255; m_raytraceData->m_texels[(i)*3+1] = 255; m_raytraceData->m_texels[(i)*3+2] = 255; } else { } } else { if (dotje == -1.f) { m_raytraceData->m_texels[(i)*3+0] = 0; m_raytraceData->m_texels[(i)*3+1] = 0; m_raytraceData->m_texels[(i)*3+2] = 255; } else { m_raytraceData->m_texels[(i)*3+0] = 255; m_raytraceData->m_texels[(i)*3+1] = 0; m_raytraceData->m_texels[(i)*3+2] = 0; } } } else { m_raytraceData->m_texels[(i)*3+0] = 128; m_raytraceData->m_texels[(i)*3+1] = 128; m_raytraceData->m_texels[(i)*3+2] = 192; } } } GLint err; { B3_PROFILE("get error"); err = glGetError(); assert(err==GL_NO_ERROR); glActiveTexture(GL_TEXTURE0); } { B3_PROFILE("glTexImage2D"); glBindTexture(GL_TEXTURE_2D, *m_raytraceData->m_texId); glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, m_raytraceData->textureWidth, m_raytraceData->textureHeight, 0, GL_RGB, GL_UNSIGNED_BYTE, m_raytraceData->m_texels); } { B3_PROFILE("glGetError"); err = glGetError(); assert(err==GL_NO_ERROR); } b3Assert(m_primRenderer); float color[4] = {1,1,1,1}; //float rect[4] = {0,0,m_raytraceData->textureWidth,m_raytraceData->textureHeight}; float rect[4] = {0,0,m_instancingRenderer->getScreenWidth(),m_instancingRenderer->getScreenHeight()}; float u[2] = {0,1}; float v[2] = {0,1}; int useRGBA = 1; { B3_PROFILE("drawTexturedRect"); m_primRenderer->drawTexturedRect(rect[0],rect[1],rect[2],rect[3],color,u[0],v[0],u[1],v[1], useRGBA); } { B3_PROFILE("glGetError"); err = glGetError(); assert(err==GL_NO_ERROR); } }
void SimpleOpenGL2Renderer::setActiveCamera(CommonCameraInterface* cam) { b3Assert(0);//not supported yet }
void b3Island::Add(b3Contact* c) { b3Assert(contactCount < contactCapacity); contacts[contactCount] = c; ++contactCount; }
void b3Island::Add(b3Joint* j) { b3Assert(jointCount < jointCapacity); joints[jointCount] = j; ++jointCount; }
void SimpleOpenGL2App::drawGrid(DrawGridData data) { int gridSize = data.gridSize; float upOffset = data.upOffset; int upAxis = data.upAxis; float gridColor[4]; gridColor[0] = data.gridColor[0]; gridColor[1] = data.gridColor[1]; gridColor[2] = data.gridColor[2]; gridColor[3] = data.gridColor[3]; int sideAxis=-1; int forwardAxis=-1; switch (upAxis) { case 1: forwardAxis=2; sideAxis=0; break; case 2: forwardAxis=1; sideAxis=0; break; default: b3Assert(0); }; //b3Vector3 gridColor = b3MakeVector3(0.5,0.5,0.5); b3AlignedObjectArray<unsigned int> indices; b3AlignedObjectArray<b3Vector3> vertices; int lineIndex=0; for(int i=-gridSize;i<=gridSize;i++) { { b3Assert(glGetError() ==GL_NO_ERROR); b3Vector3 from = b3MakeVector3(0,0,0); from[sideAxis] = float(i); from[upAxis] = upOffset; from[forwardAxis] = float(-gridSize); b3Vector3 to=b3MakeVector3(0,0,0); to[sideAxis] = float(i); to[upAxis] = upOffset; to[forwardAxis] = float(gridSize); vertices.push_back(from); indices.push_back(lineIndex++); vertices.push_back(to); indices.push_back(lineIndex++); // m_renderer->drawLine(from,to,gridColor); } b3Assert(glGetError() ==GL_NO_ERROR); { b3Assert(glGetError() ==GL_NO_ERROR); b3Vector3 from=b3MakeVector3(0,0,0); from[sideAxis] = float(-gridSize); from[upAxis] = upOffset; from[forwardAxis] = float(i); b3Vector3 to=b3MakeVector3(0,0,0); to[sideAxis] = float(gridSize); to[upAxis] = upOffset; to[forwardAxis] = float(i); vertices.push_back(from); indices.push_back(lineIndex++); vertices.push_back(to); indices.push_back(lineIndex++); // m_renderer->drawLine(from,to,gridColor); } } m_renderer->drawLines(&vertices[0].x, gridColor, vertices.size(),sizeof(b3Vector3),&indices[0],indices.size(),1); m_renderer->drawLine(b3MakeVector3(0,0,0),b3MakeVector3(1,0,0),b3MakeVector3(1,0,0),3); m_renderer->drawLine(b3MakeVector3(0,0,0),b3MakeVector3(0,1,0),b3MakeVector3(0,1,0),3); m_renderer->drawLine(b3MakeVector3(0,0,0),b3MakeVector3(0,0,1),b3MakeVector3(0,0,1),3); // void GLInstancingRenderer::drawPoints(const float* positions, const float color[4], int numPoints, int pointStrideInBytes, float pointDrawSize) //we don't use drawPoints because all points would have the same color // b3Vector3 points[3] = { b3MakeVector3(1, 0, 0), b3MakeVector3(0, 1, 0), b3MakeVector3(0, 0, 1) }; // m_renderer->drawPoints(&points[0].x, b3MakeVector3(1, 0, 0), 3, sizeof(b3Vector3), 6); }
void b3Island::Add(b3Body* b) { b3Assert(bodyCount < bodyCapacity); b->m_islandID = bodyCount; bodies[bodyCount] = b; ++bodyCount; }
b3Solver::b3Solver(cl_context ctx, cl_device_id device, cl_command_queue queue, int pairCapacity) :m_nIterations(4), m_context(ctx), m_device(device), m_queue(queue), m_batchSizes(ctx,queue) { m_sort32 = new b3RadixSort32CL(ctx,device,queue); m_scan = new b3PrefixScanCL(ctx,device,queue,B3_SOLVER_N_CELLS); m_search = new b3BoundSearchCL(ctx,device,queue,B3_SOLVER_N_CELLS); const int sortSize = B3NEXTMULTIPLEOF( pairCapacity, 512 ); m_sortDataBuffer = new b3OpenCLArray<b3SortData>(ctx,queue,sortSize); m_contactBuffer2 = new b3OpenCLArray<b3Contact4>(ctx,queue); m_numConstraints = new b3OpenCLArray<unsigned int>(ctx,queue,B3_SOLVER_N_CELLS ); m_numConstraints->resize(B3_SOLVER_N_CELLS); m_offsets = new b3OpenCLArray<unsigned int>( ctx,queue,B3_SOLVER_N_CELLS); m_offsets->resize(B3_SOLVER_N_CELLS); const char* additionalMacros = ""; const char* srcFileNameForCaching=""; cl_int pErrNum; const char* batchKernelSource = batchingKernelsCL; const char* batchKernelNewSource = batchingKernelsNewCL; const char* solverSetupSource = solverSetupCL; const char* solverSetup2Source = solverSetup2CL; const char* solveContactSource = solveContactCL; const char* solveFrictionSource = solveFrictionCL; { cl_program solveContactProg= b3OpenCLUtils::compileCLProgramFromString( ctx, device, solveContactSource, &pErrNum,additionalMacros, B3_SOLVER_CONTACT_KERNEL_PATH); b3Assert(solveContactProg); cl_program solveFrictionProg= b3OpenCLUtils::compileCLProgramFromString( ctx, device, solveFrictionSource, &pErrNum,additionalMacros, B3_SOLVER_FRICTION_KERNEL_PATH); b3Assert(solveFrictionProg); cl_program solverSetup2Prog= b3OpenCLUtils::compileCLProgramFromString( ctx, device, solverSetup2Source, &pErrNum,additionalMacros, B3_SOLVER_SETUP2_KERNEL_PATH); b3Assert(solverSetup2Prog); cl_program solverSetupProg= b3OpenCLUtils::compileCLProgramFromString( ctx, device, solverSetupSource, &pErrNum,additionalMacros, B3_SOLVER_SETUP_KERNEL_PATH); b3Assert(solverSetupProg); m_solveFrictionKernel= b3OpenCLUtils::compileCLKernelFromString( ctx, device, solveFrictionSource, "BatchSolveKernelFriction", &pErrNum, solveFrictionProg,additionalMacros ); b3Assert(m_solveFrictionKernel); m_solveContactKernel= b3OpenCLUtils::compileCLKernelFromString( ctx, device, solveContactSource, "BatchSolveKernelContact", &pErrNum, solveContactProg,additionalMacros ); b3Assert(m_solveContactKernel); m_contactToConstraintKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverSetupSource, "ContactToConstraintKernel", &pErrNum, solverSetupProg,additionalMacros ); b3Assert(m_contactToConstraintKernel); m_setSortDataKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverSetup2Source, "SetSortDataKernel", &pErrNum, solverSetup2Prog,additionalMacros ); b3Assert(m_setSortDataKernel); m_reorderContactKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverSetup2Source, "ReorderContactKernel", &pErrNum, solverSetup2Prog,additionalMacros ); b3Assert(m_reorderContactKernel); m_copyConstraintKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, solverSetup2Source, "CopyConstraintKernel", &pErrNum, solverSetup2Prog,additionalMacros ); b3Assert(m_copyConstraintKernel); } { cl_program batchingProg = b3OpenCLUtils::compileCLProgramFromString( ctx, device, batchKernelSource, &pErrNum,additionalMacros, B3_BATCHING_PATH); //cl_program batchingProg = b3OpenCLUtils::compileCLProgramFromString( ctx, device, 0, &pErrNum,additionalMacros, B3_BATCHING_PATH,true); b3Assert(batchingProg); m_batchingKernel = b3OpenCLUtils::compileCLKernelFromString( ctx, device, batchKernelSource, "CreateBatches", &pErrNum, batchingProg,additionalMacros ); b3Assert(m_batchingKernel); } { cl_program batchingNewProg = b3OpenCLUtils::compileCLProgramFromString( ctx, device, batchKernelNewSource, &pErrNum,additionalMacros, B3_BATCHING_NEW_PATH); b3Assert(batchingNewProg); m_batchingKernelNew = b3OpenCLUtils::compileCLKernelFromString( ctx, device, batchKernelNewSource, "CreateBatchesNew", &pErrNum, batchingNewProg,additionalMacros ); //m_batchingKernelNew = b3OpenCLUtils::compileCLKernelFromString( ctx, device, batchKernelNewSource, "CreateBatchesBruteForce", &pErrNum, batchingNewProg,additionalMacros ); b3Assert(m_batchingKernelNew); } }
SimpleOpenGL2App::SimpleOpenGL2App(const char* title, int width, int height) { gApp2 = this; m_data = new SimpleOpenGL2AppInternalData; m_window = new b3gDefaultOpenGLWindow(); b3gWindowConstructionInfo ci; ci.m_title = title; ci.m_openglVersion = 2; ci.m_width = width; ci.m_height = height; m_window->createWindow(ci); m_window->setWindowTitle(title); #ifndef NO_GLEW #ifndef __APPLE__ #ifndef _WIN32 //some Linux implementations need the 'glewExperimental' to be true glewExperimental = GL_TRUE; #endif //_WIN32 if (glewInit() != GLEW_OK) { b3Error("glewInit failed"); exit(1); } if (!GLEW_VERSION_2_1) // check that the machine supports the 2.1 API. { b3Error("GLEW_VERSION_2_1 needs to support 2_1"); exit(1); // or handle the error in a nicer way } #endif //__APPLE__ #endif //NO_GLEW TwGenerateDefaultFonts(); m_data->m_fontTextureId = BindFont2(g_DefaultNormalFont); m_data->m_largeFontTextureId = BindFont2(g_DefaultLargeFont); glGetError();//don't remove this call, it is needed for Ubuntu glClearColor( m_backgroundColorRGB[0], m_backgroundColorRGB[1], m_backgroundColorRGB[2], 1.f); b3Assert(glGetError() ==GL_NO_ERROR); //m_primRenderer = new GLPrimitiveRenderer(width,height); m_parameterInterface = 0; b3Assert(glGetError() ==GL_NO_ERROR); //m_renderer = new GLInstancingRenderer(128*1024,32*1024*1024); //m_renderer->init(); //m_renderer->resize(width,height); b3Assert(glGetError() ==GL_NO_ERROR); //m_renderer->InitShaders(); m_window->setMouseMoveCallback(Simple2MouseMoveCallback); m_window->setMouseButtonCallback(Simple2MouseButtonCallback); m_window->setKeyboardCallback(Simple2KeyboardCallback); m_window->setWheelCallback(Simple2WheelCallback); m_window->setResizeCallback(Simple2ResizeCallback); }
void run(int tIdx) { int offset = 0; for (int ii=0;ii<B3_MAX_NUM_BATCHES;ii++) { int numInBatch = m_batchSizes->at(m_cellIndex*B3_MAX_NUM_BATCHES+ii); if (!numInBatch) break; for (int jj=0;jj<numInBatch;jj++) { int i = m_start + offset+jj; int batchId = m_constraints[i].m_batchIdx; b3Assert(batchId==ii); float frictionCoeff = m_constraints[i].getFrictionCoeff(); int aIdx = (int)m_constraints[i].m_bodyA; int bIdx = (int)m_constraints[i].m_bodyB; int localBatch = m_constraints[i].m_batchIdx; b3RigidBodyData& bodyA = m_bodies[aIdx]; b3RigidBodyData& bodyB = m_bodies[bIdx]; if( !m_solveFriction ) { float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX}; float minRambdaDt[4] = {0.f,0.f,0.f,0.f}; solveContact<false>( m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass, (const b3Matrix3x3 &)m_shapes[aIdx].m_invInertiaWorld, (b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, (const b3Matrix3x3 &)m_shapes[bIdx].m_invInertiaWorld, maxRambdaDt, minRambdaDt ); } else { float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX}; float minRambdaDt[4] = {0.f,0.f,0.f,0.f}; float sum = 0; for(int j=0; j<4; j++) { sum +=m_constraints[i].m_appliedRambdaDt[j]; } frictionCoeff = 0.7f; for(int j=0; j<4; j++) { maxRambdaDt[j] = frictionCoeff*sum; minRambdaDt[j] = -maxRambdaDt[j]; } solveFriction( m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass,(const b3Matrix3x3 &) m_shapes[aIdx].m_invInertiaWorld, (b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass,(const b3Matrix3x3 &) m_shapes[bIdx].m_invInertiaWorld, maxRambdaDt, minRambdaDt ); } } offset+=numInBatch; } /* for (int bb=0;bb<m_maxNumBatches;bb++) { //for(int ic=m_nConstraints-1; ic>=0; ic--) for(int ic=0; ic<m_nConstraints; ic++) { int i = m_start + ic; if (m_constraints[i].m_batchIdx != bb) continue; float frictionCoeff = m_constraints[i].getFrictionCoeff(); int aIdx = (int)m_constraints[i].m_bodyA; int bIdx = (int)m_constraints[i].m_bodyB; int localBatch = m_constraints[i].m_batchIdx; b3RigidBodyData& bodyA = m_bodies[aIdx]; b3RigidBodyData& bodyB = m_bodies[bIdx]; if( !m_solveFriction ) { float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX}; float minRambdaDt[4] = {0.f,0.f,0.f,0.f}; solveContact<false>( m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass, (const b3Matrix3x3 &)m_shapes[aIdx].m_invInertiaWorld, (b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass, (const b3Matrix3x3 &)m_shapes[bIdx].m_invInertiaWorld, maxRambdaDt, minRambdaDt ); } else { float maxRambdaDt[4] = {FLT_MAX,FLT_MAX,FLT_MAX,FLT_MAX}; float minRambdaDt[4] = {0.f,0.f,0.f,0.f}; float sum = 0; for(int j=0; j<4; j++) { sum +=m_constraints[i].m_appliedRambdaDt[j]; } frictionCoeff = 0.7f; for(int j=0; j<4; j++) { maxRambdaDt[j] = frictionCoeff*sum; minRambdaDt[j] = -maxRambdaDt[j]; } solveFriction( m_constraints[i], (b3Vector3&)bodyA.m_pos, (b3Vector3&)bodyA.m_linVel, (b3Vector3&)bodyA.m_angVel, bodyA.m_invMass,(const b3Matrix3x3 &) m_shapes[aIdx].m_invInertiaWorld, (b3Vector3&)bodyB.m_pos, (b3Vector3&)bodyB.m_linVel, (b3Vector3&)bodyB.m_angVel, bodyB.m_invMass,(const b3Matrix3x3 &) m_shapes[bIdx].m_invInertiaWorld, maxRambdaDt, minRambdaDt ); } } } */ }
static __inline void solveFriction(b3GpuConstraint4& cs, const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA, const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB, float maxRambdaDt[4], float minRambdaDt[4]) { if( cs.m_fJacCoeffInv[0] == 0 && cs.m_fJacCoeffInv[0] == 0 ) return; const b3Vector3& center = (const b3Vector3&)cs.m_center; b3Vector3 n = -(const b3Vector3&)cs.m_linear; b3Vector3 tangent[2]; #if 1 b3PlaneSpace1 (n, tangent[0],tangent[1]); #else b3Vector3 r = cs.m_worldPos[0]-center; tangent[0] = cross3( n, r ); tangent[1] = cross3( tangent[0], n ); tangent[0] = normalize3( tangent[0] ); tangent[1] = normalize3( tangent[1] ); #endif b3Vector3 angular0, angular1, linear; b3Vector3 r0 = center - posA; b3Vector3 r1 = center - posB; for(int i=0; i<2; i++) { setLinearAndAngular( tangent[i], r0, r1, &linear, &angular0, &angular1 ); float rambdaDt = calcRelVel(linear, -linear, angular0, angular1, linVelA, angVelA, linVelB, angVelB ); rambdaDt *= cs.m_fJacCoeffInv[i]; { float prevSum = cs.m_fAppliedRambdaDt[i]; float updated = prevSum; updated += rambdaDt; updated = b3Max( updated, minRambdaDt[i] ); updated = b3Min( updated, maxRambdaDt[i] ); rambdaDt = updated - prevSum; cs.m_fAppliedRambdaDt[i] = updated; } b3Vector3 linImp0 = invMassA*linear*rambdaDt; b3Vector3 linImp1 = invMassB*(-linear)*rambdaDt; b3Vector3 angImp0 = (invInertiaA* angular0)*rambdaDt; b3Vector3 angImp1 = (invInertiaB* angular1)*rambdaDt; #ifdef _WIN32 b3Assert(_finite(linImp0.getX())); b3Assert(_finite(linImp1.getX())); #endif linVelA += linImp0; angVelA += angImp0; linVelB += linImp1; angVelB += angImp1; } { // angular damping for point constraint b3Vector3 ab = ( posB - posA ).normalized(); b3Vector3 ac = ( center - posA ).normalized(); if( b3Dot( ab, ac ) > 0.95f || (invMassA == 0.f || invMassB == 0.f)) { float angNA = b3Dot( n, angVelA ); float angNB = b3Dot( n, angVelB ); angVelA -= (angNA*0.1f)*n; angVelB -= (angNB*0.1f)*n; } } }
static __inline void solveContact(b3GpuConstraint4& cs, const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA, const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB, float maxRambdaDt[4], float minRambdaDt[4]) { b3Vector3 dLinVelA; dLinVelA.setZero(); b3Vector3 dAngVelA; dAngVelA.setZero(); b3Vector3 dLinVelB; dLinVelB.setZero(); b3Vector3 dAngVelB; dAngVelB.setZero(); for(int ic=0; ic<4; ic++) { // dont necessary because this makes change to 0 if( cs.m_jacCoeffInv[ic] == 0.f ) continue; { b3Vector3 angular0, angular1, linear; b3Vector3 r0 = cs.m_worldPos[ic] - (b3Vector3&)posA; b3Vector3 r1 = cs.m_worldPos[ic] - (b3Vector3&)posB; setLinearAndAngular( (const b3Vector3 &)cs.m_linear, (const b3Vector3 &)r0, (const b3Vector3 &)r1, &linear, &angular0, &angular1 ); float rambdaDt = calcRelVel((const b3Vector3 &)cs.m_linear,(const b3Vector3 &) -cs.m_linear, angular0, angular1, linVelA, angVelA, linVelB, angVelB ) + cs.m_b[ic]; rambdaDt *= cs.m_jacCoeffInv[ic]; { float prevSum = cs.m_appliedRambdaDt[ic]; float updated = prevSum; updated += rambdaDt; updated = b3Max( updated, minRambdaDt[ic] ); updated = b3Min( updated, maxRambdaDt[ic] ); rambdaDt = updated - prevSum; cs.m_appliedRambdaDt[ic] = updated; } b3Vector3 linImp0 = invMassA*linear*rambdaDt; b3Vector3 linImp1 = invMassB*(-linear)*rambdaDt; b3Vector3 angImp0 = (invInertiaA* angular0)*rambdaDt; b3Vector3 angImp1 = (invInertiaB* angular1)*rambdaDt; #ifdef _WIN32 b3Assert(_finite(linImp0.getX())); b3Assert(_finite(linImp1.getX())); #endif if( JACOBI ) { dLinVelA += linImp0; dAngVelA += angImp0; dLinVelB += linImp1; dAngVelB += angImp1; } else { linVelA += linImp0; angVelA += angImp0; linVelB += linImp1; angVelB += angImp1; } } } if( JACOBI ) { linVelA += dLinVelA; angVelA += dAngVelA; linVelB += dLinVelB; angVelB += dAngVelB; } }
void b3Point2PointConstraint::getInfo2NonVirtual (b3ConstraintInfo2* info, const b3Transform& body0_trans, const b3Transform& body1_trans) { b3Assert(!m_useSolveConstraintObsolete); //retrieve matrices // anchor points in global coordinates with respect to body PORs. // set jacobian info->m_J1linearAxis[0] = 1; info->m_J1linearAxis[info->rowskip+1] = 1; info->m_J1linearAxis[2*info->rowskip+2] = 1; b3Vector3 a1 = body0_trans.getBasis()*getPivotInA(); { b3Vector3* angular0 = (b3Vector3*)(info->m_J1angularAxis); b3Vector3* angular1 = (b3Vector3*)(info->m_J1angularAxis+info->rowskip); b3Vector3* angular2 = (b3Vector3*)(info->m_J1angularAxis+2*info->rowskip); b3Vector3 a1neg = -a1; a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); } if (info->m_J2linearAxis) { info->m_J2linearAxis[0] = -1; info->m_J2linearAxis[info->rowskip+1] = -1; info->m_J2linearAxis[2*info->rowskip+2] = -1; } b3Vector3 a2 = body1_trans.getBasis()*getPivotInB(); { // b3Vector3 a2n = -a2; b3Vector3* angular0 = (b3Vector3*)(info->m_J2angularAxis); b3Vector3* angular1 = (b3Vector3*)(info->m_J2angularAxis+info->rowskip); b3Vector3* angular2 = (b3Vector3*)(info->m_J2angularAxis+2*info->rowskip); a2.getSkewSymmetricMatrix(angular0,angular1,angular2); } // set right hand side b3Scalar currERP = (m_flags & B3_P2P_FLAGS_ERP) ? m_erp : info->erp; b3Scalar k = info->fps * currERP; int j; for (j=0; j<3; j++) { info->m_constraintError[j*info->rowskip] = k * (a2[j] + body1_trans.getOrigin()[j] - a1[j] - body0_trans.getOrigin()[j]); //printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]); } if(m_flags & B3_P2P_FLAGS_CFM) { for (j=0; j<3; j++) { info->cfm[j*info->rowskip] = m_cfm; } } b3Scalar impulseClamp = m_setting.m_impulseClamp;// for (j=0; j<3; j++) { if (m_setting.m_impulseClamp > 0) { info->m_lowerLimit[j*info->rowskip] = -impulseClamp; info->m_upperLimit[j*info->rowskip] = impulseClamp; } } info->m_damping = m_setting.m_damping; }
void InverseDynamicsExample::initPhysics() { //roboticists like Z up int upAxis = 2; m_guiHelper->setUpAxis(upAxis); createEmptyDynamicsWorld(); btVector3 gravity(0,0,0); // gravity[upAxis]=-9.8; m_dynamicsWorld->setGravity(gravity); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); { SliderParams slider("Kp",&kp); slider.m_minVal=0; slider.m_maxVal=2000; if (m_guiHelper->getParameterInterface()) m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { SliderParams slider("Kd",&kd); slider.m_minVal=0; slider.m_maxVal=50; if (m_guiHelper->getParameterInterface()) m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } if (m_option == BT_ID_PROGRAMMATICALLY) { ButtonParams button("toggle inverse model",0,true); button.m_callback = toggleUseInverseModel; m_guiHelper->getParameterInterface()->registerButtonParameter(button); } switch (m_option) { case BT_ID_LOAD_URDF: { BulletURDFImporter u2b(m_guiHelper,0,1); bool loadOk = u2b.loadURDF("kuka_iiwa/model.urdf");// lwr / kuka.urdf"); if (loadOk) { int rootLinkIndex = u2b.getRootLinkIndex(); b3Printf("urdf root link index = %d\n",rootLinkIndex); MyMultiBodyCreator creation(m_guiHelper); btTransform identityTrans; identityTrans.setIdentity(); ConvertURDF2Bullet(u2b,creation, identityTrans,m_dynamicsWorld,true,u2b.getPathPrefix()); for (int i = 0; i < u2b.getNumAllocatedCollisionShapes(); i++) { m_collisionShapes.push_back(u2b.getAllocatedCollisionShape(i)); } m_multiBody = creation.getBulletMultiBody(); if (m_multiBody) { //kuka without joint control/constraints will gain energy explode soon due to timestep/integrator //temporarily set some extreme damping factors until we have some joint control or constraints m_multiBody->setAngularDamping(0*0.99); m_multiBody->setLinearDamping(0*0.99); b3Printf("Root link name = %s",u2b.getLinkName(u2b.getRootLinkIndex()).c_str()); } } break; } case BT_ID_PROGRAMMATICALLY: { btTransform baseWorldTrans; baseWorldTrans.setIdentity(); m_multiBody = createInvertedPendulumMultiBody(m_dynamicsWorld, m_guiHelper, baseWorldTrans, false); break; } default: { b3Error("Unknown option in InverseDynamicsExample::initPhysics"); b3Assert(0); } }; if(m_multiBody) { { if (m_guiHelper->getAppInterface() && m_guiHelper->getParameterInterface()) { m_timeSeriesCanvas = new TimeSeriesCanvas(m_guiHelper->getAppInterface()->m_2dCanvasInterface,512,230, "Joint Space Trajectory"); m_timeSeriesCanvas ->setupTimeSeries(3,100, 0); } } // construct inverse model btInverseDynamics::btMultiBodyTreeCreator id_creator; if (-1 == id_creator.createFromBtMultiBody(m_multiBody, false)) { b3Error("error creating tree\n"); } else { m_inverseModel = btInverseDynamics::CreateMultiBodyTree(id_creator); } // add joint target controls qd.resize(m_multiBody->getNumDofs()); qd_name.resize(m_multiBody->getNumDofs()); q_name.resize(m_multiBody->getNumDofs()); if (m_timeSeriesCanvas && m_guiHelper->getParameterInterface()) { for(std::size_t dof=0;dof<qd.size();dof++) { qd[dof] = 0; char tmp[25]; sprintf(tmp,"q_desired[%lu]",dof); qd_name[dof] = tmp; SliderParams slider(qd_name[dof].c_str(),&qd[dof]); slider.m_minVal=-3.14; slider.m_maxVal=3.14; sprintf(tmp,"q[%lu]",dof); q_name[dof] = tmp; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); btVector4 color = sJointCurveColors[dof&7]; m_timeSeriesCanvas->addDataSource(q_name[dof].c_str(), color[0]*255,color[1]*255,color[2]*255); } } } m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); }