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

  Program:   Visualization Toolkit
  Module:    vtkActor.cxx

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/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 notice for more information.

=========================================================================*/
#include "vtkActor.h"

#include "vtkAssemblyPaths.h"
#include "vtkDataArray.h"
#include "vtkGraphicsFactory.h"
#include "vtkImageData.h"
#include "vtkLinearTransform.h"
#include "vtkMapper.h"
#include "vtkMath.h"
#include "vtkPointData.h"
#include "vtkPropCollection.h"
#include "vtkProperty.h"
#include "vtkRenderWindow.h"
#include "vtkRenderer.h"
#include "vtkTexture.h"
#include "vtkTransform.h"

#include <math.h>

vtkCxxRevisionMacro(vtkActor, "1.130");

vtkCxxSetObjectMacro(vtkActor,Texture,vtkTexture);
vtkCxxSetObjectMacro(vtkActor,Mapper,vtkMapper);
vtkCxxSetObjectMacro(vtkActor,BackfaceProperty,vtkProperty);
vtkCxxSetObjectMacro(vtkActor,Property,vtkProperty);

//----------------------------------------------------------------------------
// Needed when we don't use the vtkStandardNewMacro.
vtkInstantiatorNewMacro(vtkActor);

// Creates an actor with the following defaults: origin(0,0,0)
// position=(0,0,0) scale=(1,1,1) visibility=1 pickable=1 dragable=1
// orientation=(0,0,0). No user defined matrix and no texture map.
vtkActor::vtkActor()
{
  this->Mapper = NULL;
  this->Property = NULL;
  this->BackfaceProperty = NULL;
  this->Texture = NULL;

  // The mapper bounds are cache to know when the bounds must be recomputed
  // from the mapper bounds.
  vtkMath::UninitializeBounds(this->MapperBounds);
}

//----------------------------------------------------------------------------
vtkActor::~vtkActor()
{
  if ( this->Property != NULL)
    {
    this->Property->UnRegister(this);
    this->Property = NULL;
    }

  if ( this->BackfaceProperty != NULL)
    {
    this->BackfaceProperty->UnRegister(this);
    this->BackfaceProperty = NULL;
    }

  if (this->Mapper)
    {
    this->Mapper->UnRegister(this);
    this->Mapper = NULL;
    }
  this->SetTexture(NULL);
}

//----------------------------------------------------------------------------
// Shallow copy of an actor.
void vtkActor::ShallowCopy(vtkProp *prop)
{
  vtkActor *a = vtkActor::SafeDownCast(prop);
  if ( a != NULL )
    {
    this->SetMapper(a->GetMapper());
    this->SetProperty(a->GetProperty());
    this->SetBackfaceProperty(a->GetBackfaceProperty());
    this->SetTexture(a->GetTexture());
    }

  // Now do superclass
  this->vtkProp3D::ShallowCopy(prop);
}

//----------------------------------------------------------------------------
// return the correct type of Actor
vtkActor *vtkActor::New()
{
  // First try to create the object from the vtkGraphicsFactory
  vtkObject* ret = vtkGraphicsFactory::CreateInstance("vtkActor");
  return (vtkActor*)ret;
}

//----------------------------------------------------------------------------
void vtkActor::GetActors(vtkPropCollection *ac)
{
  ac->AddItem(this);
}

//----------------------------------------------------------------------------
// should be called from the render methods only
int vtkActor::GetIsOpaque()
{
  if (this->Property->GetOpacity() >= 1.0)
    {
    if (this->Texture && this->Texture->GetInput())
      {
      this->Texture->GetInput()->UpdateInformation();
      this->Texture->GetInput()->SetUpdateExtent(
        this->Texture->GetInput()->GetWholeExtent());
      this->Texture->GetInput()->PropagateUpdateExtent();
      this->Texture->GetInput()->TriggerAsynchronousUpdate();
      this->Texture->GetInput()->UpdateData();
      if (this->Texture->GetInput()->GetPointData()->GetScalars() == NULL)
        { // Handle gracefully. What should it return?
        return 1;
        }
      if (this->Texture->GetInput()->GetPointData()->GetScalars()
        ->GetNumberOfComponents()%2)
        {
        return 1;
        }
      }
    else
      {
      return 1;
      }
    }
  return 0;
}

//----------------------------------------------------------------------------
// This causes the actor to be rendered. It in turn will render the actor's
// property, texture map and then mapper. If a property hasn't been
// assigned, then the actor will create one automatically. Note that a
// side effect of this method is that the visualization network is updated.
int vtkActor::RenderOpaqueGeometry(vtkViewport *vp)
{
  int          renderedSomething = 0;
  vtkRenderer* ren = static_cast<vtkRenderer*>(vp);

  if ( ! this->Mapper )
    {
    return 0;
    }

  // make sure we have a property
  if (!this->Property)
    {
    // force creation of a property
    this->GetProperty();
    }

  // is this actor opaque ?
  if (this->GetIsOpaque())
    {
    this->Property->Render(this, ren);

    // render the backface property
    if (this->BackfaceProperty)
      {
      this->BackfaceProperty->BackfaceRender(this, ren);
      }

    // render the texture
    if (this->Texture)
      {
      this->Texture->Render(ren);
      }
    this->Render(ren,this->Mapper);
    this->Property->PostRender(this, ren);
    this->EstimatedRenderTime += this->Mapper->GetTimeToDraw();
    renderedSomething = 1;
    }

  return renderedSomething;
}

//----------------------------------------------------------------------------
int vtkActor::RenderTranslucentGeometry(vtkViewport *vp)
{
  int          renderedSomething = 0;
  vtkRenderer* ren = static_cast<vtkRenderer*>(vp);

  if ( ! this->Mapper )
    {
    return 0;
    }

  // make sure we have a property
  if (!this->Property)
    {
    // force creation of a property
    this->GetProperty();
    }

  // is this actor opaque ?
  if (!this->GetIsOpaque())
    {
    this->Property->Render(this, ren);

    // render the backface property
    if (this->BackfaceProperty)
      {
      this->BackfaceProperty->BackfaceRender(this, ren);
      }

    // render the texture
    if (this->Texture)
      {
      this->Texture->Render(ren);
      }
    this->Render(ren,this->Mapper);
    this->Property->PostRender(this, ren);
    this->EstimatedRenderTime += this->Mapper->GetTimeToDraw();

    renderedSomething = 1;
    }

  return renderedSomething;
}

//----------------------------------------------------------------------------
void vtkActor::ReleaseGraphicsResources(vtkWindow *win)
{
  vtkRenderWindow *renWin = (vtkRenderWindow *)win;

  // pass this information onto the mapper
  if (this->Mapper)
    {
    this->Mapper->ReleaseGraphicsResources(renWin);
    }

  // pass this information onto the texture
  if (this->Texture)
    {
    this->Texture->ReleaseGraphicsResources(renWin);
    }
}

//----------------------------------------------------------------------------
vtkProperty* vtkActor::MakeProperty()
{
  return vtkProperty::New();
}

//----------------------------------------------------------------------------
vtkProperty *vtkActor::GetProperty()
{
  if ( this->Property == NULL )
    {
    vtkProperty *p = this->MakeProperty();
    this->SetProperty(p);
    p->Delete();
    }
  return this->Property;
}

//----------------------------------------------------------------------------
// Get the bounds for this Actor as (Xmin,Xmax,Ymin,Ymax,Zmin,Zmax).
double *vtkActor::GetBounds()
{
  int i,n;
  double *bounds, bbox[24], *fptr;

  vtkDebugMacro( << "Getting Bounds" );

  // get the bounds of the Mapper if we have one
  if (!this->Mapper)
    {
    return this->Bounds;
    }

  bounds = this->Mapper->GetBounds();
  // Check for the special case when the mapper's bounds are unknown
  if (!bounds)
    {
    return bounds;
    }

  // Check for the special case when the actor is empty.
  if (!vtkMath::AreBoundsInitialized(bounds))
    {
    memcpy( this->MapperBounds, bounds, 6*sizeof(double) );
    vtkMath::UninitializeBounds(this->Bounds);
    this->BoundsMTime.Modified();
    return this->Bounds;
    }

  // Check if we have cached values for these bounds - we cache the
  // values returned by this->Mapper->GetBounds() and we store the time
  // of caching. If the values returned this time are different, or
  // the modified time of this class is newer than the cached time,
  // then we need to rebuild.
  if ( ( memcmp( this->MapperBounds, bounds, 6*sizeof(double) ) != 0 ) ||
       ( this->GetMTime() > this->BoundsMTime ) )
    {
    vtkDebugMacro( << "Recomputing bounds..." );

    memcpy( this->MapperBounds, bounds, 6*sizeof(double) );

    // fill out vertices of a bounding box
    bbox[ 0] = bounds[1]; bbox[ 1] = bounds[3]; bbox[ 2] = bounds[5];
    bbox[ 3] = bounds[1]; bbox[ 4] = bounds[2]; bbox[ 5] = bounds[5];
    bbox[ 6] = bounds[0]; bbox[ 7] = bounds[2]; bbox[ 8] = bounds[5];
    bbox[ 9] = bounds[0]; bbox[10] = bounds[3]; bbox[11] = bounds[5];
    bbox[12] = bounds[1]; bbox[13] = bounds[3]; bbox[14] = bounds[4];
    bbox[15] = bounds[1]; bbox[16] = bounds[2]; bbox[17] = bounds[4];
    bbox[18] = bounds[0]; bbox[19] = bounds[2]; bbox[20] = bounds[4];
    bbox[21] = bounds[0]; bbox[22] = bounds[3]; bbox[23] = bounds[4];

    // save the old transform
    this->Transform->Push();
    this->Transform->SetMatrix(this->GetMatrix());

    // and transform into actors coordinates
    fptr = bbox;
    for (n = 0; n < 8; n++)
      {
      this->Transform->TransformPoint(fptr,fptr);
      fptr += 3;
      }

    this->Transform->Pop();

    // now calc the new bounds
    this->Bounds[0] = this->Bounds[2] = this->Bounds[4] = VTK_DOUBLE_MAX;
    this->Bounds[1] = this->Bounds[3] = this->Bounds[5] = -VTK_DOUBLE_MAX;
    for (i = 0; i < 8; i++)
      {
      for (n = 0; n < 3; n++)
        {
        if (bbox[i*3+n] < this->Bounds[n*2])
          {
          this->Bounds[n*2] = bbox[i*3+n];
          }
        if (bbox[i*3+n] > this->Bounds[n*2+1])
          {
          this->Bounds[n*2+1] = bbox[i*3+n];
          }
        }
      }
    this->BoundsMTime.Modified();
    }

  return this->Bounds;
}

//----------------------------------------------------------------------------
unsigned long int vtkActor::GetMTime()
{
  unsigned long mTime=this->Superclass::GetMTime();
  unsigned long time;

  if ( this->Property != NULL )
    {
    time = this->Property->GetMTime();
    mTime = ( time > mTime ? time : mTime );
    }

  if ( this->BackfaceProperty != NULL )
    {
    time = this->BackfaceProperty->GetMTime();
    mTime = ( time > mTime ? time : mTime );
    }

  if ( this->Texture != NULL )
    {
    time = this->Texture->GetMTime();
    mTime = ( time > mTime ? time : mTime );
    }

  return mTime;
}

//----------------------------------------------------------------------------
unsigned long int vtkActor::GetRedrawMTime()
{
  unsigned long mTime=this->GetMTime();
  unsigned long time;

  if ( this->Mapper != NULL )
    {
    time = this->Mapper->GetMTime();
    mTime = ( time > mTime ? time : mTime );
    if (this->GetMapper()->GetInput() != NULL)
      {
      this->GetMapper()->GetInput()->Update();
      time = this->Mapper->GetInput()->GetMTime();
      mTime = ( time > mTime ? time : mTime );
      }
    }

  return mTime;
}

//----------------------------------------------------------------------------
void vtkActor::PrintSelf(ostream& os, vtkIndent indent)
{
  this->Superclass::PrintSelf(os,indent);

  if ( this->Mapper )
    {
    os << indent << "Mapper:\n";
    this->Mapper->PrintSelf(os,indent.GetNextIndent());
    }
  else
    {
    os << indent << "Mapper: (none)\n";
    }

  if ( this->Property )
    {
    os << indent << "Property:\n";
    this->Property->PrintSelf(os,indent.GetNextIndent());
    }
  else
    {
    os << indent << "Property: (none)\n";
    }

  if ( this->Texture )
    {
    os << indent << "Texture: " << this->Texture << "\n";
    }
  else
    {
    os << indent << "Texture: (none)\n";
    }

}

//----------------------------------------------------------------------------
// Compatibility methods...to be deprecated in the future.
#ifndef VTK_LEGACY_REMOVE
#include "vtkAssemblyNode.h"
void vtkActor::InitPartTraversal()
{
  VTK_LEGACY_REPLACED_BODY(vtkActor::InitPartTraversal, "VTK 5.2",
                           vtkActor::InitPathTraversal);
  this->InitPathTraversal();
}

//----------------------------------------------------------------------------
vtkActor *vtkActor::GetNextPart()
{
  VTK_LEGACY_REPLACED_BODY(vtkActor::GetNextPart, "VTK 5.2",
                           vtkActor::GetNextPath);
  vtkAssemblyPath *path = this->GetNextPath();
  if ( !path )
    {
    return NULL;
    }
  else
    {
    vtkAssemblyNode *node = path->GetLastNode();
    if ( node && node->GetViewProp()->IsA("vtkActor") )
      {
      return (vtkActor *)node->GetViewProp();
      }
    }
  return NULL;
}

//----------------------------------------------------------------------------
int vtkActor::GetNumberOfParts()
{
  VTK_LEGACY_REPLACED_BODY(vtkActor::GetNumberOfParts, "VTK 5.2",
                           vtkActor::GetNumberOfPaths);
  return this->GetNumberOfPaths();
}
#endif

<commit_msg>BUG: Fix PrintSelf test<commit_after>/*=========================================================================

  Program:   Visualization Toolkit
  Module:    vtkActor.cxx

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/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 notice for more information.

=========================================================================*/
#include "vtkActor.h"

#include "vtkAssemblyPaths.h"
#include "vtkDataArray.h"
#include "vtkGraphicsFactory.h"
#include "vtkImageData.h"
#include "vtkLinearTransform.h"
#include "vtkMapper.h"
#include "vtkMath.h"
#include "vtkPointData.h"
#include "vtkPropCollection.h"
#include "vtkProperty.h"
#include "vtkRenderWindow.h"
#include "vtkRenderer.h"
#include "vtkTexture.h"
#include "vtkTransform.h"

#include <math.h>

vtkCxxRevisionMacro(vtkActor, "1.131");

vtkCxxSetObjectMacro(vtkActor,Texture,vtkTexture);
vtkCxxSetObjectMacro(vtkActor,Mapper,vtkMapper);
vtkCxxSetObjectMacro(vtkActor,BackfaceProperty,vtkProperty);
vtkCxxSetObjectMacro(vtkActor,Property,vtkProperty);

//----------------------------------------------------------------------------
// Needed when we don't use the vtkStandardNewMacro.
vtkInstantiatorNewMacro(vtkActor);

// Creates an actor with the following defaults: origin(0,0,0)
// position=(0,0,0) scale=(1,1,1) visibility=1 pickable=1 dragable=1
// orientation=(0,0,0). No user defined matrix and no texture map.
vtkActor::vtkActor()
{
  this->Mapper = NULL;
  this->Property = NULL;
  this->BackfaceProperty = NULL;
  this->Texture = NULL;

  // The mapper bounds are cache to know when the bounds must be recomputed
  // from the mapper bounds.
  vtkMath::UninitializeBounds(this->MapperBounds);
}

//----------------------------------------------------------------------------
vtkActor::~vtkActor()
{
  if ( this->Property != NULL)
    {
    this->Property->UnRegister(this);
    this->Property = NULL;
    }

  if ( this->BackfaceProperty != NULL)
    {
    this->BackfaceProperty->UnRegister(this);
    this->BackfaceProperty = NULL;
    }

  if (this->Mapper)
    {
    this->Mapper->UnRegister(this);
    this->Mapper = NULL;
    }
  this->SetTexture(NULL);
}

//----------------------------------------------------------------------------
// Shallow copy of an actor.
void vtkActor::ShallowCopy(vtkProp *prop)
{
  vtkActor *a = vtkActor::SafeDownCast(prop);
  if ( a != NULL )
    {
    this->SetMapper(a->GetMapper());
    this->SetProperty(a->GetProperty());
    this->SetBackfaceProperty(a->GetBackfaceProperty());
    this->SetTexture(a->GetTexture());
    }

  // Now do superclass
  this->vtkProp3D::ShallowCopy(prop);
}

//----------------------------------------------------------------------------
// return the correct type of Actor
vtkActor *vtkActor::New()
{
  // First try to create the object from the vtkGraphicsFactory
  vtkObject* ret = vtkGraphicsFactory::CreateInstance("vtkActor");
  return (vtkActor*)ret;
}

//----------------------------------------------------------------------------
void vtkActor::GetActors(vtkPropCollection *ac)
{
  ac->AddItem(this);
}

//----------------------------------------------------------------------------
// should be called from the render methods only
int vtkActor::GetIsOpaque()
{
  if (this->Property->GetOpacity() >= 1.0)
    {
    if (this->Texture && this->Texture->GetInput())
      {
      this->Texture->GetInput()->UpdateInformation();
      this->Texture->GetInput()->SetUpdateExtent(
        this->Texture->GetInput()->GetWholeExtent());
      this->Texture->GetInput()->PropagateUpdateExtent();
      this->Texture->GetInput()->TriggerAsynchronousUpdate();
      this->Texture->GetInput()->UpdateData();
      if (this->Texture->GetInput()->GetPointData()->GetScalars() == NULL)
        { // Handle gracefully. What should it return?
        return 1;
        }
      if (this->Texture->GetInput()->GetPointData()->GetScalars()
        ->GetNumberOfComponents()%2)
        {
        return 1;
        }
      }
    else
      {
      return 1;
      }
    }
  return 0;
}

//----------------------------------------------------------------------------
// This causes the actor to be rendered. It in turn will render the actor's
// property, texture map and then mapper. If a property hasn't been
// assigned, then the actor will create one automatically. Note that a
// side effect of this method is that the visualization network is updated.
int vtkActor::RenderOpaqueGeometry(vtkViewport *vp)
{
  int          renderedSomething = 0;
  vtkRenderer* ren = static_cast<vtkRenderer*>(vp);

  if ( ! this->Mapper )
    {
    return 0;
    }

  // make sure we have a property
  if (!this->Property)
    {
    // force creation of a property
    this->GetProperty();
    }

  // is this actor opaque ?
  if (this->GetIsOpaque())
    {
    this->Property->Render(this, ren);

    // render the backface property
    if (this->BackfaceProperty)
      {
      this->BackfaceProperty->BackfaceRender(this, ren);
      }

    // render the texture
    if (this->Texture)
      {
      this->Texture->Render(ren);
      }
    this->Render(ren,this->Mapper);
    this->Property->PostRender(this, ren);
    this->EstimatedRenderTime += this->Mapper->GetTimeToDraw();
    renderedSomething = 1;
    }

  return renderedSomething;
}

//----------------------------------------------------------------------------
int vtkActor::RenderTranslucentGeometry(vtkViewport *vp)
{
  int          renderedSomething = 0;
  vtkRenderer* ren = static_cast<vtkRenderer*>(vp);

  if ( ! this->Mapper )
    {
    return 0;
    }

  // make sure we have a property
  if (!this->Property)
    {
    // force creation of a property
    this->GetProperty();
    }

  // is this actor opaque ?
  if (!this->GetIsOpaque())
    {
    this->Property->Render(this, ren);

    // render the backface property
    if (this->BackfaceProperty)
      {
      this->BackfaceProperty->BackfaceRender(this, ren);
      }

    // render the texture
    if (this->Texture)
      {
      this->Texture->Render(ren);
      }
    this->Render(ren,this->Mapper);
    this->Property->PostRender(this, ren);
    this->EstimatedRenderTime += this->Mapper->GetTimeToDraw();

    renderedSomething = 1;
    }

  return renderedSomething;
}

//----------------------------------------------------------------------------
void vtkActor::ReleaseGraphicsResources(vtkWindow *win)
{
  vtkRenderWindow *renWin = (vtkRenderWindow *)win;

  // pass this information onto the mapper
  if (this->Mapper)
    {
    this->Mapper->ReleaseGraphicsResources(renWin);
    }

  // pass this information onto the texture
  if (this->Texture)
    {
    this->Texture->ReleaseGraphicsResources(renWin);
    }
}

//----------------------------------------------------------------------------
vtkProperty* vtkActor::MakeProperty()
{
  return vtkProperty::New();
}

//----------------------------------------------------------------------------
vtkProperty *vtkActor::GetProperty()
{
  if ( this->Property == NULL )
    {
    vtkProperty *p = this->MakeProperty();
    this->SetProperty(p);
    p->Delete();
    }
  return this->Property;
}

//----------------------------------------------------------------------------
// Get the bounds for this Actor as (Xmin,Xmax,Ymin,Ymax,Zmin,Zmax).
double *vtkActor::GetBounds()
{
  int i,n;
  double *bounds, bbox[24], *fptr;

  vtkDebugMacro( << "Getting Bounds" );

  // get the bounds of the Mapper if we have one
  if (!this->Mapper)
    {
    return this->Bounds;
    }

  bounds = this->Mapper->GetBounds();
  // Check for the special case when the mapper's bounds are unknown
  if (!bounds)
    {
    return bounds;
    }

  // Check for the special case when the actor is empty.
  if (!vtkMath::AreBoundsInitialized(bounds))
    {
    memcpy( this->MapperBounds, bounds, 6*sizeof(double) );
    vtkMath::UninitializeBounds(this->Bounds);
    this->BoundsMTime.Modified();
    return this->Bounds;
    }

  // Check if we have cached values for these bounds - we cache the
  // values returned by this->Mapper->GetBounds() and we store the time
  // of caching. If the values returned this time are different, or
  // the modified time of this class is newer than the cached time,
  // then we need to rebuild.
  if ( ( memcmp( this->MapperBounds, bounds, 6*sizeof(double) ) != 0 ) ||
       ( this->GetMTime() > this->BoundsMTime ) )
    {
    vtkDebugMacro( << "Recomputing bounds..." );

    memcpy( this->MapperBounds, bounds, 6*sizeof(double) );

    // fill out vertices of a bounding box
    bbox[ 0] = bounds[1]; bbox[ 1] = bounds[3]; bbox[ 2] = bounds[5];
    bbox[ 3] = bounds[1]; bbox[ 4] = bounds[2]; bbox[ 5] = bounds[5];
    bbox[ 6] = bounds[0]; bbox[ 7] = bounds[2]; bbox[ 8] = bounds[5];
    bbox[ 9] = bounds[0]; bbox[10] = bounds[3]; bbox[11] = bounds[5];
    bbox[12] = bounds[1]; bbox[13] = bounds[3]; bbox[14] = bounds[4];
    bbox[15] = bounds[1]; bbox[16] = bounds[2]; bbox[17] = bounds[4];
    bbox[18] = bounds[0]; bbox[19] = bounds[2]; bbox[20] = bounds[4];
    bbox[21] = bounds[0]; bbox[22] = bounds[3]; bbox[23] = bounds[4];

    // save the old transform
    this->Transform->Push();
    this->Transform->SetMatrix(this->GetMatrix());

    // and transform into actors coordinates
    fptr = bbox;
    for (n = 0; n < 8; n++)
      {
      this->Transform->TransformPoint(fptr,fptr);
      fptr += 3;
      }

    this->Transform->Pop();

    // now calc the new bounds
    this->Bounds[0] = this->Bounds[2] = this->Bounds[4] = VTK_DOUBLE_MAX;
    this->Bounds[1] = this->Bounds[3] = this->Bounds[5] = -VTK_DOUBLE_MAX;
    for (i = 0; i < 8; i++)
      {
      for (n = 0; n < 3; n++)
        {
        if (bbox[i*3+n] < this->Bounds[n*2])
          {
          this->Bounds[n*2] = bbox[i*3+n];
          }
        if (bbox[i*3+n] > this->Bounds[n*2+1])
          {
          this->Bounds[n*2+1] = bbox[i*3+n];
          }
        }
      }
    this->BoundsMTime.Modified();
    }

  return this->Bounds;
}

//----------------------------------------------------------------------------
unsigned long int vtkActor::GetMTime()
{
  unsigned long mTime=this->Superclass::GetMTime();
  unsigned long time;

  if ( this->Property != NULL )
    {
    time = this->Property->GetMTime();
    mTime = ( time > mTime ? time : mTime );
    }

  if ( this->BackfaceProperty != NULL )
    {
    time = this->BackfaceProperty->GetMTime();
    mTime = ( time > mTime ? time : mTime );
    }

  if ( this->Texture != NULL )
    {
    time = this->Texture->GetMTime();
    mTime = ( time > mTime ? time : mTime );
    }

  return mTime;
}

//----------------------------------------------------------------------------
unsigned long int vtkActor::GetRedrawMTime()
{
  unsigned long mTime=this->GetMTime();
  unsigned long time;

  if ( this->Mapper != NULL )
    {
    time = this->Mapper->GetMTime();
    mTime = ( time > mTime ? time : mTime );
    if (this->GetMapper()->GetInput() != NULL)
      {
      this->GetMapper()->GetInput()->Update();
      time = this->Mapper->GetInput()->GetMTime();
      mTime = ( time > mTime ? time : mTime );
      }
    }

  return mTime;
}

//----------------------------------------------------------------------------
void vtkActor::PrintSelf(ostream& os, vtkIndent indent)
{
  this->Superclass::PrintSelf(os,indent);

  if ( this->Mapper )
    {
    os << indent << "Mapper:\n";
    this->Mapper->PrintSelf(os,indent.GetNextIndent());
    }
  else
    {
    os << indent << "Mapper: (none)\n";
    }

  if ( this->Property )
    {
    os << indent << "Property:\n";
    this->Property->PrintSelf(os,indent.GetNextIndent());
    }
  else
    {
    os << indent << "Property: (none)\n";
    }

  if ( this->BackfaceProperty )
    {
    os << indent << "BackfaceProperty:\n";
    this->BackfaceProperty->PrintSelf(os,indent.GetNextIndent());
    }
  else
    {
    os << indent << "BackfaceProperty: (none)\n";
    }

  if ( this->Texture )
    {
    os << indent << "Texture: " << this->Texture << "\n";
    }
  else
    {
    os << indent << "Texture: (none)\n";
    }

}

//----------------------------------------------------------------------------
// Compatibility methods...to be deprecated in the future.
#ifndef VTK_LEGACY_REMOVE
#include "vtkAssemblyNode.h"
void vtkActor::InitPartTraversal()
{
  VTK_LEGACY_REPLACED_BODY(vtkActor::InitPartTraversal, "VTK 5.2",
                           vtkActor::InitPathTraversal);
  this->InitPathTraversal();
}

//----------------------------------------------------------------------------
vtkActor *vtkActor::GetNextPart()
{
  VTK_LEGACY_REPLACED_BODY(vtkActor::GetNextPart, "VTK 5.2",
                           vtkActor::GetNextPath);
  vtkAssemblyPath *path = this->GetNextPath();
  if ( !path )
    {
    return NULL;
    }
  else
    {
    vtkAssemblyNode *node = path->GetLastNode();
    if ( node && node->GetViewProp()->IsA("vtkActor") )
      {
      return (vtkActor *)node->GetViewProp();
      }
    }
  return NULL;
}

//----------------------------------------------------------------------------
int vtkActor::GetNumberOfParts()
{
  VTK_LEGACY_REPLACED_BODY(vtkActor::GetNumberOfParts, "VTK 5.2",
                           vtkActor::GetNumberOfPaths);
  return this->GetNumberOfPaths();
}
#endif

<|endoftext|>


<commit_before>// Copyright (c) 2010 The Chromium Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include "remoting/client/x11_view.h"

#include <X11/Xlib.h>
#include <X11/Xutil.h>
#include <X11/extensions/Xrender.h>
#include <X11/extensions/Xcomposite.h>

#include "base/logging.h"
#include "remoting/base/decoder_verbatim.h"

namespace remoting {

X11View::X11View()
    : display_(NULL),
      window_(0),
      width_(0),
      height_(0),
      picture_(0) {
}

X11View::X11View(Display* display, XID window, int width, int height)
    : display_(display),
      window_(window),
      width_(width),
      height_(height),
      picture_(0) {
  media::VideoFrame::CreateFrame(media::VideoFrame::RGB32, width_, height_,
                                 base::TimeDelta(), base::TimeDelta(), &frame_);
  DCHECK(frame_);
}

X11View::~X11View() {
  DCHECK(!display_);
  DCHECK(!window_);
}

bool X11View::Initialize() {
  display_ = XOpenDisplay(NULL);
  if (!display_) {
    return false;
  }

  // Get properties of the screen.
  int screen = DefaultScreen(display_);
  int root_window = RootWindow(display_, screen);

  // Creates the window.
  window_ = XCreateSimpleWindow(display_, root_window, 1, 1, 640, 480, 0,
                                BlackPixel(display_, screen),
                                BlackPixel(display_, screen));
  DCHECK(window_);
  XStoreName(display_, window_, "X11 Remoting");

  // Specifies what kind of messages we want to receive.
  XSelectInput(display_, window_, ExposureMask | ButtonPressMask);
  XMapWindow(display_, window_);
  return true;
}

void X11View::TearDown() {
  if (display_ && window_) {
    // Shutdown the window system.
    XDestroyWindow(display_, window_);
    XCloseDisplay(display_);
  }
  display_ = NULL;
  window_ = 0;
}

void X11View::Paint() {
  // Don't bother attempting to paint if the display hasn't been set up.
  if (!display_ || !window_ || !height_ || !width_ || !frame_) {
    return;
  }

  // TODO(hclam): Paint only the updated regions.
  all_update_rects_.clear();

  // If we have not initialized the render target then do it now.
  if (!picture_)
    InitPaintTarget();

  // Upload the image to a pixmap. And then create a picture from the pixmap
  // and composite the picture over the picture representing the window.

  // Creates a XImage.
  XImage image;
  memset(&image, 0, sizeof(image));
  image.width = width_;
  image.height = height_;
  image.depth = 32;
  image.bits_per_pixel = 32;
  image.format = ZPixmap;
  image.byte_order = LSBFirst;
  image.bitmap_unit = 8;
  image.bitmap_bit_order = LSBFirst;
  image.bytes_per_line = frame_->stride(media::VideoFrame::kRGBPlane);
  image.red_mask = 0xff;
  image.green_mask = 0xff00;
  image.blue_mask = 0xff0000;
  image.data = reinterpret_cast<char*>(
      frame_->data(media::VideoFrame::kRGBPlane));

  // Creates a pixmap and uploads from the XImage.
  unsigned long pixmap = XCreatePixmap(display_, window_, width_, height_, 32);

  GC gc = XCreateGC(display_, pixmap, 0, NULL);
  XPutImage(display_, pixmap, gc, &image, 0, 0, 0, 0, width_, height_);
  XFreeGC(display_, gc);

  // Creates the picture representing the pixmap.
  XID picture = XRenderCreatePicture(
      display_, pixmap,
      XRenderFindStandardFormat(display_, PictStandardARGB32),
      0, NULL);

  // Composite the picture over the picture representing the window.
  XRenderComposite(display_, PictOpSrc, picture, 0,
                   picture_, 0, 0, 0, 0, 0, 0,
                   width_, height_);

  XRenderFreePicture(display_, picture);
  XFreePixmap(display_, pixmap);
}

void X11View::SetSolidFill(uint32 color) {
  // TODO(garykac): Implement.
  // NOTIMPLEMENTED();
}

void X11View::UnsetSolidFill() {
  // TODO(garykac): Implement.
  // NOTIMPLEMENTED();
}

void X11View::SetViewport(int x, int y, int width, int height) {
  // TODO(garykac): Implement.
  // NOTIMPLEMENTED();
}

void X11View::SetHostScreenSize(int width, int height) {
  width_ = width;
  height_ = height;
  XResizeWindow(display_, window_, width_, height_);
  media::VideoFrame::CreateFrame(media::VideoFrame::RGB32, width_, height_,
                                 base::TimeDelta(), base::TimeDelta(), &frame_);
}

void X11View::InitPaintTarget() {
  // Testing XRender support.
  int dummy;
  bool xrender_support = XRenderQueryExtension(display_, &dummy, &dummy);
  CHECK(xrender_support) << "XRender is not supported!";

  XWindowAttributes attr;
  XGetWindowAttributes(display_, window_, &attr);

  XRenderPictFormat* pictformat = XRenderFindVisualFormat(
      display_,
      attr.visual);
  CHECK(pictformat) << "XRENDER does not support default visual";

  picture_ = XRenderCreatePicture(display_, window_, pictformat, 0, NULL);
  CHECK(picture_) << "Backing picture not created";
}

void X11View::HandleBeginUpdateStream(HostMessage* msg) {
  scoped_ptr<HostMessage> deleter(msg);

  // TODO(hclam): Use the information from the message to create the decoder.
  // We lazily construct the decoder.
  if (!decoder_.get()) {
    decoder_.reset(new DecoderVerbatim());
  }

  // Tell the decoder to do start decoding.
  decoder_->BeginDecode(frame_, &update_rects_,
      NewRunnableMethod(this, &X11View::OnPartialDecodeDone),
      NewRunnableMethod(this, &X11View::OnDecodeDone));
}

void X11View::HandleUpdateStreamPacket(HostMessage* msg) {
  decoder_->PartialDecode(msg);
}

void X11View::HandleEndUpdateStream(HostMessage* msg) {
  scoped_ptr<HostMessage> deleter(msg);
  decoder_->EndDecode();
}

void X11View::OnPartialDecodeDone() {
  // Decoder has produced some output so schedule a paint. We'll get a Paint()
  // call in the near future. Note that we can get UpdateStreamPacket during
  // this short period of time and we will perform decode again and the
  // information in updated rects will be lost.
  // There are several ways to solve this problem.
  // 1. Merge the updated rects and perform one paint.
  // 2. Queue the updated rects and perform two paints.
  // 3. Ignore the updated rects and always paint the full image. Since we
  //    use one frame as output this will always be correct.
  // We will take (1) and simply concat the list of rectangles.
  all_update_rects_.insert(all_update_rects_.begin() +
                           all_update_rects_.size(),
                           update_rects_.begin(), update_rects_.end());

  // TODO(hclam): Make sure we call this method on the right thread. Since
  // decoder is single-threaded we don't have a problem but we better post
  // a task to do the right thing.
  XEvent event;
  event.type = Expose;
  XSendEvent(display_, static_cast<int>(window_), true, ExposureMask, &event);
}

void X11View::OnDecodeDone() {
  // Since we do synchronous decoding here there's nothing in this method.
}

}  // namespace remoting
<commit_msg>Change Chromoting X11 client default decoder to Zlib.<commit_after>// Copyright (c) 2010 The Chromium Authors. All rights reserved.
// Use of this source code is governed by a BSD-style license that can be
// found in the LICENSE file.

#include "remoting/client/x11_view.h"

#include <X11/Xlib.h>
#include <X11/Xutil.h>
#include <X11/extensions/Xrender.h>
#include <X11/extensions/Xcomposite.h>

#include "base/logging.h"
#include "remoting/base/decoder_verbatim.h"
#include "remoting/base/decoder_zlib.h"

namespace remoting {

X11View::X11View()
    : display_(NULL),
      window_(0),
      width_(0),
      height_(0),
      picture_(0) {
}

X11View::X11View(Display* display, XID window, int width, int height)
    : display_(display),
      window_(window),
      width_(width),
      height_(height),
      picture_(0) {
  media::VideoFrame::CreateFrame(media::VideoFrame::RGB32, width_, height_,
                                 base::TimeDelta(), base::TimeDelta(), &frame_);
  DCHECK(frame_);
}

X11View::~X11View() {
  DCHECK(!display_);
  DCHECK(!window_);
}

bool X11View::Initialize() {
  display_ = XOpenDisplay(NULL);
  if (!display_) {
    return false;
  }

  // Get properties of the screen.
  int screen = DefaultScreen(display_);
  int root_window = RootWindow(display_, screen);

  // Creates the window.
  window_ = XCreateSimpleWindow(display_, root_window, 1, 1, 640, 480, 0,
                                BlackPixel(display_, screen),
                                BlackPixel(display_, screen));
  DCHECK(window_);
  XStoreName(display_, window_, "X11 Remoting");

  // Specifies what kind of messages we want to receive.
  XSelectInput(display_, window_, ExposureMask | ButtonPressMask);
  XMapWindow(display_, window_);
  return true;
}

void X11View::TearDown() {
  if (display_ && window_) {
    // Shutdown the window system.
    XDestroyWindow(display_, window_);
    XCloseDisplay(display_);
  }
  display_ = NULL;
  window_ = 0;
}

void X11View::Paint() {
  // Don't bother attempting to paint if the display hasn't been set up.
  if (!display_ || !window_ || !height_ || !width_ || !frame_) {
    return;
  }

  // TODO(hclam): Paint only the updated regions.
  all_update_rects_.clear();

  // If we have not initialized the render target then do it now.
  if (!picture_)
    InitPaintTarget();

  // Upload the image to a pixmap. And then create a picture from the pixmap
  // and composite the picture over the picture representing the window.

  // Creates a XImage.
  XImage image;
  memset(&image, 0, sizeof(image));
  image.width = width_;
  image.height = height_;
  image.depth = 32;
  image.bits_per_pixel = 32;
  image.format = ZPixmap;
  image.byte_order = LSBFirst;
  image.bitmap_unit = 8;
  image.bitmap_bit_order = LSBFirst;
  image.bytes_per_line = frame_->stride(media::VideoFrame::kRGBPlane);
  image.red_mask = 0xff;
  image.green_mask = 0xff00;
  image.blue_mask = 0xff0000;
  image.data = reinterpret_cast<char*>(
      frame_->data(media::VideoFrame::kRGBPlane));

  // Creates a pixmap and uploads from the XImage.
  unsigned long pixmap = XCreatePixmap(display_, window_, width_, height_, 32);

  GC gc = XCreateGC(display_, pixmap, 0, NULL);
  XPutImage(display_, pixmap, gc, &image, 0, 0, 0, 0, width_, height_);
  XFreeGC(display_, gc);

  // Creates the picture representing the pixmap.
  XID picture = XRenderCreatePicture(
      display_, pixmap,
      XRenderFindStandardFormat(display_, PictStandardARGB32),
      0, NULL);

  // Composite the picture over the picture representing the window.
  XRenderComposite(display_, PictOpSrc, picture, 0,
                   picture_, 0, 0, 0, 0, 0, 0,
                   width_, height_);

  XRenderFreePicture(display_, picture);
  XFreePixmap(display_, pixmap);
}

void X11View::SetSolidFill(uint32 color) {
  // TODO(garykac): Implement.
  // NOTIMPLEMENTED();
}

void X11View::UnsetSolidFill() {
  // TODO(garykac): Implement.
  // NOTIMPLEMENTED();
}

void X11View::SetViewport(int x, int y, int width, int height) {
  // TODO(garykac): Implement.
  // NOTIMPLEMENTED();
}

void X11View::SetHostScreenSize(int width, int height) {
  width_ = width;
  height_ = height;
  XResizeWindow(display_, window_, width_, height_);
  media::VideoFrame::CreateFrame(media::VideoFrame::RGB32, width_, height_,
                                 base::TimeDelta(), base::TimeDelta(), &frame_);
}

void X11View::InitPaintTarget() {
  // Testing XRender support.
  int dummy;
  bool xrender_support = XRenderQueryExtension(display_, &dummy, &dummy);
  CHECK(xrender_support) << "XRender is not supported!";

  XWindowAttributes attr;
  XGetWindowAttributes(display_, window_, &attr);

  XRenderPictFormat* pictformat = XRenderFindVisualFormat(
      display_,
      attr.visual);
  CHECK(pictformat) << "XRENDER does not support default visual";

  picture_ = XRenderCreatePicture(display_, window_, pictformat, 0, NULL);
  CHECK(picture_) << "Backing picture not created";
}

void X11View::HandleBeginUpdateStream(HostMessage* msg) {
  scoped_ptr<HostMessage> deleter(msg);

  // TODO(hclam): Use the information from the message to create the decoder.
  // We lazily construct the decoder.
  if (!decoder_.get()) {
    decoder_.reset(new DecoderZlib());
  }

  // Tell the decoder to do start decoding.
  decoder_->BeginDecode(frame_, &update_rects_,
      NewRunnableMethod(this, &X11View::OnPartialDecodeDone),
      NewRunnableMethod(this, &X11View::OnDecodeDone));
}

void X11View::HandleUpdateStreamPacket(HostMessage* msg) {
  decoder_->PartialDecode(msg);
}

void X11View::HandleEndUpdateStream(HostMessage* msg) {
  scoped_ptr<HostMessage> deleter(msg);
  decoder_->EndDecode();
}

void X11View::OnPartialDecodeDone() {
  // Decoder has produced some output so schedule a paint. We'll get a Paint()
  // call in the near future. Note that we can get UpdateStreamPacket during
  // this short period of time and we will perform decode again and the
  // information in updated rects will be lost.
  // There are several ways to solve this problem.
  // 1. Merge the updated rects and perform one paint.
  // 2. Queue the updated rects and perform two paints.
  // 3. Ignore the updated rects and always paint the full image. Since we
  //    use one frame as output this will always be correct.
  // We will take (1) and simply concat the list of rectangles.
  all_update_rects_.insert(all_update_rects_.begin() +
                           all_update_rects_.size(),
                           update_rects_.begin(), update_rects_.end());

  // TODO(hclam): Make sure we call this method on the right thread. Since
  // decoder is single-threaded we don't have a problem but we better post
  // a task to do the right thing.
  XEvent event;
  event.type = Expose;
  XSendEvent(display_, static_cast<int>(window_), true, ExposureMask, &event);
}

void X11View::OnDecodeDone() {
  // Since we do synchronous decoding here there's nothing in this method.
}

}  // namespace remoting
<|endoftext|>


<commit_before>#if defined( DEBUG ) || defined( _DEBUG)
	//#include "vld.h"
#endif

#ifdef _WIN32

#include "WindowsContainer.h"
int WINAPI WinMain(
	HINSTANCE	hInstance,
	HINSTANCE	hPrevInstance,
	LPSTR		lpCmdLine,
	int			nCmdShow)
{
	WindowsContainer win;
	win.main(hInstance, hPrevInstance, lpCmdLine, nCmdShow);

	return 0;
}

#else

#include "LinuxContainer.h"
int main(int argc, char** argv)
{
	LinuxContainer* linuxContainer = new LinuxContainer();
	linuxContainer->mainContainer(argc, argv);
	DELETE_NULL(linuxContainer);

	return 0;
}

#endif //_WIN32
<commit_msg>redundant<commit_after>#if defined( DEBUG ) || defined( _DEBUG)
	#include "vld.h"
#endif

#ifdef _WIN32

#include "WindowsContainer.h"
int WINAPI WinMain(
	HINSTANCE	hInstance,
	HINSTANCE	hPrevInstance,
	LPSTR		lpCmdLine,
	int			nCmdShow)
{
	WindowsContainer win;
	win.main(hInstance, hPrevInstance, lpCmdLine, nCmdShow);

	return 0;
}

#else

#include "LinuxContainer.h"
int main(int argc, char** argv)
{
	LinuxContainer* linuxContainer = new LinuxContainer();
	linuxContainer->mainContainer(argc, argv);
	DELETE_NULL(linuxContainer);

	return 0;
}

#endif //_WIN32
<|endoftext|>


<commit_before>// To find the largest and smallest element in a give L, R range
// in an array of elememts

#include <iostream>
#include <algorithm>
#include <cmath>
#include <cstdio>

using namespace std;

#define SIZE 1002

void min_sparse_table(int b[SIZE][SIZE], int n) {
	int y, k;

	k = log2(n);
	for(int i = 1; i <= k; i++) {
		y = (int)(pow(2.0, (double)(i)));
		for(int j = 0; j <= n-y; j++)
			b[i][j] = min(b[i-1][j] , b[i-1][j+(y/2)]);
	}
}

inline int min_query(int b[SIZE][SIZE], int l, int r) {
	int k, x;

	k = log2(r - l + 1);
	x = (int)(pow(2.0,(double)(k))) - 1;

	return min(b[k][l],b[k][r-x]);
}

void max_sparse_table(int c[SIZE][SIZE], int n) {
	int y, k;

	k = log2(n);
	for(int i = 1; i <= k; i++) {
		y = (int) (pow(2.0,(double)(i)));
		for(int j = 0; j <= n-y; j++)
			c[i][j] = max(c[i-1][j] , c[i-1][j+(y/2)]);
	}
}

inline int max_query(int c[SIZE][SIZE], int l, int r) {
	int k, x;

	k = log2(r - l + 1);
	x = (int)(pow(2.0, (double)(k))) - 1;

	return max(c[k][l], c[k][r-x]);
}

int main() {
	int N, b[SIZE][SIZE],c[SIZE][SIZE];
	int Q, L, R;

	printf("Enter N (max 1000): ");
	scanf("%d", &N);

	printf("Enter elements:\n");
	for(int i = 0; i < N; i++) {
		scanf("%d", &b[0][i]);
		c[0][i] = b[0][i];
	}

	min_sparse_table(b, N);
	max_sparse_table(c, N);

	printf("Enter number of L,R queries : ");
	scanf("%d", &Q);

	for(int i = 0; i < Q; i++) {
		printf("Enter L, R (1-based) : ");
		scanf("%d %d", &L, &R);

		printf("Minimum in range (%d, %d) = %d\n\n",
			L-1, R-1, min_query(b, L-1, R-1));
		printf("Maximum in range (%d, %d) = %d\n\n",
			L-1, R-1, max_query(c, L-1, R-1));
	}

	return 0;
}<commit_msg>Updated SparseTable.cpp<commit_after>// To find the largest and smallest element in a give
// L, R range in an array of elememts

#include <iostream>
#include <algorithm>
#include <cmath>
#include <cstdio>

using namespace std;

#define SIZE 1002

void min_sparse_table(int b[SIZE][SIZE], int n) {
	int y, k;

	k = log2(n);
	for(int i = 1; i <= k; i++) {
		y = (int)(pow(2.0, (double)(i)));
		for(int j = 0; j <= n-y; j++)
			b[i][j] = min(b[i-1][j] , b[i-1][j+(y/2)]);
	}
}

inline int min_query(int b[SIZE][SIZE], int l, int r) {
	int k, x;

	k = log2(r - l + 1);
	x = (int)(pow(2.0,(double)(k))) - 1;

	return min(b[k][l],b[k][r-x]);
}

void max_sparse_table(int c[SIZE][SIZE], int n) {
	int y, k;

	k = log2(n);
	for(int i = 1; i <= k; i++) {
		y = (int) (pow(2.0,(double)(i)));
		for(int j = 0; j <= n-y; j++)
			c[i][j] = max(c[i-1][j] , c[i-1][j+(y/2)]);
	}
}

inline int max_query(int c[SIZE][SIZE], int l, int r) {
	int k, x;

	k = log2(r - l + 1);
	x = (int)(pow(2.0, (double)(k))) - 1;

	return max(c[k][l], c[k][r-x]);
}

int main() {
	int N, b[SIZE][SIZE],c[SIZE][SIZE];
	int Q, L, R;

	printf("Enter N (max 1000): ");
	scanf("%d", &N);

	printf("Enter elements:\n");
	for(int i = 0; i < N; i++) {
		scanf("%d", &b[0][i]);
		c[0][i] = b[0][i];
	}

	min_sparse_table(b, N);
	max_sparse_table(c, N);

	printf("Enter number of L,R queries : ");
	scanf("%d", &Q);

	for(int i = 0; i < Q; i++) {
		printf("Enter L, R (1-based) : ");
		scanf("%d %d", &L, &R);

		printf("Minimum in range (%d, %d) = %d\n\n",
			L-1, R-1, min_query(b, L-1, R-1));
		printf("Maximum in range (%d, %d) = %d\n\n",
			L-1, R-1, max_query(c, L-1, R-1));
	}

	return 0;
}<|endoftext|>


<commit_before>/*
 * Software License Agreement (BSD License)
 *
 * Copyright (c) 2014, Daiki Maekawa and Robot Design and Control Lab.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 *  * Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *  * Redistributions in binary form must reproduce the above
 *    copyright notice, this list of conditions and the following
 *    disclaimer in the documentation and/or other materials provided
 *    with the distribution.
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include <ros/ros.h>
#include <std_msgs/String.h>
#include <std_srvs/Empty.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <tf/tf.h>
#include <tf/transform_listener.h>

#include <yaml-cpp/yaml.h>

#include <vector>
#include <fstream>
#include <string>

#include <visualization_msgs/MarkerArray.h>

#ifdef NEW_YAMLCPP
template<typename T>
void operator >> (const YAML::Node& node, T& i)
{
    i = node.as<T>();
}
#endif

class WaypointsNavigation{
public:
    WaypointsNavigation() :
        has_activate_(false),
        is_stop_(false),
        move_base_action_("move_base", true),
        rate_(10),
        last_moved_time_(0)
    {
        while((move_base_action_.waitForServer(ros::Duration(1.0)) == false) && (ros::ok() == true))
        {
            ROS_INFO("Waiting...");
        }

        ros::NodeHandle private_nh("~");
        private_nh.param("robot_frame", robot_frame_, std::string("/base_link"));
        private_nh.param("world_frame", world_frame_, std::string("/map"));

        double max_update_rate;
        private_nh.param("max_update_rate", max_update_rate, 10.0);
        rate_ = ros::Rate(max_update_rate);
        std::string filename = "";
        private_nh.param("filename", filename, filename);
        if(filename != ""){
            ROS_INFO_STREAM("Read waypoints data from " << filename);
            readFile(filename);
        }

        ros::NodeHandle nh;
        syscommand_sub_ = nh.subscribe("syscommand", 1, &WaypointsNavigation::syscommandCallback, this);
        cmd_vel_sub_ = nh.subscribe("icart_mini/cmd_vel", 1, &WaypointsNavigation::cmdVelCallback, this);
        stop_point_sub_ = nh.subscribe("stop_point",1,&WaypointsNavigation::stopPointCallback, this);
        marker_pub_ = nh.advertise<visualization_msgs::MarkerArray>("visualization_marker", 10);
        clear_costmaps_srv_ = nh.serviceClient<std_srvs::Empty>("/move_base/clear_costmaps");
    }

    void syscommandCallback(const std_msgs::String &msg){
        if(msg.data == "start"){
            has_activate_ = true;
        }
        if(msg.data == "restart"){
            has_restart_ = true;
            ROS_INFO_STREAM("restart command received");
        }
    }

    void cmdVelCallback(const geometry_msgs::Twist &msg){
        if(msg.linear.x > -0.001 && msg.linear.x < 0.001   &&
           msg.linear.y > -0.001 && msg.linear.y < 0.001   &&
           msg.linear.z > -0.001 && msg.linear.z < 0.001   &&
           msg.angular.x > -0.001 && msg.angular.x < 0.001 &&
           msg.angular.y > -0.001 && msg.angular.y < 0.001 &&
           msg.angular.z > -0.001 && msg.angular.z < 0.001){

            ROS_INFO("command velocity all zero");
        }else{
            last_moved_time_ = ros::Time::now().toSec();
        }
    }

    void stopPointCallback(const geometry_msgs::PointStamped &msg){
        is_stop_ = true;
        stop_point_.point.x = msg.point.x;
        stop_point_.point.y = msg.point.y;
        stop_point_.point.z = msg.point.z;
    }

    bool readFile(const std::string &filename){
        waypoints_.clear();
        try{
            std::ifstream ifs(filename.c_str(), std::ifstream::in);
            if(ifs.good() == false){
                return false;
            }

            YAML::Node node;

            #ifdef NEW_YAMLCPP
                node = YAML::Load(ifs);
            #else
                YAML::Parser parser(ifs);
                parser.GetNextDocument(node);
            #endif

            #ifdef NEW_YAMLCPP
                const YAML::Node &wp_node_tmp = node["waypoints"];
                const YAML::Node *wp_node = wp_node_tmp ? &wp_node_tmp : NULL;
            #else
                const YAML::Node *wp_node = node.FindValue("waypoints");
            #endif

            if(wp_node != NULL){
                for(int i=0; i < wp_node->size(); i++){
                    geometry_msgs::PointStamped point;

                    (*wp_node)[i]["point"]["x"] >> point.point.x;
                    (*wp_node)[i]["point"]["y"] >> point.point.y;
                    (*wp_node)[i]["point"]["z"] >> point.point.z;

                    waypoints_.push_back(point);

                }
            }else{
                return false;
            }

            #ifdef NEW_YAMLCPP
                const YAML::Node &fp_node_tmp = node["finish_pose"];
                const YAML::Node *fp_node = fp_node_tmp ? &fp_node_tmp : NULL;
            #else
                const YAML::Node *fp_node = node.FindValue("finish_pose");
            #endif

            if(fp_node != NULL){
                (*fp_node)["pose"]["position"]["x"] >> finish_pose_.position.x;
                (*fp_node)["pose"]["position"]["y"] >> finish_pose_.position.y;
                (*fp_node)["pose"]["position"]["z"] >> finish_pose_.position.z;

                (*fp_node)["pose"]["orientation"]["x"] >> finish_pose_.orientation.x;
                (*fp_node)["pose"]["orientation"]["y"] >> finish_pose_.orientation.y;
                (*fp_node)["pose"]["orientation"]["z"] >> finish_pose_.orientation.z;
                (*fp_node)["pose"]["orientation"]["w"] >> finish_pose_.orientation.w;
            }else{
                return false;
            }

        }catch(YAML::ParserException &e){
            return false;

        }catch(YAML::RepresentationException &e){
            return false;
        }

        return true;
    }

    bool shouldSendGoal(){
        bool ret = true;
        actionlib::SimpleClientGoalState state = move_base_action_.getState();
        if((state != actionlib::SimpleClientGoalState::ACTIVE) &&
           (state != actionlib::SimpleClientGoalState::PENDING) &&
           (state != actionlib::SimpleClientGoalState::RECALLED) &&
           (state != actionlib::SimpleClientGoalState::PREEMPTED))
        {
            ret = false;
        }

        if(waypoints_.empty()){
            ret = false;
        }

        return ret;
    }

    bool navigationFinished(){
        return move_base_action_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED;
    }

    bool onNavigationPoint(const geometry_msgs::Point &dest, double dist_err = 0.5){
        tf::StampedTransform robot_gl = getRobotPosGL();

        const double wx = dest.x;
        const double wy = dest.y;
        const double rx = robot_gl.getOrigin().x();
        const double ry = robot_gl.getOrigin().y();
        const double dist = std::sqrt(std::pow(wx - rx, 2) + std::pow(wy - ry, 2));

        return dist < dist_err;
    }

    tf::StampedTransform getRobotPosGL(){
        tf::StampedTransform robot_gl;
        try{
            tf_listener_.lookupTransform(world_frame_, robot_frame_, ros::Time(0.0), robot_gl);
        }catch(tf::TransformException &e){
            ROS_WARN_STREAM("tf::TransformException: " << e.what());
        }

        return robot_gl;
    }

    void sleep(){
        rate_.sleep();
        ros::spinOnce();
        publishMarkers();
    }

    void startNavigationGL(const geometry_msgs::Point &dest){
        geometry_msgs::Pose pose;
        pose.position = dest;
        pose.orientation = tf::createQuaternionMsgFromYaw(0.0);
        startNavigationGL(pose);
    }

    void startNavigationGL(const geometry_msgs::Pose &dest){
        move_base_msgs::MoveBaseGoal move_base_goal;
        move_base_goal.target_pose.header.stamp = ros::Time::now();
        move_base_goal.target_pose.header.frame_id = world_frame_;
        move_base_goal.target_pose.pose.position = dest.position;
        move_base_goal.target_pose.pose.orientation = dest.orientation;

        move_base_action_.sendGoal(move_base_goal);
    }

    void publishMarkers(){
        visualization_msgs::MarkerArray markers_array;
        for(int i=0; i < waypoints_.size(); i++){
            visualization_msgs::Marker marker, label;
            marker.header.frame_id = world_frame_;
            marker.header.stamp = ros::Time::now();
            marker.scale.x = 0.2;
            marker.scale.y = 0.2;
            marker.scale.z = 0.2;
            marker.pose.position.z = marker.scale.z / 2.0;
            marker.color.r = 0.8f;
            marker.color.g = 0.2f;
            marker.color.b = 0.2f;

            std::stringstream name;
            name << "waypoint " << i;
            marker.ns = name.str();
            marker.id = i;
            marker.pose.position.x = waypoints_[i].point.x;
            marker.pose.position.y = waypoints_[i].point.y;
            marker.type = visualization_msgs::Marker::SPHERE;
            marker.action = visualization_msgs::Marker::ADD;
            marker.color.a = 1.0f;
            markers_array.markers.push_back(marker);

            //ROS_INFO_STREAM("waypoints \n" << waypoints_[i]);
        }
        marker_pub_.publish(markers_array);
    }

    void run(){
        while(ros::ok()){
            if(has_activate_){
                std::vector<geometry_msgs::PointStamped>::iterator waypoints_it = waypoints_.begin();
                for(waypoints_it; waypoints_it != waypoints_.end(); ++waypoints_it){
                    if(is_stop_){
                        waypoints_.insert(waypoints_it, stop_point_);
                        ROS_INFO_STREAM("Stop point received");
                    }
                    ROS_INFO_STREAM("waypoint = " << *waypoints_it);

                    if(!ros::ok()) break;

                    startNavigationGL(waypoints_it->point);
                    double start_nav_time = ros::Time::now().toSec();
                    while(!onNavigationPoint(waypoints_it->point)){
                        double time = ros::Time::now().toSec();
                        if(time - start_nav_time > 10.0 && time - last_moved_time_ > 10.0){
                            ROS_WARN("Resend the navigation goal.");
                            std_srvs::Empty empty;
                            clear_costmaps_srv_.call(empty);
                            startNavigationGL(waypoints_it->point);
                            start_nav_time = time;
                        }
                        actionlib::SimpleClientGoalState state = move_base_action_.getState();
                        sleep();
                    }
                    ROS_INFO("waypoint goal");
                    if(is_stop_){
                        while(!has_restart_){
                            sleep();
                            ROS_INFO_STREAM("Wait for restart");
                        }
                        ROS_INFO_STREAM("Restart");
                        has_restart_ = false;
                        is_stop_ = false;
                    }
                }
                ROS_INFO("waypoints clear");
                waypoints_.clear();
                startNavigationGL(finish_pose_);
                while(!navigationFinished() && ros::ok()) sleep();
                has_activate_ = false;
            }

            sleep();
        }
    }

private:
    actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> move_base_action_;
    std::vector<geometry_msgs::PointStamped> waypoints_;
    geometry_msgs::PointStamped stop_point_;
    geometry_msgs::Pose finish_pose_;
    bool has_activate_;
    bool is_stop_;
    bool has_restart_;
    std::string robot_frame_, world_frame_;
    tf::TransformListener tf_listener_;
    ros::Rate rate_;
    ros::Subscriber syscommand_sub_;
    ros::Subscriber cmd_vel_sub_;
    ros::Subscriber stop_point_sub_;
    ros::Publisher marker_pub_;
    ros::ServiceClient clear_costmaps_srv_;
    double last_moved_time_;
};

int main(int argc, char *argv[]){
    ros::init(argc, argv, ROS_PACKAGE_NAME);
    WaypointsNavigation w_nav;
    w_nav.run();

    return 0;
}
<commit_msg>Change stop timing<commit_after>/*
 * Software License Agreement (BSD License)
 *
 * Copyright (c) 2014, Daiki Maekawa and Robot Design and Control Lab.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 *  * Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *  * Redistributions in binary form must reproduce the above
 *    copyright notice, this list of conditions and the following
 *    disclaimer in the documentation and/or other materials provided
 *    with the distribution.
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include <ros/ros.h>
#include <std_msgs/String.h>
#include <std_srvs/Empty.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <tf/tf.h>
#include <tf/transform_listener.h>

#include <yaml-cpp/yaml.h>

#include <vector>
#include <fstream>
#include <string>

#include <visualization_msgs/MarkerArray.h>

#ifdef NEW_YAMLCPP
template<typename T>
void operator >> (const YAML::Node& node, T& i)
{
    i = node.as<T>();
}
#endif

class WaypointsNavigation{
public:
    WaypointsNavigation() :
        has_activate_(false),
        is_stop_(false),
	in_stop_(false),
        move_base_action_("move_base", true),
        rate_(10),
        last_moved_time_(0)
    {
        while((move_base_action_.waitForServer(ros::Duration(1.0)) == false) && (ros::ok() == true))
        {
            ROS_INFO("Waiting...");
        }

        ros::NodeHandle private_nh("~");
        private_nh.param("robot_frame", robot_frame_, std::string("/base_link"));
        private_nh.param("world_frame", world_frame_, std::string("/map"));

        double max_update_rate;
        private_nh.param("max_update_rate", max_update_rate, 10.0);
        rate_ = ros::Rate(max_update_rate);
        std::string filename = "";
        private_nh.param("filename", filename, filename);
        if(filename != ""){
            ROS_INFO_STREAM("Read waypoints data from " << filename);
            readFile(filename);
        }

        ros::NodeHandle nh;
        syscommand_sub_ = nh.subscribe("syscommand", 1, &WaypointsNavigation::syscommandCallback, this);
        cmd_vel_sub_ = nh.subscribe("icart_mini/cmd_vel", 1, &WaypointsNavigation::cmdVelCallback, this);
        stop_point_sub_ = nh.subscribe("stop_point",1,&WaypointsNavigation::stopPointCallback, this);
        marker_pub_ = nh.advertise<visualization_msgs::MarkerArray>("visualization_marker", 10);
        clear_costmaps_srv_ = nh.serviceClient<std_srvs::Empty>("/move_base/clear_costmaps");
    }

    void syscommandCallback(const std_msgs::String &msg){
        if(msg.data == "start"){
            has_activate_ = true;
        }
        if(msg.data == "restart"){
            has_restart_ = true;
            ROS_INFO_STREAM("restart command received");
        }
    }

    void cmdVelCallback(const geometry_msgs::Twist &msg){
        if(msg.linear.x > -0.001 && msg.linear.x < 0.001   &&
           msg.linear.y > -0.001 && msg.linear.y < 0.001   &&
           msg.linear.z > -0.001 && msg.linear.z < 0.001   &&
           msg.angular.x > -0.001 && msg.angular.x < 0.001 &&
           msg.angular.y > -0.001 && msg.angular.y < 0.001 &&
           msg.angular.z > -0.001 && msg.angular.z < 0.001){

            ROS_INFO("command velocity all zero");
        }else{
            last_moved_time_ = ros::Time::now().toSec();
        }
    }

    void stopPointCallback(const geometry_msgs::PointStamped &msg){
        is_stop_ = true;
        stop_point_.point.x = msg.point.x;
        stop_point_.point.y = msg.point.y;
        stop_point_.point.z = msg.point.z;
    }

    bool readFile(const std::string &filename){
        waypoints_.clear();
        try{
            std::ifstream ifs(filename.c_str(), std::ifstream::in);
            if(ifs.good() == false){
                return false;
            }

            YAML::Node node;

            #ifdef NEW_YAMLCPP
                node = YAML::Load(ifs);
            #else
                YAML::Parser parser(ifs);
                parser.GetNextDocument(node);
            #endif

            #ifdef NEW_YAMLCPP
                const YAML::Node &wp_node_tmp = node["waypoints"];
                const YAML::Node *wp_node = wp_node_tmp ? &wp_node_tmp : NULL;
            #else
                const YAML::Node *wp_node = node.FindValue("waypoints");
            #endif

            if(wp_node != NULL){
                for(int i=0; i < wp_node->size(); i++){
                    geometry_msgs::PointStamped point;

                    (*wp_node)[i]["point"]["x"] >> point.point.x;
                    (*wp_node)[i]["point"]["y"] >> point.point.y;
                    (*wp_node)[i]["point"]["z"] >> point.point.z;

                    waypoints_.push_back(point);

                }
            }else{
                return false;
            }

            #ifdef NEW_YAMLCPP
                const YAML::Node &fp_node_tmp = node["finish_pose"];
                const YAML::Node *fp_node = fp_node_tmp ? &fp_node_tmp : NULL;
            #else
                const YAML::Node *fp_node = node.FindValue("finish_pose");
            #endif

            if(fp_node != NULL){
                (*fp_node)["pose"]["position"]["x"] >> finish_pose_.position.x;
                (*fp_node)["pose"]["position"]["y"] >> finish_pose_.position.y;
                (*fp_node)["pose"]["position"]["z"] >> finish_pose_.position.z;

                (*fp_node)["pose"]["orientation"]["x"] >> finish_pose_.orientation.x;
                (*fp_node)["pose"]["orientation"]["y"] >> finish_pose_.orientation.y;
                (*fp_node)["pose"]["orientation"]["z"] >> finish_pose_.orientation.z;
                (*fp_node)["pose"]["orientation"]["w"] >> finish_pose_.orientation.w;
            }else{
                return false;
            }

        }catch(YAML::ParserException &e){
            return false;

        }catch(YAML::RepresentationException &e){
            return false;
        }

        return true;
    }

    bool shouldSendGoal(){
        bool ret = true;
        actionlib::SimpleClientGoalState state = move_base_action_.getState();
        if((state != actionlib::SimpleClientGoalState::ACTIVE) &&
           (state != actionlib::SimpleClientGoalState::PENDING) &&
           (state != actionlib::SimpleClientGoalState::RECALLED) &&
           (state != actionlib::SimpleClientGoalState::PREEMPTED))
        {
            ret = false;
        }

        if(waypoints_.empty()){
            ret = false;
        }

        return ret;
    }

    bool navigationFinished(){
        return move_base_action_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED;
    }

    bool onNavigationPoint(const geometry_msgs::Point &dest, double dist_err = 0.5){
        tf::StampedTransform robot_gl = getRobotPosGL();

        const double wx = dest.x;
        const double wy = dest.y;
        const double rx = robot_gl.getOrigin().x();
        const double ry = robot_gl.getOrigin().y();
        const double dist = std::sqrt(std::pow(wx - rx, 2) + std::pow(wy - ry, 2));

        return dist < dist_err;
    }

    tf::StampedTransform getRobotPosGL(){
        tf::StampedTransform robot_gl;
        try{
            tf_listener_.lookupTransform(world_frame_, robot_frame_, ros::Time(0.0), robot_gl);
        }catch(tf::TransformException &e){
            ROS_WARN_STREAM("tf::TransformException: " << e.what());
        }

        return robot_gl;
    }

    void sleep(){
        rate_.sleep();
        ros::spinOnce();
        publishMarkers();
    }

    void startNavigationGL(const geometry_msgs::Point &dest){
        geometry_msgs::Pose pose;
        pose.position = dest;
        pose.orientation = tf::createQuaternionMsgFromYaw(0.0);
        startNavigationGL(pose);
    }

    void startNavigationGL(const geometry_msgs::Pose &dest){
        move_base_msgs::MoveBaseGoal move_base_goal;
        move_base_goal.target_pose.header.stamp = ros::Time::now();
        move_base_goal.target_pose.header.frame_id = world_frame_;
        move_base_goal.target_pose.pose.position = dest.position;
        move_base_goal.target_pose.pose.orientation = dest.orientation;

        move_base_action_.sendGoal(move_base_goal);
    }

    void publishMarkers(){
        visualization_msgs::MarkerArray markers_array;
        for(int i=0; i < waypoints_.size(); i++){
            visualization_msgs::Marker marker, label;
            marker.header.frame_id = world_frame_;
            marker.header.stamp = ros::Time::now();
            marker.scale.x = 0.2;
            marker.scale.y = 0.2;
            marker.scale.z = 0.2;
            marker.pose.position.z = marker.scale.z / 2.0;
            marker.color.r = 0.8f;
            marker.color.g = 0.2f;
            marker.color.b = 0.2f;

            std::stringstream name;
            name << "waypoint " << i;
            marker.ns = name.str();
            marker.id = i;
            marker.pose.position.x = waypoints_[i].point.x;
            marker.pose.position.y = waypoints_[i].point.y;
            marker.type = visualization_msgs::Marker::SPHERE;
            marker.action = visualization_msgs::Marker::ADD;
            marker.color.a = 1.0f;
            markers_array.markers.push_back(marker);

            //ROS_INFO_STREAM("waypoints \n" << waypoints_[i]);
        }
        marker_pub_.publish(markers_array);
    }

    void run(){
        while(ros::ok()){
            if(has_activate_){
                std::vector<geometry_msgs::PointStamped>::iterator waypoints_it = waypoints_.begin();
                for(waypoints_it; waypoints_it != waypoints_.end(); ++waypoints_it){
                    if(!ros::ok()) break;

                    if(is_stop_ && !in_stop_){
                    	ROS_INFO_STREAM("waypoint = " << *waypoints_it);
                        waypoints_.insert(waypoints_it, stop_point_);
                        ROS_INFO_STREAM("Stop point received");
			is_stop_ = false;
			in_stop_ = true;
                    }

                    startNavigationGL(waypoints_it->point);
                    double start_nav_time = ros::Time::now().toSec();
                    while(!onNavigationPoint(waypoints_it->point)){
                        double time = ros::Time::now().toSec();
                        if(time - start_nav_time > 10.0 && time - last_moved_time_ > 10.0){
                            ROS_WARN("Resend the navigation goal.");
                            std_srvs::Empty empty;
                            clear_costmaps_srv_.call(empty);
                            startNavigationGL(waypoints_it->point);
                            start_nav_time = time;
                        }
                        actionlib::SimpleClientGoalState state = move_base_action_.getState();
                        sleep();
                    }
                    ROS_INFO("waypoint goal");

                    ROS_INFO_STREAM("waypoint = " << *waypoints_it);
		    if(!is_stop_ && in_stop_ ){
                        while(!has_restart_){
                            sleep();
                            ROS_INFO_STREAM("Wait for restart");
                        }
                        ROS_INFO_STREAM("Restart");
                        has_restart_ = false;
			in_stop_ = false;
                    }
                }
                ROS_INFO("waypoints clear");
                waypoints_.clear();
                startNavigationGL(finish_pose_);
                while(!navigationFinished() && ros::ok()) sleep();
                has_activate_ = false;
            }

            sleep();
        }
    }

private:
    actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> move_base_action_;
    std::vector<geometry_msgs::PointStamped> waypoints_;
    geometry_msgs::PointStamped stop_point_;
    geometry_msgs::Pose finish_pose_;
    bool has_activate_;
    bool is_stop_;
    bool in_stop_;
    bool has_restart_;
    std::string robot_frame_, world_frame_;
    tf::TransformListener tf_listener_;
    ros::Rate rate_;
    ros::Subscriber syscommand_sub_;
    ros::Subscriber cmd_vel_sub_;
    ros::Subscriber stop_point_sub_;
    ros::Publisher marker_pub_;
    ros::ServiceClient clear_costmaps_srv_;
    double last_moved_time_;
};

int main(int argc, char *argv[]){
    ros::init(argc, argv, ROS_PACKAGE_NAME);
    WaypointsNavigation w_nav;
    w_nav.run();

    return 0;
}
<|endoftext|>


<commit_before>#ifndef MJOLNIR_POTENTIAL_GLOBAL_DEBYE_HUCKEL_POTENTIAL_HPP
#define MJOLNIR_POTENTIAL_GLOBAL_DEBYE_HUCKEL_POTENTIAL_HPP
#include <mjolnir/core/Unit.hpp>
#include <mjolnir/core/ExclusionList.hpp>
#include <mjolnir/core/System.hpp>
#include <mjolnir/math/constants.hpp>
#include <mjolnir/util/logger.hpp>
#include <vector>
#include <numeric>
#include <cmath>
#include <cassert>

namespace mjolnir
{

// Debye-Huckel type electrostatic interaction.
// This class contains charges and other parameters and calculates energy and
// derivative of the potential function.
// This class is an implementation of the electrostatic term used in the 3SPN
// series of the coarse-grained DNA models (Knotts et al., (2007), Sambriski and
// de Pablo, (2009), Hinckley et al., (2013), and Freeman et al., (2014))
// It is same as the default electrostatic term in CafeMol (Kenzaki et al. 2011)
template<typename traitsT>
class DebyeHuckelPotential
{
  public:
    using traits_type          = traitsT;
    using real_type            = typename traits_type::real_type;
    using system_type          = System<traits_type>;
    using parameter_type       = real_type;
    using container_type       = std::vector<parameter_type>;

    // `pair_parameter_type` is a parameter for a interacting pair.
    // Although it is the same type as `parameter_type` in this potential,
    // it can be different from normal parameter for each particle.
    // This enables NeighborList to cache a value to calculate forces between
    // the particles, e.g. by having qi * qj for pair of particles i and j.
    using pair_parameter_type  = parameter_type;

    // topology stuff
    using topology_type        = Topology;
    using molecule_id_type     = typename topology_type::molecule_id_type;
    using group_id_type        = typename topology_type::group_id_type;
    using connection_kind_type = typename topology_type::connection_kind_type;
    using ignore_molecule_type = IgnoreMolecule<molecule_id_type>;
    using ignore_group_type    = IgnoreGroup   <group_id_type>;
    using exclusion_list_type  = ExclusionList<traits_type>;

    static constexpr real_type default_cutoff() noexcept
    {
        return real_type(5.5);
    }
    static constexpr parameter_type default_parameter() noexcept
    {
        return real_type(0);
    }

  public:

    DebyeHuckelPotential(const real_type cutoff_ratio,
        const std::vector<std::pair<std::size_t, parameter_type>>& parameters,
        const std::map<connection_kind_type, std::size_t>& exclusions,
        ignore_molecule_type ignore_mol, ignore_group_type ignore_grp)
    : cutoff_ratio_(cutoff_ratio), temperature_(300.0), ion_strength_(0.1),
      exclusion_list_(exclusions, std::move(ignore_mol), std::move(ignore_grp))
    {
        this->parameters_  .reserve(parameters.size());
        this->participants_.reserve(parameters.size());
        for(const auto& idxp : parameters)
        {
            const auto idx = idxp.first;
            this->participants_.push_back(idx);
            if(idx >= this->parameters_.size())
            {
                this->parameters_.resize(idx+1, default_parameter());
            }
            this->parameters_.at(idx) = idxp.second;
        }
        // XXX should be updated before use because T and ion conc are default!
        this->calc_parameters();
    }
    ~DebyeHuckelPotential() = default;

    pair_parameter_type prepare_params(std::size_t i, std::size_t j) const noexcept
    {
        return this->inv_4_pi_eps0_epsk_ * this->parameters_[i] * this->parameters_[j];
    }

    real_type potential(const std::size_t i, const std::size_t j,
                        const real_type   r) const noexcept
    {
        return this->potential(r, this->prepare_params(i, j));
    }
    real_type derivative(const std::size_t i, const std::size_t j,
                         const real_type   r) const noexcept
    {
        return this->derivative(r, this->prepare_params(i, j));
    }

    real_type potential(const real_type r, const pair_parameter_type& p) const noexcept
    {
        if(this->max_cutoff_length() <= r) {return 0.0;}
        return p * (std::exp(-r * this->inv_debye_length_) / r - coef_at_cutoff_);
    }
    real_type derivative(const real_type r, const pair_parameter_type& p) const noexcept
    {
        if(this->max_cutoff_length() <= r) {return 0.0;}
        return -p * (debye_length_ + r) * this->inv_debye_length_ / (r * r) *
               std::exp(-r * this->inv_debye_length_);
    }

    real_type cutoff_ratio()   const noexcept {return this->cutoff_ratio_;}
    real_type coef_at_cutoff() const noexcept {return this->coef_at_cutoff_;}

    real_type max_cutoff_length() const noexcept
    {
        return this->debye_length_ * this->cutoff_ratio_;
    }

    void initialize(const system_type& sys, const topology_type& topol) noexcept
    {
        MJOLNIR_GET_DEFAULT_LOGGER();
        MJOLNIR_LOG_FUNCTION();

        if(!sys.has_attribute("temperature"))
        {
            MJOLNIR_LOG_ERROR("DebyeHuckel requires `temperature` attribute");
        }
        if(!sys.has_attribute("ionic_strength"))
        {
            MJOLNIR_LOG_ERROR("DebyeHuckel requires `ionic_strength` attribute");
        }
        this->update(sys, topol); // calc parameters
        return;
    }

    // for temperature/ionic concentration changes...
    void update(const system_type& sys, const topology_type& topol) noexcept
    {
        MJOLNIR_GET_DEFAULT_LOGGER();
        MJOLNIR_LOG_FUNCTION();

        assert(sys.has_attribute("temperature"));
        assert(sys.has_attribute("ionic_strength"));

        this->temperature_  = sys.attribute("temperature");
        this->ion_strength_ = sys.attribute("ionic_strength");

        MJOLNIR_LOG_INFO("temperature    = ", this->temperature_);
        MJOLNIR_LOG_INFO("ionic strength = ", this->ion_strength_);

        this->calc_parameters();

        // update exclusion list based on sys.topology()
        exclusion_list_.make(sys, topol);
        return;
    }

    // -----------------------------------------------------------------------
    // for spatial partitions
    //
    // Here, the default implementation uses Newton's 3rd law to reduce
    // calculation. For an interacting pair (i, j), forces applied to i and j
    // are equal in magnitude and opposite in direction. So, if a pair (i, j) is
    // listed, (j, i) is not needed.
    //     See implementation of VerletList, CellList and GlobalPairInteraction
    // for more details about the usage of these functions.

    std::vector<std::size_t> const& participants() const noexcept {return participants_;}

    range<typename std::vector<std::size_t>::const_iterator>
    leading_participants() const noexcept
    {
        return make_range(participants_.begin(), std::prev(participants_.end()));
    }
    range<typename std::vector<std::size_t>::const_iterator>
    possible_partners_of(const std::size_t participant_idx,
                         const std::size_t /*particle_idx*/) const noexcept
    {
        return make_range(participants_.begin() + participant_idx + 1,
                          participants_.end());
    }
    bool has_interaction(const std::size_t i, const std::size_t j) const noexcept
    {
        return (i < j) && !exclusion_list_.is_excluded(i, j);
    }

    exclusion_list_type const& exclusion_list() const noexcept
    {
        return exclusion_list_; // for testing
    }

    // ------------------------------------------------------------------------
    // used by Observer.
    static const char* name() noexcept {return "DebyeHuckel";}

    // ------------------------------------------------------------------------
    // the following accessers would be used in tests.

    // access to the parameters.
    std::vector<real_type>&       charges()       noexcept {return parameters_;}
    std::vector<real_type> const& charges() const noexcept {return parameters_;}

    real_type debye_length() const noexcept {return this->debye_length_;}

  private:

    void calc_parameters() noexcept
    {
        MJOLNIR_GET_DEFAULT_LOGGER();
        MJOLNIR_LOG_FUNCTION();

        using math_const =    math::constants<real_type>;
        using phys_const = physics::constants<real_type>;

        constexpr real_type pi   = math_const::pi();
        const     real_type kB   = phys_const::kB();
        const     real_type NA   = phys_const::NA();
        const     real_type eps0 = phys_const::eps0();

        const     real_type epsk = calc_dielectric_water(temperature_, ion_strength_);
        const     real_type T    = this->temperature_;

        MJOLNIR_LOG_INFO("kB               = ", kB);
        MJOLNIR_LOG_INFO("T                = ", T);
        MJOLNIR_LOG_INFO("NA               = ", NA);
        MJOLNIR_LOG_INFO("epsilon_0        = ", eps0);
        MJOLNIR_LOG_INFO("epsilon_k        = ", epsk);

        this->inv_4_pi_eps0_epsk_ = 1.0 / (4 * pi * eps0 * epsk);

        // convert [M] (mol/L) or [mM] -> [mol/nm^3] or [mol/A^3]
        const real_type I = ion_strength_ / phys_const::L_to_volume();

        this->debye_length_ = std::sqrt((eps0 * epsk * kB * T) / (2 * NA * I));
        this->inv_debye_length_ = 1. / this->debye_length_;

        MJOLNIR_LOG_INFO("debye length      = ", debye_length_);
        MJOLNIR_LOG_INFO("1 / debye length  = ", inv_debye_length_);
        MJOLNIR_LOG_INFO("1 / 4pi eps0 epsk = ", inv_4_pi_eps0_epsk_);

        this->coef_at_cutoff_ = std::exp(-cutoff_ratio_) /
                                (debye_length_ * cutoff_ratio_);
        return;
    }

    real_type calc_dielectric_water(
            const real_type T, const real_type c) const noexcept
    {
        return (249.4 - 0.788 * T + 7.2e-4 * T * T) *
               (1. - 2.551e-1 * c + 5.151e-2 * c * c - 6.889e-3 * c * c * c);
    }

  private:

    real_type cutoff_ratio_; // relative to the debye length
    real_type coef_at_cutoff_;
    real_type temperature_;  // [K]
    real_type ion_strength_; // [M]
    real_type inv_4_pi_eps0_epsk_;
    real_type debye_length_;
    real_type inv_debye_length_;

    container_type parameters_;
    std::vector<std::size_t> participants_;

    exclusion_list_type  exclusion_list_;
};
} // mjolnir

#ifdef MJOLNIR_SEPARATE_BUILD
#include <mjolnir/core/SimulatorTraits.hpp>
#include <mjolnir/core/BoundaryCondition.hpp>

namespace mjolnir
{
extern template class DebyeHuckelPotential<SimulatorTraits<double, UnlimitedBoundary>       >;
extern template class DebyeHuckelPotential<SimulatorTraits<float,  UnlimitedBoundary>       >;
extern template class DebyeHuckelPotential<SimulatorTraits<double, CuboidalPeriodicBoundary>>;
extern template class DebyeHuckelPotential<SimulatorTraits<float,  CuboidalPeriodicBoundary>>;
} // mjolnir
#endif// MJOLNIR_SEPARATE_BUILD

#endif /* MJOLNIR_DEBYE_HUCKEL_POTENTIAL */
<commit_msg>feat: do not put the default in D-H interaction<commit_after>#ifndef MJOLNIR_POTENTIAL_GLOBAL_DEBYE_HUCKEL_POTENTIAL_HPP
#define MJOLNIR_POTENTIAL_GLOBAL_DEBYE_HUCKEL_POTENTIAL_HPP
#include <mjolnir/core/Unit.hpp>
#include <mjolnir/core/ExclusionList.hpp>
#include <mjolnir/core/System.hpp>
#include <mjolnir/math/constants.hpp>
#include <mjolnir/util/logger.hpp>
#include <vector>
#include <numeric>
#include <cmath>
#include <cassert>

namespace mjolnir
{

// Debye-Huckel type electrostatic interaction.
// This class contains charges and other parameters and calculates energy and
// derivative of the potential function.
// This class is an implementation of the electrostatic term used in the 3SPN
// series of the coarse-grained DNA models (Knotts et al., (2007), Sambriski and
// de Pablo, (2009), Hinckley et al., (2013), and Freeman et al., (2014))
// It is same as the default electrostatic term in CafeMol (Kenzaki et al. 2011)
template<typename traitsT>
class DebyeHuckelPotential
{
  public:
    using traits_type          = traitsT;
    using real_type            = typename traits_type::real_type;
    using system_type          = System<traits_type>;
    using parameter_type       = real_type;
    using container_type       = std::vector<parameter_type>;

    // `pair_parameter_type` is a parameter for a interacting pair.
    // Although it is the same type as `parameter_type` in this potential,
    // it can be different from normal parameter for each particle.
    // This enables NeighborList to cache a value to calculate forces between
    // the particles, e.g. by having qi * qj for pair of particles i and j.
    using pair_parameter_type  = parameter_type;

    // topology stuff
    using topology_type        = Topology;
    using molecule_id_type     = typename topology_type::molecule_id_type;
    using group_id_type        = typename topology_type::group_id_type;
    using connection_kind_type = typename topology_type::connection_kind_type;
    using ignore_molecule_type = IgnoreMolecule<molecule_id_type>;
    using ignore_group_type    = IgnoreGroup   <group_id_type>;
    using exclusion_list_type  = ExclusionList<traits_type>;

    static constexpr real_type default_cutoff() noexcept
    {
        return real_type(5.5);
    }
    static constexpr parameter_type default_parameter() noexcept
    {
        return real_type(0);
    }

  public:

    DebyeHuckelPotential(const real_type cutoff_ratio,
        const std::vector<std::pair<std::size_t, parameter_type>>& parameters,
        const std::map<connection_kind_type, std::size_t>& exclusions,
        ignore_molecule_type ignore_mol, ignore_group_type ignore_grp)
    : cutoff_ratio_(cutoff_ratio),
        // XXX should be updated in the `initialize(sys)` method
      temperature_ (std::numeric_limits<real_type>::quiet_NaN()),
      ion_strength_(std::numeric_limits<real_type>::quiet_NaN()),
      exclusion_list_(exclusions, std::move(ignore_mol), std::move(ignore_grp))
    {
        this->parameters_  .reserve(parameters.size());
        this->participants_.reserve(parameters.size());
        for(const auto& idxp : parameters)
        {
            const auto idx = idxp.first;
            this->participants_.push_back(idx);
            if(idx >= this->parameters_.size())
            {
                this->parameters_.resize(idx+1, default_parameter());
            }
            this->parameters_.at(idx) = idxp.second;
        }
    }
    ~DebyeHuckelPotential() = default;

    pair_parameter_type prepare_params(std::size_t i, std::size_t j) const noexcept
    {
        return this->inv_4_pi_eps0_epsk_ * this->parameters_[i] * this->parameters_[j];
    }

    real_type potential(const std::size_t i, const std::size_t j,
                        const real_type   r) const noexcept
    {
        return this->potential(r, this->prepare_params(i, j));
    }
    real_type derivative(const std::size_t i, const std::size_t j,
                         const real_type   r) const noexcept
    {
        return this->derivative(r, this->prepare_params(i, j));
    }

    real_type potential(const real_type r, const pair_parameter_type& p) const noexcept
    {
        if(this->max_cutoff_length() <= r) {return 0.0;}
        return p * (std::exp(-r * this->inv_debye_length_) / r - coef_at_cutoff_);
    }
    real_type derivative(const real_type r, const pair_parameter_type& p) const noexcept
    {
        if(this->max_cutoff_length() <= r) {return 0.0;}
        return -p * (debye_length_ + r) * this->inv_debye_length_ / (r * r) *
               std::exp(-r * this->inv_debye_length_);
    }

    real_type cutoff_ratio()   const noexcept {return this->cutoff_ratio_;}
    real_type coef_at_cutoff() const noexcept {return this->coef_at_cutoff_;}

    real_type max_cutoff_length() const noexcept
    {
        return this->debye_length_ * this->cutoff_ratio_;
    }

    void initialize(const system_type& sys, const topology_type& topol) noexcept
    {
        MJOLNIR_GET_DEFAULT_LOGGER();
        MJOLNIR_LOG_FUNCTION();

        if(!sys.has_attribute("temperature"))
        {
            MJOLNIR_LOG_ERROR("DebyeHuckel requires `temperature` attribute");
        }
        if(!sys.has_attribute("ionic_strength"))
        {
            MJOLNIR_LOG_ERROR("DebyeHuckel requires `ionic_strength` attribute");
        }
        this->update(sys, topol); // calc parameters
        return;
    }

    // for temperature/ionic concentration changes...
    void update(const system_type& sys, const topology_type& topol) noexcept
    {
        MJOLNIR_GET_DEFAULT_LOGGER();
        MJOLNIR_LOG_FUNCTION();

        assert(sys.has_attribute("temperature"));
        assert(sys.has_attribute("ionic_strength"));

        this->temperature_  = sys.attribute("temperature");
        this->ion_strength_ = sys.attribute("ionic_strength");

        MJOLNIR_LOG_INFO("temperature    = ", this->temperature_);
        MJOLNIR_LOG_INFO("ionic strength = ", this->ion_strength_);

        this->calc_parameters();

        // update exclusion list based on sys.topology()
        exclusion_list_.make(sys, topol);
        return;
    }

    // -----------------------------------------------------------------------
    // for spatial partitions
    //
    // Here, the default implementation uses Newton's 3rd law to reduce
    // calculation. For an interacting pair (i, j), forces applied to i and j
    // are equal in magnitude and opposite in direction. So, if a pair (i, j) is
    // listed, (j, i) is not needed.
    //     See implementation of VerletList, CellList and GlobalPairInteraction
    // for more details about the usage of these functions.

    std::vector<std::size_t> const& participants() const noexcept {return participants_;}

    range<typename std::vector<std::size_t>::const_iterator>
    leading_participants() const noexcept
    {
        return make_range(participants_.begin(), std::prev(participants_.end()));
    }
    range<typename std::vector<std::size_t>::const_iterator>
    possible_partners_of(const std::size_t participant_idx,
                         const std::size_t /*particle_idx*/) const noexcept
    {
        return make_range(participants_.begin() + participant_idx + 1,
                          participants_.end());
    }
    bool has_interaction(const std::size_t i, const std::size_t j) const noexcept
    {
        return (i < j) && !exclusion_list_.is_excluded(i, j);
    }

    exclusion_list_type const& exclusion_list() const noexcept
    {
        return exclusion_list_; // for testing
    }

    // ------------------------------------------------------------------------
    // used by Observer.
    static const char* name() noexcept {return "DebyeHuckel";}

    // ------------------------------------------------------------------------
    // the following accessers would be used in tests.

    // access to the parameters.
    std::vector<real_type>&       charges()       noexcept {return parameters_;}
    std::vector<real_type> const& charges() const noexcept {return parameters_;}

    real_type debye_length() const noexcept {return this->debye_length_;}

  private:

    void calc_parameters() noexcept
    {
        MJOLNIR_GET_DEFAULT_LOGGER();
        MJOLNIR_LOG_FUNCTION();

        using math_const =    math::constants<real_type>;
        using phys_const = physics::constants<real_type>;

        constexpr real_type pi   = math_const::pi();
        const     real_type kB   = phys_const::kB();
        const     real_type NA   = phys_const::NA();
        const     real_type eps0 = phys_const::eps0();

        const     real_type epsk = calc_dielectric_water(temperature_, ion_strength_);
        const     real_type T    = this->temperature_;

        MJOLNIR_LOG_INFO("kB               = ", kB);
        MJOLNIR_LOG_INFO("T                = ", T);
        MJOLNIR_LOG_INFO("NA               = ", NA);
        MJOLNIR_LOG_INFO("epsilon_0        = ", eps0);
        MJOLNIR_LOG_INFO("epsilon_k        = ", epsk);

        this->inv_4_pi_eps0_epsk_ = 1.0 / (4 * pi * eps0 * epsk);

        // convert [M] (mol/L) or [mM] -> [mol/nm^3] or [mol/A^3]
        const real_type I = ion_strength_ / phys_const::L_to_volume();

        this->debye_length_ = std::sqrt((eps0 * epsk * kB * T) / (2 * NA * I));
        this->inv_debye_length_ = 1. / this->debye_length_;

        MJOLNIR_LOG_INFO("debye length      = ", debye_length_);
        MJOLNIR_LOG_INFO("1 / debye length  = ", inv_debye_length_);
        MJOLNIR_LOG_INFO("1 / 4pi eps0 epsk = ", inv_4_pi_eps0_epsk_);

        this->coef_at_cutoff_ = std::exp(-cutoff_ratio_) /
                                (debye_length_ * cutoff_ratio_);
        return;
    }

    real_type calc_dielectric_water(
            const real_type T, const real_type c) const noexcept
    {
        return (249.4 - 0.788 * T + 7.2e-4 * T * T) *
               (1. - 2.551e-1 * c + 5.151e-2 * c * c - 6.889e-3 * c * c * c);
    }

  private:

    real_type cutoff_ratio_; // relative to the debye length
    real_type coef_at_cutoff_;
    real_type temperature_;  // [K]
    real_type ion_strength_; // [M]
    real_type inv_4_pi_eps0_epsk_;
    real_type debye_length_;
    real_type inv_debye_length_;

    container_type parameters_;
    std::vector<std::size_t> participants_;

    exclusion_list_type  exclusion_list_;
};
} // mjolnir

#ifdef MJOLNIR_SEPARATE_BUILD
#include <mjolnir/core/SimulatorTraits.hpp>
#include <mjolnir/core/BoundaryCondition.hpp>

namespace mjolnir
{
extern template class DebyeHuckelPotential<SimulatorTraits<double, UnlimitedBoundary>       >;
extern template class DebyeHuckelPotential<SimulatorTraits<float,  UnlimitedBoundary>       >;
extern template class DebyeHuckelPotential<SimulatorTraits<double, CuboidalPeriodicBoundary>>;
extern template class DebyeHuckelPotential<SimulatorTraits<float,  CuboidalPeriodicBoundary>>;
} // mjolnir
#endif// MJOLNIR_SEPARATE_BUILD

#endif /* MJOLNIR_DEBYE_HUCKEL_POTENTIAL */
<|endoftext|>