OE_curve.cpp

/*
 * Copyright (c) 2015 Tricoire Sebastien 3dsman@free.fr
 *
 * This software is provided 'as-is', without any express or implied
 * warranty.  In no event will the authors be held liable for any damages
 * arising from the use of this software.
 *
 * Permission is granted to anyone to use this software for any purpose,
 * including commercial applications, and to alter it and redistribute it
 * freely, subject to the following restrictions:
 *
 * 1. The origin of this software must not be misrepresented; you must not
 * claim that you wrote the original software. If you use this software
 * in a product, an acknowledgment in the product documentation would be
 * appreciated but is not required.
 * 2. Altered source versions must be plainly marked as such, and must not be
 * misrepresented as being the original software.
 * 3. This notice may not be removed or altered from any source distribution.
 *
 */

#include "curves/OE_curve.h"
#include "OE_utils.h"
#include <iostream>

#include <GL/gl.h>
#include <cstdlib>
#include <math.h>
#include <cstdio>
#include <cstring>
#include <algorithm>

unsigned char OE_curve::lineColor[] = {0,160,192,255};

OE_curve::OE_curve()
{
}

OE_curve::~OE_curve()
{
}

std::ostream& operator <<(std::ostream& stream, const OE_curve& curve)
{
	stream << "closed=" << (curve.closed ? 1 : 0) << " ";
	stream << "len=" << curve.pts.size() << " ";
	stream << "pts=";
	for (auto& pt : curve.pts)
		//stream << "(" << pt.x << "," << pt.y << "),";
		stream << pt.x << "," << pt.y << ",";
	return stream;
}

std::istream& operator >>(std::istream& stream, OE_curve& curve)
{
	std::string sClosed, sLen;
	unsigned len = 0;
	char sPts[64] = {0};
	stream >> sClosed;
	if (!sClosed.substr(0, 7).compare("closed=") && sClosed.size()>=8)
		curve.closed = sClosed[7] == '1';
	stream >> sLen;
	if (!sLen.substr(0, 4).compare("len=") && sLen.size()>=5)
		len = atoi(sLen.c_str()+4); // TODO find better than atoi ?
	std::cout << "parsing a curve, closed=" << curve.closed << ", len=" << len << "\r\n";
	stream.getline(sPts, sizeof(sPts)-1, '=');
	if(!std::string(sPts).compare(" pts"))//.lstrip().compare("pts="))
	//if(std::string(sPts).lstrip().compare("pts=")) // lstrip?
	{
		double x,y;
		char c;
		while(len--)
		{
			stream >> x;
			stream >> c;
			if (c != ',')
				return stream;
			stream >> y;
			stream >> c;
			if (c != ',')
				return stream;
			curve.pts.push_back(vector_2d(x,y));
			std::cout << "adding point (" << x << ", " << y << ")\r\n";
		}
	}
	return stream;
}

float OE_curve::getLength(float maxDist)
{
	if (check())
	{
		double len = 0;
        vector_2d vect;
			
        std::vector<vector_2dt> tmpdisk = discretizeFast(maxDist);
		
		for (unsigned j = 1; j < tmpdisk.size(); j++) {
			
            vect = tmpdisk.at(j).v-tmpdisk.at(j-1).v;
            len += vect.len();
		}
		return len;
	}
	return -1;
}

#define EPSILON (1e-12)

bool OE_curve::ptInBounds( vector_2d pt, float* bounds)
{
	return pt.x >= bounds[0] && pt.x <= bounds[2] && pt.y >= bounds[1] && pt.y <= bounds[3];
}

double OE_curve::evalBezier(double t, double p0, double p1, double p2, double p3)
{
	double it = 1.0-t;
	return it*it*it*p0 + 3.0*it*it*t*p1 + 3.0*it*t*t*p2 + t*t*t*p3;
}

 std::vector<vector_2d> OE_curve::interPoint(vector_2d pt1, vector_2d pt2, vector_2d pt3, vector_2d pt4, float t)
 {
	std::vector<vector_2d> out;
	
	vector_2d pt12 = (pt2-pt1)*t+pt1;
	vector_2d pt23 = (pt3-pt2)*t+pt2;
	vector_2d pt34 = (pt4-pt3)*t+pt3;
	vector_2d pt123 = (pt23-pt12)*t+pt12;
	vector_2d pt234 = (pt34-pt23)*t+pt23;
	vector_2d pt1234 = (pt234-pt123)*t+pt123;
	
	out.push_back(pt123);
	out.push_back(pt1234);
	out.push_back(pt234);
	
	return out;
 }

void OE_curve::segmentBounds(float* bounds, vector_2d* segment)
{
	int i, j, count;
	double roots[2], a, b, c, b2ac, t, v;
	vector_2d* v0 = &segment[0];
	vector_2d* v1 = &segment[1];
	vector_2d* v2 = &segment[2];
	vector_2d* v3 = &segment[3];

	// Start the bounding box by end points
	bounds[0] = minf(v0->x, v3->x);
	bounds[1] = minf(v0->y, v3->y);
	bounds[2] = maxf(v0->x, v3->x);
	bounds[3] = maxf(v0->y, v3->y);

	// Bezier segment fits inside the convex hull of it's control points.
	// If control points are inside the bounds, we're done.
	if (ptInBounds(*v1, bounds) && ptInBounds(*v2, bounds))
		return;

	// Add bezier segment inflection points in X and Y.
	for (i = 0; i < 2; i++) {
		if (i){
			a = -3.0 * v0->y+ 9.0 * v1->y - 9.0 * v2->y + 3.0 * v3->y;
			b = 6.0 * v0->y - 12.0 * v1->y + 6.0 * v2->y;
			c = 3.0 * v1->y - 3.0 * v0->y;
		}
		else{
			a = -3.0 * v0->x + 9.0 * v1->x - 9.0 * v2->x + 3.0 * v3->x;
			b = 6.0 * v0->x - 12.0 * v1->x + 6.0 * v2->x;
			c = 3.0 * v1->x - 3.0 * v0->x;
		}
		count = 0;
		if (fabs(a) < EPSILON) {
			if (fabs(b) > EPSILON) {
				t = -c / b;
				if (t > EPSILON && t < 1.0-EPSILON)
					roots[count++] = t;
			}
		} else {
			b2ac = b*b - 4.0*c*a;
			if (b2ac > EPSILON) {
				t = (-b + sqrt(b2ac)) / (2.0 * a);
				if (t > EPSILON && t < 1.0-EPSILON)
					roots[count++] = t;
				t = (-b - sqrt(b2ac)) / (2.0 * a);
				if (t > EPSILON && t < 1.0-EPSILON)
					roots[count++] = t;
			}
		}
		for (j = 0; j < count; j++) {
			if (i){
				v = evalBezier(roots[j], v0->y, v1->y, v2->y, v3->y);
			}else{
				v = evalBezier(roots[j], v0->x, v1->x, v2->x, v3->x);
			}
				bounds[0+i] = minf(bounds[0+i], (float)v);
				bounds[2+i] = maxf(bounds[2+i], (float)v);

		}
	}
}

void OE_curve::getBound(float* xMin, float* yMin, float* xMax, float* yMax)
{
	float tmpbounds[4];
	vector_2d* curve;
	// Find bounds
	for (unsigned i = 0; i < pts.size()-1; i += 3) {
		curve = &pts[i];

		segmentBounds(tmpbounds, curve);
		if (i == 0) {
			bounds[0] = tmpbounds[0];
			bounds[1] = tmpbounds[1];
			bounds[2] = tmpbounds[2];
			bounds[3] = tmpbounds[3];
		} else {
			bounds[0] = minf(bounds[0], tmpbounds[0]);
			bounds[1] = minf(bounds[1], tmpbounds[1]);
			bounds[2] = maxf(bounds[2], tmpbounds[2]);
			bounds[3] = maxf(bounds[3], tmpbounds[3]);
		}
	}
	*xMin = bounds[0];
	*yMin = bounds[1];
	*xMax = bounds[2];
	*yMax = bounds[3];
}

std::vector<vector_2d> OE_curve::subCurve( float start, float end, bool rev)
{
	std::vector<vector_2d> out;
	
	
	// if the subcurve is not valid (start and end at the same pos or out of the curve) return
    if ((start<0)||(end<0)||(start>(pts.size()-1.0)/3.0)||(end>(pts.size()-1.0)/3.0))
		return out;
    if(start>=end)
    {
		if(start==(pts.size()-1)/3)
			start = 0;
		else if(end==0)
			end = (pts.size()-1)/3;
    }

    std::vector<vector_2d> tmpts = pts;
    if ((rev)||(start>=end)&&(!closed))
    {
        start = ((tmpts.size()-1.0)/3.0)-start;
        end = ((tmpts.size()-1.0)/3.0)-end;
        std::reverse(tmpts.begin(),tmpts.end());
    }

	std::vector<vector_2d> tangeant;
	bool loop =false;

	if (start>=end)
		loop = true;

	float scaleVect = 1;
	float nearPt;
	nearPt = floor(start);
	
    vector_2d* ps = &tmpts[nearPt*3];
	vector_2d* ps2;
	tangeant = interPoint(ps[0],ps[1],ps[2],ps[3], start-nearPt);
	out.push_back(tangeant[1]);
	out.push_back(tangeant[2]);
	
	// scale value for the next handle (the handle is scaled proportionaly to the position of the cut)
	scaleVect = 1-(start-nearPt);
	
	//middle of the curve
	while ((nearPt+1<end)||(loop))
	{
		nearPt++;
        ps = &tmpts[nearPt*3];
		ps2 = ps;
		
        if ((loop)&&(nearPt == tmpts.size()/3))
		{
            ps2 = &tmpts[0];
			nearPt = 0;
			loop =false;
		}
		out.push_back((ps[-1]-ps2[0])*scaleVect + ps2[0]);
		out.push_back(ps2[0]);
		out.push_back(ps2[1]);
		
		// reset the scale to 1 and set the start position (used to scale the last handdle if start and end are on the same segment)
		scaleVect = 1;
		start = nearPt;
	}
    ps = &tmpts[nearPt*3];
	tangeant = interPoint(ps[0],ps[1],ps[2],ps[3], end-nearPt);
	//scale the last handle proportionally to the end cut
	out.at(out.size()-1) = (out[out.size()-1]-out[out.size()-2])*(end-start)/scaleVect+out[out.size()-2];
	out.push_back((tangeant[0]-tangeant[1])*(end-start)/(end-nearPt)+tangeant[1]);
	out.push_back(tangeant[1]);
	
    //if (rev) std::reverse(out.begin(),out.end());
	
	return out;
}


int OE_curve::getNpts(){return pts.size();}
bool OE_curve::getClosed(){return closed;}
void OE_curve::setClosed(bool closed){this->closed = closed; setNeedRefresh();}

bool OE_curve::check(){return ((pts.size()>0)&&((pts.size())%3==1)) ;}

void OE_curve::refresh(float dpi)
{
    if (dpi ==0)return;
    discPts = discretizeFast(dpi);
}

float OE_curve::distPtSeg(vector_2d pt, vector_2d seg1, vector_2d seg2)
{
    return (posPtSeg(pt,seg1,seg2)-pt).len();
}

vector_2d OE_curve::posPtSeg(vector_2d pt, vector_2d seg1, vector_2d seg2)
{
	float  d2, t;
    vector_2d pq, d;
    pq = seg2-seg1;
	
    d = pt-seg1;
	
    d2 = pq.x*pq.x + pq.y*pq.y;
    t = pq.x*d.x + pq.y*d.y;
	
	if (d2 > 0) t /= d2;
	if (t < 0) t = 0;
	else if (t > 1) t = 1;
    d = seg1 + t*pq;

    return d;
}

vector_2dt OE_curve::closestT(vector_2dt pt1, vector_2dt pt2, vector_2dt pt3, vector_2dt pt4, float limit, vector_2d targetPt, int level)
{
    float d;

    vector_2dt pt12 = (pt1+pt2)*0.5f;
    vector_2dt pt23 = (pt2+pt3)*0.5f;
    vector_2dt pt34 = (pt3+pt4)*0.5f;
    vector_2dt pt123 = (pt12+pt23)*0.5f;
    vector_2dt pt234 = (pt23+pt34)*0.5f;
    vector_2dt pt1234 = (pt123+pt234)*0.5f;

    pt1234.t = (pt1.t+pt4.t)*0.5f;
    if (((pt1 - pt4).v.len()<limit)||(level > 10)) return pt1234;

    if (distPtSeg(targetPt, pt1234.v, pt1.v) < distPtSeg(targetPt, pt1234.v,pt4.v))
        return closestT(pt1, pt12, pt123, pt1234, limit, targetPt, level+1);
    else
        return closestT(pt1234, pt234, pt34, pt4, limit, targetPt, level+1);
}

vector_2dt OE_curve::closestT(vector_2d targetPt, double t1 , double t2)
{
    std::vector<vector_2d> pts = subCurve(t1, t2, false);
    return closestT( vector_2dt(pts[0],t1), vector_2dt(pts[1],t1), vector_2dt(pts[2],t2), vector_2dt(pts[3],t2), 0.01, targetPt, 0);
}

vector_2dt OE_curve::closestPoint(vector_2d pt)
{
    vector_2dt out;
    if (discPts.size())
    {
        float dist = distPtSeg(pt, discPts.at(0), discPts.at(1));
        float tmpDist;
        int id = 0;
        unsigned i;
        for ( i = 1; i < discPts.size()-1; i++) {
            tmpDist = distPtSeg(pt, discPts.at(i), discPts.at(i+1));
            if (tmpDist<dist)
            {
                dist = tmpDist;
                id = i;
            }
        }
        out = closestT(pt, discPts.at(id).t, discPts.at(id+1).t);
    }
    return out;
}

std::vector<vector_2dt> OE_curve::discretizeCubicBez(vector_2dt pt1, vector_2dt pt2, vector_2dt pt3, vector_2dt pt4, float tol, int level)
{
    std::vector<vector_2dt> out;
	float d;

	if (level > 12) return out;
	
    vector_2dt pt12 = (pt1+pt2)*0.5f;
    vector_2dt pt23 = (pt2+pt3)*0.5f;
    vector_2dt pt34 = (pt3+pt4)*0.5f;
    vector_2dt pt123 = (pt12+pt23)*0.5f;
    vector_2dt pt234 = (pt23+pt34)*0.5f;
    vector_2dt pt1234 = (pt123+pt234)*0.5f;

    pt1234.t = (pt1.t+pt4.t)*0.5f;

    d = distPtSeg(pt1234.v, pt1.v, pt4.v);
	if (d > tol*tol) {
        out = discretizeCubicBez(pt1, pt12, pt123, pt1234, tol, level+1);

        std::vector<vector_2dt> tmpdisc = discretizeCubicBez(pt1234, pt234, pt34, pt4, tol, level+1);
        out.insert(out.end(), tmpdisc.begin(), tmpdisc.end());
	} else {
        out.push_back(pt4);
	}
	return out;
}

std::vector<vector_2dt> OE_curve::discretizeFast(float maxDist)
{
    std::vector<vector_2dt> out;
			
	if (check())
	{
        out.push_back(vector_2dt(pts.at(0),0));
        float t = 0;
		for (unsigned i = 0; i < pts.size()-3; i += 3) {
            std::vector<vector_2dt> tmpdisk = discretizeCubicBez(vector_2dt(pts.at(i),t), vector_2dt(pts.at(i+1),t), vector_2dt(pts.at(i+2),t+1.0f), vector_2dt(pts.at(i+3),t+1.0f), maxDist, 0);
			out.insert(out.end(), tmpdisk.begin(), tmpdisk.end());
            t++;
		}
	}
	return out;
}

std::vector<vector_2dt> OE_curve::discretizeRegular(float dist)
{
	
    std::vector<vector_2dt> out;
	if (check())
	{
		float vectlen, tmplen = 0;
        vector_2dt vect;
        std::vector<vector_2dt> disc, tmpdisc ;
        tmpdisc = discretizeFast(dist/100.0);
/*
		tmpdisc.push_back(pts[0]);
		for (unsigned i = 0; i < pts.size()-3; i += 3) {
			std::vector<vector_2d> tmpdisk = discretizeCubicBez(pts[i], pts[i+1], pts[i+2], pts[i+3], dist/100.0, 0);

			tmpdisc.insert(tmpdisc.end(), tmpdisk.begin(), tmpdisk.end());
        }*/

		if (closed)
		{
            tmpdisc.push_back(vector_2dt(pts[0],pts.size()/3));
		}

		tmplen =0;
		
		out.push_back(tmpdisc[0]);
		for (unsigned i = 0; i < tmpdisc.size()-1; i += 1) {
            vect = tmpdisc[i]-tmpdisc[i+1];

            vectlen=vect.v.len();
			tmplen = tmplen + vectlen;

			while (tmplen >= dist)
			{
				tmplen -= dist;

				out.push_back(vect*(tmplen/vectlen)+tmpdisc.at(i+1));
			}
		}
		out.push_back(tmpdisc.at(tmpdisc.size()-1));
		
		return out;
	}
	return out;

}

void OE_curve::reverse()
{
	std::reverse(pts.begin(),pts.end());
}