Newer
Older
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
EigenTransform CoordinateSystem::getTransformation(CoordinateSystem const& c1, CoordinateSystem const& c2)
{
CoordinateSystem const* a{&c1};
CoordinateSystem const* b{&c2};
CoordinateSystem const* commonBase{nullptr};
while (a != b && b != nullptr)
{
a = &c1;
while (a != b && a != nullptr)
{
a = a->getReference();
}
if (a == b)
break;
b = b->getReference();
}
if (a == b && a != nullptr)
{
commonBase = a;
}
else
{
throw std::string("no connection between coordinate systems found!");
}
EigenTransform t = EigenTransform::Identity();
auto* p = &c1;
while (p != commonBase)
{
t = p->getTransform() * t;
p = p->getReference();
}
p = &c2;
while (p != commonBase)
{
t = p->getTransform().inverse(Eigen::TransformTraits::Isometry) * t;
p = p->getReference();
}
return t;
}