mirror of
https://github.com/OPM/ResInsight.git
synced 2025-02-25 18:55:39 -06:00
479 lines
18 KiB
C++
479 lines
18 KiB
C++
/////////////////////////////////////////////////////////////////////////////////
|
|
//
|
|
// Copyright (C) 2018- Equinor ASA
|
|
//
|
|
// ResInsight is free software: you can redistribute it and/or modify
|
|
// it under the terms of the GNU General Public License as published by
|
|
// the Free Software Foundation, either version 3 of the License, or
|
|
// (at your option) any later version.
|
|
//
|
|
// ResInsight is distributed in the hope that it will be useful, but WITHOUT ANY
|
|
// WARRANTY; without even the implied warranty of MERCHANTABILITY or
|
|
// FITNESS FOR A PARTICULAR PURPOSE.
|
|
//
|
|
// See the GNU General Public License at <http://www.gnu.org/licenses/gpl.html>
|
|
// for more details.
|
|
//
|
|
/////////////////////////////////////////////////////////////////////////////////
|
|
|
|
#include "RiaSCurveCalculator.h"
|
|
|
|
#include "RiaOffshoreSphericalCoords.h"
|
|
|
|
#include "cvfMatrix4.h"
|
|
|
|
#include <cmath>
|
|
#include <iostream>
|
|
|
|
//--------------------------------------------------------------------------------------------------
|
|
///
|
|
//--------------------------------------------------------------------------------------------------
|
|
RiaSCurveCalculator::RiaSCurveCalculator( cvf::Vec3d p1,
|
|
double azi1,
|
|
double inc1,
|
|
double rad1,
|
|
cvf::Vec3d p2,
|
|
double azi2,
|
|
double inc2,
|
|
double rad2 )
|
|
: m_isCalculationOK( false )
|
|
, m_p1( p1 )
|
|
, m_p2( p2 )
|
|
, m_firstArcEndpoint( p1 + 0.3 * ( p2 - p1 ) )
|
|
, m_secondArcStartpoint( p1 + 0.6 * ( p2 - p1 ) )
|
|
, m_r1( rad1 )
|
|
, m_r2( rad2 )
|
|
, m_ctrlPpointCurveStatus( NOT_SET )
|
|
, m_solveStatus( NOT_SOLVED )
|
|
{
|
|
initializeByFinding_q1q2( p1, azi1, inc1, rad1, p2, azi2, inc2, rad2 );
|
|
}
|
|
|
|
//--------------------------------------------------------------------------------------------------
|
|
///
|
|
//--------------------------------------------------------------------------------------------------
|
|
RiaSCurveCalculator::RiaSCurveCalculator( cvf::Vec3d p1, cvf::Vec3d q1, cvf::Vec3d p2, cvf::Vec3d q2 )
|
|
: m_isCalculationOK( true )
|
|
, m_p1( p1 )
|
|
, m_p2( p2 )
|
|
, m_ctrlPpointCurveStatus( NOT_SET )
|
|
, m_solveStatus( NOT_SOLVED )
|
|
{
|
|
using Vec3d = cvf::Vec3d;
|
|
bool isOk = true;
|
|
m_isCalculationOK = true;
|
|
|
|
Vec3d tq1q2 = ( q2 - q1 ).getNormalized( &isOk ); // !ok means the control points are in the same place. Could
|
|
// fallback to use only one circle segment + one line.
|
|
m_isCalculationOK = m_isCalculationOK && isOk;
|
|
Vec3d t1 = ( q1 - p1 ).getNormalized( &isOk ); // !ok means no tangent specified. Could fallback to use only one
|
|
// circle segment + one line.
|
|
m_isCalculationOK = m_isCalculationOK && isOk;
|
|
Vec3d t2 = ( p2 - q2 ).getNormalized( &isOk ); // !ok means no tangent specified. Could fallback to use only one
|
|
// circle segment + one line or only one straight line if both
|
|
// tangents are missing
|
|
m_isCalculationOK = m_isCalculationOK && isOk;
|
|
|
|
if ( !m_isCalculationOK )
|
|
{
|
|
m_ctrlPpointCurveStatus = FAILED_INPUT_OVERLAP;
|
|
}
|
|
|
|
{
|
|
Vec3d td1 = ( tq1q2 - t1 );
|
|
double td1Length = td1.length();
|
|
|
|
if ( td1Length > 1e-10 )
|
|
{
|
|
td1 /= td1Length;
|
|
m_c1 = q1 + ( ( q1 - p1 ).length() / ( td1 * ( -t1 ) ) ) * td1;
|
|
m_r1 = ( m_c1 - p1 ).length();
|
|
}
|
|
else // both control points are along t1. First curve has infinite radius
|
|
{
|
|
m_c1 = cvf::Vec3d::UNDEFINED;
|
|
m_r1 = std::numeric_limits<double>::infinity();
|
|
|
|
if ( m_ctrlPpointCurveStatus == NOT_SET )
|
|
{
|
|
m_ctrlPpointCurveStatus = OK_INFINITE_RADIUS1;
|
|
}
|
|
}
|
|
}
|
|
|
|
{
|
|
Vec3d td2 = ( -tq1q2 + t2 );
|
|
double td2Length = td2.length();
|
|
|
|
if ( td2Length > 1e-10 )
|
|
{
|
|
td2 /= td2Length;
|
|
m_c2 = q2 + ( ( q2 - p2 ).length() / ( td2 * ( t2 ) ) ) * td2;
|
|
m_r2 = ( m_c2 - p2 ).length();
|
|
}
|
|
else // both control points are along t2. Second curve has infinite radius
|
|
{
|
|
m_c2 = cvf::Vec3d::UNDEFINED;
|
|
m_r2 = std::numeric_limits<double>::infinity();
|
|
|
|
if ( m_ctrlPpointCurveStatus == NOT_SET )
|
|
{
|
|
m_ctrlPpointCurveStatus = OK_INFINITE_RADIUS2;
|
|
}
|
|
else if ( m_ctrlPpointCurveStatus == OK_INFINITE_RADIUS1 )
|
|
{
|
|
m_ctrlPpointCurveStatus = OK_INFINITE_RADIUS12;
|
|
}
|
|
}
|
|
}
|
|
|
|
m_firstArcEndpoint = q1 + ( q1 - p1 ).length() * tq1q2;
|
|
m_secondArcStartpoint = q2 - ( q2 - p2 ).length() * tq1q2;
|
|
|
|
if ( ( ( q1 - p1 ).length() + ( q2 - p2 ).length() ) > ( q2 - q1 ).length() ) // first arc end and second arc start
|
|
// is overlapping
|
|
{
|
|
m_ctrlPpointCurveStatus = FAILED_ARC_OVERLAP;
|
|
m_isCalculationOK = false;
|
|
}
|
|
|
|
if ( m_ctrlPpointCurveStatus == NOT_SET )
|
|
{
|
|
m_ctrlPpointCurveStatus = OK;
|
|
}
|
|
|
|
// The Circle normals. Will be set to cvf::Vec3d::ZERO if undefined.
|
|
|
|
m_n1 = ( t1 ^ tq1q2 ).getNormalized();
|
|
m_n2 = ( tq1q2 ^ t2 ).getNormalized();
|
|
}
|
|
|
|
//--------------------------------------------------------------------------------------------------
|
|
///
|
|
//--------------------------------------------------------------------------------------------------
|
|
RiaSCurveCalculator RiaSCurveCalculator::fromTangentsAndLength( cvf::Vec3d p1,
|
|
double azi1,
|
|
double inc1,
|
|
double lengthToQ1,
|
|
cvf::Vec3d p2,
|
|
double azi2,
|
|
double inc2,
|
|
double lengthToQ2 )
|
|
{
|
|
cvf::Vec3d t1( RiaOffshoreSphericalCoords::unitVectorFromAziInc( azi1, inc1 ) );
|
|
cvf::Vec3d t2( RiaOffshoreSphericalCoords::unitVectorFromAziInc( azi2, inc2 ) );
|
|
|
|
cvf::Vec3d Q1 = p1 + lengthToQ1 * t1;
|
|
cvf::Vec3d Q2 = p2 - lengthToQ2 * t2;
|
|
|
|
RiaSCurveCalculator curveFromControlPoints( p1, Q1, p2, Q2 );
|
|
|
|
return curveFromControlPoints;
|
|
}
|
|
|
|
//--------------------------------------------------------------------------------------------------
|
|
///
|
|
/// Needs to calculate J^-1 * [R1_error, R2_error]
|
|
/// | dR1_dq1 dR1_dq2 | 1 | dR2_dq2 -dR1_dq2 |
|
|
/// J = | | J^-1 = ---------------------------------- | |
|
|
/// | dR2_dq1 dR2_dq2 | dR1_dq1*dR2_dq2 - dR1_dq2*dR2_dq1 | -dR2_dq1 dR1_dq1 |
|
|
///
|
|
/// | q1_step | | R1_Error |
|
|
/// | | = - J^-1 | |
|
|
/// | q2_step | | R2_Error |
|
|
///
|
|
//--------------------------------------------------------------------------------------------------
|
|
void calculateNewStepsFromJacobi( double dR1_dq1,
|
|
double dR1_dq2,
|
|
double dR2_dq1,
|
|
double dR2_dq2,
|
|
double R1_error,
|
|
double R2_error,
|
|
double* newStepq1,
|
|
double* newStepq2 )
|
|
{
|
|
double invJacobiScale = 1.0 / ( dR1_dq1 * dR2_dq2 - dR2_dq1 * dR1_dq2 );
|
|
double invJacobi_R1q1 = invJacobiScale * dR2_dq2;
|
|
double invJacobi_R1q2 = invJacobiScale * -dR1_dq2;
|
|
double invJacobi_R2q1 = invJacobiScale * -dR2_dq1;
|
|
double invJacobi_R2q2 = invJacobiScale * dR1_dq1;
|
|
|
|
( *newStepq1 ) = -( invJacobi_R1q1 * R1_error + invJacobi_R1q2 * R2_error );
|
|
( *newStepq2 ) = -( invJacobi_R2q1 * R1_error + invJacobi_R2q2 * R2_error );
|
|
}
|
|
|
|
//--------------------------------------------------------------------------------------------------
|
|
///
|
|
//--------------------------------------------------------------------------------------------------
|
|
bool isZeroCrossing( double newError, double oldError, double maxError )
|
|
{
|
|
if ( ( newError < -maxError && maxError < oldError ) || ( newError > maxError && -maxError > oldError ) )
|
|
{
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
//--------------------------------------------------------------------------------------------------
|
|
///
|
|
/// Iterating with changing q1, q2 (lengths along tangent) to find solution with R1 = r1 and R2 = r2
|
|
/// R1(q1, q2), R2(q1, q2)
|
|
///
|
|
//--------------------------------------------------------------------------------------------------
|
|
void RiaSCurveCalculator::initializeByFinding_q1q2( cvf::Vec3d p1,
|
|
double azi1,
|
|
double inc1,
|
|
double r1,
|
|
cvf::Vec3d p2,
|
|
double azi2,
|
|
double inc2,
|
|
double r2 )
|
|
{
|
|
// Algorithm options
|
|
|
|
const int maxIterations = 40;
|
|
const double maxError = 0.01;
|
|
const double maxStepSize = 1.0e9;
|
|
const double maxLengthToQ = 1.0e10;
|
|
bool enableBackstepping = false;
|
|
//#define USE_JACOBI_UPDATE
|
|
//#define DEBUG_OUTPUT_ON
|
|
|
|
// Needs the initial partial derivatives to see the direction of change
|
|
// dR1/dq1, dR1/dq2, dR2/dq1, dR2/dq2
|
|
// Selects a sensible point in the domain for the evaluation
|
|
|
|
double p1p2Length = ( p2 - p1 ).length();
|
|
double delta = 0.01 * p1p2Length;
|
|
double initialq1q2 = 0.2 * p1p2Length;
|
|
double deltaPos = initialq1q2 + delta;
|
|
|
|
RiaSCurveCalculator ev_0 =
|
|
RiaSCurveCalculator::fromTangentsAndLength( p1, azi1, inc1, initialq1q2, p2, azi2, inc2, initialq1q2 );
|
|
|
|
if ( ev_0.curveStatus() == RiaSCurveCalculator::OK_INFINITE_RADIUS12 )
|
|
{
|
|
*this = ev_0;
|
|
this->m_solveStatus = CONVERGED;
|
|
return;
|
|
} // Todo: Handle infinite radius in one place
|
|
|
|
RiaSCurveCalculator ev_dq1 =
|
|
RiaSCurveCalculator::fromTangentsAndLength( p1, azi1, inc1, deltaPos, p2, azi2, inc2, initialq1q2 );
|
|
RiaSCurveCalculator ev_dq2 =
|
|
RiaSCurveCalculator::fromTangentsAndLength( p1, azi1, inc1, initialq1q2, p2, azi2, inc2, deltaPos );
|
|
|
|
// Initial Jacobi
|
|
double dR1_dq1 = ( ( r1 - ev_dq1.firstRadius() ) - ( r1 - ev_0.firstRadius() ) ) / delta;
|
|
double dR2_dq2 = ( ( r2 - ev_dq2.secondRadius() ) - ( r2 - ev_0.secondRadius() ) ) / delta;
|
|
|
|
// Initial function value (error)
|
|
double R1_error = r1 - ev_0.firstRadius();
|
|
double R2_error = r2 - ev_0.secondRadius();
|
|
|
|
// First steps
|
|
double q1Step = -R1_error / dR1_dq1;
|
|
double q2Step = -R2_error / dR2_dq2;
|
|
|
|
#ifdef USE_JACOBI_UPDATE
|
|
double dR1_dq2 = ( ( r1 - ev_dq2.firstRadius() ) - ( r1 - ev_0.firstRadius() ) ) / delta;
|
|
double dR2_dq1 = ( ( r2 - ev_dq1.secondRadius() ) - ( r2 - ev_0.secondRadius() ) ) / delta;
|
|
|
|
calculateNewStepsFromJacobi( dR1_dq1, dR1_dq2, dR2_dq1, dR2_dq2, R1_error, R2_error, &q1Step, &q2Step );
|
|
#endif
|
|
|
|
double q1 = initialq1q2;
|
|
double q2 = initialq1q2;
|
|
|
|
#ifdef DEBUG_OUTPUT_ON
|
|
std::cout << std::endl;
|
|
std::cout << "Targets: R1, R2: " << r1 << " , " << r2 << std::endl;
|
|
|
|
std::cout << 0 << ": " << q1Step << " , " << q2Step << " : " << q1 << " , " << q2 << " | " << ev_0.isOk() << " : "
|
|
<< ev_0.firstRadius() << " , " << ev_0.secondRadius() << " : " << R1_error << " , " << R2_error
|
|
<< std::endl;
|
|
#endif
|
|
|
|
SolveStatus solveResultStatus = NOT_SOLVED;
|
|
|
|
int backstepLevel = 0;
|
|
int iteration = 1;
|
|
for ( iteration = 1; iteration < maxIterations; ++iteration )
|
|
{
|
|
if ( fabs( q1Step ) > maxStepSize || fabs( q2Step ) > maxStepSize )
|
|
{
|
|
solveResultStatus = FAILED_MAX_TANGENT_STEP_REACHED;
|
|
|
|
break;
|
|
}
|
|
|
|
std::string q1R1StepCorrMarker;
|
|
std::string q2R2StepCorrMarker;
|
|
|
|
if ( q1 + q1Step < 0 )
|
|
{
|
|
q1Step = -0.9 * q1;
|
|
q1R1StepCorrMarker = "*";
|
|
}
|
|
if ( q2 + q2Step < 0 )
|
|
{
|
|
q2Step = -0.9 * q2;
|
|
q2R2StepCorrMarker = "*";
|
|
}
|
|
|
|
q1 += q1Step;
|
|
q2 += q2Step;
|
|
|
|
if ( fabs( q1 ) > maxLengthToQ || fabs( q2 ) > maxLengthToQ )
|
|
{
|
|
/// Max length along tangent reached
|
|
solveResultStatus = FAILED_MAX_LENGTH_ALONG_TANGENT_REACHED;
|
|
break;
|
|
}
|
|
|
|
RiaSCurveCalculator ev_1 = RiaSCurveCalculator::fromTangentsAndLength( p1, azi1, inc1, q1, p2, azi2, inc2, q2 );
|
|
|
|
double R1_error_new = r1 - ev_1.firstRadius();
|
|
double R2_error_new = r2 - ev_1.secondRadius();
|
|
|
|
#ifdef DEBUG_OUTPUT_ON
|
|
std::cout << iteration << ": " << q1Step << q1R1StepCorrMarker << " , " << q2Step << q2R2StepCorrMarker << " : "
|
|
<< q1 << " , " << q2 << " | " << ev_1.isOk() << " : " << ev_1.firstRadius() << " , "
|
|
<< ev_1.secondRadius() << " : " << R1_error_new << " , " << R2_error_new;
|
|
#endif
|
|
|
|
if ( ( fabs( R1_error_new ) < maxError || ev_1.curveStatus() == OK_INFINITE_RADIUS1 ) &&
|
|
( fabs( R2_error_new ) < maxError || ev_1.curveStatus() == OK_INFINITE_RADIUS2 ) )
|
|
{
|
|
ev_0 = ev_1;
|
|
|
|
// Result ok !
|
|
|
|
solveResultStatus = CONVERGED;
|
|
|
|
#ifdef DEBUG_OUTPUT_ON
|
|
std::cout << std::endl;
|
|
#endif
|
|
|
|
break;
|
|
}
|
|
|
|
if ( enableBackstepping ) // Experimental back-stepping
|
|
{
|
|
bool isZeroCrossingR1 = isZeroCrossing( R1_error_new, R1_error, maxError );
|
|
bool isZeroCrossingR2 = isZeroCrossing( R2_error_new, R2_error, maxError );
|
|
|
|
if ( isZeroCrossingR2 || isZeroCrossingR1 )
|
|
{
|
|
q1 -= q1Step;
|
|
q2 -= q2Step;
|
|
|
|
// if (isZeroCrossingR1)
|
|
q1Step = 0.9 * q1Step * fabs( R1_error ) / ( fabs( R1_error_new ) + fabs( R1_error ) );
|
|
// if (isZeroCrossingR2)
|
|
q2Step = 0.9 * q2Step * fabs( R2_error ) / ( fabs( R2_error_new ) + fabs( R2_error ) );
|
|
|
|
++backstepLevel;
|
|
|
|
#ifdef DEBUG_OUTPUT_ON
|
|
std::cout << " Backstep needed. " << std::endl;
|
|
#endif
|
|
|
|
continue;
|
|
}
|
|
else
|
|
{
|
|
backstepLevel = 0;
|
|
}
|
|
}
|
|
|
|
#ifdef DEBUG_OUTPUT_ON
|
|
std::cout << std::endl;
|
|
#endif
|
|
|
|
#ifdef USE_JACOBI_UPDATE
|
|
|
|
/// Update Jacobi using Broyden
|
|
// (R_error_n-Rerror_n-1) - Jn-1*dq
|
|
// J_n = Jn-1 + --------------------------------- (dq)T
|
|
// | dqn |^2
|
|
//
|
|
|
|
double dR1_error = R1_error_new - R1_error;
|
|
double dR2_error = R2_error_new - R2_error;
|
|
R1_error = R1_error_new;
|
|
R2_error = R2_error_new;
|
|
|
|
double stepNormScale = 1.0 / ( q1Step * q1Step + q2Step * q2Step );
|
|
|
|
dR1_dq1 = dR1_dq1 + stepNormScale * ( q1Step * ( dR1_error - q1Step * dR1_dq1 + q2Step * dR1_dq2 ) );
|
|
dR1_dq2 = dR1_dq2 + stepNormScale * ( q2Step * ( dR1_error - q1Step * dR1_dq1 + q2Step * dR1_dq2 ) );
|
|
dR2_dq1 = dR2_dq1 + stepNormScale * ( q1Step * ( dR2_error - q1Step * dR2_dq1 + q2Step * dR2_dq2 ) );
|
|
dR2_dq2 = dR2_dq2 + stepNormScale * ( q2Step * ( dR2_error - q1Step * dR2_dq1 + q2Step * dR2_dq2 ) );
|
|
|
|
calculateNewStepsFromJacobi( dR1_dq1, dR1_dq2, dR2_dq1, dR2_dq2, R1_error, R2_error, &q1Step, &q2Step );
|
|
|
|
#else
|
|
|
|
dR1_dq1 = ( ( r1 - ev_1.firstRadius() ) - ( r1 - ev_0.firstRadius() ) ) / q1Step;
|
|
dR2_dq2 = ( ( r2 - ev_1.secondRadius() ) - ( r2 - ev_0.secondRadius() ) ) / q2Step;
|
|
|
|
R1_error = R1_error_new;
|
|
R2_error = R2_error_new;
|
|
|
|
q1Step = -R1_error / dR1_dq1;
|
|
q2Step = -R2_error / dR2_dq2;
|
|
|
|
#endif
|
|
|
|
ev_0 = ev_1;
|
|
}
|
|
|
|
*this = ev_0;
|
|
if ( iteration >= maxIterations )
|
|
{
|
|
m_solveStatus = FAILED_MAX_ITERATIONS_REACHED;
|
|
// Max iterations reached
|
|
}
|
|
else
|
|
{
|
|
m_solveStatus = solveResultStatus;
|
|
}
|
|
}
|
|
|
|
//--------------------------------------------------------------------------------------------------
|
|
///
|
|
//--------------------------------------------------------------------------------------------------
|
|
void RiaSCurveCalculator::dump() const
|
|
{
|
|
cvf::Vec3d v_C1 = firstCenter();
|
|
cvf::Vec3d v_C2 = secondCenter();
|
|
cvf::Vec3d v_N1 = firstNormal();
|
|
cvf::Vec3d v_N2 = secondNormal();
|
|
cvf::Vec3d v_P11 = firstArcEndpoint();
|
|
cvf::Vec3d v_P22 = secondArcStartpoint();
|
|
|
|
std::cout << " P1: "
|
|
<< "[ " << m_p1[0] << " " << m_p1[1] << " " << m_p1[2] << " " << std::endl;
|
|
std::cout << " P11: "
|
|
<< "[ " << v_P11[0] << " " << v_P11[1] << " " << v_P11[2] << " " << std::endl;
|
|
std::cout << " P22: "
|
|
<< "[ " << v_P22[0] << " " << v_P22[1] << " " << v_P22[2] << " " << std::endl;
|
|
std::cout << " P2: "
|
|
<< "[ " << m_p2[0] << " " << m_p2[1] << " " << m_p2[2] << " " << std::endl;
|
|
std::cout << " C1: "
|
|
<< "[ " << v_C1[0] << " " << v_C1[1] << " " << v_C1[2] << " " << std::endl;
|
|
std::cout << " C2: "
|
|
<< "[ " << v_C2[0] << " " << v_C2[1] << " " << v_C2[2] << " " << std::endl;
|
|
std::cout << " N1: "
|
|
<< "[ " << v_N1[0] << " " << v_N1[1] << " " << v_N1[2] << " " << std::endl;
|
|
std::cout << " N2: "
|
|
<< "[ " << v_N2[0] << " " << v_N2[1] << " " << v_N2[2] << " " << std::endl;
|
|
std::cout << " R1: "
|
|
<< "[ " << firstRadius() << " ]" << std::endl;
|
|
std::cout << " R2: "
|
|
<< "[ " << secondRadius() << " ]" << std::endl;
|
|
std::cout << " CtrPointStatus: " << m_ctrlPpointCurveStatus << " SolveStatus: " << m_solveStatus << std::endl;
|
|
}
|