ell.cpp

快速fft变换的c实现

				}
			}
		}
		else{
			if(y + r < cy)
			{
				if(z + r < cz)
				{
					if(m_pChild[0] != NULL)
						m_pChild[0]->AddIntoTable(ps, table, tableSize, x, y, z, r, sizeX, sizeY, sizeZ);
					if(m_pChild[1] != NULL)
						m_pChild[1]->AddIntoTable(ps, table, tableSize, x, y, z, r, sizeX, sizeY, sizeZ);
				}
				else if(z - r >= cz)
				{
					if(m_pChild[4] != NULL)
						m_pChild[4]->AddIntoTable(ps, table, tableSize, x, y, z, r, sizeX, sizeY, sizeZ);
					if(m_pChild[5] != NULL)
						m_pChild[5]->AddIntoTable(ps, table, tableSize, x, y, z, r, sizeX, sizeY, sizeZ);
				}
				else
				{
					if(m_pChild[0] != NULL)
						m_pChild[0]->AddIntoTable(ps, table, tableSize, x, y, z, r, sizeX, sizeY, sizeZ);
					if(m_pChild[1] != NULL)
						m_pChild[1]->AddIntoTable(ps, table, tableSize, x, y, z, r, sizeX, sizeY, sizeZ);
					if(m_pChild[4] != NULL)
						m_pChild[4]->AddIntoTable(ps, table, tableSize, x, y, z, r, sizeX, sizeY, sizeZ);
					if(m_pChild[5] != NULL)
						m_pChild[5]->AddIntoTable(ps, table, tableSize, x, y, z, r, sizeX, sizeY, sizeZ);
				}
			}
			else if(y - r >= cy)
			{
				if(z + r < cz)
				{
					if(m_pChild[2] != NULL)
						m_pChild[2]->AddIntoTable(ps, table, tableSize, x, y, z, r, sizeX, sizeY, sizeZ);
					if(m_pChild[3] != NULL)
						m_pChild[3]->AddIntoTable(ps, table, tableSize, x, y, z, r, sizeX, sizeY, sizeZ);
				}
				else if(z - r >= cz)
				{
					if(m_pChild[6] != NULL)
						m_pChild[6]->AddIntoTable(ps, table, tableSize, x, y, z, r, sizeX, sizeY, sizeZ);
					if(m_pChild[7] != NULL)
						m_pChild[7]->AddIntoTable(ps, table, tableSize, x, y, z, r, sizeX, sizeY, sizeZ);
				}
				else
				{
					if(m_pChild[2] != NULL)
						m_pChild[2]->AddIntoTable(ps, table, tableSize, x, y, z, r, sizeX, sizeY, sizeZ);
					if(m_pChild[3] != NULL)
						m_pChild[3]->AddIntoTable(ps, table, tableSize, x, y, z, r, sizeX, sizeY, sizeZ);
					if(m_pChild[6] != NULL)
						m_pChild[6]->AddIntoTable(ps, table, tableSize, x, y, z, r, sizeX, sizeY, sizeZ);
					if(m_pChild[7] != NULL)
						m_pChild[7]->AddIntoTable(ps, table, tableSize, x, y, z, r, sizeX, sizeY, sizeZ);
				}
			}
			else
			{
				if(z + r < cz)
				{
					if(m_pChild[0] != NULL)
						m_pChild[0]->AddIntoTable(ps, table, tableSize, x, y, z, r, sizeX, sizeY, sizeZ);
					if(m_pChild[1] != NULL)
						m_pChild[1]->AddIntoTable(ps, table, tableSize, x, y, z, r, sizeX, sizeY, sizeZ);
					if(m_pChild[2] != NULL)
						m_pChild[2]->AddIntoTable(ps, table, tableSize, x, y, z, r, sizeX, sizeY, sizeZ);
					if(m_pChild[3] != NULL)
						m_pChild[3]->AddIntoTable(ps, table, tableSize, x, y, z, r, sizeX, sizeY, sizeZ);
				}
				else if(z - r >= cz)
				{
					if(m_pChild[4] != NULL)
						m_pChild[4]->AddIntoTable(ps, table, tableSize, x, y, z, r, sizeX, sizeY, sizeZ);
					if(m_pChild[5] != NULL)
						m_pChild[5]->AddIntoTable(ps, table, tableSize, x, y, z, r, sizeX, sizeY, sizeZ);
					if(m_pChild[6] != NULL)
						m_pChild[6]->AddIntoTable(ps, table, tableSize, x, y, z, r, sizeX, sizeY, sizeZ);
					if(m_pChild[7] != NULL)
						m_pChild[7]->AddIntoTable(ps, table, tableSize, x, y, z, r, sizeX, sizeY, sizeZ);
				}
				else
				{
					if(m_pChild[0] != NULL)
						m_pChild[0]->AddIntoTable(ps, table, tableSize, x, y, z, r, sizeX, sizeY, sizeZ);
					if(m_pChild[1] != NULL)
						m_pChild[1]->AddIntoTable(ps, table, tableSize, x, y, z, r, sizeX, sizeY, sizeZ);
					if(m_pChild[2] != NULL)
						m_pChild[2]->AddIntoTable(ps, table, tableSize, x, y, z, r, sizeX, sizeY, sizeZ);
					if(m_pChild[3] != NULL)
						m_pChild[3]->AddIntoTable(ps, table, tableSize, x, y, z, r, sizeX, sizeY, sizeZ);
					if(m_pChild[4] != NULL)
						m_pChild[4]->AddIntoTable(ps, table, tableSize, x, y, z, r, sizeX, sizeY, sizeZ);
					if(m_pChild[5] != NULL)
						m_pChild[5]->AddIntoTable(ps, table, tableSize, x, y, z, r, sizeX, sizeY, sizeZ);
					if(m_pChild[6] != NULL)
						m_pChild[6]->AddIntoTable(ps, table, tableSize, x, y, z, r, sizeX, sizeY, sizeZ);
					if(m_pChild[7] != NULL)
						m_pChild[7]->AddIntoTable(ps, table, tableSize, x, y, z, r, sizeX, sizeY, sizeZ);
				}
			}
		}
	}
}

void Cell::DrawCell(float sizeX, float sizeY, float sizeZ)
{
	if (m_bIsLeaf)
	{
		GLfloat a[3], b[3];
		a[0] = m_fOriginX;
		a[1] = m_fOriginY;
		a[2] = m_fOriginZ;

		b[0] = a[0] + GetSize(sizeX);
		b[1] = a[1] + GetSize(sizeY);
		b[2] = a[2] + GetSize(sizeZ);

		glShadeModel(GL_SMOOTH);

		glBegin(GL_LINE_LOOP);
		glVertex3f(a[0], a[1], b[2]);
		glVertex3f(b[0], a[1], b[2]);
		glVertex3f(b[0], b[1], b[2]);
		glVertex3f(a[0], b[1], b[2]);
		glEnd();

		glBegin(GL_LINES);
		glVertex3f(a[0], a[1], b[2]);
		glVertex3f(a[0], a[1], a[2]);

		glVertex3f(a[0], b[1], b[2]);
		glVertex3f(a[0], b[1], a[2]);

		glVertex3f(b[0], b[1], b[2]);
		glVertex3f(b[0], b[1], a[2]);

		glVertex3f(b[0], a[1], b[2]);
		glVertex3f(b[0], a[1], a[2]);
		glEnd();

		glBegin(GL_LINE_LOOP);
		glVertex3f(a[0], a[1], a[2]);
		glVertex3f(a[0], b[1], a[2]);
		glVertex3f(b[0], b[1], a[2]);
		glVertex3f(b[0], a[1], a[2]);
		glEnd();
	}
	else
	{
		for (int i=0; i<8; i++)
		{
			if (m_pChild[i])
				m_pChild[i]->DrawCell(sizeX, sizeY, sizeZ);
		}
	}
	glFlush();
	glDisable(GL_POINT_SMOOTH);
	glHint(GL_POINT_SMOOTH_HINT, GL_FASTEST);
}

int* Cell::GetPointIndex()
{
	return m_pPointIndex;
}

void Cell::GetCoarseGrid(int num, int *indexTable, int &tableSize)
{
	if (m_bIsLeaf && m_iPointNum>0)
	{
		for (int i=0; i<num; i++)
			indexTable[tableSize++] = m_pPointIndex[i];
	}
	else
	{
		for (int i=0; i<8; i++)
		{
			if (m_pChild[i])
				m_pChild[i]->GetCoarseGrid(num, indexTable, tableSize);
		}
	}
}

void Cell::GetLeavesNum(int &count)
{
	if (m_bIsLeaf)
	{
		if (m_iPointNum > 0)
			count++;
	}
	else
	{
		for (int i=0; i<8; i++)
		{
			if (m_pChild[i])
				m_pChild[i]->GetLeavesNum(count);
		}
	}
}

void Cell::FitRBF(PointSet* ps, float* lamta, float *b)
{
	if (m_bIsLeaf)
	{
		float off = 0.01;
		int n = m_iPointNum;
		int *index = new int[2*n+5];
		float **A = new float*[2*n+5];
		for(int i=1; i<=2*n+4; i++)
			A[i] = new float[2*n+5];
		double *solution = new double[2*n+5];

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

			float *point = ps->GetPoint(m_pPointIndex[i]);
			float *ni = ps->GetNormal(m_pPointIndex[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(m_pPointIndex[j]);
				float *pt = ps->GetPoint(m_pPointIndex[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(m_pPointIndex[j]);
				float *pt = ps->GetPoint(m_pPointIndex[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;

	//	int N = ps->GetPointNum();
		for (i=0; i<n; i++)
		{
			int j = m_pPointIndex[i];
			lamta[j+j] = solution[i+i+1];
			lamta[j+j+1] = solution[i+i+2];
		}
	/*	lamta[N+N+1] += solution[n+n+1];
		lamta[N+N+2] += solution[n+n+2];
		lamta[N+N+3] += solution[n+n+3];
		lamta[N+N+4] += solution[n+n+4];*/

		delete[] solution;
	}
	else
	{
		for (int i=0; i<8; i++)
		{
			if (m_pChild[i])
				m_pChild[i]->FitRBF(ps, lamta, b);
		}
	}
}

float Cell::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:
		return 1;
	case 1:
		return float(sqrt(sqrt(rr+c*c)));
	case 2:
		return float(sqrt(rr+c*c));
	default:
		return float(pow(sqrt(rr+c*c), k));
	}
}

void Cell::Delete()
{
	if (m_bIsLeaf)
	{
		if (m_iPointNum > 0)
			delete[] m_pPointIndex;
		m_iPointNum = 0;
	}
	else
	{
		for (int i=0; i<8; i++)
		{
			if (m_pChild[i])
			{
				m_pChild[i]->Delete();
				m_pChild[i] = 0;
			}
		}
	}
}