pointset.cpp

快速fft变换的c实现

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

#include "stdafx.h"
#include "FastRBF.h"
#include "PointSet.h"
#include <math.h>
#include <GL\glut.h>

#define PI 3.1415926535897932384626433832795

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

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

PointSet::PointSet()
{
	m_pBounds = 0;
	m_iPointNum = 0;
	m_pNormals = 0;
	m_pPoints = 0;
}

PointSet::~PointSet()
{
	if (m_pBounds)
	{
		delete[] m_pBounds;
		m_pBounds = 0;
	}
	if (m_pPoints)
	{
		delete[] m_pPoints;
		m_pPoints = 0;
	}
	if (m_pNormals)
	{
		delete[] m_pNormals;
		m_pNormals = 0;
	}
}

void PointSet::SetPointNum(int N)
{
	m_iPointNum = N;
	m_pBounds = new bool[N];
	m_pPoints = new float[N][3];
	m_pNormals = new float[N][3];
	for (int i=0; i<N; i++)
		m_pBounds[i] = false;
}

int PointSet::GetPointNum()
{
	return m_iPointNum;
}

void PointSet::SetPoint(int i, float x, float y, float z)
{
	m_pPoints[i][0] = x;
	m_pPoints[i][1] = y;
	m_pPoints[i][2] = z;
}

//return the ith point pointer
float* PointSet::GetPoint(int i)
{
//	if (!m_pPoints || i<0 || i>=m_iPointNum)
//		return false;

	return m_pPoints[i];
}

void PointSet::SetNormal(int i, float x, float y, float z)
{
	m_pNormals[i][0] = x;
	m_pNormals[i][1] = y;
	m_pNormals[i][2] = z;
}

//return the ith normal pointer
float* PointSet::GetNormal(int i)
{
//	if (!m_pNormals || i<0 || i>=m_iPointNum)
//		return false;

	return m_pNormals[i];
}

void PointSet::GetBound(float *minCoord, float *maxCoord)
{
	minCoord[0] = minCoord[1] = minCoord[2] = 10000000000;
	maxCoord[0] = maxCoord[1] = maxCoord[2] = -10000000000;
	for(int i=0; i<m_iPointNum; i++)
	{
		float *p = m_pPoints[i];
		if(p[0] < minCoord[0])
			minCoord[0] = p[0];
		if(p[0] > maxCoord[0])
			maxCoord[0] = p[0];

		if(p[1] < minCoord[1])
			minCoord[1] = p[1];
		if(p[1] > maxCoord[1])
			maxCoord[1] = p[1];

		if(p[2] < minCoord[2])
			minCoord[2] = p[2];
		if(p[2] > maxCoord[2])
			maxCoord[2] = p[2];
	}
}

void PointSet::GetCentroid(float *centriod, int startIndex, int endIndex)
{
	centriod[0] = centriod[1] = centriod[2] = 0;
	for(int i=startIndex; i<endIndex; i++)
	{
		centriod[0] += m_pPoints[i][0];
		centriod[1] += m_pPoints[i][1];
		centriod[2] += m_pPoints[i][2];
	}
	centriod[0] /= (endIndex-startIndex);
	centriod[1] /= (endIndex-startIndex);
	centriod[2] /= (endIndex-startIndex);
}

PointSet* PointSet::CopyPointSet()
{
	PointSet* ps = new PointSet;
	for (int i=0; i<m_iPointNum; i++)
	{
		ps->SetPoint(i, m_pPoints[i][0], m_pPoints[i][1], m_pPoints[i][2]);
		ps->SetNormal(i, m_pNormals[i][0], m_pNormals[i][1], m_pNormals[i][2]);
	}
	return ps;
}

void PointSet::Shift(float sx, float sy, float sz)
{
	for (int i=0; i<m_iPointNum; i++)
	{
		m_pPoints[i][0] += sx;
		m_pPoints[i][1] += sy;
		m_pPoints[i][2] += sz;
	}
}

void PointSet::Scale(float s)
{
	for (int i=0; i<m_iPointNum; i++)
	{
		m_pPoints[i][0] *= s;
		m_pPoints[i][1] *= s;
		m_pPoints[i][2] *= s;
	}
}

//Input angle, not radian
void PointSet::Rotate(float rx, float ry, float rz)
{
	rx = (float)(rx*PI/180);
	ry = (float)(ry*PI/180);
	rz = (float)(rz*PI/180);
	if(rx != 0)
	{
		double s = sin(rx);
		double c = cos(rx);
		for(int i=0; i<m_iPointNum; i++)
		{
			float y = m_pPoints[i][1];
			float z = m_pPoints[i][2];

			m_pPoints[i][1] = (float)(c*y + s*z);
			m_pPoints[i][2] = (float)(-s*y + c*z);

			y = m_pNormals[i][1];
			z = m_pNormals[i][2];

			m_pNormals[i][1] = (float)(c*y + s*z);
			m_pNormals[i][2] = (float)(-s*y + c*z);
		}
	}

	if(ry != 0)
	{
		double s = sin(ry);
		double c = cos(ry);
		for(int i=0; i<m_iPointNum; i++)
		{
			float z = m_pPoints[i][2];
			float x = m_pPoints[i][0];

			m_pPoints[i][2] = (float)(c*z + s*x);
			m_pPoints[i][0] = (float)(-s*z + c*x);

			z = m_pNormals[i][2];
			x = m_pNormals[i][0];

			m_pNormals[i][2] = (float)(c*z + s*x);
			m_pNormals[i][0] = (float)(-s*z + c*x);
		}
	}

	if(rz != 0)
	{
		double s = sin(rz);
		double c = cos(rz);
		for(int i=0; i<m_iPointNum; i++)
		{
			float x = m_pPoints[i][0];
			float y = m_pPoints[i][1];

			m_pPoints[i][0] = (float)(c*x + s*y);
			m_pPoints[i][1] = (float)(-s*x + c*y);

			x = m_pNormals[i][0];
			y = m_pNormals[i][1];

			m_pNormals[i][0] = (float)(c*x + s*y);
			m_pNormals[i][1] = (float)(-s*x + c*y);
		}
	}
}

void PointSet::SwapIndex(int i, int j)
{
	float tmp = m_pPoints[i][0];
	m_pPoints[i][0] = m_pPoints[j][0];
	m_pPoints[j][0] = tmp;

	tmp = m_pPoints[i][1];
	m_pPoints[i][1] = m_pPoints[j][1];
	m_pPoints[j][1] = tmp;

	tmp = m_pPoints[i][2];
	m_pPoints[i][2] = m_pPoints[j][2];
	m_pPoints[j][2] = tmp;
		
	tmp = m_pNormals[i][0];
	m_pNormals[i][0] = m_pNormals[j][0];
	m_pNormals[j][0] = tmp;

	tmp = m_pNormals[i][1];
	m_pNormals[i][1] = m_pNormals[j][1];
	m_pNormals[j][1] = tmp;

	tmp = m_pNormals[i][2];
	m_pNormals[i][2] = m_pNormals[j][2];
	m_pNormals[j][2] = tmp;

	bool tmpB = m_pBounds[i];
	m_pBounds[i] = m_pBounds[j];
	m_pBounds[j] = tmpB;
}

void PointSet::DrawPoints()
{
	if (!m_iPointNum || !m_pPoints || !m_pNormals)
		return;

	glShadeModel(GL_SMOOTH);
//	glDisable(GL_LIGHTING);
//	glColor3f(.0f, .0f, .0f);

	glBegin(GL_POINTS);
	for (int i=0; i<m_iPointNum; i++)
	{
//		glNormal3fv(m_pNormals[i]);
		glVertex3fv(m_pPoints[i]);
	}
	glEnd();
	glFlush();
	glDisable(GL_POINT_SMOOTH);
	glHint(GL_POINT_SMOOTH_HINT, GL_FASTEST);
//	glEnable(GL_LIGHTING);
}

void PointSet::DrawShadedPoints()
{
	if (!m_iPointNum || !m_pPoints || !m_pNormals)
		return;

	glShadeModel(GL_SMOOTH);
	glEnable(GL_LIGHTING);

	glBegin(GL_POINTS);
	for (int i=0; i<m_iPointNum; i++)
	{
		glNormal3fv(m_pNormals[i]);
		glVertex3fv(m_pPoints[i]);
	}
	glEnd();
	glFlush();
	glDisable(GL_POINT_SMOOTH);
	glHint(GL_POINT_SMOOTH_HINT, GL_FASTEST);
}

BOOL PointSet::OpenFile(CString fileName, CString fileExt)
{
	if (fileExt != "pwn")
	{//Points with normals
		TRACE("Unknow file format!");
		return FALSE;
	}

	FILE *in = fopen(fileName, "r");
	if (!in)
		return FALSE;

	//read points number
	fscanf(in, "%d", &m_iPointNum);
	SetPointNum(m_iPointNum);

	//read points coordinates
	for (int i=0; i<m_iPointNum; i++)
		fscanf(in, "%f %f %f", &m_pPoints[i][0], &m_pPoints[i][1], &m_pPoints[i][2]);

	//read points normals
	for (i=0; i<m_iPointNum; i++)
		fscanf(in, "%f %f %f", &m_pNormals[i][0], &m_pNormals[i][1], &m_pNormals[i][2]);

/*	//invert the sign of normals
	for (i=0; i<m_iPointNum; i++)
	{
		for (int j=0; j<3; j++)
			m_pNormals[i][j] = -m_pNormals[i][j];
	}*/

	//close file
	fclose(in);
	return TRUE;
}

BOOL PointSet::WriteFile(CString fileName, CString fileExt)
{
	if (fileExt != "pwn")
	{//Points with normals
		TRACE("Unknow file format!");
		return FALSE;
	}

	FILE *out = fopen(fileName, "w");
	if (!out)
		return FALSE;

	//write points number
	fprintf(out, "%d\n", m_iPointNum);

	//write points coordinate
	for (int i=0; i<m_iPointNum; i++)
		fprintf(out, "%f %f %f\n", m_pPoints[i][0], m_pPoints[i][1], m_pPoints[i][2]);

	//write points normals
	for (i=0; i<m_iPointNum; i++)
		fprintf(out, "%f %f %f\n", m_pNormals[i][0], m_pNormals[i][1], m_pNormals[i][2]);

	//close file
	fclose(out);
	return TRUE;
}

void PointSet::GetBoundingBox(float *minCoord, float *maxCoord)
{
	minCoord[0] = minCoord[1] = minCoord[2] = (float)1E+38;
	maxCoord[0] = maxCoord[1] = maxCoord[2] = (float)-1E+38;

	for (int i=0; i<m_iPointNum; i++)
	{
		for (int j=0; j<3; j++)
		{
			if (m_pPoints[i][j] < minCoord[j])
				minCoord[j] = m_pPoints[i][j];
			if (m_pPoints[i][j] > maxCoord[j])
				maxCoord[j] = m_pPoints[i][j];
		}
	}
}

void PointSet::DrawBox()
{
	if (!m_iPointNum || !m_pPoints)
		return;

	GLfloat a[3], b[3];
	GetBoundingBox(a, b);

	glShadeModel(GL_SMOOTH);
//	glEnable(GL_LIGHTING);

	glBegin(GL_LINE_LOOP);
//	glNormal3f(.0f, .0f, 1.0f);
	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);
//	glNormal3f(.0f, .0f, -1.0f);
	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();
	glFlush();
	glDisable(GL_POINT_SMOOTH);
	glHint(GL_POINT_SMOOTH_HINT, GL_FASTEST);
}

void PointSet::SmoothShading()
{
	glEnable(GL_POINT_SMOOTH);
	glHint(GL_POINT_SMOOTH_HINT, GL_NICEST);
}

vec3f PointSet::GetPoint()
{
	return m_pPoints;
}

vec3f PointSet::GetNormal()
{
	return m_pNormals;
}

void PointSet::ConvertToColor(int n, double *v, float (*color)[3])
{
	float mid, vari;
	float S = 0;
	for(int i=0; i<n; i++)
		S += (float)v[i+1];
	mid = S/(float)n;

	S = 0;
	for(i = 0; i<n; i++)
		S += (float)((v[i+1]-mid)*(v[i+1]-mid));
	vari = (float)sqrt(S/(float)n);

	for(i = 0; i<n; i++)
	{
		float s = ((float)v[i+1]-mid)/vari*1.5f;
		if(s < -2.1f)
		{
			color[i][0] = 0;
			color[i][1] = 0;
			color[i][2] = 0;
		}
		else if(s < -1.5f)
		{
			color[i][0] = 0;
			color[i][1] = 0;
			color[i][2] = (s + 2.1f)/0.6f;
		}
		else if(s < -0.9f)
		{
			color[i][0] = 0;
			color[i][1] = (s + 1.5f)/0.6f;
			color[i][2] = 1;
		}
		else if(s < -0.3f)
		{
			color[i][0] = 0;
			color[i][1] = 1;
			color[i][2] = -(s + 0.3f)/0.6f;
		}
		else if(s < 0.3f)
		{
			color[i][0] = (s + 0.3f)/0.6f;
			color[i][1] = 1;
			color[i][2] = 0;
		}
		else if(s < 0.9f)
		{
			color[i][0] = 1;
			color[i][1] = -(s - 0.9f)/0.6f;
			color[i][2] = 0;
		}
		else if(s < 1.5f)
		{
			color[i][0] = 1;
			color[i][1] = 0;
			color[i][2] = (s - 0.9f)/0.6f;
		}
		else if(s < 2.1f)
		{
			color[i][0] = 1;
			color[i][1] = (s - 1.5f)/0.8f;
			color[i][2] = 1;
		}
		else
		{
			color[i][0] = 1;
			color[i][1] = 0.75;
			color[i][2] = 1;
		}
	}
}

void PointSet::DrawSolution(double *sol)
{
	glShadeModel(GL_SMOOTH);
	glDisable(GL_LIGHTING);

	float (*color)[3] = new float[m_iPointNum][3];
	this->ConvertToColor(m_iPointNum, sol, color);
	glBegin(GL_POINTS);
	for(int i=0; i<m_iPointNum; i++)
	{
		glColor3fv(color[i]);
		glVertex3fv(m_pPoints[i]);
	}
	delete[] color;
	glEnd();
	glFlush();
	glEnable(GL_LIGHTING);
	glDisable(GL_POLYGON_SMOOTH);
	glHint(GL_POLYGON_SMOOTH_HINT, GL_FASTEST);
}