test6-teapot-in-stadium-problem-library-solution.txt

#include"FastCollisionDetectionLib.h"

#include<iostream>

template<typename CoordType>
struct Vector3D
{
	CoordType x,y,z;
	Vector3D<CoordType> crossProduct(Vector3D<CoordType> vec)
	{
		Vector3D<CoordType> res;
		res.x = y*vec.z - z*vec.y;
		res.y = z*vec.x - x*vec.z;
		res.z = x*vec.y - y*vec.x;
		return res;
	}

	Vector3D<CoordType> operator - (Vector3D<CoordType> vec)
	{
		Vector3D<CoordType> result;
		result.x = x-vec.x;
		result.y = y-vec.y;
		result.z = z-vec.z;
		return result;
	}

	Vector3D<CoordType> operator + (Vector3D<CoordType> vec)
	{
		Vector3D<CoordType> result;
		result.x = x+vec.x;
		result.y = y+vec.y;
		result.z = z+vec.z;
		return result;
	}

	Vector3D<CoordType> operator * (CoordType v)
	{
		Vector3D<CoordType> result;
		result.x = x*v;
		result.y = y*v;
		result.z = z*v;
		return result;
	}

	CoordType abs()
	{
		return std::sqrt(x*x+y*y+z*z);
	}

};


template<typename CoordType>
struct PointCloud
{
	Vector3D<CoordType> point[125];
	PointCloud(CoordType x, CoordType y, CoordType z)
	{
		for(int i=0;i<125;i++)
		{
			point[i].x=x+i%5-2.5f;
			point[i].y=y+(i/5)%5-2.5f;
			point[i].z=z+i/25-2.5f;
		}
	}
};

template<typename CoordType>
bool pointCloudIntersection(PointCloud<CoordType>& cl1, PointCloud<CoordType>& cl2)
{
	for(Vector3D<CoordType>& p:cl1.point)
	{
		for(Vector3D<CoordType>& p2:cl2.point)
		{
			if((p-p2).abs()<1.0f)
			{
				return true;
			}
		}
	}
	return false;
}

template<typename CoordType>
bool intersectDim(const CoordType minx, const CoordType maxx, const CoordType minx2, const CoordType maxx2)
{
	return !((maxx < minx2) || (maxx2 < minx));
}
#include"Generator.h"

template<typename CoordType>
struct AABBofPointCloud: public FastColDetLib::IParticle<CoordType>
{
	AABBofPointCloud(int idPrm, PointCloud<CoordType> * pCloudPrm)
	{
		id=idPrm;
		pCloud = pCloudPrm;
		xmin=pCloud->point[0].x;
		ymin=pCloud->point[0].y;
		zmin=pCloud->point[0].z;
		xmax=pCloud->point[0].x;
		ymax=pCloud->point[0].y;
		zmax=pCloud->point[0].z;
		for(int i=0;i<125;i++)
		{
			if(xmin>pCloud->point[i].x)
				xmin=pCloud->point[i].x;
			if(ymin>pCloud->point[i].y)
				ymin=pCloud->point[i].y;
			if(zmin>pCloud->point[i].z)
				zmin=pCloud->point[i].z;
			if(xmax<pCloud->point[i].x)
				xmax=pCloud->point[i].x;
			if(ymax<pCloud->point[i].y)
				ymax=pCloud->point[i].y;
			if(zmax<pCloud->point[i].z)
				zmax=pCloud->point[i].z;
		}
	}
	int id;
	PointCloud<CoordType>* pCloud;
	CoordType xmin;
	CoordType ymin;
	CoordType zmin;
	CoordType xmax;
	CoordType ymax;
	CoordType zmax;
	const CoordType getMaxX()const {return xmax;}
	const CoordType getMaxY()const {return ymax;}
	const CoordType getMaxZ()const {return zmax;}
	const CoordType getMinX()const {return xmin;}
	const CoordType getMinY()const {return ymin;}
	const CoordType getMinZ()const {return zmin;}
	const int getId()const {return id;}
};


int main()
{

	using cotype = float;
	PointCloud<cotype> ico1(0,0,0);
	// heating the CPU for benchmarking

	for(int i=0;i<10000;i++)
	{
		PointCloud<cotype> ico2(0,0.1f,i*0.1f);
		pointCloudIntersection(ico1,ico2);
	}

	const int N = 10003;
	std::vector<PointCloud<cotype>> objects;
	oofrng::Generator<64> gen;
	for(int i=0;i<N-3;i++)
	{
		objects.push_back(PointCloud<cotype>(gen.generate1Float()*450,gen.generate1Float()*450,gen.generate1Float()*450));
	}

	// the teapot in stadium problem
	objects.push_back(PointCloud<cotype>(10000,10000,10000));
	objects.push_back(PointCloud<cotype>(10001,10001,10001));
	objects.push_back(PointCloud<cotype>(10002,10002,10002));


	std::vector<AABBofPointCloud<cotype>> AABBs;
	for(int i=0;i<N;i++)
	{
		AABBs.push_back(AABBofPointCloud<cotype>(i,&objects[i]));
	}


	// benchmark begin
	for(int j=0;j<5;j++)
	{
		size_t nano;

		std::map<int,std::map<int,bool>> collisionMatrix;
		{
			FastColDetLib::Bench bench(&nano);

			// adaptive grid
			FastColDetLib::ThreadPool<cotype> thr;
			FastColDetLib::AdaptiveGrid<cotype> grid(thr,0,0,0,10005,10005,10005);

			// add AABBs to grid
			for(int i=0;i<N;i++)
			{
				grid.add(&AABBs[i],1);
			}

			std::mutex mut;

			#pragma omp parallel for
			for(int i=0;i<N;i++)
			{
				 std::vector<FastColDetLib::IParticle<cotype>*> result = grid.getDynamicCollisionListFor((FastColDetLib::IParticle<cotype>*)&AABBs[i]);
					for(auto c:result)
					{
						if(c->getId() != AABBs[i].getId())
						if(pointCloudIntersection(*AABBs[c->getId()].pCloud,*AABBs[i].pCloud))
						{
							std::lock_guard<std::mutex> lg(mut);
							collisionMatrix[c->getId()][AABBs[i].id]=true;
							collisionMatrix[AABBs[i].id][c->getId()]=true;
						}
					}
			}


		}
		std::cout<<N<<" vs "<<N<<" point-clouds collision checking by uniform grid= "<<nano<<" nanoseconds"<<std::endl;
		int total = 0;
		for(auto c:collisionMatrix)
		{
			for(auto c2:c.second)
			{
				if(c2.second)
				total++;
			}
		}
		std::cout<<total<<" total collisions (half as many for pairs)"<<std::endl;
	}
	return 0;

}