MOAB: Mesh Oriented datABase  (version 5.4.1)
NonSmoothDescent.cpp
Go to the documentation of this file.
00001 /* *****************************************************************
00002     MESQUITE -- The Mesh Quality Improvement Toolkit
00003 
00004     Copyright 2004 Sandia Corporation and Argonne National
00005     Laboratory.  Under the terms of Contract DE-AC04-94AL85000
00006     with Sandia Corporation, the U.S. Government retains certain
00007     rights in this software.
00008 
00009     This library is free software; you can redistribute it and/or
00010     modify it under the terms of the GNU Lesser General Public
00011     License as published by the Free Software Foundation; either
00012     version 2.1 of the License, or (at your option) any later version.
00013 
00014     This library is distributed in the hope that it will be useful,
00015     but WITHOUT ANY WARRANTY; without even the implied warranty of
00016     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00017     Lesser General Public License for more details.
00018 
00019     You should have received a copy of the GNU Lesser General Public License
00020     (lgpl.txt) along with this library; if not, write to the Free Software
00021     Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00022 
00023     [email protected], [email protected], [email protected],
00024     [email protected], [email protected], [email protected]
00025 
00026   ***************************************************************** */
00027 // -*- Mode : c++; tab-width: 3; c-tab-always-indent: t; indent-tabs-mode: nil; c-basic-offset: 3
00028 // -*-
00029 
00030 /*!  \File NonSmoothDescent.cpp \brief
00031 
00032   Implements the NonSmoothDescent class member functions.
00033 
00034   \author Lori Freitag
00035   \date 2002-07-20 */
00036 
00037 #include <cstdlib>
00038 #include <cstdio>
00039 #include "NonSmoothDescent.hpp"
00040 #include "MsqTimer.hpp"
00041 
00042 #undef NUMERICAL_GRADIENT
00043 
00044 namespace MBMesquite
00045 {
00046 
00047 const double EPSILON       = 1e-16;
00048 const int MSQ_MAX_OPT_ITER = 20;
00049 
00050 enum Rotate
00051 {
00052     COUNTERCLOCKWISE = 1,
00053     CLOCKWISE        = 0
00054 };
00055 
00056 NonSmoothDescent::NonSmoothDescent( QualityMetric* qm ) : currentQM( qm )
00057 {
00058 
00059     MSQ_DBGOUT( 1 ) << "- Executed NonSmoothDescent::NonSmoothDescent()\n";
00060 }
00061 
00062 std::string NonSmoothDescent::get_name() const
00063 {
00064     return "NonSmoothDescent";
00065 }
00066 
00067 PatchSet* NonSmoothDescent::get_patch_set()
00068 {
00069     return &patchSet;
00070 }
00071 
00072 void NonSmoothDescent::initialize( PatchData& /*pd*/, MsqError& /*err*/ )
00073 {
00074     minStepSize = 1e-6;
00075     MSQ_DBGOUT( 1 ) << "- Executed NonSmoothDescent::initialize()\n";
00076 }
00077 
00078 void NonSmoothDescent::initialize_mesh_iteration( PatchData& /*pd*/, MsqError& /*err*/ ) {}
00079 void NonSmoothDescent::optimize_vertex_positions( PatchData& pd, MsqError& err )
00080 {
00081     MSQ_FUNCTION_TIMER( "NonSmoothDescent" );
00082 
00083     //  cout << "- Executing NonSmoothDescent::optimize_node_positions()\n";
00084     /* perform the min max smoothing algorithm */
00085     MSQ_PRINT( 2 )( "\nInitializing the patch iteration\n" );
00086 
00087     MSQ_PRINT( 3 )( "Number of Vertices: %d\n", (int)pd.num_nodes() );
00088     MSQ_PRINT( 3 )( "Number of Elements: %d\n", (int)pd.num_elements() );
00089     // Michael: Note: is this a reliable way to get the dimension?
00090     // NOTE: Mesquite only does 3-dimensional (including surface) meshes.
00091     // mDimension = pd.get_mesh()->get_geometric_dimension(err); MSQ_ERRRTN(err);
00092     // MSQ_PRINT(3)("Spatial Dimension: %d\n",mDimension);
00093     MSQ_PRINT( 3 )( "Spatial Dimension: 3\n" );
00094 
00095     MSQ_PRINT( 3 )( "Num Free = %d\n", (int)pd.num_free_vertices() );
00096 
00097     MsqFreeVertexIndexIterator free_iter( pd, err );MSQ_ERRRTN( err );
00098     free_iter.reset();
00099     free_iter.next();
00100     freeVertexIndex = free_iter.value();
00101     MSQ_PRINT( 3 )( "Free Vertex Index = %lu\n", (unsigned long)freeVertexIndex );
00102 
00103     // TODO - need to switch to validity via metric evaluations should
00104     // be associated with the compute_function somehow
00105     /* check for an invalid mesh; if it's invalid return and ask the user
00106        to use untangle */
00107     if( !this->validity_check( pd, err ) )
00108     {
00109         MSQ_PRINT( 1 )( "ERROR: Invalid mesh\n" );
00110         MSQ_SETERR( err )
00111         ( "Invalid Mesh: Use untangle to create a valid "
00112           "triangulation",
00113           MsqError::INVALID_MESH );
00114         return;
00115     }
00116 
00117     /* initialize the optimization data up to numFunctionValues */
00118     this->init_opt( pd, err );MSQ_ERRRTN( err );
00119     this->init_max_step_length( pd, err );MSQ_ERRRTN( err );
00120     MSQ_PRINT( 3 )( "Done initializing optimization\n" );
00121 
00122     /* compute the initial function values */
00123     // TODO this should return a bool with the validity
00124     this->compute_function( &pd, originalFunction, err );MSQ_ERRRTN( err );
00125 
00126     // find the initial active set
00127     this->find_active_set( originalFunction, mActive );
00128 
00129     this->minmax_opt( pd, err );MSQ_ERRRTN( err );
00130 }
00131 
00132 void NonSmoothDescent::terminate_mesh_iteration( PatchData& /*pd*/, MsqError& /*err*/ ) {}
00133 
00134 void NonSmoothDescent::cleanup()
00135 {
00136     MSQ_DBGOUT( 1 ) << "- Executing NonSmoothDescent::cleanup()\n";
00137     MSQ_DBGOUT( 1 ) << "- Done with NonSmoothDescent::cleanup()\n";
00138 }
00139 
00140 void NonSmoothDescent::find_plane_points( Direction dir1,
00141                                           Direction dir2,
00142                                           const std::vector< Vector3D >& vec,
00143                                           Vector3D& pt1,
00144                                           Vector3D& pt2,
00145                                           Vector3D& /*pt3*/,
00146                                           Status& status,
00147                                           MsqError& )
00148 {
00149     int i;
00150     int num_min, num_max;
00151     Rotate rotate   = CLOCKWISE;
00152     int num_rotated = 0;
00153     double pt_1, pt_2;
00154     double min, inv_slope;
00155     double min_inv_slope = 0.;
00156     double max;
00157     double max_inv_slope    = 0;
00158     double inv_origin_slope = 0;
00159     const int num_vec       = vec.size();
00160     const int auto_ind_size = 50;
00161     int auto_ind[auto_ind_size];
00162     std::vector< int > heap_ind;
00163     int* ind;
00164     if( num_vec <= auto_ind_size )
00165         ind = auto_ind;
00166     else
00167     {
00168         heap_ind.resize( num_vec );
00169         ind = &heap_ind[0];
00170     }
00171 
00172     status = MSQ_CHECK_BOTTOM_UP;
00173     /* find the minimum points in dir1 starting at -1 */
00174     num_min = 0;
00175     ind[0]  = -1;
00176     ind[1]  = -1;
00177     ind[2]  = -1;
00178     min     = 1.0;
00179     for( i = 0; i < num_vec; i++ )
00180     {
00181         if( vec[i][dir1] < min )
00182         {
00183             min     = vec[i][dir1];
00184             ind[0]  = i;
00185             num_min = 1;
00186         }
00187         else if( fabs( vec[i][dir1] - min ) < EPSILON )
00188         {
00189             ind[num_min++] = i;
00190         }
00191     }
00192     if( min >= 0 ) status = MSQ_NO_EQUIL;
00193 
00194     if( status != MSQ_NO_EQUIL )
00195     {
00196         switch( num_min )
00197         {
00198             case 1: /* rotate to find the next point */
00199                 pt1  = vec[ind[0]];
00200                 pt_1 = pt1[dir1];
00201                 pt_2 = pt1[dir2];
00202                 if( pt1[dir2] <= 0 )
00203                 {
00204                     rotate        = COUNTERCLOCKWISE;
00205                     max_inv_slope = -HUGE_VAL;
00206                 }
00207                 if( pt1[dir2] > 0 )
00208                 {
00209                     rotate        = CLOCKWISE;
00210                     min_inv_slope = HUGE_VAL;
00211                 }
00212                 switch( rotate )
00213                 {
00214                     case COUNTERCLOCKWISE:
00215                         for( i = 0; i < num_vec; i++ )
00216                         {
00217                             if( i != ind[0] )
00218                             {
00219                                 inv_slope = ( vec[i][dir2] - pt_2 ) / ( vec[i][dir1] - pt_1 );
00220                                 if( ( inv_slope > max_inv_slope ) && ( fabs( inv_slope - max_inv_slope ) > EPSILON ) )
00221                                 {
00222                                     ind[1]        = i;
00223                                     max_inv_slope = inv_slope;
00224                                     num_rotated   = 1;
00225                                 }
00226                                 else if( fabs( inv_slope - max_inv_slope ) < EPSILON )
00227                                 {
00228                                     ind[2] = i;
00229                                     num_rotated++;
00230                                 }
00231                             }
00232                         }
00233                         break;
00234                     case CLOCKWISE:
00235                         for( i = 0; i < num_vec; i++ )
00236                         {
00237                             if( i != ind[0] )
00238                             {
00239                                 inv_slope = ( vec[i][dir2] - pt_2 ) / ( vec[i][dir1] - pt_1 );
00240                                 if( ( inv_slope < min_inv_slope ) && ( fabs( inv_slope - max_inv_slope ) > EPSILON ) )
00241                                 {
00242                                     ind[1]        = i;
00243                                     min_inv_slope = inv_slope;
00244                                     num_rotated   = 1;
00245                                 }
00246                                 else if( fabs( inv_slope - min_inv_slope ) < EPSILON )
00247                                 {
00248                                     ind[2] = i;
00249                                     num_rotated++;
00250                                 }
00251                             }
00252                         }
00253                 }
00254                 switch( num_rotated )
00255                 {
00256                     case 0:
00257                         MSQ_PRINT( 3 )( "No points in the rotation ... odd\n" );
00258                         status = MSQ_HULL_TEST_ERROR;
00259                         break;
00260                     case 1:
00261                         MSQ_PRINT( 3 )( "Found a line in the convex hull\n" );
00262                         pt2    = vec[ind[1]];
00263                         status = MSQ_TWO_PT_PLANE;
00264                         break;
00265                     default:
00266                         MSQ_PRINT( 3 )( "Found 2 or more points in the rotation\n" );
00267                         if( fabs( pt_1 ) > EPSILON ) inv_origin_slope = pt_2 / pt_1;
00268                         switch( rotate )
00269                         {
00270                             case COUNTERCLOCKWISE:
00271                                 if( inv_origin_slope >= max_inv_slope )
00272                                     status = MSQ_NO_EQUIL;
00273                                 else
00274                                     status = MSQ_CHECK_TOP_DOWN;
00275                                 break;
00276                             case CLOCKWISE:
00277                                 if( inv_origin_slope <= min_inv_slope )
00278                                     status = MSQ_NO_EQUIL;
00279                                 else
00280                                     status = MSQ_CHECK_TOP_DOWN;
00281                         }
00282                 }
00283                 break;
00284             case 2: /* use these two points to define the plane */
00285                 MSQ_PRINT( 3 )( "Found two minimum points to define the plane\n" );
00286                 pt1    = vec[ind[0]];
00287                 pt2    = vec[ind[1]];
00288                 status = MSQ_TWO_PT_PLANE;
00289                 break;
00290             default: /* check to see if all > 0 */
00291                 MSQ_PRINT( 3 )( "Found 3 or more points in min plane %f\n", min );
00292                 if( vec[ind[0]][dir1] >= 0 )
00293                     status = MSQ_NO_EQUIL;
00294                 else
00295                     status = MSQ_CHECK_TOP_DOWN;
00296         }
00297     }
00298 
00299     /***************************/
00300     /*  failed to find any information, checking top/down this coord*/
00301     /***************************/
00302 
00303     if( status == MSQ_CHECK_TOP_DOWN )
00304     {
00305         /* find the maximum points in dir1 starting at 1 */
00306         num_max = 0;
00307         ind[0]  = -1;
00308         ind[1]  = -1;
00309         ind[2]  = -1;
00310         max     = -1.0;
00311         for( i = 0; i < num_vec; i++ )
00312         {
00313             if( vec[i][dir1] > max )
00314             {
00315                 max     = vec[i][dir1];
00316                 ind[0]  = i;
00317                 num_max = 1;
00318             }
00319             else if( fabs( vec[i][dir1] - max ) < EPSILON )
00320             {
00321                 ind[num_max++] = i;
00322             }
00323         }
00324         if( max <= 0 ) status = MSQ_NO_EQUIL;
00325 
00326         if( status != MSQ_NO_EQUIL )
00327         {
00328             switch( num_max )
00329             {
00330                 case 1: /* rotate to find the next point */
00331                     pt1  = vec[ind[0]];
00332                     pt_1 = pt1[dir1];
00333                     pt_2 = pt1[dir2];
00334                     if( pt1[dir2] < 0 )
00335                     {
00336                         rotate        = CLOCKWISE;
00337                         min_inv_slope = HUGE_VAL;
00338                     }
00339                     if( pt1[dir2] >= 0 )
00340                     {
00341                         rotate        = COUNTERCLOCKWISE;
00342                         max_inv_slope = -HUGE_VAL;
00343                     }
00344                     switch( rotate )
00345                     {
00346                         case COUNTERCLOCKWISE:
00347                             for( i = 0; i < num_vec; i++ )
00348                             {
00349                                 if( i != ind[0] )
00350                                 {
00351                                     inv_slope = ( vec[i][dir2] - pt_2 ) / ( vec[i][dir1] - pt_1 );
00352                                     if( inv_slope > max_inv_slope )
00353                                     {
00354                                         ind[1]        = i;
00355                                         max_inv_slope = inv_slope;
00356                                         num_rotated   = 1;
00357                                     }
00358                                     else if( fabs( inv_slope - max_inv_slope ) < EPSILON )
00359                                     {
00360                                         ind[2] = i;
00361                                         num_rotated++;
00362                                     }
00363                                 }
00364                             }
00365                             break;
00366                         case CLOCKWISE:
00367                             for( i = 0; i < num_vec; i++ )
00368                             {
00369                                 if( i != ind[0] )
00370                                 {
00371                                     inv_slope = ( vec[i][dir2] - pt_2 ) / ( vec[i][dir1] - pt_1 );
00372                                     if( inv_slope < min_inv_slope )
00373                                     {
00374                                         ind[1]        = i;
00375                                         min_inv_slope = inv_slope;
00376                                         num_rotated   = 1;
00377                                     }
00378                                     else if( fabs( inv_slope - min_inv_slope ) < EPSILON )
00379                                     {
00380                                         ind[2] = i;
00381                                         num_rotated++;
00382                                     }
00383                                 }
00384                             }
00385                     }
00386                     switch( num_rotated )
00387                     {
00388                         case 0:
00389                             MSQ_PRINT( 3 )( "No points in the rotation ... odd\n" );
00390                             status = MSQ_HULL_TEST_ERROR;
00391                             break;
00392                         case 1:
00393                             MSQ_PRINT( 3 )( "Found a line in the convex hull\n" );
00394                             pt2    = vec[ind[1]];
00395                             status = MSQ_TWO_PT_PLANE;
00396                             break;
00397                         default:
00398                             MSQ_PRINT( 3 )( "Found 2 or more points in the rotation\n" );
00399                             /* check to see if rotation got past origin */
00400                             inv_origin_slope = pt_2 / pt_1;
00401                             switch( rotate )
00402                             {
00403                                 case COUNTERCLOCKWISE:
00404                                     if( inv_origin_slope >= max_inv_slope )
00405                                         status = MSQ_NO_EQUIL;
00406                                     else if( dir1 == 2 )
00407                                         status = MSQ_CHECK_Y_COORD_DIRECTION;
00408                                     else if( dir1 == 1 )
00409                                         status = MSQ_CHECK_X_COORD_DIRECTION;
00410                                     else
00411                                         status = MSQ_EQUIL;
00412                                     break;
00413                                 case CLOCKWISE:
00414                                     if( inv_origin_slope <= min_inv_slope )
00415                                         status = MSQ_NO_EQUIL;
00416                                     else if( dir1 == 2 )
00417                                         status = MSQ_CHECK_Y_COORD_DIRECTION;
00418                                     else if( dir1 == 1 )
00419                                         status = MSQ_CHECK_X_COORD_DIRECTION;
00420                                     else
00421                                         status = MSQ_EQUIL;
00422                             }
00423                     }
00424                     break;
00425                 case 2: /* use these two points to define the plane */
00426                     pt1    = vec[ind[0]];
00427                     pt2    = vec[ind[1]];
00428                     status = MSQ_TWO_PT_PLANE;
00429                     break;
00430                 default: /* check to see if all > 0 */
00431                     MSQ_PRINT( 3 )( "Found 3 in max plane %f\n", max );
00432                     if( vec[ind[0]][dir1] <= 0 )
00433                         status = MSQ_NO_EQUIL;
00434                     else if( dir1 == 2 )
00435                         status = MSQ_CHECK_Y_COORD_DIRECTION;
00436                     else if( dir1 == 1 )
00437                         status = MSQ_CHECK_X_COORD_DIRECTION;
00438                     else
00439                         status = MSQ_EQUIL;
00440             }
00441         }
00442     }
00443 }
00444 
00445 void NonSmoothDescent::search_direction( PatchData& /*pd*/, Vector3D& mSearch, MsqError& err )
00446 {
00447     bool viable;
00448     double a, b, c, denom;
00449     std::vector< Vector3D > dir;
00450     double R0, R1;
00451     SymmetricMatrix P;
00452     double x[2];
00453     double search_mag;
00454 
00455     const int num_active = mActive.active_ind.size();
00456     ;
00457 
00458     // TODO This might be o.k. actually - i don't see any dependence
00459     // on the element geometry here... try it and see if it works.
00460     // if not, try taking all of the gradients in the active set
00461     // and let the search direction be the average of those.
00462     //   MSQ_FUNCTION_TIMER( "Search Direction" );
00463 
00464     MSQ_PRINT( 2 )( "\nIn Search Direction\n" );
00465     this->print_active_set( mActive, mFunction );
00466 
00467     if( num_active == 0 )
00468     {
00469         MSQ_SETERR( err )( "No active values in search", MsqError::INVALID_STATE );
00470         return;
00471     }
00472 
00473     switch( num_active )
00474     {
00475         case 1:
00476             mSearch   = mGradient[mActive.active_ind[0]];
00477             mSteepest = mActive.active_ind[0];
00478             break;
00479         case 2:
00480             /* if there are two active points, move in the direction of the
00481            intersection of the planes.  This is the steepest descent
00482                direction found by analytically solving the QP */
00483 
00484             /* set up the active gradient directions */
00485             this->get_active_directions( mGradient, dir, err );MSQ_ERRRTN( err );
00486 
00487             /* form the grammian */
00488             this->form_grammian( dir, err );MSQ_ERRRTN( err );
00489             this->form_PD_grammian( err );MSQ_ERRRTN( err );
00490 
00491             denom  = ( mG( 0, 0 ) + mG( 1, 1 ) - 2 * mG( 0, 1 ) );
00492             viable = true;
00493             if( fabs( denom ) > EPSILON )
00494             {
00495                 /* gradients are LI, move along their intersection */
00496                 b = ( mG( 0, 0 ) - mG( 0, 1 ) ) / denom;
00497                 a = 1 - b;
00498                 if( ( b < 0 ) || ( b > 1 ) ) viable = false; /* 0 < b < 1 */
00499                 if( viable )
00500                 {
00501                     mSearch = a * dir[0] + b * dir[1];
00502                 }
00503                 else
00504                 {
00505                     /* the gradients are dependent, move along one face */
00506                     mSearch = dir[0];
00507                 }
00508             }
00509             else
00510             {
00511                 /* the gradients are dependent, move along one face */
00512                 mSearch = dir[0];
00513             }
00514             mSteepest = mActive.active_ind[0];
00515 
00516             break;
00517         default:
00518             /* as in case 2: solve the QP problem to find the steepest
00519                descent direction.  This can be done analytically - as
00520                is done in Gill, Murray and Wright
00521                  for 3 active points in 3 directions - test PD of G
00522                  otherwise we know it's SP SD so search edges and faces */
00523 
00524             /* get the active gradient directions */
00525             this->get_active_directions( mGradient, dir, err );MSQ_ERRRTN( err );
00526 
00527             /* form the entries of the grammian matrix */
00528             this->form_grammian( dir, err );MSQ_ERRRTN( err );
00529             this->form_PD_grammian( err );MSQ_ERRRTN( err );
00530 
00531             if( num_active == 3 )
00532             {
00533                 if( mG.condition3x3() < 1e14 )
00534                 {  // if not singular
00535                     /* form the entries of P=Z^T G Z where Z = [-1...-1; I ] */
00536                     this->form_reduced_matrix( P );
00537                     /* form  the RHS and solve the system for the coeffs */
00538                     R0      = mG( 0, 0 ) - mG( 1, 0 );
00539                     R1      = mG( 0, 0 ) - mG( 2, 0 );
00540                     bool ok = this->solve2x2( P( 0, 0 ), P( 0, 1 ), P( 1, 0 ), P( 1, 1 ), R0, R1, x, err );MSQ_ERRRTN( err );
00541                     if( ok )
00542                     {
00543                         a         = 1 - x[0] - x[1];
00544                         b         = x[0];
00545                         c         = x[1];
00546                         mSearch   = a * dir[0] + b * dir[1] + c * dir[2];
00547                         mSteepest = mActive.active_ind[0];
00548                     }
00549                     else
00550                     {
00551                         this->search_edges_faces( &dir[0], mSearch, err );MSQ_ERRRTN( err );
00552                     }
00553                 }
00554                 else
00555                 {
00556                     this->search_edges_faces( &dir[0], mSearch, err );MSQ_ERRRTN( err );
00557                 }
00558             }
00559             else
00560             {
00561                 this->search_edges_faces( &dir[0], mSearch, err );MSQ_ERRRTN( err );
00562             }
00563     }
00564 
00565     /* if the search direction is essentially zero, equilibrium pt */
00566     search_mag = mSearch % mSearch;
00567     MSQ_PRINT( 3 )( "  Search Magnitude %g \n", search_mag );
00568 
00569     if( fabs( search_mag ) < 1E-13 )
00570         optStatus = MSQ_ZERO_SEARCH;
00571     else
00572         mSearch /= std::sqrt( search_mag );
00573     MSQ_PRINT( 3 )
00574     ( "  Search Direction %g %g  Steepest %lu\n", mSearch[0], mSearch[1], (unsigned long)mSteepest );
00575 }
00576 
00577 void NonSmoothDescent::minmax_opt( PatchData& pd, MsqError& err )
00578 {
00579     bool equilibriumPt = false;
00580     Vector3D mSearch( 0, 0, 0 );
00581 
00582     //      int valid;
00583     MSQ_FUNCTION_TIMER( "Minmax Opt" );
00584     MSQ_PRINT( 2 )( "In minmax_opt\n" );
00585 
00586     mFunction     = originalFunction;
00587     originalValue = mActive.true_active_value;
00588 
00589     int iterCount    = 0;
00590     int optIterCount = 0;
00591 
00592     MSQ_PRINT( 3 )( "Done copying original function to function\n" );
00593 
00594     this->find_active_set( mFunction, mActive );
00595     prevActiveValues.clear();
00596     prevActiveValues.push_back( mActive.true_active_value );
00597 
00598     /* check for equilibrium point */
00599     /* compute the gradient */
00600     this->compute_gradient( &pd, mGradient, err );MSQ_ERRRTN( err );
00601 
00602     if( mActive.active_ind.size() >= 2 )
00603     {
00604         MSQ_PRINT( 3 )( "Testing for an equilibrium point \n" );
00605         equilibriumPt = this->check_equilibrium( optStatus, err );MSQ_ERRRTN( err );
00606 
00607         if( MSQ_DBG( 2 ) && equilibriumPt ) MSQ_PRINT( 2 )( "Optimization Exiting: An equilibrium point \n" );
00608     }
00609 
00610     /* terminate if we have found an equilibrium point or if the step is
00611        too small to be worthwhile continuing */
00612     while( ( optStatus != MSQ_EQUILIBRIUM ) && ( optStatus != MSQ_STEP_TOO_SMALL ) &&
00613            ( optStatus != MSQ_IMP_TOO_SMALL ) && ( optStatus != MSQ_FLAT_NO_IMP ) && ( optStatus != MSQ_ZERO_SEARCH ) &&
00614            ( optStatus != MSQ_MAX_ITER_EXCEEDED ) )
00615     {
00616 
00617         /* increase the iteration count by one */
00618         /* smooth_param->iter_count += 1; */
00619         iterCount += 1;
00620         optIterCount += 1;
00621         if( iterCount > MSQ_MAX_OPT_ITER ) optStatus = MSQ_MAX_ITER_EXCEEDED;
00622 
00623         MSQ_PRINT( 3 )( "\nITERATION %d \n", iterCount );
00624 
00625         /* compute the gradient */
00626         this->compute_gradient( &pd, mGradient, err );MSQ_ERRRTN( err );
00627 
00628         MSQ_PRINT( 3 )( "Computing the search direction \n" );
00629         this->search_direction( pd, mSearch, err );MSQ_ERRRTN( err );
00630 
00631         /* if there are viable directions to search */
00632         if( ( optStatus != MSQ_ZERO_SEARCH ) && ( optStatus != MSQ_MAX_ITER_EXCEEDED ) )
00633         {
00634 
00635             MSQ_PRINT( 3 )( "Computing the projections of the gradients \n" );
00636             this->get_gradient_projections( mSearch, err );MSQ_ERRRTN( err );
00637 
00638             MSQ_PRINT( 3 )( "Computing the initial step size \n" );
00639             this->compute_alpha( err );MSQ_ERRRTN( err );
00640 
00641             MSQ_PRINT( 3 )( "Testing whether to accept this step \n" );
00642             this->step_acceptance( pd, iterCount, mSearch, err );MSQ_ERRRTN( err );
00643             // MSQ_PRINT(3)("The new free vertex position is %f %f %f\n",
00644             //  mCoords[freeVertexIndex][0],mCoords[freeVertexIndex][1],mCoords[freeVertexIndex][2]);
00645 
00646             if( MSQ_DBG( 3 ) )
00647             {
00648                 /* Print the active set */
00649                 this->print_active_set( mActive, mFunction );
00650             }
00651 
00652             /* check for equilibrium point */
00653             if( mActive.active_ind.size() >= 2 )
00654             {
00655                 MSQ_PRINT( 3 )( "Testing for an equilibrium point \n" );
00656                 equilibriumPt = this->check_equilibrium( optStatus, err );MSQ_ERRRTN( err );
00657 
00658                 if( MSQ_DBG( 2 ) && equilibriumPt ) MSQ_PRINT( 2 )( "Optimization Exiting: An equilibrium point \n" );
00659             }
00660 
00661             /* record the values */
00662             prevActiveValues.push_back( mActive.true_active_value );
00663         }
00664         else
00665         {
00666             /* decrease the iteration count by one */
00667             /* smooth_param->iter_count -= 1; */
00668             iterCount -= 1;
00669             if( MSQ_DBG( 2 ) )
00670             {
00671                 MSQ_PRINT( 2 )
00672                 ( "Optimization Exiting: No viable directions; equilibrium point \n" );
00673                 /* Print the old active set */
00674                 this->print_active_set( mActive, mFunction );
00675             }
00676         }
00677     }
00678 
00679     MSQ_PRINT( 2 )( "Checking the validity of the mesh\n" );
00680     if( !this->validity_check( pd, err ) ) MSQ_PRINT( 2 )( "The final mesh is not valid\n" );MSQ_ERRRTN( err );
00681 
00682     MSQ_PRINT( 2 )( "Number of optimization iterations %d\n", iterCount );
00683 
00684     switch( optStatus )
00685     {
00686         default:
00687             MSQ_PRINT( 2 )( "Optimization Termination OptStatus: Invalid early termination\n" );
00688             break;
00689         case MSQ_EQUILIBRIUM:
00690             MSQ_PRINT( 2 )( "Optimization Termination OptStatus: Equilibrium\n" );
00691             break;
00692         case MSQ_STEP_TOO_SMALL:
00693             MSQ_PRINT( 2 )( "Optimization Termination OptStatus: Step Too Small\n" );
00694             break;
00695         case MSQ_IMP_TOO_SMALL:
00696             MSQ_PRINT( 2 )( "Optimization Termination OptStatus: Improvement Too Small\n" );
00697             break;
00698         case MSQ_FLAT_NO_IMP:
00699             MSQ_PRINT( 2 )( "Optimization Termination OptStatus: Flat No Improvement\n" );
00700             break;
00701         case MSQ_ZERO_SEARCH:
00702             MSQ_PRINT( 2 )( "Optimization Termination OptStatus: Zero Search\n" );
00703             break;
00704         case MSQ_MAX_ITER_EXCEEDED:
00705             MSQ_PRINT( 2 )( "Optimization Termination OptStatus: Max Iter Exceeded\n" );
00706             break;
00707     }
00708 }
00709 
00710 void NonSmoothDescent::step_acceptance( PatchData& pd, int iterCount, const Vector3D& mSearch, MsqError& err )
00711 {
00712     const double minAcceptableImprovement = 1e-6;
00713 
00714     //  int        ierr;
00715     //  int        num_values;
00716     bool step_done;
00717     bool valid = true, accept_alpha;
00718     double estimated_improvement;
00719     double current_improvement  = HUGE_VAL;
00720     double previous_improvement = HUGE_VAL;
00721     double current_percent_diff = HUGE_VAL;
00722     Vector3D original_point;
00723 
00724     //  MSQ_FUNCTION_TIMER( "Step Acceptance" );
00725     //  num_values = qmHandles.size();
00726 
00727     optStatus = MSQ_NO_STATUS;
00728 
00729     if( mAlpha < minStepSize )
00730     {
00731         optStatus = MSQ_IMP_TOO_SMALL;
00732         step_done = true;
00733         MSQ_PRINT( 3 )( "Alpha starts too small, no improvement\n" );
00734     }
00735 
00736     const MsqVertex* coords = pd.get_vertex_array( err );
00737 
00738     /* save the original function and active set */
00739     original_point   = coords[freeVertexIndex];
00740     originalFunction = mFunction;
00741     originalActive   = mActive;
00742 
00743     step_done = false;
00744     for( int num_steps = 0; !step_done && num_steps < 100; ++num_steps )
00745     {
00746         accept_alpha = false;
00747 
00748         while( !accept_alpha && mAlpha > minStepSize )
00749         {
00750 
00751             /* make the step */
00752             pd.move_vertex( -mAlpha * mSearch, freeVertexIndex, err );
00753             // pd.set_coords_array_element(coords[freeVertexIndex],0,err);
00754 
00755             MSQ_PRINT( 2 )( "search direction %f %f \n", mSearch[0], mSearch[1] );
00756             MSQ_PRINT( 2 )
00757             ( "new vertex position %f %f \n", coords[freeVertexIndex][0], coords[freeVertexIndex][1] );
00758 
00759             /* assume alpha is acceptable */
00760             accept_alpha = true;
00761 
00762             /* never take a step that makes a valid mesh invalid or worsens the quality */
00763             // TODO Validity check revision -- do the compute function up here
00764             // and then the rest based on validity
00765             valid = validity_check( pd, err );MSQ_ERRRTN( err );
00766             if( valid )
00767             {
00768                 valid = improvement_check( err );MSQ_ERRRTN( err );
00769             }
00770             if( !valid )
00771             {
00772                 accept_alpha = false;
00773                 pd.move_vertex( mAlpha * mSearch, freeVertexIndex, err );
00774                 // pd.set_coords_array_element(coords[freeVertexIndex],0,err);
00775                 mAlpha = mAlpha / 2;
00776                 MSQ_PRINT( 2 )( "Step not accepted, the new alpha %f\n", mAlpha );
00777 
00778                 if( mAlpha < minStepSize )
00779                 {
00780                     optStatus = MSQ_STEP_TOO_SMALL;
00781                     step_done = true;
00782                     MSQ_PRINT( 2 )( "Step too small\n" );
00783                     /* get back the original point, mFunction, and active set */
00784                     pd.set_vertex_coordinates( original_point, freeVertexIndex, err );
00785                     mFunction = originalFunction;
00786                     mActive   = originalActive;
00787                 }
00788             }
00789         }
00790 
00791         if( valid && ( mAlpha > minStepSize ) )
00792         {
00793             /* compute the new function and active set */
00794             this->compute_function( &pd, mFunction, err );MSQ_ERRRTN( err );
00795             this->find_active_set( mFunction, mActive );
00796 
00797             /* estimate the minimum improvement by taking this step */
00798             this->get_min_estimate( &estimated_improvement, err );MSQ_ERRRTN( err );
00799             MSQ_PRINT( 2 )
00800             ( "The estimated improvement for this step: %f\n", estimated_improvement );
00801 
00802             /* calculate the actual increase */
00803             current_improvement = mActive.true_active_value - prevActiveValues[iterCount - 1];
00804 
00805             MSQ_PRINT( 3 )( "Actual improvement %f\n", current_improvement );
00806 
00807             /* calculate the percent difference from estimated increase */
00808             current_percent_diff = fabs( current_improvement - estimated_improvement ) / fabs( estimated_improvement );
00809 
00810             /* determine whether to accept a step */
00811             if( ( fabs( previous_improvement ) > fabs( current_improvement ) ) && ( previous_improvement < 0 ) )
00812             {
00813                 /* accept the previous step - it was better */
00814                 MSQ_PRINT( 2 )( "Accepting the previous step\n" );
00815 
00816                 /* subtract alpha in again (previous step) */
00817                 pd.move_vertex( -mAlpha * mSearch, freeVertexIndex, err );
00818                 // pd.set_coords_array_element(coords[freeVertexIndex],0,err);
00819 
00820                 /* does this make an invalid mesh valid? */
00821                 // TODO Validity check revisison
00822                 valid = validity_check( pd, err );MSQ_ERRRTN( err );
00823                 if( valid ) valid = improvement_check( err );MSQ_ERRRTN( err );
00824 
00825                 /* copy test function and active set */
00826                 mFunction = testFunction;
00827                 mActive   = testActive;
00828 
00829                 optStatus = MSQ_STEP_ACCEPTED;
00830                 step_done = true;
00831 
00832                 /* check to see that we're still making good improvements */
00833                 if( fabs( previous_improvement ) < minAcceptableImprovement )
00834                 {
00835                     optStatus = MSQ_IMP_TOO_SMALL;
00836                     step_done = true;
00837                     MSQ_PRINT( 2 )( "Optimization Exiting: Improvement too small\n" );
00838                 }
00839             }
00840             else if( ( ( fabs( current_improvement ) > fabs( estimated_improvement ) ) ||
00841                        ( current_percent_diff < .1 ) ) &&
00842                      ( current_improvement < 0 ) )
00843             {
00844                 /* accept this step, exceeded estimated increase or was close */
00845                 optStatus = MSQ_STEP_ACCEPTED;
00846                 step_done = true;
00847 
00848                 /* check to see that we're still making good improvements */
00849                 if( fabs( current_improvement ) < minAcceptableImprovement )
00850                 {
00851                     MSQ_PRINT( 2 )( "Optimization Exiting: Improvement too small\n" );
00852                     optStatus = MSQ_IMP_TOO_SMALL;
00853                     step_done = true;
00854                 }
00855             }
00856             else if( ( current_improvement > 0 ) && ( previous_improvement > 0 ) &&
00857                      ( fabs( current_improvement ) < minAcceptableImprovement ) &&
00858                      ( fabs( previous_improvement ) < minAcceptableImprovement ) )
00859             {
00860 
00861                 /* we are making no progress, quit */
00862                 optStatus = MSQ_FLAT_NO_IMP;
00863                 step_done = true;
00864                 MSQ_PRINT( 2 )( "Opimization Exiting: Flat no improvement\n" );
00865 
00866                 /* get back the original point, function, and active set */
00867                 pd.set_vertex_coordinates( original_point, freeVertexIndex, err );MSQ_ERRRTN( err );
00868                 mFunction = originalFunction;
00869                 mActive   = originalActive;
00870             }
00871             else
00872             {
00873                 /* halve alpha and try again */
00874                 /* add out the old step */
00875                 pd.move_vertex( mAlpha * mSearch, freeVertexIndex, err );
00876                 // pd.set_coords_array_element(coords[freeVertexIndex],0,err);
00877 
00878                 /* halve step size */
00879                 mAlpha = mAlpha / 2;
00880                 MSQ_PRINT( 3 )( "Step not accepted, the new alpha %f\n", mAlpha );
00881 
00882                 if( mAlpha < minStepSize )
00883                 {
00884                     /* get back the original point, function, and active set */
00885                     MSQ_PRINT( 2 )( "Optimization Exiting: Step too small\n" );
00886                     pd.set_vertex_coordinates( original_point, freeVertexIndex, err );MSQ_ERRRTN( err );
00887                     mFunction = originalFunction;
00888                     mActive   = originalActive;
00889                     optStatus = MSQ_STEP_TOO_SMALL;
00890                     step_done = true;
00891                 }
00892                 else
00893                 {
00894                     testFunction         = mFunction;
00895                     testActive           = mActive;
00896                     previous_improvement = current_improvement;
00897                 }
00898             }
00899         }
00900     }
00901     if( current_improvement > 0 && optStatus == MSQ_STEP_ACCEPTED )
00902     {
00903         MSQ_PRINT( 2 )( "Accepted a negative step %f \n", current_improvement );
00904     }
00905 }
00906 
00907 bool NonSmoothDescent::compute_function( PatchData* patch_data, std::vector< double >& func, MsqError& err )
00908 {
00909     // NEED 1.0/FUNCTION WHICH IS ONLY TRUE OF CONDITION NUMBER
00910 
00911     func.resize( qmHandles.size() );
00912     bool valid_bool = true;
00913     for( size_t i = 0; i < qmHandles.size(); i++ )
00914     {
00915         valid_bool = valid_bool && currentQM->evaluate( *patch_data, qmHandles[i], func[i], err );
00916         MSQ_ERRZERO( err );
00917     }
00918 
00919     return valid_bool;
00920 }
00921 
00922 bool NonSmoothDescent::compute_gradient( PatchData* patch_data, std::vector< Vector3D >& gradient_out, MsqError& err )
00923 {
00924     MSQ_DBGOUT( 2 ) << "Computing Gradient\n";
00925 
00926     bool valid = true;
00927 
00928 #ifdef NUMERICAL_GRADIENT
00929 
00930     const double delta = 10e-6;
00931     std::vector< double > func( qmHandles.size() ), fdelta( qmHandles.size() );
00932 
00933     valid = this->compute_function( patch_data, func, err );
00934     MSQ_ERRZERO( err );
00935 
00936     /* gradient in the x, y, z direction */
00937 
00938     for( int j = 0; j < 3; j++ )
00939     {
00940 
00941         // perturb the coordinates of the free vertex in the j direction by delta
00942         Vector3D delta_3( 0, 0, 0 );
00943         Vector3D orig_pos = patch_data->vertex_by_index( freeVertexIndex );
00944         delta_3[j]        = delta;
00945         patch_data->move_vertex( delta_3, freeVertexIndex, err );
00946 
00947         // compute the function at the perturbed point location
00948         valid = valid && this->compute_function( patch_data, fdelta, err );
00949         MSQ_ERRZERO( err );
00950 
00951         // compute the numerical gradient
00952         for( size_t i = 0; i < qmHandles.size(); i++ )
00953         {
00954             mGradient[i][j] = ( fdelta[i] - func[i] ) / delta;
00955             // MSQ_DEBUG_ACTION(3,{fprintf(stdout,"  Gradient
00956             // value[%d][%d]=%g\n",i,j,mGradient[i][j]);});
00957         }
00958 
00959         // put the coordinates back where they belong
00960         patch_data->set_vertex_coordinates( orig_pos, freeVertexIndex, err );
00961     }
00962 
00963 #else
00964     double value;
00965     gradient_out.resize( qmHandles.size() );
00966     for( size_t i = 0; i < qmHandles.size(); i++ )
00967     {
00968         valid = valid && currentQM->evaluate_with_gradient( *patch_data, qmHandles[i], value, tmpIdx, tmpGrad, err );
00969         MSQ_ERRZERO( err );
00970         assert( tmpIdx.size() == 1 && tmpIdx[0] == freeVertexIndex );
00971         gradient_out[i] = tmpGrad[0];
00972     }
00973 #endif
00974 
00975     return valid;
00976 }
00977 
00978 void NonSmoothDescent::find_active_set( const std::vector< double >& function, ActiveSet& active_set )
00979 {
00980     // local parameter initialization
00981     const double activeEpsilon = .3e-4;
00982     //  activeEpsilon = .3e-8;
00983 
00984     double function_val;
00985     double active_value0;
00986     double temp;
00987 
00988     //    FUNCTION_TIMER_START("Find Active Set");
00989     MSQ_DBGOUT( 2 ) << "\nFinding the active set\n";
00990 
00991     /* the first function value is our initial active value */
00992     active_set.set( 0 );
00993     active_set.true_active_value = function[0];
00994     //    MSQ_DEBUG_ACTION(3,{fprintf(stdout,"  function value[0]: %g\n",function[0]);});
00995 
00996     /* first sort out the active set...
00997        all vals within active_epsilon of largest val */
00998 
00999     for( size_t i = 1; i < qmHandles.size(); i++ )
01000     {
01001         function_val = function[i];
01002         if( active_set.true_active_value < function_val ) active_set.true_active_value = function_val;
01003         active_value0 = function[active_set.active_ind[0]];
01004         temp          = fabs( function_val - active_value0 );
01005         //        MSQ_DEBUG_ACTION(3,{fprintf(stdout,"  function value[%d]: %g\n",i,function[i]);});
01006         if( function_val > active_value0 )
01007         {  // seek max_i function[i]
01008             if( temp >= activeEpsilon )
01009             {
01010                 active_set.set( i );  // new max
01011             }
01012             else
01013             {
01014                 active_set.add( i, fabs( function_val - active_value0 ) < EPSILON );
01015             }
01016         }
01017         else
01018         {
01019             if( temp < activeEpsilon )
01020             {
01021                 active_set.add( i, fabs( function_val - active_value0 ) < EPSILON );
01022             }
01023         }
01024     }
01025 }
01026 
01027 bool NonSmoothDescent::validity_check( PatchData& pd, MsqError& err )
01028 
01029 {
01030     //  FUNCTION_TIMER_START("Validity Check");
01031 
01032     // ONLY FOR SIMPLICIAL MESHES - THERE SHOULD BE A VALIDITY CHECKER ASSOCIATED
01033     // WITH MSQ ELEMENTS
01034 
01035     /* check that the simplicial mesh is still valid, based on right handedness.
01036          Returns a 1 or a 0 */
01037 
01038     // TODO as a first step we can switch this over to the function
01039     // evaluation and use the rest of the code as is
01040     bool valid  = true;
01041     double dEps = 1.e-13;
01042 
01043     double x1, x2, x3, x4, y1, y2, y3, y4, z1, z2, z3, z4;
01044     const MsqVertex* coords = pd.get_vertex_array( err );
01045 
01046     for( size_t i = 0; i < pd.num_elements(); i++ )
01047     {
01048         const size_t* conn = pd.element_by_index( i ).get_vertex_index_array();
01049         coords[conn[0]].get_coordinates( x1, y1, z1 );
01050         coords[conn[1]].get_coordinates( x2, y2, z2 );
01051         coords[conn[2]].get_coordinates( x3, y3, z3 );
01052         coords[conn[3]].get_coordinates( x4, y4, z4 );
01053 
01054         double dDX2 = x2 - x1;
01055         double dDX3 = x3 - x1;
01056         double dDX4 = x4 - x1;
01057 
01058         double dDY2 = y2 - y1;
01059         double dDY3 = y3 - y1;
01060         double dDY4 = y4 - y1;
01061 
01062         double dDZ2 = z2 - z1;
01063         double dDZ3 = z3 - z1;
01064         double dDZ4 = z4 - z1;
01065 
01066         /* dDet is proportional to the cell volume */
01067         double dDet = dDX2 * dDY3 * dDZ4 + dDX3 * dDY4 * dDZ2 + dDX4 * dDY2 * dDZ3 - dDZ2 * dDY3 * dDX4 -
01068                       dDZ3 * dDY4 * dDX2 - dDZ4 * dDY2 * dDX3;
01069 
01070         /* Compute a length scale based on edge lengths. */
01071         double dScale = ( sqrt( ( x1 - x2 ) * ( x1 - x2 ) + ( y1 - y2 ) * ( y1 - y2 ) + ( z1 - z2 ) * ( z1 - z2 ) ) +
01072                           sqrt( ( x1 - x3 ) * ( x1 - x3 ) + ( y1 - y3 ) * ( y1 - y3 ) + ( z1 - z3 ) * ( z1 - z3 ) ) +
01073                           sqrt( ( x1 - x4 ) * ( x1 - x4 ) + ( y1 - y4 ) * ( y1 - y4 ) + ( z1 - z4 ) * ( z1 - z4 ) ) +
01074                           sqrt( ( x2 - x3 ) * ( x2 - x3 ) + ( y2 - y3 ) * ( y2 - y3 ) + ( z2 - z3 ) * ( z2 - z3 ) ) +
01075                           sqrt( ( x2 - x4 ) * ( x2 - x4 ) + ( y2 - y4 ) * ( y2 - y4 ) + ( z2 - z4 ) * ( z2 - z4 ) ) +
01076                           sqrt( ( x3 - x4 ) * ( x3 - x4 ) + ( y3 - y4 ) * ( y3 - y4 ) + ( z3 - z4 ) * ( z3 - z4 ) ) ) /
01077                         6.;
01078 
01079         /* Use the length scale to get a better idea if the tet is flat or
01080            just really small. */
01081         if( fabs( dScale ) < EPSILON )
01082         {
01083             return false;
01084         }
01085         else
01086         {
01087             dDet /= ( dScale * dScale * dScale );
01088         }
01089 
01090         if( dDet > dEps )
01091         {
01092             valid = true;
01093         }
01094         else if( dDet < -dEps )
01095         {
01096             return false;
01097         }
01098         else
01099         {
01100             return false;
01101         }
01102     }  // end for i=1,numElements
01103 
01104     //  MSQ_DEBUG_ACTION(2,{fprintf(stdout,"Mesh Validity is: %d \n",valid);});
01105 
01106     //  FUNCTION_TIMER_END();
01107     return ( valid );
01108 }
01109 
01110 void NonSmoothDescent::get_active_directions( const std::vector< Vector3D >& pGradient,
01111                                               std::vector< Vector3D >& dir,
01112                                               MsqError& /*err*/ )
01113 {
01114     const size_t num_active = mActive.active_ind.size();
01115     dir.resize( num_active );
01116     for( size_t i = 0; i < num_active; i++ )
01117     {
01118         dir[i] = pGradient[mActive.active_ind[i]];
01119     }
01120 }
01121 
01122 bool NonSmoothDescent::check_vector_dots( const std::vector< Vector3D >& vec,
01123                                           const Vector3D& normal,
01124                                           MsqError& /*err*/ )
01125 {
01126     double test_dot = vec[0] % normal;
01127     unsigned ind    = 1;
01128     while( ( fabs( test_dot ) < EPSILON ) && ( ind < vec.size() ) )
01129     {
01130         test_dot = vec[ind] % normal;
01131         ind++;
01132     }
01133 
01134     for( unsigned i = ind; i < vec.size(); i++ )
01135     {
01136         double dot = vec[i] % normal;
01137         if( ( ( dot > 0 && test_dot < 0 ) || ( dot < 0 && test_dot > 0 ) ) && ( fabs( dot ) > EPSILON ) )
01138         {
01139             return true;
01140         }
01141     }
01142     return false;
01143 }
01144 
01145 bool NonSmoothDescent::convex_hull_test( const std::vector< Vector3D >& vec, MsqError& err )
01146 {
01147     //    int ierr;
01148     bool equil         = false;
01149     Direction dir_done = MSQ_ZDIR;
01150     Status status      = MSQ_CHECK_Z_COORD_DIRECTION;
01151     Vector3D pt1, pt2, pt3, normal;
01152 
01153     /* tries to determine equilibrium for the 3D case */
01154 
01155     if( vec.size() <= 2 ) status = MSQ_NO_EQUIL;
01156 
01157     while( ( status != MSQ_EQUIL ) && ( status != MSQ_NO_EQUIL ) && ( status != MSQ_HULL_TEST_ERROR ) )
01158     {
01159         if( status == MSQ_CHECK_Z_COORD_DIRECTION )
01160         {
01161             this->find_plane_points( MSQ_ZDIR, MSQ_YDIR, vec, pt1, pt2, pt3, status, err );
01162             dir_done = MSQ_ZDIR;
01163         }
01164         if( status == MSQ_CHECK_Y_COORD_DIRECTION )
01165         {
01166             this->find_plane_points( MSQ_YDIR, MSQ_XDIR, vec, pt1, pt2, pt3, status, err );
01167             dir_done = MSQ_YDIR;
01168         }
01169         if( status == MSQ_CHECK_X_COORD_DIRECTION )
01170         {
01171             this->find_plane_points( MSQ_XDIR, MSQ_ZDIR, vec, pt1, pt2, pt3, status, err );
01172             dir_done = MSQ_XDIR;
01173         }
01174         if( status == MSQ_TWO_PT_PLANE )
01175         {
01176             pt3 = Vector3D( 0, 0, 0 );
01177         }
01178         if( ( status == MSQ_TWO_PT_PLANE ) || ( status == MSQ_THREE_PT_PLANE ) )
01179         {
01180             this->find_plane_normal( pt1, pt2, pt3, normal, err );
01181             equil = this->check_vector_dots( vec, normal, err );
01182             if( equil )
01183             {
01184                 switch( dir_done )
01185                 {
01186                     case MSQ_ZDIR:
01187                         equil  = false;
01188                         status = MSQ_CHECK_Y_COORD_DIRECTION;
01189                         break;
01190                     case MSQ_YDIR:
01191                         equil  = false;
01192                         status = MSQ_CHECK_X_COORD_DIRECTION;
01193                         break;
01194                     case MSQ_XDIR:
01195                         equil  = true;
01196                         status = MSQ_EQUIL;
01197                 }
01198             }
01199             else if( equil == 0 )
01200             {
01201                 status = MSQ_NO_EQUIL;
01202             }
01203             else
01204             {
01205                 MSQ_SETERR( err )( "equil flag not set to 0 or 1", MsqError::INVALID_STATE );
01206             }
01207         }
01208     }
01209     switch( status )
01210     {
01211         case MSQ_NO_EQUIL:
01212             MSQ_PRINT( 3 )( "Not an equilibrium point\n" );
01213             equil = false;
01214             break;
01215         case MSQ_EQUIL:
01216             MSQ_PRINT( 3 )( "An equilibrium point\n" );
01217             equil = true;
01218             break;
01219         default:
01220             MSQ_PRINT( 3 )( "Failed to determine equil or not; status = %d\n", status );
01221     }
01222     //    FUNCTION_TIMER_END();
01223     return ( equil );
01224 }
01225 
01226 void NonSmoothDescent::form_grammian( const std::vector< Vector3D >& vec, MsqError& /*err*/ )
01227 {
01228     /* form the grammian with the dot products of the gradients */
01229     const size_t num_active = mActive.active_ind.size();
01230     mG.resize( num_active );
01231     for( size_t i = 0; i < num_active; i++ )
01232         for( size_t j = i; j < num_active; j++ )
01233             mG( i, j ) = vec[i] % vec[j];
01234 }
01235 
01236 bool NonSmoothDescent::check_equilibrium( OptStatus& status, MsqError& err )
01237 {
01238     std::vector< Vector3D > dir;
01239 
01240     // TODO - this subroutine is no longer clear to me... is it still
01241     // appropriate for quads and hexes?  I think it might be in 2D, but
01242     // 3D is less clear.  Is there a more general algorithm to use?
01243     // ask Todd/check in numerical optimization
01244 
01245     bool equil              = false;
01246     const size_t num_active = mActive.active_ind.size();
01247     ;
01248 
01249     if( num_active == qmHandles.size() )
01250     {
01251         equil  = true;
01252         status = MSQ_EQUILIBRIUM;
01253         MSQ_PRINT( 3 )( "All the function values are in the active set\n" );
01254     }
01255 
01256     /* set up the active mGradient directions */
01257     this->get_active_directions( mGradient, dir, err );
01258     MSQ_ERRZERO( err );
01259 
01260     /* normalize the active directions */
01261     for( size_t j = 0; j < num_active; j++ )
01262         dir[j] /= dir[j].length();
01263 
01264     equil = this->convex_hull_test( dir, err );
01265     if( equil ) status = MSQ_EQUILIBRIUM;
01266 
01267     return equil;
01268 }
01269 
01270 static double condition3x3( const double A[9] )
01271 {
01272     //   int ierr;
01273     double a11, a12, a13;
01274     double a21, a22, a23;
01275     double a31, a32, a33;
01276     //   double s1, s2, s4, s3, t0;
01277     double s1, s2, s3;
01278     double denom;
01279     //   double one = 1.0;
01280     double temp;
01281     bool zero_denom = true;
01282 
01283     a11 = A[0];
01284     a12 = A[1];
01285     a13 = A[2];
01286     a21 = A[3];
01287     a22 = A[4];
01288     a23 = A[5];
01289     a31 = A[6];
01290     a32 = A[7];
01291     a33 = A[8];
01292 
01293     denom = -a11 * a22 * a33 + a11 * a23 * a32 + a21 * a12 * a33 - a21 * a13 * a32 - a31 * a12 * a23 + a31 * a13 * a22;
01294 
01295     if( ( fabs( a11 ) > EPSILON ) && ( fabs( denom / a11 ) > EPSILON ) )
01296     {
01297         zero_denom = false;
01298     }
01299     if( ( fabs( a22 ) > EPSILON ) && ( fabs( denom / a22 ) > EPSILON ) )
01300     {
01301         zero_denom = false;
01302     }
01303     if( ( fabs( a33 ) > EPSILON ) && ( fabs( denom / a33 ) > EPSILON ) )
01304     {
01305         zero_denom = false;
01306     }
01307 
01308     if( zero_denom )
01309     {
01310         return HUGE_VAL;
01311     }
01312     else
01313     {
01314         s1 = sqrt( a11 * a11 + a12 * a12 + a13 * a13 + a21 * a21 + a22 * a22 + a23 * a23 + a31 * a31 + a32 * a32 +
01315                    a33 * a33 );
01316 
01317         temp = ( -a22 * a33 + a23 * a32 ) / denom;
01318         s3   = temp * temp;
01319         temp = ( a12 * a33 - a13 * a32 ) / denom;
01320         s3 += temp * temp;
01321         temp = ( a12 * a23 - a13 * a22 ) / denom;
01322         s3 += temp * temp;
01323         temp = ( a21 * a33 - a23 * a31 ) / denom;
01324         s3 += temp * temp;
01325         temp = ( a11 * a33 - a13 * a31 ) / denom;
01326         s3 += temp * temp;
01327         temp = ( a11 * a23 - a13 * a21 ) / denom;
01328         s3 += temp * temp;
01329         temp = ( a21 * a32 - a22 * a31 ) / denom;
01330         s3 += temp * temp;
01331         temp = ( -a11 * a32 + a12 * a31 ) / denom;
01332         s3 += temp * temp;
01333         temp = ( -a11 * a22 + a12 * a21 ) / denom;
01334         s3 += temp * temp;
01335 
01336         s2 = sqrt( s3 );
01337         return s1 * s2;
01338     }
01339 }
01340 
01341 double NonSmoothDescent::SymmetricMatrix::condition3x3() const
01342 {
01343     double values[9] =
01344         { operator()( 0, 0 ), operator()( 0, 1 ), operator()( 0, 2 ), operator()( 1, 0 ), operator()( 1, 1 ),
01345           operator()( 1, 2 ), operator()( 2, 0 ), operator()( 2, 1 ), operator()( 2, 2 ) };
01346     return MBMesquite::condition3x3( values );
01347 }
01348 
01349 void NonSmoothDescent::singular_test( int n, const Matrix3D& A, bool& singular, MsqError& err )
01350 {
01351     //    int test;
01352     //    double determinant;
01353     double cond;
01354 
01355     if( ( n > 3 ) || ( n < 1 ) )
01356     {
01357         MSQ_SETERR( err )( "Singular test works only for n=1 to n=3", MsqError::INVALID_ARG );
01358         return;
01359     }
01360 
01361     singular = true;
01362     switch( n )
01363     {
01364         case 1:
01365             if( A[0][0] > 0 ) singular = false;
01366             break;
01367         case 2:
01368             if( fabs( A[0][0] * A[1][1] - A[0][1] * A[1][0] ) > EPSILON ) singular = false;
01369             break;
01370         case 3:
01371             /* calculate the condition number */
01372             cond = condition3x3( A[0] );
01373             if( cond < 1E14 ) singular = false;
01374             break;
01375     }
01376 }
01377 
01378 void NonSmoothDescent::form_PD_grammian( MsqError& err )
01379 {
01380     int i, j, k;
01381     int g_ind_1;
01382     bool singular = false;
01383 
01384     const int num_active = mActive.active_ind.size();
01385 
01386     /* this assumes the grammian has been formed */
01387     for( i = 0; i < num_active; i++ )
01388     {
01389         for( j = i; j < num_active; j++ )
01390         {
01391             if( mG( i, j ) == -1 )
01392             {
01393                 MSQ_SETERR( err )( "Grammian not computed properly", MsqError::INVALID_STATE );
01394                 return;
01395             }
01396         }
01397     }
01398 
01399     /* use the first gradient in the active set */
01400     g_ind_1    = 0;
01401     mPDG[0][0] = mG( 0, 0 );
01402     pdgInd[0]  = mActive.active_ind[0];
01403 
01404     /* test the rest and add them as appropriate */
01405     k = 1;
01406     i = 1;
01407     while( ( k < 3 ) && ( i < num_active ) )
01408     {
01409         mPDG[0][k] = mPDG[k][0] = mG( 0, i );
01410         mPDG[k][k]              = mG( i, i );
01411         if( k == 2 )
01412         { /* add the dot product of g1 and g2 */
01413             mPDG[1][k] = mPDG[k][1] = mG( g_ind_1, i );
01414         }
01415         this->singular_test( k + 1, mPDG, singular, err );
01416         if( !singular )
01417         {
01418             pdgInd[k] = mActive.active_ind[i];
01419             if( k == 1 ) g_ind_1 = i;
01420             k++;
01421         }
01422         i++;
01423     }
01424 }
01425 
01426 void NonSmoothDescent::search_edges_faces( const Vector3D* dir, Vector3D& mSearch, MsqError& /*err*/ )
01427 {
01428     bool viable;
01429     double a, b, denom;
01430     Vector3D g_bar;
01431     Vector3D temp_search( 0, 0, 0 ); /* initialize the search direction to 0,0 */
01432     double projection, min_projection;
01433 
01434     const size_t num_active = mActive.active_ind.size();
01435     ;
01436 
01437     /* Check for viable faces */
01438     min_projection = HUGE_VAL;
01439     for( size_t i = 0; i < num_active; i++ )
01440     {
01441         /* FACE I */
01442         viable = true;
01443 
01444         /* test the viability */
01445         for( size_t j = 0; j < num_active; j++ )
01446         { /* lagrange multipliers>0 */
01447             if( mG( j, i ) < 0 ) viable = false;
01448         }
01449 
01450         /* find the minimum of viable directions */
01451         if( ( viable ) && ( mG( i, i ) < min_projection ) )
01452         {
01453             min_projection = mG( i, i );
01454             temp_search    = dir[i];
01455             mSteepest      = mActive.active_ind[i];
01456         }
01457 
01458         /* INTERSECTION IJ */
01459         for( size_t j = i + 1; j < num_active; j++ )
01460         {
01461             viable = true;
01462 
01463             /* find the coefficients of the intersection
01464                and test the viability */
01465             denom = 2 * mG( i, j ) - mG( i, i ) - mG( j, j );
01466             a = b = 0;
01467             if( fabs( denom ) > EPSILON )
01468             {
01469                 b = ( mG( i, j ) - mG( i, i ) ) / denom;
01470                 a = 1 - b;
01471                 if( ( b < 0 ) || ( b > 1 ) ) /* 0 < b < 1 */
01472                     viable = false;
01473                 for( size_t k = 0; k < num_active; k++ )
01474                 { /* lagrange multipliers>0 */
01475                     if( ( a * mG( k, i ) + b * mG( k, j ) ) <= 0 ) viable = false;
01476                 }
01477             }
01478             else
01479             {
01480                 viable = false; /* Linearly dependent */
01481             }
01482 
01483             /* find the minimum of viable directions */
01484             if( viable )
01485             {
01486                 g_bar      = a * dir[i] + b * dir[j];
01487                 projection = g_bar % g_bar;
01488                 if( projection < min_projection )
01489                 {
01490                     min_projection = projection;
01491                     temp_search    = g_bar;
01492                     mSteepest      = mActive.active_ind[i];
01493                 }
01494             }
01495         }
01496     }
01497     if( optStatus != MSQ_EQUILIBRIUM )
01498     {
01499         mSearch = temp_search;
01500     }
01501 }
01502 
01503 bool NonSmoothDescent::solve2x2( double a11,
01504                                  double a12,
01505                                  double a21,
01506                                  double a22,
01507                                  double b1,
01508                                  double b2,
01509                                  double x[2],
01510                                  MsqError& /*err*/ )
01511 {
01512     double factor;
01513 
01514     /* if the system is not singular, solve it */
01515     if( fabs( a11 * a22 - a21 * a12 ) > EPSILON )
01516     {
01517         if( fabs( a11 ) > EPSILON )
01518         {
01519             factor = ( a21 / a11 );
01520             x[1]   = ( b2 - factor * b1 ) / ( a22 - factor * a12 );
01521             x[0]   = ( b1 - a12 * x[1] ) / a11;
01522         }
01523         else if( fabs( a21 ) > EPSILON )
01524         {
01525             factor = ( a11 / a21 );
01526             x[1]   = ( b1 - factor * b2 ) / ( a12 - factor * a22 );
01527             x[0]   = ( b2 - a22 * x[1] ) / a21;
01528         }
01529         return true;
01530     }
01531     else
01532     {
01533         return false;
01534     }
01535 }
01536 
01537 void NonSmoothDescent::form_reduced_matrix( SymmetricMatrix& P )
01538 {
01539     const size_t P_size = mActive.active_ind.size() - 1;
01540     P.resize( P_size );
01541     for( size_t i = 0; i < P_size; i++ )
01542     {
01543         P( i, i ) = mG( 0, 0 ) - 2 * mG( 0, i + 1 ) + mG( i + 1, i + 1 );
01544         for( size_t j = i + 1; j < P_size; j++ )
01545         {
01546             P( i, j ) = mG( 0, 0 ) - mG( 0, j + 1 ) - mG( i + 1, 0 ) + mG( i + 1, j + 1 );
01547         }
01548     }
01549 }
01550 
01551 void NonSmoothDescent::get_min_estimate( double* final_est, MsqError& /*err*/ )
01552 {
01553     double est_imp;
01554 
01555     *final_est = -HUGE_VAL;
01556     for( size_t i = 0; i < mActive.active_ind.size(); i++ )
01557     {
01558         est_imp = -mAlpha * mGS[mActive.active_ind[i]];
01559         if( est_imp > *final_est ) *final_est = est_imp;
01560     }
01561     if( *final_est == 0 )
01562     {
01563         *final_est = -HUGE_VAL;
01564         for( size_t i = 0; i < qmHandles.size(); i++ )
01565         {
01566             est_imp = -mAlpha * mGS[i];
01567             if( ( est_imp > *final_est ) && ( fabs( est_imp ) > EPSILON ) )
01568             {
01569                 *final_est = est_imp;
01570             }
01571         }
01572     }
01573 }
01574 
01575 void NonSmoothDescent::get_gradient_projections( const Vector3D& mSearch, MsqError& /*err*/ )
01576 {
01577     for( size_t i = 0; i < qmHandles.size(); i++ )
01578         mGS[i] = mGradient[i] % mSearch;
01579 
01580     MSQ_PRINT( 3 )( "steepest in get_gradient_projections %lu\n", (unsigned long)mSteepest );
01581 }
01582 
01583 void NonSmoothDescent::compute_alpha( MsqError& /*err*/ )
01584 {
01585     double steepest_function;
01586     double steepest_grad;
01587     double alpha_i;
01588     double min_positive_value = HUGE_VAL;
01589 
01590     //    FUNCTION_TIMER_START("Compute Alpha");
01591 
01592     MSQ_PRINT( 2 )( "In compute alpha\n" );
01593 
01594     mAlpha = HUGE_VAL;
01595 
01596     steepest_function = mFunction[mSteepest];
01597     steepest_grad     = mGS[mSteepest];
01598     for( size_t i = 0; i < qmHandles.size(); i++ )
01599     {
01600         /* if it's not active */
01601         if( i != mSteepest )
01602         {
01603             alpha_i = steepest_function - mFunction[i];
01604 
01605             if( fabs( mGS[mSteepest] - mGS[i] ) > 1E-13 )
01606             {
01607                 /* compute line intersection */
01608                 alpha_i = alpha_i / ( steepest_grad - mGS[i] );
01609             }
01610             else
01611             {
01612                 /* the lines don't intersect - it's not under consideration*/
01613                 alpha_i = 0;
01614             }
01615             if( ( alpha_i > minStepSize ) && ( fabs( alpha_i ) < fabs( mAlpha ) ) )
01616             {
01617                 mAlpha = fabs( alpha_i );
01618                 MSQ_PRINT( 3 )( "Setting alpha %lu %g\n", (unsigned long)i, alpha_i );
01619             }
01620             if( ( alpha_i > 0 ) && ( alpha_i < min_positive_value ) )
01621             {
01622                 min_positive_value = alpha_i;
01623             }
01624         }
01625     }
01626 
01627     if( ( mAlpha == HUGE_VAL ) && ( min_positive_value != HUGE_VAL ) )
01628     {
01629         mAlpha = min_positive_value;
01630     }
01631 
01632     /* if it never gets set, set it to the default */
01633     if( mAlpha == HUGE_VAL )
01634     {
01635         mAlpha = maxAlpha;
01636         MSQ_PRINT( 3 )( "Setting alpha to the maximum step length\n" );
01637     }
01638 
01639     MSQ_PRINT( 3 )( "  The initial step size: %f\n", mAlpha );
01640 
01641     //    FUNCTION_TIMER_END();
01642 }
01643 
01644 void NonSmoothDescent::print_active_set( const ActiveSet& active_set, const std::vector< double >& func )
01645 {
01646     if( active_set.active_ind.empty() ) MSQ_DBGOUT( 3 ) << "No active values\n";
01647     /* print the active set */
01648     for( size_t i = 0; i < active_set.active_ind.size(); i++ )
01649     {
01650         MSQ_PRINT( 3 )
01651         ( "Active value %lu:   %f \n", (unsigned long)i + 1, func[active_set.active_ind[i]] );
01652     }
01653 }
01654 
01655 void NonSmoothDescent::init_opt( PatchData& pd, MsqError& err )
01656 {
01657     qmHandles.clear();
01658     currentQM->get_evaluations( pd, qmHandles, true, err );MSQ_ERRRTN( err );
01659 
01660     MSQ_PRINT( 2 )( "\nInitializing Optimization \n" );
01661 
01662     /* for the purposes of initialization will be set to zero after */
01663     optStatus = MSQ_NO_STATUS;
01664     mSteepest = 0;
01665     mAlpha    = 0;
01666     maxAlpha  = 0;
01667 
01668     MSQ_PRINT( 3 )( "  Initialized Constants \n" );
01669     pdgInd[0] = pdgInd[1] = pdgInd[2] = -1;
01670     mPDG                              = Matrix3D( 0.0 );
01671 
01672     MSQ_PRINT( 3 )( "  Initialized search and PDG \n" );
01673     mFunction.clear();
01674     mFunction.resize( qmHandles.size(), 0.0 );
01675     testFunction.clear();
01676     testFunction.resize( qmHandles.size(), 0.0 );
01677     originalFunction.clear();
01678     originalFunction.resize( qmHandles.size(), 0.0 );
01679     mGradient.clear();
01680     mGradient.resize( qmHandles.size(), Vector3D( 0.0 ) );
01681     mGS.resize( qmHandles.size() );
01682 
01683     MSQ_PRINT( 3 )( "  Initialized function/gradient \n" );
01684     mG.resize( qmHandles.size() );
01685     mG.fill( -1 );
01686     MSQ_PRINT( 3 )( "  Initialized G\n" );
01687 
01688     prevActiveValues.clear();
01689     prevActiveValues.reserve( 32 );
01690     MSQ_PRINT( 3 )( "  Initialized prevActiveValues\n" );
01691 }
01692 
01693 void NonSmoothDescent::init_max_step_length( PatchData& pd, MsqError& err )
01694 {
01695     size_t i, j;
01696     double max_diff = 0;
01697     double diff     = 0;
01698 
01699     MSQ_PRINT( 2 )( "In init_max_step_length\n" );
01700 
01701     /* check that the input data is correct */
01702     if( pd.num_elements() == 0 )
01703     {
01704         MSQ_SETERR( err )( "Num incident vtx = 0\n", MsqError::INVALID_MESH );
01705         return;
01706     }
01707 
01708     /* find the maximum distance between two incident vertex locations */
01709     const MsqVertex* coords = pd.get_vertex_array( err );
01710     for( i = 0; i < pd.num_nodes() - 1; i++ )
01711     {
01712         for( j = i; j < pd.num_nodes(); j++ )
01713         {
01714             diff = ( coords[i] - coords[j] ).length_squared();
01715             if( max_diff < diff ) max_diff = diff;
01716         }
01717     }
01718     max_diff = sqrt( max_diff );
01719     if( max_diff == 0 )
01720     {
01721         MSQ_SETERR( err )
01722         ( "Maximum distance between incident vertices = 0\n", MsqError::INVALID_MESH );
01723         return;
01724     }
01725     maxAlpha = max_diff / 100;
01726 
01727     MSQ_PRINT( 3 )( "  Maximum step is %g\n", maxAlpha );
01728 }
01729 
01730 }  // namespace MBMesquite
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines