void btMultiBody_localFrameToWorld(btMultiBody* obj, int i, const btMatrix3x3* mat,
btMatrix3x3* value)
{
BTMATRIX3X3_IN(mat);
ATTRIBUTE_ALIGNED16(btMatrix3x3) temp = obj->localFrameToWorld(i, BTMATRIX3X3_USE(mat));
BTMATRIX3X3_OUT(value, &temp);
}
glm::mat4 toMat4(const btTransform& t)
{
glm::mat4 ATTRIBUTE_ALIGNED16(glm_mat);
t.getOpenGLMatrix(glm::value_ptr(glm_mat));
return glm_mat;
}
void drawCube (const btTransform& T)
{
ATTRIBUTE_ALIGNED16(btScalar) m[16];
T.getOpenGLMatrix (&m[0]);
glPushMatrix ();
#ifdef BT_USE_DOUBLE_PRECISION
glMultMatrixd (&m[0]);
glScaled (2.0 * boxShapeHalfExtents[0], 2.0 * boxShapeHalfExtents[1], 2.0 * boxShapeHalfExtents[2]);
#else
glMultMatrixf (&m[0]);
glScalef (2.0 * boxShapeHalfExtents[0], 2.0 * boxShapeHalfExtents[1], 2.0 * boxShapeHalfExtents[2]);
#endif //BT_USE_DOUBLE_PRECISION
#ifdef __QNX__
glBegin( GL_QUADS );
glNormal3f( 1.0, 0.0, 0.0);
glVertex3f(+0.5,-0.5,+0.5);
glVertex3f(+0.5,-0.5,-0.5);
glVertex3f(+0.5,+0.5,-0.5);
glVertex3f(+0.5,+0.5,+0.5);
glNormal3f( 0.0, 1.0, 0.0);
glVertex3f(+0.5,+0.5,+0.5);
glVertex3f(+0.5,+0.5,-0.5);
glVertex3f(-0.5,+0.5,-0.5);
glVertex3f(-0.5,+0.5,+0.5);
glNormal3f( 0.0, 0.0, 1.0);
glVertex3f(+0.5,+0.5,+0.5);
glVertex3f(-0.5,+0.5,+0.5);
glVertex3f(-0.5,-0.5,+0.5);
glVertex3f(+0.5,-0.5,+0.5);
glNormal3f(-1.0, 0.0, 0.0);
glVertex3f(-0.5,-0.5,+0.5);
glVertex3f(-0.5,+0.5,+0.5);
glVertex3f(-0.5,+0.5,-0.5);
glVertex3f(-0.5,-0.5,-0.5);
glNormal3f( 0.0,-1.0, 0.0);
glVertex3f(-0.5,-0.5,+0.5);
glVertex3f(-0.5,-0.5,-0.5);
glVertex3f(+0.5,-0.5,-0.5);
glVertex3f(+0.5,-0.5,+0.5);
glNormal3f( 0.0, 0.0,-1.0);
glVertex3f(-0.5,-0.5,-0.5);
glVertex3f(-0.5,+0.5,-0.5);
glVertex3f(+0.5,+0.5,-0.5);
glVertex3f(+0.5,-0.5,-0.5);
glEnd();
#else
glutSolidCube (1.0);
#endif
glPopMatrix ();
}
void drawCube (const btTransform& T)
{
ATTRIBUTE_ALIGNED16(btScalar) m[16];
T.getOpenGLMatrix (&m[0]);
glPushMatrix ();
#ifdef BT_USE_DOUBLE_PRECISION
glMultMatrixd (&m[0]);
glScaled (2.0 * boxShapeHalfExtents[0], 2.0 * boxShapeHalfExtents[1], 2.0 * boxShapeHalfExtents[2]);
#else
glMultMatrixf (&m[0]);
glScalef (2.0 * boxShapeHalfExtents[0], 2.0 * boxShapeHalfExtents[1], 2.0 * boxShapeHalfExtents[2]);
#endif //BT_USE_DOUBLE_PRECISION
glutSolidCube (1.0);
glPopMatrix ();
}
void btTransform_to_Matrix4(JNIEnv * const &jenv, jobject &target, const btTransform &source)
{
matrix4_ensurefields(jenv, target);
jfloatArray valArray = (jfloatArray) jenv->GetObjectField(target, matrix4_val);
jfloat * elements = jenv->GetFloatArrayElements(valArray, NULL);
ATTRIBUTE_ALIGNED16(btScalar dst[16]);
source.getOpenGLMatrix(dst);
memcpy(elements, dst, sizeof(btScalar)*16);
jenv->ReleaseFloatArrayElements(valArray, elements, 0);
jenv->DeleteLocalRef(valArray);
}
void btCompoundShape::updateChildTransform(int childIndex, const btTransform& newChildTransform)
{
m_children[childIndex].m_transform = newChildTransform;
if (m_dynamicAabbTree)
{
///update the dynamic aabb tree
btVector3 localAabbMin,localAabbMax;
m_children[childIndex].m_childShape->getAabb(newChildTransform,localAabbMin,localAabbMax);
ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax);
//int index = m_children.size()-1;
m_dynamicAabbTree->update(m_children[childIndex].m_node,bounds);
}
recalculateLocalAabb();
}
void btDbvtBroadphase::getBroadphaseAabb(btVector3& aabbMin,btVector3& aabbMax) const
{
ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds;
if(!m_sets[0].empty())
if(!m_sets[1].empty()) Merge( 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=btDbvtVolume::FromCR(btVector3(0,0,0),0);
aabbMin=bounds.Mins();
aabbMax=bounds.Maxs();
}
void btDbvtBroadphase::setAabb( btBroadphaseProxy* absproxy,
const btVector3& aabbMin,
const btVector3& aabbMax,
btDispatcher* /*dispatcher*/)
{
btDbvtProxy* proxy=(btDbvtProxy*)absproxy;
ATTRIBUTE_ALIGNED16(btDbvtVolume) aabb=btDbvtVolume::FromMM(aabbMin,aabbMax);
#if DBVT_BP_PREVENTFALSEUPDATE
if(NotEqual(aabb,proxy->leaf->volume))
#endif
{
bool docollide=false;
if(proxy->stage==STAGECOUNT)
{/* fixed -> dynamic set */
m_sets[1].remove(proxy->leaf);
proxy->leaf=
void btDbvtBroadphase::setAabbForceUpdate(btBroadphaseProxy* absproxy,
const btVector3& aabbMin,
const btVector3& aabbMax,
btDispatcher* /*dispatcher*/)
{
btDbvtProxy* proxy = (btDbvtProxy*)absproxy;
ATTRIBUTE_ALIGNED16(btDbvtVolume)
aabb = btDbvtVolume::FromMM(aabbMin, aabbMax);
bool docollide = false;
if (proxy->stage == STAGECOUNT)
{ /* fixed -> dynamic set */
m_sets[1].remove(proxy->leaf);
proxy->leaf = m_sets[0].insert(aabb, proxy);
docollide = true;
}
else
{ /* dynamic set */
++m_updates_call;
/* Teleporting */
m_sets[0].update(proxy->leaf, aabb);
++m_updates_done;
docollide = true;
}
listremove(proxy, m_stageRoots[proxy->stage]);
proxy->m_aabbMin = aabbMin;
proxy->m_aabbMax = aabbMax;
proxy->stage = m_stageCurrent;
listappend(proxy, m_stageRoots[m_stageCurrent]);
if (docollide)
{
m_needcleanup = true;
if (!m_deferedcollide)
{
btDbvtTreeCollider collider(this);
m_sets[1].collideTTpersistentStack(m_sets[1].m_root, proxy->leaf, collider);
m_sets[0].collideTTpersistentStack(m_sets[0].m_root, proxy->leaf, collider);
}
}
}
void btDbvtBroadphase::setAabb(btBroadphaseProxy* absproxy,
const btVector3& aabbMin,
const btVector3& aabbMax,
btDispatcher* /*dispatcher*/)
{
btDbvtProxy* proxy = (btDbvtProxy*)absproxy;
ATTRIBUTE_ALIGNED16(btDbvtVolume)
aabb = btDbvtVolume::FromMM(aabbMin, aabbMax);
#if DBVT_BP_PREVENTFALSEUPDATE
if (NotEqual(aabb, proxy->leaf->volume))
#endif
{
bool docollide = false;
if (proxy->stage == STAGECOUNT)
{ /* fixed -> dynamic set */
m_sets[1].remove(proxy->leaf);
proxy->leaf = m_sets[0].insert(aabb, proxy);
docollide = true;
}
else
{ /* dynamic set */
++m_updates_call;
if (Intersect(proxy->leaf->volume, aabb))
{ /* Moving */
const btVector3 delta = aabbMin - proxy->m_aabbMin;
btVector3 velocity(((proxy->m_aabbMax - proxy->m_aabbMin) / 2) * m_prediction);
if (delta[0] < 0) velocity[0] = -velocity[0];
if (delta[1] < 0) velocity[1] = -velocity[1];
if (delta[2] < 0) velocity[2] = -velocity[2];
if (
m_sets[0].update(proxy->leaf, aabb, velocity, gDbvtMargin)
)
{
++m_updates_done;
docollide = true;
}
}
else
{ /* Teleporting */
m_sets[0].update(proxy->leaf, aabb);
++m_updates_done;
docollide = true;
}
}
listremove(proxy, m_stageRoots[proxy->stage]);
proxy->m_aabbMin = aabbMin;
proxy->m_aabbMax = aabbMax;
proxy->stage = m_stageCurrent;
listappend(proxy, m_stageRoots[m_stageCurrent]);
if (docollide)
{
m_needcleanup = true;
if (!m_deferedcollide)
{
btDbvtTreeCollider collider(this);
m_sets[1].collideTTpersistentStack(m_sets[1].m_root, proxy->leaf, collider);
m_sets[0].collideTTpersistentStack(m_sets[0].m_root, proxy->leaf, collider);
}
}
}
}
void TinyRendererVisualShapeConverter::render(const float viewMat[16], const float projMat[16])
{
//clear the color buffer
TGAColor clearColor;
clearColor.bgra[0] = 255;
clearColor.bgra[1] = 255;
clearColor.bgra[2] = 255;
clearColor.bgra[3] = 255;
clearBuffers(clearColor);
ATTRIBUTE_ALIGNED16(btScalar modelMat[16]);
btVector3 lightDirWorld(-5,200,-40);
switch (m_data->m_upAxis)
{
case 1:
lightDirWorld = btVector3(-50.f,100,30);
break;
case 2:
lightDirWorld = btVector3(-50.f,30,100);
break;
default:{}
};
lightDirWorld.normalize();
// printf("num m_swRenderInstances = %d\n", m_data->m_swRenderInstances.size());
for (int i=0;i<m_data->m_swRenderInstances.size();i++)
{
TinyRendererObjectArray** visualArrayPtr = m_data->m_swRenderInstances.getAtIndex(i);
if (0==visualArrayPtr)
continue;//can this ever happen?
TinyRendererObjectArray* visualArray = *visualArrayPtr;
btHashPtr colObjHash = m_data->m_swRenderInstances.getKeyAtIndex(i);
const btCollisionObject* colObj = (btCollisionObject*) colObjHash.getPointer();
for (int v=0;v<visualArray->m_renderObjects.size();v++)
{
TinyRenderObjectData* renderObj = visualArray->m_renderObjects[v];
//sync the object transform
const btTransform& tr = colObj->getWorldTransform();
tr.getOpenGLMatrix(modelMat);
for (int i=0;i<4;i++)
{
for (int j=0;j<4;j++)
{
renderObj->m_projectionMatrix[i][j] = projMat[i+4*j];
renderObj->m_modelMatrix[i][j] = modelMat[i+4*j];
renderObj->m_viewMatrix[i][j] = viewMat[i+4*j];
renderObj->m_localScaling = colObj->getCollisionShape()->getLocalScaling();
renderObj->m_lightDirWorld = lightDirWorld;
}
}
TinyRenderer::renderObject(*renderObj);
}
}
//printf("write tga \n");
//m_data->m_rgbColorBuffer.write_tga_file("camera.tga");
// printf("flipped!\n");
m_data->m_rgbColorBuffer.flip_vertically();
//flip z-buffer
{
int half = m_data->m_swHeight>>1;
for (int j=0; j<half; j++)
{
unsigned long l1 = j*m_data->m_swWidth;
unsigned long l2 = (m_data->m_swHeight-1-j)*m_data->m_swWidth;
for (int i=0;i<m_data->m_swWidth;i++)
{
btSwap(m_data->m_depthBuffer[l1+i],m_data->m_depthBuffer[l2+i]);
}
}
}
}
void btDbvtAabbMm_Lengths(btDbvtAabbMm* obj, btVector3* value)
{
ATTRIBUTE_ALIGNED16(btVector3) temp = obj->Lengths();
BTVECTOR3_SET(value, temp);
}
void btConvexHullShape_getScaledPoint(btConvexHullShape* obj, int i, btVector3* value)
{
ATTRIBUTE_ALIGNED16(btVector3) temp = obj->getScaledPoint(i);
BTVECTOR3_SET(value, temp);
}
void btGeneric6DofConstraint_getAxis(btGeneric6DofConstraint* obj, int axis_index,
btVector3* value)
{
ATTRIBUTE_ALIGNED16(btVector3) temp = obj->getAxis(axis_index);
BTVECTOR3_SET(value, temp);
}
void btMultiBody_getBaseVel(btMultiBody* obj, btVector3* value)
{
ATTRIBUTE_ALIGNED16(btVector3) temp = obj->getBaseVel();
BTVECTOR3_SET(value, temp);
}
//to be implemented by the demo
void ForkLiftDemo::renderme()
{
updateCamera();
ATTRIBUTE_ALIGNED16(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;
#ifndef __QNX__
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);
}
#endif
DemoApplication::renderme();
}
//to be implemented by the demo
void ForkLiftDemo::renderScene()
{
m_guiHelper->syncPhysicsToGraphics(m_dynamicsWorld);
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]);
}
}
m_guiHelper->render(m_dynamicsWorld);
ATTRIBUTE_ALIGNED16(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);
}
#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
}
void clientDisplay(void) {
updateCamera();
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glDisable(GL_LIGHTING);
//GL_ShapeDrawer::drawCoordSystem();
ATTRIBUTE_ALIGNED16(btScalar) m[16];
int i;
#ifdef USE_GJK
btGjkEpaPenetrationDepthSolver epa;
btGjkPairDetector convexConvex(shapePtr[0],shapePtr[1],&sGjkSimplexSolver,&epa);
btVector3 seperatingAxis(0.00000000f,0.059727669f,0.29259586f);
convexConvex.setCachedSeperatingAxis(seperatingAxis);
btPointCollector gjkOutput;
btGjkPairDetector::ClosestPointInput input;
input.m_transformA = tr[0];
input.m_transformB = tr[1];
convexConvex.getClosestPoints(input ,gjkOutput,0);
if (gjkOutput.m_hasResult)
{
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());
glEnd();
}
#else //USE_GJK
struct MyContactResultCallback : public btCollisionWorld::ContactResultCallback
{
virtual btScalar addSingleResult(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap,int partId0,int index0,const btCollisionObjectWrapper* colObj1Wrap,int partId1,int index1)
{
glBegin(GL_LINES);
glColor3f(1, 0, 0);
glVertex3d(cp.m_positionWorldOnA.getX(),cp.m_positionWorldOnA.getY(),cp.m_positionWorldOnA.getZ());
glVertex3d(cp.m_positionWorldOnB.getX(),cp.m_positionWorldOnB.getY(),cp.m_positionWorldOnB.getZ());
glEnd();
return 1.f;
}
};
btDefaultCollisionConfiguration collisionConfiguration;
btCollisionDispatcher dispatcher(&collisionConfiguration);
btDbvtBroadphase pairCache;
btCollisionWorld world (&dispatcher,&pairCache,&collisionConfiguration);
gContactBreakingThreshold=1e10f;
MyContactResultCallback result;
btCollisionObject obA;
obA.setCollisionShape(shapePtr[0]);
obA.setWorldTransform(tr[0]);
btCollisionObject obB;
obB.setCollisionShape(shapePtr[1]);
obB.setWorldTransform(tr[1]);
world.contactPairTest(&obA,&obB,result);
#endif//USE_GJK
btVector3 worldMin(-1000,-1000,-1000);
btVector3 worldMax(1000,1000,1000);
for (i=0;i<numObjects;i++)
{
tr[i].getOpenGLMatrix( m );
if (debugMode)
{
/// for polyhedral shapes
if (shapePtr[i]->isPolyhedral())
{
if (!shapePtr[i]->getUserPointer())
{
btConvexHullComputer* convexUtil = new btConvexHullComputer();
shapePtr[i]->setUserPointer(convexUtil);
btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shapePtr[i];
btAlignedObjectArray<btVector3> vertices;
vertices.resize(polyshape->getNumVertices());
for (int i=0;i<polyshape->getNumVertices();i++)
{
polyshape->getVertex(i,vertices[i]);
}
bool useDoublePrecision = false;
convexUtil->compute(&vertices[0].getX(),sizeof(btVector3), polyshape->getNumVertices(),0,0);
}
if (shapePtr[i]->getUserPointer())
{
btConvexHullComputer* convexUtil = (btConvexHullComputer*)shapePtr[i]->getUserPointer();
//printf("num faces = %d\n",convexUtil->faces.size());
for (int j=0;j<convexUtil->faces.size();j++)
{
int face = convexUtil->faces[j];
//printf("face=%d\n",face);
const btConvexHullComputer::Edge* firstEdge = &convexUtil->edges[face];
const btConvexHullComputer::Edge* edge = firstEdge;
do
{
int src = edge->getSourceVertex();
int targ = edge->getTargetVertex();
//printf("src=%d target = %d\n", src,targ);
btVector3 wa = tr[i] * convexUtil->vertices[src];
btVector3 wb = tr[i] * convexUtil->vertices[targ];
glBegin(GL_LINES);
glColor3f(1, 1, 1);
glVertex3f(wa.getX(),wa.getY(),wa.getZ());
glVertex3f(wb.getX(),wb.getY(),wb.getZ());
glEnd();
edge = edge->getNextEdgeOfFace();
} while (edge!=firstEdge);
}
}
}
} else
{
shapeDrawer.drawOpenGL(m,shapePtr[i],btVector3(1,1,1),debugMode, worldMin, worldMax);
}
}
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);
shapeDrawer.drawOpenGL(m,&simplex,btVector3(1,1,1),debugMode, worldMin,worldMax);
btQuaternion orn;
orn.setEuler(yaw,pitch,roll);
tr[0].setRotation(orn);
tr[1].setRotation(orn);
pitch += 0.005f;
yaw += 0.01f;
glFlush();
glutSwapBuffers();
}
void OpenGL2Renderer::renderscene(int pass, int numObjects, btCollisionObject** objArray)
{
GLint err = glGetError();
btAssert(err==GL_NO_ERROR);
ATTRIBUTE_ALIGNED16(btScalar) m[16];
btMatrix3x3 rot;rot.setIdentity();
btVector3 wireColor(1,0,0);
for(int i=0;i<numObjects;i++)
{
const btCollisionObject* colObj=objArray[i];
const 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);
}
}
err = glGetError();
btAssert(err==GL_NO_ERROR);
btVector3 aabbMin(-BT_LARGE_FLOAT,-BT_LARGE_FLOAT,-BT_LARGE_FLOAT);
btVector3 aabbMax(BT_LARGE_FLOAT,BT_LARGE_FLOAT,BT_LARGE_FLOAT);
//world->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;
}
}
}
}
virtual void render(const btDiscreteDynamicsWorld* rbWorld)
{
OpenGLGuiHelper::render(rbWorld);
//clear the color buffer
TGAColor clearColor;
clearColor.bgra[0] = 255;
clearColor.bgra[1] = 255;
clearColor.bgra[2] = 255;
clearColor.bgra[3] = 255;
clearBuffers(clearColor);
ATTRIBUTE_ALIGNED16(btScalar modelMat[16]);
ATTRIBUTE_ALIGNED16(float viewMat[16]);
ATTRIBUTE_ALIGNED16(float projMat[16]);
CommonRenderInterface* render = getRenderInterface();
render->getActiveCamera()->getCameraProjectionMatrix(projMat);
render->getActiveCamera()->getCameraViewMatrix(viewMat);
btVector3 lightDirWorld(-5, 200, -40);
switch (1) //app->getUpAxis())
{
case 1:
lightDirWorld = btVector3(-50.f, 100, 30);
break;
case 2:
lightDirWorld = btVector3(-50.f, 30, 100);
break;
default:
{
}
};
lightDirWorld.normalize();
for (int i = 0; i < rbWorld->getNumCollisionObjects(); i++)
{
btCollisionObject* colObj = rbWorld->getCollisionObjectArray()[i];
int colObjIndex = colObj->getUserIndex();
int shapeIndex = colObj->getCollisionShape()->getUserIndex();
if (colObjIndex >= 0 && shapeIndex >= 0)
{
TinyRenderObjectData* renderObj = 0;
int* cptr = m_swInstances[colObjIndex];
if (cptr)
{
int c = *cptr;
TinyRenderObjectData** sptr = m_swRenderObjects[c];
if (sptr)
{
renderObj = *sptr;
//sync the object transform
const btTransform& tr = colObj->getWorldTransform();
tr.getOpenGLMatrix(modelMat);
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
renderObj->m_projectionMatrix[i][j] = projMat[i + 4 * j];
renderObj->m_modelMatrix[i][j] = modelMat[i + 4 * j];
renderObj->m_viewMatrix[i][j] = viewMat[i + 4 * j];
}
}
renderObj->m_localScaling = colObj->getCollisionShape()->getLocalScaling();
renderObj->m_lightDirWorld = lightDirWorld;
renderObj->m_lightAmbientCoeff = 0.6;
renderObj->m_lightDiffuseCoeff = 0.35;
renderObj->m_lightSpecularCoeff = 0.05;
TinyRenderer::renderObject(*renderObj);
}
}
}
}
for (int y = 0; y < m_swHeight; ++y)
{
unsigned char* pi = m_image + (y)*m_swWidth * 3;
for (int x = 0; x < m_swWidth; ++x)
{
const TGAColor& color = getFrameBuffer().get(x, y);
pi[0] = color.bgra[2];
pi[1] = color.bgra[1];
pi[2] = color.bgra[0];
pi += 3;
}
}
render->activateTexture(m_textureHandle);
render->updateTexture(m_textureHandle, m_image);
static int counter = 0;
counter++;
if ((counter & 7) == 0)
{
char filename[1024];
sprintf(filename, "framebuf%d.tga", counter);
getFrameBuffer().write_tga_file(filename, true);
}
float color[4] = {1, 1, 1, 1};
m_primRenderer->drawTexturedRect(0, 0, m_swWidth, m_swHeight, color, 0, 0, 1, 1, true);
}
void btDynamicsWorld_getGravity(btDynamicsWorld* obj, btVector3* value)
{
ATTRIBUTE_ALIGNED16(btVector3) temp = obj->getGravity();
BTVECTOR3_SET(value, temp);
}
void btDbvtBroadphase::collide(btDispatcher* dispatcher)
{
/*printf("---------------------------------------------------------\n");
printf("m_sets[0].m_leaves=%d\n",m_sets[0].m_leaves);
printf("m_sets[1].m_leaves=%d\n",m_sets[1].m_leaves);
printf("numPairs = %d\n",getOverlappingPairCache()->getNumOverlappingPairs());
{
int i;
for (i=0;i<getOverlappingPairCache()->getNumOverlappingPairs();i++)
{
printf("pair[%d]=(%d,%d),",i,getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy0->getUid(),
getOverlappingPairCache()->getOverlappingPairArray()[i].m_pProxy1->getUid());
}
printf("\n");
}
*/
SPC(m_profiling.m_total);
/* optimize */
m_sets[0].optimizeIncremental(1 + (m_sets[0].m_leaves * m_dupdates) / 100);
if (m_fixedleft)
{
const int count = 1 + (m_sets[1].m_leaves * m_fupdates) / 100;
m_sets[1].optimizeIncremental(1 + (m_sets[1].m_leaves * m_fupdates) / 100);
m_fixedleft = btMax<int>(0, m_fixedleft - count);
}
/* dynamic -> fixed set */
m_stageCurrent = (m_stageCurrent + 1) % STAGECOUNT;
btDbvtProxy* current = m_stageRoots[m_stageCurrent];
if (current)
{
#if DBVT_BP_ACCURATESLEEPING
btDbvtTreeCollider collider(this);
#endif
do
{
btDbvtProxy* next = current->links[1];
listremove(current, m_stageRoots[current->stage]);
listappend(current, m_stageRoots[STAGECOUNT]);
#if DBVT_BP_ACCURATESLEEPING
m_paircache->removeOverlappingPairsContainingProxy(current, dispatcher);
collider.proxy = current;
btDbvt::collideTV(m_sets[0].m_root, current->aabb, collider);
btDbvt::collideTV(m_sets[1].m_root, current->aabb, collider);
#endif
m_sets[0].remove(current->leaf);
ATTRIBUTE_ALIGNED16(btDbvtVolume)
curAabb = btDbvtVolume::FromMM(current->m_aabbMin, current->m_aabbMax);
current->leaf = m_sets[1].insert(curAabb, current);
current->stage = STAGECOUNT;
current = next;
} while (current);
m_fixedleft = m_sets[1].m_leaves;
m_needcleanup = true;
}
/* collide dynamics */
{
btDbvtTreeCollider collider(this);
if (m_deferedcollide)
{
SPC(m_profiling.m_fdcollide);
m_sets[0].collideTTpersistentStack(m_sets[0].m_root, m_sets[1].m_root, collider);
}
if (m_deferedcollide)
{
SPC(m_profiling.m_ddcollide);
m_sets[0].collideTTpersistentStack(m_sets[0].m_root, m_sets[0].m_root, collider);
}
}
/* clean up */
if (m_needcleanup)
{
SPC(m_profiling.m_cleanup);
btBroadphasePairArray& pairs = m_paircache->getOverlappingPairArray();
if (pairs.size() > 0)
{
int ni = btMin(pairs.size(), btMax<int>(m_newpairs, (pairs.size() * m_cupdates) / 100));
for (int i = 0; i < ni; ++i)
{
btBroadphasePair& p = pairs[(m_cid + i) % pairs.size()];
btDbvtProxy* pa = (btDbvtProxy*)p.m_pProxy0;
btDbvtProxy* pb = (btDbvtProxy*)p.m_pProxy1;
if (!Intersect(pa->leaf->volume, pb->leaf->volume))
{
#if DBVT_BP_SORTPAIRS
if (pa->m_uniqueId > pb->m_uniqueId)
btSwap(pa, pb);
#endif
m_paircache->removeOverlappingPair(pa, pb, dispatcher);
--ni;
--i;
}
}
if (pairs.size() > 0)
m_cid = (m_cid + ni) % pairs.size();
else
m_cid = 0;
}
}
++m_pid;
m_newpairs = 1;
m_needcleanup = false;
if (m_updates_call > 0)
{
m_updates_ratio = m_updates_done / (btScalar)m_updates_call;
}
else
{
m_updates_ratio = 0;
}
m_updates_done /= 2;
m_updates_call /= 2;
}
void btMultiBody_getAngularMomentum(btMultiBody* obj, btVector3* value)
{
ATTRIBUTE_ALIGNED16(btVector3) temp = obj->getAngularMomentum();
BTVECTOR3_SET(value, temp);
}
void btCollisionShape_getAnisotropicRollingFrictionDirection(btCollisionShape* obj,
btVector3* value)
{
ATTRIBUTE_ALIGNED16(btVector3) temp = obj->getAnisotropicRollingFrictionDirection();
BTVECTOR3_SET(value, temp);
}
void btMultiBody_getBaseWorldTransform(btMultiBody* obj, btTransform* value)
{
ATTRIBUTE_ALIGNED16(btTransform) temp = obj->getBaseWorldTransform();
BTTRANSFORM_SET(value, temp);
}
virtual void render(const btDiscreteDynamicsWorld* rbWorld)
{
//clear the color buffer
TGAColor clearColor;
clearColor.bgra[0] = 255;
clearColor.bgra[1] = 255;
clearColor.bgra[2] = 255;
clearColor.bgra[3] = 255;
clearBuffers(clearColor);
ATTRIBUTE_ALIGNED16(btScalar modelMat[16]);
ATTRIBUTE_ALIGNED16(float viewMat[16]);
ATTRIBUTE_ALIGNED16(float projMat[16]);
m_camera.getCameraProjectionMatrix(projMat);
m_camera.getCameraViewMatrix(viewMat);
btVector3 lightDirWorld(-5,200,-40);
switch (m_upAxis)
{
case 1:
lightDirWorld = btVector3(-50.f,100,30);
break;
case 2:
lightDirWorld = btVector3(-50.f,30,100);
break;
default:{}
};
lightDirWorld.normalize();
for (int i=0;i<rbWorld->getNumCollisionObjects();i++)
{
btCollisionObject* colObj = rbWorld->getCollisionObjectArray()[i];
int colObjIndex = colObj->getUserIndex();
int shapeIndex = colObj->getCollisionShape()->getUserIndex();
if (colObjIndex>=0 && shapeIndex>=0)
{
TinyRenderObjectData* renderObj = 0;
int* cptr = m_swInstances[colObjIndex];
if (cptr)
{
int c = *cptr;
TinyRenderObjectData** sptr = m_swRenderObjects[c];
if (sptr)
{
renderObj = *sptr;
//sync the object transform
const btTransform& tr = colObj->getWorldTransform();
tr.getOpenGLMatrix(modelMat);
for (int i=0;i<4;i++)
{
for (int j=0;j<4;j++)
{
renderObj->m_projectionMatrix[i][j] = projMat[i+4*j];
renderObj->m_modelMatrix[i][j] = modelMat[i+4*j];
renderObj->m_viewMatrix[i][j] = viewMat[i+4*j];
renderObj->m_localScaling = colObj->getCollisionShape()->getLocalScaling();
renderObj->m_lightDirWorld = lightDirWorld;
}
}
TinyRenderer::renderObject(*renderObj);
}
}
}
}
static int counter=0;
counter++;
if ((counter&7)==0)
{
char filename[1024];
sprintf(filename,"framebuf%d.tga",counter);
m_rgbColorBuffer.flip_vertically();
getFrameBuffer().write_tga_file(filename,true);
}
float color[4] = {1,1,1,1};
}
void btMultiBody_worldPosToLocal(btMultiBody* obj, int i, const btVector3* vec, btVector3* value)
{
BTVECTOR3_IN(vec);
ATTRIBUTE_ALIGNED16(btVector3) temp = obj->worldPosToLocal(i, BTVECTOR3_USE(vec));
BTVECTOR3_SET(value, temp);
}
void processRaycastTask(void* userPtr, void* lsMemory)
{
RaycastTask_LocalStoreMemory* localMemory = (RaycastTask_LocalStoreMemory*)lsMemory;
SpuRaycastTaskDesc* taskDescPtr = (SpuRaycastTaskDesc*)userPtr;
SpuRaycastTaskDesc& taskDesc = *taskDescPtr;
SpuCollisionObjectWrapper* cows = (SpuCollisionObjectWrapper*)taskDesc.spuCollisionObjectsWrappers;
//spu_printf("in processRaycastTask %d\n", taskDesc.numSpuCollisionObjectWrappers);
/* for each object */
RaycastGatheredObjectData gatheredObjectData;
for (int objectId = 0; objectId < taskDesc.numSpuCollisionObjectWrappers; objectId++)
{
//spu_printf("%d / %d\n", objectId, taskDesc.numSpuCollisionObjectWrappers);
/* load initial collision shape */
GatherCollisionObjectAndShapeData (&gatheredObjectData, localMemory, (ppu_address_t)&cows[objectId]);
if (btBroadphaseProxy::isConcave (gatheredObjectData.m_shapeType))
{
SpuRaycastTaskWorkUnitOut tWorkUnitsOut[SPU_RAYCAST_WORK_UNITS_PER_TASK];
for (int rayId = 0; rayId < taskDesc.numWorkUnits; rayId++)
{
tWorkUnitsOut[rayId].hitFraction = 1.0;
}
performRaycastAgainstConcave (&gatheredObjectData, &taskDesc.workUnits[0], &tWorkUnitsOut[0], taskDesc.numWorkUnits, localMemory);
for (int rayId = 0; rayId < taskDesc.numWorkUnits; rayId++)
{
const SpuRaycastTaskWorkUnit& workUnit = taskDesc.workUnits[rayId];
if (tWorkUnitsOut[rayId].hitFraction == 1.0)
continue;
ATTRIBUTE_ALIGNED16(SpuRaycastTaskWorkUnitOut workUnitOut);
dmaLoadRayOutput ((ppu_address_t)workUnit.output, &workUnitOut, 1);
cellDmaWaitTagStatusAll(DMA_MASK(1));
/* XXX Only support taking the closest hit for now */
if (tWorkUnitsOut[rayId].hitFraction < workUnitOut.hitFraction)
{
workUnitOut.hitFraction = tWorkUnitsOut[rayId].hitFraction;
workUnitOut.hitNormal = tWorkUnitsOut[rayId].hitNormal;
}
/* write ray cast data back */
dmaStoreRayOutput ((ppu_address_t)workUnit.output, &workUnitOut, 1);
cellDmaWaitTagStatusAll(DMA_MASK(1));
}
} else if (btBroadphaseProxy::isConvex (gatheredObjectData.m_shapeType)) {
btVector3 objectBoxMin, objectBoxMax;
computeAabb (objectBoxMin, objectBoxMax, (btConvexInternalShape*)gatheredObjectData.m_spuCollisionShape, gatheredObjectData.m_collisionShape, gatheredObjectData.m_shapeType, gatheredObjectData.m_worldTransform);
for (unsigned int rayId = 0; rayId < taskDesc.numWorkUnits; rayId++)
{
const SpuRaycastTaskWorkUnit& workUnit = taskDesc.workUnits[rayId];
btScalar ignored_param = 1.0;
btVector3 ignored_normal;
if (btRayAabb(workUnit.rayFrom, workUnit.rayTo, objectBoxMin, objectBoxMax, ignored_param, ignored_normal))
{
ATTRIBUTE_ALIGNED16(SpuRaycastTaskWorkUnitOut workUnitOut);
SpuRaycastTaskWorkUnitOut tWorkUnitOut;
tWorkUnitOut.hitFraction = 1.0;
performRaycastAgainstConvex (&gatheredObjectData, workUnit, &tWorkUnitOut, localMemory);
if (tWorkUnitOut.hitFraction == 1.0)
continue;
dmaLoadRayOutput ((ppu_address_t)workUnit.output, &workUnitOut, 1);
cellDmaWaitTagStatusAll(DMA_MASK(1));
/* XXX Only support taking the closest hit for now */
if (tWorkUnitOut.hitFraction < workUnitOut.hitFraction)
{
workUnitOut.hitFraction = tWorkUnitOut.hitFraction;
workUnitOut.hitNormal = tWorkUnitOut.hitNormal;
/* write ray cast data back */
dmaStoreRayOutput ((ppu_address_t)workUnit.output, &workUnitOut, 1);
cellDmaWaitTagStatusAll(DMA_MASK(1));
}
}
}
} else if (btBroadphaseProxy::isCompound (gatheredObjectData.m_shapeType)) {
for (unsigned int rayId = 0; rayId < taskDesc.numWorkUnits; rayId++)
{
const SpuRaycastTaskWorkUnit& workUnit = taskDesc.workUnits[rayId];
ATTRIBUTE_ALIGNED16(SpuRaycastTaskWorkUnitOut workUnitOut);
SpuRaycastTaskWorkUnitOut tWorkUnitOut;
tWorkUnitOut.hitFraction = 1.0;
performRaycastAgainstCompound (&gatheredObjectData, workUnit, &tWorkUnitOut, localMemory);
if (tWorkUnitOut.hitFraction == 1.0)
continue;
dmaLoadRayOutput ((ppu_address_t)workUnit.output, &workUnitOut, 1);
cellDmaWaitTagStatusAll(DMA_MASK(1));
/* XXX Only support taking the closest hit for now */
if (tWorkUnitOut.hitFraction < workUnitOut.hitFraction)
{
workUnitOut.hitFraction = tWorkUnitOut.hitFraction;
workUnitOut.hitNormal = tWorkUnitOut.hitNormal;
}
/* write ray cast data back */
dmaStoreRayOutput ((ppu_address_t)workUnit.output, &workUnitOut, 1);
cellDmaWaitTagStatusAll(DMA_MASK(1));
}
}
}
}
void TinyRendererSetup::stepSimulation(float deltaTime)
{
m_internalData->updateTransforms();
if (!m_useSoftware)
{
for (int i=0;i<m_internalData->m_transforms.size();i++)
{
m_guiHelper->getRenderInterface()->writeSingleInstanceTransformToCPU(m_internalData->m_transforms[i].getOrigin(),m_internalData->m_transforms[i].getRotation(),i);
}
m_guiHelper->getRenderInterface()->writeTransforms();
m_guiHelper->getRenderInterface()->renderScene();
} else
{
TGAColor clearColor;
clearColor.bgra[0] = 200;
clearColor.bgra[1] = 200;
clearColor.bgra[2] = 200;
clearColor.bgra[3] = 255;
for(int y=0;y<m_internalData->m_height;++y)
{
for(int x=0;x<m_internalData->m_width;++x)
{
m_internalData->m_rgbColorBuffer.set(x,y,clearColor);
m_internalData->m_depthBuffer[x+y*m_internalData->m_width] = -1e30f;
}
}
ATTRIBUTE_ALIGNED16(btScalar modelMat2[16]);
ATTRIBUTE_ALIGNED16(float viewMat[16]);
ATTRIBUTE_ALIGNED16(float projMat[16]);
CommonRenderInterface* render = this->m_app->m_renderer;
render->getActiveCamera()->getCameraViewMatrix(viewMat);
render->getActiveCamera()->getCameraProjectionMatrix(projMat);
for (int o=0;o<this->m_internalData->m_renderObjects.size();o++)
{
const btTransform& tr = m_internalData->m_transforms[o];
tr.getOpenGLMatrix(modelMat2);
for (int i=0;i<4;i++)
{
for (int j=0;j<4;j++)
{
m_internalData->m_renderObjects[o]->m_modelMatrix[i][j] = float(modelMat2[i+4*j]);
m_internalData->m_renderObjects[o]->m_viewMatrix[i][j] = viewMat[i+4*j];
m_internalData->m_renderObjects[o]->m_projectionMatrix[i][j] = projMat[i+4*j];
float eye[4];
float center[4];
render->getActiveCamera()->getCameraPosition(eye);
render->getActiveCamera()->getCameraTargetPosition(center);
m_internalData->m_renderObjects[o]->m_eye.setValue(eye[0],eye[1],eye[2]);
m_internalData->m_renderObjects[o]->m_center.setValue(center[0],center[1],center[2]);
}
}
TinyRenderer::renderObject(*m_internalData->m_renderObjects[o]);
}
//m_app->drawText("hello",500,500);
render->activateTexture(m_internalData->m_textureHandle);
render->updateTexture(m_internalData->m_textureHandle,m_internalData->m_rgbColorBuffer.buffer());
float color[4] = {1,1,1,1};
m_app->drawTexturedRect(0,0,m_app->m_window->getWidth(), m_app->m_window->getHeight(),color,0,0,1,1,true);
}
}
void GL_ShapeDrawer::drawOpenGL(btScalar* m, const btCollisionShape* shape, const btVector3& color,int debugMode,const btVector3& worldBoundsMin,const btVector3& worldBoundsMax)
{
if (shape->getShapeType() == CUSTOM_CONVEX_SHAPE_TYPE)
{
btVector3 org(m[12], m[13], m[14]);
btVector3 dx(m[0], m[1], m[2]);
btVector3 dy(m[4], m[5], m[6]);
// btVector3 dz(m[8], m[9], m[10]);
const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape);
btVector3 halfExtent = boxShape->getHalfExtentsWithMargin();
dx *= halfExtent[0];
dy *= halfExtent[1];
// dz *= halfExtent[2];
glColor3f(1,1,1);
glDisable(GL_LIGHTING);
glLineWidth(2);
glBegin(GL_LINE_LOOP);
glDrawVector(org - dx - dy);
glDrawVector(org - dx + dy);
glDrawVector(org + dx + dy);
glDrawVector(org + dx - dy);
glEnd();
return;
}
else if((shape->getShapeType() == BOX_SHAPE_PROXYTYPE) && (debugMode & btIDebugDraw::DBG_FastWireframe))
{
btVector3 org(m[12], m[13], m[14]);
btVector3 dx(m[0], m[1], m[2]);
btVector3 dy(m[4], m[5], m[6]);
btVector3 dz(m[8], m[9], m[10]);
const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape);
btVector3 halfExtent = boxShape->getHalfExtentsWithMargin();
dx *= halfExtent[0];
dy *= halfExtent[1];
dz *= halfExtent[2];
glBegin(GL_LINE_LOOP);
glDrawVector(org - dx - dy - dz);
glDrawVector(org + dx - dy - dz);
glDrawVector(org + dx + dy - dz);
glDrawVector(org - dx + dy - dz);
glDrawVector(org - dx + dy + dz);
glDrawVector(org + dx + dy + dz);
glDrawVector(org + dx - dy + dz);
glDrawVector(org - dx - dy + dz);
glEnd();
glBegin(GL_LINES);
glDrawVector(org + dx - dy - dz);
glDrawVector(org + dx - dy + dz);
glDrawVector(org + dx + dy - dz);
glDrawVector(org + dx + dy + dz);
glDrawVector(org - dx - dy - dz);
glDrawVector(org - dx + dy - dz);
glDrawVector(org - dx - dy + dz);
glDrawVector(org - dx + dy + dz);
glEnd();
return;
}
glPushMatrix();
btglMultMatrix(m);
if (shape->getShapeType() == UNIFORM_SCALING_SHAPE_PROXYTYPE)
{
const btUniformScalingShape* scalingShape = static_cast<const btUniformScalingShape*>(shape);
const btConvexShape* convexShape = scalingShape->getChildShape();
float scalingFactor = (float)scalingShape->getUniformScalingFactor();
{
btScalar tmpScaling[4][4]={{scalingFactor,0,0,0},
{0,scalingFactor,0,0},
{0,0,scalingFactor,0},
{0,0,0,1}};
drawOpenGL( (btScalar*)tmpScaling,convexShape,color,debugMode,worldBoundsMin,worldBoundsMax);
}
glPopMatrix();
return;
}
if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
{
const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--)
{
btTransform childTrans = compoundShape->getChildTransform(i);
const btCollisionShape* colShape = compoundShape->getChildShape(i);
ATTRIBUTE_ALIGNED16(btScalar) childMat[16];
childTrans.getOpenGLMatrix(childMat);
drawOpenGL(childMat,colShape,color,debugMode,worldBoundsMin,worldBoundsMax);
}
} else
{
if(m_textureenabled&&(!m_textureinitialized))
{
GLubyte* image=new GLubyte[256*256*4];
for(int y=0;y<256;++y)
{
const int t=y>>4;
GLubyte* pi=image+y*256*3;
for(int x=0;x<256;++x)
{
const int s=x>>4;
const GLubyte b=180;
GLubyte c=b+((s+(t&1))&1)*(255-b);
pi[0]=pi[1]=pi[2]=pi[3]=c;pi+=3;
}
}
glGenTextures(1,(GLuint*)&m_texturehandle);
glBindTexture(GL_TEXTURE_2D,m_texturehandle);
glGenTextures(1,(GLuint*)&m_texturehandle);
glBindTexture(GL_TEXTURE_2D,m_texturehandle);
glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MAG_FILTER,GL_LINEAR);
glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MIN_FILTER,GL_LINEAR);
glTexImage2D(GL_TEXTURE_2D, 0, 3, 256 , 256 , 0, GL_RGB, GL_UNSIGNED_BYTE, image);
//glGenerateMipmap(GL_TEXTURE_2D);
delete[] image;
}
glMatrixMode(GL_TEXTURE);
glLoadIdentity();
glScalef(0.025f,0.025f,0.025f);
glMatrixMode(GL_MODELVIEW);
static const GLfloat planex[]={1,0,0,0};
// static const GLfloat planey[]={0,1,0,0};
static const GLfloat planez[]={0,0,1,0};
glTexGenfv(GL_S,GL_OBJECT_PLANE,planex);
glTexGenfv(GL_T,GL_OBJECT_PLANE,planez);
glTexGeni(GL_S,GL_TEXTURE_GEN_MODE,GL_OBJECT_LINEAR);
glTexGeni(GL_T,GL_TEXTURE_GEN_MODE,GL_OBJECT_LINEAR);
glEnable(GL_TEXTURE_GEN_S);
glEnable(GL_TEXTURE_GEN_T);
glEnable(GL_TEXTURE_GEN_R);
m_textureinitialized=true;
//drawCoordSystem();
//glPushMatrix();
glEnable(GL_COLOR_MATERIAL);
if(m_textureenabled)
{
glEnable(GL_TEXTURE_2D);
glBindTexture(GL_TEXTURE_2D,m_texturehandle);
} else
{
glDisable(GL_TEXTURE_2D);
}
glColor3f(color.x(),color.y(), color.z());
//bool useWireframeFallback = true;
if (!(debugMode & btIDebugDraw::DBG_DrawWireframe))
{
///you can comment out any of the specific cases, and use the default
///the benefit of 'default' is that it approximates the actual collision shape including collision margin
//int shapetype=m_textureenabled?MAX_BROADPHASE_COLLISION_TYPES:shape->getShapeType();
int shapetype=shape->getShapeType();
switch (shapetype)
{
case SPHERE_SHAPE_PROXYTYPE:
{
const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape);
float radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin
drawSphere(radius,10,10);
//useWireframeFallback = false;
break;
}
case BOX_SHAPE_PROXYTYPE:
{
const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape);
btVector3 halfExtent = boxShape->getHalfExtentsWithMargin();
static int indices[36] = {
0,1,2,
3,2,1,
4,0,6,
6,0,2,
5,1,4,
4,1,0,
7,3,1,
7,1,5,
5,4,7,
7,4,6,
7,2,3,
7,6,2};
btVector3 vertices[8]={
btVector3(halfExtent[0],halfExtent[1],halfExtent[2]),
btVector3(-halfExtent[0],halfExtent[1],halfExtent[2]),
btVector3(halfExtent[0],-halfExtent[1],halfExtent[2]),
btVector3(-halfExtent[0],-halfExtent[1],halfExtent[2]),
btVector3(halfExtent[0],halfExtent[1],-halfExtent[2]),
btVector3(-halfExtent[0],halfExtent[1],-halfExtent[2]),
btVector3(halfExtent[0],-halfExtent[1],-halfExtent[2]),
btVector3(-halfExtent[0],-halfExtent[1],-halfExtent[2])};
#if 1
glBegin (GL_TRIANGLES);
int si=36;
for (int i=0;i<si;i+=3)
{
const btVector3& v1 = vertices[indices[i]];;
const btVector3& v2 = vertices[indices[i+1]];
const btVector3& v3 = vertices[indices[i+2]];
btVector3 normal = (v3-v1).cross(v2-v1);
normal.normalize ();
glNormal3f(normal.getX(),normal.getY(),normal.getZ());
glVertex3f (v1.x(), v1.y(), v1.z());
glVertex3f (v2.x(), v2.y(), v2.z());
glVertex3f (v3.x(), v3.y(), v3.z());
}
glEnd();
#endif
//useWireframeFallback = false;
break;
}
#if 0
case CONE_SHAPE_PROXYTYPE:
{
const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
int upIndex = coneShape->getConeUpIndex();
float radius = coneShape->getRadius();//+coneShape->getMargin();
float height = coneShape->getHeight();//+coneShape->getMargin();
switch (upIndex)
{
case 0:
glRotatef(90.0, 0.0, 1.0, 0.0);
break;
case 1:
glRotatef(-90.0, 1.0, 0.0, 0.0);
break;
case 2:
break;
default:
{
}
};
glTranslatef(0.0, 0.0, -0.5*height);
glutSolidCone(radius,height,10,10);
//useWireframeFallback = false;
break;
}
#endif
case STATIC_PLANE_PROXYTYPE:
{
const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape);
btScalar planeConst = staticPlaneShape->getPlaneConstant();
const btVector3& planeNormal = staticPlaneShape->getPlaneNormal();
btVector3 planeOrigin = planeNormal * planeConst;
btVector3 vec0,vec1;
btPlaneSpace1(planeNormal,vec0,vec1);
btScalar vecLen = 100.f;
btVector3 pt0 = planeOrigin + vec0*vecLen;
btVector3 pt1 = planeOrigin - vec0*vecLen;
btVector3 pt2 = planeOrigin + vec1*vecLen;
btVector3 pt3 = planeOrigin - vec1*vecLen;
glBegin(GL_LINES);
glVertex3f(pt0.getX(),pt0.getY(),pt0.getZ());
glVertex3f(pt1.getX(),pt1.getY(),pt1.getZ());
glVertex3f(pt2.getX(),pt2.getY(),pt2.getZ());
glVertex3f(pt3.getX(),pt3.getY(),pt3.getZ());
glEnd();
break;
}
case MULTI_SPHERE_SHAPE_PROXYTYPE:
{
const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape);
btTransform childTransform;
childTransform.setIdentity();
for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--)
{
btSphereShape sc(multiSphereShape->getSphereRadius(i));
childTransform.setOrigin(multiSphereShape->getSpherePosition(i));
ATTRIBUTE_ALIGNED16(btScalar) childMat[16];
childTransform.getOpenGLMatrix(childMat);
drawOpenGL(childMat,&sc,color,debugMode,worldBoundsMin,worldBoundsMax);
}
break;
}
default:
{
if (shape->isConvex())
{
const btConvexPolyhedron* poly = shape->isPolyhedral() ? ((btPolyhedralConvexShape*) shape)->getConvexPolyhedron() : 0;
if (poly)
{
int i;
glBegin (GL_TRIANGLES);
for (i=0;i<poly->m_faces.size();i++)
{
btVector3 centroid(0,0,0);
int numVerts = poly->m_faces[i].m_indices.size();
if (numVerts>2)
{
btVector3 v1 = poly->m_vertices[poly->m_faces[i].m_indices[0]];
for (int v=0;v<poly->m_faces[i].m_indices.size()-2;v++)
{
btVector3 v2 = poly->m_vertices[poly->m_faces[i].m_indices[v+1]];
btVector3 v3 = poly->m_vertices[poly->m_faces[i].m_indices[v+2]];
btVector3 normal = (v3-v1).cross(v2-v1);
normal.normalize ();
glNormal3f(normal.getX(),normal.getY(),normal.getZ());
glVertex3f (v1.x(), v1.y(), v1.z());
glVertex3f (v2.x(), v2.y(), v2.z());
glVertex3f (v3.x(), v3.y(), v3.z());
}
}
}
glEnd ();
} else
{
ShapeCache* sc=cache((btConvexShape*)shape);
//glutSolidCube(1.0);
btShapeHull* hull = &sc->m_shapehull/*(btShapeHull*)shape->getUserPointer()*/;
if (hull->numTriangles () > 0)
{
int index = 0;
const unsigned int* idx = hull->getIndexPointer();
const btVector3* vtx = hull->getVertexPointer();
glBegin (GL_TRIANGLES);
for (int i = 0; i < hull->numTriangles (); i++)
{
int i1 = index++;
int i2 = index++;
int i3 = index++;
btAssert(i1 < hull->numIndices () &&
i2 < hull->numIndices () &&
i3 < hull->numIndices ());
int index1 = idx[i1];
int index2 = idx[i2];
int index3 = idx[i3];
btAssert(index1 < hull->numVertices () &&
index2 < hull->numVertices () &&
index3 < hull->numVertices ());
btVector3 v1 = vtx[index1];
btVector3 v2 = vtx[index2];
btVector3 v3 = vtx[index3];
btVector3 normal = (v3-v1).cross(v2-v1);
normal.normalize ();
glNormal3f(normal.getX(),normal.getY(),normal.getZ());
glVertex3f (v1.x(), v1.y(), v1.z());
glVertex3f (v2.x(), v2.y(), v2.z());
glVertex3f (v3.x(), v3.y(), v3.z());
}
glEnd ();
}
}
}
}
}
}
glNormal3f(0,1,0);
/// for polyhedral shapes
if (debugMode==btIDebugDraw::DBG_DrawFeaturesText && (shape->isPolyhedral()))
{
btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape;
{
glColor3f(1.f, 1.f, 1.f);
int i;
for (i=0;i<polyshape->getNumVertices();i++)
{
btVector3 vtx;
polyshape->getVertex(i,vtx);
char buf[12];
sprintf(buf," %d",i);
//btDrawString(BMF_GetFont(BMF_kHelvetica10),buf);
}
for (i=0;i<polyshape->getNumPlanes();i++)
{
btVector3 normal;
btVector3 vtx;
polyshape->getPlane(normal,vtx,i);
//btScalar d = vtx.dot(normal);
//char buf[12];
//sprintf(buf," plane %d",i);
//btDrawString(BMF_GetFont(BMF_kHelvetica10),buf);
}
}
}
}