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