Old b-spline interpolation and approximation code

BSpline.cpp

/*===========================================================================
	Title: BSpline.cpp
	Module: Pi/MathLib
	Author: Ignacio Castaño
	Date: 29/05/2000
	License: Public Domain
===========================================================================*/

/*----------------------------------------------------------------------------
	Doc:
----------------------------------------------------------------------------*/

/** @file BSpline.cpp
 * @brief BSpline interpolation and aproximation. (NUNRBS)
 *
 * You can find some good notes about splines here:
 * http://www.cs.mtu.edu/~shene/COURSES/cs3621/NOTES/notes.html 
**/


/*----------------------------------------------------------------------------
	Includes:
----------------------------------------------------------------------------*/

// Core
#include "Pi/Core/Memory.h"

// MathLib
#include "Pi/MathLib/BSpline.h"
#include "Pi/MathLib/Sparse.h"



/*----------------------------------------------------------------------------
	Methods:
----------------------------------------------------------------------------*/

/** Create the basis function. */
void BSplineBasis::Create(uint cpn, uint d, const float k[], bool loop) {
	piCheck(cpn > 1);
	piCheck(d > 0);
	piCheck(cpn > d);

	// Init members.
	cpoint_num = cpn;
	degree = d;
	
	// Allocate knots.
	knot_array.Resize(cpoint_num + degree + 1);
	
	// Init knots.
	if( k != NULL ) {
		foreach(i, knot_array) {
			knot_array[i] = *k++;
		}
	}
	else {
		uint i = 0;
		if( loop ) {
			// Closed uniform knots.
			while(i < degree) {
				knot_array[i] = 0.0f;
				i++;
			}
			while(i < cpoint_num) {
				knot_array[i] = float(i - degree) / float(cpoint_num - degree);
				i++;
			}
			while(i < knot_array.Size()) {
				knot_array[i] = 1.0f;
				i++;
			}
		}
		else {
			// Open uniform knots.
			while(i < knot_array.Size()) {
				knot_array[i] = float(int(i) - int(degree)) / float(cpoint_num - degree);
				i++;
			}
			
			/*while(i < degree) {
				knot_array[i] = 1 + float(int(i) - int(degree)) / float(cpoint_num - degree);
				i++;
			}
			while(i <= cpoint_num) {
				knot_array[i] = float(int(i) - int(degree)) / float(cpoint_num - degree);
				i++;
			}
			while(i < knot_array.Size()) {
				knot_array[i] = float(int(i) - int(cpoint_num)) / float(cpoint_num - degree);
				i++;
			}*/
		}
	}
	
	
	piDebug("%d Knots.\n", knot_array.Size());
	foreach(i, knot_array) {
		piDebug(" - %f.\n", knot_array[i]);
	}
	
}


/** Locate the index for the given value. @todo Use bisection. */
uint BSplineBasis::GetKey(float t) {
	piCheck(knot_array.Size() == uint(cpoint_num + degree + 1));

	uint i;
	for(i = degree+1; i <= cpoint_num; i++) {
		if( t < knot_array[i] ) {
			//break;
			return i-1;
		}
	}
	return cpoint_num-1;
}


/** Evaluate the basis for the given t, returns the indices of the first and last non-zero basis. 
 * @todo This method can be optimized for fixed degree evaluation.
 */
void BSplineBasis::EvaluateBasis(float t, int * first_out, float N[] ) {
//	piDebugCheck(t >= 0.0f);
//	piDebugCheck(t <= 1.0f);
	piDebugCheck(first_out != NULL);
	piDebugCheck(N != NULL);
	//piDebug("> %f\n", t);
	
	/*if( t <= 0.0f ) {
		first = 0;

		N[0] = 1.0f;
		for(uint d = 1; d <= degree; d++) {
			N[d] = 0.0f;
		}
	}
	else if( t >= 1.0f ) {
		first = cpoint_num - 1 - degree;

		for(uint d = 0; d < degree; d++) {
			N[d] = 0.0f;
		}
		N[degree] = 1.0f;
	}
	else*/ {
			   
		int first = GetKey(t);

		piDebugCheck(knot_array[first] <= t);
		piDebugCheck(t <= knot_array[first+1]);

		float n0, n1;

		// First step.
		N[degree] = 1.0f;

		for(int d = 1; d <= degree; d++) {
			{
				uint i = first;
				
				n1 = (knot_array[i+1] - t) / (knot_array[i+1] - knot_array[i-d+1]);
				
				N[degree-d] = n1 * N[degree-d+1];
			}
			for(int k = 1; k < d; k++) {
				uint i = first + k;
				
				n0 = (t - knot_array[i-d]) / (knot_array[i] - knot_array[i-d]);
				n1 = (knot_array[i+1] - t) / (knot_array[i+1] - knot_array[i-d+1]);
				
				N[degree-d+k] = n0 * N[degree-d+k] + n1 * N[degree-d+k+1];
			}
			{
				uint i = first + d;
				
				n0 = (t - knot_array[i-d]) / (knot_array[i] - knot_array[i-d]);
				
				N[degree] = n0 * N[degree];
			}
		}
		
		*first_out = first - degree;
	}
}


/** Create the curve. */
void BSplineCurve::Create(uint cpn, uint d, const Vec3 * cp, const float * k) {
	
	// Init basis.
	basis.Create(cpn, d, k);
	
	// Allocate control point array.
	cpoint_array.Resize(cpn);

	if( cp != NULL ) {
		// Init control point array.
		foreach(i, cpoint_array) {
			cpoint_array[i] = *cp++;
		}
	}
}


/** Evaluate the curve at the given t. */
void BSplineCurve::Evaluate(float t, Vec3 * out) {

	const uint d = basis.GetDegree();	
	int first;
	//float N[d+1]; // gcc extension
	STACK_ARRAY(float, N, d+1);

	basis.EvaluateBasis(t, &first, N);

	*out = Vec3::Origin;

	for(uint i = 0; i <= d; i++) {
		out->Mad(*out, cpoint_array[first+i], N[i]);
	}
}


/** Adjust the control points to aproximate the given set of points. */
void BSplineCurve::Aproximate(const PiArray<float> & time_array, const PiArray<Vec3> & point_array) {
	
	const uint cpoint_num = GetControlPointNum();
	const uint sample_num = time_array.Size();	
	piCheck(sample_num == point_array.Size());
	piCheck(sample_num >= GetControlPointNum());

	DenseVector bx(sample_num), by(sample_num), bz(sample_num);
	
	// Init b vector.
	for(uint i = 0; i < sample_num; i++) {
		bx[i] = point_array[i].x;
		by[i] = point_array[i].y;
		bz[i] = point_array[i].z;
	}

	const uint d = basis.GetDegree();
	
#if 0	// Using a least squares solver

	// Init N matrix.
	SparseMatrix N(cpoint_num-2, sample_num);

	int first;
	//float B[d+1]; // gcc extension
	STACK_ARRAY(float, B, d+1);

	for(uint s = 0; s < sample_num; s++) {

		basis.EvaluateBasis(time_array[s], &first, B);

		for(uint i = 0; i <= d; i++) {
			if(first+i > 0 && first+i < cpoint_num-1)  {
				if( B[i] != 0.0f ) {
					N.SetElem(first+i-1, s, B[i]);
				}
			}
		}
	}


	DenseVector px(cpoint_num-2), py(cpoint_num-2), pz(cpoint_num-2);

	LSQRSolve( N, bx, px );
	LSQRSolve( N, by, py );
	LSQRSolve( N, bz, pz );
		
	//	CGNRSolve( N, bx, px );
	//	CGNRSolve( N, by, py );
	//	CGNRSolve( N, bz, pz );
	
	ControlPoint(0) = point_array[0];
	for(uint i = 1; i < cpoint_num-1; i++) {
		ControlPoint(i) = Vec3(px[i-1], py[i-1], pz[i-1]);
		ControlPoint(i).Print();
	}
	ControlPoint(cpoint_num-1) = point_array[sample_num-1];
	

#else	// Using the normal form.

	// Init N matrix.
	SparseMatrix N(cpoint_num, sample_num);

	int first;
	STACK_ARRAY(float, B, d+1);

	for(uint s = 0; s < sample_num; s++) {

		basis.EvaluateBasis(time_array[s], &first, B);

		for(uint i = 0; i <= d; i++) {
			if( B[i] != 0.0f ) {
				N.SetElem(first+i, s, B[i]);
			}
		}
	}


	DenseVector px(cpoint_num), py(cpoint_num), pz(cpoint_num);

	// Set initial solution.
	/*for(uint i = 0; i < cpoint_num; i++) {
		px[i] = point_array[ (i * sample_num) / cpoint_num ].x;
		py[i] = point_array[ (i * sample_num) / cpoint_num ].y;
		pz[i] = point_array[ (i * sample_num) / cpoint_num ].z;
	}*/

//	piDebug("solved in: %d\n", CGNRSolve( N, bx, px, 1e-6 ) );
//	piDebug("solved in: %d\n", CGNRSolve( N, by, py, 1e-6 ) );
//	piDebug("solved in: %d\n", CGNRSolve( N, bz, pz, 1e-6 ) );
	
	CholeskySolve( N, bx, px );
	CholeskySolve( N, by, py );
	CholeskySolve( N, bz, pz );

//	LSQRSolve( N, bx, px, 1e-6f );
//	LSQRSolve( N, by, py, 1e-6f );
//	LSQRSolve( N, bz, pz, 1e-6f );


	for(uint i = 0; i < cpoint_num; i++) {
		ControlPoint(i) = Vec3(px[i], py[i], pz[i]);
	//	ControlPoint(i).Print();
	}

#endif

}



/** Interpolate the given points. */
void BSplineCurve::Interpolate(const PiArray<float> & time_array, const PiArray<Vec3> & point_array) {

	// Handle open/close.
	// Handle periodic curves.
	uint cpoint_num = GetControlPointNum();
	piCheck(cpoint_num == time_array.Size());	
	piCheck(cpoint_num == point_array.Size());
	piCheck(cpoint_num == GetControlPointNum());


	DenseVector bx(cpoint_num), by(cpoint_num), bz(cpoint_num);

	// Init b vector.
	for(uint i = 0; i < cpoint_num; i++) {
		bx[i] = point_array[i].x;
		by[i] = point_array[i].y;
		bz[i] = point_array[i].z;
	}


	// Init N matrix.
	SparseMatrix N(cpoint_num, cpoint_num);

	const uint d = basis.GetDegree();
	int first;
	//float B[d+1]; // gcc extension
	STACK_ARRAY(float, B, d+1);

	for(uint s = 0; s < cpoint_num; s++) {

		basis.EvaluateBasis(time_array[s], &first, B);

		for(uint i = 0; i <= d; i++) {
			if( B[i] != 0.0f ) {
				N.SetElem(first+i, s, B[i]);
			}
		}
	}

	// Solve N*p=b
	DenseVector px(cpoint_num), py(cpoint_num), pz(cpoint_num);

	BiCGSTABSolve( N, bx, px );
	BiCGSTABSolve( N, by, py );
	BiCGSTABSolve( N, bz, pz );

	// Setup control points.
	for(uint i = 0; i < cpoint_num; i++) {
		ControlPoint(i) = Vec3(px[i], py[i], pz[i]);
		ControlPoint(i).Print();
	}
}

BSpline.h

/*===========================================================================
	Title: BSpline.h
	Module: Pi/MathLib
	Author: Ignacio Castaño
	Date: 29/05/2000
	License: Public Domain
===========================================================================*/

#ifndef _PI_MATHLIB_BSPLINE_H_
#define _PI_MATHLIB_BSPLINE_H_

/*----------------------------------------------------------------------------
	Doc:
----------------------------------------------------------------------------*/

/** @file BSpline.h
 * @brief BSpline interpolation and aproximation. (NUNRBS)
 *
 * You can find some good notes about splines here:
 * http://www.cs.mtu.edu/~shene/COURSES/cs3621/NOTES/notes.html
**/


/*----------------------------------------------------------------------------
	Headers:
----------------------------------------------------------------------------*/

// Core
#include "Pi/Core/Containers.h"

// MathLib
#include "Pi/MathLib/MathLib.h"
#include "Pi/MathLib/Vector.h"


/*----------------------------------------------------------------------------
	Functions:
----------------------------------------------------------------------------*/

enum KnotPlacement {
	UNIFORM,
	CHORDLENGTH,
	CENTRIPETAL,
	UNIVERSAL,
	OPTIMAL
};

// Place knots.
//void SetKnots(const PiArray<Vec3> & point_array, KnotPlacement method, bool open, PiArray<float> & knot_array);



/*----------------------------------------------------------------------------
	Classes:
----------------------------------------------------------------------------*/

/** BSpline basis function. */
class BSplineBasis {
public:

	/** Ctor. */
	BSplineBasis() {
	}

	/** Ctor. */
	BSplineBasis(uint cpn, uint d, const float k[]) {
		Create(cpn, d, k);
	}


	// Create the basis function.
	MATHLIB_API void Create(uint cpn, uint d, const float k[] = NULL, bool loop = false);

	// Access knots.
	const float & Knot(uint i) const;
	
	// Access knots.
	float & Knot(uint i);

	// Locate the index for the given value.
	uint GetKey(float t);

	// Evaluate the basis for the given t, returns the indices of the first and last non-zero basis.
	MATHLIB_API void EvaluateBasis(float t, int * first, float N[]);

	uint GetDegree() const;

private:

	/// Degree of the curve. d
	uint16 degree;
	
	/// Number of control points. n = cpn-1
	uint16 cpoint_num;
	
	/// Array of control points. size = n+d+2
	PiArray<float> knot_array;

};



/** BSpline curve. */
class BSplineCurve {
public:
	
	/** Ctor. */
	BSplineCurve() {
	}

	/** Ctor. */
	BSplineCurve(uint cpn, uint d, const Vec3 cp[], const float * k = NULL) {
		Create(cpn, d, cp, k);
	}

	// Create the curve.
	MATHLIB_API void Create(uint cpn, uint d, const Vec3 cp[], const float * k = NULL);

	// Get number of control points.
	uint GetControlPointNum() const;
	
	// Access control point.
	const Vec3 & ControlPoint(uint i) const;
		
	// Access control point.
	Vec3 & ControlPoint(uint i);
	
	// Evaluate the curve.
	MATHLIB_API void Evaluate(float t, Vec3 * out);

	// Adjust the control points to aproximate the given set of points.
	MATHLIB_API void Aproximate(const PiArray<float> & time_array, const PiArray<Vec3> & point_array);

	// Create an interpolating bspline.
	MATHLIB_API void Interpolate(const PiArray<float> & time_array, const PiArray<Vec3> & point_array);
	
private:

	BSplineBasis basis;

	PiArray<Vec3> cpoint_array;
	
};


/*----------------------------------------------------------------------------
	Inlines:
----------------------------------------------------------------------------*/

/** Get the degree of the basis. */
inline uint BSplineBasis::GetDegree() const {
	return degree;
}

/** Access knots. */
inline const float & BSplineBasis::Knot(uint i) const {
	piDebugCheck(i < knot_array.Size());
	return knot_array[i];
}

/** Access knots. */
inline float & BSplineBasis::Knot(uint i) {
	piDebugCheck(i < knot_array.Size());
	return knot_array[i];
}


/** Get number of control points. */
inline uint BSplineCurve::GetControlPointNum() const {
	return cpoint_array.Size();
}

/** Access control points. */
inline const Vec3 & BSplineCurve::ControlPoint(uint i) const {
	piDebugCheck(i < cpoint_array.Size());
	return cpoint_array[i];
}

/** Access control points. */
inline Vec3 & BSplineCurve::ControlPoint(uint i) {
	piDebugCheck(i < cpoint_array.Size());
	return cpoint_array[i];
}





#endif // _PI_MATHLIB_BSPLINE_H_

test_bspline.cpp

/*============================================================================
	Title: test_bspline.cpp
	Module: Pi/TestSuite
	Author: Ignacio Castaño
	Date: 08/03/2003
============================================================================*/

/*----------------------------------------------------------------------------
	Doc
----------------------------------------------------------------------------*/

/** @file test_bspline.cpp
 * @brief Test B Splines.
**/


/*----------------------------------------------------------------------------
	Includes
----------------------------------------------------------------------------*/

// Core
#include "Pi/Core/Timer.h"
#include "Pi/Core/Repository.h"

// MathLib
#include "Pi/MathLib/BSpline.h"

// Render
#include "Pi/Render/ResourceMng.h"

// TestSuite
#include "TestApp.h"

// Extern
#include <SDL/SDL.h>


/*----------------------------------------------------------------------------
	Classes
----------------------------------------------------------------------------*/


/** Test application. */
class BSplineTest : public PiTestApp {
	PI_DECLARE_CLASS(BSplineTest, PiTestApp);
public:

	/** */
	virtual bool CustomInit() {
		if( !InitVideo("BSpline test") ) return false;
		board = new PiBlackBoard( render );
		
		PiEngine::GetResourceMng()->LoadAll();
		
		
		Vec3 cpoints[] = {
			Vec3(100, 100), Vec3(100, 400), Vec3(400, 400), Vec3(400, 100), Vec3(100, 100), Vec3(100, 400), Vec3(400, 400), Vec3(400, 100)
		};
		
		curve.Create( 24, 3, NULL );

		state = STATE_DRAWING;
		
		return true;
	}

	/** */
	virtual void CustomShut() {
		delete board; board = NULL;
		ShutVideo();
	}


	/** Main loop. */
	virtual uint Run() {

		while( true ) {
			if( !video->Update() ) {
				break;
			}

			if( input->GetAction(ACT_EXIT) & ACTION_DOWN ) {
				break;
			}

			if( input->GetKey( SDLK_F12 ) & ACTION_DOWN ) {
				video->Screenshot();
			}

			if( input->GetKey( SDLK_LALT ) && (input->GetKey( SDLK_RETURN ) & ACTION_DOWN) ) {
				ToggleFullscreen();
				continue;
			}

			int mx, my;
			input->GetMousePos( &mx, &my );

			if( state == STATE_DRAWING ) {
				if( input->GetKey( SDLK_SPACE ) & ACTION_DOWN ) {

					uint point_num = board->GetPointNum() / 2;
					//uint point_num = 32;
					//uint point_num = 1024;
					
					PiArray<float> time_array; time_array.Resize(point_num);
					PiArray<Vec3> point_array; point_array.Resize(point_num);

					for(uint i = 0; i < point_num; i++) {
						point_array[i] = board->GetPoint(2*i);
						
					//	float t = float(i) / point_num;
					//	point_array[i].x = 500 * t + 70;
					//	point_array[i].y = 240 + 100 * sin(4*PI*t);
					//	point_array[i].z = 0.0f;
						
						time_array[i] = float(i) / point_num;
					}

					curve.Aproximate(time_array, point_array);

					state = STATE_SHOWING;
				}
				
				if( input->GetAction(ACT_LEFT_CLICK) & ACTION_DOWN ) {
					board->StartLine( mx, video->GetHeight() - my );
				}
				if( input->GetAction(ACT_LEFT_CLICK) ) {
					board->AddPoint( mx, video->GetHeight() - my );
				}
			}
			else {
				if( input->GetKey( SDLK_SPACE ) & ACTION_DOWN ) {
					board->Clean();
					state = STATE_DRAWING;
				}
			}

			render->BeginFrame();
			
			viewport.ResetWindow( video->GetWidth(), video->GetHeight() );
			render->SetViewport( &viewport );
			render->SetViewMode( PI_MODE_OVERLAY );

			board->Render();

			if( state == STATE_SHOWING ) {
				static PiDefShader s_control_point_shader(Color_MapRGBAf(1, 0, 0, .75), PI_CULL_NONE, false);
				static PiDefShader s_bspline_curve_shader(Color_MapRGBAf(0, 1, 0, .75), PI_CULL_NONE, false);

				render->SetShader(&s_control_point_shader);
				render->SetPolygonMode( PI_PM_LINE, 1 );
				for(uint i = 1; i < curve.GetControlPointNum(); i++) {
					render->DrawLine( curve.ControlPoint(i-1), curve.ControlPoint(i), 0 );
				}
				render->SetPolygonMode( PI_PM_FILL, 1 );
	
			//	Vec3 out;
			//	curve.Evaluate(1.0f, &out);

				render->SetShader(&s_bspline_curve_shader);
				for(uint i = 0; i < 512; i++) {
					
					Vec3 a, b;
					curve.Evaluate(float(i)/512.0f, &a);
					curve.Evaluate(float(i+1)/512.0f, &b);
					
					render->DrawLine( a, b, 0 );
				}
			}

			overlay->Begin();			
			AutoString text( "%.2f", 1.0f / PiTimer::GetFrameTime() );
			overlay->DrawString( font, 10, video->GetHeight()-16, text );
			
			text.Format("%d points", board->GetPointNum());
			overlay->DrawString( font, 10, video->GetHeight()-32, text );
			
			overlay->End();
			overlay->Render();

			render->EndFrame();
		}

		return Succeed;
	}

private:

	PiBlackBoard * board;
	
	BSplineCurve curve;
	
	enum State {
		STATE_DRAWING,
		STATE_SHOWING
	};
	
	State state;

};


/*----------------------------------------------------------------------------
	Implementation:
----------------------------------------------------------------------------*/

PI_IMPLEMENT_CLASS(BSplineTest) = { 0 };