Homotopy-constrained-planning/append&check_signatures.h at master · nrkumar93/Homotopy-constrained-planning · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
string append_signature(string signature_state, string signature_action)
{
  if(signature_action[0]!='-')
  {
    // cout<<"positive"<<'\n';
    std::size_t found = signature_state.find(signature_action);
    if(found!=std::string::npos)
    {
      // cout<<"Found"<<'\n';
      if(signature_state[found-1]=='-')
      {
        // cout<<"found negative version"<<'\n';
        signature_state.erase(found-1,2);
      }
    }
    else
    {
      // cout<<"not found"<<'\n';
      signature_state+=signature_action;
      // cout<<signature_state;
    }
    // cout<<signature_state<<'\n';
  }
  else
  {
    // cout<<"negative";
    std::size_t found_negative_version = signature_state.find(signature_action);
    if(found_negative_version!=std::string::npos)
    {
      return signature_state;
    }
    string positive_version_action;
    positive_version_action.push_back(signature_action[1]);
    std::size_t found_positive_version = signature_state.find(positive_version_action);
    if(found_positive_version!=std::string::npos)
    {
      signature_state.erase(found_positive_version,1);
    }
    else
    {
      signature_state+=signature_action;
    }
  }
  // cout<<"BHBJ "<<signature_state;
  return signature_state;
}

string new_append_signature(string signature_state, string signature_action)
{
  if(signature_action[0]!='-')
  {
    if(signature_state[signature_state.size()-1]==signature_action[0])
    {
      if(signature_state[signature_state.size()-2]=='-') signature_state.erase(signature_state.size()-2,2);
      return signature_state;
    }
    else signature_state+=signature_action;
  }
  else
  {
    if(signature_state[signature_state.size()-1]==signature_action[1])
    {
      if(signature_state[signature_state.size()-2]!='-') signature_state.erase(signature_state.size()-1,1);
      return signature_state;
    }
    else signature_state+=signature_action;
  }
  return signature_state;
}

string find_signature_action(node *current, node*successor,vector<Point2f> representative_points, int dX)
{
  for(int i=0;i<3;i++)
    {
      if(int(representative_points[i].x)==current->x)
      {
        if(current->y>int(representative_points[i].y))
        {
          // return "hi";
          // if(dX==0) return "";
          // else if(dX==1) return std::to_string(i+1);
          if(dX==-1) return "-"+std::to_string(i+1);
        }
      }
    }
  for(int i=0;i<3;i++)
    {
      if(int(representative_points[i].x)==successor->x)
      {
        if(successor->y>int(representative_points[i].y))
        {
          // if(dX==1) return "";
          if(dX==1) return std::to_string(i+1);
          // else return "-"+std::to_string(i+1);
        }
      }
    }
  return "";
}