基于三维离散点插值的高度图方法

template<typename T1, typename T2>
float distance(const T1& p1, const T2& p2)
{
    return sqrtf((p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y));
}

float interpolate(const Point3f& p1, const Point3f& p2, const Point3f& p3, const Point2f& pt)
{
    float d1 = 0.5f * (distance(p1, p2) + distance(p1, pt) + distance(p2, pt));
    float s1 = sqrtf(d1 * (d1 - distance(p1, p2)) * (d1 - distance(p1, pt)) * (d1 - distance(p2, pt)));
    float d2 = 0.5f * (distance(p1, p3) + distance(p1, pt) + distance(p3, pt));
    float s2 = sqrtf(d2 * (d2 - distance(p1, p3)) * (d2 - distance(p1, pt)) * (d2 - distance(p3, pt)));
    float d3 = 0.5f * (distance(p2, p3) + distance(p2, pt) + distance(p3, pt));
    float s3 = sqrtf(d3 * (d3 - distance(p2, p3)) * (d3 - distance(p2, pt)) * (d3 - distance(p3, pt)));
    float ssum = s1 + s2 + s3;
    float z = (s1 * p3.z + s2 * p2.z + s3 * p1.z) / ssum;
    return z;
}

void interpolate(const Point3f* points, int count, const Size& size, float* xs, float* ys, float* zs)
{
    vector<Point2f> xys;
    xys.reserve(count);
    for (int i = 0; i < count; i++)
    {
        xys.push_back(Point2f(points[i].x, points[i].y));
    }
    Rect rect = boundingRect(xys);
    Subdiv2D div(rect);
    for (int i = 0; i < count; i++)
    {
        div.insert(xys);
    }
    vector<Vec6f> tris;
    div.getTriangleList(tris);
    float hStep = rect.height / (size.height - 1.0f);
    float wStep = rect.width / (size.width - 1.0f);
    for (int i = 0; i < size.height; i++)
    {
        Point2f curr;
        curr.x = rect.x + i * hStep;
        xs[i] = curr.x;
        for (int j = 0; j < size.width; j++)
        {
            curr.y = rect.y + j * wStep;
            ys[j] = curr.y;
            auto found = std::find_if(tris.begin(), tris.end(),
                [&curr](Vec6f& pt3) { return pointPolygonTest(Mat(3, 1, CV_32FC2, pt3.val), curr, false) >= 0; });
            if (found != tris.end())
            {
                auto p1 = std::find_if(points, points + count,
                    [&found](const Point3f& p) { return p.x == (*found)[0] && p.y == (*found)[1]; });
                auto p2 = std::find_if(points, points + count,
                    [&found](const Point3f& p) { return p.x == (*found)[2] && p.y == (*found)[3]; });
                auto p3 = std::find_if(points, points + count,
                    [&found](const Point3f& p) { return p.x == (*found)[4] && p.y == (*found)[5]; });
                *zs = interpolate(*p1, *p2, *p3, curr);
            }
            else
            {
                *zs = 0;
            }
            zs++;
        }
    }
}

int main()
{
    int count;
    std::cin >> count;
    std::vector<Point3f> input;
    for (int i = 0; i < count; i++)
    {
        Point3f temp;
        std::cin >> temp.x >> temp.y >> temp.z;
        input.push_back(temp);
    }
    Size size;
    std::cin >> size.width >> size.height;

    std::vector<float> xs(size.height), ys(size.width);
    std::vector<float> zs(size.area());
    interpolate(input.data(), count, size, xs.data(), ys.data(), zs.data());

    for (auto& item : xs)
    {
        std::cout << item << " ";
    }
    std::cout << "\n";

    for (auto& item : ys)
    {
        std::cout << item << " ";
    }
    std::cout << "\n";

    for (auto& item : zs)
    {
        std::cout << item << " ";
    }
    std::cout << "\n";

    return 0;
}