barray2d.c++

面向对象的卡尔曼滤波器源码

// barray2d.cpp		a 2D array class implementation

//                      (c) Copyright 1995, Everett F. Carter Jr.
//                      Permission is granted by the author to use
//			this software for any application provided this
//			copyright notice is preserved.

static const char rcsid[] = "@(#)barray2d.c++	1.5 12:28:01 6/3/96   EFC";

#include <sys/types.h>

#include <error.hpp>
#include <barray2d.hpp>

#ifdef DEBUG
#include <traceback.hpp>
#endif

void Basic2DArray::init(const int r,const int c)		// initialization
{
#ifdef DEBUG
	TraceBack tb( "Basic2DArray::init()" );
	cerr << "r = " << r << " c = " << c << '\n';
#endif
	int i,j;

	if ( r <= 0  || c <= 0 )
	{
		Error error( "Basic2DArray::init");
		error << "bad Basic2DArray size " << r << " and " << c;
		error.fatal();
	}
	
	rz = r;	cz = c;
#ifdef DEBUG
	cerr << "rz = " << rz << " cz = " << cz << '\n';
	cerr << "rows = " << rows << " cols = " << cols << endl;
#endif
	// m = new (calar_type *[rows];
	m = new scalar_type *[rz];        // should be same as above line but
                                            // gcc 2.7.2 croaks on 'rows'

#ifdef DEBUG
	cerr << "\tmade the rows, now want cols: " << (cols * sizeof(scalar_type))
             << '\n';
#endif
	for (j = 0; j < rows; j++)
		m[j] = (scalar_type *)new char[cols * sizeof(scalar_type)];

#ifdef DEBUG
	cerr << "\tzeroing it\n";
#endif

	scalar_type zero = 0.0;

	for (i = 0; i < rows; i++)
		for (j = 0; j < cols; j++)
			m[i][j] = zero;

}

// Basic2DArray m = a, copy constructor
Basic2DArray::Basic2DArray(const Basic2DArray &a) : rows(rz), cols(cz)
{
#ifdef DEBUG
	TraceBack tb( "Basic2DArray::Basic2DArray(Basic2DArray&)" );
#endif
	int i, j;

	init(a.rows,a.cols);
	by_columns = a.by_columns;

	for (i = 0; i < rows; i++)
		for (j = 0; j < cols; j++)
			m[i][j] = a.m[i][j];
}

Basic2DArray& Basic2DArray::operator=(const Basic2DArray &a)		// m = a
{
#ifdef DEBUG
	TraceBack tb( "Basic2DArray::operator=(Basic2DArray&)" );
#endif
	int i, j;

	if ( this == &a )
		return *this;

	for (i = 0; i < rows; i++)
		delete [](m[i]);
	delete []m;

	init(a.rows,a.cols);
	for (i = 0; i < rows; i++)
		for (j = 0; j < cols; j++)
			m[i][j] = a.m[i][j];

	by_columns = a.by_columns;

	return *this;

}

Basic2DArray::~Basic2DArray()
{
#ifdef DEBUG
	TraceBack tb( "Basic2DArray::~Basic2DArray()" );
#endif

	for (int j = 0; j < rows; j++)
		delete [](m[j]);

	delete []m;

}

void Basic2DArray::resize(const int nr,const int nc)
{
#ifdef DEBUG
	TraceBack tb( "Basic2DArray::resize()" );
#endif
	if ( rows > 0 )
	{
		for (int j = 0; j < rows; j++)
			delete [](m[j]);

		delete []m;
	}

	init(nr, nc);

}

void Basic2DArray::reset(const scalar_type v)
{
#ifdef DEBUG
	TraceBack tb( "Basic2DArray::reset()" );
#endif
	for (int i = 0; i < rows; i++)
		for (int j = 0; j < cols; j++)
			m[i][j] = v;
	
}

scalar_type& Basic2DArray::elem(const int i,const int j)
{
	if ( i < 0  || rows <= i || j < 0 || cols <= j )
	{
		Error error( "Basic2DArray::elem");
		if (i < 0 || rows <= i)
			 error << "bad first index " << i;
		if (j < 0 || cols <= j)
			 error << "bad second index " << j;
		error.fatal();
	}

	return m[i][j];
}

scalar_type Basic2DArray::elem(const int i,const int j) const
{
	if ( i < 0  || rows <= i || j < 0 || cols <= j )
	{
		Error error( "Basic2DArray::elem");
		if (i < 0 || rows <= i)
			 error << "bad first index " << i;
		if (j < 0 || cols <= j)
			 error << "bad second index " << j;
		error.fatal();
	}

	return m[i][j];
}

int Basic2DArray::write(ofstream &ofs)
{
	int count = cols * sizeof( scalar_type );

	for (int k = 0; k < rows; k++)
	{
		ofs.write( (char *)m[k], count );

		if ( ofs.fail() )
			return -1;
	}

	return 0;
 
}

int Basic2DArray::read(ifstream &ifs)
{
	int count = cols * sizeof( scalar_type );

	for (int k = 0; k < rows; k++) 
	{
		ifs.read( (char *)m[k], count );

		if ( ifs.fail() )
			return -1;
	}

	return 0;

}

istream& operator>> (istream& is,Basic2DArray& a)
{
	int i, j;
	int r = a.rows, c = a.cols;

	if ( a.by_columns )
		for (j = 0; j < c; j++)
			for (i = 0; i < r; i++)
				is >>  a.m[i][j];
	else
		for (i = 0; i < r; i++)
			for (j = 0; j < c; j++)
				is >>  a.m[i][j];

	return is;
	
}

ostream& operator<< (ostream& os,const Basic2DArray& a)
{
	int i, j;
	int r = a.rows, c = a.cols;

	if ( a.by_columns )
		for (j = 0; j < c; j++)
		{
			for (i = 0; i < r; i++)
				os <<  a.m[i][j] << ' ';
			os << '\n';
		}

	else
		for (i = 0; i < r; i++)
		{
			for (j = 0; j < c; j++)
				os <<  a.m[i][j] << ' ';
			os << '\n';
		}

	return os;
	
}