void Tractor::joinTrailer(ITrailer* trailer)
{
if(m_joinedTrailer && m_joint)
return;
NxD6JointDesc joint;
Vec3 move = getJointPoint() - trailer->getJointPoint();
//if(trailer->getJointPoint().y < getJointPoint().y)
//trailer->getActor()->setGlobalPosition(trailer->getActor()->getGlobalPosition() + NxVec3(0, getJointPoint().y - trailer->getJointPoint().y, 0));
trailer->getActor()->setGlobalPosition(trailer->getActor()->getGlobalPosition() + NxVec3(move));
joint.actor[0] = getActor();
joint.actor[1] = trailer->getActor();
joint.setGlobalAnchor(NxVec3(getJointPoint()));
joint.setGlobalAxis(NxVec3(-m_forward));
joint.xMotion = NX_D6JOINT_MOTION_LOCKED;
joint.yMotion = NX_D6JOINT_MOTION_LOCKED;
joint.zMotion = NX_D6JOINT_MOTION_LOCKED;
joint.swing1Motion = NX_D6JOINT_MOTION_LIMITED;
joint.swing2Motion = NX_D6JOINT_MOTION_LIMITED;
joint.twistMotion = NX_D6JOINT_MOTION_LIMITED;
joint.swing1Limit.value = 0.3f;
joint.swing2Limit.value = 1.8f;
joint.twistLimit.low.value = -0.1f;
joint.twistLimit.high.value = 0.1f;
m_joint = m_scene->createJoint(joint);
m_joinedTrailer = trailer;
m_joinedTrailer->joinVehicle();
return;
}
void DrawDepthMap(const xn::DepthMetaData& dmd, const xn::SceneMetaData& smd, XnUserID player)
{
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();
// Calculate the accumulative histogram
memset(g_pDepthHist, 0, MAX_DEPTH*sizeof(float));
for (nY=0; nY<g_nYRes; nY++)
{
for (nX=0; nX<g_nXRes; nX++)
{
nValue = *pDepth;
if (nValue != 0)
{
g_pDepthHist[nValue]++;
nNumberOfPoints++;
}
pDepth++;
}
}
for (nIndex=1; nIndex<MAX_DEPTH; nIndex++)
{
g_pDepthHist[nIndex] += g_pDepthHist[nIndex-1];
}
if (nNumberOfPoints)
{
for (nIndex=1; nIndex<MAX_DEPTH; nIndex++)
{
g_pDepthHist[nIndex] = (unsigned int)(256 * (1.0f - (g_pDepthHist[nIndex] / nNumberOfPoints)));
}
}
pDepth = dmd.Data();
{
XnUInt32 nIndex = 0;
// Prepare the texture map
for (nY=0; nY<g_nYRes; nY++)
{
for (nX=0; nX < g_nXRes; nX++, nIndex++)
{
nValue = *pDepth;
XnLabel label = *pLabels;
XnUInt32 nColorID = label % nColors;
if (label == 0)
{
nColorID = nColors;
}
if (nValue != 0)
{
nHistValue = g_pDepthHist[nValue];
pDestImage[0] = nHistValue * Colors[nColorID][0];
pDestImage[1] = nHistValue * Colors[nColorID][1];
pDestImage[2] = nHistValue * Colors[nColorID][2];
}
else
{
pDestImage[0] = 0;
pDestImage[1] = 0;
pDestImage[2] = 0;
}
pDepth++;
pLabels++;
pDestImage+=3;
}
pDestImage += (texWidth - g_nXRes) *3;
}
}
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[20] = "";
XnUserID aUsers[15];
XnUInt16 nUsers = 15;
g_UserGenerator.GetUsers(aUsers, nUsers);
for (int i = 0; i < nUsers; ++i)
{
XnPoint3D com;
g_UserGenerator.GetCoM(aUsers[i], com);
g_DepthGenerator.ConvertRealWorldToProjective(1, &com, &com);
if (aUsers[i] == player)
sprintf(strLabel, "%d (Player)", aUsers[i]);
else
sprintf(strLabel, "%d", aUsers[i]);
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);
}
// Draw skeleton of user
if (player != 0)
{
glBegin(GL_LINES);
glColor4f(1-Colors[player%nColors][0], 1-Colors[player%nColors][1], 1-Colors[player%nColors][2], 1);
// gesture
static int gesture = 0;
static XnPoint3D previousLeftHandPt;
XnPoint3D newLeftHandPt;
newLeftHandPt = getJointPoint(player, XN_SKEL_LEFT_HAND);
if(previousLeftHandPt.X > 0 && previousLeftHandPt.X < 640)
if(previousLeftHandPt.X - newLeftHandPt.X > 60)
gesture = 1;
else if(previousLeftHandPt.X - newLeftHandPt.X < -60)
gesture = 2;
else if(previousLeftHandPt.Y - newLeftHandPt.Y > 60)
gesture = 3;
else if(previousLeftHandPt.Y - newLeftHandPt.Y < -60)
gesture = 4;
else
gesture = 0;
if(gesture != 0)
printf("gesture: %d\n", gesture);
previousLeftHandPt = newLeftHandPt;
// head
XnPoint3D pt = getJointPoint(player, XN_SKEL_HEAD);
// save
saveLocation(pt, newLeftHandPt, gesture);
DrawLimb(player, XN_SKEL_HEAD, XN_SKEL_NECK);
DrawLimb(player, XN_SKEL_NECK, XN_SKEL_LEFT_SHOULDER);
DrawLimb(player, XN_SKEL_LEFT_SHOULDER, XN_SKEL_LEFT_ELBOW);
DrawLimb(player, XN_SKEL_LEFT_ELBOW, XN_SKEL_LEFT_HAND);
DrawLimb(player, XN_SKEL_NECK, XN_SKEL_RIGHT_SHOULDER);
DrawLimb(player, XN_SKEL_RIGHT_SHOULDER, XN_SKEL_RIGHT_ELBOW);
DrawLimb(player, XN_SKEL_RIGHT_ELBOW, XN_SKEL_RIGHT_HAND);
DrawLimb(player, XN_SKEL_LEFT_SHOULDER, XN_SKEL_TORSO);
DrawLimb(player, XN_SKEL_RIGHT_SHOULDER, XN_SKEL_TORSO);
DrawLimb(player, XN_SKEL_TORSO, XN_SKEL_LEFT_HIP);
DrawLimb(player, XN_SKEL_LEFT_HIP, XN_SKEL_LEFT_KNEE);
DrawLimb(player, XN_SKEL_LEFT_KNEE, XN_SKEL_LEFT_FOOT);
DrawLimb(player, XN_SKEL_TORSO, XN_SKEL_RIGHT_HIP);
DrawLimb(player, XN_SKEL_RIGHT_HIP, XN_SKEL_RIGHT_KNEE);
DrawLimb(player, XN_SKEL_RIGHT_KNEE, XN_SKEL_RIGHT_FOOT);
glEnd();
}
}
void printJointPoint(XnUserID player, XnSkeletonJoint eJoint) {
XnPoint3D pt = getJointPoint(player, eJoint);
printf("%f, %f, %f \n", pt.X, pt.Y, pt.Z);
}
