#include "StdAfx.h"
|
#include "PathScheduler_Quad_Sorting.h"
|
#include <algorithm>
|
|
|
#include "MotionInfo_Axis.h"
|
|
inline bool ComparePosAscend(SPathData& a, SPathData& b)
|
{
|
return (a.nPosX < b.nPosX);
|
}
|
|
inline bool ComparePosDescend(SPathData& a, SPathData& b)
|
{
|
return (a.nPosX > b.nPosX);
|
}
|
|
CPathScheduler_Quad_Sorting::CPathScheduler_Quad_Sorting(void)
|
{
|
}
|
|
|
CPathScheduler_Quad_Sorting::~CPathScheduler_Quad_Sorting(void)
|
{
|
}
|
|
int CPathScheduler_Quad_Sorting::PathScheduling( const VectorPathData& vecPathData, const VectorPathData& vecStartPath )
|
{
|
m_vecRangeRect.clear();
|
m_vecScanRangeData.clear();
|
m_vecPathSchedulerResult.clear();
|
|
int nGantryCount = 2;
|
|
int nRangeCount = CalculateRangeRect(
|
nGantryCount,
|
m_PathSchedulerParam.GetModuleCount(),
|
m_PathSchedulerParam.GetScanCount(),
|
m_PathSchedulerParam.GetGlassSizeX(),
|
m_PathSchedulerParam.GetGlassSizeY(),
|
m_PathSchedulerParam.GetScanMargin(),
|
m_PathSchedulerParam.GetOriginDir(),
|
m_PathSchedulerParam.GetBaseModuleDir(),
|
m_vecScanRangeData);
|
|
if (nRangeCount<1) return 0;
|
|
|
int nLeftCollisionPosX = -1;
|
int nRightCollisionPosX = -1;
|
|
// set collision x pos
|
switch(m_PathSchedulerParam.GetOriginDir())
|
{
|
case SchedulerDir_LeftTop: // left top
|
case SchedulerDir_LeftBottom: // left bottom
|
nLeftCollisionPosX = (m_PathSchedulerParam.GetGlassSizeX() / 2) - m_PathSchedulerParam.GetCollisionDistX();
|
nRightCollisionPosX = (m_PathSchedulerParam.GetGlassSizeX() / 2) + m_PathSchedulerParam.GetCollisionDistX();
|
case SchedulerDir_RightTop: // right top
|
case SchedulerDir_RightBottom: // right bottom
|
nLeftCollisionPosX = (m_PathSchedulerParam.GetGlassSizeX() / 2) + m_PathSchedulerParam.GetCollisionDistX();
|
nRightCollisionPosX = (m_PathSchedulerParam.GetGlassSizeX() / 2) - m_PathSchedulerParam.GetCollisionDistX();
|
break;
|
}
|
|
FILE *fp = NULL;
|
CString strFilename = _T("");
|
strFilename.Format(_T("c:\\temp\\range_info.csv"));
|
_tfopen_s(&fp, strFilename, _T("w"));
|
if (fp)
|
{
|
_ftprintf_s(fp, _T("[ModuleIdx], [ScanIdx], [Left], [Right], [Top], [Bottom], [ScanDir], [StartX], [EndX]\n"));
|
}
|
|
int nTotalCount = 0;
|
SPathData sStartPath; // ¸ðµâº° ½ºÄµ ½ÃÀÛÀ§Ä¡ ÀúÀå.
|
for (int nModuleIdx=0; nModuleIdx<m_PathSchedulerParam.GetModuleCount(); nModuleIdx++)
|
{
|
if (int(vecStartPath.size()) <= nModuleIdx) continue;
|
|
for (int nScanIdx = 0; nScanIdx<m_PathSchedulerParam.GetScanCount(); nScanIdx++)
|
{
|
SRangeData* pRangeData = GetScanRangeData(nModuleIdx, nScanIdx);
|
|
if (pRangeData==NULL) continue;
|
|
if (fp)
|
{
|
_ftprintf_s(fp, _T("%d, %d, %d, %d, %d, %d, %d, %d, %d\n"),
|
pRangeData->nModuleIdx,
|
pRangeData->nScanIdx,
|
pRangeData->nLeft,
|
pRangeData->nRight,
|
pRangeData->nTop,
|
pRangeData->nBottom,
|
pRangeData->nScanDir,
|
pRangeData->nScanStartX,
|
pRangeData->nScanEndX);
|
}
|
|
// add range rect
|
m_vecRangeRect.push_back(CRect(pRangeData->nLeft, pRangeData->nTop, pRangeData->nRight, pRangeData->nBottom));
|
|
if (nScanIdx==0) // first scan ÀÇ ½ÃÀÛÀ§Ä¡´Â ÇöÀç ¸ðµâÀÇ À§Ä¡ÀÌ´Ù.
|
{
|
sStartPath = vecStartPath[nModuleIdx];
|
}
|
|
// ¿µ¿ªº° ½ºÄÉÁ층 ¼öÇà.
|
CPathSchedulerResult schedulerResult(nModuleIdx, nScanIdx);
|
|
// °ÕÆ®¸® Ãæµ¹À§Ä¡ ÀúÀå.
|
int nGantryIdx = GetGantryIndex(nModuleIdx);
|
switch(nGantryIdx)
|
{
|
case 0:
|
schedulerResult.SetCollisionPositionX(nLeftCollisionPosX);
|
break;
|
|
case 1:
|
schedulerResult.SetCollisionPositionX(nRightCollisionPosX);
|
break;
|
}
|
|
int nCount = CalculatePath(nModuleIdx, nScanIdx, sStartPath, vecPathData, *pRangeData, schedulerResult);
|
nTotalCount += nCount;
|
if (nCount>1)
|
{
|
// ´ÙÀ½ ½ºÄµÀÇ ½ÃÀÛÀ§Ä¡´Â ÀÌÀü ½ºÄµÀÇ Á¾·á À§Ä¡ÀÌ´Ù.
|
const SSchedulerResult* pNode = schedulerResult.GetPathSchedulerResult(schedulerResult.GetPathSchedulerResultCount() - 1);
|
if (pNode)
|
{
|
sStartPath.nPosX = (pRangeData->nScanEndX);
|
sStartPath.nPosY = int(pNode->dPositionY * 1000);
|
}
|
m_vecPathSchedulerResult.push_back(schedulerResult);
|
}
|
else // ´ë»ó °áÇÔÀÌ ¾øÀ»¶§ xÃà À§Ä¡¸¸ º¯°æÇÑ´Ù.
|
{
|
sStartPath.nPosX = (pRangeData->nScanEndX); //
|
}
|
|
|
|
}
|
}
|
|
if (fp)
|
fclose(fp);
|
|
return int(m_vecPathSchedulerResult.size());
|
}
|
|
|
int CPathScheduler_Quad_Sorting::CalculatePath(int nModuleIdx, int nScanIdx, const SPathData& startPath, const VectorPathData& vecTotalPathData, const SRangeData& rtRange, CPathSchedulerResult& scheduleResult )
|
{
|
scheduleResult.Reset();
|
|
CRect rtTemp(rtRange.nLeft, rtRange.nTop, rtRange.nRight, rtRange.nBottom);
|
|
// get path data
|
VectorPathData vecPathData;
|
for (constVectorPathDataIt it=vecTotalPathData.begin(); it!=vecTotalPathData.end(); it++)
|
{
|
if ( rtTemp.PtInRect( CPoint(it->nPosX,it->nPosY )) )
|
{
|
vecPathData.push_back(*it);
|
}
|
}
|
|
// sort for scan direction
|
if (rtRange.nScanDir==0)
|
{
|
std::sort(vecPathData.begin(), vecPathData.end(), ComparePosAscend);
|
}
|
else
|
{
|
std::sort(vecPathData.begin(), vecPathData.end(), ComparePosDescend);
|
}
|
|
// set result count
|
scheduleResult.SetScheduleResultCount((int)vecPathData.size());
|
|
FILE *fp = NULL;
|
CString strFilename = _T("");
|
strFilename.Format(_T("c:\\temp\\%d_%d.csv"), nModuleIdx, nScanIdx);
|
_tfopen_s(&fp, strFilename, _T("w"));
|
if (fp)
|
{
|
_ftprintf_s(fp, _T("[Left], [Right], [Top], [Bottom], [ScanStart], [ScanEnd], [ScanDir]\n"));
|
_ftprintf_s(fp, _T("%d, %d, %d, %d, %d, %d, %d\n"),
|
rtRange.nLeft,
|
rtRange.nRight,
|
rtRange.nTop,
|
rtRange.nBottom,
|
rtRange.nScanStartX,
|
rtRange.nScanEndX,
|
rtRange.nScanDir);
|
_ftprintf_s(fp, _T("[Index], [PosX], [PosY]\n"));
|
}
|
|
// calculate path
|
for (int nIdx=0; nIdx<(int)vecPathData.size(); nIdx++)
|
{
|
SSchedulerResult sResult;
|
sResult.nTotalIndex = vecPathData[nIdx].nIndex;
|
sResult.dPositionX = double(vecPathData[nIdx].nPosX) / 1000.0;
|
sResult.dPositionY = double(vecPathData[nIdx].nPosY) / 1000.0;
|
|
memcpy(sResult.pDataType, vecPathData[nIdx].pDataType, sizeof(int)*PATH_DATA_TYPE_COUNT);
|
|
if (fp)
|
_ftprintf_s(fp, _T("%d, %d, %d\n"), nIdx, vecPathData[nIdx].nPosX, vecPathData[nIdx].nPosY);
|
|
scheduleResult.SetScheduleResult(nIdx, sResult);
|
}
|
|
if (fp)
|
fclose(fp);
|
|
return scheduleResult.GetPathSchedulerResultCount();
|
}
|
|
|
|
int CPathScheduler_Quad_Sorting::CalculateRangeRect(int nGantryCount, int nModuleCount, int nScanCount, int nGlassSizeX, int nGlassSizeY, int nScanMargin, int nOriginDir, int nBaseDir, VectorRangeData& vecRangeData)
|
{
|
int nGlassDivX = nGlassSizeX / nGantryCount;
|
int nGantryModuleCount = nModuleCount / nGantryCount;
|
int nGlassDivModuleY = nGlassSizeY / nGantryModuleCount;
|
int nGlassDivScanY = nGlassDivModuleY / nScanCount;
|
|
SRangeData tmpRange;
|
|
// x axis
|
int nTotalModuleIdx = 0;
|
for (int nGantryIdx=0; nGantryIdx<nGantryCount; nGantryIdx++)
|
{
|
// scan x range
|
tmpRange.nLeft = nGantryIdx * nGlassDivX;
|
tmpRange.nRight = tmpRange.nLeft + nGlassDivX;
|
|
for (int nModuleIdx=0; nModuleIdx<nGantryModuleCount; nModuleIdx++)
|
{
|
// module y range
|
int nModuleStart = nModuleIdx * nGlassDivModuleY;
|
|
for (int nScanIdx = 0; nScanIdx<nScanCount; nScanIdx++)
|
{
|
// scan y range
|
tmpRange.nTop = nModuleStart + (nScanIdx*nGlassDivScanY);
|
tmpRange.nBottom = tmpRange.nTop + nGlassDivScanY;
|
|
// ½ºÄµ y ½ÃÀÛÀ§Ä¡ ±âº»°ªÀº ¿µ¿ªÀÇ Áß¾ÓÀÓ.
|
// tmpRange.nScanStartY = tmpRange.nTop + ((tmpRange.nBottom - tmpRange.nTop) / 2);
|
tmpRange.nModuleIdx = nTotalModuleIdx;
|
tmpRange.nScanIdx = nScanIdx;
|
tmpRange.nScanDir = (nScanIdx % 2);
|
|
if (tmpRange.nScanDir==0) // Á¤¹æÇâ ½ºÄµ
|
{
|
tmpRange.nScanStartX = tmpRange.nLeft - nScanMargin; // ½ºÄµ ½ÃÀÛÀ§Ä¡
|
tmpRange.nScanEndX = tmpRange.nRight + nScanMargin; // ½ºÄµ Á¾·áÀ§Ä¡
|
}
|
else // ¿ª¹æÇâ ½ºÄµ
|
{
|
tmpRange.nScanStartX = tmpRange.nRight + nScanMargin; // ½ºÄµ ½ÃÀÛÀ§Ä¡
|
tmpRange.nScanEndX = tmpRange.nLeft - nScanMargin; // ½ºÄµ Á¾·áÀ§Ä¡
|
}
|
|
vecRangeData.push_back(tmpRange);
|
}
|
|
nTotalModuleIdx++;
|
}
|
}
|
|
// change range data (origin_dir / base_dir)
|
|
// int nModuleIdx; // module index
|
// int nScanIdx; // scan index
|
// int nScanDir; // scan dir 0/1
|
//
|
// int nScanStartX; // scan start x
|
// int nScanStartY; // scan start y
|
// int nScanEndX; // scan end x
|
//
|
// int nLeft; // range left
|
// int nTop; // range top
|
// int nRight; // range right
|
// int nBottom; // range bottom
|
|
VectorRangeData vecResult;
|
|
for (int nModuleIdx=0; nModuleIdx<nModuleCount; nModuleIdx++)
|
{
|
for (int nScanIdx=0; nScanIdx<nScanCount; nScanIdx++)
|
{
|
SRangeData *pData = GetScanRangeData(nModuleIdx, nScanIdx);
|
if (pData==NULL) continue;
|
|
SRangeData newData = *pData;
|
|
if (IsChangeModuleIndex(nOriginDir, nBaseDir))
|
{
|
int nTmpValue= GetChangeModuleIndex(nModuleIdx, nOriginDir, nBaseDir, nGantryModuleCount, nModuleCount);
|
newData.nModuleIdx = nTmpValue;
|
}
|
|
|
int bScanIdx = IsChangeScanIndex(nOriginDir, nBaseDir);
|
int bScanDir = IsChangeScanDir(nOriginDir, nBaseDir);
|
|
if (bScanDir==1 && bScanIdx==1) // 11
|
{
|
// change scan index
|
int nTmpValue = GetChangeScanIndex(nScanIdx, nScanCount);
|
newData.nScanIdx = nTmpValue;
|
}
|
else if (bScanDir==1 && bScanIdx==0) // 10
|
{
|
// change scan dir
|
int nTmpValue = GetChangeScanDir(newData.nScanDir);
|
newData.nScanDir = nTmpValue;
|
|
// change scan pos
|
swap_value(newData.nScanStartX, newData.nScanEndX);
|
}
|
else if (bScanDir==0 && bScanIdx==1) // 01
|
{
|
// change scan index
|
int nTmpValue = GetChangeScanIndex(nScanIdx, nScanCount);
|
newData.nScanIdx = nTmpValue;
|
|
// change scan dir
|
nTmpValue = GetChangeScanDir(newData.nScanDir);
|
newData.nScanDir = nTmpValue;
|
|
// change scan pos
|
swap_value(newData.nScanStartX, newData.nScanEndX);
|
}
|
// 00
|
|
|
vecResult.push_back(newData);
|
}
|
}
|
|
// set member data
|
vecRangeData = vecResult;
|
|
return (int)vecRangeData.size();
|
}
|
|
|
SRangeData* CPathScheduler_Quad_Sorting::GetScanRangeData( int nModuleIdx, int nScanIdx )
|
{
|
for (VectorRangeDataIt it=m_vecScanRangeData.begin(); it!=m_vecScanRangeData.end(); it++)
|
{
|
if (it->nModuleIdx==nModuleIdx && it->nScanIdx==nScanIdx)
|
{
|
return &(*it);
|
}
|
}
|
return NULL;
|
}
|
|
const SRangeData* CPathScheduler_Quad_Sorting::GetScanRangeData( int nModuleIdx, int nScanIdx ) const
|
{
|
for (constVectorRangeDataIt it=m_vecScanRangeData.begin(); it!=m_vecScanRangeData.end(); it++)
|
{
|
if (it->nModuleIdx==nModuleIdx && it->nScanIdx==nScanIdx)
|
{
|
return &(*it);
|
}
|
}
|
return NULL;
|
}
|
|
|
|
int CPathScheduler_Quad_Sorting::GetDirectionX(int nDir1, int nDir2)
|
{
|
//SchedulerDir_LeftTop=0, SchedulerDir_RightTop, SchedulerDir_LeftBottom, SchedulerDir_RightBottom
|
|
// Dir1 == Left
|
if (nDir1==SchedulerDir_LeftTop || nDir1==SchedulerDir_LeftBottom)
|
{
|
if (nDir2==SchedulerDir_LeftTop || nDir2==SchedulerDir_LeftBottom)
|
{
|
return 1;
|
}
|
else
|
{
|
return -1;
|
}
|
}
|
|
// Dir1 == Right
|
if (nDir2==SchedulerDir_RightTop || nDir2==SchedulerDir_RightBottom)
|
{
|
return 1;
|
}
|
|
return -1;
|
}
|
|
int CPathScheduler_Quad_Sorting::GetDirectionY(int nDir1, int nDir2)
|
{
|
//SchedulerDir_LeftTop=0, SchedulerDir_RightTop, SchedulerDir_LeftBottom, SchedulerDir_RightBottom
|
|
// Dir1 == Top
|
if (nDir1==SchedulerDir_LeftTop || nDir1==SchedulerDir_RightTop)
|
{
|
if (nDir2==SchedulerDir_LeftTop || nDir2==SchedulerDir_RightTop)
|
{
|
return 1;
|
}
|
else
|
{
|
return -1;
|
}
|
}
|
|
// Dir1 == Bottom
|
if (nDir2==SchedulerDir_LeftBottom || nDir2==SchedulerDir_RightBottom)
|
{
|
return 1;
|
}
|
|
return -1;
|
}
|
|
|
int CPathScheduler_Quad_Sorting::IsChangeModuleIndex(int nDir1, int nDir2)
|
{
|
return (nDir1!=nDir2);
|
}
|
|
int CPathScheduler_Quad_Sorting::IsChangeScanIndex(int nDir1, int nDir2)
|
{
|
if (GetDirectionY(nDir1, nDir2)==-1) return 1;
|
return 0;
|
}
|
|
int CPathScheduler_Quad_Sorting::IsChangeScanDir(int nDir1, int nDir2)
|
{
|
if (GetDirectionX(nDir1, nDir2)==-1) return 1;
|
return 0;
|
}
|
|
int CPathScheduler_Quad_Sorting::GetChangeModuleIndex(int nModuleIndex, int nOriginDir, int nBaseDir, int nGantryModuleCount, int nTotalModuleCount)
|
{
|
int nTmpValue = nModuleIndex;
|
|
if (GetDirectionX(nOriginDir, nBaseDir)==-1) // x ¿ª¹æÇâ
|
{
|
if (nTmpValue<nGantryModuleCount)
|
{
|
nTmpValue = nTmpValue + nGantryModuleCount;
|
}
|
else
|
{
|
nTmpValue = nTmpValue - nGantryModuleCount;
|
}
|
}
|
|
if (GetDirectionY(nOriginDir, nBaseDir)==-1) // y ¿ª¹æÇâ
|
{
|
if (nTmpValue<nGantryModuleCount)
|
{
|
nTmpValue = (nGantryModuleCount - 1) - nTmpValue;
|
}
|
else
|
{
|
nTmpValue = nTotalModuleCount + (nGantryModuleCount - 1) - nTmpValue;
|
}
|
}
|
|
return nTmpValue;
|
}
|
|
int CPathScheduler_Quad_Sorting::GetChangeScanIndex(int nScanIndex, int nTotalScanCount)
|
{
|
return (nTotalScanCount - 1) - nScanIndex;
|
}
|
|
int CPathScheduler_Quad_Sorting::GetChangeScanDir(int nScanDir)
|
{
|
return !nScanDir;
|
}
|
|
int CPathScheduler_Quad_Sorting::GetGantryIndex( int nModuleIdx )
|
{
|
int nGantryCount = 2;
|
int nGantryModuleCount = m_PathSchedulerParam.GetModuleCount() / nGantryCount;
|
|
if (nModuleIdx<nGantryModuleCount)
|
{
|
return 0;
|
}
|
|
if (nModuleIdx>nGantryModuleCount && nModuleIdx<m_PathSchedulerParam.GetModuleCount())
|
{
|
return 1;
|
}
|
|
return -1;
|
}
|
