// Calculates and return the gradient between two points double grad(Point a, Point b) { return 1.0 * (a.y() - b.y()) / (a.x() - b.x()); }

bool ConvexHull::isLeftTurn(Point c, Point a, Point b){ Point u(b.x() - a.x(), b.y() - a.y(), 0), v(c.x() - a.x(), c.y() - a.y(), 0); return u.x()*v.y() - u.y()*v.x() >= 0; }

Point :: Point(const Point &rhs){ x = rhs.getX(); y = rhs.getY(); }

void read () { o.read(), scanf("%lf", &r); }

//=========================================================================== void TorusVolume::translate(const Point& vec) //=========================================================================== { ALWAYS_ERROR_IF(dimension() != vec.dimension(), "Volume and translation vector of different dimension"); location_ += vec; }

void Canhamo::drawLine(const Point &begin, const Point &end, int r, int g, int b){ CanvasLine line = { { begin.x(), begin.y(), end.x(), end.y() }, { r, g, b } }; canvasLines.push_back(line); }

//----------------------------------------------------------------------------- void MeshSmoothing::smooth(Mesh& mesh, std::size_t num_iterations) { log(PROGRESS, "Smoothing mesh: %s", mesh.str(false).c_str()); if (mesh.geometry().degree() != 1) { dolfin_error("MeshSmoothing.cpp", "smooth mesh", "This function does not support higher-order mesh geometry"); } // Make sure we have cell-facet connectivity mesh.init(mesh.topology().dim(), mesh.topology().dim() - 1); // Make sure we have vertex-edge connectivity mesh.init(0, 1); // Make sure the mesh is ordered mesh.order(); // Mark vertices on the boundary so we may skip them BoundaryMesh boundary(mesh, "exterior"); const MeshFunction<std::size_t> vertex_map = boundary.entity_map(0); MeshFunction<bool> on_boundary(reference_to_no_delete_pointer(mesh), 0); on_boundary = false; if (boundary.num_vertices() > 0) { for (VertexIterator v(boundary); !v.end(); ++v) on_boundary[vertex_map[*v]] = true; } // Iterate over all vertices const std::size_t d = mesh.geometry().dim(); std::vector<double> xx(d); for (std::size_t iteration = 0; iteration < num_iterations; iteration++) { for (VertexIterator v(mesh); !v.end(); ++v) { // Skip vertices on the boundary if (on_boundary[*v]) continue; // Get coordinates of vertex const Point p = v->point(); // Compute center of mass of neighboring vertices for (std::size_t i = 0; i < d; i++) xx[i] = 0.0; std::size_t num_neighbors = 0; for (EdgeIterator e(*v); !e.end(); ++e) { // Get the other vertex dolfin_assert(e->num_entities(0) == 2); std::size_t other_index = e->entities(0)[0]; if (other_index == v->index()) other_index = e->entities(0)[1]; // Create the vertex Vertex vn(mesh, other_index); // Skip the vertex itself if (v->index() == vn.index()) continue; num_neighbors += 1; // Compute center of mass const double* xn = vn.x(); for (std::size_t i = 0; i < d; i++) xx[i] += xn[i]; } for (std::size_t i = 0; i < d; i++) xx[i] /= static_cast<double>(num_neighbors); // Compute closest distance to boundary of star double rmin = 0.0; for (CellIterator c(*v); !c.end(); ++c) { // Get local number of vertex relative to facet const std::size_t local_vertex = c->index(*v); // Get normal of corresponding facet Point n = c->normal(local_vertex); // Get first vertex in facet Facet f(mesh, c->entities(mesh.topology().dim() - 1)[local_vertex]); VertexIterator fv(f); // Compute length of projection of v - fv onto normal const double r = std::abs(n.dot(p - fv->point())); if (rmin == 0.0) rmin = r; else rmin = std::min(rmin, r); } // Move vertex at most a distance rmin / 2 double r = 0.0; for (std::size_t i = 0; i < d; i++) { const double dx = xx[i] - p[i]; r += dx*dx; } r = std::sqrt(r); if (r < DOLFIN_EPS) continue; rmin = std::min(0.5*rmin, r); std::vector<double> new_vertex(mesh.geometry().x(v->index()), mesh.geometry().x(v->index()) + d); for (std::size_t i = 0; i < d; i++) new_vertex[i] += rmin*(xx[i] - p[i])/r; mesh.geometry().set(v->index(), new_vertex.data()); } } if (num_iterations > 1) log(PROGRESS, "Mesh smoothing repeated %d times.", num_iterations); }

float dFunc_linear(const Point& pt) { return pt.x()+0.5; }

//draws the bitmap void draw() const { bitmap.draw(position.getX(), position.getY()); }

float dFunc_zoneplate(const Point& pt) { return std::sin( 100*M_PI * (std::pow(pt.x()+0.5, 2) + std::pow(pt.y()+0.5, 2)) )/2. + 0.5; }

float dFunc_contrast(const Point& pt) { return (pt.y()+0.5) * std::sin( 100*M_PI * std::pow(pt.x()+0.5, 2) )/2. + 0.5; }

float dFunc_radial(const Point& pt) { return pt.norm()*2.; }

float dFunc_quadra(const Point& pt) { return std::pow(pt.x()+0.5, 2); }

Point Landmark::get_point(map<string,double> values) { Point result = Point(m_x_term->evaluate(values), m_y_term->evaluate(values)); result.set_landmark_name(m_name); return result; }

Geometry2d::Point OurRobot::pointInRobotSpace(Geometry2d::Point pt) const { Point p = pt; p.rotate(pos, -angle); return p; }

void GoalDetection::performSanityChecks(const Fovea &fovea, const VisionFrame &frame, std::vector<BBox> ®ions) { std::vector<BBox> candidates (regions); regions.clear(); std::vector<BBox>::iterator it; for (it = candidates.begin(); it != candidates.end(); ++it) { // Check that the middle and the bottom 75% for edges // Only need 1 strong edge in each to be good int middle = (it->a.y() + it->b.y()) / 2; int bottom = it->a.y() + (it->b.y() - it->a.y()) * 0.75; bool keepM = false; bool keepB = false; for (int col = it->a.x(); col <= it->b.x(); ++col) { // Check middle Point edge = fovea.edge(col, middle); float magnitude = (edge.x() * edge.x()) + (edge.y() * edge.y()); if (magnitude > MIN_EDGE_THRESHOLD) keepM = true; // Check bottom edge = fovea.edge(col, bottom); magnitude = (edge.x() * edge.x()) + (edge.y() * edge.y()); if (magnitude > MIN_EDGE_THRESHOLD) keepB = true; } if (!keepM) { //std::cout << "throwing away since no keepM" << std::endl; continue ; } if (!keepB) { //std::cout << "throwing away since no keepB" << std::endl; continue; } // Check % of colour in goal post - ie compare length and colour float length = it->b.y() - it->a.y(); int centre = (it->a.x() + it->b.x()) / 2; float numColourPixels = 0; for (int row = it->a.y(); row < it->b.y(); ++row) { if (fovea.colour(centre, row) == cGOAL_YELLOW) { ++numColourPixels; } } if ((numColourPixels / length) < COLOUR_RATIO_THRESHOLD) { //std::cout << "throwing away since not enough colour" << std::endl; continue; } // Check bottom of goal post is below field edge Point fieldEdge = fovea.mapFoveaToImage(Point(centre, 0)); int fieldEdgeY = 0; if (fovea.top) { fieldEdgeY = frame.topStartScanCoords[fieldEdge.x()]; } else { fieldEdgeY = frame.botStartScanCoords[fieldEdge.x()]; } fieldEdge.y() = std::max(fieldEdge.y(), fieldEdgeY); fieldEdge = fovea.mapImageToFovea(fieldEdge); if (fieldEdge.y() > it->b.y()) { //std::cout << "throwing away since above field edge" << std::endl; continue; } regions.push_back(*it); } // If more than 2 goals, things have gone wrong, so panic if (regions.size() > 2) regions.clear(); }

void Shape::stack(Shape *p, const Shape *q) { Point n = q->north(); Point s = p->south(); p->move(n.x() - s.x(), n.y() -s.y() +1); }

// Finds the distance to post // Tunes the BBox using higher resolution foveas RRCoord GoalDetection::findDistanceToPost(VisionFrame &frame, const Fovea& fovea, BBox& goal, int numPosts, PostInfo& p) { bool trustDistance = true; float differenceThreshold = 1.7; // **** Try finding the distance using kinematics of the base **** findBaseOfPost(frame, goal); const CameraToRR *convRR = &frame.cameraToRR; Point base = Point((goal.a.x()+goal.b.x())/2, goal.b.y()); RRCoord rr = convRR->convertToRR(base, false); float kdistance = rr.distance(); // **** Try using the width the find the distance **** // Calculate width distance at 3 points and take median std::map<float, float> distances; for (float h = 0.89; h < 1; h += 0.05) { float d = widthDistanceToPost(frame, goal, h); distances.insert(std::make_pair(d,h)); } std::map<float, float>::iterator i = distances.begin(); if (distances.size() > 1) ++i; float wdistance = widthDistanceToPost(frame, goal, i->second, true); // **** Decide which distance to use **** // Kinematics is usually more accurate, so use it unless we know it's wrong // If post ends at bottom of image, probably not the bottom, so use width bool width = false; if (fovea.top && goal.b.y() > (TOP_IMAGE_ROWS-10)) { width = true; } // If still yellow below the base, probably missed the bottom, so use width // Only for 1 post though if (numPosts == 1) { Point fTop = fovea.mapImageToFovea(goal.a); Point fBot = fovea.mapImageToFovea(goal.b); const YHistogram &yhistogram = fovea.yhistogram; int height = (fBot.y() - fTop.y()) / 4; // set max scan size int endPoint = std::min(fovea.bb.height(), fBot.y() + height); int noYellow = fBot.y(); for (int i = fBot.y(); i < endPoint; ++i) { int current = yhistogram._counts[i][cGOAL_YELLOW]; if (current < 10) noYellow = i; } if (noYellow == fBot.y()) trustDistance = false; } // Decided to use width distance if (width) { if (wdistance < 1500) rr.distance() = wdistance; else trustDistance = false; // differenceThreshold = 1.7; } // Check that kinematics and width distances are similar else if (kdistance < 2500) { } else if (((kdistance / wdistance) > differenceThreshold) || ((wdistance / kdistance) > differenceThreshold)) { trustDistance = false; } // Check distance is reasonable if (rr.distance() > 12000) { trustDistance = false; rr.distance() = 12000; } // Set variables in PostInfo p.rr = rr; p.kDistance = kdistance; p.wDistance = wdistance; p.trustDistance = trustDistance; p.imageCoords = goal; return rr; }

//============================================================================== void Drawable::setOriginWithOriginalSize (const Point<float>& originWithinParent) { setTransform (AffineTransform::translation (originWithinParent.getX(), originWithinParent.getY())); }

float GoalDetection::widthDistanceToPost(VisionFrame &frame, BBox& goal, float h, bool update) { const CameraToRR *convRR = &frame.cameraToRR; float distance = -1; // Calculate where fovea should go int y = goal.a.y() + (goal.b.y() - goal.a.y()) * h; Point tl = Point (goal.a.x(), y-1); Point br = Point (goal.b.x(), y+2); int width = br.x() - tl.x(); int density = 1; // Try and extend fovea a bit // Note add extra to left since edge data seems slightly skewed left tl.x() = std::max(tl.x() - width/2, 0); br.x() = std::min(br.x() + width/2, TOP_IMAGE_COLS); // Create a high res fovea boost::shared_ptr<FoveaT<hGoals, eGrey> > goalFovea( new FoveaT<hGoals, eGrey>(BBox(tl, br), density, 0, true)); goalFovea->actuate(frame); goalFoveas.push_back(goalFovea); width = br.x() - tl.x(); // Trace the yellow from the centre outwards // Note: left and right are in fovea coords int left = -1; int right = -1; int max = 0; int lastYellow = width/2; int maxNotYellow = std::max(4, width/30); for (int x = width/2; x > 0; --x) { // Check the colour, give up after n non-yellow in a row Colour c = goalFovea->colour(x,0); if (c == cGOAL_YELLOW) { lastYellow = x; } else if (abs(lastYellow - x) > maxNotYellow) { break; } // Keep track of max edge so far Point edge = goalFovea->edge(x,0); int magnitude = (edge.x() * edge.x()) + (edge.y() * edge.y()); if (magnitude > max) { left = x; max = magnitude; } // Delete early edges if (abs(lastYellow - left) > 10) { left = -1; max = 0; } } lastYellow = width/2; max = 0; for (int x = width / 2; x < width; ++x) { // Check the colour, give up after n non-yellow in a row Colour c = goalFovea->colour(x,0); if (c == cGOAL_YELLOW) { lastYellow = x; } else if (abs(lastYellow - x) > maxNotYellow) { break; } // Keep track of max edge so far Point edge = goalFovea->edge(x,0); int magnitude = (edge.x() * edge.x()) + (edge.y() * edge.y()); if (magnitude > max) { right = x; max = magnitude; } // Delete early edges if (abs(lastYellow - right) > 10) { right = -1; max = 0; } } if (left >= 0 && right > 0 && (right-left > 0)) { distance = convRR->pixelSeparationToDistance (right-left, GOAL_POST_DIAMETER); if (update) { goal.a.x() = tl.x() + left; goal.b.x() = tl.x() + right; } } return distance; }

void ModelView::move_to_center_(void) { Point centroid = model_->centroid(); auto_trans_x_ = -centroid.x(); auto_trans_y_ = -centroid.y(); auto_trans_z_ = -centroid.z(); }

bool Line::isBehind(const Point& pt) const { float b = numerator(Line(start(), pt)); cerr << "Behindness of (" << pt.x() << "," << pt.y() << ")r is " << b << endl; return b < 0; }

float dist(float x, float y, Point & a) { float xd = x - a.x(); float yd = y - a.y(); return xd*xd + yd*yd; }

Point::Point(const Point &p) :_x(p.x()), _y(p.y()) { }

double ConvexHull::getAngleTgBetweenPoints(Point point, Point centerPoint){ return (centerPoint.y() - point.y())*1.0/(centerPoint.x() - point.x()); }

bool Rectangle::isIn(const Point& point) const { return point.getX() >= getX() && point.getX() <= getX() + length_ && point.getY() >= getY() - width_ && point.getY() <= getY(); }

/* * This method returns the angle between three points. * The current point is the centre, and the other two points its arms. * * @return double: angle in radians. */ double get_angle(Point i, Point j) { double b = get_distance(i), c = get_distance(j), a = i.get_distance(j); return acos((b*b + c*c - a*a)/(2*b*c)); }

void DragAndDropContainer::startDragging (const var& sourceDescription, Component* sourceComponent, const Image& dragImage_, const bool allowDraggingToExternalWindows, const Point<int>* imageOffsetFromMouse) { Image dragImage (dragImage_); if (dragImageComponent == nullptr) { MouseInputSource* draggingSource = Desktop::getInstance().getDraggingMouseSource (0); if (draggingSource == nullptr || ! draggingSource->isDragging()) { jassertfalse; // You must call startDragging() from within a mouseDown or mouseDrag callback! return; } const Point<int> lastMouseDown (Desktop::getLastMouseDownPosition()); Point<int> imageOffset; if (dragImage.isNull()) { dragImage = sourceComponent->createComponentSnapshot (sourceComponent->getLocalBounds()) .convertedToFormat (Image::ARGB); dragImage.multiplyAllAlphas (0.6f); const int lo = 150; const int hi = 400; Point<int> relPos (sourceComponent->getLocalPoint (nullptr, lastMouseDown)); Point<int> clipped (dragImage.getBounds().getConstrainedPoint (relPos)); Random random; for (int y = dragImage.getHeight(); --y >= 0;) { const double dy = (y - clipped.getY()) * (y - clipped.getY()); for (int x = dragImage.getWidth(); --x >= 0;) { const int dx = x - clipped.getX(); const int distance = roundToInt (std::sqrt (dx * dx + dy)); if (distance > lo) { const float alpha = (distance > hi) ? 0 : (hi - distance) / (float) (hi - lo) + random.nextFloat() * 0.008f; dragImage.multiplyAlphaAt (x, y, alpha); } } } imageOffset = clipped; } else { if (imageOffsetFromMouse == nullptr) imageOffset = dragImage.getBounds().getCentre(); else imageOffset = dragImage.getBounds().getConstrainedPoint (-*imageOffsetFromMouse); } dragImageComponent = new DragImageComponent (dragImage, sourceDescription, sourceComponent, draggingSource->getComponentUnderMouse(), *this, imageOffset); currentDragDesc = sourceDescription; if (allowDraggingToExternalWindows) { if (! Desktop::canUseSemiTransparentWindows()) dragImageComponent->setOpaque (true); dragImageComponent->addToDesktop (ComponentPeer::windowIgnoresMouseClicks | ComponentPeer::windowIsTemporary | ComponentPeer::windowIgnoresKeyPresses); } else { Component* const thisComp = dynamic_cast <Component*> (this); if (thisComp == nullptr) { jassertfalse; // Your DragAndDropContainer needs to be a Component! return; } thisComp->addChildComponent (dragImageComponent); } static_cast <DragImageComponent*> (dragImageComponent.get())->updateLocation (false, lastMouseDown); dragImageComponent->setVisible (true); #if JUCE_WINDOWS // Under heavy load, the layered window's paint callback can often be lost by the OS, // so forcing a repaint at least once makes sure that the window becomes visible.. ComponentPeer* const peer = dragImageComponent->getPeer(); if (peer != nullptr) peer->performAnyPendingRepaintsNow(); #endif } }

double operator-(const Point &rhs, const Point &lhs){ double dist; dist = sqrt((rhs.getX() - lhs.getX())*(rhs.getX() - lhs.getX()) + (rhs.getY() - lhs.getY())*(rhs.getY() - lhs.getY())); return dist; }

// Returns whether 3 points are collinear and in order a->b->c bool collinear(Point a, Point b, Point c) { if ((a.y() - b.y()) * (a.x() - c.x()) == (a.y() - c.y()) * (a.x() - b.x())) { return (a.dist(b) <= a.dist(c) && c.dist(b) <= c.dist(a)); } return false; }