自制走S型算法

先上代码，c++版

#include <iostream>
#include <vector>

template <typename T>
struct structPos {
    T x;
    T y;
};
int main() {
    std::vector<structPos<int >> vec;
    structPos<int > structNow,structInit,structEnd;
    structInit = {2,2};
    structEnd = {6,6};
    structPos<int > structMap = {10,10};
    vec.push_back(structInit);
    for (int i = structInit.y; i < structEnd.y; ++i) {
        structNow = {structMap.x,i};
        vec.push_back(structNow);
        structNow = {structMap.x,i + 1};
        vec.push_back(structNow);
        //坐标上移，方便回头
        ++i;

        structNow = {1,i};
        vec.push_back(structNow);
        structNow = {1,i + 1};
        vec.push_back(structNow);
    }
     vec.push_back(structEnd);
    for (auto &&item : vec) {
        printf("%d , %d\n",item.x,item.y);
    }
    return 0;
}

在map坐标里面设定起始坐标和结束坐标，找出需要经过的各点，本意是想做一个机器人的全局规划器

---

例子设定的是map[10,10] 起始坐标为[2,2] 结尾坐标为[6,6]
输出如图

image.png