How do I QUICKLY find the closest intersection in 2D between a ray and m polylines? - graphics

How do I find the closest intersection in 2D between a ray:
x = x0 + t*cos(a), y = y0 + t*sin(a)
and m polylines:
{(x1,y1), (x2,y2), ..., (xn,yn)}
QUICKLY?
I started by looping trough all linesegments and for each linesegment;
{(x1,y1),(x2,y2)} solving:
x1 + u*(x2-x1) = x0 + t*cos(a)
y1 + u*(y2-y1) = y0 + t*sin(a)
by Cramer's rule, and afterward sorting the intersections on distance, but that was slow :-(
BTW: the polylines happens to be monotonically increasing in x.

Coordinate system transformation
I suggest you first transform your setup to something with easier coordinates:
Take your point p = (x, y).
Move it by (-x0, -y0) so that the ray now starts at the center.
Rotate it by -a so that the ray now lies on the x axis.
So far the above operations have cost you four additions and four multiplications per point:
ca = cos(a) # computed only once
sa = sin(a) # likewise
x' = x - x0
y' = y - y0
x'' = x'*ca + y'*sa
y'' = y'*ca - x'*sa
Checking for intersections
Now you know that a segment of the polyline will only intersect the ray if the sign of its y'' value changes, i.e. y1'' * y2'' < 0. You could even postpone the computation of the x'' values until after this check. Furthermore, the segment will only intersect the ray if the intersection of the segment with the x axis occurs for x > 0, which can only happen if either value is greater than zero, i.e. x1'' > 0 or x2'' > 0. If both x'' are greater than zero, then you know there is an intersection.
The following paragraph is kind of optional, don't worry if you don't understand it, there is an alternative noted later on.
If one x'' is positive but the other is negative, then you have to check further. Suppose that the sign of y'' changed from negative to positive, i.e. y1'' < 0 < y2''. The line from p1'' to p2'' will intersect the x axis at x > 0 if and only if the triangle formed by p1'', p2'' and the origin is oriented counter-clockwise. You can determine the orientation of that triangle by examining the sign of the determinant x1''*y2'' - x2''*y1'', it will be positive for a counter-clockwise triangle. If the direction of the sign change is different, the orientation has to be different as well. So to take this together, you can check whether
(x1'' * y2'' - x2'' * y1'') * y2'' > 0
If that is the case, then you have an intersection. Notice that there were no costly divisions involved so far.
Computing intersections
As you want to not only decide whether an intersection exists, but actually find a specific one, you now have to compute that intersection. Let's call it p3. It must satisfy the equations
(x2'' - x3'')/(y2'' - y3'') = (x1'' - x3'')/(y1'' - y3'') and
y3'' = 0
which results in
x3'' = (x1'' * y1'' - x2'' * y2'')/(y1'' - y2'')
Instead of the triangle orientation check from the previous paragraph, you could always compute this x3'' value and discard any results where it turns out to be negative. Less code, but more divisions. Benchmark if in doubt about performance.
To find the point closest to the origin of the ray, you take the result with minimal x3'' value, which you can then transform back into its original position:
x3 = x3''*ca + x0
y3 = x3''*sa + y0
There you are.
Note that all of the above assumed that all numbers were either positive or negative. If you have zeros, it depends on the exact interpretation of what you actually want to compute, how you want to handle these border cases.

To avoid checking intersection with all segments, some space partition is needed, like Quadtree, BSP tree. With space partition it is needed to check ray intersection with space partitions.
In this case, since points are sorted by x-coordinate, it is possible to make space partition with boxes (min x, min y)-(max x, max y) for parts of polyline. Root box is min-max of all points, and it is split in 2 boxes for first and second part of a polyline. Number of segments in parts is same or one box has one more segment. This box splitting is done recursively until only one segment is in a box.
To check ray intersection start with root box and check is it intersected with a ray, if it is than check 2 sub-boxes for an intersection and first test closer sub-box then farther sub-box.
Checking ray-box intersection is checking if ray is crossing axis aligned line between 2 positions. That is done for 4 box boundaries.

Related

Does Piecewise Bezier curve pass vertical line test?

Consider a piecewise cubic Bezier curve with N segments, defined in terms of 4N control points. How can I determine whether this curve passes the Vertical Line Test? That is: do there exist points x,y1,y2 such that y1!=y2 and both (x,y1) and (x,y2) lie on the curve? Additionally, it would be nice but not essential to return the values of the points x,y1,y2 if such points exist.
In principle, one could just evaluate the curve explicitly at a large number of points, but in my application the curves can have a large number of segments so this is prohibitively slow. Thus I am looking for an algorithm that operates on the control points only, and does not rely on explicitly evaluating the curves at a large number of points.
Closed curves will fail your test by definition, so let's look at open curves: for a curve to fail your vertical line test, at least one segment in your piecewise cubic polyBezier needs to "reverse direction" along the x-axis, which means its x component needs to have extrema in the inclusive interval [0,1].
For example, this curve doesn't (the right side shows the component function for x, with global extrema in red and inflections in purple):
(Also note that we're only drawing the [0,1] interval, but if we were to extend it, those global extrema wouldn't actually be at t=0 and t=1. This is in fact critically important, and we'll see why, below)
This curve also doesn't (but only just):
But this curve does:
As does this one. Twice, in fact:
As does a curve "just past its cusp":
Which is easier to see when we exaggerate it:
This means we need to find the first derivative, find out where it's zero, and then make sure that (because Beziers can have cusps) the sign of that derivative flips at that point (or points, as there can be two).
Turns out, the derivative is trivially not-even-really-computed, so for each segment we have:
w = [x1, x2, x3, x4]
Bx(t) = w[0] * (1-t)³ + 3 * w[1] * t(1-t)² + w[2] * t²(1-t) + w[3] * t³
// bezier form of the derivative:
v = [
3 * (w[1] - w[0]),
3 * (w[2] - w[1]),
3 * (w[3] - w[2]),
]
Bx'(t) = v[0] * (1-t)² + 2 * v[1] * t(1-t) + v[2] * t²
Which we can trivially rewrite to polynomial form:
a = v[0] - 2*v[1] + v[2]
b = 2 * (v[1] - v[0])
c = v[0]
Bx'(t) = a * t² + b * t + c
So we find the roots for that, which is a matter of applying the quadratic formula, which gives us 0, 1, or 2 roots.
if (a == 0) there are no roots
denominator = 2 * a
discriminant = b * b - 2 * denominator * c
if (discriminant < 0) there are no (real) roots
d = sqrt(discriminant)
t1 = -(b + d) / denominator
if (0 ≤ t1 ≤ 1) then t1 is a valid root
t2 = (d - b) / denominator
if (0 ≤ t2 ≤ 1) then t2 is a valid root
If there are no (real) roots, then this segment does not cause the curve to fail your vertical line test, and we move on to the next segment and repeat until we've either found a failure, or we've run out of segments to test.
If it's 1 or 2 roots, we check that the signs of Bx'(t-ε) and Bx'(t+ε) for some very small value of ε differ (because we want to make sure we don't conclude that the above curve with a cusp fails the test: it has a zero derivative but it "keeps going in the same direction" across that root instead of flipping direction). If they do, this segment is (one of) the segment(s) that makes your curve fail your vertical line test.
Also, note that we're testing even if the root is at 0 or 1: the piecewise curve might inflect "across segments", and we can take advantage of the fact that we can evaluate Bx'(t) for t = -ε or t = 1+ε to see if we flip direction, even if we never draw the curve prior to t=0 or after t=1.
Leaving solution here for reference. Not especially elegant but gets the job done. Assume that the curve is parametrized from left to right. The key observation is that the curve intersects a vertical at two points if and only if there is a point at which the derivative x'(t) of the x component is strictly negative. For a cubic Bezier curve the derivative is a quadratic polynomial x'(t)=at^2+bt+c. So we just need to check whether the minimum of this quadratic over the interval 0<=t<=1 is negative, which is straightforward (if a bit tedious) using basic algebra.

Positioning random points on a 2D plane

So here's a little bit of geometry for you. I've been stuck on this for a while now:
I need to write a script (in C#, but feel free to answer in whatever script you'd like) that generates random points. A points has to values, x and y.
I must generate N points total (where N > 1 and is also randomly up to 100).
point 1 must be x = 0, y = 0. point 2 must be of distance 1 from point 1. So that Root(x2 + y2) = 1.
point 3 must be of distance 1 from point 2 and so on and so forth.
Now here's the tricky part - point N must be of distance 1 from point 1. So if you were to connect all points into a single shape, you'd get a closed shape with each vertices being the same length.
(vertices may cross and you may even have two points at exactly the same location. As long as it's random).
Any idea how you'd do that?
I would do it with simulation of chain there are 2 basic ways one is start from regular polygon and then randomize one point a bit (rotate a bit) then iterate the rest to maintain the segment size=1.
The second one is start with full random open chain (like in MBo answer) and then iteratively change the angles until the last point is on desired distance from first point. I think the second approach is a bit simpler to code...
If you want something more complicated then you can generate M random points and handle them as closed Bezier curve cubic patches loop control points. Then just find N equidistant points on it (this is hard task) and rescale the whole thing to match segment line size = 1
If you want to try first approach then
Regular polygon start (closed loop)
Start with regular polygon (equidistant points on circle). So divide circle to N angular segments. Select radius r so line length match l=1
so r=0.5/cos(pi/N) ... from half angle triangle
Make function to rotate i-th point by some single small step
So just rotate the i-th point around (i-1)th point with radius 1 and then iteratively change the {i+1,...N} points to match segments sizes
you can exploit symmetry to avoid bullet #2
but this will lead not to very random result for small N. Just inverse rotation of 2 touching segments for random point p(i) and loop this many times.
to make it more random you can apply symmetry on whole parts (between 2 random points) instead of on 2 lines only
The second approach is like this:
create randomized open chain (like in MBo's answer)
so all segments are already with size=1.0. Remember also the angle not just position
i-th point iteration
for simplicity let the points be called p1,p2,...pn
compute d0=||pn-p1|-1.0|
rotate point pi left by some small da angle step
compute dl=||pn-p1|-1.0|
rotate point pi right by 2.0*da
compute dr=||pn-p1|-1.0|
rotate point pi to original position ... left by da
now chose direction closer to the solution (min dl,dr,d0) so:
if d0 is minimal do not change this point at all and stop
if dl is minimal then rotate left by da while dl is lowering
if dr is minimal then rotate right by da while dr is lowering
solution
loop bullet #2 while the d=||pn-p0|-1.0| is lowering then change da to da*=0.1 and loop again. Stop if da step is too small or no change in d after loop iteration.
[notes]
Booth solutions are not precise your distances will be very close to 1.0 but can be +/- some error dependent on the last da step size. If you rotate point pi then just add/sub angle to all pi,pi+1,pi+2,..pn points
Edit: This is not an answer, closeness has not been taken into account.
It is known that Cos(Fi)^2 + Sin(Fi)^2 = 1 for any angle Fi
So you may use the next approach:
P[0].X = 0
P[0].Y = 0
for i = 1 .. N - 1:
RandomAngle = 2 * Pi * Random(0..1)
P[i].X = P[i-1].X + Cos(RandomAngle)
P[i].Y = P[i-1].Y + Sin(RandomAngle)

How to draw the normal to the plane in PCL

I have the plane equation describing the points belonging to a plane in 3D and the origin of the normal X, Y, Z. This should be enough to be able to generate something like a 3D arrow. In pcl this is possible via the viewer but I would like to actually store those 3D points inside the cloud. How to generate them then ? A cylinder with a cone on top ?
To generate a line perpendicular to the plane:
You have the plane equation. This gives you the direction of the normal to the plane. If you used PCL to get the plane, this is in ModelCoefficients. See the details here: SampleConsensusModelPerpendicularPlane
The first step is to make a line perpendicular to the normal at the point you mention (X,Y,Z). Let (NORMAL_X,NORMAL_Y,NORMAL_Z) be the normal you got from your plane equation. Something like.
pcl::PointXYZ pnt_on_line;
for(double distfromstart=0.0;distfromstart<LINE_LENGTH;distfromstart+=DISTANCE_INCREMENT){
pnt_on_line.x = X + distfromstart*NORMAL_X;
pnt_on_line.y = Y + distfromstart*NORMAL_Y;
pnt_on_line.z = Z + distfromstart*NORMAL_Z;
my_cloud.points.push_back(pnt_on_line);
}
Now you want to put a hat on your arrow and now pnt_on_line contains the end of the line exactly where you want to put it. To make the cone you could loop over angle and distance along the arrow, calculate a local x and y and z from that and convert them to points in point cloud space: the z part would be converted into your point cloud's frame of reference by multiplying with the normal vector as with above, the x and y would be multiplied into vectors perpendicular to this normal vectorE. To get these, choose an arbitrary unit vector perpendicular to the normal vector (for your x axis) and cross product it with the normal vector to find the y axis.
The second part of this explanation is fairly terse but the first part may be the more important.
Update
So possibly the best way to describe how to do the cone is to start with a cylinder, which is an extension of the line described above. In the case of the line, there is (part of) a one dimensional manifold embedded in 3D space. That is we have one variable that we loop over adding points. The cylinder is a two dimensional object so we have to loop over two dimensions: the angle and the distance. In the case of the line we already have the distance. So the above loop would now look like:
for(double distfromstart=0.0;distfromstart<LINE_LENGTH;distfromstart+=DISTANCE_INCREMENT){
for(double angle=0.0;angle<2*M_PI;angle+=M_PI/8){
//calculate coordinates of point and add to cloud
}
}
Now in order to calculate the coordinates of the new point, well we already have the point on the line, now we just need to add it to a vector to move it away from the line in the appropriate direction of the angle. Let's say the radius of our cylinder will be 0.1, and let's say an orthonormal basis that we have already calculated perpendicular to the normal of the plane (which we will see how to calculate later) is perpendicular_1 and perpendicular_2 (that is, two vectors perpendicular to each other, of length 1, also perpendicular to the vector (NORMAL_X,NORMAL_Y,NORMAL_Z)):
//calculate coordinates of point and add to cloud
pnt_on_cylinder.x = pnt_on_line.x + 0.1 * perpendicular_1.x * 0.1 * cos(angle) + perpendicular_2.x * sin(angle)
pnt_on_cylinder.y = pnt_on_line.y + perpendicular_1.y * 0.1 * cos(angle) + perpendicular_2.y * 0.1 * sin(angle)
pnt_on_cylinder.z = pnt_on_line.z + perpendicular_1.z * 0.1 * cos(angle) + perpendicular_2.z * 0.1 * sin(angle)
my_cloud.points.push_back(pnt_on_cylinder);
Actually, this is a vector summation and if we were to write the operation as vectors it would look like:
pnt_on_line+perpendicular_1*cos(angle)+perpendicular_2*sin(angle)
Now I said I would talk about how to calculate perpendicular_1 and perpendicular_2. Let K be any unit vector that is not parallel to (NORMAL_X,NORMAL_Y,NORMAL_Z) (this can be found by trying e.g. (1,0,0) then (0,1,0)).
Then
perpendicular_1 = K X (NORMAL_X,NORMAL_Y,NORMAL_Z)
perpendicular_2 = perpendicular_1 X (NORMAL_X,NORMAL_Y,NORMAL_Z)
Here X is the vector cross product and the above are vector equations. Note also that the original calculation of pnt_on_line involved a vector dot product and a vector summation (I am just writing this for completeness of the exposition).
If you can manage this then the cone is easy just by changing a couple of things in the double loop: the radius just changes along its length until it is zero at the end of the loop and in the loop distfromstart will not start at 0.

Finding the original position of a point on an image after rotation

I have the x, y co-ordinates of a point on a rotated image by certain angle. I want to find the co-ordinates of the same point in the original, non-rotated image.
Please check the first image which is simpler:
UPDATED image, SIMPLIFIED:
OLD image:
Let's say the first point is A, the second is B and the last is C. I assume you have the rotation matrice R (see Wikipedia Rotation Matrix if not) et the translation vector t, so that B = R*A and C = B+t.
It comes C = R*A + t, and so A = R^1*(C-t).
Edit: If you only need the non rotated new point, simply do D = R^-1*C.
First thing to do is defining the reference system (how "where the points lies with respect to each image" will be translated into numbers). I guess that you want to rely on a basic 2D reference system, given by a single point (a couple of X/Y values). For example: left/lower corner (min. X and min. Y).
The algorithm is pretty straightforward:
Getting the new defining reference point associated with the
rotated shape (min. X and min. Y), that is, determining RefX_new and
RefY_new.
Applying a basic conversion between reference systems:
X_old = X_new + (RefX_new - RefX_old)
Y_old = Y_new + (RefY_new -
RefY_old)
----------------- UPDATE TO RELATE FORMULAE TO NEW CAR PIC
RefX_old = min X value of the CarFrame before being rotated.
RefY_old = max Y value of the CarFrame before being rotated.
RefX_new = min X value of the CarFrame after being rotated.
RefY_new = max Y value of the CarFrame after being rotated.
X_new = X of the point with respect to the CarFrame after being rotated. For example: if RefX_new = 5 with respect to absolute frame (0,0) and X of the point with respect to this absolute frame is 8, X_new would be 3.
Y_new = Y of the point with respect to CarFrame after being rotated (equivalently to point above)
X_old_C = X_new_C(respect to CarFrame) + (RefX_new(CarFrame_C) - RefX_old(CarFrame_A))
Y_old_C = Y_new_C(respect to CarFrame) + (RefY_new(CarFrame_C) - RefY_old(CarFrame_A))
These coordinates are respect to the CarFrame and thus you might have to update them with respect to the absolute frame (0,0, I guess), as explained above, that is:
X_old_D_absolute_frame = X_old_C + (RefX_new(CarFrame_C) + RefX_global(i.e., 0))
Y_old_D_absolute_frame = Y_old_C + (RefY_new(CarFrame_C) + RefY_global(i.e., 0))
(Although you should do that once the CarFrame is in its "definitive position" with respect to the global frame, that is, on picture D (the point has the same coordinates with respect to the CarFrame in both picture C and D, but different ones with respect to the global frame).)
It might seem a bit complex put in this way; but it is really simple. You have just to think carefully about one case and create the algorithm performing all the actions. The idea is extremely simple: if I am on 8 inside something which starts in 5; I am on 3 with respect to the container.
------------ UPDATE IN THE METHODOLOGY
As said in the comment, these last pictures prove that the originally-proposed calculation of reference (max. Y/min. X) is not right: it shouldn't be the max./min. values of the carFrame but the minimum distances to the closer sides (= perpendicular line from the left/bottom side to the point).
------------ TRIGONOMETRIC CALCS FOR THE SPECIFIC EXAMPLE
The algorithm proposed is the one you should apply in any situation. Although in this specific case, the most difficult part is not moving from one reference system to the other, but defining the reference point in the rotated system. Once this is done, the application to the non-rotated case is immediate.
Here you have some calcs to perform this action (I have done it pretty quickly, thus better take it as an orientation and do it by your own); also I have only considered the case in the pictures, that is, rotation over the left/bottom point:
X_rotated = dx * Cos(alpha)
where dx = X_orig - (max_Y_CarFrame - Y_Orig) * Tan(alpha)
Y_rotated = dy * Cos(alpha)
where dy = Y_orig - X_orig * Tan(alpha)
NOTE: (max_Y_CarFrame - Y_Orig) in dx and X_orig in dy expect that the basic reference system is 0,0 (min. X and min. Y). If this is not the case, you would have to change this variables.
The X_rotated and Y_rotated give the perpendicular distance from the point to the closest side of the carFrame (respectively, left and bottom side). By applying these formulae (I insist: analyse them carefully), you get the X_old_D_absolute_frame/Y_old_D_absolute_frame that is, you have just to add the lef/bottom values from the carFrame (if it is located in 0,0, these would be the final values).

Best fit square to quadrilateral

I've got a shape consisting of four points, A, B, C and D, of which the only their position is known. The goal is to transform these points to have specific angles and offsets relative to each other.
For example: A(-1,-1) B(2,-1) C(1,1) D(-2,1), which should be transformed to a perfect square (all angles 90) with offsets between AB, BC, CD and AD all being 2. The result should be a square slightly rotated counter-clockwise.
What would be the most efficient way to do this?
I'm using this for a simple block simulation program.
As Mark alluded, we can use constrained optimization to find the side 2 square that minimizes the square of the distance to the corners of the original.
We need to minimize f = (a-A)^2 + (b-B)^2 + (c-C)^2 + (d-D)^2 (where the square is actually a dot product of the vector argument with itself) subject to some constraints.
Following the method of Lagrange multipliers, I chose the following distance constraints:
g1 = (a-b)^2 - 4
g2 = (c-b)^2 - 4
g3 = (d-c)^2 - 4
and the following angle constraints:
g4 = (b-a).(c-b)
g5 = (c-b).(d-c)
A quick napkin sketch should convince you that these constraints are sufficient.
We then want to minimize f subject to the g's all being zero.
The Lagrange function is:
L = f + Sum(i = 1 to 5, li gi)
where the lis are the Lagrange multipliers.
The gradient is non-linear, so we have to take a hessian and use multivariate Newton's method to iterate to a solution.
Here's the solution I got (red) for the data given (black):
This took 5 iterations, after which the L2 norm of the step was 6.5106e-9.
While Codie CodeMonkey's solution is a perfectly valid one (and a great use case for the Lagrangian Multipliers at that), I believe that it's worth mentioning that if the side length is not given this particular problem actually has a closed form solution.
We would like to minimise the distance between the corners of our fitted square and the ones of the given quadrilateral. This is equivalent to minimising the cost function:
f(x1,...,y4) = (x1-ax)^2+(y1-ay)^2 + (x2-bx)^2+(y2-by)^2 +
(x3-cx)^2+(y3-cy)^2 + (x4-dx)^2+(y4-dy)^2
Where Pi = (xi,yi) are the corners of the fitted square and A = (ax,ay) through D = (dx,dy) represent the given corners of the quadrilateral in clockwise order. Since we are fitting a square we have certain contraints regarding the positions of the four corners. Actually, if two opposite corners are given, they are enough to describe a unique square (save for the mirror image on the diagonal).
Parametrization of the points
This means that two opposite corners are enough to represent our target square. We can parametrise the two remaining corners using the components of the first two. In the above example we express P2 and P4 in terms of P1 = (x1,y1) and P3 = (x3,y3). If you need a visualisation of the geometrical intuition behind the parametrisation of a square you can play with the interactive version.
P2 = (x2,y2) = ( (x1+x3-y3+y1)/2 , (y1+y3-x1+x3)/2 )
P4 = (x4,y4) = ( (x1+x3+y3-y1)/2 , (y1+y3+x1-x3)/2 )
Substituting for x2,x4,y2,y4 means that f(x1,...,y4) can be rewritten to:
f(x1,x3,y1,y3) = (x1-ax)^2+(y1-ay)^2 + ((x1+x3-y3+y1)/2-bx)^2+((y1+y3-x1+x3)/2-by)^2 +
(x3-cx)^2+(y3-cy)^2 + ((x1+x3+y3-y1)/2-dx)^2+((y1+y3+x1-x3)/2-dy)^2
a function which only depends on x1,x3,y1,y3. To find the minimum of the resulting function we then set the partial derivatives of f(x1,x3,y1,y3) equal to zero. They are the following:
df/dx1 = 4x1-dy-dx+by-bx-2ax = 0 --> x1 = ( dy+dx-by+bx+2ax)/4
df/dx3 = 4x3+dy-dx-by-bx-2cx = 0 --> x3 = (-dy+dx+by+bx+2cx)/4
df/dy1 = 4y1-dy+dx-by-bx-2ay = 0 --> y1 = ( dy-dx+by+bx+2ay)/4
df/dy3 = 4y3-dy-dx-2cy-by+bx = 0 --> y3 = ( dy+dx+by-bx+2cy)/4
You may see where this is going, as simple rearrangment of the terms leads to the final solution.
Final solution

Resources