FDH.cc

Go to the documentation of this file.

#include <esg/Definitions.h>
#include <esg/geometry/FDH.h>
#include <esg/explorer/NExtentsExplorer.h>
#include <esg/mesh/PolygonalMesh.h>

using namespace esg;

/*
 * 0 = No line
 * 1 = Single point
 * 2 = Regular line
 */ 
int FDH::_cut_line(Vector3d& origin, Vector3d& direction) const
{
    double np0, np1;
    double t_in  = -MAXDOUBLE;
    double t_out =  MAXDOUBLE;
    double t1, t2;

    // Liang-Barsky clipping algorithm
    //
    // t1 = -(N * P_0 + D_1) / (N * P_1)
    // t2 = -(N * P_0 + D_2) / (N * P_1)
    // where N is normal vector of half-space
    //       p_0 is start pont of ray
    //       p_1 is direction of ray
    for (register unsigned i = 0; i < _dirs; i++) {
        np0 = (_mat[i][0] * origin.x +
               _mat[i][1] * origin.y +
               _mat[i][2] * origin.z);
        np1 = (_mat[i][0] * direction.x +
               _mat[i][1] * direction.y +
               _mat[i][2] * direction.z);

        if (np1 > -Geometry::EPS && np1 < Geometry::EPS) continue;

        t1 = - (np0 - _values[i])       / np1;  // for upper FDH
        t2 = - (np0 + _values[i+_dirs]) / np1;  // for lower FDH
    
        if (t1    > t2) { float t3 = t1; t1 = t2; t2 = t3; }
        if (t_in  < t1)  t_in = t1; 
        if (t_out > t2)  t_out = t2; 
        if (t_in  > t_out) return 0; // no intersection
    }

    Vector3d aux(origin);
    origin.set(direction);
    origin.scaleAdd(t_in, aux);
  
    if (t_in == t_out) return 1;

    direction.scaleAdd(t_out, aux);
    return 2;
}

#define FDH_UPDATE_FACET(i, k) { \
   if (facetVert && nFacetVert) { \
      bool found = false; \
      for (register unsigned m = 0; m < nFacetVert[(i)] && !found; m++) \
          if (facetVert[(i)][m] == (k)) found = true; \
      if (!found) facetVert[(i)][nFacetVert[(i)]++] = (k); \
    } \
}

Vector3d* FDH::_get_corners(unsigned  & numCorners,
                            unsigned  (*facetVert)[100],
                            unsigned    nFacetVert[]) const
{
    Vector3d  norm1, norm2;
    Vector3d  origin, direction;
    double    cosinus;
    unsigned  reposSize = 0;
    Vector3d* vertRepos = new Vector3d [_dirs*1000];
    int       mask;
    int       line;
    Vector3d  v;

    if (nFacetVert) 
        for (register unsigned i = 0; i < 2*_dirs; i++) nFacetVert[i] = 0;

    for (register unsigned i = 0; i < 2*_dirs; i++) {
        norm1.set(_mat[i][0], _mat[i][1], _mat[i][2]);
    
        for (register unsigned j = 0; j < i; j++) {
            norm2.set(_mat[j][0],_mat[j][1],_mat[j][2]);
            cosinus = norm1.dot(norm2);

            if (cosinus < 0.0 || cosinus == 1.0) continue;

            // (origin, direction) forms the line which is intersection of
            // planes having normals 'normal1' and 'normal2'
            direction.cross(norm1,norm2);
            direction.normalize(); // is it necessary ?
            v.set(norm1);
            v.scale(_values[i]);
            origin.cross(norm1, direction);
            origin.scaleAdd((_values[j]-norm2.dot(v)) / norm2.dot(origin), v);

            // get edge from line
            line = _cut_line(origin, direction);
            if (line == 0) continue;

            // Now the (origin, direction) represents end points of edge.
            // Reduce too short line, make the single point from it.
            if (line == 2) {
                Vector3d aux(direction);
                aux.sub(origin);
                if (aux.length() < Geometry::EPS) line = 1;
            }

            // Check if vertices are already known.
            mask = 0;
            for (register unsigned k = 0; k < reposSize; k++) {
                if (vertRepos[k].epsilonEquals(origin, Geometry::EPS)) {
                    mask |= (1<<0);
                    FDH_UPDATE_FACET(i, k);
                    FDH_UPDATE_FACET(j, k);
                }
                if (line == 2 &&
                    vertRepos[k].epsilonEquals(direction, Geometry::EPS)) {
                    mask |= (1<<1);
                    FDH_UPDATE_FACET(i, k);
                    FDH_UPDATE_FACET(j, k);
                }
            }

            
            if (!(mask & (1<<0))) {
                FDH_UPDATE_FACET(i, reposSize);
                FDH_UPDATE_FACET(j, reposSize);
                vertRepos[reposSize++].set(origin);
            }
            if (line == 2 && !(mask & (1<<1))) {
                FDH_UPDATE_FACET(i, reposSize);
                FDH_UPDATE_FACET(j, reposSize);
                vertRepos[reposSize++].set(direction);
            }
        }
    }

    if (reposSize > 0) {
        Vector3d* pRet = new Vector3 [reposSize];
        numCorners = reposSize;
        for (register unsigned l = 0; l < numCorners; l++)
            pRet[l].set(vertRepos[l]);
        delete [] vertRepos;
        return pRet;
    } else {
        delete [] vertRepos;
        numCorners = 0;
        return NULL;
    }
}

#undef FDH_UPDATE_FACET

Mesh* FDH::_mesh(int) const
{
    // get array of vertices
    unsigned   numC;
    unsigned (*planeVert)[100] = new unsigned [2*_dirs] [100];
    unsigned  *num             = new unsigned [2*_dirs];

    Vector3d* corners = _get_corners(numC, planeVert, num);

    if (!(corners && numC)) {
        if (corners) delete [] corners;
        delete [] planeVert;
        delete [] num;
        return NULL;
    }

    // initiate mesh
    PolygonalMesh * pMesh = new PolygonalMesh(corners, numC);

    // for each facet find vertex with minimal (local) u coordinate and then
    // find convex hull = edges
    Vector3d  u, v, w;
    unsigned  minUindex = 0;
    double    minUval   = 0.0;
    unsigned  k;
    double    len, d;
    unsigned* convexHull;
    for (register unsigned i = 0; i < 2*_dirs; i++) {
        if (num[i] < 3) continue;

        // set local coordinate system
        w.set(_mat[i][0], _mat[i][1], _mat[i][2]);
        k = 1;
        do {
            v.set(corners[planeVert[i][k++]]); 
            v.sub(corners[planeVert[i][0]]); // v = arbitrary edge
            len = v.length();
        } while (k < num[i] && len < Geometry::EPS);
        if (len < Geometry::EPS) continue; // skip too small facet
        v.scale(1.0/len); // normalize
        u.cross(w, v);

        // find vertex with minimal u coord.
        minUval = MAXFLOAT;
        for (register unsigned j = 0; j < num[i]; j++) {
            d = corners[planeVert[i][j]].dot(u);
            if (d < minUval) { minUval = d; minUindex = j; }
        }

        // find convex hull
        bool       degenerated;
        int        newVert;
        Vector3d   edge;
        double     maxCos, cos;
        ESGbitmask flag;

        convexHull = new unsigned [num[i]];
        flag = (1<<minUindex);
        degenerated = false;
        convexHull[0] = planeVert[i][minUindex];
        for (register unsigned j = 1; j < num[i] && !degenerated; j++) {
            maxCos      = -MAXFLOAT;
            newVert     = -1;
            for (k = 0; k < num[i] && !degenerated; k++) {
                if (flag & (1<<k)) continue;
                edge.set(corners[planeVert[i][k]]);
                edge.sub(corners[convexHull[j-1]]);
                if (edge.length() < Geometry::EPS) {
                    degenerated = true;
                    continue;
                }
                edge.normalize();
                cos = edge.dot(v);
                if (cos > maxCos) { maxCos = cos; newVert = k; }
            }
            if (degenerated || newVert < 0) continue;
            convexHull[j] = planeVert[i][newVert];
            flag |= (1<<newVert);
            v.set(corners[convexHull[j]]); // update 'v'
            v.sub(corners[convexHull[j-1]]);
            if (v.length() < Geometry::EPS) degenerated = true;
            else v.normalize();
        }

        // create facet
        if (!degenerated)
            if (!pMesh->addFacet(convexHull, NULL, num[i])) {
                cerr << "FDH::_mesh(): " << "Skipping mesh generation because of problems with topology" << endl;
                delete [] convexHull;
                delete [] corners;
                delete [] planeVert;
                delete [] num;
                delete pMesh;
                return NULL;
            }
        delete [] convexHull;
    }
        
    delete [] corners;
    delete [] planeVert;
    delete [] num;
    
    return pMesh;
}

void FDH::_duplicate_attributes(const Geometry& src) 
{
    Geometry::_duplicate_attributes(src);
    _values = new float [_dirs*2];
    for (register unsigned i = 0; i < _dirs*2; i++)
        _values[i] = ((FDH&)src)._values[i];
}

void FDH::_rotateX(float a)
{
    unsigned  numCorners;
    Vertex3*  corners = _get_corners(numCorners, NULL, NULL);
    
    if (!corners) return;

    Matrix3 trMat;
    trMat.rotX(a);
    
    for (register unsigned i = 0; i < _dirs; i++) {
        _values[i]       = -MAXFLOAT;
        _values[i+_dirs] = -MAXFLOAT;
    }

    for (register unsigned i = 0; i < numCorners; i++) {
        trMat.transform(corners[i]);
        for (register unsigned j = 0; j < _dirs; j++) {
            float dist = corners[i].dot(Vector3(_mat[j][0], _mat[j][1], _mat[j][2]));
            if (dist > _values[j])        _values[j]       =   dist;
            if (dist < -_values[j+_dirs]) _values[j+_dirs] = - dist;
        }
    }

    delete [] corners;
}

void FDH::_rotateY(float a)
{
    unsigned  numCorners;
    Vertex3*  corners = _get_corners(numCorners, NULL, NULL);
    
    if (!corners) return;

#if defined(_MSC_VER)
        Vector3 *rotDirs = new Vector3[_dirs];
#else
        Vector3 rotDirs[_dirs];
#endif
    Matrix3 trMat;
    trMat.rotY(a);
    
    for (register unsigned i = 0; i < _dirs; i++) {
        _values[i]       = -MAXFLOAT;
        _values[i+_dirs] = -MAXFLOAT;
        rotDirs[i].set(_mat[i][0], _mat[i][1], _mat[i][2]);
        trMat.transform(rotDirs[i]);
    }

    for (register unsigned i = 0; i < numCorners; i++) {
        trMat.transform(corners[i]);
        for (register unsigned j = 0; j < _dirs; j++) {
            float dist = corners[i].dot(Vector3(_mat[j][0], _mat[j][1], _mat[j][2]));
            if (dist > _values[j])        _values[j]       =   dist;
            if (dist < -_values[j+_dirs]) _values[j+_dirs] = - dist;
        }
    }

    delete [] corners;

#if defined(_MSC_VER)
        delete [] rotDirs;
#endif
}

void FDH::_rotateZ(float a)
{
    unsigned  numCorners;
    Vertex3*  corners = _get_corners(numCorners, NULL, NULL);
    
    if (!corners) return;

    Matrix3 trMat;
    trMat.rotZ(a);
    
    for (register unsigned i = 0; i < _dirs; i++) {
        _values[i]       = -MAXFLOAT;
        _values[i+_dirs] = -MAXFLOAT;
    }

    for (register unsigned i = 0; i < numCorners; i++) {
        trMat.transform(corners[i]);
        for (register unsigned j = 0; j < _dirs; j++) {
            float dist = corners[i].dot(Vector3(_mat[j][0], _mat[j][1], _mat[j][2]));
            if (dist > _values[j])        _values[j]       =   dist;
            if (dist < -_values[j+_dirs]) _values[j+_dirs] = - dist;
        }
    }

    delete [] corners;
}

void FDH::_rotate(float a, const Vector3& axis)
{
    unsigned  numCorners;
    Vertex3*  corners = _get_corners(numCorners, NULL, NULL);
    
    if (!corners) return;

    Matrix4 trMat;
    Matrix3 rotMat;
    trMat.rotationGL(a, axis);
    trMat.get(rotMat);
    
    for (register unsigned i = 0; i < _dirs; i++) {
        _values[i]       = -MAXFLOAT;
        _values[i+_dirs] = -MAXFLOAT;
    }

    for (register unsigned i = 0; i < numCorners; i++) {
        rotMat.transform(corners[i]);
        for (register unsigned j = 0; j < _dirs; j++) {
            float dist = corners[i].dot(Vector3(_mat[j][0], _mat[j][1], _mat[j][2]));
            if (dist > _values[j])        _values[j]       =   dist;
            if (dist < -_values[j+_dirs]) _values[j+_dirs] = - dist;
        }
    }

    delete [] corners;
}

void FDH::_rotate(const Matrix3& rotMat)
{
    unsigned  numCorners;
    Vertex3*  corners = _get_corners(numCorners, NULL, NULL);
    
    if (!corners) return;

    for (register unsigned i = 0; i < _dirs; i++) {
        _values[i]       = -MAXFLOAT;
        _values[i+_dirs] = -MAXFLOAT;
    }

    for (register unsigned i = 0; i < numCorners; i++) {
        rotMat.transform(corners[i]);
        for (register unsigned j = 0; j < _dirs; j++) {
            float dist = corners[i].dot(Vector3(_mat[j][0], _mat[j][1], _mat[j][2]));
            if (dist > _values[j])        _values[j]       =   dist;
            if (dist < -_values[j+_dirs]) _values[j+_dirs] = - dist;
        }
    }

    delete [] corners;
}

void FDH::_translate(float x, float y, float z)
{
    Vector3 v(x,y,z);
    float   d;
    
    for (register unsigned i = 0; i < _dirs; i++) {
        d = v.dot(Vector3(_mat[i][0],_mat[i][1],_mat[i][2]));
        _values[i]       += d;
        _values[i+_dirs] -= d;
    }
}

void FDH::_transform(const Matrix4& trMat)
{
    unsigned  numCorners;
    Vertex3*  corners = _get_corners(numCorners, NULL, NULL);
    
    if (!corners) return;

    //for (unsigned i = 0; i < numCorners; i++)
    //fprintf(stderr,"%f %f %f\n",corners[i].x,corners[i].y,corners[i].z);

#ifdef WIN32
        Vector3 *dirs = new Vector3[_dirs];
#else
        Vector3 dirs[_dirs];
#endif

    for (register unsigned i = 0; i < _dirs; i++) {
        _values[i]       = -MAXFLOAT;
        _values[i+_dirs] = -MAXFLOAT;
        dirs[i].set(_mat[i][0], _mat[i][1], _mat[i][2]);
    }

    for (register unsigned i = 0; i < numCorners; i++) {
        trMat.transform(corners[i]);
        for (register unsigned j = 0; j < _dirs; j++) {
            float dist = corners[i].dot(dirs[j]);
            if (dist > _values[j])        _values[j]       =   dist;
            if (dist < -_values[j+_dirs]) _values[j+_dirs] = - dist;
        }
    }

    //for (unsigned i = 0; i < numCorners; i++)
    //fprintf(stderr,"%f %f %f\n",corners[i].x,corners[i].y,corners[i].z);

    delete [] corners;

#ifdef WIN32
        delete [] dirs;
#endif
}

void FDH::_scale(float s)
{
    for (register unsigned i = 0; i < 2*_dirs; i++) _values[i] *= s;
}


//---------- public -----------

FDH::FDH(unsigned          dirs,
         const float       (*mat)[3],
         SceneGraphObject& object)
  : _dirs(dirs), _mat(mat)
{
    _values = new float [_dirs*2];

    Interval         interval;
    NExtentsExplorer explorer(_mat, _dirs);
    explorer.explore(object);
    for (register unsigned i = 0; i < _dirs; i++) {
        interval = explorer.result(i);
        _values[i]       =   interval.max;
        _values[i+_dirs] = - interval.min;
    }
}

FDH::FDH(unsigned               dirs,
         const float            (*mat)[3],
         List<SceneGraphObject>& list)
  : _dirs(dirs), _mat(mat)
{
    _values = new float [_dirs*2];
    for (register unsigned i = 0; i < _dirs; i++) 
        _values[i] = (_values[i+_dirs] = - MAXFLOAT);
    
    Interval          interval;
    SceneGraphObject* pObject = list.firstItem();
    while (pObject) {
        NExtentsExplorer explorer(_mat, _dirs);
        explorer.explore(*pObject);
        for (register unsigned i = 0; i < _dirs; i++) {
            interval = explorer.result(i);
            if (interval.max >  _values[i])
                _values[i] = interval.max;
            if (interval.min < -_values[i+_dirs])
                _values[i+_dirs] = - interval.min;
        }
        pObject = list.nextItem();
    }
}

FDH::FDH(unsigned               dirs,
         const float            (*mat)[3],
         const Geometry&        geom1,
         const Geometry&        geom2)
  : _dirs(dirs), _mat(mat)
{
  _values = new float [_dirs*2];

  Interval interval1, interval2;
  for (register unsigned i = 0; i < _dirs; i++) {
    interval1 = geom1.extent(_mat[i][0], _mat[i][1], _mat[i][2]);
    interval2 = geom2.extent(_mat[i][0], _mat[i][1], _mat[i][2]);
    if (interval1.max >  interval2.max) _values[i]       =  interval1.max;
    else                                _values[i]       =  interval2.max;
    if (interval1.min < interval2.min)   _values[i+_dirs] = -interval1.min;
    else                                _values[i+_dirs] = -interval2.min;
  }
}

FDH::FDH(unsigned               dirs,
         const float            (*mat)[3],
         const Geometry&        geom)
    : _dirs(dirs), _mat(mat)
{
    _values = new float [_dirs*2];

    Interval interval;
    for (register unsigned i = 0; i < _dirs; i++) {
        interval = geom.extent(_mat[i][0], _mat[i][1], _mat[i][2]);
        _values[i]       =  interval.max;
        _values[i+_dirs] = -interval.min;
    }
}

FDH::FDH(unsigned    dirs,
         const float (*mat)[3],
         const float values[])
    : _dirs(dirs), _mat(mat)
{
    _values = new float [_dirs*2];
    
    for (register unsigned i = 0; i < 2*_dirs; i++) _values[i] = values[i];
}

FDH::~FDH()
{
  if (_values) delete [] _values;
}

void FDH::rayIntersection(PointEnv*      pPE,
                          int            mask,
                          const Vector3& origin,
                          const Vector3& direction,
                          float          maxDist)
{
    if (!pPE) {
        fprintf(stderr,"FDH error: No point environment structure defined\n");
        return;
    }
    
    pPE->mask = ENV_HAVE_NOTHING;


    int   plane1 = 0, plane2 = 0;
    float t_in = -MAXFLOAT, t_out = MAXFLOAT;
    float t1, t2;

    if (mask&ENV_WANT_UV_COORD)
        fprintf(stderr,"FDH::rayIntersection(): UV mapping is not implemented\n");

    if (mask&ENV_USE_CACHE) {
        if (!pPE->pCache)
            pPE->pCache = new FDHCache(_mat,_dirs,origin,direction);
        FDHCache * c = (FDHCache*) pPE->pCache;
        if (mask & ENV_WANT_NORMAL) {
            FDH_CLIP_LINE_INDEX_INIT(c->oval[0], c->dval[0], 0, _dirs);
            for (register unsigned i = 1; i < _dirs; i++) {
                FDH_CLIP_LINE_INDEX_CONT(c->oval[i], c->dval[i], i, i+_dirs);
            }
        } else {
            FDH_CLIP_LINE_INIT(c->oval[0], c->dval[0], 0, _dirs);
            for (register unsigned i = 1; i < _dirs; i++) {
                FDH_CLIP_LINE_CONT(c->oval[i], c->dval[i], i, i+_dirs);
            }
        }
    } else {
        register double np1 = (_mat[0][0] * direction.x +
                               _mat[0][1] * direction.y +
                               _mat[0][2] * direction.z);
        register double np0 = (_mat[0][0] * origin.x +
                               _mat[0][1] * origin.y +
                               _mat[0][2] * origin.z);
        if (mask & ENV_WANT_NORMAL) {
            FDH_CLIP_LINE_INDEX_INIT(np0, np1, 0, _dirs);
            for (register unsigned i = 1; i < _dirs; i++) {
                np1 = (_mat[i][0] * direction.x +
                       _mat[i][1] * direction.y +
                       _mat[i][2] * direction.z);
                np0 = (_mat[i][0] * origin.x +
                       _mat[i][1] * origin.y +
                       _mat[i][2] * origin.z);
                FDH_CLIP_LINE_INDEX_CONT(np0, np1, 0, _dirs);
            }
        } else {
            FDH_CLIP_LINE_INIT(np0, np1, 0, _dirs);
            for (register unsigned i = 1; i < _dirs; i++) {
                np1 = (_mat[i][0] * direction.x +
                       _mat[i][1] * direction.y +
                       _mat[i][2] * direction.z);
                np0 = (_mat[i][0] * origin.x +
                       _mat[i][1] * origin.y +
                       _mat[i][2] * origin.z);
                FDH_CLIP_LINE_CONT(np0, np1, 0, _dirs);
            }
        }
    } // for cycle


    pPE->mask |= ENV_HAVE_INTERFERENCE;
    
    if (mask & ENV_WANT_NORMAL) {
        if (t_in < Geometry::EPS)    // ray starts inside FDH
            pPE->normal.set(_mat[plane2][0],_mat[plane2][1],_mat[plane2][2]);
        else
            pPE->normal.set(_mat[plane1][0],_mat[plane1][1],_mat[plane1][2]);
        pPE->normalOrientation = PointEnv::OUTWARDS_NORMAL;
        pPE->mask |= ENV_HAVE_NORMAL;
    }
    
    if (mask & ENV_WANT_INTERSECTION) {
        pPE->intersection.set(direction);
        if (t_in < Geometry::EPS)    // ray starts inside FDH
            pPE->intersection.scaleAdd(t_out, origin);
        else
            pPE->intersection.scaleAdd(t_in, origin);
        pPE->mask |= ENV_HAVE_INTERSECTION;
    }
    
    if (mask & ENV_WANT_DISTANCE) {
        if (t_in < Geometry::EPS) pPE->distance = t_out;
        else                      pPE->distance = t_in;
        pPE->mask |= ENV_HAVE_DISTANCE;
    }
}

bool FDH::mapToUV(const Vector3& v, Vector2& uv)
{
    fprintf(stderr,"FDH::mapToUV(): Not implemented\n");
    return false;
}

void FDH::randomSample(int mask, PointEnv& pe, double* pdf)
{
    pe.mask = ENV_HAVE_NOTHING;

    fprintf(stderr,"FDH::randomSample(): Not implemented\n");
}

bool FDH::randomDirection(const Vector3& pov, Vector3& dir, double* pdf) 
{
    fprintf(stderr,"FDH::randomDirection(): Not implemented\n");
    return false;
}

Interval FDH::extent(const Vector3& direction) const
{
  Interval  retInt;
  unsigned  numCorners;
  float     dist;
  Vertex3*  corners;

  for (register unsigned i = 0; i < _dirs; i++)
    if (_mat[i][0] == direction.x &&
        _mat[i][1] == direction.y &&
        _mat[i][2] == direction.z) {
      retInt.max =  _values[i];
      retInt.min = -_values[i+_dirs];
      return retInt;
    }

  retInt.min =  MAXFLOAT;
  retInt.max = -MAXFLOAT;

  if (!(corners = _get_corners(numCorners, NULL, NULL))) return retInt;

  for (register unsigned i = 0; i < numCorners; i++) {
    dist = direction.dot(corners[i]);
    if (dist < retInt.min) retInt.min = dist;
    if (dist > retInt.max) retInt.max = dist;
  }

  delete [] corners;

  return retInt;
}

Geometry* FDH::clone(const Matrix4* pTrMat) const
{
  FDH* pRet = new FDH(_dirs, _mat);
  pRet->_duplicate_attributes(*this);
  if (pTrMat) pRet->_transform(*pTrMat);
  return pRet;
}

Vector3 FDH::centroid() const
{
    register float dist;
  
    // Get POG of basic cube:
    Vector3 pog((_values[0] - _values[0+_dirs]) / 2,
                (_values[1] - _values[1+_dirs]) / 2,
                (_values[2] - _values[2+_dirs]) / 2);
    
    // Move POG respecting cutted corners:
    for (register unsigned i = 3; i < _dirs; i++) {
        dist = ((_values[i] - _values[i+_dirs]) / 2) - (_mat[i][0] * pog.x +
                                                        _mat[i][1] * pog.y +
                                                        _mat[i][2] * pog.z);
        pog.x += _mat[i][0]*dist;
        pog.y += _mat[i][1]*dist;
        pog.z += _mat[i][2]*dist; 
    }
    
    return pog;
}

double FDH::radius(const Vector3& centroid) const
{
    register double r = - MAXDOUBLE;
    register double d,e;
    for (register unsigned i = 0; i < _dirs; i++) {
        d = (centroid.x * _mat[i][0] +
             centroid.y * _mat[i][1] +
             centroid.z * _mat[i][2]);
        e = _values[i] - d;
        if (e > r) r = e;
        e = d - _values[i+_dirs];
        if (e > r) r = e;
    }
    return r;
}

bool FDH::separation(Geometry& geom, Vector3* pDir)
{
    Interval ext;
    try {
        FDH& fdh = dynamic_cast<FDH&>(geom);
        for (register unsigned i = 0; i < _dirs; i++) {
            if (_values[i] < - fdh._values[i+_dirs]) {
                if (pDir) pDir->set(_mat[i][0], _mat[i][1], _mat[i][2]);
                return true;
            }
            if (-_values[i+_dirs] > fdh._values[i])  {
                if (pDir) pDir->set(_mat[i+_dirs][0],
                                    _mat[i+_dirs][1],
                                    _mat[i+_dirs][2]);
                return true;
            }
        }
    } catch (bad_cast) { // geom is not FDH
        for (register unsigned i = 0; i < _dirs; i++) {
            ext = geom.extent(_mat[i][0], _mat[i][1], _mat[i][2]);
            if (_values[i] < ext.min || - _values[i+_dirs] > ext.max) {
                if (pDir) pDir->set(_mat[i][0], _mat[i][1], _mat[i][2]);
                return true;
            }
        }
    }
    return false;
}

double FDH::distance(const Geometry& geom, Vector3* pDir) 
{
    double   ret = -MAXDOUBLE;
    double   d;
    Interval ext;
    
    for (register unsigned i = 0; i < _dirs; i++) {
        ext = geom.extent(_mat[i][0], _mat[i][1], _mat[i][2]);
        d = ext.min - _values[i];
        if (d > ret) {
            if (pDir) pDir->set(_mat[i][0], _mat[i][1], _mat[i][2]);
            ret = d;
        }
        d = - (_values[i+_dirs] + ext.max);
        if (d > ret) {
            if (pDir) pDir->set(_mat[i+_dirs][0],
                                _mat[i+_dirs][1],
                                _mat[i+_dirs][2]);
            ret = d;
        }
    }
    
    return ret;
}

void FDH::dump(const char* intent, const char* tab)
{
    printf("%s geometry FDH {\n", intent);
    printf("%s %s dimension %d\n", intent, tab, _dirs);
    printf("%s %s direction [\n", intent, tab);
    for (register unsigned i = 0; i < _dirs; i++)
        printf("%s %s %s %f %f %f\n", intent, tab, tab, _mat[i][0], _mat[i][1], _mat[i][2]);
    printf("%s %s ]\n", intent, tab);
    printf("%s %s extent [ # upper, lower\n", intent, tab);
    for (register unsigned i = 0; i < _dirs; i++)
        printf("%s %s %s %f, %f\n", intent, tab, tab, _values[i], -_values[i+_dirs]);
    printf("%s %s ]\n", intent, tab);
    printf("%s }\n", intent);
}

bool FDH::checkVolume() const
{
    for (register unsigned i = 0; i < _dirs; i++) 
        if (_values[i] < -_values[i+_dirs]) return false;
    return true;
}

---