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:   I = matrix(eye(num_spat_dim))
    
    ! assemble jacobian
    jac(1,1) = alpha%a * alpha%C_mat
    jac(2,1) = skew(alpha%C_mat * alpha%r_dot) * alpha%S
    jac(3,1) = -1.0_dp*U
    jac(4,1) = O
  
    jac(1,2) = O
    jac(2,2) = alpha%S_dot + skew(alpha%S*alpha%theta_dot)*alpha%S + alpha%a * alpha%S
    jac(3,2) = O
    jac(4,2) = -1.0_dp*U
  
    jac(1,3) = O
    jac(2,3) = O
    jac(3,3) = alpha%m*(alpha%a*U + skew(alpha%omega))
    jac(4,3) = -alpha%a*skew(alpha%c) + skew(skew(alpha%c)*alpha%omega)  - alpha%m*skew(alpha%V) - skew(alpha%omega)*skew(alpha%C)
  
    jac(1,4) = O
    jac(2,4) = O
    jac(3,4) = alpha%a*skew(alpha%C) + skew(alpha%C) * skew(alpha%omega)
    jac(4,4) = alpha%a*alpha%J  + skew(alpha%omega)*alpha%J - skew(alpha%J*alpha%omega) -skew(alpha%c)*skew(alpha%V)
  
  !  real(dp)     :: OO(num_spat_dim,num_spat_dim) ! zero matrix
  !  real(dp)     :: UU(num_spat_dim,num_spat_dim) ! unit matrix
  
negative.code: contains
  
  ! find the jacobian of the equation of motion for the supplied BODY alpha
  function D_R(alpha)
  
    type(body)   :: alpha
    type(matrix) :: D_R(4,4) ! where 4 is the state vector length
    type(matrix)  :: O ! zero matrix
    type(matrix)  :: U ! unit matrix
    type(matrix)  :: I ! identity matrix
  