bool Octree::boxTriangleIntersection(BoundingBox Box, Triangle triangle)
{
//This algorithm is a composition of 13 tests to determine intersection
//if all pass, there is overlap
float min,max,d,p0,p1,p2,rad,fex,fey,fez;
//center the box at (0,0,0) with respect to the triangle
//translate triangle vertices
Vector v0,v1,v2;
//edges of new triangle
Vector e0,e1,e2;
//Vector that goes half the diagonal of the bounding box
Vector boxHalfSize = (Box.boxMaxExtent - Box.boxMinExtent) / 2.0;
point boxCenter( (Box.boxMinExtent.px + Box.boxMaxExtent.px)/2.0,
(Box.boxMinExtent.py + Box.boxMaxExtent.py)/2.0,
(Box.boxMinExtent.pz + Box.boxMaxExtent.pz)/2.0 );
/*v0.set(triangle.Vertex[0].px-boxCenter.px,triangle.Vertex[0].py-boxCenter.py,triangle.Vertex[0].pz-boxCenter.pz);
v1.set(triangle.Vertex[1].px-boxCenter.px,triangle.Vertex[1].py-boxCenter.py,triangle.Vertex[1].pz-boxCenter.pz);
v2.set(triangle.Vertex[2].px-boxCenter.px,triangle.Vertex[2].py-boxCenter.py,triangle.Vertex[2].pz-boxCenter.pz);*/
v0 = triangle.Vertex[0] - boxCenter;
v1 = triangle.Vertex[1] - boxCenter;
v2 = triangle.Vertex[2] - boxCenter;
e0 = v1 - v0;
e1 = v2 - v1;
e2 = v0 - v2;
//the first 9 tests
fex = fabs(e0.px);
fey = fabs(e0.py);
fez = fabs(e0.pz);
AXISTEST_X01(e0.pz, e0.py, fez, fey);
AXISTEST_Y02(e0.pz, e0.px, fez, fex);
AXISTEST_Z12(e0.py, e0.px, fey, fex);
fex = fabs(e1.px);
fey = fabs(e1.py);
fez = fabs(e1.pz);
AXISTEST_X01(e1.pz, e1.py, fez, fey);
AXISTEST_Y02(e1.pz, e1.px, fez, fex);
AXISTEST_Z0(e1.py, e1.px, fey, fex);
fex = fabs(e2.px);
fey = fabs(e2.py);
fez = fabs(e2.pz);
AXISTEST_X2(e2.pz, e2.py, fez, fey);
AXISTEST_Y1(e2.pz, e2.px, fez, fex);
AXISTEST_Z12(e2.py, e2.px, fey, fex);
//now the 3 tests
//X test
FINDMINMAX(v0.px,v1.px,v2.px,min,max);
if(min>boxHalfSize.px || max<-boxHalfSize.px)
return false;
//Y test
FINDMINMAX(v0.py,v1.py,v2.py,min,max);
if(min>boxHalfSize.py || max<-boxHalfSize.py)
return false;
//Z test
FINDMINMAX(v0.pz,v1.pz,v2.pz,min,max);
if(min>boxHalfSize.pz || max<-boxHalfSize.pz)
return false;
Vector normal = e0.crossProduct(e1);
d = -(normal.dotProduct(v0));
if(!planeBoxOverlap(normal,d,boxHalfSize))
return false;
return true;
}
void Octree::buildOctree(BoundingBox** Box)
{
//if we get below the threshold number of objects in a box, don't subdivide any longer
if ( (*Box)->Storage.size() <= maxObjectsPerBox )
return;
//(*Box)->childBoxes = new BoundingBox*[8];
for (int i=0; i < 8; i++)
{
(*Box)->childBoxes[i] = new BoundingBox;
}
int count = 0;
Vector displacement;
point boxCenter( ((*Box)->boxMinExtent.px + (*Box)->boxMaxExtent.px)/2.0,
((*Box)->boxMinExtent.py + (*Box)->boxMaxExtent.py)/2.0,
((*Box)->boxMinExtent.pz + (*Box)->boxMaxExtent.pz)/2.0 );
Vector diagonal = ((*Box)->boxMaxExtent - (*Box)->boxMinExtent) / 2.0;
//if not, subdivide box into eight equal octants
for (int z=0; z < 2; z++)
{
for (int y=0; y < 2; y++)
{
for (int x=0; x < 2; x++)
{
displacement.set(diagonal.px*(float)x, diagonal.py*(float)y, diagonal.pz*(float)z);
(*Box)->childBoxes[count++]->set((*Box)->boxMinExtent+displacement, boxCenter+displacement);
}
}
}
//now, see whether the children lie inside the child boxes
//this requires a triangle-box intersection and sphere-box intersection tests
for (int i=0; i < (*Box)->Storage.size(); i++)
{
for (int k=0; k < 8; k++)
{
if ( (*Box)->Storage[i]->getRadius() > 0.0 )
{
if ( boxSphereIntersection(*((*Box)->childBoxes[k]), *((*Box)->Storage[i])) )
{
//(*Box)->childBoxes[k]->numObjects++;
(*Box)->childBoxes[k]->Storage.push_back((*Box)->Storage[i]);
}
}
else
{
for (int j=0; j < (*Box)->Storage[i]->pNumTriangles; j++)
{
if ( boxTriangleIntersection(*((*Box)->childBoxes[k]), (*Box)->Storage[i]->Triangle_Array[j]) )
{
//add obj inc numObj, break
(*Box)->childBoxes[k]->Storage.push_back((*Box)->Storage[i]);
//(*Box)->childBoxes[k]->numObjects++;
break;
}
}
}
}
}
//now, recursively go to each of the children and see if they need to be subdivided
for (int i=0; i < 8; i++)
{
buildOctree(&((*Box)->childBoxes[i]));
}
}
bool CommandCrossSection::process(ccCommandLineInterface &cmd)
{
cmd.print("[CROSS SECTION]");
static QString s_xmlCloudCompare = "CloudCompare";
static QString s_xmlBoxThickness = "BoxThickness";
static QString s_xmlBoxCenter = "BoxCenter";
static QString s_xmlRepeatDim = "RepeatDim";
static QString s_xmlRepeatGap = "RepeatGap";
static QString s_xmlFilePath = "FilePath";
static QString s_outputXmlFilePath = "OutputFilePath";
//expected argument: XML file
if (cmd.arguments().empty())
return cmd.error(QString("Missing parameter: XML parameters file after \"-%1\"").arg(COMMAND_CROSS_SECTION));
QString xmlFilename = cmd.arguments().takeFirst();
//read the XML file
CCVector3 boxCenter(0, 0, 0), boxThickness(0, 0, 0);
bool repeatDim[3] = { false, false, false };
double repeatGap = 0.0;
bool inside = true;
bool autoCenter = true;
QString inputFilePath;
QString outputFilePath;
{
QFile file(xmlFilename);
if (!file.open(QFile::ReadOnly | QFile::Text))
{
return cmd.error(QString("Couldn't open XML file '%1'").arg(xmlFilename));
}
//read file content
QXmlStreamReader stream(&file);
//expected: CloudCompare
if (!stream.readNextStartElement()
|| stream.name() != s_xmlCloudCompare)
{
return cmd.error(QString("Invalid XML file (should start by '<%1>')").arg(s_xmlCloudCompare));
}
unsigned mandatoryCount = 0;
while (stream.readNextStartElement()) //loop over the elements
{
if (stream.name() == s_xmlBoxThickness)
{
QXmlStreamAttributes attributes = stream.attributes();
if (!readVector(attributes, boxThickness, s_xmlBoxThickness, cmd))
return false;
stream.skipCurrentElement();
++mandatoryCount;
}
else if (stream.name() == s_xmlBoxCenter)
{
QXmlStreamAttributes attributes = stream.attributes();
if (!readVector(attributes, boxCenter, s_xmlBoxCenter, cmd))
return false;
stream.skipCurrentElement();
autoCenter = false;
}
else if (stream.name() == s_xmlRepeatDim)
{
QString itemValue = stream.readElementText();
bool ok = false;
int dim = itemValue.toInt(&ok);
if (!ok || dim < 0 || dim > 2)
{
return cmd.error(QString("Invalid XML file (invalid value for '<%1>')").arg(s_xmlRepeatDim));
}
repeatDim[dim] = true;
}
else if (stream.name() == s_xmlRepeatGap)
{
QString itemValue = stream.readElementText();
bool ok = false;
repeatGap = itemValue.toDouble(&ok);
if (!ok)
{
return cmd.error(QString("Invalid XML file (invalid value for '<%1>')").arg(s_xmlRepeatGap));
}
}
else if (stream.name() == s_xmlFilePath)
{
inputFilePath = stream.readElementText();
if (!QDir(inputFilePath).exists())
{
return cmd.error(QString("Invalid file path (directory pointed by '<%1>' doesn't exist)").arg(s_xmlFilePath));
}
//++mandatoryCount;
}
else if (stream.name() == s_outputXmlFilePath)
{
outputFilePath = stream.readElementText();
if (!QDir(outputFilePath).exists())
{
return cmd.error(QString("Invalid output file path (directory pointed by '<%1>' doesn't exist)").arg(s_outputXmlFilePath));
}
//++mandatoryCount;
}
else
{
cmd.warning(QString("Unknown element: %1").arg(stream.name().toString()));
stream.skipCurrentElement();
}
}
if (mandatoryCount < 1 || (!repeatDim[0] && !repeatDim[1] && !repeatDim[2]))
{
return cmd.error(QString("Some mandatory elements are missing in the XML file (see documentation)"));
}
}
//safety checks
if ( boxThickness.x < ZERO_TOLERANCE
|| boxThickness.y < ZERO_TOLERANCE
|| boxThickness.z < ZERO_TOLERANCE
)
{
return cmd.error(QString("Invalid box thickness"));
}
CCVector3 repeatStep = boxThickness + CCVector3(repeatGap, repeatGap, repeatGap);
if ( (repeatDim[0] && repeatStep.x < ZERO_TOLERANCE)
|| (repeatDim[1] && repeatStep.y < ZERO_TOLERANCE)
|| (repeatDim[2] && repeatStep.z < ZERO_TOLERANCE)
)
{
return cmd.error(QString("Repeat gap can't be equal or smaller than 'minus' box width"));
}
if (outputFilePath.isEmpty())
{
outputFilePath = inputFilePath;
}
int iterationCount = 1;
//shall we load the entities?
QStringList files;
QDir dir;
bool fromFiles = false;
if (!inputFilePath.isEmpty())
{
//look for all files in the input directory
dir = QDir(inputFilePath);
assert(dir.exists());
files = dir.entryList(QDir::Files);
iterationCount = files.size();
fromFiles = true;
//remove any cloud or mesh in memory!
cmd.removeClouds();
cmd.removeMeshes();
}
for (int f = 0; f < iterationCount; ++f)
{
//shall we load files?
QString filename;
if (fromFiles)
{
assert(f < files.size());
filename = dir.absoluteFilePath(files[f]);
QFileInfo fileinfo(filename);
if (!fileinfo.isFile() || fileinfo.suffix().toUpper() == "XML")
{
continue;
}
//let's try to load the file
cmd.print(QString("Processing file: '%1'").arg(files[f]));
bool result = false;
{
//hack: replace the current argument list by a fake 'load file' sequence
QStringList realArguments = cmd.arguments();
QStringList loadArguments;
loadArguments << filename;
cmd.arguments() = loadArguments;
result = CommandLoad().process(cmd);
//end of hack: restore the current argument list
cmd.arguments() = realArguments;
}
if (!result)
{
cmd.warning("\tFailed to load file!");
continue;
}
}
else
{
assert(iterationCount == 1);
}
//repeat crop process on each file (or do it only once on the currently loaded entities)
{
ccHObject::Container entities;
try
{
for (size_t i = 0; i < cmd.clouds().size(); ++i)
entities.push_back(cmd.clouds()[i].pc);
for (size_t j = 0; j < cmd.meshes().size(); ++j)
entities.push_back(cmd.meshes()[j].mesh);
}
catch (const std::bad_alloc&)
{
return cmd.error("Not enough memory!");
}
for (size_t i = 0; i < entities.size(); ++i)
{
//check entity bounding-box
ccHObject* ent = entities[i];
ccBBox bbox = ent->getOwnBB();
if (!bbox.isValid())
{
cmd.warning(QString("Entity '%1' has an invalid bounding-box!").arg(ent->getName()));
continue;
}
//browse to/create a subdirectory with the (base) filename as name
QString basename;
if (fromFiles)
{
basename = QFileInfo(filename).baseName();
}
else
{
basename = i < cmd.clouds().size() ? cmd.clouds()[i].basename : cmd.meshes()[i - cmd.clouds().size()].basename;
}
if (entities.size() > 1)
basename += QString("_%1").arg(i + 1);
QDir outputDir(outputFilePath);
if (outputFilePath.isEmpty())
{
if (fromFiles)
{
assert(false);
outputDir = QDir::current();
}
else
{
outputDir = QDir(i < cmd.clouds().size() ? cmd.clouds()[i].path : cmd.meshes()[i - cmd.clouds().size()].path);
}
}
assert(outputDir.exists());
if (outputDir.cd(basename))
{
//if the directory already exists...
cmd.warning(QString("Subdirectory '%1' already exists").arg(basename));
}
else if (outputDir.mkdir(basename))
{
outputDir.cd(basename);
}
else
{
cmd.warning(QString("Failed to create subdirectory '%1' (check access rights and base name validity!)").arg(basename));
continue;
}
int toto = ceil(-0.4);
int toto2 = ceil(-0.6);
//place the initial box at the beginning of the entity bounding box
CCVector3 C0 = autoCenter ? bbox.getCenter() : boxCenter;
unsigned steps[3] = { 1, 1, 1 };
for (unsigned d = 0; d < 3; ++d)
{
if (repeatDim[d])
{
PointCoordinateType boxHalfWidth = boxThickness.u[d] / 2;
PointCoordinateType distToMinBorder = C0.u[d] - boxHalfWidth - bbox.minCorner().u[d];
int stepsToMinBorder = static_cast<int>(ceil(distToMinBorder / repeatStep.u[d]));
C0.u[d] -= stepsToMinBorder * repeatStep.u[d];
PointCoordinateType distToMaxBorder = bbox.maxCorner().u[d] - C0.u[d] - boxHalfWidth;
int stepsToMaxBoder = static_cast<int>(ceil(distToMaxBorder / repeatStep.u[d]) + 1);
assert(stepsToMaxBoder >= 0);
steps[d] = std::max<unsigned>(stepsToMaxBoder, 1);
}
}
cmd.print(QString("Will extract up to (%1 x %2 x %3) = %4 sections").arg(steps[0]).arg(steps[1]).arg(steps[2]).arg(steps[0] * steps[1] * steps[2]));
//now extract the slices
for (unsigned dx = 0; dx < steps[0]; ++dx)
{
for (unsigned dy = 0; dy < steps[1]; ++dy)
{
for (unsigned dz = 0; dz < steps[2]; ++dz)
{
CCVector3 C = C0 + CCVector3(dx*repeatStep.x, dy*repeatStep.y, dz*repeatStep.z);
ccBBox cropBox(C - boxThickness / 2, C + boxThickness / 2);
cmd.print(QString("Box (%1;%2;%3) --> (%4;%5;%6)")
.arg(cropBox.minCorner().x).arg(cropBox.minCorner().y).arg(cropBox.minCorner().z)
.arg(cropBox.maxCorner().x).arg(cropBox.maxCorner().y).arg(cropBox.maxCorner().z)
);
ccHObject* croppedEnt = ccCropTool::Crop(ent, cropBox, inside);
if (croppedEnt)
{
QString outputBasename = basename + QString("_%1_%2_%3").arg(C.x).arg(C.y).arg(C.z);
QString errorStr;
//original entity is a cloud?
if (i < cmd.clouds().size())
{
CLCloudDesc desc(static_cast<ccPointCloud*>(croppedEnt),
outputBasename,
outputDir.absolutePath(),
entities.size() > 1 ? static_cast<int>(i) : -1);
errorStr = cmd.exportEntity(desc);
}
else //otherwise it's a mesh
{
CLMeshDesc desc(static_cast<ccMesh*>(croppedEnt),
outputBasename,
outputDir.absolutePath(),
entities.size() > 1 ? static_cast<int>(i) : -1);
errorStr = cmd.exportEntity(desc);
}
delete croppedEnt;
croppedEnt = 0;
if (!errorStr.isEmpty())
return cmd.error(errorStr);
}
}
}
}
}
if (fromFiles)
{
//unload entities
cmd.removeClouds();
cmd.removeMeshes();
}
}
}
return true;
}
