Skip to content
Permalink
Branch: master
Find file Copy path
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
354 lines (307 sloc) 17.5 KB
#include<woo/pkg/dem/Ellipsoid.hpp>
#include<woo/pkg/dem/Sphere.hpp>
#include <boost/math/tools/minima.hpp>
#include <boost/math/tools/tuple.hpp>
WOO_PLUGIN(dem,(Ellipsoid)(Bo1_Ellipsoid_Aabb)(Cg2_Wall_Ellipsoid_L6Geom)(Cg2_Facet_Ellipsoid_L6Geom)(Cg2_Ellipsoid_Ellipsoid_L6Geom)(Cg2_Sphere_Ellipsoid_L6Geom));
WOO_IMPL__CLASS_BASE_DOC_ATTRS_CTOR(woo_dem_Ellipsoid__CLASS_BASE_DOC_ATTRS_CTOR);
WOO_IMPL__CLASS_BASE_DOC(woo_dem_Bo1_Ellipsoid_Aabb__CLASS_BASE_DOC);
WOO_IMPL__CLASS_BASE_DOC_ATTRS(woo_dem_Cg2_Ellipsoid_Ellipsoid_L6Geom__CLASS_BASE_DOC_ATTRS);
WOO_IMPL__CLASS_BASE_DOC(woo_dem_Cg2_Wall_Ellipsoid_L6Geom__CLASS_BASE_DOC);
WOO_IMPL__CLASS_BASE_DOC(woo_dem_Cg2_Sphere_Ellipsoid_L6Geom__CLASS_BASE_DOC);
WOO_IMPL__CLASS_BASE_DOC(woo_dem_Cg2_Facet_Ellipsoid_L6Geom__CLASS_BASE_DOC);
#ifdef WOO_OPENGL
WOO_PLUGIN(gl,(Gl1_Ellipsoid));
WOO_IMPL__CLASS_BASE_DOC(woo_dem_Gl1_Ellipsoid__CLASS_BASE_DOC_ATTRS);
#endif
void woo::Ellipsoid::selfTest(const shared_ptr<Particle>& p){
if(!(semiAxes.minCoeff()>0)) throw std::runtime_error("Ellipsoid #"+to_string(p->id)+": all semi-princial semiAxes must be positive (current minimum is "+to_string(semiAxes.minCoeff())+")");
if(!numNodesOk()) throw std::runtime_error("Ellipsoid #"+to_string(p->id)+": numNodesOk() failed: must be 1, not "+to_string(nodes.size())+".");
Shape::selfTest(p);
}
Real woo::Ellipsoid::equivRadius() const {
// volume-based equivalent radius
return cbrt(semiAxes.prod());
};
Real woo::Ellipsoid::volume() const { return (4/3.)*M_PI*semiAxes.prod(); }
void woo::Ellipsoid::applyScale(Real scale) { semiAxes*=scale; }
bool woo::Ellipsoid::isInside(const Vector3r& pt) const {
Vector3r l=nodes[0]->glob2loc(pt);
return pow2(l[0]/semiAxes[0])+pow2(l[1]/semiAxes[1])+pow2(l[2]/semiAxes[2])<=1;
}
void woo::Ellipsoid::lumpMassInertia(const shared_ptr<Node>&, Real density, Real& mass, Matrix3r& I, bool& rotateOk){
rotateOk=false;
checkNodesHaveDemData();
Real m=(4/3.)*M_PI*semiAxes.prod()*density;
mass+=m;
I.diagonal()+=(1/5.)*m*Vector3r(pow2(semiAxes[1])+pow2(semiAxes[2]),pow2(semiAxes[2])+pow2(semiAxes[0]),pow2(semiAxes[0])+pow2(semiAxes[1]));
}
void woo::Ellipsoid::asRaw(Vector3r& _center, Real& _radius, vector<shared_ptr<Node>>&nn, vector<Real>& raw) const{
_center=nodes[0]->pos;
_radius=semiAxes.maxCoeff();
AngleAxisr aa(nodes[0]->ori);
raw.resize(6);
Eigen::Map<Vector3r> rawOri(raw.data());
Eigen::Map<Vector3r> rawSemiAxes(raw.data()+3);
rawOri=aa.axis()*aa.angle();
rawSemiAxes=semiAxes;
}
void woo::Ellipsoid::setFromRaw(const Vector3r& _center, const Real& _radius, vector<shared_ptr<Node>>& nn, const vector<Real>& raw){
Shape::setFromRaw_helper_checkRaw_makeNodes(raw,6);
Eigen::Map<const Vector3r> rawOri(raw.data());
Eigen::Map<const Vector3r> rawSemiAxes(raw.data()+3);
nodes[0]->pos=_center;
Real n=rawOri.norm();
if(n==0.) nodes[0]->ori=Quaternionr::Identity();
else nodes[0]->ori=Quaternionr(AngleAxisr(n,rawOri/n));
nn.push_back(nodes[0]);
semiAxes=rawSemiAxes;
}
/* return matrix transforming unit sphere to this ellipsoid */
Matrix3r woo::Ellipsoid::trsfFromUnitSphere() const{
Matrix3r M;
for(int i:{0,1,2}) M.col(i)=nodes[0]->ori*(semiAxes[i]*Vector3r::Unit(i));
return M;
}
/* return extents (half-size) spanned in the direction of *axis*; this is the same algo
as when computing Aabb, but evaluated for one axis only.
*/
Real woo::Ellipsoid::axisExtent(short axis) const {
Matrix3r M=trsfFromUnitSphere();
return M.row(axis).norm();
}
AlignedBox3r Ellipsoid::alignedBox() const {
Matrix3r M=trsfFromUnitSphere();
// http://www.loria.fr/~shornus/ellipsoid-bbox.html
const Vector3r& pos(nodes[0]->pos);
Vector3r delta(M.row(0).norm(),M.row(1).norm(),M.row(2).norm());
return AlignedBox3r(pos-delta,pos+delta);
};
void Bo1_Ellipsoid_Aabb::go(const shared_ptr<Shape>& sh){
if(!sh->bound){ sh->bound=make_shared<Aabb>(); /* consider rotation*/ sh->bound->cast<Aabb>().maxRot=0.; }
Aabb& aabb=sh->bound->cast<Aabb>();
const Vector3r& pos(sh->nodes[0]->pos);
if(!scene->isPeriodic || !scene->cell->hasShear()){
Matrix3r M=sh->cast<Ellipsoid>().trsfFromUnitSphere();
// http://www.loria.fr/~shornus/ellipsoid-bbox.html
Vector3r delta(M.row(0).norm(),M.row(1).norm(),M.row(2).norm());
aabb.min=pos-delta;
aabb.max=pos+delta;
} else {
Vector3r extents;
const Matrix3r& sT=scene->cell->getShearTrsf();
for(int i:{0,1,2}){
Quaternionr q; q.setFromTwoVectors(Vector3r::Unit(i),sT.col((i+1)%3).cross(sT.col((i+2)%3)));
Matrix3r M=sh->cast<Ellipsoid>().trsfFromUnitSphere(q.conjugate());
extents[i]=M.row(i).norm();
}
extents=scene->cell->shearAlignedExtents(extents);
aabb.min=scene->cell->unshearPt(pos)-extents;
aabb.max=scene->cell->unshearPt(pos)+extents;
}
}
// the same funcs but with extra rotation
// (for computing in rotated coordinates)
Matrix3r woo::Ellipsoid::trsfFromUnitSphere(const Quaternionr& ori) const{
Matrix3r M;
for(int i:{0,1,2}) M.col(i)=ori*nodes[0]->ori*(semiAxes[i]*Vector3r::Unit(i));
return M;
}
Real woo::Ellipsoid::rotatedExtent(short axis, const Quaternionr& ori) const {
Matrix3r M=trsfFromUnitSphere(ori);
return M.row(axis).norm();
}
bool Cg2_Facet_Ellipsoid_L6Geom::go(const shared_ptr<Shape>& s1, const shared_ptr<Shape>& s2, const Vector3r& shift2, const bool& force, const shared_ptr<Contact>& C){
const Facet& facet(s1->cast<Facet>());
const Ellipsoid& ell(s2->cast<Ellipsoid>()); const Quaternionr& ellOri(ell.nodes[0]->ori);
const Vector3r& facetPosA(facet.nodes[0]->pos); const Vector3r ellPos(ell.nodes[0]->pos+shift2);
// compute local orientation where facet normal is the z-axis
Vector3r facetNormal=facet.getNormal();
Quaternionr locOri; locOri.setFromTwoVectors(facetNormal,Vector3r::UnitZ()); // use this instead of the static Quaternionr::FromTwoVector which did not exist in earlier Eigen version (!)
Real locZExtent=ell.rotatedExtent(2,locOri); // always positive
// put ellipsoid in origin, and facet somewhere, perpendicular to z
// the z-coordinate should be the same for all facet vertices
Vector3r facetLocPosA=locOri*(facetPosA-ellPos);
Real uN=abs(facetLocPosA[2])-facet.halfThick-locZExtent;
if(uN>0 && !C->isReal() && !force){ return false; }
#if 1
static bool notImpl=false; // give the warning only once
if(notImpl){
cerr<<"[Cg2_Facet_Ellipsoid_L6Geom: not yet implemented]";
notImpl=true;
}
return false;
// algo A: transform ellipsoid to unit sphere in origin, compute closest point with thus transformed triangle
// the contact point is transformed back to global coords
// if the CP is inside the triangle, the normal is coincident with facet's normal
// otherwise compute the ellipsoid normal at the contact point (potential steepest descent) and use that
// Move the point by halfThick away from the facet
Matrix3r fromUnitSphere=ell.trsfFromUnitSphere();
Matrix3r toUnitSphere=fromUnitSphere.inverse();
// all points end with 't', signifying the transformed space
Vector3r vt[3]={toUnitSphere*(facet.nodes[0]->pos-ellPos),toUnitSphere*(facet.nodes[1]->pos-ellPos),toUnitSphere*(facet.nodes[2]->pos-ellPos)};
Vector3r nt=((vt[1]-vt[0]).cross(vt[2]-vt[0])).normalized(); // facet normal
Real distt=vt[0].dot(nt); // distance origin-plane
Vector3r pot=nt*distt; // origin projected onto the plane
Vector3r ont[3]={(vt[1]-vt[0]).cross(nt),(vt[2]-vt[1]).cross(nt),(vt[0]-vt[2]).cross(nt)}; // outer normals
cerr<<"-----------------"<<endl;
cerr<<"vt: "<<vt[0].transpose()<<" | "<<vt[1].transpose()<<" | "<<vt[2].transpose()<<endl;
cerr<<"distt: "<<distt<<endl;
cerr<<"nt: "<<nt.transpose()<<endl;
cerr<<"pot: "<<pot.transpose()<<endl;
cerr<<"ont :"<<ont[0].transpose()<<" | "<<ont[1].transpose()<<" | "<<ont[2].transpose()<<endl;
cerr<<"vg: "<<(fromUnitSphere*vt[0]).transpose()<<" | "<<(fromUnitSphere*vt[1]).transpose()<<" | "<<(fromUnitSphere*vt[2]).transpose()<<endl;
cerr<<"ng: "<<(fromUnitSphere*nt).transpose()<<endl;
cerr<<"pog: "<<(fromUnitSphere*pot).transpose()<<endl;
// this algo is similar to what is in Facet::getNearestPt, but we need to know whether the point is inside or at the edge in addition
short w=0;
for(int i:{0,1,2}) w&=(ont[i].dot(pot-vt[i])>0?1:0)<<i;
Vector3r cpt; // contact point in transformed space
switch(w){
case 0: cpt=pot; break; // ---: inside triangle
case 1: cpt=CompUtils::closestSegmentPt(pot,vt[0],vt[1]); break; // +-- (n1)
case 2: cpt=CompUtils::closestSegmentPt(pot,vt[1],vt[2]); break; // -+- (n2)
case 4: cpt=CompUtils::closestSegmentPt(pot,vt[2],vt[0]); break; // --+ (n3)
case 3: cpt=vt[1]; break; // ++- (v1)
case 5: cpt=vt[0]; break; // +-+ (v0)
case 6: cpt=vt[2]; break; // -++ (v2)
case 7: throw logic_error("Cg2_Facet_Ellipsoid_L6Geom: Impossible sphere-facet intersection (all points are outside the edges). (please report bug)"); // +++ (impossible)
default: throw logic_error("Cg2_Facet_Ellipsoid_L6Geom: Nonsense intersection value. (please report bug)");
}
// transform contact point back (origin in ellipsoid's center, global orientation)
Vector3r cpe=fromUnitSphere*cpt;
// cerr<<"{"<<cpt.norm()<<","<<w<<"}"; // should be 1 or less (with no halfThick)
// contact normal, in global space
Vector3r normal;
if(w==0){
// face contact, easy
normal=facetNormal;
} else{
cerr<<".";
// ellipsoid gradient is 2(x/a²,y/b²,z/c²) in local coords, use that to compute the normal
Vector3r cpl=ellOri.conjugate()*cpe; //contact point in ellipsoid-local coords
Vector3r gradl(cpl[0]/ell.semiAxes[0],cpl[1]/ell.semiAxes[1],cpl[2]/ell.semiAxes[2]);
normal=ellOri*gradl.normalized(); // transform back to global coords
}
Real contR=cpe.norm(); // distance to the contact point
Real facetContR=(facet.halfThick>0?-contR:facet.halfThick); // take halfThick in account here
Vector3r contPt=cpe+ellPos; // in global coords
if(facet.halfThick>0) contPt+=normal*facet.halfThick;
Vector3r facetLinVel,facetAngVel;
std::tie(facetLinVel,facetAngVel)=facet.interpolatePtLinAngVel(contPt);
// FIXME: the uN is totally bogus here, it is only valid when for face contact!!!!
const DemData& ellDyn(ell.nodes[0]->getData<DemData>());
// const DemData& facetDyn(facet.nodes[0]->getData<DemData>());
handleSpheresLikeContact(C,contPt,facetLinVel,facetAngVel,ellPos,ellDyn.vel,ellDyn.angVel,normal,contPt,uN,facetContR,contR);
return true;
#endif
}
bool Cg2_Wall_Ellipsoid_L6Geom::go(const shared_ptr<Shape>& s1, const shared_ptr<Shape>& s2, const Vector3r& shift2, const bool& force, const shared_ptr<Contact>& C){
const Wall& wall(s1->cast<Wall>());
const Ellipsoid& ell(s2->cast<Ellipsoid>());
const auto& ax(wall.axis); const auto& sense(wall.sense);
const Vector3r& wallPos(wall.nodes[0]->pos); const Vector3r ellPos(ell.nodes[0]->pos+shift2);
Real extent=ell.axisExtent(ax);
if(((wallPos[ax]<(ellPos[ax]-extent)) || (wallPos[ax]>(ellPos[ax]+extent))) && !C->isReal() && !force){ return false; }
// penetration distance and normal can be computed simply, being axis-aligned
Real dist=ellPos[ax]-wallPos[ax]; // signed distance: positive for ellipsoid above the wall
assert(sense==-1 || sense==0 || sense==1);
short normAxSgn; // sign of the normal along wall's axis
if(wall.sense==0) normAxSgn=dist>0?1:-1; // both-side wall
else normAxSgn=(sense==1?1:-1);
Vector3r normal=normAxSgn*Vector3r::Unit(ax);
Real uN=normAxSgn*dist-extent;
// for small deformations, we can suppose the contact point is the one touching the bounding box
// http://www.loria.fr/~shornus/ellipsoid-bbox.html -- using their notation
Matrix3r M(ell.trsfFromUnitSphere());
Matrix3r Mprime=M.transpose();
for(short i:{0,1,2}) Mprime.col(i).normalize();
// Columns of this product should be deltas from ellipsoid center to the extremal point, projected to the plane.
// normAxSign is ±1 depending on sense and position, re-use that one here, but opposite:
// it wall it touched from above, we need the lower point on the ellipsoid and vice versa
Vector3r contPt=ellPos+(-normAxSgn)*(M*Mprime).col(ax);
contPt[ax]=wallPos[ax];
Real contR=(contPt-ellPos).norm();
const DemData& ellDyn(ell.nodes[0]->getData<DemData>()); const DemData& wallDyn(wall.nodes[0]->getData<DemData>());
handleSpheresLikeContact(C,wallPos,wallDyn.vel,wallDyn.angVel,ellPos,ellDyn.vel,ellDyn.angVel,normal,contPt,uN,/*negative for wall*/-contR,contR);
return true;
}
void Cg2_Sphere_Ellipsoid_L6Geom::setMinDist00Sq(const shared_ptr<Shape>& s1, const shared_ptr<Shape>& s2, const shared_ptr<Contact>& C){ C->minDist00Sq=pow2(s1->cast<Sphere>().radius+s2->cast<Ellipsoid>().semiAxes.maxCoeff()); }
bool Cg2_Sphere_Ellipsoid_L6Geom::go(const shared_ptr<Shape>& s1, const shared_ptr<Shape>& s2, const Vector3r& shift2, const bool& force, const shared_ptr<Contact>& C){ return Cg2_Ellipsoid_Ellipsoid_L6Geom::go_Ellipsoid_or_Sphere(s1,s1->cast<Sphere>().radius*Vector3r::Ones(),s2,s2->cast<Ellipsoid>().semiAxes,shift2,force,C); }
void Cg2_Ellipsoid_Ellipsoid_L6Geom::setMinDist00Sq(const shared_ptr<Shape>& s1, const shared_ptr<Shape>& s2, const shared_ptr<Contact>& C){ C->minDist00Sq=pow2(s1->cast<Ellipsoid>().semiAxes.maxCoeff()+s2->cast<Ellipsoid>().semiAxes.maxCoeff()); }
bool Cg2_Ellipsoid_Ellipsoid_L6Geom::go(const shared_ptr<Shape>& s1, const shared_ptr<Shape>& s2, const Vector3r& shift2, const bool& force, const shared_ptr<Contact>& C){ return go_Ellipsoid_or_Sphere(s1,s1->cast<Ellipsoid>().semiAxes,s2,s2->cast<Ellipsoid>().semiAxes,shift2,force,C); }
// passes semiAxes explicitly, hence can be also called for sphere+ellipsoid
bool Cg2_Ellipsoid_Ellipsoid_L6Geom::go_Ellipsoid_or_Sphere(const shared_ptr<Shape>& s1, const Vector3r& semiAxesA, const shared_ptr<Shape>& s2, const Vector3r& semiAxesB, const Vector3r& shift2, const bool& force, const shared_ptr<Contact>& C){
/* notation taken from Perram, Rasmussen, Præstgaard, Lebowtz: Ellipsoid contact potential */
const Vector3r& ra(s1->nodes[0]->pos); Vector3r rb(s2->nodes[0]->pos+shift2);
const Vector3r& a(semiAxesA); const Vector3r& b(semiAxesB);
const Quaternionr& oa(s1->nodes[0]->ori); const Quaternionr& ob(s2->nodes[0]->ori);
Vector3r u[]={oa*Vector3r::UnitX(),oa*Vector3r::UnitY(),oa*Vector3r::UnitZ()};
Vector3r v[]={ob*Vector3r::UnitX(),ob*Vector3r::UnitY(),ob*Vector3r::UnitZ()};
const DemData& dyn1(s1->nodes[0]->getData<DemData>());const DemData& dyn2(s2->nodes[0]->getData<DemData>());
Vector3r R(rb-ra); // (2.1)
Matrix3r A(Matrix3r::Zero()), B(Matrix3r::Zero());
for(int k:{0,1,2}){
A+=u[k]*u[k].transpose()/pow2(a[k]); // (2.2a)
B+=v[k]*v[k].transpose()/pow2(b[k]); // (2.2b)
}
Matrix3r Ainv=A.inverse(), Binv=B.inverse(); // (2.3a), (2.3b)
// (2.4), for Brent's maximization: return only the function value
// the result is negated so that we can use boost::math::brent_find_minima which finds the minimum
auto neg_S_lambda_0=[&](const Real& l) -> Real { return -l*(1-l)*R.transpose()*((1-l)*Ainv+l*Binv).inverse()*R; };
// cerr<<"S(l):"; for(Real l=0; l<1; l+=.05){ cerr<<" "<<-neg_S_lambda_0(l); }; cerr<<endl;
// TODO: define funcs returning 1st derivative (zero at the extreme) and 2nd derivative (used by Newton-Raphson)
#if 0
// (2.4) + (2.8) for Newton-Raphson, return both value and derivative
auto S_lambda_01=[&](const Real& l) -> boost::math::tuple {
Matrix3r G=(1-l)*Ainv+l*Binv; // (2.6)
Matrix3r GinvR=G.inverse()*R; // (2.9): GinvR≡X (GX=R)
return boost::math::make_tuple(
l*(a-l)*R.transpose()*GinvR,
GinvR.transpose()*(pow2(1-l)*Ainv-pow2(l)*Binv)*GinvR
);
}
#endif
// set once the iteration has result
Real L,Fab;
if(brent){
// boost docs claims that accuracy higher than half bits is ignored, so set just that -- half of 8*sizeof(Real)
auto lambda_negSmin=boost::math::tools::brent_find_minima(neg_S_lambda_0,0.,1.,brentBits);
L=boost::math::get<0>(lambda_negSmin);
Fab=-boost::math::get<1>(lambda_negSmin); // invert the sign
} else {
#if 0
// use Newton-Raphson
// FIXME: we should use the previous value of lambda as the initial guess!
Real l0=a.maxCoeff()/(a.maxCoeff()+b/maxCoeff()); // initial guess as for spheres; Donev, pg 773
// this is what boost docs do in the example on newton_raphson_iterate
int digits=(2*std::numeric_limits<T>::digits)/3;
boost::math::newton_raphson_iterate(S_lambda_01,...)
#else
throw std::runtime_error("Cg2_Ellipsoid_Ellipsoid_L6Geom::go: Newton-Raphson iteration is not yet implemented; use Brent's method by saying brent=True.");
#endif
}
// cerr<<"l="<<L<<", Fab="<<Fab<<endl;
// Perram, Rasmussen, pg 6567
if(Fab>1 && !C->isReal() && !force){ return false; }
Matrix3r G=(1-L)*Ainv+L*Binv; // (2.6)
Vector3r nUnnorm=G.inverse()*R; // Donev, (19)
Vector3r contPt=ra+(1-L)*Ainv*nUnnorm; // Donev, (19)
// compute uN: Perram&Wertheim (3.17): Fab=μ², where μ is the scaling factor
Real mu=sqrt(Fab);
// μ scales sizes while keeping ellipsoid distance
// 1/μ therefore scales distance keeping ellipsoid sizes
// with d (≡|R|) being the current distance, we have
// uN'=d-d₀=d-(1/μ)*d=d(1-1/μ)
// however, the scaling approaches ellipsoids at that rate in the direction of R
// so if the normal has different orientation, we need to scale that by the cosine,
// which happens to be the scalar product of normalized rUnit and nUnit (normalized R and nUnnorm)
// so finally we have
// uN=uN'*cos(rUnit,nUnit)=|R|(1-1/μ)*(rUnit·nUnit)
Real Rnorm=R.norm();
Vector3r nUnit=nUnnorm.normalized(), rUnit=R/Rnorm;
Real uN=Rnorm*(1-1/mu)*rUnit.dot(nUnit);
// cerr<<"Fab="<<Fab<<", mu="<<mu<<", cos="<<rUnit.dot(nUnit)<<", l="<<l<<", l0="<<l0<<", |nUnnorm|="<<nUnnorm.norm()<<", uN="<<uN<<endl;
// cerr<<"lambda: "<<L<<", Fab: "<<Fab<<", n: "<<nUnnorm.transpose()<<", CP: "<<contPt.transpose()<<endl;
handleSpheresLikeContact(C,ra,dyn1.vel,dyn1.angVel,rb,dyn2.vel,dyn2.angVel,nUnit,contPt,uN,(contPt-ra).norm(),(contPt-rb).norm());
return true;
}
You can’t perform that action at this time.