<commit_before>/******************************************************************************
*       SOFA, Simulation Open-Framework Architecture, version 1.0 beta 4      *
*                (c) 2006-2009 MGH, INRIA, USTL, UJF, CNRS                    *
*                                                                             *
* This library is free software; you can redistribute it and/or modify it     *
* under the terms of the GNU Lesser General Public License as published by    *
* the Free Software Foundation; either version 2.1 of the License, or (at     *
* your option) any later version.                                             *
*                                                                             *
* This library is distributed in the hope that it will be useful, but WITHOUT *
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or       *
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
* for more details.                                                           *
*                                                                             *
* You should have received a copy of the GNU Lesser General Public License    *
* along with this library; if not, write to the Free Software Foundation,     *
* Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA.          *
*******************************************************************************
*                               SOFA :: Modules                               *
*                                                                             *
* Authors: The SOFA Team and external contributors (see Authors.txt)          *
*                                                                             *
* Contact information: contact@sofa-framework.org                             *
******************************************************************************/

#include <sofa/component/constraintset/LMConstraintDirectSolver.h>
#include <sofa/component/constraintset/ContactDescription.h>
#include <sofa/core/ObjectFactory.h>
#include <Eigen/QR>
#include <Eigen/SVD>

namespace sofa
{

namespace component
{

namespace constraintset
{

LMConstraintDirectSolver::LMConstraintDirectSolver():
    solverAlgorithm(initData(&solverAlgorithm, "solverAlgorithm", "Algorithm used to solve the system W.Lambda=c"))
{
    sofa::helper::OptionsGroup algo(2,"SVD","QR");
    solverAlgorithm.setValue(algo);
}

bool LMConstraintDirectSolver::buildSystem(double dt, VecId id, core::behavior::BaseConstraintSet::ConstOrder order)
{
    bool sucess=LMConstraintSolver::buildSystem(dt,id,order);

    return sucess;
}

bool LMConstraintDirectSolver::solveSystem(double dt, VecId id, core::behavior::BaseConstraintSet::ConstOrder order)
{
    //First, do n iterations of Gauss Seidel
    bool success=LMConstraintSolver::solveSystem(dt,id,order);

    if (order != core::behavior::BaseConstraintSet::VEL) return success;

    //Then process to a direct solution of the system

    //We need to find all the constraint related to contact
    // 1. extract the information about the state of the contact and build the new L, L^T matrices
    // 2. build the full system
    // 3. solve

    //------------------------------------------------------------------
    // extract the information about the state of the contact
    //------------------------------------------------------------------

    //************************************************************
#ifdef SOFA_DUMP_VISITOR_INFO
    sofa::simulation::Visitor::printNode("AnalyseConstraints");
#endif
    const helper::vector< BaseLMConstraint* > &LMConstraints=LMConstraintVisitor.getConstraints();

    JacobianRows rowsL ; rowsL.reserve(numConstraint);
    JacobianRows rowsLT; rowsLT.reserve(numConstraint);
    helper::vector< unsigned int > rightHandElements;

    analyseConstraints(LMConstraints, order,
            rowsL, rowsLT, rightHandElements);

#ifdef SOFA_DUMP_VISITOR_INFO
    sofa::simulation::Visitor::printCloseNode("AnalyseConstraints");
#endif
    if (rowsL.empty() || rowsLT.empty()) return success;

#ifdef SOFA_DUMP_VISITOR_INFO
    sofa::simulation::Visitor::printNode("BuildFullSystem");
#endif
    //------------------------------------------------------------------
    // build c: right hand term
    //------------------------------------------------------------------
    VectorEigen previousC(c);
    //TODO: change newC by c
    c=VectorEigen::Zero(rowsL.size());
    unsigned int idx=0;
    for (helper::vector<unsigned int >::const_iterator it=rightHandElements.begin(); it!=rightHandElements.end(); ++it)
        c[idx++]=previousC[*it];

    //------------------------------------------------------------------
    // build the L and LT matrices
    //------------------------------------------------------------------

    DofToMatrix LMatricesDirectSolver;
    DofToMatrix LTMatricesDirectSolver;
    for (DofToMatrix::iterator it=LMatrices.begin(); it!=LMatrices.end(); ++it)
    {
        //------------------------------------------------------------------
        const SparseMatrixEigen& matrix= it->second;
        //Init the manipulator with the full matrix
        linearsolver::LMatrixManipulator manip;
        manip.init(matrix);

        //------------------------------------------------------------------
        SparseMatrixEigen  L (rowsL.size(),  matrix.cols());
        L.startFill(rowsL.size()*matrix.cols());
        manip.buildLMatrix(rowsL ,L);
        L.endFill();
        LMatricesDirectSolver.insert (std::make_pair(it->first,L ));

        //------------------------------------------------------------------
        SparseMatrixEigen  LT(rowsLT.size(), matrix.cols());
        LT.startFill(rowsLT.size()*matrix.cols());
        manip.buildLMatrix(rowsLT,LT);
        LT.endFill();
        LTMatricesDirectSolver.insert(std::make_pair(it->first,LT));
    }

    //------------------------------------------------------------------
    // build the full system
    //------------------------------------------------------------------
    const  int rows=rowsL.size();
    const  int cols=rowsLT.size();
    SparseColMajorMatrixEigen Wsparse(rows,cols);
    buildLeftRectangularMatrix(invMassMatrix, LMatricesDirectSolver, LTMatricesDirectSolver, Wsparse,invMass_Ltrans);

    //------------------------------------------------------------------
    // conversion from sparse to dense matrix
    //------------------------------------------------------------------
    Lambda=VectorEigen::Zero(rows);

    W=MatrixEigen::Zero(rows,cols);

    SparseMatrixEigen Wresult(Wsparse);
    for (int k=0; k<Wresult.outerSize(); ++k)
        for (SparseMatrixEigen::InnerIterator it(Wresult,k); it; ++it) W(it.row(),it.col()) = it.value();

#ifdef SOFA_DUMP_VISITOR_INFO
    sofa::simulation::Visitor::printCloseNode("BuildFullSystem");
#endif

    //------------------------------------------------------------------
    // Solve the system
    //------------------------------------------------------------------
    const std::string &algo=solverAlgorithm.getValue().getSelectedItem() ;
#ifdef SOFA_DUMP_VISITOR_INFO
    simulation::Visitor::TRACE_ARGUMENT arg1;
    arg1.push_back(std::make_pair("Algorithm", algo));
    arg1.push_back(std::make_pair("Dimension", printDimension(W)));
    sofa::simulation::Visitor::printNode("DirectSolveSystem", "",arg1);
#endif
    if (algo == "QR")
    {
        Eigen::QR< MatrixEigen > solverQR(W);
        if (this->f_printLog.getValue()) sout << printDimension(W) << " is W invertible? " << std::boolalpha << solverQR.isInvertible()  << sendl;
        if (solverQR.isInvertible())
            solverQR.solve(c, &Lambda);
        else
        {
            if (this->f_printLog.getValue()) sout << "Fallback on SVD decomposition" << sendl;
            Eigen::SVD< MatrixEigen > solverSVD(W);
            solverSVD.solve(c, &Lambda);
        }

    }
    else if(algo == "SVD")
    {
        Eigen::SVD< MatrixEigen > solverSVD(W);
        solverSVD.solve(c, &Lambda);
    }

    if (this->f_printLog.getValue())
    {
        sout << "W" <<  printDimension(W) <<  "  Lambda" << printDimension(Lambda) << "  c" << printDimension(c) << sendl;
        sout << "\nW     ===============================================\n" << W
                <<  "\nLambda===============================================\n" << Lambda
                <<  "\nc     ===============================================\n" << c << sendl;
    }

#ifdef SOFA_DUMP_VISITOR_INFO
    sofa::simulation::Visitor::printCloseNode("DirectSolveSystem");
#endif
    return success;

}

void LMConstraintDirectSolver::analyseConstraints(const helper::vector< BaseLMConstraint* > &LMConstraints, core::behavior::BaseConstraintSet::ConstOrder order,
        JacobianRows &rowsL,JacobianRows &rowsLT, helper::vector< unsigned int > &rightHandElements) const
{
    //Iterate among all the Sofa LMConstraint
    for (unsigned int componentConstraint=0; componentConstraint<LMConstraints.size(); ++componentConstraint)
    {
        BaseLMConstraint *constraint=LMConstraints[componentConstraint];
        //Find the constraint dealing with contact
        if (ContactDescriptionHandler* contactDescriptor=dynamic_cast<ContactDescriptionHandler*>(constraint))
        {
            const helper::vector< BaseLMConstraint::ConstraintGroup* > &constraintOrder=constraint->getConstraintsOrder(order);
            //Iterate among all the contacts
            for (helper::vector< BaseLMConstraint::ConstraintGroup* >::const_iterator itGroup=constraintOrder.begin(); itGroup!=constraintOrder.end(); ++itGroup)
            {
                const BaseLMConstraint::ConstraintGroup* group=*itGroup;
                const ContactDescription& contact=contactDescriptor->getContactDescription(group);

                const unsigned int idxEquation=group->getConstraint(0).idx;

                switch(contact.state)
                {
                case VANISHING:
                {
//                    serr <<"Constraint " << idxEquation << " VANISHING" << sendl;
                    //0 equation
                    break;
                }
                case STICKING:
                {
//                    serr << "Constraint " <<idxEquation << " STICKING" << sendl;
                    const unsigned int i=rowsL.size();
                    rowsL.push_back(linearsolver::LLineManipulator().addCombination(idxEquation  ));
                    rowsL.push_back(linearsolver::LLineManipulator().addCombination(idxEquation+1));
                    rowsL.push_back(linearsolver::LLineManipulator().addCombination(idxEquation+2));

                    //3 equations
                    rowsLT.push_back(rowsL[i  ]);
                    rowsLT.push_back(rowsL[i+1]);
                    rowsLT.push_back(rowsL[i+2]);

                    rightHandElements.push_back(idxEquation  );
                    rightHandElements.push_back(idxEquation+1);
                    rightHandElements.push_back(idxEquation+2);
                    break;
                }
                case SLIDING:
                {
//                    serr << "Constraint " <<idxEquation << " SLIDING" << sendl;
                    rowsL.push_back(linearsolver::LLineManipulator().addCombination(idxEquation  ));
                    rowsL.push_back(linearsolver::LLineManipulator().addCombination(idxEquation+1));
                    rowsL.push_back(linearsolver::LLineManipulator().addCombination(idxEquation+2));

                    //1 equation with the response force along the Coulomb friction cone
                    rowsLT.push_back(linearsolver::LLineManipulator()
                            .addCombination(idxEquation  ,contact.coeff[0])
                            .addCombination(idxEquation+1,contact.coeff[1])
                            .addCombination(idxEquation+2,contact.coeff[2]));

                    rightHandElements.push_back(idxEquation  );
                    rightHandElements.push_back(idxEquation+1);
                    rightHandElements.push_back(idxEquation+2);
                    break;
                }
                }
            }
        }
        else
        {
            //Non contact constraints: we add all the equations
            const helper::vector< BaseLMConstraint::ConstraintGroup* > &constraintOrder=constraint->getConstraintsOrder(order);
            for (helper::vector< BaseLMConstraint::ConstraintGroup* >::const_iterator itGroup=constraintOrder.begin(); itGroup!=constraintOrder.end(); ++itGroup)
            {
                const BaseLMConstraint::ConstraintGroup* group=*itGroup;
                std::pair< BaseLMConstraint::ConstraintGroup::EquationConstIterator,BaseLMConstraint::ConstraintGroup::EquationConstIterator> range=group->data();
                for ( BaseLMConstraint::ConstraintGroup::EquationConstIterator it=range.first; it!=range.second; ++it)
                {
                    rowsL.push_back(linearsolver::LLineManipulator().addCombination(it->idx));
                    rowsLT.push_back(rowsL.back());
                }
            }
        }
    }
}

void LMConstraintDirectSolver::buildLeftRectangularMatrix(const DofToMatrix& invMassMatrix,
        DofToMatrix& LMatrix, DofToMatrix& LTMatrix,
        SparseColMajorMatrixEigen &LeftMatrix, DofToMatrix &invMass_Ltrans) const
{
    invMass_Ltrans.clear();
    for (SetDof::const_iterator itDofs=setDofs.begin(); itDofs!=setDofs.end(); ++itDofs)
    {
        const sofa::core::behavior::BaseMechanicalState* dofs=*itDofs;
        const SparseMatrixEigen &invMass=invMassMatrix.find(dofs)->second;
        SparseMatrixEigen &L =LMatrix[dofs];  L.endFill();
        SparseMatrixEigen &LT=LTMatrix[dofs]; LT.endFill();

        const SparseMatrixEigen &invMass_LT=invMass.marked<Eigen::SelfAdjoint|Eigen::UpperTriangular>()*LT.transpose();
        invMass_Ltrans.insert(std::make_pair(dofs, invMass_LT));
        const SparseColMajorMatrixEigen& temp=L*invMass_LT;
        LeftMatrix += temp;
    }
}
int LMConstraintDirectSolverClass = core::RegisterObject("A Direct Constraint Solver working specifically with LMConstraint based components")
        .add< LMConstraintDirectSolver >();

SOFA_DECL_CLASS(LMConstraintDirectSolver);

} // namespace constraintset

} // namespace component

} // namespace sofa
<commit_msg>r7951/sofa-dev : FIX: bug in DirectSolver<commit_after>/******************************************************************************
*       SOFA, Simulation Open-Framework Architecture, version 1.0 beta 4      *
*                (c) 2006-2009 MGH, INRIA, USTL, UJF, CNRS                    *
*                                                                             *
* This library is free software; you can redistribute it and/or modify it     *
* under the terms of the GNU Lesser General Public License as published by    *
* the Free Software Foundation; either version 2.1 of the License, or (at     *
* your option) any later version.                                             *
*                                                                             *
* This library is distributed in the hope that it will be useful, but WITHOUT *
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or       *
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
* for more details.                                                           *
*                                                                             *
* You should have received a copy of the GNU Lesser General Public License    *
* along with this library; if not, write to the Free Software Foundation,     *
* Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA.          *
*******************************************************************************
*                               SOFA :: Modules                               *
*                                                                             *
* Authors: The SOFA Team and external contributors (see Authors.txt)          *
*                                                                             *
* Contact information: contact@sofa-framework.org                             *
******************************************************************************/

#include <sofa/component/constraintset/LMConstraintDirectSolver.h>
#include <sofa/component/constraintset/ContactDescription.h>
#include <sofa/core/ObjectFactory.h>
#include <Eigen/QR>
#include <Eigen/SVD>

namespace sofa
{

namespace component
{

namespace constraintset
{

LMConstraintDirectSolver::LMConstraintDirectSolver():
    solverAlgorithm(initData(&solverAlgorithm, "solverAlgorithm", "Algorithm used to solve the system W.Lambda=c"))
{
    sofa::helper::OptionsGroup algo(2,"SVD","QR");
    solverAlgorithm.setValue(algo);
}

bool LMConstraintDirectSolver::buildSystem(double dt, VecId id, core::behavior::BaseConstraintSet::ConstOrder order)
{
    bool sucess=LMConstraintSolver::buildSystem(dt,id,order);

    return sucess;
}

bool LMConstraintDirectSolver::solveSystem(double dt, VecId id, core::behavior::BaseConstraintSet::ConstOrder order)
{
    //First, do n iterations of Gauss Seidel
    bool success=LMConstraintSolver::solveSystem(dt,id,order);

    if (order != core::behavior::BaseConstraintSet::VEL) return success;

    //Then process to a direct solution of the system

    //We need to find all the constraint related to contact
    // 1. extract the information about the state of the contact and build the new L, L^T matrices
    // 2. build the full system
    // 3. solve

    //------------------------------------------------------------------
    // extract the information about the state of the contact
    //------------------------------------------------------------------

    //************************************************************
#ifdef SOFA_DUMP_VISITOR_INFO
    sofa::simulation::Visitor::printNode("AnalyseConstraints");
#endif
    const helper::vector< BaseLMConstraint* > &LMConstraints=LMConstraintVisitor.getConstraints();

    JacobianRows rowsL ; rowsL.reserve(numConstraint);
    JacobianRows rowsLT; rowsLT.reserve(numConstraint);
    helper::vector< unsigned int > rightHandElements;

    analyseConstraints(LMConstraints, order,
            rowsL, rowsLT, rightHandElements);

#ifdef SOFA_DUMP_VISITOR_INFO
    sofa::simulation::Visitor::printCloseNode("AnalyseConstraints");
#endif
    if (rowsL.empty() || rowsLT.empty()) return success;

#ifdef SOFA_DUMP_VISITOR_INFO
    sofa::simulation::Visitor::printNode("BuildFullSystem");
#endif
    //------------------------------------------------------------------
    // build c: right hand term
    //------------------------------------------------------------------
    VectorEigen previousC(c);
    //TODO: change newC by c
    c=VectorEigen::Zero(rowsL.size());
    unsigned int idx=0;
    for (helper::vector<unsigned int >::const_iterator it=rightHandElements.begin(); it!=rightHandElements.end(); ++it)
        c[idx++]=previousC[*it];

    //------------------------------------------------------------------
    // build the L and LT matrices
    //------------------------------------------------------------------

    DofToMatrix LMatricesDirectSolver;
    DofToMatrix LTMatricesDirectSolver;
    for (DofToMatrix::iterator it=LMatrices.begin(); it!=LMatrices.end(); ++it)
    {
        //------------------------------------------------------------------
        const SparseMatrixEigen& matrix= it->second;
        //Init the manipulator with the full matrix
        linearsolver::LMatrixManipulator manip;
        manip.init(matrix);

        //------------------------------------------------------------------
        SparseMatrixEigen  L (rowsL.size(),  matrix.cols());
        L.startFill(rowsL.size()*matrix.cols());
        manip.buildLMatrix(rowsL ,L);
        L.endFill();
        LMatricesDirectSolver.insert (std::make_pair(it->first,L ));

        //------------------------------------------------------------------
        SparseMatrixEigen  LT(rowsLT.size(), matrix.cols());
        LT.startFill(rowsLT.size()*matrix.cols());
        manip.buildLMatrix(rowsLT,LT);
        LT.endFill();
        LTMatricesDirectSolver.insert(std::make_pair(it->first,LT));
    }

    //------------------------------------------------------------------
    // build the full system
    //------------------------------------------------------------------
    const  int rows=rowsL.size();
    const  int cols=rowsLT.size();
    SparseColMajorMatrixEigen Wsparse(rows,cols);
    buildLeftRectangularMatrix(invMassMatrix, LMatricesDirectSolver, LTMatricesDirectSolver, Wsparse,invMass_Ltrans);

    //------------------------------------------------------------------
    // conversion from sparse to dense matrix
    //------------------------------------------------------------------
    Lambda=VectorEigen::Zero(rows);

    W=MatrixEigen::Zero(rows,cols);

    SparseMatrixEigen Wresult(Wsparse);
    for (int k=0; k<Wresult.outerSize(); ++k)
        for (SparseMatrixEigen::InnerIterator it(Wresult,k); it; ++it) W(it.row(),it.col()) = it.value();

#ifdef SOFA_DUMP_VISITOR_INFO
    sofa::simulation::Visitor::printCloseNode("BuildFullSystem");
#endif

    //------------------------------------------------------------------
    // Solve the system
    //------------------------------------------------------------------
    const std::string &algo=solverAlgorithm.getValue().getSelectedItem() ;
#ifdef SOFA_DUMP_VISITOR_INFO
    simulation::Visitor::TRACE_ARGUMENT arg1;
    arg1.push_back(std::make_pair("Algorithm", algo));
    arg1.push_back(std::make_pair("Dimension", printDimension(W)));
    sofa::simulation::Visitor::printNode("DirectSolveSystem", "",arg1);
#endif
    if (algo == "QR")
    {
        Eigen::QR< MatrixEigen > solverQR(W);
        if (this->f_printLog.getValue()) sout << printDimension(W) << " is W invertible? " << std::boolalpha << solverQR.isInvertible()  << sendl;
        if (solverQR.isInvertible())
            solverQR.solve(c, &Lambda);
        else
        {
            if (this->f_printLog.getValue()) sout << "Fallback on SVD decomposition" << sendl;
            Eigen::SVD< MatrixEigen > solverSVD(W);
            solverSVD.solve(c, &Lambda);
        }

    }
    else if(algo == "SVD")
    {
        Eigen::SVD< MatrixEigen > solverSVD(W);
        solverSVD.solve(c, &Lambda);
    }

    if (this->f_printLog.getValue())
    {
        sout << "W" <<  printDimension(W) <<  "  Lambda" << printDimension(Lambda) << "  c" << printDimension(c) << sendl;
        sout << "\nW     ===============================================\n" << W
                <<  "\nLambda===============================================\n" << Lambda
                <<  "\nc     ===============================================\n" << c << sendl;
    }

#ifdef SOFA_DUMP_VISITOR_INFO
    sofa::simulation::Visitor::printCloseNode("DirectSolveSystem");
#endif
    return success;

}

void LMConstraintDirectSolver::analyseConstraints(const helper::vector< BaseLMConstraint* > &LMConstraints, core::behavior::BaseConstraintSet::ConstOrder order,
        JacobianRows &rowsL,JacobianRows &rowsLT, helper::vector< unsigned int > &rightHandElements) const
{
    //Iterate among all the Sofa LMConstraint
    for (unsigned int componentConstraint=0; componentConstraint<LMConstraints.size(); ++componentConstraint)
    {
        BaseLMConstraint *constraint=LMConstraints[componentConstraint];
        //Find the constraint dealing with contact
        if (ContactDescriptionHandler* contactDescriptor=dynamic_cast<ContactDescriptionHandler*>(constraint))
        {
            const helper::vector< BaseLMConstraint::ConstraintGroup* > &constraintOrder=constraint->getConstraintsOrder(order);
            //Iterate among all the contacts
            for (helper::vector< BaseLMConstraint::ConstraintGroup* >::const_iterator itGroup=constraintOrder.begin(); itGroup!=constraintOrder.end(); ++itGroup)
            {
                const BaseLMConstraint::ConstraintGroup* group=*itGroup;
                const ContactDescription& contact=contactDescriptor->getContactDescription(group);

                const unsigned int idxEquation=group->getConstraint(0).idx;

                switch(contact.state)
                {
                case VANISHING:
                {
//                    serr <<"Constraint " << idxEquation << " VANISHING" << sendl;
                    //0 equation
                    break;
                }
                case STICKING:
                {
//                    serr << "Constraint " <<idxEquation << " STICKING" << sendl;
                    const unsigned int i=rowsL.size();
                    rowsL.push_back(linearsolver::LLineManipulator().addCombination(idxEquation  ));
                    rowsL.push_back(linearsolver::LLineManipulator().addCombination(idxEquation+1));
                    rowsL.push_back(linearsolver::LLineManipulator().addCombination(idxEquation+2));

                    //3 equations
                    rowsLT.push_back(rowsL[i  ]);
                    rowsLT.push_back(rowsL[i+1]);
                    rowsLT.push_back(rowsL[i+2]);

                    rightHandElements.push_back(idxEquation  );
                    rightHandElements.push_back(idxEquation+1);
                    rightHandElements.push_back(idxEquation+2);
                    break;
                }
                case SLIDING:
                {
//                    serr << "Constraint " <<idxEquation << " SLIDING" << sendl;
                    rowsL.push_back(linearsolver::LLineManipulator().addCombination(idxEquation  ));
                    rowsL.push_back(linearsolver::LLineManipulator().addCombination(idxEquation+1));
                    rowsL.push_back(linearsolver::LLineManipulator().addCombination(idxEquation+2));

                    //1 equation with the response force along the Coulomb friction cone
                    rowsLT.push_back(linearsolver::LLineManipulator()
                            .addCombination(idxEquation  ,contact.coeff[0])
                            .addCombination(idxEquation+1,contact.coeff[1])
                            .addCombination(idxEquation+2,contact.coeff[2]));

                    rightHandElements.push_back(idxEquation  );
                    rightHandElements.push_back(idxEquation+1);
                    rightHandElements.push_back(idxEquation+2);
                    break;
                }
                }
            }
        }
        else
        {
            //Non contact constraints: we add all the equations
            const helper::vector< BaseLMConstraint::ConstraintGroup* > &constraintOrder=constraint->getConstraintsOrder(order);
            for (helper::vector< BaseLMConstraint::ConstraintGroup* >::const_iterator itGroup=constraintOrder.begin(); itGroup!=constraintOrder.end(); ++itGroup)
            {
                const BaseLMConstraint::ConstraintGroup* group=*itGroup;
                std::pair< BaseLMConstraint::ConstraintGroup::EquationConstIterator,BaseLMConstraint::ConstraintGroup::EquationConstIterator> range=group->data();
                for ( BaseLMConstraint::ConstraintGroup::EquationConstIterator it=range.first; it!=range.second; ++it)
                {
                    rowsL.push_back(linearsolver::LLineManipulator().addCombination(it->idx));
                    rowsLT.push_back(rowsL.back());
                    rightHandElements.push_back(it->idx);
                }
            }
        }
    }
}

void LMConstraintDirectSolver::buildLeftRectangularMatrix(const DofToMatrix& invMassMatrix,
        DofToMatrix& LMatrix, DofToMatrix& LTMatrix,
        SparseColMajorMatrixEigen &LeftMatrix, DofToMatrix &invMass_Ltrans) const
{
    invMass_Ltrans.clear();
    for (SetDof::const_iterator itDofs=setDofs.begin(); itDofs!=setDofs.end(); ++itDofs)
    {
        const sofa::core::behavior::BaseMechanicalState* dofs=*itDofs;
        const SparseMatrixEigen &invMass=invMassMatrix.find(dofs)->second;
        SparseMatrixEigen &L =LMatrix[dofs];  L.endFill();
        SparseMatrixEigen &LT=LTMatrix[dofs]; LT.endFill();

        const SparseMatrixEigen &invMass_LT=invMass.marked<Eigen::SelfAdjoint|Eigen::UpperTriangular>()*LT.transpose();
        invMass_Ltrans.insert(std::make_pair(dofs, invMass_LT));
        const SparseColMajorMatrixEigen& temp=L*invMass_LT;
        LeftMatrix += temp;
    }
}
int LMConstraintDirectSolverClass = core::RegisterObject("A Direct Constraint Solver working specifically with LMConstraint based components")
        .add< LMConstraintDirectSolver >();

SOFA_DECL_CLASS(LMConstraintDirectSolver);

} // namespace constraintset

} // namespace component

} // namespace sofa
<|endoftext|>


<commit_before>/*******************************************************************************
 * BSD 3-Clause License
 *
 * Copyright (c) 2019, Los Alamos National Security, LLC
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * * Redistributions of source code must retain the above copyright notice, this
 *   list of conditions and the following disclaimer.
 *
 * * Redistributions in binary form must reproduce the above copyright notice,
 *   this list of conditions and the following disclaimer in the documentation
 *   and/or other materials provided with the distribution.
 *
 * * Neither the name of the copyright holder nor the names of its
 *   contributors may be used to endorse or promote products derived from
 *   this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/

/*      Title     : collision_check_thread.cpp
 *      Project   : moveit_jog_arm
 *      Created   : 1/11/2019
 *      Author    : Brian O'Neil, Andy Zelenak, Blake Anderson
 */

#include <moveit_jog_arm/collision_check_thread.h>

static const std::string LOGNAME = "collision_check_thread";
static const double MIN_RECOMMENDED_COLLISION_RATE = 10;

namespace moveit_jog_arm
{
// Constructor for the class that handles collision checking
CollisionCheckThread::CollisionCheckThread(
    const moveit_jog_arm::JogArmParameters& parameters,
    const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor)
  : parameters_(parameters), planning_scene_monitor_(planning_scene_monitor)
{
  if (parameters_.collision_check_rate < MIN_RECOMMENDED_COLLISION_RATE)
    ROS_WARN_STREAM_THROTTLE_NAMED(5, LOGNAME, "Collision check rate is low, increase it in yaml file if CPU allows");
}

planning_scene_monitor::LockedPlanningSceneRO CollisionCheckThread::getLockedPlanningSceneRO() const
{
  return planning_scene_monitor::LockedPlanningSceneRO(planning_scene_monitor_);
}

void CollisionCheckThread::startMainLoop(JogArmShared& shared_variables)
{
  // Reset loop termination flag
  stop_requested_ = false;

  // Init collision request
  collision_detection::CollisionRequest collision_request;
  collision_request.group_name = parameters_.move_group_name;
  collision_detection::CollisionResult collision_result;

  // Copy the planning scene's version of current state into new memory
  moveit::core::RobotState current_state(getLockedPlanningSceneRO()->getCurrentState());

  double velocity_scale_coefficient = -log(0.001) / parameters_.collision_proximity_threshold;
  ros::Rate collision_rate(parameters_.collision_check_rate);

  double collision_distance = 0;

  // Scale robot velocity according to collision proximity and user-defined thresholds.
  // I scaled exponentially (cubic power) so velocity drops off quickly after the threshold.
  // Proximity decreasing --> decelerate
  double velocity_scale = 1;

  /////////////////////////////////////////////////
  // Spin while checking collisions
  /////////////////////////////////////////////////
  while (ros::ok() && !stop_requested_)
  {
    shared_variables.lock();
    sensor_msgs::JointState jts = shared_variables.joints;
    shared_variables.unlock();

    for (std::size_t i = 0; i < jts.position.size(); ++i)
      current_state.setJointPositions(jts.name[i], &jts.position[i]);

    collision_result.clear();
    current_state.updateCollisionBodyTransforms();
    // Do a thread-safe distance-based collision detection
    collision_request.distance = true;
    getLockedPlanningSceneRO()->checkCollision(collision_request, collision_result, current_state);
    collision_distance = collision_result.distance;

    // Do a binary collision detection (helps with strange edge cases like being in collision initially)
    collision_request.distance = false;
    getLockedPlanningSceneRO()->checkCollision(collision_request, collision_result, current_state);

    // If we're definitely in collision, stop immediately
    if (collision_result.collision)
    {
      velocity_scale = 0;
    }

    // If we are far from a collision, velocity_scale should be 1.
    // If we are very close to a collision, velocity_scale should be ~zero.
    // When collision_proximity_threshold is breached, start decelerating exponentially.
    else if (collision_distance < parameters_.collision_proximity_threshold)
    {
      // velocity_scale = e ^ k * (collision_distance - threshold)
      // k = - ln(0.001) / collision_proximity_threshold
      // velocity_scale should equal one when collision_distance is at collision_proximity_threshold.
      // velocity_scale should equal 0.001 when collision_distance is at zero.
      velocity_scale =
          exp(velocity_scale_coefficient * (collision_distance - parameters_.collision_proximity_threshold));
    }

    shared_variables.lock();
    shared_variables.collision_velocity_scale = velocity_scale;
    shared_variables.unlock();

    collision_rate.sleep();
  }
}

void CollisionCheckThread::stopMainLoop()
{
  stop_requested_ = true;
}
}  // namespace moveit_jog_arm
<commit_msg>Remove duplicate collision check in JogArm (#1986)<commit_after>/*******************************************************************************
 * BSD 3-Clause License
 *
 * Copyright (c) 2019, Los Alamos National Security, LLC
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * * Redistributions of source code must retain the above copyright notice, this
 *   list of conditions and the following disclaimer.
 *
 * * Redistributions in binary form must reproduce the above copyright notice,
 *   this list of conditions and the following disclaimer in the documentation
 *   and/or other materials provided with the distribution.
 *
 * * Neither the name of the copyright holder nor the names of its
 *   contributors may be used to endorse or promote products derived from
 *   this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/

/*      Title     : collision_check_thread.cpp
 *      Project   : moveit_jog_arm
 *      Created   : 1/11/2019
 *      Author    : Brian O'Neil, Andy Zelenak, Blake Anderson
 */

#include <moveit_jog_arm/collision_check_thread.h>

static const std::string LOGNAME = "collision_check_thread";
static const double MIN_RECOMMENDED_COLLISION_RATE = 10;

namespace moveit_jog_arm
{
// Constructor for the class that handles collision checking
CollisionCheckThread::CollisionCheckThread(
    const moveit_jog_arm::JogArmParameters& parameters,
    const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor)
  : parameters_(parameters), planning_scene_monitor_(planning_scene_monitor)
{
  if (parameters_.collision_check_rate < MIN_RECOMMENDED_COLLISION_RATE)
    ROS_WARN_STREAM_THROTTLE_NAMED(5, LOGNAME, "Collision check rate is low, increase it in yaml file if CPU allows");
}

planning_scene_monitor::LockedPlanningSceneRO CollisionCheckThread::getLockedPlanningSceneRO() const
{
  return planning_scene_monitor::LockedPlanningSceneRO(planning_scene_monitor_);
}

void CollisionCheckThread::startMainLoop(JogArmShared& shared_variables)
{
  // Reset loop termination flag
  stop_requested_ = false;

  // Init collision request
  collision_detection::CollisionRequest collision_request;
  collision_request.group_name = parameters_.move_group_name;
  collision_request.distance = true;  // enable distance-based collision checking
  collision_detection::CollisionResult collision_result;

  // Copy the planning scene's version of current state into new memory
  moveit::core::RobotState current_state(getLockedPlanningSceneRO()->getCurrentState());

  double velocity_scale_coefficient = -log(0.001) / parameters_.collision_proximity_threshold;
  ros::Rate collision_rate(parameters_.collision_check_rate);

  // Scale robot velocity according to collision proximity and user-defined thresholds.
  // I scaled exponentially (cubic power) so velocity drops off quickly after the threshold.
  // Proximity decreasing --> decelerate
  double velocity_scale = 1;

  /////////////////////////////////////////////////
  // Spin while checking collisions
  /////////////////////////////////////////////////
  while (ros::ok() && !stop_requested_)
  {
    shared_variables.lock();
    sensor_msgs::JointState jts = shared_variables.joints;
    shared_variables.unlock();

    for (std::size_t i = 0; i < jts.position.size(); ++i)
      current_state.setJointPositions(jts.name[i], &jts.position[i]);

    collision_result.clear();
    current_state.updateCollisionBodyTransforms();

    // Do a thread-safe distance-based collision detection
    getLockedPlanningSceneRO()->checkCollision(collision_request, collision_result, current_state);

    // If we're definitely in collision, stop immediately
    if (collision_result.collision)
    {
      velocity_scale = 0;
    }

    // If we are far from a collision, velocity_scale should be 1.
    // If we are very close to a collision, velocity_scale should be ~zero.
    // When collision_proximity_threshold is breached, start decelerating exponentially.
    else if (collision_result.distance < parameters_.collision_proximity_threshold)
    {
      // velocity_scale = e ^ k * (collision_result.distance - threshold)
      // k = - ln(0.001) / collision_proximity_threshold
      // velocity_scale should equal one when collision_result.distance is at collision_proximity_threshold.
      // velocity_scale should equal 0.001 when collision_result.distance is at zero.
      velocity_scale =
          exp(velocity_scale_coefficient * (collision_result.distance - parameters_.collision_proximity_threshold));
    }

    shared_variables.lock();
    shared_variables.collision_velocity_scale = velocity_scale;
    shared_variables.unlock();

    collision_rate.sleep();
  }
}

void CollisionCheckThread::stopMainLoop()
{
  stop_requested_ = true;
}
}  // namespace moveit_jog_arm
<|endoftext|>


<commit_before>/*
 * Copyright (c) 2008, Willow Garage, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Willow Garage, Inc. nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

/* Author: Ioan Sucan */

#include <moveit/point_containment_filter/shape_mask.h>
#include <geometric_shapes/body_operations.h>
#include <ros/console.h>

point_containment_filter::ShapeMask::ShapeMask(const TransformCallback& transform_callback) :
  transform_callback_(transform_callback),
  next_handle_ (1),
  min_handle_ (1)
{
}

point_containment_filter::ShapeMask::~ShapeMask()
{
  freeMemory();
}

void point_containment_filter::ShapeMask::freeMemory()
{
  for (std::set<SeeShape>::const_iterator it = bodies_.begin() ; it != bodies_.end() ; ++it)
    delete it->body;
  bodies_.clear();
}

point_containment_filter::ShapeHandle point_containment_filter::ShapeMask::addShape(const shapes::ShapeConstPtr &shape, double scale, double padding)
{
  SeeShape ss;
  ss.body = bodies::createBodyFromShape(shape.get());
  if (ss.body)
  {
    ss.body->setScale(scale);
    ss.body->setPadding(padding);
    ss.volume = ss.body->computeVolume();
    ss.handle = next_handle_;
    used_handles_[next_handle_] = bodies_.insert(ss).first;
  }
  else
    return 0;

  ShapeHandle ret = next_handle_;
  const std::size_t sz = min_handle_ + bodies_.size() + 1;
  for (std::size_t i = min_handle_ ; i < sz ; ++i)
    if (used_handles_.find(i) == used_handles_.end())
    {
      next_handle_ = i;
      break;
    }
  min_handle_ = next_handle_;

  return ret;
}

void point_containment_filter::ShapeMask::removeShape(ShapeHandle handle)
{
  std::map<ShapeHandle, std::set<SeeShape, SortBodies>::iterator>::iterator it = used_handles_.find(handle);
  if (it != used_handles_.end())
  {
    delete it->second->body;
    bodies_.erase(it->second);
    used_handles_.erase(it);
    min_handle_ = handle;
  }
}

void point_containment_filter::ShapeMask::maskContainment(const pcl::PointCloud<pcl::PointXYZ>& data_in,
                                                          const Eigen::Vector3d &sensor_origin,
                                                          const double min_sensor_dist, const double max_sensor_dist,
                                                          std::vector<int> &mask)
{
  mask.resize(data_in.points.size());
  if (bodies_.empty())
    std::fill(mask.begin(), mask.end(), (int)OUTSIDE);
  else
  {
    Eigen::Affine3d tmp;
    bspheres_.resize(bodies_.size());
    std::size_t j = 0;
    for (std::set<SeeShape>::const_iterator it = bodies_.begin() ; it != bodies_.end() ; ++it)
    {
      if (transform_callback_(it->handle, tmp))
      {
        it->body->setPose(tmp);
        it->body->computeBoundingSphere(bspheres_[j++]);
      }
    }

    const unsigned int np = data_in.points.size();

    // compute a sphere that bounds the entire robot
    bodies::BoundingSphere bound;
    bodies::mergeBoundingSpheres(bspheres_, bound);
    const double radiusSquared = bound.radius * bound.radius;

    // we now decide which points we keep
#pragma omp parallel for schedule(dynamic)
    for (int i = 0 ; i < (int)np ; ++i)
    {
      Eigen::Vector3d pt = Eigen::Vector3d(data_in.points[i].x, data_in.points[i].y, data_in.points[i].z);
      int out = OUTSIDE;
      if ((bound.center - pt).squaredNorm() < radiusSquared)
      {
        double d = (sensor_origin - pt).norm();
        if (d < min_sensor_dist || d > max_sensor_dist)
        {
          out = CLIP;
        }
        else
          for (std::set<SeeShape>::const_iterator it = bodies_.begin() ; it != bodies_.end() && out == OUTSIDE ; ++it)
          {
            if (it->body->containsPoint(pt))
            {
              out = INSIDE;
            }
          }
      }
      mask[i] = out;
    }
  }
}

int point_containment_filter::ShapeMask::getMaskContainment(const Eigen::Vector3d &pt) const
{
  int out = OUTSIDE;
  for (std::set<SeeShape>::const_iterator it = bodies_.begin() ; it != bodies_.end() && out == OUTSIDE ; ++it)
    if (it->body->containsPoint(pt))
      out = INSIDE;
  return out;
}

int point_containment_filter::ShapeMask::getMaskContainment(double x, double y, double z) const
{
  return getMaskContainment(Eigen::Vector3d(x, y, z));
}
<commit_msg>add forgotten function<commit_after>/*
 * Copyright (c) 2008, Willow Garage, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Willow Garage, Inc. nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

/* Author: Ioan Sucan */

#include <moveit/point_containment_filter/shape_mask.h>
#include <geometric_shapes/body_operations.h>
#include <ros/console.h>

point_containment_filter::ShapeMask::ShapeMask(const TransformCallback& transform_callback) :
  transform_callback_(transform_callback),
  next_handle_ (1),
  min_handle_ (1)
{
}

point_containment_filter::ShapeMask::~ShapeMask()
{
  freeMemory();
}

void point_containment_filter::ShapeMask::freeMemory()
{
  for (std::set<SeeShape>::const_iterator it = bodies_.begin() ; it != bodies_.end() ; ++it)
    delete it->body;
  bodies_.clear();
}

void point_containment_filter::ShapeMask::setTransformCallback(const TransformCallback& transform_callback)
{
  transform_callback_ = transform_callback;
}

point_containment_filter::ShapeHandle point_containment_filter::ShapeMask::addShape(const shapes::ShapeConstPtr &shape, double scale, double padding)
{
  SeeShape ss;
  ss.body = bodies::createBodyFromShape(shape.get());
  if (ss.body)
  {
    ss.body->setScale(scale);
    ss.body->setPadding(padding);
    ss.volume = ss.body->computeVolume();
    ss.handle = next_handle_;
    used_handles_[next_handle_] = bodies_.insert(ss).first;
  }
  else
    return 0;

  ShapeHandle ret = next_handle_;
  const std::size_t sz = min_handle_ + bodies_.size() + 1;
  for (std::size_t i = min_handle_ ; i < sz ; ++i)
    if (used_handles_.find(i) == used_handles_.end())
    {
      next_handle_ = i;
      break;
    }
  min_handle_ = next_handle_;

  return ret;
}

void point_containment_filter::ShapeMask::removeShape(ShapeHandle handle)
{
  std::map<ShapeHandle, std::set<SeeShape, SortBodies>::iterator>::iterator it = used_handles_.find(handle);
  if (it != used_handles_.end())
  {
    delete it->second->body;
    bodies_.erase(it->second);
    used_handles_.erase(it);
    min_handle_ = handle;
  }
}

void point_containment_filter::ShapeMask::maskContainment(const pcl::PointCloud<pcl::PointXYZ>& data_in,
                                                          const Eigen::Vector3d &sensor_origin,
                                                          const double min_sensor_dist, const double max_sensor_dist,
                                                          std::vector<int> &mask)
{
  mask.resize(data_in.points.size());
  if (bodies_.empty())
    std::fill(mask.begin(), mask.end(), (int)OUTSIDE);
  else
  {
    Eigen::Affine3d tmp;
    bspheres_.resize(bodies_.size());
    std::size_t j = 0;
    for (std::set<SeeShape>::const_iterator it = bodies_.begin() ; it != bodies_.end() ; ++it)
    {
      if (transform_callback_(it->handle, tmp))
      {
        it->body->setPose(tmp);
        it->body->computeBoundingSphere(bspheres_[j++]);
      }
    }

    const unsigned int np = data_in.points.size();

    // compute a sphere that bounds the entire robot
    bodies::BoundingSphere bound;
    bodies::mergeBoundingSpheres(bspheres_, bound);
    const double radiusSquared = bound.radius * bound.radius;

    // we now decide which points we keep
#pragma omp parallel for schedule(dynamic)
    for (int i = 0 ; i < (int)np ; ++i)
    {
      Eigen::Vector3d pt = Eigen::Vector3d(data_in.points[i].x, data_in.points[i].y, data_in.points[i].z);
      int out = OUTSIDE;
      if ((bound.center - pt).squaredNorm() < radiusSquared)
      {
        double d = (sensor_origin - pt).norm();
        if (d < min_sensor_dist || d > max_sensor_dist)
        {
          out = CLIP;
        }
        else
          for (std::set<SeeShape>::const_iterator it = bodies_.begin() ; it != bodies_.end() && out == OUTSIDE ; ++it)
          {
            if (it->body->containsPoint(pt))
            {
              out = INSIDE;
            }
          }
      }
      mask[i] = out;
    }
  }
}

int point_containment_filter::ShapeMask::getMaskContainment(const Eigen::Vector3d &pt) const
{
  int out = OUTSIDE;
  for (std::set<SeeShape>::const_iterator it = bodies_.begin() ; it != bodies_.end() && out == OUTSIDE ; ++it)
    if (it->body->containsPoint(pt))
      out = INSIDE;
  return out;
}

int point_containment_filter::ShapeMask::getMaskContainment(double x, double y, double z) const
{
  return getMaskContainment(Eigen::Vector3d(x, y, z));
}
<|endoftext|>


<commit_before>// File: btBoostDynamicsShapes.hpp
#ifndef _btBoostDynamicsShapes_hpp
#define _btBoostDynamicsShapes_hpp

#include <btBulletDynamicsCommon.h>
#include <BulletCollision/CollisionShapes/btBox2dShape.h>
#include <BulletCollision/CollisionShapes/btConvex2dShape.h>
#include <BulletCollision/CollisionShapes/btConvexPointCloudShape.h>
#include <boost/python.hpp>
#include "array_helpers.hpp"
#include "btBoostLinearMathAlignedObjectArray.hpp"

using namespace boost::python;

BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(chullshape_addPoint_overloads,
                                       btConvexHullShape::addPoint,
                                       1, 2)

btTransform&    (btCompoundShape::*cs_getChildTransformRef)(int)        = &btCompoundShape::getChildTransform;
btTransform&    (btCompoundShape::*cs_getChildTransformConstRef)(int)   = &btCompoundShape::getChildTransform;
btCollisionShape*       (btCompoundShape::*cs_getChildShapeRef)(int)            = &btCompoundShape::getChildShape;
const btCollisionShape* (btCompoundShape::*cs_getChildShapeConstRef)(int) const = &btCompoundShape::getChildShape;
btDbvt*     (btCompoundShape::*cs_getDynamicAabbTree)()     = &btCompoundShape::getDynamicAabbTree;
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(compoundshape_updateChildTransform_overloads,
                                       btCompoundShape::updateChildTransform,
                                       2, 3)

void cs_removeChildShape(btCompoundShape& cs, btCollisionShape& shape)
{
    return cs.removeChildShape(&shape);
}

btConvex2dShape*
make_btConvex2dShape(btConvexShape& childShape)
{
    return new btConvex2dShape(&childShape);
}
btConvexShape*  (btConvex2dShape::*c2ds_getChildShapeRef)()     = &btConvex2dShape::getChildShape;

typedef btAlignedObjectArray<btCompoundShapeChild> btCompoundShapeChildArray;

void defineShapes()
{
    // Base classes, not for developer use

    class_<btCollisionShape, boost::noncopyable>
        ("btCollisionShape", no_init)
        .def_readonly("polyhedral", &btCollisionShape::isPolyhedral)
        .def_readonly("convex2d", &btCollisionShape::isConvex2d)
        .def_readonly("convex", &btCollisionShape::isConvex)
        .def_readonly("non_moving", &btCollisionShape::isNonMoving)
        .def_readonly("concave", &btCollisionShape::isConcave)
        .def_readonly("compound", &btCollisionShape::isCompound)
        .def_readonly("soft_body", &btCollisionShape::isSoftBody)
        .def_readonly("infinite", &btCollisionShape::isInfinite)
        .def_readonly("shape_type", &btCollisionShape::getShapeType)
        .add_property("margin", &btCollisionShape::getMargin,
                      &btCollisionShape::setMargin)
        .def("get_aabb", &btCollisionShape::getAabb)
        .def_readonly("aabb", &btCollisionShape::getAabb)
        .def("get_bounding_sphere", &btCollisionShape::getBoundingSphere)
        .def_readonly("angular_motion_disc",
                      &btCollisionShape::getAngularMotionDisc)
        .def("get_contact_breaking_threshold",
             &btCollisionShape::getContactBreakingThreshold)
        .def("calculate_temporal_aabb",
             &btCollisionShape::calculateTemporalAabb)
        .add_property("local_scaling",
                      make_function(&btCollisionShape::getLocalScaling,
                                    return_value_policy<copy_const_reference>()),
                      &btCollisionShape::setLocalScaling)
        .def("calculate_local_inertia", &btCollisionShape::calculateLocalInertia)
        .def("anisotropic_rolling_friction_direction",
             &btCollisionShape::getAnisotropicRollingFrictionDirection)
    ;

    class_<btConvexShape, bases<btCollisionShape>, boost::noncopyable>
        ("btConvexShape", no_init)
        .def("local_get_supporting_vertex",
             &btConvexShape::localGetSupportingVertexWithoutMargin)
        .def("local_get_supporting_vertex_without_margin_non_virtual",
             &btConvexShape::localGetSupportVertexWithoutMarginNonVirtual)
        .def("local_get_support_vertex_non_virtual",
             &btConvexShape::localGetSupportVertexNonVirtual)
        .def("get_margin_non_virtual",
             &btConvexShape::getMarginNonVirtual)
        .def("get_aabb_non_virtual",
             &btConvexShape::getAabbNonVirtual)
        .def("project", &btConvexShape::project)
        .def("batched_unit_vector_get_supporting_vertex_without_margin",
             &btConvexShape::batchedUnitVectorGetSupportingVertexWithoutMargin)
        .def("get_aabb_slow",
             &btConvexShape::getAabbSlow)
    ;

    class_<btConvexInternalShape, bases<btConvexShape>, boost::noncopyable>
        ("btConvexInternalShape", no_init)
        .add_property("implicit_shape_dimensions",
                      make_function(&btConvexInternalShape::getImplicitShapeDimensions,
                                    return_value_policy<copy_const_reference>()),
                      &btConvexInternalShape::setImplicitShapeDimensions)
        // TODO: wrap setSafeMargin overloads
        .def_readonly("local_scaling_non_virtual",
                      make_function(&btConvexInternalShape::getLocalScalingNV,
                                    return_value_policy<copy_const_reference>()))
        .def_readonly("margin_non_virtual",
                      make_function(&btConvexInternalShape::getMarginNV,
                                    return_value_policy<return_by_value>()))
        .def_readonly("num_preferred_penetration_directions",
                      &btConvexInternalShape::getNumPreferredPenetrationDirections)
        .def("get_preferred_penetration_direction",
             &btConvexInternalShape::getPreferredPenetrationDirection)
    ;

    class_<btPolyhedralConvexShape, bases<btConvexInternalShape>,
           boost::noncopyable>
        ("btPolyhedralConvexShape", no_init)
        .def_readonly("num_vertices", &btPolyhedralConvexShape::getNumVertices)
        .def_readonly("num_edges", &btPolyhedralConvexShape::getNumEdges)
        .def("get_edge", &btPolyhedralConvexShape::getEdge)
        .def("get_vertex", &btPolyhedralConvexShape::getVertex)
        .def_readonly("num_planes", &btPolyhedralConvexShape::getNumPlanes)
        .def("get_plane", &btPolyhedralConvexShape::getPlane)
        .def_readonly("is_inside", &btPolyhedralConvexShape::isInside)
    ;

    class_<btPolyhedralConvexAabbCachingShape, bases<btPolyhedralConvexShape>,
           boost::noncopyable>
        ("btPolyhedralConvexAabbCachingShape", no_init)
        .def("get_nonvirtual_aabb",
             &btPolyhedralConvexAabbCachingShape::getNonvirtualAabb)
        .def("recalc_local_aabb",
             &btPolyhedralConvexAabbCachingShape::getAabb)
    ;

    class_<btConcaveShape, bases<btCollisionShape>, boost::noncopyable>
        ("btConcaveShape", no_init)
    ;

    class_<btTriangleMeshShape, bases<btConcaveShape>, boost::noncopyable>
        ("btTriangleMeshShape", no_init)
    ;

    // End base classes

    // Some internal data passing classes

    class_<btCompoundShapeChild>("btCompoundShapeChild")
        .def(self == other<btCompoundShapeChild>())
        .def_readwrite("transform", &btCompoundShapeChild::m_transform)
        .add_property("child_shape",
             make_getter(&btCompoundShapeChild::m_childShape, return_value_policy<reference_existing_object>()),
             make_setter(&btCompoundShapeChild::m_childShape, return_value_policy<reference_existing_object>()))
        .def_readwrite("child_shape_type", &btCompoundShapeChild::m_childShapeType)
        .def_readwrite("child_margin", &btCompoundShapeChild::m_childMargin)
        .add_property("node",
             make_getter(&btCompoundShapeChild::m_node, return_value_policy<reference_existing_object>()),
             make_setter(&btCompoundShapeChild::m_node, return_value_policy<reference_existing_object>()))
    ;

    class_<btCompoundShapeChildArray>("btCompoundShapeChildArray")
        .def(init<btCompoundShapeChildArray>())
        .def(bt_ref_index_suite<btCompoundShapeChildArray>())
        .def("append", &btCompoundShapeChildArray::push_back)
    ;

    // End internal data passing

    // TODO: Add tests
    class_<btConvexHullShape, bases<btPolyhedralConvexAabbCachingShape> >
        ("btConvexHullShape")
        .def("add_point", &btConvexHullShape::addPoint,
             chullshape_addPoint_overloads())
        .def("get_unscaled_points",
             &btConvexHullShape::getUnscaledPoints_wrap,
             return_internal_reference<>())
        .def("get_scaled_point", &btConvexHullShape::getScaledPoint)
        .def("get_num_points", &btConvexHullShape::getNumPoints)
    ;

    // TODO: Add tests
    class_<btBox2dShape, bases<btPolyhedralConvexShape> >
        ("btBox2dShape", init<const btVector3&>())
        .def_readonly("half_extents_with_margin",
                      make_function(&btBox2dShape::getHalfExtentsWithMargin,
                                    return_value_policy<return_by_value>()))
        .def_readonly("half_extents_without_margin",
                      make_function(&btBox2dShape::getHalfExtentsWithoutMargin,
                                    return_internal_reference<>()))
    ;

    // TODO: Add tests
    class_<btBoxShape, bases<btPolyhedralConvexShape> >
        ("btBoxShape", init<const btVector3&>())
        .def_readonly("half_extents_with_margin",
                      make_function(&btBoxShape::getHalfExtentsWithMargin,
                                    return_value_policy<return_by_value>()))
        .def_readonly("half_extents_without_margin",
                      make_function(&btBoxShape::getHalfExtentsWithoutMargin,
                                    return_internal_reference<>()))
    ;

    class_<btBvhTriangleMeshShape, bases<btTriangleMeshShape>, boost::noncopyable>
        ("btBvhTriangleMeshShape", no_init)
    ; // TODO: Implement - left empty while deciding whether to define and wrap btStridingMeshInterface

    // TODO: Implement tests
    class_<btCapsuleShape, bases<btConvexInternalShape> >
        ("btCapsuleShape", init<btScalar, btScalar>())
        .def_readonly("up_axis", &btCapsuleShape::getUpAxis)
        .def_readonly("radius", &btCapsuleShape::getRadius)
        .def_readonly("half_height", &btCapsuleShape::getHalfHeight)
    ;

    // TODO: Implement tests
    class_<btCapsuleShapeX, bases<btCapsuleShape> >
        ("btCapsuleShapeX", init<btScalar, btScalar>())
    ;

    // TODO: Implement tests
    class_<btCapsuleShapeZ, bases<btCapsuleShape> >
        ("btCapsuleShapeZ", init<btScalar, btScalar>())
    ;

    // TODO: Implement tests
    class_<btCompoundShape, bases<btCollisionShape> >
        ("btCompoundShape")
        .def(init<bool>())
        .def("add_child_shape", &btCompoundShape::addChildShape)
        .def("remove_child_shape_by_index",
             &btCompoundShape::removeChildShapeByIndex)
        .def("remove_child_shape",
             cs_removeChildShape)
        .def_readonly("num_child_shapes", &btCompoundShape::getNumChildShapes)
        // TODO: Check implementation of
        //       const btCollisionShape* getChildShape(int) const
        .def("get_child_shape",
             make_function(cs_getChildShapeRef,
                           return_value_policy<reference_existing_object>()))
        // TODO: Same goes for this one, but we can't prevent python callers from
        //       modifying internal references, we'd just get a stack dump...
        .def("get_child_transform",
             make_function(cs_getChildTransformRef,
                           return_internal_reference<>()))
        .def("update_child_transform", &btCompoundShape::updateChildTransform,
             compoundshape_updateChildTransform_overloads())
        .def("get_child_list", &btCompoundShape::getChildListRef,
             return_internal_reference<>())
        .def_readonly("children", make_function(&btCompoundShape::getChildListRef,
                      return_internal_reference<>()))
        .def_readonly("dynamic_aabb_tree", make_function(cs_getDynamicAabbTree,
                      return_value_policy<reference_existing_object>()))
        .def("create_aabb_from_children", &btCompoundShape::createAabbTreeFromChildren)
        // TODO: Investigate implementation of calculatePrincipalAxisTransform
        .def_readonly("update_revision", &btCompoundShape::getUpdateRevision)
    ;

    // TODO: Implement tests
    class_<btConeShape, bases<btConvexInternalShape> >
        ("btConeShape", init<btScalar, btScalar>())
        .def_readonly("radius", &btConeShape::getRadius)
        .def_readonly("height", &btConeShape::getHeight)
        .def_readonly("cone_up_index", &btConeShape::getConeUpIndex)
    ;
    class_<btConeShapeX, bases<btConeShape> >
        ("btConeShapeX", init<btScalar, btScalar>())
    ;
    class_<btConeShapeZ, bases<btConeShape> >
        ("btConeShapeZ", init<btScalar, btScalar>())
    ;

    // TODO: Implement tests
    class_<btConvex2dShape, bases<btConvexShape> >
        ("btConvex2dShape", no_init)
        .def("__init__", make_constructor(&make_btConvex2dShape))
        .def_readonly("child_shape", make_function(c2ds_getChildShapeRef,
                      return_value_policy<reference_existing_object>()))
    ;

    class_<btConvexPointCloudShape, bases<btPolyhedralConvexAabbCachingShape> >
        ("btConvexPointCloudShape")
        // NOTE: This does not implement the pointer referencing ctors
        //       or getter/setter methods
        .def_readonly("num_points", &btConvexPointCloudShape::getNumPoints)
        .def("get_scaled_point", &btConvexPointCloudShape::getScaledPoint)
    ;
}

#endif // _btBoostDynamicsShapes_hpp
<commit_msg>Added convex polyhedron.<commit_after>// File: btBoostDynamicsShapes.hpp
#ifndef _btBoostDynamicsShapes_hpp
#define _btBoostDynamicsShapes_hpp

#include <btBulletDynamicsCommon.h>
#include <BulletCollision/CollisionShapes/btBox2dShape.h>
#include <BulletCollision/CollisionShapes/btConvex2dShape.h>
#include <BulletCollision/CollisionShapes/btConvexPointCloudShape.h>
#include <BulletCollision/CollisionShapes/btConvexPolyhedron.h>
#include <boost/python.hpp>
#include "array_helpers.hpp"
#include "btBoostLinearMathAlignedObjectArray.hpp"

using namespace boost::python;

BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(chullshape_addPoint_overloads,
                                       btConvexHullShape::addPoint,
                                       1, 2)

btTransform&    (btCompoundShape::*cs_getChildTransformRef)(int)        = &btCompoundShape::getChildTransform;
btTransform&    (btCompoundShape::*cs_getChildTransformConstRef)(int)   = &btCompoundShape::getChildTransform;
btCollisionShape*       (btCompoundShape::*cs_getChildShapeRef)(int)            = &btCompoundShape::getChildShape;
const btCollisionShape* (btCompoundShape::*cs_getChildShapeConstRef)(int) const = &btCompoundShape::getChildShape;
btDbvt*     (btCompoundShape::*cs_getDynamicAabbTree)()     = &btCompoundShape::getDynamicAabbTree;
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(compoundshape_updateChildTransform_overloads,
                                       btCompoundShape::updateChildTransform,
                                       2, 3)

void cs_removeChildShape(btCompoundShape& cs, btCollisionShape& shape)
{
    return cs.removeChildShape(&shape);
}

btConvex2dShape*
make_btConvex2dShape(btConvexShape& childShape)
{
    return new btConvex2dShape(&childShape);
}
btConvexShape*  (btConvex2dShape::*c2ds_getChildShapeRef)()     = &btConvex2dShape::getChildShape;

typedef btAlignedObjectArray<btCompoundShapeChild> btCompoundShapeChildArray;

typedef btAlignedObjectArray<btFace> btFaceArray;

tuple btConvexPolyhedron_project(btConvexPolyhedron& self,
                                 const btTransform& trans,
                                 const btVector3& dir,
                                 btScalar& minProj,
                                 btScalar& maxProj,
                                 btVector3& witnesPtMin,
                                 btVector3& witnesPtMax)
{
    self.project(trans, dir, minProj, maxProj, witnesPtMin, witnesPtMax);
    return make_tuple(minProj, maxProj);
}

void defineShapes()
{
    // Base classes, not for developer use

    class_<btCollisionShape, boost::noncopyable>
        ("btCollisionShape", no_init)
        .def_readonly("polyhedral", &btCollisionShape::isPolyhedral)
        .def_readonly("convex2d", &btCollisionShape::isConvex2d)
        .def_readonly("convex", &btCollisionShape::isConvex)
        .def_readonly("non_moving", &btCollisionShape::isNonMoving)
        .def_readonly("concave", &btCollisionShape::isConcave)
        .def_readonly("compound", &btCollisionShape::isCompound)
        .def_readonly("soft_body", &btCollisionShape::isSoftBody)
        .def_readonly("infinite", &btCollisionShape::isInfinite)
        .def_readonly("shape_type", &btCollisionShape::getShapeType)
        .add_property("margin", &btCollisionShape::getMargin,
                      &btCollisionShape::setMargin)
        .def("get_aabb", &btCollisionShape::getAabb)
        .def_readonly("aabb", &btCollisionShape::getAabb)
        .def("get_bounding_sphere", &btCollisionShape::getBoundingSphere)
        .def_readonly("angular_motion_disc",
                      &btCollisionShape::getAngularMotionDisc)
        .def("get_contact_breaking_threshold",
             &btCollisionShape::getContactBreakingThreshold)
        .def("calculate_temporal_aabb",
             &btCollisionShape::calculateTemporalAabb)
        .add_property("local_scaling",
                      make_function(&btCollisionShape::getLocalScaling,
                                    return_value_policy<copy_const_reference>()),
                      &btCollisionShape::setLocalScaling)
        .def("calculate_local_inertia", &btCollisionShape::calculateLocalInertia)
        .def("anisotropic_rolling_friction_direction",
             &btCollisionShape::getAnisotropicRollingFrictionDirection)
    ;

    class_<btConvexShape, bases<btCollisionShape>, boost::noncopyable>
        ("btConvexShape", no_init)
        .def("local_get_supporting_vertex",
             &btConvexShape::localGetSupportingVertexWithoutMargin)
        .def("local_get_supporting_vertex_without_margin_non_virtual",
             &btConvexShape::localGetSupportVertexWithoutMarginNonVirtual)
        .def("local_get_support_vertex_non_virtual",
             &btConvexShape::localGetSupportVertexNonVirtual)
        .def("get_margin_non_virtual",
             &btConvexShape::getMarginNonVirtual)
        .def("get_aabb_non_virtual",
             &btConvexShape::getAabbNonVirtual)
        .def("project", &btConvexShape::project)
        .def("batched_unit_vector_get_supporting_vertex_without_margin",
             &btConvexShape::batchedUnitVectorGetSupportingVertexWithoutMargin)
        .def("get_aabb_slow",
             &btConvexShape::getAabbSlow)
    ;

    class_<btConvexInternalShape, bases<btConvexShape>, boost::noncopyable>
        ("btConvexInternalShape", no_init)
        .add_property("implicit_shape_dimensions",
                      make_function(&btConvexInternalShape::getImplicitShapeDimensions,
                                    return_value_policy<copy_const_reference>()),
                      &btConvexInternalShape::setImplicitShapeDimensions)
        // TODO: wrap setSafeMargin overloads
        .def_readonly("local_scaling_non_virtual",
                      make_function(&btConvexInternalShape::getLocalScalingNV,
                                    return_value_policy<copy_const_reference>()))
        .def_readonly("margin_non_virtual",
                      make_function(&btConvexInternalShape::getMarginNV,
                                    return_value_policy<return_by_value>()))
        .def_readonly("num_preferred_penetration_directions",
                      &btConvexInternalShape::getNumPreferredPenetrationDirections)
        .def("get_preferred_penetration_direction",
             &btConvexInternalShape::getPreferredPenetrationDirection)
    ;

    class_<btPolyhedralConvexShape, bases<btConvexInternalShape>,
           boost::noncopyable>
        ("btPolyhedralConvexShape", no_init)
        .def_readonly("num_vertices", &btPolyhedralConvexShape::getNumVertices)
        .def_readonly("num_edges", &btPolyhedralConvexShape::getNumEdges)
        .def("get_edge", &btPolyhedralConvexShape::getEdge)
        .def("get_vertex", &btPolyhedralConvexShape::getVertex)
        .def_readonly("num_planes", &btPolyhedralConvexShape::getNumPlanes)
        .def("get_plane", &btPolyhedralConvexShape::getPlane)
        .def_readonly("is_inside", &btPolyhedralConvexShape::isInside)
    ;

    class_<btPolyhedralConvexAabbCachingShape, bases<btPolyhedralConvexShape>,
           boost::noncopyable>
        ("btPolyhedralConvexAabbCachingShape", no_init)
        .def("get_nonvirtual_aabb",
             &btPolyhedralConvexAabbCachingShape::getNonvirtualAabb)
        .def("recalc_local_aabb",
             &btPolyhedralConvexAabbCachingShape::getAabb)
    ;

    class_<btConcaveShape, bases<btCollisionShape>, boost::noncopyable>
        ("btConcaveShape", no_init)
    ;

    class_<btTriangleMeshShape, bases<btConcaveShape>, boost::noncopyable>
        ("btTriangleMeshShape", no_init)
    ;

    // End base classes

    // Some internal data passing classes

    class_<btCompoundShapeChild>("btCompoundShapeChild")
        .def(self == other<btCompoundShapeChild>())
        .def_readwrite("transform", &btCompoundShapeChild::m_transform)
        .add_property("child_shape",
             make_getter(&btCompoundShapeChild::m_childShape, return_value_policy<reference_existing_object>()),
             make_setter(&btCompoundShapeChild::m_childShape, return_value_policy<reference_existing_object>()))
        .def_readwrite("child_shape_type", &btCompoundShapeChild::m_childShapeType)
        .def_readwrite("child_margin", &btCompoundShapeChild::m_childMargin)
        .add_property("node",
             make_getter(&btCompoundShapeChild::m_node, return_value_policy<reference_existing_object>()),
             make_setter(&btCompoundShapeChild::m_node, return_value_policy<reference_existing_object>()))
    ;

    class_<btCompoundShapeChildArray>("btCompoundShapeChildArray")
        .def(init<btCompoundShapeChildArray>())
        .def(bt_ref_index_suite<btCompoundShapeChildArray>())
        .def("append", &btCompoundShapeChildArray::push_back)
    ;

    // End internal data passing

    // TODO: Add tests
    class_<btConvexHullShape, bases<btPolyhedralConvexAabbCachingShape> >
        ("btConvexHullShape")
        .def("add_point", &btConvexHullShape::addPoint,
             chullshape_addPoint_overloads())
        .def("get_unscaled_points",
             &btConvexHullShape::getUnscaledPoints_wrap,
             return_internal_reference<>())
        .def("get_scaled_point", &btConvexHullShape::getScaledPoint)
        .def("get_num_points", &btConvexHullShape::getNumPoints)
    ;

    // TODO: Add tests
    class_<btBox2dShape, bases<btPolyhedralConvexShape> >
        ("btBox2dShape", init<const btVector3&>())
        .def_readonly("half_extents_with_margin",
                      make_function(&btBox2dShape::getHalfExtentsWithMargin,
                                    return_value_policy<return_by_value>()))
        .def_readonly("half_extents_without_margin",
                      make_function(&btBox2dShape::getHalfExtentsWithoutMargin,
                                    return_internal_reference<>()))
    ;

    // TODO: Add tests
    class_<btBoxShape, bases<btPolyhedralConvexShape> >
        ("btBoxShape", init<const btVector3&>())
        .def_readonly("half_extents_with_margin",
                      make_function(&btBoxShape::getHalfExtentsWithMargin,
                                    return_value_policy<return_by_value>()))
        .def_readonly("half_extents_without_margin",
                      make_function(&btBoxShape::getHalfExtentsWithoutMargin,
                                    return_internal_reference<>()))
    ;

    class_<btBvhTriangleMeshShape, bases<btTriangleMeshShape>, boost::noncopyable>
        ("btBvhTriangleMeshShape", no_init)
    ; // TODO: Implement - left empty while deciding whether to define and wrap btStridingMeshInterface

    // TODO: Implement tests
    class_<btCapsuleShape, bases<btConvexInternalShape> >
        ("btCapsuleShape", init<btScalar, btScalar>())
        .def_readonly("up_axis", &btCapsuleShape::getUpAxis)
        .def_readonly("radius", &btCapsuleShape::getRadius)
        .def_readonly("half_height", &btCapsuleShape::getHalfHeight)
    ;

    // TODO: Implement tests
    class_<btCapsuleShapeX, bases<btCapsuleShape> >
        ("btCapsuleShapeX", init<btScalar, btScalar>())
    ;

    // TODO: Implement tests
    class_<btCapsuleShapeZ, bases<btCapsuleShape> >
        ("btCapsuleShapeZ", init<btScalar, btScalar>())
    ;

    // TODO: Implement tests
    class_<btCompoundShape, bases<btCollisionShape> >
        ("btCompoundShape")
        .def(init<bool>())
        .def("add_child_shape", &btCompoundShape::addChildShape)
        .def("remove_child_shape_by_index",
             &btCompoundShape::removeChildShapeByIndex)
        .def("remove_child_shape",
             cs_removeChildShape)
        .def_readonly("num_child_shapes", &btCompoundShape::getNumChildShapes)
        // TODO: Check implementation of
        //       const btCollisionShape* getChildShape(int) const
        .def("get_child_shape",
             make_function(cs_getChildShapeRef,
                           return_value_policy<reference_existing_object>()))
        // TODO: Same goes for this one, but we can't prevent python callers from
        //       modifying internal references, we'd just get a stack dump...
        .def("get_child_transform",
             make_function(cs_getChildTransformRef,
                           return_internal_reference<>()))
        .def("update_child_transform", &btCompoundShape::updateChildTransform,
             compoundshape_updateChildTransform_overloads())
        .def("get_child_list", &btCompoundShape::getChildListRef,
             return_internal_reference<>())
        .def_readonly("children", make_function(&btCompoundShape::getChildListRef,
                      return_internal_reference<>()))
        .def_readonly("dynamic_aabb_tree", make_function(cs_getDynamicAabbTree,
                      return_value_policy<reference_existing_object>()))
        .def("create_aabb_from_children", &btCompoundShape::createAabbTreeFromChildren)
        // TODO: Investigate implementation of calculatePrincipalAxisTransform
        .def_readonly("update_revision", &btCompoundShape::getUpdateRevision)
    ;

    // TODO: Implement tests
    class_<btConeShape, bases<btConvexInternalShape> >
        ("btConeShape", init<btScalar, btScalar>())
        .def_readonly("radius", &btConeShape::getRadius)
        .def_readonly("height", &btConeShape::getHeight)
        .def_readonly("cone_up_index", &btConeShape::getConeUpIndex)
    ;
    class_<btConeShapeX, bases<btConeShape> >
        ("btConeShapeX", init<btScalar, btScalar>())
    ;
    class_<btConeShapeZ, bases<btConeShape> >
        ("btConeShapeZ", init<btScalar, btScalar>())
    ;

    // TODO: Implement tests
    class_<btConvex2dShape, bases<btConvexShape> >
        ("btConvex2dShape", no_init)
        .def("__init__", make_constructor(&make_btConvex2dShape))
        .def_readonly("child_shape", make_function(c2ds_getChildShapeRef,
                      return_value_policy<reference_existing_object>()))
    ;

    // TODO: Implement tests
    class_<btConvexPointCloudShape, bases<btPolyhedralConvexAabbCachingShape> >
        ("btConvexPointCloudShape")
        // NOTE: This does not implement the pointer referencing ctors
        //       or getter/setter methods
        .def_readonly("num_points", &btConvexPointCloudShape::getNumPoints)
        .def("get_scaled_point", &btConvexPointCloudShape::getScaledPoint)
    ;

    // TODO: Implement tests
    class_<btConvexPolyhedron>
        ("btConvexPolyhedron")
        .def_readwrite("vertices", &btConvexPolyhedron::m_vertices)
        .def_readwrite("faces", &btConvexPolyhedron::m_faces)
        .def_readwrite("unique_edges", &btConvexPolyhedron::m_uniqueEdges)
        .def_readwrite("local_center", &btConvexPolyhedron::m_localCenter)
        .def_readwrite("extents", &btConvexPolyhedron::m_extents)
        .def_readwrite("radius", &btConvexPolyhedron::m_radius)
        .def_readwrite("mC", &btConvexPolyhedron::mC)
        .def_readwrite("mE", &btConvexPolyhedron::mE)
        .def("initialize", &btConvexPolyhedron::initialize)
        .def("test_containment", &btConvexPolyhedron::testContainment)
        .def("project", &btConvexPolyhedron_project)
    ;
}

#endif // _btBoostDynamicsShapes_hpp
<|endoftext|>


<commit_before>/* +---------------------------------------------------------------------------+
   |                 The Mobile Robot Programming Toolkit (MRPT)               |
   |                                                                           |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2012, Individual contributors, see AUTHORS file        |
   | Copyright (c) 2005-2012, MAPIR group, University of Malaga                |
   | Copyright (c) 2012, University of Almeria                                 |
   | All rights reserved.                                                      |
   |                                                                           |
   | Redistribution and use in source and binary forms, with or without        |
   | modification, are permitted provided that the following conditions are    |
   | met:                                                                      |
   |    * Redistributions of source code must retain the above copyright       |
   |      notice, this list of conditions and the following disclaimer.        |
   |    * Redistributions in binary form must reproduce the above copyright    |
   |      notice, this list of conditions and the following disclaimer in the  |
   |      documentation and/or other materials provided with the distribution. |
   |    * Neither the name of the copyright holders nor the                    |
   |      names of its contributors may be used to endorse or promote products |
   |      derived from this software without specific prior written permission.|
   |                                                                           |
   | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS       |
   | 'AS IS' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED |
   | TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR|
   | PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS BE LIABLE |
   | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL|
   | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR|
   |  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)       |
   | HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,       |
   | STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN  |
   | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE           |
   | POSSIBILITY OF SUCH DAMAGE.                                               |
   +---------------------------------------------------------------------------+ */

#include <mrpt/base.h>  // Precompiled headers

#include <mrpt/utils/CTimeLogger.h>
#include <mrpt/utils/CFileOutputStream.h>
#include <mrpt/system/string_utils.h>

using namespace mrpt;
using namespace mrpt::utils;
using namespace mrpt::system;
using namespace std;

CTimeLogger mrpt::utils::global_profiler;

namespace mrpt
{
	namespace utils
	{
		void global_profiler_enter(const char *func_name) MRPT_NO_THROWS {
			global_profiler.enter(func_name);
		}
		void global_profiler_leave(const char *func_name) MRPT_NO_THROWS {
			global_profiler.leave(func_name);
		}
	}
}

CTimeLogger::CTimeLogger(bool enabled) : m_tictac(), m_enabled(enabled)
{
	m_tictac.Tic();
}

CTimeLogger::~CTimeLogger()
{
	// Dump all stats:
    if (!m_data.empty()) // If logging is disabled, do nothing...
        dumpAllStats();
}

void CTimeLogger::clear(bool deep_clear)
{
	if (deep_clear)
		m_data.clear();
	else
	{
		for (map<string,TCallData>::iterator i=m_data.begin();i!=m_data.end();++i)
			i->second = TCallData();
	}
}

std::string  aux_format_string_multilines(const std::string &s, const size_t len)
{
	std::string ret;

	for (size_t p=0;p<s.size();p+=len)
	{
		ret+=rightPad(s.c_str()+p,len,true);
		if (p+len<s.size())
			ret+="\n";
	}
	return ret;
}

void CTimeLogger::getStats(std::map<std::string,TCallStats> &out_stats) const
{
	out_stats.clear();
	for (map<string,TCallData>::const_iterator i=m_data.begin();i!=m_data.end();++i)
	{
		TCallStats &cs = out_stats[i->first];
		cs.min_t   = i->second.min_t;
		cs.max_t   = i->second.max_t;
		cs.total_t = i->second.mean_t;
		cs.mean_t  = i->second.n_calls ? i->second.mean_t/i->second.n_calls : 0;
		cs.n_calls = i->second.n_calls;
	}
}

std::string CTimeLogger::getStatsAsText(const size_t column_width)  const
{
	std::string s;

	s+="--------------------------- MRPT CTimeLogger report --------------------------\n";
	s+="           FUNCTION                         #CALLS  MIN.T  MEAN.T MAX.T TOTAL \n";
	s+="------------------------------------------------------------------------------\n";
	for (map<string,TCallData>::const_iterator i=m_data.begin();i!=m_data.end();++i)
	{
		const string sMinT   = unitsFormat(i->second.min_t,1,false);
		const string sMaxT   = unitsFormat(i->second.max_t,1,false);
		const string sTotalT = unitsFormat(i->second.mean_t,1,false);
		const string sMeanT  = unitsFormat(i->second.n_calls ? i->second.mean_t/i->second.n_calls : 0,1,false);

		s+=format("%s %7u %6ss %6ss %6ss %6ss\n",
			aux_format_string_multilines(i->first,40).c_str(),
			static_cast<unsigned int>(i->second.n_calls),
			sMinT.c_str(),
			sMeanT.c_str(),
			sMaxT.c_str(),
			sTotalT.c_str() );
	}

	s+="---------------------- End of MRPT CTimeLogger report ------------------------\n";

	return s;
}

void CTimeLogger::saveToCSVFile(const std::string &csv_file)  const
{
	std::string s;
	s+="FUNCTION, #CALLS, MIN.T, MEAN.T, MAX.T, TOTAL.T\n";
	for (map<string,TCallData>::const_iterator i=m_data.begin();i!=m_data.end();++i)
	{
		s+=format("\"%s\",\"%7u\",\"%e\",\"%e\",\"%e\",\"%e\"\n",
			i->first.c_str(),
			static_cast<unsigned int>(i->second.n_calls),
			i->second.min_t,
			i->second.n_calls ? i->second.mean_t/i->second.n_calls : 0,
			i->second.max_t,
			i->second.mean_t );
	}
	CFileOutputStream(csv_file).printf("%s",s.c_str() );
}

void CTimeLogger::dumpAllStats(const size_t  column_width) const
{
	string s = getStatsAsText(column_width);
	printf_debug("\n%s\n", s.c_str() );
}

void CTimeLogger::do_enter(const char *func_name)
{
	const string  s = func_name;
	TCallData &d = m_data[s];

	d.n_calls++;
	d.open_calls.push(0);  // Dummy value, it'll be written below
	d.open_calls.top() = m_tictac.Tac(); // to avoid possible delays.
}

double CTimeLogger::do_leave(const char *func_name)
{
	const double tim = m_tictac.Tac();

	const string  s = func_name;
	TCallData &d = m_data[s];

	if (!d.open_calls.empty())
	{
		const double At = tim - d.open_calls.top();
		d.open_calls.pop();

		d.mean_t+=At;
		if (d.n_calls==1)
		{
			d.min_t= At;
			d.max_t= At;
		}
		else
		{
			mrpt::utils::keep_min( d.min_t, At);
			mrpt::utils::keep_max( d.max_t, At);
		}
		return At;
	}
	else return 0; // This shouldn't happen!
}

CTimeLogger::TCallData::TCallData() :
	n_calls	(0),
	min_t	(0),
	max_t	(0),
	mean_t	(0)
{
}

double CTimeLogger::getMeanTime(const std::string &name)  const
{
	map<string,TCallData>::const_iterator it = m_data.find(name);
	if (it==m_data.end())
		 return 0;
	else return it->second.n_calls ? it->second.mean_t/it->second.n_calls : 0;
}
<commit_msg>minor formating fix in CTimeLogger output<commit_after>/* +---------------------------------------------------------------------------+
   |                 The Mobile Robot Programming Toolkit (MRPT)               |
   |                                                                           |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2012, Individual contributors, see AUTHORS file        |
   | Copyright (c) 2005-2012, MAPIR group, University of Malaga                |
   | Copyright (c) 2012, University of Almeria                                 |
   | All rights reserved.                                                      |
   |                                                                           |
   | Redistribution and use in source and binary forms, with or without        |
   | modification, are permitted provided that the following conditions are    |
   | met:                                                                      |
   |    * Redistributions of source code must retain the above copyright       |
   |      notice, this list of conditions and the following disclaimer.        |
   |    * Redistributions in binary form must reproduce the above copyright    |
   |      notice, this list of conditions and the following disclaimer in the  |
   |      documentation and/or other materials provided with the distribution. |
   |    * Neither the name of the copyright holders nor the                    |
   |      names of its contributors may be used to endorse or promote products |
   |      derived from this software without specific prior written permission.|
   |                                                                           |
   | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS       |
   | 'AS IS' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED |
   | TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR|
   | PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS BE LIABLE |
   | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL|
   | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR|
   |  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)       |
   | HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,       |
   | STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN  |
   | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE           |
   | POSSIBILITY OF SUCH DAMAGE.                                               |
   +---------------------------------------------------------------------------+ */

#include <mrpt/base.h>  // Precompiled headers

#include <mrpt/utils/CTimeLogger.h>
#include <mrpt/utils/CFileOutputStream.h>
#include <mrpt/system/string_utils.h>

using namespace mrpt;
using namespace mrpt::utils;
using namespace mrpt::system;
using namespace std;

CTimeLogger mrpt::utils::global_profiler;

namespace mrpt
{
	namespace utils
	{
		void global_profiler_enter(const char *func_name) MRPT_NO_THROWS {
			global_profiler.enter(func_name);
		}
		void global_profiler_leave(const char *func_name) MRPT_NO_THROWS {
			global_profiler.leave(func_name);
		}
	}
}

CTimeLogger::CTimeLogger(bool enabled) : m_tictac(), m_enabled(enabled)
{
	m_tictac.Tic();
}

CTimeLogger::~CTimeLogger()
{
	// Dump all stats:
    if (!m_data.empty()) // If logging is disabled, do nothing...
        dumpAllStats();
}

void CTimeLogger::clear(bool deep_clear)
{
	if (deep_clear)
		m_data.clear();
	else
	{
		for (map<string,TCallData>::iterator i=m_data.begin();i!=m_data.end();++i)
			i->second = TCallData();
	}
}

std::string  aux_format_string_multilines(const std::string &s, const size_t len)
{
	std::string ret;

	for (size_t p=0;p<s.size();p+=len)
	{
		ret+=rightPad(s.c_str()+p,len,true);
		if (p+len<s.size())
			ret+="\n";
	}
	return ret;
}

void CTimeLogger::getStats(std::map<std::string,TCallStats> &out_stats) const
{
	out_stats.clear();
	for (map<string,TCallData>::const_iterator i=m_data.begin();i!=m_data.end();++i)
	{
		TCallStats &cs = out_stats[i->first];
		cs.min_t   = i->second.min_t;
		cs.max_t   = i->second.max_t;
		cs.total_t = i->second.mean_t;
		cs.mean_t  = i->second.n_calls ? i->second.mean_t/i->second.n_calls : 0;
		cs.n_calls = i->second.n_calls;
	}
}

std::string CTimeLogger::getStatsAsText(const size_t column_width)  const
{
	std::string s;

	s+="--------------------------- MRPT CTimeLogger report --------------------------\n";
	s+="           FUNCTION                         #CALLS  MIN.T  MEAN.T MAX.T TOTAL \n";
	s+="------------------------------------------------------------------------------\n";
	for (map<string,TCallData>::const_iterator i=m_data.begin();i!=m_data.end();++i)
	{
		const string sMinT   = unitsFormat(i->second.min_t,1,false);
		const string sMaxT   = unitsFormat(i->second.max_t,1,false);
		const string sTotalT = unitsFormat(i->second.mean_t,1,false);
		const string sMeanT  = unitsFormat(i->second.n_calls ? i->second.mean_t/i->second.n_calls : 0,1,false);

		s+=format("%s %7u %6ss %6ss %6ss %6ss\n",
			aux_format_string_multilines(i->first,39).c_str(),
			static_cast<unsigned int>(i->second.n_calls),
			sMinT.c_str(),
			sMeanT.c_str(),
			sMaxT.c_str(),
			sTotalT.c_str() );
	}

	s+="---------------------- End of MRPT CTimeLogger report ------------------------\n";

	return s;
}

void CTimeLogger::saveToCSVFile(const std::string &csv_file)  const
{
	std::string s;
	s+="FUNCTION, #CALLS, MIN.T, MEAN.T, MAX.T, TOTAL.T\n";
	for (map<string,TCallData>::const_iterator i=m_data.begin();i!=m_data.end();++i)
	{
		s+=format("\"%s\",\"%7u\",\"%e\",\"%e\",\"%e\",\"%e\"\n",
			i->first.c_str(),
			static_cast<unsigned int>(i->second.n_calls),
			i->second.min_t,
			i->second.n_calls ? i->second.mean_t/i->second.n_calls : 0,
			i->second.max_t,
			i->second.mean_t );
	}
	CFileOutputStream(csv_file).printf("%s",s.c_str() );
}

void CTimeLogger::dumpAllStats(const size_t  column_width) const
{
	string s = getStatsAsText(column_width);
	printf_debug("\n%s\n", s.c_str() );
}

void CTimeLogger::do_enter(const char *func_name)
{
	const string  s = func_name;
	TCallData &d = m_data[s];

	d.n_calls++;
	d.open_calls.push(0);  // Dummy value, it'll be written below
	d.open_calls.top() = m_tictac.Tac(); // to avoid possible delays.
}

double CTimeLogger::do_leave(const char *func_name)
{
	const double tim = m_tictac.Tac();

	const string  s = func_name;
	TCallData &d = m_data[s];

	if (!d.open_calls.empty())
	{
		const double At = tim - d.open_calls.top();
		d.open_calls.pop();

		d.mean_t+=At;
		if (d.n_calls==1)
		{
			d.min_t= At;
			d.max_t= At;
		}
		else
		{
			mrpt::utils::keep_min( d.min_t, At);
			mrpt::utils::keep_max( d.max_t, At);
		}
		return At;
	}
	else return 0; // This shouldn't happen!
}

CTimeLogger::TCallData::TCallData() :
	n_calls	(0),
	min_t	(0),
	max_t	(0),
	mean_t	(0)
{
}

double CTimeLogger::getMeanTime(const std::string &name)  const
{
	map<string,TCallData>::const_iterator it = m_data.find(name);
	if (it==m_data.end())
		 return 0;
	else return it->second.n_calls ? it->second.mean_t/it->second.n_calls : 0;
}
<|endoftext|>


<commit_before><commit_msg>gltfio: clean up finalize().<commit_after><|endoftext|>