Création d'un compte pour un collaborateur extérieur au laboratoire depuis l'intranet ICube : https://intranet.icube.unistra.fr/fr/labs/member/profile

Commit de5af8c7 authored by Thomas Jund's avatar Thomas Jund
Browse files

avant passage a GL2

parent 90052d0d
......@@ -7,7 +7,7 @@
#include "env_map.h"
#include "spatialHashing.h"
#define SECURED
//#define SECURED
#ifdef SECURED
#include "Algo/MovingObjects/particle_cell_2D_secured.h"
......@@ -84,6 +84,8 @@ public:
static unsigned int cptAgent ;
VEC3 forces;
float color1;
float color2;
float color3;
......
......@@ -18,10 +18,8 @@ template <typename PFP> Algo::Modelisation::Polyhedron<PFP> generateGrid(EnvMap&
template <typename PFP> void generateCity(EnvMap& envMap, unsigned int nbBuildings) ;
template <typename PFP>
Algo::Modelisation::Polyhedron<PFP> generateTrianGrid(typename PFP::MAP& map,
typename PFP::TVEC3& position,
unsigned int cX, unsigned int cY,
float sideLength, CellMarker<EDGE>& obstacleMark,
Algo::Modelisation::Polyhedron<PFP> generateTrianGrid(EnvMap& envMap,
CellMarker<EDGE>& obstacleMark,
CellMarker<FACE>& buildingMark) ;
template <typename PFP>
......
......@@ -83,57 +83,59 @@ void generateCity(EnvMap& envMap, unsigned int nbBuildings)
}
template <typename PFP>
Algo::Modelisation::Polyhedron<PFP> generateTrianGrid(typename PFP::MAP& map,
typename PFP::TVEC3& position,
unsigned int cX, unsigned int cY,
float sideLength, CellMarker<EDGE>& obstacleMark,
Algo::Modelisation::Polyhedron<PFP> generateTrianGrid(EnvMap& envMap,
CellMarker<EDGE>& obstacleMark,
CellMarker<FACE>& buildingMark)
{
Algo::Modelisation::Polyhedron<PFP> prim(map, position) ;
prim.grid_topo(cX, cY) ;
unsigned int nx = envMap.geometry.size(0) / envMap.maxCellSize ;
unsigned int ny = envMap.geometry.size(1) / envMap.maxCellSize ;
if (nx < 1) nx = 1 ;
if (ny < 1) ny = 1 ;
prim.embedGrid(sideLength * cX, sqrt(sideLength * sideLength * 3.0f / 4.0f) * cY) ;
Algo::Modelisation::Polyhedron<PFP> prim(envMap.map, envMap.position) ;
prim.grid_topo(nx, ny) ;
prim.embedGrid(envMap.geometry.size(0), envMap.geometry.size(1), 0.0f) ;
Dart dY = prim.getDart() ; //remind the first quad of the line
Dart dX = prim.getDart() ; //goes through the line
bool odd = true ; //odd line or not
for (unsigned int i = 0; i < cX * cY;)
for (unsigned int i = 0; i < nx * ny;)
{
Dart dNext = map.phi1(map.phi2(map.phi1(dX))) ;
Dart dNext = envMap.map.phi1(envMap.map.phi2(envMap.map.phi1(dX))) ;
Dart toCut = dX ;
if (odd)
{
toCut = map.phi1(toCut) ; //change the side of the split face
position[toCut][0] -= sideLength / 2.0f ; //move vertices for equilateral triangles
toCut = envMap.map.phi1(toCut) ; //change the side of the split face
envMap.position[toCut][0] -= envMap.maxCellSize / 2.0f ; //move vertices for equilateral triangles
}
map.splitFace(toCut, map.phi1(map.phi1(toCut))) ;
envMap.map.splitFace(toCut, envMap.map.phi1(envMap.map.phi1(toCut))) ;
++i ;
if (i % cX == 0 && i > 0) //goes up and change side of split
if (i % nx == 0 && i > 0) //goes up and change side of split
{
Dart endSquare = map.newOrientedFace(3) ; //add triangle add end of lines to make a square
Dart endSquare = envMap.map.newFace(3) ; //add triangle add end of lines to make a square
Dart dN ;
if (odd)
{
dN = map.phi1(map.phi2(map.phi1(dX))) ;
map.sewFaces(dN, endSquare) ;
position[map.phi_1(endSquare)] = position[map.phi1(endSquare)] ;
position[map.phi_1(endSquare)][0] += sideLength / 2.0f ;
dN = envMap.map.phi1(envMap.map.phi2(envMap.map.phi1(dX))) ;
envMap.map.sewFaces(dN, endSquare) ;
envMap.position[envMap.map.phi_1(endSquare)] = envMap.position[envMap.map.phi1(endSquare)] ;
envMap.position[envMap.map.phi_1(endSquare)][0] += envMap.maxCellSize / 2.0f ;
}
else
{
dN = map.phi1(dX) ;
map.sewFaces(dN, endSquare) ;
position[map.phi_1(endSquare)] = position[endSquare] ;
dN = envMap.map.phi1(dX) ;
envMap.map.sewFaces(dN, endSquare) ;
envMap.position[envMap.map.phi_1(endSquare)] = envMap.position[endSquare] ;
}
if (odd)
dY = map.phi2(map.phi_1(map.phi2(map.phi1(dY)))) ;
dY = envMap.map.phi2(envMap.map.phi_1(envMap.map.phi2(envMap.map.phi1(dY)))) ;
else
dY = map.phi2(map.phi1(map.phi2(map.phi_1(dY)))) ;
dY = envMap.map.phi2(envMap.map.phi1(envMap.map.phi2(envMap.map.phi_1(dY)))) ;
dX = dY ;
odd = !odd ;
......@@ -142,44 +144,44 @@ Algo::Modelisation::Polyhedron<PFP> generateTrianGrid(typename PFP::MAP& map,
dX = dNext ;
}
Dart boundary ;
for (Dart d = map.begin(); d != map.end(); map.next(d))
{
if (map.phi2(d) == d)
{
Dart dA = map.alpha1(map.phi1(d)) ;
if (map.phi2(dA) == dA && position[dA] == position[map.phi1(d)]
&& position[map.phi1(dA)] == position[d])
map.sewFaces(dA, d) ;
else
{
obstacleMark.mark(d) ;
boundary = d ;
}
}
}
map.closeHole(boundary) ;
buildingMark.mark(map.phi2(boundary)) ;
// Dart boundary ;
// for (Dart d = envMap.map.begin(); d != envMap.map.end(); envMap.map.next(d))
// {
// if (envMap.map.phi2(d) == d)
// {
// Dart dA = envMap.map.alpha1(envMap.map.phi1(d)) ;
// if (envMap.map.phi2(dA) == dA && envMap.position[dA] == envMap.position[envMap.map.phi1(d)]
// && envMap.position[envMap.map.phi1(dA)] == envMap.position[d])
// envMap.map.sewFaces(dA, d) ;
// else
// {
// obstacleMark.mark(d) ;
// boundary = d ;
// }
// }
// }
//
// envMap.map.closeHole(boundary) ;
// buildingMark.mark(envMap.map.phi2(boundary)) ;
//
// if (odd) //last top line if odd
// {
// for (unsigned int i = 0; i < nx; ++i)
// {
// dX = envMap.map.phi1(dX) ;
// envMap.position[dX][0] -= envMap.maxCellSize / 2.0f ;
// }
// }
if (odd) //last top line if odd
//add hexagons
for(Dart d = envMap.map.begin(); d != envMap.map.end(); envMap.map.next(d))
{
for (unsigned int i = 0; i < cX; ++i)
if(envMap.map.vertexDegree(d)==6)
{
dX = map.phi1(dX) ;
position[dX][0] -= sideLength / 2.0f ;
envMap.map.deleteVertex(d);
}
}
//add hexagons
// for(Dart d = map.begin(); d != map.end(); map.next(d))
// {
// if(map.vertexDegree(d)==6)
// {
// map.deleteVertex(d);
// }
// }
return prim ;
}
......
......@@ -48,7 +48,9 @@ inline void renderPredictionTriangle(EnvMap& m, Dart d, PFP::VEC3 p)
inline void renderObstacle(EnvMap& m, MovingObstacle * obst, bool showBelonging=false, bool renderPath = false, bool showVertices=false)
{
// glPolygonMode(GL_FRONT_AND_BACK, GL_LINE) ;
glColor3f(1.0,1.0,1.0);
glBegin(GL_POLYGON) ;
for (unsigned int i = 0 ; i < (obst->nbVertices) ; ++i)
{
......@@ -57,7 +59,7 @@ inline void renderObstacle(EnvMap& m, MovingObstacle * obst, bool showBelonging=
}
glEnd() ;
// glPolygonMode(GL_FRONT_AND_BACK, GL_LINE) ;
// glColor3f(0,1,0);
// Algo::Render::GL1::renderTriQuadPoly<PFP>(obst->map, Algo::Render::GL1::LINE,
// 1.0, obst->position,
......@@ -146,21 +148,47 @@ inline void renderAgent(EnvMap& m, Agent* agent, bool showNeighborDist = false,
// pos[2] -= 1000;
// Geom::intersectionPlaneRay(pl,pos,VEC3(0,0,-1),pos);
glLineWidth(1.0f) ;
VEC3 col = Utils::color_map_BCGYR(float(agent->agentNo)/float(agent->sim_->agents_.size()));
glLineWidth(1.0f) ;
// glBegin(GL_LINES);
// glVertex3fv(pos.data());
// VEC3 f = pos + agent->forces/100.0;
// glVertex3fv(f.data());
// glEnd();
//
//
// glLineWidth(5.0f);
// VEC3 col;
// if(agent->movingObstacleNeighbors_.size()>0)
// {
// glBegin(GL_LINES);
// col = VEC3(1,0,0);
// for(unsigned int i = 0 ; i < agent->movingObstacleNeighbors_.size() ; ++i)
// {
// glVertex3fv(pos.data());
// VEC3 oP = (agent->movingObstacleNeighbors_[i].second->p1+agent->movingObstacleNeighbors_[i].second->p2)*0.5f;
// glVertex3fv(oP.data());
// }
// glEnd();
// }
// else if(agent->obstacleNeighbors_.size()>0)
// col = VEC3(1,1,0);
// else
// col = VEC3(0,1,0);
glLineWidth(1.0f) ;
glColor3fv(col.data());
// glColor3f(c1,c2,c3) ;
glBegin(GL_POLYGON) ;
for(unsigned int i = 0 ; i < 5 ; ++i)
glVertex3f(pos[0] + (cosT[i] * radius), pos[1] + (sinT[i] * radius), pos[2]+0.01f) ;
glEnd() ;
// VEC3 posi = agent->finalGoal;
// glColor3f(0.0f, 0.0f, 1.0f);
// glBegin(GL_POLYGON);
// for(unsigned int i = 0; i < 5; ++i)
// glVertex3f(posi[0] + (cosT[i] * 1.5f), posi[1] + (sinT[i] * 1.5f), posi[2]-0.01f);
// glEnd();
VEC3 dir = agent->prefVelocity_ ;
dir.normalize() ;
......@@ -203,6 +231,12 @@ inline void renderAgent(EnvMap& m, Agent* agent, bool showNeighborDist = false,
glVertex3fv(g.data()) ;
glEnd() ;
glPointSize(1.0f) ;
glBegin(GL_LINES);
VEC3 p = agent->getPosition();
glVertex3fv(p.data()) ;
glVertex3fv(g.data()) ;
glEnd();
}
if (showNeighborDist)
......
......@@ -38,6 +38,8 @@ public:
void attachMesh(MovingMesh* mm);
void updateMesh();
void attachAgent(Agent* ag);
unsigned int nbVertices;
CGoGN::Algo::MovingObjects::ParticleCell2DMemo<PFP> *registering_part;
......@@ -49,7 +51,9 @@ public:
VertexAttribute<VEC3> normal;
VertexAttribute<VEC3> deformation;
VertexAttribute<VEC3> velocity;
VertexAttribute<VEC3> forces;
EdgeAttribute<float> edgeLength;
DartAttribute<float> vertexAngle;
Dart groundFace;
......@@ -108,6 +112,7 @@ public:
int index_parent;
MovingMesh* mm_;
Agent* ag_;
VertexAttribute<NoMathIONameAttribute<std::vector<PFP::REAL> > > mvc_;
};
......
......@@ -7,7 +7,7 @@ class Obstacle
{
public:
Obstacle(const VEC3 point1, const VEC3 point2, const VEC3 prevPoint, const VEC3 nextPoint,
MovingObstacle * moving1, unsigned int ind) :
MovingObstacle * moving1=NULL, unsigned int ind=0) :
p1(point1), p2(point2), prevP(prevPoint), nextP(nextPoint),
mo(moving1), index(ind)
{
......
......@@ -94,7 +94,7 @@ public:
void setupCorridorScenario(unsigned int nbAgents, unsigned int nbObstacles) ;
void setupSnakeCorridorScenario(unsigned int nbAgents, unsigned int nbSnakes, int snakeSize) ;
void setupCityScenario(int nbLines, int nbRank) ;
void setupScenario(unsigned int nbMaxAgent) ;
void setupScenario(unsigned int nbMaxAgent, bool pedWay=false) ;
void addMovingObstacles(unsigned int nb);
void addMovingObstacle(Dart d, unsigned int obstType=0);
......
......@@ -79,6 +79,8 @@ public:
unsigned int nbIterations ;
unsigned int maxIterations ;
unsigned int nbGeneratedPov;
// to count fps
unsigned int frames ;
struct timespec startTime ;
......
......@@ -8,9 +8,10 @@
#include "agent.h"
#include "simulator.h"
#include "Geometry/frame.h"
unsigned int Agent::maxNeighbors_ = 10 ;
unsigned int Agent::maxMovingObstacles_ = 10;
unsigned int Agent::maxMovingObstacles_ = 20;
float Agent::averageMaxSpeed_ = 2.0f ;
// float Agent::averageMaxSpeed_ = 20.0f ;
// float Agent::neighborDist_ = 10.0f ;
......@@ -206,8 +207,8 @@ void Agent::updateObstacleNeighbors()
if ((*it)->mo==NULL)
{
float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
if ((obstacleNeighbors_.size() < maxNeighbors_ || distSq < maxDistObst)
&& distSq < rangeSq_)
if ((obstacleNeighbors_.size() < maxMovingObstacles_|| distSq < maxDistObst)
&& distSq < rangeSq_)
{
if (Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
{
......@@ -222,19 +223,22 @@ void Agent::updateObstacleNeighbors()
}
else
{
float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
if ((movingObstacleNeighbors_.size() < maxNeighbors_ || distSq < maxDistMovingObst)
&& distSq < rangeSq_)
// if((*it)->mo->index!=agentNo)
{
// if (Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
float distSq = distSqPointLineSegment((*it)->p1, (*it)->p2, part_.getPosition()) ;
if ((movingObstacleNeighbors_.size() < maxMovingObstacles_ || distSq < maxDistMovingObst)
&& distSq < rangeSq_)
{
// if (Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::LEFT)
{
if (distSq > maxDistMovingObst)
maxDistMovingObst = distSq ;
movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
if (distSq > maxDistMovingObst)
maxDistMovingObst = distSq ;
movingObstacleNeighbors_.push_back(std::make_pair(distSq, *it)) ;
}
}
}
}
}
}
......@@ -263,7 +267,7 @@ void Agent::updateObstacleNeighbors()
if ((movingObstacleNeighbors_.size() < maxNeighbors_ || distSq < maxDistMovingObst)
&&distSq < rangeSq_)
{
// if (Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::RIGHT)
// if (Geom::testOrientation2D(part_.getPosition(), (*it)->p1, (*it)->p2) == Geom::LEFT)
{
if (distSq > maxDistMovingObst) maxDistMovingObst = distSq ;
......@@ -329,7 +333,7 @@ void Agent::update()
#ifdef SECURED
if(target != part_.getPosition())
std::cout << "Problem agent " << agentNo << " (position : " << part_.getPosition() << ") time " << sim_->globalTime_ << std::endl;
std::cout << "Problem agent " << agentNo << " (position : " << part_.getPosition() << ") time " << sim_->nbSteps_ << std::endl;
#endif
#endif
......@@ -485,7 +489,7 @@ void Agent::computePrefVelocity()
// Si l'agent arrive à proximité de l'objectif,
// alors il passe à l'objectif suivant
if (goalDist2 < radius_*radius_)
if (goalDist2 < radius_*radius_*100.0f)
{
curGoal_ = (curGoal_ + 1) % goals_.size() ;
goalVector = goals_[curGoal_] - getPosition() ;
......@@ -556,7 +560,10 @@ void Agent::computeNewVelocity()
srand48(agentNo);
double rand = 2.0*drand48()-1.0; // compris entre -1 et 1
double ag_mass = average_mass*(1 + rand*mass_var); // valeurs uniformement réparties entre min et max
// double ag_mass = average_mass*(1 + rand*mass_var); // valeurs uniformement réparties entre min et max
double ag_mass = 50.0;
/*
rand = 2.0*drand48()-1.0; // compris entre -1 et 1
radius_ = average_radius + rand*radius_var; // valeurs uniformement réparties entre min et max
......@@ -565,7 +572,9 @@ void Agent::computeNewVelocity()
*/
//-------------
VEC3 vec, forces, previous_pos;
// VEC3 forces;
forces = VEC3(0);
VEC3 vec, previous_pos;
forces.zero();
previous_pos = getPosition() - velocity_*sim_->timeStep_;
......@@ -589,9 +598,11 @@ void Agent::computeNewVelocity()
// double obst_stiffness = 10000.0; // agent-obstacle interaction stiffness
double obst_stiffness = 1000.0; // agent-obstacle interaction stiffness
// double obst_damping = 1.0; // agent-obstacle interaction damping
int obst_power = 2; // the power to which elevate the agent-obstacle distance
int obst_power = 2 ; // the power to which elevate the agent-obstacle distance
Obstacle* obst ;
#define ARASH
#ifdef ARASH
nb_mos = 0;
for(std::vector<std::pair<float, Obstacle*> >::iterator it = movingObstacleNeighbors_.begin() ;
it != movingObstacleNeighbors_.end() ;
......@@ -629,6 +640,39 @@ void Agent::computeNewVelocity()
forces += force_value * norm;
}
}
#else
float force_value;
for(std::vector<std::pair<float, Obstacle*> >::iterator it = movingObstacleNeighbors_.begin() ;
it != movingObstacleNeighbors_.end() ; ++it)
{
Obstacle * obst = it->second;
float dist = it->first;
//project agent on obstacle line
VEC3 vO(obst->p1-obst->p2);
VEC3 vAO(getPosition()-obst->p2);
vO.normalize();
//proportion on obstacle line
float prop = (vAO*vO);
prop = std::min(prop,1.0f);
prop = std::max(prop,0.0f);
prop *= M_PI;
norm = VEC3(cos(prop),sin(prop),0);
float a = Geom::angle(VEC3(1,0,0), vO);
VEC3 norm2 = Geom::rotate(VEC3(0,0,1), a, norm);
force_value = obst_stiffness*pow(1/dist,obst_power);
std::cout << "ag " << agentNo << "angle " << a << " force val " << force_value << " " << norm2 << " dist " << dist << std::endl;
forces += force_value * norm2;
}
#endif
//----- forces dues à la répulsion des obstacles fixes ----------
......
......@@ -77,7 +77,8 @@ void EnvMap::init(unsigned int config, REAL width, REAL height, REAL minSize, RE
CityGenerator::generateCity<PFP>(*this,0) ;
break ;
case 1 :
CityGenerator::generateGrid<PFP>(*this) ;
// CityGenerator::generateGrid<PFP>(*this) ;
CityGenerator::generateTrianGrid<PFP>(*this,obstacleMark, buildingMark);
break ;
case 2 :
CityGenerator::generateGrid<PFP>(*this) ;
......
......@@ -19,7 +19,7 @@ constrainedV(map)
scaleValue = std::max(area/1400.0f,10.0f);
std::cout << "scaleVal " << scaleValue << std::endl;
// scaleValue = 4.0f;
scale(scaleValue);
scale(scaleValue/1.8f);
// scale(2.5f);
// scale(0.5f);
......@@ -162,7 +162,7 @@ std::vector<VEC3> MovingMesh::computeProjectedPointSet(float maxHeight)
std::vector<bool> active;
active.assign(points.size(), true);
simplifyCurve(points, active, 0, points.size()-1, 0.2f);
simplifyCurve(points, active, 0, points.size()-1, 0.3f);
std::vector<VEC3> res;
for(unsigned int i = 0; i < points.size() ; ++i)
......@@ -215,7 +215,8 @@ std::vector<VEC3> MovingMesh::jarvisConvexHull(const std::vector<VEC3>& pointSet
std::cout << "pointSet " << pointSet.size() << std::endl;
assert(pointSet.size()>2);
std::vector<unsigned int> convHull;
// std::vector<unsigned int> convHull;
std::vector<VEC3> res;
unsigned int minPoint=0;
unsigned int curr;
......@@ -223,7 +224,7 @@ std::vector<VEC3> MovingMesh::jarvisConvexHull(const std::vector<VEC3>& pointSet
if(pointSet[i][1]>pointSet[minPoint][1])
minPoint=i;
convHull.push_back(minPoint);
// convHull.push_back(minPoint);
curr = minPoint;
do {
......@@ -251,7 +252,8 @@ std::vector<VEC3> MovingMesh::jarvisConvexHull(const std::vector<VEC3>& pointSet
if(trouve)
{
curr = i-1;
convHull.push_back(curr);
// convHull.push_back(curr);
res.push_back(pointSet[curr]);
}
else
{
......@@ -261,11 +263,10 @@ std::vector<VEC3> MovingMesh::jarvisConvexHull(const std::vector<VEC3>& pointSet
} while(curr != minPoint);
std::vector<VEC3> res;
res.reserve(convHull.size());
for(std::vector<unsigned int>::iterator it = convHull.begin() ; it != convHull.end() ; ++it)
res.push_back(pointSet[*it]);
// res.reserve(convHull.size());
//
// for(std::vector<unsigned int>::iterator it = convHull.begin() ; it != convHull.end() ; ++it)
// res.push_back(pointSet[*it]);
return res;
}
......
......@@ -90,6 +90,7 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
rigid_(rigid),
spinning(spin),
parent(art),
ag_(NULL),
index_parent(indParent)
{
assert(pos.size() > 2);
......@@ -110,7 +111,9 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
if(!rigid_)
{
velocity = map.addAttribute<VEC3, VERTEX>("velocity") ;
forces = map.addAttribute<VEC3, VERTEX>("force") ;
edgeLength = map.addAttribute<float, EDGE>("edgeLength") ;
vertexAngle = map.addAttribute<float, DART>("vertexAngle") ;
}
groundFace = map.newFace(nbVertices);
......@@ -122,7 +125,10 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
deformation[d] = VEC3(0);
if(!rigid_)
{
velocity[d] = VEC3(0);
forces[d] = VEC3(0);
}
center += pos[i];
......@@ -146,6 +152,16 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
edgeLength[d] = VEC3(position[map.phi1(d)]-position[d]).norm();
}
DartMarker treated(map);
for(Dart d = map.begin() ; d != map.end() ; map.next(d))
{
if(!map.isBoundaryMarked(d) && !treated.isMarked(d))
{
treated.mark(d);
vertexAngle[d] = Algo::Geometry::angle<PFP>(map,map.phi_1(d),map.phi1(d),position);
}
}
map.enableQuickTraversal<VERTEX>();
dDir=dInside;
......@@ -202,8 +218,8 @@ MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, s
VEC3 MovingObstacle::getDilatedPosition(unsigned int ind)
{
Dart d(ind); //WARNING : works only for one face created at start !
// return position[d]+deformation[d];
return position[d];
return position[d]+deformation[d];
// return position[d];
}