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
72
|
/* SPDX-FileCopyrightText: 2009 Benoit Bolsee
*
* SPDX-License-Identifier: LGPL-2.1-or-later */
/** \file
* \ingroup intern_itasc
*/
#include "FixedObject.hpp"
namespace iTaSC{
FixedObject::FixedObject():UncontrolledObject(),
m_finalized(false), m_nframe(0)
{
}
FixedObject::~FixedObject()
{
m_frameArray.clear();
}
int FixedObject::addFrame(const std::string& name, const Frame& frame)
{
if (m_finalized)
return -1;
FrameList::iterator it;
unsigned int i;
for (i=0, it=m_frameArray.begin(); i<m_nframe; i++, it++) {
if (it->first == name) {
// this frame will replace the old frame
it->second = frame;
return i;
}
}
m_frameArray.push_back(FrameList::value_type(name,frame));
return m_nframe++;
}
int FixedObject::addEndEffector(const std::string& name)
{
// verify that this frame name exist
FrameList::iterator it;
unsigned int i;
for (i=0, it=m_frameArray.begin(); i<m_nframe; i++, it++) {
if (it->first == name) {
return i;
}
}
return -1;
}
bool FixedObject::finalize()
{
if (m_finalized)
return true;
initialize(0, m_nframe);
m_finalized = true;
return true;
}
const Frame& FixedObject::getPose(const unsigned int frameIndex)
{
if (frameIndex < m_nframe) {
return m_frameArray[frameIndex].second;
} else {
return F_identity;
}
}
}
|
