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); }