1 // =============================================================================
2 // PROJECT CHRONO - http://projectchrono.org
3 //
4 // Copyright (c) 2014 projectchrono.org
5 // All rights reserved.
6 //
7 // Use of this source code is governed by a BSD-style license that can be found
8 // in the LICENSE file at the top level of the distribution and at
9 // http://projectchrono.org/license-chrono.txt.
10 //
11 // =============================================================================
12 // Authors: Alessandro Tasora, Radu Serban
13 // =============================================================================
14
15 #include "chrono/motion_functions/ChFunction_Fillet3.h"
16
17 namespace chrono {
18
19 // Register into the object factory, to enable run-time dynamic creation and persistence
CH_FACTORY_REGISTER(ChFunction_Fillet3)20 CH_FACTORY_REGISTER(ChFunction_Fillet3)
21
22 ChFunction_Fillet3::ChFunction_Fillet3(const ChFunction_Fillet3& other) {
23 end = other.end;
24 y1 = other.y1;
25 y2 = other.y2;
26 dy1 = other.dy1;
27 dy2 = other.dy2;
28 c1 = other.c1;
29 c2 = other.c2;
30 c3 = other.c3;
31 c4 = other.c4;
32 }
33
Get_y(double x) const34 double ChFunction_Fillet3::Get_y(double x) const {
35 double ret = 0;
36 if (x <= 0)
37 return y1;
38 if (x >= end)
39 return y2;
40 ret = c1 * pow(x, 3) + c2 * pow(x, 2) + c3 * x + c4;
41 return ret;
42 }
43
Get_y_dx(double x) const44 double ChFunction_Fillet3::Get_y_dx(double x) const {
45 double ret = 0;
46 if (x <= 0)
47 return 0;
48 if (x >= end)
49 return 0;
50 ret = 3 * c1 * pow(x, 2) + 2 * c2 * x + c3;
51 return ret;
52 }
53
Get_y_dxdx(double x) const54 double ChFunction_Fillet3::Get_y_dxdx(double x) const {
55 double ret = 0;
56 if (x <= 0)
57 return 0;
58 if (x >= end)
59 return 0;
60 ret = 6 * c1 * x + 2 * c2;
61 return ret;
62 }
63
SetupCoefficients()64 void ChFunction_Fillet3::SetupCoefficients() {
65 ChMatrixDynamic<> ma(4, 4);
66 ChMatrixDynamic<> mb(4, 1);
67 ChMatrixDynamic<> mx(4, 1);
68
69 mb(0, 0) = y1;
70 mb(1, 0) = y2;
71 mb(2, 0) = dy1;
72 mb(3, 0) = dy2;
73
74 ma(0, 3) = 1.0;
75
76 ma(1, 0) = pow(end, 3);
77 ma(1, 1) = pow(end, 2);
78 ma(1, 2) = end;
79 ma(1, 3) = 1.0;
80
81 ma(2, 2) = 1.0;
82
83 ma(3, 0) = 3 * pow(end, 2);
84 ma(3, 1) = 2 * end;
85 ma(3, 2) = 1.0;
86
87 mx = ma.colPivHouseholderQr().solve(mb);
88
89 c1 = mx(0, 0);
90 c2 = mx(1, 0);
91 c3 = mx(2, 0);
92 c4 = mx(3, 0);
93 }
94
ArchiveOUT(ChArchiveOut & marchive)95 void ChFunction_Fillet3::ArchiveOUT(ChArchiveOut& marchive) {
96 // version number
97 marchive.VersionWrite<ChFunction_Fillet3>();
98 // serialize parent class
99 ChFunction::ArchiveOUT(marchive);
100 // serialize all member data:
101 marchive << CHNVP(end);
102 marchive << CHNVP(y1);
103 marchive << CHNVP(y2);
104 marchive << CHNVP(dy1);
105 marchive << CHNVP(dy2);
106 }
107
ArchiveIN(ChArchiveIn & marchive)108 void ChFunction_Fillet3::ArchiveIN(ChArchiveIn& marchive) {
109 // version number
110 /*int version =*/ marchive.VersionRead<ChFunction_Fillet3>();
111 // deserialize parent class
112 ChFunction::ArchiveIN(marchive);
113 // stream in all member data:
114 marchive >> CHNVP(end);
115 marchive >> CHNVP(y1);
116 marchive >> CHNVP(y2);
117 marchive >> CHNVP(dy1);
118 marchive >> CHNVP(dy2);
119 SetupCoefficients();
120 }
121
122 } // end namespace chrono
123