<commit_before>/*
   Copyright (c) 2009-2014, Jack Poulson
   All rights reserved.

   This file is part of Elemental and is under the BSD 2-Clause License,
   which can be found in the LICENSE file in the root directory, or at
   http://opensource.org/licenses/BSD-2-Clause
*/
#pragma once
#ifndef ELEM_SCHUR_QR_HPP
#define ELEM_SCHUR_QR_HPP

#include ELEM_MAKETRIANGULAR_INC
#include ELEM_HESSENBERG_INC
#include ELEM_IDENTITY_INC

namespace elem {
namespace schur {

template<typename F>
inline void
QR( Matrix<F>& A, Matrix<Complex<BASE(F)>>& w, bool fullTriangle=false )
{
    DEBUG_ONLY(CallStackEntry cse("schur::qr"))
    const Int n = A.Height();
    w.Resize( n, 1 );
    lapack::Schur( n, A.Buffer(), A.LDim(), w.Buffer(), fullTriangle );
    if( fullTriangle )
        MakeTriangular( UPPER, A );
}

template<typename F>
inline void
QR
( Matrix<F>& A, Matrix<Complex<BASE(F)>>& w, Matrix<F>& Q,
  bool fullTriangle=true )
{
    DEBUG_ONLY(CallStackEntry cse("schur::qr"))
    const Int n = A.Height();
    Q.Resize( n, n );
    w.Resize( n, 1 );
    lapack::Schur
    ( n, A.Buffer(), A.LDim(), Q.Buffer(), Q.LDim(), w.Buffer(), fullTriangle );
    if( fullTriangle )
        MakeTriangular( UPPER, A );
}

template<typename F>
inline void
QR
( BlockDistMatrix<F>& A, DistMatrix<Complex<BASE(F)>,VR,STAR>& w,
  bool fullTriangle=false, bool aed=false )
{
    DEBUG_ONLY(CallStackEntry cse("schur::qr"))
#ifdef ELEM_HAVE_SCALAPACK
    const Int n = A.Height();
    const int bhandle = blacs::Handle( A.DistComm().comm );
    const int context =
        blacs::GridInit
        ( bhandle, A.Grid().Order()==COLUMN_MAJOR,
          A.ColStride(), A.RowStride() );
    if( A.ColStride() != blacs::GridHeight(context) )
        LogicError("Grid height did not match BLACS");
    if( A.RowStride() != blacs::GridWidth(context) )
        LogicError("Grid width did not match BLACS");
    if( A.ColRank() != blacs::GridRow(context) )
        LogicError("Grid row did not match BLACS");
    if( A.RowRank() != blacs::GridCol(context) )
        LogicError("Grid col did not match BLACS");
    auto desca = FillDesc( A, context );

    // Reduce the matrix to upper-Hessenberg form in an elemental form
    DistMatrix<F> AElem( A );
    DistMatrix<F,STAR,STAR> t( A.Grid() );
    Hessenberg( UPPER, AElem, t );
    MakeTrapezoidal( UPPER, AElem, -1 );
    A = AElem;
    if( A.ColStride() != blacs::GridHeight(context) )
        LogicError("Grid height did not match BLACS");
    if( A.RowStride() != blacs::GridWidth(context) )
        LogicError("Grid width did not match BLACS");
    if( A.ColRank() != blacs::GridRow(context) )
        LogicError("Grid row did not match BLACS");
    if( A.RowRank() != blacs::GridCol(context) )
        LogicError("Grid col did not match BLACS");

    // Run the QR algorithm in block form
    DistMatrix<Complex<Base<F>>,STAR,STAR> w_STAR_STAR( n, 1, A.Grid() );
    scalapack::HessenbergSchur
    ( n, A.Buffer(), desca.data(), w_STAR_STAR.Buffer(), fullTriangle, aed );
    w = w_STAR_STAR;

    blacs::FreeGrid( context );
    blacs::FreeHandle( bhandle );
    blacs::Exit();
#else
    LogicError("Distributed schur::QR currently requires ScaLAPACK support");
#endif
    if( fullTriangle )
        MakeTriangular( UPPER, A );
}

template<typename F>
inline void
QR
( BlockDistMatrix<F>& A, DistMatrix<Complex<BASE(F)>,VR,STAR>& w,
  BlockDistMatrix<F>& Q, bool fullTriangle=true, bool aed=false )
{
    DEBUG_ONLY(CallStackEntry cse("schur::qr"))
#ifdef ELEM_HAVE_SCALAPACK
    const Int n = A.Height();
    const int bhandle = blacs::Handle( A.DistComm().comm );
    const int context =
        blacs::GridInit
        ( bhandle, A.Grid().Order()==COLUMN_MAJOR,
          A.ColStride(), A.RowStride() );
    Q.AlignWith( A );
    Q.Resize( n, n, A.LDim() );
    if( A.ColStride() != blacs::GridHeight(context) ||
        Q.ColStride() != blacs::GridHeight(context) )
        LogicError("Grid height did not match BLACS");
    if( A.RowStride() != blacs::GridWidth(context) ||
        Q.RowStride() != blacs::GridWidth(context) )
        LogicError("Grid width did not match BLACS");
    if( A.ColRank() != blacs::GridRow(context) ||
        Q.ColRank() != blacs::GridRow(context) )
        LogicError("Grid row did not match BLACS");
    if( A.RowRank() != blacs::GridCol(context) ||
        Q.RowRank() != blacs::GridCol(context) )
        LogicError("Grid col did not match BLACS");
    auto desca = FillDesc( A, context );
    auto descq = FillDesc( Q, context );

    // Reduce A to upper-Hessenberg form in an element-wise distribution
    // and form the explicit reflector matrix
    DistMatrix<F> AElem( A ), QElem( A.Grid() );
    DistMatrix<F,STAR,STAR> t( A.Grid() );
    Hessenberg( UPPER, AElem, t );
    // There is not yet a 'form Q'
    Identity( QElem, n, n );
    hessenberg::ApplyQ( UPPER, LEFT, NORMAL, AElem, t, QElem );
    MakeTrapezoidal( UPPER, AElem, -1 );
    A = AElem;
    Q = QElem;
    if( A.ColStride() != blacs::GridHeight(context) ||
        Q.ColStride() != blacs::GridHeight(context) )
        LogicError("Grid height did not match BLACS");
    if( A.RowStride() != blacs::GridWidth(context) ||
        Q.RowStride() != blacs::GridWidth(context) )
        LogicError("Grid width did not match BLACS");
    if( A.ColRank() != blacs::GridRow(context) ||
        Q.ColRank() != blacs::GridRow(context) )
        LogicError("Grid row did not match BLACS");
    if( A.RowRank() != blacs::GridCol(context) ||
        Q.RowRank() != blacs::GridCol(context) )
        LogicError("Grid col did not match BLACS");

    // Compute the Schur decomposition in block form, multiplying the
    // accumulated Householder reflectors from the right
    DistMatrix<Complex<Base<F>>,STAR,STAR> w_STAR_STAR( n, 1, A.Grid() );
    const bool multiplyQ = true;
    scalapack::HessenbergSchur
    ( n, A.Buffer(), desca.data(), w_STAR_STAR.Buffer(),
      Q.Buffer(), descq.data(), fullTriangle, multiplyQ, aed );
    w = w_STAR_STAR;

    blacs::FreeGrid( context );
    blacs::FreeHandle( bhandle );
    blacs::Exit();
#else
    LogicError("Distributed schur::QR currently requires ScaLAPACK support");
#endif
    if( fullTriangle )
        MakeTriangular( UPPER, A );
}

template<typename F>
inline void
QR
( DistMatrix<F>& A, DistMatrix<Complex<BASE(F)>,VR,STAR>& w,
  bool fullTriangle=false, bool aed=false )
{
    DEBUG_ONLY(CallStackEntry cse("schur::qr"))
#ifdef ELEM_HAVE_SCALAPACK
    // Reduce the matrix to upper-Hessenberg form in an elemental form
    DistMatrix<F,STAR,STAR> t( A.Grid() );
    Hessenberg( UPPER, A, t );
    MakeTrapezoidal( UPPER, A, -1 );

    // Run the QR algorithm in block form
    // TODO: Create schur::HessenbergQR
    const Int n = A.Height();
    const Int nb = DefaultBlockHeight();
    BlockDistMatrix<F> ABlock( n, n, A.Grid(), nb, nb );
    ABlock = A;
    const int bhandle = blacs::Handle( ABlock.DistComm().comm );
    const int context =
        blacs::GridInit
        ( bhandle, ABlock.Grid().Order()==COLUMN_MAJOR,
          ABlock.ColStride(), ABlock.RowStride() );
    if( ABlock.ColStride() != blacs::GridHeight(context) )
        LogicError("Grid height did not match BLACS");
    if( ABlock.RowStride() != blacs::GridWidth(context) )
        LogicError("Grid width did not match BLACS");
    if( ABlock.ColRank() != blacs::GridRow(context) )
        LogicError("Grid row did not match BLACS");
    if( ABlock.RowRank() != blacs::GridCol(context) )
        LogicError("Grid col did not match BLACS");
    blacs::Desc desca = FillDesc( ABlock, context );
    DistMatrix<Complex<Base<F>>,STAR,STAR> w_STAR_STAR( n, 1, A.Grid() );
    scalapack::HessenbergSchur
    ( n, ABlock.Buffer(), desca.data(), w_STAR_STAR.Buffer(),
      fullTriangle, aed );
    A = ABlock;
    w = w_STAR_STAR;

    blacs::FreeGrid( context );
    blacs::FreeHandle( bhandle );
    blacs::Exit();
#else
    LogicError("Distributed schur::QR currently requires ScaLAPACK support");
#endif
    if( fullTriangle )
        MakeTriangular( UPPER, A );
}

template<typename F>
inline void
QR
( DistMatrix<F>& A, DistMatrix<Complex<BASE(F)>,VR,STAR>& w, DistMatrix<F>& Q,
  bool fullTriangle=true, bool aed=false )
{
    DEBUG_ONLY(CallStackEntry cse("schur::qr"))
#ifdef ELEM_HAVE_SCALAPACK
    const Int n = A.Height();
    // Reduce A to upper-Hessenberg form in an element-wise distribution
    // and form the explicit reflector matrix
    DistMatrix<F,STAR,STAR> t( A.Grid() );
    Hessenberg( UPPER, A, t );
    // There is not yet a 'form Q'
    Identity( Q, n, n );
    hessenberg::ApplyQ( UPPER, LEFT, NORMAL, A, t, Q );
    MakeTrapezoidal( UPPER, A, -1 );

    // Run the Hessenberg QR algorithm in block form
    const Int nb = DefaultBlockHeight();
    BlockDistMatrix<F> ABlock( n, n, A.Grid(), nb, nb ),
                       QBlock( n, n, A.Grid(), nb, nb );
    ABlock = A;
    QBlock = Q;
    const int bhandle = blacs::Handle( ABlock.DistComm().comm );
    const int context =
        blacs::GridInit
        ( bhandle, ABlock.Grid().Order()==COLUMN_MAJOR,
          ABlock.ColStride(), ABlock.RowStride() );
    if( ABlock.ColStride() != blacs::GridHeight(context) ||
        QBlock.ColStride() != blacs::GridHeight(context) )
        LogicError("Grid height did not match BLACS");
    if( ABlock.RowStride() != blacs::GridWidth(context) ||
        QBlock.RowStride() != blacs::GridWidth(context) )
        LogicError("Grid width did not match BLACS");
    if( ABlock.ColRank() != blacs::GridRow(context) ||
        QBlock.ColRank() != blacs::GridRow(context) )
        LogicError("Grid row did not match BLACS");
    if( ABlock.RowRank() != blacs::GridCol(context) ||
        QBlock.RowRank() != blacs::GridCol(context) )
        LogicError("Grid col did not match BLACS");
    auto desca = FillDesc( ABlock, context );
    auto descq = FillDesc( QBlock, context );

    // Compute the Schur decomposition in block form, multiplying the
    // accumulated Householder reflectors from the right
    DistMatrix<Complex<Base<F>>,STAR,STAR> w_STAR_STAR( n, 1, A.Grid() );
    const bool multiplyQ = true;
    scalapack::HessenbergSchur
    ( n, ABlock.Buffer(), desca.data(), w_STAR_STAR.Buffer(),
      QBlock.Buffer(), descq.data(), fullTriangle, multiplyQ, aed );
    A = ABlock;
    w = w_STAR_STAR;
    Q = QBlock;

    blacs::FreeGrid( context );
    blacs::FreeHandle( bhandle );
    blacs::Exit();
#else
    LogicError("Distributed schur::QR currently requires ScaLAPACK support");
#endif
    if( fullTriangle )
        MakeTriangular( UPPER, A );
}

} // namespace schur
} // namespace elem

#endif // ifndef ELEM_SCHUR_QR_HPP
<commit_msg>Updating schur::QR to accept the eigenvalue parameter before the eigenvector parameters.<commit_after>/*
   Copyright (c) 2009-2014, Jack Poulson
   All rights reserved.

   This file is part of Elemental and is under the BSD 2-Clause License,
   which can be found in the LICENSE file in the root directory, or at
   http://opensource.org/licenses/BSD-2-Clause
*/
#pragma once
#ifndef ELEM_SCHUR_QR_HPP
#define ELEM_SCHUR_QR_HPP

#include ELEM_MAKETRIANGULAR_INC
#include ELEM_HESSENBERG_INC
#include ELEM_IDENTITY_INC

namespace elem {
namespace schur {

template<typename F>
inline void
QR( Matrix<F>& A, Matrix<Complex<BASE(F)>>& w, bool fullTriangle=false )
{
    DEBUG_ONLY(CallStackEntry cse("schur::qr"))
    const Int n = A.Height();
    w.Resize( n, 1 );
    lapack::Schur( n, A.Buffer(), A.LDim(), w.Buffer(), fullTriangle );
    if( fullTriangle )
        MakeTriangular( UPPER, A );
}

template<typename F>
inline void
QR
( Matrix<F>& A, Matrix<Complex<BASE(F)>>& w, Matrix<F>& Q,
  bool fullTriangle=true )
{
    DEBUG_ONLY(CallStackEntry cse("schur::qr"))
    const Int n = A.Height();
    Q.Resize( n, n );
    w.Resize( n, 1 );
    lapack::Schur
    ( n, A.Buffer(), A.LDim(), w.Buffer(), Q.Buffer(), Q.LDim(), fullTriangle );
    if( fullTriangle )
        MakeTriangular( UPPER, A );
}

template<typename F>
inline void
QR
( BlockDistMatrix<F>& A, DistMatrix<Complex<BASE(F)>,VR,STAR>& w,
  bool fullTriangle=false, bool aed=false )
{
    DEBUG_ONLY(CallStackEntry cse("schur::qr"))
#ifdef ELEM_HAVE_SCALAPACK
    const Int n = A.Height();
    const int bhandle = blacs::Handle( A.DistComm().comm );
    const int context =
        blacs::GridInit
        ( bhandle, A.Grid().Order()==COLUMN_MAJOR,
          A.ColStride(), A.RowStride() );
    if( A.ColStride() != blacs::GridHeight(context) )
        LogicError("Grid height did not match BLACS");
    if( A.RowStride() != blacs::GridWidth(context) )
        LogicError("Grid width did not match BLACS");
    if( A.ColRank() != blacs::GridRow(context) )
        LogicError("Grid row did not match BLACS");
    if( A.RowRank() != blacs::GridCol(context) )
        LogicError("Grid col did not match BLACS");
    auto desca = FillDesc( A, context );

    // Reduce the matrix to upper-Hessenberg form in an elemental form
    DistMatrix<F> AElem( A );
    DistMatrix<F,STAR,STAR> t( A.Grid() );
    Hessenberg( UPPER, AElem, t );
    MakeTrapezoidal( UPPER, AElem, -1 );
    A = AElem;
    if( A.ColStride() != blacs::GridHeight(context) )
        LogicError("Grid height did not match BLACS");
    if( A.RowStride() != blacs::GridWidth(context) )
        LogicError("Grid width did not match BLACS");
    if( A.ColRank() != blacs::GridRow(context) )
        LogicError("Grid row did not match BLACS");
    if( A.RowRank() != blacs::GridCol(context) )
        LogicError("Grid col did not match BLACS");

    // Run the QR algorithm in block form
    DistMatrix<Complex<Base<F>>,STAR,STAR> w_STAR_STAR( n, 1, A.Grid() );
    scalapack::HessenbergSchur
    ( n, A.Buffer(), desca.data(), w_STAR_STAR.Buffer(), fullTriangle, aed );
    w = w_STAR_STAR;

    blacs::FreeGrid( context );
    blacs::FreeHandle( bhandle );
    blacs::Exit();
#else
    LogicError("Distributed schur::QR currently requires ScaLAPACK support");
#endif
    if( fullTriangle )
        MakeTriangular( UPPER, A );
}

template<typename F>
inline void
QR
( BlockDistMatrix<F>& A, DistMatrix<Complex<BASE(F)>,VR,STAR>& w,
  BlockDistMatrix<F>& Q, bool fullTriangle=true, bool aed=false )
{
    DEBUG_ONLY(CallStackEntry cse("schur::qr"))
#ifdef ELEM_HAVE_SCALAPACK
    const Int n = A.Height();
    const int bhandle = blacs::Handle( A.DistComm().comm );
    const int context =
        blacs::GridInit
        ( bhandle, A.Grid().Order()==COLUMN_MAJOR,
          A.ColStride(), A.RowStride() );
    Q.AlignWith( A );
    Q.Resize( n, n, A.LDim() );
    if( A.ColStride() != blacs::GridHeight(context) ||
        Q.ColStride() != blacs::GridHeight(context) )
        LogicError("Grid height did not match BLACS");
    if( A.RowStride() != blacs::GridWidth(context) ||
        Q.RowStride() != blacs::GridWidth(context) )
        LogicError("Grid width did not match BLACS");
    if( A.ColRank() != blacs::GridRow(context) ||
        Q.ColRank() != blacs::GridRow(context) )
        LogicError("Grid row did not match BLACS");
    if( A.RowRank() != blacs::GridCol(context) ||
        Q.RowRank() != blacs::GridCol(context) )
        LogicError("Grid col did not match BLACS");
    auto desca = FillDesc( A, context );
    auto descq = FillDesc( Q, context );

    // Reduce A to upper-Hessenberg form in an element-wise distribution
    // and form the explicit reflector matrix
    DistMatrix<F> AElem( A ), QElem( A.Grid() );
    DistMatrix<F,STAR,STAR> t( A.Grid() );
    Hessenberg( UPPER, AElem, t );
    // There is not yet a 'form Q'
    Identity( QElem, n, n );
    hessenberg::ApplyQ( UPPER, LEFT, NORMAL, AElem, t, QElem );
    MakeTrapezoidal( UPPER, AElem, -1 );
    A = AElem;
    Q = QElem;
    if( A.ColStride() != blacs::GridHeight(context) ||
        Q.ColStride() != blacs::GridHeight(context) )
        LogicError("Grid height did not match BLACS");
    if( A.RowStride() != blacs::GridWidth(context) ||
        Q.RowStride() != blacs::GridWidth(context) )
        LogicError("Grid width did not match BLACS");
    if( A.ColRank() != blacs::GridRow(context) ||
        Q.ColRank() != blacs::GridRow(context) )
        LogicError("Grid row did not match BLACS");
    if( A.RowRank() != blacs::GridCol(context) ||
        Q.RowRank() != blacs::GridCol(context) )
        LogicError("Grid col did not match BLACS");

    // Compute the Schur decomposition in block form, multiplying the
    // accumulated Householder reflectors from the right
    DistMatrix<Complex<Base<F>>,STAR,STAR> w_STAR_STAR( n, 1, A.Grid() );
    const bool multiplyQ = true;
    scalapack::HessenbergSchur
    ( n, A.Buffer(), desca.data(), w_STAR_STAR.Buffer(),
      Q.Buffer(), descq.data(), fullTriangle, multiplyQ, aed );
    w = w_STAR_STAR;

    blacs::FreeGrid( context );
    blacs::FreeHandle( bhandle );
    blacs::Exit();
#else
    LogicError("Distributed schur::QR currently requires ScaLAPACK support");
#endif
    if( fullTriangle )
        MakeTriangular( UPPER, A );
}

template<typename F>
inline void
QR
( DistMatrix<F>& A, DistMatrix<Complex<BASE(F)>,VR,STAR>& w,
  bool fullTriangle=false, bool aed=false )
{
    DEBUG_ONLY(CallStackEntry cse("schur::qr"))
#ifdef ELEM_HAVE_SCALAPACK
    // Reduce the matrix to upper-Hessenberg form in an elemental form
    DistMatrix<F,STAR,STAR> t( A.Grid() );
    Hessenberg( UPPER, A, t );
    MakeTrapezoidal( UPPER, A, -1 );

    // Run the QR algorithm in block form
    // TODO: Create schur::HessenbergQR
    const Int n = A.Height();
    const Int nb = DefaultBlockHeight();
    BlockDistMatrix<F> ABlock( n, n, A.Grid(), nb, nb );
    ABlock = A;
    const int bhandle = blacs::Handle( ABlock.DistComm().comm );
    const int context =
        blacs::GridInit
        ( bhandle, ABlock.Grid().Order()==COLUMN_MAJOR,
          ABlock.ColStride(), ABlock.RowStride() );
    if( ABlock.ColStride() != blacs::GridHeight(context) )
        LogicError("Grid height did not match BLACS");
    if( ABlock.RowStride() != blacs::GridWidth(context) )
        LogicError("Grid width did not match BLACS");
    if( ABlock.ColRank() != blacs::GridRow(context) )
        LogicError("Grid row did not match BLACS");
    if( ABlock.RowRank() != blacs::GridCol(context) )
        LogicError("Grid col did not match BLACS");
    blacs::Desc desca = FillDesc( ABlock, context );
    DistMatrix<Complex<Base<F>>,STAR,STAR> w_STAR_STAR( n, 1, A.Grid() );
    scalapack::HessenbergSchur
    ( n, ABlock.Buffer(), desca.data(), w_STAR_STAR.Buffer(),
      fullTriangle, aed );
    A = ABlock;
    w = w_STAR_STAR;

    blacs::FreeGrid( context );
    blacs::FreeHandle( bhandle );
    blacs::Exit();
#else
    LogicError("Distributed schur::QR currently requires ScaLAPACK support");
#endif
    if( fullTriangle )
        MakeTriangular( UPPER, A );
}

template<typename F>
inline void
QR
( DistMatrix<F>& A, DistMatrix<Complex<BASE(F)>,VR,STAR>& w, DistMatrix<F>& Q,
  bool fullTriangle=true, bool aed=false )
{
    DEBUG_ONLY(CallStackEntry cse("schur::qr"))
#ifdef ELEM_HAVE_SCALAPACK
    const Int n = A.Height();
    // Reduce A to upper-Hessenberg form in an element-wise distribution
    // and form the explicit reflector matrix
    DistMatrix<F,STAR,STAR> t( A.Grid() );
    Hessenberg( UPPER, A, t );
    // There is not yet a 'form Q'
    Identity( Q, n, n );
    hessenberg::ApplyQ( UPPER, LEFT, NORMAL, A, t, Q );
    MakeTrapezoidal( UPPER, A, -1 );

    // Run the Hessenberg QR algorithm in block form
    const Int nb = DefaultBlockHeight();
    BlockDistMatrix<F> ABlock( n, n, A.Grid(), nb, nb ),
                       QBlock( n, n, A.Grid(), nb, nb );
    ABlock = A;
    QBlock = Q;
    const int bhandle = blacs::Handle( ABlock.DistComm().comm );
    const int context =
        blacs::GridInit
        ( bhandle, ABlock.Grid().Order()==COLUMN_MAJOR,
          ABlock.ColStride(), ABlock.RowStride() );
    if( ABlock.ColStride() != blacs::GridHeight(context) ||
        QBlock.ColStride() != blacs::GridHeight(context) )
        LogicError("Grid height did not match BLACS");
    if( ABlock.RowStride() != blacs::GridWidth(context) ||
        QBlock.RowStride() != blacs::GridWidth(context) )
        LogicError("Grid width did not match BLACS");
    if( ABlock.ColRank() != blacs::GridRow(context) ||
        QBlock.ColRank() != blacs::GridRow(context) )
        LogicError("Grid row did not match BLACS");
    if( ABlock.RowRank() != blacs::GridCol(context) ||
        QBlock.RowRank() != blacs::GridCol(context) )
        LogicError("Grid col did not match BLACS");
    auto desca = FillDesc( ABlock, context );
    auto descq = FillDesc( QBlock, context );

    // Compute the Schur decomposition in block form, multiplying the
    // accumulated Householder reflectors from the right
    DistMatrix<Complex<Base<F>>,STAR,STAR> w_STAR_STAR( n, 1, A.Grid() );
    const bool multiplyQ = true;
    scalapack::HessenbergSchur
    ( n, ABlock.Buffer(), desca.data(), w_STAR_STAR.Buffer(),
      QBlock.Buffer(), descq.data(), fullTriangle, multiplyQ, aed );
    A = ABlock;
    w = w_STAR_STAR;
    Q = QBlock;

    blacs::FreeGrid( context );
    blacs::FreeHandle( bhandle );
    blacs::Exit();
#else
    LogicError("Distributed schur::QR currently requires ScaLAPACK support");
#endif
    if( fullTriangle )
        MakeTriangular( UPPER, A );
}

} // namespace schur
} // namespace elem

#endif // ifndef ELEM_SCHUR_QR_HPP
<|endoftext|>


<commit_before>/*
** Author(s):
**  - Cedric GESTES <gestes@aldebaran-robotics.com>
**
** Copyright (C) 2012 Cedric GESTES
*/

#include "remoteobject_p.hpp"
#include <qimessaging/message.hpp>
#include <qimessaging/transport_socket.hpp>
#include <qi/log.hpp>
#include <boost/thread/mutex.hpp>

namespace qi {

RemoteObject::RemoteObject(qi::TransportSocket *ts, unsigned int service, qi::MetaObject *mo)
  : _ts(ts)
  , _service(service)
{
  _ts->setCallbacks(this);
  //allocate by caller, but owned by us then
  _meta = mo;
}

RemoteObject::~RemoteObject()
{
  delete _ts;
  delete _meta;
}

void RemoteObject::onSocketReadyRead(TransportSocket *client, int id)
{
  qi::FunctorResult                          promise;
  qi::Message                                msg;
  std::map<int, qi::FunctorResult>::iterator it;

  client->read(id, &msg);

  {
    boost::mutex::scoped_lock lock(_mutex);
    it = _promises.find(id);
    if (it != _promises.end()) {
      promise = _promises[id];
      _promises.erase(it);
    }
  }

  if (promise.isValid()) {
    promise.setValue(msg.buffer());
  } else {
    qiLogError("remoteobject") << "no promise found for req id:" << id;
    return;
  }
}

void RemoteObject::metaCall(unsigned int method, const std::string &sig, const FunctorParameters &in, FunctorResult out)
{
  qi::Message msg;
  msg.setBuffer(in.buffer());
  qi::Message ret;
  msg.setType(qi::Message::Type_Call);
  msg.setService(_service);
  msg.setPath(qi::Message::Path_Main);
  //todo handle failure
  msg.setFunction(method);

  //allocated from caller, owned by us then. (clean up by onReadyRead)

  {
    boost::mutex::scoped_lock lock(_mutex);
    _promises[msg.id()] = out;
  }
  if (!_ts->send(msg)) {
    //TODO
    //out.setError(1);
    return;
  }
}

}
<commit_msg>remoteobject: avoid double free<commit_after>/*
** Author(s):
**  - Cedric GESTES <gestes@aldebaran-robotics.com>
**
** Copyright (C) 2012 Cedric GESTES
*/

#include "remoteobject_p.hpp"
#include <qimessaging/message.hpp>
#include <qimessaging/transport_socket.hpp>
#include <qi/log.hpp>
#include <boost/thread/mutex.hpp>

namespace qi {

RemoteObject::RemoteObject(qi::TransportSocket *ts, unsigned int service, qi::MetaObject *mo)
  : _ts(ts)
  , _service(service)
{
  _ts->setCallbacks(this);
  //allocate by caller, but owned by us then
  _meta = mo;
}

RemoteObject::~RemoteObject()
{
  delete _ts;
}

void RemoteObject::onSocketReadyRead(TransportSocket *client, int id)
{
  qi::FunctorResult                          promise;
  qi::Message                                msg;
  std::map<int, qi::FunctorResult>::iterator it;

  client->read(id, &msg);

  {
    boost::mutex::scoped_lock lock(_mutex);
    it = _promises.find(id);
    if (it != _promises.end()) {
      promise = _promises[id];
      _promises.erase(it);
    }
  }

  if (promise.isValid()) {
    promise.setValue(msg.buffer());
  } else {
    qiLogError("remoteobject") << "no promise found for req id:" << id;
    return;
  }
}

void RemoteObject::metaCall(unsigned int method, const std::string &sig, const FunctorParameters &in, FunctorResult out)
{
  qi::Message msg;
  msg.setBuffer(in.buffer());
  qi::Message ret;
  msg.setType(qi::Message::Type_Call);
  msg.setService(_service);
  msg.setPath(qi::Message::Path_Main);
  //todo handle failure
  msg.setFunction(method);

  //allocated from caller, owned by us then. (clean up by onReadyRead)

  {
    boost::mutex::scoped_lock lock(_mutex);
    _promises[msg.id()] = out;
  }
  if (!_ts->send(msg)) {
    qiLogError("remoteobject") << "error while sending answer";
    //TODO
    //out.setError(1);
    return;
  }
}

}
<|endoftext|>


<commit_before><commit_msg>Add defensive code in pickle to preclude realloc of shared header_ memory.<commit_after><|endoftext|>


<commit_before>#pragma once

#include <vector>

#include "nifty/math/math.hxx"
#include "nifty/histogram/histogram.hxx"
#include "nifty/cgp/geometry.hxx"
#include "nifty/cgp/bounds.hxx"
#include "nifty/marray/marray.hxx"
#include "nifty/filters/gaussian_curvature.hxx"
#include "nifty/features/accumulated_features.hxx"

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/point_xy.hpp>

namespace nifty{
namespace cgp{

    class Cell1CurvatureFeatures2D{
    private:
        typedef nifty::features::DefaultAccumulatedStatistics<float> AccType;
    public:
        Cell1CurvatureFeatures2D(
            const std::vector<float> & sigmas  = std::vector<float>({1.0f, 2.0f, 4.0f})
        )
        :   sigmas_(sigmas)
        {

        }

        size_t numberOfFeatures()const{
            return sigmas_.size() * AccType::NFeatures::value;
        }

        std::vector<std::string> names()const{

            std::string accNames [] = {
                std::string("Mean"),
                std::string("Sum"),
                std::string("Min"),
                std::string("Max"),
                std::string("Moment2"),
                std::string("Moment3"),
                std::string("Q0.10"),
                std::string("Q0.25"),
                std::string("Q0.50"),
                std::string("Q0.75"),
                std::string("Q0.90")
            };
            std::vector<std::string> res;
            auto baseName = std::string("GaussianCurvatureSigma");
            for(auto sigmaIndex=0; sigmaIndex<sigmas_.size(); ++sigmaIndex){
                auto name  = baseName + std::to_string(sigmas_[sigmaIndex]);
                for(const auto & accName : accNames){
                    name += accName;
                }
                res.push_back(name);
            }

        }

        template<class T>
        void operator()(
            const CellGeometryVector<2,1>  & cell1GeometryVector,
            const CellBoundedByVector<2,1> & cell1BoundedByVector,
            nifty::marray::View<T> & features
        )const{

            std::vector<float> curvature;
            std::vector<float> buffer(AccType::NFeatures::value);

            for(auto sigmaIndex=0; sigmaIndex<sigmas_.size(); ++sigmaIndex){
                const auto sigma = sigmas_[sigmaIndex];
                nifty::filters::GaussianCurvature2D<> op(sigma, -1, 2.5);

                for(auto cell1Index=0; cell1Index<cell1GeometryVector.size(); ++cell1Index){
                    const auto & geo = cell1GeometryVector[cell1Index];

                    if(geo.size()>=4){

                        // we use a larger size?
                        if(curvature.capacity() < geo.size()){
                            curvature.resize(geo.size()*2);
                        }
                        else{
                            curvature.resize(geo.size());
                        }

                        // calculate curvature
                        //std::cout<<"    is closed "<<"\n";
                        const auto closedLine = cell1BoundedByVector[cell1Index].size() == 0;
                        //std::cout<<"    calculate curvature "<<"\n";
                        op(geo.begin(), geo.end(), curvature.begin(), closedLine);

                        // accumulate the values
                        AccType acc;
                        for(auto pass=0; pass < acc.requiredPasses(); ++pass){
                            for(const auto & c : curvature){
                                acc.acc(c, pass);
                            }
                        }
                        // write to buffer
                        acc.result(buffer.begin(), buffer.end());

                        // write to features out
                        const auto fIndex = sigmaIndex*AccType::NFeatures::value;
                        for(auto afi=0; afi<AccType::NFeatures::value; ++afi){
                            features(cell1Index, fIndex + afi) = buffer[afi];
                        }

                    }
                    else{
                        // write to features out
                        // a zero value seems legit since we
                        // assume a constant zero curvature
                        const auto fIndex = sigmaIndex*AccType::NFeatures::value;
                        for(auto afi=0; afi<AccType::NFeatures::value; ++afi){
                            features(cell1Index, fIndex + afi) = 0.0;
                        }

                    }
                }
            }
        }
    private:
        std::vector<float> sigmas_;
    };

    class Cell1LineSegmentDist2D{
    private:
        typedef nifty::features::DefaultAccumulatedStatistics<float> AccType;
    public:
        Cell1LineSegmentDist2D(
            const std::vector<size_t> & dists  = std::vector<size_t>({size_t(3),size_t(5),size_t(7)})
        )
        :   dists_(dists)
        {
        }

        size_t numberOfFeatures()const{
            return  dists_.size()*AccType::NFeatures::value;
        }

        template<class T>
        void operator()(
            const CellGeometryVector<2,1>  & cell1GeometryVector,
            nifty::marray::View<T> & features
        )const{

            std::vector<float> buffer(AccType::NFeatures::value);

            typedef boost::geometry::model::d2::point_xy<double> point_type;
            typedef boost::geometry::model::linestring<point_type> linestring_type;

            for(auto di=0; di<dists_.size(); ++di){
                const auto ld = dists_[di];
                for(auto cell1Index=0; cell1Index<cell1GeometryVector.size(); ++cell1Index){
                    const auto & geo = cell1GeometryVector[cell1Index];
                    if(geo.size()>=4){

                        AccType acc;

                        for(auto pass=0; pass < acc.requiredPasses(); ++pass){

                            for(auto i=0; i<geo.size()-1; ++i){
                                const auto j = std::min(int(i + ld), int(geo.size()-1));
                                const auto & pS = geo[i];
                                const auto & pE = geo[j];

                                linestring_type line;
                                line.push_back(point_type(pS[0], pS[1]));
                                line.push_back(point_type(pE[0], pE[1]));

                                for(auto ii=i+1; ii<j-1; ++ii){
                                    const point_type p(geo[ii][0], geo[ii][1]);
                                    const auto d = boost::geometry::distance(p, line);
                                    acc.acc(d, pass);
                                }
                            }
                        }
                        // write to buffer
                        acc.result(buffer.begin(), buffer.end());

                        // write to features out
                        const auto fIndex = di*AccType::NFeatures::value;
                        for(auto afi=0; afi<AccType::NFeatures::value; ++afi){
                            features(cell1Index, fIndex + afi) = buffer[afi];
                        }

                    }
                    else{
                        // write to features out
                        // a zero value seems legit since we
                        // assume a constant zero curvature
                        const auto fIndex = di*AccType::NFeatures::value;
                        for(auto afi=0; afi<AccType::NFeatures::value; ++afi){
                            features(cell1Index, fIndex + afi) = 0.0;
                        }
                    }
                }
            }
        }
    private:
        std::vector<size_t> dists_;
    };

    class Cell1BasicGeometricFeatures2D{
    public:
        Cell1BasicGeometricFeatures2D(){
        }

        size_t numberOfFeatures()const{
            return 20;
        }

        template<class T>
        void operator()(
            const CellGeometryVector<2,1>   & cell1GeometryVector,
            const CellGeometryVector<2,2>   & cell2GeometryVector,
            const CellBoundsVector<2,1>     & cell1BoundsVector,
            nifty::marray::View<T> & features
        )const{
            using namespace nifty::math;
            for(auto cell1Index=0; cell1Index<cell1GeometryVector.size(); ++cell1Index){

                const auto & cell1Bounds = cell1BoundsVector[cell1Index];
                const auto cell2UIndex = cell1Bounds[0]-1;
                const auto cell2VIndex = cell1Bounds[1]-1;

                auto fIndex = 0;

                auto insertCell2ValFeats = [&](const float uVal, const float vVal){
                    features(cell1Index, fIndex++) = std::min(uVal, vVal);
                    features(cell1Index, fIndex++) = std::max(uVal, vVal);
                    features(cell1Index, fIndex++) = uVal + vVal;
                    features(cell1Index, fIndex++) = std::abs(uVal-vVal);
                };

                const auto & geoE = cell1GeometryVector[cell1Index];
                const auto & geoU = cell2GeometryVector[cell2UIndex];
                const auto & geoV = cell2GeometryVector[cell2VIndex];

                // size based features
                const auto eSize = float(geoE.size());
                const auto uSize = float(geoU.size());
                const auto vSize = float(geoV.size());

                features(cell1Index, fIndex++) = eSize;
                insertCell2ValFeats(uSize, vSize);

                // size ratios
                const auto uNSize = std::sqrt(uSize);
                const auto vNSize = std::sqrt(vSize);
                {
                    const auto ratU =  uNSize/eSize;
                    const auto ratV =  vNSize/eSize;
                    insertCell2ValFeats(ratU, ratV);
                }

                // endpoint distance and ratios
                const auto endpointDistance = euclideanDistance(geoE.front(), geoE.back());
                features(cell1Index, fIndex++) = endpointDistance;
                features(cell1Index, fIndex++) = endpointDistance/eSize;

                // distance between cell2CenterOfMass  and cell1CenterOfMass
                const auto  comE = geoE.centerOfMass();
                const auto  comU = geoU.centerOfMass();
                const auto  comV = geoV.centerOfMass();
                {
                    const auto dUV  = euclideanDistance(comU, comV);
                    const auto dUE  = euclideanDistance(comU, comE);
                    const auto dVE  = euclideanDistance(comV, comE);

                    features(cell1Index, fIndex++) = dUV;
                    insertCell2ValFeats(dUE, dVE);

                    // distance ratios between cell2CenterOfMass  and cell1CenterOfMass

                    const auto ratU =  uNSize/dUE;
                    const auto ratV =  vNSize/dVE;
                    insertCell2ValFeats(ratU, ratV);
                }

                // angle between cell2CenterOfMass  and cell1CenterOfMass
                // ...

            }

        }
    private:
        std::vector<size_t> dists_;
    };

    class Cell1Cell2CurvatureFeatures2D{
    };

    class Cell1GeoHistFeatures{
        typedef nifty::features::DefaultAccumulatedStatistics<float> AccType;
    public:
        Cell1GeoHistFeatures(){
        }

        size_t numberOfFeatures()const{
            return AccType::NFeatures::value;
        }

        template<class T>
        void operator()(
            const CellGeometryVector<2,1> & cellsGeometry,
            nifty::marray::View<T> & features
        )const{

            std::vector<float> buffer(AccType::NFeatures::value);
            for(auto cellIndex=0; cellIndex<cellsGeometry.size(); ++cellIndex){

                const auto & cellGeometry = cellsGeometry[cellIndex];
                const auto centerOfMass = cellGeometry.centerOfMass();

                const float minVal = 0.0;
                const float maxVal = std::exp(gamma_*1.0);

                histogram::Histogram<float> histogram(minVal, maxVal, bincount_);
                std::vector<float> out(quantiles_.size());

                AccType acc;

                for(auto i=0; i<cellGeometry.size(); ++i){
                    const auto & coord = cellGeometry[i];
                    const auto d  = nifty::math::euclideanDistance(coord, centerOfMass);
                    acc.acc(d);
                }

                // write to buffer
                acc.result(buffer.begin(), buffer.end());

                // write results
                for(auto i=0; i<buffer.size(); ++i){
                    features(cellIndex, i) = buffer[i];
                }

            }
        }

    private:

    };

}
}<commit_msg>fixed bug in geometric_features<commit_after>#pragma once

#include <vector>

#include "nifty/math/math.hxx"
#include "nifty/histogram/histogram.hxx"
#include "nifty/cgp/geometry.hxx"
#include "nifty/cgp/bounds.hxx"
#include "nifty/marray/marray.hxx"
#include "nifty/filters/gaussian_curvature.hxx"
#include "nifty/features/accumulated_features.hxx"

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/point_xy.hpp>

namespace nifty{
namespace cgp{

    class Cell1CurvatureFeatures2D{
    private:
        typedef nifty::features::DefaultAccumulatedStatistics<float> AccType;
    public:
        Cell1CurvatureFeatures2D(
            const std::vector<float> & sigmas  = std::vector<float>({1.0f, 2.0f, 4.0f})
        )
        :   sigmas_(sigmas)
        {

        }

        size_t numberOfFeatures()const{
            return sigmas_.size() * AccType::NFeatures::value;
        }

        std::vector<std::string> names()const{

            std::string accNames [] = {
                std::string("Mean"),
                std::string("Sum"),
                std::string("Min"),
                std::string("Max"),
                std::string("Moment2"),
                std::string("Moment3"),
                std::string("Q0.10"),
                std::string("Q0.25"),
                std::string("Q0.50"),
                std::string("Q0.75"),
                std::string("Q0.90")
            };
            std::vector<std::string> res;
            auto baseName = std::string("GaussianCurvatureSigma");
            for(auto sigmaIndex=0; sigmaIndex<sigmas_.size(); ++sigmaIndex){
                auto name  = baseName + std::to_string(sigmas_[sigmaIndex]);
                for(const auto & accName : accNames){
                    name += accName;
                }
                res.push_back(name);
            }

        }

        template<class T>
        void operator()(
            const CellGeometryVector<2,1>  & cell1GeometryVector,
            const CellBoundedByVector<2,1> & cell1BoundedByVector,
            nifty::marray::View<T> & features
        )const{

            std::vector<float> curvature;
            std::vector<float> buffer(AccType::NFeatures::value);

            for(auto sigmaIndex=0; sigmaIndex<sigmas_.size(); ++sigmaIndex){
                const auto sigma = sigmas_[sigmaIndex];
                nifty::filters::GaussianCurvature2D<> op(sigma, -1, 2.5);

                for(auto cell1Index=0; cell1Index<cell1GeometryVector.size(); ++cell1Index){
                    const auto & geo = cell1GeometryVector[cell1Index];

                    if(geo.size()>=4){

                        // we use a larger size?
                        if(curvature.capacity() < geo.size()){
                            curvature.resize(geo.size()*2);
                        }
                        else{
                            curvature.resize(geo.size());
                        }

                        // calculate curvature
                        //std::cout<<"    is closed "<<"\n";
                        const auto closedLine = cell1BoundedByVector[cell1Index].size() == 0;
                        //std::cout<<"    calculate curvature "<<"\n";
                        op(geo.begin(), geo.end(), curvature.begin(), closedLine);

                        // accumulate the values
                        AccType acc;
                        for(auto pass=0; pass < acc.requiredPasses(); ++pass){
                            for(const auto & c : curvature){
                                acc.acc(c, pass);
                            }
                        }
                        // write to buffer
                        acc.result(buffer.begin(), buffer.end());

                        // write to features out
                        const auto fIndex = sigmaIndex*AccType::NFeatures::value;
                        for(auto afi=0; afi<AccType::NFeatures::value; ++afi){
                            features(cell1Index, fIndex + afi) = buffer[afi];
                        }

                    }
                    else{
                        // write to features out
                        // a zero value seems legit since we
                        // assume a constant zero curvature
                        const auto fIndex = sigmaIndex*AccType::NFeatures::value;
                        for(auto afi=0; afi<AccType::NFeatures::value; ++afi){
                            features(cell1Index, fIndex + afi) = 0.0;
                        }

                    }
                }
            }
        }
    private:
        std::vector<float> sigmas_;
    };

    class Cell1LineSegmentDist2D{
    private:
        typedef nifty::features::DefaultAccumulatedStatistics<float> AccType;
    public:
        Cell1LineSegmentDist2D(
            const std::vector<size_t> & dists  = std::vector<size_t>({size_t(3),size_t(5),size_t(7)})
        )
        :   dists_(dists)
        {
        }

        size_t numberOfFeatures()const{
            return  dists_.size()*AccType::NFeatures::value;
        }

        template<class T>
        void operator()(
            const CellGeometryVector<2,1>  & cell1GeometryVector,
            nifty::marray::View<T> & features
        )const{

            std::vector<float> buffer(AccType::NFeatures::value);

            typedef boost::geometry::model::d2::point_xy<double> point_type;
            typedef boost::geometry::model::linestring<point_type> linestring_type;

            for(auto di=0; di<dists_.size(); ++di){
                const auto ld = dists_[di];
                for(auto cell1Index=0; cell1Index<cell1GeometryVector.size(); ++cell1Index){
                    const auto & geo = cell1GeometryVector[cell1Index];
                    if(geo.size()>=4){

                        AccType acc;

                        for(auto pass=0; pass < acc.requiredPasses(); ++pass){

                            for(auto i=0; i<geo.size()-1; ++i){
                                const auto j = std::min(int(i + ld), int(geo.size()-1));
                                const auto & pS = geo[i];
                                const auto & pE = geo[j];

                                linestring_type line;
                                line.push_back(point_type(pS[0], pS[1]));
                                line.push_back(point_type(pE[0], pE[1]));

                                for(auto ii=i+1; ii<j-1; ++ii){
                                    const point_type p(geo[ii][0], geo[ii][1]);
                                    const auto d = boost::geometry::distance(p, line);
                                    acc.acc(d, pass);
                                }
                            }
                        }
                        // write to buffer
                        acc.result(buffer.begin(), buffer.end());

                        // write to features out
                        const auto fIndex = di*AccType::NFeatures::value;
                        for(auto afi=0; afi<AccType::NFeatures::value; ++afi){
                            features(cell1Index, fIndex + afi) = buffer[afi];
                        }

                    }
                    else{
                        // write to features out
                        // a zero value seems legit since we
                        // assume a constant zero curvature
                        const auto fIndex = di*AccType::NFeatures::value;
                        for(auto afi=0; afi<AccType::NFeatures::value; ++afi){
                            features(cell1Index, fIndex + afi) = 0.0;
                        }
                    }
                }
            }
        }
    private:
        std::vector<size_t> dists_;
    };

    class Cell1BasicGeometricFeatures2D{
    public:
        Cell1BasicGeometricFeatures2D(){
        }

        size_t numberOfFeatures()const{
            return 20;
        }

        template<class T>
        void operator()(
            const CellGeometryVector<2,1>   & cell1GeometryVector,
            const CellGeometryVector<2,2>   & cell2GeometryVector,
            const CellBoundsVector<2,1>     & cell1BoundsVector,
            nifty::marray::View<T> & features
        )const{
            using namespace nifty::math;
            for(auto cell1Index=0; cell1Index<cell1GeometryVector.size(); ++cell1Index){

                const auto & cell1Bounds = cell1BoundsVector[cell1Index];
                const auto cell2UIndex = cell1Bounds[0]-1;
                const auto cell2VIndex = cell1Bounds[1]-1;

                auto fIndex = 0;

                auto insertCell2ValFeats = [&](const float uVal, const float vVal){
                    features(cell1Index, fIndex++) = std::min(uVal, vVal);
                    features(cell1Index, fIndex++) = std::max(uVal, vVal);
                    features(cell1Index, fIndex++) = uVal + vVal;
                    features(cell1Index, fIndex++) = std::abs(uVal-vVal);
                };

                const auto & geoE = cell1GeometryVector[cell1Index];
                const auto & geoU = cell2GeometryVector[cell2UIndex];
                const auto & geoV = cell2GeometryVector[cell2VIndex];

                // size based features
                const auto eSize = float(geoE.size());
                const auto uSize = float(geoU.size());
                const auto vSize = float(geoV.size());

                features(cell1Index, fIndex++) = eSize;
                insertCell2ValFeats(uSize, vSize);

                // size ratios
                const auto uNSize = std::sqrt(uSize);
                const auto vNSize = std::sqrt(vSize);
                {
                    const auto ratU =  uNSize/eSize;
                    const auto ratV =  vNSize/eSize;
                    insertCell2ValFeats(ratU, ratV);
                }

                // endpoint distance and ratios
                const auto endpointDistance = euclideanDistance(geoE.front(), geoE.back());
                features(cell1Index, fIndex++) = endpointDistance;
                features(cell1Index, fIndex++) = endpointDistance/eSize;

                // distance between cell2CenterOfMass  and cell1CenterOfMass
                const auto  comE = geoE.centerOfMass();
                const auto  comU = geoU.centerOfMass();
                const auto  comV = geoV.centerOfMass();
                {
                    const auto dUV  = euclideanDistance(comU, comV);
                    const auto dUE  = euclideanDistance(comU, comE);
                    const auto dVE  = euclideanDistance(comV, comE);

                    features(cell1Index, fIndex++) = dUV;
                    insertCell2ValFeats(dUE, dVE);

                    // distance ratios between cell2CenterOfMass  and cell1CenterOfMass

                    const auto ratU =  uNSize/dUE;
                    const auto ratV =  vNSize/dVE;
                    insertCell2ValFeats(ratU, ratV);
                }

                // angle between cell2CenterOfMass  and cell1CenterOfMass
                // ...

            }

        }
    private:
        std::vector<size_t> dists_;
    };

    class Cell1Cell2CurvatureFeatures2D{
    };

    class Cell1GeoHistFeatures{
        typedef nifty::features::DefaultAccumulatedStatistics<float> AccType;
    public:
        Cell1GeoHistFeatures(){
        }

        size_t numberOfFeatures()const{
            return AccType::NFeatures::value;
        }

        template<class T>
        void operator()(
            const CellGeometryVector<2,1> & cellsGeometry,
            nifty::marray::View<T> & features
        )const{

            std::vector<float> buffer(AccType::NFeatures::value);
            for(auto cellIndex=0; cellIndex<cellsGeometry.size(); ++cellIndex){

                const auto & cellGeometry = cellsGeometry[cellIndex];
                const auto centerOfMass = cellGeometry.centerOfMass();

                AccType acc;

                for(auto i=0; i<cellGeometry.size(); ++i){
                    const auto & coord = cellGeometry[i];
                    const auto d  = nifty::math::euclideanDistance(coord, centerOfMass);
                    acc.acc(d);
                }

                // write to buffer
                acc.result(buffer.begin(), buffer.end());

                // write results
                for(auto i=0; i<buffer.size(); ++i){
                    features(cellIndex, i) = buffer[i];
                }

            }
        }

    private:

    };

}
}<|endoftext|>


<commit_before>///////////////////////////////////////////////////////////////////////////////
//
// File: testNekMatrix.cpp
//
// For more information, please see: http://www.nektar.info
//
// The MIT License
//
// Copyright (c) 2006 Division of Applied Mathematics, Brown University (USA),
// Department of Aeronautics, Imperial College London (UK), and Scientific
// Computing and Imaging Institute, University of Utah (USA).
//
// License for the specific language governing rights and limitations under
// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included
// in all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.
//
// Description: Tests NekMatrix functionality.
//
///////////////////////////////////////////////////////////////////////////////

#include <UnitTests/testNekMatrix.h>
#include <LibUtilities/NekMatrix.hpp>

#include <boost/test/auto_unit_test.hpp>
#include <boost/test/test_case_template.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/progress.hpp>

namespace Nektar
{
    namespace UnitTests
    {
        using namespace Nektar::LibUtilities;

        void testNekMatrixConstruction()
        {
            // Basic, dense matrix construction.
            {
                double buf[] = { 1.0, 2.0, 3.0,
                                4.0, 5.0, 6.0,
                                7.0, 8.0, 9.0,
                                10.0, 11.0, 12.0 };

                NekMatrix<double> dynamic_matrix(4, 3, buf);

                BOOST_CHECK(dynamic_matrix.rows() == 4);
                BOOST_CHECK(dynamic_matrix.columns() == 3);

                for(unsigned int i = 0; i < 4; ++i)
                {
                    for(unsigned int j = 0; j < 3; ++j)
                    {
                        BOOST_CHECK(dynamic_matrix(i,j) == buf[3*i + j]);
                    }
                }
            }

            {

                NekMatrix<float> dynamic_matrix(7, 3, 7.8);

                for(unsigned int i = 0; i < 7; ++i)
                {
                    for(unsigned int j = 0; j < 3; ++j)
                    {
                        BOOST_CHECK(dynamic_matrix(i,j) == 7.8f);
                    }
                }
            }

        }

        void testNekMatrixAccess()
        {
            // We need to be able to access any element in the matrix, and
            // assign into the matrix at any location.
            NekMatrix<unsigned int> static_matrix(3,3);

            for(unsigned int i = 0; i < 3; ++i)
            {
                for(unsigned int j = 0; j < 3; ++j)
                {
                    static_matrix(i,j) = 10*i + j;
                }
            }

            for(unsigned int i = 0; i < 3; ++i)
            {
                for(unsigned int j = 0; j < 3; ++j)
                {
                    BOOST_CHECK(static_matrix(i,j) == 10*i + j);
                }
            }

            // Invalid access is an unrecoverable error.
            //BOOST_CHECK_THROW(static_matrix(3,2), OutOfBoundsError);
            //BOOST_CHECK_THROW(static_matrix(2,3), OutOfBoundsError);
            BOOST_CHECK_NO_THROW(static_matrix(2,2));

        }

        void testNekMatrixBasicMath()
        {
            // Addition tests.
            {
                double buf[] = {1.0, 2.0, 3.0,
                    4.0, 5.0, 6.0,
                    7.0, 8.0, 9.0 };

                NekMatrix<double> m1(3, 3, buf);
                NekMatrix<double> m2(3, 3, buf);
                NekMatrix<double> m3 = m1 + m2;

                for(unsigned int i = 0; i < 3; ++i)
                {
                    for(unsigned int j = 0; j < 3; ++j)
                    {
                        BOOST_CHECK(m3(i,j) == buf[3*i+j] + buf[3*i+j]);
                    }
                }

                NekMatrix<double> m4(3, 3, buf);
                NekMatrix<double> m5(3, 3, buf);
                NekMatrix<double> m6 = m4+m5;

                for(unsigned int i = 0; i < 3; ++i)
                {
                    for(unsigned int j = 0; j < 3; ++j)
                    {
                        BOOST_CHECK(m6(i,j) == buf[3*i+j] + buf[3*i+j]);
                    }
                }
            }
            // Multiply
            {
                unsigned int buf1[] = {1, 2, 3,
                                       4, 5, 6,
                                       7, 8, 9};
                unsigned int buf2[] = { 10, 11, 12, 14,
                                        15, 16, 17, 18,
                                        19, 20, 21, 22 };

                NekMatrix<unsigned int> lhs(3, 3, buf1);
                NekMatrix<unsigned int> rhs(3, 4, buf2);
                NekMatrix<unsigned int> result = lhs*rhs;

                BOOST_CHECK(result.rows() == 3);
                BOOST_CHECK(result.columns() == 4);

                BOOST_CHECK(result(0,0) == 97);
                BOOST_CHECK(result(0,1) == 103);
                BOOST_CHECK(result(0,2) == 109);
                BOOST_CHECK(result(0,3) == 116);

                BOOST_CHECK(result(1,0) == 229);
                BOOST_CHECK(result(1,1) == 244);
                BOOST_CHECK(result(1,2) == 259);
                BOOST_CHECK(result(1,3) == 278);

                BOOST_CHECK(result(2,0) == 361);
                BOOST_CHECK(result(2,1) == 385);
                BOOST_CHECK(result(2,2) == 409);
                BOOST_CHECK(result(2,3) == 440);
            }

            {
                unsigned int buf1[] = {1, 2, 3,
                                       4, 5, 6,
                                       7, 8, 9};
                unsigned int buf2[] = { 1, 2, 3};

                NekMatrix<unsigned int> lhs(3, 3, buf1);
                NekVector<unsigned int> rhs(3, buf2);

                NekVector<unsigned int> result = lhs*rhs;

                BOOST_CHECK(result[0] == 14);
                BOOST_CHECK(result[1] == 32);
                BOOST_CHECK(result[2] == 50);
            }

            // Transpose

            // Determinant.

            // Invert/Check for singularity.

            // Eigenvalues/vectors

            // Condition number wrt various norms.

            // Various norm computations.

            // LU Decomposition?  More appropriate in LinAlg?
        }

        void testDiagonalMatrix()
        {
        }
    }
}

/**
    $Log: testNekMatrix.cpp,v $
    Revision 1.8  2006/05/29 03:40:48  bnelson
    Updated the tests to reflect the changed parameter order in the NekMatrix constructor.

    Revision 1.7  2006/05/25 02:54:54  bnelson
    Added Matrix/Vector multiplication test.

    Revision 1.6  2006/05/18 04:25:19  bnelson
    Added a multiplication test.

    Revision 1.5  2006/05/16 20:35:30  jfrazier
    Added the float literal specifier to make the unit test happy.

    Revision 1.4  2006/05/15 05:06:07  bnelson
    Added addition tests.

    Revision 1.3  2006/05/15 04:10:35  bnelson
    no message

    Revision 1.2  2006/05/14 21:33:58  bnelson
    *** empty log message ***

    Revision 1.1  2006/05/07 21:10:09  bnelson
    *** empty log message ***

 **/

<commit_msg>Updated NekMatrix method names for the coding standard.<commit_after>///////////////////////////////////////////////////////////////////////////////
//
// File: testNekMatrix.cpp
//
// For more information, please see: http://www.nektar.info
//
// The MIT License
//
// Copyright (c) 2006 Division of Applied Mathematics, Brown University (USA),
// Department of Aeronautics, Imperial College London (UK), and Scientific
// Computing and Imaging Institute, University of Utah (USA).
//
// License for the specific language governing rights and limitations under
// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included
// in all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.
//
// Description: Tests NekMatrix functionality.
//
///////////////////////////////////////////////////////////////////////////////

#include <UnitTests/testNekMatrix.h>
#include <LibUtilities/NekMatrix.hpp>

#include <boost/test/auto_unit_test.hpp>
#include <boost/test/test_case_template.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/progress.hpp>

namespace Nektar
{
    namespace UnitTests
    {
        using namespace Nektar::LibUtilities;

        void testNekMatrixConstruction()
        {
            // Basic, dense matrix construction.
            {
                double buf[] = { 1.0, 2.0, 3.0,
                                4.0, 5.0, 6.0,
                                7.0, 8.0, 9.0,
                                10.0, 11.0, 12.0 };

                NekMatrix<double> dynamic_matrix(4, 3, buf);

                BOOST_CHECK(dynamic_matrix.GetRows() == 4);
                BOOST_CHECK(dynamic_matrix.GetColumns() == 3);

                for(unsigned int i = 0; i < 4; ++i)
                {
                    for(unsigned int j = 0; j < 3; ++j)
                    {
                        BOOST_CHECK(dynamic_matrix(i,j) == buf[3*i + j]);
                    }
                }
            }

            {

                NekMatrix<float> dynamic_matrix(7, 3, 7.8);

                for(unsigned int i = 0; i < 7; ++i)
                {
                    for(unsigned int j = 0; j < 3; ++j)
                    {
                        BOOST_CHECK(dynamic_matrix(i,j) == 7.8f);
                    }
                }
            }

        }

        void testNekMatrixAccess()
        {
            // We need to be able to access any element in the matrix, and
            // assign into the matrix at any location.
            NekMatrix<unsigned int> static_matrix(3,3);

            for(unsigned int i = 0; i < 3; ++i)
            {
                for(unsigned int j = 0; j < 3; ++j)
                {
                    static_matrix(i,j) = 10*i + j;
                }
            }

            for(unsigned int i = 0; i < 3; ++i)
            {
                for(unsigned int j = 0; j < 3; ++j)
                {
                    BOOST_CHECK(static_matrix(i,j) == 10*i + j);
                }
            }

            // Invalid access is an unrecoverable error.
            //BOOST_CHECK_THROW(static_matrix(3,2), OutOfBoundsError);
            //BOOST_CHECK_THROW(static_matrix(2,3), OutOfBoundsError);
            BOOST_CHECK_NO_THROW(static_matrix(2,2));

        }

        void testNekMatrixBasicMath()
        {
            // Addition tests.
            {
                double buf[] = {1.0, 2.0, 3.0,
                    4.0, 5.0, 6.0,
                    7.0, 8.0, 9.0 };

                NekMatrix<double> m1(3, 3, buf);
                NekMatrix<double> m2(3, 3, buf);
                NekMatrix<double> m3 = m1 + m2;

                for(unsigned int i = 0; i < 3; ++i)
                {
                    for(unsigned int j = 0; j < 3; ++j)
                    {
                        BOOST_CHECK(m3(i,j) == buf[3*i+j] + buf[3*i+j]);
                    }
                }

                NekMatrix<double> m4(3, 3, buf);
                NekMatrix<double> m5(3, 3, buf);
                NekMatrix<double> m6 = m4+m5;

                for(unsigned int i = 0; i < 3; ++i)
                {
                    for(unsigned int j = 0; j < 3; ++j)
                    {
                        BOOST_CHECK(m6(i,j) == buf[3*i+j] + buf[3*i+j]);
                    }
                }
            }
            // Multiply
            {
                unsigned int buf1[] = {1, 2, 3,
                                       4, 5, 6,
                                       7, 8, 9};
                unsigned int buf2[] = { 10, 11, 12, 14,
                                        15, 16, 17, 18,
                                        19, 20, 21, 22 };

                NekMatrix<unsigned int> lhs(3, 3, buf1);
                NekMatrix<unsigned int> rhs(3, 4, buf2);
                NekMatrix<unsigned int> result = lhs*rhs;

                BOOST_CHECK(result.GetRows() == 3);
                BOOST_CHECK(result.GetColumns() == 4);

                BOOST_CHECK(result(0,0) == 97);
                BOOST_CHECK(result(0,1) == 103);
                BOOST_CHECK(result(0,2) == 109);
                BOOST_CHECK(result(0,3) == 116);

                BOOST_CHECK(result(1,0) == 229);
                BOOST_CHECK(result(1,1) == 244);
                BOOST_CHECK(result(1,2) == 259);
                BOOST_CHECK(result(1,3) == 278);

                BOOST_CHECK(result(2,0) == 361);
                BOOST_CHECK(result(2,1) == 385);
                BOOST_CHECK(result(2,2) == 409);
                BOOST_CHECK(result(2,3) == 440);
            }

            {
                unsigned int buf1[] = {1, 2, 3,
                                       4, 5, 6,
                                       7, 8, 9};
                unsigned int buf2[] = { 1, 2, 3};

                NekMatrix<unsigned int> lhs(3, 3, buf1);
                NekVector<unsigned int> rhs(3, buf2);

                NekVector<unsigned int> result = lhs*rhs;

                BOOST_CHECK(result[0] == 14);
                BOOST_CHECK(result[1] == 32);
                BOOST_CHECK(result[2] == 50);
            }

            // Transpose

            // Determinant.

            // Invert/Check for singularity.

            // Eigenvalues/vectors

            // Condition number wrt various norms.

            // Various norm computations.

            // LU Decomposition?  More appropriate in LinAlg?
        }

        void testDiagonalMatrix()
        {
        }
    }
}

/**
    $Log: testNekMatrix.cpp,v $
    Revision 1.9  2006/05/31 04:19:36  bnelson
    Removed a test for invalid access to a matrix.

    Revision 1.8  2006/05/29 03:40:48  bnelson
    Updated the tests to reflect the changed parameter order in the NekMatrix constructor.

    Revision 1.7  2006/05/25 02:54:54  bnelson
    Added Matrix/Vector multiplication test.

    Revision 1.6  2006/05/18 04:25:19  bnelson
    Added a multiplication test.

    Revision 1.5  2006/05/16 20:35:30  jfrazier
    Added the float literal specifier to make the unit test happy.

    Revision 1.4  2006/05/15 05:06:07  bnelson
    Added addition tests.

    Revision 1.3  2006/05/15 04:10:35  bnelson
    no message

    Revision 1.2  2006/05/14 21:33:58  bnelson
    *** empty log message ***

    Revision 1.1  2006/05/07 21:10:09  bnelson
    *** empty log message ***

 **/

<|endoftext|>


<commit_before>#include "Player.hpp"

using namespace ild;

Entity JumpyPlayer::Create(JumpyGameSystems *gameSystems)
{
    auto entity = gameSystems->GetSystemManager().CreateEntity();
    auto physicsComponent = gameSystems->GetPhysics().CreateComponent(entity);
    physicsComponent->GetActions().SetAffectedByGravity(true);

    //Set up input
    auto inputHandler = new JumpyKeyboard();
    auto controlComponent = new JumpyInputComponent(entity, *physicsComponent, *inputHandler);
    gameSystems->GetInput().AddComponent(entity, controlComponent);

    auto drawable = new SpriteDrawable(&gameSystems->GetPhysics(),"flappy",0,0,Point());
    auto drawableComponent = gameSystems->GetDrawable().CreateComponent(entity);
    drawableComponent->AddDrawable("sprite", drawable);
    drawableComponent->FetchDependencies(entity);

    auto size = drawable->GetSize();
    gameSystems
        ->GetCollision()
        .CreateComponent(entity,sf::Vector3f(size.x,size.y,0), 0,BodyType::Solid);

    return entity;
}

<commit_msg>Finish implementing loading for JumpyGame.<commit_after>#include "Player.hpp"

using namespace ild;

Entity JumpyPlayer::Create(JumpyGameSystems *gameSystems)
{
    auto entity = gameSystems->GetSystemManager().GetEntity("player");
    auto physicsComponent = gameSystems->GetPhysics()[entity];

    //Set up input
    auto inputHandler = new JumpyKeyboard();
    auto controlComponent = new JumpyInputComponent(entity, *physicsComponent, *inputHandler);
    gameSystems->GetInput().AddComponent(entity, controlComponent);

    return entity;
}

<|endoftext|>


<commit_before>#include <iostream>

#include "drake/Path.h"
#include "drake/util/Polynomial.h"
#include "drake/systems/Simulation.h"
#include "drake/systems/plants/BotVisualizer.h"
#include "drake/systems/LCMSystem.h"
#include "drake/systems/cascade_system.h"
#include "drake/util/drakeAppUtil.h"
#include "drake/systems/plants/RigidBodySystem.h"
#include "drake/examples/kuka_iiwa_arm/robot_state_tap.h"

using Drake::RigidBodySystem;
using Drake::BotVisualizer;
using Eigen::VectorXd;
using drake::RobotStateTap;

int main(int argc, char* argv[]) {
  // Initializes LCM.
  std::shared_ptr<lcm::LCM> lcm = std::make_shared<lcm::LCM>();

  // Instantiates a rigid body system and adds the robot arm to it.
  auto rigid_body_sys = std::allocate_shared<RigidBodySystem>(
      Eigen::aligned_allocator<RigidBodySystem>());

  rigid_body_sys->addRobotFromFile(
    Drake::getDrakePath() + "/examples/kuka_iiwa_arm/urdf/iiwa14.urdf",
    DrakeJoint::FIXED);

  // Obtains a reference to the rigid body tree within the rigid body system.
  auto const& tree = rigid_body_sys->getRigidBodyTree();

  // Adds the ground.
  {
    double box_width = 3;
    double box_depth = 0.2;
    DrakeShapes::Box geom(Eigen::Vector3d(box_width, box_width, box_depth));
    Eigen::Isometry3d T_element_to_link = Eigen::Isometry3d::Identity();
    T_element_to_link.translation() << 0, 0,
        -box_depth / 2.0;  // top of the box is at z = 0
    RigidBody& world = tree->world();
    Eigen::Vector4d color;
    color << 0.9297, 0.7930, 0.6758,
        1;  // was hex2dec({'ee','cb','ad'})'/256 in matlab
    world.addVisualElement(
        DrakeShapes::VisualElement(geom, T_element_to_link, color));
    tree->addCollisionElement(
        RigidBody::CollisionElement(geom, T_element_to_link, &world), world,
        "terrain");
    tree->updateStaticCollisionElements();
  }

  // Sets the stiffness of the ground.
  {
    rigid_body_sys->penetration_stiffness = 3000.0;
    rigid_body_sys->penetration_damping = 0;
  }

  // Instantiates additional systems and cascades them with the rigid body
  // system.
  auto visualizer =
      std::make_shared<BotVisualizer<RigidBodySystem::StateVector>>(lcm, tree);

  auto robot_state_tap = std::make_shared<RobotStateTap<RigidBodySystem::StateVector>>();

  auto sys = cascade(cascade(rigid_body_sys, visualizer), robot_state_tap);

  // Obtains an initial state of the simulation.
  VectorXd x0 = VectorXd::Zero(rigid_body_sys->getNumStates());
  x0.head(tree->number_of_positions()) = tree->getZeroConfiguration();

  // Specifies the simulation options.
  Drake::SimulationOptions options;
  options.realtime_factor = 1.0;
  options.initial_step_size = 0.002;

  // Prevents exception from being thrown when simulation runs slower than real
  // time, which it most likely will given the small step size.
  options.warn_real_time_violation = true;

  // Starts the simulation.
  Drake::simulate(*sys.get(), 0, 5, x0, options);

  auto final_robot_state = robot_state_tap->get_input_vector();
  int num_positions = rigid_body_sys->number_of_positions();
  int num_velocities = rigid_body_sys->number_of_velocities();

  // Ensures the size of the output is correct.
  if (final_robot_state.size() != rigid_body_sys->getNumOutputs()) {
    throw std::runtime_error(
      "ERROR: Size of final robot state ("
      + std::to_string(final_robot_state.size())
      + ") does not match size of rigid body system's output ("
      + std::to_string(rigid_body_sys->getNumOutputs()) + ").");
  }

  // Ensures the number of position states equals the number of velocity states.
  if (num_positions != num_velocities) {
    throw std::runtime_error(
      "ERROR: Number of positions ("
      + std::to_string(num_positions)
      + ") does not match the number of velocities ("
      + std::to_string(num_velocities) + ").");
  }

  // Ensures the number of position and velocity states match the size of the
  // final robot state.
  if ((num_positions + num_velocities) != final_robot_state.size()) {
    throw std::runtime_error(
      "ERROR: Total number of positions and velocities ("
      + std::to_string(num_positions + num_velocities)
      + ") does not match size of robot state ("
      + std::to_string(final_robot_state.size()) + ").");
  }

  // Ensures the robot's joints are within their position limits.
  std::vector<std::unique_ptr<RigidBody>>& bodies = tree->bodies;
  for (int robot_state_index = 0, body_index = 0; body_index < bodies.size();
      ++body_index) {

    // Skips rigid bodies without a parent (this includes the world link).
    if (!bodies[body_index]->hasParent()) continue;

    const DrakeJoint& joint = bodies[body_index]->getJoint();
    const Eigen::VectorXd& min_limit = joint.getJointLimitMin();
    const Eigen::VectorXd& max_limit = joint.getJointLimitMax();

    for (int ii = 0; ii < joint.getNumPositions(); ++ii) {
      double position = final_robot_state[robot_state_index++];
      if (position < min_limit[ii]) {
        throw std::runtime_error("ERROR: Joint " + joint.getName() + " (DOF "
          + joint.getPositionName(ii) + ") violated minimum position limit ("
          + std::to_string(position) + " < "
          + std::to_string(min_limit[ii]) + ").");
      }
      if (position > max_limit[ii]) {
        throw std::runtime_error("ERROR: Joint " + joint.getName() + " (DOF "
          + joint.getPositionName(ii) + ") violated maximum position limit ("
          + std::to_string(position) + " > "
          + std::to_string(max_limit[ii]) + ").");
      }
    }
  }

  // Unfortunately we cannot check the joint velocities since the velocity
  // limits are not being parsed.
}
<commit_msg>Ran clang-format.<commit_after>#include <iostream>

#include "drake/Path.h"
#include "drake/examples/kuka_iiwa_arm/robot_state_tap.h"
#include "drake/systems/LCMSystem.h"
#include "drake/systems/Simulation.h"
#include "drake/systems/cascade_system.h"
#include "drake/systems/plants/BotVisualizer.h"
#include "drake/systems/plants/RigidBodySystem.h"
#include "drake/util/Polynomial.h"
#include "drake/util/drakeAppUtil.h"

using Drake::RigidBodySystem;
using Drake::BotVisualizer;
using Eigen::VectorXd;
using drake::RobotStateTap;

int main(int argc, char* argv[]) {
  // Initializes LCM.
  std::shared_ptr<lcm::LCM> lcm = std::make_shared<lcm::LCM>();

  // Instantiates a rigid body system and adds the robot arm to it.
  auto rigid_body_sys = std::allocate_shared<RigidBodySystem>(
      Eigen::aligned_allocator<RigidBodySystem>());

  rigid_body_sys->addRobotFromFile(
      Drake::getDrakePath() + "/examples/kuka_iiwa_arm/urdf/iiwa14.urdf",
      DrakeJoint::FIXED);

  // Obtains a reference to the rigid body tree within the rigid body system.
  auto const& tree = rigid_body_sys->getRigidBodyTree();

  // Adds the ground.
  {
    double box_width = 3;
    double box_depth = 0.2;
    DrakeShapes::Box geom(Eigen::Vector3d(box_width, box_width, box_depth));
    Eigen::Isometry3d T_element_to_link = Eigen::Isometry3d::Identity();
    T_element_to_link.translation() << 0, 0,
        -box_depth / 2.0;  // top of the box is at z = 0
    RigidBody& world = tree->world();
    Eigen::Vector4d color;
    color << 0.9297, 0.7930, 0.6758,
        1;  // was hex2dec({'ee','cb','ad'})'/256 in matlab
    world.addVisualElement(
        DrakeShapes::VisualElement(geom, T_element_to_link, color));
    tree->addCollisionElement(
        RigidBody::CollisionElement(geom, T_element_to_link, &world), world,
        "terrain");
    tree->updateStaticCollisionElements();
  }

  // Sets the stiffness of the ground.
  {
    rigid_body_sys->penetration_stiffness = 3000.0;
    rigid_body_sys->penetration_damping = 0;
  }

  // Instantiates additional systems and cascades them with the rigid body
  // system.
  auto visualizer =
      std::make_shared<BotVisualizer<RigidBodySystem::StateVector>>(lcm, tree);

  auto robot_state_tap =
      std::make_shared<RobotStateTap<RigidBodySystem::StateVector>>();

  auto sys = cascade(cascade(rigid_body_sys, visualizer), robot_state_tap);

  // Obtains an initial state of the simulation.
  VectorXd x0 = VectorXd::Zero(rigid_body_sys->getNumStates());
  x0.head(tree->number_of_positions()) = tree->getZeroConfiguration();

  // Specifies the simulation options.
  Drake::SimulationOptions options;
  options.realtime_factor = 1.0;
  options.initial_step_size = 0.002;

  // Prevents exception from being thrown when simulation runs slower than real
  // time, which it most likely will given the small step size.
  options.warn_real_time_violation = true;

  // Starts the simulation.
  Drake::simulate(*sys.get(), 0, 5, x0, options);

  auto final_robot_state = robot_state_tap->get_input_vector();
  int num_positions = rigid_body_sys->number_of_positions();
  int num_velocities = rigid_body_sys->number_of_velocities();

  // Ensures the size of the output is correct.
  if (final_robot_state.size() != rigid_body_sys->getNumOutputs()) {
    throw std::runtime_error(
        "ERROR: Size of final robot state (" +
        std::to_string(final_robot_state.size()) +
        ") does not match size of rigid body system's output (" +
        std::to_string(rigid_body_sys->getNumOutputs()) + ").");
  }

  // Ensures the number of position states equals the number of velocity states.
  if (num_positions != num_velocities) {
    throw std::runtime_error("ERROR: Number of positions (" +
                             std::to_string(num_positions) +
                             ") does not match the number of velocities (" +
                             std::to_string(num_velocities) + ").");
  }

  // Ensures the number of position and velocity states match the size of the
  // final robot state.
  if ((num_positions + num_velocities) != final_robot_state.size()) {
    throw std::runtime_error(
        "ERROR: Total number of positions and velocities (" +
        std::to_string(num_positions + num_velocities) +
        ") does not match size of robot state (" +
        std::to_string(final_robot_state.size()) + ").");
  }

  // Ensures the robot's joints are within their position limits.
  std::vector<std::unique_ptr<RigidBody>>& bodies = tree->bodies;
  for (int robot_state_index = 0, body_index = 0; body_index < bodies.size();
       ++body_index) {
    // Skips rigid bodies without a parent (this includes the world link).
    if (!bodies[body_index]->hasParent()) continue;

    const DrakeJoint& joint = bodies[body_index]->getJoint();
    const Eigen::VectorXd& min_limit = joint.getJointLimitMin();
    const Eigen::VectorXd& max_limit = joint.getJointLimitMax();

    for (int ii = 0; ii < joint.getNumPositions(); ++ii) {
      double position = final_robot_state[robot_state_index++];
      if (position < min_limit[ii]) {
        throw std::runtime_error("ERROR: Joint " + joint.getName() + " (DOF " +
                                 joint.getPositionName(ii) +
                                 ") violated minimum position limit (" +
                                 std::to_string(position) + " < " +
                                 std::to_string(min_limit[ii]) + ").");
      }
      if (position > max_limit[ii]) {
        throw std::runtime_error("ERROR: Joint " + joint.getName() + " (DOF " +
                                 joint.getPositionName(ii) +
                                 ") violated maximum position limit (" +
                                 std::to_string(position) + " > " +
                                 std::to_string(max_limit[ii]) + ").");
      }
    }
  }

  // Unfortunately we cannot check the joint velocities since the velocity
  // limits are not being parsed.
}
<|endoftext|>


<commit_before>/*************************************************************************
 *
 *  OpenOffice.org - a multi-platform office productivity suite
 *
 *  $RCSfile: basenode.hxx,v $
 *
 *  $Revision: 1.7 $
 *
 *  last change: $Author: rt $ $Date: 2006-07-26 07:35:12 $
 *
 *  The Contents of this file are made available subject to
 *  the terms of GNU Lesser General Public License Version 2.1.
 *
 *
 *    GNU Lesser General Public License Version 2.1
 *    =============================================
 *    Copyright 2005 by Sun Microsystems, Inc.
 *    901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *    This library is free software; you can redistribute it and/or
 *    modify it under the terms of the GNU Lesser General Public
 *    License version 2.1, as published by the Free Software Foundation.
 *
 *    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., 59 Temple Place, Suite 330, Boston,
 *    MA  02111-1307  USA
 *
 ************************************************************************/
#ifndef INCLUDED_SLIDESHOW_BASENODE_HXX
#define INCLUDED_SLIDESHOW_BASENODE_HXX

#include "canvas/debug.hxx"
#include "osl/diagnose.hxx"
#include "animationnode.hxx"
#include "slideshowcontext.hxx"
#include "shapesubset.hxx"
#include "boost/noncopyable.hpp"
#include <vector>

namespace presentation {
namespace internal {

/** Context for every node.

    Besides the global AnimationNodeFactory::Context data,
    this struct also contains the current DocTree subset
    for this node. If start and end index of the
    DocTreeNode are equal, the node should use the
    complete shape.
*/
struct NodeContext
{
    NodeContext( const SlideShowContext& rContext )
        : maContext( rContext ),
          mpMasterShapeSubset(),
          mnStartDelay(0.0),
          mbIsIndependentSubset( true )
        {}

    void dispose() { maContext.dispose(); }

    /// Context as passed to createAnimationNode()
    SlideShowContext        maContext;

    /// Shape to be used (provided by parent, e.g. for iterations)
    ShapeSubsetSharedPtr    mpMasterShapeSubset;

    /// Additional delay to node begin (to offset iterate effects)
    double                  mnStartDelay;

    /// When true, subset must be created during slide initialization
    bool                    mbIsIndependentSubset;
};

class BaseContainerNode;

/** This interface extends AnimationNode with some
    file-private accessor methods.
*/
class BaseNode : public AnimationNode,
                 private ::osl::DebugBase<BaseNode>,
                 private ::boost::noncopyable
{
public:
    BaseNode( ::com::sun::star::uno::Reference<
              ::com::sun::star::animations::XAnimationNode> const& xNode,
              ::boost::shared_ptr<BaseContainerNode> const& pParent,
              NodeContext const& rContext );

    /** Provide the node with a shared_ptr to itself.

        Since implementation has to create objects which need
        a shared_ptr to this node, and a pointee cannot
        retrieve a shared_ptr to itself internally, have to
        set that from the outside.
    */
    void setSelf( const ::boost::shared_ptr< BaseNode >& rSelf );

#if defined(VERBOSE) && defined(DBG_UTIL)
    virtual void showState() const;
    virtual const char* getDescription() const;
    void showTreeFromWithin() const;
#endif

    const ::boost::shared_ptr< BaseContainerNode >& getParentNode() const
        { return mpParent; }

    // Disposable:
    virtual void dispose();

    // AnimationNode:
    virtual bool init();
    virtual bool resolve();
    virtual bool activate();
    virtual void deactivate();
    virtual void end();
    virtual ::com::sun::star::uno::Reference<
        ::com::sun::star::animations::XAnimationNode> getXAnimationNode() const;
    virtual NodeState getState() const;
    virtual bool registerDeactivatingListener(
        const AnimationNodeSharedPtr& rNotifee );
    // nop:
    virtual void notifyDeactivating( const AnimationNodeSharedPtr& rNotifier );

protected:
    void scheduleDeactivationEvent( EventSharedPtr const& pEvent =
                                    EventSharedPtr() );

    SlideShowContext const& getContext() const { return maContext; }

    ::boost::shared_ptr<BaseNode> const& getSelf() const { return mpSelf; }

    bool isMainSequenceRootNode() const { return mbIsMainSequenceRootNode; }

    bool checkValidNode() const {
        ENSURE_AND_THROW( mpSelf, "no self ptr set!" );
        bool const bRet = (meCurrState != INVALID);
        OSL_ENSURE( bRet, "### INVALID node!" );
        return bRet;
    }

private:
    // all state affecting methods have "_st" counterparts being called at
    // derived classes when in state transistion: no-ops here at BaseNode...
    virtual bool init_st();
    virtual bool resolve_st();
    virtual void activate_st();
    virtual void deactivate_st( NodeState eDestState );

private:
    /// notifies
    /// - all registered deactivation listeners
    /// - single animation end (every node)
    /// - slide animations (if main sequence root node)
    void notifyEndListeners() const;

    /// Get the node's restart mode
    sal_Int16 getRestartMode();

    /** Get the default restart mode

        If this node's default mode is
        AnimationRestart::DEFAULT, this method recursively
        calls the parent node.
    */
    sal_Int16 getRestartDefaultMode() const;

    /// Get the node's fill mode
    sal_Int16 getFillMode();

    /** Get the default fill mode.

        If this node's default mode is AnimationFill::DEFAULT,
        this method recursively calls the parent node.
    */
    sal_Int16 getFillDefaultMode() const;

    bool isTransition( NodeState eFromState, NodeState eToState,
                       bool debugAssert = true ) const {
        bool const bRet =((mpStateTransitionTable[eFromState] & eToState) != 0);
        OSL_ENSURE( !debugAssert || bRet, "### state unreachable!" );
        return bRet;
    }

    bool inStateOrTransition( int mask ) const {
        return ((meCurrState & mask) != 0 ||
                (meCurrentStateTransition & mask) != 0);
    }

    class StateTransition;
    friend class StateTransition;

private:
    SlideShowContext                                   maContext;

    typedef ::std::vector< AnimationNodeSharedPtr >    ListenerVector;

    ListenerVector                                     maDeactivatingListeners;
    ::com::sun::star::uno::Reference<
        ::com::sun::star::animations::XAnimationNode > mxAnimationNode;
    ::boost::shared_ptr< BaseContainerNode >           mpParent;
    ::boost::shared_ptr< BaseNode >                    mpSelf;
    const int*                                         mpStateTransitionTable;
    const double                                       mnStartDelay;
    NodeState                                          meCurrState;
    int                                                meCurrentStateTransition;
    EventSharedPtr                                     mpCurrentEvent;
    const bool                                         mbIsMainSequenceRootNode;
};

typedef ::boost::shared_ptr< BaseNode > BaseNodeSharedPtr;

} // namespace internal
} // namespace presentation

#endif /* INCLUDED_SLIDESHOW_BASENODE_HXX */

<commit_msg>#i10000# ::osl::DebugBase<DrawShape> must not be private<commit_after>/*************************************************************************
 *
 *  OpenOffice.org - a multi-platform office productivity suite
 *
 *  $RCSfile: basenode.hxx,v $
 *
 *  $Revision: 1.8 $
 *
 *  last change: $Author: rt $ $Date: 2006-07-26 15:05:48 $
 *
 *  The Contents of this file are made available subject to
 *  the terms of GNU Lesser General Public License Version 2.1.
 *
 *
 *    GNU Lesser General Public License Version 2.1
 *    =============================================
 *    Copyright 2005 by Sun Microsystems, Inc.
 *    901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *    This library is free software; you can redistribute it and/or
 *    modify it under the terms of the GNU Lesser General Public
 *    License version 2.1, as published by the Free Software Foundation.
 *
 *    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., 59 Temple Place, Suite 330, Boston,
 *    MA  02111-1307  USA
 *
 ************************************************************************/
#ifndef INCLUDED_SLIDESHOW_BASENODE_HXX
#define INCLUDED_SLIDESHOW_BASENODE_HXX

#include "canvas/debug.hxx"
#include "osl/diagnose.hxx"
#include "animationnode.hxx"
#include "slideshowcontext.hxx"
#include "shapesubset.hxx"
#include "boost/noncopyable.hpp"
#include <vector>

namespace presentation {
namespace internal {

/** Context for every node.

    Besides the global AnimationNodeFactory::Context data,
    this struct also contains the current DocTree subset
    for this node. If start and end index of the
    DocTreeNode are equal, the node should use the
    complete shape.
*/
struct NodeContext
{
    NodeContext( const SlideShowContext& rContext )
        : maContext( rContext ),
          mpMasterShapeSubset(),
          mnStartDelay(0.0),
          mbIsIndependentSubset( true )
        {}

    void dispose() { maContext.dispose(); }

    /// Context as passed to createAnimationNode()
    SlideShowContext        maContext;

    /// Shape to be used (provided by parent, e.g. for iterations)
    ShapeSubsetSharedPtr    mpMasterShapeSubset;

    /// Additional delay to node begin (to offset iterate effects)
    double                  mnStartDelay;

    /// When true, subset must be created during slide initialization
    bool                    mbIsIndependentSubset;
};

class BaseContainerNode;

/** This interface extends AnimationNode with some
    file-private accessor methods.
*/
class BaseNode : public AnimationNode,
                 protected ::osl::DebugBase<BaseNode>,
                 private ::boost::noncopyable
{
public:
    BaseNode( ::com::sun::star::uno::Reference<
              ::com::sun::star::animations::XAnimationNode> const& xNode,
              ::boost::shared_ptr<BaseContainerNode> const& pParent,
              NodeContext const& rContext );

    /** Provide the node with a shared_ptr to itself.

        Since implementation has to create objects which need
        a shared_ptr to this node, and a pointee cannot
        retrieve a shared_ptr to itself internally, have to
        set that from the outside.
    */
    void setSelf( const ::boost::shared_ptr< BaseNode >& rSelf );

#if defined(VERBOSE) && defined(DBG_UTIL)
    virtual void showState() const;
    virtual const char* getDescription() const;
    void showTreeFromWithin() const;
#endif

    const ::boost::shared_ptr< BaseContainerNode >& getParentNode() const
        { return mpParent; }

    // Disposable:
    virtual void dispose();

    // AnimationNode:
    virtual bool init();
    virtual bool resolve();
    virtual bool activate();
    virtual void deactivate();
    virtual void end();
    virtual ::com::sun::star::uno::Reference<
        ::com::sun::star::animations::XAnimationNode> getXAnimationNode() const;
    virtual NodeState getState() const;
    virtual bool registerDeactivatingListener(
        const AnimationNodeSharedPtr& rNotifee );
    // nop:
    virtual void notifyDeactivating( const AnimationNodeSharedPtr& rNotifier );

protected:
    void scheduleDeactivationEvent( EventSharedPtr const& pEvent =
                                    EventSharedPtr() );

    SlideShowContext const& getContext() const { return maContext; }

    ::boost::shared_ptr<BaseNode> const& getSelf() const { return mpSelf; }

    bool isMainSequenceRootNode() const { return mbIsMainSequenceRootNode; }

    bool checkValidNode() const {
        ENSURE_AND_THROW( mpSelf, "no self ptr set!" );
        bool const bRet = (meCurrState != INVALID);
        OSL_ENSURE( bRet, "### INVALID node!" );
        return bRet;
    }

private:
    // all state affecting methods have "_st" counterparts being called at
    // derived classes when in state transistion: no-ops here at BaseNode...
    virtual bool init_st();
    virtual bool resolve_st();
    virtual void activate_st();
    virtual void deactivate_st( NodeState eDestState );

private:
    /// notifies
    /// - all registered deactivation listeners
    /// - single animation end (every node)
    /// - slide animations (if main sequence root node)
    void notifyEndListeners() const;

    /// Get the node's restart mode
    sal_Int16 getRestartMode();

    /** Get the default restart mode

        If this node's default mode is
        AnimationRestart::DEFAULT, this method recursively
        calls the parent node.
    */
    sal_Int16 getRestartDefaultMode() const;

    /// Get the node's fill mode
    sal_Int16 getFillMode();

    /** Get the default fill mode.

        If this node's default mode is AnimationFill::DEFAULT,
        this method recursively calls the parent node.
    */
    sal_Int16 getFillDefaultMode() const;

    bool isTransition( NodeState eFromState, NodeState eToState,
                       bool debugAssert = true ) const {
        bool const bRet =((mpStateTransitionTable[eFromState] & eToState) != 0);
        OSL_ENSURE( !debugAssert || bRet, "### state unreachable!" );
        return bRet;
    }

    bool inStateOrTransition( int mask ) const {
        return ((meCurrState & mask) != 0 ||
                (meCurrentStateTransition & mask) != 0);
    }

    class StateTransition;
    friend class StateTransition;

private:
    SlideShowContext                                   maContext;

    typedef ::std::vector< AnimationNodeSharedPtr >    ListenerVector;

    ListenerVector                                     maDeactivatingListeners;
    ::com::sun::star::uno::Reference<
        ::com::sun::star::animations::XAnimationNode > mxAnimationNode;
    ::boost::shared_ptr< BaseContainerNode >           mpParent;
    ::boost::shared_ptr< BaseNode >                    mpSelf;
    const int*                                         mpStateTransitionTable;
    const double                                       mnStartDelay;
    NodeState                                          meCurrState;
    int                                                meCurrentStateTransition;
    EventSharedPtr                                     mpCurrentEvent;
    const bool                                         mbIsMainSequenceRootNode;
};

typedef ::boost::shared_ptr< BaseNode > BaseNodeSharedPtr;

} // namespace internal
} // namespace presentation

#endif /* INCLUDED_SLIDESHOW_BASENODE_HXX */

<|endoftext|>