Applied-Math-Assignment-2/link_length_error_func.m at main · HiGoldAnimationStudios/Applied-Math-Assignment-2 · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
%Error function that encodes the link length constraints
%INPUTS:
%vertex_coords: a column vector containing the (x,y) coordinates of every vertex
% in the linkage. There are two ways that I would recommend stacking
% the coordinates. You could alternate between x and y coordinates:
% i.e. vertex_coords = [x1;y1;x2;y2;...;xn;y_n], or alternatively
% you could do all the x's first followed by all of the y's
% i.e. vertex_coords = [x1;x2;...xn;y1;y2;...;yn]. You could also do
% something else entirely, the choice is up to you.
%leg_params: a struct containing the parameters that describe the linkage
% importantly, leg_params.link_lengths is a list of linakge lengths
% and leg_params.link_to_vertex_list is a two column matrix where
% leg_params.link_to_vertex_list(i,1) and
% leg_params.link_to_vertex_list(i,2) are the pair of vertices connected
% by the ith link in the mechanism
%OUTPUTS:
%length_errors: a column vector describing the current distance error of the ith
% link specifically, length_errors(i) = (xb-xa)ˆ2 + (yb-ya)ˆ2 - d_iˆ2
% where (xa,ya) and (xb,yb) are the coordinates of the vertices that
% are connected by the ith link, and d_i is the length of the ith link
function length_errors = link_length_error_func(vertex_coords, leg_params)
    %your code here
    num_vertices=leg_params.num_vertices;
    num_linkages=leg_params.num_linkages;
    link_to_vertex_list=leg_params.link_to_vertex_list;
    link_lengths=leg_params.link_lengths;
    crank_length=leg_params.crank_length;
    vertex_pos0=leg_params.vertex_pos0;
    vertex_pos2=leg_params.vertex_pos2;

    vertex_coords_mat=column_to_matrix(vertex_coords);

    ei=zeros(num_linkages,1);

    for i =1:num_linkages
        a_vertex=link_to_vertex_list(i,1);
        b_vertex=link_to_vertex_list(i,2);
        d=link_lengths(i);

        x_a=vertex_coords_mat(a_vertex,1);
        y_a=vertex_coords_mat(a_vertex,2);
        x_b=vertex_coords_mat(b_vertex,1);
        y_b=vertex_coords_mat(b_vertex,2);

        ei(i)=(x_b-x_a)^2+(y_b-y_a)^2-d^2;
    end
    length_errors=ei;
end