1 |
|
|
// |
2 |
|
|
// node-rod.cpp |
3 |
|
|
// gepetto-viewer |
4 |
|
|
// |
5 |
|
|
// Created by Pierre Fernbach in may 2015. |
6 |
|
|
// Copyright (c) 2015 LAAS-CNRS. All rights reserved. |
7 |
|
|
// |
8 |
|
|
#include <gepetto/viewer/node-rod.h> |
9 |
|
|
|
10 |
|
|
#include <sstream> |
11 |
|
|
|
12 |
|
|
#include "log.hh" |
13 |
|
|
|
14 |
|
|
namespace gepetto { |
15 |
|
|
namespace viewer { |
16 |
|
|
|
17 |
|
|
// ---- protected constructor ---- // |
18 |
|
|
|
19 |
|
|
NodeRod::NodeRod(const std::string& name, osgVector4 color, float radius, |
20 |
|
|
float totalLength, size_t maxCapsule) |
21 |
|
|
: Node(name), list_of_capsule_() { |
22 |
|
|
radius_ = radius; |
23 |
|
|
totalLength_ = totalLength; |
24 |
|
|
color_ = osgVector4(color); |
25 |
|
|
maxCapsule_ = maxCapsule; |
26 |
|
|
|
27 |
|
|
osgVector3 zero; |
28 |
|
|
// Z -> -X |
29 |
|
|
osgQuat y_90(M_PI / 2, osgVector3(0, 1, 0)); |
30 |
|
|
for (size_t i = 0; i < maxCapsule; i++) { |
31 |
|
|
std::stringstream nameCap; |
32 |
|
|
nameCap << name << "_cap" << i; |
33 |
|
|
LeafNodeCapsulePtr_t cap = LeafNodeCapsule::create( |
34 |
|
|
nameCap.str(), radius, (totalLength / ((float)maxCapsule)), color); |
35 |
|
|
cap->setStaticTransform(zero, y_90); |
36 |
|
|
list_of_capsule_.push_back(cap); |
37 |
|
|
this->asQueue()->addChild(cap->asGroup()); |
38 |
|
|
} |
39 |
|
|
} |
40 |
|
|
|
41 |
|
|
NodeRod::NodeRod(const NodeRod& other) : Node(other) { |
42 |
|
|
size_t i; |
43 |
|
|
radius_ = other.radius(); |
44 |
|
|
totalLength_ = other.totalLength(); |
45 |
|
|
color_ = other.color(); |
46 |
|
|
maxCapsule_ = other.maxCapsule(); |
47 |
|
|
|
48 |
|
|
for (i = 0; i < (size_t)other.maxCapsule(); i++) { |
49 |
|
|
LeafNodeCapsulePtr_t cap = LeafNodeCapsule::createCopy(other.getCapsule(i)); |
50 |
|
|
list_of_capsule_.push_back(cap); |
51 |
|
|
} |
52 |
|
|
} |
53 |
|
|
|
54 |
|
|
void NodeRod::initWeakPtr(NodeRodWeakPtr other_weak_ptr) { |
55 |
|
|
weak_ptr_ = other_weak_ptr; |
56 |
|
|
} |
57 |
|
|
|
58 |
|
|
//------- public :------- // |
59 |
|
|
|
60 |
|
|
NodeRodPtr_t NodeRod::create(const std::string& name, osgVector4 color, |
61 |
|
|
float radius, float totalLength, |
62 |
|
|
short maxCapsule) { |
63 |
|
|
NodeRodPtr_t shared_ptr( |
64 |
|
|
new NodeRod(name, color, radius, totalLength, maxCapsule)); |
65 |
|
|
|
66 |
|
|
// Add reference to itself |
67 |
|
|
shared_ptr->initWeakPtr(shared_ptr); |
68 |
|
|
|
69 |
|
|
return shared_ptr; |
70 |
|
|
} |
71 |
|
|
|
72 |
|
|
NodeRodPtr_t NodeRod::createCopy(NodeRodPtr_t other) { |
73 |
|
|
NodeRodPtr_t shared_ptr(new NodeRod(*other)); |
74 |
|
|
|
75 |
|
|
// Add reference to itself |
76 |
|
|
shared_ptr->initWeakPtr(shared_ptr); |
77 |
|
|
|
78 |
|
|
return shared_ptr; |
79 |
|
|
} |
80 |
|
|
|
81 |
|
|
NodeRodPtr_t NodeRod::self(void) const { return weak_ptr_.lock(); } |
82 |
|
|
|
83 |
|
|
std::string NodeRod::getCapsuleName(size_t i) { |
84 |
|
|
return list_of_capsule_.at(i)->getID(); |
85 |
|
|
} |
86 |
|
|
|
87 |
|
|
LeafNodeCapsulePtr_t NodeRod::getCapsule(size_t i) const { |
88 |
|
|
return list_of_capsule_.at(i); |
89 |
|
|
} |
90 |
|
|
|
91 |
|
|
/** |
92 |
|
|
Applyconfiguration to all the capsules of the rod, |
93 |
|
|
params is a sequence of maxCapsule parameter, size 8 (length + pos + quat), or |
94 |
|
|
size 7 (pos + quat) quat : (w,x,y,z) |
95 |
|
|
*/ |
96 |
|
|
void NodeRod::applyConfiguration(std::vector<std::vector<value_type> > params) { |
97 |
|
|
if (params.size() != (size_t)maxCapsule_) |
98 |
|
|
log() << "Error in rod Applyconfiguration : the size of the sequence must " |
99 |
|
|
"be the same as the number of capsules" |
100 |
|
|
<< std::endl; |
101 |
|
|
|
102 |
|
|
for (size_t i = 0; i < params.size(); i++) { |
103 |
|
|
if (params.at(i).size() == 8) { |
104 |
|
|
list_of_capsule_.at(i)->setHeight(params.at(i)[0]); |
105 |
|
|
::osg::Quat quat = ::osg::Quat(params.at(i)[5], params.at(i)[6], |
106 |
|
|
params.at(i)[7], params.at(i)[4]); |
107 |
|
|
::osg::Vec3f pos = |
108 |
|
|
::osg::Vec3f(params.at(i)[1], params.at(i)[2], params.at(i)[3]); |
109 |
|
|
list_of_capsule_.at(i)->applyConfiguration(pos, quat); |
110 |
|
|
} else if (params.at(i).size() == 7) { |
111 |
|
|
::osg::Quat quat = ::osg::Quat(params.at(i)[4], params.at(i)[5], |
112 |
|
|
params.at(i)[6], params.at(i)[3]); |
113 |
|
|
::osg::Vec3f pos = |
114 |
|
|
::osg::Vec3f(params.at(i)[0], params.at(i)[1], params.at(i)[2]); |
115 |
|
|
list_of_capsule_.at(i)->applyConfiguration(pos, quat); |
116 |
|
|
|
117 |
|
|
} else |
118 |
|
|
log() << "Parameter must have 7 or 8 elements" << std::endl; |
119 |
|
|
} |
120 |
|
|
} |
121 |
|
|
|
122 |
|
|
} /* namespace viewer */ |
123 |
|
|
} // namespace gepetto |