eic-opticks/sysrap/stra.h

0001 #pragma once
0002 /**
0003 stra.h
0004 ========
0005
0006 Following some investigations of Geant4 transform handling
0007 and noting that inverses are being done their at source
0008 have concluded that dealing with transforms together with
0009 their inverses is not worth the overhead and complication.
0010 Of course inverting should be minimized.
0011
0012 Hence are bringing over functionality from stran.h as its needed
0013 in new code.
0014
0015 **/
0016
0017 #include <array>
0018
0019 #include "glm/glm.hpp"
0020 #include <glm/gtc/constants.hpp>
0021 #include "glm/gtx/string_cast.hpp"
0022 #include <glm/gtx/transform.hpp>
0023 #include <glm/gtc/type_ptr.hpp>
0024 #include <glm/gtx/component_wise.hpp>
0025
0026 #include "NP.hh"
0027
0028
0029 template<typename T>
0030 struct stra
0031 {
0032     static constexpr const bool VERBOSE = false ;
0033
0034     static std::string Desc(const glm::tmat4x4<T>& t, const glm::tmat4x4<T>& v, const char* tl, const char* vl );
0035     static std::string Desc(
0036         const glm::tmat4x4<double>& a,
0037         const glm::tmat4x4<double>& b,
0038         const glm::tmat4x4<double>& c,
0039         const char* al,
0040         const char* bl,
0041         const char* cl);
0042
0043     static std::string Desc(const T* aa, const T* bb, const T* cc, const char* al, const char* bl, const char* cl) ;
0044
0045     static std::string Desc(const glm::tmat4x4<T>& tr);
0046     static std::string Desc(const glm::tvec4<T>& t);
0047     static std::string Desc(const glm::tvec3<T>& t);
0048     static std::string Desc(const glm::tvec2<T>& t);
0049     static std::string Desc(const T* tt, int num);
0050     static std::string Desc(const T& t);
0051
0052     static std::string DescItems(const T* tt, int num_elem, int num_item, int edge_items=10 );
0053
0054     static std::string Desc(const T* tt, int ni, int nj, int item_stride=0) ;
0055
0056
0057     static std::string Array(const glm::tmat4x4<T>& tr);
0058     static std::string Array(const T* tt, int num);
0059
0060
0061     static glm::tmat4x4<T> FromData(const T* data );
0062
0063     static glm::tmat4x4<T> Translate( const T tx, const T ty, const T tz, const T sc, bool flip=false );
0064     static glm::tmat4x4<T> Translate( const glm::tvec3<T>& tlate, bool flip=false );
0065
0066     static glm::tmat4x4<T> Rotate( const T ax, const T ay, const T az, const T angle_deg, bool flip=false) ;
0067
0068     static glm::tvec3<T>   LeastParallelAxis(const glm::tvec3<T>& a );
0069     static glm::tmat4x4<T> RotateA2B_nonparallel( const glm::tvec3<T>& a, const glm::tvec3<T>& b, bool flip );
0070     static glm::tmat4x4<T> RotateA2B_parallel(    const glm::tvec3<T>& a, const glm::tvec3<T>& b, bool flip );
0071     static glm::tmat4x4<T> RotateA2B(             const glm::tvec3<T>& a, const glm::tvec3<T>& b, bool flip );
0072
0073     static glm::tmat4x4<T> Place(                 const glm::tvec3<T>& a,  const glm::tvec3<T>& b, const glm::tvec3<T>& c, bool flip);
0074     static glm::tmat4x4<T> Model2World(           const glm::tvec3<T>& ax, const glm::tvec3<T>& up, const glm::tvec3<T>& translation );
0075
0076     static glm::tmat4x4<T> Dupe(                  const glm::tvec3<T>& a, const glm::tvec3<T>& b, const glm::tvec3<T>& c, bool flip);
0077
0078     static T Maxdiff_from_Identity(const glm::tmat4x4<T>& m);
0079     static bool IsIdentity(const glm::tmat4x4<T>& m, T epsilon=1e-6);
0080     static bool IsIdentity(const glm::tmat4x4<T>& t, const glm::tmat4x4<T>& v, T epsilon=1e-6);
0081
0082     static void Rows(glm::tvec4<T>& q0,
0083                      glm::tvec4<T>& q1,
0084                      glm::tvec4<T>& q2,
0085                      glm::tvec4<T>& q3,
0086                      const glm::tmat4x4<T>& t ) ;
0087
0088     static void Min(glm::tvec4<T>& q, const glm::tvec4<T>& a, const glm::tvec4<T>& b);
0089     static void Max(glm::tvec4<T>& q, const glm::tvec4<T>& a, const glm::tvec4<T>& b);
0090
0091     static void Transform_AABB( T* aabb1, const T* aabb0, const glm::tmat4x4<T>& t );
0092     static void Transform_AABB_Inplace(         T* aabb,  const glm::tmat4x4<T>& t );
0093
0094     static void Transform_Vec( glm::tvec4<T>& pos, const glm::tvec4<T>&  pos0, const glm::tmat4x4<T>& t );
0095     static void Transform_Strided( T* _pos, const T* _pos0,  int ni, int nj, int item_stride, const glm::tmat4x4<T>& t, T w = 1.  );
0096     static void Transform_Strided_Inplace(        T* _pos,   int ni, int nj, int item_stride, const glm::tmat4x4<T>& t, T w = 1.  );
0097     static void Transform_Data(    T* _pos, const T* _pos0,  const glm::tmat4x4<T>* t, T w = 1.  );
0098     static void Transform_Array( NP* d , const NP* s, const glm::tmat4x4<T>* t, T w=1.  , int stride=0, int offset=0 );
0099
0100     static NP* MakeTransformedArray(const NP* a, const glm::tmat4x4<T>* t, T w=1.  , int stride=0, int offset=0 );
0101
0102
0103     static void Copy_Columns_3x4( T* dst, const T* src );
0104     static void Copy_Columns_3x4( T* dst, const glm::tmat4x4<T>& tr );
0105     static void Copy_Columns_3x4( glm::tmat4x4<T>& dst, const glm::tmat4x4<T>& src );
0106
0107
0108 };
0109
0110
0111 template<typename T>
0112 std::string stra<T>::Desc(const glm::tmat4x4<T>& t, const glm::tmat4x4<T>& v, const char* tl, const char* vl )  // static
0113 {
0114     glm::tmat4x4<double> tv = t*v ;
0115
0116     std::stringstream ss ;
0117     ss << tl << "*" << vl ;
0118     std::string tv_l = ss.str();
0119
0120     return Desc( t, v, tv,  tl, vl, tv_l.c_str() );
0121 }
0122
0123 template<typename T>
0124 std::string stra<T>::Desc(
0125     const glm::tmat4x4<double>& a,
0126     const glm::tmat4x4<double>& b,
0127     const glm::tmat4x4<double>& c,
0128     const char* al,
0129     const char* bl,
0130     const char* cl)
0131 {
0132     return Desc(glm::value_ptr(a),glm::value_ptr(b),glm::value_ptr(c),al,bl,cl) ;
0133 }
0134
0135 template<typename T>
0136 std::string stra<T>::Desc(const T* aa, const T* bb, const T* cc, const char* al, const char* bl, const char* cl)
0137 {
0138     std::stringstream ss ;
0139     ss << "\n" ;
0140
0141     if(al) ss << " " << std::setw(54) << std::left << al ;
0142     if(bl) ss << " " << std::setw(54) << std::left << bl ;
0143     if(cl) ss << " " << std::setw(54) << std::left << cl ;
0144
0145     for(int i=0 ; i < 4 ; i++)
0146     {
0147         ss << "\n" ;
0148         if(aa) for(int j=0 ; j < 4 ; j++) ss << " " << std::fixed << std::setw(10) << std::setprecision(4) << aa[i*4+j] ;
0149         ss << " " << std::setw(10) << " " ;
0150         if(bb) for(int j=0 ; j < 4 ; j++) ss << " " << std::fixed << std::setw(10) << std::setprecision(4) << bb[i*4+j] ;
0151         ss << " " << std::setw(10) << " " ;
0152         if(cc) for(int j=0 ; j < 4 ; j++) ss << " " << std::fixed << std::setw(10) << std::setprecision(4) << cc[i*4+j] ;
0153     }
0154     ss << "\n" ;
0155     std::string s = ss.str();
0156     return s ;
0157 }
0158
0159
0160 template<typename T>
0161 std::string stra<T>::Desc(const glm::tmat4x4<T>& tr)
0162 {
0163     const T* tt = glm::value_ptr(tr);
0164     return Desc(tt, 16 );
0165 }
0166 template<typename T>
0167 std::string stra<T>::Desc(const glm::tvec4<T>& t)
0168 {
0169     const T* tt = glm::value_ptr(t);
0170     return Desc(tt, 4 );
0171 }
0172 template<typename T>
0173 std::string stra<T>::Desc(const glm::tvec3<T>& t)
0174 {
0175     const T* tt = glm::value_ptr(t);
0176     return Desc(tt, 3 );
0177 }
0178 template<typename T>
0179 std::string stra<T>::Desc(const glm::tvec2<T>& t)
0180 {
0181     const T* tt = glm::value_ptr(t);
0182     return Desc(tt, 2 );
0183 }
0184
0185
0186
0187 template<typename T>
0188 std::string stra<T>::Desc(const T* tt, int num)
0189 {
0190     std::stringstream ss ;
0191     for(int i=0 ; i < num ; i++)
0192         ss
0193             << ( i % 4 == 0 && num > 4 ? ".\n" : "" )
0194             << " " << Desc(tt[i])
0195             << ( i == num-1 && num > 4 ? ".\n" : "" )
0196             ;
0197
0198     std::string str = ss.str();
0199     return str ;
0200 }
0201
0202 template<typename T>
0203 std::string stra<T>::Desc(const T& t)
0204 {
0205     std::stringstream ss ;
0206     ss << std::fixed << std::setw(10) << std::setprecision(4) << t ;
0207     std::string str = ss.str();
0208     return str ;
0209 }
0210
0211
0212 template<typename T>
0213 std::string stra<T>::DescItems(const T* tt, int num_elem, int num_item, int edge_items )
0214 {
0215     std::stringstream ss ;
0216     for(int i=0 ; i < num_item ; i++ )
0217     {
0218         if( i < edge_items || i > (num_item - edge_items) ) ss
0219             << " i " << i
0220             << std::endl
0221             << Desc( tt + i*num_elem, num_elem )
0222             << std::endl
0223             ;
0224     }
0225     std::string str = ss.str();
0226     return str ;
0227 }
0228
0229
0230
0231
0232
0233
0234
0235
0236 template<typename T>
0237 std::string stra<T>::Desc(const T* tt, int ni, int nj, int item_stride)
0238 {
0239     int stride = item_stride == 0 ? nj : item_stride ;
0240
0241     std::stringstream ss ;
0242     for(int i=0 ; i < ni ; i++)
0243     {
0244         for(int j=0 ; j < nj ; j++)
0245         {
0246             int idx = i*stride + j ;
0247             ss
0248                 << " " << std::fixed << std::setw(10) << std::setprecision(4) << tt[idx]
0249                 ;
0250         }
0251         if( i < ni - 1) ss << std::endl ;
0252     }
0253     std::string s = ss.str();
0254     return s ;
0255 }
0256
0257
0258
0259
0260
0261
0262
0263 template<typename T>
0264 std::string stra<T>::Array(const glm::tmat4x4<T>& tr)
0265 {
0266     const T* tt = glm::value_ptr(tr);
0267     return Array(tt, 16 );
0268 }
0269
0270 template<typename T>
0271 std::string stra<T>::Array(const T* tt, int num)
0272 {
0273     int w = 7 ;
0274     int p = 3 ;
0275
0276     std::stringstream ss ;
0277     ss << "np.array(" ;
0278     if(num == 16 ) ss << "[[" ;
0279
0280     for(int i=0 ; i < num ; i++) ss
0281         << ( i % 4 == 0 && num > 4 && i > 0 ? "],[" : ( i > 0 ? "," : "" ) )
0282         << std::fixed << std::setw(w) << std::setprecision(p) << tt[i]
0283         ;
0284
0285     if(num == 16 ) ss << "]]" ;
0286     ss << ",dtype=np.float64)" ;
0287     std::string s = ss.str();
0288
0289     bool squeeze = true ;
0290     if( squeeze )
0291     {
0292         const char* p = s.c_str();
0293         std::stringstream zz ;
0294         for(int i=0 ; i < int(strlen(p)) ; i++) if(p[i] != ' ') zz << p[i] ;
0295         s = zz.str();
0296     }
0297     return s ;
0298 }
0299
0300
0301
0302
0303
0304
0305
0306 template<typename T>
0307 glm::tmat4x4<T> stra<T>::FromData(const T* data)  // static
0308 {
0309     glm::tmat4x4<T> tran(1.);
0310     memcpy( glm::value_ptr(tran), data, 16*sizeof(T) );
0311     return tran ;
0312 }
0313
0314
0315
0316
0317
0318 template<typename T>
0319 inline glm::tmat4x4<T> stra<T>::Translate(  const T tx, const T ty, const T tz, const T sc, bool flip )
0320 {
0321     glm::tvec3<T> tlate(tx*sc,ty*sc,tz*sc);
0322     return Translate(tlate, flip) ;
0323 }
0324
0325 template<typename T>
0326 inline glm::tmat4x4<T> stra<T>::Translate( const glm::tvec3<T>& tlate, bool flip )
0327 {
0328     glm::tmat4x4<T> tr = glm::translate(glm::tmat4x4<T>(1.), tlate ) ;
0329     return flip ? glm::transpose(tr) : tr ;
0330 }
0331
0332 template<typename T>
0333 inline glm::tmat4x4<T> stra<T>::Rotate( const T ax, const T ay, const T az, const T angle_deg, bool flip)
0334 {
0335     T angle_rad = glm::pi<T>()*angle_deg/T(180.) ;
0336     glm::tvec3<T> axis(ax,ay,az);
0337     glm::tmat4x4<T> tr = glm::rotate(glm::tmat4x4<T>(1.), ( flip ? -1. : 1. )* angle_rad, axis ) ;
0338     return tr ;
0339 }
0340
0341
0342
0343
0344
0345 template<typename T>
0346 inline glm::tvec3<T> stra<T>::LeastParallelAxis( const glm::tvec3<T>& a )
0347 {
0348     glm::tvec3<T> aa( glm::abs(a) );
0349     glm::tvec3<T> lpa(0.);
0350
0351     if( aa.x <= aa.y && aa.x <= aa.z )
0352     {
0353         lpa.x = 1.f ;
0354     }
0355     else if( aa.y <= aa.x && aa.y <= aa.z )
0356     {
0357         lpa.y = 1.f ;
0358     }
0359     else
0360     {
0361         lpa.z = 1.f ;
0362     }
0363
0364     if(VERBOSE) std::cout
0365         << "stra::LeastParallelAxis"
0366         << " a " <<  glm::to_string(a)
0367         << " aa " <<  glm::to_string(aa)
0368         << " lpa " << glm::to_string(lpa)
0369         << std::endl
0370         ;
0371
0372     return lpa ;
0373 }
0374
0375
0376
0377 /**
0378 stra::RotateA2B
0379 ----------------------
0380
0381 See ana/make_rotation_matrix.py
0382
0383 * http://cs.brown.edu/research/pubs/pdfs/1999/Moller-1999-EBA.pdf
0384   "Efficiently Building a Matrix to Rotate One Vector To Another"
0385   Tomas Moller and John F Hughes
0386
0387 * ~/opticks_refs/Build_Rotation_Matrix_vec2vec_Moller-1999-EBA.pdf
0388
0389 Found this paper via thread:
0390
0391 * https://math.stackexchange.com/questions/180418/calculate-rotation-matrix-to-align-vector-a-to-vector-b-in-3d
0392
0393 Another very long discussion of rotation matrices by Rebecca Brannon:
0394
0395 * https://my.mech.utah.edu/~brannon/public/rotation.pdf
0396 * ~/opticks_refs/utah_brannon_opus_rotation.pdf
0397
0398 TODO: compare with Quaternions
0399
0400 **/
0401
0402 template<typename T>
0403 inline glm::tmat4x4<T> stra<T>::RotateA2B_nonparallel(const glm::tvec3<T>& a, const glm::tvec3<T>& b, bool flip )
0404 {
0405     T one(1.);
0406     T zero(0.);
0407
0408     T c = glm::dot(a,b);
0409     T h = (one - c)/(one - c*c);
0410
0411     glm::tvec3<T> v = glm::cross(a, b) ;
0412     T vx = v.x ;
0413     T vy = v.y ;
0414     T vz = v.z ;
0415
0416     if(VERBOSE) std::cout
0417         << "stra::RotateA2B_nonparallel"
0418         << " a " << glm::to_string(a)
0419         << " b " << glm::to_string(b)
0420         << " c " << std::fixed << std::setw(10) << std::setprecision(5) << c
0421         << " h " << std::fixed << std::setw(10) << std::setprecision(5) << h
0422         << " vx " << std::fixed << std::setw(10) << std::setprecision(5) << vx
0423         << " vy " << std::fixed << std::setw(10) << std::setprecision(5) << vy
0424         << " vz " << std::fixed << std::setw(10) << std::setprecision(5) << vz
0425         << std::endl
0426         ;
0427
0428
0429     T hxx = h*vx*vx ;
0430     T hyy = h*vy*vy ;
0431     T hzz = h*vz*vz ;
0432
0433     T hxy = h*vx*vy ;
0434     T hxz = h*vx*vz ;
0435     T hyz = h*vy*vz ;
0436
0437     T f = flip ? -1. : 1. ;   // flip:true is equivalent to transposing the matrix
0438
0439     std::array<T, 16> vals = {{
0440           c + hxx    , hxy - f*vz   ,  hxz + f*vy  , zero,
0441           hxy + f*vz , c + hyy      ,  hyz - f*vx  , zero,
0442           hxz - f*vy , hyz + f*vx   ,  c + hzz     , zero,
0443           zero       , zero         ,  zero        , one
0444     }};
0445
0446     return glm::make_mat4x4<T>( vals.data() );
0447 }
0448
0449
0450 template<typename T>
0451 inline glm::tmat4x4<T> stra<T>::RotateA2B_parallel(const glm::tvec3<T>& a, const glm::tvec3<T>& b, bool flip )
0452 {
0453     glm::tvec3<T> x = LeastParallelAxis(a);
0454     glm::tvec3<T> u = x - a ;
0455     glm::tvec3<T> v = x - b  ;
0456
0457     if(VERBOSE) std::cout
0458         << "Trab::RotateA2B_parallel"
0459         << std::endl
0460         << " x         " << glm::to_string(x) << std::endl
0461         << " u = x - a " << glm::to_string(u) << std::endl
0462         << " v = x - b " << glm::to_string(v) << std::endl
0463         ;
0464
0465
0466     T uu = glm::dot(u, u);
0467     T vv = glm::dot(v, v);
0468     T uv = glm::dot(u, v);
0469
0470     std::array<T, 16> vals ;
0471     vals.fill(0.) ;
0472
0473     for(int i=0 ; i < 3 ; i++) for(int j=0 ; j < 3 ; j++)
0474     {
0475         int idx = flip == false ? i*4+j : j*4 + i ;
0476         vals[idx] = ( i == j ? 1. : 0.)  - 2.*u[i]*u[j]/uu -2.*v[i]*v[j]/vv + 4.*uv*v[i]*u[j]/(uu*vv) ;
0477
0478     }
0479     vals[15] = 1. ;
0480
0481     return glm::make_mat4x4<T>( vals.data() );
0482 }
0483
0484
0485 /**
0486 stra::RotateA2B
0487 ------------------
0488
0489 Form a rotation transform that rotates vector a to vector b with proper handling
0490 of parallel or anti-parallel edge cases when the absolute dot product of a and b
0491 is close to 1.
0492
0493 **/
0494
0495 template<typename T>
0496 inline glm::tmat4x4<T> stra<T>::RotateA2B(const glm::tvec3<T>& a, const glm::tvec3<T>& b, bool flip)
0497 {
0498     T c = glm::dot(a,b);
0499     return std::abs(c) < 0.99 ? RotateA2B_nonparallel(a,b,flip) : RotateA2B_parallel(a,b,flip) ;
0500 }
0501
0502
0503
0504 /**
0505 stra::Place
0506 -------------
0507
0508 Create a transform combining rotation from a to b and translation of c
0509 The flip argument controls transposes being applied to the rotation
0510 matrix and the order of multiplication of the rotation and the translation.
0511
0512 HMM: not sure regards the "tla*rot" vs "rot*tla"  order flip :
0513 it depends on whether will using the transform to right or left multiply.
0514
0515 BUT what is certain is that the placement transform needs to rotate first and then translate
0516 The complexity comes from different packages needing different layouts,
0517 so generally have to experiment to get things to work.
0518 **/
0519
0520 template<typename T>
0521 inline glm::tmat4x4<T> stra<T>::Place(const glm::tvec3<T>& a, const glm::tvec3<T>& b, const glm::tvec3<T>& c, bool flip )
0522 {
0523     glm::tmat4x4<T> rot = RotateA2B(a,b,flip) ;
0524     glm::tmat4x4<T> tla = Translate(c) ;              // this has the translation in last row
0525     glm::tmat4x4<T> tra = flip == true ? tla * rot : rot * tla ;
0526
0527     return tra ;
0528 }
0529
0530
0531 template<typename T>
0532 inline glm::tmat4x4<T> stra<T>::Model2World(const glm::tvec3<T>& ax, const glm::tvec3<T>& up, const glm::tvec3<T>& translation )
0533 {
0534     // 1. Calculate the 'Right' vector (X-axis)
0535     // Right = Up x Forward (ax)
0536     glm::tvec3<T> right = glm::normalize(glm::cross(up, ax));
0537
0538     // 2. Re-calculate Up to ensure perfect perpendicularity
0539     // Up = Forward (ax) x Right
0540     glm::tvec3<T> up_rectified = glm::cross(ax, right);
0541
0542     // 3. Create the 4x4 Identity Matrix
0543     glm::tmat4x4<T> m2w = glm::tmat4x4<T>(1.0f);
0544
0545     // 4. Set the columns (Basis vectors)
0546     // GLM is Column-Major: mat[column_index] = vec4(vector, 0.0 or 1.0)
0547     m2w[0] = glm::tvec4<T>(right,        0.0f); // X-axis (Column 0)
0548     m2w[1] = glm::tvec4<T>(up_rectified, 0.0f); // Y-axis (Column 1)
0549     m2w[2] = glm::tvec4<T>(ax,           0.0f); // Z-axis (Column 2)
0550
0551     // 5. Set the translation column
0552     m2w[3] = glm::tvec4<T>(translation,  1.0f); // Position (Column 3)
0553
0554     return m2w;
0555 }
0556
0557
0558
0559 template<typename T>
0560 inline glm::tmat4x4<T> stra<T>::Dupe(const glm::tvec3<T>& a, const glm::tvec3<T>& b, const glm::tvec3<T>& c, bool flip )
0561 {
0562     glm::tmat4x4<T> tr(1.);
0563     T* src = glm::value_ptr(tr) ;
0564
0565     if( flip == false )
0566     {
0567         for(int l=0 ; l < 3 ; l++) src[4*0+l] = a[l] ;
0568         for(int l=0 ; l < 3 ; l++) src[4*1+l] = b[l] ;
0569         for(int l=0 ; l < 3 ; l++) src[4*2+l] = c[l] ;
0570     }
0571     else
0572     {
0573         for(int l=0 ; l < 3 ; l++) src[4*l+0] = a[l] ;
0574         for(int l=0 ; l < 3 ; l++) src[4*l+1] = b[l] ;
0575         for(int l=0 ; l < 3 ; l++) src[4*l+2] = c[l] ;
0576     }
0577     return tr ;
0578 }
0579
0580
0581
0582
0583
0584
0585
0586
0587 template<typename T>
0588 inline T stra<T>::Maxdiff_from_Identity(const glm::tmat4x4<T>& m)
0589 {
0590     T mxdif = 0. ;
0591     for(int j=0 ; j < 4 ; j++ )
0592     for(int k=0 ; k < 4 ; k++ )
0593     {
0594         T val = m[j][k] ;
0595         T xval = j == k ? T(1) : T(0) ;
0596         T dif = std::abs( val - xval ) ;
0597         if(dif > mxdif) mxdif = dif ;
0598     }
0599     return mxdif ;
0600 }
0601
0602 template<typename T>
0603 inline bool stra<T>::IsIdentity(const glm::tmat4x4<T>& m, T epsilon )
0604 {
0605     T mxdif = Maxdiff_from_Identity(m);
0606     return mxdif < epsilon ;
0607 }
0608
0609 template<typename T>
0610 inline bool stra<T>::IsIdentity(const glm::tmat4x4<T>& t, const glm::tmat4x4<T>& v, T epsilon )
0611 {
0612     return IsIdentity(t, epsilon) && IsIdentity(v, epsilon) ;
0613 }
0614
0615 template<typename T>
0616 inline void stra<T>::Rows(
0617     glm::tvec4<T>& q0,
0618     glm::tvec4<T>& q1,
0619     glm::tvec4<T>& q2,
0620     glm::tvec4<T>& q3,
0621     const glm::tmat4x4<T>& t )
0622 {
0623     memcpy( glm::value_ptr(q0), glm::value_ptr(t) +  0,  4*sizeof(T) );
0624     memcpy( glm::value_ptr(q1), glm::value_ptr(t) +  4,  4*sizeof(T) );
0625     memcpy( glm::value_ptr(q2), glm::value_ptr(t) +  8,  4*sizeof(T) );
0626     memcpy( glm::value_ptr(q3), glm::value_ptr(t) + 12,  4*sizeof(T) );
0627 }
0628
0629 template<typename T>
0630 void stra<T>::Min(glm::tvec4<T>& q, const glm::tvec4<T>& a, const glm::tvec4<T>& b)
0631 {
0632     q.x = std::min( a.x, b.x );
0633     q.y = std::min( a.y, b.y );
0634     q.z = std::min( a.z, b.z );
0635     q.w = std::min( a.w, b.w );
0636 }
0637
0638 template<typename T>
0639 void stra<T>::Max(glm::tvec4<T>& q, const glm::tvec4<T>& a, const glm::tvec4<T>& b)
0640 {
0641     q.x = std::max( a.x, b.x );
0642     q.y = std::max( a.y, b.y );
0643     q.z = std::max( a.z, b.z );
0644     q.w = std::max( a.w, b.w );
0645 }
0646
0647
0648
0649
0650 /**
0651 stra::Transform_AABB
0652 ---------------------
0653
0654 Impl from sqat4.h transform_aabb_inplace
0655
0656 **/
0657
0658 template<typename T>
0659 inline void stra<T>::Transform_AABB( T* aabb_1, const T* aabb, const glm::tmat4x4<T>& t )
0660 {
0661     glm::tvec4<T> q0(0.);
0662     glm::tvec4<T> q1(0.);
0663     glm::tvec4<T> q2(0.);
0664     glm::tvec4<T> q3(0.);
0665
0666     Rows(q0,q1,q2,q3,t);
0667
0668     T x0 = *(aabb+0) ;
0669     T y0 = *(aabb+1) ;
0670     T z0 = *(aabb+2) ;
0671     T x1 = *(aabb+3) ;
0672     T y1 = *(aabb+4) ;
0673     T z1 = *(aabb+5) ;
0674
0675     glm::tvec4<T> xa = q0 * x0  ;
0676     glm::tvec4<T> xb = q0 * x1 ;
0677     glm::tvec4<T> xmi ;
0678     glm::tvec4<T> xma ;
0679     stra<T>::Min( xmi, xa, xb);
0680     stra<T>::Max( xma, xa, xb);
0681
0682     glm::tvec4<T> ya = q1 * y0 ;
0683     glm::tvec4<T> yb = q1 * y1 ;
0684     glm::tvec4<T> ymi ;
0685     glm::tvec4<T> yma ;
0686     stra<T>::Min( ymi, ya, yb);
0687     stra<T>::Max( yma, ya, yb);
0688
0689     glm::tvec4<T> za = q2 * z0 ;
0690     glm::tvec4<T> zb = q2 * z1 ;
0691     glm::tvec4<T> zmi ;
0692     glm::tvec4<T> zma ;
0693     stra<T>::Min( zmi, za, zb);
0694     stra<T>::Max( zma, za, zb);
0695
0696     *(aabb_1+0) = xmi.x + ymi.x + zmi.x + q3.x ;
0697     *(aabb_1+1) = xmi.y + ymi.y + zmi.y + q3.y ;
0698     *(aabb_1+2) = xmi.z + ymi.z + zmi.z + q3.z ;
0699     *(aabb_1+3) = xma.x + yma.x + zma.x + q3.x ;
0700     *(aabb_1+4) = xma.y + yma.y + zma.y + q3.y ;
0701     *(aabb_1+5) = xma.z + yma.z + zma.z + q3.z ;
0702 }
0703
0704 template<typename T>
0705 inline void stra<T>::Transform_AABB_Inplace( T* aabb, const glm::tmat4x4<T>& t )
0706 {
0707     Transform_AABB( aabb, aabb, t );
0708 }
0709
0710
0711
0712 template<typename T>
0713 inline void stra<T>::Transform_Vec( glm::tvec4<T>& pos, const glm::tvec4<T>&  pos0, const glm::tmat4x4<T>& t )
0714 {
0715     pos = t * pos0 ;
0716 }
0717
0718
0719
0720
0721 template<typename T>
0722 inline void stra<T>::Transform_Strided( T* _pos, const T* _pos0, int ni, int nj, int item_stride, const glm::tmat4x4<T>& t, T w  )
0723 {
0724     assert( nj == 3 || nj == 4 );
0725     int stride = item_stride == 0 ? nj : item_stride ;
0726
0727     for(int i=0 ; i < ni ; i++)
0728     {
0729         glm::tvec4<T> pos0 ;
0730
0731         for(int j=0 ; j < nj ; j++)
0732         {
0733             int idx = i*stride + j ;
0734             switch(j)
0735             {
0736                 case 0: pos0.x = _pos0[idx] ; break ;
0737                 case 1: pos0.y = _pos0[idx] ; break ;
0738                 case 2: pos0.z = _pos0[idx] ; break ;
0739                 case 3: pos0.w = _pos0[idx] ; break ;
0740             }
0741         }
0742         if( nj == 3 ) pos0.w = w ;
0743
0744         glm::tvec4<T> pos = t * pos0 ;
0745
0746
0747         for(int j=0 ; j < nj ; j++)
0748         {
0749             int idx = i*stride + j ;
0750             switch(j)
0751             {
0752                 case 0: _pos[idx] = pos.x ; break ;
0753                 case 1: _pos[idx] = pos.y ; break ;
0754                 case 2: _pos[idx] = pos.z ; break ;
0755                 case 3: _pos[idx] = pos.w ; break ;
0756             }
0757         }
0758     }
0759 }
0760
0761
0762 template<typename T>
0763 inline void stra<T>::Transform_Strided_Inplace( T* _pos, int ni, int nj, int item_stride, const glm::tmat4x4<T>& t, T w  )
0764 {
0765     const T* _pos0 = const_cast<const T*>(_pos) ;
0766     Transform_Strided( _pos, _pos0, ni, nj, item_stride, t, w );
0767 }
0768
0769
0770
0771
0772 /**
0773 stra::Transform_Data
0774 ----------------------
0775
0776 1. form pos0:tvec4 from _pos0:T pointer using w param (so pos0 can be three elements)
0777 2. pre-multiply transform t and pos0 to give pos:tvec4
0778 3. copy pos elements into _pos
0779
0780 **/
0781
0782 template<typename T>
0783 inline void stra<T>::Transform_Data( T* _pos, const T* _pos0,  const glm::tmat4x4<T>* t_ptr, T w  )
0784 {
0785     glm::tvec4<T> pos0 ;
0786     pos0.x = *(_pos0 + 0) ;
0787     pos0.y = *(_pos0 + 1) ;
0788     pos0.z = *(_pos0 + 2) ;
0789     pos0.w = w ;
0790
0791     //std::cout << "stra::Transform_Data  pos0 " << glm::to_string(pos0) << std::endl ;
0792
0793     glm::tvec4<T> pos = (t_ptr == nullptr ) ? pos0 : (*t_ptr) * pos0 ;
0794
0795     //std::cout << "stra::Transform_Data  pos  " << glm::to_string(pos) << std::endl ;
0796
0797     *(_pos+0) = pos.x ;
0798     *(_pos+1) = pos.y ;
0799     *(_pos+2) = pos.z ;
0800 }
0801
0802 template<typename T>
0803 inline void stra<T>::Transform_Array( NP* d , const NP* s, const glm::tmat4x4<T>* t, T w , int stride, int offset ) // static
0804 {
0805     assert( s->shape.size() == 2 && s->shape[1] >= 3 );
0806     assert( s->shape == d->shape );
0807     assert( s->uifc == d->uifc );
0808
0809     int num_item = s->shape[0] ;
0810     if(stride == 0) stride = s->num_itemvalues() ;
0811
0812     bool dump = false ;
0813     if(dump) std::cout
0814          << "stra::Transform_Array "
0815          << " num_item " << num_item
0816          << " stride " << stride
0817          <<  std::endl
0818          ;
0819
0820     const T* ss = s->cvalues<T>();
0821     T* dd = d->values<T>();
0822
0823     for(int i=0 ; i < num_item ; i++)
0824     {
0825         const T* v0 = ss + i*stride + offset ;
0826         T*       v  = dd + i*stride + offset ;
0827         Transform_Data( v, v0, t, w );
0828     }
0829 }
0830
0831 template<typename T>
0832 inline NP* stra<T>::MakeTransformedArray(const NP* a, const glm::tmat4x4<T>* t, T w , int stride, int offset )
0833 {
0834     bool dump = false ;
0835     if(dump) std::cout << "[ stra::MakeTransformedArray" << std::endl ;
0836
0837     NP* b = NP::MakeLike(a);
0838     Transform_Array( b, a, t, w, stride, offset );
0839
0840     if(dump) std::cout << "] stra::MakeTransformedArray" << std::endl ;
0841     return b ;
0842 }
0843
0844
0845 /**
0846 stra::Copy_Columns_3x4
0847 -----------------------
0848
0849 * After sqat4.h qat4::copy_columns_3x4
0850
0851 * Suitable for filling optixInstance transforms
0852
0853 * Assumes standard OpenGL memory layout of the 16 elements of source
0854   with translation in slots 12,13,14::
0855
0856      . 0  1  2  3
0857        4  5  6  7
0858        8  9 10 11
0859      [12 13 14]15     tx ty tz
0860
0861 dst::
0862
0863      . 0  4  8  -
0864        1  5  9  -
0865        2  6 10  -
0866        3  7 11  -
0867
0868 **/
0869
0870 template<typename T>
0871 inline void stra<T>::Copy_Columns_3x4( T* dst, const T* src ) // static
0872 {
0873     dst[0] = src[0] ;
0874     dst[1] = src[4] ;
0875     dst[2] = src[8] ;
0876     dst[3] = src[12] ;
0877
0878     dst[4] = src[1] ;
0879     dst[5] = src[5] ;
0880     dst[6] = src[9] ;
0881     dst[7] = src[13] ;
0882
0883     dst[8]  = src[2] ;
0884     dst[9]  = src[6] ;
0885     dst[10] = src[10] ;
0886     dst[11] = src[14] ;
0887 }
0888
0889 template<typename T>
0890 inline void stra<T>::Copy_Columns_3x4( T* dst, const glm::tmat4x4<T>& src )
0891 {
0892     Copy_Columns_3x4( dst, glm::value_ptr(src) );
0893 }
0894
0895 template<typename T>
0896 inline void stra<T>::Copy_Columns_3x4( glm::tmat4x4<T>& dst, const glm::tmat4x4<T>& src )
0897 {
0898     Copy_Columns_3x4( glm::value_ptr(dst), glm::value_ptr(src) );
0899 }
0900
0901
0902
0903
0904 template<typename T>
0905 inline std::ostream& operator<<(std::ostream& os, const glm::tmat4x4<T>& m)
0906 {
0907     os << stra<T>::Desc(m) ;
0908     return os;
0909 }
0910
0911 template<typename T>
0912 inline std::ostream& operator<<(std::ostream& os, const glm::tvec4<T>& v)
0913 {
0914     os << stra<T>::Desc(v) ;
0915     return os;
0916 }
0917
0918
0919