1
// © Václav Šmilauer <eudoxos@arcig.cz>
2
#include "Dem3DofGeom_FacetSphere.hpp"
3
#include<yade/pkg/common/Sphere.hpp>
4
#include<yade/pkg/common/Facet.hpp>
5
YADE_PLUGIN((Dem3DofGeom_FacetSphere)
7
(Gl1_Dem3DofGeom_FacetSphere)
9
(Ig2_Facet_Sphere_Dem3DofGeom));
11
CREATE_LOGGER(Dem3DofGeom_FacetSphere);
12
Dem3DofGeom_FacetSphere::~Dem3DofGeom_FacetSphere(){}
14
void Dem3DofGeom_FacetSphere::setTgPlanePts(const Vector3r& p1new, const Vector3r& p2new){
15
TRVAR3(cp1pt,cp2rel,contPtInTgPlane2()-contPtInTgPlane1());
16
cp1pt=se31.orientation.conjugate()*(turnPlanePt(p1new,normal,se31.orientation*localFacetNormal)+contactPoint-se31.position);
17
cp2rel=se32.orientation.conjugate()*Dem3DofGeom_SphereSphere::rollPlanePtToSphere(p2new,effR2,-normal);
18
TRVAR3(cp1pt,cp2rel,contPtInTgPlane2()-contPtInTgPlane1());
21
void Dem3DofGeom_FacetSphere::relocateContactPoints(const Vector3r& p1, const Vector3r& p2){
22
//TRVAR2(p2.norm(),effR2);
23
if(p2.squaredNorm()>pow(effR2,2)){
24
setTgPlanePts(Vector3r::Zero(),p2-p1);
28
Real Dem3DofGeom_FacetSphere::slipToDisplacementTMax(Real displacementTMax){
29
//FIXME: not yet tested
30
// negative or zero: reset shear
31
if(displacementTMax<=0.){ setTgPlanePts(Vector3r(0,0,0),Vector3r(0,0,0)); return displacementTMax;}
33
Vector3r p1=contPtInTgPlane1(), p2=contPtInTgPlane2();
34
Real currDistSq=(p2-p1).squaredNorm();
35
if(currDistSq<pow(displacementTMax,2)) return 0; // close enough, no slip needed
36
//Vector3r diff=.5*(sqrt(currDistSq)/displacementTMax-1)*(p2-p1); setTgPlanePts(p1+diff,p2-diff);
37
Real scale=displacementTMax/sqrt(currDistSq); setTgPlanePts(scale*p1,scale*p2);
38
return (displacementTMax/scale)*(1-scale);
41
CREATE_LOGGER(Ig2_Facet_Sphere_Dem3DofGeom);
42
bool Ig2_Facet_Sphere_Dem3DofGeom::go(const shared_ptr<Shape>& cm1, const shared_ptr<Shape>& cm2, const State& state1, const State& state2, const Vector3r& shift2, const bool& force, const shared_ptr<Interaction>& c){
43
Facet* facet=static_cast<Facet*>(cm1.get());
44
Real sphereRadius=static_cast<Sphere*>(cm2.get())->radius;
46
// IGeomFunctor::go(cm1,cm2,state1,state2,shift2,force,c);
49
/* new code written from scratch, to make sure the algorithm is correct; it is about the same speed
50
as sega's algo below, but seems more readable to me.
51
The FACET_TOPO thing is still missing here but can be copied literally once it is tested */
52
// begin facet-local coordinates
53
Vector3r cogLine=state1.ori.conjugate()*(state2.pos+shift2-state1.pos); // connect centers of gravity
54
//TRVAR4(state1.pos,state1.ori,state2.pos,cogLine);
55
Vector3r normal=facet->normal;
56
Real planeDist=normal.dot(cogLine);
57
if(planeDist<0){normal*=-1; planeDist*=-1; }
58
if(planeDist>sphereRadius && !c->isReal() && !force) { /* LOG_TRACE("Sphere too far ("<<planeDist<<") from plane"); */ return false; }
59
Vector3r planarPt=cogLine-planeDist*normal; // project sphere center to the facet plane
61
Vector3r contactPt(Vector3r::Zero());
62
for(int i=0; i<3; i++) normDotPt[i]=facet->ne[i].dot(planarPt-facet->vertices[i]);
63
short w=(normDotPt[0]>0?1:0)+(normDotPt[1]>0?2:0)+(normDotPt[2]>0?4:0);
64
//TRVAR4(planarPt,normDotPt[0],normDotPt[1],normDotPt[2]);
65
//TRVAR2(normal,cogLine);
66
//TRVAR3(facet->vertices[0],facet->vertices[1],facet->vertices[2]);
68
case 0: contactPt=planarPt; break; // inside triangle
69
case 1: contactPt=getClosestSegmentPt(planarPt,facet->vertices[0],facet->vertices[1]); break; // +-- (n1)
70
case 2: contactPt=getClosestSegmentPt(planarPt,facet->vertices[1],facet->vertices[2]); break; // -+- (n2)
71
case 4: contactPt=getClosestSegmentPt(planarPt,facet->vertices[2],facet->vertices[0]); break; // --+ (n3)
72
case 3: contactPt=facet->vertices[1]; break; // ++- (v1)
73
case 5: contactPt=facet->vertices[0]; break; // +-+ (v0)
74
case 6: contactPt=facet->vertices[2]; break; // -++ (v2)
75
case 7: throw logic_error("Impossible triangle intersection?"); // +++ (impossible)
76
default: throw logic_error("Nonsense intersection value!");
78
normal=cogLine-contactPt; // called normal, but it is no longer the facet's normal (for compat)
79
//TRVAR3(normal,contactPt,sphereRadius);
80
if(!c->isReal() && normal.squaredNorm()>sphereRadius*sphereRadius && !force) { /* LOG_TRACE("Sphere too far from closest point"); */ return false; } // fast test before sqrt
81
Real norm=normal.norm(); normal/=norm; // normal is unit vector now
82
Real penetrationDepth=sphereRadius-norm;
84
/* This code was mostly copied from InteractingFacet2InteractinSphere4SpheresContactGeometry */
85
// begin facet-local coordinates
86
Vector3r contactLine=state1.ori.Conjugate()*(state2.pos+shift2-state1.pos);
87
Vector3r normal=facet->normal;
88
Real L=normal.Dot(contactLine); // height/depth of sphere's center from facet's plane
89
if(L<0){normal*=-1; L*=-1;}
90
if(L>sphereRadius && !c->isReal()) return false; // sphere too far away from the plane
92
Vector3r contactPt=contactLine-L*normal; // projection of sphere's center to facet's plane (preliminary contact point)
93
const Vector3r* edgeNormals=facet->ne; // array[3] of edge normals (in facet plane)
94
int edgeMax=0; Real distMax=edgeNormals[0].Dot(contactPt);
95
for(int i=1; i<3; i++){
96
Real dist=edgeNormals[i].Dot(contactPt);
97
if(distMax<dist){edgeMax=i; distMax=dist;}
99
//TRVAR2(distMax,edgeMax);
100
// OK, what's the logic here? Copying from IF2IS4SCG…
101
Real sphereRReduced=shrinkFactor*sphereRadius;
102
Real inCircleR=facet->icr-sphereRReduced;
103
Real penetrationDepth;
104
if(inCircleR<0){inCircleR=facet->icr; sphereRReduced=0;}
105
if(distMax<inCircleR){// contact with facet's surface
106
penetrationDepth=sphereRadius-L;
108
} else { // contact with the edge
109
contactPt+=edgeNormals[edgeMax]*(inCircleR-distMax);
110
bool noVertexContact=false;
111
//TRVAR3(edgeNormals[edgeMax],inCircleR,distMax);
112
// contact with vertex no. edgeMax
113
// FIXME: this is the original version, but why (edgeMax-1)%3? IN that case, edgeNormal to edgeMax would never be tried
114
// if (contactPt.Dot(edgeNormals[ (edgeMax-1)%3])>inCircleR) contactPt=facet->vu[edgeMax]*(facet->vl[edgeMax]-sphereRReduced);
115
if (contactPt.Dot(edgeNormals[ edgeMax ])>inCircleR) contactPt=facet->vu[edgeMax]*(facet->vl[edgeMax]-sphereRReduced);
116
// contact with vertex no. edgeMax+1
117
else if(contactPt.Dot(edgeNormals[edgeMax=(edgeMax+1)%3])>inCircleR) contactPt=facet->vu[edgeMax]*(facet->vl[edgeMax]-sphereRReduced);
118
// contact with edge no. edgeMax
119
else noVertexContact=true;
120
normal=contactLine-contactPt;
122
if(noVertexContact && facet->edgeAdjIds[edgeMax]!=Body::ID_NONE){
123
// find angle between our normal and the facet's normal (still local coords)
124
Quaternionr q; q.Align(facet->normal,normal); AngleAxisr aa(q);
125
assert(aa.angle()>=0 && aa.angle()<=Mathr::PI);
126
if(edgeNormals[edgeMax].Dot(aa.axis())<0) aa.angle()*=-1.;
127
bool negFace=normal.Dot(facet->normal)<0; // contact in on the negative facet's face
128
Real halfAngle=(negFace?-1.:1.)*facet->edgeAdjHalfAngle[edgeMax];
129
if(halfAngle<0 && aa.angle()>halfAngle) return false; // on concave boundary, and if in the other facet's sector, no contact
130
// otherwise the contact will be created
133
//TRVAR4(contactLine,contactPt,normal,normal.norm());
134
//TRVAR3(se31.orientation*contactLine,se31.position+se31.orientation*contactPt,se31.orientation*normal);
135
Real norm=normal.norm(); normal/=norm;
136
penetrationDepth=sphereRadius-norm;
137
//TRVAR1(penetrationDepth);
139
// end facet-local coordinates
142
if(penetrationDepth<0 && !c->isReal()) return false;
145
shared_ptr<Dem3DofGeom_FacetSphere> fs;
146
Vector3r normalGlob=state1.ori*normal;
148
if(c->geom) fs=YADE_PTR_CAST<Dem3DofGeom_FacetSphere>(c->geom);
150
// LOG_TRACE("Creating new Dem3DofGeom_FacetSphere");
151
fs=shared_ptr<Dem3DofGeom_FacetSphere>(new Dem3DofGeom_FacetSphere());
154
fs->effR2=sphereRadius-penetrationDepth;
155
fs->refR1=-1; fs->refR2=sphereRadius;
156
// postponed till below, to avoid numeric issues
157
// see https://lists.launchpad.net/yade-dev/msg02794.html
158
// since displacementN() is computed from fs->contactPoint,
159
// it was returning something like +1e-16 at the very first step
160
// when it was created ⇒ the constitutive law was erasing the
161
// contact as soon as it was created.
163
fs->cp1pt=contactPt; // facet-local intial contact point
164
fs->localFacetNormal=facet->normal;
165
fs->cp2rel.setFromTwoVectors(Vector3r::UnitX(),state2.ori.conjugate()*(-normalGlob)); // initial sphere-local center-contactPt orientation WRT +x
166
fs->cp2rel.normalize();
168
fs->se31=state1.se3; fs->se32=state2.se3; fs->se32.position+=shift2;
169
fs->normal=normalGlob;
170
fs->contactPoint=state2.pos+shift2+(-normalGlob)*(sphereRadius-penetrationDepth);
171
// this refLength computation mimics what displacementN() does inside
172
// displcementN will therefore return exactly zero at the step the contact
173
// was created, which is what we want
174
if(isNew) fs->refLength=(state2.pos+shift2-fs->contactPoint).norm();
180
#include<yade/lib/opengl/OpenGLWrapper.hpp>
181
#include<yade/lib/opengl/GLUtils.hpp>
183
bool Gl1_Dem3DofGeom_FacetSphere::normal=false;
184
bool Gl1_Dem3DofGeom_FacetSphere::rolledPoints=false;
185
bool Gl1_Dem3DofGeom_FacetSphere::unrolledPoints=false;
186
bool Gl1_Dem3DofGeom_FacetSphere::shear=false;
187
bool Gl1_Dem3DofGeom_FacetSphere::shearLabel=false;
189
void Gl1_Dem3DofGeom_FacetSphere::go(const shared_ptr<IGeom>& ig, const shared_ptr<Interaction>& ip, const shared_ptr<Body>& b1, const shared_ptr<Body>& b2, bool wireFrame){
190
Dem3DofGeom_FacetSphere* fs = static_cast<Dem3DofGeom_FacetSphere*>(ig.get());
191
const Se3r& se31=b1->state->se3,se32=b2->state->se3;
192
const Vector3r& pos1=se31.position; const Vector3r& pos2=se32.position;
193
const Quaternionr& ori1=se31.orientation; const Quaternionr& ori2=se32.orientation;
194
const Vector3r& contPt=fs->contactPoint;
197
GLUtils::GLDrawArrow(contPt,contPt+fs->refLength*fs->normal); // normal of the contact
199
// sphere center to point on the sphere
201
//cerr<<pos1<<" "<<pos1+ori1*fs->cp1pt<<" "<<contPt<<endl;
202
GLUtils::GLDrawLine(pos1+ori1*fs->cp1pt,contPt,Vector3r(0,.5,1));
203
GLUtils::GLDrawLine(pos2,pos2+(ori2*fs->cp2rel*Vector3r::UnitX()*fs->effR2),Vector3r(0,1,.5));
205
// contact point to projected points
206
if(unrolledPoints||shear){
207
Vector3r ptTg1=fs->contPtInTgPlane1(), ptTg2=fs->contPtInTgPlane2();
209
//TRVAR3(ptTg1,ptTg2,ss->normal)
210
GLUtils::GLDrawLine(contPt,contPt+ptTg1,Vector3r(0,.5,1));
211
GLUtils::GLDrawLine(contPt,contPt+ptTg2,Vector3r(0,1,.5)); GLUtils::GLDrawLine(pos2,contPt+ptTg2,Vector3r(0,1,.5));
214
GLUtils::GLDrawLine(contPt+ptTg1,contPt+ptTg2,Vector3r(1,1,1));
215
if(shearLabel) GLUtils::GLDrawNum(fs->displacementT().norm(),contPt,Vector3r(1,1,1));
added
removed
pkg/dem/Dem3DofGeom_FacetSphere.cpp
