ModFEM: mod_fem_viewer/FemViewer/inc/MathHelper.h Source File

00001 #ifndef _MATH_HELPER_H_
00002 #define _MATH_HELPER_H_
00003 
00004 #include <limits>
00005 #include <cmath>
00006 #include <ostream>
00007 #include <map>
00008 #include <vector>
00009 #include "fv_compiler.h"
00010 #include <stdint.h>
00011 #if defined(WIN32) && defined(_MSC_VER)
00012 # pragma warning( disable : 4201 )
00013 #endif
00014 
00015 
00016 namespace FemViewer {
00017 namespace fvmath {
00018 
00019 #define AVEC3(T)    union { struct { T x,y,z; }; T v[3]; }
00020 #define AVEC4(T)    union { struct { T x,y,z,w; }; T v[4]; }
00021 #ifdef max
00022 #undef max
00023 #endif
00024 
00025 #ifdef min
00026 #undef min
00027 #endif
00028 
00029 static const float kFloatMaximumValue = std::numeric_limits<float>::max();
00030 static const float kInfinity = std::numeric_limits<float>::infinity();
00031 
00032 template<typename T>
00033 struct Vec3 {
00034     AVEC3(T);
00035 };
00036 
00037 template<typename T>
00038 struct Vec4 {
00039     AVEC4(T);
00040 };
00041 
00042 typedef Vec4<float> Vec4f;
00043 
00044 
00045 template<typename T>
00046 class Ray3 {
00047     Vec3<T> org, dir;
00048 };
00049 
00050 
00051 
00052 template<typename T> class CVec3;
00053 
00054 template<typename T>
00055 std::ostream& operator << (std::ostream& os, const CVec3<T>& rhs);
00056 
00057 template<typename T>
00058 class CVec3 : public Vec3<T> {
00059 public:
00060     CVec3(T x_= T(0), T y_=T(0), T z_=T(0)) //: Vec3<T>::x(x_), Vec3<T>::y(y_), Vec3<T>::z(z_)
00061     { Vec3<T>::x = x_; Vec3<T>::y = y_; Vec3<T>::z = z_; }
00062     template<typename U> CVec3(const U v_[]);
00063     template<typename U> CVec3(U x_, U y_, U z_);
00064     template<typename U> CVec3(const Vec3<U>& v_);
00065     template<typename U = T> CVec3<T>& operator=(const Vec3<U>& v_);
00066     template<typename U>
00067     void operator+=(const fvmath::Vec3<U>& v_);
00068     template<typename U>
00069     operator U*() { return static_cast<U *>(this->v); }
00070     T operator[](int idx) const { return this->v[idx]; }
00071     T& operator[](int idx) { return this->v[idx]; }
00072     template<typename U>
00073     void operator +=(const U* rhs);
00074     void operator *=(const T& alpha);
00075     void operator -=(const Vec3<T>& rhs);
00076     void operator /=(const Vec3<T>& rhs);
00077 
00078     friend std::ostream& operator << <>(std::ostream& os, const CVec3& rhs);
00079 };
00080 template<typename T> template<typename U>
00081 inline CVec3<T>::CVec3(const U v_[]) {
00082     for(int i(0);i<3;i++) Vec3<T>::v[i] = static_cast<T>(v_[i]);
00083 }
00084 
00085 template<> template<>
00086 inline CVec3<float>::CVec3(const float v_[]) {
00087     for(int i(0);i<3;i++) Vec3<float>::v[i] = v_[i];
00088 }
00089 
00090 template<typename T> template<typename U>
00091 inline CVec3<T>::CVec3(U x_,U y_, U z_)
00092 {
00093     Vec3<T>::x = static_cast<T>(x_);
00094     Vec3<T>::y = static_cast<T>(y_);
00095     Vec3<T>::z = static_cast<T>(z_);
00096 }
00097 
00098 
00099 
00100 template<typename T> template<typename U>
00101 inline CVec3<T>::CVec3(const Vec3<U>& v_)
00102 {
00103     for(int i(0);i<3;i++) Vec3<T>::v[i] = v_.v[i];
00104 }
00105 
00106 template<typename T> template<typename U>
00107 inline CVec3<T>& CVec3<T>::operator=(const Vec3<U>& v_)
00108 {
00109     Vec3<T>::x = static_cast<T>(v_.x);
00110     Vec3<T>::y = static_cast<T>(v_.y);
00111     Vec3<T>::z = static_cast<T>(v_.z);
00112 
00113     return *this;
00114 }
00115 
00116 
00117 template<> template<>
00118 inline CVec3<float>& CVec3<float>::operator=(const Vec3<float>& v) {
00119     for (unsigned i(0);i<3;++i) this->v[i] = v.v[i];
00120     return *this;
00121 }
00122 
00123 template<typename T> template<typename U>
00124 inline void CVec3<T>::operator+=(const Vec3<U>& v_)
00125 {
00126     Vec3<T>::x += static_cast<T>(v_.x);
00127     Vec3<T>::y += static_cast<T>(v_.y);
00128     Vec3<T>::z += static_cast<T>(v_.z);
00129 }
00130 
00131 template<> template<>
00132 inline void CVec3<float>::operator+=(const Vec3<float>& v_)
00133 {
00134     Vec3<float>::x += v_.x;
00135     Vec3<float>::y += v_.y;
00136     Vec3<float>::z += v_.z;
00137 }
00138 
00139 
00140 template<typename T> template<typename U>
00141 inline void CVec3<T>::operator+=(const U* rhs)
00142 {
00143     for (int i(0);i<3;++i) Vec3<T>::v[i] = static_cast<T>(rhs[i]);
00144 }
00145 
00146 template<typename T>
00147 inline void CVec3<T>::operator*=(const T& alpha)
00148 {
00149     for (int i(0);i<3;++i) Vec3<T>::v[i] *= alpha;
00150 }
00151 
00152 template<typename T>
00153 inline void CVec3<T>::operator-=(const Vec3<T>& rhs)
00154 {
00155     for (int i(0);i<3;++i) Vec3<T>::v[i] -= rhs.v[i];
00156 }
00157 
00158 template<typename T>
00159 inline void CVec3<T>::operator/=(const Vec3<T>& rhs)
00160 {
00161     for (int i(0);i<3;++i) Vec3<T>::v[i] /= rhs.v[i];
00162 }
00163 
00164 template<typename T>
00165 std::ostream& operator <<(std::ostream& os, const CVec3<T>& rhs)
00166 {
00167     return os << "x = " << rhs.x << " y = " << rhs.y << " z = " << rhs.z;
00168 }
00169 
00170 typedef Vec3<int> Vec3i;
00171 typedef Vec3<uint32_t> Vec3u;
00172 typedef Vec3<float> Vec3f;
00173 typedef Vec3<double> Vec3d;
00174 typedef Vec3d Point3d;
00175 typedef CVec3<int> CVec3i;
00176 typedef CVec3<uint32_t> CVec3u;
00177 typedef CVec3<float> CVec3f;
00178 typedef CVec3<double> CVec3d;
00179 
00180 template<typename T> inline T Norm(const Vec3<T>& v)
00181 { return sqrt(v.x*v.x+v.y*v.y+v.z*v.z); }
00182 
00183 template<typename T> T Normalize(Vec3<T>& v) {
00184     T len = Norm<T>(v);
00185     if (len == 0) len = 1;
00186     T inv = 1 / len;
00187         //throw("Can't normalize null vector!");
00188     v.x *= inv;
00189     v.y *= inv;
00190     v.z *= inv;
00191 
00192     return(len);
00193 }
00194 
00195 template<typename T>
00196 inline T Normalize(Vec3<T> *v) {
00197     return Normalize(*v);
00198 }
00199 
00200 template<typename T> inline CVec3<T> operator+(const Vec3<T>& v, const Vec3<T>& u )
00201 { return CVec3<T>( v.x+u.x, v.y+u.y, v.z+u.z ); }
00202 
00203 template<typename T> inline CVec3<T> operator-( const Vec3<T>& v, const Vec3<T>& u )
00204 {
00205     Vec3<T> res;
00206     for (int i(0);i<3;++i) res.v[i] = v.v[i] - u.v[i];
00207     return res;
00208 }
00209 
00210 /*
00211 template<typename T> inline CVec3<T> operator-( const Vec3<T> *v, const Vec3<T> *u )
00212 {
00213     return CVec3<T>(*v - *u);
00214 }
00215 */
00216 
00217 template<typename T> T Dot(const Vec3<T>& v, const Vec3<T>& u)
00218 { return (v.x*u.x+v.y*u.y+v.z*u.z); }
00219 
00220 template<typename T> inline CVec3<T> operator*( const Vec3<T>& v, const Vec3<T>& u)
00221 { return CVec3<T>( v.y*u.z - v.z*u.y,
00222                    v.z*u.x - v.x*u.z,
00223                    v.x*u.y - v.y*u.x );
00224 }
00225 
00226 template<typename T> inline void Cross( const Vec3<T>* v, const Vec3<T>* u, Vec3<T>* out)
00227 {
00228     out->x = (v->y*u->z - v->z*u->y);
00229     out->y = (v->z*u->x - v->x*u->z);
00230     out->z = (v->x*u->y - v->y*u->x);
00231     Normalize(out);
00232 }
00233 
00234 template<typename T> inline CVec3<T> operator *(T alfa, const Vec3<T>& v)
00235 { return CVec3<T>(alfa*v.x,alfa*v.y,alfa*v.z); }
00236 
00237 template<typename T> inline CVec3<T> operator *(const Vec3<T>& v,T alfa) {
00238     return CVec3<T>(alfa*v.x,alfa*v.y,alfa*v.z);
00239 }
00240 
00241 template<typename T> inline CVec3<T> operator / (const T& l,const Vec3<T>& rhs){
00242     CVec3<T> w;
00243     for (int i = 0; i<3; ++i) w.v[i] = l / rhs.v[i];
00244     return w;
00245 }
00246 
00247 template<typename T> inline Vec3<T> operator / (const Vec3<T>& lhs,const Vec3<T>& rhs) {
00248     Vec3<T> res;
00249     for (int i = 0; i<3; ++i) res.v[i] = lhs.v[i] / rhs.v[i];
00250     return res;
00251 }
00252 
00253 template<typename T> inline bool operator ==(const CVec3<T>& lhs,const CVec3<T>& rhs) {
00254     for (unsigned int i(0); i < 3; ++i) {
00255         if (lhs[i] != rhs[i]) return false;
00256     }
00257     return true;
00258 }
00259 
00260 template<typename T>
00261 inline CVec3<T> MulM3x3V(const T mat3[9], const Vec3<T>& v) {
00262     CVec3<T> row0(mat3[0],mat3[3],mat3[6]);
00263     CVec3<T> row1(mat3[1],mat3[4],mat3[7]);
00264     CVec3<T> row2(mat3[2],mat3[5],mat3[8]);
00265 
00266     return CVec3<T>(Dot(row0,v),Dot(row1,v),Dot(row2,v));
00267 }
00268 
00269 
00270 #undef AVEC3
00271 #undef AVEC4
00272 
00273 template<typename T>
00274 inline bool ValueWithin(const T& v, const T& v1, const T& v2)
00275 { return (v >= v1 && v <= v2) || (v >= v2 && v <= v1); }
00276 
00277 template<typename T> bool Compare(const T& arg1,const T& arg2);
00278 
00279 template<>
00280 bool Compare<float>(const float& arg1,const float& arg2);
00281 template<>
00282 bool Compare<double>(const double& arg1,const double& arg2);
00283 template<>
00284 bool Compare<CVec3f>(const CVec3f& v1,const CVec3f& v2);
00285 
00286 template<typename T> bool Less(const T& arg1, const T& arg2);
00287 template<> bool Less(const double& arg1,const double& arg2);
00288 
00289 template<typename T>
00290 inline T max(T arg1, T arg2) {
00291     return (arg1 > arg2) ? arg1 : arg2;
00292 }
00293 
00294 template<typename T>
00295 inline T min(T arg1, T arg2) {
00296     return (arg1 < arg2) ? arg1 : arg2;
00297 }
00298 
00299 template<typename T>
00300 T DotProd(const T v1[], const T v2[]);
00301 
00302 
00303 
00304 extern const float  epsil;
00305 extern const double ref;
00306 
00307 template<typename T> inline const T abs(const T& arg)
00308 { return (arg < T(0)) ? -arg : arg; }
00309 
00310 template<typename T> inline T clamp(const T &v, const T &lo, const T &hi)
00311 { return max(lo, min(v, hi)); }
00312 
00313 // 32-bit calculations
00314 
00315 /* Find integer log base 2 of a 32-bit IEEE float */
00316 /* Conditions: f > 0.0 && finite(f) && isnormal(f) */
00317 inline int log2(const float f)
00318 {
00319     int c;
00320     c = *(unsigned int*) &f;
00321     return ((c >> 23) - 127);
00322 }
00323 
00324 
00325 
00326 /* Round up to the next highest power of 2 of 32-bit IEE float*/
00327 inline int npow2(const float f)
00328 {
00329     unsigned int r;
00330     if (f > 1.0f) {
00331         const unsigned int t = 1U << ((*(unsigned int *)&f >> 23) - 0x7f);
00332         r = t << (unsigned int)((t < (unsigned int)f));
00333     } else {
00334         r = 1;
00335     }
00336 
00337     return(r);
00338 }
00339 
00340 inline int npow2(unsigned int v)
00341 {
00342     return npow2((float)v);
00343 }
00344 
00345 
00346 
00347 // CCW-direction p1,p2,p3
00348 template<typename T,typename U>
00349 CVec3<T> GetNormal2Plane(const U* A,const U* B,const U* C)
00350 {
00351     CVec3<T> vAC( C[0] - A[0], C[1] - A[1], C[2] - A[2]);
00352     CVec3<T> vAB( B[0] - A[0], B[1] - A[1], B[2] - A[2]);
00353     CVec3<T> nv((vAC * vAB).v);
00354     Normalize(nv);
00355     return nv;
00356 }
00357 
00358 /* Routines for ray triangle test */
00359 /* Muller algorithm */
00360 template<typename T>
00361 static int testIntersection(const Ray3<T>& ray,
00362                             const CVec3<T>& A, const CVec3<T>& B, const CVec3<T>& C,
00363                             T& t, T& u, T& v,
00364                             bool singleSide = true)
00365 {
00366     // Make triangle
00367     CVec3<T> AB = B - A;
00368     CVec3<T> AC = C - A;
00369     CVec3<T> pvec = ray.dir * AC;
00370     T det = Dot(AB, pvec);
00371     // test
00372     if (singleSide) {
00373         if (det < epsil)
00374             return 0;
00375         T tvec = ray.orig - A;
00376         u = Dot(tvec, pvec);
00377         if (u < 0 || u > det)
00378             return 0;
00379         CVec3<T> qvec = tvec - AB;
00380         v = Dot(ray.dir, qvec);
00381         if (v < 0 || u + v > det)
00382             return 0;
00383         t = Dot(AC, qvec);
00384         T inv_det = 1 / det;
00385         t *= inv_det;
00386         v *= inv_det;
00387         u *= inv_det;
00388     } else {
00389         if (det < -epsil && det > epsil)
00390             return 0;
00391         T inv_det = 1 / det;
00392         T tvec = ray.orig - A;
00393         u = Dot(tvec, pvec) * inv_det;
00394         if (u < 0 || u > 1)
00395             return 0;
00396         CVec3<T> qvec = tvec - AB;
00397         v = Dot(ray.dir, qvec) * inv_det;
00398         if (v < 0 || u + v > 1)
00399             return 0;
00400         t = t = Dot(AC, qvec) * inv_det;
00401     }
00402     return 1;
00403 }
00404 
00405 inline int roundup(const int value,const int div)
00406 {
00407     int ret = value % div ? (value / div + 1)*div : value;
00408     return ret;
00409 }
00410 
00411 template <typename T>
00412 inline bool is_near(T v1, T v2, T margin) {
00413     return fabs(v1-v2) < margin;
00414 }
00415 
00416 // Convert value to rgb color
00417 template<typename TValue>
00418 CVec3<TValue> convertToRGB(TValue f)
00419 {
00420     const TValue dx = 0.8;
00421     f = (f < 0) ? 0 : (f > 1) ? 1 : f;
00422     TValue g = (6 - 2*dx)*f + dx;
00423     TValue R = max(0, (3 -fabs(g-4)-fabs(g-5))/2);
00424     TValue G = max(0, (4 -fabs(g-2)-fabs(g-4))/2);
00425     TValue B = max(0, (3 -fabs(g-1)-fabs(g-2))/2);
00426 
00427     return CVec3<TValue>(R, G, B);
00428 
00429 }
00430 
00431 typedef unsigned int indextype;
00432 
00433 template< typename TVertex >
00434 struct ComparatorSpec
00435 {
00436     bool operator ()(const TVertex& lh,const TVertex& rh)
00437     {
00438         return lh.info < rh.info;
00439     }
00440 };
00441 
00442 
00443 template< typename TIndex,
00444           typename TVertex,
00445           typename TComp = ComparatorSpec<TVertex>>
00446 bool getSimilarVertexIndex(
00447     TVertex & packed,
00448     std::map<TVertex,TIndex,TComp> & VertexToOutIndex,
00449     TIndex & result)
00450 {
00451     typename std::map<TVertex,TIndex,TComp>::iterator it = VertexToOutIndex.find(packed);
00452     if ( it == VertexToOutIndex.end() ){
00453         return false;
00454     }else{
00455         result = it->second;
00456         return true;
00457     }
00458 }
00459 
00460 
00461 
00462 template< typename TIndex,
00463           typename TVertex,
00464           typename TComp = ComparatorSpec<TVertex>>
00465 void removeDuplicates(std::vector< TVertex >& inout_vertices,
00466               std::vector< TIndex >& inout_indices)
00467 {
00468     // Temporary container
00469     std::vector<TVertex> out_vertices;
00470     out_vertices.reserve(inout_vertices.size());
00471 
00472     assert(inout_indices.empty()==false);
00473 
00474     TIndex index = 0;
00475     TIndex num = inout_indices.size();
00476     typename std::map<TVertex,TIndex,TComp>  VertexToOutIndex;
00477 
00478     // For each input index
00479     for (TIndex i=0; i < num; i++)
00480     {
00481         // Get index value
00482         TIndex vt_index = inout_indices[i];
00483         // Get vertex from input vector
00484         TVertex v = TVertex(inout_vertices[ vt_index ]);
00485 
00486         // Check whether vertex is present already
00487         TIndex ii;
00488         bool found = getSimilarVertexIndex(v,VertexToOutIndex,ii);
00489 
00490         if (found) {
00491             inout_indices[i] = ii;
00492         }
00493         else {
00494             //mfp_debug("Size of vertices: %u",out_vertices.size());
00495             out_vertices.push_back(v);
00496             TIndex newindex = out_vertices.size() - 1;
00497             inout_indices[i] = newindex;
00498             VertexToOutIndex[v] = newindex;
00499         }
00500     }
00501     // Resize
00502     //std::vector<Vertex>(out_vertices).swap(inout_vertices);
00503     inout_vertices = out_vertices;
00504 }
00505 
00506         
00507 } // end namespace fvmath
00508 } // end namespace FemViewer
00509 
00510 #endif /* _MATH_HELPER_H_
00511 */