void EscapingPlanner::SelMidPoint()
{
int lObsNum = lObsPoses.size();
int rObsNum = rObsPoses.size();
int pairObsNum = min(lObsNum, rObsNum);
for (int i = 0; i < pairObsNum; i++)
{
Point2D tempMidpoint;
GetMidPoint(lObsPoses[i], rObsPoses[i], tempMidpoint);
midPoints.push_back(tempMidpoint);
midPoints.push_back(tempMidpoint);
}
for (int i = 0; i < pairObsNum - 1; i++)
{
Point2D point1, point2;
GetMidPoint(lObsPoses[i + 1], rObsPoses[i], point1);
GetMidPoint(lObsPoses[i], rObsPoses[i + 1], point2);
double dist1 = sqrt(pow((point1.x - midPoints[i * 2].x), 2) + pow((point1.y - midPoints[i * 2].y), 2))
+ sqrt(pow((point1.x - midPoints[(i + 1) * 2].x), 2) + pow((point1.y - midPoints[(i + 1) * 2].y), 2));
double dist2 = sqrt(pow((point2.x - midPoints[i * 2].x), 2) + pow((point2.y - midPoints[i * 2].y), 2))
+ sqrt(pow((point2.x - midPoints[(i + 1) * 2].x), 2) + pow((point2.y - midPoints[(i + 1) * 2].y), 2));
if (dist1 >= dist2)
{
midPoints[i * 2 + 1] = point2;
}
else
{
midPoints[i * 2 + 1] = point1;
}
}
if (lObsNum - rObsNum != 0)
{
Point2D tempPoint;
GetMidPoint(lObsPoses[lObsNum - 1], rObsPoses[rObsNum - 1], tempPoint);
midPoints[2 * pairObsNum - 1] = tempPoint;
}
else
{
midPoints.pop_back();
}
//for (unsigned int i = 0; i < midPoints.size(); i++)
//{
// cout << midPoints[i].x << " " << midPoints[i].y << endl;
//}
}
void NervDialog::OnPaint(NervGdiDeviceContext& in_dc){
in_dc.Rect(0, 0, size.x, size.y, 0x00303030);
auto text_mid_pt = GetMidPoint();
auto old_ta = SetTextAlign(in_dc(), TA_CENTER);
auto old_opacity = SetBkMode(in_dc(), TRANSPARENT);
in_dc.WriteText(text_mid_pt.x, text_mid_pt.y, 0, message.c_str(), message.size());
text_mid_pt = button.GetMidPoint();
in_dc.Rect(button, 0x00505050);
in_dc.WriteText(text_mid_pt.x, text_mid_pt.y, 0, "OK", 3);
SetTextAlign(in_dc(), old_ta);
SetBkMode(in_dc(), old_opacity);
}
