#include <gp_Trsf.hxx>
#include <gp_Vec.hxx>
#include <math_FunctionSetRoot.hxx>
+#include <math_Matrix.hxx>
+#include <math_Vector.hxx>
#include "SMDS_MeshNode.hxx"
#include "SMDS_MeshVolume.hxx"
const vector<gp_XYZ>& p = thePointOnShape;
thePoint =
- x1 * p[ID_F0yz] + x * p[ID_F1yz]
- + y1 * p[ID_Fx0z] + y * p[ID_Fx1z]
- + z1 * p[ID_Fxy0] + z * p[ID_Fxy1]
- + x1 * (y1 * (z1 * p[ID_V000] + z * p[ID_V001])
- + y * (z1 * p[ID_V010] + z * p[ID_V011]))
- + x * (y1 * (z1 * p[ID_V100] + z * p[ID_V101])
- + y * (z1 * p[ID_V110] + z * p[ID_V111]));
+ x1 * p[ID_F0yz] + x * p[ID_F1yz] +
+ y1 * p[ID_Fx0z] + y * p[ID_Fx1z] +
+ z1 * p[ID_Fxy0] + z * p[ID_Fxy1] +
+ x1 * (y1 * (z1 * p[ID_V000] + z * p[ID_V001]) +
+ y * (z1 * p[ID_V010] + z * p[ID_V011])) +
+ x * (y1 * (z1 * p[ID_V100] + z * p[ID_V101]) +
+ y * (z1 * p[ID_V110] + z * p[ID_V111]));
thePoint -=
- x1 * (y1 * p[ID_E00z] + y * p[ID_E01z])
- + x * (y1 * p[ID_E10z] + y * p[ID_E11z])
- + y1 * (z1 * p[ID_Ex00] + z * p[ID_Ex01])
- + y * (z1 * p[ID_Ex10] + z * p[ID_Ex11])
- + z1 * (x1 * p[ID_E0y0] + x * p[ID_E1y0])
- + z * (x1 * p[ID_E0y1] + x * p[ID_E1y1]);
+ x1 * (y1 * p[ID_E00z] + y * p[ID_E01z]) +
+ x * (y1 * p[ID_E10z] + y * p[ID_E11z]) +
+ y1 * (z1 * p[ID_Ex00] + z * p[ID_Ex01]) +
+ y * (z1 * p[ID_Ex10] + z * p[ID_Ex11]) +
+ z1 * (x1 * p[ID_E0y0] + x * p[ID_E1y0]) +
+ z * (x1 * p[ID_E0y1] + x * p[ID_E1y1]);
return true;
}
+//=======================================================================
+//function : Constructor
+//purpose :
+//=======================================================================
+
+SMESH_Block::SMESH_Block():
+ myNbIterations(0),
+ mySumDist(0.),
+ myTolerance(-1.) // to be re-initialized
+{
+}
+
+
//=======================================================================
//function : NbVariables
//purpose :
Standard_Boolean SMESH_Block::Derivatives(const math_Vector& XYZ,math_Matrix& Df)
{
- MESSAGE( "SMESH_Block::Derivatives()");
math_Vector F(1,3);
return Values(XYZ,F,Df);
}
//=======================================================================
-//function : Constructor
+//function : GetStateNumber
//purpose :
//=======================================================================
-SMESH_Block::SMESH_Block():
- myNbIterations(0),
- mySumDist(0.),
- myTolerance(-1.) // to be re-initialized
+Standard_Integer SMESH_Block::GetStateNumber ()
{
+ return 0; //myValues[0] < 1e-1;
}
//=======================================================================
// 3 partial derivatives
gp_Vec drv[ 3 ]; // where we move with a small step in each direction
for ( int iP = 1; iP <= 3; iP++ ) {
- if ( iP - 1 == myFaceIndex ) {
+ if ( iP == myFaceIndex ) {
drv[ iP - 1 ] = gp_Vec(0,0,0);
continue;
}
return true;
}
+//============================================================================
+//function : computeParameters
+//purpose : compute point parameters in the block using math_FunctionSetRoot
+//============================================================================
+
+bool SMESH_Block::computeParameters(const gp_Pnt& thePoint,
+ gp_XYZ& theParams,
+ const gp_XYZ& theParamsHint)
+{
+ myPoint = thePoint.XYZ();
+
+ myParam.SetCoord( -1,-1,-1 );
+ myValues[ SQUARE_DIST ] = 1e100;
+
+ math_Vector low ( 1, 3, 0.0 );
+ math_Vector up ( 1, 3, 1.0 );
+ math_Vector tol ( 1, 3, 1e-4 );
+ math_Vector start( 1, 3, 0.0 );
+ start( 1 ) = theParamsHint.X();
+ start( 2 ) = theParamsHint.Y();
+ start( 3 ) = theParamsHint.Z();
+
+ math_FunctionSetRoot paramSearch( *this, tol );
+
+ mySquareFunc = 0; // large approaching steps
+ //if ( hasHint ) mySquareFunc = 1; // small approaching steps
+
+ double loopTol = 10 * myTolerance;
+ int nbLoops = 0;
+ while ( distance() > loopTol && nbLoops <= 3 )
+ {
+ paramSearch.Perform ( *static_cast<math_FunctionSetWithDerivatives*>(this),
+ start, low, up );
+ start( 1 ) = myParam.X();
+ start( 2 ) = myParam.Y();
+ start( 3 ) = myParam.Z();
+ mySquareFunc = !mySquareFunc;
+ nbLoops++;
+ }
+#ifdef DEBUG_PARAM_COMPUTE
+ mySumDist += distance();
+ cout << " ------ SOLUTION: ( "<< myParam.X() <<" "<< myParam.Y() <<" "<< myParam.Z() <<" )"<<endl
+ << " ------ DIST : " << distance() << "\t Tol=" << myTolerance << "\t Nb LOOPS=" << nbLoops << endl
+ << " ------ NB IT: " << myNbIterations << ", SUM DIST: " << mySumDist << endl;
+#endif
+
+ theParams = myParam;
+
+ if ( myFaceIndex > 0 )
+ theParams.SetCoord( myFaceIndex, myFaceParam );
+
+ return true;
+}
+
//=======================================================================
//function : ComputeParameters
//purpose : compute point parameters in the block
return false;
}
- myPoint = thePoint.XYZ();
-
- myParam.SetCoord( -1,-1,-1 );
- myValues[ SQUARE_DIST ] = 1e100;
-
const bool isOnFace = IsFaceID( theShapeID );
double * coef = GetShapeCoef( theShapeID );
// Find the first guess paremeters
- math_Vector start( 1, 3, 0.0 );
+ gp_XYZ start(0, 0, 0);
bool hasHint = ( 0 <= theParamsHint.X() && theParamsHint.X() <= 1 &&
0 <= theParamsHint.Y() && theParamsHint.Y() <= 1 &&
iEdge += 4;
continue;
}
+ double sumParam = 0;
for ( int iE = 0; iE < 4; iE++, iEdge++ ) { // loop on 4 parallel edges
gp_Pnt p0 = myEdge[ iEdge ].Point( par000 );
gp_Pnt p1 = myEdge[ iEdge ].Point( par111 );
break;
}
}
- start( iParam ) += par;
+ sumParam += par;
}
- start( iParam ) /= 4.;
+ start.SetCoord( iParam, sumParam / 4.);
}
if ( needGrid ) {
// compute nodes of 3 x 3 x 3 grid
myTolerance = sqrt( box.SquareExtent() ) * 1e-5;
}
}
- if ( myTolerance < 0 ) myTolerance = 1e-6;
- double loopTol = 10 * myTolerance; // to stop restarting solution search
if ( hasHint )
{
- gp_Pnt hintPoint = myPoint;
- ShellPoint( theParamsHint, hintPoint.ChangeCoord() );
- double sqDist = hintPoint.SquareDistance( myPoint );
- if ( sqrt( sqDist ) < loopTol ) {
- // theParamsHint is good, no need to solve
- myValues[ SQUARE_DIST ] = sqDist;
- myParam = theParamsHint;
- }
- start( 1 ) = theParamsHint.X();
- start( 2 ) = theParamsHint.Y();
- start( 3 ) = theParamsHint.Z();
+ start = theParamsHint;
}
else if ( myGridComputed )
{
bestParam = & prmPtn.first;
}
}
- start( 1 ) = bestParam->X();
- start( 2 ) = bestParam->Y();
- start( 3 ) = bestParam->Z();
+ start = *bestParam;
}
- myFaceIndex = -1;
+ int myFaceIndex = -1;
+ double myFaceParam = 0.;
if ( isOnFace ) {
// put a point on the face
for ( int iCoord = 0; iCoord < 3; iCoord++ )
if ( coef[ iCoord ] ) {
- myFaceIndex = iCoord;
- myFaceParam = ( coef[ myFaceIndex ] < 0.5 ) ? 0.0 : 1.0;
- start( iCoord + 1 ) = myFaceParam;
+ myFaceIndex = iCoord + 1;
+ myFaceParam = ( coef[ iCoord ] < 0.5 ) ? 0.0 : 1.0;
+ start.SetCoord( myFaceIndex, myFaceParam );
}
}
#ifdef DEBUG_PARAM_COMPUTE
cout << " #### POINT " <<thePoint.X()<<" "<<thePoint.Y()<<" "<<thePoint.Z()<<" ####"<< endl;
- cout << " ** START ** " << start(1) << " " << start(2) << " " << start(3) << " " << endl;
#endif
- math_Vector low ( 1, 3, 0.0 );
- math_Vector up ( 1, 3, 1.0 );
- math_Vector tol ( 1, 3, 1e-4 );
- math_FunctionSetRoot paramSearch( *this, tol );
+ if ( myTolerance < 0 ) myTolerance = 1e-6;
- mySquareFunc = 0; // large approaching steps
- //if ( hasHint ) mySquareFunc = 1; // small approaching steps
+ const double parDelta = 1e-4;
+ const double sqTolerance = myTolerance * myTolerance;
- int nbLoops = 0;
- while ( distance() > loopTol && nbLoops <= 3 )
+ gp_XYZ solution = start, params = start;
+ double sqDistance = 1e100;
+ int nbLoops = 0, nbGetWorst = 0;
+
+ while ( nbLoops <= 100 )
{
- paramSearch.Perform ( *static_cast<math_FunctionSetWithDerivatives*>(this),
- start, low, up );
- start( 1 ) = myParam.X();
- start( 2 ) = myParam.Y();
- start( 3 ) = myParam.Z();
- mySquareFunc = !mySquareFunc;
+ gp_XYZ P, Pi;
+ ShellPoint( params, P );
+
+ gp_Vec dP( thePoint, P );
+ double sqDist = dP.SquareMagnitude();
+
+ if ( sqDist > sqDistance ) { // solution get worse
+ if ( ++nbGetWorst > 2 )
+ return computeParameters( thePoint, theParams, solution );
+ }
+#ifdef DEBUG_PARAM_COMPUTE
+ cout << "PARAMS: ( " << params.X() <<" "<< params.Y() <<" "<< params.Z() <<" )"<< endl;
+ cout << "DIST: " << sqrt( sqDist ) << endl;
+#endif
+
+ if ( sqDist < sqDistance ) { // get better
+ sqDistance = sqDist;
+ solution = params;
+ nbGetWorst = 0;
+ if ( sqDistance < sqTolerance ) // a solution found
+ break;
+ }
+
+ // look for a next better solution
+ for ( int iP = 1; iP <= 3; iP++ ) {
+ if ( iP == myFaceIndex )
+ continue;
+ // see where we move with a small (=parDelta) step in this direction
+ gp_XYZ nearParams = params;
+ bool onEdge = ( params.Coord( iP ) + parDelta > 1. );
+ if ( onEdge )
+ nearParams.SetCoord( iP, params.Coord( iP ) - parDelta );
+ else
+ nearParams.SetCoord( iP, params.Coord( iP ) + parDelta );
+ ShellPoint( nearParams, Pi );
+ gp_Vec dPi ( P, Pi );
+ if ( onEdge ) dPi *= -1.;
+ // modify a parameter
+ double mag = dPi.Magnitude();
+ if ( mag < DBL_MIN )
+ continue;
+ gp_Vec dir = dPi / mag; // dir we move modifying the parameter
+ double dist = dir * dP; // where we should get to
+ double dPar = dist / mag * parDelta; // predict parameter change
+ double curPar = params.Coord( iP );
+ double par = curPar - dPar; // new parameter value
+ while ( par > 1 || par < 0 ) {
+ dPar /= 2.;
+ par = curPar - dPar;
+ }
+ params.SetCoord( iP, par );
+ }
+
nbLoops++;
}
#ifdef DEBUG_PARAM_COMPUTE
- mySumDist += distance();
- cout << " ------ SOLUTION: ( "<< myParam.X() <<" "<< myParam.Y() <<" "<< myParam.Z() <<" )"<<endl
- << " ------ DIST : " << distance() << "\t Tol=" << myTolerance << "\t Nb LOOPS=" << nbLoops << endl
+ myNbIterations += nbLoops*4; // how many times ShellPoint called
+ mySumDist += sqrt( sqDistance );
+ cout << " ------ SOLUTION: ( "<<solution.X()<<" "<<solution.Y()<<" "<<solution.Z()<<" )"<<endl
+ << " ------ DIST : " << sqrt( sqDistance ) << "\t Tol=" << myTolerance << "\t Nb LOOPS=" << nbLoops << endl
<< " ------ NB IT: " << myNbIterations << ", SUM DIST: " << mySumDist << endl;
#endif
-// if ( nbLoops == 0 && hasHint )
-// theParams = theParamsHint;
-// else
- theParams = myParam;
+ theParams = solution;
- if ( myFaceIndex >= 0 )
- theParams.SetCoord( myFaceIndex + 1, myFaceParam );
+ if ( myFaceIndex > 0 )
+ theParams.SetCoord( myFaceIndex, myFaceParam );
return true;
}
return false;
}
-//=======================================================================
-//function : GetStateNumber
-//purpose :
-//=======================================================================
-
-Standard_Integer SMESH_Block::GetStateNumber ()
-{
- return 0; //myValues[0] < 1e-1;
-}
-
//=======================================================================
//function : DumpShapeID
//purpose : debug an id of a block sub-shape
MESSAGE(" GetEdgeVertexIDs(), wrong edge ID: " << edgeID );
}
}
-
