1 #define EZC3D_API_EXPORTS
2 ///
3 /// \file Parameters.cpp
4 /// \brief Implementation of Parameters class
5 /// \author Pariterre
6 /// \version 1.0
7 /// \date October 17th, 2018
8 ///
9
10 #include "Parameters.h"
11 #include "Header.h"
12
Parameters()13 ezc3d::ParametersNS::Parameters::Parameters():
14 _parametersStart(1),
15 _checksum(0x50),
16 _nbParamBlock(0),
17 _processorType(PROCESSOR_TYPE::NO_PROCESSOR_TYPE) {
18 setMandatoryParameters();
19 }
20
Parameters(ezc3d::c3d & c3d,std::fstream & file)21 ezc3d::ParametersNS::Parameters::Parameters(
22 ezc3d::c3d &c3d,
23 std::fstream &file) :
24 _parametersStart(0),
25 _checksum(0),
26 _nbParamBlock(0),
27 _processorType(PROCESSOR_TYPE::NO_PROCESSOR_TYPE) {
28 // Read the Parameters Header (assuming Intel processor)
29 _parametersStart = c3d.readUint(
30 processorType(),
31 file,
32 1*ezc3d::DATA_TYPE::BYTE, static_cast<int>(
33 256*ezc3d::DATA_TYPE::WORD*(
34 c3d.header().parametersAddress()-1)
35 + c3d.header().nbOfZerosBeforeHeader()),
36 std::ios::beg);
37 _checksum = c3d.readUint(
38 processorType(), file, 1*ezc3d::DATA_TYPE::BYTE);
39 _nbParamBlock = c3d.readUint(
40 processorType(), file, 1*ezc3d::DATA_TYPE::BYTE);
41 size_t processorTypeId = c3d.readUint(
42 processorType(), file, 1*ezc3d::DATA_TYPE::BYTE);
43 if (_checksum == 0 && _parametersStart == 0){
44 // In theory, if this happens, this is a bad c3d formatting and should
45 // return an error, but for some reason Qualisys decided that they
46 // would not comply to the standard.
47 // Therefore set put "_parameterStart" and "_checksum" to 0
48 // This is a patch for Qualisys bad formatting c3d
49 _parametersStart = 1;
50 _checksum = 0x50;
51 }
52 if (_checksum != 0x50) // If checkbyte is wrong
53 throw std::ios_base::failure("File must be a valid c3d file");
54
55 if (processorTypeId == 84)
56 _processorType = ezc3d::PROCESSOR_TYPE::INTEL;
57 else if (processorTypeId == 85)
58 _processorType = ezc3d::PROCESSOR_TYPE::DEC;
59 else if (processorTypeId == 86){
60 _processorType = ezc3d::PROCESSOR_TYPE::MIPS;
61 throw std::runtime_error(
62 "MIPS processor type not supported yet, please open a "
63 "GitHub issue to report that you want this feature!");
64 }
65 else
66 throw std::runtime_error("Could not read the processor type");
67
68 // Read parameter or group
69 std::streampos nextParamByteInFile(
70 static_cast<int>(file.tellg())
71 + static_cast<int>(_parametersStart) - ezc3d::DATA_TYPE::BYTE);
72 while (nextParamByteInFile)
73 {
74 // Check if we spontaneously got to the next parameter.
75 // Otherwise c3d is messed up
76 if (file.tellg() != nextParamByteInFile)
77 throw std::ios_base::failure("Bad c3d formatting");
78
79 // Nb of char in the group name, locked if negative,
80 // 0 if we finished the section
81 int nbCharInName(
82 c3d.readInt(
83 processorType(), file, 1*ezc3d::DATA_TYPE::BYTE));
84 if (nbCharInName == 0)
85 break;
86 int id(c3d.readInt(processorType(), file, 1*ezc3d::DATA_TYPE::BYTE));
87
88 // Make sure there at least enough group
89 for (size_t i = _groups.size(); i < static_cast<size_t>(abs(id)); ++i)
90 _groups.push_back(ezc3d::ParametersNS::GroupNS::Group());
91
92 // Group ID always negative for groups
93 // and positive parameter of group ID
94 if (id < 0) {
95 nextParamByteInFile = group(
96 static_cast<size_t>
97 (abs(id)-1)).read(c3d, *this, file, nbCharInName);
98 }
99 else {
100 nextParamByteInFile = group(
101 static_cast<size_t>(id-1)).parameter(
102 c3d, *this, file, nbCharInName);
103 }
104 }
105
106 // If some mandatory groups/parameters are not set by having a non
107 // compliant C3D, fix it
108 setMandatoryParameters();
109 }
110
isMandatory(const std::string & groupName)111 bool ezc3d::ParametersNS::Parameters::isMandatory(
112 const std::string &groupName)
113 {
114 if (!groupName.compare("POINT")
115 || !groupName.compare("ANALOG")
116 || !groupName.compare("FORCE_PLATFORM")){
117 return true;
118 }
119 return false;
120 }
121
isMandatory(const std::string & groupName,const std::string & parameterName)122 bool ezc3d::ParametersNS::Parameters::isMandatory(
123 const std::string &groupName,
124 const std::string ¶meterName)
125 {
126 if (!groupName.compare("POINT")){
127 if (!parameterName.compare("USED")
128 || !parameterName.compare("LABELS")
129 || !parameterName.compare("DESCRIPTIONS")
130 || !parameterName.compare("SCALE")
131 || !parameterName.compare("UNITS")
132 || !parameterName.compare("RATE")
133 || !parameterName.compare("DATA_START")
134 || !parameterName.compare("FRAMES")){
135 return true;
136 }
137 } else if (!groupName.compare("ANALOG")){
138 if (!parameterName.compare("USED")
139 || !parameterName.compare("LABELS")
140 || !parameterName.compare("DESCRIPTIONS")
141 || !parameterName.compare("GEN_SCALE")
142 || !parameterName.compare("SCALE")
143 || !parameterName.compare("OFFSET")
144 || !parameterName.compare("UNITS")
145 || !parameterName.compare("RATE")
146 || !parameterName.compare("FORMAT")
147 || !parameterName.compare("BITS")){
148 return true;
149 }
150 } else if (!groupName.compare("FORCE_PLATFORM")){
151 if (!parameterName.compare("USED")
152 || !parameterName.compare("TYPE")
153 || !parameterName.compare("CHANNEL")
154 || !parameterName.compare("ZERO")
155 || !parameterName.compare("ORIGIN")
156 || !parameterName.compare("CORNERS")
157 || !parameterName.compare("CAL_MATRIX")){
158 return true;
159 }
160 }
161 return false;
162 }
163
setMandatoryParameters()164 void ezc3d::ParametersNS::Parameters::setMandatoryParameters() {
165 // Mandatory groups
166 {
167 if (!isGroup("POINT")){
168 group(ezc3d::ParametersNS::GroupNS::Group ("POINT"));
169 }
170
171 ezc3d::ParametersNS::GroupNS::Group& grp(group("POINT"));
172 if (!grp.isParameter("USED")){
173 ezc3d::ParametersNS::GroupNS::Parameter p("USED", "");
174 p.set(0);
175 p.lock();
176 grp.parameter(p);
177 }
178 if (!grp.isParameter("LABELS")){
179 ezc3d::ParametersNS::GroupNS::Parameter p("LABELS", "");
180 p.set(std::vector<std::string>()={});
181 grp.parameter(p);
182 }
183 if (!grp.isParameter("DESCRIPTIONS")){
184 ezc3d::ParametersNS::GroupNS::Parameter p("DESCRIPTIONS", "");
185 p.set(std::vector<std::string>()={});
186 grp.parameter(p);
187 }
188 if (!grp.isParameter("SCALE")){
189 ezc3d::ParametersNS::GroupNS::Parameter p("SCALE", "");
190 p.set(-1.0);
191 p.lock();
192 grp.parameter(p);
193 }
194 if (!grp.isParameter("UNITS")){
195 ezc3d::ParametersNS::GroupNS::Parameter p("UNITS", "");
196 p.set(std::vector<std::string>()={});
197 grp.parameter(p);
198 }
199 if (!grp.isParameter("RATE")){
200 ezc3d::ParametersNS::GroupNS::Parameter p("RATE", "");
201 p.set(0.0);
202 p.lock();
203 grp.parameter(p);
204 }
205 if (!grp.isParameter("DATA_START")){
206 ezc3d::ParametersNS::GroupNS::Parameter p("DATA_START", "");
207 p.set(0);
208 p.lock();
209 grp.parameter(p);
210 }
211 if (!grp.isParameter("FRAMES")){
212 ezc3d::ParametersNS::GroupNS::Parameter p("FRAMES", "");
213 p.set(0);
214 p.lock();
215 grp.parameter(p);
216 }
217 }
218 {
219 if (!isGroup("ANALOG")){
220 group(ezc3d::ParametersNS::GroupNS::Group ("ANALOG"));
221 }
222
223 ezc3d::ParametersNS::GroupNS::Group& grp(group("ANALOG"));
224 if (!grp.isParameter("USED")){
225 ezc3d::ParametersNS::GroupNS::Parameter p("USED", "");
226 p.set(0);
227 p.lock();
228 grp.parameter(p);
229 }
230 if (!grp.isParameter("LABELS")){
231 ezc3d::ParametersNS::GroupNS::Parameter p("LABELS", "");
232 p.set(std::vector<std::string>()={});
233 grp.parameter(p);
234 }
235 if (!grp.isParameter("DESCRIPTIONS")){
236 ezc3d::ParametersNS::GroupNS::Parameter p("DESCRIPTIONS", "");
237 p.set(std::vector<std::string>()={});
238 grp.parameter(p);
239 }
240 if (!grp.isParameter("GEN_SCALE")){
241 ezc3d::ParametersNS::GroupNS::Parameter p("GEN_SCALE", "");
242 p.set(1.0);
243 grp.parameter(p);
244 }
245 if (!grp.isParameter("SCALE")){
246 ezc3d::ParametersNS::GroupNS::Parameter p("SCALE", "");
247 p.set(std::vector<double>()={});
248 grp.parameter(p);
249 }
250 if (!grp.isParameter("OFFSET")){
251 ezc3d::ParametersNS::GroupNS::Parameter p("OFFSET", "");
252 p.set(std::vector<int>()={});
253 grp.parameter(p);
254 }
255 if (!grp.isParameter("UNITS")){
256 ezc3d::ParametersNS::GroupNS::Parameter p("UNITS", "");
257 p.set(std::vector<std::string>()={});
258 grp.parameter(p);
259 }
260 if (!grp.isParameter("RATE")){
261 ezc3d::ParametersNS::GroupNS::Parameter p("RATE", "");
262 p.set(0.0);
263 p.lock();
264 grp.parameter(p);
265 }
266 if (!grp.isParameter("FORMAT")){
267 ezc3d::ParametersNS::GroupNS::Parameter p("FORMAT", "");
268 p.set(std::vector<std::string>()={});
269 grp.parameter(p);
270 }
271 if (!grp.isParameter("BITS")){
272 ezc3d::ParametersNS::GroupNS::Parameter p("BITS", "");
273 p.set(std::vector<int>()={});
274 grp.parameter(p);
275 }
276 }
277 {
278 if (!isGroup("FORCE_PLATFORM")){
279 group(ezc3d::ParametersNS::GroupNS::Group ("FORCE_PLATFORM"));
280 }
281
282 ezc3d::ParametersNS::GroupNS::Group& grp(group("FORCE_PLATFORM"));
283 if (!grp.isParameter("USED")){
284 ezc3d::ParametersNS::GroupNS::Parameter p("USED", "");
285 p.set(0);
286 grp.parameter(p);
287 }
288 if (!grp.isParameter("TYPE")){
289 ezc3d::ParametersNS::GroupNS::Parameter p("TYPE", "");
290 p.set(std::vector<int>()={});
291 grp.parameter(p);
292 }
293 if (!grp.isParameter("ZERO")){
294 ezc3d::ParametersNS::GroupNS::Parameter p("ZERO", "");
295 p.set(std::vector<int>()={1,0});
296 grp.parameter(p);
297 }
298 if (!grp.isParameter("CORNERS")){
299 ezc3d::ParametersNS::GroupNS::Parameter p("CORNERS", "");
300 p.set(std::vector<double>()={});
301 grp.parameter(p);
302 }
303 if (!grp.isParameter("ORIGIN")){
304 ezc3d::ParametersNS::GroupNS::Parameter p("ORIGIN", "");
305 p.set(std::vector<double>()={});
306 grp.parameter(p);
307 }
308 if (!grp.isParameter("CHANNEL")){
309 ezc3d::ParametersNS::GroupNS::Parameter p("CHANNEL", "");
310 p.set(std::vector<int>()={});
311 grp.parameter(p);
312 }
313 if (!grp.isParameter("CAL_MATRIX")){
314 ezc3d::ParametersNS::GroupNS::Parameter p("CAL_MATRIX", "");
315 p.set(std::vector<double>()={});
316 grp.parameter(p);
317 }
318 }
319 }
320
print() const321 void ezc3d::ParametersNS::Parameters::print() const {
322 std::cout << "Parameters header" << std::endl;
323 std::cout << "parametersStart = " << parametersStart() << std::endl;
324 std::cout << "nbParamBlock = " << nbParamBlock() << std::endl;
325 std::cout << "processorType = " << processorType() << std::endl;
326
327 for (size_t i = 0; i < nbGroups(); ++i){
328 std::cout << "Group " << i << std::endl;
329 group(i).print();
330 std::cout << std::endl;
331 }
332 std::cout << std::endl;
333 }
334
write(std::fstream & f,std::streampos & dataStartPosition,const ezc3d::Header & header,const ezc3d::WRITE_FORMAT & format) const335 ezc3d::ParametersNS::Parameters ezc3d::ParametersNS::Parameters::write(
336 std::fstream &f,
337 std::streampos &dataStartPosition,
338 const ezc3d::Header& header,
339 const ezc3d::WRITE_FORMAT& format) const {
340 ezc3d::ParametersNS::Parameters p(prepareCopyForWriting(header, format));
341
342 // Write the header of parameters
343 f.write(reinterpret_cast<const char*>(&p._parametersStart), ezc3d::BYTE);
344 int checksum(0x50);
345 f.write(reinterpret_cast<const char*>(&checksum), ezc3d::BYTE);
346 // Leave a blank space which will be later fill
347 // (number of block can't be known before writing them)
348 std::streampos pos(f.tellg()); // remember where to input this value later
349 int blankValue(0);
350 f.write(reinterpret_cast<const char*>(&blankValue), ezc3d::BYTE);
351 int processorType = PROCESSOR_TYPE::INTEL;
352 f.write(reinterpret_cast<const char*>(&processorType), ezc3d::BYTE);
353
354 // Write each groups
355 for (size_t i=0; i < p.nbGroups(); ++i){
356 const ezc3d::ParametersNS::GroupNS::Group& currentGroup(p.group(i));
357 if (!currentGroup.isEmpty())
358 currentGroup.write(f, -static_cast<int>(i+1), dataStartPosition);
359 }
360
361 // Move the cursor to a beginning of a block
362 std::streampos currentPos(f.tellg());
363 for (int i=0; i<512 - static_cast<int>(currentPos) % 512; ++i){
364 f.write(reinterpret_cast<const char*>(&blankValue), ezc3d::BYTE);
365 }
366 // Go back at the left blank space and write the current position
367 currentPos = f.tellg();
368 f.seekg(pos);
369 int nBlocksToNext = int(currentPos - pos-2)/512;
370 if (int(currentPos - pos-2) % 512 > 0)
371 ++nBlocksToNext;
372 f.write(reinterpret_cast<const char*>(&nBlocksToNext), ezc3d::BYTE);
373 f.seekg(currentPos);
374
375 return p;
376 }
377
378 ezc3d::ParametersNS::Parameters
prepareCopyForWriting(const ezc3d::Header & header,const ezc3d::WRITE_FORMAT & format) const379 ezc3d::ParametersNS::Parameters::prepareCopyForWriting(
380 const ezc3d::Header& header,
381 const ezc3d::WRITE_FORMAT& format) const
382 {
383 // A copy must be done since modifications are made to some parameters
384 ezc3d::ParametersNS::Parameters params(*this);
385
386 // Reevalute the number of frames
387 int nFrames(this->group("POINT").parameter("FRAMES").valuesAsInt()[0]);
388 if (nFrames > 0xFFFF){
389 ezc3d::ParametersNS::GroupNS::Parameter frames(
390 params.group("POINT").parameter("FRAMES"));
391 frames.set(-1);
392 params.group("POINT").parameter(frames);
393 }
394
395 // Ensure that the right point scale is in the file
396 ezc3d::ParametersNS::GroupNS::Parameter scaleFactorParam;
397 double pointScaleFactor;
398 if (params.group("POINT").parameter("SCALE").valuesAsDouble().size() ){
399 scaleFactorParam = params.group("POINT").parameter("SCALE");
400 pointScaleFactor = -fabs(scaleFactorParam.valuesAsDouble()[0]);
401 }
402 else {
403 pointScaleFactor = -fabs(header.scaleFactor());
404 scaleFactorParam.name("SCALE");
405 }
406 scaleFactorParam.set(pointScaleFactor);
407 params.group("POINT").parameter(scaleFactorParam);
408
409 // Ensure that the right analog scale is in the file
410 ezc3d::ParametersNS::GroupNS::Parameter analogScaleFactorParam;
411 std::vector<double> analogScaleFactor;
412 if (params.group("ANALOG").parameter("USED").valuesAsInt()[0] == 0){
413 analogScaleFactorParam.name("SCALE");
414 }
415 else if (params.group("ANALOG").parameter("SCALE").valuesAsDouble().size() > 0) {
416 analogScaleFactorParam = params.group("ANALOG").parameter("SCALE");
417 analogScaleFactor = params.group("ANALOG").parameter("SCALE").valuesAsDouble();
418 }
419 else {
420 analogScaleFactor.push_back(header.scaleFactor());
421 analogScaleFactorParam.name("SCALE");
422 }
423 analogScaleFactorParam.set(analogScaleFactor);
424 params.group("ANALOG").parameter(analogScaleFactorParam);
425
426 // Use Intel floating with no extra scaling
427 ezc3d::ParametersNS::GroupNS::Parameter genScale(
428 params.group("ANALOG").parameter("GEN_SCALE"));
429 genScale.set(1.0);
430 params.group("ANALOG").parameter(genScale);
431
432 size_t cmp = 1;
433 std::string mod = "";
434 do {
435 auto offset(params.group("ANALOG").parameter("OFFSET" + mod));
436 std::vector<int> offsetValues(offset.valuesAsInt().size());
437 for (size_t i=0; i<offsetValues.size(); ++i){
438 offsetValues[i] = 0;
439 }
440 offset.set(offsetValues);
441 params.group("ANALOG").parameter(offset);
442 ++cmp;
443 mod = std::to_string(cmp);
444 } while (params.group("ANALOG").isParameter("OFFSET" + mod));
445
446 // Add the parameter EZC3D:VERSION and EZC3D:CONTACT
447 if (!params.isGroup("EZC3D")){
448 params.group(ezc3d::ParametersNS::GroupNS::Group("EZC3D"));
449 }
450 // Add/replace the version in the EZC3D group
451 ezc3d::ParametersNS::GroupNS::Parameter version("VERSION");
452 version.set(EZC3D_VERSION);
453 params.group("EZC3D").parameter(version);
454 // Add/replace the CONTACT in the EZC3D group
455 ezc3d::ParametersNS::GroupNS::Parameter contact("CONTACT");
456 contact.set(EZC3D_CONTACT);
457 params.group("EZC3D").parameter(contact);
458
459 if (format == WRITE_FORMAT::NEXUS){
460 // Do some stuff related to Nexus
461 }
462 return params;
463 }
464
parametersStart() const465 size_t ezc3d::ParametersNS::Parameters::parametersStart() const {
466 return _parametersStart;
467 }
468
checksum() const469 size_t ezc3d::ParametersNS::Parameters::checksum() const {
470 return _checksum;
471 }
472
nbParamBlock() const473 size_t ezc3d::ParametersNS::Parameters::nbParamBlock() const {
474 return _nbParamBlock;
475 }
476
processorType() const477 ezc3d::PROCESSOR_TYPE ezc3d::ParametersNS::Parameters::processorType() const {
478 return _processorType;
479 }
480
nbGroups() const481 size_t ezc3d::ParametersNS::Parameters::nbGroups() const {
482 return _groups.size();
483 }
484
isGroup(const std::string & groupName) const485 bool ezc3d::ParametersNS::Parameters::isGroup(
486 const std::string &groupName) const
487 {
488 try {
489 groupIdx(groupName);
490 return true;
491 } catch (std::invalid_argument) {
492 return false;
493 }
494 }
495
groupIdx(const std::string & groupName) const496 size_t ezc3d::ParametersNS::Parameters::groupIdx(
497 const std::string &groupName) const {
498 for (size_t i = 0; i < nbGroups(); ++i)
499 if (!group(i).name().compare(groupName))
500 return i;
501 throw std::invalid_argument(
502 "Parameters::groupIdx could not find " + groupName);
503 }
504
505 const ezc3d::ParametersNS::GroupNS::Group&
group(size_t idx) const506 ezc3d::ParametersNS::Parameters::group(
507 size_t idx) const {
508 try {
509 return _groups.at(idx);
510 } catch(std::out_of_range) {
511 throw std::out_of_range(
512 "Parameters::group method is trying to access the group "
513 + std::to_string(idx) +
514 " while the maximum number of groups is "
515 + std::to_string(nbGroups()) + ".");
516 }
517 }
518
519 ezc3d::ParametersNS::GroupNS::Group&
group(size_t idx)520 ezc3d::ParametersNS::Parameters::group(
521 size_t idx) {
522 try {
523 return _groups.at(idx);
524 } catch(std::out_of_range) {
525 throw std::out_of_range(
526 "Parameters::group method is trying to access the group "
527 + std::to_string(idx) +
528 " while the maximum number of groups is "
529 + std::to_string(nbGroups()) + ".");
530 }
531 }
532
533 const ezc3d::ParametersNS::GroupNS::Group&
group(const std::string & groupName) const534 ezc3d::ParametersNS::Parameters::group(
535 const std::string &groupName) const {
536 return group(groupIdx(groupName));
537 }
538
539 ezc3d::ParametersNS::GroupNS::Group&
group(const std::string & groupName)540 ezc3d::ParametersNS::Parameters::group(
541 const std::string &groupName) {
542 return group(groupIdx(groupName));
543 }
544
group(const ezc3d::ParametersNS::GroupNS::Group & g)545 void ezc3d::ParametersNS::Parameters::group(
546 const ezc3d::ParametersNS::GroupNS::Group &g) {
547 // If the group already exist, override and merge
548 size_t alreadyExtIdx(SIZE_MAX);
549 for (size_t i = 0; i < nbGroups(); ++i)
550 if (!group(i).name().compare(g.name()))
551 alreadyExtIdx = i;
552 if (alreadyExtIdx == SIZE_MAX)
553 _groups.push_back(g);
554 else {
555 for (size_t i=0; i < g.nbParameters(); ++i)
556 _groups[alreadyExtIdx].parameter(g.parameter(i));
557 }
558 }
559
remove(const std::string & name)560 void ezc3d::ParametersNS::Parameters::remove(
561 const std::string &name)
562 {
563 remove(groupIdx(name));
564 }
565
remove(size_t idx)566 void ezc3d::ParametersNS::Parameters::remove(
567 size_t idx)
568 {
569 if (idx >= nbGroups()){
570 throw std::out_of_range(
571 "Parameters::group method is trying to access the group "
572 + std::to_string(idx) +
573 " while the maximum number of groups is "
574 + std::to_string(nbGroups()) + ".");
575 }
576 _groups.erase(_groups.begin() + idx);
577 }
578
579 const std::vector<ezc3d::ParametersNS::GroupNS::Group>&
groups() const580 ezc3d::ParametersNS::Parameters::groups() const {
581 return _groups;
582 }
583