envire::Map<3>::Extents MapSegment::getExtents() const
{
// combine the extents of the children of this map
Eigen::AlignedBox<double, 3> extents;
for( std::vector<Part>::const_iterator it = parts.begin(); it != parts.end(); it++ )
{
const envire::CartesianMap *map = it->map.get();
const Map<2>* map2 = dynamic_cast<const Map<2>*>( map );
const Map<3>* map3 = dynamic_cast<const Map<3>*>( map );
assert( this->isAttached() );
assert( map->isAttached() );
Eigen::Affine3d g2m = map->getFrameNode()->relativeTransform( this->getFrameNode() );
// currently this assumes that child grids are not rotated
// TODO handle rotated child grids here
if( map3 )
{
extents.extend( map3->getExtents().translate( g2m.translation() ) );
}
else if( map2 )
{
Eigen::AlignedBox<double, 3> e3;
e3.min().head<2>() = map2->getExtents().min();
e3.max().head<2>() = map2->getExtents().max();
extents.extend( e3.translate( g2m.translation() ) );
}
}
return extents;
}
Eigen::AlignedBox<double, 2> MLSMap::getExtents() const
{
// combine the extents of the children of this map
Eigen::AlignedBox<double, 2> extents;
std::list<const Layer*> children = env->getChildren( this );
for( std::list<const Layer*>::const_iterator it = children.begin(); it != children.end(); it++ )
{
const envire::MLSGrid *grid = dynamic_cast<const envire::MLSGrid*>( *it );
// currently this assumes that child grids are not rotated
// TODO handle rotated child grids here
Eigen::Affine3d g2m = grid->getFrameNode()->relativeTransform( this->getFrameNode() );
extents.extend( grid->getExtents().translate( g2m.translation().head<2>() ) );
}
return extents;
}
void igl::copyleft::offset_surface(
const Eigen::MatrixBase<DerivedV> & V,
const Eigen::MatrixBase<DerivedF> & F,
const isolevelType isolevel,
const typename Derivedside::Scalar s,
const SignedDistanceType & signed_distance_type,
Eigen::PlainObjectBase<DerivedSV> & SV,
Eigen::PlainObjectBase<DerivedSF> & SF,
Eigen::PlainObjectBase<DerivedGV> & GV,
Eigen::PlainObjectBase<Derivedside> & side,
Eigen::PlainObjectBase<DerivedS> & S)
{
typedef typename DerivedV::Scalar Scalar;
typedef typename DerivedF::Scalar Index;
{
Eigen::AlignedBox<Scalar,3> box;
typedef Eigen::Matrix<Scalar,1,3> RowVector3S;
assert(V.cols() == 3 && "V must contain positions in 3D");
RowVector3S min_ext = V.colwise().minCoeff().array() - isolevel;
RowVector3S max_ext = V.colwise().maxCoeff().array() + isolevel;
box.extend(min_ext.transpose());
box.extend(max_ext.transpose());
igl::voxel_grid(box,s,1,GV,side);
}
const Scalar h =
(GV.col(0).maxCoeff()-GV.col(0).minCoeff())/((Scalar)(side(0)-1));
const Scalar lower_bound = isolevel-sqrt(3.0)*h;
const Scalar upper_bound = isolevel+sqrt(3.0)*h;
{
Eigen::Matrix<Index,Eigen::Dynamic,1> I;
Eigen::Matrix<typename DerivedV::Scalar,Eigen::Dynamic,3> C,N;
igl::signed_distance(
GV,V,F,signed_distance_type,lower_bound,upper_bound,S,I,C,N);
}
igl::flood_fill(side,S);
DerivedS SS = S.array()-isolevel;
igl::copyleft::marching_cubes(SS,GV,side(0),side(1),side(2),SV,SF);
}
bool load_frame( int frame_num, bool &stop ){
int n_meshes = meshes.size();
int n_obs = obstacles.size();
// Winding order matters for whatever is reading in the abc file.
bool ccw_output = false;
std::string framestr = pad_leading_zeros(6,frame_num);
// Load all deforming meshes
for( int m=0; m<n_meshes; ++m ){
std::string meshstr = pad_leading_zeros(2,m);
std::string objfile = dir + framestr + '_' + meshstr + ".obj";
// If the file doesn't exists, we have reached the end of the sim
if( !file_exists(objfile) ){
stop = true;
break;
}
// Load the deforming mesh
TriangleMesh *mesh = meshes[m].get();
if( !meshio::load_obj( mesh, objfile, false, false, false ) ){
std::cerr << "\n**arcsimeToAlembic Error: Failed to load " << objfile << "\n" << std::endl;
return false;
}
mesh->need_normals();
// Add it to the exporter
exporter.add_frame( deform_handles[m], &mesh->vertices[0][0], mesh->vertices.size(),
&mesh->faces[0][0], mesh->faces.size(), ccw_output );
} // end loop meshes
// Load all obstacle transforms
for( int o=0; o<n_obs && !stop; ++o ){
std::string meshstr = pad_leading_zeros(2,o);
std::string xformfile = dir + framestr + "obs" + meshstr + ".txt";
pugi::xml_document doc;
pugi::xml_parse_result result = doc.load_file(xformfile.c_str());
if( !result ){
std::cerr << "\n**arcsimeToAlembic Error: Unable to load " << xformfile << std::endl;
return false;
}
TriangleMesh *mesh = obstacles[o].get();
Eigen::AlignedBox<float,3> aabb = obs_aabb[o];
Vec3f obs_center = aabb.center();
pugi::xml_node::iterator node_iter = doc.first_child();
for( ; node_iter != doc.end(); node_iter++ ){
pugi::xml_node curr_node = *node_iter;
std::string name = curr_node.name();
XForm<float> xf; xf.setIdentity();
if( name == "rotate" ){
float angle = curr_node.attribute("angle").as_float() * (180.f/M_PI);
float x = curr_node.attribute("x").as_float();
float y = curr_node.attribute("y").as_float();
float z = curr_node.attribute("z").as_float();
// Translate obs to origin before rotation
XForm<float> xf0 = xform::make_trans<float>(-obs_center);
XForm<float> xf1 = xform::make_rot<float>( angle, Vec3f(x,y,z) );
XForm<float> xf2 = xform::make_trans<float>(obs_center);
xf = xf2 * xf1 * xf0;
}
else if( name == "scale" ){
float s = curr_node.attribute("value").as_float();
xf = xform::make_scale<float>(s,s,s);
}
else if( name == "translate" ){
float x = curr_node.attribute("x").as_float();
float y = curr_node.attribute("y").as_float();
float z = curr_node.attribute("z").as_float();
xf = xform::make_trans<float>(x,y,z);
}
mesh->apply_xform(xf);
} // end load xform
// Add it to the exporter
exporter.add_frame( obs_handles[o], &mesh->vertices[0][0], mesh->vertices.size(),
&mesh->faces[0][0], mesh->faces.size(), ccw_output );
} // end loop obstacles
return true;
}
IGL_INLINE bool igl::ray_box_intersect(
const Eigen::PlainObjectBase<Derivedsource> & origin,
const Eigen::PlainObjectBase<Deriveddir> & dir,
const Eigen::AlignedBox<Scalar,3> & box,
const Scalar & t0,
const Scalar & t1,
Scalar & tmin,
Scalar & tmax)
{
#ifdef false
// https://github.com/RMonica/basic_next_best_view/blob/master/src/RayTracer.cpp
const auto & intersectRayBox = [](
const Eigen::Vector3f& rayo,
const Eigen::Vector3f& rayd,
const Eigen::Vector3f& bmin,
const Eigen::Vector3f& bmax,
float & tnear,
float & tfar
)->bool
{
Eigen::Vector3f bnear;
Eigen::Vector3f bfar;
// Checks for intersection testing on each direction coordinate
// Computes
float t1, t2;
tnear = -1e+6f, tfar = 1e+6f; //, tCube;
bool intersectFlag = true;
for (int i = 0; i < 3; ++i) {
// std::cout << "coordinate " << i << ": bmin " << bmin(i) << ", bmax " << bmax(i) << std::endl;
assert(bmin(i) <= bmax(i));
if (::fabs(rayd(i)) < 1e-6) { // Ray parallel to axis i-th
if (rayo(i) < bmin(i) || rayo(i) > bmax(i)) {
intersectFlag = false;
}
}
else {
// Finds the nearest and the farthest vertices of the box from the ray origin
if (::fabs(bmin(i) - rayo(i)) < ::fabs(bmax(i) - rayo(i))) {
bnear(i) = bmin(i);
bfar(i) = bmax(i);
}
else {
bnear(i) = bmax(i);
bfar(i) = bmin(i);
}
// std::cout << " bnear " << bnear(i) << ", bfar " << bfar(i) << std::endl;
// Finds the distance parameters t1 and t2 of the two ray-box intersections:
// t1 must be the closest to the ray origin rayo.
t1 = (bnear(i) - rayo(i)) / rayd(i);
t2 = (bfar(i) - rayo(i)) / rayd(i);
if (t1 > t2) {
std::swap(t1,t2);
}
// The two intersection values are used to saturate tnear and tfar
if (t1 > tnear) {
tnear = t1;
}
if (t2 < tfar) {
tfar = t2;
}
// std::cout << " t1 " << t1 << ", t2 " << t2 << ", tnear " << tnear << ", tfar " << tfar
// << " tnear > tfar? " << (tnear > tfar) << ", tfar < 0? " << (tfar < 0) << std::endl;
if(tnear > tfar) {
intersectFlag = false;
}
if(tfar < 0) {
intersectFlag = false;
}
}
}
// Checks whether intersection occurs or not
return intersectFlag;
};
float tmin_f, tmax_f;
bool ret = intersectRayBox(
origin. template cast<float>(),
dir. template cast<float>(),
box.min().template cast<float>(),
box.max().template cast<float>(),
tmin_f,
tmax_f);
tmin = tmin_f;
tmax = tmax_f;
return ret;
#else
using namespace Eigen;
// This should be precomputed and provided as input
typedef Matrix<Scalar,1,3> RowVector3S;
const RowVector3S inv_dir( 1./dir(0),1./dir(1),1./dir(2));
const std::vector<bool> sign = { inv_dir(0)<0, inv_dir(1)<0, inv_dir(2)<0};
// http://people.csail.mit.edu/amy/papers/box-jgt.pdf
// "An Efficient and Robust Ray–Box Intersection Algorithm"
Scalar tymin, tymax, tzmin, tzmax;
std::vector<RowVector3S> bounds = {box.min(),box.max()};
tmin = ( bounds[sign[0]](0) - origin(0)) * inv_dir(0);
tmax = ( bounds[1-sign[0]](0) - origin(0)) * inv_dir(0);
tymin = (bounds[sign[1]](1) - origin(1)) * inv_dir(1);
tymax = (bounds[1-sign[1]](1) - origin(1)) * inv_dir(1);
if ( (tmin > tymax) || (tymin > tmax) )
{
return false;
}
if (tymin > tmin)
{
tmin = tymin;
}
if (tymax < tmax)
{
tmax = tymax;
}
tzmin = (bounds[sign[2]](2) - origin(2)) * inv_dir(2);
tzmax = (bounds[1-sign[2]](2) - origin(2)) * inv_dir(2);
if ( (tmin > tzmax) || (tzmin > tmax) )
{
return false;
}
if (tzmin > tmin)
{
tmin = tzmin;
}
if (tzmax < tmax)
{
tmax = tzmax;
}
if(!( (tmin < t1) && (tmax > t0) ))
{
return false;
}
return true;
#endif
}