Matlab atan2用于两个3D向量之间的角度
我需要计算两个平面之间的角度。一个平面是手平面,第二个平面是前臂平面。我计算了该平面的法线,并使用公式Matlab atan2用于两个3D向量之间的角度,matlab,angle,euler-angles,Matlab,Angle,Euler Angles,我需要计算两个平面之间的角度。一个平面是手平面,第二个平面是前臂平面。我计算了该平面的法线,并使用公式atan2(norm(cross(var.n1,var.n2)),dot(var.n1,var.n2))在MATLAB中。我想看看手腕的弯曲/伸展角度,其特点是正负峰值,但通过这个公式,我只得到正峰值 %% Script to compute the angles of wrist flexion/extension and adduction/abduction based on Vicon
atan2(norm(cross(var.n1,var.n2)),dot(var.n1,var.n2))代码>在MATLAB中。我想看看手腕的弯曲/伸展角度,其特点是正负峰值,但通过这个公式,我只得到正峰值
%% 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
我想知道为什么我的角度总是正的?我需要看到正峰值和负峰值。。。
谢谢你的帮助 由于两个平面之间的角度与其法线之间的角度相同,因此我将把讨论限制为两个向量之间的角度
我们知道两个向量(a和b)之间的叉积是另一个向量,垂直于这两个向量
我们正在处理区分角度-T和+T的问题,其中T是某个角度
使用您使用的公式,这两个角度将给出相同的结果,因为使用的是基本公式本身:
atan2(| a x b |,a.b)
这是因为a.b在这两种情况下都是相同的,但a x b不同,正好是两个向量的法线符号不同,其他什么都没有(使用手动规则验证您自己)。当我们计算该向量的范数时,有关其符号的信息丢失,因此函数总是返回正值
你能做些什么
您需要跟踪a x b的符号,以确定角度是正还是负
注意:由于我从手机回复,我无法添加更好的格式或代码,将很快更新答案