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 "OE_curve.h"
#include "OE_utils.h"
#include <iostream>

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

static inline float minf(float a, float b) { return a < b ? a : b; }
static inline float maxf(float a, float b) { return a > b ? a : b; }

OE_curve::OE_curve()
{
    //next = 0;		// Pointer to next curve, or NULL if last element.
    npts = 0;					// Total number of bezier points.
    cpts = 0;					// Total alocated space for bezier points.
    closed = false;				// Flag indicating if shapes should be treated as closed.
    //pts = 0;
    lineColor[0] = 0;
    lineColor[1] = 160;
    lineColor[2] = 192;
    lineColor[3] = 255;
    controlPointColor[0] = 100;
    controlPointColor[1] = 60;
    controlPointColor[2] = 92;
    controlPointColor[3] = 255;
    controlLineColor[0] = 60;
    controlLineColor[1] = 100;
    controlLineColor[2] = 92;
    controlLineColor[3] = 100;
    controlEndPointColor[0] = 200;
    controlEndPointColor[1] = 0;
    controlEndPointColor[2] = 0;
    controlEndPointColor[3] = 255;

}

OE_curve::~OE_curve()
{
   // if (pts)
   //     free(pts);
}

bool OE_curve::getPoint(uint16_t nb, float* x, float* y)
{
    if ((npts)&&(nb<=npts))
    {
        std::cout << nb<<"--"<<npts<<"   ";
        *x = pts[(nb-1)*2+0];
        *y = pts[(nb-1)*2+1];
        return true;
	}
	return false;
}

bool OE_curve::setPoint(uint16_t nb, float x, float y)
{
    if (nb<=npts)
    {
        pts[(nb-1)*2+0] = x;
        pts[(nb-1)*2+1] = y;
        return true;
	}
	return false;
}

bool OE_curve::addPoint( float x, float y)
{
	pts.push_back(x);
	pts.push_back(y);
	npts++;
	cpts = npts+1;
	return true;
}

void OE_curve::moveTo(float x, float y)
{
    addPoint( x, y);
}

void OE_curve::lineTo(float x, float y)
{
	float px,py, dx,dy;
	if (npts > 0) {

        getPoint(npts, &px, &py);
		dx = x - px;
		dy = y - py;
		addPoint(px + dx/3.0f, py + dy/3.0f);
		addPoint(x - dx/3.0f, y - dy/3.0f);
		addPoint(x, y);
	}
}

void OE_curve::cubicBezTo(float cpx1, float cpy1, float cpx2, float cpy2, float x, float y)
{
	addPoint(cpx1, cpy1);
	addPoint(cpx2, cpy2);
	addPoint(x, y);
}

#define EPSILON (1e-12)

int OE_curve::ptInBounds(float* pt, float* bounds)
{
	return pt[0] >= bounds[0] && pt[0] <= bounds[2] && pt[1] >= bounds[1] && pt[1] <= 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;
}

double OE_curve::evalBezier(double t, double p0, double p1, double p2)
{
	double it = 1.0-t;
	//(1-t)²A + 2t(1-t)B + t²C
	return it*it*p0 + 2.0*it*t*p1 + t*t*p2;
}

void OE_curve::curveBounds(float* bounds, float* curve)
{
	int i, j, count;
	double roots[2], a, b, c, b2ac, t, v;
	float* v0 = &curve[0];
	float* v1 = &curve[2];
	float* v2 = &curve[4];
	float* v3 = &curve[6];

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

	// Bezier curve 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 curve inflection points in X and Y.
	for (i = 0; i < 2; i++) {
		a = -3.0 * v0[i] + 9.0 * v1[i] - 9.0 * v2[i] + 3.0 * v3[i];
		b = 6.0 * v0[i] - 12.0 * v1[i] + 6.0 * v2[i];
		c = 3.0 * v1[i] - 3.0 * v0[i];
		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++) {

			v = evalBezier(roots[j], v0[i], v1[i], v2[i], v3[i]);
			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];
float* curve;
// Find bounds
	for (int i = 0; i < npts-1; i += 3) {
		curve = &pts[i*2];

		curveBounds(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];
}

bool OE_curve::setNext(OE_curve* next)
{
    this->next = next;
    return true;
}

OE_curve* OE_curve::getNext()
{
    return next;
}

int OE_curve::getNpts(){return npts;}
char OE_curve::getClosed(){return closed;}
void OE_curve::setClosed(char closed){this->closed = closed;}

/** \brief draw the curve on screen
 *
 * \param dpi is the discretisation level of the spline curve to display
 * \return true if all is ok
 *
 */

bool OE_curve::draw(float dpi)
{

	int i;
	std::vector<float> disc = discretizeFast(dpi);
	/*for (i = 0; i < npts-1; i += 3) {
		float* p = &pts[i*2];
		disc = discretizeCubicBez(p[0],p[1], p[2],p[3], p[4],p[5], p[6],p[7], dpi, 0);
	}*/
	glLineWidth(1.5);
	glBegin(GL_LINE_STRIP);
	glColor4ubv(lineColor);
	glVertex2f(pts[0], pts[1]);
	int discsize = disc.size();
	for (i = 0; i < discsize-1; i += 2) {
            glVertex2f(disc[i], disc[i+1]);
	}/*
	for (i = 0; i < npts-1; i += 3) {
		float* p = &pts[i*2];
		cubicBez(p[0],p[1], p[2],p[3], p[4],p[5], p[6],p[7], dpi, 0);
	}*/
	if (closed) {
		glVertex2f(pts[0], pts[1]);
	}
	glEnd();

    //draw controls
    if (controls)
   {

		std::vector<float> discReg = discretizeRegular(1);

        glPointSize(3.0f);
        glBegin(GL_POINTS);
        glColor3f(0.8,0.1,0.1);
        discsize = discReg.size();
        for (i = 0; i < discsize-1; i += 2) {
            glVertex2f(discReg[i], discReg[i+1]);
        }
        glEnd();

        // Control lines
        glLineWidth(1.5);
        glColor4ubv(controlLineColor);
        glBegin(GL_LINES);
        for (i = 0; i < npts-1; i += 3) {
            float* p = &pts[i*2];
            glVertex2f(p[0],p[1]);
            glVertex2f(p[2],p[3]);
            glVertex2f(p[4],p[5]);
            glVertex2f(p[6],p[7]);
        }
        glEnd();
        // Points
        glPointSize(5.0f);
        glColor4ubv(controlEndPointColor);

        glBegin(GL_POINTS);
        glVertex2f(pts[0],pts[1]);

        glColor4ubv(controlPointColor);

        for (i = 0; i < npts-2; i += 3) {
            float* p = &pts[i*2];
            glVertex2f(p[6],p[7]);
        }

        glColor4ubv(controlEndPointColor);
        glVertex2f(pts[(npts-1)*2],pts[(npts-1)*2+1]);

        glEnd();

        // Points
        //glPointSize(5.0f);

        glPointSize(3.0f);
        glColor4ubv(controlPointColor);
        glBegin(GL_POINTS);
        //glColor4ubv(bgColor);
        //glColor4f(1.0,0.0,0.0,1.0);
       // glVertex2f(pts[0],pts[1]);

        for (i = 0; i < npts-1; i += 3) {
            float* p = &pts[i*2];
            glVertex2f(p[2],p[3]);
            glVertex2f(p[4],p[5]);
            //glColor4ubv(bgColor);
            //glVertex2f(p[6],p[7]);
        }
        //glPointSize(5.0f);
        //glColor4f(1.0,0.0,0.0,1.0);
        //glVertex2f(pts[(npts-1)*2+6],pts[(npts-1)*2+7]);
        glEnd();
    }
	return true;
}


float OE_curve::distPtSeg(float x, float y, float px, float py, float qx, float qy)
{
	float pqx, pqy, dx, dy, d, t;
	pqx = qx-px;
	pqy = qy-py;
	dx = x-px;
	dy = y-py;
	d = pqx*pqx + pqy*pqy;
	t = pqx*dx + pqy*dy;
	if (d > 0) t /= d;
	if (t < 0) t = 0;
	else if (t > 1) t = 1;
	dx = px + t*pqx - x;
	dy = py + t*pqy - y;
	return dx*dx + dy*dy;
}

std::vector<float> OE_curve::discretizeCubicBez(float x1, float y1, float x2, float y2,
					 float x3, float y3, float x4, float y4,
					 float tol, int level)
{
    std::vector<float> out;
    float x12,y12,x23,y23,x34,y34,x123,y123,x234,y234,x1234,y1234;
	float d;

	if (level > 12) return out;

	x12 = (x1+x2)*0.5f;
	y12 = (y1+y2)*0.5f;
	x23 = (x2+x3)*0.5f;
	y23 = (y2+y3)*0.5f;
	x34 = (x3+x4)*0.5f;
	y34 = (y3+y4)*0.5f;
	x123 = (x12+x23)*0.5f;
	y123 = (y12+y23)*0.5f;
	x234 = (x23+x34)*0.5f;
	y234 = (y23+y34)*0.5f;
	x1234 = (x123+x234)*0.5f;
	y1234 = (y123+y234)*0.5f;

	d = distPtSeg(x1234, y1234, x1,y1, x4,y4);
	if (d > tol*tol) {
        out = discretizeCubicBez(x1,y1, x12,y12, x123,y123, x1234,y1234, tol, level+1);

        std::vector<float> tmpdisc = discretizeCubicBez(x1234,y1234, x234,y234, x34,y34, x4,y4, tol, level+1);
        out.insert(out.end(), tmpdisc.begin(), tmpdisc.end());
	} else {
	    out.push_back(x4);
        out.push_back(y4);
	}
	return out;
}

std::vector<float> OE_curve::discretizeFast(float maxDist)
{
	std::vector<float> out;
	for (int i = 0; i < npts-3; i += 3) {
		float* p = &pts[i*2];
		std::vector<float> tmpdisk = discretizeCubicBez(p[0],p[1], p[2],p[3], p[4],p[5], p[6],p[7], maxDist, 0);
		out.insert(out.end(), tmpdisk.begin(), tmpdisk.end());
	}
	return out;
}

std::vector<float> OE_curve::discretizeRegular(float dist)
{
	float x, y, vectlen, tmplen = 0;
    std::vector<float> out;
	std::vector<float> disc, tmpdisc ;

	tmpdisc.push_back(pts[0]);
	tmpdisc.push_back(pts[1]);
	for (int i = 0; i < npts-3; i += 3) {
		float* p = &pts[i*2];
		std::vector<float> tmpdisk = discretizeCubicBez(p[0],p[1], p[2],p[3], p[4],p[5], p[6],p[7], dist/10.0, 0);

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

	if (closed)
	{
		tmpdisc.push_back(pts[0]);
		tmpdisc.push_back(pts[1]);
	}

	tmplen =0;

	int discsize;
	discsize = tmpdisc.size();
	for (int i = 0; i < discsize-3; i += 2) {
		x = tmpdisc[i+0]-tmpdisc[i+2];
		y = tmpdisc[i+1]-tmpdisc[i+3];

		vectlen=sqrt(x*x+y*y);
		tmplen = tmplen + vectlen;

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

			out.push_back(x*(tmplen/vectlen)+tmpdisc[i+2]);
			out.push_back(y*(tmplen/vectlen)+tmpdisc[i+3]);
		}
	}
    return out;

}
/*
void OE_curve::cubicBez(float x1, float y1, float x2, float y2,
					 float x3, float y3, float x4, float y4,
					 float tol, int level)
{
	float x12,y12,x23,y23,x34,y34,x123,y123,x234,y234,x1234,y1234;
	float d;

	if (level > 12) return;

	x12 = (x1+x2)*0.5f;
	y12 = (y1+y2)*0.5f;
	x23 = (x2+x3)*0.5f;
	y23 = (y2+y3)*0.5f;
	x34 = (x3+x4)*0.5f;
	y34 = (y3+y4)*0.5f;
	x123 = (x12+x23)*0.5f;
	y123 = (y12+y23)*0.5f;
	x234 = (x23+x34)*0.5f;
	y234 = (y23+y34)*0.5f;
	x1234 = (x123+x234)*0.5f;
	y1234 = (y123+y234)*0.5f;

	d = distPtSeg(x1234, y1234, x1,y1, x4,y4);
	if (d > tol*tol) {
		cubicBez(x1,y1, x12,y12, x123,y123, x1234,y1234, tol, level+1);
		cubicBez(x1234,y1234, x234,y234, x34,y34, x4,y4, tol, level+1);
	} else {
		glVertex2f(x4, y4);
	}
}
*/