//to be implemented by the demo
void ForkLiftDemo::renderme()
{
updateCamera();
btScalar m[16];
int i;
btVector3 wheelColor(1,0,0);
btVector3 worldBoundsMin,worldBoundsMax;
getDynamicsWorld()->getBroadphase()->getBroadphaseAabb(worldBoundsMin,worldBoundsMax);
for (i=0;i<m_vehicle->getNumWheels();i++)
{
//synchronize the wheels with the (interpolated) chassis worldtransform
m_vehicle->updateWheelTransform(i,true);
//draw wheels (cylinders)
m_vehicle->getWheelInfo(i).m_worldTransform.getOpenGLMatrix(m);
m_shapeDrawer->drawOpenGL(m,m_wheelShape,wheelColor,getDebugMode(),worldBoundsMin,worldBoundsMax);
}
int lineWidth=250;
int xStart = m_glutScreenWidth - lineWidth;
int yStart = 20;
if((getDebugMode() & btIDebugDraw::DBG_NoHelpText)==0)
{
setOrthographicProjection();
glDisable(GL_LIGHTING);
glColor3f(0, 0, 0);
char buf[124];
glRasterPos3f(xStart, yStart, 0);
sprintf(buf,"SHIFT+Cursor Left/Right - rotate lift");
GLDebugDrawString(xStart,20,buf);
yStart+=20;
glRasterPos3f(xStart, yStart, 0);
sprintf(buf,"SHIFT+Cursor UP/Down - move fork up/down");
yStart+=20;
GLDebugDrawString(xStart,yStart,buf);
glRasterPos3f(xStart, yStart, 0);
sprintf(buf,"F5 - toggle camera mode");
yStart+=20;
GLDebugDrawString(xStart,yStart,buf);
glRasterPos3f(xStart, yStart, 0);
sprintf(buf,"Click inside this window for keyboard focus");
yStart+=20;
GLDebugDrawString(xStart,yStart,buf);
resetPerspectiveProjection();
glEnable(GL_LIGHTING);
}
DemoApplication::renderme();
}
void BazARTracker::show_result(CamAugmentation &augment, IplImage *video, IplImage **dst)
{
if (getDebugMode()){
if (*dst==0) *dst=cvCloneImage(video);
else cvCopy(video, *dst);
}
CvMat *m = augment.GetProjectionMatrix(0);
// Flip...(This occured from OpenGL origin / camera origin)
CvMat *coordinateTrans = cvCreateMat(3, 3, CV_64F);
cvmSetIdentity(coordinateTrans);
cvmSet(coordinateTrans, 1, 1, -1);
cvmSet(coordinateTrans, 1, 2, m_cparam->cparam.ysize);
cvMatMul(coordinateTrans, m, m);
// extract intrinsic camera parameters from bazar's projection matrix..
GetARToolKitRTfromBAZARProjMat(g_matIntrinsic, m, matCameraRT4_4);
cvTranspose(matCameraRT4_4, matCameraRT4_4);
cvReleaseMat(&coordinateTrans);
// Debug
if (getDebugMode()) {
// draw the coordinate system axes
double w =video->width/2.0;
double h =video->height/2.0;
// 3D coordinates of an object
double pts[4][4] = {
{w,h,0, 1}, // 0,0,0,1
{w*2,h,0, 1}, // w, 0
{w,h*2,0, 1}, // 0, h
{w,h,-w-h, 1} // 0, 0, -
};
CvMat ptsMat, projectedMat;
cvInitMatHeader(&ptsMat, 4, 4, CV_64FC1, pts);
cvInitMatHeader(&projectedMat, 3, 4, CV_64FC1, projected);
cvGEMM(m, &ptsMat, 1, 0, 0, &projectedMat, CV_GEMM_B_T );
for (int i=0; i<4; i++)
{
projected[0][i] /= projected[2][i];
projected[1][i] /= projected[2][i];
}
// draw the projected lines
cvLine(*dst, cvPoint((int)projected[0][0], (int)projected[1][0]),
cvPoint((int)projected[0][1], (int)projected[1][1]), CV_RGB(255,0,0), 2);
cvLine(*dst, cvPoint((int)projected[0][0], (int)projected[1][0]),
cvPoint((int)projected[0][2], (int)projected[1][2]), CV_RGB(0,255,0), 2);
cvLine(*dst, cvPoint((int)projected[0][0], (int)projected[1][0]),
cvPoint((int)projected[0][3], (int)projected[1][3]), CV_RGB(0,0,255), 2);
}
}
void BazARTracker::update()
{
// Do not update with a null image
if (m_imageptr == NULL) return;
// Image format conversion for BAZAR
for(int i=0; i<m_cparam->cparam.xsize*m_cparam->cparam.ysize*4; i++)
{
image->imageData[i] = m_imageptr[i];
}
// Image format change for processing color->gray
if(image->nChannels == 4)
{
cvCvtColor(image, gray, CV_RGBA2GRAY);
}
else if(image->nChannels == 3)
{
cvCvtColor(image, gray, CV_BGR2GRAY);
}
else
{
gray = image;
}
cvFlip(gray, gray); // bazar uses hoizontially flipped input images
if(getDebugMode()) cvShowImage("Gray", gray); // show what bazar will see
// if the trained planar surface is detected
if (detector.detect(gray))
{
// start on a new frame
augment.Clear();
// we have only 1 camera
add_detected_homography(detector, augment);
// bundle adjust
augment.Accomodate(4, 1e-4);
show_result(augment, image, &display); // GL-projection matrix, if debug: render additional output
if (getDebugMode()) cvShowImage("Result_BAZAR", display);
(static_cast<BazARMarker*>(m_markerlist[0].get()))->update(matCameraRT4_4);
}
else
{
if (getDebugMode()) cvShowImage("Result_BAZAR", display);
(static_cast<BazARMarker*>(m_markerlist[0].get()))->update(NULL);
}
}
void FractureDemo::showMessage()
{
if((getDebugMode() & btIDebugDraw::DBG_DrawText))
{
setOrthographicProjection();
glDisable(GL_LIGHTING);
glColor3f(0, 0, 0);
char buf[124];
int lineWidth=380;
int xStart = m_glutScreenWidth - lineWidth;
int yStart = 20;
btFractureDynamicsWorld* world = (btFractureDynamicsWorld*)m_dynamicsWorld;
if (world->getFractureMode())
{
sprintf(buf,"Fracture mode");
} else
{
sprintf(buf,"Glue mode");
}
GLDebugDrawString(xStart,yStart,buf);
sprintf(buf,"f to toggle fracture/glue mode");
yStart+=20;
GLDebugDrawString(xStart,yStart,buf);
sprintf(buf,"space to restart, mouse to pick/shoot");
yStart+=20;
GLDebugDrawString(xStart,yStart,buf);
resetPerspectiveProjection();
glEnable(GL_LIGHTING);
}
}
//to be implemented by the demo
void VehicleDemo::renderme()
{
updateCamera();
btScalar m[16];
int i;
btVector3 wheelColor(1,0,0);
btVector3 worldBoundsMin,worldBoundsMax;
getDynamicsWorld()->getBroadphase()->getBroadphaseAabb(worldBoundsMin,worldBoundsMax);
for (i=0;i<m_vehicle->getNumWheels();i++)
{
//synchronize the wheels with the (interpolated) chassis worldtransform
m_vehicle->updateWheelTransform(i,true);
//draw wheels (cylinders)
m_vehicle->getWheelInfo(i).m_worldTransform.getOpenGLMatrix(m);
m_shapeDrawer->drawOpenGL(m,m_wheelShape,wheelColor,getDebugMode(),worldBoundsMin,worldBoundsMax);
}
DemoApplication::renderme();
}
void CcdPhysicsDemo::displayText()
{
#ifndef __QNX__
int lineWidth=440;
int xStart = m_glutScreenWidth - lineWidth;
int yStart = 20;
if((getDebugMode() & btIDebugDraw::DBG_DrawText)!=0)
{
setOrthographicProjection();
glDisable(GL_LIGHTING);
glColor3f(0, 0, 0);
char buf[124];
glRasterPos3f(xStart, yStart, 0);
switch (m_ccdMode)
{
case USE_CCD:
{
sprintf(buf,"Predictive contacts and motion clamping");
break;
}
case USE_NO_CCD:
{
sprintf(buf,"CCD handling disabled");
break;
}
default:
{
sprintf(buf,"unknown CCD setting");
};
};
GLDebugDrawString(xStart,20,buf);
glRasterPos3f(xStart, yStart, 0);
sprintf(buf,"Press 'p' to change CCD mode");
yStart+=20;
GLDebugDrawString(xStart,yStart,buf);
glRasterPos3f(xStart, yStart, 0);
sprintf(buf,"Press '.' or right mouse to shoot bullets");
yStart+=20;
GLDebugDrawString(xStart,yStart,buf);
glRasterPos3f(xStart, yStart, 0);
sprintf(buf,"space to restart, h(elp), t(ext), w(ire)");
yStart+=20;
GLDebugDrawString(xStart,yStart,buf);
resetPerspectiveProjection();
glEnable(GL_LIGHTING);
}
#endif
}
static void
init_once()
{
av_register_all();
avdevice_register_all();
#if LIBAVFORMAT_VERSION_INT >= AV_VERSION_INT(53, 13, 0)
avformat_network_init();
#endif
av_lockmgr_register(avcodecManageMutex);
if (getDebugMode())
setAvLogLevel();
}
void renderme()
{
float m[16];
int i;
for (i=0;i<numObjects;i++)
{
SimdTransform transA;
transA.setIdentity();
float pos[3];
float rot[4];
ms[i].getWorldPosition(pos[0],pos[1],pos[2]);
ms[i].getWorldOrientation(rot[0],rot[1],rot[2],rot[3]);
SimdQuaternion q(rot[0],rot[1],rot[2],rot[3]);
transA.setRotation(q);
SimdPoint3 dpos;
dpos.setValue(pos[0],pos[1],pos[2]);
transA.setOrigin( dpos );
transA.getOpenGLMatrix( m );
SimdVector3 wireColor(0.f,0.f,1.f); //wants deactivation
///color differently for active, sleeping, wantsdeactivation states
if (physObjects[i]->GetRigidBody()->GetActivationState() == 1) //active
{
wireColor = SimdVector3 (1.f,0.f,0.f);
}
if (physObjects[i]->GetRigidBody()->GetActivationState() == 2) //ISLAND_SLEEPING
{
wireColor = SimdVector3 (0.f,1.f,0.f);
}
char extraDebug[125];
//sprintf(extraDebug,"islId, Body=%i , %i",physObjects[i]->GetRigidBody()->m_islandTag1,physObjects[i]->GetRigidBody()->m_debugBodyId);
shapePtr[shapeIndex[i]]->SetExtraDebugInfo(extraDebug);
GL_ShapeDrawer::DrawOpenGL(m,shapePtr[shapeIndex[i]],wireColor,getDebugMode());
}
}
void clientResetScene()
{
int i;
for (i=0;i<numObjects;i++)
{
if (i>0)
{
if ((getDebugMode() & IDebugDraw::DBG_NoHelpText))
{
if (physObjects[i]->GetRigidBody()->GetCollisionShape()->GetShapeType() == BOX_SHAPE_PROXYTYPE)
{
physObjects[i]->GetRigidBody()->SetCollisionShape(shapePtr[2]);
} else
{
physObjects[i]->GetRigidBody()->SetCollisionShape(shapePtr[1]);
}
BroadphaseProxy* bpproxy = physObjects[i]->GetRigidBody()->m_broadphaseHandle;
physicsEnvironmentPtr->GetBroadphase()->CleanProxyFromPairs(bpproxy);
}
//stack them
int colsize = 10;
int row = (i*CUBE_HALF_EXTENTS*2)/(colsize*2*CUBE_HALF_EXTENTS);
int row2 = row;
int col = (i)%(colsize)-colsize/2;
if (col>3)
{
col=11;
row2 |=1;
}
physObjects[i]->setPosition(col*2*CUBE_HALF_EXTENTS + (row2%2)*CUBE_HALF_EXTENTS,
row*2*CUBE_HALF_EXTENTS+CUBE_HALF_EXTENTS+EXTRA_HEIGHT,0);
physObjects[i]->setOrientation(0,0,0,1);
physObjects[i]->SetLinearVelocity(0,0,0,false);
physObjects[i]->SetAngularVelocity(0,0,0,false);
}
}
}
void SimplexDemo::displayCallback()
{
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glDisable(GL_LIGHTING);
GL_ShapeDrawer::drawCoordSystem();
btScalar m[16];
int i;
btVector3 worldBoundsMin(-1000,-1000,-1000);
btVector3 worldBoundsMax(1000,1000,1000);
for (i=0; i<numObjects; i++)
{
btTransform transA;
transA.setIdentity();
btVector3 dpos(0.f,5.f,0.f);
transA.setOrigin( dpos );
btQuaternion orn;
orn.setEuler(yaw,pitch,roll);
transA.setRotation(orn);
transA.getOpenGLMatrix( m );
/// draw the simplex
m_shapeDrawer.drawOpenGL(m,shapePtr[i],btVector3(1,1,1),getDebugMode(),worldBoundsMin,worldBoundsMax);
/// calculate closest point from simplex to the origin, and draw this vector
simplex.calcClosest(m);
}
pitch += 0.005f;
yaw += 0.01f;
glFlush();
glutSwapBuffers();
}
void DemoApplication::renderme()
{
myinit();
updateCamera();
if (m_dynamicsWorld)
{
if(m_enableshadows)
{
glClear(GL_STENCIL_BUFFER_BIT);
glEnable(GL_CULL_FACE);
renderscene(0);
glDisable(GL_LIGHTING);
glDepthMask(GL_FALSE);
glDepthFunc(GL_LEQUAL);
glEnable(GL_STENCIL_TEST);
glColorMask(GL_FALSE,GL_FALSE,GL_FALSE,GL_FALSE);
glStencilFunc(GL_ALWAYS,1,0xFFFFFFFFL);
glFrontFace(GL_CCW);
glStencilOp(GL_KEEP,GL_KEEP,GL_INCR);
renderscene(1);
glFrontFace(GL_CW);
glStencilOp(GL_KEEP,GL_KEEP,GL_DECR);
renderscene(1);
glFrontFace(GL_CCW);
glPolygonMode(GL_FRONT,GL_FILL);
glPolygonMode(GL_BACK,GL_FILL);
glShadeModel(GL_SMOOTH);
glEnable(GL_DEPTH_TEST);
glDepthFunc(GL_LESS);
glEnable(GL_LIGHTING);
glDepthMask(GL_TRUE);
glCullFace(GL_BACK);
glFrontFace(GL_CCW);
glEnable(GL_CULL_FACE);
glColorMask(GL_TRUE,GL_TRUE,GL_TRUE,GL_TRUE);
glDepthFunc(GL_LEQUAL);
glStencilFunc( GL_NOTEQUAL, 0, 0xFFFFFFFFL );
glStencilOp( GL_KEEP, GL_KEEP, GL_KEEP );
glDisable(GL_LIGHTING);
renderscene(2);
glEnable(GL_LIGHTING);
glDepthFunc(GL_LESS);
glDisable(GL_STENCIL_TEST);
glDisable(GL_CULL_FACE);
}
else
{
glDisable(GL_CULL_FACE);
renderscene(0);
}
int xOffset = 10;
int yStart = 20;
int yIncr = 20;
glDisable(GL_LIGHTING);
glColor3f(0, 0, 0);
if ((m_debugMode & btIDebugDraw::DBG_NoHelpText)==0)
{
setOrthographicProjection();
showProfileInfo(xOffset,yStart,yIncr);
#ifdef USE_QUICKPROF
if ( getDebugMode() & btIDebugDraw::DBG_ProfileTimings)
{
static int counter = 0;
counter++;
std::map<std::string, hidden::ProfileBlock*>::iterator iter;
for (iter = btProfiler::mProfileBlocks.begin(); iter != btProfiler::mProfileBlocks.end(); ++iter)
{
char blockTime[128];
sprintf(blockTime, "%s: %lf",&((*iter).first[0]),btProfiler::getBlockTime((*iter).first, btProfiler::BLOCK_CYCLE_SECONDS));//BLOCK_TOTAL_PERCENT));
glRasterPos3f(xOffset,yStart,0);
GLDebugDrawString(BMF_GetFont(BMF_kHelvetica10),blockTime);
yStart += yIncr;
}
}
#endif //USE_QUICKPROF
resetPerspectiveProjection();
}
glEnable(GL_LIGHTING);
}
updateCamera();
}
//------------------------------------------------------------------------------
void GimpactConcaveDemo::renderme()
{
updateCamera();
btScalar m[16];
if (m_dynamicsWorld)
{
btVector3 worldBoundsMin,worldBoundsMax;
getDynamicsWorld()->getBroadphase()->getBroadphaseAabb(worldBoundsMin,worldBoundsMax);
int numObjects = m_dynamicsWorld->getNumCollisionObjects();
btVector3 wireColor(1,0,0);
for (int i=0;i<numObjects;i++)
{
btCollisionObject* colObj = m_dynamicsWorld->getCollisionObjectArray()[i];
btRigidBody* body = btRigidBody::upcast(colObj);
if (body && body->getMotionState())
{
btDefaultMotionState* myMotionState = (btDefaultMotionState*)body->getMotionState();
myMotionState->m_graphicsWorldTrans.getOpenGLMatrix(m);
} else
{
colObj->getWorldTransform().getOpenGLMatrix(m);
}
btVector3 wireColor(1.f,1.0f,0.5f); //wants deactivation
if (i & 1)
{
wireColor = btVector3(0.f,0.0f,1.f);
}
///color differently for active, sleeping, wantsdeactivation states
if (colObj->getActivationState() == 1) //active
{
if (i & 1)
{
wireColor += btVector3 (0.8f,0.1f,0.1f);
} else
{
wireColor += btVector3 (0.5f,0.f,0.f);
}
}
if (colObj->getActivationState() == 2) //ISLAND_SLEEPING
{
if (i & 1)
{
wireColor += btVector3 (0.5f,0.8f, 0.5f);
} else
{
wireColor += btVector3 (0.f,0.5f,0.f);
}
}
m_shapeDrawer->drawOpenGL(m,colObj->getCollisionShape(),wireColor,getDebugMode(),worldBoundsMin,worldBoundsMax);
}
float xOffset = 10.f;
float yStart = 20.f;
float yIncr = 20.f;
char buf[124];
glColor3f(0, 0, 0);
setOrthographicProjection();
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"mouse to interact");
GLDebugDrawString(xOffset,yStart,buf);
yStart += yIncr;
/* glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"space to reset");
GLDebugDrawString(xOffset,yStart,buf);
yStart += yIncr;
*/
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"cursor keys and z,x to navigate");
GLDebugDrawString(xOffset,yStart,buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"i to toggle simulation, s single step");
GLDebugDrawString(xOffset,yStart,buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"q to quit");
GLDebugDrawString(xOffset,yStart,buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,". to shoot TRIMESH (dot)");
GLDebugDrawString(xOffset,yStart,buf);
yStart += yIncr;
// not yet hooked up again after refactoring...
/* glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"d to toggle deactivation");
GLDebugDrawString(xOffset,yStart,buf);
yStart += yIncr;
*/
/*
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"a to draw temporal AABBs");
GLDebugDrawString(xOffset,yStart,buf);
yStart += yIncr;
*/
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"h to toggle help text");
GLDebugDrawString(xOffset,yStart,buf);
yStart += yIncr;
//bool useBulletLCP = !(getDebugMode() & btIDebugDraw::DBG_DisableBulletLCP);
bool useCCD = ((getDebugMode() & btIDebugDraw::DBG_EnableCCD) != 0);
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"1 CCD mode (adhoc) = %i",useCCD);
GLDebugDrawString(xOffset,yStart,buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"+- shooting speed = %10.2f",m_ShootBoxInitialSpeed);
GLDebugDrawString(xOffset,yStart,buf);
yStart += yIncr;
#ifdef SHOW_NUM_DEEP_PENETRATIONS
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"gNumDeepPenetrationChecks = %d",gNumDeepPenetrationChecks);
GLDebugDrawString(xOffset,yStart,buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"gNumSplitImpulseRecoveries= %d",gNumSplitImpulseRecoveries);
GLDebugDrawString(xOffset,yStart,buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"gNumGjkChecks= %d",gNumGjkChecks);
GLDebugDrawString(xOffset,yStart,buf);
yStart += yIncr;
#endif //SHOW_NUM_DEEP_PENETRATIONS
resetPerspectiveProjection();
}
}
void DoublePrecisionDemo::displayCallback(void)
{
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glDisable(GL_LIGHTING);
collisionWorld->getDispatchInfo().m_debugDraw = &debugDrawer;
if (collisionWorld)
collisionWorld->performDiscreteCollisionDetection();
int i;
btVector3 worldBoundsMin,worldBoundsMax;
collisionWorld->getBroadphase()->getBroadphaseAabb(worldBoundsMin,worldBoundsMax);
///one way to draw all the contact points is iterating over contact manifolds / points:
int numManifolds = collisionWorld->getDispatcher()->getNumManifolds();
for (i=0;i<numManifolds;i++)
{
btPersistentManifold* contactManifold = collisionWorld->getDispatcher()->getManifoldByIndexInternal(i);
btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());
contactManifold->refreshContactPoints(obA->getWorldTransform(),obB->getWorldTransform());
int numContacts = contactManifold->getNumContacts();
for (int j=0;j<numContacts;j++)
{
btManifoldPoint& pt = contactManifold->getContactPoint(j);
glBegin(GL_LINES);
glColor3f(1, 1, 1);
btVector3 ptA = pt.getPositionWorldOnA() - m_cameraPosition;
btVector3 ptB = pt.getPositionWorldOnB() - m_cameraPosition;
glVertex3d(ptA.x(),ptA.y(),ptA.z());
glVertex3d(ptB.x(),ptB.y(),ptB.z());
glEnd();
}
//you can un-comment out this line, and then all points are removed
//contactManifold->clearManifold();
}
btScalar m[16];
btTransform temp;
btVector3 color;
//int i;
for (i=0;i<numObjects;i++)
{
if (i % 2)
{
color = btVector3(1,0,0);
}
else
{
color = btVector3(0,0,1);
}
temp = objects[i].getWorldTransform();
temp.setOrigin(temp.getOrigin() - m_cameraPosition);
temp.getOpenGLMatrix( m );
m_shapeDrawer->drawOpenGL(m,objects[i].getCollisionShape(),color,getDebugMode(),worldBoundsMin,worldBoundsMax);
}
objects[1].getWorldTransform().setOrigin(objects[1].getWorldTransform().getOrigin()+btVector3(-VERY_SMALL_INCREMENT,-VERY_SMALL_INCREMENT,0));
objects[0].getWorldTransform().setOrigin(objects[0].getWorldTransform().getOrigin()+btVector3(VERY_SMALL_INCREMENT,VERY_SMALL_INCREMENT,0));
float yStart = 20.f;
float yIncr = 20.f;
char buf[124];
glColor3f(0, 0, 0);
setOrthographicProjection();
glRasterPos3f(10.0f,yStart,0);
#ifdef BT_USE_DOUBLE_PRECISION
GLDebugDrawString(10.f,yStart,"Double Precision Mode");
#else
GLDebugDrawString(10.f,yStart,"Single Precision Mode");
#endif
yStart += yIncr;
glRasterPos3f(10.0f,yStart,0);
sprintf(buf,"Movement distance in x and y axis = %lf", VERY_SMALL_INCREMENT);
GLDebugDrawString(10.f,yStart,buf);
yStart += yIncr;
glRasterPos3f(10.0f,yStart,0);
btScalar xValue = objects[0].getWorldTransform().getOrigin().x();
btScalar yValue = objects[0].getWorldTransform().getOrigin().y();
btScalar zValue = objects[0].getWorldTransform().getOrigin().z();
sprintf(buf,"Cube 0 location = ( %lf, %lf, %lf )", xValue, yValue, zValue);
GLDebugDrawString(10.f,yStart,buf);
yStart += yIncr;
xValue = objects[1].getWorldTransform().getOrigin().x();
yValue = objects[1].getWorldTransform().getOrigin().y();
zValue = objects[1].getWorldTransform().getOrigin().z();
glRasterPos3f(10.0f,yStart,0);
sprintf(buf,"Cube 1 location = ( %lf, %lf, %lf )", xValue, yValue, zValue);
GLDebugDrawString(10.f,yStart,buf);
yStart += yIncr;
glRasterPos3f(10.0f,yStart,0);
GLDebugDrawString(10.f,yStart,"w=toggle wireframe/solid");
resetPerspectiveProjection();
glFlush();
glutSwapBuffers();
}
void DemoApplication::renderscene(int pass)
{
btScalar m[16];
btMatrix3x3 rot;rot.setIdentity();
const int numObjects=m_dynamicsWorld->getNumCollisionObjects();
btVector3 wireColor(1,0,0);
for(int i=0;i<numObjects;i++)
{
btCollisionObject* colObj=m_dynamicsWorld->getCollisionObjectArray()[i];
btRigidBody* body=btRigidBody::upcast(colObj);
if(body&&body->getMotionState())
{
btDefaultMotionState* myMotionState = (btDefaultMotionState*)body->getMotionState();
myMotionState->m_graphicsWorldTrans.getOpenGLMatrix(m);
rot=myMotionState->m_graphicsWorldTrans.getBasis();
}
else
{
colObj->getWorldTransform().getOpenGLMatrix(m);
rot=colObj->getWorldTransform().getBasis();
}
btVector3 wireColor(1.f,1.0f,0.5f); //wants deactivation
if(i&1) wireColor=btVector3(0.f,0.0f,1.f);
///color differently for active, sleeping, wantsdeactivation states
if (colObj->getActivationState() == 1) //active
{
if (i & 1)
{
wireColor += btVector3 (1.f,0.f,0.f);
}
else
{
wireColor += btVector3 (.5f,0.f,0.f);
}
}
if(colObj->getActivationState()==2) //ISLAND_SLEEPING
{
if(i&1)
{
wireColor += btVector3 (0.f,1.f, 0.f);
}
else
{
wireColor += btVector3 (0.f,0.5f,0.f);
}
}
btVector3 aabbMin,aabbMax;
m_dynamicsWorld->getBroadphase()->getBroadphaseAabb(aabbMin,aabbMax);
aabbMin-=btVector3(BT_LARGE_FLOAT,BT_LARGE_FLOAT,BT_LARGE_FLOAT);
aabbMax+=btVector3(BT_LARGE_FLOAT,BT_LARGE_FLOAT,BT_LARGE_FLOAT);
// printf("aabbMin=(%f,%f,%f)\n",aabbMin.getX(),aabbMin.getY(),aabbMin.getZ());
// printf("aabbMax=(%f,%f,%f)\n",aabbMax.getX(),aabbMax.getY(),aabbMax.getZ());
// m_dynamicsWorld->getDebugDrawer()->drawAabb(aabbMin,aabbMax,btVector3(1,1,1));
if (!(getDebugMode()& btIDebugDraw::DBG_DrawWireframe))
{
switch(pass)
{
case 0: m_shapeDrawer->drawOpenGL(m,colObj->getCollisionShape(),wireColor,getDebugMode(),aabbMin,aabbMax);break;
case 1: m_shapeDrawer->drawShadow(m,m_sundirection*rot,colObj->getCollisionShape(),aabbMin,aabbMax);break;
case 2: m_shapeDrawer->drawOpenGL(m,colObj->getCollisionShape(),wireColor*btScalar(0.3),0,aabbMin,aabbMax);break;
}
}
}
}
void btContinuousConvexCollisionDemo::displayCallback(void) {
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glDisable(GL_LIGHTING);
//GL_ShapeDrawer::drawCoordSystem();
btScalar m[16];
int i;
btVector3 worldBoundsMin(-1000,-1000,-1000);
btVector3 worldBoundsMax(1000,1000,1000);
/*for (i=0;i<numObjects;i++)
{
fromTrans[i].getOpenGLMatrix( m );
m_shapeDrawer.drawOpenGL(m,shapePtr[i]);
}
*/
if (getDebugMode()==btIDebugDraw::DBG_DrawAabb)
{
i=0;//for (i=1;i<numObjects;i++)
{
//for each object, subdivide the from/to transform in 10 equal steps
int numSubSteps = 10;
for (int s=0;s<10;s++)
{
btScalar subStep = s * 1.f/(float)numSubSteps;
btTransform interpolatedTrans;
btTransformUtil::integrateTransform(fromTrans[i],linVels[i],angVels[i],subStep,interpolatedTrans);
//fromTrans[i].getOpenGLMatrix(m);
//m_shapeDrawer.drawOpenGL(m,shapePtr[i]);
//toTrans[i].getOpenGLMatrix(m);
//m_shapeDrawer.drawOpenGL(m,shapePtr[i]);
interpolatedTrans.getOpenGLMatrix( m );
m_shapeDrawer.drawOpenGL(m,shapePtr[i],btVector3(1,0,1),getDebugMode(),worldBoundsMin,worldBoundsMax);
}
}
}
btMatrix3x3 mat;
mat.setEulerZYX(yaw,pitch,roll);
btQuaternion orn;
mat.getRotation(orn);
orn.setEuler(yaw,pitch,roll);
fromTrans[1].setRotation(orn);
toTrans[1].setRotation(orn);
if (m_stepping || m_singleStep)
{
m_singleStep = false;
pitch += 0.005f;
// yaw += 0.01f;
}
// btVector3 fromA(-25,11,0);
// btVector3 toA(-15,11,0);
// btQuaternion ornFromA(0.f,0.f,0.f,1.f);
// btQuaternion ornToA(0.f,0.f,0.f,1.f);
// btTransform rayFromWorld(ornFromA,fromA);
// btTransform rayToWorld(ornToA,toA);
btTransform rayFromWorld = fromTrans[0];
btTransform rayToWorld = toTrans[0];
if (drawLine)
{
glBegin(GL_LINES);
glColor3f(0, 0, 1);
glVertex3d(rayFromWorld.getOrigin().x(), rayFromWorld.getOrigin().y(),rayFromWorld.getOrigin().z());
glVertex3d(rayToWorld.getOrigin().x(),rayToWorld.getOrigin().y(),rayToWorld.getOrigin().z());
glEnd();
}
//now perform a raycast on the shapes, in local (shape) space
gGjkSimplexSolver.reset();
//choose one of the following lines
for (i=0;i<numObjects;i++)
{
fromTrans[i].getOpenGLMatrix(m);
m_shapeDrawer.drawOpenGL(m,shapePtr[i],btVector3(1,1,1),getDebugMode(),worldBoundsMin,worldBoundsMax);
}
btDebugCastResult rayResult1(fromTrans[0],shapePtr[0],linVels[0],angVels[0],&m_shapeDrawer);
for (i=1;i<numObjects;i++)
{
btConvexCast::CastResult rayResult2;
btConvexCast::CastResult* rayResultPtr;
if (btIDebugDraw::DBG_DrawAabb)
{
rayResultPtr = &rayResult1;
} else
{
rayResultPtr = &rayResult2;
}
//GjkConvexCast convexCaster(&gGjkSimplexSolver);
//SubsimplexConvexCast convexCaster(&gGjkSimplexSolver);
//optional
btConvexPenetrationDepthSolver* penetrationDepthSolver = 0;
btContinuousConvexCollision convexCaster(shapePtr[0],shapePtr[i],&gGjkSimplexSolver,penetrationDepthSolver );
gGjkSimplexSolver.reset();
if (convexCaster.calcTimeOfImpact(fromTrans[0],toTrans[0],fromTrans[i] ,toTrans[i] ,*rayResultPtr))
{
glDisable(GL_DEPTH_TEST);
btTransform hitTrans;
btTransformUtil::integrateTransform(fromTrans[0],linVels[0],angVels[0],rayResultPtr->m_fraction,hitTrans);
hitTrans.getOpenGLMatrix(m);
m_shapeDrawer.drawOpenGL(m,shapePtr[0],btVector3(0,1,0),getDebugMode(),worldBoundsMin,worldBoundsMax);
btTransformUtil::integrateTransform(fromTrans[i],linVels[i],angVels[i],rayResultPtr->m_fraction,hitTrans);
hitTrans.getOpenGLMatrix(m);
m_shapeDrawer.drawOpenGL(m,shapePtr[i],btVector3(0,1,1),getDebugMode(),worldBoundsMin,worldBoundsMax);
}
}
glFlush();
glutSwapBuffers();
}
void LinearConvexCastDemo::displayCallback(void)
{
updateCamera();
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glDisable(GL_LIGHTING);
GL_ShapeDrawer::drawCoordSystem();
static btScalar angle = 0.f;
angle+=getDeltaTimeMicroseconds()/1000000.0;
tr[1].setRotation(btQuaternion(btVector3(1,0,0),angle));
btTransform toA, toB;
toA = tr[0];
toA.setOrigin( btVector3( 0.0f, 0.f, 0.0f ) );
toB = tr[1];
toB.setOrigin( btVector3( 0.0f, 0.0f, 0.0f ) );
gGjkSimplexSolver.reset();
btVector3 worldBoundsMin(-1000,-1000,-1000);
btVector3 worldBoundsMax(1000,1000,1000);
//btGjkConvexCast convexCaster(shapePtr[ 0 ], shapePtr[ 1 ], &gGjkSimplexSolver );
btSubsimplexConvexCast convexCaster( shapePtr[ 0 ], shapePtr[ 1 ], &gGjkSimplexSolver );
btConvexCast::CastResult result;
result.m_hitPoint.setValue(0,0,0);
convexCaster.calcTimeOfImpact( tr[ 0 ], toA, tr[ 1 ], toB, result );
btScalar m1[16], m2[16],m3[16];
tr[ 0 ].getOpenGLMatrix( m1 );
tr[ 1 ].getOpenGLMatrix( m2 );
btSphereShape sphere(0.2);
btTransform tmp = tr[0];
tmp.setOrigin(result.m_hitPoint);
tmp.getOpenGLMatrix(m3);
m_shapeDrawer.drawOpenGL( m3, &sphere, btVector3( 1, 0, 1 ), getDebugMode() ,worldBoundsMin,worldBoundsMax);
m_shapeDrawer.drawOpenGL( m1, shapePtr[ 0 ], btVector3( 1, 0, 0 ), getDebugMode() ,worldBoundsMin,worldBoundsMax);
m_shapeDrawer.drawOpenGL( m2, shapePtr[ 1 ], btVector3( 1, 0, 0 ), getDebugMode() ,worldBoundsMin,worldBoundsMax);
btVector3 originA, originB;
originA.setInterpolate3( tr[ 0 ].getOrigin(), toA.getOrigin(), result.m_fraction );
originB.setInterpolate3( tr[ 1 ].getOrigin(), toB.getOrigin(), result.m_fraction );
btTransform A = tr[ 0 ];
A.setOrigin( originA );
btTransform B = tr[ 1 ];
B.setOrigin( originB );
A.getOpenGLMatrix( m1 );
B.getOpenGLMatrix( m2 );
m_shapeDrawer.drawOpenGL( m1, shapePtr[ 0 ], btVector3( 1, 1, 0 ), getDebugMode() ,worldBoundsMin,worldBoundsMax);
m_shapeDrawer.drawOpenGL( m2, shapePtr[ 1 ], btVector3( 1, 1, 0 ), getDebugMode() ,worldBoundsMin,worldBoundsMax);
glFlush();
glutSwapBuffers();
}
void LinearConvexCastDemo::displayCallback(void)
{
updateCamera();
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glDisable(GL_LIGHTING);
//GL_ShapeDrawer::DrawCoordSystem();
float m[16];
int i;
for (i=0;i<numObjects;i++)
{
tr[i].getOpenGLMatrix( m );
GL_ShapeDrawer::DrawOpenGL(m,shapePtr[i],SimdVector3(1,1,1),getDebugMode());
}
int shapeIndex = 1;
SimdQuaternion orn;
orn.setEuler(yaw,pitch,roll);
tr[shapeIndex].setRotation(orn);
if (m_stepping || m_singleStep)
{
m_singleStep = false;
pitch += 0.005f;
yaw += 0.01f;
}
SimdVector3 fromA(-25,11,0);
SimdVector3 toA(15,11,0);
SimdQuaternion ornFromA(0.f,0.f,0.f,1.f);
SimdQuaternion ornToA(0.f,0.f,0.f,1.f);
SimdTransform rayFromWorld(ornFromA,fromA);
SimdTransform rayToWorld(ornToA,toA);
tr[0] = rayFromWorld;
if (drawLine)
{
glBegin(GL_LINES);
glColor3f(0, 0, 1);
glVertex3d(rayFromWorld.getOrigin().x(), rayFromWorld.getOrigin().y(),rayFromWorld.getOrigin().z());
glVertex3d(rayToWorld.getOrigin().x(),rayToWorld.getOrigin().y(),rayToWorld.getOrigin().z());
glEnd();
}
//now perform a raycast on the shapes, in local (shape) space
//choose one of the following lines
for (i=1;i<numObjects;i++)
{
ContinuousConvexCollision convexCaster0(shapePtr[0],shapePtr[i],&gGjkSimplexSolver,0);
GjkConvexCast convexCaster1(shapePtr[0],shapePtr[i],&gGjkSimplexSolver);
//BU_CollisionPair (algebraic version) is currently broken, will look into this
//BU_CollisionPair convexCaster2(shapePtr[0],shapePtr[i]);
SubsimplexConvexCast convexCaster3(shapePtr[0],shapePtr[i],&gGjkSimplexSolver);
gGjkSimplexSolver.reset();
ConvexCast::CastResult rayResult;
if (convexCaster3.calcTimeOfImpact(rayFromWorld,rayToWorld,tr[i],tr[i],rayResult))
{
glDisable(GL_DEPTH_TEST);
SimdVector3 hitPoint;
hitPoint.setInterpolate3(rayFromWorld.getOrigin(),rayToWorld.getOrigin(),rayResult.m_fraction);
//draw the raycast result
glBegin(GL_LINES);
glColor3f(1, 1, 1);
glVertex3d(rayFromWorld.getOrigin().x(), rayFromWorld.getOrigin().y(),rayFromWorld.getOrigin().z());
glVertex3d(hitPoint.x(),hitPoint.y(),hitPoint.z());
glEnd();
glEnable(GL_DEPTH_TEST);
SimdTransform toTransWorld;
toTransWorld = tr[0];
toTransWorld.setOrigin(hitPoint);
toTransWorld.getOpenGLMatrix( m );
GL_ShapeDrawer::DrawOpenGL(m,shapePtr[0],SimdVector3(0,1,1),getDebugMode());
}
}
glFlush();
glutSwapBuffers();
}
void InternalEdgeDemo::clientMoveAndDisplay()
{
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
float dt = getDeltaTimeMicroseconds() * 0.000001f;
if (m_animatedMesh)
{
static float offset=0.f;
offset+=0.01f;
// setVertexPositions(waveheight,offset);
#if 0 ///not currently supported, we need to update the btInternalTriangleInfoMap
int i;
int j;
btVector3 aabbMin(BT_LARGE_FLOAT,BT_LARGE_FLOAT,BT_LARGE_FLOAT);
btVector3 aabbMax(-BT_LARGE_FLOAT,-BT_LARGE_FLOAT,-BT_LARGE_FLOAT);
for ( i=NUM_VERTS_X/2-3;i<NUM_VERTS_X/2+2;i++)
{
for (j=NUM_VERTS_X/2-3;j<NUM_VERTS_Y/2+2;j++)
{
aabbMax.setMax(gVertices[i+j*NUM_VERTS_X]);
aabbMin.setMin(gVertices[i+j*NUM_VERTS_X]);
gVertices[i+j*NUM_VERTS_X].setValue((i-NUM_VERTS_X*0.5f)*TRIANGLE_SIZE,
0.f,
//waveheight*sinf((float)i+offset)*cosf((float)j+offset),
(j-NUM_VERTS_Y*0.5f)*TRIANGLE_SIZE);
aabbMin.setMin(gVertices[i+j*NUM_VERTS_X]);
aabbMax.setMax(gVertices[i+j*NUM_VERTS_X]);
}
}
trimeshShape->partialRefitTree(aabbMin,aabbMax);
#else
btVector3 aabbMin,aabbMax;
trimeshShape->getMeshInterface()->calculateAabbBruteForce(aabbMin,aabbMax);
trimeshShape->refitTree(aabbMin,aabbMax);
#endif
//for debugging: clear all contact points involving mesh proxy. Note: this is a slow/unoptimized operation.
//m_dynamicsWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(staticBody->getBroadphaseHandle(),getDynamicsWorld()->getDispatcher());
}
m_dynamicsWorld->stepSimulation(dt);
///enable one of the following to debug (render debug lines each frame)
//m_dynamicsWorld->stepSimulation(1./800.,0);
//m_dynamicsWorld->stepSimulation(1./60.,100,1./800.);
//m_dynamicsWorld->stepSimulation(1./60.,0);
int lineWidth=450;
int xStart = m_glutScreenWidth - lineWidth;
int yStart = 20;
#ifndef __QNX__
if((getDebugMode() & btIDebugDraw::DBG_DrawText)!=0)
{
setOrthographicProjection();
glDisable(GL_LIGHTING);
glColor3f(0, 0, 0);
char buf[124];
glRasterPos3f(xStart, yStart, 0);
if (enable)
{
sprintf(buf,"InternalEdgeUtility enabled");
} else
{
sprintf(buf,"InternalEdgeUtility disabled");
}
GLDebugDrawString(xStart,20,buf);
yStart+=20;
glRasterPos3f(xStart, yStart, 0);
sprintf(buf,"Press 'n' to toggle InternalEdgeUtility");
yStart+=20;
GLDebugDrawString(xStart,yStart,buf);
glRasterPos3f(xStart, yStart, 0);
resetPerspectiveProjection();
glEnable(GL_LIGHTING);
}
#endif
renderme();
//optional but useful: debug drawing
m_dynamicsWorld->debugDrawWorld();
glFlush();
swapBuffers();
}
void clientDisplay(void) {
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glDisable(GL_LIGHTING);
if (collisionWorld)
collisionWorld->PerformDiscreteCollisionDetection();
///one way to draw all the contact points is iterating over contact manifolds / points:
int i;
int numManifolds = collisionWorld->GetDispatcher()->GetNumManifolds();
for (i=0;i<numManifolds;i++)
{
PersistentManifold* contactManifold = collisionWorld->GetDispatcher()->GetManifoldByIndexInternal(i);
CollisionObject* obA = static_cast<CollisionObject*>(contactManifold->GetBody0());
CollisionObject* obB = static_cast<CollisionObject*>(contactManifold->GetBody1());
contactManifold->RefreshContactPoints(obA->m_worldTransform,obB->m_worldTransform);
int numContacts = contactManifold->GetNumContacts();
for (int j=0;j<numContacts;j++)
{
ManifoldPoint& pt = contactManifold->GetContactPoint(j);
glBegin(GL_LINES);
glColor3f(1, 0, 1);
SimdVector3 ptA = pt.GetPositionWorldOnA();
SimdVector3 ptB = pt.GetPositionWorldOnB();
glVertex3d(ptA.x(),ptA.y(),ptA.z());
glVertex3d(ptB.x(),ptB.y(),ptB.z());
glEnd();
}
}
//GL_ShapeDrawer::DrawCoordSystem();
float m[16];
for (i=0;i<numObjects;i++)
{
objects[i].m_worldTransform.getOpenGLMatrix( m );
GL_ShapeDrawer::DrawOpenGL(m,objects[i].m_collisionShape,SimdVector3(1,1,1),getDebugMode());
}
SimdQuaternion orn;
orn.setEuler(yaw,pitch,roll);
objects[1].m_worldTransform.setOrigin(objects[1].m_worldTransform.getOrigin()+SimdVector3(0,-0.01,0));
//objects[0].m_worldTransform.setRotation(orn);
pitch += 0.005f;
yaw += 0.01f;
glFlush();
glutSwapBuffers();
}
void ARTracker::processDebugRequest(cv::Mat &frameMat, cv::Mat &greyMat, cv::Mat &binaryMat, std::vector<std::vector<cv::Point> >& contours, std::vector<ARMarker>& potentialMarkers, std::vector<ARMarker>& detectedMarkers){
if( getDebugMode() == DebugMode_Greyscale || getDebugMode() == DebugMode_Blur ){
cv::cvtColor(greyMat, _wipMat, CV_GRAY2BGRA);
}
else if( getDebugMode() == DebugMode_Binarization ){
cv::cvtColor(binaryMat, _wipMat, CV_GRAY2BGRA);
}
else if( getDebugMode() == DebugMode_Contours ){
cv::cvtColor(binaryMat, _wipMat, CV_GRAY2BGRA);
cv::drawContours(_wipMat, contours, -1, cv::Scalar(0,0,255), 2);
}
else if( getDebugMode() == DebugMode_PotentialMarkers ){
cv::cvtColor(binaryMat, _wipMat, CV_GRAY2BGRA);
int fontFace = cv::FONT_HERSHEY_SCRIPT_SIMPLEX;
double fontScale = 0.3f;
for( int i=0; i<potentialMarkers.size(); i++ ){
for( int j=0; j<4; j++ ){
int idx = j;
int nextIdx = (j+1)%4;
cv::line(_wipMat, potentialMarkers[i].corners[idx], potentialMarkers[i].corners[nextIdx], cv::Scalar(0, 250, 250), 2.0f);
cv::Point textOrg = potentialMarkers[i].corners[idx];
char txt[1];
sprintf(txt,"%d", idx);
cv::putText(_wipMat, txt, textOrg, fontFace, fontScale, cv::Scalar(0,0,255), 1.2f,1);
}
}
}
else if( getDebugMode() == DebugMode_ProjectedPatterns ){
_wipMat = binaryMat.clone();
_wipMat.setTo(cv::Scalar(0));
cv::Mat homoMatrix;
cv::Mat warpedMat = cv::Mat(_perspectiveTransformSize.width, _perspectiveTransformSize.height, binaryMat.type());
cv::Mat warpedResizedMat;
for( int i=0; i<potentialMarkers.size(); i++ ){
ARMarker& marker = potentialMarkers[i];
// finds the transformation that maps our marker onto a flat plane (_perspectiveTarget)
homoMatrix = cv::getPerspectiveTransform(marker.corners, _perspectiveTransformTarget);
// now unwrap the image
cv::warpPerspective(binaryMat, warpedMat, homoMatrix, _perspectiveTransformSize);
float warpedResizedMatWidthAndHeight = MIN(MIN(marker.sizeFromTopLeft().width, marker.sizeFromTopLeft().height), _perspectiveTransformSize.width);
cv::resize(warpedMat, warpedResizedMat, cv::Size(warpedResizedMatWidthAndHeight,warpedResizedMatWidthAndHeight));
cv::Rect roi = cv::Rect(marker.corners[0].x + (marker.sizeFromTopLeft().width - warpedResizedMat.cols)/2, // left,
marker.corners[0].y + (marker.sizeFromTopLeft().height - warpedResizedMat.rows)/2, // top
warpedResizedMat.cols, // right
warpedResizedMat.rows // bottom
);
warpedResizedMat.copyTo(_wipMat.colRange(roi.x, roi.x + roi.width)
.rowRange(roi.y, roi.y + roi.height));
}
cv::cvtColor(_wipMat, _wipMat, CV_GRAY2BGRA);
}
else if( getDebugMode() == DebugMode_DetectedMarkers ){
cv::cvtColor(binaryMat, _wipMat, CV_GRAY2BGRA);
// identify detected markers by a green outline and label otherwise red
int fontFace = cv::FONT_HERSHEY_SCRIPT_SIMPLEX;
double fontScale = 0.3f;
for( int i=0; i<detectedMarkers.size(); i++ ){
cv::Point textOrg = potentialMarkers[i].corners[0];
char txt[1];
sprintf(txt,"%d", detectedMarkers[i].id);
cv::putText(_wipMat, txt, textOrg, fontFace, fontScale, cv::Scalar(0,0,255), 1.2f,1);
for( int j=0; j<4; j++ ){
int idx = j;
int nextIdx = (j+1)%4;
cv::line(_wipMat, potentialMarkers[i].corners[idx], potentialMarkers[i].corners[nextIdx], cv::Scalar(0, 250, 0), 2.0f);
}
}
for( int i=0; i<potentialMarkers.size(); i++ ){
if( potentialMarkers[i].id == -99 ){
continue;
}
for( int j=0; j<4; j++ ){
int idx = j;
int nextIdx = (j+1)%4;
cv::line(_wipMat, potentialMarkers[i].corners[idx], potentialMarkers[i].corners[nextIdx], cv::Scalar(0, 0, 250), 2.0f);
}
}
}
}
void bulletBaseApp::renderme()
{
myinit();
updateCamera();
if (m_dynamicsWorld)
{
if(m_enableshadows)
{
glClear(GL_STENCIL_BUFFER_BIT);
glEnable(GL_CULL_FACE);
renderscene(0);
glDisable(GL_LIGHTING);
glDepthMask(GL_FALSE);
glDepthFunc(GL_LEQUAL);
glEnable(GL_STENCIL_TEST);
glColorMask(GL_FALSE,GL_FALSE,GL_FALSE,GL_FALSE);
glStencilFunc(GL_ALWAYS,1,0xFFFFFFFFL);
glFrontFace(GL_CCW);
glStencilOp(GL_KEEP,GL_KEEP,GL_INCR);
renderscene(1);
glFrontFace(GL_CW);
glStencilOp(GL_KEEP,GL_KEEP,GL_DECR);
renderscene(1);
glFrontFace(GL_CCW);
glPolygonMode(GL_FRONT,GL_FILL);
glPolygonMode(GL_BACK,GL_FILL);
glShadeModel(GL_SMOOTH);
glEnable(GL_DEPTH_TEST);
glDepthFunc(GL_LESS);
glEnable(GL_LIGHTING);
glDepthMask(GL_TRUE);
glCullFace(GL_BACK);
glFrontFace(GL_CCW);
glEnable(GL_CULL_FACE);
glColorMask(GL_TRUE,GL_TRUE,GL_TRUE,GL_TRUE);
glDepthFunc(GL_LEQUAL);
glStencilFunc( GL_NOTEQUAL, 0, 0xFFFFFFFFL );
glStencilOp( GL_KEEP, GL_KEEP, GL_KEEP );
glDisable(GL_LIGHTING);
renderscene(2);
glEnable(GL_LIGHTING);
glDepthFunc(GL_LESS);
glDisable(GL_STENCIL_TEST);
glDisable(GL_CULL_FACE);
}
else
{
glDisable(GL_CULL_FACE);
renderscene(0);
}
int xOffset = 10;
int yStart = 20;
int yIncr = 20;
char buf[124];
glDisable(GL_LIGHTING);
glColor3f(0, 0, 0);
if ((m_debugMode & btIDebugDraw::DBG_NoHelpText)==0)
{
setOrthographicProjection();
showProfileInfo(xOffset,yStart,yIncr);
#ifdef USE_QUICKPROF
if ( getDebugMode() & btIDebugDraw::DBG_ProfileTimings)
{
static int counter = 0;
counter++;
std::map<std::string, hidden::ProfileBlock*>::iterator iter;
for (iter = btProfiler::mProfileBlocks.begin(); iter != btProfiler::mProfileBlocks.end(); ++iter)
{
char blockTime[128];
sprintf(blockTime, "%s: %lf",&((*iter).first[0]),btProfiler::getBlockTime((*iter).first, btProfiler::BLOCK_CYCLE_SECONDS));//BLOCK_TOTAL_PERCENT));
glRasterPos3f(xOffset,yStart,0);
GLDebugDrawString(BMF_GetFont(BMF_kHelvetica10),blockTime);
yStart += yIncr;
}
}
#endif //USE_QUICKPROF
sprintf(buf,"mouse to interact");
GLDebugDrawString(xOffset,yStart,buf);
yStart += yIncr;
sprintf(buf,"ALT + mouse to move camera");
GLDebugDrawString(xOffset,yStart,buf);
yStart += yIncr;
sprintf(buf,"space to reset");
GLDebugDrawString(xOffset,yStart,buf);
yStart += yIncr;
sprintf(buf,"cursor keys and z,x to navigate");
GLDebugDrawString(xOffset,yStart,buf);
yStart += yIncr;
sprintf(buf,"i to toggle simulation, s single step");
GLDebugDrawString(xOffset,yStart,buf);
yStart += yIncr;
sprintf(buf,"q to quit");
GLDebugDrawString(xOffset,yStart,buf);
yStart += yIncr;
sprintf(buf,". to shoot box");
GLDebugDrawString(xOffset,yStart,buf);
yStart += yIncr;
// not yet hooked up again after refactoring...
sprintf(buf,"d to toggle deactivation");
GLDebugDrawString(xOffset,yStart,buf);
yStart += yIncr;
sprintf(buf,"g to toggle mesh animation (ConcaveDemo)");
GLDebugDrawString(xOffset,yStart,buf);
yStart += yIncr;
sprintf(buf,"h to toggle help text");
GLDebugDrawString(xOffset,yStart,buf);
yStart += yIncr;
sprintf(buf,"o to toggle orthogonal/perspective view");
GLDebugDrawString(xOffset,yStart,buf);
yStart += yIncr;
//bool useBulletLCP = !(getDebugMode() & btIDebugDraw::DBG_DisableBulletLCP);
//bool useCCD = (getDebugMode() & btIDebugDraw::DBG_EnableCCD);
//glRasterPos3f(xOffset,yStart,0);
//sprintf(buf,"1 CCD mode (adhoc) = %i",useCCD);
//GLDebugDrawString(BMF_GetFont(BMF_kHelvetica10),buf);
//yStart += yIncr;
sprintf(buf,"+- shooting speed = %10.2f",m_ShootBoxInitialSpeed);
GLDebugDrawString(xOffset,yStart,buf);
yStart += yIncr;
#ifdef SHOW_NUM_DEEP_PENETRATIONS
sprintf(buf,"gNumDeepPenetrationChecks = %d",gNumDeepPenetrationChecks);
GLDebugDrawString(xOffset,yStart,buf);
yStart += yIncr;
sprintf(buf,"gNumGjkChecks= %d",gNumGjkChecks);
GLDebugDrawString(xOffset,yStart,buf);
yStart += yIncr;
sprintf(buf,"gNumClampedCcdMotions = %d",gNumClampedCcdMotions);
GLDebugDrawString(xOffset,yStart,buf);
yStart += yIncr;
sprintf(buf,"gNumSplitImpulseRecoveries= %d",gNumSplitImpulseRecoveries);
GLDebugDrawString(xOffset,yStart,buf);
yStart += yIncr;
sprintf(buf,"gNumAlignedAllocs = %d",gNumAlignedAllocs);
GLDebugDrawString(xOffset,yStart,buf);
yStart += yIncr;
sprintf(buf,"gNumAlignedFree= %d",gNumAlignedFree);
GLDebugDrawString(xOffset,yStart,buf);
yStart += yIncr;
sprintf(buf,"# alloc-free = %d",gNumAlignedAllocs-gNumAlignedFree);
GLDebugDrawString(xOffset,yStart,buf);
yStart += yIncr;
//enable BT_DEBUG_MEMORY_ALLOCATIONS define in Bullet/src/LinearMath/btAlignedAllocator.h for memory leak detection
#ifdef BT_DEBUG_MEMORY_ALLOCATIONS
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"gTotalBytesAlignedAllocs = %d",gTotalBytesAlignedAllocs);
GLDebugDrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
#endif //BT_DEBUG_MEMORY_ALLOCATIONS
if (getDynamicsWorld())
{
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"# objects = %d",getDynamicsWorld()->getNumCollisionObjects());
GLDebugDrawString(xOffset,yStart,buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"# pairs = %d",getDynamicsWorld()->getBroadphase()->getOverlappingPairCache()->getNumOverlappingPairs());
GLDebugDrawString(xOffset,yStart,buf);
yStart += yIncr;
sprintf(buf,"# hitPos = %f,%f,%f",gHitPos.getX(),gHitPos.getY(),gHitPos.getZ());
GLDebugDrawString(xOffset,yStart,buf);
yStart += yIncr;
}
#endif //SHOW_NUM_DEEP_PENETRATIONS
resetPerspectiveProjection();
}
glEnable(GL_LIGHTING);
}
updateCamera();
}
void CollisionInterfaceDemo::displayCallback(void) {
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glDisable(GL_LIGHTING);
collisionWorld->getDispatchInfo().m_debugDraw = &debugDrawer;
if (collisionWorld)
collisionWorld->performDiscreteCollisionDetection();
int i;
///one way to draw all the contact points is iterating over contact manifolds / points:
int numManifolds = collisionWorld->getDispatcher()->getNumManifolds();
for (i=0;i<numManifolds;i++)
{
btPersistentManifold* contactManifold = collisionWorld->getDispatcher()->getManifoldByIndexInternal(i);
btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());
int numContacts = contactManifold->getNumContacts();
for (int j=0;j<numContacts;j++)
{
btManifoldPoint& pt = contactManifold->getContactPoint(j);
glBegin(GL_LINES);
glColor3f(1, 0, 1);
btVector3 ptA = pt.getPositionWorldOnA();
btVector3 ptB = pt.getPositionWorldOnB();
glVertex3d(ptA.x(),ptA.y(),ptA.z());
glVertex3d(ptB.x(),ptB.y(),ptB.z());
glEnd();
}
//you can un-comment out this line, and then all points are removed
//contactManifold->clearManifold();
}
//GL_ShapeDrawer::drawCoordSystem();
btScalar m[16];
btVector3 worldBoundsMin,worldBoundsMax;
collisionWorld->getBroadphase()->getBroadphaseAabb(worldBoundsMin,worldBoundsMax);
for (i=0;i<numObjects;i++)
{
objects[i].getWorldTransform().getOpenGLMatrix( m );
m_shapeDrawer.drawOpenGL(m,objects[i].getCollisionShape(),btVector3(1,1,1),getDebugMode(),worldBoundsMin,worldBoundsMax);
}
btQuaternion orn;
orn.setEuler(yaw,pitch,roll);
objects[1].getWorldTransform().setOrigin(objects[1].getWorldTransform().getOrigin()+btVector3(0,-0.01,0));
objects[0].getWorldTransform().setRotation(orn);
pitch += 0.005f;
yaw += 0.01f;
glFlush();
glutSwapBuffers();
}
bool BazARTracker::init(int xsize, int ysize,
const std::string& bazar_config_name,
const std::string& camera_name)
{
/*OSGART*/
ARParam wparam;
// Set the initial camera parameters.
cparamName = camera_name;
if(arParamLoad((char*)cparamName.c_str(), 1, &wparam) < 0) {
std::cerr << "ERROR: Camera parameter load error." << std::endl;
return false;
}
arParamChangeSize(&wparam, xsize, ysize,&(m_cparam->cparam));
arInitCparam(&(m_cparam->cparam));
arParamDisp(&(m_cparam->cparam));
/*BAZAR*/
// load BazAR's configuration files and detector parameters
if (!loadBazARConfig(bazar_config_name, &bazconf)) exit(0);
// load BazAR's camera calibration file
if (!loadBazARCamParams((char*)(bazconf.camCalFileName),&bazconf)) exit(0);
// init bazar tracker
matCameraRT4_4 = cvCreateMat(4, 4, CV_64F); //64 bit double precision float
g_matIntrinsic = cvCreateMat(3, 3, CV_64F);
// output windows
if (getDebugMode()){
cvNamedWindow("Gray", CV_WINDOW_AUTOSIZE);
cvNamedWindow("Result_BAZAR", CV_WINDOW_AUTOSIZE);
}
// fine tuning for accuracy - careful!!
detector.ransac_dist_threshold = (float)bazconf.ransac_dist_threshold;
detector.max_ransac_iterations = (float)bazconf.max_ransac_iterations;
detector.non_linear_refine_threshold = (float)bazconf.non_linear_refine_threshold;
detector.match_score_threshold = (float)bazconf.match_score_threshold; // A lower threshold will allow detection in harder conditions, but
// might lead to false positives
// Train or load classifier
if(!detector.build_with_cache(
// hse25: no hard coded stuff :) -- jaja
(char*)(bazconf.modelFileName), // mode image file name
400, // maximum number of keypoints on the model
32, // patch size in pixels
3, // yape radius. Use 3,5 or 7.
16, // number of trees for the classifier. Somewhere between 12-50
3 // number of levels in the gaussian pyramid
))
{
cerr << "BazARTracker: Unable to load the model image " << (char*)(bazconf.modelFileName) <<" or its classifier.\n";
return false;
}
// set camera parameters for BAZAR
char *camCal = (char*)(bazconf.camCalFileName);
char *camExt = (char*)(bazconf.camExtFileName);
if(!augment.LoadOptimalStructureFromFile(camCal, camExt))
{
std::cerr << "BazARTracker: couldn't load camera parameters: " << camCal << " " << camExt << std::endl;
return false;
}
// image buffers needed for detection, conversion..
image = cvCreateImage(cvSize(xsize, ysize), IPL_DEPTH_8U, 4); // captured image
gray = cvCreateImage(cvSize(xsize, ysize), IPL_DEPTH_8U, 1); // detector input
display = cvCreateImage(cvSize(xsize, ysize), IPL_DEPTH_8U, 4); // debug
// use bazar's camera calibration
m_cparam->cparam.mat[0][0] = bazconf.camCalMatrix[0][0]; m_cparam->cparam.mat[0][1] = bazconf.camCalMatrix[0][1]; m_cparam->cparam.mat[0][2] = bazconf.camCalMatrix[0][2];
m_cparam->cparam.mat[1][0] = bazconf.camCalMatrix[1][0]; m_cparam->cparam.mat[1][1] = bazconf.camCalMatrix[1][1]; m_cparam->cparam.mat[1][2] = bazconf.camCalMatrix[1][2];
m_cparam->cparam.mat[2][0] = bazconf.camCalMatrix[2][0]; m_cparam->cparam.mat[2][1] = bazconf.camCalMatrix[2][1]; m_cparam->cparam.mat[2][2] = bazconf.camCalMatrix[2][2];
for(int i=0; i<3; i++)
{
for(int j=0; j<3; j++)
{
cvmSet(g_matIntrinsic, i, j, m_cparam->cparam.mat[i][j]);
}
}
// end BAZAR
setProjection(10.0f, 8000.0f);
setDebugMode(m_debugmode);
setupMarkers();
// Success
return true;
}
//to be implemented by the demo
void Hinge2Vehicle::renderScene()
{
m_guiHelper->syncPhysicsToGraphics(m_dynamicsWorld);
#if 0
for (int i=0;i<m_vehicle->getNumWheels();i++)
{
//synchronize the wheels with the (interpolated) chassis worldtransform
m_vehicle->updateWheelTransform(i,true);
CommonRenderInterface* renderer = m_guiHelper->getRenderInterface();
if (renderer)
{
btTransform tr = m_vehicle->getWheelInfo(i).m_worldTransform;
btVector3 pos=tr.getOrigin();
btQuaternion orn = tr.getRotation();
renderer->writeSingleInstanceTransformToCPU(pos,orn,m_wheelInstances[i]);
}
}
#endif
m_guiHelper->render(m_dynamicsWorld);
btVector3 wheelColor(1,0,0);
btVector3 worldBoundsMin,worldBoundsMax;
getDynamicsWorld()->getBroadphase()->getBroadphaseAabb(worldBoundsMin,worldBoundsMax);
#if 0
int lineWidth=400;
int xStart = m_glutScreenWidth - lineWidth;
int yStart = 20;
if((getDebugMode() & btIDebugDraw::DBG_NoHelpText)==0)
{
setOrthographicProjection();
glDisable(GL_LIGHTING);
glColor3f(0, 0, 0);
char buf[124];
sprintf(buf,"SHIFT+Cursor Left/Right - rotate lift");
GLDebugDrawString(xStart,20,buf);
yStart+=20;
sprintf(buf,"SHIFT+Cursor UP/Down - fork up/down");
yStart+=20;
GLDebugDrawString(xStart,yStart,buf);
if (m_useDefaultCamera)
{
sprintf(buf,"F5 - camera mode (free)");
} else
{
sprintf(buf,"F5 - camera mode (follow)");
}
yStart+=20;
GLDebugDrawString(xStart,yStart,buf);
yStart+=20;
if (m_dynamicsWorld->getConstraintSolver()->getSolverType()==BT_MLCP_SOLVER)
{
sprintf(buf,"F6 - solver (direct MLCP)");
} else
{
sprintf(buf,"F6 - solver (sequential impulse)");
}
GLDebugDrawString(xStart,yStart,buf);
btDiscreteDynamicsWorld* world = (btDiscreteDynamicsWorld*) m_dynamicsWorld;
if (world->getLatencyMotionStateInterpolation())
{
sprintf(buf,"F7 - motionstate interpolation (on)");
} else
{
sprintf(buf,"F7 - motionstate interpolation (off)");
}
yStart+=20;
GLDebugDrawString(xStart,yStart,buf);
sprintf(buf,"Click window for keyboard focus");
yStart+=20;
GLDebugDrawString(xStart,yStart,buf);
resetPerspectiveProjection();
glEnable(GL_LIGHTING);
}
#endif
}
//to be implemented by the demo
void renderme()
{
debugDrawer.SetDebugMode(getDebugMode());
//render the hinge axis
if (createConstraint)
{
SimdVector3 color(1,0,0);
SimdVector3 dirLocal(0,1,0);
SimdVector3 pivotInA(CUBE_HALF_EXTENTS,-CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS);
SimdVector3 pivotInB(-CUBE_HALF_EXTENTS,-CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS);
SimdVector3 from = physObjects[1]->GetRigidBody()->getCenterOfMassTransform()(pivotInA);
SimdVector3 fromB = physObjects[2]->GetRigidBody()->getCenterOfMassTransform()(pivotInB);
SimdVector3 dirWorldA = physObjects[1]->GetRigidBody()->getCenterOfMassTransform().getBasis() * dirLocal ;
SimdVector3 dirWorldB = physObjects[2]->GetRigidBody()->getCenterOfMassTransform().getBasis() * dirLocal ;
debugDrawer.DrawLine(from,from+dirWorldA,color);
debugDrawer.DrawLine(fromB,fromB+dirWorldB,color);
}
float m[16];
int i;
if (getDebugMode() & IDebugDraw::DBG_DisableBulletLCP)
{
//don't use Bullet, use quickstep
physicsEnvironmentPtr->setSolverType(0);
} else
{
//Bullet LCP solver
physicsEnvironmentPtr->setSolverType(1);
}
if (getDebugMode() & IDebugDraw::DBG_EnableCCD)
{
physicsEnvironmentPtr->setCcdMode(3);
} else
{
physicsEnvironmentPtr->setCcdMode(0);
}
bool isSatEnabled = (getDebugMode() & IDebugDraw::DBG_EnableSatComparison);
physicsEnvironmentPtr->EnableSatCollisionDetection(isSatEnabled);
#ifdef USE_HULL
//some testing code for SAT
if (isSatEnabled)
{
for (int s=0;s<numShapes;s++)
{
CollisionShape* shape = shapePtr[s];
if (shape->IsPolyhedral())
{
PolyhedralConvexShape* polyhedron = static_cast<PolyhedralConvexShape*>(shape);
if (!polyhedron->m_optionalHull)
{
//first convert vertices in 'Point3' format
int numPoints = polyhedron->GetNumVertices();
Point3* points = new Point3[numPoints+1];
//first 4 points should not be co-planar, so add central point to satisfy MakeHull
points[0] = Point3(0.f,0.f,0.f);
SimdVector3 vertex;
for (int p=0;p<numPoints;p++)
{
polyhedron->GetVertex(p,vertex);
points[p+1] = Point3(vertex.getX(),vertex.getY(),vertex.getZ());
}
Hull* hull = Hull::MakeHull(numPoints+1,points);
polyhedron->m_optionalHull = hull;
}
}
}
}
#endif //USE_HULL
for (i=0;i<numObjects;i++)
{
SimdTransform transA;
transA.setIdentity();
float pos[3];
float rot[4];
ms[i].getWorldPosition(pos[0],pos[1],pos[2]);
ms[i].getWorldOrientation(rot[0],rot[1],rot[2],rot[3]);
SimdQuaternion q(rot[0],rot[1],rot[2],rot[3]);
transA.setRotation(q);
SimdPoint3 dpos;
dpos.setValue(pos[0],pos[1],pos[2]);
transA.setOrigin( dpos );
transA.getOpenGLMatrix( m );
SimdVector3 wireColor(1.f,1.0f,0.5f); //wants deactivation
if (i & 1)
{
wireColor = SimdVector3(0.f,0.0f,1.f);
}
///color differently for active, sleeping, wantsdeactivation states
if (physObjects[i]->GetRigidBody()->GetActivationState() == 1) //active
{
if (i & 1)
{
wireColor += SimdVector3 (1.f,0.f,0.f);
} else
{
wireColor += SimdVector3 (.5f,0.f,0.f);
}
}
if (physObjects[i]->GetRigidBody()->GetActivationState() == 2) //ISLAND_SLEEPING
{
if (i & 1)
{
wireColor += SimdVector3 (0.f,1.f, 0.f);
} else
{
wireColor += SimdVector3 (0.f,0.5f,0.f);
}
}
char extraDebug[125];
sprintf(extraDebug,"islId, Body=%i , %i",physObjects[i]->GetRigidBody()->m_islandTag1,physObjects[i]->GetRigidBody()->m_debugBodyId);
physObjects[i]->GetRigidBody()->GetCollisionShape()->SetExtraDebugInfo(extraDebug);
GL_ShapeDrawer::DrawOpenGL(m,physObjects[i]->GetRigidBody()->GetCollisionShape(),wireColor,getDebugMode());
///this block is just experimental code to show some internal issues with replacing shapes on the fly.
if (getDebugMode()!=0 && (i>0))
{
if (physObjects[i]->GetRigidBody()->GetCollisionShape()->GetShapeType() == EMPTY_SHAPE_PROXYTYPE)
{
physObjects[i]->GetRigidBody()->SetCollisionShape(shapePtr[1]);
//remove the persistent collision pairs that were created based on the previous shape
BroadphaseProxy* bpproxy = physObjects[i]->GetRigidBody()->m_broadphaseHandle;
physicsEnvironmentPtr->GetBroadphase()->CleanProxyFromPairs(bpproxy);
SimdVector3 newinertia;
SimdScalar newmass = 10.f;
physObjects[i]->GetRigidBody()->GetCollisionShape()->CalculateLocalInertia(newmass,newinertia);
physObjects[i]->GetRigidBody()->setMassProps(newmass,newinertia);
physObjects[i]->GetRigidBody()->updateInertiaTensor();
}
}
}
if (!(getDebugMode() & IDebugDraw::DBG_NoHelpText))
{
float xOffset = 10.f;
float yStart = 20.f;
float yIncr = -2.f;
char buf[124];
glColor3f(0, 0, 0);
#ifdef USE_QUICKPROF
if ( getDebugMode() & IDebugDraw::DBG_ProfileTimings)
{
static int counter = 0;
counter++;
std::map<std::string, hidden::ProfileBlock*>::iterator iter;
for (iter = Profiler::mProfileBlocks.begin(); iter != Profiler::mProfileBlocks.end(); ++iter)
{
char blockTime[128];
sprintf(blockTime, "%s: %lf",&((*iter).first[0]),Profiler::getBlockTime((*iter).first, Profiler::BLOCK_CYCLE_SECONDS));//BLOCK_TOTAL_PERCENT));
glRasterPos3f(xOffset,yStart,0);
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),blockTime);
yStart += yIncr;
}
}
#endif //USE_QUICKPROF
//profiling << Profiler::createStatsString(Profiler::BLOCK_TOTAL_PERCENT);
//<< std::endl;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"mouse to interact");
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"space to reset");
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"cursor keys and z,x to navigate");
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"i to toggle simulation, s single step");
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"q to quit");
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"d to toggle deactivation");
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"a to draw temporal AABBs");
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"h to toggle help text");
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
bool useBulletLCP = !(getDebugMode() & IDebugDraw::DBG_DisableBulletLCP);
bool useCCD = (getDebugMode() & IDebugDraw::DBG_EnableCCD);
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"m Bullet GJK = %i",!isSatEnabled);
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"n Bullet LCP = %i",useBulletLCP);
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"1 CCD mode (adhoc) = %i",useCCD);
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
glRasterPos3f(xOffset,yStart,0);
sprintf(buf,"+- shooting speed = %10.2f",bulletSpeed);
BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
yStart += yIncr;
}
}
void CollisionInterfaceDemo::displayCallback(void) {
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glDisable(GL_LIGHTING);
btScalar m[16];
btVector3 worldBoundsMin,worldBoundsMax;
collisionWorld->getBroadphase()->getBroadphaseAabb(worldBoundsMin,worldBoundsMax);
int i;
for (i=0;i<numObjects;i++)
{
objects[i].getWorldTransform().getOpenGLMatrix( m );
m_shapeDrawer->drawOpenGL(m,objects[i].getCollisionShape(),btVector3(1,1,1),getDebugMode(),worldBoundsMin,worldBoundsMax);
}
collisionWorld->getDispatchInfo().m_debugDraw = &debugDrawer;
if (collisionWorld)
collisionWorld->performDiscreteCollisionDetection();
#ifndef TEST_NOT_ADDING_OBJECTS_TO_WORLD
collisionWorld->debugDrawWorld();
///one way to draw all the contact points is iterating over contact manifolds in the dispatcher:
int numManifolds = collisionWorld->getDispatcher()->getNumManifolds();
for (i=0;i<numManifolds;i++)
{
btPersistentManifold* contactManifold = collisionWorld->getDispatcher()->getManifoldByIndexInternal(i);
btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());
int numContacts = contactManifold->getNumContacts();
for (int j=0;j<numContacts;j++)
{
btManifoldPoint& pt = contactManifold->getContactPoint(j);
glBegin(GL_LINES);
glColor3f(0, 0, 0);
btVector3 ptA = pt.getPositionWorldOnA();
btVector3 ptB = pt.getPositionWorldOnB();
glVertex3d(ptA.x(),ptA.y(),ptA.z());
glVertex3d(ptB.x(),ptB.y(),ptB.z());
glEnd();
}
//you can un-comment out this line, and then all points are removed
//contactManifold->clearManifold();
}
#else
glDisable(GL_TEXTURE_2D);
for (i=0;i<numObjects;i++)
{
collisionWorld->debugDrawObject(objects[i].getWorldTransform(),objects[i].getCollisionShape(), btVector3(1,1,0));
}
btDrawingResult renderCallback;
//collisionWorld->contactPairTest(&objects[0],&objects[1], renderCallback);
collisionWorld->contactTest(&objects[0],renderCallback);
#if 0
//another way is to directly query the dispatcher for both objects. The objects don't need to be inserted into the world
btCollisionAlgorithm* algo = collisionWorld->getDispatcher()->findAlgorithm(&objects[0],&objects[1]);
btManifoldResult contactPointResult(&objects[0],&objects[1]);
algo->processCollision(&objects[0],&objects[1],collisionWorld->getDispatchInfo(),&contactPointResult);
btManifoldArray manifoldArray;
algo->getAllContactManifolds(manifoldArray);
int numManifolds = manifoldArray.size();
for (i=0;i<numManifolds;i++)
{
btPersistentManifold* contactManifold = manifoldArray[i];
btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
// btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());
glDisable(GL_DEPTH_TEST);
int numContacts = contactManifold->getNumContacts();
bool swap = obA == &objects[0];
for (int j=0;j<numContacts;j++)
{
btManifoldPoint& pt = contactManifold->getContactPoint(j);
glBegin(GL_LINES);
glColor3f(0, 0, 0);
btVector3 ptA = swap ?pt.getPositionWorldOnA():pt.getPositionWorldOnB();
btVector3 ptB = swap ? pt.getPositionWorldOnB():pt.getPositionWorldOnA();
glVertex3d(ptA.x(),ptA.y(),ptA.z());
glVertex3d(ptB.x(),ptB.y(),ptB.z());
glEnd();
}
//you can un-comment out this line, and then all points are removed
//contactManifold->clearManifold();
}
#endif
#endif
//GL_ShapeDrawer::drawCoordSystem();
btQuaternion qA = objects[0].getWorldTransform().getRotation();
btQuaternion qB = objects[1].getWorldTransform().getRotation();
if (!m_idle)
{
btScalar timeInSeconds = getDeltaTimeMicroseconds()/1000.f;
btQuaternion orn;
objects[0].getWorldTransform().getBasis().getEulerYPR(yaw,pitch,roll);
pitch += 0.00005f*timeInSeconds;
yaw += 0.0001f*timeInSeconds;
objects[0].getWorldTransform().getBasis().setEulerYPR(yaw,pitch,roll);
orn.setEuler(yaw,pitch,roll);
objects[1].getWorldTransform().setOrigin(objects[1].getWorldTransform().getOrigin()+btVector3(0,-0.00001*timeInSeconds,0));
//objects[0].getWorldTransform().setRotation(orn);
}
glFlush();
swapBuffers();
}
bool PhysicsWorld::IsDebugGeometryEnabled() const
{
return getDebugMode() != btIDebugDraw::DBG_NoDebug;
}
void CollisionDemo::displayCallback(void) {
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glDisable(GL_LIGHTING);
btVoronoiSimplexSolver sGjkSimplexSolver;
btGjkPairDetector convexConvex(shapePtr[0],shapePtr[1],&sGjkSimplexSolver,0);
btPointCollector gjkOutput;
btGjkPairDetector::ClosestPointInput input;
input.m_transformA = tr[0];
input.m_transformB = tr[1];
convexConvex.getClosestPoints(input, gjkOutput, 0);
if (gjkOutput.m_hasResult)
{
//VECCOPY(pa, gjkOutput.m_pointInWorld);
//VECCOPY(pb, gjkOutput.m_pointInWorld);
//VECADDFAC(pb, pb, gjkOutput.m_normalOnBInWorld, gjkOutput.m_distance);
printf("bullet: %10.10f\n", gjkOutput.m_distance); // = 0.24 => that's absolutely wrong!
btVector3 endPt = gjkOutput.m_pointInWorld +
gjkOutput.m_normalOnBInWorld*gjkOutput.m_distance;
glBegin(GL_LINES);
glColor3f(1, 0, 0);
glVertex3d(gjkOutput.m_pointInWorld.x(), gjkOutput.m_pointInWorld.y(),gjkOutput.m_pointInWorld.z());
glVertex3d(endPt.x(),endPt.y(),endPt.z());
//glVertex3d(gjkOutputm_pointInWorld.x(), gjkOutputm_pointInWorld.y(),gjkOutputm_pointInWorld.z());
//glVertex3d(gjkOutputm_pointInWorld.x(), gjkOutputm_pointInWorld.y(),gjkOutputm_pointInWorld.z());
glEnd();
}
//GL_ShapeDrawer::drawCoordSystem();
btScalar m[16];
int i;
// btGjkPairDetector convexConvex(shapePtr[0],shapePtr[1],&sGjkSimplexSolver,0);
convexConvex.getClosestPoints(input ,gjkOutput,0);
btVector3 worldBoundsMin(-1000,-1000,-1000);
btVector3 worldBoundsMax(1000,1000,1000);
for (i=0;i<numObjects;i++)
{
tr[i].getOpenGLMatrix( m );
m_shapeDrawer.drawOpenGL(m,shapePtr[i],btVector3(1,1,1),getDebugMode(),worldBoundsMin,worldBoundsMax);
}
simplex.setSimplexSolver(&sGjkSimplexSolver);
btVector3 ybuf[4],pbuf[4],qbuf[4];
int numpoints = sGjkSimplexSolver.getSimplex(pbuf,qbuf,ybuf);
simplex.reset();
for (i=0;i<numpoints;i++)
simplex.addVertex(ybuf[i]);
btTransform ident;
ident.setIdentity();
ident.getOpenGLMatrix(m);
m_shapeDrawer.drawOpenGL(m,&simplex,btVector3(1,1,1),getDebugMode(),worldBoundsMin,worldBoundsMax);
btQuaternion orn;
orn.setEuler(yaw,pitch,roll);
//let it rotate
//tr[0].setRotation(orn);
pitch += 0.005f;
yaw += 0.01f;
glFlush();
glutSwapBuffers();
}
void clientDisplay(void) {
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glDisable(GL_LIGHTING);
//GL_ShapeDrawer::DrawCoordSystem();
float m[16];
int i;
GjkPairDetector convexConvex(shapePtr[0],shapePtr[1],&sGjkSimplexSolver,0);
SimdVector3 seperatingAxis(0.00000000f,0.059727669f,0.29259586f);
convexConvex.SetCachedSeperatingAxis(seperatingAxis);
PointCollector gjkOutput;
GjkPairDetector::ClosestPointInput input;
input.m_transformA = tr[0];
input.m_transformB = tr[1];
convexConvex.GetClosestPoints(input ,gjkOutput,0);
if (gjkOutput.m_hasResult)
{
SimdVector3 endPt = gjkOutput.m_pointInWorld +
gjkOutput.m_normalOnBInWorld*gjkOutput.m_distance;
glBegin(GL_LINES);
glColor3f(1, 0, 0);
glVertex3d(gjkOutput.m_pointInWorld.x(), gjkOutput.m_pointInWorld.y(),gjkOutput.m_pointInWorld.z());
glVertex3d(endPt.x(),endPt.y(),endPt.z());
//glVertex3d(gjkOutputm_pointInWorld.x(), gjkOutputm_pointInWorld.y(),gjkOutputm_pointInWorld.z());
//glVertex3d(gjkOutputm_pointInWorld.x(), gjkOutputm_pointInWorld.y(),gjkOutputm_pointInWorld.z());
glEnd();
}
for (i=0; i<numObjects; i++)
{
tr[i].getOpenGLMatrix( m );
GL_ShapeDrawer::DrawOpenGL(m,shapePtr[i],SimdVector3(1,1,1),getDebugMode());
}
simplex.SetSimplexSolver(&sGjkSimplexSolver);
SimdPoint3 ybuf[4],pbuf[4],qbuf[4];
int numpoints = sGjkSimplexSolver.getSimplex(pbuf,qbuf,ybuf);
simplex.Reset();
for (i=0; i<numpoints; i++)
simplex.AddVertex(ybuf[i]);
SimdTransform ident;
ident.setIdentity();
ident.getOpenGLMatrix(m);
GL_ShapeDrawer::DrawOpenGL(m,&simplex,SimdVector3(1,1,1),getDebugMode());
SimdQuaternion orn;
orn.setEuler(yaw,pitch,roll);
tr[0].setRotation(orn);
// pitch += 0.005f;
// yaw += 0.01f;
glFlush();
glutSwapBuffers();
}