C++ MsqError类代码示例

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;
}

void CompositeOFTest::test_eval_fails( ObjectiveFunction& OF )
{
    MsqError err;
    double value;
    OF.evaluate( ObjectiveFunction::CALCULATE, patch(), value, false, err );
    CPPUNIT_ASSERT_EQUAL(MsqError::INTERNAL_ERROR, err.error_code());
}

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;
}

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;
}

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 );
    }
  }
}

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 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 );
}

/*!Reset function using using a PatchData object.  This function is
  called for the inner-stopping criterion directly from the
  loop over mesh function in VertexMover.  For outer criterion,
  it is called from the reset function which takes a MeshSet object.
  This function prepares the object to be used by setting the initial
  values of some of the data members.  As examples, if needed, it resets
  the cpu timer to zero, the iteration counter to zero, and the
  initial and previous objective function values to the current
  objective function value for this patch.
  The return value for this function is similar to that of terminate().
  The function returns false if the checked criteria have not been
  satisfied, and true if they have been.  reset() only checks the
  GRADIENT_INF_NORM_ABSOLUTE, GRADIENT_L2_NORM_ABSOLUTE, and the
  QUALITY_IMPROVEMENT_ABSOLUTE criteria.  Checking these criteria
  allows the QualityImprover to skip the entire optimization if
  the initial mesh satisfies the appropriate conditions.
 */
void TerminationCriterion::reset_inner(PatchData &pd, OFEvaluator& obj_eval,
                                    MsqError &err)
{
  const unsigned long totalFlag = terminationCriterionFlag | cullingMethodFlag;
  
    // clear flag for BOUNDED_VERTEX_MOVEMENT
  vertexMovementExceedsBound = 0;
  
    // Use -1 to denote that this isn't initialized yet.
    // As all valid values must be >= 0.0, a negative
    // value indicates that it is uninitialized and is
    // always less than any valid value.
  maxSquaredMovement = -1;
  
    // Clear the iteration count.
  iterationCounter = 0;
  
    //reset the inner timer if needed
  if(totalFlag & CPU_TIME){
    mTimer.reset();
  }
   
    //GRADIENT
  currentGradInfNorm = initialGradInfNorm = 0.0;
  currentGradL2NormSquared = initialGradL2NormSquared = 0.0;
  if(totalFlag & GRAD_FLAGS)
  {
    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;
    } 
    int num_vertices=pd.num_free_vertices();
    mGrad.resize( num_vertices );

      //get gradient and make sure it is valid
    bool b = obj_eval.evaluate(pd, currentOFValue, mGrad, err); MSQ_ERRRTN(err);
    if (!b) {
      MSQ_SETERR(err)("Initial patch is invalid for gradient computation.", 
                      MsqError::INVALID_STATE);
      return;
    } 

      //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;
//.........这里部分代码省略.........

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;
//.........这里部分代码省略.........

void TrustRegion::optimize_vertex_positions( PatchData& pd, MsqError& err )
{
    TerminationCriterion& term = *get_inner_termination_criterion();
    OFEvaluator& func = get_objective_function_evaluator();

    const double cg_tol = 1e-2;
    const double eta_1  = 0.01;
    const double eta_2  = 0.90;
    const double tr_incr = 10;
    const double tr_decr_def = 0.25;
    const double tr_decr_undef = 0.25;
    const double tr_num_tol = 1e-6;
    const int max_cg_iter = 10000;

    double radius = 1000;		/* delta*delta */

    const int nn = pd.num_free_vertices();
    wVect.resize(nn);
    Vector3D* w = arrptr(wVect);
    zVect.resize(nn);
    Vector3D* z = arrptr(zVect);
    dVect.resize(nn);
    Vector3D* d = arrptr(dVect);
    pVect.resize(nn);
    Vector3D* p = arrptr(pVect);
    rVect.resize(nn);
    Vector3D* r = arrptr(rVect);

    double norm_r, norm_g;
    double alpha, beta, kappa;
    double rz, rzm1;
    double dMp, norm_d, norm_dp1, norm_p;
    double obj, objn;

    int cg_iter;
    bool valid;

    mHess.initialize( pd, err );  //hMesh(mesh);
    valid = func.update( pd, obj, mGrad, mHess, err );
    MSQ_ERRRTN(err);
    if (!valid) {
        MSQ_SETERR(err)("Initial objective function is not valid", MsqError::INVALID_MESH);
        return;
    }
    compute_preconditioner( err );
    MSQ_ERRRTN(err);
    pd.recreate_vertices_memento( mMemento, err );
    MSQ_ERRRTN(err);

    while (!term.terminate() && (radius > 1e-20)) {

        norm_r = length_squared(arrptr(mGrad), nn);
        norm_g = sqrt(norm_r);

        memset(d, 0, 3*sizeof(double)*nn);
        memcpy(r, arrptr(mGrad), nn*sizeof(Vector3D)); //memcpy(r, mesh->g, 3*sizeof(double)*nn);
        norm_g *= cg_tol;

        apply_preconditioner( z, r, err);
        MSQ_ERRRTN(err); //prec->apply(z, r, prec, mesh);
        negate(p, z, nn);
        rz = inner(r, z, nn);

        dMp    = 0;
        norm_p = rz;
        norm_d = 0;

        cg_iter = 0;
        while ((sqrt(norm_r) > norm_g) &&
#ifdef DO_STEEP_DESC
                (norm_d > tr_num_tol) &&
#endif
                (cg_iter < max_cg_iter))
        {
            ++cg_iter;

            memset(w, 0, 3*sizeof(double)*nn);
            //matmul(w, mHess, p); //matmul(w, mesh, p);
            mHess.product( w, p );

            kappa = inner(p, w, nn);
            if (kappa <= 0.0) {
                alpha = (sqrt(dMp*dMp+norm_p*(radius-norm_d))-dMp)/norm_p;
                plus_eq_scaled( d, alpha, p, nn );
                break;
            }

            alpha = rz / kappa;

            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);

//.........这里部分代码省略.........

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;
  }
}