I have a surface Z on a X-Y grid for which I want to find the intersection point with a line. I used so far this code for finding the intersection:
x_ray = x_source + t * x_dir
y_ray = y_source + t * y_dir
z_ray = z_source + t * z_dir
height_above_plane = #(t) z_source + t * z_dir - interp2(X, Y, Z, ...
x_source + t*x_dir, y_source + t*y_dir)
t_intercept = fzero(height_above_plane, 0);
my problem is that when my surface is "wiggly", the function has several zero crossing points, and I want to find the minimal out of them.
How can I do that?
Thanks
A possible approach is to project the ray onto the XY domain and draw the corresponding Bresenham line. As you go along this line, grid cell per grid cell, you will compute the Z altitudes along the ray and check if their range overlaps the range of altitudes of the surface (i.e. the min and max value in this cell).
If yes, you have to find the 3D intersection between the ray and the interpolating surface, an hyperbolic paraboloid. If the intersection does fall inside the grid cell considered, you are done. Otherwise, continue the march along the ray.
Convert the surface to matlab mesh, then use this code.
Related
I have this 3D image generated from the simple code below.
% Input Image size
imageSizeY = 200;
imageSizeX = 120;
imageSizeZ = 100;
%# create coordinates
[rowsInImage, columnsInImage, pagesInImage] = meshgrid(1:imageSizeY, 1:imageSizeX, 1:imageSizeZ);
%# get coordinate array of vertices
vertexCoords = [rowsInImage(:), columnsInImage(:), pagesInImage(:)];
centerY = imageSizeY/2;
centerX = imageSizeX/2;
centerZ = imageSizeZ/2;
radius = 28;
%# calculate distance from center of the cube
sphereVoxels = (rowsInImage - centerY).^2 + (columnsInImage - centerX).^2 + (pagesInImage - centerZ).^2 <= radius.^2;
%# Now, display it using an isosurface and a patch
fv = isosurface(sphereVoxels,0);
patch(fv,'FaceColor',[0 0 .7],'EdgeColor',[0 0 1]); title('Binary volume of a sphere');
view(45,45);
axis equal;
grid on;
xlabel('x-axis [pixels]'); ylabel('y-axis [pixels]'); zlabel('z-axis [pixels]')
I have tried plotting the image with isosurface and some other volume visualization tools, but there remains quite a few surprises for me from the plots.
The code has been written to conform to the image coordinate system (eg. see: vertexCoords) which is a left-handed coordinate system I presume. Nonetheless, the image is displayed in the Cartesian (right-handed) coordinate system. I have tried to see this displayed as the figure below, but that’s simply not happening.
I am wondering if the visualization functions have been written to display the image the way they do.
Image coordinate system:
Going forward, there are other aspects of the code I am to write for example if I have an input image sphereVoxels as in above, in addition to visualizing it, I would want to find north, south east, west, top and bottom locations in the image, as well as number and count the coordinates of the vertices, plus more.
I foresee this would likely become confusing for me if I don’t stick to one coordinate system, and considering that the visualization tools predominantly use the right-hand coordinate system, I would want to stick with that from the onset. However, I really do not know how to go about this.
Right-hand coordinate system:
Any suggestions to get through this?
When you call meshgrid, the dimensions x and y axes are switched (contrary to ndgrid). For example, in your case, it means that rowsInImage is a [120x100x200] = [x,y,z] array and not a [100x120x200] = [y,x,z] array even if meshgrid was called with arguments in the y,x,z order. I would change those two lines to be in the classical x,y,z order :
[columnsInImage, rowsInImage, pagesInImage] = meshgrid(1:imageSizeX, 1:imageSizeY, 1:imageSizeZ);
vertexCoords = [columnsInImage(:), rowsInImage(:), pagesInImage(:)];
I have a list of X,Y coordinates that represents a road. For every 5 meters, I need to calculate the angle of the tangent on this road, as I have tried to illustrate in the image.
My problem is that this road is not represented by a mathematical function that I can simply derive, it is represented by a list of coordinates (UTM33N).
In my other similar projects we use ArcGIS/ESRI libraries to perform geographical functions such as this, but in this project I need to be independent of any software that require the end user to have a license, so I need to do the calculations myself (or find a free/open source library that can do it).
I am using a cubic spline function to make the line rounded between the coordinates, since all tangents on a line segment would just be parallell to the segment otherwise.
But now I am stuck. I am considering simply calculating the angle between any three points on the line (given enough points), and using this to find the tangents, but that doesn't sound like a good method. Any suggestions?
In the end, I concluded that the points were plentiful enough to give an accurate angle using simple geometry:
//Calculate delta values
var dx = next.X - curr.X;
var dy = next.Y - curr.Y;
var dz = next.Z - curr.Z;
//Calculate horizontal and 3D length of this segment.
var hLength = Math.Sqrt(dx * dx + dy * dy);
var length = Math.Sqrt(hLength * hLength + dz * dz);
//Calculate horizontal and vertical angles.
hAngle = Math.Atan(dy/dx);
vAngle = Math.Atan(dz/hLength);
I have data that records the x and y positions of an animal in a 2D assay over time stored in a matlab matrix. I can plot these co-ordinates over time, and extract the velocity information and plot this using cline.
The problem I am having at the moment is calculating the heading angle. It should be a trivial trigonometry question, but I am drawing a blank on the best way to start.
The data is stored in a matrix xy representing x and y co-ordinates:
796.995391705069 151.755760368664
794.490825688073 150.036697247706
788.098591549296 145.854460093897
786.617021276596 144.327659574468
781.125000000000 140.093750000000
779.297872340426 138.072340425532
775.294642857143 133.879464285714
What I would like to be able to do is know the angle of the line drawn from (796.995, 151.755) to (794.490, 150.036), and so on. My research suggests atan2 will be the appropriate function, but I am unsure how to call it correctly to give useful information.
difx = xy(1,1) - xy(2,1);
dify = xy(1,2) - xy(2,2);
angle = atan2(dify,difx);
angle = angle*180/pi % convert to degrees
The result is 34.4646. Is this correct?
If it is correct, how do I get the value to be in the range 0-360?
You can use the diff function to get all the differences at once:
dxy = diff(xy); % will contain [xy(2,1)-xy(1,1) xy(2,2)-xy(1,2); ...
Then you compute the angle using the atan2 function:
a = atan2(dxy(:,2), dxy(:,1));
You convert to degrees with
aDeg = 180 * a / pi;
And finally take the angle modulo 360 to get it between 0 and 360:
aDeg = mod(aDeg, 360);
So - you pretty much got it right, yes. Except that you have calculated the heading from point 2 to point 1, and I suspect you want to start at 1 and move towards 2. That would give you a negative number - or modulo 360, an angle of about 325 degrees.
Also, using the diff function gets you the entire array of headings all at once which is a slight improvement over your code.
[rc mi]=
EDIT the problem of "phase wrapping" - when the heading goes from 359 to 0 - is quite a common problem. If you are interested in knowing when a large change happens, you can try the following trick (using aDeg from above - angle in degrees).
dDeg1 = diff(aDeg); % the change in angle
dDeg2 = diff(mod(aDeg + 90, 360)); % we moved the phase wrap point by 180 degrees
dDeg12 = [dDeg1(:) dDeg2(:)]';
[rc mi]= min(abs(dDeg12));
indx = sub2ind(size(dDeg12), mi, 1:size(dDeg12, 2));
result = dDeg12(ii);
What I did there: one of the variables (dDeg or dDeg2) does not see the phase wrap, and the min function finds out which one (it will have a smaller absolute difference). The sub2ind looks up that number (it is either positive or negative - but it's the smaller one of the two), and that is the value that ends up in result.
You can verify the angle by plotting a little line that starts at the first point and end in the direction of the heading. If the angle is correct, it will point in the direction of the next point in xy. Everything depends on where yo define 0 degrees at (straight up, say) from and whether positive degrees is rotation counterclockwise (I do) or clockwise. In MATLAB you can get the numbers between 0 and 360 but using modulo---or you can just add 180 to your results but this will change the definition of where the 0 degree mark is.
I made the following script that is a bit complex but shows how to calculate the heading/angle for all points in vector format and then displays them.
xy =[ 796.995391705069 151.755760368664
794.490825688073 150.036697247706
788.098591549296 145.854460093897
786.617021276596 144.327659574468
781.125000000000 140.093750000000
779.297872340426 138.072340425532
775.294642857143 133.879464285714];
% t = linspace(0,3/2*pi, 14)';
% xy = [sin(t), cos(t)];
% calculate the angle:
myDiff = diff(xy);
myAngle = mod(atan2(myDiff(:,1), myDiff(:,2))*180/pi, 360);
% Plot the original Data:
figure(1);
clf;
subplot(1,3,1);
plot(xy(:,1), xy(:,2), '-bx', 'markersize', 12);
hold all
axis equal;grid on;
title('Original Data');
% Plot the calculated angle:
subplot(1,3,2);
plot(myAngle);
axis tight; grid on;
title('Heading');
% Now plot the result with little lines pointing int he heading:
subplot(1,3,3);
plot(xy(:,1), xy(:,2), '-bx', 'markersize', 12);
hold all
% Just for visualization:
vectorLength = max(.8, norm(xy(1,:)- xy(2,:)));
for ind = 1:length(xy)-1
startPoint = xy(ind,:)';
endPoint = startPoint + vectorLength*[sind(myAngle(ind)); cosd(myAngle(ind))];
myLine = [startPoint, endPoint];
plot(myLine(1,:), myLine(2, :), ':r ', 'linewidth', 2)
end
axis equal;grid on;
title('Original Data with Heading Drawn On');
For example, if you use my test data
t = linspace(0,3/2*pi, 14)';
xy = [sin(t), cos(t)];
You get the following:
and if you do yours you get
Note how the little red line starts at the original data point and moves in the direction of the next point---just like the original blue line connecting the points.
Also note that the use of diff in the code to difference all the points properly at once. This is faster and avoids any problems with the direction--looks like in your case it's swapped.
I need to make a small program that draws three circles, a line between the first two, and then determines if the third touches or intersects the line. I have done everything but the last part. I am trying to use the points to determine if the area is 0, which would mean that the third point is, in fact, intersecting the line. Right? Or I could use another way. Technically the third circle can be within 3 pixels of the line. The problem is near the bottom at the hashtag. I would appreciate any help or suggestions that move this in another direction. Thank you.
import turtle
x1, y1 = eval(input("Enter coordinates for the first point x, y: "))
x2, y2 = eval(input("Enter coordinates for the second point x, y: "))
x3, y3 = eval(input("Enter coordinates for the third point x, y: "))
turtle.penup()
turtle.goto(x1, y1)
turtle.pendown()
turtle.circle(3)
turtle.penup()
turtle.goto(x2, y2)
turtle.pendown()
turtle.circle(3)
turtle.penup()
turtle.goto(x3, y3)
turtle.pendown()
turtle.circle(3)
turtle.penup()
turtle.color("red")
turtle.goto(x1, y1)
turtle.pendown()
turtle.goto(x2, y2)
a = (x1, y1)
c = (x3, y3)
#can't multiply sequence by non-int of type 'tuple'
area = (a * c) / 2
if area == 0:
print("Hit")
else:
print("Miss")
Ther center of 3rd circle is (x3,y3) and have a radius 3 and you are trying to determine any intersection with ([x1,y1],[x2,y2]) line segment.
If any point in the line is within the circle, then there is an intersection.
Circle region formula is: (x-x3)^2 + (y-y3)^2 < 3^2
You should test for every point on the line whether this inequality holds and if any single point satisfies this condition, then you can conclude that the line and circle intersect.
The first step would be to determine coordinate points of line segment (all points between [x1,y1],[x2,y2] points in a straight line) then you can try to test these points in a loop.
You can calculate the area of the triangle by defining vectors from one vertex to the other two (adding a third constant coordinate to embed the plane in 3-dimensional space (so cross-products make sense)),
#pseudocode
b = (x2, y2, 1) - (x1, y1, 1) = (x2-x1, y2-y1, 0)
c = (x3, y3, 1) - (x1, y1, 1) = (x3-x1, y3-y1, 0)
then take the cross-product of these,
a = b cross c = (by*cz-bz*cy, bz*cx-bx*cz, bx*cy-by*cx)
then take the magnitude of this resulting vector which is the area of the parallelogram defined by the two vectors,
pa = |a| = ax^2 + ay^2 + az^2
then divide by two to get the area of the triangle (half of the parallelogram).
ta = pa/2
Source: http://en.wikipedia.org/wiki/Triangle_area#Using_vectors
Am I wright? The position of the circles to each other does not matter?
Make a linear function from the line between the two center points. (ax+b=y)
Where a is the gradient and b is the y-intersection.
To rotate a through 90° is easy. Inverse and negate a.
Find b of the second linear function. b'=y-a'*x .
At once replace the x,y with the coordinates of your 3. circle point.
Now you have a linear function which is rectangular to the old one and where the third circle point is part of.
Intersect the old linear function with the new one.
You'll get the lot point.
You need to find out the distance between the 3. circle point and the lot point and whether it is greater than the radius.
You need there functions (JS):
function makelinear (x1,y1,x2,y2){
var temp=x2-x1;
if(temp==0)temp=0.00000000000001;//not clean but fast.
var a=(y2-y1)/temp,
b=y1-a*x1;
return[a,b];
}
function ninetydeg(a,b,x,y){
var aout=1/a,
bout=y+aout*x;
return [aout,bout];
}
function lineintersection(a1,b1,a2,b2){
var temp=a1-a2;
if(temp==0)temp=0.00000000000001;
var x=(b2-b1)/temp,
y=a1*x+b1;
return[x,y];
}
function distance(x1,y1,x2,y2){
var x=x1-x2,
y=y1-y2;
return(Math.sqrt(x*x+y*y));
}
Sorry for to be complicate, I've found no other solution in short time. May be there is a vector solution.
I'm solving the following problem: I have an object and I know its position now and its position 300ms ago. I assume the object is moving. I have a point to which I want the object to get.
What I need is to get the angle from my current object to the destination point in such a format that I know whether to turn left or right.
The idea is to assume the current angle from the last known position and the current position.
I'm trying to solve this in MATLAB. I've tried using several variations with atan2 but either I get the wrong angle in some situations (like when my object is going in circles) or I get the wrong angle in all situations.
Examples of code that screws up:
a = new - old;
b = dest - new;
alpha = atan2(a(2) - b(2), a(1) - b(1);
where new is the current position (eg. x = 40; y = 60; new = [x y];), old is the 300ms old position and dest is the destination point.
Edit
Here's a picture to demonstrate the problem with a few examples:
In the above image there are a few points plotted and annotated. The black line indicates our estimated current facing of the object.
If the destination point is dest1 I would expect an angle of about 88°.
If the destination point is dest2 I would expect an angle of about 110°.
If the destination point is dest3 I would expect an angle of about -80°.
Firstly, you need to note the scale on the sample graph you show above. The x-axis ticks move in steps of 1, and the y-axis ticks move in steps of 20. The picture with the two axes appropriately scaled (like with the command axis equal) would be a lot narrower than you have, so the angles you expect to get are not right. The expected angles will be close to right angles, just a few degrees off from 90 degrees.
The equation Nathan derives is valid for column vector inputs a and b:
theta = acos(a'*b/(sqrt(a'*a) * sqrt(b'*b)));
If you want to change this equation to work with row vectors, you would have to switch the transpose operator in both the calculation of the dot product as well as the norms, like so:
theta = acos(a*b'/(sqrt(a*a') * sqrt(b*b')));
As an alternative, you could just use the functions DOT and NORM:
theta = acos(dot(a,b)/(norm(a)*norm(b)));
Finally, you have to account for the direction, i.e. whether the angle should be positive (turn clockwise) or negative (turn counter-clockwise). You can do this by computing the sign of the z component for the cross product of b and a. If it's positive, the angle should be positive. If it's negative, the angle should be negative. Using the function SIGN, our new equation becomes:
theta = sign(b(1)*a(2)-b(2)*a(1)) * acos(dot(a,b)/(norm(a)*norm(b)));
For your examples, the above equation gives an angle of 88.85, 92.15, and -88.57 for your three points dest1, dest2, and dest3.
NOTE: One special case you will need to be aware of is if your object is moving directly away from the destination point, i.e. if the angle between a and b is 180 degrees. In such a case you will have to pick an arbitrary turn direction (left or right) and a number of degrees to turn (180 would be ideal ;) ). Here's one way you could account for this condition using the function EPS:
theta = acos(dot(a,b)/(norm(a)*norm(b))); %# Compute theta
if abs(theta-pi) < eps %# Check if theta is within some tolerance of pi
%# Pick your own turn direction and amount here
else
theta = sign(b(1)*a(2)-b(2)*a(1))*theta; %# Find turn direction
end
You can try using the dot-product of the vectors.
Define the vectors 'a' and 'b' as:
a = new - old;
b = dest - new;
and use the fact that the dot product is:
a dot b = norm2(a) * norm2(b) * cos(theta)
where theta is the angle between two vectors, and you get:
cos(theta) = (a dot b)/ (norm2(a) * norm2(b))
The best way to calculate a dot b, assuming they are column vectors, is like this:
a_dot_b = a'*b;
and:
norm2(a) = sqrt(a'*a);
so you get:
cos(theta) = a'*b/(sqrt((a'*a)) * sqrt((b'*b)))
Depending on the sign of the cosine you either go left or right
Essentially you have a line defined by the points old and new and wish to determine if dest is on right or the left of that line? In which case have a look at this previous question.