#include "StdAfx.h"
|
#include "PathScheduler_Dual_Active.h"
|
#include <algorithm>
|
|
|
CPathScheduler_Dual_Active::CPathScheduler_Dual_Active(void)
|
{
|
}
|
|
|
CPathScheduler_Dual_Active::~CPathScheduler_Dual_Active(void)
|
{
|
}
|
|
inline bool ComparePosX(SPathData& a, SPathData& b)
|
{
|
return (a.nPosX < b.nPosX);
|
}
|
inline bool ComparePosDescend(SPathData& a, SPathData& b)
|
{
|
return (a.nPosX > b.nPosX);
|
}
|
|
int GetDiffCount(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);
|
}
|
|
int CPathScheduler_Dual_Active::PathScheduling(const VectorPathData& vecPathData, const VectorPathData& vecStartPath )
|
{
|
if (m_PathSchedulerParam.GetSelectModuleType() > 0)
|
{
|
|
if (m_PathSchedulerParam.GetUseShortPath() == FALSE)
|
{
|
if (vecStartPath.size() < 1) return 0;
|
|
m_vecPathSchedulerResult.clear();
|
m_vecRangeRect.clear();
|
|
VectorPathData vecSortData = vecPathData;
|
|
int nCenterPosX = 0;//= vecSortData[nSize / 2].nPosX;
|
if (m_PathSchedulerParam.GetSelectModuleType() == 1)//left
|
{
|
// sort
|
std::sort(vecSortData.begin(), vecSortData.end(), ComparePosX);
|
nCenterPosX = m_PathSchedulerParam.GetGlassSizeX() - m_PathSchedulerParam.GetCollisionDistX();
|
}
|
else if (m_PathSchedulerParam.GetSelectModuleType() == 2) //Right
|
{
|
// sort
|
std::sort(vecSortData.begin(), vecSortData.end(), ComparePosDescend);
|
nCenterPosX = m_PathSchedulerParam.GetCollisionDistX();
|
}
|
else
|
{
|
return 0;
|
}
|
|
|
int nSize = (int)vecSortData.size();
|
int nCollisionDistX = m_PathSchedulerParam.GetCollisionDistX();
|
|
|
// init
|
CRect rtLeftRange1(0, 0, m_PathSchedulerParam.GetGlassSizeX(), m_PathSchedulerParam.GetGlassSizeY());
|
CRect rtLeftRange2(0, 0, m_PathSchedulerParam.GetGlassSizeX(), m_PathSchedulerParam.GetGlassSizeY());
|
CRect rtRightRange1(0, 0, m_PathSchedulerParam.GetGlassSizeX(), m_PathSchedulerParam.GetGlassSizeY());
|
CRect rtRightRange2(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
|
nLeftCollisionPosX = nCenterPosX;
|
nRightCollisionPosX = nCenterPosX;
|
|
//±×³É Range1 ¿¡¼ ´Ù ÇÒ°Å´Ï ´Ù¹ÚÀÚ
|
|
rtLeftRange1.left = 0;
|
rtLeftRange1.right = nCenterPosX;
|
|
rtLeftRange2.left = nLeftCollisionPosX;
|
rtLeftRange2.right = nCenterPosX;
|
|
|
rtRightRange1.left = nCenterPosX;
|
rtRightRange1.right = m_PathSchedulerParam.GetGlassSizeX();
|
|
rtRightRange2.left = nRightCollisionPosX;
|
rtRightRange2.right = m_PathSchedulerParam.GetGlassSizeX();
|
/*
|
nLeftCollisionPosX = nCenterPosX - nCollisionDistX;
|
nRightCollisionPosX = nCenterPosX + nCollisionDistX;
|
|
rtLeftRange1.left = 0;
|
rtLeftRange1.right = nLeftCollisionPosX;
|
|
rtLeftRange2.left = nLeftCollisionPosX;
|
rtLeftRange2.right = nCenterPosX;
|
|
rtRightRange1.left = nCenterPosX;
|
rtRightRange1.right = nRightCollisionPosX;
|
|
rtRightRange2.left = nRightCollisionPosX;
|
rtRightRange2.right = m_PathSchedulerParam.GetGlassSizeX();*/
|
break;
|
|
case SchedulerDir_LeftBottom: // left bottom
|
nLeftCollisionPosX = nCenterPosX;
|
nRightCollisionPosX = nCenterPosX;
|
|
//±×³É Range1 ¿¡¼ ´Ù ÇÒ°Å´Ï ´Ù¹ÚÀÚ
|
|
rtLeftRange1.left = 0;
|
rtLeftRange1.right = nCenterPosX;
|
|
rtLeftRange2.left = nLeftCollisionPosX;
|
rtLeftRange2.right = nCenterPosX;
|
|
|
rtRightRange1.left = nCenterPosX;
|
rtRightRange1.right = m_PathSchedulerParam.GetGlassSizeX();
|
|
rtRightRange2.left = nRightCollisionPosX;
|
rtRightRange2.right = m_PathSchedulerParam.GetGlassSizeX();
|
/*
|
nLeftCollisionPosX = nCenterPosX - nCollisionDistX;
|
nRightCollisionPosX = nCenterPosX + nCollisionDistX;
|
|
rtLeftRange1.left = 0;
|
rtLeftRange1.right = nLeftCollisionPosX;
|
|
rtLeftRange2.left = nLeftCollisionPosX;
|
rtLeftRange2.right = nCenterPosX;
|
|
rtRightRange1.left = nCenterPosX;
|
rtRightRange1.right = nRightCollisionPosX;
|
|
rtRightRange2.left = nRightCollisionPosX;
|
rtRightRange2.right = m_PathSchedulerParam.GetGlassSizeX();*/
|
break;
|
|
case SchedulerDir_RightTop: // right top
|
nLeftCollisionPosX = nCenterPosX;
|
nRightCollisionPosX = nCenterPosX;
|
|
rtLeftRange1.left = m_PathSchedulerParam.GetGlassSizeX();
|
rtLeftRange1.right = nLeftCollisionPosX;
|
|
rtLeftRange2.left = nLeftCollisionPosX;
|
rtLeftRange2.right = nCenterPosX;
|
|
|
//±×³É Range1 ¿¡¼ ´Ù ÇÒ°Å´Ï ´Ù¹ÚÀÚ
|
rtRightRange1.left = nCenterPosX;
|
rtRightRange1.right = 0;
|
|
rtRightRange2.left = nRightCollisionPosX;
|
rtRightRange2.right = 0;
|
|
|
/*
|
nLeftCollisionPosX = nCenterPosX + nCollisionDistX;
|
nRightCollisionPosX = nCenterPosX - nCollisionDistX;
|
|
rtLeftRange1.left = m_PathSchedulerParam.GetGlassSizeX();
|
rtLeftRange1.right = nLeftCollisionPosX;
|
|
rtLeftRange2.left = nLeftCollisionPosX;
|
rtLeftRange2.right = nCenterPosX;
|
|
rtRightRange1.left = nCenterPosX;
|
rtRightRange1.right = nRightCollisionPosX;
|
|
rtRightRange2.left = nRightCollisionPosX;
|
rtRightRange2.right = 0;*/
|
break;
|
case SchedulerDir_RightBottom: // right bottom
|
nLeftCollisionPosX = nCenterPosX;
|
nRightCollisionPosX = nCenterPosX;
|
|
rtLeftRange1.left = m_PathSchedulerParam.GetGlassSizeX();
|
rtLeftRange1.right = nLeftCollisionPosX;
|
|
rtLeftRange2.left = nLeftCollisionPosX;
|
rtLeftRange2.right = nCenterPosX;
|
|
|
//±×³É Range1 ¿¡¼ ´Ù ÇÒ°Å´Ï ´Ù¹ÚÀÚ
|
rtRightRange1.left = nCenterPosX;
|
rtRightRange1.right = 0;
|
|
rtRightRange2.left = nRightCollisionPosX;
|
rtRightRange2.right = 0;
|
|
|
/*
|
nLeftCollisionPosX = nCenterPosX + nCollisionDistX;
|
nRightCollisionPosX = nCenterPosX - nCollisionDistX;
|
|
rtLeftRange1.left = m_PathSchedulerParam.GetGlassSizeX();
|
rtLeftRange1.right = nLeftCollisionPosX;
|
|
rtLeftRange2.left = nLeftCollisionPosX;
|
rtLeftRange2.right = nCenterPosX;
|
|
rtRightRange1.left = nCenterPosX;
|
rtRightRange1.right = nRightCollisionPosX;
|
|
rtRightRange2.left = nRightCollisionPosX;
|
rtRightRange2.right = 0;*/
|
break;
|
}
|
|
// normalize
|
rtLeftRange1.NormalizeRect();
|
rtLeftRange2.NormalizeRect();
|
rtRightRange1.NormalizeRect();
|
rtRightRange2.NormalizeRect();
|
|
// center
|
if (m_PathSchedulerParam.GetCenterCoord())
|
{
|
rtLeftRange1.MoveToXY(-m_PathSchedulerParam.GetGlassSizeX() / 2, -m_PathSchedulerParam.GetGlassSizeY() / 2);
|
rtLeftRange2.MoveToXY(-m_PathSchedulerParam.GetGlassSizeX() / 2, -m_PathSchedulerParam.GetGlassSizeY() / 2);
|
rtRightRange1.MoveToXY(-m_PathSchedulerParam.GetGlassSizeX() / 2, -m_PathSchedulerParam.GetGlassSizeY() / 2);
|
rtRightRange2.MoveToXY(-m_PathSchedulerParam.GetGlassSizeX() / 2, -m_PathSchedulerParam.GetGlassSizeY() / 2);
|
}
|
|
// add range rect
|
m_vecRangeRect.push_back(rtLeftRange1);
|
m_vecRangeRect.push_back(rtLeftRange2);
|
m_vecRangeRect.push_back(rtRightRange1);
|
m_vecRangeRect.push_back(rtRightRange2);
|
|
int nCollisionResultIdx = -1;
|
double dCollisionResultTime = 0;
|
|
CPathSchedulerResult scheduleResultLeft1(vecStartPath[0].nIndex);
|
CPathSchedulerResult scheduleResultLeft2(vecStartPath[0].nIndex);
|
CPathSchedulerResult scheduleResultRight1(vecStartPath[1].nIndex);
|
CPathSchedulerResult scheduleResultRight2(vecStartPath[1].nIndex);
|
|
|
// 1-1. right gantry result
|
SPathData ptRightStartPath = vecStartPath[1];
|
CalculatePath(ptRightStartPath, vecPathData, rtRightRange1, scheduleResultRight1);
|
|
// 1-2. get right result
|
nCollisionResultIdx = scheduleResultRight1.GetPathSchedulerResultCount() - 1;
|
dCollisionResultTime = scheduleResultRight1.GetTotalSchedulerTime();
|
scheduleResultRight1.SetCollisionResultIndex(nCollisionResultIdx);
|
scheduleResultRight1.SetCollisionResultTime(dCollisionResultTime);
|
|
const SSchedulerResult* pLastPoint = scheduleResultRight1.GetPathSchedulerResult(nCollisionResultIdx);
|
if (pLastPoint)
|
{
|
ptRightStartPath.nPosX = (int)(pLastPoint->dPositionX*1000.0);
|
ptRightStartPath.nPosY = (int)(pLastPoint->dPositionY*1000.0);
|
}
|
|
// 1-3. right gantry result2
|
//CalculatePath(ptRightStartPath, vecSortData, rtRightRange2, scheduleResultRight2);
|
|
// 1-4. merge right1 and right2
|
//scheduleResultRight1.AddTailScheduleResult(scheduleResultRight2);
|
|
// 2-1.left gantry result1
|
SPathData ptLeftStartPath = vecStartPath[0];
|
CalculatePath(ptLeftStartPath, vecPathData, rtLeftRange1, scheduleResultLeft1);
|
|
// 2-2. get left result1
|
nCollisionResultIdx = scheduleResultLeft1.GetPathSchedulerResultCount() - 1;
|
dCollisionResultTime = scheduleResultLeft1.GetTotalSchedulerTime();
|
scheduleResultLeft1.SetCollisionResultIndex(nCollisionResultIdx);
|
scheduleResultLeft1.SetCollisionResultTime(dCollisionResultTime);
|
pLastPoint = scheduleResultLeft1.GetPathSchedulerResult(nCollisionResultIdx);
|
if (pLastPoint)
|
{
|
ptLeftStartPath.nPosX = (int)(pLastPoint->dPositionX*1000.0);
|
ptLeftStartPath.nPosY = (int)(pLastPoint->dPositionY*1000.0);
|
}
|
|
// 2-3. left gantry result2
|
//CalculatePath(ptLeftStartPath, vecSortData, rtLeftRange2, scheduleResultLeft2);
|
|
// 2-4. merge left1 and left2
|
// scheduleResultLeft1.AddTailScheduleResult(scheduleResultLeft2);
|
|
// set Collision Position
|
scheduleResultLeft1.SetCollisionPositionX(nLeftCollisionPosX);
|
scheduleResultRight1.SetCollisionPositionX(nRightCollisionPosX);
|
|
if (m_PathSchedulerParam.GetSelectModuleType() == 1)
|
{
|
// add schedule result
|
m_vecPathSchedulerResult.push_back(scheduleResultLeft1);
|
}
|
else
|
{
|
|
|
m_vecPathSchedulerResult.push_back(scheduleResultRight1);
|
|
}
|
|
return (int)m_vecPathSchedulerResult.size();
|
}
|
else
|
{
|
if (vecStartPath.size() < 1) return 0;
|
|
m_vecPathSchedulerResult.clear();
|
m_vecRangeRect.clear();
|
|
VectorPathData vecSortData = vecPathData;
|
|
int nCenterPosX = 0;//= vecSortData[nSize / 2].nPosX;
|
if (m_PathSchedulerParam.GetSelectModuleType() == 1)//left
|
{
|
// sort
|
std::sort(vecSortData.begin(), vecSortData.end(), ComparePosX);
|
nCenterPosX = m_PathSchedulerParam.GetGlassSizeX() - m_PathSchedulerParam.GetCollisionDistX();
|
}
|
else if (m_PathSchedulerParam.GetSelectModuleType() == 2) //Right
|
{
|
// sort
|
std::sort(vecSortData.begin(), vecSortData.end(), ComparePosDescend);
|
nCenterPosX = m_PathSchedulerParam.GetCollisionDistX();
|
}
|
else
|
{
|
return 0;
|
}
|
|
|
int nSize = (int)vecSortData.size();
|
int nCollisionDistX = m_PathSchedulerParam.GetCollisionDistX();
|
|
|
// init
|
CRect rtLeftRange1(0, 0, m_PathSchedulerParam.GetGlassSizeX(), m_PathSchedulerParam.GetGlassSizeY());
|
CRect rtLeftRange2(0, 0, m_PathSchedulerParam.GetGlassSizeX(), m_PathSchedulerParam.GetGlassSizeY());
|
CRect rtRightRange1(0, 0, m_PathSchedulerParam.GetGlassSizeX(), m_PathSchedulerParam.GetGlassSizeY());
|
CRect rtRightRange2(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
|
nLeftCollisionPosX = nCenterPosX;
|
nRightCollisionPosX = nCenterPosX;
|
|
//±×³É Range1 ¿¡¼ ´Ù ÇÒ°Å´Ï ´Ù¹ÚÀÚ
|
|
rtLeftRange1.left = 0;
|
rtLeftRange1.right = nCenterPosX;
|
|
rtLeftRange2.left = nLeftCollisionPosX;
|
rtLeftRange2.right = nCenterPosX;
|
|
|
rtRightRange1.left = nCenterPosX;
|
rtRightRange1.right = m_PathSchedulerParam.GetGlassSizeX();
|
|
rtRightRange2.left = nRightCollisionPosX;
|
rtRightRange2.right = m_PathSchedulerParam.GetGlassSizeX();
|
/*
|
nLeftCollisionPosX = nCenterPosX - nCollisionDistX;
|
nRightCollisionPosX = nCenterPosX + nCollisionDistX;
|
|
rtLeftRange1.left = 0;
|
rtLeftRange1.right = nLeftCollisionPosX;
|
|
rtLeftRange2.left = nLeftCollisionPosX;
|
rtLeftRange2.right = nCenterPosX;
|
|
rtRightRange1.left = nCenterPosX;
|
rtRightRange1.right = nRightCollisionPosX;
|
|
rtRightRange2.left = nRightCollisionPosX;
|
rtRightRange2.right = m_PathSchedulerParam.GetGlassSizeX();*/
|
break;
|
|
case SchedulerDir_LeftBottom: // left bottom
|
nLeftCollisionPosX = nCenterPosX;
|
nRightCollisionPosX = nCenterPosX;
|
|
//±×³É Range1 ¿¡¼ ´Ù ÇÒ°Å´Ï ´Ù¹ÚÀÚ
|
|
rtLeftRange1.left = 0;
|
rtLeftRange1.right = nCenterPosX;
|
|
rtLeftRange2.left = nLeftCollisionPosX;
|
rtLeftRange2.right = nCenterPosX;
|
|
|
rtRightRange1.left = nCenterPosX;
|
rtRightRange1.right = m_PathSchedulerParam.GetGlassSizeX();
|
|
rtRightRange2.left = nRightCollisionPosX;
|
rtRightRange2.right = m_PathSchedulerParam.GetGlassSizeX();
|
/*
|
nLeftCollisionPosX = nCenterPosX - nCollisionDistX;
|
nRightCollisionPosX = nCenterPosX + nCollisionDistX;
|
|
rtLeftRange1.left = 0;
|
rtLeftRange1.right = nLeftCollisionPosX;
|
|
rtLeftRange2.left = nLeftCollisionPosX;
|
rtLeftRange2.right = nCenterPosX;
|
|
rtRightRange1.left = nCenterPosX;
|
rtRightRange1.right = nRightCollisionPosX;
|
|
rtRightRange2.left = nRightCollisionPosX;
|
rtRightRange2.right = m_PathSchedulerParam.GetGlassSizeX();*/
|
break;
|
|
case SchedulerDir_RightTop: // right top
|
nLeftCollisionPosX = nCenterPosX;
|
nRightCollisionPosX = nCenterPosX;
|
|
rtLeftRange1.left = m_PathSchedulerParam.GetGlassSizeX();
|
rtLeftRange1.right = nLeftCollisionPosX;
|
|
rtLeftRange2.left = nLeftCollisionPosX;
|
rtLeftRange2.right = nCenterPosX;
|
|
|
//±×³É Range1 ¿¡¼ ´Ù ÇÒ°Å´Ï ´Ù¹ÚÀÚ
|
rtRightRange1.left = nCenterPosX;
|
rtRightRange1.right = 0;
|
|
rtRightRange2.left = nRightCollisionPosX;
|
rtRightRange2.right = 0;
|
|
|
/*
|
nLeftCollisionPosX = nCenterPosX + nCollisionDistX;
|
nRightCollisionPosX = nCenterPosX - nCollisionDistX;
|
|
rtLeftRange1.left = m_PathSchedulerParam.GetGlassSizeX();
|
rtLeftRange1.right = nLeftCollisionPosX;
|
|
rtLeftRange2.left = nLeftCollisionPosX;
|
rtLeftRange2.right = nCenterPosX;
|
|
rtRightRange1.left = nCenterPosX;
|
rtRightRange1.right = nRightCollisionPosX;
|
|
rtRightRange2.left = nRightCollisionPosX;
|
rtRightRange2.right = 0;*/
|
break;
|
case SchedulerDir_RightBottom: // right bottom
|
nLeftCollisionPosX = nCenterPosX;
|
nRightCollisionPosX = nCenterPosX;
|
|
rtLeftRange1.left = m_PathSchedulerParam.GetGlassSizeX();
|
rtLeftRange1.right = nLeftCollisionPosX;
|
|
rtLeftRange2.left = nLeftCollisionPosX;
|
rtLeftRange2.right = nCenterPosX;
|
|
|
//±×³É Range1 ¿¡¼ ´Ù ÇÒ°Å´Ï ´Ù¹ÚÀÚ
|
rtRightRange1.left = nCenterPosX;
|
rtRightRange1.right = 0;
|
|
rtRightRange2.left = nRightCollisionPosX;
|
rtRightRange2.right = 0;
|
|
|
/*
|
nLeftCollisionPosX = nCenterPosX + nCollisionDistX;
|
nRightCollisionPosX = nCenterPosX - nCollisionDistX;
|
|
rtLeftRange1.left = m_PathSchedulerParam.GetGlassSizeX();
|
rtLeftRange1.right = nLeftCollisionPosX;
|
|
rtLeftRange2.left = nLeftCollisionPosX;
|
rtLeftRange2.right = nCenterPosX;
|
|
rtRightRange1.left = nCenterPosX;
|
rtRightRange1.right = nRightCollisionPosX;
|
|
rtRightRange2.left = nRightCollisionPosX;
|
rtRightRange2.right = 0;*/
|
break;
|
}
|
|
// normalize
|
rtLeftRange1.NormalizeRect();
|
rtLeftRange2.NormalizeRect();
|
rtRightRange1.NormalizeRect();
|
rtRightRange2.NormalizeRect();
|
|
// center
|
if (m_PathSchedulerParam.GetCenterCoord())
|
{
|
rtLeftRange1.MoveToXY(-m_PathSchedulerParam.GetGlassSizeX() / 2, -m_PathSchedulerParam.GetGlassSizeY() / 2);
|
rtLeftRange2.MoveToXY(-m_PathSchedulerParam.GetGlassSizeX() / 2, -m_PathSchedulerParam.GetGlassSizeY() / 2);
|
rtRightRange1.MoveToXY(-m_PathSchedulerParam.GetGlassSizeX() / 2, -m_PathSchedulerParam.GetGlassSizeY() / 2);
|
rtRightRange2.MoveToXY(-m_PathSchedulerParam.GetGlassSizeX() / 2, -m_PathSchedulerParam.GetGlassSizeY() / 2);
|
}
|
|
// add range rect
|
m_vecRangeRect.push_back(rtLeftRange1);
|
m_vecRangeRect.push_back(rtLeftRange2);
|
m_vecRangeRect.push_back(rtRightRange1);
|
m_vecRangeRect.push_back(rtRightRange2);
|
|
int nCollisionResultIdx = -1;
|
double dCollisionResultTime = 0;
|
|
CPathSchedulerResult scheduleResultLeft1(vecStartPath[0].nIndex);
|
CPathSchedulerResult scheduleResultLeft2(vecStartPath[0].nIndex);
|
CPathSchedulerResult scheduleResultRight1(vecStartPath[1].nIndex);
|
CPathSchedulerResult scheduleResultRight2(vecStartPath[1].nIndex);
|
|
|
// 1-1. right gantry result
|
SPathData ptRightStartPath = vecStartPath[1];
|
CalculatePath(ptRightStartPath, vecSortData, rtRightRange1, scheduleResultRight1);
|
|
// 1-2. get right result
|
nCollisionResultIdx = scheduleResultRight1.GetPathSchedulerResultCount() - 1;
|
dCollisionResultTime = scheduleResultRight1.GetTotalSchedulerTime();
|
scheduleResultRight1.SetCollisionResultIndex(nCollisionResultIdx);
|
scheduleResultRight1.SetCollisionResultTime(dCollisionResultTime);
|
|
const SSchedulerResult* pLastPoint = scheduleResultRight1.GetPathSchedulerResult(nCollisionResultIdx);
|
if (pLastPoint)
|
{
|
ptRightStartPath.nPosX = (int)(pLastPoint->dPositionX*1000.0);
|
ptRightStartPath.nPosY = (int)(pLastPoint->dPositionY*1000.0);
|
}
|
|
// 1-3. right gantry result2
|
//CalculatePath(ptRightStartPath, vecSortData, rtRightRange2, scheduleResultRight2);
|
|
// 1-4. merge right1 and right2
|
//scheduleResultRight1.AddTailScheduleResult(scheduleResultRight2);
|
|
// 2-1.left gantry result1
|
SPathData ptLeftStartPath = vecStartPath[0];
|
CalculatePath(ptLeftStartPath, vecSortData, rtLeftRange1, scheduleResultLeft1);
|
|
// 2-2. get left result1
|
nCollisionResultIdx = scheduleResultLeft1.GetPathSchedulerResultCount() - 1;
|
dCollisionResultTime = scheduleResultLeft1.GetTotalSchedulerTime();
|
scheduleResultLeft1.SetCollisionResultIndex(nCollisionResultIdx);
|
scheduleResultLeft1.SetCollisionResultTime(dCollisionResultTime);
|
pLastPoint = scheduleResultLeft1.GetPathSchedulerResult(nCollisionResultIdx);
|
if (pLastPoint)
|
{
|
ptLeftStartPath.nPosX = (int)(pLastPoint->dPositionX*1000.0);
|
ptLeftStartPath.nPosY = (int)(pLastPoint->dPositionY*1000.0);
|
}
|
|
// 2-3. left gantry result2
|
//CalculatePath(ptLeftStartPath, vecSortData, rtLeftRange2, scheduleResultLeft2);
|
|
// 2-4. merge left1 and left2
|
// scheduleResultLeft1.AddTailScheduleResult(scheduleResultLeft2);
|
|
// set Collision Position
|
scheduleResultLeft1.SetCollisionPositionX(nLeftCollisionPosX);
|
scheduleResultRight1.SetCollisionPositionX(nRightCollisionPosX);
|
|
if (m_PathSchedulerParam.GetSelectModuleType() == 1)
|
{
|
// add schedule result
|
m_vecPathSchedulerResult.push_back(scheduleResultLeft1);
|
}
|
else
|
{
|
|
|
m_vecPathSchedulerResult.push_back(scheduleResultRight1);
|
|
}
|
|
return (int)m_vecPathSchedulerResult.size();
|
}
|
}
|
else
|
{
|
|
if (vecStartPath.size()<1) return 0;
|
|
m_vecPathSchedulerResult.clear();
|
m_vecRangeRect.clear();
|
|
VectorPathData vecSortData = vecPathData;
|
|
// sort
|
std::sort(vecSortData.begin(), vecSortData.end(), ComparePosX);
|
int nSize = (int)vecSortData.size();
|
int nCenterPosX = vecSortData[nSize/2].nPosX;
|
int nCollisionDistX = m_PathSchedulerParam.GetCollisionDistX();
|
|
|
|
// À§Ä¡°¡ ³Ê¹« À۰ųª Ŭ¶§?
|
if ( nCenterPosX<(nCollisionDistX*2) || nCenterPosX>=(m_PathSchedulerParam.GetGlassSizeX()-(nCollisionDistX*2)) )
|
{
|
int nLeftPos = (nCollisionDistX*2);
|
int nRightPos = m_PathSchedulerParam.GetGlassSizeX()-(nCollisionDistX*2);
|
|
// Á¿ìÀÇ Å©±â Â÷ÀÌ°¡ ÀÛÀº À§Ä¡·Î ÇÒ´çÇÏÀÚ!
|
nCenterPosX = (GetDiffCount(vecPathData, nLeftPos) < GetDiffCount(vecPathData, nRightPos)) ? nLeftPos: nRightPos;
|
}
|
|
// init
|
CRect rtLeftRange1(0, 0, m_PathSchedulerParam.GetGlassSizeX(), m_PathSchedulerParam.GetGlassSizeY());
|
CRect rtLeftRange2(0, 0, m_PathSchedulerParam.GetGlassSizeX(), m_PathSchedulerParam.GetGlassSizeY());
|
CRect rtRightRange1(0, 0, m_PathSchedulerParam.GetGlassSizeX(), m_PathSchedulerParam.GetGlassSizeY());
|
CRect rtRightRange2(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
|
nLeftCollisionPosX = nCenterPosX - nCollisionDistX;
|
nRightCollisionPosX = nCenterPosX + nCollisionDistX;
|
|
rtLeftRange1.left = 0;
|
rtLeftRange1.right = nLeftCollisionPosX;
|
|
rtLeftRange2.left = nLeftCollisionPosX;
|
rtLeftRange2.right = nCenterPosX;
|
|
rtRightRange1.left = nCenterPosX;
|
rtRightRange1.right = nRightCollisionPosX;
|
|
rtRightRange2.left = nRightCollisionPosX;
|
rtRightRange2.right = m_PathSchedulerParam.GetGlassSizeX();
|
break;
|
|
case SchedulerDir_RightTop: // right top
|
case SchedulerDir_RightBottom: // right bottom
|
nLeftCollisionPosX = nCenterPosX + nCollisionDistX;
|
nRightCollisionPosX = nCenterPosX - nCollisionDistX;
|
|
rtLeftRange1.left = m_PathSchedulerParam.GetGlassSizeX();
|
rtLeftRange1.right = nLeftCollisionPosX;
|
|
rtLeftRange2.left = nLeftCollisionPosX;
|
rtLeftRange2.right = nCenterPosX;
|
|
rtRightRange1.left = nCenterPosX;
|
rtRightRange1.right = nRightCollisionPosX;
|
|
rtRightRange2.left = nRightCollisionPosX;
|
rtRightRange2.right = 0;
|
break;
|
}
|
|
// normalize
|
rtLeftRange1.NormalizeRect();
|
rtLeftRange2.NormalizeRect();
|
rtRightRange1.NormalizeRect();
|
rtRightRange2.NormalizeRect();
|
|
// center
|
if (m_PathSchedulerParam.GetCenterCoord())
|
{
|
rtLeftRange1.MoveToXY(-m_PathSchedulerParam.GetGlassSizeX()/2, -m_PathSchedulerParam.GetGlassSizeY()/2);
|
rtLeftRange2.MoveToXY(-m_PathSchedulerParam.GetGlassSizeX()/2, -m_PathSchedulerParam.GetGlassSizeY()/2);
|
rtRightRange1.MoveToXY(-m_PathSchedulerParam.GetGlassSizeX()/2, -m_PathSchedulerParam.GetGlassSizeY()/2);
|
rtRightRange2.MoveToXY(-m_PathSchedulerParam.GetGlassSizeX()/2, -m_PathSchedulerParam.GetGlassSizeY()/2);
|
}
|
|
// add range rect
|
m_vecRangeRect.push_back(rtLeftRange1);
|
m_vecRangeRect.push_back(rtLeftRange2);
|
m_vecRangeRect.push_back(rtRightRange1);
|
m_vecRangeRect.push_back(rtRightRange2);
|
|
int nCollisionResultIdx = -1;
|
double dCollisionResultTime = 0;
|
|
CPathSchedulerResult scheduleResultLeft1(vecStartPath[0].nIndex);
|
CPathSchedulerResult scheduleResultLeft2(vecStartPath[0].nIndex);
|
CPathSchedulerResult scheduleResultRight1(vecStartPath[1].nIndex);
|
CPathSchedulerResult scheduleResultRight2(vecStartPath[1].nIndex);
|
|
// 1-1. right gantry result
|
SPathData ptRightStartPath = vecStartPath[1];
|
CalculatePath(ptRightStartPath, vecPathData, rtRightRange1, scheduleResultRight1);
|
|
// 1-2. get right result
|
nCollisionResultIdx = scheduleResultRight1.GetPathSchedulerResultCount()-1;
|
dCollisionResultTime = scheduleResultRight1.GetTotalSchedulerTime();
|
scheduleResultRight1.SetCollisionResultIndex(nCollisionResultIdx);
|
scheduleResultRight1.SetCollisionResultTime(dCollisionResultTime);
|
|
const SSchedulerResult* pLastPoint = scheduleResultRight1.GetPathSchedulerResult(nCollisionResultIdx);
|
if (pLastPoint)
|
{
|
ptRightStartPath.nPosX = (int)(pLastPoint->dPositionX*1000.0);
|
ptRightStartPath.nPosY = (int)(pLastPoint->dPositionY*1000.0);
|
}
|
|
// 1-3. right gantry result2
|
CalculatePath(ptRightStartPath, vecPathData, rtRightRange2, scheduleResultRight2);
|
|
// 1-4. merge right1 and right2
|
scheduleResultRight1.AddTailScheduleResult(scheduleResultRight2);
|
|
// 2-1.left gantry result1
|
SPathData ptLeftStartPath = vecStartPath[0];
|
CalculatePath(ptLeftStartPath, vecPathData, rtLeftRange1, scheduleResultLeft1);
|
|
// 2-2. get left result1
|
nCollisionResultIdx = scheduleResultLeft1.GetPathSchedulerResultCount()-1;
|
dCollisionResultTime = scheduleResultLeft1.GetTotalSchedulerTime();
|
scheduleResultLeft1.SetCollisionResultIndex(nCollisionResultIdx);
|
scheduleResultLeft1.SetCollisionResultTime(dCollisionResultTime);
|
pLastPoint = scheduleResultLeft1.GetPathSchedulerResult(nCollisionResultIdx);
|
if (pLastPoint)
|
{
|
ptLeftStartPath.nPosX = (int)(pLastPoint->dPositionX*1000.0);
|
ptLeftStartPath.nPosY = (int)(pLastPoint->dPositionY*1000.0);
|
}
|
|
// 2-3. left gantry result2
|
CalculatePath(ptLeftStartPath, vecPathData, rtLeftRange2, scheduleResultLeft2);
|
|
// 2-4. merge left1 and left2
|
scheduleResultLeft1.AddTailScheduleResult(scheduleResultLeft2);
|
|
// set Collision Position
|
scheduleResultLeft1.SetCollisionPositionX(nLeftCollisionPosX);
|
scheduleResultRight1.SetCollisionPositionX(nRightCollisionPosX);
|
|
// add schedule result
|
m_vecPathSchedulerResult.push_back(scheduleResultLeft1);
|
m_vecPathSchedulerResult.push_back(scheduleResultRight1);
|
|
return (int) m_vecPathSchedulerResult.size();
|
}
|
|
}
|
