/* curve.c */
/* Programed by AZO */
/* This code is free. Use at Your Own Risk. */

/* build: gcc curve.c -lm -o curve.a */

#include "curve.h"

/* 0.0 <= i_dT <= i_uiControlCount - 1 */
void bspline2(pos2d_t* o_ptPos, const unsigned int i_uiControlCount, const pos2d_t* i_ltControls, const double i_dT) {
	int j, k;
	double t, d, r;

	o_ptPos->x = 0.0;
	o_ptPos->y = 0.0;
	for(j = -2 ; j <= (int)i_uiControlCount + 1 ; j++) {
		k = j;
		if(j < 0) {
			k = 0;
		} else if(j > i_uiControlCount - 1) {
			k = i_uiControlCount - 1;
		}
		
		t = i_dT - j;
		if(t < 0.0) {
			t = -t;
		}
		if(t < 0.5) {
			r = -1.0 * t * t + 0.75;
		} else if(t < 1.5) {
			d = t - 1.5;
			r = d * d * 0.5;
		} else {
			r = 0.0;
		}

		o_ptPos->x += r * i_ltControls[k].x;
		o_ptPos->y += r * i_ltControls[k].y;
	}
}

/* 0.0 <= i_dT <= i_uiControlCount - 1 */
void bspline3(pos2d_t* o_ptPos, const unsigned int i_uiControlCount, const pos2d_t* i_ltControls, const double i_dT) {
	int j, k;
	double t, d, r;

	o_ptPos->x = 0.0;
	o_ptPos->y = 0.0;
	for(j = -2 ; j <= (int)i_uiControlCount + 1 ; j++) {
		k = j;
		if(j < 0) {
			k = 0;
		} else if(j > i_uiControlCount - 1) {
			k = i_uiControlCount - 1;
		}
		
		t = i_dT - j;
		if(t < 0.0) {
			t = -t;
		}
		if(t < 1.0) {
			r = (3.0 * t * t * t - 6.0 * t * t + 4.0) / 6.0;
		} else if(t < 2.0) {
			d = t - 2.0;
			r = -1.0 * d * d * d / 6.0;
		} else {
			r = 0.0;
		}

		o_ptPos->x += r * i_ltControls[k].x;
		o_ptPos->y += r * i_ltControls[k].y;
	}
}

/* 0.0 <= i_dRatio <= 1.0 */
void quadratic_bezier(pos2d_t* o_ptPos, const pos2d_t* i_ptFrom, const pos2d_t* i_ptControl, const pos2d_t* i_ptTo, const double i_dRatio) {
	pos2d_t ltTemp[2]; 
	ltTemp[0].x = i_ptFrom->x + (i_ptControl->x - i_ptFrom->x) * i_dRatio;
	ltTemp[0].y = i_ptFrom->y + (i_ptControl->y - i_ptFrom->y) * i_dRatio;
	ltTemp[1].x = i_ptControl->x + (i_ptTo->x - i_ptControl->x) * i_dRatio;
	ltTemp[1].y = i_ptControl->y + (i_ptTo->y - i_ptControl->y) * i_dRatio;
	o_ptPos->x = ltTemp[0].x + (ltTemp[1].x - ltTemp[0].x) * i_dRatio;
	o_ptPos->y = ltTemp[0].y + (ltTemp[1].y - ltTemp[0].y) * i_dRatio;
}

/* 0.0 <= i_dRatio <= 1.0 */
void cubic_bezier(pos2d_t* o_ptPos, const pos2d_t* i_ptFrom, const pos2d_t* i_ptControl1, const pos2d_t* i_ptControl2, const pos2d_t* i_ptTo, const double i_dRatio) {
	pos2d_t ltTemp1[3];
	pos2d_t ltTemp2[2];
	ltTemp1[0].x = i_ptFrom->x + (i_ptControl1->x - i_ptFrom->x) * i_dRatio;
	ltTemp1[0].y = i_ptFrom->y + (i_ptControl1->y - i_ptFrom->y) * i_dRatio;
	ltTemp1[1].x = i_ptControl1->x + (i_ptControl2->x - i_ptControl1->x) * i_dRatio;
	ltTemp1[1].y = i_ptControl1->y + (i_ptControl2->y - i_ptControl1->y) * i_dRatio;
	ltTemp1[2].x = i_ptControl2->x + (i_ptTo->x - i_ptControl2->x) * i_dRatio;
	ltTemp1[2].y = i_ptControl2->y + (i_ptTo->y - i_ptControl2->y) * i_dRatio;
	ltTemp2[0].x = ltTemp1[0].x + (ltTemp1[1].x - ltTemp1[0].x) * i_dRatio;
	ltTemp2[0].y = ltTemp1[0].y + (ltTemp1[1].y - ltTemp1[0].y) * i_dRatio;
	ltTemp2[1].x = ltTemp1[1].x + (ltTemp1[2].x - ltTemp1[1].x) * i_dRatio;
	ltTemp2[1].y = ltTemp1[1].y + (ltTemp1[2].y - ltTemp1[1].y) * i_dRatio;
	o_ptPos->x = ltTemp2[0].x + (ltTemp2[1].x - ltTemp2[0].x) * i_dRatio;
	o_ptPos->y = ltTemp2[0].y + (ltTemp2[1].y - ltTemp2[0].y) * i_dRatio;
}

/* i_dRotate & i_dAngle : radian */
void ellipse(pos2d_t* o_ptPos, const ellipse2d_t* i_ptEllipse, const double i_dRotate, const double i_dAngle) {
	o_ptPos->x = i_ptEllipse->center.x + i_ptEllipse->radius1 * cos(i_dAngle) * cos(i_dRotate) - i_ptEllipse->radius2 * sin(i_dAngle) * sin(i_dRotate);
	o_ptPos->y = i_ptEllipse->center.y + i_ptEllipse->radius1 * cos(i_dAngle) * sin(i_dRotate) + i_ptEllipse->radius2 * sin(i_dAngle) * cos(i_dRotate);
}

/* i_dRotate & idAngle & i_dShearAngle : radian / -M_PI/2 < i_dShearAngle < M_PI/2 */
void sheared_ellipse(pos2d_t* o_ptPos, const ellipse2d_t* i_ptEllipse, const double i_dRotate, const double i_dShearAngle, const double i_dAngle) {
	o_ptPos->x = i_ptEllipse->center.x + i_ptEllipse->radius1 * cos(i_dAngle) * cos(i_dRotate) - i_ptEllipse->radius2 * sin(i_dAngle) * sin(i_dRotate) + (i_ptEllipse->radius1 * cos(i_dAngle) * sin(i_dRotate) + i_ptEllipse->radius2 * sin(i_dAngle) * cos(i_dRotate)) * tan(i_dShearAngle);
	o_ptPos->y = i_ptEllipse->center.y + i_ptEllipse->radius1 * cos(i_dAngle) * sin(i_dRotate) + i_ptEllipse->radius2 * sin(i_dAngle) * cos(i_dRotate);
}

/* i_dRotate & idAngle & i_dShearAngle : radian / -M_PI/2 < i_dShearAngle < M_PI/2 */
void sheared_ellipse_convert(ellipse2d_t* o_ptEllipse, double* o_pdRotate, const ellipse2d_t* i_ptEllipse, const double i_dRotate, const double i_dShearAngle) {
	o_ptEllipse->center.x = i_ptEllipse->center.x;
	o_ptEllipse->center.y = i_ptEllipse->center.y;
	o_ptEllipse->radius1 = sqrt((i_ptEllipse->radius1 * cos(i_dShearAngle) * cos(i_dRotate) - i_ptEllipse->radius2 * sin(i_dShearAngle) * sin(i_dRotate) + (i_ptEllipse->radius1 * cos(i_dShearAngle) * sin(i_dRotate) + i_ptEllipse->radius2 * sin(i_dShearAngle) * cos(i_dRotate)) * tan(i_dShearAngle)) * (i_ptEllipse->radius1 * cos(i_dShearAngle) * cos(i_dRotate) - i_ptEllipse->radius2 * sin(i_dShearAngle) * sin(i_dRotate) + (i_ptEllipse->radius1 * cos(i_dShearAngle) * sin(i_dRotate) + i_ptEllipse->radius2 * sin(i_dShearAngle) * cos(i_dRotate)) * tan(i_dShearAngle)) + (i_ptEllipse->radius1 * cos(i_dShearAngle) * sin(i_dRotate) + i_ptEllipse->radius2 * sin(i_dShearAngle) * cos(i_dRotate)) * (i_ptEllipse->radius1 * cos(i_dShearAngle) * sin(i_dRotate) + i_ptEllipse->radius2 * sin(i_dShearAngle) * cos(i_dRotate)));
	o_ptEllipse->radius2 = sqrt((i_ptEllipse->radius1 * cos(i_dShearAngle + M_PI / 2) * cos(i_dRotate) - i_ptEllipse->radius2 * sin(i_dShearAngle + M_PI / 2) * sin(i_dRotate) + (i_ptEllipse->radius1 * cos(i_dShearAngle + M_PI / 2) * sin(i_dRotate) + i_ptEllipse->radius2 * sin(i_dShearAngle + M_PI / 2) * cos(i_dRotate)) * tan(i_dShearAngle + M_PI / 2)) * (i_ptEllipse->radius1 * cos(i_dShearAngle + M_PI / 2) * cos(i_dRotate) - i_ptEllipse->radius2 * sin(i_dShearAngle + M_PI / 2) * sin(i_dRotate) + (i_ptEllipse->radius1 * cos(i_dShearAngle + M_PI / 2) * sin(i_dRotate) + i_ptEllipse->radius2 * sin(i_dShearAngle + M_PI / 2) * cos(i_dRotate)) * tan(i_dShearAngle + M_PI / 2)) + (i_ptEllipse->radius1 * cos(i_dShearAngle + M_PI / 2) * sin(i_dRotate) + i_ptEllipse->radius2 * sin(i_dShearAngle + M_PI / 2) * cos(i_dRotate)) * (i_ptEllipse->radius1 * cos(i_dShearAngle + M_PI / 2) * sin(i_dRotate) + i_ptEllipse->radius2 * sin(i_dShearAngle + M_PI / 2) * cos(i_dRotate)));
	*o_pdRotate = (M_PI / 2 - i_dShearAngle) / 2;
}

/* i_dAngle : radian */
void involute(pos2d_t* o_ptPos, const pos2d_t* i_ptCenter, const double i_dRadius, const double i_dAndle) {
	o_ptPos->x = i_ptCenter->x + i_dRadius * (cos(i_dAndle) + i_dAndle * sin(i_dAndle));
	o_ptPos->y = i_ptCenter->y + i_dRadius * (sin(i_dAndle) - i_dAndle * cos(i_dAndle));
}

/* i_dAngle : radian / i_dRadius = i_dLength -> cycroid */
void trochoid(pos2d_t* o_ptPos, const double i_dRadius, const double i_dLength, const double i_dAndle) {
	o_ptPos->x = i_dRadius * i_dAndle - i_dLength * sin(i_dAndle);
	o_ptPos->y = i_dRadius - i_dLength * cos(i_dAndle);
}

/* i_dAngle : radian / i_dDynRadius = i_dLength -> epicycroid */
void epitrochoid(pos2d_t* o_ptPos, const double i_dConstRadius, const double i_dDynRadius, const double i_dLength, const double i_dAndle) {
	o_ptPos->x = (i_dConstRadius + i_dDynRadius) * cos(i_dAndle) - i_dLength * cos((i_dConstRadius + i_dDynRadius) / i_dDynRadius * i_dAndle);
	o_ptPos->y = (i_dConstRadius + i_dDynRadius) * sin(i_dAndle) - i_dLength * sin((i_dConstRadius + i_dDynRadius) / i_dDynRadius * i_dAndle);
}

/* i_dAngle : radian / i_dDynRadius = i_dLength -> hypocycroid */
void hypotrochoid(pos2d_t* o_ptPos, const double i_dConstRadius, const double i_dDynRadius, const double i_dLength, const double i_dAndle) {
	o_ptPos->x = (i_dConstRadius - i_dDynRadius) * cos(i_dAndle) - i_dLength * cos((i_dConstRadius - i_dDynRadius) / i_dDynRadius * i_dAndle);
	o_ptPos->y = (i_dConstRadius - i_dDynRadius) * sin(i_dAndle) - i_dLength * sin((i_dConstRadius - i_dDynRadius) / i_dDynRadius * i_dAndle);
}

/* 0.0 <= i_dT <= i_uiControlCount - 1 */
void bspline2_3d(pos3d_t* o_ptPos, const unsigned int i_uiControlCount, const pos3d_t* i_ltControls, const double i_dT) {
	int j, k;
	double t, d, r;

	o_ptPos->x = 0.0;
	o_ptPos->y = 0.0;
	o_ptPos->z = 0.0;
	for(j = -2 ; j <= (int)i_uiControlCount + 1 ; j++) {
		k = j;
		if(j < 0) {
			k = 0;
		} else if(j > i_uiControlCount - 1) {
			k = i_uiControlCount - 1;
		}
		
		t = i_dT - j;
		if(t < 0.0) {
			t = -t;
		}
		if(t < 0.5) {
			r = -1.0 * t * t + 0.75;
		} else if(t < 1.5) {
			d = t - 1.5;
			r = d * d * 0.5;
		} else {
			r = 0.0;
		}

		o_ptPos->x += r * i_ltControls[k].x;
		o_ptPos->y += r * i_ltControls[k].y;
		o_ptPos->z += r * i_ltControls[k].z;
	}
}

/* 0.0 <= i_dT <= i_uiControlCount - 1 */
void bspline3_3d(pos3d_t* o_ptPos, const unsigned int i_uiControlCount, const pos3d_t* i_ltControls, const double i_dT) {
	int j, k;
	double t, d, r;

	o_ptPos->x = 0.0;
	o_ptPos->y = 0.0;
	o_ptPos->z = 0.0;
	for(j = -2 ; j <= (int)i_uiControlCount + 1 ; j++) {
		k = j;
		if(j < 0) {
			k = 0;
		} else if(j > i_uiControlCount - 1) {
			k = i_uiControlCount - 1;
		}
		
		t = i_dT - j;
		if(t < 0.0) {
			t = -t;
		}
		if(t < 1.0) {
			r = (3.0 * t * t * t - 6.0 * t * t + 4.0) / 6.0;
		} else if(t < 2.0) {
			d = t - 2.0;
			r = -1.0 * d * d * d / 6.0;
		} else {
			r = 0.0;
		}

		o_ptPos->x += r * i_ltControls[k].x;
		o_ptPos->y += r * i_ltControls[k].y;
		o_ptPos->z += r * i_ltControls[k].z;
	}
}

/* 0.0 <= i_dRatio <= 1.0 */
void quadratic_bezier_3d(pos3d_t* o_ptPos, const pos3d_t* i_ptFrom, const pos3d_t* i_ptControl, const pos3d_t* i_ptTo, const double i_dRatio) {
	pos3d_t ltTemp[2]; 
	ltTemp[0].x = i_ptFrom->x + (i_ptControl->x - i_ptFrom->x) * i_dRatio;
	ltTemp[0].y = i_ptFrom->y + (i_ptControl->y - i_ptFrom->y) * i_dRatio;
	ltTemp[0].z = i_ptFrom->z + (i_ptControl->z - i_ptFrom->z) * i_dRatio;
	ltTemp[1].x = i_ptControl->x + (i_ptTo->x - i_ptControl->x) * i_dRatio;
	ltTemp[1].y = i_ptControl->y + (i_ptTo->y - i_ptControl->y) * i_dRatio;
	ltTemp[1].z = i_ptControl->z + (i_ptTo->z - i_ptControl->z) * i_dRatio;
	o_ptPos->x = ltTemp[0].x + (ltTemp[1].x - ltTemp[0].x) * i_dRatio;
	o_ptPos->y = ltTemp[0].y + (ltTemp[1].y - ltTemp[0].y) * i_dRatio;
	o_ptPos->z = ltTemp[0].z + (ltTemp[1].z - ltTemp[0].z) * i_dRatio;
}

/* 0.0 <= i_dRatio <= 1.0 */
void cubic_bezier_3d(pos3d_t* o_ptPos, const pos3d_t* i_ptFrom, const pos3d_t* i_ptControl1, const pos3d_t* i_ptControl2, const pos3d_t* i_ptTo, const double i_dRatio) {
	pos3d_t ltTemp1[3];
	pos3d_t ltTemp2[2];
	ltTemp1[0].x = i_ptFrom->x + (i_ptControl1->x - i_ptFrom->x) * i_dRatio;
	ltTemp1[0].y = i_ptFrom->y + (i_ptControl1->y - i_ptFrom->y) * i_dRatio;
	ltTemp1[0].z = i_ptFrom->z + (i_ptControl1->z - i_ptFrom->z) * i_dRatio;
	ltTemp1[1].x = i_ptControl1->x + (i_ptControl2->x - i_ptControl1->x) * i_dRatio;
	ltTemp1[1].y = i_ptControl1->y + (i_ptControl2->y - i_ptControl1->y) * i_dRatio;
	ltTemp1[1].z = i_ptControl1->z + (i_ptControl2->z - i_ptControl1->z) * i_dRatio;
	ltTemp1[2].x = i_ptControl2->x + (i_ptTo->x - i_ptControl2->x) * i_dRatio;
	ltTemp1[2].y = i_ptControl2->y + (i_ptTo->y - i_ptControl2->y) * i_dRatio;
	ltTemp1[2].z = i_ptControl2->z + (i_ptTo->z - i_ptControl2->z) * i_dRatio;
	ltTemp2[0].x = ltTemp1[0].x + (ltTemp1[1].x - ltTemp1[0].x) * i_dRatio;
	ltTemp2[0].y = ltTemp1[0].y + (ltTemp1[1].y - ltTemp1[0].y) * i_dRatio;
	ltTemp2[0].z = ltTemp1[0].z + (ltTemp1[1].z - ltTemp1[0].z) * i_dRatio;
	ltTemp2[1].x = ltTemp1[1].x + (ltTemp1[2].x - ltTemp1[1].x) * i_dRatio;
	ltTemp2[1].y = ltTemp1[1].y + (ltTemp1[2].y - ltTemp1[1].y) * i_dRatio;
	ltTemp2[1].z = ltTemp1[1].z + (ltTemp1[2].z - ltTemp1[1].z) * i_dRatio;
	o_ptPos->x = ltTemp2[0].x + (ltTemp2[1].x - ltTemp2[0].x) * i_dRatio;
	o_ptPos->y = ltTemp2[0].y + (ltTemp2[1].y - ltTemp2[0].y) * i_dRatio;
	o_ptPos->z = ltTemp2[0].z + (ltTemp2[1].z - ltTemp2[0].z) * i_dRatio;
}

