<commit_before>// ======================================================================== //
// Copyright 2009-2015 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.                                           //
// ======================================================================== //

#include "PLYGeometryFile.h"
#include <fstream>
#include <iostream>
#include <map>
#include <cstdio>

//! Error checking.
void exitOnCondition(bool condition, const std::string &message) { if (condition) throw std::runtime_error("PLYGeometryFile error: " + message + "."); }

//! String comparison helper.
bool startsWith(const std::string &haystack, const std::string &needle) {
  return needle.length() <= haystack.length() && equal(needle.begin(), needle.end(), haystack.begin());
}

PLYGeometryFile::PLYGeometryFile(const std::string &filename) : filename(filename) {

  //! Parse the file.
  exitOnCondition(parse() != true, "error parsing geometry file");
}

OSPTriangleMesh PLYGeometryFile::getOSPTriangleMesh() {

  OSPTriangleMesh triangleMesh = ospNewTriangleMesh();

  OSPData vertexData = ospNewData(vertices.size(), OSP_FLOAT3A, &vertices[0].x);
  ospSetData(triangleMesh, "vertex", vertexData);

  OSPData vertexColorData = ospNewData(vertexColors.size(), OSP_FLOAT3A, &vertexColors[0].x);
  ospSetData(triangleMesh, "vertex.color", vertexColorData);

  OSPData indexData = ospNewData(triangles.size(), OSP_INT3, &triangles[0].x);
  ospSetData(triangleMesh, "index", indexData);

  ospCommit(triangleMesh);

  return triangleMesh;
}

bool PLYGeometryFile::parse() {

  std::ifstream in(filename.c_str());
  exitOnCondition(!in.is_open(), "unable to open geometry file.");

  //! Verify header.
  char line[1024];
  in.getline(line, 1024);
  exitOnCondition(std::string(line) != "ply", "ply header not found.");

  in.getline(line, 1024);
  exitOnCondition(std::string(line) != "format ascii 1.0", "only ascii PLY files are supported.");

  //! Parse the rest of the header line by line.
  int numVertices = 0;
  int numVertexProperties = 0;
  std::map<std::string, int> vertexPropertyToIndex;
  int numFaces = 0;

  while(in.good()) {

    //! Read a new line.
    in.getline(line, 1024);
    std::string lineString(line);

    //! Ignore comments.
    if(startsWith(lineString, "comment"))
      continue;

    //! Vertex header information.
    else if(startsWith(lineString, "element vertex")) {

      //! Number of vertices.
      exitOnCondition(sscanf(line, "element vertex %i", &numVertices) != 1, "could not read number of vertices.");

      //! Read vertex property definitions.
      while(in.good()) {
        std::streampos currentPos = in.tellg();

        in.getline(line, 1024);
        lineString = std::string(line);

        //! Seek backward and break after last property is found.
        if(!startsWith(lineString, "property")) {
          in.seekg(currentPos);
          break;
        }

        //! Read property name.
        char propertyType[64];
        char propertyName[64];
        exitOnCondition(sscanf(line, "property %s %s", propertyType, propertyName) != 2, "could not read vertex property definition.");

        //! Save property information.
        vertexPropertyToIndex[std::string(propertyName)] = numVertexProperties;
        numVertexProperties++;
      }
    }

    //! Face header information.
    else if(startsWith(lineString, "element face")) {

      //! Number of faces.
      exitOnCondition(sscanf(line, "element face %i", &numFaces) != 1, "could not read number of faces.");

      //! Next line should be "property list uchar int vertex_indices"
      in.getline(line, 1024);
      lineString = std::string(line);
      exitOnCondition(!startsWith(lineString, "property list uchar int vertex_indices"), "unexpected face header information.");
    }

    //! Break on end of header.
    else if(startsWith(lineString, "end_header"))
      break;

    //! Unexpected line.
    else
      exitOnCondition(true, "unexpected line in header: " + lineString);
  }

  std::cout << "numVertices = " << numVertices << ", numVertexProperties = " << numVertexProperties << ", numFaces = " << numFaces << std::endl;

  //! Make sure we have the required vertex properties.
  exitOnCondition(!vertexPropertyToIndex.count("x") || !vertexPropertyToIndex.count("y") || !vertexPropertyToIndex.count("z"), "vertex coordinate properties missing.");

  int xIndex = vertexPropertyToIndex["x"];
  int yIndex = vertexPropertyToIndex["y"];
  int zIndex = vertexPropertyToIndex["z"];

  //! See if we have vertex colors.
  bool haveVertexColors = false;
  int rIndex, gIndex, bIndex, aIndex;

  if(vertexPropertyToIndex.count("red") && vertexPropertyToIndex.count("green") && vertexPropertyToIndex.count("blue") && vertexPropertyToIndex.count("alpha")) {

    haveVertexColors = true;
    rIndex = vertexPropertyToIndex["red"];
    gIndex = vertexPropertyToIndex["green"];
    bIndex = vertexPropertyToIndex["blue"];
    aIndex = vertexPropertyToIndex["alpha"];
  }

  //! Read the vertex data.
  for(int i=0; i<numVertices; i++) {

    std::vector<float> vertexProperties;

    for(int j=0; j<numVertexProperties; j++) {
      float value;
      in >> value;
      exitOnCondition(!in.good(), "error reading vertex data.");

      vertexProperties.push_back(value);
    }

    vertices.push_back(osp::vec3fa(vertexProperties[xIndex], vertexProperties[yIndex], vertexProperties[zIndex]));

    if(haveVertexColors)
      vertexColors.push_back(1.f/255.f * osp::vec3fa(vertexProperties[rIndex], vertexProperties[gIndex], vertexProperties[bIndex]));
  }

  //! Read the face data.
  for(int i=0; i<numFaces; i++) {

    int count;
    in >> count;
    exitOnCondition(count != 3, "only triangle faces are supported.");

    osp::vec3i triangle;
    in >> triangle.x >> triangle.y >> triangle.z;
    exitOnCondition(!in.good(), "error reading face data.");

    triangles.push_back(triangle);
  }

  std::cout << "done." << std::endl;

  return true;
}
<commit_msg>fixes for volume viewer PLY loader: ignoring obj_info lines, defaulting to white if no vertex colors present.<commit_after>// ======================================================================== //
// Copyright 2009-2015 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.                                           //
// ======================================================================== //

#include "PLYGeometryFile.h"
#include <fstream>
#include <iostream>
#include <map>
#include <cstdio>

//! Error checking.
void exitOnCondition(bool condition, const std::string &message) { if (condition) throw std::runtime_error("PLYGeometryFile error: " + message + "."); }

//! String comparison helper.
bool startsWith(const std::string &haystack, const std::string &needle) {
  return needle.length() <= haystack.length() && equal(needle.begin(), needle.end(), haystack.begin());
}

PLYGeometryFile::PLYGeometryFile(const std::string &filename) : filename(filename) {

  //! Parse the file.
  exitOnCondition(parse() != true, "error parsing geometry file");
}

OSPTriangleMesh PLYGeometryFile::getOSPTriangleMesh() {

  OSPTriangleMesh triangleMesh = ospNewTriangleMesh();

  OSPData vertexData = ospNewData(vertices.size(), OSP_FLOAT3A, &vertices[0].x);
  ospSetData(triangleMesh, "vertex", vertexData);

  OSPData vertexColorData = ospNewData(vertexColors.size(), OSP_FLOAT3A, &vertexColors[0].x);
  ospSetData(triangleMesh, "vertex.color", vertexColorData);

  OSPData indexData = ospNewData(triangles.size(), OSP_INT3, &triangles[0].x);
  ospSetData(triangleMesh, "index", indexData);

  ospCommit(triangleMesh);

  return triangleMesh;
}

bool PLYGeometryFile::parse() {

  std::ifstream in(filename.c_str());
  exitOnCondition(!in.is_open(), "unable to open geometry file.");

  //! Verify header.
  char line[1024];
  in.getline(line, 1024);
  exitOnCondition(std::string(line) != "ply", "ply header not found.");

  in.getline(line, 1024);
  exitOnCondition(std::string(line) != "format ascii 1.0", "only ascii PLY files are supported.");

  //! Parse the rest of the header line by line.
  int numVertices = 0;
  int numVertexProperties = 0;
  std::map<std::string, int> vertexPropertyToIndex;
  int numFaces = 0;

  while(in.good()) {

    //! Read a new line.
    in.getline(line, 1024);
    std::string lineString(line);

    //! Ignore comment and obj_info lines.
    if(startsWith(lineString, "comment") || startsWith(lineString, "obj_info"))
      continue;

    //! Vertex header information.
    else if(startsWith(lineString, "element vertex")) {

      //! Number of vertices.
      exitOnCondition(sscanf(line, "element vertex %i", &numVertices) != 1, "could not read number of vertices.");

      //! Read vertex property definitions.
      while(in.good()) {
        std::streampos currentPos = in.tellg();

        in.getline(line, 1024);
        lineString = std::string(line);

        //! Seek backward and break after last property is found.
        if(!startsWith(lineString, "property")) {
          in.seekg(currentPos);
          break;
        }

        //! Read property name.
        char propertyType[64];
        char propertyName[64];
        exitOnCondition(sscanf(line, "property %s %s", propertyType, propertyName) != 2, "could not read vertex property definition.");

        //! Save property information.
        vertexPropertyToIndex[std::string(propertyName)] = numVertexProperties;
        numVertexProperties++;
      }
    }

    //! Face header information.
    else if(startsWith(lineString, "element face")) {

      //! Number of faces.
      exitOnCondition(sscanf(line, "element face %i", &numFaces) != 1, "could not read number of faces.");

      //! Next line should be "property list uchar int vertex_indices"
      in.getline(line, 1024);
      lineString = std::string(line);
      exitOnCondition(!startsWith(lineString, "property list uchar int vertex_indices"), "unexpected face header information.");
    }

    //! Break on end of header.
    else if(startsWith(lineString, "end_header"))
      break;

    //! Unexpected line.
    else
      exitOnCondition(true, "unexpected line in header: " + lineString);
  }

  std::cout << "numVertices = " << numVertices << ", numVertexProperties = " << numVertexProperties << ", numFaces = " << numFaces << std::endl;

  //! Make sure we have the required vertex properties.
  exitOnCondition(!vertexPropertyToIndex.count("x") || !vertexPropertyToIndex.count("y") || !vertexPropertyToIndex.count("z"), "vertex coordinate properties missing.");

  int xIndex = vertexPropertyToIndex["x"];
  int yIndex = vertexPropertyToIndex["y"];
  int zIndex = vertexPropertyToIndex["z"];

  //! See if we have vertex colors.
  bool haveVertexColors = false;
  int rIndex, gIndex, bIndex, aIndex;

  if(vertexPropertyToIndex.count("red") && vertexPropertyToIndex.count("green") && vertexPropertyToIndex.count("blue") && vertexPropertyToIndex.count("alpha")) {

    haveVertexColors = true;
    rIndex = vertexPropertyToIndex["red"];
    gIndex = vertexPropertyToIndex["green"];
    bIndex = vertexPropertyToIndex["blue"];
    aIndex = vertexPropertyToIndex["alpha"];
  }

  //! Read the vertex data.
  for(int i=0; i<numVertices; i++) {

    std::vector<float> vertexProperties;

    for(int j=0; j<numVertexProperties; j++) {
      float value;
      in >> value;
      exitOnCondition(!in.good(), "error reading vertex data.");

      vertexProperties.push_back(value);
    }

    vertices.push_back(osp::vec3fa(vertexProperties[xIndex], vertexProperties[yIndex], vertexProperties[zIndex]));

    //! Use vertex colors if we have them; otherwise default to white (note that the volume renderer currently requires a color for every vertex).
    if(haveVertexColors)
      vertexColors.push_back(1.f/255.f * osp::vec3fa(vertexProperties[rIndex], vertexProperties[gIndex], vertexProperties[bIndex]));
    else
      vertexColors.push_back(osp::vec3fa(1.f));
  }

  //! Read the face data.
  for(int i=0; i<numFaces; i++) {

    int count;
    in >> count;
    exitOnCondition(count != 3, "only triangle faces are supported.");

    osp::vec3i triangle;
    in >> triangle.x >> triangle.y >> triangle.z;
    exitOnCondition(!in.good(), "error reading face data.");

    triangles.push_back(triangle);
  }

  std::cout << "done." << std::endl;

  return true;
}
<|endoftext|>


<commit_before>/*
 * Copyright © 2012, United States Government, as represented by the
 * Administrator of the National Aeronautics and Space Administration.
 * All rights reserved.
 *
 * The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is 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.
*/

/**
 * @file SpineOnlineControl.cpp
 * @brief A controller for the template class BaseSpineModelLearning
 * @author Brian Mirletz
 * @version 1.1.0
 * $Id$
 */

#include "SpineOnlineControl.h"

#include "dev/btietz/TC_goal/FlemonsSpineModelGoal.h"

// Should include tgString, but compiler complains since its been
// included from BaseSpineModelLearning. Perhaps we should move things
// to a cpp over there
#include "core/tgSpringCableActuator.h"
#include "core/tgBasicActuator.h"
#include "tgcreator/tgUtil.h"
#include "controllers/tgImpedanceController.h"
#include "examples/learningSpines/tgCPGActuatorControl.h"
#include "dev/CPG_feedback/tgCPGCableControl.h"

#include "helpers/FileHelpers.h"

#include "learning/AnnealEvolution/AnnealEvolution.h"
#include "learning/Configuration/configuration.h"

#include "dev/CPG_feedback/CPGEquationsFB.h"
#include "dev/CPG_feedback/CPGNodeFB.h"

#include <iterator>     // std::iterator
#include <string>

//#define LOGGING
#define USE_KINEMATIC

using namespace std;

SpineOnlineControl::Config::Config(int ss,
                                        int tm,
                                        int om,
                                        int param,
                                        int segnum,
                                        double ct,
                                        double la,
                                        double ha,
                                        double lp,
                                        double hp,
                                        double kt,
                                        double kp,
                                        double kv,
                                        bool def,
                                        double cl,
                                        double lf,
                                        double hf,
                                        double ffMin,
                                        double ffMax,
                                        double afMin,
                                        double afMax,
                                        double pfMin,
                                        double pfMax,
                                        double tf,
                                        double feedTime ) :
SpineGoalControl::Config::Config(ss, tm, om, param, segnum, ct, la, ha,
                                    lp, hp, kt, kp, kv, def, cl, lf, hf,
                                    ffMin, ffMax, afMin, afMax, pfMin, pfMax, tf),
feedbackTime(feedTime)
{

}
/**
 * Defining the adapters here assumes the controller is around and
 * attached for the lifecycle of the learning runs. I.E. that the setup
 * and teardown functions are used for tgModel
 */
SpineOnlineControl::SpineOnlineControl(SpineOnlineControl::Config config,
                                                std::string args,
                                                std::string resourcePath,
                                                std::string ec,
                                                std::string nc,
                                                std::string fc,
                                                std::string gc) :
SpineGoalControl(config, args, resourcePath, ec, nc, fc, gc),
m_config(config),
m_feedbackControlTime(0.0)
{
}

void SpineOnlineControl::onSetup(BaseSpineModelLearning& subject)
{
    m_feedbackControlTime = 0.0;

    const FlemonsSpineModelGoal* goalSubject = tgCast::cast<BaseSpineModelLearning, FlemonsSpineModelGoal>(subject);

    m_lastGoalDist = getGoalDist(goalSubject);
    m_controllerStartDist = m_lastGoalDist;

    SpineGoalControl::onSetup(subject);
}

void SpineOnlineControl::onStep(BaseSpineModelLearning& subject, double dt)
{
    m_updateTime += dt;
    m_feedbackControlTime += dt;
    if (m_updateTime >= m_config.controlTime)
    {
        if(m_feedbackControlTime > m_config.feedbackTime)
        {
            const FlemonsSpineModelGoal* goalSubject = tgCast::cast<BaseSpineModelLearning, FlemonsSpineModelGoal>(subject);

            const double dist = getGoalDist(goalSubject);

            if (dist > m_lastGoalDist)
            {
                // Moved away from the goal, get a new controller
                std::vector<double> tempScores;
                tempScores.push_back(m_controllerStartDist - dist);
                tempScores.push_back(0.0);
                goalAdapter.endEpisode(tempScores);

                goalAdapter.initialize(&goalEvolution,
                        goalLearning,
                        goalConfigData);
                m_controllerStartDist = dist;
            }

            m_feedbackControlTime = 0;
            m_lastGoalDist = dist;
        }
#if (1)
        const FlemonsSpineModelGoal* goalSubject = tgCast::cast<BaseSpineModelLearning, FlemonsSpineModelGoal>(subject);
        std::vector<double> desComs = getGoalFeedback(goalSubject);
#else
        std::vector<double> desComs = getFeedback(subject);

#endif
        try
        {
            m_pCPGSys->update(desComs, m_updateTime);
        }
        catch (std::runtime_error& e)
        {
            //  Stops the trial immediately,  lets teardown know it broke
            bogus = true;
            throw (e);
        }

#ifdef LOGGING // Conditional compile for data logging
        m_dataObserver.onStep(subject, m_updateTime);
#endif
		notifyStep(m_updateTime);
        m_updateTime = 0;
    }

    double currentHeight = subject.getSegmentCOM(m_config.segmentNumber)[1];

    /// @todo add to config
    if (currentHeight > 25 || currentHeight < 1.0)
    {
		/// @todo if bogus, stop trial (reset simulation)
		bogus = true;
		throw std::runtime_error("Height out of range");
	}
}

void SpineOnlineControl::onTeardown(BaseSpineModelLearning& subject)
{
    scores.clear();
    // @todo - check to make sure we ran for the right amount of time

    const FlemonsSpineModelGoal* goalSubject = tgCast::cast<BaseSpineModelLearning, FlemonsSpineModelGoal>(subject);

    const double distanceMoved = calculateDistanceMoved(goalSubject);

    if (bogus)
    {
        scores.push_back(-1.0);
    }
    else
    {
        scores.push_back(distanceMoved);
    }

    /// @todo - consolidate with other controller classes.
    /// @todo - return length scale as a parameter
    double totalEnergySpent=0;

    std::vector<tgSpringCableActuator* > tmpStrings = subject.getAllMuscles();

    for(int i=0; i<tmpStrings.size(); i++)
    {
        tgSpringCableActuator::SpringCableActuatorHistory stringHist = tmpStrings[i]->getHistory();

        for(int j=1; j<stringHist.tensionHistory.size(); j++)
        {
            const double previousTension = stringHist.tensionHistory[j-1];
            const double previousLength = stringHist.restLengths[j-1];
            const double currentLength = stringHist.restLengths[j];
            //TODO: examine this assumption - free spinning motor may require more power
            double motorSpeed = (currentLength-previousLength);
            if(motorSpeed > 0) // Vestigial code
                motorSpeed = 0;
            const double workDone = previousTension * motorSpeed;
            totalEnergySpent += workDone;
        }
    }

    scores.push_back(totalEnergySpent);

    edgeAdapter.endEpisode(scores);
    nodeAdapter.endEpisode(scores);
    feedbackAdapter.endEpisode(scores);

    const double dist = getGoalDist(goalSubject);
    std::vector<double> tempScores;
    tempScores.push_back(m_controllerStartDist - dist);
    tempScores.push_back(0.0);
    goalAdapter.endEpisode(tempScores);

    delete m_pCPGSys;
    m_pCPGSys = NULL;

    for(size_t i = 0; i < m_allControllers.size(); i++)
    {
        delete m_allControllers[i];
    }
    m_allControllers.clear();
}

double SpineOnlineControl::getGoalDist(const FlemonsSpineModelGoal* subject) const
{
    // TODO: consider comparing all segments instead of just the one specified by config
    std::vector<double> finalConditions = subject->getSegmentCOM(m_config.segmentNumber);

    const btVector3 goalPos = subject->goalBoxPosition();

    double x= finalConditions[0] - goalPos.getX();
    double z= finalConditions[2] - goalPos.getZ();
    double distanceNew=sqrt(x*x + z*z);

    return distanceNew;
}
<commit_msg>Add exception for fresh start to trials<commit_after>/*
 * Copyright © 2012, United States Government, as represented by the
 * Administrator of the National Aeronautics and Space Administration.
 * All rights reserved.
 *
 * The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is 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.
*/

/**
 * @file SpineOnlineControl.cpp
 * @brief A controller for the template class BaseSpineModelLearning
 * @author Brian Mirletz
 * @version 1.1.0
 * $Id$
 */

#include "SpineOnlineControl.h"

#include "dev/btietz/TC_goal/FlemonsSpineModelGoal.h"

// Should include tgString, but compiler complains since its been
// included from BaseSpineModelLearning. Perhaps we should move things
// to a cpp over there
#include "core/tgSpringCableActuator.h"
#include "core/tgBasicActuator.h"
#include "tgcreator/tgUtil.h"
#include "controllers/tgImpedanceController.h"
#include "examples/learningSpines/tgCPGActuatorControl.h"
#include "dev/CPG_feedback/tgCPGCableControl.h"

#include "helpers/FileHelpers.h"

#include "learning/AnnealEvolution/AnnealEvolution.h"
#include "learning/Configuration/configuration.h"

#include "dev/CPG_feedback/CPGEquationsFB.h"
#include "dev/CPG_feedback/CPGNodeFB.h"

#include <iterator>     // std::iterator
#include <string>

//#define LOGGING
#define USE_KINEMATIC

using namespace std;

SpineOnlineControl::Config::Config(int ss,
                                        int tm,
                                        int om,
                                        int param,
                                        int segnum,
                                        double ct,
                                        double la,
                                        double ha,
                                        double lp,
                                        double hp,
                                        double kt,
                                        double kp,
                                        double kv,
                                        bool def,
                                        double cl,
                                        double lf,
                                        double hf,
                                        double ffMin,
                                        double ffMax,
                                        double afMin,
                                        double afMax,
                                        double pfMin,
                                        double pfMax,
                                        double tf,
                                        double feedTime ) :
SpineGoalControl::Config::Config(ss, tm, om, param, segnum, ct, la, ha,
                                    lp, hp, kt, kp, kv, def, cl, lf, hf,
                                    ffMin, ffMax, afMin, afMax, pfMin, pfMax, tf),
feedbackTime(feedTime)
{

}
/**
 * Defining the adapters here assumes the controller is around and
 * attached for the lifecycle of the learning runs. I.E. that the setup
 * and teardown functions are used for tgModel
 */
SpineOnlineControl::SpineOnlineControl(SpineOnlineControl::Config config,
                                                std::string args,
                                                std::string resourcePath,
                                                std::string ec,
                                                std::string nc,
                                                std::string fc,
                                                std::string gc) :
SpineGoalControl(config, args, resourcePath, ec, nc, fc, gc),
m_config(config),
m_feedbackControlTime(0.0)
{
}

void SpineOnlineControl::onSetup(BaseSpineModelLearning& subject)
{
    m_feedbackControlTime = 0.0;

    const FlemonsSpineModelGoal* goalSubject = tgCast::cast<BaseSpineModelLearning, FlemonsSpineModelGoal>(subject);

    m_lastGoalDist = getGoalDist(goalSubject);
    m_controllerStartDist = m_lastGoalDist;

    SpineGoalControl::onSetup(subject);
}

void SpineOnlineControl::onStep(BaseSpineModelLearning& subject, double dt)
{
    m_updateTime += dt;
    m_feedbackControlTime += dt;
    if (m_updateTime >= m_config.controlTime)
    {
        if(m_feedbackControlTime > m_config.feedbackTime)
        {
            const FlemonsSpineModelGoal* goalSubject = tgCast::cast<BaseSpineModelLearning, FlemonsSpineModelGoal>(subject);

            const double dist = getGoalDist(goalSubject);

            if (dist > m_lastGoalDist)
            {
#if (0)
                // Moved away from the goal, get a new controller
                std::vector<double> tempScores;
                tempScores.push_back(m_controllerStartDist - dist);
                tempScores.push_back(0.0);
                goalAdapter.endEpisode(tempScores);

                goalAdapter.initialize(&goalEvolution,
                        goalLearning,
                        goalConfigData);
                m_controllerStartDist = dist;
#else
                throw std::runtime_error("Moved away from goal");
#endif
            }

            m_feedbackControlTime = 0;
            m_lastGoalDist = dist;
        }
#if (1)
        const FlemonsSpineModelGoal* goalSubject = tgCast::cast<BaseSpineModelLearning, FlemonsSpineModelGoal>(subject);
        std::vector<double> desComs = getGoalFeedback(goalSubject);
#else
        std::vector<double> desComs = getFeedback(subject);

#endif
        try
        {
            m_pCPGSys->update(desComs, m_updateTime);
        }
        catch (std::runtime_error& e)
        {
            //  Stops the trial immediately,  lets teardown know it broke
            bogus = true;
            throw (e);
        }

#ifdef LOGGING // Conditional compile for data logging
        m_dataObserver.onStep(subject, m_updateTime);
#endif
		notifyStep(m_updateTime);
        m_updateTime = 0;
    }

    double currentHeight = subject.getSegmentCOM(m_config.segmentNumber)[1];

    /// @todo add to config
    if (currentHeight > 25 || currentHeight < 1.0)
    {
		/// @todo if bogus, stop trial (reset simulation)
		bogus = true;
		throw std::runtime_error("Height out of range");
	}
}

void SpineOnlineControl::onTeardown(BaseSpineModelLearning& subject)
{
    scores.clear();
    // @todo - check to make sure we ran for the right amount of time

    const FlemonsSpineModelGoal* goalSubject = tgCast::cast<BaseSpineModelLearning, FlemonsSpineModelGoal>(subject);

    const double distanceMoved = calculateDistanceMoved(goalSubject);

    if (bogus)
    {
        scores.push_back(-1.0);
    }
    else
    {
        scores.push_back(distanceMoved);
    }

    /// @todo - consolidate with other controller classes.
    /// @todo - return length scale as a parameter
    double totalEnergySpent=0;

    std::vector<tgSpringCableActuator* > tmpStrings = subject.getAllMuscles();

    for(int i=0; i<tmpStrings.size(); i++)
    {
        tgSpringCableActuator::SpringCableActuatorHistory stringHist = tmpStrings[i]->getHistory();

        for(int j=1; j<stringHist.tensionHistory.size(); j++)
        {
            const double previousTension = stringHist.tensionHistory[j-1];
            const double previousLength = stringHist.restLengths[j-1];
            const double currentLength = stringHist.restLengths[j];
            //TODO: examine this assumption - free spinning motor may require more power
            double motorSpeed = (currentLength-previousLength);
            if(motorSpeed > 0) // Vestigial code
                motorSpeed = 0;
            const double workDone = previousTension * motorSpeed;
            totalEnergySpent += workDone;
        }
    }

    scores.push_back(totalEnergySpent);

    edgeAdapter.endEpisode(scores);
    nodeAdapter.endEpisode(scores);
    feedbackAdapter.endEpisode(scores);

    const double dist = getGoalDist(goalSubject);
    std::vector<double> tempScores;
    tempScores.push_back(m_controllerStartDist - dist);
    tempScores.push_back(0.0);
    goalAdapter.endEpisode(tempScores);

    delete m_pCPGSys;
    m_pCPGSys = NULL;

    for(size_t i = 0; i < m_allControllers.size(); i++)
    {
        delete m_allControllers[i];
    }
    m_allControllers.clear();
}

double SpineOnlineControl::getGoalDist(const FlemonsSpineModelGoal* subject) const
{
    // TODO: consider comparing all segments instead of just the one specified by config
    std::vector<double> finalConditions = subject->getSegmentCOM(m_config.segmentNumber);

    const btVector3 goalPos = subject->goalBoxPosition();

    double x= finalConditions[0] - goalPos.getX();
    double z= finalConditions[2] - goalPos.getZ();
    double distanceNew=sqrt(x*x + z*z);

    return distanceNew;
}
<|endoftext|>


<commit_before>/*
 * Copyright 2012 The Android Open Source Project
 *
 * Use of this source code is governed by a BSD-style license that can be
 * found in the LICENSE file.
 */

#include "SkBlendImageFilter.h"
#include "SkCanvas.h"
#include "SkColorPriv.h"
#include "SkFlattenableBuffers.h"
#if SK_SUPPORT_GPU
#include "SkGr.h"
#include "SkGrPixelRef.h"
#include "gl/GrGLProgramStage.h"
#endif

namespace {

SkXfermode::Mode modeToXfermode(SkBlendImageFilter::Mode mode)
{
    switch (mode) {
      case SkBlendImageFilter::kNormal_Mode:
        return SkXfermode::kSrcOver_Mode;
      case SkBlendImageFilter::kMultiply_Mode:
        return SkXfermode::kMultiply_Mode;
      case SkBlendImageFilter::kScreen_Mode:
        return SkXfermode::kScreen_Mode;
      case SkBlendImageFilter::kDarken_Mode:
        return SkXfermode::kDarken_Mode;
      case SkBlendImageFilter::kLighten_Mode:
        return SkXfermode::kLighten_Mode;
    }
    SkASSERT(0);
    return SkXfermode::kSrcOver_Mode;
}

SkPMColor multiply_proc(SkPMColor src, SkPMColor dst) {
    int omsa = 255 - SkGetPackedA32(src);
    int sr = SkGetPackedR32(src), sg = SkGetPackedG32(src), sb = SkGetPackedB32(src);
    int omda = 255 - SkGetPackedA32(dst);
    int dr = SkGetPackedR32(dst), dg = SkGetPackedG32(dst), db = SkGetPackedB32(dst);
    int a = 255 - SkMulDiv255Round(omsa, omda);
    int r = SkMulDiv255Round(omsa, dr) + SkMulDiv255Round(omda, sr) + SkMulDiv255Round(sr, dr);
    int g = SkMulDiv255Round(omsa, dg) + SkMulDiv255Round(omda, sg) + SkMulDiv255Round(sg, dg);
    int b = SkMulDiv255Round(omsa, db) + SkMulDiv255Round(omda, sb) + SkMulDiv255Round(sb, db);
    return SkPackARGB32(a, r, g, b);
}

};

///////////////////////////////////////////////////////////////////////////////

SkBlendImageFilter::SkBlendImageFilter(SkBlendImageFilter::Mode mode, SkImageFilter* background, SkImageFilter* foreground)
  : fMode(mode)
{
    fBackground = background;
    SkASSERT(background != NULL);
    SkSafeRef(fBackground);
    fForeground = foreground;
    SkSafeRef(fForeground);
}

SkBlendImageFilter::~SkBlendImageFilter() {
    SkSafeUnref(fBackground);
    SkSafeUnref(fForeground);
}

SkBlendImageFilter::SkBlendImageFilter(SkFlattenableReadBuffer& buffer)
  : INHERITED(buffer)
{
    fMode = (SkBlendImageFilter::Mode) buffer.readInt();
    fBackground = buffer.readFlattenableT<SkImageFilter>();
    if (buffer.readBool()) {
        fForeground = buffer.readFlattenableT<SkImageFilter>();
    } else {
        fForeground = NULL;
    }
}

void SkBlendImageFilter::flatten(SkFlattenableWriteBuffer& buffer) const {
    this->INHERITED::flatten(buffer);
    buffer.writeInt((int) fMode);
    buffer.writeFlattenable(fBackground);
    buffer.writeBool(NULL != fForeground);
    if (NULL != fForeground) {
        buffer.writeFlattenable(fForeground);
    }
}

bool SkBlendImageFilter::onFilterImage(Proxy* proxy,
                                       const SkBitmap& src,
                                       const SkMatrix& ctm,
                                       SkBitmap* dst,
                                       SkIPoint* offset) {
    SkBitmap background, foreground = src;
    SkASSERT(fBackground);
    if (!fBackground->filterImage(proxy, src, ctm, &background, offset)) {
        return false;
    }
    if (fForeground && !fForeground->filterImage(proxy, src, ctm, &foreground, offset)) {
        return false;
    }
    SkAutoLockPixels alp_foreground(foreground), alp_background(background);
    if (!foreground.getPixels() || !background.getPixels()) {
        return false;
    }
    dst->setConfig(background.config(), background.width(), background.height());
    dst->allocPixels();
    SkCanvas canvas(*dst);
    SkPaint paint;
    paint.setXfermodeMode(SkXfermode::kSrc_Mode);
    canvas.drawBitmap(background, 0, 0, &paint);
    // FEBlend's multiply mode is (1 - Sa) * Da + (1 - Da) * Sc + Sc * Dc
    // Skia's is just Sc * Dc.  So we use a custom proc to implement FEBlend's
    // version.
    if (fMode == SkBlendImageFilter::kMultiply_Mode) {
        paint.setXfermode(new SkProcXfermode(multiply_proc))->unref();
    } else {
        paint.setXfermodeMode(modeToXfermode(fMode));
    }
    canvas.drawBitmap(foreground, 0, 0, &paint);
    return true;
}

///////////////////////////////////////////////////////////////////////////////

#if SK_SUPPORT_GPU
class GrGLBlendEffect  : public GrGLProgramStage {
public:
    GrGLBlendEffect(const GrProgramStageFactory& factory,
                    const GrCustomStage& stage);
    virtual ~GrGLBlendEffect();

    virtual void emitFS(GrGLShaderBuilder* builder,
                        const char* outputColor,
                        const char* inputColor,
                        const char* samplerName) SK_OVERRIDE;

    virtual void emitVS(GrGLShaderBuilder* builder,
                        const char* vertexCoords) SK_OVERRIDE {}

    static inline StageKey GenKey(const GrCustomStage& s, const GrGLCaps&);

private:
    typedef GrGLProgramStage INHERITED;
    SkBlendImageFilter::Mode fMode;
};

///////////////////////////////////////////////////////////////////////////////

class GrBlendEffect : public GrSingleTextureEffect {
public:
    GrBlendEffect(SkBlendImageFilter::Mode mode, GrTexture* foreground);
    virtual ~GrBlendEffect();

    virtual bool isEqual(const GrCustomStage&) const SK_OVERRIDE;
    const GrProgramStageFactory& getFactory() const;
    SkBlendImageFilter::Mode mode() const { return fMode; }

    typedef GrGLBlendEffect GLProgramStage;
    static const char* Name() { return "Blend"; }

private:
    typedef GrSingleTextureEffect INHERITED;
    SkBlendImageFilter::Mode fMode;
};

// FIXME:  This should be refactored with SkSingleInputImageFilter's version.
static GrTexture* getInputResultAsTexture(SkImageFilter* input,
                                          GrTexture* src,
                                          const SkRect& rect) {
    GrTexture* resultTex;
    if (!input) {
        resultTex = src;
    } else if (input->canFilterImageGPU()) {
        // onFilterImageGPU() already refs the result, so just return it here.
        return input->onFilterImageGPU(src, rect);
    } else {
        SkBitmap srcBitmap, result;
        srcBitmap.setConfig(SkBitmap::kARGB_8888_Config, src->width(), src->height());
        srcBitmap.setPixelRef(new SkGrPixelRef(src))->unref();
        SkIPoint offset;
        if (input->filterImage(NULL, srcBitmap, SkMatrix(), &result, &offset)) {
            if (result.getTexture()) {
                resultTex = (GrTexture*) result.getTexture();
            } else {
                resultTex = GrLockCachedBitmapTexture(src->getContext(), result, NULL);
                SkSafeRef(resultTex);
                GrUnlockCachedBitmapTexture(resultTex);
                return resultTex;
            }
        } else {
            resultTex = src;
        }
    }
    SkSafeRef(resultTex);
    return resultTex;
}

GrTexture* SkBlendImageFilter::onFilterImageGPU(GrTexture* src, const SkRect& rect) {
    SkAutoTUnref<GrTexture> background(getInputResultAsTexture(fBackground, src, rect));
    SkAutoTUnref<GrTexture> foreground(getInputResultAsTexture(fForeground, src, rect));
    GrContext* context = src->getContext();

    GrTextureDesc desc;
    desc.fFlags = kRenderTarget_GrTextureFlagBit | kNoStencil_GrTextureFlagBit;
    desc.fWidth = SkScalarCeilToInt(rect.width());
    desc.fHeight = SkScalarCeilToInt(rect.height());
    desc.fConfig = kRGBA_8888_GrPixelConfig;

    GrAutoScratchTexture ast(context, desc);
    GrTexture* dst = ast.detach();
    GrContext::AutoMatrix avm(context, GrMatrix::I());
    GrContext::AutoRenderTarget art(context, dst->asRenderTarget());
    GrContext::AutoClip ac(context, rect);

    GrMatrix sampleM;
    sampleM.setIDiv(background->width(), background->height());
    GrPaint paint;
    paint.reset();
    paint.textureSampler(0)->reset(sampleM);
    paint.textureSampler(0)->setCustomStage(SkNEW_ARGS(GrSingleTextureEffect, (background.get())))->unref();
    paint.textureSampler(1)->reset(sampleM);
    paint.textureSampler(1)->setCustomStage(SkNEW_ARGS(GrBlendEffect, (fMode, foreground.get())))->unref();
    context->drawRect(paint, rect);
    return dst;
}

///////////////////////////////////////////////////////////////////////////////

GrBlendEffect::GrBlendEffect(SkBlendImageFilter::Mode mode, GrTexture* foreground)
    : INHERITED(foreground), fMode(mode) {
}

GrBlendEffect::~GrBlendEffect() {
}

bool GrBlendEffect::isEqual(const GrCustomStage& sBase) const {
    const GrBlendEffect& s = static_cast<const GrBlendEffect&>(sBase);
    return INHERITED::isEqual(sBase) &&
           fMode == s.fMode;
}

const GrProgramStageFactory& GrBlendEffect::getFactory() const {
    return GrTProgramStageFactory<GrBlendEffect>::getInstance();
}

///////////////////////////////////////////////////////////////////////////////

GrGLBlendEffect::GrGLBlendEffect(const GrProgramStageFactory& factory,
                                 const GrCustomStage& stage)
    : GrGLProgramStage(factory),
      fMode(static_cast<const GrBlendEffect&>(stage).mode()) {
}

GrGLBlendEffect::~GrGLBlendEffect() {
}

void GrGLBlendEffect::emitFS(GrGLShaderBuilder* builder,
                             const char* outputColor,
                             const char* inputColor,
                             const char* samplerName) {
    SkString* code = &builder->fFSCode;
    const char* bgColor = inputColor;
    const char* fgColor = "fgColor";
    code->appendf("\t\tvec4 %s = ", fgColor);
    builder->emitTextureLookup(samplerName);
    code->appendf(";\n");
    code->appendf("\t\t%s.a = 1.0 - (1.0 - %s.a) * (1.0 - %s.b);\n", outputColor, bgColor, fgColor);
    switch (fMode) {
      case SkBlendImageFilter::kNormal_Mode:
        code->appendf("\t\t%s.rgb = (1.0 - %s.a) * %s.rgb + %s.rgb;\n", outputColor, fgColor, bgColor, fgColor);
        break;
      case SkBlendImageFilter::kMultiply_Mode:
        code->appendf("\t\t%s.rgb = (1.0 - %s.a) * %s.rgb + (1.0 - %s.a) * %s.rgb + %s.rgb * %s.rgb;\n", outputColor, fgColor, bgColor, bgColor, fgColor, fgColor, bgColor);
        break;
      case SkBlendImageFilter::kScreen_Mode:
        code->appendf("\t\t%s.rgb = %s.rgb + %s.rgb - %s.rgb * %s.rgb;\n", outputColor, bgColor, fgColor, fgColor, bgColor);
        break;
      case SkBlendImageFilter::kDarken_Mode:
        code->appendf("\t\t%s.rgb = min((1.0 - %s.a) * %s.rgb + %s.rgb, (1.0 - %s.a) * %s.rgb + %s.rgb);\n", outputColor, fgColor, bgColor, fgColor, bgColor, fgColor, bgColor);
        break;
      case SkBlendImageFilter::kLighten_Mode:
        code->appendf("\t\t%s.rgb = max((1.0 - %s.a) * %s.rgb + %s.rgb, (1.0 - %s.a) * %s.rgb + %s.rgb);\n", outputColor, fgColor, bgColor, fgColor, bgColor, fgColor, bgColor);
        break;
    }
}

GrGLProgramStage::StageKey GrGLBlendEffect::GenKey(const GrCustomStage& s, const GrGLCaps&) {
    return static_cast<const GrBlendEffect&>(s).mode();
}
#endif

SK_DEFINE_FLATTENABLE_REGISTRAR(SkBlendImageFilter)
<commit_msg>Speculative Android GM blend fix (unreviewed).<commit_after>/*
 * Copyright 2012 The Android Open Source Project
 *
 * Use of this source code is governed by a BSD-style license that can be
 * found in the LICENSE file.
 */

#include "SkBlendImageFilter.h"
#include "SkCanvas.h"
#include "SkColorPriv.h"
#include "SkFlattenableBuffers.h"
#if SK_SUPPORT_GPU
#include "SkGr.h"
#include "SkGrPixelRef.h"
#include "gl/GrGLProgramStage.h"
#endif

namespace {

SkXfermode::Mode modeToXfermode(SkBlendImageFilter::Mode mode)
{
    switch (mode) {
      case SkBlendImageFilter::kNormal_Mode:
        return SkXfermode::kSrcOver_Mode;
      case SkBlendImageFilter::kMultiply_Mode:
        return SkXfermode::kMultiply_Mode;
      case SkBlendImageFilter::kScreen_Mode:
        return SkXfermode::kScreen_Mode;
      case SkBlendImageFilter::kDarken_Mode:
        return SkXfermode::kDarken_Mode;
      case SkBlendImageFilter::kLighten_Mode:
        return SkXfermode::kLighten_Mode;
    }
    SkASSERT(0);
    return SkXfermode::kSrcOver_Mode;
}

SkPMColor multiply_proc(SkPMColor src, SkPMColor dst) {
    int omsa = 255 - SkGetPackedA32(src);
    int sr = SkGetPackedR32(src), sg = SkGetPackedG32(src), sb = SkGetPackedB32(src);
    int omda = 255 - SkGetPackedA32(dst);
    int dr = SkGetPackedR32(dst), dg = SkGetPackedG32(dst), db = SkGetPackedB32(dst);
    int a = 255 - SkMulDiv255Round(omsa, omda);
    int r = SkMulDiv255Round(omsa, dr) + SkMulDiv255Round(omda, sr) + SkMulDiv255Round(sr, dr);
    int g = SkMulDiv255Round(omsa, dg) + SkMulDiv255Round(omda, sg) + SkMulDiv255Round(sg, dg);
    int b = SkMulDiv255Round(omsa, db) + SkMulDiv255Round(omda, sb) + SkMulDiv255Round(sb, db);
    return SkPackARGB32(a, r, g, b);
}

};

///////////////////////////////////////////////////////////////////////////////

SkBlendImageFilter::SkBlendImageFilter(SkBlendImageFilter::Mode mode, SkImageFilter* background, SkImageFilter* foreground)
  : fMode(mode), fBackground(background), fForeground(foreground)
{
    SkASSERT(NULL != background);
    SkSafeRef(fBackground);
    SkSafeRef(fForeground);
}

SkBlendImageFilter::~SkBlendImageFilter() {
    SkSafeUnref(fBackground);
    SkSafeUnref(fForeground);
}

SkBlendImageFilter::SkBlendImageFilter(SkFlattenableReadBuffer& buffer)
  : INHERITED(buffer)
{
    fMode = (SkBlendImageFilter::Mode) buffer.readInt();
    fBackground = buffer.readFlattenableT<SkImageFilter>();
    if (buffer.readBool()) {
        fForeground = buffer.readFlattenableT<SkImageFilter>();
    } else {
        fForeground = NULL;
    }
}

void SkBlendImageFilter::flatten(SkFlattenableWriteBuffer& buffer) const {
    this->INHERITED::flatten(buffer);
    buffer.writeInt((int) fMode);
    buffer.writeFlattenable(fBackground);
    buffer.writeBool(NULL != fForeground);
    if (NULL != fForeground) {
        buffer.writeFlattenable(fForeground);
    }
}

bool SkBlendImageFilter::onFilterImage(Proxy* proxy,
                                       const SkBitmap& src,
                                       const SkMatrix& ctm,
                                       SkBitmap* dst,
                                       SkIPoint* offset) {
    SkBitmap background, foreground = src;
    SkASSERT(NULL != fBackground);
    if (!fBackground->filterImage(proxy, src, ctm, &background, offset)) {
        return false;
    }
    if (fForeground && !fForeground->filterImage(proxy, src, ctm, &foreground, offset)) {
        return false;
    }
    SkAutoLockPixels alp_foreground(foreground), alp_background(background);
    if (!foreground.getPixels() || !background.getPixels()) {
        return false;
    }
    dst->setConfig(background.config(), background.width(), background.height());
    dst->allocPixels();
    SkCanvas canvas(*dst);
    SkPaint paint;
    paint.setXfermodeMode(SkXfermode::kSrc_Mode);
    canvas.drawBitmap(background, 0, 0, &paint);
    // FEBlend's multiply mode is (1 - Sa) * Da + (1 - Da) * Sc + Sc * Dc
    // Skia's is just Sc * Dc.  So we use a custom proc to implement FEBlend's
    // version.
    if (fMode == SkBlendImageFilter::kMultiply_Mode) {
        paint.setXfermode(new SkProcXfermode(multiply_proc))->unref();
    } else {
        paint.setXfermodeMode(modeToXfermode(fMode));
    }
    canvas.drawBitmap(foreground, 0, 0, &paint);
    return true;
}

///////////////////////////////////////////////////////////////////////////////

#if SK_SUPPORT_GPU
class GrGLBlendEffect  : public GrGLProgramStage {
public:
    GrGLBlendEffect(const GrProgramStageFactory& factory,
                    const GrCustomStage& stage);
    virtual ~GrGLBlendEffect();

    virtual void emitFS(GrGLShaderBuilder* builder,
                        const char* outputColor,
                        const char* inputColor,
                        const char* samplerName) SK_OVERRIDE;

    virtual void emitVS(GrGLShaderBuilder* builder,
                        const char* vertexCoords) SK_OVERRIDE {}

    static inline StageKey GenKey(const GrCustomStage& s, const GrGLCaps&);

private:
    typedef GrGLProgramStage INHERITED;
    SkBlendImageFilter::Mode fMode;
};

///////////////////////////////////////////////////////////////////////////////

class GrBlendEffect : public GrSingleTextureEffect {
public:
    GrBlendEffect(SkBlendImageFilter::Mode mode, GrTexture* foreground);
    virtual ~GrBlendEffect();

    virtual bool isEqual(const GrCustomStage&) const SK_OVERRIDE;
    const GrProgramStageFactory& getFactory() const;
    SkBlendImageFilter::Mode mode() const { return fMode; }

    typedef GrGLBlendEffect GLProgramStage;
    static const char* Name() { return "Blend"; }

private:
    typedef GrSingleTextureEffect INHERITED;
    SkBlendImageFilter::Mode fMode;
};

// FIXME:  This should be refactored with SkSingleInputImageFilter's version.
static GrTexture* getInputResultAsTexture(SkImageFilter* input,
                                          GrTexture* src,
                                          const SkRect& rect) {
    GrTexture* resultTex;
    if (!input) {
        resultTex = src;
    } else if (input->canFilterImageGPU()) {
        // onFilterImageGPU() already refs the result, so just return it here.
        return input->onFilterImageGPU(src, rect);
    } else {
        SkBitmap srcBitmap, result;
        srcBitmap.setConfig(SkBitmap::kARGB_8888_Config, src->width(), src->height());
        srcBitmap.setPixelRef(new SkGrPixelRef(src))->unref();
        SkIPoint offset;
        if (input->filterImage(NULL, srcBitmap, SkMatrix(), &result, &offset)) {
            if (result.getTexture()) {
                resultTex = (GrTexture*) result.getTexture();
            } else {
                resultTex = GrLockCachedBitmapTexture(src->getContext(), result, NULL);
                SkSafeRef(resultTex);
                GrUnlockCachedBitmapTexture(resultTex);
                return resultTex;
            }
        } else {
            resultTex = src;
        }
    }
    SkSafeRef(resultTex);
    return resultTex;
}

GrTexture* SkBlendImageFilter::onFilterImageGPU(GrTexture* src, const SkRect& rect) {
    SkAutoTUnref<GrTexture> background(getInputResultAsTexture(fBackground, src, rect));
    SkAutoTUnref<GrTexture> foreground(getInputResultAsTexture(fForeground, src, rect));
    GrContext* context = src->getContext();

    GrTextureDesc desc;
    desc.fFlags = kRenderTarget_GrTextureFlagBit | kNoStencil_GrTextureFlagBit;
    desc.fWidth = SkScalarCeilToInt(rect.width());
    desc.fHeight = SkScalarCeilToInt(rect.height());
    desc.fConfig = kRGBA_8888_GrPixelConfig;

    GrAutoScratchTexture ast(context, desc);
    GrTexture* dst = ast.detach();
    GrContext::AutoMatrix avm(context, GrMatrix::I());
    GrContext::AutoRenderTarget art(context, dst->asRenderTarget());
    GrContext::AutoClip ac(context, rect);

    GrMatrix sampleM;
    sampleM.setIDiv(background->width(), background->height());
    GrPaint paint;
    paint.reset();
    paint.textureSampler(0)->reset(sampleM);
    paint.textureSampler(0)->setCustomStage(SkNEW_ARGS(GrSingleTextureEffect, (background.get())))->unref();
    paint.textureSampler(1)->reset(sampleM);
    paint.textureSampler(1)->setCustomStage(SkNEW_ARGS(GrBlendEffect, (fMode, foreground.get())))->unref();
    context->drawRect(paint, rect);
    return dst;
}

///////////////////////////////////////////////////////////////////////////////

GrBlendEffect::GrBlendEffect(SkBlendImageFilter::Mode mode, GrTexture* foreground)
    : INHERITED(foreground), fMode(mode) {
}

GrBlendEffect::~GrBlendEffect() {
}

bool GrBlendEffect::isEqual(const GrCustomStage& sBase) const {
    const GrBlendEffect& s = static_cast<const GrBlendEffect&>(sBase);
    return INHERITED::isEqual(sBase) &&
           fMode == s.fMode;
}

const GrProgramStageFactory& GrBlendEffect::getFactory() const {
    return GrTProgramStageFactory<GrBlendEffect>::getInstance();
}

///////////////////////////////////////////////////////////////////////////////

GrGLBlendEffect::GrGLBlendEffect(const GrProgramStageFactory& factory,
                                 const GrCustomStage& stage)
    : GrGLProgramStage(factory),
      fMode(static_cast<const GrBlendEffect&>(stage).mode()) {
}

GrGLBlendEffect::~GrGLBlendEffect() {
}

void GrGLBlendEffect::emitFS(GrGLShaderBuilder* builder,
                             const char* outputColor,
                             const char* inputColor,
                             const char* samplerName) {
    SkString* code = &builder->fFSCode;
    const char* bgColor = inputColor;
    const char* fgColor = "fgColor";
    code->appendf("\t\tvec4 %s = ", fgColor);
    builder->emitTextureLookup(samplerName);
    code->appendf(";\n");
    code->appendf("\t\t%s.a = 1.0 - (1.0 - %s.a) * (1.0 - %s.b);\n", outputColor, bgColor, fgColor);
    switch (fMode) {
      case SkBlendImageFilter::kNormal_Mode:
        code->appendf("\t\t%s.rgb = (1.0 - %s.a) * %s.rgb + %s.rgb;\n", outputColor, fgColor, bgColor, fgColor);
        break;
      case SkBlendImageFilter::kMultiply_Mode:
        code->appendf("\t\t%s.rgb = (1.0 - %s.a) * %s.rgb + (1.0 - %s.a) * %s.rgb + %s.rgb * %s.rgb;\n", outputColor, fgColor, bgColor, bgColor, fgColor, fgColor, bgColor);
        break;
      case SkBlendImageFilter::kScreen_Mode:
        code->appendf("\t\t%s.rgb = %s.rgb + %s.rgb - %s.rgb * %s.rgb;\n", outputColor, bgColor, fgColor, fgColor, bgColor);
        break;
      case SkBlendImageFilter::kDarken_Mode:
        code->appendf("\t\t%s.rgb = min((1.0 - %s.a) * %s.rgb + %s.rgb, (1.0 - %s.a) * %s.rgb + %s.rgb);\n", outputColor, fgColor, bgColor, fgColor, bgColor, fgColor, bgColor);
        break;
      case SkBlendImageFilter::kLighten_Mode:
        code->appendf("\t\t%s.rgb = max((1.0 - %s.a) * %s.rgb + %s.rgb, (1.0 - %s.a) * %s.rgb + %s.rgb);\n", outputColor, fgColor, bgColor, fgColor, bgColor, fgColor, bgColor);
        break;
    }
}

GrGLProgramStage::StageKey GrGLBlendEffect::GenKey(const GrCustomStage& s, const GrGLCaps&) {
    return static_cast<const GrBlendEffect&>(s).mode();
}
#endif

SK_DEFINE_FLATTENABLE_REGISTRAR(SkBlendImageFilter)
<|endoftext|>


<commit_before>/*************************************************************************
 *
 *  OpenOffice.org - a multi-platform office productivity suite
 *
 *  $RCSfile: browserview.cxx,v $
 *
 *  $Revision: 1.11 $
 *
 *  last change: $Author: vg $ $Date: 2006-03-14 11:17:48 $
 *
 *  The Contents of this file are made available subject to
 *  the terms of GNU Lesser General Public License Version 2.1.
 *
 *
 *    GNU Lesser General Public License Version 2.1
 *    =============================================
 *    Copyright 2005 by Sun Microsystems, Inc.
 *    901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *    This library is free software; you can redistribute it and/or
 *    modify it under the terms of the GNU Lesser General Public
 *    License version 2.1, as published by the Free Software Foundation.
 *
 *    This library is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *    Lesser General Public License for more details.
 *
 *    You should have received a copy of the GNU Lesser General Public
 *    License along with this library; if not, write to the Free Software
 *    Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *    MA  02111-1307  USA
 *
 ************************************************************************/

#ifndef _EXTENSIONS_PROPCTRLR_BROWSERVIEW_HXX_
#include "browserview.hxx"
#endif
#ifndef _EXTENSIONS_PROPCTRLR_PROPERTYEDITOR_HXX_
#include "propertyeditor.hxx"
#endif
#ifndef _EXTENSIONS_FORMCTRLR_FORMHELPID_HRC_
#include "formhelpid.hrc"
#endif
#ifndef _TOOLS_DEBUG_HXX
#include <tools/debug.hxx>
#endif
#include <memory>

//............................................................................
namespace pcr
{
//............................................................................

    using namespace ::com::sun::star::uno;
    using namespace ::com::sun::star::lang;

    //========================================================================
    //= class OPropertyBrowserView
    //========================================================================
    DBG_NAME(OPropertyBrowserView)
    //------------------------------------------------------------------------
    OPropertyBrowserView::OPropertyBrowserView( const Reference< XMultiServiceFactory >& _rxORB,
                                 Window* _pParent, WinBits nBits)
                  :Window(_pParent, nBits | WB_3DLOOK)
                  ,m_xORB(_rxORB)
                  ,m_nActivePage(0)
    {
        DBG_CTOR(OPropertyBrowserView,NULL);

        m_pPropBox = new OPropertyEditor( this );
        m_pPropBox->SetHelpId(HID_FM_PROPDLG_TABCTR);
        m_pPropBox->setPageActivationHandler(LINK(this, OPropertyBrowserView, OnPageActivation));

        m_pPropBox->Show();
    }

    //------------------------------------------------------------------------
    IMPL_LINK(OPropertyBrowserView, OnPageActivation, void*, EMPTYARG)
    {
        m_nActivePage = m_pPropBox->GetCurPage();
        if (m_aPageActivationHandler.IsSet())
            m_aPageActivationHandler.Call(NULL);
        return 0L;
    }

    //------------------------------------------------------------------------
    OPropertyBrowserView::~OPropertyBrowserView()
    {
        if(m_pPropBox)
        {
            sal_uInt16 nTmpPage = m_pPropBox->GetCurPage();
            if (nTmpPage)
                m_nActivePage = nTmpPage;
            ::std::auto_ptr<Window> aTemp(m_pPropBox);
            m_pPropBox = NULL;
        }
        m_xORB = NULL;

        DBG_DTOR(OPropertyBrowserView, NULL);
    }

    //------------------------------------------------------------------------
    void OPropertyBrowserView::activatePage(sal_uInt16 _nPage)
    {
        m_nActivePage = _nPage;
        getPropertyBox()->SetPage(m_nActivePage);
    }

    //------------------------------------------------------------------------
    void OPropertyBrowserView::GetFocus()
    {
        if (m_pPropBox)
            m_pPropBox->GrabFocus();
        else
            Window::GetFocus();
    }

    //------------------------------------------------------------------------
    void OPropertyBrowserView::Resize()
    {
        Size aSize = GetOutputSizePixel();
        m_pPropBox->SetSizePixel(aSize);
    }

    // #95343# ---------------------------------------------------------------
    ::com::sun::star::awt::Size OPropertyBrowserView::getMinimumSize()
    {
        Size aSize = GetOutputSizePixel();
        aSize.setHeight( 250 );
        if( m_pPropBox )
            aSize.setWidth( m_pPropBox->getMinimumWidth() );
        return ::com::sun::star::awt::Size( aSize.Width(), aSize.Height() );
    }

//............................................................................
} // namespace pcr
//............................................................................

<commit_msg>INTEGRATION: CWS pchfix02 (1.11.94); FILE MERGED 2006/09/01 17:26:38 kaib 1.11.94.1: #i68856# Added header markers and pch files<commit_after>/*************************************************************************
 *
 *  OpenOffice.org - a multi-platform office productivity suite
 *
 *  $RCSfile: browserview.cxx,v $
 *
 *  $Revision: 1.12 $
 *
 *  last change: $Author: obo $ $Date: 2006-09-16 13:13:40 $
 *
 *  The Contents of this file are made available subject to
 *  the terms of GNU Lesser General Public License Version 2.1.
 *
 *
 *    GNU Lesser General Public License Version 2.1
 *    =============================================
 *    Copyright 2005 by Sun Microsystems, Inc.
 *    901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *    This library is free software; you can redistribute it and/or
 *    modify it under the terms of the GNU Lesser General Public
 *    License version 2.1, as published by the Free Software Foundation.
 *
 *    This library is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *    Lesser General Public License for more details.
 *
 *    You should have received a copy of the GNU Lesser General Public
 *    License along with this library; if not, write to the Free Software
 *    Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *    MA  02111-1307  USA
 *
 ************************************************************************/

// MARKER(update_precomp.py): autogen include statement, do not remove
#include "precompiled_extensions.hxx"

#ifndef _EXTENSIONS_PROPCTRLR_BROWSERVIEW_HXX_
#include "browserview.hxx"
#endif
#ifndef _EXTENSIONS_PROPCTRLR_PROPERTYEDITOR_HXX_
#include "propertyeditor.hxx"
#endif
#ifndef _EXTENSIONS_FORMCTRLR_FORMHELPID_HRC_
#include "formhelpid.hrc"
#endif
#ifndef _TOOLS_DEBUG_HXX
#include <tools/debug.hxx>
#endif
#include <memory>

//............................................................................
namespace pcr
{
//............................................................................

    using namespace ::com::sun::star::uno;
    using namespace ::com::sun::star::lang;

    //========================================================================
    //= class OPropertyBrowserView
    //========================================================================
    DBG_NAME(OPropertyBrowserView)
    //------------------------------------------------------------------------
    OPropertyBrowserView::OPropertyBrowserView( const Reference< XMultiServiceFactory >& _rxORB,
                                 Window* _pParent, WinBits nBits)
                  :Window(_pParent, nBits | WB_3DLOOK)
                  ,m_xORB(_rxORB)
                  ,m_nActivePage(0)
    {
        DBG_CTOR(OPropertyBrowserView,NULL);

        m_pPropBox = new OPropertyEditor( this );
        m_pPropBox->SetHelpId(HID_FM_PROPDLG_TABCTR);
        m_pPropBox->setPageActivationHandler(LINK(this, OPropertyBrowserView, OnPageActivation));

        m_pPropBox->Show();
    }

    //------------------------------------------------------------------------
    IMPL_LINK(OPropertyBrowserView, OnPageActivation, void*, EMPTYARG)
    {
        m_nActivePage = m_pPropBox->GetCurPage();
        if (m_aPageActivationHandler.IsSet())
            m_aPageActivationHandler.Call(NULL);
        return 0L;
    }

    //------------------------------------------------------------------------
    OPropertyBrowserView::~OPropertyBrowserView()
    {
        if(m_pPropBox)
        {
            sal_uInt16 nTmpPage = m_pPropBox->GetCurPage();
            if (nTmpPage)
                m_nActivePage = nTmpPage;
            ::std::auto_ptr<Window> aTemp(m_pPropBox);
            m_pPropBox = NULL;
        }
        m_xORB = NULL;

        DBG_DTOR(OPropertyBrowserView, NULL);
    }

    //------------------------------------------------------------------------
    void OPropertyBrowserView::activatePage(sal_uInt16 _nPage)
    {
        m_nActivePage = _nPage;
        getPropertyBox()->SetPage(m_nActivePage);
    }

    //------------------------------------------------------------------------
    void OPropertyBrowserView::GetFocus()
    {
        if (m_pPropBox)
            m_pPropBox->GrabFocus();
        else
            Window::GetFocus();
    }

    //------------------------------------------------------------------------
    void OPropertyBrowserView::Resize()
    {
        Size aSize = GetOutputSizePixel();
        m_pPropBox->SetSizePixel(aSize);
    }

    // #95343# ---------------------------------------------------------------
    ::com::sun::star::awt::Size OPropertyBrowserView::getMinimumSize()
    {
        Size aSize = GetOutputSizePixel();
        aSize.setHeight( 250 );
        if( m_pPropBox )
            aSize.setWidth( m_pPropBox->getMinimumWidth() );
        return ::com::sun::star::awt::Size( aSize.Width(), aSize.Height() );
    }

//............................................................................
} // namespace pcr
//............................................................................

<|endoftext|>


<commit_before>/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2010-2011, 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 Willow Garage, Inc. 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.
 *
 * $Id$
 *
 */

#ifndef PCL_FEATURES_IMPL_CVFH_H_
#define PCL_FEATURES_IMPL_CVFH_H_

#include <pcl/features/cvfh.h>
#include <pcl/features/pfh.h>

//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointInT, typename PointNT, typename PointOutT> void
pcl::CVFHEstimation<PointInT, PointNT, PointOutT>::extractEuclideanClustersSmooth (
    const pcl::PointCloud<pcl::PointNormal> &cloud,
    const pcl::PointCloud<pcl::PointNormal> &normals,
    float tolerance,
    const pcl::search::Search<pcl::PointNormal>::Ptr &tree,
    std::vector<pcl::PointIndices> &clusters,
    double eps_angle,
    unsigned int min_pts_per_cluster,
    unsigned int max_pts_per_cluster)
{
  if (tree->getInputCloud ()->points.size () != cloud.points.size ())
  {
    PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%zu) than the input cloud (%zu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ());
    return;
  }
  if (cloud.points.size () != normals.points.size ())
  {
    PCL_ERROR ("[pcl::extractEuclideanClusters] Number of points in the input point cloud (%zu) different than normals (%zu)!\n", cloud.points.size (), normals.points.size ());
    return;
  }

  // Create a bool vector of processed point indices, and initialize it to false
  std::vector<bool> processed (cloud.points.size (), false);

  std::vector<int> nn_indices;
  std::vector<float> nn_distances;
  // Process all points in the indices vector
  for (int i = 0; i < static_cast<int> (cloud.points.size ()); ++i)
  {
    if (processed[i])
      continue;

    std::vector<unsigned int> seed_queue;
    int sq_idx = 0;
    seed_queue.push_back (i);

    processed[i] = true;

    while (sq_idx < static_cast<int> (seed_queue.size ()))
    {
      // Search for sq_idx
      if (!tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances))
      {
        sq_idx++;
        continue;
      }

      for (size_t j = 1; j < nn_indices.size (); ++j) // nn_indices[0] should be sq_idx
      {
        if (processed[nn_indices[j]]) // Has this point been processed before ?
          continue;

        //processed[nn_indices[j]] = true;
        // [-1;1]

        double dot_p = normals.points[seed_queue[sq_idx]].normal[0] * normals.points[nn_indices[j]].normal[0]
                     + normals.points[seed_queue[sq_idx]].normal[1] * normals.points[nn_indices[j]].normal[1]
                     + normals.points[seed_queue[sq_idx]].normal[2] * normals.points[nn_indices[j]].normal[2];

        if (fabs (acos (dot_p)) < eps_angle)
        {
          processed[nn_indices[j]] = true;
          seed_queue.push_back (nn_indices[j]);
        }
      }

      sq_idx++;
    }

    // If this queue is satisfactory, add to the clusters
    if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
    {
      pcl::PointIndices r;
      r.indices.resize (seed_queue.size ());
      for (size_t j = 0; j < seed_queue.size (); ++j)
        r.indices[j] = seed_queue[j];

      std::sort (r.indices.begin (), r.indices.end ());
      r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ());

      r.header = cloud.header;
      clusters.push_back (r); // We could avoid a copy by working directly in the vector
    }
  }
}

//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointInT, typename PointNT, typename PointOutT> void
pcl::CVFHEstimation<PointInT, PointNT, PointOutT>::filterNormalsWithHighCurvature (
    const pcl::PointCloud<PointNT> & cloud,
    std::vector<int> &indices_to_use,
    std::vector<int> &indices_out,
    std::vector<int> &indices_in,
    float threshold)
{
  indices_out.resize (cloud.points.size ());
  indices_in.resize (cloud.points.size ());

  size_t in, out;
  in = out = 0;

  for (int i = 0; i < static_cast<int> (indices_to_use.size ()); i++)
  {
    if (cloud.points[indices_to_use[i]].curvature > threshold)
    {
      indices_out[out] = indices_to_use[i];
      out++;
    }
    else
    {
      indices_in[in] = indices_to_use[i];
      in++;
    }
  }

  indices_out.resize (out);
  indices_in.resize (in);
}

//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointInT, typename PointNT, typename PointOutT> void
pcl::CVFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
{
  // Check if input was set
  if (!normals_)
  {
    PCL_ERROR ("[pcl::%s::computeFeature] No input dataset containing normals was given!\n", getClassName ().c_str ());
    output.width = output.height = 0;
    output.points.clear ();
    return;
  }
  if (normals_->points.size () != surface_->points.size ())
  {
    PCL_ERROR ("[pcl::%s::computeFeature] The number of points in the input dataset differs from the number of points in the dataset containing the normals!\n", getClassName ().c_str ());
    output.width = output.height = 0;
    output.points.clear ();
    return;
  }

  centroids_dominant_orientations_.clear ();

  // ---[ Step 0: remove normals with high curvature
  std::vector<int> indices_out;
  std::vector<int> indices_in;
  filterNormalsWithHighCurvature (*normals_, *indices_, indices_out, indices_in, curv_threshold_);

  pcl::PointCloud<pcl::PointNormal>::Ptr normals_filtered_cloud (new pcl::PointCloud<pcl::PointNormal> ());
  normals_filtered_cloud->width = static_cast<uint32_t> (indices_in.size ());
  normals_filtered_cloud->height = 1;
  normals_filtered_cloud->points.resize (normals_filtered_cloud->width);

  for (size_t i = 0; i < indices_in.size (); ++i)
  {
    normals_filtered_cloud->points[i].x = surface_->points[indices_in[i]].x;
    normals_filtered_cloud->points[i].y = surface_->points[indices_in[i]].y;
    normals_filtered_cloud->points[i].z = surface_->points[indices_in[i]].z;
  }

  std::vector<pcl::PointIndices> clusters;

  if(normals_filtered_cloud->points.size() >= min_points_) {
    //recompute normals and use them for clustering
    KdTreePtr normals_tree_filtered (new pcl::search::KdTree<pcl::PointNormal> (false));
    normals_tree_filtered->setInputCloud (normals_filtered_cloud);

    NormalEstimator n3d;
    n3d.setRadiusSearch (radius_normals_);
    n3d.setSearchMethod (normals_tree_filtered);
    n3d.setInputCloud (normals_filtered_cloud);
    n3d.compute (*normals_filtered_cloud);

    KdTreePtr normals_tree (new pcl::search::KdTree<pcl::PointNormal> (false));
    normals_tree->setInputCloud (normals_filtered_cloud);

    extractEuclideanClustersSmooth (*normals_filtered_cloud,
                                    *normals_filtered_cloud,
                                    cluster_tolerance_,
                                    normals_tree,
                                    clusters,
                                    eps_angle_threshold_,
                                    static_cast<unsigned int> (min_points_));

  }

  VFHEstimator vfh;
  vfh.setInputCloud (surface_);
  vfh.setInputNormals (normals_);
  vfh.setIndices(indices_);
  vfh.setSearchMethod (this->tree_);
  vfh.setUseGivenNormal (true);
  vfh.setUseGivenCentroid (true);
  vfh.setNormalizeBins (normalize_bins_);
  vfh.setNormalizeDistance (true);
  vfh.setFillSizeComponent (true);
  output.height = 1;

  // ---[ Step 1b : check if any dominant cluster was found
  if (clusters.size () > 0)
  { // ---[ Step 1b.1 : If yes, compute CVFH using the cluster information

    for (size_t i = 0; i < clusters.size (); ++i) //for each cluster
    {
      Eigen::Vector4f avg_normal = Eigen::Vector4f::Zero ();
      Eigen::Vector4f avg_centroid = Eigen::Vector4f::Zero ();

      for (size_t j = 0; j < clusters[i].indices.size (); j++)
      {
        avg_normal += normals_filtered_cloud->points[clusters[i].indices[j]].getNormalVector4fMap ();
        avg_centroid += normals_filtered_cloud->points[clusters[i].indices[j]].getVector4fMap ();
      }

      avg_normal /= static_cast<float> (clusters[i].indices.size ());
      avg_centroid /= static_cast<float> (clusters[i].indices.size ());

      Eigen::Vector4f centroid_test;
      pcl::compute3DCentroid (*normals_filtered_cloud, centroid_test);
      avg_normal.normalize ();

      Eigen::Vector3f avg_norm (avg_normal[0], avg_normal[1], avg_normal[2]);
      Eigen::Vector3f avg_dominant_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);

      //append normal and centroid for the clusters
      dominant_normals_.push_back (avg_norm);
      centroids_dominant_orientations_.push_back (avg_dominant_centroid);
    }

    //compute modified VFH for all dominant clusters and add them to the list!
    output.points.resize (dominant_normals_.size ());
    output.width = static_cast<uint32_t> (dominant_normals_.size ());

    for (size_t i = 0; i < dominant_normals_.size (); ++i)
    {
      //configure VFH computation for CVFH
      vfh.setNormalToUse (dominant_normals_[i]);
      vfh.setCentroidToUse (centroids_dominant_orientations_[i]);
      pcl::PointCloud<pcl::VFHSignature308> vfh_signature;
      vfh.compute (vfh_signature);
      output.points[i] = vfh_signature.points[0];
    }
  }
  else
  { // ---[ Step 1b.1 : If no, compute CVFH using all the object points
    Eigen::Vector4f avg_centroid;
    pcl::compute3DCentroid (*surface_, avg_centroid);
    Eigen::Vector3f cloud_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
    centroids_dominant_orientations_.push_back (cloud_centroid);

    //configure VFH computation for CVFH using all object points
    vfh.setCentroidToUse (cloud_centroid);
    vfh.setUseGivenNormal (false);

    pcl::PointCloud<pcl::VFHSignature308> vfh_signature;
    vfh.compute (vfh_signature);

    output.points.resize (1);
    output.width = 1;

    output.points[0] = vfh_signature.points[0];
  }
}

#define PCL_INSTANTIATE_CVFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::CVFHEstimation<T,NT,OutT>;

#endif    // PCL_FEATURES_IMPL_VFH_H_
<commit_msg>Small indentation error...<commit_after>/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2010-2011, 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 Willow Garage, Inc. 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.
 *
 * $Id$
 *
 */

#ifndef PCL_FEATURES_IMPL_CVFH_H_
#define PCL_FEATURES_IMPL_CVFH_H_

#include <pcl/features/cvfh.h>
#include <pcl/features/pfh.h>

//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointInT, typename PointNT, typename PointOutT> void
pcl::CVFHEstimation<PointInT, PointNT, PointOutT>::extractEuclideanClustersSmooth (
    const pcl::PointCloud<pcl::PointNormal> &cloud,
    const pcl::PointCloud<pcl::PointNormal> &normals,
    float tolerance,
    const pcl::search::Search<pcl::PointNormal>::Ptr &tree,
    std::vector<pcl::PointIndices> &clusters,
    double eps_angle,
    unsigned int min_pts_per_cluster,
    unsigned int max_pts_per_cluster)
{
  if (tree->getInputCloud ()->points.size () != cloud.points.size ())
  {
    PCL_ERROR ("[pcl::extractEuclideanClusters] Tree built for a different point cloud dataset (%zu) than the input cloud (%zu)!\n", tree->getInputCloud ()->points.size (), cloud.points.size ());
    return;
  }
  if (cloud.points.size () != normals.points.size ())
  {
    PCL_ERROR ("[pcl::extractEuclideanClusters] Number of points in the input point cloud (%zu) different than normals (%zu)!\n", cloud.points.size (), normals.points.size ());
    return;
  }

  // Create a bool vector of processed point indices, and initialize it to false
  std::vector<bool> processed (cloud.points.size (), false);

  std::vector<int> nn_indices;
  std::vector<float> nn_distances;
  // Process all points in the indices vector
  for (int i = 0; i < static_cast<int> (cloud.points.size ()); ++i)
  {
    if (processed[i])
      continue;

    std::vector<unsigned int> seed_queue;
    int sq_idx = 0;
    seed_queue.push_back (i);

    processed[i] = true;

    while (sq_idx < static_cast<int> (seed_queue.size ()))
    {
      // Search for sq_idx
      if (!tree->radiusSearch (seed_queue[sq_idx], tolerance, nn_indices, nn_distances))
      {
        sq_idx++;
        continue;
      }

      for (size_t j = 1; j < nn_indices.size (); ++j) // nn_indices[0] should be sq_idx
      {
        if (processed[nn_indices[j]]) // Has this point been processed before ?
          continue;

        //processed[nn_indices[j]] = true;
        // [-1;1]

        double dot_p = normals.points[seed_queue[sq_idx]].normal[0] * normals.points[nn_indices[j]].normal[0]
                     + normals.points[seed_queue[sq_idx]].normal[1] * normals.points[nn_indices[j]].normal[1]
                     + normals.points[seed_queue[sq_idx]].normal[2] * normals.points[nn_indices[j]].normal[2];

        if (fabs (acos (dot_p)) < eps_angle)
        {
          processed[nn_indices[j]] = true;
          seed_queue.push_back (nn_indices[j]);
        }
      }

      sq_idx++;
    }

    // If this queue is satisfactory, add to the clusters
    if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
    {
      pcl::PointIndices r;
      r.indices.resize (seed_queue.size ());
      for (size_t j = 0; j < seed_queue.size (); ++j)
        r.indices[j] = seed_queue[j];

      std::sort (r.indices.begin (), r.indices.end ());
      r.indices.erase (std::unique (r.indices.begin (), r.indices.end ()), r.indices.end ());

      r.header = cloud.header;
      clusters.push_back (r); // We could avoid a copy by working directly in the vector
    }
  }
}

//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointInT, typename PointNT, typename PointOutT> void
pcl::CVFHEstimation<PointInT, PointNT, PointOutT>::filterNormalsWithHighCurvature (
    const pcl::PointCloud<PointNT> & cloud,
    std::vector<int> &indices_to_use,
    std::vector<int> &indices_out,
    std::vector<int> &indices_in,
    float threshold)
{
  indices_out.resize (cloud.points.size ());
  indices_in.resize (cloud.points.size ());

  size_t in, out;
  in = out = 0;

  for (int i = 0; i < static_cast<int> (indices_to_use.size ()); i++)
  {
    if (cloud.points[indices_to_use[i]].curvature > threshold)
    {
      indices_out[out] = indices_to_use[i];
      out++;
    }
    else
    {
      indices_in[in] = indices_to_use[i];
      in++;
    }
  }

  indices_out.resize (out);
  indices_in.resize (in);
}

//////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointInT, typename PointNT, typename PointOutT> void
pcl::CVFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
{
  // Check if input was set
  if (!normals_)
  {
    PCL_ERROR ("[pcl::%s::computeFeature] No input dataset containing normals was given!\n", getClassName ().c_str ());
    output.width = output.height = 0;
    output.points.clear ();
    return;
  }
  if (normals_->points.size () != surface_->points.size ())
  {
    PCL_ERROR ("[pcl::%s::computeFeature] The number of points in the input dataset differs from the number of points in the dataset containing the normals!\n", getClassName ().c_str ());
    output.width = output.height = 0;
    output.points.clear ();
    return;
  }

  centroids_dominant_orientations_.clear ();

  // ---[ Step 0: remove normals with high curvature
  std::vector<int> indices_out;
  std::vector<int> indices_in;
  filterNormalsWithHighCurvature (*normals_, *indices_, indices_out, indices_in, curv_threshold_);

  pcl::PointCloud<pcl::PointNormal>::Ptr normals_filtered_cloud (new pcl::PointCloud<pcl::PointNormal> ());
  normals_filtered_cloud->width = static_cast<uint32_t> (indices_in.size ());
  normals_filtered_cloud->height = 1;
  normals_filtered_cloud->points.resize (normals_filtered_cloud->width);

  for (size_t i = 0; i < indices_in.size (); ++i)
  {
    normals_filtered_cloud->points[i].x = surface_->points[indices_in[i]].x;
    normals_filtered_cloud->points[i].y = surface_->points[indices_in[i]].y;
    normals_filtered_cloud->points[i].z = surface_->points[indices_in[i]].z;
  }

  std::vector<pcl::PointIndices> clusters;

  if(normals_filtered_cloud->points.size() >= min_points_)
  {
    //recompute normals and use them for clustering
    KdTreePtr normals_tree_filtered (new pcl::search::KdTree<pcl::PointNormal> (false));
    normals_tree_filtered->setInputCloud (normals_filtered_cloud);

    NormalEstimator n3d;
    n3d.setRadiusSearch (radius_normals_);
    n3d.setSearchMethod (normals_tree_filtered);
    n3d.setInputCloud (normals_filtered_cloud);
    n3d.compute (*normals_filtered_cloud);

    KdTreePtr normals_tree (new pcl::search::KdTree<pcl::PointNormal> (false));
    normals_tree->setInputCloud (normals_filtered_cloud);

    extractEuclideanClustersSmooth (*normals_filtered_cloud,
                                    *normals_filtered_cloud,
                                    cluster_tolerance_,
                                    normals_tree,
                                    clusters,
                                    eps_angle_threshold_,
                                    static_cast<unsigned int> (min_points_));

  }

  VFHEstimator vfh;
  vfh.setInputCloud (surface_);
  vfh.setInputNormals (normals_);
  vfh.setIndices(indices_);
  vfh.setSearchMethod (this->tree_);
  vfh.setUseGivenNormal (true);
  vfh.setUseGivenCentroid (true);
  vfh.setNormalizeBins (normalize_bins_);
  vfh.setNormalizeDistance (true);
  vfh.setFillSizeComponent (true);
  output.height = 1;

  // ---[ Step 1b : check if any dominant cluster was found
  if (clusters.size () > 0)
  { // ---[ Step 1b.1 : If yes, compute CVFH using the cluster information

    for (size_t i = 0; i < clusters.size (); ++i) //for each cluster
    {
      Eigen::Vector4f avg_normal = Eigen::Vector4f::Zero ();
      Eigen::Vector4f avg_centroid = Eigen::Vector4f::Zero ();

      for (size_t j = 0; j < clusters[i].indices.size (); j++)
      {
        avg_normal += normals_filtered_cloud->points[clusters[i].indices[j]].getNormalVector4fMap ();
        avg_centroid += normals_filtered_cloud->points[clusters[i].indices[j]].getVector4fMap ();
      }

      avg_normal /= static_cast<float> (clusters[i].indices.size ());
      avg_centroid /= static_cast<float> (clusters[i].indices.size ());

      Eigen::Vector4f centroid_test;
      pcl::compute3DCentroid (*normals_filtered_cloud, centroid_test);
      avg_normal.normalize ();

      Eigen::Vector3f avg_norm (avg_normal[0], avg_normal[1], avg_normal[2]);
      Eigen::Vector3f avg_dominant_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);

      //append normal and centroid for the clusters
      dominant_normals_.push_back (avg_norm);
      centroids_dominant_orientations_.push_back (avg_dominant_centroid);
    }

    //compute modified VFH for all dominant clusters and add them to the list!
    output.points.resize (dominant_normals_.size ());
    output.width = static_cast<uint32_t> (dominant_normals_.size ());

    for (size_t i = 0; i < dominant_normals_.size (); ++i)
    {
      //configure VFH computation for CVFH
      vfh.setNormalToUse (dominant_normals_[i]);
      vfh.setCentroidToUse (centroids_dominant_orientations_[i]);
      pcl::PointCloud<pcl::VFHSignature308> vfh_signature;
      vfh.compute (vfh_signature);
      output.points[i] = vfh_signature.points[0];
    }
  }
  else
  { // ---[ Step 1b.1 : If no, compute CVFH using all the object points
    Eigen::Vector4f avg_centroid;
    pcl::compute3DCentroid (*surface_, avg_centroid);
    Eigen::Vector3f cloud_centroid (avg_centroid[0], avg_centroid[1], avg_centroid[2]);
    centroids_dominant_orientations_.push_back (cloud_centroid);

    //configure VFH computation for CVFH using all object points
    vfh.setCentroidToUse (cloud_centroid);
    vfh.setUseGivenNormal (false);

    pcl::PointCloud<pcl::VFHSignature308> vfh_signature;
    vfh.compute (vfh_signature);

    output.points.resize (1);
    output.width = 1;

    output.points[0] = vfh_signature.points[0];
  }
}

#define PCL_INSTANTIATE_CVFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::CVFHEstimation<T,NT,OutT>;

#endif    // PCL_FEATURES_IMPL_VFH_H_
<|endoftext|>