std::vector<std::vector<float>> solve(float x, float y, float z) {
static bool init = false;
static float px, py, pz;
std::vector<std::vector<float>> ts{};
mat lines;
vec p_init(3);
vec pf(3);
if (!init) {
init = true;
solve_init();
goto over_and_out;
}
p_init <<= px, py, pz;
pf <<= x, y, z;
lines = follow_line(p_init, pf, 5, R0, w, q, R0, p0);
for (int i = 0; i < lines.size1(); i++) {
std::vector<float> t;
for (int j = 0; j < 5; j++)
t.push_back(lines(i, j));
ts.push_back(t);
}
over_and_out:
px = x;
py = y;
pz = z;
return ts;
}
int main(int argc, char *argv[]) {
ds_active(&LIGHTSENS);
locate_line();
follow_line();
return 0;
}
bool traverse_edge(int a, int b,int from) {
int multi;
int test = 0;
if(a < 5)
{
multi = -1;
a = 12-a;
b = 12-b;
}
else multi = 1;
switch(a)
{
case 12:
if (b == 5)
{
move(500);
test = follow_line(no_stop);
while(!test)
test = follow_line(no_stop);
if (test == FORK)
return true;
}
if (b == 11)
{
move(50);
rotate(multi*90);
move(10);
test = follow_line(no_stop);
while(!test)
{
test = follow_line(no_stop);
}
if (test == FORK) return true;
}
case 11:
if (b == 10)
{
move(50);
test = follow_line(no_stop);
while(!test)
test = follow_line(no_stop);
if (test == FORK) return true;
}
case 10:
if (b == 9)
{
test = follow_line(no_stop);
while(!test)
test = follow_line(no_stop);
if (test == FORK) return true;
}
case 9:
if (b == 8)
{
rotate(multi*30);
move(10);
test = follow_line(no_stop);
while(!test)
test = follow_line(no_stop);
if (test == FORK) return true;
}
if (b == 6)
{
rotate(multi*60);
move(10);
test = follow_line(no_stop);
while(!test)
test = follow_line(no_stop);
if (test == FORK) return true;
}
case 8:
if (b == 7)
{
rotate(multi*-90);
test = follow_line(no_stop);
while(!test)
test = follow_line(IR_trigger);
if (test == FORK) return true;
}
if (b == 9)
{
rotate(multi*180);
test = follow_line(no_stop);
while(!test)
test = follow_line(no_stop);
if (test == FORK) return true;
}
if (b == 4)
{
test = follow_line(no_stop);
while(!test)
test = follow_line(no_stop);
if (test == FORK) return true;
}
case 7:
if (b == 8)
{
rotate(multi*180);
test = follow_line(no_stop);
while(!test)
test = follow_line(IR_trigger);
if (test == FORK) return true;
}
if (b == 4)
{
test = follow_line(no_stop);
while(!test)
test = follow_line(no_stop);
if (test == FORK) return true;
}
}
return true;
}
