%*** CHAPTER 3: RIGID-BODY MOTIONS ***

function V = se3ToVec(se3mat)
% Takes se3mat a 4x4 se(3) matrix
% Returns the corresponding 6-vector (representing spatial velocity).
% Example Input:
%{
  clear;clc;
  se3mat = [[0, -3, 2, 4]; [3, 0, -1, 5]; [-2, 1, 0, 6]; [0, 0, 0, 0]];
  V = se3ToVec(se3mat)
%} 
% Output:
% V =
%     1
%     2
%     3
%     4
%     5
%     6

V = [se3mat(3,2); se3mat(1,3); se3mat(2,1); se3mat(1:3,4)];
end