anchor.diff:  !  print*, cross(B%C_mat * B%r_dot, B%S)
   !  print*, B%C_mat * B%r_dot * B%S ! vector right now should be matrix
   
  -end function jac
  +end function D_R
  +
  +
  +! jacobian of the structural degree of freedom
  +function S_R(alpha)
  +
  +  type(body)   :: alpha
  +  type(matrix) :: S_R(4,4) ! where 4 is the state vector length
  +  type(matrix)  :: O ! zero matrix
  +
  +  O = matrix(zeros(num_spat_dim))
  +
  +  S_R(1,1) = O
  +  S_R(2,1) = O
  +  S_R(3,1) = O
  +  S_R(4,1) = O
  +
  +  S_R(1,2) = O
  +  S_R(2,2) = O
  +  S_R(3,2) = O
  +  S_R(4,2) = O
  +
  +  S_R(1,3) = O
  +  S_R(2,3) = O
  +  S_R(3,3) = O
  +  S_R(4,3) = - skew(alpha%p*alpha%qs_dot)
  +
  +  S_R(1,4) = O
  +  S_R(2,4) = O
  +  S_R(3,4) = - skew(alpha%p*alpha%qs_dot)
  +  S_R(4,4) = - skew(alpha%h*alpha%qs_dot)
  +
  +end function S_R
  +
   
   end module test_body
  
positive.code: contains
  
  ! find the jacobian of the equation of motion for the supplied BODY alpha
  function jac(alpha)
  
    type(body)   :: alpha
    type(matrix) :: jac(4,4) ! where 4 is the state vector length
    type(matrix)  :: O ! zero matrix
    type(matrix)  :: U ! unit matrix
    type(matrix)  :: I ! identity matrix
  
negative.code: 
    type body
       
       real(dp)     :: m                          ! mass
  
       type(vector) :: r, r_dot                   ! radius of origin
       type(vector) :: theta, theta_dot           ! orientation of the body frame with respect to inertial
  
       type(vector) :: v                          ! the velocity and acceleration of the origin
       type(vector) :: omega                      ! the angular velocity and acceleration of the body axis
       type(vector) :: qs, qs_dot, qs_double_dot  ! elastic state vectors due to deformation
  
       type(vector) :: F                          ! external forces
       type(vector) :: G                          ! external moments (torque)
  
       type(matrix) :: C_mat                      ! rotation matrix
       type(matrix) :: S, S_dot                   ! transformation matrix
  
       type(vector) :: c                          ! first moment of inertia
       type(matrix) :: J                          ! second moment of inertia
       type(matrix) :: K                          ! stiffness matrix
  
       type(matrix) :: p                          ! 
       type(matrix) :: h                          ! 
  
       real(dp)     :: a, b                       ! constant multipliers
  
  
       ! for elastic
       ! type(vector) :: q_dot !? maybe qs
  
    end type body
    
  