wnoise.cc

一个用MATLAB语言编写的摄像机标定工具箱,内容丰富

//
// wnoise.cc
//
// $Id: wnoise.cc,v 1.1.1.1 2001/02/28 00:28:40 cstolte Exp $
//

#define MINMAX
#include <sgl/wnoise.h>
#include <sgl/vecmath3.h>
#include <stdlib.h>

#ifdef _WIN32
#include <stdlib.h>
static void srand48(int seed) { srand(seed); }
static double drand48(void) { return (double)rand() / (RAND_MAX + 1); }
#endif

bool sgl_wnoise_exhaustive = false;

static void doVoxel(int x, int y, int z, const Tuple3d &p, double density,
		    int num, double *dist, Vector3 *vec);
static void doWNoise(const Tuple3d &p, double density, int num, 
		     int dimensions, double *dist, Vector3 *vec);

double
wNoise1(const Tuple3d &p, double density)
{
    double d;
    doWNoise(p, density, 1, 3, &d, 0);
    return sqrt(d);
}

Vector3
wVNoise1(const Tuple3d &p, double density)
{
    Vector3 v;
    doWNoise(p, density, 1, 3, 0, &v);
    return v;
}

double
wNoise2(const Tuple3d &p, double density)
{
    double d[2];
    doWNoise(p, density, 2, 3, d, 0);
    return sqrt(d[1]);
}

Vector3
wVNoise2(const Tuple3d &p, double density)
{
    Vector3 v[2];
    doWNoise(p, density, 2, 3, 0, v);
    return v[1];
}

double
wNoise3(const Tuple3d &p, double density)
{
    double d[3];
    doWNoise(p, density, 3, 3, d, 0);
    return sqrt(d[2]);
}

Vector3
wVNoise3(const Tuple3d &p, double density)
{
    Vector3 v[3];
    doWNoise(p, density, 3, 3, 0, v);
    return v[2];
}

static double
uniformRandom()
{
    return drand48();
}

static void
seedRandom(int x, int y, int z)
{
    long seed = (x << 16) + (y << 8) + z;
    srand48(seed);
}

static void
doVoxel(int x, int y, int z, const Tuple3d &p, double density,
	int num, double *dist2, Vector3 *vec)
{
    seedRandom(x, y, z);

    int npts = max(1, int(.5 + lerp(uniformRandom(), 
				    density / 2., 2. * density)));
    
    for (int i = 0; i < npts; ++i) {
	Point3 noisePt(x + uniformRandom(),
		       y + uniformRandom(),
		       z + uniformRandom());

	double d2 = distance_squared(p, noisePt);

	for (int j = 0; j < num; ++j)
	    if (d2 < dist2[j]) {
		for (int k = j; k < num - 1; ++k) {
		    dist2[k+1] = dist2[k];
		    if (vec)
			vec[k+1] = vec[k];
		}
		dist2[j] = d2;
		if (vec)
		    vec[j] = noisePt - Point3(p);
		break;
	    }
	
    }
    for (int j = 0; j < num-1; ++j)
	assert(dist2[j] <= dist2[j+1]);
}

static void
doWNoise(const Tuple3d &pp, double density, int num, int dimensions,
	 double *dist2, Vector3 *vec)
{
    // hack, but easier than handling points that are exactly at negative
    // integer latice-points correctly.
    Tuple3d p(pp.x + 1e-7, pp.y + 1e-7, pp.z + 1e-7);

    int i;
    int xx = floor(p.x), yy = floor(p.y), zz = floor(p.z);
    double dd[10];
    assert(num < 10);
    double *d = dist2;
    if (!d)
	d = dd;

    for (i = 0; i < num; ++i)
	d[i] = HUGE_VAL;

    if (!sgl_wnoise_exhaustive) {
	// check voxel the point is in
	doVoxel(xx, yy, zz, p, density, num, d, vec);

	double d2 = d[num - 1];

	// check each of the voxels that share a face with the
	// point's voxel, if they're close enough to possibly
	// make a difference
	double dpx2 = p.x >= 0. ? sqr(1. - frac(p.x)) : sqr(frac(p.x));
	if (dpx2 < d2) {
	    doVoxel(xx+1, yy, zz, p, density, num, d, vec);
	    d2 = d[num - 1];
	}
	double dnx2 = p.x >= 0. ? sqr(frac(p.x)) : sqr(1. - frac(p.x));
	if (dnx2 < d2) {
	    doVoxel(xx-1, yy, zz, p, density, num, d, vec);
	    d2 = d[num - 1];
	}
	double dpy2 = p.y >= 0. ? sqr(1. - frac(p.y)) : sqr(frac(p.y));
	if (dpy2 < d2) {
	    doVoxel(xx, yy+1, zz, p, density, num, d, vec);
	    d2 = d[num - 1];
	}
	double dny2 = p.y >= 0. ? sqr(frac(p.y)) : sqr(1. - frac(p.y));
	if (dny2 < d2) {
	    doVoxel(xx, yy-1, zz, p, density, num, d, vec);
	    d2 = d[num - 1];
	}
	double dpz2 = p.z >= 0. ? sqr(1. - frac(p.z)) : sqr(frac(p.z));
	if (dpz2 < d2) {
	    doVoxel(xx, yy, zz+1, p, density, num, d, vec);
	    d2 = d[num - 1];
	}
	double dnz2 = p.z >= 0. ? sqr(frac(p.z)) : sqr(1. - frac(p.z));
	if (dnz2 < d2) {
	    doVoxel(xx, yy, zz-1, p, density, num, d, vec);
	    d2 = d[num - 1];
	}

	// finally check the remaining adjacent voxels
	for (i = -1; i <= 1; ++i)
	    for (int j = -1; j <= 1; ++j)
		for (int k = -1; k <= 1; ++k) {
		    // don't check the ones we already did above
		    if (abs(i) + abs(j) + abs(k) <= 1)
			continue;

		    // find squared distance to voxel
		    double vd2 = 0;
		    if (i < 0)
			vd2 += dnx2;
		    else if (i > 0)
			vd2 += dpx2;

		    if (j < 0)
			vd2 += dny2;
		    else if (j > 0)
			vd2 += dpy2;

		    if (k < 0)
			vd2 += dnz2;
		    else if (k > 0)
			vd2 += dpz2;

		    // and check it if it's close enough to matter
		    if (vd2 < d2) {
			doVoxel(xx + i, yy + j, zz + k, p, density, num, d, vec);
			d2 = d[num - 1];
		    }
		}
    }
    else {
	for (i = -1; i <= 1; ++i)
	    for (int j = -1; j <= 1; ++j)
		for (int k = -1; k <= 1; ++k)
		    doVoxel(xx + i, yy + j, zz + k, p, density, num, d, vec);
    }
}