Permalink
Switch branches/tags
Nothing to show
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
465 lines (426 sloc) 21.6 KB
#include<woo/pkg/dem/Capsule.hpp>
#include<woo/pkg/dem/Sphere.hpp>
WOO_PLUGIN(dem,(Capsule)(Bo1_Capsule_Aabb)(Cg2_Sphere_Capsule_L6Geom)(Cg2_Wall_Capsule_L6Geom)(Cg2_Facet_Capsule_L6Geom)(Cg2_InfCylinder_Capsule_L6Geom)(Cg2_Capsule_Capsule_L6Geom));
WOO_IMPL__CLASS_BASE_DOC_ATTRS_CTOR_PY(woo_dem_Capsule__CLASS_BASE_DOC_ATTRS_CTOR_PY);
WOO_IMPL__CLASS_BASE_DOC(woo_dem_Bo1_Capsule_Aabb__CLASS_BASE_DOC);
WOO_IMPL__CLASS_BASE_DOC(woo_dem_Cg2_Capsule_Capsule_L6Geom__CLASS_BASE_DOC);
WOO_IMPL__CLASS_BASE_DOC(woo_dem_Cg2_Wall_Capsule_L6Geom__CLASS_BASE_DOC);
WOO_IMPL__CLASS_BASE_DOC(woo_dem_Cg2_Facet_Capsule_L6Geom__CLASS_BASE_DOC);
WOO_IMPL__CLASS_BASE_DOC(woo_dem_Cg2_InfCylinder_Capsule_L6Geom__CLASS_BASE_DOC);
WOO_IMPL__CLASS_BASE_DOC(woo_dem_Cg2_Sphere_Capsule_L6Geom__CLASS_BASE_DOC);
#ifdef WOO_OPENGL
WOO_PLUGIN(gl,(Gl1_Capsule));
WOO_IMPL__CLASS_BASE_DOC(woo_dem_Gl1_Capsule__CLASS_BASE_DOC);
#endif
void Capsule::selfTest(const shared_ptr<Particle>& p){
if(!(radius>0.) || !(shaft>=0.)) throw std::runtime_error("Capsule #"+to_string(p->id)+": radius must be positive and shaft non-negative (current: radius="+to_string(radius)+", shaft="+to_string(shaft)+").");
if(!numNodesOk()) throw std::runtime_error("Capsule #"+to_string(p->id)+": numNodesOk() failed: must be 1, not "+to_string(nodes.size())+".");
Shape::selfTest(p);
}
Real Capsule::volume() const {
return (4/3.)*M_PI*pow3(radius)+M_PI*pow2(radius)*shaft;
}
Real Capsule::equivRadius() const {
return cbrt(this->Capsule::volume()*3/(4*M_PI));
}
bool Capsule::isInside(const Vector3r& pt) const{
Vector3r l(nodes[0]->glob2loc(pt));
// cylinder test
if(abs(l[0])<shaft/2.) return pow2(l[1])+pow2(l[2])<pow2(radius);
// cap test (distance to endpoint)
return (Vector3r(l[0]<0?-shaft/2.:shaft/2.,0,0)-l).squaredNorm()<pow2(radius);
}
void Capsule::applyScale(Real scale){ radius*=scale; shaft*=scale; }
void Capsule::lumpMassInertia(const shared_ptr<Node>& n, Real density, Real& mass, Matrix3r& I, bool& rotateOk){
if(n.get()!=nodes[0].get()) return; // not our node
rotateOk=false; // node may not be rotated without geometry change
checkNodesHaveDemData();
Real r2(pow2(radius)), r3(pow3(radius));
Real mCaps=(4/3.)*M_PI*r3*density;
Real mShaft=M_PI*r2*shaft*density;
Real distCap=.5*shaft+(3/8.)*radius; // distance between centroid and the cap's centroid
Real Ix=2*mCaps*r2/5.+.5*mShaft*r2;
Real Iy=(83/320.)*mCaps*r2+mCaps*pow2(distCap)+(1/12.)*mShaft*(3*r2+pow2(shaft));
I.diagonal()+=Vector3r(Ix,Iy,Iy);
mass+=mCaps+mShaft;
}
#if 0
void Capsule::updateMassInertia(const Real& density) {
Real m(0.); Matrix3r I(Matrix3r::Zero()); bool rotateOk;
lumpMassInertia(nodes[0],density,m,I,rotateOk);
// FIXME: only correct if this is the only particle in the node
}
#endif
AlignedBox3r Capsule::alignedBox() const {
const auto& pos(nodes[0]->pos); const auto& ori(nodes[0]->ori);
Vector3r dShaft=ori*Vector3r(.5*shaft,0,0);
AlignedBox3r ret;
for(int a:{-1,1}) for(int b:{-1,1}) ret.extend(pos+a*dShaft+b*radius*Vector3r::Ones());
return ret;
}
void Capsule::asRaw(Vector3r& _center, Real& _radius, vector<shared_ptr<Node>>&nn, vector<Real>& raw) const {
_center=nodes[0]->pos;
_radius=radius+.5*shaft;
// store as half-shaft vector
// radius will be bounding sphere radius minus shaft/2
raw.resize(3);
Eigen::Map<Vector3r> hShaft(raw.data());
hShaft=nodes[0]->ori*Vector3r(.5*shaft,0,0);
}
void Capsule::setFromRaw(const Vector3r& _center, const Real& _radius, vector<shared_ptr<Node>>& nn, const vector<Real>& raw) {
Shape::setFromRaw_helper_checkRaw_makeNodes(raw,3);
Eigen::Map<const Vector3r> hShaft(raw.data());
nodes[0]->pos=_center;
shaft=2*hShaft.norm();
radius=_radius-.5*shaft;
if(shaft>0.) nodes[0]->ori.setFromTwoVectors(Vector3r::UnitX(),hShaft.normalized());
else nodes[0]->ori=Quaternionr::Identity();
nn.push_back(nodes[0]);
}
void Bo1_Capsule_Aabb::go(const shared_ptr<Shape>& sh){
if(!sh->bound){ sh->bound=make_shared<Aabb>(); /* consider node rotation*/ sh->bound->cast<Aabb>().maxRot=0.; }
Aabb& aabb=sh->bound->cast<Aabb>();
const auto& c(sh->cast<Capsule>());
if(!scene->isPeriodic || !scene->cell->hasShear()){
aabb.box=c.Capsule::alignedBox(); // non-virtual call
} else {
aabb.box.setEmpty();
Vector3r extents=scene->cell->shearAlignedExtents(Vector3r::Constant(c.radius));
// as if sphere for both endpoints
for(int e:{0,1}){
Vector3r ue=scene->cell->unshearPt(c.endPt(e));
aabb.box.extend(ue+extents);
aabb.box.extend(ue-extents);
}
}
}
bool Cg2_Wall_Capsule_L6Geom::go(const shared_ptr<Shape>& s1, const shared_ptr<Shape>& s2, const Vector3r& shift2, const bool& force, const shared_ptr<Contact>& C){
if(scene->isPeriodic && scene->cell->hasShear()) throw std::logic_error("Cg2_Wall_Capsule_L6Geom does not handle periodic boundary conditions with skew (Scene.cell.trsf is not diagonal).");
const auto& wall(s1->cast<Wall>()); const auto& cap(s2->cast<Capsule>());
const auto& wallPos(wall.nodes[0]->pos);
const auto& capNode(cap.nodes[0]);
const Vector3r capPos(capNode->pos+shift2); const auto& capOri(capNode->ori);
const DemData& wallDyn(wall.nodes[0]->getData<DemData>()); const DemData& capDyn(capNode->getData<DemData>());
const int& ax=wall.axis; const int& sense=wall.sense;
Vector3r AB[2]={capPos-capOri*Vector3r(cap.shaft/2.,0,0),capPos+capOri*Vector3r(cap.shaft/2.,0,0)};
Vector2r sgnAxDiff(AB[0][ax]-wallPos[ax],AB[1][ax]-wallPos[ax]);
// too far from each other
if(!C->isReal() && (abs(sgnAxDiff[0])>cap.radius && abs(sgnAxDiff[1])>cap.radius) && !force) return false;
Real cDist=capPos[ax]-wallPos[ax]; // centroid position decides which way to fly
// normal vector
Vector3r normal(Vector3r::Zero());
if(sense==0){
if(!C->geom){
normal[ax]=cDist>0?1.:-1.; // new contacts (depending on current position)
}
else normal[ax]=C->geom->cast<L6Geom>().trsf.col(0)[ax]; // existing contacts (preserve previous)
}
else normal[ax]=(sense==1?1.:-1);
// contact point
Vector3r contPt;
Real uN;
// overlaps for both sides of the capsule
Vector2r unun=sgnAxDiff*normal[ax]-Vector2r(cap.radius,cap.radius);
if(unun.prod()<0){
// points on opposite sides of the wall
int p=(unun[0]<0?0:1); // the point with overlap (uN<0) takes all
contPt=AB[p];
uN=unun[p];
} else if(unun.sum()!=0.){
// position according the level of overlap on each side, using weighted average
Vector2r rel=unun/unun.sum();
if(unun[0]>0) std::swap(rel[0],rel[1]); // inverse the weights if there is no overlap -- the one close to the wall gets more
// alternative: rel=unun.normalized();
assert(rel[0]>=0 && rel[1]>=0);
contPt=rel[0]*AB[0]+rel[1]*AB[1];
uN=rel[0]*unun[0]+rel[1]*unun[1];
} else { // this is very unlikely
contPt=.5*(AB[0]+AB[1]);
uN=0.;
}
contPt[ax]=wallPos[ax];
handleSpheresLikeContact(C,wallPos,wallDyn.vel,wallDyn.angVel,capPos,capDyn.vel,capDyn.angVel,normal,contPt,uN,/*r1*/-cap.radius,cap.radius);
return true;
}
bool Cg2_InfCylinder_Capsule_L6Geom::go(const shared_ptr<Shape>& s1, const shared_ptr<Shape>& s2, const Vector3r& shift2, const bool& force, const shared_ptr<Contact>& C){
if(scene->isPeriodic && scene->cell->hasShear()) throw std::logic_error("Cg2_InfCylinder_Capsule_L6Geom does not handle periodic boundary conditions with skew (Scene.cell.trsf is not diagonal).");
const auto& cyl(s1->cast<InfCylinder>()); const auto& cap(s2->cast<Capsule>());
const auto& cylPos(cyl.nodes[0]->pos);
const auto& capNode(cap.nodes[0]);
const Vector3r capPos(capNode->pos+shift2); const auto& capOri(capNode->ori);
const DemData& cylDyn(cyl.nodes[0]->getData<DemData>()); const DemData& capDyn(capNode->getData<DemData>());
const int& ax=cyl.axis; const int ax1((ax+1)%3), ax2((ax+2)%3);
// do the computation in 2d
Vector2r capPos2(capPos[ax1],capPos[ax2]);
Vector3r capHalf=capOri*Vector3r(cap.shaft/2.,0,0);
Vector2r capHalf2(capHalf[ax1],capHalf[ax2]);
Vector2r cylPos2(cylPos[ax1],cylPos[ax2]);
// see http://www.geometrictools.com/Documentation/DistancePointLine.pdf
Real t=CompUtils::clamped(capHalf2.dot(cylPos2-capPos2)/capHalf2.squaredNorm(),-1,1);
Vector2r cyl2seg2=(capPos2+t*capHalf2)-cylPos2; // relative position from cylinder to segment in 2d
Real distSq=cyl2seg2.squaredNorm();
if(!C->isReal() && distSq>pow2(cyl.radius+cap.radius) && !force) return false;
Vector3r normal=Vector3r::Zero(); normal[ax1]=cyl2seg2[0]; normal[ax2]=cyl2seg2[1]; normal.normalize();
Real uN=sqrt(distSq)-(cyl.radius+cap.radius);
Vector3r contPt=cylPos; contPt[ax]=(capPos+t*capHalf)[ax]; contPt+=(cyl.radius+.5*uN)*normal;
handleSpheresLikeContact(C,cylPos,cylDyn.vel,cylDyn.angVel,capPos,capDyn.vel,capDyn.angVel,normal,contPt,uN,cyl.radius,cap.radius);
return true;
}
void Cg2_Sphere_Capsule_L6Geom::setMinDist00Sq(const shared_ptr<Shape>& s1, const shared_ptr<Shape>& s2, const shared_ptr<Contact>& C){
const auto& s(s1->cast<Sphere>()); const auto& c(s2->cast<Capsule>());
C->minDist00Sq=pow2(s.radius+c.radius+.5*c.shaft);
}
bool Cg2_Sphere_Capsule_L6Geom::go(const shared_ptr<Shape>& s1, const shared_ptr<Shape>& s2, const Vector3r& shift2, const bool& force, const shared_ptr<Contact>& C){
const auto& sphere(s1->cast<Sphere>()); const auto& cap(s2->cast<Capsule>());
const auto& spherePos(sphere.nodes[0]->pos);
const auto& capNode(cap.nodes[0]);
const Vector3r capPos(capNode->pos+shift2); const auto& capOri(capNode->ori);
const DemData& sphereDyn(sphere.nodes[0]->getData<DemData>()); const DemData& capDyn(capNode->getData<DemData>());
Real rSum(sphere.radius+cap.radius);
Vector3r AB[2]={capPos-capOri*Vector3r(cap.shaft/2.,0,0),capPos+capOri*Vector3r(cap.shaft/2.,0,0)};
Vector3r pt=CompUtils::closestSegmentPt(spherePos,AB[0],AB[1]);
Real distSq=(pt-spherePos).squaredNorm();
if(!C->isReal() && distSq>pow2(rSum) && !force) return false;
Real dist=sqrt(distSq);
Vector3r normal=(pt-spherePos)/dist;
Real uN=dist-rSum;
Vector3r contPt=spherePos+normal*(sphere.radius+.5*uN);
handleSpheresLikeContact(C,spherePos,sphereDyn.vel,sphereDyn.angVel,capPos,capDyn.vel,capDyn.angVel,normal,contPt,uN,sphere.radius,cap.radius);
return true;
}
void Cg2_Capsule_Capsule_L6Geom::setMinDist00Sq(const shared_ptr<Shape>& s1, const shared_ptr<Shape>& s2, const shared_ptr<Contact>& C){
const auto& c1(s1->cast<Capsule>()); const auto& c2(s2->cast<Capsule>());
C->minDist00Sq=pow2(c1.radius+.5*c1.shaft+c2.radius+.5*c2.shaft);
}
bool Cg2_Capsule_Capsule_L6Geom::go(const shared_ptr<Shape>& s1, const shared_ptr<Shape>& s2, const Vector3r& shift2, const bool& force, const shared_ptr<Contact>& C){
const auto& c1(s1->cast<Capsule>()); const auto& c2(s2->cast<Capsule>());
const Vector3r& pos1(c1.nodes[0]->pos); const Vector3r pos2(c2.nodes[0]->pos+shift2);
const auto& ori1(c1.nodes[0]->ori); const auto& ori2(c2.nodes[0]->ori);
const auto& dyn1(c1.nodes[0]->getData<DemData>()); const auto& dyn2(c2.nodes[0]->getData<DemData>());
Vector3r dir1(ori1*Vector3r::UnitX()), dir2(ori2*Vector3r::UnitX());
Real rSum=(c1.radius+c2.radius);
Vector2r uv;
bool parallel;
Real distSq=CompUtils::distSq_SegmentSegment(pos1,dir1,.5*c1.shaft,pos2,dir2,.5*c2.shaft,uv,parallel);
if(!C->isReal() && distSq>pow2(rSum) && !force) return false;
/* now check all endpoints against the other's segment, to detect if the capsules cross, or if they touch in the elongated direction, in which case we have to interpolate between ends just like in the wall contact (here, things are more complicated, as there are two capsules).
The trick is to use the closest distance for overlap and normal computation, but endpoint overlaps for weighted contact point position. This should ensure smooth transition for all possible configurations without jump.
*/
//cerr<<"segment dist "<<sqrt(distSq)<<", capsule gap "<<sqrt(distSq)-rSum*rSum<<endl;
//cerr<<"parameters: "<<uv.transpose()<<endl;
Vector3r closePts[2]={pos1+dir1*uv[0],pos2+dir2*uv[1]};
//cerr<<"segment point 1: "<<closePts[0]<<endl;
//cerr<<"segment point 2: "<<closePts[1]<<endl;
Vector3r ends[2][2]={{pos1-.5*c1.shaft*dir1,pos1+.5*c1.shaft*dir1},{pos2-.5*c2.shaft*dir2,pos2+.5*c2.shaft*dir2}};
Real dist=sqrt(distSq);
Real uN=dist-(c1.radius+c2.radius);
Vector3r normal=(closePts[1]-closePts[0])/dist;
Vector3r contPt;
int bitsOver=0; // bitmap for overlaps
short nOver=0; // number of overlaps - count of set bits (perhaps not the fastest, but does the job)
Vector3r eClose[2][2];
Real eDistSq[2][2];
for(int a:{0,1}){
int b((a+1)%2);
for(int e:{0,1}){
eClose[a][e]=CompUtils::closestSegmentPt(ends[a][e],ends[b][0],ends[b][1]);
eDistSq[a][e]=(eClose[a][e]-ends[a][e]).squaredNorm();
if(eDistSq[a][e]<pow2(rSum)){ bitsOver|=1<<(2*a+e); nOver++; }
}
}
Real r1r=c1.radius/rSum, r2r=c2.radius/rSum;
if(nOver<2){
contPt=closePts[0]+(c1.radius+0.5*uN)*normal;
}
else if(nOver==4){
/* average all four points with their weights */
//cerr<<"============"<<endl;
Vector3r pp=Vector3r::Zero(); Real ww=0.;
for(short ix0:{0,1}) for(short ix1:{0,1}){
Real rel=(ix0==0?r2r:r1r);
Real w=rSum-sqrt(eDistSq[ix0][ix1]);
Vector3r p=eClose[ix0][ix1]+(ends[ix0][ix1]-eClose[ix0][ix1])*rel;
//cerr<<"["<<ix0<<","<<ix1<<"]: p="<<p<<", w="<<w<<", rel"<<"="<<rel<<endl;
//cerr<<"close "<<eClose[ix0][ix1]<<endl<<"end "<<ends[ix0][ix1]<<endl;
pp+=w*p;
ww+=w;
}
contPt=pp/ww;
//cerr<<"contact point "<<contPt<<endl;
} else {
assert(nOver==2 || nOver==3);
short iii[16][2][2]={
{{0,0},{0,0}}, /* 0; unused */
{{0,0},{0,0}}, /* 1; unused */
{{0,0},{0,0}}, /* 2; unused */
{{0,0},{0,1}}, /* 3=1+2 */
{{0,0},{0,0}}, /* 4; unused */
{{0,0},{1,0}}, /* 5=1+4 */
{{0,1},{1,0}}, /* 6=2+4 */
{{0,0},{0,1}}, /* 7=1+2+4: just like 1+2 */
{{0,0},{0,0}}, /* 8; unused */
{{0,0},{1,1}}, /* 9=1+8 */
{{0,1},{1,1}}, /* A=10=2+8 */
{{0,0},{0,1}}, /* B=11=1+2+8; just like 1+2 */
{{1,0},{1,1}}, /* C=12=4+8 */
{{1,0},{1,1}}, /* D=13=1+4+8; just like 4+8 */
{{1,0},{1,1}}, /* E=14=2+4+8; just like 4+8 */
{{0,0},{0,0}} /* F=1+2+4+8; unused */
};
const auto& ix(iii[bitsOver]);
Vector3r pts[2];
Vector2r weights;
//cerr<<"============"<<endl;
//cerr<<"nOver="<<nOver<<", bitsOver="<<bitsOver<<endl;
for(int i:{0,1}){
const auto& ix0(ix[i][0]); const auto& ix1(ix[i][1]);
Real rel=(ix0==0?r2r:r1r);
pts[i]=eClose[ix0][ix1]+(ends[ix0][ix1]-eClose[ix0][ix1])*rel;
weights[i]=rSum-sqrt(eDistSq[ix0][ix1]);
//cerr<<"indices "<<ix0<<","<<ix1<<endl;
//cerr<<"endpoint "<<ends[ix0][ix1]<<", close point "<<eClose[ix0][ix1]<<endl;
//cerr<<"pts["<<i<<"]="<<pts[i]<<", weights["<<i<<"]="<<weights[i]<<endl;
assert(weights[i]>0);
}
contPt=(weights[0]*pts[0]+weights[1]*pts[1])/weights.sum();
}
handleSpheresLikeContact(C,pos1,dyn1.vel,dyn1.angVel,pos2,dyn2.vel,dyn2.angVel,normal,contPt,uN,c1.radius,c2.radius);
return true;
}
WOO_IMPL_LOGGER(Cg2_Facet_Capsule_L6Geom);
bool Cg2_Facet_Capsule_L6Geom::go(const shared_ptr<Shape>& s1, const shared_ptr<Shape>& s2, const Vector3r& shift2, const bool& force, const shared_ptr<Contact>& C){
const auto& facet(s1->cast<Facet>()); const auto& cap(s2->cast<Capsule>());
// const auto& wallPos(wall.nodes[0]->pos);
const auto& capNode(cap.nodes[0]);
const Vector3r capPos(capNode->pos+shift2); const auto& capOri(capNode->ori);
const DemData& capDyn(capNode->getData<DemData>());
Vector3r fNormal=facet.getNormal();
Vector3r AB[2]={capPos-capOri*Vector3r(cap.shaft/2.,0,0),capPos+capOri*Vector3r(cap.shaft/2.,0,0)};
const Vector3r& f0pos(facet.nodes[0]->pos);
typedef Eigen::Array<Real,2,1> Array2r;
Array2r planeDists;
for(int i:{0,1}) planeDists[i]=(AB[i]-f0pos).dot(fNormal);
bool mayFail=(!C->isReal() && !force);
// points on the same side and too far from the plane
Real touchDist=facet.halfThick+cap.radius;
if(mayFail && (std::signbit(planeDists[0])==std::signbit(planeDists[1])) && planeDists.abs().minCoeff()>touchDist){
// cerr<<"Too far from the plane: "<<planeDists.transpose()<<endl;
return false;
}
// compute points on the facet which are the closest to the segment ends
Vector3r ffp[2]={facet.getNearestPt(AB[0]),facet.getNearestPt(AB[1])};
// find points on the segment closest to those points (project back onto the segment)
Array2r rp; // relative positions on the segment
Vector3r ccp[2]={CompUtils::closestSegmentPt(ffp[0],AB[0],AB[1],&rp[0]),CompUtils::closestSegmentPt(ffp[1],AB[0],AB[1],&rp[1])};
Array2r fcd2((ccp[0]-ffp[0]).squaredNorm(),(ccp[1]-ffp[1]).squaredNorm());
if(mayFail && fcd2.minCoeff()>pow2(touchDist)){
#if 0
cerr<<"===================================================="<<endl;
cerr<<" Too far from the triangle: "<<fcd2.transpose()<<endl;
cerr<<" Segment endpoints: "<<AB[0].transpose()<<"; "<<AB[1].transpose()<<endl;
cerr<<" Triangle points: "<<ffp[0].transpose()<<"; "<<ffp[1].transpose()<<endl;
cerr<<" Segment points: "<<ccp[0].transpose()<<"; "<<ccp[1].transpose()<<endl;
#endif
return false; // too far from the triangle
}
Array2r fcd=fcd2.sqrt();
// penetration is the geometrical max
Real uN=fcd.minCoeff()-touchDist;
// normal vector and contact point are geometry-dependent
Vector3r normal, contPt;
// if one of the points is beyond physical touch, do no interpolation and take the close one
// (if both are beyond, then take the closer one anyway as the contact must be computed;
// the case of too far was handled above already)
if(fcd.maxCoeff()>touchDist){
// cerr<<"[1]";
Array2r::Index ix;
fcd.minCoeff(&ix);
normal=(ccp[ix]-ffp[ix]).normalized();
contPt=ffp[ix]+(facet.halfThick+0.5*uN)*normal;
} else {
//cerr<<"[2]";
Array2r weights(fcd[0]-touchDist,fcd[1]-touchDist);
weights/=weights.sum();
// if(weights.maxCoeff()>0) LOG_ERROR("weights are positive but should not be: "<<weights.transpose());
normal=(weights[0]*(ccp[0]-ffp[0])+weights[1]*(ccp[1]-ffp[1])).normalized(); // interpolated normal; is this not a nonsense?!
#if 0
contPt=weights[0]*ccp[0]+weights[1]*ccp[1]; // point on the segment line weighted by distance
contPt+=-normal*(cap.radius+.5*uN);
#else
contPt=weights[0]*ffp[0]+weights[1]*ffp[1]+(facet.halfThick+.5*uN)*normal;
#endif
// interpolate between two points; this also handles the case when both
#if 0
cerr<<"===================================================="<<endl;
cerr<<" Segment endpoints: "<<AB[0].transpose()<<"; "<<AB[1].transpose()<<endl;
cerr<<" Triangle points: "<<ffp[0].transpose()<<"; "<<ffp[1].transpose()<<endl;
cerr<<" Segment points: "<<ccp[0].transpose()<<"; "<<ccp[1].transpose()<<endl;
cerr<<" normal: "<<normal<<endl;
cerr<<" weights: "<<weights.transpose()<<endl;
cerr<<" contPt: "<<contPt.transpose()<<endl;
#endif
}
Vector3r linVel,angVel;
std::tie(linVel,angVel)=facet.interpolatePtLinAngVel(contPt);
handleSpheresLikeContact(C,contPt,linVel,angVel,capPos,capDyn.vel,capDyn.angVel,normal,contPt,uN,/*r1*/max(facet.halfThick,cap.radius),cap.radius);
return true;
}
#ifdef WOO_OPENGL
#include<woo/pkg/gl/Functors.hpp>
#include<woo/lib/opengl/OpenGLWrapper.hpp>
#include<woo/pkg/gl/Renderer.hpp>
#include<woo/lib/base/CompUtils.hpp>
#include<woo/lib/opengl/GLUtils.hpp>
void Gl1_Capsule::go(const shared_ptr<Shape>& shape, const Vector3r& shift, bool wire2,const GLViewInfo& glInfo){
const shared_ptr<Node>& n=shape->nodes[0];
Vector3r dPos=(n->hasData<GlData>()?n->getData<GlData>().dGlPos:Vector3r::Zero());
Quaternionr dOri=(n->hasData<GlData>()?n->getData<GlData>().dGlOri:Quaternionr::Identity());
const auto& capsule(shape->cast<Capsule>());
Real r=scale*capsule.radius;
Real shaft=scale*capsule.shaft;
bool doPoints=(glInfo.renderer->fastDraw || quality<0 || (int)(quality*glutSlices)<2 || (int)(quality*glutStacks)<2);
if(doPoints){
if(smooth) glEnable(GL_POINT_SMOOTH);
else glDisable(GL_POINT_SMOOTH);
GLUtils::setLocalCoords(dPos+shape->nodes[0]->pos+shift,(dOri*shape->nodes[0]->ori));
glPointSize(1.);
if(shaft>0){
glBegin(GL_LINE_STRIP);
glVertex3v(Vector3r(-shaft/2.,0,0));
glVertex3v(Vector3r( shaft/2.,0,0));
glEnd();
} else {
glBegin(GL_POINTS);
glVertex3v(Vector3r(0,0,0));
glEnd();
}
return;
}
static double clipPlaneA[4]={0,0,-1,0};
static double clipPlaneB[4]={0,0,1,0};
// 2nd rotation: rotate so that the OpenGL z-axis aligns with the local x-axis that way the glut sphere poles will be at the capsule's poles, looking prettier
// 1st rotation: rotate around x so that vertices or segments of cylinder and half-spheres coindice
Quaternionr axisRot=/* Quaternionr(AngleAxisr(M_PI/2.,Vector3r::UnitX()))* */Quaternionr(AngleAxisr(M_PI/2.,Vector3r::UnitY())); // XXX: to be fixed
GLUtils::setLocalCoords(dPos+shape->nodes[0]->pos+shift,(dOri*shape->nodes[0]->ori*axisRot));
int cylStacks(max(1.,.5*(shaft/r)*quality*glutStacks)); // approx the same dist as on the caps
if (wire || wire2 ){
glLineWidth(1.);
if(!smooth) glDisable(GL_LINE_SMOOTH);
GLUtils::Cylinder(Vector3r(0,0,-shaft/2.),Vector3r(0,0,shaft/2.),r,/*color: keep current*/Vector3r(-1,-1,-1),/*wire*/true,/*caps*/false,r,quality*glutSlices,/*stacks*/cylStacks);
glEnable(GL_CLIP_PLANE0);
glTranslatef(0,0,-shaft/2.); glClipPlane(GL_CLIP_PLANE0,clipPlaneA); glutWireSphere(r,quality*glutSlices,quality*glutStacks);
glTranslatef(0,0,shaft); glClipPlane(GL_CLIP_PLANE0,clipPlaneB); glutWireSphere(r,quality*glutSlices,quality*glutStacks);
glDisable(GL_CLIP_PLANE0);
if(!smooth) glEnable(GL_LINE_SMOOTH); // re-enable
}
else {
glEnable(GL_LIGHTING);
glShadeModel(GL_SMOOTH);
GLUtils::Cylinder(Vector3r(0,0,-shaft/2.),Vector3r(0,0,shaft/2.),r,/*color: keep current*/Vector3r(-1,-1,-1),/*wire*/false,/*caps*/false,r,quality*glutSlices,/*stacks*/cylStacks);
glEnable(GL_CLIP_PLANE0);
glTranslatef(0,0,-shaft/2.); glClipPlane(GL_CLIP_PLANE0,clipPlaneA); glutSolidSphere(r,quality*glutSlices,quality*glutStacks);
glTranslatef(0,0,shaft); glClipPlane(GL_CLIP_PLANE0,clipPlaneB); glutSolidSphere(r,quality*glutSlices,quality*glutStacks);
glDisable(GL_CLIP_PLANE0);
}
}
#endif /* WOO_OPENGL */