lars/geant4
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Import Geant4 10.2.0 source tree

Browse Source
This commit is contained in:
Gabriele Cosmo
2016-06-10 14:11:04 +02:00
parent c9b32a6c0a
commit d4af681f38
4886 changed files with 420149 additions and 1023309 deletions
Download Patch File Download Diff File Expand all files Collapse all files
source/analysis/g4tools/include/tools/lina/MATCOM
+657
View File
@@ -0,0 +1,657 @@
// 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;
}
*/
// 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();\
}\
public:\
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* res = new T[dimension()];\
bool status = mul_vec(a_vec,res);\
delete [] res;\
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;\
}\
\
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 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;\
}\
\
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;\
}\
\
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;\
}\
\
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());\
}\
\
T determinant(unsigned int a_tmp_rs[],unsigned int a_tmp_cs[]) const { /*[rord=dim-1]*/ \
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 v01 = *(m_vec+ord);\
T v10 = *(m_vec+1);\
T v11 = *(m_vec+1+ord);\
return (v00 * v11 - v10 * v01);\
} else if(ord==3) {\
/* 00 01 02 \
10 11 12 \
20 21 22 \
*/\
T v01 = *(m_vec+ord);\
T v02 = *(m_vec+2*ord);\
T v11 = *(m_vec+1+ord);\
T v12 = *(m_vec+1+2*ord);\
T v21 = *(m_vec+2+ord);\
T v22 = *(m_vec+2+2*ord);\
T cof_00 = v11 * v22 - v21 * v12;\
T cof_10 = v01 * v22 - v21 * v02;\
T cof_20 = v01 * v12 - v11 * v02;\
T v00 = *m_vec;\
T v10 = *(m_vec+1);\
T v20 = *(m_vec+2);\
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(v_rc!=zero()) {\
T subdet = sub_determinant(rord,a_tmp_rs,a_tmp_cs);\
if(sg) \
det += v_rc * subdet;\
else\
det -= v_rc * subdet;\
}\
sg = sg?false:true;\
}\
\
return det;\
}\
\
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 v01 = *(m_vec+ord);\
T v10 = *(m_vec+1);\
T v11 = *(m_vec+1+ord);\
return (v00 * v11 - v10 * v01);\
} else if(ord==3) {\
T v01 = *(m_vec+ord);\
T v02 = *(m_vec+2*ord);\
T v11 = *(m_vec+1+ord);\
T v12 = *(m_vec+1+2*ord);\
T v21 = *(m_vec+2+ord);\
T v22 = *(m_vec+2+2*ord);\
T cof_00 = v11 * v22 - v21 * v12;\
T cof_10 = v01 * v22 - v21 * v02;\
T cof_20 = v01 * v12 - v11 * v02;\
T v00 = *m_vec;\
T v10 = *(m_vec+1);\
T v20 = *(m_vec+2);\
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;\
}\
\
bool invert(TOOLS_MAT_CLASS& a_res) const {\
/*Generic invertion method.*/\
unsigned int ord = dimension();\
if(ord==0) return true;\
\
if(ord==1) {\
T v = value(0,0);\
if(v==zero()) return false;\
a_res.set_value(0,0,one()/v);\
return true;\
}\
\
unsigned int rord = ord-1;\
unsigned int* cs = new unsigned int[rord];\
unsigned int* rs = new unsigned int[rord];\
\
/* Get det with r = 0;*/\
T det = zero();\
{\
{for(unsigned int i=0;i<rord;i++) {rs[i] = i+1;}}\
unsigned int r = 0;\
/*if(r>=1) rs[r-1] = r-1;*/\
\
{for(unsigned int i=0;i<rord;i++) {cs[i] = i+1;}}\
bool sg = true; /*r=0+c=0*/\
for(unsigned int c=0;c<ord;c++) {\
if(c>=1) cs[c-1] = c-1;\
T subdet = sub_determinant(rord,rs,cs);\
T sgn = sg ? one() : minus_one();\
det += value(r,c) * subdet * sgn;\
T _value = subdet * sgn;\
a_res.set_value(c,r,_value);\
sg = sg?false:true;\
}}\
\
if(det==zero()) {\
delete [] cs;\
delete [] rs;\
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++) {rs[i] = i+1;}}\
bool sgr = false; /*r=1+c=0*/\
for(unsigned int r=1;r<ord;r++) {\
if(r>=1) rs[r-1] = r-1;\
{for(unsigned int i=0;i<rord;i++) {cs[i] = i+1;}}\
bool sg = sgr;\
for(unsigned int c=0;c<ord;c++) {\
if(c>=1) cs[c-1] = c-1;\
T subdet = sub_determinant(rord,rs,cs);\
T sgn = sg ? one() : minus_one();\
T _value = (subdet * sgn)/det;\
a_res.set_value(c,r,_value);\
sg = sg?false:true;\
}\
sgr = sgr?false:true;\
}\
\
delete [] cs;\
delete [] rs;\
\
return true;\
}\
\
void power(unsigned int a_n,TOOLS_MAT_CLASS& a_res) const {\
a_res.set_identity();\
for(unsigned int i=0;i<a_n;i++) {\
a_res._mul_mtx(m_vec);\
}\
}\
\
void exp(unsigned int a_le,TOOLS_MAT_CLASS& a_res) const {\
/* result = I + M + M**2/2! + M**3/3! + .... */\
a_res.set_identity();\
TOOLS_MAT_CLASS tmp(*this);\
tmp.set_identity();\
for(unsigned int i=1;i<=a_le;i++) {\
tmp._mul_mtx(m_vec);\
tmp.multiply(one()/T(i)); \
a_res += 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 = -1;\
\
TOOLS_MAT_CLASS tmp;\
\
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);\
}\
}\
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[](unsigned int a_index) { /*for inlib/sg/sf_vec*/\
/*WARNING : no check on a_index.*/\
return m_vec[a_index];\
}\
const T& operator[](unsigned int 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* res = new T[dim2()];\
_mul_mtx(a_m,res);\
delete [] res;\
}\
\
void _left_mul_mtx(const T a_m[]) {\
/* this = a_m * this */\
unsigned int _D = dimension();\
T* res = 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.*/\
}\
*(res+r+c*_D) = _value;\
}\
}\
_copy(res);\
delete [] res;\
}\
\
T sub_determinant(unsigned int a_ord,unsigned int aRs[],unsigned int aCs[]) 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 r_0 = aRs[0];\
unsigned int r_1 = aRs[1];\
unsigned int c_0 = aCs[0];\
unsigned int c_1 = aCs[1];\
\
unsigned int _ord = dimension();\
const T* p = m_vec;\
\
return ( (*(p+r_0+c_0*_ord)) * (*(p+r_1+c_1*_ord)) -\
(*(p+r_1+c_0*_ord)) * (*(p+r_0+c_1*_ord)) );\
}\
\
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(v_rc!=zero()) {\
T subdet = sub_determinant(rord,rs,cs);\
if(sg)\
det += v_rc * subdet;\
else\
det -= v_rc * subdet;\
}\
sg = sg?false:true;\
}\
\
delete [] cs;\
delete [] rs;\
\
return det;\
}
#endif
source/analysis/g4tools/include/tools/lina/box3
+145
View File
@@ -0,0 +1,145 @@
// Copyright (C) 2010, Guy Barrand. All rights reserved.
// See the file tools.license for terms.
#ifndef tools_box3
#define tools_box3
#include "../mnmx"
//#include <limits>
#include <ostream>
namespace tools {
template <class VEC3>
class box3 {
protected:
typedef typename VEC3::elem_t T_t;
//static T_t num_max() {return std::numeric_limits<T_t>::max();} //max is a forever pain on Windows.
protected:
box3(){
//make_empty();
}
public:
virtual ~box3() {}
public:
box3(const box3& a_from)
:m_min(a_from.m_min)
,m_max(a_from.m_max)
{}
box3& operator=(const box3& a_from){
m_min = a_from.m_min;
m_max = a_from.m_max;
return *this;
}
public:
bool center(VEC3& a_center) const {
if(is_empty()) {a_center.set_value(0,0,0);return false;} //??
a_center.set_value((m_max[0] + m_min[0])/T_t(2),
(m_max[1] + m_min[1])/T_t(2),
(m_max[2] + m_min[2])/T_t(2));
return true;
}
bool set_bounds(const VEC3& a_mn,const VEC3& a_mx){
if( a_mn[0]>a_mx[0] || a_mn[1]>a_mx[1] || a_mn[2]>a_mx[2]) return false;
m_min = a_mn;
m_max = a_mx;
return true;
}
bool set_bounds(T_t a_mn_x,T_t a_mn_y,T_t a_mn_z,
T_t a_mx_x,T_t a_mx_y,T_t a_mx_z){
if( a_mn_x>a_mx_x || a_mn_y>a_mx_y || a_mn_z>a_mx_z ) return false;
m_min.set_value(a_mn_x,a_mn_y,a_mn_z);
m_max.set_value(a_mx_x,a_mx_y,a_mx_z);
return true;
}
bool get_size(T_t& a_dx,T_t& a_dy,T_t& a_dz) const {
if(is_empty()) {a_dx = 0;a_dy = 0;a_dz = 0;return false;}
a_dx = m_max[0] - m_min[0];
a_dy = m_max[1] - m_min[1];
a_dz = m_max[2] - m_min[2];
return true;
}
bool is_empty() const {return m_max[0] < m_min[0];}
const VEC3& mn() const {return m_min;}
const VEC3& mx() const {return m_max;}
bool back(VEC3& a_min,VEC3& a_min_y,VEC3& a_min_xy,VEC3& a_min_x) const {
T_t dx,dy,dz;
if(!get_size(dx,dy,dz)) return false; //WARNING : a_vecs not touched.
// back (from m_min, clockwise order looking toward +z) :
a_min = m_min;
a_min_y.set_value (m_min.x() ,m_min.y()+dy,m_min.z());
a_min_xy.set_value(m_min.x()+dx,m_min.y()+dy,m_min.z());
a_min_x.set_value (m_min.x()+dx,m_min.y() ,m_min.z());
return true;
}
bool front(VEC3& a_max,VEC3& a_max_x,VEC3& a_max_xy,VEC3& a_max_y) const {
T_t dx,dy,dz;
if(!get_size(dx,dy,dz)) return false; //WARNING : a_vecs not touched.
// front (from m_max, clockwise order looking toward -z) :
a_max = m_max;
a_max_x.set_value (m_max.x()-dx,m_max.y() ,m_max.z());
a_max_xy.set_value(m_max.x()-dx,m_max.y()-dy,m_max.z());
a_max_y.set_value (m_max.x() ,m_max.y()-dy,m_max.z());
return true;
}
void extend_by(const VEC3& a_point) {
// Extend the boundaries of the box by the given point, i.e. make the
// point fit inside the box if it isn't already so.
if(is_empty()) {
set_bounds(a_point,a_point);
} else {
m_min.set_value(tools::mn<T_t>(a_point[0],m_min[0]),
tools::mn<T_t>(a_point[1],m_min[1]),
tools::mn<T_t>(a_point[2],m_min[2]));
m_max.set_value(tools::mx<T_t>(a_point[0],m_max[0]),
tools::mx<T_t>(a_point[1],m_max[1]),
tools::mx<T_t>(a_point[2],m_max[2]));
}
}
void extend_by(T_t a_x,T_t a_y,T_t a_z) {
// Extend the boundaries of the box by the given point, i.e. make the
// point fit inside the box if it isn't already so.
if(is_empty()) {
set_bounds(a_x,a_y,a_z,a_x,a_y,a_z);
} else {
m_min.set_value(tools::mn<T_t>(a_x,m_min[0]),
tools::mn<T_t>(a_y,m_min[1]),
tools::mn<T_t>(a_z,m_min[2]));
m_max.set_value(tools::mx<T_t>(a_x,m_max[0]),
tools::mx<T_t>(a_y,m_max[1]),
tools::mx<T_t>(a_z,m_max[2]));
}
}
//NOTE : print is a Python keyword.
void dump(std::ostream& a_out) {
T_t dx,dy,dz;
if(!get_size(dx,dy,dz)) {
a_out << "box is empty." << std::endl;
} else {
a_out << " size " << dx << " " << dy << " " << dz << std::endl;
}
a_out << " min " << m_min[0] << " " << m_min[1] << " " << m_min[2] << std::endl;
a_out << " max " << m_max[0] << " " << m_max[1] << " " << m_max[2] << std::endl;
VEC3 c;
center(c);
a_out << " center " << c[0] << " " << c[1] << " " << c[2] << std::endl;
}
protected:
VEC3 m_min;
VEC3 m_max;
};
}
#endif
source/analysis/g4tools/include/tools/lina/box3f
+34
View File
@@ -0,0 +1,34 @@
// Copyright (C) 2010, Guy Barrand. All rights reserved.
// See the file tools.license for terms.
#ifndef tools_box3f
#define tools_box3f
#include "box3"
#include "vec3f"
#include <cfloat> //FLT_MAX
namespace tools {
class box3f : public box3<vec3f> {
typedef box3<vec3f> parent;
static float num_max() {return FLT_MAX;}
public:
box3f():parent(){make_empty();}
virtual ~box3f() {}
public:
box3f(const box3f& a_from):parent(a_from){}
box3f& operator=(const box3f& a_from){
parent::operator=(a_from);
return *this;
}
public:
void make_empty(){
m_min.set_value( num_max(), num_max(), num_max());
m_max.set_value(-num_max(), -num_max(), -num_max());
}
};
}
#endif
source/analysis/g4tools/include/tools/lina/box_3f
+51
View File
@@ -0,0 +1,51 @@
// Copyright (C) 2010, Guy Barrand. All rights reserved.
// See the file tools.license for terms.
#ifndef tools_box_3f
#define tools_box_3f
#include "../mnmx"
#include <cfloat> // FLT_MAX
namespace tools {
// optimization (used in exlib/sg/text_hershey) :
inline void box_3f_make_empty(float& a_mn_x,float& a_mn_y,float& a_mn_z,
float& a_mx_x,float& a_mx_y,float& a_mx_z){
a_mn_x = FLT_MAX;
a_mn_y = FLT_MAX;
a_mn_z = FLT_MAX;
a_mx_x = -FLT_MAX;
a_mx_y = -FLT_MAX;
a_mx_z = -FLT_MAX;
}
inline void box_3f_extend_by(float& a_mn_x,float& a_mn_y,float& a_mn_z,
float& a_mx_x,float& a_mx_y,float& a_mx_z,
float a_x,float a_y,float a_z){
if(a_mx_x<a_mn_x){ //is empty.
a_mn_x = a_x;
a_mn_y = a_y;
a_mn_z = a_z;
a_mx_x = a_x;
a_mx_y = a_y;
a_mx_z = a_z;
} else {
a_mn_x = mn<float>(a_x,a_mn_x);
a_mn_y = mn<float>(a_y,a_mn_y);
a_mn_z = mn<float>(a_z,a_mn_z);
a_mx_x = mx<float>(a_x,a_mx_x);
a_mx_y = mx<float>(a_y,a_mx_y);
a_mx_z = mx<float>(a_z,a_mx_z);
}
}
inline bool box_3f_is_empty(float a_mn_x,float a_mx_x) {
return a_mx_x < a_mn_x;
}
}
#endif
source/analysis/g4tools/include/tools/lina/geom3
+191
View File
@@ -0,0 +1,191 @@
// Copyright (C) 2010, Guy Barrand. All rights reserved.
// See the file tools.license for terms.
#ifndef tools_geom3
#define tools_geom3
#include "line"
#include "plane"
namespace tools {
template <class T>
class cubic {
public:
cubic(const vec3<T>& a_p0,const vec3<T>& a_v0,
const vec3<T>& a_p1,const vec3<T>& a_v1) {
// Construct a cubic given 2 points and their tangents.
initialize(a_p0.x(),a_p0.y(),a_p0.z(),
a_v0.x(),a_v0.y(),a_v0.z(),
a_p1.x(),a_p1.y(),a_p1.z(),
a_v1.x(),a_v1.y(),a_v1.z());
}
cubic(const T& a_p0_x,const T& a_p0_y,const T& a_p0_z,
const T& a_v0_x,const T& a_v0_y,const T& a_v0_z,
const T& a_p1_x,const T& a_p1_y,const T& a_p1_z,
const T& a_v1_x,const T& a_v1_y,const T& a_v1_z){
initialize(a_p0_x,a_p0_y,a_p0_z,
a_v0_x,a_v0_y,a_v0_z,
a_p1_x,a_p1_y,a_p1_z,
a_v1_x,a_v1_y,a_v1_z);
}
virtual ~cubic() {}
public:
cubic(const cubic& a_from)
:m_a(a_from.m_a)
,m_b(a_from.m_b)
,m_c(a_from.m_c)
,m_d(a_from.m_d)
{}
cubic& operator=(const cubic& a_from) {
m_a = a_from.m_a;
m_b = a_from.m_b;
m_c = a_from.m_c;
m_d = a_from.m_d;
return *this;
}
public:
void get_point(unsigned int a_index,unsigned int a_num,vec3<T>& a_p){
//a_index = 0 is a_p0
//a_index = a_num-1 is a_p1
T s = T(a_index)/T(a_num-1);
T s2 = s*s;
T s3 = s2*s;
a_p = m_a*s3 + m_b*s2 + m_c*s + m_d;
}
void get_point(unsigned int a_index,unsigned int a_num,T& a_x,T& a_y,T& a_z){
//a_index = 0 is a_p0
//a_index = a_num-1 is a_p1
T s = T(a_index)/T(a_num-1);
T s2 = s*s;
T s3 = s2*s;
a_x = m_a.x()*s3 + m_b.x()*s2 + m_c.x()*s + m_d.x();
a_y = m_a.y()*s3 + m_b.y()*s2 + m_c.y()*s + m_d.y();
a_z = m_a.z()*s3 + m_b.z()*s2 + m_c.z()*s + m_d.z();
}
protected:
void initialize(const T& a_p0_x,const T& a_p0_y,const T& a_p0_z,
const T& a_v0_x,const T& a_v0_y,const T& a_v0_z,
const T& a_p1_x,const T& a_p1_y,const T& a_p1_z,
const T& a_v1_x,const T& a_v1_y,const T& a_v1_z){
// Construct a cubic given 2 points and their tangents.
// f(s) = a s**3 + b s**2 + c s + d
// f'(s) = 3 a s**2 + 2 b s + c
// f(0) = d = p0
// f'(0) = c = v0
// f(1) = a + b + v0 + p0 = p1
// f'(1) = 3 a + 2 b + v0 = v1
// f(1) = a + b = p1 - v0 - p0
// f'(1) = 3 a + 2 b = v1 - v0
// b = 3(p1-v0-p0)-(v1-v0)
// a = p1-v0-p0 - b = p1-v0-p0-3(p1-v0-p0)+(v1-v0)
// a = -2p1 + v0 + 2p0 + v1
T a_x = -2*a_p1_x + a_v0_x + 2*a_p0_x + a_v1_x;
T a_y = -2*a_p1_y + a_v0_y + 2*a_p0_y + a_v1_y;
T a_z = -2*a_p1_z + a_v0_z + 2*a_p0_z + a_v1_z;
m_a.set_value(a_x,a_y,a_z);
T b_x = 3*(a_p1_x - a_v0_x - a_p0_x) - (a_v1_x - a_v0_x);
T b_y = 3*(a_p1_y - a_v0_y - a_p0_y) - (a_v1_y - a_v0_y);
T b_z = 3*(a_p1_z - a_v0_z - a_p0_z) - (a_v1_z - a_v0_z);
m_b.set_value(b_x,b_y,b_z);
m_c.set_value(a_v0_x,a_v0_y,a_v0_z);
m_d.set_value(a_p0_x,a_p0_y,a_p0_z);
}
protected:
vec3<T> m_a;
vec3<T> m_b;
vec3<T> m_c;
vec3<T> m_d;
};
}
#include <vector>
namespace tools {
// not tested yet.
template <class T>
class clip {
public:
clip():m_cur(0){}
virtual ~clip() {}
private:
clip(const clip&):m_cur(0){}
clip& operator=(const clip&){return *this;}
public:
void reset() {
m_data[0].clear();
m_data[1].clear();
m_cur = 0;
}
void add(const vec3<T>& a_point) {m_data[m_cur].push_back(a_point);}
void execute(const plane<T>& plane) {
//Clip polygon against plane. This might change the number of
//vertices in the polygon.
unsigned int n = m_data[m_cur].size();
if (n == 0) return;
// create a loop :
vec3<T> dummy = m_data[m_cur][0];
m_data[m_cur].push_back(dummy);
const vec3<T>& planeN = plane.normal();
for(unsigned int i = 0; i < n; i++) {
vec3<T> v0 = m_data[m_cur][i];
vec3<T> v1 = m_data[m_cur][i+1];
T d0 = plane.distance(v0);
T d1 = plane.distance(v1);
if (d0 >= 0.0f && d1 < 0.0f) { // exit plane
vec3<T> dir = v1-v0;
// we know that v0 != v1 since we got here
dir.normalize();
T dot = dir.dot(planeN);
vec3<T> newvertex = v0 - dir * (d0/dot);
out_point(newvertex);
} else if (d0 < 0.0f && d1 >= 0.0f) { // enter plane
vec3<T> dir = v1-v0;
// we know that v0 != v1 since we got here
dir.normalize();
T dot = dir.dot(planeN);
vec3<T> newvertex = v0 - dir * (d0/dot);
out_point(newvertex);
out_point(v1);
} else if (d0 >= 0.0f && d1 >= 0.0f) { // in plane
out_point(v1);
}
}
m_data[m_cur].clear();
m_cur ^= 1;
}
const std::vector< vec3<T> >& result() const {return m_data[m_cur];}
protected:
void out_point(const vec3<T>& a_p) {m_data[m_cur ^ 1].push_back(a_p);}
protected:
std::vector< vec3<T> > m_data[2];
unsigned int m_cur;
};
}
#endif
source/analysis/g4tools/include/tools/lina/line
+135
View File
@@ -0,0 +1,135 @@
// Copyright (C) 2010, Guy Barrand. All rights reserved.
// See the file tools.license for terms.
#ifndef tools_line
#define tools_line
#include "vec3"
namespace tools {
// Parametric description:
// l(t) = pos + t * dir
template <class T>
class line {
public:
line(){}
line(const vec3<T>& a_p0,const vec3<T>& a_p1) {
// Construct a line from two points lying on the line. If you
// want to construct a line from a position and a direction, use
// line(p, p + d).
// line is directed from p0 to p1.
m_pos = a_p0;
//m_dir = a_p1-a_p0;
m_dir = a_p0;
m_dir.multiply(-1);
m_dir.add(a_p1);
m_dir.normalize();
}
line(const T& a_0_x,const T& a_0_y,const T& a_0_z,
const T& a_1_x,const T& a_1_y,const T& a_1_z) {
m_pos.set_value(a_0_x,a_0_y,a_0_z);
m_dir.set_value(a_1_x-a_0_x,a_1_y-a_0_y,a_1_z-a_0_z);
m_dir.normalize();
}
virtual ~line() {}
public:
line(const line& a_from)
:m_pos(a_from.m_pos)
,m_dir(a_from.m_dir)
{}
line& operator=(const line& a_from) {
m_pos = a_from.m_pos;
m_dir = a_from.m_dir;
return *this;
}
public:
void set_value(const vec3<T>& a_p0,const vec3<T>& a_p1) {
m_pos = a_p0;
m_dir = a_p0;
m_dir.multiply(-1);
m_dir.add(a_p1);
m_dir.normalize();
}
void set_value(const T& a_0_x,const T& a_0_y,const T& a_0_z,
const T& a_1_x,const T& a_1_y,const T& a_1_z) {
m_pos.set_value(a_0_x,a_0_y,a_0_z);
m_dir.set_value(a_1_x-a_0_x,a_1_y-a_0_y,a_1_z-a_0_z);
m_dir.normalize();
}
const vec3<T>& position() const {return m_pos;}
const vec3<T>& direction() const {return m_dir;}
/* not tested :
vec3<T> closest_point(const vec3<T>& a_point) const {
//from coin3d/SbLine.cpp.
//
// a_out
// m_pos x-----x-------> m_dir
// \ |
// \ |
// \ |
// \ |
// \|
// x a_point
return m_pos + m_dir * ((a_point - m_pos).dot(m_dir));
}
bool closest_points(const line<T>& a_line,
vec3<T>& a_on_this,vec3<T>& a_on_line) const {
//from coin3d/SbLine.cpp.
//WARNING : if ret false, a_on_this, a_on_line not set.
// Check if the lines are parallel.
// FIXME: should probably use equals() here.
if(a_line.m_dir == m_dir) return false;
if(a_line.m_dir == T(-1)*m_dir) return false;
vec3<T> P0 = m_pos;
vec3<T> P1 = a_line.m_pos;
vec3<T> D0 = m_dir;
vec3<T> D1 = a_line.m_dir;
vec3<T> D0N = D0;
T c[3], d[3];
for(unsigned int i=0;i<3;i++) {
d[i] =
D1[i] - D0N[i]*(D0[0]*D1[0] + D0[1]*D1[1] + D0[2]*D1[2]);
c[i] =
P1[i] - P0[i] + D0N[i]*(D0[0]*P0[0] + D0[1]*P0[1] + D0[2]*P0[2]);
}
T den = d[0]*d[0]+d[1]*d[1]+d[2]*d[2];
if(den==T()) return false;
T t = -(c[0]*d[0]+c[1]*d[1]+c[2]*d[2]) / den;
a_on_line = a_line.m_pos + a_line.m_dir * t;
a_on_this = closest_point(a_on_line);
return true;
}
bool intersect(const line<T>& a_line,vec3<T>& a_out,const T& a_prec) const {
vec3<T> p,q;
if(!closest_points(a_line,p,q)) return false;
if((q-p).length()>a_prec) return false;
a_out = p;
return true;
}
*/
protected:
vec3<T> m_pos;
vec3<T> m_dir; //normalized.
};
}
#endif
source/analysis/g4tools/include/tools/lina/mat
+538
View File
@@ -0,0 +1,538 @@
// Copyright (C) 2010, Guy Barrand. All rights reserved.
// See the file tools.license for terms.
#ifndef tools_mat
#define tools_mat
#ifdef TOOLS_MEM
#include "mem"
#endif
#include "MATCOM"
//#include <cstring> //memcpy
//#define TOOLS_MAT_NEW
namespace tools {
template <class T,unsigned int D>
class mat {
static const unsigned int _D2 = D*D;
unsigned int dim2() const {return _D2;}
#define TOOLS_MAT_CLASS mat
TOOLS_MATCOM
#undef TOOLS_MAT_CLASS
private:
#ifdef TOOLS_MEM
static const std::string& s_class() {
static const std::string s_v("tools::mat");
return s_v;
}
#endif
public:
mat() {
#ifdef TOOLS_MEM
mem::increment(s_class().c_str());
#endif
#ifdef TOOLS_MAT_NEW
m_vec = new T[D*D];
#endif
for(unsigned int i=0;i<_D2;i++) m_vec[i] = zero();
}
virtual ~mat() {
#ifdef TOOLS_MAT_NEW
delete [] m_vec;
#endif
#ifdef TOOLS_MEM
mem::decrement(s_class().c_str());
#endif
}
public:
mat(const mat& a_from) {
#ifdef TOOLS_MEM
mem::increment(s_class().c_str());
#endif
#ifdef TOOLS_MAT_NEW
m_vec = new T[D*D];
#endif
_copy(a_from.m_vec);
}
mat& operator=(const mat& a_from){
if(&a_from==this) return *this;
_copy(a_from.m_vec);
return *this;
}
public:
mat(const T a_v[]){
#ifdef TOOLS_MEM
mem::increment(s_class().c_str());
#endif
#ifdef TOOLS_MAT_NEW
m_vec = new T[D*D];
#endif
_copy(a_v);
}
public:
unsigned int dimension() const {return D;}
protected:
#ifdef TOOLS_MAT_NEW
T* m_vec;
#else
T m_vec[D*D];
#endif
private:static void check_instantiation() {mat<float,2> dummy;}
};
template <class T>
class nmat {
unsigned int dim2() const {return m_D2;}
#define TOOLS_MAT_CLASS nmat
TOOLS_MATCOM
#undef TOOLS_MAT_CLASS
private:
#ifdef TOOLS_MEM
static const std::string& s_class() {
static const std::string s_v("tools::nmat");
return s_v;
}
#endif
public:
nmat(unsigned int a_D):m_D(a_D),m_D2(a_D*a_D),m_vec(0) {
#ifdef TOOLS_MEM
mem::increment(s_class().c_str());
#endif
m_vec = new T[m_D2];
for(unsigned int i=0;i<m_D2;i++) m_vec[i] = zero();
}
virtual ~nmat() {
delete [] m_vec;
#ifdef TOOLS_MEM
mem::decrement(s_class().c_str());
#endif
}
public:
nmat(const nmat& a_from)
:m_D(a_from.m_D),m_D2(a_from.m_D2),m_vec(0)
{
#ifdef TOOLS_MEM
mem::increment(s_class().c_str());
#endif
m_vec = new T[m_D2];
_copy(a_from.m_vec);
}
nmat& operator=(const nmat& a_from){
if(&a_from==this) return *this;
if(a_from.m_D!=m_D) {
m_D = a_from.m_D;
m_D2 = a_from.m_D2;
delete [] m_vec;
m_vec = new T[m_D2];
}
_copy(a_from.m_vec);
return *this;
}
public:
nmat(unsigned int a_D,const T a_v[])
:m_D(a_D),m_D2(a_D*a_D),m_vec(0)
{
#ifdef TOOLS_MEM
mem::increment(s_class().c_str());
#endif
m_vec = new T[m_D2];
_copy(a_v);
}
public:
unsigned int dimension() const {return m_D;}
protected:
unsigned int m_D;
unsigned int m_D2;
T* m_vec;
private:static void check_instantiation() {nmat<float> dummy(2);}
};
template <class T,unsigned int D>
inline nmat<T> copy(const mat<T,D>& a_from) {
unsigned int D2 = D*D;
nmat<T> v(D);
for(unsigned int i=0;i<D2;i++) v[i] = a_from[i];
return v;
}
template <class MAT>
inline void conjugate(MAT& a_m) {
typedef typename MAT::elem_t T;
T* pos = const_cast<T*>(a_m.data());
unsigned int D2 = a_m.dimension()*a_m.dimension();
for(unsigned int i=0;i<D2;i++,pos++) {
*pos = _conj(*pos); //T = std::complex<>
}
}
template <class MAT>
inline void dagger(MAT& a_m) {
conjugate<MAT>(a_m);
a_m.transpose();
}
//for sf, mf :
//template <class T,unsigned int D>
//inline const T* get_data(const mat<T,D>& a_v) {return a_v.data();}
template <class MAT>
inline MAT commutator(const MAT& a1,const MAT& a2) {
return a1*a2-a2*a1;
}
template <class MAT>
inline MAT anticommutator(const MAT& a1,const MAT& a2) {
return a1*a2+a2*a1;
}
template <class MAT>
inline void commutator(const MAT& a1,const MAT& a2,MAT& a_tmp,MAT& a_res) {
a_res = a1;
a_res *= a2;
a_tmp = a2;
a_tmp *= a1;
a_res -= a_tmp;
}
template <class MAT,class T>
inline void commutator(const MAT& a1,const MAT& a2,MAT& a_tmp,T a_vtmp[],MAT& a_res) {
a_res = a1;
a_res.mul_mtx(a2,a_vtmp);
a_tmp = a2;
a_tmp.mul_mtx(a1,a_vtmp);
a_res -= a_tmp;
}
template <class MAT>
inline void anticommutator(const MAT& a1,const MAT& a2,MAT& a_tmp,MAT& a_res) {
a_res = a1;
a_res *= a2;
a_tmp = a2;
a_tmp *= a1;
a_res += a_tmp;
}
template <class MAT,class T>
inline void anticommutator(const MAT& a1,const MAT& a2,MAT& a_tmp,T a_vtmp[],MAT& a_res) {
a_res = a1;
a_res.mul_mtx(a2,a_vtmp);
a_tmp = a2;
a_tmp.mul_mtx(a1,a_vtmp);
a_res += a_tmp;
}
template <class T,unsigned int D>
inline bool commutator_equal(const mat<T,D>& a_1,const mat<T,D>& a_2,const mat<T,D>& a_c,const T& a_prec) {
unsigned int order = D;
const T* p1 = a_1.data();
const T* p2 = a_2.data();
const T* pc = a_c.data();
for(unsigned int r=0;r<order;r++) {
for(unsigned int c=0;c<order;c++) {
T _12 = T();
{for(unsigned int i=0;i<order;i++) {
_12 += (*(p1+r+i*order)) * (*(p2+i+c*order));
}}
T _21 = T();
{for(unsigned int i=0;i<order;i++) {
_21 += (*(p2+r+i*order)) * (*(p1+i+c*order));
}}
T diff = (_12-_21) - *(pc+r+c*order);
if(diff<T()) diff *= T(-1);
if(diff>=a_prec) return false;
}
}
return true;
}
template <class T,unsigned int D>
inline bool anticommutator_equal(const mat<T,D>& a_1,const mat<T,D>& a_2,const mat<T,D>& a_c,const T& a_prec) {
unsigned int order = D;
const T* p1 = a_1.data();
const T* p2 = a_2.data();
const T* pc = a_c.data();
for(unsigned int r=0;r<order;r++) {
for(unsigned int c=0;c<order;c++) {
T _12 = T();
{for(unsigned int i=0;i<order;i++) {
_12 += (*(p1+r+i*order)) * (*(p2+i+c*order));
}}
T _21 = T();
{for(unsigned int i=0;i<order;i++) {
_21 += (*(p2+r+i*order)) * (*(p1+i+c*order));
}}
T diff = (_12+_21) - *(pc+r+c*order);
if(diff<T()) diff *= T(-1);
if(diff>=a_prec) return false;
}
}
return true;
}
template <class T,unsigned int D>
inline mat<T,D> operator-(const mat<T,D>& a1,const mat<T,D>& a2) {
mat<T,D> res(a1);
res -= a2;
return res;
}
template <class T,unsigned int D>
inline mat<T,D> operator+(const mat<T,D>& a1,const mat<T,D>& a2) {
mat<T,D> res(a1);
res += a2;
return res;
}
template <class T,unsigned int D>
inline mat<T,D> operator*(const mat<T,D>& a1,const mat<T,D>& a2) {
mat<T,D> res(a1);
res *= a2;
return res;
}
template <class T,unsigned int D>
inline mat<T,D> operator*(const T& a_fac,const mat<T,D>& a_m) {
mat<T,D> res(a_m);
res *= a_fac;
return res;
}
/*
template <class T,unsigned int D>
inline mat<T,D> operator*(const mat<T,D>& a_m,const T& a_fac) {
mat<T,D> res(a_m);
res *= a_fac;
return res;
}
*/
template <class T>
inline nmat<T> operator-(const nmat<T>& a1,const nmat<T>& a2) {
nmat<T> res(a1);
res -= a2;
return res;
}
template <class T>
inline nmat<T> operator+(const nmat<T>& a1,const nmat<T>& a2) {
nmat<T> res(a1);
res += a2;
return res;
}
template <class T>
inline nmat<T> operator*(const nmat<T>& a1,const nmat<T>& a2) {
nmat<T> res(a1);
res *= a2;
return res;
}
template <class T>
inline nmat<T> operator*(const T& a_fac,const nmat<T>& a_m) {
nmat<T> res(a_m);
res *= a_fac;
return res;
}
/*
template <class T>
inline nmat<T> operator*(const nmat<T>& a_m,const T& a_fac) {
nmat<T> res(a_m);
res *= a_fac;
return res;
}
*/
}
namespace tools {
////////////////////////////////////////////////
/// specific D=2 ///////////////////////////////
////////////////////////////////////////////////
template <class MAT>
inline void matrix_set(MAT& a_m
,const typename MAT::elem_t& a_00,const typename MAT::elem_t& a_01
,const typename MAT::elem_t& a_10,const typename MAT::elem_t& a_11
){
//a_<R><C>
//vec[R + C * 2];
typename MAT::elem_t* vec = const_cast<typename MAT::elem_t*>(a_m.data());
vec[0] = a_00;vec[2] = a_01;
vec[1] = a_10;vec[3] = a_11;
}
template <class MAT>
inline void set_epsilon(MAT& a_m) {
matrix_set<MAT>(a_m, 0, 1,
-1, 0);
}
// Pauli matrices :
// P1 P2 P3
// 0 1 0 -i 1 0
// 1 0 i 0 0 -1
template <class MAT>
inline void set_P1(MAT& a_m) {
matrix_set<MAT>(a_m, 0, 1,
1, 0);
}
template <class MAT>
inline void set_P2(MAT& a_m) {
typedef typename MAT::elem_t T;
//T i;set_i(i); //with inlib::symbol
T i(0,1);
T _i = T(-1)*i;
matrix_set<MAT>(a_m, 0, _i,
i, 0);
}
template <class MAT>
inline void set_P3(MAT& a_m) {
matrix_set<MAT>(a_m, 1, 0,
0,-1);
}
////////////////////////////////////////////////
/// specific D=3 ///////////////////////////////
////////////////////////////////////////////////
template <class MAT>
inline void matrix_set(MAT& a_m
,const typename MAT::elem_t& a_00,const typename MAT::elem_t& a_01,const typename MAT::elem_t& a_02 //1 row
,const typename MAT::elem_t& a_10,const typename MAT::elem_t& a_11,const typename MAT::elem_t& a_12 //2 row
,const typename MAT::elem_t& a_20,const typename MAT::elem_t& a_21,const typename MAT::elem_t& a_22 //3 row
){
//a_<R><C>
//vec[R + C * 3];
typename MAT::elem_t* vec = const_cast<typename MAT::elem_t*>(a_m.data());
vec[0] = a_00;vec[3] = a_01;vec[6] = a_02;
vec[1] = a_10;vec[4] = a_11;vec[7] = a_12;
vec[2] = a_20;vec[5] = a_21;vec[8] = a_22;
}
// Generators of rotation group :
// Rk(i,j) = epsilon(k,i,j) i,j,k=1,2,3
template <class MAT>
inline void set_R1(MAT& a_m) {
matrix_set<MAT>(a_m, 0, 0, 0,
0, 0, 1,
0,-1, 0);
}
template <class MAT>
inline void set_R2(MAT& a_m) {
matrix_set<MAT>(a_m, 0, 0,-1,
0, 0, 0,
1, 0, 0);
}
template <class MAT>
inline void set_R3(MAT& a_m) {
matrix_set<MAT>(a_m, 0, 1, 0,
-1, 0, 0,
0, 0, 0);
}
////////////////////////////////////////////////
/// specific D=4 ///////////////////////////////
////////////////////////////////////////////////
template <class MAT>
inline void matrix_set(MAT& a_m
,const typename MAT::elem_t& a_00,const typename MAT::elem_t& a_01,const typename MAT::elem_t& a_02,const typename MAT::elem_t& a_03 //1 row
,const typename MAT::elem_t& a_10,const typename MAT::elem_t& a_11,const typename MAT::elem_t& a_12,const typename MAT::elem_t& a_13 //2 row
,const typename MAT::elem_t& a_20,const typename MAT::elem_t& a_21,const typename MAT::elem_t& a_22,const typename MAT::elem_t& a_23 //3 row
,const typename MAT::elem_t& a_30,const typename MAT::elem_t& a_31,const typename MAT::elem_t& a_32,const typename MAT::elem_t& a_33 //4 row
){
//a_<R><C>
//vec[R + C * 4];
typename MAT::elem_t* vec = const_cast<typename MAT::elem_t*>(a_m.data());
vec[0] = a_00;vec[4] = a_01;vec[ 8] = a_02;vec[12] = a_03;
vec[1] = a_10;vec[5] = a_11;vec[ 9] = a_12;vec[13] = a_13;
vec[2] = a_20;vec[6] = a_21;vec[10] = a_22;vec[14] = a_23;
vec[3] = a_30;vec[7] = a_31;vec[11] = a_32;vec[15] = a_33;
}
template <class MAT,class RANDOM>
inline void set_random_antisym(MAT& a_m,RANDOM& a_rd) {
typedef typename MAT::elem_t T;
T v01 = a_rd.shoot();
T v02 = a_rd.shoot();
T v03 = a_rd.shoot();
T v12 = a_rd.shoot();
T v13 = a_rd.shoot();
T v23 = a_rd.shoot();
matrix_set(a_m,
0, v01, v02, v03,
-v01, 0, v12, v13,
-v02,-v12, 0, v23,
-v03,-v13,-v23, 0);
}
template <class MAT>
inline void set_eta(MAT& a_m) {
a_m.set_zero();
a_m.set_value(0,0, 1);
a_m.set_value(1,1,-1);
a_m.set_value(2,2,-1);
a_m.set_value(3,3,-1);
}
////////////////////////////////////////////////
/// specific D=6 ///////////////////////////////
////////////////////////////////////////////////
/*
template <class T>
inline void set_matrix(mat<T,6>& a_m
,const T& a_00,const T& a_01,const T& a_02,const T& a_03,const T& a_04,const T& a_05 //1 row
,const T& a_10,const T& a_11,const T& a_12,const T& a_13,const T& a_14,const T& a_15 //2 row
,const T& a_20,const T& a_21,const T& a_22,const T& a_23,const T& a_24,const T& a_25 //3 row
,const T& a_30,const T& a_31,const T& a_32,const T& a_33,const T& a_34,const T& a_35 //4 row
,const T& a_40,const T& a_41,const T& a_42,const T& a_43,const T& a_44,const T& a_45 //5 row
,const T& a_50,const T& a_51,const T& a_52,const T& a_53,const T& a_54,const T& a_55 //6 row
){
//a_<R><C>
//vec[R + C * 6];
T* vec = const_cast<T*>(a_m.data());
vec[0] = a_00;vec[6] = a_01;vec[12] = a_02;vec[18] = a_03;vec[24] = a_04;vec[30] = a_05;
vec[1] = a_10;vec[7] = a_11;vec[13] = a_12;vec[19] = a_13;vec[25] = a_14;vec[31] = a_15;
vec[2] = a_20;vec[8] = a_21;vec[14] = a_22;vec[20] = a_23;vec[26] = a_24;vec[32] = a_25;
vec[3] = a_30;vec[9] = a_31;vec[15] = a_32;vec[21] = a_33;vec[27] = a_34;vec[33] = a_35;
vec[4] = a_40;vec[10] = a_41;vec[16] = a_42;vec[22] = a_43;vec[28] = a_44;vec[34] = a_45;
vec[5] = a_50;vec[11] = a_51;vec[17] = a_52;vec[23] = a_53;vec[29] = a_54;vec[35] = a_55;
}
*/
}
////////////////////////////////////////////////
////////////////////////////////////////////////
////////////////////////////////////////////////
#include <ostream>
namespace tools {
//NOTE : print is a Python keyword.
template <class MAT>
inline void dump(std::ostream& a_out,const std::string& aCMT,const MAT& a_matrix) {
if(aCMT.size()) a_out << aCMT << std::endl;
unsigned int D = a_matrix.dimension();
for(unsigned int r=0;r<D;r++) {
for(unsigned int c=0;c<D;c++) {
a_out << " " << a_matrix.value(r,c);
}
a_out << std::endl;
}
}
template <class MAT>
inline bool check_invert(const MAT& a_matrix,std::ostream& a_out) {
MAT I;I.set_identity();
MAT tmp;
if(!a_matrix.invert(tmp)) return false;
tmp.mul_mtx(a_matrix);
if(!tmp.equal(I)) {
dump(a_out,"problem with inv of :",a_matrix);
return false;
}
return true;
}
}
#endif
source/analysis/g4tools/include/tools/lina/mat4
+476
View File
@@ -0,0 +1,476 @@
// Copyright (C) 2010, Guy Barrand. All rights reserved.
// See the file tools.license for terms.
#ifndef tools_mat4
#define tools_mat4
#include "mat"
#include <cmath>
namespace tools {
template <class T>
class mat4 : public mat<T,4> {
typedef mat<T,4> parent;
typedef mat<T,4> pr;
public:
mat4():parent() {}
mat4(const mat<T,4>& a_from):parent(a_from){}
virtual ~mat4() {}
public:
mat4(const mat4& a_from):parent(a_from){}
mat4& operator=(const mat4& a_from){
parent::operator=(a_from);
return *this;
}
public:
mat4(const T& a_00,const T& a_01,const T& a_02,const T& a_03, //first row
const T& a_10,const T& a_11,const T& a_12,const T& a_13, //second row
const T& a_20,const T& a_21,const T& a_22,const T& a_23, //third row
const T& a_30,const T& a_31,const T& a_32,const T& a_33) //fourth row
{
set_matrix(a_00,a_01,a_02,a_03,
a_10,a_11,a_12,a_13,
a_20,a_21,a_22,a_23,
a_30,a_31,a_32,a_33);
}
public:
void set_matrix(const mat4<T>& a_m){
parent::set_matrix(a_m);
}
void set_matrix(
const T& a_00,const T& a_01,const T& a_02,const T& a_03, //1 row
const T& a_10,const T& a_11,const T& a_12,const T& a_13, //2 row
const T& a_20,const T& a_21,const T& a_22,const T& a_23, //3 row
const T& a_30,const T& a_31,const T& a_32,const T& a_33) //4 row
{
//a_<R><C>
//pr::m_vec[R + C * 4];
pr::m_vec[0] = a_00;pr::m_vec[4] = a_01;pr::m_vec[ 8] = a_02;pr::m_vec[12] = a_03;
pr::m_vec[1] = a_10;pr::m_vec[5] = a_11;pr::m_vec[ 9] = a_12;pr::m_vec[13] = a_13;
pr::m_vec[2] = a_20;pr::m_vec[6] = a_21;pr::m_vec[10] = a_22;pr::m_vec[14] = a_23;
pr::m_vec[3] = a_30;pr::m_vec[7] = a_31;pr::m_vec[11] = a_32;pr::m_vec[15] = a_33;
}
void set_scale(const T& a_s) {
_set_scale(a_s,a_s,a_s,pr::m_vec);
}
void set_scale(const T& a_1,const T& a_2,const T& a_3) {
_set_scale(a_1,a_2,a_3,pr::m_vec);
}
void set_translate(const T& a_x,const T& a_y,const T& a_z) {
_set_translate(a_x,a_y,a_z,pr::m_vec);
}
void set_rotate(const T& a_x,const T& a_y,const T& a_z,const T& a_angle) {
_set_rotate(a_x,a_y,a_z,a_angle,pr::m_vec);
}
void set_ortho(const T& a_l,const T& a_r, //left,right
const T& a_b,const T& a_t, //bottom,top
const T& a_n,const T& a_f) { //znear,zfar
// from man glOrtho.
T tx = -(a_r+a_l)/(a_r-a_l);
T ty = -(a_t+a_b)/(a_t-a_b);
T tz = -(a_f+a_n)/(a_f-a_n);
T a_00,a_01,a_02,a_03;
T a_10,a_11,a_12,a_13;
T a_20,a_21,a_22,a_23;
T a_30,a_31,a_32,a_33;
a_00 = 2/(a_r-a_l);a_01 = 0;a_02 = 0;a_03 = tx;
a_10 = 0;a_11 = 2/(a_t-a_b);a_12 = 0;a_13 = ty;
a_20 = 0;a_21 = 0;a_22 = -2/(a_f-a_n);a_23 = tz;
a_30 = 0;a_31 = 0;a_32 = 0;a_33 = 1;
set_matrix(a_00,a_01,a_02,a_03, //1 row
a_10,a_11,a_12,a_13, //2 row
a_20,a_21,a_22,a_23, //3 row
a_30,a_31,a_32,a_33); //4 row
//NOTE : Z(x,y, z,1) = -2z/(f-n)+tz = [-2z-f-n]/(f-n)
// W(x,y, z,1) = 1 -> Z/W = Z
// Z(x,y,-n,1) = -1
// Z(x,y,-f,1) = 1
// Z(x,y,-(f+n)/2,1) = 0
// X(x,0, z,1) = 2x/(r-l)+tx = [2x-r-l]/(r-l)
// X(r,0, z,1) = 1
// the view direction is then (0,0,1) in the final projection.
}
void set_frustum(const T& a_l,const T& a_r, //left,right
const T& a_b,const T& a_t, //bottom,top
const T& a_n,const T& a_f) { //znear,zfar
// from man glFrustum.
T A = (a_r+a_l)/(a_r-a_l);
T B = (a_t+a_b)/(a_t-a_b);
T C = -(a_f+a_n)/(a_f-a_n);
T D = -(2*a_f*a_n)/(a_f-a_n);
T a_00,a_01,a_02,a_03;
T a_10,a_11,a_12,a_13;
T a_20,a_21,a_22,a_23;
T a_30,a_31,a_32,a_33;
a_00 = (2*a_n)/(a_r-a_l);a_01 = 0;a_02 = A;a_03 = 0;
a_10 = 0;a_11 = (2*a_n)/(a_t-a_b);a_12 = B;a_13 = 0;
a_20 = 0;a_21 = 0;a_22 = C;a_23 = D;
a_30 = 0;a_31 = 0;a_32 = -1;a_33 = 0;
set_matrix(a_00,a_01,a_02,a_03, //1 row
a_10,a_11,a_12,a_13, //2 row
a_20,a_21,a_22,a_23, //3 row
a_30,a_31,a_32,a_33); //4 row
//NOTE : Z(x,y, z,1) = C*z+D = -[(f+n)*z+2*f*n]/(f-n)
// Z(x,y,-n,1) = -[-fn-nn+2fn]/(f-n) = -[fn-nn]/(f-n) = -n
// W(x,y,-n,1) = n
// -> Z/W(x,y,-n,1) = -1
// Z(x,y,-2fn/(f+n),1) = 0
// -> Z/W = 0
// Z(x,y,-f,1) = -[-ff-fn+2fn]/(f-n) = -[fn-ff]/(f-n) = f
// W(x,y,-f,1) = f
// -> Z/W(x,y,-f,1) = 1
// X(x,0, z,1) = 2nx/(r-l)+z(r+l)/(r-l) = [2nx+zr+zl]/(r-l)
// X(r,0,-n,1) = [nr-nl]/(r-l) = n
// W(r,0,-n,1) = n
// -> X/W(r,0,-n,1) = 1
// X(l,0,-n,1) = (2nl-n(r+l))/(r-l) = -n
// W(l,0,-n,1) = n
// -> X/W(l,0,-n,1) = -1
// lrbt corners are in plane z=-1 at xy=+/-1.
// eye ?
// eye before ? (0,0,z,1) -> (zA,zB,zC+D,-z) /W -> (-A,-B,-(C+D/z),1)
// z=0 -> (0,0,D=-2fn(f-n),0)
}
void get_translate(T& a_x,T& a_y,T& a_z) const {
a_x = pr::m_vec[12];
a_y = pr::m_vec[13];
a_z = pr::m_vec[14];
}
public:
void mul_4(T& a_x,T& a_y,T& a_z,T& a_p) const {
// a_[x,y,z,p] = this * a_[x,y,z,p]
//pr::m_vec[R + C * 4];
//pr::m_vec[0]= 00;pr::m_vec[4] = 01;pr::m_vec[ 8] = 02;pr::m_vec[12] = 03;
//pr::m_vec[1]= 10;pr::m_vec[5] = 11;pr::m_vec[ 9] = 12;pr::m_vec[13] = 13;
//pr::m_vec[2]= 20;pr::m_vec[6] = 21;pr::m_vec[10] = 22;pr::m_vec[14] = 23;
//pr::m_vec[3]= 30;pr::m_vec[7] = 31;pr::m_vec[11] = 32;pr::m_vec[15] = 33;
T x= pr::m_vec[0]*a_x+pr::m_vec[4]*a_y+pr::m_vec[ 8]*a_z+pr::m_vec[12]*a_p;
T y= pr::m_vec[1]*a_x+pr::m_vec[5]*a_y+pr::m_vec[ 9]*a_z+pr::m_vec[13]*a_p;
T z= pr::m_vec[2]*a_x+pr::m_vec[6]*a_y+pr::m_vec[10]*a_z+pr::m_vec[14]*a_p;
T p= pr::m_vec[3]*a_x+pr::m_vec[7]*a_y+pr::m_vec[11]*a_z+pr::m_vec[15]*a_p;
a_x = x;
a_y = y;
a_z = z;
a_p = p;
}
void mul_3(T& a_x,T& a_y,T& a_z) const {
// a_[x,y,z] = this * a_[x,y,z]
//pr::m_vec[R + C * 4];
//pr::m_vec[0]= 00;pr::m_vec[4] = 01;pr::m_vec[ 8] = 02;pr::m_vec[12] = 03;
//pr::m_vec[1]= 10;pr::m_vec[5] = 11;pr::m_vec[ 9] = 12;pr::m_vec[13] = 13;
//pr::m_vec[2]= 20;pr::m_vec[6] = 21;pr::m_vec[10] = 22;pr::m_vec[14] = 23;
//pr::m_vec[3]= 30;pr::m_vec[7] = 31;pr::m_vec[11] = 32;pr::m_vec[15] = 33;
T x = pr::m_vec[0]*a_x+pr::m_vec[4]*a_y+pr::m_vec[ 8]*a_z+pr::m_vec[12];
T y = pr::m_vec[1]*a_x+pr::m_vec[5]*a_y+pr::m_vec[ 9]*a_z+pr::m_vec[13];
T z = pr::m_vec[2]*a_x+pr::m_vec[6]*a_y+pr::m_vec[10]*a_z+pr::m_vec[14];
a_x = x;
a_y = y;
a_z = z;
}
void mul_2(T& a_x,T& a_y) const {
// a_[x,y] = this * a_[x,y]
//pr::m_vec[R + C * 4];
//pr::m_vec[0]= 00;pr::m_vec[4] = 01;pr::m_vec[ 8] = 02;pr::m_vec[12] = 03;
//pr::m_vec[1]= 10;pr::m_vec[5] = 11;pr::m_vec[ 9] = 12;pr::m_vec[13] = 13;
//pr::m_vec[2]= 20;pr::m_vec[6] = 21;pr::m_vec[10] = 22;pr::m_vec[14] = 23;
//pr::m_vec[3]= 30;pr::m_vec[7] = 31;pr::m_vec[11] = 32;pr::m_vec[15] = 33;
T x = pr::m_vec[0]*a_x+pr::m_vec[4]*a_y+pr::m_vec[12];
T y = pr::m_vec[1]*a_x+pr::m_vec[5]*a_y+pr::m_vec[13];
a_x = x;
a_y = y;
}
void mul_dir_3(T& a_x,T& a_y,T& a_z) const {
// used to multiply normals.
// a_[x,y,z] = rot_part(this) * a_[x,y,z]
T x = pr::m_vec[0]*a_x+pr::m_vec[4]*a_y+pr::m_vec[ 8]*a_z;
T y = pr::m_vec[1]*a_x+pr::m_vec[5]*a_y+pr::m_vec[ 9]*a_z;
T z = pr::m_vec[2]*a_x+pr::m_vec[6]*a_y+pr::m_vec[10]*a_z;
a_x = x;
a_y = y;
a_z = z;
}
template <class VEC>
void mul_dir_3(VEC& a_dir) const {
mul_dir_3(a_dir[0],a_dir[1],a_dir[2]);
}
void mul_scale(const T& a_sx,const T& a_sy,const T& a_sz) {
// this = this * mat4_scale(a_s[x,y,z]
//pr::m_vec[R + C * 4];
//pr::m_vec[0]= 00;pr::m_vec[4] = 01;pr::m_vec[ 8] = 02;pr::m_vec[12] = 03;
//pr::m_vec[1]= 10;pr::m_vec[5] = 11;pr::m_vec[ 9] = 12;pr::m_vec[13] = 13;
//pr::m_vec[2]= 20;pr::m_vec[6] = 21;pr::m_vec[10] = 22;pr::m_vec[14] = 23;
//pr::m_vec[3]= 30;pr::m_vec[7] = 31;pr::m_vec[11] = 32;pr::m_vec[15] = 33;
pr::m_vec[0] *= a_sx;
pr::m_vec[1] *= a_sx;
pr::m_vec[2] *= a_sx;
pr::m_vec[3] *= a_sx;
pr::m_vec[4] *= a_sy;
pr::m_vec[5] *= a_sy;
pr::m_vec[6] *= a_sy;
pr::m_vec[7] *= a_sy;
pr::m_vec[ 8] *= a_sz;
pr::m_vec[ 9] *= a_sz;
pr::m_vec[10] *= a_sz;
pr::m_vec[11] *= a_sz;
}
void mul_scale(const T& a_s) {
pr::m_vec[0] *= a_s;
pr::m_vec[1] *= a_s;
pr::m_vec[2] *= a_s;
pr::m_vec[3] *= a_s;
pr::m_vec[4] *= a_s;
pr::m_vec[5] *= a_s;
pr::m_vec[6] *= a_s;
pr::m_vec[7] *= a_s;
pr::m_vec[ 8] *= a_s;
pr::m_vec[ 9] *= a_s;
pr::m_vec[10] *= a_s;
pr::m_vec[11] *= a_s;
}
void mul_translate(const T& a_x,const T& a_y,const T& a_z) {
pr::m_vec[12] = pr::m_vec[0]*a_x+pr::m_vec[4]*a_y+pr::m_vec[ 8]*a_z+pr::m_vec[12];
pr::m_vec[13] = pr::m_vec[1]*a_x+pr::m_vec[5]*a_y+pr::m_vec[ 9]*a_z+pr::m_vec[13];
pr::m_vec[14] = pr::m_vec[2]*a_x+pr::m_vec[6]*a_y+pr::m_vec[10]*a_z+pr::m_vec[14];
pr::m_vec[15] = pr::m_vec[3]*a_x+pr::m_vec[7]*a_y+pr::m_vec[11]*a_z+pr::m_vec[15];
}
void mul_rotate(const T& a_x,const T& a_y,const T& a_z,const T& a_angle) {
T rot[16];
_set_rotate(a_x,a_y,a_z,a_angle,rot);
parent::_mul_mtx(rot);
}
void left_mul_rotate(const T& a_x,const T& a_y,const T& a_z,
const T& a_angle) {
T _m[16];
_set_rotate(a_x,a_y,a_z,a_angle,_m);
parent::_left_mul_mtx(_m);
}
void left_mul_scale(const T& a_x,const T& a_y,const T& a_z) {
T _m[16];
_set_scale(a_x,a_y,a_z,_m);
parent::_left_mul_mtx(_m);
}
void left_mul_translate(const T& a_x,const T& a_y,const T& a_z) {
T _m[16];
_set_translate(a_x,a_y,a_z,_m);
parent::_left_mul_mtx(_m);
}
void v00(const T& a_value){pr::m_vec[0+0*4] = a_value;}
void v10(const T& a_value){pr::m_vec[1+0*4] = a_value;}
void v20(const T& a_value){pr::m_vec[2+0*4] = a_value;}
void v30(const T& a_value){pr::m_vec[3+0*4] = a_value;}
void v01(const T& a_value){pr::m_vec[0+1*4] = a_value;}
void v11(const T& a_value){pr::m_vec[1+1*4] = a_value;}
void v21(const T& a_value){pr::m_vec[2+1*4] = a_value;}
void v31(const T& a_value){pr::m_vec[3+1*4] = a_value;}
void v02(const T& a_value){pr::m_vec[0+2*4] = a_value;}
void v12(const T& a_value){pr::m_vec[1+2*4] = a_value;}
void v22(const T& a_value){pr::m_vec[2+2*4] = a_value;}
void v32(const T& a_value){pr::m_vec[3+2*4] = a_value;}
void v03(const T& a_value){pr::m_vec[0+3*4] = a_value;}
void v13(const T& a_value){pr::m_vec[1+3*4] = a_value;}
void v23(const T& a_value){pr::m_vec[2+3*4] = a_value;}
void v33(const T& a_value){pr::m_vec[3+3*4] = a_value;}
const T& v00() const {return pr::m_vec[0+0*4];}
const T& v10() const {return pr::m_vec[1+0*4];}
const T& v20() const {return pr::m_vec[2+0*4];}
const T& v30() const {return pr::m_vec[3+0*4];}
const T& v01() const {return pr::m_vec[0+1*4];}
const T& v11() const {return pr::m_vec[1+1*4];}
const T& v21() const {return pr::m_vec[2+1*4];}
const T& v31() const {return pr::m_vec[3+1*4];}
const T& v02() const {return pr::m_vec[0+2*4];}
const T& v12() const {return pr::m_vec[1+2*4];}
const T& v22() const {return pr::m_vec[2+2*4];}
const T& v32() const {return pr::m_vec[3+2*4];}
const T& v03() const {return pr::m_vec[0+3*4];}
const T& v13() const {return pr::m_vec[1+3*4];}
const T& v23() const {return pr::m_vec[2+3*4];}
const T& v33() const {return pr::m_vec[3+3*4];}
protected:
static void _set_translate(const T& a_x,const T& a_y,const T& a_z,T v[]) {
v[0] = T(1);v[4] = 0;v[ 8] = 0;v[12] = a_x;
v[1] = 0;v[5] = T(1);v[ 9] = 0;v[13] = a_y;
v[2] = 0;v[6] = 0;v[10] = T(1);v[14] = a_z;
v[3] = 0;v[7] = 0;v[11] = 0;v[15] = T(1);
}
static void _set_scale(const T& a_1,const T& a_2,const T& a_3,T v[]) {
v[0] = a_1;v[4] = 0;v[ 8] = 0;v[12] = 0;
v[1] = 0;v[5] = a_2;v[ 9] = 0;v[13] = 0;
v[2] = 0;v[6] = 0;v[10] = a_3;v[14] = 0;
v[3] = 0;v[7] = 0;v[11] = 0;v[15] = T(1);
}
static void _set_rotate(const T& a_x,const T& a_y,const T& a_z,
const T& a_angle,T v[]) {
//WARNING : (a_x,a_y,a_z) must be a normalized vector.
T co = (T)::cos(a_angle);
T si = (T)::sin(a_angle);
T x = a_x;
T y = a_y;
T z = a_z;
T x2 = x*x;
T y2 = y*y;
T z2 = z*z;
T xy = x*y;
T xz = x*z;
T yz = y*z;
v[0] = x2+(1-x2)*co;v[4] = xy*(1-co)-z*si;v[ 8] = xz*(1-co)+y*si;v[12] = 0;
v[1] = xy*(1-co)+z*si;v[5] = y2+(1-y2)*co;v[ 9] = yz*(1-co)-x*si;v[13] = 0;
v[2] = xz*(1-co)-y*si;v[6] = yz*(1-co)+x*si;v[10] = z2+(1-z2)*co;v[14] = 0;
v[3] = 0;v[7] = 0;v[11] = 0;v[15] = 1;
// If :
// n =(x,y,z)
// n2 = x2+y2+z2 = 1
// n.E = x*E1+y*E2+z*E3
// with :
// E1 E2 E3
// 0 0 0 0 0 -1 0 1 0
// 0 0 1 0 0 0 -1 0 0
// 0 -1 0 1 0 0 0 0 0
//
// R(r,c) = cos(theta)*Id(r,c)+(1-cos(theta))*nr*nc-sin(theta)*(n.E)(r,c)
//
// R = exp(-theta*(n.E))
}
public:
void mul_mtx_rot_root(const T& a_00,const T& a_01,const T& a_02, //1 row
const T& a_10,const T& a_11,const T& a_12, //2 row
const T& a_20,const T& a_21,const T& a_22) //3 row
{
T* tv = pr::m_vec;
//pr::m_vec[0] = 00;pr::m_vec[4] = 01;pr::m_vec[ 8] = 02;pr::m_vec[12] = 03;
//pr::m_vec[1] = 10;pr::m_vec[5] = 11;pr::m_vec[ 9] = 12;pr::m_vec[13] = 13;
//pr::m_vec[2] = 20;pr::m_vec[6] = 21;pr::m_vec[10] = 22;pr::m_vec[14] = 23;
//pr::m_vec[3] = 30;pr::m_vec[7] = 31;pr::m_vec[11] = 32;pr::m_vec[15] = 33;
T tv_0 = tv[0];
T tv_1 = tv[1];
T tv_2 = tv[2];
T tv_3 = tv[3];
T tv_4 = tv[4];
T tv_5 = tv[5];
T tv_6 = tv[6];
T tv_7 = tv[7];
T tv_8 = tv[8];
T tv_9 = tv[9];
T tv_10 = tv[10];
T tv_11 = tv[11];
T tv_12 = tv[12];
T tv_13 = tv[13];
T tv_14 = tv[14];
T tv_15 = tv[15];
T fv_0 = a_00;
T fv_1 = a_10;
T fv_2 = a_20;
//T fv_3 = 0;
T fv_4 = a_01;
T fv_5 = a_11;
T fv_6 = a_21;
//T fv_7 = 0;
T fv_8 = a_02;
T fv_9 = a_12;
T fv_10 = a_22;
//T fv_11 = 0;
//T fv_12 = 0;
//T fv_13 = 0;
//T fv_14 = 0;
//T fv_15 = 1;
tv[0] = tv_0*fv_0+tv_4*fv_1+ tv_8*fv_2;
tv[1] = tv_1*fv_0+tv_5*fv_1+ tv_9*fv_2;
tv[2] = tv_2*fv_0+tv_6*fv_1+tv_10*fv_2;
tv[3] = tv_3*fv_0+tv_7*fv_1+tv_11*fv_2;
tv[4] = tv_0*fv_4+tv_4*fv_5+ tv_8*fv_6;
tv[5] = tv_1*fv_4+tv_5*fv_5+ tv_9*fv_6;
tv[6] = tv_2*fv_4+tv_6*fv_5+tv_10*fv_6;
tv[7] = tv_3*fv_4+tv_7*fv_5+tv_11*fv_6;
tv[8] = tv_0*fv_8+tv_4*fv_9+ tv_8*fv_10;
tv[9] = tv_1*fv_8+tv_5*fv_9+ tv_9*fv_10;
tv[10] = tv_2*fv_8+tv_6*fv_9+tv_10*fv_10;
tv[11] = tv_3*fv_8+tv_7*fv_9+tv_11*fv_10;
tv[12] = tv_12;
tv[13] = tv_13;
tv[14] = tv_14;
tv[15] = tv_15;
}
private:static void check_instantiation() {mat4<float> dummy;}
};
//for sf, mf :
template <class T>
inline const T* get_data(const mat4<T>& a_v) {return a_v.data();}
}
#include <ostream>
namespace tools {
template <class T>
inline std::ostream& operator<<(std::ostream& a_out,const mat4<T>& a_mtx){
const T* v = a_mtx.data();
a_out << v[0] << "," << v[4] << "," << v[ 8] << "," << v[12]
<< std::endl
<< v[1] << "," << v[5] << "," << v[ 9] << "," << v[13]
<< std::endl
<< v[2] << "," << v[6] << "," << v[10] << "," << v[14]
<< std::endl
<< v[3] << "," << v[7] << "," << v[11] << "," << v[15]
<< std::endl;
return a_out;
}
}
#endif
source/analysis/g4tools/include/tools/lina/mat4f
+60
View File
@@ -0,0 +1,60 @@
// Copyright (C) 2010, Guy Barrand. All rights reserved.
// See the file tools.license for terms.
#ifndef tools_mat4f
#define tools_mat4f
#include "mat4"
namespace tools {
class mat4f : public mat4<float> {
public:
mat4f(){}
virtual ~mat4f() {}
public:
mat4f(const mat4f& a_from):mat4<float>(a_from){}
mat4f& operator=(const mat4f& a_from){
mat4<float>::operator=(a_from);
return *this;
}
public:
mat4f(float a_00,float a_01,float a_02,float a_03, //first row
float a_10,float a_11,float a_12,float a_13, //second row
float a_20,float a_21,float a_22,float a_23, //third row
float a_30,float a_31,float a_32,float a_33) //fourth row
:mat4<float>(a_00,a_01,a_02,a_03,
a_10,a_11,a_12,a_13,
a_20,a_21,a_22,a_23,
a_30,a_31,a_32,a_33)
{}
mat4f(const mat4<float>& a_from):mat4<float>(a_from){}
mat4f& operator=(const mat4<float>& a_from){
mat4<float>::operator=(a_from);
return *this;
}
public: //backward compatibility
void mul_2f(float& a_x,float& a_y) const {
mat4<float>::mul_2(a_x,a_y);
}
void mul_3f(float& a_x,float& a_y,float& a_z) const {
mat4<float>::mul_3(a_x,a_y,a_z);
}
void mul_dir_3f(float& a_x,float& a_y,float& a_z) const {
mat4<float>::mul_dir_3(a_x,a_y,a_z);
}
void mul_4f(float& a_x,float& a_y,float& a_z,float& a_w) const {
mat4<float>::mul_4(a_x,a_y,a_z,a_w);
}
public: //operators
};
// for sf_vec<mat4f,float>, sf_mat4f :
inline const std::string& stype(const mat4f&) {
static const std::string s_v("tools::mat4f");
return s_v;
}
}
#endif
source/analysis/g4tools/include/tools/lina/plane
+125
View File
@@ -0,0 +1,125 @@
// Copyright (C) 2010, Guy Barrand. All rights reserved.
// See the file tools.license for terms.
#ifndef tools_plane
#define tools_plane
#include "line"
namespace tools {
template <class T>
class plane {
public:
plane(){}
plane(const vec3<T>& a_p0,const vec3<T>& a_p1,const vec3<T>& a_p2) {
// Construct a plane given 3 points.
// Orientation is computed by taking (p1 - p0) x (p2 - p0) and
// pointing the normal in that direction.
vec3<T> P = a_p1;
P.subtract(a_p0);
vec3<T> P2 = a_p2;
P2.subtract(a_p0);
m_normal = P.cross(P2);
if(!m_normal.normalize()) {} //throw
m_distance =
m_normal.v0() * a_p0.v0() +
m_normal.v1() * a_p0.v1() +
m_normal.v2() * a_p0.v2();
}
plane(const vec3<T>& a_normal,const T& a_distance){
set(a_normal,a_distance);
}
plane(const vec3<T>& a_normal,const vec3<T>& a_point){
set(a_normal,a_point);
}
virtual ~plane() {}
public:
plane(const plane& a_from)
:m_normal(a_from.m_normal)
,m_distance(a_from.m_distance)
{}
plane& operator=(const plane& a_from) {
m_normal = a_from.m_normal;
m_distance = a_from.m_distance;
return *this;
}
public:
bool is_valid() const {return m_normal.length()?true:false;}
void offset(const T& a_distance){
// Offset a plane by a given distance.
m_distance += a_distance;
}
bool intersect(const line<T>& a_line,vec3<T>& a_intersection) const {
// Intersect line and plane, returning true if there is an intersection
// false if line is parallel to plane
const vec3<T>& pos = a_line.position();
const vec3<T>& dir = a_line.direction();
T d = m_normal.dot(dir);
if(d==T()) return false;
T t = (m_distance - m_normal.dot(pos))/d;
a_intersection = dir;
a_intersection.multiply(t);
a_intersection.add(pos);
//a_intersection = pos + t * dir;
return true;
}
bool is_in_half_space(const vec3<T>& a_point) const {
// Returns true if the given point is within the half-space
// defined by the plane
//vec pos = m_normal * m_distance;
vec3<T> pos = m_normal;
pos.multiply(-m_distance);
pos.add(a_point);
return (m_normal.dot(pos) >= T() ? true : false);
}
const vec3<T>& normal() const {return m_normal;}
T distance_from_origin() const {return m_distance;}
T distance(const vec3<T>& a_point) const {
// Return the distance from point to plane. Positive distance means
// the point is in the plane's half space.
return a_point.dot(m_normal) - m_distance;
}
void set(const vec3<T>& a_normal,const T& a_distance){
m_normal = a_normal;
if(!m_normal.normalize()) {} //throw
m_distance = a_distance;
}
void set(const vec3<T>& a_normal,const vec3<T>& a_point){
// Construct a plane given normal and a point to pass through
// Orientation is given by the normal vector n.
m_normal = a_normal;
if(!m_normal.normalize()) {} //throw
m_distance =
m_normal.v0() * a_point.v0() +
m_normal.v1() * a_point.v1() +
m_normal.v2() * a_point.v2();
}
public: //iv2sg
const vec3<T>& getNormal() const {return m_normal;}
protected:
// equation of the plane is :
// norm[0]*x+norm[1]*y+norm[2]*z = dist
vec3<T> m_normal; //normalized.
T m_distance;
};
}
#endif
source/analysis/g4tools/include/tools/lina/qrot
+350
View File
@@ -0,0 +1,350 @@
// Copyright (C) 2010, Guy Barrand. All rights reserved.
// See the file tools.license for terms.
#ifndef tools_qrot
#define tools_qrot
// rotation done with quaternion.
#include "vec4"
#include "vec3"
#include "mat4"
namespace tools {
template <class T>
class qrot {
public:
qrot()
:m_quat(0,0,0,1) //zero rotation around the positive Z axis.
{}
qrot(const vec3<T>& a_axis,T a_radians){
if(!set_value(a_axis,a_radians)) {} //FIXME : throw
}
qrot(const vec3<T>& a_from,const vec3<T>& a_to){set_value(a_from,a_to);}
virtual ~qrot(){}
public:
qrot(const qrot& a_from)
:m_quat(a_from.m_quat)
{}
qrot& operator=(const qrot& a_from){
m_quat = a_from.m_quat;
return *this;
}
protected:
qrot(T a_q0,T a_q1,T a_q2,T a_q3)
:m_quat(a_q0,a_q1,a_q2,a_q3)
{
if(!m_quat.normalize()) {} //FIXME throw
}
public:
qrot& operator*=(const qrot& a_q) {
//Multiplies the quaternions.
//Note that order is important when combining quaternions with the
//multiplication operator.
// Formula from <http://www.lboro.ac.uk/departments/ma/gallery/quat/>
T tx = m_quat.v0();
T ty = m_quat.v1();
T tz = m_quat.v2();
T tw = m_quat.v3();
T qx = a_q.m_quat.v0();
T qy = a_q.m_quat.v1();
T qz = a_q.m_quat.v2();
T qw = a_q.m_quat.v3();
m_quat.set_value(qw*tx + qx*tw + qy*tz - qz*ty,
qw*ty - qx*tz + qy*tw + qz*tx,
qw*tz + qx*ty - qy*tx + qz*tw,
qw*tw - qx*tx - qy*ty - qz*tz);
m_quat.normalize();
return *this;
}
bool operator==(const qrot& a_r) const {
return m_quat.equal(a_r.m_quat);
}
bool operator!=(const qrot& a_r) const {
return !operator==(a_r);
}
qrot operator*(const qrot& a_r) const {
qrot tmp(*this);
tmp *= a_r;
return tmp;
}
bool invert(){
T length = m_quat.length();
if(length==T()) return false;
// Optimize by doing 1 div and 4 muls instead of 4 divs.
T inv = one() / length;
m_quat.set_value(-m_quat.v0() * inv,
-m_quat.v1() * inv,
-m_quat.v2() * inv,
m_quat.v3() * inv);
return true;
}
bool inverse(qrot& a_r) const {
//Non-destructively inverses the rotation and returns the result.
T length = m_quat.length();
if(length==T()) return false;
// Optimize by doing 1 div and 4 muls instead of 4 divs.
T inv = one() / length;
a_r.m_quat.set_value(-m_quat.v0() * inv,
-m_quat.v1() * inv,
-m_quat.v2() * inv,
m_quat.v3() * inv);
return true;
}
bool set_value(const vec3<T>& a_axis,T a_radians) {
// Reset rotation with the given axis-of-rotation and rotation angle.
// Make sure axis is not the null vector when calling this method.
// From <http://www.automation.hut.fi/~jaro/thesis/hyper/node9.html>.
if(a_axis.length()==T()) return false;
m_quat.v3(::cos(a_radians/2));
T sineval = ::sin(a_radians/2);
vec3<T> a = a_axis;
a.normalize();
m_quat.v0(a.v0() * sineval);
m_quat.v1(a.v1() * sineval);
m_quat.v2(a.v2() * sineval);
return true;
}
bool set_value(const vec3<T>& a_from,const vec3<T>& a_to) {
// code taken from coin3d/SbRotation.
vec3<T> from(a_from);
if(from.normalize()==T()) return false;
vec3<T> to(a_to);
if(to.normalize()==T()) return false;
T dot = from.dot(to);
vec3<T> crossvec = from.cross(to);
T crosslen = crossvec.normalize();
if(crosslen == T()) { // Parallel vectors
// Check if they are pointing in the same direction.
if (dot > T()) {
m_quat.set_value(0,0,0,1);
} else {
// Ok, so they are parallel and pointing in the opposite direction
// of each other.
// Try crossing with x axis.
vec3<T> t = from.cross(vec3<T>(1,0,0));
// If not ok, cross with y axis.
if(t.normalize() == T()) {
t = from.cross(vec3<T>(0,1,0));
t.normalize();
}
m_quat.set_value(t[0],t[1],t[2],0);
}
} else { // Vectors are not parallel
// The fabs() wrapping is to avoid problems when `dot' "overflows"
// a tiny wee bit, which can lead to sqrt() returning NaN.
crossvec *= (T)::sqrt(half() * ::fabs(one() - dot));
// The fabs() wrapping is to avoid problems when `dot' "underflows"
// a tiny wee bit, which can lead to sqrt() returning NaN.
m_quat.set_value(crossvec[0], crossvec[1], crossvec[2],(T)::sqrt(half()*::fabs(one()+dot)));
}
return true;
}
bool value(vec3<T>& a_axis,T& a_radians) const {
//WARNING : can fail.
if( (m_quat.v3()<minus_one()) || (m_quat.v3()> one()) ){ ////???
a_axis.set_value(0,0,1);
a_radians = 0;
return false;
}
a_radians = ::acos(m_quat.v3()) * 2;
T sineval = ::sin(a_radians/2);
if(sineval==T()) { //???
a_axis.set_value(0,0,1);
a_radians = 0;
return false;
}
a_axis.set_value(m_quat.v0()/sineval,
m_quat.v1()/sineval,
m_quat.v2()/sineval);
return true;
}
void set_value(const mat4<T>& a_m) {
//WARNING : not tested.
//Set the rotation from the components of the given matrix.
T scalerow = a_m.v00() + a_m.v11() + a_m.v22();
if (scalerow > T()) {
T _s = ::sqrt(scalerow + a_m.v33());
m_quat.v3(_s * half());
_s = half() / _s;
m_quat.v0((a_m.v12() - a_m.v21()) * _s);
m_quat.v1((a_m.v20() - a_m.v02()) * _s);
m_quat.v2((a_m.v01() - a_m.v10()) * _s);
} else {
unsigned int i = 0;
if (a_m.v11() > a_m.v00()) i = 1;
if (a_m.v22() > a_m.value(i,i)) i = 2;
unsigned int j = (i+1)%3;
unsigned int k = (j+1)%3;
T _s = ::sqrt((a_m.value(i,i) - (a_m.value(j,j) + a_m.value(k,k))) + a_m.v33());
m_quat.set_value(i,_s * half());
_s = half() / _s;
m_quat.v3((a_m.value(j,k) - a_m.value(k,j)) * _s);
m_quat.set_value(j,(a_m.value(i,j) + a_m.value(j,i)) * _s);
m_quat.set_value(k,(a_m.value(i,k) + a_m.value(k,i)) * _s);
}
if (a_m.v33()!=one()) {
m_quat.multiply(one()/::sqrt(a_m.v33()));
}
}
void value(mat4<T>& a_m) const {
//Return this rotation in the form of a matrix.
const T x = m_quat.v0();
const T y = m_quat.v1();
const T z = m_quat.v2();
const T w = m_quat.v3();
// z = w + x * i + y * j + z * k
// first row :
a_m.v00(w*w + x*x - y*y - z*z);
a_m.v01(2*x*y - 2*w*z);
a_m.v02(2*x*z + 2*w*y);
a_m.v03(0);
// second row :
a_m.v10(2*x*y + 2*w*z);
a_m.v11(w*w - x*x + y*y - z*z);
a_m.v12(2*y*z - 2*w*x);
a_m.v13(0);
// third row :
a_m.v20(2*x*z - 2*w*y);
a_m.v21(2*y*z + 2*w*x);
a_m.v22(w*w - x*x - y*y + z*z);
a_m.v23(0);
// fourth row :
a_m.v30(0);
a_m.v31(0);
a_m.v32(0);
a_m.v33(w*w + x*x + y*y + z*z);
}
void mul_vec(const vec3<T>& a_in,vec3<T>& a_out) const {
const T x = m_quat.v0();
const T y = m_quat.v1();
const T z = m_quat.v2();
const T w = m_quat.v3();
// first row :
T v0 = (w*w + x*x - y*y - z*z) * a_in.v0()
+ (2*x*y - 2*w*z) * a_in.v1()
+ (2*x*z + 2*w*y) * a_in.v2();
T v1 = (2*x*y + 2*w*z) * a_in.v0()
+(w*w - x*x + y*y - z*z) * a_in.v1()
+ (2*y*z - 2*w*x) * a_in.v2();
T v2 = (2*x*z - 2*w*y) * a_in.v0()
+ (2*y*z + 2*w*x) * a_in.v1()
+(w*w - x*x - y*y + z*z) * a_in.v2();
a_out.set_value(v0,v1,v2);
}
void mul_vec(vec3<T>& a_v) const {
const T x = m_quat.v0();
const T y = m_quat.v1();
const T z = m_quat.v2();
const T w = m_quat.v3();
// first row :
T v0 = (w*w + x*x - y*y - z*z) * a_v.v0()
+ (2*x*y - 2*w*z) * a_v.v1()
+ (2*x*z + 2*w*y) * a_v.v2();
T v1 = (2*x*y + 2*w*z) * a_v.v0()
+(w*w - x*x + y*y - z*z) * a_v.v1()
+ (2*y*z - 2*w*x) * a_v.v2();
T v2 = (2*x*z - 2*w*y) * a_v.v0()
+ (2*y*z + 2*w*x) * a_v.v1()
+(w*w - x*x - y*y + z*z) * a_v.v2();
a_v.set_value(v0,v1,v2);
}
void mul_3(T& a_x,T& a_y,T& a_z) const {
const T x = m_quat.v0();
const T y = m_quat.v1();
const T z = m_quat.v2();
const T w = m_quat.v3();
// first row :
T v0 = (w*w + x*x - y*y - z*z) * a_x
+ (2*x*y - 2*w*z) * a_y
+ (2*x*z + 2*w*y) * a_z;
T v1 = (2*x*y + 2*w*z) * a_x
+(w*w - x*x + y*y - z*z) * a_y
+ (2*y*z - 2*w*x) * a_z;
T v2 = (2*x*z - 2*w*y) * a_x
+ (2*y*z + 2*w*x) * a_y
+(w*w - x*x - y*y + z*z) * a_z;
a_x = v0;
a_y = v1;
a_z = v2;
}
public: //for io::streamer
const vec4<T>& quat() const {return m_quat;}
vec4<T>& quat() {return m_quat;}
protected:
static T one() {return T(1);}
static T minus_one() {return T(-1);}
static T half() {return T(0.5);}
protected:
vec4<T> m_quat;
public:
//NOTE : don't handle a static object because of mem balance.
//static const qrot<double>& identity() {
// static const qrot<double> s_v(0,0,0,1);
// return s_v;
//}
private:static void check_instantiation() {qrot<float> v;}
};
}
#endif
source/analysis/g4tools/include/tools/lina/rotf
+83
View File
@@ -0,0 +1,83 @@
// Copyright (C) 2010, Guy Barrand. All rights reserved.
// See the file tools.license for terms.
#ifndef tools_rotf
#define tools_rotf
// rotation done with quaternion.
#include "qrot"
#include "vec3f"
namespace tools {
class rotf : public qrot<float> {
rotf(float a_q0,float a_q1,float a_q2,float a_q3)
:qrot<float>(a_q0,a_q1,a_q2,a_q3)
{}
public:
rotf()
:qrot<float>() //zero rotation around the positive Z axis.
{}
rotf(const vec3f& a_axis,float a_radians)
:qrot<float>(a_axis,a_radians)
{}
rotf(const vec3f& a_from,const vec3f& a_to)
:qrot<float>(a_from,a_to)
{}
virtual ~rotf(){}
public:
rotf(const rotf& a_from)
:qrot<float>(a_from)
{}
rotf& operator=(const rotf& a_from){
qrot<float>::operator=(a_from);
return *this;
}
public:
rotf& operator*=(const rotf& a_q) {
qrot<float>::operator*=(a_q);
return *this;
}
rotf operator*(const rotf& a_r) const {
rotf tmp(*this);
tmp *= a_r;
return tmp;
}
public:
bool set_value(const vec3f& a_from,const vec3f& a_to){
return qrot<float>::set_value(a_from,a_to);
}
bool set_value(const vec3f& a_from,float a_a){
return qrot<float>::set_value(a_from,a_a);
}
//NOTE : don't handle a static object because of mem balance.
//static const rotf& identity() {
// static const rotf s_v(0,0,0,1);
// return s_v;
//}
};
}
#include <sstream>
namespace tools {
inline bool tos(const rotf& a_v,std::string& a_s) {
vec3f axis;
float angle;
if(!a_v.value(axis,angle)) {a_s.clear();return false;}
std::ostringstream strm;
strm << axis[0] << " "
<< axis[1] << " "
<< axis[2] << " "
<< angle;
a_s = strm.str();
return true;
}
}
#endif
source/analysis/g4tools/include/tools/lina/vec2
+219
View File
@@ -0,0 +1,219 @@
// Copyright (C) 2010, Guy Barrand. All rights reserved.
// See the file tools.license for terms.
#ifndef tools_vec2
#define tools_vec2
#include <cmath> //sqrt
#ifdef TOOLS_MEM
#include "mem"
#endif
namespace tools {
template <class T>
class vec2 {
#ifdef TOOLS_MEM
static const std::string& s_class() {
static const std::string s_v("tools::vec2");
return s_v;
}
#endif
public:
unsigned int dimension() const {return 2;}
public:
vec2(){
#ifdef TOOLS_MEM
mem::increment(s_class().c_str());
#endif
m_data[0] = T();
m_data[1] = T();
}
vec2(const T a_vec[2]) {
#ifdef TOOLS_MEM
mem::increment(s_class().c_str());
#endif
m_data[0] = a_vec[0];
m_data[1] = a_vec[1];
}
vec2(const T& a0,const T& a1) {
#ifdef TOOLS_MEM
mem::increment(s_class().c_str());
#endif
m_data[0] = a0;
m_data[1] = a1;
}
virtual ~vec2() {
#ifdef TOOLS_MEM
mem::decrement(s_class().c_str());
#endif
}
public:
vec2(const vec2& a_from){
#ifdef TOOLS_MEM
mem::increment(s_class().c_str());
#endif
m_data[0] = a_from.m_data[0];
m_data[1] = a_from.m_data[1];
}
vec2& operator=(const vec2& a_from) {
m_data[0] = a_from.m_data[0];
m_data[1] = a_from.m_data[1];
return *this;
}
public:
T v0() const { return m_data[0];}
T v1() const { return m_data[1];}
void v0(const T& a_value) { m_data[0] = a_value;}
void v1(const T& a_value) { m_data[1] = a_value;}
T x() const {return m_data[0];}
T y() const {return m_data[1];}
void set_value(const T& a0,const T& a1) {
m_data[0] = a0;
m_data[1] = a1;
}
void set_value(const T aV[2]) {
m_data[0] = aV[0];
m_data[1] = aV[1];
}
void value(T& a0,T& a1) const {
a0 = m_data[0];
a1 = m_data[1];
}
//bool set_value(unsigned int a_index,const T& a_value) {
// if(a_index>=2) return false;
// m_[a_index] = a_value;
// return true;
//}
T length() const {
return (T)::sqrt(m_data[0]*m_data[0]+m_data[1]*m_data[1]);
}
T normalize() {
T norme = length();
if(norme==T()) return T();
divide(norme);
return norme;
}
T dot(const vec2& aV) const {
return (m_data[0] * aV.m_data[0] +
m_data[1] * aV.m_data[1]);
}
T cross(const vec2& aV) const {
return (m_data[0] * aV.m_data[1] - m_data[1] * aV.m_data[0]);
}
bool equal(const vec2& aV) const {
if(m_data[0]!=aV.m_data[0]) return false;
if(m_data[1]!=aV.m_data[1]) return false;
return true;
}
bool divide(const T& a_T) {
if(a_T==T()) return false;
m_data[0] /= a_T;
m_data[1] /= a_T;
return true;
}
void add(const vec2& a_v) {
m_data[0] += a_v.m_data[0];
m_data[1] += a_v.m_data[1];
}
void add(const T& a0,const T& a1) {
m_data[0] += a0;
m_data[1] += a1;
}
void subtract(const vec2& a_v) {
m_data[0] -= a_v.m_data[0];
m_data[1] -= a_v.m_data[1];
}
void subtract(const T& a0,const T& a1) {
m_data[0] -= a0;
m_data[1] -= a1;
}
public: //operators
T& operator[](unsigned int a_index) {
//WARNING : no check on a_index.
return m_data[a_index];
}
const T& operator[](unsigned int a_index) const {
//WARNING : no check on a_index.
return m_data[a_index];
}
vec2 operator+(const vec2& a_v) const {
return vec2(m_data[0]+a_v.m_data[0],
m_data[1]+a_v.m_data[1]);
}
vec2 operator-(const vec2& a_v) const {
return vec2(m_data[0]-a_v.m_data[0],
m_data[1]-a_v.m_data[1]);
}
vec2 operator*(const T& a_v) const {
return vec2(m_data[0]*a_v,
m_data[1]*a_v);
}
bool operator==(const vec2& a_v) const {return equal(a_v);}
bool operator!=(const vec2& a_v) const {return !operator==(a_v);}
public: //for inlib/sg/sf_vec
typedef unsigned int size_type;
size_type size() const {return 2;}
const T* data() const {return m_data;}
public: //for iv2sg
const T* getValue() const {return m_data;}
void getValue(T& a0,T& a1) const {
a0 = m_data[0];
a1 = m_data[1];
}
void setValue(const T& a0,const T& a1) {
m_data[0] = a0;
m_data[1] = a1;
}
void setValue(const T aV[2]) {
m_data[0] = aV[0];
m_data[1] = aV[1];
}
protected:
T m_data[2];
private:static void check_instantiation() {vec2<float> v;}
};
//for sf, mf :
template <class T>
inline const T* get_data(const vec2<T>& a_v) {return a_v.data();}
}
#include <ostream>
namespace tools {
// for sf_vec::dump().
template <class T>
inline std::ostream& operator<<(std::ostream& a_out,const vec2<T>& a_this){
a_out << "x = " << a_this.v0()
<< ",y = " << a_this.v1();
return a_out;
}
}
#endif
source/analysis/g4tools/include/tools/lina/vec2f
+95
View File
@@ -0,0 +1,95 @@
// Copyright (C) 2010, Guy Barrand. All rights reserved.
// See the file tools.license for terms.
#ifndef tools_vec2f
#define tools_vec2f
#include "vec2"
#include "../S_STRING"
namespace tools {
class vec2f : public vec2<float> {
typedef vec2<float> parent;
public:
TOOLS_SCLASS(tools::vec2f) //for stype()
public:
vec2f():parent(){}
vec2f(const float a_vec[2]):parent(a_vec){}
vec2f(float a0,float a1):parent(a0,a1){}
virtual ~vec2f() {}
public:
vec2f(const vec2f& a_from): parent(a_from){}
vec2f& operator=(const vec2f& a_from){
parent::operator=(a_from);
return *this;
}
vec2f(const parent& a_from):parent(a_from){}
public: //operators
vec2f operator*(float a_v) const {
return vec2f(m_data[0]*a_v,
m_data[1]*a_v);
}
vec2f operator+(const vec2f& a_v) const {
return vec2f(m_data[0]+a_v.m_data[0],
m_data[1]+a_v.m_data[1]);
}
vec2f operator-(const vec2f& a_v) const {
return vec2f(m_data[0]-a_v.m_data[0],
m_data[1]-a_v.m_data[1]);
}
vec2f& operator+=(const vec2f& a_v) {
m_data[0] += a_v.m_data[0];
m_data[1] += a_v.m_data[1];
return *this;
}
vec2f& operator*=(float a_v) {
m_data[0] *= a_v;
m_data[1] *= a_v;
return *this;
}
vec2f operator-() const {
return vec2f(-m_data[0],-m_data[1]);
}
public: //iv2sg
bool equals(const vec2f& a_v,const float a_epsil) const {
//if(a_epsil<0.0f))
float d0 = m_data[0]-a_v.m_data[0];
float d1 = m_data[1]-a_v.m_data[1];
return ((d0*d0+d1*d1)<=a_epsil);
}
void negate() {
m_data[0] = -m_data[0];
m_data[1] = -m_data[1];
}
private:static void check_instantiation() {vec2f v(0,0);v.set_value(1,1);}
};
inline vec2f operator*(float a_f,const vec2f& a_v) {
vec2f res(a_v);
res *= a_f;
return res;
}
}
#include <vector>
namespace tools {
#ifndef SWIG
//for sf, mf :
inline bool set_from_vec(vec2f& a_v,const std::vector<float>& a_sv) {
if(a_sv.size()!=2) return false;
a_v[0] = a_sv[0];
a_v[1] = a_sv[1];
return true;
}
#endif
}
#endif
source/analysis/g4tools/include/tools/lina/vec3
+342
View File
@@ -0,0 +1,342 @@
// Copyright (C) 2010, Guy Barrand. All rights reserved.
// See the file tools.license for terms.
#ifndef tools_vec3
#define tools_vec3
#include <cmath> //sqrt
#ifdef TOOLS_MEM
#include "mem"
#endif
namespace tools {
template <class T>
class vec3 {
#ifdef TOOLS_MEM
static const std::string& s_class() {
static const std::string s_v("tools::vec3");
return s_v;
}
#endif
public:
typedef T elem_t;
unsigned int dimension() const {return 3;}
public:
vec3(){
#ifdef TOOLS_MEM
mem::increment(s_class().c_str());
#endif
m_data[0] = T();
m_data[1] = T();
m_data[2] = T();
}
vec3(const T a_vec[3]) {
#ifdef TOOLS_MEM
mem::increment(s_class().c_str());
#endif
m_data[0] = a_vec[0];
m_data[1] = a_vec[1];
m_data[2] = a_vec[2];
}
vec3(const T& a0,const T& a1,const T& a2,bool a_inc = true) {
#ifdef TOOLS_MEM
if(a_inc) mem::increment(s_class().c_str());
#else
(void)a_inc;
#endif
m_data[0] = a0;
m_data[1] = a1;
m_data[2] = a2;
}
virtual ~vec3() {
#ifdef TOOLS_MEM
mem::decrement(s_class().c_str());
#endif
}
public:
vec3(const vec3& a_from){
#ifdef TOOLS_MEM
mem::increment(s_class().c_str());
#endif
m_data[0] = a_from.m_data[0];
m_data[1] = a_from.m_data[1];
m_data[2] = a_from.m_data[2];
}
vec3& operator=(const vec3& a_from) {
m_data[0] = a_from.m_data[0];
m_data[1] = a_from.m_data[1];
m_data[2] = a_from.m_data[2];
return *this;
}
public:
T v0() const { return m_data[0];}
T v1() const { return m_data[1];}
T v2() const { return m_data[2];}
void v0(const T& a_value) { m_data[0] = a_value;}
void v1(const T& a_value) { m_data[1] = a_value;}
void v2(const T& a_value) { m_data[2] = a_value;}
T x() const {return m_data[0];}
T y() const {return m_data[1];}
T z() const {return m_data[2];}
void set_value(const T& a0,const T& a1,const T& a2) {
m_data[0] = a0;
m_data[1] = a1;
m_data[2] = a2;
}
void set_value(const T aV[3]) {
m_data[0] = aV[0];
m_data[1] = aV[1];
m_data[2] = aV[2];
}
void value(T& a0,T& a1,T& a2) const {
a0 = m_data[0];
a1 = m_data[1];
a2 = m_data[2];
}
//bool set_value(unsigned int a_index,const T& a_value) {
// if(a_index>=3) return false;
// m_[a_index] = a_value;
// return true;
//}
T length() const {
return (T)::sqrt(m_data[0]*m_data[0]
+m_data[1]*m_data[1]
+m_data[2]*m_data[2]);
}
T normalize() {
T norme = length();
if(norme==T()) return T();
divide(norme);
return norme;
}
T dot(const vec3& aV) const {
return (m_data[0] * aV.m_data[0] +
m_data[1] * aV.m_data[1] +
m_data[2] * aV.m_data[2]);
}
vec3<T> cross(const vec3<T>& aV) const {
return vec3<T>(m_data[1] * aV.m_data[2] - m_data[2] * aV.m_data[1],
m_data[2] * aV.m_data[0] - m_data[0] * aV.m_data[2],
m_data[0] * aV.m_data[1] - m_data[1] * aV.m_data[0]);
}
bool equal(const vec3& aV) const {
if(m_data[0]!=aV.m_data[0]) return false;
if(m_data[1]!=aV.m_data[1]) return false;
if(m_data[2]!=aV.m_data[2]) return false;
return true;
}
bool divide(const T& a_T) {
if(a_T==T()) return false;
m_data[0] /= a_T;
m_data[1] /= a_T;
m_data[2] /= a_T;
return true;
}
void multiply(const T& a_T) {
m_data[0] *= a_T;
m_data[1] *= a_T;
m_data[2] *= a_T;
}
void add(const vec3& a_v) {
m_data[0] += a_v.m_data[0];
m_data[1] += a_v.m_data[1];
m_data[2] += a_v.m_data[2];
}
void add(const T& a0,const T& a1,const T& a2) {
m_data[0] += a0;
m_data[1] += a1;
m_data[2] += a2;
}
void subtract(const vec3& a_v) {
m_data[0] -= a_v.m_data[0];
m_data[1] -= a_v.m_data[1];
m_data[2] -= a_v.m_data[2];
}
void subtract(const T& a0,const T& a1,const T& a2) {
m_data[0] -= a0;
m_data[1] -= a1;
m_data[2] -= a2;
}
bool cos_angle(const vec3& a_v,T& a_cos) const {
//WARNING : if ret false, a_cos is not set.
if(length()==T()) return false;
if(a_v.length()==T()) return false;
a_cos = dot(a_v)/(length()*a_v.length());
return true;
}
bool theta_phi(T& a_theta,T& a_phi) const {
//WARNING : if ret false, a_theta, a_phi are not set.
if(length()==T()) return false;
a_phi = (T)::atan2(m_data[1],m_data[0]);
T xy = (T)::sqrt(m_data[0]*m_data[0]+m_data[1]*m_data[1]);
a_theta = (T)::atan2(xy,m_data[2]);
return true;
}
public: //operators
T& operator[](unsigned int a_index) {
//WARNING : no check on a_index.
return m_data[a_index];
}
const T& operator[](unsigned int a_index) const {
//WARNING : no check on a_index.
return m_data[a_index];
}
vec3& operator*=(const T& a_v) {
m_data[0] *= a_v;
m_data[1] *= a_v;
m_data[2] *= a_v;
return *this;
}
vec3 operator+(const vec3& a_v) const {
return vec3(m_data[0]+a_v.m_data[0],
m_data[1]+a_v.m_data[1],
m_data[2]+a_v.m_data[2]);
}
vec3 operator-(const vec3& a_v) const {
return vec3(m_data[0]-a_v.m_data[0],
m_data[1]-a_v.m_data[1],
m_data[2]-a_v.m_data[2]);
}
vec3 operator*(const T& a_v) const {
return vec3(m_data[0]*a_v,
m_data[1]*a_v,
m_data[2]*a_v);
}
vec3 operator/(const T& a_v) const {
if(a_v==T()) return vec3();
return vec3(m_data[0]/a_v,
m_data[1]/a_v,
m_data[2]/a_v);
}
bool operator==(const vec3& a_v) const {return equal(a_v);}
bool operator!=(const vec3& a_v) const {return !operator==(a_v);}
public: //for inlib/sg/sf_vec
typedef unsigned int size_type;
size_type size() const {return 3;}
const T* data() const {return m_data;}
public: //for iv2sg
const T* getValue() const {return m_data;}
void setValue(const T& a0,const T& a1,const T& a2) {
m_data[0] = a0;
m_data[1] = a1;
m_data[2] = a2;
}
void getValue(T& a0,T& a1,T& a2) const {
a0 = m_data[0];
a1 = m_data[1];
a2 = m_data[2];
}
void setValue(const vec3& a_v) {
m_data[0] = a_v.m_data[0];
m_data[1] = a_v.m_data[1];
m_data[2] = a_v.m_data[2];
}
void setValue(const T aV[3]) {
m_data[0] = aV[0];
m_data[1] = aV[1];
m_data[2] = aV[2];
}
vec3& setValue(const vec3& a_bary,
const vec3& a_v0,const vec3& a_v1,const vec3& a_v2) {
m_data[0] = a_bary[0]*a_v0[0]+a_bary[1]*a_v1[0]+a_bary[2]*a_v2[0];
m_data[1] = a_bary[0]*a_v0[1]+a_bary[1]*a_v1[1]+a_bary[2]*a_v2[1];
m_data[2] = a_bary[0]*a_v0[2]+a_bary[1]*a_v1[2]+a_bary[2]*a_v2[2];
return *this;
}
public:
static const vec3<T>& s_x() {
static const vec3<T> s_v(1,0,0,false);
return s_v;
}
static const vec3<T>& s_y() {
static const vec3<T> s_v(0,1,0,false);
return s_v;
}
static const vec3<T>& s_z() {
static const vec3<T> s_v(0,0,1,false);
return s_v;
}
protected:
T m_data[3];
private:static void check_instantiation() {vec3<float> v;}
};
//for sf, mf :
template <class T>
inline const T* get_data(const vec3<T>& a_v) {return a_v.data();}
template <class T>
inline void normal(const vec3<T>& a_p0,const vec3<T>& a_p1,const vec3<T>& a_p2,vec3<T>& a_nm) {
a_nm = (a_p1-a_p0).cross(a_p2-a_p1);
a_nm.normalize();
}
template <class T>
inline vec3<T> direction(const vec3<T>& a_p0,
const vec3<T>& a_p1,
const vec3<T>& a_p2) {
// Orientation is computed by taking (p1 - p0) x (p2 - p0)
vec3<T> P = a_p1;
P.subtract(a_p0);
vec3<T> P2 = a_p2;
P2.subtract(a_p0);
return P.cross(P2);
}
template <class T>
inline vec3<T> direction(const T& a_0_x,const T& a_0_y,const T& a_0_z,
const T& a_1_x,const T& a_1_y,const T& a_1_z,
const T& a_2_x,const T& a_2_y,const T& a_2_z) {
return direction(vec3<T>(a_0_x,a_0_y,a_0_z),
vec3<T>(a_1_x,a_1_y,a_1_z),
vec3<T>(a_2_x,a_2_y,a_2_z));
}
}
#include <ostream>
namespace tools {
// for sf_vec::dump().
template <class T>
inline std::ostream& operator<<(std::ostream& a_out,const vec3<T>& a_this){
a_out << "x = " << a_this.v0()
<< ",y = " << a_this.v1()
<< ",z = " << a_this.v2();
return a_out;
}
}
#endif
source/analysis/g4tools/include/tools/lina/vec3d
+96
View File
@@ -0,0 +1,96 @@
// Copyright (C) 2010, Guy Barrand. All rights reserved.
// See the file tools.license for terms.
#ifndef tools_vec3d
#define tools_vec3d
#include "vec3"
#include "../S_STRING"
namespace tools {
class vec3d : public vec3<double> {
public:
TOOLS_SCLASS(tools::vec3d) //for stype()
public:
vec3d():vec3<double>(){}
vec3d(const double a_vec[3]):vec3<double>(a_vec){}
vec3d(double a0,double a1,double a2):vec3<double>(a0,a1,a2){}
virtual ~vec3d() {}
public:
vec3d(const vec3d& a_from):vec3<double>(a_from){}
vec3d& operator=(const vec3d& a_from){
vec3<double>::operator=(a_from);
return *this;
}
vec3d(const vec3<double>& a_from):vec3<double>(a_from){}
public: //operators
vec3d operator*(double a_v) const {
return vec3d(m_data[0]*a_v,
m_data[1]*a_v,
m_data[2]*a_v);
}
vec3d operator+(const vec3d& a_v) const {
return vec3d(m_data[0]+a_v.m_data[0],
m_data[1]+a_v.m_data[1],
m_data[2]+a_v.m_data[2]);
}
vec3d operator-(const vec3d& a_v) const {
return vec3d(m_data[0]-a_v.m_data[0],
m_data[1]-a_v.m_data[1],
m_data[2]-a_v.m_data[2]);
}
vec3d& operator+=(const vec3d& a_v) {
m_data[0] += a_v.m_data[0];
m_data[1] += a_v.m_data[1];
m_data[2] += a_v.m_data[2];
return *this;
}
vec3d& operator-=(const vec3d& a_v) {
m_data[0] -= a_v.m_data[0];
m_data[1] -= a_v.m_data[1];
m_data[2] -= a_v.m_data[2];
return *this;
}
vec3d& operator*=(double a_v) {
m_data[0] *= a_v;
m_data[1] *= a_v;
m_data[2] *= a_v;
return *this;
}
vec3d operator-() const {
return vec3d(-m_data[0],-m_data[1],-m_data[2]);
}
private:static void check_instantiation() {vec3d v(0,0,0);v.set_value(1,1,1);}
};
inline vec3d operator*(double a_f,const vec3d& a_v) {
vec3d res(a_v);
res *= a_f;
return res;
}
}
#include <vector>
namespace tools {
#ifndef SWIG
//for sf, mf :
inline bool set_from_vec(vec3d& a_v,const std::vector<double>& a_sv) {
if(a_sv.size()!=3) return false;
a_v[0] = a_sv[0];
a_v[1] = a_sv[1];
a_v[2] = a_sv[2];
return true;
}
#endif
}
#endif
source/analysis/g4tools/include/tools/lina/vec3f
+109
View File
@@ -0,0 +1,109 @@
// Copyright (C) 2010, Guy Barrand. All rights reserved.
// See the file tools.license for terms.
#ifndef tools_vec3f
#define tools_vec3f
#include "vec3"
#include "../S_STRING"
namespace tools {
class vec3f : public vec3<float> {
public:
TOOLS_SCLASS(tools::vec3f) //for stype()
public:
vec3f():vec3<float>(){}
vec3f(const float a_vec[3]):vec3<float>(a_vec){}
vec3f(float a0,float a1,float a2):vec3<float>(a0,a1,a2){}
virtual ~vec3f() {}
public:
vec3f(const vec3f& a_from):vec3<float>(a_from){}
vec3f& operator=(const vec3f& a_from){
vec3<float>::operator=(a_from);
return *this;
}
vec3f(const vec3<float>& a_from):vec3<float>(a_from){}
public: //operators
vec3f operator*(float a_v) const {
return vec3f(m_data[0]*a_v,
m_data[1]*a_v,
m_data[2]*a_v);
}
vec3f operator+(const vec3f& a_v) const {
return vec3f(m_data[0]+a_v.m_data[0],
m_data[1]+a_v.m_data[1],
m_data[2]+a_v.m_data[2]);
}
vec3f operator-(const vec3f& a_v) const {
return vec3f(m_data[0]-a_v.m_data[0],
m_data[1]-a_v.m_data[1],
m_data[2]-a_v.m_data[2]);
}
vec3f& operator+=(const vec3f& a_v) {
m_data[0] += a_v.m_data[0];
m_data[1] += a_v.m_data[1];
m_data[2] += a_v.m_data[2];
return *this;
}
vec3f& operator-=(const vec3f& a_v) {
m_data[0] -= a_v.m_data[0];
m_data[1] -= a_v.m_data[1];
m_data[2] -= a_v.m_data[2];
return *this;
}
vec3f& operator*=(float a_v) {
m_data[0] *= a_v;
m_data[1] *= a_v;
m_data[2] *= a_v;
return *this;
}
vec3f operator-() const {
return vec3f(-m_data[0],-m_data[1],-m_data[2]);
}
public: //iv2sg
bool equals(const vec3f& a_v,const float a_epsil) const {
//if(a_epsil<0.0f))
float d0 = m_data[0]-a_v.m_data[0];
float d1 = m_data[1]-a_v.m_data[1];
float d2 = m_data[2]-a_v.m_data[2];
return ((d0*d0+d1*d1+d2*d2)<=a_epsil);
}
void negate() {
m_data[0] = -m_data[0];
m_data[1] = -m_data[1];
m_data[2] = -m_data[2];
}
private:static void check_instantiation() {vec3f v(0,0,0);v.set_value(1,1,1);}
};
inline vec3f operator*(float a_f,const vec3f& a_v) {
vec3f res(a_v);
res *= a_f;
return res;
}
}
#include <vector>
namespace tools {
#ifndef SWIG
//for sf, mf :
inline bool set_from_vec(vec3f& a_v,const std::vector<float>& a_sv) {
if(a_sv.size()!=3) return false;
a_v[0] = a_sv[0];
a_v[1] = a_sv[1];
a_v[2] = a_sv[2];
return true;
}
#endif
}
#endif
source/analysis/g4tools/include/tools/lina/vec4
+286
View File
@@ -0,0 +1,286 @@
// Copyright (C) 2010, Guy Barrand. All rights reserved.
// See the file tools.license for terms.
#ifndef tools_vec4
#define tools_vec4
#include <cmath> //sqrt
#ifdef TOOLS_MEM
#include "mem"
#endif
namespace tools {
template <class T>
class vec4 {
#ifdef TOOLS_MEM
static const std::string& s_class() {
static const std::string s_v("tools::vec4");
return s_v;
}
#endif
protected:
static T zero() {return T();}
static T minus_one() {return T(-1);}
public:
unsigned int dimension() const {return 4;}
public:
vec4(){
#ifdef TOOLS_MEM
mem::increment(s_class().c_str());
#endif
m_data[0] = T();
m_data[1] = T();
m_data[2] = T();
m_data[3] = T();
}
vec4(const T a_vec[4]) {
#ifdef TOOLS_MEM
mem::increment(s_class().c_str());
#endif
m_data[0] = a_vec[0];
m_data[1] = a_vec[1];
m_data[2] = a_vec[2];
m_data[3] = a_vec[3];
}
vec4(const T& a0,const T& a1,const T& a2,const T& a3,bool a_inc = true) {
#ifdef TOOLS_MEM
if(a_inc) mem::increment(s_class().c_str());
#else
(void)a_inc;
#endif
m_data[0] = a0;
m_data[1] = a1;
m_data[2] = a2;
m_data[3] = a3;
}
virtual ~vec4() {
#ifdef TOOLS_MEM
mem::decrement(s_class().c_str());
#endif
}
public:
vec4(const vec4& a_from){
#ifdef TOOLS_MEM
mem::increment(s_class().c_str());
#endif
m_data[0] = a_from.m_data[0];
m_data[1] = a_from.m_data[1];
m_data[2] = a_from.m_data[2];
m_data[3] = a_from.m_data[3];
}
vec4& operator=(const vec4& a_from) {
m_data[0] = a_from.m_data[0];
m_data[1] = a_from.m_data[1];
m_data[2] = a_from.m_data[2];
m_data[3] = a_from.m_data[3];
return *this;
}
public:
T v0() const { return m_data[0];}
T v1() const { return m_data[1];}
T v2() const { return m_data[2];}
T v3() const { return m_data[3];}
void v0(const T& a_value) { m_data[0] = a_value;}
void v1(const T& a_value) { m_data[1] = a_value;}
void v2(const T& a_value) { m_data[2] = a_value;}
void v3(const T& a_value) { m_data[3] = a_value;}
//T x() const {return m_data[0];}
//T y() const {return m_data[1];}
//T z() const {return m_data[2];}
void set_value(const T& a0,const T& a1,const T& a2,const T& a3) {
m_data[0] = a0;
m_data[1] = a1;
m_data[2] = a2;
m_data[3] = a3;
}
void set_value(const T aV[4]) {
m_data[0] = aV[0];
m_data[1] = aV[1];
m_data[2] = aV[2];
m_data[3] = aV[3];
}
void value(T& a0,T& a1,T& a2,T& a3) const {
a0 = m_data[0];
a1 = m_data[1];
a2 = m_data[2];
a3 = m_data[3];
}
bool set_value(unsigned int a_index,const T& a_value) {
if(a_index>=4) return false;
m_data[a_index] = a_value;
return true;
}
T length() const {
return (T)::sqrt(m_data[0]*m_data[0]
+m_data[1]*m_data[1]
+m_data[2]*m_data[2]
+m_data[3]*m_data[3]);
}
T normalize() {
T norme = length();
if(norme==T()) return T();
divide(norme);
return norme;
}
bool equal(const vec4& a_vec) const {
if(m_data[0]!=a_vec.m_data[0]) return false;
if(m_data[1]!=a_vec.m_data[1]) return false;
if(m_data[2]!=a_vec.m_data[2]) return false;
if(m_data[3]!=a_vec.m_data[3]) return false;
return true;
}
bool equal(const vec4& a_vec,const T& a_epsil) const {
T* tp = (T*)m_data;
T* ap = (T*)a_vec.m_data;
for(unsigned int i=0;i<4;i++,tp++,ap++) {
T diff = (*tp) - (*ap);
if(diff<zero()) diff *= minus_one();
if(diff>=a_epsil) return false;
}
return true;
}
bool is_proportional(const vec4& a_vec,T& a_factor) const {
// If true, then : a_vec = a_factor * this.
a_factor = zero();
bool first = true;
T* tp = (T*)m_data;
T* ap = (T*)a_vec.m_data;
for(unsigned int i=0;i<4;i++,tp++,ap++) {
if( ((*tp)==zero()) && ((*ap)==zero())) {
continue;
} else if( ((*tp)!=zero()) && ((*ap)==zero())) {
return false;
} else if( ((*tp)==zero()) && ((*ap)!=zero())) {
return false;
} else {
if(first) {
a_factor = (*ap)/(*tp);
first = false;
} else {
if((*ap)!=(*tp)*a_factor) return false;
}
}
}
return true;
}
void multiply(const T& a_T) {
m_data[0] *= a_T;
m_data[1] *= a_T;
m_data[2] *= a_T;
m_data[3] *= a_T;
}
bool divide(const T& a_T) {
if(a_T==T()) return false;
m_data[0] /= a_T;
m_data[1] /= a_T;
m_data[2] /= a_T;
m_data[3] /= a_T;
return true;
}
void add(const vec4& a_v) {
m_data[0] += a_v.m_data[0];
m_data[1] += a_v.m_data[1];
m_data[2] += a_v.m_data[2];
m_data[3] += a_v.m_data[3];
}
void add(const T& a0,const T& a1,const T& a2,const T& a3) {
m_data[0] += a0;
m_data[1] += a1;
m_data[2] += a2;
m_data[3] += a3;
}
void subtract(const vec4& a_v) {
m_data[0] -= a_v.m_data[0];
m_data[1] -= a_v.m_data[1];
m_data[2] -= a_v.m_data[2];
m_data[3] -= a_v.m_data[3];
}
void subtract(const T& a0,const T& a1,const T& a2,const T& a3) {
m_data[0] -= a0;
m_data[1] -= a1;
m_data[2] -= a2;
m_data[3] -= a3;
}
public: //operators
T& operator[](unsigned int a_index) {
//WARNING : no check on a_index.
return m_data[a_index];
}
const T& operator[](unsigned int a_index) const {
//WARNING : no check on a_index.
return m_data[a_index];
}
vec4 operator+(const vec4& a_v) const {
return vec4(m_data[0]+a_v.m_data[0],
m_data[1]+a_v.m_data[1],
m_data[2]+a_v.m_data[2],
m_data[3]+a_v.m_data[3]);
}
vec4 operator-(const vec4& a_v) const {
return vec4(m_data[0]-a_v.m_data[0],
m_data[1]-a_v.m_data[1],
m_data[2]-a_v.m_data[2],
m_data[3]-a_v.m_data[3]);
}
vec4 operator*(const T& a_v) const {
return vec4(m_data[0]*a_v,
m_data[1]*a_v,
m_data[2]*a_v,
m_data[3]*a_v);
}
bool operator==(const vec4& a_v) const {return equal(a_v);}
bool operator!=(const vec4& a_v) const {return !operator==(a_v);}
public: //for inlib/sg/sf_vec
typedef unsigned int size_type;
size_type size() const {return 4;}
const T* data() const {return m_data;}
protected:
T m_data[4];
private:static void check_instantiation() {vec4<float> v;}
};
//for sf, mf :
template <class T>
inline const T* get_data(const vec4<T>& a_v) {return a_v.data();}
}
#include <ostream>
namespace tools {
// for sf_vec::dump().
template <class T>
inline std::ostream& operator<<(std::ostream& a_out,const vec4<T>& a_this){
a_out << "x = " << a_this.v0()
<< ",y = " << a_this.v1()
<< ",z = " << a_this.v2()
<< ",t = " << a_this.v3();
return a_out;
}
}
#endif
source/analysis/g4tools/include/tools/lina/vec4f
+85
View File
@@ -0,0 +1,85 @@
// Copyright (C) 2010, Guy Barrand. All rights reserved.
// See the file tools.license for terms.
#ifndef tools_vec4f
#define tools_vec4f
#include "vec4"
#include "../S_STRING"
namespace tools {
class vec4f : public vec4<float> {
public:
TOOLS_SCLASS(tools::vec4f) //for stype()
public:
vec4f():vec4<float>() {}
vec4f(const float a_vec[4]):vec4<float>(a_vec) {}
vec4f(const float& a0,const float& a1,const float& a2,const float& a3)
:vec4<float>(a0,a1,a2,a3){}
virtual ~vec4f() {}
public:
vec4f(const vec4f& a_from):vec4<float>(a_from){}
vec4f& operator=(const vec4f& a_from){
vec4<float>::operator=(a_from);
return *this;
}
public: //operators
vec4f operator*(float a_v) const {
return vec4f(m_data[0]*a_v,
m_data[1]*a_v,
m_data[2]*a_v,
m_data[3]*a_v);
}
vec4f operator+(const vec4f& a_v) const {
return vec4f(m_data[0]+a_v.m_data[0],
m_data[1]+a_v.m_data[1],
m_data[2]+a_v.m_data[2],
m_data[3]+a_v.m_data[3]);
}
vec4f operator-(const vec4f& a_v) const {
return vec4f(m_data[0]-a_v.m_data[0],
m_data[1]-a_v.m_data[1],
m_data[2]-a_v.m_data[2],
m_data[3]-a_v.m_data[3]);
}
vec4f& operator+=(const vec4f& a_v) {
m_data[0] += a_v.m_data[0];
m_data[1] += a_v.m_data[1];
m_data[2] += a_v.m_data[2];
m_data[3] += a_v.m_data[3];
return *this;
}
vec4f& operator*=(float a_v) {
m_data[0] *= a_v;
m_data[1] *= a_v;
m_data[2] *= a_v;
m_data[3] *= a_v;
return *this;
}
vec4f operator-() const {
return vec4f(-m_data[0],-m_data[1],-m_data[2],-m_data[3]);
}
};
}
#include <vector>
namespace tools {
#ifndef SWIG
//for sf, mf :
inline bool set_from_vec(vec4f& a_v,const std::vector<float>& a_sv) {
if(a_sv.size()!=4) return false;
a_v[0] = a_sv[0];
a_v[1] = a_sv[1];
a_v[2] = a_sv[2];
a_v[3] = a_sv[3];
return true;
}
#endif
}
#endif