VoxelList.cpp

//==================================================================
/// VoxelList.cpp
///
/// Created by Davide Pasca - 2012/7/4
/// See the file "license.txt" that comes with this project for
/// copyright info. 
//==================================================================

#include "stdafx.h"
#include "VoxelList.h"

//#define TEST_WORK

#if defined(TEST_WORK)
# define TWASSERT   DASSERT
# define TWASSERT_RET   DASSERT
#else
# define TWASSERT   
# define TWASSERT_RET(_X_)  if (!(_X_)) return 
#endif

//==================================================================
DFORCEINLINE u_int log2ceil( u_int val )
{
    for (u_int i=0; i < 32; ++i)
    {
        if ( ((u_int)1<<i) >= val )
            return i;
    }

    TWASSERT( 0 );
    return (u_int)DNPOS;
}

//==================================================================
VoxelList::VoxelList() :
            mUnit(0,0,0),
            mVS_LS(0,0,0),
            mOOUnitForTess(0),
            mN0(0),
            mN1(0),
            mN2(0)
{
}

//==================================================================
void VoxelList::BuildFromTrigs( const DVec<Float3> &trigs, float baseUnit )
{
    mBBoxMin = Float3(  FLT_MAX ); 
    mBBoxMax = Float3( -FLT_MAX ); 

    TWASSERT( trigs.size() % 3 == 0 );

    for (size_t i=0; i < trigs.size(); ++i)
    {
        mBBoxMin = DMin( mBBoxMin, trigs[i] );
        mBBoxMax = DMax( mBBoxMax, trigs[i] );
    }
    Float3	bboxSiz = mBBoxMax - mBBoxMin;

    mN0 = log2ceil( (u_int)ceilf(bboxSiz[0] / baseUnit) );
    mN1 = log2ceil( (u_int)ceilf(bboxSiz[1] / baseUnit) );
    mN2 = log2ceil( (u_int)ceilf(bboxSiz[2] / baseUnit) );

    mCells.resize( 1 << (mN0 + mN1 + mN2) );

    float nn0 = (float)(1 << mN0);
    float nn1 = (float)(1 << mN1);
    float nn2 = (float)(1 << mN2);

    mUnit[0] = bboxSiz[0] / nn0;
    mUnit[1] = bboxSiz[1] / nn1;
    mUnit[2] = bboxSiz[2] / nn2;

    mVS_LS[0] = (nn0-1) / (bboxSiz[0] ? bboxSiz[0] : 1);
    mVS_LS[1] = (nn1-1) / (bboxSiz[1] ? bboxSiz[1] : 1);
    mVS_LS[2] = (nn2-1) / (bboxSiz[2] ? bboxSiz[2] : 1);

    // avoid picking a 0 potential 2D gementry aligned to some axis
    float minUnit = FLT_MAX;
    const float EPS = 0.0001f;
    if ( mUnit[0] > EPS ) minUnit = DMin( minUnit, mUnit[0] );
    if ( mUnit[1] > EPS ) minUnit = DMin( minUnit, mUnit[1] );
    if ( mUnit[2] > EPS ) minUnit = DMin( minUnit, mUnit[2] );

    TWASSERT_RET( minUnit > EPS && minUnit != FLT_MAX );

    mOOUnitForTess = 1.f / minUnit * 0.25f;

    for (size_t i=0, j=0; i < trigs.size(); i += 3, j += 1)
    {
        tessTri(
            trigs[i+0],
            trigs[i+1],
            trigs[i+2],
            j );
    }
}

//==================================================================
DFORCEINLINE void VoxelList::addElem( const Float3 &pos, u_int idx )
{
    TWASSERT(
        (pos[0] >= mBBoxMin[0] && pos[0] <= mBBoxMax[0]) &&
        (pos[1] >= mBBoxMin[1] && pos[1] <= mBBoxMax[1]) &&
        (pos[2] >= mBBoxMin[2] && pos[2] <= mBBoxMax[2]) );

    Float3	cellIdxF = (pos - mBBoxMin) * mVS_LS;

    int	cell0 = (int)cellIdxF[0];
    int	cell1 = (int)cellIdxF[1];
    int	cell2 = (int)cellIdxF[2];

    TWASSERT(
        cell0 >= 0 && cell0 < (1 << mN0) &&
        cell1 >= 0 && cell1 < (1 << mN1) &&
        cell2 >= 0 && cell2 < (1 << mN2) );

    Cell &cell = mCells[
                    (cell2 << (mN1 + mN0)) +
                    (cell1 << mN0) +
                     cell0 ];

    cell.mData.find_or_push_back( (u_short)idx );
}

//==================================================================
void VoxelList::tessQuad(
        const Float3 &p00,
        const Float3 &p01,
        const Float3 &p10,
        const Float3 &p11,
        u_int idx )
{
    Float3 dh0 = p01 - p00;
    Float3 dh1 = p11 - p10;
    Float3 dv0 = p10 - p00;
    Float3 dv1 = p11 - p01;

    float msqr_h0 = DLengthSqr( dh0 );	
    float msqr_h1 = DLengthSqr( dh1 );	
    float msqr_v0 = DLengthSqr( dv0 );	
    float msqr_v1 = DLengthSqr( dv1 );	

    float maxh = sqrtf( DMax( msqr_h0, msqr_h1 ) );
    float maxv = sqrtf( DMax( msqr_v0, msqr_v1 ) );

    // degenerate case.. when it's a line or a point
    static const float EPS = 0.0001f;
    if ( maxh < EPS || maxv < EPS )
        return;

    float nhf = ceilf( maxh * mOOUnitForTess );
    float nvf = ceilf( maxv * mOOUnitForTess );
    TWASSERT( nhf != 0 && nvf != 0 );

    float oonhf = 1.f / nhf;
    float oonvf = 1.f / nvf;

    Float3 ddv0 = dv0 * oonvf;
    Float3 ddv1 = dv1 * oonvf;
    Float3 pv0 = p00;
    Float3 pv1 = p01;

    u_int nh = (u_int)nhf;
    u_int nv = (u_int)nvf;
    for (u_int i=0; i < nv; ++i, pv0 += ddv0, pv1 += ddv1)
    {
        Float3 ddh = (pv1 - pv0) * oonhf;
        Float3 ph = pv0;
        for (u_int j=0; j < nh; ++j, ph += ddh)
        {
            addElem( ph, idx );
        }
    }
}

//==================================================================
void VoxelList::tessTri(
        const Float3 &v0, 
        const Float3 &v1, 
        const Float3 &v2,
        u_int idx )
{
    Float3  mid = (v0 + v1 + v2) * (1.0f/3);
    Float3  a	= (v0 + v1) * 0.5f;
    Float3  b	= (v1 + v2) * 0.5f;
    Float3  c	= (v0 + v2) * 0.5f;

    tessQuad( v0, a, c, mid, idx );
    tessQuad( v1, a, b, mid, idx );
    tessQuad( v2, b, c, mid, idx );
}

//==================================================================
DFORCEINLINE void halfSpace(
                    Float3 &v,
                    const Float3 &d,
                    float lim,
                    u_int x,
                    u_int y0,
                    u_int y1 )
{
    float adj = lim - v[x];

    v[x] = lim;

    if ( d[x] ) {
        adj /= d[x];
        v[y0] += adj * d[y0];
        v[y1] += adj * d[y1];
    }
}

//==================================================================
bool VoxelList::clipLine( Float3 verts[2] ) const
{
    const Float3 &v0 = verts[0];
    const Float3 &v1 = verts[1];

    // early check ..if both ends in the wrong quadrant
    if ((v0[0] < mBBoxMin[0] && v1[0] < mBBoxMin[0]) ||
        (v0[1] < mBBoxMin[1] && v1[1] < mBBoxMin[1]) ||
        (v0[2] < mBBoxMin[2] && v1[2] < mBBoxMin[2]) ||
        (v0[0] > mBBoxMax[0] && v1[0] > mBBoxMax[0]) ||
        (v0[1] > mBBoxMax[1] && v1[1] > mBBoxMax[1]) ||
        (v0[2] > mBBoxMax[2] && v1[2] > mBBoxMax[2]) )
    {
        return false;
    }

    // do the half-space clipping
    for (int i=0; i < 2; ++i)
    {
        Float3	&v = verts[i];
        if ( v[0] < mBBoxMin[0] ) halfSpace( v, verts[1]-verts[0], mBBoxMin[0], 0, 1, 2 );
        if ( v[1] < mBBoxMin[1] ) halfSpace( v, verts[1]-verts[0], mBBoxMin[1], 1, 2, 0 );
        if ( v[2] < mBBoxMin[2] ) halfSpace( v, verts[1]-verts[0], mBBoxMin[2], 2, 0, 1 );
        if ( v[0] > mBBoxMax[0] ) halfSpace( v, verts[1]-verts[0], mBBoxMax[0], 0, 1, 2 );
        if ( v[1] > mBBoxMax[1] ) halfSpace( v, verts[1]-verts[0], mBBoxMax[1], 1, 2, 0 );
        if ( v[2] > mBBoxMax[2] ) halfSpace( v, verts[1]-verts[0], mBBoxMax[2], 2, 0, 1 );
    }

    // last check.. returns false if it's still outside
    // (different ends outside different quadrants and line doesn't intersect bbox)
    return
         v0[0] >= mBBoxMin[0] && v0[1] >= mBBoxMin[1] && v0[2] >= mBBoxMin[2] &&
         v0[0] <= mBBoxMax[0] && v0[1] <= mBBoxMax[1] && v0[2] <= mBBoxMax[2] &&
         v1[0] >= mBBoxMin[0] && v1[1] >= mBBoxMin[1] && v1[2] >= mBBoxMin[2] &&
         v1[0] <= mBBoxMax[0] && v1[1] <= mBBoxMax[1] && v1[2] <= mBBoxMax[2];
}

//==================================================================
bool VoxelList::FindClosestNonEmptyCellCtr(
                        const Float3 &posLS,
                        Float3 &out_foundCellCenterLS ) const
{
    int nn0 = 1 << mN0;
    int nn1 = 1 << mN1;
    int nn2 = 1 << mN2;

    // position to check in Voxel Space
    Float3 posVS = mVS_LS * (posLS - mBBoxMin);

    float   closestSqr   = FLT_MAX;
    Float3  closestCtrVS( 0, 0, 0 );

    for (int i2=0; i2 < nn2; ++i2)
    {
        for (int i1=0; i1 < nn1; ++i1)
        {
            for (int i0=0; i0 < nn0; ++i0)
            {
                const Cell &cell = mCells[
                                (i2 << (mN1 + mN0)) +
                                (i1 << mN0) +
                                 i0 ];
                // make sure that it's not empty
                if NOT( cell.mData.size() )
                    continue;

                // center of the cell in Voxel Space
                Float3 cellCtrVS(
                            ((float)i0+0.5f) * mUnit[0],
                            ((float)i1+0.5f) * mUnit[1],
                            ((float)i2+0.5f) * mUnit[2] );

                float distSqr = DLengthSqr( cellCtrVS - posVS );
                if ( distSqr < closestSqr )
                {
                    closestSqr = distSqr;
                    closestCtrVS = cellCtrVS;
                }
            }
        }
    }

    if ( closestSqr == FLT_MAX )
        return false;
    
    // give out the center of the cell in Local Space
    out_foundCellCenterLS = closestCtrVS * mUnit + mBBoxMin;

    return true;
}

//==================================================================
const DVec<VoxelList::Cell *>
                &VoxelList::CheckLine( const Float3 &lineSta, const Float3 &lineEnd )
{
    mCheckRes.clear();

    // clip local-space line
    Float3 clipped[2] = { lineSta, lineEnd };
    if NOT( clipLine( clipped ) )
        return mCheckRes; // just get out

    // voxel-space line
    Float3 vox[] = {
            (clipped[0] - mBBoxMin) * mVS_LS,
            (clipped[1] - mBBoxMin) * mVS_LS
    };

    TWASSERT( (int)vox[0][0] >= 0 && (int)vox[0][0] < (int)(1 << mN0) );
    TWASSERT( (int)vox[0][1] >= 0 && (int)vox[0][1] < (int)(1 << mN1) );
    TWASSERT( (int)vox[0][2] >= 0 && (int)vox[0][2] < (int)(1 << mN2) );
    TWASSERT( (int)vox[1][0] >= 0 && (int)vox[1][0] < (int)(1 << mN0) );
    TWASSERT( (int)vox[1][1] >= 0 && (int)vox[1][1] < (int)(1 << mN1) );
    TWASSERT( (int)vox[1][2] >= 0 && (int)vox[1][2] < (int)(1 << mN2) );

    Float3 diff = vox[1] - vox[0];

    u_int ia, ta;
    u_int ib, tb;
    u_int ic, tc;
#if defined(TEST_WORK)
    u_int nna, nnb, nnc;
#endif

    float adiff0 = fabs( diff[0] );
    float adiff1 = fabs( diff[1] );
    float adiff2 = fabs( diff[2] );

    if ( adiff0 > adiff1 )
    {
        if ( adiff0 > adiff2 ) {
            ia = 0; ta = 0;
            ib = 1; tb = mN0;
            ic = 2; tc = mN0 + mN1;
#if defined(TEST_WORK)
            nna = 1 << mN0;
            nnb = 1 << mN1;
            nnc = 1 << mN2;
#endif
        } else {
            ia = 2; ta = mN0 + mN1;
            ib = 0; tb = 0;
            ic = 1; tc = mN0;
#if defined(TEST_WORK)
            nna = 1 << mN2;
            nnb = 1 << mN0;
            nnc = 1 << mN1;
#endif
        }
    }
    else
    {
        if ( adiff1 > adiff2 ) {
            ia = 1; ta = mN0;
            ib = 2; tb = mN0 + mN1;
            ic = 0; tc = 0;
#if defined(TEST_WORK)
            nna = 1 << mN1;
            nnb = 1 << mN2;
            nnc = 1 << mN0;
#endif
        } else {
            ia = 2; ta = mN0 + mN1;
            ib = 0; tb = 0;
            ic = 1; tc = mN0;
#if defined(TEST_WORK)
            nna = 1 << mN2;
            nnb = 1 << mN0;
            nnc = 1 << mN1;
#endif
        }
    }

    if ( vox[0][ia] > vox[1][ia] ) {
        std::swap( vox[0], vox[1] );
        diff = -diff;
    }

    u_int a0 = (u_int)vox[0][ia];
    u_int a1 = (u_int)vox[1][ia];

    //float fa = vox[0][ia] - (float)a0;

    u_int lena = a1 - a0 + 1;

    mCheckRes.resize( lena );

    float oolena = 1.f / (float)lena;

    float db = diff[ib] * oolena;
    float dc = diff[ic] * oolena;

    float b = vox[0][ib];// + fa * db;
    float c = vox[0][ic];// + fa * dc;
    for (u_int a=a0; a <= a1; a += 1, b += db, c += dc)
    {
#if defined(TEST_WORK)
        TWASSERT( (u_int)a < nna && (u_int)b < nnb && (u_int)c < nnc );
#endif

        u_int a_idx = (u_int)a << ta;
        u_int b_idx = (u_int)b << tb;
        u_int c_idx = (u_int)c << tc;
        mCheckRes[a-a0] = &mCells[ a_idx + b_idx + c_idx ];
    }

    return mCheckRes;
}

VoxelList.h

//==================================================================
/// VoxelList.h
///
/// Created by Davide Pasca - 2012/7/4
/// See the file "license.txt" that comes with this project for
/// copyright info. 
//==================================================================

#ifndef VOXELLIST_H
#define VOXELLIST_H

//==================================================================
class VoxelList
{
public:
    class Cell
    {
    public:
        DVec<u_short>	mData;
    };

    DVec<Cell>	mCells;

private:
    DVec<Cell*>	mCheckRes;

public:
    Float3		mBBoxMin;
    Float3		mBBoxMax;
    Float3		mUnit;
private:
    Float3		mVS_LS; // Voxel Space from Local Space (scale only)
    float		mOOUnitForTess;
    u_int		mN0;
    u_int		mN1;
    u_int		mN2;

public:
    VoxelList();

    void BuildFromTrigs( const DVec<Float3> &trigs, float baseUnit );

    const DVec<Cell *> &CheckLine( const Float3 &lineSta, const Float3 &lineEnd );

    bool FindClosestNonEmptyCellCtr(
                        const Float3 &posLS,
                        Float3 &out_foundCellCenterLS ) const;

private:
    void tessQuad(
            const Float3 &p00,
            const Float3 &p01,
            const Float3 &p10,
            const Float3 &p11,
            u_int idx );

    void tessTri(
            const Float3 &v0, 
            const Float3 &v1, 
            const Float3 &v2,
            u_int idx );

    void addElem( const Float3 &pos, u_int idx );

    bool clipLine( Float3 verts[2] ) const;
};

#endif