SyFi 0.3
|
00001 // Copyright (C) 2006-2009 Kent-Andre Mardal and Simula Research Laboratory. 00002 // Licensed under the GNU GPL Version 2, or (at your option) any later version. 00003 00004 #include "Ptv_tools.h" 00005 #include <iostream> 00006 #include <stdexcept> 00007 #include <math.h> 00008 #include <vector> 00009 #include <algorithm> 00010 00011 using namespace std; 00012 00013 namespace SyFi 00014 { 00015 00016 void sort_vector(vector<Ptv>& a) 00017 { 00018 sort(a.begin(), a.end(), Ptv_is_less()); 00019 } 00020 00021 void set_tolerance(double tolerance) 00022 { 00023 Ptv::tol = tolerance; 00024 } 00025 00026 double mul(const Ptv&a, const Ptv& b) 00027 { 00028 if ( a.size() != b.size() ) 00029 { 00030 throw(std::logic_error("Exception from mul(const Ptv&, const Ptv&): The dimentions of a and b must be the same.")); 00031 } 00032 00033 double sum = 0; 00034 for (unsigned int i=0; i< a.size(); i++) 00035 { 00036 sum += (a[i])*(b[i]); 00037 } 00038 return sum; 00039 } 00040 00041 double norm(const Ptv& a) 00042 { 00043 double sum = 0.0; 00044 for (unsigned int i=0; i < a.size(); i++) 00045 { 00046 sum += a[i]*a[i]; 00047 } 00048 00049 sum = sqrt(sum); 00050 return sum; 00051 } 00052 00053 void normalize(Ptv& a) 00054 { 00055 double invn = 1.0/norm(a); 00056 for (unsigned int i=0; i< a.size(); i++) 00057 { 00058 a[i] *= invn; 00059 } 00060 } 00061 00062 void add(const Ptv&a, const Ptv& b, Ptv& c) 00063 { 00064 if ( a.size() != b.size() ) 00065 { 00066 throw(std::logic_error("Exception from add(const Ptv&, const Ptv&, Ptv&): The dimentions of a and b must be the same.")); 00067 } 00068 00069 c.redim(a.size()); 00070 for (unsigned int i=0; i< c.size(); i++) 00071 { 00072 c[i] = a[i] + b[i]; 00073 } 00074 } 00075 00076 void sub(const Ptv&a, const Ptv& b, Ptv& c) 00077 { 00078 if ( a.size() != b.size() ) 00079 { 00080 throw(std::logic_error("Exception from add(const Ptv&, const Ptv&, Ptv&): The dimentions of a and b must be the same.")); 00081 } 00082 00083 c.redim(a.size()); 00084 for (unsigned int i=0; i< c.size(); i++) 00085 { 00086 c[i] = a[i] - b[i]; 00087 } 00088 } 00089 00090 void cross(const Ptv& a, const Ptv& b, Ptv& c) 00091 { 00092 if ( a.size() != b.size() ) 00093 { 00094 throw(std::logic_error("Exception from cross (const Ptv&, const Ptv&, Ptv&): The dimentions of a and b must be the same.")); 00095 } 00096 00097 if ( a.size() == 2 ) 00098 { 00099 c.redim(1); 00100 c[0] = a[0]*b[1] - a[1]*b[0]; 00101 } 00102 00103 else if ( a.size() == 3 ) 00104 { 00105 c.redim(3); 00106 c[0] = a[1]*b[2] - b[1]*a[2]; 00107 c[1] = - a[0]*b[2] + b[0]*a[2]; 00108 c[2] = a[0]*b[1] - b[0]*a[1]; 00109 } 00110 00111 else 00112 { 00113 throw(std::logic_error("The cross product can only be computed in 2D and 3D.")); 00114 } 00115 00116 } 00117 00118 bool is_equal(Ptv& a, Ptv& b) 00119 { 00120 if (a.size() != b.size()) return false; 00121 00122 for (unsigned int i=0; i < a.size(); i++ ) 00123 { 00124 if ( fabs( a[i] - b[i]) > Ptv::tol ) 00125 { 00126 return false; 00127 } 00128 } 00129 00130 return true; 00131 } 00132 00133 bool line_contains(Ptv& e0, Ptv& e1, Ptv& p) 00134 { 00135 00136 if ( is_equal(e0, p) || is_equal(e1, p) ) return true; 00137 00138 // vec0 = e1-e0 00139 Ptv vec0; 00140 sub(e1,e0, vec0); 00141 // vec1 = e1-p 00142 Ptv vec1; 00143 sub(e1, p, vec1); 00144 00145 // check if the vec0 and vec1 are parallel 00146 Ptv c; 00147 cross(vec0, vec1, c); 00148 if (norm(c) > Ptv::tol) 00149 { 00150 return false; 00151 } 00152 00153 // check whether the edge (e0,e1) contains p . 00154 if ( e0.less(p) && e1.less(p) ) return false; 00155 if ( p.less(e0) && p.less(e1) ) return false; 00156 00157 return true; 00158 } 00159 00160 bool is_inside_triangle(Ptv& e0, Ptv& e1, Ptv& e2, Ptv& p) 00161 { 00162 00163 Ptv n0; 00164 sub(e0, p, n0); 00165 normalize(n0); 00166 00167 Ptv n1; 00168 sub(e1, p, n1); 00169 normalize(n1); 00170 00171 Ptv n2; 00172 sub(e2, p, n2); 00173 normalize(n2); 00174 00175 double c0 = acos(mul(n0,n1)); 00176 double c1 = acos(mul(n1,n2)); 00177 double c2 = acos(mul(n2,n1)); 00178 00179 if ( fabs(c0 + c1 + c2 - 2*3.1415926535897931) < Ptv::tol) return true; 00180 00181 return false; 00182 } 00183 00184 // FIXME this code can be made more general and put in 00185 // a more appropriate place. For now it is only used by 00186 // the boundary function. It only works in 2D. 00187 bool contains2D(Ptv& e0, Ptv& e1, Ptv& p) 00188 { 00189 00190 if ( e0.size() != e1.size() || e0.size() != p.size() ) 00191 { 00192 throw(std::logic_error("Exception from contains2D(Ptv&, Ptv&, Ptv&): The dimentions of a and b must be the same.")); 00193 } 00194 00195 bool b = line_contains(e0, e1, p); 00196 00197 return b; 00198 } 00199 00200 bool contains3D(Ptv& e0, Ptv& e1, Ptv& e2, Ptv& p) 00201 { 00202 00203 // check if p is either e0, e1, or e2 00204 if ( is_equal(e0, p) ) return true; 00205 else if ( is_equal(e1, p) ) return true; 00206 else if ( is_equal(e2, p) ) return true; 00207 00208 // check if p is on the lines connecting e0, e1, and e2 00209 if ( line_contains(e0, e1, p) ) return true; 00210 else if ( line_contains(e1, e2, p) ) return true; 00211 else if ( line_contains(e2, e1, p) ) return true; 00212 00213 // check if p is inside the triangle with verticies e0, e1, and e2 00214 if ( is_inside_triangle(e0, e1, e2, p) ) return true; 00215 00216 return false; 00217 00218 } 00219 00220 } // namespace SyFi