tgNodes.h

Go to the documentation of this file.

/*
 * Copyright © 2012, United States Government, as represented by the
 * Administrator of the National Aeronautics and Space Administration.
 * All rights reserved.
 * 
 * The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is licensed
 * under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 * http://www.apache.org/licenses/LICENSE-2.0.
 * 
 * Unless required by applicable law or agreed to in writing,
 * software distributed under the License is distributed on an
 * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,
 * either express or implied. See the License for the specific language
 * governing permissions and limitations under the License.
*/

#ifndef TG_NODES_H
#define TG_NODES_H

#include "LinearMath/btVector3.h"
#include <iostream>
#include <vector>
#include <map>
#include <set>
#include <stdexcept>
#include <sstream>

#include "tgNode.h"
#include "core/tgTaggables.h"

class tgPair;
 
class tgNodes : public tgTaggables<tgNode> {
public:
    
    tgNodes() : tgTaggables()
    {
    }

    tgNodes(std::vector<btVector3>& nodes) : tgTaggables()
    {
        // All elements must be unique
        assertUniqueElements("All nodes must be unique.");

        // @todo: There has to be a better way to do this (maybe initializer lists with upcasting btVector3 => tgNode?) 
        for(std::size_t i = 0; i < nodes.size(); i++) {
            addElement(tgNode(nodes[i]));
        }
        
    }
     
    tgNodes(std::vector<tgNode>& nodes) : tgTaggables(nodes) {
        // All elements must be unique
        assertUniqueElements("All nodes must be unique.");
        
    }
        
    ~tgNodes() { }
    
    void setNode(int key, const btVector3& node)
    {
        setNode(key, tgNode(node));
    };

    void setNode(int key, const tgNode& node)
    {
        setElement(key, node);
    }
    
    std::vector<tgNode>& getNodes() 
    {
        return getElements();
    };

    const std::vector<tgNode>& getNodes() const
    {
        return getElements();
    };


    bool nodeExists(int key) const
    {
        return keyExists(key);
    }

    int addNode(const btVector3& node) {
        return addNode(tgNode(node));
    };

    int addNode(const btVector3& node, std::string tags) {
        return addNode(tgNode(node, tags));
    };
    
    int addNode(const tgNode& node) {
        return addElement(node);
    }

     // @todo: Maybe replace this with something like:
     //     myNodes += tgNode(1,2,3, "some tags");
     //     or just use myNodes.addNode(tgNode(1,2,3, "tags here"));
    int addNode(double x, double y, double z)
    {
        const btVector3 node(x, y, z);
        return addNode(node);
    }

    int addNode(double x, double y, double z, std::string tags)
    {
        const tgNode node(x, y, z, tags);
        return addNode(node);
    }

    tgPair pair(int from, int to, std::string tags = "");

    void move(const btVector3& offset)
    {
        std::vector<tgNode>& nodes = getElements();
        for(std::size_t i = 0; i < nodes.size(); i++) {
            nodes[i] += offset;
        }
    }

    void moveNode(int idx, const btVector3 offset)
    {
        (*this)[idx] += offset;
    }
    
    void addRotation(const btVector3& fixedPoint,
                     const btVector3& axis,
                     double angle)
    {
        btQuaternion rotation(axis, angle);
        addRotation(fixedPoint, rotation);
    }

    void addRotation(const btVector3& fixedPoint,
                     const btVector3& fromOrientation,
                     const btVector3& toOrientation)
    {
        btQuaternion rotation = tgUtil::getQuaternionBetween(fromOrientation, 
                                                             toOrientation);
        addRotation(fixedPoint, rotation);
    }

    void addRotation(const btVector3& fixedPoint,
                     const btQuaternion& rotation)
    {
        std::vector<tgNode>& nodes = getNodes();
        for(std::size_t i = 0; i < nodes.size(); i++) {
            nodes[i].addRotation(fixedPoint, rotation);
        }
    }

    /*
     * Scales nodes relative to a reference point
     * @param[in] referencePoint a btVector3 reference point to scale the nodes from/to
     * @param[in] scaleFactor the scale factor by which to scale the nodes
     */
    void scale(const btVector3& referencePoint, double scaleFactor) {
        std::vector<tgNode>& nodes = getNodes();
        for(int i = 0; i < nodes.size(); i++) {
            nodes[i].setX((nodes[i].x() - referencePoint.x()) * scaleFactor + referencePoint.x());
            nodes[i].setY((nodes[i].y() - referencePoint.y()) * scaleFactor + referencePoint.y());
            nodes[i].setZ((nodes[i].z() - referencePoint.z()) * scaleFactor + referencePoint.z());
        }
    }
    
protected:
    
    // A map of m_nodes keys to names. Note that not all m_nodes will have names.
    std::map<int, std::string> m_names;  // @todo: remove this...
    
    void assertNodeExists(int key) const
    {
        if(!keyExists(key)) {
            std::stringstream ss; 
            ss << key;
            throw std::out_of_range("Node at index " + ss.str() + " does not exist");
        }        
    }
    
    void assertUniqueNodes() const
    {
        assertUniqueElements("Nodes muse be unique.");
    }
    
};


inline std::ostream&
operator<<(std::ostream& os, const tgNodes& n)
{
    os << "tgNodes(" << std::endl;
    const std::vector<tgNode>& nodes = n.getNodes();
    for(std::size_t i = 0; i < nodes.size(); i++) {
        os << "  " << nodes[i] << std::endl;
    }
    os << ")";

    return os;
};


inline std::string asYamlItems(const tgNodes& nodes, int indentLevel=0)
{
    std::stringstream os;
    std::string indent = std::string(2 * (indentLevel), ' ');

    if (nodes.size() == 0) {
        os << indent << "nodes: []" << std::endl;
        return os.str();
    }

    os << indent << "nodes:" << std::endl;
    for(size_t i = 0; i < nodes.size(); i++)
    {
        os << asYamlItem(nodes[i], indentLevel+1);
    }
    return os.str();
};



#endif