<commit_before>/*
   The MIT License

   Copyright (c) 2010 Department Image Processing,
   Fraunhofer ITWM.

   Permission is hereby granted, free of charge, to any person obtaining a
   copy of this software and associated documentation files (the "Software"),
   to deal in the Software without restriction, including without limitation
   the rights to use, copy, modify, merge, publish, distribute, sublicense,
   and/or sell copies of the Software, and to permit persons to whom the
   Software is furnished to do so, subject to the following conditions:

   The above copyright notice and this permission notice shall be included
   in all copies or substantial portions of the Software.

   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
   OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
   THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
   FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
   DEALINGS IN THE SOFTWARE.
*/

/**
  \file    IASSConverter.cpp
  \author  Andre Liebscher
           Department Image Processing
           Fraunhofer ITWM
  \date    March 2010
*/

#include <cctype>
#include <cstring>
#include <fstream>
#include <sstream>
#include "boost/cstdint.hpp"
#include "3rdParty/zlib/zlib.h"
#include "IASSConverter.h"
#include <Basics/EndianConvert.h>
#include <Controller/Controller.h>

using namespace std;
using namespace tuvok;

IASSConverter::IASSConverter()
{
  m_vConverterDesc = "Fraunhofer MAVI Volume";
  m_vSupportedExt.push_back("IASS");
  m_vSupportedExt.push_back("IASS.GZ");
}

bool
IASSConverter::ConvertToRAW(const std::string& strSourceFilename,
                           const std::string& strTempDir,
                           bool, UINT64& iHeaderSkip,
                           UINT64& iComponentSize,
                           UINT64& iComponentCount,
                           bool& bConvertEndianess, bool& bSigned,
                           bool& bIsFloat, UINT64VECTOR3& vVolumeSize,
                           FLOATVECTOR3& vVolumeAspect,
                           std::string& strTitle,
                           UVFTables::ElementSemanticTable& eType,
                           std::string& strIntermediateFile,
                           bool& bDeleteIntermediateFile)
{
  MESSAGE("Attempting to convert IASS dataset %s", strSourceFilename.c_str());

  // Find out machine endianess
  bConvertEndianess = EndianConvert::IsBigEndian();

  // Check whether file is compressed and uncompress if necessary
  string strInputFile;
  if (IsZipped(strSourceFilename)) {
    MESSAGE("IASS data is GZIP compressed.");
    strInputFile = strTempDir + SysTools::GetFilename(strSourceFilename) +
                   ".uncompressed";
    if (!ExtractGZIPDataset(strSourceFilename, strInputFile, 0)) {
      WARNING("Error while decompressing %s", strSourceFilename.c_str());
      return false;
    }
  } else {
    strInputFile = strSourceFilename;
  }

  // Read header and check for "magic" values of the IASS file
  stHeader header;
  ifstream fileData(strInputFile.c_str());

  if (fileData.is_open()) {
    if(!ReadHeader(header,fileData)) {
      WARNING("The file %s is not a IASS file (missing magic)",
              strInputFile.c_str());
      return false;
    }
  } else {
    WARNING("Could not open IASS file %s", strInputFile.c_str());
    return false;
  }
  fileData.close();

  // Init data
  strTitle = "Fraunhofer MAVI Volume";
  eType = UVFTables::ES_UNDEFINED;
  vVolumeAspect = FLOATVECTOR3(1,1,1);
  iComponentCount = 1;
  vVolumeSize[0] = header.size.x;
  vVolumeSize[1] = header.size.y;
  vVolumeSize[2] = header.size.z;
  iComponentSize = header.bpp*8;
  iHeaderSkip = 0;
  bDeleteIntermediateFile = true;

  if (header.type >= MONO && header.type <= GREY_32) {
    bSigned = false;
    bIsFloat = false;
  } else if (header.type == GREY_F) {
    bSigned = true;
    bIsFloat = true;
  } else if (header.type == COLOR) {
    bSigned = false;
    bIsFloat = false;
    iComponentCount = 3;
  } else {
    T_ERROR("Unsupported image type in file %s", strInputFile.c_str());
    return false;
  }

  // Convert raw data to x-locality and decode MONO files
  LargeRAWFile zLocalData(strInputFile, header.skip);
  zLocalData.Open(false);

  if (!zLocalData.IsOpen()) {
    T_ERROR("Unable to open source file %s", strInputFile.c_str());
    return false;
  }

  strIntermediateFile = strTempDir + SysTools::GetFilename(strSourceFilename)
                        + ".x-local";
  LargeRAWFile xLocalData(strIntermediateFile);
  xLocalData.Create();

  if (!xLocalData.IsOpen()) {
    T_ERROR("Unable to open temp file %s for locality conversion",
            strIntermediateFile.c_str());
    xLocalData.Close();
    return false;
  }

  UINT64 strideZ = header.size.x*header.size.y*header.bpp-header.bpp;
  UINT64 sliceSize = header.size.y * header.size.z * header.bpp;
  unsigned char* sliceBuffer = new unsigned char[static_cast<size_t>(sliceSize)];

  if (header.type == MONO) {
    unsigned char* rleBuffer = new unsigned char[static_cast<size_t>(header.rleLength)];
    zLocalData.ReadRAW(rleBuffer,header.rleLength);

    UINT64 posRLEStream = 0;
    UINT64 posOutStream = 0;
    UINT64 currLength = *rleBuffer;
    UINT64 sliceIndex = 0;
    UINT64 currValue = 1;
    do {
      // This loop is entered if another pixel run would exceed the size of
      // the slice buffer
      while (posOutStream + currLength >= sliceSize) {
        // How many pixels are left to fill up buffer
        UINT64 restLength = sliceSize - posOutStream;

        // Set these remaining pixels
        memset(&sliceBuffer[posOutStream],
               static_cast<unsigned char>((currValue%2)*0xff), static_cast<size_t>(restLength));

        for (UINT64 y = 0; y < header.size.y; y++) {
          xLocalData.SeekPos(y*header.size.x+sliceIndex);
          for (UINT64 z = 0; z < header.size.z; z++) {
            xLocalData.WriteRAW(&sliceBuffer[y*header.size.z+z],1);
            xLocalData.SeekPos(xLocalData.GetPos()+strideZ);
          }
        }

        currLength -= restLength;
        posOutStream = 0;
        sliceIndex++;
      }

      // Set pixels of required length
      if (currLength > 0) {
        memset(&sliceBuffer[posOutStream],
               static_cast<unsigned char>((currValue%2)*0xff), static_cast<size_t>(currLength));
        posOutStream += currLength;
      }

      currValue++;
      posRLEStream++;
      currLength = *(rleBuffer + posRLEStream);
    } while (posRLEStream < header.rleLength);

    delete[] rleBuffer;
  } else {
    for (UINT64 x = 0; x < header.size.x; x++) {
      zLocalData.ReadRAW(sliceBuffer,sliceSize);
      for (UINT64 y = 0; y < header.size.y; y++) {
        xLocalData.SeekPos((y*header.size.x+x)*header.bpp);
        for (UINT64 z = 0; z < header.size.z; z++) {
          xLocalData.WriteRAW(&sliceBuffer[(y*header.size.z+z)*header.bpp],
                              header.bpp);
          xLocalData.SeekPos(xLocalData.GetPos()+strideZ);
        }
      }
    }
  }
  delete[] sliceBuffer;

  if ( strInputFile != strSourceFilename )
    Remove(strInputFile, Controller::Debug::Out());

  return true;
}

// unimplemented!
bool
IASSConverter::ConvertToNative(const std::string&,
                               const std::string&,
                               UINT64, UINT64,
                               UINT64, bool,
                               bool,
                               UINT64VECTOR3,
                               FLOATVECTOR3,
                               bool,
                               bool)
{
  return false;
}

bool
IASSConverter::CanRead(const std::string& fn,
                             const std::vector<int8_t>&) const
{
  std::string ext = SysTools::GetExt(fn);
  std::transform(ext.begin(), ext.end(), ext.begin(),
                 (int(*)(int))std::toupper);

  if (ext != "IASS") {
    std::string extPt1 = SysTools::GetExt(SysTools::RemoveExt(fn));
    std::transform(extPt1.begin(), extPt1.end(), extPt1.begin(),
                   (int(*)(int))std::toupper);
    ext = extPt1 + "." + ext;
  }

  return SupportedExtension(ext);
}

bool
IASSConverter::IsZipped(const std::string& strFile)
{
  gzFile file = gzopen(strFile.c_str(), "rb");
  if (file==0)
    return false;

  int rt = gzdirect(file);
  gzclose(file);
  return rt == 1 ? false : true;
}

IASSConverter::stHeader::stHeader()
  : type(INVALID), bpp(0), skip(0), rleLength(0), size(), spacing(),
    creator(""), history("") {}

void
IASSConverter::stHeader::Reset()
{
  type = INVALID;
  bpp = 0;
  skip = 0;
  rleLength = 0;
  size.x = 0;
  size.y = 0;
  size.z = 0;
  spacing.x = 0.0;
  spacing.y = 0.0;
  spacing.z = 0.0;
  creator = "";
  history = "";
}

bool
IASSConverter::ReadHeader(stHeader& header, std::istream& inputStream)
{
  header.Reset();

  if(!inputStream.good())
    return false;

  std::string strLine;

  // read magic
  getline(inputStream, strLine);
  if (!(strLine.substr(0, 9) == "SVstatmat" ||
      strLine.substr(0, 4) == "a4iL") ||
      inputStream.fail()) {
    return false;
  }

  // read header
  do {
    getline(inputStream, strLine);

    if (inputStream.fail())
      return false;

    if (strLine.substr(0, 11) == "# SPACING: ") {
      std::istringstream in(strLine.substr(11, strLine.length()));
      in >> header.spacing.x >> header.spacing.y >> header.spacing.z;
    }
    else if (strLine.substr(0, 11) == "# CREATOR: ")
      header.creator = strLine.substr(11, strLine.length());
    else if (strLine.substr(0, 11) == "# HISTORY: ")
      header.history = strLine.substr(11, strLine.length());
    else if (strLine.substr(0, 8) == "# TYPE: ") {
      if (isdigit(strLine[8])) {
        std::string tmp = strLine.substr(8, strLine.length());
        std::istringstream input_helper(tmp);
        unsigned int type;

        input_helper >> type;
        header.type = static_cast<ePixelType>(type);

        if (header.type < 0 || header.type >= INVALID)
          return false;
      } else {
        if (strLine.substr(8, strLine.length()) == "MONO") {
          header.type = MONO;
        } else if (strLine.substr(8, strLine.length()) == "GREY_8") {
          header.type = GREY_8;
        } else if (strLine.substr(8, strLine.length()) == "GREY_16") {
          header.type = GREY_16;
        } else if (strLine.substr(8, strLine.length()) == "GREY_32") {
          header.type = GREY_32;
        } else if (strLine.substr(8, strLine.length()) == "GREY_F") {
          header.type = GREY_F;
        } else if (strLine.substr(8, strLine.length()) == "COLOR" ||
                   strLine.substr(8, strLine.length()) == "RGB_8") {
          header.type = COLOR;
        } else if (strLine.substr(8, strLine.length()) == "COMPLEX_F") {
          header.type = COMPLEX_F;
        } else {
          return false;
        }
      }
    }
  } while (strLine[0] == '#');

  if (inputStream.fail() || inputStream.eof()) {
    return false;
  } else {
    std::istringstream input_helper(strLine);
    input_helper >> header.size.x >> header.size.y >> header.size.z;
    if (inputStream.fail()) {
      return false;
    }
  }

  if (header.type == MONO) {
    getline(inputStream, strLine);

    if (inputStream.fail() || inputStream.eof()) {
      return false;
    }

    std::istringstream input_helper(strLine);
    input_helper >> header.rleLength;
  }

  header.skip = inputStream.tellg();

  switch(header.type) {
    case(MONO):
      header.bpp = 1;
      break;
    case(GREY_8):
      header.bpp = 1;
      break;
    case(GREY_16):
      header.bpp = 2;
      break;
    case(GREY_32):
      header.bpp = 4;
      break;
    case(GREY_F):
      header.bpp = 4;
      break;
    case(COLOR):
      header.bpp = 3;
      break;
    case(COMPLEX_F):
      header.bpp = 8;
      break;
    case(INVALID):
      return false;
  };

  return true;
}
<commit_msg>Add define to get rid of zlib.<commit_after>/*
   The MIT License

   Copyright (c) 2010 Department Image Processing,
   Fraunhofer ITWM.

   Permission is hereby granted, free of charge, to any person obtaining a
   copy of this software and associated documentation files (the "Software"),
   to deal in the Software without restriction, including without limitation
   the rights to use, copy, modify, merge, publish, distribute, sublicense,
   and/or sell copies of the Software, and to permit persons to whom the
   Software is furnished to do so, subject to the following conditions:

   The above copyright notice and this permission notice shall be included
   in all copies or substantial portions of the Software.

   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
   OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
   THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
   FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
   DEALINGS IN THE SOFTWARE.
*/

/**
  \file    IASSConverter.cpp
  \author  Andre Liebscher
           Department Image Processing
           Fraunhofer ITWM
  \date    March 2010
*/

#include <cctype>
#include <cstring>
#include <fstream>
#include <sstream>
#include "boost/cstdint.hpp"
#ifndef TUVOK_NO_ZLIB
# include "3rdParty/zlib/zlib.h"
#endif
#include "IASSConverter.h"
#include <Basics/EndianConvert.h>
#include <Controller/Controller.h>

using namespace std;
using namespace tuvok;

IASSConverter::IASSConverter()
{
  m_vConverterDesc = "Fraunhofer MAVI Volume";
  m_vSupportedExt.push_back("IASS");
  m_vSupportedExt.push_back("IASS.GZ");
}

bool
IASSConverter::ConvertToRAW(const std::string& strSourceFilename,
                           const std::string& strTempDir,
                           bool, UINT64& iHeaderSkip,
                           UINT64& iComponentSize,
                           UINT64& iComponentCount,
                           bool& bConvertEndianess, bool& bSigned,
                           bool& bIsFloat, UINT64VECTOR3& vVolumeSize,
                           FLOATVECTOR3& vVolumeAspect,
                           std::string& strTitle,
                           UVFTables::ElementSemanticTable& eType,
                           std::string& strIntermediateFile,
                           bool& bDeleteIntermediateFile)
{
  MESSAGE("Attempting to convert IASS dataset %s", strSourceFilename.c_str());

  // Find out machine endianess
  bConvertEndianess = EndianConvert::IsBigEndian();

  // Check whether file is compressed and uncompress if necessary
  string strInputFile;
  if (IsZipped(strSourceFilename)) {
    MESSAGE("IASS data is GZIP compressed.");
    strInputFile = strTempDir + SysTools::GetFilename(strSourceFilename) +
                   ".uncompressed";
    if (!ExtractGZIPDataset(strSourceFilename, strInputFile, 0)) {
      WARNING("Error while decompressing %s", strSourceFilename.c_str());
      return false;
    }
  } else {
    strInputFile = strSourceFilename;
  }

  // Read header and check for "magic" values of the IASS file
  stHeader header;
  ifstream fileData(strInputFile.c_str());

  if (fileData.is_open()) {
    if(!ReadHeader(header,fileData)) {
      WARNING("The file %s is not a IASS file (missing magic)",
              strInputFile.c_str());
      return false;
    }
  } else {
    WARNING("Could not open IASS file %s", strInputFile.c_str());
    return false;
  }
  fileData.close();

  // Init data
  strTitle = "Fraunhofer MAVI Volume";
  eType = UVFTables::ES_UNDEFINED;
  vVolumeAspect = FLOATVECTOR3(1,1,1);
  iComponentCount = 1;
  vVolumeSize[0] = header.size.x;
  vVolumeSize[1] = header.size.y;
  vVolumeSize[2] = header.size.z;
  iComponentSize = header.bpp*8;
  iHeaderSkip = 0;
  bDeleteIntermediateFile = true;

  if (header.type >= MONO && header.type <= GREY_32) {
    bSigned = false;
    bIsFloat = false;
  } else if (header.type == GREY_F) {
    bSigned = true;
    bIsFloat = true;
  } else if (header.type == COLOR) {
    bSigned = false;
    bIsFloat = false;
    iComponentCount = 3;
  } else {
    T_ERROR("Unsupported image type in file %s", strInputFile.c_str());
    return false;
  }

  // Convert raw data to x-locality and decode MONO files
  LargeRAWFile zLocalData(strInputFile, header.skip);
  zLocalData.Open(false);

  if (!zLocalData.IsOpen()) {
    T_ERROR("Unable to open source file %s", strInputFile.c_str());
    return false;
  }

  strIntermediateFile = strTempDir + SysTools::GetFilename(strSourceFilename)
                        + ".x-local";
  LargeRAWFile xLocalData(strIntermediateFile);
  xLocalData.Create();

  if (!xLocalData.IsOpen()) {
    T_ERROR("Unable to open temp file %s for locality conversion",
            strIntermediateFile.c_str());
    xLocalData.Close();
    return false;
  }

  UINT64 strideZ = header.size.x*header.size.y*header.bpp-header.bpp;
  UINT64 sliceSize = header.size.y * header.size.z * header.bpp;
  unsigned char* sliceBuffer = new unsigned char[static_cast<size_t>(sliceSize)];

  if (header.type == MONO) {
    unsigned char* rleBuffer = new unsigned char[static_cast<size_t>(header.rleLength)];
    zLocalData.ReadRAW(rleBuffer,header.rleLength);

    UINT64 posRLEStream = 0;
    UINT64 posOutStream = 0;
    UINT64 currLength = *rleBuffer;
    UINT64 sliceIndex = 0;
    UINT64 currValue = 1;
    do {
      // This loop is entered if another pixel run would exceed the size of
      // the slice buffer
      while (posOutStream + currLength >= sliceSize) {
        // How many pixels are left to fill up buffer
        UINT64 restLength = sliceSize - posOutStream;

        // Set these remaining pixels
        memset(&sliceBuffer[posOutStream],
               static_cast<unsigned char>((currValue%2)*0xff), static_cast<size_t>(restLength));

        for (UINT64 y = 0; y < header.size.y; y++) {
          xLocalData.SeekPos(y*header.size.x+sliceIndex);
          for (UINT64 z = 0; z < header.size.z; z++) {
            xLocalData.WriteRAW(&sliceBuffer[y*header.size.z+z],1);
            xLocalData.SeekPos(xLocalData.GetPos()+strideZ);
          }
        }

        currLength -= restLength;
        posOutStream = 0;
        sliceIndex++;
      }

      // Set pixels of required length
      if (currLength > 0) {
        memset(&sliceBuffer[posOutStream],
               static_cast<unsigned char>((currValue%2)*0xff), static_cast<size_t>(currLength));
        posOutStream += currLength;
      }

      currValue++;
      posRLEStream++;
      currLength = *(rleBuffer + posRLEStream);
    } while (posRLEStream < header.rleLength);

    delete[] rleBuffer;
  } else {
    for (UINT64 x = 0; x < header.size.x; x++) {
      zLocalData.ReadRAW(sliceBuffer,sliceSize);
      for (UINT64 y = 0; y < header.size.y; y++) {
        xLocalData.SeekPos((y*header.size.x+x)*header.bpp);
        for (UINT64 z = 0; z < header.size.z; z++) {
          xLocalData.WriteRAW(&sliceBuffer[(y*header.size.z+z)*header.bpp],
                              header.bpp);
          xLocalData.SeekPos(xLocalData.GetPos()+strideZ);
        }
      }
    }
  }
  delete[] sliceBuffer;

  if ( strInputFile != strSourceFilename )
    Remove(strInputFile, Controller::Debug::Out());

  return true;
}

// unimplemented!
bool
IASSConverter::ConvertToNative(const std::string&,
                               const std::string&,
                               UINT64, UINT64,
                               UINT64, bool,
                               bool,
                               UINT64VECTOR3,
                               FLOATVECTOR3,
                               bool,
                               bool)
{
  return false;
}

bool
IASSConverter::CanRead(const std::string& fn,
                             const std::vector<int8_t>&) const
{
  std::string ext = SysTools::GetExt(fn);
  std::transform(ext.begin(), ext.end(), ext.begin(),
                 (int(*)(int))std::toupper);

  if (ext != "IASS") {
    std::string extPt1 = SysTools::GetExt(SysTools::RemoveExt(fn));
    std::transform(extPt1.begin(), extPt1.end(), extPt1.begin(),
                   (int(*)(int))std::toupper);
    ext = extPt1 + "." + ext;
  }

  return SupportedExtension(ext);
}

bool
IASSConverter::IsZipped(const std::string& strFile)
{
#ifdef TUVOK_NO_ZLIB
  T_ERROR("No zlib support!  Faking this...");
  return false;
#else
  gzFile file = gzopen(strFile.c_str(), "rb");
  if (file==0)
    return false;

  int rt = gzdirect(file);
  gzclose(file);
  return rt == 1 ? false : true;
#endif
}

IASSConverter::stHeader::stHeader()
  : type(INVALID), bpp(0), skip(0), rleLength(0), size(), spacing(),
    creator(""), history("") {}

void
IASSConverter::stHeader::Reset()
{
  type = INVALID;
  bpp = 0;
  skip = 0;
  rleLength = 0;
  size.x = 0;
  size.y = 0;
  size.z = 0;
  spacing.x = 0.0;
  spacing.y = 0.0;
  spacing.z = 0.0;
  creator = "";
  history = "";
}

bool
IASSConverter::ReadHeader(stHeader& header, std::istream& inputStream)
{
  header.Reset();

  if(!inputStream.good())
    return false;

  std::string strLine;

  // read magic
  getline(inputStream, strLine);
  if (!(strLine.substr(0, 9) == "SVstatmat" ||
      strLine.substr(0, 4) == "a4iL") ||
      inputStream.fail()) {
    return false;
  }

  // read header
  do {
    getline(inputStream, strLine);

    if (inputStream.fail())
      return false;

    if (strLine.substr(0, 11) == "# SPACING: ") {
      std::istringstream in(strLine.substr(11, strLine.length()));
      in >> header.spacing.x >> header.spacing.y >> header.spacing.z;
    }
    else if (strLine.substr(0, 11) == "# CREATOR: ")
      header.creator = strLine.substr(11, strLine.length());
    else if (strLine.substr(0, 11) == "# HISTORY: ")
      header.history = strLine.substr(11, strLine.length());
    else if (strLine.substr(0, 8) == "# TYPE: ") {
      if (isdigit(strLine[8])) {
        std::string tmp = strLine.substr(8, strLine.length());
        std::istringstream input_helper(tmp);
        unsigned int type;

        input_helper >> type;
        header.type = static_cast<ePixelType>(type);

        if (header.type < 0 || header.type >= INVALID)
          return false;
      } else {
        if (strLine.substr(8, strLine.length()) == "MONO") {
          header.type = MONO;
        } else if (strLine.substr(8, strLine.length()) == "GREY_8") {
          header.type = GREY_8;
        } else if (strLine.substr(8, strLine.length()) == "GREY_16") {
          header.type = GREY_16;
        } else if (strLine.substr(8, strLine.length()) == "GREY_32") {
          header.type = GREY_32;
        } else if (strLine.substr(8, strLine.length()) == "GREY_F") {
          header.type = GREY_F;
        } else if (strLine.substr(8, strLine.length()) == "COLOR" ||
                   strLine.substr(8, strLine.length()) == "RGB_8") {
          header.type = COLOR;
        } else if (strLine.substr(8, strLine.length()) == "COMPLEX_F") {
          header.type = COMPLEX_F;
        } else {
          return false;
        }
      }
    }
  } while (strLine[0] == '#');

  if (inputStream.fail() || inputStream.eof()) {
    return false;
  } else {
    std::istringstream input_helper(strLine);
    input_helper >> header.size.x >> header.size.y >> header.size.z;
    if (inputStream.fail()) {
      return false;
    }
  }

  if (header.type == MONO) {
    getline(inputStream, strLine);

    if (inputStream.fail() || inputStream.eof()) {
      return false;
    }

    std::istringstream input_helper(strLine);
    input_helper >> header.rleLength;
  }

  header.skip = inputStream.tellg();

  switch(header.type) {
    case(MONO):
      header.bpp = 1;
      break;
    case(GREY_8):
      header.bpp = 1;
      break;
    case(GREY_16):
      header.bpp = 2;
      break;
    case(GREY_32):
      header.bpp = 4;
      break;
    case(GREY_F):
      header.bpp = 4;
      break;
    case(COLOR):
      header.bpp = 3;
      break;
    case(COMPLEX_F):
      header.bpp = 8;
      break;
    case(INVALID):
      return false;
  };

  return true;
}
<|endoftext|>


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

/* Author: Wim Meeussen */

#include <boost/algorithm/string.hpp>
#include <vector>
#include "urdf_parser.h"

const bool debug = true;

namespace urdf{

bool parseMaterial(Material &material, TiXmlElement *config);
bool parseLink(Link &link, TiXmlElement *config);
bool parseJoint(Joint &joint, TiXmlElement *config);

// Added by achq on 2012/10/13 *******************//

  /**
    * @function isObjectURDF
    */
  bool isObjectURDF( const std::string &_xml_string ) {
  	TiXmlDocument xml_doc;
 	 	xml_doc.Parse( _xml_string.c_str() );

  	TiXmlElement *test_xml = xml_doc.FirstChildElement("object");
  	if ( !test_xml ) { return false; }
  	else { return true; }
	}

  /**
    * @function isRobotURDF
    */
  bool isRobotURDF( const std::string &_xml_string ) {
  	TiXmlDocument xml_doc;
 	 	xml_doc.Parse( _xml_string.c_str() );

  	TiXmlElement *test_xml = xml_doc.FirstChildElement("robot");
  	if ( !test_xml ) { return false; }
  	else { return true; }
  }

// *************************************************//

boost::shared_ptr<ModelInterface>  parseURDF(const std::string &xml_string)
{

  boost::shared_ptr<ModelInterface> model(new ModelInterface);
  model->clear();

  TiXmlDocument xml_doc;
  xml_doc.LoadFile(xml_string.c_str());
  xml_doc.Parse(xml_string.c_str());

  TiXmlElement *robot_xml = xml_doc.FirstChildElement("robot");
  if (!robot_xml)
  {
    // Added the object if by achq for DART - GRIP (2012/10/13
    // all the rest is the same as original
    robot_xml = xml_doc.FirstChildElement("object");
    if( !robot_xml ) {
      if(debug) printf ( "Could find neither a robot nor an object element in the xml file \n" );
      model.reset();
      return model;
    }
    else {
      if(debug) printf ("Found an object file in the xml file!  \n");
    }
  }
  else {
    if(debug) printf (" Found a robot object in urdf file! \n");
  }

  // Get robot name
  const char *name = robot_xml->Attribute("name");
  if (!name)
  {
    if(debug) printf ("No name given for the robot. \n");
    model.reset();
    return model;
  }
  model->name_ = std::string(name);

  // Get all Material elements
  for (TiXmlElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material"))
  {
    boost::shared_ptr<Material> material;
    material.reset(new Material);

    try {
      parseMaterial(*material, material_xml);
      if (model->getMaterial(material->name))
      {
        if(debug) printf ("material '%s' is not unique. \n", material->name.c_str());
        material.reset();
        model.reset();
        return model;
      }
      else
      {
        model->materials_.insert(make_pair(material->name,material));
        if(debug) printf ("successfully added a new material '%s' \n", material->name.c_str());
      }
    }
    catch (ParseError &e) {
      if(debug) printf ("material xml is not initialized correctly \n");
      material.reset();
      model.reset();
      return model;
    }
  }

  // Get all Link elements
  for (TiXmlElement* link_xml = robot_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link"))
  {
    boost::shared_ptr<Link> link;
    link.reset(new Link);

    try {
      parseLink(*link, link_xml);
      if (model->getLink(link->name))
      {
        if(debug) printf ("link '%s' is not unique. \n", link->name.c_str());
        model.reset();
        return model;
      }
      else
      {
        // set link visual material
        if(debug) printf ("setting link '%s' material \n", link->name.c_str());
        if (link->visual)
        {
          if (!link->visual->material_name.empty())
          {
            if (model->getMaterial(link->visual->material_name))
            {
              if(debug) printf ("setting link '%s' material to '%s' \n", link->name.c_str(),link->visual->material_name.c_str());
              link->visual->material = model->getMaterial( link->visual->material_name.c_str() );
            }
            else
            {
              if (link->visual->material)
              {
                if(debug) printf ("link '%s' material '%s' defined in Visual. \n", link->name.c_str(),link->visual->material_name.c_str());
                model->materials_.insert(make_pair(link->visual->material->name,link->visual->material));
              }
              else
              {
                if(debug) printf ("link '%s' material '%s' undefined. \n", link->name.c_str(),link->visual->material_name.c_str());
                model.reset();
                return model;
              }
            }
          }
        }

        model->links_.insert(make_pair(link->name,link));
        if(debug) printf ("successfully added a new link '%s' \n", link->name.c_str());
      }
    }
    catch (ParseError &e) {
      if(debug) printf ("link xml is not initialized correctly \n");
      model.reset();
      return model;
    }
  }
  if (model->links_.empty()){
    if(debug) printf ("No link elements found in urdf file \n");
    model.reset();
    return model;
  }

  // Get all Joint elements
  for (TiXmlElement* joint_xml = robot_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint"))
  {
    boost::shared_ptr<Joint> joint;
    joint.reset(new Joint);

    if (parseJoint(*joint, joint_xml))
    {
      if (model->getJoint(joint->name))
      {
        if(debug) printf ("joint '%s' is not unique. \n", joint->name.c_str());
        model.reset();
        return model;
      }
      else
      {
        model->joints_.insert(make_pair(joint->name,joint));
        if(debug) printf ("successfully added a new joint '%s' \n", joint->name.c_str());
      }
    }
    else
    {
      if(debug) printf ("joint xml is not initialized correctly \n");
      model.reset();
      return model;
    }
  }

  // every link has children links and joints, but no parents, so we create a
  // local convenience data structure for keeping child->parent relations
  std::map<std::string, std::string> parent_link_tree;
  parent_link_tree.clear();

  // building tree: name mapping
  try
  {
    model->initTree(parent_link_tree);
  }
  catch(ParseError &e)
  {
    if(debug) printf ("Failed to build tree: %s \n", e.what());
    model.reset();
    return model;
  }

  // find the root link
  try
  {
    model->initRoot(parent_link_tree);
  }
  catch(ParseError &e)
  {
    if(debug) printf ("Failed to find root link: %s \n", e.what());
    model.reset();
    return model;
  }

  return model;
}

bool exportMaterial(Material &material, TiXmlElement *config);
bool exportLink(Link &link, TiXmlElement *config);
bool exportJoint(Joint &joint, TiXmlElement *config);
TiXmlDocument*  exportURDF(boost::shared_ptr<ModelInterface> &model)
{
  TiXmlDocument *doc = new TiXmlDocument();

  TiXmlElement *robot = new TiXmlElement("robot");
  robot->SetAttribute("name", model->name_);
  doc->LinkEndChild(robot);

  for (std::map<std::string, boost::shared_ptr<Link> >::const_iterator l=model->links_.begin(); l!=model->links_.end(); l++)
    exportLink(*(l->second), robot);

  for (std::map<std::string, boost::shared_ptr<Joint> >::const_iterator j=model->joints_.begin(); j!=model->joints_.end(); j++)
  {
    if(debug) printf ("exporting joint [%s]\n",j->second->name.c_str());
    exportJoint(*(j->second), robot);
  }

  return doc;
}

}

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

/* Author: Wim Meeussen */

#include <boost/algorithm/string.hpp>
#include <vector>
#include "urdf_parser.h"

const bool debug = true;

namespace urdf{

bool parseMaterial(Material &material, TiXmlElement *config);
bool parseLink(Link &link, TiXmlElement *config);
bool parseJoint(Joint &joint, TiXmlElement *config);

// Added by achq on 2012/10/13 *******************//

  /**
    * @function isObjectURDF
    */
  bool isObjectURDF( const std::string &_xml_string ) {
  	TiXmlDocument xml_doc;
 	 	xml_doc.Parse( _xml_string.c_str() );

  	TiXmlElement *test_xml = xml_doc.FirstChildElement("object");
  	if ( !test_xml ) { return false; }
  	else { return true; }
	}

  /**
    * @function isRobotURDF
    */
  bool isRobotURDF( const std::string &_xml_string ) {
  	TiXmlDocument xml_doc;
 	 	xml_doc.Parse( _xml_string.c_str() );

  	TiXmlElement *test_xml = xml_doc.FirstChildElement("robot");
  	if ( !test_xml ) { return false; }
  	else { return true; }
  }

// *************************************************//

boost::shared_ptr<ModelInterface>  parseURDF(const std::string &xml_string)
{

  boost::shared_ptr<ModelInterface> model(new ModelInterface);
  model->clear();

  TiXmlDocument xml_doc;
  xml_doc.LoadFile(xml_string.c_str());
  //xml_doc.Parse(xml_string.c_str());

  TiXmlElement *robot_xml = xml_doc.FirstChildElement("robot");
  if (!robot_xml)
  {
    // Added the object if by achq for DART - GRIP (2012/10/13
    // all the rest is the same as original
    robot_xml = xml_doc.FirstChildElement("object");
    if( !robot_xml ) {
      if(debug) printf ( "Could find neither a robot nor an object element in the xml file \n" );
      model.reset();
      return model;
    }
    else {
      if(debug) printf ("Found an object file in the xml file!  \n");
    }
  }
  else {
    if(debug) printf (" Found a robot object in urdf file! \n");
  }

  // Get robot name
  const char *name = robot_xml->Attribute("name");
  if (!name)
  {
    if(debug) printf ("No name given for the robot. \n");
    model.reset();
    return model;
  }
  model->name_ = std::string(name);

  // Get all Material elements
  for (TiXmlElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material"))
  {
    boost::shared_ptr<Material> material;
    material.reset(new Material);

    try {
      parseMaterial(*material, material_xml);
      if (model->getMaterial(material->name))
      {
        if(debug) printf ("material '%s' is not unique. \n", material->name.c_str());
        material.reset();
        model.reset();
        return model;
      }
      else
      {
        model->materials_.insert(make_pair(material->name,material));
        if(debug) printf ("successfully added a new material '%s' \n", material->name.c_str());
      }
    }
    catch (ParseError &e) {
      if(debug) printf ("material xml is not initialized correctly \n");
      material.reset();
      model.reset();
      return model;
    }
  }

  // Get all Link elements
  for (TiXmlElement* link_xml = robot_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link"))
  {
    boost::shared_ptr<Link> link;
    link.reset(new Link);

    try {
      parseLink(*link, link_xml);
      if (model->getLink(link->name))
      {
        if(debug) printf ("link '%s' is not unique. \n", link->name.c_str());
        model.reset();
        return model;
      }
      else
      {
        // set link visual material
        if(debug) printf ("setting link '%s' material \n", link->name.c_str());
        if (link->visual)
        {
          if (!link->visual->material_name.empty())
          {
            if (model->getMaterial(link->visual->material_name))
            {
              if(debug) printf ("setting link '%s' material to '%s' \n", link->name.c_str(),link->visual->material_name.c_str());
              link->visual->material = model->getMaterial( link->visual->material_name.c_str() );
            }
            else
            {
              if (link->visual->material)
              {
                if(debug) printf ("link '%s' material '%s' defined in Visual. \n", link->name.c_str(),link->visual->material_name.c_str());
                model->materials_.insert(make_pair(link->visual->material->name,link->visual->material));
              }
              else
              {
                if(debug) printf ("link '%s' material '%s' undefined. \n", link->name.c_str(),link->visual->material_name.c_str());
                model.reset();
                return model;
              }
            }
          }
        }

        model->links_.insert(make_pair(link->name,link));
        if(debug) printf ("successfully added a new link '%s' \n", link->name.c_str());
      }
    }
    catch (ParseError &e) {
      if(debug) printf ("link xml is not initialized correctly \n");
      model.reset();
      return model;
    }
  }
  if (model->links_.empty()){
    if(debug) printf ("No link elements found in urdf file \n");
    model.reset();
    return model;
  }

  // Get all Joint elements
  for (TiXmlElement* joint_xml = robot_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint"))
  {
    boost::shared_ptr<Joint> joint;
    joint.reset(new Joint);

    if (parseJoint(*joint, joint_xml))
    {
      if (model->getJoint(joint->name))
      {
        if(debug) printf ("joint '%s' is not unique. \n", joint->name.c_str());
        model.reset();
        return model;
      }
      else
      {
        model->joints_.insert(make_pair(joint->name,joint));
        if(debug) printf ("successfully added a new joint '%s' \n", joint->name.c_str());
      }
    }
    else
    {
      if(debug) printf ("joint xml is not initialized correctly \n");
      model.reset();
      return model;
    }
  }

  // every link has children links and joints, but no parents, so we create a
  // local convenience data structure for keeping child->parent relations
  std::map<std::string, std::string> parent_link_tree;
  parent_link_tree.clear();

  // building tree: name mapping
  try
  {
    model->initTree(parent_link_tree);
  }
  catch(ParseError &e)
  {
    if(debug) printf ("Failed to build tree: %s \n", e.what());
    model.reset();
    return model;
  }

  // find the root link
  try
  {
    model->initRoot(parent_link_tree);
  }
  catch(ParseError &e)
  {
    if(debug) printf ("Failed to find root link: %s \n", e.what());
    model.reset();
    return model;
  }

  return model;
}

bool exportMaterial(Material &material, TiXmlElement *config);
bool exportLink(Link &link, TiXmlElement *config);
bool exportJoint(Joint &joint, TiXmlElement *config);
TiXmlDocument*  exportURDF(boost::shared_ptr<ModelInterface> &model)
{
  TiXmlDocument *doc = new TiXmlDocument();

  TiXmlElement *robot = new TiXmlElement("robot");
  robot->SetAttribute("name", model->name_);
  doc->LinkEndChild(robot);

  for (std::map<std::string, boost::shared_ptr<Link> >::const_iterator l=model->links_.begin(); l!=model->links_.end(); l++)
    exportLink(*(l->second), robot);

  for (std::map<std::string, boost::shared_ptr<Joint> >::const_iterator j=model->joints_.begin(); j!=model->joints_.end(); j++)
  {
    if(debug) printf ("exporting joint [%s]\n",j->second->name.c_str());
    exportJoint(*(j->second), robot);
  }

  return doc;
}

}

<|endoftext|>


<commit_before>//
// This file is part of the Marble Virtual Globe.
//
// This program is free software licensed under the GNU LGPL. You can
// find a copy of this license in LICENSE.txt in the top directory of
// the source code.
//
// Copyright 2009      Bastian Holst <bastianholst@gmx.de>
//

// Self
#include "AbstractDataPlugin.h"

// Marble
#include "AbstractDataPluginModel.h"
#include "AbstractDataPluginItem.h"
#include "GeoPainter.h"
#include "GeoSceneLayer.h"
#include "MarbleModel.h"
#include "ViewportParams.h"
#include "MarbleDebug.h"

// Qt
#include <QtCore/QEvent>
#include <QtGui/QMouseEvent>
#include <QtGui/QRegion>
#include <QtDeclarative/QDeclarativeComponent>
#include <QtDeclarative/QDeclarativeItem>
#include <QtDeclarative/QDeclarativeContext>

namespace Marble
{

class AbstractDataPluginPrivate
{
 public:
    AbstractDataPluginPrivate()
        : m_model( 0 ),
          m_numberOfItems( 10 ),
          m_delegate( 0 ),
          m_delegateParent( 0 )
    {
    }

    ~AbstractDataPluginPrivate() {
        delete m_model;
    }

    AbstractDataPluginModel *m_model;
    quint32 m_numberOfItems;
    QDeclarativeComponent* m_delegate;
    QGraphicsItem* m_delegateParent;
    QMap<AbstractDataPluginItem*,QDeclarativeItem*> m_delegateInstances;
};

AbstractDataPlugin::AbstractDataPlugin( const MarbleModel *marbleModel )
    : RenderPlugin( marbleModel ),
      d( new AbstractDataPluginPrivate )
{
}

AbstractDataPlugin::~AbstractDataPlugin()
{
    delete d;
}

bool AbstractDataPlugin::isInitialized() const
{
    return model() != 0;
}

QStringList AbstractDataPlugin::backendTypes() const
{
    return QStringList( name() );
}

QString AbstractDataPlugin::renderPolicy() const
{
    return QString( "ALWAYS" );
}

QStringList AbstractDataPlugin::renderPosition() const
{
    return QStringList( "ALWAYS_ON_TOP" );
}

bool AbstractDataPlugin::render( GeoPainter *painter, ViewportParams *viewport,
             const QString& renderPos, GeoSceneLayer * layer)
{
    Q_UNUSED( renderPos );
    Q_UNUSED( layer );

    if ( !d->m_model || !isInitialized() ) {
        return true;
    }

    if ( d->m_delegate ) {
        handleViewportChange( viewport );
    } else {
        QList<AbstractDataPluginItem*> items = d->m_model->items( viewport,
                                                                  marbleModel(),
                                                                  numberOfItems() );
        painter->save();

        // Paint the most important item at last
        for( int i = items.size() - 1; i >= 0; --i ) {
            items.at( i )->paintEvent( painter, viewport );
        }

        painter->restore();
    }

    return true;
}

AbstractDataPluginModel *AbstractDataPlugin::model() const
{
    return d->m_model;
}

void AbstractDataPlugin::setModel( AbstractDataPluginModel* model )
{
    if ( d->m_model ) {
        disconnect( d->m_model, SIGNAL( itemsUpdated() ), this, SIGNAL( repaintNeeded() ) );
        delete d->m_model;
    }
    d->m_model = model;

    connect( d->m_model, SIGNAL( itemsUpdated() ), this, SIGNAL( repaintNeeded() ) );
    connect( d->m_model, SIGNAL( favoriteItemsChanged( const QStringList& ) ), this,
             SLOT( favoriteItemsChanged( const QStringList& ) ) );
    connect( d->m_model, SIGNAL( favoriteItemsOnlyChanged() ), this,
                         SIGNAL( favoriteItemsOnlyChanged() ) );

    emit favoritesModelChanged();
}

const PluginManager* AbstractDataPlugin::pluginManager() const
{
    return marbleModel()->pluginManager();
}

quint32 AbstractDataPlugin::numberOfItems() const
{
    return d->m_numberOfItems;
}

void AbstractDataPlugin::setNumberOfItems( quint32 number )
{
    bool changed = ( number != d->m_numberOfItems );
    d->m_numberOfItems = number;

    if ( changed )
        emit changedNumberOfItems( number );
}

QList<AbstractDataPluginItem *> AbstractDataPlugin::whichItemAt( const QPoint& curpos )
{
    if ( d->m_model && enabled() && visible()) {
        return d->m_model->whichItemAt( curpos );
    }
    else {
        return QList<AbstractDataPluginItem *>();
    }
}

RenderPlugin::RenderType AbstractDataPlugin::renderType() const
{
    return Online;
}

void AbstractDataPlugin::setDelegate( QDeclarativeComponent *delegate, QGraphicsItem* parent )
{
    qDeleteAll( d->m_delegateInstances.values() );
    d->m_delegateInstances.clear();

    d->m_delegate = delegate;
    d->m_delegateParent = parent;
}

void AbstractDataPlugin::setFavoriteItemsOnly( bool favoriteOnly )
{
    if ( d->m_model && d->m_model->isFavoriteItemsOnly() != favoriteOnly ) {
        d->m_model->setFavoriteItemsOnly( favoriteOnly );
    }
}

bool AbstractDataPlugin::isFavoriteItemsOnly() const
{
    return d->m_model->isFavoriteItemsOnly();
}

QObject *AbstractDataPlugin::favoritesModel()
{
    return d->m_model ? d->m_model->favoritesModel() : 0;
}

void AbstractDataPlugin::handleViewportChange( ViewportParams* viewport )
{
    QList<AbstractDataPluginItem*> orphane = d->m_delegateInstances.keys();
    QList<AbstractDataPluginItem*> const items = d->m_model->items( viewport, marbleModel(), numberOfItems() );
    foreach( AbstractDataPluginItem* item, items ) {
        qreal x, y;
        Marble::GeoDataCoordinates const coordinates = item->coordinate();
        bool const visible = viewport->screenCoordinates( coordinates.longitude(), coordinates.latitude(), x, y );

        if ( !d->m_delegateInstances.contains( item ) ) {
            if ( !visible ) {
                // We don't have, but don't need it either. Shouldn't happen though as the model checks for it already.
                continue;
            }

            // Create a new QML object instance using the delegate as the factory. The original
            // data plugin item is set as the context object, i.e. all its properties are available
            // to QML directly with their names
            QDeclarativeContext *context = new QDeclarativeContext( qmlContext( d->m_delegate ) );
            context->setContextObject( item );
            QObject* component = d->m_delegate->create( context );
            QDeclarativeItem* newItem = qobject_cast<QDeclarativeItem*>( component );
            QGraphicsItem* graphicsItem = qobject_cast<QGraphicsItem*>( component );
            if ( graphicsItem && newItem ) {
                graphicsItem->setParentItem( d->m_delegateParent );
            }

            if ( newItem ) {
                d->m_delegateInstances[item] = newItem;
            } else {
                mDebug() << "Failed to create delegate";
                continue;
            }
        } else if ( !visible ) {
            // Previously visible but not anymore => needs to be deleted. Orphane list takes care of it later.
            // Shouldn't happen though as the model checks for it already.
            continue;
        }

        Q_ASSERT( visible );
        QDeclarativeItem* declarativeItem = d->m_delegateInstances[item];
        Q_ASSERT( declarativeItem );

        // Make sure we have a valid bounding rect for collision detection
        item->setProjection( viewport );
        item->setSize( QSizeF( declarativeItem->boundingRect().size() ) );

        int shiftX( 0 ), shiftY( 0 );
        switch( declarativeItem->transformOrigin() ) {
        case QDeclarativeItem::TopLeft:
        case QDeclarativeItem::Top:
        case QDeclarativeItem::TopRight:
            break;
        case QDeclarativeItem::Left:
        case QDeclarativeItem::Center:
        case QDeclarativeItem::Right:
            shiftY = declarativeItem->height() / 2;
            break;
        case QDeclarativeItem::BottomLeft:
        case QDeclarativeItem::Bottom:
        case QDeclarativeItem::BottomRight:
            shiftY = declarativeItem->height();
            break;
        }

        switch( declarativeItem->transformOrigin() ) {
        case QDeclarativeItem::TopLeft:
        case QDeclarativeItem::Left:
        case QDeclarativeItem::BottomLeft:
            break;
        case QDeclarativeItem::Top:
        case QDeclarativeItem::Center:
        case QDeclarativeItem::Bottom:
            shiftX = declarativeItem->width() / 2;
            break;
        case QDeclarativeItem::TopRight:
        case QDeclarativeItem::Right:
        case QDeclarativeItem::BottomRight:
            shiftX = declarativeItem->width();
            break;
        }

        declarativeItem->setPos( x - shiftX, y - shiftY );
        orphane.removeOne( item );
    }

    // Cleanup
    foreach( AbstractDataPluginItem* item, orphane ) {
        Q_ASSERT( d->m_delegateInstances.contains( item ) );
        delete d->m_delegateInstances[item];
        d->m_delegateInstances.remove( item );
    }
}

void AbstractDataPlugin::favoriteItemsChanged( const QStringList& favoriteItems )
{
    Q_UNUSED( favoriteItems )
}

} // namespace Marble

#include "AbstractDataPlugin.moc"
<commit_msg>m_model can be 0.<commit_after>//
// This file is part of the Marble Virtual Globe.
//
// This program is free software licensed under the GNU LGPL. You can
// find a copy of this license in LICENSE.txt in the top directory of
// the source code.
//
// Copyright 2009      Bastian Holst <bastianholst@gmx.de>
//

// Self
#include "AbstractDataPlugin.h"

// Marble
#include "AbstractDataPluginModel.h"
#include "AbstractDataPluginItem.h"
#include "GeoPainter.h"
#include "GeoSceneLayer.h"
#include "MarbleModel.h"
#include "ViewportParams.h"
#include "MarbleDebug.h"

// Qt
#include <QtCore/QEvent>
#include <QtGui/QMouseEvent>
#include <QtGui/QRegion>
#include <QtDeclarative/QDeclarativeComponent>
#include <QtDeclarative/QDeclarativeItem>
#include <QtDeclarative/QDeclarativeContext>

namespace Marble
{

class AbstractDataPluginPrivate
{
 public:
    AbstractDataPluginPrivate()
        : m_model( 0 ),
          m_numberOfItems( 10 ),
          m_delegate( 0 ),
          m_delegateParent( 0 )
    {
    }

    ~AbstractDataPluginPrivate() {
        delete m_model;
    }

    AbstractDataPluginModel *m_model;
    quint32 m_numberOfItems;
    QDeclarativeComponent* m_delegate;
    QGraphicsItem* m_delegateParent;
    QMap<AbstractDataPluginItem*,QDeclarativeItem*> m_delegateInstances;
};

AbstractDataPlugin::AbstractDataPlugin( const MarbleModel *marbleModel )
    : RenderPlugin( marbleModel ),
      d( new AbstractDataPluginPrivate )
{
}

AbstractDataPlugin::~AbstractDataPlugin()
{
    delete d;
}

bool AbstractDataPlugin::isInitialized() const
{
    return model() != 0;
}

QStringList AbstractDataPlugin::backendTypes() const
{
    return QStringList( name() );
}

QString AbstractDataPlugin::renderPolicy() const
{
    return QString( "ALWAYS" );
}

QStringList AbstractDataPlugin::renderPosition() const
{
    return QStringList( "ALWAYS_ON_TOP" );
}

bool AbstractDataPlugin::render( GeoPainter *painter, ViewportParams *viewport,
             const QString& renderPos, GeoSceneLayer * layer)
{
    Q_UNUSED( renderPos );
    Q_UNUSED( layer );

    if ( !d->m_model || !isInitialized() ) {
        return true;
    }

    if ( d->m_delegate ) {
        handleViewportChange( viewport );
    } else {
        QList<AbstractDataPluginItem*> items = d->m_model->items( viewport,
                                                                  marbleModel(),
                                                                  numberOfItems() );
        painter->save();

        // Paint the most important item at last
        for( int i = items.size() - 1; i >= 0; --i ) {
            items.at( i )->paintEvent( painter, viewport );
        }

        painter->restore();
    }

    return true;
}

AbstractDataPluginModel *AbstractDataPlugin::model() const
{
    return d->m_model;
}

void AbstractDataPlugin::setModel( AbstractDataPluginModel* model )
{
    if ( d->m_model ) {
        disconnect( d->m_model, SIGNAL( itemsUpdated() ), this, SIGNAL( repaintNeeded() ) );
        delete d->m_model;
    }
    d->m_model = model;

    connect( d->m_model, SIGNAL( itemsUpdated() ), this, SIGNAL( repaintNeeded() ) );
    connect( d->m_model, SIGNAL( favoriteItemsChanged( const QStringList& ) ), this,
             SLOT( favoriteItemsChanged( const QStringList& ) ) );
    connect( d->m_model, SIGNAL( favoriteItemsOnlyChanged() ), this,
                         SIGNAL( favoriteItemsOnlyChanged() ) );

    emit favoritesModelChanged();
}

const PluginManager* AbstractDataPlugin::pluginManager() const
{
    return marbleModel()->pluginManager();
}

quint32 AbstractDataPlugin::numberOfItems() const
{
    return d->m_numberOfItems;
}

void AbstractDataPlugin::setNumberOfItems( quint32 number )
{
    bool changed = ( number != d->m_numberOfItems );
    d->m_numberOfItems = number;

    if ( changed )
        emit changedNumberOfItems( number );
}

QList<AbstractDataPluginItem *> AbstractDataPlugin::whichItemAt( const QPoint& curpos )
{
    if ( d->m_model && enabled() && visible()) {
        return d->m_model->whichItemAt( curpos );
    }
    else {
        return QList<AbstractDataPluginItem *>();
    }
}

RenderPlugin::RenderType AbstractDataPlugin::renderType() const
{
    return Online;
}

void AbstractDataPlugin::setDelegate( QDeclarativeComponent *delegate, QGraphicsItem* parent )
{
    qDeleteAll( d->m_delegateInstances.values() );
    d->m_delegateInstances.clear();

    d->m_delegate = delegate;
    d->m_delegateParent = parent;
}

void AbstractDataPlugin::setFavoriteItemsOnly( bool favoriteOnly )
{
    if ( d->m_model && d->m_model->isFavoriteItemsOnly() != favoriteOnly ) {
        d->m_model->setFavoriteItemsOnly( favoriteOnly );
    }
}

bool AbstractDataPlugin::isFavoriteItemsOnly() const
{
    return d->m_model && d->m_model->isFavoriteItemsOnly();
}

QObject *AbstractDataPlugin::favoritesModel()
{
    return d->m_model ? d->m_model->favoritesModel() : 0;
}

void AbstractDataPlugin::handleViewportChange( ViewportParams* viewport )
{
    QList<AbstractDataPluginItem*> orphane = d->m_delegateInstances.keys();
    QList<AbstractDataPluginItem*> const items = d->m_model->items( viewport, marbleModel(), numberOfItems() );
    foreach( AbstractDataPluginItem* item, items ) {
        qreal x, y;
        Marble::GeoDataCoordinates const coordinates = item->coordinate();
        bool const visible = viewport->screenCoordinates( coordinates.longitude(), coordinates.latitude(), x, y );

        if ( !d->m_delegateInstances.contains( item ) ) {
            if ( !visible ) {
                // We don't have, but don't need it either. Shouldn't happen though as the model checks for it already.
                continue;
            }

            // Create a new QML object instance using the delegate as the factory. The original
            // data plugin item is set as the context object, i.e. all its properties are available
            // to QML directly with their names
            QDeclarativeContext *context = new QDeclarativeContext( qmlContext( d->m_delegate ) );
            context->setContextObject( item );
            QObject* component = d->m_delegate->create( context );
            QDeclarativeItem* newItem = qobject_cast<QDeclarativeItem*>( component );
            QGraphicsItem* graphicsItem = qobject_cast<QGraphicsItem*>( component );
            if ( graphicsItem && newItem ) {
                graphicsItem->setParentItem( d->m_delegateParent );
            }

            if ( newItem ) {
                d->m_delegateInstances[item] = newItem;
            } else {
                mDebug() << "Failed to create delegate";
                continue;
            }
        } else if ( !visible ) {
            // Previously visible but not anymore => needs to be deleted. Orphane list takes care of it later.
            // Shouldn't happen though as the model checks for it already.
            continue;
        }

        Q_ASSERT( visible );
        QDeclarativeItem* declarativeItem = d->m_delegateInstances[item];
        Q_ASSERT( declarativeItem );

        // Make sure we have a valid bounding rect for collision detection
        item->setProjection( viewport );
        item->setSize( QSizeF( declarativeItem->boundingRect().size() ) );

        int shiftX( 0 ), shiftY( 0 );
        switch( declarativeItem->transformOrigin() ) {
        case QDeclarativeItem::TopLeft:
        case QDeclarativeItem::Top:
        case QDeclarativeItem::TopRight:
            break;
        case QDeclarativeItem::Left:
        case QDeclarativeItem::Center:
        case QDeclarativeItem::Right:
            shiftY = declarativeItem->height() / 2;
            break;
        case QDeclarativeItem::BottomLeft:
        case QDeclarativeItem::Bottom:
        case QDeclarativeItem::BottomRight:
            shiftY = declarativeItem->height();
            break;
        }

        switch( declarativeItem->transformOrigin() ) {
        case QDeclarativeItem::TopLeft:
        case QDeclarativeItem::Left:
        case QDeclarativeItem::BottomLeft:
            break;
        case QDeclarativeItem::Top:
        case QDeclarativeItem::Center:
        case QDeclarativeItem::Bottom:
            shiftX = declarativeItem->width() / 2;
            break;
        case QDeclarativeItem::TopRight:
        case QDeclarativeItem::Right:
        case QDeclarativeItem::BottomRight:
            shiftX = declarativeItem->width();
            break;
        }

        declarativeItem->setPos( x - shiftX, y - shiftY );
        orphane.removeOne( item );
    }

    // Cleanup
    foreach( AbstractDataPluginItem* item, orphane ) {
        Q_ASSERT( d->m_delegateInstances.contains( item ) );
        delete d->m_delegateInstances[item];
        d->m_delegateInstances.remove( item );
    }
}

void AbstractDataPlugin::favoriteItemsChanged( const QStringList& favoriteItems )
{
    Q_UNUSED( favoriteItems )
}

} // namespace Marble

#include "AbstractDataPlugin.moc"
<|endoftext|>


<commit_before>//
// Copyright (c) 2014 CNRS
// Authors: Florent Lamiraux
//
// This file is part of hpp-core
// hpp-core is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-core is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-core  If not, see
// <http://www.gnu.org/licenses/>.

#include <hpp/util/debug.hh>
#include <hpp/core/continuous-collision-checking/dichotomy.hh>
#include <hpp/core/straight-path.hh>

#include "continuous-collision-checking/dichotomy/body-pair-collision.hh"

namespace hpp {
  namespace core {
    namespace continuousCollisionChecking {

      using dichotomy::BodyPairCollision;
      using dichotomy::BodyPairCollisionPtr_t;
      using dichotomy::BodyPairCollisions_t;

      bool compareBodyPairCol (const BodyPairCollisionPtr_t& bodyPair1,
			       const BodyPairCollisionPtr_t& bodyPair2)
      {
	assert (!bodyPair1->validSubset ().list ().empty ());
	assert (!bodyPair2->validSubset ().list ().empty ());
	return bodyPair1->validSubset ().list ().begin ()->second <
	  bodyPair2->validSubset ().list ().begin ()->second;
      }

      bool compareReverseBodyPairCol (const BodyPairCollisionPtr_t& bodyPair1,
				      const BodyPairCollisionPtr_t& bodyPair2)
      {
	assert (!bodyPair1->validSubset ().list ().empty ());
	assert (!bodyPair2->validSubset ().list ().empty ());
	return bodyPair1->validSubset ().list ().rbegin ()->first >
	  bodyPair2->validSubset ().list ().rbegin ()->first;
      }

      // Partially sort a list where only the first element is not sorted
      template <typename Compare>
      void partialSort (BodyPairCollisions_t& list, Compare comp)
      {
	BodyPairCollisions_t::iterator it1 = list.begin ();
	if (it1 == list.end ()) return;
	BodyPairCollisions_t::iterator it2 = it1; ++it2;
	while (it2 != list.end ()) {
	  if (comp (*it2, *it1))
	    std::iter_swap (it1, it2);
	  else
	    return;
	  it1 = it2;
	  ++it2;
	}
      }
      DichotomyPtr_t
      Dichotomy::create (const DevicePtr_t& robot, const value_type& tolerance)
      {
	Dichotomy* ptr =
	  new Dichotomy (robot, tolerance);
	DichotomyPtr_t shPtr (ptr);
	return shPtr;
      }

      bool Dichotomy::validate
      (const PathPtr_t& path, bool reverse, PathPtr_t& validPart)
      {
	StraightPathPtr_t straightPath = HPP_DYNAMIC_PTR_CAST
	  (StraightPath, path);
	// for each BodyPairCollision
	//   - set path,
	//   - compute valid interval at start (end if reverse)
	value_type t0 = path->timeRange ().first;
	value_type t1 = path->timeRange ().second;
	if (reverse) {
	  for (BodyPairCollisions_t::iterator itPair =
		 bodyPairCollisions_.begin ();
	       itPair != bodyPairCollisions_.end (); ++itPair) {
	    (*itPair)->path (straightPath);
	    // If collision at end point, return false
	    if (!(*itPair)->validateInterval (t1)) {
	      validPart = path->extract (interval_t (t1, t1));
	      return false;
	    }
	    assert ((*itPair)->validSubset ().contains (t1));
	  }
	  // Sort collision pairs
	  bodyPairCollisions_.sort (compareReverseBodyPairCol);

	  BodyPairCollisionPtr_t first = *(bodyPairCollisions_.begin ());
	  while (!first->validSubset ().contains (t0, true)) {
	    // find middle of first non valid interval
	    const std::list <interval_t>& intervals =
	      first->validSubset ().list ();
	    std::list <interval_t>::const_reverse_iterator lastInterval =
	      intervals.rbegin ();
	    std::list <interval_t>::const_reverse_iterator beforeLastInterval =
	      lastInterval; ++beforeLastInterval;
	    value_type upper = lastInterval->first;
	    value_type lower;
	    if (beforeLastInterval != intervals.rend ()) {
	      lower = beforeLastInterval->second;
	    } else {
	      lower = t0;
	    }
	    value_type middle = .5 * (lower + upper);
	    if (first->validateInterval (middle)) {
	      partialSort (bodyPairCollisions_, compareReverseBodyPairCol);
	    } else {
	      validPart = path->extract (interval_t (upper, t1));
	      return false;
	    }
	    first = *(bodyPairCollisions_.begin ());
	  }
	} else {
	  for (BodyPairCollisions_t::iterator itPair =
		 bodyPairCollisions_.begin ();
	       itPair != bodyPairCollisions_.end (); ++itPair) {
	    (*itPair)->path (straightPath);
	    // If collision at start point, return false
	    bool valid = (*itPair)->validateInterval (t0);
	    if (!valid) {
	      validPart = path->extract (interval_t (t0, t0));
	      hppDout (error, "Initial position in collision.");
	      return false;
	    }
	    assert ((*itPair)->validSubset ().contains (t0));
	  }
	  // Sort collision pairs
	  bodyPairCollisions_.sort (compareBodyPairCol);
	  BodyPairCollisionPtr_t first = *(bodyPairCollisions_.begin ());
	  while (!first->validSubset ().contains (t1)) {
	    // find middle of first non valid interval
	    const std::list <interval_t>& intervals =
	      first->validSubset ().list ();
	    std::list <interval_t>::const_iterator firstInterval =
	      intervals.begin ();
	    std::list <interval_t>::const_iterator secondInterval =
	      firstInterval;
	    ++secondInterval;
	    value_type lower = firstInterval->second;
	    value_type upper;
	    if (secondInterval != intervals.end ()) {
	      upper = secondInterval->first;
	    } else {
	      upper = t1;
	    }
	    value_type middle = .5 * (lower + upper);
	    if (first->validateInterval (middle)) {
	      partialSort (bodyPairCollisions_, compareBodyPairCol);
	    } else {
	      validPart = path->extract (interval_t (t0, lower));
	      hppDout (info, "Return path valid on [" << t0 << "," << lower
		       << "]");
	      return false;
	    }
	    first = *(bodyPairCollisions_.begin ());
	  }
	}
	validPart = path;
	hppDout (info, "Path valid defined on [" << t0 << "," << t1 << "]");
	return true;
      }

      void Dichotomy::addObstacle
      (const CollisionObjectPtr_t& object)
      {
	const JointVector_t& jv = robot_->getJointVector ();
	for (JointVector_t::const_iterator itJoint = jv.begin ();
	     itJoint != jv.end (); ++itJoint) {
	  BodyPtr_t body = (*itJoint)->linkedBody ();
	  bool foundPair = false;
	  if (body) {
	    ObjectVector_t objects;
	    objects.push_back (object);
	    bodyPairCollisions_.push_back
	      (BodyPairCollision::create (*itJoint, objects, tolerance_));
	  }
	}
      }

      void Dichotomy::removeObstacleFromJoint
      (const JointPtr_t& joint, const CollisionObjectPtr_t& obstacle)
      {
	for (BodyPairCollisions_t::iterator itPair =
	       bodyPairCollisions_.begin ();
	     itPair != bodyPairCollisions_.end (); ++itPair) {
	  if (!(*itPair)->joint_b () && (*itPair)->joint_a () == joint) {
	    if ((*itPair)->removeObjectTo_b (obstacle)) {
	      if ((*itPair)->objects_b ().empty ()) {
		bodyPairCollisions_.erase (itPair);
	      }
	      return;
	    }
	  }
	}
      }

      Dichotomy::~Dichotomy ()
      {
      }

      Dichotomy::Dichotomy
      (const DevicePtr_t& robot, const value_type& tolerance) :
	robot_ (robot), tolerance_ (tolerance),
	bodyPairCollisions_ ()
      {
	// Tolerance should be equal to 0, otherwise end of valid
	// sub-path might be in collision.
	if (tolerance != 0) {
	  throw std::runtime_error ("Dichotomy path validation method does not"
				    "support penetration.");
	}
	typedef model::Device::CollisionPairs_t CollisionPairs_t;
	// Build body pairs for collision checking
	// First auto-collision
	const CollisionPairs_t& colPairs = robot->collisionPairs
	  (model::COLLISION);
	for (CollisionPairs_t::const_iterator itPair = colPairs.begin ();
	     itPair != colPairs.end (); ++itPair) {
	  bodyPairCollisions_.push_back (BodyPairCollision::create
					 (itPair->first, itPair->second,
					  tolerance_));
	}
      }
    } // namespace continuousCollisionChecking
  } // namespace core
} // namespace hpp
<commit_msg>Create one BodyPairCollision object for each pair (joint, obstacle) only for<commit_after>//
// Copyright (c) 2014 CNRS
// Authors: Florent Lamiraux
//
// This file is part of hpp-core
// hpp-core is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-core is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-core  If not, see
// <http://www.gnu.org/licenses/>.

#include <hpp/util/debug.hh>
#include <hpp/core/continuous-collision-checking/dichotomy.hh>
#include <hpp/core/straight-path.hh>

#include "continuous-collision-checking/dichotomy/body-pair-collision.hh"

namespace hpp {
  namespace core {
    namespace continuousCollisionChecking {

      using dichotomy::BodyPairCollision;
      using dichotomy::BodyPairCollisionPtr_t;
      using dichotomy::BodyPairCollisions_t;

      bool compareBodyPairCol (const BodyPairCollisionPtr_t& bodyPair1,
			       const BodyPairCollisionPtr_t& bodyPair2)
      {
	assert (!bodyPair1->validSubset ().list ().empty ());
	assert (!bodyPair2->validSubset ().list ().empty ());
	return bodyPair1->validSubset ().list ().begin ()->second <
	  bodyPair2->validSubset ().list ().begin ()->second;
      }

      bool compareReverseBodyPairCol (const BodyPairCollisionPtr_t& bodyPair1,
				      const BodyPairCollisionPtr_t& bodyPair2)
      {
	assert (!bodyPair1->validSubset ().list ().empty ());
	assert (!bodyPair2->validSubset ().list ().empty ());
	return bodyPair1->validSubset ().list ().rbegin ()->first >
	  bodyPair2->validSubset ().list ().rbegin ()->first;
      }

      // Partially sort a list where only the first element is not sorted
      template <typename Compare>
      void partialSort (BodyPairCollisions_t& list, Compare comp)
      {
	BodyPairCollisions_t::iterator it1 = list.begin ();
	if (it1 == list.end ()) return;
	BodyPairCollisions_t::iterator it2 = it1; ++it2;
	while (it2 != list.end ()) {
	  if (comp (*it2, *it1))
	    std::iter_swap (it1, it2);
	  else
	    return;
	  it1 = it2;
	  ++it2;
	}
      }
      DichotomyPtr_t
      Dichotomy::create (const DevicePtr_t& robot, const value_type& tolerance)
      {
	Dichotomy* ptr =
	  new Dichotomy (robot, tolerance);
	DichotomyPtr_t shPtr (ptr);
	return shPtr;
      }

      bool Dichotomy::validate
      (const PathPtr_t& path, bool reverse, PathPtr_t& validPart)
      {
	StraightPathPtr_t straightPath = HPP_DYNAMIC_PTR_CAST
	  (StraightPath, path);
	// for each BodyPairCollision
	//   - set path,
	//   - compute valid interval at start (end if reverse)
	value_type t0 = path->timeRange ().first;
	value_type t1 = path->timeRange ().second;
	if (reverse) {
	  for (BodyPairCollisions_t::iterator itPair =
		 bodyPairCollisions_.begin ();
	       itPair != bodyPairCollisions_.end (); ++itPair) {
	    (*itPair)->path (straightPath);
	    // If collision at end point, return false
	    if (!(*itPair)->validateInterval (t1)) {
	      validPart = path->extract (interval_t (t1, t1));
	      return false;
	    }
	    assert ((*itPair)->validSubset ().contains (t1));
	  }
	  // Sort collision pairs
	  bodyPairCollisions_.sort (compareReverseBodyPairCol);

	  BodyPairCollisionPtr_t first = *(bodyPairCollisions_.begin ());
	  while (!first->validSubset ().contains (t0, true)) {
	    // find middle of first non valid interval
	    const std::list <interval_t>& intervals =
	      first->validSubset ().list ();
	    std::list <interval_t>::const_reverse_iterator lastInterval =
	      intervals.rbegin ();
	    std::list <interval_t>::const_reverse_iterator beforeLastInterval =
	      lastInterval; ++beforeLastInterval;
	    value_type upper = lastInterval->first;
	    value_type lower;
	    if (beforeLastInterval != intervals.rend ()) {
	      lower = beforeLastInterval->second;
	    } else {
	      lower = t0;
	    }
	    value_type middle = .5 * (lower + upper);
	    if (first->validateInterval (middle)) {
	      partialSort (bodyPairCollisions_, compareReverseBodyPairCol);
	    } else {
	      validPart = path->extract (interval_t (upper, t1));
	      return false;
	    }
	    first = *(bodyPairCollisions_.begin ());
	  }
	} else {
	  for (BodyPairCollisions_t::iterator itPair =
		 bodyPairCollisions_.begin ();
	       itPair != bodyPairCollisions_.end (); ++itPair) {
	    (*itPair)->path (straightPath);
	    // If collision at start point, return false
	    bool valid = (*itPair)->validateInterval (t0);
	    if (!valid) {
	      validPart = path->extract (interval_t (t0, t0));
	      hppDout (error, "Initial position in collision.");
	      return false;
	    }
	    assert ((*itPair)->validSubset ().contains (t0));
	  }
	  // Sort collision pairs
	  bodyPairCollisions_.sort (compareBodyPairCol);
	  BodyPairCollisionPtr_t first = *(bodyPairCollisions_.begin ());
	  while (!first->validSubset ().contains (t1)) {
	    // find middle of first non valid interval
	    const std::list <interval_t>& intervals =
	      first->validSubset ().list ();
	    std::list <interval_t>::const_iterator firstInterval =
	      intervals.begin ();
	    std::list <interval_t>::const_iterator secondInterval =
	      firstInterval;
	    ++secondInterval;
	    value_type lower = firstInterval->second;
	    value_type upper;
	    if (secondInterval != intervals.end ()) {
	      upper = secondInterval->first;
	    } else {
	      upper = t1;
	    }
	    value_type middle = .5 * (lower + upper);
	    if (first->validateInterval (middle)) {
	      partialSort (bodyPairCollisions_, compareBodyPairCol);
	    } else {
	      validPart = path->extract (interval_t (t0, lower));
	      hppDout (info, "Return path valid on [" << t0 << "," << lower
		       << "]");
	      return false;
	    }
	    first = *(bodyPairCollisions_.begin ());
	  }
	}
	validPart = path;
	hppDout (info, "Path valid defined on [" << t0 << "," << t1 << "]");
	return true;
      }

      void Dichotomy::addObstacle
      (const CollisionObjectPtr_t& object)
      {
	const JointVector_t& jv = robot_->getJointVector ();
	for (JointVector_t::const_iterator itJoint = jv.begin ();
	     itJoint != jv.end (); ++itJoint) {
	  BodyPtr_t body = (*itJoint)->linkedBody ();
	  bool foundPair = false;
	  if (body) {
	    for (BodyPairCollisions_t::iterator itPair =
		   bodyPairCollisions_.begin ();
		 itPair != bodyPairCollisions_.end (); ++itPair) {
	      if (((*itPair)->joint_a () == *itJoint) &&
		  (!(*itPair)->joint_b ())) {
		(*itPair)->addObjectTo_b (object);
		foundPair = true;
	      }
	    }
	    if (!foundPair) {
	      ObjectVector_t objects;
	      objects.push_back (object);
	      bodyPairCollisions_.push_back
		(BodyPairCollision::create (*itJoint, objects, tolerance_));
	    }
	  }
	}
      }

      void Dichotomy::removeObstacleFromJoint
      (const JointPtr_t& joint, const CollisionObjectPtr_t& obstacle)
      {
	for (BodyPairCollisions_t::iterator itPair =
	       bodyPairCollisions_.begin ();
	     itPair != bodyPairCollisions_.end (); ++itPair) {
	  if (!(*itPair)->joint_b () && (*itPair)->joint_a () == joint) {
	    if ((*itPair)->removeObjectTo_b (obstacle)) {
	      if ((*itPair)->objects_b ().empty ()) {
		bodyPairCollisions_.erase (itPair);
	      }
	      return;
	    }
	  }
	}
      }

      Dichotomy::~Dichotomy ()
      {
      }

      Dichotomy::Dichotomy
      (const DevicePtr_t& robot, const value_type& tolerance) :
	robot_ (robot), tolerance_ (tolerance),
	bodyPairCollisions_ ()
      {
	// Tolerance should be equal to 0, otherwise end of valid
	// sub-path might be in collision.
	if (tolerance != 0) {
	  throw std::runtime_error ("Dichotomy path validation method does not"
				    "support penetration.");
	}
	typedef model::Device::CollisionPairs_t CollisionPairs_t;
	// Build body pairs for collision checking
	// First auto-collision
	const CollisionPairs_t& colPairs = robot->collisionPairs
	  (model::COLLISION);
	for (CollisionPairs_t::const_iterator itPair = colPairs.begin ();
	     itPair != colPairs.end (); ++itPair) {
	  bodyPairCollisions_.push_back (BodyPairCollision::create
					 (itPair->first, itPair->second,
					  tolerance_));
	}
      }
    } // namespace continuousCollisionChecking
  } // namespace core
} // namespace hpp
<|endoftext|>