Gabriele Cosmo
1139 lines
33 KiB
Plaintext
1139 lines
33 KiB
Plaintext
// Copyright (C) 2010, Guy Barrand. All rights reserved.
|
|
// See the file tools.license for terms.
|
|
|
|
#ifndef tools_MATCOM
|
|
#define tools_MATCOM
|
|
|
|
/* NOTE : bof, no big improvement.
|
|
#include <cstring> //memcpy
|
|
inline void vec_copy(double* a_to,const double* a_from,unsigned int a_number) {
|
|
::memcpy(a_to,a_from,a_number*sizeof(double));
|
|
}
|
|
|
|
template <class T>
|
|
inline void vec_copy(T* a_to,const T* a_from,unsigned int a_number) {
|
|
T* pto = (T*)a_to;
|
|
T* pfm = (T*)a_from;
|
|
for(unsigned int i=0;i<a_number;i++,pto++,pfm++) *pto = *pfm;
|
|
}
|
|
*/
|
|
|
|
/*NOTE : bof, no big improvement.
|
|
#include <Accelerate/Accelerate.h>
|
|
|
|
inline void vec_add(const float* a_1,const float* a_2,float* a_res,unsigned int a_number) {
|
|
::vDSP_vadd(a_1,1,a_2,1,a_res,1,a_number);
|
|
}
|
|
inline void vec_sub(const float* a_1,const float* a_2,float* a_res,unsigned int a_number) {
|
|
::vDSP_vsub(a_1,1,a_2,1,a_res,1,a_number);
|
|
}
|
|
*/
|
|
|
|
/*
|
|
template <class T>
|
|
inline void vec_add(const T* a_1,const T* a_2,T* a_res,unsigned int a_number) {
|
|
T* p1 = (T*)a_1;
|
|
T* p2 = (T*)a_2;
|
|
T* pr = (T*)a_res;
|
|
for(unsigned int i=0;i<a_number;i++,p1++,p2++,pr++) *pr = *p1+*p2;
|
|
}
|
|
|
|
template <class T>
|
|
inline void vec_sub(const T* a_1,const T* a_2,T* a_res,unsigned int a_number) {
|
|
T* p1 = (T*)a_1;
|
|
T* p2 = (T*)a_2;
|
|
T* pr = (T*)a_res;
|
|
for(unsigned int i=0;i<a_number;i++,p1++,p2++,pr++) *pr = *p1-*p2;
|
|
}
|
|
*/
|
|
|
|
#include "eqT"
|
|
|
|
#include <cstddef> //size_t
|
|
|
|
// common code to class mat and nmat.
|
|
|
|
#define TOOLS_MATCOM \
|
|
protected:\
|
|
static T zero() {return T();}\
|
|
static T one() {return T(1);}\
|
|
static T minus_one() {return T(-1);}\
|
|
static T two() {return T(2);}\
|
|
public:\
|
|
typedef T elem_t;\
|
|
typedef unsigned int size_type;\
|
|
public:\
|
|
unsigned int rows() const {return dimension();}\
|
|
unsigned int cols() const {return dimension();}\
|
|
\
|
|
void set_value(unsigned int aR,unsigned int aC,const T& a_value) { \
|
|
m_vec[aR + aC * dimension()] = a_value;\
|
|
}\
|
|
\
|
|
const T& value(unsigned int aR,unsigned int aC) const { \
|
|
return m_vec[aR + aC * dimension()];\
|
|
}\
|
|
\
|
|
T value(unsigned int aR,unsigned int aC) { \
|
|
return m_vec[aR + aC * dimension()];\
|
|
}\
|
|
\
|
|
void set_matrix(const TOOLS_MAT_CLASS& a_m){ /*optimization.*/\
|
|
_copy(a_m.m_vec);\
|
|
}\
|
|
\
|
|
void set_constant(const T& a_v){\
|
|
for(unsigned int i=0;i<dim2();i++) m_vec[i] = a_v;\
|
|
}\
|
|
void set_zero(){\
|
|
set_constant(zero());\
|
|
}\
|
|
void set_identity() {\
|
|
set_zero();\
|
|
for(unsigned int i=0;i<dimension();i++) m_vec[i+i*dimension()] = one();\
|
|
}\
|
|
void set_diagonal(const T& a_s) {\
|
|
set_zero();\
|
|
for(unsigned int i=0;i<dimension();i++) m_vec[i+i*dimension()] = a_s;\
|
|
}\
|
|
typedef T (*func)(const T&);\
|
|
void apply_func(func a_func) {\
|
|
T* pos = m_vec;\
|
|
for(unsigned int i=0;i<dim2();i++,pos++) *pos = a_func(*pos);\
|
|
}\
|
|
template <class RANDOM>\
|
|
void set_random(RANDOM& a_random) {\
|
|
for(unsigned int i=0;i<dim2();i++) m_vec[i] = a_random.shoot();\
|
|
}\
|
|
template <class RANDOM>\
|
|
void set_symmetric_random(RANDOM& a_random) {\
|
|
unsigned int _D = dimension();\
|
|
{for(unsigned int r=0;r<_D;r++) set_value(r,r,a_random.shoot());}\
|
|
T rd;\
|
|
{for(unsigned int r=0;r<_D;r++) {\
|
|
for(unsigned int c=(r+1);c<_D;c++) {\
|
|
rd = a_random.shoot();\
|
|
set_value(r,c,rd);\
|
|
set_value(c,r,rd);\
|
|
}\
|
|
}}\
|
|
}\
|
|
template <class RANDOM>\
|
|
void set_antisymmetric_random(RANDOM& a_random) {\
|
|
unsigned int _D = dimension();\
|
|
{for(unsigned int r=0;r<_D;r++) set_value(r,r,zero());}\
|
|
T rd;\
|
|
{for(unsigned int r=0;r<_D;r++) {\
|
|
for(unsigned int c=(r+1);c<_D;c++) {\
|
|
rd = a_random.shoot();\
|
|
set_value(r,c,rd);\
|
|
set_value(c,r,minus_one()*rd);\
|
|
}\
|
|
}}\
|
|
}\
|
|
public:\
|
|
template <class ARRAY>\
|
|
bool mul_array(ARRAY& a_array,T a_tmp[]) const {\
|
|
/* a_array = this *= a_array */\
|
|
unsigned int _dim = dimension();\
|
|
T* pos = a_tmp;\
|
|
for(unsigned int r=0;r<_dim;r++,pos++) {\
|
|
*pos = T();\
|
|
for(unsigned int c=0;c<_dim;c++) *pos += m_vec[r+c*_dim]*a_array[c];\
|
|
}\
|
|
{for(unsigned int i=0;i<_dim;i++) a_array[i] = a_tmp[i];}\
|
|
return true;\
|
|
}\
|
|
template <class VEC>\
|
|
bool mul_vec(VEC& a_vec,T a_tmp[]) const {\
|
|
/* a_vec = this *= a_vec */\
|
|
unsigned int _dim = dimension();\
|
|
if(a_vec.dimension()!=_dim) return false;\
|
|
T* pos = a_tmp;\
|
|
for(unsigned int r=0;r<_dim;r++,pos++) {\
|
|
*pos = T();\
|
|
for(unsigned int c=0;c<_dim;c++) *pos += m_vec[r+c*_dim]*a_vec[c];\
|
|
}\
|
|
{for(unsigned int i=0;i<_dim;i++) a_vec[i] = a_tmp[i];}\
|
|
return true;\
|
|
}\
|
|
template <class VEC>\
|
|
bool mul_vec(VEC& a_vec) const {\
|
|
T* _tmp = new T[dimension()];\
|
|
bool status = mul_vec(a_vec,_tmp);\
|
|
delete [] _tmp;\
|
|
return status;\
|
|
}\
|
|
\
|
|
bool mul_array(T a_vec[],T a_tmp[]) const {\
|
|
/* a_vec = this *= a_vec */\
|
|
unsigned int _dim = dimension();\
|
|
T* pos = a_tmp;\
|
|
for(unsigned int r=0;r<_dim;r++,pos++) {\
|
|
*pos = T();\
|
|
for(unsigned int c=0;c<_dim;c++) *pos += m_vec[r+c*_dim]*a_vec[c];\
|
|
}\
|
|
{for(unsigned int i=0;i<_dim;i++) a_vec[i] = a_tmp[i];}\
|
|
return true;\
|
|
}\
|
|
bool mul_array(T a_vec[]) const {\
|
|
T* _tmp = new T[dimension()];\
|
|
bool status = mul_array(a_vec,_tmp);\
|
|
delete [] _tmp;\
|
|
return status;\
|
|
}\
|
|
\
|
|
void mul_mtx(const TOOLS_MAT_CLASS& a_m) {\
|
|
_mul_mtx(a_m.m_vec);\
|
|
}\
|
|
void mul_mtx(const TOOLS_MAT_CLASS& a_m,T a_tmp[]) {\
|
|
_mul_mtx(a_m.m_vec,a_tmp);\
|
|
}\
|
|
void left_mul_mtx(const TOOLS_MAT_CLASS& a_m) { \
|
|
/* this = a_m * this :*/\
|
|
_left_mul_mtx(a_m.m_vec);\
|
|
}\
|
|
bool equal(const TOOLS_MAT_CLASS& a_m) const {\
|
|
if(&a_m==this) return true;\
|
|
for(unsigned int i=0;i<dim2();i++) {\
|
|
if(m_vec[i]!=a_m.m_vec[i]) return false;\
|
|
}\
|
|
return true;\
|
|
}\
|
|
\
|
|
bool equal(const TOOLS_MAT_CLASS& a_m,const T& a_prec) const {\
|
|
if(&a_m==this) return true;\
|
|
T* tp = (T*)m_vec;\
|
|
T* mp = (T*)a_m.m_vec;\
|
|
for(unsigned int i=0;i<dim2();i++,tp++,mp++) {\
|
|
T diff = (*tp) - (*mp);\
|
|
if(diff<zero()) diff *= minus_one();\
|
|
if(diff>=a_prec) return false;\
|
|
}\
|
|
return true;\
|
|
}\
|
|
\
|
|
template <class PREC>\
|
|
bool equal_prec(const TOOLS_MAT_CLASS& a_m,const PREC& a_prec,PREC(*a_fabs)(const T&)) const {\
|
|
if(&a_m==this) return true;\
|
|
T* tp = (T*)m_vec;\
|
|
T* mp = (T*)a_m.m_vec;\
|
|
for(unsigned int i=0;i<dim2();i++,tp++,mp++) {\
|
|
T diff = (*tp) - (*mp);\
|
|
if(a_fabs(diff)>=a_prec) return false;\
|
|
}\
|
|
return true;\
|
|
}\
|
|
\
|
|
void mx_diff(const TOOLS_MAT_CLASS& a_m,T& a_mx_diff) const {\
|
|
T* tp = (T*)m_vec;\
|
|
T* mp = (T*)a_m.m_vec;\
|
|
a_mx_diff = (*tp) - (*mp);\
|
|
if(a_mx_diff<zero()) a_mx_diff *= minus_one();\
|
|
for(unsigned int i=0;i<dim2();i++,tp++,mp++) {\
|
|
T diff = (*tp) - (*mp);\
|
|
if(diff<zero()) diff *= minus_one();\
|
|
a_mx_diff = (diff>a_mx_diff?diff:a_mx_diff);\
|
|
}\
|
|
}\
|
|
\
|
|
bool to_rm_is_proportional(const TOOLS_MAT_CLASS& a_m,const T& a_prec,T& a_factor) const {\
|
|
if(&a_m==this) {a_factor=one();return true;}\
|
|
/* If true, then : a_m = a_factor * this.*/\
|
|
a_factor = zero();\
|
|
T* tp = (T*)m_vec;\
|
|
T* mp = (T*)a_m.m_vec;\
|
|
bool first = true;\
|
|
for(unsigned int i=0;i<dim2();i++,tp++,mp++) {\
|
|
if( ((*tp)==zero()) && ((*mp)==zero())) {\
|
|
continue;\
|
|
/*} else if( ((*tp)!=zero()) && ((*mp)==zero())) {\
|
|
return false;*/\
|
|
} else if( ((*tp)==zero()) && ((*mp)!=zero())) {\
|
|
return false;\
|
|
} else {\
|
|
if(first) {\
|
|
a_factor = (*mp)/(*tp);\
|
|
first = false;\
|
|
} else {\
|
|
T diff = (*tp)*a_factor - (*mp);\
|
|
if(diff<zero()) diff *= minus_one();\
|
|
if(diff>=a_prec) return false;\
|
|
}\
|
|
}\
|
|
}\
|
|
return true;\
|
|
}\
|
|
\
|
|
public:\
|
|
bool is_proportional(const TOOLS_MAT_CLASS& a_right,T& a_factor) const {\
|
|
/* If true, then : a_right = a_factor * this. a_factor could be zero.*/\
|
|
if(this==&a_right) {a_factor=T(1);return true;}\
|
|
a_factor = zero();\
|
|
if(dimension()!=a_right.dimension()) return false;\
|
|
T* lp = (T*)m_vec;\
|
|
T* rp = (T*)a_right.m_vec;\
|
|
bool first = true;\
|
|
size_t _data_size = data_size();\
|
|
for(size_t i=0;i<_data_size;i++,lp++,rp++) {\
|
|
if(*lp==zero()) {\
|
|
if(*rp==zero()) continue;\
|
|
return false;\
|
|
}\
|
|
if(first) {\
|
|
a_factor = (*rp)/(*lp);\
|
|
first = false;\
|
|
continue;\
|
|
}\
|
|
if((*lp)*a_factor!=(*rp)) return false;\
|
|
}\
|
|
return true;\
|
|
}\
|
|
\
|
|
template <class PREC>\
|
|
bool is_proportional_prec(const TOOLS_MAT_CLASS& a_right,const PREC& a_prec,PREC(*a_fabs)(const T&),T& a_factor) const {\
|
|
/* If true, then : a_right = a_factor * this. a_factor could be zero.*/\
|
|
if(this==&a_right) {a_factor=T(1);return true;}\
|
|
a_factor = zero();\
|
|
if(dimension()!=a_right.dimension()) return false;\
|
|
T* lp = (T*)m_vec;\
|
|
T* rp = (T*)a_right.m_vec;\
|
|
bool first = true;\
|
|
size_t _data_size = data_size();\
|
|
for(size_t i=0;i<_data_size;i++,lp++,rp++) {\
|
|
if(is_zero(*lp,a_prec,a_fabs)) {\
|
|
if(is_zero(*rp,a_prec,a_fabs)) continue;\
|
|
return false;\
|
|
}\
|
|
if(first) {\
|
|
a_factor = (*rp)/(*lp);\
|
|
first = false;\
|
|
continue;\
|
|
}\
|
|
if(!numbers_are_equal((*lp)*a_factor,*rp,a_prec,a_fabs)) return false;\
|
|
}\
|
|
return true;\
|
|
}\
|
|
\
|
|
bool is_diagonal() const {\
|
|
unsigned int _D = dimension();\
|
|
for(unsigned int r=0;r<_D;r++) {\
|
|
for(unsigned int c=0;c<_D;c++) {\
|
|
if(c!=r) {if(value(r,c)!=zero()) return false;}\
|
|
}\
|
|
}\
|
|
return true;\
|
|
}\
|
|
\
|
|
template <class PREC>\
|
|
bool is_diagonal_prec(const PREC& a_prec,PREC(*a_fabs)(const T&)) const {\
|
|
unsigned int _D = dimension();\
|
|
for(unsigned int r=0;r<_D;r++) {\
|
|
for(unsigned int c=0;c<_D;c++) {\
|
|
if(c!=r) {if(!is_zero(value(r,c),a_prec,a_fabs)) return false;}\
|
|
}\
|
|
}\
|
|
return true;\
|
|
}\
|
|
\
|
|
bool is_identity() const {\
|
|
unsigned int _D = dimension();\
|
|
{for(unsigned int r=0;r<_D;r++) {\
|
|
if(value(r,r)!=one()) return false;\
|
|
}}\
|
|
{for(unsigned int r=0;r<_D;r++) {\
|
|
for(unsigned int c=0;c<_D;c++) {\
|
|
if(c!=r) {if(value(r,c)!=zero()) return false;}\
|
|
}\
|
|
}}\
|
|
return true;\
|
|
}\
|
|
\
|
|
template <class PREC>\
|
|
bool is_identity_prec(const PREC& a_prec,PREC(*a_fabs)(const T&)) const {\
|
|
unsigned int _D = dimension();\
|
|
{for(unsigned int r=0;r<_D;r++) {\
|
|
if(!numbers_are_equal(value(r,r),one(),a_prec,a_fabs)) return false;\
|
|
}}\
|
|
{for(unsigned int r=0;r<_D;r++) {\
|
|
for(unsigned int c=0;c<_D;c++) {\
|
|
if(c!=r) {if(!is_zero(value(r,c),a_prec,a_fabs)) return false;}\
|
|
}\
|
|
}}\
|
|
return true;\
|
|
}\
|
|
\
|
|
const T* data() const {return m_vec;}\
|
|
unsigned int size() const {return dim2();}\
|
|
unsigned int data_size() const {return dim2();} /*for mathz*/\
|
|
\
|
|
T trace() const {\
|
|
T _value = zero();\
|
|
unsigned int _D = dimension();\
|
|
for(unsigned int c=0;c<_D;c++) _value += m_vec[c+c*_D];\
|
|
return _value;\
|
|
}\
|
|
\
|
|
void transpose() {\
|
|
unsigned int _D = dimension();\
|
|
for(unsigned int r=0;r<_D;r++) {\
|
|
for(unsigned int c=(r+1);c<_D;c++) {\
|
|
T vrc = value(r,c);\
|
|
T vcr = value(c,r);\
|
|
set_value(r,c,vcr);\
|
|
set_value(c,r,vrc);\
|
|
}\
|
|
}\
|
|
}\
|
|
\
|
|
void multiply(const T& a_T) {\
|
|
for(unsigned int i=0;i<dim2();i++) m_vec[i] *= a_T;\
|
|
}\
|
|
\
|
|
bool is_symmetric() const {\
|
|
unsigned int _D = dimension();\
|
|
for(unsigned int r=0;r<_D;r++) {\
|
|
for(unsigned int c=(r+1);c<_D;c++) {\
|
|
if(value(r,c)!=value(c,r)) return false;\
|
|
}\
|
|
}\
|
|
return true;\
|
|
}\
|
|
\
|
|
template <class PREC>\
|
|
bool is_symmetric_prec(const PREC& a_prec,PREC(*a_fabs)(const T&)) const {\
|
|
unsigned int _D = dimension();\
|
|
for(unsigned int r=0;r<_D;r++) {\
|
|
for(unsigned int c=(r+1);c<_D;c++) {\
|
|
T diff = value(r,c)-value(c,r);\
|
|
if(a_fabs(diff)>=a_prec) return false;\
|
|
}\
|
|
}\
|
|
return true;\
|
|
}\
|
|
\
|
|
bool is_antisymmetric() const {\
|
|
unsigned int _D = dimension();\
|
|
{for(unsigned int r=0;r<_D;r++) {\
|
|
if(value(r,r)!=zero()) return false;\
|
|
}}\
|
|
for(unsigned int r=0;r<_D;r++) {\
|
|
for(unsigned int c=(r+1);c<_D;c++) {\
|
|
if(value(r,c)!=minus_one()*value(c,r)) return false;\
|
|
}\
|
|
}\
|
|
return true;\
|
|
}\
|
|
\
|
|
template <class PREC>\
|
|
bool is_antisymmetric_prec(const PREC& a_prec,PREC(*a_fabs)(const T&)) const {\
|
|
unsigned int _D = dimension();\
|
|
{for(unsigned int r=0;r<_D;r++) {\
|
|
if(a_fabs(value(r,r))>=a_prec) return false;\
|
|
}}\
|
|
for(unsigned int r=0;r<_D;r++) {\
|
|
for(unsigned int c=(r+1);c<_D;c++) {\
|
|
T diff = value(r,c)-minus_one()*value(c,r);\
|
|
if(a_fabs(diff)>=a_prec) return false;\
|
|
}\
|
|
}\
|
|
return true;\
|
|
}\
|
|
\
|
|
void symmetric_part(TOOLS_MAT_CLASS& a_res) const {\
|
|
a_res = *this;\
|
|
a_res.transpose();\
|
|
a_res += *this;\
|
|
a_res.multiply(one()/two());\
|
|
}\
|
|
\
|
|
void antisymmetric_part(TOOLS_MAT_CLASS& a_res) const {\
|
|
a_res = *this;\
|
|
a_res.transpose();\
|
|
a_res.multiply(minus_one());\
|
|
a_res += *this;\
|
|
a_res.multiply(one()/two());\
|
|
}\
|
|
\
|
|
template <class PREC>\
|
|
bool is_block_UL_DR(const PREC& a_prec,PREC(*a_fabs)(const T&)) const {\
|
|
/* Look if even dim matrix is of the form : |X 0|\
|
|
|0 Y|*/\
|
|
unsigned int _D = dimension();\
|
|
unsigned int _D_2 = _D/2;\
|
|
if(2*_D_2!=_D) return false;\
|
|
for(unsigned int r=0;r<_D_2;r++) {\
|
|
for(unsigned int c=_D_2;c<_D;c++) {\
|
|
if(a_fabs(value(r,c))>=a_prec) return false;\
|
|
}\
|
|
}\
|
|
for(unsigned int r=_D_2;r<_D;r++) {\
|
|
for(unsigned int c=0;c<_D_2;c++) {\
|
|
if(a_fabs(value(r,c))>=a_prec) return false;\
|
|
}\
|
|
}\
|
|
return true;\
|
|
}\
|
|
\
|
|
template <class PREC>\
|
|
bool is_block_UR_DL(const PREC& a_prec,PREC(*a_fabs)(const T&)) const {\
|
|
/* Look if even dim matrix is of the form : |0 X|\
|
|
|Y 0|*/\
|
|
unsigned int _D = dimension();\
|
|
unsigned int _D_2 = _D/2;\
|
|
if(2*_D_2!=_D) return false;\
|
|
for(unsigned int r=0;r<_D_2;r++) {\
|
|
for(unsigned int c=0;c<_D_2;c++) {\
|
|
if(a_fabs(value(r,c))>=a_prec) return false;\
|
|
}\
|
|
}\
|
|
for(unsigned int r=_D_2;r<_D;r++) {\
|
|
for(unsigned int c=_D_2;c<_D;c++) {\
|
|
if(a_fabs(value(r,c))>=a_prec) return false;\
|
|
}\
|
|
}\
|
|
return true;\
|
|
}\
|
|
\
|
|
template <class PREC>\
|
|
bool is_decomplex(const PREC& a_prec,PREC(*a_fabs)(const T&)) const {\
|
|
/* Look if even dim matrix is of the form : | X Y|\
|
|
|-Y X|*/\
|
|
unsigned int _D = dimension();\
|
|
unsigned int _D_2 = _D/2;\
|
|
if(2*_D_2!=_D) return false;\
|
|
for(unsigned int r=0;r<_D_2;r++) {\
|
|
for(unsigned int c=0;c<_D_2;c++) {\
|
|
if(a_fabs(value(r,c)-value(r+_D_2,c+_D_2))>=a_prec) return false;\
|
|
}\
|
|
}\
|
|
for(unsigned int r=0;r<_D_2;r++) {\
|
|
for(unsigned int c=_D_2;c<_D;c++) {\
|
|
if(a_fabs(value(r,c)+value(r+_D_2,c-_D_2))>=a_prec) return false;\
|
|
}\
|
|
}\
|
|
return true;\
|
|
}\
|
|
\
|
|
template <class PREC>\
|
|
T determinant_prec(unsigned int a_tmp_rs[],unsigned int a_tmp_cs[], /*[rord=dim-1]*/\
|
|
const PREC& a_prec,PREC(*a_fabs)(const T&)) const {\
|
|
unsigned int ord = dimension();\
|
|
if(ord==0) {\
|
|
return zero();\
|
|
} else if(ord==1) {\
|
|
return *m_vec;\
|
|
} else if(ord==2) {\
|
|
T v00 = *m_vec;\
|
|
T v10 = *(m_vec+1);\
|
|
T v01 = *(m_vec+2);\
|
|
T v11 = *(m_vec+3);\
|
|
return (v00 * v11 - v10 * v01);\
|
|
} else if(ord==3) {\
|
|
/* 00 01 02 \
|
|
10 11 12 \
|
|
20 21 22 \
|
|
*/\
|
|
T v00 = *m_vec;\
|
|
T v10 = *(m_vec+1);\
|
|
T v20 = *(m_vec+2);\
|
|
T v01 = *(m_vec+3);\
|
|
T v11 = *(m_vec+4);\
|
|
T v21 = *(m_vec+5);\
|
|
T v02 = *(m_vec+6);\
|
|
T v12 = *(m_vec+7);\
|
|
T v22 = *(m_vec+8);\
|
|
T cof_00 = v11 * v22 - v21 * v12;\
|
|
T cof_10 = v01 * v22 - v21 * v02;\
|
|
T cof_20 = v01 * v12 - v11 * v02;\
|
|
return (v00*cof_00-v10*cof_10+v20*cof_20);\
|
|
}\
|
|
\
|
|
unsigned int rord = ord-1;\
|
|
\
|
|
T v_rc;\
|
|
\
|
|
T det = zero();\
|
|
{for(unsigned int i=0;i<rord;i++) {a_tmp_cs[i] = i+1;}}\
|
|
unsigned int c = 0;\
|
|
\
|
|
{for(unsigned int i=0;i<rord;i++) {a_tmp_rs[i] = i+1;}}\
|
|
bool sg = true; /*c=0+r=0*/\
|
|
for(unsigned int r=0;r<ord;r++) {\
|
|
if(r>=1) a_tmp_rs[r-1] = r-1;\
|
|
v_rc = value(r,c);\
|
|
if(!is_zero(v_rc,a_prec,a_fabs)) {\
|
|
T subdet = sub_determinant_prec(rord,a_tmp_rs,a_tmp_cs,a_prec,a_fabs);\
|
|
if(sg) \
|
|
det += v_rc * subdet;\
|
|
else\
|
|
det -= v_rc * subdet;\
|
|
}\
|
|
sg = sg?false:true;\
|
|
}\
|
|
\
|
|
return det;\
|
|
}\
|
|
\
|
|
T determinant(unsigned int a_tmp_rs[],unsigned int a_tmp_cs[]) const { /*[rord=dim-1]*/ \
|
|
return determinant_prec<double>(a_tmp_rs,a_tmp_cs,0,zero_fabs);\
|
|
}\
|
|
\
|
|
T determinant() const {\
|
|
unsigned int ord = dimension();\
|
|
if(ord==0) {\
|
|
return zero();\
|
|
} else if(ord==1) {\
|
|
return *m_vec;\
|
|
} else if(ord==2) {\
|
|
T v00 = *m_vec;\
|
|
T v10 = *(m_vec+1);\
|
|
T v01 = *(m_vec+2);\
|
|
T v11 = *(m_vec+3);\
|
|
return (v00 * v11 - v10 * v01);\
|
|
} else if(ord==3) {\
|
|
/* 00 01 02 \
|
|
10 11 12 \
|
|
20 21 22 \
|
|
*/\
|
|
T v00 = *m_vec;\
|
|
T v10 = *(m_vec+1);\
|
|
T v20 = *(m_vec+2);\
|
|
T v01 = *(m_vec+3);\
|
|
T v11 = *(m_vec+4);\
|
|
T v21 = *(m_vec+5);\
|
|
T v02 = *(m_vec+6);\
|
|
T v12 = *(m_vec+7);\
|
|
T v22 = *(m_vec+8);\
|
|
T cof_00 = v11 * v22 - v21 * v12;\
|
|
T cof_10 = v01 * v22 - v21 * v02;\
|
|
T cof_20 = v01 * v12 - v11 * v02;\
|
|
return (v00*cof_00-v10*cof_10+v20*cof_20);\
|
|
}\
|
|
unsigned int rord = ord-1;\
|
|
unsigned int* rs = new unsigned int[rord];\
|
|
unsigned int* cs = new unsigned int[rord];\
|
|
T det = determinant(rs,cs);\
|
|
delete [] rs;\
|
|
delete [] cs;\
|
|
return det;\
|
|
}\
|
|
\
|
|
template <class PREC>\
|
|
bool invert_prec(TOOLS_MAT_CLASS& a_res,\
|
|
unsigned int a_tmp_rs[],unsigned int a_tmp_cs[], /*[rord=dim-1]*/\
|
|
const PREC& a_prec,PREC(*a_fabs)(const T&) /*for det=?zero*/ \
|
|
) const { \
|
|
unsigned int ord = dimension();\
|
|
if(ord==0) return true;\
|
|
\
|
|
if(ord==1) {\
|
|
T det = value(0,0);\
|
|
if(is_zero(det,a_prec,a_fabs)) return false;\
|
|
a_res.set_value(0,0,one()/det);\
|
|
return true;\
|
|
} else if(ord==2) {\
|
|
T v00 = *m_vec;\
|
|
T v10 = *(m_vec+1);\
|
|
T v01 = *(m_vec+2);\
|
|
T v11 = *(m_vec+3);\
|
|
T det = (v00 * v11 - v10 * v01);\
|
|
if(is_zero(det,a_prec,a_fabs)) return false;\
|
|
a_res.set_value(0,0,v11/det);\
|
|
a_res.set_value(1,1,v00/det);\
|
|
a_res.set_value(0,1,minus_one()*v01/det);\
|
|
a_res.set_value(1,0,minus_one()*v10/det);\
|
|
return true;\
|
|
} else if(ord==3) {\
|
|
/* 00 01 02 \
|
|
10 11 12 \
|
|
20 21 22 \
|
|
*/\
|
|
T v00 = *m_vec;\
|
|
T v10 = *(m_vec+1);\
|
|
T v20 = *(m_vec+2);\
|
|
T v01 = *(m_vec+3);\
|
|
T v11 = *(m_vec+4);\
|
|
T v21 = *(m_vec+5);\
|
|
T v02 = *(m_vec+6);\
|
|
T v12 = *(m_vec+7);\
|
|
T v22 = *(m_vec+8);\
|
|
T cof_00 = v11 * v22 - v21 * v12;\
|
|
T cof_10 = v01 * v22 - v21 * v02;\
|
|
T cof_20 = v01 * v12 - v11 * v02;\
|
|
T det = (v00*cof_00-v10*cof_10+v20*cof_20);\
|
|
if(is_zero(det,a_prec,a_fabs)) return false;\
|
|
T cof_01 = v10 * v22 - v20 * v12;\
|
|
T cof_11 = v00 * v22 - v20 * v02;\
|
|
T cof_21 = v00 * v12 - v10 * v02;\
|
|
T cof_02 = v10 * v21 - v20 * v11;\
|
|
T cof_12 = v00 * v21 - v20 * v01;\
|
|
T cof_22 = v00 * v11 - v10 * v01;\
|
|
a_res.set_value(0,0,cof_00/det);\
|
|
a_res.set_value(1,0,minus_one()*cof_01/det);\
|
|
a_res.set_value(2,0,cof_02/det);\
|
|
a_res.set_value(0,1,minus_one()*cof_10/det);\
|
|
a_res.set_value(1,1,cof_11/det);\
|
|
a_res.set_value(2,1,minus_one()*cof_12/det);\
|
|
a_res.set_value(0,2,cof_20/det);\
|
|
a_res.set_value(1,2,minus_one()*cof_21/det);\
|
|
a_res.set_value(2,2,cof_22/det);\
|
|
return true;\
|
|
}\
|
|
\
|
|
/*Generic invertion method.*/\
|
|
\
|
|
unsigned int rord = ord-1;\
|
|
\
|
|
/* Get det with r = 0;*/\
|
|
T det = zero();\
|
|
T subdet;\
|
|
{\
|
|
{for(unsigned int i=0;i<rord;i++) {a_tmp_rs[i] = i+1;}}\
|
|
unsigned int r = 0;\
|
|
/*if(r>=1) a_tmp_rs[r-1] = r-1;*/\
|
|
\
|
|
{for(unsigned int i=0;i<rord;i++) {a_tmp_cs[i] = i+1;}}\
|
|
bool sg = true; /*r=0+c=0*/\
|
|
for(unsigned int c=0;c<ord;c++) {\
|
|
if(c>=1) a_tmp_cs[c-1] = c-1;\
|
|
subdet = sub_determinant_prec(rord,a_tmp_rs,a_tmp_cs,a_prec,a_fabs);\
|
|
if(sg) {\
|
|
det += value(r,c) * subdet;\
|
|
a_res.set_value(c,r,subdet);\
|
|
sg = false;\
|
|
} else {\
|
|
det += value(r,c) * subdet * minus_one();\
|
|
a_res.set_value(c,r,subdet * minus_one());\
|
|
sg = true;\
|
|
}\
|
|
}}\
|
|
\
|
|
if(is_zero(det,a_prec,a_fabs)) return false;\
|
|
\
|
|
{for(unsigned int c=0;c<ord;c++) {\
|
|
a_res.set_value(c,0,a_res.value(c,0)/det);\
|
|
}}\
|
|
\
|
|
{for(unsigned int i=0;i<rord;i++) {a_tmp_rs[i] = i+1;}}\
|
|
bool sgr = false; /*r=1+c=0*/\
|
|
for(unsigned int r=1;r<ord;r++) {\
|
|
if(r>=1) a_tmp_rs[r-1] = r-1;\
|
|
{for(unsigned int i=0;i<rord;i++) {a_tmp_cs[i] = i+1;}}\
|
|
bool sg = sgr;\
|
|
for(unsigned int c=0;c<ord;c++) {\
|
|
if(c>=1) a_tmp_cs[c-1] = c-1;\
|
|
subdet = sub_determinant_prec(rord,a_tmp_rs,a_tmp_cs,a_prec,a_fabs);\
|
|
if(sg) {\
|
|
a_res.set_value(c,r,subdet/det);\
|
|
sg = false;\
|
|
} else {\
|
|
a_res.set_value(c,r,(subdet * minus_one())/det);\
|
|
sg = true;\
|
|
}\
|
|
}\
|
|
sgr = sgr?false:true;\
|
|
}\
|
|
\
|
|
return true;\
|
|
}\
|
|
\
|
|
template <class PREC>\
|
|
bool invert_prec(TOOLS_MAT_CLASS& a_res,const PREC& a_prec,PREC(*a_fabs)(const T&)) const {\
|
|
unsigned int ord = dimension();\
|
|
if(ord==0) return true;\
|
|
\
|
|
if(ord==1) {\
|
|
T det = value(0,0);\
|
|
if(is_zero(det,a_prec,a_fabs)) return false;\
|
|
a_res.set_value(0,0,one()/det);\
|
|
return true;\
|
|
} else if(ord==2) {\
|
|
T v00 = *m_vec;\
|
|
T v10 = *(m_vec+1);\
|
|
T v01 = *(m_vec+2);\
|
|
T v11 = *(m_vec+3);\
|
|
T det = (v00 * v11 - v10 * v01);\
|
|
if(is_zero(det,a_prec,a_fabs)) return false;\
|
|
a_res.set_value(0,0,v11/det);\
|
|
a_res.set_value(1,1,v00/det);\
|
|
a_res.set_value(0,1,minus_one()*v01/det);\
|
|
a_res.set_value(1,0,minus_one()*v10/det);\
|
|
return true;\
|
|
} else if(ord==3) {\
|
|
/* 00 01 02 \
|
|
10 11 12 \
|
|
20 21 22 \
|
|
*/\
|
|
T v00 = *m_vec;\
|
|
T v10 = *(m_vec+1);\
|
|
T v20 = *(m_vec+2);\
|
|
T v01 = *(m_vec+3);\
|
|
T v11 = *(m_vec+4);\
|
|
T v21 = *(m_vec+5);\
|
|
T v02 = *(m_vec+6);\
|
|
T v12 = *(m_vec+7);\
|
|
T v22 = *(m_vec+8);\
|
|
T cof_00 = v11 * v22 - v21 * v12;\
|
|
T cof_10 = v01 * v22 - v21 * v02;\
|
|
T cof_20 = v01 * v12 - v11 * v02;\
|
|
T det = (v00*cof_00-v10*cof_10+v20*cof_20);\
|
|
if(is_zero(det,a_prec,a_fabs)) return false;\
|
|
T cof_01 = v10 * v22 - v20 * v12;\
|
|
T cof_11 = v00 * v22 - v20 * v02;\
|
|
T cof_21 = v00 * v12 - v10 * v02;\
|
|
T cof_02 = v10 * v21 - v20 * v11;\
|
|
T cof_12 = v00 * v21 - v20 * v01;\
|
|
T cof_22 = v00 * v11 - v10 * v01;\
|
|
a_res.set_value(0,0,cof_00/det);\
|
|
a_res.set_value(1,0,minus_one()*cof_01/det);\
|
|
a_res.set_value(2,0,cof_02/det);\
|
|
a_res.set_value(0,1,minus_one()*cof_10/det);\
|
|
a_res.set_value(1,1,cof_11/det);\
|
|
a_res.set_value(2,1,minus_one()*cof_12/det);\
|
|
a_res.set_value(0,2,cof_20/det);\
|
|
a_res.set_value(1,2,minus_one()*cof_21/det);\
|
|
a_res.set_value(2,2,cof_22/det);\
|
|
return true;\
|
|
}\
|
|
\
|
|
unsigned int rord = ord-1;\
|
|
unsigned int* cs = new unsigned int[rord];\
|
|
unsigned int* rs = new unsigned int[rord];\
|
|
bool status = invert_prec(a_res,rs,cs,a_prec,a_fabs);\
|
|
delete [] cs;\
|
|
delete [] rs;\
|
|
return status;\
|
|
}\
|
|
\
|
|
bool invert(TOOLS_MAT_CLASS& a_res,unsigned int a_tmp_rs[],unsigned int a_tmp_cs[]) const { /*[rord=dim-1]*/ \
|
|
return invert_prec<double>(a_res,a_tmp_rs,a_tmp_cs,0,zero_fabs);\
|
|
}\
|
|
\
|
|
bool invert(TOOLS_MAT_CLASS& a_res) const {\
|
|
return invert_prec<double>(a_res,0,zero_fabs);\
|
|
}\
|
|
\
|
|
void power(unsigned int a_n,TOOLS_MAT_CLASS& a_res) const {\
|
|
a_res.set_identity();\
|
|
T* _tmp = new T[dim2()];\
|
|
for(unsigned int i=0;i<a_n;i++) {\
|
|
a_res._mul_mtx(m_vec,_tmp);\
|
|
}\
|
|
delete [] _tmp;\
|
|
}\
|
|
\
|
|
void exp(unsigned int a_le,TOOLS_MAT_CLASS& a_res,TOOLS_MAT_CLASS& a_tmp,T a_tmp_2[]) const { /*OPTIMIZATION*/\
|
|
/* result = I + M + M**2/2! + M**3/3! + .... */\
|
|
a_res.set_identity();\
|
|
a_tmp.set_identity();\
|
|
for(unsigned int i=1;i<=a_le;i++) {\
|
|
a_tmp._mul_mtx(m_vec,a_tmp_2);\
|
|
a_tmp.multiply(one()/T(i));\
|
|
a_res += a_tmp;\
|
|
}\
|
|
}\
|
|
\
|
|
void exp(unsigned int a_le,TOOLS_MAT_CLASS& a_res) const {\
|
|
/* result = I + M + M**2/2! + M**3/3! + .... */\
|
|
TOOLS_MAT_CLASS tmp(*this);\
|
|
tmp.set_identity();\
|
|
T* tmp_2 = new T[dim2()];\
|
|
exp(a_le,a_res,tmp,tmp_2);\
|
|
delete [] tmp_2;\
|
|
}\
|
|
\
|
|
void cosh(unsigned int a_le,TOOLS_MAT_CLASS& a_res) const {\
|
|
/* result = I + M**2/2! + M**4/4! + .... */\
|
|
a_res.set_identity();\
|
|
TOOLS_MAT_CLASS M_2(*this);\
|
|
M_2._mul_mtx(m_vec);\
|
|
TOOLS_MAT_CLASS tmp(*this);\
|
|
tmp.set_identity();\
|
|
T* _tmp = new T[dim2()];\
|
|
for(unsigned int i=1;i<=a_le;i+=2) {\
|
|
tmp._mul_mtx(M_2.m_vec,_tmp);\
|
|
tmp.multiply(one()/T(i+0)); \
|
|
tmp.multiply(one()/T(i+1)); \
|
|
a_res += tmp;\
|
|
}\
|
|
delete [] _tmp;\
|
|
}\
|
|
\
|
|
void sinh(unsigned int a_le,TOOLS_MAT_CLASS& a_res) const {\
|
|
/* result = M + M**3/3! + .... */\
|
|
a_res._copy(m_vec);\
|
|
TOOLS_MAT_CLASS M_2(*this);\
|
|
M_2._mul_mtx(m_vec);\
|
|
TOOLS_MAT_CLASS tmp(*this);\
|
|
tmp._copy(m_vec);\
|
|
T* _tmp = new T[dim2()];\
|
|
for(unsigned int i=1;i<=a_le;i+=2) {\
|
|
tmp._mul_mtx(M_2.m_vec,_tmp);\
|
|
tmp.multiply(one()/T(i+1)); \
|
|
tmp.multiply(one()/T(i+2)); \
|
|
a_res += tmp;\
|
|
}\
|
|
delete [] _tmp;\
|
|
}\
|
|
\
|
|
void log(unsigned int a_le,TOOLS_MAT_CLASS& a_res) const {\
|
|
/* result = (M-I) - (M-I)**2/2 + (M-I)**3/3 +... */\
|
|
/* WARNING : touchy, it may not converge ! */\
|
|
a_res.set_zero();\
|
|
\
|
|
TOOLS_MAT_CLASS M_I;\
|
|
M_I.set_identity();\
|
|
M_I.multiply(minus_one());\
|
|
M_I._add_mtx(m_vec);\
|
|
\
|
|
TOOLS_MAT_CLASS M_Ip(M_I);\
|
|
T fact = minus_one();\
|
|
\
|
|
TOOLS_MAT_CLASS tmp;\
|
|
\
|
|
T* _tmp = new T[dim2()];\
|
|
for(unsigned int i=0;i<=a_le;i++) {\
|
|
fact *= minus_one(); \
|
|
tmp = M_Ip;\
|
|
tmp.multiply(fact/T(i+1)); \
|
|
a_res += tmp;\
|
|
M_Ip._mul_mtx(M_I.m_vec,_tmp);\
|
|
}\
|
|
delete [] _tmp;\
|
|
}\
|
|
\
|
|
void omega(unsigned int a_le,TOOLS_MAT_CLASS& a_res,TOOLS_MAT_CLASS& a_tmp,T a_tmp_2[]) const { /*OPTIMIZATION*/\
|
|
/* result = I + M/2! + M**2/3! + M**3/4! + .... */\
|
|
a_res.set_identity();\
|
|
a_tmp.set_identity();\
|
|
for(unsigned int i=1;i<=a_le;i++) {\
|
|
a_tmp._mul_mtx(m_vec,a_tmp_2);\
|
|
a_tmp.multiply(one()/T(i+1));\
|
|
a_res += a_tmp;\
|
|
}\
|
|
}\
|
|
\
|
|
void omega(unsigned int a_le,TOOLS_MAT_CLASS& a_res) const {\
|
|
/* result = I + M/2! + M**2/3! + M**3/4! + .... */\
|
|
TOOLS_MAT_CLASS tmp(*this);\
|
|
tmp.set_identity();\
|
|
T* tmp_2 = new T[dim2()];\
|
|
omega(a_le,a_res,tmp,tmp_2);\
|
|
delete [] tmp_2;\
|
|
}\
|
|
\
|
|
template <class MAT>\
|
|
bool copy(const MAT& a_from) {\
|
|
/*for exa from a double matrix to a symbol matrix*/\
|
|
unsigned int _D = dimension();\
|
|
if(a_from.dimension()!=_D) return false;\
|
|
for(unsigned int r=0;r<_D;r++) {\
|
|
for(unsigned int c=0;c<_D;c++) {\
|
|
set_value(r,c,a_from.value(r,c));\
|
|
}\
|
|
}\
|
|
return true;\
|
|
}\
|
|
public: /*operators*/\
|
|
T operator()(unsigned int a_r,unsigned int a_c) const {\
|
|
/*WARNING : no check on a_r,a_c.*/\
|
|
return m_vec[a_r + a_c * dimension()];\
|
|
}\
|
|
\
|
|
T& operator[](size_t a_index) { /*for inlib/sg/sf_vec*/\
|
|
/*WARNING : no check on a_index.*/\
|
|
return m_vec[a_index];\
|
|
}\
|
|
const T& operator[](size_t a_index) const {\
|
|
/*WARNING : no check on a_index.*/\
|
|
return m_vec[a_index];\
|
|
}\
|
|
bool operator==(const TOOLS_MAT_CLASS& a_array) const {\
|
|
return equal(a_array);\
|
|
}\
|
|
bool operator!=(const TOOLS_MAT_CLASS& a_array) const {\
|
|
return !operator==(a_array);\
|
|
}\
|
|
TOOLS_MAT_CLASS& operator*=(const TOOLS_MAT_CLASS& a_m) {\
|
|
_mul_mtx(a_m.m_vec);\
|
|
return *this;\
|
|
}\
|
|
TOOLS_MAT_CLASS& operator+=(const TOOLS_MAT_CLASS& a_m) {\
|
|
_add_mtx(a_m.m_vec);\
|
|
return *this;\
|
|
}\
|
|
TOOLS_MAT_CLASS& operator-=(const TOOLS_MAT_CLASS& a_m) {\
|
|
_sub_mtx(a_m.m_vec);\
|
|
return *this;\
|
|
}\
|
|
TOOLS_MAT_CLASS& operator*=(const T& a_fac) {\
|
|
for(unsigned int i=0;i<dim2();i++) m_vec[i] *= a_fac;\
|
|
return *this;\
|
|
}\
|
|
\
|
|
TOOLS_MAT_CLASS operator*(const T& a_fac) {\
|
|
TOOLS_MAT_CLASS res;\
|
|
res.operator*=(a_fac);\
|
|
return res;\
|
|
}\
|
|
protected:\
|
|
void _copy(const T a_m[]) {\
|
|
T* tp = (T*)m_vec;T* ap = (T*)a_m;\
|
|
for(unsigned int i=0;i<dim2();i++,tp++,ap++) *tp = *ap;\
|
|
/*{for(unsigned int i=0;i<dim2();i++) m_vec[i] = a_m[i];}*/\
|
|
/* memcpy does not work with std::complex<> and mat<symbol,4> see inlib/tests/symbolic.cpp */\
|
|
/*vec_copy(m_vec,a_m,dim2());*/\
|
|
}\
|
|
\
|
|
void _add_mtx(const T a_m[]) { /* this = this + a_m, */\
|
|
T* tp = (T*)m_vec;T* ap = (T*)a_m;\
|
|
for(unsigned int i=0;i<dim2();i++,tp++,ap++) *tp += *ap;\
|
|
/*for(unsigned int i=0;i<dim2();i++) m_vec[i] += a_m[i];*/\
|
|
/*vec_add(m_vec,a_m,m_vec,dim2());*/\
|
|
}\
|
|
void _sub_mtx(const T a_m[]) { /* this = this - a_m, */\
|
|
T* tp = (T*)m_vec;T* ap = (T*)a_m;\
|
|
for(unsigned int i=0;i<dim2();i++,tp++,ap++) *tp -= *ap;\
|
|
/*for(unsigned int i=0;i<dim2();i++) m_vec[i] -= a_m[i];*/\
|
|
/*vec_sub(m_vec,a_m,m_vec,dim2());*/\
|
|
}\
|
|
\
|
|
/*\
|
|
void _mul_mtx(const T a_m[],T a_tmp[]) {\
|
|
unsigned int ord = dimension();\
|
|
for(unsigned int r=0;r<ord;r++) {\
|
|
for(unsigned int c=0;c<ord;c++) {\
|
|
T _value = zero();\
|
|
for(unsigned int i=0;i<ord;i++) {\
|
|
_value += (*(m_vec+r+i*ord)) * (*(a_m+i+c*ord)); //optimize.\
|
|
}\
|
|
*(a_tmp+r+c*ord) = _value;\
|
|
}\
|
|
}\
|
|
_copy(a_tmp);\
|
|
}\
|
|
*/\
|
|
void _mul_mtx(const T a_m[],T a_tmp[]) { /*OPTIMIZATION*/\
|
|
/* this = this * a_m */\
|
|
typedef T* Tp;\
|
|
Tp tpos,ttpos,rpos,apos,mpos,aapos;\
|
|
T _value;\
|
|
unsigned int r,c,i;\
|
|
\
|
|
unsigned int _D = dimension();\
|
|
\
|
|
tpos = a_tmp;\
|
|
for(r=0;r<_D;r++,tpos++) {\
|
|
ttpos = tpos;\
|
|
rpos = m_vec+r;\
|
|
apos = (T*)a_m;\
|
|
for(c=0;c<_D;c++,ttpos+=_D,apos+=_D) {\
|
|
_value = zero();\
|
|
mpos = rpos;\
|
|
aapos = apos;\
|
|
for(i=0;i<_D;i++,mpos+=_D,aapos++) _value += (*mpos) * (*aapos);\
|
|
*ttpos = _value;\
|
|
}\
|
|
}\
|
|
_copy(a_tmp);\
|
|
}\
|
|
\
|
|
void _mul_mtx(const T a_m[]) {\
|
|
T* _tmp = new T[dim2()];\
|
|
_mul_mtx(a_m,_tmp);\
|
|
delete [] _tmp;\
|
|
}\
|
|
\
|
|
void _left_mul_mtx(const T a_m[]) {\
|
|
/* this = a_m * this */\
|
|
unsigned int _D = dimension();\
|
|
T* _tmp = new T[dim2()];\
|
|
for(unsigned int r=0;r<_D;r++) {\
|
|
for(unsigned int c=0;c<_D;c++) {\
|
|
T _value = zero();\
|
|
for(unsigned int i=0;i<_D;i++) {\
|
|
_value += (*(a_m+r+i*_D)) * (*(m_vec+i+c*_D)); /*optimize.*/\
|
|
}\
|
|
*(_tmp+r+c*_D) = _value;\
|
|
}\
|
|
}\
|
|
_copy(_tmp);\
|
|
delete [] _tmp;\
|
|
}\
|
|
\
|
|
template <class PREC>\
|
|
T sub_determinant_prec(unsigned int a_ord,unsigned int aRs[],unsigned int aCs[],\
|
|
const PREC& a_prec,PREC(*a_fabs)(const T&)) const {\
|
|
/*WARNING : to optimize, we do not check the content of aRs, aCs.*/\
|
|
unsigned int ord = a_ord;\
|
|
if(ord==0) return zero();\
|
|
else if(ord==1) return value(aRs[0],aCs[0]);\
|
|
else if(ord==2) {\
|
|
/*return (value(aRs[0],aCs[0]) * value(aRs[1],aCs[1]) -\
|
|
value(aRs[1],aCs[0]) * value(aRs[0],aCs[1])); \
|
|
Optimize the upper :*/\
|
|
\
|
|
unsigned int _ord = dimension();\
|
|
\
|
|
return ( (*(m_vec+aRs[0]+aCs[0]*_ord)) * (*(m_vec+aRs[1]+aCs[1]*_ord)) -\
|
|
(*(m_vec+aRs[1]+aCs[0]*_ord)) * (*(m_vec+aRs[0]+aCs[1]*_ord)) );\
|
|
\
|
|
} else if(ord==3) {\
|
|
/* 00 01 02 \
|
|
10 11 12 \
|
|
20 21 22 \
|
|
*/\
|
|
unsigned int _ord = dimension();\
|
|
\
|
|
T v00 = *(m_vec+aRs[0]+aCs[0]*_ord);\
|
|
T v10 = *(m_vec+aRs[1]+aCs[0]*_ord);\
|
|
T v20 = *(m_vec+aRs[2]+aCs[0]*_ord);\
|
|
T v01 = *(m_vec+aRs[0]+aCs[1]*_ord);\
|
|
T v11 = *(m_vec+aRs[1]+aCs[1]*_ord);\
|
|
T v21 = *(m_vec+aRs[2]+aCs[1]*_ord);\
|
|
T v02 = *(m_vec+aRs[0]+aCs[2]*_ord);\
|
|
T v12 = *(m_vec+aRs[1]+aCs[2]*_ord);\
|
|
T v22 = *(m_vec+aRs[2]+aCs[2]*_ord);\
|
|
T cof_00 = v11 * v22 - v21 * v12;\
|
|
T cof_10 = v01 * v22 - v21 * v02;\
|
|
T cof_20 = v01 * v12 - v11 * v02;\
|
|
return (v00*cof_00-v10*cof_10+v20*cof_20);\
|
|
}\
|
|
\
|
|
unsigned int rord = ord-1;\
|
|
unsigned int* cs = new unsigned int[rord];\
|
|
unsigned int* rs = new unsigned int[rord];\
|
|
\
|
|
T v_rc;\
|
|
\
|
|
T det = zero();\
|
|
{for(unsigned int i=0;i<rord;i++) {cs[i] = aCs[i+1];}}\
|
|
unsigned int c = 0;\
|
|
/*if(c>=1) cs[c-1] = c-1;*/\
|
|
\
|
|
{for(unsigned int i=0;i<rord;i++) {rs[i] = aRs[i+1];}}\
|
|
bool sg = true; /*c=0+r=0*/\
|
|
for(unsigned int r=0;r<ord;r++) {\
|
|
if(r>=1) rs[r-1] = aRs[r-1];\
|
|
v_rc = value(aRs[r],aCs[c]);\
|
|
if(!is_zero(v_rc,a_prec,a_fabs)) {\
|
|
T subdet = sub_determinant_prec(rord,rs,cs,a_prec,a_fabs);\
|
|
if(sg)\
|
|
det += v_rc * subdet;\
|
|
else\
|
|
det -= v_rc * subdet;\
|
|
}\
|
|
sg = sg?false:true;\
|
|
}\
|
|
\
|
|
delete [] cs;\
|
|
delete [] rs;\
|
|
\
|
|
return det;\
|
|
}\
|
|
\
|
|
static double zero_fabs(const T& a_number) {return a_number==zero()?0:1000000;}
|
|
|
|
#endif
|