void readOOBB(std::string filePath, Vector3& minP, Vector3& maxP, Matrix33& R_KI, Vector3List& pointList) { auto v = getPointsFromFile3D(filePath); if(v.size() != 5 + 8) { ApproxMVBB_ERRORMSG("Wrong number of points in OOBB file: " << filePath << " points: " << v.size()) } minP = v[0]; maxP = v[1]; R_KI.row(0) = v[2]; R_KI.row(1) = v[3]; R_KI.row(2) = v[4]; pointList.resize(8); for(unsigned int i = 0; i < 8; ++i) { pointList[i] = v[5 + i]; } }
Vector2List getPointsFromFile2D(std::string filePath) { std::ifstream file; // creates stream myFile file.open(filePath.c_str()); // opens .txt file if(!file.is_open()) { // check file is open, quit if not ApproxMVBB_ERRORMSG("Could not open file: " << filePath) } PREC a, b; Vector2List v; while(file.good()) { file >> a >> b; v.emplace_back(a, b); } file.close(); return v; }
void dumpOOBB(std::string filePath, const OOBB& oobb) { std::ofstream l; l << std::setprecision(12); l.open(filePath.c_str()); if(!l.good()) { ApproxMVBB_ERRORMSG("Could not open file: " << filePath << std::endl) } l << oobb.m_minPoint.transpose().format(MyMatrixIOFormat::SpaceSep) << std::endl; l << oobb.m_maxPoint.transpose().format(MyMatrixIOFormat::SpaceSep) << std::endl; l << oobb.m_q_KI.matrix().format(MyMatrixIOFormat::SpaceSep); // Export all corner points in I system auto points = oobb.getCornerPoints(); for(auto& p : points) { l << std::endl << p.transpose().format(MyMatrixIOFormat::SpaceSep); } l.close(); }
APPROXMVBB_EXPORT void samplePointsGrid(Matrix3Dyn & newPoints, const MatrixBase<Derived> & points, const unsigned int nPoints, OOBB & oobb) { if(nPoints >= points.cols() || nPoints < 2) { ApproxMVBB_ERRORMSG("Wrong arguements!") } newPoints.resize(3,nPoints); std::random_device rd; std::mt19937 gen(rd()); std::uniform_int_distribution<unsigned int> dis(0, points.cols()-1); //total points = bottomPoints=gridSize^2 + topPoints=gridSize^2 unsigned int gridSize = std::max( static_cast<unsigned int>( std::sqrt( static_cast<double>(nPoints) / 2.0 )) , 1U ); // Set z-Axis to longest dimension //std::cout << oobb.m_minPoint.transpose() << std::endl; oobb.setZAxisLongest(); unsigned int halfSampleSize = gridSize*gridSize; std::vector< std::pair<unsigned int , PREC > > topPoints(halfSampleSize, std::pair<unsigned int,PREC>{} ); // grid of indices of the top points (indexed from 1 ) std::vector< std::pair<unsigned int , PREC > > bottomPoints(halfSampleSize, std::pair<unsigned int,PREC>{} ); // grid of indices of the bottom points (indexed from 1 ) using LongInt = long long int; MyMatrix<LongInt>::Array2 idx; // Normalized P //std::cout << oobb.extent() << std::endl; //std::cout << oobb.m_minPoint.transpose() << std::endl; Array2 dxdyInv = Array2(gridSize,gridSize) / oobb.extent().head<2>(); // in K Frame; Vector3 K_p; Matrix33 A_KI(oobb.m_q_KI.matrix().transpose()); // Register points in grid auto size = points.cols(); for(unsigned int i=0; i<size; ++i) { K_p = A_KI * points.col(i); // get x index in grid idx = ( (K_p - oobb.m_minPoint).head<2>().array() * dxdyInv ).template cast<LongInt>(); // map to grid idx(0) = std::max( std::min( LongInt(gridSize-1), idx(0)), 0LL ); idx(1) = std::max( std::min( LongInt(gridSize-1), idx(1)), 0LL ); //std::cout << idx.transpose() << std::endl; unsigned int pos = idx(0) + idx(1)*gridSize; // Register points in grid // if z axis of p is > topPoints[pos] -> set new top point at pos // if z axis of p is < bottom[pos] -> set new bottom point at pos if( topPoints[pos].first == 0) { topPoints[pos].first = bottomPoints[pos].first = i+1; topPoints[pos].second = bottomPoints[pos].second = K_p(2); } else { if( topPoints[pos].second < K_p(2) ) { topPoints[pos].first = i+1; topPoints[pos].second = K_p(2); } if( bottomPoints[pos].second > K_p(2) ) { bottomPoints[pos].first = i+1; bottomPoints[pos].second = K_p(2); } } } // Copy top and bottom points unsigned int k=0; // k does not overflow -> 2* halfSampleSize = 2*gridSize*gridSize <= nPoints; for(unsigned int i=0; i<halfSampleSize; ++i) { if( topPoints[i].first != 0 ) { // comment in if you want the points top points of the grid // Array3 a(i % gridSize,i/gridSize,oobb.m_maxPoint(2)-oobb.m_minPoint(2)); // a.head<2>()*=dxdyInv.inverse(); newPoints.col(k++) = points.col(topPoints[i].first-1); // A_KI.transpose()*(oobb.m_minPoint + a.matrix()).eval() ; if(topPoints[i].first != bottomPoints[i].first) { // comment in if you want the bottom points of the grid // Array3 a(i % gridSize,i/gridSize,0); // a.head<2>()*=dxdyInv.inverse(); newPoints.col(k++) = points.col(bottomPoints[i].first-1); // A_KI.transpose()*(oobb.m_minPoint + a.matrix()).eval() ; } } } // Add random points! while( k < nPoints) { newPoints.col(k++) = points.col( dis(gen) ); //= Vector3(0,0,0);// } }