misc.cpp

Go to the documentation of this file.

/* misc.cpp :  various  numerical functions
 * Copyright (C) 2020  Daniel Haley
 * 
 * This program 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.
 * 
 * This program 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 for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "atomprobe/helper/maths/misc.h"

#include "gsl/gsl_linalg.h"
#include "atomprobe/helper/aptAssert.h"
namespace AtomProbe
{
void gsl_matrix_mult(const gsl_matrix *A, const gsl_matrix *B, gsl_matrix* &res,
    bool alloc)
{
    //Check matrices are correctly sized, and allocate as needed
    ASSERT(B->size1 ==A->size2);
    if(alloc)
    {
        res = gsl_matrix_alloc(A->size1,B->size2);
    }
    else
    {
        ASSERT(res->size1 == A->size1 &&
            res->size2 == B->size2);
    }

    //Naïve matrix mult
    for(unsigned int ui=0; ui<A->size1; ui++)
    {
        for(unsigned int uj=0;uj<B->size2; uj++)
        {
            double cuml;
            cuml=0.0;
            for(unsigned int uk=0;uk<B->size1; uk++)
            {
                cuml+= gsl_matrix_get(A,ui,uk)
                    *gsl_matrix_get(B,uk,uj);
            }
            
            gsl_matrix_set(res, ui,uj,cuml);    
        }
    }
}


unsigned int estimateRank(const gsl_matrix *m, float tolerance)
{

    auto w = gsl_vector_alloc(m->size2);
    auto s = gsl_vector_alloc(m->size2);

    auto v = gsl_matrix_alloc(m->size2,m->size2);
    
    auto m2 = gsl_matrix_alloc(m->size1,m->size2);
    gsl_matrix_memcpy(m2,m);
    if(gsl_linalg_SV_decomp(m2,v,s,w))
    {
        gsl_vector_free(w);
        gsl_vector_free(s);
        gsl_matrix_free(v);

        gsl_matrix_free(m2);
        return -1;
    }
    unsigned int rank=0;
    for(auto ui=0;ui<s->size; ui++)
    {
        if(gsl_vector_get(s,ui) > tolerance) 
            rank=ui+1;
        else
            break;
    }

    gsl_vector_free(w);
    gsl_vector_free(s);
    gsl_matrix_free(v);
    gsl_matrix_free(m2);

    return rank;
}

bool solveLeastSquares(gsl_matrix *m, gsl_vector *b, gsl_vector* &x)
{
    auto w = gsl_vector_alloc(m->size2);
    auto s = gsl_vector_alloc(m->size2);

    auto v = gsl_matrix_alloc(m->size2,m->size2);
    
    if(gsl_linalg_SV_decomp(m,v,s,w))
    {
        gsl_vector_free(w);
        gsl_vector_free(s);
        gsl_matrix_free(v);
        return false;
    }


    if(gsl_linalg_SV_solve(m, v, s, b, x))
    {
        gsl_vector_free(x);
        gsl_vector_free(w);
        gsl_vector_free(s);
        gsl_matrix_free(v);
        return false;
    }

    gsl_vector_free(w);
    gsl_vector_free(s);
    gsl_matrix_free(v);


    return true;
}

unsigned int vectorPointDir(const Point3D &pA, const Point3D &pB, 
                const Point3D &vC, const Point3D &vD)
{
    //Check which way vectors attached to two 3D points "point", 
    // - "together", "apart" or "in common"
    
    //calculate AB.CA, BA.DB
    float dot1  = (pB-pA).dotProd(vC - pA);
    float dot2= (pA - pB).dotProd(vD - pB);

    //We shall somehwat arbitrarily call perpendicular cases "together"
    if(dot1 ==0.0f || dot2 == 0.0f)
        return POINTDIR_TOGETHER;

    //If they have opposite signs, then they are "in common"
    if(( dot1  < 0.0f  && dot2 > 0.0f) || (dot1 > 0.0f && dot2 < 0.0f) )
        return POINTDIR_IN_COMMON;

    if( dot1 < 0.0f && dot2 <0.0f )
        return POINTDIR_APART; 

    if(dot1 > 0.0f && dot2 > 0.0f )
        return POINTDIR_TOGETHER;

    ASSERT(false)
    return 0; 
}

//Distance between a line segment and a point in 3D space
float distanceToSegment(const Point3D &fA, const Point3D &fB, const Point3D &p)
{

    //If the vectors ar pointing "together" then use  point-line formula
    if(vectorPointDir(fA,fB,p,p) == POINTDIR_TOGETHER)
    {
        Point3D closestPt;
        Point3D vAB= fB-fA;

        //Use formula d^2 = |(B-A)(cross)(A-P)|^2/|B-A|^2
        return sqrt( (vAB.crossProd(fA-p)).sqrMag()/(vAB.sqrMag()));
    }

    return sqrt( std::min(fB.sqrDist(p), fA.sqrDist(p)) );
}

float signedDistanceToFacet(const Point3D &fA, const Point3D &fB, 
            const Point3D &fC,  const Point3D &normal,const Point3D &p)
{

    Point3D pTri[3];
    pTri[0]=fA;
    pTri[1]=fB;
    pTri[2]=fC;

    bool insideTri;

    float baryCentricCoord[3];
    for(size_t ui=0; ui<3;ui++)
    {
        //Find the midpoint of the triangle on this axis
        Point3D mid,tmp;
        mid = (pTri[(ui+1)%3] -pTri[ui])*0.5f + pTri[ui];
        //Vector between current "apex" and midpoint of opposite side
        tmp = (pTri[(ui+2)%3]-mid);

        baryCentricCoord[ui] = tmp.dotProd(p -mid)/(tmp.mag());
    }

    insideTri=true;
    for(size_t ui=0;ui<3;ui++)
    {
        insideTri&= (0.0f <=baryCentricCoord[ui] &&  
                baryCentricCoord[ui] <=1.0f);
    }
        

    //Check to see if any of them are "in common"
    if(!insideTri)
    {
        //if so, we have to check each edge for its closest point
        //then pick the best
        float bestDist[3];
        bestDist[0] = distanceToSegment(fA,fB,p);
        bestDist[1] = distanceToSegment(fA,fC,p);
        bestDist[2] = distanceToSegment(fB,fC,p);


        float minV=std::numeric_limits<float>::max();
        size_t offset;
        for(size_t ui=0;ui<3;ui++)
        {
            if( bestDist[ui] < minV)
            {
                offset=ui;
                minV=bestDist[ui];
            }
        }

        //Check which side of the triangle it sits on
        // using one of the vertices in the plane of the triangle 
        //(any, doesn't matter which (ign. fp..) )
        float sign;
        sign =normal.dotProd(p-fA);
        if(sign > 0)
            return bestDist[offset];
        else
            return -bestDist[offset];
    }

    float temp;

    temp = (p-fA).dotProd(normal);

    //check that the other points were not better than this!
    ASSERT(sqrt(fA.sqrDist(p)) >= fabs(temp) - sqrt(std::numeric_limits<float>::epsilon()));
    ASSERT(sqrt(fB.sqrDist(p)) >= fabs(temp) - sqrt(std::numeric_limits<float>::epsilon()));
    ASSERT(sqrt(fC.sqrDist(p)) >= fabs(temp) - sqrt(std::numeric_limits<float>::epsilon()));

    //Point lies above/below facet, use plane formula
    return temp; 
}


float distanceToFacet(const Point3D &fA, const Point3D &fB, 
            const Point3D &fC, const Point3D &normal,const Point3D &p)
{
    return fabs(signedDistanceToFacet(fA,fB,fC,normal,p));
}

double det3by3(const double *ptArray)
{
    return (ptArray[0]*(ptArray[4]*ptArray[8]
                    - ptArray[7]*ptArray[5]) 
        - ptArray[1]*(ptArray[3]*ptArray[8]
                    - ptArray[6]*ptArray[5]) 
        + ptArray[2]*(ptArray[3]*ptArray[7] 
                - ptArray[4]*ptArray[6]));
}

bool triIsDegenerate(const Point3D &fA, const Point3D &fB, 
            const Point3D &fC)
{
    Point3D pTri[3];
    pTri[0]=fA;
    pTri[1]=fB;
    pTri[2]=fC;

    for(size_t ui=0; ui<3;ui++)
    {
        //Find the midpoint of the triangle on this axis
        Point3D mid,tmp;
        mid = (pTri[(ui+1)%3] -pTri[ui])*0.5f + pTri[ui];
        //Vector between current "apex" and midpoint of opposite side
        tmp = (pTri[(ui+2)%3]-mid);

        if(tmp.mag() < sqrt(std::numeric_limits<float>::epsilon()))
            return true;

    }

    return false;
}

double pyramidVol(const Point3D *planarPts, const Point3D &apex)
{

    //Array for 3D simplex volumed determination
    //      | (a_x - b_x)   (b_x - c_x)   (c_x - d_x) |
    //v_simplex =1/6| (a_y - b_y)   (b_y - c_y)   (c_y - d_y) |
    //      | (a_z - b_z)   (b_z - c_z)   (c_z - d_z) |
    double simplexA[9];

    //simplex A (a,b,c,apex) is as follows
    //a=planarPts[0] b=planarPts[1] c=planarPts[2]
    
    simplexA[0] = (double)( (planarPts[0])[0] - (planarPts[1])[0] );
    simplexA[1] = (double)( (planarPts[1])[0] - (planarPts[2])[0] );
    simplexA[2] = (double)( (planarPts[2])[0] - (apex)[0] );
    
    simplexA[3] = (double)( (planarPts[0])[1] - (planarPts[1])[1] );
    simplexA[4] = (double)( (planarPts[1])[1] - (planarPts[2])[1] );
    simplexA[5] = (double)( (planarPts[2])[1] - (apex)[1] );
    
    simplexA[6] = (double)( (planarPts[0])[2] - (planarPts[1])[2] );
    simplexA[7] = (double)( (planarPts[1])[2] - (planarPts[2])[2] );
    simplexA[8] = (double)( (planarPts[2])[2] - (apex)[2] );
    
    return 1.0/6.0 * (fabs(det3by3(simplexA))); 
}
#ifdef DEBUG

bool runMiscMathsTests()
{
    for(auto ui=1;ui<10;ui++)
    {
        gsl_matrix *m;
        m= gsl_matrix_alloc(ui,ui);

        gsl_matrix_set_identity(m);

        ASSERT(estimateRank(m) ==ui);
        gsl_matrix_free(m);
    }


    auto *m = gsl_matrix_alloc(2,2);
    gsl_matrix_set_zero(m);
    ASSERT(estimateRank(m) ==0);
    gsl_matrix_set(m,1,1,0.5);
    ASSERT(estimateRank(m) ==1);
    gsl_matrix_free(m);


    return true;
}
#endif
}