<commit_before>/*---------------------------------------------------------------------------*\
 *                                OpenSG                                     *
 *                                                                           *
 *                                                                           *
 *               Copyright (C) 2000-2006 by the OpenSG Forum                 *
 *                                                                           *
 *                            www.opensg.org                                 *
 *                                                                           *
 *   contact: dirk@opensg.org, gerrit.voss@vossg.org, jbehr@zgdv.de          *
 *                                                                           *
\*---------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------*\
 *                                License                                    *
 *                                                                           *
 * This library is free software; you can redistribute it and/or modify it   *
 * under the terms of the GNU Library General Public License as published    *
 * by the Free Software Foundation, version 2.                               *
 *                                                                           *
 * 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         *
 * Library General Public License for more details.                          *
 *                                                                           *
 * You should have received a copy of the GNU Library General Public         *
 * License along with this library; if not, write to the Free Software       *
 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.                 *
 *                                                                           *
\*---------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------*\
 *                                Changes                                    *
 *                                                                           *
 *                                                                           *
 *                                                                           *
 *                                                                           *
 *                                                                           *
 *                                                                           *
\*---------------------------------------------------------------------------*/

//---------------------------------------------------------------------------
//  Includes
//---------------------------------------------------------------------------

#include <cstdlib>
#include <cstdio>

#include "OSGConfig.h"

#include "OSGSkeletonJoint.h"
#include "OSGAnimation.h"
#include "OSGAnimBindAction.h"
#include "OSGAnimChannel.h"
#include "OSGAnimDataSource.h"
#include "OSGAnimTargetAttachment.h"
#include "OSGRenderAction.h"

#include <boost/cast.hpp>

OSG_BEGIN_NAMESPACE

// Documentation for this class is emitted in the
// OSGSkeletonJointBase.cpp file.
// To modify it, please change the .fcd file (OSGSkeletonJoint.fcd) and
// regenerate the base file.

/***************************************************************************\
 *                           Class variables                               *
\***************************************************************************/

//const Int16 SkeletonJoint::INVALID_JOINT_ID;

/***************************************************************************\
 *                           Class methods                                 *
\***************************************************************************/

void SkeletonJoint::initMethod(InitPhase ePhase)
{
    Inherited::initMethod(ePhase);

    if(ePhase == TypeObject::SystemPost)
    {
        RenderAction::registerEnterDefault(
            SkeletonJoint::getClassType(),
            reinterpret_cast<Action::Callback>(&SkeletonJoint::renderEnter));
        RenderAction::registerLeaveDefault(
            SkeletonJoint::getClassType(),
            reinterpret_cast<Action::Callback>(&SkeletonJoint::renderLeave));

        AnimBindAction::registerEnterDefault(
            SkeletonJoint::getClassType(),
            reinterpret_cast<Action::Callback>(&SkeletonJoint::animBindEnter));
    }
}

/***************************************************************************\
 *                           Instance methods                              *
\***************************************************************************/

/*-------------------------------------------------------------------------*\
 -  private                                                                 -
\*-------------------------------------------------------------------------*/

/*----------------------- constructors & destructors ----------------------*/

SkeletonJoint::SkeletonJoint(void)
    : Inherited()
{
}

SkeletonJoint::SkeletonJoint(const SkeletonJoint &source)
    : Inherited(source)
{
}

SkeletonJoint::~SkeletonJoint(void)
{
}

/*----------------------------- class specific ----------------------------*/

void SkeletonJoint::changed(ConstFieldMaskArg whichField,
                            UInt32            origin,
                            BitVector         details)
{
    if(((InvBindMatrixFieldMask |
         MatrixFieldMask          ) & whichField) != 0x0000)
    {
        invalidateVolume();

        if(_sfSkeleton.getValue() != NULL)
            _sfSkeleton.getValue()->editSFJointsChanged();
    }

    Inherited::changed(whichField, origin, details);
}

Action::ResultE
SkeletonJoint::renderEnter(Action *action)
{
    Action::ResultE  res  = Action::Continue;
    RenderAction    *ract =
        boost::polymorphic_downcast<RenderAction *>(action);
    Skeleton        *skel = getSkeleton();

#ifdef OSG_DEBUG
    if(_sfJointId.getValue() == INVALID_JOINT_ID)
    {
        SWARNING << "SkeletonJoint::renderEnter: Joint has invalid jointId. "
                 << "Ignoring." << std::endl;
        return res;
    }

    if(skel == NULL)
    {
        SWARNING << "SkeletonJoint::renderEnter: Joint has no skeleton. "
                 << "Ignoring." << std::endl;
        return res;
    }
#endif

    Int16                                jointId    = _sfJointId.getValue();
    Skeleton::MFJointMatricesType       *jointMats  =
        skel->editMFJointMatrices();
    Skeleton::MFJointNormalMatricesType *jointNMats =
        skel->editMFJointNormalMatrices();

    ract->pushMatrix(_sfMatrix.getValue());

    Matrix jointMat(ract->topMatrix());

    if(skel->getUseInvBindMatrix() == true)
        jointMat.mult(_sfInvBindMatrix.getValue());

    (*jointMats)[jointId] = jointMat;

    if(skel->getCalcNormalMatrices() == true)
    {
        jointMat.invert   ();
        jointMat.transpose();

        (*jointNMats)[jointId] = jointMat;
    }

    return res;
}

Action::ResultE
SkeletonJoint::renderLeave(Action *action)
{
    Action::ResultE  res  = Action::Continue;
    RenderAction    *ract =
        boost::polymorphic_downcast<RenderAction *>(action);
    Skeleton        *skel = getSkeleton();

    // need to make symmetric checks to renderEnter, otherwise the
    // matrix stack gets corrupted
#ifdef OSG_DEBUG
    if(_sfJointId.getValue() == INVALID_JOINT_ID)
    {
        SWARNING << "SkeletonJoint::renderLeave: Joint has invalid jointId. "
                 << "Ignoring." << std::endl;
        return res;
    }

    if(skel == NULL)
    {
        SWARNING << "SkeletonJoint::renderLeave: Joint has no skeleton. "
                 << "Ignoring." << std::endl;
        return res;
    }
#endif

    ract->popMatrix();

    return res;
}

Action::ResultE
SkeletonJoint::animBindEnter(Action *action)
{
    Action::ResultE  res     = Action::Continue;
    AnimBindAction  *bindAct =
        boost::polymorphic_downcast<AnimBindAction *>(action);

    SLOG << "SkeletonJoint::animBindEnter: jointId ["
         << _sfJointId.getValue() << "]" << std::endl;

    bindAct->bindFields(this);

    return res;
}

void
SkeletonJoint::accumulateMatrix(Matrix &result)
{
    result.mult(_sfMatrix.getValue());
}

void
SkeletonJoint::adjustVolume(Volume &volume)
{
    volume.transform(_sfMatrix.getValue()   );
    volume.extendBy (Pnt3f( Eps,  Eps,  Eps));
    volume.extendBy (Pnt3f(-Eps, -Eps, -Eps));

//     volume.extendBy (Pnt3f(-0.01f, -0.01f, -0.01f));
//     volume.extendBy (Pnt3f( 0.01f,  0.01f,  0.01f));
}

void SkeletonJoint::dump(      UInt32    ,
                         const BitVector ) const
{
    SLOG << "Dump SkeletonJoint NI" << std::endl;
}

OSG_END_NAMESPACE
<commit_msg>[PATCH 13/19] fixed: debug printout<commit_after>/*---------------------------------------------------------------------------*\
 *                                OpenSG                                     *
 *                                                                           *
 *                                                                           *
 *               Copyright (C) 2000-2006 by the OpenSG Forum                 *
 *                                                                           *
 *                            www.opensg.org                                 *
 *                                                                           *
 *   contact: dirk@opensg.org, gerrit.voss@vossg.org, jbehr@zgdv.de          *
 *                                                                           *
\*---------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------*\
 *                                License                                    *
 *                                                                           *
 * This library is free software; you can redistribute it and/or modify it   *
 * under the terms of the GNU Library General Public License as published    *
 * by the Free Software Foundation, version 2.                               *
 *                                                                           *
 * 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         *
 * Library General Public License for more details.                          *
 *                                                                           *
 * You should have received a copy of the GNU Library General Public         *
 * License along with this library; if not, write to the Free Software       *
 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.                 *
 *                                                                           *
\*---------------------------------------------------------------------------*/
/*---------------------------------------------------------------------------*\
 *                                Changes                                    *
 *                                                                           *
 *                                                                           *
 *                                                                           *
 *                                                                           *
 *                                                                           *
 *                                                                           *
\*---------------------------------------------------------------------------*/

//---------------------------------------------------------------------------
//  Includes
//---------------------------------------------------------------------------

#include <cstdlib>
#include <cstdio>

#include "OSGConfig.h"

#include "OSGSkeletonJoint.h"
#include "OSGAnimation.h"
#include "OSGAnimBindAction.h"
#include "OSGAnimChannel.h"
#include "OSGAnimDataSource.h"
#include "OSGAnimTargetAttachment.h"
#include "OSGRenderAction.h"

#include <boost/cast.hpp>

OSG_BEGIN_NAMESPACE

// Documentation for this class is emitted in the
// OSGSkeletonJointBase.cpp file.
// To modify it, please change the .fcd file (OSGSkeletonJoint.fcd) and
// regenerate the base file.

/***************************************************************************\
 *                           Class variables                               *
\***************************************************************************/

//const Int16 SkeletonJoint::INVALID_JOINT_ID;

/***************************************************************************\
 *                           Class methods                                 *
\***************************************************************************/

void SkeletonJoint::initMethod(InitPhase ePhase)
{
    Inherited::initMethod(ePhase);

    if(ePhase == TypeObject::SystemPost)
    {
        RenderAction::registerEnterDefault(
            SkeletonJoint::getClassType(),
            reinterpret_cast<Action::Callback>(&SkeletonJoint::renderEnter));
        RenderAction::registerLeaveDefault(
            SkeletonJoint::getClassType(),
            reinterpret_cast<Action::Callback>(&SkeletonJoint::renderLeave));

        AnimBindAction::registerEnterDefault(
            SkeletonJoint::getClassType(),
            reinterpret_cast<Action::Callback>(&SkeletonJoint::animBindEnter));
    }
}

/***************************************************************************\
 *                           Instance methods                              *
\***************************************************************************/

/*-------------------------------------------------------------------------*\
 -  private                                                                 -
\*-------------------------------------------------------------------------*/

/*----------------------- constructors & destructors ----------------------*/

SkeletonJoint::SkeletonJoint(void)
    : Inherited()
{
}

SkeletonJoint::SkeletonJoint(const SkeletonJoint &source)
    : Inherited(source)
{
}

SkeletonJoint::~SkeletonJoint(void)
{
}

/*----------------------------- class specific ----------------------------*/

void SkeletonJoint::changed(ConstFieldMaskArg whichField,
                            UInt32            origin,
                            BitVector         details)
{
    if(((InvBindMatrixFieldMask |
         MatrixFieldMask          ) & whichField) != 0x0000)
    {
        invalidateVolume();

        if(_sfSkeleton.getValue() != NULL)
            _sfSkeleton.getValue()->editSFJointsChanged();
    }

    Inherited::changed(whichField, origin, details);
}

Action::ResultE
SkeletonJoint::renderEnter(Action *action)
{
    Action::ResultE  res  = Action::Continue;
    RenderAction    *ract =
        boost::polymorphic_downcast<RenderAction *>(action);
    Skeleton        *skel = getSkeleton();

#ifdef OSG_DEBUG
    if(_sfJointId.getValue() == INVALID_JOINT_ID)
    {
        SWARNING << "SkeletonJoint::renderEnter: Joint has invalid jointId. "
                 << "Ignoring." << std::endl;
        return res;
    }

    if(skel == NULL)
    {
        SWARNING << "SkeletonJoint::renderEnter: Joint has no skeleton. "
                 << "Ignoring." << std::endl;
        return res;
    }
#endif

    Int16                                jointId    = _sfJointId.getValue();
    Skeleton::MFJointMatricesType       *jointMats  =
        skel->editMFJointMatrices();
    Skeleton::MFJointNormalMatricesType *jointNMats =
        skel->editMFJointNormalMatrices();

    ract->pushMatrix(_sfMatrix.getValue());

    Matrix jointMat(ract->topMatrix());

    if(skel->getUseInvBindMatrix() == true)
        jointMat.mult(_sfInvBindMatrix.getValue());

    (*jointMats)[jointId] = jointMat;

    if(skel->getCalcNormalMatrices() == true)
    {
        jointMat.invert   ();
        jointMat.transpose();

        (*jointNMats)[jointId] = jointMat;
    }

    return res;
}

Action::ResultE
SkeletonJoint::renderLeave(Action *action)
{
    Action::ResultE  res  = Action::Continue;
    RenderAction    *ract =
        boost::polymorphic_downcast<RenderAction *>(action);
    Skeleton        *skel = getSkeleton();

    // need to make symmetric checks to renderEnter, otherwise the
    // matrix stack gets corrupted
#ifdef OSG_DEBUG
    if(_sfJointId.getValue() == INVALID_JOINT_ID)
    {
        SWARNING << "SkeletonJoint::renderLeave: Joint has invalid jointId. "
                 << "Ignoring." << std::endl;
        return res;
    }

    if(skel == NULL)
    {
        SWARNING << "SkeletonJoint::renderLeave: Joint has no skeleton. "
                 << "Ignoring." << std::endl;
        return res;
    }
#endif

    ract->popMatrix();

    return res;
}

Action::ResultE
SkeletonJoint::animBindEnter(Action *action)
{
    Action::ResultE  res     = Action::Continue;
    AnimBindAction  *bindAct =
        boost::polymorphic_downcast<AnimBindAction *>(action);

    bindAct->bindFields(this);

    return res;
}

void
SkeletonJoint::accumulateMatrix(Matrix &result)
{
    result.mult(_sfMatrix.getValue());
}

void
SkeletonJoint::adjustVolume(Volume &volume)
{
    volume.transform(_sfMatrix.getValue()   );
    volume.extendBy (Pnt3f( Eps,  Eps,  Eps));
    volume.extendBy (Pnt3f(-Eps, -Eps, -Eps));

//     volume.extendBy (Pnt3f(-0.01f, -0.01f, -0.01f));
//     volume.extendBy (Pnt3f( 0.01f,  0.01f,  0.01f));
}

void SkeletonJoint::dump(      UInt32    ,
                         const BitVector ) const
{
    SLOG << "Dump SkeletonJoint NI" << std::endl;
}

OSG_END_NAMESPACE
<|endoftext|>


<commit_before>/*=========================================================================

  Program:   Insight Segmentation & Registration Toolkit
  Module:    itkSplineKernelTransformTest.cxx
  Language:  C++
  Date:      $Date$
  Version:   $Revision$

  Copyright (c) 2002 Insight Consortium. All rights reserved.
  See ITKCopyright.txt or http://www.itk.org/HTML/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notices for more information.

=========================================================================*/
#if defined(_MSC_VER)
#pragma warning ( disable : 4786 )
#endif

/**
 * This tests the elastic body spline and thin plate spline
 * transform classes by warping a unit cube into a cube with side length 3.
 * It performs the test for 2D, 3D, and 4D to ensure that the
 * class works in N dimensions
 */
#include "itkElasticBodySplineKernelTransform.h"
#include "itkThinPlateSplineKernelTransform.h"

using namespace itk;

int itkSplineKernelTransformTest(int argc, char** argv)
{

  const double epsilon = 1e-12;

  // 2-D case
  int i, j;

  typedef ElasticBodySplineKernelTransform<double, 2>   EBSTransform2DType;
  typedef ThinPlateSplineKernelTransform<double, 2>     TPSTransform2DType;

  typedef EBSTransform2DType::InputPointType PointType2D;
  typedef EBSTransform2DType::PointsIterator Points2DIteratorType;

  PointType2D sourcePoint2D;
  PointType2D targetPoint2D;
  PointType2D mappedPoint2D;

  EBSTransform2DType::Pointer ebs2D = EBSTransform2DType::New();
  TPSTransform2DType::Pointer tps2D = TPSTransform2DType::New();

  // Reserve memory for the number of points
  ebs2D->GetTargetLandmarks()->GetPoints()->Reserve( 4 );
  tps2D->GetTargetLandmarks()->GetPoints()->Reserve( 4 );

  ebs2D->GetSourceLandmarks()->GetPoints()->Reserve( 4 );
  tps2D->GetSourceLandmarks()->GetPoints()->Reserve( 4 );

  // Create landmark sets
  Points2DIteratorType ebs2Ds = ebs2D->GetSourceLandmarks()->GetPoints()->Begin();
  Points2DIteratorType ebs2Dt = ebs2D->GetTargetLandmarks()->GetPoints()->Begin();
  Points2DIteratorType tps2Ds = tps2D->GetSourceLandmarks()->GetPoints()->Begin();
  Points2DIteratorType tps2Dt = tps2D->GetTargetLandmarks()->GetPoints()->Begin();

  Points2DIteratorType ebs2DsEnd  = ebs2D->GetSourceLandmarks()->GetPoints()->End();
  Points2DIteratorType tps2DsEnd  = tps2D->GetSourceLandmarks()->GetPoints()->End();

  for (i = 0; i < 2; i++)
    {
    for (j = 0; j < 2; j++)
      {
      sourcePoint2D[0] = j;
      sourcePoint2D[1] = i;
      ebs2Ds.Value() = sourcePoint2D;
      tps2Ds.Value() = sourcePoint2D;
      targetPoint2D[0] = 3*j;
      targetPoint2D[1] = 3*i;
      ebs2Dt.Value() = targetPoint2D;
      tps2Dt.Value() = targetPoint2D;
      ebs2Ds++;
      ebs2Dt++;
      tps2Ds++;
      tps2Dt++;
      }
    }

  std::cout << "EBS 2D Test:" << std::endl;
  ebs2D->SetAlpha(0.25);
  ebs2D->ComputeWMatrix();
  ebs2Ds = ebs2D->GetSourceLandmarks()->GetPoints()->Begin();
  ebs2Dt = ebs2D->GetTargetLandmarks()->GetPoints()->Begin();
  ebs2DsEnd  = ebs2D->GetSourceLandmarks()->GetPoints()->End();
  while( ebs2Ds != ebs2DsEnd )
  {
    sourcePoint2D = ebs2Ds.Value();
    targetPoint2D = ebs2Dt.Value();
    mappedPoint2D = ebs2D->TransformPoint(sourcePoint2D);
    std::cout << sourcePoint2D << " : " << targetPoint2D;
    std::cout << " warps to: " << mappedPoint2D << std::endl;
    if( mappedPoint2D.EuclideanDistanceTo( targetPoint2D ) > epsilon )
    {
      return EXIT_FAILURE;
    }
    ebs2Ds++;
    ebs2Dt++;
  }
  std::cout << std::endl;

  std::cout << "TPS 2D Test:" << std::endl;
  tps2D->ComputeWMatrix();
  tps2Ds = tps2D->GetSourceLandmarks()->GetPoints()->Begin();
  tps2Dt = tps2D->GetTargetLandmarks()->GetPoints()->Begin();
  tps2DsEnd  = tps2D->GetSourceLandmarks()->GetPoints()->End();
  while( tps2Ds != tps2DsEnd )
  {
    sourcePoint2D = tps2Ds.Value();
    targetPoint2D = tps2Dt.Value();
    mappedPoint2D = tps2D->TransformPoint(sourcePoint2D);
    std::cout << sourcePoint2D << " : " << targetPoint2D;
    std::cout << " warps to: " << mappedPoint2D << std::endl;
    if( mappedPoint2D.EuclideanDistanceTo( targetPoint2D ) > epsilon )
    {
      return EXIT_FAILURE;
    }
    tps2Ds++;
    tps2Dt++;
  }
  std::cout << std::endl;

   // 3-D case
  int k;
  typedef ElasticBodySplineKernelTransform<double, 3> EBSTransform3DType;
  typedef ThinPlateSplineKernelTransform<double, 3>   TPSTransform3DType;

  typedef EBSTransform3DType::InputPointType PointType3D;
  typedef EBSTransform3DType::PointsIterator Points3DIteratorType;

  PointType3D sourcePoint3D;
  PointType3D targetPoint3D;
  PointType3D mappedPoint3D;

  EBSTransform3DType::Pointer ebs3D = EBSTransform3DType::New();
  TPSTransform3DType::Pointer tps3D = TPSTransform3DType::New();

  // Reserve memory for the number of points
  ebs3D->GetTargetLandmarks()->GetPoints()->Reserve( 8 );
  tps3D->GetTargetLandmarks()->GetPoints()->Reserve( 8 );
  ebs3D->GetSourceLandmarks()->GetPoints()->Reserve( 8 );
  tps3D->GetSourceLandmarks()->GetPoints()->Reserve( 8 );

  // Create landmark sets
  Points3DIteratorType ebs3Ds = ebs3D->GetSourceLandmarks()->GetPoints()->Begin();
  Points3DIteratorType ebs3Dt = ebs3D->GetTargetLandmarks()->GetPoints()->Begin();
  Points3DIteratorType tps3Ds = tps3D->GetSourceLandmarks()->GetPoints()->Begin();
  Points3DIteratorType tps3Dt = tps3D->GetTargetLandmarks()->GetPoints()->Begin();

  Points3DIteratorType ebs3DsEnd  = ebs3D->GetSourceLandmarks()->GetPoints()->End();
  Points3DIteratorType tps3DsEnd  = tps3D->GetSourceLandmarks()->GetPoints()->End();

  for (i = 0; i < 2; i++)
    {
    for (j = 0; j < 2; j++)
      {
      for (k = 0; k < 2; k++)
        {
        sourcePoint3D[0] = k;
        sourcePoint3D[1] = j;
        sourcePoint3D[2] = i;
        ebs3Ds.Value() = sourcePoint3D;
        tps3Ds.Value() = sourcePoint3D;
        targetPoint3D[0] = 3*k;
        targetPoint3D[1] = 3*j;
        targetPoint3D[2] = 3*i;
        ebs3Dt.Value() = targetPoint3D;
        tps3Dt.Value() = targetPoint3D;
        ebs3Ds++;
        ebs3Dt++;
        tps3Ds++;
        tps3Dt++;
        }
      }
    }

  std::cout << "EBS 3D Test:" << std::endl;
  ebs3D->SetAlpha(0.25);
  ebs3D->ComputeWMatrix();

  ebs3Ds     = ebs3D->GetSourceLandmarks()->GetPoints()->Begin();
  ebs3Dt     = ebs3D->GetTargetLandmarks()->GetPoints()->Begin();
  ebs3DsEnd  = ebs3D->GetSourceLandmarks()->GetPoints()->End();

  while( ebs3Ds != ebs3DsEnd )
  {
    sourcePoint3D = ebs3Ds.Value();
    targetPoint3D = ebs3Dt.Value();
    mappedPoint3D = ebs3D->TransformPoint(sourcePoint3D);
    std::cout << sourcePoint3D << " : " << targetPoint3D;
    std::cout << " warps to: " << mappedPoint3D << std::endl;
    if( mappedPoint3D.EuclideanDistanceTo( targetPoint3D ) > epsilon )
    {
      return EXIT_FAILURE;
    }
    ebs3Ds++;
    ebs3Dt++;
  }
  std::cout << std::endl;

  std::cout << "TPS 3D Test:" << std::endl;

  tps3D->ComputeWMatrix();

  tps3Ds = tps3D->GetSourceLandmarks()->GetPoints()->Begin();
  tps3Dt = tps3D->GetTargetLandmarks()->GetPoints()->Begin();
  tps3DsEnd  = tps3D->GetSourceLandmarks()->GetPoints()->End();

  while( tps3Ds != tps3DsEnd )
  {
    sourcePoint3D = tps3Ds.Value();
    targetPoint3D = tps3Dt.Value();
    mappedPoint3D = tps3D->TransformPoint(sourcePoint3D);
    std::cout << sourcePoint3D << " : " << targetPoint3D;
    std::cout << " warps to: " << mappedPoint3D << std::endl;
    if( mappedPoint3D.EuclideanDistanceTo( targetPoint3D ) > epsilon )
    {
      return EXIT_FAILURE;
    }
    tps3Ds++;
    tps3Dt++;
  }
  std::cout << std::endl;

  // 4-D case
  int l;
  typedef ElasticBodySplineKernelTransform<double, 4> EBSTransform4DType;
  typedef ThinPlateSplineKernelTransform<double, 4>   TPSTransform4DType;

  typedef EBSTransform4DType::InputPointType PointType4D;
  typedef EBSTransform4DType::PointsIterator Points4DIteratorType;

  PointType4D sourcePoint4D;
  PointType4D targetPoint4D;
  PointType4D mappedPoint4D;

  EBSTransform4DType::Pointer ebs4D = EBSTransform4DType::New();
  TPSTransform4DType::Pointer tps4D = TPSTransform4DType::New();

  // Reserve memory for the number of points
  ebs4D->GetTargetLandmarks()->GetPoints()->Reserve( 16 );
  tps4D->GetTargetLandmarks()->GetPoints()->Reserve( 16 );

  ebs4D->GetSourceLandmarks()->GetPoints()->Reserve( 16 );
  tps4D->GetSourceLandmarks()->GetPoints()->Reserve( 16 );

  // Create landmark sets
  Points4DIteratorType ebs4Ds = ebs4D->GetSourceLandmarks()->GetPoints()->Begin();
  Points4DIteratorType ebs4Dt = ebs4D->GetTargetLandmarks()->GetPoints()->Begin();
  Points4DIteratorType tps4Ds = tps4D->GetSourceLandmarks()->GetPoints()->Begin();
  Points4DIteratorType tps4Dt = tps4D->GetTargetLandmarks()->GetPoints()->Begin();

  Points4DIteratorType ebs4DsEnd  = ebs4D->GetSourceLandmarks()->GetPoints()->End();
  Points4DIteratorType tps4DsEnd  = tps4D->GetSourceLandmarks()->GetPoints()->End();

  for (i = 0; i < 2; i++)
    {
    for (j = 0; j < 2; j++)
      {
      for (k = 0; k < 2; k++)
        {
        for (l = 0; l < 2; l++)
          {
          sourcePoint4D[0] = l;
          sourcePoint4D[1] = k;
          sourcePoint4D[2] = j;
          sourcePoint4D[3] = i;
          ebs4Ds.Value() = sourcePoint4D;
          tps4Ds.Value() = sourcePoint4D;
          targetPoint4D[0] = 3*l;
          targetPoint4D[1] = 3*k;
          targetPoint4D[2] = 3*j;
          targetPoint4D[3] = 3*i;
          ebs4Dt.Value() = targetPoint4D;
          tps4Dt.Value() = targetPoint4D;
          ebs4Ds++;
          ebs4Dt++;
          tps4Ds++;
          tps4Dt++;
          }
        }
      }
    }
  std::cout << "EBS 4D Test:" << std::endl;
  ebs4D->SetAlpha(0.25);
  ebs4D->ComputeWMatrix();

  ebs4Ds = ebs4D->GetSourceLandmarks()->GetPoints()->Begin();
  ebs4Dt = ebs4D->GetTargetLandmarks()->GetPoints()->Begin();
  ebs4DsEnd  = ebs4D->GetSourceLandmarks()->GetPoints()->End();

  while( ebs4Ds != ebs4DsEnd )
  {
    sourcePoint4D = ebs4Ds.Value();
    targetPoint4D = ebs4Dt.Value();
    mappedPoint4D = ebs4D->TransformPoint(sourcePoint4D);
    std::cout << sourcePoint4D << " : " << targetPoint4D;
    std::cout << " warps to: " << mappedPoint4D << std::endl;
    if( mappedPoint4D.EuclideanDistanceTo( targetPoint4D ) > epsilon )
    {
      return EXIT_FAILURE;
    }
    ebs4Ds++;
    ebs4Dt++;
  }
  std::cout << std::endl;

  std::cout << "TPS 4D Test:" << std::endl;
  tps4D->ComputeWMatrix();

  tps4Ds = tps4D->GetSourceLandmarks()->GetPoints()->Begin();
  tps4Dt = tps4D->GetTargetLandmarks()->GetPoints()->Begin();
  tps4DsEnd  = tps4D->GetSourceLandmarks()->GetPoints()->End();
  while( tps4Ds != tps4DsEnd )
  {
    sourcePoint4D = tps4Ds.Value();
    targetPoint4D = tps4Dt.Value();
    mappedPoint4D = tps4D->TransformPoint(sourcePoint4D);
    std::cout << sourcePoint4D << " : " << targetPoint4D;
    std::cout << " warps to: " << mappedPoint4D << std::endl;
    if( mappedPoint4D.EuclideanDistanceTo( targetPoint4D ) > epsilon )
    {
      return EXIT_FAILURE;
    }
    tps4Ds++;
    tps4Dt++;
  }
  std::cout << std::endl;

  std::cout << ebs2D << std::endl;

  return EXIT_SUCCESS;

}

<commit_msg>ENH: Small test for Set/Get Parameters() added.<commit_after>/*=========================================================================

  Program:   Insight Segmentation & Registration Toolkit
  Module:    itkSplineKernelTransformTest.cxx
  Language:  C++
  Date:      $Date$
  Version:   $Revision$

  Copyright (c) 2002 Insight Consortium. All rights reserved.
  See ITKCopyright.txt or http://www.itk.org/HTML/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notices for more information.

=========================================================================*/
#if defined(_MSC_VER)
#pragma warning ( disable : 4786 )
#endif

/**
 * This tests the elastic body spline and thin plate spline
 * transform classes by warping a unit cube into a cube with side length 3.
 * It performs the test for 2D, 3D, and 4D to ensure that the
 * class works in N dimensions
 */
#include "itkElasticBodySplineKernelTransform.h"
#include "itkThinPlateSplineKernelTransform.h"

using namespace itk;

int itkSplineKernelTransformTest(int argc, char** argv)
{

  const double epsilon = 1e-12;

  // 2-D case
  int i, j;

  typedef ElasticBodySplineKernelTransform<double, 2>   EBSTransform2DType;
  typedef ThinPlateSplineKernelTransform<double, 2>     TPSTransform2DType;

  typedef EBSTransform2DType::InputPointType PointType2D;
  typedef EBSTransform2DType::PointsIterator Points2DIteratorType;

  PointType2D sourcePoint2D;
  PointType2D targetPoint2D;
  PointType2D mappedPoint2D;

  EBSTransform2DType::Pointer ebs2D = EBSTransform2DType::New();
  TPSTransform2DType::Pointer tps2D = TPSTransform2DType::New();

  // Reserve memory for the number of points
  ebs2D->GetTargetLandmarks()->GetPoints()->Reserve( 4 );
  tps2D->GetTargetLandmarks()->GetPoints()->Reserve( 4 );

  ebs2D->GetSourceLandmarks()->GetPoints()->Reserve( 4 );
  tps2D->GetSourceLandmarks()->GetPoints()->Reserve( 4 );

  // Create landmark sets
  Points2DIteratorType ebs2Ds = ebs2D->GetSourceLandmarks()->GetPoints()->Begin();
  Points2DIteratorType ebs2Dt = ebs2D->GetTargetLandmarks()->GetPoints()->Begin();
  Points2DIteratorType tps2Ds = tps2D->GetSourceLandmarks()->GetPoints()->Begin();
  Points2DIteratorType tps2Dt = tps2D->GetTargetLandmarks()->GetPoints()->Begin();

  Points2DIteratorType ebs2DsEnd  = ebs2D->GetSourceLandmarks()->GetPoints()->End();
  Points2DIteratorType tps2DsEnd  = tps2D->GetSourceLandmarks()->GetPoints()->End();

  for (i = 0; i < 2; i++)
    {
    for (j = 0; j < 2; j++)
      {
      sourcePoint2D[0] = j;
      sourcePoint2D[1] = i;
      ebs2Ds.Value() = sourcePoint2D;
      tps2Ds.Value() = sourcePoint2D;
      targetPoint2D[0] = 3*j;
      targetPoint2D[1] = 3*i;
      ebs2Dt.Value() = targetPoint2D;
      tps2Dt.Value() = targetPoint2D;
      ebs2Ds++;
      ebs2Dt++;
      tps2Ds++;
      tps2Dt++;
      }
    }

  std::cout << "EBS 2D Test:" << std::endl;
  ebs2D->SetAlpha(0.25);
  ebs2D->ComputeWMatrix();
  ebs2Ds = ebs2D->GetSourceLandmarks()->GetPoints()->Begin();
  ebs2Dt = ebs2D->GetTargetLandmarks()->GetPoints()->Begin();
  ebs2DsEnd  = ebs2D->GetSourceLandmarks()->GetPoints()->End();
  while( ebs2Ds != ebs2DsEnd )
  {
    sourcePoint2D = ebs2Ds.Value();
    targetPoint2D = ebs2Dt.Value();
    mappedPoint2D = ebs2D->TransformPoint(sourcePoint2D);
    std::cout << sourcePoint2D << " : " << targetPoint2D;
    std::cout << " warps to: " << mappedPoint2D << std::endl;
    if( mappedPoint2D.EuclideanDistanceTo( targetPoint2D ) > epsilon )
    {
      return EXIT_FAILURE;
    }
    ebs2Ds++;
    ebs2Dt++;
  }
  std::cout << std::endl;

  std::cout << "TPS 2D Test:" << std::endl;
  tps2D->ComputeWMatrix();
  tps2Ds = tps2D->GetSourceLandmarks()->GetPoints()->Begin();
  tps2Dt = tps2D->GetTargetLandmarks()->GetPoints()->Begin();
  tps2DsEnd  = tps2D->GetSourceLandmarks()->GetPoints()->End();
  while( tps2Ds != tps2DsEnd )
  {
    sourcePoint2D = tps2Ds.Value();
    targetPoint2D = tps2Dt.Value();
    mappedPoint2D = tps2D->TransformPoint(sourcePoint2D);
    std::cout << sourcePoint2D << " : " << targetPoint2D;
    std::cout << " warps to: " << mappedPoint2D << std::endl;
    if( mappedPoint2D.EuclideanDistanceTo( targetPoint2D ) > epsilon )
    {
      return EXIT_FAILURE;
    }
    tps2Ds++;
    tps2Dt++;
  }
  std::cout << std::endl;

   // 3-D case
  int k;
  typedef ElasticBodySplineKernelTransform<double, 3> EBSTransform3DType;
  typedef ThinPlateSplineKernelTransform<double, 3>   TPSTransform3DType;

  typedef EBSTransform3DType::InputPointType PointType3D;
  typedef EBSTransform3DType::PointsIterator Points3DIteratorType;

  PointType3D sourcePoint3D;
  PointType3D targetPoint3D;
  PointType3D mappedPoint3D;

  EBSTransform3DType::Pointer ebs3D = EBSTransform3DType::New();
  TPSTransform3DType::Pointer tps3D = TPSTransform3DType::New();

  // Reserve memory for the number of points
  ebs3D->GetTargetLandmarks()->GetPoints()->Reserve( 8 );
  tps3D->GetTargetLandmarks()->GetPoints()->Reserve( 8 );
  ebs3D->GetSourceLandmarks()->GetPoints()->Reserve( 8 );
  tps3D->GetSourceLandmarks()->GetPoints()->Reserve( 8 );

  // Create landmark sets
  Points3DIteratorType ebs3Ds = ebs3D->GetSourceLandmarks()->GetPoints()->Begin();
  Points3DIteratorType ebs3Dt = ebs3D->GetTargetLandmarks()->GetPoints()->Begin();
  Points3DIteratorType tps3Ds = tps3D->GetSourceLandmarks()->GetPoints()->Begin();
  Points3DIteratorType tps3Dt = tps3D->GetTargetLandmarks()->GetPoints()->Begin();

  Points3DIteratorType ebs3DsEnd  = ebs3D->GetSourceLandmarks()->GetPoints()->End();
  Points3DIteratorType tps3DsEnd  = tps3D->GetSourceLandmarks()->GetPoints()->End();

  for (i = 0; i < 2; i++)
    {
    for (j = 0; j < 2; j++)
      {
      for (k = 0; k < 2; k++)
        {
        sourcePoint3D[0] = k;
        sourcePoint3D[1] = j;
        sourcePoint3D[2] = i;
        ebs3Ds.Value() = sourcePoint3D;
        tps3Ds.Value() = sourcePoint3D;
        targetPoint3D[0] = 3*k;
        targetPoint3D[1] = 3*j;
        targetPoint3D[2] = 3*i;
        ebs3Dt.Value() = targetPoint3D;
        tps3Dt.Value() = targetPoint3D;
        ebs3Ds++;
        ebs3Dt++;
        tps3Ds++;
        tps3Dt++;
        }
      }
    }

  std::cout << "EBS 3D Test:" << std::endl;
  ebs3D->SetAlpha(0.25);
  ebs3D->ComputeWMatrix();

  ebs3Ds     = ebs3D->GetSourceLandmarks()->GetPoints()->Begin();
  ebs3Dt     = ebs3D->GetTargetLandmarks()->GetPoints()->Begin();
  ebs3DsEnd  = ebs3D->GetSourceLandmarks()->GetPoints()->End();

  while( ebs3Ds != ebs3DsEnd )
  {
    sourcePoint3D = ebs3Ds.Value();
    targetPoint3D = ebs3Dt.Value();
    mappedPoint3D = ebs3D->TransformPoint(sourcePoint3D);
    std::cout << sourcePoint3D << " : " << targetPoint3D;
    std::cout << " warps to: " << mappedPoint3D << std::endl;
    if( mappedPoint3D.EuclideanDistanceTo( targetPoint3D ) > epsilon )
    {
      return EXIT_FAILURE;
    }
    ebs3Ds++;
    ebs3Dt++;
  }
  std::cout << std::endl;

  std::cout << "TPS 3D Test:" << std::endl;

  tps3D->ComputeWMatrix();

  tps3Ds = tps3D->GetSourceLandmarks()->GetPoints()->Begin();
  tps3Dt = tps3D->GetTargetLandmarks()->GetPoints()->Begin();
  tps3DsEnd  = tps3D->GetSourceLandmarks()->GetPoints()->End();

  while( tps3Ds != tps3DsEnd )
  {
    sourcePoint3D = tps3Ds.Value();
    targetPoint3D = tps3Dt.Value();
    mappedPoint3D = tps3D->TransformPoint(sourcePoint3D);
    std::cout << sourcePoint3D << " : " << targetPoint3D;
    std::cout << " warps to: " << mappedPoint3D << std::endl;
    if( mappedPoint3D.EuclideanDistanceTo( targetPoint3D ) > epsilon )
    {
      return EXIT_FAILURE;
    }
    tps3Ds++;
    tps3Dt++;
  }
  std::cout << std::endl;

  std::cout << "Get/Set Parameters test " << std::endl;
  TPSTransform3DType::ParametersType parameters1 = tps3D->GetParameters();
  tps3D->SetParameters( parameters1 );
  TPSTransform3DType::ParametersType parameters2 = tps3D->GetParameters();
  const unsigned int numberOfParameters = parameters1.Size();
  const double tolerance = 1e-7;
  for(unsigned int pr = 0; pr < numberOfParameters; pr++)
    {
    if( vnl_math_abs( parameters1[pr] - parameters2[pr] ) > tolerance )
      {
      std::cout << "Parameters were not correctly recovered " << std::endl;
      return EXIT_FAILURE;
      }
    }
  std::cout << "Get/Set Parameters Passed" << std::endl << std::endl;

  // 4-D case
  int l;
  typedef ElasticBodySplineKernelTransform<double, 4> EBSTransform4DType;
  typedef ThinPlateSplineKernelTransform<double, 4>   TPSTransform4DType;

  typedef EBSTransform4DType::InputPointType PointType4D;
  typedef EBSTransform4DType::PointsIterator Points4DIteratorType;

  PointType4D sourcePoint4D;
  PointType4D targetPoint4D;
  PointType4D mappedPoint4D;

  EBSTransform4DType::Pointer ebs4D = EBSTransform4DType::New();
  TPSTransform4DType::Pointer tps4D = TPSTransform4DType::New();

  // Reserve memory for the number of points
  ebs4D->GetTargetLandmarks()->GetPoints()->Reserve( 16 );
  tps4D->GetTargetLandmarks()->GetPoints()->Reserve( 16 );

  ebs4D->GetSourceLandmarks()->GetPoints()->Reserve( 16 );
  tps4D->GetSourceLandmarks()->GetPoints()->Reserve( 16 );

  // Create landmark sets
  Points4DIteratorType ebs4Ds = ebs4D->GetSourceLandmarks()->GetPoints()->Begin();
  Points4DIteratorType ebs4Dt = ebs4D->GetTargetLandmarks()->GetPoints()->Begin();
  Points4DIteratorType tps4Ds = tps4D->GetSourceLandmarks()->GetPoints()->Begin();
  Points4DIteratorType tps4Dt = tps4D->GetTargetLandmarks()->GetPoints()->Begin();

  Points4DIteratorType ebs4DsEnd  = ebs4D->GetSourceLandmarks()->GetPoints()->End();
  Points4DIteratorType tps4DsEnd  = tps4D->GetSourceLandmarks()->GetPoints()->End();

  for (i = 0; i < 2; i++)
    {
    for (j = 0; j < 2; j++)
      {
      for (k = 0; k < 2; k++)
        {
        for (l = 0; l < 2; l++)
          {
          sourcePoint4D[0] = l;
          sourcePoint4D[1] = k;
          sourcePoint4D[2] = j;
          sourcePoint4D[3] = i;
          ebs4Ds.Value() = sourcePoint4D;
          tps4Ds.Value() = sourcePoint4D;
          targetPoint4D[0] = 3*l;
          targetPoint4D[1] = 3*k;
          targetPoint4D[2] = 3*j;
          targetPoint4D[3] = 3*i;
          ebs4Dt.Value() = targetPoint4D;
          tps4Dt.Value() = targetPoint4D;
          ebs4Ds++;
          ebs4Dt++;
          tps4Ds++;
          tps4Dt++;
          }
        }
      }
    }
  std::cout << "EBS 4D Test:" << std::endl;
  ebs4D->SetAlpha(0.25);
  ebs4D->ComputeWMatrix();

  ebs4Ds = ebs4D->GetSourceLandmarks()->GetPoints()->Begin();
  ebs4Dt = ebs4D->GetTargetLandmarks()->GetPoints()->Begin();
  ebs4DsEnd  = ebs4D->GetSourceLandmarks()->GetPoints()->End();

  while( ebs4Ds != ebs4DsEnd )
  {
    sourcePoint4D = ebs4Ds.Value();
    targetPoint4D = ebs4Dt.Value();
    mappedPoint4D = ebs4D->TransformPoint(sourcePoint4D);
    std::cout << sourcePoint4D << " : " << targetPoint4D;
    std::cout << " warps to: " << mappedPoint4D << std::endl;
    if( mappedPoint4D.EuclideanDistanceTo( targetPoint4D ) > epsilon )
    {
      return EXIT_FAILURE;
    }
    ebs4Ds++;
    ebs4Dt++;
  }
  std::cout << std::endl;

  std::cout << "TPS 4D Test:" << std::endl;
  tps4D->ComputeWMatrix();

  tps4Ds = tps4D->GetSourceLandmarks()->GetPoints()->Begin();
  tps4Dt = tps4D->GetTargetLandmarks()->GetPoints()->Begin();
  tps4DsEnd  = tps4D->GetSourceLandmarks()->GetPoints()->End();
  while( tps4Ds != tps4DsEnd )
  {
    sourcePoint4D = tps4Ds.Value();
    targetPoint4D = tps4Dt.Value();
    mappedPoint4D = tps4D->TransformPoint(sourcePoint4D);
    std::cout << sourcePoint4D << " : " << targetPoint4D;
    std::cout << " warps to: " << mappedPoint4D << std::endl;
    if( mappedPoint4D.EuclideanDistanceTo( targetPoint4D ) > epsilon )
    {
      return EXIT_FAILURE;
    }
    tps4Ds++;
    tps4Dt++;
  }
  std::cout << std::endl;

  std::cout << ebs2D << std::endl;

  return EXIT_SUCCESS;

}

<|endoftext|>


<commit_before>#include "core/wordwrap.h"

namespace euphoria::core
{
    std::vector<std::string>
    WordWrap
    (
        const std::string& str,
        std::function<bool (const std::string&)> measure
    )
    {
        using I = std::string::size_type;
        if(measure(str)) { return {str}; }

        auto ret = std::vector<std::string>{};

        int watchdog = 20;

        I start = 0;
        I last = 0;

        auto get_string = [&str, &start](I end)
        {
            if(end == std::string::npos)
            {
                return str.substr(start);
            }
            else
            {
                return str.substr(start, end - (start+1));
            }
        };

        while(true)
        {
            watchdog -= 1;
            if(watchdog < 0) { ret.emplace_back("watchdog"); return ret; }

            if(last == std::string::npos)
            {
                return ret;
            }
            const auto first = str.find(' ', last);
            const auto s = get_string(first);
            const auto fit = measure(s);

            if(fit)
            {
                // fit.. continue
                last = (first != std::string::npos) ? first+1 : first;
            }
            if(!fit || last == std::string::npos)
            {
                // new doesnt fit, use last
                const auto p = last == start ? first : last;
                const auto pp = last == start && p != std::string::npos ? p+1 : p;
                ret.emplace_back(get_string(pp));
                if(p == std::string::npos)
                {
                    return ret;
                }
                if(last == start)
                {
                    start = p+1;
                    last = p;
                }
                else start = p;
            }
        }

        return ret;
    }
}

<commit_msg>forgot to remove the watchdog<commit_after>#include "core/wordwrap.h"

namespace euphoria::core
{
    std::vector<std::string>
    WordWrap
    (
        const std::string& str,
        std::function<bool (const std::string&)> measure
    )
    {
        using I = std::string::size_type;
        if(measure(str)) { return {str}; }

        auto ret = std::vector<std::string>{};

        I start = 0;
        I last = 0;

        auto get_string = [&str, &start](I end)
        {
            if(end == std::string::npos)
            {
                return str.substr(start);
            }
            else
            {
                return str.substr(start, end - (start+1));
            }
        };

        while(true)
        {
            if(last == std::string::npos)
            {
                return ret;
            }
            const auto first = str.find(' ', last);
            const auto s = get_string(first);
            const auto fit = measure(s);

            if(fit)
            {
                // fit.. continue
                last = (first != std::string::npos) ? first+1 : first;
            }
            if(!fit || last == std::string::npos)
            {
                // new doesnt fit, use last
                const auto p = last == start ? first : last;
                const auto pp = last == start && p != std::string::npos ? p+1 : p;
                ret.emplace_back(get_string(pp));
                if(p == std::string::npos)
                {
                    return ret;
                }
                if(last == start)
                {
                    start = p+1;
                    last = p;
                }
                else start = p;
            }
        }

        return ret;
    }
}

<|endoftext|>


<commit_before>/*
 * opencog/attentionbank/AttentionBank.cc
 *
 * Copyright (C) 2013 Linas Vepstas <linasvepstas@gmail.com>
 * All Rights Reserved
 *
 * Written by Joel Pitt
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU Affero General Public License v3 as
 * published by the Free Software Foundation and including the exceptions
 * at http://opencog.org/wiki/Licenses
 *
 * This program 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 General Public License for more details.
 *
 * You should have received a copy of the GNU Affero General Public License
 * along with this program; if not, write to:
 * Free Software Foundation, Inc.,
 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
 */

#include <boost/bind.hpp>
#include <opencog/util/Config.h>

#include <opencog/atoms/base/Handle.h>
#include <opencog/atomspace/AtomSpace.h>
#include "AttentionBank.h"

using namespace opencog;

AttentionBank::AttentionBank(AtomSpace* asp) :
    _importanceIndex(*this)
{
    startingFundsSTI = fundsSTI = config().get_int("STARTING_STI_FUNDS", 100000);
    startingFundsLTI = fundsLTI = config().get_int("STARTING_LTI_FUNDS", 100000);
    stiFundsBuffer = config().get_int("STI_FUNDS_BUFFER", 10000);
    ltiFundsBuffer = config().get_int("LTI_FUNDS_BUFFER", 10000);
    targetLTI = config().get_int("TARGET_LTI_FUNDS", 10000);
    targetSTI = config().get_int("TARGET_STI_FUNDS", 10000);
    STIAtomWage = config().get_int("ECAN_STARTING_ATOM_STI_WAGE", 10);
    LTIAtomWage = config().get_int("ECAN_STARTING_ATOM_LTI_WAGE", 10);
    minAFSize = config().get_int("ECAN_MIN_AF_SIZE", 100);

    _removeAtomConnection =
        asp->removeAtomSignal(
            boost::bind(&AttentionBank::remove_atom_from_index, this, _1));
}

AttentionBank::~AttentionBank()
{
    _removeAtomConnection.disconnect();
}

void AttentionBank::remove_atom_from_index(const AtomPtr& atom)
{
    std::unique_lock<std::mutex> lck(_idx_mtx);

    auto pr = _atom_index.find(Handle(atom));
    if (pr == _atom_index.end()) return;
    AttentionValuePtr av = pr->second;
    _atom_index.erase(pr->first);
    lck.unlock();

    int bin = ImportanceIndex::importanceBin(av->getSTI());

    _importanceIndex.removeAtom(atom.operator->(), bin);
}

void AttentionBank::change_av(const Handle& h, AttentionValuePtr newav)
{
    std::unique_lock<std::mutex> lck(_idx_mtx);

    AttentionValuePtr oldav = AttentionValue::DEFAULT_AV();
    auto pr = _atom_index.find(h);
    if (pr != _atom_index.end()) oldav = pr->second;

    _atom_index[h] = newav;
    lck.unlock();

    // XXX FIXME the code below could be made more efficient,
    // by avoiding the calls to atom->getAttentionValue() in
    // the ImportanceIndex code.

    // Get old and new bins.
    int oldBin = ImportanceIndex::importanceBin(oldav->getSTI());
    int newBin = ImportanceIndex::importanceBin(newav->getSTI());

    // If the atom importance has changed its bin,
    // update the importance index.
    if (oldBin != newBin) updateImportanceIndex(h, oldBin, newBin);

    AVChanged(h, oldav, newav);
}

void AttentionBank::set_sti(const Handle& h, AttentionValue::sti_t stiValue)
{
    AttentionValuePtr old_av = AttentionValue::DEFAULT_AV();
    AttentionValuePtr new_av = createAV(
        stiValue, old_av->getLTI(), old_av->getVLTI());

    change_av(h, new_av);
}

void AttentionBank::set_lti(const Handle& h, AttentionValue::lti_t ltiValue)
{
    std::unique_lock<std::mutex> lck(_idx_mtx);

    AttentionValuePtr old_av = AttentionValue::DEFAULT_AV();
    auto pr = _atom_index.find(h);
    if (pr != _atom_index.end()) old_av = pr->second;

    AttentionValuePtr new_av = createAV(
        old_av->getSTI(), ltiValue, old_av->getVLTI());
    _atom_index[h] = new_av;
    lck.unlock();

    AVChanged(h, old_av, new_av);
}

void AttentionBank::change_vlti(const Handle& h, int unit)
{
    std::unique_lock<std::mutex> lck(_idx_mtx);

    AttentionValuePtr old_av = AttentionValue::DEFAULT_AV();
    auto pr = _atom_index.find(h);
    if (pr != _atom_index.end()) old_av = pr->second;

    AttentionValuePtr new_av = createAV(
        old_av->getSTI(),
        old_av->getLTI(),
        old_av->getVLTI() + unit);

    _atom_index[h] = new_av;
    lck.unlock();

    AVChanged(h, old_av, new_av);
}

AttentionValuePtr AttentionBank::get_av(const Handle& h)
{
    std::lock_guard<std::mutex> lck(_idx_mtx);
    auto pr = _atom_index.find(h);
    if (pr == _atom_index.end()) return AttentionValue::DEFAULT_AV();

    return pr->second;
}

void AttentionBank::AVChanged(const Handle& h,
                              const AttentionValuePtr& old_av,
                              const AttentionValuePtr& new_av)
{
    AttentionValue::sti_t oldSti = old_av->getSTI();
    AttentionValue::sti_t newSti = new_av->getSTI();

    // Add the old attention values to the AttentionBank funds and
    // subtract the new attention values from the AttentionBank funds
    updateSTIFunds(oldSti - newSti);
    updateLTIFunds(old_av->getLTI() - new_av->getLTI());

    // Update MinMax STI values
    AttentionValue::sti_t minSTISeen = 0;
    UnorderedHandleSet minbin = _importanceIndex.getMinBinContents();
    auto minit = std::min_element(minbin.begin(), minbin.end(),
            [&](const Handle& h1, const Handle& h2) {
                return get_sti(h1) < get_sti(h2);
            });
    if (minit != minbin.end()) minSTISeen = get_sti(*minit);

    AttentionValue::sti_t maxSTISeen = 0;
    UnorderedHandleSet maxbin = _importanceIndex.getMinBinContents();
    auto maxit = std::max_element(maxbin.begin(), maxbin.end(),
            [&](const Handle& h1, const Handle& h2) {
                return get_sti(h1) < get_sti(h2);
            });
    if (maxit != maxbin.end()) maxSTISeen = get_sti(*maxit);

    if (minSTISeen > maxSTISeen) {
        minSTISeen = maxSTISeen;
    }

    updateMinSTI(minSTISeen);
    updateMaxSTI(maxSTISeen);

    logger().fine("AVChanged: fundsSTI = %d, old_av: %d, new_av: %d",
                   fundsSTI.load(), oldSti, newSti);

    // Notify any interested parties that the AV changed.
    _AVChangedSignal(h, old_av, new_av);

    //update AF
    updateAttentionalFocus(h, old_av, new_av);
}

void AttentionBank::stimulate(const Handle& h, double stimulus)
{
    // XXX This is not protected or made atomic in any way ...
    // If two different threads stimulate the same atom at the same
    // time, then the calculations will be bad. Does it matter?
    AttentionValue::sti_t sti   = get_sti(h);
    AttentionValue::lti_t lti   = get_lti(h);
    AttentionValue::vlti_t vlti = get_vlti(h);

    AttentionValue::sti_t stiWage = calculateSTIWage() * stimulus;
    AttentionValue::lti_t ltiWage = calculateLTIWage() * stimulus;
    AttentionValuePtr new_av = createAV(sti + stiWage, lti + ltiWage, vlti);
    change_av(h, new_av);
}

void AttentionBank::updateMaxSTI(AttentionValue::sti_t m)
{
    std::lock_guard<std::mutex> lock(_lock_maxSTI);
    _maxSTI.update(m);
}

void AttentionBank::updateMinSTI(AttentionValue::sti_t m)
{
    std::lock_guard<std::mutex> lock(_lock_minSTI);
    _minSTI.update(m);
}

AttentionValue::sti_t AttentionBank::getMaxSTI(bool average) const
{
    std::lock_guard<std::mutex> lock(_lock_maxSTI);
    if (average) {
        return (AttentionValue::sti_t) _maxSTI.recent;
    } else {
        return _maxSTI.val;
    }
}

AttentionValue::sti_t AttentionBank::getMinSTI(bool average) const
{
    std::lock_guard<std::mutex> lock(_lock_minSTI);
    if (average) {
        return (AttentionValue::sti_t) _minSTI.recent;
    } else {
        return _minSTI.val;
    }
}

AttentionValue::sti_t AttentionBank::calculateSTIWage()
{
    long funds = getSTIFunds();
    double diff  = funds - targetSTI;
    double ndiff = diff / stiFundsBuffer;
    ndiff = std::min(ndiff, 1.0);
    ndiff = std::max(ndiff, -1.0);

    return STIAtomWage + (STIAtomWage * ndiff);
}

AttentionValue::lti_t AttentionBank::calculateLTIWage()
{
    long funds = getLTIFunds();
    double diff  = funds - targetLTI;
    double ndiff = diff / ltiFundsBuffer;
    ndiff = std::min(ndiff, 1.0);
    ndiff = std::max(ndiff, -1.0);

    return LTIAtomWage + (LTIAtomWage * ndiff);
}

double AttentionBank::getNormalisedSTI(AttentionValuePtr av,
                                   bool average, bool clip) const
{
    double val;
    // get normalizer (maxSTI - attention boundary)
    AttentionValue::sti_t s = av->getSTI();
    if (s > get_af_max_sti()) {
        int normaliser = (int) getMaxSTI(average) - get_af_max_sti();
        if (normaliser == 0) return 0.0;
        val = (s - get_af_max_sti()) / (double) normaliser;
    } else {
        int normaliser = -((int) getMinSTI(average) + get_af_max_sti());
        if (normaliser == 0) return 0.0;
        val = (s + get_af_max_sti()) / (double) normaliser;
    }

    if (clip) return std::max(-1.0, std::min(val, 1.0));
    return val;
}

double AttentionBank::getNormalisedSTI(AttentionValuePtr av) const
{
    AttentionValue::sti_t s = av->getSTI();
    auto normaliser =
            s > get_af_max_sti() ? getMaxSTI() : getMinSTI();

    return (s / normaliser);
}

double AttentionBank::getNormalisedZeroToOneSTI(AttentionValuePtr av,
                                    bool average, bool clip) const
{
    AttentionValue::sti_t s = av->getSTI();
    int normaliser = getMaxSTI(average) - getMinSTI(average);
    if (normaliser == 0) return 0.0;

    double val = (s - getMinSTI(average)) / (double) normaliser;
    if (clip) return std::max(0.0, std::min(val, 1.0));
    return val;
}

/** Unique singleton instance (for now) */
// This implementation is pretty hokey, and is a stop-gap until some
// sort of moreelegant way of managing the attentionbank is found.
// One of the issues is that access via this function can be CPU-wasteful.
AttentionBank& opencog::attentionbank(AtomSpace* asp)
{
    static AttentionBank* _instance = nullptr;
    static AtomSpace* _as = nullptr;

    // Protect setting and getting against thread races.
    // This is probably not needed.
    static std::map<AtomSpace*, AttentionBank*> banksy;
    static std::mutex art;
    std::unique_lock<std::mutex> graffiti(art);

    if (_as != asp and _instance) {
        delete _instance;
        _instance = nullptr;
        _as = nullptr;
    }
    if (asp and nullptr == _instance) {
        _instance = new AttentionBank(asp);
        _as = asp;
    }
    return *_instance;
}

Handle AttentionBank::getRandomAtom()
{
   return _importanceIndex.getRandomAtom();
}

bool AttentionBank::atom_is_in_AF(const Handle& h)
{
    auto it = std::find_if(attentionalFocus.begin(), attentionalFocus.end(),
            [h](std::pair<Handle, AttentionValuePtr> p)
            { if(p.first == h )
                      return true;
               return false;
            });
    return  (it != attentionalFocus.end());
}

/**
 *  Updates list of top K important atoms based on STI value.
 */
void AttentionBank::updateAttentionalFocus(const Handle& h,
                    const AttentionValuePtr& old_av,
                    const AttentionValuePtr& new_av)
{
    std::lock_guard<std::mutex> lock(AFMutex);
    AttentionValue::sti_t sti = new_av->getSTI();
    auto least = attentionalFocus.begin();
    bool insertable = false;
    auto it = std::find_if(attentionalFocus.begin(), attentionalFocus.end(),
            [h](std::pair<Handle, AttentionValuePtr> p)
            { if(p.first == h ) return true; return false;});

    // Update the STI value if atoms was already in AF
    if(it != attentionalFocus.end()){
        attentionalFocus.erase(it);
        attentionalFocus.insert(std::make_pair(h, new_av));
        return;
    } else if(attentionalFocus.size() < minAFSize){
        insertable = true;
    } else if( sti > (least->second)->getSTI()){
        attentionalFocus.erase(least);
        AFCHSigl& afch = RemoveAFSignal();
        afch(least->first, least->second, get_av(least->first));
        insertable = true;
    }

    if(insertable){
        attentionalFocus.insert(std::make_pair(h, new_av));
        AFCHSigl& afch = AddAFSignal();
        afch(h, old_av, new_av);
    }
}

<commit_msg>Bug fix and more documentation<commit_after>/*
 * opencog/attentionbank/AttentionBank.cc
 *
 * Copyright (C) 2013 Linas Vepstas <linasvepstas@gmail.com>
 * All Rights Reserved
 *
 * Written by Joel Pitt
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU Affero General Public License v3 as
 * published by the Free Software Foundation and including the exceptions
 * at http://opencog.org/wiki/Licenses
 *
 * This program 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 General Public License for more details.
 *
 * You should have received a copy of the GNU Affero General Public License
 * along with this program; if not, write to:
 * Free Software Foundation, Inc.,
 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
 */

#include <boost/bind.hpp>
#include <opencog/util/Config.h>

#include <opencog/atoms/base/Handle.h>
#include <opencog/atomspace/AtomSpace.h>
#include "AttentionBank.h"

using namespace opencog;

AttentionBank::AttentionBank(AtomSpace* asp) :
    _importanceIndex(*this)
{
    startingFundsSTI = fundsSTI = config().get_int("STARTING_STI_FUNDS", 100000);
    startingFundsLTI = fundsLTI = config().get_int("STARTING_LTI_FUNDS", 100000);
    stiFundsBuffer = config().get_int("STI_FUNDS_BUFFER", 10000);
    ltiFundsBuffer = config().get_int("LTI_FUNDS_BUFFER", 10000);
    targetLTI = config().get_int("TARGET_LTI_FUNDS", 10000);
    targetSTI = config().get_int("TARGET_STI_FUNDS", 10000);
    STIAtomWage = config().get_int("ECAN_STARTING_ATOM_STI_WAGE", 10);
    LTIAtomWage = config().get_int("ECAN_STARTING_ATOM_LTI_WAGE", 10);
    minAFSize = config().get_int("ECAN_MIN_AF_SIZE", 100);

    _removeAtomConnection =
        asp->removeAtomSignal(
            boost::bind(&AttentionBank::remove_atom_from_index, this, _1));
}

AttentionBank::~AttentionBank()
{
    _removeAtomConnection.disconnect();
}

void AttentionBank::remove_atom_from_index(const AtomPtr& atom)
{
    std::unique_lock<std::mutex> lck(_idx_mtx);

    auto pr = _atom_index.find(Handle(atom));
    if (pr == _atom_index.end()) return;
    AttentionValuePtr av = pr->second;
    _atom_index.erase(pr->first);
    lck.unlock();

    int bin = ImportanceIndex::importanceBin(av->getSTI());

    _importanceIndex.removeAtom(atom.operator->(), bin);
}

void AttentionBank::change_av(const Handle& h, AttentionValuePtr newav)
{
    std::unique_lock<std::mutex> lck(_idx_mtx);

    AttentionValuePtr oldav = AttentionValue::DEFAULT_AV();
    auto pr = _atom_index.find(h);
    if (pr != _atom_index.end()) oldav = pr->second;

    _atom_index[h] = newav;
    lck.unlock();

    // XXX FIXME the code below could be made more efficient,
    // by avoiding the calls to atom->getAttentionValue() in
    // the ImportanceIndex code.

    // Get old and new bins.
    int oldBin = ImportanceIndex::importanceBin(oldav->getSTI());
    int newBin = ImportanceIndex::importanceBin(newav->getSTI());

    // If the atom importance has changed its bin,
    // update the importance index.
    if (oldBin != newBin) updateImportanceIndex(h, oldBin, newBin);

    AVChanged(h, oldav, newav);
}

void AttentionBank::set_sti(const Handle& h, AttentionValue::sti_t stiValue)
{
    AttentionValuePtr old_av = AttentionValue::DEFAULT_AV();
    AttentionValuePtr new_av = createAV(
        stiValue, old_av->getLTI(), old_av->getVLTI());

    change_av(h, new_av);
}

void AttentionBank::set_lti(const Handle& h, AttentionValue::lti_t ltiValue)
{
    std::unique_lock<std::mutex> lck(_idx_mtx);

    AttentionValuePtr old_av = AttentionValue::DEFAULT_AV();
    auto pr = _atom_index.find(h);
    if (pr != _atom_index.end()) old_av = pr->second;

    AttentionValuePtr new_av = createAV(
        old_av->getSTI(), ltiValue, old_av->getVLTI());
    _atom_index[h] = new_av;
    lck.unlock();

    AVChanged(h, old_av, new_av);
}

void AttentionBank::change_vlti(const Handle& h, int unit)
{
    std::unique_lock<std::mutex> lck(_idx_mtx);

    AttentionValuePtr old_av = AttentionValue::DEFAULT_AV();
    auto pr = _atom_index.find(h);
    if (pr != _atom_index.end()) old_av = pr->second;

    AttentionValuePtr new_av = createAV(
        old_av->getSTI(),
        old_av->getLTI(),
        old_av->getVLTI() + unit);

    _atom_index[h] = new_av;
    lck.unlock();

    AVChanged(h, old_av, new_av);
}

AttentionValuePtr AttentionBank::get_av(const Handle& h)
{
    std::lock_guard<std::mutex> lck(_idx_mtx);
    auto pr = _atom_index.find(h);
    if (pr == _atom_index.end()) return AttentionValue::DEFAULT_AV();

    return pr->second;
}

void AttentionBank::AVChanged(const Handle& h,
                              const AttentionValuePtr& old_av,
                              const AttentionValuePtr& new_av)
{
    AttentionValue::sti_t oldSti = old_av->getSTI();
    AttentionValue::sti_t newSti = new_av->getSTI();

    // Add the old attention values to the AttentionBank funds and
    // subtract the new attention values from the AttentionBank funds
    updateSTIFunds(oldSti - newSti);
    updateLTIFunds(old_av->getLTI() - new_av->getLTI());

    // Update MinMax STI values
    AttentionValue::sti_t minSTISeen = 0;
    UnorderedHandleSet minbin = _importanceIndex.getMinBinContents();
    auto minit = std::min_element(minbin.begin(), minbin.end(),
            [&](const Handle& h1, const Handle& h2) {
                return get_sti(h1) < get_sti(h2);
            });
    if (minit != minbin.end()) minSTISeen = get_sti(*minit);

    AttentionValue::sti_t maxSTISeen = 0;
    UnorderedHandleSet maxbin = _importanceIndex.getMinBinContents();
    auto maxit = std::max_element(maxbin.begin(), maxbin.end(),
            [&](const Handle& h1, const Handle& h2) {
                return get_sti(h1) < get_sti(h2);
            });
    if (maxit != maxbin.end()) maxSTISeen = get_sti(*maxit);

    if (minSTISeen > maxSTISeen) {
        minSTISeen = maxSTISeen;
    }

    updateMinSTI(minSTISeen);
    updateMaxSTI(maxSTISeen);

    logger().fine("AVChanged: fundsSTI = %d, old_av: %d, new_av: %d",
                   fundsSTI.load(), oldSti, newSti);

    // Notify any interested parties that the AV changed.
    _AVChangedSignal(h, old_av, new_av);

    //update AF
    updateAttentionalFocus(h, old_av, new_av);
}

void AttentionBank::stimulate(const Handle& h, double stimulus)
{
    // XXX This is not protected or made atomic in any way ...
    // If two different threads stimulate the same atom at the same
    // time, then the calculations will be bad. Does it matter?
    AttentionValue::sti_t sti   = get_sti(h);
    AttentionValue::lti_t lti   = get_lti(h);
    AttentionValue::vlti_t vlti = get_vlti(h);

    AttentionValue::sti_t stiWage = calculateSTIWage() * stimulus;
    AttentionValue::lti_t ltiWage = calculateLTIWage() * stimulus;
    AttentionValuePtr new_av = createAV(sti + stiWage, lti + ltiWage, vlti);
    change_av(h, new_av);
}

void AttentionBank::updateMaxSTI(AttentionValue::sti_t m)
{
    std::lock_guard<std::mutex> lock(_lock_maxSTI);
    _maxSTI.update(m);
}

void AttentionBank::updateMinSTI(AttentionValue::sti_t m)
{
    std::lock_guard<std::mutex> lock(_lock_minSTI);
    _minSTI.update(m);
}

AttentionValue::sti_t AttentionBank::getMaxSTI(bool average) const
{
    std::lock_guard<std::mutex> lock(_lock_maxSTI);
    if (average) {
        return (AttentionValue::sti_t) _maxSTI.recent;
    } else {
        return _maxSTI.val;
    }
}

AttentionValue::sti_t AttentionBank::getMinSTI(bool average) const
{
    std::lock_guard<std::mutex> lock(_lock_minSTI);
    if (average) {
        return (AttentionValue::sti_t) _minSTI.recent;
    } else {
        return _minSTI.val;
    }
}

AttentionValue::sti_t AttentionBank::calculateSTIWage()
{
    long funds = getSTIFunds();
    double diff  = funds - targetSTI;
    double ndiff = diff / stiFundsBuffer;
    ndiff = std::min(ndiff, 1.0);
    ndiff = std::max(ndiff, -1.0);

    return STIAtomWage + (STIAtomWage * ndiff);
}

AttentionValue::lti_t AttentionBank::calculateLTIWage()
{
    long funds = getLTIFunds();
    double diff  = funds - targetLTI;
    double ndiff = diff / ltiFundsBuffer;
    ndiff = std::min(ndiff, 1.0);
    ndiff = std::max(ndiff, -1.0);

    return LTIAtomWage + (LTIAtomWage * ndiff);
}

double AttentionBank::getNormalisedSTI(AttentionValuePtr av,
                                   bool average, bool clip) const
{
    double val;
    // get normalizer (maxSTI - attention boundary)
    AttentionValue::sti_t s = av->getSTI();
    if (s > get_af_max_sti()) {
        int normaliser = (int) getMaxSTI(average) - get_af_max_sti();
        if (normaliser == 0) return 0.0;
        val = (s - get_af_max_sti()) / (double) normaliser;
    } else {
        int normaliser = -((int) getMinSTI(average) + get_af_max_sti());
        if (normaliser == 0) return 0.0;
        val = (s + get_af_max_sti()) / (double) normaliser;
    }

    if (clip) return std::max(-1.0, std::min(val, 1.0));
    return val;
}

double AttentionBank::getNormalisedSTI(AttentionValuePtr av) const
{
    AttentionValue::sti_t s = av->getSTI();
    auto normaliser =
            s > get_af_max_sti() ? getMaxSTI() : getMinSTI();

    return (s / normaliser);
}

double AttentionBank::getNormalisedZeroToOneSTI(AttentionValuePtr av,
                                    bool average, bool clip) const
{
    AttentionValue::sti_t s = av->getSTI();
    int normaliser = getMaxSTI(average) - getMinSTI(average);
    if (normaliser == 0) return 0.0;

    double val = (s - getMinSTI(average)) / (double) normaliser;
    if (clip) return std::max(0.0, std::min(val, 1.0));
    return val;
}

/** Unique singleton instance (for now) */
// This implementation is pretty hokey, and is a stop-gap until some
// sort of moreelegant way of managing the attentionbank is found.
// One of the issues is that access via this function can be CPU-wasteful.
AttentionBank& opencog::attentionbank(AtomSpace* asp)
{
    static AttentionBank* _instance = nullptr;
    static AtomSpace* _as = nullptr;

    // Protect setting and getting against thread races.
    // This is probably not needed.
    static std::map<AtomSpace*, AttentionBank*> banksy;
    static std::mutex art;
    std::unique_lock<std::mutex> graffiti(art);

    if (_as != asp and _instance) {
        delete _instance;
        _instance = nullptr;
        _as = nullptr;
    }
    if (asp and nullptr == _instance) {
        _instance = new AttentionBank(asp);
        _as = asp;
    }
    return *_instance;
}

Handle AttentionBank::getRandomAtom()
{
   return _importanceIndex.getRandomAtom();
}

bool AttentionBank::atom_is_in_AF(const Handle& h)
{
    auto it = std::find_if(attentionalFocus.begin(), attentionalFocus.end(),
            [h](std::pair<Handle, AttentionValuePtr> p)
            { if(p.first == h )
                      return true;
               return false;
            });
    return  (it != attentionalFocus.end());
}

/**
 *  Updates list of top K important atoms based on STI value.
 */
void AttentionBank::updateAttentionalFocus(const Handle& h,
                    const AttentionValuePtr& old_av,
                    const AttentionValuePtr& new_av)
{
    std::lock_guard<std::mutex> lock(AFMutex);
    AttentionValue::sti_t sti = new_av->getSTI();
    auto least = attentionalFocus.begin(); // Atom to be removed from the AF
    bool insertable = false;
    auto it = std::find_if(attentionalFocus.begin(), attentionalFocus.end(),
            [h](std::pair<Handle, AttentionValuePtr> p)
            { if(p.first == h ) return true; return false;});

    // Update the STI value if atoms was already in AF
    if(it != attentionalFocus.end()){
        attentionalFocus.erase(it);
        attentionalFocus.insert(std::make_pair(h, new_av));
        return;
    // Simply insert the new Atom if AF is not full yet.
    } else if(attentionalFocus.size() < minAFSize){
        insertable = true;
    // Remove the least sti valued atom in the AF and repace
    // it with the new atom holding higher STI value.
    } else if( sti > (least->second)->getSTI()){
        Handle hrm = least->first;
        AttentionValuePtr hrm_new_av = get_av(hrm);
        // Value recorded when this atom entered into AF
        AttentionValuePtr hrm_old_av = least->second;

        attentionalFocus.erase(least);
        AFCHSigl& afch = RemoveAFSignal();
        afch(hrm, hrm_old_av, hrm_new_av);
        insertable = true;
    }

    // Insert the new atom in to AF and emit the AddAFSignal.
    if(insertable){
        attentionalFocus.insert(std::make_pair(h, new_av));
        AFCHSigl& afch = AddAFSignal();
        afch(h, old_av, new_av);
    }
}

<|endoftext|>


<commit_before>

#include "Arduino.h"
#include <Shiftduino.h>
#include "Square.h"

char RED = 'r';
char GREEN = 'g';
char NONE = 'n';

int _analogPin;
int _position; // from 1 to 9

//Squares 1 to 3 are in shiftreg 1
//Squares 4 to 6 are in shiftreg 2
//Squares 7 to 9 are in shiftreg 3
//In each shiftreg there is one unused square position
//int _index;

//Each square has two leds, green and red
//The index is the position of the red led and index-1 the position of the green led
int greenIndex;
int redIndex;

int colorValue;
char color; //g = green, r = red, n = none

//Calibration for the CNY70
int _maxGreen;
int _minGreen;

int _maxRed;
int _minRed;

Shiftduino * sd;

//************* PUBLIC ************************
Square::Square(uint8_t analogPin, uint8_t pos, Shiftduino * reg){
	sd = reg;

	_analogPin = analogPin;
	_position = pos;

	//_index = calculateIndex();

	greenIndex = calculateGreenIndex(pos);
	redIndex = greenIndex+1;

	//Reset the leds
	clear();

	_maxGreen = 0;
	_minGreen = 0;
	_maxRed = 0;
	_minRed = 0;
	colorValue = 0;
	color = 'n';
}

void Square::calibrateCNY70(int maxGreen, int minGreen, int maxRed, int minRed){
	_maxGreen = maxGreen;
	_minGreen = minGreen;
	_maxRed = maxRed;
	_minRed = minRed;
}

void Square::setGreen(){
        clear();
	writeValue(greenIndex, 1);
        writeValue(redIndex, 0);
}

void Square::setRed(){
	writeValue(greenIndex, 0);
	writeValue(redIndex, 1);
}

void Square::clear(){
	writeValue(greenIndex, 0);
	writeValue(redIndex, 0);
}

char Square::checkColor(){
	colorValue = analogRead(_analogPin);
	if (colorValue >= _minGreen && colorValue <= _maxGreen){
		color = GREEN;
	} else if (colorValue >= _minRed && colorValue <= _maxRed){
		color = RED;
	} else {
		color = NONE;
	}

	return color;
}

int Square::getPosition(){
        return _position;
}

int Square::getGreenIndex(){
        return greenIndex;
}

int Square::getRedIndex(){
        return redIndex;
}

//************* PRIVATE ************************

int Square::calculateIndex(){
	int index = 0;
	if (_position < 4){
		index = _position;
	}else if (_position > 3 && _position < 7){
		index = _position + 1;
	}else if (_position > 6){
		index = _position + 2;
	}

	return index;
}

void Square::writeValue(int index, int value){
	sd->setPin(index, value);
}

/*
This one is a bit tricky. The green and red pin index of each square depend on the square postion ([1..9]),
but they are not consecutive, becouse there is a "gap" in each shift register.

Register 1: Squares 1, 2 and 3
Register 2: Squares 4, 5 and 6
Register 3: Sqyares 7, 8 and 9

So the pins for the led will be (green and red):
Square 1: 0 and 1
Square 2: 2 and 3
Square 3: 4 and 5
-gap-
Square 4: 8 and 9
Square 5: 10 and 11
Square 6: 12 and 13
-gap
Square 7: 16 and 17
Square 8: 18 and 19
Square 9: 20 and 21

*/
int Square::calculateGreenIndex(int pos){
  if (pos == 1){
    return 0;
  }
  else if (pos == 2){
    return 2;
  }
  else if (pos == 3){
    return 3;
  }
  else if (pos == 4){
    return 4;
  }
  else if (pos == 5){
    return 10;
  }
  else if (pos == 6){
    return 12;
  }
  else if (pos == 7){
    return 16;
  }
  else if (pos == 8){
    return 18;
  }
  else if (pos == 9){
    return 20;
  }

}

<commit_msg>fixed bug calculating pins, now it should be working (for real this time)<commit_after>

#include "Arduino.h"
#include <Shiftduino.h>
#include "Square.h"

char RED = 'r';
char GREEN = 'g';
char NONE = 'n';

int _analogPin;
int _position; // from 1 to 9

//Squares 1 to 3 are in shiftreg 1
//Squares 4 to 6 are in shiftreg 2
//Squares 7 to 9 are in shiftreg 3
//In each shiftreg there is one unused square position
//int _index;

//Each square has two leds, green and red
//The index is the position of the red led and index-1 the position of the green led
int greenIndex;
int redIndex;

int colorValue;
char color; //g = green, r = red, n = none

//Calibration for the CNY70
int _maxGreen;
int _minGreen;

int _maxRed;
int _minRed;

Shiftduino * sd;

//************* PUBLIC ************************
Square::Square(uint8_t analogPin, uint8_t pos, Shiftduino * reg){
	sd = reg;

	_analogPin = analogPin;
	_position = pos;

	//_index = calculateIndex();

	greenIndex = calculateGreenIndex(pos);
	redIndex = greenIndex+1;

	//Reset the leds
	clear();

	_maxGreen = 0;
	_minGreen = 0;
	_maxRed = 0;
	_minRed = 0;
	colorValue = 0;
	color = 'n';
}

void Square::calibrateCNY70(int maxGreen, int minGreen, int maxRed, int minRed){
	_maxGreen = maxGreen;
	_minGreen = minGreen;
	_maxRed = maxRed;
	_minRed = minRed;
}

void Square::setGreen(){
        clear();
	writeValue(greenIndex, 1);
        writeValue(redIndex, 0);
}

void Square::setRed(){
	writeValue(greenIndex, 0);
	writeValue(redIndex, 1);
}

void Square::clear(){
	writeValue(greenIndex, 0);
	writeValue(redIndex, 0);
}

char Square::checkColor(){
	colorValue = analogRead(_analogPin);
	if (colorValue >= _minGreen && colorValue <= _maxGreen){
		color = GREEN;
	} else if (colorValue >= _minRed && colorValue <= _maxRed){
		color = RED;
	} else {
		color = NONE;
	}

	return color;
}

int Square::getPosition(){
        return _position;
}

int Square::getGreenIndex(){
        return greenIndex;
}

int Square::getRedIndex(){
        return redIndex;
}

//************* PRIVATE ************************

int Square::calculateIndex(){
	int index = 0;
	if (_position < 4){
		index = _position;
	}else if (_position > 3 && _position < 7){
		index = _position + 1;
	}else if (_position > 6){
		index = _position + 2;
	}

	return index;
}

void Square::writeValue(int index, int value){
	sd->setPin(index, value);
}

/*
This one is a bit tricky. The green and red pin index of each square depend on the square postion ([1..9]),
but they are not consecutive, becouse there is a "gap" in each shift register.

Register 1: Squares 1, 2 and 3
Register 2: Squares 4, 5 and 6
Register 3: Sqyares 7, 8 and 9

So the pins for the led will be (green and red):
Square 1: 0 and 1
Square 2: 2 and 3
Square 3: 4 and 5
-gap-
Square 4: 8 and 9
Square 5: 10 and 11
Square 6: 12 and 13
-gap
Square 7: 16 and 17
Square 8: 18 and 19
Square 9: 20 and 21

*/
int Square::calculateGreenIndex(int pos){
  if (pos == 1){
    return 0;
  }
  else if (pos == 2){
    return 2;
  }
  else if (pos == 3){
    return 4;
  }
  else if (pos == 4){
    return 8;
  }
  else if (pos == 5){
    return 10;
  }
  else if (pos == 6){
    return 12;
  }
  else if (pos == 7){
    return 16;
  }
  else if (pos == 8){
    return 18;
  }
  else if (pos == 9){
    return 20;
  }

}

<|endoftext|>


<commit_before>#ifndef STAN_MATH_PRIM_MAT_FUN_COV_EXP_QUAD_HPP
#define STAN_MATH_PRIM_MAT_FUN_COV_EXP_QUAD_HPP

#include <stan/math/prim/scal/meta/return_type.hpp>
#include <stan/math/prim/scal/err/check_size_match.hpp>
#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <stan/math/prim/mat/fun/squared_distance.hpp>
#include <stan/math/prim/scal/err/check_not_nan.hpp>
#include <stan/math/prim/scal/err/check_positive.hpp>
#include <stan/math/prim/scal/fun/square.hpp>
#include <stan/math/prim/scal/fun/exp.hpp>
#include <vector>
#include <cmath>

namespace stan {
  namespace math {

    /**
     * Returns a squared exponential kernel.
     *
     * @tparam T_x type of std::vector of elements
     * @tparam T_sigma type of sigma
     * @tparam T_l type of length scale
     *
     * @param x std::vector of elements that can be used in square distance.
     *    This function assumes each element of x is the same size.
     * @param sigma standard deviation
     * @param l length scale
     * @return squared distance
     * @throw std::domain_error if sigma <= 0, l <= 0, or
     *   x is nan or infinite
     */
    template <typename T_x, typename T_sigma, typename T_l>
    inline
    typename Eigen::Matrix<typename stan::return_type<T_x, T_sigma, T_l>::type,
                           Eigen::Dynamic, Eigen::Dynamic>
    cov_exp_quad(const std::vector<T_x>& x, const T_sigma& sigma,
                 const T_l& l) {
      using std::exp;
      check_positive("cov_exp_quad", "marginal variance", sigma);
      check_positive("cov_exp_quad", "length-scale", l);
      for (size_t n = 0; n < x.size(); ++n)
	check_not_nan("cov_exp_quad", "x", x[n]);

      Eigen::Matrix<typename stan::return_type<T_x, T_sigma, T_l>::type,
		    Eigen::Dynamic, Eigen::Dynamic>
	cov(x.size(), x.size());

      int x_size = x.size();
      if (x_size == 0)
	return cov;

      T_sigma sigma_sq = square(sigma);
      T_l neg_half_inv_l_sq = -0.5 / square(l);

      for (int j = 0; j < (x_size - 1); ++j) {
	cov(j, j) = sigma_sq;
	for (int i = j + 1; i < x_size; ++i) {
	  cov(i, j)
	    = sigma_sq * exp(squared_distance(x[i], x[j]) * neg_half_inv_l_sq);
	  cov(j, i) = cov(i, j);
	}
      }
      cov(x_size - 1, x_size - 1) = sigma_sq;
      return cov;
    }

    /**
     * Returns a squared exponential kernel.
     *
     * @tparam T_x type of std::vector of elements
     * @tparam T_sigma type of sigma
     * @tparam T_l type of length scale
     *
     * @param x std::vector of elements that can be used in square distance.
     *    This function assumes each element of x is the same size.
     * @param sigma standard deviation
     * @param l length scale
     * @return squared distance
     * @throw std::domain_error if sigma <= 0, l <= 0, or
     *   x is nan or infinite
     */
    template<typename T_x, typename T_sigma, typename T_l>
    inline typename
    Eigen::Matrix<typename stan::return_type<T_x, T_sigma, T_l>::type,
                  Eigen::Dynamic, Eigen::Dynamic>
    cov_exp_quad(const std::vector<T_x>& x,
                 const T_sigma& sigma,
		 const std::vector<T_l>& l) {
      using std::exp;
      check_positive("cov_exp_quad", "marginal variance", sigma);
      // check_positive("cov_exp_quad", "length-scale", l); // old code
      for (size_t n = 0; n < x.size(); ++n) {
        check_not_nan("cov_exp_quad", "x", x[n]);
      }
      for(size_t n = 0; n < l.size(); ++n) {
	check_positive("cov_exp_quad", "l", l[n]);
	check_not_nan("cov_exp_quad", "l", l[n]);
      }

      Eigen::Matrix<typename stan::return_type<T_x, T_sigma, T_l>::type,
                    Eigen::Dynamic, Eigen::Dynamic>
        cov(x.size(), x.size());

      int x_size = x.size();
      int l_size = l.size();
      if (x_size == 0)
        return cov;

      T_sigma sigma_sq = square(sigma);
      // T_l neg_half_inv_l_sq = - 0.5 / square(l); // old code
      double temp_exp = 0;

      for (int j = 0; j < (x_size - 1); ++j) {
        cov(j, j) = sigma_sq;
        for (int i = j + 1; i < x_size; ++i) {
          // cov(i, j) = sigma_sq * exp(squared_distance(x[i], x[j]) // old code
          //                            * neg_half_inv_l_sq); // old code

	  temp_exp = 0;
	  for(int k = 0; k < l_size; ++k) { // sum through exponential length scale for ARD priors
	    temp_exp += squared_distance(x[i], x[j]) / square(l[k]);
	  }
	  cov(i, j) = sigma_sq * exp(- 0.5 * temp_exp);
          cov(j, i) = cov(i, j);
        }
      }
      cov(x_size - 1, x_size - 1) = sigma_sq;
      return cov;
    }

    /**
     * Returns a squared exponential kernel.
     *
     * @tparam T_x1 type of first std::vector of elements
     * @tparam T_x2 type of second std::vector of elements
     * @tparam T_sigma type of sigma
     * @tparam T_l type of length scale
     *
     * @param x1 std::vector of elements that can be used in square distance
     * @param x2 std::vector of elements that can be used in square distance
     * @param sigma standard deviation
     * @param l length scale
     * @return squared distance
     * @throw std::domain_error if sigma <= 0, l <= 0, or
     *   x is nan or infinite
     */
    template<typename T_x1, typename T_x2, typename T_sigma, typename T_l>
    inline typename
    Eigen::Matrix<typename stan::return_type<T_x1, T_x2, T_sigma, T_l>::type,
                  Eigen::Dynamic, Eigen::Dynamic>
    cov_exp_quad(const std::vector<T_x1>& x1,
                 const std::vector<T_x2>& x2,
                 const T_sigma& sigma,
                 const T_l& l) {
		 // const Eigen::Matrix(T_l, Eigen::Dynamic, 1> &l) {
      using std::exp;
      check_positive("cov_exp_quad", "marginal variance", sigma);
      check_positive("cov_exp_quad", "length-scale", l);
      for (size_t n = 0; n < x1.size(); ++n)
        check_not_nan("cov_exp_quad", "x1", x1[n]);
      for (size_t n = 0; n < x2.size(); ++n)
        check_not_nan("cov_exp_quad", "x2", x2[n]);
      // for(size_t n = 0; n < l.size(); ++n) {
      // 	check_positive("cov_exp_quad", "l", l[n]);
      // 	check_positive("cov_exp_quad", "l", l[n]);
      // }

      Eigen::Matrix<typename stan::return_type<T_x1, T_x2, T_sigma, T_l>::type,
                    Eigen::Dynamic, Eigen::Dynamic>
        cov(x1.size(), x2.size());
      if (x1.size() == 0 || x2.size() == 0)
        return cov;

      T_sigma sigma_sq = square(sigma);
      T_l neg_half_inv_l_sq = - 0.5 / square(l);

      for (size_t i = 0; i < x1.size(); ++i) {
        for (size_t j = 0; j < x2.size(); ++j) {
          cov(i, j) = sigma_sq * exp(squared_distance(x1[i], x2[j])
                                     * neg_half_inv_l_sq);
        }
      }
      return cov;
    }

  }
}
#endif
<commit_msg>Delete cov_exp_quad.hpp<commit_after><|endoftext|>