TorusPrimitiveShape.cpp

#include "TorusPrimitiveShape.h"
#include "PrimitiveShapeVisitor.h"
#include <MiscLib/Performance.h>
#include <sstream>
#include "ConePrimitiveShape.h"
#include "CylinderPrimitiveShape.h"
#include "SpherePrimitiveShape.h"
#include "PlanePrimitiveShape.h"
extern MiscLib::performance_t totalTime_torusConnected;

TorusPrimitiveShape::TorusPrimitiveShape(const Torus &torus)
: m_torus(torus)
, m_parametrization(m_torus)
{}

TorusPrimitiveShape::TorusPrimitiveShape(const TorusPrimitiveShape &tps)
: BitmapPrimitiveShape(tps)
, m_torus(tps.m_torus)
, m_parametrization(tps.m_parametrization)
{
	m_parametrization.Shape(m_torus);
}

size_t TorusPrimitiveShape::Identifier() const
{
	return 4;
}

PrimitiveShape *TorusPrimitiveShape::Clone() const
{
	return new TorusPrimitiveShape(*this);
}

float TorusPrimitiveShape::Distance(const Vec3f &p) const
{
	return m_torus.Distance(p);
}

float TorusPrimitiveShape::SignedDistance(const Vec3f &p) const
{
	return m_torus.SignedDistance(p);
}

float TorusPrimitiveShape::NormalDeviation(const Vec3f &p,
	const Vec3f &n) const
{
	Vec3f normal;
	m_torus.Normal(p, &normal);
	return n.dot(normal);
}

void TorusPrimitiveShape::DistanceAndNormalDeviation(const Vec3f &p,
	const Vec3f &n, std::pair< float, float > *dn) const
{
	Vec3f normal;
	dn->first = m_torus.DistanceAndNormal(p, &normal);
	dn->second = n.dot(normal);
}

void TorusPrimitiveShape::Project(const Vec3f &p, Vec3f *pp) const
{
	m_torus.Project(p, pp);
}

void TorusPrimitiveShape::Normal(const Vec3f &p, Vec3f *n) const
{
	m_torus.Normal(p, n);
}

unsigned int TorusPrimitiveShape::ConfidenceTests(unsigned int numTests,
	float epsilon, float normalThresh, float rms, const PointCloud &pc,
	const MiscLib::Vector< size_t > &indices) const
{
	return BasePrimitiveShape::ConfidenceTests< Torus >(numTests, epsilon,
		normalThresh, rms, pc, indices);
}

void TorusPrimitiveShape::Description(std::string *s) const
{
	std::ostringstream ostr;
	ostr << "Torus (minor=" << m_torus.MinorRadius()
		<< " major=" << m_torus.MajorRadius() << ")";
	*s = ostr.str();
//	*s = "Torus";
}

bool TorusPrimitiveShape::Fit(const PointCloud &pc, float epsilon,
	float normalThresh,
	MiscLib::Vector< size_t >::const_iterator begin,
	MiscLib::Vector< size_t >::const_iterator end)
{
	Torus fit = m_torus;
	if(fit.LeastSquaresFit(pc, begin, end))
	{
		m_torus = fit;
		m_parametrization.Shape(m_torus);
		return true;
	}
	return false;
}

PrimitiveShape *TorusPrimitiveShape::LSFit(const PointCloud &pc, float epsilon,
	float normalThresh, MiscLib::Vector< size_t >::const_iterator begin,
	MiscLib::Vector< size_t >::const_iterator end,
	std::pair< size_t, float > *score) const
{
	Torus fit = m_torus;
	if(fit.LeastSquaresFit(pc, begin, end))
	{
		score->first = -1;
		return new TorusPrimitiveShape(fit);
	}
	score->first = 0;
	return NULL;
}

LevMarFunc< float > *TorusPrimitiveShape::SignedDistanceFunc() const
{
	return new TorusLevMarFunc(m_torus);
}

void TorusPrimitiveShape::Serialize(std::ostream *o, bool binary) const
{
	if(binary)
	{
		const char id = 4;
		(*o) << id;
	}
	else
		(*o) << "4" << " ";
	m_torus.Serialize(binary, o);
	m_parametrization.Serialize(o, binary);
	if(!binary)
		*o << std::endl;
}

void TorusPrimitiveShape::Deserialize(std::istream *i, bool binary)
{
	m_torus.Init(binary, i);
	m_parametrization.Shape(m_torus);
	m_parametrization.Deserialize(i, binary);
}

size_t TorusPrimitiveShape::SerializedSize() const
{
	return m_torus.SerializedSize() + m_parametrization.SerializedSize() + 1;
}

void TorusPrimitiveShape::Transform(float scale, const Vec3f &translate)
{
	m_torus.Transform(scale, translate);
}

void TorusPrimitiveShape::Visit(PrimitiveShapeVisitor *visitor) const
{
	visitor->Visit(*this);
}

void TorusPrimitiveShape::SuggestSimplifications(const PointCloud &pc,
	MiscLib::Vector< size_t >::const_iterator begin,
	MiscLib::Vector< size_t >::const_iterator end, float distThresh,
	MiscLib::Vector< MiscLib::RefCountPtr< PrimitiveShape > > *suggestions) const
{
	// sample the bounding box in parameter space at 25 locations
	// these points are used to estimate the other shapes
	// if the shapes succeed the suggestion is returned
	MiscLib::Vector< Vec3f > samples(2 * 25);
	float uStep = (m_extBbox.Max()[0] - m_extBbox.Min()[0]) / 4;
	float vStep = (m_extBbox.Max()[1] - m_extBbox.Min()[1]) / 4;
	float u = m_extBbox.Min()[0];
	for(unsigned int i = 0; i < 5; ++i, u += uStep)
	{
		float v = m_extBbox.Min()[1];
		for(unsigned int j = 0; j < 5; ++j, v += vStep)
		{
			float bmpu;
			if(m_torus.MajorRadius() < m_torus.MinorRadius() * 2)
				bmpu = u * (m_torus.MajorRadius() + m_torus.MinorRadius());
			else
				bmpu = u * m_torus.MajorRadius();
			InSpace(bmpu, v * m_torus.MinorRadius(), &samples[i * 5 + j],
				&samples[i * 5 + j + 25]);
		}
	}
	size_t c = samples.size() / 2;
	// now check all the shape types
	Cone cone;
	if(cone.InitAverage(samples))
	{
		cone.LeastSquaresFit(samples.begin(), samples.begin() + c);
		bool failed = false;
		for(size_t i = 0; i < c; ++i)
			if(cone.Distance(samples[i]) > distThresh)
			{
				failed = true;
				break;
			}
		if(!failed)
		{
			suggestions->push_back(new ConePrimitiveShape(cone));
			suggestions->back()->Release();
		}
	}
	Cylinder cylinder;
	if(cylinder.InitAverage(samples))
	{
		cylinder.LeastSquaresFit(samples.begin(), samples.begin() + c);
		bool failed = false;
		for(size_t i = 0; i < c; ++i)
			if(cylinder.Distance(samples[i]) > distThresh)
			{
				failed = true;
				break;
			}
		if(!failed)
		{
			suggestions->push_back(new CylinderPrimitiveShape(cylinder));
			suggestions->back()->Release();
		}
	}
	Sphere sphere;
	if(sphere.Init(samples))
	{
		sphere.LeastSquaresFit(samples.begin(), samples.begin() + c);
		bool failed = false;
		for(size_t i = 0; i < c; ++i)
			if(sphere.Distance(samples[i]) > distThresh)
			{
				failed = true;
				break;
			}
		if(!failed)
		{
			suggestions->push_back(new SpherePrimitiveShape(sphere));
			suggestions->back()->Release();
		}
	}
	Plane plane;
	if(plane.LeastSquaresFit(samples.begin(), samples.begin() + c))
	{
		bool failed = false;
		for(size_t i = 0; i < c; ++i)
			if(plane.Distance(samples[i]) > distThresh)
			{
				failed = true;
				break;
			}
		if(!failed)
		{
			suggestions->push_back(new PlanePrimitiveShape(plane));
			suggestions->back()->Release();
		}
	}
	/*// although theoretically possible, we never suggest a cone since a misclassification
	// of a cone as a torus is extremley seldom
	// The parametrization is given as major arclength and minor arclength
	float radialMajor = m_extBbox.Max()[0] - m_extBbox.Min()[0];
	float radialMinor = m_extBbox.Max()[1] - m_extBbox.Min()[1];
	float lengthMajor = radialMajor * m_torus.MajorRadius();
	float lengthMinor = radialMinor * m_torus.MinorRadius();
	float meanRadius = (m_torus.MajorRadius() + m_torus.MinorRadius()) / 2;
	// suggest a cylinder if either of the two radii can be replaced by
	// a non curved direction
	// we suggest a cylinder if the major radius causes an error less than distThresh
	// this tests if the major radius can be replaced
	float radiusDiffMajor = (m_torus.MajorRadius() - (std::cos(radialMajor / 2)
		* m_torus.MajorRadius())) / 2;
	if(radiusDiffMajor < distThresh)
	{
		// construct the cylinder
		// the axis of the cylinder is given
		float majorCenter = (m_extBbox.Max()[0] - m_extBbox.Min()[0]) / 2
			* m_torus.MajorRadius();
		Vec3f pos, normal, cyAxisDir;
		InSpace(majorCenter, M_PI * m_torus.MinorRadius(), &pos, &normal);
		cyAxisDir = normal.cross(m_torus.AxisDirection());
		cyAxisDir.normalize();
		pos -= (m_torus.MinorRadius() - radiusDiffMajor) * normal;
		Cylinder cylinder(cyAxisDir, pos, m_torus.MinorRadius());
		suggestions->push_back(new CylinderPrimitiveShape(cylinder));
		suggestions->back()->Release();
	}
	// now test if the minor radius can be replaced
	float radiusDiffMinor = (m_torus.MinorRadius() - (std::cos(radialMinor / 2)
		* m_torus.MinorRadius())) / 2;
	if(radiusDiffMinor < distThresh)
	{
		// if the minor radius is replaced 
	}
	// we suggest a sphere if the torus is apple shaped
	if(m_torus.IsAppleShaped())
	{
		Sphere sphere(m_torus.Center(),
			(m_torus.MajorRadius() + m_torus.MinorRadius()) / 2);
		suggestions->push_back(new SpherePrimitiveShape(sphere));
		suggestions->back()->Release();
	}
	// we can also suggest a sphere if the error introduced by a common radius
	// for minor and major does not introduce an error
	//else if()
	//{
	//}
	// we suggest a plane if both major and minor radius cause ony small error
	float radiusDiffMinor = (m_torus.MinorRadius() - (std::cos(radialMinor / 2)
		* m_torus.MinorRadius())) / 2;
	if(radiusDiffMajor < distThresh && radiusDiffMinor < distThresh)
	{
		GfxTL::Vector2Df paramCenter;
		m_extBbox.Center(&paramCenter);
		Vec3f pos, normal;
		InSpace(paramCenter[0] * m_torus.MajorRadius(),
			paramCenter[1] * m_torus.MinorRadius(), &pos, &normal);
		Plane plane(pos, normal);
		suggestions->push_back(new PlanePrimitiveShape(plane));
		suggestions->back()->Release();
	}*/
}

void TorusPrimitiveShape::OptimizeParametrization(const PointCloud &pc,
	size_t begin, size_t end, float epsilon)
{
	m_parametrization.Optimize(GfxTL::IndexIterate(IndexIterator(begin), pc.begin()),
		GfxTL::IndexIterate(IndexIterator(end), pc.begin()), epsilon);
}

bool TorusPrimitiveShape::Similar(float tolerance,
	const TorusPrimitiveShape &shape) const
{
	return m_torus.MajorRadius() <= (1.f + tolerance) * shape.m_torus.MajorRadius()
		&& (1.f + tolerance) * m_torus.MajorRadius() >= shape.m_torus.MajorRadius()
		&& m_torus.MinorRadius() <= (1.f + tolerance) * shape.m_torus.MinorRadius()
		&& (1.f + tolerance) * m_torus.MinorRadius() >= shape.m_torus.MinorRadius();
}

void TorusPrimitiveShape::Parameters(const Vec3f &p,
	std::pair< float, float > *param) const
{
	m_parametrization.Parameters(p, param);
}

void TorusPrimitiveShape::Parameters(
	GfxTL::IndexedIterator< MiscLib::Vector< size_t >::iterator,
		PointCloud::const_iterator > begin,
	GfxTL::IndexedIterator< MiscLib::Vector< size_t >::iterator,
		PointCloud::const_iterator > end,
	MiscLib::Vector< std::pair< float, float > > *bmpParams) const
{
	ParametersImpl(begin, end, bmpParams);
}

void TorusPrimitiveShape::Parameters(
	GfxTL::IndexedIterator< IndexIterator,
		PointCloud::const_iterator > begin,
	GfxTL::IndexedIterator< IndexIterator,
		PointCloud::const_iterator > end,
	MiscLib::Vector< std::pair< float, float > > *bmpParams) const
{
	ParametersImpl(begin, end, bmpParams);
}

bool TorusPrimitiveShape::InSpace(float u, float v, Vec3f *p, Vec3f *n) const
{
	return m_parametrization.InSpace(u, v, p, n);
}

void TorusPrimitiveShape::BitmapExtent(float epsilon,
	GfxTL::AABox< GfxTL::Vector2Df > *bbox,
	MiscLib::Vector< std::pair< float, float > > *params,
	size_t *uextent, size_t *vextent)
{
	*uextent = std::ceil((bbox->Max()[0] - bbox->Min()[0]) / epsilon);
	*vextent = std::ceil((bbox->Max()[1] - bbox->Min()[1]) / epsilon);
}

void TorusPrimitiveShape::InBitmap(const std::pair< float, float > &param,
	float epsilon, const GfxTL::AABox< GfxTL::Vector2Df > &bbox,
	size_t uextent, size_t vextent,
	std::pair< int, int > *inBmp) const
{
	inBmp->first = std::floor((param.first - bbox.Min()[0]) / epsilon);
	inBmp->second = std::floor((param.second - bbox.Min()[1]) / epsilon);
}

void TorusPrimitiveShape::WrapBitmap(
	const GfxTL::AABox< GfxTL::Vector2Df > &bbox,
	float epsilon, bool *uwrap, bool *vwrap) const
{
	m_parametrization.WrapBitmap(bbox, epsilon, uwrap, vwrap);
}

void TorusPrimitiveShape::WrapComponents(const GfxTL::AABox< GfxTL::Vector2Df > &bbox,
	float epsilon, size_t uextent, size_t vextent,
	MiscLib::Vector< int > *componentImg,
	MiscLib::Vector< std::pair< int, size_t > > *labels) const
{
	m_parametrization.WrapComponents(bbox, epsilon, uextent, vextent,
		componentImg, labels);
}

void TorusPrimitiveShape::SetExtent(
	const GfxTL::AABox< GfxTL::Vector2Df > &bbox,
	const MiscLib::Vector< int > &componentsImg, size_t uextent,
	size_t vextent, float epsilon, int label)
{}

bool TorusPrimitiveShape::InSpace(size_t u, size_t v, float epsilon,
	const GfxTL::AABox< GfxTL::Vector2Df > &bbox, size_t uextent,
	size_t vextent, Vec3f *p, Vec3f *n) const
{
	return m_parametrization.InSpace((u + .5f) * epsilon + bbox.Min()[0],
		(v + .5f) * epsilon + bbox.Min()[1], p, n);
}