bool CSPrimRotPoly::IsInside(const double* inCoord, double /*tol*/)
{
if (inCoord==NULL) return false;
double Coord[3];
//transform incoming coordinates into cartesian coords
TransformCoordSystem(inCoord,Coord,m_MeshType,CARTESIAN);
if (m_Transform && Type==ROTPOLY)
TransformCoords(Coord,true, CARTESIAN);
double origin[3]={0,0,0};
double dir[3]={0,0,0};
dir[m_RotAxisDir] = 1;
double foot;
double dist;
Point_Line_Distance(Coord, origin, dir, foot, dist);
int raP = (m_RotAxisDir+1)%3;
int raPP = (m_RotAxisDir+2)%3;
double alpha = atan2(Coord[raPP],Coord[raP]);
if (raP == m_NormDir)
alpha=alpha-M_PI/2;
if (alpha<0)
alpha+=2*M_PI;
origin[0] = dist;origin[1] = dist;origin[2] = dist;
origin[m_NormDir] = 0;
origin[m_RotAxisDir] = foot;
if (alpha<m_StartStopAng[0])
alpha+=2*M_PI;
if ((CSPrimPolygon::IsInside(origin)) && (alpha<m_StartStopAng[1]))
return true;
dist*=-1;
alpha=alpha+M_PI;
if (alpha>2*M_PI)
alpha-=2*M_PI;
if (alpha<m_StartStopAng[0])
alpha+=2*M_PI;
if (alpha>m_StartStopAng[1])
return false;
origin[0] = dist;origin[1] = dist;origin[2] = dist;
origin[m_NormDir] = 0;
origin[m_RotAxisDir] = foot;
return CSPrimPolygon::IsInside(origin);
}
struct Operator::Grid_Path Operator::FindPath(double start[], double stop[])
{
struct Grid_Path path;
// double dV[] = {stop[0]-start[0],stop[1]-start[1],stop[2]-start[2]};
unsigned int uiStart[3],uiStop[3],currPos[3];
SnapToMesh(start,uiStart);
SnapToMesh(stop,uiStop);
currPos[0]=uiStart[0];
currPos[1]=uiStart[1];
currPos[2]=uiStart[2];
double meshStart[] = {discLines[0][uiStart[0]], discLines[1][uiStart[1]], discLines[2][uiStart[2]]};
double meshStop[] = {discLines[0][uiStop[0]], discLines[1][uiStop[1]], discLines[2][uiStop[2]]};
bool UpDir = false;
double foot=0,dist=0,minFoot=0,minDist=0;
int minDir=0;
unsigned int minPos[3];
double startFoot,stopFoot,currFoot;
Point_Line_Distance(meshStart,start,stop,startFoot,dist);
Point_Line_Distance(meshStop,start,stop,stopFoot,dist);
currFoot=startFoot;
minFoot=startFoot;
double P[3];
while (minFoot<stopFoot)
{
minDist=1e300;
for (int n=0; n<3; ++n) //check all 6 surrounding points
{
P[0] = discLines[0][currPos[0]];
P[1] = discLines[1][currPos[1]];
P[2] = discLines[2][currPos[2]];
if (((int)currPos[n]-1)>=0)
{
P[n] = discLines[n][currPos[n]-1];
Point_Line_Distance(P,start,stop,foot,dist);
if ((foot>currFoot) && (dist<minDist))
{
minFoot=foot;
minDist=dist;
minDir = n;
UpDir = false;
}
}
if ((currPos[n]+1)<numLines[n])
{
P[n] = discLines[n][currPos[n]+1];
Point_Line_Distance(P,start,stop,foot,dist);
if ((foot>currFoot) && (dist<minDist))
{
minFoot=foot;
minDist=dist;
minDir = n;
UpDir = true;
}
}
}
minPos[0]=currPos[0];
minPos[1]=currPos[1];
minPos[2]=currPos[2];
if (UpDir)
{
currPos[minDir]+=1;
}
else
{
currPos[minDir]+=-1;
minPos[minDir]-=1;
}
path.posPath[0].push_back(minPos[0]);
path.posPath[1].push_back(minPos[1]);
path.posPath[2].push_back(minPos[2]);
currFoot=minFoot;
path.dir.push_back(minDir);
}
//close missing edges, if currPos is not equal to uiStopPos
for (int n=0; n<3; ++n)
{
if (currPos[n]>uiStop[n])
{
--currPos[n];
path.posPath[0].push_back(currPos[0]);
path.posPath[1].push_back(currPos[1]);
path.posPath[2].push_back(currPos[2]);
path.dir.push_back(n);
}
else if (currPos[n]<uiStop[n])
{
path.posPath[0].push_back(currPos[0]);
path.posPath[1].push_back(currPos[1]);
path.posPath[2].push_back(currPos[2]);
path.dir.push_back(n);
}
}
return path;
}