int GetLimb(XnUserID user, cv::Mat *body, cv::Mat *skin, XnSkeletonJoint joint1, XnSkeletonJoint joint2, int w, cv::Size size, cv::Point2i pos)
{
    XnPoint3D p1 = PointForJoint(user, joint1);
    XnPoint3D p2 = PointForJoint(user, joint2);
    if (!PointIsValid(p1) || !PointIsValid(p2)) return -1;
    
    float dx = p1.X-p2.X;
    float dy = p1.Y-p2.Y;
    float l = sqrt(dx*dx+dy*dy);
    dx /= l;
    dy /= l;
    
//    printf("p1 (%f, %f) (%f, %f) (%f, %f) p2 (%f, %f) (%f, %f) (%f, %f)\n", p1.X+(w/2)*dy, p1.Y-(w/2)*dx, p1.X, p1.Y, p1.X-(w/2)*dy, p1.Y+(w/2)*dx,
//    p2.X+(w/2)*dy, p2.Y-(w/2)*dx, p2.X, p2.Y, p2.X-(w/2)*dy, p2.Y+(w/2)*dx);
    cv::Point2f cameraPoints[] = {cv::Point2f(p1.X+(w/2)*dy, p1.Y-(w/2)*dx), cv::Point2f(p1.X-(w/2)*dy, p1.Y+(w/2)*dx),
                                  cv::Point2f(p2.X+(w/2)*dy, p2.Y-(w/2)*dx), cv::Point2f(p2.X-(w/2)*dy, p2.Y+(w/2)*dx)};
    
    cv::Point2f skinPoints[] = {cv::Point2f(0, 0), cv::Point2f(size.width-1, 0),
                                cv::Point2f(0, size.height-1), cv::Point2f(size.width-1, size.height-1)};
    
    cv::Mat transform = cv::getPerspectiveTransform(cameraPoints, skinPoints);
    cv::Mat transformed = cv::Mat(size, CV_8UC3);
    cv::warpPerspective(*body, transformed, transform, size);
    
    CopyBodyPart(&transformed, skin, pos);
    
    return 0;
}
CRhinoGet::result CRhGetRegionPoints::GetPoints()
{
    CRhinoGet::result rc = CRhinoGet::cancel;

    m_points.Reserve( 2048 );
    m_points.SetCount( 0 );
    m_prev_point = CPoint( -1, -1 );

    AcceptNothing();
    ConstrainToTargetPlane();
    EnableObjectSnapCursors( false );
    PermitObjectSnap( false );
    m_2dClamp = true;

    for (;;)
    {
        m_temp_point = CPoint( -1, -1 );

        rc = GetPoint( 0, true );

        if( rc == CRhinoGet::point )
        {
            CPoint pt = Point2d();
            if( PointIsValid(pt) )
            {
                m_prev_point = pt;
                m_points.Append( pt );
            }
            continue;
        }

        if( rc == CRhinoGet::nothing )
        {
            if( PointIsValid(m_temp_point) )
                m_points.Append( m_temp_point );

            if( m_points.Count() > 2 )
                rc = CRhinoGet::point;
        }

        break;
    }

    return rc;
}
int GetHead(XnUserID user, cv::Mat *body, cv::Mat *skin)
{
    XnPoint3D h = PointForJoint(user, XN_SKEL_HEAD);
    if (!PointIsValid(h)) return -1;
    
    int w = 12;
    cv::Point2f tl = cv::Point2f(h.X+w, h.Y-w*2.0);
    cv::Point2f tr = cv::Point2f(h.X-w, h.Y-w*2.0);
    cv::Point2f bl = cv::Point2f(h.X+w*0.75, h.Y+w*1.5);
    cv::Point2f br = cv::Point2f(h.X-w*0.75, h.Y+w*1.5);
    
    cv::Point2f xoffset = cv::Point2f(1.0, 0.0);
    cv::Point2f yoffset = cv::Point2f(0.0, 1.0);
    
    cv::Point2f facePoints[] = {tl, tr, bl, br};
    cv::Point2f leftPoints[] = {tl+xoffset*4.0, tl, bl+xoffset*4.0, bl};
    cv::Point2f rightPoints[] = {tr, tr-xoffset*4.0, br, br-xoffset*4.0};
    cv::Point2f topPoints[] = {tl-yoffset*4.0, tr-yoffset*4.0, tl, tr};
    cv::Point2f bottomPoints[] = {bl, br, bl+yoffset*4.0, br+yoffset*4.0};
    
    cv::Point2f skinPoints[] = {cv::Point2f(7, 0), cv::Point2f(0, 0), cv::Point2f(7, 7), cv::Point2f(0, 7)};
    cv::Size size = cv::Size(8, 8);
    
    cv::Mat transform = cv::getPerspectiveTransform(facePoints, skinPoints);
    cv::Mat transformed = cv::Mat(size, CV_8UC3);
    cv::warpPerspective(*body, transformed, transform, size);
    CleanFace(&transformed);
    CopyBodyPart(&transformed, skin, cv::Point2i(8, 8));
    
    transform = cv::getPerspectiveTransform(leftPoints, skinPoints);
    transformed = cv::Mat(size, CV_8UC3);
    cv::warpPerspective(*body, transformed, transform, size);
    CopyBodyPart(&transformed, skin, cv::Point2i(16, 8));
    
    transform = cv::getPerspectiveTransform(rightPoints, skinPoints);
    transformed = cv::Mat(size, CV_8UC3);
    cv::warpPerspective(*body, transformed, transform, size);
    CopyBodyPart(&transformed, skin, cv::Point2i(0, 8));
    
    transform = cv::getPerspectiveTransform(topPoints, skinPoints);
    transformed = cv::Mat(size, CV_8UC3);
    cv::warpPerspective(*body, transformed, transform, size);
    CopyBodyPart(&transformed, skin, cv::Point2i(8, 0));
    
    // Use the forehead/top area as the back as well
    transform = cv::getPerspectiveTransform(topPoints, skinPoints);
    transformed = cv::Mat(size, CV_8UC3);
    cv::warpPerspective(*body, transformed, transform, size);
    CopyBodyPart(&transformed, skin, cv::Point2i(24, 8));
    
    transform = cv::getPerspectiveTransform(bottomPoints, skinPoints);
    transformed = cv::Mat(size, CV_8UC3);
    cv::warpPerspective(*body, transformed, transform, size);
    CopyBodyPart(&transformed, skin, cv::Point2i(16, 0));
    
    return 0;
}
int GetTorso(XnUserID user, cv::Mat *body, cv::Mat *skin)
{
    XnPoint3D ls = PointForJoint(user, XN_SKEL_LEFT_SHOULDER);
    XnPoint3D rs = PointForJoint(user, XN_SKEL_RIGHT_SHOULDER);
    XnPoint3D lh = PointForJoint(user, XN_SKEL_LEFT_HIP);
    XnPoint3D rh = PointForJoint(user, XN_SKEL_RIGHT_HIP);
    if (!PointIsValid(ls) || !PointIsValid(rs) || !PointIsValid(lh) || !PointIsValid(rh)) return -1;
    
//    printf("(%f,%f,%f) (%f, %f, %f) (%f, %f, %f) (%f, %f, %f)\n", ls.X, ls.Y, ls.Z, rs.X, rs.Y, rs.Z, lh.X, lh.Y, lh.Z, rh.X, rh.Y, rh.Z);
    
    cv::Point2f cameraPoints[] = {cv::Point2f(ls.X, ls.Y), cv::Point2f(rs.X, rs.Y), cv::Point2f(lh.X, lh.Y), cv::Point2f(rh.X, rh.Y)};
    cv::Point2f skinPoints[] = {cv::Point2f(7, 0), cv::Point2f(0, 0), cv::Point2f(7, 11), cv::Point2f(0, 11)};
    
    cv::Size size = cv::Size(8, 12);
    cv::Mat transform = cv::getPerspectiveTransform(cameraPoints, skinPoints);
    
    cv::Mat transformed = cv::Mat(size, CV_8UC3);
    cv::warpPerspective(*body, transformed, transform, size);

    CopyBodyPart(&transformed, skin, cv::Point2i(20, 20));
    CopyBodyPart(&transformed, skin, cv::Point2i(32, 20));
    
    return 0;
}
int GetEnd(XnUserID user, cv::Mat *body, cv::Mat *skin, XnSkeletonJoint joint, cv::Point2i pos)
{
    XnPoint3D p = PointForJoint(user, joint);
    if (!PointIsValid(p)) return -1;
    
    int s = 2;
    
    cv::Point2f cameraPoints[] = {cv::Point2f(p.X-s, p.Y-s), cv::Point2f(p.X+s, p.Y-s), cv::Point2f(p.X-s, p.Y+s), cv::Point2f(p.X+s, p.Y+s)};
    cv::Point2f skinPoints[] = {cv::Point2f(0, 0), cv::Point2f(3, 0), cv::Point2f(0, 3), cv::Point2f(3, 3)};
    
    cv::Size size = cv::Size(4, 4);
    cv::Mat transform = cv::getPerspectiveTransform(cameraPoints, skinPoints);
    cv::Mat transformed = cv::Mat(size, CV_8UC3);
    cv::warpPerspective(*body, transformed, transform, size);
    
    CopyBodyPart(&transformed, skin, pos);
    
    return 0;
}
void DrawJointPoint(XnUserID user, cv::Mat *input, XnSkeletonJoint joint)
{
    XnPoint3D p = PointForJoint(user, joint);
    if (!PointIsValid(p)) return;
    cv::Point2i point = cv::Point2i(p.X, p.Y);

    for(int y = point.y-1; y < point.y+2; y++) {
        unsigned char *row = input->ptr<unsigned char>(y);
        row+= (point.x-1)*3;
        *row++ = 0;
        *row++ = 0;
        *row++ = 255;
        *row++ = 0;
        *row++ = 0;
        *row++ = 255;
        *row++ = 0;
        *row++ = 0;
        *row++ = 255;
    }
}
void CRhGetRegionPoints::OnMouseMove( CRhinoViewport& vp, UINT flags, const ON_3dPoint& point, const CPoint* pt )
{
    if( pt && m_points.Count() > 0 )
    {
        if( flags & MK_LBUTTON )
        {
            if( PointIsValid(*pt) )
            {
                m_prev_point = *pt;
                m_temp_point = *pt;
                m_points.Append( m_prev_point );
            }
        }
        else
        {
            m_temp_point = *pt;
        }
    }

    CRhinoGetPoint::OnMouseMove( vp, flags, point, pt );
}