Plane.cpp

#include "Plane.h"
//#include "pca.h"
#include <MiscLib/Performance.h>
#include "LevMarFitting.h"
#include <GfxTL/VectorXD.h>
#include <GfxTL/IndexedIterator.h>
#include <GfxTL/Plane.h>
#include <GfxTL/Mean.h>

Plane::Plane(Vec3f p1, Vec3f p2, Vec3f p3)
{
   m_normal= (p2-p1).cross(p3-p2);
   m_normal.normalize();		
   m_pos = p1;
   m_dist = m_pos.dot(m_normal);
}

Plane::Plane(Vec3f p1, Vec3f normal)
{
   m_normal = normal;
   m_pos = p1;
   m_dist = m_pos.dot(m_normal);
}

Plane::~Plane()
{
}

bool Plane::Init(Vec3f p1, Vec3f p2, Vec3f p3)
{
	m_normal= (p2-p1).cross(p3-p2);
	if(m_normal.sqrLength() < 1E-6f)
		return false;
	m_normal.normalize();		
	m_pos = p1;
	m_dist = m_pos.dot(m_normal);
	return true;
}

bool Plane::Init(const MiscLib::Vector< Vec3f > &samples)
{
	if(samples.size() < 6)
		return false;
	return Init(samples[0], samples[1], samples[2]);
}

bool Plane::InitAverage(const MiscLib::Vector< Vec3f > &samples)
{
	if(samples.size() < 1)
		return false;
	m_normal = Vec3f(0, 0, 0);
	m_pos = Vec3f(0, 0, 0);
	size_t c = samples.size() / 2;
	MiscLib::Vector< GfxTL::Vector3Df > normals(c);
	for(intptr_t i = 0; i < c; ++i)
		normals[i] = GfxTL::Vector3Df(samples[i + c]);
	GfxTL::Vector3Df meanNormal;
	GfxTL::MeanOfNormals(normals.begin(), normals.end(), &meanNormal);
	m_normal = Vec3f(meanNormal.Data());
	GfxTL::Vector3Df mean;
	GfxTL::Mean(samples.begin(), samples.begin() + c, &mean);
	m_pos = Vec3f(mean.Data());
	m_dist = m_pos.dot(m_normal);
	return true;
}

bool Plane::Init(bool binary, std::istream *i)
{
	if(binary)
	{
		i->read((char *)&m_normal, sizeof(m_normal));
		i->read((char *)&m_dist, sizeof(m_dist));
		i->read((char *)&m_pos, sizeof(m_pos));
	}
	else
	{
		for(size_t j = 0; j < 3; ++j)
			(*i) >> m_normal[j];
		(*i) >> m_dist;
		for(size_t j = 0; j < 3; ++j)
			(*i) >> m_pos[j];
	}
	return true;
}

void Plane::Init(float* array){
	for(int i=0;i<3;i++){
		m_normal[i] = array[i];
		m_pos[i] = array[i+4];
	}
	m_dist=array[3];
}

void Plane::Init(FILE *i)
{
	fread(&m_normal, sizeof(m_normal), 1, i);
	fread(&m_dist, sizeof(m_dist), 1, i);
	fread(&m_pos, sizeof(m_pos), 1, i);
}

bool Plane::equals (Plane other)
{
   return ( (other.getNormal().dot (getNormal() ) > 0.90) &&
        (getDistance (other.getPosition()) < 0.2) );   
}

template< class WeightT >
class LevMarPlane
: public WeightT
{
public:
	enum { NumParams = 4 };
	typedef float ScalarType;

	LevMarPlane()
	{}
	// parametrization:
	// params[0] - params[2] = normal vector
	// params[3] = dist to origin

	template< class IteratorT >
	ScalarType Chi(const ScalarType *params, IteratorT begin, IteratorT end,
		ScalarType *values, ScalarType *temp) const
	{
		ScalarType chi = 0;
		int size = end - begin;
#pragma omp parallel for schedule(static) reduction(+:chi)
		for(int idx = 0; idx < size; ++idx)
		{
			temp[idx] = params[0] * begin[idx][0] + params[1] * begin[idx][1]
				+ params[2] * begin[idx][2] - params[3];
			chi += (values[idx] = WeightT::Weigh(temp[idx]))
				* values[idx];
		}
		return chi;
	}

	template< class IteratorT >
	void Derivatives(const ScalarType *params, IteratorT begin, IteratorT end,
		const ScalarType *values, const ScalarType *temp, ScalarType *matrix) const
	{
		int size = end - begin;
#pragma omp parallel for schedule(static)
		for(int idx = 0; idx < size; ++idx)
		{
			matrix[idx * NumParams + 0] = begin[idx][0];
			matrix[idx * NumParams + 1] = begin[idx][1];
			matrix[idx * NumParams + 2] = begin[idx][2];
			matrix[idx * NumParams + 3] = -1;
			WeightT::template DerivWeigh< NumParams >(temp[idx],
				matrix + idx * NumParams);
		}
	}

	void Normalize(ScalarType *params) const
	{
		ScalarType len = std::sqrt(params[0] * params[0]
			+ params[1] * params[1] + params[2] * params[2]);
		params[0] /= len;
		params[1] /= len;
		params[2] /= len;
	}
};

bool Plane::LeastSquaresFit(const PointCloud &pc,
	MiscLib::Vector< size_t >::const_iterator begin,
	MiscLib::Vector< size_t >::const_iterator end)
{
	LeastSquaresFit(GfxTL::IndexIterate(begin, pc.begin()),
		GfxTL::IndexIterate(end, pc.begin()));
	return true;
}

bool Plane::Interpolate(const MiscLib::Vector< Plane > &planes,
	const MiscLib::Vector< float > &weights, Plane *ip)
{
	Vec3f normal(0, 0, 0);
	Vec3f position(0, 0, 0);
	for(size_t i = 0; i < planes.size(); ++i)
	{
		normal += weights[i] * planes[i].getNormal();
		position += weights[i] * planes[i].getPosition();
	}
	normal.normalize();
	*ip = Plane(position, normal);
	return true;
}

void Plane::Serialize(bool binary, std::ostream *o) const
{
	if(binary)
	{
		o->write((const char *)&m_normal, sizeof(m_normal));
		o->write((const char *)&m_dist, sizeof(m_dist));
		o->write((const char *)&m_pos, sizeof(m_pos));
	}
	else
#ifdef DIRK_FORMAT
	{
		(*o) << std::endl 
			<< "n " << m_normal[0] << " " << m_normal[1] << " " << m_normal[2] << std::endl 
			<< "d " << m_dist << std::endl
			<< "p ";
		for(size_t i = 0; i < 3; ++i)
			(*o) << m_pos[i] << " ";
		(*o) << std::endl;
	}
#else
	{
		(*o) << m_normal[0] << " " << m_normal[1] << " " << m_normal[2] << " "
			<< m_dist << " ";
		for(size_t i = 0; i < 3; ++i)
			(*o) << m_pos[i] << " ";
	}
#endif
}

size_t Plane::SerializedSize()
{
	return sizeof(Vec3f)
		+ sizeof(float)
		+ sizeof(Vec3f);
}

size_t Plane::SerializedFloatSize()
{
	return 7;
}

void Plane::Serialize(float* array) const
{
	for(int i=0;i<3;i++){
		array[i] = m_normal[i];
		array[i+4] = m_pos[i];
	}
	array[3] = m_dist;
}


void Plane::Serialize(FILE *o) const
{
	fwrite(&m_normal, sizeof(m_normal), 1, o);
	fwrite(&m_dist, sizeof(m_dist), 1, o);
	fwrite(&m_pos, sizeof(m_pos), 1, o);
}

void Plane::Transform(float scale, const Vec3f &translate)
{
	m_pos *= scale;
	m_pos += translate;
	m_dist = m_pos.dot(m_normal);
}

float Plane::Intersect(const Vec3f &p, const Vec3f &r) const
{
	return -SignedDistance(p) / (m_normal.dot(r));
}