vector<Real> DemFuncs::contactCoordQuantiles(const shared_ptr<DemField>& dem, const vector<Real>& quantiles, const shared_ptr<Node>& node, const AlignedBox3r& box){
vector<Real> ret;
ret.reserve(quantiles.size());
if(quantiles.empty()) return ret;
vector<Real> coords;
// count contacts first
// this count will be off when range is given; it will be larger, so copying is avoided at the cost of some spurious allocation
size_t N;
#if 0
N=boost::range::count_if(*dem->contacts,[&](const shared_ptr<Contact>& C)->bool{ return C->isReal(); });
coords.reserve(N);
#endif
for(const shared_ptr<Contact>& C: *dem->contacts){
if(!C->isReal()) continue;
Vector3r p=node?node->glob2loc(C->geom->node->pos):C->geom->node->pos;
if(!box.isEmpty() && !box.contains(p)) continue;
coords.push_back(p[2]);
};
// no contacts yet, return NaNs
if(coords.empty()){
for(size_t i=0; i<quantiles.size(); i++) ret.push_back(NaN);
return ret;
}
boost::range::sort(coords);
N=coords.size(); // should be the same as N before
for(Real q: quantiles){
// clamp the value to 0,1
q=min(max(0.,q),1.);
if(isnan(q)) q=0.; // just to make sure
int i=min((int)((N-1)*q),(int)N-1);
ret.push_back(coords[i]);
}
return ret;
}
AlignedBox3r Field::renderingBbox() const {
AlignedBox3r b;
for(const shared_ptr<Node>& n: nodes){
if(!n) continue;
b.extend(n->pos);
}
return b;
}
Real DemFuncs::porosity(const shared_ptr<DemField>& dem, const shared_ptr<Node>& node, const AlignedBox3r& box){
Real Vs=0; // solid volume
Real V=box.volume(); // box volume
for(const auto& p: *dem->particles){
if(!p->shape->isA<Sphere>()) continue;
const Vector3r& pos=p->shape->nodes[0]->pos;
if(!box.contains(node?node->glob2loc(pos):pos)) continue;
Vs+=(4/3.)*M_PI*pow(p->shape->cast<Sphere>().radius,3);
}
return Vs/V;
}
vector<Vector2r> DemFuncs::boxPsd(const Scene* scene, const DemField* dem, const AlignedBox3r& box, bool mass, int num, int mask, Vector2r rRange){
bool haveBox=!isnan(box.min()[0]) && !isnan(box.max()[0]);
return psd(
*dem->particles|boost::adaptors::filtered([&](const shared_ptr<Particle>&p){ return p && p->shape && p->shape->nodes.size()==1 && (mask?(p->mask&mask):true) && (bool)(dynamic_pointer_cast<woo::Sphere>(p->shape)) && (haveBox?box.contains(p->shape->nodes[0]->pos):true); }),
/*cumulative*/true,/*normalize*/true,
num,
rRange,
/*diameter getter*/[](const shared_ptr<Particle>&p) ->Real { return 2.*p->shape->cast<Sphere>().radius; },
/*weight getter*/[&](const shared_ptr<Particle>&p) -> Real{ return mass?p->shape->nodes[0]->getData<DemData>().mass:1.; }
);
}
bool ArcInlet::validateBox(const AlignedBox3r& b) {
for(const auto& c:{AlignedBox3r::BottomLeftFloor,AlignedBox3r::BottomRightFloor,AlignedBox3r::TopLeftFloor,AlignedBox3r::TopRightFloor,AlignedBox3r::BottomLeftCeil,AlignedBox3r::BottomRightCeil,AlignedBox3r::TopLeftCeil,AlignedBox3r::TopRightCeil}){
// FIXME: all boxes fail?!
if(!CompUtils::cylCoordBox_contains_cartesian(cylBox,node->glob2loc(b.corner(c)))) return false;
}
return true;
}
bool BoxInlet::validatePeriodicBox(const AlignedBox3r& b) const {
if(periSpanMask==0) return box.contains(b);
// otherwise just enlarge our box in all directions
AlignedBox3r box2(box);
for(int i:{0,1,2}){
if(periSpanMask&(1<<i)) continue;
Real extra=b.sizes()[i]/2.;
box2.min()[i]-=extra; box2.max()[i]+=extra;
}
return box2.contains(b);
}
bool CylinderInlet::validateBox(const AlignedBox3r& b) {
if(!node) throw std::runtime_error("CylinderInlet.node==None.");
/* check all corners are inside the cylinder */
#if 0
boxesTried.push_back(b);
#endif
for(AlignedBox3r::CornerType c:{AlignedBox3r::BottomLeft,AlignedBox3r::BottomRight,AlignedBox3r::TopLeft,AlignedBox3r::TopRight,AlignedBox3r::BottomLeftCeil,AlignedBox3r::BottomRightCeil,AlignedBox3r::TopLeftCeil,AlignedBox3r::TopRightCeil}){
Vector3r p=node->glob2loc(b.corner(c));
if(p[0]<0. || p[0]>height) return false;
if(Vector2r(p[1],p[2]).squaredNorm()>pow(radius,2)) return false;
}
return true;
}
/* See https://yade-dem.org/doc/yade.utils.html#yade.utils.avgNumInteractions for docs */
Real DemFuncs::coordNumber(const shared_ptr<DemField>& dem, const shared_ptr<Node>& node, const AlignedBox3r& box, int mask, bool skipFree){
long C2=0; // twice the number of contacts (counted for each participating particle)
long N0=0; // number of particles without contact (floaters)
long N1=0; // number of particles with one contact (rattlers)
long N=0; // number of all particles
for(const auto& p: *dem->particles){
const Vector3r& pos=p->shape->nodes[0]->pos;
if(mask && (mask&p->mask)==0) continue;
if(p->shape->nodes[0]->getData<DemData>().isClumped()) throw std::runtime_error("Not yet implemented for clumps.");
if(!box.contains(node?node->glob2loc(pos):pos)) continue;
int n=p->countRealContacts();
if(n==0) N0++;
else if(n==1) N1++;
N++;
C2+=n;
}
if(skipFree) return (C2-N1)*1./(N-N0-N1);
else return C2*1./N;
}
// grid sampling
Matrix3r woo::Volumetric::tetraInertia_grid(const Vector3r v[4], int div){
AlignedBox3r b; for(int i:{0,1,2,3}) b.extend(v[i]);
std::cerr<<"bbox "<<b.min()<<", "<<b.max()<<std::endl;
Real dd=b.sizes().minCoeff()/div;
Vector3r xyz;
// point inside test: http://steve.hollasch.net/cgindex/geometry/ptintet.html
typedef Eigen::Matrix<Real,4,4> Matrix4r;
Matrix4r M0; M0<<v[0].transpose(),1,v[1].transpose(),1,v[2].transpose(),1,v[3].transpose(),1;
Real D0=M0.determinant();
// Matrix3r I(Matrix3r::Zero());
Matrix3r C(Matrix3r::Zero());
Real dV=pow(dd,3);
// std::ofstream dbg("/tmp/tetra.txt");
for(xyz.x()=b.min().x()+dd/2.; xyz.x()<b.max().x(); xyz.x()+=dd){
for(xyz.y()=b.min().y()+dd/2.; xyz.y()<b.max().y(); xyz.y()+=dd){
for(xyz.z()=b.min().z()+dd/2.; xyz.z()<b.max().z(); xyz.z()+=dd){
bool inside=true;
for(int i:{0,1,2,3}){
Matrix4r D=M0;
D.row(i).head<3>()=xyz;
if(std::signbit(D.determinant())!=std::signbit(D0)){ inside=false; break; }
}
if(inside){
C+=dV*(xyz*xyz.transpose());
// dbg<<xyz[0]<<" "<<xyz[1]<<" "<<xyz[2]<<" "<<dd/2.<<endl;
}
}
}
}
return Matrix3r::Identity()*C.trace()-C;
}
void SphereClumpGeom::recompute(int _div, bool failOk, bool fastOnly){
if((centers.empty() && radii.empty()) || centers.size()!=radii.size()){
if(failOk) { makeInvalid(); return;}
throw std::runtime_error("SphereClumpGeom.recompute: centers and radii must have the same length (len(centers)="+to_string(centers.size())+", len(radii)="+to_string(radii.size())+"), and may not be empty.");
}
div=_div;
// one single sphere: simple
if(centers.size()==1){
pos=centers[0];
ori=Quaternionr::Identity();
volume=(4/3.)*M_PI*pow(radii[0],3);
inertia=Vector3r::Constant((2/5.)*volume*pow(radii[0],2));
equivRad=radii[0];
return;
}
volume=0;
Vector3r Sg=Vector3r::Zero();
Matrix3r Ig=Matrix3r::Zero();
if(_div<=0){
// non-intersecting: Steiner's theorem
for(size_t i=0; i<centers.size(); i++){
const Real& r(radii[i]); const Vector3r& x(centers[i]);
Real v=(4/3.)*M_PI*pow(r,3);
volume+=v;
Sg+=v*x;
Ig+=woo::Volumetric::inertiaTensorTranslate(Vector3r::Constant((2/5.)*v*pow(r,2)).asDiagonal(),v,-1.*x);
}
} else {
// intersecting: grid sampling
Real rMin=Inf; AlignedBox3r aabb;
for(size_t i=0; i<centers.size(); i++){
aabb.extend(centers[i]+Vector3r::Constant(radii[i]));
aabb.extend(centers[i]-Vector3r::Constant(radii[i]));
rMin=min(rMin,radii[i]);
}
if(rMin<=0){
if(failOk){ makeInvalid(); return; }
throw std::runtime_error("SphereClumpGeom.recompute: minimum radius must be positive (not "+to_string(rMin)+")");
}
Real dx=rMin/_div; Real dv=pow(dx,3);
long nCellsApprox=(aabb.sizes()/dx).prod();
// don't compute anything, it would take too long
if(fastOnly && nCellsApprox>1e5){ makeInvalid(); return; }
if(nCellsApprox>1e8) LOG_WARN("SphereClumpGeom: space grid has "<<nCellsApprox<<" cells, computing inertia can take a long time.");
Vector3r x;
for(x.x()=aabb.min().x()+dx/2.; x.x()<aabb.max().x(); x.x()+=dx){
for(x.y()=aabb.min().y()+dx/2.; x.y()<aabb.max().y(); x.y()+=dx){
for(x.z()=aabb.min().z()+dx/2.; x.z()<aabb.max().z(); x.z()+=dx){
for(size_t i=0; i<centers.size(); i++){
if((x-centers[i]).squaredNorm()<pow(radii[i],2)){
volume+=dv;
Sg+=dv*x;
Ig+=dv*(x.dot(x)*Matrix3r::Identity()-x*x.transpose())+/*along princial axes of dv; perhaps negligible?*/Matrix3r(Vector3r::Constant(dv*pow(dx,2)/6.).asDiagonal());
break;
}
}
}
}
}
}
woo::Volumetric::computePrincipalAxes(volume,Sg,Ig,pos,ori,inertia);
equivRad=(inertia.array()/volume).sqrt().mean(); // mean of radii of gyration
}
AlignedBox3r woo::Sphere::alignedBox() const {
AlignedBox3r ret; ret.extend(nodes[0]->pos-radius*Vector3r::Ones()); ret.extend(nodes[0]->pos+radius*Vector3r::Ones()); return ret;
}
void RandomInlet::run(){
DemField* dem=static_cast<DemField*>(field.get());
if(!generator) throw std::runtime_error("RandomInlet.generator==None!");
if(materials.empty()) throw std::runtime_error("RandomInlet.materials is empty!");
if(collideExisting){
if(!collider){
for(const auto& e: scene->engines){ collider=dynamic_pointer_cast<Collider>(e); if(collider) break; }
if(!collider) throw std::runtime_error("RandomInlet: no Collider found within engines (needed for collisions detection with already existing particles; if you don't need that, set collideExisting=False.)");
}
if(dynamic_pointer_cast<InsertionSortCollider>(collider)) static_pointer_cast<InsertionSortCollider>(collider)->forceInitSort=true;
}
if(isnan(massRate)) throw std::runtime_error("RandomInlet.massRate must be given (is "+to_string(massRate)+"); if you want to generate as many particles as possible, say massRate=0.");
if(massRate<=0 && maxAttempts==0) throw std::runtime_error("RandomInlet.massFlowRate<=0 (no massFlowRate prescribed), but RandomInlet.maxAttempts==0. (unlimited number of attempts); this would cause infinite loop.");
if(maxAttempts<0){
std::runtime_error("RandomInlet.maxAttempts must be non-negative. Negative value, leading to meaking engine dead, is achieved by setting atMaxAttempts=RandomInlet.maxAttDead now.");
}
spheresOnly=generator->isSpheresOnly();
padDist=generator->padDist();
if(isnan(padDist) || padDist<0) throw std::runtime_error(generator->pyStr()+".padDist(): returned invalid value "+to_string(padDist));
// as if some time has already elapsed at the very start
// otherwise mass flow rate is too big since one particle during Δt exceeds it easily
// equivalent to not running the first time, but without time waste
if(stepPrev==-1 && stepPeriod>0) stepPrev=-stepPeriod;
long nSteps=scene->step-stepPrev;
// to be attained in this step;
stepGoalMass+=massRate*scene->dt*nSteps; // stepLast==-1 if never run, which is OK
vector<AlignedBox3r> genBoxes; // of particles created in this step
vector<shared_ptr<Particle>> generated;
Real stepMass=0.;
SpherePack spheres;
//
if(spheresOnly){
spheres.pack.reserve(dem->particles->size()*1.2); // something extra for generated particles
// HACK!!!
bool isBox=dynamic_cast<BoxInlet*>(this);
AlignedBox3r box;
if(isBox){ box=this->cast<BoxInlet>().box; }
for(const auto& p: *dem->particles){
if(p->shape->isA<Sphere>() && (!isBox || box.contains(p->shape->nodes[0]->pos))) spheres.pack.push_back(SpherePack::Sph(p->shape->nodes[0]->pos,p->shape->cast<Sphere>().radius));
}
}
while(true){
// finished forever
if(everythingDone()) return;
// finished in this step
if(massRate>0 && mass>=stepGoalMass) break;
shared_ptr<Material> mat;
Vector3r pos=Vector3r::Zero();
Real diam;
vector<ParticleGenerator::ParticleAndBox> pee;
int attempt=-1;
while(true){
attempt++;
/***** too many tries, give up ******/
if(attempt>=maxAttempts){
generator->revokeLast(); // last particle could not be placed
if(massRate<=0){
LOG_DEBUG("maxAttempts="<<maxAttempts<<" reached; since massRate is not positive, we're done in this step");
goto stepDone;
}
switch(atMaxAttempts){
case MAXATT_ERROR: throw std::runtime_error("RandomInlet.maxAttempts reached ("+lexical_cast<string>(maxAttempts)+")"); break;
case MAXATT_DEAD:{
LOG_INFO("maxAttempts="<<maxAttempts<<" reached, making myself dead.");
this->dead=true;
return;
}
case MAXATT_WARN: LOG_WARN("maxAttempts "<<maxAttempts<<" reached before required mass amount was generated; continuing, since maxAttemptsError==False"); break;
case MAXATT_SILENT: break;
default: throw std::invalid_argument("Invalid value of RandomInlet.atMaxAttempts="+to_string(atMaxAttempts)+".");
}
}
/***** each maxAttempts/attPerPar, try a new particles *****/
if((attempt%(maxAttempts/attemptPar))==0){
LOG_DEBUG("attempt "<<attempt<<": trying with a new particle.");
if(attempt>0) generator->revokeLast(); // if not at the beginning, revoke the last particle
// random choice of material with equal probability
if(materials.size()==1) mat=materials[0];
else{
size_t i=max(size_t(materials.size()*Mathr::UnitRandom()),materials.size()-1);;
mat=materials[i];
}
// generate a new particle
std::tie(diam,pee)=(*generator)(mat,scene->time);
assert(!pee.empty());
LOG_TRACE("Placing "<<pee.size()<<"-sized particle; first component is a "<<pee[0].par->getClassName()<<", extents from "<<pee[0].extents.min().transpose()<<" to "<<pee[0].extents.max().transpose());
}
pos=randomPosition(diam,padDist); // overridden in child classes
LOG_TRACE("Trying pos="<<pos.transpose());
for(const auto& pe: pee){
// make translated copy
AlignedBox3r peBox(pe.extents); peBox.translate(pos);
// box is not entirely within the factory shape
if(!validateBox(peBox)){ LOG_TRACE("validateBox failed."); goto tryAgain; }
const shared_ptr<woo::Sphere>& peSphere=dynamic_pointer_cast<woo::Sphere>(pe.par->shape);
if(spheresOnly){
if(!peSphere) throw std::runtime_error("RandomInlet.spheresOnly: is true, but a nonspherical particle ("+pe.par->shape->pyStr()+") was returned by the generator.");
Real r=peSphere->radius;
Vector3r subPos=peSphere->nodes[0]->pos;
for(const auto& s: spheres.pack){
// check dist && don't collide with another sphere from this clump
// (abuses the *num* counter for clumpId)
if((s.c-(pos+subPos)).squaredNorm()<pow(s.r+r,2)){
LOG_TRACE("Collision with a particle in SpherePack (a particle generated in this step).");
goto tryAgain;
}
}
// don't add to spheres until all particles will have been checked for overlaps (below)
} else {
// see intersection with existing particles
bool overlap=false;
if(collideExisting){
vector<Particle::id_t> ids=collider->probeAabb(peBox.min(),peBox.max());
for(const auto& id: ids){
LOG_TRACE("Collider reports intersection with #"<<id);
if(id>(Particle::id_t)dem->particles->size() || !(*dem->particles)[id]) continue;
const shared_ptr<Shape>& sh2((*dem->particles)[id]->shape);
// no spheres, or they are too close
if(!peSphere || !sh2->isA<woo::Sphere>() || 1.1*(pos-sh2->nodes[0]->pos).squaredNorm()<pow(peSphere->radius+sh2->cast<Sphere>().radius,2)) goto tryAgain;
}
}
// intersection with particles generated by ourselves in this step
for(size_t i=0; i<genBoxes.size(); i++){
overlap=(genBoxes[i].squaredExteriorDistance(peBox)==0.);
if(overlap){
const auto& genSh(generated[i]->shape);
// for spheres, try to compute whether they really touch
if(!peSphere || !genSh->isA<Sphere>() || (pos-genSh->nodes[0]->pos).squaredNorm()<pow(peSphere->radius+genSh->cast<Sphere>().radius,2)){
LOG_TRACE("Collision with "<<i<<"-th particle generated in this step.");
goto tryAgain;
}
}
}
}
}
LOG_DEBUG("No collision (attempt "<<attempt<<"), particle will be created :-) ");
if(spheresOnly){
// num will be the same for all spheres within this clump (abuse the *num* counter)
for(const auto& pe: pee){
Vector3r subPos=pe.par->shape->nodes[0]->pos;
Real r=pe.par->shape->cast<Sphere>().radius;
spheres.pack.push_back(SpherePack::Sph((pos+subPos),r,/*clumpId*/(pee.size()==1?-1:num)));
}
}
break;
tryAgain: ; // try to position the same particle again
}
// particle was generated successfully and we have place for it
for(const auto& pe: pee){
genBoxes.push_back(AlignedBox3r(pe.extents).translate(pos));
generated.push_back(pe.par);
}
num+=1;
#ifdef WOO_OPENGL
Real color_=isnan(color)?Mathr::UnitRandom():color;
#endif
if(pee.size()>1){ // clump was generated
//LOG_WARN("Clumps not yet tested properly.");
vector<shared_ptr<Node>> nn;
for(auto& pe: pee){
auto& p=pe.par;
p->mask=mask;
#ifdef WOO_OPENGL
assert(p->shape);
if(color_>=0) p->shape->color=color_; // otherwise keep generator-assigned color
#endif
if(p->shape->nodes.size()!=1) LOG_WARN("Adding suspicious clump containing particle with more than one node (please check, this is perhaps not tested");
for(const auto& n: p->shape->nodes){
nn.push_back(n);
n->pos+=pos;
}
dem->particles->insert(p);
}
shared_ptr<Node> clump=ClumpData::makeClump(nn,/*no central node pre-given*/shared_ptr<Node>(),/*intersection*/false);
auto& dyn=clump->getData<DemData>();
if(shooter) (*shooter)(clump);
if(scene->trackEnergy) scene->energy->add(-DemData::getEk_any(clump,true,true,scene),"kinInlet",kinEnergyIx,EnergyTracker::ZeroDontCreate);
if(dyn.angVel!=Vector3r::Zero()){
throw std::runtime_error("pkg/dem/RandomInlet.cpp: generated particle has non-zero angular velocity; angular momentum should be computed so that rotation integration is correct, but it was not yet implemented.");
// TODO: compute initial angular momentum, since we will (very likely) use the aspherical integrator
}
ClumpData::applyToMembers(clump,/*reset*/false); // apply velocity
#ifdef WOO_OPENGL
boost::mutex::scoped_lock lock(dem->nodesMutex);
#endif
dyn.linIx=dem->nodes.size();
dem->nodes.push_back(clump);
mass+=clump->getData<DemData>().mass;
stepMass+=clump->getData<DemData>().mass;
} else {
auto& p=pee[0].par;
p->mask=mask;
assert(p->shape);
#ifdef WOO_OPENGL
if(color_>=0) p->shape->color=color_;
#endif
assert(p->shape->nodes.size()==1); // if this fails, enable the block below
const auto& node0=p->shape->nodes[0];
assert(node0->pos==Vector3r::Zero());
node0->pos+=pos;
auto& dyn=node0->getData<DemData>();
if(shooter) (*shooter)(node0);
if(scene->trackEnergy) scene->energy->add(-DemData::getEk_any(node0,true,true,scene),"kinInlet",kinEnergyIx,EnergyTracker::ZeroDontCreate);
mass+=dyn.mass;
stepMass+=dyn.mass;
assert(node0->hasData<DemData>());
dem->particles->insert(p);
#ifdef WOO_OPENGL
boost::mutex::scoped_lock lock(dem->nodesMutex);
#endif
dyn.linIx=dem->nodes.size();
dem->nodes.push_back(node0);
// handle multi-nodal particle (unused now)
#if 0
// TODO: track energy of the shooter
// in case the particle is multinodal, apply the same to all nodes
if(shooter) shooter(p.shape->nodes[0]);
const Vector3r& vel(p->shape->nodes[0]->getData<DemData>().vel); const Vector3r& angVel(p->shape->nodes[0]->getData<DemData>().angVel);
for(const auto& n: p.shape->nodes){
auto& dyn=n->getData<DemData>();
dyn.vel=vel; dyn.angVel=angVel;
mass+=dyn.mass;
n->linIx=dem->nodes.size();
dem->nodes.push_back(n);
}
#endif
}
};
stepDone:
setCurrRate(stepMass/(nSteps*scene->dt));
}
int spheroidsToSTL(const string& out, const shared_ptr<DemField>& dem, Real tol, const string& solid, int mask, bool append, bool clipCell, bool merge){
if(tol==0 || isnan(tol)) throw std::runtime_error("tol must be non-zero.");
#ifndef WOO_GTS
if(merge) throw std::runtime_error("woo.triangulated.spheroidsToSTL: merge=True only possible in builds with the 'gts' feature.");
#endif
// first traversal to find reference radius
auto particleOk=[&](const shared_ptr<Particle>&p){ return (mask==0 || (p->mask & mask)) && (p->shape->isA<Sphere>() || p->shape->isA<Ellipsoid>() || p->shape->isA<Capsule>()); };
int numTri=0;
if(tol<0){
LOG_DEBUG("tolerance is negative, taken as relative to minimum radius.");
Real minRad=Inf;
for(const auto& p: *dem->particles){
if(particleOk(p)) minRad=min(minRad,p->shape->equivRadius());
}
if(isinf(minRad) || isnan(minRad)) throw std::runtime_error("Minimum radius not found (relative tolerance specified); no matching particles?");
tol=-minRad*tol;
LOG_DEBUG("Minimum radius "<<minRad<<".");
}
LOG_DEBUG("Triangulation tolerance is "<<tol);
std::ofstream stl(out,append?(std::ofstream::app|std::ofstream::binary):std::ofstream::binary); // binary better, anyway
if(!stl.good()) throw std::runtime_error("Failed to open output file "+out+" for writing.");
Scene* scene=dem->scene;
if(!scene) throw std::logic_error("DEM field has not associated scene?");
// periodicity, cache that for later use
AlignedBox3r cell;
/*
wasteful memory-wise, but we need to store the whole triangulation in case *merge* is in effect,
when it is only an intermediary result and will not be output as-is
*/
vector<vector<Vector3r>> ppts;
vector<vector<Vector3i>> ttri;
vector<Particle::id_t> iid;
for(const auto& p: *dem->particles){
if(!particleOk(p)) continue;
const auto sphere=dynamic_cast<Sphere*>(p->shape.get());
const auto ellipsoid=dynamic_cast<Ellipsoid*>(p->shape.get());
const auto capsule=dynamic_cast<Capsule*>(p->shape.get());
vector<Vector3r> pts;
vector<Vector3i> tri;
if(sphere || ellipsoid){
Real r=sphere?sphere->radius:ellipsoid->semiAxes.minCoeff();
// 1 is for icosahedron
int tess=ceil(M_PI/(5*acos(1-tol/r)));
LOG_DEBUG("Tesselation level for #"<<p->id<<": "<<tess);
tess=max(tess,0);
auto uSphTri(CompUtils::unitSphereTri20(/*0 for icosahedron*/max(tess-1,0)));
const auto& uPts=std::get<0>(uSphTri); // unit sphere point coords
pts.resize(uPts.size());
const auto& node=(p->shape->nodes[0]);
Vector3r scale=(sphere?sphere->radius*Vector3r::Ones():ellipsoid->semiAxes);
for(size_t i=0; i<uPts.size(); i++){
pts[i]=node->loc2glob(uPts[i].cwiseProduct(scale));
}
tri=std::get<1>(uSphTri); // this makes a copy, but we need out own for capsules
}
if(capsule){
#ifdef WOO_VTK
int subdiv=max(4.,ceil(M_PI/(acos(1-tol/capsule->radius))));
std::tie(pts,tri)=VtkExport::triangulateCapsule(static_pointer_cast<Capsule>(p->shape),subdiv);
#else
throw std::runtime_error("Triangulation of capsules is (for internal and entirely fixable reasons) only available when compiled with the 'vtk' features.");
#endif
}
// do not write out directly, store first for later
ppts.push_back(pts);
ttri.push_back(tri);
LOG_TRACE("#"<<p->id<<" triangulated: "<<tri.size()<<","<<pts.size()<<" faces,vertices.");
if(scene->isPeriodic){
// make sure we have aabb, in skewed coords and such
if(!p->shape->bound){
// this is a bit ugly, but should do the trick; otherwise we would recompute all that ourselves here
if(sphere) Bo1_Sphere_Aabb().go(p->shape);
else if(ellipsoid) Bo1_Ellipsoid_Aabb().go(p->shape);
else if(capsule) Bo1_Capsule_Aabb().go(p->shape);
}
assert(p->shape->bound);
const AlignedBox3r& box(p->shape->bound->box);
AlignedBox3r cell(Vector3r::Zero(),scene->cell->getSize()); // possibly in skewed coords
// central offset
Vector3i off0;
scene->cell->canonicalizePt(p->shape->nodes[0]->pos,off0); // computes off0
Vector3i off; // offset from the original cell
//cerr<<"#"<<p->id<<" at "<<p->shape->nodes[0]->pos.transpose()<<", off0="<<off0<<endl;
for(off[0]=off0[0]-1; off[0]<=off0[0]+1; off[0]++) for(off[1]=off0[1]-1; off[1]<=off0[1]+1; off[1]++) for(off[2]=off0[2]-1; off[2]<=off0[2]+1; off[2]++){
Vector3r dx=scene->cell->intrShiftPos(off);
//cerr<<" off="<<off.transpose()<<", dx="<<dx.transpose()<<endl;
AlignedBox3r boxOff(box); boxOff.translate(dx);
//cerr<<" boxOff="<<boxOff.min()<<";"<<boxOff.max()<<" | cell="<<cell.min()<<";"<<cell.max()<<endl;
if(boxOff.intersection(cell).isEmpty()) continue;
// copy the entire triangulation, offset by dx
vector<Vector3r> pts2(pts); for(auto& p: pts2) p+=dx;
vector<Vector3i> tri2(tri); // same topology
ppts.push_back(pts2);
ttri.push_back(tri2);
LOG_TRACE(" offset "<<off.transpose()<<": #"<<p->id<<": "<<tri2.size()<<","<<pts2.size()<<" faces,vertices.");
}
}
}
if(!merge){
LOG_DEBUG("Will export (unmerged) "<<ppts.size()<<" particles to STL.");
stl<<"solid "<<solid<<"\n";
for(size_t i=0; i<ppts.size(); i++){
const auto& pts(ppts[i]);
const auto& tri(ttri[i]);
LOG_TRACE("Exporting "<<i<<" with "<<tri.size()<<" faces.");
for(const Vector3i& t: tri){
Vector3r pp[]={pts[t[0]],pts[t[1]],pts[t[2]]};
// skip triangles which are entirely out of the canonical periodic cell
if(scene->isPeriodic && clipCell && (!scene->cell->isCanonical(pp[0]) && !scene->cell->isCanonical(pp[1]) && !scene->cell->isCanonical(pp[2]))) continue;
numTri++;
Vector3r n=(pp[1]-pp[0]).cross(pp[2]-pp[1]).normalized();
stl<<" facet normal "<<n.x()<<" "<<n.y()<<" "<<n.z()<<"\n";
stl<<" outer loop\n";
for(auto p: {pp[0],pp[1],pp[2]}){
stl<<" vertex "<<p[0]<<" "<<p[1]<<" "<<p[2]<<"\n";
}
stl<<" endloop\n";
stl<<" endfacet\n";
}
}
stl<<"endsolid "<<solid<<"\n";
stl.close();
return numTri;
}
#if WOO_GTS
/*****
Convert all triangulation to GTS surfaces, find their distances, isolate connected components,
merge these components incrementally and write to STL
*****/
// total number of points
const size_t N(ppts.size());
// bounds for collision detection
struct Bound{
Bound(Real _coord, int _id, bool _isMin): coord(_coord), id(_id), isMin(_isMin){};
Bound(): coord(NaN), id(-1), isMin(false){}; // just for allocation
Real coord;
int id;
bool isMin;
bool operator<(const Bound& b) const { return coord<b.coord; }
};
vector<Bound> bounds[3]={vector<Bound>(2*N),vector<Bound>(2*N),vector<Bound>(2*N)};
/* construct GTS surface objects; all objects must be deleted explicitly! */
vector<GtsSurface*> ssurf(N);
vector<vector<GtsVertex*>> vvert(N);
vector<vector<GtsEdge*>> eedge(N);
vector<AlignedBox3r> boxes(N);
for(size_t i=0; i<N; i++){
LOG_TRACE("** Creating GTS surface for #"<<i<<", with "<<ttri[i].size()<<" faces, "<<ppts[i].size()<<" vertices.");
AlignedBox3r box;
// new surface object
ssurf[i]=gts_surface_new(gts_surface_class(),gts_face_class(),gts_edge_class(),gts_vertex_class());
// copy over all vertices
vvert[i].reserve(ppts[i].size());
eedge[i].reserve(size_t(1.5*ttri[i].size())); // each triangle consumes 1.5 edges, for closed surfs
for(size_t v=0; v<ppts[i].size(); v++){
vvert[i].push_back(gts_vertex_new(gts_vertex_class(),ppts[i][v][0],ppts[i][v][1],ppts[i][v][2]));
box.extend(ppts[i][v]);
}
// create faces, and create edges on the fly as needed
std::map<std::pair<int,int>,int> edgeIndices;
for(size_t t=0; t<ttri[i].size(); t++){
//const Vector3i& t(ttri[i][t]);
//LOG_TRACE("Face with vertices "<<ttri[i][t][0]<<","<<ttri[i][t][1]<<","<<ttri[i][t][2]);
Vector3i eIxs;
for(int a:{0,1,2}){
int A(ttri[i][t][a]), B(ttri[i][t][(a+1)%3]);
auto AB=std::make_pair(min(A,B),max(A,B));
auto ABI=edgeIndices.find(AB);
if(ABI==edgeIndices.end()){ // this edge not created yet
edgeIndices[AB]=eedge[i].size(); // last index
eIxs[a]=eedge[i].size();
//LOG_TRACE(" New edge #"<<eIxs[a]<<": "<<A<<"--"<<B<<" (length "<<(ppts[i][A]-ppts[i][B]).norm()<<")");
eedge[i].push_back(gts_edge_new(gts_edge_class(),vvert[i][A],vvert[i][B]));
} else {
eIxs[a]=ABI->second;
//LOG_TRACE(" Found edge #"<<ABI->second<<" for "<<A<<"--"<<B);
}
}
//LOG_TRACE(" New face: edges "<<eIxs[0]<<"--"<<eIxs[1]<<"--"<<eIxs[2]);
GtsFace* face=gts_face_new(gts_face_class(),eedge[i][eIxs[0]],eedge[i][eIxs[1]],eedge[i][eIxs[2]]);
gts_surface_add_face(ssurf[i],face);
}
// make sure the surface is OK
if(!gts_surface_is_orientable(ssurf[i])) LOG_ERROR("Surface of #"+to_string(iid[i])+" is not orientable (expect troubles).");
if(!gts_surface_is_closed(ssurf[i])) LOG_ERROR("Surface of #"+to_string(iid[i])+" is not closed (expect troubles).");
assert(!gts_surface_is_self_intersecting(ssurf[i]));
// copy bounds
LOG_TRACE("Setting bounds of surf #"<<i);
boxes[i]=box;
for(int ax:{0,1,2}){
bounds[ax][2*i+0]=Bound(box.min()[ax],/*id*/i,/*isMin*/true);
bounds[ax][2*i+1]=Bound(box.max()[ax],/*id*/i,/*isMin*/false);
}
}
/*
broad-phase collision detection between GTS surfaces
only those will be probed with exact algorithms below and merged if needed
*/
for(int ax:{0,1,2}) std::sort(bounds[ax].begin(),bounds[ax].end());
vector<Bound>& bb(bounds[0]); // run the search along x-axis, does not matter really
std::list<std::pair<int,int>> int0; // broad-phase intersections
for(size_t i=0; i<2*N; i++){
if(!bb[i].isMin) continue; // only start with lower bound
// go up to the upper bound, but handle overflow safely (no idea why it would happen here) as well
for(size_t j=i+1; j<2*N && bb[j].id!=bb[i].id; j++){
if(bb[j].isMin) continue; // this is handled by symmetry
#if EIGEN_VERSION_AT_LEAST(3,2,5)
if(!boxes[bb[i].id].intersects(boxes[bb[j].id])) continue; // no intersection along all axes
#else
// old, less elegant
if(boxes[bb[i].id].intersection(boxes[bb[j].id]).isEmpty()) continue;
#endif
int0.push_back(std::make_pair(min(bb[i].id,bb[j].id),max(bb[i].id,bb[j].id)));
LOG_TRACE("Broad-phase collision "<<int0.back().first<<"+"<<int0.back().second);
}
}
/*
narrow-phase collision detection between GTS surface
this must be done via gts_surface_inter_new, since gts_surface_distance always succeeds
*/
std::list<std::pair<int,int>> int1;
for(const std::pair<int,int> ij: int0){
LOG_TRACE("Testing narrow-phase collision "<<ij.first<<"+"<<ij.second);
#if 0
GtsRange gr1, gr2;
gts_surface_distance(ssurf[ij.first],ssurf[ij.second],/*delta ??*/(gfloat).2,&gr1,&gr2);
if(gr1.min>0 && gr2.min>0) continue;
LOG_TRACE(" GTS reports collision "<<ij.first<<"+"<<ij.second<<" (min. distances "<<gr1.min<<", "<<gr2.min);
#else
GtsSurface *s1(ssurf[ij.first]), *s2(ssurf[ij.second]);
GNode* t1=gts_bb_tree_surface(s1);
GNode* t2=gts_bb_tree_surface(s2);
GtsSurfaceInter* I=gts_surface_inter_new(gts_surface_inter_class(),s1,s2,t1,t2,/*is_open_1*/false,/*is_open_2*/false);
GSList* l=gts_surface_intersection(s1,s2,t1,t2); // list of edges describing intersection
int n1=g_slist_length(l);
// extra check by looking at number of faces of the intersected surface
#if 1
GtsSurface* s12=gts_surface_new(gts_surface_class(),gts_face_class(),gts_edge_class(),gts_vertex_class());
gts_surface_inter_boolean(I,s12,GTS_1_OUT_2);
gts_surface_inter_boolean(I,s12,GTS_2_OUT_1);
int n2=gts_surface_face_number(s12);
gts_object_destroy(GTS_OBJECT(s12));
#endif
gts_bb_tree_destroy(t1,TRUE);
gts_bb_tree_destroy(t2,TRUE);
gts_object_destroy(GTS_OBJECT(I));
g_slist_free(l);
if(n1==0) continue;
#if 1
if(n2==0){ LOG_ERROR("n1==0 but n2=="<<n2<<" (no narrow-phase collision)"); continue; }
#endif
LOG_TRACE(" GTS reports collision "<<ij.first<<"+"<<ij.second<<" ("<<n<<" edges describe the intersection)");
#endif
int1.push_back(ij);
}
/*
connected components on the graph: graph nodes are 0…(N-1), graph edges are in int1
see http://stackoverflow.com/a/37195784/761090
*/
typedef boost::subgraph<boost::adjacency_list<boost::vecS,boost::vecS,boost::undirectedS,boost::property<boost::vertex_index_t,int>,boost::property<boost::edge_index_t,int>>> Graph;
Graph graph(N);
for(const auto& ij: int1) boost::add_edge(ij.first,ij.second,graph);
vector<size_t> clusters(boost::num_vertices(graph));
size_t numClusters=boost::connected_components(graph,clusters.data());
for(size_t n=0; n<numClusters; n++){
// beginning cluster #n
// first, count how many surfaces are in this cluster; if 1, things are easier
int numThisCluster=0; int cluster1st=-1;
for(size_t i=0; i<N; i++){ if(clusters[i]!=n) continue; numThisCluster++; if(cluster1st<0) cluster1st=(int)i; }
GtsSurface* clusterSurf=NULL;
LOG_DEBUG("Cluster "<<n<<" has "<<numThisCluster<<" surfaces.");
if(numThisCluster==1){
clusterSurf=ssurf[cluster1st];
} else {
clusterSurf=ssurf[cluster1st]; // surface of the cluster itself
LOG_TRACE(" Initial cluster surface from "<<cluster1st<<".");
/* composed surface */
for(size_t i=0; i<N; i++){
if(clusters[i]!=n || ((int)i)==cluster1st) continue;
LOG_TRACE(" Adding "<<i<<" to the cluster");
// ssurf[i] now belongs to cluster #n
// trees need to be rebuild every time anyway, since the merged surface keeps changing in every cycle
//if(gts_surface_face_number(clusterSurf)==0) LOG_ERROR("clusterSurf has 0 faces.");
//if(gts_surface_face_number(ssurf[i])==0) LOG_ERROR("Surface #"<<i<<" has 0 faces.");
GNode* t1=gts_bb_tree_surface(clusterSurf);
GNode* t2=gts_bb_tree_surface(ssurf[i]);
GtsSurfaceInter* I=gts_surface_inter_new(gts_surface_inter_class(),clusterSurf,ssurf[i],t1,t2,/*is_open_1*/false,/*is_open_2*/false);
GtsSurface* merged=gts_surface_new(gts_surface_class(),gts_face_class(),gts_edge_class(),gts_vertex_class());
gts_surface_inter_boolean(I,merged,GTS_1_OUT_2);
gts_surface_inter_boolean(I,merged,GTS_2_OUT_1);
gts_object_destroy(GTS_OBJECT(I));
gts_bb_tree_destroy(t1,TRUE);
gts_bb_tree_destroy(t2,TRUE);
if(gts_surface_face_number(merged)==0){
LOG_ERROR("Cluster #"<<n<<": 0 faces after fusing #"<<i<<" (why?), adding #"<<i<<" separately!");
// this will cause an extra 1-particle cluster to be created
clusters[i]=numClusters;
numClusters+=1;
} else {
// not from global vectors (cleanup at the end), explicit delete!
if(clusterSurf!=ssurf[cluster1st]) gts_object_destroy(GTS_OBJECT(clusterSurf));
clusterSurf=merged;
}
}
}
#if 0
LOG_TRACE(" GTS surface cleanups...");
pygts_vertex_cleanup(clusterSurf,.1*tol); // cleanup 10× smaller than tolerance
pygts_edge_cleanup(clusterSurf);
pygts_face_cleanup(clusterSurf);
#endif
LOG_TRACE(" STL: cluster "<<n<<" output");
stl<<"solid "<<solid<<"_"<<n<<"\n";
/* output cluster to STL here */
_gts_face_to_stl_data data(stl,scene,clipCell,numTri);
gts_surface_foreach_face(clusterSurf,(GtsFunc)_gts_face_to_stl,(gpointer)&data);
stl<<"endsolid\n";
if(clusterSurf!=ssurf[cluster1st]) gts_object_destroy(GTS_OBJECT(clusterSurf));
}
// this deallocates also edges and vertices
for(size_t i=0; i<ssurf.size(); i++) gts_object_destroy(GTS_OBJECT(ssurf[i]));
return numTri;
#endif /* WOO_GTS */
}