From ffa8cf078b72ca93eb1dd6ebb8c17b5a31fd0dcc Mon Sep 17 00:00:00 2001
From: acrovato <a.crovato@uliege.be>
Date: Tue, 1 Oct 2024 17:37:31 +0200
Subject: [PATCH] Add pitch and plunge to GAF and gradients. Ensure that xRef
 and zRef are consistent throughout the computation.

---
 sdpm/src/sdpmBody.cpp     |   8 +--
 sdpm/src/sdpmBody.h       |   5 +-
 sdpm/src/sdpmGradient.cpp | 102 ++++++++++++++++++++++++--------------
 sdpm/src/sdpmGradient.h   |  42 ++++++++--------
 sdpm/src/sdpmMotion.cpp   |   9 ++--
 sdpm/src/sdpmMotion.h     |   9 ++--
 sdpm/src/sdpmProblem.cpp  |   2 +-
 sdpm/src/sdpmSolver.cpp   |   7 ++-
 sdpm/tests/lann.py        |   2 +-
 sdpm/tests/lann_ad.py     |   2 +-
 sdpm/tests/lann_grad.py   |  17 +++++--
 sdpm/tests/lann_xsonic.py |   2 +-
 12 files changed, 122 insertions(+), 85 deletions(-)

diff --git a/sdpm/src/sdpmBody.cpp b/sdpm/src/sdpmBody.cpp
index 1b50be4..134a9d0 100644
--- a/sdpm/src/sdpmBody.cpp
+++ b/sdpm/src/sdpmBody.cpp
@@ -200,9 +200,9 @@ void Body::addMotion()
 /**
  * @brief Set rigid motion parameters
  */
-void Body::setRigidMotion(size_t m, double aAmp, double hAmp, double xRef, double zRef)
+void Body::setRigidMotion(size_t m, double aAmp, double hAmp)
 {
-    _motions[m]->setRigid(aAmp, hAmp, xRef, zRef);
+    _motions[m]->setRigid(aAmp, hAmp);
 }
 
 /**
@@ -246,14 +246,14 @@ std::pair<std::vector<Element *>, std::vector<Element *>> Body::getTrailingEdgeE
 /**
  * @brief Compute the motion-induced velocity on the body surface
  */
-void Body::computeVelocity(size_t m, sdpmVector3d const &ui)
+void Body::computeVelocity(size_t m, sdpmVector3d const &ui, sdpmDouble xRef, sdpmDouble zRef)
 {
     std::vector<Element *> elems = getElements();
     std::vector<sdpmVector3d> ucS(elems.size()), ucH(elems.size());
     for (size_t i = 0; i < elems.size(); ++i)
     {
         ucS[i] = _motions[m]->computeSteady(*elems[i], ui);
-        ucH[i] = _motions[m]->computeHarmonic(*elems[i]);
+        ucH[i] = _motions[m]->computeHarmonic(*elems[i], xRef, zRef);
     }
     _iVelocityS[m] = ucS;
     _iVelocityH[m] = ucH;
diff --git a/sdpm/src/sdpmBody.h b/sdpm/src/sdpmBody.h
index 2754d2c..a6bb652 100644
--- a/sdpm/src/sdpmBody.h
+++ b/sdpm/src/sdpmBody.h
@@ -87,7 +87,7 @@ public:
     sdpmDouble getUnsteadyMomentCoeffImag(size_t imd, size_t ifq) const { return _cm1Im[imd][ifq]; }
     void setNodes(std::vector<std::vector<double>> const &coords);
     void addMotion();
-    void setRigidMotion(size_t m, double aAmp, double hAmp, double xRef, double zRef);
+    void setRigidMotion(size_t m, double aAmp, double hAmp);
     void setFlexibleMotion(size_t m, double mAmp, std::vector<std::vector<double>> const &xMod);
     void setTransonicPressureGrad(std::vector<double> const &dCpA);
 #ifndef SWIG
@@ -96,6 +96,7 @@ public:
     Wake const &getWake() const { return *_wake; }
     size_t getNumMotions() const { return _motions.size(); }
     sdpmDouble getPitchAmplitude(size_t imd) const { return _motions[imd]->getPitchAmplitude(); }
+    sdpmDouble getPlungeAmplitude(size_t imd) const { return _motions[imd]->getPlungeAmplitude(); }
     std::vector<sdpmDouble> const &getModeZ(size_t imd) const { return _motions[imd]->getModeZ(); }
     std::vector<sdpmVector3d> const &getSteadyVelocity(size_t imd) const { return _iVelocityS[imd]; };
     std::vector<sdpmVector3d> const &getHarmonicVelocity(size_t imd) const { return _iVelocityH[imd]; };
@@ -110,7 +111,7 @@ public:
     inline void setUnsteadyDragCoeff(size_t imd, size_t ifq, sdpmComplex cd);
     inline void setUnsteadySideforceCoeff(size_t imd, size_t ifq, sdpmComplex cs);
     inline void setUnsteadyMomentCoeff(size_t imd, size_t ifq, sdpmComplex cm);
-    void computeVelocity(size_t imd, sdpmVector3d const &ui);
+    void computeVelocity(size_t imd, sdpmVector3d const &ui, sdpmDouble xRef, sdpmDouble zRef);
     virtual void write(std::ostream &out) const override;
 #endif
 };
diff --git a/sdpm/src/sdpmGradient.cpp b/sdpm/src/sdpmGradient.cpp
index cfa0a80..1603da9 100644
--- a/sdpm/src/sdpmGradient.cpp
+++ b/sdpm/src/sdpmGradient.cpp
@@ -34,6 +34,7 @@ Gradient::Gradient(Solver &sol) : _sol(sol),
     _nN = _rowsN.size();
 
     // Resize displacement and load containers
+    _x = sdpmVectorXd::Zero(_nN);
     _dz.resize(_nM, sdpmVectorXd::Zero(_nN));
     _clz1Re.resize(_nM, std::vector<sdpmVectorXd>(_nF, sdpmVectorXd::Zero(_nN)));
     _clz1Im.resize(_nM, std::vector<sdpmVectorXd>(_nF, sdpmVectorXd::Zero(_nN)));
@@ -63,6 +64,10 @@ Gradient::Gradient(Solver &sol) : _sol(sol),
     _dCp1Im_dzDotDot.resize(_nF, sdpmMatrixXd::Zero(_sol._nP, _sol._nP));
 
     // Resize load gradient containers
+    _dClz1Re_a.resize(_nF, sdpmVectorXd::Zero(_nN));
+    _dClz1Im_a.resize(_nF, sdpmVectorXd::Zero(_nN));
+    _dClz1Re_h.resize(_nF, sdpmVectorXd::Zero(_nN));
+    _dClz1Im_h.resize(_nF, sdpmVectorXd::Zero(_nN));
     _dClz1Re_rx.resize(_nF, sdpmMatrixXd::Zero(_nN, _nN));
     _dClz1Im_rx.resize(_nF, sdpmMatrixXd::Zero(_nN, _nN));
     _dClz1Re_ry.resize(_nF, sdpmMatrixXd::Zero(_nN, _nN));
@@ -81,19 +86,24 @@ Gradient::~Gradient()
  */
 void Gradient::run()
 {
-    // Store loads and displacements
+    // Store coordinates, displacements and loads
     for (auto body : _sol._pbl.getBodies())
     {
+        sdpmDouble xRef = _sol._pbl.getRefCtr()(0);
+        std::vector<Node *> nods = body->getNodes();
+        for (auto n : nods)
+            _x(_rowsN.at(n)) = n->getCoords()(0) - xRef;
         for (size_t imd = 0; imd < _nM; ++imd)
         {
+            sdpmDouble aAmp = body->getPitchAmplitude(imd);
+            sdpmDouble hAmp = body->getPlungeAmplitude(imd);
             std::vector<sdpmDouble> dz = body->getModeZ(imd);
-            for (auto n : body->getNodes())
-                _dz[imd](_rowsN.at(n)) = dz[n->getId() - 1];
+            for (auto n : nods)
+                _dz[imd](_rowsN.at(n)) = dz[n->getId() - 1] + aAmp * _x(_rowsN.at(n)) + hAmp;
             for (size_t ifq = 0; ifq < _nF; ++ifq)
             {
                 std::vector<sdpmVector3d> clzre = body->getUnsteadyLoadsReal(imd, ifq);
                 std::vector<sdpmVector3d> clzim = body->getUnsteadyLoadsImag(imd, ifq);
-                std::vector<Node *> nods = body->getNodes();
                 for (size_t i = 0; i < nods.size(); ++i)
                 {
                     _clz1Re[imd][ifq](_rowsN.at(nods[i])) = clzre[i](2);
@@ -167,7 +177,7 @@ void Gradient::run()
             Kx[idim] = dN[idim] * L * K;
 
         // Pressure stiffness terms (multiplied by 1)
-        // w.r.t angle of attack
+        // w.r.t pitch
         sdpmVectorXcd cp_a = (2 / beta * u.col(0).array() - 2 * mach * mach / beta * ux.array()) * (uinf(0) * n.col(2).array() * n.col(0).array() + (Kx[0] * uinf(0) * n.col(2)).array()) + 2 * u.col(1).array() * (uinf(0) * n.col(2).array() * n.col(1).array() + (Kx[1] * uinf(0) * n.col(2)).array()) + 2 * u.col(2).array() * (-uinf(0) + uinf(0) * n.col(2).array() * n.col(2).array() + (Kx[2] * uinf(0) * n.col(2)).array());
         _dCp1Re_a[ifq] = cp_a.real();
         _dCp1Im_a[ifq] = cp_a.imag();
@@ -181,12 +191,12 @@ void Gradient::run()
         _dCp1Im_ry[ifq] = cp_ry.imag();
 
         // Pressure damping terms (multiplied by i*omega)
-        // w.r.t time derivative of angle of attack
+        // w.r.t time derivative of pitch
         sdpmVectorXcd cp_aDot = 2 * u.col(0).array() * (cg.col(2).array() - xRef(2)) + (2 / beta * u.col(0).array() - 2 * mach * mach / beta * ux.array()) * ((-(cg.col(2).array() - xRef(2)) / beta * n.col(0).array() + (cg.col(0).array() - xRef(0)) * n.col(2).array()) * n.col(0).array() + (Kx[0] * (-(cg.col(2).array() - xRef(2)) / beta * n.col(0).array() + (cg.col(0).array() - xRef(0)) * n.col(2).array()).matrix()).array()) + 2 * u.col(1).array() * ((-(cg.col(2).array() - xRef(2)) / beta * n.col(0).array() + (cg.col(0).array() - xRef(0)) * n.col(2).array()) * n.col(1).array() + (Kx[1] * (-(cg.col(2).array() - xRef(2)) / beta * n.col(0).array() + (cg.col(0).array() - xRef(0)) * n.col(2).array()).matrix()).array()) + 2 * u.col(2).array() * (-(cg.col(0).array() - xRef(0)) + (-(cg.col(2).array() - xRef(2)) / beta * n.col(0).array() + (cg.col(0).array() - xRef(0)) * n.col(2).array()) * n.col(2).array() + (Kx[2] * (-(cg.col(2).array() - xRef(2)) / beta * n.col(0).array() + (cg.col(0).array() - xRef(0)) * n.col(2).array()).matrix()).array()) + 2 * (K * uinf(0) * n.col(2)).array() * (1 - mach * mach * ux.array());
         cp_aDot *= sdpmComplex(0., omega[ifq]);
         _dCp1Re_aDot[ifq] = cp_aDot.real();
         _dCp1Im_aDot[ifq] = cp_aDot.imag();
-        // w.r.t time derivative of z-displacement
+        // w.r.t time derivative of plunge
         sdpmVectorXcd cp_hDot = (2 / beta * u.col(0).array() - 2 * mach * mach / beta * ux.array()) * (n.col(2).array() * n.col(0).array() + (Kx[0] * n.col(2)).array()) + 2 * u.col(1).array() * (n.col(2).array() * n.col(1).array() + (Kx[1] * n.col(2)).array()) + 2 * u.col(2).array() * (-1 + n.col(2).array() * n.col(2).array() + (Kx[2] * n.col(2)).array());
         cp_hDot *= sdpmComplex(0., omega[ifq]);
         _dCp1Re_hDot[ifq] = cp_hDot.real();
@@ -206,11 +216,11 @@ void Gradient::run()
         _dCp1Im_dzDot[ifq] = cp_dzDot.imag();
 
         // Pressure inertia terms (multiplied by (i*omega)^2)
-        // w.r.t second time derivative of angle of attack
+        // w.r.t second time derivative of pitch
         sdpmVectorXcd cp_aDotDot = -2 * omega[ifq] * omega[ifq] * (1 - mach * mach * ux.array()) * (K * (-(cg.col(2).array() - xRef(2)) / beta * n.col(0).array() + (cg.col(0).array() - xRef(0)) * n.col(2).array()).matrix()).array();
         _dCp1Re_aDotDot[ifq] = cp_aDotDot.real();
         _dCp1Im_aDotDot[ifq] = cp_aDotDot.imag();
-        // w.r.t second time derivative of z-displacement
+        // w.r.t second time derivative of plunge
         sdpmVectorXcd cp_hDotDot = -2 * omega[ifq] * omega[ifq] * (1 - mach * mach * ux.array()) * (K * n.col(2)).array();
         _dCp1Re_hDotDot[ifq] = cp_hDotDot.real();
         _dCp1Im_hDotDot[ifq] = cp_hDotDot.imag();
@@ -220,6 +230,12 @@ void Gradient::run()
         _dCp1Im_dzDotDot[ifq] = cp_dzDotDot.imag();
 
         // Load terms
+        sdpmVectorXcd clz_a = L1.transpose() * s.asDiagonal() * nz.asDiagonal() * (cp_a + cp_aDot + cp_aDotDot);
+        _dClz1Re_a[ifq] = clz_a.real();
+        _dClz1Im_a[ifq] = clz_a.imag();
+        sdpmVectorXcd clz_h = L1.transpose() * s.asDiagonal() * nz.asDiagonal() * (cp_hDot + cp_hDotDot);
+        _dClz1Re_h[ifq] = clz_h.real();
+        _dClz1Im_h[ifq] = clz_h.imag();
         sdpmMatrixXcd clz_rx = L1.transpose() * s.asDiagonal() * nz.asDiagonal() * (cp_rx + cp_rxDot) * L1;
         _dClz1Re_rx[ifq] = clz_rx.real();
         _dClz1Im_rx[ifq] = clz_rx.imag();
@@ -240,21 +256,36 @@ std::vector<sdpmDouble> Gradient::computePartialsGafRe(size_t ifq, int imd, int
 {
     // Check input
     sdpmMatrixXd dClz1;
-    if (wrt == "rx")
+    if (wrt == "a")
+        dClz1 = _dClz1Re_a[ifq];
+    else if (wrt == "h")
+        dClz1 = _dClz1Re_h[ifq];
+    else if (wrt == "rx")
         dClz1 = _dClz1Re_rx[ifq];
     else if (wrt == "ry")
         dClz1 = _dClz1Re_ry[ifq];
     else if (wrt == "dz")
         dClz1 = _dClz1Re_dz[ifq];
     else
-        throw std::runtime_error("sdpm::Gradient::computePartialsGafRe: wrt parameter should be either 'rx', 'ry' or 'dz'!\n");
+        throw std::runtime_error("sdpm::Gradient::computePartialsGafRe: wrt parameter should be either 'a', 'h', 'rx', 'ry' or 'dz'!\n");
 
     // Compute derivative
-    sdpmVectorXd dq = sdpmVectorXd::Zero(_nN);
+    sdpmVectorXd dq;
+    if (wrt == "a" || wrt == "h")
+        dq = sdpmVectorXd::Zero(1);
+    else
+        dq = sdpmVectorXd::Zero(_nN);
     if (jcl == imd)
         dq += dClz1.transpose() * _dz[irw];
-    if (irw == imd && wrt == "dz")
-        dq -= _clz1Re[jcl][ifq];
+    if (irw == imd)
+    {
+        if (wrt == "a")
+            dq -= _x.transpose() * _clz1Re[jcl][ifq];
+        else if (wrt == "h")
+            dq -= sdpmVectorXd::Constant(_nN, 1.0).transpose() * _clz1Re[jcl][ifq];
+        else if (wrt == "dz")
+            dq -= _clz1Re[jcl][ifq];
+    }
     return std::vector<sdpmDouble>(dq.data(), dq.data() + dq.size());
 }
 
@@ -265,42 +296,37 @@ std::vector<sdpmDouble> Gradient::computePartialsGafIm(size_t ifq, int imd, int
 {
     // Check input
     sdpmMatrixXd dClz1;
-    if (wrt == "rx")
+    if (wrt == "a")
+        dClz1 = _dClz1Im_a[ifq];
+    else if (wrt == "h")
+        dClz1 = _dClz1Im_h[ifq];
+    else if (wrt == "rx")
         dClz1 = _dClz1Im_rx[ifq];
     else if (wrt == "ry")
         dClz1 = _dClz1Im_ry[ifq];
     else if (wrt == "dz")
         dClz1 = _dClz1Im_dz[ifq];
     else
-        throw std::runtime_error("sdpm::Gradient::computePartialsGafIm: wrt parameter should be either 'rx', 'ry' or 'dz'!\n");
+        throw std::runtime_error("sdpm::Gradient::computePartialsGafIm: wrt parameter should be either 'a', 'h', 'rx', 'ry' or 'dz'!\n");
 
     // Compute derivative
-    sdpmVectorXd dq = sdpmVectorXd::Zero(_nN);
+    sdpmVectorXd dq;
+    if (wrt == "a" || wrt == "h")
+        dq = sdpmVectorXd::Zero(1);
+    else
+        dq = sdpmVectorXd::Zero(_nN);
     if (jcl == imd)
         dq += dClz1.transpose() * _dz[irw];
-    if (irw == imd && wrt == "dz")
-        dq -= _clz1Im[jcl][ifq];
-    return std::vector<sdpmDouble>(dq.data(), dq.data() + dq.size());
-}
-
-/**
- * @brief Test pressure derivatives by mulitplying with DOF and comparing to pressure
- * @todo remove
- */
-void Gradient::test(double a_amp, double h_amp)
-{
-    sdpmVectorXd cp1re = (_dCp1Re_a[0] + _dCp1Re_aDot[0] + _dCp1Re_aDotDot[0]) * a_amp + (_dCp1Re_hDot[0] + _dCp1Re_hDotDot[0]) * h_amp;
-    sdpmVectorXd cp1im = (_dCp1Im_a[0] + _dCp1Im_aDot[0] + _dCp1Im_aDotDot[0]) * a_amp + (_dCp1Im_hDot[0] + _dCp1Im_hDotDot[0]) * h_amp;
-    for (auto b : _sol._pbl.getBodies())
+    if (irw == imd)
     {
-        for (auto e : b->getElements())
-        {
-            sdpmDouble deltaCp1Re = _sol._cp1Re[0][0][e->getId() - 1] - cp1re(_sol._rows.at(e));
-            sdpmDouble deltaCp1Im = _sol._cp1Im[0][0][e->getId() - 1] - cp1im(_sol._rows.at(e));
-            if (abs(deltaCp1Re) > 1e-12 || abs(deltaCp1Im) > 1e-12)
-                throw std::runtime_error("sdpm::Gradient::test() failed!\n");
-        }
+        if (wrt == "a")
+            dq -= _x.transpose() * _clz1Im[jcl][ifq];
+        else if (wrt == "h")
+            dq -= sdpmVectorXd::Constant(_nN, 1.0).transpose() * _clz1Im[jcl][ifq];
+        else if (wrt == "dz")
+            dq -= _clz1Im[jcl][ifq];
     }
+    return std::vector<sdpmDouble>(dq.data(), dq.data() + dq.size());
 }
 
 void Gradient::write(std::ostream &out) const
diff --git a/sdpm/src/sdpmGradient.h b/sdpm/src/sdpmGradient.h
index 4da6672..c76ea6b 100644
--- a/sdpm/src/sdpmGradient.h
+++ b/sdpm/src/sdpmGradient.h
@@ -28,7 +28,6 @@ namespace sdpm
 /**
  * @brief Source-doublet panel gradient interface
  * @authors Adrien Crovato
- * @todo remove dummy test class once rigid motions (a and h) have been included in GAF matrix computation
  */
 class SDPM_API Gradient : public sdpmObject
 {
@@ -38,23 +37,24 @@ class SDPM_API Gradient : public sdpmObject
     size_t _nN;                   ///< number of nodes
     std::map<Node *, int> _rowsN; ///< node row identification map
 
-    // Displacements and loads
+    // Coordinates, displacements and loads
+    sdpmVectorXd _x;                                ///< x-coordinate of nodes w.r.t. reference center
     std::vector<sdpmVectorXd> _dz;                  ///< z-displacement at nodes
     std::vector<std::vector<sdpmVectorXd>> _clz1Re; ///< z-component of real part of pressure load at nodes
     std::vector<std::vector<sdpmVectorXd>> _clz1Im; ///< z-component of real imaginary of pressure load at nodes
 
     // Pressure derivatives (stiffness terms)
-    std::vector<sdpmVectorXd> _dCp1Re_a;  ///< gradient of real part of unsteady pressure w.r.t. angle of attack
-    std::vector<sdpmVectorXd> _dCp1Im_a;  ///< gradient of imaginary part of unsteady pressure w.r.t. angle of attack
+    std::vector<sdpmVectorXd> _dCp1Re_a;  ///< gradient of real part of unsteady pressure w.r.t. pitch
+    std::vector<sdpmVectorXd> _dCp1Im_a;  ///< gradient of imaginary part of unsteady pressure w.r.t. pitch
     std::vector<sdpmMatrixXd> _dCp1Re_rx; ///< gradient of real part of unsteady pressure w.r.t. modal rotations about x
     std::vector<sdpmMatrixXd> _dCp1Im_rx; ///< gradient of imaginary part of unsteady pressure w.r.t. modal rotations about x
     std::vector<sdpmMatrixXd> _dCp1Re_ry; ///< gradient of real part of unsteady pressure w.r.t. modal rotations about y
     std::vector<sdpmMatrixXd> _dCp1Im_ry; ///< gradient of imaginary part of unsteady pressure w.r.t. modal rotations about y
     // Pressure derivatives (damping terms)
-    std::vector<sdpmVectorXd> _dCp1Re_aDot;  ///< gradient of real part of unsteady pressure w.r.t. time derivative of angle of attack
-    std::vector<sdpmVectorXd> _dCp1Im_aDot;  ///< gradient of imaginary part of unsteady pressure w.r.t. time derivative of angle of attack
-    std::vector<sdpmVectorXd> _dCp1Re_hDot;  ///< gradient of real part of unsteady pressure w.r.t. time derivative of vertical displacement
-    std::vector<sdpmVectorXd> _dCp1Im_hDot;  ///< gradient of imaginary part of unsteady pressure w.r.t. time derivative of vertical displacement
+    std::vector<sdpmVectorXd> _dCp1Re_aDot;  ///< gradient of real part of unsteady pressure w.r.t. time derivative of pitch
+    std::vector<sdpmVectorXd> _dCp1Im_aDot;  ///< gradient of imaginary part of unsteady pressure w.r.t. time derivative of pitch
+    std::vector<sdpmVectorXd> _dCp1Re_hDot;  ///< gradient of real part of unsteady pressure w.r.t. time derivative of plunge
+    std::vector<sdpmVectorXd> _dCp1Im_hDot;  ///< gradient of imaginary part of unsteady pressure w.r.t. time derivative of plunge
     std::vector<sdpmMatrixXd> _dCp1Re_rxDot; ///< gradient of real part of unsteady pressure w.r.t. time derivative of modal rotations about x
     std::vector<sdpmMatrixXd> _dCp1Im_rxDot; ///< gradient of imaginary part of unsteady pressure w.r.t. time derivative of modal rotations about x
     std::vector<sdpmMatrixXd> _dCp1Re_ryDot; ///< gradient of real part of unsteady pressure w.r.t. time derivative of modal rotations about y
@@ -62,20 +62,24 @@ class SDPM_API Gradient : public sdpmObject
     std::vector<sdpmMatrixXd> _dCp1Re_dzDot; ///< gradient of real part of unsteady pressure w.r.t. time derivative of modal displacements along z
     std::vector<sdpmMatrixXd> _dCp1Im_dzDot; ///< gradient of imaginary part of unsteady pressure w.r.t. time derivative of modal displacements along z
     // Pressure derivatives (inertia terms)
-    std::vector<sdpmVectorXd> _dCp1Re_aDotDot;  ///< gradient of real part of unsteady pressure w.r.t. second time derivative of angle of attack
-    std::vector<sdpmVectorXd> _dCp1Im_aDotDot;  ///< gradient of imaginary part of unsteady pressure w.r.t. second time derivative of angle of attack
-    std::vector<sdpmVectorXd> _dCp1Re_hDotDot;  ///< gradient of real part of unsteady pressure w.r.t. second time derivative of vertical displacement
-    std::vector<sdpmVectorXd> _dCp1Im_hDotDot;  ///< gradient of imaginary part of unsteady pressure w.r.t. second time derivative of vertical displacement
+    std::vector<sdpmVectorXd> _dCp1Re_aDotDot;  ///< gradient of real part of unsteady pressure w.r.t. second time derivative of pitch
+    std::vector<sdpmVectorXd> _dCp1Im_aDotDot;  ///< gradient of imaginary part of unsteady pressure w.r.t. second time derivative of pitch
+    std::vector<sdpmVectorXd> _dCp1Re_hDotDot;  ///< gradient of real part of unsteady pressure w.r.t. second time derivative of plunge
+    std::vector<sdpmVectorXd> _dCp1Im_hDotDot;  ///< gradient of imaginary part of unsteady pressure w.r.t. second time derivative of plunge
     std::vector<sdpmMatrixXd> _dCp1Re_dzDotDot; ///< gradient of real part of unsteady pressure w.r.t. second time derivative of modal displacements along z
     std::vector<sdpmMatrixXd> _dCp1Im_dzDotDot; ///< gradient of imaginary part of unsteady pressure w.r.t. second time derivative of modal displacements along z
 
     // Loads derivatives
-    std::vector<sdpmMatrixXd> _dClz1Re_rx; ///< gradient of real part of unsteady loads z-component w.r.t. modal rotations about x
-    std::vector<sdpmMatrixXd> _dClz1Im_rx; ///< gradient of imaginary part of unsteady loads z-component w.r.t. modal rotations about x
-    std::vector<sdpmMatrixXd> _dClz1Re_ry; ///< gradient of real part of unsteady loads z-component w.r.t. modal rotations about y
-    std::vector<sdpmMatrixXd> _dClz1Im_ry; ///< gradient of imaginary part of unsteady loads z-component w.r.t. modal rotations about y
-    std::vector<sdpmMatrixXd> _dClz1Re_dz; ///< gradient of real part of unsteady loads z-component w.r.t. modal displacements along z
-    std::vector<sdpmMatrixXd> _dClz1Im_dz; ///< gradient of imaginary part of unsteady loads z-component w.r.t. modal displacements along z
+    std::vector<sdpmVectorXd> _dClz1Re_a;  ///< gradient of real part of z-component of unsteady loads w.r.t. pitch
+    std::vector<sdpmVectorXd> _dClz1Im_a;  ///< gradient of imaginary part of z-component of unsteady loads w.r.t. pitch
+    std::vector<sdpmVectorXd> _dClz1Re_h;  ///< gradient of real part of z-component of unsteady loads w.r.t. plunge
+    std::vector<sdpmVectorXd> _dClz1Im_h;  ///< gradient of imaginary part of z-component of unsteady loads w.r.t. plunge
+    std::vector<sdpmMatrixXd> _dClz1Re_rx; ///< gradient of real part of z-component of unsteady loads w.r.t. modal rotations about x
+    std::vector<sdpmMatrixXd> _dClz1Im_rx; ///< gradient of imaginary part of z-component of unsteady loads w.r.t. modal rotations about x
+    std::vector<sdpmMatrixXd> _dClz1Re_ry; ///< gradient of real part of z-component of unsteady loads w.r.t. modal rotations about y
+    std::vector<sdpmMatrixXd> _dClz1Im_ry; ///< gradient of imaginary part of z-component of unsteady loads w.r.t. modal rotations about y
+    std::vector<sdpmMatrixXd> _dClz1Re_dz; ///< gradient of real part of z-component of unsteady loads w.r.t. modal displacements along z
+    std::vector<sdpmMatrixXd> _dClz1Im_dz; ///< gradient of imaginary part of z-component of unsteady loads w.r.t. modal displacements along z
 
 public:
     Gradient(Solver &sol);
@@ -85,8 +89,6 @@ public:
     std::vector<sdpmDouble> computePartialsGafRe(size_t ifq, int imd, int irw, int jcl, std::string const &wrt);
     std::vector<sdpmDouble> computePartialsGafIm(size_t ifq, int imd, int irw, int jcl, std::string const &wrt);
 
-    void test(double a_amp, double h_amp);
-
 #ifndef SWIG
     virtual void write(std::ostream &out) const override;
 #endif
diff --git a/sdpm/src/sdpmMotion.cpp b/sdpm/src/sdpmMotion.cpp
index 72ced15..b4e5fb9 100644
--- a/sdpm/src/sdpmMotion.cpp
+++ b/sdpm/src/sdpmMotion.cpp
@@ -22,7 +22,6 @@ using namespace sdpm;
 
 Motion::Motion(size_t n) : sdpmObject(),
                            _aAmp(0.), _hAmp(0.),
-                           _xRef(0.), _zRef(0.),
                            _dzMod(n, 0.), _rxMod(n, 0.), _ryMod(n, 0.)
 {
 }
@@ -30,12 +29,10 @@ Motion::Motion(size_t n) : sdpmObject(),
 /**
  * @brief Set rigid motion parameters
  */
-void Motion::setRigid(double aAmp, double hAmp, double xRef, double zRef)
+void Motion::setRigid(double aAmp, double hAmp)
 {
     _aAmp = aAmp;
     _hAmp = hAmp;
-    _xRef = xRef;
-    _zRef = zRef;
 }
 
 /**
@@ -74,11 +71,11 @@ sdpmVector3d Motion::computeSteady(Element const &e, sdpmVector3d const &ui)
 /**
  * @brief Compute the harmonic part of the motion induced velocity on an element of the body
  */
-sdpmVector3d Motion::computeHarmonic(Element const &e)
+sdpmVector3d Motion::computeHarmonic(Element const &e, sdpmDouble xRef, sdpmDouble zRef)
 {
     // Rigid contribution
     sdpmVector3d cg = e.getCg();
-    sdpmVector3d rigd(-_aAmp * (cg(2) - _zRef), 0., _aAmp * (cg(0) - _xRef) + _hAmp);
+    sdpmVector3d rigd(-_aAmp * (cg(2) - zRef), 0., _aAmp * (cg(0) - xRef) + _hAmp);
     // Flexible contribution
     sdpmVector3d flex = sdpmVector3d::Zero();
     for (auto n : e.getNodes())
diff --git a/sdpm/src/sdpmMotion.h b/sdpm/src/sdpmMotion.h
index 4920054..92a132a 100644
--- a/sdpm/src/sdpmMotion.h
+++ b/sdpm/src/sdpmMotion.h
@@ -32,22 +32,21 @@ class SDPM_API Motion : public sdpmObject
 {
     sdpmDouble _aAmp;               ///< pitch amplitude
     sdpmDouble _hAmp;               ///< plunge amplitude
-    sdpmDouble _xRef;               ///< x-coordinate of center of pitch
-    sdpmDouble _zRef;               ///< z-coordinate of center of pitch
     std::vector<sdpmDouble> _dzMod; ///< modal displacements along z
     std::vector<sdpmDouble> _rxMod; ///< modal rotations about x
     std::vector<sdpmDouble> _ryMod; ///< modal rotations about y
 
 public:
     Motion(size_t n);
-    ~Motion() {};
+    ~Motion() {}
 
     sdpmDouble getPitchAmplitude() const { return _aAmp; }
+    sdpmDouble getPlungeAmplitude() const { return _hAmp; }
     std::vector<sdpmDouble> const &getModeZ() const { return _dzMod; }
-    void setRigid(double aAmp, double hAmp, double xRef, double zRef);
+    void setRigid(double aAmp, double hAmp);
     void setFlexible(double mAmp, std::vector<std::vector<double>> const &xMod, std::vector<Node *> const &nodes);
     sdpmVector3d computeSteady(Element const &e, sdpmVector3d const &ui);
-    sdpmVector3d computeHarmonic(Element const &e);
+    sdpmVector3d computeHarmonic(Element const &e, sdpmDouble xRef, sdpmDouble zRef);
 };
 
 } // namespace sdpm
diff --git a/sdpm/src/sdpmProblem.cpp b/sdpm/src/sdpmProblem.cpp
index df54c34..1e80e8b 100644
--- a/sdpm/src/sdpmProblem.cpp
+++ b/sdpm/src/sdpmProblem.cpp
@@ -114,7 +114,7 @@ void Problem::update()
     // Unsteady motion-induced velocities
     for (size_t imd = 0; imd < _nModes; ++imd)
         for (auto body : _bodies)
-            body->computeVelocity(imd, _dDir);
+            body->computeVelocity(imd, _dDir, _xRef(0), _xRef(2));
 }
 
 /**
diff --git a/sdpm/src/sdpmSolver.cpp b/sdpm/src/sdpmSolver.cpp
index ab12fb6..a37d8d9 100644
--- a/sdpm/src/sdpmSolver.cpp
+++ b/sdpm/src/sdpmSolver.cpp
@@ -616,7 +616,9 @@ void Solver::computeGAF(size_t ifq)
     // Get sizes
     size_t n = _pbl.getNodes().size();
     size_t nm = _pbl.getNumModes();
-    // Get z-component of modal displacements and nodal loads
+    // Get x-component of reference center
+    sdpmDouble xRef = _pbl.getRefCtr()(0);
+    // Get z-component of displacements and nodal loads
     sdpmMatrixXd mz1 = sdpmMatrixXd::Zero(nm, n);
     sdpmMatrixXd lz1Re = sdpmMatrixXd::Zero(n, nm);
     sdpmMatrixXd lz1Im = sdpmMatrixXd::Zero(n, nm);
@@ -624,12 +626,15 @@ void Solver::computeGAF(size_t ifq)
     {
         for (auto b : _pbl.getBodies())
         {
+            sdpmDouble aAmp = b->getPitchAmplitude(imd);
+            sdpmDouble hAmp = b->getPlungeAmplitude(imd);
             mz1.row(imd) = Eigen::Map<const sdpmVectorXd>(b->getModeZ(imd).data(), n).transpose();
             std::vector<Node *> nods = b->getNodes();
             std::vector<sdpmVector3d> l1Re = b->getUnsteadyLoadsReal(imd, ifq);
             std::vector<sdpmVector3d> l1Im = b->getUnsteadyLoadsImag(imd, ifq);
             for (size_t i = 0; i < nods.size(); ++i)
             {
+                mz1(imd, nods[i]->getId() - 1) += aAmp * (nods[i]->getCoords()(0) - xRef) + hAmp;
                 lz1Re(nods[i]->getId() - 1, imd) = -l1Re[i](2);
                 lz1Im(nods[i]->getId() - 1, imd) = -l1Im[i](2);
             }
diff --git a/sdpm/tests/lann.py b/sdpm/tests/lann.py
index 5144209..f2a0f52 100644
--- a/sdpm/tests/lann.py
+++ b/sdpm/tests/lann.py
@@ -52,7 +52,7 @@ def main():
     pbl.setUnsteady(1, [kred])
     wing = sdpm.Body(msh, 'wing', 'te', crd, wnc, 1, 1)
     wing.addMotion()
-    wing.setRigidMotion(0, amp, 0., xf, 0.)
+    wing.setRigidMotion(0, amp, 0.)
     pbl.addBody(wing)
     tms['pbl'].stop()
     # solver
diff --git a/sdpm/tests/lann_ad.py b/sdpm/tests/lann_ad.py
index 88227e4..6daf572 100644
--- a/sdpm/tests/lann_ad.py
+++ b/sdpm/tests/lann_ad.py
@@ -52,7 +52,7 @@ def main():
     pbl.setUnsteady(1, [kred])
     wing = sdpm.Body(msh, 'wing', 'te', crd, wnc, 1, 1)
     wing.addMotion()
-    wing.setRigidMotion(0, amp, 0., xf, 0.)
+    wing.setRigidMotion(0, amp, 0.)
     pbl.addBody(wing)
     tms['pbl'].stop()
     # solver
diff --git a/sdpm/tests/lann_grad.py b/sdpm/tests/lann_grad.py
index 85d5e33..0c886ed 100644
--- a/sdpm/tests/lann_grad.py
+++ b/sdpm/tests/lann_grad.py
@@ -21,7 +21,7 @@ import sdpm
 from sdpm.gmsh import MeshLoader
 from sdpm.utils import build_fpath
 from sdpm.testing import *
-import math
+import numpy as np
 
 def main():
     # geometry parameters
@@ -32,9 +32,9 @@ def main():
     xf = 0.224 # x-coordinate of rotation center
     # flow and time parameters
     minf = 0.621 # freestream Mach number
-    aoa = 0.59 * math.pi / 180
+    aoa = 0.59 * np.pi / 180
     kred = 0.133 # reduced frequency
-    a_amp = 0.5 * 0.25 * math.pi / 180 # semi pitching amplitude
+    a_amp = 0.5 * 0.25 * np.pi / 180 # semi pitching amplitude
     h_amp = 0.1 # plunging amplitude
 
     # start timer
@@ -52,7 +52,7 @@ def main():
     pbl.setUnsteady(1, [kred])
     wing = sdpm.Body(msh, 'wing', 'te', crd, wnc, 1, 1)
     wing.addMotion()
-    wing.setRigidMotion(0, a_amp, h_amp, xf, 0.)
+    wing.setRigidMotion(0, a_amp, h_amp)
     pbl.addBody(wing)
     tms['pbl'].stop()
     # solver
@@ -71,9 +71,16 @@ def main():
     tms['total'].stop()
     print(tms)
 
+    # recover GAF matrix from partial gradients (NB: since GAF is quadratic in DOF, result must be divided by 2)
+    q_re = float(sol.getGafMatrixRe(0)[0][0])
+    q_im = float(sol.getGafMatrixIm(0)[0][0])
+    dq_re = 0.5 * (float(grd.computePartialsGafRe(0, 0, 0, 0, 'a')[0]) * a_amp + float(grd.computePartialsGafRe(0, 0, 0, 0, 'h')[0]) * h_amp)
+    dq_im = 0.5 * (float(grd.computePartialsGafIm(0, 0, 0, 0, 'a')[0]) * a_amp + float(grd.computePartialsGafIm(0, 0, 0, 0, 'h')[0]) * h_amp)
+
     # test
-    grd.test(a_amp, h_amp) # TODO, replace by GAF matrix gradient check once implemented
     tests = CTests()
+    tests.add(CTest('Re(Q)', q_re - dq_re, 0.0, 1e-10, forceabs=True))
+    tests.add(CTest('Im(Q)', q_im - dq_im, 0.0, 1e-10, forceabs=True))
     tests.run()
 
     # eof
diff --git a/sdpm/tests/lann_xsonic.py b/sdpm/tests/lann_xsonic.py
index 26b9b34..c00894e 100644
--- a/sdpm/tests/lann_xsonic.py
+++ b/sdpm/tests/lann_xsonic.py
@@ -52,7 +52,7 @@ def main():
     pbl.setUnsteady(1, [kred])
     wing = sdpm.Body(msh, 'wing', 'te', crd, wnc, 1, 1)
     wing.addMotion()
-    wing.setRigidMotion(0, amp, 0., xf, 0.)
+    wing.setRigidMotion(0, amp, 0.)
     dcp = interpolate_pressure(build_fpath('models/lann_dcp_rans.csv', __file__, 1), wing.getNodes())
     wing.setTransonicPressureGrad(dcp[:,0])
     pbl.addBody(wing)
-- 
GitLab