Wavefield_TEST.cc
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2019 Rhys Mainwaring
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16 */
17 
18 #include <Eigen/Dense>
19 #include <gtest/gtest.h>
20 #include <iostream>
21 #include <string>
22 
23 #include "wave_gazebo_plugins/Wavefield.hh"
24 
25 using namespace asv;
26 
28 // Utilities
29 std::ostream& operator<<(std::ostream& os, const std::vector<double>& _vec)
30 {
31  for (auto&& v : _vec ) // NOLINT
32  os << v << ", ";
33  return os;
34 }
35 
36 std::ostream& operator<<(std::ostream& os,
37  const std::vector<Eigen::Vector2d>& _vec)
38 {
39  for (auto&& v : _vec ) // NOLINT
40  os << v.transpose() << ", ";
41  return os;
42 }
43 
45 // Define tests
46 
47 // One dimension, one wave component
48 TEST(Wavefield, WaveSolver1D)
49 {
50  struct WaveParams
51  {
52  double a;
53  double k;
54  double omega;
55  double phi;
56  };
57 
58  auto dispersion = [=](auto omega)
59  {
60  return omega * omega / 9.8;
61  };
62 
63  auto wave = [=](auto x, auto t, auto& wp)
64  {
65  const double theta = wp.k * x - wp.omega * t;
66  const double s = std::sin(theta);
67  const double px = x - wp.a * s;
68  return px;
69  };
70 
71  auto wave_f = [=](auto x, auto p, auto t, auto& wp)
72  {
73  const double theta = wp.k * x - wp.omega * t;
74  const double s = std::sin(theta);
75  const double f = p - x + wp.a * s;
76  return f;
77  };
78 
79  auto wave_df = [=](auto x, auto p, auto t, auto& wp)
80  {
81  const double theta = wp.k * x - wp.omega * t;
82  const double c = std::cos(theta);
83  const double df = wp.a * wp.k * c - 1;
84  return df;
85  };
86 
87  auto solver = [=](auto& func, auto& dfunc, auto x0, auto p,
88  auto t, auto& wp, auto tol, auto nmax)
89  {
90  int n = 0;
91  double err = 1;
92  double xn = x0;
93  while (std::abs(err) > tol && n < nmax)
94  {
95  const double f = func(x0, p, t, wp);
96  const double df = dfunc(x0, p, t, wp);
97  const double d = -f/df;
98  xn = x0 + d;
99  x0 = xn;
100  err = f;
101  n++;
102  }
103  return xn;
104  };
105 
106  WaveParams wp;
107  wp.a = 3.0;
108  wp.omega = 2*M_PI/4.0;
109  wp.k = dispersion(wp.omega);
110  wp.phi = 0.0;
111 
112  double t = 0;
113  double x0 = 2.0;
114 
115  double p = wave(x0, t, wp);
116  double f = wave_f(x0, p, t, wp);
117  double df = wave_df(x0, p, t, wp);
118 
119  const double tol = 1E-8;
120  const double nmax = 20;
121  double x = solver(wave_f, wave_df, p, p, t, wp, tol, nmax);
122 
123  const double eps = 1E-8;
124  EXPECT_NEAR(wp.a, 3.0, eps);
125  EXPECT_NEAR(wp.k, 0.251775622, eps);
126  EXPECT_NEAR(wp.omega, 1.570796327, eps);
127  EXPECT_NEAR(x0, 2.0, eps);
128  EXPECT_NEAR(p, 0.552382941, eps);
129  EXPECT_LE(f, eps);
130  EXPECT_NEAR(df, -0.338428477, eps);
131  EXPECT_NEAR(x, 2.0, eps);
132 
133  // std::cout << "a: " << wp.a << std::endl;
134  // std::cout << "k: " << wp.k << std::endl;
135  // std::cout << "omega: " << wp.omega << std::endl;
136  // std::cout << "x0: " << x0 << std::endl;
137  // std::cout << "p: " << p << std::endl;
138  // std::cout << "f: " << f << std::endl;
139  // std::cout << "df: " << df << std::endl;
140  // std::cout << "x: " << x << std::endl;
141 }
142 
143 // Two dimensions, one wave component
144 TEST(Wavefield, WaveSolver2D)
145 {
146  struct WaveParams
147  {
148  double a;
149  double k;
150  double omega;
151  double phi;
152  Eigen::Vector2d dir;
153  };
154 
155  auto dispersion = [=](auto omega)
156  {
157  return omega * omega / 9.8;
158  };
159 
160  auto wave = [=](auto x, auto t, auto& wp)
161  {
162  const double theta = wp.k * x.dot(wp.dir) - wp.omega * t;
163  const double s = std::sin(theta);
164  const double c = std::cos(theta);
165  const double dx = wp.dir.x();
166  const double dy = wp.dir.y();
167  const Eigen::Vector3d p(
168  x.x() - wp.a * dx * s,
169  x.y() - wp.a * dy * s,
170  wp.a * c);
171  return p;
172  };
173 
174  auto wave_f = [=](auto x, auto p, auto t, auto& wp)
175  {
176  const double theta = wp.k * x.dot(wp.dir) - wp.omega * t;
177  const double s = std::sin(theta);
178  const double dx = wp.dir.x();
179  const double dy = wp.dir.y();
180  const Eigen::Vector2d f(
181  p.x() - x.x() + wp.a * dx * s,
182  p.y() - x.y() + wp.a * dy * s);
183  return f;
184  };
185 
186  auto wave_df = [=](auto x, auto p, auto t, auto& wp)
187  {
188  const double theta = wp.k * x.dot(wp.dir) - wp.omega * t;
189  const double c = std::cos(theta);
190  const double dx = wp.dir.x();
191  const double dy = wp.dir.y();
192  const double akc = wp.a * wp.k * c;
193  const double df1x = akc * dx * dx - 1.0;
194  const double df1y = akc * dx * dy;
195  const double df2x = df1y;
196  const double df2y = akc * dy * dy - 1.0;
197  Eigen::Matrix2d J;
198  J(0, 0) = df1x;
199  J(0, 1) = df1y;
200  J(1, 0) = df2x;
201  J(1, 1) = df2y;
202  return J;
203  };
204 
205  auto solver = [=](auto& func, auto& dfunc, auto x0, auto p,
206  auto t, auto& wp, auto tol, auto nmax)
207  {
208  int n = 0;
209  double err = 1;
210  auto xn = x0;
211  while (std::abs(err) > tol && n < nmax)
212  {
213  const auto F = func(x0, p, t, wp);
214  const auto J = dfunc(x0, p, t, wp);
215  xn = x0 - J.inverse() * F;
216  x0 = xn;
217  err = F.norm();
218  n++;
219  }
220  return xn;
221  };
222 
223  WaveParams wp;
224  wp.a = 3.0;
225  wp.omega = 2*M_PI/4.0;
226  wp.k = dispersion(wp.omega);
227  wp.phi = 0.0;
228  wp.dir = Eigen::Vector2d(1, 0);
229 
230  double t = 0;
231  Eigen::Vector2d x0(2.0, 3.0);
232 
233  auto p3 = wave(x0, t, wp);
234  Eigen::Vector2d p(p3(0), p3(1));
235  auto F = wave_f(x0, p, t, wp);
236  auto J = wave_df(x0, p, t, wp);
237 
238  const double tol = 1E-8;
239  const double nmax = 20;
240  auto x = solver(wave_f, wave_df, p, p, t, wp, tol, nmax);
241 
242  const double eps = 1E-8;
243  EXPECT_NEAR(wp.a, 3.0, eps);
244  EXPECT_NEAR(wp.k, 0.251775622, eps);
245  EXPECT_NEAR(wp.omega, 1.570796327, eps);
246  EXPECT_DOUBLE_EQ(wp.dir.x(), 1.0);
247  EXPECT_DOUBLE_EQ(wp.dir.y(), 0.0);
248  EXPECT_DOUBLE_EQ(x0.x(), 2.0);
249  EXPECT_DOUBLE_EQ(x0.y(), 3.0);
250  EXPECT_DOUBLE_EQ(x.x(), 2.0);
251  EXPECT_DOUBLE_EQ(x.y(), 3.0);
252  EXPECT_LE(F.norm(), eps);
253 
254  // std::cout << "a: " << wp.a << std::endl;
255  // std::cout << "k: " << wp.k << std::endl;
256  // std::cout << "omega: " << wp.omega << std::endl;
257  // std::cout << "x0: " << x0.transpose() << std::endl;
258  // std::cout << "p3: " << p3.transpose() << std::endl;
259  // std::cout << "p: " << p.transpose() << std::endl;
260  // std::cout << "F: " << F.transpose() << std::endl;
261  // std::cout << "J: " << J << std::endl;
262  // std::cout << "x: " << x.transpose() << std::endl;
263 }
264 
265 // Two dimensions, many wave components
266 TEST(Wavefield, NWaveSolver2D)
267 {
268  struct WaveParams
269  {
270  std::vector<double> a;
271  std::vector<double> k;
272  std::vector<double> omega;
273  std::vector<double> phi;
274  std::vector<Eigen::Vector2d> dir;
275  };
276 
277  auto dispersion = [=](auto omega)
278  {
279  return omega * omega / 9.8;
280  };
281 
282  auto wave = [=](auto x, auto t, auto& wp)
283  {
284  Eigen::Vector3d p(x.x(), x.y(), 0.0);
285  const size_t n = wp.a.size();
286  for (auto&& i = 0; i < n; ++i) // NOLINT
287  {
288  const double theta = wp.k[i] * x.dot(wp.dir[i]) - wp.omega[i] * t;
289  const double s = std::sin(theta);
290  const double c = std::cos(theta);
291  const double dx = wp.dir[i].x();
292  const double dy = wp.dir[i].y();
293  const double a = wp.a[i];
294  p += Eigen::Vector3d(
295  - a * dx * s,
296  - a * dy * s,
297  a * c);
298  }
299  return p;
300  };
301 
302  auto wave_f = [=](auto x, auto p, auto t, auto& wp)
303  {
304  Eigen::Vector2d f(p.x() - x.x(), p.y() - x.y());
305  const size_t n = wp.a.size();
306  for (auto&& i = 0; i < n; ++i) // NOLINT
307  {
308  const double theta = wp.k[i] * x.dot(wp.dir[i]) - wp.omega[i] * t;
309  const double s = std::sin(theta);
310  const double dx = wp.dir[i].x();
311  const double dy = wp.dir[i].y();
312  const double a = wp.a[i];
313  f += Eigen::Vector2d(
314  a * dx * s,
315  a * dy * s);
316  }
317  return f;
318  };
319 
320  auto wave_df = [=](auto x, auto p, auto t, auto& wp)
321  {
322  Eigen::Matrix2d J;
323  J(0, 0) = -1;
324  J(0, 1) = 0;
325  J(1, 0) = 0;
326  J(1, 1) = -1;
327  const size_t n = wp.a.size();
328  for (auto&& i = 0; i < n; ++i) // NOLINT
329  {
330  const double theta = wp.k[i] * x.dot(wp.dir[i]) - wp.omega[i] * t;
331  const double c = std::cos(theta);
332  const double dx = wp.dir[i].x();
333  const double dy = wp.dir[i].y();
334  const double akc = wp.a[i] * wp.k[i] * c;
335  const double df1x = akc * dx * dx;
336  const double df1y = akc * dx * dy;
337  const double df2x = df1y;
338  const double df2y = akc * dy * dy;
339  J(0, 0) += df1x;
340  J(0, 1) += df1y;
341  J(1, 0) += df2x;
342  J(1, 1) += df2y;
343  }
344  return J;
345  };
346 
347  auto solver = [=](auto& func, auto& dfunc, auto x0, auto p,
348  auto t, auto& wp, auto tol, auto nmax)
349  {
350  int n = 0;
351  double err = 1;
352  auto xn = x0;
353  while (std::abs(err) > tol && n < nmax)
354  {
355  const auto F = func(x0, p, t, wp);
356  const auto J = dfunc(x0, p, t, wp);
357  xn = x0 - J.inverse() * F;
358  x0 = xn;
359  err = F.norm();
360  n++;
361  }
362  return xn;
363  };
364 
365  WaveParams wp;
366  // wp.a = { 3.0, 0.0, 0.0 };
367  // wp.omega = { 2*M_PI/4.0, 2*M_PI, 2*M_PI };
368  wp.a = { 1.0, 2.0, 3.0 };
369  wp.omega = { 2*M_PI/50.0, 2*M_PI/10.0, 2*M_PI/20.0 };
370  wp.k = { dispersion(wp.omega[0]), dispersion(wp.omega[1]),
371  dispersion(wp.omega[2]) };
372  wp.phi = { 0.0, 0.0, 0.0 };
373  wp.dir = { Eigen::Vector2d(1, 0), Eigen::Vector2d(1, 0),
374  Eigen::Vector2d(1, 0) };
375 
376  double t = 0;
377  Eigen::Vector2d x0(2.0, 3.0);
378 
379  auto p3 = wave(x0, t, wp);
380  Eigen::Vector2d p(p3(0), p3(1));
381  auto F = wave_f(x0, p, t, wp);
382  auto J = wave_df(x0, p, t, wp);
383 
384  const double tol = 1E-8;
385  const double nmax = 20;
386  auto x = solver(wave_f, wave_df, p, p, t, wp, tol, nmax);
387 
388  const double eps = 1E-8;
389  EXPECT_DOUBLE_EQ(x0.x(), 2.0);
390  EXPECT_DOUBLE_EQ(x0.y(), 3.0);
391  EXPECT_DOUBLE_EQ(x.x(), 2.0);
392  EXPECT_DOUBLE_EQ(x.y(), 3.0);
393  EXPECT_LE(F.norm(), eps);
394 
395  // std::cout << "a: " << wp.a << std::endl;
396  // std::cout << "k: " << wp.k << std::endl;
397  // std::cout << "omega: " << wp.omega << std::endl;
398  // std::cout << "dir: " << wp.dir << std::endl;
399  // std::cout << "x0: " << x0.transpose() << std::endl;
400  // std::cout << "p3: " << p3.transpose() << std::endl;
401  // std::cout << "p: " << p.transpose() << std::endl;
402  // std::cout << "F: " << F.transpose() << std::endl;
403  // std::cout << "J: " << J << std::endl;
404  // std::cout << "x: " << x.transpose() << std::endl;
405 }
406 
407 // Two dimensions, many wave components, combined function
408 // and jacobian calculation
409 TEST(Wavefield, NWaveFdFSolver2D)
410 {
411  struct WaveParams
412  {
413  std::vector<double> a;
414  std::vector<double> k;
415  std::vector<double> omega;
416  std::vector<double> phi;
417  std::vector<Eigen::Vector2d> dir;
418  };
419 
420  auto dispersion = [=](auto omega)
421  {
422  return omega * omega / 9.8;
423  };
424 
425  auto wave = [=](auto x, auto t, auto& wp)
426  {
427  Eigen::Vector3d p(x.x(), x.y(), 0.0);
428  const size_t n = wp.a.size();
429  for (auto&& i = 0; i < n; ++i) // NOLINT
430  {
431  const double theta = wp.k[i] * x.dot(wp.dir[i]) - wp.omega[i] * t;
432  const double s = std::sin(theta);
433  const double c = std::cos(theta);
434  const double dx = wp.dir[i].x();
435  const double dy = wp.dir[i].y();
436  const double a = wp.a[i];
437  p += Eigen::Vector3d(
438  - a * dx * s,
439  - a * dy * s,
440  a * c);
441  }
442  return p;
443  };
444 
445  auto wave_fdf = [=](auto x, auto p, auto t, auto& wp, auto& F, auto& J)
446  {
447  F(0) = p.x() - x.x();
448  F(1) = p.y() - x.y();
449  J(0, 0) = -1;
450  J(0, 1) = 0;
451  J(1, 0) = 0;
452  J(1, 1) = -1;
453  const size_t n = wp.a.size();
454  for (auto&& i = 0; i < n; ++i) // NOLINT
455  {
456  const double theta = wp.k[i] * x.dot(wp.dir[i]) - wp.omega[i] * t;
457  const double s = std::sin(theta);
458  const double c = std::cos(theta);
459  const double dx = wp.dir[i].x();
460  const double dy = wp.dir[i].y();
461  const double a = wp.a[i];
462  const double akc = a * wp.k[i] * c;
463  const double df1x = akc * dx * dx;
464  const double df1y = akc * dx * dy;
465  const double df2x = df1y;
466  const double df2y = akc * dy * dy;
467  F(0) += a * dx * s;
468  F(1) += a * dy * s;
469  J(0, 0) += df1x;
470  J(0, 1) += df1y;
471  J(1, 0) += df2x;
472  J(1, 1) += df2y;
473  }
474  };
475 
476  auto solver = [=](auto& fdfunc, auto x0, auto p, auto t, auto& wp,
477  auto tol, auto nmax)
478  {
479  int n = 0;
480  double err = 1;
481  auto xn = x0;
482  Eigen::Vector2d F;
483  Eigen::Matrix2d J;
484  while (std::abs(err) > tol && n < nmax)
485  {
486  fdfunc(x0, p, t, wp, F, J);
487  xn = x0 - J.inverse() * F;
488  x0 = xn;
489  err = F.norm();
490  n++;
491  }
492  return xn;
493  };
494 
495  WaveParams wp;
496  wp.a = { 1.0, 2.0, 3.0 };
497  wp.omega = { 2*M_PI/50.0, 2*M_PI/10.0, 2*M_PI/20.0 };
498  wp.k = { dispersion(wp.omega[0]), dispersion(wp.omega[1]),
499  dispersion(wp.omega[2]) };
500  wp.phi = { 0.0, 0.0, 0.0 };
501  wp.dir = { Eigen::Vector2d(1, 0), Eigen::Vector2d(1, 0),
502  Eigen::Vector2d(1, 0) };
503 
504  double t = 0;
505  Eigen::Vector2d x0(2.0, 3.0);
506 
507  auto p3 = wave(x0, t, wp);
508  Eigen::Vector2d p(p3(0), p3(1));
509  Eigen::Vector2d F;
510  Eigen::Matrix2d J;
511  wave_fdf(x0, p, t, wp, F, J);
512 
513  const double tol = 1E-8;
514  const double nmax = 20;
515  auto x = solver(wave_fdf, p, p, t, wp, tol, nmax);
516 
517  const double eps = 1E-8;
518  EXPECT_DOUBLE_EQ(x0.x(), 2.0);
519  EXPECT_DOUBLE_EQ(x0.y(), 3.0);
520  EXPECT_DOUBLE_EQ(x.x(), 2.0);
521  EXPECT_DOUBLE_EQ(x.y(), 3.0);
522  EXPECT_LE(F.norm(), eps);
523 }
524 
526 // Run tests
527 
528 int main(int argc, char **argv)
529 {
530  testing::InitGoogleTest(&argc, argv);
531  return RUN_ALL_TESTS();
532 }
533 
d
d
f
f
asv::operator<<
std::ostream & operator<<(std::ostream &os, const std::vector< double > &_vec)
Definition: Wavefield.cc:43
s
XmlRpcServer s
TEST
TEST(Wavefield, WaveSolver1D)
Definition: Wavefield_TEST.cc:48
main
int main(int argc, char **argv)
Definition: Wavefield_TEST.cc:528
x
TFSIMD_FORCE_INLINE const tfScalar & x() const
asv
Definition: Geometry.hh:28
Wavefield.hh
This file contains definitions for classes used to manage a wave field. This includes wave parameters...

wave_gazebo_plugins
Author(s): Rhys Mainwaring
autogenerated on Thu May 7 2020 03:54:44