<commit_before>#include <stdio.h>
#include <stdlib.h>
#include <cstring>
#include <string>
#include <sstream>
#include "phIO.h"
#include "phComm.h"
#include "phio_base.h"

#define PHIO_TRACING 0
namespace {
  void trace(const char* key, const char* aux="", void* obj=NULL) {
    if(PHIO_TRACING)
      fprintf(stderr, "PHIO_TRACE entering %s %s %p\n", key, aux, obj);
  }
}

#ifdef __cplusplus
extern "C" {
#endif

void phio_readheader(
    phio_fp f,
    const  char keyphrase[],
    void* valueArray,
    int*  nItems,
    const char  datatype[],
    const char  iotype[] ) {
  f->ops->readheader(f->file, keyphrase, valueArray,
      nItems, datatype, iotype);
}
void phio_writeheader(
    phio_fp f,
    const char keyphrase[],
    const void* valueArray,
    const int* nItems,
    const int* ndataItems,
    const char datatype[],
    const char iotype[] ) {
  f->ops->writeheader(f->file, keyphrase, valueArray,
      nItems, ndataItems, datatype, iotype);
}
void phio_readdatablock(
    phio_fp f,
    const  char keyphrase[],
    void* valueArray,
    int*  nItems,
    const char  datatype[],
    const char  iotype[] ) {
  f->ops->readdatablock(f->file, keyphrase, valueArray,
      nItems, datatype, iotype);
}
void phio_writedatablock(
    phio_fp f,
    const char keyphrase[],
    const void* valueArray,
    const int* nItems,
    const char datatype[],
    const char iotype[]) {
  f->ops->writedatablock(f->file, keyphrase, valueArray,
      nItems, datatype, iotype);
}

void phio_constructName(
    phio_fp f,
    const char inName[],
    char* outName) {
  f->ops->constructname(inName, outName);
}

void phio_openfile(
    const char filename[],
    phio_fp f) {
  trace("openfile",filename,f);
  f->ops->openfile(filename, f);
}

void phio_closefile(phio_fp f) {
  trace("closefile","unknown",f);
  f->ops->closefile(f);
}

void phio_appendInt(char* dest, int v) {
  std::stringstream ss;
  ss << dest << v << '.';
  std::string s = ss.str();
  strcpy(dest, s.c_str());
}

#ifdef __cplusplus
}
#endif
<commit_msg>io timers<commit_after>#include <stdio.h>
#include <stdlib.h>
#include <cstring>
#include <string>
#include <sstream>
#include "phIO.h"
#include "phComm.h"
#include "phio_base.h"
#include <mpi.h>

#ifndef PHASTAIO_TIMERS_ON
#define PHASTAIO_TIMERS_ON 0
#endif

namespace {
  inline double getTime() {
    return MPI_Wtime();
  }
  inline bool isRankZero() {
    int rank = 0;
    MPI_Comm_rank(MPI_COMM_WORLD, &rank);
    return !rank;
  }
  inline printTime(const char* key, double t) {
#if PHASTAIO_TIMERS_ON==1
    if( isRankZero() )
      fprintf(stderr, "%s %f\n", key, t);
#endif
  }
}

#define PHIO_TRACING 0
namespace {
  void trace(const char* key, const char* aux="", void* obj=NULL) {
    if(PHIO_TRACING)
      fprintf(stderr, "PHIO_TRACE entering %s %s %p\n", key, aux, obj);
  }
}

#ifdef __cplusplus
extern "C" {
#endif

void phio_readheader(
    phio_fp f,
    const  char keyphrase[],
    void* valueArray,
    int*  nItems,
    const char  datatype[],
    const char  iotype[] ) {
  const double t0 = getTime();
  f->ops->readheader(f->file, keyphrase, valueArray,
      nItems, datatype, iotype);
  printTime(__func__, getTime()-t0);
}
void phio_writeheader(
    phio_fp f,
    const char keyphrase[],
    const void* valueArray,
    const int* nItems,
    const int* ndataItems,
    const char datatype[],
    const char iotype[] ) {
  const double t0 = getTime();
  f->ops->writeheader(f->file, keyphrase, valueArray,
      nItems, ndataItems, datatype, iotype);
  printTime(__func__, getTime()-t0);
}
void phio_readdatablock(
    phio_fp f,
    const  char keyphrase[],
    void* valueArray,
    int*  nItems,
    const char  datatype[],
    const char  iotype[] ) {
  const double t0 = getTime();
  f->ops->readdatablock(f->file, keyphrase, valueArray,
      nItems, datatype, iotype);
  printTime(__func__, getTime()-t0);
}
void phio_writedatablock(
    phio_fp f,
    const char keyphrase[],
    const void* valueArray,
    const int* nItems,
    const char datatype[],
    const char iotype[]) {
  const double t0 = getTime();
  f->ops->writedatablock(f->file, keyphrase, valueArray,
      nItems, datatype, iotype);
  printTime(__func__, getTime()-t0);
}

void phio_constructName(
    phio_fp f,
    const char inName[],
    char* outName) {
  const double t0 = getTime();
  f->ops->constructname(inName, outName);
  printTime(__func__, getTime()-t0);
}

void phio_openfile(
    const char filename[],
    phio_fp f) {
  const double t0 = getTime();
  trace("openfile",filename,f);
  f->ops->openfile(filename, f);
  printTime(__func__, getTime()-t0);
}

void phio_closefile(phio_fp f) {
  const double t0 = getTime();
  trace("closefile","unknown",f);
  f->ops->closefile(f);
  printTime(__func__, getTime()-t0);
}

void phio_appendInt(char* dest, int v) {
  std::stringstream ss;
  ss << dest << v << '.';
  std::string s = ss.str();
  strcpy(dest, s.c_str());
}

#ifdef __cplusplus
}
#endif
<|endoftext|>


<commit_before>#include "inputHandler.h"

#include "glm/glm.hpp"
#include "platform.h"
#include <cmath>

// Damping factor for translation; reciprocal of the decay period in seconds
#define DAMPING_PAN 4.0f

// Damping factor for zoom; reciprocal of the decay period in seconds
#define DAMPING_ZOOM 6.0f

// Minimum translation at which momentum should start (pixels per second)
#define THRESHOLD_START_PAN 350.f

// Minimum translation at which momentum should stop (pixels per second)
#define THRESHOLD_STOP_PAN 24.f

// Minimum zoom at which momentum should start (zoom levels per second)
#define THRESHOLD_START_ZOOM 1.f

// Minimum zoom at which momentum should stop (zoom levels per second)
#define THRESHOLD_STOP_ZOOM 0.3f

namespace Tangram {

InputHandler::InputHandler(std::shared_ptr<View> _view) : m_view(_view) {}

void InputHandler::update(float _dt) {

    auto velocityPanPixels = m_view->pixelsPerMeter() / m_view->pixelScale() * m_velocityPan;

    bool isFlinging = glm::length(velocityPanPixels) > THRESHOLD_STOP_PAN || std::abs(m_velocityZoom) > THRESHOLD_STOP_ZOOM;

    if (!m_gestureOccured && isFlinging) {

        m_velocityPan -= _dt * DAMPING_PAN * m_velocityPan;
        m_view->translate(_dt * m_velocityPan.x, _dt * m_velocityPan.y);

        m_velocityZoom -= _dt * DAMPING_ZOOM * m_velocityZoom;
        m_view->zoom(m_velocityZoom * _dt);

        requestRender();

    }

    m_gestureOccured = false;
}

void InputHandler::handleTapGesture(float _posX, float _posY) {

    onGesture();

    double viewCenterX = 0.5 * m_view->getWidth();
    double viewCenterY = 0.5 * m_view->getHeight();

    m_view->screenToGroundPlane(viewCenterX, viewCenterY);
    m_view->screenToGroundPlane(_posX, _posY);

    m_view->translate((_posX - viewCenterX), (_posY - viewCenterY));

}

void InputHandler::handleDoubleTapGesture(float _posX, float _posY) {

    handlePinchGesture(_posX, _posY, 2.f, 0.f);

}

void InputHandler::handlePanGesture(float _startX, float _startY, float _endX, float _endY) {

    onGesture();

    m_view->screenToGroundPlane(_startX, _startY);
    m_view->screenToGroundPlane(_endX, _endY);

    float dx = _startX - _endX;
    float dy = _startY - _endY;

    m_view->translate(dx, dy);

}

void InputHandler::handleFlingGesture(float _posX, float _posY, float _velocityX, float _velocityY) {

    if (glm::length(glm::vec2(_velocityX, _velocityY)) / m_view->pixelScale() <= THRESHOLD_START_PAN) {
        return;
    }

    const static float epsilon = 0.0167f;

    float startX = _posX;
    float startY = _posY;
    float endX = _posX + epsilon * _velocityX;
    float endY = _posY + epsilon * _velocityY;

    m_view->screenToGroundPlane(startX, startY);
    m_view->screenToGroundPlane(endX, endY);

    float dx = (startX - endX) / epsilon;
    float dy = (startY - endY) / epsilon;

    setVelocity(0.f, glm::vec2(dx, dy));

}

void InputHandler::handlePinchGesture(float _posX, float _posY, float _scale, float _velocity) {

    onGesture();

    float z = m_view->getZoom();
    static float invLog2 = 1 / log(2);
    m_view->zoom(log(_scale) * invLog2);

    m_view->screenToGroundPlane(_posX, _posY);
    float s = pow(2, m_view->getZoom() - z) - 1;
    m_view->translate(s * _posX, s * _posY);

    // Take the derivative of zoom as a function of scale:
    // z(s) = log2(s) + C
    // z'(s) = s' / s / log(2)
    float vz = _velocity / _scale * invLog2;
    if (std::abs(vz) >= THRESHOLD_START_ZOOM) {
        setVelocity(vz, glm::vec2(0.f));
    }

}

void InputHandler::handleRotateGesture(float _posX, float _posY, float _radians) {

    onGesture();
    m_view->roll(_radians);

}

void InputHandler::handleShoveGesture(float _distance) {

    onGesture();

    float angle = -M_PI * _distance / m_view->getHeight();
    m_view->pitch(angle);

}

void InputHandler::cancelFling() {

    setVelocity(0.f, { 0.f, 0.f});

}

void InputHandler::onGesture() {

    m_gestureOccured = true;
    setVelocity(0.f, { 0.f, 0.f });
    requestRender();

}

void InputHandler::setVelocity(float _zoom, glm::vec2 _translate) {

    // setup deltas for momentum on gesture
    m_velocityPan = _translate;
    m_velocityZoom = _zoom;
}

}
<commit_msg>Modify rotate gesture handler to apply rotation around gesture center<commit_after>#include "inputHandler.h"

#include "glm/vec2.hpp"
#include "glm/gtx/rotate_vector.hpp"
#include "platform.h"
#include <cmath>

// Damping factor for translation; reciprocal of the decay period in seconds
#define DAMPING_PAN 4.0f

// Damping factor for zoom; reciprocal of the decay period in seconds
#define DAMPING_ZOOM 6.0f

// Minimum translation at which momentum should start (pixels per second)
#define THRESHOLD_START_PAN 350.f

// Minimum translation at which momentum should stop (pixels per second)
#define THRESHOLD_STOP_PAN 24.f

// Minimum zoom at which momentum should start (zoom levels per second)
#define THRESHOLD_START_ZOOM 1.f

// Minimum zoom at which momentum should stop (zoom levels per second)
#define THRESHOLD_STOP_ZOOM 0.3f

namespace Tangram {

InputHandler::InputHandler(std::shared_ptr<View> _view) : m_view(_view) {}

void InputHandler::update(float _dt) {

    auto velocityPanPixels = m_view->pixelsPerMeter() / m_view->pixelScale() * m_velocityPan;

    bool isFlinging = glm::length(velocityPanPixels) > THRESHOLD_STOP_PAN || std::abs(m_velocityZoom) > THRESHOLD_STOP_ZOOM;

    if (!m_gestureOccured && isFlinging) {

        m_velocityPan -= _dt * DAMPING_PAN * m_velocityPan;
        m_view->translate(_dt * m_velocityPan.x, _dt * m_velocityPan.y);

        m_velocityZoom -= _dt * DAMPING_ZOOM * m_velocityZoom;
        m_view->zoom(m_velocityZoom * _dt);

        requestRender();

    }

    m_gestureOccured = false;
}

void InputHandler::handleTapGesture(float _posX, float _posY) {

    onGesture();

    double viewCenterX = 0.5 * m_view->getWidth();
    double viewCenterY = 0.5 * m_view->getHeight();

    m_view->screenToGroundPlane(viewCenterX, viewCenterY);
    m_view->screenToGroundPlane(_posX, _posY);

    m_view->translate((_posX - viewCenterX), (_posY - viewCenterY));

}

void InputHandler::handleDoubleTapGesture(float _posX, float _posY) {

    handlePinchGesture(_posX, _posY, 2.f, 0.f);

}

void InputHandler::handlePanGesture(float _startX, float _startY, float _endX, float _endY) {

    onGesture();

    m_view->screenToGroundPlane(_startX, _startY);
    m_view->screenToGroundPlane(_endX, _endY);

    float dx = _startX - _endX;
    float dy = _startY - _endY;

    m_view->translate(dx, dy);

}

void InputHandler::handleFlingGesture(float _posX, float _posY, float _velocityX, float _velocityY) {

    if (glm::length(glm::vec2(_velocityX, _velocityY)) / m_view->pixelScale() <= THRESHOLD_START_PAN) {
        return;
    }

    const static float epsilon = 0.0167f;

    float startX = _posX;
    float startY = _posY;
    float endX = _posX + epsilon * _velocityX;
    float endY = _posY + epsilon * _velocityY;

    m_view->screenToGroundPlane(startX, startY);
    m_view->screenToGroundPlane(endX, endY);

    float dx = (startX - endX) / epsilon;
    float dy = (startY - endY) / epsilon;

    setVelocity(0.f, glm::vec2(dx, dy));

}

void InputHandler::handlePinchGesture(float _posX, float _posY, float _scale, float _velocity) {

    onGesture();

    float z = m_view->getZoom();
    static float invLog2 = 1 / log(2);
    m_view->zoom(log(_scale) * invLog2);

    m_view->screenToGroundPlane(_posX, _posY);
    float s = pow(2, m_view->getZoom() - z) - 1;
    m_view->translate(s * _posX, s * _posY);

    // Take the derivative of zoom as a function of scale:
    // z(s) = log2(s) + C
    // z'(s) = s' / s / log(2)
    float vz = _velocity / _scale * invLog2;
    if (std::abs(vz) >= THRESHOLD_START_ZOOM) {
        setVelocity(vz, glm::vec2(0.f));
    }

}

void InputHandler::handleRotateGesture(float _posX, float _posY, float _radians) {

    onGesture();

    // Get vector from center of rotation to view center
    m_view->screenToGroundPlane(_posX, _posY);
    glm::vec2 offset(_posX, _posY);

    // Rotate vector by gesture rotation and apply difference as translation
    glm::vec2 translation = offset - glm::rotate(offset, _radians);
    m_view->translate(translation.x, translation.y);

    m_view->roll(_radians);

}

void InputHandler::handleShoveGesture(float _distance) {

    onGesture();

    float angle = -M_PI * _distance / m_view->getHeight();
    m_view->pitch(angle);

}

void InputHandler::cancelFling() {

    setVelocity(0.f, { 0.f, 0.f});

}

void InputHandler::onGesture() {

    m_gestureOccured = true;
    setVelocity(0.f, { 0.f, 0.f });
    requestRender();

}

void InputHandler::setVelocity(float _zoom, glm::vec2 _translate) {

    // setup deltas for momentum on gesture
    m_velocityPan = _translate;
    m_velocityZoom = _zoom;
}

}
<|endoftext|>


<commit_before>/*
    Copyright 2013 Brain Research Institute, Melbourne, Australia

    Written by David Raffelt, 31/01/2013

    This file is part of MRtrix.

    MRtrix is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    MRtrix is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with MRtrix.  If not, see <http://www.gnu.org/licenses/>.

*/

#include "command.h"
#include "math/median.h"
#include "dwi/tractography/properties.h"
#include "dwi/tractography/scalar_file.h"

using namespace MR;
using namespace App;

void usage ()
{
  AUTHOR = "David Raffelt (d.raffelt@brain.org.au)";

  DESCRIPTION
  + "Gaussian filter a track scalar file";

  ARGUMENTS
  + Argument ("input",  "the input track scalar file.").type_file_in ()
  + Argument ("output", "the output track scalar file").type_file_out ();

  OPTIONS
  + Option ("stdev", "apply Gaussian smoothing with the specified standard deviation. "
            "The standard deviation is defined in units of track points (Default: 4)")
  + Argument ("sigma").type_float();
}

typedef float value_type;

void run ()
{
  DWI::Tractography::Properties properties;
  DWI::Tractography::ScalarReader<value_type> reader (argument[0], properties);
  DWI::Tractography::ScalarWriter<value_type> writer (argument[1], properties);

  Options opt = get_options("stdev");
  float stdev = 4.0;
  if (opt.size())
    stdev = opt[0][0];

  std::vector<float> kernel (2 * ceil(2.5 * stdev) + 1, 0);
  float norm_factor = 0.0;
  float radius = (kernel.size() - 1.0) / 2.0;
  for (size_t c = 0; c < kernel.size(); ++c) {
    kernel[c] = exp(-(c - radius) * (c - radius)  / (2 * stdev * stdev));
    norm_factor += kernel[c];
  }
  for (size_t c = 0; c < kernel.size(); c++)
    kernel[c] /= norm_factor;

  std::vector<value_type> tck_scalar;
  while (reader (tck_scalar)) {
    std::vector<value_type> tck_scalars_smoothed (tck_scalar.size());

    for (size_t i = 0; i < tck_scalar.size(); ++i) {
      float norm_factor = 0.0;
      float value = 0.0;
      for (int k = -(int)radius; k <= (int)radius; ++k) {
        if (i + k >= 0 && i + k < tck_scalar.size()) {
          value += kernel[k + radius] * tck_scalar[i + k];
          norm_factor += kernel[k + radius];
        }
      }
      tck_scalars_smoothed[i] = value / norm_factor;
    }
    writer (tck_scalars_smoothed);
  }
}

<commit_msg>Change to tsfsmooth to prevent compiler warning<commit_after>/*
    Copyright 2013 Brain Research Institute, Melbourne, Australia

    Written by David Raffelt, 31/01/2013

    This file is part of MRtrix.

    MRtrix is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    MRtrix is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with MRtrix.  If not, see <http://www.gnu.org/licenses/>.

*/

#include "command.h"
#include "math/median.h"
#include "dwi/tractography/properties.h"
#include "dwi/tractography/scalar_file.h"

using namespace MR;
using namespace App;

void usage ()
{
  AUTHOR = "David Raffelt (d.raffelt@brain.org.au)";

  DESCRIPTION
  + "Gaussian filter a track scalar file";

  ARGUMENTS
  + Argument ("input",  "the input track scalar file.").type_file_in ()
  + Argument ("output", "the output track scalar file").type_file_out ();

  OPTIONS
  + Option ("stdev", "apply Gaussian smoothing with the specified standard deviation. "
            "The standard deviation is defined in units of track points (Default: 4)")
  + Argument ("sigma").type_float();
}

typedef float value_type;

void run ()
{
  DWI::Tractography::Properties properties;
  DWI::Tractography::ScalarReader<value_type> reader (argument[0], properties);
  DWI::Tractography::ScalarWriter<value_type> writer (argument[1], properties);

  Options opt = get_options("stdev");
  float stdev = 4.0;
  if (opt.size())
    stdev = opt[0][0];

  std::vector<float> kernel (2 * ceil(2.5 * stdev) + 1, 0);
  float norm_factor = 0.0;
  float radius = (kernel.size() - 1.0) / 2.0;
  for (size_t c = 0; c < kernel.size(); ++c) {
    kernel[c] = exp(-(c - radius) * (c - radius)  / (2 * stdev * stdev));
    norm_factor += kernel[c];
  }
  for (size_t c = 0; c < kernel.size(); c++)
    kernel[c] /= norm_factor;

  std::vector<value_type> tck_scalar;
  while (reader (tck_scalar)) {
    std::vector<value_type> tck_scalars_smoothed (tck_scalar.size());

    for (int i = 0; i < (int)tck_scalar.size(); ++i) {
      float norm_factor = 0.0;
      float value = 0.0;
      for (int k = -(int)radius; k <= (int)radius; ++k) {
        if (i + k >= 0 && i + k < tck_scalar.size()) {
          value += kernel[k + radius] * tck_scalar[i + k];
          norm_factor += kernel[k + radius];
        }
      }
      tck_scalars_smoothed[i] = value / norm_factor;
    }
    writer (tck_scalars_smoothed);
  }
}

<|endoftext|>


<commit_before>/*
* This file is part of ATLAS. It is subject to the license terms in
* the LICENSE file found in the top-level directory of this distribution.
* (Also avialable at http://www.apache.org/licenses/LICENSE-2.0.txt)
* You may not use this file except in compliance with the License.
*/
#define _USE_MATH_DEFINES

#include "AMLFrameImporter.hpp"
#include "../../internal/Exception.hpp"
#include <Poco/DOM/NamedNodeMap.h>
#include <Poco/DOM/AutoPtr.h>

namespace AssimpWorker{

	using ATLAS::Model::DataDeletingBlob;

	AMLFrameImporter::AMLFrameImporter(){
		return;
	}

	ATLAS::Model::Folder& AMLFrameImporter::createParentHierarchy(Poco::XML::Node* node, ATLAS::Model::Folder& root){
		std::stack<Poco::XML::Node*> parentNodeStack;
		buildHierarchyStack(node, parentNodeStack);
		return createFolderStructureForStack(parentNodeStack, root);
	}

	void AMLFrameImporter::buildHierarchyStack(const Poco::XML::Node* node, std::stack<Poco::XML::Node*>& parentNodes) {
		Poco::XML::Node* transformationRoot((Poco::XML::Node*) node);
		while (transformationRoot->localName() == "InternalElement" || transformationRoot->localName() == "ExternalInterface") {
			// Walk our way up the InternalElement hierarchy and add each node to a stack. Later we use the stack to
			// walk back down the tree to build the transformation matrices and folder structure
			parentNodes.push(transformationRoot);
			transformationRoot = transformationRoot->parentNode();
		}
	}

	ATLAS::Model::Folder& AMLFrameImporter::createFolderStructureForStack(std::stack<Poco::XML::Node*>& parentNodeStack, ATLAS::Model::Folder& root) {
		Poco::XML::Node* currentNode = parentNodeStack.top();
		ATLAS::Model::Folder& currentFolder = getFolderForNode(currentNode, root);
		parentNodeStack.pop();
		if (parentNodeStack.empty()) {
			return currentFolder;
		}
		else {
			return createFolderStructureForStack(parentNodeStack, currentFolder);
		}
	}

	std::string AMLFrameImporter::getNameAttributeForNode(Poco::XML::Node* node) {
		std::string name = node->nodeName();
		Poco::XML::AutoPtr<Poco::XML::NamedNodeMap> attributes = node->attributes();
		Poco::XML::Node* nameNode = attributes->getNamedItem("Name");
		if (nameNode == nullptr) {
			nameNode = attributes->getNamedItem("name");
		}
		if (nameNode != nullptr) {
			name = nameNode->getNodeValue();
		}
		attributes->release();
		return name;
	}

	ATLAS::Model::Folder& AMLFrameImporter::getFolderForNode(Poco::XML::Node* node, ATLAS::Model::Folder& parentFolder) {
		auto visited = visitedNodes.find(node);
		if (visited != visitedNodes.end()) {
			return *visited->second; //Already created a folder for this node so just return that one
		}
		else {
			ATLAS::Model::Folder& folderForNode = createFolderForNode(node, parentFolder);
			Poco::XML::Node* frame = node->getNodeByPath("/Attribute[@Name='Frame']");
			aiMatrix4x4 localTransform = createFrameTransformMatrix(frame);
			addTransformBlobToFolder(localTransform, folderForNode, "transform");
			return folderForNode;
		}
	}

	ATLAS::Model::Folder& AMLFrameImporter::createFolderForNode(Poco::XML::Node* node, ATLAS::Model::Folder& parentFolder) {
		ATLAS::Model::Folder& nodeFolder = parentFolder.appendChild("node");
		nodeFolder.setName(getNameAttributeForNode(node));
		std::pair<Poco::XML::Node*, ATLAS::Model::Folder*> toInsert = std::make_pair(node, &nodeFolder);
		visitedNodes.insert(toInsert);
		return nodeFolder;
	}

	void AMLFrameImporter::addTransformBlobToFolder(const aiMatrix4x4& transform, ATLAS::Model::Folder& folder, const std::string& transformType) {
		aiMatrix4x4* transformCopy = new aiMatrix4x4(transform);
		DataDeletingBlob<aiMatrix4x4> transformBlob(transformCopy);
		folder.addBlob(transformType, transformBlob);
	}

	aiMatrix4x4 AMLFrameImporter::createFrameTransformMatrix(const Poco::XML::Node* frame) {
		if (frame == nullptr) {
			aiMatrix4x4 identity;
			return identity;
		}
		try {
			double tx = ::atof(frame->getNodeByPath("/Attribute[@Name='x']/Value")->innerText().c_str());
			double ty = ::atof(frame->getNodeByPath("/Attribute[@Name='y']/Value")->innerText().c_str());
			double tz = ::atof(frame->getNodeByPath("/Attribute[@Name='z']/Value")->innerText().c_str());
			double rx = ::atof(frame->getNodeByPath("/Attribute[@Name='rx']/Value")->innerText().c_str());
			double ry = ::atof(frame->getNodeByPath("/Attribute[@Name='ry']/Value")->innerText().c_str());
			double rz = ::atof(frame->getNodeByPath("/Attribute[@Name='rz']/Value")->innerText().c_str());
			const double toRad = M_PI / 180;
			aiMatrix4x4 temp;
			aiMatrix4x4 localTransform;
			// re-assemble Frame transform (which is in right-hand z-up) into a right-hand y-up transform
			// hence the shuffling of parameters and negative factors. mind the order of transforms!
			localTransform *= aiMatrix4x4::Translation(aiVector3D(tx,tz,-ty),temp);
			localTransform *= aiMatrix4x4::RotationX(rx*toRad, temp);
			localTransform *= aiMatrix4x4::RotationY(rz*toRad, temp);
			localTransform *= aiMatrix4x4::RotationZ(-ry*toRad, temp);
			return localTransform;
		}
		catch (const Exception& e) {
			throw AMLException("Syntax error while parsing a Frame attribute. This transformation will be ignored.");
		}
	}
}
<commit_msg>Frame transforms again, this time with explicit axis changes.<commit_after>/*
* This file is part of ATLAS. It is subject to the license terms in
* the LICENSE file found in the top-level directory of this distribution.
* (Also avialable at http://www.apache.org/licenses/LICENSE-2.0.txt)
* You may not use this file except in compliance with the License.
*/
#define _USE_MATH_DEFINES

#include "AMLFrameImporter.hpp"
#include "../../internal/Exception.hpp"
#include <Poco/DOM/NamedNodeMap.h>
#include <Poco/DOM/AutoPtr.h>

namespace AssimpWorker{

	using ATLAS::Model::DataDeletingBlob;

	AMLFrameImporter::AMLFrameImporter(){
		return;
	}

	ATLAS::Model::Folder& AMLFrameImporter::createParentHierarchy(Poco::XML::Node* node, ATLAS::Model::Folder& root){
		std::stack<Poco::XML::Node*> parentNodeStack;
		buildHierarchyStack(node, parentNodeStack);
		return createFolderStructureForStack(parentNodeStack, root);
	}

	void AMLFrameImporter::buildHierarchyStack(const Poco::XML::Node* node, std::stack<Poco::XML::Node*>& parentNodes) {
		Poco::XML::Node* transformationRoot((Poco::XML::Node*) node);
		while (transformationRoot->localName() == "InternalElement" || transformationRoot->localName() == "ExternalInterface") {
			// Walk our way up the InternalElement hierarchy and add each node to a stack. Later we use the stack to
			// walk back down the tree to build the transformation matrices and folder structure
			parentNodes.push(transformationRoot);
			transformationRoot = transformationRoot->parentNode();
		}
	}

	ATLAS::Model::Folder& AMLFrameImporter::createFolderStructureForStack(std::stack<Poco::XML::Node*>& parentNodeStack, ATLAS::Model::Folder& root) {
		Poco::XML::Node* currentNode = parentNodeStack.top();
		ATLAS::Model::Folder& currentFolder = getFolderForNode(currentNode, root);
		parentNodeStack.pop();
		if (parentNodeStack.empty()) {
			return currentFolder;
		}
		else {
			return createFolderStructureForStack(parentNodeStack, currentFolder);
		}
	}

	std::string AMLFrameImporter::getNameAttributeForNode(Poco::XML::Node* node) {
		std::string name = node->nodeName();
		Poco::XML::AutoPtr<Poco::XML::NamedNodeMap> attributes = node->attributes();
		Poco::XML::Node* nameNode = attributes->getNamedItem("Name");
		if (nameNode == nullptr) {
			nameNode = attributes->getNamedItem("name");
		}
		if (nameNode != nullptr) {
			name = nameNode->getNodeValue();
		}
		attributes->release();
		return name;
	}

	ATLAS::Model::Folder& AMLFrameImporter::getFolderForNode(Poco::XML::Node* node, ATLAS::Model::Folder& parentFolder) {
		auto visited = visitedNodes.find(node);
		if (visited != visitedNodes.end()) {
			return *visited->second; //Already created a folder for this node so just return that one
		}
		else {
			ATLAS::Model::Folder& folderForNode = createFolderForNode(node, parentFolder);
			Poco::XML::Node* frame = node->getNodeByPath("/Attribute[@Name='Frame']");
			aiMatrix4x4 localTransform = createFrameTransformMatrix(frame);
			addTransformBlobToFolder(localTransform, folderForNode, "transform");
			return folderForNode;
		}
	}

	ATLAS::Model::Folder& AMLFrameImporter::createFolderForNode(Poco::XML::Node* node, ATLAS::Model::Folder& parentFolder) {
		ATLAS::Model::Folder& nodeFolder = parentFolder.appendChild("node");
		nodeFolder.setName(getNameAttributeForNode(node));
		std::pair<Poco::XML::Node*, ATLAS::Model::Folder*> toInsert = std::make_pair(node, &nodeFolder);
		visitedNodes.insert(toInsert);
		return nodeFolder;
	}

	void AMLFrameImporter::addTransformBlobToFolder(const aiMatrix4x4& transform, ATLAS::Model::Folder& folder, const std::string& transformType) {
		aiMatrix4x4* transformCopy = new aiMatrix4x4(transform);
		DataDeletingBlob<aiMatrix4x4> transformBlob(transformCopy);
		folder.addBlob(transformType, transformBlob);
	}

	aiMatrix4x4 AMLFrameImporter::createFrameTransformMatrix(const Poco::XML::Node* frame) {
		if (frame == nullptr) {
			aiMatrix4x4 identity;
			return identity;
		}
		try {
			double tx = ::atof(frame->getNodeByPath("/Attribute[@Name='x']/Value")->innerText().c_str());
			double ty = ::atof(frame->getNodeByPath("/Attribute[@Name='y']/Value")->innerText().c_str());
			double tz = ::atof(frame->getNodeByPath("/Attribute[@Name='z']/Value")->innerText().c_str());
			double rx = ::atof(frame->getNodeByPath("/Attribute[@Name='rx']/Value")->innerText().c_str());
			double ry = ::atof(frame->getNodeByPath("/Attribute[@Name='ry']/Value")->innerText().c_str());
			double rz = ::atof(frame->getNodeByPath("/Attribute[@Name='rz']/Value")->innerText().c_str());
			const double toRad = M_PI / 180;
			aiMatrix4x4 temp;
			aiMatrix4x4 localTransform;
			// Due to the associativity of matrix muliplication, read the following bottom-up,
			// i.e. start in y-up, go to z-up, rotate x, then y, then z, translate, back to y-up
			localTransform *= aiMatrix4x4(
			                      1,  0,  0,  0,
			                      0,  0,  1,  0,
			                      0, -1,  0,  0,
			                      0,  0,  0,  1); // z-up to y-up
			localTransform *= aiMatrix4x4::Translation(aiVector3D(tx,ty,tz),temp);
			localTransform *= aiMatrix4x4::RotationZ(rz*toRad, temp);
			localTransform *= aiMatrix4x4::RotationY(ry*toRad, temp);
			localTransform *= aiMatrix4x4::RotationX(rx*toRad, temp);
			localTransform *= aiMatrix4x4(
			                      1,  0,  0,  0,
			                      0,  0, -1,  0,
			                      0,  1,  0,  0,
			                      0,  0,  0,  1); // y-up to z-up
			return localTransform;
		}
		catch (const Exception& e) {
			throw AMLException("Syntax error while parsing a Frame attribute. This transformation will be ignored.");
		}
	}
}
<|endoftext|>


<commit_before>//===--- swift-demangle.cpp - Swift Demangler app -------------------------===//
//
// This source file is part of the Swift.org open source project
//
// Copyright (c) 2014 - 2016 Apple Inc. and the Swift project authors
// Licensed under Apache License v2.0 with Runtime Library Exception
//
// See https://swift.org/LICENSE.txt for license information
// See https://swift.org/CONTRIBUTORS.txt for the list of Swift project authors
//
//===----------------------------------------------------------------------===//
//
// This is the entry point.
//
//===----------------------------------------------------------------------===//

#include "swift/Basic/DemangleWrappers.h"
#include "swift/Basic/ManglingMacros.h"
#include "llvm/ADT/SmallString.h"
#include "llvm/Support/CommandLine.h"
#include "llvm/Support/MemoryBuffer.h"
#include "llvm/Support/PrettyStackTrace.h"
#include "llvm/Support/Regex.h"
#include "llvm/Support/Signals.h"
#include "llvm/Support/raw_ostream.h"

#include <cstdlib>
#include <string>
#if !defined(_MSC_VER) && !defined(__MINGW32__)
#include <unistd.h>
#else
#include <io.h>
#endif

static llvm::cl::opt<bool>
ExpandMode("expand",
               llvm::cl::desc("Expand mode (show node structure of the demangling)"));

static llvm::cl::opt<bool>
CompactMode("compact",
          llvm::cl::desc("Compact mode (only emit the demangled names)"));

static llvm::cl::opt<bool>
TreeOnly("tree-only",
           llvm::cl::desc("Tree-only mode (do not show the demangled string)"));

static llvm::cl::opt<bool>
RemangleMode("test-remangle",
           llvm::cl::desc("Remangle test mode (show the remangled string)"));

static llvm::cl::opt<bool>
DisableSugar("no-sugar",
           llvm::cl::desc("No sugar mode (disable common language idioms such as ? and [] from the output)"));

static llvm::cl::opt<bool>
Simplified("simplified",
           llvm::cl::desc("Don't display module names or implicit self types"));

static llvm::cl::list<std::string>
InputNames(llvm::cl::Positional, llvm::cl::desc("[mangled name...]"),
               llvm::cl::ZeroOrMore);

static llvm::StringRef substrBefore(llvm::StringRef whole,
                                    llvm::StringRef part) {
  return whole.slice(0, part.data() - whole.data());
}

static llvm::StringRef substrAfter(llvm::StringRef whole,
                                   llvm::StringRef part) {
  return whole.substr((part.data() - whole.data()) + part.size());
}

static void demangle(llvm::raw_ostream &os, llvm::StringRef name,
                     const swift::Demangle::DemangleOptions &options) {
  bool hadLeadingUnderscore = false;
  if (name.startswith("__")) {
    hadLeadingUnderscore = true;
    name = name.substr(1);
  }
  swift::Demangle::NodePointer pointer =
      swift::demangle_wrappers::demangleSymbolAsNode(name);
  if (ExpandMode || TreeOnly) {
    llvm::outs() << "Demangling for " << name << '\n';
    swift::demangle_wrappers::NodeDumper(pointer).print(llvm::outs());
  }
  if (RemangleMode) {
    std::string remangled;
    if (!pointer) {
      // Just reprint the original mangled name if it didn't demangle.
      // This makes it easier to share the same database between the
      // mangling and demangling tests.
      remangled = name;
    } else {
      remangled = swift::Demangle::mangleNode(pointer,
                                        /*NewMangling*/ name.startswith("_S"));
      if (name != remangled) {
        llvm::errs() << "\nError: re-mangled name \n  " << remangled
                     << "\ndoes not match original name\n  " << name << '\n';
        exit(1);
      }
    }
    if (hadLeadingUnderscore) llvm::outs() << '_';
    llvm::outs() << remangled;
    return;
  }
  if (!TreeOnly) {
    std::string string = swift::Demangle::nodeToString(pointer, options);
    if (!CompactMode)
      llvm::outs() << name << " ---> ";
    llvm::outs() << (string.empty() ? name : llvm::StringRef(string));
  }
}

static int demangleSTDIN(const swift::Demangle::DemangleOptions &options) {
  // This doesn't handle Unicode symbols, but maybe that's okay.
  llvm::Regex maybeSymbol("(_T|" MANGLING_PREFIX_STR ")[_a-zA-Z0-9$]+");

  while (true) {
    char *inputLine = nullptr;
    size_t size;
    if (getline(&inputLine, &size, stdin) == -1 || size <= 0) {
      if (errno == 0) {
        break;
      }

      return EXIT_FAILURE;
    }

    llvm::StringRef inputContents(inputLine);
    llvm::SmallVector<llvm::StringRef, 1> matches;
    while (maybeSymbol.match(inputContents, &matches)) {
      llvm::outs() << substrBefore(inputContents, matches.front());
      demangle(llvm::outs(), matches.front(), options);
      inputContents = substrAfter(inputContents, matches.front());
    }

    llvm::outs() << inputContents;
    free(inputLine);
  }

  return EXIT_SUCCESS;
}

int main(int argc, char **argv) {
#if defined(__CYGWIN__)
  // Cygwin clang 3.5.2 with '-O3' generates CRASHING BINARY,
  // if main()'s first function call is passing argv[0].
  std::rand();
#endif
  llvm::cl::ParseCommandLineOptions(argc, argv);

  swift::Demangle::DemangleOptions options;
  options.SynthesizeSugarOnTypes = !DisableSugar;
  if (Simplified)
    options = swift::Demangle::DemangleOptions::SimplifiedUIDemangleOptions();

  if (InputNames.empty()) {
    CompactMode = true;
    return demangleSTDIN(options);
  } else {
    for (llvm::StringRef name : InputNames) {
      demangle(llvm::outs(), name, options);
      llvm::outs() << '\n';
    }

    return EXIT_SUCCESS;
  }
}
<commit_msg>swift-demangle: don’t hardcode the mangling prefix when processing the -test-remangle option<commit_after>//===--- swift-demangle.cpp - Swift Demangler app -------------------------===//
//
// This source file is part of the Swift.org open source project
//
// Copyright (c) 2014 - 2016 Apple Inc. and the Swift project authors
// Licensed under Apache License v2.0 with Runtime Library Exception
//
// See https://swift.org/LICENSE.txt for license information
// See https://swift.org/CONTRIBUTORS.txt for the list of Swift project authors
//
//===----------------------------------------------------------------------===//
//
// This is the entry point.
//
//===----------------------------------------------------------------------===//

#include "swift/Basic/DemangleWrappers.h"
#include "swift/Basic/ManglingMacros.h"
#include "llvm/ADT/SmallString.h"
#include "llvm/Support/CommandLine.h"
#include "llvm/Support/MemoryBuffer.h"
#include "llvm/Support/PrettyStackTrace.h"
#include "llvm/Support/Regex.h"
#include "llvm/Support/Signals.h"
#include "llvm/Support/raw_ostream.h"

#include <cstdlib>
#include <string>
#if !defined(_MSC_VER) && !defined(__MINGW32__)
#include <unistd.h>
#else
#include <io.h>
#endif

static llvm::cl::opt<bool>
ExpandMode("expand",
               llvm::cl::desc("Expand mode (show node structure of the demangling)"));

static llvm::cl::opt<bool>
CompactMode("compact",
          llvm::cl::desc("Compact mode (only emit the demangled names)"));

static llvm::cl::opt<bool>
TreeOnly("tree-only",
           llvm::cl::desc("Tree-only mode (do not show the demangled string)"));

static llvm::cl::opt<bool>
RemangleMode("test-remangle",
           llvm::cl::desc("Remangle test mode (show the remangled string)"));

static llvm::cl::opt<bool>
DisableSugar("no-sugar",
           llvm::cl::desc("No sugar mode (disable common language idioms such as ? and [] from the output)"));

static llvm::cl::opt<bool>
Simplified("simplified",
           llvm::cl::desc("Don't display module names or implicit self types"));

static llvm::cl::list<std::string>
InputNames(llvm::cl::Positional, llvm::cl::desc("[mangled name...]"),
               llvm::cl::ZeroOrMore);

static llvm::StringRef substrBefore(llvm::StringRef whole,
                                    llvm::StringRef part) {
  return whole.slice(0, part.data() - whole.data());
}

static llvm::StringRef substrAfter(llvm::StringRef whole,
                                   llvm::StringRef part) {
  return whole.substr((part.data() - whole.data()) + part.size());
}

static void demangle(llvm::raw_ostream &os, llvm::StringRef name,
                     const swift::Demangle::DemangleOptions &options) {
  bool hadLeadingUnderscore = false;
  if (name.startswith("__")) {
    hadLeadingUnderscore = true;
    name = name.substr(1);
  }
  swift::Demangle::NodePointer pointer =
      swift::demangle_wrappers::demangleSymbolAsNode(name);
  if (ExpandMode || TreeOnly) {
    llvm::outs() << "Demangling for " << name << '\n';
    swift::demangle_wrappers::NodeDumper(pointer).print(llvm::outs());
  }
  if (RemangleMode) {
    std::string remangled;
    if (!pointer) {
      // Just reprint the original mangled name if it didn't demangle.
      // This makes it easier to share the same database between the
      // mangling and demangling tests.
      remangled = name;
    } else {
      remangled = swift::Demangle::mangleNode(pointer,
                          /*NewMangling*/ name.startswith(MANGLING_PREFIX_STR));
      if (name != remangled) {
        llvm::errs() << "\nError: re-mangled name \n  " << remangled
                     << "\ndoes not match original name\n  " << name << '\n';
        exit(1);
      }
    }
    if (hadLeadingUnderscore) llvm::outs() << '_';
    llvm::outs() << remangled;
    return;
  }
  if (!TreeOnly) {
    std::string string = swift::Demangle::nodeToString(pointer, options);
    if (!CompactMode)
      llvm::outs() << name << " ---> ";
    llvm::outs() << (string.empty() ? name : llvm::StringRef(string));
  }
}

static int demangleSTDIN(const swift::Demangle::DemangleOptions &options) {
  // This doesn't handle Unicode symbols, but maybe that's okay.
  llvm::Regex maybeSymbol("(_T|" MANGLING_PREFIX_STR ")[_a-zA-Z0-9$]+");

  while (true) {
    char *inputLine = nullptr;
    size_t size;
    if (getline(&inputLine, &size, stdin) == -1 || size <= 0) {
      if (errno == 0) {
        break;
      }

      return EXIT_FAILURE;
    }

    llvm::StringRef inputContents(inputLine);
    llvm::SmallVector<llvm::StringRef, 1> matches;
    while (maybeSymbol.match(inputContents, &matches)) {
      llvm::outs() << substrBefore(inputContents, matches.front());
      demangle(llvm::outs(), matches.front(), options);
      inputContents = substrAfter(inputContents, matches.front());
    }

    llvm::outs() << inputContents;
    free(inputLine);
  }

  return EXIT_SUCCESS;
}

int main(int argc, char **argv) {
#if defined(__CYGWIN__)
  // Cygwin clang 3.5.2 with '-O3' generates CRASHING BINARY,
  // if main()'s first function call is passing argv[0].
  std::rand();
#endif
  llvm::cl::ParseCommandLineOptions(argc, argv);

  swift::Demangle::DemangleOptions options;
  options.SynthesizeSugarOnTypes = !DisableSugar;
  if (Simplified)
    options = swift::Demangle::DemangleOptions::SimplifiedUIDemangleOptions();

  if (InputNames.empty()) {
    CompactMode = true;
    return demangleSTDIN(options);
  } else {
    for (llvm::StringRef name : InputNames) {
      demangle(llvm::outs(), name, options);
      llvm::outs() << '\n';
    }

    return EXIT_SUCCESS;
  }
}
<|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: Ioan Sucan */

#include "ompl/geometric/planners/rrt/RRTConnect.h"
#include "ompl/datastructures/NearestNeighborsSqrtApprox.h"
#include "ompl/base/GoalSampleableRegion.h"

void ompl::geometric::RRTConnect::setup(void)
{
    Planner::setup();
    checkMotionLength(this, maxDistance_);

    if (!tStart_)
        tStart_.reset(new NearestNeighborsSqrtApprox<Motion*>());
    if (!tGoal_)
        tGoal_.reset(new NearestNeighborsSqrtApprox<Motion*>());
    tStart_->setDistanceFunction(boost::bind(&RRTConnect::distanceFunction, this, _1, _2));
    tGoal_->setDistanceFunction(boost::bind(&RRTConnect::distanceFunction, this, _1, _2));
}

void ompl::geometric::RRTConnect::freeMemory(void)
{
    std::vector<Motion*> motions;

    if (tStart_)
    {
        tStart_->list(motions);
        for (unsigned int i = 0 ; i < motions.size() ; ++i)
        {
            if (motions[i]->state)
                si_->freeState(motions[i]->state);
            delete motions[i];
        }
    }

    if (tGoal_)
    {
        tGoal_->list(motions);
        for (unsigned int i = 0 ; i < motions.size() ; ++i)
        {
            if (motions[i]->state)
                si_->freeState(motions[i]->state);
            delete motions[i];
        }
    }
}

void ompl::geometric::RRTConnect::clear(void)
{
    Planner::clear();
    sampler_.reset();
    freeMemory();
    if (tStart_)
        tStart_->clear();
    if (tStart_)
        tGoal_->clear();
}

ompl::geometric::RRTConnect::GrowState ompl::geometric::RRTConnect::growTree(TreeData &tree, TreeGrowingInfo &tgi, Motion *rmotion)
{
    /* find closest state in the tree */
    Motion *nmotion = tree->nearest(rmotion);

    /* assume we can reach the state we go towards */
    bool reach = true;

    /* find state to add */
    base::State *dstate = rmotion->state;
    double d = si_->distance(nmotion->state, rmotion->state);
    if (d > maxDistance_)
    {
        si_->getStateManifold()->interpolate(nmotion->state, rmotion->state, maxDistance_ / d, tgi.xstate);
        dstate = tgi.xstate;
        reach = false;
    }

    if (si_->checkMotion(nmotion->state, dstate))
    {
        /* create a motion */
        Motion *motion = new Motion(si_);
        si_->copyState(motion->state, dstate);
        motion->parent = nmotion;
        motion->root = nmotion->root;
        tgi.xmotion = motion;

        tree->add(motion);
        if (reach)
            return REACHED;
        else
            return ADVANCED;
    }
    else
        return TRAPPED;
}

bool ompl::geometric::RRTConnect::solve(const base::PlannerTerminationCondition &ptc)
{
    checkValidity();
    base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());

    if (!goal)
    {
        msg_.error("Unknown type of goal (or goal undefined)");
        return false;
    }

    while (const base::State *st = pis_.nextStart())
    {
        Motion *motion = new Motion(si_);
        si_->copyState(motion->state, st);
        motion->root = st;
        tStart_->add(motion);
    }

    if (tStart_->size() == 0)
    {
        msg_.error("Motion planning start tree could not be initialized!");
        return false;
    }

    if (!goal->canSample())
    {
        msg_.error("Insufficient states in sampleable goal region");
        return false;
    }

    if (!sampler_)
        sampler_ = si_->allocManifoldStateSampler();

    msg_.inform("Starting with %d states", (int)(tStart_->size() + tGoal_->size()));

    TreeGrowingInfo tgi;
    tgi.xstate = si_->allocState();

    Motion   *rmotion   = new Motion(si_);
    base::State *rstate = rmotion->state;
    bool   startTree    = true;

    while (ptc() == false)
    {
        TreeData &tree      = startTree ? tStart_ : tGoal_;
        startTree = !startTree;
        TreeData &otherTree = startTree ? tStart_ : tGoal_;

        if (tGoal_->size() == 0 || pis_.getSampledGoalsCount() < tGoal_->size() / 2)
        {
            const base::State *st = tGoal_->size() == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
            if (st)
            {
                Motion* motion = new Motion(si_);
                si_->copyState(motion->state, st);
                motion->root = motion->state;
                tGoal_->add(motion);
            }

            if (tGoal_->size() == 0)
            {
                msg_.error("Unable to sample any valid states for goal tree");
                break;
            }
        }

        /* sample random state */
        sampler_->sampleUniform(rstate);

        GrowState gs = growTree(tree, tgi, rmotion);

        if (gs != TRAPPED)
        {
            /* remember which motion was just added */
            Motion *addedMotion = tgi.xmotion;

            /* attempt to connect trees */

            /* if reached, it means we used rstate directly, no need top copy again */
            if (gs != REACHED)
                si_->copyState(rstate, tgi.xstate);

            GrowState gsc = ADVANCED;
            while (gsc == ADVANCED)
                gsc = growTree(otherTree, tgi, rmotion);

            /* if we connected the trees in a valid way (start and goal pair is valid)*/
            if (gsc == REACHED && goal->isStartGoalPairValid(startTree ? tgi.xmotion->root : addedMotion->root,
                                                             startTree ? addedMotion->root : tgi.xmotion->root))
            {
                /* construct the solution path */
                Motion *solution = tgi.xmotion;
                std::vector<Motion*> mpath1;
                while (solution != NULL)
                {
                    mpath1.push_back(solution);
                    solution = solution->parent;
                }

                solution = addedMotion;
                std::vector<Motion*> mpath2;
                while (solution != NULL)
                {
                    mpath2.push_back(solution);
                    solution = solution->parent;
                }

                if (!startTree)
                    mpath2.swap(mpath1);

                std::vector<Motion*> sol;
                for (int i = mpath1.size() - 1 ; i >= 0 ; --i)
                    sol.push_back(mpath1[i]);
                sol.insert(sol.end(), mpath2.begin(), mpath2.end());

                PathGeometric *path = new PathGeometric(si_);
                for (unsigned int i = 0 ; i < sol.size() ; ++i)
                    path->states.push_back(si_->cloneState(sol[i]->state));

                goal->setDifference(0.0);
                goal->setSolutionPath(base::PathPtr(path));
                break;
            }
        }
    }

    si_->freeState(tgi.xstate);
    si_->freeState(rstate);
    delete rmotion;

    msg_.inform("Created %u states (%u start + %u goal)", tStart_->size() + tGoal_->size(), tStart_->size(), tGoal_->size());

    return goal->isAchieved();
}

void ompl::geometric::RRTConnect::getPlannerData(base::PlannerData &data) const
{
    Planner::getPlannerData(data);

    std::vector<Motion*> motions;
    if (tStart_)
        tStart_->list(motions);

    for (unsigned int i = 0 ; i < motions.size() ; ++i)
        data.recordEdge(motions[i]->parent ? motions[i]->parent->state : NULL, motions[i]->state);

    motions.clear();
    if (tGoal_)
        tGoal_->list(motions);

    for (unsigned int i = 0 ; i < motions.size() ; ++i)
        data.recordEdge(motions[i]->parent ? motions[i]->parent->state : NULL, motions[i]->state);
}
<commit_msg>fixing typo for RRTConnect<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: Ioan Sucan */

#include "ompl/geometric/planners/rrt/RRTConnect.h"
#include "ompl/datastructures/NearestNeighborsSqrtApprox.h"
#include "ompl/base/GoalSampleableRegion.h"

void ompl::geometric::RRTConnect::setup(void)
{
    Planner::setup();
    checkMotionLength(this, maxDistance_);

    if (!tStart_)
        tStart_.reset(new NearestNeighborsSqrtApprox<Motion*>());
    if (!tGoal_)
        tGoal_.reset(new NearestNeighborsSqrtApprox<Motion*>());
    tStart_->setDistanceFunction(boost::bind(&RRTConnect::distanceFunction, this, _1, _2));
    tGoal_->setDistanceFunction(boost::bind(&RRTConnect::distanceFunction, this, _1, _2));
}

void ompl::geometric::RRTConnect::freeMemory(void)
{
    std::vector<Motion*> motions;

    if (tStart_)
    {
        tStart_->list(motions);
        for (unsigned int i = 0 ; i < motions.size() ; ++i)
        {
            if (motions[i]->state)
                si_->freeState(motions[i]->state);
            delete motions[i];
        }
    }

    if (tGoal_)
    {
        tGoal_->list(motions);
        for (unsigned int i = 0 ; i < motions.size() ; ++i)
        {
            if (motions[i]->state)
                si_->freeState(motions[i]->state);
            delete motions[i];
        }
    }
}

void ompl::geometric::RRTConnect::clear(void)
{
    Planner::clear();
    sampler_.reset();
    freeMemory();
    if (tStart_)
        tStart_->clear();
    if (tGoal_)
        tGoal_->clear();
}

ompl::geometric::RRTConnect::GrowState ompl::geometric::RRTConnect::growTree(TreeData &tree, TreeGrowingInfo &tgi, Motion *rmotion)
{
    /* find closest state in the tree */
    Motion *nmotion = tree->nearest(rmotion);

    /* assume we can reach the state we go towards */
    bool reach = true;

    /* find state to add */
    base::State *dstate = rmotion->state;
    double d = si_->distance(nmotion->state, rmotion->state);
    if (d > maxDistance_)
    {
        si_->getStateManifold()->interpolate(nmotion->state, rmotion->state, maxDistance_ / d, tgi.xstate);
        dstate = tgi.xstate;
        reach = false;
    }

    if (si_->checkMotion(nmotion->state, dstate))
    {
        /* create a motion */
        Motion *motion = new Motion(si_);
        si_->copyState(motion->state, dstate);
        motion->parent = nmotion;
        motion->root = nmotion->root;
        tgi.xmotion = motion;

        tree->add(motion);
        if (reach)
            return REACHED;
        else
            return ADVANCED;
    }
    else
        return TRAPPED;
}

bool ompl::geometric::RRTConnect::solve(const base::PlannerTerminationCondition &ptc)
{
    checkValidity();
    base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());

    if (!goal)
    {
        msg_.error("Unknown type of goal (or goal undefined)");
        return false;
    }

    while (const base::State *st = pis_.nextStart())
    {
        Motion *motion = new Motion(si_);
        si_->copyState(motion->state, st);
        motion->root = st;
        tStart_->add(motion);
    }

    if (tStart_->size() == 0)
    {
        msg_.error("Motion planning start tree could not be initialized!");
        return false;
    }

    if (!goal->canSample())
    {
        msg_.error("Insufficient states in sampleable goal region");
        return false;
    }

    if (!sampler_)
        sampler_ = si_->allocManifoldStateSampler();

    msg_.inform("Starting with %d states", (int)(tStart_->size() + tGoal_->size()));

    TreeGrowingInfo tgi;
    tgi.xstate = si_->allocState();

    Motion   *rmotion   = new Motion(si_);
    base::State *rstate = rmotion->state;
    bool   startTree    = true;

    while (ptc() == false)
    {
        TreeData &tree      = startTree ? tStart_ : tGoal_;
        startTree = !startTree;
        TreeData &otherTree = startTree ? tStart_ : tGoal_;

        if (tGoal_->size() == 0 || pis_.getSampledGoalsCount() < tGoal_->size() / 2)
        {
            const base::State *st = tGoal_->size() == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
            if (st)
            {
                Motion* motion = new Motion(si_);
                si_->copyState(motion->state, st);
                motion->root = motion->state;
                tGoal_->add(motion);
            }

            if (tGoal_->size() == 0)
            {
                msg_.error("Unable to sample any valid states for goal tree");
                break;
            }
        }

        /* sample random state */
        sampler_->sampleUniform(rstate);

        GrowState gs = growTree(tree, tgi, rmotion);

        if (gs != TRAPPED)
        {
            /* remember which motion was just added */
            Motion *addedMotion = tgi.xmotion;

            /* attempt to connect trees */

            /* if reached, it means we used rstate directly, no need top copy again */
            if (gs != REACHED)
                si_->copyState(rstate, tgi.xstate);

            GrowState gsc = ADVANCED;
            while (gsc == ADVANCED)
                gsc = growTree(otherTree, tgi, rmotion);

            /* if we connected the trees in a valid way (start and goal pair is valid)*/
            if (gsc == REACHED && goal->isStartGoalPairValid(startTree ? tgi.xmotion->root : addedMotion->root,
                                                             startTree ? addedMotion->root : tgi.xmotion->root))
            {
                /* construct the solution path */
                Motion *solution = tgi.xmotion;
                std::vector<Motion*> mpath1;
                while (solution != NULL)
                {
                    mpath1.push_back(solution);
                    solution = solution->parent;
                }

                solution = addedMotion;
                std::vector<Motion*> mpath2;
                while (solution != NULL)
                {
                    mpath2.push_back(solution);
                    solution = solution->parent;
                }

                if (!startTree)
                    mpath2.swap(mpath1);

                std::vector<Motion*> sol;
                for (int i = mpath1.size() - 1 ; i >= 0 ; --i)
                    sol.push_back(mpath1[i]);
                sol.insert(sol.end(), mpath2.begin(), mpath2.end());

                PathGeometric *path = new PathGeometric(si_);
                for (unsigned int i = 0 ; i < sol.size() ; ++i)
                    path->states.push_back(si_->cloneState(sol[i]->state));

                goal->setDifference(0.0);
                goal->setSolutionPath(base::PathPtr(path));
                break;
            }
        }
    }

    si_->freeState(tgi.xstate);
    si_->freeState(rstate);
    delete rmotion;

    msg_.inform("Created %u states (%u start + %u goal)", tStart_->size() + tGoal_->size(), tStart_->size(), tGoal_->size());

    return goal->isAchieved();
}

void ompl::geometric::RRTConnect::getPlannerData(base::PlannerData &data) const
{
    Planner::getPlannerData(data);

    std::vector<Motion*> motions;
    if (tStart_)
        tStart_->list(motions);

    for (unsigned int i = 0 ; i < motions.size() ; ++i)
        data.recordEdge(motions[i]->parent ? motions[i]->parent->state : NULL, motions[i]->state);

    motions.clear();
    if (tGoal_)
        tGoal_->list(motions);

    for (unsigned int i = 0 ; i < motions.size() ; ++i)
        data.recordEdge(motions[i]->parent ? motions[i]->parent->state : NULL, motions[i]->state);
}
<|endoftext|>


<commit_before>/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2011, Rice University
*  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 Rice University nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

/* Author: Ioan Sucan */

#include "ompl/tools/spaces/StateSpaceCollection.h"
#include "ompl/util/Exception.h"

const std::string& ompl::StateSpaceCollection::getName(void) const
{
    return name_;
}

void ompl::StateSpaceCollection::setName(const std::string &name)
{
    name_ = name;
}

void ompl::StateSpaceCollection::collect(const std::vector<base::StateSpacePtr> &spaces)
{
    for (std::size_t i = 0 ; i < spaces.size() ; ++i)
        collect(spaces[i]);
}

void ompl::StateSpaceCollection::collect(const base::StateSpacePtr &space)
{
    if (haveSpace(space))
        return;

    msg_.debug("Became aware of space '%s'", space->getName().c_str());

    spaces_.push_back(space);
    if (space->isCompound())
        collect(space->as<base::CompoundStateSpace>()->getSubSpaces());
}

const ompl::base::StateSpacePtr& ompl::StateSpaceCollection::getSpace(const std::string &name) const
{
    for (std::size_t i = 0 ; i < spaces_.size() ; ++i)
        if (spaces_[i]->getName() == name)
            return spaces_[i];
    throw Exception("Collection does not include state space '" + name + "'");
}

bool ompl::StateSpaceCollection::haveSpace(const std::string &name) const
{
    for (std::size_t i = 0 ; i < spaces_.size() ; ++i)
        if (spaces_[i]->getName() == name)
            return true;
    return false;
}

bool ompl::StateSpaceCollection::haveSpace(const base::StateSpacePtr &space) const
{
    return haveSpace(space->getName());
}

const ompl::base::StateSpacePtr& ompl::StateSpaceCollection::combine(const std::vector<base::StateSpacePtr> &components)
{
    std::vector<double> weights(components.size(), 1.0);
    return combine(components, weights);
}

const ompl::base::StateSpacePtr& ompl::StateSpaceCollection::combine(const std::vector<base::StateSpacePtr> &components, const std::vector<bool> &mask)
{
    std::vector<double> weights(components.size(), 1.0);
    return combine(components, mask, weights);
}

const ompl::base::StateSpacePtr& ompl::StateSpaceCollection::combine(const std::vector<base::StateSpacePtr> &components,
                                                                     const std::vector<bool> &mask, const std::vector<double> &weights)
{
    if (components.size() != mask.size())
        throw Exception("Number of components not equal to number of mask bits");
    if (components.size() != weights.size())
        throw Exception("Number of components not equal to number of weights");

    std::vector<base::StateSpacePtr> realComp;
    std::vector<double>              realW;
    for (std::size_t i = 0 ; i < components.size() ; ++i)
        if (mask[i])
        {
            realComp.push_back(components[i]);
            realW.push_back(weights[i]);
        }
    return combine(realComp, realW);
}

const ompl::base::StateSpacePtr& ompl::StateSpaceCollection::combine(const std::vector<base::StateSpacePtr> &components, const std::vector<double> &weights)
{
    if (components.empty())
        throw Exception("No spaces to combine");
    if (components.size() != weights.size())
        throw Exception("Number of components not equal to number of weights");

    collect(components);

    if (components.size() == 1)
        return getSpace(components[0]->getName());

    for (std::size_t i = 0 ; i < spaces_.size() ; ++i)
        if (spaces_[i]->isCompound())
            if (spaces_[i]->as<base::CompoundStateSpace>()->getSubSpaces() == components &&
                spaces_[i]->as<base::CompoundStateSpace>()->getSubSpaceWeights() == weights)
                return spaces_[i];

    base::StateSpacePtr space(new base::CompoundStateSpace(components, weights));
    std::string name = components[0]->getName();
    for (std::size_t i = 1 ; i < components.size() ; ++i)
        name += join_ + components[i]->getName();
    name = prefix_ + name + suffix_;
    if (!name_.empty())
        name = name_ + ":" + name;
    space->setName(name);

    msg_.debug("Created new state space: '%s'", space->getName().c_str());

    spaces_.push_back(space);
    return spaces_.back();
}

void ompl::StateSpaceCollection::setAutomaticNames(const std::string &join, const std::string &prefix, const std::string &suffix)
{
    join_ = join;
    prefix_ = prefix;
    suffix_ = suffix;
}

/// @cond IGNORE
namespace ompl
{
    static void generateBits(std::vector< std::vector<bool> > &options, std::vector<bool> &bits, unsigned int start)
    {
        if (start >= bits.size())
        {
            for (unsigned int i = 0 ; i < bits.size() ; ++i)
                if (bits[i])
                {
                    options.push_back(bits);
                    break;
                }
        }
        else
        {
            bits[start] = false;
            generateBits(options, bits, start + 1);
            bits[start] = true;
            generateBits(options, bits, start + 1);
        }
    }

    static std::vector< std::vector<bool> > generateCombinations(unsigned int size)
    {
        std::vector< std::vector<bool> > options;
        std::vector<bool> bits(size);
        generateBits(options, bits, 0);
        return options;
    }
}
/// @endcond

std::vector<ompl::base::StateSpacePtr> ompl::StateSpaceCollection::allCombinations(const std::vector<base::StateSpacePtr> &components, const std::vector<double> &weights)
{
    std::vector<base::StateSpacePtr> opt;
    const std::vector< std::vector<bool> > &comb = generateCombinations(components.size());
    for (std::size_t i = 0 ; i < comb.size() ; ++i)
        opt.push_back(combine(components, comb[i], weights));
    return opt;
}

std::vector<ompl::base::StateSpacePtr> ompl::StateSpaceCollection::allCombinations(const std::vector<base::StateSpacePtr> &components)
{
    std::vector<double> weights(components.size(), 1.0);
    return allCombinations(components, weights);
}
<commit_msg>minor fixes<commit_after>/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2011, Rice University
*  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 Rice University nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

/* Author: Ioan Sucan */

#include "ompl/tools/spaces/StateSpaceCollection.h"
#include "ompl/util/Exception.h"

const std::string& ompl::StateSpaceCollection::getName(void) const
{
    return name_;
}

void ompl::StateSpaceCollection::setName(const std::string &name)
{
    name_ = name;
}

void ompl::StateSpaceCollection::collect(const std::vector<base::StateSpacePtr> &spaces)
{
    for (std::size_t i = 0 ; i < spaces.size() ; ++i)
        collect(spaces[i]);
}

void ompl::StateSpaceCollection::collect(const base::StateSpacePtr &space)
{
    if (!space || haveSpace(space))
        return;

    msg_.debug("Became aware of space '%s'", space->getName().c_str());

    spaces_.push_back(space);
    if (space->isCompound())
        collect(space->as<base::CompoundStateSpace>()->getSubSpaces());
}

const ompl::base::StateSpacePtr& ompl::StateSpaceCollection::getSpace(const std::string &name) const
{
    for (std::size_t i = 0 ; i < spaces_.size() ; ++i)
        if (spaces_[i]->getName() == name)
            return spaces_[i];
    throw Exception("Collection does not include state space '" + name + "'");
}

bool ompl::StateSpaceCollection::haveSpace(const std::string &name) const
{
    for (std::size_t i = 0 ; i < spaces_.size() ; ++i)
        if (spaces_[i]->getName() == name)
            return true;
    return false;
}

bool ompl::StateSpaceCollection::haveSpace(const base::StateSpacePtr &space) const
{
    if (space)
        return haveSpace(space->getName());
    return false;
}

const ompl::base::StateSpacePtr& ompl::StateSpaceCollection::combine(const std::vector<base::StateSpacePtr> &components)
{
    std::vector<double> weights(components.size(), 1.0);
    return combine(components, weights);
}

const ompl::base::StateSpacePtr& ompl::StateSpaceCollection::combine(const std::vector<base::StateSpacePtr> &components, const std::vector<bool> &mask)
{
    std::vector<double> weights(components.size(), 1.0);
    return combine(components, mask, weights);
}

const ompl::base::StateSpacePtr& ompl::StateSpaceCollection::combine(const std::vector<base::StateSpacePtr> &components,
                                                                     const std::vector<bool> &mask, const std::vector<double> &weights)
{
    if (components.size() != mask.size())
        throw Exception("Number of components not equal to number of mask bits");
    if (components.size() != weights.size())
        throw Exception("Number of components not equal to number of weights");

    std::vector<base::StateSpacePtr> realComp;
    std::vector<double>              realW;
    for (std::size_t i = 0 ; i < components.size() ; ++i)
        if (mask[i])
        {
            realComp.push_back(components[i]);
            realW.push_back(weights[i]);
        }
    return combine(realComp, realW);
}

const ompl::base::StateSpacePtr& ompl::StateSpaceCollection::combine(const std::vector<base::StateSpacePtr> &components, const std::vector<double> &weights)
{
    if (components.empty())
        throw Exception("No spaces to combine");
    if (components.size() != weights.size())
        throw Exception("Number of components not equal to number of weights");

    collect(components);

    if (components.size() == 1)
        return getSpace(components[0]->getName());

    for (std::size_t i = 0 ; i < spaces_.size() ; ++i)
        if (spaces_[i]->isCompound())
            if (spaces_[i]->as<base::CompoundStateSpace>()->getSubSpaces() == components &&
                spaces_[i]->as<base::CompoundStateSpace>()->getSubSpaceWeights() == weights)
                return spaces_[i];

    base::StateSpacePtr space(new base::CompoundStateSpace(components, weights));
    std::string name = components[0]->getName();
    for (std::size_t i = 1 ; i < components.size() ; ++i)
        name += join_ + components[i]->getName();
    name = prefix_ + name + suffix_;
    if (!name_.empty())
        name = name_ + ":" + name;
    space->setName(name);

    msg_.debug("Created new state space: '%s'", space->getName().c_str());

    spaces_.push_back(space);
    return spaces_.back();
}

void ompl::StateSpaceCollection::setAutomaticNames(const std::string &join, const std::string &prefix, const std::string &suffix)
{
    join_ = join;
    prefix_ = prefix;
    suffix_ = suffix;
}

/// @cond IGNORE
namespace ompl
{
    static void generateBits(std::vector< std::vector<bool> > &options, std::vector<bool> &bits, unsigned int start)
    {
        if (start >= bits.size())
        {
            for (unsigned int i = 0 ; i < bits.size() ; ++i)
                if (bits[i])
                {
                    options.push_back(bits);
                    break;
                }
        }
        else
        {
            bits[start] = false;
            generateBits(options, bits, start + 1);
            bits[start] = true;
            generateBits(options, bits, start + 1);
        }
    }

    static std::vector< std::vector<bool> > generateCombinations(unsigned int size)
    {
        std::vector< std::vector<bool> > options;
        std::vector<bool> bits(size);
        generateBits(options, bits, 0);
        return options;
    }
}
/// @endcond

std::vector<ompl::base::StateSpacePtr> ompl::StateSpaceCollection::allCombinations(const std::vector<base::StateSpacePtr> &components, const std::vector<double> &weights)
{
    std::vector<base::StateSpacePtr> opt;
    const std::vector< std::vector<bool> > &comb = generateCombinations(components.size());
    for (std::size_t i = 0 ; i < comb.size() ; ++i)
        opt.push_back(combine(components, comb[i], weights));
    return opt;
}

std::vector<ompl::base::StateSpacePtr> ompl::StateSpaceCollection::allCombinations(const std::vector<base::StateSpacePtr> &components)
{
    std::vector<double> weights(components.size(), 1.0);
    return allCombinations(components, weights);
}
<|endoftext|>


<commit_before>#define EIGEN_USE_THREADS

#include "data_augmentation.h"
#include "tensorflow/core/framework/op_kernel.h"
#include "tensorflow/core/framework/register_types.h"
#include "tensorflow/core/framework/types.h"
#include "tensorflow/core/platform/types.h"

namespace tensorflow {
typedef Eigen::GpuDevice GPUDevice;

template<typename Device>
class DataAugmentation : public OpKernel {
public:

  explicit DataAugmentation(OpKernelConstruction *ctx) : OpKernel(ctx) {
    // Get the crop [height, width] tensor and verify its dimensions
    OP_REQUIRES_OK(ctx, ctx->GetAttr("crop", &crop_));
    OP_REQUIRES(ctx, crop_.size() == 2,
                errors::InvalidArgument("crop must be 2 dimensions"));

    // TODO: Verify params are all the same length

    // Get the tensors for params_a and verify their dimensions
    OP_REQUIRES_OK(ctx, ctx->GetAttr("params_a_name", &params_a_name_));
    OP_REQUIRES_OK(ctx, ctx->GetAttr("params_a_rand_type", &params_a_rand_type_));
    OP_REQUIRES_OK(ctx, ctx->GetAttr("params_a_exp", &params_a_exp_));
    OP_REQUIRES_OK(ctx, ctx->GetAttr("params_a_mean", &params_a_mean_));
    OP_REQUIRES_OK(ctx, ctx->GetAttr("params_a_spread", &params_a_spread_));
    OP_REQUIRES_OK(ctx, ctx->GetAttr("params_a_prob", &params_a_prob_));

    // Get the tensors for params_b and verify their dimensions
    OP_REQUIRES_OK(ctx, ctx->GetAttr("params_b_name", &params_b_name_));
    OP_REQUIRES_OK(ctx, ctx->GetAttr("params_b_rand_type", &params_b_rand_type_));
    OP_REQUIRES_OK(ctx, ctx->GetAttr("params_b_exp", &params_b_exp_));
    OP_REQUIRES_OK(ctx, ctx->GetAttr("params_b_mean", &params_b_mean_));
    OP_REQUIRES_OK(ctx, ctx->GetAttr("params_b_spread", &params_b_spread_));
    OP_REQUIRES_OK(ctx, ctx->GetAttr("params_b_prob", &params_b_prob_));
  }

  void Compute(OpKernelContext *ctx) override {
    // Get the input images
    const Tensor& input_a_t = ctx->input(0);
    const Tensor& input_b_t = ctx->input(1);

    // Allocate the memory for the output images
    Tensor *output_a_t;
    Tensor *output_b_t;

    OP_REQUIRES_OK(ctx, ctx->allocate_output(
                     0,
                     TensorShape({ input_a_t.dim_size(0), crop_[0], crop_[1],
                                   input_a_t.dim_size(3) }), &output_a_t));
    OP_REQUIRES_OK(ctx, ctx->allocate_output(
                     1,
                     TensorShape({ input_b_t.dim_size(0), crop_[0], crop_[1],
                                   input_b_t.dim_size(3) }), &output_b_t));

    // Allocate the memory for the output spatial transforms
    Tensor *spat_transform_t;
    Tensor *inv_spat_transform_t;

    OP_REQUIRES_OK(ctx, ctx->allocate_output(
                     2,
                     TensorShape({ input_a_t.dim_size(0), 6 }),
                     &spat_transform_t));
    OP_REQUIRES_OK(ctx, ctx->allocate_output(
                     3,
                     TensorShape({ input_a_t.dim_size(0), 6 }),
                     &inv_spat_transform_t));

    // Perform data augmentation on image A
    auto input_a                    = input_a_t.tensor<float, 4>();
    auto output_a                   = output_a_t->tensor<float, 4>();
    AugmentationParams aug_params_a = AugmentationParams(crop_[0], crop_[1],
                                                         params_a_name_,
                                                         params_a_rand_type_,
                                                         params_a_exp_,
                                                         params_a_mean_,
                                                         params_a_spread_,
                                                         params_a_prob_);
    std::vector<AugmentationCoeffs> coeffs_a;
    ComputeDataAugmentation(ctx,
                            ctx->eigen_gpu_device(),
                            input_a,
                            aug_params_a,
                            output_a,
                            coeffs_a, // We have no incoming coefficients
                            coeffs_a);

    // Perform data augmentation on image B using coeffs generated above
    auto input_b                    = input_b_t.tensor<float, 4>();
    auto output_b                   = output_b_t->tensor<float, 4>();
    AugmentationParams aug_params_b = AugmentationParams(crop_[0], crop_[1],
                                                         params_b_name_,
                                                         params_b_rand_type_,
                                                         params_b_exp_,
                                                         params_b_mean_,
                                                         params_b_spread_,
                                                         params_b_prob_);
    std::vector<AugmentationCoeffs> coeffs_b;
    ComputeDataAugmentation(ctx,
                            ctx->eigen_gpu_device(),
                            input_b,
                            aug_params_b,
                            output_b,
                            coeffs_a,
                            coeffs_b);

    const int src_height       = input_a.dimension(1);
    const int src_width        = input_a.dimension(2);
    auto inv_transforms_from_a = spat_transform_t->tensor<float, 2>();
    auto transforms_from_b     = inv_spat_transform_t->tensor<float, 2>();

    for (int i = 0; i < coeffs_a.size(); i++) {
      auto coeffs = coeffs_a[i];
      coeffs.store_spatial_matrix(crop_[0],
                                  crop_[1],
                                  src_height,
                                  src_width,
                                  inv_transforms_from_a.data() + i * 6,
                                  true);
    }

    for (int i = 0; i < coeffs_b.size(); i++) {
      auto coeffs = coeffs_b[i];
      coeffs.store_spatial_matrix(crop_[0],
                                  crop_[1],
                                  src_height,
                                  src_width,
                                  transforms_from_b.data() + i * 6,
                                  false);
    }
  }

private:

  std::vector<int32>crop_;

  // Params A
  std::vector<string>params_a_name_;
  std::vector<string>params_a_rand_type_;
  std::vector<bool>params_a_exp_;
  std::vector<float>params_a_mean_;
  std::vector<float>params_a_spread_;
  std::vector<float>params_a_prob_;

  // Params B
  std::vector<string>params_b_name_;
  std::vector<string>params_b_rand_type_;
  std::vector<bool>params_b_exp_;
  std::vector<float>params_b_mean_;
  std::vector<float>params_b_spread_;
  std::vector<float>params_b_prob_;
};

REGISTER_KERNEL_BUILDER(Name("DataAugmentation")
                        .Device(DEVICE_GPU)
                        .HostMemory("spatial_transform_a")
                        .HostMemory("inv_spatial_transform_b"),
                        DataAugmentation<GPUDevice>)
} // end namespace tensorflow
<commit_msg>forgot to change kernel builder<commit_after>#define EIGEN_USE_THREADS

#include "data_augmentation.h"
#include "tensorflow/core/framework/op_kernel.h"
#include "tensorflow/core/framework/register_types.h"
#include "tensorflow/core/framework/types.h"
#include "tensorflow/core/platform/types.h"

namespace tensorflow {
typedef Eigen::GpuDevice GPUDevice;

template<typename Device>
class DataAugmentation : public OpKernel {
public:

  explicit DataAugmentation(OpKernelConstruction *ctx) : OpKernel(ctx) {
    // Get the crop [height, width] tensor and verify its dimensions
    OP_REQUIRES_OK(ctx, ctx->GetAttr("crop", &crop_));
    OP_REQUIRES(ctx, crop_.size() == 2,
                errors::InvalidArgument("crop must be 2 dimensions"));

    // TODO: Verify params are all the same length

    // Get the tensors for params_a and verify their dimensions
    OP_REQUIRES_OK(ctx, ctx->GetAttr("params_a_name", &params_a_name_));
    OP_REQUIRES_OK(ctx, ctx->GetAttr("params_a_rand_type", &params_a_rand_type_));
    OP_REQUIRES_OK(ctx, ctx->GetAttr("params_a_exp", &params_a_exp_));
    OP_REQUIRES_OK(ctx, ctx->GetAttr("params_a_mean", &params_a_mean_));
    OP_REQUIRES_OK(ctx, ctx->GetAttr("params_a_spread", &params_a_spread_));
    OP_REQUIRES_OK(ctx, ctx->GetAttr("params_a_prob", &params_a_prob_));

    // Get the tensors for params_b and verify their dimensions
    OP_REQUIRES_OK(ctx, ctx->GetAttr("params_b_name", &params_b_name_));
    OP_REQUIRES_OK(ctx, ctx->GetAttr("params_b_rand_type", &params_b_rand_type_));
    OP_REQUIRES_OK(ctx, ctx->GetAttr("params_b_exp", &params_b_exp_));
    OP_REQUIRES_OK(ctx, ctx->GetAttr("params_b_mean", &params_b_mean_));
    OP_REQUIRES_OK(ctx, ctx->GetAttr("params_b_spread", &params_b_spread_));
    OP_REQUIRES_OK(ctx, ctx->GetAttr("params_b_prob", &params_b_prob_));
  }

  void Compute(OpKernelContext *ctx) override {
    // Get the input images
    const Tensor& input_a_t = ctx->input(0);
    const Tensor& input_b_t = ctx->input(1);

    // Allocate the memory for the output images
    Tensor *output_a_t;
    Tensor *output_b_t;

    OP_REQUIRES_OK(ctx, ctx->allocate_output(
                     0,
                     TensorShape({ input_a_t.dim_size(0), crop_[0], crop_[1],
                                   input_a_t.dim_size(3) }), &output_a_t));
    OP_REQUIRES_OK(ctx, ctx->allocate_output(
                     1,
                     TensorShape({ input_b_t.dim_size(0), crop_[0], crop_[1],
                                   input_b_t.dim_size(3) }), &output_b_t));

    // Allocate the memory for the output spatial transforms
    Tensor *spat_transform_t;
    Tensor *inv_spat_transform_t;

    OP_REQUIRES_OK(ctx, ctx->allocate_output(
                     2,
                     TensorShape({ input_a_t.dim_size(0), 6 }),
                     &spat_transform_t));
    OP_REQUIRES_OK(ctx, ctx->allocate_output(
                     3,
                     TensorShape({ input_a_t.dim_size(0), 6 }),
                     &inv_spat_transform_t));

    // Perform data augmentation on image A
    auto input_a                    = input_a_t.tensor<float, 4>();
    auto output_a                   = output_a_t->tensor<float, 4>();
    AugmentationParams aug_params_a = AugmentationParams(crop_[0], crop_[1],
                                                         params_a_name_,
                                                         params_a_rand_type_,
                                                         params_a_exp_,
                                                         params_a_mean_,
                                                         params_a_spread_,
                                                         params_a_prob_);
    std::vector<AugmentationCoeffs> coeffs_a;
    ComputeDataAugmentation(ctx,
                            ctx->eigen_gpu_device(),
                            input_a,
                            aug_params_a,
                            output_a,
                            coeffs_a, // We have no incoming coefficients
                            coeffs_a);

    // Perform data augmentation on image B using coeffs generated above
    auto input_b                    = input_b_t.tensor<float, 4>();
    auto output_b                   = output_b_t->tensor<float, 4>();
    AugmentationParams aug_params_b = AugmentationParams(crop_[0], crop_[1],
                                                         params_b_name_,
                                                         params_b_rand_type_,
                                                         params_b_exp_,
                                                         params_b_mean_,
                                                         params_b_spread_,
                                                         params_b_prob_);
    std::vector<AugmentationCoeffs> coeffs_b;
    ComputeDataAugmentation(ctx,
                            ctx->eigen_gpu_device(),
                            input_b,
                            aug_params_b,
                            output_b,
                            coeffs_a,
                            coeffs_b);

    const int src_height       = input_a.dimension(1);
    const int src_width        = input_a.dimension(2);
    auto inv_transforms_from_a = spat_transform_t->tensor<float, 2>();
    auto transforms_from_b     = inv_spat_transform_t->tensor<float, 2>();

    for (int i = 0; i < coeffs_a.size(); i++) {
      auto coeffs = coeffs_a[i];
      coeffs.store_spatial_matrix(crop_[0],
                                  crop_[1],
                                  src_height,
                                  src_width,
                                  inv_transforms_from_a.data() + i * 6,
                                  true);
    }

    for (int i = 0; i < coeffs_b.size(); i++) {
      auto coeffs = coeffs_b[i];
      coeffs.store_spatial_matrix(crop_[0],
                                  crop_[1],
                                  src_height,
                                  src_width,
                                  transforms_from_b.data() + i * 6,
                                  false);
    }
  }

private:

  std::vector<int32>crop_;

  // Params A
  std::vector<string>params_a_name_;
  std::vector<string>params_a_rand_type_;
  std::vector<bool>params_a_exp_;
  std::vector<float>params_a_mean_;
  std::vector<float>params_a_spread_;
  std::vector<float>params_a_prob_;

  // Params B
  std::vector<string>params_b_name_;
  std::vector<string>params_b_rand_type_;
  std::vector<bool>params_b_exp_;
  std::vector<float>params_b_mean_;
  std::vector<float>params_b_spread_;
  std::vector<float>params_b_prob_;
};

REGISTER_KERNEL_BUILDER(Name("DataAugmentation")
                        .Device(DEVICE_GPU)
                        .HostMemory("inv_transforms_from_a")
                        .HostMemory("transforms_from_b"),
                        DataAugmentation<GPUDevice>)
} // end namespace tensorflow
<|endoftext|>


<commit_before>// ======================================================================== //
// Copyright 2009-2018 Intel Corporation                                    //
//                                                                          //
// Licensed under the Apache License, Version 2.0 (the "License");          //
// you may not use this file except in compliance with the License.         //
// You may obtain a copy of the License at                                  //
//                                                                          //
//     http://www.apache.org/licenses/LICENSE-2.0                           //
//                                                                          //
// Unless required by applicable law or agreed to in writing, software      //
// distributed under the License is distributed on an "AS IS" BASIS,        //
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. //
// See the License for the specific language governing permissions and      //
// limitations under the License.                                           //
// ======================================================================== //

#ifdef USE_OPENIMAGEIO

#include "image.h"
#include <iostream>
#include <vector>

/*! include OpenImageIO headers */
#include <OpenImageIO/imageio.h>
OIIO_NAMESPACE_USING

namespace embree
{
  Ref<Image> loadOIIO(const FileName& fileName)
  {
    ImageInput* in = ImageInput::open(fileName.str().c_str());
    if (!in)
      THROW_RUNTIME_ERROR("error opening file " + fileName.str());

    const ImageSpec& spec = in->spec();
    size_t width = spec.width;
    size_t height = spec.height;
    size_t channels = spec.nchannels;
    std::vector<unsigned char> pixels(width*height*channels);
    in->read_image(TypeDesc::UINT8, pixels.data());
    in->close();
    ImageInput::destroy(in);

    Image* out = new Image4uc(width, height, fileName);
    const float rcpMaxColor = 1.f/255.f;
    for (size_t y = 0; y < height; y++)
    {
      for (size_t x = 0; x < width; x++)
      {
        float r = (channels > 0) ? (float(pixels[(y*width+x)*channels+0]) * rcpMaxColor) : 0.f;
        float g = (channels > 1) ? (float(pixels[(y*width+x)*channels+1]) * rcpMaxColor) : r;
        float b = (channels > 2) ? (float(pixels[(y*width+x)*channels+2]) * rcpMaxColor) : ((channels == 1) ? r : 0.f);
        float a = (channels > 3) ? (float(pixels[(y*width+x)*channels+3]) * rcpMaxColor) : 1.f;
        out->set(x, height-y-1, Color4(r,g,b,a)); // flip image
      }
    }

    return out;
  }

  void storeOIIO(const Ref<Image>& img, const FileName& fileName)
  {
    ImageOutput* out = ImageOutput::create(fileName.c_str());
    if (!out) THROW_RUNTIME_ERROR("unsupported output file format " + fileName.str());

    std::vector<unsigned char> pixels(img->width*img->height*3);
    const float maxColor = 255.f;
    for (size_t y = 0; y < img->height; y++)
    {
      for (size_t x = 0; x < img->width; x++)
      {
        Color4 c = img->get(x, y);
        pixels[(y*img->width+x)*3+0] = (unsigned char)(clamp(c.r) * maxColor);
        pixels[(y*img->width+x)*3+1] = (unsigned char)(clamp(c.g) * maxColor);
        pixels[(y*img->width+x)*3+2] = (unsigned char)(clamp(c.b) * maxColor);
      }
    }

    ImageSpec spec(int(img->width), int(img->height), 3, TypeDesc::UINT8);
    if (!out->open(fileName.c_str(), spec))
    {
      ImageOutput::destroy(out);
      THROW_RUNTIME_ERROR("error opening file " + fileName.str());
    }
    out->write_image(TypeDesc::UINT8, pixels.data());
    out->close();
    ImageOutput::destroy(out);
  }
}

#endif // USE_OPENIMAGEIO

<commit_msg>due to older oiio versions using delete instead of ::destry call for the oiio interface<commit_after>// ======================================================================== //
// Copyright 2009-2018 Intel Corporation                                    //
//                                                                          //
// Licensed under the Apache License, Version 2.0 (the "License");          //
// you may not use this file except in compliance with the License.         //
// You may obtain a copy of the License at                                  //
//                                                                          //
//     http://www.apache.org/licenses/LICENSE-2.0                           //
//                                                                          //
// Unless required by applicable law or agreed to in writing, software      //
// distributed under the License is distributed on an "AS IS" BASIS,        //
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. //
// See the License for the specific language governing permissions and      //
// limitations under the License.                                           //
// ======================================================================== //

#ifdef USE_OPENIMAGEIO

#include "image.h"
#include <iostream>
#include <vector>

/*! include OpenImageIO headers */
#include <OpenImageIO/imageio.h>
OIIO_NAMESPACE_USING

namespace embree
{
  Ref<Image> loadOIIO(const FileName& fileName)
  {
    ImageInput* in = ImageInput::open(fileName.str().c_str());
    if (!in)
      THROW_RUNTIME_ERROR("error opening file " + fileName.str());

    const ImageSpec& spec = in->spec();
    size_t width = spec.width;
    size_t height = spec.height;
    size_t channels = spec.nchannels;
    std::vector<unsigned char> pixels(width*height*channels);
    in->read_image(TypeDesc::UINT8, pixels.data());
    in->close();
    delete in;
    //ImageInput::destroy(in);

    Image* out = new Image4uc(width, height, fileName);
    const float rcpMaxColor = 1.f/255.f;
    for (size_t y = 0; y < height; y++)
    {
      for (size_t x = 0; x < width; x++)
      {
        float r = (channels > 0) ? (float(pixels[(y*width+x)*channels+0]) * rcpMaxColor) : 0.f;
        float g = (channels > 1) ? (float(pixels[(y*width+x)*channels+1]) * rcpMaxColor) : r;
        float b = (channels > 2) ? (float(pixels[(y*width+x)*channels+2]) * rcpMaxColor) : ((channels == 1) ? r : 0.f);
        float a = (channels > 3) ? (float(pixels[(y*width+x)*channels+3]) * rcpMaxColor) : 1.f;
        out->set(x, height-y-1, Color4(r,g,b,a)); // flip image
      }
    }

    return out;
  }

  void storeOIIO(const Ref<Image>& img, const FileName& fileName)
  {
    ImageOutput* out = ImageOutput::create(fileName.c_str());
    if (!out) THROW_RUNTIME_ERROR("unsupported output file format " + fileName.str());

    std::vector<unsigned char> pixels(img->width*img->height*3);
    const float maxColor = 255.f;
    for (size_t y = 0; y < img->height; y++)
    {
      for (size_t x = 0; x < img->width; x++)
      {
        Color4 c = img->get(x, y);
        pixels[(y*img->width+x)*3+0] = (unsigned char)(clamp(c.r) * maxColor);
        pixels[(y*img->width+x)*3+1] = (unsigned char)(clamp(c.g) * maxColor);
        pixels[(y*img->width+x)*3+2] = (unsigned char)(clamp(c.b) * maxColor);
      }
    }

    ImageSpec spec(int(img->width), int(img->height), 3, TypeDesc::UINT8);
    if (!out->open(fileName.c_str(), spec))
    {
      delete out;
      //ImageOutput::destroy(out);
      THROW_RUNTIME_ERROR("error opening file " + fileName.str());
    }
    out->write_image(TypeDesc::UINT8, pixels.data());
    out->close();
    //ImageOutput::destroy(out);
    delete out;
  }
}

#endif // USE_OPENIMAGEIO

<|endoftext|>