void b2World::DrawDebugData()
{
if (m_debugDraw == NULL)
{
return;
}
uint32 flags = m_debugDraw->GetFlags();
if (flags & b2DebugDraw::e_shapeBit)
{
for (b2Body* b = m_bodyList; b; b = b->GetNext())
{
const b2Transform& xf = b->GetTransform();
for (b2Fixture* f = b->GetFixtureList(); f; f = f->GetNext())
{
if (b->IsStatic())
{
DrawShape(f, xf, b2Color(0.5f, 0.9f, 0.5f));
}
else if (b->IsSleeping())
{
DrawShape(f, xf, b2Color(0.5f, 0.5f, 0.9f));
}
else
{
DrawShape(f, xf, b2Color(0.9f, 0.9f, 0.9f));
}
}
}
}
if (flags & b2DebugDraw::e_jointBit)
{
for (b2Joint* j = m_jointList; j; j = j->GetNext())
{
if (j->GetType() != e_mouseJoint)
{
DrawJoint(j);
}
}
}
if (flags & b2DebugDraw::e_pairBit)
{
// TODO_ERIN
}
if (flags & b2DebugDraw::e_aabbBit)
{
b2Color color(0.9f, 0.3f, 0.9f);
b2BroadPhase* bp = &m_contactManager.m_broadPhase;
for (b2Body* b = m_bodyList; b; b = b->GetNext())
{
for (b2Fixture* f = b->GetFixtureList(); f; f = f->GetNext())
{
b2AABB aabb = bp->GetFatAABB(f->m_proxyId);
b2Vec2 vs[4];
vs[0].Set(aabb.lowerBound.x, aabb.lowerBound.y);
vs[1].Set(aabb.upperBound.x, aabb.lowerBound.y);
vs[2].Set(aabb.upperBound.x, aabb.upperBound.y);
vs[3].Set(aabb.lowerBound.x, aabb.upperBound.y);
m_debugDraw->DrawPolygon(vs, 4, color);
}
}
}
if (flags & b2DebugDraw::e_centerOfMassBit)
{
for (b2Body* b = m_bodyList; b; b = b->GetNext())
{
b2Transform xf = b->GetTransform();
xf.position = b->GetWorldCenter();
m_debugDraw->DrawXForm(xf);
}
}
}
void b2World::DrawDebugData()
{
if (m_debugDraw == NULL)
{
return;
}
uint32 flags = m_debugDraw->GetFlags();
if (flags & b2DebugDraw::e_shapeBit)
{
bool core = (flags & b2DebugDraw::e_coreShapeBit) == b2DebugDraw::e_coreShapeBit;
for (b2Body* b = m_bodyList; b; b = b->GetNext())
{
const b2XForm& xf = b->GetXForm();
for (b2Shape* s = b->GetShapeList(); s; s = s->GetNext())
{
if (b->IsStatic())
{
DrawShape(s, xf, b2Color(0.5f, 0.9f, 0.5f), core);
}
else if (b->IsSleeping())
{
DrawShape(s, xf, b2Color(0.5f, 0.5f, 0.9f), core);
}
else
{
DrawShape(s, xf, b2Color(0.9f, 0.9f, 0.9f), core);
}
}
}
}
if (flags & b2DebugDraw::e_jointBit)
{
for (b2Joint* j = m_jointList; j; j = j->GetNext())
{
if (j->GetType() != e_mouseJoint)
{
DrawJoint(j);
}
}
}
if (flags & b2DebugDraw::e_controllerBit)
{
for (b2Controller* c = m_controllerList; c; c= c->GetNext())
{
c->Draw(m_debugDraw);
}
}
if (flags & b2DebugDraw::e_pairBit)
{
b2BroadPhase* bp = m_broadPhase;
b2Vec2 invQ;
invQ.Set(1.0f / bp->m_quantizationFactor.x, 1.0f / bp->m_quantizationFactor.y);
b2Color color(0.9f, 0.9f, 0.3f);
for (int32 i = 0; i < b2_tableCapacity; ++i)
{
uint16 index = bp->m_pairManager.m_hashTable[i];
while (index != b2_nullPair)
{
b2Pair* pair = bp->m_pairManager.m_pairs + index;
b2Proxy* p1 = bp->m_proxyPool + pair->proxyId1;
b2Proxy* p2 = bp->m_proxyPool + pair->proxyId2;
b2AABB b1, b2;
b1.lowerBound.x = bp->m_worldAABB.lowerBound.x + invQ.x * bp->m_bounds[0][p1->lowerBounds[0]].value;
b1.lowerBound.y = bp->m_worldAABB.lowerBound.y + invQ.y * bp->m_bounds[1][p1->lowerBounds[1]].value;
b1.upperBound.x = bp->m_worldAABB.lowerBound.x + invQ.x * bp->m_bounds[0][p1->upperBounds[0]].value;
b1.upperBound.y = bp->m_worldAABB.lowerBound.y + invQ.y * bp->m_bounds[1][p1->upperBounds[1]].value;
b2.lowerBound.x = bp->m_worldAABB.lowerBound.x + invQ.x * bp->m_bounds[0][p2->lowerBounds[0]].value;
b2.lowerBound.y = bp->m_worldAABB.lowerBound.y + invQ.y * bp->m_bounds[1][p2->lowerBounds[1]].value;
b2.upperBound.x = bp->m_worldAABB.lowerBound.x + invQ.x * bp->m_bounds[0][p2->upperBounds[0]].value;
b2.upperBound.y = bp->m_worldAABB.lowerBound.y + invQ.y * bp->m_bounds[1][p2->upperBounds[1]].value;
b2Vec2 x1 = 0.5f * (b1.lowerBound + b1.upperBound);
b2Vec2 x2 = 0.5f * (b2.lowerBound + b2.upperBound);
m_debugDraw->DrawSegment(x1, x2, color);
index = pair->next;
}
}
}
if (flags & b2DebugDraw::e_aabbBit)
{
b2BroadPhase* bp = m_broadPhase;
b2Vec2 worldLower = bp->m_worldAABB.lowerBound;
b2Vec2 worldUpper = bp->m_worldAABB.upperBound;
b2Vec2 invQ;
invQ.Set(1.0f / bp->m_quantizationFactor.x, 1.0f / bp->m_quantizationFactor.y);
b2Color color(0.9f, 0.3f, 0.9f);
for (int32 i = 0; i < b2_maxProxies; ++i)
{
b2Proxy* p = bp->m_proxyPool + i;
if (p->IsValid() == false)
{
continue;
}
b2AABB b;
b.lowerBound.x = worldLower.x + invQ.x * bp->m_bounds[0][p->lowerBounds[0]].value;
b.lowerBound.y = worldLower.y + invQ.y * bp->m_bounds[1][p->lowerBounds[1]].value;
b.upperBound.x = worldLower.x + invQ.x * bp->m_bounds[0][p->upperBounds[0]].value;
b.upperBound.y = worldLower.y + invQ.y * bp->m_bounds[1][p->upperBounds[1]].value;
b2Vec2 vs[4];
vs[0].Set(b.lowerBound.x, b.lowerBound.y);
vs[1].Set(b.upperBound.x, b.lowerBound.y);
vs[2].Set(b.upperBound.x, b.upperBound.y);
vs[3].Set(b.lowerBound.x, b.upperBound.y);
m_debugDraw->DrawPolygon(vs, 4, color);
}
b2Vec2 vs[4];
vs[0].Set(worldLower.x, worldLower.y);
vs[1].Set(worldUpper.x, worldLower.y);
vs[2].Set(worldUpper.x, worldUpper.y);
vs[3].Set(worldLower.x, worldUpper.y);
m_debugDraw->DrawPolygon(vs, 4, b2Color(0.3f, 0.9f, 0.9f));
}
if (flags & b2DebugDraw::e_obbBit)
{
b2Color color(0.5f, 0.3f, 0.5f);
for (b2Body* b = m_bodyList; b; b = b->GetNext())
{
const b2XForm& xf = b->GetXForm();
for (b2Shape* s = b->GetShapeList(); s; s = s->GetNext())
{
if (s->GetType() != e_polygonShape)
{
continue;
}
b2PolygonShape* poly = (b2PolygonShape*)s;
const b2OBB& obb = poly->GetOBB();
b2Vec2 h = obb.extents;
b2Vec2 vs[4];
vs[0].Set(-h.x, -h.y);
vs[1].Set( h.x, -h.y);
vs[2].Set( h.x, h.y);
vs[3].Set(-h.x, h.y);
for (int32 i = 0; i < 4; ++i)
{
vs[i] = obb.center + b2Mul(obb.R, vs[i]);
vs[i] = b2Mul(xf, vs[i]);
}
m_debugDraw->DrawPolygon(vs, 4, color);
}
}
}
if (flags & b2DebugDraw::e_centerOfMassBit)
{
for (b2Body* b = m_bodyList; b; b = b->GetNext())
{
b2XForm xf = b->GetXForm();
xf.position = b->GetWorldCenter();
m_debugDraw->DrawXForm(xf);
}
}
}
void b2World::DrawDebugData()
{
if (m_debugDraw == NULL)
{
return;
}
uint32 flags = m_debugDraw->GetFlags();
if (flags & b2Draw::e_shapeBit)
{
for (b2Body* b = m_bodyList; b; b = b->GetNext())
{
const b2Transform& xf = b->GetTransform();
for (b2Fixture* f = b->GetFixtureList(); f; f = f->GetNext())
{
if (b->IsActive() == false)
{
DrawShape(f, xf, b2Color(0.5f, 0.5f, 0.3f));
}
else if (b->GetType() == b2_staticBody)
{
DrawShape(f, xf, b2Color(0.5f, 0.9f, 0.5f));
}
else if (b->GetType() == b2_kinematicBody)
{
DrawShape(f, xf, b2Color(0.5f, 0.5f, 0.9f));
}
else if (b->IsAwake() == false)
{
DrawShape(f, xf, b2Color(0.6f, 0.6f, 0.6f));
}
else
{
DrawShape(f, xf, b2Color(0.9f, 0.7f, 0.7f));
}
}
}
}
if (flags & b2Draw::e_jointBit)
{
for (b2Joint* j = m_jointList; j; j = j->GetNext())
{
DrawJoint(j);
}
}
if (flags & b2Draw::e_pairBit)
{
b2Color color(0.3f, 0.9f, 0.9f);
for (b2Contact* c = m_contactManager.m_contactList; c; c = c->GetNext())
{
//b2Fixture* fixtureA = c->GetFixtureA();
//b2Fixture* fixtureB = c->GetFixtureB();
//b2Vec2 cA = fixtureA->GetAABB().GetCenter();
//b2Vec2 cB = fixtureB->GetAABB().GetCenter();
//m_debugDraw->DrawSegment(cA, cB, color);
}
}
if (flags & b2Draw::e_aabbBit)
{
b2Color color(0.9f, 0.3f, 0.9f);
b2BroadPhase* bp = &m_contactManager.m_broadPhase;
for (b2Body* b = m_bodyList; b; b = b->GetNext())
{
if (b->IsActive() == false)
{
continue;
}
for (b2Fixture* f = b->GetFixtureList(); f; f = f->GetNext())
{
for (int32 i = 0; i < f->m_proxyCount; ++i)
{
b2FixtureProxy* proxy = f->m_proxies + i;
b2AABB aabb = bp->GetFatAABB(proxy->proxyId);
b2Vec2 vs[4];
vs[0].Set(aabb.lowerBound.x, aabb.lowerBound.y);
vs[1].Set(aabb.upperBound.x, aabb.lowerBound.y);
vs[2].Set(aabb.upperBound.x, aabb.upperBound.y);
vs[3].Set(aabb.lowerBound.x, aabb.upperBound.y);
m_debugDraw->DrawPolygon(vs, 4, color);
}
}
}
}
if (flags & b2Draw::e_centerOfMassBit)
{
for (b2Body* b = m_bodyList; b; b = b->GetNext())
{
b2Transform xf = b->GetTransform();
xf.p = b->GetWorldCenter();
m_debugDraw->DrawTransform(xf);
}
}
}
void DrawDepthMap(const xn::DepthMetaData& dmd, const xn::SceneMetaData& smd)
{
static bool bInitialized = false;
static GLuint depthTexID;
static unsigned char* pDepthTexBuf;
static int texWidth, texHeight;
float topLeftX;
float topLeftY;
float bottomRightY;
float bottomRightX;
float texXpos;
float texYpos;
if(!bInitialized)
{
texWidth = getClosestPowerOfTwo(dmd.XRes());
texHeight = getClosestPowerOfTwo(dmd.YRes());
// printf("Initializing depth texture: width = %d, height = %d\n", texWidth, texHeight);
depthTexID = initTexture((void**)&pDepthTexBuf,texWidth, texHeight) ;
// printf("Initialized depth texture: width = %d, height = %d\n", texWidth, texHeight);
bInitialized = true;
topLeftX = dmd.XRes();
topLeftY = 0;
bottomRightY = dmd.YRes();
bottomRightX = 0;
texXpos =(float)dmd.XRes()/texWidth;
texYpos =(float)dmd.YRes()/texHeight;
memset(texcoords, 0, 8*sizeof(float));
texcoords[0] = texXpos, texcoords[1] = texYpos, texcoords[2] = texXpos, texcoords[7] = texYpos;
}
unsigned int nValue = 0;
unsigned int nHistValue = 0;
unsigned int nIndex = 0;
unsigned int nX = 0;
unsigned int nY = 0;
unsigned int nNumberOfPoints = 0;
XnUInt16 g_nXRes = dmd.XRes();
XnUInt16 g_nYRes = dmd.YRes();
unsigned char* pDestImage = pDepthTexBuf;
const XnDepthPixel* pDepth = dmd.Data();
const XnLabel* pLabels = smd.Data();
static unsigned int nZRes = dmd.ZRes();
static float* pDepthHist = (float*)malloc(nZRes* sizeof(float));
// Calculate the accumulative histogram
memset(pDepthHist, 0, nZRes*sizeof(float));
for (nY=0; nY<g_nYRes; nY++)
{
for (nX=0; nX<g_nXRes; nX++)
{
nValue = *pDepth;
if (nValue != 0)
{
pDepthHist[nValue]++;
nNumberOfPoints++;
}
pDepth++;
}
}
for (nIndex=1; nIndex<nZRes; nIndex++)
{
pDepthHist[nIndex] += pDepthHist[nIndex-1];
}
if (nNumberOfPoints)
{
for (nIndex=1; nIndex<nZRes; nIndex++)
{
pDepthHist[nIndex] = (unsigned int)(256 * (1.0f - (pDepthHist[nIndex] / nNumberOfPoints)));
}
}
pDepth = dmd.Data();
if (g_bDrawPixels)
{
XnUInt32 nIndex = 0;
// Prepare the texture map
for (nY=0; nY<g_nYRes; nY++)
{
for (nX=0; nX < g_nXRes; nX++, nIndex++)
{
pDestImage[0] = 0;
pDestImage[1] = 0;
pDestImage[2] = 0;
if (g_bDrawBackground || *pLabels != 0)
{
nValue = *pDepth;
XnLabel label = *pLabels;
XnUInt32 nColorID = label % nColors;
if (label == 0)
{
nColorID = nColors;
}
if (nValue != 0)
{
nHistValue = pDepthHist[nValue];
pDestImage[0] = nHistValue * Colors[nColorID][0];
pDestImage[1] = nHistValue * Colors[nColorID][1];
pDestImage[2] = nHistValue * Colors[nColorID][2];
}
}
pDepth++;
pLabels++;
pDestImage+=3;
}
pDestImage += (texWidth - g_nXRes) *3;
}
}
else
{
xnOSMemSet(pDepthTexBuf, 0, 3*2*g_nXRes*g_nYRes);
}
glBindTexture(GL_TEXTURE_2D, depthTexID);
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, texWidth, texHeight, 0, GL_RGB, GL_UNSIGNED_BYTE, pDepthTexBuf);
// Display the OpenGL texture map
glColor4f(0.75,0.75,0.75,1);
glEnable(GL_TEXTURE_2D);
DrawTexture(dmd.XRes(),dmd.YRes(),0,0);
glDisable(GL_TEXTURE_2D);
char strLabel[50] = "";
XnUserID aUsers[1];
XnUInt16 nUsers = 1;
//XnUserID aUsers[1];
//XnUInt16 nUsers = 1;
g_UserGenerator.GetUsers(aUsers, nUsers);
for (int i = 0; i < nUsers; ++i)
{
#ifndef USE_GLES
if (g_bPrintID)
{
XnPoint3D com;
g_UserGenerator.GetCoM(aUsers[i], com);
g_DepthGenerator.ConvertRealWorldToProjective(1, &com, &com);
XnUInt32 nDummy = 0;
xnOSMemSet(strLabel, 0, sizeof(strLabel));
if (!g_bPrintState)
{
// Tracking
xnOSStrFormat(strLabel, sizeof(strLabel), &nDummy, "%d", aUsers[i]);
}
else if (g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i]))
{
// Tracking
xnOSStrFormat(strLabel, sizeof(strLabel), &nDummy, "%d - Tracking", aUsers[i]);
}
else if (g_UserGenerator.GetSkeletonCap().IsCalibrating(aUsers[i]))
{
// Calibrating
xnOSStrFormat(strLabel, sizeof(strLabel), &nDummy, "%d - Calibrating [%s]", aUsers[i], GetCalibrationErrorString(m_Errors[aUsers[i]].first));
}
else
{
// Nothing
xnOSStrFormat(strLabel, sizeof(strLabel), &nDummy, "%d - Looking for pose [%s]", aUsers[i], GetPoseErrorString(m_Errors[aUsers[i]].second));
}
glColor4f(1-Colors[i%nColors][0], 1-Colors[i%nColors][1], 1-Colors[i%nColors][2], 1);
glRasterPos2i(com.X, com.Y);
glPrintString(GLUT_BITMAP_HELVETICA_18, strLabel);
}
#endif
if (g_bDrawSkeleton && g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i]))
{
glColor4f(1-Colors[aUsers[i]%nColors][0], 1-Colors[aUsers[i]%nColors][1], 1-Colors[aUsers[i]%nColors][2], 1);
// Draw Joints
/*if (g_bMarkJoints)
{
// Try to draw all joints
DrawJoint(aUsers[i], XN_SKEL_HEAD);
DrawJoint(aUsers[i], XN_SKEL_NECK);
DrawJoint(aUsers[i], XN_SKEL_TORSO);
DrawJoint(aUsers[i], XN_SKEL_WAIST);
DrawJoint(aUsers[i], XN_SKEL_LEFT_COLLAR);
DrawJoint(aUsers[i], XN_SKEL_LEFT_SHOULDER);
DrawJoint(aUsers[i], XN_SKEL_LEFT_ELBOW);
DrawJoint(aUsers[i], XN_SKEL_LEFT_WRIST);*/
DrawJoint(aUsers[i], XN_SKEL_LEFT_HAND);
/*DrawJoint(aUsers[i], XN_SKEL_LEFT_FINGERTIP);
DrawJoint(aUsers[i], XN_SKEL_RIGHT_COLLAR);
DrawJoint(aUsers[i], XN_SKEL_RIGHT_SHOULDER);
DrawJoint(aUsers[i], XN_SKEL_RIGHT_ELBOW);
DrawJoint(aUsers[i], XN_SKEL_RIGHT_WRIST);*/
DrawJoint(aUsers[i], XN_SKEL_RIGHT_HAND);
/*DrawJoint(aUsers[i], XN_SKEL_RIGHT_FINGERTIP);
DrawJoint(aUsers[i], XN_SKEL_LEFT_HIP);
DrawJoint(aUsers[i], XN_SKEL_LEFT_KNEE);
DrawJoint(aUsers[i], XN_SKEL_LEFT_ANKLE);
DrawJoint(aUsers[i], XN_SKEL_LEFT_FOOT);
DrawJoint(aUsers[i], XN_SKEL_RIGHT_HIP);
DrawJoint(aUsers[i], XN_SKEL_RIGHT_KNEE);
DrawJoint(aUsers[i], XN_SKEL_RIGHT_ANKLE);
DrawJoint(aUsers[i], XN_SKEL_RIGHT_FOOT);
}*/
#ifndef USE_GLES
glBegin(GL_LINES);
#endif
// Draw Limbs
DrawLimb(aUsers[i], XN_SKEL_HEAD, XN_SKEL_NECK);
DrawLimb(aUsers[i], XN_SKEL_NECK, XN_SKEL_LEFT_SHOULDER);
DrawLimb(aUsers[i], XN_SKEL_LEFT_SHOULDER, XN_SKEL_LEFT_ELBOW);
if (!DrawLimb(aUsers[i], XN_SKEL_LEFT_ELBOW, XN_SKEL_LEFT_WRIST))
{
DrawLimb(aUsers[i], XN_SKEL_LEFT_ELBOW, XN_SKEL_LEFT_HAND);
}
else
{
DrawLimb(aUsers[i], XN_SKEL_LEFT_WRIST, XN_SKEL_LEFT_HAND);
DrawLimb(aUsers[i], XN_SKEL_LEFT_HAND, XN_SKEL_LEFT_FINGERTIP);
}
DrawLimb(aUsers[i], XN_SKEL_NECK, XN_SKEL_RIGHT_SHOULDER);
DrawLimb(aUsers[i], XN_SKEL_RIGHT_SHOULDER, XN_SKEL_RIGHT_ELBOW);
if (!DrawLimb(aUsers[i], XN_SKEL_RIGHT_ELBOW, XN_SKEL_RIGHT_WRIST))
{
DrawLimb(aUsers[i], XN_SKEL_RIGHT_ELBOW, XN_SKEL_RIGHT_HAND);
}
else
{
DrawLimb(aUsers[i], XN_SKEL_RIGHT_WRIST, XN_SKEL_RIGHT_HAND);
DrawLimb(aUsers[i], XN_SKEL_RIGHT_HAND, XN_SKEL_RIGHT_FINGERTIP);
}
DrawLimb(aUsers[i], XN_SKEL_LEFT_SHOULDER, XN_SKEL_TORSO);
DrawLimb(aUsers[i], XN_SKEL_RIGHT_SHOULDER, XN_SKEL_TORSO);
DrawLimb(aUsers[i], XN_SKEL_TORSO, XN_SKEL_LEFT_HIP);
DrawLimb(aUsers[i], XN_SKEL_LEFT_HIP, XN_SKEL_LEFT_KNEE);
DrawLimb(aUsers[i], XN_SKEL_LEFT_KNEE, XN_SKEL_LEFT_FOOT);
DrawLimb(aUsers[i], XN_SKEL_TORSO, XN_SKEL_RIGHT_HIP);
DrawLimb(aUsers[i], XN_SKEL_RIGHT_HIP, XN_SKEL_RIGHT_KNEE);
DrawLimb(aUsers[i], XN_SKEL_RIGHT_KNEE, XN_SKEL_RIGHT_FOOT);
DrawLimb(aUsers[i], XN_SKEL_LEFT_HIP, XN_SKEL_RIGHT_HIP);
#ifndef USE_GLES
glEnd();
#endif
}
}
if (g_bPrintFrameID)
{
static XnChar strFrameID[80];
xnOSMemSet(strFrameID, 0, 80);
XnUInt32 nDummy = 0;
xnOSStrFormat(strFrameID, sizeof(strFrameID), &nDummy, "%d", dmd.FrameID());
glColor4f(1, 0, 0, 1);
glRasterPos2i(10, 10);
glPrintString(GLUT_BITMAP_HELVETICA_18, strFrameID);
}
}