tgUtil.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_UTIL_H
#define TG_UTIL_H

#include "btBulletDynamicsCommon.h" // stream operations for collision shapes
#include "LinearMath/btQuaternion.h"
#include "LinearMath/btTransform.h"
#include "LinearMath/btVector3.h"
#include <cmath>
#include <iostream>
#include <sstream>
#include <string>

#include "tgRigidInfo.h"

class tgUtil 
{
public:

    inline static btVector3 upVector() 
    {
        return btVector3(0.0, 1.0, 0.0);
    };
    
    inline static std::string degSymbol() 
    {
        return "\u00B0";
    }

    inline static btVector3 center(const btVector3& start,
                       const btVector3& end)
    {
        return (start + end) / 2.0;
    }

    inline static btTransform getTransform(const btVector3& startOrientation,
                           const btVector3& start,
                           const btVector3& end)
    {

        const btVector3 origin = center(start, end);
        btTransform t = btTransform();
        t.setIdentity();
        t.setOrigin(origin);
        // If they for some reason gave us the same vector, keep identity
        
                                t.setRotation(getQuaternionBetween(startOrientation,
                                                                                   getVector(start, end)));

        return t;
    }
    
    inline static btTransform getTransform(const btVector3& start,
                           const btVector3& end)
    {
        return getTransform(upVector(), start, end);
    }

                inline static btTransform getTransform(const btVector3& center)
                {
                                btTransform t = btTransform();
        t.setIdentity();
        t.setOrigin(center);
                                // No point in rotating, keep identity
        return t;
                }

    static btVector3 getVector(const btVector3& from,
                   const btVector3& to)
    {
        return to - from;
    }

    static btVector3 getRadiusVector(btVector3 axis,
                     double radius,
                     btVector3 target) 
    {
        // NOTE: axis and target are passed by value,
            // so we can alter them.
        axis.normalize();
        target.normalize();
        // Get a vector normal to both
        const btVector3 norm = axis.cross(target);
        // Rotate the axis by 90 degrees around the normal vector
        axis.rotate(norm, (M_PI / 2.0)); 
        return axis;
    }

    static bool almostEqual(const btVector3& from,
                const btVector3& to, 
                int precision = 5)
    {
        return from.distance(to) < pow(10.0, (precision < 0 ? 5 : -precision));
    }

    static btQuaternion getQuaternionBetween(btVector3 a, btVector3 b) 
    {
        a.normalize();
        b.normalize();

        // The return value
        btQuaternion result;
                                
        // Account for equal vectors (can't calculate c in this case)
        if (almostEqual(a, b)) {
            result = btQuaternion::getIdentity();
        } else if (almostEqual(a, -b)) {
            // Account for opposing vectors (can't calculate c in
            // this case either)
            btVector3 xAxis(1.0, 0.0, 0.0);
            if (a.dot(xAxis) == 0.0)
            {
                                                                // Gets around bad btTransforms with an up vector
                                                                result = btQuaternion (-1.0, 0.0, 0.0, 0.0);
                                                }
                                                else
                                                {               
                                                                const btVector3 arb =
                                                                a + getArbitraryNonParallelVector(a);
                                                                const btVector3 c = (a.cross(arb)).normalize();
                                                                result = btQuaternion(c, M_PI).normalize();
                                                }
        } else {
            // Create a vector normal to both a and b
            const btVector3 c = (a.cross(b)).normalize();

            // Create a quaternion that represents a rotation about
            // c by the angle between a and b
            result = btQuaternion(c, acos(a.dot(b))).normalize();
        }
        return result;
    }

    inline static btVector3 getArbitraryNonParallelVector(btVector3 v)
    {
        btVector3 arb;
        v.normalize();
        do {
            arb = btVector3(rand()%10, rand()%10, rand()%10).normalize();
        } while (arb == v || arb == -v);
        return arb;
    }

    inline static void addRotation(btVector3& v, 
                                   const btVector3& fixedPoint, 
                                   const btVector3& axis, 
                                   double angle)
    {
        // Get a vector from fixedPoint to this
        btVector3 relative = v - fixedPoint;

        // Rotate the relative vector
        btVector3 rotated = relative.rotate(axis, angle);

        // Set our new values
        v.setX(fixedPoint.x() + rotated.x());
        v.setY(fixedPoint.y() + rotated.y());
        v.setZ(fixedPoint.z() + rotated.z());
    }

    inline static void addRotation(btVector3& v, 
                                   const btVector3& fixedPoint, 
                                   const btVector3& fromOrientation, 
                                   const btVector3& toOrientation)
    {
        btQuaternion rotation = getQuaternionBetween(fromOrientation, toOrientation);
        addRotation(v, fixedPoint, rotation);
    }
    
    inline static void addRotation(btVector3& v, 
                                   const btVector3& fixedPoint, 
                                   const btQuaternion& rotation)
    {
        addRotation(v, fixedPoint, rotation.getAxis(), rotation.getAngle());
    }
                
    inline static double rad2deg(double radians)
    {
        return radians * 180.0 / M_PI;
    }

    inline static double deg2rad(double degrees)
    {
        return degrees * (M_PI / 180.0);
    }

    inline static std::string strDeg(double degrees) {
            std::ostringstream s;
            s << degrees << degSymbol();
            return s.str();
    }
    //static std::string strDeg(double degrees);

    inline static double round(double d, int precision = 5)
    {
        const double base = 10.0;
        const int m = static_cast<int>(pow(base, precision));
        return floor(d * m + 0.5)/m;
    }

};

inline std::ostream&
operator<<(std::ostream& os, const btQuaternion& q)
{
        os << "btQuaternion( " 
       << q.x() << ", " << q.y() << ", " << q.z() << ", " << q.w()
       << " )";
    return os;
}

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

inline std::ostream&
operator<<(std::ostream& os, const btTransform& xf)
{
        os << "btTransform: origin = " << xf.getOrigin()
       << "; rotation = " << xf.getRotation();
    return os;
}

inline std::ostream&
operator<<(std::ostream& os, const btRigidBody& rb)
{
    os << "btRigidBody: " << rb.getCollisionShape();
    return os;
}

inline std::ostream&
operator<<(std::ostream& os, const btCollisionShape& cs)
{
    os << "btCollisionShape: " << cs.getShapeType();
    return os;
}

inline std::ostream&
operator<<(std::ostream& os, const btCompoundShape& cs)
{
        os << "btCompoundShape: " << cs.getShapeType() << std::endl
       << "  # Children: " << cs.getNumChildShapes() << std::endl;
    return os;
}



#endif  // TG_UTIL_H