<commit_before>/*
 * Copyright (C) 2015 Fondazione Istituto Italiano di Tecnologia
 * Authors: Silvio Traversaro
 * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
 *
 */

#include <iDynTree/Core/AngularMotionVector3.h>
#include <iDynTree/Core/ClassicalAcc.h>
#include <iDynTree/Core/Rotation.h>
#include <iDynTree/Core/Axis.h>
#include <iDynTree/Core/Direction.h>
#include <iDynTree/Core/Position.h>
#include <iDynTree/Core/Wrench.h>
#include <iDynTree/Core/RotationalInertiaRaw.h>
#include <iDynTree/Core/Twist.h>
#include <iDynTree/Core/SpatialAcc.h>
#include <iDynTree/Core/SpatialMomentum.h>
#include <iDynTree/Core/Utils.h>
#include <iDynTree/Core/PrivateSemanticsMacros.h>
#include <iDynTree/Core/EigenHelpers.h>

#include <Eigen/Dense>

#include <cassert>
#include <iostream>
#include <sstream>

namespace iDynTree
{
    /**
     * Local static functions
     */

    template <class SpatialMotionForceVectorT>
    static SpatialMotionForceVectorT changeCoordFrameOfT(const Rotation & rot,
                                                         const SpatialMotionForceVectorT & other)
    {
        return SpatialMotionForceVectorT(other.getLinearVec3().changeCoordFrame(rot),
                                         other.getAngularVec3().changeCoordFrame(rot));
    }

    /**
     * class Method definitions
     */

    typedef Eigen::Matrix<double,3,3,Eigen::RowMajor> Matrix3dRowMajor;

    Rotation::Rotation(): RotationRaw()
    {
    }

    Rotation::Rotation(double xx, double xy, double xz,
                       double yx, double yy, double yz,
                       double zx, double zy, double zz): RotationRaw(xx,xy,xz,
                                                                     yx,yy,yz,
                                                                     zx,zy,zz)
    {
    }

    Rotation::Rotation(const Rotation & other): RotationRaw(other)
    {
        iDynTreeSemanticsOp(this->semantics = other.getSemantics());
    }

    Rotation::Rotation(const RotationRaw& other): RotationRaw(other)
    {

    }

    Rotation::Rotation(const RotationRaw& otherPos, RotationSemantics & otherSem): RotationRaw(otherPos)
    {
        iDynTreeSemanticsOp(this->semantics = otherSem);
    }

    RotationSemantics& Rotation::getSemantics()
    {
        return this->semantics;
    }

    const RotationSemantics& Rotation::getSemantics() const
    {
        return this->semantics;
    }

    const Rotation& Rotation::changeOrientFrame(const Rotation& newOrientFrame)
    {
        iDynTreeAssert( this->semantics.changeOrientFrame(newOrientFrame.semantics) );
        this->RotationRaw::changeOrientFrame(newOrientFrame);
        return *this;
    }

    const Rotation& Rotation::changeRefOrientFrame(const Rotation& newRefOrientFrame)
    {
        iDynTreeAssert( this->semantics.changeRefOrientFrame(newRefOrientFrame.semantics) );
        this->RotationRaw::changeRefOrientFrame(newRefOrientFrame);
        return *this;
    }

    const Rotation& Rotation::changeCoordinateFrame(const Rotation& newCoordinateFrame)
    {
        return this->changeRefOrientFrame(newCoordinateFrame);
    }

    Rotation Rotation::compose(const Rotation& op1, const Rotation& op2)
    {
        RotationSemantics resultSemantics;
        iDynTreeAssert( RotationSemantics::compose(op1.semantics,op2.semantics,resultSemantics) );
        return Rotation(RotationRaw::compose(op1,op2),resultSemantics);
    }

    Rotation Rotation::inverse2(const Rotation& orient)
    {
        RotationSemantics resultSemantics;
        iDynTreeAssert( RotationSemantics::inverse2(orient.getSemantics(),resultSemantics) );
        return Rotation(RotationRaw::inverse2(orient),resultSemantics);
    }

    Position Rotation::changeCoordFrameOf(const Position & other) const
    {
        PositionSemantics resultSemantics;
        iDynTreeAssert( this->semantics.changeCoordFrameOf(other.getSemantics(), resultSemantics) );
        return Position(this->RotationRaw::changeCoordFrameOf(other), resultSemantics);
    }

    SpatialMotionVector Rotation::changeCoordFrameOf(const SpatialMotionVector& other) const
    {
        return changeCoordFrameOfT<SpatialMotionVector>(*this, other);
    }

    SpatialForceVector Rotation::changeCoordFrameOf(const SpatialForceVector& other) const
    {
        return changeCoordFrameOfT<SpatialForceVector>(*this, other);
    }

    Twist Rotation::changeCoordFrameOf(const Twist& other) const
    {
        return changeCoordFrameOfT<Twist>(*this, other);
    }

    SpatialAcc Rotation::changeCoordFrameOf(const SpatialAcc & other) const
    {
        return changeCoordFrameOfT<SpatialAcc>(*this, other);
    }

    SpatialMomentum Rotation::changeCoordFrameOf(const SpatialMomentum & other) const
    {
        return changeCoordFrameOfT<SpatialMomentum>(*this, other);
    }

    Wrench Rotation::changeCoordFrameOf(const Wrench &other) const
    {
        return changeCoordFrameOfT<Wrench>(*this, other);
    }

    Direction Rotation::changeCoordFrameOf(const Direction& other) const
    {
        Direction result;

        // \todo TODO add semantics to Direction
        Eigen::Map<const Matrix3dRowMajor> newCoordFrame(m_data);
        Eigen::Map<const Eigen::Vector3d> directionCoord(other.data());
        Eigen::Map<Eigen::Vector3d> resultData(result.data());

        resultData = newCoordFrame*directionCoord;

        return result;
    }

    ClassicalAcc Rotation::changeCoordFrameOf(const ClassicalAcc &other) const
    {
        ClassicalAcc result;

        // \todo TODO add semantics to ClassicalAcc
        result = RotationRaw::changeCoordFrameOf(other);

        return result;
    }

    RotationalInertiaRaw Rotation::changeCoordFrameOf(const RotationalInertiaRaw &other) const
    {
        RotationalInertiaRaw result;

        // \todo TODO add semantics to ClassicalAcc
        result = RotationRaw::changeCoordFrameOf(other);

        return result;
    }

    Axis Rotation::changeCoordFrameOf(const Axis& other) const
    {
        return Axis(this->changeCoordFrameOf(other.getDirection()),this->changeCoordFrameOf(other.getOrigin()));
    }

    Rotation Rotation::inverse() const
    {
        return inverse2(*this);
    }

    Rotation Rotation::operator*(const Rotation& other) const
    {
        return compose(*this,other);
    }

    Position Rotation::operator*(const Position& other) const
    {
        return changeCoordFrameOf(other);
    }

    SpatialForceVector Rotation::operator*(const SpatialForceVector& other) const
    {
        return changeCoordFrameOfT<SpatialForceVector>(*this, other);
    }

    Twist Rotation::operator*(const Twist& other) const
    {
        return changeCoordFrameOfT<Twist>(*this, other);
    }

    SpatialAcc Rotation::operator*(const SpatialAcc & other) const
    {
        return changeCoordFrameOfT<SpatialAcc>(*this, other);
    }

    SpatialMomentum Rotation::operator*(const SpatialMomentum & other) const
    {
        return changeCoordFrameOfT<SpatialMomentum>(*this, other);
    }

    Wrench Rotation::operator*(const Wrench& other) const
    {
        return changeCoordFrameOfT<Wrench>(*this, other);
    }

    Direction Rotation::operator*(const Direction& other) const
    {
        return changeCoordFrameOf(other);
    }

    ClassicalAcc Rotation::operator*(const ClassicalAcc& other) const
    {
        return changeCoordFrameOf(other);
    }

    Axis Rotation::operator*(const Axis& other) const
    {
        return changeCoordFrameOf(other);
    }

    RotationalInertiaRaw Rotation::operator*(const RotationalInertiaRaw& other) const
    {
        return changeCoordFrameOf(other);
    }

    void Rotation::getRPY(double& r, double& p, double& y)
    {
        Eigen::Map<const Matrix3dRowMajor> R(m_data);

        if (R(2,0)<1.0)
        {
            if (R(2,0)>-1.0)
            {
                r=atan2(R(2,1),R(2,2));
                p=asin(-R(2,0));
                y=atan2(R(1,0),R(0,0));
            }
            else
            {
                // Not a unique solution
                r=0.0;
                p=M_PI/2.0;
                y=-atan2(-R(1,2),R(1,1));
            }
        }
        else
        {
            // Not a unique solution
            r=0.0;
            p=-M_PI/2.0;
            y=atan2(-R(1,2),R(1,1));
        }
    }

    bool Rotation::getQuaternion(iDynTree::Vector4& quaternion) const
    {
        return getQuaternion(quaternion(0), quaternion(1), quaternion(2), quaternion(3));
    }

    bool Rotation::getQuaternion(double &s, double &r1, double &r2, double &r3) const
    {
        Eigen::Map<const Matrix3dRowMajor> R(m_data);

        //Taken from "Contributions au contrôle automatique de véhicules aériens"
        //PhD thesis of "Minh Duc HUA"
        //INRIA Sophia Antipolis
        //Equation 3.9 (page 101)

        double q0 = ( R(0,0) + R(1,1) + R(2,2) + 1.0) / 4.0;
        double q1 = ( R(0,0) - R(1,1) - R(2,2) + 1.0) / 4.0;
        double q2 = (-R(0,0) + R(1,1) - R(2,2) + 1.0) / 4.0;
        double q3 = (-R(0,0) - R(1,1) + R(2,2) + 1.0) / 4.0;

        if (q0 < 0.0) q0 = 0.0;
        if (q1 < 0.0) q1 = 0.0;
        if (q2 < 0.0) q2 = 0.0;
        if (q3 < 0.0) q3 = 0.0;

        q0 = std::sqrt(q0);
        q1 = std::sqrt(q1);
        q2 = std::sqrt(q2);
        q3 = std::sqrt(q3);

        if (q0 >= q1 && q0 >= q2 && q0 >= q3) {
            q0 *= 1.0;
            q1 *= (R(2,1) - R(1,2)) >= 0 ? 1 : -1;
            q2 *= (R(0,2) - R(2,0)) >= 0 ? 1 : -1;
            q3 *= (R(1,0) - R(0,1)) >= 0 ? 1 : -1;
        } else if (q1 >= q0 && q1 >= q2 && q1 >= q3) {
            q0 *= (R(2,1) - R(1,2)) >= 0 ? 1 : -1;
            q1 *= 1.0;
            q2 *= (R(1,0) + R(0,1)) >= 0 ? 1 : -1;
            q3 *= (R(2,0) + R(0,2)) >= 0 ? 1 : -1;
        } else if (q2 >= q0 && q2 >= q1 && q2 >= q3) {
            q0 *= (R(0,2) - R(2,0)) >= 0 ? 1 : -1;
            q1 *= (R(1,0) + R(0,1)) >= 0 ? 1 : -1;
            q2 *= 1.0;
            q3 *= (R(1,2) + R(2,1)) >= 0 ? 1 : -1;
        } else if (q3 >= q0 && q3 >= q1 && q3 >= q2) {
            q0 *= (R(1,0) - R(0,1)) >= 0 ? 1 : -1;
            q1 *= (R(2,0) + R(0,2)) >= 0 ? 1 : -1;
            q2 *= (R(1,2) + R(2,1)) >= 0 ? 1 : -1;
            q3 *= 1.0;
        } else {
            reportError("Rotation", "getQuaternion", "Quaternion numerically bad conditioned");
            return false;
        }

        double quaternionNorm = std::sqrt(q0 * q0 +
                                          q1 * q1 +
                                          q2 * q2 +
                                          q3 * q3);
        s  = q0 / quaternionNorm;
        r1 = q1 / quaternionNorm;
        r2 = q2 / quaternionNorm;
        r3 = q3 / quaternionNorm;
        return true;
    }

    iDynTree::Vector4 Rotation::asQuaternion() const
    {
        iDynTree::Vector4 quaternion;
        quaternion.zero();
        getQuaternion(quaternion);
        return quaternion;
    }

    AngularMotionVector3 Rotation::log() const
    {
        AngularMotionVector3 ret;

        Eigen::AngleAxisd aa(Eigen::Map<const Matrix3dRowMajor>(this->data()));

        // Implementation inspired from DART, see
        // https://github.com/dartsim/dart/pull/407/files
        // https://github.com/dartsim/dart/pull/334
        // https://github.com/dartsim/dart/issues/88
        Eigen::Map<Eigen::Vector3d>(ret.data()) = aa.angle()*aa.axis();

        return ret;
    }

    Rotation Rotation::RotX(const double angle)
    {
        return Rotation(RotationRaw::RotX(angle));
    }

    Rotation Rotation::RotY(const double angle)
    {
        return Rotation(RotationRaw::RotY(angle));
    }

    Rotation Rotation::RotZ(const double angle)
    {
        return Rotation(RotationRaw::RotZ(angle));
    }

    Rotation Rotation::RotAxis(const Direction & direction, const double angle)
    {
        Rotation result;
        Eigen::Map<Matrix3dRowMajor> thisData(result.data());
        Eigen::Map<const Eigen::Vector3d>   directionData(direction.data());
        thisData = Eigen::AngleAxisd(angle, directionData).matrix();

        return result;
    }

    Matrix3x3 Rotation::RotAxisDerivative(const Direction& direction, const double angle)
    {
        Matrix3x3 result;

        Eigen::Map<Matrix3dRowMajor> res(result.data());
        Eigen::Map<const Eigen::Vector3d> d(direction.data());
        Matrix3dRowMajor skewd = skew(d);

        res = skewd*cos(angle)+skewd*skewd*sin(angle);

        return result;
    }

    Rotation Rotation::RPY(const double roll, const double pitch, const double yaw)
    {
        return Rotation(RotationRaw::RPY(roll, pitch, yaw));
    }

    Rotation Rotation::Identity()
    {
        return RotationRaw::Identity();
    }

    Rotation Rotation::RotationFromQuaternion(const iDynTree::Vector4& _quaternion)
    {
        //Taken from "Contributions au contrôle automatique de véhicules aériens"
        //PhD thesis of "Minh Duc HUA"
        //INRIA Sophia Antipolis
        //Equation 3.8 (page 101)

        // Rodriques's formula
        // R = I3 + 2s S(r) + 2S(r)^2,
        Rotation _rotation;
        Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > rotation = toEigen(_rotation);
        rotation.setIdentity();

        Eigen::Map<const Eigen::Vector4d> quaternion(_quaternion.data());
        Eigen::Matrix3d skewSymmetricMatrix = iDynTree::skew(quaternion.tail<3>());

        rotation += 2 * quaternion(0) * skewSymmetricMatrix + 2 * skewSymmetricMatrix * skewSymmetricMatrix;

        return _rotation;
    }

    std::string Rotation::toString() const
    {
        std::stringstream ss;

        ss << RotationRaw::toString();
        iDynTreeSemanticsOp(ss << " " << semantics.toString());

        return ss.str();
    }

    std::string Rotation::reservedToString() const
    {
        return this->toString();
    }

}
<commit_msg>Updated way to compute quaternion<commit_after>/*
 * Copyright (C) 2015 Fondazione Istituto Italiano di Tecnologia
 * Authors: Silvio Traversaro
 * CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
 *
 */

#include <iDynTree/Core/AngularMotionVector3.h>
#include <iDynTree/Core/ClassicalAcc.h>
#include <iDynTree/Core/Rotation.h>
#include <iDynTree/Core/Axis.h>
#include <iDynTree/Core/Direction.h>
#include <iDynTree/Core/Position.h>
#include <iDynTree/Core/Wrench.h>
#include <iDynTree/Core/RotationalInertiaRaw.h>
#include <iDynTree/Core/Twist.h>
#include <iDynTree/Core/SpatialAcc.h>
#include <iDynTree/Core/SpatialMomentum.h>
#include <iDynTree/Core/Utils.h>
#include <iDynTree/Core/PrivateSemanticsMacros.h>
#include <iDynTree/Core/EigenHelpers.h>

#include <Eigen/Dense>

#include <cassert>
#include <iostream>
#include <sstream>

namespace iDynTree
{
    /**
     * Local static functions
     */

    template <class SpatialMotionForceVectorT>
    static SpatialMotionForceVectorT changeCoordFrameOfT(const Rotation & rot,
                                                         const SpatialMotionForceVectorT & other)
    {
        return SpatialMotionForceVectorT(other.getLinearVec3().changeCoordFrame(rot),
                                         other.getAngularVec3().changeCoordFrame(rot));
    }

    /**
     * class Method definitions
     */

    typedef Eigen::Matrix<double,3,3,Eigen::RowMajor> Matrix3dRowMajor;

    Rotation::Rotation(): RotationRaw()
    {
    }

    Rotation::Rotation(double xx, double xy, double xz,
                       double yx, double yy, double yz,
                       double zx, double zy, double zz): RotationRaw(xx,xy,xz,
                                                                     yx,yy,yz,
                                                                     zx,zy,zz)
    {
    }

    Rotation::Rotation(const Rotation & other): RotationRaw(other)
    {
        iDynTreeSemanticsOp(this->semantics = other.getSemantics());
    }

    Rotation::Rotation(const RotationRaw& other): RotationRaw(other)
    {

    }

    Rotation::Rotation(const RotationRaw& otherPos, RotationSemantics & otherSem): RotationRaw(otherPos)
    {
        iDynTreeSemanticsOp(this->semantics = otherSem);
    }

    RotationSemantics& Rotation::getSemantics()
    {
        return this->semantics;
    }

    const RotationSemantics& Rotation::getSemantics() const
    {
        return this->semantics;
    }

    const Rotation& Rotation::changeOrientFrame(const Rotation& newOrientFrame)
    {
        iDynTreeAssert( this->semantics.changeOrientFrame(newOrientFrame.semantics) );
        this->RotationRaw::changeOrientFrame(newOrientFrame);
        return *this;
    }

    const Rotation& Rotation::changeRefOrientFrame(const Rotation& newRefOrientFrame)
    {
        iDynTreeAssert( this->semantics.changeRefOrientFrame(newRefOrientFrame.semantics) );
        this->RotationRaw::changeRefOrientFrame(newRefOrientFrame);
        return *this;
    }

    const Rotation& Rotation::changeCoordinateFrame(const Rotation& newCoordinateFrame)
    {
        return this->changeRefOrientFrame(newCoordinateFrame);
    }

    Rotation Rotation::compose(const Rotation& op1, const Rotation& op2)
    {
        RotationSemantics resultSemantics;
        iDynTreeAssert( RotationSemantics::compose(op1.semantics,op2.semantics,resultSemantics) );
        return Rotation(RotationRaw::compose(op1,op2),resultSemantics);
    }

    Rotation Rotation::inverse2(const Rotation& orient)
    {
        RotationSemantics resultSemantics;
        iDynTreeAssert( RotationSemantics::inverse2(orient.getSemantics(),resultSemantics) );
        return Rotation(RotationRaw::inverse2(orient),resultSemantics);
    }

    Position Rotation::changeCoordFrameOf(const Position & other) const
    {
        PositionSemantics resultSemantics;
        iDynTreeAssert( this->semantics.changeCoordFrameOf(other.getSemantics(), resultSemantics) );
        return Position(this->RotationRaw::changeCoordFrameOf(other), resultSemantics);
    }

    SpatialMotionVector Rotation::changeCoordFrameOf(const SpatialMotionVector& other) const
    {
        return changeCoordFrameOfT<SpatialMotionVector>(*this, other);
    }

    SpatialForceVector Rotation::changeCoordFrameOf(const SpatialForceVector& other) const
    {
        return changeCoordFrameOfT<SpatialForceVector>(*this, other);
    }

    Twist Rotation::changeCoordFrameOf(const Twist& other) const
    {
        return changeCoordFrameOfT<Twist>(*this, other);
    }

    SpatialAcc Rotation::changeCoordFrameOf(const SpatialAcc & other) const
    {
        return changeCoordFrameOfT<SpatialAcc>(*this, other);
    }

    SpatialMomentum Rotation::changeCoordFrameOf(const SpatialMomentum & other) const
    {
        return changeCoordFrameOfT<SpatialMomentum>(*this, other);
    }

    Wrench Rotation::changeCoordFrameOf(const Wrench &other) const
    {
        return changeCoordFrameOfT<Wrench>(*this, other);
    }

    Direction Rotation::changeCoordFrameOf(const Direction& other) const
    {
        Direction result;

        // \todo TODO add semantics to Direction
        Eigen::Map<const Matrix3dRowMajor> newCoordFrame(m_data);
        Eigen::Map<const Eigen::Vector3d> directionCoord(other.data());
        Eigen::Map<Eigen::Vector3d> resultData(result.data());

        resultData = newCoordFrame*directionCoord;

        return result;
    }

    ClassicalAcc Rotation::changeCoordFrameOf(const ClassicalAcc &other) const
    {
        ClassicalAcc result;

        // \todo TODO add semantics to ClassicalAcc
        result = RotationRaw::changeCoordFrameOf(other);

        return result;
    }

    RotationalInertiaRaw Rotation::changeCoordFrameOf(const RotationalInertiaRaw &other) const
    {
        RotationalInertiaRaw result;

        // \todo TODO add semantics to ClassicalAcc
        result = RotationRaw::changeCoordFrameOf(other);

        return result;
    }

    Axis Rotation::changeCoordFrameOf(const Axis& other) const
    {
        return Axis(this->changeCoordFrameOf(other.getDirection()),this->changeCoordFrameOf(other.getOrigin()));
    }

    Rotation Rotation::inverse() const
    {
        return inverse2(*this);
    }

    Rotation Rotation::operator*(const Rotation& other) const
    {
        return compose(*this,other);
    }

    Position Rotation::operator*(const Position& other) const
    {
        return changeCoordFrameOf(other);
    }

    SpatialForceVector Rotation::operator*(const SpatialForceVector& other) const
    {
        return changeCoordFrameOfT<SpatialForceVector>(*this, other);
    }

    Twist Rotation::operator*(const Twist& other) const
    {
        return changeCoordFrameOfT<Twist>(*this, other);
    }

    SpatialAcc Rotation::operator*(const SpatialAcc & other) const
    {
        return changeCoordFrameOfT<SpatialAcc>(*this, other);
    }

    SpatialMomentum Rotation::operator*(const SpatialMomentum & other) const
    {
        return changeCoordFrameOfT<SpatialMomentum>(*this, other);
    }

    Wrench Rotation::operator*(const Wrench& other) const
    {
        return changeCoordFrameOfT<Wrench>(*this, other);
    }

    Direction Rotation::operator*(const Direction& other) const
    {
        return changeCoordFrameOf(other);
    }

    ClassicalAcc Rotation::operator*(const ClassicalAcc& other) const
    {
        return changeCoordFrameOf(other);
    }

    Axis Rotation::operator*(const Axis& other) const
    {
        return changeCoordFrameOf(other);
    }

    RotationalInertiaRaw Rotation::operator*(const RotationalInertiaRaw& other) const
    {
        return changeCoordFrameOf(other);
    }

    void Rotation::getRPY(double& r, double& p, double& y)
    {
        Eigen::Map<const Matrix3dRowMajor> R(m_data);

        if (R(2,0)<1.0)
        {
            if (R(2,0)>-1.0)
            {
                r=atan2(R(2,1),R(2,2));
                p=asin(-R(2,0));
                y=atan2(R(1,0),R(0,0));
            }
            else
            {
                // Not a unique solution
                r=0.0;
                p=M_PI/2.0;
                y=-atan2(-R(1,2),R(1,1));
            }
        }
        else
        {
            // Not a unique solution
            r=0.0;
            p=-M_PI/2.0;
            y=atan2(-R(1,2),R(1,1));
        }
    }

    bool Rotation::getQuaternion(iDynTree::Vector4& quaternion) const
    {
        return getQuaternion(quaternion(0), quaternion(1), quaternion(2), quaternion(3));
    }

    bool Rotation::getQuaternion(double &s, double &r1, double &r2, double &r3) const
    {
        Eigen::Map<const Matrix3dRowMajor> R(m_data);

        //Taken from "Contributions au contrôle automatique de véhicules aériens"
        //PhD thesis of "Minh Duc HUA"
        //INRIA Sophia Antipolis
        //Equation 3.9 (page 101)

        //Diagonal elements used only to find the maximum
        //the furthest value from zero
        //we use this value as denominator to find the other elements
        double q0 = ( R(0,0) + R(1,1) + R(2,2) + 1.0);
        double q1 = ( R(0,0) - R(1,1) - R(2,2) + 1.0);
        double q2 = (-R(0,0) + R(1,1) - R(2,2) + 1.0);
        double q3 = (-R(0,0) - R(1,1) + R(2,2) + 1.0);

        if (q0 < 0.0) q0 = 0.0;
        if (q1 < 0.0) q1 = 0.0;
        if (q2 < 0.0) q2 = 0.0;
        if (q3 < 0.0) q3 = 0.0;

        if (q0 >= q1 && q0 >= q2 && q0 >= q3) {
            q0 = std::sqrt(q0);
            q1 = (R(2,1) - R(1,2)) / (2.0 * q0);
            q2 = (R(0,2) - R(2,0)) / (2.0 * q0);
            q3 = (R(1,0) - R(0,1)) / (2.0 * q0);
            q0 /= 2.0;
        } else if (q1 >= q0 && q1 >= q2 && q1 >= q3) {
            q1 = std::sqrt(q1);
            q0 = (R(2,1) - R(1,2)) / (2.0 * q1);
            q2 = (R(1,0) + R(0,1)) / (2.0 * q1);
            q3 = (R(2,0) + R(0,2)) / (2.0 * q1);
            q1 /= 2.0;
        } else if (q2 >= q0 && q2 >= q1 && q2 >= q3) {
            q2 = std::sqrt(q2);
            q0 = (R(0,2) - R(2,0)) / (2.0 * q2);
            q1 = (R(1,0) + R(0,1)) / (2.0 * q2);
            q3 = (R(1,2) + R(2,1)) / (2.0 * q2);
            q2 /= 2.0;
        } else if (q3 >= q0 && q3 >= q1 && q3 >= q2) {
            q3 = std::sqrt(q3);
            q0 = (R(1,0) - R(0,1)) / (2.0 * q3);
            q1 = (R(2,0) + R(0,2)) / (2.0 * q3);
            q2 = (R(1,2) + R(2,1)) / (2.0 * q3);
            q3 /= 2.0;
        } else {
            reportError("Rotation", "getQuaternion", "Quaternion numerically bad conditioned");
            return false;
        }

        //Here we impose that the leftmost nonzero element of the quaternion is positive
        double eps = 1e-7;
        double sign = 1.0;
        if (q0 > eps || q0 < -eps) {
            sign = q0 > 0 ? 1.0 : -1.0;
        } else if (q1 > eps || q1 < -eps) {
            sign = q1 > 0 ? 1.0 : -1.0;
        } else if (q2 > eps || q2 < -eps) {
            sign = q2 > 0 ? 1.0 : -1.0;
        } else if (q3 > eps || q3 < -eps) {
            sign = q3 > 0 ? 1.0 : -1.0;
        }

        q0 /= sign;
        q1 /= sign;
        q2 /= sign;
        q3 /= sign;

        double quaternionNorm = std::sqrt(q0 * q0 +
                                          q1 * q1 +
                                          q2 * q2 +
                                          q3 * q3);
        s  = q0 / quaternionNorm;
        r1 = q1 / quaternionNorm;
        r2 = q2 / quaternionNorm;
        r3 = q3 / quaternionNorm;
        return true;
    }

    iDynTree::Vector4 Rotation::asQuaternion() const
    {
        iDynTree::Vector4 quaternion;
        quaternion.zero();
        getQuaternion(quaternion);
        return quaternion;
    }

    AngularMotionVector3 Rotation::log() const
    {
        AngularMotionVector3 ret;

        Eigen::AngleAxisd aa(Eigen::Map<const Matrix3dRowMajor>(this->data()));

        // Implementation inspired from DART, see
        // https://github.com/dartsim/dart/pull/407/files
        // https://github.com/dartsim/dart/pull/334
        // https://github.com/dartsim/dart/issues/88
        Eigen::Map<Eigen::Vector3d>(ret.data()) = aa.angle()*aa.axis();

        return ret;
    }

    Rotation Rotation::RotX(const double angle)
    {
        return Rotation(RotationRaw::RotX(angle));
    }

    Rotation Rotation::RotY(const double angle)
    {
        return Rotation(RotationRaw::RotY(angle));
    }

    Rotation Rotation::RotZ(const double angle)
    {
        return Rotation(RotationRaw::RotZ(angle));
    }

    Rotation Rotation::RotAxis(const Direction & direction, const double angle)
    {
        Rotation result;
        Eigen::Map<Matrix3dRowMajor> thisData(result.data());
        Eigen::Map<const Eigen::Vector3d>   directionData(direction.data());
        thisData = Eigen::AngleAxisd(angle, directionData).matrix();

        return result;
    }

    Matrix3x3 Rotation::RotAxisDerivative(const Direction& direction, const double angle)
    {
        Matrix3x3 result;

        Eigen::Map<Matrix3dRowMajor> res(result.data());
        Eigen::Map<const Eigen::Vector3d> d(direction.data());
        Matrix3dRowMajor skewd = skew(d);

        res = skewd*cos(angle)+skewd*skewd*sin(angle);

        return result;
    }

    Rotation Rotation::RPY(const double roll, const double pitch, const double yaw)
    {
        return Rotation(RotationRaw::RPY(roll, pitch, yaw));
    }

    Rotation Rotation::Identity()
    {
        return RotationRaw::Identity();
    }

    Rotation Rotation::RotationFromQuaternion(const iDynTree::Vector4& _quaternion)
    {
        //Taken from "Contributions au contrôle automatique de véhicules aériens"
        //PhD thesis of "Minh Duc HUA"
        //INRIA Sophia Antipolis
        //Equation 3.8 (page 101)

        // Rodriques's formula
        // R = I3 + 2s S(r) + 2S(r)^2,
        Rotation _rotation;
        Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > rotation = toEigen(_rotation);
        rotation.setIdentity();

        Eigen::Map<const Eigen::Vector4d> quaternion(_quaternion.data());
        Eigen::Matrix3d skewSymmetricMatrix = iDynTree::skew(quaternion.tail<3>());

        rotation += 2 * quaternion(0) * skewSymmetricMatrix + 2 * skewSymmetricMatrix * skewSymmetricMatrix;

        return _rotation;
    }

    std::string Rotation::toString() const
    {
        std::stringstream ss;

        ss << RotationRaw::toString();
        iDynTreeSemanticsOp(ss << " " << semantics.toString());

        return ss.str();
    }

    std::string Rotation::reservedToString() const
    {
        return this->toString();
    }

}
<|endoftext|>


<commit_before>//
// Copyright (c) 2010 Advanced Micro Devices, Inc. All rights reserved.
//

#ifndef VERSIONS_HPP_
#define VERSIONS_HPP_

#include "utils/macros.hpp"

#ifndef AMD_PLATFORM_NAME
# define AMD_PLATFORM_NAME "AMD Accelerated Parallel Processing"
#endif // AMD_PLATFORM_NAME

#ifndef AMD_PLATFORM_BUILD_NUMBER
# define AMD_PLATFORM_BUILD_NUMBER 1693
#endif // AMD_PLATFORM_BUILD_NUMBER

#ifndef AMD_PLATFORM_REVISION_NUMBER
# define AMD_PLATFORM_REVISION_NUMBER 0
#endif // AMD_PLATFORM_REVISION_NUMBER

#ifndef AMD_PLATFORM_RELEASE_INFO
# define AMD_PLATFORM_RELEASE_INFO
#endif // AMD_PLATFORM_RELEASE_INFO

#define AMD_BUILD_STRING XSTR(AMD_PLATFORM_BUILD_NUMBER) \
    "." XSTR(AMD_PLATFORM_REVISION_NUMBER)

#ifndef AMD_PLATFORM_INFO
# define AMD_PLATFORM_INFO "AMD-APP" AMD_PLATFORM_RELEASE_INFO \
    DEBUG_ONLY("." IF(IS_OPTIMIZED,"opt","dbg")) " (" AMD_BUILD_STRING ")"
#endif // ATI_PLATFORM_INFO

#endif // VERSIONS_HPP_
<commit_msg>P4 to Git Change 1101372 by johtaylo@johtaylo-JTBUILDER03-increment on 2014/11/29 03:00:11<commit_after>//
// Copyright (c) 2010 Advanced Micro Devices, Inc. All rights reserved.
//

#ifndef VERSIONS_HPP_
#define VERSIONS_HPP_

#include "utils/macros.hpp"

#ifndef AMD_PLATFORM_NAME
# define AMD_PLATFORM_NAME "AMD Accelerated Parallel Processing"
#endif // AMD_PLATFORM_NAME

#ifndef AMD_PLATFORM_BUILD_NUMBER
# define AMD_PLATFORM_BUILD_NUMBER 1694
#endif // AMD_PLATFORM_BUILD_NUMBER

#ifndef AMD_PLATFORM_REVISION_NUMBER
# define AMD_PLATFORM_REVISION_NUMBER 0
#endif // AMD_PLATFORM_REVISION_NUMBER

#ifndef AMD_PLATFORM_RELEASE_INFO
# define AMD_PLATFORM_RELEASE_INFO
#endif // AMD_PLATFORM_RELEASE_INFO

#define AMD_BUILD_STRING XSTR(AMD_PLATFORM_BUILD_NUMBER) \
    "." XSTR(AMD_PLATFORM_REVISION_NUMBER)

#ifndef AMD_PLATFORM_INFO
# define AMD_PLATFORM_INFO "AMD-APP" AMD_PLATFORM_RELEASE_INFO \
    DEBUG_ONLY("." IF(IS_OPTIMIZED,"opt","dbg")) " (" AMD_BUILD_STRING ")"
#endif // ATI_PLATFORM_INFO

#endif // VERSIONS_HPP_
<|endoftext|>


<commit_before>// Copyright (c) 2011, the Dart project authors.  Please see the AUTHORS file
// for details. All rights reserved. Use of this source code is governed by a
// BSD-style license that can be found in the LICENSE file.

#include "vm/disassembler.h"

#include "vm/assembler.h"
#include "vm/deopt_instructions.h"
#include "vm/globals.h"
#include "vm/il_printer.h"
#include "vm/instructions.h"
#include "vm/json_stream.h"
#include "vm/log.h"
#include "vm/os.h"
#include "vm/code_patcher.h"

namespace dart {

#ifndef PRODUCT

DECLARE_FLAG(bool, trace_inlining_intervals);

void DisassembleToStdout::ConsumeInstruction(const Code& code,
                                             char* hex_buffer,
                                             intptr_t hex_size,
                                             char* human_buffer,
                                             intptr_t human_size,
                                             Object* object,
                                             uword pc) {
  static const int kHexColumnWidth = 23;
  uint8_t* pc_ptr = reinterpret_cast<uint8_t*>(pc);
  THR_Print("%p    %s", pc_ptr, hex_buffer);
  int hex_length = strlen(hex_buffer);
  if (hex_length < kHexColumnWidth) {
    for (int i = kHexColumnWidth - hex_length; i > 0; i--) {
      THR_Print(" ");
    }
  }
  THR_Print("%s", human_buffer);
  if (object != NULL) {
    THR_Print("   %s", object->ToCString());
  }
  THR_Print("\n");
}

void DisassembleToStdout::Print(const char* format, ...) {
  va_list args;
  va_start(args, format);
  THR_VPrint(format, args);
  va_end(args);
}

void DisassembleToJSONStream::ConsumeInstruction(const Code& code,
                                                 char* hex_buffer,
                                                 intptr_t hex_size,
                                                 char* human_buffer,
                                                 intptr_t human_size,
                                                 Object* object,
                                                 uword pc) {
  // Instructions are represented as four consecutive values in a JSON array.
  // The first is the address of the instruction, the second is the hex string,
  // of the code, and the third is a human readable string, and the fourth is
  // the object loaded by the instruction.
  jsarr_.AddValueF("%" Pp "", pc);
  jsarr_.AddValue(hex_buffer);
  jsarr_.AddValue(human_buffer);

  if (object != NULL) {
    jsarr_.AddValue(*object);
  } else {
    jsarr_.AddValueNull();  // Not a reference to null.
  }
}

void DisassembleToJSONStream::Print(const char* format, ...) {
  va_list args;
  va_start(args, format);
  intptr_t len = OS::VSNPrint(NULL, 0, format, args);
  va_end(args);
  char* p = reinterpret_cast<char*>(malloc(len+1));
  va_start(args, format);
  intptr_t len2 = OS::VSNPrint(p, len, format, args);
  va_end(args);
  ASSERT(len == len2);
  for (intptr_t i = 0; i < len; i++) {
    if (p[i] == '\n' || p[i] == '\r') {
      p[i] = ' ';
    }
  }
  // Instructions are represented as four consecutive values in a JSON array.
  // Comments only use the third slot. See above comment for more information.
  jsarr_.AddValueNull();
  jsarr_.AddValueNull();
  jsarr_.AddValue(p);
  jsarr_.AddValueNull();
  free(p);
}

void Disassembler::Disassemble(uword start,
                               uword end,
                               DisassemblyFormatter* formatter,
                               const Code& code) {
  NoSafepointScope no_safepoint;
  const Code::Comments& comments =
      code.IsNull() ? Code::Comments::New(0) : code.comments();
  ASSERT(formatter != NULL);
  char hex_buffer[kHexadecimalBufferSize];  // Instruction in hexadecimal form.
  char human_buffer[kUserReadableBufferSize];  // Human-readable instruction.
  uword pc = start;
  intptr_t comment_finger = 0;
  GrowableArray<Function*> inlined_functions;
  while (pc < end) {
    const intptr_t offset = pc - start;
    const intptr_t old_comment_finger = comment_finger;
    while (comment_finger < comments.Length() &&
           comments.PCOffsetAt(comment_finger) <= offset) {
      formatter->Print(
          "        ;; %s\n",
          String::Handle(comments.CommentAt(comment_finger)).ToCString());
      comment_finger++;
    }
    if (old_comment_finger != comment_finger) {
      char str[4000];
      BufferFormatter f(str, sizeof(str));
      // Comment emitted, emit inlining information.
      code.GetInlinedFunctionsAt(offset, &inlined_functions);
      // Skip top scope function printing (last entry in 'inlined_functions').
      bool first = true;
      for (intptr_t i = inlined_functions.length() - 2; i >= 0; i--) {
        const char* name = inlined_functions[i]->ToQualifiedCString();
        if (first) {
          f.Print("        ;; Inlined [%s", name);
          first = false;
        } else {
          f.Print(" -> %s", name);
        }
      }
      if (!first) {
        f.Print("]\n");
        formatter->Print(str);
      }
    }
    int instruction_length;
    Object* object;
    DecodeInstruction(hex_buffer,
                      sizeof(hex_buffer),
                      human_buffer,
                      sizeof(human_buffer),
                      &instruction_length, code, &object, pc);
    formatter->ConsumeInstruction(code,
                                  hex_buffer,
                                  sizeof(hex_buffer),
                                  human_buffer,
                                  sizeof(human_buffer),
                                  object,
                                  pc);
    pc += instruction_length;
  }
}

void Disassembler::DisassembleCodeHelper(
    const char* function_fullname, const Code& code, bool optimized) {
  LocalVarDescriptors& var_descriptors = LocalVarDescriptors::Handle();
  if (FLAG_print_variable_descriptors) {
    // This flag is not on by default, and for debugging purposes only.
    // Since this may allocate, do it outside the NoSafepointScope.
    var_descriptors = code.GetLocalVarDescriptors();
  }
  NoSafepointScope no_safepoint;
  THR_Print("Code for %sfunction '%s' {\n",
            optimized ? "optimized " : "",
            function_fullname);
  code.Disassemble();
  THR_Print("}\n");

#if defined(TARGET_ARCH_IA32)
  THR_Print("Pointer offsets for function: {\n");
  // Pointer offsets are stored in descending order.
  Object& obj = Object::Handle();
  for (intptr_t i = code.pointer_offsets_length() - 1; i >= 0; i--) {
    const uword addr = code.GetPointerOffsetAt(i) + code.PayloadStart();
    obj = *reinterpret_cast<RawObject**>(addr);
    THR_Print(" %d : %#" Px " '%s'\n",
              code.GetPointerOffsetAt(i), addr, obj.ToCString());
  }
  THR_Print("}\n");
#else
  ASSERT(code.pointer_offsets_length() == 0);
#endif

  const ObjectPool& object_pool = ObjectPool::Handle(code.GetObjectPool());
  object_pool.DebugPrint();

  THR_Print("PC Descriptors for function '%s' {\n", function_fullname);
  PcDescriptors::PrintHeaderString();
  const PcDescriptors& descriptors =
      PcDescriptors::Handle(code.pc_descriptors());
  THR_Print("%s}\n", descriptors.ToCString());

  uword start = Instructions::Handle(code.instructions()).PayloadStart();
  const Array& deopt_table = Array::Handle(code.deopt_info_array());
  intptr_t deopt_table_length = DeoptTable::GetLength(deopt_table);
  if (deopt_table_length > 0) {
    THR_Print("DeoptInfo: {\n");
    Smi& offset = Smi::Handle();
    TypedData& info = TypedData::Handle();
    Smi& reason_and_flags = Smi::Handle();
    for (intptr_t i = 0; i < deopt_table_length; ++i) {
      DeoptTable::GetEntry(deopt_table, i, &offset, &info, &reason_and_flags);
      const intptr_t reason =
          DeoptTable::ReasonField::decode(reason_and_flags.Value());
      ASSERT((0 <= reason) && (reason < ICData::kDeoptNumReasons));
      THR_Print("%4" Pd ": 0x%" Px "  %s  (%s)\n",
                i,
                start + offset.Value(),
                DeoptInfo::ToCString(deopt_table, info),
                DeoptReasonToCString(
                    static_cast<ICData::DeoptReasonId>(reason)));
    }
    THR_Print("}\n");
  }

  THR_Print("Stackmaps for function '%s' {\n", function_fullname);
  if (code.stackmaps() != Array::null()) {
    const Array& stackmap_table = Array::Handle(code.stackmaps());
    Stackmap& map = Stackmap::Handle();
    for (intptr_t i = 0; i < stackmap_table.Length(); ++i) {
      map ^= stackmap_table.At(i);
      THR_Print("%s\n", map.ToCString());
    }
  }
  THR_Print("}\n");

  if (FLAG_print_variable_descriptors) {
    THR_Print("Variable Descriptors for function '%s' {\n",
              function_fullname);
    intptr_t var_desc_length =
        var_descriptors.IsNull() ? 0 : var_descriptors.Length();
    String& var_name = String::Handle();
    for (intptr_t i = 0; i < var_desc_length; i++) {
      var_name = var_descriptors.GetName(i);
      RawLocalVarDescriptors::VarInfo var_info;
      var_descriptors.GetInfo(i, &var_info);
      const int8_t kind = var_info.kind();
      if (kind == RawLocalVarDescriptors::kSavedCurrentContext) {
        THR_Print("  saved current CTX reg offset %d\n", var_info.index());
      } else {
        if (kind == RawLocalVarDescriptors::kContextLevel) {
          THR_Print("  context level %d scope %d", var_info.index(),
              var_info.scope_id);
        } else if (kind == RawLocalVarDescriptors::kStackVar) {
          THR_Print("  stack var '%s' offset %d",
            var_name.ToCString(), var_info.index());
        } else {
          ASSERT(kind == RawLocalVarDescriptors::kContextVar);
          THR_Print("  context var '%s' level %d offset %d",
              var_name.ToCString(), var_info.scope_id, var_info.index());
        }
        THR_Print(" (valid %s-%s)\n", var_info.begin_pos.ToCString(),
                                      var_info.end_pos.ToCString());
      }
    }
    THR_Print("}\n");
  }

  THR_Print("Exception Handlers for function '%s' {\n", function_fullname);
  const ExceptionHandlers& handlers =
        ExceptionHandlers::Handle(code.exception_handlers());
  THR_Print("%s}\n", handlers.ToCString());

  {
    THR_Print("Static call target functions {\n");
    const Array& table = Array::Handle(code.static_calls_target_table());
    Smi& offset = Smi::Handle();
    Function& function = Function::Handle();
    Code& code = Code::Handle();
    for (intptr_t i = 0; i < table.Length();
        i += Code::kSCallTableEntryLength) {
      offset ^= table.At(i + Code::kSCallTableOffsetEntry);
      function ^= table.At(i + Code::kSCallTableFunctionEntry);
      code ^= table.At(i + Code::kSCallTableCodeEntry);
      if (function.IsNull()) {
        Class& cls = Class::Handle();
        cls ^= code.owner();
        if (cls.IsNull()) {
          THR_Print("  0x%" Px ": %s, %p\n",
              start + offset.Value(),
              code.Name(),
              code.raw());
        } else {
          THR_Print("  0x%" Px ": allocation stub for %s, %p\n",
              start + offset.Value(),
              cls.ToCString(),
              code.raw());
        }
      } else {
        THR_Print("  0x%" Px ": %s, %p\n",
            start + offset.Value(),
            function.ToFullyQualifiedCString(),
            code.raw());
      }
    }
    THR_Print("}\n");
  }
  if (optimized && FLAG_trace_inlining_intervals) {
    code.DumpInlinedIntervals();
  }
}

void Disassembler::DisassembleCode(const Function& function, bool optimized) {
  const char* function_fullname = function.ToFullyQualifiedCString();
  const Code& code = Code::Handle(function.CurrentCode());
  DisassembleCodeHelper(function_fullname, code, optimized);
}

void Disassembler::DisassembleCodeUnoptimized(
    const Function& function, bool optimized) {
  const char* function_fullname = function.ToFullyQualifiedCString();
  const Code& code = Code::Handle(function.unoptimized_code());
  DisassembleCodeHelper(function_fullname, code, optimized);
}

#endif  // !PRODUCT

}  // namespace dart
<commit_msg>Fix too strict debug assertion in disassembler.<commit_after>// Copyright (c) 2011, the Dart project authors.  Please see the AUTHORS file
// for details. All rights reserved. Use of this source code is governed by a
// BSD-style license that can be found in the LICENSE file.

#include "vm/disassembler.h"

#include "vm/assembler.h"
#include "vm/deopt_instructions.h"
#include "vm/globals.h"
#include "vm/il_printer.h"
#include "vm/instructions.h"
#include "vm/json_stream.h"
#include "vm/log.h"
#include "vm/os.h"
#include "vm/code_patcher.h"

namespace dart {

#ifndef PRODUCT

DECLARE_FLAG(bool, trace_inlining_intervals);

void DisassembleToStdout::ConsumeInstruction(const Code& code,
                                             char* hex_buffer,
                                             intptr_t hex_size,
                                             char* human_buffer,
                                             intptr_t human_size,
                                             Object* object,
                                             uword pc) {
  static const int kHexColumnWidth = 23;
  uint8_t* pc_ptr = reinterpret_cast<uint8_t*>(pc);
  THR_Print("%p    %s", pc_ptr, hex_buffer);
  int hex_length = strlen(hex_buffer);
  if (hex_length < kHexColumnWidth) {
    for (int i = kHexColumnWidth - hex_length; i > 0; i--) {
      THR_Print(" ");
    }
  }
  THR_Print("%s", human_buffer);
  if (object != NULL) {
    THR_Print("   %s", object->ToCString());
  }
  THR_Print("\n");
}

void DisassembleToStdout::Print(const char* format, ...) {
  va_list args;
  va_start(args, format);
  THR_VPrint(format, args);
  va_end(args);
}

void DisassembleToJSONStream::ConsumeInstruction(const Code& code,
                                                 char* hex_buffer,
                                                 intptr_t hex_size,
                                                 char* human_buffer,
                                                 intptr_t human_size,
                                                 Object* object,
                                                 uword pc) {
  // Instructions are represented as four consecutive values in a JSON array.
  // The first is the address of the instruction, the second is the hex string,
  // of the code, and the third is a human readable string, and the fourth is
  // the object loaded by the instruction.
  jsarr_.AddValueF("%" Pp "", pc);
  jsarr_.AddValue(hex_buffer);
  jsarr_.AddValue(human_buffer);

  if (object != NULL) {
    jsarr_.AddValue(*object);
  } else {
    jsarr_.AddValueNull();  // Not a reference to null.
  }
}

void DisassembleToJSONStream::Print(const char* format, ...) {
  va_list args;
  va_start(args, format);
  intptr_t len = OS::VSNPrint(NULL, 0, format, args);
  va_end(args);
  char* p = reinterpret_cast<char*>(malloc(len+1));
  va_start(args, format);
  intptr_t len2 = OS::VSNPrint(p, len, format, args);
  va_end(args);
  ASSERT(len == len2);
  for (intptr_t i = 0; i < len; i++) {
    if (p[i] == '\n' || p[i] == '\r') {
      p[i] = ' ';
    }
  }
  // Instructions are represented as four consecutive values in a JSON array.
  // Comments only use the third slot. See above comment for more information.
  jsarr_.AddValueNull();
  jsarr_.AddValueNull();
  jsarr_.AddValue(p);
  jsarr_.AddValueNull();
  free(p);
}

void Disassembler::Disassemble(uword start,
                               uword end,
                               DisassemblyFormatter* formatter,
                               const Code& code) {
  const Code::Comments& comments =
      code.IsNull() ? Code::Comments::New(0) : code.comments();
  ASSERT(formatter != NULL);
  char hex_buffer[kHexadecimalBufferSize];  // Instruction in hexadecimal form.
  char human_buffer[kUserReadableBufferSize];  // Human-readable instruction.
  uword pc = start;
  intptr_t comment_finger = 0;
  GrowableArray<Function*> inlined_functions;
  while (pc < end) {
    const intptr_t offset = pc - start;
    const intptr_t old_comment_finger = comment_finger;
    while (comment_finger < comments.Length() &&
           comments.PCOffsetAt(comment_finger) <= offset) {
      formatter->Print(
          "        ;; %s\n",
          String::Handle(comments.CommentAt(comment_finger)).ToCString());
      comment_finger++;
    }
    if (old_comment_finger != comment_finger) {
      char str[4000];
      BufferFormatter f(str, sizeof(str));
      // Comment emitted, emit inlining information.
      code.GetInlinedFunctionsAt(offset, &inlined_functions);
      // Skip top scope function printing (last entry in 'inlined_functions').
      bool first = true;
      for (intptr_t i = inlined_functions.length() - 2; i >= 0; i--) {
        const char* name = inlined_functions[i]->ToQualifiedCString();
        if (first) {
          f.Print("        ;; Inlined [%s", name);
          first = false;
        } else {
          f.Print(" -> %s", name);
        }
      }
      if (!first) {
        f.Print("]\n");
        formatter->Print(str);
      }
    }
    int instruction_length;
    Object* object;
    DecodeInstruction(hex_buffer,
                      sizeof(hex_buffer),
                      human_buffer,
                      sizeof(human_buffer),
                      &instruction_length, code, &object, pc);
    formatter->ConsumeInstruction(code,
                                  hex_buffer,
                                  sizeof(hex_buffer),
                                  human_buffer,
                                  sizeof(human_buffer),
                                  object,
                                  pc);
    pc += instruction_length;
  }
}

void Disassembler::DisassembleCodeHelper(
    const char* function_fullname, const Code& code, bool optimized) {
  LocalVarDescriptors& var_descriptors = LocalVarDescriptors::Handle();
  if (FLAG_print_variable_descriptors) {
    var_descriptors = code.GetLocalVarDescriptors();
  }
  THR_Print("Code for %sfunction '%s' {\n",
            optimized ? "optimized " : "",
            function_fullname);
  code.Disassemble();
  THR_Print("}\n");

#if defined(TARGET_ARCH_IA32)
  THR_Print("Pointer offsets for function: {\n");
  // Pointer offsets are stored in descending order.
  Object& obj = Object::Handle();
  for (intptr_t i = code.pointer_offsets_length() - 1; i >= 0; i--) {
    const uword addr = code.GetPointerOffsetAt(i) + code.PayloadStart();
    obj = *reinterpret_cast<RawObject**>(addr);
    THR_Print(" %d : %#" Px " '%s'\n",
              code.GetPointerOffsetAt(i), addr, obj.ToCString());
  }
  THR_Print("}\n");
#else
  ASSERT(code.pointer_offsets_length() == 0);
#endif

  const ObjectPool& object_pool = ObjectPool::Handle(code.GetObjectPool());
  object_pool.DebugPrint();

  THR_Print("PC Descriptors for function '%s' {\n", function_fullname);
  PcDescriptors::PrintHeaderString();
  const PcDescriptors& descriptors =
      PcDescriptors::Handle(code.pc_descriptors());
  THR_Print("%s}\n", descriptors.ToCString());

  uword start = Instructions::Handle(code.instructions()).PayloadStart();
  const Array& deopt_table = Array::Handle(code.deopt_info_array());
  intptr_t deopt_table_length = DeoptTable::GetLength(deopt_table);
  if (deopt_table_length > 0) {
    THR_Print("DeoptInfo: {\n");
    Smi& offset = Smi::Handle();
    TypedData& info = TypedData::Handle();
    Smi& reason_and_flags = Smi::Handle();
    for (intptr_t i = 0; i < deopt_table_length; ++i) {
      DeoptTable::GetEntry(deopt_table, i, &offset, &info, &reason_and_flags);
      const intptr_t reason =
          DeoptTable::ReasonField::decode(reason_and_flags.Value());
      ASSERT((0 <= reason) && (reason < ICData::kDeoptNumReasons));
      THR_Print("%4" Pd ": 0x%" Px "  %s  (%s)\n",
                i,
                start + offset.Value(),
                DeoptInfo::ToCString(deopt_table, info),
                DeoptReasonToCString(
                    static_cast<ICData::DeoptReasonId>(reason)));
    }
    THR_Print("}\n");
  }

  THR_Print("Stackmaps for function '%s' {\n", function_fullname);
  if (code.stackmaps() != Array::null()) {
    const Array& stackmap_table = Array::Handle(code.stackmaps());
    Stackmap& map = Stackmap::Handle();
    for (intptr_t i = 0; i < stackmap_table.Length(); ++i) {
      map ^= stackmap_table.At(i);
      THR_Print("%s\n", map.ToCString());
    }
  }
  THR_Print("}\n");

  if (FLAG_print_variable_descriptors) {
    THR_Print("Variable Descriptors for function '%s' {\n",
              function_fullname);
    intptr_t var_desc_length =
        var_descriptors.IsNull() ? 0 : var_descriptors.Length();
    String& var_name = String::Handle();
    for (intptr_t i = 0; i < var_desc_length; i++) {
      var_name = var_descriptors.GetName(i);
      RawLocalVarDescriptors::VarInfo var_info;
      var_descriptors.GetInfo(i, &var_info);
      const int8_t kind = var_info.kind();
      if (kind == RawLocalVarDescriptors::kSavedCurrentContext) {
        THR_Print("  saved current CTX reg offset %d\n", var_info.index());
      } else {
        if (kind == RawLocalVarDescriptors::kContextLevel) {
          THR_Print("  context level %d scope %d", var_info.index(),
              var_info.scope_id);
        } else if (kind == RawLocalVarDescriptors::kStackVar) {
          THR_Print("  stack var '%s' offset %d",
            var_name.ToCString(), var_info.index());
        } else {
          ASSERT(kind == RawLocalVarDescriptors::kContextVar);
          THR_Print("  context var '%s' level %d offset %d",
              var_name.ToCString(), var_info.scope_id, var_info.index());
        }
        THR_Print(" (valid %s-%s)\n", var_info.begin_pos.ToCString(),
                                      var_info.end_pos.ToCString());
      }
    }
    THR_Print("}\n");
  }

  THR_Print("Exception Handlers for function '%s' {\n", function_fullname);
  const ExceptionHandlers& handlers =
        ExceptionHandlers::Handle(code.exception_handlers());
  THR_Print("%s}\n", handlers.ToCString());

  {
    THR_Print("Static call target functions {\n");
    const Array& table = Array::Handle(code.static_calls_target_table());
    Smi& offset = Smi::Handle();
    Function& function = Function::Handle();
    Code& code = Code::Handle();
    for (intptr_t i = 0; i < table.Length();
        i += Code::kSCallTableEntryLength) {
      offset ^= table.At(i + Code::kSCallTableOffsetEntry);
      function ^= table.At(i + Code::kSCallTableFunctionEntry);
      code ^= table.At(i + Code::kSCallTableCodeEntry);
      if (function.IsNull()) {
        Class& cls = Class::Handle();
        cls ^= code.owner();
        if (cls.IsNull()) {
          THR_Print("  0x%" Px ": %s, %p\n",
              start + offset.Value(),
              code.Name(),
              code.raw());
        } else {
          THR_Print("  0x%" Px ": allocation stub for %s, %p\n",
              start + offset.Value(),
              cls.ToCString(),
              code.raw());
        }
      } else {
        THR_Print("  0x%" Px ": %s, %p\n",
            start + offset.Value(),
            function.ToFullyQualifiedCString(),
            code.raw());
      }
    }
    THR_Print("}\n");
  }
  if (optimized && FLAG_trace_inlining_intervals) {
    code.DumpInlinedIntervals();
  }
}

void Disassembler::DisassembleCode(const Function& function, bool optimized) {
  const char* function_fullname = function.ToFullyQualifiedCString();
  const Code& code = Code::Handle(function.CurrentCode());
  DisassembleCodeHelper(function_fullname, code, optimized);
}

void Disassembler::DisassembleCodeUnoptimized(
    const Function& function, bool optimized) {
  const char* function_fullname = function.ToFullyQualifiedCString();
  const Code& code = Code::Handle(function.unoptimized_code());
  DisassembleCodeHelper(function_fullname, code, optimized);
}

#endif  // !PRODUCT

}  // namespace dart
<|endoftext|>


<commit_before>// Copyright (c) 2020 by Robert Bosch GmbH. All rights reserved.
// Copyright (c) 2021 - 2022 by Apex.AI Inc. All rights reserved.
//
// 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.
//
// SPDX-License-Identifier: Apache-2.0
#ifndef IOX_HOOFS_CXX_STRING_INTERNAL_HPP
#define IOX_HOOFS_CXX_STRING_INTERNAL_HPP

#include "iceoryx_hoofs/cxx/attributes.hpp"

#include <cstdint>
#include <cstring>
#include <string>

namespace iox
{
namespace cxx
{
template <uint64_t>
class string;

namespace internal
{
template <uint64_t N>
// c array not used here, it is a type alias for easier access
// NOLINTNEXTLINE(hicpp-avoid-c-arrays, cppcoreguidelines-avoid-c-arrays)
using charArray = char[N];

/// @brief struct to get capacity of iox::cxx::string/char array/char
/// @note capa is a dummy value for any type other than cxx::string and char
template <typename T>
struct GetCapa
{
    static constexpr uint64_t capa = 0U;
};

template <uint64_t N>
struct GetCapa<string<N>>
{
    static constexpr uint64_t capa = N;
};

template <uint64_t N>
// used to acquire char array capacity safely at compile time
// NOLINTNEXTLINE(hicpp-avoid-c-arrays, cppcoreguidelines-avoid-c-arrays)
struct GetCapa<char[N]>
{
    static constexpr uint64_t capa = N - 1U;
};

template <>
struct GetCapa<char>
{
    static constexpr uint64_t capa = 1U;
};

/// @brief struct to get size of iox::cxx::string/std::string/char array/char
template <typename T>
struct GetSize;

template <uint64_t N>
struct GetSize<string<N>>
{
    static uint64_t call(const string<N>& data) noexcept
    {
        return data.size();
    }
};

template <uint64_t N>
// used to acquire size of c array safely, strnlen only accesses N elements which is the maximum capacity of the array
// where N is a compile time constant
// NOLINTNEXTLINE(hicpp-avoid-c-arrays, cppcoreguidelines-avoid-c-arrays)
struct GetSize<char[N]>
{
    static uint64_t call(const charArray<N>& data) noexcept
    {
        return strnlen(data, N);
    }
};

template <>
struct GetSize<std::string>
{
    static uint64_t call(const std::string& data) noexcept
    {
        return data.size();
    }
};

template <>
struct GetSize<char>
{
    static uint64_t call(char c IOX_MAYBE_UNUSED) noexcept
    {
        return 1U;
    }
};

/// @brief struct to get a pointer to the char array of the fixed string/string literal/std::string
template <typename T>
struct GetData;

template <uint64_t N>
struct GetData<string<N>>
{
    static const char* call(const string<N>& data) noexcept
    {
        return data.c_str();
    }
};

template <uint64_t N>
// provides uniform and safe access (in combination with GetCapa and GetSize) to string like constructs like
// cxx::string, std::string, string literal, char
// NOLINTNEXTLINE(hicpp-avoid-c-arrays, cppcoreguidelines-avoid-c-arrays)
struct GetData<char[N]>
{
    static const char* call(const charArray<N>& data) noexcept
    {
        return &data[0];
    }
};

template <>
struct GetData<std::string>
{
    static const char* call(const std::string& data) noexcept
    {
        return data.data();
    }
};

template <>
struct GetData<char>
{
    static const char* call(const char& data) noexcept
    {
        return &data;
    }
};

/// @brief struct to get the sum of the capacities of iox::cxx::strings/char arrays/chars
template <typename... Targs>
struct SumCapa;

template <>
struct SumCapa<>
{
    static constexpr uint64_t value = 0U;
};

template <typename T, typename... Targs>
struct SumCapa<T, Targs...>
{
    static constexpr uint64_t value = GetCapa<T>::capa + SumCapa<Targs...>::value;
};
} // namespace internal
} // namespace cxx
} // namespace iox
#endif // IOX_HOOFS_CXX_STRING_INTERNAL_HPP
<commit_msg>iox-#1394 fix axivion reported violations for string_internal.hpp<commit_after>// Copyright (c) 2020 by Robert Bosch GmbH. All rights reserved.
// Copyright (c) 2021 - 2022 by Apex.AI Inc. All rights reserved.
//
// 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.
//
// SPDX-License-Identifier: Apache-2.0
#ifndef IOX_HOOFS_CXX_STRING_INTERNAL_HPP
#define IOX_HOOFS_CXX_STRING_INTERNAL_HPP

#include "iceoryx_hoofs/cxx/attributes.hpp"

#include <cstdint>
#include <cstring>
#include <string>

namespace iox
{
namespace cxx
{
// AXIVION DISABLE STYLE AutosarC++19_03-A3.9.1: Basic numerical type of char shall be used
// AXIVION DISABLE STYLE AutosarC++19_03-A18.1.1: C-style arrays are used to acquire size of c
// array safely, strnlen only accesses N elements which is the maximum capacity of the array
// where N is a compile time constant
template <uint64_t>
class string;

namespace internal
{
template <uint64_t N>
// c array not used here, it is a type alias for easier access
// NOLINTNEXTLINE(hicpp-avoid-c-arrays, cppcoreguidelines-avoid-c-arrays)
using charArray = char[N];

/// @brief struct to get capacity of iox::cxx::string/char array/char
/// @note capa is a dummy value for any type other than cxx::string and char
template <typename T>
struct GetCapa
{
    static constexpr uint64_t capa{0U};
};

template <uint64_t N>
struct GetCapa<string<N>>
{
    static constexpr uint64_t capa{N};
};

template <uint64_t N>
// used to acquire char array capacity safely at compile time
// NOLINTNEXTLINE(hicpp-avoid-c-arrays, cppcoreguidelines-avoid-c-arrays)
struct GetCapa<char[N]>
{
    static constexpr uint64_t capa{N - 1U};
};

template <>
struct GetCapa<char>
{
    static constexpr uint64_t capa{1U};
};

/// @brief struct to get size of iox::cxx::string/std::string/char array/char
template <typename T>
struct GetSize;

template <uint64_t N>
struct GetSize<string<N>>
{
    static uint64_t call(const string<N>& data) noexcept
    {
        return data.size();
    }
};

template <uint64_t N>
// used to acquire size of c array safely, strnlen only accesses N elements which is the maximum capacity of the array
// where N is a compile time constant
// NOLINTNEXTLINE(hicpp-avoid-c-arrays, cppcoreguidelines-avoid-c-arrays)
struct GetSize<char[N]>
{
    static uint64_t call(const charArray<N>& data) noexcept
    {
        return strnlen(data, N);
    }
};

template <>
struct GetSize<std::string>
{
    static uint64_t call(const std::string& data) noexcept
    {
        return data.size();
    }
};

template <>
struct GetSize<char>
{
    static uint64_t call(char c IOX_MAYBE_UNUSED) noexcept
    {
        return 1U;
    }
};

/// @brief struct to get a pointer to the char array of the fixed string/string literal/std::string
template <typename T>
struct GetData;

template <uint64_t N>
struct GetData<string<N>>
{
    static const char* call(const string<N>& data) noexcept
    {
        return data.c_str();
    }
};

template <uint64_t N>
// provides uniform and safe access (in combination with GetCapa and GetSize) to string like constructs like
// cxx::string, std::string, string literal, char
// NOLINTNEXTLINE(hicpp-avoid-c-arrays, cppcoreguidelines-avoid-c-arrays)
struct GetData<char[N]>
{
    static const char* call(const charArray<N>& data) noexcept
    {
        return &data[0];
    }
};

template <>
struct GetData<std::string>
{
    static const char* call(const std::string& data) noexcept
    {
        return data.data();
    }
};

template <>
struct GetData<char>
{
    static const char* call(const char& data) noexcept
    {
        // AXIVION Next Construct AutosarC++19_03-A7.5.1 : Used for template meta-programming and
        // safe in this context
        return &data;
    }
};

/// @brief struct to get the sum of the capacities of iox::cxx::strings/char arrays/chars
template <typename... Targs>
struct SumCapa;

template <>
struct SumCapa<>
{
    static constexpr uint64_t value{0U};
};

template <typename T, typename... Targs>
struct SumCapa<T, Targs...>
{
    static constexpr uint64_t value{GetCapa<T>::capa + SumCapa<Targs...>::value};
};
} // namespace internal
// AXIVION ENABLE STYLE AutosarC++19_03-A3.9.1
// AXIVION ENABLE STYLE AutosarC++19_03-A18.1.1
} // namespace cxx
} // namespace iox
#endif // IOX_HOOFS_CXX_STRING_INTERNAL_HPP
<|endoftext|>


<commit_before>//   Copyright 2015 Patrick Putnam
//
//   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 CLOTHO_ROW_GROUPED_FREE_SPACE_EVALUATOR_HPP_
#define CLOTHO_ROW_GROUPED_FREE_SPACE_EVALUATOR_HPP_

#include "clotho/data_spaces/association_matrix/row_grouped_association_matrix.hpp"
#include "clotho/utility/bit_helper.hpp"
#include "clotho/utility/debruijn_bit_walker.hpp"
#include <map>
#include <set>

namespace clotho {
namespace genetics {

template < class BlockType, unsigned char P >
class free_space_evaluator< association_matrix< BlockType, row_grouped< P > > > {
public:
    typedef association_matrix< BlockType, row_grouped< P > > space_type;
    typedef typename space_type::block_type                 block_type;
    typedef size_t *                                        result_type;

    typedef typename space_type::raw_block_pointer    block_pointer;

    typedef clotho::utility::debruijn_bit_walker< block_type >  bit_walker_type;
    typedef clotho::utility::BitHelper< block_type >            bit_helper_type;

    void operator()( space_type & ss, result_type res, size_t & fixed_offset, size_t & lost_offset, size_t & free_count, size_t M ) {

        size_t W = bit_helper_type::padded_block_count( M );

        block_type * tmp = new block_type[ 2 * W ];

        memset( tmp, 255, sizeof(block_type) * W );
        memset( tmp + W, 0, sizeof(block_type) * W );

        const size_t row_count = ss.block_row_count();

        size_t fo = fixed_offset;
        size_t lo = lost_offset;
        size_t fr = free_count;

        block_type * fx_ptr = tmp;
        block_type * var_ptr = tmp + W;

        size_t k = 0;
        while( k < row_count ) {

            block_pointer start = ss.begin_block_row( k );
            block_pointer end = ss.end_block_row( k );

            fx_ptr = tmp;
            var_ptr = tmp + W;

            size_t i = 0;
            while( start != end ) {
                block_type b = *start++;
                *fx_ptr &= b;
                *var_ptr |= b;
                if( ++i == row_grouped< P >::GROUP_SIZE ) {
                    ++fx_ptr;
                    ++var_ptr;
                    i = 0;
                }
            }

            assert(fx_ptr == tmp + W);

            k += row_grouped< P >::GROUP_SIZE;
        }

        size_t j = 0;

        fx_ptr = tmp;
        var_ptr = tmp + W;
        while( fx_ptr != tmp + W ) {
            block_type fx = *fx_ptr++;
            block_type var = *var_ptr++;

            block_type ls = ~(fx | var);

            while( fx ) {
                size_t b_idx = bit_walker_type::unset_next_index( fx ) + j;
                if( b_idx < M ) {
                    res[ fr++ ] = b_idx;
                    res[ fo++ ] = b_idx;
                }
            }

            while( ls ) {
                size_t idx = bit_walker_type::unset_next_index( ls ) + j;
                if( idx < M ) {
                    res[ fr++ ] = idx;
                    res[ lo++ ] = idx;
                }
            }

            j += bit_helper_type::BITS_PER_BLOCK;
        }

        fixed_offset = fo;
        lost_offset = lo;
        free_count = fr;

        delete [] tmp;
    }
};
/*
template < class BlockType >
class free_space_evaluator< association_matrix< BlockType, row_grouped< 1 > > > {
public:
    typedef association_matrix< BlockType, row_grouped< 1 > > space_type;
    typedef typename space_type::row_vector                 row_vector;
    typedef typename row_vector::block_type                 block_type;
    typedef size_t *                                        result_type;

    typedef typename row_vector::raw_pointer                raw_pointer;

    typedef clotho::utility::debruijn_bit_walker< block_type >  bit_walker_type;
    typedef clotho::utility::BitHelper< block_type >            bit_helper_type;

    free_space_evaluator() :
        tmp(NULL)
        , m_alloc_size(0)
    {}

    void operator()( space_type & ss, result_type res, size_t & fixed_offset, size_t & lost_offset, size_t & free_count, size_t M ) {

//        size_t W = bit_helper_type::padded_block_count( M );
        size_t W = ss.hard_block_count();

        resize( W );

        for( size_t i = 0; i < W; ++i ) {
            tmp[ 2 * i ] = bit_helper_type::ALL_SET;
            tmp[ 2 * i + 1 ] = bit_helper_type::ALL_UNSET;
        }

        std::set< raw_pointer > analyzed;

        const size_t row_count = ss.row_count();

        size_t fo = fixed_offset;
        size_t lo = lost_offset;
        size_t fr = free_count;

        size_t k = 0;
        while( k < row_count ) {

            raw_pointer start = ss.getRow( k ).get();

            if( analyzed.find( start ) == analyzed.end() ) {
                analyzed.insert( start );
                size_t  N = ss.getRow( k ).m_size;

#ifdef DEBUGGING
                assert( N <= W );
#endif // DEBUGGING

//                size_t i = 0;
//                while( i < N ) {
//                    block_type b = start[i];
//                    tmp[ 2 * i ] &= b;
//                    tmp[ 2 * i + 1 ] |= b;
//                    ++i;
//                }

                raw_pointer end = start + N;
                block_type * t = tmp;
                while( start != end ) {
                    *t++ &= *start;
                    *t++ |= *start++;
                }

                size_t i = N;
                while( i < W ) {
                    tmp[ 2 * i ] = bit_helper_type::ALL_UNSET;
                    ++i;
                }
            }

            ++k;
        }

#ifdef DEBUGGING
        std::cerr << "Free Space analyzed: " << analyzed.size() << std::endl;
#endif  // DEBUGGING

        size_t j = 0;

        for( unsigned int i = 0; i < W; ++i ) {
            block_type fx = tmp[ 2 *i ];
            block_type var = tmp[2 * i + 1];

            block_type ls = ~(fx | var);

            while( fx ) {
                size_t b_idx = bit_walker_type::unset_next_index( fx ) + j;
                if( b_idx < M ) {
                    res[ fr++ ] = b_idx;
                    res[ fo++ ] = b_idx;
                }
            }

            while( ls ) {
                size_t idx = bit_walker_type::unset_next_index( ls ) + j;
                if( idx < M ) {
                    res[ fr++ ] = idx;
                    res[ lo++ ] = idx;
                }
            }

            j += bit_helper_type::BITS_PER_BLOCK;
        }

        fixed_offset = fo;
        lost_offset = lo;
        free_count = fr;
    }

    virtual ~free_space_evaluator() {
        if( tmp != NULL ) {
            delete [] tmp;
        }
    }

protected:
    void resize( size_t W ) {
        if( 2 * W > m_alloc_size ) {
            if( tmp != NULL ) {
                delete [] tmp;
            }

            tmp = new block_type[ 2 * W ];
            m_alloc_size = 2 * W;
        }
    }
    block_type * tmp;
    size_t m_alloc_size;
};*/

/// this approach eliminates writing to the heap
/// utilizes stack buffers to write temporary data
template < class BlockType >
class free_space_evaluator< association_matrix< BlockType, row_grouped< 1 > > > {
public:
    typedef association_matrix< BlockType, row_grouped< 1 > > space_type;
    typedef typename space_type::row_vector                 row_vector;
    typedef typename row_vector::block_type                 block_type;
    typedef size_t *                                        result_type;

    typedef typename row_vector::raw_pointer                raw_pointer;

    typedef clotho::utility::debruijn_bit_walker< block_type >  bit_walker_type;
    typedef clotho::utility::BitHelper< block_type >            bit_helper_type;

    free_space_evaluator() {}

    void operator()( space_type & ss, result_type res, size_t & fixed_offset, size_t & lost_offset, size_t & free_count, size_t M ) {

        size_t W = ss.hard_block_count();

        std::map< raw_pointer, size_t > analyzed;
        typedef typename std::map< raw_pointer, size_t >::iterator iterator;

        const size_t row_count = ss.row_count();

        size_t fo = fixed_offset;
        size_t lo = lost_offset;
        size_t fr = free_count;

        // pre-scan population for all unique sequences
        size_t k = 0;
        while( k < row_count ) {
            raw_pointer start = ss.getRow( k ).get();
            size_t s = ss.getRow( k ).m_size;

            if( analyzed.find( start ) == analyzed.end() ) {
                analyzed[ start ] = s;
            }
            ++k;
        }

#ifdef DEBUGGING
        std::cerr << "Free Space analyzing: " << analyzed.size() << std::endl;
#endif  // DEBUGGING

        const size_t BUFFER_SIZE = 8;
        k = 0;
        while( k < W ) {
//            // 16 * sizeof(block_type) * 2 == 16 * 8 * 2 == 256 byte buffer
            block_type fx_buffer[ BUFFER_SIZE ];
            block_type var_buffer[ BUFFER_SIZE ];

            // initialize buffers
            for( int x = 0; x < BUFFER_SIZE; ++x ) {
                fx_buffer[ x ] = bit_helper_type::ALL_SET;
                var_buffer[ x ] = bit_helper_type::ALL_UNSET;
            }

            // analyze block columns
            for( iterator it = analyzed.begin(); it != analyzed.end(); it++ ) {
                size_t S = it->second;
                size_t N = ((k >= S)? 0 : (( k + BUFFER_SIZE <= S ) ? BUFFER_SIZE : (S - k)));
                size_t x = 0;
                raw_pointer r = it->first;
                while( x < N ) {
                    block_type b = r[ k + x ];
                    fx_buffer[ x ] &= b;
                    var_buffer[ x ] |= b;
                    ++x;
                }

                while( x < BUFFER_SIZE ) {
                    fx_buffer[ x ] = bit_helper_type::ALL_UNSET;
                    ++x;
                }
            }

            // write results
            size_t j = k * bit_helper_type::BITS_PER_BLOCK;

            for( unsigned int i = 0; i < BUFFER_SIZE; ++i ) {
                block_type fx = fx_buffer[ i ];
                block_type var = var_buffer[ i ];

                block_type ls = ~(fx | var);

                while( fx ) {
                    size_t b_idx = bit_walker_type::unset_next_index( fx ) + j;
                    if( b_idx < M ) {
                        res[ fr++ ] = b_idx;
                        res[ fo++ ] = b_idx;
                    }
                }

                while( ls ) {
                    size_t idx = bit_walker_type::unset_next_index( ls ) + j;
                    if( idx < M ) {
                        res[ fr++ ] = idx;
                        res[ lo++ ] = idx;
                    }
                }

                j += bit_helper_type::BITS_PER_BLOCK;
            }

            k += BUFFER_SIZE;
        }

        fixed_offset = fo;
        lost_offset = lo;
        free_count = fr;
    }

    virtual ~free_space_evaluator() { }
};

}   // namespace genetics
}   // namespace clotho

#endif  // CLOTHO_ROW_GROUPED_FREE_SPACE_EVALUATOR_HPP_
<commit_msg>Increased buffer size<commit_after>//   Copyright 2015 Patrick Putnam
//
//   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 CLOTHO_ROW_GROUPED_FREE_SPACE_EVALUATOR_HPP_
#define CLOTHO_ROW_GROUPED_FREE_SPACE_EVALUATOR_HPP_

#include "clotho/data_spaces/association_matrix/row_grouped_association_matrix.hpp"
#include "clotho/utility/bit_helper.hpp"
#include "clotho/utility/debruijn_bit_walker.hpp"
#include <map>
#include <set>

namespace clotho {
namespace genetics {

template < class BlockType, unsigned char P >
class free_space_evaluator< association_matrix< BlockType, row_grouped< P > > > {
public:
    typedef association_matrix< BlockType, row_grouped< P > > space_type;
    typedef typename space_type::block_type                 block_type;
    typedef size_t *                                        result_type;

    typedef typename space_type::raw_block_pointer    block_pointer;

    typedef clotho::utility::debruijn_bit_walker< block_type >  bit_walker_type;
    typedef clotho::utility::BitHelper< block_type >            bit_helper_type;

    void operator()( space_type & ss, result_type res, size_t & fixed_offset, size_t & lost_offset, size_t & free_count, size_t M ) {

        size_t W = bit_helper_type::padded_block_count( M );

        block_type * tmp = new block_type[ 2 * W ];

        memset( tmp, 255, sizeof(block_type) * W );
        memset( tmp + W, 0, sizeof(block_type) * W );

        const size_t row_count = ss.block_row_count();

        size_t fo = fixed_offset;
        size_t lo = lost_offset;
        size_t fr = free_count;

        block_type * fx_ptr = tmp;
        block_type * var_ptr = tmp + W;

        size_t k = 0;
        while( k < row_count ) {

            block_pointer start = ss.begin_block_row( k );
            block_pointer end = ss.end_block_row( k );

            fx_ptr = tmp;
            var_ptr = tmp + W;

            size_t i = 0;
            while( start != end ) {
                block_type b = *start++;
                *fx_ptr &= b;
                *var_ptr |= b;
                if( ++i == row_grouped< P >::GROUP_SIZE ) {
                    ++fx_ptr;
                    ++var_ptr;
                    i = 0;
                }
            }

            assert(fx_ptr == tmp + W);

            k += row_grouped< P >::GROUP_SIZE;
        }

        size_t j = 0;

        fx_ptr = tmp;
        var_ptr = tmp + W;
        while( fx_ptr != tmp + W ) {
            block_type fx = *fx_ptr++;
            block_type var = *var_ptr++;

            block_type ls = ~(fx | var);

            while( fx ) {
                size_t b_idx = bit_walker_type::unset_next_index( fx ) + j;
                if( b_idx < M ) {
                    res[ fr++ ] = b_idx;
                    res[ fo++ ] = b_idx;
                }
            }

            while( ls ) {
                size_t idx = bit_walker_type::unset_next_index( ls ) + j;
                if( idx < M ) {
                    res[ fr++ ] = idx;
                    res[ lo++ ] = idx;
                }
            }

            j += bit_helper_type::BITS_PER_BLOCK;
        }

        fixed_offset = fo;
        lost_offset = lo;
        free_count = fr;

        delete [] tmp;
    }
};
/*
template < class BlockType >
class free_space_evaluator< association_matrix< BlockType, row_grouped< 1 > > > {
public:
    typedef association_matrix< BlockType, row_grouped< 1 > > space_type;
    typedef typename space_type::row_vector                 row_vector;
    typedef typename row_vector::block_type                 block_type;
    typedef size_t *                                        result_type;

    typedef typename row_vector::raw_pointer                raw_pointer;

    typedef clotho::utility::debruijn_bit_walker< block_type >  bit_walker_type;
    typedef clotho::utility::BitHelper< block_type >            bit_helper_type;

    free_space_evaluator() :
        tmp(NULL)
        , m_alloc_size(0)
    {}

    void operator()( space_type & ss, result_type res, size_t & fixed_offset, size_t & lost_offset, size_t & free_count, size_t M ) {

//        size_t W = bit_helper_type::padded_block_count( M );
        size_t W = ss.hard_block_count();

        resize( W );

        for( size_t i = 0; i < W; ++i ) {
            tmp[ 2 * i ] = bit_helper_type::ALL_SET;
            tmp[ 2 * i + 1 ] = bit_helper_type::ALL_UNSET;
        }

        std::set< raw_pointer > analyzed;

        const size_t row_count = ss.row_count();

        size_t fo = fixed_offset;
        size_t lo = lost_offset;
        size_t fr = free_count;

        size_t k = 0;
        while( k < row_count ) {

            raw_pointer start = ss.getRow( k ).get();

            if( analyzed.find( start ) == analyzed.end() ) {
                analyzed.insert( start );
                size_t  N = ss.getRow( k ).m_size;

#ifdef DEBUGGING
                assert( N <= W );
#endif // DEBUGGING

//                size_t i = 0;
//                while( i < N ) {
//                    block_type b = start[i];
//                    tmp[ 2 * i ] &= b;
//                    tmp[ 2 * i + 1 ] |= b;
//                    ++i;
//                }

                raw_pointer end = start + N;
                block_type * t = tmp;
                while( start != end ) {
                    *t++ &= *start;
                    *t++ |= *start++;
                }

                size_t i = N;
                while( i < W ) {
                    tmp[ 2 * i ] = bit_helper_type::ALL_UNSET;
                    ++i;
                }
            }

            ++k;
        }

#ifdef DEBUGGING
        std::cerr << "Free Space analyzed: " << analyzed.size() << std::endl;
#endif  // DEBUGGING

        size_t j = 0;

        for( unsigned int i = 0; i < W; ++i ) {
            block_type fx = tmp[ 2 *i ];
            block_type var = tmp[2 * i + 1];

            block_type ls = ~(fx | var);

            while( fx ) {
                size_t b_idx = bit_walker_type::unset_next_index( fx ) + j;
                if( b_idx < M ) {
                    res[ fr++ ] = b_idx;
                    res[ fo++ ] = b_idx;
                }
            }

            while( ls ) {
                size_t idx = bit_walker_type::unset_next_index( ls ) + j;
                if( idx < M ) {
                    res[ fr++ ] = idx;
                    res[ lo++ ] = idx;
                }
            }

            j += bit_helper_type::BITS_PER_BLOCK;
        }

        fixed_offset = fo;
        lost_offset = lo;
        free_count = fr;
    }

    virtual ~free_space_evaluator() {
        if( tmp != NULL ) {
            delete [] tmp;
        }
    }

protected:
    void resize( size_t W ) {
        if( 2 * W > m_alloc_size ) {
            if( tmp != NULL ) {
                delete [] tmp;
            }

            tmp = new block_type[ 2 * W ];
            m_alloc_size = 2 * W;
        }
    }
    block_type * tmp;
    size_t m_alloc_size;
};*/

/// this approach eliminates writing to the heap
/// utilizes stack buffers to write temporary data
template < class BlockType >
class free_space_evaluator< association_matrix< BlockType, row_grouped< 1 > > > {
public:
    typedef association_matrix< BlockType, row_grouped< 1 > > space_type;
    typedef typename space_type::row_vector                 row_vector;
    typedef typename row_vector::block_type                 block_type;
    typedef size_t *                                        result_type;

    typedef typename row_vector::raw_pointer                raw_pointer;

    typedef clotho::utility::debruijn_bit_walker< block_type >  bit_walker_type;
    typedef clotho::utility::BitHelper< block_type >            bit_helper_type;

    free_space_evaluator() {}

    void operator()( space_type & ss, result_type res, size_t & fixed_offset, size_t & lost_offset, size_t & free_count, size_t M ) {

        size_t W = ss.hard_block_count();

        std::map< raw_pointer, size_t > analyzed;
        typedef typename std::map< raw_pointer, size_t >::iterator iterator;

        const size_t row_count = ss.row_count();

        size_t fo = fixed_offset;
        size_t lo = lost_offset;
        size_t fr = free_count;

        // pre-scan population for all unique sequences
        size_t k = 0;
        while( k < row_count ) {
            raw_pointer start = ss.getRow( k ).get();
            size_t s = ss.getRow( k ).m_size;

            if( analyzed.find( start ) == analyzed.end() ) {
                analyzed[ start ] = s;
            }
            ++k;
        }

#ifdef DEBUGGING
        std::cerr << "Free Space analyzing: " << analyzed.size() << std::endl;
#endif  // DEBUGGING

        const size_t BUFFER_SIZE = 128;
        k = 0;
        while( k < W ) {
//            // 16 * sizeof(block_type) * 2 == 16 * 8 * 2 == 256 byte buffer
            block_type fx_buffer[ BUFFER_SIZE ];
            block_type var_buffer[ BUFFER_SIZE ];

            // initialize buffers
            for( int x = 0; x < BUFFER_SIZE; ++x ) {
                fx_buffer[ x ] = bit_helper_type::ALL_SET;
                var_buffer[ x ] = bit_helper_type::ALL_UNSET;
            }

            // analyze block columns
            for( iterator it = analyzed.begin(); it != analyzed.end(); it++ ) {
                size_t S = it->second;
                size_t N = ((k >= S)? 0 : (( k + BUFFER_SIZE <= S ) ? BUFFER_SIZE : (S - k)));
                size_t x = 0;
                raw_pointer r = it->first;
                while( x < N ) {
                    block_type b = r[ k + x ];
                    fx_buffer[ x ] &= b;
                    var_buffer[ x ] |= b;
                    ++x;
                }

                while( x < BUFFER_SIZE ) {
                    fx_buffer[ x ] = bit_helper_type::ALL_UNSET;
                    ++x;
                }
            }

            // write results
            size_t j = k * bit_helper_type::BITS_PER_BLOCK;

            for( unsigned int i = 0; i < BUFFER_SIZE; ++i ) {
                block_type fx = fx_buffer[ i ];
                block_type var = var_buffer[ i ];

                block_type ls = ~(fx | var);

                while( fx ) {
                    size_t b_idx = bit_walker_type::unset_next_index( fx ) + j;
                    if( b_idx < M ) {
                        res[ fr++ ] = b_idx;
                        res[ fo++ ] = b_idx;
                    }
                }

                while( ls ) {
                    size_t idx = bit_walker_type::unset_next_index( ls ) + j;
                    if( idx < M ) {
                        res[ fr++ ] = idx;
                        res[ lo++ ] = idx;
                    }
                }

                j += bit_helper_type::BITS_PER_BLOCK;
            }

            k += BUFFER_SIZE;
        }

        fixed_offset = fo;
        lost_offset = lo;
        free_count = fr;
    }

    virtual ~free_space_evaluator() { }
};

}   // namespace genetics
}   // namespace clotho

#endif  // CLOTHO_ROW_GROUPED_FREE_SPACE_EVALUATOR_HPP_
<|endoftext|>