C++ MsqError::clear方法代码示例

double 
NonGradient::evaluate( double *point,  PatchData &pd, MsqError &err )
{
  if( pd.num_free_vertices() > 1 )
  {
    MSQ_SETERR(err)("Only one free vertex per patch implemented", MsqError::NOT_IMPLEMENTED);
  }
  const size_t vertexIndex = 0; 
  Vector3D originalVec = pd.vertex_by_index(vertexIndex);
  Vector3D pointVec;
  for( int index = 0; index<3; index++)
  {
    pointVec[index] = point[index];
  }
  pd.set_vertex_coordinates( pointVec, vertexIndex, err ); 
  pd.snap_vertex_to_domain( vertexIndex, err );  

  OFEvaluator& obj_func = get_objective_function_evaluator();
  TerminationCriterion* term_crit=get_inner_termination_criterion();

  double value;
  bool feasible = obj_func.evaluate( pd, value, err ); // MSQ_ERRZERO(err);
  term_crit->accumulate_inner( pd, value, 0, err );  //MSQ_CHKERR(err);
  if (err.error_code() == err.BARRIER_VIOLATED)
    err.clear();   // barrier violation not an error here
  MSQ_ERRZERO(err);
  pd.set_vertex_coordinates( originalVec, vertexIndex, err ); 
  if( !feasible && mUseExactPenaltyFunction )  
  { // "value" undefined btw
    double ensureFiniteRtol= .25;
    value = DBL_MAX * ensureFiniteRtol;
    //std::cout << " Infeasible patch: " << value << std::endl; printPatch( pd, err );
  }
  return value;
}

void TagVertexMesh::initialize( Mesh* mesh, std::string name, MsqError& err )
{
  MeshDecorator::set_mesh( mesh );
  tagName = name;

  tagHandle = get_mesh()->tag_get( tagName, err );
    // If tag isn't defined yet, we're done for now.
  if (err.error_code() == MsqError::TAG_NOT_FOUND) {
    err.clear();
    return;
  } 
  else if (MSQ_CHKERR(err))
    return;
  
    // If tag is already defined, make sure it is the correct type.
  std::string t_name;
  Mesh::TagType type;
  unsigned length;
  tag_properties( tagHandle, t_name, type, length, err ); 
  MSQ_ERRRTN(err);
  if (!(type == Mesh::DOUBLE && length == 3) &&
      !(type == Mesh::BYTE && length == 3*sizeof(double)))
    MSQ_SETERR(err)(MsqError::TAG_ALREADY_EXISTS,
                    "Tag \"%s\" has invalid type or size.",
                    tagName.c_str());
 
    // If tag is already defined and init was true, reset tag
    // values.
  haveTagHandle = true;
}

TagHandle TargetWriter::get_tag_handle( const std::string& base_name,
                                        unsigned num_dbl, Mesh* mesh, MsqError& err )
{
  std::ostringstream sstr;
  sstr << base_name << num_dbl;
  
  TagHandle handle = mesh->tag_get( sstr.str().c_str(), err );
  if (!MSQ_CHKERR(err))
  {
    std::string temp_name;
    Mesh::TagType temp_type;
    unsigned temp_length;
    mesh->tag_properties( handle, temp_name, temp_type, temp_length, err );
    MSQ_ERRZERO(err);
    
    if (temp_type != Mesh::DOUBLE || temp_length != num_dbl)
    {
      MSQ_SETERR(err)( MsqError::TAG_ALREADY_EXISTS,
                      "Mismatched type or length for existing tag \"%s\"",
                       sstr.str().c_str() );
    }
  }
  else if (err.error_code() == MsqError::TAG_NOT_FOUND)
  {
    err.clear();
    handle = mesh->tag_create( sstr.str().c_str(), Mesh::DOUBLE, num_dbl, 0, err );
    MSQ_ERRZERO(err);
  }
  return handle;
}

size_t MeshImplTags::create( const TagDescription& desc,
                             const void* defval,
                             MsqError& err )
{
  size_t h = handle( desc.name.c_str(), err );
  if (h)
  {
    MSQ_SETERR(err)(desc.name.c_str(), MsqError::TAG_ALREADY_EXISTS);
    return 0;
  }
  
  err.clear();
  if (desc.size == 0 || (desc.size % size_from_tag_type(desc.type)) != 0)
  {
    MSQ_SETERR(err)(MsqError::INVALID_ARG);
    return 0;
  }
  
  TagData* tag = new TagData( desc );
  h = tagList.size();
  tagList.push_back(tag);
  
  if (defval)
  {
    tag->defaultValue = malloc( tag->desc.size );
    memcpy( tag->defaultValue, defval, tag->desc.size );
  }
  
  return h+1;
}

/*! 
  Numerically Calculates the gradient of the ObjectiveFunction for the
  free vertices in the patch.  Returns 'false' if the patch is outside
  of a required feasible region, returns 'ture' otherwise.
  The behavior of the function depends on the value of the boolean
  useLocalGradient.  If useLocalGradient is set to
  'true', compute_numerical_gradient creates a sub-patch around a free
  vertex, and then perturbs that vertex in one of the coordinate directions.
  Only the ObjectiveFunction value on the local sub-patch is used in the
  computation of the gradient.  Therefore, useLocalGradient should only
  be set to 'true' for ObjectiveFunctions which can use this method.  Unless
  the concrete ObjectiveFunction sets useLocalGradient to 'true' in its
  constructor, the value will be 'false'.  In this case, the objective
  function value for the entire patch is used in the calculation of the
  gradient.  This is computationally expensive, but it is numerically
  correct for all (C_1) functions.
  \param pd  PatchData on which the gradient is taken.
  \param grad  Array of Vector3D of length the number of vertices used to store gradient.
  \param OF_val will be set to the objective function value.
 */
bool ObjectiveFunction::evaluate_with_gradient( EvalType eval_type,
                                                PatchData &pd,
                                                double& OF_val,
                                                std::vector<Vector3D>& grad,
                                                MsqError &err )
{
  bool b;
  grad.resize( pd.num_free_vertices() );
  
    // Fast path for single-free-vertex patch
  if (pd.num_free_vertices() == 1) {
    const EvalType sub_type = (eval_type == CALCULATE) ? CALCULATE : TEMPORARY;
    b = compute_subpatch_numerical_gradient( eval_type, sub_type, pd, OF_val, grad[0], err );
    return !MSQ_CHKERR(err) && b;
  }
  
  ObjectiveFunction* of = this;
  std::auto_ptr<ObjectiveFunction> deleter;
  if (eval_type == CALCULATE) {
    of->clear();
    b = of->evaluate( ACCUMULATE, pd, OF_val, OF_FREE_EVALS_ONLY, err );
    if (err) { // OF doesn't support BCD type evals, try slow method
      err.clear();
      of->clear();
      b = compute_patch_numerical_gradient( CALCULATE, CALCULATE, pd, OF_val, grad, err );
      return !MSQ_CHKERR(err) && b;
    }
    else if (!b)
      return b;
  } 
  else {
    b = this->evaluate( eval_type, pd, OF_val, OF_FREE_EVALS_ONLY, err );
    if (MSQ_CHKERR(err) || !b)
      return false;
    of = this->clone();
    deleter = std::auto_ptr<ObjectiveFunction>(of);
  }

    // Determine number of layers of adjacent elements based on metric type.
  unsigned layers = min_patch_layers();
  
    // Create a subpatch for each free vertex and use it to evaluate the
    // gradient for that vertex.
  double flocal;
  PatchData subpatch;
  for (size_t i = 0; i < pd.num_free_vertices(); ++i)
  {
    pd.get_subpatch( i, layers, subpatch, err ); MSQ_ERRZERO(err);
    b = of->compute_subpatch_numerical_gradient( SAVE, TEMPORARY, subpatch, flocal, grad[i], err );
    if (MSQ_CHKERR(err) || !b) {
      of->clear();
      return false;
    }
  }
  
  of->clear();
  return true;
}

void LinearMappingFunctionTest::test_deriv_fail( MappingFunction3D& mf, unsigned subdim )
{
    // make sure it fails if called
  MsqError err;
  MsqVector<3> coeff[100];
  size_t verts[100], num_coeff;
  mf.derivatives( Sample(subdim, 0), NodeSet(), verts, coeff, num_coeff, err );
  CPPUNIT_ASSERT(err);
  err.clear();
}

void LinearMappingFunctionTest::test_coeff_fail( MappingFunction& mf, unsigned subdim )
{
    // make sure it fails if called
  MsqError err;
  double coeff[100];
  size_t num_coeff, indices[100];
  mf.coefficients( Sample(subdim, 0), NodeSet(), coeff, indices, num_coeff, err );
  CPPUNIT_ASSERT(err);
  err.clear();
}  

void LinearMappingFunctionTest::do_coeff_test( MappingFunction& mf, 
                                               unsigned subdim,
                                               map_func mf2,
                                               unsigned count,
                                               double* xi )
{
    // make sure it fails if passed a nonlinear element
  MsqError err;
  double coeff[100];
  size_t indices[100];
  size_t num_coeff = 100;
  NodeSet tmp_set;
  tmp_set.set_mid_edge_node(1);
  mf.coefficients( Sample(0, 1), tmp_set, coeff, indices, num_coeff, err );
  CPPUNIT_ASSERT(err);
  err.clear();
  
    // get number of vertices in element
  const unsigned n = TopologyInfo::corners( mf.element_topology() );
  const unsigned d = TopologyInfo::dimension( mf.element_topology() );
  
    // compare coefficients at each location
  vector<double> comp(n);
  for (unsigned i = 0; i < count; ++i)
  {
    num_coeff = 101;
    mf.coefficients( Sample(subdim, i), NodeSet(), coeff, indices, num_coeff, err );
    CPPUNIT_ASSERT(!err);
    
    mf2( xi, &comp[0] );
    string xi_str;
    for (unsigned j = 0; j < d; ++j) {
      xi_str += !j ? "(" : ", ";
      xi_str += dtostr(xi[j]);
    }
    xi_str += ")";
    xi += d;
    
    for (unsigned j = 0; j < n; ++j)
    {
      double mf_val = 0.0;
      size_t idx = std::find( indices, indices+num_coeff, j ) - indices;
      if (idx < num_coeff)
	mf_val = coeff[idx];

      CppUnit::Message message( "Coefficients do not match." );
      message.addDetail( string("Entity:             ") + itostr( i ) );
      message.addDetail( string("Coefficient number: ") + itostr( j ) );
      message.addDetail( string("Xi:             ") + xi_str );
      message.addDetail( string("Expected value: ") + dtostr( comp[j] ) );
      message.addDetail( string("Actual value:   ") + dtostr( mf_val ) );
      ASSERT_MESSAGE( message, fabs(comp[j]-mf_val) < DBL_EPSILON );
    }
  }
}

void ObjectiveFunctionTests::test_clone( ObjectiveFunctionTemplate* of )
{
  const double some_vals[] = { 1, 2, 3, 4, 5, 6, 0.5 };
  const unsigned num_vals = sizeof(some_vals)/sizeof(some_vals[0]);
  OFTestQM metric( some_vals, num_vals );
  of->set_quality_metric(&metric);
  
    // test that we get the same value from both
  auto_ptr<ObjectiveFunction> of2( of->clone() );
  double exp_val, val;
  exp_val = evaluate_internal( ObjectiveFunction::CALCULATE, EVAL, of );
  val = evaluate_internal( ObjectiveFunction::CALCULATE, EVAL, of2.get() );
  CPPUNIT_ASSERT_DOUBLES_EQUAL( exp_val, val, 1e-12 );
  
    // check if OF supports BCD -- if not then done
  MsqError err;
  of->evaluate( ObjectiveFunction::UPDATE, patch(), val, false, err );
  if (err) {
    err.clear();
    return;
  }
  
    // build up some saved state in the objective function
  of->clear();
  const double vals1[] = { 1.0, 3.0, 4.0, 8.0 };
  const size_t vals1_len = sizeof(vals1)/sizeof(vals1[0]);
  const double vals2[] = { 21.5, 11.1, 30.0, 0.5 };
  const size_t vals2_len = sizeof(vals2)/sizeof(vals2[0]);
  metric.set_values( vals1, vals1_len );
  evaluate_internal( ObjectiveFunction::SAVE, EVAL, of );
  metric.append_values( vals2, vals2_len );
  evaluate_internal( ObjectiveFunction::ACCUMULATE, EVAL, of );
  
    // check that clone has same accumulated data
  of2 = auto_ptr<ObjectiveFunction>(of->clone());
  metric.set_values( some_vals, num_vals );
  exp_val = evaluate_internal( ObjectiveFunction::UPDATE, EVAL, of );
  val = evaluate_internal( ObjectiveFunction::UPDATE, EVAL, of2.get() );
  CPPUNIT_ASSERT_DOUBLES_EQUAL( exp_val, val, 1e-12 );
}

//.........这里部分代码省略.........
      //get the gradient norms
    if (totalFlag & (GRADIENT_INF_NORM_ABSOLUTE|GRADIENT_INF_NORM_RELATIVE))
    {
      currentGradInfNorm = initialGradInfNorm = Linf(mGrad);
      MSQ_DBGOUT_P0_ONLY(debugLevel) << par_string() << "  o Initial gradient Inf norm: " << " "
                                     << RPM(initialGradInfNorm) << std::endl;
    }  
      
    if (totalFlag & (GRADIENT_L2_NORM_ABSOLUTE|GRADIENT_L2_NORM_RELATIVE))
    {
      currentGradL2NormSquared = initialGradL2NormSquared = length_squared(mGrad);
      MSQ_DBGOUT_P0_ONLY(debugLevel) << par_string() << "  o Initial gradient L2 norm: " << " "
                                     << RPM(std::sqrt(initialGradL2NormSquared)) << std::endl;
    }  

      //the OFvalue comes for free, so save it
    previousOFValue=currentOFValue;
    initialOFValue=currentOFValue;
  }
  //find the initial objective function value if needed and not already
  //computed.  If we needed the gradient, we have the OF value for free.
  // Also, if possible, get initial OF value if writing plot file.  Solvers
  // often supply the OF value for subsequent iterations so by calculating
  // the initial value we can generate OF value plots.
  else if ((totalFlag & OF_FLAGS) || 
           (plotFile.is_open() && pd.num_free_vertices() && obj_eval.have_objective_function()))
  {
      //ensure the obj_ptr is not null
    if(!obj_eval.have_objective_function()){
      MSQ_SETERR(err)("Error termination criteria set which uses objective "
                      "functions, but no objective function is available.",
                      MsqError::INVALID_STATE);
      return;
    }
    
    bool b = obj_eval.evaluate(pd, currentOFValue, err); MSQ_ERRRTN(err);
    if (!b){
      MSQ_SETERR(err)("Initial patch is invalid for evaluation.",MsqError::INVALID_STATE);
      return;
    }
      //std::cout<<"\nReseting initial of value = "<<initialOFValue;
    previousOFValue=currentOFValue;
    initialOFValue=currentOFValue;
  }
  
  if (totalFlag & (GRAD_FLAGS|OF_FLAGS))
    MSQ_DBGOUT_P0_ONLY(debugLevel) << par_string() << "  o Initial OF value: " << " " << RPM(initialOFValue) << std::endl;
  
    // Store current vertex locations now, because we'll
    // need them later to compare the current movement with.
  if (totalFlag & VERTEX_MOVEMENT_RELATIVE)
  {
    if (initialVerticesMemento)
    {
      pd.recreate_vertices_memento( initialVerticesMemento, err );
    }
    else
    {
      initialVerticesMemento = pd.create_vertices_memento( err );
    }
    MSQ_ERRRTN(err);
    maxSquaredInitialMovement = DBL_MAX;
  }
  else {
    maxSquaredInitialMovement = 0;
  }
  
  if (terminationCriterionFlag & UNTANGLED_MESH) {
    globalInvertedCount = count_inverted( pd, err );
    //if (innerOuterType==TYPE_OUTER) MSQ_DBGOUT_P0_ONLY(debugLevel) << par_string() << "  o Num Inverted: " << " " << globalInvertedCount << std::endl;
    patchInvertedCount = 0;
    MSQ_ERRRTN(err);
  }

  if (timeStepFileType) {
      // If didn't already calculate gradient abive, calculate it now.
    if (!(totalFlag & GRAD_FLAGS)) {
      mGrad.resize( pd.num_free_vertices() );
      obj_eval.evaluate(pd, currentOFValue, mGrad, err);
      err.clear();
    }
    write_timestep( pd, mGrad.empty() ? 0 : arrptr(mGrad), err);
  }
    
  if (plotFile.is_open()) {
      // two newlines so GNU plot knows that we are starting a new data set
    plotFile << std::endl << std::endl;
      // write column headings as comment in data file
    plotFile << "#Iter\tCPU\tObjFunc\tGradL2\tGradInf\tMovement\tInverted" << std::endl;
      // write initial values
    plotFile << 0 
     << '\t' << mTimer.since_birth() 
     << '\t' << initialOFValue 
     << '\t' << std::sqrt( currentGradL2NormSquared ) 
     << '\t' << currentGradInfNorm 
     << '\t' << 0.0
     << '\t' << globalInvertedCount
     << std::endl;
  }
}

void SteepestDescent::optimize_vertex_positions(PatchData &pd, 
                                                MsqError &err)
{
  MSQ_FUNCTION_TIMER( "SteepestDescent::optimize_vertex_positions" );

  const int SEARCH_MAX = 100;
  const double c1 = 1e-4;
  //std::vector<Vector3D> unprojected(pd.num_free_vertices()); 
  std::vector<Vector3D> gradient(pd.num_free_vertices()); 
  bool feasible=true;//bool for OF values
  double min_edge_len, max_edge_len;
  double step_size=0, original_value=0, new_value=0;
  double norm_squared=0;
  PatchDataVerticesMemento* pd_previous_coords;
  TerminationCriterion* term_crit=get_inner_termination_criterion();
  OFEvaluator& obj_func = get_objective_function_evaluator();
  
    // get vertex memento so we can restore vertex coordinates for bad steps.
  pd_previous_coords = pd.create_vertices_memento( err ); MSQ_ERRRTN(err);
    // use auto_ptr to automatically delete memento when we exit this function
  std::auto_ptr<PatchDataVerticesMemento> memento_deleter( pd_previous_coords );

    // Evaluate objective function.
    //
    // Always use 'update' version when beginning optimization so that
    // if doing block coordinate descent the OF code knows the set of
    // vertices we are modifying during the optimziation (the subset
    // of the mesh contained in the current patch.)  This has to be
    // done up-front because typically an OF will just store the portion
    // of the OF value (e.g. the numeric contribution to the sum for an
    // averaging OF) for the initial patch.
  feasible = obj_func.update( pd, original_value, gradient, err ); MSQ_ERRRTN(err);
    // calculate gradient dotted with itself
  norm_squared = length_squared( gradient );
  
    //set an error if initial patch is invalid.
  if(!feasible){
    MSQ_SETERR(err)("SteepestDescent passed invalid initial patch.",
                    MsqError::INVALID_ARG);
    return;
  }

    // use edge length as an initial guess for for step size
  pd.get_minmax_edge_length( min_edge_len, max_edge_len );
  //step_size = max_edge_len / std::sqrt(norm_squared);
  //if (!finite(step_size))  // zero-length gradient
  //  return;
//  if (norm_squared < DBL_EPSILON)
//    return;
  if (norm_squared >= DBL_EPSILON)
    step_size = max_edge_len / std::sqrt(norm_squared) * pd.num_free_vertices();

    // The steepest descent loop...
    // We loop until the user-specified termination criteria are met.
  while (!term_crit->terminate()) {
    MSQ_DBGOUT(3) << "Iteration " << term_crit->get_iteration_count() << std::endl;
    MSQ_DBGOUT(3) << "  o  original_value: " << original_value << std::endl;
    MSQ_DBGOUT(3) << "  o  grad norm suqared: " << norm_squared << std::endl;

      // Save current vertex coords so that they can be restored if
      // the step was bad.
    pd.recreate_vertices_memento( pd_previous_coords, err ); MSQ_ERRRTN(err);

      // Reduce step size until it satisfies Armijo condition
    int counter = 0;
    for (;;) {
      if (++counter > SEARCH_MAX || step_size < DBL_EPSILON) {
        MSQ_DBGOUT(3) << "    o  No valid step found.  Giving Up." << std::endl;
        return;
      }
      
      // Move vertices to new positions.
      // Note: step direction is -gradient so we pass +gradient and 
      //       -step_size to achieve the same thing.
      pd.move_free_vertices_constrained( arrptr(gradient), gradient.size(), -step_size, err ); MSQ_ERRRTN(err);
      // Evaluate objective function for new vertices.  We call the
      // 'evaluate' form here because we aren't sure yet if we want to
      // keep these vertices.  Until we call 'update', we have the option
      // of reverting a block coordinate decent objective function's state
      // to that of the initial vertex coordinates.  However, for block
      // coordinate decent to work correctly, we will need to call an
      // 'update' form if we decide to keep the new vertex coordinates.
      feasible = obj_func.evaluate( pd, new_value, err ); 
      if (err.error_code() == err.BARRIER_VIOLATED) 
        err.clear();  // barrier violated does not represent an actual error here
      MSQ_ERRRTN(err);
      MSQ_DBGOUT(3) << "    o  step_size: " << step_size << std::endl;
      MSQ_DBGOUT(3) << "    o  new_value: " << new_value << std::endl;

      if (!feasible) {
        // OF value is invalid, decrease step_size a lot
        step_size *= 0.2;
      }
      else if (new_value > original_value - c1 * step_size * norm_squared) {
        // Armijo condition not met.
        step_size *= 0.5;
      }
      else {
        // Armijo condition met, stop
        break;
//.........这里部分代码省略.........

//.........这里部分代码省略.........
            norm_dp1 = norm_d + 2.0*alpha*dMp + alpha*alpha*norm_p;
            if (norm_dp1 >= radius) {
                alpha = (sqrt(dMp*dMp+norm_p*(radius-norm_d))-dMp)/norm_p;
                plus_eq_scaled( d, alpha, p, nn );
                break;
            }

            plus_eq_scaled( d, alpha, p, nn );
            plus_eq_scaled( r, alpha, w, nn );
            norm_r = length_squared(r, nn);

            apply_preconditioner( z, r, err);
            MSQ_ERRRTN(err); //prec->apply(z, r, prec, mesh);

            rzm1 = rz;
            rz = inner(r, z, nn);
            beta = rz / rzm1;
            times_eq_minus( p, beta, z, nn );

            dMp = beta*(dMp + alpha*norm_p);
            norm_p = rz + beta*beta*norm_p;
            norm_d = norm_dp1;
        }

#ifdef DO_STEEP_DESC
        if (norm_d <= tr_num_tol) {
            norm_g = length(arrptr(mGrad), nn);
            double ll = 1.0;
            if (norm_g < tr_num_tol)
                break;
            if (norm_g > radius)
                ll = radius / nurm_g;
            for (int i = 0; i < nn; ++i)
                d[i] = ll * mGrad[i];
        }
#endif

        alpha = inner( arrptr(mGrad), d, nn ); // inner(mesh->g, d, nn);

        memset(p, 0, 3*sizeof(double)*nn);
        //matmul(p, mHess, d); //matmul(p, mesh, d);
        mHess.product( p, d );
        beta = 0.5*inner(p, d, nn);
        kappa = alpha + beta;

        /* Put the new point into the locations */
        pd.move_free_vertices_constrained( d, nn, 1.0, err );
        MSQ_ERRRTN(err);

        valid = func.evaluate( pd, objn, err );
        if (err.error_code() == err.BARRIER_VIOLATED)
            err.clear();  // barrier violated does not represent an actual error here
        MSQ_ERRRTN(err);

        if (!valid) {
            /* Function not defined at trial point */
            radius *= tr_decr_undef;
            pd.set_to_vertices_memento( mMemento, err );
            MSQ_ERRRTN(err);
            continue;
        }


        if ((fabs(kappa) <= tr_num_tol) && (fabs(objn - obj) <= tr_num_tol)) {
            kappa = 1;
        }
        else {
            kappa = (objn - obj) / kappa;
        }

        if (kappa < eta_1) {
            /* Iterate is unacceptable */
            radius *= tr_decr_def;
            pd.set_to_vertices_memento( mMemento, err );
            MSQ_ERRRTN(err);
            continue;
        }

        /* Iterate is acceptable */
        if (kappa >= eta_2) {
            /* Iterate is a very good step, increase radius */
            radius *= tr_incr;
            if (radius > 1e20) {
                radius = 1e20;
            }
        }

        func.update( pd, obj, mGrad, mHess, err );
        compute_preconditioner( err );
        MSQ_ERRRTN(err);
        pd.recreate_vertices_memento( mMemento, err );
        MSQ_ERRRTN(err);

        // checks stopping criterion
        term.accumulate_patch( pd, err );
        MSQ_ERRRTN(err);
        term.accumulate_inner( pd, objn, arrptr(mGrad), err );
        MSQ_ERRRTN(err);
    }
}

void LinearMappingFunctionTest::do_deriv_test( MappingFunction2D& mf, 
                                               unsigned subdim,
                                               map_func mf2,
                                               unsigned count,
                                               double* xi )
{
    // make sure it fails if passed a nonlinear element
  MsqError err;
  MsqVector<2> derivs[100];
  size_t verts[100], num_vtx = 37;
  NodeSet tmp_set;
  tmp_set.set_mid_edge_node(1);
  mf.derivatives( Sample(subdim, 0), tmp_set, verts, derivs, num_vtx, err );
  CPPUNIT_ASSERT(err);
  err.clear();
  
    // get number of vertices in element
  const unsigned n = TopologyInfo::corners( mf.element_topology() );
  
    // compare coefficients at each location
  vector<double> comp(2*n);
  for (unsigned i = 0; i < count; ++i)
  {
    num_vtx = 33;
    mf.derivatives( Sample(subdim, i), NodeSet(), verts, derivs, num_vtx, err );
    CPPUNIT_ASSERT(!err);
    CPPUNIT_ASSERT( num_vtx > 0 );
    CPPUNIT_ASSERT( num_vtx <= n );
    
    mf2( xi, &comp[0] );
    string xi_str;
    for (unsigned j = 0; j < 2; ++j) {
      xi_str += !j ? "(" : ", ";
      xi_str += dtostr(xi[j]);
    }
    xi_str += ")";
    xi += 2;
    
    for (unsigned j = 0; j < num_vtx; ++j)
    {
      bool all_zero = true;
      for (unsigned k = 0; k < 2; ++k)
      {
        CppUnit::Message message( "Coefficient derivatives do not match." );
        message.addDetail( string("Entity:             ") + itostr( i ) );
        message.addDetail( string("Coefficient number: ") + itostr( j ) );
        message.addDetail( string("Xi:             ") + xi_str );
        message.addDetail( string("Axis:           ") + itostr( k ) );
        message.addDetail( string("Expected value: ") + dtostr( comp[2*verts[j]+k] ) );
        message.addDetail( string("Actual value:   ") + dtostr( derivs[j][k] ) );
        ASSERT_MESSAGE( message, fabs(comp[2*verts[j]+k]-derivs[j][k]) < DBL_EPSILON );
        if (fabs(derivs[j][k]) > DBL_EPSILON)
          all_zero = false;
      }

        // if vertex has all zero values, it shouldn't have been in the
        // vertex list at all, as the Jacobian will not depend on that vertex.
      CPPUNIT_ASSERT( !all_zero );
    }
    
      // If any vertex is not in the list, then its values must be zero.
    sort( verts, verts + num_vtx );
    for (unsigned j = 0; j < num_vtx; ++j) {
      if (!binary_search( verts, verts+num_vtx, j )) {
        for (unsigned k = 0; k < 2; ++k)
        {
          CppUnit::Message message( "Missing coefficient derivatives." );
          message.addDetail( string("Entity:              ") + itostr( i ) );
          message.addDetail( string("Coefficient number:  ") + itostr( j ) );
          message.addDetail( string("Axis:                ") + itostr( k ) );
          message.addDetail( string("Expected derivative: ") + dtostr( comp[2*j+k] ) );
          ASSERT_MESSAGE( message, fabs(comp[2*j+k]) < DBL_EPSILON );
        }
      }
    }
  }
}

int uwt( bool skip,
         UntangleWrapper::UntangleMetric metric,
         const char* input_file_base,
         int expected,
         bool flip_domain )
{
  if (skip)
    return 0;

  if (!brief_output)
    std::cout << std::endl << "**********************************************" << std::endl;
  std::cout << "Running \"" << input_file_base << "\" for " << tostr(metric) << std::endl;
  if (!brief_output)
    std::cout << "**********************************************" << std::endl << std::endl;

    // get mesh
  MsqError err;
  MeshImpl mesh;
  std::string input_file( VTK_2D_DIR );
  input_file += input_file_base;
  mesh.read_vtk( input_file.c_str(), err );
  if (err) {
    std::cerr << err << std::endl;
    std::cerr << "ERROR: " << input_file << " : failed to read file" << std::endl;
    return 1;
  }
    // get domain
  std::vector<Mesh::VertexHandle> verts;
  mesh.get_all_vertices( verts, err );
  if (err || verts.empty()) abort();
  MsqVertex coords;
  mesh.vertices_get_coordinates( arrptr(verts), &coords, 1, err );
  if (err) abort();
  Vector3D norm(0,0,flip_domain ? -1 : 1);
  PlanarDomain domain( norm, coords );
    // run wrapper
  UntangleWrapper wrapper( metric );
  wrapper.set_vertex_movement_limit_factor( 0.005 );
  double constant = (metric == UntangleWrapper::BETA) ? beta : mu_sigma;
  if (constant > 0)
    wrapper.set_metric_constant( constant );
  if (brief_output)
    wrapper.quality_assessor().disable_printing_results();
  MeshDomainAssoc mesh_and_domain = MeshDomainAssoc(&mesh, &domain);
  wrapper.run_instructions( &mesh_and_domain, err );
  if (err) {
    std::cerr << err << std::endl;
    std::cerr << "ERROR: optimization failed" << std::endl;
    return 1;
  }
    // write output file
  if (write_output) {
    std::string result_file(tostr(metric));
    result_file += "-";
    result_file += input_file_base;
    mesh.write_vtk( result_file.c_str(), err );
    if (err) {
      std::cerr << err << std::endl;
      std::cerr << "ERROR: " << result_file << " : failed to write file" << std::endl;
      err.clear();
    }
    else {
      std::cerr << "Wrote file: " << result_file << std::endl;
    }
  }
  
    // test number of inverted elements
  int count, junk;
  wrapper.quality_assessor().get_inverted_element_count( count, junk, err );
  if (err) abort();
  if (count < expected) {
    std::cout << "WARNING: expected " << expected 
              << " inverted elements but finished with only " 
              << count << std::endl
              << "Test needs to be updated?" << std::endl << std::endl;
    return 0;
  }
  else if (count == expected) {
    std::cout << "Completed with " << count << " inverted elements remaining" 
              << std::endl << std::endl;
    return 0;
  }
  else {
    std::cerr << "ERROR: expected " << expected 
              << " inverted elements but finished with " 
              << count << std::endl << std::endl;
    return 1;
  }
}

void Mesquite::PlanarDomain::fit_vertices( Mesh* mesh, MsqError& err, double epsilon )
{
    // Our goal here is to consider only the boundary (fixed) vertices
    // when calculating the plane.  If for some reason the user wants
    // to snap a not-quite-planar mesh to a plane during optimization, 
    // if possible we want to start with the plane containing the fixed
    // vertices, as those won't get snapped.  If no vertices are fixed,
    // then treat them all as fixed for the purpose calculating the plane
    // (consider them all.)

  std::vector<Mesh::VertexHandle> verts, fixed;
  mesh->get_all_vertices( verts, err ); MSQ_ERRRTN(err);
  DomainUtil::get_fixed_vertices( mesh, arrptr(verts), verts.size(), fixed, err ); MSQ_ERRRTN(err);
  
  bool do_all_verts = true;
  if (fixed.size() > 2) {
    do_all_verts = false;
    fit_vertices( mesh, arrptr(fixed), fixed.size(), err, epsilon );
    
      // if we failed with only the fixed vertices, try again with all of the 
      // vertices
    if (err) {
      err.clear();
      do_all_verts = true;
    }
  }
  
  if (do_all_verts) {
    fit_vertices( mesh, arrptr(verts), verts.size(), err, epsilon );
    MSQ_ERRRTN(err);
  }
  
    // now count inverted elements
  size_t inverted_count = 0, total_count = 0;
  std::vector<Mesh::ElementHandle> elems;
  std::vector<size_t> junk;
  std::vector<MsqVertex> coords;
  mesh->get_all_elements( elems, err ); MSQ_ERRRTN(err);
  for (size_t i = 0; i < elems.size(); ++i) {

    EntityTopology type;
    mesh->elements_get_topologies( &elems[i], &type, 1, err );
    if (TopologyInfo::dimension(type) != 2)
      continue;
    
    verts.clear();
    mesh->elements_get_attached_vertices( arrptr(elems), 1, verts, junk, err ); MSQ_ERRRTN(err);
    if (verts.size() < 3)
      continue;
    
    coords.resize( verts.size() );
    mesh->vertices_get_coordinates( arrptr(verts), arrptr(coords), 3, err ); MSQ_ERRRTN(err);
    Vector3D n = (coords[1] - coords[0]) * (coords[2] - coords[0]);
    ++total_count;
    if (n % mNormal < 0.0)
      ++inverted_count;
  }
  
    // if most elements are inverted, flip normal
  if (2*inverted_count > total_count)
    this->flip();
}