Repository: FloatingArrayDesign/MoorDyn (language: python)

Issue:
Body kinematics when origin not at center of mass
When the origin of a body is not at the center of mass (including any attached objects), the kinematics of bodies are not correct.

As a somewhat extreme demonstration of this, we can set up two kinematically identical situations, one where the body is at the center of mass, and one where the body is far from the COM.

```
--------------------- MoorDyn Input File -------------------------------------------------------
Testing body kinematics when body origin != COM
---------------------- ROD TYPES ------------------------------------
TypeName      Diam     Mass/m    Cd     Ca      CdEnd    CaEnd
(name)        (m)      (kg/m)    (-)    (-)     (-)      (-)
buoy          1       1.0e1     1.2    0.0     1.2      0.0
---------------------------- BODIES -----------------------------------------------------
ID   Attachment  X0     Y0    Z0     r0      p0     y0     Mass  CG*   I*      Volume   CdA*   Ca
(#)     (-)      (m)    (m)   (m)   (deg)   (deg)  (deg)   (kg)  (m)  (kg-m^2)  (m^3)   (m^2)  (-)
1       free      0     0      20      0.0       0      0      0     0    0        0       0      0
2       free      0     10      20     0.0       0      0      0     0    0        0       0      0
---------------------- RODS ----------------------------------------
ID     RodType   Attachment  Xa    Ya    Za    Xb    Yb    Zb    NumSegs   RodOutputs
(#)    (name)    (#/key)     (m)   (m)   (m)   (m)   (m)   (m)   (-)       (-)
1      buoy     Body1         0     0     -2     0   0.0    2     10        -
2      buoy     Body2         0     -10   -2     0  -10.0   2     10        -
```

In this case we have the same 4 meter tall rod, with it's center at (0, 0, 20). 
Body 1 starts at (0, 0, 20), and body 2 starts 10 meters to the side in the +y direction (with the rod a corresponding -10 meters to the side to be back at the origin).

In this simulation, gravity is set to 0, and we give the two bodies velocities equivalent to their rod rotating at 0.2 rad/sec around the x axis. 
For body 1 this means an initial velocity vector of  
`0.0, 0.0, 0.0, 0.2, 0.0, 0.0` 
and for body 2 this means an initial velocity vector of  
`0.0, 0.0, 10 * 0.2, 0.2, 0.0, 0.0`.  
The 10*0.2 m/s velocity in the +z direction is due to the fact that the body is 10 meters from the center of rotation. 

Running this simulation and viewing the results in paraview this is what we see:

https://github.com/FloatingArrayDesign/MoorDyn/assets/129765925/585ad2e4-9520-4bb9-9e00-ce97854c2b29


This is obviously incorrect, since both situations should be equivalent in terms of rod position/rotation.

In terms of solutions, I don't have much beyond avoiding this sort of scenario. I'm pretty certain that the problem is related to missing non-inertial force on the body (likely at least the centrifugal force). But I don't have a good grasp on how that would need to be implemented. 
I will note that in more practical situations, things like fluid drag or buoyancy forces often do a pretty good job of mitigating most of the error caused by the incorrect kinematics.

Additional complications include
* Added mass turns masses from scalars to matrices makes deriving the necessary equations more complicated. 
* I've found very little literature on handling kinematics of an object in a global coordinate frame when the object origin is not at the center of mass. Most things I've found either uses a body fixed coordinate frame, or require the object origin to be at the center of mass.

I don't necessarily have a strong feeling about how much effort should be put into addressing this issue. If there was sufficient reference theory and desire from other, I likely could put a fair amount of time into this. But I figured I'd bring this up to determine if it was a known limitation, and if there had been any other work (perhaps in OpenFast) to handle issues like this.

Body kinematics when origin not at center of mass
When the origin of a body is not at the center of mass (including any attached objects), the kinematics of bodies are not correct.

As a somewhat extreme demonstration of this, we can set up two kinematically identical situations, one where the body is at the center of mass, and one where the body is far from the COM.

```
--------------------- MoorDyn Input File -------------------------------------------------------
Testing body kinematics when body origin != COM
---------------------- ROD TYPES ------------------------------------
TypeName      Diam     Mass/m    Cd     Ca      CdEnd    CaEnd
(name)        (m)      (kg/m)    (-)    (-)     (-)      (-)
buoy          1       1.0e1     1.2    0.0     1.2      0.0
---------------------------- BODIES -----------------------------------------------------
ID   Attachment  X0     Y0    Z0     r0      p0     y0     Mass  CG*   I*      Volume   CdA*   Ca
(#)     (-)      (m)    (m)   (m)   (deg)   (deg)  (deg)   (kg)  (m)  (kg-m^2)  (m^3)   (m^2)  (-)
1       free      0     0      20      0.0       0      0      0     0    0        0       0      0
2       free      0     10      20     0.0       0      0      0     0    0        0       0      0
---------------------- RODS ----------------------------------------
ID     RodType   Attachment  Xa    Ya    Za    Xb    Yb    Zb    NumSegs   RodOutputs
(#)    (name)    (#/key)     (m)   (m)   (m)   (m)   (m)   (m)   (-)       (-)
1      buoy     Body1         0     0     -2     0   0.0    2     10        -
2      buoy     Body2         0     -10   -2     0  -10.0   2     10        -
```

In this case we have the same 4 meter tall rod, with it's center at (0, 0, 20). 
Body 1 starts at (0, 0, 20), and body 2 starts 10 meters to the side in the +y direction (with the rod a corresponding -10 meters to the side to be back at the origin).

In this simulation, gravity is set to 0, and we give the two bodies velocities equivalent to their rod rotating at 0.2 rad/sec around the x axis. 
For body 1 this means an initial velocity vector of  
`0.0, 0.0, 0.0, 0.2, 0.0, 0.0` 
and for body 2 this means an initial velocity vector of  
`0.0, 0.0, 10 * 0.2, 0.2, 0.0, 0.0`.  
The 10*0.2 m/s velocity in the +z direction is due to the fact that the body is 10 meters from the center of rotation. 

Running this simulation and viewing the results in paraview this is what we see:

https://github.com/FloatingArrayDesign/MoorDyn/assets/129765925/585ad2e4-9520-4bb9-9e00-ce97854c2b29


This is obviously incorrect, since both situations should be equivalent in terms of rod position/rotation.

In terms of solutions, I don't have much beyond avoiding this sort of scenario. I'm pretty certain that the problem is related to missing non-inertial force on the body (likely at least the centrifugal force). But I don't have a good grasp on how that would need to be implemented. 
I will note that in more practical situations, things like fluid drag or buoyancy forces often do a pretty good job of mitigating most of the error caused by the incorrect kinematics.

Additional complications include
* Added mass turns masses from scalars to matrices makes deriving the necessary equations more complicated. 
* I've found very little literature on handling kinematics of an object in a global coordinate frame when the object origin is not at the center of mass. Most things I've found either uses a body fixed coordinate frame, or require the object origin to be at the center of mass.

I don't necessarily have a strong feeling about how much effort should be put into addressing this issue. If there was sufficient reference theory and desire from other, I likely could put a fair amount of time into this. But I figured I'd bring this up to determine if it was a known limitation, and if there had been any other work (perhaps in OpenFast) to handle issues like this.



Discussion / hints:
@AlexWKinley does this also happen if you define the body's center of gravity in the input file (should be defined in the bodies reference frame)?
Assuming that this would be the correct way to do that
```
---------------------------- BODIES -----------------------------------------------------
ID      Attachment  X0     Y0      Z0      r0        p0     y0      Mass  CG*        I*        Volume   CdA*   Ca
(#)     (-)         (m)    (m)     (m)     (deg)     (deg)  (deg)   (kg)  (m)        (kg-m^2)  (m^3)    (m^2)  (-)
1       free        0      0       20      0.0       0      0       0     0          0         0        0      0
2       free        0      10      20      0.0       0      0       0     0|-10|0    0         0        0      0
```
then it does still happen with the CG defined at the rods center of gravity. Which I think makes sense since the body cg is only used to account for the body's own mass, which is zero in this situation. 

Edit:
If you want to experiment with this more, my full setup is
You'll need to have VTK enabled, but you can also strip out all the vtk parts if needed.
If the body deviates from the correct path (a circle around the rod), the test prints out the error in the position.

[tests/Mooring/body_tests/floating_bodies.txt](https://github.com/FloatingArrayDesign/MoorDyn/files/14996888/floating_bodies.txt)
[tests/bodies.zip](https://github.com/FloatingArrayDesign/MoorDyn/files/14996901/bodies.zip)

I am dealing with a model now which is failing because of this. I think I will spend a bit of time fixing it, so I assign this bug to myself.

If anyone has already done some advances on this topic, please let me know
@AlexWKinley does this also happen if you define the body's center of gravity in the input file (should be defined in the bodies reference frame)?
Assuming that this would be the correct way to do that
```
---------------------------- BODIES -----------------------------------------------------
ID      Attachment  X0     Y0      Z0      r0        p0     y0      Mass  CG*        I*        Volume   CdA*   Ca
(#)     (-)         (m)    (m)     (m)     (deg)     (deg)  (deg)   (kg)  (m)        (kg-m^2)  (m^3)    (m^2)  (-)
1       free        0      0       20      0.0       0      0       0     0          0         0        0      0
2       free        0      10      20      0.0       0      0       0     0|-10|0    0         0        0      0
```
then it does still happen with the CG defined at the rods center of gravity. Which I think makes sense since the body cg is only used to account for the body's own mass, which is zero in this situation. 

Edit:
If you want to experiment with this more, my full setup is
You'll need to have VTK enabled, but you can also strip out all the vtk parts if needed.
If the body deviates from the correct path (a circle around the rod), the test prints out the error in the position.

[tests/Mooring/body_tests/floating_bodies.txt](https://github.com/FloatingArrayDesign/MoorDyn/files/14996888/floating_bodies.txt)
[tests/bodies.zip](https://github.com/FloatingArrayDesign/MoorDyn/files/14996901/bodies.zip)

I am dealing with a model now which is failing because of this. I think I will spend a bit of time fixing it, so I assign this bug to myself.

If anyone has already done some advances on this topic, please let me know

Fix patch:
diff --git a/source/Body.cpp b/source/Body.cpp
index a7019532..8acdc640 100644
--- a/source/Body.cpp
+++ b/source/Body.cpp
@@ -127,7 +127,7 @@ Body::setup(int number_in,
 	v6 = vec6::Zero();
 
 	// calculate orientation matrix based on latest angles
-	OrMat = r7.quat.toRotationMatrix();
+	OrMat = r7.quat.normalized().toRotationMatrix();
 
 	LOGDBG << "Set up Body " << number << ", type " << TypeName(type) << ". "
 	       << endl;
@@ -269,6 +269,7 @@ Body::setDependentStates()
 		                    rPoint,
 		                    rdPoint); //<<< should double check this function
 
+
 		// pass above to the point and get it to calculate the forces
 		try {
 			attachedP[i]->setKinematics(rPoint, rdPoint);
@@ -288,13 +289,13 @@ Body::setDependentStates()
 
 		// do 3d details of Rod ref point
 		vec tmpr, tmprd;
+		// set first three entires (end A translation) of rRod and rdRod
 		transformKinematics(r6RodRel[i](Eigen::seqN(0, 3)),
 		                    OrMat,
 		                    r7.pos,
 		                    v6,
 		                    tmpr,
-		                    tmprd); // set first three entires (end A
-		                            // translation) of rRod and rdRod
+		                    tmprd);
 		// does the above function need to take in all 6 elements of r6RodRel??
 		rRod(Eigen::seqN(0, 3)) = tmpr;
 		rdRod(Eigen::seqN(0, 3)) = tmprd;
@@ -430,7 +431,7 @@ Body::updateFairlead(real time)
 		a6 = rdd_ves;
 
 		// calculate orientation matrix based on latest angles
-		OrMat = r7.quat.toRotationMatrix();
+		OrMat = r7.quat.normalized().toRotationMatrix();
 
 		// set positions of any dependent points and rods
 		setDependentStates();
@@ -465,7 +466,7 @@ Body::setState(XYZQuat pos, vec6 vel)
 	}
 
 	// calculate orientation matrix based on latest angles
-	OrMat = r7.quat.toRotationMatrix();
+	OrMat = r7.quat.normalized().toRotationMatrix();
 
 	// set positions of any dependent points and rods
 	setDependentStates();
@@ -557,6 +558,11 @@ Body::doRHS()
 	F6net(Eigen::seqN(0, 3)) = Fgrav;
 	F6net(Eigen::seqN(3, 3)) = body_rCGrotated.cross(Fgrav);
 
+	// Centrifugal force due to COM not being at body origin
+	const vec w = v6.tail<3>();
+	F6net.head<3>() -=
+		M.topLeftCorner(3, 3) * (w.cross(w.cross(body_rCGrotated)));
+
 	// --------------------------------- apply wave kinematics
 	// ------------------------------------
 
@@ -587,7 +593,7 @@ Body::doRHS()
 		// orientation)
 		vec6 F6_i;
 		mat6 M6_i;
-		attached->getNetForceAndMass(F6_i, M6_i, r7.pos);
+		attached->getNetForceAndMass(F6_i, M6_i, r7.pos, v6);
 
 		// sum quantitites
 		F6net += F6_i;
@@ -600,7 +606,7 @@ Body::doRHS()
 		// orientation)
 		vec6 F6_i;
 		mat6 M6_i;
-		attached->getNetForceAndMass(F6_i, M6_i, r7.pos);
+		attached->getNetForceAndMass(F6_i, M6_i, r7.pos, v6);
 
 		//			// calculate relative location of rod about body center in
 		// global orientation 			double rRod_i[3];
diff --git a/source/Body.hpp b/source/Body.hpp
index d123f1de..26b00c20 100644
--- a/source/Body.hpp
+++ b/source/Body.hpp
@@ -369,7 +369,7 @@ class Body final : public io::IO, public SuperCFL
 	 * @param r The position
 	 * @param rd The velocity
 	 */
-	void setState(XYZQuat r, vec6 rd);
+	void DECLDIR setState(XYZQuat r, vec6 rd);
 
 	/** @brief calculate the forces and state derivatives of the body
 	 *
diff --git a/source/Misc.hpp b/source/Misc.hpp
index 13c6421f..2514ca72 100644
--- a/source/Misc.hpp
+++ b/source/Misc.hpp
@@ -173,7 +173,7 @@ inline vec3
 canonicalEulerAngles(const quaternion& quat, int a0, int a1, int a2)
 {
 	// From issue #163: https://github.com/FloatingArrayDesign/MoorDyn/issues/163
-	mat3 coeff = quat.toRotationMatrix();
+	mat3 coeff = quat.normalized().toRotationMatrix();
 	vec3 res{};
 	using Index = int;
 	using Scalar = real;
@@ -829,8 +829,8 @@ getH(vec r)
 {
 	mat H;
 	// clang-format off
-	H <<   0, r[2], -r[1], 
-		-r[2],     0, r[0],
+	H << 0, r[2], -r[1],
+		-r[2], 0, r[0],
 		r[1], -r[0], 0;
 	// clang-format on
 	return H;
diff --git a/source/MoorDyn2.cpp b/source/MoorDyn2.cpp
index c0037786..ca14d00d 100644
--- a/source/MoorDyn2.cpp
+++ b/source/MoorDyn2.cpp
@@ -869,14 +869,13 @@ moordyn::MoorDyn::ReadInFile()
 {
 	int i = 0;
 
-	// We are really interested in looking for the writeLog option, to start
-	// logging as soon as possible
 	vector<string> in_txt;
 	if (readFileIntoBuffers(in_txt) != MOORDYN_SUCCESS) {
-		// BREAK
 		return MOORDYN_INVALID_INPUT_FILE;
 	}
-	// Skip until we find the options header line
+
+	// We are really interested in looking for the writeLog option, to start
+	// logging as soon as possible
 	if ((i = findStartOfSection(in_txt, { "OPTIONS" })) != -1) {
 		LOGDBG << "   Reading options:" << endl;
 		// Parse options until the next header or the end of the file
@@ -890,16 +889,31 @@ moordyn::MoorDyn::ReadInFile()
 			const string name = entries[1];
 
 			if (name == "writeLog") {
-				env->writeLog = atoi(entries[0].c_str());
+				env->writeLog = atoi(value.c_str());
 				const moordyn::error_id err = SetupLog();
 				if (err != MOORDYN_SUCCESS)
 					return err;
+				break;
+			}
+			i++;
+		}
+	}
+	// Now we can read all the options
+	if ((i = findStartOfSection(in_txt, { "OPTIONS" })) != -1) {
+		LOGDBG << "   Reading options:" << endl;
+		// Parse options until the next header or the end of the file
+		while ((in_txt[i].find("---") == string::npos) && (i < in_txt.size())) {
+			vector<string> entries = moordyn::str::split(in_txt[i], ' ');
+			if (entries.size() < 2) {
 				i++;
+				continue;
+			}
+			const string name = entries[1];
 
-			} else {
+			if (name != "writeLog") {
 				readOptionsLine(in_txt, i);
-				i++;
 			}
+			i++;
 		}
 	}
 
diff --git a/source/Point.cpp b/source/Point.cpp
index aea793e2..e2eb2979 100644
--- a/source/Point.cpp
+++ b/source/Point.cpp
@@ -299,7 +299,7 @@ Point::getStateDeriv()
 };
 
 void
-Point::getNetForceAndMass(vec6& Fnet_out, mat6& M_out, vec rBody)
+Point::getNetForceAndMass(vec6& Fnet_out, mat6& M_out, vec rBody, vec6 vBody)
 {
 	doRHS();
 
@@ -310,6 +310,8 @@ Point::getNetForceAndMass(vec6& Fnet_out, mat6& M_out, vec rBody)
 	// convert segment net force into 6dof force about body ref point
 	Fnet_out(Eigen::seqN(0, 3)) = Fnet;
 	Fnet_out(Eigen::seqN(3, 3)) = rRel.cross(Fnet);
+	// add the centripetal force
+	Fnet_out(Eigen::seqN(0, 3)) += getCentripetalForce(rBody, vBody.tail<3>());
 
 	// convert segment mass matrix to 6by6 mass matrix about body ref point
 	M_out = translateMass(rRel, M);
diff --git a/source/Point.hpp b/source/Point.hpp
index 466b5498..0e2eb07c 100644
--- a/source/Point.hpp
+++ b/source/Point.hpp
@@ -367,11 +367,13 @@ class Point final : public io::IO, public SuperCFL
 	 * parent body
 	 * @param Fnet_out Output Force about body ref point
 	 * @param M_out Output Mass matrix about body ref point
-	 * @param rBody The body position. If NULL, {0, 0, 0} is considered
+	 * @param rBody The body position
+	 * @param vBody The body velocity
 	 */
 	void getNetForceAndMass(vec6& Fnet_out,
 	                        mat6& M_out,
-	                        vec rBody = vec::Zero());
+	                        vec rBody = vec::Zero(),
+	                        vec6 vBody = vec6::Zero());
 
 	/** @brief Calculates the forces and mass on the point, including from
 	 * attached lines
@@ -419,6 +421,18 @@ class Point final : public io::IO, public SuperCFL
 	 */
 	void saveVTK(const char* filename) const;
 #endif
+
+  private:
+	/** @brief Calculate the centripetal force on a body
+	 * @param r The body position
+	 * @param w The body angular velocity
+	 * @return Centripetal force on the body
+	 */
+	inline vec getCentripetalForce(vec r, vec w) const
+	{
+		return -M * (w.cross(w.cross(this->r - r)));
+	}
+
 };
 
 } // ::moordyn
diff --git a/source/Rod.cpp b/source/Rod.cpp
index 6d868602..06c0c04f 100644
--- a/source/Rod.cpp
+++ b/source/Rod.cpp
@@ -452,7 +452,7 @@ Rod::setState(XYZQuat pos, vec6 vel)
 
 	// update Rod direction unit vector (simply equal to last three entries of
 	// r6)
-	const mat OrMat = r7.quat.toRotationMatrix();
+	const mat OrMat = r7.quat.normalized().toRotationMatrix();
 	q = OrMat * q0;
 }
 
@@ -537,7 +537,7 @@ Rod::setKinematics(vec6 r_in, vec6 rd_in)
 	// update Rod direction unit vector (presumably these were set elsewhere for
 	// pinned Rods)
 	// TODO - don't recalculate OrMat here
-	const mat OrMat = r7.quat.toRotationMatrix();
+	const mat OrMat = r7.quat.normalized().toRotationMatrix();
 	q = OrMat * q0;
 }
 
@@ -554,7 +554,7 @@ Rod::setDependentStates()
 	if (N > 0) {
 		// set end B nodes only if the rod isn't zero length
 		// TODO - determine if q has been calculated here
-		q = r7.quat.toRotationMatrix() * q0;
+		q = r7.quat.normalized().toRotationMatrix() * q0;
 		const vec rRel = UnstrLen * q;
 		r[N] = r[0] + rRel;
 		const vec w = v6.tail<3>();
@@ -741,7 +741,7 @@ Rod::getFnet() const
 
 // calculate the aggregate 6DOF rigid-body force and mass data of the rod
 void
-Rod::getNetForceAndMass(vec6& Fnet_out, mat6& M_out, vec rRef)
+Rod::getNetForceAndMass(vec6& Fnet_out, mat6& M_out, vec rRef, vec6 vRef)
 {
 	// rBody is the location of the body reference point. A NULL pointer value
 	// means the end A coordinates should be used instead.
@@ -765,6 +765,8 @@ Rod::getNetForceAndMass(vec6& Fnet_out, mat6& M_out, vec rRef)
 	// shift net forces and add the existing moments
 	const vec f3net = F6net(Eigen::seqN(0, 3));
 	Fnet_out(Eigen::seqN(3, 3)) = F6net(Eigen::seqN(3, 3)) + rRel.cross(f3net);
+	// add the centripetal force
+	Fnet_out(Eigen::seqN(0, 3)) += getCentripetalForce(rRef, vRef.tail<3>());
 
 	// shift mass matrix to be about ref point
 	M_out = translateMass6(rRel, M6net);
diff --git a/source/Rod.hpp b/source/Rod.hpp
index 3eb7a999..d426183f 100644
--- a/source/Rod.hpp
+++ b/source/Rod.hpp
@@ -431,6 +431,24 @@ class Rod final : public io::IO, public SuperCFL
 	 */
 	inline void setTime(real time) { t = time; }
 
+	/** @brief Get the body kinematics
+	 * @param pos The output position
+	 * @param vel The output velocity
+	 */
+	inline void getState(XYZQuat& pos, vec6& vel) const
+	{
+		pos = r7;
+		vel = v6;
+	}
+
+	/** @brief Get the body kinematics
+	 * @return Position and velocity
+	 */
+	inline std::pair<XYZQuat, vec6> getState() const
+	{
+		return std::make_pair(r7, v6);
+	}
+
 	/** @brief Set the rod state
 	 *
 	 * for a free Rod, there are 12 states:
@@ -520,9 +538,13 @@ class Rod final : public io::IO, public SuperCFL
 	 * parent body
 	 * @param Fnet_out Output Force about body ref point
 	 * @param M_out Output Mass matrix about body ref point
-	 * @param rBody The body position. If NULL, {0, 0, 0} is considered
+	 * @param rBody The body position
+	 * @param vBody The body velocity
 	 */
-	void getNetForceAndMass(vec6& Fnet_out, mat6& M_out, vec rBody);
+	void getNetForceAndMass(vec6& Fnet_out,
+	                        mat6& M_out,
+	                        vec rBody,
+	                        vec6 vBody);
 
 	/** @brief Calculate the force and mass contributions of the point on the
 	 * parent body
@@ -531,7 +553,7 @@ class Rod final : public io::IO, public SuperCFL
 	 */
 	inline void getNetForceAndMass(vec6& Fnet_out, mat6& M_out)
 	{
-		getNetForceAndMass(Fnet_out, M_out, r[0]);
+		getNetForceAndMass(Fnet_out, M_out, r[0], vec6::Zero());
 	}
 
 	/** @brief This is the big function that calculates the forces on the rod,
@@ -580,6 +602,26 @@ class Rod final : public io::IO, public SuperCFL
 	 */
 	void saveVTK(const char* filename) const;
 #endif
+
+  private:
+	/** @brief Calculate the centripetal force on a body
+	 * @param r The body position
+	 * @param w The body angular velocity
+	 * @return Centripetal force on the body
+	 */
+	inline vec getCentripetalForce(vec r, vec w) const
+	{
+		if (!N)
+			return vec::Zero();
+
+		vec F = vec::Zero();
+		for (unsigned int i = 0; i <= N; i++) {
+			F -= M[i] * (w.cross(w.cross(this->r[i] - r)));
+		}
+		return F;
+	}
+
+
 };
 
 } // ::moordyn


Test patch:
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index 392cdc05..5987e7a7 100644
--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -35,12 +35,12 @@ set(CPP_TESTS
     seafloor
     wavekin
     wavekin_7
-    bodies
 )
 
 set(CATCH2_TESTS 
     math_tests
     testDecomposeString
+    bodies
     beam
     conveying_fluid
     polyester
diff --git a/tests/Mooring/body_tests/orbitalBody.txt b/tests/Mooring/body_tests/orbitalBody.txt
new file mode 100644
index 00000000..e13f19f9
--- /dev/null
+++ b/tests/Mooring/body_tests/orbitalBody.txt
@@ -0,0 +1,34 @@
+--------------------- MoorDyn Input File -------------------------------------------------------
+Testing body kinematics when body origin != COM
+---------------------- ROD TYPES ------------------------------------
+TypeName      Diam     Mass/m    Cd     Ca      CdEnd    CaEnd
+(name)        (m)      (kg/m)    (-)    (-)     (-)      (-)
+buoy          1        1.0e1     0.0    0.0     0.0      0.0
+---------------------------- BODIES -----------------------------------------------------
+ID      Attachment  X0     Y0      Z0      r0        p0     y0      Mass  CG*        I*        Volume   CdA*   Ca
+(#)     (-)         (m)    (m)     (m)     (deg)     (deg)  (deg)   (kg)  (m)        (kg-m^2)  (m^3)    (m^2)  (-)
+1       free        0      10      20      0.0       0      0       0     0|-10|0    0         0        0      0
+2       free        0      10      20      0.0       0      0       0     0|-10|0    0         0        0      0
+----------------------- POINTS ----------------------------------------------
+Node      Type       X         Y          Z        M        V          CdA    CA
+(-)       (-)        (m)       (m)        (m)      (kg)     (m^3)      (m^2)  (-)
+1         Body1      0.0       -10        -2       20.0     0          0      0
+2         Body1      0.0       -10        2        20.0     0          0      0
+---------------------- RODS ----------------------------------------
+ID     RodType   Attachment  Xa    Ya    Za    Xb    Yb     Zb    NumSegs   RodOutputs
+(#)    (name)    (#/key)     (m)   (m)   (m)   (m)   (m)    (m)   (-)       (-)
+1      buoy      Body2       0     -10   -2    0     -10.0  2     10        pvf
+-------------------------- SOLVER OPTIONS---------------------------------------------------
+2        writeLog     - Write a log file
+0.0      g            - No gravity
+1e-3     dtM          - time step to use in mooring integration
+3.0e6    kb           - bottom stiffness
+3.0e5    cb           - bottom damping
+70       WtrDpth      - water depth
+3.0      ICDfac       - factor by which to scale drag coefficients during dynamic relaxation IC gen
+0.015    threshIC     - threshold for IC convergence
+0.0      TmaxIC       - threshold for IC convergence
+0.01     dtIC         - Time lapse between convergence tests (s)
+0        Currents     - Whether or not to pull in currents
+0        WaveKin      - Whether or not to pull in waves
+--------------------------- need this line -------------------------------------------------
diff --git a/tests/bodies.cpp b/tests/bodies.cpp
index 83928458..17bb1fc7 100644
--- a/tests/bodies.cpp
+++ b/tests/bodies.cpp
@@ -44,48 +44,11 @@
 #include <limits>
 #include <vector>
 #include <numeric>
-
+#include <catch2/catch_test_macros.hpp>
 #include "util.h"
 
 using namespace std;
 
-#ifdef USE_VTK
-
-bool
-write_system_vtk(MoorDyn in_system, double time, SeriesWriter* series_writer)
-{
-	// if no series writer, we do nothing but pretend to work
-	if (series_writer == NULL) {
-		return true;
-	}
-
-	moordyn::MoorDyn* system = (moordyn::MoorDyn*)(in_system);
-	std::stringstream filename;
-	std::stringstream element_name;
-	element_name << "vtk_system";
-	auto& vtp_series = series_writer->getSeries(element_name.str());
-	auto step_num = vtp_series.time_steps.size();
-
-	filename << element_name.str() << "." << step_num << ".vtm";
-	std::string full_path = "../../vtk_out/" + filename.str();
-	// std::cout << "***     Saving on '" << full_path << "'..." <<
-	// std::endl;
-	vtp_series.time_steps.push_back({ filename.str(), time });
-	system->saveVTK(full_path.c_str());
-	return true;
-}
-
-#else
-class SeriesWriter;
-
-bool
-write_system_vtk(MoorDyn in_system, double time, SeriesWriter* series_writer)
-{
-	return true;
-}
-
-#endif
-
 /**
  * @brief Represents a number of times and data for coupled DOFs
  *
@@ -138,7 +101,6 @@ struct Trajectory
  * @param trajectory The trajectory to use for the coupled DOFs
  * @param t Reference to the time, makes it easier to integrate into program
  * flow
- * @param series_writer Used to write out vtk series
  * @return true Success
  * @return false Failure
  */
@@ -146,8 +108,7 @@ template<typename T>
 bool
 followTrajectory(MoorDyn& system,
                  const Trajectory<T>& trajectory,
-                 double& t,
-                 SeriesWriter* series_writer)
+                 double& t)
 {
 
 	int err;
@@ -162,15 +123,8 @@ followTrajectory(MoorDyn& system,
 		std::vector<double> f(x.size());
 		double dt = trajectory.times[i + 1] - trajectory.times[i];
 		err = MoorDyn_Step(system, x.data(), dx.data(), f.data(), &t, &dt);
-		if (err != MOORDYN_SUCCESS) {
-			cerr << "Failure during the mooring initialization: " << err
-			     << endl;
+		if (err != MOORDYN_SUCCESS)
 			return false;
-		}
-
-		if (!write_system_vtk(system, t, series_writer)) {
-			return false;
-		}
 	}
 	return true;
 }
@@ -179,7 +133,6 @@ followTrajectory(MoorDyn& system,
  * @brief Uses a line between a point on the body and a coupled point to rotate
  * the body around
  *
- *
  * The point on the body starts at (1, 0, 0) relative to the body.
  * Then we drag the point to (0, 0, 1) to rotate the body -90 degrees around the
  * y-axis Then we drag the point to (0, 1, 0) to rotate the body -90 degrees
@@ -188,34 +141,15 @@ followTrajectory(MoorDyn& system,
  *
  * The final result of this should be that the body gets rotated -90 degrees
  * around the x-axis.
- *
- *
- * @param series_writer
- * @return true
- * @return false
  */
-bool
-rotatingBody(SeriesWriter* series_writer)
+TEST_CASE("Rotating body")
 {
-	int err;
-	cout << endl << " => " << __PRETTY_FUNC_NAME__ << "..." << endl;
-
 	MoorDyn system = MoorDyn_Create("Mooring/body_tests/rotatingBody.txt");
-	if (!system) {
-		cerr << "Failure Creating the Mooring system" << endl;
-		return false;
-	}
+	REQUIRE(system);
 
 	unsigned int n_dof;
-	err = MoorDyn_NCoupledDOF(system, &n_dof);
-	if (err != MOORDYN_SUCCESS) {
-		cerr << "Failure getting NCoupledDOF: " << err << endl;
-		return false;
-	}
-	if (n_dof != 3) {
-		cerr << "Expected 3 DOFs but got " << n_dof << endl;
-		return false;
-	}
+	REQUIRE(MoorDyn_NCoupledDOF(system, &n_dof) == MOORDYN_SUCCESS);
+	REQUIRE(n_dof == 3);
 
 	const moordyn::vec3 body_center{ 0, 0, 5 };
 	const moordyn::real radius = 2.0;
@@ -223,29 +157,15 @@ rotatingBody(SeriesWriter* series_writer)
 	moordyn::vec3 dx{ 0, 0, 0 };
 	double f[3];
 
-	err = MoorDyn_Init(system, x.data(), dx.data());
-	if (err != MOORDYN_SUCCESS) {
-		cerr << "Failure during the mooring initialization: " << err << endl;
-		return false;
-	}
+	REQUIRE(MoorDyn_Init(system, x.data(), dx.data()) == MOORDYN_SUCCESS);
 
 	MoorDynPoint point = MoorDyn_GetPoint(system, 8);
-
-	if (!write_system_vtk(system, 0, series_writer)) {
-		return false;
-	}
+	REQUIRE(point);
 
 	double t = 0, dt = 0.1;
 	// do one outer time step just to make sure everything is settled
-	err = MoorDyn_Step(system, x.data(), dx.data(), f, &t, &dt);
-	if (err != MOORDYN_SUCCESS) {
-		cerr << "Failure during the mooring initialization: " << err << endl;
-		return false;
-	}
-
-	if (!write_system_vtk(system, t, series_writer)) {
-		return false;
-	}
+	REQUIRE(MoorDyn_Step(
+		system, x.data(), dx.data(), f, &t, &dt) == MOORDYN_SUCCESS);
 	double start_t = t;
 
 	// goes from (2, 0, 0) to (0, 0, 2) in 2 seconds
@@ -257,36 +177,23 @@ rotatingBody(SeriesWriter* series_writer)
 		x = body_center + (radius * moordyn::vec3{ cos(angle), 0, sin(angle) });
 		return x;
 	});
-	if (!followTrajectory(system, trajectory, t, series_writer)) {
-		return false;
-	}
+	REQUIRE(followTrajectory(system, trajectory, t));
 
 	start_t = t;
 	x = body_center + moordyn::vec3(0.0, 0.0, 0.05 + radius);
 	dx = moordyn::vec3(0, 0, 0.0);
 	// give 0.5 seconds to settle at the top
 	while (t < start_t + 0.5) {
-		err = MoorDyn_Step(system, x.data(), dx.data(), f, &t, &dt);
-		if (err != MOORDYN_SUCCESS) {
-			cerr << "Failure during the mooring initialization: " << err
-			     << endl;
-			return false;
-		}
-		if (!write_system_vtk(system, t, series_writer)) {
-			return false;
-		}
+		REQUIRE(MoorDyn_Step(
+			system, x.data(), dx.data(), f, &t, &dt) == MOORDYN_SUCCESS);
 	}
 
 	moordyn::vec3 point_pos;
-	MoorDyn_GetPointPos(point, point_pos.data());
+	REQUIRE(MoorDyn_GetPointPos(point, point_pos.data()) == MOORDYN_SUCCESS);
 	{
 		moordyn::vec3 rel_pos = point_pos - body_center;
 		moordyn::vec3 expected_pos{ 0, 0, 1.0 };
-		if (!allclose(rel_pos, expected_pos, 0.1, 0.15)) {
-			cerr << "Point 8 relative to body center should be "
-			     << expected_pos.transpose() << " but was "
-			     << rel_pos.transpose() << " at t = " << t << endl;
-		}
+		REQUIRE(allclose(rel_pos, expected_pos, 0.1, 0.15));
 	}
 
 	start_t = t;
@@ -300,36 +207,23 @@ rotatingBody(SeriesWriter* series_writer)
 		x = body_center + (radius * moordyn::vec3{ 0, sin(angle), cos(angle) });
 		return x;
 	});
-	if (!followTrajectory(system, trajectory2, t, series_writer)) {
-		return false;
-	}
+	REQUIRE(followTrajectory(system, trajectory2, t));
 
 	start_t = t;
 	x = body_center + moordyn::vec3(0.0, radius + 0.05, 0);
 	dx = moordyn::vec3(0, 0.0, 0.0);
 	// give 0.5 seconds to settle at the top
 	while (t < start_t + 0.5) {
-		err = MoorDyn_Step(system, x.data(), dx.data(), f, &t, &dt);
-		if (err != MOORDYN_SUCCESS) {
-			cerr << "Failure during the mooring initialization: " << err
-			     << endl;
-			return false;
-		}
-		if (!write_system_vtk(system, t, series_writer)) {
-			return false;
-		}
+		REQUIRE(MoorDyn_Step(
+			system, x.data(), dx.data(), f, &t, &dt) == MOORDYN_SUCCESS);
 	}
 
-	MoorDyn_GetPointPos(point, point_pos.data());
+	REQUIRE(MoorDyn_GetPointPos(point, point_pos.data()) == MOORDYN_SUCCESS);
 	{
 
 		moordyn::vec3 rel_pos = point_pos - body_center;
 		moordyn::vec3 expected_pos{ 0, 1.0, 0.0 };
-		if (!allclose(rel_pos, expected_pos, 0.1, 0.15)) {
-			cerr << "Point 8 relative to body center should be "
-			     << expected_pos.transpose() << " but was "
-			     << rel_pos.transpose() << " at t = " << t << endl;
-		}
+		REQUIRE(allclose(rel_pos, expected_pos, 0.1, 0.15));
 	}
 
 	start_t = t;
@@ -343,41 +237,30 @@ rotatingBody(SeriesWriter* series_writer)
 		x = body_center + (radius * moordyn::vec3{ sin(angle), cos(angle), 0 });
 		return x;
 	});
-	if (!followTrajectory(system, trajectory3, t, series_writer)) {
-		return false;
-	}
+	REQUIRE(followTrajectory(system, trajectory3, t));
 
 	start_t = t;
 	x = body_center + moordyn::vec3(radius + 0.05, 0, 0);
 	dx = moordyn::vec3(0, 0.0, 0.0);
 	// give 0.1 seconds to settle at the top
 	while (t < start_t + 0.5) {
-		err = MoorDyn_Step(system, x.data(), dx.data(), f, &t, &dt);
-		if (err != MOORDYN_SUCCESS) {
-			cerr << "Failure during the mooring initialization: " << err
-			     << endl;
-			return false;
-		}
-		if (!write_system_vtk(system, t, series_writer)) {
-			return false;
-		}
+		REQUIRE(MoorDyn_Step(
+			system, x.data(), dx.data(), f, &t, &dt) == MOORDYN_SUCCESS);
 	}
 
-	MoorDyn_GetPointPos(point, point_pos.data());
+	REQUIRE(MoorDyn_GetPointPos(point, point_pos.data()) == MOORDYN_SUCCESS);
 	{
 
 		moordyn::vec3 rel_pos = point_pos - body_center;
 		moordyn::vec3 expected_pos{ 1.0, 0.0, 0.0 };
-		if (!allclose(rel_pos, expected_pos, 0.1, 0.15)) {
-			cerr << "Point 8 relative to body center should be "
-			     << expected_pos.transpose() << "but was "
-			     << rel_pos.transpose() << " at t=" << t << endl;
-		}
+		REQUIRE(allclose(rel_pos, expected_pos, 0.1, 0.15));
 	}
 
 	auto body = MoorDyn_GetBody(system, 1);
+	REQUIRE(body);
 	moordyn::vec6 r, rd;
-	MoorDyn_GetBodyState(body, r.data(), rd.data());
+	REQUIRE(MoorDyn_GetBodyState(
+		body, r.data(), rd.data()) == MOORDYN_SUCCESS);
 	// We want axis-angle representation, and it's easier to compute that from
 	// quaternion so we convert back to quaternion
 	auto xyz_quat = moordyn::XYZQuat::fromVec6(r);
@@ -386,14 +269,9 @@ rotatingBody(SeriesWriter* series_writer)
 	double angle = moordyn::rad2deg * 2 * acos(q.w());
 	double denom = (sqrt(1 - q.w() * q.w()));
 	moordyn::vec3 axis{ q.x() / denom, q.y() / denom, q.z() / denom };
-	if (!(abs(axis.x()) > 0.85 && abs(axis.y()) < 0.2 &&
-	      abs(axis.z()) < 0.2)) { // if we are just checking axis direction then
-		                          // +/- does not matter
-		cerr << "The final rotation of the body in angle axis form should "
-		        "have an axis in the x direction, but axis is "
-		     << axis.transpose() << endl;
-		return false;
-	}
+	REQUIRE((abs(axis.x()) > 0.85 &&
+	         abs(axis.y()) < 0.2 &&
+	         abs(axis.z()) < 0.2));
 	// normalize angle between +180 and -180
 	while (angle > 180.) {
 		angle -= 360;
@@ -401,29 +279,17 @@ rotatingBody(SeriesWriter* series_writer)
 	while (angle < -180) {
 		angle += 360;
 	}
-	if (!(abs(angle - 90) < 10)) {
-		cerr << "The final rotation of the body in angle-axis form should "
-		        "have a angle near 90 degrees but angle is "
-		     << angle << endl;
-		return false;
-	}
+	REQUIRE(abs(angle - 90) < 10);
 
 	cout << "Body r = " << r.transpose() << endl;
 	cout << "Axis-Angle rotation: axis = " << axis.transpose()
 	     << ", angle = " << angle << " degrees" << endl;
-	err = MoorDyn_Close(system);
-	if (err != MOORDYN_SUCCESS) {
-		cerr << "Failure closing Moordyn: " << err << endl;
-		return false;
-	}
-
-	return true;
+	REQUIRE(MoorDyn_Close(system) == MOORDYN_SUCCESS);
 }
 
 /**
  * @brief Compares inertial deflection of a pinned body to analytical solution
  *
- *
  * The coupled pinned body that is massless and volumeless with a rod fixed to
  * it is moved with constant acceleration in a vaccum (0 water density). The
  * resulting avg inertial deflection should match an analytical solution of
@@ -434,76 +300,39 @@ rotatingBody(SeriesWriter* series_writer)
  *
  * This only tests the inertial properties of pinned bodies, other tests deal
  * with hydrodynamics and general body properties
- *
- *
- * @param series_writer
- * @return true
- * @return false
  */
-bool
-pinnedBody(SeriesWriter* series_writer)
+TEST_CASE("Pinned body")
 {
-	int err;
-	cout << endl << " => " << __PRETTY_FUNC_NAME__ << "..." << endl;
-
 	MoorDyn system = MoorDyn_Create("Mooring/body_tests/pinnedBody.txt");
-	if (!system) {
-		cerr << "Failure Creating the Mooring system" << endl;
-		return false;
-	}
+	REQUIRE(system);
 
 	unsigned int n_dof;
-	err = MoorDyn_NCoupledDOF(system, &n_dof);
-	if (err != MOORDYN_SUCCESS) {
-		cerr << "Failure getting NCoupledDOF: " << err << endl;
-		return false;
-	}
-	if (n_dof !=
-	    6) { // rotational DOF are ignored by MDC, same as a coupled pinned rods
-		cerr << "Expected 6 DOFs but got " << n_dof << endl;
-		return false;
-	}
+	REQUIRE(MoorDyn_NCoupledDOF(system, &n_dof) == MOORDYN_SUCCESS);
+	REQUIRE(n_dof ==6);
 
 	moordyn::vec6 x{ 0, 0, -5, 0, 0, 0 };
 	moordyn::vec6 xd{ 0, 0, 0, 0, 0, 0 };
 	double f[6];
 
-	err = MoorDyn_Init(system, x.data(), xd.data());
-	if (err != MOORDYN_SUCCESS) {
-		cerr << "Failure during the mooring initialization: " << err << endl;
-		return false;
-	}
-
-	if (!write_system_vtk(system, 0, series_writer)) {
-		return false;
-	}
+	REQUIRE(MoorDyn_Init(system, x.data(), xd.data()) == MOORDYN_SUCCESS);
 
 	auto body = MoorDyn_GetBody(system, 1);
+	REQUIRE(body);
 	moordyn::vec6 r, rd;
 	vector<double> roll;
 	int i = 0, j = 0;
 	double t = 0.0, dt = 0.01, accel = 0.5;
 	bool local_min_max;
 
-	if (!write_system_vtk(system, t, series_writer)) {
-		return false;
-	}
-
 	while (t < 50.0) {
 
 		x[1] = 0.5 * accel * pow(t, 2);
 		xd[1] = accel * t;
-		err = MoorDyn_Step(system, x.data(), xd.data(), f, &t, &dt);
-		if (err != MOORDYN_SUCCESS) {
-			cerr << "Failure during the mooring initialization: " << err
-			     << endl;
-			return false;
-		}
-		if (!write_system_vtk(system, t, series_writer)) {
-			return false;
-		}
+		REQUIRE(MoorDyn_Step(
+			system, x.data(), xd.data(), f, &t, &dt) == MOORDYN_SUCCESS);
 
-		MoorDyn_GetBodyState(body, r.data(), rd.data());
+		REQUIRE(MoorDyn_GetBodyState(
+			body, r.data(), rd.data()) == MOORDYN_SUCCESS);
 		roll.push_back(r[3]);
 
 		if (i >= 30) { // after the simulation has run for a few time steps
@@ -522,120 +351,84 @@ pinnedBody(SeriesWriter* series_writer)
 	}
 	double theta = atan(-accel / 9.80665);
 	double average = reduce(roll.begin(), roll.end()) / roll.size();
-	if (abs(average - theta) > 0.001) {
-		cerr << "Pinned body inertial deflection should be " << theta
-		     << " but it is " << average << endl;
-		return false;
-	}
-
-	err = MoorDyn_Close(system);
-	if (err != MOORDYN_SUCCESS) {
-		cerr << "Failure closing Moordyn: " << err << endl;
-		return false;
-	}
+	REQUIRE(abs(average - theta) <= 0.001);
 
-	cout << setprecision(4) << "Average roll is " << average << endl;
-	cout << setprecision(4) << "Theoretical roll is " << theta << endl;
+	REQUIRE(MoorDyn_Close(system) == MOORDYN_SUCCESS);
 
-	return true;
+	// NOTE: jlcercos has dropped setprecision since it was not restored to the
+	// default value afterwards, which might impair the subsequent exexutions
+	cout << "Average roll is " << average << endl;
+	cout << "Theoretical roll is " << theta << endl;
 }
 
-bool
-bodyDrag(SeriesWriter* series_writer)
+TEST_CASE("Body drag")
 {
-	int err;
-	cout << endl << " => " << __PRETTY_FUNC_NAME__ << "..." << endl;
-
 	MoorDyn system = MoorDyn_Create("Mooring/body_tests/bodyDrag.txt");
-	if (!system) {
-		cerr << "Failure Creating the Mooring system" << endl;
-		return false;
-	}
+	REQUIRE(system);
 
 	unsigned int n_dof;
-	err = MoorDyn_NCoupledDOF(system, &n_dof);
-	if (err != MOORDYN_SUCCESS) {
-		cerr << "Failure getting NCoupledDOF: " << err << endl;
-		return false;
-	}
-	if (n_dof != 0) {
-		cerr << "Expected 0 DOFs but got " << n_dof << endl;
-		return false;
-	}
+	REQUIRE(MoorDyn_NCoupledDOF(system, &n_dof) == MOORDYN_SUCCESS);
+	REQUIRE(n_dof == 0);
 
 	double f[3];
 
-	err = MoorDyn_Init(system, nullptr, nullptr);
-	if (err != MOORDYN_SUCCESS) {
-		cerr << "Failure during the mooring initialization: " << err << endl;
-		return false;
-	}
-
-	if (!write_system_vtk(system, 0, series_writer)) {
-		return false;
-	}
+	REQUIRE(MoorDyn_Init(system, nullptr, nullptr) == MOORDYN_SUCCESS);
 
 	double t = 0, dt = 0.1;
 	double max_t = 5;
 	while (t < max_t) {
 		// do one outer time step just to make sure everything is settled
-		err = MoorDyn_Step(system, nullptr, nullptr, f, &t, &dt);
-		if (err != MOORDYN_SUCCESS) {
-			cerr << "Failure during the mooring dynamics: " << err << endl;
-			return false;
-		}
-
-		if (!write_system_vtk(system, t, series_writer)) {
-			return false;
-		}
+		REQUIRE(MoorDyn_Step(
+			system, nullptr, nullptr, f, &t, &dt) == MOORDYN_SUCCESS);
 	}
 
-	err = MoorDyn_Close(system);
-	if (err != MOORDYN_SUCCESS) {
-		cerr << "Failure closing Moordyn: " << err << endl;
-		return false;
-	}
-
-	return true;
+	REQUIRE(MoorDyn_Close(system) == MOORDYN_SUCCESS);
 }
 
-/** @brief Runs all the test
- * @return 0 if the tests have ran just fine. The index of the failing test
- * otherwise
- */
-int
-main(int, char**)
+TEST_CASE("Excentric body")
 {
-	try {
-		// SeriesWriter series_writer;
-		if (!rotatingBody(NULL)) {
-			// series_writer.writeJson("../../vtk_out/");
-			return 3;
-		}
-		// series_writer.writeJson("../../vtk_out/");
-
-	} catch (std::exception& e) {
-		cerr << "rotatingBody failed with exception " << e.what() << endl;
-		return 3;
-	}
+	moordyn::MoorDyn system{ "Mooring/body_tests/orbitalBody.txt" };
 
-	try {
-		// SeriesWriter series_writer;
-		if (!pinnedBody(NULL)) {
-			// series_writer.writeJson("../../vtk_out/");
-			return 3;
+	double f[3];
+	const double radius = 10.0;
+	const double omega = 0.2;
+	const moordyn::vec r0 = moordyn::vec(0, 0, 20.0);
+	auto bodies = system.GetBodies();
+	for (auto body : bodies)
+	{
+		const auto [pos, vel] = body->getState();
+		const moordyn::vec6 new_vel = moordyn::vec6(
+		    0.0, 0.0, radius * omega, omega, 0.0, 0.0);
+		body->setState(pos, new_vel);
+	}
+
+	REQUIRE(system.Init(nullptr, nullptr) == MOORDYN_SUCCESS);
+
+	double t = 0, dt = 0.25;
+	double t_max = 10.0;
+
+	// I do this just so that my first vtk output includes correct forces and
+	// stuff
+	double small_dt = 1e-6;
+	REQUIRE(system.Step(NULL, NULL, f, t, small_dt) == MOORDYN_SUCCESS);
+	while ((t_max - t) > (0.1 * dt)) {
+		REQUIRE(system.Step(NULL, NULL, f, t, dt) == MOORDYN_SUCCESS);
+		for (auto body : bodies)
+		{
+			const auto [pos, vel] = body->getState();
+			const moordyn::vec3 global_pos = pos.pos;
+			const moordyn::vec q = (global_pos - r0) / radius;
+			const moordyn::real w = atan2(q.z(), q.y()) / t;
+			const auto err_r = (global_pos - r0).norm() - radius;
+			const auto err_w = atan2(q.z(), q.y()) / t - omega;
+
+			// Check that we are orbiting at the right radius
+			REQUIRE(abs(err_r) <= 1e-6 * radius);
+			// Check that we are orbiting at the right velocity
+			// NOTE: While orbiting around the rod the angular speed is really
+			// stable. The same cannot be said of the orbital kinematics around
+			// the point, where the velocity can be almost 0.3 rad/s
+			REQUIRE(abs(err_w) <= 1e-6 * omega);
 		}
-		// series_writer.writeJson("../../vtk_out/");
-
-	} catch (std::exception& e) {
-		cerr << "pinnedBody failed with exception " << e.what() << endl;
-		return 3;
 	}
-
-	if (!bodyDrag(NULL)) {
-		return 2;
-	}
-
-	cout << "bodies.cpp passed successfully" << endl;
-	return 0;
 }