C++ Vec3d::length2方法代码示例

void CameraController::updateBaseToWorldMatrix()
{
	Vec3d zAt = _testAt - _testBase;

	/* At point与Base point重合 */
	if (zAt.length2() < 1e-10)
	{
		_testBaseEyeToWorldMatrix = _testAtToWorldMatrix;
		_testBaseAtToWorldMatrix = _testAtToWorldMatrix;
		return;
	}

	Vec3d zEye = _testEye - _testBase;

	/* Eye point与Base point重合 */
	if (zEye.length2() < 1e-10)
	{
		_testBaseEyeToWorldMatrix = _testBaseEyeToWorldMatrix;
		
		Vec3d p1 = Vec3d(0, 0, 0);
		Vec3d p2 = Vec3d(1, 0, 0);
		p1 = switchCoordinateSystem_point(p1, _eyeToWorldMatrix, WORLD_MATRIX);	
		p2 = switchCoordinateSystem_point(p2, _eyeToWorldMatrix, WORLD_MATRIX);	

		Matrixd mat1, mat2, mat3, matFinal;
		mat1.makeTranslate(-p1);
		mat2.makeRotate(PI, p2-p1);
		mat3.makeTranslate(p1);
		matFinal = mat1 * mat2 * mat3;

		_testBaseAtToWorldMatrix = _testBaseEyeToWorldMatrix * matFinal;
		return;
	}

	Vec3d yEye;
	Vec3d yAt;
	if (inSameLine(_testEye, _testAt, _testBase)) // eye, at, base 三点共线
	{
		/* y轴以up向量为参照 */
		yEye = _testUp;
		yAt = _testUp;
	}
	else
	{
		/* y轴以视线为参照 */
		Vec3d sight = _testAt - _testEye;
		yEye = zEye ^ sight ^ zEye;
		yAt = zAt ^ sight ^ zAt;
	}

	_testBaseEyeToWorldMatrix = calcMatrixBasedOnAxis(_testBase, yEye, zEye);
	_testBaseAtToWorldMatrix = calcMatrixBasedOnAxis(_testBase, yAt, zAt);
}

 void add_point(const Vec3d& point){
     uchar b = ~bits_; //Flip bits
     b &= -b; //Last set (available) bit
     uchar pos = s_pos[b]; //Get the bit position from the lookup table
     last_sb_ = pos;
     bits_ |= b; //Insert the new bit
     ++size_;
     p_[pos] = point;
     double l2 = point.length2();
     if(l2 > max_vert2) max_vert2 = l2;
 }

bool PoissonFillaar::satisfyPoissonDiskCriterion(const Vec3d& p) const
{
	// Declarations
	double r = _resolution;
	double r2 = r*r;
	
	const std::deque<Vec3d>& points = getPoints();
	
	// Test cell containing point
	int pIndex = getGridIndex(p);
	bool doesNotSatisfyCriterion = false;
	if (_grid[pIndex] < 0)
	{
		// Test neighbor cells
		int xIndex = getGridXIndex(pIndex);
		int yIndex = getGridYIndex(pIndex);
		int startX = getGridXIndex(p.x-_resolution); if (startX < 0) { startX = 0; }
		int startY = getGridYIndex(p.y-_resolution); if (startY < 0) { startY = 0; }
		int endX = getGridXIndex(p.x+_resolution); if (endX >(_gridResX-1)) { endX = _gridResX-1; }
		int endY = getGridYIndex(p.y+_resolution); if (endY >(_gridResY-1)) { endY = _gridResY-1; }
		for (int ix=startX; !doesNotSatisfyCriterion && (ix<=endX); ++ix)
		{
			for (int iy=startY; !doesNotSatisfyCriterion && (iy<=endY); ++iy)
			{
				if (ix!=xIndex || iy!=yIndex)
				{
					int neighborIndex = getGridIndex(ix, iy);
					int neighborID = _grid[neighborIndex];
					if (neighborID > -1)
					{
						Vec3d delta = p;
						delta -= points[neighborID];
						
						if (delta.length2() < r2)
						{
							doesNotSatisfyCriterion = true;
						}
					}
				}
			}
		}
	}
	else
	{
		doesNotSatisfyCriterion = true;
	}
	
	return !doesNotSatisfyCriterion;
}

bool RayIntersector::intersects(const BoundingSphere& bs)
{
    // if bs not valid then return false based on the assumption that the node is empty.
    if (!bs.valid()) return false;

    // test for _start inside the bounding sphere
    Vec3d sm = _start - bs._center;
    double c = sm.length2() - bs._radius * bs._radius;
    if (c<0.0) return true;

    // solve quadratic equation
    double a = _direction.length2();
    double b = (sm * _direction) * 2.0;
    double d = b * b - 4.0 * a * c;

    // no intersections if d<0
    if (d<0.0) return false;

    // compute two solutions of quadratic equation
    d = sqrt(d);
    double div = 1.0/(2.0*a);
    double r1 = (-b-d)*div;
    double r2 = (-b+d)*div;

    // return false if both intersections are before the ray start
    if (r1<=0.0 && r2<=0.0) return false;

    // if LIMIT_NEAREST and closest point of bounding sphere is further than already found intersection, return false
    if (_intersectionLimit == LIMIT_NEAREST && !getIntersections().empty())
    {
        double minDistance = sm.length() - bs._radius;
        if (minDistance >= getIntersections().begin()->distance) return false;
    }

    // passed all the rejection tests so line must intersect bounding sphere, return true.
    return true;
}

/** Make a rotation Quat which will rotate vec1 to vec2

This routine uses only fast geometric transforms, without costly acos/sin computations.
It's exact, fast, and with less degenerate cases than the acos/sin method.

For an explanation of the math used, you may see for example:
http://logiciels.cnes.fr/MARMOTTES/marmottes-mathematique.pdf

@note This is the rotation with shortest angle, which is the one equivalent to the
acos/sin transform method. Other rotations exists, for example to additionally keep
a local horizontal attitude.

@author Nicolas Brodu
*/
void Quat::makeRotate( const Vec3d& from, const Vec3d& to )
{

    // This routine takes any vector as argument but normalized
    // vectors are necessary, if only for computing the dot product.
    // Too bad the API is that generic, it leads to performance loss.
    // Even in the case the 2 vectors are not normalized but same length,
    // the sqrt could be shared, but we have no way to know beforehand
    // at this point, while the caller may know.
    // So, we have to test... in the hope of saving at least a sqrt
    Vec3d sourceVector = from;
    Vec3d targetVector = to;

    value_type fromLen2 = from.length2();
    value_type fromLen;
    // normalize only when necessary, epsilon test
    if ((fromLen2 < 1.0-1e-7) || (fromLen2 > 1.0+1e-7)) {
        fromLen = sqrt(fromLen2);
        sourceVector /= fromLen;
    } else fromLen = 1.0;

    value_type toLen2 = to.length2();
    // normalize only when necessary, epsilon test
    if ((toLen2 < 1.0-1e-7) || (toLen2 > 1.0+1e-7)) {
        value_type toLen;
        // re-use fromLen for case of mapping 2 vectors of the same length
        if ((toLen2 > fromLen2-1e-7) && (toLen2 < fromLen2+1e-7)) {
            toLen = fromLen;
        }
        else toLen = sqrt(toLen2);
        targetVector /= toLen;
    }


    // Now let's get into the real stuff
    // Use "dot product plus one" as test as it can be re-used later on
    double dotProdPlus1 = 1.0 + sourceVector * targetVector;

    // Check for degenerate case of full u-turn. Use epsilon for detection
    if (dotProdPlus1 < 1e-7) {

        // Get an orthogonal vector of the given vector
        // in a plane with maximum vector coordinates.
        // Then use it as quaternion axis with pi angle
        // Trick is to realize one value at least is >0.6 for a normalized vector.
        if (fabs(sourceVector.x()) < 0.6) {
            const double norm = sqrt(1.0 - sourceVector.x() * sourceVector.x());
            _v[0] = 0.0;
            _v[1] = sourceVector.z() / norm;
            _v[2] = -sourceVector.y() / norm;
            _v[3] = 0.0;
        } else if (fabs(sourceVector.y()) < 0.6) {
            const double norm = sqrt(1.0 - sourceVector.y() * sourceVector.y());
            _v[0] = -sourceVector.z() / norm;
            _v[1] = 0.0;
            _v[2] = sourceVector.x() / norm;
            _v[3] = 0.0;
        } else {
            const double norm = sqrt(1.0 - sourceVector.z() * sourceVector.z());
            _v[0] = sourceVector.y() / norm;
            _v[1] = -sourceVector.x() / norm;
            _v[2] = 0.0;
            _v[3] = 0.0;
        }
    }

    else {
        // Find the shortest angle quaternion that transforms normalized vectors
        // into one other. Formula is still valid when vectors are colinear
        const double s = sqrt(0.5 * dotProdPlus1);
        const Vec3d tmp = sourceVector ^ targetVector / (2.0*s);
        _v[0] = tmp.x();
        _v[1] = tmp.y();
        _v[2] = tmp.z();
        _v[3] = s;
    }
}

void PoissonFillaar::relaxation(const Volume* v, const std::vector<int>& pointsToRelax, int nbSweeps, bool alwaysProjectToSurface)
{
	// Declarations
	double r = _resolution;
	double twoR = 2.0*r;
    double twoR2 = twoR*twoR;
	double randMax = static_cast<double>(RAND_MAX);
	double PI = 3.14159265;
	double twoPI = 2.0*PI;
	
	std::deque<Vec3d>& points = getPoints();
	std::vector<int> neighbors;

	// Relax those tense points ;)
	for (int k=0; k<nbSweeps; ++k)
	{
		std::vector<int>::const_iterator it = pointsToRelax.begin();
		std::vector<int>::const_iterator itEnd = pointsToRelax.end();
		for (; it!=itEnd; ++it)
		{
			Vec3d *p = &points[*it];
			
			neighbors.clear();
			
			// Find neighbors within 2.0*r of p and compute
			// the smallest distance between p and its neighbors
            double dmin2 = 100000000000000000.0;
			int startX = getGridXIndex(p->x - twoR); if (startX<0) startX = 0;
			int startY = getGridYIndex(p->y - twoR); if (startY<0) startY = 0;
			int endX = getGridXIndex(p->x + twoR); if (endX>=_gridResX) endX = _gridResX-1;
			int endY = getGridYIndex(p->y + twoR); if (endY>=_gridResY) endY = _gridResY-1;
			bool hasNeighbors = false;
			for (int ix=startX; ix<=endX; ++ix)
			{
				for (int iy=startY; iy<=endY; ++iy)
				{
					int id = _grid[getGridIndex(ix, iy)];
					if ((id < 0) || (id==(*it)))
						continue;
					
					Vec3d delta = points[id];
					delta -= *p;
                    double dist2 = delta.length2();
                    if (dist2 <= twoR2)
					{
						hasNeighbors = true;
						neighbors.push_back(id);
                        if (dist2<dmin2)
						{
                            dmin2 = dist2;
						}
					}
				}
			}
			
			if (!hasNeighbors)
			{
				continue;
			}
			
			// Try to find a better point (a point further from its nearest neighbors)
			Vec3d pnew = *p;
			double tmax = 100;
			for (int t=0; t<tmax; ++t)
			{
				double tau = static_cast<double>(tmax-t)/static_cast<double>(tmax);
				
				// Generate a random point around p
				double randTheta = (static_cast<double>(rand())/randMax) * twoPI;
				Vec3d pcand(cos(randTheta),sin(randTheta),0.0);
				pcand *= r*tau;
				pcand += *p;
				
				// Project point onto surface if necessary
				if (alwaysProjectToSurface || !v->pointIsInside(pcand))
				{
					Vec3d proj;
					Vec3d normal;
					v->projectPointOntoSurface(pcand, proj, normal);
					pcand = proj;
				}
				
				// Get the smallest distance between pcand and its neighbors
				int nbNeighbors = neighbors.size();
                double dminCand2 = 100000000000000000.0;
				for (int i=0; i<nbNeighbors; ++i)
				{
					Vec3d delta = points[neighbors[i]];
					delta -= pcand;
                    double dist2 = delta.length();
                    if (dist2<dminCand2)
					{
                        dminCand2 = dist2;
					}
				}
				
                if (dminCand2 > dmin2)
				{
					pnew = pcand;
                    dmin2 = dminCand2;
//.........这里部分代码省略.........

void CameraController::testTranslateCamera(const Vec3d& vec, CoordType coordType, PointType pointType,
										   Vec3d& resEye, Vec3d& resAt, Vec3d& resBase)
{
	assert(_camera != NULL);

	_testEye = _currEye;
	_testAt = _currAt;
	_testBase = _currBase;
	_testUp = _currUp;

	if (vec.length2() < 1e-10) { return; }

	/* 把位移向量转换到世界坐标系下 */

	Vec3d tmpVec;
	switch (coordType)
	{
	case COORD_TYPE_WORLD:
		tmpVec = vec;
		break;

	case COORD_TYPE_EYE:
		tmpVec = switchCoordinateSystem_vector(vec, _eyeToWorldMatrix, WORLD_MATRIX);		
		break;

	case COORD_TYPE_AT:
		tmpVec = switchCoordinateSystem_vector(vec, _atToWorldMatrix, WORLD_MATRIX);
		break;

	case COORD_TYPE_BASE_EYE:
	case COORD_TYPE_BASE_EYE_2:
		tmpVec = switchCoordinateSystem_vector(vec, _baseEyeToWorldMatrix, WORLD_MATRIX);		
		break;

	case COORD_TYPE_BASE_AT:
	case COORD_TYPE_BASE_AT_2:
		tmpVec = switchCoordinateSystem_vector(vec, _baseAtToWorldMatrix, WORLD_MATRIX);		
		break;
	}

	/* 移动指定点 */
	Matrixd mat;
	mat.makeTranslate(tmpVec);

	switch (pointType)
	{
	case EYE_POINT:
		_testEye = _currEye * mat;
		_testUp = calcUp(_currAt - _currEye, _testAt - _testEye, _currUp);
		updateEyeAndAtToWorldMatrix();
		updateBaseToWorldMatrix();
		break;

	case AT_POINT:
		_testAt = _currAt * mat;
		_testUp = calcUp(_currAt - _currEye, _testAt - _testEye, _currUp);
		updateEyeAndAtToWorldMatrix();
		updateBaseToWorldMatrix();
		break;

	case BASE_POINT:
		_testBase = _currBase * mat;
		updateBaseToWorldMatrix();
		break;

	case EYE_POINT_AND_AT_POINT:
		_testEye = _currEye * mat;
		_testAt = _currAt * mat;
		_testEyeToWorldMatrix = _eyeToWorldMatrix * mat;
		_testAtToWorldMatrix = _atToWorldMatrix * mat;
		updateBaseToWorldMatrix();
		break;

	case ALL_POINTS:
		_testEye = _currEye * mat;
		_testAt = _currAt * mat;
		_testBase = _currBase * mat;
		_testEyeToWorldMatrix = _eyeToWorldMatrix * mat;
		_testAtToWorldMatrix = _atToWorldMatrix * mat;
		_testBaseEyeToWorldMatrix = _baseEyeToWorldMatrix * mat;
		_testBaseAtToWorldMatrix = _baseAtToWorldMatrix * mat;
		break;
	}

	resEye = _testEye;
	resAt = _testAt;
	resBase = _testBase;
}