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_Mocap.h"
16
17 namespace chrono {
18
19 // Register into the object factory, to enable run-time dynamic creation and persistence
CH_FACTORY_REGISTER(ChFunction_Mocap)20 CH_FACTORY_REGISTER(ChFunction_Mocap)
21
22 ChFunction_Mocap::ChFunction_Mocap() {
23 Set_samples(2); // this creates arrays
24 Set_samp_freq(50);
25 }
26
ChFunction_Mocap(int m_samples,double freq)27 ChFunction_Mocap::ChFunction_Mocap(int m_samples, double freq) {
28 Set_samples(m_samples); // this creates arrays
29 Set_samp_freq(freq);
30 }
31
ChFunction_Mocap(const ChFunction_Mocap & other)32 ChFunction_Mocap::ChFunction_Mocap(const ChFunction_Mocap& other) {
33 Set_samples(other.samples); // this creates arrays
34 Set_samp_freq(other.samp_freq);
35
36 array_y = other.array_y;
37 array_y_dt = other.array_y_dt;
38 array_y_dtdt = other.array_y_dtdt;
39 }
40
Estimate_x_range(double & xmin,double & xmax) const41 void ChFunction_Mocap::Estimate_x_range(double& xmin, double& xmax) const {
42 xmin = 0.0;
43 xmax = Get_timetot();
44 }
45
Set_samp_freq(double m_fr)46 void ChFunction_Mocap::Set_samp_freq(double m_fr) {
47 samp_freq = m_fr;
48 timetot = ((double)samples / samp_freq);
49 }
50
Set_samples(int m_samples)51 void ChFunction_Mocap::Set_samples(int m_samples) {
52 samples = m_samples;
53
54 if (samples < 2)
55 samples = 2;
56
57 timetot = ((double)samples / samp_freq);
58
59 array_y.resize(samples);
60 array_y_dt.resize(samples);
61 array_y_dtdt.resize(samples);
62 }
63
64 // Compute all the y_dt basing on y, using the trapezoidal rule for numerical differentiation
Compute_array_dt(const ChArray<> & array_A,ChArray<> & array_A_dt) const65 void ChFunction_Mocap::Compute_array_dt(const ChArray<>& array_A, ChArray<>& array_A_dt) const {
66 int i, ia, ib;
67 double y_dt;
68
69 for (i = 0; i < samples; i++) {
70 ia = i - 1; // boundaries cases
71 if (ia <= 0) {
72 ia = 0;
73 }
74 ib = i + 1;
75 if (ib >= samples) {
76 ib = i;
77 }
78 // trapezoidal differentiation
79 y_dt = ((array_A(ib)) - (array_A(ia))) / Get_timeslice();
80
81 array_A_dt(i) = y_dt;
82 }
83 }
84
85 // Interpolation of the in-between values, given the discrete sample array (uniformly spaced points)
LinInterp(const ChArray<> & array,double x,double x_max) const86 double ChFunction_Mocap::LinInterp(const ChArray<>& array, double x, double x_max) const {
87 double position;
88 double weightA, weightB;
89 int ia, ib;
90
91 position = ((double)samples * (x / x_max));
92
93 ia = (int)floor(position);
94 if (ia < 0) {
95 ia = 0;
96 };
97 if (ia >= samples) {
98 ia = samples - 1;
99 }
100
101 ib = ia + 1;
102 if (ib < 0) {
103 ib = 0;
104 };
105 if (ib >= samples) {
106 ib = samples - 1;
107 };
108
109 weightB = position - (int)position;
110 weightA = 1 - weightB;
111
112 return (weightA * array(ia) + weightB * array(ib));
113 }
114
115 // Setup of arrays, provided as external vectors of
116 // samples. These functions automatically compute the
117 // derivatives (the arrays .._y_dt and y_dtdt)
Set_array_y(const ChArray<> & m_array_y)118 void ChFunction_Mocap::Set_array_y(const ChArray<>& m_array_y) {
119 array_y = m_array_y;
120
121 Compute_array_dt(array_y, array_y_dt);
122 Compute_array_dt(array_y_dt, array_y_dtdt);
123 }
124
Set_array_y_dt(const ChArray<> & m_array_y_dt)125 void ChFunction_Mocap::Set_array_y_dt(const ChArray<>& m_array_y_dt) {
126 // *** TO DO ***
127 }
128
Set_array_y_dtdt(const ChArray<> & m_array_y_dtdt)129 void ChFunction_Mocap::Set_array_y_dtdt(const ChArray<>& m_array_y_dtdt) {
130 // *** TO DO ***
131 }
132
133 // Parsing of external files, to create mocap streams
134 // from the output of mocap devices
Parse_array_AOA()135 bool ChFunction_Mocap::Parse_array_AOA() {
136 // *** TO DO **** //
137 return true;
138 }
139
Parse_array_Elite()140 bool ChFunction_Mocap::Parse_array_Elite() {
141 // *** TO DO **** //
142 return true;
143 }
144
145 // Return the value of the evaluated function, using
146 // linear interpolation to guess the in-between points,
147 // having the array samples as references.
Get_y(double x) const148 double ChFunction_Mocap::Get_y(double x) const {
149 return LinInterp(array_y, x, timetot);
150 }
151
Get_y_dx(double x) const152 double ChFunction_Mocap::Get_y_dx(double x) const {
153 return LinInterp(array_y_dt, x, timetot);
154 }
155
Get_y_dxdx(double x) const156 double ChFunction_Mocap::Get_y_dxdx(double x) const {
157 return LinInterp(array_y_dtdt, x, timetot);
158 }
159
ArchiveOUT(ChArchiveOut & marchive)160 void ChFunction_Mocap::ArchiveOUT(ChArchiveOut& marchive) {
161 // version number
162 marchive.VersionWrite<ChFunction_Mocap>();
163 // serialize parent class
164 ChFunction::ArchiveOUT(marchive);
165 // serialize all member data:
166 ////marchive << CHNVP(array_y);
167 ////marchive << CHNVP(array_y_dt);
168 ////marchive << CHNVP(array_y_dtdt);
169 marchive << CHNVP(samp_freq);
170 marchive << CHNVP(samples);
171 marchive << CHNVP(timetot);
172 }
173
ArchiveIN(ChArchiveIn & marchive)174 void ChFunction_Mocap::ArchiveIN(ChArchiveIn& marchive) {
175 // version number
176 /*int version =*/ marchive.VersionRead<ChFunction_Mocap>();
177 // deserialize parent class
178 ChFunction::ArchiveIN(marchive);
179 // stream in all member data:
180 ////marchive >> CHNVP(array_y);
181 ////marchive >> CHNVP(array_y_dt);
182 ////marchive >> CHNVP(array_y_dtdt);
183 marchive >> CHNVP(samp_freq);
184 marchive >> CHNVP(samples);
185 marchive >> CHNVP(timetot);
186 }
187
188 } // end namespace chrono
189