OE_curve.cpp

/*
* This file is part of project OpenEmbroidery. It's copyrighted by
* the contributors recorded in the version control history of the file.
* Original project location https://code.electrolab.fr/openEmbroidery/openEmbroidery_software
*
* SPDX-License-Identifier: CECILL-2.1
* License-Filename: Licence_CeCILL_V2.1-en.txt
*/

#include "curves/OE_curve.h"
#include "OE_utils.h"
#include "Archive.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() : controls(false), closed(false)
{
}

OE_curve::~OE_curve()
{
}

void OE_curve::persist(Pakal::Archive* archive)
{
	//archive->set_type_name("OE_curve");
	archive->value("closed", closed);
	archive->value("pts", pts);
}

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(pt1);
	out.push_back(pt12);
	out.push_back(pt123);
	out.push_back(pt1234);
	out.push_back(pt234);
	out.push_back(pt34);
	out.push_back(pt4);

	return out;
}

BoundingBox OE_curve::segmentBounds(vector_2d* segment)
{
	float bounds[4];
	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] = std::min(v0->x, v3->x);
	bounds[1] = std::min(v0->y, v3->y);
	bounds[2] = std::max(v0->x, v3->x);
	bounds[3] = std::max(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 BoundingBox();
		return BoundingBox(bounds[0], bounds[1], bounds[2], bounds[3]);
	}

	// 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] = std::min(bounds[0+i], (float)v);
			bounds[2+i] = std::max(bounds[2+i], (float)v);
		}
	}
	return BoundingBox(bounds[0], bounds[1], bounds[2], bounds[3]);
}

BoundingBox OE_curve::getBound()
{
	vector_2d* curve;
	bounds.init = false;
	// Find bounds
	for (unsigned i=0; i<pts.size()-1; i+=3)
	{
		curve = &pts[i];
		bounds += segmentBounds(curve);
	}
	return bounds;
}

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 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 && closed)
	{
		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[3]);
	out.push_back(tangeant[4]);

	// 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[2]-tangeant[3])*(end-start)/(end-nearPt)+tangeant[3]);
	out.push_back(tangeant[3]);

	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, bool force)
{
	if (!dpi || !check() || !(needRefresh||force))
	{
		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)
{
	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);
	}
	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;
		for (unsigned 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;
}

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

void OE_curve::transform(const OE_Matrix& m)
{
	for (auto& pt : pts)
	{
		m.apply(pt.x, pt.y);
	}
}