Commit 406d546c authored by pitiot's avatar pitiot

up

parent 60f3bfbe
...@@ -33,7 +33,7 @@ class ArticulatedObstacle; ...@@ -33,7 +33,7 @@ class ArticulatedObstacle;
#include "pfp.h" #include "pfp.h"
//#define EXPORTING3 //#define EXPORTING3
#define TWO_AND_HALF_DIM //#define TWO_AND_HALF_DIM
#ifdef EXPORTING3 #ifdef EXPORTING3
#include "Utils/Shaders/shaderPhongTexture.h" #include "Utils/Shaders/shaderPhongTexture.h"
...@@ -67,6 +67,7 @@ public: ...@@ -67,6 +67,7 @@ public:
void scale(float val); void scale(float val);
bool is_insideConvexCell2D(VEC3 p, Dart d); bool is_insideConvexCell2D(VEC3 p, Dart d);
bool isInsideFace3D(VEC3 p, Dart d);
void markPedWay() ; void markPedWay() ;
unsigned int mapMemoryCost() ; unsigned int mapMemoryCost() ;
......
...@@ -28,7 +28,7 @@ ...@@ -28,7 +28,7 @@
using namespace std; using namespace std;
PFP::VEC3 rotate2D (PFP::VEC3 pos1, PFP::VEC3 center, float angle); PFP::VEC3 rotate2D (PFP::VEC3 pos1, PFP::VEC3 center, float angle);
PFP::VEC3 rotate3D (PFP::VEC3 pos1,PFP::VEC3 pos2, PFP::VEC3 center, float angle);
float get_angle (PFP::VEC3 v1, PFP::VEC3 v2); float get_angle (PFP::VEC3 v1, PFP::VEC3 v2);
PFP::VEC3 get_center (ArticulatedObstacle * art, int index); PFP::VEC3 get_center (ArticulatedObstacle * art, int index);
class Simulator ; class Simulator ;
......
...@@ -612,7 +612,8 @@ Dart EnvMap::getBelongingCellOnSurface(const PFP::VEC3& pos) ...@@ -612,7 +612,8 @@ Dart EnvMap::getBelongingCellOnSurface(const PFP::VEC3& pos)
if (!m.isMarked(d)) if (!m.isMarked(d))
{ {
m.mark(d) ; m.mark(d) ;
if (!buildingMark.isMarked(d) && Algo::Surface::Geometry::isPointInConvexFace<PFP>(map, d, position, pos, true)) // if (!buildingMark.isMarked(d) && Algo::Surface::Geometry::isPointInConvexFace<PFP>(map, d, position, pos, true))
if (!buildingMark.isMarked(d) && isInsideFace3D(pos,d) )
{ {
return d ; return d ;
} }
...@@ -1427,6 +1428,34 @@ void EnvMap::resetAgentInFace(Agent* agent) ...@@ -1427,6 +1428,34 @@ void EnvMap::resetAgentInFace(Agent* agent)
} }
#endif #endif
bool EnvMap::isInsideFace3D(VEC3 p, Dart d)
{
VEC3 vec,vec2, norm=Algo::Surface::Geometry::faceNormal<PFP>(map, d, position), p1,p2 = Algo::Surface::Geometry::faceCentroid<PFP>(map, d, position) ;
Dart dd = d,ddd;
do
{
ddd = map.phi1(dd) ;
p1 = position[dd];
vec = VEC3(p2 - p1);
vec2 = VEC3(p -p1);
if (vec*vec2 < 0){
// CGoGNout<<"not inside !"<<CGoGNendl;
return false;
}
dd = ddd ;
} while (dd != d) ;
float dist = (p2-p)*norm ;
if(dist*dist>10.0f) return false;
// CGoGNout<<"c'est dedans !"<<CGoGNendl;
return true;
}
#ifdef SPATIAL_HASHING #ifdef SPATIAL_HASHING
Geom::Vec2ui EnvMap::agentPositionCell(Agent* a) Geom::Vec2ui EnvMap::agentPositionCell(Agent* a)
{ {
......
...@@ -63,20 +63,19 @@ bool MovingObstacle::removeGeneralCell (Dart d) ...@@ -63,20 +63,19 @@ bool MovingObstacle::removeGeneralCell (Dart d)
bool MovingObstacle::is_inside(VEC3 p) bool MovingObstacle::is_inside(VEC3 p)
{ {
// return false; // return false;
VEC3 vec, norm, p1,p2; VEC3 vec, norm, p1,vec2,p2;
for (unsigned int i = 0; i < nbVertices; i++) for (unsigned int i = 0; i < nbVertices; i++)
{ {
p1 = getPosition(i); p1 = getPosition(i);
p2 = getPosition((i+1)%nbVertices); p2 = center;
vec = VEC3(p2 - p1); vec = VEC3(p2 - p1);
norm[0] = vec[1];
norm[1] = -vec[0];
vec = VEC3(p2 -p);
if (vec*norm < 0) vec2 = VEC3(p -p1);
if (vec*vec2 < 0)
return false; return false;
} }
...@@ -120,14 +119,6 @@ VEC3 rotate2D(VEC3 pos1, VEC3 center, float angle) // renvoie le déplacement ne ...@@ -120,14 +119,6 @@ VEC3 rotate2D(VEC3 pos1, VEC3 center, float angle) // renvoie le déplacement ne
return pos2; return pos2;
} }
VEC3 rotate3D(VEC3 pos1,VEC3 pos2, VEC3 center, float angle) // renvoie le déplacement necessaire depuis pos1 pour effectuer la rotation centrée en center d'angle angle sur le plan pos1 pos2 center
{
VEC3 pos3;
/// rotation dans la base (center,pos1,pos2)
// PFP::VEC3 inBase =rotate2D(VEC3(1,0,0),VEC3(0,0,0), angle);
///// changer de base ///
return pos3;
}
MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, std::vector<VEC3> goals, bool rigid, bool spin,int curGoal, Dart dInside, ArticulatedObstacle * art, int indParent) : MovingObstacle::MovingObstacle(Simulator* sim, int ind, std::vector<VEC3> pos, std::vector<VEC3> goals, bool rigid, bool spin,int curGoal, Dart dInside, ArticulatedObstacle * art, int indParent) :
nbVertices(pos.size()), nbVertices(pos.size()),
...@@ -348,10 +339,10 @@ void MovingObstacle::draw(bool showPath) ...@@ -348,10 +339,10 @@ void MovingObstacle::draw(bool showPath)
VEC3 col = Utils::color_map_BCGYR(float(index)/float(sim_->movingObstacles_.size())); VEC3 col = Utils::color_map_BCGYR(float(index)/float(sim_->movingObstacles_.size()));
m_ds->color3f(col[0],col[1],col[2]); m_ds->color3f(col[0],col[1],col[2]);
m_ds->vertex(center);
for(std::vector<VEC3>::iterator it = goals_.begin() ; it != goals_.end() ; ++it) for(unsigned int i = 0 ; i < goals_.size() ; i++)
{ {
m_ds->vertex(*it); m_ds->vertex(goals_[(curGoal_+i)%(goals_.size())]);
} }
m_ds->end(); m_ds->end();
...@@ -363,7 +354,12 @@ VEC3 MovingObstacle::getDilatedPosition(unsigned int ind) ...@@ -363,7 +354,12 @@ VEC3 MovingObstacle::getDilatedPosition(unsigned int ind)
{ {
Dart d(ind); //WARNING : works only for one face created at start ! Dart d(ind); //WARNING : works only for one face created at start !
#ifndef TWO_AND_HALF_DIM #ifndef TWO_AND_HALF_DIM
return position[d]+deformation[d]; // return position[d]+deformation[d];
if(!rigid_)
{
return position[d];
}
else return position[d]+deformation[d];
#else #else
return position[d]; return position[d];
#endif #endif
...@@ -1003,7 +999,16 @@ void MovingObstacle::updateForces() ...@@ -1003,7 +999,16 @@ void MovingObstacle::updateForces()
for (unsigned int i = 0; i < nbVertices; ++i) for (unsigned int i = 0; i < nbVertices; ++i)
{ {
#ifdef TWO_AND_HALF_DIM #ifdef TWO_AND_HALF_DIM
// position[d] += rotate3D(position[d],position[map.phi1(d)], center, abs_angle*rotor);
//// rotation pour les obstacles rectangulaires
VEC3 x = parts_[i]->getPosition()-center;
VEC3 normale = (parts_[(i+1)%nbVertices]->getPosition()-center)^(x);
normale.normalize();
VEC3 y = normale ^x;
y.normalize();
VEC3 nouvpos = rotate2D(VEC3 (1,0,0),(0,0,0),angle);
position[d] += nouvpos[0]*x+nouvpos[1]*y;
#else #else
position[d] += rotate2D(position[d], center, abs_angle*rotor); position[d] += rotate2D(position[d], center, abs_angle*rotor);
#endif #endif
...@@ -1012,7 +1017,7 @@ void MovingObstacle::updateForces() ...@@ -1012,7 +1017,7 @@ void MovingObstacle::updateForces()
#ifdef EXPORTING_BOXES #ifdef EXPORTING_BOXES
#ifndef TWO_AND_HALF_DIM #ifndef TWO_AND_HALF_DIM
position[map.phi<211>(d)] += rotate(position[map.phi<211>(d)], center, abs_angle*rotor); position[map.phi<211>(d)] += rotate2D(position[map.phi<211>(d)], center, abs_angle*rotor);
position[map.phi<211>(d)] += (velocity_ * sim_->timeStep_); position[map.phi<211>(d)] += (velocity_ * sim_->timeStep_);
#endif #endif
......
...@@ -144,7 +144,13 @@ void Simulator::doStep() ...@@ -144,7 +144,13 @@ void Simulator::doStep()
movingObstacles_[i]->initForces(); movingObstacles_[i]->initForces();
clock_gettime(CLOCK_MONOTONIC, &endTime) ; clock_gettime(CLOCK_MONOTONIC, &endTime) ;
time_behave+= timespec_delta(begTime,endTime).tv_nsec; time_behave+= timespec_delta(begTime,endTime).tv_nsec;
// CGoGNout<<" Obstacle "<<i<<" : ";
// for(unsigned int j=0;j<movingObstacles_[i]->nbVertices;j++)
// {
// VEC3 dist =movingObstacles_[i]->parts_[j]->getPosition()-movingObstacles_[i]->parts_[(j+1)%movingObstacles_[i]->nbVertices]->getPosition();
// CGoGNout<<dist.norm()<<" || ";
// }
// CGoGNout<<CGoGNendl;
} }
...@@ -807,41 +813,19 @@ void Simulator::addMovingObstacle(Dart d, unsigned int obstType) ...@@ -807,41 +813,19 @@ void Simulator::addMovingObstacle(Dart d, unsigned int obstType)
VEC3 ySide (0.0f,5.0f,0.0f); VEC3 ySide (0.0f,5.0f,0.0f);
#ifdef TWO_AND_HALF_DIM #ifdef TWO_AND_HALF_DIM
float xSideF=2.0f;
float ySideF=5.0f;
Geom::Plane3D<PFP::REAL> pl(Algo::Surface::Geometry::facePlane<PFP>(envMap_.map, d, envMap_.position)); Geom::Plane3D<PFP::REAL> pl(Algo::Surface::Geometry::facePlane<PFP>(envMap_.map, d, envMap_.position));
VEC3 norm =Algo::Surface::Geometry::faceNormal<PFP>(envMap_.map, d, envMap_.position) ;
VEC3 dir1 =VEC3(norm[1]+norm[2],norm[2]-norm[0],-norm[0]-norm[1]);
VEC3 np; dir1.normalize();
VEC3 dir2 = norm ^dir1;
VEC3 displ = -xSide-ySide;
Agent::rotate(Agent::xyPlane, pl.normal(), displ);
np = start+displ;
pl.project(np); //<- without this line, the point is not on the plane
pl.project(np); //<- without this line, the point is not on the plane (no doublon)
vPos.push_back(np);
displ = xSide-ySide;
Agent::rotate(Agent::xyPlane, pl.normal(), displ);
np = start+displ;
pl.project(np);
vPos.push_back(np);
displ = xSide+ySide;
Agent::rotate(Agent::xyPlane, pl.normal(), displ);
np = start+displ;
pl.project(np);
vPos.push_back(np);
displ = ySide-xSide;
Agent::rotate(Agent::xyPlane, pl.normal(), displ);
np = start+displ;
pl.project(np);
vPos.push_back(np); vPos.push_back(start-xSideF*dir1-ySideF*dir2);
vPos.push_back(start+xSideF*dir1-ySideF*dir2);
vPos.push_back(start+xSideF*dir1+ySideF*dir2);
vPos.push_back(start-xSideF*dir1+ySideF*dir2);
#else #else
vPos.push_back(start-xSide-ySide); vPos.push_back(start-xSide-ySide);
vPos.push_back(start+xSide-ySide); vPos.push_back(start+xSide-ySide);
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment