#include "StdAfx.h"
|
#include "PathScheduler_Dual_Sorting.h"
|
#include "PathScheduler_Sorting.h"
|
#include <algorithm>
|
|
CPathScheduler_Dual_Sorting::CPathScheduler_Dual_Sorting(void)
|
{
|
}
|
|
|
CPathScheduler_Dual_Sorting::~CPathScheduler_Dual_Sorting(void)
|
{
|
}
|
|
int CPathScheduler_Dual_Sorting::CalculatePath( const SPathData& startPath, const VectorPathData& vecTotalPathData, const CRect& rtRange, CPathSchedulerResult& scheduleResult )
|
{
|
scheduleResult.Reset();
|
|
// get path data
|
VectorPathData vecPathData;
|
for (constVectorPathDataIt it=vecTotalPathData.begin(); it!=vecTotalPathData.end(); it++)
|
{
|
if ( rtRange.PtInRect( CPoint(it->nPosX,it->nPosY )) )
|
{
|
vecPathData.push_back(*it);
|
}
|
}
|
|
// check path count
|
int nPathCount = (int) vecPathData.size();
|
if (nPathCount<1) return 0;
|
|
// ¿øÁ¡ ¹æÇâ¿¡ µû¶ó ¼ÒÆà ¼ø¼°¡ ´Þ¶óÁø´Ù.
|
int nOrder = m_PathSchedulerParam.GetSortOrderType();
|
switch(m_PathSchedulerParam.GetOriginDir())
|
{
|
case SchedulerDir_LeftTop: // left top
|
case SchedulerDir_LeftBottom: // left bottom
|
nOrder = 0;
|
break;
|
|
case SchedulerDir_RightTop: // right top
|
case SchedulerDir_RightBottom: // right bottom
|
nOrder = 1;
|
break;
|
}
|
|
|
switch(m_PathSchedulerParam.GetSortAxisType())
|
{
|
case 0: // x Axis
|
if (nOrder==0) // ascend
|
{
|
std::sort(vecPathData.begin(), vecPathData.end(), AscendAxisX);
|
}
|
else // descend
|
{
|
std::sort(vecPathData.begin(), vecPathData.end(), DescendAxisX);
|
}
|
break;
|
|
case 1: // y Axis
|
if (nOrder==0) // ascend
|
{
|
std::sort(vecPathData.begin(), vecPathData.end(), AscendAxisY);
|
}
|
else // descend
|
{
|
std::sort(vecPathData.begin(), vecPathData.end(), DescendAxisY);
|
}
|
break;
|
|
default:
|
return 0;
|
break;
|
}
|
|
// resize result count
|
scheduleResult.SetScheduleResultCount(nPathCount);
|
|
// ½ºÄÉÁì ½ÃÀÛÀ§Ä¡..
|
BOOL bStartPos = FALSE;
|
SSchedulerResult sStartPos;
|
if (startPath.nPosX!=NO_PATH_DATA && startPath.nPosY!=NO_PATH_DATA)
|
{
|
sStartPos.nTotalIndex = -1;
|
sStartPos.nPointIndex = -1;
|
sStartPos.dPositionX = startPath.nPosX / 1000.0;
|
sStartPos.dPositionY = startPath.nPosY / 1000.0;
|
sStartPos.dAutoFocusTime = m_PathSchedulerParam.GetAutoFocusTime();
|
sStartPos.dImageGrabTime = m_PathSchedulerParam.GetImageGrabTime();
|
sStartPos.dMotionDelayTime = m_PathSchedulerParam.GetMotionDelayTime();
|
bStartPos = TRUE;
|
}
|
|
for (int i=0; i<nPathCount; i++)
|
{
|
SSchedulerResult *pCurResult = scheduleResult.GetPathSchedulerResult(i);
|
if (pCurResult==NULL) continue;
|
|
pCurResult->nPointIndex = i;
|
|
const SPathData *pPathData = &vecPathData[pCurResult->nPointIndex];
|
if (pPathData==NULL) continue;
|
pCurResult->nTotalIndex = pPathData->nIndex;
|
pCurResult->dPositionX = double(pPathData->nPosX) / 1000.0;
|
pCurResult->dPositionY = double(pPathData->nPosY) / 1000.0;
|
pCurResult->dAutoFocusTime = m_PathSchedulerParam.GetAutoFocusTime();
|
pCurResult->dImageGrabTime = m_PathSchedulerParam.GetImageGrabTime();
|
pCurResult->dMotionDelayTime = m_PathSchedulerParam.GetMotionDelayTime();
|
|
memcpy(pCurResult->pDataType, pPathData->pDataType, sizeof(int)*PATH_DATA_TYPE_COUNT);
|
|
if (i==0) // first point
|
{
|
// exist start pos
|
if (bStartPos)
|
{
|
if (!CalculateDistanceSpeedTime(*pCurResult, sStartPos))
|
{
|
continue;
|
}
|
}
|
else if(!CalculateDistanceSpeedTime(*pCurResult, *pCurResult))
|
{
|
continue;
|
}
|
}
|
else // from second point
|
{
|
SSchedulerResult *pPrevResult = scheduleResult.GetPathSchedulerResult(i-1);
|
if (!CalculateDistanceSpeedTime(*pCurResult, *pPrevResult))
|
{
|
continue;
|
}
|
}
|
}
|
|
return scheduleResult.GetPathSchedulerResultCount();
|
}
|
|
int CPathScheduler_Dual_Sorting::PathScheduling(const VectorPathData& vecPathData, const VectorPathData& vecStartPath)
|
{
|
if (vecStartPath.size()<1) return 0;
|
|
m_vecPathSchedulerResult.clear();
|
m_vecRangeRect.clear();
|
|
// init
|
CRect rtLeftRange(0, 0, m_PathSchedulerParam.GetGlassSizeX(), m_PathSchedulerParam.GetGlassSizeY());
|
CRect rtRightRange(0, 0, m_PathSchedulerParam.GetGlassSizeX(), m_PathSchedulerParam.GetGlassSizeY());
|
|
int nLeftCollisionPosX = -1;
|
int nRightCollisionPosX = -1;
|
|
// set range
|
switch(m_PathSchedulerParam.GetOriginDir())
|
{
|
case SchedulerDir_LeftTop: // left top
|
case SchedulerDir_LeftBottom: // left bottom
|
rtLeftRange.left = 0;
|
rtLeftRange.right = (m_PathSchedulerParam.GetGlassSizeX() / 2);
|
rtRightRange.left = rtLeftRange.right;
|
rtRightRange.right = m_PathSchedulerParam.GetGlassSizeX();
|
|
nLeftCollisionPosX = (m_PathSchedulerParam.GetGlassSizeX() / 2) - m_PathSchedulerParam.GetCollisionDistX();
|
nRightCollisionPosX = (m_PathSchedulerParam.GetGlassSizeX() / 2) + m_PathSchedulerParam.GetCollisionDistX();
|
break;
|
|
case SchedulerDir_RightTop: // right top
|
case SchedulerDir_RightBottom: // right bottom
|
rtLeftRange.left = m_PathSchedulerParam.GetGlassSizeX();
|
rtLeftRange.right = (m_PathSchedulerParam.GetGlassSizeX() / 2);
|
rtRightRange.left = rtLeftRange.right;
|
rtRightRange.right = 0;
|
|
nLeftCollisionPosX = (m_PathSchedulerParam.GetGlassSizeX() / 2) + m_PathSchedulerParam.GetCollisionDistX();
|
nRightCollisionPosX = (m_PathSchedulerParam.GetGlassSizeX() / 2) - m_PathSchedulerParam.GetCollisionDistX();
|
break;
|
}
|
|
// normalize
|
rtLeftRange.NormalizeRect();
|
rtRightRange.NormalizeRect();
|
|
// add range rect
|
m_vecRangeRect.push_back(rtLeftRange);
|
m_vecRangeRect.push_back(rtRightRange);
|
|
// center coord?
|
if (m_PathSchedulerParam.GetCenterCoord())
|
{
|
rtLeftRange.MoveToXY(-m_PathSchedulerParam.GetGlassSizeX()/2, -m_PathSchedulerParam.GetGlassSizeY()/2);
|
rtRightRange.MoveToXY(-m_PathSchedulerParam.GetGlassSizeX()/2, -m_PathSchedulerParam.GetGlassSizeY()/2);
|
}
|
|
// left gantry result
|
CPathSchedulerResult scheduleResultLeft(vecStartPath[0].nIndex);
|
CalculatePath(vecStartPath[0], vecPathData, rtLeftRange, scheduleResultLeft);
|
scheduleResultLeft.SetCollisionPositionX(nLeftCollisionPosX);
|
|
// right gantry result
|
CPathSchedulerResult scheduleResultRight(vecStartPath[1].nIndex);
|
CalculatePath(vecStartPath[1], vecPathData, rtRightRange, scheduleResultRight);
|
scheduleResultRight.SetCollisionPositionX(nRightCollisionPosX);
|
|
// add schedule result
|
m_vecPathSchedulerResult.push_back(scheduleResultLeft);
|
m_vecPathSchedulerResult.push_back(scheduleResultRight);
|
|
return (int) m_vecPathSchedulerResult.size();
|
}
|
