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);//
}
}
