Point.h

//===========================================================================
// GoTools - SINTEF Geometry Tools version 1.1
//
// GoTools module: CORE
//
// Copyright (C) 2000-2007 SINTEF ICT, Applied Mathematics, Norway.
//
// This program 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 version 2 of the License. 
//
// This program 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 this program; if not, write to the Free Software            
// Foundation, Inc.,                                                      
// 59 Temple Place - Suite 330,                                           
// Boston, MA  02111-1307, USA.                                           
//
// Contact information: E-mail: tor.dokken@sintef.no                      
// SINTEF ICT, Department of Applied Mathematics,                         
// P.O. Box 124 Blindern,                                                 
// 0314 Oslo, Norway.                                                     
//
// Other licenses are also available for this software, notably licenses
// for:
// - Building commercial software.                                        
// - Building software whose source code you wish to keep private.        
//===========================================================================
#ifndef _GOPOINT_H
#define _GOPOINT_H


#include "Array.h"
#include "errormacros.h"
#include <algorithm>
#include <math.h>

namespace Go {



class Point
{
private:
    double* pstart_;
    int n_;
    bool owns_;

public:
    Point()
        : pstart_(0), n_(0), owns_(true)
    {}
    explicit Point(int dim)
        : pstart_(new double[dim]), n_(dim), owns_(true)
    {}
    Point(double x, double y)
        : pstart_(new double[2]), n_(2), owns_(true)
    {
        pstart_[0] = x;
        pstart_[1] = y;
    }
    Point(double x, double y, double z)
        : pstart_(new double[3]), n_(3), owns_(true)
    {
        pstart_[0] = x;
        pstart_[1] = y;
        pstart_[2] = z;
    }

    // @@@ 'typename T' must be present in order to compile on MS
    // v6.0. (Using just 'int Dim' and 'Array<double, Dim>' will cause
    // "internal compiler error" !) @jbt
    template <typename T, int Dim>
    explicit Point(const Array<T, Dim>& v)
        : pstart_(0), n_(Dim), owns_(true)
    {
        pstart_ = new double[n_];
        std::copy(v.begin(), v.end(), pstart_);
    }

// #ifndef _MSC_VER
    template <typename RandomAccessIterator>
    Point(RandomAccessIterator first, RandomAccessIterator last)
        : pstart_(0), n_(last - first), owns_(true)
    {
        pstart_ = new double[n_];
        std::copy(first, last, pstart_);
    }
// #endif
// #ifdef _MSC_VER
// #if _MSC_VER > 1200
//     template <typename ForwardIterator>
//     Point(ForwardIterator first, ForwardIterator last)
//      : pstart_(0), n_(last - first), owns_(true)
//     {
//      pstart_ = new double[n_];
//      std::copy(first, last, pstart_);
//     }
// #else // Old version
//     Point(const double* first, const double* last)
//      : pstart_(0), n_(last - first), owns_(true)
//     {
//      pstart_ = new double[n_];
//      std::copy(first, last, pstart_);
//     }
//     Point(const float* first, const float* last)
//      : pstart_(0), n_(last - first), owns_(true)
//     {
//      pstart_ = new double[n_];
//      std::copy(first, last, pstart_);
//     }
// #endif // _MSC_VER > 1200
// #endif

    Point(double* begin, double* end, bool own)
        : pstart_(0), n_(end-begin), owns_(own)
    {
        if (owns_) {
            pstart_ = new double[n_];
            std::copy(begin, end, pstart_);
        } else {
            pstart_ = begin;
        }
    }

    Point(const Point& v)
        : pstart_(0), n_(v.n_), owns_(true)
    {
        pstart_ = new double[n_];
        std::copy(v.pstart_, v.pstart_ + n_, pstart_);
    }

    Point& operator = (const Point &v)
    {
        Point temp(v);
        swap(temp);
        return *this;
    }

    ~Point()
    {
        if (owns_) delete [] pstart_;
    }

    void swap(Point& other)
    {
        std::swap(pstart_, other.pstart_);
        std::swap(n_, other.n_);
        std::swap(owns_, other.owns_);
    }

    void read(std::istream& is);

    void write(std::ostream& os) const;


    const double& operator [] (int i) const  { return pstart_[i]; }
    double& operator [] (int i)        { return pstart_[i]; }
    const double* begin() const { return pstart_; }
    double* begin()       { return pstart_; }
    const double* end() const { return pstart_ + n_; }
    double* end()       { return pstart_ + n_; }
    int size() const { return n_; }
    int dimension() const { return n_; }

    void resize(int d)
    {
        if (n_ < d) {
            Point temp(d);
            swap(temp);
        } else {
            n_ = d;
        }
    }

    void  setValue(double x, double y)
    {
        resize(2);
        pstart_[0] = x;
        pstart_[1] = y;
    }
    void  setValue(double x, double y, double z)
    {
        resize(3);
        pstart_[0] = x;
        pstart_[1] = y;
        pstart_[2] = z;
    }
    void  setValue(const double* array)
    {
        for (int i = 0; i < n_; ++i) {
            pstart_[i] = array[i];
        }
    }
    void  setValue(double val)
    {
        for (int i = 0; i < n_; ++i) {
            pstart_[i] = val;
        }
    }

    void resetValue(int idx, double val)
        {
            DEBUG_ERROR_IF(idx < 0 || idx >= n_,
                 "Dimension mismatch.");
            pstart_[idx] = val;
        }

    double length2() const
    {
        double l2 = 0;
        for (int i = 0; i < n_; ++i)
            l2 += pstart_[i]*pstart_[i];
        return l2;
    }
    double length()  const
    {
        return sqrt(length2());
    }
    double lengthInf() const
    {
        double linf = 0;
        for (int i = 0; i < n_; ++i) {
            linf = std::max(linf, fabs(pstart_[i]));
        }
        return linf;
    }
    double dist2(const Point &v) const
    {
        double l2 = 0;
        double d;
        for (int i = 0; i < n_; ++i) {
            d = pstart_[i] - v.pstart_[i];
            l2 += d*d;
        }
        return l2;
    }
    double dist(const Point &v) const
    {
        return sqrt(dist2(v));
    }
    double distInf(const Point &v) const
    {
        double linf = 0;
        double d;
        for (int i = 0; i < n_; ++i) {
            d = pstart_[i] - v.pstart_[i];
            linf = std::max(linf, fabs(d));
        }
        return linf;
    }
    void normalize()
    {
        double tl = length();
        DEBUG_ERROR_IF(tl == 0.0, "Cannot normalize vector of zero length");
        (*this) /= tl;
    }

    double normalize_checked() 
    {
        double tl = length(); 
        if (tl<1.0e-12)
            return 0;

        (*this) /= tl; 
        return tl;
    }

    Point  operator + (const Point     &v) const
    {
        Point res(*this);
        res += v;
        return res;
    }
    void     operator +=(const Point     &v)
    {
        DEBUG_ERROR_IF(n_!=v.n_,
                 "Dimension mismatch.");
        for (int i = 0; i < n_; ++i)
            pstart_[i] += v.pstart_[i];
    }

    Point  operator - (const Point     &v) const
    {
        Point res(*this);
        res -= v;
        return res;
    }
    void     operator -=(const Point     &v)
    {
        DEBUG_ERROR_IF(n_!=v.n_,
                 "Dimension mismatch.");
        for (int i = 0; i < n_; ++i)
            pstart_[i] -= v.pstart_[i];
    }
    Point  operator * (double d) const
    {
        Point res(*this);
        res *= d;
        return res;
    }
    void     operator *=(double d)
    {
        for (int i = 0; i < n_; ++i)
            pstart_[i] *= d;
    }

    Point  operator / (double d) const
    {
        Point res(*this);
        res /= d;
        return res;
    }
    void     operator /=(double d)
    {
        for (int i = 0; i < n_; ++i)
            pstart_[i] /= d;
    }

    Point  operator - () const
    {
        Point res(*this);
        for (int i = 0; i < n_; ++i)
            res.pstart_[i] = - pstart_[i];
        return res;
    }

    double   operator * (const Point     &v) const
    {
        DEBUG_ERROR_IF(n_!=v.n_,
                 "Dimension mismatch.");
        double res = 0;
        for (int i = 0; i < n_; ++i)
            res += pstart_[i]*v.pstart_[i];
        return res;
    }

    Point  operator % (const Point     &v) const
    {
        DEBUG_ERROR_IF(n_ != 3 || v.n_ != 3,
                 "Dimension mismatch.");
        return Point(pstart_[1]*v.pstart_[2] - pstart_[2]*v.pstart_[1],
                     pstart_[2]*v.pstart_[0] - pstart_[0]*v.pstart_[2],
                     pstart_[0]*v.pstart_[1] - pstart_[1]*v.pstart_[0]);
    }

    Point cross(const Point     &v) const
    {
        return operator%(v);
    }

    void setToCrossProd(const Point &u, const Point &v)
    {
        DEBUG_ERROR_IF(u.n_!=v.n_,
                 "Dimension mismatch.");
        DEBUG_ERROR_IF(u.n_!=3,
                 "Dimension must be 3.");

        bool have_already = owns_ && (n_ >= v.n_);
        if (!have_already) {
            Point temp(3);
            swap(temp);
        } else {
            n_ = 3;
        }
        pstart_[0]=u.pstart_[1]*v.pstart_[2] - u.pstart_[2]*v.pstart_[1];
        pstart_[1]=u.pstart_[2]*v.pstart_[0] - u.pstart_[0]*v.pstart_[2];
        pstart_[2]=u.pstart_[0]*v.pstart_[1] - u.pstart_[1]*v.pstart_[0];
    }

    double cosAngle(const Point& v) const
    {
        double tl1 = length();
        double tl2 = v.length();
        //(DEBUG_ERROR_IF(tl1*tl2 == 0.0, "Vector of zero length");
        if (tl1*tl2 == 0.0) 
        {
            //MESSAGE("Vector of zero length in angle compuation");
            return 0.0;
        }
        double res = ((*this)*v)/(tl1*tl2);
        res = std::min(1.0, std::max(-1.0, res));
        return res;
    }

    double angle(const Point& v) const
    {
        return acos(cosAngle(v));
    }

    double angle_smallest(const Point& v) const
    {
        double tl1 = length();
        double tl2 = v.length();
        //DEBUG_ERROR_IF(tl1*tl2 == 0.0, "Vector of zero length");
        double tcos = fabs(((*this)*v)/(tl1*tl2));
        tcos = std::min(1.0, tcos);
        return acos(tcos);
    }

};

    inline Point operator * (double d, const Point& p)
    { return p*d; }

    inline std::istream& operator>>(std::istream& is, Go::Point& v)
    { v.read(is); return is; }

    inline std::ostream& operator<<(std::ostream& os, const Go::Point& v)
    { v.write(os); return os; }

    // comparison operator
    inline bool operator<(const Point& p1, const Point& p2)
    {
        const int dim = p1.dimension();
        DEBUG_ERROR_IF(p2.dimension() != dim, "Dimension Mismatch");
        for (int i = dim-1; i >= 0; --i) {
            if (p1[i] > p2[i]) return false;
            if (p1[i] < p2[i]) return true;
        }
        return false;
    }

} // End of namespace Go



#endif // _GOPOINT_H

---