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.
Related
so I am working for my disertation thesis and I have to detect the pupil from images using Hough Transform. So far I wrote a code that identifies 2 circles on my image, but right now I have to keep the black circle from the pupil.
When I run the code, it identifies me the pupil, but also a random circle on the cheek. My professor said that I should calculate the pixels mean and, considering the fact that the pupil is black, to keep the pixels from only that region. I don't know how to do this.
I will let my code here to have a look and if someone has an ideea on how should I write this and keep only the black pixels would be great. I also attached to this the final image to see what I obtained.
close all
clear all
path='C:\Users\Ioana PMEC\OneDrive\Ioana personal\Disertatie\test.jpg';
%Citire imagine initiala
xx = imread(path);
figure
imshow(xx)
title('Imagine initiala');% Binarizarea imaginii initiale
yy = rgb2gray(xx);
figure
imshow(yy);
title('Imagine binarizata');
e = edge(yy, 'canny');
imshow(e);
radii = 11:1:30;
h = circle_hough(e, radii, 'same', 'normalise');
peaks = circle_houghpeaks(h, radii, 'nhoodxy', 15, 'nhoodr', 21, 'npeaks', 2);
imshow(yy);
hold on;
for peak = peaks
[x, y]=circlepoints(peak(3));
plot(x+peak(1), y+peak(2), 'r-');
end
hold off
testimage
finalimage
I implemented something which should get the task done for you. The example is done with the image you provided.
Step 1: Read the file(s) and convert them to grayscale.
path = %user input;
RGB = imread(path);
lab = rgb2lab(RGB);
grayscale_image = rgb2gray(RGB);
Step 2: Do the Hough transformation with given parameters.
These, and also the sensitivity can be adapted according to your task. Hint: Play around in the Image Segmenter toolbox for fast parameter finding. Next, the inferred circles are converted to integer values, since these are required for indexing.
min_radius = 10;
max_radius = 50;
% Find circles
[centers,radii,~] = imfindcircles(RGB,[min_radius max_radius],'ObjectPolarity','dark','Sensitivity',0.95);
centers = uint16(centers);
radii = uint16(radii);
The annotated image appears as follows:
Step 3: Get the brightness values of the circles.
From the circle center and radius values, we infer their respective brightness. It is sufficient to only check for the x and y pixel values left/right and above/below the center. (-1 is just a safety margin to completely stay within the circles.)
brightness_checker = zeros(2, max(radii), 2);
for i=1:size(centers,1)
current_radii = radii(i)-1;
for j=1:current_radii
% X-center minus radius, step along x-axis
brightness_checker(i, j, 1) = grayscale_image((centers(i,2) - current_radii/2) + j,...
(centers(i,1) - radii(i)/2) + j);
% Y-center minus radius, step along y-axis
brightness_checker(i, j, 2) = grayscale_image((centers(i,2) - current_radii/2) + j,...
(centers(i,1) - current_radii/2) + j);
end
end
Step 4: Check which circle is a pupil.
The determined value of 30 could potentially be enhanced.
median_x = median(brightness_checker(:,:,1),2);
median_y = median(brightness_checker(:,:,2),2);
is_pupil = (median_x<30)&(median_y<30);
pupils_center = centers(is_pupil == true,:);
Step 5: Draw the pupils.
The marker can be changed. Refer to:
https://de.mathworks.com/help/matlab/ref/matlab.graphics.chart.primitive.line-properties.html
figure
imshow(grayscale_image);
hold on
plot(centers(:,1), centers(:,2), 'r+', 'MarkerSize', 20, 'LineWidth', 2);
hold on
plot(pupils_center(:,1), pupils_center(:,2), 'b+', 'MarkerSize', 20, 'LineWidth', 2);
This is the final output:
I want to write a code where an arc stops rotating as soon as it comes in contact with a semi-circle.
I have written a code to do so, my arc does not just touch the circle but it slightly intersects it.
I have put rotation of arc inside a while loop by using linspace() to change theta. I used polyxpoly() for finding intersection. Condition for the while loop is that as long as I have empty array the loop continues, but as soon as I get a value from polyxpoly() my loop stops.
However, at the place of touch the theta value exceeds what I needed, so as a result I get an intersection.
How do I modify the code so that the arc will touch the semi-circle and not intersect it?
Here is the output. Click the link below
Image of intersection but I need touch and not intersection
clc,clear
R = 5; % radius of a circle
r = 10; % radius of arc
aa = 60*pi/180; % arc angle
ap = 0*pi/180; % arc position angle
% defining the semi-circle about the origin
t = linspace(0,pi);
[x,y] = pol2cart(t,R); % circle data
% Shifting circle centre to (3.5,0)
x=x+3.5;
y=y+0;
% defining the arc about the origin
t1 = linspace(0,aa)-aa/2+ap;
[x1,y1] = pol2cart(t1,r); % arc data
% shifting arc-lower-end to (14,0)
delx=14-x1(1); % Finding the x difference between arc-lower-end x-coordinate & 14
dely=0-y1(1); % Finding the y difference between arc-lower-end y-coordinate & 0
x1=x1+delx;
y1=y1+dely;
theta =linspace(0,pi,1000);
i=1;
xc=[];
yc=[];
while isempty(xc)&& isempty(yc)
% create a matrix of these points, which will be useful in future calculations
v = [x1;y1];
% choose a point which will be the center of rotation
x_center = 14;
y_center = 0;
% create a matrix which will be used later in calculations
center = repmat([x_center; y_center], 1, length(x1));
% define a 60 degree counter-clockwise rotation matrix
R = [cos(theta(i)) -sin(theta(i)); sin(theta(i)) cos(theta(i))];
% do the rotation...
s = v - center; % shift points in the plane so that the center of rotation is at the origin
so = R*s; % apply the rotation about the origin
vo = so + center; % shift again so the origin goes back to the desired center of rotation
% this can be done in one line as:
% vo = R*(v - center) + center
% pick out the vectors of rotated x- and y-data
x_rotated = vo(1,:);
y_rotated = vo(2,:);
[xc,yc] = polyxpoly(x_rotated,y_rotated,x,y)
[xc1,yc1] = polyxpoly(x1,y1,x,y)
i=i+1;
end
% make a plot
plot(x,y)
hold on
plot(x1, y1, 'k-', x_rotated, y_rotated, 'r-', x_center, y_center, 'bo');
axis equal
I need to find way it contacts a circle and does not intersect it.
The code is in matlab.
Any suggestions are welcome.
This is the problem of collision detection. Most, if not all, methods I know of in collision detection requires the computer to check for intersections of some sort. It's very difficult (if not impossible) to have two objects "just touch" in simulations, because you'll need the precise (analytically solved) location of the boundaries of those objects.
polyxpoly() is a function to return the intersection of two polygons. So unfortunately, if you insist that the arc cannot touch, then you cannot use polyxpoly(). In that extreme case, you'll have to solve some mathematical equation for when the tip of the arc coincides with a point on the circle perfectly, then simulate up until that point in time.
But realistically, what you need is a finer simulation (although I personally think what you have alone is good enough). So in every simulation step, you calculate a smaller movement, so that when the arc eventually intersects, only a very small amount of the arc intersects.
I use camera calibration in matlab to detect some checkerboard patterns, after
figure; showExtrinsics(cameraParams, 'CameraCentric');
Now, I want to rotate the checkerboard patterns around the x-axis such that all of them have nearly the same y coordinates in the camera frame.
Method:
I get the positions of all patterns in the camera's frame. Then I do optimization,where the objective function is to minimize variance in y and the variable is rotation about x ranging from o to 360.
Problem:
But when I plot the transformed y-coordinates, they are even nearly in a line.
Code:
Get the checkerboad points:
%% Get rotation and translation matrices for each image;
T_cw=cell(num_imgs,1); % stores camera to world rotation and translation for each image
pixel_coordinates=zeros(num_imgs,2); % stores the pixel coordinates of each checkerboard origin
for ii=1:num_imgs,
% Calibrate the camera
im=imread(list_imgs_path{ii});
[imagePoints, boardSize] = detectCheckerboardPoints(im);
[r_wc, t_wc] = extrinsics(imagePoints, worldPoints, cameraParams);
T_wc=[r_wc,t_wc';0 0 0 1];
% World to camera matrix
T_cw{ii} = inv(T_wc);
t_cw{ii}=T_cw{ii}(1:3,4); % x,y,z coordinates in camera's frame
end
Data(num_imgs=10):
t_cw
[-1072.01388542262;1312.20387622761;-1853.34408157349]
[-1052.07856598756;1269.03455126794;-1826.73576892251]
[-1091.85978641218;1351.08261414473;-1668.88197803184]
[-1337.56358084648;1373.78548638383;-1396.87603554914]
[-1555.19509876309;1261.60428874489;-1174.63047408086]
[-1592.39596647158;1066.82210015055;-1165.34417772659]
[-1523.84307918660;963.781819272748;-1207.27444716506]
[-1614.00792252030;893.962075837621;-1114.73528985018]
[-1781.83112607964;708.973204727939;-797.185326205240]
[-1781.83112607964;708.973204727939;-797.185326205240]
Main code (Optimization and transformation):
%% Get theta for rotation
f_obj = #(x)var_ycors(x,t_cw);
opt_theta = fminbnd(f_obj,0,360);
%% Plotting (rotate ycor and check to fix theta)
y_rotated=zeros(1,num_imgs);
for ii=1:num_imgs,
y_rotated(ii)=rotate_cor(opt_theta,t_cw{ii});
end
plot(1:numel(y_rotated),y_rotated);
function var_computed=var_ycors(theta,t_cw)
ycor=zeros(1,numel(t_cw));
for ii =1:numel(t_cw),
ycor(ii)=rotate_cor(theta,t_cw{ii});
end
var_computed=var(ycor);
end
function ycor=rotate_cor(theta,mat)
r_x=[1 0 0; 0 cosd(theta) -sind(theta); 0 sind(theta) cosd(theta)];
rotate_mat=mat'*r_x;
ycor=rotate_mat(2);
end
This is a clear eigenvector problem!
Take your centroids:
t_cw=[-1072.01388542262;1312.20387622761;-1853.34408157349
-1052.07856598756;1269.03455126794;-1826.73576892251
-1091.85978641218;1351.08261414473;-1668.88197803184
-1337.56358084648;1373.78548638383;-1396.87603554914
-1555.19509876309;1261.60428874489;-1174.63047408086
-1592.39596647158;1066.82210015055;-1165.34417772659
-1523.84307918660;963.781819272748;-1207.27444716506
-1614.00792252030;893.962075837621;-1114.73528985018
-1781.83112607964;708.973204727939;-797.185326205240
-1781.83112607964;708.973204727939;-797.185326205240];
t_cw=reshape(t_cw,[3,10])';
compute PCA on them, so we know the principal conponents:
[R]=pca(t_cw);
And.... thats it! R is now the transformation matrix between your original points and the rotated coordinate system. As an example, I will draw in red the old points and in blue the new ones:
hold on
plot3(t_cw(:,1),t_cw(:,2),t_cw(:,3),'ro')
trans=t_cw*R;
plot3(trans(:,1),trans(:,2),trans(:,3),'bo')
You can see that now the blue ones are in a plane, with the best possible fit to the X direction. If you want them in Y direction, just rotate 90 degrees in Z (I am sure you can figure out how to do this with 2 minutes of Google ;) ).
Note: This is mathematically the best possible fit. I know they are not as "in a row" as one would like, but this is because of the data, this is honestly the best possible fit, as that is what the eigenvectors are!
I'm trying to estimate a position based on signal strength received from 4 Wi-Fi Access Points. I measure the signal strength from 4 access points located in each corner of a square room with 100 square meters (10x10). I recorded the signal strengths in a known position (x, y) = (9.5, 1.5) using an Android phone. Now I want to check how accurate can a multilateration method be under the circumstances.
Using MATLAB, I applied a formula to calculate distance using the signal strength. The following MATLAB function shows the application of the formula:
function [ d_vect ] = distance( RSS )
% Calculate distance from signal strength
result = (27.55 - (20 * log10(2400)) + abs(RSS)) / 20;
d_vect = power(10, result);
end
The input RSS is a vector with the four signal strengths measured in the test point (x,y) = (9.5, 1.5). The RSS vector looks like this:
RSS =
-57.6000
-60.4000
-44.7000
-54.4000
and the resultant vector with all the estimated distances to each access points looks like this:
d_vect =
7.5386
10.4061
1.7072
5.2154
Now I want to estimate my position based on these distances and the access points position in order to find the error between the estimated position and the known position (9.5, 1.5). I want to find the intersection area (In order to estimate a position) between four circles where each access point is the center of one of the circles and the distance is the radius of the circle.
I want to find the grey area as shown in this image :
http://www.biologycorner.com/resources/venn4.gif
If you want an alternative way of estimating the location without estimating the intersection of circles you can use trilateration. It is a common technique in navigation (e.g. GPS) to estimate a position given a set of distance measurements.
Also, if you wanted the area because you also need an estimate of the uncertainty of the position I would recommend solving the trilateration problem using least squares which will easily give you an estimate of the parameters involved and an error propagation to yield an uncertainty of the location.
I found an answear that solved perfectly the question. It is explained in detail in this link:
https://gis.stackexchange.com/questions/40660/trilateration-algorithm-for-n-amount-of-points
I also developed some MATLAB code for the problem. Here it goes:
Estimate distances from the Access Points:
function [ d_vect ] = distance( RSS )
result = (27.55 - (20 * log10(2400)) + abs(RSS)) / 20;
d_vect = power(10, result);
end
The trilateration function:
function [] = trilat( X, d, real1, real2 )
cla
circles(X(1), X(5), d(1), 'edgecolor', [0 0 0],'facecolor', 'none','linewidth',4); %AP1 - black
circles(X(2), X(6), d(2), 'edgecolor', [0 1 0],'facecolor', 'none','linewidth',4); %AP2 - green
circles(X(3), X(7), d(3), 'edgecolor', [0 1 1],'facecolor', 'none','linewidth',4); %AP3 - cyan
circles(X(4), X(8), d(4), 'edgecolor', [1 1 0],'facecolor', 'none','linewidth',4); %AP4 - yellow
axis([0 10 0 10])
hold on
tbl = table(X, d);
d = d.^2;
weights = d.^(-1);
weights = transpose(weights);
beta0 = [5, 5];
modelfun = #(b,X)(abs(b(1)-X(:,1)).^2+abs(b(2)-X(:,2)).^2).^(1/2);
mdl = fitnlm(tbl,modelfun,beta0, 'Weights', weights);
b = mdl.Coefficients{1:2,{'Estimate'}}
scatter(b(1), b(2), 70, [0 0 1], 'filled')
scatter(real1, real2, 70, [1 0 0], 'filled')
hold off
end
Where,
X: matrix with APs coordinates
d: distance estimation vector
real1: real position x
real2: real position y
If you have three sets of measurements with (x,y) coordinates of location and corresponding signal strength. such as:
m1 = (x1,y1,s1)
m2 = (x2,y2,s2)
m3 = (x3,y3,s3)
Then you can calculate distances between each of the point locations:
d12 = Sqrt((x1 - x2)^2 + (y1 - y2)^2)
d13 = Sqrt((x1 - x3)^2 + (y1 - y3)^2)
d23 = Sqrt((x2 - x3)^2 + (y2 - y3)^2)
Now consider that each signal strength measurement signifies an emitter for that signal, that comes from a location somewhere at a distance. That distance would be a radius from the location where the signal strength was measured, because one would not know at this point the direction from where the signal came from. Also, the weaker the signal... the larger the radius. In other words, the signal strength measurement would be inversely proportional to the radius. The smaller the signal strength the larger the radius, and vice versa. So, calculate the proportional, although not yet accurate, radius's of our three points:
r1 = 1/s1
r2 = 1/s2
r3 = 1/s3
So now, at each point pair, set apart by their distance we can calculate a constant (C) where the radius's from each location will just touch one another. For example, for the point pair 1 & 2:
Ca * r1 + Ca * r2 = d12
... solving for the constant Ca:
Ca = d12 / (r1 + r2)
... and we can do this for the other two pairs, as well.
Cb = d13 / (r1 + r3)
Cc = d23 / (r2 + r3)
All right... select the largest C constant, either Ca, Cb, or Cc. Then, use the parametric equation for a circle to find where the coordinates meet. I will explain.
The parametric equation for a circle is:
x = radius * Cos(theta)
y = radius * Sin(theta)
If Ca was the largest constant found, then you would compare points 1 & 2, such as:
Ca * r1 * Cos(theta1) == Ca * r2 * Cos(theta2) &&
Ca * r1 * Sin(theta1) == Ca * r2 * Sin(theta2)
... iterating theta1 and theta2 from 0 to 360 degrees, for both circles. You might write code like:
for theta1 in 0 ..< 360 {
for theta2 in 0 ..< 360 {
if( abs(Ca*r1*cos(theta1) - Ca*r2*cos(theta2)) < 0.01 && abs(Ca*r1*sin(theta1) - Ca*r2*sin(theta2)) < 0.01 ) {
print("point is: (", Ca*r1*cos(theta1), Ca*r1*sin(theta1),")")
}
}
}
Depending on what your tolerance was for a match, you wouldn't have to do too many iterations around the circumferences of each signal radius to determine an estimate for the location of the signal source.
So basically you need to intersect 4 circles. There can be many approaches to it, and there are two that will generate the exact intersection area.
First approach is to start with one circle, intersect it with the second circle, then intersect the resulting area with the third circle and so on. that is, on each step you know current intersection area, and you intersect it with a new circle. The intersection area will always be a region bounded by circle arcs, so to intersect it with a new circle you walk along the boundary of the area and check whether each bounding arc intersects with a new circle. If it does, then you leave only the part of the arc that lies inside a new circle, remember that you should continue with an arc from a new circle, and continue traversing the boundary until you find the next intersection.
Another approach that seems to result in a worse time complexity, but in your case of 4 circles this will not be important, is to find all the intersection points of two circles and choose only those points that are of interest for you, that is which lie inside all other circles. These points will be the corners of your area, and then it is rather easy to reconstruct the area. After googling a bit, I have even found a live demo of this approach.
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.