/* libsegl - Sebas Extended GL Library
 *           Collection of Opengl/3D-Math helpers
 *
 *      Copyright (c) 2008 by Sebastian Lohff, seba@seba-geek.de
 *      http://www.seba-geek.de
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Library General Public
 * License as published by the Free Software Foundation; either
 * version 2 of the License, or (at your option) any later version.
 * 
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Library General Public License for more details.
 * 
 * You should have received a copy of the GNU Library General Public
 * License along with this library; if not, write to the
 * Free Software Foundation, Inc., 51 Franklin St, Fifth Floor,
 * Boston, MA  02110-1301, USA.
 */

#include "catmullromspline.h"

namespace segl {

CatmullRomSpline::CatmullRomSpline(float _s) {
	cr = new Matrix(4, 4);
	setS(_s);
}

void CatmullRomSpline::makeMatrix() {
	cr->set(-s	, 0, 0);
	cr->set( 2-s	, 0, 1);
	cr->set( s-2	, 0, 2);
	cr->set( s	, 0, 3);
	
	cr->set( 2*s	 , 1, 0);
	cr->set( s-3  , 1, 1);
	cr->set( 3-2*s, 1, 2);
	cr->set(-s    , 1, 3);

	cr->set(-s, 2, 0);
	cr->set( 0, 2, 1);
	cr->set( s, 2, 2);
	cr->set( 0, 2, 3);

	cr->set( 0, 3, 0);
	cr->set( 1, 3, 1);
	cr->set( 0, 3, 2);
	cr->set( 0, 3, 3);
}

Matrix CatmullRomSpline::getPosition(float u, Matrix &controlpoints) {
// 	Matrix controlpoints(4, a.getN());
// 	for(int i=0; i<a.getN(); i++) {
// 		controlpoints.set(a.get(0, i), 0, i);
// 		controlpoints.set(b.get(0, i), 1, i);
// 		controlpoints.set(c.get(0, i), 2, i);
// 		controlpoints.set(d.get(0, i), 3, i);
// 	}
	Matrix param(1, 4);
	param.set(u*u*u, 0, 0);
	param.set(u*u,   0, 1);
	param.set(u,     0, 2);
	param.set(1,     0, 3);
	
	return param * (*cr) * controlpoints;
}

Punkt3D CatmullRomSpline::getPosition(float u, Punkt3D a, Punkt3D b, Punkt3D c, Punkt3D d) {
	Matrix erg(1, 3);
	Matrix controlpoints(4, 3);
	
	controlpoints.set(a.x, 0, 0);
	controlpoints.set(a.y, 0, 1);
	controlpoints.set(a.z, 0, 2);
	
	controlpoints.set(b.x, 1, 0);
	controlpoints.set(b.y, 1, 1);
	controlpoints.set(b.z, 1, 2);
	
	controlpoints.set(c.x, 2, 0);
	controlpoints.set(c.y, 2, 1);
	controlpoints.set(c.z, 2, 2);
	
	controlpoints.set(d.x, 3, 0);
	controlpoints.set(d.y, 3, 1);
	controlpoints.set(d.z, 3, 2);
	
	erg = getPosition(u, controlpoints);
	
	return Punkt3D(erg.get(0,0), erg.get(0,1), erg.get(0,2));
}

Punkt2D CatmullRomSpline::getPosition(float u, Punkt2D a, Punkt2D b, Punkt2D c, Punkt2D d) {
	Matrix erg(1, 2);
	Matrix controlpoints(4, 2);
	
	controlpoints.set(a.x, 0, 0);
	controlpoints.set(a.y, 0, 1);
	
	controlpoints.set(b.x, 1, 0);
	controlpoints.set(b.y, 1, 1);
	
	controlpoints.set(c.x, 2, 0);
	controlpoints.set(c.y, 2, 1);
	
	controlpoints.set(d.x, 3, 0);
	controlpoints.set(d.y, 3, 1);
	
	erg = getPosition(u, controlpoints);
	
	return Punkt2D(erg.get(0,0), erg.get(0,1));
}


void CatmullRomSpline::setS(float _s) {
	s = _s;
	makeMatrix();
}

CatmullRomSpline::~CatmullRomSpline() {
	delete(cr);
}

} // namespace segl