示例#1
0
void addJoint(XnUserID userID, XnSkeletonJoint eJoint, int dstJointID, PackedType<Vector3<double> > & packedVector3) {
	XnSkeletonJointPosition joint;
	g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(userID, eJoint, joint);
	
	if (joint.fConfidence == 0) {
		return;
	}

	XnPoint3D pt;
	pt = joint.position;
	g_DepthGenerator.ConvertRealWorldToProjective(1, &pt, &pt);

		//@see: http://graphics.stanford.edu/~mdfisher/Kinect.html
	static const double fx_d = 1.0/5.9421434211923247e+02;
	static const double fy_d = 1.0/5.9104053696870778e+02;
	static const double cx_d = 3.3930780975300314e+02;
	static const double cy_d = 2.4273913761751615e+02;

	double z = pt.Z/1000;
	double x = (pt.X - cx_d)*z*fx_d;
	double y = (pt.Y - cy_d)*z*fy_d*-1;

	vector<pair<int, Vector3<double> > > & v = packedVector3.getItems();
	v.push_back(pair<int, Vector3<double> >(dstJointID, Vector3<double>(x, y, z)));
}
示例#2
0
bool DumpJoint(XnUserID player, XnSkeletonJoint eJoint, Joint &out_joint)
{
	// Check the user is being tracked
	if (!g_UserGenerator.GetSkeletonCap().IsTracking(player))
	{
		return false;
	}

	// Check the joint is actually there
	if (!g_UserGenerator.GetSkeletonCap().IsJointActive(eJoint))
	{
		return false;
	}

	// Extract joint positions
	XnSkeletonJointPosition joint;
	g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint, joint);

	XnPoint3D pt, imagePt;
	pt = joint.position;

	g_DepthGenerator.ConvertRealWorldToProjective(1, &pt, &imagePt);

	out_joint.id = eJoint;
	out_joint.confidence = joint.fConfidence;
	out_joint.x = pt.X;
	out_joint.y = pt.Y;
	out_joint.z = pt.Z;
	out_joint.u = imagePt.X;
	out_joint.v = imagePt.Y;
	out_joint.w = imagePt.Z;

	return true;
}
示例#3
0
void DrawLimb(XnUserID player, XnSkeletonJoint eJoint1, XnSkeletonJoint eJoint2)
{
	if (!g_UserGenerator.GetSkeletonCap().IsTracking(player))
	{
		printf("not tracked!\n");
		return;
	}

	XnSkeletonJointPosition joint1, joint2;
	g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint1, joint1);
	g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint2, joint2);

	if (joint1.fConfidence < 0.5 || joint2.fConfidence < 0.5)
	{
		return;
	}

	XnPoint3D pt[2];
	pt[0] = joint1.position;
	pt[1] = joint2.position;

	g_DepthGenerator.ConvertRealWorldToProjective(2, pt, pt);
#ifndef USE_GLES
	glVertex3i(pt[0].X, pt[0].Y, 0);
	glVertex3i(pt[1].X, pt[1].Y, 0);
#else
	GLfloat verts[4] = {pt[0].X, pt[0].Y, pt[1].X, pt[1].Y};
	glVertexPointer(2, GL_FLOAT, 0, verts);
	glDrawArrays(GL_LINES, 0, 2);
	glFlush();
#endif
}
示例#4
0
//draws one limb
void DrawLimb(XnUserID player, XnSkeletonJoint eJoint1, XnSkeletonJoint eJoint2)
{
	if (!g_UserGenerator.GetSkeletonCap().IsTracking(player))
	{
		printf("not tracked!\n");
		return;
	}

	XnSkeletonJointPosition joint1, joint2;
	g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint1, joint1);
	g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint2, joint2);

	if (joint1.fConfidence < 0.5 || joint2.fConfidence < 0.5)
	{
		return;
	}

	XnPoint3D pt[2];
	pt[0] = joint1.position;
	pt[1] = joint2.position;

	g_DepthGenerator.ConvertRealWorldToProjective(2, pt, pt);

	glVertex3i(pt[0].X, pt[0].Y, 0);
	glVertex3i(pt[1].X, pt[1].Y, 0);
	//std::cout << pt[0].X << " ," << pt[0].Y << std::endl;
}
示例#5
0
void DrawLimb(XnUserID player, XnSkeletonJoint eJoint1, XnSkeletonJoint eJoint2)
{
	if (!g_UserGenerator.GetSkeletonCap().IsCalibrated(player))
	{
		printf("not calibrated!\n");
		return;
	}
	if (!g_UserGenerator.GetSkeletonCap().IsTracking(player))
	{
		printf("not tracked!\n");
		return;
	}

	XnSkeletonJointPosition joint1, joint2;
	g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint1, joint1);
	g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint2, joint2);

	if (joint1.fConfidence < 0.75 || joint2.fConfidence < 0.75)
	{
		return;
	}

	XnPoint3D pt[2];
	pt[0] = joint1.position;bool isCalibrated = false;
	pt[1] = joint2.position;

	g_DepthGenerator.ConvertRealWorldToProjective(2, pt, pt);
	glVertex3i(pt[0].X, pt[0].Y, 0);
	glVertex3i(pt[1].X, pt[1].Y, 0);
//	printf("Points: %f, %f, %f \n");
}
void DrawJoint(XnUserID player, XnSkeletonJoint eJoint)
{
	if (!g_UserGenerator.GetSkeletonCap().IsTracking(player))
	{
		printf("not tracked!\n");
		return;
	}

	if (!g_UserGenerator.GetSkeletonCap().IsJointActive(eJoint))
	{
		return;
	}

	XnSkeletonJointPosition joint;
	g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint, joint);

	if (joint.fConfidence < 0.5)
	{
		return;
	}

	XnPoint3D pt;
	pt = joint.position;

	g_DepthGenerator.ConvertRealWorldToProjective(1, &pt, &pt);

	drawCircle(pt.X, pt.Y, 2);
}
示例#7
0
void XN_CALLBACK_TYPE Hand_Update(
    xn::HandsGenerator& generator,
    XnUserID nId,
    const XnPoint3D* pPosition,
    XnFloat fTime,
    void* pCookie)
{
  depth_generator.ConvertRealWorldToProjective(1, pPosition, &projective_point);
}
示例#8
0
void ConvertSkeletonMarkersToScreenPoints(
    xn::DepthGenerator& depth_generator,
    std::vector<XnSkeletonJointPosition>& skeleton_markers,
    std::vector<sf::Vector2f>& screen_points)
  {
    for (auto it = skeleton_markers.begin(); it != skeleton_markers.end(); ++it)
    {
      XnPoint3D screen_point;
      depth_generator.ConvertRealWorldToProjective(1, &(it->position), &screen_point);
      screen_points.push_back(sf::Vector2f(screen_point.X, screen_point.Y));
    }
  }
XnPoint3D PointForJoint(XnUserID user, XnSkeletonJoint joint)
{
    XnSkeletonJointPosition jointPos;
    g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, joint, jointPos);
    XnPoint3D pt = jointPos.position;
    
//    printf("%f %f %f confidence %f\n",jointPos.position.X, jointPos.position.Y, jointPos.position.Z, jointPos.fConfidence);

	g_DepthGenerator.ConvertRealWorldToProjective(1, &pt, &pt);
	
	return pt;
}
示例#10
0
文件: kica.cpp 项目: phelrine/kica
void gl_labelCapturingUser(XnUserID id)
{
  XnVector3D pos;
  gUserGenerator.GetCoM(id, pos);
  gDepthGenerator.ConvertRealWorldToProjective(1, &pos, &pos);
  glColor4f(0, 0, 0, 0);
  xn::DepthMetaData depthMD;
  gDepthGenerator.GetMetaData(depthMD);
  glRasterPos2i(pos.X/depthMD.XRes()*WIN_SIZE_X, 
		pos.Y/depthMD.YRes()*WIN_SIZE_Y);
  gl_printString(GLUT_BITMAP_HELVETICA_18, "Capturing");
  glColor4f(1.0, 1.0, 1.0, 1.0);
}
示例#11
0
// スケルトンを描画する
void DrawSkelton(XnUserID player, int idx){

    // 線を引く開始と終了のJointの定義
    XnSkeletonJoint joints[][2] = {
        {XN_SKEL_HEAD, XN_SKEL_NECK},
        {XN_SKEL_NECK, XN_SKEL_LEFT_SHOULDER},
        {XN_SKEL_LEFT_SHOULDER, XN_SKEL_LEFT_ELBOW},
        {XN_SKEL_LEFT_ELBOW, XN_SKEL_LEFT_HAND},
        {XN_SKEL_NECK, XN_SKEL_RIGHT_SHOULDER},
        {XN_SKEL_RIGHT_SHOULDER, XN_SKEL_RIGHT_ELBOW},
        {XN_SKEL_RIGHT_ELBOW, XN_SKEL_RIGHT_HAND},
        {XN_SKEL_LEFT_SHOULDER, XN_SKEL_TORSO},
        {XN_SKEL_RIGHT_SHOULDER, XN_SKEL_TORSO},
        {XN_SKEL_TORSO, XN_SKEL_LEFT_HIP},
        {XN_SKEL_LEFT_HIP, XN_SKEL_LEFT_KNEE},
        {XN_SKEL_LEFT_KNEE, XN_SKEL_LEFT_FOOT},
        {XN_SKEL_TORSO, XN_SKEL_RIGHT_HIP},
        {XN_SKEL_RIGHT_HIP, XN_SKEL_RIGHT_KNEE},
        {XN_SKEL_RIGHT_KNEE, XN_SKEL_RIGHT_FOOT},
        {XN_SKEL_LEFT_HIP, XN_SKEL_RIGHT_HIP}
    };

	XnSkeletonJointPosition joint1, joint2;
    int nJointsCount = sizeof(joints) / sizeof(joints[0]);
    int color_idx = idx;
    if( color_idx > (sizeof(g_Colors) / sizeof(g_Colors[0])) ){
        color_idx = (sizeof(g_Colors) / sizeof(g_Colors[0])) - 1;
    }
       
    for(int i = 0; i < nJointsCount;i++){
        g_SkeletonCap.GetSkeletonJointPosition(player, joints[i][0], joint1);
        g_SkeletonCap.GetSkeletonJointPosition(player, joints[i][1], joint2);        
        if (joint1.fConfidence < 0.2 || joint2.fConfidence < 0.2){
            return;
        }
        
        XnPoint3D pt[2];
        pt[0] = joint1.position;
        pt[1] = joint2.position;
        
        g_DepthGenerator.ConvertRealWorldToProjective(2, pt, pt);
        
        // 線で結んで
        cvLine( g_rgbImage, cvPoint(pt[0].X, pt[0].Y), cvPoint(pt[1].X, pt[1].Y), g_Colors[color_idx], 1, CV_AA);
        // それぞれの点を塗りつぶす
        cvCircle(g_rgbImage, cvPoint(pt[0].X, pt[0].Y), 2, g_Colors[color_idx], -1, CV_AA, 0);
        cvCircle(g_rgbImage, cvPoint(pt[1].X, pt[1].Y), 2, g_Colors[color_idx], -1, CV_AA, 0);
    }
    
}
示例#12
0
/**
 * Body に画像をコラージュ
 */
void drawBodyPreLoad(xn::DepthGenerator& depth, xn::SkeletonCapability& capability, XnSkeletonJoint joint, XnUserID user, XnMapOutputMode mapMode, IplImage* preLoadImage, IplImage **rgbImage) {
	XnSkeletonJointPosition pos;
    
    IplImage *partImage = NULL; 
    IplImage *fallImage = NULL; 
    
	// ジョイント座標の取得
	capability.GetSkeletonJointPosition(user, joint, pos);
	XnPoint3D pReal[1] = {pos.position};
	XnPoint3D pProjective[1];
    
	// 世界座標系から表示座標系に変換した座標を取得
	depth.ConvertRealWorldToProjective(1, pReal, pProjective);
    
    
    // 重ね合わせよう画像
    partImage = preLoadImage;
    fallImage = cvCreateImage(cvSize(mapMode.nXRes, mapMode.nYRes), IPL_DEPTH_8U, 3);    
    CvPoint2D32f original[3];
    CvPoint2D32f transform[3];
    
    original[0] = cvPoint2D32f(0, 0);
    original[1] = cvPoint2D32f(mapMode.nXRes, 0);
    original[2] = cvPoint2D32f( 0, mapMode.nYRes);
    
    CvSize sizeOfPart = cvGetSize(partImage);
    
    int transX = pProjective[0].X - (sizeOfPart.width / 2);
    int transY = pProjective[0].Y - (sizeOfPart.height / 2);
    
    transform[0] = cvPoint2D32f( transX, transY);
    transform[1] = cvPoint2D32f( transX + mapMode.nXRes, transY);
    transform[2] = cvPoint2D32f( transX , transY + mapMode.nYRes);
    
    // 行列作成
    CvMat *affineMatrix = cvCreateMat(2, 3, CV_32FC1);
    cvGetAffineTransform(original, transform, affineMatrix);
    
    // 移動
    cvWarpAffine(partImage, fallImage, affineMatrix, CV_INTER_LINEAR | CV_WARP_FILL_OUTLIERS, cvScalarAll(0));
    
    
    // 画像の重ね合わせ
    fallPartImage(fallImage, *rgbImage);
    
    // 解放
    cvReleaseImage(&fallImage);	
    //cvReleaseImage(&partImage);	
}
示例#13
0
void handtrajectory(xn::UserGenerator& userGenerator,
              xn::DepthGenerator& depthGenerator,
              XnUserID player, XnSkeletonJoint eJoint, bool updateHistory)
{
	XnSkeletonJointPosition joint;
	userGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint, joint);

	if (joint.fConfidence < 0.5){
           return;
        }
            
        XnPoint3D pt_world, pt_screen;
	pt_world = joint.position;
        depthGenerator.ConvertRealWorldToProjective(1, &pt_world, &pt_screen);
      
	History *history;
	if (GetHistoryForJoint (eJoint, &history) == false) return;

	if (updateHistory) history->StoreValue (pt_world, pt_screen); // store value in the history

	// Visualize history
	//
	XnFloat pVertexBuffer [HISTORY_DRAW_SIZE * sizeof (float) * 3];
	XnFloat *pVertex = pVertexBuffer;
	
	// Prepare vertex buffer for drawing
	XnPoint3D pt;
	for (int k = 0; k < history->Size(); ++k) {
		history->GetValueScreen (k, pt);

		*pVertex++ = pt.X;
		*pVertex++ = pt.Y;
		*pVertex++ = 0.0f;
	}

	glColor3f(0.f, 1.f, 0.f);
	glVertexPointer(3, GL_FLOAT, 0, pVertexBuffer);

	// draw trajectory
	glLineWidth(2);
	glDrawArrays(GL_LINE_STRIP, 0, history->Size());

	// draw history points
	glPointSize(8);
	glDrawArrays(GL_POINTS, 0, history->Size());
	
	

}
示例#14
0
 void Skeleton :: computeJoints(int user_id,
                                        xn::UserGenerator& user_generator,
                                        xn::DepthGenerator& depth_generator)
 {
   XnPoint3D ni_p[NumJoints];
   for (int j = 0; j < NumJoints; ++j)
   {
     XnSkeletonJointPosition joint;
     user_generator.GetSkeletonCap().GetSkeletonJointPosition(user_id,
                                                              xnJointList(j),
                                                              joint);
     ni_p[j] = joint.position;
     m_joints[j] = Point3f(ni_p[0].X, ni_p[0].Y, ni_p[0].Z);
   }

   depth_generator.ConvertRealWorldToProjective(NumJoints, ni_p, ni_p);

   for (int j = 0; j < NumJoints; ++j)
   {
     m_projected_joints[j] = Point3f(ni_p[j].X, ni_p[j].Y, ni_p[j].Z);
   }
 }
示例#15
0
XnPoint3D getJointPoint(XnUserID player, XnSkeletonJoint eJoint) {
	  XnPoint3D pt;
    if (!g_UserGenerator.GetSkeletonCap().IsCalibrated(player))
    {
        printf("not calibrated!\n");
        return pt;
    }
    if (!g_UserGenerator.GetSkeletonCap().IsTracking(player))
    {
        printf("not tracked!\n");
        return pt;
    }

    XnSkeletonJointPosition joint;
    g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint, joint);

    if (joint.fConfidence < 0.5)
    {
        return pt;
    }

    g_DepthGenerator.ConvertRealWorldToProjective(2, &joint.position, &pt);
    return pt;
}
示例#16
0
void DrawCircle(xn::UserGenerator& userGenerator,
              xn::DepthGenerator& depthGenerator,
              XnUserID player, XnSkeletonJoint eJoint, float radius, XnFloat *color3f) 
{ 

	XnSkeletonJointPosition joint;
	userGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint, joint);

	if (joint.fConfidence < 0.5){
           return;
        }	

    	XnPoint3D pt;
	pt = joint.position;
        depthGenerator.ConvertRealWorldToProjective(1, &pt, &pt);
        float cx = pt.X;
	float cy = pt.Y;
        float r = radius;
	int num_segments = 16;

	glColor3f(color3f[0], color3f[1], color3f[2]);

	glBegin(GL_TRIANGLE_FAN); 
	glVertex2f(cx , cy);
	for(int i = 0; i <= num_segments; i++) 
	{ 
		float theta = 2.0f * 3.1415926f * float(i) / float(num_segments);//get the current angle 

		float x = r * cosf(theta);//calculate the x component 
		float y = r * sinf(theta);//calculate the y component 

		glVertex2f(x +cx , y + cy);//output vertex 

	} 
	glEnd(); 
}
示例#17
0
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();

    // 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();
    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 = g_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[15];
    XnUInt16 nUsers = 15;
    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);

            xnOSMemSet(strLabel, 0, sizeof(strLabel));
            if (!g_bPrintState)
            {
                // Tracking
                sprintf(strLabel, "%d", aUsers[i]);
            }
            else if (g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i]))
            {
                // Tracking
                sprintf(strLabel, "%d - Tracking", aUsers[i]);
            }
            else if (g_UserGenerator.GetSkeletonCap().IsCalibrating(aUsers[i]))
            {
                // Calibrating
                sprintf(strLabel, "%d - Calibrating [%s]", aUsers[i], GetCalibrationErrorString(m_Errors[aUsers[i]].first));
            }
            else
            {
                // Nothing
                sprintf(strLabel, "%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]))
        {
#ifndef USE_GLES
            glBegin(GL_LINES);
#endif
            glColor4f(1-Colors[aUsers[i]%nColors][0], 1-Colors[aUsers[i]%nColors][1], 1-Colors[aUsers[i]%nColors][2], 1);
            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);
            DrawLimb(aUsers[i], XN_SKEL_LEFT_ELBOW, XN_SKEL_LEFT_HAND);

            DrawLimb(aUsers[i], XN_SKEL_NECK, XN_SKEL_RIGHT_SHOULDER);
            DrawLimb(aUsers[i], XN_SKEL_RIGHT_SHOULDER, XN_SKEL_RIGHT_ELBOW);
            DrawLimb(aUsers[i], XN_SKEL_RIGHT_ELBOW, XN_SKEL_RIGHT_HAND);

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


            //---------------- get skeleton data ----------------------

            g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
                    XN_SKEL_HEAD, head);
            g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
                    XN_SKEL_NECK, neck);
            g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
                    XN_SKEL_TORSO, torso);

            g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
                    XN_SKEL_LEFT_SHOULDER, leftShoulder);
            g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
                    XN_SKEL_LEFT_ELBOW, leftElbow);
            g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
                    XN_SKEL_LEFT_HAND, leftHand);

            g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
                    XN_SKEL_RIGHT_SHOULDER, rightShoulder);
            g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
                    XN_SKEL_RIGHT_ELBOW, rightElbow);
            g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
                    XN_SKEL_RIGHT_HAND, rightHand);

            g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
                    XN_SKEL_LEFT_HIP, leftHip);
            g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
                    XN_SKEL_LEFT_KNEE, leftKnee);
            g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
                    XN_SKEL_LEFT_FOOT, leftFoot);

            g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
                    XN_SKEL_RIGHT_HIP, rightHip);
            g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
                    XN_SKEL_RIGHT_KNEE, rightKnee);
            g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],
                    XN_SKEL_RIGHT_FOOT, rightFoot);

            //-------------- store frame data in frameData ------------
            char headX[30], headY[30], headZ[30], headC[30];
            strcpy(frameData, "");
            char space[2] = " ";

            sprintf(headX, "%f", head.position.X);
            sprintf(headY, "%f", head.position.Y);
            sprintf(headZ, "%f", head.position.Z);
            sprintf(headC, "%f", head.fConfidence);

            strcat(frameData, headX);
            strcat(frameData, space);
            strcat(frameData, headY);
            strcat(frameData, space);
            strcat(frameData, headZ);
            strcat(frameData, space);
            strcat(frameData, headC);
            strcat(frameData, space);

            //std::cout << "Frame data: " << frameData << std::endl;
            std::cout << "frameData to be sent: " << frameData << std::endl;
            HandleConnection(frameData);



            ////------- print to console -------
            //xnOSGetTimeStamp(&nNow);
            //nNow /= 1000;
            //std::cout << "-------------time: " << nNow << "---------------\n"
            //	<< "User " << aUsers[i] <<std::endl
            //	<< "	Head: " << head.position.X << " "
            //	<< head.position.Y << " " << head.position.Z
            //	<< head.fConfidence << std::endl;


#ifndef USE_GLES
            glEnd();
#endif
        }
    }
}
示例#18
0
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();

	// 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();
	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 = g_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[15];
	XnUInt16 nUsers = 15;
	g_UserGenerator.GetUsers(aUsers, nUsers);
	for (int i = 0; i < nUsers; ++i)
	{

		if (g_bPrintID)
		{
			XnPoint3D com;
			g_UserGenerator.GetCoM(aUsers[i], com);
			g_DepthGenerator.ConvertRealWorldToProjective(1, &com, &com);

			xnOSMemSet(strLabel, 0, sizeof(strLabel));
			if (!g_bPrintState)
			{
				// Tracking
				sprintf(strLabel, "%d", aUsers[i]);
			}
			else if (g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i]))
			{
				// Tracking
				sprintf(strLabel, "%d - Tracking", aUsers[i]);
			}
			else if (g_UserGenerator.GetSkeletonCap().IsCalibrating(aUsers[i]))
			{
				// Calibrating
				sprintf(strLabel, "%d - Calibrating [%s]", aUsers[i], GetCalibrationErrorString(m_Errors[aUsers[i]].first));
			}
			else
			{
				// Nothing
				sprintf(strLabel, "%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);
		}

		//Draw skeleton
		if (g_bDrawSkeleton && g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i]))
		{

			glBegin(GL_LINES);

			glColor4f(1-Colors[aUsers[i]%nColors][0], 1-Colors[aUsers[i]%nColors][1], 1-Colors[aUsers[i]%nColors][2], 1);
			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);
			DrawLimb(aUsers[i], XN_SKEL_LEFT_ELBOW, XN_SKEL_LEFT_HAND);

			DrawLimb(aUsers[i], XN_SKEL_NECK, XN_SKEL_RIGHT_SHOULDER);
			DrawLimb(aUsers[i], XN_SKEL_RIGHT_SHOULDER, XN_SKEL_RIGHT_ELBOW);
			DrawLimb(aUsers[i], XN_SKEL_RIGHT_ELBOW, XN_SKEL_RIGHT_HAND);

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

			glEnd();

		}
	}
}
示例#19
0
//左上腕(LEFT_ELBOW - LEFT_SHOULDER)に画像をコラージュ
void drawLeftArm(xn::DepthGenerator& depth, xn::SkeletonCapability& capability, XnUserID user, XnMapOutputMode mapMode, IplImage* preLoadImage, IplImage **rgbImage) {
	XnSkeletonJointPosition pos1,pos2;
  
    IplImage *partImage = NULL; 
    IplImage *fallImage = NULL; 
    
	// elbowのジョイント座標の取得
	capability.GetSkeletonJointPosition(user, XN_SKEL_LEFT_ELBOW, pos1);
	XnPoint3D pReal1[1] = {pos1.position};
	XnPoint3D pProjective1[1];
	// 世界座標系から表示座標系に変換した座標を取得
	depth.ConvertRealWorldToProjective(1, pReal1, pProjective1);
    
    // sholderのジョイント座標の取得
	capability.GetSkeletonJointPosition(user, XN_SKEL_LEFT_SHOULDER, pos2);
	XnPoint3D pReal2[1] = {pos2.position};
	XnPoint3D pProjective2[1];
	// 世界座標系から表示座標系に変換した座標を取得
	depth.ConvertRealWorldToProjective(1, pReal2, pProjective2);
    
    // 重ね合わせよう画像
    partImage = preLoadImage;
    fallImage = cvCreateImage(cvSize(mapMode.nXRes, mapMode.nYRes), IPL_DEPTH_8U, 3);    
    CvPoint2D32f original[3];
    CvPoint2D32f transform[3];
    
    original[0] = cvPoint2D32f(0, 0);
    original[1] = cvPoint2D32f(mapMode.nXRes, 0);
    original[2] = cvPoint2D32f( 0, mapMode.nYRes);
    
    CvSize sizeOfPart = cvGetSize(partImage);
    
    //ELBOWの位置
    int transX1 = pProjective1[0].X;
    int transY1 = pProjective1[0].Y;
    //SHOULDERの位置
    int transX2 = pProjective2[0].X;
    int transY2 = pProjective2[0].Y;
    //中間の座標
    int transX3 = (transX1 + transX2) / 2;
    int transY3 = (transY1 + transY2) / 2;
    //画像の始点
    int transX = transX3 - (sizeOfPart.width / 2);
    int transY = transY3 - (sizeOfPart.height / 2);
    //角度
    float ang = cvFastArctan(transY2 - transY1, transX2 - transX1); //+ cvFastArctan(transY1, transX1);
    
    transform[0] = cvPoint2D32f( transX, transY);
    transform[1] = cvPoint2D32f( transX + mapMode.nXRes, transY);
    transform[2] = cvPoint2D32f( transX, transY + mapMode.nYRes);
    
    // 行列作成
    CvMat *affineMatrix = cvCreateMat(2, 3, CV_32FC1);
    cvGetAffineTransform(original, transform, affineMatrix);
    
    // 移動
    cvWarpAffine(partImage, fallImage, affineMatrix, CV_INTER_LINEAR | CV_WARP_FILL_OUTLIERS, cvScalarAll(0));
    
    
    //回転行列作成
    CvPoint2D32f center = cvPoint2D32f(transX3, transY3);
    IplImage *fallImage2 = cvCreateImage(cvSize(mapMode.nXRes, mapMode.nYRes), IPL_DEPTH_8U, 3); 
    CvMat *rotationMatrix = cvCreateMat(2, 3, CV_32FC1);
    cv2DRotationMatrix(center, 90.0 - ang, 1.0, rotationMatrix);
    //回転
    cvWarpAffine(fallImage, fallImage2, rotationMatrix, CV_INTER_LINEAR | CV_WARP_FILL_OUTLIERS, cvScalarAll(0));
    
    // 画像の重ね合わせ
    fallPartImage(fallImage2, *rgbImage);
    
    // 解放
    cvReleaseImage(&fallImage);	
    cvReleaseImage(&fallImage2);	
}
示例#20
0
cv::Point xncv::worldToProjective(const XnPoint3D& point, const xn::DepthGenerator& depth)
{
	XnPoint3D p;
	depth.ConvertRealWorldToProjective(1, &point, &p);
	return cv::Point(static_cast<int>(p.X), static_cast<int>(p.Y));
}
/* The matlab mex function */
void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
{
    XnUInt64 *MXadress;
    double *Pos;
    
    int Jdimsc[2];
    Jdimsc[0]=225; Jdimsc[1]=7;
    plhs[0] = mxCreateNumericArray(2, Jdimsc, mxDOUBLE_CLASS, mxREAL);
    Pos = mxGetPr(plhs[0]);
     
    if(nrhs==0)
    {
       printf("Open failed: Give Pointer to Kinect as input\n");
       mexErrMsgTxt("Kinect Error"); 
    }
        
    MXadress = (XnUInt64*)mxGetData(prhs[0]);
    if(MXadress[0]>0){ g_Context = ((xn::Context*) MXadress[0])[0]; }
    if(MXadress[2]>0)
	{ 
		g_DepthGenerator = ((xn::DepthGenerator*) MXadress[2])[0]; 
	}
	else
	{
		mexErrMsgTxt("No Depth Node in Kinect Context"); 
	}
    if(MXadress[4]>0)
	{ 
		g_UserGenerator = ((xn::UserGenerator*) MXadress[4])[0]; 
	}
    else
	{
		mexErrMsgTxt("No User Node in Kinect Context"); 
	}

    XnStatus nRetVal = XN_STATUS_OK;

    XnCallbackHandle hUserCallbacks, hCalibrationCallbacks, hPoseCallbacks;
    if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON))
    {
        printf("Supplied user generator doesn't support skeleton\n");
        return;
    }
    g_UserGenerator.RegisterUserCallbacks(User_NewUser, User_LostUser, NULL, hUserCallbacks);
    g_UserGenerator.GetSkeletonCap().RegisterCalibrationCallbacks(UserCalibration_CalibrationStart, UserCalibration_CalibrationEnd, NULL, hCalibrationCallbacks);

    if (g_UserGenerator.GetSkeletonCap().NeedPoseForCalibration())
    {
        g_bNeedPose = TRUE;
        if (!g_UserGenerator.IsCapabilitySupported(XN_CAPABILITY_POSE_DETECTION))
        {
            printf("Pose required, but not supported\n");
            return;
        }
        g_UserGenerator.GetPoseDetectionCap().RegisterToPoseCallbacks(UserPose_PoseDetected, NULL, NULL, hPoseCallbacks);
        g_UserGenerator.GetSkeletonCap().GetCalibrationPose(g_strPose);
    }

    g_UserGenerator.GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL);
 
    char strLabel[50] = "";
    XnUserID aUsers[15];
    XnUInt16 nUsers = 15;
    int r=0;
    xn::SceneMetaData sceneMD;
	xn::DepthMetaData depthMD;
	
    // Process the data
    g_DepthGenerator.GetMetaData(depthMD);
    g_UserGenerator.GetUserPixels(0, sceneMD);
    g_UserGenerator.GetUsers(aUsers, nUsers);
 	for (int i = 0; i < nUsers; ++i)
 	{
        if (g_UserGenerator.GetSkeletonCap().IsTracking(aUsers[i]))
		{
            //printf(strLabel, "%d - Looking for pose", aUsers[i]);
   
            XnSkeletonJointPosition joint[15];
            g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_HEAD, joint[0]);
            g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_NECK, joint[1]);
            g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_LEFT_SHOULDER, joint[2]);
            g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_LEFT_ELBOW, joint[3]);
            g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_LEFT_HAND, joint[4]);
            g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_RIGHT_SHOULDER, joint[5]);
            g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_RIGHT_ELBOW, joint[6]);
            g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_RIGHT_HAND, joint[7]);
            g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_TORSO, joint[8]);
            g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_LEFT_HIP, joint[9]);
            g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_LEFT_KNEE, joint[10]);
            g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_LEFT_FOOT, joint[11]);
            g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_RIGHT_HIP, joint[12]);
            g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_RIGHT_KNEE, joint[13]);
            g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i],XN_SKEL_RIGHT_FOOT, joint[14]);

            XnPoint3D pt[1];
            for(int j=0; j<15; j++)
            {
                Pos[j            +r]=aUsers[i];
                Pos[j+Jdimsc[0]  +r]=joint[j].fConfidence;
                Pos[j+Jdimsc[0]*2+r]=joint[j].position.X;
                Pos[j+Jdimsc[0]*3+r]=joint[j].position.Y;
                Pos[j+Jdimsc[0]*4+r]=joint[j].position.Z;
                pt[0] = joint[j].position;
                g_DepthGenerator.ConvertRealWorldToProjective(1, pt, pt);
                Pos[j+Jdimsc[0]*5+r]=pt[0].X;
                Pos[j+Jdimsc[0]*6+r]=pt[0].Y;
            }        
            r+=15;
        }
     }
            
}
示例#22
0
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 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 not initialized
	set parameters and reserve memory space
	*/
	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();

	// get the depth resolution
	static unsigned int nZRes = dmd.ZRes();
	static float* pDepthHist = (float*)malloc(nZRes* sizeof(float));

	// Calculate the accumulative histogram
	memset(pDepthHist, 0, nZRes*sizeof(float));

	// count the number of pixels of every possible depth value
	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];
	}

	// calculate percentage for every depth value
	// the larger the value is, the darker the pixel should be 
	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[15];
	XnUInt16 nUsers = 15;
	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);
	}
}
示例#24
0
文件: Test.cpp 项目: robotota/edu
/**********************************************************************************
Following the point created method, any update in the hand point coordinates are 
reflected through this event handler
**********************************************************************************/
void XN_CALLBACK_TYPE pointUpdate(const XnVHandPointContext *pContext, void *cxt){
	XnPoint3D coords(pContext->ptPosition);
	depthGen.ConvertRealWorldToProjective(1,&coords,&handPointCoords);	
}
示例#25
0
void DrawJoint(XnUserID player, XnSkeletonJoint eJoint)
{
	if (!g_UserGenerator.GetSkeletonCap().IsTracking(player))
	{
		printf("not tracked!\n");
		return;
	}

	if (!g_UserGenerator.GetSkeletonCap().IsJointActive(eJoint))
	{
		return;
	}

	XnSkeletonJointPosition joint;
	g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint, joint);

	if (joint.fConfidence < 0.5)
	{
		return;
	}

	XnPoint3D pt;
	pt = joint.position;

	g_DepthGenerator.ConvertRealWorldToProjective(1, &pt, &pt);

	drawCircle(pt.X, pt.Y, 2);

	//////////////////////////////////code/////////////////////////////////////////


	if(eJoint==XN_SKEL_LEFT_HAND && tmp==0 && player==1){//左手
		//printf("ske,%d,0,%f,%f,%f,",player,pt.X,pt.Y,pt.Z);
		//fflush(stdout);
		za_tmpX[zaC]=pt.X;
		za_tmpY[zaC]=pt.Y;
		za_tmpZ[zaC]=pt.Z;
		H_L_X=pt.X;
		H_L_Y=pt.Y;
		H_L_Z=pt.Z;
		tmp=1;
		zaC++;

	}else if(eJoint==XN_SKEL_RIGHT_HAND && tmp==1 && player==1){//右手
		//printf("ske,%d,1,%f,%f,%f,",player,pt.X,pt.Y,pt.Z);
		za_tmpX[zaC]=pt.X;
		za_tmpY[zaC]=pt.Y;
		za_tmpZ[zaC]=pt.Z;
		H_R_X=pt.X;
		H_R_Y=pt.Y;
		H_R_Z=pt.Z;
		tmp=0;
		zaC++;
		LRh=1;
		//printf("zaC=%d\n",zaC);
	}
	


	//左zaC_MIN<-0,2,4,6,8,10,12,14=zaC_MAX-2
	//右zaC_MIN+1<-1,3...15=zaC_MAX-1
	//za_tmpなんとか
	//ズーム判定
	//else if(H_L_Z>kijyunZ && H_R_Z<kijyunZ && zaC==zaC_MAX && player==1){//X,Yの動き判定(右手)
	if(H_L_Z>kijyunZ && H_R_Z<kijyunZ && zaC==zaC_MAX && player==1){//X,Yの動き判定(右手)
		/*
		if((za_tmpX[zaC_MAX-1]-za_tmpX[zaC_MIN+1])<=0){//左方向に進んだ
			X=X+abs(za_tmpX[zaC_MAX-1]-za_tmpX[zaC_MIN+1]);
		}else if((za_tmpX[zaC_MAX-1]-za_tmpX[zaC_MIN+1])>=0){//右方向に進んだ
			X=X-(za_tmpX[zaC_MAX-1]-za_tmpX[zaC_MIN+1]);
		}

		if((za_tmpY[zaC_MAX-1]-za_tmpY[zaC_MIN+1])<=0){//上方向に進んだ
			Y=Y+abs(za_tmpY[zaC_MAX-1]-za_tmpY[zaC_MIN+1]);
		}else if((za_tmpY[zaC_MAX-1]-za_tmpY[zaC_MIN+1])>=0){//下方向に進んだ
			Y=Y-(za_tmpY[zaC_MAX-1]-za_tmpY[zaC_MIN+1]);
		}
		*/

		if((za_tmpX[zaC_MAX-1]-za_tmpX[zaC_MIN+1])<=0){//左方向に進んだ
			X=X+(za_tmpX[zaC_MAX-1]-za_tmpX[zaC_MIN+1]);
		}else if((za_tmpX[zaC_MAX-1]-za_tmpX[zaC_MIN+1])>=0){//右方向に進んだ
			X=X+abs(za_tmpX[zaC_MAX-1]-za_tmpX[zaC_MIN+1]);
		}

		if((za_tmpY[zaC_MAX-1]-za_tmpY[zaC_MIN+1])<=0){//上方向に進んだ
			Y=Y+(za_tmpY[zaC_MAX-1]-za_tmpY[zaC_MIN+1]);
		}else if((za_tmpY[zaC_MAX-1]-za_tmpY[zaC_MIN+1])>=0){//下方向に進んだ
			Y=Y+abs(za_tmpY[zaC_MAX-1]-za_tmpY[zaC_MIN+1]);
		}



		/*
		if(X>=10000) X=10000;
		if(X<=(-10000)) X=(-10000);

		if(Y>=10000) X=10000;
		if(Y<=(-10000)) Y=(-10000);
		*/
		

		printf("ske,%d,1,%f,%f,%f,%f,%f,%f,%f,%f,\n",player,za_tmpX[zaC_MAX-2],za_tmpY[zaC_MAX-2],za_tmpZ[zaC_MAX-2],za_tmpX[zaC_MAX-1],za_tmpY[zaC_MAX-1],za_tmpZ[zaC_MAX-1],X,Y);
		//printf("X=%f,Y=%f\n",X,Y);
		fflush(stdout);
	
	
	}else if(H_L_Z<kijyunZ && H_R_Z<kijyunZ && zaC==zaC_MAX && player==1){//両手判定
		/*
		float scaleX_tmp=0;
		float scaleY_tmp=0;
		scaleX_tmp=abs((za_tmpX[zaC_MAX-2]-za_tmpX[zaC_MAX-1])/(za_tmpX[zaC_MIN]-za_tmpX[zaC_MIN+1]));
		scaleY_tmp=abs((za_tmpY[zaC_MAX-2]-za_tmpY[zaC_MAX-1])/(za_tmpY[zaC_MIN]-za_tmpY[zaC_MIN+1]));

		if(scaleX_tmp>=1) scaleX=scaleX+(scaleX_tmp*0.1);
		else if(scaleX_tmp<=1) scaleX=scaleX-((1/scaleX_tmp)*0.1);

		if(scaleY_tmp>=1) scaleY=scaleY+(scaleY_tmp*0.1);
		else if(scaleY_tmp<=1) scaleY=scaleY-((1/scaleY_tmp)*0.1);
		
		if(scaleX<=0.01) scaleX=0.01;
		if(scaleX>=100) scaleX=100;

		if(scaleY<=0.01) scaleY=0.01;
		if(scaleY>=100) scaleY=100;

		*/

		float scaleX_tmp=0;
		float scaleY_tmp=0;
		float scaleX_tmp2=0;
		float scaleY_tmp2=0;

		
		scaleX_tmp=abs((za_tmpX[zaC_MAX-2]-za_tmpX[zaC_MAX-1])/(za_tmpX[zaC_MIN]-za_tmpX[zaC_MIN+1]));
		scaleY_tmp=abs((za_tmpY[zaC_MAX-2]-za_tmpY[zaC_MAX-1])/(za_tmpY[zaC_MIN]-za_tmpY[zaC_MIN+1]));
		
		//scaleX_tmp=SG( scaleX_tmp2*10.0 )/10.0;
		//scaleY_tmp=SG( scaleY_tmp2*10.0 )/10.0;

		if(scaleX_tmp>=1) scaleX=scaleX+(scaleX_tmp*0.01);
		else if(scaleX_tmp<=1) scaleX=scaleX-((1/scaleX_tmp)*0.01);

		if(scaleY_tmp>=1) scaleY=scaleY+(scaleY_tmp*0.01);
		else if(scaleY_tmp<=1) scaleY=scaleY-((1/scaleY_tmp)*0.01);

		
		if(scaleX<=0.01) scaleX=0.01;
		if(scaleX>=100) scaleX=80;

		if(scaleY<=0.01) scaleY=0.01;
		if(scaleY>=100) scaleY=80;


		
		//printf("scaleX=%f\nscaleY=%f\n",scaleX,scaleY);//テスト
		printf("ske,%d,2,%f,%f,%f,%f,%f,%f,%f,%f,\n",player,za_tmpX[zaC_MAX-2],za_tmpY[zaC_MAX-2],za_tmpZ[zaC_MAX-2],za_tmpX[zaC_MAX-1],za_tmpY[zaC_MAX-1],za_tmpZ[zaC_MAX-1],scaleX,scaleY);
		fflush(stdout);

	}else{
			if(LRh==1 && player==1){
				printf("ske,%d,0,%f,%f,%f,%f,%f,%f,\n",player,H_L_X,H_L_Y,H_L_Z,H_R_X,H_R_Y,H_R_Z);
				//printf("Z=%f\n",H_L_Z);
				fflush(stdout);
				LRh=0;
		
		}
	}
	
	if(zaC==zaC_MAX)zaC=0;
	//if(LRh==1)LRh=0;


	fflush(stdout);



	//////////////////////////////////code/////////////////////////////////////////

}