rbfunc.cpp

快速fft变换的c实现

// RBFunc.cpp: implementation of the RBFunc class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "FastRBF.h"
#include "RBFunc.h"
#include <math.h>
#include "MathMethod\LU.h"
#include "Bloomenthal.h"

#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

RBFunc::RBFunc()
{
	m_pPointSet = 0;
	m_fPrecision = 0.01f;
	m_pOctaTree = 0;
	m_pSolution = 0;
	m_fSupport = 0;
}

RBFunc::~RBFunc()
{
	if (m_pOctaTree)
	{
		delete m_pOctaTree;
		m_pOctaTree = 0;
	}
	if (m_pSolution)
	{
		delete[] m_pSolution;
		m_pSolution = 0;
	}
}

float RBFunc::GetValue(float x, float y, float z)
{
	if (m_pSolution == NULL)
		return 0;

	double f = 0;
	float off = 0.01f;
	int n = m_pPointSet->GetPointNum();

	for (int i=0; i<n; i++)
	{
		float *c = m_pPointSet->GetPoint(i);
		float *ni = m_pPointSet->GetNormal(i);
		float p[3];
		float v[3];
		p[0] = c[0] - off*ni[0];
		p[1] = c[1] - off*ni[1];
		p[2] = c[2] - off*ni[2];

		v[0] = x;
		v[1] = y;
		v[2] = z;

		f += m_pSolution[2*i+1]*BasicFunc(v, p, 2, m_fSupport);
	}
	for (i=0; i<n; i++)
	{
		float *c = m_pPointSet->GetPoint(i);
		float *ni = m_pPointSet->GetNormal(i);
		float p[3];
		float v[3];
		p[0] = c[0] + off*ni[0];
		p[1] = c[1] + off*ni[1];
		p[2] = c[2] + off*ni[2];

		v[0] = x;
		v[1] = y;
		v[2] = z;

		f += m_pSolution[2*i+2]*BasicFunc(v, p, 2, m_fSupport);
	}

	f += (x*m_pSolution[2*n+1] + y*m_pSolution[2*n+2] + z*m_pSolution[2*n+3] + m_pSolution[2*n+4]);

	return (float)f;
}

double* RBFunc::FitCoarseGrid(PointSet *ps, double *solution, int *pointIndex, float *b, int size, float tolerance)
{
	float off = 0.01f;
	int n = size;
	int *index = new int[2*n+5];
	float **A = new float*[2*n+5];
	for (int i=1; i<2*n+5; i++)
		A[i] = new float[2*n+5];

	for(i=0; i<n; i++)
	{
		float vx, vy, vz;
		float p[3];

		float *point = ps->GetPoint(pointIndex[i]);
		float *ni = ps->GetNormal(pointIndex[i]);
		p[0] = point[0] - off*ni[0];
		p[1] = point[1] - off*ni[1];
		p[2] = point[2] - off*ni[2];
		for(int j=0; j<n; j++)
		{
			float q[3];
			float *m = ps->GetNormal(pointIndex[j]);
			float *pt = ps->GetPoint(pointIndex[j]);
			q[0] = pt[0] - off*m[0];
			q[1] = pt[1] - off*m[1];
			q[2] = pt[2] - off*m[2];

			vx = p[0] - q[0];
			vy = p[1] - q[1];
			vz = p[2] - q[2];
			A[2*i+1][2*j+1] = (float)sqrt(vx*vx + vy*vy + vz*vz);

			q[0] = pt[0] + off*m[0];
			q[1] = pt[1] + off*m[1];
			q[2] = pt[2] + off*m[2];
				
			vx = p[0] - q[0];
			vy = p[1] - q[1];
			vz = p[2] - q[2];
			A[2*i+1][2*j+2] = (float)sqrt(vx*vx + vy*vy + vz*vz);
		}
		A[2*i+1][2*n+1] = p[0];
		A[2*i+1][2*n+2] = p[1];
		A[2*i+1][2*n+3] = p[2];
		A[2*i+1][2*n+4] = 1;
		A[2*n+1][2*i+1] = p[0];
		A[2*n+2][2*i+1] = p[1];
		A[2*n+3][2*i+1] = p[2];
		A[2*n+4][2*i+1] = 1;
		solution[2*i+1] = b[i+i];

		p[0] = point[0] + off*ni[0];
		p[1] = point[1] + off*ni[1];
		p[2] = point[2] + off*ni[2];
		for(j=0; j<n; j++)
		{
			float q[3];
			float *m = ps->GetNormal(pointIndex[j]);
			float *pt = ps->GetPoint(pointIndex[j]);
			q[0] = pt[0] - off*m[0];
			q[1] = pt[1] - off*m[1];
			q[2] = pt[2] - off*m[2];

			vx = p[0] - q[0];
			vy = p[1] - q[1];
			vz = p[2] - q[2];
			A[2*i+2][2*j+1] = (float)sqrt(vx*vx + vy*vy + vz*vz);

			q[0] = pt[0] + off*m[0];
			q[1] = pt[1] + off*m[1];
			q[2] = pt[2] + off*m[2];
				
			vx = p[0] - q[0];
			vy = p[1] - q[1];
			vz = p[2] - q[2];
			A[2*i+2][2*j+2] = (float)sqrt(vx*vx + vy*vy + vz*vz);
		}
		A[2*i+2][2*n+1] = p[0];
		A[2*i+2][2*n+2] = p[1];
		A[2*i+2][2*n+3] = p[2];
		A[2*i+2][2*n+4] = 1;
		A[2*n+1][2*i+2] = p[0];
		A[2*n+2][2*i+2] = p[1];
		A[2*n+3][2*i+2] = p[2];
		A[2*n+4][2*i+2] = 1;
		solution[2*i+2] = b[i+i+1];
	}
	for (i=2*n+1; i<=2*n+4; i++)
	{
		for(int j=2*n+1; j<=2*n+4; j++)
		{
			A[i][j] = 0;
		}
		solution[i] = 0;
	}

	LU lu;
	float d;
	lu.ludcmp(A, 2*n+4, index, &d);
	lu.lubksb(A, 2*n+4, index, solution);
	delete[] index;
	for(i=1; i<=2*n+4; i++)
		delete[] A[i];
	delete[] A;

	return solution;
}

///////////////////////////////////////////////////////////////////////////////////////////////////////
// Function: Fast multipole method fit rbf
// Author: Tingfang Zhou
// Date: 2003-11
// Ref: Get from B.K.Beatson's paper
// <<Fast Sulotion For The Radial Basis Function InterPolation Equations>>
// Method: Domain Decomposition Methods
// tolerance -- the desired accuracy
// f  -- the right-hand side of equation
// Rg -- the current residual
// Sg -- the current approximation to the interpolation
// Lg -- the parameters of the current approximation to the interpolation at the finest or global
// Cg -- level
///////////////////////////////////////////////////////////////////////////////////////////////////////
float* RBFunc::FMMFitGlobal(PointSet *ps, float tolerance)
{
	int n = ps->GetPointNum();
	int M = n+n;
	float off=0.01f;
	if (m_pSolution)
		delete[] m_pSolution;
	m_pSolution = new double[M+5];
	//Input the finite node set ps
	m_pPointSet = ps;

	//Input the right-hand side
	float *f = new float[M];
	for (int i=0; i<n; i++)
	{
		f[i+i] = -off;
		f[i+i+1] = off;
	}

	//Subdivide space into overlapping subdomains
	if (m_pOctaTree)
		delete m_pOctaTree;
	m_pOctaTree = new OctaTree;
	m_pOctaTree->SetPointSet(ps);

	//Choose a coarse grid Y(2) containing some points from each inner subdomain
	int num = 1;
	int size = m_pOctaTree->GetLeavesNum();
	int *coarseGridIndex = new int[size*num];
	m_pOctaTree->GetCoarseGrid(num, coarseGridIndex, 0);

	//Initialize Rg, Sg, Lg, Cg
	float *Rg = new float[M];
	float *Sg = new float[M];
	float *Lg = new float[M+4];
	int Cg = 0;
	for (i=0; i<M; i++)
	{
		Rg[i] = f[i];
		Sg[i] = 0;
		Lg[i] = 0;
	}

	//Iterative fit rbf
	while (Norm(Rg, M) > tolerance)
	{
		//Solve Lg(i) from the spline space Xj to Rg|Xj
		for (int j=0; j<size; j++)
		{
			m_pOctaTree->FitSubdomains(ps, Lg, size, Rg);
		}

		//Correct Lg to be orthogonal to PI_(k-1)
		float p[4];
		p[0] = p[1] = p[2] = p[3] = 0;
		for (i=0; i<n; i++)
		{
			float *point = ps->GetPoint(i);
			float *normal = ps->GetNormal(i);
			p[0] += ((point[0]+off*normal[0])*Lg[2*i] + (point[0]-off*normal[0])*Lg[2*i+1]);
			p[1] += ((point[1]+off*normal[1])*Lg[2*i] + (point[1]-off*normal[1])*Lg[2*i+1]);
			p[2] += ((point[2]+off*normal[2])*Lg[2*i] + (point[2]-off*normal[2])*Lg[2*i+1]);
			p[3] += (Lg[2*i] + Lg[2*i+1]);
		}
		for (i=0; i<n; i++)
		{
			float *point = ps->GetPoint(i);
			float *normal = ps->GetNormal(i);
			for (int k=0; k<3; k++)
			{
				Lg[i+i] -= ((point[k] + off*normal[k])*p[k]);
				Lg[i+i+1] -= ((point[k] - off*normal[k])*p[k]);
			}
			Lg[i+i] -= p[3];
			Lg[i+i+1] -= p[3];
		}

		//S1 = SIGMA(Lg * Xj)
		float *S1 = new float[n+n];
		for (i=0; i<n+n; i++)
			S1[i] = 0;
		for (i=0; i<n; i++)
		{
			float *point = ps->GetPoint(i);
			float *normal = ps->GetNormal(i);
			for (int k=0; k<n; k++)
			{
				float *pt = ps->GetPoint(k);
				float *nor = ps->GetNormal(k);
				float q[3];
				q[0] = pt[0] + off*nor[0];
				q[1] = pt[1] + off*nor[1];
				q[2] = pt[2] + off*nor[2];
				S1[i+i] += Lg[i+i]*BasicFunc(point, q, 2, 0);
				q[0] = pt[0] - off*nor[0];
				q[1] = pt[1] - off*nor[1];
				q[2] = pt[2] - off*nor[2];
				S1[i+i+1] += Lg[i+i+1]*BasicFunc(point, q, 2, 0);
			}
		}

		//Evaluate the residual R1=Rg-S1 at the coarse grid
		i = 0;
		float *R1 = new float[2*size*num];
		while (i<size*num)
		{
			int k = coarseGridIndex[i];
			R1[k+k] = Rg[k+k] - S1[k+k];
			R1[k+k+1] = Rg[k+k+1] - S1[k+k+1];
			i++;
		}
		
		//Interpolate to R1 at the coarse grid points Y(2) using a spline
		//S2 (including the polynomial part) from y(2)
		double *coarseSol = new double[2*size*num+5];
		FitCoarseGrid(ps, coarseSol, coarseGridIndex, R1, size*num, tolerance);
		delete[] R1;
		float *S2 = new float[2*size*num+4];
		for (i=0; i<2*size*num+4; i++)
			S2[i] = (float)coarseSol[i+1];

		delete[] coarseSol;

		//Sg = Sg + S1 + S2
		for (i=0; i<n+n; i++)
		{
			Sg[i] += S1[i];
		}
		for (i=0; i<size*num; i++)
		{
			int k = coarseGridIndex[i];
			Sg[k+k] += S2[k+k];
			Sg[k+k+1] += S2[k+k+1];
		}
		Lg[M] = S2[2*size*num];
		Lg[M+1] = S2[2*size*num+1];
		Lg[M+2] = S2[2*size*num+2];
		Lg[M+3] = S2[2*size*num+3];
		delete[] S1;
		delete[] S2;

		//Reevaluate the global residual Rg = f - Sg at all the points of X
		for (i=0; i<M; i++)
			Rg[i] = f[i] - Sg[i];
	}

	delete[] coarseGridIndex;
	delete[] f;
	return Lg;
}

float RBFunc::Norm(float *p, int n)
{
	double q=0;
	for (int i=0; i<n; i++)
		q += p[i]*p[i];

	q = sqrt(q);
	return (float)q;
}

float RBFunc::BasicFunc(float *X, float *C, int k, float c)
{
	float p[3];
	p[0] = X[0]-C[0];
	p[1] = X[1]-C[1];
	p[2] = X[2]-C[2];
	float rr = p[0]*p[0] + p[1]*p[1] + p[2]*p[2];
	switch(k)
	{
	case 0:
		if (rr == 0)
			return 0;
		else
			return float(rr*log(sqrt(rr)));
	case 1:
		return float(-sqrt(rr+c*c));
	case 2:
		return float(sqrt(rr+c*c));
	case 3:
		return float(1/sqrt(rr+c*c));
	default:
		return float(pow(sqrt(rr+c*c), k));
	}
}

void RBFunc::Draw(PointSet *ps)
{
	if (m_pOctaTree)
		delete m_pOctaTree;
	m_pOctaTree = new OctaTree;
	m_pOctaTree->SetPointSet(ps);
	m_pOctaTree->Draw();
}

void RBFunc::DirectFitGlobal(PointSet *ps, int functype)
{
	m_pPointSet = ps;
	float off = 0.01f;
	int n = ps->GetPointNum();
	int *index = new int[2*n+5];
	float **A = new float*[2*n+5];
	for (int i=1; i<2*n+5; i++)
		A[i] = new float[2*n+5];
	if (m_pSolution)
		delete[] m_pSolution;
	m_pSolution = new double[2*n+5];

	///////////////////////////////////////////////////////////////
	for(i=0; i<n; i++)
	{
		float p[3];

		float *point = ps->GetPoint(i);
		float *ni = ps->GetNormal(i);
		p[0] = point[0] - off*ni[0];
		p[1] = point[1] - off*ni[1];
		p[2] = point[2] - off*ni[2];
		for(int j=0; j<n; j++)
		{
			float q[3];
			float *m = ps->GetNormal(j);
			float *pt = ps->GetPoint(j);
			q[0] = pt[0] - off*m[0];
			q[1] = pt[1] - off*m[1];
			q[2] = pt[2] - off*m[2];

			A[2*i+1][2*j+1] = BasicFunc(p, q, functype, m_fSupport);

			q[0] = pt[0] + off*m[0];
			q[1] = pt[1] + off*m[1];
			q[2] = pt[2] + off*m[2];
				
			A[2*i+1][2*j+2] = BasicFunc(p, q, functype, m_fSupport);
		}
		A[2*i+1][2*n+1] = p[0];
		A[2*i+1][2*n+2] = p[1];
		A[2*i+1][2*n+3] = p[2];
		A[2*i+1][2*n+4] = 1;
		A[2*n+1][2*i+1] = p[0];
		A[2*n+2][2*i+1] = p[1];
		A[2*n+3][2*i+1] = p[2];
		A[2*n+4][2*i+1] = 1;
		m_pSolution[2*i+1] = -off;

		p[0] = point[0] + off*ni[0];
		p[1] = point[1] + off*ni[1];
		p[2] = point[2] + off*ni[2];
		for(j=0; j<n; j++)
		{
			float q[3];
			float *m = ps->GetNormal(j);
			float *pt = ps->GetPoint(j);
			q[0] = pt[0] - off*m[0];
			q[1] = pt[1] - off*m[1];
			q[2] = pt[2] - off*m[2];

			A[2*i+2][2*j+1] = BasicFunc(p, q, functype, m_fSupport);

			q[0] = pt[0] + off*m[0];
			q[1] = pt[1] + off*m[1];
			q[2] = pt[2] + off*m[2];
				
			A[2*i+2][2*j+2] = BasicFunc(p, q, functype, m_fSupport);
		}
		A[2*i+2][2*n+1] = p[0];
		A[2*i+2][2*n+2] = p[1];
		A[2*i+2][2*n+3] = p[2];
		A[2*i+2][2*n+4] = 1;
		A[2*n+1][2*i+2] = p[0];
		A[2*n+2][2*i+2] = p[1];
		A[2*n+3][2*i+2] = p[2];
		A[2*n+4][2*i+2] = 1;
		m_pSolution[2*i+2] = off;
	}
	for (i=2*n+1; i<=2*n+4; i++)
	{
		for(int j=2*n+1; j<=2*n+4; j++)
		{
			A[i][j] = 0;
		}
		m_pSolution[i] = 0;
	}

	LU lu;
	float d;
	lu.ludcmp(A, 2*n+4, index, &d);
	lu.lubksb(A, 2*n+4, index, m_pSolution);
	delete[] index;
	for(i=1; i<=2*n+4; i++)
		delete[] A[i];
	delete[] A;

	m_fSupport = 1;
}

Mesh* RBFunc::Bloomenthal(float size, float o[], float box[])
{
	float s = (box[0] - box[1])*0.1f;
	box[0] += s; box[1] -= s;
	s = (box[2] - box[3])*0.1f;
	box[2] += s; box[3] -= s;
	s = (box[4] - box[5])*0.1f;
	box[4] += s; box[5] -= s;
	BLOOMENTHAL::Bloomenthal bloom;
	bloom.func = this;
	Mesh* mesh = new Mesh;
	bloom.polygonize(mesh, size, box, o[0], o[1], o[2], 1); 
	return mesh;
}

double* RBFunc::GetSolution()
{
	return m_pSolution;
}