bool SubSurface::Subtag( const vec3d & center )
{
    UpdatePolygonPnts(); // Update polygon vector

    for ( int p = 0; p < ( int )m_PolyPntsVec.size(); p++ )
    {
        bool inPoly = PointInPolygon( vec2d( center.x(), center.y() ), m_PolyPntsVec[p] );

        if ( inPoly && m_TestType() == vsp::INSIDE )
        {
            return true;
        }
        else if ( inPoly && m_TestType() == vsp::OUTSIDE )
        {
            return false;
        }
    }

    if ( m_TestType() == vsp::OUTSIDE )
    {
        return true;
    }

    return false;
}

//to return projection vector
vec3d vec3d :: proj(const vec3d& vec) const
{
	float m = vec.mag();
	if(m == 0)
		throw "invalid vector";
	return vec.unit()*(operator*(vec)/m);
}

void Surf::ApplyES( vec3d uw, double t )
{
    double grm1 = m_GridDensityPtr->m_GrowRatio() - 1.0;
    int nmapu = m_SrcMap.size();
    int nmapw = m_SrcMap[0].size();

    int ibase, jbase;
    double u = uw.x();
    double w = uw.y();
    UWtoTargetMapij( u, w, ibase, jbase );

    vec3d p = m_SurfCore.CompPnt( u, w );

    int iadd[] = { 0, 1, 0, 1 };
    int jadd[] = { 0, 0, 1, 1 };

    for( int i = 0; i < 4; i++ )
    {
        int itarget = ibase + iadd[i];
        int jtarget = jbase + jadd[i];

        if( itarget < nmapu && itarget >= 0 && jtarget < nmapw && jtarget >= 0 )
        {
            vec3d p2 = m_SrcMap[ itarget ][ jtarget ].m_pt;
            double r = ( p2 - p ).mag();
            double targetstr = t + r * grm1;
            if( m_SrcMap[ itarget ][ jtarget ].m_str > targetstr )
            {
                m_SrcMap[ itarget ][ jtarget ].m_str = targetstr;
                WalkMap( itarget, jtarget, itarget, jtarget );
            }
        }
    }
}

vec3d SSLineSeg::CompPnt( VspSurf* surf, vec3d uw_pnt ) const
{
    if ( !surf )
    {
        return vec3d();
    }

    double maxu = surf->GetUMax();
    double maxw = surf->GetWMax();

    if ( uw_pnt.x() < 0.0 )
    {
        uw_pnt.set_x( 0.0 );
    }
    else if ( uw_pnt.x() > maxu )
    {
        uw_pnt.set_x( maxu );
    }

    if ( uw_pnt.y() < 0.0 )
    {
        uw_pnt.set_y( 0.0 );
    }
    else if ( uw_pnt.y() > maxw )
    {
        uw_pnt.set_y( maxw );
    }

    return surf->CompPnt( uw_pnt.x(), uw_pnt.y() );
}

void VspCurve::Reflect( vec3d axis, double d )
{
    curve_point_type a;

    a << axis.x(), axis.y(), axis.z();
    m_Curve.reflect( a, d );
}

//===== Offset =====//
void VspCurve::Offset( vec3d offvec )
{
    curve_point_type tr;
    tr << offvec.x(), offvec.y(), offvec.z();

    m_Curve.translate( tr );
}

// Spherical linear interpolation between direction vectors.
// Intermediate vectors follow great circle path with constant velocity.
vec3d slerp( const vec3d& a, const vec3d& b, const double &t )
{
    vec3d an = a / a.mag();
    vec3d bn = b / b.mag();

    double dp = dot( an, bn );

    double theta = 0.0;
    if ( dp >= -1.0 && dp <= 1.0 )
    {
        theta = acos( dp );
    }

    // Initialize retvec as a-direction.
    vec3d retvec = an;

    // If vectors are not parallel, interpolate between them.
    if ( std::abs( theta ) > 1.0e-6 )
    {
        // Drop division by sin(theta) because .normalize() will scale
        double coeff1 = sin( ( 1.0 - t ) * theta );  // implied  / sin(theta)
        double coeff2 = sin( t * theta );            // implied  / sin(theta)

        retvec = coeff1 * an + coeff2 * bn;
        retvec.normalize();
    }

    return retvec;
}

//////////////////////////////////////////////////////////////////////
//==== IPnt Bin ====//
//////////////////////////////////////////////////////////////////////
int IPntBin::ComputeID( vec3d & pos )
{
	int id =	(int)(pos.x()*10000.0) + 
				(int)(pos.y()*10000.0) +
				(int)(pos.z()*10000.0);

	return id;
}

//******* Angle Between Vectors ******//
double angle(const vec3d& a, const vec3d& b)
{
   double angle = dot(a, b)/(a.mag()*b.mag());

   if ( angle < -1.0 )			angle = -1.0;
   else if ( angle > 1.0 )		angle = 1.0;

   return(acos(angle));

 }

static FLOAT64 vec_angle( vec3d v1, vec3d v2 ) {
	FLOAT64 len1 = v1.len();
	FLOAT64 len2 = v2.len();
	FLOAT64 dot = len1 * len2;
	if( dot ) {
		FLOAT64 res = acos(fmin(1.0-1e-8,fmax(-1.0+1e-8,v1*v2/dot)));
		return res;
	}
	return 0.0;
}

double VspCurve::FindNearest( double &u, const vec3d &pt, const double &u0 ) const
{
    double dist;
    curve_point_type p;
    p << pt.x(), pt.y(), pt.z();

    dist = eli::geom::intersect::minimum_distance( u, m_Curve, p, u0 );

    return dist;
}

//******* Cosine of Angle Between Vectors ******//
double cos_angle(const vec3d& a, const vec3d& b)
{
   double angle = dot(a, b)/(a.mag()*b.mag());

   if (angle < -1.0 )
	   return -1.0;
   else if ( angle > 1.0)
	   return 1.0;

   return angle;
}

void PlanetViewController::move(vec3d &oldp, vec3d &p)
{
    oldp = oldp.normalize();
    p = p.normalize();
    double oldlat = safe_asin(oldp.z);
    double oldlon = atan2(oldp.y, oldp.x);
    double lat = safe_asin(p.z);
    double lon = atan2(p.y, p.x);
    x0 -= lon - oldlon;
    y0 -= lat - oldlat;
    y0 = max(-M_PI / 2.0, min(M_PI / 2.0, y0));
}

//******* Angle Between Vectors ******//
double angle( const vec3d& a, const vec3d& b )
{
    double angle = dot( a, b ) / ( a.mag() * b.mag() );

    if ( angle >= -1.0 && angle <= 1.0 )
    {
        return( acos( angle ) );
    }
    else
    {
        return( 0.0 );
    }
}

vec4d planeByPointNormal(vec3d point, vec3d normal) {
  vec4d ret;
  for (int i = 0; i < 3; i++)
    ret.v[i] = normal.v[i];
  ret.v[3] = -point.dot(normal);
  return ret;
}

double SurfCore::FindNearest( double &u, double &w, const vec3d &pt, double u0, double w0 ) const
{
    double dist;
    surface_point_type p;
    pt.get_pnt( p );

    double umn = m_Surface.get_u0();
    double wmn = m_Surface.get_v0();

    double umx = m_Surface.get_umax();
    double wmx = m_Surface.get_vmax();

    double slop = 1e-3;
    if( u0 < (umn - slop) || w0 < (wmn - slop) || u0 > (umx + slop) || w0 > (wmx + slop) )
    {
        printf("BAD parameter in SurfCore::FindNearest! %f %f\n", u0, w0 );
        assert(false);
    }

    if ( u0 < umn )
        u0 = umn;

    if ( w0 < wmn )
        w0 = wmn;

    if ( u0 > umx )
        u0 = umx;

    if ( w0 > wmx )
        w0 = wmx;

    dist = eli::geom::intersect::minimum_distance( u, w, m_Surface, p, u0, w0 );

    return dist;
}

mat44 translationMatrix(vec3d x) {
  mat44 ret;
  for (int i = 0; i < 4; i++)
    for (int j = 0; j < 3; j++)
      ret.m[i][j] = (i == j) ? 1 : 0;
  ret.setcolumn(3, x.tohc());
  return ret;
}

//to return the angle between vectors in radians
double vec3d :: angle(const vec3d& vec) const
{
	float this_mag = mag();
	float vec_mag = vec.mag();
	if(this_mag == 0 || vec_mag == 0)
		throw "invalid vector";
	return acos(operator*(vec)/(this_mag*vec_mag));
}

vec4d quatFromAA(vec3d axis, double angle) {
  vec4d ret;
  vec3d a = axis.normalize();
  for (int i = 0; i < 3; i++)
    ret.v[i] = a.v[i] * sin(angle / 2);
  ret.v[3] = cos(angle / 2);
  return ret;
}

void Mesh::drawSurface(GLenum primitive, vec3d axis, float limit, int numSteps){
    if (!isValid) return;
    
    std::vector<Face> surface;
    double inc = limit/(numSteps-1);
    axis.normalize();
    
    for (unsigned u=0; u < mesh.size(); ++u){
        surface.clear();
        surface.push_back(mesh[u]);
        for (unsigned i = 1; i < numSteps; ++i){
            Face f;
            for (unsigned j=0; j< mesh[u].v.size(); ++j ){
                // translate all vertices
                f.v.push_back(surface[i-1].v[j] + axis*inc);
            }
            surface.push_back(f);        
        }

        // draw surface
        glColor3f(1,1,1);

        GLfloat gray[] = {1.0f, 0.5f, 0.5f, 1.0f};
        glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, gray);
        glColorMaterial(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE);

        for (unsigned i=0; i < surface.size()-1; ++i){
            Face *f0 = &surface[i];
            Face *f1 = &surface[i+1];

            unsigned size = f0->v.size();

            glBegin(primitive);        
            for (unsigned j=0; j < size; ++j){
                unsigned c1 = j;
                unsigned c2 = (j+1)%size;
                vec3d n1 = cross(f0->v[c2] - f0->v[c1], f1->v[c1] - f0->v[c1]);
                vec3d n2 = cross(f1->v[c2] - f0->v[c2], f1->v[c1] - f0->v[c2]);
                n1.normalize();
                n2.normalize();

                // Tri 1
                glNormal3f(n1.x, n1.y, n1.z);
                glVertex3f(f0->v[c1].x, f0->v[c1].y, f0->v[c1].z); 
                glVertex3f(f0->v[c2].x, f0->v[c2].y, f0->v[c2].z);
                glVertex3f(f1->v[c1].x, f1->v[c1].y, f1->v[c1].z);

                // Tri 2
                glNormal3f(n2.x, n2.y, n2.z);
                glVertex3f(f0->v[c2].x, f0->v[c2].y, f0->v[c2].z); 
                glVertex3f(f1->v[c2].x, f1->v[c2].y, f1->v[c2].z);
                glVertex3f(f1->v[c1].x, f1->v[c1].y, f1->v[c1].z);             
            }
            glEnd();
        }
    }
}