Matlab atan2 for angle between two 3D vectors - matlab

Question

I need to calculate the angle between two plane. One plane is the hand plane and the second is the forearm plane. I calculated the normals of that planes and used the formula atan2(norm(cross(var.n1,var.n2)),dot(var.n1,var.n2)); in MATLAB. I want to see the flexion/extension angle of the wrist that is characterised by positive and negative peaks but with this formula I obtain only positive peaks.
%% Script to compute the angles of wrist flexion/extension and adduction/abduction based on Vicon data
% REF: Cheryl et al., March 2008
% Order of the markers: 1.WRR 2.WRU 3.FAU 4.FAR 5.CMC2 6.CMC5 7.MCP5 8.MCP2
clc
close all
clear all
%% Initialization
% dir_kinematic = input('Path of the folder containing the kinematic files: ','s');
dir_kinematic = 'C:\Users\Utente\Desktop\TESI\Vicon\test.mat';
cd(dir_kinematic);
fileList = getAllFiles(dir_kinematic,0); % Get names of all kinematic files
f=1;
%% Conversion to angles
for f = 1:length(fileList)
if ~isempty(strfind(fileList{f},'mat')) % Take only mat files
% 0. Loading
load(fileList{f});
% 1. Filtering
frameRate = kinematic.framerate;
n = 9;
Wn = 2/(frameRate/2);
ftype = 'low';
[b,a] = butter(n,Wn,ftype);
kinematic.x = filtfilt(b,a,kinematic.x);
kinematic.y = filtfilt(b,a,kinematic.y);
kinematic.z = filtfilt(b,a,kinematic.z);
% 2. Create vectors
var.n=length(kinematic.x);
% Forearm plane
var.FAU_WRU=[kinematic.x(:,2)-kinematic.x(:,3),kinematic.y(:,2)-kinematic.y(:,3),kinematic.z(:,2)-kinematic.z(:,3)];
var.WRR_WRU=[kinematic.x(:,2)-kinematic.x(:,1),kinematic.y(:,2)-kinematic.y(:,1),kinematic.z(:,2)-kinematic.z(:,1)];
% Hand plane
var.CMC5_MCP5=[kinematic.x(:,7)-kinematic.x(:,6),kinematic.y(:,7)-kinematic.y(:,6),kinematic.z(:,7)-kinematic.z(:,6)];
var.MCP2_MCP5=[kinematic.x(:,7)-kinematic.x(:,8),kinematic.y(:,7)-kinematic.y(:,8),kinematic.z(:,7)-kinematic.z(:,8)];
% Transpose
var.FAU_WRU = var.FAU_WRU';
var.WRR_WRU = var.WRR_WRU';
var.CMC5_MCP5 = var.CMC5_MCP5';
var.MCP2_MCP5 = var.MCP2_MCP5';
% 3. Calculate angle of wrist flexion/extension
% Cross vector function for all time => create normal vector plane
var.forearm_n=[];
var.hand_n=[];
var.theta_rad=[];
for i = 1:var.n % Loop through experiment
% vector x and y of the forearm plane
var.v1=var.FAU_WRU(:,i); % take x,y,z of the vector for every time
var.v2=var.WRR_WRU(:,i);
% vector x and y of the hand plane
var.v3=var.CMC5_MCP5(:,i);
var.v4=var.MCP2_MCP5(:,i);
var.forearm_n= [var.forearm_n, cross(var.v1,var.v2)];
var.hand_n=[var.hand_n, cross(var.v3,var.v4)];
end
% Calculate angle
for i = 1:var.n
var.n1=(var.forearm_n(:,i));
var.n2=var.hand_n(:,i);
var.scalar_product(i) = dot(var.n1,var.n2);
%Equation (2) of the paper
var.theta_rad=[var.theta_rad, atan2(norm(cross(var.n1,var.n2)),dot(var.n1,var.n2))]; % result in radian
angle.flex_deflex_wrist{f}=(var.theta_rad*180)/pi;
end
% 4. Calculate angle of wrist adduction/abduction
% Projection vector onto plane
var.MCP2_MCP5_forearmproj=[];
var.WRR_WRU_forearmproj=[];
var.rad_ul_angle_rad=[];
for i=1:var.n
%take x,y,z of the vector for each time
var.v1=var.MCP2_MCP5(:,i);
var.v2=var.WRR_WRU(:,i);
% vector x and y of the forearm plane
var.vfx=var.FAU_WRU(:,i); % take x,y,z of the vector for every time
var.vfy=var.WRR_WRU(:,i);
%projection of vector MCP2_MCP5 and WRR_WRU onto forearm plane
var.squNorm1=(norm(var.vfx)*norm(var.vfx));
var.squNorm2=(norm(var.vfy)*norm(var.vfy));
var.MCP2_MCP5_forearmproj=[var.MCP2_MCP5_forearmproj,((((var.v1')*var.vfx)*var.vfx/var.squNorm1)+(((var.v1')*var.vfy)*var.vfy/var.squNorm2))];
var.WRR_WRU_forearmproj=[var.WRR_WRU_forearmproj,((var.vfx*((var.v2')*var.vfx/var.squNorm1))+(var.vfy*((var.v2')*var.vfy/var.squNorm2)))];
end
% Calculate angle
for i=1:var.n
var.n1=var.MCP2_MCP5_forearmproj(:,i)';
var.n2=var.WRR_WRU_forearmproj(:,i);
var.product=var.n1*var.n2;
var.rad_ul_angle_rad=[var.rad_ul_angle_rad, atan2(norm(cross(var.n1,var.n2)),dot(var.n1,var.n2))];% en rad
angle.rad_ul_wrist{f}=(var.rad_ul_angle_rad*180)/pi;
end
end
end
I want to know why my angles are always positive? I need to see positive and negative peaks...
Thanks for the help!

Answer 1

Since the angle between two planes is the same as that between it's normals, I will constrain the discussion to angle between two vectors.
We know that the cross product between two vectors (a and b) is another vector, perpendicular to both of them.
We are dealing with the problem of distinguishing angles -T and +T, where T is some angle.
Using the formula you used, these two angles would give the same result, due to the fundamental formula itself that is used:
atan2 (|a x b|, a.b)
This is because while a.b is the same in both cases, a x b differs, exactly in the sign of the normal to the two vectors, and nothing else (verify yourself using hand rules). When we compute the norm of this vector, information about its sign is lost, due to which the function always returns positive values.
What you can do about it
You need to keep track of the sign of a x b, to determine if the angle is positive or negative.
Note: As I'm replying from my phone I am unable to add better formatting or code, will update the answer soon.