pointset.h

来自「ＲＢＦ平台」· C头文件 代码 · 共 306 行

#ifndef POINTSET_H 
#define POINTSET_H 

#include <stdio.h>
#include <math.h>


#define PI 3.1415926535897932384626433832795

// Dummy
class ImplicitOctTree;

class PointSet{
	
public:
	float (*_point)[3];
	float (*_normal)[3];
	bool *_bound;
	int _pointN;
	ImplicitOctTree* _tree;

public:
	PointSet(){
		_pointN = 0;
		_point = NULL;
		_normal = NULL;
		_bound = NULL;
	};
	
	~PointSet(){
		if(_point != NULL)
			delete[] _point;
		if(_normal != NULL)
			delete[] _normal;
		if(_bound != NULL)
			delete[] _bound;
	}
	
	void setPointSize(int N){
		_pointN = N;
		_point = new float[N][3];
		_normal = new float[N][3];
		//_bound = new bool[N];
		//for(int i=0; i<N; i++)
		//_bound[i] = false;
	}
	
	inline void swapIndex(int i, int j){

		float tmp = _point[i][0];
		//swap points
		_point[i][0] = _point[j][0];
		_point[j][0] = tmp;
		
		tmp = _point[i][1];
		_point[i][1] = _point[j][1];
		_point[j][1] = tmp;
		
		tmp = _point[i][2];
		_point[i][2] = _point[j][2];
		_point[j][2] = tmp;
		
		//swap normal
		tmp = _normal[i][0];
		_normal[i][0] = _normal[j][0];
		_normal[j][0] = tmp;
		
		tmp = _normal[i][1];
		_normal[i][1] = _normal[j][1];
		_normal[j][1] = tmp;
		
		tmp = _normal[i][2];
		_normal[i][2] = _normal[j][2];
		_normal[j][2] = tmp;
		
	}
	
	void setPoint(int i, float x, float y, float z){
		_point[i][0] = x;
		_point[i][1] = y;
		_point[i][2] = z;
	}
	
	void setNormal(int i, float x, float y, float z){
		_normal[i][0] = x;
		_normal[i][1] = y;
		_normal[i][2] = z;
	}
	
	//Index "end" is not taken int account
	void centroid(float c[3], int start, int end){
		c[0] = c[1] = c[2] = 0;
		for(int i=start; i<end; i++){
			c[0] += _point[i][0];
			c[1] += _point[i][1];
			c[2] += _point[i][2];
		}
		c[0] /= (end-start);
		c[1] /= (end-start);
		c[2] /= (end-start);
	}
	
	//Index "end" is not taken int account
	void averagedNormal(float n[3], int start, int end){
		n[0] = n[1] = n[2] = 0;
		for(int i=start; i<end; i++){
			n[0] += _normal[i][0];
			n[1] += _normal[i][1];
			n[2] += _normal[i][2];
		}
		double len = sqrt(n[0]*n[0] + n[1]*n[1] + n[2]*n[2]);
		if((float)len != 0){
			n[0] = (float)(n[0]/len);
			n[1] = (float)(n[1]/len);
			n[2] = (float)(n[2]/len);
		}
	}
	
	void bound(float min[], float max[]){
		min[0] = min[1] = min[2] = 10000000000;
		max[0] = max[1] = max[2] = -10000000000;
		for(int i=0; i<_pointN; i++){
			float *p = _point[i];
			if(p[0] < min[0])
				min[0] = p[0];
			if(p[0] > max[0])
				max[0] = p[0];
			
			if(p[1] < min[1])
				min[1] = p[1];
			if(p[1] > max[1])
				max[1] = p[1];
			
			if(p[2] < min[2])
				min[2] = p[2];
			if(p[2] > max[2])
				max[2] = p[2];
		}
	}
	
	void bound(float min[], float max[], int start, int end){
		min[0] = min[1] = min[2] = 10000000000;
		max[0] = max[1] = max[2] = -10000000000;
		for(int i=start; i<end; i++){
			float *p = _point[i];
			if(p[0] < min[0])
				min[0] = p[0];
			if(p[0] > max[0])
				max[0] = p[0];
			
			if(p[1] < min[1])
				min[1] = p[1];
			if(p[1] > max[1])
				max[1] = p[1];
			
			if(p[2] < min[2])
				min[2] = p[2];
			if(p[2] > max[2])
				max[2] = p[2];
		}
	}
	
	void shift(float sx, float sy, float sz){
		for(int i=0; i<_pointN; i++){
			_point[i][0] += sx;
			_point[i][1] += sy;
			_point[i][2] += sz;
		}
	}
	
	void scale(float s){
		for(int i=0; i<_pointN; i++){
			_point[i][0] *= s;
			_point[i][1] *= s;
			_point[i][2] *= s;
		}
	}
	
	void 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<_pointN; i++){
				float y = _point[i][1];
				float z = _point[i][2];
				
				_point[i][1] = (float)(c*y + s*z);
				_point[i][2] = (float)(-s*y + c*z);
				
				y = _normal[i][1];
				z = _normal[i][2];
				
				_normal[i][1] = (float)(c*y + s*z);
				_normal[i][2] = (float)(-s*y + c*z);
			}
		}
		if(ry != 0){
			double s = sin(ry);
			double c = cos(ry);
			for(int i=0; i<_pointN; i++){
				float z = _point[i][2];
				float x = _point[i][0];
				
				_point[i][2] = (float)(c*z + s*x);
				_point[i][0] = (float)(-s*z + c*x);
				
				z = _normal[i][2];
				x = _normal[i][0];
				
				_normal[i][2] = (float)(c*z + s*x);
				_normal[i][0] = (float)(-s*z + c*x);
			}
		}
		if(rz != 0){
			double s = sin(rz);
			double c = cos(rz);
			for(int i=0; i<_pointN; i++){
				float x = _point[i][0];
				float y = _point[i][1];
				
				_point[i][0] = (float)(c*x + s*y);
				_point[i][1] = (float)(-s*x + c*y);
				
				x = _normal[i][0];
				y = _normal[i][1];
				
				_normal[i][0] = (float)(c*x + s*y);
				_normal[i][1] = (float)(-s*x + c*y);
			}
		}
	}
	
	PointSet* copyPoints(){
		PointSet* c = new PointSet();
		c->setPointSize(_pointN);
		for(int i=0; i<_pointN; i++){
			c->setPoint(i, _point[i][0], _point[i][1], _point[i][2]);
			c->setNormal(i, _normal[i][0], _normal[i][1], _normal[i][2]);
		}
		return c;
	}

	//quick sort the pointset along the longest axis from start to end point
	bool QuickSort(unsigned char _l_axis, int start, int end, bool bAscending = true)
	{
		int i,j;
		
		float midpoint;

		if( end == _pointN)
			i = end - 1;
		else
			i = end;

		j = start;

		midpoint = _point[((int) ((start+end) / 2))][_l_axis];

		do {

			if (bAscending) { 
				while (_point[j][_l_axis] < midpoint) j++;
				while (_point[i][_l_axis] > midpoint) i--;
			} else {
				while (_point[j][_l_axis] > midpoint) j++;
				while (_point[i][_l_axis] < midpoint) i--;
			}

			if ( i >= j ) {
				if ( i != j ) {

					swapIndex(i,j);
					
				}

				i--;
				j++;
			}
		} while (j <= i);


		if (start < i) QuickSort(_l_axis, start,i, bAscending);
		if (j < end)   QuickSort(_l_axis, j,end, bAscending);

		return true;
	}

	void Offsurfpoint(float (*offsurfpnt)[3], int start, int end, float k = 0.01)
	{
		
		ASSERT(end - start);
				
		for(int i = start; i < end; i++)
		{
			offsurfpnt[i - start][0] = _point[i][0] + k * _normal[i][0];
			offsurfpnt[i - start][1] = _point[i][1] + k * _normal[i][1];
			offsurfpnt[i - start][2] = _point[i][2] + k * _normal[i][2];
		}
	}

};

#endif