n dimensional interpolator.cpp

#ifndef UTP_TABULAR_FUNCTION_MD
#define UTP_TABULAR_FUNCTION_MD

#include "UtpTabularFunction1D.h"
#include "UtpFunction.h"
#include "UtpArray.h"
#include "UtpVector.h"
#include "UtpBinaryFile.h"
#include "UtpInterpolate.h"

#include <string>
using namespace std;

// first index outermost, last index is innermost dimension
// ( i+1 element stored next to ith element)
// Generic Vector Index Counting Functions
void MDVectorIncrement(UtpVector<long> &Index,const UtpVector<long> &Dimension);
void MDFixedVectorIncrement(UtpVector<long> &Index,long Dim);
// map from multidimensional index vector to 1D vector offset
long MDGenericIndex(const UtpVector<long> &IndexArray,long AllDim);
long MDVectorIndex(const UtpVector<long> &Index,const UtpVector<long> &Dimension);
template<class T> long MDVectorIndex(const UtpVector<long> &Index,const UtpArray< UtpVector<T> > &XSet);

template<class T> class UtpTabularFunctionMD;
template<class T>
ostream& operator<<(ostream& ros, const UtpTabularFunctionMD<T> &rTable);

// Functions Extrapolate Beyond Table bounds, the Evaluation will ignore this and continue, the 
// function virtual void TableValue( const UtpVector<T>& X, T &Value ) const
// generates exceptions
template<class T>
class UtpTabularFunctionMD { // public UtpDifferentiableFunctionMD
public:
	//  Types

	// Constructors
	UtpTabularFunctionMD( const UtpVector<long> &TableDimensions = 0, bool Interpolate = true,INTERPOLATION_TYPE Type = LINEAR );
	UtpTabularFunctionMD( const UtpTabularFunctionMD<T>& Right );

	//  Destructor
	virtual ~UtpTabularFunctionMD();

	//  Member Functions
	void ResetTableDimensions( const UtpVector<long> &TableDimensions );
	void X( const UtpArray< UtpVector<T> > & XVectorSet );
	void Y( const UtpVector<long> &IndexArray, T Value );
	T Y (const UtpVector <long> &IndexArray);
	void InterpolationType( INTERPOLATION_TYPE Type ) { m_Type = Type; }
    void LoadDataInterp(const UtpVector<long> &StartInd,long NEED,UtpVector<double> &DataInterp) const;
	void TableInterpolate(long InterpDim,
		const UtpVector<long> &StartInd, 
		long NEED, 
		long TYPE, 
		const UtpVector<double> &Xxxx,
		UtpVector<double> &DataInterp,
		string &EdgeString,
		bool &OutOfTable) const;

	UtpTabularFunctionMD<T>& operator=( const UtpTabularFunctionMD<T>& Right );
	
	T TableValue( const UtpVector<T>& X ) const;
	//virtual T Evaluation( const UtpVector<T>& X );
	void TableValue( const UtpVector<T>& X, T &Value ) const;

	const UtpArray< UtpVector<T> > &XSet() const;

	// Binary File I/O
	void WriteBinary(const UtpString &Outname) const;
	void ReadBinary(const UtpString &DBname);

	friend ostream& operator<< <T>(ostream& ros, const UtpTabularFunctionMD<T> &rTable);

protected:
	bool m_InterpolateFlag;
	UtpVector<T> m_Y;
	INTERPOLATION_TYPE m_Type;
	UtpArray< UtpVector<T> > m_XSet;
};

template<class T>
UtpTabularFunctionMD<T>::UtpTabularFunctionMD( const UtpVector<long> &TableDimensions, bool Interpolate, INTERPOLATION_TYPE Type  )
: m_InterpolateFlag(Interpolate), m_Type( Type )/*, UtpDifferentiableFunctionMD( TableDimensions.Dimension() )*/
{ 
	ResetTableDimensions( TableDimensions );
}
	
template<class T>
UtpTabularFunctionMD<T>::UtpTabularFunctionMD( const UtpTabularFunctionMD<T>& Right )
{ *this = Right; }

template<class T>
UtpTabularFunctionMD<T>::~UtpTabularFunctionMD(){ }

template<class T>
void UtpTabularFunctionMD<T>::ResetTableDimensions( const UtpVector<long> &TableDimensions )
{
	//  Table
	m_XSet.Reset( TableDimensions.Dimension());
	long MaxDim=1;
	for( long i = 0; i < TableDimensions.Dimension(); i++ ){
		MaxDim *= TableDimensions[i];
		m_XSet[i].ResetDimension(TableDimensions[i]);
	}
	m_Y.ResetDimension( MaxDim);

}
	
template<class T>
void UtpTabularFunctionMD<T>::X( const UtpArray< UtpVector<T> > & XVectorSet )
{
	if (XVectorSet.Size() != m_XSet.Size()) {
		cout << "Xvectorset != m_XSetSize : " << XVectorSet.Size() << " " << m_XSet.Size() << endl;
		exit(-1);
	}
	for (long i=0;i < XVectorSet.Size();i++) {
		Utp_Allege( XVectorSet[i].Dimension() == m_XSet[i].Dimension(), "UtpTabularFunctionMD::XVectorSet (inner dimensions) Not Correctly Dimensioned" );
		//  Assign aMD Check
		for(long j = 0; j < XVectorSet[i].Dimension(); j++ ){
			m_XSet[i][j] = XVectorSet[i][j];
			if( j >= 1 ){
				if (m_XSet[i][j] <= m_XSet[i][j-1]) {
					cout << "UtpTabularFunctionMD:X will not function if X not increasing " << m_XSet[i][j-1] << " " << m_XSet[i][j] << endl;
					cout << "Index i j " << i << " " << j << endl;
					if (i > 0) cout <<"Prev X vect " << m_XSet[0] << endl;
					exit(-1);
				}
			}
		}
	}
}
	
template<class T>
T UtpTabularFunctionMD<T>::Y (const UtpVector <long> &IndexArray)
{
	return (m_Y[MDVectorIndex(IndexArray,m_XSet)]);
}

template<class T>
void UtpTabularFunctionMD<T>::Y( const UtpVector<long> &IndexArray, T Value  )
{
	m_Y[MDVectorIndex(IndexArray,m_XSet)] = Value;
}

template<class T>
UtpTabularFunctionMD<T>& UtpTabularFunctionMD<T>::operator=( const UtpTabularFunctionMD<T>& Right )
{
	if(this != &Right){
		/*UtpDifferentiableFunctionMD::operator=( Right );	*/
		m_XSet = Right.m_XSet;
		m_Y = Right.m_Y;
		m_Type = Right.m_Type;
	}
	return *this;
}

template<class T>
const UtpArray< UtpVector<T> > &UtpTabularFunctionMD<T>::XSet() const
{
	return m_XSet;
}

/*
template<class T>
T UtpTabularFunctionMD<T>::Evaluation( const UtpVector<T>& X )
{
	T Value = TableValue(X);
	IncrementEvaluations();
	return Value;
}
*/

template<class T>
T UtpTabularFunctionMD<T>::TableValue( const UtpVector<T>& X) const
{
	T Output;
	try {
		TableValue(X,Output);
	}
	catch(const string &) { // this function ignores string thrown exceptions
		return Output;
	}
	return Output;
}

template<class T>
void UtpTabularFunctionMD<T>::TableValue( const UtpVector<T>& X, T &Value) const
{
	//  Error Checks
	Utp_Allege( X.Dimension() == m_XSet.Size(), "UtpTabularFunctionND::Evaluation - X Not of Correct Dimension" );

	//  Number Of Points Needed For Interpolation
	long NEED = 0;
	long TYPE = LINEAR;
	if( m_Type == LINEAR ){
		NEED = 2; TYPE = LINEAR;
	}
	if( m_Type == QUADRATIC ){
		NEED = 3; TYPE = QUADRATIC;
	}
	string EdgeString;
	const long EBSIZE = 200;
//	char ExtrapBuf[EBSIZE];
	bool OutOfTable=false;
	UtpInterpolate<T> Table;
	UtpVector<long> StartInd(X.Dimension());
	if (m_InterpolateFlag) {
		long i;
		for (i=0;i < X.Dimension();i++) {
			if (m_XSet[i].Dimension() < NEED) {
				cout << "UtpTabularFunctionND::Evaluation i xset dim need ";
				cout << i << " " << m_XSet[i] << " " << m_XSet[i].Dimension() << " " << NEED << endl;
			}
			Utp_Allege( 
				(m_XSet[i].Dimension() >= NEED) && ( (m_XSet[i][0] != 0.0) || (m_XSet[i][1] != 0.0) ),
				"UtpTabularFunctionND::Evaluation - XSet ith vector Not Properly Set & cannot be used for interpolation"
				);
			StartInd[i] = Table.GetIndex(m_XSet[i],X[i]);
			if (StartInd[i] && m_Type==LINEAR) {
				if (X[i] < m_XSet[i][StartInd[i]]) StartInd[i]--;
			}
			StartInd[i] = Utp_Min( StartInd[i],m_XSet[i].Dimension() - NEED );
		}
		
		UtpVector<T> DataInterp;
		LoadDataInterp(StartInd,NEED,DataInterp);
		//	cout << "Loaded Data for Interpolation: " << DataInterp << endl;
		for (i=0;i < X.Dimension();i++) {
			TableInterpolate(i,StartInd,NEED,TYPE,X,DataInterp,EdgeString,OutOfTable);		
		}
		Value = DataInterp[0];
	}
	else { // pick closest point if not interpolating
		for (long i=0;i < X.Dimension();i++) {
			StartInd[i] = Table.GetIndex(m_XSet[i],X[i]);
			if (StartInd[i] >= m_XSet[i].Dimension() || StartInd[i] < 0) {
//				sprintf_s(ExtrapBuf,EBSIZE,"void UtpFixedTabularFunctionMD::TableValue using edge point: point desired val %lf lower %lf upper %lf range\n",
//					X[i],m_XSet[i][0],m_XSet[i][m_XSet[i].Dimension()-1]);
//				EdgeString = EdgeString + string(ExtrapBuf); 
				OutOfTable = true;
			}
			StartInd[i] = Utp_Min(StartInd[i],m_XSet[i].Dimension()-NEED);
			StartInd[i] = Utp_Max(StartInd[i],(long)0);
		}
		Value = m_Y[MDVectorIndex(StartInd,m_XSet)];
	}
	if (OutOfTable) throw EdgeString;
}

template<class T>
void UtpTabularFunctionMD<T>::LoadDataInterp(const UtpVector<long> &StartInd,long NEED,UtpVector<double> &DataInterp) const
{
	UtpVector<long> Ctr(StartInd.Dimension());
	long npts = (long)(pow((T)NEED,(T)StartInd.Dimension()));
	DataInterp.ResetDimension(npts);
	for (long i=0;i < npts;i++) {
		DataInterp[i] = m_Y[MDVectorIndex(Ctr+StartInd,m_XSet)];
		MDFixedVectorIncrement(Ctr,NEED);
	}
}

template<class T>
void UtpTabularFunctionMD<T>::TableInterpolate(long InterpIndex,
											const UtpVector<long> &StartInd, 
											long NEED, 
											long TYPE, 
											const UtpVector<double> &X,
											UtpVector<double> &DataInterp,
											string &EdgeString,
											bool &OutOfTable) const
{		
	const long EBSIZE = 200;
//	char ExtrapBuf[EBSIZE];
	long NewInterpDim = X.Dimension()-1-InterpIndex; // InterpDim 0...N-1
	UtpVector<T> NewInterp((long)pow((T)NEED,(T)NewInterpDim));
	UtpVector<T> XVector(NEED);
	UtpVector<T> YVector(NEED);
	UtpVector<T> TempInterp(NEED);
	UtpInterpolate<T> Table;
	UtpVector<long> OrigCtr(NewInterpDim+1);
	UtpVector<long> Ctr(NewInterpDim+1); 
	long i,j;
	for(j = 0; j < XVector.Dimension(); j++ ){
		XVector[j] = m_XSet[NewInterpDim][StartInd[NewInterpDim]+j];
	}

	for (i=0;i < NewInterp.Dimension();i++) {
		for( j = 0; j < XVector.Dimension(); j++ ){
			YVector[j] = DataInterp[MDGenericIndex(OrigCtr,NEED)];
			MDFixedVectorIncrement(OrigCtr,NEED);
		}
		Table.Interpolate(XVector,YVector,X[NewInterpDim],NewInterp[MDGenericIndex(Ctr,NEED)] );

		// Set value to edge point if goes past end of table
		if (X[NewInterpDim] < m_XSet[NewInterpDim][0])  NewInterp[MDGenericIndex(Ctr,NEED)] = YVector[0];
		if (X[NewInterpDim] > m_XSet[NewInterpDim][m_XSet[NewInterpDim].Dimension()-1]) 
			NewInterp[MDGenericIndex(Ctr,NEED)] = YVector[YVector.Dimension()-1];

		if (X[NewInterpDim] < m_XSet[NewInterpDim][0] || X[NewInterpDim] > m_XSet[NewInterpDim][m_XSet[NewInterpDim].Dimension()-1]) {
//			sprintf_s(ExtrapBuf,EBSIZE,"void UtpTabularFunctionMD::TableInterpolate extrapolating: point desired %lf lower %lf upper %lf range\n",
//				X[NewInterpDim],m_XSet[NewInterpDim][0],m_XSet[NewInterpDim][m_XSet[NewInterpDim].Dimension()-1]);
//			EdgeString = EdgeString + string(ExtrapBuf);
			OutOfTable = true;
		}
		MDFixedVectorIncrement(Ctr,NEED);
	}
	DataInterp = NewInterp;
}

template<class T>
void UtpTabularFunctionMD<T>::WriteBinary(const UtpString &Outname) const
{
	UtpBinaryFile out(Outname.GetString(),ios::out);
	// First output # of x vectors=Dimension
	long Dimension = m_XSet.Size();
	out.write(&Dimension,sizeof(long),1);
	T total=0;
	total += sizeof(long)*1;

	// next write out each vectors size
	long idim;
	for (idim=0;idim < m_XSet.Size();idim++) {
		long vDim = m_XSet[idim].Dimension();
		out.write(&vDim,sizeof(long),1);
	}
	total += sizeof(long)*m_XSet.Size();

	// next write each x vector as Ts
	for (idim=0;idim < m_XSet.Size();idim++) {
		out.write(m_XSet[idim].Vector(),sizeof(T),m_XSet[idim].Dimension());
		total += sizeof(T)*m_XSet[idim].Dimension();
	}

	// next write out the database as it is stored in Y
	out.write(m_Y.Vector(),sizeof(T),m_Y.Dimension());
	total += m_Y.Dimension()*sizeof(T);

	out.close();
}

// reads in a multi dimensional binary data base
template<class T>
void UtpTabularFunctionMD<T>::ReadBinary(const UtpString &DBname)
{
	UtpBinaryFile in(DBname.GetString(),ios::in);
	if (!in.rdbuf()->is_open()) {
		cout << "UtpTabularFunctionMD::ReadBinary couldn't open file " << DBname << endl;
		exit(-1);
	}

	// First read # of x vectors=Dimension
	long Dimension;
	in.read(&Dimension,sizeof(long),1);
	m_XSet.Reset(Dimension);
	
	// next read in each vectors size
	long idim;
	for (idim=0;idim < m_XSet.Size();idim++) {
		long vDim;
		in.read(&vDim,sizeof(long),1);
		m_XSet[idim].ResetDimension(vDim);
	}

	long MaxDim=1;
	UtpArray<T> Temp;
	// next read each x vector as Ts
	for (idim=0;idim < m_XSet.Size();idim++) {
		Temp.Resize(m_XSet[idim].Dimension());
		MaxDim *= Temp.Size();
		in.read(Temp.Array(),sizeof(T),Temp.Size());
		m_XSet[idim].SetVector( Temp.Size(), Temp.Array() );
	}

	// next read in the database as it is stored into Y
	Temp.Resize(MaxDim);
	in.read(Temp.Array(),sizeof(T),Temp.Size());
	m_Y.SetVector(Temp.Size(),Temp.Array());

	in.close();
}

template<class T>
ostream& operator<<(ostream& ros, const UtpTabularFunctionMD<T> &rTable)
{
	ros << "TabularFunction MD, Dim = " << rTable.XSet().Size() << endl;

	UtpVector<long> Dimension(rTable.XSet().Size());

	long MaxDim=1;
	// next write out each vectors size
	long idim;
	for (idim=0;idim < rTable.XSet().Size();idim++) {
		ros << rTable.XSet()[idim].Dimension() << " ";
		Dimension[idim] = rTable.XSet()[idim].Dimension();
		MaxDim *= Dimension[idim];
	}
	ros << endl;

	for (idim=0;idim < rTable.XSet().Size();idim++) {
		ros << "x[" << idim << "] " << rTable.XSet()[idim] << endl;
	}
	ros << "Table Data\n";
	UtpVector<long> Ind(rTable.XSet().Size());
	UtpVector<T> X(Ind.Dimension());
	for (long i=0;i < MaxDim;i++) {
		for (long j=0;j < rTable.XSet().Size();j++) {
			X[j] = rTable.XSet()[j][Ind[j]];
			ros << X[j] << " ";

		}
		ros << rTable.TableValue(X) << endl;
		MDVectorIncrement(Ind,Dimension);
	}
	return ros;
}

template<class T>
long MDVectorIndex(const UtpVector<long> &Index,const UtpArray< UtpVector<T> > &XSet)
{
	long Ind=0;
	long BlockDim = 1;
	for (long i=Index.Dimension()-1;i >= 0;i--) {
		if (Index[i] >= 0 && Index[i] < XSet[i].Dimension()) {
			Ind += BlockDim * Index[i];
			BlockDim *= XSet[i].Dimension();
//			cout << "Index BlockDim: " << Index << " " << BlockDim << endl;
		}		
		else {
			cout << "MDVectorIndex Index " << Index[i];
			cout << " out of bounds 0 " << XSet[i].Dimension()-1 << endl;
			exit(-1);
		}
	}
	return Ind;
}




#endif