function [E,G,A,f,g,h] = make_ega(p)
% make rotation matrix using Euler parameters
%   A = E * trans(G)
%
% INPUT
% p = 4x1 vector of Euler parameters (unit quaternion)
%
% OUTPUT
% E = 3x4 matrix
% G = 3x4 matrix
% A = 3x3 orthonormal rotation matrix
% f = 3x1 unit vector first column of A
% g = 3x1 unit vector second column of A
% h = 3x1 unit vector third column of A
%
% REFERENCE
% Haug, Computer-Aided Kinematics and Dynamics of Mechanical Systems, Allyn-Bacon, 1989
%
% UPDATE
% 12.04.23, tested under MATLAB v5.2
%
% AUTHOR
% H.J. Sommer III, Ph.D., Professor of Mechanical Engineering, 337 Leonhard Bldg
% The Pennsylvania State University, University Park, PA  16802
% (814)863-8997  FAX (814)865-9693  hjs1@psu.edu  www.me.psu.edu/sommer/

E = [ -p(2)  +p(1)  -p(4)  +p(3);
      -p(3)  +p(4)  +p(1)  -p(2);
      -p(4)  -p(3)  +p(2)  +p(1)];

G = [ -p(2)  +p(1)  +p(4)  -p(3);
      -p(3)  -p(4)  +p(1)  +p(2);
      -p(4)  +p(3)  -p(2)  +p(1)];

A = E * G';

% explicit
%A = 2 * [ p(1)*p(1)+p(2)*p(2)-0.5   p(2)*p(3)-p(1)*p(4)       p(2)*p(4)+p(1)*p(3)     ;
%          p(2)*p(3)+p(1)*p(4)       p(1)*p(1)+p(3)*p(3)-0.5   p(3)*p(4)-p(1)*p(2)     ;
%          p(2)*p(4)-p(1)*p(3)       p(3)*p(4)+p(1)*p(2)       p(1)*p(1)+p(4)*p(4)-0.5 ];

f = A(:,1);
g = A(:,2);
h = A(:,3);

% bottom of make_ega
