Ptv_tools.cpp

Go to the documentation of this file.

// Copyright (C) 2006-2009 Kent-Andre Mardal and Simula Research Laboratory
//
// This file is part of SyFi.
//
// SyFi is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 2 of the License, or
// (at your option) any later version.
//
// SyFi is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with SyFi. If not, see <http://www.gnu.org/licenses/>.

#include "Ptv_tools.h"
#include <iostream>
#include <stdexcept>
#include <math.h>
#include <vector>
#include <algorithm>

using namespace std;

namespace SyFi
{

        void sort_vector(vector<Ptv>& a)
        {
                sort(a.begin(), a.end(), Ptv_is_less());
        }

        void set_tolerance(double tolerance)
        {
                Ptv::tol = tolerance;
        }

        double mul(const Ptv&a, const Ptv& b)
        {
                if ( a.size() != b.size() )
                {
                        throw(std::logic_error("Exception from mul(const Ptv&, const Ptv&):  The dimentions of a and b must be the same."));
                }

                double sum = 0;
                for (unsigned int i=0; i< a.size(); i++)
                {
                        sum += (a[i])*(b[i]);
                }
                return sum;
        }

        double norm(const Ptv& a)
        {
                double sum = 0.0;
                for (unsigned int i=0; i < a.size(); i++)
                {
                        sum += a[i]*a[i];
                }

                sum = sqrt(sum);
                return sum;
        }

        void normalize(Ptv& a)
        {
                double invn = 1.0/norm(a);
                for (unsigned int i=0; i< a.size(); i++)
                {
                        a[i] *= invn;
                }
        }

        void add(const Ptv&a, const Ptv& b, Ptv& c)
        {
                if ( a.size() != b.size() )
                {
                        throw(std::logic_error("Exception from add(const Ptv&, const Ptv&, Ptv&):  The dimentions of a and b must be the same."));
                }

                c.redim(a.size());
                for (unsigned int i=0; i< c.size(); i++)
                {
                        c[i] = a[i] + b[i];
                }
        }

        void sub(const Ptv&a, const Ptv& b, Ptv& c)
        {
                if ( a.size() != b.size() )
                {
                        throw(std::logic_error("Exception from add(const Ptv&, const Ptv&, Ptv&):  The dimentions of a and b must be the same."));
                }

                c.redim(a.size());
                for (unsigned int i=0; i< c.size(); i++)
                {
                        c[i] = a[i] - b[i];
                }
        }

        void cross(const Ptv& a, const Ptv& b, Ptv& c)
        {
                if ( a.size() != b.size() )
                {
                        throw(std::logic_error("Exception from cross (const Ptv&, const Ptv&, Ptv&): The dimentions of a and b must be the same."));
                }

                if ( a.size() == 2 )
                {
                        c.redim(1);
                        c[0] = a[0]*b[1] - a[1]*b[0];
                }

                else if ( a.size() == 3 )
                {
                        c.redim(3);
                        c[0] =   a[1]*b[2] - b[1]*a[2];
                        c[1] = - a[0]*b[2] + b[0]*a[2];
                        c[2] =   a[0]*b[1] - b[0]*a[1];
                }

                else
                {
                        throw(std::logic_error("The cross product can only be computed in 2D and 3D."));
                }

        }

        bool is_equal(Ptv& a, Ptv& b)
        {
                if (a.size() != b.size()) return false;

                for (unsigned int i=0; i < a.size(); i++ )
                {
                        if ( fabs( a[i] - b[i]) > Ptv::tol )
                        {
                                return false;
                        }
                }

                return true;
        }

        bool line_contains(Ptv& e0, Ptv& e1, Ptv& p)
        {

                if ( is_equal(e0, p) || is_equal(e1, p) ) return true;

                // vec0 = e1-e0
                Ptv vec0;
                sub(e1,e0, vec0);
                // vec1 = e1-p
                Ptv vec1;
                sub(e1, p, vec1);

                // check if the vec0 and vec1 are parallel
                Ptv c;
                cross(vec0, vec1, c);
                if (norm(c) > Ptv::tol)
                {
                        return false;
                }

                // check whether the edge (e0,e1) contains p .
                if ( e0.less(p) && e1.less(p) ) return false;
                if ( p.less(e0) && p.less(e1) ) return false;

                return true;
        }

        bool is_inside_triangle(Ptv& e0, Ptv& e1, Ptv& e2, Ptv& p)
        {

                Ptv n0;
                sub(e0, p, n0);
                normalize(n0);

                Ptv n1;
                sub(e1, p, n1);
                normalize(n1);

                Ptv n2;
                sub(e2, p, n2);
                normalize(n2);

                double c0 = acos(mul(n0,n1));
                double c1 = acos(mul(n1,n2));
                double c2 = acos(mul(n2,n1));

                if ( fabs(c0 + c1 + c2 - 2*3.1415926535897931) < Ptv::tol) return true;

                return false;
        }

        // FIXME this code can be made more general and put in
        // a more appropriate place. For now it is only used by
        // the boundary function. It only works in 2D.
        bool contains2D(Ptv& e0, Ptv& e1, Ptv& p)
        {

                if ( e0.size() != e1.size() || e0.size() != p.size()  )
                {
                        throw(std::logic_error("Exception from contains2D(Ptv&, Ptv&, Ptv&): The dimentions of a and b must be the same."));
                }

                bool b = line_contains(e0, e1, p);

                return b;
        }

        bool contains3D(Ptv& e0, Ptv& e1, Ptv& e2, Ptv& p)
        {

                // check if p is either e0, e1, or e2
                if ( is_equal(e0, p) )  return true;
                else if ( is_equal(e1, p) )  return true;
                else if ( is_equal(e2, p) )  return true;

                // check if p is on the lines connecting e0, e1, and e2
                if ( line_contains(e0, e1, p) ) return true;
                else if ( line_contains(e1, e2, p) ) return true;
                else if ( line_contains(e2, e1, p) ) return true;

                // check if p is inside the triangle with verticies e0, e1, and e2
                if ( is_inside_triangle(e0, e1, e2, p) ) return true;

                return false;

        }

}                                                                // namespace SyFi