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; } } // Update p! *p = pnew; } } }
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; }