写 带mesh的ply格式文件

void write_mesh_ply(Point3f * pCameraCoordinates, Point2f * pColorCoordinatesOfDepth, int deepth_width, int deepth_height, int rgb_width, int rgb_height, int step = 1)
{
    if(NULL == pCameraCoordinates)
    {
        return;
    }

    mesh_face_typedef ply_face;
    vector<mesh_face_typedef> v_ply_face;

    Point3f p1;
    Point3f p2;
    Point3f p3;
    Point3f p4;
    Point3f* ptr = NULL;

    Point2f uv1;
    Point2f uv2;
    Point2f uv3;
    Point2f uv4;
    Point2f* ptr2 = NULL;

    float f_width = (float)deepth_width;
    float f_height = (float)deepth_height;

    float z_th = 0.5;
    int tri_count_flag = 0;

    /* 这里解释为什么取20 http://stackoverflow.com/questions/11902593/get-depth-at-color-position-kinect-sdk */
    for(int i = 20; i < deepth_height - step - 20; i += step)
    {
        ptr = pCameraCoordinates + i * deepth_width;
        ptr2 = pColorCoordinatesOfDepth + i * deepth_width;

        for(int j = 20; j < deepth_width - step - 20; j += step)
        {
            p1 = ptr[j];
            p2 = ptr[j + step];
            p3 = ptr[deepth_width * step + j];
            p4 = ptr[deepth_width * step + step + j];

            uv1 = ptr2[j];
            uv2 = ptr2[j + step];
            uv3 = ptr2[deepth_width * step + j];
            uv4 = ptr2[deepth_width * step + step + j];

            tri_count_flag = 0;

            /* 取1 2 3 点画三角形 */
            if((abs(p1.Z - p2.Z) < z_th) && (abs(p2.Z - p3.Z) < z_th) && (abs(p1.Z - p3.Z) < z_th))
            {
                ply_face.p1 = p1;
                ply_face.p2 = p2;
                ply_face.p3 = p3;
                ply_face.uv1.X = uv1.X / (float)rgb_width;
                ply_face.uv1.Y = uv1.Y / (float)rgb_height;
                ply_face.uv2.X = uv2.X / (float)rgb_width;
                ply_face.uv2.Y = uv2.Y / (float)rgb_height;
                ply_face.uv3.X = uv3.X / (float)rgb_width;
                ply_face.uv3.Y = uv3.Y / (float)rgb_height;
                v_ply_face.push_back(ply_face);
                tri_count_flag++;
            }

            /* 取2 3 4点画三角形 */
            if((abs(p2.Z - p3.Z) < z_th) && (abs(p2.Z - p4.Z) < z_th) && (abs(p3.Z - p4.Z) < z_th))
            {
                ply_face.p1 = p2;
                ply_face.p2 = p4;
                ply_face.p3 = p3;
                ply_face.uv1.X = uv2.X / (float)rgb_width;
                ply_face.uv1.Y = uv2.Y / (float)rgb_height;
                ply_face.uv2.X = uv4.X / (float)rgb_width;
                ply_face.uv2.Y = uv4.Y / (float)rgb_height;
                ply_face.uv3.X = uv3.X / (float)rgb_width;
                ply_face.uv3.Y = uv3.Y / (float)rgb_height;
                v_ply_face.push_back(ply_face);
                tri_count_flag++;
            }

            if(!tri_count_flag)
            {
                /* 取1 2 4 点画三角形 */
                if((abs(p1.Z - p2.Z) < z_th) && (abs(p2.Z - p4.Z) < z_th) && (abs(p1.Z - p4.Z) < z_th))
                {
                    ply_face.p1 = p1;
                    ply_face.p2 = p2;
                    ply_face.p3 = p4;
                    ply_face.uv1.X = uv1.X / (float)rgb_width;
                    ply_face.uv1.Y = uv1.Y / (float)rgb_height;
                    ply_face.uv2.X = uv2.X / (float)rgb_width;
                    ply_face.uv2.Y = uv2.Y / (float)rgb_height;
                    ply_face.uv3.X = uv4.X / (float)rgb_width;
                    ply_face.uv3.Y = uv4.Y / (float)rgb_height;
                    v_ply_face.push_back(ply_face);
                }

                /* 取1 3 4点画三角形 */
                if((abs(p1.Z - p3.Z) < z_th) && (abs(p1.Z - p4.Z) < z_th) && (abs(p3.Z - p4.Z) < z_th))
                {
                    ply_face.p1 = p1;
                    ply_face.p2 = p4;
                    ply_face.p3 = p3;
                    ply_face.uv1.X = uv1.X / (float)rgb_width;
                    ply_face.uv1.Y = uv1.Y / (float)rgb_height;
                    ply_face.uv2.X = uv4.X / (float)rgb_width;
                    ply_face.uv2.Y = uv4.Y / (float)rgb_height;
                    ply_face.uv3.X = uv3.X / (float)rgb_width;
                    ply_face.uv3.Y = uv3.Y / (float)rgb_height;
                    v_ply_face.push_back(ply_face);
                }
            }
        }
    }

    FILE *f_ply;
    fopen_s(&f_ply, "output_ply.ply", "w");

    //write ply header
    fprintf(f_ply, "ply\n");
    fprintf(f_ply, "format ascii 1.0\n");
    fprintf(f_ply, "comment TextureFile rgb.png\n");
    fprintf(f_ply, "element vertex %d\n", v_ply_face.size() * 3);
    fprintf(f_ply, "property float x\n");
    fprintf(f_ply, "property float y\n");
    fprintf(f_ply, "property float z\n");
    fprintf(f_ply, "element face %d\n", v_ply_face.size());
    fprintf(f_ply, "property list uchar int vertex_indices\n");
    fprintf(f_ply, "property list uchar float texcoord\n");
    fprintf(f_ply, "end_header\n");

    for(auto iter = v_ply_face.begin(); iter != v_ply_face.end(); iter++)
    {
        fprintf(f_ply, "%f %f %f\n", iter->p1.X, iter->p1.Y, iter->p1.Z);
        fprintf(f_ply, "%f %f %f\n", iter->p2.X, iter->p2.Y, iter->p2.Z);
        fprintf(f_ply, "%f %f %f\n", iter->p3.X, iter->p3.Y, iter->p3.Z);
    }

    int index = 0;

    for(auto iter = v_ply_face.begin(); iter != v_ply_face.end(); iter++)
    {
        fprintf(f_ply, "3 %d %d %d ", index, index + 1, index + 2);

        //print triangle uv
        fprintf(f_ply, "6 %f %f %f %f %f %f\n",
                iter->uv1.X, iter->uv1.Y,
                iter->uv2.X, iter->uv2.Y,
                iter->uv3.X, iter->uv3.Y);
        index += 3;
    }

    fclose(f_ply);
}