Beispiel #1
0
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);
}
Beispiel #2
0
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;
}