%% Planar Exact Constraint Design
%
% Author: Oscar Viquez
%
% Date Created: Feb 9, 2016
%
% This script returns the reaction forces at the contraint points for a
% square or rectangular body on a planar exact constraint device. Results
% are based on the inputs listed, and are based on a Cartesian coordinate
% system. The screen represents the contact surface, with two (2x)
% constraint points along the X-axis and one (1x) constraint point along
% the Y-axis. Body resides in 1st Quadrant.


%% Inputs
%
% This section defines the geometry and force input for the planar exact
% constraint device.

% Length of side of the body:
Lx = 10;
Ly = 10;

% Position of constraint points on X-axis:
% [must be smaller than L]
a = 2;
b = 8;

% Position of constraint point on Y-axis:
% [must be smaller than L]
c = 5;

% Force - point of application:
x = 5;
y = 5;

% Force - magnitudes:
% [values are direction-sensitive]
Fx = -2;
Fy = -2;


%% Check Inputs
%
% This section verifies that the user has provided valid quantities for all
% geometric inputs.

if a>Lx || a<0 || b>Lx || b<0
    disp('Please check that your X-axis constraints are within range (0 <= a,b <= Lx), then try again');
    return;
end

if c>Ly
    disp('Please check that your Y-axis constraint is within range (0 <= c <= Ly), then try again');
    return;
end

if x>Lx || x<0
    disp('Please check that your force is applied within the bounds of the body (0 <= x <= Lx), then try again');
    return;
end

if y>Ly || y<0
    disp('Please check that your force is applied within the bounds of the body (0 <= y <= Ly), then try again');
    return;
end


%% Solver
%
% This section computes the reaction forces at each constraint point. Force
% balance and moment balance equations are written in matrix form to solve
% by linear algebra. The basic form is Ax=b, where x is the vector of
% unknowns. In this case, the unknowns are Ra, Rb, Rc - the reactions (R)
% at each node (a, b, c).

% Input vector b:
b_vec = [-Fx; -Fy; (y*Fx-x*Fy)];

% Matrix A:
A = [0 0 1; 1 1 0; a b -c];

% Solve system of linear equations:
x_vec = A\b_vec;


%% Analyzer
%
% This section reviews the results produced by the solver and provides the
% user with feedback about the design's performance.

if x_vec(1)<0 || x_vec(2)<0
    disp('This configuration is pulling away from one of the constraint points on the X-axis. Please check your parameters and try again.');
    return;
elseif x_vec(3)<0
    disp('This configuration is pulling away from the constraint point on the Y-axis. Please check your parameters and try again.');
    return;
end


%% Plot
%
% This section provides a rendering of the planar EC device configuration defined
% by the user.

figure;
hold on;
plot([0 Lx Lx 0 0],[0 0 Ly Ly 0],'k-','linewidth',2);
q1 = quiver([a b -1],[-1 -1 c],[0 0 1],[1 1 0],0,'b-','linewidth',1.5);
% q1.MaxHeadSize = 1;
q2 = quiver(x,y,Fx/(abs(Fx)),Fy/(abs(Fy)),0,'r-o','linewidth',1.5);
q2.MaxHeadSize = 2;
q2.ShowArrowHead = 'on';
axis([-.3*Lx 1.1*Lx -.3*Ly 1.1*Ly]);
axis equal;

legend('Square Body','Constraints','Force','Location','Southwest');

print('fig_planar_EC','-djpeg');








