SyFi 0.3
Ptv_tools.cpp
Go to the documentation of this file.
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
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Defines