Im trying to calculate the angle between two edges in a graph, in order to do that I transfer both edges to origin and then used dot product to calculate the angle. my problem is that for some edges like e1 and e2 the output of angle(e1,e2) is -1.#INDOO.
what is this output? is it an error?
Here is my code:
double angle(Edge e1, Edge e2){
Edge t1 = e1, t2 = e2;
Point tail1 = t1.getTail(), head1 = t1.getHead();
Point u(head1.getX() - tail1.getX(), head1.getY() - tail1.getY());
Point tail2 = t2.getTail(), head2 = t2.getHead();
Point v(head2.getX() - tail2.getX(), head2.getY() - tail2.getY());
double dotProduct = u.getX()*v.getX() + u.getY()*v.getY();
double cosAlpha = dotProduct / (e1.getLength()*e2.getLength());
return acos(cosAlpha);
}
Edge is a class that holds two Points, and Point is a class that holds two double numbers as x and y.
Im using angle(e1,e2) to calculate the orthogonal projection length of a vector like b on to a vector like a :
double orthogonalProjectionLength(Edge b, Edge a){
return (b.getLength()*sin(angle(b, a) * (PI / 180)));
}
and this function also sometimes gives me -1.#INDOO. you can see the implementation of Point and Edge here.
My input is a set S of n Points in 2D space. Iv constructed all edges between p and q (p,q are in S) and then tried to calculate the angle like this:
for (int i = 0; i < E.size(); i++)
for (int j = 0; j < E.size(); j++){
if (i == j)
cerr << fixed << angle(E[i], E[j]) << endl; //E : set of all edges
}
If the problem comes from cos() and sin() functions, how can I fix it? is here other libraries that calculate sin and cos in more efficient way?
look at this example.
the inputs in this example are two distinct points(like p and q), and there are two Edges between them (pq and qp). shouldnt the angle(pq , qp) always be 180 ? and angle(pq,pq) and angle(qp,qp) should be 0. my programm shows two different kinds of behavior, sometimes angle(qp,qp) == angle(pq,pq) ==0 and angle(pq , qp) == angle(pq , qp) == 180.0, and sometimes the answer is -1.#INDOO for all four edges.
Here is a code example.
run it for several times and you will see the error.
You want the projection and you go via all this trig? You just need to dot b with the unit vector in the direction of a. So the final answer is
(Xa.Xb + Ya.Yb) / square_root(Xa^2 + Ya^2)
Did you check that cosAlpha doesn't reach 1.000000000000000000000001? That would explain the results, and provide another reason not to go all around the houses like this.
It seems like dividing by zero. Make sure that your vectors always have 0< length.
Answer moved from mine comment
check if your dot product is in <-1,+1> range ...
due to float rounding it can be for example 1.000002045 which will cause acos to fail.
so add two ifs and clamp to this range.
or use faster way: acos(0.99999*dot)
but that lowers the precision for all angles
and also if 0.9999 constant is too big then the error is still present
A recommended way to compute angles is by means of the atan2 function, taking two arguments. It returns the angle on four quadrants.
You can use it in two ways:
compute the angles of u and v separately and subtract: atan2(Vy, Vx) - atan2(Uy, Ux).
compute the cross- and dot-products: atan2(Ux.Vy - Uy.Vx, Ux.Uy + Vx.Vy).
The only case of failure is (0, 0).
Related
I have N distinct lines on a cartesian plane. Since slope-intercept form of a line is, y = mx + c, slope and y-intercept of these lines are given. I have to find the y coordinate of the bottommost intersection of any two lines.
I have implemented a O(N^2) solution in C++ which is the brute-force approach and is too slow for N = 10^5. Here is my code:
int main() {
int n;
cin >> n;
vector<pair<int, int>> lines(n);
for (int i = 0; i < n; ++i) {
int slope, y_intercept;
cin >> slope >> y_intercept;
lines[i].first = slope;
lines[i].second = y_intercept;
}
double min_y = 1e9;
for (int i = 0; i < n; ++i) {
for (int j = i + 1; j < n; ++j) {
if (lines[i].first ==
lines[j].first) // since lines are distinct, two lines with same slope will never intersect
continue;
double x = (double) (lines[j].second - lines[i].second) / (lines[i].first - lines[j].first); //x-coordinate of intersection point
double y = lines[i].first * x + lines[i].second; //y-coordinate of intersection point
min_y = min(y, min_y);
}
}
cout << min_y << endl;
}
How to solve this efficiently?
In case you are considering solving this by means of Linear Programming (LP), it could be done efficiently, since the solution which minimizes or maximizes the objective function always lies in the intersection of the constraint equations. I will show you how to model this problem as a maximization LP. Suppose you have N=2 first degree equations to consider:
y = 2x + 3
y = -4x + 7
then you will set up your simplex tableau like this:
x0 x1 x2 x3 b
-2 1 1 0 3
4 1 0 1 7
where row x0 represents the negation of the coefficient of "x" in the original first degree functions, x1 represents the coefficient of "y" which is generally +1, x2 and x3 represent the identity matrix of dimensions N by N (they are the slack variables), and b represents the value of the idepent term. In this case, the constraints are subject to <= operator.
Now, the objective function should be:
x0 x1 x2 x3
1 1 0 0
To solve this LP, you may use the "simplex" algorithm which is generally efficient.
Furthermore, the result will be an array representing the assigned values to each variable. In this scenario the solution is:
x0 x1 x2 x3
0.6666666667 4.3333333333 0.0 0.0
The pair (x0, x1) represents the point which you are looking for, where x0 is its x-coordinate and x1 is it's y-coordinate. There are other different results that you could get, for an example, there could exist no solution, you may find out more at plenty of books such as "Linear Programming and Extensions" by George Dantzig.
Keep in mind that the simplex algorithm only works for positive values of X0, x1, ..., xn. This means that before applying the simplex, you must make sure the optimum point which you are looking for is not outside of the feasible region.
EDIT 2:
I believe making the problem feasible could be done easily in O(N) by shifting the original functions into a new position by means of adding a big factor to the independent terms of each function. Check my comment below. (EDIT 3: this implies it won't work for every possible scenario, though it's quite easy to implement. If you want an exact answer for any possible scenario, check the following explanation on how to convert the infeasible quadrants into the feasible back and forth)
EDIT 3:
A better method to address this problem, one that is capable of precisely inferring the minimum point even if it is in the negative side of either x or y: converting to quadrant 1 all of the other 3.
Consider the following generic first degree function template:
f(x) = mx + k
Consider the following generic cartesian plane point template:
p = (p0, p1)
Converting a function and a point from y-negative quadrants to y-positive:
y_negative_to_y_positive( f(x) ) = -mx - k
y_negative_to_y_positive( p ) = (p0, -p1)
Converting a function and a point from x-negative quadrants to x-positive:
x_negative_to_x_positive( f(x) ) = -mx + k
x_negative_to_x_positive( p ) = (-p0, p1)
Summarizing:
quadrant sign of corresponding (x, y) converting f(x) or p to Q1
Quadrant 1 (+, +) f(x)
Quadrant 2 (-, +) x_negative_to_x_positive( f(x) )
Quadrant 3 (-, -) y_negative_to_y_positive( x_negative_to_x_positive( f(x) ) )
Quadrant 4 (+, -) y_negative_to_y_positive( f(x) )
Now convert the functions from quadrants 2, 3 and 4 into quadrant 1. Run simplex 4 times, one based on the original quadrant 1 and the other 3 times based on the converted quadrants 2, 3 and 4. For the cases originating from a y-negative quadrant, you will need to model your simplex as a minimization instance, with negative slack variables, which will turn your constraints to the >= format. I will leave to you the details on how to model the same problem based on a minimization task.
Once you have the results of each quadrant, you will have at hands at most 4 points (because you might find out, for example, that there is no point on a specific quadrant). Convert each of them back to their original quadrant, going back in an analogous manner as the original conversion.
Now you may freely compare the 4 points with each other and decide which one is the one you need.
EDIT 1:
Note that you may have the quantity N of first degree functions as huge as you wish.
Other methods for solving this problem could be better.
EDIT 3: Check out the complexity of simplex. In the average case scenario, it works efficiently.
Cheers!
I know there are many sites which explain how to check for an intersection of two lines, but I find it utterly boring to just copy and paste code for such a simple mathematical task. The more it frustrates me that I cannot get my code to work. I know questions with "What is wrong in my code?" are stupid, but I don't know what the hell is wrong with my mathematics / code, also my code is documented nicely (except of admittedly bad variable naming), so I guess there should be someone who is interested in the math behind it:
bool segment::checkforIntersection(QPointF a, QPointF b) { //line 1: a+bx, line 2: c+dx, note that a and c are called offset and bx and dx are called gradients in this code
QPointF bx = b-a;
double firstGradient = bx.y() / bx.x(); //gradient of line 1
//now we have to calculate the offset of line 1: we have b from a+bx. Since QPointF a is on that line, it is:
//a + b * a.x = a.y with a as free variable, which yields a = a.y - b*a.x.
//One could also use the second point b for this calculation.
double firstOffset = a.y() - firstGradient * a.x();
double secondGradient, secondOffset;
for (int i = 0; i < poscount-3; i++) { //we dont check with the last line, because that could be the same line, as the one that emited intersection checking
QPointF c = pos[i];
QPointF d = pos[i+1];
QPointF dx = d-c;
secondGradient = dx.y() / dx.x(); //same formula as above
secondOffset = c.y() - secondGradient * c.x();
//a+bx=c+dx <=> a-c = (d-b)x <=> (a-c)/(d-b) = x
double x = (firstOffset - secondOffset) / (secondGradient - firstGradient);
//we have to check, if those lines intersect with a x \in [a.x,b.x] and x \in [c.x,d.x]. If this is the case, we have a collision
if (x >= a.x() && x <= b.x() && x >= c.x() && x <= d.x()) {
return true;
}
}
return false;
}
So what this does, it has 4 points a, b, c, d (line 1: a--b, line 2: c--d) (ignore the for loop) which have an absolute x and y value. First it calculates the gradient of the lines by calculating deltay/deltax. Then it calculates the offset by using the fact that point a (or c respectively) are on the lines. This way we transformed the 4 points into mathematical representation of these lines as equation a+bx, whereas a x of 0 means that we are at the first point (a / c) and a x of 1 means that we are on the second point (b/d). Next we calculate the intersection of those two lines (basic algebra). After that we check if the intersection's x value is valid. To my understanding this is all correct. Does anyone see the error?
This was empirically checked to be incorrect. The code does not give any false Positives (says there is an intersection, when there isn't), but it gives false Negatives (says there is no intersection, when there actually is). So when it says there is an Intersection it is correct, however if it says there is no intersection, you cannot always believe my algorithm.
Again, I checked online, but the algorithms are different (with some orientation tricks and something), I just wanted to come up with my own algorithm, I would be so glad if someone could help. :)
Edit: Here is a minimal reproducable not working example, this time without Qt but with C++ only:
#include <iostream>
#include <math.h>
using namespace std;
class Point {
private:
double xval, yval;
public:
// Constructor uses default arguments to allow calling with zero, one,
// or two values.
Point(double x = 0.0, double y = 0.0) {
xval = x;
yval = y;
}
// Extractors.
double x() { return xval; }
double y() { return yval; }
Point sub(Point b)
{
return Point(xval - b.xval, yval - b.yval);
}
};
bool checkforIntersection(Point a, Point b, Point c, Point d) { //line 1: a+bx, line 2: c+dx, note that a and c are called offset and bx and dx are called gradients in this code
Point bx = b.sub(a);
double firstGradient = bx.y() / bx.x(); //gradient of line 1
//now we have to calculate the offset of line 1: we have b from a+bx. Since Point a is on that line, it is:
//a + b * a.x = a.y with a as free variable, which yields a = a.y - b*a.x.
//One could also use the second point b for this calculation.
double firstOffset = a.y() - firstGradient * a.x();
double secondGradient, secondOffset;
Point dx = d.sub(c);
secondGradient = dx.y() / dx.x(); //same formula as above
secondOffset = c.y() - secondGradient * c.x();
//a+bx=c+dx <=> a-c = (d-b)x <=> (a-c)/(d-b) = x
double x = (firstOffset - secondOffset) / (secondGradient - firstGradient);
//we have to check, if those lines intersect with a x \in [a.x,b.x] and x \in [c.x,d.x]. If this is the case, we have a collision
if (x >= a.x() && x <= b.x() && x >= c.x() && x <= d.x()) {
return true;
}
return false;
}
int main(int argc, char const *argv[]) {
if (checkforIntersection(Point(310.374,835.171),Point(290.434,802.354), Point(333.847,807.232), Point(301.03,827.172)) == true) {
cout << "These lines do intersect so I should be printed out\n";
} else {
cout << "The algorithm does not work, so instead I do get printed out\n";
}
return 0;
}
So as example I took the points ~ (310,835) -- (290,802), and (333,807) -- (301,827). These lines do intersect:
\documentclass[crop,tikz]{standalone}
\begin{document}
\begin{tikzpicture}[x=.1cm,y=.1cm]
\draw (310,835) -- (290,802);
\draw (333,807) -- (301,827);
\end{tikzpicture}
\end{document}
Proof of intersection
However when running the above C++ code, it says that they do not intersect
(you may call me a pedant, but the terminology is important)
If you want to see if the line segments intersect, then rely on the parametric representation of your two segments, solve the system in the two parameters and see if both of the solution for both of the parameters falls into [0,1] range.
Parametric representation for segment [a, b], component-wise
{x, y}(t) = {(1-t)*ax+t*bx, (1-t)*ay+t*by} with t in the [0,1] range
Quick check - at t=0 you get a, at t=1 you get b, the expression is linear in t, so there you have it.
So, your (a,b) (c,d) intersection problem becomes:
// for some t1 and t2, the x coordinate is the same
(1-t1)*ax+t*bx=(1-t2)*cx+t2*dx;
(1-t1)*ay+t*by=(1-t2)*cy+t2*dy; // and so is the y coordinate
Solve the system in t1 and t2. If t1 is in the [0,1] range, then the intersection lies between a and b, the same goes for t2 in respect with c and d.
It is left as an exercise for the reader the study of what effects will have on the system above the following conditions and what checks should be implemented for a robust algorithm:
segment degeneracy - coincident ends for one or both segments
collinear segments with non-void overlap. Particular case when there's a single point of overlap (necessary, that point will be one of the ends)
collinear segments with no overlap
parallel segments
First it calculates the gradient of the lines by calculating deltay/deltax.
And what happens when deltax is very close to zero?
Look, what you are doing is exposing yourself to ill-conditioned situations - always fear divisions and straight comparison with 0.0 when it comes to computational geometry.
Alternative:
two lines will intersect if they are not parallel
two distinct lines will be parallel if their definition vectors will have a zero cross-product.
Cross-product of your (a,b) x (c,d) = (ax-bx)*(cy-dy)-(ay-by)*(cx-dx) - if this is close enough to zero, for all practical purposes there's no intersection between your lines (the intersection is so far away it doesn't matter).
Now, what remains to be said:
there will need to be a "are those line distinct?" test before going into computing the cross-product. Even more, you will need to treat degenerate cases (one or both of the lines are reduced to a point by coincident ends - like a==b and/or c==d)
the "close enough to zero" test is ambiguous if you don't normalize your definition vectors - imagine a 1 lightsecond-length vector defining the first line and a 1 parsec-length vector for the other (What test for 'proximity to zero' should you use in this case?) To normalize the two vectors, just apply a division ... (a division you say? I'm already shaking with fear) ... mmm.. I was saying to divide the resulted cross-product with hypot(ax-bx, ay-by)*hypot(cx-dx,cy-dy) (do you see why the degeneracy cases need to be treated in advance?)
after the normalization, once again, what would be a good 'proximity to zero' test for the resulted cross-product? Well, I think I can go on with the analysis for another hour or so (e.g. how far the intersection would be when compared with the extent of your {a,b,c,d} polygon), but... since the cross-product of two unitary vectors (after normalization) is sin(angle-between-versors), you may use your common sense and say 'if the angle is less that 1 degree, will this be good enough to consider the two lines parallel? No? What about 1 arcsecond?"
I want to fit a plane to a 3D point cloud. I use a RANSAC approach, where I sample several points from the point cloud, calculate the plane, and store the plane with the smallest error. The error is the distance between the points and the plane. I want to do this in C++, using Eigen.
So far, I sample points from the point cloud and center the data. Now, I need to fit the plane to the samples points. I know I need to solve Mx = 0, but how do I do this? So far I have M (my samples), I want to know x (the plane) and this fit needs to be as close to 0 as possible.
I have no idea where to continue from here. All I have are my sampled points and I need more data.
From you question I assume that you are familiar with the Ransac algorithm, so I will spare you of lengthy talks.
In a first step, you sample three random points. You can use the Random class for that but picking them not truly random usually gives better results. To those points, you can simply fit a plane using Hyperplane::Through.
In the second step, you repetitively cross out some points with large Hyperplane::absDistance and perform a least-squares fit on the remaining ones. It may look like this:
Vector3f mu = mean(points);
Matrix3f covar = covariance(points, mu);
Vector3 normal = smallest_eigenvector(covar);
JacobiSVD<Matrix3f> svd(covariance, ComputeFullU);
Vector3f normal = svd.matrixU().col(2);
Hyperplane<float, 3> result(normal, mu);
Unfortunately, the functions mean and covariance are not built-in, but they are rather straightforward to code.
Recall that the equation for a plane passing through origin is Ax + By + Cz = 0, where (x, y, z) can be any point on the plane and (A, B, C) is the normal vector perpendicular to this plane.
The equation for a general plane (that may or may not pass through origin) is Ax + By + Cz + D = 0, where the additional coefficient D represents how far the plane is away from the origin, along the direction of the normal vector of the plane. [Note that in this equation (A, B, C) forms a unit normal vector.]
Now, we can apply a trick here and fit the plane using only provided point coordinates. Divide both sides by D and rearrange this term to the right-hand side. This leads to A/D x + B/D y + C/D z = -1. [Note that in this equation (A/D, B/D, C/D) forms a normal vector with length 1/D.]
We can set up a system of linear equations accordingly, and then solve it by an Eigen solver as follows.
// Example for 5 points
Eigen::Matrix<double, 5, 3> matA; // row: 5 points; column: xyz coordinates
Eigen::Matrix<double, 5, 1> matB = -1 * Eigen::Matrix<double, 5, 1>::Ones();
// Find the plane normal
Eigen::Vector3d normal = matA.colPivHouseholderQr().solve(matB);
// Check if the fitting is healthy
double D = 1 / normal.norm();
normal.normalize(); // normal is a unit vector from now on
bool planeValid = true;
for (int i = 0; i < 5; ++i) { // compare Ax + By + Cz + D with 0.2 (ideally Ax + By + Cz + D = 0)
if ( fabs( normal(0)*matA(i, 0) + normal(1)*matA(i, 1) + normal(2)*matA(i, 2) + D) > 0.2) {
planeValid = false; // 0.2 is an experimental threshold; can be tuned
break;
}
}
This method is equivalent to the typical SVD-based method, but much faster. It is suitable for use when points are known to be roughly in a plane shape. However, the SVD-based method is more numerically stable (when the plane is far far away from origin) and robust to outliers.
I was doing a practice question and it was something like this,We are given N pair of coordinates (x,y) and we are given a central point too which is (x0,y0).We were asked to find maximum number of points lying on a line passing from (x0,y0).
My approach:- I tried to maintain a hash map having slope as the key and I thought to get the maximum second value to get maximum number of points on the same line.Something like this
mp[(yi-y0)/(xi-x0))]++; //i from 0 to n
And iterating map and doing something line this
if(it->second >max) //it is the iterator
max=it->second;
and printing max at last;
Problem With my approach- Whenever I get (xi-x0) as 0 I get runtime error.I also tried atan(slope) so that i would get degrees instead of some not defined value but still its not working.
What i expect->How to remove this runtime error and is my approach correct for finding maximum points on a line passing from a point(x0,y0).
P.S -My native language is not english so please ignore if something goes wrong.I tried my best to make everything clear If i am not clear enough please tell me
I'm assuming no other points have the same coordinates as your "origin".
If all your coordinates happen to be integers, you can keep a rational number (i.e. a pair of integers, i.e. a numerator and a denominator) as the slope, instead of a single real number.
The slope is DeltaY / DeltaX, so all you have to do is keep the pair of numbers separate. You just need to take care to divide the pair by their greatest common divisor, and handle the case where DeltaX is zero. For example:
pair<int, int> CalcSlope (int x0, int y0, int x1, int y1)
{
int dx = abs(x1 - x0), dy = abs(y1 - y0);
int g = GCD(dx, dy);
return {dy / g, dx / g};
}
Now just use the return value of CalcSlope() as your map key.
In case you need it, here's one way to calculate the GCD:
int GCD (int a, int b)
{
if (0 == b) return a;
else return gcd(b, a % b);
}
You have already accepted an answer, but I would like to share my approach anyway. This method uses the fact that three points a, b, and c are covariant if and only if
(a.first-c.first)*(b.second-c.second) - (a.second-c.second)*(b.first-c.first) == 0
You can use this property to create a custom comparison object like this
struct comparePoints {
comparePoints(int x0 = 0, int y0 = 0) : _x0(x0), _y0(y0) {}
bool operator()(const point& a, const point& b) {
return (a.first-_x0)*(b.second-_y0) - (b.first-_x0)*(a.second-_y0) < 0;
}
private:
int _x0, _y0;
};
which you can then use as a comparison object of a map according to
comparePoints comparator(x0, y0);
map<pair<int, int>, int, comparePoints> counter(comparator);
You can then add points to this map similar to what you did before:
if (!(x == x0 && y == y0))
counter[{x,y}]++;
By using comparitor as a comparison object, two keys a, b in the map are considered equal if !comparator(a, b) && !comparator(b,a), which is true if and only if a, b and {x0,y0} are collinear.
The advantage of this method is that you don't need to divide the coordinates which avoids rounding errors and problems with dividing by zero, or calculate the atan which is a costly operation.
Move everything so that the zero point is at the origin:
(xi, yi) -= (x0, y0)
Then for each point (xi, yi), find the greatest common divisor of xi and yi and divide both numbers by it:
k = GCD(xi, yi)
(xi', yi`) = (yi/k, yi/k)
Now points that are on the same ray will map to equal points. If (xi, yi) is on the same ray as (xj, yj) then (xi', yi') = (xj', yj').
Now find the largest set of equal points (don't forget any (xi, yi) = (0, 0)) and you have your answer.
You've a very original approach here !
Nevertheless, a vertical line has a infinite slope and this is the problem here: dividing by 0 is not allowed.
Alternative built on your solution (slope):
...
int mpvertical=0; // a separate couner for verticals
if (xi-x0)
mp[(yi-y0)/(xi-x0))]++;
else if (yi-y0)
mpvertical++;
// else the point (xi,yi) is the point (x0,y0): it shall not be counted)
This is cumbersome, because you have everything in the map plus this extra counter. But it will be exact. A workaround could be to count such points in mp[std::numeric_limits<double>::max()], but this would be an approximation.
Remark: the case were xi==x0 AND yi==y0 corresponds to your origin point. These points have to be discarded as they belong to every line line.
Trigonomic alternative (angle):
This uses the general atan2 formula used to converting cartesian coordinates into polar coordinates, to get the angle:
if (xi!=x0 && yi!=y0) // the other case can be ignored
mp[ 2*atan((yi-y0)/((xi-x0)+sqrt(pow(xi-x0,2)+pow(yi-y0,2)))) ]++;
so your key for mp will be an angle between -pi and +pi. No more extra case, but slightly more calculations.
You can hide these extra details and use the slighltly more optimized build in function:
if (xi!=x0 && yi!=y0) // the other case can be ignored
mp[ atan2(yi-y0, xi-x0) ]++;
you can give this approach a try
struct vec2
{
vec2(float a,float b):x(a),y(b){}
float x,y;
};
bool isColinear(vec2 a, vec2 b, vec2 c)
{
return fabs((a.y-b.y)*(a.x-c.x) - (a.y-c.y)*(a.x-b.x)) <= 0.000001 ;
}
Ok, I'm having a bit of trouble finding a solution for this that seems to be a simple geometry problem.
I have a list of triple coordinates that form a square angle.
Between all these triple-coordinates I want to find a pair that forms up a square.
I believe the best I can do to exemplify is show an image:
and 2. are irrelevant. 3. and 4. are what I'm looking for.
For each triple coordinate I have the midle point, where the angle is, and two other points that describe the two segments that form the angle.
Summing it up, given six points, 2 for the diagonal + 4 other points, how can I find if these make a square?
obs: the two lines that make the angle are consistent but don't have the same size.
obs2:the lines from different triples may not intersect
Thank you for time and any help and insight provided.
If any term I used is incorrect or just plain hard to understand let me know, I'm not a native english speaker.
Edit: The code as it stands.
//for all triples
for (size_t i = 0; i < toTry.size() - 1; i++) {
Vec2i center_i = toTry[i].avg;
//NormalizedDiagonal = ((Side1 - Center) + (Side2 - Center));
Vec2i a = toTry[i].p, b = toTry[i].q;
Vec2f normalized_i = normalizedDiagonal(center_i, toTry[i].p, toTry[i].q);
for (size_t j = i + 1; j < toTry.size(); j++) {
Vec2i center_j = toTry[j].avg;
//Se os pontos sao proximos, nao importam
if (areClose(center_i, center_j, 25))
continue;
Vec2f normalized_j = normalizedDiagonal(center_j, toTry[j].p, toTry[j].q);
line(src, Point(center_i[0], center_i[1]), Point(center_i[0] + 1 * normalized_i[0], center_i[1] + 1 * normalized_i[1]), Scalar(255, 255, 255), 1);
//test if antiparallel
if (abs(normalized_i[0] - normalized_j[0]) > 0.1 || abs(normalized_i[1] - normalized_j[1] > 0.1))
continue;
Vec2f delta;
delta[0] = center_j[0] - center_i[0]; delta[1] = center_j[1] - center_i[1];
double dd = sqrt(pow((center_i[0] - center_j[0]), 2) + pow((center_i[1] - center_j[1]), 2));
//delta[0] = delta[0] / dd;
//delta[1] = delta[1] / dd;
float dotProduct = normalized_i[0] * delta[0] + normalized_i[1] * delta[1];
//test if do product < 0
if (dotProduct < 0)
continue;
float deltaDotDiagonal = delta[0] * normalized_i[0] + delta[1] * normalized_i[1];
menor_d[0] = delta[0] - deltaDotDiagonal * normalized_i[0];
menor_d[1] = delta[1] - deltaDotDiagonal * normalized_i[1];
dd = sqrt(pow((center_j[0] - menor_d[0]), 2) + pow((center_j[1] - menor_d[1]), 2));
if(dd < 25)
[...]
Just to be clear, the actual lengths of the side segments is irrelevant, right? All you care about is whether the semi-infinite lines formed by the side segments of two triples form a square? Or do the actual segments need to intersect?
Assuming the former, a method to check whether two triples form a square is as follows. Let's use the Point3D and Vector3D from the System.Windows.Media.Media3D namespace to define some terminology, since these are decent general-purpose 3d double precision points and vectors that support basic linear algebra methods. These are c# so you can't use them directly but I'd like to be able to refer to some of the basic methods mentioned there.
Here is the basic method to check if two triples intersect:
Define a triple as follows: Center, Side1 and Side2 as three Point3D structures.
For each triple, define the normalized diagonal vector as
NormalizedDiagonal = ((Side1 - Center) + (Side2 - Center));
NormalizedDiagonal.Normalize()
(You might want to cache this for performance.)
Check if the two centers are equal within some linear tolerance you define. If equal, return false -- it's a degenerate case.
Check if the two diagonal vectors are antiparallel within some angular tolerance you define. (I.e. NormalizedDiagonal1 == -NormalizedDiagonal2 with some tolerance.) If not, return false, not a square.
Compute the vector from triple2.Center to triple2.Center: delta = triple2.Center - triple1.Center.
If double deltaDotDiagonal = DotProduct(delta, triple1.NormalizedDiagonal) < 0, return false - the two triples point away from each other.
Finally, compute the distance from the center of triple2 to the (infinite) diagonal line passing through the center triple1. If zero (within your linear tolerance) they form a square.
To compute that distance: distance = (delta - deltaDotDiagonal*triple1.NormalizedDiagonal).Length
Note: deltaDotDiagonal*triple1.NormalizedDiagonal is the projection of the delta vector onto triple1.NormalizedDiagonal, and thus delta - deltaDotDiagonal*triple1.NormalizedDiagonal is the component of delta that is perpendicular to that diagonal. Its length is the distance we seek.
Finally, If your definition of a square requires that the actual side segments intersect, you can add an extra check that the lengths of all the side segments are less than sqrt(2) * delta.Length.
This method checks if two triples form a square. Finding all triples that form squares is, of course, O(N-squared). If this is a problem, you can put them in an array and sort then by angle = Atan2(NormalizedDiagonal.Y, NormalizedDiagonal.X). Having done that, you can find triples that potentially form squares with a given triple by binary-searching the array for triples with angles = +/- π from the angle of the current triple, within your angular tolerance. (When the angle is near π you will need to check both the beginning and end of the array.)
Update
OK, let's see if I can do this with your classes. I don't have definitions for Vec2i and Vec2f so I could get this wrong...
double getLength(Vec2f vector)
{
return sqrt(pow(vector[0], 2) + pow(vector[1], 2));
}
Vec2f scaleVector(Vec2f vec, float scale)
{
Vec2f scaled;
scaled[0] = vec[0] * scale;
scaled[1] = vec[1] * scale;
return scaled;
}
Vec2f subtractVectorsAsFloat(Vec2i first, Vec2i second)
{
// return first - second as float.
Vec2f diff;
diff[0] = first[0] - second[0];
diff[1] = first[1] - second[1];
return diff;
}
Vec2f subtractVectorsAsFloat(Vec2f first, Vec2f second)
{
// return first - second as float.
Vec2f diff;
diff[0] = first[0] - second[0];
diff[1] = first[1] - second[1];
return diff;
}
double dot(Vec2f first, Vec2f second)
{
return first[0] * second[0] + first[1] * second[1];
}
//for all triples
for (size_t i = 0; i < toTry.size() - 1; i++) {
Vec2i center_i = toTry[i].avg;
//NormalizedDiagonal = ((Side1 - Center) + (Side2 - Center));
Vec2i a = toTry[i].p, b = toTry[i].q;
Vec2f normalized_i = normalizedDiagonal(center_i, toTry[i].p, toTry[i].q);
for (size_t j = i + 1; j < toTry.size(); j++) {
Vec2i center_j = toTry[j].avg;
//Se os pontos sao proximos, nao importam
if (areClose(center_i, center_j, 25))
continue;
Vec2f normalized_j = normalizedDiagonal(center_j, toTry[j].p, toTry[j].q);
//test if antiparallel
if (abs(normalized_i[0] - normalized_j[0]) > 0.1 || abs(normalized_i[1] - normalized_j[1] > 0.1))
continue;
// get a vector pointing from center_i to center_j.
Vec2f delta = subtractVectorsAsFloat(center_j, center_i);
//test if do product < 0
float deltaDotDiagonal = dot(delta, normalized_i);
if (deltaDotDiagonal < 0)
continue;
Vec2f deltaProjectedOntoDiagonal = scaleVector(normalized_i, deltaDotDiagonal);
// Subtracting the dot product of delta projected onto normalized_i will leave the component
// of delta which is perpendicular to normalized_i...
Vec2f distanceVec = subtractVectorsAsFloat(deltaProjectedOntoDiagonal, center_j);
// ... the length of which is the distance from center_j
// to the diagonal through center_i.
double distance = getLength(distanceVec);
if(distance < 25) {
}
}
There are two approaches to solving this. One is a very direct approach that involves finding the intersection of two line segments.
You just use the triple coordinates to figure out the midpoint, and the two line segments that protrude from it (trivial). Do this for both triple-sets.
Now calculate the intersection points, if they exist, for all four possible permutations of the extending line segments. From the original answer to a similar question:
You might look at the code I wrote for Computational Geometry in C,
which discusses this question in detail (Chapter 1, Section 5). The
code is available as SegSegInt from the links at that web site.
In a nutshell, I recommend a different approach, using signed area of
triangles. Then comparing appropriate triples of points, one can
distinguish proper from improper intersections, and all degenerate
cases. Once they are distinguished, finding the point of intersection
is easy.
An alternate, image processing approach, would be to render the lines, define one unique color for the lines, and then apply an seed/flood fill algorithm to the first white zone found, applying a new unique color to future zones, until you flood fill an enclosed area that doesn't touch the border of the image.
Good luck!
References
finding the intersection of two line segments in 2d (with potential degeneracies), Accessed 2014-08-18, <https://math.stackexchange.com/questions/276735/finding-the-intersection-of-two-line-segments-in-2d-with-potential-degeneracies>
In a pair of segments, call one "the base segment" and one that is obtained by rotating the base segment by π/2 counterclockwise is "the other segment".
For each triple, compute the angle between the base segment and the X axis. Call this its principal angle.
Sort triples by the principal angle.
Now for each triple with the principal angle of α any potential square-forming mate has the principal angle of α+π (mod 2π). This is easy to find by binary search.
Furthermore, for two candidate triples with vertices a and a' and principal angles α and α+π, the angle of vector aa' should be α+π/4.
Finally, if each of the four segments is at least |aa'|/√2 long, we have a square.