#include "StdAfx.h"
|
#include "CHPathScheduler/PathScheduler.h"
|
#include "DynamicTSP.h"
|
#include "AnnealingTSP.h"
|
|
|
CPathScheduler::CPathScheduler(void)
|
{
|
Reset();
|
}
|
|
|
CPathScheduler::~CPathScheduler(void)
|
{
|
Reset();
|
}
|
|
void CPathScheduler::Reset()
|
{
|
m_nPointMin = 20;
|
m_nPointMax = 500;
|
|
m_PathSchedulerParam.Reset();
|
|
m_vecPathSchedulerResult.clear();
|
}
|
|
CPathSchedulerResult* CPathScheduler::GetPathSchedulerResult(int nIndex)
|
{
|
if (nIndex<0 || nIndex>=(int)m_vecPathSchedulerResult.size()) return NULL;
|
|
return &m_vecPathSchedulerResult[nIndex];
|
}
|
|
|
// getter
|
int CPathScheduler::GetPathSchedulerResultCount() const
|
{
|
return (int) m_vecPathSchedulerResult.size();
|
}
|
|
double CPathScheduler::GetTotalSchedulerTime(int nIndex) const
|
{
|
if (nIndex<0 || nIndex>=(int)m_vecPathSchedulerResult.size()) return 0.0;
|
|
return m_vecPathSchedulerResult[nIndex].GetTotalSchedulerTime();
|
}
|
|
double CPathScheduler::GetTotalSchedulerDistance(int nIndex) const
|
{
|
if (nIndex<0 || nIndex>=(int)m_vecPathSchedulerResult.size()) return 0.0;
|
|
return m_vecPathSchedulerResult[nIndex].GetTotalSchedulerDistance();
|
}
|
|
CPathSchedulerParm* CPathScheduler::GetPathSchedulerParam()
|
{
|
return &m_PathSchedulerParam;
|
}
|
|
|
int CPathScheduler::GetRangeRectCount() const
|
{
|
return (int)m_vecRangeRect.size();
|
}
|
|
CRect* CPathScheduler::GetRangeRect(int nIndex)
|
{
|
if (nIndex<0 ||nIndex>=GetRangeRectCount()) return NULL;
|
return &m_vecRangeRect[nIndex];
|
}
|
|
const CRect* CPathScheduler::GetRangeRect(int nIndex) const
|
{
|
if (nIndex<0 ||nIndex>=GetRangeRectCount()) return NULL;
|
return &m_vecRangeRect[nIndex];
|
}
|
|
|
// setter
|
void CPathScheduler::SetPointInfo(int nMin, int nMax)
|
{
|
m_nPointMin = nMin;
|
m_nPointMax = nMax;
|
}
|
|
void CPathScheduler::SetPathSchedulerParam(const CPathSchedulerParm& scheduleParam)
|
{
|
m_PathSchedulerParam = scheduleParam;
|
}
|
|
BOOL CPathScheduler::CalculateDistanceSpeedTime(SSchedulerResult& pt1, const SSchedulerResult &pt2)
|
{
|
double dx, dy, sx, sy, accelx, accely, decelx, decely;
|
|
double dtx=0.0, dty=0.0;
|
dx = fabs(pt1.dPositionX-pt2.dPositionX);
|
dy = fabs(pt1.dPositionY-pt2.dPositionY);
|
|
const SMotionInfo *pInfo = NULL;
|
|
switch(m_PathSchedulerParam.GetMoveType())
|
{
|
case Motion_MaxAxisTime: // max pos
|
if (dx==0.0 && dy==0.0)
|
{
|
pt1.nAxisType = 1;
|
pt1.dMoveTime = 0.0;
|
pt1.dMoveSpeed = 0.0;
|
pt1.dMoveDistance = 0.0;
|
pt1.dMoveAccel = 0.0;
|
pt1.dMoveDecel = 0.0;
|
pt1.dMotionDelayTime = 0.0;
|
return TRUE;
|
}
|
|
dtx = dty = 0.0;
|
if ( (pInfo=m_PathSchedulerParam.GetMotionInfoX(dx))!=NULL)
|
{
|
dtx = pInfo->GetDurationTime(m_PathSchedulerParam.GetCurveType(), dx);
|
sx = pInfo->dMoveSpeed;
|
accelx = pInfo->dAccelTime;
|
decelx = pInfo->dDecelTime;
|
}
|
|
if ( (pInfo=m_PathSchedulerParam.GetMotionInfoY(dy))!=NULL)
|
{
|
dty = pInfo->GetDurationTime(m_PathSchedulerParam.GetCurveType(), dy);
|
sy = pInfo->dMoveSpeed;
|
accely = pInfo->dAccelTime;
|
decely = pInfo->dDecelTime;
|
}
|
|
if (dtx > dty)
|
{
|
pt1.nAxisType = 0;
|
pt1.dMoveTime = dtx;
|
pt1.dMoveSpeed = sx;
|
pt1.dMoveDistance = dx;
|
pt1.dMoveAccel = accelx;
|
pt1.dMoveDecel = decelx;
|
}
|
else
|
{
|
pt1.nAxisType = 1;
|
pt1.dMoveTime = dty;
|
pt1.dMoveSpeed = sy;
|
pt1.dMoveDistance = dy;
|
pt1.dMoveAccel = accely;
|
pt1.dMoveDecel = decely;
|
}
|
return TRUE;
|
|
case Motion_XAxisTime: // x pos
|
if (dx==0.0)
|
{
|
pt1.nAxisType = 0;
|
pt1.dMoveTime = 0.0;
|
pt1.dMoveSpeed = 0.0;
|
pt1.dMoveDistance = 0.0;
|
pt1.dMoveAccel = 0.0;
|
pt1.dMoveDecel = 0.0;
|
pt1.dMotionDelayTime = 0.0;
|
return TRUE;
|
}
|
|
if ( (pInfo=m_PathSchedulerParam.GetMotionInfoX(dx))!=NULL)
|
{
|
pt1.nAxisType = 0;
|
pt1.dMoveTime = pInfo->GetDurationTime(m_PathSchedulerParam.GetCurveType(), dx);
|
pt1.dMoveSpeed = pInfo->dMoveSpeed;
|
pt1.dMoveDistance = dx;
|
pt1.dMoveAccel = pInfo->dAccelTime;
|
pt1.dMoveDecel = pInfo->dDecelTime;
|
}
|
return TRUE;
|
|
case Motion_YAxisTime: // y pos
|
if (dy==0.0)
|
{
|
pt1.nAxisType = 1;
|
pt1.dMoveTime = 0.0;
|
pt1.dMoveSpeed = 0.0;
|
pt1.dMoveDistance = 0.0;
|
pt1.dMoveAccel = 0.0;
|
pt1.dMoveDecel = 0.0;
|
pt1.dMotionDelayTime = 0.0;
|
return TRUE;
|
}
|
|
if ( (pInfo=m_PathSchedulerParam.GetMotionInfoY(dy))!=NULL)
|
{
|
pt1.nAxisType = 1;
|
pt1.dMoveTime = pInfo->GetDurationTime(m_PathSchedulerParam.GetCurveType(), dy);
|
pt1.dMoveSpeed = pInfo->dMoveSpeed;
|
pt1.dMoveDistance = dy;
|
pt1.dMoveAccel = pInfo->dAccelTime;
|
pt1.dMoveDecel = pInfo->dDecelTime;
|
}
|
return TRUE;
|
|
|
case Motion_XYAxisTime: // xy distance
|
return FALSE;
|
}
|
|
return FALSE;
|
}
|
|
|
BOOL CPathScheduler::CalculateDistanceSpeedTime(SSchedulerResult& pt1, double dPosX, double dPosY)
|
{
|
double dx, dy, sx, sy, accelx, accely, decelx, decely;
|
|
double dtx=0.0, dty=0.0;
|
|
const SMotionInfo *pInfo = NULL;
|
|
switch(m_PathSchedulerParam.GetMoveType())
|
{
|
case Motion_MaxAxisTime: // max pos
|
dtx = dty = 0.0;
|
dx = fabs(pt1.dPositionX-dPosX);
|
dy = fabs(pt1.dPositionY-dPosY);
|
|
if ( (pInfo=m_PathSchedulerParam.GetMotionInfoX(dx))!=NULL)
|
{
|
dtx = pInfo->GetDurationTime(m_PathSchedulerParam.GetCurveType(), dx);
|
sx = pInfo->dMoveSpeed;
|
accelx = pInfo->dAccelTime;
|
decelx = pInfo->dDecelTime;
|
}
|
|
if ( (pInfo=m_PathSchedulerParam.GetMotionInfoY(dy))!=NULL)
|
{
|
dty = pInfo->GetDurationTime(m_PathSchedulerParam.GetCurveType(), dy);
|
sy = pInfo->dMoveSpeed;
|
accely = pInfo->dAccelTime;
|
decely = pInfo->dDecelTime;
|
}
|
|
if (dtx > dty)
|
{
|
pt1.nAxisType = 0;
|
pt1.dMoveTime = dtx;
|
pt1.dMoveSpeed = sx;
|
pt1.dMoveDistance = dx;
|
pt1.dMoveAccel = accelx;
|
pt1.dMoveDecel = decelx;
|
}
|
else
|
{
|
pt1.nAxisType = 1;
|
pt1.dMoveTime = dty;
|
pt1.dMoveSpeed = sy;
|
pt1.dMoveDistance = dy;
|
pt1.dMoveAccel = accely;
|
pt1.dMoveDecel = decely;
|
}
|
return TRUE;
|
|
case Motion_XAxisTime: // x pos
|
dx = fabs(pt1.dPositionX-dPosX);
|
if ( (pInfo=m_PathSchedulerParam.GetMotionInfoX(dx))!=NULL)
|
{
|
pt1.nAxisType = 0;
|
pt1.dMoveTime = pInfo->GetDurationTime(m_PathSchedulerParam.GetCurveType(), dx);
|
pt1.dMoveSpeed = pInfo->dMoveSpeed;
|
pt1.dMoveDistance = dx;
|
pt1.dMoveAccel = pInfo->dAccelTime;
|
pt1.dMoveDecel = pInfo->dDecelTime;
|
}
|
|
return TRUE;
|
|
case Motion_YAxisTime: // y pos
|
dy = fabs(pt1.dPositionY-dPosY);
|
if ( (pInfo=m_PathSchedulerParam.GetMotionInfoY(dy))!=NULL)
|
{
|
pt1.nAxisType = 1;
|
pt1.dMoveTime = pInfo->GetDurationTime(m_PathSchedulerParam.GetCurveType(), dy);
|
pt1.dMoveSpeed = pInfo->dMoveSpeed;
|
pt1.dMoveDistance = dy;
|
pt1.dMoveAccel = pInfo->dAccelTime;
|
pt1.dMoveDecel = pInfo->dDecelTime;
|
}
|
|
return TRUE;
|
|
case Motion_XYAxisTime: // xy distance
|
return FALSE;
|
}
|
|
|
return FALSE;
|
}
|
|
|
int CPathScheduler::CalculatePath(const SPathData& startPath, const VectorPathData& vecTotalPathData, const CRect& rtRange, CPathSchedulerResult& scheduleResult)
|
{
|
scheduleResult.Reset();
|
|
// filtering path data
|
VectorPathData vecPathData;
|
for (constVectorPathDataIt it=vecTotalPathData.begin(); it!=vecTotalPathData.end(); it++)
|
{
|
if ( (*it == startPath) && rtRange.PtInRect(CPoint(it->nPosX,it->nPosY)) )
|
{
|
vecPathData.push_back(*it);
|
}
|
}
|
|
// check path count
|
int nPathCount = (int) vecPathData.size();
|
if (nPathCount<1) 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;
|
|
const SPathData *pPathData = &vecPathData[i];
|
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 (int) vecPathData.size();
|
}
|
|
int CPathScheduler::CalculatePath(const SPathData& startPath, const VectorPathData& vecTotalPathData, CPathSchedulerResult& scheduleResult)
|
{
|
scheduleResult.Reset();
|
|
// filtering path data
|
VectorPathData vecPathData;
|
for (constVectorPathDataIt it=vecTotalPathData.begin(); it!=vecTotalPathData.end(); it++)
|
{
|
if (*it == startPath )
|
{
|
vecPathData.push_back(*it);
|
}
|
}
|
|
// check path count
|
int nPathCount = (int) vecPathData.size();
|
if (nPathCount<1) 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;
|
|
const SPathData *pPathData = &vecPathData[i];
|
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 (int) vecPathData.size();
|
}
|
|
int CPathScheduler::CalculatePath( const SPathData& startPath, const VectorPathData& vecFirstPathData, const VectorPathData& vecSecondPathData, CPathSchedulerResult& scheduleResult )
|
{
|
return 1;
|
}
|
|
int CPathScheduler::PathScheduling( const SPathData* pPathData, int nPathDataCount, const SPathData* pStartPath, int nStartPathCount )
|
{
|
VectorPathData vecPathData, vecStartPath;
|
vecPathData.reserve(nPathDataCount);
|
vecStartPath.reserve(nStartPathCount);
|
|
for (int nIdx=0; nIdx<nPathDataCount; nIdx++)
|
{
|
vecPathData.push_back(pPathData[nIdx]);
|
}
|
|
for (int nIdx=0; nIdx<nStartPathCount; nIdx++)
|
{
|
vecStartPath.push_back(pStartPath[nIdx]);
|
}
|
|
return PathScheduling(vecPathData, vecStartPath);
|
}
|
|
int CPathScheduler::PathScheduling( const SPathData* pPathData, int nPathDataCount, const SPathData& startPath )
|
{
|
VectorPathData vecPathData;
|
vecPathData.reserve(nPathDataCount);
|
|
for (int nIdx=0; nIdx<nPathDataCount; nIdx++)
|
{
|
vecPathData.push_back(pPathData[nIdx]);
|
}
|
|
return PathScheduling(vecPathData, startPath);
|
}
|
|
|
int CPathScheduler::PathScheduling(const VectorPathData& vecPathData, const VectorPathData& vecStartPath)
|
{
|
m_vecPathSchedulerResult.clear();
|
|
for (constVectorPathDataIt it=vecStartPath.begin(); it!=vecStartPath.end(); it++)
|
{
|
CPathSchedulerResult scheduleResult(it->nIndex);
|
|
CalculatePath(*it, vecPathData, scheduleResult);
|
|
m_vecPathSchedulerResult.push_back(scheduleResult);
|
}
|
|
return (int) m_vecPathSchedulerResult.size();
|
}
|
|
int CPathScheduler::PathScheduling(const VectorPathData& vecPathData, const SPathData& startPath)
|
{
|
m_vecPathSchedulerResult.clear();
|
|
CPathSchedulerResult scheduleResult(startPath.nIndex);
|
|
CalculatePath(startPath, vecPathData, scheduleResult);
|
|
m_vecPathSchedulerResult.push_back(scheduleResult);
|
|
return (int) m_vecPathSchedulerResult.size();
|
}
|
|
|
BOOL CPathScheduler::LoadPathSchedulerParam(const CString& strFilename)
|
{
|
return CPathSchedulerParm::LoadParm(strFilename, m_PathSchedulerParam);
|
}
|
|
BOOL CPathScheduler::SavePathSchedulerParam(const CString& strFilename)
|
{
|
return CPathSchedulerParm::SaveParm(strFilename, m_PathSchedulerParam);
|
}
|
|
int CPathScheduler::PathScheduling( const VectorPathData& vecPathData, const VectorPathData& vecStackPathData, const VectorPathData& vecStartPath )
|
{
|
m_vecPathSchedulerResult.clear();
|
|
for (constVectorPathDataIt it=vecStartPath.begin(); it!=vecStartPath.end(); it++)
|
{
|
CPathSchedulerResult scheduleResult(it->nIndex);
|
|
CalculatePath(*it, vecPathData, vecStackPathData, scheduleResult);
|
|
m_vecPathSchedulerResult.push_back(scheduleResult);
|
}
|
|
return (int) m_vecPathSchedulerResult.size();
|
}
|
