Find Joint Angle for Animation

I have a stick figure animation and would like to display joint angles on the plot... I am having trouble calculating the joint angles...

The angles appear to flip (ex change from 170 degrees to 10 degrees) and at times all together wrong...

Here is an example of my code... based on code I found online...

u1(:,1)=M1(:,2);  u1(:,2)=M2(:,2);  u1(:,3)=M3(:,2);
v1(:,1)=M1(:,1);  v1(:,2)=M2(:,1);  v1(:,3)=M3(:,1);

for i=1:length(u1);
    CosTheta1(i,1)= dot(u1(i,:),v1(i,:))/(norm(u1(i,:))*norm(v1(i,:)));
    ThetaInDegrees1(i,1)=acos(CosTheta1(i,1))*180/pi;
end

M1 is point #1, M2 is point #2 & M3 is point #3. Column 1 is Y data & Column 2 is X data. u1 & v1 are 3x1908. For my current file 'i' is is 1908.

I want to find the angle <123 with the vertex at M2.

Why am I getting wrong values? How do I prevent flipping?

Thanks ~Dan

Answer 1

[Richard Brown]

You are taking the dot products of the wrong vectors! You need to take the vectors emanating from M2. The following should do the trick

u1 = M1 - M2;
v1 = M3 - M2;

% Easiest to normalise first
u1 = bsxfun(@rdivide, u1, hypot(u1(:, 1), u1(:, 2)));
v1 = bsxfun(@rdivide, v1, hypot(v1(:, 1), v1(:, 2)));

% Can compute the angle in one vectorised hit
theta_deg = acos(dot(u1, v1, 2)) * 180/pi;

edit I initially wrote the code for the M's being transposed. Fixed now.

edit2 Missing parenthesis added in last line

[Richard Brown]

Actually, that's not 100% right - check back in a minute! I misread what your M matrices were

[Daniel]

The 'theta_deg' equation produces complex numbers...

Am I missing something? Is there another piece of code that I need to run 'theta_deg' through to get the angle?

Is it okay that M1, M2 and M3 are vectors with Y data in column 1 and X data in column 2?

Thank you for your help!

[Daniel]

Oh and u1 and v1 are all between -1 and 1...

[Daniel]

I think I figured it out...

theta_deg = acos(dot(u1, v1, 2) * 180/pi;

should be...

theta_deg = acos(dot(u1, v1, 2)) * 180/pi;

[Richard Brown]

oops! I wrote the code without actually testing it