#include "StdAfx.h"
|
#include "PathScheduler_Quad_Active_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);
|
}
|
|
|
inline bool ComparePosX(SPathData& a, SPathData& b)
|
{
|
return (a.nPosX < b.nPosX);
|
}
|
|
|
inline bool ComparePosY(SPathData& a, SPathData& b)
|
{
|
return (a.nPosY < b.nPosY);
|
}
|
|
inline int GetDiffCountX(const VectorPathData& vecPathData, int nCenterPos)
|
{
|
int nLeft = 0;
|
int nRight = 0;
|
for (constVectorPathDataIt it=vecPathData.begin(); it!=vecPathData.end(); it++)
|
{
|
if (it->nPosX < nCenterPos)
|
{
|
nLeft++;
|
}
|
else
|
{
|
nRight++;
|
}
|
}
|
|
return abs(nLeft-nRight);
|
}
|
|
inline int GetDiffCountY(const VectorPathData& vecPathData, int nCenterPos)
|
{
|
int nTop = 0;
|
int nBottom = 0;
|
for (constVectorPathDataIt it=vecPathData.begin(); it!=vecPathData.end(); it++)
|
{
|
if (it->nPosY < nCenterPos)
|
{
|
nTop++;
|
}
|
else
|
{
|
nBottom++;
|
}
|
}
|
|
return abs(nTop-nBottom);
|
}
|
|
inline int GetCollisionPosX(const VectorPathData& vecPathData, int nGlassSizeX, int nCollisionDist)
|
{
|
VectorPathData vecSortData = vecPathData;
|
|
// sort
|
std::sort(vecSortData.begin(), vecSortData.end(), ComparePosX);
|
int nSize = (int)vecSortData.size();
|
int nCenterPosX = vecSortData[nSize/2].nPosX;
|
int nCollisionDistX = nCollisionDist;
|
|
// À§Ä¡°¡ ³Ê¹« À۰ųª Ŭ¶§?
|
if ( nCenterPosX<(nCollisionDistX*2) || nCenterPosX>=(nGlassSizeX-(nCollisionDistX*2)) )
|
{
|
int nLeftPos = (nCollisionDistX*2);
|
int nRightPos = nGlassSizeX-(nCollisionDistX*2);
|
|
// Á¿ìÀÇ Å©±â Â÷ÀÌ°¡ ÀÛÀº À§Ä¡·Î ÇÒ´çÇÏÀÚ!
|
nCenterPosX = (GetDiffCountX(vecPathData, nLeftPos) < GetDiffCountX(vecPathData, nRightPos)) ? nLeftPos: nRightPos;
|
}
|
|
return nCenterPosX;
|
}
|
|
|
inline int GetCollisionPosY(const VectorPathData& vecPathData, int nGlassSizeY, int nCollisionDist)
|
{
|
VectorPathData vecSortData = vecPathData;
|
|
// sort
|
std::sort(vecSortData.begin(), vecSortData.end(), ComparePosY);
|
int nSize = (int)vecSortData.size();
|
int nCenterPosY = vecSortData[nSize/2].nPosY;
|
int nCollisionDistY = nCollisionDist;
|
|
// À§Ä¡°¡ ³Ê¹« À۰ųª Ŭ¶§?
|
if ( nCenterPosY<(nCollisionDistY*2) || nCenterPosY>=(nGlassSizeY-(nCollisionDistY*2)) )
|
{
|
int nTopPos = (nCollisionDistY*2);
|
int nBottomPos = nGlassSizeY-(nCollisionDistY*2);
|
|
// Á¿ìÀÇ Å©±â Â÷ÀÌ°¡ ÀÛÀº À§Ä¡·Î ÇÒ´çÇÏÀÚ!
|
nCenterPosY = (GetDiffCountY(vecPathData, nTopPos) < GetDiffCountY(vecPathData, nBottomPos)) ? nTopPos: nBottomPos;
|
}
|
|
return nCenterPosY;
|
}
|
|
|
CPathScheduler_Quad_Active_Sorting::CPathScheduler_Quad_Active_Sorting(void)
|
{
|
}
|
|
CPathScheduler_Quad_Active_Sorting::~CPathScheduler_Quad_Active_Sorting(void)
|
{
|
}
|
|
int CPathScheduler_Quad_Active_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.GetGlassSizeX(),
|
m_PathSchedulerParam.GetGlassSizeY(),
|
m_PathSchedulerParam.GetCollisionDistX(),
|
m_PathSchedulerParam.GetCollisionDistY(),
|
m_PathSchedulerParam.GetOriginDir(),
|
m_PathSchedulerParam.GetBaseModuleDir(),
|
vecPathData,
|
m_vecScanRangeData);
|
|
if (nRangeCount<1) return 0;
|
|
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], [ColliX], [ColliY]\n"));
|
}
|
|
m_vecRangeRect.clear();
|
|
int nTotalCount = 0;
|
SPathData sStartPath; // ¸ðµâº° ½ºÄµ ½ÃÀÛÀ§Ä¡ ÀúÀå.
|
for (int nModuleIdx=0; nModuleIdx<m_PathSchedulerParam.GetModuleCount(); nModuleIdx++)
|
{
|
if (int(vecStartPath.size()) <= nModuleIdx) continue;
|
|
SRangeData* pRangeData = GetScanRangeData(nModuleIdx, 0); // ½ºÄµ¼ö´Â 1ȸÀÓ.
|
if (pRangeData==NULL) continue;
|
|
CRect rect = CRect(pRangeData->nLeft, pRangeData->nTop, pRangeData->nRight, pRangeData->nBottom);
|
rect.NormalizeRect();
|
m_vecRangeRect.push_back(rect);
|
|
if (fp)
|
{
|
_ftprintf_s(fp, _T("%d, %d, %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,
|
pRangeData->nColliPosX,
|
pRangeData->nColliPosY);
|
}
|
|
// ½ÃÀÛÀ§Ä¡´Â ÇöÀç¸ðµâÀÇ À§Ä¡ÀÌ´Ù.
|
sStartPath = vecStartPath[nModuleIdx];
|
|
// ¿µ¿ªº° ½ºÄÉÁ층 ¼öÇà.
|
CPathSchedulerResult schedulerResult(nModuleIdx, 0);
|
|
// °æ·Î °è»ê
|
int nCount = CalculatePath(nModuleIdx, 0, sStartPath, vecPathData, *pRangeData, schedulerResult);
|
nTotalCount += nCount;
|
|
// ¸ðµâÀÇ Ãæµ¹À§Ä¡ ÀúÀå
|
schedulerResult.SetCollisionPositionX(pRangeData->nColliPosX);
|
schedulerResult.SetCollisionPositionY(pRangeData->nColliPosY);
|
|
// °á°ú ÀúÀå.
|
m_vecPathSchedulerResult.push_back(schedulerResult);
|
}
|
|
if (fp) fclose(fp);
|
|
return int(m_vecPathSchedulerResult.size());
|
}
|
|
|
int CPathScheduler_Quad_Active_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
|
|
SSchedulerResult sStartPos;
|
sStartPos.dPositionX = double(startPath.nPosX / 1000.0);
|
sStartPos.dPositionY = double(startPath.nPosY / 1000.0);
|
|
|
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;
|
sResult.dAutoFocusTime = m_PathSchedulerParam.GetAutoFocusTime();
|
sResult.dImageGrabTime = m_PathSchedulerParam.GetImageGrabTime();
|
sResult.dMotionDelayTime = m_PathSchedulerParam.GetMotionDelayTime();
|
|
memcpy(sResult.pDataType, vecPathData[nIdx].pDataType, sizeof(int)*PATH_DATA_TYPE_COUNT);
|
|
// exist start pos
|
if (!CalculateDistanceSpeedTime(sResult, sStartPos))
|
{
|
continue;
|
}
|
|
// save cur_pos to start_pos
|
sStartPos = sResult;
|
|
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_Active_Sorting::CalculateRangeRect(int nGantryCount, int nModuleCount, int nGlassSizeX, int nGlassSizeY,
|
int nDistX, int nDistY, int nOriginDir, int nBaseDir, const VectorPathData& vecPathData, VectorRangeData& vecRangeData)
|
{
|
// get x pos
|
int nResultPosX = GetCollisionPosX(vecPathData, m_PathSchedulerParam.GetGlassSizeX(), m_PathSchedulerParam.GetCollisionDistX());
|
|
// get left or right
|
VectorPathData vecLeft, vecRight;
|
for (constVectorPathDataIt it=vecPathData.begin(); it!=vecPathData.end(); it++)
|
{
|
if (it->nPosX > nResultPosX)
|
{
|
vecRight.push_back(*it);
|
}
|
else
|
{
|
vecLeft.push_back(*it);
|
}
|
}
|
|
// get y pos
|
int nResultLeftY = GetCollisionPosY(vecLeft, m_PathSchedulerParam.GetGlassSizeY(), 0);
|
|
// get y pos
|
int nResultRightY = GetCollisionPosY(vecRight, m_PathSchedulerParam.GetGlassSizeY(), 0);
|
|
|
SRangeData range;
|
range.nScanIdx = 0;
|
range.nScanDir = 0;
|
|
// left top
|
range.nTop = 0;
|
range.nBottom = nResultLeftY;
|
range.nLeft = 0;
|
range.nRight = nResultPosX;
|
|
range.nModuleIdx = 0;
|
range.nScanStartX = range.nLeft; // ½ºÄµ ½ÃÀÛÀ§Ä¡
|
range.nScanEndX = range.nRight; // ½ºÄµ Á¾·áÀ§Ä¡
|
range.nColliPosX = range.nRight - m_PathSchedulerParam.GetCollisionDistX();
|
range.nColliPosY = range.nBottom - m_PathSchedulerParam.GetCollisionDistY();
|
vecRangeData.push_back(range);
|
|
// left bottom
|
range.nTop = nResultLeftY;
|
range.nBottom = m_PathSchedulerParam.GetGlassSizeY();
|
range.nLeft = 0;
|
range.nRight = nResultPosX;
|
|
range.nModuleIdx = 1;
|
range.nScanStartX = range.nLeft; // ½ºÄµ ½ÃÀÛÀ§Ä¡
|
range.nScanEndX = range.nRight; // ½ºÄµ Á¾·áÀ§Ä¡
|
range.nColliPosX = range.nRight - m_PathSchedulerParam.GetCollisionDistX();
|
range.nColliPosY = range.nTop + m_PathSchedulerParam.GetCollisionDistY();
|
vecRangeData.push_back(range);
|
|
// right top
|
range.nTop = 0;
|
range.nBottom = nResultRightY;
|
range.nLeft = nResultPosX;
|
range.nRight = m_PathSchedulerParam.GetGlassSizeX();
|
|
range.nModuleIdx = 2;
|
range.nScanStartX = range.nLeft; // ½ºÄµ ½ÃÀÛÀ§Ä¡
|
range.nScanEndX = range.nRight; // ½ºÄµ Á¾·áÀ§Ä¡
|
range.nColliPosX = range.nLeft + m_PathSchedulerParam.GetCollisionDistX();
|
range.nColliPosY = range.nBottom - m_PathSchedulerParam.GetCollisionDistY();
|
vecRangeData.push_back(range);
|
|
// right bottom
|
range.nTop = nResultRightY;
|
range.nBottom = m_PathSchedulerParam.GetGlassSizeY();
|
range.nLeft = nResultPosX;
|
range.nRight = m_PathSchedulerParam.GetGlassSizeX();
|
|
range.nModuleIdx = 3;
|
range.nScanStartX = range.nLeft; // ½ºÄµ ½ÃÀÛÀ§Ä¡
|
range.nScanEndX = range.nRight; // ½ºÄµ Á¾·áÀ§Ä¡
|
range.nColliPosX = range.nLeft + m_PathSchedulerParam.GetCollisionDistX();
|
range.nColliPosY = range.nTop + m_PathSchedulerParam.GetCollisionDistY();
|
vecRangeData.push_back(range);
|
|
// set info
|
int nGlassDivX = nGlassSizeX / nGantryCount;
|
int nGantryModuleCount = nModuleCount / nGantryCount;
|
int nGlassDivModuleY = nGlassSizeY / nGantryModuleCount;
|
int nGlassDivScanY = nGlassDivModuleY;
|
int nScanCount = 1;
|
int nScanIdx = 0;
|
|
VectorRangeData vecResult;
|
|
for (int nModuleIdx=0; nModuleIdx<nModuleCount; nModuleIdx++)
|
{
|
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 bScanDir = IsChangeScanDir(nOriginDir, nBaseDir);
|
if (bScanDir==1) // 10
|
{
|
// change scan dir
|
int nTmpValue = GetChangeScanDir(newData.nScanDir);
|
newData.nScanDir = nTmpValue;
|
|
// change scan pos
|
swap_value(newData.nScanStartX, newData.nScanEndX);
|
}
|
|
|
vecResult.push_back(newData);
|
}
|
|
// set member data
|
vecRangeData = vecResult;
|
|
return (int)vecRangeData.size();
|
}
|
|
|
SRangeData* CPathScheduler_Quad_Active_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_Active_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_Active_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_Active_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_Active_Sorting::IsChangeModuleIndex(int nDir1, int nDir2)
|
{
|
return (nDir1!=nDir2);
|
}
|
|
int CPathScheduler_Quad_Active_Sorting::IsChangeScanIndex(int nDir1, int nDir2)
|
{
|
if (GetDirectionY(nDir1, nDir2)==-1) return 1;
|
return 0;
|
}
|
|
int CPathScheduler_Quad_Active_Sorting::IsChangeScanDir(int nDir1, int nDir2)
|
{
|
if (GetDirectionX(nDir1, nDir2)==-1) return 1;
|
return 0;
|
}
|
|
int CPathScheduler_Quad_Active_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_Active_Sorting::GetChangeScanIndex(int nScanIndex, int nTotalScanCount)
|
{
|
return (nTotalScanCount - 1) - nScanIndex;
|
}
|
|
int CPathScheduler_Quad_Active_Sorting::GetChangeScanDir(int nScanDir)
|
{
|
return !nScanDir;
|
}
|
|
int CPathScheduler_Quad_Active_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;
|
}
|
