bool CollisionHandlerDefault::IsSoftColliding(const Robot& robot, const Tree& tree)
{
ReachableObstaclesContainerCollide rc(world_, tree, robot);
world_.Accept(rc);
T_Point points = TreeToSegments(robot, tree);
if(points.size() >= 2)
{
if(points.size() >= 3)
{
matrices::Vector3 pt2 = points[points.size()-1];
points.pop_back();
matrices::Vector3 pt1 = points[points.size()-1];
points.push_back(pt1 + (pt2 - pt1) * 0.9);
}
for(ReachableObstaclesContainerCollide::T_ObstaclesCIT it = rc.obstacles_.begin(); it!= rc.obstacles_.end(); ++it)
{
T_Point::const_iterator ptIt = points.begin();;
T_Point::const_iterator ptIt2 = points.begin(); ++ptIt2;
for(;ptIt2 != points.end(); ++ptIt, ++ptIt2)
{
if(intersection_.Intersect(*ptIt, *ptIt2, **it))
{
return true;
}
}
}
}
return false;
}
T_Point vectorFromEigenArray(const PointList& array)
{
T_Point res;
for(int i =0;i<array.cols();++i)
res.push_back(array.col(i));
return res;
}
void WorldParserObj::CreateObstacle(const std::vector<std::string>& lines, bool isGround)
{
vector<long int> indices;
T_Point points;
for(int i =0; i<2; ++i)
{
string ligne = doubleSlash(lines[i]);
ligne=remplacerSlash(ligne); //On remplace les '/' par des espaces, ex : pour "f 1/2/3 4/5/6 7/8/9" on obtiendra "f 1 2 3 4 5 6 7 8 9"
vector<string> termes=splitSpace(ligne.substr(2)); //On éclate la chaîne en ses espaces (le substr permet d'enlever "f ")
int ndonnees=(int)termes.size()/3;
for(int i=0; i <ndonnees;i++) //On aurait très bien pu mettre i<ndonnees mais je veux vraiment limiter à 3 ou 4
{
long int idx = (strtol(termes[i*3].c_str(),NULL, 10)) - 1;
std::vector<long int>::iterator it = indices.begin();
it = find (indices.begin(), indices.end(), idx);
if(it == indices.end())
{
indices.push_back(idx);
points.push_back(points_[(int)idx]);
}
}
}
//T_Point points;
//for(int i =0; i<4; ++i)
//{
// char x[255],y[255],z[255];
// sscanf(lines[i].c_str(),"v %s %s %s",x,z,y);
// points.push_back(Vector3(-strtod (x, NULL), strtod(y, NULL), strtod(z, NULL)));
//}
Vector3 p1(points[0]);
Vector3 p2(points[1]);
Vector3 p3(points[2]);
Vector3 p4(points[3]);
Vector3 u_(p3-p4);
Vector3 v_(p1-p4);
if(abs( u_.dot(v_)) > 0.001) v_ = p2 - p4;
double a_, b_, c_, d_, norm_, normsquare_;
//we need convex hull of this crap
Vector3 normal (u_.cross(v_));
a_ = (float)(normal.x());
b_ = (float)(normal.y());
c_ = (float)(normal.z());
//if (c_ < 0) c_ = -c_;
norm_ = (float)(normal.norm());
normsquare_ = norm_ * norm_;
d_ = (float)(-(a_ * p1.x() + b_ * p1.y() + c_ * p1.z()));
Matrix4 basis_ = Matrix4::Zero();
Vector3 x = u_; x.normalize();
Vector3 y = v_; y.normalize();
normal.normalize();
basis_.block(0,0,3,1) = x;
basis_.block(0,1,3,1) = y;
basis_.block(0,2,3,1) = normal;
basis_.block(0,3,3,1) = p4;
basis_(3,3) = 1;
Matrix4 basisInverse_ = basis_.inverse();
T_Point transformedPoints;
for(int i=0; i<4; ++i)
{
transformedPoints.push_back(matrices::matrix4TimesVect3(basisInverse_, points[i]));
}
points = ConvexHull(transformedPoints);
transformedPoints.clear();
for(int i=0; i<4; ++i)
{
transformedPoints.push_back(matrices::matrix4TimesVect3(basis_, points[i]));
}
// make sure normal in the good sense :
u_ = transformedPoints[2] - transformedPoints[3];
v_ = transformedPoints[0] - transformedPoints[3];
normal = u_.cross(v_);
normals_[(int)indices[0]];
if(isGround)
{
if(normal.dot(normals_[(int)indices[0]]) > 0 )
{
manager_.AddGround(transformedPoints[0], transformedPoints[1], transformedPoints[2], transformedPoints[3]);
}
else
{
manager_.AddGround(transformedPoints[2], transformedPoints[1], transformedPoints[0], transformedPoints[3]);
}
}
else
{
if(normal.dot(normals_[(int)indices[0]]) > 0 )
{
manager_.AddObstacle(transformedPoints[0], transformedPoints[1], transformedPoints[2], transformedPoints[3]);
}
else
{
manager_.AddObstacle(transformedPoints[2], transformedPoints[1], transformedPoints[0], transformedPoints[3]);
}
}
}
void WorldParserObj::CreateObstacle(const std::string& line)
{
if(line.find("c ") == 0)
{
char r[255],g[255],b[255],t[255];
sscanf(line.c_str(),"c %s %s %s %s",r,g,b,t);
//manager_.SetNextColor(strtod (r, NULL), strtod(g, NULL), strtod(b, NULL));
//manager_.SetNextTransparency(strtod (t, NULL));
return;
}
vector<long int> indices;
vector<long int> normalindices;
T_Point points;
for(int i =0; i<1; ++i)
{
string ligne = doubleSlash(line);
ligne=remplacerSlash(ligne); //On remplace les '/' par des espaces, ex : pour "f 1/2/3 4/5/6 7/8/9" on obtiendra "f 1 2 3 4 5 6 7 8 9"
vector<string> termes=splitSpace(ligne.substr(2)); //On éclate la chaîne en ses espaces (le substr permet d'enlever "f ")
int ndonnees=(int)termes.size()/3;
for(int i=0; i <ndonnees;i++) //On aurait très bien pu mettre i<ndonnees mais je veux vraiment limiter à 3 ou 4
{
long int idx = (strtol(termes[i*3].c_str(),NULL, 10)) - 1;
long int idn = (strtol(termes[i*3+2].c_str(),NULL, 10)) - 1;
std::vector<long int>::iterator it = indices.begin();
it = find (indices.begin(), indices.end(), idx);
if(it == indices.end())
{
indices.push_back(idx);
points.push_back(points_[(int)idx]);
}
it = find (normalindices.begin(), normalindices.end(), idn);
if(it == normalindices.end())
{
normalindices.push_back(idn);
}
}
}
Vector3d p1(points[0]);
Vector3d p2(points[1]);
Vector3d p3(points[2]);
Vector3d u_(p3-p1);
Vector3d v_(p2-p1);
if(abs( u_.dot(v_)) > 0.001) { v_ = u_; u_ = p2-p1;}
//we need convex hull of this crap
Vector3d normal (u_.cross(v_));
normals_[(int)normalindices[0]];
SampleGenerator * generator = SampleGenerator::GetInstance();
Obstacle * obstacle;
if(normal.dot(normals_[(int)normalindices[0]]) > 0 )
{
generator->AddObstacle(new Obstacle(p1, p2, p3));
}
else
{
generator->AddObstacle(new Obstacle(p3, p2, p1));
}
}