TEST(IOBoardModelTest, SetMultipleOutOfOrder) {
IOBoardModel test = IOBoardModel(
Quaternionr(1, 0, 0, 0),
Vector3r(0, 0, 0),
Quaternionr(1, 0, 0, 0),
Quaternionr(1, 0, 0, 0),
Vector3r(0, 0, 0));
MeasurementVector target(10);
test.set_magnetometer(Vector3r(4, 5, 6));
test.set_accelerometer(Vector3r(1, 2, 3));
test.set_barometer_amsl(10);
test.set_gps_velocity(Vector3r(7, 8, 9));
target << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10;
EXPECT_EQ(test.collate(), target);
}
void Bo1_Polyhedra_Aabb::go(const shared_ptr<Shape>& ig, shared_ptr<Bound>& bv, const Se3r& se3, const Body*){
Polyhedra* t=static_cast<Polyhedra*>(ig.get());
if (!t->IsInitialized()) t->Initialize();
if(!bv){ bv=shared_ptr<Bound>(new Aabb); }
Aabb* aabb=static_cast<Aabb*>(bv.get());
//Quaternionr invRot=se3.orientation.conjugate();
int N = (int) t->v.size();
Vector3r v_g, mincoords(0.,0.,0.), maxcoords(0.,0.,0.);
for(int i=0; i<N; i++) {
v_g=se3.orientation*t->v[i]; // vertices in global coordinates
mincoords = Vector3r(min(mincoords[0],v_g[0]),min(mincoords[1],v_g[1]),min(mincoords[2],v_g[2]));
maxcoords = Vector3r(max(maxcoords[0],v_g[0]),max(maxcoords[1],v_g[1]),max(maxcoords[2],v_g[2]));
}
aabb->min=se3.position+mincoords;
aabb->max=se3.position+maxcoords;
}
void CylGlRep::render(const shared_ptr<Node>& node, const GLViewInfo* viewInfo){
Real radius=viewInfo->sceneRadius*relSz*(isnan(rad)?1:(rangeRad?rangeRad->norm(rad):1));
Real ccol=isnan(col)?0:col;
Vector3r color=(rangeCol?rangeCol->color(ccol):CompUtils::scalarOnColorScale(ccol,0,1));
if(isnan(color.maxCoeff())) return;
Vector3r A=(node->pos+node->ori.conjugate()*Vector3r(xx[0],0,0)), B=(node->pos+node->ori.conjugate()*Vector3r(xx[1],0,0));
// cerr<<"A="<<A.transpose()<<", B="<<B.transpose()<<", r="<<radius<<endl;
GLUtils::Cylinder(A,B,radius,color,/*wire*/false,/*caps*/false,/*rad2*/-1,/*slices*/10,2);
}
void NewtonIntegrator::ensureSync()
{
if (syncEnsured) return;
YADE_PARALLEL_FOREACH_BODY_BEGIN(const shared_ptr<Body>& b, scene->bodies){
// if(b->isClump()) continue;
scene->forces.addForce(b->getId(),Vector3r(0,0,0));
} YADE_PARALLEL_FOREACH_BODY_END();
syncEnsured=true;
}
void In2_Membrane_ElastMat::go(const shared_ptr<Shape>& sh, const shared_ptr<Material>& m, const shared_ptr<Particle>& particle){
auto& ff=sh->cast<Membrane>();
if(!particle->contacts.empty()) distributeForces(particle,sh->cast<Facet>(),/*bary*/bending||applyBary);
ff.stepUpdate(scene->dt,rotIncr);
// assemble local stiffness matrix, in case it does not exist yet
ff.ensureStiffnessMatrices(particle->material->cast<ElastMat>().young,nu,thickness,/*bending*/bending,bendThickness);
// compute nodal forces response here
// ?? CST forces are applied with the - sign, DKT with the + sign; are uXy/phiXy introduced differently?
Vector6r Fcst=-(ff.KKcst*ff.uXy).transpose();
Vector9r Fdkt;
if(bending){
#ifdef MEMBRANE_CONDENSE_DKT
assert(ff.KKdkt.size()==54);
Fdkt=(ff.KKdkt*ff.phiXy).transpose();
#else
assert(ff.KKdkt.size()==81);
Vector9r uDkt_;
uDkt_<<0,ff.phiXy.segment<2>(0),0,ff.phiXy.segment<2>(2),0,ff.phiXy.segment<2>(4);
Fdkt=(ff.KKdkt*uDkt_).transpose();
#ifdef MEMBRANE_DEBUG_ROT
ff.uDkt=uDkt_; // debugging copy, acessible from python
#endif
#endif
} else {
Fdkt=Vector9r::Zero();
}
LOG_TRACE("CST: "<<Fcst.transpose())
LOG_TRACE("DKT: "<<Fdkt.transpose())
// surface load, if any
Real surfLoadForce=0.;
if(!isnan(ff.surfLoad) && ff.surfLoad!=0.){ surfLoadForce=(1/3.)*ff.getArea()*ff.surfLoad; }
// apply nodal forces
for(int i:{0,1,2}){
Vector3r Fl=Vector3r(Fcst[2*i],Fcst[2*i+1],Fdkt[3*i]+surfLoadForce);
Vector3r Tl=Vector3r(Fdkt[3*i+1],Fdkt[3*i+2],0);
ff.nodes[i]->getData<DemData>().addForceTorque(ff.node->ori*Fl,ff.node->ori*Tl);
LOG_TRACE(" "<<i<<" F: "<<Fl.transpose()<<" \t| "<<ff.node->ori*Fl);
LOG_TRACE(" "<<i<<" T: "<<Tl.transpose()<<" \t| "<<ff.node->ori*Tl);
}
}
/*! Draws a 3D arrow between the 3D point \p from and the 3D point \p to, both defined in the
current ModelView coordinates system.
See drawArrow(float length, float radius, int nbSubdivisions) for details. */
void GLUtils::QGLViewer::drawArrow(const Vector3r& from, const Vector3r& to, float radius, int nbSubdivisions)
{
glPushMatrix();
glTranslatef(from[0],from[1],from[2]);
Quaternionr q(Quaternionr().setFromTwoVectors(Vector3r(0,0,1),to-from));
//glMultMatrixd(q.toRotationMatrix().data());
glMultMatrix(q.toRotationMatrix().data());
drawArrow((to-from).norm(), radius, nbSubdivisions);
glPopMatrix();
}
bool Law2_ScGeom_MindlinPhys_HertzWithLinearShear::go(shared_ptr<IGeom>& ig, shared_ptr<IPhys>& ip, Interaction* contact){
Body::id_t id1(contact->getId1()), id2(contact->getId2());
ScGeom* geom = static_cast<ScGeom*>(ig.get());
MindlinPhys* phys=static_cast<MindlinPhys*>(ip.get());
const Real uN=geom->penetrationDepth;
if (uN<0) {
if (neverErase) {phys->shearForce = phys->normalForce = Vector3r::Zero(); phys->kn=phys->ks=0; return true;}
else return false;
}
// normal force
Real Fn=phys->kno*pow(uN,3/2.);
phys->normalForce=Fn*geom->normal;
//phys->kn=3./2.*phys->kno*std::pow(uN,0.5); // update stiffness, not needed
// shear force
Vector3r& Fs=geom->rotate(phys->shearForce);
Real ks= nonLin>0 ? phys->kso*std::pow(uN,0.5) : phys->kso;
Vector3r shearIncrement;
if(nonLin>1){
State *de1=Body::byId(id1,scene)->state.get(), *de2=Body::byId(id2,scene)->state.get();
Vector3r shiftVel=scene->isPeriodic ? Vector3r(scene->cell->velGrad*scene->cell->hSize*contact->cellDist.cast<Real>()) : Vector3r::Zero();
Vector3r shift2 = scene->isPeriodic ? Vector3r(scene->cell->hSize*contact->cellDist.cast<Real>()): Vector3r::Zero();
Vector3r incidentV = geom->getIncidentVel(de1, de2, scene->dt, shift2, shiftVel, /*preventGranularRatcheting*/ nonLin>2 );
Vector3r incidentVn = geom->normal.dot(incidentV)*geom->normal; // contact normal velocity
Vector3r incidentVs = incidentV-incidentVn; // contact shear velocity
shearIncrement=incidentVs*scene->dt;
} else { shearIncrement=geom->shearIncrement(); }
Fs-=ks*shearIncrement;
// Mohr-Coulomb slip
Real maxFs2=pow(Fn,2)*pow(phys->tangensOfFrictionAngle,2);
if(Fs.squaredNorm()>maxFs2) Fs*=sqrt(maxFs2)/Fs.norm();
// apply forces
Vector3r f=-phys->normalForce-phys->shearForce; /* should be a reference returned by geom->rotate */ assert(phys->shearForce==Fs);
scene->forces.addForce(id1,f);
scene->forces.addForce(id2,-f);
scene->forces.addTorque(id1,(geom->radius1-.5*geom->penetrationDepth)*geom->normal.cross(f));
scene->forces.addTorque(id2,(geom->radius2-.5*geom->penetrationDepth)*geom->normal.cross(f));
return true;
}
void Gl1_Facet::go(const shared_ptr<Shape>& cm, const shared_ptr<State>& ,bool wire,const GLViewInfo&)
{
Facet* facet = static_cast<Facet*>(cm.get());
const vector<Vector3r>& vertices = facet->vertices;
const Vector3r* ne = facet->ne;
const Real& icr = facet->icr;
if(cm->wire || wire){
// facet
glBegin(GL_LINE_LOOP);
glColor3v(normals ? Vector3r(1,0,0): cm->color);
glVertex3v(vertices[0]);
glVertex3v(vertices[1]);
glVertex3v(vertices[2]);
glEnd();
if(!normals) return;
// facet's normal
glBegin(GL_LINES);
glColor3(0.0,0.0,1.0);
glVertex3(0.0,0.0,0.0);
glVertex3v(facet->normal);
glEnd();
// normal of edges
glColor3(0.0,0.0,1.0);
glBegin(GL_LINES);
glVertex3(0.0,0.0,0.0); glVertex3v(Vector3r(icr*ne[0]));
glVertex3(0.0,0.0,0.0); glVertex3v(Vector3r(icr*ne[1]));
glVertex3(0.0,0.0,0.0); glVertex3v(Vector3r(icr*ne[2]));
glEnd();
} else {
glDisable(GL_CULL_FACE);
Vector3r normal=(facet->vertices[1]-facet->vertices[0]).cross(facet->vertices[2]-facet->vertices[1]); normal.normalize();
glColor3v(cm->color);
glBegin(GL_TRIANGLES);
glNormal3v(normal); // this makes every triangle different WRT the light direction; important!
glVertex3v(facet->vertices[0]);
glVertex3v(facet->vertices[1]);
glVertex3v(facet->vertices[2]);
glEnd();
}
}
TEST(IOBoardModelTest, PredictBarometerAMSL) {
IOBoardModel test = IOBoardModel(
Quaternionr(1, 0, 0, 0),
Vector3r(0, 0, 0),
Quaternionr(1, 0, 0, 0),
Quaternionr(1, 0, 0, 0),
Vector3r(7, 8, 9));
State test_state;
test_state << -37, 145, 150,
7, 8, 9,
1, 2, 3,
0.7071, 0, 0, 0.7071,
4, 5, 6,
0, 0, 0,
1, 2, 3,
0, 0, 0;
test.set_barometer_amsl(0);
EXPECT_TRUE(abs(test.predict(test_state)(0) - 150) < 0.1);
}
TEST(IOBoardModelTest, PredictGyroscope) {
IOBoardModel test = IOBoardModel(
Quaternionr(1, 0, 0, 0),
Vector3r(0, 0, 0),
Quaternionr(1, 0, 0, 0),
Quaternionr(1, 0, 0, 0),
Vector3r(0, 0, 0));
State test_state;
test_state << 0, 0, 0,
0, 0, 0,
1, 2, 3,
0.7071, 0, 0, 0.7071,
4, 5, 6,
0, 0, 0,
0, 0, 0,
0, 0, 0;
test.set_gyroscope(Vector3r(0, 0, 0));
EXPECT_EQ(test.predict(test_state), Vector3r(4, 5, 6));
}
TEST(IOBoardModelTest, PredictPitotTAS) {
IOBoardModel test = IOBoardModel(
Quaternionr(1, 0, 0, 0),
Vector3r(0, 0, 0),
Quaternionr(1, 0, 0, 0),
Quaternionr(1, 0, 0, 0),
Vector3r(7, 8, 9));
State test_state;
test_state << 0, 0, 0,
7, 8, 9,
1, 2, 3,
0.7071, 0, 0, 0.7071,
4, 5, 6,
0, 0, 0,
1, 2, 3,
0, 0, 0;
test.set_pitot_tas(0);
EXPECT_EQ(test.predict(test_state)(0), 6);
}
void PeriIsoCompressor::avgStressIsoStiffness(const Vector3r& cellAreas, Vector3r& stress, Real& isoStiff){
Vector3r force(Vector3r::Zero()); Real stiff=0; long n=0;
FOREACH(const shared_ptr<Contact>& C, *dem->contacts){
const auto fp=dynamic_pointer_cast<FrictPhys>(C->phys); // needed for stiffness
if(!fp) continue;
force+=(C->geom->node->ori*C->phys->force).array().abs().matrix();
stiff+=(1/3.)*fp->kn+(2/3.)*fp->kt; // count kn in one direction and ks in the other two
n++;
}
isoStiff= n>0 ? (1./n)*stiff : -1;
stress=-Vector3r(force[0]/cellAreas[0],force[1]/cellAreas[1],force[2]/cellAreas[2]);
}
SafetyEval::EvalResult SafetyEval::isSafeVelocityZ(const Vector3r& cur_pos, float vx, float vy, float z, const Quaternionr& quaternion)
{
SafetyEval::EvalResult result;
Vector3r dest_pos = getDestination(cur_pos, Vector3r(vx, vy, 0));
dest_pos.z() = z;
//check if dest_pos is safe
isSafeDestination(dest_pos, cur_pos, quaternion, result);
return result;
}
void Disp2DPropLoadEngine::letDisturb()
{
const Real& dt = scene->dt;
dgamma=cos(theta*Mathr::PI/180.0)*v*dt;
dh=sin(theta*Mathr::PI/180.0)*v*dt;
Real Ysup = topbox->state->pos.y();
Real Ylat = leftbox->state->pos.y();
// Changes in vertical and horizontal position :
topbox->state->pos += Vector3r(dgamma,dh,0);
leftbox->state->pos += Vector3r(dgamma/2.0,dh/2.0,0);
rightbox->state->pos += Vector3r(dgamma/2.0,dh/2.0,0);
Real Ysup_mod = topbox->state->pos.y();
Real Ylat_mod = leftbox->state->pos.y();
// with the corresponding velocities :
topbox->state->vel=Vector3r(dgamma/dt,dh/dt,0);
leftbox->state->vel = Vector3r((dgamma/dt)/2.0,dh/(2.0*dt),0);
rightbox->state->vel = Vector3r((dgamma/dt)/2.0,dh/(2.0*dt),0);
// Then computation of the angle of the rotation to be done :
computeAlpha();
if (alpha == Mathr::PI/2.0) // Case of the very beginning
{
dalpha = - atan( dgamma / (Ysup_mod -Ylat_mod) );
}
else
{
Real A = (Ysup_mod - Ylat_mod) * 2.0*tan(alpha) / (2.0*(Ysup - Ylat) + dgamma*tan(alpha) );
dalpha = atan( (A - tan(alpha))/(1.0 + A * tan(alpha)));
}
Quaternionr qcorr(AngleAxisr(dalpha,Vector3r::UnitZ()));
if(LOG)
cout << "Quaternion associe a la rotation incrementale : " << qcorr.w() << " " << qcorr.x() << " " << qcorr.y() << " " << qcorr.z() << endl;
// On applique la rotation en changeant l'orientation des plaques, leurs vang et en affectant donc alpha
leftbox->state->ori = qcorr*leftbox->state->ori;
leftbox->state->angVel = Vector3r(0,0,1)*dalpha/dt;
rightbox->state->ori = qcorr*leftbox->state->ori;
rightbox->state->angVel = Vector3r(0,0,1)*dalpha/dt;
}
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;
}
TEST(IOBoardModelTest, PredictMagnetometer) {
IOBoardModel test = IOBoardModel(
Quaternionr(1, 0, 0, 0),
Vector3r(0, 0, 0),
Quaternionr(1, 0, 0, 0),
Quaternionr(1, 0, 0, 0),
Vector3r(7, 8, 9));
State test_state;
test_state << 0, 0, 0,
0, 0, 0,
1, 2, 3,
0.7071, 0, 0, 0.7071,
4, 5, 6,
0, 0, 0,
0, 0, 0,
0, 0, 0;
test.set_magnetometer(Vector3r(0, 0, 0));
EXPECT_TRUE(test.predict(test_state).isApprox(
Vector3r(7, -9, 8), 0.001));
}
TEST(IOBoardModelTest, PredictGPSPosition) {
IOBoardModel test = IOBoardModel(
Quaternionr(1, 0, 0, 0),
Vector3r(0, 0, 0),
Quaternionr(1, 0, 0, 0),
Quaternionr(1, 0, 0, 0),
Vector3r(7, 8, 9));
State test_state;
test_state << -37, 145, 0,
0, 0, 0,
1, 2, 3,
0.7071, 0, 0, 0.7071,
4, 5, 6,
0, 0, 0,
0, 0, 0,
0, 0, 0;
test.set_gps_position(Vector3r(0, 0, 0));
EXPECT_TRUE(test.predict(test_state).isApprox(
Vector3r(-37, 145, 0), 0.001));
}
TEST(IOBoardModelTest, PredictGPSVelocity) {
IOBoardModel test = IOBoardModel(
Quaternionr(1, 0, 0, 0),
Vector3r(0, 0, 0),
Quaternionr(1, 0, 0, 0),
Quaternionr(1, 0, 0, 0),
Vector3r(7, 8, 9));
State test_state;
test_state << 0, 0, 0,
7, 8, 9,
1, 2, 3,
0.7071, 0, 0, 0.7071,
4, 5, 6,
0, 0, 0,
0, 0, 0,
0, 0, 0;
test.set_gps_velocity(Vector3r(0, 0, 0));
EXPECT_TRUE(test.predict(test_state).isApprox(
Vector3r(7, 8, 9), 0.001));
}
void TensorGlRep::postLoad(TensorGlRep&,void*){
// symmetrize the tensor
Matrix3r sym=.5*(val+val.transpose());
Eigen::SelfAdjointEigenSolver<Matrix3r> eig(sym);
// multiply rotation matrix (princ) rows by diagonal entries
eigVec=eig.eigenvectors();
eigVal=eig.eigenvalues();
// non-symmetric tensor in principal coordinates, extract the asymmetric part
Matrix3r valP=eigVec*val*eigVec.transpose();
Matrix3r skewM=.5*(valP-valP.transpose());
skew=2*Vector3r(skewM(1,2),skewM(0,2),skewM(0,1));
};
void Renderer::setClippingPlanes(){
// clipping
assert(clipPlaneNormals.size()==(size_t)numClipPlanes);
for(size_t i=0;i<(size_t)numClipPlanes; i++){
// someone could have modified those from python and truncate the vectors; fill those here in that case
if(i==clipPlanePos.size()) clipPlanePos.push_back(Vector3r::Zero());
if(i==clipPlaneOri.size()) clipPlaneOri.push_back(Quaternionr::Identity());
if(i==clipPlaneActive.size()) clipPlaneActive.push_back(false);
if(i==clipPlaneNormals.size()) clipPlaneNormals.push_back(Vector3r::UnitX());
// end filling stuff modified from python
if(clipPlaneActive[i]) clipPlaneNormals[i]=clipPlaneOri[i]*Vector3r(0,0,1);
/* glBegin(GL_LINES);glVertex3v(clipPlanePos[i]);glVertex3v(clipPlanePos[i]+clipPlaneNormals[i]);glEnd(); */
}
}
/* Generate SnubCube*/
vector<Vector3r> SnubCubePoints(Vector3r radii){
vector<Vector3r> v;
double c1 = 0.337754;
double c2 = 1.14261;
double c3 = 0.621226;
Vector3r f,b;
f = radii/1.3437133737446;
vector<Vector3r> A;
A.push_back(Vector3r(c2,c1,c3)); A.push_back(Vector3r(c1,c3,c2)); A.push_back(Vector3r(c3,c2,c1));
A.push_back(Vector3r(-c1,-c2,-c3)); A.push_back(Vector3r(-c2,-c3,-c1)); A.push_back(Vector3r(-c3,-c1,-c2));
for(int i=0;i<(int) A.size();i++){
b = Vector3r(A[i][0]*f[0],A[i][1]*f[1],A[i][2]*f[2]);
v.push_back(b);
v.push_back(Vector3r(-b[0],-b[1], b[2]));
v.push_back(Vector3r(-b[0], b[1],-b[2]));
v.push_back(Vector3r( b[0],-b[1],-b[2]));
}
return v;
}
void SimpleShear::createBox(shared_ptr<Body>& body, Vector3r position, Vector3r extents)
{
body = shared_ptr<Body>(new Body); body->groupMask=1;
shared_ptr<NormalInelasticMat> mat(new NormalInelasticMat);
shared_ptr<Aabb> aabb(new Aabb);
// shared_ptr<BoxModel> gBox(new BoxModel);
shared_ptr<Box> iBox(new Box);
iBox->wire = true;
body->setDynamic(false);
body->state->angVel = Vector3r(0,0,0);
body->state->vel = Vector3r(0,0,0);
// NB : mass and inertia not defined because not used, since Box are not dynamics
body->state->pos = position;
body->state->ori = Quaternionr::Identity();
mat->young = boxYoungModulus;
mat->poisson = boxPoissonRatio;
mat->frictionAngle = 0.0; //default value, modified after for w2 and w4 to have good values of phi(sphere-walls)
body->material = mat;
aabb->color = Vector3r(1,0,0);
/* gBox->extents = extents;
gBox->color = Vector3r(1,0,0);
gBox->wire = true;
gBox->shadowCaster = false;*/
iBox->extents = extents;
iBox->color = Vector3r(1,0,0);
body->bound = aabb;
body->shape = iBox;
// body->geometricalModel = gBox;
}
void GLUtils::Parallelepiped(const Vector3r& a, const Vector3r& b, const Vector3r& c){
glBegin(GL_LINE_STRIP);
glVertex3v(b); glVertex3v(Vector3r(Vector3r::Zero())); glVertex3v(a); glVertex3v(Vector3r(a+b)); glVertex3v(Vector3r(a+b+c)); glVertex3v(Vector3r(b+c)); glVertex3v(b); glVertex3v(Vector3r(a+b));
glEnd();
glBegin(GL_LINE_STRIP);
glVertex3v(Vector3r(b+c)); glVertex3v(c); glVertex3v(Vector3r(a+c)); glVertex3v(a);
glEnd();
glBegin(GL_LINES);
glVertex3v(Vector3r(Vector3r::Zero())); glVertex3v(c);
glEnd();
glBegin(GL_LINES);
glVertex3v(Vector3r(a+c)); glVertex3v(Vector3r(a+b+c));
glEnd();
}
// draw periodic cell, if active
void Renderer::drawPeriodicCell(){
if(!scene->isPeriodic || !cell) return;
glColor3v(Vector3r(1,1,0));
glPushMatrix();
const Matrix3r& hSize=scene->cell->hSize;
if(scaleOn && dispScale!=Vector3r::Ones()){
const Matrix3r& refHSize(scene->cell->refHSize);
Matrix3r scaledHSize;
for(int i=0; i<3; i++) scaledHSize.col(i)=refHSize.col(i)+((dispScale-Vector3r::Ones()).array()*Vector3r(hSize.col(i)-refHSize.col(i)).array()).matrix();
GLUtils::Parallelepiped(scaledHSize.col(0),scaledHSize.col(1),scaledHSize.col(2));
} else
{
GLUtils::Parallelepiped(hSize.col(0),hSize.col(1),hSize.col(2));
}
glPopMatrix();
}
// draw periodic cell, if active
void OpenGLRenderer::drawPeriodicCell(){
if(!scene->isPeriodic) return;
glColor3v(cellColor);
glPushMatrix();
// Vector3r size=scene->cell->getSize();
const Matrix3r& hSize=scene->cell->hSize;
if(dispScale!=Vector3r::Ones()){
const Matrix3r& refHSize(scene->cell->refHSize);
Matrix3r scaledHSize;
for(int i=0; i<3; i++) scaledHSize.col(i)=refHSize.col(i)+dispScale.cwiseProduct(Vector3r(hSize.col(i)-refHSize.col(i)));
GLUtils::Parallelepiped(scaledHSize.col(0),scaledHSize.col(1),scaledHSize.col(2));
} else {
GLUtils::Parallelepiped(hSize.col(0),hSize.col(1),hSize.col(2));
}
glPopMatrix();
}
//**********************************************************************************
//returns approximate size of polyhedron
Vector3r SizeOfPolyhedra(const shared_ptr<Shape>& cm1){
Polyhedra* A = static_cast<Polyhedra*>(cm1.get());
double minx = 0, maxx = 0, miny = 0, maxy = 0, minz = 0, maxz = 0;
for (vector<Vector3r>::iterator i=A->v.begin(); i!=A->v.end(); ++i){
minx = min(minx,(*i)[0]);
maxx = max(maxx,(*i)[0]);
miny = min(miny,(*i)[1]);
maxy = max(maxy,(*i)[1]);
minz = min(minz,(*i)[2]);
maxz = max(maxz,(*i)[2]);
}
return Vector3r(maxx-minx, maxy-miny, maxz-minz);
}
size_t DemFuncs::radialAxialForce(const Scene* scene, const DemField* dem, int mask, Vector3r axis, bool shear, Vector2r& radAxF){
size_t ret=0;
radAxF=Vector2r::Zero();
axis.normalize();
for(const shared_ptr<Particle>& p: *dem->particles){
if(!(p->mask & mask) || !p->shape) continue;
ret++;
for(const auto& idC: p->contacts){
const shared_ptr<Contact>& C(idC.second);
if(!C->isReal()) continue;
Vector3r F=C->geom->node->ori*((shear?C->phys->force:Vector3r(C->phys->force[0],0,0))*C->forceSign(p));
Vector3r axF=F.dot(axis);
radAxF+=Vector2r(axF,(F-axF).norm());
}
}
return ret;
}
// called for rendering spheres both and ellipsoids, differing in the scale parameter
void Gl1_Sphere::renderScaledSphere(const shared_ptr<Shape>& shape, const Vector3r& shift, bool wire2, const GLViewInfo& glInfo, const Real& radius, const Vector3r& scaleAxes){
const shared_ptr<Node>& n=shape->nodes[0];
Vector3r dPos=(n->hasData<GlData>()?n->getData<GlData>().dGlPos:Vector3r::Zero());
GLUtils::setLocalCoords(shape->nodes[0]->pos+shift+dPos,shape->nodes[0]->ori);
// for rendering ellipsoid
if(!isnan(scaleAxes[0])) glScalev(scaleAxes);
glClearDepth(1.0f);
glEnable(GL_NORMALIZE);
if(quality>10) quality=10; // insane setting can quickly kill the GPU
Real r=radius*scale;
//glColor3v(CompUtils::mapColor(shape->getBaseColor()));
bool doPoints=(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);
glPointSize(1.);
glBegin(GL_POINTS);
glVertex3v(Vector3r(0,0,0));
glEnd();
} else if (wire || wire2 ){
glLineWidth(1.);
if(!smooth) glDisable(GL_LINE_SMOOTH);
glutWireSphere(r,quality*glutSlices,quality*glutStacks);
if(!smooth) glEnable(GL_LINE_SMOOTH); // re-enable
}
else {
glEnable(GL_LIGHTING);
glShadeModel(GL_SMOOTH);
glutSolidSphere(r,quality*glutSlices,quality*glutStacks);
#if 0
//Check if quality has been modified or if previous lists are invalidated (e.g. by creating a new qt view), then regenerate lists
bool somethingChanged = (abs(quality-prevQuality)>0.001 || glIsList(glStripedSphereList)!=GL_TRUE);
if (somethingChanged) {initStripedGlList(); initGlutGlList(); prevQuality=quality;}
glScalef(r,r,r);
if(stripes) glCallList(glStripedSphereList);
else glCallList(glGlutSphereList);
#endif
}
return;
}
void Gl1_PotentialParticle::calcMinMax(const PotentialParticle& pp) {
int planeNo = pp.d.size();
Real maxD = pp.d[0];
for (int i=0; i<planeNo; ++i) {
if (pp.d[i] > maxD) {
maxD = pp.d[i];
}
}
min = -aabbEnlargeFactor*pp.minAabb;
max = aabbEnlargeFactor*pp.maxAabb;
Real dx = (max[0]-min[0])/((Real)(sizeX-1));
Real dy = (max[1]-min[1])/((Real)(sizeY-1));
Real dz = (max[2]-min[2])/((Real)(sizeZ-1));
isoStep=Vector3r(dx,dy,dz);
}
void Membrane::computeNodalDisplacements(Real dt, bool rotIncr){
assert(hasRefConf());
// supposes node is updated already
for(int i:{0,1,2}){
Vector3r xy=node->glob2loc(nodes[i]->pos);
// relative tolerance of 1e-6 was too little, in some cases?!
#ifdef MEMBRANE_DEBUG_ROT
if(xy[2]>1e-5*(max(abs(xy[0]),abs(xy[1])))){
LOG_ERROR("local z-coordinate is not zero for node "<<i<<": node->ori="<<AngleAxisr(node->ori).axis()<<" !"<<AngleAxisr(node->ori).angle()<<", xy="<<xy<<", position in global-oriented centroid-origin CS "<<(nodes[i]->pos-node->pos).transpose());
}
#else
assert(xy[2]<1e-5*(max(abs(xy[0]),abs(xy[1]))));
#endif
// displacements
uXy.segment<2>(2*i)=xy.head<2>()-refPos.segment<2>(2*i);
// rotations
if(rotIncr){
// incremental
Vector3r angVelL=node->glob2loc(nodes[i]->getData<DemData>().angVel); // angular velocity in element coords
phiXy.segment<2>(2*i)-=dt*angVelL.head<2>();
// throw std::runtime_error("Incremental rotation (In2_ElastMat_Membrane.rotIncr) is not yet implemented properly.");
} else {
// from total rotation difference
AngleAxisr aa(refRot[i].conjugate()*(nodes[i]->ori.conjugate()*node->ori));
/* aa sometimes gives angle close to 2π for rotations which are actually small and negative;
I posted that question at http://forum.kde.org/viewtopic.php?f=74&t=110854 .
Such a result is fixed by conditionally subtracting 2π:
*/
if(aa.angle()>M_PI) aa.angle()-=2*M_PI;
Vector3r rot=Vector3r(aa.angle()*aa.axis()); // rotation vector in local coords
// if(aa.angle()>3)
if(rot.head<2>().squaredNorm()>3.1*3.1) LOG_WARN("Membrane's in-plane rotation in a node is > 3.1 radians, expect unstability!");
phiXy.segment<2>(2*i)=rot.head<2>(); // drilling rotation discarded
#ifdef MEMBRANE_DEBUG_ROT
AngleAxisr rr(refRot[i]);
AngleAxisr cr(nodes[i]->ori.conjugate()*node->ori);
LOG_TRACE("node "<<i<<"\n refRot : "<<rr.axis()<<" !"<<rr.angle()<<"\n currRot: "<<cr.axis()<<" !"<<cr.angle()<<"\n diffRot: "<<aa.axis()<<" !"<<aa.angle());
drill[i]=rot[2];
currRot[i]=nodes[i]->ori.conjugate()*node->ori;
#endif
}
}
};