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; }