tgNode.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_NODE_H
#define TG_NODE_H

#include "tgUtil.h"
#include "core/tgTaggable.h"

//Bullet Physics
#include "LinearMath/btVector3.h"
#include "LinearMath/btQuaternion.h"

// NOTE: Should tgNode hold a quaternion to specify rotation/orientation? 


class tgNode : public btVector3, public tgTaggable
{
public:

    tgNode(const btVector3& v = btVector3(),
             const std::string& tags = "") :
        btVector3(v), tgTaggable(tags)  
    {}
        
    tgNode(double x, double y, double z, 
             const std::string& tags = "") : 
        btVector3(x,y,z), tgTaggable(tags)
    {}

    void addRotation(const btVector3& fixedPoint, 
                     const btVector3& fromOrientation,
                     const btVector3& toOrientation)
    {
        // Note: there's likely a more efficient way to do this...
        addRotation(fixedPoint, 
                    tgUtil::getQuaternionBetween(fromOrientation, 
                                                 toOrientation));
    }

    void addRotation(const btVector3& fixedPoint, 
                     const btVector3& axis,
                     double angle)
    {
        tgUtil::addRotation(*this, fixedPoint, axis, angle);       
    }

    
    void addRotation(const btVector3& fixedPoint, 
                     const btQuaternion& rotation)
    {
        addRotation(fixedPoint, 
                    rotation.getAxis(), 
                    rotation.getAngle());
    }    

    // Note: Required for extending tgTaggable
    bool operator==(const tgNode& other) const
    {
      return (this->x() == other.x()) &&
             (this->y() == other.y()) &&
             (this->z() == other.z());
    }

};



inline std::ostream&
operator<<(std::ostream& os, const tgNode& node) 
{
    os << "tgNode(" << node.x() << ", " << node.y() << ", " << node.z() << ", {" << node.getTagStr(", ") << "})";
    return os;
};


inline std::string asYamlItem(const tgNode& node, int indentLevel=0)
{
    std::stringstream os;
    std::string indent = std::string(2 * (indentLevel), ' ');
    os << indent << "- tags: " << asYamlList(node.getTags()) << std::endl;
    os << indent << "  xyz: [" << node.x() << ", " << node.y() << ", " << node.z() << "]" << std::endl;
    return os.str();
};


#endif