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 GLInstancingRenderer::updateCamera(int upAxis)
{
b3Assert(glGetError() ==GL_NO_ERROR);
m_upAxis = upAxis;
switch (upAxis)
{
case 1:
gLightPos = b3MakeVector3(-50.f,100,30);
break;
case 2:
gLightPos = b3MakeVector3(-50.f,30,100);
break;
default:
b3Assert(0);
};
m_data->m_defaultCamera.setCameraUpAxis(upAxis);
m_data->m_defaultCamera.setAspectRatio((float)m_screenWidth/(float)m_screenHeight);
m_data->m_defaultCamera.update();
m_data->m_defaultCamera.getCameraProjectionMatrix(m_data->m_projectionMatrix);
m_data->m_defaultCamera.getCameraViewMatrix(m_data->m_viewMatrix);
}
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));
}
}
}
LWRigidBody()
:m_linearVelocity(b3MakeVector3(0,0,0)),
m_angularVelocity(b3MakeVector3(0,0,0)),
m_gravityAcceleration(b3MakeVector3(0,0,0)),//-10,0)),
m_flags(LWFLAG_USE_QUATERNION_DERIVATIVE)
{
}
b3GpuGridBroadphase::b3GpuGridBroadphase(cl_context ctx, cl_device_id device, cl_command_queue q)
: m_context(ctx),
m_device(device),
m_queue(q),
m_allAabbsGPU1(ctx, q),
m_smallAabbsMappingGPU(ctx, q),
m_largeAabbsMappingGPU(ctx, q),
m_gpuPairs(ctx, q),
m_hashGpu(ctx, q),
m_cellStartGpu(ctx, q),
m_paramsGPU(ctx, q)
{
b3Vector3 gridSize = b3MakeVector3(3, 3, 3);
b3Vector3 invGridSize = b3MakeVector3(1.f / gridSize[0], 1.f / gridSize[1], 1.f / gridSize[2]);
m_paramsCPU.m_gridSize[0] = 128;
m_paramsCPU.m_gridSize[1] = 128;
m_paramsCPU.m_gridSize[2] = 128;
m_paramsCPU.m_gridSize[3] = maxBodiesPerCell;
m_paramsCPU.setMaxBodiesPerCell(maxBodiesPerCell);
m_paramsCPU.m_invCellSize[0] = invGridSize[0];
m_paramsCPU.m_invCellSize[1] = invGridSize[1];
m_paramsCPU.m_invCellSize[2] = invGridSize[2];
m_paramsCPU.m_invCellSize[3] = 0.f;
m_paramsGPU.push_back(m_paramsCPU);
cl_int errNum = 0;
{
const char* sapSrc = sapCL;
cl_program sapProg = b3OpenCLUtils::compileCLProgramFromString(m_context, m_device, sapSrc, &errNum, "", B3_BROADPHASE_SAP_PATH);
b3Assert(errNum == CL_SUCCESS);
m_copyAabbsKernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, sapSrc, "copyAabbsKernel", &errNum, sapProg);
m_sap2Kernel = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, sapSrc, "computePairsKernelTwoArrays", &errNum, sapProg);
b3Assert(errNum == CL_SUCCESS);
}
{
cl_program gridProg = b3OpenCLUtils::compileCLProgramFromString(m_context, m_device, gridBroadphaseCL, &errNum, "", B3_GRID_BROADPHASE_PATH);
b3Assert(errNum == CL_SUCCESS);
kCalcHashAABB = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, gridBroadphaseCL, "kCalcHashAABB", &errNum, gridProg);
b3Assert(errNum == CL_SUCCESS);
kClearCellStart = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, gridBroadphaseCL, "kClearCellStart", &errNum, gridProg);
b3Assert(errNum == CL_SUCCESS);
kFindCellStart = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, gridBroadphaseCL, "kFindCellStart", &errNum, gridProg);
b3Assert(errNum == CL_SUCCESS);
kFindOverlappingPairs = b3OpenCLUtils::compileCLKernelFromString(m_context, m_device, gridBroadphaseCL, "kFindOverlappingPairs", &errNum, gridProg);
b3Assert(errNum == CL_SUCCESS);
}
m_sorter = new b3RadixSort32CL(m_context, m_device, m_queue);
}
void SimpleCamera::update()
{
int forwardAxis(-1);
switch (m_data->m_cameraUpAxis)
{
case 1:
forwardAxis = 2;
m_data->m_cameraUp = b3MakeVector3(0,1,0);
//gLightPos = b3MakeVector3(-50.f,100,30);
break;
case 2:
forwardAxis = 1;
m_data->m_cameraUp = b3MakeVector3(0,0,1);
//gLightPos = b3MakeVector3(-50.f,30,100);
break;
default:
{
b3Assert(0);
return;
}
};
b3Vector3 eyePos = b3MakeVector3(0,0,0);
eyePos[forwardAxis] = -m_data->m_cameraDistance;
m_data->m_cameraForward = b3MakeVector3(eyePos[0],eyePos[1],eyePos[2]);
if (m_data->m_cameraForward.length2() < B3_EPSILON)
{
m_data->m_cameraForward.setValue(1.f,0.f,0.f);
} else
{
m_data->m_cameraForward.normalize();
}
// m_azi=m_azi+0.01;
b3Scalar rele = m_data->m_yaw * b3Scalar(0.01745329251994329547);// rads per deg
b3Scalar razi = m_data->m_pitch * b3Scalar(0.01745329251994329547);// rads per deg
b3Quaternion rot(m_data->m_cameraUp,razi);
b3Vector3 right = m_data->m_cameraUp.cross(m_data->m_cameraForward);
b3Quaternion roll(right,-rele);
eyePos = b3Matrix3x3(rot) * b3Matrix3x3(roll) * eyePos;
m_data->m_cameraPosition = eyePos;
m_data->m_cameraPosition+= m_data->m_cameraTargetPosition;
}
SimpleCameraInternalData()
:m_cameraTargetPosition(b3MakeVector3(0,0,0)),
m_cameraDistance(20),
m_cameraUp(b3MakeVector3(0,1,0)),
m_cameraUpAxis(1),
m_cameraForward(b3MakeVector3(1,0,0)),
m_frustumZNear(0.01),
m_frustumZFar(1000),
m_yaw(20),
m_pitch(0),
m_aspect(1)
{
}
int main(int argc, char* argv[])
{
float dt = 1./120.f;
int width = 1024;
int height=768;
SimpleOpenGL3App* app = new SimpleOpenGL3App("AllBullet2Demos",width,height);
app->m_instancingRenderer->setCameraDistance(13);
app->m_instancingRenderer->setCameraPitch(0);
app->m_instancingRenderer->setCameraTargetPosition(b3MakeVector3(0,0,0));
app->m_window->setMouseMoveCallback(MyMouseMoveCallback);
app->m_window->setMouseButtonCallback(MyMouseButtonCallback);
GLint err = glGetError();
assert(err==GL_NO_ERROR);
sth_stash* fontstash=app->getFontStash();
gui = new GwenUserInterface;
gui->init(width,height,fontstash,app->m_window->getRetinaScale());
const char* names[] = {"test1", "test2","test3"};
gui->registerComboBox(13,3,&names[0],1);
const char* names2[] = {"comboF", "comboG","comboH"};
gui->registerComboBox(2,3,&names2[0],1);
gui->setComboBoxCallback(MyComboBoxCallback);
do
{
GLint err = glGetError();
assert(err==GL_NO_ERROR);
app->m_instancingRenderer->init();
app->m_instancingRenderer->updateCamera();
app->drawGrid();
char bla[1024];
static int frameCount = 0;
frameCount++;
sprintf(bla,"Simple test frame %d", frameCount);
app->drawText(bla,10,10);
static int toggle = 1;
if (1)
{
gui->draw(app->m_instancingRenderer->getScreenWidth(),app->m_instancingRenderer->getScreenHeight());
}
toggle=1-toggle;
app->swapBuffer();
} while (!app->m_window->requestedExit());
delete gui;
delete app;
return 0;
}
void ComputeClosestPointsSphereSphere(b3Scalar sphereARadius, const b3Vector3& sphereAPosWorld, b3Scalar sphereBRadius, const b3Vector3& sphereBPosWorld, plContactCache* contactCache) {
if (contactCache->numAddedPoints < contactCache->pointCapacity)
{
lwContactPoint& pointOut = contactCache->pointsOut[contactCache->numAddedPoints];
b3Vector3 diff = sphereAPosWorld-sphereBPosWorld;
b3Scalar len = diff.length();
pointOut.m_distance = len - (sphereARadius+sphereBRadius);
if (pointOut.m_distance<=0)
{
b3Vector3 normOnB = b3MakeVector3(1,0,0);
if (len > B3_EPSILON) {
normOnB = diff / len;
}
plVecCopy(pointOut.m_normalOnB,normOnB);
b3Vector3 ptAWorld = sphereAPosWorld - sphereARadius*normOnB;
plVecCopy(pointOut.m_ptOnAWorld,ptAWorld);
plVecCopy(pointOut.m_ptOnBWorld,ptAWorld-normOnB*pointOut.m_distance);
contactCache->numAddedPoints++;
}
}
}
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);
}
}
void ExplicitEuler::integrateExplicitEuler(struct CpuSoftClothDemoInternalData* clothData, char* vertexPositions, int vertexStride,float deltaTime)
{
B3_PROFILE("integrateEuler");
b3Vector3 deltaTimeVec = b3MakeVector3(deltaTime,deltaTime,deltaTime,0);
int numPoints = clothData->m_particleMasses.size();
for (int i=0;i<numPoints;i++)
{
float mass = clothData->m_particleMasses[i];
if (mass)
{
b3Vector3 dv = (clothData->m_forces[i]/mass)*deltaTimeVec;
clothData->m_velocities[i]+= dv;
clothData->m_velocities[i]*=0.999;
b3Vector3& pos = (b3Vector3&)vertexPositions[i*vertexStride];
pos += clothData->m_velocities[i]*deltaTimeVec;
}
}
}
void b3GpuPgsConstraintSolver::initSolverBody(int bodyIndex, b3GpuSolverBody* solverBody, b3RigidBodyData* rb)
{
solverBody->m_deltaLinearVelocity.setValue(0.f, 0.f, 0.f);
solverBody->m_deltaAngularVelocity.setValue(0.f, 0.f, 0.f);
solverBody->internalGetPushVelocity().setValue(0.f, 0.f, 0.f);
solverBody->internalGetTurnVelocity().setValue(0.f, 0.f, 0.f);
b3Assert(rb);
// solverBody->m_worldTransform = getWorldTransform(rb);
solverBody->internalSetInvMass(b3MakeVector3(rb->m_invMass, rb->m_invMass, rb->m_invMass));
solverBody->m_originalBodyIndex = bodyIndex;
solverBody->m_angularFactor = b3MakeVector3(1, 1, 1);
solverBody->m_linearFactor = b3MakeVector3(1, 1, 1);
solverBody->m_linearVelocity = getLinearVelocity(rb);
solverBody->m_angularVelocity = getAngularVelocity(rb);
}
int b3CpuNarrowPhase::registerConvexHullShape(b3ConvexUtility* utilPtr)
{
int collidableIndex = allocateCollidable();
if (collidableIndex < 0)
return collidableIndex;
b3Collidable& col = m_data->m_collidablesCPU[collidableIndex];
col.m_shapeType = SHAPE_CONVEX_HULL;
col.m_shapeIndex = -1;
{
b3Vector3 localCenter = b3MakeVector3(0, 0, 0);
for (int i = 0; i < utilPtr->m_vertices.size(); i++)
localCenter += utilPtr->m_vertices[i];
localCenter *= (1.f / utilPtr->m_vertices.size());
utilPtr->m_localCenter = localCenter;
col.m_shapeIndex = registerConvexHullShapeInternal(utilPtr, col);
}
if (col.m_shapeIndex >= 0)
{
b3Aabb aabb;
b3Vector3 myAabbMin = b3MakeVector3(1e30f, 1e30f, 1e30f);
b3Vector3 myAabbMax = b3MakeVector3(-1e30f, -1e30f, -1e30f);
for (int i = 0; i < utilPtr->m_vertices.size(); i++)
{
myAabbMin.setMin(utilPtr->m_vertices[i]);
myAabbMax.setMax(utilPtr->m_vertices[i]);
}
aabb.m_min[0] = myAabbMin[0];
aabb.m_min[1] = myAabbMin[1];
aabb.m_min[2] = myAabbMin[2];
aabb.m_minIndices[3] = 0;
aabb.m_max[0] = myAabbMax[0];
aabb.m_max[1] = myAabbMax[1];
aabb.m_max[2] = myAabbMax[2];
aabb.m_signedMaxIndices[3] = 0;
m_data->m_localShapeAABBCPU.push_back(aabb);
}
return collidableIndex;
}
//clear the simplex, remove all the vertices
void b3VoronoiSimplexSolver::reset()
{
m_cachedValidClosest = false;
m_numVertices = 0;
m_needsUpdate = true;
m_lastW = b3MakeVector3(b3Scalar(B3_LARGE_FLOAT),b3Scalar(B3_LARGE_FLOAT),b3Scalar(B3_LARGE_FLOAT));
m_cachedBC.reset();
}
void b3RequestCameraImageSetViewMatrix(b3SharedMemoryCommandHandle commandHandle, const float cameraPosition[3], const float cameraTargetPosition[3], const float cameraUp[3])
{
b3Vector3 eye = b3MakeVector3(cameraPosition[0], cameraPosition[1], cameraPosition[2]);
b3Vector3 center = b3MakeVector3(cameraTargetPosition[0], cameraTargetPosition[1], cameraTargetPosition[2]);
b3Vector3 up = b3MakeVector3(cameraUp[0], cameraUp[1], cameraUp[2]);
b3Vector3 f = (center - eye).normalized();
b3Vector3 u = up.normalized();
b3Vector3 s = (f.cross(u)).normalized();
u = s.cross(f);
float viewMatrix[16];
viewMatrix[0*4+0] = s.x;
viewMatrix[1*4+0] = s.y;
viewMatrix[2*4+0] = s.z;
viewMatrix[0*4+1] = u.x;
viewMatrix[1*4+1] = u.y;
viewMatrix[2*4+1] = u.z;
viewMatrix[0*4+2] =-f.x;
viewMatrix[1*4+2] =-f.y;
viewMatrix[2*4+2] =-f.z;
viewMatrix[0*4+3] = 0.f;
viewMatrix[1*4+3] = 0.f;
viewMatrix[2*4+3] = 0.f;
viewMatrix[3*4+0] = -s.dot(eye);
viewMatrix[3*4+1] = -u.dot(eye);
viewMatrix[3*4+2] = f.dot(eye);
viewMatrix[3*4+3] = 1.f;
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
for (int i=0;i<16;i++)
{
command->m_requestPixelDataArguments.m_viewMatrix[i] = viewMatrix[i];
}
command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES;
}
void ExplicitEuler::computeGravityForces(struct CpuSoftClothDemoInternalData* clothData, char* vertexPositions, int vertexStride, float dt)
{
B3_PROFILE("computeForces");
int numPoints = clothData->m_particleMasses.size();
b3Vector3 gravityAcceleration = b3MakeVector3(0,-9.8,0);
//f=m*a
for (int i=0;i<numPoints;i++)
{
{
float particleMass = clothData->m_particleMasses[i];
b3Vector3 particleMassVec = b3MakeVector3(particleMass,particleMass,particleMass,0);
clothData->m_forces[i] = gravityAcceleration*particleMass;
}
}
}
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();
}
int b3CpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* position, const float* orientation, int collidableIndex, int userData)
{
b3RigidBodyData body;
int bodyIndex = m_data->m_rigidBodies.size();
body.m_invMass = mass ? 1.f/mass : 0.f;
body.m_angVel.setValue(0,0,0);
body.m_collidableIdx = collidableIndex;
body.m_frictionCoeff = 0.3f;
body.m_linVel.setValue(0,0,0);
body.m_pos.setValue(position[0],position[1],position[2]);
body.m_quat.setValue(orientation[0],orientation[1],orientation[2],orientation[3]);
body.m_restituitionCoeff = 0.f;
m_data->m_rigidBodies.push_back(body);
if (collidableIndex>=0)
{
b3Aabb& worldAabb = m_data->m_aabbWorldSpace.expand();
b3Aabb localAabb = m_data->m_np->getLocalSpaceAabb(collidableIndex);
b3Vector3 localAabbMin=b3MakeVector3(localAabb.m_min[0],localAabb.m_min[1],localAabb.m_min[2]);
b3Vector3 localAabbMax=b3MakeVector3(localAabb.m_max[0],localAabb.m_max[1],localAabb.m_max[2]);
b3Scalar margin = 0.01f;
b3Transform t;
t.setIdentity();
t.setOrigin(b3MakeVector3(position[0],position[1],position[2]));
t.setRotation(b3Quaternion(orientation[0],orientation[1],orientation[2],orientation[3]));
b3TransformAabb(localAabbMin,localAabbMax, margin,t,worldAabb.m_minVec,worldAabb.m_maxVec);
m_data->m_bp->createProxy(worldAabb.m_minVec,worldAabb.m_maxVec,bodyIndex,0,1,1);
// b3Vector3 aabbMin,aabbMax;
// m_data->m_bp->getAabb(bodyIndex,aabbMin,aabbMax);
} else
{
b3Error("registerPhysicsInstance using invalid collidableIndex\n");
}
return bodyIndex;
}
InternalDataRenderer() :
m_cameraPosition(b3MakeVector3(0,0,0)),
m_cameraTargetPosition(b3MakeVector3(15,2,-24)),
m_cameraDistance(150),
m_cameraUp(b3MakeVector3(0,1,0)),
m_azi(100.f),//135.f),
//m_ele(25.f),
m_ele(25.f),
m_mouseInitialized(false),
m_leftMouseButton(0),
m_middleMouseButton(0),
m_rightMouseButton(0),
m_shadowMap(0),
m_shadowTexture(0),
m_altPressed(0),
m_controlPressed(0)
{
}
void ComputeClosestPointsSphereSphere(const LWSphere& sphereA, const LWPose& sphereAPose, const LWSphere& sphereB, const LWPose& sphereBPose, LWContactPoint& pointOut) {
b3Vector3 diff = sphereAPose.m_position-sphereBPose.m_position;
btScalar len = diff.length();
pointOut.m_distance = len - (sphereA.m_radius+sphereB.m_radius);
pointOut.m_normalOnB = b3MakeVector3(1,0,0);
if (len > B3_EPSILON) {
pointOut.m_normalOnB = diff / len;
}
pointOut.m_ptOnAWorld = sphereAPose.m_position - sphereA.m_radius*pointOut.m_normalOnB;
pointOut.m_ptOnBWorld = pointOut.m_ptOnAWorld-pointOut.m_normalOnB*pointOut.m_distance;
}
void Tutorial::tutorialCollisionUpdate(float deltaTime,LWContactPoint& contact)
{
m_bodies[1]->m_worldPose.m_position.z = 3;
ComputeClosestPointsSphereSphere(m_bodies[0]->m_collisionShape.m_sphere,
m_bodies[0]->m_worldPose,
m_bodies[1]->m_collisionShape.m_sphere,
m_bodies[1]->m_worldPose,
contact);
switch (m_stage)
{
case 0:
{
m_bodies[0]->m_angularVelocity=b3MakeVector3(0,0,0);
m_bodies[0]->m_linearVelocity=b3MakeVector3(1,0,0);
break;
}
case 1:
{
m_bodies[0]->m_linearVelocity=b3MakeVector3(-1,0,0);
break;
}
case 2:
{
m_bodies[0]->m_linearVelocity=b3MakeVector3(0,1,0);
break;
}
case 3:
{
m_bodies[0]->m_linearVelocity=b3MakeVector3(0,-1,0);
break;
}
case 4:
{
m_bodies[0]->m_linearVelocity=b3MakeVector3(0,0,1);
break;
}
case 5:
{
m_bodies[0]->m_linearVelocity=b3MakeVector3(0,0,-1);
break;
}
default:{}
};
m_counter++;
if (m_counter>120)
{
m_counter=0;
m_stage++;
if (m_stage>5)
m_stage=0;
}
}
void b3CpuRigidBodyPipeline::integrate(float deltaTime)
{
float angDamping=0.f;
b3Vector3 gravityAcceleration=b3MakeVector3(0,-9,0);
//integrate transforms (external forces/gravity should be moved into constraint solver)
for (int i=0;i<m_data->m_rigidBodies.size();i++)
{
b3IntegrateTransform(&m_data->m_rigidBodies[i],deltaTime,angDamping,gravityAcceleration);
}
}
int main(int argc, char* argv[])
{
float dt = 1./120.f;
#ifdef BT_DEBUG
char* name = "Bullet 2 CPU FeatherstoneMultiBodyDemo (Debug build=SLOW)";
#else
char* name = "Bullet 2 CPU FeatherstoneMultiBodyDemo";
#endif
SimpleOpenGL3App* app = new SimpleOpenGL3App(name,1024,768);
app->m_instancingRenderer->setCameraDistance(40);
app->m_instancingRenderer->setCameraPitch(0);
app->m_instancingRenderer->setCameraTargetPosition(b3MakeVector3(0,0,0));
app->m_window->setMouseMoveCallback(MyMouseMoveCallback);
app->m_window->setMouseButtonCallback(MyMouseButtonCallback);
BasicDemo* demo = new BasicDemo(app);
demo->initPhysics();
sDemo = demo;
GLint err = glGetError();
assert(err==GL_NO_ERROR);
do
{
GLint err = glGetError();
assert(err==GL_NO_ERROR);
app->m_instancingRenderer->init();
app->m_instancingRenderer->updateCamera();
demo->stepSimulation();
demo->drawObjects();
app->drawGrid(10,0.01);
char bla[1024];
static int frameCount = 0;
frameCount++;
sprintf(bla,"Simulation frame %d", frameCount);
app->drawText(bla,10,10);
app->swapBuffer();
} while (!app->m_window->requestedExit());
demo->exitPhysics();
delete demo;
delete app;
return 0;
}
void SimpleOpenGL2Renderer::drawLines(const float* positions, const float color[4], int numPoints, int pointStrideInBytes, const unsigned int* indices, int numIndices, float pointDrawSize)
{
int pointStrideInFloats = pointStrideInBytes/4;
glLineWidth(pointDrawSize);
for (int i=0; i<numIndices; i+=2)
{
int index0 = indices[i];
int index1 = indices[i+1];
b3Vector3 fromColor = b3MakeVector3(color[0],color[1],color[2]);
b3Vector3 toColor = b3MakeVector3(color[0],color[1],color[2]);
b3Vector3 from= b3MakeVector3(positions[index0*pointStrideInFloats],positions[index0*pointStrideInFloats+1],positions[index0*pointStrideInFloats+2]);
b3Vector3 to= b3MakeVector3(positions[index1*pointStrideInFloats],positions[index1*pointStrideInFloats+1],positions[index1*pointStrideInFloats+2]);
glBegin(GL_LINES);
glColor3f(fromColor.getX(), fromColor.getY(), fromColor.getZ());
glVertex3d(from.getX(), from.getY(), from.getZ());
glColor3f(toColor.getX(), toColor.getY(), toColor.getZ());
glVertex3d(to.getX(), to.getY(), to.getZ());
glEnd();
}
}
bool rayConvex(const b3Vector3& rayFromLocal, const b3Vector3& rayToLocal, const b3ConvexPolyhedronData& poly,
const b3AlignedObjectArray<b3GpuFace>& faces, float& hitFraction, b3Vector3& hitNormal)
{
float exitFraction = hitFraction;
float enterFraction = -0.1f;
b3Vector3 curHitNormal=b3MakeVector3(0,0,0);
for (int i=0;i<poly.m_numFaces;i++)
{
const b3GpuFace& face = faces[poly.m_faceOffset+i];
float fromPlaneDist = b3Dot(rayFromLocal,face.m_plane)+face.m_plane.w;
float toPlaneDist = b3Dot(rayToLocal,face.m_plane)+face.m_plane.w;
if (fromPlaneDist<0.f)
{
if (toPlaneDist >= 0.f)
{
float fraction = fromPlaneDist / (fromPlaneDist-toPlaneDist);
if (exitFraction>fraction)
{
exitFraction = fraction;
}
}
} else
{
if (toPlaneDist<0.f)
{
float fraction = fromPlaneDist / (fromPlaneDist-toPlaneDist);
if (enterFraction <= fraction)
{
enterFraction = fraction;
curHitNormal = face.m_plane;
curHitNormal.w = 0.f;
}
} else
{
return false;
}
}
if (exitFraction <= enterFraction)
return false;
}
if (enterFraction < 0.f)
return false;
hitFraction = enterFraction;
hitNormal = curHitNormal;
return true;
}
void b3DynamicBvhBroadphase::getBroadphaseAabb(b3Vector3& aabbMin,b3Vector3& aabbMax) const
{
B3_ATTRIBUTE_ALIGNED16(b3DbvtVolume) bounds;
if(!m_sets[0].empty())
if(!m_sets[1].empty()) b3Merge( m_sets[0].m_root->volume,
m_sets[1].m_root->volume,bounds);
else
bounds=m_sets[0].m_root->volume;
else if(!m_sets[1].empty()) bounds=m_sets[1].m_root->volume;
else
bounds=b3DbvtVolume::FromCR(b3MakeVector3(0,0,0),0);
aabbMin=bounds.Mins();
aabbMax=bounds.Maxs();
}
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++;
}
}
}
}
virtual void renderScene()
{
m_app->m_renderer->renderScene();
m_app->drawText3D("X",1,0,0,1);
m_app->drawText3D("Y",0,1,0,1);
m_app->drawText3D("Z",0,0,1,1);
for (int i=0;i<m_contactPoints.size();i++)
{
const LWContactPoint& contact = m_contactPoints[i];
b3Vector3 color=b3MakeVector3(1,1,0);
float lineWidth=3;
if (contact.m_distance<0)
{
color.setValue(1,0,0);
}
m_app->m_renderer->drawLine(contact.m_ptOnAWorld,contact.m_ptOnBWorld,color,lineWidth);
}
}
virtual void stepSimulation(float deltaTime)
{
m_x+=0.01f;
m_y+=0.01f;
m_z+=0.01f;
int index=0;
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[m_app->getUpAxis()] = 1+1*b3Sin(m_x+i-j);
float orn[4]={0,0,0,1};
m_app->m_renderer->writeSingleInstanceTransformToCPU(pos,orn,m_movingInstances[index++]);
}
}
m_app->m_renderer->writeTransforms();
}
int b3CpuNarrowPhase::registerConvexHullShape(const float* vertices, int strideInBytes, int numVertices, const float* scaling)
{
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]));
}
b3ConvexUtility* utilPtr = new b3ConvexUtility();
bool merge = true;
if (numVertices)
{
utilPtr->initializePolyhedralFeatures(&verts[0], verts.size(), merge);
}
int collidableIndex = registerConvexHullShape(utilPtr);
delete utilPtr;
return collidableIndex;
}