SContour::SContour(std::vector<Point_3> points, Plane planeOrig) :
id(-1),
ns((int) points.size()),
plane(planeOrig),
poly(NULL),
sides()
{
DEBUG_START;
sides = new SideOfContour[ns];
auto point = points.begin();
for (int i = 0; i < ns; ++i)
{
ASSERT(point != points.end());
sides[i].A1 = Vector3d(point->x(), point->y(), point->z());
++point;
}
for (int i = 0; i < ns; ++i)
{
int iNext = (ns + i + 1) % ns;
sides[i].A2 = sides[iNext].A1;
ASSERT(std::isfinite(qmod(sides[i].A1 - sides[i].A2)));
ASSERT(qmod(sides[i].A1 - sides[i].A2) > 0);
}
DEBUG_END;
}
std::vector<Vector3d> SContour::getPoints()
{
DEBUG_START;
std::vector<Vector3d> points;
/* Iterate through the array of sides of current contour. */
/*
* FIXME: Is it true that contours are ususally not connected between first
* and last sides?
*/
for (int iSide = 0; iSide < ns; ++iSide)
{
SideOfContour* sideCurr = &sides[iSide];
ASSERT(std::isfinite(sideCurr->A1.x));
ASSERT(std::isfinite(sideCurr->A1.y));
ASSERT(std::isfinite(sideCurr->A1.z));
/* Project current vertex to the plane of contour. */
Vector3d vCurr = plane.project(sideCurr->A1);
points.push_back(vCurr);
/*
* Check that second vertex of previous side lies closely to first
* vertex of the current side.
*/
int iSidePrev = (ns + iSide - 1) % ns;
SideOfContour* sidePrev DEBUG_VARIABLE =
&sides[iSidePrev];
ASSERT(std::isfinite(sidePrev->A2.x));
ASSERT(std::isfinite(sidePrev->A2.y));
ASSERT(std::isfinite(sidePrev->A2.z));
DEBUG_PRINT("sides[%d]->A2 = (%lf, %lf, %lf)",
iSidePrev,
sidePrev->A2.x, sidePrev->A2.y,
sidePrev->A2.z);
DEBUG_PRINT("sides[%d]->A1 = (%lf, %lf, %lf)",
iSide, sideCurr->A1.x, sideCurr->A1.y,
sideCurr->A1.z);
Vector3d diff DEBUG_VARIABLE = sidePrev->A2 - sideCurr->A1;
ASSERT(std::isfinite(diff.x));
ASSERT(std::isfinite(diff.y));
ASSERT(std::isfinite(diff.z));
DEBUG_PRINT(" difference = (%lf, %lf, %lf)",
diff.x, diff.y, diff.z);
if (qmod(diff) >= EPS_MIN_DOUBLE)
{
vCurr = plane.project(sidePrev->A2);
points.push_back(vCurr);
DEBUG_PRINT(COLOUR_RED
"Warning: contour %d is not connected! "
"A gap found between %d-th and %d-th sides."
COLOUR_NORM, id, iSidePrev, iSide);
}
}
DEBUG_END;
return points;
}
//should move this to helper function
void robot::feed()
{
DriverStationLCD *lcd = DriverStationLCD::GetInstance();
lcd->Clear();
float th = gyro.getangle();
a = qmod(th * dt + a, -180, 180);
lcd->PrintfLine(DriverStationLCD::kUser_Line1, "BUILD: %i", BUILD);
lcd->PrintfLine(DriverStationLCD::kUser_Line2, "%f", gyrob.GetAngle());
lcd->PrintfLine(DriverStationLCD::kUser_Line3, "%f", arma.GetVoltage());
lcd->PrintfLine(DriverStationLCD::kUser_Line4, "%f", armb.GetVoltage());
//lcd->PrintfLine(DriverStationLCD::kUser_Line5, "%f", aa);
lcd->UpdateLCD();
}
SContour *SContour::convexify()
{
DEBUG_START;
ASSERT(!!plane.norm && "nu is null vector");
/*
* TODO: This is a temporal workaround. In future we need to perform
* convexification so that to remember what points are vertices of
* convex hull.
*/
auto points = getPoints();
auto pointsMapped = mapPointsToOXYplane(points, plane.norm);
std::vector<Point_2> hull;
convex_hull_2(pointsMapped.begin(), pointsMapped.end(),
std::back_inserter(hull));
if ((int) hull.size() != (int) points.size())
{
ASSERT(hull.size() < points.size());
MAIN_PRINT(COLOUR_RED
"Warning: contour #%d is non-convex, its hull "
"contains %d of %d of its points"
COLOUR_NORM, id, (int) hull.size(),
(int) points.size());
}
/* TODO: Add automatic conversion from Vector3d to Vector_3 !!! */
auto extremePoints = mapPointsFromOXYplane(hull,
Vector_3(plane.norm.x, plane.norm.y,
plane.norm.z));
SContour *contour = new SContour(extremePoints, plane);
contour->id = id;
contour->poly = poly;
#ifndef NDEBUG
for (int iSide = 0; iSide < contour->ns; ++iSide)
{
SideOfContour *side = &contour->sides[iSide];
ASSERT(qmod(side->A1 - side->A2) > 0);
}
#endif /* NDEBUG */
DEBUG_END;
return contour;
}
void EdgeList::get_next_edge(Plane iplane, int& v0, int& v1, int& i0, int& i1,
int& next_f, int& next_d)
{
DEBUG_START;
int i, tmp, i_next = -1;
int id0;
int id1;
int sign0;
int sign1;
int incr = 1;
int nv;
Plane plane;
plane = poly->facets[next_f].plane;
double err;
err = qmod(plane.norm - iplane.norm)
+ (plane.dist - iplane.dist) * (plane.dist - iplane.dist);
if (num < 1 && (fabs(err) > 1e-16))
{
err = qmod(plane.norm + iplane.norm)
+ (plane.dist + iplane.dist) * (plane.dist + iplane.dist);
}
if (num < 1 && fabs(err) <= 0.0000000000000001)
{
DEBUG_PRINT("\t\t --- SPECIAL FACET ---\n\n");
sign0 = poly->signum(poly->vertices[v0], iplane);
sign1 = poly->signum(poly->vertices[v1], iplane);
if (1)
{
id0 = poly->facets[next_f].find_vertex(v0);
id1 = poly->facets[next_f].find_vertex(v1);
if (id0 == -1 || id1 == -1)
{
ERROR_PRINT("\tError: cannot find vertexes %d (%d) "
"and %d (%d) facet %d.\n",
v0, id0, v1, id1, next_f);
DEBUG_END;
return;
}
nv = poly->facets[next_f].numVertices;
if (sign0 >= 0 && sign1 <= 0)
{
if (id1 == id0 + 1 || (id1 == 0 && id0 == nv - 1))
incr = 1;
else if (id1 == id0 - 1 || (id0 == 0 && id1 == nv - 1))
incr = -1;
id0 = id1;
v0 = v1;
id1 = (id0 + incr + nv) % nv;
v1 = poly->facets[next_f].indVertices[id1];
}
else if (sign0 <= 0 && sign1 >= 0)
{
if (id1 == id0 + 1 || (id1 == 0 && id0 == nv - 1))
incr = -1;
else if (id1 == id0 - 1 || (id0 == 0 && id1 == nv - 1))
incr = 1;
id1 = (id0 + incr + nv) % nv;
v1 = poly->facets[next_f].indVertices[id1];
}
else if (sign0 <= 0 && sign1 <= 0 && (next_d == -1 || next_d == 1))
{
incr = next_d;
if ((id1 - id0 + nv) % nv == next_d)
{
id0 = id1;
v0 = v1;
id1 = (id0 + incr + nv) % nv;
v1 = poly->facets[next_f].indVertices[id1];
}
else
{
id1 = (id0 + incr + nv) % nv;
v1 = poly->facets[next_f].indVertices[id1];
}
}
else
{
ERROR_PRINT("Error. Cannot define the direction.");
ERROR_PRINT("sign(%d) = %d, sign(%d) = %d, drctn = %d",
v0, sign0, v1, sign1, next_d);
DEBUG_END;
return;
}
sign0 = poly->signum(poly->vertices[v0], iplane);
sign1 = poly->signum(poly->vertices[v1], iplane);
if (sign1 == 1)
{
//next_f = :
poly->facets[next_f].find_next_facet(incr == 1 ? v0 : v1,
next_f);
next_d = 0;
}
else
{
next_d = incr;
}
}
v1 = v0;
#ifdef DEBUG1
DEBUG_PRINT("\tRESULTING NEXT EDGE : %d %d - GO TO FACET %d\n",
v0, v1, next_f);
#endif
DEBUG_END;
return;
}
else if (num < 1)
{
ERROR_PRINT("Error. num < 1, err = %.16lf, if = %d\n", err,
fabs(err) < 1e-16);
DEBUG_END;
return;
}
if (v0 > v1)
{
tmp = v0;
v0 = v1;
v1 = tmp;
}
#ifdef DEBUG1
DEBUG_PRINT("EdgeList::get_next_edge %d\n",
id);
// this->my_fprint(stdout);
#endif
for (i = 0; i < num; ++i)
{
//#ifdef DEBUG1
// DEBUG_PRINT(" edge0[%d] = %d, edge1[%d] = %d\n",
// i, edge0[i], i, edge1[i]);
//#endif
if (edge0[i] == v0 && edge1[i] == v1)
{
// set_pointer(i);
isUsed[i] = true;
switch (next_direction[i])
{
case 1:
i_next = i < num - 1 ? i + 1 : 0;
break;
case -1:
i_next = i > 0 ? i - 1 : num - 1;
break;
case 0:
switch (next_d)
{
case 1:
i_next = i < num - 1 ? i + 1 : 0;
break;
case -1:
i_next = i > 0 ? i - 1 : num - 1;
break;
default:
break;
}
break;
default:
break;
}
v0 = edge0[i_next];
v1 = edge1[i_next];
i0 = ind0[i_next];
i1 = ind1[i_next];
next_f = next_facet[i_next];
if (next_direction[i] != 0)
next_d = next_direction[i];
isUsed[i_next] = true;
#ifdef DEBUG1
DEBUG_PRINT("\tRESULTING NEXT EDGE : %d %d - GO TO FACET %d\n",
v0, v1, next_f);
#endif
DEBUG_END;
return;
}
}
// set_pointer(0);
v0 = v1 = next_f = -1;
next_d = -2;
DEBUG_END;
}
int main() {
int64 a, b, c;
while (~scanf("%lld%lld%lld", &a, &b, &c))
printf("%lld\n", qmod(a, b, c));
return 0;
}
