inpoly.c

pathgenerator source, filtering for matlab

        
        cnect[Nnode*2-2] = Nnode;
        
        cnect[Nnode*2-1] = 1.0;
        
    }
    
    else
    {
        
        cnect  = mxGetPr(prhs[2]); // (2 x Ncnect) vector
        
        Ncnect = mxGetN(prhs[2]);
        
    }
    
    
    
    /* Outputs */
    
    plhs[0]   = mxCreateLogicalMatrix(1 , N);
    
    in        = (bool *)mxGetPr(plhs[0]);
    
    
    plhs[1]   = mxCreateLogicalMatrix(1 , N);
    
    bnd       = (bool *)mxGetPr(plhs[1]);
    
    
    
    /* Temporal vectors */
    
    cn        = (bool *)mxMalloc(N*sizeof(bool));
    
    on        = (bool *)mxMalloc(N*sizeof(bool));
    
    x         = (double *)mxMalloc(N*sizeof(double));
    
    y         = (double *)mxMalloc(N*sizeof(double));
    
    index     = (int *)mxMalloc(N*sizeof(int));
    
    
    
    
    for (i = 0; i < 2*N ; i=i+2)
        
    {
        
        temp  = X[i];
        
        
        if(temp < minix)
        {
            
            minix = temp;
        }
        
        if(temp >maxix)
            
        {
            
            maxix = temp;
            
        }
        
        temp  = X[i + 1];
        
        if(temp < miniy)
        {
            
            miniy = temp;
        }
        
        if(temp > maxiy)
            
        {
            
            maxiy = temp;
            
        }
        
    }
    
    for (i = 0 ; i < 2*Nnode ; i = i + 2)
        
    {
        
        temp = fabs(node[i]);
        
        if(temp > norminf)
            
        {
            
            norminf = temp;
        }
        
        temp = fabs(node[i + 1]);
        
        if(temp > norminf)
            
        {
            
            norminf = temp;
            
        }
        
    }
    
    
    tol = norminf*eps;
    
    lim = norminf + tol;
    
    
    if ((maxix - minix) > (maxiy-miniy))
        
    {
        
        for (i = 0; i < 2*N ; i=i+2)
            
        {
            
            temp     = X[i];
            
            X[i]     = X[i + 1];
            
            X[i + 1] = temp;
            
        }
        
        for (i = 0 ; i < 2*Nnode ; i=i+2)
            
        {
            
            temp         = node[i];
            
            node[i]      = node[i + 1];
            
            node[i + 1]  = temp;
            
        }
        
    }
    
    for (i = 0 ; i< N ; i++)
        
    {
        
        cn[i]    = 0;
        
        on[i]    = 0;
        
        y[i]     = X[2*i + 1];
        
        index[i] = i;
        
        
    }
    
    
    
    qsindex( y , index , 0 , N - 1 );
    
    for (i = 0 ; i < N ; i++)
    {
        
        x[i]     = X[2*index[i]];
        
    }
    
    
    
    for (k = 0 ; k < Ncnect ; k++)
        
    {
        
        n1 = (int) cnect[2*k] - 1;
        
        n2 = (int) cnect[2*k + 1] - 1;
        
        x1 = node[2*n1];
        
        y1 = node[2*n1 + 1];
        
        
        x2 = node[2*n2];
        
        y2 = node[2*n2 + 1];
        
        
        if (x1 > x2)
        {
            xmin = x2;
            
            xmax = x1;
        }
        
        else
        {
            xmin = x1;
            
            xmax = x2;
        }
        
        if (y1 > y2)
        {
            ymin = y2;
            
            ymax = y1;
            
        }
        
        else
        {
            ymin = y1;
            
            ymax = y2;
            
        }
        
        if (y[0] == ymin)
        {
            start = 0;
            
        }
        else if (y[N-1] <= ymin)
        {
            
            start = N - 1;
            
        }
        else
            
        {
            
            lower = 0;
            
            upper = N - 1;
            
            start = ((lower+upper)/2);
            
            for (l = 0 ; l < N ; l++)
            {
                if (y[start] < ymin)
                {
                    
                    lower = start;
                    
                    start = ((lower+upper)/2);
                    
                }
                else if (y[start-1]<ymin)
                {
                    
                    break;
                    
                }
                else
                    
                {
                    upper = start;
                    
                    start = ((lower+upper)/2);
                    
                }
            }
            
            start--;
        }
        
        for (j = start ; j < N ; j++)
        {
            
            YY = y[j];
            
            if (YY <= ymax)
            {
                if (YY >= ymin)
                {
                    XX = x[j];
                    
                    if (XX >= xmin)
                    {
                        if (XX <= xmax)
                        {
                            
                            on[j] = on[j] || ( fabs( (y2-YY)*(x1-XX) - (y1-YY)*(x2-XX) )<tol );
                            
                            if (YY < ymax)
                            {
                                ub = ( (x2-x1)*(y1-YY) - (y2-y1)*(x1 - XX) )/( (XX - lim)*(y2-y1) );
                                
                                if ( (ub > -tol) && (ub < (1.0 + tol) ) )
                                {
                                    cn[j] = !cn[j];
                                    
                                }
                            }
                        }
                    }
                    
                    else if (YY < ymax)
                        
                    {
                        
                        cn[j] = !cn[j];
                        
                    }
                    
                }
            }
            else
                
            {
                break;
            }
            
        }
        
    }
    
    
    for(i = 0 ; i < N ; i++)
    {
        
        ind      = index[i];
        
        in[ind]  = (cn[i] || on[i]);
        
        bnd[ind] = on[i];
        
    }
    
    mxFree(cn);
    
    mxFree(on);
    
    mxFree(x);
    
    mxFree(y);
    
    mxFree(index);
    
}


/* ----------------------------------------------------------------------------- */

void qsindex( double *array, int *index , int left, int right )
{
    
    double pivot;	// pivot element.
    
    int ipivot , holex;	// hole index.
    
    int i;
    
    holex          = left + ( right - left ) / 2;
    
    pivot          = array[holex];		     // get pivot from middle of array.
    
    ipivot         = index[holex];
    
    array[holex]   = array[left];              // move "hole" to beginning of
    
    index[holex]   = index[left];              // move "hole" to beginning of
    
    holex          = left;			           // range we are sorting.
    
    for ( i = left + 1 ; i <= right ; i++ )
    {
        if ( array[ i ] <= pivot )
        {
            array[ holex ] = array[ i ];
            
            index[ holex ] = index[ i ];
            
            
            array[ i ]     = array[ ++holex ];
            
            index[ i ]     = index[ holex ];
            
        }
    }
    
    if ( holex - left > 1 )
    {
        
        qsindex( array, index , left, holex - 1 );
        
    }
    if ( right - holex > 1 )
    {
        
        qsindex( array, index , holex + 1, right );
        
    }
    
    array[holex] = pivot;
    
    index[holex] = ipivot;
    
}


/* ----------------------------------------------------------------------------- */