www.gusucode.com > 三维手臂动态仿真源码程序 > 三维手臂动态仿真源码程序/DEMO_LinearBlendSkinning/loadbvh.m

    function [skeleton,time] = loadbvh(fname)
%% LOADBVH  Load a .bvh (Biovision) file.
%
% Loads BVH file specified by FNAME (with or without .bvh extension)
% and parses the file, calculating joint kinematics and storing the
% output in SKELETON.
%
% Some details on the BVH file structure are given in "Motion Capture File
% Formats Explained": http://www.dcs.shef.ac.uk/intranet/research/resmes/CS0111.pdf
% But most of it is fairly self-evident.

%% Load and parse header data
%
% The file is opened for reading, primarily to extract the header data (see
% next section). However, I don't know how to ask Matlab to read only up
% until the line "MOTION", so we're being a bit inefficient here and
% loading the entire file into memory. Oh well.

% add a file extension if necessary:
if ~strncmpi(fliplr(fname),'hvb.',4)
    fname = [fname,'.bvh'];
end

fid = fopen(fname);
C = textscan(fid,'%s');
fclose(fid);
C = C{1};


%% Parse data
%
% This is a cheap tokeniser, not particularly clever.
% Iterate word-by-word, counting braces and extracting data.

% Initialise:
skeleton = [];
ii = 1;
nn = 0;
brace_count = 1;

while ~strcmp( C{ii} , 'MOTION' )
    
    ii = ii+1;
    token = C{ii};
    
    if strcmp( token , '{' )
        
        brace_count = brace_count + 1;
        
    elseif strcmp( token , '}' )
        
        brace_count = brace_count - 1;
        
    elseif strcmp( token , 'OFFSET' )
        
        skeleton(nn).offsetFromParent = [str2double(C(ii+1)) ; str2double(C(ii+2)) ; str2double(C(ii+3))];
        ii = ii+3;
        
    elseif strcmp( token , 'CHANNELS' )
        
        skeleton(nn).Nchannels = str2double(C(ii+1));
        
        % The 'order' field is an index corresponding to the order of 'X' 'Y' 'Z'.
        % Subtract 87 because char numbers "X" == 88, "Y" == 89, "Z" == 90.
        if skeleton(nn).Nchannels == 3
            skeleton(nn).order = [C{ii+2}(1),C{ii+3}(1),C{ii+4}(1)]-87;
        elseif skeleton(nn).Nchannels == 6
            skeleton(nn).order = [C{ii+5}(1),C{ii+6}(1),C{ii+7}(1)]-87;
        else
            error('Not sure how to handle not (3 or 6) number of channels.')
        end
        
        if ~all(sort(skeleton(nn).order)==[1 2 3])
            error('Cannot read channels order correctly. Should be some permutation of [''X'' ''Y'' ''Z''].')
        end
        
        ii = ii + skeleton(nn).Nchannels + 1;
        
    elseif strcmp( token , 'JOINT' ) || strcmp( token , 'ROOT' )
        % Regular joint
        
        nn = nn+1;
        
        skeleton(nn).name = C{ii+1};
        skeleton(nn).nestdepth = brace_count;
        
        if brace_count == 1
            % root node
            skeleton(nn).parent = 0;
        elseif skeleton(nn-1).nestdepth + 1 == brace_count;
            % if I am a child, the previous node is my parent:
            skeleton(nn).parent = nn-1;
        else
            % if not, what is the node corresponding to this brace count?
            prev_parent = skeleton(nn-1).parent;
            while skeleton(prev_parent).nestdepth+1 ~= brace_count
                prev_parent = skeleton(prev_parent).parent;
            end
            skeleton(nn).parent = prev_parent;
        end
        
        ii = ii+1;
        
    elseif strcmp( [C{ii},' ',C{ii+1}] , 'End Site' )
        % End effector; unnamed terminating joint
        %
        % N.B. The "two word" token here is why we don't use a switch statement
        % for this code.
        
        nn = nn+1;
        
        skeleton(nn).name = ' ';
        skeleton(nn).offsetFromParent = [str2double(C(ii+4)) ; str2double(C(ii+5)) ; str2double(C(ii+6))];
        skeleton(nn).parent = nn-1; % always the direct child
        skeleton(nn).nestdepth = brace_count;
        skeleton(nn).Nchannels = 0;
        
    end
    
end

%% Initial processing and error checking

Nnodes = numel(skeleton);
Nchannels = sum([skeleton.Nchannels]);
Nchainends = sum([skeleton.Nchannels]==0);

% Calculate number of header lines:
%  - 5 lines per joint
%  - 4 lines per chain end
%  - 4 additional lines (first one and last three)
Nheaderlines = (Nnodes-Nchainends)*5 + Nchainends*4 + 4;

rawdata = importdata(fname,' ',Nheaderlines);

index = strncmp(rawdata.textdata,'Frames:',7);
Nframes = sscanf(rawdata.textdata{index},'Frames: %f');

index = strncmp(rawdata.textdata,'Frame Time:',10);
frame_time = sscanf(rawdata.textdata{index},'Frame Time: %f');

time = frame_time*(0:Nframes-1);

if size(rawdata.data,2) ~= Nchannels
    error('Error reading BVH file: channels count does not match.')
end

if size(rawdata.data,1) ~= Nframes
    warning('LOADBVH:frames_wrong','Error reading BVH file: frames count does not match; continuing anyway.')
end

%% Load motion data into skeleton structure
%
% We have three possibilities for each node we come across:
% (a) a root node that has displacements already defined,
%     for which the transformation matrix can be directly calculated;
% (b) a joint node, for which the transformation matrix must be calculated
%     from the previous points in the chain; and
% (c) an end effector, which only has displacement to calculate from the
%     previous node's transformation matrix and the offsetFromParent of the end
%     joint.
%
% These are indicated in the skeleton structure, respectively, by having
% six, three, and zero "channels" of data.
% In this section of the code, the channels are read in where appropriate
% and the relevant arrays are pre-initialised for the subsequent calcs.

channel_count = 0;

for nn = 1:Nnodes
    
    if skeleton(nn).parent
        skeleton(nn).head0 = skeleton(nn).offsetFromParent + skeleton(skeleton(nn).parent).head0; 
    else
        skeleton(nn).head0 = skeleton(nn).offsetFromParent; % Root Node has a global position
    end
    
    
    if skeleton(nn).Nchannels == 6 % root node 
        %NOTE: this might be the source of a bug. Although in typical
        %situations the only element that has 6 channels is the root there
        %are in fact cases where other bones also have 6 channels.
        %Specifically cases where the children are offset.
        % None of the code in this file takes this case into account
        % and so the hope is that even if there are offsets that the actual
        % skeleton stays the same throughout which would mean
        % that the xyz part of the 6 channels has no additional meaning and
        % can be ingored (which is what is done in this file...)
        
        
        % assume translational data is always ordered XYZ
        % The following is ONLY relevant for the rootnode and will be
        % overwritten later on for other nodes with 6 channels
        skeleton(nn).t_xyz = repmat(skeleton(nn).offsetFromParent,[1 Nframes]) + rawdata.data(:,channel_count+[1 2 3])';
        
        %This is valid for all 6 channel nodes
        skeleton(nn).R_xyz(skeleton(nn).order,:) = rawdata.data(:,channel_count+[4 5 6])';
        
        % Kinematics of the root element (will be overwritten for other
        % nodes with 6 channels)
        skeleton(nn).T = nan(4,4,Nframes);
        for ff = 1:Nframes
            skeleton(nn).T(:,:,ff) = transformation_matrix(skeleton(nn).t_xyz(:,ff) , skeleton(nn).R_xyz(:,ff) , skeleton(nn).order);
        end
        
    elseif skeleton(nn).Nchannels == 3 % joint node
        
        skeleton(nn).R_xyz(skeleton(nn).order,:) = rawdata.data(:,channel_count+[1 2 3])';
        skeleton(nn).t_xyz  = nan(3,Nframes);
        skeleton(nn).T = nan(4,4,Nframes);
        
    elseif skeleton(nn).Nchannels == 0 % end node
        skeleton(nn).t_xyz  = nan(3,Nframes);
    end
    
    channel_count = channel_count + skeleton(nn).Nchannels;
    
    skeleton(nn).children = [];
end

for nn = 1:Nnodes
    if skeleton(nn).parent
        skeleton(skeleton(nn).parent).children = [skeleton(skeleton(nn).parent).children nn];
    end
end

for nn = 1:Nnodes
    if skeleton(nn).Nchannels % then it has children
        skeleton(nn).tail0 = mean([skeleton(skeleton(nn).children).head0]',1)';
    end
end

for nn=1:Nnodes    

    if skeleton(nn).Nchannels
        yvec = skeleton(nn).tail0' - skeleton(nn).head0';
        zvec = [0 0 1];
        xvec = cross(yvec,zvec);
        zvec = cross(xvec,yvec);
        skeleton(nn).R0 = [xvec'/norm(xvec) yvec'/norm(yvec) zvec'/norm(zvec)];
    else
        skeleton(nn).R0 = eye(3)'
    end
end


%% Calculate kinematics
%
% No calculations are required for the root nodes.

% For each joint, calculate the transformation matrix and for convenience
% extract each position in a separate vector.
for nn = find([skeleton.parent] ~= 0 & [skeleton.Nchannels] ~= 0)
    
    parent = skeleton(nn).parent;
    
    for ff = 1:Nframes
        transM = transformation_matrix( skeleton(nn).offsetFromParent , skeleton(nn).R_xyz(:,ff) , skeleton(nn).order );
        skeleton(nn).T(:,:,ff) = skeleton(parent).T(:,:,ff) * transM;
        skeleton(nn).t_xyz(:,ff) = skeleton(nn).T([1 2 3],4,ff);
    end
    
    % In the above trans calculation the angles are relative to the base
    % rotations which means that if we multiply 
    % Thus the following gives a Rotation matrix whose columns point along the local bone
    % axes : sk(i).T(1:3,1:3,j)*sk(i).R0
    
    
end

% For an end effector we don't have rotation data;
% just need to calculate the final position.
for nn = find([skeleton.Nchannels] == 0)
    
    parent = skeleton(nn).parent;
    
    for ff = 1:Nframes
        transM = skeleton(parent).T(:,:,ff) * [eye(3), skeleton(nn).offsetFromParent; 0 0 0 1];
        skeleton(nn).t_xyz(:,ff) = transM([1 2 3],4);
    end
    
end

end



function transM = transformation_matrix(displ,rxyz,order)
% Constructs the transformation for given displacement, DISPL, and
% rotations RXYZ. The vector RYXZ is of length three corresponding to
% rotations around the X, Y, Z axes.
%
% The third input, ORDER, is a vector indicating which order to apply
% the planar rotations. E.g., [3 1 2] refers applying rotations RXYZ
% around Z first, then X, then Y.
%
% Years ago we benchmarked that multiplying the separate rotation matrices
% was more efficient than pre-calculating the final rotation matrix
% symbolically, so we don't "optimise" by having a hard-coded rotation
% matrix for, say, 'ZXY' which seems more common in BVH files.
% Should revisit this assumption one day.
%
% Precalculating the cosines and sines saves around 38% in execution time.

c = cosd(rxyz);
s = sind(rxyz);

RxRyRz(:,:,1) = [1 0 0; 0 c(1) -s(1); 0 s(1) c(1)];
RxRyRz(:,:,2) = [c(2) 0 s(2); 0 1 0; -s(2) 0 c(2)];
RxRyRz(:,:,3) = [c(3) -s(3) 0; s(3) c(3) 0; 0 0 1];

rotM = RxRyRz(:,:,order(1))*RxRyRz(:,:,order(2))*RxRyRz(:,:,order(3));

transM = [rotM, displ; 0 0 0 1];

end