vector.c++

卡尔曼滤波类

// Vector.cpp		a vector 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[] = "@(#)vector.c++	1.12 10:24:47 5/11/95   EFC";

#include <error.hpp>
#include <vector.hpp>

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

ostream* Vector::ostr = &cerr;

Vector& Vector::operator=(const Vector &b)
{
	if ( size() != b.size())
	{
		Error error("Vector =", *ostr);
		error << "a = b different sizes, a = " << size()
		      << ", b = " << b.size();
		 error.fatal();
	}

	(BasicArray& )*this = (BasicArray &)b;

	return *this;

}

Vector& Vector::operator=(const BasicArray &b)
{
	if ( size() != b.size())
	{
		Error error("Vector =", *ostr);
		error << "a = b different sizes, a = " << size()
		      << ", b = " << b.size();
		 error.fatal();
	}

	(BasicArray& )*this = b;

	return *this;

}

scalar_type Vector::operator=(const scalar_type d)
{
#ifdef DEBUG
	TraceBack tb( "Vector::operator=(scalar)" );
#endif
	const int sz = size(); 
	for (int i = 0; i < sz; i++)
		( *this )[i] = d;

	return d;	
}

Vector& Vector::operator+=(const Vector &a)		// v += a
{
	if ( a.size() != size())
	{
		Error error("Vector += ", *ostr);
		error << "vector a += vector b different sizes, a = " << size()
		      << ", b = " << a.size();
		error.fatal();
	}
	const int sz = size();
	for (int i = 0; i < sz; i++)
		( *this )[i] += a[i];

	return *this;
}

Vector& Vector::operator-=(const Vector &a)		// v -= a
{
	if ( a.size() != size())
	{
		Error error("Vector -= ", *ostr);
		error << "vector a -= vector b different sizes, a = " << size()
		      << ", b = " << a.size();
		error.fatal();
	}

	const int sz = size(); 
	for (int i = 0; i < sz; i++)
		( *this )[i] -= a[i];

	return *this;
}

Vector operator+(const Vector& a, const Vector &b)		//  v = a + b
{
	if ( a.size() != b.size())
	{
		Error error("Vector +", *Vector::ostr);
		error << "vector a + vector b different sizes, a = " << a.size()
		      << ", b = " << b.size();
		error.fatal();

	}

	int sz = a.size();
	Vector sum(sz);

	for (int i = 0; i < sz; i++)
		sum[i] = a[i] + b[i];

	return sum;

}

Vector operator-(const Vector& a, const Vector &b)		//  v = a - b
{

	if ( a.size() != b.size())
	{
		Error error("Vector -", *Vector::ostr);
		error << "vector a - vector b different sizes, a = " << a.size()
		      << ", b = " << b.size();
		error.fatal();
	}

	const int sz = a.size();
	Vector diff(sz);

	for (int i = 0; i < sz; i++)
		diff[i] = a[i] - b[i];

	return diff;

}

// vector dot product
scalar_type operator*(const Vector &a,const Vector &b)		//  v = a * b
{
	if ( a.size() != b.size() )
	{
		Error error("Vector *", *Vector::ostr);
		error << "vector a * vector b incommensurate, a = " << a.size()
			<< ", b = " << b.size();
		error.fatal();
	}

	int i, sz = a.size();
	scalar_type prod, zero = 0.0;

	for (i = 0, prod = zero; i < sz; i++)
			prod += a[i] * b[i];

	return prod;

}

Vector  operator*(const Vector& v, const double d)		// v = a * d
{
#ifdef DEBUG
	TraceBack tb( "Vector::operator*(Vector,scalar)" );
#endif
	int i, sz = v.size();
	Vector b = v;

#ifdef DEBUG
	tb << "dimension: " << v.size() << endl;
#endif

	for (i = 0; i < sz; i++)
			b[i] *= d;

	return b;

}

#ifdef COMPLEX
Vector  operator*(Vector& v, const Complex d)		// v = a * c
{
#ifdef DEBUG
	TraceBack tb( "Vector::operator*(Vector,complex)" );
#endif
	int i, sz = v.size();
	Vector b = v;

#ifdef DEBUG
	tb << "dimension: " << size() << endl;
#endif

	for (i = 0; i < sz; i++)
			b[i] *= d;

	return b;

}
#endif

bool operator==(const Vector &a,const Vector &b)       //	bool i = (a == b)
{
	if ( a.size() != b.size() )
	{
		Error error("Vector ==", *Vector::ostr);
		error << "vector a and vector b different sizes, a = " << a.size()
			<< ", b = " << b.size();
		error.fatal();
	}

	int i, sz = a.size();
	bool l = true;

	for (i = 0; i < sz; i++)
			l = l && (bool)( a[i] == b[i] );

	return l;

}