Marine systems simulation
PotentialData.h
1#ifndef _CPOTENTIAL_DATA_H_
2#define _CPOTENTIAL_DATA_H_
3
4#include <iostream>
5#include <iomanip>
6#include <fstream>
7#include <algorithm>
8#include <string>
9#include <vector>
10#include <complex>
11#include <map>
12
13#include <Eigen/Eigen>
14#include <Eigen/Eigenvalues>
15#include <Eigen/Core>
16
17
18#include "../HullData/HullData.h"
19
20#include "./Wave/FroudeKrylov.h"
21#include "./Wave/WaveDrift.h"
22
23#include "Identification/FreqDomainIdent.h"
24
25
26namespace Ship{
27 class PotentialData
28 {
29 public:
30 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
31
32 struct TimeDomainRadiation
33 {
34 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
35
36 TimeDomainRadiation()
37 {
38 Speed = -10;
39 Ainf = Eigen::Matrix< double, 6, 6 >::Zero( );
40
41 for( int i=0; i <6; i++)
42 for( int j=0; j <6; j++)
43 {
44 A[i][j] = Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic >::Zero( 1, 1 );;
45 B[i][j] = Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic >::Zero( 1, 1 );;
46 C[i][j] = Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic >::Zero( 1, 1 );;
47 D[i][j] = Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic >::Zero( 1, 1 );;
48 }
49 }
50
51 // Ainf
52 Eigen::Matrix<double, 6, 6> Ainf;
53
54 double Speed;
55
56 // companion system matrices
57 Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> A[6][6];
58 Eigen::Matrix<double, Eigen::Dynamic, 1 > B[6][6];
59 Eigen::Matrix<double, 1 , Eigen::Dynamic> C[6][6];
60 Eigen::Matrix<double, Eigen::Dynamic, 1 > D[6][6];
61 double Direction;
62 };
63
64 virtual ~PotentialData()
65 {
66 delete Speed;
67 delete Heading;
68 delete Frequency;
69 }
70
71 virtual void Read( const std::string &fname, int flipPhaseForOSC =0) = 0;
72 virtual Eigen::Matrix<double,6,6> GetHToO() = 0;
73
74 virtual void SetOriginWorigin() = 0;
75 virtual void SetOriginGorigin() = 0;
76 virtual void SetOrigin( Eigen::Matrix<double,3,1> &r ) = 0;
77
78 Eigen::Matrix<double,6,6> GetMassRigidBody()
79 {
80 return GetHToO().transpose()*Mrbeig*GetHToO();
81 }
82
83 Eigen::Matrix<double,6,6> GetAinf(unsigned int speedno=0)
84 {
85 return RadiationApproximation[speedno].Ainf;
86 }
87
88 Eigen::Matrix<double,6,6> GetMassTotal(unsigned int speedno=0)
89 {
90 return RadiationApproximation[speedno].Ainf+GetHToO().transpose()*Mrbeig*GetHToO();
91 }
92
93 Eigen::Matrix<double,6,6> GetAw(unsigned int FreqNo, unsigned int SpeedNo=0)
94 {
95 return GetHToO().transpose()*A[SpeedNo][FreqNo]*GetHToO();
96 }
97
98 Eigen::Matrix<double,6,6> GetBw(unsigned int FreqNo, unsigned int SpeedNo=0)
99 {
100 return GetHToO().transpose()*B[SpeedNo][FreqNo]*GetHToO();
101 }
102
103 virtual Eigen::Matrix<double,6,6> GetCw(unsigned int FreqNo,unsigned int SpeedNo=0)
104 {
105 return GetHToO().transpose()*C[SpeedNo][FreqNo]*GetHToO();
106 }
107
108 Eigen::Matrix<double,1,Eigen::Dynamic> GetAij(unsigned int DOF1, unsigned int DOF2, unsigned int SpeedNo=0)
109 {
110 Eigen::Matrix<double,1,Eigen::Dynamic> A = Eigen::Matrix<double,1,Eigen::Dynamic>::Zero(1,NoFrequency);
111
112 for( unsigned int i=0; i < NoFrequency; i++)
113 {
114 Eigen::Matrix<double,6,6> Aw = GetAw(i,SpeedNo);
115 A(i) = Aw(DOF1,DOF2);
116 }
117 return A;
118 }
119
120 Eigen::Matrix<double,1,Eigen::Dynamic> GetBij(unsigned int DOF1, unsigned int DOF2, unsigned int SpeedNo=0)
121 {
122 Eigen::Matrix<double,1,Eigen::Dynamic> B = Eigen::Matrix<double,1,Eigen::Dynamic>::Zero(1,NoFrequency);
123
124 for( unsigned int i=0; i < NoFrequency; i++)
125 {
126 Eigen::Matrix<double,6,6> Bw = GetBw(i,SpeedNo);
127 B(i) = Bw(DOF1,DOF2);
128 }
129 return B;
130 }
131
132 Eigen::Matrix<double,1,Eigen::Dynamic> GetCij(unsigned int DOF1, unsigned int DOF2, unsigned int SpeedNo=0)
133 {
134 Eigen::Matrix<double,1,Eigen::Dynamic> C = Eigen::Matrix<double,1,Eigen::Dynamic>::Zero(1,NoFrequency);
135
136 for( unsigned int i=0; i < NoFrequency; i++)
137 {
138 Eigen::Matrix<double,6,6> Cw = GetCw(i,SpeedNo);
139 C(i) = Cw(DOF1,DOF2);
140 }
141 return C;
142 }
143
144 Eigen::Matrix<double,1,Eigen::Dynamic> GetSpeed()
145 {
146 Eigen::Matrix<double,1,Eigen::Dynamic> speed = Eigen::Matrix<double,1,Eigen::Dynamic>::Zero(1,NoSpeed);
147 for(unsigned int i=0; i < NoSpeed; i++ ) speed(i) = Speed[i];
148 return speed;
149 }
150
151 Eigen::Matrix<double,1,Eigen::Dynamic> GetHeading()
152 {
153 Eigen::Matrix<double,1,Eigen::Dynamic> head = Eigen::Matrix<double,1,Eigen::Dynamic>::Zero(1,NoHeading);
154 for(unsigned int i=0; i < NoHeading; i++ ) head(i) = Heading[i];
155 return head;
156 }
157
158 Eigen::Matrix<double,1,Eigen::Dynamic> GetFrequency(int SpeedNo=0)
159 {
160 Eigen::Matrix<double,1,Eigen::Dynamic> freq = Eigen::Matrix<double,1,Eigen::Dynamic>::Zero(1,NoFrequency);
161 for(unsigned int i=0; i < NoFrequency; i++ ) freq(i) = Frequency[SpeedNo][i];
162 return freq;
163 }
164
165 virtual Eigen::Matrix<double,3,1> GetRg()
166 {
167 return rg;
168 }
169
170 virtual Eigen::Matrix<double,3,1> GetRo()
171 {
172 return ro;
173 }
174
175 Eigen::Matrix<double,6,1> GetFirstOrderForce( const Eigen::Matrix<double,3,1> &Kxy, double ShipSpeed,
176 double WaveDirection, double WaveAmplitude, double WaveFrequency, double WaveNumber, double WavePhase,
177 double Time )
178 {
179 double Fptr[6];
180
181 double EncounterFrequency = WaveFrequency-WaveNumber*ShipSpeed;
182 double Direction = EncounterFrequency/std::abs(EncounterFrequency);
183 EncounterFrequency = std::abs(EncounterFrequency);
184
185 //std::cout << "T: " << Time << "\t";
186
187 for( int DOF=0; DOF < 6; DOF++)
188 {
189
190 std::complex<double> RAO = FroudeFrylov->GetRAOValue(DOF,WaveDirection,std::abs(ShipSpeed),WaveFrequency);
191
192
193 double Amplitude = std::abs(RAO);
194 double Phase = atan2( RAO.imag(), RAO.real() );
195
196 //std::cout << DOF << " " <<RAO.real() << " " << RAO.imag() << "i" << " , Lag:" << 1/WaveFrequency*(Kxy(0) - Kxy(1) - Phase - WavePhase) << "s ";
197
198 double ThisWaveHeight = WaveAmplitude*sin(EncounterFrequency *Time -Kxy(0) -Kxy(1) + Phase - WavePhase );
199 Fptr[DOF] = Direction*Amplitude*ThisWaveHeight;
200 }
201 //std::cout << std::endl;
202 Eigen::Map<Eigen::Matrix<double,6,1>,Eigen::Unaligned> tmp(Fptr);
203 return GetHToO().transpose()*tmp;
204 }
205
206 Eigen::Matrix<double,6,1> GetSecondOrderForce(Eigen::Matrix<double,3,1> ShipPosition, double ShipSpeed,
207 double WaveDirection, double WaveHeight, double WaveFrequency, double WaveNumber, double WavePhase,
208 double Time )
209 {
210 double Fptr[6];
211 for( int DOF=0; DOF < 6; DOF++)
212 {
213 if( WaveDrift )
214 {
215 std::complex<double> Amplitude = WaveDrift->GetRAOValue(DOF,ShipSpeed,WaveFrequency,WaveDirection);
216 Fptr[DOF] = std::pow(WaveHeight,2)*Amplitude.real();
217 }
218 }
219 Eigen::Map<Eigen::Matrix<double,6,1>,Eigen::Unaligned> tmp(Fptr);
220 return GetHToO().transpose()*tmp;
221 }
222
223 void GenerateZeroSpeedTimeDomainApproximation()
224 {
225 //std::cout << "Generating zero speed time domain state radiation model"<<std::endl;
226 if( !RadiationApproximation.empty() ) RadiationApproximation.clear();
227 if( ZeroSpeedIdentificationFromCache() ){
228 //std::cout << "using model from cache" << std::endl;
229 return;
230 }else{
231 //std::cout << "identifying new model" << std::endl;
232 }
233
234 FreqDomainIdent Identification;
235 Identification.SetPotentialData(this);
236 Identification.IdentificationMethod(FreqDomainIdent::VECTOR_FITTING);
237
238 TimeDomainRadiation zeroSpeedSystem;
239
240
241 for( int i=0; i < 6; i++)
242 {
243
244 for( int j=0; j <= i; j++)
245 {
246 if( (i == 1 || i == 3 || i == 5) && (j==0 || j==2 || j==4) ) continue;
247 if( (j == 1 || j == 3 || j == 5) && (i==0 || i==2 || i==4) ) continue;
248
249 std::cout << "Identifying (i/j) = (" << i << "/" << j << ") ";
250 fflush(0);
251 if( is2Ddata() && (i!=0 && j!=0)){
252 FreqDomainIdent::StateSpace SS = Identification.IdentifySS(i,j);
253 zeroSpeedSystem.Ainf(i,j) = Identification.GetAinf(i,j);
254 zeroSpeedSystem.A[i][j] = SS.A;
255 zeroSpeedSystem.B[i][j] = SS.B;
256 zeroSpeedSystem.C[i][j] = SS.C;
257 zeroSpeedSystem.D[i][j] = SS.D;
258
259 zeroSpeedSystem.Ainf(j,i) = zeroSpeedSystem.Ainf(i,j);
260 zeroSpeedSystem.A[j][i] = zeroSpeedSystem.A[i][j];
261 zeroSpeedSystem.B[j][i] = zeroSpeedSystem.B[i][j];
262 zeroSpeedSystem.C[j][i] = zeroSpeedSystem.C[i][j];
263 zeroSpeedSystem.D[j][i] = zeroSpeedSystem.D[i][j];
264 }else{
265 std::cout << " Zero data" << std::endl;
266 zeroSpeedSystem.Ainf(i,j) = GetAij(i,j,0)[0];
267 zeroSpeedSystem.A[i][j] = Eigen::Matrix<double,1,1>::Zero();
268 zeroSpeedSystem.B[i][j] = Eigen::Matrix<double,1,1>::Zero();
269 zeroSpeedSystem.C[i][j] = Eigen::Matrix<double,1,1>::Zero();
270 zeroSpeedSystem.D[i][j] = Eigen::Matrix<double,1,1>::Zero();
271
272 zeroSpeedSystem.Ainf(j,i) = GetAij(i,j,0)[0];
273 zeroSpeedSystem.A[j][i] = Eigen::Matrix<double,1,1>::Zero();
274 zeroSpeedSystem.B[j][i] = Eigen::Matrix<double,1,1>::Zero();
275 zeroSpeedSystem.C[j][i] = Eigen::Matrix<double,1,1>::Zero();
276 zeroSpeedSystem.D[j][i] = Eigen::Matrix<double,1,1>::Zero();
277 }
278 }
279 }
280 RadiationApproximation.insert( std::make_pair(0,zeroSpeedSystem));
281 ZeroSpeedIdentificaitonToCache();
282 }
283
284 void GenerateTimeDomainApproximation()
285 {
286 //std::cout << "Generating time domain state radiation model"<<std::endl;
287 if( IdentificationFromCache() ) return;
288
289 FreqDomainIdent Identification;
290 Identification.SetPotentialData(this);
291 Identification.IdentificationMethod(FreqDomainIdent::VECTOR_FITTING);
292
293 for (unsigned int speed=0; speed < NoSpeed; speed++ )
294 {
295 TimeDomainRadiation speedSystem;
296
297 for( int i=0; i < 6; i++)
298 {
299 for( int j=0; j <= i; j++)
300 {
301 if( (i == 1 || i == 3 || i == 5) && (j==0 || j==2 || j==4) ) continue;
302 if( (j == 1 || j == 3 || j == 5) && (i==0 || i==2 || i==4) ) continue;
303
304 std::cout << "Identifying (i/j) = ("<<speed<<")(" << i << "/" << j << ") ";
305 //CPrecisionTimer T; T.Start();
306 if( is2Ddata() && (i==0||j==0)){
307 FreqDomainIdent::StateSpace SS = Identification.IdentifySS(i,j,speed);
308 speedSystem.Ainf(i,j) = Identification.GetAinf(i,j,speed);
309 speedSystem.A[i][j] = SS.A;
310 speedSystem.B[i][j] = SS.B;
311 speedSystem.C[i][j] = SS.C;
312 speedSystem.D[i][j] = SS.D;
313
314 speedSystem.Ainf(j,i) = speedSystem.Ainf(i,j);
315 speedSystem.A[j][i] = speedSystem.A[i][j];
316 speedSystem.B[j][i] = speedSystem.B[i][j];
317 speedSystem.C[j][i] = speedSystem.C[i][j];
318 speedSystem.D[j][i] = speedSystem.D[i][j];
319 }else{
320 FreqDomainIdent::StateSpace SS = Identification.IdentifySS(i,j);
321 speedSystem.Ainf(i,j) = GetAij(i,j,speed)[0];
322 speedSystem.A[i][j] = Eigen::Matrix<double,1,1>::Zero();
323 speedSystem.B[i][j] = Eigen::Matrix<double,1,1>::Zero();
324 speedSystem.C[i][j] = Eigen::Matrix<double,1,1>::Zero();
325 speedSystem.D[i][j] = Eigen::Matrix<double,1,1>::Zero();
326
327 speedSystem.Ainf(j,i) = GetAij(i,j,speed)[0];
328 speedSystem.A[j][i] = Eigen::Matrix<double,1,1>::Zero();
329 speedSystem.B[j][i] = Eigen::Matrix<double,1,1>::Zero();
330 speedSystem.C[j][i] = Eigen::Matrix<double,1,1>::Zero();
331 speedSystem.D[j][i] = Eigen::Matrix<double,1,1>::Zero();
332 }
333
334 std::cout << "identified new data - " /*<< T.Stop() << " seconds"*/ << std::endl;
335 }
336 }
337 RadiationApproximation.insert( std::make_pair(speed,speedSystem));
338 }
339 IdentificaitonToCache();
340 }
341
342
343 void PrintShip()
344 {
345 Eigen::Matrix<double,6,6> M = GetMassRigidBody();
346
347 std::cout << "%Ship information form file: " << fname << std::endl;
348 std::cout << "%\tLpp:\t" << Lpp << std::endl;
349 std::cout << "%\tB:\t" << Bredth << std::endl;
350 std::cout << "%\tT:\t" << Draught << std::endl;
351 std::cout << "%\tr_cg\t" << LCG<<"\t,\t0\t,\t"<<VCG<< " (from AP and base line)" << std::endl;
352 std::cout << "%\tr_cb\t" << LCB<<"\t,\t0\t,\t"<<VCB<< " (from AP and base line)" << std::endl;
353
354 std::cout << "%\tPotential data set information: " << std::endl;
355 std::cout << "%\t\tNumber of speeds:\t" <<NoSpeed<< std::endl;
356 std::cout << "speed = [\t\t" ;
357 for(unsigned int i=0; i < NoSpeed; i++ ) std::cout << Speed[i] << "\t";
358 std::cout << "];" << std::endl;
359
360 std::cout << "%\t\tNumber of headings:\t" <<NoHeading<< std::endl;
361 std::cout << "head =[\t\t" ;
362 for(unsigned int i=0; i < NoHeading; i++ ) std::cout << Heading[i] << "\t";
363 std::cout << "];" << std::endl;
364
365 std::cout << "%\t\tNumber of frequencies:\t" <<NoFrequency<< std::endl;
366 std::cout << "freq =[\t\t" ;
367 for(unsigned int i=0; i < NoFrequency; i++ ) std::cout << Frequency[i] << "\t";
368 std::cout << "];" << std::endl;
369
370 for(unsigned int i=0; i < NoSpeed; i++ )
371 {
372 std::cout << "%Total mass matrix for Speed " << i << " V="<<Speed[i] << " m/s" << std::endl;
373
374 Eigen::Matrix<double,6,6> Ainf = GetAinf(i);//FIXME: Direction
375
376 Eigen::Matrix<double,6,6> Mtotal = M + Ainf;
377 std::cout << std::scientific;
378 for( int j=0; j < 6; j++)
379 {
380 if( j== 0) std::cout << "M{" <<i+1<<"}\t=[";
381 std::cout <<std::setprecision(4) <<"\t"<< Mtotal(j,0) << "\t," << Mtotal(j,1) << "\t," << Mtotal(j,2) << "\t," << Mtotal(j,3) << "\t," << Mtotal(j,4) << "\t,"<<Mtotal(j,5);
382 if( j== 5) std::cout <<"];";
383 std::cout <<";" <<std::endl;
384 }
385
386 }
387
388
389 for(unsigned int i=0; i < NoSpeed; i++)
390 {
391 std::cout << "%Fluid memory model for Speed " << i << " V="<<Speed[i] << " m/s" << std::endl;
392 TimeDomainRadiation mod = RadiationApproximation[i];
393
394 for( int j=0; j < 6; j++ )
395 {
396 for( int k=0; k < 6; k++ )
397 {
398 size_t num_states = mod.A[k][j].cols();
399
400 if( num_states == 1 )
401 {
402 std::cout << "A{"<<i+1<<"}{"<<j+1<<"}{"<<k+1<<"}" <<"=\t" << "[" << mod.A[j][k](0,0) << "];"<< std::endl;
403 std::cout << std::endl;
404 std::cout << "B{"<<i+1<<"}{"<<j+1<<"}{"<<k+1<<"}" <<" =\t" << "[" << mod.B[j][k](0,0) << "];"<< std::endl;
405 std::cout << std::endl;
406 std::cout << "C{"<<i+1<<"}{"<<j+1<<"}{"<<k+1<<"}" <<" =\t" << "[" << mod.C[j][k](0,0) << "];"<< std::endl;
407 std::cout << std::endl;
408 std::cout << "D{"<<i+1<<"}{"<<j+1<<"}{"<<k+1<<"}" <<" =\t" << "[" << mod.D[j][k](0,0) << "];"<< std::endl;
409 }
410 else if (num_states == 2 )
411 {
412 std::cout << "A{"<<i+1<<"}{"<<j+1<<"}{"<<k+1<<"}"<<" =\t" << "[" << mod.A[j][k](0,0) << "\t,"<<mod.A[j][k](0,1)<<"\t;" << mod.A[j][k](1,0) << "\t,"<<mod.A[j][k](1,1)<<"\t;"<< "];"<< std::endl;
413 std::cout << std::endl;
414 std::cout << "B{"<<i+1<<"}{"<<j+1<<"}{"<<k+1<<"}"<<" =\t" << "[" << mod.B[j][k](0,0) << "\t;" << mod.B[j][k](1,0) <<"];"<< std::endl;
415 std::cout << std::endl;
416 std::cout << "C{"<<i+1<<"}{"<<j+1<<"}{"<<k+1<<"}"<<" =\t" << "[" << mod.C[j][k](0,0) << "\t," << mod.C[j][k](0,1) <<"];"<< std::endl;
417 std::cout << std::endl;
418 std::cout << "D{"<<i+1<<"}{"<<j+1<<"}{"<<k+1<<"}"<<" =\t" << "[" << mod.D[j][k](0,0) <<"];"<< std::endl;
419 }
420 else
421 {
422
423 std::cout << "A{"<<i+1<<"}{"<<j+1<<"}{"<<k+1<<"}"<<" =\t" << "[";
424 for( size_t s=0; s < num_states; s++)
425 {
426 if(s!=0) std::cout <<" \t ";
427 for( size_t c=0; c < num_states; c++)
428 {
429 std::cout << mod.A[j][k](s,c);
430 if( c < (num_states-1) ) std::cout << "\t,";
431 }
432 if( s < (num_states-1) ) std::cout << std::endl;
433 }
434 std::cout << "];"<< std::endl;
435
436 std::cout << std::endl;
437 std::cout << "B{"<<i+1<<"}{"<<j+1<<"}{"<<k+1<<"}"<<" =\t" << "[";
438 for( size_t s=0; s < num_states; s++)
439 {
440 if( s != 0 )std::cout <<" \t ";
441 std::cout << mod.B[j][k](s,0);
442 if( s < (num_states-1) ) std::cout << ";" <<std::endl;
443 }
444 std::cout << "];"<< std::endl;
445 std::cout << std::endl;
446
447 std::cout << "C{"<<i+1<<"}{"<<j+1<<"}{"<<k+1<<"}"<<" =\t" << "[";
448 for( size_t s=0; s < num_states; s++)
449 {
450 std::cout << mod.C[j][k](0,s);
451 if( s < (num_states-1) ) std::cout << ",";
452 }
453 std::cout << "];"<< std::endl;
454
455 std::cout << std::endl;
456 std::cout << "D{"<<i+1<<"}{"<<j+1<<"}{"<<k+1<<"}"<<" =\t" << "[" << mod.D[j][k](0,0) << "];"<< std::endl;
457 }
458 }
459 }
460 }
461 }
462
463 size_t GetTimeDomainStateSize( int DOF1, int DOF2, int SpeedNo=0 )
464 {
465 return RadiationApproximation[SpeedNo].A[DOF1][DOF2].cols();
466 }
467
468 void ZeroSpeedIdentificaitonToCache()
469 {
470 std::string cachebase = fname;
471 std::stringstream ss; ss << cachebase << ".identcache0";
472
473 std::fstream cache(ss.str().c_str(),std::ios::out|std::ios::binary);
474
475 int a_size[2];
476 int b_size[2];
477 int c_size[2];
478 int d_size[2];
479
480 for( int i=0; i < 6; i++)
481 {
482 for( int j=0; j < 6; j++)
483 {
484// a_size[0] = static_cast<int>(RadiationApproximation[0].A[i][j].rows());
485// a_size[1] = static_cast<int>(RadiationApproximation[0].A[i][j].cols());
486
487 a_size[0] = static_cast<int>(RadiationApproximation[0].A[i][j].rows());
488 a_size[1] = static_cast<int>(RadiationApproximation[0].A[i][j].cols());
489
490 cache.write( reinterpret_cast<char*>(a_size), sizeof(a_size) );
491 cache.write( reinterpret_cast<char*>(RadiationApproximation[0].A[i][j].data()),sizeof(double)*a_size[0]*a_size[1] );
492
493// b_size[0] = static_cast<int>(RadiationApproximation[0].B[i][j].rows());
494// b_size[1] = static_cast<int>(RadiationApproximation[0].B[i][j].cols());
495
496 b_size[0] = static_cast<int>(RadiationApproximation[0].B[i][j].rows());
497 b_size[1] = static_cast<int>(RadiationApproximation[0].B[i][j].cols());
498
499 cache.write( reinterpret_cast<char*>(b_size), sizeof(b_size) );
500 cache.write( reinterpret_cast<char*>(RadiationApproximation[0].B[i][j].data()),sizeof(double)*b_size[0]*b_size[1] );
501
502
503 // c_size[0] = static_cast<int>(RadiationApproximation[0].C[i][j].rows());
504 // c_size[1] = static_cast<int>(RadiationApproximation[0].C[i][j].cols());
505
506 c_size[0] = static_cast<int>(RadiationApproximation[0].C[i][j].rows());
507 c_size[1] = static_cast<int>(RadiationApproximation[0].C[i][j].cols());
508
509 cache.write( reinterpret_cast<char*>(c_size), sizeof(c_size) );
510 cache.write( reinterpret_cast<char*>(RadiationApproximation[0].C[i][j].data()),sizeof(double)*c_size[0]*c_size[1] );
511
512// d_size[0] = static_cast<int>(RadiationApproximation[0].D[i][j].rows());
513// d_size[1] = static_cast<int>(RadiationApproximation[0].D[i][j].cols());
514//
515 d_size[0] = static_cast<int>(RadiationApproximation[0].D[i][j].rows());
516 d_size[1] = static_cast<int>(RadiationApproximation[0].D[i][j].cols());
517
518 cache.write( reinterpret_cast<char*>(d_size), sizeof(d_size) );
519 cache.write( reinterpret_cast<char*>(RadiationApproximation[0].D[i][j].data()),sizeof(double)*d_size[0]*d_size[1] );
520
521 //std::cout << "Caching speed: " << MySpeed << " ("<<i<<","<<j<<") - " << " wrote A[ "<<a_size[0]<<" x "<<a_size[1]<<" ]"<<std::endl;
522 }
523 }
524
525 cache.write( reinterpret_cast<char*>(RadiationApproximation[0].Ainf.data()),sizeof(double)*6*6 );
526 cache.close();
527 }
528
529 bool ZeroSpeedIdentificationFromCache()
530 {
531 std::string cachebase = fname;
532 std::stringstream ss; ss << cachebase <<".identcache0";
533 std::fstream cache(ss.str().c_str(),std::ios::in|std::ios::binary);
534 if( !cache.good() ) return false;
535
536 int a_size[2];
537 int b_size[2];
538 int c_size[2];
539 int d_size[2];
540
541 double *A_tmp;
542 double *B_tmp;
543 double *C_tmp;
544 double *D_tmp;
545 double Ainf_tmp[6*6];
546 TimeDomainRadiation radiationModel;
547 for( int i=0; i < 6; i++)
548 {
549 for( int j=0; j < 6; j++)
550 {
551 cache.read( reinterpret_cast<char*>(a_size), sizeof(a_size) );
552 A_tmp = new double[a_size[0]*a_size[1]];
553
554 cache.read( reinterpret_cast<char*>(A_tmp),sizeof(double)*a_size[0]*a_size[1] );
555
556
557 cache.read( reinterpret_cast<char*>(b_size), sizeof(b_size) );
558 B_tmp = new double[b_size[0]*b_size[1]];
559
560 cache.read( reinterpret_cast<char*>(B_tmp),sizeof(double)*b_size[0]*b_size[1] );
561
562 cache.read( reinterpret_cast<char*>(c_size), sizeof(c_size) );
563 C_tmp = new double[c_size[0]*c_size[1]];
564
565 cache.read( reinterpret_cast<char*>(C_tmp),sizeof(double)*c_size[0]*c_size[1] );
566
567
568 cache.read( reinterpret_cast<char*>(d_size), sizeof(d_size) );
569 D_tmp = new double[d_size[0]*d_size[1]];
570
571 cache.read( reinterpret_cast<char*>(D_tmp),sizeof(double)*d_size[0]*d_size[1] );
572
573 //std::cout << "Recovered speed: " << MySpeed << " ("<<i<<","<<j<<") - " << " wrote A["<<a_size[0]<<"x"<<a_size[1]<<"]"<<std::endl;
574
575 radiationModel.A[i][j] = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>::Zero(a_size[0],a_size[1]);
576 radiationModel.B[i][j] = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>::Zero(b_size[0],b_size[1]);
577 radiationModel.C[i][j] = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>::Zero(c_size[0],c_size[1]);
578 radiationModel.D[i][j] = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>::Zero(d_size[0],d_size[1]);
579
580 for( int k=0; k < a_size[0]; k++)
581 for(int l=0; l < a_size[1]; l++)
582 {
583 assert(a_size[0]*a_size[1] > (l+k*a_size[1]) );
584 radiationModel.A[i][j](k,l) = A_tmp[k+l*a_size[0]];
585 }
586 for( int k=0; k < b_size[0]; k++)
587 for(int l=0; l < b_size[1]; l++)
588 {
589 assert(b_size[0]*b_size[1] > (l+k*b_size[1]) );
590 radiationModel.B[i][j](k,l) = B_tmp[k+l*b_size[0]];
591 }
592
593 for( int k=0; k < c_size[0]; k++)
594 for(int l=0; l < c_size[1]; l++)
595 {
596 assert(c_size[0]*c_size[1] > (l+k*c_size[1]) );
597 radiationModel.C[i][j](k,l) = C_tmp[k+l*c_size[0]];
598 }
599 for( int k=0; k < d_size[0]; k++)
600 for(int l=0; l < d_size[1]; l++)
601 {
602 assert(d_size[0]*d_size[1] > (l+k*d_size[1]) );
603 radiationModel.D[i][j](k,l) = D_tmp[k+l*d_size[0]];
604 }
605 delete []A_tmp;
606 delete []B_tmp;
607 delete []C_tmp;
608 delete []D_tmp;
609 }
610 }
611
612 cache.read( reinterpret_cast<char*>(Ainf_tmp),sizeof(double)*6*6);
613
614 for(int i=0; i < 6; i++)
615 for(int j=0; j < 6; j++)
616 radiationModel.Ainf(i,j)=Ainf_tmp[j+i*6];
617
618 radiationModel.Speed = 0;
619 RadiationApproximation.insert( std::make_pair(0,radiationModel) );
620 cache.close();
621 return true;
622 }
623
624 void IdentificaitonToCache()
625 {
626 std::string cachebase = fname;
627 std::stringstream ss; ss << cachebase << ".identcache1";
628
629 std::fstream cache(ss.str().c_str(),std::ios::out|std::ios::binary);
630
631 int a_size[2];
632 int b_size[2];
633 int c_size[2];
634 int d_size[2];
635
636 cache.write( reinterpret_cast<char*>(&NoSpeed),sizeof(unsigned int) );
637
638 for(unsigned int SpeedIdx=0; SpeedIdx < NoSpeed; SpeedIdx++)
639 {
640 double MySpeed = Speed[SpeedIdx];
641
642 cache.write( reinterpret_cast<char*>(&MySpeed),sizeof(double) );
643 for( int i=0; i < 6; i++)
644 {
645 for( int j=0; j < 6; j++)
646 {
647// a_size[0] = static_cast<int>(RadiationApproximation[SpeedIdx].A[i][j].rows());
648// a_size[1] = static_cast<int>(RadiationApproximation[SpeedIdx].A[i][j].cols());
649
650 a_size[0] = static_cast<int>(RadiationApproximation[SpeedIdx].A[i][j].rows());
651 a_size[1] = static_cast<int>(RadiationApproximation[SpeedIdx].A[i][j].cols());
652
653 cache.write( reinterpret_cast<char*>(a_size), sizeof(a_size) );
654 cache.write( reinterpret_cast<char*>(RadiationApproximation[SpeedIdx].A[i][j].data()),sizeof(double)*a_size[0]*a_size[1] );
655
656// b_size[0] = static_cast<int>(RadiationApproximation[SpeedIdx].B[i][j].rows());
657// b_size[1] = static_cast<int>(RadiationApproximation[SpeedIdx].B[i][j].cols());
658
659 b_size[0] = static_cast<int>(RadiationApproximation[SpeedIdx].B[i][j].rows());
660 b_size[1] = static_cast<int>(RadiationApproximation[SpeedIdx].B[i][j].cols());
661
662 cache.write( reinterpret_cast<char*>(b_size), sizeof(b_size) );
663 cache.write( reinterpret_cast<char*>(RadiationApproximation[SpeedIdx].B[i][j].data()),sizeof(double)*b_size[0]*b_size[1] );
664
665// c_size[0] = static_cast<int>(RadiationApproximation[SpeedIdx].C[i][j].rows());
666// c_size[1] = static_cast<int>(RadiationApproximation[SpeedIdx].C[i][j].cols());
667
668 c_size[0] = static_cast<int>(RadiationApproximation[SpeedIdx].C[i][j].rows());
669 c_size[1] = static_cast<int>(RadiationApproximation[SpeedIdx].C[i][j].cols());
670
671 cache.write( reinterpret_cast<char*>(c_size), sizeof(c_size) );
672 cache.write( reinterpret_cast<char*>(RadiationApproximation[SpeedIdx].C[i][j].data()),sizeof(double)*c_size[0]*c_size[1] );
673
674// d_size[0] = static_cast<int>(RadiationApproximation[SpeedIdx].D[i][j].rows());
675// d_size[1] = static_cast<int>(RadiationApproximation[SpeedIdx].D[i][j].cols());
676
677 d_size[0] = static_cast<int>(RadiationApproximation[SpeedIdx].D[i][j].rows());
678 d_size[1] = static_cast<int>(RadiationApproximation[SpeedIdx].D[i][j].cols());
679
680 cache.write( reinterpret_cast<char*>(d_size), sizeof(d_size) );
681 cache.write( reinterpret_cast<char*>(RadiationApproximation[SpeedIdx].D[i][j].data()),sizeof(double)*d_size[0]*d_size[1] );
682
683 //std::cout << "Caching speed: " << MySpeed << " ("<<i<<","<<j<<") - " << " wrote A[ "<<a_size[0]<<" x "<<a_size[1]<<" ]"<<std::endl;
684 }
685 }
686 cache.write( reinterpret_cast<char*>(RadiationApproximation[SpeedIdx].Ainf.data()),sizeof(double)*6*6 );
687 }
688 cache.close();
689 }
690
691 bool IdentificationFromCache()
692 {
693 std::string cachebase = fname;
694 std::stringstream ss; ss << cachebase <<".identcache1";
695 std::fstream cache(ss.str().c_str(),std::ios::in|std::ios::binary);
696 if( !cache.good() ) return false;
697
698 int a_size[2];
699 int b_size[2];
700 int c_size[2];
701 int d_size[2];
702
703 double *A_tmp;
704 double *B_tmp;
705 double *C_tmp;
706 double *D_tmp;
707 double Ainf_tmp[6*6];
708
709 cache.read( reinterpret_cast<char*>(&NoSpeed),sizeof(unsigned int) );
710
711 for(unsigned int SpeedIdx=0; SpeedIdx < NoSpeed; SpeedIdx++)
712 {
713 double MySpeed;
714
715 cache.read( reinterpret_cast<char*>(&MySpeed),sizeof(double) );
716
717 TimeDomainRadiation radiationModel;
718 for( int i=0; i < 6; i++)
719 {
720 for( int j=0; j < 6; j++)
721 {
722 cache.read( reinterpret_cast<char*>(a_size), sizeof(a_size) );
723 A_tmp = new double[a_size[0]*a_size[1]];
724
725 cache.read( reinterpret_cast<char*>(A_tmp),sizeof(double)*a_size[0]*a_size[1] );
726
727 cache.read( reinterpret_cast<char*>(b_size), sizeof(b_size) );
728 B_tmp = new double[b_size[0]*b_size[1]];
729
730 cache.read( reinterpret_cast<char*>(B_tmp),sizeof(double)*b_size[0]*b_size[1] );
731
732 cache.read( reinterpret_cast<char*>(c_size), sizeof(c_size) );
733 C_tmp = new double[c_size[0]*c_size[1]];
734
735 cache.read( reinterpret_cast<char*>(C_tmp),sizeof(double)*c_size[0]*c_size[1] );
736
737 cache.read( reinterpret_cast<char*>(d_size), sizeof(d_size) );
738 D_tmp = new double[d_size[0]*d_size[1]];
739
740 cache.read( reinterpret_cast<char*>(D_tmp),sizeof(double)*d_size[0]*d_size[1] );
741 //std::cout << "Recovered speed: " << MySpeed << " ("<<i<<","<<j<<") - " << " wrote A["<<a_size[0]<<"x"<<a_size[1]<<"]"<<std::endl;
742
743 radiationModel.A[i][j] = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>::Zero(a_size[0],a_size[1]);
744 radiationModel.B[i][j] = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>::Zero(b_size[0],b_size[1]);
745 radiationModel.C[i][j] = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>::Zero(c_size[0],c_size[1]);
746 radiationModel.D[i][j] = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>::Zero(d_size[0],d_size[1]);
747
748 for( int k=0; k < a_size[0]; k++)
749 for(int l=0; l < a_size[1]; l++)
750 {
751 assert(a_size[0]*a_size[1] > (l+k*a_size[1]) );
752 radiationModel.A[i][j](k,l) = A_tmp[k+l*a_size[0]];
753 }
754 for( int k=0; k < b_size[0]; k++)
755 for(int l=0; l < b_size[1]; l++)
756 {
757 assert(b_size[0]*b_size[1] > (l+k*b_size[1]) );
758 radiationModel.B[i][j](k,l) = B_tmp[k+l*b_size[0]];
759 }
760
761 for( int k=0; k < c_size[0]; k++)
762 for(int l=0; l < c_size[1]; l++)
763 {
764 assert(c_size[0]*c_size[1] > (l+k*c_size[1]) );
765 radiationModel.C[i][j](k,l) = C_tmp[k+l*c_size[0]];
766 }
767 for( int k=0; k < d_size[0]; k++)
768 for(int l=0; l < d_size[1]; l++)
769 {
770 assert(d_size[0]*d_size[1] > (l+k*d_size[1]) );
771 radiationModel.D[i][j](k,l) = D_tmp[k+l*d_size[0]];
772 }
773 delete []A_tmp;
774 delete []B_tmp;
775 delete []C_tmp;
776 delete []D_tmp;
777 }
778 }
779
780 cache.read( reinterpret_cast<char*>(Ainf_tmp),sizeof(double)*6*6);
781
782 for(int i=0; i < 6; i++)
783 for(int j=0; j < 6; j++)
784 radiationModel.Ainf(i,j)=Ainf_tmp[j+i*6];
785
786 radiationModel.Speed = MySpeed;
787 RadiationApproximation.insert( std::make_pair(SpeedIdx, radiationModel ));
788 }
789 cache.close();
790 return true;
791 }
792
793
794 void RadiationDerivative(int DOF1, int DOF2, double *derivative, const double *const radiation_state, const double *ship_state, unsigned int SpeedNo=0 )
795 {
796 const TimeDomainRadiation& radiationModel = RadiationApproximation[SpeedNo];
797
798 const size_t state_size = radiationModel.A[DOF1][DOF2].cols();
799 Eigen::Map<const Eigen::MatrixXd,Eigen::Unaligned> X(radiation_state,state_size,1);
800 Eigen::Map<Eigen::MatrixXd,Eigen::Unaligned> dotX(derivative,state_size,1);
801 dotX.noalias() = radiationModel.A[DOF1][DOF2]*X + radiationModel.B[DOF1][DOF2]*ship_state[DOF2];
802 }
803
804 double RadiationOutput(int DOF1, int DOF2, const double *const radiation_state, const double * const ship_state, unsigned int SpeedNo=0)
805 {
806 const TimeDomainRadiation& radiationModel = RadiationApproximation[SpeedNo];
807
808 const size_t state_size = radiationModel.A[DOF1][DOF2].cols();
809 Eigen::Map<const Eigen::MatrixXd,Eigen::Unaligned> X(radiation_state,state_size,1);
810 return (radiationModel.C[DOF1][DOF2]*X + radiationModel.D[DOF1][DOF2]*ship_state[DOF2]).value();
811 }
812
813
814 std::map<int,TimeDomainRadiation,std::less<int>,Eigen::aligned_allocator<std::pair<const int,TimeDomainRadiation> > > RadiationApproximation;
815
816 protected:
817
818 virtual bool is2Ddata(){
819 return false;
820 }
821
822 std::string fname;
823
824 double Lpp,Bredth,Draught;
825
826 double LCG,VCG;
827 double LCB,VCB;
828
829
830 unsigned int NoSpeed;
831 unsigned int NoHeading;
832 unsigned int NoDOF;
833 unsigned int NoFrequency;
834
835 double *Speed;
836 double *Heading;
837 double **Frequency;
838
839 FroudeKrylovForce *FroudeFrylov;
840 WaveDriftForce *WaveDrift;
841
842 Eigen::Matrix<double,6,6> Mrbeig;
843 Eigen::Matrix<double,6,6> **A;
844 Eigen::Matrix<double,6,6> **B;
845 Eigen::Matrix<double,6,6> **C;
846 Eigen::Matrix<double,6,6> **Aadd;
847 Eigen::Matrix<double,6,6> **Badd;
848 Eigen::Matrix<double,6,6> **Cadd;
849
850 Eigen::Matrix<double,3,1> rg;
851 Eigen::Matrix<double,3,1> ro;
852 std::map<std::string,double> param;
853 };
854}
855#endif
Ship::PotentialData
Definition: PotentialData.h:28
Ship
Simple waypoint object.
Definition: CableAttach.h:16
Ship::PotentialData::TimeDomainRadiation
Definition: PotentialData.h:33