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);
}
