SmoothFace.cpp

Go to the documentation of this file.

#include <iostream>
#include <fstream>
#include <algorithm>
#include <iomanip>
#include <cassert>
#include <limits>
#include "moab/OrientedBoxTreeTool.hpp"
#include "SmoothFace.hpp"

#define GEOMETRY_RESABS 1.e-6
#define mbsqr( a )      ( ( a ) * ( a ) )
#define mbcube( a )     ( mbsqr( a ) * ( a ) )
#define mbquart( a )    ( mbsqr( a ) * mbsqr( a ) )

namespace moab
{

bool within_tolerance( CartVect& p1, CartVect& p2, const double& tolerance )
{
    if( ( fabs( p1[0] - p2[0] ) < tolerance ) && ( fabs( p1[1] - p2[1] ) < tolerance ) &&
        ( fabs( p1[2] - p2[2] ) < tolerance ) )
        return true;
    return false;
}
int numAdjTriInSet( Interface* mb, EntityHandle startEdge, EntityHandle set )
{
    std::vector< EntityHandle > adjTri;
    mb->get_adjacencies( &startEdge, 1, 2, false, adjTri, Interface::UNION );
    // count how many are in the set
    int nbInSet = 0;
    for( size_t i = 0; i < adjTri.size(); i++ )
    {
        EntityHandle tri = adjTri[i];
        if( mb->contains_entities( set, &tri, 1 ) ) nbInSet++;
    }
    return nbInSet;
}

bool debug_surf_eval1 = false;

SmoothFace::SmoothFace( Interface* mb, EntityHandle surface_set, GeomTopoTool* gTool )
    : _markTag( 0 ), _gradientTag( 0 ), _tangentsTag( 0 ), _edgeCtrlTag( 0 ), _facetCtrlTag( 0 ),
      _facetEdgeCtrlTag( 0 ), _planeTag( 0 ), _mb( mb ), _set( surface_set ), _my_geomTopoTool( gTool ), _obb_root( 0 ),
      _evaluationsCounter( 0 )
{
    //_smooth_face = NULL;
    //_mbOut->create_meshset(MESHSET_SET, _oSet); //will contain the
    // get also the obb_root
    if( _my_geomTopoTool )
    {
        _my_geomTopoTool->get_root( this->_set, _obb_root );
        if( debug_surf_eval1 ) _my_geomTopoTool->obb_tree()->stats( _obb_root, std::cout );
    }
}

SmoothFace::~SmoothFace() {}

double SmoothFace::area()
{
    // find the area of this entity
    // assert(_smooth_face);
    // double area1 = _smooth_face->area();
    double totArea = 0.;
    for( Range::iterator it = _triangles.begin(); it != _triangles.end(); ++it )
    {
        EntityHandle tria = *it;
        const EntityHandle* conn3;
        int nnodes;
        _mb->get_connectivity( tria, conn3, nnodes );
        //
        // double coords[9]; // store the coordinates for the nodes
        //_mb->get_coords(conn3, 3, coords);
        CartVect p[3];
        _mb->get_coords( conn3, 3, (double*)&p[0] );
        // need to compute the angles
        // compute angles and the normal
        // CartVect p1(&coords[0]), p2(&coords[3]), p3(&coords[6]);

        CartVect AB( p[1] - p[0] );  //(p2 - p1);
        CartVect BC( p[2] - p[1] );  //(p3 - p2);
        CartVect normal = AB * BC;
        totArea += normal.length() / 2;
    }
    return totArea;
}
// these tags will be collected for deletion
void SmoothFace::append_smooth_tags( std::vector< Tag >& smoothTags )
{
    // these are created locally, for each smooth face
    smoothTags.push_back( _gradientTag );
    smoothTags.push_back( _planeTag );
}
void SmoothFace::bounding_box( double box_min[3], double box_max[3] )
{

    for( int i = 0; i < 3; i++ )
    {
        box_min[i] = _minim[i];
        box_max[i] = _maxim[i];
    }
    // _minim, _maxim
}

void SmoothFace::move_to_surface( double& x, double& y, double& z )
{

    CartVect loc2( x, y, z );
    bool trim    = false;  // is it needed?
    bool outside = true;
    CartVect closestPoint;

    ErrorCode rval = project_to_facets_main( loc2, trim, outside, &closestPoint, NULL );
    if( MB_SUCCESS != rval ) return;
    assert( rval == MB_SUCCESS );
    x = closestPoint[0];
    y = closestPoint[1];
    z = closestPoint[2];
}
/*
 void SmoothFace::move_to_surface(double& x, double& y, double& z,
 double& u_guess, double& v_guess) {
 if (debug_surf_eval1) {
 std::cout << "move_to_surface called." << std::endl;
 }
 }*/

bool SmoothFace::normal_at( double x, double y, double z, double& nx, double& ny, double& nz )
{

    CartVect loc2( x, y, z );

    bool trim    = false;  // is it needed?
    bool outside = true;
    // CartVect closestPoint;// not needed
    // not interested in normal
    CartVect normal;
    ErrorCode rval = project_to_facets_main( loc2, trim, outside, NULL, &normal );
    if( MB_SUCCESS != rval ) return false;
    assert( rval == MB_SUCCESS );
    nx = normal[0];
    ny = normal[1];
    nz = normal[2];

    return true;
}

ErrorCode SmoothFace::compute_control_points_on_edges( double min_dot, Tag edgeCtrlTag, Tag markTag )
{

    _edgeCtrlTag = edgeCtrlTag;
    _markTag     = markTag;

    // now, compute control points for all edges that are not marked already (they are no on the
    // boundary!)
    for( Range::iterator it = _edges.begin(); it != _edges.end(); ++it )
    {
        EntityHandle edg = *it;
        // is the edge marked? already computed
        unsigned char tagVal = 0;
        _mb->tag_get_data( _markTag, &edg, 1, &tagVal );
        if( tagVal ) continue;
        // double min_dot;
        init_bezier_edge( edg, min_dot );
        tagVal = 1;
        _mb->tag_set_data( _markTag, &edg, 1, &tagVal );
    }
    return MB_SUCCESS;
}

int SmoothFace::init_gradient()
{
    // first, create a Tag for gradient (or normal)
    // loop over all triangles in set, and modify the normals according to the angle as weight
    // get all the edges from this subset
    if( NULL == _mb ) return 1;  // fail
    _triangles.clear();
    ErrorCode rval = _mb->get_entities_by_type( _set, MBTRI, _triangles );
    if( MB_SUCCESS != rval ) return 1;
    // get a new range of edges, and decide the loops from here
    _edges.clear();
    rval = _mb->get_adjacencies( _triangles, 1, true, _edges, Interface::UNION );
    assert( rval == MB_SUCCESS );

    rval = _mb->get_adjacencies( _triangles, 0, false, _nodes, Interface::UNION );
    assert( rval == MB_SUCCESS );

    // initialize bounding box limits
    CartVect vert1;
    EntityHandle v1 = _nodes[0];  // first vertex
    _mb->get_coords( &v1, 1, (double*)&vert1 );
    _minim = vert1;
    _maxim = vert1;

    double defNormal[] = { 0., 0., 0. };
    // look for a tag name here that is definitely unique. We do not want the tags to interfere with
    // each other this normal will be for each node in a face some nodes have multiple normals, if
    // they are at the feature edges
    unsigned long setId = _mb->id_from_handle( _set );
    char name[50]       = { 0 };
    sprintf( name, "GRADIENT%lu",
             setId );  // name should be something like GRADIENT29, where 29 is the set ID of the face
    rval = _mb->tag_get_handle( name, 3, MB_TYPE_DOUBLE, _gradientTag, MB_TAG_DENSE | MB_TAG_CREAT, &defNormal );
    assert( rval == MB_SUCCESS );

    double defPlane[4] = { 0., 0., 1., 0. };
    // also define a plane tag ; this will be for each triangle
    char namePlaneTag[50] = { 0 };
    sprintf( namePlaneTag, "PLANE%lu", setId );
    rval = _mb->tag_get_handle( "PLANE", 4, MB_TYPE_DOUBLE, _planeTag, MB_TAG_DENSE | MB_TAG_CREAT, &defPlane );
    assert( rval == MB_SUCCESS );
    // the fourth double is for weight, accumulated at each vertex so far
    // maybe not needed in the end
    for( Range::iterator it = _triangles.begin(); it != _triangles.end(); ++it )
    {
        EntityHandle tria = *it;
        const EntityHandle* conn3;
        int nnodes;
        _mb->get_connectivity( tria, conn3, nnodes );
        if( nnodes != 3 ) return 1;  // error
        // double coords[9]; // store the coordinates for the nodes
        //_mb->get_coords(conn3, 3, coords);
        CartVect p[3];
        _mb->get_coords( conn3, 3, (double*)&p[0] );
        // need to compute the angles
        // compute angles and the normal
        // CartVect p1(&coords[0]), p2(&coords[3]), p3(&coords[6]);

        CartVect AB( p[1] - p[0] );  //(p2 - p1);
        CartVect BC( p[2] - p[1] );  //(p3 - p2);
        CartVect CA( p[0] - p[2] );  //(p1 - p3);
        double a[3];
        a[1]            = angle( AB, -BC );  // angle at B (p2), etc.
        a[2]            = angle( BC, -CA );
        a[0]            = angle( CA, -AB );
        CartVect normal = -AB * CA;
        normal.normalize();
        double plane[4];

        const double* coordNormal = normal.array();

        plane[0] = coordNormal[0];
        plane[1] = coordNormal[1];
        plane[2] = coordNormal[2];
        plane[3] = -normal % p[0];  // dot product
        // set the plane
        rval = _mb->tag_set_data( _planeTag, &tria, 1, plane );
        assert( rval == MB_SUCCESS );

        // add to each vertex the tag value the normal multiplied by the angle
        double values[9];

        _mb->tag_get_data( _gradientTag, conn3, 3, values );
        for( int i = 0; i < 3; i++ )
        {
            // values[4*i]+=a[i]; // first is the weight, which we do not really need
            values[3 * i + 0] += a[i] * coordNormal[0];
            values[3 * i + 1] += a[i] * coordNormal[1];
            values[3 * i + 2] += a[i] * coordNormal[2];
        }

        // reset those values
        _mb->tag_set_data( _gradientTag, conn3, 3, values );
    }
    // normalize the gradients at each node; maybe not needed here?
    // no, we will do it, it is important
    int numNodes      = _nodes.size();
    double* normalVal = new double[3 * numNodes];
    _mb->tag_get_data( _gradientTag, _nodes,
                       normalVal );  // get all the normal values at the _nodes
    for( int i = 0; i < numNodes; i++ )
    {
        CartVect p1( &normalVal[3 * i] );
        p1.normalize();
        p1.get( &normalVal[3 * i] );
    }

    // reset the normal values after normalization
    _mb->tag_set_data( _gradientTag, _nodes, normalVal );
    // print the loops size and some other stuff
    if( debug_surf_eval1 )
    {
        std::cout << " normals at  " << numNodes << " nodes" << std::endl;
        int i = 0;
        for( Range::iterator it = _nodes.begin(); it != _nodes.end(); ++it, i++ )
        {
            EntityHandle node = *it;
            std::cout << " Node id " << _mb->id_from_handle( node ) << "  " << normalVal[3 * i] << " "
                      << normalVal[3 * i + 1] << " " << normalVal[3 * i + 2] << std::endl;
        }
    }

    delete[] normalVal;

    return 0;
}

// init bezier edges
// start copy
//===========================================================================
// Function Name: init_bezier_edge
//
// Member Type:  PRIVATE
// Description:  compute the control points for an edge
//===========================================================================
ErrorCode SmoothFace::init_bezier_edge( EntityHandle edge, double )
{
    // min dot was used for angle here
    // int stat = 0; // CUBIT_SUCCESS;
    // all boundaries will be simple, initially
    // we may complicate them afterwards

    CartVect ctrl_pts[3];
    int nnodes                = 0;
    const EntityHandle* conn2 = NULL;
    ErrorCode rval            = _mb->get_connectivity( edge, conn2, nnodes );
    assert( rval == MB_SUCCESS );
    if( MB_SUCCESS != rval ) return rval;

    assert( 2 == nnodes );
    // double coords[6]; // store the coordinates for the nodes
    CartVect P[2];
    // ErrorCode rval = _mb->get_coords(conn2, 2, coords);
    rval = _mb->get_coords( conn2, 2, (double*)&P[0] );
    assert( rval == MB_SUCCESS );
    if( MB_SUCCESS != rval ) return rval;

    // CartVect P0(&coords[0]);
    // CartVect P3(&coords[3]);

    // double normalVec[6];
    CartVect N[2];
    //_mb->tag_get_data(_gradientTag, conn2, 2, normalVec);
    rval = _mb->tag_get_data( _gradientTag, conn2, 2, (double*)&N[0] );
    assert( rval == MB_SUCCESS );
    if( MB_SUCCESS != rval ) return rval;

    CartVect T[2];  // T0, T3

    rval = _mb->tag_get_data( _tangentsTag, &edge, 1, &T[0] );
    assert( rval == MB_SUCCESS );
    if( MB_SUCCESS != rval ) return rval;

    rval = init_edge_control_points( P[0], P[1], N[0], N[1], T[0], T[1], ctrl_pts );
    assert( rval == MB_SUCCESS );
    if( MB_SUCCESS != rval ) return rval;

    rval = _mb->tag_set_data( _edgeCtrlTag, &edge, 1, &ctrl_pts[0] );
    assert( rval == MB_SUCCESS );
    if( MB_SUCCESS != rval ) return rval;

    if( debug_surf_eval1 )
    {
        std::cout << "edge: " << _mb->id_from_handle( edge ) << " tangents: " << T[0] << T[1] << std::endl;
        std::cout << "  points: " << P[0] << " " << P[1] << std::endl;
        std::cout << "  normals: " << N[0] << " " << N[1] << std::endl;
        std::cout << "  Control points  " << ctrl_pts[0] << " " << ctrl_pts[1] << " " << ctrl_pts[2] << std::endl;
    }

    return MB_SUCCESS;
}

ErrorCode SmoothFace::compute_tangents_for_each_edge()
// they will be used for control points
{
    double defTangents[6] = { 0., 0., 0., 0., 0., 0. };
    ErrorCode rval =
        _mb->tag_get_handle( "TANGENTS", 6, MB_TYPE_DOUBLE, _tangentsTag, MB_TAG_DENSE | MB_TAG_CREAT, &defTangents );
    if( MB_SUCCESS != rval ) return MB_FAILURE;

    // now, compute Tangents for all edges that are not on boundary, so they are not marked
    for( Range::iterator it = _edges.begin(); it != _edges.end(); ++it )
    {
        EntityHandle edg = *it;

        int nnodes;
        const EntityHandle* conn2;  //
        _mb->get_connectivity( edg, conn2, nnodes );
        assert( nnodes == 2 );
        CartVect P[2];  // store the coordinates for the nodes
        rval = _mb->get_coords( conn2, 2, (double*)&P[0] );
        if( MB_SUCCESS != rval ) return rval;
        assert( rval == MB_SUCCESS );
        CartVect T[2];
        T[0] = P[1] - P[0];
        T[0].normalize();
        T[1] = T[0];  //
        _mb->tag_set_data( _tangentsTag, &edg, 1,
                           (double*)&T[0] );  // set the tangents computed at every edge
    }
    return MB_SUCCESS;
}
// start copy
//===========================================================================
// Function Name: init_edge_control_points
//
// Member Type:  PRIVATE
// Description:  compute the control points for an edge
//===========================================================================
ErrorCode SmoothFace::init_edge_control_points( CartVect& P0, CartVect& P3, CartVect& N0, CartVect& N3, CartVect& T0,
                                                CartVect& T3, CartVect* Pi )
{
    CartVect Vi[4];
    Vi[0] = P0;
    Vi[3] = P3;
    CartVect P03( P3 - P0 );
    double di    = P03.length();
    double ai    = N0 % N3;  // this is the dot operator, the same as in cgm for CubitVector
    double ai0   = N0 % T0;
    double ai3   = N3 % T3;
    double denom = 4 - ai * ai;
    if( fabs( denom ) < 1e-20 )
    {
        return MB_FAILURE;  // CUBIT_FAILURE;
    }
    double row   = 6.0e0 * ( 2.0e0 * ai0 + ai * ai3 ) / denom;
    double omega = 6.0e0 * ( 2.0e0 * ai3 + ai * ai0 ) / denom;
    Vi[1]        = Vi[0] + ( di * ( ( ( 6.0e0 * T0 ) - ( ( 2.0e0 * row ) * N0 ) + ( omega * N3 ) ) / 18.0e0 ) );
    Vi[2]        = Vi[3] - ( di * ( ( ( 6.0e0 * T3 ) + ( row * N0 ) - ( ( 2.0e0 * omega ) * N3 ) ) / 18.0e0 ) );
    // CartVect Wi[3];
    // Wi[0] = Vi[1] - Vi[0];
    // Wi[1] = Vi[2] - Vi[1];
    // Wi[2] = Vi[3] - Vi[2];

    Pi[0] = 0.25 * Vi[0] + 0.75 * Vi[1];
    Pi[1] = 0.50 * Vi[1] + 0.50 * Vi[2];
    Pi[2] = 0.75 * Vi[2] + 0.25 * Vi[3];

    return MB_SUCCESS;
}

ErrorCode SmoothFace::find_edges_orientations( EntityHandle edges[3], const EntityHandle* conn3,
                                               int orient[3] )  // maybe we will set it?
{
    // find the edge that is adjacent to 2 vertices at a time
    for( int i = 0; i < 3; i++ )
    {
        // edge 0 is 1-2, 1 is 3-1, 2 is 0-1
        EntityHandle v[2];
        v[0] = conn3[( i + 1 ) % 3];
        v[1] = conn3[( i + 2 ) % 3];
        std::vector< EntityHandle > adjacencies;
        // generate all edges for these two hexes
        ErrorCode rval = _mb->get_adjacencies( v, 2, 1, false, adjacencies, Interface::INTERSECT );
        if( MB_SUCCESS != rval ) return rval;

        // find the edge connected to both vertices, and then see its orientation
        assert( adjacencies.size() == 1 );
        const EntityHandle* conn2 = NULL;
        int nnodes                = 0;
        rval                      = _mb->get_connectivity( adjacencies[0], conn2, nnodes );
        assert( rval == MB_SUCCESS );
        assert( 2 == nnodes );
        edges[i] = adjacencies[0];
        // what is the story morning glory?
        if( conn2[0] == v[0] && conn2[1] == v[1] )
            orient[i] = 1;
        else if( conn2[0] == v[1] && conn2[1] == v[0] )
            orient[i] = -1;
        else
            return MB_FAILURE;
    }
    return MB_SUCCESS;
}
ErrorCode SmoothFace::compute_internal_control_points_on_facets( double, Tag facetCtrlTag, Tag facetEdgeCtrlTag )
{
    // collect from each triangle the control points in order
    //

    _facetCtrlTag     = facetCtrlTag;
    _facetEdgeCtrlTag = facetEdgeCtrlTag;

    for( Range::iterator it = _triangles.begin(); it != _triangles.end(); ++it )
    {
        EntityHandle tri = *it;
        // first get connectivity, and the edges
        // we need a fast method to retrieve the adjacent edges to each triangle
        const EntityHandle* conn3;
        int nnodes;
        ErrorCode rval = _mb->get_connectivity( tri, conn3, nnodes );
        assert( MB_SUCCESS == rval );
        if( MB_SUCCESS != rval ) return rval;
        assert( 3 == nnodes );

        // would it be easier to do
        CartVect vNode[3];  // position at nodes
        rval = _mb->get_coords( conn3, 3, (double*)&vNode[0] );
        assert( MB_SUCCESS == rval );
        if( MB_SUCCESS != rval ) return rval;

        // get gradients (normal) at each node of triangle
        CartVect NN[3];
        rval = _mb->tag_get_data( _gradientTag, conn3, 3, &NN[0] );
        assert( MB_SUCCESS == rval );
        if( MB_SUCCESS != rval ) return rval;

        EntityHandle edges[3];
        int orient[3];  // + 1 or -1, if the edge is positive or negative within the face
        rval = find_edges_orientations( edges, conn3, orient );  // maybe we will set it?
        assert( MB_SUCCESS == rval );
        if( MB_SUCCESS != rval ) return rval;
        // maybe we will store some tags with edges and their orientation with respect to
        // a triangle;
        CartVect P[3][5];
        CartVect N[6], G[6];
        // create the linear array for control points on edges, for storage (expensive !!!)
        CartVect CP[9];
        int index = 0;
        // maybe store a tag / entity handle for edges?
        for( int i = 0; i < 3; i++ )
        {
            // populate P and N with the right vectors
            int i1       = ( i + 1 ) % 3;  // the first node of the edge
            int i2       = ( i + 2 ) % 3;  // the second node of the edge
            N[2 * i]     = NN[i1];
            N[2 * i + 1] = NN[i2];
            P[i][0]      = vNode[i1];
            rval         = _mb->tag_get_data( _edgeCtrlTag, &edges[i], 1, &( P[i][1] ) );
            // if sense is -1, swap 1 and 3 control points
            if( orient[i] == -1 )
            {
                CartVect tmp;
                tmp     = P[i][1];
                P[i][1] = P[i][3];
                P[i][3] = tmp;
            }
            P[i][4] = vNode[i2];
            for( int j = 1; j < 4; j++ )
                CP[index++] = P[i][j];

            // the first edge control points
        }

        //  stat = facet->get_edge_control_points( P );
        init_facet_control_points( N, P, G );
        // what do we need to store in the tag control points?
        rval = _mb->tag_set_data( _facetCtrlTag, &tri, 1, &G[0] );
        assert( MB_SUCCESS == rval );
        if( MB_SUCCESS != rval ) return rval;

        // store here again the 9 control points on the edges
        rval = _mb->tag_set_data( _facetEdgeCtrlTag, &tri, 1, &CP[0] );
        assert( MB_SUCCESS == rval );
        if( MB_SUCCESS != rval ) return rval;
        // look at what we retrieve later

        // adjust the bounding box
        int j = 0;
        for( j = 0; j < 3; j++ )
            adjust_bounding_box( vNode[j] );
        // edge control points
        for( j = 0; j < 9; j++ )
            adjust_bounding_box( CP[j] );
        // internal facet control points
        for( j = 0; j < 6; j++ )
            adjust_bounding_box( G[j] );
    }
    return MB_SUCCESS;
}
void SmoothFace::adjust_bounding_box( CartVect& vect )
{
    // _minim, _maxim
    for( int j = 0; j < 3; j++ )
    {
        if( _minim[j] > vect[j] ) _minim[j] = vect[j];
        if( _maxim[j] < vect[j] ) _maxim[j] = vect[j];
    }
}
//===============================================================
////Function Name: init_facet_control_points
////
////Member Type:  PRIVATE
////Description:  compute the control points for a facet
////===============================================================
ErrorCode SmoothFace::init_facet_control_points( CartVect N[6],     // vertex normals (per edge)
                                                 CartVect P[3][5],  // edge control points
                                                 CartVect G[6] )    // return internal control points
{
    CartVect Di[4], Ai[3], N0, N3, Vi[4], Wi[3];
    double denom;
    double lambda[2], mu[2];

    ErrorCode rval = MB_SUCCESS;

    for( int i = 0; i < 3; i++ )
    {
        N0    = N[i * 2];
        N3    = N[i * 2 + 1];
        Vi[0] = P[i][0];
        Vi[1] = ( P[i][1] - 0.25 * P[i][0] ) / 0.75;
        Vi[2] = ( P[i][3] - 0.25 * P[i][4] ) / 0.75;
        Vi[3] = P[i][4];
        Wi[0] = Vi[1] - Vi[0];
        Wi[1] = Vi[2] - Vi[1];
        Wi[2] = Vi[3] - Vi[2];
        Di[0] = P[( i + 2 ) % 3][3] - 0.5 * ( P[i][1] + P[i][0] );
        Di[3] = P[( i + 1 ) % 3][1] - 0.5 * ( P[i][4] + P[i][3] );
        Ai[0] = ( N0 * Wi[0] ) / Wi[0].length();
        Ai[2] = ( N3 * Wi[2] ) / Wi[2].length();
        Ai[1] = Ai[0] + Ai[2];
        denom = Ai[1].length();
        Ai[1] /= denom;
        lambda[0] = ( Di[0] % Wi[0] ) / ( Wi[0] % Wi[0] );
        lambda[1] = ( Di[3] % Wi[2] ) / ( Wi[2] % Wi[2] );
        mu[0]     = ( Di[0] % Ai[0] );
        mu[1]     = ( Di[3] % Ai[2] );
        G[i * 2]  = 0.5 * ( P[i][1] + P[i][2] ) + 0.66666666666666 * lambda[0] * Wi[1] +
                   0.33333333333333 * lambda[1] * Wi[0] + 0.66666666666666 * mu[0] * Ai[1] +
                   0.33333333333333 * mu[1] * Ai[0];
        G[i * 2 + 1] = 0.5 * ( P[i][2] + P[i][3] ) + 0.33333333333333 * lambda[0] * Wi[2] +
                       0.66666666666666 * lambda[1] * Wi[1] + 0.33333333333333 * mu[0] * Ai[2] +
                       0.66666666666666 * mu[1] * Ai[1];
    }
    return rval;
}

void SmoothFace::DumpModelControlPoints()
{
    // here, we will dump all control points from edges and facets (6 control points for each facet)
    // we may also create some edges; maybe later...
    // create a point3D file
    // output a Point3D file (special visit format)
    unsigned long setId = _mb->id_from_handle( _set );
    char name[50]       = { 0 };
    sprintf( name, "%lucontrol.Point3D", setId );  // name should be something 2control.Point3D
    std::ofstream point3DFile;
    point3DFile.open( name );  //("control.Point3D");
    point3DFile << "# x y z \n";
    std::ofstream point3DEdgeFile;
    sprintf( name, "%lucontrolEdge.Point3D", setId );  //
    point3DEdgeFile.open( name );                      //("controlEdge.Point3D");
    point3DEdgeFile << "# x y z \n";
    std::ofstream smoothPoints;
    sprintf( name, "%lusmooth.Point3D", setId );  //
    smoothPoints.open( name );                    //("smooth.Point3D");
    smoothPoints << "# x y z \n";
    CartVect controlPoints[3];  // edge control points
    for( Range::iterator it = _edges.begin(); it != _edges.end(); ++it )
    {
        EntityHandle edge = *it;

        _mb->tag_get_data( _edgeCtrlTag, &edge, 1, (double*)&controlPoints[0] );
        for( int i = 0; i < 3; i++ )
        {
            CartVect& c = controlPoints[i];
            point3DEdgeFile << std::setprecision( 11 ) << c[0] << " " << c[1] << " " << c[2] << " \n";
        }
    }
    CartVect controlTriPoints[6];  // triangle control points
    CartVect P_facet[3];           // result in 3 "mid" control points
    for( Range::iterator it2 = _triangles.begin(); it2 != _triangles.end(); ++it2 )
    {
        EntityHandle tri = *it2;

        _mb->tag_get_data( _facetCtrlTag, &tri, 1, (double*)&controlTriPoints[0] );

        // draw a line of points between pairs of control points
        int numPoints = 7;
        for( int n = 0; n < numPoints; n++ )
        {
            double a = 1. * n / ( numPoints - 1 );
            double b = 1.0 - a;

            P_facet[0] = a * controlTriPoints[3] + b * controlTriPoints[4];
            // 1,2,1
            P_facet[1] = a * controlTriPoints[0] + b * controlTriPoints[5];
            // 1,1,2
            P_facet[2] = a * controlTriPoints[1] + b * controlTriPoints[2];
            for( int i = 0; i < 3; i++ )
            {
                CartVect& c = P_facet[i];
                point3DFile << std::setprecision( 11 ) << c[0] << " " << c[1] << " " << c[2] << " \n";
            }
        }

        // evaluate for each triangle a lattice of points
        int N = 40;
        for( int k = 0; k <= N; k++ )
        {
            for( int m = 0; m <= N - k; m++ )
            {
                int n = N - m - k;
                CartVect areacoord( 1. * k / N, 1. * m / N, 1. * n / N );
                CartVect pt;
                eval_bezier_patch( tri, areacoord, pt );
                smoothPoints << std::setprecision( 11 ) << pt[0] << " " << pt[1] << " " << pt[2] << " \n";
            }
        }
    }
    point3DFile.close();
    smoothPoints.close();
    point3DEdgeFile.close();
    return;
}
//===========================================================================
// Function Name: evaluate_single
//
// Member Type:  PUBLIC
// Description:  evaluate edge not associated with a facet (this is used
// by camal edge mesher!!!)
// Note:         t is a value from 0 to 1, for us
//===========================================================================
ErrorCode SmoothFace::evaluate_smooth_edge( EntityHandle eh, double& tt, CartVect& outv )
{
    CartVect P[2];              // P0 and P1
    CartVect controlPoints[3];  // edge control points
    double t4, t3, t2, one_minus_t, one_minus_t2, one_minus_t3, one_minus_t4;

    // project the position to the linear edge
    // t is from 0 to 1 only!!
    // double tt = (t + 1) * 0.5;
    if( tt <= 0.0 ) tt = 0.0;
    if( tt >= 1.0 ) tt = 1.0;

    int nnodes                = 0;
    const EntityHandle* conn2 = NULL;
    ErrorCode rval            = _mb->get_connectivity( eh, conn2, nnodes );
    assert( rval == MB_SUCCESS );
    if( MB_SUCCESS != rval ) return rval;

    rval = _mb->get_coords( conn2, 2, (double*)&P[0] );
    assert( rval == MB_SUCCESS );
    if( MB_SUCCESS != rval ) return rval;

    rval = _mb->tag_get_data( _edgeCtrlTag, &eh, 1, (double*)&controlPoints[0] );
    assert( rval == MB_SUCCESS );
    if( MB_SUCCESS != rval ) return rval;

    t2           = tt * tt;
    t3           = t2 * tt;
    t4           = t3 * tt;
    one_minus_t  = 1. - tt;
    one_minus_t2 = one_minus_t * one_minus_t;
    one_minus_t3 = one_minus_t2 * one_minus_t;
    one_minus_t4 = one_minus_t3 * one_minus_t;

    outv = one_minus_t4 * P[0] + 4. * one_minus_t3 * tt * controlPoints[0] + 6. * one_minus_t2 * t2 * controlPoints[1] +
           4. * one_minus_t * t3 * controlPoints[2] + t4 * P[1];

    return MB_SUCCESS;
}

ErrorCode SmoothFace::eval_bezier_patch( EntityHandle tri, CartVect& areacoord, CartVect& pt )
{
    //
    // interpolate internal control points

    CartVect gctrl_pts[6];
    // get the control points  facet->get_control_points( gctrl_pts );
    // init_facet_control_points( N, P, G) ;
    // what do we need to store in the tag control points?
    ErrorCode rval = _mb->tag_get_data( _facetCtrlTag, &tri, 1, &gctrl_pts[0] );  // get all 6 control points
    assert( MB_SUCCESS == rval );
    if( MB_SUCCESS != rval ) return rval;
    const EntityHandle* conn3 = NULL;
    int nnodes                = 0;
    rval                      = _mb->get_connectivity( tri, conn3, nnodes );
    assert( MB_SUCCESS == rval );

    CartVect vN[3];
    _mb->get_coords( conn3, 3, (double*)&vN[0] );  // fill the coordinates of the vertices

    if( fabs( areacoord[1] + areacoord[2] ) < 1.0e-6 )
    {
        pt = vN[0];
        return MB_SUCCESS;
    }
    if( fabs( areacoord[0] + areacoord[2] ) < 1.0e-6 )
    {
        pt = vN[0];
        return MB_SUCCESS;
    }
    if( fabs( areacoord[0] + areacoord[1] ) < 1.0e-6 )
    {
        pt = vN[0];
        return MB_SUCCESS;
    }

    CartVect P_facet[3];
    // 2,1,1
    P_facet[0] =
        ( 1.0e0 / ( areacoord[1] + areacoord[2] ) ) * ( areacoord[1] * gctrl_pts[3] + areacoord[2] * gctrl_pts[4] );
    // 1,2,1
    P_facet[1] =
        ( 1.0e0 / ( areacoord[0] + areacoord[2] ) ) * ( areacoord[0] * gctrl_pts[0] + areacoord[2] * gctrl_pts[5] );
    // 1,1,2
    P_facet[2] =
        ( 1.0e0 / ( areacoord[0] + areacoord[1] ) ) * ( areacoord[0] * gctrl_pts[1] + areacoord[1] * gctrl_pts[2] );

    // sum the contribution from each of the control points

    pt = CartVect( 0. );  // set all to 0, we start adding / accumulating different parts
    // first edge is from node 0 to 1, index 2 in

    // retrieve the points, in order, and the control points on edges

    // store here again the 9 control points on the edges
    CartVect CP[9];
    rval = _mb->tag_get_data( _facetEdgeCtrlTag, &tri, 1, &CP[0] );
    assert( MB_SUCCESS == rval );

    // CubitFacetEdge *edge;
    // edge = facet->edge(2);! start with edge 2, from 0-1
    int k = 0;
    CartVect ctrl_pts[5];
    // edge->control_points(facet, ctrl_pts);
    ctrl_pts[0] = vN[0];  //
    for( k = 1; k < 4; k++ )
        ctrl_pts[k] = CP[k + 5];  // for edge index 2
    ctrl_pts[4] = vN[1];          //

    // i=4; j=0; k=0;
    double B = mbquart( areacoord[0] );
    pt += B * ctrl_pts[0];

    // i=3; j=1; k=0;
    B = 4.0 * mbcube( areacoord[0] ) * areacoord[1];
    pt += B * ctrl_pts[1];

    // i=2; j=2; k=0;
    B = 6.0 * mbsqr( areacoord[0] ) * mbsqr( areacoord[1] );
    pt += B * ctrl_pts[2];

    // i=1; j=3; k=0;
    B = 4.0 * areacoord[0] * mbcube( areacoord[1] );
    pt += B * ctrl_pts[3];

    // edge = facet->edge(0);
    // edge->control_points(facet, ctrl_pts);
    // edge index 0, from 1 to 2
    ctrl_pts[0] = vN[1];  //
    for( k = 1; k < 4; k++ )
        ctrl_pts[k] = CP[k - 1];  // for edge index 0
    ctrl_pts[4] = vN[2];          //

    // i=0; j=4; k=0;
    B = mbquart( areacoord[1] );
    pt += B * ctrl_pts[0];

    // i=0; j=3; k=1;
    B = 4.0 * mbcube( areacoord[1] ) * areacoord[2];
    pt += B * ctrl_pts[1];

    // i=0; j=2; k=2;
    B = 6.0 * mbsqr( areacoord[1] ) * mbsqr( areacoord[2] );
    pt += B * ctrl_pts[2];

    // i=0; j=1; k=3;
    B = 4.0 * areacoord[1] * mbcube( areacoord[2] );
    pt += B * ctrl_pts[3];

    // edge = facet->edge(1);
    // edge->control_points(facet, ctrl_pts);
    // edge index 1, from 2 to 0
    ctrl_pts[0] = vN[2];  //
    for( k = 1; k < 4; k++ )
        ctrl_pts[k] = CP[k + 2];  // for edge index 0
    ctrl_pts[4] = vN[0];          //

    // i=0; j=0; k=4;
    B = mbquart( areacoord[2] );
    pt += B * ctrl_pts[0];

    // i=1; j=0; k=3;
    B = 4.0 * areacoord[0] * mbcube( areacoord[2] );
    pt += B * ctrl_pts[1];

    // i=2; j=0; k=2;
    B = 6.0 * mbsqr( areacoord[0] ) * mbsqr( areacoord[2] );
    pt += B * ctrl_pts[2];

    // i=3; j=0; k=1;
    B = 4.0 * mbcube( areacoord[0] ) * areacoord[2];
    pt += B * ctrl_pts[3];

    // i=2; j=1; k=1;
    B = 12.0 * mbsqr( areacoord[0] ) * areacoord[1] * areacoord[2];
    pt += B * P_facet[0];

    // i=1; j=2; k=1;
    B = 12.0 * areacoord[0] * mbsqr( areacoord[1] ) * areacoord[2];
    pt += B * P_facet[1];

    // i=1; j=1; k=2;
    B = 12.0 * areacoord[0] * areacoord[1] * mbsqr( areacoord[2] );
    pt += B * P_facet[2];

    return MB_SUCCESS;
}

//===========================================================================
// Function Name: project_to_facet_plane
//
// Member Type:  PUBLIC
// Descriptoin:  Project a point to the plane of a facet
//===========================================================================
void SmoothFace::project_to_facet_plane( EntityHandle tri, CartVect& pt, CartVect& point_on_plane,
                                         double& dist_to_plane )
{
    double plane[4];

    ErrorCode rval = _mb->tag_get_data( _planeTag, &tri, 1, plane );
    if( MB_SUCCESS != rval ) return;
    assert( rval == MB_SUCCESS );
    // _planeTag
    CartVect normal( &plane[0] );  // just first 3 components are used

    double dist    = normal % pt + plane[3];  // coeff d is saved!!!
    dist_to_plane  = fabs( dist );
    point_on_plane = pt - dist * normal;

    return;
}

//===========================================================================
// Function Name: facet_area_coordinate
//
// Member Type:  PUBLIC
// Descriptoin:  Determine the area coordinates of a point on the plane
//              of a facet
//===========================================================================
void SmoothFace::facet_area_coordinate( EntityHandle facet, CartVect& pt_on_plane, CartVect& areacoord )
{

    const EntityHandle* conn3 = NULL;
    int nnodes                = 0;
    ErrorCode rval            = _mb->get_connectivity( facet, conn3, nnodes );
    assert( MB_SUCCESS == rval );
    if( rval ) {}  // empty statement to prevent compiler warning

    // double coords[9]; // store the coordinates for the nodes
    //_mb->get_coords(conn3, 3, coords);
    CartVect p[3];
    rval = _mb->get_coords( conn3, 3, (double*)&p[0] );
    assert( MB_SUCCESS == rval );
    if( rval ) {}  // empty statement to prevent compiler warning
    double plane[4];

    rval = _mb->tag_get_data( _planeTag, &facet, 1, plane );
    assert( rval == MB_SUCCESS );
    if( rval ) {}                  // empty statement to prevent compiler warning
    CartVect normal( &plane[0] );  // just first 3 components are used

    double area2;

    double tol = GEOMETRY_RESABS * 1.e-5;  // 1.e-11;

    CartVect v1( p[1] - p[0] );
    CartVect v2( p[2] - p[0] );

    area2 = ( v1 * v2 ).length_squared();  // the same for CartVect
    if( area2 < 100 * tol ) { tol = .01 * area2; }
    CartVect absnorm( fabs( normal[0] ), fabs( normal[1] ), fabs( normal[2] ) );

    // project to the closest coordinate plane so we only have to do this in 2D

    if( absnorm[0] >= absnorm[1] && absnorm[0] >= absnorm[2] )
    {
        area2 = determ3( p[0][1], p[0][2], p[1][1], p[1][2], p[2][1], p[2][2] );
        if( fabs( area2 ) < tol )
        {
            areacoord = CartVect( -std::numeric_limits< double >::min() );  // .set(
                                                                            // -std::numeric_limits<double>::min(),
                                                                            // -std::numeric_limits<double>::min(),
                                                                            // -std::numeric_limits<double>::min() );
        }
        else if( within_tolerance( p[0], pt_on_plane, GEOMETRY_RESABS ) )
        {
            areacoord = CartVect( 1., 0., 0. );  //.set( 1.0, 0.0, 0.0 );
        }
        else if( within_tolerance( p[1], pt_on_plane, GEOMETRY_RESABS ) )
        {
            areacoord = CartVect( 0., 1., 0. );  //.set( 0.0, 1.0, 0.0 );
        }
        else if( within_tolerance( p[2], pt_on_plane, GEOMETRY_RESABS ) )
        {
            areacoord = CartVect( 0., 0., 1. );  //.set( 0.0, 0.0, 1.0 );
        }
        else
        {

            areacoord[0] = determ3( pt_on_plane[1], pt_on_plane[2], p[1][1], p[1][2], p[2][1], p[2][2] ) / area2;

            areacoord[1] = determ3( p[0][1], p[0][2], pt_on_plane[1], pt_on_plane[2], p[2][1], p[2][2] ) / area2;

            areacoord[2] = determ3( p[0][1], p[0][2], p[1][1], p[1][2], pt_on_plane[1], pt_on_plane[2] ) / area2;
        }
    }
    else if( absnorm[1] >= absnorm[0] && absnorm[1] >= absnorm[2] )
    {

        area2 = determ3( p[0][0], p[0][2], p[1][0], p[1][2], p[2][0], p[2][2] );
        if( fabs( area2 ) < tol )
        {
            areacoord = CartVect( -std::numeric_limits< double >::min() );  //.set(
                                                                            //-std::numeric_limits<double>::min(),
                                                                            //-std::numeric_limits<double>::min(),
                                                                            //-std::numeric_limits<double>::min() );
        }
        else if( within_tolerance( p[0], pt_on_plane, GEOMETRY_RESABS ) )
        {
            areacoord = CartVect( 1., 0., 0. );  //.set( 1.0, 0.0, 0.0 );
        }
        else if( within_tolerance( p[1], pt_on_plane, GEOMETRY_RESABS ) )
        {
            areacoord = CartVect( 0., 1., 0. );  //.set( 0.0, 1.0, 0.0 );
        }
        else if( within_tolerance( p[2], pt_on_plane, GEOMETRY_RESABS ) )
        {
            areacoord = CartVect( 0., 0., 1. );  //.set( 0.0, 0.0, 1.0 );
        }
        else
        {

            areacoord[0] = determ3( pt_on_plane[0], pt_on_plane[2], p[1][0], p[1][2], p[2][0], p[2][2] ) / area2;

            areacoord[1] = determ3( p[0][0], p[0][2], pt_on_plane[0], pt_on_plane[2], p[2][0], p[2][2] ) / area2;

            areacoord[2] = determ3( p[0][0], p[0][2], p[1][0], p[1][2], pt_on_plane[0], pt_on_plane[2] ) / area2;
        }
    }
    else
    {
        /*area2 = determ3(pt0->x(), pt0->y(),
         pt1->x(), pt1->y(),
         pt2->x(), pt2->y());*/
        area2 = determ3( p[0][0], p[0][1], p[1][0], p[1][1], p[2][0], p[2][1] );
        if( fabs( area2 ) < tol )
        {
            areacoord = CartVect( -std::numeric_limits< double >::min() );  //.set(
                                                                            //-std::numeric_limits<double>::min(),
                                                                            //-std::numeric_limits<double>::min(),
                                                                            //-std::numeric_limits<double>::min() );
        }
        else if( within_tolerance( p[0], pt_on_plane, GEOMETRY_RESABS ) )
        {
            areacoord = CartVect( 1., 0., 0. );  //.set( 1.0, 0.0, 0.0 );
        }
        else if( within_tolerance( p[1], pt_on_plane, GEOMETRY_RESABS ) )
        {
            areacoord = CartVect( 0., 1., 0. );  //.set( 0.0, 1.0, 0.0 );
        }
        else if( within_tolerance( p[2], pt_on_plane, GEOMETRY_RESABS ) )
        {
            areacoord = CartVect( 0., 0., 1. );  //.set( 0.0, 0.0, 1.0 );
        }
        else
        {

            areacoord[0] = determ3( pt_on_plane[0], pt_on_plane[1], p[1][0], p[1][1], p[2][0], p[2][1] ) / area2;

            areacoord[1] = determ3( p[0][0], p[0][1], pt_on_plane[0], pt_on_plane[1], p[2][0], p[2][1] ) / area2;

            areacoord[2] = determ3( p[0][0], p[0][1], p[1][0], p[1][1], pt_on_plane[0], pt_on_plane[1] ) / area2;
        }
    }
}

ErrorCode SmoothFace::project_to_facets_main( CartVect& this_point, bool trim, bool& outside,
                                              CartVect* closest_point_ptr, CartVect* normal_ptr )
{

    // if there are a lot of facets on this surface - use the OBB search first
    // to narrow the selection

    _evaluationsCounter++;
    double tolerance = 1.e-5;
    std::vector< EntityHandle > facets_out;
    // we will start with a list of facets anyway, the best among them wins
    ErrorCode rval =
        _my_geomTopoTool->obb_tree()->closest_to_location( (double*)&this_point, _obb_root, tolerance, facets_out );
    if( MB_SUCCESS != rval ) return rval;

    int interpOrder        = 4;
    double compareTol      = 1.e-5;
    EntityHandle lastFacet = facets_out.front();
    rval = project_to_facets( facets_out, lastFacet, interpOrder, compareTol, this_point, trim, outside,
                              closest_point_ptr, normal_ptr );

    return rval;
}
ErrorCode SmoothFace::project_to_facets( std::vector< EntityHandle >& facet_list, EntityHandle& lastFacet,
                                         int interpOrder, double compareTol, CartVect& this_point, bool, bool& outside,
                                         CartVect* closest_point_ptr, CartVect* normal_ptr )
{

    bool outside_facet      = false;
    bool best_outside_facet = true;
    double mindist          = 1.e20;
    CartVect close_point, best_point( mindist, mindist, mindist ), best_areacoord;
    EntityHandle best_facet = 0L;  // no best facet found yet
    EntityHandle facet;
    assert( facet_list.size() > 0 );

    double big_dist = compareTol * 1.0e3;

    // from the list of close facets, determine the closest point
    for( size_t i = 0; i < facet_list.size(); i++ )
    {
        facet = facet_list[i];
        CartVect pt_on_plane;
        double dist_to_plane;
        project_to_facet_plane( facet, this_point, pt_on_plane, dist_to_plane );

        CartVect areacoord;
        // CartVect close_point;
        facet_area_coordinate( facet, pt_on_plane, areacoord );
        if( interpOrder != 0 )
        {

            // modify the areacoord - project to the bezier patch- snaps to the
            // edge of the patch if necessary

            if( project_to_facet( facet, this_point, areacoord, close_point, outside_facet, compareTol ) != MB_SUCCESS )
            { return MB_FAILURE; }
            // if (closest_point_ptr)
            //*closest_point_ptr = close_point;
        }
        // keep track of the minimum distance

        double dist = ( close_point - this_point ).length();  // close_point.distance_between(this_point);
        if( ( best_outside_facet == outside_facet && dist < mindist ) ||
            ( best_outside_facet && !outside_facet && ( dist < big_dist || best_facet == 0L /*!best_facet*/ ) ) )
        {
            mindist            = dist;
            best_point         = close_point;
            best_facet         = facet;
            best_areacoord     = areacoord;
            best_outside_facet = outside_facet;

            if( dist < compareTol ) { break; }
            big_dist = 10.0 * mindist;
        }
        // facet->marked(1);
        // used_facet_list.append(facet);
    }

    if( normal_ptr )
    {
        CartVect normal;
        if( eval_bezier_patch_normal( best_facet, best_areacoord, normal ) != MB_SUCCESS ) { return MB_FAILURE; }
        *normal_ptr = normal;
    }

    if( closest_point_ptr ) { *closest_point_ptr = best_point; }

    outside   = best_outside_facet;
    lastFacet = best_facet;

    return MB_SUCCESS;
    // end copy
}

//===========================================================================
// Function Name: project_to_patch
//
// Member Type:  PUBLIC
// Description:  Project a point to a bezier patch. Pass in the areacoord
//              of the point projected to the linear facet.  Function
//              assumes that the point is contained within the patch -
//              if not, it will project to one of its edges.
//===========================================================================
ErrorCode SmoothFace::project_to_patch( EntityHandle facet,   // (IN) the facet where the patch is defined
                                        CartVect& ac,         // (IN) area coordinate initial guess (from linear facet)
                                        CartVect& pt,         // (IN) point we are projecting to patch
                                        CartVect& eval_pt,    // (OUT) The projected point
                                        CartVect* eval_norm,  // (OUT) normal at evaluated point
                                        bool& outside,        // (OUT) the closest point on patch to pt is on an edge
                                        double compare_tol,   // (IN) comparison tolerance
                                        int edge_id )         // (IN) only used if this is to be projected to one
//      of the edges.  Otherwise, should be -1
{
    ErrorCode status = MB_SUCCESS;

    // see if we are at a vertex

#define INCR 0.01
    const double tol = compare_tol;

    if( is_at_vertex( facet, pt, ac, compare_tol, eval_pt, eval_norm ) )
    {
        outside = false;
        return MB_SUCCESS;
    }

    // check if the start ac is inside the patch -if not, then move it there

    int nout          = 0;
    const double atol = 0.001;
    if( move_ac_inside( ac, atol ) ) nout++;

    int diverge = 0;
    int iter    = 0;
    CartVect newpt;
    eval_bezier_patch( facet, ac, newpt );
    CartVect move   = pt - newpt;
    double lastdist = move.length();
    double bestdist = lastdist;
    CartVect bestac = ac;
    CartVect bestpt = newpt;
    CartVect bestnorm( 0, 0, 0 );

    // If we are already close enough, then return now

    if( lastdist <= tol && !eval_norm && nout == 0 )
    {
        eval_pt = pt;
        outside = false;
        return status;
    }

    double ratio, mag, umove, vmove, det, distnew, movedist;
    CartVect lastpt = newpt;
    CartVect lastac = ac;
    CartVect norm;
    CartVect xpt, ypt, zpt, xac, yac, zac, xvec, yvec, zvec;
    CartVect du, dv, newac;
    bool done = false;
    while( !done )
    {

        // We will be locating the projected point within the u,v,w coordinate
        // system of the triangular bezier patch.  Since u+v+w=1, the components
        // are not linearly independent.  We will choose only two of the
        // coordinates to use and compute the third.

        int system;
        if( lastac[0] >= lastac[1] && lastac[0] >= lastac[2] ) { system = 0; }
        else if( lastac[1] >= lastac[2] )
        {
            system = 1;
        }
        else
        {
            system = 2;
        }

        // compute the surface derivatives with respect to each
        // of the barycentric coordinates

        if( system == 1 || system == 2 )
        {
            xac[0] = lastac[0] + INCR;  // xac.x( lastac.x() + INCR );
            if( lastac[1] + lastac[2] == 0.0 ) return MB_FAILURE;
            ratio  = lastac[2] / ( lastac[1] + lastac[2] );  // ratio = lastac.z() / (lastac.y() + lastac.z());
            xac[1] = ( 1.0 - xac[0] ) * ( 1.0 - ratio );     // xac.y( (1.0 - xac.x()) * (1.0 - ratio) );
            xac[2] = 1.0 - xac[0] - xac[1];                  // xac.z( 1.0 - xac.x() - xac.y() );
            eval_bezier_patch( facet, xac, xpt );
            xvec = xpt - lastpt;
            xvec /= INCR;
        }
        if( system == 0 || system == 2 )
        {
            yac[1] = ( lastac[1] + INCR );      // yac.y( lastac.y() + INCR );
            if( lastac[0] + lastac[2] == 0.0 )  // if (lastac.x() + lastac.z() == 0.0)
                return MB_FAILURE;
            ratio  = lastac[2] / ( lastac[0] + lastac[2] );   // ratio = lastac.z() / (lastac.x() + lastac.z());
            yac[0] = ( ( 1.0 - yac[1] ) * ( 1.0 - ratio ) );  // yac.x( (1.0 - yac.y()) * (1.0 - ratio) );
            yac[2] = ( 1.0 - yac[0] - yac[1] );               // yac.z( 1.0 - yac.x() - yac.y() );
            eval_bezier_patch( facet, yac, ypt );
            yvec = ypt - lastpt;
            yvec /= INCR;
        }
        if( system == 0 || system == 1 )
        {
            zac[2] = ( lastac[2] + INCR );      // zac.z( lastac.z() + INCR );
            if( lastac[0] + lastac[1] == 0.0 )  // if (lastac.x() + lastac.y() == 0.0)
                return MB_FAILURE;
            ratio  = lastac[1] / ( lastac[0] + lastac[1] );   // ratio = lastac.y() / (lastac.x() + lastac.y());
            zac[0] = ( ( 1.0 - zac[2] ) * ( 1.0 - ratio ) );  // zac.x( (1.0 - zac.z()) * (1.0 - ratio) );
            zac[1] = ( 1.0 - zac[0] - zac[2] );               // zac.y( 1.0 - zac.x() - zac.z() );
            eval_bezier_patch( facet, zac, zpt );
            zvec = zpt - lastpt;
            zvec /= INCR;
        }

        // compute the surface normal

        switch( system )
        {
            case 0:
                du = yvec;
                dv = zvec;
                break;
            case 1:
                du = zvec;
                dv = xvec;
                break;
            case 2:
                du = xvec;
                dv = yvec;
                break;
        }
        norm = du * dv;
        mag  = norm.length();
        if( mag < DBL_EPSILON )
        {
            return MB_FAILURE;
            // do something else here (it is likely a flat triangle -
            // so try evaluating just an edge of the bezier patch)
        }
        norm /= mag;
        if( iter == 0 ) bestnorm = norm;

        // project the move vector to the tangent plane

        move = ( norm * move ) * norm;

        // compute an equivalent u-v-w vector

        CartVect absnorm( fabs( norm[0] ), fabs( norm[1] ), fabs( norm[2] ) );
        if( absnorm[2] >= absnorm[1] && absnorm[2] >= absnorm[0] )
        {
            det = du[0] * dv[1] - dv[0] * du[1];
            if( fabs( det ) <= DBL_EPSILON )
            {
                return MB_FAILURE;  // do something else here
            }
            umove = ( move[0] * dv[1] - dv[0] * move[1] ) / det;
            vmove = ( du[0] * move[1] - move[0] * du[1] ) / det;
        }
        else if( absnorm[1] >= absnorm[2] && absnorm[1] >= absnorm[0] )
        {
            det = du[0] * dv[2] - dv[0] * du[2];
            if( fabs( det ) <= DBL_EPSILON ) { return MB_FAILURE; }
            umove = ( move[0] * dv[2] - dv[0] * move[2] ) / det;
            vmove = ( du[0] * move[2] - move[0] * du[2] ) / det;
        }
        else
        {
            det = du[1] * dv[2] - dv[1] * du[2];
            if( fabs( det ) <= DBL_EPSILON ) { return MB_FAILURE; }
            umove = ( move[1] * dv[2] - dv[1] * move[2] ) / det;
            vmove = ( du[1] * move[2] - move[1] * du[2] ) / det;
        }

        /* === compute the new u-v coords and evaluate surface at new location */

        switch( system )
        {
            case 0:
                newac[1] = ( lastac[1] + umove );          // newac.y( lastac.y() + umove );
                newac[2] = ( lastac[2] + vmove );          // newac.z( lastac.z() + vmove );
                newac[0] = ( 1.0 - newac[1] - newac[2] );  // newac.x( 1.0 - newac.y() - newac.z() );
                break;
            case 1:
                newac[2] = ( lastac[2] + umove );          // newac.z( lastac.z() + umove );
                newac[0] = ( lastac[0] + vmove );          // newac.x( lastac.x() + vmove );
                newac[1] = ( 1.0 - newac[2] - newac[0] );  // newac.y( 1.0 - newac.z() - newac.x() );
                break;
            case 2:
                newac[0] = ( lastac[0] + umove );          // newac.x( lastac.x() + umove );
                newac[1] = ( lastac[1] + vmove );          // newac.y( lastac.y() + vmove );
                newac[2] = ( 1.0 - newac[0] - newac[1] );  // newac.z( 1.0 - newac.x() - newac.y() );
                break;
        }

        // Keep it inside the patch

        if( newac[0] >= -atol && newac[1] >= -atol && newac[2] >= -atol ) { nout = 0; }
        else
        {
            if( move_ac_inside( newac, atol ) ) nout++;
        }

        // Evaluate at the new location

        if( edge_id != -1 ) ac_at_edge( newac, newac, edge_id );  // move to edge first
        eval_bezier_patch( facet, newac, newpt );

        // Check for convergence

        distnew  = ( pt - newpt ).length();  // pt.distance_between(newpt);
        move     = newpt - lastpt;
        movedist = move.length();
        if( movedist < tol || distnew < tol )
        {
            done = true;
            if( distnew < bestdist )
            {
                bestdist = distnew;
                bestac   = newac;
                bestpt   = newpt;
                bestnorm = norm;
            }
        }
        else
        {

            // don't allow more than 30 iterations

            iter++;
            if( iter > 30 )
            {
                // if (movedist > tol * 100.0) nout=1;
                done = true;
            }

            // Check for divergence - don't allow more than 5 divergent
            // iterations

            if( distnew > lastdist )
            {
                diverge++;
                if( diverge > 10 )
                {
                    done = true;
                    // if (movedist > tol * 100.0) nout=1;
                }
            }

            // Check if we are continuing to project outside the facet.
            // If so, then stop now

            if( nout > 3 ) { done = true; }

            // set up for next iteration

            if( !done )
            {
                if( distnew < bestdist )
                {
                    bestdist = distnew;
                    bestac   = newac;
                    bestpt   = newpt;
                    bestnorm = norm;
                }
                lastdist = distnew;
                lastpt   = newpt;
                lastac   = newac;
                move     = pt - lastpt;
            }
        }
    }

    eval_pt = bestpt;
    if( eval_norm ) { *eval_norm = bestnorm; }
    outside = ( nout > 0 ) ? true : false;
    ac      = bestac;

    return status;
}

//===========================================================================
// Function Name: ac_at_edge
//
// Member Type:  PRIVATE
// Description:  determine the area coordinate of the facet at the edge
//===========================================================================
void SmoothFace::ac_at_edge( CartVect& fac,  // facet area coordinate
                             CartVect& eac,  // edge area coordinate
                             int edge_id )   // id of edge
{
    double u, v, w;
    switch( edge_id )
    {
        case 0:
            u = 0.0;
            v = fac[1] / ( fac[1] + fac[2] );  // v = fac.y() / (fac.y() + fac.z());
            w = 1.0 - v;
            break;
        case 1:
            u = fac[0] / ( fac[0] + fac[2] );  // u = fac.x() / (fac.x() + fac.z());
            v = 0.0;
            w = 1.0 - u;
            break;
        case 2:
            u = fac[0] / ( fac[0] + fac[1] );  // u = fac.x() / (fac.x() + fac.y());
            v = 1.0 - u;
            w = 0.0;
            break;
        default:
            assert( 0 );
            u = -1;  // needed to eliminate warnings about used before set
            v = -1;  // needed to eliminate warnings about used before set
            w = -1;  // needed to eliminate warnings about used before set
            break;
    }
    eac[0] = u;
    eac[1] = v;
    eac[2] = w;  //= CartVect(u, v, w);
}

//===========================================================================
// Function Name: project_to_facet
//
// Member Type:  PUBLIC
// Description:  project to a single facet.  Uses the input areacoord as
//              a starting guess.
//===========================================================================
ErrorCode SmoothFace::project_to_facet( EntityHandle facet, CartVect& pt, CartVect& areacoord, CartVect& close_point,
                                        bool& outside_facet, double compare_tol )
{
    const EntityHandle* conn3 = NULL;
    int nnodes                = 0;
    _mb->get_connectivity( facet, conn3, nnodes );
    //
    // double coords[9]; // store the coordinates for the nodes
    //_mb->get_coords(conn3, 3, coords);
    CartVect p[3];
    _mb->get_coords( conn3, 3, (double*)&p[0] );

    int edge_id    = -1;
    ErrorCode stat = project_to_patch( facet, areacoord, pt, close_point, NULL, outside_facet, compare_tol, edge_id );
    /* }
     break;
     }*/

    return stat;
}

//===========================================================================
// Function Name: is_at_vertex
//
// Member Type:  PRIVATE
// Description:  determine if the point is at one of the facet's vertices
//===========================================================================
bool SmoothFace::is_at_vertex( EntityHandle facet,        // (IN) facet we are evaluating
                               CartVect& pt,              // (IN) the point
                               CartVect& ac,              // (IN) the ac of the point on the facet plane
                               double compare_tol,        // (IN) return TRUE of closer than this
                               CartVect& eval_pt,         // (OUT) location at vertex if TRUE
                               CartVect* eval_norm_ptr )  // (OUT) normal at vertex if TRUE
{
    double dist;
    CartVect vert_loc;
    const double actol = 0.1;
    // get coordinates get_coords
    const EntityHandle* conn3 = NULL;
    int nnodes                = 0;
    _mb->get_connectivity( facet, conn3, nnodes );
    //
    // double coords[9]; // store the coordinates for the nodes
    //_mb->get_coords(conn3, 3, coords);
    CartVect p[3];
    _mb->get_coords( conn3, 3, (double*)&p[0] );
    // also get the normals at nodes
    CartVect NN[3];
    _mb->tag_get_data( _gradientTag, conn3, 3, (double*)&NN[0] );
    if( fabs( ac[0] ) < actol && fabs( ac[1] ) < actol )
    {
        vert_loc = p[2];
        dist     = ( pt - vert_loc ).length();  // pt.distance_between( vert_loc );
        if( dist <= compare_tol )
        {
            eval_pt = vert_loc;
            if( eval_norm_ptr ) { *eval_norm_ptr = NN[2]; }
            return true;
        }
    }

    if( fabs( ac[0] ) < actol && fabs( ac[2] ) < actol )
    {
        vert_loc = p[1];
        dist     = ( pt - vert_loc ).length();  // pt.distance_between( vert_loc );
        if( dist <= compare_tol )
        {
            eval_pt = vert_loc;
            if( eval_norm_ptr )
            {
                *eval_norm_ptr = NN[1];  // facet->point(1)->normal( facet );
            }
            return true;
        }
    }

    if( fabs( ac[1] ) < actol && fabs( ac[2] ) < actol )
    {
        vert_loc = p[0];
        dist     = ( pt - vert_loc ).length();  // pt.distance_between( vert_loc );
        if( dist <= compare_tol )
        {
            eval_pt = vert_loc;
            if( eval_norm_ptr ) { *eval_norm_ptr = NN[0]; }
            return true;
        }
    }

    return false;
}

//===========================================================================
// Function Name: move_ac_inside
//
// Member Type:  PRIVATE
// Description:  find the closest area coordinate to the boundary of the
//              patch if any of its components are < 0
//              Return if the ac was modified.
//===========================================================================
bool SmoothFace::move_ac_inside( CartVect& ac, double tol )
{
    int nout = 0;
    if( ac[0] < -tol )
    {
        ac[0] = 0.0;
        ac[1] = ac[1] / ( ac[1] + ac[2] );  //( ac.y() / (ac.y() + ac.z()) ;
        ac[2] = 1. - ac[1];                 // ac.z( 1.0 - ac.y() );
        nout++;
    }
    if( ac[1] < -tol )
    {
        ac[1] = 0.;                         // ac.y( 0.0 );
        ac[0] = ac[0] / ( ac[0] + ac[2] );  // ac.x( ac.x() / (ac.x() + ac.z()) );
        ac[2] = 1. - ac[0];                 // ac.z( 1.0 - ac.x() );
        nout++;
    }
    if( ac[2] < -tol )
    {
        ac[2] = 0.;                         // ac.z( 0.0 );
        ac[0] = ac[0] / ( ac[0] + ac[1] );  // ac.x( ac.x() / (ac.x() + ac.y()) );
        ac[1] = 1. - ac[0];                 // ac.y( 1.0 - ac.x() );
        nout++;
    }
    return ( nout > 0 ) ? true : false;
}

//===========================================================================
// Function Name: hodograph
//
// Member Type:  PUBLIC
// Description:  get the hodograph control points for the facet
// Note:  This is a triangle cubic patch that is defined by the
//       normals of quartic facet control point lattice.  Returned coordinates
//       in Nijk are defined by the following diagram
//
//
//                         *9               index  polar
//                        / \                 0     300    point(0)
//                       /   \                1     210
//                     7*-----*8              2     120
//                     / \   / \              3     030    point(1)
//                    /   \ /   \             4     201
//                  4*----5*-----*6           5     111
//                  / \   / \   / \           6     021
//                 /   \ /   \ /   \          7     102
//                *-----*-----*-----*         8     012
//                0     1     2     3         9     003    point(2)
//

//===========================================================================
// Function Name: eval_bezier_patch_normal
//
// Member Type:  PRIVATE
// Description:  evaluate the Bezier patch defined at a facet
//===========================================================================
ErrorCode SmoothFace::eval_bezier_patch_normal( EntityHandle facet, CartVect& areacoord, CartVect& normal )
{
    // interpolate internal control points

    CartVect gctrl_pts[6];
    // facet->get_control_points( gctrl_pts );
    ErrorCode rval = _mb->tag_get_data( _facetCtrlTag, &facet, 1, &gctrl_pts[0] );
    assert( rval == MB_SUCCESS );
    if( MB_SUCCESS != rval ) return rval;
    // _gradientTag
    // get normals at points
    const EntityHandle* conn3 = NULL;
    int nnodes                = 0;
    rval                      = _mb->get_connectivity( facet, conn3, nnodes );
    if( MB_SUCCESS != rval ) return rval;

    CartVect NN[3];
    rval = _mb->tag_get_data( _gradientTag, conn3, 3, &NN[0] );
    assert( rval == MB_SUCCESS );
    if( MB_SUCCESS != rval ) return rval;

    if( fabs( areacoord[1] + areacoord[2] ) < 1.0e-6 )
    {
        normal = NN[0];
        return MB_SUCCESS;
    }
    if( fabs( areacoord[0] + areacoord[2] ) < 1.0e-6 )
    {
        normal = NN[1];  // facet->point(1)->normal(facet);
        return MB_SUCCESS;
    }
    if( fabs( areacoord[0] + areacoord[1] ) < 1.0e-6 )
    {
        normal = NN[2];  // facet->point(2)->normal(facet);
        return MB_SUCCESS;
    }

    // compute the hodograph of the quartic Gregory patch

    CartVect Nijk[10];
    // hodograph(facet,areacoord,Nijk);
    // start copy from hodograph
    // CubitVector gctrl_pts[6];
    // facet->get_control_points( gctrl_pts );
    CartVect P_facet[3];

    // 2,1,1
    /*P_facet[0] = (1.0e0 / (areacoord.y() + areacoord.z())) *
     (areacoord.y() * gctrl_pts[3] +
     areacoord.z() * gctrl_pts[4]);*/
    P_facet[0] =
        ( 1.0e0 / ( areacoord[1] + areacoord[2] ) ) * ( areacoord[1] * gctrl_pts[3] + areacoord[2] * gctrl_pts[4] );
    // 1,2,1
    /*P_facet[1] = (1.0e0 / (areacoord.x() + areacoord.z())) *
     (areacoord.x() * gctrl_pts[0] +
     areacoord.z() * gctrl_pts[5]);*/
    P_facet[1] =
        ( 1.0e0 / ( areacoord[0] + areacoord[2] ) ) * ( areacoord[0] * gctrl_pts[0] + areacoord[2] * gctrl_pts[5] );
    // 1,1,2
    /*P_facet[2] = (1.0e0 / (areacoord.x() + areacoord.y())) *
     (areacoord.x() * gctrl_pts[1] +
     areacoord.y() * gctrl_pts[2]);*/
    P_facet[2] =
        ( 1.0e0 / ( areacoord[0] + areacoord[1] ) ) * ( areacoord[0] * gctrl_pts[1] + areacoord[1] * gctrl_pts[2] );

    // corner control points are just the normals at the points

    // 3, 0, 0
    Nijk[0] = NN[0];
    // 0, 3, 0
    Nijk[3] = NN[1];
    // 0, 0, 3
    Nijk[9] = NN[2];  // facet->point(2)->normal(facet);

    // fill in the boundary control points.  Define as the normal to the local
    // triangle formed by the quartic control point lattice

    // store here again the 9 control points on the edges
    CartVect CP[9];  // 9 control points on the edges,
    rval = _mb->tag_get_data( _facetEdgeCtrlTag, &facet, 1, &CP[0] );
    if( MB_SUCCESS != rval ) return rval;
    // there are 3 CP for each edge, 0, 1, 2; first edge is 1-2
    // CubitFacetEdge *edge;
    // edge = facet->edge( 2 );
    // CubitVector ctrl_pts[5];
    // edge->control_points(facet, ctrl_pts);

    // 2, 1, 0
    // Nijk[1] = (ctrl_pts[2] - ctrl_pts[1]) * (P_facet[0] - ctrl_pts[1]);
    Nijk[1] = ( CP[7] - CP[6] ) * ( P_facet[0] - CP[6] );
    Nijk[1].normalize();

    // 1, 2, 0
    // Nijk[2] = (ctrl_pts[3] - ctrl_pts[2]) * (P_facet[1] - ctrl_pts[2]);
    Nijk[2] = ( CP[8] - CP[7] ) * ( P_facet[1] - CP[7] );
    Nijk[2].normalize();

    // edge = facet->edge( 0 );
    // edge->control_points(facet, ctrl_pts);

    // 0, 2, 1
    // Nijk[6] = (ctrl_pts[1] - P_facet[1]) * (ctrl_pts[2] - P_facet[1]);
    Nijk[6] = ( CP[0] - P_facet[1] ) * ( CP[1] - P_facet[1] );
    Nijk[6].normalize();

    // 0, 1, 2
    // Nijk[8] = (ctrl_pts[2] - P_facet[2]) * (ctrl_pts[3] - P_facet[2]);
    Nijk[8] = ( CP[1] - P_facet[2] ) * ( CP[2] - P_facet[2] );
    Nijk[8].normalize();

    // edge = facet->edge( 1 );
    // edge->control_points(facet, ctrl_pts);

    // 1, 0, 2
    // Nijk[7] = (P_facet[2] - ctrl_pts[2]) * (ctrl_pts[1] - ctrl_pts[2]);
    Nijk[7] = ( P_facet[2] - CP[4] ) * ( CP[3] - CP[4] );
    Nijk[7].normalize();

    // 2, 0, 1
    // Nijk[4] = (P_facet[0] - ctrl_pts[3]) * (ctrl_pts[2] - ctrl_pts[3]);
    Nijk[4] = ( P_facet[0] - CP[5] ) * ( CP[4] - CP[5] );
    Nijk[4].normalize();

    // 1, 1, 1
    Nijk[5] = ( P_facet[1] - P_facet[0] ) * ( P_facet[2] - P_facet[0] );
    Nijk[5].normalize();
    // end copy from hodograph

    // sum the contribution from each of the control points

    normal = CartVect( 0.0e0, 0.0e0, 0.0e0 );

    // i=3; j=0; k=0;
    // double Bsum = 0.0;
    double B = mbcube( areacoord[0] );
    // Bsum += B;
    normal += B * Nijk[0];

    // i=2; j=1; k=0;
    B = 3.0 * mbsqr( areacoord[0] ) * areacoord[1];
    // Bsum += B;
    normal += B * Nijk[1];

    // i=1; j=2; k=0;
    B = 3.0 * areacoord[0] * mbsqr( areacoord[1] );
    // Bsum += B;
    normal += B * Nijk[2];

    // i=0; j=3; k=0;
    B = mbcube( areacoord[1] );
    // Bsum += B;
    normal += B * Nijk[3];

    // i=2; j=0; k=1;
    B = 3.0 * mbsqr( areacoord[0] ) * areacoord[2];
    // Bsum += B;
    normal += B * Nijk[4];

    // i=1; j=1; k=1;
    B = 6.0 * areacoord[0] * areacoord[1] * areacoord[2];
    // Bsum += B;
    normal += B * Nijk[5];

    // i=0; j=2; k=1;
    B = 3.0 * mbsqr( areacoord[1] ) * areacoord[2];
    // Bsum += B;
    normal += B * Nijk[6];

    // i=1; j=0; k=2;
    B = 3.0 * areacoord[0] * mbsqr( areacoord[2] );
    // Bsum += B;
    normal += B * Nijk[7];

    // i=0; j=1; k=2;
    B = 3.0 * areacoord[1] * mbsqr( areacoord[2] );
    // Bsum += B;
    normal += B * Nijk[8];

    // i=0; j=0; k=3;
    B = mbcube( areacoord[2] );
    // Bsum += B;
    normal += B * Nijk[9];

    // assert(fabs(Bsum - 1.0) < 1e-9);

    normal.normalize();

    return MB_SUCCESS;
}

ErrorCode SmoothFace::get_normals_for_vertices( const EntityHandle* conn2, CartVect N[2] )
// this method will be called to retrieve the normals needed in the calculation of control edge
// points..
{
    // CartVect N[2];
    //_mb->tag_get_data(_gradientTag, conn2, 2, normalVec);
    ErrorCode rval = _mb->tag_get_data( _gradientTag, conn2, 2, (double*)&N[0] );
    return rval;
}

ErrorCode SmoothFace::ray_intersection_correct( EntityHandle,       // (IN) the facet where the patch is defined
                                                CartVect& pt,       // (IN) shoot from
                                                CartVect& ray,      // (IN) ray direction
                                                CartVect& eval_pt,  // (INOUT) The intersection point
                                                double& distance,   // (IN OUT) the new distance
                                                bool& outside )
{
    // find a point on the smooth surface
    CartVect currentPoint = eval_pt;
    int numIter           = 0;
    double improvement    = 1.e20;
    CartVect diff;
    while( numIter++ < 5 && improvement > 0.01 )
    {
        CartVect newPos;

        bool trim = false;  // is it needed?
        outside   = true;
        CartVect closestPoint;
        CartVect normal;

        ErrorCode rval = project_to_facets_main( currentPoint, trim, outside, &newPos, &normal );
        if( MB_SUCCESS != rval ) return rval;
        assert( rval == MB_SUCCESS );
        diff        = newPos - currentPoint;
        improvement = diff.length();
        // ( pt + t * ray - closest ) % normal = 0;
        // intersect tangent plane that goes through closest point with the direction
        // t = normal%(closest-pt) / normal%ray;
        double dot = normal % ray;  // if it is 0, get out while we can
        if( dot < 0.00001 )
        {
            // bad convergence, get out, do not modify anything
            return MB_SUCCESS;
        }
        double t     = ( ( newPos - pt ) % normal ) / ( dot );
        currentPoint = pt + t * ray;
    }
    eval_pt  = currentPoint;
    diff     = currentPoint - pt;
    distance = diff.length();
    return MB_SUCCESS;
}
}  // namespace moab