C++ misclib::Vector类代码示例

void Components(const MiscLib::Vector< char > &bitmap,
	size_t uextent, size_t vextent, bool uwrap, bool vwrap,
	MiscLib::Vector< int > *componentsImg,
	MiscLib::Vector< std::pair< int, size_t > > *labels)
{
	componentsImg->resize(uextent * vextent);
	MiscLib::Vector< std::pair< int, size_t > > tempLabels;
	tempLabels.reserve(componentsImg->size() / 2 + 1); // this is the maximum of possible tempLabels
	tempLabels.push_back(std::make_pair(0, size_t(0)));
	// use an eight neighborhood
	// first row is special
	// first pixel is special
	// wrapping does not make sense in the first row: no pixels have been
	// assigned components yet
	int curLabel = 0;
	if(bitmap[0])
	{
		(*componentsImg)[0] = ++curLabel;
		tempLabels.push_back(std::make_pair(curLabel, size_t(1)));
	}
	else
	{
		(*componentsImg)[0] = 0;
		++tempLabels[0].second;
	}
	// handle first row
	for(size_t i = 1; i < uextent; ++i)
	{
		// in the first row only the previous pixel has to be considered
		if(bitmap[i])
		{
			if((*componentsImg)[i - 1])
			{
				(*componentsImg)[i] = (*componentsImg)[i - 1];
				++tempLabels[(*componentsImg)[i]].second;
			}
			else
			{
				(*componentsImg)[i] = ++curLabel;
				tempLabels.push_back(std::make_pair(curLabel, size_t(1)));
			}
		}
		else
		{
			(*componentsImg)[i] = 0;
			++tempLabels[0].second;
		}
	}
	size_t prevRow, row = 0;
	size_t jend = vwrap? vextent - 1 : vextent;
	for(size_t j = 1; j < jend; ++j)
	{
		prevRow = row;
		row = prevRow + uextent;
		// first pixel in row gets different treatment
		if(bitmap[row])
		{
			if((*componentsImg)[prevRow]) // pixel above in component?
			{
				(*componentsImg)[row] = (*componentsImg)[prevRow];
				++tempLabels[(*componentsImg)[row]].second;
			}
			else
			{
				int n[2];
				n[0] = (*componentsImg)[prevRow + 1];
				if(uwrap)
					n[1] = (*componentsImg)[prevRow + uextent - 1];
				(*componentsImg)[row] = Label(n, uwrap? 2 : 1, &curLabel,
					&tempLabels);
			}
		}
		else
		{
			(*componentsImg)[row] = 0;
			++tempLabels[0].second;
		}
		for(size_t i = 1; i < uextent - 1; ++i)
		{
			if(!bitmap[row + i])
			{
				(*componentsImg)[row + i] = 0;
				++tempLabels[0].second;
				continue;
			}
			int n[4];
			n[0] = (*componentsImg)[row + i - 1];
			n[1] = (*componentsImg)[prevRow + i - 1];
			n[2] = (*componentsImg)[prevRow + i];
			n[3] = (*componentsImg)[prevRow + i + 1];
			(*componentsImg)[row + i] = Label(n, 4, &curLabel, &tempLabels);
		}
		// last pixel in the row
		if(!bitmap[row + uextent - 1])
		{
			(*componentsImg)[row + uextent - 1] = 0;
			++tempLabels[0].second;
			continue;
		}
		int n[5];
//.........这里部分代码省略.........

void ErodeCross(const MiscLib::Vector< char > &bitmap,
	size_t uextent, size_t vextent, bool uwrap, bool vwrap,
	MiscLib::Vector< char > *eroded)
{
	// first pixel is special
	(*eroded)[0] = bitmap[0] && bitmap[1] &&
		bitmap[uextent];
	if(vwrap)
		(*eroded)[0] = (*eroded)[0] &&
			bitmap[(vextent - 1) * uextent];
	if(uwrap)
		(*eroded)[0] = (*eroded)[0] && bitmap[uextent - 1];
	// first row is special
	if(!vwrap)
	{
		for(size_t i = 1; i < uextent - 1; ++i)
		{
			(*eroded)[i] = bitmap[i - 1] && bitmap[i] && bitmap[i + 1] &&
				bitmap[uextent + i];
		}
	}
	else
	{
		for(size_t i = 1; i < uextent - 1; ++i)
		{
			(*eroded)[i] = bitmap[i - 1] && bitmap[i] && bitmap[i + 1] &&
				bitmap[uextent + i] && bitmap[(vextent - 1) * uextent + i];
		}
	}
	// last pixel of first row is special
	(*eroded)[uextent - 1] = bitmap[uextent - 1] && bitmap[uextent - 2] &&
		bitmap[2 * uextent - 1];
	if(vwrap)
		(*eroded)[uextent - 1] = (*eroded)[uextent - 1] &&
			bitmap[vextent * uextent - 1];
	if(uwrap)
		(*eroded)[uextent - 1] = (*eroded)[uextent - 1] && bitmap[0];
	size_t row = 0, prevRow, nextRow = uextent;
	for(size_t j = 1; j < vextent - 1; ++j)
	{
		prevRow = row;
		row = nextRow;
		nextRow = row + uextent;
		// first pixel in row is special
		(*eroded)[row] = bitmap[prevRow] &&
			bitmap[row] && bitmap[row + 1] && bitmap[nextRow];
		if(uwrap)
			(*eroded)[row] = (*eroded)[row] && bitmap[nextRow - 1];
		for(size_t i = 1; i < uextent - 1; ++i)
		{
			(*eroded)[row + i] = bitmap[prevRow + i ] &&
				bitmap[row + i - 1] && bitmap[row + i] &&
				bitmap[row + i + 1] && bitmap[nextRow + i];
		}
		// last pixel in row is special
		(*eroded)[row + uextent - 1] = bitmap[prevRow + uextent - 1] &&
			bitmap[row + uextent - 2] && bitmap[row + uextent - 1] &&
			bitmap[nextRow + uextent - 1];
		if(uwrap)
			(*eroded)[row + uextent - 1] = (*eroded)[row + uextent - 1] &&
				bitmap[row];
	}
	// first pixel of last row is special
	(*eroded)[(vextent - 1) * uextent] = bitmap[(vextent - 1) * uextent] &&
		bitmap[(vextent - 1) * uextent + 1] &&
		bitmap[(vextent - 2) * uextent];
	if(vwrap)
		(*eroded)[(vextent - 1) * uextent] =
			(*eroded)[(vextent - 1) * uextent] && bitmap[0];
	if(uwrap)
		(*eroded)[(vextent - 1) * uextent] =
			(*eroded)[(vextent - 1) * uextent] &&
			bitmap[vextent * uextent - 1];
	// last row is special
	if(!vwrap)
	{
		for(size_t i = 1; i < uextent - 1; ++i)
		{
			(*eroded)[(vextent - 1) * uextent + i] =
				bitmap[(vextent - 1) * uextent + i] &&
				bitmap[(vextent - 1) * uextent + i - 1] &&
				bitmap[(vextent - 1) * uextent + i + 1] &&
				bitmap[(vextent - 2) * uextent + i];
		}
	}
	else
	{
		for(size_t i = 1; i < uextent - 1; ++i)
		{
			(*eroded)[(vextent - 1) * uextent + i] =
				bitmap[(vextent - 1) * uextent + i] &&
				bitmap[(vextent - 1) * uextent + i - 1] &&
				bitmap[(vextent - 1) * uextent + i + 1] &&
				bitmap[(vextent - 2) * uextent + i] &&
				bitmap[i];
		}
	}
	// last pixel
	(*eroded)[bitmap.size() - 1] = bitmap[bitmap.size() - 1] &&
		bitmap[bitmap.size() - 2] && bitmap[bitmap.size() - uextent - 1];
//.........这里部分代码省略.........

void PreWrappedComponents(const MiscLib::Vector< char > &bitmap, size_t uextent,
	size_t vextent, MiscLib::Vector< int > *preWrappedComponentsImg,
	MiscLib::Vector< int > *relabelComponentsImg,
	const MiscLib::Vector< std::pair< int, size_t > > &inLabels,
	MiscLib::Vector< std::pair< int, size_t > > *labels)
{
	MiscLib::Vector< std::pair< int, size_t > > tempLabels(inLabels);
	tempLabels.reserve(bitmap.size() / 2 + 1); // this is the maximum of possible tempLabels
	if(!tempLabels.size())
		tempLabels.push_back(std::make_pair(0, size_t(0)));
	int curLabel = tempLabels.size() - 1;
	size_t prevRow, row = 0, nextRow = uextent;
	for(size_t j = 1; j < vextent - 1; ++j)
	{
		prevRow = row;
		row = nextRow;
		nextRow = row + uextent;
		for(size_t i = 1; i < uextent - 1; ++i)
		{
			if(!bitmap[row + i])
			{
				(*preWrappedComponentsImg)[row + i] = 0;
				++tempLabels[0].second;
				continue;
			}
			// get neighborhood
			int n[8];
			n[0] = (*preWrappedComponentsImg)[prevRow + i - 1];
			n[1] = (*preWrappedComponentsImg)[prevRow + i];
			n[2] = (*preWrappedComponentsImg)[prevRow + i + 1];
			n[3] = (*preWrappedComponentsImg)[row + i - 1];
			n[4] = (*preWrappedComponentsImg)[row + i + 1];
			n[5] = (*preWrappedComponentsImg)[nextRow + i - 1];
			n[6] = (*preWrappedComponentsImg)[nextRow + i];
			n[7] = (*preWrappedComponentsImg)[nextRow + i + 1];
			(*preWrappedComponentsImg)[row + i] = Label(n, 8,
				&curLabel, &tempLabels);
		}
	}

	// reduce the tempLabels
	for(size_t i = tempLabels.size() - 1; i > 0; --i)
		tempLabels[i].first = ReduceLabel(i, tempLabels);
	MiscLib::Vector< int > condensed(tempLabels.size());
	labels->clear();
	labels->reserve(condensed.size());
	int count = 0;
    for(size_t i = 0; i < tempLabels.size(); ++i)
		if(i == (size_t)tempLabels[i].first)
		{
			labels->push_back(std::make_pair(count, tempLabels[i].second));
			condensed[i] = count;
			++count;
		}
		else
			(*labels)[condensed[tempLabels[i].first]].second
				+= tempLabels[i].second;
	// set new component ids
	for(size_t i = 0; i < preWrappedComponentsImg->size(); ++i)
		(*preWrappedComponentsImg)[i] =
			condensed[tempLabels[(*preWrappedComponentsImg)[i]].first];
	for(size_t i = 0; i < relabelComponentsImg->size(); ++i)
		(*relabelComponentsImg)[i] =
			condensed[tempLabels[(*relabelComponentsImg)[i]].first];
}

size_t BitmapPrimitiveShape::ConnectedComponent(
	const PointCloud &pc, float epsilon,
	std::shared_ptr<MiscLib::Vector< size_t > >indices, bool doFiltering, float* borderRatio )
{
	MiscLib::Vector< int > componentsImg;
	MiscLib::Vector< std::pair< int, size_t > > labels;

	BitmapInfo bitmapInfo;
	if( AllConnectedComponents( pc, epsilon, bitmapInfo, indices, componentsImg, labels, doFiltering ) <= 1 )
		return 0;

	size_t size = indices->size();
	MiscLib::Vector< size_t >::iterator begin = indices->begin();

	// find the largest component
	size_t maxComp = 1;
	for(size_t i = 2; i < labels.size(); ++i)
		if(labels[maxComp].second < labels[i].second)
			maxComp = i;

	GfxTL::AABox< GfxTL::Vector2Df > bbox;
	bbox.Min() = GfxTL::Vector2Df(
			std::numeric_limits< float >::infinity(),
			std::numeric_limits< float >::infinity());
	bbox.Max() = -bbox.Min();
	// compute bbox and update indices
	size_t offset = 0;
	for(size_t i = 0; i < size; ++i)
	{
		if(componentsImg[bitmapInfo.bmpIdx[i]] == labels[maxComp].first)
		{
			std::swap(begin[offset], begin[i]);
			offset++;
			// update bounding box
			if(bbox.Min()[0] > bitmapInfo.params[i].first)
				bbox.Min()[0] = bitmapInfo.params[i].first;
			if(bbox.Max()[0] < bitmapInfo.params[i].first)
				bbox.Max()[0] = bitmapInfo.params[i].first;
			if(bbox.Min()[1] > bitmapInfo.params[i].second)
				bbox.Min()[1] = bitmapInfo.params[i].second;
			if(bbox.Max()[1] < bitmapInfo.params[i].second)
				bbox.Max()[1] = bitmapInfo.params[i].second;
		}
	}

	// ratio between border and connected-comp size should be calculated if borderRatio is a valid pointer
	if( borderRatio )
	{
		int borderPixels = 0;
		int maxLabel = labels[maxComp].first;
		int row = bitmapInfo.uextent;
		int pos = 0;
		char numNeighbours = 0;
		int ccSize = 0;

		// test neightbourhood for all bitmappixels that are not marginal
		for( size_t v = 1; v < bitmapInfo.vextent-1; ++v )
		{
			for( size_t u = 1; u < bitmapInfo.uextent-1; ++u )
			{
				pos = row + u;

				if( componentsImg[pos] == maxLabel )
				{
					ccSize++;
					numNeighbours = bitmapInfo.bitmap[pos-1] + bitmapInfo.bitmap[pos+1] + 
								bitmapInfo.bitmap[pos-bitmapInfo.uextent-1] + bitmapInfo.bitmap[pos-bitmapInfo.uextent+1] +
								bitmapInfo.bitmap[pos+bitmapInfo.uextent-1] + bitmapInfo.bitmap[pos+bitmapInfo.uextent+1] +
								bitmapInfo.bitmap[pos-bitmapInfo.uextent] + bitmapInfo.bitmap[pos+bitmapInfo.uextent];

					if( (int)numNeighbours != 8 )
						++borderPixels;
				}
			}
			row += bitmapInfo.uextent;
		}

		// check left/right margins
		row = bitmapInfo.uextent;
		for( size_t v = 1; v < bitmapInfo.vextent-1; ++v )
		{
			ccSize++;
			if( componentsImg[row] == maxLabel )
				++borderPixels;

			ccSize++;
			if( componentsImg[row+bitmapInfo.uextent-1] == maxLabel )
				++borderPixels;

			row += bitmapInfo.uextent;
		}

		// check top/bottom margins
		row = ( bitmapInfo.vextent-1 ) * bitmapInfo.uextent;
		for( size_t u = 0; u < bitmapInfo.uextent; ++u )
		{
			ccSize++;
			if( componentsImg[u] == maxLabel )
				++borderPixels;

//.........这里部分代码省略.........

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

void qRansacSD::doAction()
{
	assert(m_app);
	if (!m_app)
		return;

	const ccHObject::Container& selectedEntities = m_app->getSelectedEntities();
	size_t selNum = selectedEntities.size();
	if (selNum!=1)
	{
		m_app->dispToConsole("Select only one cloud!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
		return;
	}

	ccHObject* ent = selectedEntities[0];
	assert(ent);
	if (!ent || !ent->isA(CC_TYPES::POINT_CLOUD))
	{
		m_app->dispToConsole("Select a real point cloud!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
		return;
	}

	ccPointCloud* pc = static_cast<ccPointCloud*>(ent);

	//input cloud
	unsigned count = pc->size();
	bool hasNorms = pc->hasNormals();
	CCVector3 bbMin, bbMax;
	pc->getBoundingBox(bbMin,bbMax);
	const CCVector3d& globalShift = pc->getGlobalShift();
	double globalScale = pc->getGlobalScale();

	//Convert CC point cloud to RANSAC_SD type
	PointCloud cloud;
	{
		try
		{
			cloud.reserve(count);
		}
		catch(...)
		{
			m_app->dispToConsole("Not enough memory!",ccMainAppInterface::ERR_CONSOLE_MESSAGE);
			return;
		}

		//default point & normal
		Point Pt;
		Pt.normal[0] = 0.0;
		Pt.normal[1] = 0.0;
		Pt.normal[2] = 0.0;
		for (unsigned i=0; i<count; ++i)
		{
			const CCVector3* P = pc->getPoint(i);
			Pt.pos[0] = static_cast<float>(P->x);
			Pt.pos[1] = static_cast<float>(P->y);
			Pt.pos[2] = static_cast<float>(P->z);
			if (hasNorms)
			{
				const CCVector3& N = pc->getPointNormal(i);
				Pt.normal[0] = static_cast<float>(N.x);
				Pt.normal[1] = static_cast<float>(N.y);
				Pt.normal[2] = static_cast<float>(N.z);
			}
			cloud.push_back(Pt);
		}
		
		//manually set bounding box!
		Vec3f cbbMin,cbbMax;
		cbbMin[0] = static_cast<float>(bbMin.x);
		cbbMin[1] = static_cast<float>(bbMin.y);
		cbbMin[2] = static_cast<float>(bbMin.z);
		cbbMax[0] = static_cast<float>(bbMax.x);
		cbbMax[1] = static_cast<float>(bbMax.y);
		cbbMax[2] = static_cast<float>(bbMax.z);
		cloud.setBBox(cbbMin,cbbMax);
	}

	//cloud scale (useful for setting several parameters
	const float scale = cloud.getScale();

	//init dialog with default values
	ccRansacSDDlg rsdDlg(m_app->getMainWindow());
	rsdDlg.epsilonDoubleSpinBox->setValue(.005f * scale);		// set distance threshold to 0.5% of bounding box width
	rsdDlg.bitmapEpsilonDoubleSpinBox->setValue(.01f * scale);	// set bitmap resolution (= sampling resolution) to 1% of bounding box width
	rsdDlg.supportPointsSpinBox->setValue(s_supportPoints);
	rsdDlg.maxNormDevAngleSpinBox->setValue(s_maxNormalDev_deg);
	rsdDlg.probaDoubleSpinBox->setValue(s_proba);
	rsdDlg.planeCheckBox->setChecked(s_primEnabled[0]);
	rsdDlg.sphereCheckBox->setChecked(s_primEnabled[1]);
	rsdDlg.cylinderCheckBox->setChecked(s_primEnabled[2]);
	rsdDlg.coneCheckBox->setChecked(s_primEnabled[3]);
	rsdDlg.torusCheckBox->setChecked(s_primEnabled[4]);

	if (!rsdDlg.exec())
		return;

	//for parameters persistence
	{
		s_supportPoints = rsdDlg.supportPointsSpinBox->value();
		s_maxNormalDev_deg = rsdDlg.maxNormDevAngleSpinBox->value();
//.........这里部分代码省略.........

bool Cone::InitAverage(const MiscLib::Vector< Vec3f > &samples)
{
	// setup all the planes
	size_t c = samples.size() / 2;
	MiscLib::Vector< GfxTL::Vector4Df > planes(c);
	#pragma omp parallel for schedule(static)
	for (int i = 0; i < static_cast<int>(c); ++i)
	{
		for(unsigned int j = 0; j < 3; ++j)
			planes[i][j] = samples[i][j];
		planes[i][3] = samples[i].dot(samples[i + c]);
	}
	// compute center by intersecting the three planes given by (p1, n1)
	// (p2, n2) and (p3, n3)
	// set up linear system
	double a[4 * 3];
	double d1 = samples[0].dot(samples[c + 0]);
	double d2 = samples[1].dot(samples[c + 1]);
	double d3 = samples[2].dot(samples[c + 2]);
	// column major
	a[0 + 0 * 3] = samples[c + 0][0];
	a[1 + 0 * 3] = samples[c + 1][0];
	a[2 + 0 * 3] = samples[c + 2][0];
	a[0 + 1 * 3] = samples[c + 0][1];
	a[1 + 1 * 3] = samples[c + 1][1];
	a[2 + 1 * 3] = samples[c + 2][1];
	a[0 + 2 * 3] = samples[c + 0][2];
	a[1 + 2 * 3] = samples[c + 1][2];
	a[2 + 2 * 3] = samples[c + 2][2];
	a[0 + 3 * 3] = d1;
	a[1 + 3 * 3] = d2;
	a[2 + 3 * 3] = d3;
	if(dmat_solve(3, 1, a))
		return false;
	m_center[0] = a[0 + 3 * 3];
	m_center[1] = a[1 + 3 * 3];
	m_center[2] = a[2 + 3 * 3];

	LevMarPlaneDistance planeDistance;
	LevMar(planes.begin(), planes.end(), planeDistance,
		(float *)m_center);

	MiscLib::Vector< GfxTL::Vector3Df > spoints(c);
	#pragma omp parallel for schedule(static)
	for (int i = 0; i < static_cast<int>(c); ++i)
	{
		spoints[i] = GfxTL::Vector3Df(samples[i] - m_center);
		spoints[i].Normalize();
	}
	GfxTL::Vector3Df axisDir;
	GfxTL::MeanOfNormals(spoints.begin(), spoints.end(), &axisDir);
	m_axisDir = GfxTL::Vector3Df(axisDir);

	// make sure axis points in good direction
	// the axis is defined to point into the interior of the cone
	float heightSum = 0;
	#pragma omp parallel for schedule(static) reduction(+:heightSum)
	for(int i = 0; i < static_cast<int>(c); ++i)
		heightSum += Height(samples[i]);
	if(heightSum < 0)
		m_axisDir *= -1;

	float angleReduction = 0;
	#pragma omp parallel for schedule(static) reduction(+:angleReduction)
	for(int i = 0; i < static_cast<int>(c); ++i)
	{
		float angle = m_axisDir.dot(samples[i + c]);
		if(angle < -1) // clamp angle to [-1, 1]
			angle = -1;
		else if(angle > 1)
			angle = 1;
		if(angle < 0)
			// m_angle = omega + 90
			angle = std::acos(angle) - float(M_PI) / 2;
		else
			// m_angle = 90 - omega
			angle = float(M_PI) / 2 - std::acos(angle);
		angleReduction += angle;
	}
	angleReduction /= c;
	m_angle = angleReduction;
	if(m_angle < 1.0e-6 || m_angle > float(M_PI) / 2 - 1.0e-6)
		return false;
	//if(m_angle > 1.3962634015954636615389526147909) // 80 degrees
	if(m_angle > 1.4835298641951801403851371532153f) // 85 degrees
		return false;
	m_normal = Vec3f(std::cos(-m_angle), std::sin(-m_angle), 0);
	m_normalY = m_normal[1] * m_axisDir;
	m_n2d[0] = std::cos(m_angle);
	m_n2d[1] = -std::sin(m_angle);
	m_hcs.FromNormal(m_axisDir);
	m_angularRotatedRadians = 0;
	return true;
}

bool Cylinder::InitAverage(const MiscLib::Vector< Vec3f > &samples)
{
	if(samples.size() < 4)
		return false;
	// estimate axis from covariance of normal vectors
	MiscLib::Vector< GfxTL::Vector3Df > normals;
	size_t c = samples.size() / 2;
	for(size_t i = c; i < samples.size(); ++i)
	{
		normals.push_back(GfxTL::Vector3Df(samples[i]));
		normals.push_back(GfxTL::Vector3Df(-samples[i]));
	}
	GfxTL::MatrixXX< 3, 3, float > cov, eigenVectors;
	GfxTL::Vector3Df eigenValues;
	GfxTL::CovarianceMatrix(GfxTL::Vector3Df(0, 0, 0),
		normals.begin(), normals.end(), &cov);
	GfxTL::Jacobi(cov, &eigenValues, &eigenVectors);
	// find the minimal eigenvalue and corresponding vector
	float minEigVal = eigenValues[0];
	unsigned int minEigIdx = 0;
	for(unsigned int i = 1; i < 3; ++i)
		if(eigenValues[i] < minEigVal)
		{
			minEigVal = eigenValues[i];
			minEigIdx = i;
		}
	m_axisDir = Vec3f(eigenVectors[minEigIdx]);
	// get a point on the axis from all pairs
	m_axisPos = Vec3f(0, 0, 0);
	m_radius = 0;
	size_t pointCount = 0;
	size_t pairCount = 0;
	for(size_t i = 0; i < c - 1; ++i)
		for(size_t j = i + 1; j < c; ++j)
		{
			// project first normal into plane
			float l = m_axisDir.dot(samples[i + c]);
			Vec3f xdir = samples[i + c] - l * m_axisDir;
			xdir.normalize();
			Vec3f ydir = m_axisDir.cross(xdir);
			ydir.normalize();
			// xdir is the x axis in the plane (y = 0) samples[i] is the origin
			float lineBnx = ydir.dot(samples[j + c]);
			if(abs(lineBnx) < .05f)
				continue;
			float lineBny = -xdir.dot(samples[j + c]);
			// origin of lineB
			Vec3f originB = samples[j] - samples[i];
			float lineBOx = xdir.dot(originB);
			float lineBOy = ydir.dot(originB);
			float lineBd = lineBnx * lineBOx + lineBny * lineBOy;
			// lineB in the plane complete
			// point of intersection is y = 0 and x = lineBd / lineBnx
			float radius = lineBd / lineBnx;
			m_axisPos += samples[i] + radius * xdir;
			m_radius += abs(radius);
			m_radius += std::sqrt((radius - lineBOx) * (radius - lineBOx) + lineBOy * lineBOy);
			++pointCount;
		}
	if(!pointCount)
		return false;
	m_axisPos /= pointCount;
	m_radius /= pointCount * 2;
	if(m_radius > 1e6)
		return false;

	// find point on axis closest to origin
	float lambda = m_axisDir.dot(-m_axisPos);
	m_axisPos = m_axisPos + lambda * m_axisDir;

	m_hcs.FromNormal(m_axisDir);
	m_angularRotatedRadians = 0;
	return true;
}