73 lines
1.3 KiB
C++
73 lines
1.3 KiB
C++
/* 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;
|
|
}
|
|
}
|
|
|
|
}
|