Newer
Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
#ifndef COORDINATESYSTEM_H_
#define COORDINATESYSTEM_H_
#include "QuantityVector.h"
#include <Eigen/Dense>
#include <phys/units/quantity.hpp>
typedef Eigen::Transform<double, 3, Eigen::Affine> EigenTransform;
typedef Eigen::Translation<double, 3> EigenTranslation;
class CoordinateSystem
{
CoordinateSystem const* reference = nullptr;
EigenTransform transf;
CoordinateSystem(CoordinateSystem const& reference, EigenTransform const& transf) :
reference(&reference),
transf(transf)
{
}
public:
static EigenTransform getTransformation(CoordinateSystem const& c1, CoordinateSystem const& c2);
CoordinateSystem() : // for creating the root CS
transf(EigenTransform::Identity())
{
}
auto& operator=(const CoordinateSystem& pCS)
{
reference = pCS.reference;
transf = pCS.transf;
return *this;
}
auto translate(QuantityVector<phys::units::length_d> vector) const
{
EigenTransform const translation{EigenTranslation(vector.eVector)};
return CoordinateSystem(*this, translation);
}
auto rotate(QuantityVector<phys::units::length_d> axis, double angle) const
{
EigenTransform const rotation{Eigen::AngleAxisd(M_PI / 6, axis.eVector.normalized())};
return CoordinateSystem(*this, rotation);
}
auto translateAndRotate(QuantityVector<phys::units::length_d> translation, QuantityVector<phys::units::length_d> axis, double angle)
{
EigenTransform const transf{Eigen::AngleAxisd(M_PI / 6, axis.eVector.normalized()) * EigenTranslation(translation.eVector)};
return CoordinateSystem(*this, transf);
}
auto const* getReference() const
{
return reference;
}
auto const& getTransform() const
{
return transf;
}
};
#endif