#include "StdAfx.h"
|
#include "PathScheduler_Dual_Active_TSP.h"
|
#include "DynamicTSP.h"
|
#include "AnnealingTSP.h"
|
|
CPathScheduler_Dual_Active_TSP::CPathScheduler_Dual_Active_TSP(void)
|
{
|
}
|
|
|
CPathScheduler_Dual_Active_TSP::~CPathScheduler_Dual_Active_TSP(void)
|
{
|
}
|
|
int CPathScheduler_Dual_Active_TSP::CalculatePath( const SPathData& startPath, const VectorPathData& vecTotalPathData, const CRect& rtRange, CPathSchedulerResult& scheduleResult )
|
{
|
bool bShortPath= GetPathSchedulerParam()->GetUseShortPath();
|
|
if (bShortPath == TRUE)
|
{
|
//½ºÄÉÁ층 20210111
|
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;
|
|
// new TSP
|
CGreedyTSP *pTSP = NULL;
|
if (nPathCount <= m_nPointMin)
|
{
|
pTSP = new CDynamicTSP();
|
}
|
else if (nPathCount <= m_nPointMax)
|
{
|
pTSP = new CAnnealingTSP();
|
}
|
else
|
{
|
pTSP = new CGreedyTSP();
|
}
|
if (pTSP == NULL) return 0;
|
|
// set path data
|
pTSP->SetPathData(vecPathData, startPath);
|
|
// cal tsp
|
if (pTSP->CalculateTSP() < 0.1)
|
{
|
delete pTSP;
|
return 0;
|
}
|
|
// check path count
|
nPathCount = pTSP->GetPathCount();
|
if (nPathCount < 1)
|
{
|
delete pTSP;
|
return 0;
|
}
|
|
// 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 = pTSP->GetPathResult(i);
|
if (pCurResult->nPointIndex < 0) continue;
|
|
const SPathData *pPathData = pTSP->GetPathData(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;
|
}
|
}
|
}
|
|
delete pTSP;
|
|
return (int)vecPathData.size();
|
|
}
|
else
|
{
|
//½ºÄÉÁ층 20210111
|
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;
|
|
// new TSP
|
CGreedyTSP *pTSP = NULL;
|
if (nPathCount <= m_nPointMin)
|
{
|
pTSP = new CDynamicTSP();
|
}
|
else if (nPathCount <= m_nPointMax)
|
{
|
pTSP = new CAnnealingTSP();
|
}
|
else
|
{
|
pTSP = new CGreedyTSP();
|
}
|
if (pTSP == NULL) return 0;
|
|
// set path data
|
pTSP->SetPathData(vecPathData, startPath);
|
|
// cal tsp
|
if (pTSP->CalculateTSP() < 0.1)
|
{
|
delete pTSP;
|
return 0;
|
}
|
|
// check path count
|
nPathCount = pTSP->GetPathCount();
|
if (nPathCount < 1)
|
{
|
delete pTSP;
|
return 0;
|
}
|
|
// 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 = pTSP->GetPathResult(i);
|
pCurResult->nPointIndex = i;
|
if (pCurResult->nPointIndex < 0) continue;
|
|
//const SPathData *pPathData = pTSP->GetPathData(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();
|
|
pCurResult->nTotalIndex = vecPathData[i].nIndex;
|
pCurResult->dPositionX = double(vecPathData[i].nPosX) / 1000.0;
|
pCurResult->dPositionY = double(vecPathData[i].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;
|
// }
|
//}
|
}
|
|
//delete pTSP;
|
return (int)vecPathData.size();
|
|
}
|
|
|
}
|
