#include "munga.h"
#pragma once

#include "spline.h"

#if defined(USE_SIGNATURE)
	int Is_Signature_Bad(const volatile CubicCurve *)
	{
		return False;
	}
#endif

Logical CubicCurve::TestInstance() const
{
	return True;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
CubicCurve::CubicCurve(
	const Point3D& p1,
	const Vector3D& r1,
	const Point3D& p4,
	const Vector3D& r4
)
{
	Check_Pointer(this);
	Check(&p1);
	Check(&r1);
	Check(&p4);
	Check(&r4);

	//
	//----------------------------------
	// Generate the hermite basis matrix
	//----------------------------------
	//
	basisMatrix(0,0) = 2.0f*(p1.x - p4.x) + r1.x + r4.x;
	basisMatrix(0,1) = 2.0f*(p1.y - p4.y) + r1.y + r4.y;
	basisMatrix(0,2) = 2.0f*(p1.z - p4.z) + r1.z + r4.z;

	basisMatrix(1,0) = 3.0f*(p4.x - p1.x) - 2.0f*r1.x - r4.x;
	basisMatrix(1,1) = 3.0f*(p4.y - p1.y) - 2.0f*r1.y - r4.y;
	basisMatrix(1,2) = 3.0f*(p4.z - p1.z) - 2.0f*r1.z - r4.z;

	basisMatrix(2,0) = r1.x;
	basisMatrix(2,1) = r1.y;
	basisMatrix(2,2) = r1.z;

	basisMatrix(3,0) = p1.x;
	basisMatrix(3,1) = p1.y;
	basisMatrix(3,2) = p1.z;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
CubicCurve::CubicCurve(
	const Point3D& p1,
	const Point3D& p2,
	const Point3D& p3,
	const Point3D& p4
)
{
	Check_Pointer(this);
	Check(&p1);
	Check(&p2);
	Check(&p3);
	Check(&p4);

	//
	//---------------------------------
	// Generate the bezier basis matrix
	//---------------------------------
	//
	basisMatrix(0,0) = p4.x - p1.x + 3.0f*(p2.x - p3.x);
	basisMatrix(0,1) = p4.y - p1.y + 3.0f*(p2.y - p3.y);
	basisMatrix(0,2) = p4.z - p1.z + 3.0f*(p2.z - p3.z);

	basisMatrix(1,0) = 3.0f*(p1.x + p3.x) - 6.0f*p2.x;
	basisMatrix(1,1) = 3.0f*(p1.y + p3.y) - 6.0f*p2.y;
	basisMatrix(1,2) = 3.0f*(p1.z + p3.z) - 6.0f*p2.z;

	basisMatrix(2,0) = 3.0f*(p2.x - p1.x);
	basisMatrix(2,1) = 3.0f*(p2.y - p1.y);
	basisMatrix(2,2) = 3.0f*(p2.z - p1.z);

	basisMatrix(3,0) = p1.x;
	basisMatrix(3,1) = p1.y;
	basisMatrix(3,2) = p1.z;
}

//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
void
	CubicCurve::Evaluate(
		Scalar t,
		Point3D *p,
		Vector3D *v
	)
{
	Check(this);
	Check_Pointer(p);
	Check_Pointer(v);
	Verify(t >= 0.0f && t <= 1.0f);

	Point3D t_pos;
	t_pos.z = t;
	t_pos.y = t_pos.z * t;
	t_pos.x = t_pos.y * t;
	p->Multiply(t_pos, basisMatrix);

	Vector3D t_vec(3.0f*t*t, 2.0f*t, 1.0f);
	v->Multiply(t_vec, basisMatrix);
}