diff --git a/src/BC-Cylinder.f90 b/src/BC-Cylinder.f90
index a492e3b9d20ebcfbb677fd10e79f140b91244f18..dfc6751398f050462844182b01d20a2810a4f80e 100644
--- a/src/BC-Cylinder.f90
+++ b/src/BC-Cylinder.f90
@@ -76,7 +76,7 @@ contains
           do i=nxi,nxf
              xm=real(i-1,mytype)*dx
              r=sqrt_prec((xm-cexx)**two+(ym-ceyy)**two)
-             if (r-ra.gt.zeromach) then
+             if (r-ra(1).gt.zeromach) then
                 cycle
              endif
              epsi(i,j,k)=remp
diff --git a/src/BC-Ellipsoid.f90 b/src/BC-Ellipsoid.f90
index 94fc9562556b0085d6336ecaa5154f0a679effd9..a919ae23a8563ec72d57be47cd704dd8aa4d228e 100644
--- a/src/BC-Ellipsoid.f90
+++ b/src/BC-Ellipsoid.f90
@@ -22,7 +22,7 @@ subroutine geomcomplex_ellip(epsi,nxi,nxf,ny,nyi,nyf,nzi,nzf,dx,yp,dz,remp)
     use param, only : one, two, ten
     use ibm_param
     use dbg_schemes, only: sqrt_prec
-    use ellipsoid_utils, only: NormalizeQuaternion, EllipsoidalRadius
+    use ellipsoid_utils, only: NormalizeQuaternion, EllipsoidalRadius, EllipsoidalRadius_debug
 
     implicit none
 
@@ -85,7 +85,9 @@ subroutine geomcomplex_ellip(epsi,nxi,nxf,ny,nyi,nyf,nzi,nzf,dx,yp,dz,remp)
                 if (cube_flag.eq.0) then 
                     call EllipsoidalRadius(point,position(i_body,:),orientation(i_body,:),shape(i_body,:),r)
                     is_inside = (r-ra(i_body)).lt.zeromach
-
+                    ! if (is_inside) then 
+                    !     call EllipsoidalRadius_debug(point,position(i_body,:),orientation(i_body,:),shape(i_body,:),r)
+                    ! endif
                     if (ra(i_body) /= ra(i_body)) then
                         write(*,*) "Nrank = ", nrank
                         write(*,*) "Point = ", point
diff --git a/src/case.f90 b/src/case.f90
index a334b2b4ccbc168fc7ab85ccc34074abf3dd02ae..e613c258845c6a914ab76ef2dd2a74e66ef8a5df 100644
--- a/src/case.f90
+++ b/src/case.f90
@@ -369,7 +369,7 @@ contains
 
     if (iforces.eq.1) then
       !  write(*,*) "Calling force from case"
-       call force(ux,uy,uz,ep,dummy1,dummy2,dummy3,0)
+      !  call force(ux,uy,uz,ep,dummy1,dummy2,dummy3,0)
        call restart_forces(1)
     endif
 
diff --git a/src/ellip_utils.f90 b/src/ellip_utils.f90
index ee2cf0b54cae8ab77b4c22cc7a6f4b36940bac8f..deaa590cdcd1bdf09baba2a91a0ba8048579dde9 100644
--- a/src/ellip_utils.f90
+++ b/src/ellip_utils.f90
@@ -232,6 +232,50 @@ contains
 
    end subroutine    
 
+   subroutine EllipsoidalRadius_debug(point, centre, orientation, shape, radius)
+    real(mytype), intent(in) :: point(3), centre(3), orientation(4), shape(3)
+    real(mytype), intent(out) :: radius
+    real(mytype) :: trans_point(3),rotated_point(3),scaled_point(3), orientation_c(4)
+    integer :: i
+
+    !translate point to body frame
+    trans_point = point-centre
+
+    write(*,*) "Translated point = ", trans_point
+
+    write(*,*) "Orientation = ", orientation
+
+    call QuaternionConjugate(orientation, orientation_c)
+
+    write(*,*) "Orientation inverse = ", orientation_c
+
+    !rotate point into body frame (using inverse(conjugate) of orientation)
+    call RotatePoint(trans_point, orientation, rotated_point)
+
+    write(*,*) "Rotated point = ", rotated_point
+    do i = 1,3
+       scaled_point(i)=rotated_point(i)/shape(i)
+    end do 
+
+    write(*,*) "Scaled point  = ", scaled_point
+
+    radius=sqrt_prec(scaled_point(1)**2+scaled_point(2)**2+scaled_point(3)**2)
+
+    write(*,*) "Radius = ", radius
+
+    ! if (radius /= radius) then
+    !     write(*,*) "Got an error in grid check!"
+    !     write(*,*) "Radius = ", radius
+    !     write(*,*) "point = ", point
+    !     write(*,*) "Body centre = ", centre
+    !     write(*,*) "Translated point = ", trans_point
+    !     write(*,*) "Orientation = ", orientation
+    !     write(*,*) "Rotated point = ", rotated_point
+    !     write(*,*) "Scaled Point = ", scaled_point
+    ! endif
+
+ end subroutine    
+
   subroutine CrossProduct(a, b, result)
     real(mytype), intent(in) :: a(3), b(3)
     real(mytype), intent(inout) :: result(3)
@@ -269,7 +313,14 @@ contains
       call EllipsoidalRadius(point, position(i,:), orientation(i,:), shape(i,:), r)
       radii(i) = r
     enddo
-    i_closest = minloc(radii)
+    i_closest=1
+    if (nbody.gt.1) then 
+      do i = 2,nbody
+        if (radii(i) < radii(i_closest)) then 
+          i_closest=i
+        endif
+      enddo
+    endif
 
     call CalculatePointVelocity(point, position(i_closest,:), angularVelocity(i_closest,:), linearVelocity(i_closest, :), pointVelocity)
     end subroutine
@@ -286,10 +337,9 @@ contains
   end subroutine is_inside_ellipsoid
 
 
-  subroutine navierFieldGen(center, linearVelocity, angularVelocity, ep1, ep1_x, ep1_y, ep1_z)
+  subroutine navierFieldGen(ep1, ep1_x, ep1_y, ep1_z)
     use param
     use decomp_2d
-    real(mytype), intent(in) :: center(3), linearVelocity(3), angularVelocity(4)
     real(mytype), dimension(xsize(1),xsize(2),xsize(3)), intent(in) :: ep1
     real(mytype),dimension(xsize(1),xsize(2),xsize(3)),intent(out) :: ep1_x, ep1_y, ep1_z
     real(mytype) :: xm, ym, zm, point(3), x_pv, y_pv, z_pv, pointVelocity(3)
@@ -303,7 +353,7 @@ contains
           xm=real(i+xstart(1)-2, mytype)*dx
           point=[xm,ym,zm]
           if (ep1(i,j,k).eq.1) then 
-            call CalculatePointVelocity(point, center, angularVelocity, linearVelocity, pointVelocity)
+            call CalculatePointVelocity_Multi(point, pointVelocity)
             x_pv=pointVelocity(1)
             y_pv=pointVelocity(2)
             z_pv=pointVelocity(3)
@@ -572,10 +622,10 @@ contains
 
     end subroutine ang_full_step
 
-    subroutine ang_step(q,omega_q,torque_vec,time_step,q1,omega1)
+    subroutine ang_step(q,omega_q,torque_vec,inertia,time_step,q1,omega1)
       use param
-      use ibm_param, only: inertia
-      real(mytype),intent(in) :: q(4),omega_q(4),torque_vec(3),time_step
+      ! use ibm_param, only: inertia
+      real(mytype),intent(in) :: q(4),omega_q(4),torque_vec(3),inertia(3,3),time_step
       real(mytype),intent(out):: q1(4),omega1(4)
       real(mytype)            :: torque_q(4),omega_b(4),torque_b(4),omega_half_b(4),omega1_b(4)
       real(mytype)            :: ang_accel_b(4), omega_half(4)
@@ -607,9 +657,9 @@ contains
     end subroutine ang_step
 
 
-    subroutine lin_step(position,linearVelocity,linearForce,time_step,position_1,linearVelocity_1)
-      use ibm_param, only: ellip_m
-      real(mytype),intent(in)   :: position(3),linearVelocity(3),linearForce(3),time_step
+    subroutine lin_step(position,linearVelocity,linearForce,ellip_m,time_step,position_1,linearVelocity_1)
+      ! use ibm_param, only: ellip_m
+      real(mytype),intent(in)   :: position(3),linearVelocity(3),linearForce(3),ellip_m,time_step
       real(mytype),intent(out)  :: position_1(3),linearVelocity_1(3)
       real(mytype)              :: linearAcceleration(3)
 
@@ -654,5 +704,38 @@ contains
       inertia(3,3)=i3
 
     end subroutine ellipInertiaCalculate
+
+    subroutine ibm_bcimp_calc(pointVelocity, lind, bcimp)
+      real(mytype), intent(in)  :: pointVelocity(3)
+      integer, intent(in)       :: lind
+      real(mytype), intent(out) :: bcimp
+      real(mytype)              :: x_pv, y_pv, z_pv
+
+      x_pv=pointVelocity(1)
+      y_pv=pointVelocity(2)
+      z_pv=pointVelocity(3)
+      if (lind.eq.0) then
+        bcimp=zero
+      elseif (lind.eq.1) then
+        bcimp=x_pv
+        ! write(*,*) bcimp
+      elseif (lind.eq.2) then
+        bcimp=y_pv
+      elseif (lind.eq.3) then
+        bcimp=z_pv
+      elseif (lind.eq.4) then 
+        bcimp=x_pv*x_pv
+      elseif (lind.eq.5) then
+        bcimp=y_pv*y_pv
+      elseif (lind.eq.6) then 
+        bcimp=z_pv*z_pv
+      elseif (lind.eq.7) then
+        bcimp=x_pv*y_pv 
+      elseif (lind.eq.8) then 
+        bcimp=x_pv*z_pv
+      elseif (lind.eq.9) then
+        bcimp=y_pv*z_pv
+      endif
+    end subroutine
       
 end module ellipsoid_utils
diff --git a/src/genepsi3d.f90 b/src/genepsi3d.f90
index dcc3e91db3cf7a086de030e36e85b965715b17f0..0a1f106200f69f4c4d0e538abef78349a1a3f6b4 100644
--- a/src/genepsi3d.f90
+++ b/src/genepsi3d.f90
@@ -91,7 +91,7 @@ contains
    use ibm_param
    use ellipsoid_utils, only: NormalizeQuaternion,ellipInertiaCalculate,ellipMassCalculate
    use param
-   real(mytype) :: eqr, ori_dummy(4)
+   real(mytype) :: eqr, ori_dummy(4), ellip_m_dummy, inertia_dummy(3,3)
    integer :: i,ii,j
 
    do i =1,nbody 
@@ -105,8 +105,6 @@ contains
       do j = 1,3
          shape(i,j) = sh(ii+j)/eqr
       enddo
-   enddo
-   do i = 1,nbody 
       write(*,*) i, "'s shape = ", shape(i,:)
    enddo
 
@@ -150,9 +148,21 @@ contains
       enddo
       write(*,*) "Nbody", i, "angvel = ", angularVelocity(i, :)
    enddo
-   write(*,*) "Ra = ", ra
-   call ellipInertiaCalculate(shape,rho_s,inertia)
-   call ellipMassCalculate(shape,rho_s,ellip_m)
+   ! write(*,*) "Ra = ", ra
+
+   do i = 1,nbody
+      write(*,*) "Nbody = ", i, "Radius = ", ra(i)
+   enddo
+   do i = 1,nbody
+      call ellipInertiaCalculate(shape(i,:),rho_s(i),inertia_dummy)
+      inertia(i,:,:) = inertia_dummy
+      write(*,*) "Nbody", i, "InertiaM = ", inertia(i,:,:)
+   enddo
+   do i = 1,nbody
+      call ellipMassCalculate(shape(i,:), rho_s(i), ellip_m_dummy)
+      ellip_m(i) = ellip_m_dummy
+      write(*,*) "Nbody", i, "ellip_m = ", ellip_m(i)
+   enddo
 
   end subroutine param_assign
 !############################################################################
diff --git a/src/ibm.f90 b/src/ibm.f90
index 8a389b5742f5aca912ad31a1c0c88c8135a12dcb..14c56931d9eb53bb7916501cabc6e7bf696d7b84 100644
--- a/src/ibm.f90
+++ b/src/ibm.f90
@@ -402,7 +402,7 @@ subroutine cubsplx(u,lind)
   USE decomp_2d
   USE variables
   USE ibm_param
-  USE ellipsoid_utils, ONLY: CalculatePointVelocity, EllipsoidalRadius
+  USE ellipsoid_utils, ONLY: CalculatePointVelocity_Multi, ibm_bcimp_calc
     !
   implicit none
   !
@@ -420,7 +420,7 @@ subroutine cubsplx(u,lind)
   integer                                            :: inxi,inxf
   real(mytype)                                       :: ana_resi,ana_resf         ! Position of Boundary (Analytically)
   real(mytype)                                       :: point(3),pointVelocity(3)
-  real(mytype)                                       :: xm,ym,zm,x_pv,y_pv,z_pv, dummy
+  real(mytype)                                       :: xm,ym,zm
   !
   ! Initialise Arrays
   xa(:)=0.
@@ -430,7 +430,6 @@ subroutine cubsplx(u,lind)
 !   bcimp=lind  
 !   write(*,*) lind
   !
-  dummy=0.0
   do k=1,xsize(3)
    zm=real(xstart(3)+k-1,mytype)*dz
      do j=1,xsize(2)
@@ -452,7 +451,7 @@ subroutine cubsplx(u,lind)
                  xa(ia)=ana_resi
               endif
               point=[xm,ym,zm]
-              call CalculatePointVelocity(point, position, angularVelocity, linearVelocity, pointVelocity)
+              call CalculatePointVelocity_Multi(point, pointVelocity)
             !   write(*,*) "Called CPV at ", point, "returned", pointVelocity
             !   call EllipsoidalRadius(point, position, orientation, shape, dummy)
             !   dummy = maxval(abs((point(2:3)-position(2:3))))
@@ -461,31 +460,8 @@ subroutine cubsplx(u,lind)
             !   endif
             !   write(*,*) "Radius = ", dummy
                ! write(*,*) "radius = ", dummy, "At point", point
-              x_pv=pointVelocity(1)
-              y_pv=pointVelocity(2)
-              z_pv=pointVelocity(3)
-              if (lind.eq.0) then
-               bcimp=zero
-              elseif (lind.eq.1) then
-               bcimp=x_pv
-               ! write(*,*) bcimp
-              elseif (lind.eq.2) then
-               bcimp=y_pv
-              elseif (lind.eq.3) then
-               bcimp=z_pv
-              elseif (lind.eq.4) then 
-               bcimp=x_pv*x_pv
-              elseif (lind.eq.5) then
-               bcimp=y_pv*y_pv
-              elseif (lind.eq.6) then 
-               bcimp=z_pv*z_pv
-              elseif (lind.eq.7) then
-               bcimp=x_pv*y_pv 
-              elseif (lind.eq.8) then 
-               bcimp=x_pv*z_pv
-              elseif (lind.eq.9) then
-               bcimp=y_pv*z_pv
-              endif
+              call ibm_bcimp_calc(pointVelocity, lind, bcimp)
+
               ya(ia)=bcimp
               if(xi(i,j,k).gt.0.)then ! Immersed Object
                  inxi=0
@@ -532,7 +508,7 @@ subroutine cubsplx(u,lind)
               endif              
               xm = xf(i,j,k)
               point=[xm,ym,zm]
-              call CalculatePointVelocity(point, position, angularVelocity, linearVelocity, pointVelocity)
+              call CalculatePointVelocity_Multi(point, pointVelocity)
             !   write(*,*) "Called CPV at ", point, "returned", pointVelocity
             !   call EllipsoidalRadius(point, position, orientation, shape, dummy)
             !   dummy = maxval(abs(point-position))
@@ -541,31 +517,8 @@ subroutine cubsplx(u,lind)
             !   if ((dummy.gt.(1.01)).or.(dummy.lt.0.99)) then
             !    write(*,*) "At ", point, "r = ", dummy
             !   endif
-              x_pv=pointVelocity(1)
-              y_pv=pointVelocity(2)
-              z_pv=pointVelocity(3)
-              if (lind.eq.0) then
-               bcimp=zero
-              elseif (lind.eq.1) then
-               bcimp=x_pv
-               ! write(*,*) bcimp
-              elseif (lind.eq.2) then
-               bcimp=y_pv
-              elseif (lind.eq.3) then
-               bcimp=z_pv
-              elseif (lind.eq.4) then 
-               bcimp=x_pv*x_pv
-              elseif (lind.eq.5) then
-               bcimp=y_pv*y_pv
-              elseif (lind.eq.6) then 
-               bcimp=z_pv*z_pv
-              elseif (lind.eq.7) then
-               bcimp=x_pv*y_pv 
-              elseif (lind.eq.8) then 
-               bcimp=x_pv*z_pv
-              elseif (lind.eq.9) then
-               bcimp=y_pv*z_pv
-              endif
+              call ibm_bcimp_calc(pointVelocity, lind, bcimp)  !take correct part of pointVelocity for equation type.
+
               ya(ia)=bcimp              
               if(xf(i,j,k).lt.xlx)then ! Immersed Object
                  inxf=0
@@ -641,7 +594,7 @@ subroutine cubsply(u,lind)
   USE decomp_2d
   USE variables
   USE ibm_param
-  USE ellipsoid_utils, ONLY: CalculatePointVelocity
+  USE ellipsoid_utils, ONLY: CalculatePointVelocity_Multi, ibm_bcimp_calc
   !
   implicit none
   !
@@ -659,7 +612,7 @@ subroutine cubsply(u,lind)
   integer                                            :: inxi,inxf  
   real(mytype)                                       :: ana_resi,ana_resf
   real(mytype)                                       :: point(3),pointVelocity(3)
-  real(mytype)                                       :: xm,ym,zm,x_pv,y_pv,z_pv
+  real(mytype)                                       :: xm,ym,zm
   !
   ! Initialise Arrays
   xa(:)=0.
@@ -690,31 +643,10 @@ subroutine cubsply(u,lind)
               endif  
               ym = yi(j,i,k)
               point=[xm,ym,zm]
-              call CalculatePointVelocity(point, position, angularVelocity, linearVelocity, pointVelocity)
-              x_pv=pointVelocity(1)
-              y_pv=pointVelocity(2)
-              z_pv=pointVelocity(3)
-              if (lind.eq.0) then
-               bcimp=zero
-              elseif (lind.eq.1) then
-               bcimp=x_pv
-              elseif (lind.eq.2) then
-               bcimp=y_pv
-              elseif (lind.eq.3) then
-               bcimp=z_pv
-              elseif (lind.eq.4) then 
-               bcimp=x_pv*x_pv
-              elseif (lind.eq.5) then
-               bcimp=y_pv*y_pv
-              elseif (lind.eq.6) then 
-               bcimp=z_pv*z_pv
-              elseif (lind.eq.7) then
-               bcimp=x_pv*y_pv 
-              elseif (lind.eq.8) then 
-               bcimp=x_pv*z_pv
-              elseif (lind.eq.9) then
-               bcimp=y_pv*z_pv
-              endif
+              call CalculatePointVelocity_Multi(point, pointVelocity)
+              
+              call ibm_bcimp_calc(pointVelocity, lind, bcimp)  !take correct part of pointVelocity for equation type.
+
               ya(ia)=bcimp
               if(yi(j,i,k).gt.0.)then ! Immersed Object
                  jy=1
@@ -766,31 +698,10 @@ subroutine cubsply(u,lind)
               endif  
               ym = yf(j,i,k)
               point=[xm,ym,zm]
-              call CalculatePointVelocity(point, position, angularVelocity, linearVelocity, pointVelocity)
-              x_pv=pointVelocity(1)
-              y_pv=pointVelocity(2)
-              z_pv=pointVelocity(3)
-              if (lind.eq.0) then
-               bcimp=zero
-              elseif (lind.eq.1) then
-               bcimp=x_pv
-              elseif (lind.eq.2) then
-               bcimp=y_pv
-              elseif (lind.eq.3) then
-               bcimp=z_pv
-              elseif (lind.eq.4) then 
-               bcimp=x_pv*x_pv
-              elseif (lind.eq.5) then
-               bcimp=y_pv*y_pv
-              elseif (lind.eq.6) then 
-               bcimp=z_pv*z_pv
-              elseif (lind.eq.7) then
-               bcimp=x_pv*y_pv 
-              elseif (lind.eq.8) then 
-               bcimp=x_pv*z_pv
-              elseif (lind.eq.9) then
-               bcimp=y_pv*z_pv
-              endif
+              call CalculatePointVelocity_Multi(point, pointVelocity)
+              
+              call ibm_bcimp_calc(pointVelocity, lind, bcimp)  !take correct part of pointVelocity for equation type.
+
               ya(ia)=bcimp
               if(yf(j,i,k).lt.yly)then ! Immersed Object
                  jy=1
@@ -866,7 +777,7 @@ subroutine cubsplz(u,lind)
   USE decomp_2d
   USE variables
   USE ibm_param
-  USE ellipsoid_utils, ONLY: CalculatePointVelocity
+  USE ellipsoid_utils, ONLY: CalculatePointVelocity_Multi, ibm_bcimp_calc
   !
   implicit none
   !
@@ -884,7 +795,7 @@ subroutine cubsplz(u,lind)
   integer                                            :: inxi,inxf  
   real(mytype)                                       :: ana_resi,ana_resf
   real(mytype)                                       :: point(3),pointVelocity(3)
-  real(mytype)                                       :: xm,ym,zm,x_pv,y_pv,z_pv
+  real(mytype)                                       :: xm,ym,zm
 
   !
   ! Initialise Arrays
@@ -916,31 +827,10 @@ subroutine cubsplz(u,lind)
               endif  
               zm = zi(k,i,j)
               point=[xm,ym,zm]
-              call CalculatePointVelocity(point, position, angularVelocity, linearVelocity, pointVelocity)
-              x_pv=pointVelocity(1)
-              y_pv=pointVelocity(2)
-              z_pv=pointVelocity(3)
-              if (lind.eq.0) then
-               bcimp=zero
-              elseif (lind.eq.1) then
-               bcimp=x_pv
-              elseif (lind.eq.2) then
-               bcimp=y_pv
-              elseif (lind.eq.3) then
-               bcimp=z_pv
-              elseif (lind.eq.4) then 
-               bcimp=x_pv*x_pv
-              elseif (lind.eq.5) then
-               bcimp=y_pv*y_pv
-              elseif (lind.eq.6) then 
-               bcimp=z_pv*z_pv
-              elseif (lind.eq.7) then
-               bcimp=x_pv*y_pv 
-              elseif (lind.eq.8) then 
-               bcimp=x_pv*z_pv
-              elseif (lind.eq.9) then
-               bcimp=y_pv*z_pv
-              endif
+              call CalculatePointVelocity_Multi(point, pointVelocity)
+              
+              call ibm_bcimp_calc(pointVelocity, lind, bcimp)  !take correct part of pointVelocity for equation type.
+
               ya(ia)=bcimp
               if(zi(k,i,j).gt.0.)then ! Immersed Object
                  inxi=0
@@ -985,31 +875,10 @@ subroutine cubsplz(u,lind)
               endif
               zm = zi(k,i,j)
               point=[xm,ym,zm]
-              call CalculatePointVelocity(point, position, angularVelocity, linearVelocity, pointVelocity)
-              x_pv=pointVelocity(1)
-              y_pv=pointVelocity(2)
-              z_pv=pointVelocity(3)
-              if (lind.eq.0) then
-               bcimp=zero
-              elseif (lind.eq.1) then
-               bcimp=x_pv
-              elseif (lind.eq.2) then
-               bcimp=y_pv
-              elseif (lind.eq.3) then
-               bcimp=z_pv
-              elseif (lind.eq.4) then 
-               bcimp=x_pv*x_pv
-              elseif (lind.eq.5) then
-               bcimp=y_pv*y_pv
-              elseif (lind.eq.6) then 
-               bcimp=z_pv*z_pv
-              elseif (lind.eq.7) then
-               bcimp=x_pv*y_pv 
-              elseif (lind.eq.8) then 
-               bcimp=x_pv*z_pv
-              elseif (lind.eq.9) then
-               bcimp=y_pv*z_pv
-              endif
+              call CalculatePointVelocity_Multi(point, pointVelocity)
+              
+              call ibm_bcimp_calc(pointVelocity, lind, bcimp)  !take correct part of pointVelocity for equation type.
+
               ya(ia)=bcimp
               if(zf(k,i,j).lt.zlz)then  ! Immersed Object
                  inxf=0
@@ -1197,9 +1066,9 @@ subroutine ana_y_cyl(i,y_pos,ana_res)
      ceyy = cey
   endif
   if (y_pos.gt.ceyy) then     ! Impose analytical BC
-      ana_res=ceyy + sqrt(ra**2.0-((i+ystart(1)-1-1)*dx-cexx)**2.0)
+      ana_res=ceyy + sqrt(ra(1)**2.0-((i+ystart(1)-1-1)*dx-cexx)**2.0)
   else
-      ana_res=ceyy - sqrt(ra**2.0-((i+ystart(1)-1-1)*dx-cexx)**2.0)
+      ana_res=ceyy - sqrt(ra(1)**2.0-((i+ystart(1)-1-1)*dx-cexx)**2.0)
   endif     
   !
   return
@@ -1228,9 +1097,9 @@ subroutine ana_x_cyl(j,x_pos,ana_res)
      ceyy = cey
   endif
   if (x_pos.gt.cexx) then     ! Impose analytical BC
-      ana_res = cexx + sqrt(ra**2.0-(yp(j+xstart(2)-1)-ceyy)**2.0)
+      ana_res = cexx + sqrt(ra(1)**2.0-(yp(j+xstart(2)-1)-ceyy)**2.0)
   else
-      ana_res = cexx - sqrt(ra**2.0-(yp(j+xstart(2)-1)-ceyy)**2.0)
+      ana_res = cexx - sqrt(ra(1)**2.0-(yp(j+xstart(2)-1)-ceyy)**2.0)
   endif     
   !
   return
diff --git a/src/module_param.f90 b/src/module_param.f90
index 49be2679888ae711ce149a6ea520f2a0fa79c424..33778c7054ce1e11ad08450f89f6eb303d0adbe5 100644
--- a/src/module_param.f90
+++ b/src/module_param.f90
@@ -617,9 +617,9 @@ end module simulation_stats
 !############################################################################
 module ibm_param
   use decomp_2d, only : mytype
-  real(mytype) :: cex,cey,cez,shx,shy,shz,oriw,orii,orij,orik,lvx,lvy,lvz,avx,avy,avz,ubcx,ubcy,ubcz,rads,ellip_m,c_air,cvl_scalar,grav_y,grav_x,grav_z
-  real(mytype) :: position(10,3),orientation(10,4),linearVelocity(10,3),angularVelocity(10,4),linearAcceleration(3),linearForce(10,3),torque(10,3),shape(10,3),inertia(3,3)
-  real(mytype) :: position_1(10,3),linearVelocity_1(10,3),orientation_1(10,4),angularVelocity_1(10,4),ra(10),rho_s(10)
+  real(mytype) :: cex,cey,cez,shx,shy,shz,oriw,orii,orij,orik,lvx,lvy,lvz,avx,avy,avz,ubcx,ubcy,ubcz,rads,c_air,cvl_scalar,grav_y,grav_x,grav_z
+  real(mytype) :: position(10,3),orientation(10,4),linearVelocity(10,3),angularVelocity(10,4),linearAcceleration(3),linearForce(10,3),torque(10,3),shape(10,3)
+  real(mytype) :: position_1(3),linearVelocity_1(3),orientation_1(4),angularVelocity_1(4),ra(10),rho_s(10),ellip_m(10),inertia(10,3,3)
   real(mytype) :: chord,thickness,omega, tconv2_sign, shear_velocity
   real(mytype) :: ce(30),sh(30),ori(40), lv(30), av(30)
   integer :: inana ! Analytical BC as Input
diff --git a/src/navier.f90 b/src/navier.f90
index 534f775433bf35a92f65860936ad1265fe1b5eb5..5196846e6720526d57b448810668f12f98ce0dcb 100644
--- a/src/navier.f90
+++ b/src/navier.f90
@@ -289,7 +289,7 @@ contains
        tb1(:,:,:) = uy1(:,:,:)
        tc1(:,:,:) = uz1(:,:,:)
     else if (itype.eq.itype_ellip) then
-       call navierFieldGen(position, linearVelocity, angularVelocity,ep1, ep1_ux, ep1_uy, ep1_uz)
+       call navierFieldGen(ep1, ep1_ux, ep1_uy, ep1_uz)
        ta1(:,:,:) = (one - ep1(:,:,:)) * ux1(:,:,:) + ep1(:,:,:)*ep1_ux(:,:,:)
        tb1(:,:,:) = (one - ep1(:,:,:)) * uy1(:,:,:) + ep1(:,:,:)*ep1_uy(:,:,:)
        tc1(:,:,:) = (one - ep1(:,:,:)) * uz1(:,:,:) + ep1(:,:,:)*ep1_uz(:,:,:)
diff --git a/src/tools.f90 b/src/tools.f90
index 2723f7ac51f8f76529d6771c04e45d7f91cc7ab0..fe4712e5036d40aaa7974d6f1db1e7a49963cafb 100644
--- a/src/tools.f90
+++ b/src/tools.f90
@@ -166,8 +166,8 @@ contains
           call cpu_time(time1)
           write(*,*) '==========================================================='
           write(*,"(' Time step =',i7,'/',i7,', Time unit =',F9.4)") itime,ilast,t
-          write(*,*) 'Centroid position = ',real(position(1),4),real(position(2),4),real(position(3),4)
-          write(*,*) 'Orientation       = ',real(orientation(1),4),real(orientation(2),4),real(orientation(3),4),real(orientation(4),4)
+         !  write(*,*) 'Centroid position = ',real(position(1),4),real(position(2),4),real(position(3),4)
+         !  write(*,*) 'Orientation       = ',real(orientation(1),4),real(orientation(2),4),real(orientation(3),4),real(orientation(4),4)
        endif
     else if ((iwhen == 3).and.(itime > ifirst)) then !AT THE END OF A TIME STEP
        if (nrank == 0.and.(mod(itime, ilist) == 0 .or. itime == ifirst .or. itime==ilast)) then
diff --git a/src/xcompact3d.f90 b/src/xcompact3d.f90
index 4127f9bd63dd88ccb2000b394c6c8efe4a4dd86b..785bd7a247ba996d43d116fb82b6d2db947c30d1 100644
--- a/src/xcompact3d.f90
+++ b/src/xcompact3d.f90
@@ -167,15 +167,17 @@ program xcompact3d
       !   if (nrank==0) then
 
       !   if (bodies_fixed==0) then 
-         call lin_step(position,linearVelocity,linearForce,dt,position_1,linearVelocity_1)
-         call ang_step(orientation,angularVelocity,torque,dt,orientation_1,angularVelocity_1)
+        do i = 1,nvol
 
-         position = position_1
-         linearVelocity = linearVelocity_1
-
-         orientation = orientation_1
-         angularVelocity = angularVelocity_1
+         call lin_step(position(i,:),linearVelocity(i,:),linearForce(i,:),ellip_m(i),dt,position_1,linearVelocity_1)
+         call ang_step(orientation(i,:),angularVelocity(i,:),torque(i,:),inertia(i,:,:),dt,orientation_1,angularVelocity_1)
+        
+         position(i,:) = position_1
+         linearVelocity(i,:) = linearVelocity_1
 
+         orientation(i,:) = orientation_1
+         angularVelocity(i,:) = angularVelocity_1
+        enddo
 
 
       !   if (nrank==0) then
@@ -185,12 +187,15 @@ program xcompact3d
       !   endif 
 
          if ((nrank==0).and.(mod(itime,ilist)==0)) then 
-            write(*,*) "Position =         ", position_1
-            write(*,*) "Orientation =      ", orientation_1
-            write(*,*) "Linear velocity =  ", linearVelocity
-            write(*,*) "Angular velocity = ", angularVelocity
-            write(*,*) "Linear Force = ", [drag,lift,lat]
-            write(*,*) "Torque = ", [xtorq,ytorq,ztorq]
+            do i = 1,nbody
+               write(*,*) "Body", i
+               write(*,*) "Position =         ", position(i,:)
+               write(*,*) "Orientation =      ", orientation(i,:)
+               write(*,*) "Linear velocity =  ", linearVelocity(i,:)
+               write(*,*) "Angular velocity = ", angularVelocity(i,:)
+               write(*,*) "Linear Force = ", linearForce(i,:)
+               write(*,*) "Torque = ", torque(i,:)
+            enddo
          ! call QuaternionNorm(angularVelocity,dummy)
 
          ! write(*,*) 'Norm of angvel = ', dummy