00001
00002
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
00139 Ptv vec0;
00140 sub(e1,e0, vec0);
00141
00142 Ptv vec1;
00143 sub(e1, p, vec1);
00144
00145
00146 Ptv c;
00147 cross(vec0, vec1, c);
00148 if (norm(c) > Ptv::tol)
00149 {
00150 return false;
00151 }
00152
00153
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
00185
00186
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
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
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
00214 if ( is_inside_triangle(e0, e1, e2, p) ) return true;
00215
00216 return false;
00217
00218 }
00219
00220 }