ResInsight/ApplicationCode/ReservoirDataModel/RigWellPathGeometryTools.cpp

/////////////////////////////////////////////////////////////////////////////////
//
//  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 "RigWellPathGeometryTools.h"

#include "RigWellPath.h"

#include "cvfMath.h"
#include "cvfMatrix3.h"

#include <QDebug>

#include <algorithm>
#include <cmath>

//--------------------------------------------------------------------------------------------------
///
//--------------------------------------------------------------------------------------------------
std::vector<cvf::Vec3d> RigWellPathGeometryTools::calculateLineSegmentNormals(const std::vector<cvf::Vec3d>& vertices,
                                                                              double                         planeAngle)
{
    std::vector<cvf::Vec3d> pointNormals;

    if (vertices.empty()) return pointNormals;

    pointNormals.reserve(vertices.size());

    const cvf::Vec3d up(0, 0, 1);
    const cvf::Vec3d rotatedUp = up.getTransformedVector(cvf::Mat3d::fromRotation(cvf::Vec3d(0.0, 1.0, 0.0), planeAngle));


    const cvf::Vec3d dominantDirection = estimateDominantDirectionInXYPlane(vertices);

    const cvf::Vec3d projectionPlaneNormal = (up ^ dominantDirection).getNormalized();
    CVF_ASSERT(projectionPlaneNormal * dominantDirection <= std::numeric_limits<double>::epsilon());

    double sumDotWithRotatedUp = 0.0;
    for (size_t i = 0; i < vertices.size() - 1; ++i)
    {
        cvf::Vec3d p1 = vertices[i];
        cvf::Vec3d p2 = vertices[i + 1];

        cvf::Vec3d tangent = (p2 - p1).getNormalized();
        cvf::Vec3d normal(0, 0, 0);
        if (cvf::Math::abs(tangent * projectionPlaneNormal) < 0.7071)
        {
            cvf::Vec3d projectedTangent = (tangent - (tangent * projectionPlaneNormal) * projectionPlaneNormal).getNormalized();
            normal                      = (projectedTangent ^ projectionPlaneNormal).getNormalized();
            normal                      = normal.getTransformedVector(cvf::Mat3d::fromRotation(tangent, planeAngle));
        }
        pointNormals.push_back(normal);
        sumDotWithRotatedUp += normal * rotatedUp;
    }
    
    pointNormals.push_back(pointNormals.back());

    if (sumDotWithRotatedUp < 0.0)
    {
        for (cvf::Vec3d& normal : pointNormals)
        {
            normal *= -1.0;            
        }
    }


    return interpolateUndefinedNormals(up, pointNormals, vertices);
}

//--------------------------------------------------------------------------------------------------
/// Lets you estimate MD values from an existing md/tvd relationship and a new set of TVD-values
/// Requires the points to be ordered from the start/top of the well path to the end/bottom.
//--------------------------------------------------------------------------------------------------
std::vector<double> RigWellPathGeometryTools::interpolateMdFromTvd(const std::vector<double>& originalMdValues, const std::vector<double>& originalTvdValues, const std::vector<double>& tvdValuesToInterpolateFrom)
{
    CVF_ASSERT(!originalMdValues.empty());
    if (originalMdValues.size() < 2u)
    {
        return {originalMdValues};
    }

    std::vector<double> interpolatedMdValues;
    interpolatedMdValues.reserve(tvdValuesToInterpolateFrom.size());

    QPolygonF originalPoints;
    for (size_t i = 0; i < originalMdValues.size(); ++i)
    {
        originalPoints << QPointF(originalMdValues[i], originalTvdValues[i]);
    }
    QwtSpline spline;
    spline.setPoints(originalPoints);

    double lastTVDValue = -1.0;
    std::vector<int> segmentStartIndices = findSegmentIndices(originalMdValues, originalTvdValues, tvdValuesToInterpolateFrom);
    for (size_t i = 0; i < segmentStartIndices.size(); ++i)
    {
        double currentTVDValue = tvdValuesToInterpolateFrom[i];
        int startIndex = segmentStartIndices[i];
        int endIndex = startIndex + 1;

        // Search interval for best MD value
        double startMD = originalMdValues[startIndex];
        double endMD;
        
        double mdDiff = 0.0;
        if (interpolatedMdValues.size() > 1)
        {
            mdDiff = interpolatedMdValues[i - 1] - interpolatedMdValues[i - 2];
        }

       
        if (endIndex == originalMdValues.size())
        {
            endMD = originalMdValues[startIndex] + mdDiff;
        }
        else
        {
            if (!interpolatedMdValues.empty())
            {
                startMD = std::max(startMD, interpolatedMdValues.back() + 0.25 * mdDiff);
            }
            endMD = originalMdValues[endIndex] + mdDiff * 0.5;
        }

        double mdValue = solveForX(spline, startMD, endMD, currentTVDValue);
        interpolatedMdValues.push_back(mdValue);

        lastTVDValue = currentTVDValue;
    }
    return interpolatedMdValues;
}

//--------------------------------------------------------------------------------------------------
///
//--------------------------------------------------------------------------------------------------
std::vector<int> RigWellPathGeometryTools::findSegmentIndices(const std::vector<double>& originalMdValues, const std::vector<double>& originalTvdValues, const std::vector<double>& tvdValuesToInterpolateFrom)
{
    CVF_ASSERT(!originalMdValues.empty());
    if (originalMdValues.size() < 2u)
    {
        return {0};
    }

    QPolygonF polygon;
    for (size_t i = 0; i < originalMdValues.size(); ++i)
    {
        polygon << QPointF(originalMdValues[i], originalTvdValues[i]);
    }
    QwtSpline spline;
    spline.setPoints(polygon);
    for (double md = 0.0; md < 1000; md += 50)
    {
        qDebug() << md << ", " << spline.value(md);
    }

    std::vector<int> segmentStartIndices;
    segmentStartIndices.reserve(tvdValuesToInterpolateFrom.size());

    int lastStartIndex = 0;
    for (std::vector<double>::const_iterator it = tvdValuesToInterpolateFrom.begin(); it != tvdValuesToInterpolateFrom.end(); ++it)
    {
        double tvdValue = *it;
        int currentStartIndex = lastStartIndex;
       
        // Increment current_it until we find an interval containing our TVD
        while (currentStartIndex < (int)(originalTvdValues.size() - 1))
        {
            double diffCurrent = originalTvdValues[currentStartIndex] - tvdValue;
            if (std::abs(diffCurrent) < 1.0e-8) // Current is matching the point
            {
                break;
            }

            int nextStartIndex = currentStartIndex + 1;

            double diffNext = originalTvdValues[nextStartIndex] - tvdValue;
            if (diffCurrent * diffNext < 0.0) // One is above, the other is below
            {
                break;
            }
            // Attempt to interpolate
            double mdCurrent = originalMdValues[currentStartIndex];
            double mdNext = originalMdValues[nextStartIndex];
            double tvdCurrent = spline.value(mdCurrent);
            double tvdNext = spline.value(mdNext);
            if (std::abs(tvdCurrent - tvdValue) < std::abs(tvdNext - tvdValue))
            {
                break;
            }
            currentStartIndex = nextStartIndex;
        }
        segmentStartIndices.push_back(currentStartIndex);

        lastStartIndex = currentStartIndex;
    }

    return segmentStartIndices;
}

std::vector<cvf::Vec3d> RigWellPathGeometryTools::interpolateUndefinedNormals(const cvf::Vec3d&              planeNormal,
                                                                              const std::vector<cvf::Vec3d>& normals,
                                                                              const std::vector<cvf::Vec3d>& vertices)
{
    std::vector<cvf::Vec3d> interpolated(normals);
    cvf::Vec3d              lastNormalNonInterpolated(0, 0, 0);
    cvf::Vec3d              lastNormalAny(0, 0, 0);
    double distanceFromLast = 0.0;

    for (size_t i = 0; i < normals.size(); ++i)
    {
        cvf::Vec3d currentNormal       = normals[i];
        bool       currentInterpolated = false;
        if (i > 0)
        {
            distanceFromLast += (vertices[i] - vertices[i - 1]).length();
        }

        if (currentNormal.length() == 0.0) // Undefined. Need to estimate from neighbors.
        {
            currentInterpolated = true;
            currentNormal = planeNormal; // By default use the plane normal

            cvf::Vec3d nextNormal(0, 0, 0);
            double     distanceToNext = 0.0;
            for (size_t j = i + 1; j < normals.size() && nextNormal.length() == 0.0; ++j)
            {
                nextNormal = normals[j];
                distanceToNext += (vertices[j] - vertices[j - 1]).length();
            }

            if (lastNormalNonInterpolated.length() > 0.0 && nextNormal.length() > 0.0)
            {
                // Both last and next are acceptable, interpolate!
                currentNormal = (distanceToNext * lastNormalNonInterpolated + distanceFromLast * nextNormal).getNormalized();
            }
            else if (lastNormalNonInterpolated.length() > 0.0)
            {
                currentNormal = lastNormalNonInterpolated;
            }
            else if (nextNormal.length() > 0.0)
            {
                currentNormal = nextNormal;
            }
        }
        if (i > 0 && currentNormal * lastNormalAny < -std::numeric_limits<double>::epsilon())
        {
            currentNormal *= -1.0;
        }
        if (!currentInterpolated)
        {
            lastNormalNonInterpolated = currentNormal;
            distanceFromLast = 0.0; // Reset distance
        }
        lastNormalAny = currentNormal;
        interpolated[i] = currentNormal;
    }
    return interpolated;
}

cvf::Vec3d RigWellPathGeometryTools::estimateDominantDirectionInXYPlane(const std::vector<cvf::Vec3d>& vertices)
{
    cvf::Vec3d directionSum(0, 0, 0);
    for (size_t i = 1; i < vertices.size(); ++i)
    {
        cvf::Vec3d vec = vertices[i] - vertices[i - 1];
        vec.z()        = 0.0;
        if (directionSum.length() > 0.0 && (directionSum * vec) < 0.0)
        {
            vec *= -1;
        }
        directionSum += vec;
    }
    
    if (directionSum.length() < 1.0e-8)
    {
        directionSum = cvf::Vec3d(0, -1, 0);
    }

    return directionSum.getNormalized();
}

//--------------------------------------------------------------------------------------------------
///
//--------------------------------------------------------------------------------------------------
double RigWellPathGeometryTools::solveForX(const QwtSpline& spline, double minX, double maxX, double y)
{
    const double phi = (1.0 + std::sqrt(5.0)) / 2.0;
    const double tol = 1.0e-8;

    double a = minX, b = maxX;
    double c = b - (b - a) / phi;
    double d = a + (b - a) / phi;

    double fa = spline.value(a) - y;
    double fb = spline.value(b) - y;
    double fc = spline.value(c) - y;
    double fd = spline.value(d) - y;

    for (int n = 0; n < 100; ++n)
    {
        if (std::fabs(c - d) < tol)
        {
            break;
        }
        
        if (std::fabs(fc) < std::fabs(fd))
        {
            b = d;
            fb = fd;
            d = c;
            fd = fc;
            c = b - (b - a) / phi;
            fc = spline.value(c) - y;
        }
        else
        {
            a = c;
            fa = fc;
            c = d;
            fc = fd;
            d = a + (b - a) / phi;
            fd = spline.value(d) - y;
        }
    }
    return (a + b) / 2.0;
}