33 #include <LinearMath/btQuaternion.h>
34 #include <LinearMath/btVector3.h>
45 m_children(orig.m_children.size()), m_nodes(orig.m_nodes), m_pairs(orig.m_pairs)
49 for (std::size_t i = 0; i < orig.m_children.size(); ++i) {
50 m_children[i] =
new tgStructure(*orig.m_children[i]);
58 tgStructure::tgStructure(
const std::string& space_separated_tags) :
tgTaggable(space_separated_tags)
62 tgStructure::~tgStructure()
64 for (std::size_t i = 0; i < m_children.size(); ++i)
82 addPair(m_nodes[fromNodeIdx], m_nodes[toNodeIdx], tags);
89 if (!m_pairs.contains(p))
91 m_pairs.addPair(
tgPair(from, to, tags));
95 std::ostringstream os;
96 os <<
"A pair matching " << p <<
" already exists in this structure.";
101 void tgStructure::removePair(
const tgPair& pair) {
102 m_pairs.removePair(pair);
103 for (
unsigned int i = 0; i < m_children.size(); i++) {
104 m_children[i]->removePair(pair);
108 void tgStructure::move(
const btVector3& offset)
110 m_nodes.
move(offset);
111 m_pairs.move(offset);
112 for (
size_t i = 0; i < m_children.size(); ++i)
115 assert(pStructure != NULL);
116 pStructure->move(offset);
121 const btVector3& axis,
124 const btQuaternion rotation(axis, angle);
129 const btVector3& fromOrientation,
130 const btVector3& toOrientation)
138 const btQuaternion& rotation)
140 m_nodes.addRotation(fixedPoint, rotation);
141 m_pairs.addRotation(fixedPoint, rotation);
143 for (std::size_t i = 0; i < m_children.size(); ++i)
146 assert(pStructure != NULL);
151 void tgStructure::scale(
double scaleFactor) {
153 scale(structureCentroid, scaleFactor);
156 void tgStructure::scale(
const btVector3& referencePoint,
double scaleFactor) {
157 m_nodes.scale(referencePoint, scaleFactor);
158 m_pairs.scale(referencePoint, scaleFactor);
160 for (
int i = 0; i < m_children.size(); i++) {
162 assert(childStructure != NULL);
163 childStructure->scale(referencePoint, scaleFactor);
175 m_children.push_back(pChild);
186 btVector3 centroid = btVector3(0, 0, 0);
188 std::queue< const tgStructure*> q;
195 for (
int i = 0; i < structure->m_nodes.size(); i++) {
196 centroid += structure->m_nodes[i];
199 for (
int i = 0; i < structure->m_children.size(); i++) {
200 q.push(structure->m_children[i]);
203 return centroid/numNodes;
207 std::queue<tgStructure*> q;
214 for (
int i = 0; i < structure->m_nodes.size(); i++) {
215 if (structure->m_nodes[i].hasAllTags(tags)) {
216 return structure->m_nodes[i];
219 for (
int i = 0; i < structure->m_children.size(); i++) {
220 q.push(structure->m_children[i]);
223 throw std::invalid_argument(
"Node not found: " + tags);
227 std::queue<tgStructure*> q;
234 for (
int i = 0; i < structure->m_pairs.size(); i++) {
235 if ((structure->m_pairs[i].getFrom() == from && structure->m_pairs[i].getTo() == to) ||
236 (structure->m_pairs[i].getFrom() == to && structure->m_pairs[i].getTo() == from)) {
237 return structure->m_pairs[i];
240 for (
int i = 0; i < structure->m_children.size(); i++) {
241 q.push(structure->m_children[i]);
244 std::ostringstream pairString;
245 pairString << from <<
", " << to;
246 throw std::invalid_argument(
"Pair not found: " + pairString.str());
250 std::queue<tgStructure*> q;
252 for (
int i = 0; i < m_children.size(); i++) {
253 q.push(m_children[i]);
259 if (structure->hasAllTags(tags)) {
262 for (
int i = 0; i < structure->m_children.size(); i++) {
263 q.push(structure->m_children[i]);
266 throw std::invalid_argument(
"Child structure not found: " + tags);
272 std::stringstream os;
273 std::string indent = std::string(2 * (indentLevel),
' ');
274 os << indent <<
"structure:" << std::endl;
275 os << indent <<
" tags: " <<
asYamlList(structure.getTags()) << std::endl;
284 std::stringstream os;
285 std::string indent = std::string(2 * (indentLevel),
' ');
286 os << indent <<
"- tags: " <<
asYamlList(structure.getTags()) << std::endl;
293 std::string
asYamlItems(
const std::vector<tgStructure*> structures,
int indentLevel)
295 std::stringstream os;
296 std::string indent = std::string(2 * (indentLevel),
' ');
297 if (structures.size() == 0) {
298 os << indent <<
"structures: []" << std::endl;
302 os << indent <<
"structures:" << std::endl;
303 for(
size_t i = 0; i < structures.size(); i++)
305 os <<
asYamlItem(*structures[i], indentLevel+1);
std::string asYamlElement(const tgStructure &structure, int indentLevel)
tgStructure & findChild(const std::string &name)
static btQuaternion getQuaternionBetween(btVector3 a, btVector3 b, const btVector3 &fallbackAxis=btVector3(0, 0, 0))
tgNode & findNode(const std::string &name)
const std::vector< tgStructure * > & getChildren() const
void addChild(tgStructure *child)
Definition of class tgPair.
btVector3 getCentroid() const
const tgNodes & getNodes() const
void addPair(int fromNodeIdx, int toNodeIdx, std::string tags="")
const tgPairs & getPairs() const
void addRotation(const btVector3 &fixedPoint, const btVector3 &axis, double angle)
Definition of class tgNode.
Definition of class tgStructure.
tgPair & findPair(const btVector3 &from, const btVector3 &to)
void move(const btVector3 &offset)
std::string asYamlItem(const tgStructure &structure, int indentLevel)
std::string asYamlItems(const std::vector< tgStructure * > structures, int indentLevel)
int addNode(const btVector3 &node)
void addNode(double x, double y, double z, std::string tags="")