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;
}
bool CollisionHandlerDefault::IsColliding(const Robot& robot, const Tree& tree)
{
ReachableObstaclesContainerCollide rc(world_, tree, robot);
world_.Accept(rc);
T_Point points = TreeToSegments(robot, tree);
if(points.size() >= 2)
{
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;
}
int PrioritySearchTree<T_Point>::
FindIndexOfMaxPoint(int curVertexIndex, std::vector<T_Point> &points,
KeyType maxKey, int &maxPointValue,
int &maxPointIndex) {
//
// Attempt to find the leaf vertex beneath this vertex that has
// the largest score, with a key less than max key.
//
// On return:
// Return 1 if a value is assigned to maxPointValue, 0 otherwise.
// If a value is assigned to maxPointValue, this sets:
// maxPointValue is the score of the maximum point.
// maxPointIndex the index of the point in 'points' that has
// the maximum score.
//
//
// The vertex at curVertexIndex has a max score node beneath it,
// if it has been initialized. If the maxScoreNode has a key less
// than the current maxKey, then we know the maximum value is
// contained beneath this vertex, AND that its key is within the
// range in the rage maximum query.
// That means that there is no need to continue the search below here.
//
if ((*treePtr)[curVertexIndex].maxScoreNode == -1) {
return 0;
}
T_Point thisPoint = points[(*treePtr)[curVertexIndex].maxScoreNode];
if (thisPoint.GetKey() < maxKey) {
if (thisPoint.GetScore() >= maxPointValue) {
maxPointValue = thisPoint.GetScore();
maxPointIndex = (*treePtr)[curVertexIndex].maxScoreNode;
return 1;
}
else {
return 0;
}
}
//
// Otherwise, the maximum scoring node beneath this node has a
// key greater than the max key. That means that the search must
// continue for the maximum value node with a key less than 'maxKey'.
//
// The search has two cases:
// First, if the median key of this node is greater than the maxKey,
// all keys on the right side of the tree are greater than maxKey,
// so do not search there.
//
// If the median key of this node si less than maxKey, there may
// be a node on the left or right child of the current node with
// a maximum key. Search both to the left and right.
//
else {
if (!(*treePtr)[curVertexIndex].isALeaf) {
if (maxKey <= (*treePtr)[curVertexIndex].medianKey) {
return FindIndexOfMaxPoint(
(*treePtr)[curVertexIndex].leftChildIndex,
points, maxKey, maxPointValue, maxPointIndex);
}
else {
int foundValueLeft, foundValueRight;
foundValueLeft = FindIndexOfMaxPoint(
(*treePtr)[curVertexIndex].leftChildIndex,
points, maxKey, maxPointValue, maxPointIndex);
foundValueRight = FindIndexOfMaxPoint(
(*treePtr)[curVertexIndex].rightChildIndex,
points, maxKey, maxPointValue, maxPointIndex);
return (foundValueLeft or foundValueRight);
}
}
else {
//
// The current node is a leaf node, but due to the condition
// from before, its key is greater than or equal to the max key,
// therefore its score cannot be used for the maximum score.
// Returning 0 here signifies that this search-branch did not
// turn up any candidates for
// the maximum scoring node.
return 0;
}
}
}
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]);
}
}
}
Bezier* wrapBezierConstructorConstraintsTemplate(const PointList& array, const CurveConstraints& constraints, const real ub =1.)
{
T_Point asVector = vectorFromEigenArray<PointList, T_Point>(array);
return new Bezier(asVector.begin(), asVector.end(), constraints, ub);
}
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));
}
}