C++ CCVector3::norm方法代码示例

void ccGBLSensor::projectPoint(	const CCVector3& sourcePoint,
								CCVector2& destPoint,
								PointCoordinateType &depth,
								double posIndex/*=0*/) const
{
	//project point in sensor world
	CCVector3 P = sourcePoint;

	//sensor to world global transformation = sensor position * rigid transformation
	ccIndexedTransformation sensorPos; //identity by default
	if (m_posBuffer)
		m_posBuffer->getInterpolatedTransformation(posIndex,sensorPos);
	sensorPos *= m_rigidTransformation;

	//apply (inverse) global transformation (i.e world to sensor)
	sensorPos.inverse().apply(P);

	//convert to 2D sensor field of view + compute its distance
	switch (m_rotationOrder)
	{
	case YAW_THEN_PITCH:
	{
		//yaw = angle around Z, starting from 0 in the '+X' direction
		destPoint.x = atan2(P.y,P.x);
		//pitch = angle around the lateral axis, between -pi (-Z) to pi (+Z) by default
		destPoint.y = atan2(P.z,sqrt(P.x*P.x + P.y*P.y));
		break;
	}
	case PITCH_THEN_YAW:
	{
		//FIXME
		//yaw = angle around Z, starting from 0 in the '+X' direction
		destPoint.x = -atan2(sqrt(P.y*P.y + P.z*P.z),P.x);
		//pitch = angle around the lateral axis, between -pi (-Z) to pi (+Z) by default
		destPoint.y = -atan2(P.y,P.z);
		break;
	}
	default:
		assert(false);
	}
	
	//if the yaw angles are shifted
	if (m_yawAnglesAreShifted && destPoint.x < 0)
		destPoint.x += static_cast<PointCoordinateType>(2.0*M_PI);
	//if the pitch angles are shifted
	if (m_pitchAnglesAreShifted && destPoint.y < 0)
		destPoint.y += static_cast<PointCoordinateType>(2.0*M_PI);

	depth = P.norm();
}

bool GeometricalAnalysisTools::computeSphereFrom4(	const CCVector3& A,
													const CCVector3& B,
													const CCVector3& C,
													const CCVector3& D,
													CCVector3& center,
													PointCoordinateType& radius )
{
	//inspired from 'tetrahedron_circumsphere_3d' by Adrian Bowyer and John Woodwark

	//Set up the linear system.
	double a[12];
	{
		CCVector3 AB = B-A;
		a[0] = AB.x;
		a[3] = AB.y;
		a[6] = AB.z;
		a[9] = AB.norm2d();
	}
	{
		CCVector3 AC = C-A;
		a[1]  = AC.x;
		a[4]  = AC.y;
		a[7]  = AC.z;
		a[10] = AC.norm2d();
	}
	{
		CCVector3 AD = D-A;
		a[2]  = AD.x;
		a[5]  = AD.y;
		a[8]  = AD.z;
		a[11] = AD.norm2d();
	}

	//  Solve the linear system (with Gauss-Jordan elimination)
	if ( dmat_solve ( 3, 1, a ) != 0 )
	{
		//system is singular?
		return false;
	}

	//  Compute the radius and center.
	CCVector3 u = CCVector3(static_cast<PointCoordinateType>(a[0+3*3]),
							static_cast<PointCoordinateType>(a[1+3*3]),
							static_cast<PointCoordinateType>(a[2+3*3])) / 2;
	radius = u.norm();
	center = A + u;

	return true;
}

void cc2DLabel::getLabelInfo3(LabelInfo3& info) const
{
	info.cloud1 = info.cloud2 = info.cloud3 = 0;
	if (m_points.size() != 3)
		return;
	//1st point
	info.cloud1 = m_points[0].cloud;
	info.point1Index = m_points[0].index;
	const CCVector3* P1 = info.cloud1->getPointPersistentPtr(info.point1Index);
	//2nd point
	info.cloud2 = m_points[1].cloud;
	info.point2Index = m_points[1].index;
	const CCVector3* P2 = info.cloud2->getPointPersistentPtr(info.point2Index);
	//3rd point
	info.cloud3 = m_points[2].cloud;
	info.point3Index = m_points[2].index;
	const CCVector3* P3 = info.cloud3->getPointPersistentPtr(info.point3Index);

	//area
	CCVector3 P1P2 = *P2-*P1;
	CCVector3 P1P3 = *P3-*P1;
	CCVector3 P2P3 = *P3-*P2;
	CCVector3 N = P1P2.cross(P1P3); //N = ABxAC
	info.area = N.norm()/2;

	//normal
	N.normalize();
	info.normal = N;

	//edges length
	info.edges.u[0] = P1P2.norm2d();  //edge 1-2
	info.edges.u[1] = P2P3.norm2d();  //edge 2-3
	info.edges.u[2] = P1P3.norm2d();  //edge 3-1

	//angle
	info.angles.u[0] = GetAngle_deg(P1P2,P1P3);   //angleAtP1
	info.angles.u[1] = GetAngle_deg(P2P3,-P1P2);  //angleAtP2
	info.angles.u[2] = GetAngle_deg(-P1P3,-P2P3); //angleAtP3 (should be equal to 180-a1-a2!)
}

SimpleCloud* MeshSamplingTools::samplePointsOnMesh(GenericMesh* theMesh,
													double samplingDensity,
													unsigned theoricNumberOfPoints,
													GenericProgressCallback* progressCb,
													GenericChunkedArray<1,unsigned>* triIndices/*=0*/)
{
	assert(theMesh);
	unsigned triCount = (theMesh ? theMesh->size() : 0);
	if (triCount==0)
		return 0;

	if (theoricNumberOfPoints < 1)
        return 0;


	SimpleCloud* sampledCloud = new SimpleCloud();
	if (!sampledCloud->reserve(theoricNumberOfPoints)) //not enough memory
	{
		delete sampledCloud;
		return 0;
	}

	if (triIndices)
	{
	    triIndices->clear();
		//not enough memory? DGM TODO: we should warn the caller
		if (!triIndices->reserve(theoricNumberOfPoints) || triIndices->capacity() < theoricNumberOfPoints)
		{
			delete sampledCloud;
			triIndices->clear();
			return 0;
		}
	}

	NormalizedProgress* normProgress=0;
    if(progressCb)
    {
		normProgress = new NormalizedProgress(progressCb,triCount);
		progressCb->setMethodTitle("Mesh sampling");
		char buffer[256];
		sprintf(buffer,"Triangles: %i\nPoints: %i",triCount,theoricNumberOfPoints);
		progressCb->setInfo(buffer);
        progressCb->reset();
		progressCb->start();
	}

	unsigned addedPoints=0;

	//for each triangle
	theMesh->placeIteratorAtBegining();
	for (unsigned n=0;n<triCount;++n)
	{
		const GenericTriangle* tri = theMesh->_getNextTriangle();

		//summits (OAB)
		const CCVector3 *O = tri->_getA();
		const CCVector3 *A = tri->_getB();
		const CCVector3 *B = tri->_getC();

		//edges (OA and OB)
		CCVector3 u = *A - *O;
		CCVector3 v = *B - *O;

		//we compute the (twice) the triangle area
		CCVector3 N = u.cross(v);
		double S = N.norm()/2.0;

		//we deduce the number of points to generate on this face
		double fPointsToAdd = S*samplingDensity;
		unsigned pointsToAdd = static_cast<unsigned>(fPointsToAdd);

        //if the face area is smaller than the surface/random point
		if (pointsToAdd == 0)
		{
			//we add a point with the same probability as its (relative) area
			if (static_cast<double>(rand()) <= fPointsToAdd * static_cast<double>(RAND_MAX))
                pointsToAdd = 1;
		}

		if (pointsToAdd)
		{
			if (addedPoints + pointsToAdd >= theoricNumberOfPoints)
			{
				theoricNumberOfPoints+=pointsToAdd;
				if (!sampledCloud->reserve(theoricNumberOfPoints)
					|| (triIndices && triIndices->capacity() < theoricNumberOfPoints && !triIndices->reserve(theoricNumberOfPoints))) //not enough memory
				{
					delete sampledCloud;
					sampledCloud = 0;
					triIndices->clear();
					break;
				}
			}

			for (unsigned i=0; i<pointsToAdd; ++i)
			{
				//we generates random points as in:
				//'Greg Turk. Generating random points in triangles. In A. S. Glassner, editor,Graphics Gems, pages 24-28. Academic Press, 1990.'
				double x = static_cast<double>(rand())/static_cast<double>(RAND_MAX);
				double y = static_cast<double>(rand())/static_cast<double>(RAND_MAX);
//.........这里部分代码省略.........

ccGBLSensor::NormalGrid* ccGBLSensor::projectNormals(	CCLib::GenericCloud* cloud,
														const NormalGrid& theNorms,
														double posIndex/*=0*/) const
{
	if (!cloud || !theNorms.isAllocated())
		return 0;

	unsigned size = m_depthBuffer.height*m_depthBuffer.width;
	if (size == 0)
		return 0; //depth buffer empty/not initialized!

	NormalGrid* normalGrid = new NormalGrid;
	if (!normalGrid->resize(size,0))
		return 0; //not enough memory

	//sensor to world global transformation = sensor position * rigid transformation
	ccIndexedTransformation sensorPos; //identity by default
	if (m_posBuffer)
		m_posBuffer->getInterpolatedTransformation(posIndex,sensorPos);
	sensorPos *= m_rigidTransformation;

	//poject each point + normal
	{
		cloud->placeIteratorAtBegining();
		unsigned pointCount = cloud->size();
		for (unsigned i=0; i<pointCount; ++i)
		{
			const CCVector3* P = cloud->getNextPoint();
			const PointCoordinateType* N = theNorms.getValue(i);

			//project point
			CCVector2 Q;
			PointCoordinateType depth1;
			projectPoint(*P,Q,depth1,m_activeIndex);

			CCVector3 S;

			CCVector3 U = *P - sensorPos.getTranslationAsVec3D();
			PointCoordinateType distToSensor = U.norm();

			if (distToSensor > ZERO_TOLERANCE)
			{
				//normal component along sensor viewing dir.
				S.z = -CCVector3::vdot(N,U.u)/distToSensor;

				if (S.z > 1.0-ZERO_TOLERANCE)
				{
					S.x = 0;
					S.y = 0;
				}
				else
				{
					//and point+normal
					CCVector3 P2 = *P + CCVector3(N);
					CCVector2 S2;
					PointCoordinateType depth2;
					projectPoint(P2,S2,depth2,m_activeIndex);

					//deduce other normals components
					PointCoordinateType coef = sqrt((1 - S.z*S.z)/(S.x*S.x + S.y*S.y));
					S.x = coef * (S2.x - Q.x);
					S.y = coef * (S2.y - Q.y);
				}
			}
			else
			{
				S = CCVector3(N);
			}

			//project in Z-buffer
			unsigned x,y;
			if (convertToDepthMapCoords(Q.x,Q.y,x,y))
			{
				//add the transformed normal
				PointCoordinateType* newN = normalGrid->getValue(y*m_depthBuffer.width + x);
				CCVector3::vadd(newN,S.u,newN);
			}
			else
			{
				//shouldn't happen!
				assert(false);
			}
		}
	}

	//normalize
	{
		normalGrid->placeIteratorAtBegining();
		for (unsigned i=0; i<m_depthBuffer.height*m_depthBuffer.width; ++i)
		{
			PointCoordinateType* newN = normalGrid->getCurrentValue();
			CCVector3::vnormalize(newN);
			normalGrid->forwardIterator();
		}
	}

	return normalGrid;
}

QStringList cc2DLabel::getLabelContent(int precision)
{
	QStringList body;

	switch(m_points.size())
	{
	case 1: //point
		{
			//init title
			/*title = m_title;
			//automatically elide the title
			title = titleFontMetrics.elidedText(title,Qt::ElideRight,dx);
			//*/

			//coordinates
			ccGenericPointCloud* cloud = m_points[0].cloud;
			const unsigned& pointIndex = m_points[0].index;
			const CCVector3* P = cloud->getPointPersistentPtr(pointIndex);
			QString coordStr = QString("P#%0: (%1;%2;%3)").arg(pointIndex).arg(P->x,0,'f',precision).arg(P->y,0,'f',precision).arg(P->z,0,'f',precision);
			body << coordStr;

			//normal
			if (cloud->hasNormals())
			{
				const PointCoordinateType* N = cloud->getPointNormal(pointIndex);
				assert(N);
				QString normStr = QString("Normal: (%1;%2;%3)").arg(N[0],0,'f',precision).arg(N[1],0,'f',precision).arg(N[2],0,'f',precision);
				body << normStr;
			}
			//color
			if (cloud->hasColors())
			{
				const colorType* C = cloud->getPointColor(pointIndex);
				assert(C);
				QString colorStr = QString("Color: (%1;%2;%3)").arg(C[0]).arg(C[1]).arg(C[2]);
				body << colorStr;
			}
			//scalar field
			if (cloud->hasDisplayedScalarField())
			{
				ScalarType D = cloud->getPointScalarValue(pointIndex);
				QString sfStr = QString("Scalar: %1").arg(D,0,'f',precision);
				body << sfStr;
			}
		}
		break;
	case 2: //vector
		{
			//1st point
			ccGenericPointCloud* cloud1 = m_points[0].cloud;
			const unsigned& pointIndex1 = m_points[0].index;
			const CCVector3* P1 = cloud1->getPointPersistentPtr(pointIndex1);
			//2nd point
			ccGenericPointCloud* cloud2 = m_points[1].cloud;
			const unsigned& pointIndex2 = m_points[1].index;
			const CCVector3* P2 = cloud2->getPointPersistentPtr(pointIndex2);

			PointCoordinateType d = (*P1-*P2).norm();
			QString distStr = QString("Distance = %1").arg(d,0,'f',precision);
			body << distStr;

			QString coordStr1 = QString("P#%0: (%1;%2;%3)").arg(pointIndex1).arg(P1->x,0,'f',precision).arg(P1->y,0,'f',precision).arg(P1->z,0,'f',precision);
			body << coordStr1;
			QString coordStr2 = QString("P#%0: (%1;%2;%3)").arg(pointIndex2).arg(P2->x,0,'f',precision).arg(P2->y,0,'f',precision).arg(P2->z,0,'f',precision);
			body << coordStr2;
		}
		break;
	case 3: //triangle/plane
		{
			//1st point
			ccGenericPointCloud* cloud1 = m_points[0].cloud;
			const unsigned& pointIndex1 = m_points[0].index;
			const CCVector3* P1 = cloud1->getPointPersistentPtr(pointIndex1);
			//2nd point
			ccGenericPointCloud* cloud2 = m_points[1].cloud;
			const unsigned& pointIndex2 = m_points[1].index;
			const CCVector3* P2 = cloud2->getPointPersistentPtr(pointIndex2);
			//3rd point
			ccGenericPointCloud* cloud3 = m_points[2].cloud;
			const unsigned& pointIndex3 = m_points[2].index;
			const CCVector3* P3 = cloud3->getPointPersistentPtr(pointIndex3);

			//area
			CCVector3 P1P2 = *P2-*P1;
			CCVector3 P1P3 = *P3-*P1;
			CCVector3 N = P1P2.cross(P1P3); //N=ABxAC
			PointCoordinateType area = N.norm()*(PointCoordinateType)0.5;
			QString areaStr = QString("Area = %1").arg(area,0,'f',precision);
			body << areaStr;

			//coordinates
			QString coordStr1 = QString("A#%0: (%1;%2;%3)").arg(pointIndex1).arg(P1->x,0,'f',precision).arg(P1->y,0,'f',precision).arg(P1->z,0,'f',precision);
			body << coordStr1;
			QString coordStr2 = QString("B#%0: (%1;%2;%3)").arg(pointIndex2).arg(P2->x,0,'f',precision).arg(P2->y,0,'f',precision).arg(P2->z,0,'f',precision);
			body << coordStr2;
			QString coordStr3 = QString("C#%0: (%1;%2;%3)").arg(pointIndex3).arg(P3->x,0,'f',precision).arg(P3->y,0,'f',precision).arg(P3->z,0,'f',precision);
			body << coordStr3;

			//normal
			N.normalize();
//.........这里部分代码省略.........

bool ccGriddedTools::DetectParameters(	const ccPointCloud* cloud,
										const ccPointCloud::Grid::Shared grid,
										GridParameters& parameters,
										bool verbose/*=false*/,
										ccGLMatrix* cloudToSensorTrans/*=0*/)
{
	if (!cloud || !grid)
	{
		assert(false);
		return false;
	}

	parameters.minPhi = static_cast<PointCoordinateType>(M_PI);
	parameters.maxPhi = -parameters.minPhi;
	parameters.minTheta = static_cast<PointCoordinateType>(M_PI);
	parameters.maxTheta = -parameters.minTheta;
	parameters.deltaPhiRad = 0;
	parameters.deltaThetaRad = 0;
	parameters.maxRange = 0;

	//we must test if the angles are shifted (i.e the scan spans above theta = pi)
	//we'll compute all parameters for both cases, and choose the best one at the end!
	PointCoordinateType minPhiShifted = parameters.minPhi, maxPhiShifted = parameters.maxPhi;
	PointCoordinateType minThetaShifted = parameters.minTheta, maxThetaShifted = parameters.maxTheta;

	try
	{
		//determine the PITCH angular step
		{
			std::vector< AngleAndSpan > angles;
			std::vector< AngleAndSpan > anglesShifted;

			//for each ROW we determine the min and max valid grid point (i.e. index >= 0)
			const int* _indexGrid = &(grid->indexes[0]);
			for (unsigned j=0; j<grid->h; ++j)
			{
				unsigned minIndex = grid->w;
				unsigned maxIndex = 0;
				for (unsigned i=0; i<grid->w; ++i)
				{
					if (_indexGrid[i] >= 0)
					{
						if (i < minIndex)
							minIndex = i;
						if (i > maxIndex)
							maxIndex = i;
					}
				}

				if (maxIndex > minIndex)
				{
					PointCoordinateType minPhiCurrentLine = 0, maxPhiCurrentLine = 0;
					PointCoordinateType minPhiCurrentLineShifted = 0, maxPhiCurrentLineShifted = 0;
					for (unsigned k=minIndex; k<=maxIndex; ++k)
					{
						int index = _indexGrid[k];
						if (index >= 0)
						{
							CCVector3 P = *(cloud->getPoint(static_cast<unsigned>(index)));
							if (cloudToSensorTrans)
								cloudToSensorTrans->apply(P);
							PointCoordinateType p = atan2(P.z,sqrt(P.x*P.x + P.y*P.y)); //see ccGBLSensor::projectPoint
							PointCoordinateType pShifted = (p < 0 ? p + static_cast<PointCoordinateType>(2.0*M_PI) : p);
							if (k != minIndex)
							{
								if (minPhiCurrentLine > p)
									minPhiCurrentLine = p;
								else if (maxPhiCurrentLine < p)
									maxPhiCurrentLine = p;

								if (minPhiCurrentLineShifted > pShifted)
									minPhiCurrentLineShifted = pShifted;
								else if (maxPhiCurrentLineShifted < pShifted)
									maxPhiCurrentLineShifted = pShifted;
							}
							else
							{
								minPhiCurrentLine = maxPhiCurrentLine = p;
								minPhiCurrentLineShifted = maxPhiCurrentLineShifted = pShifted;
							}

							//find max range
							PointCoordinateType range = P.norm();
							if (range > parameters.maxRange)
								parameters.maxRange = range;
						}
					}

					if (parameters.minPhi > minPhiCurrentLine)
						parameters.minPhi = minPhiCurrentLine;
					if (parameters.maxPhi < maxPhiCurrentLine)
						parameters.maxPhi = maxPhiCurrentLine;

					if (minPhiShifted > minPhiCurrentLineShifted)
						minPhiShifted = minPhiCurrentLineShifted;
					if (maxPhiShifted < maxPhiCurrentLineShifted)
						maxPhiShifted = maxPhiCurrentLineShifted;

					unsigned span = maxIndex-minIndex+1;
					ScalarType angle_rad = static_cast<ScalarType>((maxPhiCurrentLine-minPhiCurrentLine) / span);
//.........这里部分代码省略.........

//.........这里部分代码省略.........
				m_box.minCorner().z = m_box.maxCorner().z;
			break;
		case Z_PLUS_ARROW:
			m_box.maxCorner().z += u.z;
			if (m_box.minCorner().z > m_box.maxCorner().z)
				m_box.maxCorner().z = m_box.minCorner().z;
			break;
		case CROSS:
			m_box.minCorner() += u;
			m_box.maxCorner() += u;
			break;
		default:
			assert(false);
			return false;
		}
		
		//send 'modified' signal
		emit boxModified(&m_box);
	}
	else if (m_activeComponent == SPHERE)
	{
		//handled by move2D!
		return false;
	}
	else if (m_activeComponent >= X_MINUS_TORUS && m_activeComponent <= Z_PLUS_TORUS)
	{
		//we guess the rotation order by comparing the current screen 'normal'
		//and the vector prod of u and the current rotation axis
		CCVector3 Rb(0.0,0.0,0.0);
		switch(m_activeComponent)
		{
		case X_MINUS_TORUS:
			Rb.x = -1.0;
			break;
		case X_PLUS_TORUS:
			Rb.x = 1.0;
			break;
		case Y_MINUS_TORUS:
			Rb.y = -1.0;
			break;
		case Y_PLUS_TORUS:
			Rb.y = 1.0;
			break;
		case Z_MINUS_TORUS:
			Rb.z = -1.0;
			break;
		case Z_PLUS_TORUS:
			Rb.z = 1.0;
			break;
		default:
			assert(false);
			return false;
		}
		
		CCVector3 R = Rb;
		if (m_glTransEnabled)
			m_glTrans.applyRotation(R);

		CCVector3 RxU = R.cross(u);

		//look for the most parallel dimension
		int minDim = 0;
		PointCoordinateType maxDot = CCVector3(m_viewMatrix.getColumn(0)).dot(RxU);
		for (int i=1;i<3;++i)
		{
			PointCoordinateType dot = CCVector3(m_viewMatrix.getColumn(i)).dot(RxU);
			if (fabs(dot) > fabs(maxDot))
			{
				maxDot = dot;
				minDim = i;
			}
		}

		//angle is proportional to absolute displacement
		PointCoordinateType angle_rad = u.norm()/m_box.getDiagNorm() * M_PI;
		if (maxDot < 0.0)
			angle_rad = -angle_rad;

		ccGLMatrix rotMat;
		rotMat.initFromParameters(angle_rad,Rb,CCVector3(0.0,0.0,0.0));

		CCVector3 C = m_box.getCenter();
		ccGLMatrix transMat;
		transMat.setTranslation(-C);
		transMat = rotMat * transMat;
		transMat.setTranslation(CCVector3(transMat.getTranslation())+C);

		m_glTrans = m_glTrans * transMat.inverse();
		enableGLTransformation(true);
	}
	else
	{
		assert(false);
		return false;
	}

	update();

	return true;
}

PointCoordinateType* GroundBasedLidarSensor::projectNormals(GenericCloud* aCloud, GenericChunkedArray<3,PointCoordinateType>& theNorms)
{
    if ((!aCloud)||(!theNorms.isAllocated()))
        return 0;

    unsigned size = 3*dB.h_buff*dB.l_buff;
    PointCoordinateType* theNewNorms = new PointCoordinateType[size];
    if (!theNewNorms)
        return 0; //not enough memory
    memset(theNewNorms,0,size*sizeof(PointCoordinateType));

    float dt = 1.0f/deltaTheta;
    float dp = 1.0f/deltaPhi;
    int x,y;

    PointCoordinateType *N,*_theNewNorms;
    CCVector3 R,S;
    CCVector2 Q,S2;
    DistanceType dist1,dist2;

    aCloud->placeIteratorAtBegining();
    theNorms.placeIteratorAtBegining();
    unsigned i,n = aCloud->size();
    for (i=0;i<n;++i)
    {
        //le ième point et sa normale
        const CCVector3 *P = aCloud->getNextPoint();
        N = theNorms.getCurrentValue();

        CCVector3 U = *P - sensorCenter;
        U.x += base;

        //norme du vecteur "direction de visée laser"
        float norm = U.norm();


        //si sa norme est nulle, alors le point serait confondu avec le centre du capteur (peu probable ...)
        if (norm>1e-7)
        {
            //composante de la normale selon la direction de visée ("S.z")
            S.z = -CCVector3::vdot(N,U.u)/norm;

            if (S.z > 0.99999)
            {
                S.x = 0.0;
                S.y = 0.0;
            }
            else
            {
                //on projette le point
                projectPoint(*P,Q,dist1);

                //et l'autre extrémité du vecteur normal
                R.x = P->x + N[0];
                R.y = P->y + N[1];
                R.z = P->z + N[2];
                projectPoint(R,S2,dist2);

                //la normale projetée est égale selon les autres dimensions à la différence des deux (à un coefficient près ;)
                S.x = S2.x - Q.x;
                S.y = S2.y - Q.y;

                float coef = sqrt((1.0f-S.z*S.z)/(S.x*S.x+S.y*S.y));

                S.x = coef * S.x;
                S.y = coef * S.y;
            }
        }
        else
        {
            //sinon on se casse pas la tête, on renvoie la même normale
            S.x = N[0];
            S.y = N[1];
            S.z = N[2];
        }

        //on projette dans le Z-buffer
        x = int(floor((Q.x-thetaMin)*dt));
        y = int(floor((Q.y-phiMin)*dp));

        //on ajoute la normale transformée
        _theNewNorms = theNewNorms + 3*(y*dB.l_buff+x);
        CCVector3::vadd(_theNewNorms,S.u,_theNewNorms);

        theNorms.forwardIterator();
    }

    //on normalise tout (au cas où il y ait des accumulations)
    _theNewNorms = theNewNorms;
    for (i=0;i<unsigned(dB.h_buff*dB.l_buff);++i)
    {
        CCVector3::vnormalize(_theNewNorms);
        _theNewNorms+=3;
    }

    return theNewNorms;
}

CCLib::ReferenceCloud* qHPR::removeHiddenPoints(CCLib::GenericIndexedCloudPersist* theCloud, const CCVector3& viewPoint, float fParam)
{
	assert(theCloud);

	unsigned nbPoints = theCloud->size();
	if (nbPoints == 0)
		return 0;

	//less than 4 points? no need for calculation, we return the whole cloud
	if (nbPoints < 4)
	{
		CCLib::ReferenceCloud* visiblePoints = new CCLib::ReferenceCloud(theCloud);
		if (!visiblePoints->addPointIndex(0,nbPoints)) //well even for less than 4 points we never know ;)
		{
			//not enough memory!
			delete visiblePoints;
			visiblePoints = 0;
		}
		return visiblePoints;
	}

	PointCoordinateType maxRadius = 0;

	//convert point cloud to an array of double triplets (for qHull)
	coordT* pt_array = new coordT[(nbPoints+1)*3];
	{
		coordT* _pt_array = pt_array;

		for (unsigned i=0; i<nbPoints; ++i)
		{
			CCVector3 P = *theCloud->getPoint(i) - viewPoint;
			*_pt_array++ = (coordT)P.x;
			*_pt_array++ = (coordT)P.y;
			*_pt_array++ = (coordT)P.z;

			//we keep track of the highest 'radius'
			PointCoordinateType r2 = P.norm2();
			if (maxRadius < r2)
				maxRadius = r2;
		}
		
		//we add the view point (Cf. HPR)
		*_pt_array++ = 0;
		*_pt_array++ = 0;
		*_pt_array++ = 0;

		maxRadius = sqrt(maxRadius);
	}

	//apply spherical flipping
	{
		maxRadius *= 2.0f*pow(10.0f,fParam);
	
		coordT* _pt_array = pt_array;
		for (unsigned i=0; i<nbPoints; ++i)
		{
			CCVector3 P = *theCloud->getPoint(i) - viewPoint;

			double r = (double)(maxRadius/P.norm()) - 1.0;
			*_pt_array++ *= r;
			*_pt_array++ *= r;
			*_pt_array++ *= r;
		}
	}

	//array to flag points on the convex hull
	std::vector<bool> pointBelongsToCvxHull;

	if (!qh_new_qhull(3,nbPoints+1,pt_array,False,(char*)"qhull QJ Qci",0,stderr))
	{
		try
		{
			pointBelongsToCvxHull.resize(nbPoints+1,false);
		}
		catch(std::bad_alloc)
		{
			//not enough memory!
			delete[] pt_array;
			return 0;
		}

		vertexT *vertex = 0,**vertexp = 0;
		facetT *facet = 0;

		FORALLfacets
		{
			//if (!facet->simplicial)
			//	error("convhulln: non-simplicial facet"); // should never happen with QJ

			setT* vertices = qh_facet3vertex(facet);
			FOREACHvertex_(vertices)
			{
				pointBelongsToCvxHull[qh_pointid(vertex->point)] = true;
			}
			qh_settempfree(&vertices);
		}
	}

	delete[] pt_array;
	pt_array=0;
//.........这里部分代码省略.........

PointCoordinateType* ccGBLSensor::projectNormals(CCLib::GenericCloud* aCloud, GenericChunkedArray<3,PointCoordinateType>& theNorms) const
{
	if (!aCloud || !theNorms.isAllocated())
		return 0;

	unsigned size = 3*m_depthBuffer.height*m_depthBuffer.width;
	if (size == 0)
		return 0; //depth buffer empty/not initialized!

	PointCoordinateType* theNewNorms = new PointCoordinateType[size];
	if (!theNewNorms)
		return 0; //not enough memory
	memset(theNewNorms,0,size*sizeof(PointCoordinateType));

	//poject each point+normal
	{
		aCloud->placeIteratorAtBegining();
		theNorms.placeIteratorAtBegining();
		unsigned pointCount = aCloud->size();
		for (unsigned i=0;i<pointCount;++i)
		{
			const CCVector3* P = aCloud->getNextPoint();
			const PointCoordinateType* N = theNorms.getCurrentValue();

			CCVector3 U = *P - sensorCenter;
			U.x += base;

			CCVector3 S;
			CCVector2 Q;

			PointCoordinateType norm = U.norm();
			if (norm>ZERO_TOLERANCE)
			{
				//normal component along sensor viewing dir.
				S.z = -CCVector3::vdot(N,U.u)/norm;

				if (S.z > 1.0-ZERO_TOLERANCE)
				{
					S.x = 0;
					S.y = 0;
				}
				else
				{
					//project point
					ScalarType depth1;
					projectPoint(*P,Q,depth1);

					//and point+normal
					CCVector3 R = *P + CCVector3(N);
					CCVector2 S2;
					ScalarType depth2;
					projectPoint(R,S2,depth2);

					//deduce other normals components
					PointCoordinateType coef = sqrt((1 - S.z*S.z)/(S.x*S.x + S.y*S.y));
					S.x = coef * (S2.x - Q.x);
					S.y = coef * (S2.y - Q.y);
				}
			}
			else
			{
				S = CCVector3(N);
			}

			//project in Z-buffer
			int x = static_cast<int>(floor((Q.x-thetaMin)/deltaTheta));
			int y = static_cast<int>(floor((Q.y-phiMin)/deltaPhi));

			//on ajoute la normale transformee
			PointCoordinateType* newN = theNewNorms+3*(y*m_depthBuffer.width+x);
			CCVector3::vadd(newN,S.u,newN);

			theNorms.forwardIterator();
		}
	}

	//normalize
	{
		PointCoordinateType* newN = theNewNorms;
		for (int i=0; i<m_depthBuffer.height*m_depthBuffer.width; ++i,newN+=3)
		{
			CCVector3::vnormalize(newN);
		}
	}

	return theNewNorms;
}

ccGBLSensor* ccGriddedTools::ComputeBestSensor(ccPointCloud* cloud, const std::vector<int>& indexGrid, unsigned width, unsigned height, ccGLMatrix* cloudToSensorTrans/*=0*/)
{
	PointCoordinateType minPhi = static_cast<PointCoordinateType>(M_PI), maxPhi = -minPhi;
	PointCoordinateType minTheta = static_cast<PointCoordinateType>(M_PI), maxTheta = -minTheta;
	PointCoordinateType deltaPhiRad = 0, deltaThetaRad = 0;
	PointCoordinateType maxRange = 0;

	//we must test if the angles are shifted (i.e the scan spans above theta = pi)
	//we'll compute all parameters for both cases, and choose the best one at the end!
	PointCoordinateType minPhiShifted = minPhi, maxPhiShifted = maxPhi;
	PointCoordinateType minThetaShifted = minTheta, maxThetaShifted = maxTheta;

	try
	{
		//determine the PITCH angular step
		{
			std::vector< AngleAndSpan > angles;
			std::vector< AngleAndSpan > anglesShifted;

			//for each LINE we determine the min and max valid grid point (i.e. != (0,0,0))
			const int* _indexGrid = &(indexGrid[0]);
			for (unsigned j=0; j<height; ++j)
			{
				unsigned minIndex = width;
				unsigned maxIndex = 0;
				for (unsigned i=0; i<width; ++i)
				{
					if (_indexGrid[i] >= 0)
					{
						if (i < minIndex)
							minIndex = i;
						if (i > maxIndex)
							maxIndex = i;
					}
				}

				if (maxIndex > minIndex)
				{
					PointCoordinateType minPhiCurrentLine = 0, maxPhiCurrentLine = 0;
					PointCoordinateType minPhiCurrentLineShifted = 0, maxPhiCurrentLineShifted = 0;
					for (unsigned k=minIndex; k<=maxIndex; ++k)
					{
						int index = _indexGrid[k];
						if (index >= 0)
						{
							CCVector3 P = *(cloud->getPoint(static_cast<unsigned>(index)));
							if (cloudToSensorTrans)
								cloudToSensorTrans->apply(P);
							PointCoordinateType p = atan2(P.z,sqrt(P.x*P.x + P.y*P.y)); //see ccGBLSensor::projectPoint
							PointCoordinateType pShifted = (p < 0 ? p + static_cast<PointCoordinateType>(2.0*M_PI) : p);
							if (k != minIndex)
							{
								if (minPhiCurrentLine > p)
									minPhiCurrentLine = p;
								else if (maxPhiCurrentLine < p)
									maxPhiCurrentLine = p;

								if (minPhiCurrentLineShifted > pShifted)
									minPhiCurrentLineShifted = pShifted;
								else if (maxPhiCurrentLineShifted < pShifted)
									maxPhiCurrentLineShifted = pShifted;
							}
							else
							{
								minPhiCurrentLine = maxPhiCurrentLine = p;
								minPhiCurrentLineShifted = maxPhiCurrentLineShifted = pShifted;
							}

							//find max range
							PointCoordinateType range = P.norm();
							if (range > maxRange)
								maxRange = range;
						}
					}

					if (minPhi > minPhiCurrentLine)
						minPhi = minPhiCurrentLine;
					if (maxPhi < maxPhiCurrentLine)
						maxPhi = maxPhiCurrentLine;

					if (minPhiShifted > minPhiCurrentLineShifted)
						minPhiShifted = minPhiCurrentLineShifted;
					if (maxPhiShifted < maxPhiCurrentLineShifted)
						maxPhiShifted = maxPhiCurrentLineShifted;

					unsigned span = maxIndex-minIndex+1;
					ScalarType angle_rad = static_cast<ScalarType>((maxPhiCurrentLine-minPhiCurrentLine) / span);
					angles.push_back(AngleAndSpan(angle_rad,span));

					ScalarType angleShifted_rad = static_cast<ScalarType>((maxPhiCurrentLineShifted-minPhiCurrentLineShifted) / span);
					anglesShifted.push_back(AngleAndSpan(angleShifted_rad,span));
				}

				_indexGrid += width;
			}

			if (!angles.empty())
			{
				//check the 'shifted' hypothesis
				PointCoordinateType spanShifted = maxPhiShifted - minPhiShifted;
//.........这里部分代码省略.........