/*========================================================================
    Vector.h : Simple vector class for 2D and 3D vectors
  ------------------------------------------------------------------------
    Copyright (C) 1999-2002  James R. Bruce
    School of Computer Science, Carnegie Mellon University
  ------------------------------------------------------------------------
    This software is distributed under the GNU General Public License,
    version 2.  If you do not have a copy of this licence, visit
    www.gnu.org, or write: Free Software Foundation, 59 Temple Place,
    Suite 330 Boston, MA 02111-1307 USA.  This program is distributed
    in the hope that it will be useful, but WITHOUT ANY WARRANTY,
    including MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  ========================================================================*/

#ifndef __GVECTOR_H__
#define __GVECTOR_H__

#include <math.h>
#include "Util.h"

#define V3COMP(p) (p).x,(p).y,(p).z
#define V2COMP(p) (p).x,(p).y

namespace GVector {

//#define point3d vector3d
//#define point2d vector2d

/*
inline int sqrt(int n){
  return((int)sqrt((double)n));
}
*/

//=====================================================================//
//  Vector3D Class
//=====================================================================//

template <class num>
class vector3d{
public:
  num x,y,z;

  vector3d()
    {}
  vector3d(num nx,num ny,num nz)
    {x=nx; y=ny; z=nz;}

  void set(num nx,num ny,num nz)
    {x=nx; y=ny; z=nz;}
  void set(const vector3d<num> p)
    {x=p.x; y=p.y; z=p.z;}

  // I am broken. :....O(
  // We think it's a GCC bug.
  // vector3d a;
  // a = vector3d(1.0, 2.0, 3.0); 
  // actually fails. It's freaky.
  //vector3d<num> &operator=(const vector3d<num> p)
  //  {set(p); return(*this);}

  vector3d<num> &operator=(const vector3d<num> p)
    { x = p.x; y = p.y; z = p.z; return(*this);}

  num length() const;
  num sqlength() const;
  vector3d<num> norm() const;
  void normalize();

  num dot(const vector3d<num> p) const;
  vector3d<num> cross(const vector3d<num> p) const;

  vector3d<num> &operator+=(const vector3d<num> p);
  vector3d<num> &operator-=(const vector3d<num> p);
  vector3d<num> &operator*=(const vector3d<num> p);
  vector3d<num> &operator/=(const vector3d<num> p);

  vector3d<num> operator+(const vector3d<num> p) const;
  vector3d<num> operator-(const vector3d<num> p) const;
  vector3d<num> operator*(const vector3d<num> p) const;
  vector3d<num> operator/(const vector3d<num> p) const;

  vector3d<num> operator*(num f) const;
  vector3d<num> operator/(num f) const;
  vector3d<num> &operator*=(num f);
  vector3d<num> &operator/=(num f);

  vector3d<num> operator-() const;

  bool operator==(const vector3d<num> p) const;
  bool operator!=(const vector3d<num> p) const;
  bool operator< (const vector3d<num> p) const;
  bool operator> (const vector3d<num> p) const;
  bool operator<=(const vector3d<num> p) const;
  bool operator>=(const vector3d<num> p) const;

  vector3d<num> rotate_x(const double a) const;
  vector3d<num> rotate_y(const double a) const;
  vector3d<num> rotate_z(const double a) const;
};

template <class num>
num vector3d<num>::length() const
{
  return(sqrt(x*x + y*y + z*z));
}

template <class num>
num vector3d<num>::sqlength() const
{
  return(x*x + y*y + z*z);
}

template <class num>
vector3d<num> vector3d<num>::norm() const
{
  vector3d<num> p;
  num l;

  l = sqrt(x*x + y*y + z*z);
  if(l!=0.0) {
    p.x = x / l;
    p.y = y / l;
    p.z = z / l;
  }else{
    p.set(0.0,0.0,0.0);
  }

  return(p);
}

template <class num>
void vector3d<num>::normalize()
{
  num l;

  l = sqrt(x*x + y*y + z*z);
  if(l!=0.0) {
    x /= l;
    y /= l;
    z /= l;
  }
}

template <class num>
num vector3d<num>::dot(const vector3d<num> p) const
{
  return(x*p.x + y*p.y + z*p.z);
}

template <class num>
num dot(const vector3d<num> a,const vector3d<num> b)
{
  return(a.x*b.x + a.y*b.y + a.z*b.z);
}

template <class num>
vector3d<num> vector3d<num>::cross(const vector3d<num> p) const
{
  vector3d<num> r;

  // right handed
  r.x = y*p.z - z*p.y;
  r.y = z*p.x - x*p.z;
  r.z = x*p.y - y*p.x;

  return(r);
}

template <class num>
vector3d<num> cross(const vector3d<num> a,const vector3d<num> b)
{
  vector3d<num> r;

  // right handed
  r.x = a.y*b.z - a.z*b.y;
  r.y = a.z*b.x - a.x*b.z;
  r.z = a.x*b.y - a.y*b.x;

  return(r);
}

#define VECTOR3D_EQUAL_BINARY_OPERATOR(opr) \
  template <class num> \
  vector3d<num> &vector3d<num>::operator opr (const vector3d<num> p) \
  {                  \
    x = x opr p.x;   \
    y = y opr p.y;   \
    z = z opr p.z;   \
    return(*this);   \
  }

VECTOR3D_EQUAL_BINARY_OPERATOR(+=)
VECTOR3D_EQUAL_BINARY_OPERATOR(-=)
VECTOR3D_EQUAL_BINARY_OPERATOR(*=)
VECTOR3D_EQUAL_BINARY_OPERATOR(/=)

#define VECTOR3D_BINARY_OPERATOR(opr) \
  template <class num> \
  vector3d<num> vector3d<num>::operator opr (const vector3d<num> p) const \
  {                  \
    vector3d<num> r; \
    r.x = x opr p.x; \
    r.y = y opr p.y; \
    r.z = z opr p.z; \
    return(r);       \
  }

VECTOR3D_BINARY_OPERATOR(+)
VECTOR3D_BINARY_OPERATOR(-)
VECTOR3D_BINARY_OPERATOR(*)
VECTOR3D_BINARY_OPERATOR(/)

#define VECTOR3D_SCALAR_OPERATOR(opr) \
  template <class num> \
  vector3d<num> vector3d<num>::operator opr (const num f) const \
  {                  \
    vector3d<num> r; \
    r.x = x opr f;   \
    r.y = y opr f;   \
    r.z = z opr f;   \
    return(r);       \
  }

VECTOR3D_SCALAR_OPERATOR(*)
VECTOR3D_SCALAR_OPERATOR(/)

#define VECTOR3D_EQUAL_SCALAR_OPERATOR(opr) \
  template <class num> \
  vector3d<num> &vector3d<num>::operator opr (num f) \
  {                \
    x = x opr f;   \
    y = y opr f;   \
    z = z opr f;   \
    return(*this); \
  }

VECTOR3D_EQUAL_SCALAR_OPERATOR(*=)
VECTOR3D_EQUAL_SCALAR_OPERATOR(/=)

#define VECTOR3D_LOGIC_OPERATOR(opr,combine) \
  template <class num> \
  bool vector3d<num>::operator opr (const vector3d<num> p) const \
  {                            \
    return((x opr p.x) combine \
           (y opr p.y) combine \
           (z opr p.z));       \
  }

VECTOR3D_LOGIC_OPERATOR(==,&&)
VECTOR3D_LOGIC_OPERATOR(!=,||)

VECTOR3D_LOGIC_OPERATOR(< ,&&)
VECTOR3D_LOGIC_OPERATOR(> ,&&)
VECTOR3D_LOGIC_OPERATOR(<=,&&)
VECTOR3D_LOGIC_OPERATOR(>=,&&)

template <class num>
vector3d<num> vector3d<num>::operator-() const
{
  vector3d<num> r;
  r.x = -x;
  r.y = -y;
  r.z = -z;
  return(r);
}

template <class num>
inline vector3d<num> abs(vector3d<num> a)
{
  a.x = ::fabs(a.x);
  a.y = ::fabs(a.y);
  a.z = ::fabs(a.z);

  return(a);
}

template <class num>
inline vector3d<num> max(vector3d<num> a,vector3d<num> b)
{
  vector3d<num> v;

  v.x = ::max(a.x,b.x);
  v.y = ::max(a.y,b.y);
  v.z = ::max(a.z,b.z);

  return(v);
}

template <class num>
inline vector3d<num> bound(vector3d<num> v,num low,num high)
{
  v.x = ::bound(v.x,low,high);
  v.y = ::bound(v.y,low,high);
  v.z = ::bound(v.z,low,high);

  return(v);
}

template <class num>
inline vector3d<num> bound(vector3d<num> v,
			   vector3d<num> low, vector3d<num> high)
{
  v.x = ::bound(v.x,low.x,high.x);
  v.y = ::bound(v.y,low.y,high.y);
  v.z = ::bound(v.z,low.z,high.z);

  return(v);
}

// returns point rotated around X axis by <a> radians (right handed)
template <class num>
vector3d<num> vector3d<num>::rotate_x(const double a) const
{
  vector3d<num> q;
  double s,c;

  s = sin(a);
  c = cos(a);

  q.x = x;
  q.y = c*y + -s*z;
  q.z = s*y + c*z;

  return(q);
}

// returns point rotated around Y axis by <a> radians (right handed)
template <class num>
vector3d<num> vector3d<num>::rotate_y(const double a) const
{
  vector3d<num> q;
  double s,c;

  s = sin(a);
  c = cos(a);

  q.x = c*x + s*z;
  q.y = y;
  q.z = -s*x + c*z;

  return(q);
}

// returns point rotated around Z axis by <a> radians (right handed)
template <class num>
vector3d<num> vector3d<num>::rotate_z(const double a) const
{
  vector3d<num> q;
  double s,c;

  s = sin(a);
  c = cos(a);

  q.x = c*x + -s*y;
  q.y = s*x + c*y;
  q.z = z;

  return(q);
}

// returns distance between two points
template <class num>
num distance(const vector3d<num> a,const vector3d<num> b)
{
  num dx,dy,dz;

  dx = a.x - b.x;
  dy = a.y - b.y;
  dz = a.z - b.z;

  return(sqrt(dx*dx + dy*dy + dz*dz));
}

// returns square of distance between two points
template <class num>
num sdistance(const vector3d<num> a,const vector3d<num> b)
{
  num dx,dy,dz;

  dx = a.x - b.x;
  dy = a.y - b.y;
  dz = a.z - b.z;

  return(dx*dx + dy*dy + dz*dz);
}

// returns distance from point p to line x0-x1
template <class num>
num distance_to_line(const vector3d<num> x0,const vector3d<num> x1,const vector3d<num> p)
{
  vector3d<num> n,v;

  // get normal to line
  n = x1 - x0;
  n.set(n.y,-n.x);

  v = p - x0;

  return(fabs(n.dot(v)));
}


//=====================================================================//
//  Vector2D Class
//=====================================================================//

template <class num>
class vector2d{
public:
  num x,y;

  vector2d()
    {}
  vector2d(num nx,num ny)
    {x=nx; y=ny;}

  void set(num nx,num ny)
    {x=nx; y=ny;}
  void set(vector2d<num> p)
    {x=p.x; y=p.y;}
  vector2d<num> &operator=(vector2d<num> p)
    {set(p); return(*this);}

  num length() const;
  num sqlength() const;
  num angle() const
    {return(atan2a(y,x));}

  vector2d<num> norm() const;
  void normalize();

  num dot(const vector2d<num> p) const;
  num cross(const vector2d<num> p) const;

  // rotate by pi/2 CCW
  vector2d<num> perp() // FIXME:const me
    {return(vector2d(-y, x));}

  vector2d<num> &operator+=(const vector2d<num> p);
  vector2d<num> &operator-=(const vector2d<num> p);
  vector2d<num> &operator*=(const vector2d<num> p);
  vector2d<num> &operator/=(const vector2d<num> p);

  vector2d<num> operator+(const vector2d<num> p) const;
  vector2d<num> operator-(const vector2d<num> p) const;
  vector2d<num> operator*(const vector2d<num> p) const;
  vector2d<num> operator/(const vector2d<num> p) const;

  vector2d<num> operator*(const num f) const;
  vector2d<num> operator/(const num f) const;
  vector2d<num> &operator*=(num f);
  vector2d<num> &operator/=(num f);

  vector2d<num> operator-() const;

  bool operator==(const vector2d<num> p) const;
  bool operator!=(const vector2d<num> p) const;
  bool operator< (const vector2d<num> p) const;
  bool operator> (const vector2d<num> p) const;
  bool operator<=(const vector2d<num> p) const;
  bool operator>=(const vector2d<num> p) const;

  vector2d<num> rotate_fast(const num a) const;
  vector2d<num> rotate(const num a) const;
  // the source point and the basis are in the same frame in this one
  // "up" one reference frame
  vector2d<num> project(const vector2d<num> basis) const;
  // return a vector that matches the current vector in a new
  // basis defined by <new_basis, new_basis.perp()>
  // "down" one reference frame
  vector2d<num> rebase(const vector2d<num> new_basis) const;

  void add_to_bound(vector2d<num> &bound_low,vector2d<num> &bound_high) const;
};

template <class num>
num vector2d<num>::length() const
{
  return(sqrt(x*x + y*y));
}

template <class num>
num vector2d<num>::sqlength() const
{
  return(x*x + y*y);
}

template <class num>
vector2d<num> vector2d<num>::norm() const
{
  vector2d<num> p;
  num l;

  l = sqrt(x*x + y*y);
  if(l >= 1e-6) {
    p.x = x / l;
    p.y = y / l;
  }

  return(p);
}

template <class num>
void vector2d<num>::normalize()
{
  num l;

  l = sqrt(x*x + y*y);
  if(l!=0.0) {
    x /= l;
    y /= l;
  }
}

template <class num>
num vector2d<num>::dot(const vector2d<num> p) const
{
  return(x*p.x + y*p.y);
}

template <class num>
num dot(const vector2d<num> a,const vector2d<num> b)
{
  return(a.x*b.x + a.y*b.y);
}

template <class num>
num vector2d<num>::cross(const vector2d<num> p) const
{
  return(x*p.y - y*p.x);
}


#define VECTOR2D_EQUAL_BINARY_OPERATOR(opr) \
  template <class num> \
  vector2d<num> &vector2d<num>::operator opr (const vector2d<num> p) \
  {                  \
    x = x opr p.x;   \
    y = y opr p.y;   \
    return(*this);   \
  }

VECTOR2D_EQUAL_BINARY_OPERATOR(+=)
VECTOR2D_EQUAL_BINARY_OPERATOR(-=)
VECTOR2D_EQUAL_BINARY_OPERATOR(*=)
VECTOR2D_EQUAL_BINARY_OPERATOR(/=)

#define VECTOR2D_BINARY_OPERATOR(opr) \
  template <class num> \
  vector2d<num> vector2d<num>::operator opr (const vector2d<num> p) const \
  {                  \
    vector2d<num> r; \
    r.x = x opr p.x; \
    r.y = y opr p.y; \
    return(r);       \
  }

VECTOR2D_BINARY_OPERATOR(+)
VECTOR2D_BINARY_OPERATOR(-)
VECTOR2D_BINARY_OPERATOR(*)
VECTOR2D_BINARY_OPERATOR(/)

#define VECTOR2D_SCALAR_OPERATOR(opr) \
  template <class num> \
  vector2d<num> vector2d<num>::operator opr (const num f) const \
  {                  \
    vector2d<num> r;  \
    r.x = x opr f;   \
    r.y = y opr f;   \
    return(r);       \
  }

VECTOR2D_SCALAR_OPERATOR(*)
VECTOR2D_SCALAR_OPERATOR(/)

#define VECTOR2D_EQUAL_SCALAR_OPERATOR(opr) \
  template <class num> \
  vector2d<num> &vector2d<num>::operator opr (num f) \
  {                \
    x = x opr f;   \
    y = y opr f;   \
    return(*this); \
  }

VECTOR2D_EQUAL_SCALAR_OPERATOR(*=)
VECTOR2D_EQUAL_SCALAR_OPERATOR(/=)

#define VECTOR2D_LOGIC_OPERATOR(opr,combine) \
  template <class num> \
  bool vector2d<num>::operator opr (const vector2d<num> p) const \
  {                            \
    return((x opr p.x) combine \
           (y opr p.y));       \
  }

VECTOR2D_LOGIC_OPERATOR(==,&&)
VECTOR2D_LOGIC_OPERATOR(!=,||)

VECTOR2D_LOGIC_OPERATOR(< ,&&)
VECTOR2D_LOGIC_OPERATOR(> ,&&)
VECTOR2D_LOGIC_OPERATOR(<=,&&)
VECTOR2D_LOGIC_OPERATOR(>=,&&)


template <class num>
vector2d<num> vector2d<num>::operator-() const
{
  vector2d<num> r;
  r.x = -x;
  r.y = -y;
  return(r);
}

template <class num>
inline vector2d<num> abs(vector2d<num> a)
{
  a.x = ::fabs(a.x);
  a.y = ::fabs(a.y);

  return(a);
}

template <class num>
inline vector2d<num> max(vector2d<num> a,vector2d<num> b)
{
  vector2d<num> v;

  v.x = ::max(a.x,b.x);
  v.y = ::max(a.y,b.y);

  return(v);
}

template <class num>
inline vector2d<num> bound(vector2d<num> v,num low,num high)
{
  v.x = ::bound(v.x,low,high);
  v.y = ::bound(v.y,low,high);

  return(v);
}

template <class num>
inline vector2d<num> bound(vector2d<num> v,
			   vector2d<num> low, vector2d<num> high)
{
  v.x = ::bound(v.x,low.x,high.x);
  v.y = ::bound(v.y,low.y,high.y);

  return(v);
}

template <class num>
vector2d<num> vector2d<num>::rotate_fast(const num a) const {
  vector2d<num> q;
  double s,c;

  s = sinf(a);
  c = cosf(a);

  q.x = c*x + -s*y;
  q.y = s*x + c*y;

  return(q);
}

template <class num>
vector2d<num> vector2d<num>::rotate(const num a) const {
  vector2d<num> q;
  double s,c;

  s = sin(a);
  c = cos(a);

  q.x = c*x + -s*y;
  q.y = s*x + c*y;

  return(q);
}

template <class num>
vector2d<num> vector2d<num>::rebase(const vector2d<num> new_basis) const {
  vector2d<num> q;

  q.x =  new_basis.x*x + new_basis.y*y;
  q.y = -new_basis.y*x + new_basis.x*y;

  return(q);
}

template <class num>
vector2d<num> vector2d<num>::project(const vector2d<num> basis) const {
  vector2d<num> q;

  q.x =  basis.x*x - basis.y*y;
  q.y =  basis.y*x + basis.x*y;

  return(q);
}

template <class num>
void vector2d<num>::add_to_bound(vector2d<num> &bound_low,vector2d<num> &bound_high) const
{
  if(x < bound_low.x){
    bound_low.x = x;
  }else if(x > bound_high.x){
    bound_high.x = x;
  }

  if(y < bound_low.y){
    bound_low.y = y;
  }else if(y > bound_high.y){
    bound_high.y = y;
  }
}

template <class num>
num distance(const vector2d<num> a,const vector2d<num> b)
{
  num dx,dy;

  dx = a.x - b.x;
  dy = a.y - b.y;

  return(sqrt(dx*dx + dy*dy));
}

// returns square of distance between two points
template <class num>
num sdistance(const vector2d<num> a,const vector2d<num> b)
{
  num dx,dy;

  dx = a.x - b.x;
  dy = a.y - b.y;

  return(dx*dx + dy*dy);
}

// returns perpendicular offset from line x0-x1 to point p
template <class num>
num offset_to_line(const vector2d<num> x0,const vector2d<num> x1,const vector2d<num> p)
{
  vector2d<num> n,v;

  // get normal to line
  n = x1 - x0;
  n.set(n.y,-n.x);

  // bug fix???
  if(n.length()!=0)
    n.normalize();

  v = p - x0;

  return(n.dot(v));
}

// returns distance from point p to line x0-x1
template <class num>
num signed_distance_to_line(const vector2d<num> x0,const vector2d<num> x1,const vector2d<num> p)
{
  return(offset_to_line(x0,x1,p));
}

// returns distance from point p to line x0-x1
template <class num>
num distance_to_line(const vector2d<num> x0,const vector2d<num> x1,const vector2d<num> p)
{
  return(fabs(offset_to_line(x0,x1,p)));
}

// returns true if a0-a1 intersects b0-b1.
template <class num>
bool segment_intersect(const vector2d<num> a0, const vector2d<num>a1,
		       const vector2d<num> b0, const vector2d<num>b1) {
  
  vector2d<num> da, db;
  double t_a = 0.0, t_b = 0.0;
  
  da = a1 - a0;
  db = b1 - b0;

  // line 1 = a0 + da*t_a
  // line 2 = b0 + db*t_b
  
  // Solve for t_a and t_b.
  
  // Check for divide by zero (with lots of error)
  if(fabs(da.x*db.y - da.y*db.x) < 0.0001)
    return false;
  
  t_a = db.x*(a0.y - b0.y) - db.y*(a0.x - b0.x);
  t_a /= da.x*db.y - da.y*db.x;
  
  t_b = da.x*(a0.y - b0.y) - da.y*(a0.x - b0.x);
  t_b /= da.x*db.y - da.y*db.x;
  
  return t_a >= 0.0 && t_a <= 1.0 &&
    t_b >= 0.0 && t_b <= 1.0;
}

// returns true if the two lines (a and b) intersect and sets pt to the intersection pt
// each line is represented by a normal and an offset from the origin
template <class num>
bool intersect(vector2d<num> &pt,const vector2d<num> na, num off_a, const vector2d<num> nb, num off_b)
{
  num denom;

  denom = na.x*nb.y - na.y*nb.x;

  if(denom == 0.0)
    return false;

  pt.x = (off_a*nb.y - na.y*off_b) / denom;
  pt.y = (off_b*na.x - nb.x*off_a) / denom;

  return true;
}

//==== Generic functions =============================================//
// (work on 2d or 3d vectors)

template <class vector>
vector point_on_line(const vector x0,const vector x1,const vector p)
{
  vector q,dx,dp;
  double t,sl;

  dx = x1 - x0;
  dp =  p - x0;
  sl = dx.sqlength();
  t = dot(dx,dp) / sl;

  q = x0 + dx * t;

  return(q);
}

// returns nearest point on line segment x0-x1 to point p
template <class vector>
vector point_on_segment(const vector x0,const vector x1,const vector p)
{
  vector sx,sp,r;
  double f,l;

  sx = x1 - x0;
  sp = p  - x0;

  f = dot(sx,sp);
  if(f <= 0.0) return(x0); // also handles x0=x1 case

  l = sx.sqlength();
  if(f >= l) return(x1);

  r = x0 + sx * (f / l);

  return(r);
}

// ray: r0=origin of ray, rd=direction vector of ray
// plane: p0=point on plane, pn=plane normal
// result = point on plane where ray intersects
//   if ray intersects plane (going forward or backward) set to point of intersection otherwise not touched
// returns true if ray intersects plane (going forward along direction)
template <class vector>
bool intersect_ray_plane(const vector r0, const vector rd, const vector p0, const vector pn, vector &result) {
  double t;
  double d;

  if(rd.dot(pn)==0.0)
    return false;

  d = -p0.dot(pn);

  t = -(r0.dot(pn) + d) / (rd.dot(pn));

  result = r0 + rd * t;

  return (t>=0.0);
}

// same as intersect_ray_plane except also return t value
// ray: r0=origin of ray, rd=direction vector of ray
// plane: p0=point on plane, pn=plane normal
// result = point on plane where ray intersects
//   if ray intersects plane (going forward or backward) set to point of intersection otherwise not touched
// returns true if ray intersects plane (going forward along direction)
template <class vector>
bool intersect_ray_plane_t(const vector r0, const vector rd, const vector p0, const vector pn,
                           vector &result, double &t) {
  double d;

  t = 0.0;

  if(rd.dot(pn)==0.0)
    return false;

  d = -p0.dot(pn);

  t = -(r0.dot(pn) + d) / (rd.dot(pn));

  result = r0 + rd * t;

  return (t >= 0.0);
}

// pt: q=query point
// plane: p0=point on plane, pn=plane normal
template <class vector>
bool above_pt_plane(const vector q, const vector p0, const vector pn) {
  double d,dp;

  d  = p0.dot(pn);
  dp =  q.dot(pn);

  return (dp >= d);
}

template <class vec2d, class coord>
unsigned char compute_out_code(const vec2d &vec, coord xl, coord xh, coord yl, coord yh) {
  unsigned char out_code=0;

  out_code |= (vec.y < yl) << 0;
  out_code |= (vec.y > yh) << 1;
  out_code |= (vec.x < xl) << 2;
  out_code |= (vec.x > xh) << 3;

  return out_code;
}

template <class integer>
integer bot_mask(integer val) {
  return val & -val;
}

// 2D clipping routine
// uses Cohen-Sutherland clipping algorithm
template <class vec2d, class coord>
bool clip_ray(vec2d &p0, vec2d &pn, coord xl, coord xh, coord yl, coord yh) {
  unsigned char out_code0, out_coden;

  out_code0 = compute_out_code(p0,xl,xh,yl,yh);
  out_coden = compute_out_code(pn,xl,xh,yl,yh);
    
  //printf("out_code0 %x out_coden %x p0 (%g,%g) pn (%g,%g)\n",
  //       out_code0,out_coden,p0.x,p0.y,pn.x,pn.y);

  if(out_code0==0 && out_coden==0)
    return true;
    
  if((out_code0 & out_coden)!=0)
    return false;
    
  vec2d pdir;
  pdir = pn - p0;
    
  while(out_code0!=0) {
    int edge=bot_mask(out_code0);
    switch(edge) {
    case 1: // out yl
      // pdir.y != 0 since otherwise wouldn't hit this case
      p0.x = p0.x + (yl - p0.y) * pdir.x / pdir.y;
      p0.y = yl;
      break;
    case 2: // out yh
      // pdir.y != 0 since otherwise wouldn't hit this case
      p0.x = p0.x + (yh - p0.y) * pdir.x / pdir.y;
      p0.y = yh;
      break;
    case 4: // out xl
      p0.y = p0.y + (xl - p0.x) * pdir.y / pdir.x;
      p0.x = xl;
      break;
    case 8: // out xh
      p0.y = p0.y + (xh - p0.x) * pdir.y / pdir.x;
      p0.x = xh;
      break;
    }
    out_code0 = compute_out_code(p0,xl,xh,yl,yh);

    //printf("0 out_code0 %x out_coden %x edge %d p0 (%g,%g) pn (%g,%g)\n",
    //       out_code0,out_coden,edge,p0.x,p0.y,pn.x,pn.y);

    if(out_code0==0 && out_coden==0)
      return true;
    
    if((out_code0 & out_coden)!=0)
      return false;
  }

  while(out_coden!=0) {
    int edge=bot_mask(out_coden);
    switch(edge) {
    case 1: // out yl
      // pdir.y != 0 since otherwise wouldn't hit this case
      pn.x = pn.x + (yl - pn.y) * pdir.x / pdir.y;
      pn.y = yl;
      break;
    case 2: // out yh
      // pdir.y != 0 since otherwise wouldn't hit this case
      pn.x = pn.x + (yh - pn.y) * pdir.x / pdir.y;
      pn.y = yh;
      break;
    case 4: // out xl
      pn.y = pn.y + (xl - pn.x) * pdir.y / pdir.x;
      pn.x = xl;
      break;
    case 8: // out xh
      pn.y = pn.y + (xh - pn.x) * pdir.y / pdir.x;
      pn.x = xh;
      break;
    }
    out_coden = compute_out_code(pn,xl,xh,yl,yh);

    //printf("n out_code0 %x out_coden %x edge %d p0 (%g,%g) pn (%g,%g)\n",
    //       out_code0,out_coden,edge,p0.x,p0.y,pn.x,pn.y);

    if(out_code0==0 && out_coden==0)
      return true;
    
    if((out_code0 & out_coden)!=0)
      return false;
  }

  return true; // shouldn't reach here
}

} // namespace GVector

#endif
// __GVECTOR_H__