bool operator()(const Vector3r& pt, Real pad=0.) const {
		Real u=(pt.dot(c12)-c1.dot(c12))/(ht*ht); // normalized coordinate along the c1--c2 axis
		if((u*ht<0+pad) || (u*ht>ht-pad)) return false; // out of cylinder along the axis
		Real axisDist=((pt-c1).cross(pt-c2)).norm()/ht;
		if(axisDist>radius-pad) return false;
		return true;
	}

// http://en.wikipedia.org/wiki/Inertia_tensor_of_triangle
Matrix3r woo::Volumetric::triangleInertia(const Vector3r& v0, const Vector3r& v1, const Vector3r& v2){
	Matrix3r V; V<<v0.transpose(),v1.transpose(),v2.transpose(); // rows!
	Real a=((v1-v0).cross(v2-v0)).norm(); // twice the triangle area
	Matrix3r S; S<<2,1,1, 1,2,1, 1,1,2; S*=(1/24.);
	Matrix3r C=a*V.transpose()*S*V;
	return Matrix3r::Identity()*C.trace()-C;
};

/*
	The following code is based on GPL-licensed K-3d importer module from
	https://github.com/K-3D/k3d/blob/master/modules/stl_io/mesh_reader.cpp

	TODO: read color/material, convert to scalar color in Woo
*/
vector<shared_ptr<Particle>> DemFuncs::importSTL(const string& filename, const shared_ptr<Material>& mat, int mask, Real color, Real scale, const Vector3r& shift, const Quaternionr& ori, Real threshold, Real maxBox, bool readColors, bool flex, Real thickness){
	vector<shared_ptr<Particle>> ret;
	std::ifstream in(filename,std::ios::in|std::ios::binary);
	if(!in) throw std::runtime_error("Error opening "+filename+" for reading (STL import).");

	char buffer[80]; in.read(buffer, 80); in.seekg(0, std::ios::beg);
	bool isAscii=boost::algorithm::starts_with(buffer,"solid");

	// linear array of vertices, each triplet is one face
	// this is filled from ASCII and binary formats as intermediary representation
	
	// coordinate system change using (ori, scale, shift) is done already when reading vertices from the file
	vector<Vector3r> vertices;
	if(isAscii){
		LOG_TRACE("STL: ascii format detected");
		string lineBuf;
		long lineNo=-1;
		int fVertsNum=0;  // number of vertices in this facet (for checking)
		for(std::getline(in,lineBuf); in; getline(in,lineBuf)){
			lineNo++;
			string tok;
			std::istringstream line(lineBuf);
			line>>tok;
			if(tok=="facet"){
				string tok2; line>>tok2;
				if(tok2!="normal") LOG_WARN("STL: 'normal' expected after 'facet' (line "+to_string(lineNo)+")");
				// we ignore normal values:
				// Vector3r normal; line>>normal.x(); line>>normal.y(); line>>normal.z();
			} else if(tok=="vertex"){
				Vector3r p; line>>p.x(); line>>p.y(); line>>p.z();
				vertices.push_back(ori*(p*scale+shift));
				fVertsNum++;
			} else if(tok=="endfacet"){

bool DihedralConstraint::initConstraint(SimulationModel &model, const unsigned int particle1, const unsigned int particle2,
										const unsigned int particle3, const unsigned int particle4)
{
	m_bodies[0] = particle1;
	m_bodies[1] = particle2;
	m_bodies[2] = particle3;
	m_bodies[3] = particle4;
	ParticleData &pd = model.getParticles();

	const Vector3r &p0 = pd.getPosition0(particle1);
	const Vector3r &p1 = pd.getPosition0(particle2);
	const Vector3r &p2 = pd.getPosition0(particle3);
	const Vector3r &p3 = pd.getPosition0(particle4);

	Vector3r e = p3 - p2;
	Real  elen = e.norm();
	if (elen < 1e-6)
		return false;

	Real invElen = 1.0 / elen;

	Vector3r n1 = (p2 - p0).cross(p3 - p0); n1 /= n1.squaredNorm();
	Vector3r n2 = (p3 - p1).cross(p2 - p1); n2 /= n2.squaredNorm();

	n1.normalize();
	n2.normalize();
	Real dot = n1.dot(n2);

	if (dot < -1.0) dot = -1.0;
	if (dot > 1.0) dot = 1.0;

	m_restAngle = acos(dot);

	return true;
}

	notInNotch(const Vector3r& _c, const Vector3r& _edge, const Vector3r& _normal, Real _aperture){
		c=_c;
		edge=_edge; edge.normalize();
		normal=_normal; normal-=edge*edge.dot(normal); normal.normalize();
		inside=edge.cross(normal);
		aperture=_aperture;
		// LOG_DEBUG("edge="<<edge<<", normal="<<normal<<", inside="<<inside<<", aperture="<<aperture);
	}

// function used to check if the newly broken bond belongs in a cluster or not, if so attach to proper cluster and set appropriate flags
void Law2_ScGeom_JCFpmPhys_JointedCohesiveFrictionalPM::checkForCluster(JCFpmPhys* phys, ScGeom* geom, Body* b1, Body* b2, Interaction* contact){

	const shared_ptr<Shape>& sphere1 = b1->shape;
	const shared_ptr<Shape>& sphere2 = b2->shape;
	const Real sphereRadius1 = static_cast<Sphere*>(sphere1.get())->radius;
	const Real sphereRadius2 = static_cast<Sphere*>(sphere2.get())->radius;
	const Real avgDiameter = (sphereRadius1+sphereRadius2);
	Vector3r& brokenInteractionLocation = geom->contactPoint;
	phys->nearbyFound=0;

	FOREACH(const shared_ptr<Interaction>& I, *scene->interactions){
	//#endif
		JCFpmPhys* nearbyPhys;
		const ScGeom* nearbyGeom;
		if (!I || !I->geom.get() || !I->phys.get()) continue;
		if (I && I->isReal() && JCFpmPhys::getClassIndexStatic()==I->phys->getClassIndex()){
			nearbyPhys = YADE_CAST<JCFpmPhys*>(I->phys.get());
			if (!nearbyPhys) continue;
			if (I->geom.get() /*&& !nearbyPhys->momentBroken*/){
				nearbyGeom = YADE_CAST<ScGeom*> (I->geom.get());
                    		if (!nearbyGeom) continue;
				Vector3r nearbyInteractionLocation = nearbyGeom->contactPoint;
				Vector3r proximityVector = nearbyInteractionLocation-brokenInteractionLocation;
				Real proximity = proximityVector.norm();
				
				// this logic is finding interactions within a radius of the broken one, and identifiying if it is an original event or if it belongs in a cluster
				if (proximity < avgDiameter*momentRadiusFactor && proximity != 0){
					if (nearbyPhys->originalClusterEvent && !nearbyPhys->momentCalculated && !phys->clusteredEvent && clusterMoments) {
						phys->eventNumber = nearbyPhys->eventNumber; 
						phys->clusteredEvent = true;
						phys->originalEvent = I.get();
					} else if (nearbyPhys->clusteredEvent && !phys->clusteredEvent && clusterMoments){
						JCFpmPhys* originalPhys = YADE_CAST<JCFpmPhys*>(nearbyPhys->originalEvent->phys.get());
						if (!originalPhys->momentCalculated){
							phys->eventNumber = nearbyPhys->eventNumber;
							phys->clusteredEvent = true;
							phys->originalEvent = nearbyPhys->originalEvent;
						}
					} 

					if (nearbyPhys->momentBroken) continue;
					phys->nearbyInts.push_back(I.get());
				}
			}
		}
	}
	if (!phys->clusteredEvent) {
		phys->originalClusterEvent = true; // if not clustered, its an original event. We use this interaction as the master for the cluster. Its list of nearbyInts will expand if other ints break nearby. 
		phys->eventBeginTime=scene->time;
		phys->originalEvent = contact;
		eventNumber += 1;
		phys->eventNumber = eventNumber;
	}
	phys->checkedForCluster = true;
}

// ----------------------------------------------------------------------------------------------
bool PositionBasedFluids::computePBFLagrangeMultiplier(
	const unsigned int particleIndex,
	const unsigned int numberOfParticles,
	const Vector3r x[],	
	const Real mass[],
	const Vector3r boundaryX[],
	const Real boundaryPsi[],
	const Real density,
	const unsigned int numNeighbors,
	const unsigned int neighbors[],
	const Real density0,
	const bool boundaryHandling,
	Real &lambda)
{
	const Real eps = 1.0e-6;

	// Evaluate constraint function
	const Real C = std::max(density / density0 - 1.0, 0.0);			// clamp to prevent particle clumping at surface

	if (C != 0.0)
	{
		// Compute gradients dC/dx_j 
		Real sum_grad_C2 = 0.0;
		Vector3r gradC_i(0.0, 0.0, 0.0);

		for (unsigned int j = 0; j < numNeighbors; j++)
		{
			const unsigned int neighborIndex = neighbors[j];
			if (neighborIndex < numberOfParticles)		// Test if fluid particle
			{
				const Vector3r gradC_j = -mass[neighborIndex] / density0 * CubicKernel::gradW(x[particleIndex] - x[neighborIndex]);
				sum_grad_C2 += gradC_j.squaredNorm();
				gradC_i -= gradC_j;
			}
			else if (boundaryHandling)
			{
				// Boundary: Akinci2012
				const Vector3r gradC_j = -boundaryPsi[neighborIndex - numberOfParticles] / density0 * CubicKernel::gradW(x[particleIndex] - boundaryX[neighborIndex - numberOfParticles]);
				sum_grad_C2 += gradC_j.squaredNorm();
				gradC_i -= gradC_j;
			}
		}

		sum_grad_C2 += gradC_i.squaredNorm();

		// Compute lambda
		lambda = -C / (sum_grad_C2 + eps);
	}
	else
		lambda = 0.0;

	return true;
}

void SafetyEval::isSafeDestination(const Vector3r& dest_pos, const Vector3r& cur_pos, const Quaternionr& quaternion, SafetyEval::EvalResult& result)
{
    //this function should work even when dest_pos == cur_pos
    result.dest_pos = dest_pos;
    result.cur_pos = cur_pos;

    //is this dest_pos cur_pos within the fence?
    checkFence(dest_pos, cur_pos, result);
 
    if (!(enable_reasons_ & SafetyViolationType_::Obstacle))
        return;

    //transform dest_pos vector to body frame
    const Vector3r cur_dest = dest_pos - cur_pos;
    float cur_dest_norm = cur_dest.norm();    
    
    //check for approx zero vectors to avoid random yaw angles
    if (cur_dest_norm < vehicle_params_.distance_accuracy) {
        //we are hovering
        result.dest_risk_dist = Utils::nan<float>();
        isCurrentSafer(result);
    }
    else { //see if we have obstacle in direction 
        result.cur_dest_body = VectorMath::transformToBodyFrame(cur_dest, quaternion, true);

        //get yaw in body frame, ie, front is always 0 radians
        float point_angle = std::atan2(result.cur_dest_body[1], result.cur_dest_body[0]);

        //yaw to ticks
        int point_tick = obs_xy_ptr_->angleToTick(point_angle);

        //get obstacles in the window at the tick direction around the window
        result.dest_obs = obs_xy_ptr_->hasObstacle(point_tick - vehicle_params_.obs_window, point_tick + vehicle_params_.obs_window);

        //less risk distance is better
        result.dest_risk_dist = cur_dest_norm + adjustClearanceForPrStl(vehicle_params_.obs_clearance, result.dest_obs.confidence) - result.dest_obs.distance;
        if (result.dest_risk_dist >= 0) { //potential collision
            //check obstacles around current position and see if it has lower risk
            isCurrentSafer(result);
        }
        //else obstacle is too far
    }

    //if we detected unsafe condition due to obstacle, find direction to move away to
    if (!result.is_safe && result.reason & SafetyViolationType_::Obstacle) {
        //look for each surrounding tick to see if we have obstacle free angle
        setSuggestedVelocity(result, quaternion);

        Utils::log(Utils::stringf("isSafeDestination: cur_dest_norm=%f, result=%s, quaternion=%s", 
            cur_dest_norm, result.toString().c_str(), VectorMath::toString(quaternion, true).c_str()));
    }
    //else no suggestions required
}

bool Law2_ScGeom_ElectrostaticPhys::go(shared_ptr<IGeom>& iGeom, shared_ptr<IPhys>& iPhys, Interaction* interaction)
{
	// Physic
    ElectrostaticPhys* phys=static_cast<ElectrostaticPhys*>(iPhys.get());

    // Geometry
    ScGeom* geom=static_cast<ScGeom*>(iGeom.get());

    // Get bodies properties
    Body::id_t id1 = interaction->getId1();
    Body::id_t id2 = interaction->getId2();
    const shared_ptr<Body> b1 = Body::byId(id1,scene);
    const shared_ptr<Body> b2 = Body::byId(id2,scene);
    State* s1 = b1->state.get();
    State* s2 = b2->state.get();

    // geometric parameters
    Real a((geom->radius1+geom->radius2)/2.);
    bool isNew=false;
	
	// Speeds
    Vector3r shiftVel=scene->isPeriodic ? Vector3r(scene->cell->velGrad*scene->cell->hSize*interaction->cellDist.cast<Real>()) : Vector3r::Zero();
    Vector3r shift2 = scene->isPeriodic ? Vector3r(scene->cell->hSize*interaction->cellDist.cast<Real>()): Vector3r::Zero();

    Vector3r relV = geom->getIncidentVel(s1, s2, scene->dt, shift2, shiftVel, false );
	Real undot = relV.dot(geom->normal); // Normal velocity norm
	
    if(-geom->penetrationDepth > a && -geom->penetrationDepth > 10.*phys->DebyeLength) { return false; }
    
    // inititalization
	if(phys->u == -1. ) {phys->u = -geom->penetrationDepth; isNew=true;}
	
	// Solve normal
	normalForce_DL_Adim(phys,geom, undot,isNew);
	
	// Solve shear and torques
	Vector3r C1 = Vector3r::Zero();
	Vector3r C2 = Vector3r::Zero();
	
	computeShearForceAndTorques_log(phys, geom, s1, s2, C1, C2);
	
    // Apply!
    scene->forces.addForce(id1,phys->normalForce+phys->shearForce);
    scene->forces.addTorque(id1,C1);

    scene->forces.addForce(id2,-(phys->normalForce+phys->shearForce));
    scene->forces.addTorque(id2,C2);

    return true;
}

	inParallelepiped(const Vector3r& o, const Vector3r& a, const Vector3r& b, const Vector3r& c){
		Vector3r A(o), B(a), C(a+(b-o)), D(b), E(c), F(c+(a-o)), G(c+(a-o)+(b-o)), H(c+(b-o));
		Vector3r x(B-A), y(D-A), z(E-A);
		n[0]=-y.cross(z).normalized(); n[1]=-n[0]; pts[0]=A; pts[1]=B;
		n[2]=-z.cross(x).normalized(); n[3]=-n[2]; pts[2]=A; pts[3]=D;
		n[4]=-x.cross(y).normalized(); n[5]=-n[4]; pts[4]=A; pts[5]=E;
		// bounding box
		Vector3r vertices[8]={A,B,C,D,E,F,G,H};
		mn=mx=vertices[0];
		for(int i=1; i<8; i++){ 
			mn=mn.cwiseMin(vertices[i]); 
			mx=mx.cwiseMax(vertices[i]); 
		}
	}

string GenerateCloud_water(vector<BasicSphere>& sphere_list, Vector3r lowerCorner, Vector3r upperCorner, long number, Real rad_std_dev, Real porosity)
{
	typedef boost::minstd_rand StdGenerator;
	static StdGenerator generator;
	static boost::variate_generator<StdGenerator&, boost::uniform_real<> >
			random1(generator, boost::uniform_real<>(0,1));
        //         static boost::variate_generator<StdGenerator&, boost::normal_distribution<> >
        //         randomN(generator, boost::normal_distribution<>(aggregateMeanRadius,aggregateSigmaRadius));

	sphere_list.clear();
	long tries = 1000; //nb of tries for positionning the next sphere
	Vector3r dimensions = upperCorner - lowerCorner;
		
	Real mean_radius = std::pow(dimensions.x()*dimensions.y()*dimensions.z()*(1-porosity)/(3.1416*1.3333*number),0.333333);
        //cerr << mean_radius;
        Real Rmin=mean_radius, Rmax=mean_radius;

	std::cerr << "generating aggregates ... ";
	
	long t, i;
	for (i=0; i<number; ++i) {
		BasicSphere s;
		s.second = (random1()-0.5)*rad_std_dev*mean_radius+mean_radius;
		for (t=0; t<tries; ++t) {
			s.first.x() = lowerCorner.x()+s.second+(dimensions.x()-2*s.second)*random1();
			s.first.y() = lowerCorner.y()+s.second+(dimensions.y()-2*s.second)*random1();
			s.first.z() = lowerCorner.z()+s.second+(dimensions.z()-2*s.second)*random1();
			bool overlap=false;
			for (long j=0; (j<i && !overlap); j++)
				if ( pow(sphere_list[j].second+s.second, 2) > (sphere_list[j].first-s.first).squaredNorm()) overlap=true;
			if (!overlap){
				sphere_list.push_back(s);
				Rmin = std::min(Rmin,s.second);
				Rmax = std::max(Rmax,s.second);
				break;}
		}
		if (t==tries) return "More than " + lexical_cast<string>(tries) +
					" tries while generating sphere number " +
					lexical_cast<string>(i+1) + "/" + lexical_cast<string>(number) + ".";
	}
	return "Generated a sample with " + lexical_cast<string>(number) + "spheres inside box of dimensions: (" 
			+ lexical_cast<string>(dimensions[0]) + "," 
			+ lexical_cast<string>(dimensions[1]) + "," 
			+ lexical_cast<string>(dimensions[2]) + ")."
			+ "  mean radius=" + lexical_cast<string>(mean_radius) +
			+ "  Rmin =" + lexical_cast<string>(Rmin) +
			+ "  Rmax =" + lexical_cast<string>(Rmax) + ".";
}

void SimpleShear::createSphere(shared_ptr<Body>& body, Vector3r position, Real radius)
{
	body = shared_ptr<Body>(new Body); body->groupMask=1;
	shared_ptr<NormalInelasticMat> mat(new NormalInelasticMat);
	shared_ptr<Aabb> aabb(new Aabb);
	shared_ptr<Sphere> iSphere(new Sphere);
	
	body->state->pos		=position;
	body->state->ori		=Quaternionr::Identity();
	body->state->vel		=Vector3r(0,0,0);
	body->state->angVel		=Vector3r(0,0,0);

	Real masse			=4.0/3.0*Mathr::PI*radius*radius*radius*density;
	body->state->mass		=masse;
	body->state->inertia		= Vector3r(2.0/5.0*masse*radius*radius,2.0/5.0*masse*radius*radius,2.0/5.0*masse*radius*radius);

	mat->young			= sphereYoungModulus;
	mat->poisson			= spherePoissonRatio;
	mat->frictionAngle		= sphereFrictionDeg * Mathr::PI/180.0;
	body->material = mat;

	aabb->color		= Vector3r(0,1,0);

	iSphere->radius			= radius;
	iSphere->color		= ((int)(floor(8*position.x()/length)))%2?Vector3r(0.7,0.7,0.7):Vector3r(0.45,0.45,0.45);// so that we have eight different colour bands

	body->shape			= iSphere;
	body->bound			= aabb;
}

// ----------------------------------------------------------------------------------------------
bool PositionBasedFluids::solveDensityConstraint(
	const unsigned int particleIndex,
	const unsigned int numberOfParticles,
	const Vector3r x[],	
	const Real mass[],
	const Vector3r boundaryX[],
	const Real boundaryPsi[],
	const unsigned int numNeighbors,
	const unsigned int neighbors[],
	const Real density0,
	const bool boundaryHandling,
	const Real lambda[],
	Vector3r &corr)
{
	// Compute position correction
	corr.setZero();
	for (unsigned int j = 0; j < numNeighbors; j++)
	{
		const unsigned int neighborIndex = neighbors[j];
		if (neighborIndex < numberOfParticles)		// Test if fluid particle
		{
			const Vector3r gradC_j = -mass[neighborIndex] / density0 * CubicKernel::gradW(x[particleIndex] - x[neighborIndex]);
			corr -= (lambda[particleIndex] + lambda[neighborIndex]) * gradC_j;
		}
		else if (boundaryHandling)
		{
			// Boundary: Akinci2012
			const Vector3r gradC_j = -boundaryPsi[neighborIndex - numberOfParticles] / density0 * CubicKernel::gradW(x[particleIndex] - boundaryX[neighborIndex - numberOfParticles]);
			corr -= (lambda[particleIndex]) * gradC_j;
		}
	}

	return true;
}

string SimpleShear::GenerateCloud(vector<BasicSphere>& sphere_list,Vector3r lowerCorner,Vector3r upperCorner,long number,Real rad_std_dev, Real porosity)
{
	sphere_list.clear();
	long tries = 1000; //nb max of tries for positionning the next sphere
	Vector3r dimensions = upperCorner - lowerCorner;
	Real mean_radius = pow(dimensions.x()*dimensions.y()*dimensions.z()*(1-porosity)/(4.0/3.0*Mathr::PI*number),1.0/3.0);
	cerr << " mean radius " << mean_radius << endl;;

// 	std::cerr << "generating aggregates ... ";
	
	long t, i;
	for (i=0; i<number; ++i) 
	{
		BasicSphere s;
		for (t=0; t<tries; ++t) 
		{
			s.second = (Mathr::UnitRandom()-0.5)*rad_std_dev*mean_radius+mean_radius;
			s.first.x() = lowerCorner.x()+s.second+(dimensions.x()-2*s.second)*Mathr::UnitRandom();
			s.first.y() = lowerCorner.y()+s.second+(dimensions.y()-2*s.second)*Mathr::UnitRandom();
			s.first.z() = lowerCorner.z()+s.second+(dimensions.z()-2*s.second)*Mathr::UnitRandom();
			bool overlap=false;
			for (long j=0; (j<i && !overlap); j++)
				if ( pow(sphere_list[j].second+s.second, 2) > (sphere_list[j].first-s.first).squaredNorm()) overlap=true;
			if (!overlap)
			{
				sphere_list.push_back(s);
// 				cout << "j'ai bien rajoute une sphere dans la liste" << endl;
				break;
			}
		}
		if (t==tries) 
		{
		string str1="Generated a sample with " + boost::lexical_cast<string>(i) + " spheres inside box of dimensions: (" 
			+ boost::lexical_cast<string>(dimensions[0]) + "," 
			+ boost::lexical_cast<string>(dimensions[1]) + "," 
			+ boost::lexical_cast<string>(dimensions[2]) + ").\n";
		return str1 + "More than " + boost::lexical_cast<string>(tries) +	" tries while generating sphere number " +
					boost::lexical_cast<string>(i+1) + "/" + boost::lexical_cast<string>(number) + ".";
		}
	}
	return "Generated a sample with " + boost::lexical_cast<string>(number) + " spheres inside box of dimensions: (" 
			+ boost::lexical_cast<string>(dimensions[0]) + "," 
			+ boost::lexical_cast<string>(dimensions[1]) + "," 
			+ boost::lexical_cast<string>(dimensions[2]) + ").";
}

void NewtonIntegrator::updateEnergy(const shared_ptr<Body>& b, const State* state, const Vector3r& fluctVel, const Vector3r& f, const Vector3r& m){
	assert(b->isStandalone() || b->isClump());
	// always positive dissipation, by-component: |F_i|*|v_i|*damping*dt (|T_i|*|ω_i|*damping*dt for rotations)
	if(damping!=0. && state->isDamped){
		scene->energy->add(fluctVel.cwise().abs().dot(f.cwise().abs())*damping*scene->dt,"nonviscDamp",nonviscDampIx,/*non-incremental*/false);
		// when the aspherical integrator is used, torque is damped instead of ang acceleration; this code is only approximate
		scene->energy->add(state->angVel.cwise().abs().dot(m.cwise().abs())*damping*scene->dt,"nonviscDamp",nonviscDampIx,false);
	}
	// kinetic energy
	Real Etrans=.5*state->mass*fluctVel.squaredNorm();
	Real Erot;
	// rotational terms
	if(b->isAspherical()){
		Matrix3r mI; mI<<state->inertia[0],0,0, 0,state->inertia[1],0, 0,0,state->inertia[2];
		Matrix3r T(state->ori);
		Erot=.5*b->state->angVel.transpose().dot((T.transpose()*mI*T)*b->state->angVel);
	} else { Erot=0.5*state->angVel.dot(state->inertia.cwise()*state->angVel); }
	if(!kinSplit) scene->energy->add(Etrans+Erot,"kinetic",kinEnergyIx,/*non-incremental*/true);
	else{ scene->energy->add(Etrans,"kinTrans",kinEnergyTransIx,true); scene->energy->add(Erot,"kinRot",kinEnergyRotIx,true); }
}

Vector3r SafetyEval::getDestination(const Vector3r& cur_pos, const Vector3r& velocity) const
{
    //breaking distance at this velocity
    float velocity_mag = velocity.norm();
    float dest_pos_dist = std::max(velocity_mag * vehicle_params_.vel_to_breaking_dist,
        vehicle_params_.min_breaking_dist);

    //calculate dest_pos cur_pos we will be if we had to break suddenly
    return velocity_mag >= vehicle_params_.distance_accuracy ? 
        (cur_pos + (velocity / velocity_mag) * dest_pos_dist) : cur_pos;
}

	void _gts_face_to_stl(GtsTriangle* t,_gts_face_to_stl_data* data){
		GtsVertex* v[3];
		Vector3r n;
		gts_triangle_vertices(t,&v[0],&v[1],&v[2]);
		if(data->clipCell && data->scene->isPeriodic){
			for(short ax:{0,1,2}){
				Vector3r p(GTS_POINT(v[ax])->x,GTS_POINT(v[ax])->y,GTS_POINT(v[ax])->z);
				if(!data->scene->cell->isCanonical(p)) return;
			}
		}
		gts_triangle_normal(t,&n[0],&n[1],&n[2]);
		n.normalize();
		std::ofstream& stl(data->stl);
		stl<<"  facet normal "<<n[0]<<" "<<n[1]<<" "<<n[2]<<"\n";
		stl<<"    outer loop\n";
		for(GtsVertex* _v: v){
			stl<<"      vertex "<<GTS_POINT(_v)->x<<" "<<GTS_POINT(_v)->y<<" "<<GTS_POINT(_v)->z<<"\n";
		}
		stl<<"    endloop\n";
		stl<<"  endfacet\n";
		data->numTri+=3;
	}

std::tuple<vector<shared_ptr<Node>>,vector<shared_ptr<Particle>>> SphereClumpGeom::makeParticles(const shared_ptr<Material>& mat, const Vector3r& clumpPos, const Quaternionr& clumpOri, int mask, Real scale){
	ensureOk();
	assert(centers.size()==radii.size());
	const auto N=centers.size();
	if(N==1){ // fast path for a single sphere (do not clump at all)
		auto s=DemFuncs::makeSphere(radii[0]*scale,mat);
		s->mask=mask;
		s->shape->nodes[0]->pos=(isnan(clumpPos.maxCoeff())?centers[0]:clumpPos); // natural or forced position
		return std::make_tuple(vector<shared_ptr<Node>>({s->shape->nodes[0]}),vector<shared_ptr<Particle>>({s}));
	}
	vector<shared_ptr<Particle>> par(N);
	auto n=make_shared<Node>();
	auto cd=make_shared<ClumpData>();
	n->setData<DemData>(cd);
	n->pos=(isnan(clumpPos.maxCoeff())?pos:clumpPos);
	n->ori=clumpOri;
	cd->nodes.resize(N);
	cd->relPos.resize(N);
	cd->relOri.resize(N);
	for(size_t i=0; i<N; i++){
		par[i]=DemFuncs::makeSphere(radii[i]*scale,mat);
		par[i]->mask=mask;
		cd->nodes[i]=par[i]->shape->nodes[0];
		cd->nodes[i]->getData<DemData>().setClumped(n); // sets flag and assigned master node
		cd->relPos[i]=(centers[i]-pos)*scale;
		cd->relOri[i]=ori.conjugate(); // nice to set, but not really important
	}
	// sets particles in global space based on relPos, relOri
	ClumpData::applyToMembers(n);
	// set clump properties
	assert(!isnan(volume));
	cd->setClump(); assert(cd->isClump());
	// scale = length scale (but not density scale)
	cd->mass=mat->density*volume*pow(scale,3);
	cd->inertia=mat->density*inertia*pow(scale,5);
	cd->equivRad=equivRad;
	return std::make_tuple(vector<shared_ptr<Node>>({n}),par);
}

void Leapfrog::leapfrogAsphericalRotate(const shared_ptr<Node>& node, const Vector3r& M){
	Quaternionr& ori(node->ori); DemData& dyn(node->getData<DemData>());
	Vector3r& angMom(dyn.angMom);	Vector3r& angVel(dyn.angVel);	const Vector3r& inertia(dyn.inertia);
	// initialize angular momentum; it is in local coordinates, therefore angVel must be rotated
	if(isnan(angMom.minCoeff())){
		angMom=dyn.inertia.asDiagonal()*node->ori.conjugate()*dyn.angVel;
	}

	Matrix3r A=ori.conjugate().toRotationMatrix(); // rotation matrix from global to local r.f.
	const Vector3r l_n = angMom + dt/2 * M; // global angular momentum at time n
	const Vector3r l_b_n = A*l_n; // local angular momentum at time n
	const Vector3r angVel_b_n = (l_b_n.array()/inertia.array()).matrix(); // local angular velocity at time n
	const Quaternionr dotQ_n=DotQ(angVel_b_n,ori); // dQ/dt at time n
	const Quaternionr Q_half = ori + dt/2 * dotQ_n; // Q at time n+1/2
	angMom+=dt*M; // global angular momentum at time n+1/2
	const Vector3r l_b_half = A*angMom; // local angular momentum at time n+1/2
	Vector3r angVel_b_half = (l_b_half.array()/inertia.array()).matrix(); // local angular velocity at time n+1/2
	const Quaternionr dotQ_half=DotQ(angVel_b_half,Q_half); // dQ/dt at time n+1/2
	ori=ori+dt*dotQ_half; // Q at time n+1
	angVel=ori*angVel_b_half; // global angular velocity at time n+1/2

	ori.normalize();
}

//float/vec parameters can have NaN which makes them optional
void SafetyEval::setSafety(SafetyViolationType enable_reasons, float obs_clearance, SafetyEval::ObsAvoidanceStrategy obs_strategy,
    const Vector3r& origin, float xy_length, float max_z, float min_z)
{
    if (!origin.hasNaN() && !std::isnan(xy_length) && !std::isnan(max_z) && !std::isnan(min_z))    
        fence_ptr_->setBoundry(origin, xy_length, max_z, min_z);

    if (!std::isnan(obs_clearance))
        vehicle_params_.obs_clearance = obs_clearance;

    enable_reasons_ = enable_reasons;
    setObsAvoidanceStrategy(obs_strategy);

    Utils::log(Utils::stringf("enable_reasons: %X", uint(enable_reasons)));
}