toys/gps/docs/source/math/frechet_dist.cpp

#include <cmath>
#include "geo/private/math/frechet_dist.h"

namespace ns
{
namespace geo
{

namespace
{

typedef struct _sPoint
{
    double x;
    double y;
} sPoint;

#define max(a, b) (((a) > (b)) ? (a) : (b))

double dis(sPoint* p1, sPoint* p2, int i, int j)
{
    return sqrt(pow(p1[i].x - p2[j].x, 2) + pow(p1[i].y - p2[j].y, 2));
}

double min3(double f1, double f2, double f3)
{
    double minf = (f1 < f2) ? f1 : f2;
    minf = (minf < f3) ? minf : f3;
    return minf;
}

double Cal(sPoint* p1, sPoint* p2, double** ca, int i, int j)
{
    if (ca[i][j] > -1.0)
    {
        return ca[i][j];
    }
    else if (i == 0 && j == 0)
    {
        ca[i][j] = dis(p1, p2, 0, 0);
    }
    else if (i > 0 && j == 0)
    {
        ca[i][j] = max(Cal(p1, p2, ca, i - 1, 0), dis(p1, p2, i, 0));
    }
    else if (i == 0 && j > 0)
    {
        ca[i][j] = max(Cal(p1, p2, ca, 0, j - 1), dis(p1, p2, 0, j));
    }
    else if (i > 0 && j > 0)
    {
        ca[i][j] = max(min3(Cal(p1, p2, ca, i - 1, j), Cal(p1, p2, ca, i - 1, j - 1),
                            Cal(p1, p2, ca, i, j - 1)),
                       dis(p1, p2, i, j));
    }
    else
    {
        ca[i][j] = 0xFFFFFFFF;
    }

    return ca[i][j];
}

double calcFrechet(sPoint* p1, int p, sPoint* p2, int q)
{
    // init ca[p][q]
    double** ca = new double*[p];
    for (int i = 0; i < p; i++)
    {
        ca[i] = new double[q];
    }

    for (int i = 0; i < p; i++)
    {
        for (int j = 0; j < q; j++)
        {
            ca[i][j] = -1.0;
        }
    }

    double r = Cal(p1, p2, ca, p - 1, q - 1);

    // for (int i = 0; i < p; i++)
    // {
    //     for (int j = 0; j < q; j++)
    //     {
    //         printf("%f ", ca[i][j]);
    //     }
    //     printf("\n");
    // }

    for (int i = 0; i < p; i++)
        delete[] ca[i];
    delete[] ca;

    return r;
}

double calcFrechetBottomUp(sPoint* p1, int p, sPoint* p2, int q)
{
    double** ca = new double*[p];
    for (int i = 0; i < p; i++)
    {
        ca[i] = new double[q];
    }

    for (int i = 0; i < p; i++)
    {
        for (int j = 0; j < q; j++)
        {
            ca[i][j] = -1.0;
        }
    }

    ca[0][0] = dis(p1, p2, 0, 0);
    for (int i = 1; i < p; i++)
        ca[i][0] = max(ca[i - 1][0], dis(p1, p2, i, 0));

    for (int i = 1; i < p; i++)
        ca[0][i] = max(ca[0][i - 1], dis(p1, p2, 0, i));

    for (int i = 1; i < p; i++)
    {
        for (int j = 1; j < q; j++)
        {
            ca[i][j] = max(min3(ca[i - 1][j], ca[i - 1][j - 1], ca[i][j - 1]), dis(p1, p2, i, j));
        }
    }
    double r = ca[p - 1][q - 1];

    for (int i = 0; i < p; i++)
        delete[] ca[i];
    delete[] ca;

    return r;
}

} // namespace

double Frechet::calcFrechetDistance(const std::vector<PointD>& line1,
                                    const std::vector<PointD>& line2)
{
    double dis = 0;
    sPoint* p1 = new sPoint[line1.size()];
    for (size_t i = 0; i < line1.size(); i++)
    {
        p1[i].x = line1[i].x;
        p1[i].y = line1[i].y;
    }

    sPoint* p2 = new sPoint[line2.size()];
    for (size_t i = 0; i < line2.size(); i++)
    {
        p2[i].x = line2[i].x;
        p2[i].y = line2[i].y;
    }

    dis = calcFrechet(p1, static_cast<int>(line1.size()), p2, static_cast<int>(line2.size()));

    delete[] p2;
    delete[] p1;

    // sPoint p1[5] = {{1.0,2.0},{2.0,3.0},{3.0,4.0},{4.0,5.0},{5.0,6.0}};
    // sPoint p2[7] = {{1.0,-2.0},{2.0,-3.0},{3.0,-4.0},{4.0,-5.0},{5.0,-6.0},{6.0,-7.0},{7.0,-8.0}};
    // sPoint p3[7] = {{7.0,-8.0},{6.0,-7.0},{5.0,-6.0},{4.0,-5.0},{3.0,-4.0},{2.0,-3.0},{1.0,-2.0}};
    // dis = calcFrechet(p1, 5, p2, 7);
    // dis = calcFrechet(p1, 5, p3, 7);

    return dis;
}

} // namespace geo
} // namespace ns