geometry.h

//
//  geometry.h
//  GLAMER
//
//  Created by bmetcalf on 7/24/14.
//
//
 
#ifndef __GLAMER__geometry__
#define __GLAMER__geometry__
 
#include <iostream>
#include <array>
#include "standard.h"
#include "point.h"
//
//  geometry.h
//  GLAMER
//
//  Created by bmetcalf on 7/23/14.
//
//
namespace Utilities {

namespace Geometry{

template <typename T = double>
class SphericalPoint{
  
public:
  SphericalPoint(T r,T theta,T phi):
  r(r),theta(theta),phi(phi){};
  SphericalPoint():r(0),theta(0),phi(0){};
  SphericalPoint(Point_3d<T> &x){
    TOspherical(x);
  }
  
  SphericalPoint & operator=(const SphericalPoint &p){
    if(&p != this){
      r = p.r;
      theta = p.theta;
      phi = p.phi;
    }
    return *this;
  }
  
  SphericalPoint & operator=(Point_3d<T> &x){
    TOspherical(x);
    return *this;
  }
  
  bool operator==(const SphericalPoint &p) const{
    if(&p == this) return true;
    return (r==p.r)*(theta==p.theta)*(phi==p.phi);
  }
  
  SphericalPoint operator*(T a){
    SphericalPoint<T> p = *this;
    p.r *= a;
    return p;
  }
  
  SphericalPoint & operator*=(T a){
    r *= a;
    return *this;
  }
  
  T operator*(const SphericalPoint<T> &p){
    return r * p.r * ( cos(theta)*cos(p.theta)*cos(phi-p.phi) + sin(theta)*sin(p.theta) );
  }
  
  T r;
  T theta;
  T phi;
  
  void TOcartisian(T x[]) const;
  Point_3d<T> TOcartisian() const;
  
  void cartisianTOspherical(T const x[]);
  void TOspherical(Point_3d<T> &x);
  
  void StereographicProjection(const SphericalPoint &central
                               ,T x[]) const;
  void StereographicProjection(const SphericalPoint &central,Point_2d &x) const;
  Point_2d StereographicProjection(const SphericalPoint &central) const;
  
  void OrthographicProjection(const SphericalPoint &central
                              ,T x[]) const;
  Point_2d OrthographicProjection(const SphericalPoint &central) const;
  void InverseOrthographicProjection(const SphericalPoint &central,T const x[]);
  void InverseOrthographicProjection(const SphericalPoint &central,const Point_2d &x);
  SphericalPoint<T> InverseOrthographicProjection(const Point_2d &x);
  
  
  T angular_separation(SphericalPoint &p){
    double s1 = sin((theta - p.theta)/2);
    double s2 = sin((phi - p.phi)/2);
    
    return 2*asin(sqrt( s1*s1 + cos(theta)*cos(p.theta)*s2*s2 ) );
  }
  
  Point_3d<T> unitPhi(){return phi_hat();};
  Point_3d<T> unitTheta(){return theta_hat();};
  
  T OrthographicAngleTheta(const SphericalPoint &central);
  T OrthographicAnglePhi(const SphericalPoint &central);
  
  // returns the unit theta vector
  Point_3d<T> theta_hat() const{
    return Point_3d<T>(-cos(phi)*sin(theta),-sin(theta)*sin(phi),cos(theta));
  }
  
  // returns the unit phi vector
  Point_3d<T> phi_hat() const{
    return Point_3d<T>(-sin(phi),cos(phi),0);
  }
  
};
 

template <typename T = double>
class Quaternion{
public:
  Quaternion(){
    v[0] = v[1] = v[2] = v[3] = 0;
  }
  Quaternion(T s,T x,T y,T z){
    v[0] = s;
    v[1] = x;
    v[2] = y;
    v[3] = z;
  }
  Quaternion(const Quaternion &q){
    v[0] = q.v[0];
    v[1] = q.v[1];
    v[2] = q.v[2];
    v[3] = q.v[3];
  }
  Quaternion(Point_3d<T> p){
    v[0] = 0;
    v[1] = p[0];
    v[2] = p[1];
    v[3] = p[2];
  }
  Quaternion(SphericalPoint<T> sp){
    sp.TOcartisian(v+1);
    v[0] = 0;
  }
  
  Quaternion &operator=(const Quaternion &q){
    v[0] = q.v[0];
    v[1] = q.v[1];
    v[2] = q.v[2];
    v[3] = q.v[3];
    
    return *this;
  }
  
  Quaternion operator*(const Quaternion &q) const{
    return Quaternion(
                      v[0]*q.v[0] - v[1]*q.v[1] - v[2]*q.v[2] - v[3]*q.v[3],
                      v[0]*q.v[1] + v[1]*q.v[0] + v[2]*q.v[3] - v[3]*q.v[2],
                      v[0]*q.v[2] - v[1]*q.v[3] + v[2]*q.v[0] + v[3]*q.v[1],
                      v[0]*q.v[3] + v[1]*q.v[2] - v[2]*q.v[1] + v[3]*q.v[0]);
  }
  
  Quaternion conj() const{
    return Quaternion(v[0],-v[1],-v[2],-v[3]);
  }
  Quaternion inverse() const{
    return this->conj()/(v[0]*v[0] + v[1]*v[1] + v[2]*v[2] + v[3]*v[3]);
  }
  
  double norm() const{
    return sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2] + v[3]*v[3]);
  }
  
  Quaternion RotateX(T theta){
    Quaternion R = q_x_rotation(theta);
    return R*(*this)*R.conj();
  }
  Quaternion RotateY(T theta){
    Quaternion R = q_y_rotation(theta);
    return R*(*this)*R.conj();
  }
  Quaternion RotateZ(T theta){
    Quaternion R = q_z_rotation(theta);
    return R*(*this)*R.conj();
  }
  Quaternion RotateYZ(T theta,T phi){
    Quaternion R = q_z_rotation(theta)*q_z_rotation(phi);
    return R*(*this)*R.conj();
  }
  
  Quaternion Rotate(const Quaternion &R) const{
    return R*(*this)*R.conj();
  }
  
  void RotInplace(const Quaternion &R){
    *this = R*(*this)*R.conj();
  }
  
  
  static Point_3d<T> Rotate(const Point_3d<T> &p
                            ,const Quaternion &R){
    Quaternion q(p);
    q.RotInplace(R);
    return q.to_point_3d();
  }
  
  
  static SphericalPoint<T> Rotate(const SphericalPoint<T> &p
                                  ,const Quaternion &R){
    Quaternion q(p);
    q.RotInplace(R);
    return q.to_SpericalPoint();
  }
  
  Quaternion operator+(const Quaternion &q) const{
    return Quaternion(v[0] + q.v[0] , v[1] + q.v[1] , v[2] + q.v[2] , v[3] + q.v[3] );
    
  }
  Quaternion operator-(const Quaternion &q) const{
    return Quaternion(v[0] - q.v[0] , v[1] - q.v[1] , v[2] - q.v[2] , v[3] - q.v[3] );
  }
  
  Quaternion operator/(double s) const{
    return Quaternion(v[0]/s,v[1]/s,v[2]/s,v[3]/s);
  }
  Quaternion operator*(double s) const{
    return Quaternion(v[0]*s,v[1]*s,v[2]*s,v[3]*s);
  }
  
  Point_3d<T> cross(const Quaternion &q) const{
    return (*this*q).to_point_3d();
  }
  
  T scalor(const Quaternion &q) const{
    return v[1]*q.v[1] + v[2]*q.v[2] + v[3]*q.v[3];
  }
  
  Point_3d<T> to_point_3d() const{
    return Point_3d<T>(v[1],v[2],v[3]);
  }
  
  SphericalPoint<T> to_SpericalPoint() const{
    SphericalPoint<T> sp;
    sp.cartisianTOspherical(v+1);
    return sp;
  }
  
  
  T v[4];
  T & operator[](int i){return v[i];}
  
  static Quaternion q_x_rotation(T theta){
    return Quaternion(cos(theta/2),sin(theta/2),0,0);
  }
  static Quaternion q_y_rotation(T theta){
    return Quaternion(cos(theta/2),0,-sin(theta/2),0);
  }
  static Quaternion q_z_rotation(T theta){
    return Quaternion(cos(theta/2),0,0,sin(theta/2));
  }
  
};

bool intersect(const PosType a1[],const PosType a2[]
               ,const PosType b1[],const PosType b2[]);
int intersect(const std::vector<Point_2d> &curve);
int intersections(const std::vector<Point_2d> &curve,std::vector<Point_2d> &intersections,std::vector<std::pair<int,int> > &segments);

int orientation(const PosType p[],const PosType q[],const PosType r[]);
bool onSegment(const PosType p[], const PosType q[], const PosType r[]);

double AngleBetween2d(double v1[],double v2[]);
 
}
}
 
namespace Utilities {
namespace Geometry{

template <typename T>
void SphericalPoint<T>::TOcartisian(T x[]) const{
  x[0] = r*cos(theta)*cos(phi);
  x[1] = r*cos(theta)*sin(phi);
  x[2] = r*sin(theta);
}

template <typename T>
Point_3d<T> SphericalPoint<T>::TOcartisian() const{
  return Point_3d<T>(r*cos(theta)*cos(phi)
                     ,r*cos(theta)*sin(phi),r*sin(theta) );
}

template <typename T>
void SphericalPoint<T>::cartisianTOspherical(T const x[]){
  r = sqrt( x[0]*x[0] + x[1]*x[1] +x[2]*x[2]);
  theta = asin(x[2]/r);
  phi = atan2(x[1],x[0]);
}
 
template <typename T>
void SphericalPoint<T>::TOspherical(Point_3d<T> &x){
  r = sqrt( x[0]*x[0] + x[1]*x[1] +x[2]*x[2]);
  theta = asin(x[2]/r);
  phi = atan2(x[1],x[0]);
}
 

template <typename T>
void SphericalPoint<T>::StereographicProjection(
                                                const SphericalPoint<T> &central   
                                                ,T x[]             
) const{
  double ct = cos(theta),st = sin(theta);
  double cosphi = cos(phi - central.phi);
  double so = sin(central.theta);
  double co = cos(central.theta);
  
  PosType k = 2/( 1 + so*st + co*ct*cosphi );
  
  x[0] = k*(ct*sin(phi - central.phi));
  x[1] = k*(co*st - so*ct*cosphi);
}
template <typename T>
void SphericalPoint<T>::StereographicProjection(
                                                const SphericalPoint<T> &central   
                                                ,Point_2d &x  
) const{
  double ct = cos(theta),st = sin(theta);
  double cosphi = cos(phi - central.phi);
  double so = sin(central.theta);
  double co = cos(central.theta);
  
  PosType k = 2/( 1 + so*st + co*ct*cosphi );
  
  x[0] = k*(ct*sin(phi - central.phi));
  x[1] = k*(co*st - so*ct*cosphi);
}
 
template <typename T>
Point_2d SphericalPoint<T>::StereographicProjection(
                                                    const SphericalPoint<T> &central   
) const{
  double ct = cos(theta),st = sin(theta);
  double cosphi = cos(phi - central.phi);
  double so = sin(central.theta);
  double co = cos(central.theta);
  
  PosType k = 2/( 1 + so*st + co*ct*cosphi );
  
  return Point_2d(k*(ct*sin(phi - central.phi)),k*(co*st - so*ct*cosphi));
}
 

template <typename T>
void SphericalPoint<T>::OrthographicProjection(
                                               const SphericalPoint<T> &central   
                                               ,T x[]             
) const{
  double ct = cos(theta),st = sin(theta);
  double cosphi = cos(phi - central.phi);
  double so = sin(central.theta);
  double co = cos(central.theta);
  
  x[0] = ct*sin(phi - central.phi);
  x[1] = co*st - so*ct*cosphi;
  
  if(st*so + ct*co*cosphi < 0 ){  // point is on the other hemosphere
    double s = sqrt( x[0]*x[0] + x[1]*x[0] );
    x[0] /=s;
    x[1] /=s;
  }
}
 
template <typename T>
Point_2d SphericalPoint<T>::OrthographicProjection(
                                                   const SphericalPoint<T> &central   
) const{
  double c = cos(theta);
  return Point_2d( c*sin(phi - central.phi),
                  cos(central.theta)*sin(theta) - sin(central.theta)*c*cos(phi - central.phi) );
}

template <typename T>
void SphericalPoint<T>::InverseOrthographicProjection(
                                                      const SphericalPoint<T> &central   
                                                      ,T const x[]             
){
  PosType rho = sqrt(x[0]*x[0] + x[1]*x[1]);
  PosType c = asin(rho);
  r=1.0;
  theta = asin( cos(c)*sin(central.theta) + x[1]*cos(central.theta) );
  phi = central.phi + atan2(x[0] , cos(central.theta)*cos(c)
                            - x[1]*sin(central.theta) );
}
 
template <typename T>
void SphericalPoint<T>::InverseOrthographicProjection(
                                                      const SphericalPoint<T> &central   
                                                      ,const Point_2d &x             
){
  PosType rho = sqrt(x[0]*x[0] + x[1]*x[1]);
  PosType c = asin(rho);
  r=1.0;
  double at=cos(c)*sin(central.theta) + x[1]*cos(central.theta);
  at = (at > 1) ? 1 : at;
  at = (at < -1) ? -1 : at;
  theta = asin( at );
  phi = central.phi + atan2(x[0] , cos(central.theta)*cos(c)
                            - x[1]*sin(central.theta) );
}
 
template <typename T>
T SphericalPoint<T>::OrthographicAngleTheta(
                                            const SphericalPoint<T> &central   
){
  return atan2( sin(central.theta) * sin(phi - central.phi)
               , cos(phi - central.phi) );
}
 
template <typename T>
T SphericalPoint<T>::OrthographicAnglePhi(
                                          const SphericalPoint<T> &central   
){
  return atan2( cos(central.theta) * cos(theta) + sin(central.theta) * sin(central.theta)
               *cos(phi - central.phi)
               , -sin(phi - central.phi) * sin(phi - central.phi) );
}

template <typename T>
SphericalPoint<T> SphericalPoint<T>::InverseOrthographicProjection(
                                                                   const Point_2d &x             
){
  PosType rho = sqrt(x[0]*x[0] + x[1]*x[1]);
  if(rho==0) return *this;
  
  PosType c = asin(rho);
  T new_theta = asin( cos(c)*sin(theta) + x[1]*sin(c)*cos(theta)/rho );
  T new_phi = phi + atan2(x[0]*sin(c),rho*cos(theta)*cos(c)
                          - x[1]*sin(theta)*sin(c) );
  
  return SphericalPoint<T>(1,new_theta,new_phi);
}

template <typename T>
T Seporation(const SphericalPoint<T> &p1
             ,const SphericalPoint<T> &p2){
  T x1[3],x2[3];
  
  p1.TOcartisian(x1);
  p2.TOcartisian(x2);
  return sqrt( (x1[0]-x2[0])*(x1[0]-x2[0]) + (x1[1]-x2[1])*(x1[1]-x2[1])
              + (x1[2]-x2[2])*(x1[2]-x2[2]) );
}

template <typename T>
T AngleSeporation(const SphericalPoint<T> &p1
                  ,const SphericalPoint<T> &p2){
  if(p1 == p2) return 0;
  double ans = sin(p1.theta)*sin(p2.theta) + cos(p1.theta)*cos(p2.theta)*cos(p1.phi-p2.phi);
  if(fabs(ans) > 1) return 0;
  return acos(sin(p1.theta)*sin(p2.theta) + cos(p1.theta)*cos(p2.theta)*cos(p1.phi-p2.phi));
}
 

struct CYCLIC{
  CYCLIC(long n):n(n){}
  
  // returns the integer in bounds
  long operator[](long i){
    while(i<0) i+=n;
    return i%n;
  }
  
  long n;
};
 
/* \brief finds the edge-wise concave hull of a complex curve
 
 The returned curve will consist of segments of the edges of the original curve that form a closed
 curve that does not cross itself and will enclose all the verticies.
 */
//std::vector<Point_2d> MagicHull(const std::vector<Point_2d> &points);
 
std::vector<Point_2d> DeInterset(const std::vector<Point_2d> &points);
  
}
}
 
template <typename T>
std::ostream &operator<<(std::ostream &os, Utilities::Geometry::SphericalPoint<T> const &p){
  return os << "r: " << p.r << " theta: " << p.theta << " phi: " << p.phi;
}
 
 
#endif /* defined(__GLAMER__geometry__) */