#include "track.h"
#include "ASM.h"


void  SPLINE3_GetSplinePoint3(float F, Vector *OUTVc,Vector *P)
{
	float F2 = F * F;
	*OUTVc = P[0] * F * F2 + P[1] * F2 + P[2] * F + P[3];
}
void  SPLINE3_GetNurbPoint3(float F, Vector *OUTVc,Vector *P)
{
	SPLINE3_GetSplinePoint3(F, OUTVc,P);
	*OUTVc *= 1.0f/ OUTVc->z;
}
/*
D² = F²(A - 2B + C) + F(2B - 2A) + A 
D³ = F³(-A + 3B - 3C + D) + F²(3A - 6B + 3C) + F(3B - 3A) + A
*/
void  SPLINE3_GetSplineParams3(Vector *P1,Vector *P2,Vector *P3,Vector *P4,Vector *OutputP)
{
	//F³(-A + 3B - 3C + D)
	OutputP[0] = *P1 *-1.0f + *P2 * 3.0f - *P3 * 3.0f + *P4;
	//F²(3A - 6B + 3C)
	OutputP[1] = *P1 * 3.0f - *P2 * 6.0f + *P3 * 3.0f;
	//F(3B - 3A)
	OutputP[2] = *P2 * 3.0f - *P1 * 3.0f;
	//A
	OutputP[3] = *P1;
}
void  SPLINE3_GetNurbParams3(Vector *P1,Vector *P2,Vector *P3,Vector *P4,Vector *OutputP)
{
	Vector hP1,hP2,hP3,hP4;
	hP1 = *P1;hP2 = *P2;hP3 = *P3;hP4 = *P4;
	hP1.x *= hP1.z;
	hP1.y *= hP1.z;
	hP2.x *= hP2.z;
	hP2.y *= hP2.z;
	hP3.x *= hP3.z;
	hP3.y *= hP3.z;
	hP4.x *= hP4.z;
	hP4.y *= hP4.z;
	SPLINE3_GetSplineParams3(&hP1,&hP2,&hP3,&hP4,OutputP);
}
void SPLINE3_DrawSpline3(MAP *pSrc,Vector *P1,Vector *P2,Vector *P3,Vector *P4)
{
	Vector Params[4];
	Vector PointP,PointC;
/*	DrawLine(pSrc,P1->x,P1->y,P2->x,P2->y,0xffff00);
	DrawLine(pSrc,P3->x,P3->y,P2->x,P2->y,0xffff00);
	DrawLine(pSrc,P3->x,P3->y,P4->x,P4->y,0xffff00);//*/
	SPLINE3_GetSplineParams3(P1,P2,P3,P4,Params);
	SPLINE3_GetSplinePoint3(0.0f,&PointP,Params);
	for (int i = 1 ; i < 30 ; i++)
	{
		SPLINE3_GetSplinePoint3((float)i/30.0f,&PointC,Params);
		DrawLine(pSrc,PointC.x,PointC.y,PointP.x,PointP.y,0xffff);
		PointP = PointC;
	}
}
void SPLINE3_DrawNurb3(MAP *pSrc,Vector *P1,Vector *P2,Vector *P3,Vector *P4)
{
	Vector Params[4];
	Vector PointP,PointC;
/*	DrawLine(pSrc,P1->x,P1->y,P2->x,P2->y,0xffff00);
	DrawLine(pSrc,P3->x,P3->y,P2->x,P2->y,0xffff00);
	DrawLine(pSrc,P3->x,P3->y,P4->x,P4->y,0xffff00);//*/
	SPLINE3_GetNurbParams3(P1,P2,P3,P4,Params);
	SPLINE3_GetNurbPoint3(0.0f,&PointP,Params);
	for (int i = 1 ; i < 30 ; i++)
	{
		SPLINE3_GetNurbPoint3((float)i/30.0f,&PointC,Params);
		DrawLine(pSrc,PointC.x,PointC.y,PointP.x,PointP.y,0xffff);
		PointP = PointC;
	}
}