Gabriele Cosmo
237 lines
7.6 KiB
Plaintext
237 lines
7.6 KiB
Plaintext
// Copyright (C) 2010, Guy Barrand. All rights reserved.
|
|
// See the file tools.license for terms.
|
|
|
|
#ifndef tools_mat3
|
|
#define tools_mat3
|
|
|
|
#include "mat"
|
|
|
|
//#include <cmath>
|
|
|
|
namespace tools {
|
|
|
|
template <class T>
|
|
class mat3 : public mat<T,3> {
|
|
typedef mat<T,3> parent;
|
|
typedef mat<T,3> pr;
|
|
public:
|
|
#ifdef TOOLS_MEM
|
|
mat3(bool a_inc = true):parent(a_inc) {}
|
|
#else
|
|
mat3():parent() {}
|
|
#endif
|
|
mat3(const mat<T,3>& a_from):parent(a_from){}
|
|
virtual ~mat3() {}
|
|
public:
|
|
mat3(const mat3& a_from):parent(a_from){}
|
|
mat3& operator=(const mat3& a_from){
|
|
parent::operator=(a_from);
|
|
return *this;
|
|
}
|
|
public:
|
|
mat3(const T& a_00,const T& a_01,const T& a_02, //first row
|
|
const T& a_10,const T& a_11,const T& a_12, //second row
|
|
const T& a_20,const T& a_21,const T& a_22) //third row
|
|
{
|
|
set_matrix(a_00,a_01,a_02,
|
|
a_10,a_11,a_12,
|
|
a_20,a_21,a_22);
|
|
}
|
|
public:
|
|
void set_matrix(const mat3<T>& a_m){parent::set_matrix(a_m);}
|
|
void set_matrix(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
|
|
//a_<R><C>
|
|
//pr::m_vec[R + C * 3];
|
|
pr::m_vec[0] = a_00;pr::m_vec[3] = a_01;pr::m_vec[6] = a_02;
|
|
pr::m_vec[1] = a_10;pr::m_vec[4] = a_11;pr::m_vec[7] = a_12;
|
|
pr::m_vec[2] = a_20;pr::m_vec[5] = a_21;pr::m_vec[8] = a_22;
|
|
}
|
|
|
|
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_rotate(const T& a_x,const T& a_y,const T& a_z,const T& a_angle,T(*a_sin)(T),T(*a_cos)(T)) {
|
|
_set_rotate(a_x,a_y,a_z,a_angle,pr::m_vec,a_sin,a_cos);
|
|
}
|
|
|
|
bool get_rotate(T& a_x,T& a_y,T& a_z,T& a_angle,T(*a_acos)(T),T(*a_sin)(T),T(*a_sqrt)(T)) { //warning : acos and not cos.
|
|
// used in tests/check_pauli. Same code as matTs/get_rotate.
|
|
// we assume that we have a rotation matrix.
|
|
// get (a_angle,a_n(x,y,z)) such that matrix is exp(a_angle*a_n*Rs) (passive(=coord) rotation) and then :
|
|
// exp(a_angle*n*Rs) = cos_angle*I3+(1-cos_angle)*n*n+sin_theta*n*Rs
|
|
// trace = 3*cos_angle+(1-cos_angle)*n2
|
|
T cos_angle = T(0.5)*(pr::m_vec[0]+pr::m_vec[4]+pr::m_vec[8]-T(1));
|
|
if((cos_angle<T(-1))||(T(1)<cos_angle)) { //matrix is not a rotation :
|
|
a_angle = T(0);
|
|
a_x = 0;
|
|
a_y = 0;
|
|
a_z = 0;
|
|
return false;
|
|
}
|
|
if(cos_angle==T(1)) {
|
|
a_angle = T(0);
|
|
a_x = 0;
|
|
a_y = 0;
|
|
a_z = 1;
|
|
return true; //vector is undefined but we return true anyway.
|
|
}
|
|
T one_minus_cos_angle = T(1)-cos_angle;
|
|
T x2 = (pr::m_vec[0]-cos_angle)/one_minus_cos_angle;
|
|
//T y2 = (pr::m_vec[4]-cos_angle)/one_minus_cos_angle;
|
|
//T z2 = (pr::m_vec[8]-cos_angle)/one_minus_cos_angle;
|
|
/*
|
|
if((x2<T(0))||(y2<T(0))||(z2<T(0))||(x2+y2+z2)!=T(1)) { //not a rotation :
|
|
a_angle = T(0);
|
|
a_x = 0;
|
|
a_y = 0;
|
|
a_z = 0;
|
|
return false;
|
|
}
|
|
*/
|
|
a_angle = a_acos(cos_angle); //in ]0,pi].
|
|
T sin_angle = a_sin(a_angle); //in [0,1].
|
|
if(sin_angle==T(0)) { //angle is pi. //sym part can determine vector, but up to a sign :
|
|
// sym part is : cos_angle*I+(1-cos_angle)*n*n
|
|
// cos_angle = -1
|
|
T xy = T(0.5)*(pr::m_vec[3]+pr::m_vec[1])/one_minus_cos_angle;
|
|
T xz = T(0.5)*(pr::m_vec[6]+pr::m_vec[2])/one_minus_cos_angle;
|
|
//T yz = T(0.5)*(pr::m_vec[7]+pr::m_vec[5])/one_minus_cos_angle;
|
|
a_x = a_sqrt(x2); //up to a sign.
|
|
a_y = xy/a_x;
|
|
a_z = xz/a_x;
|
|
return true; //vector is defined up to a sign.
|
|
}
|
|
// antisym part is : sin(theta)*n.Rs
|
|
a_z = T(0.5)*(pr::m_vec[3]-pr::m_vec[1])/sin_angle;
|
|
a_y = T(-0.5)*(pr::m_vec[6]-pr::m_vec[2])/sin_angle; //warning : -1.
|
|
a_x = T(0.5)*(pr::m_vec[7]-pr::m_vec[5])/sin_angle;
|
|
return true;
|
|
}
|
|
|
|
public:
|
|
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 * 3];
|
|
T x = pr::m_vec[0]*a_x+pr::m_vec[3]*a_y+pr::m_vec[6]*a_z;
|
|
T y = pr::m_vec[1]*a_x+pr::m_vec[4]*a_y+pr::m_vec[7]*a_z;
|
|
T z = pr::m_vec[2]*a_x+pr::m_vec[5]*a_y+pr::m_vec[8]*a_z;
|
|
a_x = x;
|
|
a_y = y;
|
|
a_z = z;
|
|
}
|
|
|
|
void mul_rotate(const T& a_x,const T& a_y,const T& a_z,const T& a_angle,T(*a_sin)(T),T(*a_cos)(T)) {
|
|
T rot[9];
|
|
_set_rotate(a_x,a_y,a_z,a_angle,rot,a_sin,a_cos);
|
|
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(*a_sin)(T),T(*a_cos)(T)) {
|
|
T _m[9];
|
|
_set_rotate(a_x,a_y,a_z,a_angle,_m,a_sin,a_cos);
|
|
parent::_left_mul_mtx(_m);
|
|
}
|
|
|
|
void v00(const T& a_value){pr::m_vec[0+0*3] = a_value;}
|
|
void v10(const T& a_value){pr::m_vec[1+0*3] = a_value;}
|
|
void v20(const T& a_value){pr::m_vec[2+0*3] = a_value;}
|
|
|
|
void v01(const T& a_value){pr::m_vec[0+1*3] = a_value;}
|
|
void v11(const T& a_value){pr::m_vec[1+1*3] = a_value;}
|
|
void v21(const T& a_value){pr::m_vec[2+1*3] = a_value;}
|
|
|
|
void v02(const T& a_value){pr::m_vec[0+2*3] = a_value;}
|
|
void v12(const T& a_value){pr::m_vec[1+2*3] = a_value;}
|
|
void v22(const T& a_value){pr::m_vec[2+2*3] = a_value;}
|
|
|
|
const T& v00() const {return pr::m_vec[0+0*3];}
|
|
const T& v10() const {return pr::m_vec[1+0*3];}
|
|
const T& v20() const {return pr::m_vec[2+0*3];}
|
|
|
|
const T& v01() const {return pr::m_vec[0+1*3];}
|
|
const T& v11() const {return pr::m_vec[1+1*3];}
|
|
const T& v21() const {return pr::m_vec[2+1*3];}
|
|
|
|
const T& v02() const {return pr::m_vec[0+2*3];}
|
|
const T& v12() const {return pr::m_vec[1+2*3];}
|
|
const T& v22() const {return pr::m_vec[2+2*3];}
|
|
|
|
protected:
|
|
static void _set_scale(const T& a_1,const T& a_2,const T& a_3,T v[]) {
|
|
v[0] = a_1;v[3] = 0;v[6] = 0;
|
|
v[1] = 0;v[4] = a_2;v[7] = 0;
|
|
v[2] = 0;v[5] = 0;v[8] = a_3;
|
|
}
|
|
|
|
static void _set_rotate(const T& a_x,const T& a_y,const T& a_z,const T& a_angle,T v[],T(*a_sin)(T),T(*a_cos)(T)) {
|
|
//WARNING : it models the rotation of an object (active rotation) = exp(-a_angle*n(a_x,a_y,a_z)*Rs).
|
|
//WARNING : (a_x,a_y,a_z) must be a normalized vector.
|
|
T si = a_sin(a_angle);
|
|
T co = a_cos(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[3] = xy*(1-co)-z*si;v[6] = xz*(1-co)+y*si;
|
|
v[1] = xy*(1-co)+z*si;v[4] = y2+(1-y2)*co;v[7] = yz*(1-co)-x*si;
|
|
v[2] = xz*(1-co)-y*si;v[5] = yz*(1-co)+x*si;v[8] = z2+(1-z2)*co;
|
|
|
|
// 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:
|
|
private:static void check_instantiation() {mat3<float> dummy;}
|
|
};
|
|
|
|
////////////////////////////////////////////////
|
|
/// common matrices : //////////////////////////
|
|
////////////////////////////////////////////////
|
|
|
|
template <class T>
|
|
inline const mat3<T>& mat3_zero() {
|
|
static const mat3<T> s_v(false); //inc mem count = false
|
|
return s_v;
|
|
}
|
|
|
|
//for sf, mf :
|
|
//template <class T>
|
|
//inline const T* get_data(const mat3<T>& a_v) {return a_v.data();}
|
|
|
|
}
|
|
|
|
#include <ostream>
|
|
|
|
namespace tools {
|
|
|
|
template <class T>
|
|
inline std::ostream& operator<<(std::ostream& a_out,const mat3<T>& a_mtx){
|
|
const T* v = a_mtx.data();
|
|
a_out << v[0] << "," << v[3] << "," << v[6] << std::endl
|
|
<< v[1] << "," << v[4] << "," << v[7] << std::endl
|
|
<< v[2] << "," << v[5] << "," << v[8] << std::endl;
|
|
return a_out;
|
|
}
|
|
|
|
}
|
|
|
|
#endif
|