bool CHESS_STATE::calculateLinearConflict(int ai, int aj, int bi, int bj,
int aGoalI, int aGoalJ, int bGoalI, int bGoalJ) {
double k, b;
// first, judge whether the 4 points are in one line
// second, judege whether the vector(ai - bi, aj - bj)
// and the vector(aGoalI - bGoalI, aGoalJ - bGoalJ) is in contrast
// i = k * j + b
if (aj != bj) {
k = (1.0 * ai - bi) / (aj - bj);
b = ai - aj * k;
if (isTheSame(aGoalI - aGoalJ * k, b) && isTheSame(bGoalI - bGoalJ * k, b)) {
// yes, the 4 points are in one line
double v1i = ai - bi, v1j = aj - bj;
double v2i = aGoalI - bGoalI, v2j = aGoalJ - bGoalJ;
return isTheSame(
(v1i * v2i + v1j * v2j) /
sqrt((v1i * v1i + v1j * v1j) * (v2i * v2i + v2j * v2j)), -1);
}
}
else {
if (aGoalJ == aj && bGoalJ == bj) {
// yes, the 4 points are in one line
double v1i = ai - bi, v1j = aj - bj;
double v2i = aGoalI - bGoalI, v2j = aGoalJ - bGoalJ;
return isTheSame(
(v1i * v2i + v1j * v2j) /
sqrt((v1i * v1i + v1j * v1j) * (v2i * v2i + v2j * v2j)), -1);
}
}
return false;
}
/**
* Returns whether or not the given point p belongs to the face defined
* by the vector points.
*/
static bool onFace(gml::Point3D const & p, std::vector < gml::Point3D > const & points)
{
std::vector<double> plane;
// double dist= 0.0;
std::vector <int> samePoints;
std::vector < gml::Point3D > allPoints;
// Firstly copy of the vector points in the vector
// samePoints, if all the points are different
for(int i=0; i<(int)points.size(); i++)
{
for(int j=i+1; j<(int)points.size();j++)
{
if (isTheSame(points[i], points[j]))
{
samePoints.push_back(i);
}
}
}
// Construction of the vector allPoints, with only the different points
bool diff=true;
for (int i=0; i<(int)points.size();i++)
{
int j=0;
while (j<(int)samePoints.size() && diff)
{
if (i==samePoints[j])
{
diff = false;
}
else
{
j++;
}
}
if (diff)
{
allPoints.push_back(points[i]);
}
diff = true;
}
// 1) Check if the vector allPoints is a really a plan
plane = definePlane(allPoints);
if( plane.empty() )
{
return false;
}
// 2) Check if the current point is on the plan
if( ! onPlane(p, allPoints) )
{
return false;
}
// 3) Check if the point belows to one of edge of the face
for (int i=0 ; i<(int)allPoints.size() ; i++)
{
int next = i+1;
if (next == (int)allPoints.size())
{
next = 0;
}
double t;
if (isOnEdge(p, allPoints[i], allPoints[next], &t))
{
return true;
}
}
// 4) Computation of a line
// double diffx = allPoints[0][0] - allPoints[1][0];
// double diffy = allPoints[0][1] - allPoints[1][1];
// double diffz = allPoints[0][2] - allPoints[1][2];
gml::Vector3D diffVector = allPoints[0] - allPoints[1];
gml::Point3D newPoint = p + diffVector;
// newPoint[0] = p[0] + diffx;
// newPoint[1] = p[1] + diffy;
// newPoint[2] = p[2] + diffz;
// 5) Check if the line cut or not the plan
int nbCut = 0;
for (int i=0 ; i<(int)allPoints.size() ; i++)
{
int next = i+1;
if (next == (int)allPoints.size())
{
next = 0;
}
std::vector< gml::Point3D > newPlane;
newPlane.push_back(newPoint);
newPlane.push_back(allPoints[i]);
newPlane.push_back(allPoints[next]);
double t1, t2;
int valueCut = inter(p, newPlane, &t1, &t2);
if (valueCut > 0)
{
if (isGreaterOrEqual(t1, 0) &&
isGreaterOrEqual(t2, 0) &&
isLesserOrEqual(t2, 1))
{
nbCut++;
}
}
if (nbCut == 1)
{
return true;
}
}
return false;
} //end of method on face
//Returns wheter or not the segment [ a , b]
// intersect he plane defined by the points in the vector
// Valeurs retournées :
// -1 : problème dans la definition du plan
// 0 : le segment n'intersecte pas la face
// 1 : le segment intersecte la face de façon incorrecte
// 2 : le segment intersecte la face de façon correcte
static int interPlan(gml::Point3D & a, gml::Point3D & b,
std::vector < gml::Point3D > const & points,
double *t)
{
std::vector<double> plane;
double null_value = 0.0;
double one = 1.0;
// 1) If the normal of the plan and the vector of the
//edge are perpendicular, then there is no verification
double vectorEdgeX = a[0] - b[0];
double vectorEdgeY = a[1] - b[1];
double vectorEdgeZ = a[2] - b[2];
plane = definePlane(points);
if( plane.empty() )
{
return -1;
}
// Premiere verification, on regarde si le segment est parallèle au plan
// Il y a alors deux cas :
// - le segment est en dehors du plan
// - le segment est sur le plan
if(isEqual((plane[0]*vectorEdgeX + plane[1]*vectorEdgeY + plane[2]*vectorEdgeZ), null_value))
{
// Si le segment ne se trouve pas sur le plan, alors on retourne 0
if (!onPlane(a, points) && !onPlane(b, points)) {
return 0;
}
// Cas ou le segment se trouve sur le plan
else {
/*if (onPlane(a, points) && !onPlane(b, points)) {
std::cout << "a est sur le plan mais pas b" << std::endl;
}
if (!onPlane(a, points) && onPlane(b, points)) {
std::cout << "b est sur le plan mais pas a" << std::endl;
}*/
// Recherche si les points du segment ne correspondent pas aux points de la face
bool founda = false;
bool foundb = false;
int i=0;
int j=0;
while ((!founda || !foundb ) && i<points.size()) {
// Si le premier point a est un point de la face
if (isTheSame(a, points[i])) {
founda = true;
// Gestion des indices
if (i+1 == points.size()) {
j=0;
}
else {
j=i+1;
}
// Si le segment est une arrete de la face on stoppe l'execution
if (isTheSame(b, points[j])) {
return 2;
}
}
// Si le premier point b est un point de la face
if (isTheSame(b, points[i])) {
foundb = true;
// Gestion des indices
if (i+1 == points.size()) {
j=0;
}
else {
j=i+1;
}
// Si le segment est une arrete de la face on stoppe l'execution
if (isTheSame(a, points[j])) {
return 2;
}
}
i++;
}
// Si les deux points du segment sont des points de la face mais pas des points successifs
if (founda && foundb) {
*t=0.0;
return 1;
}
else {
// Le point b est un point de la face mais pas le point a
if (!founda && foundb) {
// Cependant le point a est un point sur la face
if (onFace(a, points)) {
*t=0.0;
return 1;
}
// Sinon
else {
return 2;
}
}
else {
// Le point a est un point de la face mais pas le point b
if (founda && !foundb) {
// Cependant le point b est un point sur la face
if (onFace(b, points)) {
*t=1.0;
return 1;
}
else {
return 2;
}
}
// Aucun des points a ou b n'est un point de la face
else {
// Si le point a est sur la face
if (onFace(a, points)) {
*t=0.0;
return 1;
}
// Si le point b est sur la face
if (onFace(b, points)) {
*t=1.0;
return 1;
}
return 0;
}
}
}
// Probleme de valeur
return -1;
}
}
// 3) Compute of t
*t = (-plane[0] * a[0] - plane[1] * a[1] - plane[2] * a[2] - plane[3]) /
(plane[0]*vectorEdgeX + plane[1]*vectorEdgeY + plane[2]*vectorEdgeZ);
//Intersection avec la droite mais PAS le segment
if ( isLesser(*t, null_value) || isGreater(*t ,one) )
{
return 0;
}
else
{
#ifndef CORE_LEVEL
gml::Point3D interPoint = a.collinear(b, *t);
#else
//TODO : check more accurately
gml::Point3D interPoint = a.collinear(b, (*t).doubleValue() );
#endif
if (! onFace(interPoint, points))
{
return 0;
}
else
{
return 1;
}
}
}
/**
* Returns whether or not Point3D p is on edge [a,b]
*
*/
static bool isOnEdge(gml::Point3D const & p,
gml::Point3D const & a, gml::Point3D const & b,
double * t)
{
double null_value = 0;
double one = 1;
//Is this a real edge ?
if ( isTheSame(a, b) )
{
if ( isTheSame(p, a) )
{
*t = 0.0;
return true;
}
else
{
return false;
}
}
//Point is the first vertex of the edge?
if ( isTheSame(p, a) )
{
*t = 0.0;
return true;
}
//Point is the second vertex of the edge?
if ( isTheSame(p, b) )
{
*t = 1.0;
return true;
}
else //test if the point is between the two points on the edge
{
//OLD FASHION
// double deltaX, deltaY, deltaZ;
// double p0 = p[0]; double p1 = p[1]; double p2 = p[2];
// double a0 = a[0]; double a1 = a[1]; double a2 = a[2];
// double b0 = b[0]; double b1 = b[1]; double b2 = b[2];
// deltaX = (p0 - a0)/(b0 - a0);
// deltaY = (p1 - a1)/(b1 - a1);
// deltaZ = (p2 - a2)/(b2 - a2);
// deltaX = (p[0] - a[0])/(b[0] - a[0]);
// deltaY = (p[1] - a[1])/(b[1] - a[1]);
// deltaZ = (p[2] - a[2])/(b[2] - a[2]);
// if ( isEqual(deltaX, deltaY) && isEqual(deltaY, deltaZ))
// {
// if (isGreaterOrEqual(deltaX, null_value) && isLesserOrEqual(deltaX, one) )
// {
// *t = deltaX;
// return true;
// }
// }
//NEW FASHION
double ux = b[0] - a[0];
double uy = b[1] - a[1];
double uz = b[2] - a[2];
double segmentLength2 = ux*ux + uy*uy + uz*uz;
double inprod = ux*(p[0] - a[0]) + uy*(p[1] - a[1]) + uz*(p[2] - a[2]);
if ( isGreaterOrEqual(inprod, null_value) && isLesserOrEqual(inprod, segmentLength2) )
{
*t = inprod / segmentLength2;
return true;
}
else
{
return false;
}
}
return false;
} //end of method isOnEdge
