Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

Download ZIP
Fetching contributors…

Cannot retrieve contributors at this time

2623 lines (2289 sloc) 86.383 kb
// __BEGIN_LICENSE__
// Copyright (C) 2006-2011 United States Government as represented by
// the Administrator of the National Aeronautics and Space Administration.
// All Rights Reserved.
// __END_LICENSE__
/// \file nff_terrain.cc
///
/************************************************************************/
/* File: nff_terrain.c */
/* Date: August 1996 */
/* By: Eric Zbinden */
/* Modified: August 1999, by Larry Edwards */
/* For: NASA Ames Research Center, Intelligent Mechanisms Group */
/* Function: Main program for the stereo panorama pipeline */
/* Links: stereo.h stereo.c */
/* filters.h filters.c */
/* stereo_lib.h stereolib.c */
/* model_lib.h modellib.c */
/* stereo.default */
/* */
/* Notes: Stereo.c rebuild a 3D model of the world from a stereo */
/* Panorama. St_pan->filtering->disparity_map->range_map: */
/* -> dot cloud 3D model */
/* -> 3D .nff file */
/* */
/************************************************************************/
/* The following three includes are for debugging -LJE */
#include <sys/types.h>
#ifdef __APPLE__
#include <malloc/malloc.h>
#include <float.h> // for DBL_MAX & FLT_MAX
#else
#include <malloc.h>
#include <values.h> // for DBL_MAX & FLT_MAX
#endif
#include <assert.h>
#include <string.h> /* strrchr() */
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
// STL include
#include <stdexcept>
#include <vector>
// ASP includes:
#include <asp/asp_config.h>
#include <asp/Tools/nff_terrain.h>
// VW
#include <vw/Core/Exception.h>
#include <vw/Math/Vector.h>
struct PixelCoords
{
PixelCoords() { row = col = 0; }
PixelCoords(int x, int y) { row = y; col = x; }
int PixelIndex(int imageWidth);
int row,col;
}; /* xy vector or vertex position */
#if defined(ASP_HAVE_PKG_OPENSCENEGRAPH) && ASP_HAVE_PKG_OPENSCENEGRAPH==1
#include <osg/Geometry>
#include <osg/Material>
#include <osg/Geode>
#include <osg/Texture2D>
#include <osg/Image>
#include <osgDB/WriteFile>
#include <osgDB/ReadFile>
#endif
// STL includes:
#include <iostream> // debugging
#include <list>
#include <set>
#include <functional>
// Boost includes
#include <boost/algorithm/string.hpp>
#include <boost/algorithm/string/split.hpp> // For manipulating paths
using namespace std;
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
/* -- Constants -- */
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
#define THETA_LIMIT 1E-3
enum { LeftSide = 0, RightSide = 1, BottomSide = 2, TopSide = 3 };
enum { RightToLeftDiagonal = 0, LeftToRightDiagonal = 1 };
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
/* -- Type declarations -- */
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
typedef double Array3x3[3][3];
class Line
{
public:
double EvaluateLineFunction(POS2D point);
void SetEquation(POS2D *pt1, POS2D *pt2);
void SetEquation(PixelCoords *pt1, PixelCoords *pt2);
void GetEquation(double coefficients[3]);
double a,b,c;
};
class Plane
{
public:
double EvaluatePlaneFunction(POS3D *point);
void SetEquation(POS3D *pt1, POS3D *pt2, POS3D *pt3);
void GetEquation(double coefficients[4]);
private:
double a,b,c,d;
};
class QuadtreeNode
{
public:
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
QuadtreeNode(PixelCoords lowerLeft, PixelCoords upperRight);
void SetTriangleGradients(BUFFER *buffer, int width);
void SetTrianglePlanes(BUFFER *buffer, int width);
void SetTriangles(int leftToRight, BUFFER *buffer, int width);
void SetError(double error);
void AverageDistanceError(BUFFER *buffer, int width, double *error);
void MaxDistanceError(BUFFER *buffer, int width, double *error);
void OldGradientError(BUFFER *buffer, int width, POS2D *diff);
void AverageGradientError(BUFFER *buffer, int width, double *error);
void MaxGradientError(BUFFER *buffer, int width, double *error);
double CalculateError(BUFFER *buffer, int width);
void PickMinErrorTriangles(BUFFER *buffer, int width);
QuadtreeNode *GetChild(int verticalSide, int horizontalSide);
void SetChildNeighbors();
int Subdivide();
int Descendants(list<QuadtreeNode *> *nodes);
int Leaves(list<QuadtreeNode *> *leaves);
bool IsLeaf();
bool HasGrandChildren();
bool IsAdjacent(QuadtreeNode *other, int *adjacentSide);
QuadtreeNode *GetAncestralNeighbor(int side);
// bool operator<(QuadtreeNode &x) { return (m_error < x.m_error); }
// bool operator>(QuadtreeNode &x) { return (m_error > x.m_error); }
// bool operator==(QuadtreeNode &x) { return (m_error == x.m_error); }
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
QuadtreeNode *m_parent;
list<QuadtreeNode *> *m_children;
QuadtreeNode *m_neighbors[4];
PixelCoords m_lowerLeft,m_upperRight;
Line m_leftRightDiagonal,m_rightLeftDiagonal;
bool m_diagonalIsLeftRight;
POS2D m_triangleGradients[2];
Plane m_trianglePlanes[2];
bool m_goodSide[4];
double m_error;
protected:
private:
void SetExternalNeighbor(int parentSide, int childQualifier);
bool OverLappingInterval(int aLeft, int aRight, int bLeft, int bRight);
};
class Quadtree
{
public:
Quadtree(BUFFER *buffer, int width, int height);
void Subdivide(double threshold, int maxNumTriangles);
void CreateTriangleMesh();
QuadtreeNode *Root() { return m_root; }
void Root(QuadtreeNode *root) { m_root = root; }
protected:
private:
struct ContentsGreater
{
bool operator()(QuadtreeNode *x, QuadtreeNode *y)
{
return (x->m_error > y->m_error);
}
};
QuadtreeNode *m_root;
BUFFER *m_buffer;
int m_bufferWidth;
int m_bufferHeight;
};
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
/* -- Forward function declarations -- */
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
static void AddQuadTriangles(QuadtreeNode *quad, BUFFER *buffer,
int width, int *numTriangles, int *numVertices,
int *indexMap);
static void MendTriangleMeshSeams(BUFFER *buffer, int width, int *indexMap,
Quadtree *quadtree);
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
/* -- Global variables private to this file -- */
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
/* -- For Debugging -- */
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
// #if 0
// static void
// FindGradientMinMax(BUFFER *buffer, int size, double *min, double *max,
// char direction)
// {
// pixel *disparities = buffer->disp;
// int index;
// double magGradient;
// /* find first good pixel */
// for (index = 0; index < size; index++)
// if ((disparities[index] != 0) && (disparities[index] != MISSING_PIXEL) &&
// (disparities[index] != FAR_FIELD_PIXEL))
// break;
// switch (direction)
// {
// case 'x':
// *min = *max = buffer->gradients[index++].x;
// for (; index < size; index++)
// {
// if ((disparities[index] != 0) && (disparities[index] != MISSING_PIXEL) &&
// (disparities[index] != FAR_FIELD_PIXEL)) {
// if (buffer->gradients[index].x < *min) {
// *min = buffer->gradients[index].x;
// }
// else if (buffer->gradients[index].x > *max) {
// *max = buffer->gradients[index].x;
// }
// }
// }
// break;
// case 'y':
// *min = *max = buffer->gradients[index++].y;
// for (; index < size; index++)
// {
// if ((disparities[index] != 0) && (disparities[index] != MISSING_PIXEL) &&
// (disparities[index] != FAR_FIELD_PIXEL)) {
// if (buffer->gradients[index].y < *min) {
// *min = buffer->gradients[index].y;
// }
// else if (buffer->gradients[index].y > *max) {
// *max = buffer->gradients[index].y;
// }
// }
// }
// break;
// case 'm':
// *min = *max = sqrt(buffer->gradients[index].x*buffer->gradients[index].x +
// buffer->gradients[index].y*buffer->gradients[index].y);
// index++;
// for (; index < size; index++)
// {
// if ((disparities[index] != 0) && (disparities[index] != MISSING_PIXEL) &&
// (disparities[index] != FAR_FIELD_PIXEL))
// {
// magGradient =
// sqrt(buffer->gradients[index].x*buffer->gradients[index].x +
// buffer->gradients[index].y*buffer->gradients[index].y);
// if (magGradient < *min)
// *min = magGradient;
// else if (magGradient > *max)
// *max = magGradient;
// }
// }
// }
// }
// static void
// FindGradientMax(BUFFER *buffer, int size, double *max, char direction)
// {
// pixel *disparities = buffer->disp;
// int index;
// double magGradient;
// /* find first good pixel */
// for (index = 0; index < size; index++)
// if ((disparities[index] != 0) && (disparities[index] != MISSING_PIXEL) &&
// (disparities[index] != FAR_FIELD_PIXEL))
// break;
// switch (direction)
// {
// case 'x':
// *max = fabs(buffer->gradients[index++].x);
// for (; index < size; index++)
// {
// if (buffer->gradients[index].x > *max)
// *max = fabs(buffer->gradients[index].x);
// }
// break;
// case 'y':
// *max = fabs(buffer->gradients[index++].y);
// for (; index < size; index++)
// {
// if ((disparities[index] != 0) && (disparities[index] != MISSING_PIXEL) &&
// (disparities[index] != FAR_FIELD_PIXEL))
// if (buffer->gradients[index].y > *max)
// *max = fabs(buffer->gradients[index].y);
// }
// break;
// case 'm':
// *max = sqrt(buffer->gradients[index].x*buffer->gradients[index].x +
// buffer->gradients[index].y*buffer->gradients[index].y);
// index++;
// for (; index < size; index++)
// {
// magGradient = sqrt(buffer->gradients[index].x*buffer->gradients[index].x +
// buffer->gradients[index].y*buffer->gradients[index].y);
// if (magGradient > *max)
// *max = magGradient;
// }
// }
// }
// static void
// MapGradientToImage(BUFFER *buffer, int size, double min, double max,
// int direction, pixel *imageBuffer)
// {
// double absGrad, scale, offset, range = max - min;
// pixel *disparities = buffer->disp;
// double fudge = 0.5;
// int i;
// scale = 255.0/range;
// offset = -min*scale;
// switch (direction)
// {
// case 'x':
// for (i=0; i < size; i++)
// {
// if ((disparities[i] != 0) && (disparities[i] != MISSING_PIXEL) &&
// (disparities[i] != FAR_FIELD_PIXEL))
// {
// absGrad = fabs(buffer->gradients[i].x);
// imageBuffer[i] = scale*absGrad + offset;
// }
// else
// imageBuffer[i] = min;
// }
// break;
// case 'y':
// for (i=0; i < size; i++)
// {
// if ((disparities[i] != 0) && (disparities[i] != MISSING_PIXEL) &&
// (disparities[i] != FAR_FIELD_PIXEL))
// {
// absGrad = fabs(buffer->gradients[i].y);
// imageBuffer[i] = scale*absGrad + offset;
// }
// else
// imageBuffer[i] = min;
// }
// break;
// case 'm':
// for (i=0; i < size; i++)
// {
// if ((disparities[i] != 0) && (disparities[i] != MISSING_PIXEL) &&
// (disparities[i] != FAR_FIELD_PIXEL))
// {
// absGrad = sqrt(buffer->gradients[i].x*buffer->gradients[i].x +
// buffer->gradients[i].y*buffer->gradients[i].y);
// if ((fudge*scale*absGrad + offset) <= 255.0)
// imageBuffer[i] = fudge*scale*absGrad + offset;
// else
// imageBuffer[i] = 255;
// }
// else
// imageBuffer[i] = min;
// }
// break;
// }
// }
// #endif
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
/* -- PixelCoord functions -- */
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
int
PixelCoords::PixelIndex(int imageWidth)
{
return (row * imageWidth + col);
}
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
/* -- Line functions -- */
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
double
Line::EvaluateLineFunction(POS2D point)
{
/* If a^2 + b^2 == 1 then the value returned is the signed distance of */
/* the point to the line. */
/* If c < 0 then positive values mean the point is on the same side */
/* as the normal. */
return (a*point.x + b*point.y + c);
}
void
Line::SetEquation(POS2D *pt1, POS2D *pt2)
{
double deltaX = pt2->x - pt1->x;
double deltaY = pt2->y - pt1->y;
a = deltaY;
b = -deltaX;
c = -a*pt1->x - b*pt1->y;
}
void
Line::SetEquation(PixelCoords *pt1, PixelCoords *pt2)
{
double deltaX = pt2->col - pt1->col;
double deltaY = pt2->row - pt1->row;
a = deltaY;
b = -deltaX;
c = -a * ((double) pt1->col) - b * ((double) pt1->row);
}
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
/* -- Plane functions -- */
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
double
Plane::EvaluatePlaneFunction(POS3D *point)
{
/* If a^2 + b^2 + c^2 == 1 then the value returned is the signed distance */
/* of the point to the plane. */
/* If d < 0 then positive values mean the point is on the same side */
/* as the normal. */
return (a*point->x + b*point->y + c*point->z + d);
}
void
Plane::SetEquation(POS3D *pt1, POS3D *pt2, POS3D *pt3)
{
POS3D normal,*v1, *v2;
double normalizer,mag;
/* This is Newell's method for generating a plane equation */
v1 = pt1;
v2 = pt2;
normal.x = (v1->y - v2->y) * (v1->z + v2->z);
normal.y = (v1->z - v2->z) * (v1->x + v2->x);
normal.z = (v1->x - v2->x) * (v1->y + v2->y);
v1 = pt2;
v2 = pt3;
normal.x += (v1->y - v2->y) * (v1->z + v2->z);
normal.y += (v1->z - v2->z) * (v1->x + v2->x);
normal.z += (v1->x - v2->x) * (v1->y + v2->y);
v1 = pt3;
v2 = pt1;
normal.x += (v1->y - v2->y) * (v1->z + v2->z);
normal.y += (v1->z - v2->z) * (v1->x + v2->x);
normal.z += (v1->x - v2->x) * (v1->y + v2->y);
if ((mag = sqrt(normal.x*normal.x + normal.y*normal.y +
normal.z*normal.z)) == 0)
{
/* If this happens points are geometrically coincident, and we
arbitrarily pick a normal */
/* printf("Warning in SetEquation: points are "
"geometrically concident.\n"); */
a = b = 0.0;
c = 1.0;
d = 0.0;
return;
}
normalizer = 1.0/mag;
a = normal.x * normalizer;
b = normal.y * normalizer;
c = normal.z * normalizer;
d = -(pt1->x*a + pt1->y*b + pt1->z*c);
}
void
Plane::GetEquation(double coefficients[4])
{
coefficients[0] = a;
coefficients[1] = b;
coefficients[2] = c;
coefficients[3] = d;
}
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
/* -- QuadtreeNode Functions -- */
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
QuadtreeNode::QuadtreeNode(PixelCoords lowerLeft, PixelCoords upperRight)
{ /* QuadtreeNode constructor */
PixelCoords lowerRight(upperRight.col,lowerLeft.row);
PixelCoords upperLeft(lowerLeft.col,upperRight.row);
m_parent = 0;
m_children = new list<QuadtreeNode *>;
m_neighbors[0] = m_neighbors[1] = m_neighbors[2] = m_neighbors[3] = 0;
m_lowerLeft = lowerLeft;
m_upperRight = upperRight;
/* diagonal from lower left to upper right */
m_leftRightDiagonal.SetEquation(&lowerLeft,&upperRight);
/* diagonal from lower right to upper left */
m_rightLeftDiagonal.SetEquation(&lowerRight,&upperLeft);
/* arbitrarily choose one diagonal initially */
m_diagonalIsLeftRight = 1;
m_goodSide[0] = m_goodSide[1] = m_goodSide[2] = m_goodSide[3] = false;
m_error = 0.0;
}
void
QuadtreeNode::SetTriangleGradients(BUFFER *buffer, int width)
{
POS3D lowerLeft,lowerRight,upperLeft,upperRight;
int pixelIndex, rowStartIndex = m_lowerLeft.row * width;
pixelIndex = rowStartIndex + m_lowerLeft.col;
lowerLeft = buffer->dot[pixelIndex];
pixelIndex = rowStartIndex + m_upperRight.col;
lowerRight = buffer->dot[pixelIndex];
rowStartIndex = m_upperRight.row *width;
pixelIndex = rowStartIndex + m_lowerLeft.col;
upperLeft = buffer->dot[pixelIndex];
pixelIndex = rowStartIndex + m_upperRight.col;
upperRight = buffer->dot[pixelIndex];
if (m_diagonalIsLeftRight)
{
m_triangleGradients[0].x = lowerRight.z - lowerLeft.z;
m_triangleGradients[0].y = upperRight.z - lowerRight.z;
m_triangleGradients[1].x = upperRight.z - upperLeft.z;
m_triangleGradients[1].y = upperLeft.z - lowerLeft.z;
}
else
{
m_triangleGradients[0].x = upperRight.z - upperLeft.z;
m_triangleGradients[0].y = upperRight.z - lowerRight.z;
m_triangleGradients[1].x = lowerRight.z - lowerLeft.z;
m_triangleGradients[1].y = upperLeft.z - lowerLeft.z;
}
}
void
QuadtreeNode::SetTrianglePlanes(BUFFER *buffer, int width)
{
POS3D lowerLeft,lowerRight,upperLeft,upperRight;
int pixelIndex, rowStartIndex = m_lowerLeft.row * width;
pixelIndex = rowStartIndex + m_lowerLeft.col;
lowerLeft = buffer->dot[pixelIndex];
pixelIndex = rowStartIndex + m_upperRight.col;
lowerRight = buffer->dot[pixelIndex];
rowStartIndex = m_upperRight.row *width;
pixelIndex = rowStartIndex + m_lowerLeft.col;
upperLeft = buffer->dot[pixelIndex];
pixelIndex = rowStartIndex + m_upperRight.col;
upperRight = buffer->dot[pixelIndex];
if (m_diagonalIsLeftRight)
{
m_trianglePlanes[0].SetEquation(&lowerLeft,&lowerRight,&upperRight);
m_trianglePlanes[1].SetEquation(&upperLeft,&lowerLeft,&upperRight);
}
else
{
m_trianglePlanes[0].SetEquation(&upperRight,&upperLeft,&lowerRight);
m_trianglePlanes[1].SetEquation(&lowerLeft,&lowerRight,&upperLeft);
}
}
void
QuadtreeNode::SetTriangles(int leftToRight, BUFFER *buffer, int width)
{
m_diagonalIsLeftRight = leftToRight;
SetTriangleGradients(buffer,width);
SetTrianglePlanes(buffer,width);
}
void
QuadtreeNode::SetError(double error)
{
m_error = error;
}
void
QuadtreeNode::AverageDistanceError(BUFFER *buffer, int width, double *error)
{
int row,col,rowStartIndex,pixelIndex,numPixels;
int minRow,minCol,stopRow,stopCol,quadHeight,quadWidth;
POS2D point;
Line *diagonal;
double d;
quadWidth = m_upperRight.col - m_lowerLeft.col + 1;
quadHeight = m_upperRight.row - m_lowerLeft.row + 1;
numPixels = quadWidth*quadHeight;
minRow = m_lowerLeft.row;
minCol = m_lowerLeft.col;
stopRow = minRow+quadHeight;
stopCol = minCol+quadWidth;
rowStartIndex = minRow*width;
diagonal = (m_diagonalIsLeftRight ?
&(m_leftRightDiagonal) : &(m_rightLeftDiagonal));
*error = 0.0;
for (row = minRow; row < stopRow; rowStartIndex += width, row++)
{
for (col = minCol; col < stopCol; col++)
{
pixelIndex = rowStartIndex+col;
point.x = col;
point.y = row;
if ((d = diagonal->EvaluateLineFunction(point)) > 0.0)
*error += fabs(m_trianglePlanes[0].
EvaluatePlaneFunction(&buffer->dot[pixelIndex]));
else if (d <= 0.0)
*error += fabs(m_trianglePlanes[1].
EvaluatePlaneFunction(&buffer->dot[pixelIndex]));
}
}
/* we average the distance error... */
*error = *error / (double) numPixels;
}
void
QuadtreeNode::MaxDistanceError(BUFFER *buffer, int width, double *error)
{
int row,col,rowStartIndex,pixelIndex;
int minRow,minCol,stopRow,stopCol,quadHeight,quadWidth;
POS2D point;
Line *diagonal;
double dist;
quadWidth = m_upperRight.col - m_lowerLeft.col + 1;
quadHeight = m_upperRight.row - m_lowerLeft.row + 1;
minRow = m_lowerLeft.row;
minCol = m_lowerLeft.col;
stopRow = minRow+quadHeight;
stopCol = minCol+quadWidth;
rowStartIndex = minRow*width;
diagonal = (m_diagonalIsLeftRight ?
&(m_leftRightDiagonal) : &(m_rightLeftDiagonal));
*error = 0.0;
for (row = minRow; row < stopRow; rowStartIndex += width, row++)
{
for (col = minCol; col < stopCol; col++)
{
pixelIndex = rowStartIndex+col;
point.x = col;
point.y = row;
if ((diagonal->EvaluateLineFunction(point)) > 0.0)
dist = fabs(m_trianglePlanes[0].
EvaluatePlaneFunction(&buffer->dot[pixelIndex]));
else
dist = fabs(m_trianglePlanes[1].
EvaluatePlaneFunction(&buffer->dot[pixelIndex]));
if (dist > *error)
*error = dist;
}
}
}
/* Can do this calc. (at least) two different ways: find the max */
/* error, or find the average error. I think the latter method */
/* should give better results */
void
QuadtreeNode::OldGradientError(BUFFER *buffer, int width, POS2D *diff)
{
int row,col,rowStartIndex,pixelIndex,numPixels;
int minRow,minCol,stopRow,stopCol,quadHeight,quadWidth;
POS2D point;
Line *diagonal;
double d;
quadWidth = m_upperRight.col - m_lowerLeft.col + 1;
quadHeight = m_upperRight.row - m_lowerLeft.row + 1;
numPixels = quadWidth*quadHeight;
minRow = m_lowerLeft.row;
minCol = m_lowerLeft.col;
stopRow = minRow+quadHeight;
stopCol = minCol+quadWidth;
rowStartIndex = minRow*width;
diagonal = (m_diagonalIsLeftRight ?
&(m_leftRightDiagonal) : &(m_rightLeftDiagonal));
diff->x = diff->y = 0.0;
for (row = minRow; row < stopRow; rowStartIndex += width, row++)
{
for (col = minCol; col < stopCol; col++)
{
pixelIndex = rowStartIndex+col;
point.x = col;
point.y = row;
if ((d = diagonal->EvaluateLineFunction(point)) > 0.0)
{
diff->x += buffer->gradients[pixelIndex].x - m_triangleGradients[0].x;
diff->y += buffer->gradients[pixelIndex].y - m_triangleGradients[0].y;
}
else if (d < 0.0)
{
diff->x += buffer->gradients[pixelIndex].x - m_triangleGradients[1].x;
diff->y += buffer->gradients[pixelIndex].y - m_triangleGradients[1].y;
}
else /* we average the triangle gradients */
{
diff->x += (buffer->gradients[pixelIndex].x -
0.5*(m_triangleGradients[0].x + m_triangleGradients[1].x));
diff->y += (buffer->gradients[pixelIndex].y -
0.5*(m_triangleGradients[0].y + m_triangleGradients[1].y));
}
}
}
/* we average the gradient error... */
diff->x = diff->x / (double) numPixels;
diff->y = diff->y / (double) numPixels;
}
void
QuadtreeNode::AverageGradientError(BUFFER *buffer, int width, double *error)
{
int row,col,rowStartIndex,pixelIndex,numPixels;
int minRow,minCol,stopRow,stopCol,quadHeight,quadWidth;
POS2D diff,point;
Line *diagonal;
double d;
quadWidth = m_upperRight.col - m_lowerLeft.col + 1;
quadHeight = m_upperRight.row - m_lowerLeft.row + 1;
numPixels = quadWidth*quadHeight;
minRow = m_lowerLeft.row;
minCol = m_lowerLeft.col;
stopRow = minRow+quadHeight;
stopCol = minCol+quadWidth;
rowStartIndex = minRow*width;
diagonal = (m_diagonalIsLeftRight ?
&(m_leftRightDiagonal) : &(m_rightLeftDiagonal));
*error = 0.0;
for (row = minRow; row < stopRow; rowStartIndex += width, row++)
{
for (col = minCol; col < stopCol; col++)
{
pixelIndex = rowStartIndex+col;
point.x = col;
point.y = row;
if ((d = diagonal->EvaluateLineFunction(point)) > 0.0)
{
diff.x = buffer->gradients[pixelIndex].x - m_triangleGradients[0].x;
diff.y = buffer->gradients[pixelIndex].y - m_triangleGradients[0].y;
}
else if (d < 0.0)
{
diff.x = buffer->gradients[pixelIndex].x - m_triangleGradients[1].x;
diff.y = buffer->gradients[pixelIndex].y - m_triangleGradients[1].y;
}
else /* we average the triangle gradients */
{
diff.x = (buffer->gradients[pixelIndex].x -
0.5*(m_triangleGradients[0].x + m_triangleGradients[1].x));
diff.y = (buffer->gradients[pixelIndex].y -
0.5*(m_triangleGradients[0].y + m_triangleGradients[1].y));
}
*error += sqrt(diff.x*diff.x + diff.y*diff.y);
}
}
/* we average the gradient error... */
*error = *error / (double) numPixels;
}
void
QuadtreeNode::MaxGradientError(BUFFER *buffer, int width, double *error)
{
int row,col,rowStartIndex,pixelIndex;
int minRow,minCol,stopRow,stopCol,quadHeight,quadWidth;
POS2D diff,point;
Line *diagonal;
double d,magSqrDiff;
quadWidth = m_upperRight.col - m_lowerLeft.col + 1;
quadHeight = m_upperRight.row - m_lowerLeft.row + 1;
minRow = m_lowerLeft.row;
minCol = m_lowerLeft.col;
stopRow = minRow+quadHeight;
stopCol = minCol+quadWidth;
rowStartIndex = minRow*width;
diagonal = (m_diagonalIsLeftRight ?
&(m_leftRightDiagonal) : &(m_rightLeftDiagonal));
*error = 0.0;
for (row = minRow; row < stopRow; rowStartIndex += width, row++)
{
for (col = minCol; col < stopCol; col++)
{
pixelIndex = rowStartIndex+col;
point.x = col;
point.y = row;
if ((d = diagonal->EvaluateLineFunction(point)) > 0.0)
{
diff.x = buffer->gradients[pixelIndex].x - m_triangleGradients[0].x;
diff.y = buffer->gradients[pixelIndex].y - m_triangleGradients[0].y;
}
else if (d < 0.0)
{
diff.x = buffer->gradients[pixelIndex].x - m_triangleGradients[1].x;
diff.y = buffer->gradients[pixelIndex].y - m_triangleGradients[1].y;
}
else /* we average the triangle gradients */
{
diff.x = (buffer->gradients[pixelIndex].x -
0.5*(m_triangleGradients[0].x + m_triangleGradients[1].x));
diff.y = (buffer->gradients[pixelIndex].y -
0.5*(m_triangleGradients[0].y + m_triangleGradients[1].y));
}
if ((magSqrDiff = (diff.x*diff.x + diff.y*diff.y)) > *error)
*error = magSqrDiff;
}
}
*error = sqrt(*error);
}
double
QuadtreeNode::CalculateError(BUFFER *buffer, int width)
{
double error;
AverageDistanceError(buffer,width,&error);
#if 0
MaxDistanceError(buffer,width,&error);
MaxGradientError(buffer,width,&error);
AverageGradientError(buffer,width,&error);
#endif
return (error);
}
void
QuadtreeNode::PickMinErrorTriangles(BUFFER *buffer, int width)
{
double error,minError;
SetTriangles(RightToLeftDiagonal,buffer,width);
minError = CalculateError(buffer,width);
SetTriangles(LeftToRightDiagonal,buffer,width);
if ((error = CalculateError(buffer,width)) < minError)
minError = error;
else /* use the original 2 triangles we tried */
SetTriangles(RightToLeftDiagonal,buffer,width);
SetError(minError);
}
QuadtreeNode *
QuadtreeNode::GetChild(int verticalSide, int horizontalSide)
{
int numChildren,quadWidth,i,iStop;
list<QuadtreeNode *>::iterator element;
if (m_children->empty())
return (0);
numChildren = m_children->size();
element = m_children->begin();
if (numChildren == 4)
{
if (horizontalSide == LeftSide)
iStop = (verticalSide == TopSide) ? 2 : 0;
else
iStop = (verticalSide == TopSide) ? 3 : 1;
}
else if (numChildren == 2)
{
quadWidth = m_upperRight.col - m_lowerLeft.col + 1;
if (quadWidth > 2)
iStop = (horizontalSide == LeftSide) ? 0 : 1;
else
iStop = (verticalSide == BottomSide) ? 0 : 1;
}
else /* should never happen */
{
printf("ERROR in GetChild: numChildren(=%d) is not 0, 2, or 4. "
"Aborting.\n",numChildren);
return (0);
}
for (i = 0; i < iStop; i++)
++element;
return (*element);
}
void
QuadtreeNode::SetExternalNeighbor(int parentSide, int childQualifier)
{
QuadtreeNode *neighborChild;
int neighborSide;
if (m_parent == 0)
return;
if (m_parent->m_neighbors[parentSide] == 0)
{
m_neighbors[parentSide] = 0;
return;
}
if (parentSide > RightSide)
{
neighborSide = BottomSide+TopSide - parentSide;
neighborChild =
m_parent->m_neighbors[parentSide]->GetChild(neighborSide,childQualifier);
}
else
{
neighborSide = LeftSide+RightSide - parentSide;
neighborChild =
m_parent->m_neighbors[parentSide]->GetChild(childQualifier,neighborSide);
}
m_neighbors[parentSide] = neighborChild;
if (neighborChild != 0)
neighborChild->m_neighbors[neighborSide] = this;
}
void
QuadtreeNode::SetChildNeighbors()
{
QuadtreeNode *children[4];
list<QuadtreeNode *>::iterator element;
int i,numChildren, quadWidth = m_upperRight.col - m_lowerLeft.col + 1;
for (i = 0, element = m_children->begin(); !(element == m_children->end());
element++, i++)
children[i] = *element;
numChildren = i;
/* external neighbors common to both 4 child and 2 child subdivisions */
children[0]->SetExternalNeighbor(LeftSide,BottomSide);
children[0]->SetExternalNeighbor(BottomSide,LeftSide);
if (numChildren == 4)
{
/* external neighbors */
children[1]->SetExternalNeighbor(BottomSide,RightSide);
children[1]->SetExternalNeighbor(RightSide,BottomSide);
children[2]->SetExternalNeighbor(LeftSide,TopSide);
children[2]->SetExternalNeighbor(TopSide,LeftSide);
children[3]->SetExternalNeighbor(RightSide,TopSide);
children[3]->SetExternalNeighbor(TopSide,RightSide);
/* internal neigbors */
children[0]->m_neighbors[RightSide] = children[1];
children[1]->m_neighbors[LeftSide] = children[0];
children[0]->m_neighbors[TopSide] = children[2];
children[2]->m_neighbors[BottomSide] = children[0];
children[1]->m_neighbors[TopSide] = children[3];
children[3]->m_neighbors[BottomSide] = children[1];
children[2]->m_neighbors[RightSide] = children[3];
children[3]->m_neighbors[LeftSide] = children[2];
}
else if (numChildren == 2) /* numChildren should always be 2 or 4 */
{
if (quadWidth < 3)
{
/* external neighbors */
children[0]->SetExternalNeighbor(RightSide,BottomSide);
children[1]->SetExternalNeighbor(LeftSide,TopSide);
children[1]->SetExternalNeighbor(RightSide,TopSide);
children[1]->SetExternalNeighbor(TopSide,RightSide);
/* internal neighbors */
children[1]->m_neighbors[BottomSide] = children[0];
children[0]->m_neighbors[TopSide] = children[1];
}
else
{
/* external neighbors */
children[0]->SetExternalNeighbor(TopSide,LeftSide);
children[1]->SetExternalNeighbor(RightSide,TopSide);
children[1]->SetExternalNeighbor(TopSide,RightSide);
children[1]->SetExternalNeighbor(BottomSide,RightSide);
children[1]->m_neighbors[RightSide] = m_neighbors[RightSide];
children[1]->m_neighbors[TopSide] = m_neighbors[TopSide];
children[1]->m_neighbors[BottomSide] = m_neighbors[BottomSide];
/* internal neighbors */
children[0]->m_neighbors[RightSide] = children[1];
children[1]->m_neighbors[LeftSide] = children[0];
}
}
else
{
printf("ERROR in SetChildNeighbors: numChildren is not 2 or 4. "
"Aborting.\n");
}
}
int
QuadtreeNode::Subdivide()
{
PixelCoords leftMiddle,lowerMiddle,rightMiddle,upperMiddle,middle;
QuadtreeNode *newQuadtreeNodes[4] = {0,0,0,0};
int quadWidth,quadHeight, numChildren = 0;
// int i,j; /* debugging */
if (!m_children->empty()) /* only subdivide leaf nodes */
{
printf("WARNING in Subdivide(): this QuadtreeNode already has children. "
"Aborting.\n");
return (numChildren);
}
/* cannot subdivide if quad has <= 4 pixels */
/* also must have at least 2 pixels in both dimensions */
quadWidth = m_upperRight.col - m_lowerLeft.col + 1;
quadHeight = m_upperRight.row - m_lowerLeft.row + 1;
if ((quadWidth < 2) || (quadHeight < 2) ||
((quadWidth < 3) && (quadHeight < 3)))
return (numChildren);
/* set indices of new quad corners */
middle.row = (m_lowerLeft.row + m_upperRight.row)/2;
middle.col = (m_lowerLeft.col + m_upperRight.col)/2;
if (quadHeight == 2)
middle.row += 1;
if (quadWidth == 2)
middle.col += 1;
leftMiddle.row = middle.row;
leftMiddle.col = m_lowerLeft.col;
rightMiddle.row = middle.row;
rightMiddle.col = m_upperRight.col;
lowerMiddle.row = m_lowerLeft.row;
lowerMiddle.col = middle.col;
upperMiddle.row = m_upperRight.row;
upperMiddle.col = middle.col;
/* the order of the children will be:
lower-left, lower-right, upper-left, upper-right */
/*
* If the mesh elements are much wider than they are long, or vice versa,
* we should subdivide the mesh in the direction that makes the elements more
* square.
*/
if (quadWidth > (2 * quadHeight))
{
newQuadtreeNodes[0] = new QuadtreeNode(m_lowerLeft,upperMiddle);
newQuadtreeNodes[0]->m_parent = this;
m_children->push_back(newQuadtreeNodes[0]);
numChildren++;
if (quadWidth > 2)
{
newQuadtreeNodes[1] = new QuadtreeNode(lowerMiddle,m_upperRight);
newQuadtreeNodes[1]->m_parent = this;
m_children->push_back(newQuadtreeNodes[1]);
numChildren++;
}
}
else if (quadHeight > (2 * quadWidth))
{
newQuadtreeNodes[0] = new QuadtreeNode(m_lowerLeft, rightMiddle);
newQuadtreeNodes[0]->m_parent = this;
m_children->push_back(newQuadtreeNodes[0]);
numChildren++;
if (quadHeight > 2)
{
newQuadtreeNodes[1] = new QuadtreeNode(leftMiddle,m_upperRight);
newQuadtreeNodes[1]->m_parent = this;
m_children->push_back(newQuadtreeNodes[1]);
numChildren++;
}
}
else /* The aspect ratio of the quads is fairly square */
{
newQuadtreeNodes[0] = new QuadtreeNode(m_lowerLeft,middle);
newQuadtreeNodes[0]->m_parent = this;
m_children->push_back(newQuadtreeNodes[0]);
numChildren++;
if (quadWidth > 2)
{
newQuadtreeNodes[1] = new QuadtreeNode(lowerMiddle,rightMiddle);
newQuadtreeNodes[1]->m_parent = this;
m_children->push_back(newQuadtreeNodes[1]);
numChildren++;
}
if (quadHeight > 2)
{
newQuadtreeNodes[numChildren] = new QuadtreeNode(leftMiddle,upperMiddle);
newQuadtreeNodes[numChildren]->m_parent = this;
m_children->push_back(newQuadtreeNodes[numChildren]);
numChildren++;
}
if ((quadWidth > 2) && (quadHeight > 2))
{
newQuadtreeNodes[3] = new QuadtreeNode(middle,m_upperRight);
newQuadtreeNodes[3]->m_parent = this;
m_children->push_back(newQuadtreeNodes[3]);
numChildren++;
}
}
SetChildNeighbors();
// for (i = 0; i < numChildren; i++)
// for (j = 0; j < numChildren; j++)
// if ((newQuadtreeNodes[i] != 0) &&
// (newQuadtreeNodes[i]->m_neighbors[j] != 0))
// assert(IsAdjacent(newQuadtreeNodes[i],
// newQuadtreeNodes[i]->m_neighbors[j],&j));
return (numChildren);
}
/* FIX: Larry should really document what funky recursive code like
this is supposed to do. */
/* Descendants finds all the nodes below a given node in a Quadtree
and places them in a list.
The tree is traversed in preorder fashion.
*/
int
QuadtreeNode::Descendants(list<QuadtreeNode *> *nodes)
{
static int recurseLevel = 0;
static int numNodes = 0;
QuadtreeNode *childNode;
list<QuadtreeNode *>::iterator element,endElement;
int returnValue;
++recurseLevel;
++numNodes;
nodes->push_back(this);
element = m_children->begin();
endElement = m_children->end();
while (!(element == endElement))
{
childNode = *element;
childNode->Descendants(nodes);
++element;
}
--recurseLevel;
returnValue = numNodes;
if (0 == recurseLevel)
numNodes = 0;
return (returnValue);
}
int
QuadtreeNode::Leaves(list<QuadtreeNode *> *leaves)
{
list<QuadtreeNode *>::iterator element,endElement,nextElement;
QuadtreeNode *node;
unsigned int numLeaves;
numLeaves = Descendants(leaves);
assert(leaves->size() == numLeaves);
element = leaves->begin();
endElement = leaves->end();
while (!(element == endElement))
{
nextElement = element;
++nextElement;
node = *element;
if (!node->m_children->empty())
{
leaves->erase(element);
--numLeaves;
if (0 > numLeaves)
{
fprintf (stdout,"Leaves: numLeaves < 0 (%d)\n",numLeaves);
abort();
}
}
element = nextElement;
}
return (numLeaves);
}
bool
QuadtreeNode::IsLeaf()
{
return (m_children->empty());
}
bool
QuadtreeNode::HasGrandChildren()
{
list<QuadtreeNode *>::iterator element;
QuadtreeNode *childQuadtreeNode;
element = m_children->begin();
while (!(element == m_children->end()))
{
childQuadtreeNode = *element;
if (!childQuadtreeNode->IsLeaf())
return (true);
++element;
}
return (false);
}
QuadtreeNode *
QuadtreeNode::GetAncestralNeighbor(int side)
{
QuadtreeNode *neighbor;
if (m_parent == 0)
return (0);
if ((neighbor = m_parent->m_neighbors[side]) != 0)
return (neighbor);
else
return (m_parent->GetAncestralNeighbor(side));
}
bool
QuadtreeNode::OverLappingInterval(int aLeft, int aRight, int bLeft, int bRight)
{
return (((aRight <= bRight) && (aLeft >= bLeft)) ||
((aRight >= bRight) && (aLeft <= bLeft)));
}
bool
QuadtreeNode::IsAdjacent(QuadtreeNode *other, int *adjacentSide)
{
/* if side is passed (i.e. != -1) we only look for adjacency on that side */
if (*adjacentSide == -1)
{
if ((m_lowerLeft.row == other->m_upperRight.row) &&
OverLappingInterval(m_lowerLeft.col,m_upperRight.col,
other->m_lowerLeft.col,
other->m_upperRight.col))
*adjacentSide = BottomSide;
else if ((m_upperRight.row == other->m_lowerLeft.row) &&
OverLappingInterval(m_lowerLeft.col,m_upperRight.col,
other->m_lowerLeft.col,
other->m_upperRight.col))
*adjacentSide = TopSide;
else if ((m_upperRight.col == other->m_lowerLeft.col) &&
OverLappingInterval(m_lowerLeft.row,m_upperRight.row,
other->m_lowerLeft.row,
other->m_upperRight.row))
*adjacentSide = RightSide;
else if ((m_lowerLeft.col == other->m_upperRight.col) &&
OverLappingInterval(m_lowerLeft.row,m_upperRight.row,
other->m_lowerLeft.row,
other->m_upperRight.row))
*adjacentSide = LeftSide;
else
*adjacentSide = -1;
}
else
{
switch (*adjacentSide)
{
case BottomSide:
if ((m_lowerLeft.row != other->m_upperRight.row) ||
!OverLappingInterval(m_lowerLeft.col,m_upperRight.col,
other->m_lowerLeft.col,other->m_upperRight.col))
*adjacentSide = -1;
break;
case TopSide:
if ((m_upperRight.row != other->m_lowerLeft.row) ||
!OverLappingInterval(m_lowerLeft.col,m_upperRight.col,
other->m_lowerLeft.col,other->m_upperRight.col))
*adjacentSide = -1;
break;
case RightSide:
if ((m_upperRight.col != other->m_lowerLeft.col) ||
!OverLappingInterval(m_lowerLeft.row,m_upperRight.row,
other->m_lowerLeft.row,other->m_upperRight.row))
*adjacentSide = -1;
break;
case LeftSide:
if ((m_lowerLeft.col != other->m_upperRight.col) ||
!OverLappingInterval(m_lowerLeft.row,m_upperRight.row,
other->m_lowerLeft.row,other->m_upperRight.row))
*adjacentSide = -1;
break;
default:
*adjacentSide = -1;
break;
}
}
return (*adjacentSide != -1);
}
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
/* -- Quadtree Functions -- */
/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
/* Quadtree constructor */
Quadtree::Quadtree(BUFFER *buffer, int width, int height)
{
PixelCoords lowerLeft,upperRight;
lowerLeft.row = 0;
lowerLeft.col = 0;
upperRight.row = height - 1;
upperRight.col = width - 1;
m_buffer = buffer;
m_bufferWidth = width;
m_bufferHeight = height;
m_root = new QuadtreeNode(lowerLeft,upperRight);
m_root->m_parent = 0;
/* pick the diagonal of the root that minimizes error */
m_root->PickMinErrorTriangles(buffer,width);
}
#if 0
/* depth-first Recursive version */
void
Quadtree::Subdivide(QuadtreeNode *quad, double threshold)
{
list<QuadtreeNode *>::iterator element;
QuadtreeNode *childQuadtreeNode;
if (m_error < threshold)
return;
else
{
quad->Subdivide(); // Subdivide this quadtreenode
element = quad->m_children->begin();
while (!(element == quad->m_children->end()))
{
childQuadtreeNode = *element;
childQuadtreeNode->PickMinErrorTriangles(m_buffer,m_bufferWidth);
Subdivide(childQuadtreeNode,threshold);
++element;
}
}
}
#endif
/* breadth-first iterative version */
/*
* NOTE :
*
* Eventually we plan to switch over from using an STL list to an STL multiset
* for storing the qElements. There is some commented out code in here to
* that effect, however it does not yet work properly. At some point in the future
* we'll take another look. -Larry and Michael
*/
void
Quadtree::Subdivide(double threshold, int maxNumTriangles)
{
/* For gradient: */
/* static const double threshold = 0.0077; */
/* static const int maxTriangles = 500000; */
/* static const double threshold = -0.5; */
/* static const int maxTriangles = 40; */
/* static const int maxTriangles = 500000; */
list<QuadtreeNode *>::iterator qElement, element, endElement;
// list<QuadtreeNode *>::iterator element, endElement;
// multiset<QuadtreeNode *>::iterator qElement;
QuadtreeNode *childQuadtreeNode,*quadtreeNode;
list<QuadtreeNode *> *queue = new list<QuadtreeNode *>;
// multiset<QuadtreeNode *,ContentsGreater> *queue = new multiset<QuadtreeNode *,ContentsGreater>;
int numTriangles = 0, maxQueueSize = 0, queueSize = 0; /* debugging */
int numChildren;
queue->push_back(m_root);
// queue->insert(m_root);
queueSize = maxQueueSize = 1;
numTriangles += 1*2;
while (!queue->empty() && (numTriangles < maxNumTriangles))
{
qElement = queue->begin();
quadtreeNode = *qElement;
queue->pop_front();
// queue->erase(queue->begin());
--queueSize;
if (quadtreeNode->m_error > threshold)
{
if ((numChildren = quadtreeNode->Subdivide()) > 0)
{
numTriangles -= (1*2);
numTriangles += (numChildren*2);
}
element = quadtreeNode->m_children->begin();
endElement = quadtreeNode->m_children->end();
while (!(element == endElement))
{
childQuadtreeNode = *element;
childQuadtreeNode->PickMinErrorTriangles(m_buffer,m_bufferWidth);
queue->push_back(childQuadtreeNode);
// queue->insert(childQuadtreeNode);
++queueSize;
element = ++element;
}
}
if (queueSize > maxQueueSize)
maxQueueSize = queueSize;
}
queue->clear();
delete queue;
printf("Max queue size = %d\n",maxQueueSize);
printf("Number of triangles = %d\n",numTriangles);
}
void
Quadtree::CreateTriangleMesh()
{
list<QuadtreeNode *> *leaves = new list<QuadtreeNode *>;
list<QuadtreeNode *>::iterator element,endElement;
int numLeaves = 0, numVertices = 0, numPoints = m_bufferWidth*m_bufferHeight;
int *indexMap,pixelIndex,i,j,row,col,rowStart, numTriangles = 0;
float deltaU,deltaV,v;
QuadtreeNode *leaf;
numLeaves = m_root->Leaves(leaves);
if (0 > numLeaves)
{
fprintf(stderr,"CreateTriangleMesh: numLeave=%d\n",numLeaves);
abort();
}
/* Allocate memory for index map */
if ((indexMap = (int *) malloc(numPoints*sizeof(int))) == NULL)
{
fprintf(stderr, "triangle mesh index map allocation failed.\n");
exit(EXIT_FAILURE);
}
for (i = 0; i < numPoints; i++) /* init so we can tell unused indices */
indexMap[i] = -1;
/* Allocate memory for triangle buffer */
if ((m_buffer->nff.triangle =
(NFF_TR *) malloc(2*numLeaves*sizeof(NFF_TR))) == NULL)
{
fprintf(stderr, "CreateTriangleMesh: malloc numLeaves=%d\n",numLeaves );
fprintf(stderr, "nff triangle buffer allocation failed.\n");
exit(EXIT_FAILURE);
}
/* Generate triangle list */
element = leaves->begin();
endElement = leaves->end();
while (!(element == endElement))
{
leaf = *element;
AddQuadTriangles(leaf,m_buffer,m_bufferWidth,
&numTriangles,&numVertices,indexMap);
++element;
}
/* Allocate memory for vertex buffer */
m_buffer->nff.pt_number = numVertices;
if ((m_buffer->nff.vtx =
(POS3D *) malloc(numVertices * sizeof(POS3D))) == NULL)
{
fprintf(stderr, "nff vertex buffer allocation failed.\n");
exit(EXIT_FAILURE);
}
/* Set vertex coordinates */
for (i = 0, j = 0; i < numPoints; i++)
{
if (indexMap[i] != -1)
{
m_buffer->nff.vtx[indexMap[i]] = m_buffer->dot[i];
if (++j >= numVertices)
break;
}
}
MendTriangleMeshSeams(m_buffer,m_bufferWidth,indexMap,this);
/* Allocate memory for texture coord. buffer */
if ((m_buffer->nff.tex =
(UV_TEX *) malloc(numVertices * sizeof(UV_TEX))) == NULL)
{
fprintf(stderr, "nff texture buffer allocation failed.\n");
exit(EXIT_FAILURE);
}
/* Generate texture coordinates */
deltaU = 1.0/(float) m_bufferWidth;
deltaV = 1.0/(float) m_bufferHeight;
for (row = 0, rowStart = 0, j = 0; row < m_bufferHeight; row++)
{
v = deltaV * (float) (m_bufferHeight - row);
for (col = 0; col < m_bufferWidth; col++)
{
pixelIndex = rowStart + col;
if (indexMap[pixelIndex] != -1)
{
m_buffer->nff.tex[indexMap[pixelIndex]].u = deltaU * (float) col;
m_buffer->nff.tex[indexMap[pixelIndex]].v = v;
if (++j >= numVertices)
break;
}
}
rowStart += m_bufferWidth;
}
m_buffer->nff.tr_number = numTriangles;
free(indexMap);
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - //
// Triangle mesh routines
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - //
// Determine if all vertices of a triangle are associated with good
// data.
static bool
GoodTriangle(BUFFER *buffer, int index1, int index2, int index3)
{
bool good;
good = !(buffer->dot[index1].x == 0 && buffer->dot[index1].y == 0 && buffer->dot[index1].z == 0);
good = good && !(buffer->dot[index2].x == 0 && buffer->dot[index2].y == 0 && buffer->dot[index2].z == 0);
good = good && !(buffer->dot[index3].x == 0 && buffer->dot[index3].y == 0 && buffer->dot[index3].z == 0);
return good;
}
// Add a triangle to the mesh
static void
AddTriangle(int index1, int index2, int index3, BUFFER *buffer,
int *numTriangles, int *indexMap, int *numVertices)
{
NFF_TR *triangle;
if (indexMap[index1] == -1)
indexMap[index1] = (*numVertices)++;
if (indexMap[index2] == -1)
indexMap[index2] = (*numVertices)++;
if (indexMap[index3] == -1)
indexMap[index3] = (*numVertices)++;
triangle = &(buffer->nff.triangle[*numTriangles]);
triangle->vtx1 = indexMap[index1];
triangle->vtx2 = indexMap[index2];
triangle->vtx3 = indexMap[index3];
(*numTriangles)++;
}
// Add the triangles associated with a given quad to the mesh
static void
AddQuadTriangles(QuadtreeNode *quad, BUFFER *buffer, int width,
int *numTriangles, int *numVertices, int *indexMap)
{
int quadVertices[4],goodTriangle;
PixelCoords lowerRight,upperLeft;
lowerRight.row = quad->m_lowerLeft.row;
lowerRight.col = quad->m_upperRight.col;
upperLeft.col = quad->m_lowerLeft.col;
upperLeft.row = quad->m_upperRight.row;
quadVertices[0] = quad->m_lowerLeft.PixelIndex(width);
quadVertices[1] = lowerRight.PixelIndex(width);
quadVertices[2] = quad->m_upperRight.PixelIndex(width);
quadVertices[3] = upperLeft.PixelIndex(width);
if (quad->m_diagonalIsLeftRight) /* diagonal is lower-left to upper-right */
{
/* lower triangle */
goodTriangle = GoodTriangle(buffer,quadVertices[0],quadVertices[1],
quadVertices[2]);
quad->m_goodSide[BottomSide] = quad->m_goodSide[RightSide] = goodTriangle;
if (goodTriangle)
AddTriangle(quadVertices[0],quadVertices[1],quadVertices[2],
buffer,numTriangles,indexMap,numVertices);
/* upper triangle */
goodTriangle = GoodTriangle(buffer,quadVertices[2],quadVertices[3],
quadVertices[0]);
quad->m_goodSide[LeftSide] = quad->m_goodSide[TopSide] = goodTriangle;
if (goodTriangle)
AddTriangle(quadVertices[2],quadVertices[3],quadVertices[0],
buffer,numTriangles,indexMap,numVertices);
}
else /* diagonal is lower-right to upper-left */
{
/* lower triangle */
goodTriangle = GoodTriangle(buffer,quadVertices[0],quadVertices[1],
quadVertices[3]);
quad->m_goodSide[LeftSide] = quad->m_goodSide[BottomSide] = goodTriangle;
if (goodTriangle)
AddTriangle(quadVertices[0],quadVertices[1],quadVertices[3],
buffer,numTriangles,indexMap,numVertices);
/* upper triangle */
goodTriangle = GoodTriangle(buffer,quadVertices[3],quadVertices[1],
quadVertices[2]);
quad->m_goodSide[TopSide] = quad->m_goodSide[RightSide] = goodTriangle;
if (goodTriangle)
AddTriangle(quadVertices[3],quadVertices[1],quadVertices[2],
buffer,numTriangles,indexMap,numVertices);
}
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - //
// Routines to align vertices with neighboring quad edges
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - //
// AlignPt aligns point3 with the point closest to it on the line
// defined by point1 & point2
static void
AlignPt(PixelCoords *point1, PixelCoords *point2, PixelCoords *point3,
int *indexMap, POS3D *vertices, int bufferWidth)
{
int vert1Index,vert2Index,vert3Index;
POS3D *pt1,*pt2,*pt3,unitRayVec,rayVec,proj;
float normalizer,magProj;
// We check indices in case any of the vertices are from a region of
// missing or bad data missing data
if ((vert1Index = indexMap[point1->PixelIndex(bufferWidth)]) < 0)
return;
if ((vert2Index = indexMap[point2->PixelIndex(bufferWidth)]) < 0)
return;
if ((vert3Index = indexMap[point3->PixelIndex(bufferWidth)]) < 0)
return;
if ((vert3Index == vert1Index) || (vert3Index == vert2Index))
{
/* printf("REMARK in AlignPt: vert3index is the same as vert2Index or "
"vert1Index.\n"); */
return;
}
pt1 = &vertices[vert1Index];
pt2 = &vertices[vert2Index];
pt3 = &vertices[vert3Index];
rayVec.x = pt2->x - pt1->x;
rayVec.y = pt2->y - pt1->y;
rayVec.z = pt2->z - pt1->z;
if ((rayVec.x == 0.0) && (rayVec.y == 0.0) && (rayVec.z == 0.0))
{
printf("WARNING in AlignPt: point1 and point2 are geometrically "
"coincident. (%f %f %f) (%f %f %f)\n",
pt1->x, pt1->y, pt1->z,
pt2->x, pt2->y, pt2->z);
pt3->x = pt1->x;
pt3->y = pt1->y;
pt3->z = pt1->z;
return;
}
normalizer = 1.0/sqrt(rayVec.x*rayVec.x +
rayVec.y*rayVec.y +
rayVec.z*rayVec.z);
unitRayVec.x = rayVec.x*normalizer;
unitRayVec.y = rayVec.y*normalizer;
unitRayVec.z = rayVec.z*normalizer;
magProj = ((pt3->x - pt1->x)*unitRayVec.x +
(pt3->y - pt1->y)*unitRayVec.y +
(pt3->z - pt1->z)*unitRayVec.z);
proj.x = magProj*unitRayVec.x;
proj.y = magProj*unitRayVec.y;
proj.z = magProj*unitRayVec.z;
pt3->x = pt1->x + proj.x;
pt3->y = pt1->y + proj.y;
pt3->z = pt1->z + proj.z;
}
static void
AlignLeafToNeighbor(QuadtreeNode *leaf, QuadtreeNode *neighborLeaf,
int side, int *indexMap, POS3D *vertices, int bufferWidth)
{
PixelCoords upperLeft,lowerRight,nbrUpperLeft,nbrLowerRight;
if (neighborLeaf == 0)
return;
if (leaf == neighborLeaf)
{
printf("WARNING in AlignLeafToNeighbor: leaf == neighborLeaf. "
"Aborting.\n");
return;
}
upperLeft.row = leaf->m_upperRight.row;
upperLeft.col = leaf->m_lowerLeft.col;
lowerRight.row = leaf->m_lowerLeft.row;
lowerRight.col = leaf->m_upperRight.col;
nbrUpperLeft.row = neighborLeaf->m_upperRight.row;
nbrUpperLeft.col = neighborLeaf->m_lowerLeft.col;
nbrLowerRight.row = neighborLeaf->m_lowerLeft.row;
nbrLowerRight.col = neighborLeaf->m_upperRight.col;
switch (side)
{
case LeftSide:
AlignPt(&nbrLowerRight,&(neighborLeaf->m_upperRight),
&(leaf->m_lowerLeft),indexMap,vertices,bufferWidth);
AlignPt(&nbrLowerRight,&(neighborLeaf->m_upperRight),
&upperLeft,indexMap,vertices,bufferWidth);
break;
case RightSide:
AlignPt(&(neighborLeaf->m_lowerLeft),&nbrUpperLeft,
&(leaf->m_upperRight),indexMap,vertices,bufferWidth);
AlignPt(&(neighborLeaf->m_lowerLeft),&nbrUpperLeft,
&lowerRight,indexMap,vertices,bufferWidth);
break;
case TopSide:
AlignPt(&(neighborLeaf->m_lowerLeft),&nbrLowerRight,
&(leaf->m_upperRight),indexMap,vertices,bufferWidth);
AlignPt(&(neighborLeaf->m_lowerLeft),&nbrLowerRight,
&upperLeft,indexMap,vertices,bufferWidth);
break;
case BottomSide:
AlignPt(&nbrUpperLeft,&(neighborLeaf->m_upperRight),
&(leaf->m_lowerLeft),indexMap,vertices,bufferWidth);
AlignPt(&nbrUpperLeft,&(neighborLeaf->m_upperRight),
&lowerRight,indexMap,vertices,bufferWidth);
break;
}
}
static void
AlignSeamVertices(QuadtreeNode *quadtreeNode, int *indexMap,
POS3D *vertices, int width)
{
list<QuadtreeNode *>::iterator element,endElement;
QuadtreeNode *leaf;
QuadtreeNode *neighbor;
list<QuadtreeNode *> *leaves = new list<QuadtreeNode *>;
// int numLeaves; // debugging
// int side; // debugging
if (quadtreeNode->IsLeaf())
return;
if (!quadtreeNode->HasGrandChildren())
return;
quadtreeNode->Leaves(leaves);
// numLeaves = Leaves(quadtreeNode,leaves); // debugging
// printf("numLeaves = %d\n",numLeaves); // debugging
element = leaves->begin();
endElement = leaves->end();
while (!(element == endElement))
{
leaf = *element;
if (leaf->m_neighbors[LeftSide] == 0)
{
neighbor = leaf->GetAncestralNeighbor(LeftSide);
if (neighbor)
{
// side = LeftSide; // Debugging...
// assert(IsAdjacent(leaf,neighbor,&side));
if (neighbor->m_goodSide[RightSide])
AlignLeafToNeighbor(leaf,neighbor,LeftSide,indexMap,vertices,width);
}
}
if (leaf->m_neighbors[RightSide] == 0)
{
neighbor = leaf->GetAncestralNeighbor(RightSide);
if (neighbor)
{
// side = RightSide; // Debugging...
// assert(IsAdjacent(leaf,neighbor,&side));
if (neighbor->m_goodSide[LeftSide])
AlignLeafToNeighbor(leaf,neighbor,RightSide,indexMap,vertices,width);
}
}
if (leaf->m_neighbors[BottomSide] == 0)
{
neighbor = leaf->GetAncestralNeighbor(BottomSide);
if (neighbor)
{
// side = BottomSide; // Debugging...
// assert(IsAdjacent(leaf,neighbor,&side));
if (neighbor->m_goodSide[TopSide])
AlignLeafToNeighbor(leaf,neighbor,BottomSide,indexMap,vertices,width);
}
}
if (leaf->m_neighbors[TopSide] == 0)
{
neighbor = leaf->GetAncestralNeighbor(TopSide);
if (neighbor)
{
// side = TopSide; // Debugging...
// assert(IsAdjacent(leaf,neighbor,&side));
if (neighbor->m_goodSide[BottomSide])
AlignLeafToNeighbor(leaf,neighbor,TopSide,indexMap,vertices,width);
}
}
++element;
}
delete leaves;
}
static void
MendTriangleMeshSeams(BUFFER *buffer, int width,
int *indexMap, Quadtree *quadtree)
{
POS3D *vertices = buffer->nff.vtx;
AlignSeamVertices(quadtree->Root(),indexMap,vertices,width);
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - //
// Main triangle mesh creation routine
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - //
/************************************************************************/
/* */
/* dot to nff updated version */
/* */
/************************************************************************/
// #if 0
// static void
// SyntheticTerrain(BUFFER *b, int width, int height)
// {
// int rowStart,row,col,pixelIndex;
// pixel *disparities = b->disp;
// double x,y;
// double maxZ = -999999.9;
// /* double a = 0.2, k = 10.0; works */
// double a = 1.0, k = 10.0;
// rowStart = 0;
// for (row = 0; row < height; row++)
// {
// y = (double)row - ((double)height-1.0)/2.0;
// for (col = 0; col < width; col++)
// {
// pixelIndex = rowStart + col;
// disparities[pixelIndex] = 32;
// b->dot[pixelIndex].x = x = (double)col - ((double)width-1.0)/2.0;
// b->dot[pixelIndex].y = y;
// b->dot[pixelIndex].z = k*(((1.0-exp(-a*x))/(1.0+exp(-a*x))) -
// ((1.0-exp(-a*(x-y)))/(1.0+exp(-a*(x-y)))));
// /* b->dot[pixelIndex].z = 10.0; */
// if (b->dot[pixelIndex].z > maxZ)
// maxZ = b->dot[pixelIndex].z;
// }
// rowStart += width;
// }
// printf("SyntheticTerrain: maxZ = %e\n",maxZ);
// }
//#endif
static void
Init(BUFFER *b, int width, int height)
{
b->nff.pt_number = width * height;
/* Allocate memory for gradient buffer */
if ((b->gradients =
(POS2D *) malloc(b->nff.pt_number * sizeof(POS3D))) == NULL)
{
fprintf(stderr, "gradient buffer allocation failed.\n");
exit(EXIT_FAILURE);
}
/* SyntheticTerrain(b,width,height); */
}
static double
Convolve(Array3x3 mask, int row, int col, BUFFER *buffer, int width)
{
static const int maskRowOrigin = 1;
static const int maskColOrigin = 1;
// pixel *disparities = buffer->disp;
int i,j,pixelIndex,rowStartIndex;
double result = 0.0;
rowStartIndex = (row-maskRowOrigin)*width;
for (i=0; i < 3; rowStartIndex += width, i++)
{
for (j=0; j < 3; j++)
{
pixelIndex = rowStartIndex + (col + j-maskColOrigin);
if (buffer->dot[pixelIndex].x == 0 &&
buffer->dot[pixelIndex].y == 0 &&
buffer->dot[pixelIndex].z == 0)
return (0.0);
// if ((disparities[pixelIndex] == 0) ||
// (disparities[pixelIndex] == MISSING_PIXEL) ||
// (disparities[pixelIndex] == FAR_FIELD_PIXEL))
result += mask[i][j]*buffer->dot[pixelIndex].z;
}
}
return (result);
}
static void
CalculateGradients(BUFFER *b, int width, int height)
{
// pixel *disparities = b->disp;
int row,col,pixelIndex, numPixels = width*height;
Array3x3 xMask = {{-1.0/6.0, 0.0, 1.0/6.0},
{-1.0/6.0, 0.0, 1.0/6.0},
{-1.0/6.0, 0.0, 1.0/6.0}};
Array3x3 yMask = {{ 1.0/6.0, 1.0/6.0, 1.0/6.0},
{ 0.0, 0.0, 0.0 },
{-1.0/6.0,-1.0/6.0,-1.0/6.0}};
/* zero out top border row */
for (pixelIndex = 0; pixelIndex < width; pixelIndex++)
{
b->gradients[pixelIndex].x = 0.0;
b->gradients[pixelIndex].y = 0.0;
}
for (row = 1; row < (height-1); row++)
{
/* zero out left border column */
b->gradients[pixelIndex].x = 0.0;
b->gradients[pixelIndex].y = 0.0;
pixelIndex++;
for (col = 1; col < (width - 1); col++)
{
if (b->dot[pixelIndex].x == 0 &&
b->dot[pixelIndex].y == 0 &&
b->dot[pixelIndex].z == 0)
{
b->gradients[pixelIndex].x = Convolve(xMask,row,col,b,width);
b->gradients[pixelIndex].y = Convolve(yMask,row,col,b,width);
}
else
{
b->gradients[pixelIndex].x = 0.0;
b->gradients[pixelIndex].y = 0.0;
}
pixelIndex++;
}
/* zero out right border column */
b->gradients[pixelIndex].x = 0.0;
b->gradients[pixelIndex].y = 0.0;
pixelIndex++;
}
/* zero out bottom border row */
for (; pixelIndex < numPixels; pixelIndex++)
{
b->gradients[pixelIndex].x = 0.0;
b->gradients[pixelIndex].y = 0.0;
}
}
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
// Publicly visible functions
// - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
void
dot_to_adaptative_mesh(BUFFER *b, int width, int height, double mesh_tolerance, int max_triangles)
{
Quadtree *quadtree;
Init(b,width,height);
CalculateGradients(b,width,height);
/*WriteGradientImageFile(b,dft,hd,width,height,'m');*/
quadtree = new Quadtree(b,width,height);
printf("\tNumber of triangles in full mesh (%d,%d) = %d\n",
width,height,2*(width-1)*(height-1));
printf("\tMesh parameters --> tol: %f max triangles: %d\n", mesh_tolerance, max_triangles);
quadtree->Subdivide(mesh_tolerance, max_triangles);
printf("Number of triangles in subdivided mesh = %d\n",b->nff.tr_number);
quadtree->CreateTriangleMesh();
printf("Number of triangles in reduced mesh = %d\n",b->nff.tr_number);
}
/* original code that place the vertices on a regular grid */
void
dot_to_mesh(BUFFER *b, int width, int height, int h_step, int v_step)
{
int y, x, x_grid, y_grid;
// int max_jump = dft->nff_max_jump;
int grid_width, grid_height;
int buff_pos;
int n = 0; /* number of triangles created */
int AI, BI, CI, DI; /* shift from pointer to the 4 corners Image */
int AG, BG, CG, DG; /* shift from pointer to the 4 corners Grid */
double A1,A2,A3,B1,B2,B3,C1,C2,C3,D1,D2,D3; /* easier to read */
double distBC,distAD;
#if 0
double distAB,distAC,distBD,distCD;
double max_dist;
double pixel_disp;
double sin_theta1, sin_theta2;
#endif
double u,v;
/* initialize the constants */
grid_width = (width-1)/h_step+1;
grid_height = (height-1)/v_step+1;
b->nff.pt_number = (grid_width) * (grid_height);
b->nff.tr_number = (grid_width-1)*(grid_height-1)*2;
x_grid = 0;
y_grid = 0;
/* debug */
printf("w=%d h=%d\n", width, height);
printf("sw=%d sh=%d\n", h_step, v_step);
printf("gw=%d gh=%d\n", grid_width, grid_height);
printf("pt=%d tr=%d\n", b->nff.pt_number, b->nff.tr_number);
/* Allocate memory for vertex buffer */
if ((b->nff.vtx = (POS3D *)malloc(b->nff.pt_number * sizeof(POS3D))) == NULL)
{
fprintf(stderr, "nff vertex buffer allocation failed.\n");
exit(EXIT_FAILURE);
}
/* Allocate memory for triangle buffer */
if ((b->nff.triangle = (NFF_TR *)malloc(b->nff.tr_number * sizeof(NFF_TR))) ==
NULL)
{
fprintf(stderr, "nff triangle buffer allocation failed.\n");
exit (EXIT_FAILURE);
}
/* Allocate memory for texure buffer */
if ((b->nff.tex = (UV_TEX *)malloc(b->nff.pt_number * sizeof(UV_TEX))) ==
NULL)
{
fprintf(stderr, "nff texure buffer allocation failed.\n");
exit(EXIT_FAILURE);
}
/* make the regular grid point */
for( y=0, y_grid=0 ; y_grid < grid_height ; y += v_step, y_grid++){
buff_pos = y_grid*grid_width;
v = (double)(height-y)/(double)(height);
for( x=0, x_grid=0 ; x_grid < grid_width ; x += h_step, x_grid ++){
u = (double)(x)/(double)(width);
b->nff.vtx[buff_pos+x_grid].x = b->dot[y*width+x].x;
b->nff.vtx[buff_pos+x_grid].y = b->dot[y*width+x].y;
b->nff.vtx[buff_pos+x_grid].z = b->dot[y*width+x].z;
b->nff.tex[buff_pos+x_grid].u = u;
b->nff.tex[buff_pos+x_grid].v = v;
}
}
// make the triangle
for( y=0; y < (grid_height-1) ; y ++){
buff_pos = y*grid_width;
for( x=0 ; x < (grid_width-1) ; x ++){
// #if 0
// /* calcul max_dist */
// pixel_disp = (double)b->disp[y*v_step*width+(x+1)*h_step];
// sin_theta1 = sin ((double)hd->h_theta_pixel * pixel_disp / 2.0);
// sin_theta2 = sin ((double)hd->h_theta_pixel* (pixel_disp - max_jump) /
// 2.0);
// if(sin_theta1 < THETA_LIMIT)
// sin_theta1 = THETA_LIMIT;
// if(sin_theta2 < THETA_LIMIT)
// sin_theta2 = THETA_LIMIT;
// max_dist = (double)(dft->baseline/sin_theta1 - dft->baseline/sin_theta2);
// if (max_dist < 0)
// max_dist = max_dist *(-1);
// #endif
/* calcul the distance between the first triangle sommets */
/* top left corner X */
AG = buff_pos+x;
AI = (y)*v_step*width+(x)*h_step;
A1 = b->nff.vtx[AG].x;
A2 = b->nff.vtx[AG].y;
A3 = b->nff.vtx[AG].z;
/* top right corner Y */
BG = buff_pos+x+1;
BI = (y)*v_step*width+(x+1)*h_step;
B1 = b->nff.vtx[BG].x;
B2 = b->nff.vtx[BG].y;
B3 = b->nff.vtx[BG].z;
/* botom left corner Z */
CG = buff_pos+x+grid_width;
CI = (y+1)*v_step*width+(x)*h_step;
C1 = b->nff.vtx[CG].x;
C2 = b->nff.vtx[CG].y;
C3 = b->nff.vtx[CG].z;
/* botom right corner Y */
DG = buff_pos+grid_width+x+1;
DI = (y+1)*v_step*width+(x+1)*h_step;
D1 = b->nff.vtx[DG].x;
D2 = b->nff.vtx[DG].y;
D3 = b->nff.vtx[DG].z;
distBC = (B1-C1)*(B1-C1) + (B2-C2)*(B2-C2) + (B3-C3)*(B3-C3);
distAD = (A1-D1)*(A1-D1) + (A2-D2)*(A2-D2) + (A3-D3)*(A3-D3);
#if 0
distAB = (A1-B1)*(A1-B1) + (A2-B2)*(A2-B2) + (A3-B3)*(A3-B3);
distAC = (A1-C1)*(A1-C1) + (A2-C2)*(A2-C2) + (A3-C3)*(A3-C3);
distBD = (B1-D1)*(B1-D1) + (B2-D2)*(B2-D2) + (B3-D3)*(B3-D3);
distCD = (C1-D1)*(C1-D1) + (C2-D2)*(C2-D2) + (C3-D3)*(C3-D3);
#endif
/* build the triangles with the smallest diagonal |\| versus |/| */
if(distBC < distAD){ /* case |/| */
/* draw triangle |/ if the jumps in disp is not bigger than allowed */
// if (!(abs(int(b->disp[AI] - b->disp[BI])) > max_jump ||
// abs(int(b->disp[BI] - b->disp[CI])) > max_jump ||
// abs(int(b->disp[CI] - b->disp[DI])) > max_jump ||
// b->disp[AI] == MISSING_PIXEL ||
// b->disp[BI] == MISSING_PIXEL ||
// b->disp[CI] == MISSING_PIXEL)) {
// Draw a triangle if the vertices are not missing pixels
if(!((b->dot[AI].x == 0 && b->dot[AI].y ==0 && b->dot[AI].z == 0) ||
(b->dot[BI].x == 0 && b->dot[BI].y ==0 && b->dot[BI].z == 0) ||
(b->dot[CI].x == 0 && b->dot[CI].y ==0 && b->dot[CI].z == 0))) {
b->nff.triangle[n].vtx1 = AG;
b->nff.triangle[n].vtx2 = BG;
b->nff.triangle[n].vtx3 = CG;
n ++; /* you've got one more triangle */
}
// /* draw triangle /| if the jumps in disp is not bigger than allowed */
// if (!(abs(int(b->disp[BI] - b->disp[DI])) > max_jump ||
// abs(int(b->disp[DI] - b->disp[CI])) > max_jump ||
// abs(int(b->disp[CI] - b->disp[BI])) > max_jump ||
// b->disp[BI] == MISSING_PIXEL ||
// b->disp[CI] == MISSING_PIXEL ||
// b->disp[DI] == MISSING_PIXEL)) {
// Draw a triangle if the vertices are not missing pixels
if(!((b->dot[BI].x == 0 && b->dot[BI].y ==0 && b->dot[BI].z == 0) ||
(b->dot[CI].x == 0 && b->dot[CI].y ==0 && b->dot[CI].z == 0) ||
(b->dot[DI].x == 0 && b->dot[DI].y ==0 && b->dot[DI].z == 0))) {
b->nff.triangle[n].vtx1 = BG;
b->nff.triangle[n].vtx2 = DG;
b->nff.triangle[n].vtx3 = CG;
n ++; /* and a second one */
}
}
else { /* distBC <= distAD case |\| */
// /* draw triangle |\ if the jumps in disp is not bigger than allowed */
// if (!(abs(int(b->disp[AI] - b->disp[DI])) > max_jump ||
// abs(int(b->disp[DI] - b->disp[CI])) > max_jump ||
// abs(int(b->disp[CI] - b->disp[AI])) > max_jump ||
// b->disp[AI] == MISSING_PIXEL ||
// b->disp[CI] == MISSING_PIXEL ||
// b->disp[DI] == MISSING_PIXEL)) {
// Draw a triangle if the vertices are not missing pixels
if(!((b->dot[AI].x == 0 && b->dot[AI].y ==0 && b->dot[AI].z == 0) ||
(b->dot[CI].x == 0 && b->dot[CI].y ==0 && b->dot[CI].z == 0) ||
(b->dot[DI].x == 0 && b->dot[DI].y ==0 && b->dot[DI].z == 0))) {
b->nff.triangle[n].vtx1 = AG;
b->nff.triangle[n].vtx2 = DG;
b->nff.triangle[n].vtx3 = CG;
n ++; /* you've got one more triangle */
}
// if (!(abs(int(b->disp[BI] - b->disp[DI])) > max_jump ||
// abs(int(b->disp[DI] - b->disp[AI])) > max_jump ||
// abs(int(b->disp[AI] - b->disp[BI])) > max_jump ||
// b->disp[AI] == MISSING_PIXEL ||
// b->disp[BI] == MISSING_PIXEL ||
// b->disp[DI] == MISSING_PIXEL)) {
// Draw a triangle if the vertices are not missing pixels
if(!((b->dot[AI].x == 0 && b->dot[AI].y ==0 && b->dot[AI].z == 0) ||
(b->dot[BI].x == 0 && b->dot[BI].y ==0 && b->dot[BI].z == 0) ||
(b->dot[DI].x == 0 && b->dot[DI].y ==0 && b->dot[DI].z == 0))) {
b->nff.triangle[n].vtx1 = BG;
b->nff.triangle[n].vtx2 = DG;
b->nff.triangle[n].vtx3 = AG;
n ++; /* and a second one */
}
}
}
}
b->nff.tr_number = n;
return;
}
/***********************/
/* write inventor_file */
/***********************/
#if defined(ASP_HAVE_PKG_OPENSCENEGRAPH) && ASP_HAVE_PKG_OPENSCENEGRAPH==1
void write_osg_impl(BUFFER *b, std::string const& filename,
std::string const& texture_filename) {
std::cout << "Writing " << filename << "..." << std::flush;
// A geode is a "geometry node". It is-a 'Node' and contains 'Drawable's.
osg::ref_ptr<osg::Geode> geode (new osg::Geode());
// 'Geometry' is-a 'Drawable'. It is a collection of vertices, normals,
// colors, texture coordinates and so on. It is organized in "primitive
// sets", that allow to say that, e.g., "from vertex to 0 to 8 render as
// triangles, from 9 to 13 render as points, please". For those
// OpenGL-inclined, think of 'Geometry' as a wrapper around vertex (and
// normals, and texcoord) arrays and 'glDrawElements()'
osg::ref_ptr<osg::Geometry> geometry (new osg::Geometry());
// Create and set the vertex array for the geometry object
osg::ref_ptr<osg::Vec3Array> vertices (new osg::Vec3Array());
for (int i = 0; i < b->nff.pt_number; ++i)
vertices->push_back (osg::Vec3 (b->nff.vtx[i].x, b->nff.vtx[i].y, b->nff.vtx[i].z) );
geometry->setVertexArray (vertices.get());
std::vector<vw::Vector3> vertex_normals(b->nff.pt_number);
// Now create the "primitive set", which describes which vertices
// are to be used when rendering the triangles.
osg::ref_ptr<osg::DrawElementsUInt> faces(new osg::DrawElementsUInt(osg::PrimitiveSet::TRIANGLES, 0));
for (int i = 0; i < b->nff.tr_number; ++i) {
faces->push_back(b->nff.triangle[i].vtx1);
faces->push_back(b->nff.triangle[i].vtx2);
faces->push_back(b->nff.triangle[i].vtx3);
vw::Vector3 v1(b->nff.vtx[b->nff.triangle[i].vtx1].x,
b->nff.vtx[b->nff.triangle[i].vtx1].y,
b->nff.vtx[b->nff.triangle[i].vtx1].z);
vw::Vector3 v2(b->nff.vtx[b->nff.triangle[i].vtx2].x,
b->nff.vtx[b->nff.triangle[i].vtx2].y,
b->nff.vtx[b->nff.triangle[i].vtx2].z);
vw::Vector3 v3(b->nff.vtx[b->nff.triangle[i].vtx3].x,
b->nff.vtx[b->nff.triangle[i].vtx3].y,
b->nff.vtx[b->nff.triangle[i].vtx3].z);
vw::Vector3 l1 = v3-v1;
vw::Vector3 l2 = v3-v2;
vw::Vector3 c = normalize(vw::math::cross_prod(l1,l2));
vertex_normals[b->nff.triangle[i].vtx1] = c;
vertex_normals[b->nff.triangle[i].vtx2] = c;
vertex_normals[b->nff.triangle[i].vtx3] = c;
}
// Create an array for the single normal.
osg::ref_ptr<osg::Vec3Array> n = new osg::Vec3Array;
geometry->setNormalArray( n.get() );
for (int i = 0; i < b->nff.pt_number; ++i)
n->push_back( osg::Vec3( vertex_normals[i].x(),
vertex_normals[i].y(),
vertex_normals[i].z()) );
geometry->addPrimitiveSet(faces.get());
// Next, we associate some texture coordinates with the vertices
osg::Vec2Array* texcoords = new osg::Vec2Array(b->nff.pt_number);
for (int i = 0; i < b->nff.pt_number; ++i)
(*texcoords)[i].set(b->nff.tex[i].u, b->nff.tex[i].v);
geometry->setTexCoordArray(0,texcoords);
// The geometry is now full specified, so we associate it with the geode.
geode->addDrawable (geometry.get());
// We must now associate the texture in a file with the geode
osg::ref_ptr<osg::Texture2D> texture(new osg::Texture2D);
// protect from being optimized away as static state
texture->setDataVariance(osg::Object::DYNAMIC);
texture->setUseHardwareMipMapGeneration(false);
osg::Image* texture_image = osgDB::readImageFile(texture_filename.c_str());
if (!texture_image)
throw vw::ArgumentErr() << "OpenSceneGraph couldn't read the texture, " << texture_filename << ".";
texture->setImage(texture_image);
// Create a new StateSet with default settings:
osg::ref_ptr<osg::StateSet> texture_state_set(new osg::StateSet());
// Assign texture unit 0 of our new StateSet to the texture
// we just created and enable the texture.
texture_state_set->setTextureAttributeAndModes(0,texture.get(),osg::StateAttribute::ON);
geode->setStateSet(texture_state_set.get());
// Set up lighting properties
osg::StateSet* state = geode->getOrCreateStateSet();
osg::ref_ptr<osg::Material> mat = new osg::Material;
mat->setDiffuse( osg::Material::FRONT_AND_BACK,
osg::Vec4( .7f, .7f, .7f, 1.f ) );
mat->setSpecular( osg::Material::FRONT_AND_BACK,
osg::Vec4( .1f, .1f, .1f, 1.f ) );
mat->setShininess( osg::Material::FRONT_AND_BACK, 20.f );
state->setAttribute( mat.get() );
// Add the geometry to the geode and save the geode to a file o disk.
osgDB::writeNodeFile(*(geode.get()), filename);
std::cout << " done.\n";
}
#else // HAVE_PKG_OPENSCENEGRAPH
void write_osg_impl(BUFFER *b, std::string const& filename,
std::string const& texture_filename) {
std::cout << "WARNING: could not write " << filename << ". Compiled without open scene graph support.";
}
#endif // HAVE_PKG_OPENSCENEGRAPH
void write_inventor_impl(BUFFER *b, std::string const& filename,
std::string const& texture_filename,
bool flip_triangles) {
FILE *outflow;
int i;
std::vector<std::string> texture_path_components;
boost::split( texture_path_components, texture_filename, boost::is_any_of("/") );
std::string relative_texture_path = "./" + texture_path_components[texture_path_components.size()-1];
// open output file
if((outflow = fopen (filename.c_str(), "w" )) == 0) {
fprintf (stderr, "write_inventor: cannot open output file: %s\n", filename.c_str());
exit(EXIT_FAILURE);
}
fprintf (outflow, "#Inventor V2.0 ascii\n\n");
fprintf (outflow, "# Created by the Intelligent Robotics Group,\n");
fprintf (outflow, "# NASA Ames Research Center\n");
fprintf (outflow, "# File generated by the NASA Ames Stereo-Pipeline.\n");
fprintf (outflow, "# %d vertices, %d triangles\n\n", b->nff.pt_number, b->nff.tr_number);
fprintf (outflow, "Separator {\n");
fprintf (outflow, " Texture2 {\n");
// Grab the end of the full path
fprintf (outflow, " filename \"%s\"\n", relative_texture_path.c_str());
fprintf (outflow, " }\n");
// Material Section
fprintf (outflow, "Material {\n");
fprintf (outflow, " ambientColor 1.00 1.00 1.00\n");
fprintf (outflow, " diffuseColor 1.00 1.00 1.00\n");
fprintf (outflow, " specularColor 0.00 0.00 0.00\n");
fprintf (outflow, " emissiveColor 0.00 0.00 0.00\n");
fprintf (outflow, " shininess 0.00\n");
fprintf (outflow, " transparency 0.00\n");
fprintf (outflow, "}\n");
// Gouraud shading (comment out in the file, but make it easy to
// turn on if desired.
if (flip_triangles) {
fprintf (outflow, "ShapeHints {\n");
fprintf (outflow, " vertexOrdering CLOCKWISE\n");
fprintf (outflow, "# creaseAngle 3.1\n");
fprintf (outflow, "}\n");
} else {
fprintf (outflow, "#ShapeHints {\n");
fprintf (outflow, "# creaseAngle 3.1\n");
fprintf (outflow, "#}\n");
}
// vertices position
fprintf (outflow, " Coordinate3 {\n");
fprintf (outflow, " point [ ");
fprintf (outflow, "# %d vertices\n", b->nff.pt_number);
for(i = 0 ; i < b->nff.pt_number; i++ )
fprintf (outflow, "%f %f %f,\n",
b->nff.vtx[i].x, b->nff.vtx[i].y, b->nff.vtx[i].z);
fprintf (outflow, " ]\n");
fprintf (outflow, " }\n");
// uv coordinate
fprintf (outflow, " TextureCoordinate2 {\n");
fprintf (outflow, " point [ ");
for(i = 0 ; i < b->nff.pt_number; i++ )
fprintf (outflow, "%f %f,\n", b->nff.tex[i].u, b->nff.tex[i].v);
fprintf (outflow, " ]\n");
fprintf (outflow, " }\n");
// polygons
fprintf (outflow, " IndexedFaceSet {\n");
fprintf (outflow, " coordIndex [ \n");
fprintf (outflow, "# %d triangles\n", b->nff.tr_number);
for(i = 0 ; i < b->nff.tr_number; i++ )
fprintf (outflow, "%d, %d, %d, -1,\n", b->nff.triangle[i].vtx1,
b->nff.triangle[i].vtx3, b->nff.triangle[i].vtx2);
fprintf (outflow, " ]\n");
fprintf (outflow, " }\n");
fprintf (outflow, "}\n");
#if 0
fprintf (outflow, " ShapeHints {\n");
fprintf (outflow, " vertexOrdering COUNTERCLOCKWISE\n");
fprintf (outflow, " creaseAngle 1.0\n");
fprintf (outflow, " }\n");
fprintf (outflow, " Material {\n");
fprintf (outflow, " ambientColor 0.2 0.2 0.2\n");
fprintf (outflow, " diffuseColor 0.42 0.41 0.35\n");
fprintf (outflow, " specularColor 0.2 0.2 0.2\n");
fprintf (outflow, " emissiveColor 0 0 0\n");
fprintf (outflow, " shininess 0\n");
fprintf (outflow, " transparency 0.5\n");
fprintf (outflow, " }\n");
fprintf (outflow, "# Texture2 {\n");
fprintf (outflow, "# filename \"tex2.rgb\"\n");
fprintf (outflow, "# wrapS REPEAT\n");
fprintf (outflow, "# wrapT REPEAT\n");
fprintf (outflow, "# model MODULATE\n");
fprintf (outflow, "# }\n");
fprintf (outflow, "# Texture2Transform {\n");
fprintf (outflow, "# rotation 0.0\n");
fprintf (outflow, "# center 0.5 0.5\n");
fprintf (outflow, "# translation 0.0 0.0\n");
fprintf (outflow, "# scaleFactor 1.0 1.0\n");
fprintf (outflow, "# }\n");
fprintf (outflow, " Coordinate3 {\n");
fprintf (outflow, " point [ ");
fprintf (outflow, "# %d vertices\n", b->nff.pt_number);
for(i = 0 ; i < b->nff.pt_number; i++ )
fprintf (outflow, "%f %f %f,\n", b->nff.vtx[i].x, b->nff.vtx[i].y,
b->nff.vtx[i].z);
fprintf (outflow, " ]\n");
fprintf (outflow, " }\n");
fprintf (outflow, " IndexedFaceSet {\n");
fprintf (outflow, " coordIndex [ \n");
fprintf (outflow, "# %d triangles\n", b->nff.tr_number);
for(i = 0 ; i < b->nff.tr_number; i++ )
fprintf (outflow, "%d, %d, %d, -1,\n", b->nff.triangle[i].vtx1,
b->nff.triangle[i].vtx2, b->nff.triangle[i].vtx3);
fprintf (outflow, " ]\n");
fprintf (outflow, " }\n");
fprintf (outflow, "}\n");
#endif
printf("%s writen successfully\n", filename.c_str());
fclose(outflow);
} // write_inventor_file
/*******************/
/* write vrml_file */
/*******************/
void write_vrml_impl(BUFFER *b, std::string const& filename, std::string const& texture_filename) {
FILE *outflow;
int i;
// open output file
if((outflow = fopen (filename.c_str(), "w" )) == 0) {
fprintf (stderr, "write_vrml(): cannot open output file: %s\n", filename.c_str());
exit(EXIT_FAILURE);
}
fprintf(outflow,"#VRML V1.0 ascii\n#\n");
fprintf(outflow,"# Created by the Intelligent Robotics Group,\n");
fprintf(outflow,"# NASA Ames Research Center\n");
fprintf(outflow,"# File generated by the NASA Ames Stereo-Pipeline.\n");
fprintf(outflow,"# %d vertices, %d triangles\n\n", b->nff.pt_number, b->nff.tr_number);
fprintf (outflow, "Separator {\n");
// fprintf (outflow, "RotationXYZ {\n");
// fprintf (outflow, "axis X\n");
// fprintf (outflow, "angle 3.14159265358979\n");
// fprintf (outflow, "}\n");
// Disabled for now until we have a better mechanism for passing in
// these settings. -mbroxton
//
// fprintf (outflow, "Material {\n");
// fprintf (outflow, " ambientColor %4.2f %4.2f %4.2f \n", dft->ambiColorRed,
// dft->ambiColorGreen, dft->ambiColorBlue );
// fprintf (outflow, " diffuseColor %4.2f %4.2f %4.2f \n", dft->diffColorRed,
// dft->diffColorGreen, dft->diffColorBlue );
// fprintf (outflow, " specularColor %4.2f %4.2f %4.2f \n", dft->specColorRed,
// dft->specColorGreen, dft->specColorBlue );
// fprintf (outflow, " emissiveColor %4.2f %4.2f %4.2f \n", dft->emisColorRed,
// dft->emisColorGreen, dft->emisColorBlue );
// fprintf (outflow, " shininess %4.2f \n", dft->shininess);
// fprintf (outflow, " transparency %4.2f \n}\n\n", dft->transparency);
fprintf (outflow, "ShapeHints {\n");
fprintf (outflow, " vertexOrdering COUNTERCLOCKWISE\n");
// if(dft->shapeType_solid)
fprintf (outflow, " shapeType SOLID\n");
// else
// fprintf (outflow, " shapeType UNKNOWN_SHAPE_TYPE\n");
fprintf (outflow, " creaseAngle 3.1\n}\n\n");
/* vertices position */
fprintf (outflow, "Coordinate3 { \n");
fprintf (outflow, " point [\n");
for(i = 0 ; i < b->nff.pt_number; i++ )
fprintf (outflow, " %f %f %f,\n", b->nff.vtx[i].x,
b->nff.vtx[i].y, b->nff.vtx[i].z);
fprintf (outflow, " ]\n");
fprintf (outflow, "}\n\n");
// Texture file
fprintf (outflow, "Texture2 {\n");
fprintf (outflow, " filename \"%s\"\n", texture_filename.c_str());
fprintf (outflow, "}\n\n");
/* uv coordinate */
fprintf (outflow, "TextureCoordinate2 {\n");
fprintf (outflow, " point [\n");
for(i = 0 ; i < b->nff.pt_number; i++ )
fprintf (outflow, " %f %f,\n", b->nff.tex[i].u, b->nff.tex[i].v);
fprintf (outflow, " ]\n");
fprintf (outflow, "}\n\n");
/* polygons */
fprintf (outflow, "IndexedFaceSet {\n");
fprintf (outflow, " coordIndex [\n");
for(i = 0 ; i < b->nff.tr_number; i++ )
fprintf (outflow, " %d, %d, %d, -1,\n", b->nff.triangle[i].vtx1,
b->nff.triangle[i].vtx3, b->nff.triangle[i].vtx2);
fprintf (outflow, " ]\n\n");
fprintf (outflow, " textureCoordIndex [\n");
for(i = 0 ; i < b->nff.tr_number; i++ )
fprintf (outflow, " %d, %d, %d, -1,\n", b->nff.triangle[i].vtx1,
b->nff.triangle[i].vtx3, b->nff.triangle[i].vtx2);
fprintf (outflow, " ]\n");
fprintf (outflow, "}\n");
// Close off Separator
fprintf (outflow, "}\n");
printf("%s written successfully\n", filename.c_str());
fclose(outflow);
} // write_vrml_file
void write_trimesh_impl(BUFFER *b, std::string const& filename, bool flip_triangles) {
FILE *outflow;
int i;
// open output file
if((outflow = fopen (filename.c_str(), "w" )) == 0) {
fprintf (stderr, "write_inventor: cannot open output file: %s\n", filename.c_str());
exit(EXIT_FAILURE);
}
// vertices
fprintf (outflow, "%d\n", b->nff.pt_number);
for(i = 0 ; i < b->nff.pt_number; i++ )
fprintf (outflow, "%f %f %f\n", b->nff.vtx[i].x,
b->nff.vtx[i].y, b->nff.vtx[i].z);
// triangles
fprintf (outflow, "%d\n", b->nff.tr_number);
for(i = 0 ; i < b->nff.tr_number; i++ ) {
if (flip_triangles) {
fprintf (outflow, "%d %d %d\n", b->nff.triangle[i].vtx1,
b->nff.triangle[i].vtx2, b->nff.triangle[i].vtx3);
} else {
fprintf (outflow, "%d %d %d\n", b->nff.triangle[i].vtx1,
b->nff.triangle[i].vtx3, b->nff.triangle[i].vtx2);
}
}
} // write_trimesh_file
Jump to Line
Something went wrong with that request. Please try again.