FixedObject.cpp 1.3 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475
  1. /** \file itasc/FixedObject.cpp
  2. * \ingroup itasc
  3. */
  4. /*
  5. * FixedObject.cpp
  6. *
  7. * Created on: Feb 10, 2009
  8. * Author: benoitbolsee
  9. */
  10. #include "FixedObject.hpp"
  11. namespace iTaSC{
  12. FixedObject::FixedObject():UncontrolledObject(),
  13. m_finalized(false), m_nframe(0)
  14. {
  15. }
  16. FixedObject::~FixedObject()
  17. {
  18. m_frameArray.clear();
  19. }
  20. int FixedObject::addFrame(const std::string& name, const Frame& frame)
  21. {
  22. if (m_finalized)
  23. return -1;
  24. FrameList::iterator it;
  25. unsigned int i;
  26. for (i=0, it=m_frameArray.begin(); i<m_nframe; i++, it++) {
  27. if (it->first == name) {
  28. // this frame will replace the old frame
  29. it->second = frame;
  30. return i;
  31. }
  32. }
  33. m_frameArray.push_back(FrameList::value_type(name,frame));
  34. return m_nframe++;
  35. }
  36. int FixedObject::addEndEffector(const std::string& name)
  37. {
  38. // verify that this frame name exist
  39. FrameList::iterator it;
  40. unsigned int i;
  41. for (i=0, it=m_frameArray.begin(); i<m_nframe; i++, it++) {
  42. if (it->first == name) {
  43. return i;
  44. }
  45. }
  46. return -1;
  47. }
  48. bool FixedObject::finalize()
  49. {
  50. if (m_finalized)
  51. return true;
  52. initialize(0, m_nframe);
  53. m_finalized = true;
  54. return true;
  55. }
  56. const Frame& FixedObject::getPose(const unsigned int frameIndex)
  57. {
  58. if (frameIndex < m_nframe) {
  59. return m_frameArray[frameIndex].second;
  60. } else {
  61. return F_identity;
  62. }
  63. }
  64. }