CurveBrokenLine

//2015.11.23
#include <iostream>
#include <vector>
#include <stack>
#include "Defines.h"
#include "OneSeg.h"

//定义点
typedef struct _MyPoint
{
    bool flag;
    double x;
    double y;

    _MyPoint(double _x = 0.0, double _y = 0.0, bool _flag = false)
    {
        x = _x;
        y = _y;
        flag = _flag;
    }
}MyPoint;

double CalcAngle(MyPoint first, MyPoint cen, MyPoint second)
{
    double dx1, dx2, dy1, dy2; 
    double angle; 

    dx1 = first.x - cen.x; 
    dy1 = first.y - cen.y; 

    dx2 = second.x - cen.x; 

    dy2 = second.y - cen.y; 

    double c = (double)sqrt(dx1 * dx1 + dy1 * dy1) * (double)sqrt(dx2 * dx2 + dy2 * dy2); 

    if (c == 0) return -1; 

    angle = (double)acos((dx1 * dx2 + dy1 * dy2) / c); 

    return angle / PAI * 180; 
}

/* cp 在此是四个元素的数组:
cp[0] 为起点，或上图中的 P0
cp[1] 为第一控制点，或上图中的 P1
cp[2] 为第二控制点，或上图中的 P2
cp[3] 为结束点，或上图中的 P3
t 为参数值，0 <= t <= 1 */
MyPoint PointOnCubicBezier(MyPoint* cp, double t)
{
    double ax, bx, cx; double ay, by, cy;
    double tSquared, tCubed; MyPoint result;
    /* 计算多项式系数 */
    cx = 3.0 * (cp[1].x - cp[0].x);
    bx = 3.0 * (cp[2].x - cp[1].x) - cx;
    ax = cp[3].x - cp[0].x - cx - bx;
    cy = 3.0 * (cp[1].y - cp[0].y);
    by = 3.0 * (cp[2].y - cp[1].y) - cy;
    ay = cp[3].y - cp[0].y - cy - by;
    /* 计算t位置的点值 */
    tSquared = t * t;
    tCubed = tSquared * t;
    result.x = (ax * tCubed) + (bx * tSquared) + (cx * t) + cp[0].x;
    result.y = (ay * tCubed) + (by * tSquared) + (cy * t) + cp[0].y;
    return result;
}
/* ComputeBezier 以控制点 cp 所产生的曲线点，填入 MyPoint 结构数组。
调用方必须分配足够的空间以供输出，<sizeof(MyPoint) numberOfPoints> */
void ComputeBezier( MyPoint* cp, int numberOfPoints, MyPoint* curve )
{
    double dt; int i;
    dt = 1.0 / ( numberOfPoints - 1 );
    for( i = 0; i < numberOfPoints; i++)
        curve[i] = PointOnCubicBezier( cp, i*dt );
}

int FindSplit(std::vector<MyPoint>& V, int& i, int& j, int* f, double* dist)
{
    if (i + 1 > j)
    {
        return -1;
    }

    *f = i + 1;
    COneSeg os(V[i].x, V[i].y, V[j].x, V[j].y);
    *dist = os.GapFromPoint(cv::Point2d(V[*f].x, V[*f].y));
    for (int m = *f + 1; m < j; ++m)
    {
        double dTmpGap = os.GapFromPoint(cv::Point2d(V[m].x, V[m].y));
        if (dTmpGap > *dist)
        {
            *dist = dTmpGap;
            *f = m;
        }
    }

    return 0;
}

int DouglasPeucker(std::vector<MyPoint>& V, int &i, int &j, double e)
{
    if (V.size() <= 2)
    {
        //不需要处理
        return -1;
    }

    double dist = 9999;
    int f = 0;            //最大距离的点的序号
    std::stack<int> tempVertex;            //STL实现的栈
    tempVertex.push(j);

    do
    {
        //循环i和j之间距离直线ij最大的点
        FindSplit(V, i, j, &f, &dist);

        if (dist > e)      //大于阈值
        {
            V[f].flag = true;

            j = f;    //更新B值

            tempVertex.push(f);                    //记录最大距离点，放入栈中存储
            continue;
        }

        else
        {
            if (!tempVertex.empty() && j != tempVertex.top())    //判断后一点是否和当前栈顶相等
            {
                i = f;
                j = tempVertex.top();
                continue;
            }
            else
            {
                if (j != V.size())        //判断最后一个点是否和当前线段的B点重合
                {
                    i = j;
                    if (!tempVertex.empty())
                    {
                        std::deque<int> cont = tempVertex._Get_container();
                        if (cont.size() > 1) //栈中至少还有两个点
                        {
                            j = cont[cont.size() - 2];
                        }
                        else if (cont.size() == 1)    //栈中只有一个点
                        {
                            j = cont[cont.size() - 1];
                        }
                        tempVertex.pop();
                        continue;
                    }
                }
            }
        }
    } while (!tempVertex.empty());

    //保留首尾，去掉无用的点
    for (int i = 1; i < V.size() - 1;)
    {
        if (V[i].flag == false)
        {
            V.erase(V.begin() + i);
        }
        else
        {
            i++;
        }
    }

    return int(tempVertex.size());
}

int GetLineKeyPt(double* pX, double* pY, const int nNum, double** pOutX, double** pOutY, int *pOutNum, double dGap = 10.0)
{
    std::vector<MyPoint> V;
    for (int m = 0; m < nNum; ++m)
    {
        V.push_back(MyPoint(pX[m], pY[m]));
    }

    int i = 0;
    int j = int(V.size() - 1);

    DouglasPeucker(V, i, j, dGap);

    //输出
    *pOutNum = (int)V.size();
    *pOutX = new double[*pOutNum];
    *pOutY = new double[*pOutNum];
    for (int i = 0; i < *pOutNum; ++i)
    {
        (*pOutX)[i] = V[i].x;
        (*pOutY)[i] = V[i].y;
    }

    V.clear();

    return 0;
}

double Length(MyPoint p1, MyPoint p2)
{
    return sqrt((p1.y - p2.y) * (p1.y - p2.y) + (p1.x - p2.x) * (p1.x - p2.x));
}

double Slope(MyPoint p1, MyPoint p2)
{
    if (std::fabs(p1.x - p2.x) <= 1e-7)
    {
        return DBL_MAX;
    }

    return (p1.y - p2.y) / (p1.x - p2.x);
}

double GapFromPoint(MyPoint p1, MyPoint p2, const MyPoint& pt )
{
    double k = Slope(p1, p2);

    if (k == DBL_MAX)
    {
        //垂直的时候直接返回差值
        return fabs(p1.x - pt.x);
    }

    double b = p1.y - k * p1.x;

    return fabs(k * pt.x - pt.y + b) / sqrt( 1 + k * k);
}

bool IsPointInHalfLine(MyPoint p1, MyPoint p2, const MyPoint& pt, const double dDiff /*= 1.0*/ )
{
    return GapFromPoint(p1, p2, pt) <= dDiff;
}

MyPoint GetCenter(MyPoint p1, MyPoint p2)
{
    return MyPoint((p1.x + p2.x) / 2, (p1.y + p2.y) / 2);
}

bool GetPointInSeg(MyPoint p1, MyPoint p2, MyPoint& pRes, const double dGap /*= 10*/ )
{
    if (Length(p1, p2) < dGap)
    {
        return false;
    }

    if (p1.y == p2.y)
    {
        if (p1.x < p2.x)
        {
            pRes = MyPoint(p2.x - dGap, p1.y);
        }
        else
        {
            pRes = MyPoint(p2.x + dGap, p1.y);
        }

        return true;
    }

    if (p1.x == p2.x)
    {
        if (p1.y < p2.y)
        {
            pRes = MyPoint(p1.x, p2.y - dGap);
        }
        else
        {
            pRes = MyPoint(p1.x, p2.y + dGap);
        }
        return true;
    }

    double dResX = p2.x - (p2.x - p1.x) * dGap / Length(p1, p2);
    double dResY = p1.y - (p1.y - p2.y)*(Length(p1, p2) - dGap)/Length(p1, p2);

    pRes = MyPoint(dResX, dResY);

    return true;
}

double TwoPointsGap(const double x1, const double y1, const double x2, const double y2)
{
    return sqrt((y1 - y2) * (y1 - y2) + (x1 - x2) * (x1 - x2));
}

double TwoPointsGap(const MyPoint& p1, const MyPoint& p2)
{
    return TwoPointsGap(p1.x, p1.y, p2.x, p2.y);
}

int RemovePointByGap(double** pX, double** pY, int* nNum, const double& dGap = 1.0)
{
    int nOldNum = *nNum;

    if (nOldNum < 3)
    {
        return -1;
    }

    //获取输入
    std::vector<MyPoint> vPts(nOldNum);
    std::vector<MyPoint> vOut;
    for (int i = 0; i < nOldNum; ++i)
    {
        vPts[i] = MyPoint((*pX)[i], (*pY)[i]);
    }

    //取顺序的两个点
    MyPoint ptStart, ptCurrent;
    ptStart = vPts[0];
    ptCurrent = vPts[1];
    vOut.push_back(ptStart);
    for (int i = 1; i < nOldNum - 1; ++i)
    {
        ptCurrent = vPts[i];
        double dLength = Length(ptStart, ptCurrent);
        if (dLength > dGap)
        {
            vOut.push_back(ptCurrent);
            ptStart = ptCurrent;
        }
    }

    vOut.push_back(MyPoint((*pX)[nOldNum - 1], (*pY)[nOldNum - 1]));
    //释放原来的值
    delete[] (*pX);
    delete[] (*pY);
    *pX = NULL;
    *pY = NULL;
    *nNum = (int)vOut.size();
    *pX = new double[*nNum];
    *pY = new double[*nNum];
    for (int i = 0; i < *nNum; ++i)
    {
        (*pX)[i] = vOut[i].x;
        (*pY)[i] = vOut[i].y;
    }

    //注意这里不能delete，因为外部还会使用

    return 0;
}

int CurveBrokenLine(std::vector<cv::Point>& vPts)
{
    int nNum = vPts.size();
    MyPoint* pt = new MyPoint[nNum];
    for (int i = 0; i < nNum; ++i)
    {
        pt[i] = MyPoint(vPts[i].x, vPts[i].y);
    }

    for (int i = 0; i < nNum - 3; ++i)
    {
        ComputeBezier(pt, 4, pt);
    }

    for (int i = 0; i < nNum; ++i)
    {
        vPts[i] = cv::Point(pt[i].x, pt[i].y);
    }

    delete[] pt;
    pt = nullptr;

    return 0;
}

int SmoothBrokenLine(double* pX, double* pY, const int nNum)
{
    if (nNum < 4)
    {
        return -1;
    }

    const int nBezierNum = 4;
    int nTime = nNum / 4;
    MyPoint* pPts = new MyPoint[nBezierNum];
    MyPoint* pOut = new MyPoint[nBezierNum];
    for (int i = 0; i < nTime; ++i)
    {

        for (int j = 0; j < nBezierNum; ++j)
        {
            pPts[j] = MyPoint(pX[i * 4 + j], pY[i * 4 + j]);
        }
        ComputeBezier(pPts, 4, pOut);
        for (int j = 0; j < nBezierNum; ++j)
        {
            pX[i * 4 + j] = pOut[j].x;
            pY[i * 4 + j] = pOut[j].y;
        }
    }

    //最后4个结果再次拟合
    for (int j = 0; j < nBezierNum; ++j)
    {
        pPts[j] = MyPoint(pX[nNum - 4 + j], pY[nNum - 4 + j]);
    }

    ComputeBezier(pPts, 4, pOut);
    for (int j = 0; j < nBezierNum; ++j)
    {
        pX[nNum - 4 + j] = pOut[j].x;
        pY[nNum - 4 + j] = pOut[j].y;
    }

    delete[] pPts;
    delete[] pOut;
    pPts = NULL;
    pOut = NULL;

    return 0;
}