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; }