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 &parameterName)
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