Commit 406d546c authored by pitiot's avatar pitiot

up

parent 60f3bfbe
......@@ -33,7 +33,7 @@ class ArticulatedObstacle;
#include "pfp.h"
//#define EXPORTING3
#define TWO_AND_HALF_DIM
//#define TWO_AND_HALF_DIM
#ifdef EXPORTING3
#include "Utils/Shaders/shaderPhongTexture.h"
......@@ -67,6 +67,7 @@ public:
void scale(float val);
bool is_insideConvexCell2D(VEC3 p, Dart d);
bool isInsideFace3D(VEC3 p, Dart d);
void markPedWay() ;
unsigned int mapMemoryCost() ;
......
......@@ -28,7 +28,7 @@
using namespace std;
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);
PFP::VEC3 get_center (ArticulatedObstacle * art, int index);
class Simulator ;
......
......@@ -612,7 +612,8 @@ Dart EnvMap::getBelongingCellOnSurface(const PFP::VEC3& pos)
if (!m.isMarked(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 ;
}
......@@ -1427,6 +1428,34 @@ void EnvMap::resetAgentInFace(Agent* agent)
}
#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
Geom::Vec2ui EnvMap::agentPositionCell(Agent* a)
{
......
......@@ -63,20 +63,19 @@ bool MovingObstacle::removeGeneralCell (Dart d)
bool MovingObstacle::is_inside(VEC3 p)
{
// return false;
VEC3 vec, norm, p1,p2;
VEC3 vec, norm, p1,vec2,p2;
for (unsigned int i = 0; i < nbVertices; i++)
{
p1 = getPosition(i);
p2 = getPosition((i+1)%nbVertices);
p2 = center;
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;
}
......@@ -120,14 +119,6 @@ VEC3 rotate2D(VEC3 pos1, VEC3 center, float angle) // renvoie le déplacement ne
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) :
nbVertices(pos.size()),
......@@ -348,10 +339,10 @@ void MovingObstacle::draw(bool showPath)
VEC3 col = Utils::color_map_BCGYR(float(index)/float(sim_->movingObstacles_.size()));
m_ds->color3f(col[0],col[1],col[2]);
for(std::vector<VEC3>::iterator it = goals_.begin() ; it != goals_.end() ; ++it)
m_ds->vertex(center);
for(unsigned int i = 0 ; i < goals_.size() ; i++)
{
m_ds->vertex(*it);
m_ds->vertex(goals_[(curGoal_+i)%(goals_.size())]);
}
m_ds->end();
......@@ -363,7 +354,12 @@ VEC3 MovingObstacle::getDilatedPosition(unsigned int ind)
{
Dart d(ind); //WARNING : works only for one face created at start !
#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
return position[d];
#endif
......@@ -1003,7 +999,16 @@ void MovingObstacle::updateForces()
for (unsigned int i = 0; i < nbVertices; ++i)
{
#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
position[d] += rotate2D(position[d], center, abs_angle*rotor);
#endif
......@@ -1012,7 +1017,7 @@ void MovingObstacle::updateForces()
#ifdef EXPORTING_BOXES
#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_);
#endif
......
......@@ -144,7 +144,13 @@ void Simulator::doStep()
movingObstacles_[i]->initForces();
clock_gettime(CLOCK_MONOTONIC, &endTime) ;
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)
VEC3 ySide (0.0f,5.0f,0.0f);
#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));
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;
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);
dir1.normalize();
VEC3 dir2 = norm ^dir1;
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
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