|
shape_doc 0.1
|
|
shape_doc 0.1
|
00001 #ifndef MATH_MATH_H 00002 #define MATH_MATH_H 00003 #include <algorithm> 00004 #include <math.h> 00005 00006 template<class K> inline 00007 K abs( const K& k ) { return std::abs(k); }; 00008 00009 template<class K> inline 00010 K det22( const K& a, const K& b, 00011 const K& c, const K& d ) 00012 { return a*d - b*c; }; 00013 00014 template<class K> 00015 void tcoff33( K& r00, K& r01, K& r02, 00016 K& r10, K& r11, K& r12, 00017 K& r20, K& r21, K& r22, 00018 const K& a00, const K& a01, const K& a02, 00019 const K& a10, const K& a11, const K& a12, 00020 const K& a20, const K& a21, const K& a22 ) 00021 { 00022 r00 = a11*a22-a12*a21; 00023 r01 = -a01*a22+a02*a21; 00024 r02 = a01*a12-a02*a11; 00025 r10 = -a10*a22+a12*a20; 00026 r11 = a00*a22-a02*a20; 00027 r12 = -a00*a12+a02*a10; 00028 r20 = a10*a21-a11*a20; 00029 r21 = -a00*a21+a01*a20; 00030 r22 = a00*a11-a01*a10; 00031 }; 00032 00033 template<class K> 00034 void inv33( K& r00, K& r01, K& r02, 00035 K& r10, K& r11, K& r12, 00036 K& r20, K& r21, K& r22, 00037 const K& a00, const K& a01, const K& a02, 00038 const K& a10, const K& a11, const K& a12, 00039 const K& a20, const K& a21, const K& a22 ) 00040 00041 { 00042 tcoff33( r00, r01, r02, 00043 r10, r11, r12, 00044 r20, r21, r22, 00045 a00, a01, a02, 00046 a10, a11, a12, 00047 a20, a21, a22 ); 00048 K idet = 1.0/(a00*r00+a10*r01+a20*r02); 00049 r00 *= idet; 00050 r01 *= idet; 00051 r02 *= idet; 00052 r10 *= idet; 00053 r11 *= idet; 00054 r12 *= idet; 00055 r20 *= idet; 00056 r21 *= idet; 00057 r22 *= idet; 00058 }; 00059 00060 template<class K> 00061 K det33( const K& a00, const K& a01, const K& a02, 00062 const K& a10, const K& a11, const K& a12, 00063 const K& a20, const K& a21, const K& a22 ) 00064 { 00065 K tmp[9]; 00066 tcoff33( tmp[0], tmp[1], tmp[2], 00067 tmp[3], tmp[4], tmp[5], 00068 tmp[6], tmp[7], tmp[8], 00069 a00, a01, a02, 00070 a10, a11, a12, 00071 a20, a21, a22 ); 00072 return a00 * tmp[0] + a10 * tmp[1] + a20 * tmp[2]; 00073 }; 00074 00075 template<class K> inline 00076 K Sqrt( const K& x , const K& prec = 1e-19) 00077 { 00078 00079 K r = x/2; 00080 do 00081 { 00082 r = (r+(x/r))/2; 00083 } 00084 while( abs(r*r-x) > prec ); 00085 return r; 00086 }; 00087 00088 template<> inline 00089 double Sqrt( const double& x, const double& prec ) { return sqrt(x); }; 00090 template<> inline 00091 float Sqrt( const float& x, const float& prec ) { return sqrt(x); }; 00092 00093 #endif
