29 #include <bias_config.h>
36 #ifdef BIAS_HAVE_CPP11
37 #include <initializer_list>
40 #include <Base/Debug/Error.hh>
41 #include <Base/Debug/Exception.hh>
42 #include <Base/Math/Vector.hh>
43 #include <Base/Math/Matrix3x3.hh>
44 #include <Base/Math/Operators.hh>
45 #include <Base/Math/Utils.hh>
47 #define VECTOR3_SIZE 3
74 class BIASMathBase_EXPORT
Vector3 {
88 explicit inline Vector3(
const T & scalar) { Set(scalar); };
93 explicit inline Vector3(
const T* pv) { Set(pv); };
98 explicit inline Vector3(
char *s);
102 inline Vector3(
const T &x,
const T &y,
const T &z) { Set(x, y, z); };
108 #ifdef BIAS_HAVE_CPP11
114 :
Vector3(static_cast<T>(v[0]), static_cast<T>(v[1]),
115 static_cast<T>(v[2])) {}
120 Vector3(
const std::initializer_list< T> &i) {
121 BIASASSERT(i.size() >= 3);
122 auto iter = i.begin();
124 data_[1] = *(++iter);
125 data_[2] = *(++iter);
143 inline T
const *
end()
const {
144 return &data_[VECTOR3_SIZE];
147 return &data_[VECTOR3_SIZE];
157 inline void Set(
const T *pv);
161 inline void Set(
const T& scalar) { Set(scalar, scalar, scalar); };
166 inline void Set(
const T& x,
const T& y,
const T& z);
173 inline void SetZero();
184 inline const unsigned int Size()
const {
return size(); };
186 inline const unsigned int size()
const {
return VECTOR3_SIZE; };
188 inline const unsigned int GetNumElements()
const {
return size(); };
191 inline bool IsZero()
const;
194 inline double Length()
const
195 {
return NormL2(); };
203 inline const T *
GetData()
const {
return data_; };
204 inline T *GetData() {
return data_; };
210 inline T NormL1()
const;
214 inline double NormL2()
const;
220 inline double Dist(
const Vector3<T> &vec)
const;
226 inline double DistLinf(
const Vector3<T> &vec)
const;
237 inline Vector3<T>& Normalize();
242 Vector3<T>& Normalize(Matrix3x3<T>& cov);
262 inline void CrossProduct(
const Vector3<T>& argvec,
263 Vector3<T>& destvec)
const;
269 {
Vector3<T> destvec; CrossProduct(argvec, destvec);
return destvec; };
274 inline void ScalarProduct(
const Vector3<T>& argvec, T& result)
const;
279 inline T ScalarProduct(
const Vector3<T>& argvec)
const;
284 inline void ElementwiseProduct(
const Vector3<T>& argvec,
292 ElementwiseProduct(argvec, destvec);
297 inline void ElementwiseDivision(
const Vector3<T>& argvec,
304 ElementwiseDivision(argvec, destvec);
311 inline void AddIP(
const T& scalar) { Add(scalar, *
this); }
315 inline void Add(
const T& scalar,
Vector3<T>& dest)
const;
319 inline void SubIP(
const T& scalar) { Sub(scalar, *
this); }
323 inline void Sub(
const T& scalar,
Vector3<T>& dest)
const;
327 inline void MultiplyIP(
const T& scalar) { Multiply(scalar, *
this); }
328 inline void MultIP(
const T& scalar) { MultiplyIP(scalar); }
332 inline void Multiply(
const T& scalar,
Vector3<T>& dest)
const;
333 inline void Mult(
const T& scalar,
Vector3<T>& dest)
const
334 { Multiply(scalar, dest); }
338 inline void DivideIP(
const T& scalar);
339 inline void DivIP(
const T& scalar) { DivideIP(scalar); }
343 inline void Divide(
const T& scalar,
Vector3<T>& dest)
const ;
344 inline void Div(
const T& scalar,
Vector3<T>& dest)
const
345 {Divide(scalar, dest);};
350 inline void AddIP(
const Vector3<T>&argvec) { Add(argvec, *
this);}
360 inline void SubIP(
const Vector3<T>& argvec) { Sub(argvec, *
this); }
390 Matrix<T> GetSkewSymmetricMatrix()
const;
405 return this->CoordSphereToEuclidean();
409 return this->CoordSphereToEuclidean();
424 return this->CoordEuclideanToSphere();
428 return this->CoordEuclideanToSphere();
433 {
return (*
this)[0]; };
434 inline T Radius()
const
435 {
return (*
this)[0]; };
438 {
return (*
this)[1]; };
440 {
return (*
this)[1]; };
443 {
return (*
this)[2]; };
444 inline T Theta()
const
445 {
return (*
this)[2]; };
452 int LoadBogTC(
const std::string & filename);
458 bool Load(
const std::string & filename);
464 bool Save(
const std::string & filename)
const;
475 { memcpy((
void *)data_, (
void *)vec.
GetData(), VECTOR3_SIZE*
sizeof(T));
481 {
if (vec.
size() == VECTOR3_SIZE)
482 memcpy((
void *)data_, (
void *)vec.
GetData(), VECTOR3_SIZE*
sizeof(T));
490 inline const T& operator[] (
const int i)
const;
496 inline T& operator[] (
const int i);
501 inline bool operator==(
const Vector3<T>& arg)
const
502 {
return ((data_[0]==arg.
data_[0])&&(data_[1]==arg.
data_[1])&&(data_[2]==arg.
data_[2])); }
506 inline bool operator!=(
const Vector3<T>& arg)
const
507 {
return ! operator==(arg); }
515 T data_[VECTOR3_SIZE];
525 std::istringstream inst(s);
548 if (vec.
size() != VECTOR3_SIZE){
549 BIASERR(
"The vector for initialization of Vector3 has not length 3");
572 return ( (data_[0]==(T)0)
574 && (data_[2]==(T)0) );
582 BIASASSERT(i < VECTOR3_SIZE);
589 BIASASSERT(i < VECTOR3_SIZE);
596 destvec[0] = data_[1] * argvec[2] - data_[2] * argvec[1];
597 destvec[1] = data_[2] * argvec[0] - data_[0] * argvec[2];
598 destvec[2] = data_[0] * argvec[1] - data_[1] * argvec[0];
604 result = data_[0] * argvec[0];
605 result += data_[1] * argvec[1];
606 result += data_[2] * argvec[2];
612 ScalarProduct(argvec, result);
619 destvec[0] = data_[0] * argvec[0];
620 destvec[1] = data_[1] * argvec[1];
621 destvec[2] = data_[2] * argvec[2];
627 destvec[0] = data_[0] / argvec[0];
628 destvec[1] = data_[1] / argvec[1];
629 destvec[2] = data_[2] / argvec[2];
634 return sqrt(
double(data_[0]*data_[0] + data_[1]*data_[1]
635 + data_[2]*data_[2]));
640 return sqrt(
double((data_[0]-vec[0])*(data_[0]-vec[0]) +
641 (data_[1]-vec[1])*(data_[1]-vec[1]) +
642 (data_[2]-vec[2])*(data_[2]-vec[2])));
651 for(
unsigned int i = 0; i < 3; i++)
653 tmp = fabs(static_cast<double> (data_[i] - vec[i]));
664 const T act_length = (T) NormL2();
666 if (act_length > std::numeric_limits<T>::epsilon() )
669 this->DivideIP(act_length);
678 return (data_[0] > 0 ? data_[0] : data_[0] * -1)
679 + (data_[1] > 0 ? data_[1] : data_[1] * -1)
680 + (data_[2] > 0 ? data_[2] : data_[2] * -1);
685 dest.
data_[0] = data_[0] + scalar;
686 dest.
data_[1] = data_[1] + scalar;
687 dest.
data_[2] = data_[2] + scalar;
692 destvec[0] = data_[0] + argvec[0];
693 destvec[1] = data_[1] + argvec[1];
694 destvec[2] = data_[2] + argvec[2];
699 dest.
data_[0] = data_[0] * scalar;
700 dest.
data_[1] = data_[1] * scalar;
701 dest.
data_[2] = data_[2] * scalar;
706 dest.
data_[0] = data_[0] - scalar;
707 dest.
data_[1] = data_[1] - scalar;
708 dest.
data_[2] = data_[2] - scalar;
713 destvec[0] = data_[0] - argvec[0];
714 destvec[1] = data_[1] - argvec[1];
715 destvec[2] = data_[2] - argvec[2];
720 Divide(scalar, *
this);
726 BEXCEPTION(
"Division by Zero!");
727 dest.
data_[0] = data_[0] / scalar;
728 dest.
data_[1] = data_[1] / scalar;
729 dest.
data_[2] = data_[2] / scalar;
809 operator<<(std::ostream& os, const BIAS::Vector3<unsigned char>& vec)
813 os << VECTOR3_SIZE << NL;
814 for (
int row = 0; row < VECTOR3_SIZE; row++) {
815 os << (
unsigned int)vec.GetData()[row] <<
" " << NL;
820 for (
int row = 0; row < VECTOR3_SIZE; row++)
821 os<<std::setw(3)<<(
unsigned int)vec.GetData()[row]<<
" ";
829 operator<<(std::ostream& os, const BIAS::Vector3<char>& vec)
833 os << VECTOR3_SIZE << NL;
834 for (
int row = 0; row < VECTOR3_SIZE; row++) {
835 os << (
unsigned int)vec.GetData()[row] <<
" " << NL;
840 for (
int row = 0; row < VECTOR3_SIZE; row++)
841 os<<std::setw(3)<<(int)vec.GetData()[row]<<
" ";
855 operator<<(std::ostream& os, const BIAS::Vector3<T>& vec)
859 os << VECTOR3_SIZE << NL;
860 for (
int row = 0; row < VECTOR3_SIZE; row++) {
861 os<<std::setprecision( DecimalsToStore<T>() );
863 os<<vec.GetData()[row] <<
" " << NL;
868 for (
int row = 0; row < VECTOR3_SIZE; row++){
869 os<<std::setprecision(CONSOLE_PRECISION_DIGITS);
870 os<<vec.GetData()[row] <<
" ";
885 inline std::istream &
890 if (N != VECTOR3_SIZE) {
891 BIASERR(
"can't assign to a Vector3<unsigned char> from a vector with dim " << N );
895 for (
int row = 0; row < N; row++)
898 BIASASSERT(tmp>=std::numeric_limits<unsigned char>::min() );
899 BIASASSERT(tmp<=std::numeric_limits<unsigned char>::max() );
900 vec.
GetData()[row] = (
unsigned char)tmp;
906 inline std::istream &
911 if (N != VECTOR3_SIZE) {
912 BIASERR(
"can't assign to a Vector3<unsigned char> from a vector with dim " << N );
916 for (
int row = 0; row < N; row++)
919 BIASASSERT(tmp>=std::numeric_limits< char>::min() );
920 BIASASSERT(tmp<=std::numeric_limits< char>::max() );
921 vec.
GetData()[row] = (char)tmp;
938 if (N != VECTOR3_SIZE) {
939 BIASERR(
"can't assign to a Vector3<T> from a vector with dim " << N );
942 for (
int row = 0; row < N; row++) {
953 #endif // _Vector3_hh_
Vector3(const Vector3< T > &vec)
copy constructor
void Set(const T *pv)
copy the array of vectorsize beginning at *T to this->data_
const T * GetData() const
get the data pointer the member function itself is const (before {..}) because it doesn't change the ...
double DistLinf(const Vector3< T > &vec) const
Return the L inf distance of 2 vectors.
T NormL1() const
Return the L1 norm: |a| + |b| + |c|.
class for column vectors with arbitrary size
void Add(const T &scalar, Vector3< T > &dest) const
Addition with a scalar, storing results in destionation vector.
Vector3< T > & operator*=(Vector3< T > &vec, const T &scalar)
Vector3(const T &x, const T &y, const T &z)
constructor with element assigment using explicit values
void ScalarProduct(const Vector3< T > &argvec, T &result) const
scalar product (=inner product) of two vectors, storing the result in result
void SetZero()
set all values to 0
bool IsConsoleStream(const std::ostream &os)
Helper returning true on console ostreams like cerr, cout Useful to distinguish between console (cout...
void Multiply(const T &scalar, Vector3< T > &dest) const
Multiplication with a scalar, storing results in destination vector.
class BIASMathBase_EXPORT Vector3
void clear()
stl conform interface
std::istream & operator>>(std::istream &is, Vector3< T > &vec)
Input with streams.
Vector3< T > & operator+=(Vector3< T > &vec, const T &scalar)
double Dist(const Vector3< T > &vec) const
Return the euclidean distance of 2 vectors.
Vector3(const T *pv)
assignment with an array of values which is copied into this ones class members
void CrossProduct(const Vector3< T > &argvec, Vector3< T > &destvec) const
cross product of two vectors destvec = this x argvec
T const * begin() const
Iterator to first component of vector.
void AddIP(const T &scalar)
Addition (in place) of an scalar.
void ElementwiseProduct(const Vector3< T > &argvec, Vector3< T > &destvec) const
multiply two vectors elementwise and store the result vector to destvec
Vector3< T > & operator+=(Vector3< T > &vec, const Vector3< T > &argvec)
const T & operator[](const int i) const
access an element of the vector with 0-based indizes.
is a 'fixed size' quadratic matrix of dim.
void MultiplyIP(const T &scalar)
Multiplication (in place) of an scalar.
class Vector3 contains a Vector of fixed dim.
void ElementwiseDivision(const Vector3< T > &argvec, Vector3< T > &destvec) const
void Sub(const T &scalar, Vector3< T > &dest) const
Substraction with a scalar, storing results in destination vector.
T * GetData() const
get the pointer to the data array of the vector (for faster direct memory access) ...
matrix class with arbitrary size, indexing is row major.
void SubIP(const T &scalar)
Substraction (in place) of an scalar.
Vector3< T > & operator/=(Vector3< T > &vec, const T &scalar)
std::istream & operator>>(std::istream &is, Vector3< unsigned char > &vec)
Input with streams specialization for numerical display of unsigned char instead of alphebetically...
void Divide(const T &scalar, Vector3< T > &dest) const
Division with a scalar, storing results in destination vector.
Vector3< T > & operator-=(Vector3< T > &vec, const Vector3< T > &argvec)
void DivideIP(const T &scalar)
Division (in place) of an scalar.
T const * end() const
Iterator pointing to one element after the last vector element.
Vector3< T > & Normalize()
normalize this vector to length 1
double NormL2() const
the L2 norm sqrt(a^2 + b^2 + c^2)
BIASCommon_EXPORT std::istream & operator>>(std::istream &is, BIAS::TimeStamp &ts)
Standard input operator for TimeStamps.
Vector3(const T &scalar)
assignment with a constant value for all elements
Vector3< T > & operator-=(Vector3< T > &vec, const T &scalar)