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

#include "geometrypicking.h"

#include <inviwo/core/interaction/pickingmanager.h>
#include <inviwo/core/datastructures/geometry/simplemeshcreator.h>
#include <modules/opengl/rendering/meshrenderer.h>
#include <modules/opengl/glwrap/shader.h>
#include <modules/opengl/textureutils.h>

namespace inviwo {

ProcessorClassIdentifier(GeometryPicking, "org.inviwo.GeometryPicking");
ProcessorDisplayName(GeometryPicking, "Geometry Picking");
ProcessorTags(GeometryPicking, Tags::GL);
ProcessorCategory(GeometryPicking, "Geometry Rendering");
ProcessorCodeState(GeometryPicking, CODE_STATE_STABLE);

GeometryPicking::GeometryPicking()
    : CompositeProcessorGL()
    , geometryInport_("geometryInport")
    , imageInport_("imageInport")
    , outport_("outport")
    , position_("position", "Position", vec3(0.0f), vec3(-100.f), vec3(100.f))
    , camera_("camera", "Camera", vec3(0.0f, 0.0f, -2.0f), vec3(0.0f, 0.0f, 0.0f),
              vec3(0.0f, 1.0f, 0.0f))
    , trackball_(&camera_) {
    addPort(geometryInport_);
    addPort(imageInport_);
    addPort(outport_);
    addProperty(position_);
    addProperty(camera_);
    addProperty(trackball_);
}

GeometryPicking::~GeometryPicking() {}

void GeometryPicking::initialize() {
    CompositeProcessorGL::initialize();
    shader_ = new Shader("standard.vert", "picking.frag");
    widgetPickingObject_ = PickingManager::getPtr()->registerPickingCallback(
        this, &GeometryPicking::updateWidgetPositionFromPicking);
}

void GeometryPicking::deinitialize() {
    CompositeProcessorGL::deinitialize();
    delete shader_;
    shader_ = NULL;
    PickingManager::getPtr()->unregisterPickingObject(widgetPickingObject_);
}

void GeometryPicking::updateWidgetPositionFromPicking(const PickingObject* p) {
    vec2 move = p->getPickingMove();

    if (move.x == 0.f && move.y == 0.f) return;

    vec2 pos = p->getPickingPosition();
    float depth = static_cast<float>(p->getPickingDepth());
    vec3 startNdc = vec3(2.f * pos - 1.f, 2.f * depth - 1.f);
    vec3 endNdc = vec3(2.f * (pos + move) - 1.f, 2.f * depth - 1.f);
    vec3 startWorld = camera_.getWorldPosFromNormalizedDeviceCoords(startNdc);
    vec3 endWorld = camera_.getWorldPosFromNormalizedDeviceCoords(endNdc);
    position_.set(position_.get() + (endWorld - startWorld));
    invalidate(INVALID_OUTPUT);
}

void GeometryPicking::process() {
    utilgl::activateAndClearTarget(outport_, COLOR_DEPTH_PICKING);
    MeshRenderer renderer(static_cast<const Mesh*>(geometryInport_.getData()));
    shader_->activate();
    shader_->setUniform("pickingColor_", widgetPickingObject_->getPickingColor());

    const SpatialCameraCoordinateTransformer<3>& ct =
        geometryInport_.getData()->getCoordinateTransformer(&camera_);

    mat4 dataToClip_ =
        ct.getWorldToClipMatrix() * glm::translate(position_.get()) * ct.getDataToWorldMatrix();

    shader_->setUniform("dataToClip_", dataToClip_);

    glEnable(GL_CULL_FACE);
    glCullFace(GL_BACK);
    glDepthFunc(GL_ALWAYS);
    renderer.render();
    glDepthFunc(GL_LESS);
    glDisable(GL_CULL_FACE);

    shader_->deactivate();
    utilgl::deactivateCurrentTarget();
    compositePortsToOutport(outport_, COLOR_ONLY, imageInport_);
}

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

#include "geometrypicking.h"

#include <inviwo/core/interaction/pickingmanager.h>
#include <inviwo/core/datastructures/geometry/simplemeshcreator.h>
#include <modules/opengl/rendering/meshrenderer.h>
#include <modules/opengl/glwrap/shader.h>
#include <modules/opengl/textureutils.h>

namespace inviwo {

ProcessorClassIdentifier(GeometryPicking, "org.inviwo.GeometryPicking");
ProcessorDisplayName(GeometryPicking, "Geometry Picking");
ProcessorTags(GeometryPicking, Tags::GL);
ProcessorCategory(GeometryPicking, "Geometry Rendering");
ProcessorCodeState(GeometryPicking, CODE_STATE_STABLE);

GeometryPicking::GeometryPicking()
    : CompositeProcessorGL()
    , geometryInport_("geometryInport")
    , imageInport_("imageInport")
    , outport_("outport")
    , position_("position", "Position", vec3(0.0f), vec3(-100.f), vec3(100.f))
    , camera_("camera", "Camera", vec3(0.0f, 0.0f, -2.0f), vec3(0.0f, 0.0f, 0.0f),
              vec3(0.0f, 1.0f, 0.0f))
    , trackball_(&camera_) {
    addPort(geometryInport_);
    addPort(imageInport_);
    addPort(outport_);
    addProperty(position_);
    addProperty(camera_);
    addProperty(trackball_);
}

GeometryPicking::~GeometryPicking() {}

void GeometryPicking::initialize() {
    CompositeProcessorGL::initialize();
    shader_ = new Shader("standard.vert", "picking.frag");
    widgetPickingObject_ = PickingManager::getPtr()->registerPickingCallback(
        this, &GeometryPicking::updateWidgetPositionFromPicking);
}

void GeometryPicking::deinitialize() {
    CompositeProcessorGL::deinitialize();
    delete shader_;
    shader_ = NULL;
    PickingManager::getPtr()->unregisterPickingObject(widgetPickingObject_);
}

void GeometryPicking::updateWidgetPositionFromPicking(const PickingObject* p) {
    vec2 move = p->getPickingMove();

    if (move.x == 0.f && move.y == 0.f) return;

    vec2 pos = p->getPickingPosition();
    float depth = static_cast<float>(p->getPickingDepth());
    vec3 startNdc = vec3(2.f * pos - 1.f, 2.f * depth - 1.f);
    vec3 endNdc = vec3(2.f * (pos + move) - 1.f, 2.f * depth - 1.f);
    vec3 startWorld = camera_.getWorldPosFromNormalizedDeviceCoords(startNdc);
    vec3 endWorld = camera_.getWorldPosFromNormalizedDeviceCoords(endNdc);
    position_.set(position_.get() + (endWorld - startWorld));
    invalidate(INVALID_OUTPUT);
}

void GeometryPicking::process() {
    utilgl::activateAndClearTarget(outport_, COLOR_DEPTH_PICKING);
    MeshRenderer renderer(static_cast<const Mesh*>(geometryInport_.getData()));
    shader_->activate();
    shader_->setUniform("pickingColor_", widgetPickingObject_->getPickingColor());

    const SpatialCameraCoordinateTransformer<3>& ct =
        geometryInport_.getData()->getCoordinateTransformer(&camera_);

    mat4 dataToClip_ =
        ct.getWorldToClipMatrix() * glm::translate(position_.get()) * ct.getDataToWorldMatrix();

    shader_->setUniform("dataToClip_", dataToClip_);

    glEnable(GL_CULL_FACE);
    glCullFace(GL_BACK);
    glDepthFunc(GL_ALWAYS);
    renderer.render();
    glDepthFunc(GL_LESS);
    glDisable(GL_CULL_FACE);

    shader_->deactivate();
    utilgl::deactivateCurrentTarget();
    compositePortsToOutport(outport_, COLOR_DEPTH_PICKING, imageInport_);
}

}  // namespace
<|endoftext|>


<commit_before>// =============================================================================
// PROJECT CHRONO - http://projectchrono.org
//
// Copyright (c) 2014 projectchrono.org
// All right reserved.
//
// Use of this source code is governed by a BSD-style license that can be found
// in the LICENSE file at the top level of the distribution and at
// http://projectchrono.org/license-chrono.txt.
//
// =============================================================================
// Authors: Radu Serban
// =============================================================================
//
// Main driver function for a vehicle specified through JSON files.
//
// If using the Irrlicht interface, driver inputs are obtained from the keyboard.
//
// The vehicle reference frame has Z up, X towards the front of the vehicle, and
// Y pointing to the left.
//
// =============================================================================

#include <vector>

#include "core/ChFileutils.h"
#include "core/ChStream.h"
#include "core/ChRealtimeStep.h"
#include "physics/ChSystem.h"
#include "physics/ChLinkDistance.h"

#include "ChronoT_config.h"

#include "utils/ChUtilsInputOutput.h"
#include "utils/ChUtilsData.h"

#include "subsys/vehicle/Vehicle.h"
#include "subsys/powertrain/SimplePowertrain.h"
#include "subsys/driver/ChDataDriver.h"
#include "subsys/tire/RigidTire.h"
#include "subsys/terrain/RigidTerrain.h"

// If Irrlicht support is available...
#if IRRLICHT_ENABLED
  // ...include additional headers
# include "unit_IRRLICHT/ChIrrApp.h"
# include "subsys/driver/ChIrrGuiDriver.h"

  // ...and specify whether the demo should actually use Irrlicht
# define USE_IRRLICHT
#endif

using namespace chrono;

// =============================================================================

// JSON file for vehicle model
std::string vehicle_file = utils::GetModelDataFile("hmmwv/vehicle/HMMWV_Vehicle.json");
//std::string vehicle_file = utils::GetModelDataFile("generic/vehicle/Vehicle_DoubleWishbones.json");
//std::string vehicle_file = utils::GetModelDataFile("generic/vehicle/Vehicle_MultiLinks.json");
//std::string vehicle_file = utils::GetModelDataFile("generic/vehicle/Vehicle_SolidAxles.json");
//std::string vehicle_file = utils::GetModelDataFile("generic/vehicle/Vehicle_ThreeAxles.json");

// JSON files for tire models (rigid) and powertrain (simple)
std::string rigidtire_file = utils::GetModelDataFile("hmmwv/tire/HMMWV_RigidTire.json");
std::string simplepowertrain_file = utils::GetModelDataFile("hmmwv/powertrain/HMMWV_SimplePowertrain.json");

// Driver input file (if not using Irrlicht)
std::string driver_file = utils::GetModelDataFile("generic/driver/Sample_Maneuver.txt");

// Initial vehicle position
ChVector<> initLoc(0, 0, 1.0);

// Initial vehicle orientation
ChQuaternion<> initRot(1, 0, 0, 0);
//ChQuaternion<> initRot(0.866025, 0, 0, 0.5);
//ChQuaternion<> initRot(0.7071068, 0, 0, 0.7071068);
//ChQuaternion<> initRot(0.25882, 0, 0, 0.965926);
//ChQuaternion<> initRot(0, 0, 0, 1);

// Rigid terrain dimensions
double terrainHeight = 0;
double terrainLength = 100.0;   // size in X direction
double terrainWidth  = 100.0;   // size in Y direction

// Simulation step size
double step_size = 0.001;

// Time interval between two render frames
double render_step_size = 1.0 / 50;   // FPS = 50

// Time interval between two output frames
double output_step_size = 1.0 / 1;    // once a second

#ifdef USE_IRRLICHT
  // Point on chassis tracked by the camera
  ChVector<> trackPoint(0.0, 0.0, 1.75);
#else
  double tend = 20.0;

  const std::string out_dir = "../VEHICLE";
  const std::string pov_dir = out_dir + "/POVRAY";
#endif

// =============================================================================

int main(int argc, char* argv[])
{
  SetChronoDataPath(CHRONO_DATA_DIR);

  // --------------------------
  // Create the various modules
  // --------------------------

  // Create the vehicle system
  Vehicle vehicle(vehicle_file, false);
  vehicle.Initialize(ChCoordsys<>(initLoc, initRot));

  // Create the ground
  RigidTerrain terrain(vehicle, terrainHeight, terrainLength, terrainWidth, 0.8);

  // Create and initialize the powertrain system
  SimplePowertrain powertrain(simplepowertrain_file);
  powertrain.Initialize();

  // Create and initialize the tires
  int num_axles = vehicle.GetNumberAxles();
  int num_wheels = 2 * num_axles;
  std::vector<ChSharedPtr<ChTire> > tires(num_wheels);

  for (int i = 0; i < num_wheels; i++) {
    ChSharedPtr<RigidTire> tire(new RigidTire(rigidtire_file, terrain));
    tire->Initialize(vehicle.GetWheelBody(i));
    tires[i] = tire;
  }

#ifdef USE_IRRLICHT
  irr::ChIrrApp application(&vehicle,
                            L"Vehicle demo",
                            irr::core::dimension2d<irr::u32>(1000, 800),
                            false,
                            true);

  // make a skybox that has Z pointing up (default application.AddTypicalSky() makes Y up)
  std::string mtexturedir = GetChronoDataFile("skybox/");
  std::string str_lf = mtexturedir + "sky_lf.jpg";
  std::string str_up = mtexturedir + "sky_up.jpg";
  std::string str_dn = mtexturedir + "sky_dn.jpg";
  irr::video::ITexture* map_skybox_side =
      application.GetVideoDriver()->getTexture(str_lf.c_str());
  irr::scene::ISceneNode* mbox = application.GetSceneManager()->addSkyBoxSceneNode(
      application.GetVideoDriver()->getTexture(str_up.c_str()),
      application.GetVideoDriver()->getTexture(str_dn.c_str()),
      map_skybox_side,
      map_skybox_side,
      map_skybox_side,
      map_skybox_side);
  mbox->setRotation( irr::core::vector3df(90,0,0));

  bool do_shadows = true; // shadow map is experimental
  irr::scene::ILightSceneNode* mlight = 0;

  if (do_shadows)
  {
    mlight = application.AddLightWithShadow(
      irr::core::vector3df(10.f, 30.f, 60.f),
      irr::core::vector3df(0.f, 0.f, 0.f),
      150, 60, 80, 15, 512, irr::video::SColorf(1, 1, 1), false, false);
  }
  else
  {
    application.AddTypicalLights(
      irr::core::vector3df(30.f, -30.f, 100.f),
      irr::core::vector3df(30.f, 50.f, 100.f),
      250, 130);
  }

  application.SetTimestep(step_size);

  ChIrrGuiDriver driver(application, vehicle, powertrain, trackPoint, 6.0, 0.5, true);

  // Set the time response for steering and throttle keyboard inputs.
  // NOTE: this is not exact, since we do not render quite at the specified FPS.
  double steering_time = 1.0;  // time to go from 0 to +1 (or from 0 to -1)
  double throttle_time = 1.0;  // time to go from 0 to +1
  double braking_time = 0.3;   // time to go from 0 to +1
  driver.SetSteeringDelta(render_step_size / steering_time);
  driver.SetThrottleDelta(render_step_size / throttle_time);
  driver.SetBrakingDelta(render_step_size / braking_time);

  // Set up the assets for rendering
  application.AssetBindAll();
  application.AssetUpdateAll();
  if (do_shadows)
  {
    application.AddShadowAll();
  }
#else
  ChDataDriver driver(driver_file);
#endif

  // ---------------
  // Simulation loop
  // ---------------

  // Inter-module communication data
  ChTireForces   tire_forces(num_wheels);
  ChWheelStates  wheel_states(num_wheels);
  double         driveshaft_speed;
  double         powertrain_torque;
  double         throttle_input;
  double         steering_input;
  double         braking_input;

  // Number of simulation steps between two 3D view render frames
  int render_steps = (int)std::ceil(render_step_size / step_size);

  // Number of simulation steps between two output frames
  int output_steps = (int)std::ceil(output_step_size / step_size);

  // Initialize simulation frame counter and simulation time
  int step_number = 0;
  double time = 0;

#ifdef USE_IRRLICHT

  ChRealtimeStepTimer realtime_timer;

  while (application.GetDevice()->run())
  {
    // update the position of the shadow mapping so that it follows the car
    if (do_shadows)
    {
      ChVector<> lightaim = vehicle.GetChassisPos();
      ChVector<> lightpos = vehicle.GetChassisPos() + ChVector<>(10, 30, 60);
      irr::core::vector3df mlightpos((irr::f32)lightpos.x, (irr::f32)lightpos.y, (irr::f32)lightpos.z);
      irr::core::vector3df mlightaim((irr::f32)lightaim.x, (irr::f32)lightaim.y, (irr::f32)lightaim.z);
      application.GetEffects()->getShadowLight(0).setPosition(mlightpos);
      application.GetEffects()->getShadowLight(0).setTarget(mlightaim);
      mlight->setPosition(mlightpos);
    }

    // Render scene
    if (step_number % render_steps == 0) {
      application.GetVideoDriver()->beginScene(true, true, irr::video::SColor(255, 140, 161, 192));
      driver.DrawAll();
      application.GetVideoDriver()->endScene();
    }

    // Collect output data from modules (for inter-module communication)
    throttle_input = driver.GetThrottle();
    steering_input = driver.GetSteering();
    braking_input = driver.GetBraking();
    powertrain_torque = powertrain.GetOutputTorque();
    driveshaft_speed = vehicle.GetDriveshaftSpeed();
    for (int i = 0; i < num_wheels; i++) {
      tire_forces[i] = tires[i]->GetTireForce();
      wheel_states[i] = vehicle.GetWheelState(i);
    }

    // Update modules (process inputs from other modules)
    time = vehicle.GetChTime();
    driver.Update(time);
    powertrain.Update(time, throttle_input, driveshaft_speed);
    vehicle.Update(time, steering_input, braking_input, powertrain_torque, tire_forces);
    terrain.Update(time);
    for (int i = 0; i < num_wheels; i++)
      tires[i]->Update(time, wheel_states[i]);

    // Advance simulation for one timestep for all modules
    double step = realtime_timer.SuggestSimulationStep(step_size);
    driver.Advance(step);
    powertrain.Advance(step);
    vehicle.Advance(step);
    terrain.Advance(step);
    for (int i = 0; i < num_wheels; i++)
      tires[i]->Advance(step);

    // Increment frame number
    step_number++;
  }

  application.GetDevice()->drop();

#else

  int render_frame = 0;

  if(ChFileutils::MakeDirectory(out_dir.c_str()) < 0) {
    std::cout << "Error creating directory " << out_dir << std::endl;
    return 1;
  }
  if(ChFileutils::MakeDirectory(pov_dir.c_str()) < 0) {
    std::cout << "Error creating directory " << pov_dir << std::endl;
    return 1;
  }

  vehicle.ExportMeshPovray(out_dir);

  char filename[100];

  while (time < tend)
  {
    if (step_number % render_steps == 0) {
      // Output render data
      sprintf(filename, "%s/data_%03d.dat", pov_dir.c_str(), render_frame + 1);
      utils::WriteShapesPovray(&vehicle, filename);
      std::cout << "Output frame:   " << render_frame << std::endl;
      std::cout << "Sim frame:      " << step_number << std::endl;
      std::cout << "Time:           " << time << std::endl;
      std::cout << "   throttle: " << driver.GetThrottle()
                << "   steering: " << driver.GetSteering()
                << "   braking:  " << driver.GetBraking() << std::endl;
      std::cout << std::endl;
      render_frame++;
    }

    // Collect output data from modules (for inter-module communication)
    throttle_input = driver.GetThrottle();
    steering_input = driver.GetSteering();
    braking_input = driver.GetBraking();
    powertrain_torque = powertrain.GetOutputTorque();
    driveshaft_speed = vehicle.GetDriveshaftSpeed();
    for (int i = 0; i < num_wheels; i++) {
      tire_forces[i] = tires[i]->GetTireForce();
      wheel_states[i] = vehicle.GetWheelState(i);
    }

    // Update modules (process inputs from other modules)
    time = vehicle.GetChTime();
    driver.Update(time);
    powertrain.Update(time, throttle_input, driveshaft_speed);
    vehicle.Update(time, steering_input, braking_input, powertrain_torque, tire_forces);
    terrain.Update(time);
    for (int i = 0; i < num_wheels; i++)
      tires[i]->Update(time, wheel_states[i]);

    // Update modules (process inputs from other modules)
    time = vehicle.GetChTime();
    driver.Update(time);
    powertrain.Update(time, throttle_input, driveshaft_speed);
    vehicle.Update(time, steering_input, braking_input, powertrain_torque, tire_forces);
    terrain.Update(time);
    for (int i = 0; i < num_wheels; i++)
      tires[i]->Update(time, wheel_states[i]);

    // Advance simulation for one timestep for all modules
    driver.Advance(step_size);
    powertrain.Advance(step_size);
    vehicle.Advance(step_size);
    terrain.Advance(step_size);
    for (int i = 0; i < num_wheels; i++)
      tires[i]->Advance(step_size);

    // Increment frame number
    step_number++;
  }

#endif

  return 0;
}
<commit_msg>add Vehicle.DebugLog() to the demo_Vehicle main loop.<commit_after>// =============================================================================
// PROJECT CHRONO - http://projectchrono.org
//
// Copyright (c) 2014 projectchrono.org
// All right reserved.
//
// Use of this source code is governed by a BSD-style license that can be found
// in the LICENSE file at the top level of the distribution and at
// http://projectchrono.org/license-chrono.txt.
//
// =============================================================================
// Authors: Radu Serban
// =============================================================================
//
// Main driver function for a vehicle specified through JSON files.
//
// If using the Irrlicht interface, driver inputs are obtained from the keyboard.
//
// The vehicle reference frame has Z up, X towards the front of the vehicle, and
// Y pointing to the left.
//
// =============================================================================

#include <vector>

#include "core/ChFileutils.h"
#include "core/ChStream.h"
#include "core/ChRealtimeStep.h"
#include "physics/ChSystem.h"
#include "physics/ChLinkDistance.h"

#include "ChronoT_config.h"

#include "utils/ChUtilsInputOutput.h"
#include "utils/ChUtilsData.h"

#include "subsys/vehicle/Vehicle.h"
#include "subsys/powertrain/SimplePowertrain.h"
#include "subsys/driver/ChDataDriver.h"
#include "subsys/tire/RigidTire.h"
#include "subsys/terrain/RigidTerrain.h"

// If Irrlicht support is available...
#if IRRLICHT_ENABLED
  // ...include additional headers
# include "unit_IRRLICHT/ChIrrApp.h"
# include "subsys/driver/ChIrrGuiDriver.h"

  // ...and specify whether the demo should actually use Irrlicht
# define USE_IRRLICHT
#endif

// DEBUGGING: uncomment to print debug info
#define DEBUG_LOG

using namespace chrono;

// =============================================================================

// JSON file for vehicle model
std::string vehicle_file = utils::GetModelDataFile("hmmwv/vehicle/HMMWV_Vehicle_balanced.json");
//std::string vehicle_file = utils::GetModelDataFile("generic/vehicle/Vehicle_DoubleWishbones.json");
//std::string vehicle_file = utils::GetModelDataFile("generic/vehicle/Vehicle_MultiLinks.json");
//std::string vehicle_file = utils::GetModelDataFile("generic/vehicle/Vehicle_SolidAxles.json");
//std::string vehicle_file = utils::GetModelDataFile("generic/vehicle/Vehicle_ThreeAxles.json");

// JSON files for tire models (rigid) and powertrain (simple)
std::string rigidtire_file = utils::GetModelDataFile("hmmwv/tire/HMMWV_RigidTire.json");
std::string simplepowertrain_file = utils::GetModelDataFile("hmmwv/powertrain/HMMWV_SimplePowertrain.json");

// Driver input file (if not using Irrlicht)
std::string driver_file = utils::GetModelDataFile("generic/driver/Sample_Maneuver.txt");

// Initial vehicle position
ChVector<> initLoc(0, 0, 1.0);

// Initial vehicle orientation
ChQuaternion<> initRot(1, 0, 0, 0);
//ChQuaternion<> initRot(0.866025, 0, 0, 0.5);
//ChQuaternion<> initRot(0.7071068, 0, 0, 0.7071068);
//ChQuaternion<> initRot(0.25882, 0, 0, 0.965926);
//ChQuaternion<> initRot(0, 0, 0, 1);

// Rigid terrain dimensions
double terrainHeight = 0;
double terrainLength = 100.0;   // size in X direction
double terrainWidth  = 100.0;   // size in Y direction

// Simulation step size
double step_size = 0.001;

// Time interval between two render frames
double render_step_size = 1.0 / 50;   // FPS = 50

// Time interval between two output frames
double output_step_size = 1.0 / 1;    // once a second

#ifdef USE_IRRLICHT
  // Point on chassis tracked by the camera
  ChVector<> trackPoint(0.0, 0.0, 1.75);
#else
  double tend = 20.0;

  const std::string out_dir = "../VEHICLE";
  const std::string pov_dir = out_dir + "/POVRAY";
#endif

// =============================================================================

int main(int argc, char* argv[])
{
  SetChronoDataPath(CHRONO_DATA_DIR);

  // --------------------------
  // Create the various modules
  // --------------------------

  // Create the vehicle system
  Vehicle vehicle(vehicle_file, false);
  vehicle.Initialize(ChCoordsys<>(initLoc, initRot));

  // Create the ground
  RigidTerrain terrain(vehicle, terrainHeight, terrainLength, terrainWidth, 0.8);

  // Create and initialize the powertrain system
  SimplePowertrain powertrain(simplepowertrain_file);
  powertrain.Initialize();

  // Create and initialize the tires
  int num_axles = vehicle.GetNumberAxles();
  int num_wheels = 2 * num_axles;
  std::vector<ChSharedPtr<ChTire> > tires(num_wheels);

  for (int i = 0; i < num_wheels; i++) {
    ChSharedPtr<RigidTire> tire(new RigidTire(rigidtire_file, terrain));
    tire->Initialize(vehicle.GetWheelBody(i));
    tires[i] = tire;
  }

#ifdef USE_IRRLICHT
  irr::ChIrrApp application(&vehicle,
                            L"Vehicle demo",
                            irr::core::dimension2d<irr::u32>(1000, 800),
                            false,
                            true);

  // make a skybox that has Z pointing up (default application.AddTypicalSky() makes Y up)
  std::string mtexturedir = GetChronoDataFile("skybox/");
  std::string str_lf = mtexturedir + "sky_lf.jpg";
  std::string str_up = mtexturedir + "sky_up.jpg";
  std::string str_dn = mtexturedir + "sky_dn.jpg";
  irr::video::ITexture* map_skybox_side =
      application.GetVideoDriver()->getTexture(str_lf.c_str());
  irr::scene::ISceneNode* mbox = application.GetSceneManager()->addSkyBoxSceneNode(
      application.GetVideoDriver()->getTexture(str_up.c_str()),
      application.GetVideoDriver()->getTexture(str_dn.c_str()),
      map_skybox_side,
      map_skybox_side,
      map_skybox_side,
      map_skybox_side);
  mbox->setRotation( irr::core::vector3df(90,0,0));

  bool do_shadows = true; // shadow map is experimental
  irr::scene::ILightSceneNode* mlight = 0;

  if (do_shadows)
  {
    mlight = application.AddLightWithShadow(
      irr::core::vector3df(10.f, 30.f, 60.f),
      irr::core::vector3df(0.f, 0.f, 0.f),
      150, 60, 80, 15, 512, irr::video::SColorf(1, 1, 1), false, false);
  }
  else
  {
    application.AddTypicalLights(
      irr::core::vector3df(30.f, -30.f, 100.f),
      irr::core::vector3df(30.f, 50.f, 100.f),
      250, 130);
  }

  application.SetTimestep(step_size);

  ChIrrGuiDriver driver(application, vehicle, powertrain, trackPoint, 6.0, 0.5, true);

  // Set the time response for steering and throttle keyboard inputs.
  // NOTE: this is not exact, since we do not render quite at the specified FPS.
  double steering_time = 1.0;  // time to go from 0 to +1 (or from 0 to -1)
  double throttle_time = 1.0;  // time to go from 0 to +1
  double braking_time = 0.3;   // time to go from 0 to +1
  driver.SetSteeringDelta(render_step_size / steering_time);
  driver.SetThrottleDelta(render_step_size / throttle_time);
  driver.SetBrakingDelta(render_step_size / braking_time);

  // Set up the assets for rendering
  application.AssetBindAll();
  application.AssetUpdateAll();
  if (do_shadows)
  {
    application.AddShadowAll();
  }
#else
  ChDataDriver driver(driver_file);
#endif

  // ---------------
  // Simulation loop
  // ---------------

  // Inter-module communication data
  ChTireForces   tire_forces(num_wheels);
  ChWheelStates  wheel_states(num_wheels);
  double         driveshaft_speed;
  double         powertrain_torque;
  double         throttle_input;
  double         steering_input;
  double         braking_input;

  // Number of simulation steps between two 3D view render frames
  int render_steps = (int)std::ceil(render_step_size / step_size);

  // Number of simulation steps between two output frames
  int output_steps = (int)std::ceil(output_step_size / step_size);

  // Initialize simulation frame counter and simulation time
  int step_number = 0;
  double time = 0;

#ifdef USE_IRRLICHT

  ChRealtimeStepTimer realtime_timer;

  while (application.GetDevice()->run())
  {
    // update the position of the shadow mapping so that it follows the car
    if (do_shadows)
    {
      ChVector<> lightaim = vehicle.GetChassisPos();
      ChVector<> lightpos = vehicle.GetChassisPos() + ChVector<>(10, 30, 60);
      irr::core::vector3df mlightpos((irr::f32)lightpos.x, (irr::f32)lightpos.y, (irr::f32)lightpos.z);
      irr::core::vector3df mlightaim((irr::f32)lightaim.x, (irr::f32)lightaim.y, (irr::f32)lightaim.z);
      application.GetEffects()->getShadowLight(0).setPosition(mlightpos);
      application.GetEffects()->getShadowLight(0).setTarget(mlightaim);
      mlight->setPosition(mlightpos);
    }

    // Render scene
    if (step_number % render_steps == 0) {
      application.GetVideoDriver()->beginScene(true, true, irr::video::SColor(255, 140, 161, 192));
      driver.DrawAll();
      application.GetVideoDriver()->endScene();
    }

#ifdef DEBUG_LOG
    if (step_number % output_steps == 0) {
      GetLog() << "\n\n============ System Information ============\n";
      GetLog() << "Time = " << time << "\n\n";
      vehicle.DebugLog(DBG_SPRINGS);
    }
#endif

    // Collect output data from modules (for inter-module communication)
    throttle_input = driver.GetThrottle();
    steering_input = driver.GetSteering();
    braking_input = driver.GetBraking();
    powertrain_torque = powertrain.GetOutputTorque();
    driveshaft_speed = vehicle.GetDriveshaftSpeed();
    for (int i = 0; i < num_wheels; i++) {
      tire_forces[i] = tires[i]->GetTireForce();
      wheel_states[i] = vehicle.GetWheelState(i);
    }

    // Update modules (process inputs from other modules)
    time = vehicle.GetChTime();
    driver.Update(time);
    powertrain.Update(time, throttle_input, driveshaft_speed);
    vehicle.Update(time, steering_input, braking_input, powertrain_torque, tire_forces);
    terrain.Update(time);
    for (int i = 0; i < num_wheels; i++)
      tires[i]->Update(time, wheel_states[i]);

    // Advance simulation for one timestep for all modules
    double step = realtime_timer.SuggestSimulationStep(step_size);
    driver.Advance(step);
    powertrain.Advance(step);
    vehicle.Advance(step);
    terrain.Advance(step);
    for (int i = 0; i < num_wheels; i++)
      tires[i]->Advance(step);

    // Increment frame number
    step_number++;
  }

  application.GetDevice()->drop();

#else

  int render_frame = 0;

  if(ChFileutils::MakeDirectory(out_dir.c_str()) < 0) {
    std::cout << "Error creating directory " << out_dir << std::endl;
    return 1;
  }
  if(ChFileutils::MakeDirectory(pov_dir.c_str()) < 0) {
    std::cout << "Error creating directory " << pov_dir << std::endl;
    return 1;
  }

  vehicle.ExportMeshPovray(out_dir);

  char filename[100];

  while (time < tend)
  {
    if (step_number % render_steps == 0) {
      // Output render data
      sprintf(filename, "%s/data_%03d.dat", pov_dir.c_str(), render_frame + 1);
      utils::WriteShapesPovray(&vehicle, filename);
      std::cout << "Output frame:   " << render_frame << std::endl;
      std::cout << "Sim frame:      " << step_number << std::endl;
      std::cout << "Time:           " << time << std::endl;
      std::cout << "   throttle: " << driver.GetThrottle()
                << "   steering: " << driver.GetSteering()
                << "   braking:  " << driver.GetBraking() << std::endl;
      std::cout << std::endl;
      render_frame++;
    }

    // Collect output data from modules (for inter-module communication)
    throttle_input = driver.GetThrottle();
    steering_input = driver.GetSteering();
    braking_input = driver.GetBraking();
    powertrain_torque = powertrain.GetOutputTorque();
    driveshaft_speed = vehicle.GetDriveshaftSpeed();
    for (int i = 0; i < num_wheels; i++) {
      tire_forces[i] = tires[i]->GetTireForce();
      wheel_states[i] = vehicle.GetWheelState(i);
    }

    // Update modules (process inputs from other modules)
    time = vehicle.GetChTime();
    driver.Update(time);
    powertrain.Update(time, throttle_input, driveshaft_speed);
    vehicle.Update(time, steering_input, braking_input, powertrain_torque, tire_forces);
    terrain.Update(time);
    for (int i = 0; i < num_wheels; i++)
      tires[i]->Update(time, wheel_states[i]);

    // Update modules (process inputs from other modules)
    time = vehicle.GetChTime();
    driver.Update(time);
    powertrain.Update(time, throttle_input, driveshaft_speed);
    vehicle.Update(time, steering_input, braking_input, powertrain_torque, tire_forces);
    terrain.Update(time);
    for (int i = 0; i < num_wheels; i++)
      tires[i]->Update(time, wheel_states[i]);

    // Advance simulation for one timestep for all modules
    driver.Advance(step_size);
    powertrain.Advance(step_size);
    vehicle.Advance(step_size);
    terrain.Advance(step_size);
    for (int i = 0; i < num_wheels; i++)
      tires[i]->Advance(step_size);

    // Increment frame number
    step_number++;
  }

#endif

  return 0;
}
<|endoftext|>


<commit_before>#ifndef ALEPH_PERSISTENT_HOMOLOGY_PHI_PERSISTENCE_HH__
#define ALEPH_PERSISTENT_HOMOLOGY_PHI_PERSISTENCE_HH__

#include <aleph/persistenceDiagrams/PersistenceDiagram.hh>

#include <aleph/topology/Conversions.hh>
#include <aleph/topology/Intersections.hh>
#include <aleph/topology/SimplicialComplex.hh>

#include <aleph/persistentHomology/algorithms/Twist.hh>

#include <initializer_list>
#include <map>
#include <stdexcept>
#include <utility>
#include <vector>

namespace aleph
{

/**
  Partitions a simplicial complex according to its $\phi$-persistence
  values. This follows the persistent intersection homology algorithm
  in:

    Persistent Intersection Homology
    Paul Bendich and John Harer

  The function expects a simplicial complex $K$ and a function $\phi$
  that determines whether a simplex is proper or not. The function is
  going to create a new simplicial complex. This complex contains all
  proper simplices (in their original order) followed by all improper
  ones.
*/

template <class Simplex, class Function> std::pair<topology::SimplicialComplex<Simplex>, std::size_t> partition( const topology::SimplicialComplex<Simplex>& K, Function phi )
{
  topology::SimplicialComplex<Simplex> L;

  for( auto&& simplex : K )
  {
    if( phi(simplex) )
      L.push_back( simplex );
  }

  auto s = L.size();

  for( auto&& simplex : K )
  {
    if( !phi(simplex) )
      L.push_back( simplex );
  }

  return std::make_pair( L, s );
}

/**
  Models a perversity in the sense of intersection homology. The class
  ensures that all values satisfy

  \f
    -1 \leq p_k \leq k-1
  \f
*/

class Perversity
{
public:
  Perversity( std::initializer_list<int> values )
  {
    _values.assign( values.begin(), values.end() );

    for( std::size_t k = 0; k < _values.size(); k++ )
    {
      if( _values[k] < -1 )
        _values[k] = -1;
      // There is an index shift going on here: since $k$ runs from $0$
      // to $d-1$, there is no need to shift the upper bound.
      else if( _values[k] > static_cast<int>( k ) )
        _values[k] = static_cast<int>( k );
    }
  }

  /**
    Queries the perversity value in a given dimension $d$. Invalid
    dimension values only cause the function to return a zero.
  */

  int operator()( std::size_t d ) const noexcept
  {
    if( d < _values.size() )
      return _values[d];
    else
      return 0;
  }

private:
  std::vector<int> _values;
};

template <class Simplex> auto calculateIntersectionHomology( const aleph::topology::SimplicialComplex<Simplex>& K,
                                                             const std::vector< aleph::topology::SimplicialComplex<Simplex> >& X,
                                                             const Perversity& p ) -> std::vector< PersistenceDiagram<typename Simplex::DataType> >
{
  // 0. Check consistency of strata
  // 1. Create allowability function based on the dimensionality of the
  //    intersection of simplices with individual strata.
  // 2. Calculate $phi$-persistence
  // 3. Convert the result into a persistence diagram.

  // Check consistency of strata ---------------------------------------
  //
  // The maximum dimension of the strata has to match the dimension of
  // the simplicial complex.

  {
    std::size_t minDimension = K.dimension();
    std::size_t maxDimension = 0;

    for( auto&& stratum : X )
    {
      minDimension = std::min( minDimension, stratum.dimension() );
      maxDimension = std::max( maxDimension, stratum.dimension() );
    }

    if( maxDimension != K.dimension() )
      throw std::runtime_error( "Invalid stratification" );
  }

  // Check whether simplex is allowable --------------------------------

  std::map<Simplex, bool> phi;

  {
    auto d = K.dimension();

    for( auto&& s : K )
    {
      bool admissible = true;

      for( std::size_t k = 0; k < d; k++ )
      {
        // The notation follows Bendich and Harer, so $i$ is actually
        // referring to a dimension instead of an index. Beware!
        auto i            = s.dimension();
        auto intersection = aleph::topology::intersect( X.at( d - 1 - k ), s );
        auto dimension    = intersection.empty() ? -1 : static_cast<long>( intersection.rbegin()->dimension() );
        admissible        = admissible && static_cast<long>( dimension ) <= static_cast<long>( i - k + p(k) );
      }

      phi[s] = admissible;
    }
  }

  // Partition according to allowable simplices ------------------------

  aleph::topology::SimplicialComplex<Simplex> L;
  std::size_t s = 0;

  std::tie( L, s ) =
    aleph::partition( K, [&phi] ( const Simplex& s )
                         {
                           return phi.at(s);
                         } );

  // Calculate persistent intersection homology ------------------------

  auto boundaryMatrix = aleph::topology::makeBoundaryMatrix( L, s );
  using IndexType     = typename decltype(boundaryMatrix)::Index;

  aleph::persistentHomology::algorithms::Twist algorithm;
  algorithm( boundaryMatrix );

  using DataType = typename Simplex::DataType;

  // FIXME: support more persistence diagrams...
  aleph::PersistenceDiagram<DataType> diagram;

  for( IndexType i = 0; i < boundaryMatrix.getNumColumns(); i++ )
  {
    auto lowestOne = boundaryMatrix.getMaximumIndex( i );
    if( lowestOne.second && lowestOne.first <= s )
    {
      auto&& simplex = L[i];
      auto&& partner = L[lowestOne.first];

      diagram.add( simplex.data(), partner.data() );
    }
    else if( !lowestOne.second )
    {
      auto&& simplex = L[i];
      diagram.add( simplex.data() );
    }
  }

  return { diagram };
}

} // namespace aleph

#endif
<commit_msg>Fixed implicit cast<commit_after>#ifndef ALEPH_PERSISTENT_HOMOLOGY_PHI_PERSISTENCE_HH__
#define ALEPH_PERSISTENT_HOMOLOGY_PHI_PERSISTENCE_HH__

#include <aleph/persistenceDiagrams/PersistenceDiagram.hh>

#include <aleph/topology/Conversions.hh>
#include <aleph/topology/Intersections.hh>
#include <aleph/topology/SimplicialComplex.hh>

#include <aleph/persistentHomology/algorithms/Twist.hh>

#include <initializer_list>
#include <map>
#include <stdexcept>
#include <utility>
#include <vector>

namespace aleph
{

/**
  Partitions a simplicial complex according to its $\phi$-persistence
  values. This follows the persistent intersection homology algorithm
  in:

    Persistent Intersection Homology
    Paul Bendich and John Harer

  The function expects a simplicial complex $K$ and a function $\phi$
  that determines whether a simplex is proper or not. The function is
  going to create a new simplicial complex. This complex contains all
  proper simplices (in their original order) followed by all improper
  ones.
*/

template <class Simplex, class Function> std::pair<topology::SimplicialComplex<Simplex>, std::size_t> partition( const topology::SimplicialComplex<Simplex>& K, Function phi )
{
  topology::SimplicialComplex<Simplex> L;

  for( auto&& simplex : K )
  {
    if( phi(simplex) )
      L.push_back( simplex );
  }

  auto s = L.size();

  for( auto&& simplex : K )
  {
    if( !phi(simplex) )
      L.push_back( simplex );
  }

  return std::make_pair( L, s );
}

/**
  Models a perversity in the sense of intersection homology. The class
  ensures that all values satisfy

  \f
    -1 \leq p_k \leq k-1
  \f
*/

class Perversity
{
public:
  Perversity( std::initializer_list<int> values )
  {
    _values.assign( values.begin(), values.end() );

    for( std::size_t k = 0; k < _values.size(); k++ )
    {
      if( _values[k] < -1 )
        _values[k] = -1;
      // There is an index shift going on here: since $k$ runs from $0$
      // to $d-1$, there is no need to shift the upper bound.
      else if( _values[k] > static_cast<int>( k ) )
        _values[k] = static_cast<int>( k );
    }
  }

  /**
    Queries the perversity value in a given dimension $d$. Invalid
    dimension values only cause the function to return a zero.
  */

  int operator()( std::size_t d ) const noexcept
  {
    if( d < _values.size() )
      return _values[d];
    else
      return 0;
  }

private:
  std::vector<int> _values;
};

template <class Simplex> auto calculateIntersectionHomology( const aleph::topology::SimplicialComplex<Simplex>& K,
                                                             const std::vector< aleph::topology::SimplicialComplex<Simplex> >& X,
                                                             const Perversity& p ) -> std::vector< PersistenceDiagram<typename Simplex::DataType> >
{
  // 0. Check consistency of strata
  // 1. Create allowability function based on the dimensionality of the
  //    intersection of simplices with individual strata.
  // 2. Calculate $phi$-persistence
  // 3. Convert the result into a persistence diagram.

  // Check consistency of strata ---------------------------------------
  //
  // The maximum dimension of the strata has to match the dimension of
  // the simplicial complex.

  {
    std::size_t minDimension = K.dimension();
    std::size_t maxDimension = 0;

    for( auto&& stratum : X )
    {
      minDimension = std::min( minDimension, stratum.dimension() );
      maxDimension = std::max( maxDimension, stratum.dimension() );
    }

    if( maxDimension != K.dimension() )
      throw std::runtime_error( "Invalid stratification" );
  }

  // Check whether simplex is allowable --------------------------------

  std::map<Simplex, bool> phi;

  {
    auto d = K.dimension();

    for( auto&& s : K )
    {
      bool admissible = true;

      for( std::size_t k = 0; k < d; k++ )
      {
        // The notation follows Bendich and Harer, so $i$ is actually
        // referring to a dimension instead of an index. Beware!
        auto i            = s.dimension();
        auto intersection = aleph::topology::intersect( X.at( d - 1 - k ), s );
        auto dimension    = intersection.empty() ? -1 : static_cast<long>( intersection.rbegin()->dimension() );
        admissible        = admissible && static_cast<long>( dimension ) <= static_cast<long>( i - k + static_cast<long>( p(k) ) );
      }

      phi[s] = admissible;
    }
  }

  // Partition according to allowable simplices ------------------------

  aleph::topology::SimplicialComplex<Simplex> L;
  std::size_t s = 0;

  std::tie( L, s ) =
    aleph::partition( K, [&phi] ( const Simplex& s )
                         {
                           return phi.at(s);
                         } );

  // Calculate persistent intersection homology ------------------------

  auto boundaryMatrix = aleph::topology::makeBoundaryMatrix( L, s );
  using IndexType     = typename decltype(boundaryMatrix)::Index;

  aleph::persistentHomology::algorithms::Twist algorithm;
  algorithm( boundaryMatrix );

  using DataType = typename Simplex::DataType;

  // FIXME: support more persistence diagrams...
  aleph::PersistenceDiagram<DataType> diagram;

  for( IndexType i = 0; i < boundaryMatrix.getNumColumns(); i++ )
  {
    auto lowestOne = boundaryMatrix.getMaximumIndex( i );
    if( lowestOne.second && lowestOne.first <= s )
    {
      auto&& simplex = L[i];
      auto&& partner = L[lowestOne.first];

      diagram.add( simplex.data(), partner.data() );
    }
    else if( !lowestOne.second )
    {
      auto&& simplex = L[i];
      diagram.add( simplex.data() );
    }
  }

  return { diagram };
}

} // namespace aleph

#endif
<|endoftext|>


<commit_before>/*M///////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                           License Agreement
//                For Open Source Computer Vision Library
//
// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's 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.
//
//   * The name of the copyright holders may not 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 Intel Corporation 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.
//
//M*/

#include "test_precomp.hpp"

#ifdef HAVE_TBB
#include "tbb/task_scheduler_init.h"
#endif

using namespace cv;
using namespace std;

class CV_solvePnPRansac_Test : public cvtest::BaseTest
{
public:
    CV_solvePnPRansac_Test()
    {
        eps[ITERATIVE] = 1.0e-2;
        eps[EPNP] = 1.0e-2;
        eps[P3P] = 1.0e-2;
        eps[DLS] = 1.0e-2;
        totalTestsCount = 10;
    }
    ~CV_solvePnPRansac_Test() {}
protected:
    void generate3DPointCloud(vector<Point3f>& points, Point3f pmin = Point3f(-1,
        -1, 5), Point3f pmax = Point3f(1, 1, 10))
    {
        const Point3f delta = pmax - pmin;
        for (size_t i = 0; i < points.size(); i++)
        {
            Point3f p(float(rand()) / RAND_MAX, float(rand()) / RAND_MAX,
                float(rand()) / RAND_MAX);
            p.x *= delta.x;
            p.y *= delta.y;
            p.z *= delta.z;
            p = p + pmin;
            points[i] = p;
        }
    }

    void generateCameraMatrix(Mat& cameraMatrix, RNG& rng)
    {
        const double fcMinVal = 1e-3;
        const double fcMaxVal = 100;
        cameraMatrix.create(3, 3, CV_64FC1);
        cameraMatrix.setTo(Scalar(0));
        cameraMatrix.at<double>(0,0) = rng.uniform(fcMinVal, fcMaxVal);
        cameraMatrix.at<double>(1,1) = rng.uniform(fcMinVal, fcMaxVal);
        cameraMatrix.at<double>(0,2) = rng.uniform(fcMinVal, fcMaxVal);
        cameraMatrix.at<double>(1,2) = rng.uniform(fcMinVal, fcMaxVal);
        cameraMatrix.at<double>(2,2) = 1;
    }

    void generateDistCoeffs(Mat& distCoeffs, RNG& rng)
    {
        distCoeffs = Mat::zeros(4, 1, CV_64FC1);
        for (int i = 0; i < 3; i++)
            distCoeffs.at<double>(i,0) = rng.uniform(0.0, 1.0e-6);
    }

    void generatePose(Mat& rvec, Mat& tvec, RNG& rng)
    {
        const double minVal = 1.0e-3;
        const double maxVal = 1.0;
        rvec.create(3, 1, CV_64FC1);
        tvec.create(3, 1, CV_64FC1);
        for (int i = 0; i < 3; i++)
        {
            rvec.at<double>(i,0) = rng.uniform(minVal, maxVal);
            tvec.at<double>(i,0) = rng.uniform(minVal, maxVal/10);
        }
    }

    virtual bool runTest(RNG& rng, int mode, int method, const vector<Point3f>& points, const double* epsilon, double& maxError)
    {
        Mat rvec, tvec;
        vector<int> inliers;
        Mat trueRvec, trueTvec;
        Mat intrinsics, distCoeffs;
        generateCameraMatrix(intrinsics, rng);
        if (mode == 0)
            distCoeffs = Mat::zeros(4, 1, CV_64FC1);
        else
            generateDistCoeffs(distCoeffs, rng);
        generatePose(trueRvec, trueTvec, rng);

        vector<Point2f> projectedPoints;
        projectedPoints.resize(points.size());
        projectPoints(Mat(points), trueRvec, trueTvec, intrinsics, distCoeffs, projectedPoints);
        for (size_t i = 0; i < projectedPoints.size(); i++)
        {
            if (i % 20 == 0)
            {
                projectedPoints[i] = projectedPoints[rng.uniform(0,(int)points.size()-1)];
            }
        }

        solvePnPRansac(points, projectedPoints, intrinsics, distCoeffs, rvec, tvec,
            false, 500, 0.5, 0.9, inliers, method);

        bool isTestSuccess = inliers.size() >= points.size()*0.95;

        double rvecDiff = norm(rvec-trueRvec), tvecDiff = norm(tvec-trueTvec);
        isTestSuccess = isTestSuccess && rvecDiff < epsilon[method] && tvecDiff < epsilon[method];
        double error = rvecDiff > tvecDiff ? rvecDiff : tvecDiff;
        //cout << error << " " << inliers.size() << " " << eps[method] << endl;
        if (error > maxError)
            maxError = error;

        return isTestSuccess;
    }

    void run(int)
    {
        ts->set_failed_test_info(cvtest::TS::OK);

        vector<Point3f> points, points_dls;
        const int pointsCount = 500;
        points.resize(pointsCount);
        generate3DPointCloud(points);

        const int methodsCount = 4;
        RNG rng = ts->get_rng();

        for (int mode = 0; mode < 2; mode++)
        {
            for (int method = 0; method < methodsCount; method++)
            {
                double maxError = 0;
                int successfulTestsCount = 0;
                for (int testIndex = 0; testIndex < totalTestsCount; testIndex++)
                {
                    if (runTest(rng, mode, method, points, eps, maxError))
                        successfulTestsCount++;
                }
                //cout <<  maxError << " " << successfulTestsCount << endl;
                if (successfulTestsCount < 0.7*totalTestsCount)
                {
                    ts->printf( cvtest::TS::LOG, "Invalid accuracy for method %d, failed %d tests from %d, maximum error equals %f, distortion mode equals %d\n",
                        method, totalTestsCount - successfulTestsCount, totalTestsCount, maxError, mode);
                    ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
                }
            }
        }
    }
    double eps[4];
    int totalTestsCount;
};

class CV_solvePnP_Test : public CV_solvePnPRansac_Test
{
public:
    CV_solvePnP_Test()
    {
        eps[ITERATIVE] = 1.0e-6;
        eps[EPNP] = 1.0e-6;
        eps[P3P] = 1.0e-4;
        eps[DLS] = 1.0e-6;
        totalTestsCount = 1000;
    }

    ~CV_solvePnP_Test() {}
protected:
    virtual bool runTest(RNG& rng, int mode, int method, const vector<Point3f>& points, const double* epsilon, double& maxError)
    {
        Mat rvec, tvec;
        Mat trueRvec, trueTvec;
        Mat intrinsics, distCoeffs;
        generateCameraMatrix(intrinsics, rng);
        if (mode == 0)
            distCoeffs = Mat::zeros(4, 1, CV_64FC1);
        else
            generateDistCoeffs(distCoeffs, rng);
        generatePose(trueRvec, trueTvec, rng);

        std::vector<Point3f> opoints;
        if (method == 2)
        {
            opoints = std::vector<Point3f>(points.begin(), points.begin()+4);
        }
        else if(method == 3)
        {
            opoints = std::vector<Point3f>(points.begin(), points.begin()+10);
        }
        else
            opoints = points;

        vector<Point2f> projectedPoints;
        projectedPoints.resize(opoints.size());
        projectPoints(Mat(opoints), trueRvec, trueTvec, intrinsics, distCoeffs, projectedPoints);

        solvePnP(opoints, projectedPoints, intrinsics, distCoeffs, rvec, tvec,
            false, method);

        double rvecDiff = norm(rvec-trueRvec), tvecDiff = norm(tvec-trueTvec);
        bool isTestSuccess = rvecDiff < epsilon[method] && tvecDiff < epsilon[method];

        double error = rvecDiff > tvecDiff ? rvecDiff : tvecDiff;
        if (error > maxError)
            maxError = error;

        return isTestSuccess;
    }
};

TEST(Calib3d_SolvePnPRansac, accuracy) { CV_solvePnPRansac_Test test; test.safe_run(); }
TEST(Calib3d_SolvePnP, accuracy) { CV_solvePnP_Test test; test.safe_run(); }

#ifdef HAVE_TBB

TEST(DISABLED_Calib3d_SolvePnPRansac, concurrency)
{
    int count = 7*13;

    Mat object(1, count, CV_32FC3);
    randu(object, -100, 100);

    Mat camera_mat(3, 3, CV_32FC1);
    randu(camera_mat, 0.5, 1);
    camera_mat.at<float>(0, 1) = 0.f;
    camera_mat.at<float>(1, 0) = 0.f;
    camera_mat.at<float>(2, 0) = 0.f;
    camera_mat.at<float>(2, 1) = 0.f;

    Mat dist_coef(1, 8, CV_32F, cv::Scalar::all(0));

    vector<cv::Point2f> image_vec;
    Mat rvec_gold(1, 3, CV_32FC1);
    randu(rvec_gold, 0, 1);
    Mat tvec_gold(1, 3, CV_32FC1);
    randu(tvec_gold, 0, 1);
    projectPoints(object, rvec_gold, tvec_gold, camera_mat, dist_coef, image_vec);

    Mat image(1, count, CV_32FC2, &image_vec[0]);

    Mat rvec1, rvec2;
    Mat tvec1, tvec2;

    {
        // limit concurrency to get deterministic result
        cv::theRNG().state = 20121010;
        tbb::task_scheduler_init one_thread(1);
        solvePnPRansac(object, image, camera_mat, dist_coef, rvec1, tvec1);
    }

    if(1)
    {
        Mat rvec;
        Mat tvec;
        // parallel executions
        for(int i = 0; i < 10; ++i)
        {
            cv::theRNG().state = 20121010;
            solvePnPRansac(object, image, camera_mat, dist_coef, rvec, tvec);
        }
    }

    {
        // single thread again
        cv::theRNG().state = 20121010;
        tbb::task_scheduler_init one_thread(1);
        solvePnPRansac(object, image, camera_mat, dist_coef, rvec2, tvec2);
    }

    double rnorm = cvtest::norm(rvec1, rvec2, NORM_INF);
    double tnorm = cvtest::norm(tvec1, tvec2, NORM_INF);

    EXPECT_LT(rnorm, 1e-6);
    EXPECT_LT(tnorm, 1e-6);

}
#endif
<commit_msg>DLS test<commit_after>/*M///////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                           License Agreement
//                For Open Source Computer Vision Library
//
// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's 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.
//
//   * The name of the copyright holders may not 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 Intel Corporation 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.
//
//M*/

#include "test_precomp.hpp"

#ifdef HAVE_TBB
#include "tbb/task_scheduler_init.h"
#endif

using namespace cv;
using namespace std;

class CV_solvePnPRansac_Test : public cvtest::BaseTest
{
public:
    CV_solvePnPRansac_Test()
    {
        eps[ITERATIVE] = 1.0e-2;
        eps[EPNP] = 1.0e-2;
        eps[P3P] = 1.0e-2;
        eps[DLS] = 1.0e-2;
        totalTestsCount = 10;
    }
    ~CV_solvePnPRansac_Test() {}
protected:
    void generate3DPointCloud(vector<Point3f>& points, Point3f pmin = Point3f(-1,
        -1, 5), Point3f pmax = Point3f(1, 1, 10))
    {
        const Point3f delta = pmax - pmin;
        for (size_t i = 0; i < points.size(); i++)
        {
            Point3f p(float(rand()) / RAND_MAX, float(rand()) / RAND_MAX,
                float(rand()) / RAND_MAX);
            p.x *= delta.x;
            p.y *= delta.y;
            p.z *= delta.z;
            p = p + pmin;
            points[i] = p;
        }
    }

    void generateCameraMatrix(Mat& cameraMatrix, RNG& rng)
    {
        const double fcMinVal = 1e-3;
        const double fcMaxVal = 100;
        cameraMatrix.create(3, 3, CV_64FC1);
        cameraMatrix.setTo(Scalar(0));
        cameraMatrix.at<double>(0,0) = rng.uniform(fcMinVal, fcMaxVal);
        cameraMatrix.at<double>(1,1) = rng.uniform(fcMinVal, fcMaxVal);
        cameraMatrix.at<double>(0,2) = rng.uniform(fcMinVal, fcMaxVal);
        cameraMatrix.at<double>(1,2) = rng.uniform(fcMinVal, fcMaxVal);
        cameraMatrix.at<double>(2,2) = 1;
    }

    void generateDistCoeffs(Mat& distCoeffs, RNG& rng)
    {
        distCoeffs = Mat::zeros(4, 1, CV_64FC1);
        for (int i = 0; i < 3; i++)
            distCoeffs.at<double>(i,0) = rng.uniform(0.0, 1.0e-6);
    }

    void generatePose(Mat& rvec, Mat& tvec, RNG& rng)
    {
        const double minVal = 1.0e-3;
        const double maxVal = 1.0;
        rvec.create(3, 1, CV_64FC1);
        tvec.create(3, 1, CV_64FC1);
        for (int i = 0; i < 3; i++)
        {
            rvec.at<double>(i,0) = rng.uniform(minVal, maxVal);
            tvec.at<double>(i,0) = rng.uniform(minVal, maxVal/10);
        }
    }

    virtual bool runTest(RNG& rng, int mode, int method, const vector<Point3f>& points, const double* epsilon, double& maxError)
    {
        Mat rvec, tvec;
        vector<int> inliers;
        Mat trueRvec, trueTvec;
        Mat intrinsics, distCoeffs;
        generateCameraMatrix(intrinsics, rng);
        if (mode == 0)
            distCoeffs = Mat::zeros(4, 1, CV_64FC1);
        else
            generateDistCoeffs(distCoeffs, rng);
        generatePose(trueRvec, trueTvec, rng);

        vector<Point2f> projectedPoints;
        projectedPoints.resize(points.size());
        projectPoints(Mat(points), trueRvec, trueTvec, intrinsics, distCoeffs, projectedPoints);
        for (size_t i = 0; i < projectedPoints.size(); i++)
        {
            if (i % 20 == 0)
            {
                projectedPoints[i] = projectedPoints[rng.uniform(0,(int)points.size()-1)];
            }
        }

        solvePnPRansac(points, projectedPoints, intrinsics, distCoeffs, rvec, tvec,
            false, 500, 0.5, 0.9, inliers, method);

        bool isTestSuccess = inliers.size() >= points.size()*0.95;

        double rvecDiff = norm(rvec-trueRvec), tvecDiff = norm(tvec-trueTvec);
        isTestSuccess = isTestSuccess && rvecDiff < epsilon[method] && tvecDiff < epsilon[method];
        double error = rvecDiff > tvecDiff ? rvecDiff : tvecDiff;
        //cout << error << " " << inliers.size() << " " << eps[method] << endl;
        if (error > maxError)
            maxError = error;

        return isTestSuccess;
    }

    void run(int)
    {
        ts->set_failed_test_info(cvtest::TS::OK);

        vector<Point3f> points, points_dls;
        const int pointsCount = 500;
        points.resize(pointsCount);
        generate3DPointCloud(points);

        const int methodsCount = 4;
        RNG rng = ts->get_rng();

        for (int mode = 0; mode < 2; mode++)
        {
            for (int method = 0; method < methodsCount; method++)
            {
                double maxError = 0;
                int successfulTestsCount = 0;
                for (int testIndex = 0; testIndex < totalTestsCount; testIndex++)
                {
                    if (runTest(rng, mode, method, points, eps, maxError))
                        successfulTestsCount++;
                }
                //cout <<  maxError << " " << successfulTestsCount << endl;
                if (successfulTestsCount < 0.7*totalTestsCount)
                {
                    ts->printf( cvtest::TS::LOG, "Invalid accuracy for method %d, failed %d tests from %d, maximum error equals %f, distortion mode equals %d\n",
                        method, totalTestsCount - successfulTestsCount, totalTestsCount, maxError, mode);
                    ts->set_failed_test_info(cvtest::TS::FAIL_BAD_ACCURACY);
                }
            }
        }
    }
    double eps[4];
    int totalTestsCount;
};

class CV_solvePnP_Test : public CV_solvePnPRansac_Test
{
public:
    CV_solvePnP_Test()
    {
        eps[ITERATIVE] = 1.0e-6;
        eps[EPNP] = 1.0e-6;
        eps[P3P] = 1.0e-4;
        eps[DLS] = 1.0e-6;
        totalTestsCount = 1000;
    }

    ~CV_solvePnP_Test() {}
protected:
    virtual bool runTest(RNG& rng, int mode, int method, const vector<Point3f>& points, const double* epsilon, double& maxError)
    {
        Mat rvec, tvec;
        Mat trueRvec, trueTvec;
        Mat intrinsics, distCoeffs;
        generateCameraMatrix(intrinsics, rng);
        if (mode == 0)
            distCoeffs = Mat::zeros(4, 1, CV_64FC1);
        else
            generateDistCoeffs(distCoeffs, rng);
        generatePose(trueRvec, trueTvec, rng);

        std::vector<Point3f> opoints;
        if (method == 2)
        {
            opoints = std::vector<Point3f>(points.begin(), points.begin()+4);
        }
        else if(method == 3)
        {
            opoints = std::vector<Point3f>(points.begin(), points.begin()+50);
        }
        else
            opoints = points;

        vector<Point2f> projectedPoints;
        projectedPoints.resize(opoints.size());
        projectPoints(Mat(opoints), trueRvec, trueTvec, intrinsics, distCoeffs, projectedPoints);

        solvePnP(opoints, projectedPoints, intrinsics, distCoeffs, rvec, tvec,
            false, method);

        double rvecDiff = norm(rvec-trueRvec), tvecDiff = norm(tvec-trueTvec);
        bool isTestSuccess = rvecDiff < epsilon[method] && tvecDiff < epsilon[method];

        double error = rvecDiff > tvecDiff ? rvecDiff : tvecDiff;
        if (error > maxError)
            maxError = error;

        return isTestSuccess;
    }
};

TEST(Calib3d_SolvePnPRansac, accuracy) { CV_solvePnPRansac_Test test; test.safe_run(); }
TEST(Calib3d_SolvePnP, accuracy) { CV_solvePnP_Test test; test.safe_run(); }

#ifdef HAVE_TBB

TEST(DISABLED_Calib3d_SolvePnPRansac, concurrency)
{
    int count = 7*13;

    Mat object(1, count, CV_32FC3);
    randu(object, -100, 100);

    Mat camera_mat(3, 3, CV_32FC1);
    randu(camera_mat, 0.5, 1);
    camera_mat.at<float>(0, 1) = 0.f;
    camera_mat.at<float>(1, 0) = 0.f;
    camera_mat.at<float>(2, 0) = 0.f;
    camera_mat.at<float>(2, 1) = 0.f;

    Mat dist_coef(1, 8, CV_32F, cv::Scalar::all(0));

    vector<cv::Point2f> image_vec;
    Mat rvec_gold(1, 3, CV_32FC1);
    randu(rvec_gold, 0, 1);
    Mat tvec_gold(1, 3, CV_32FC1);
    randu(tvec_gold, 0, 1);
    projectPoints(object, rvec_gold, tvec_gold, camera_mat, dist_coef, image_vec);

    Mat image(1, count, CV_32FC2, &image_vec[0]);

    Mat rvec1, rvec2;
    Mat tvec1, tvec2;

    {
        // limit concurrency to get deterministic result
        cv::theRNG().state = 20121010;
        tbb::task_scheduler_init one_thread(1);
        solvePnPRansac(object, image, camera_mat, dist_coef, rvec1, tvec1);
    }

    if(1)
    {
        Mat rvec;
        Mat tvec;
        // parallel executions
        for(int i = 0; i < 10; ++i)
        {
            cv::theRNG().state = 20121010;
            solvePnPRansac(object, image, camera_mat, dist_coef, rvec, tvec);
        }
    }

    {
        // single thread again
        cv::theRNG().state = 20121010;
        tbb::task_scheduler_init one_thread(1);
        solvePnPRansac(object, image, camera_mat, dist_coef, rvec2, tvec2);
    }

    double rnorm = cvtest::norm(rvec1, rvec2, NORM_INF);
    double tnorm = cvtest::norm(tvec1, tvec2, NORM_INF);

    EXPECT_LT(rnorm, 1e-6);
    EXPECT_LT(tnorm, 1e-6);

}
#endif
<|endoftext|>


<commit_before>#ifndef ALEPH_PERSISTENT_HOMOLOGY_PHI_PERSISTENCE_HH__
#define ALEPH_PERSISTENT_HOMOLOGY_PHI_PERSISTENCE_HH__

#include <aleph/topology/Conversions.hh>
#include <aleph/topology/SimplicialComplex.hh>

#include <utility>

namespace aleph
{

/**
  Partitions a simplicial complex according to its $\phi$-persistence
  values. This follows the persistent intersection homology algorithm
  in:

    Persistent Intersection Homology
    Paul Bendich and John Harer

  The function expects a simplicial complex $K$ and a function $\phi$
  that determines whether a simplex is proper or not. The function is
  going to create a new simplicial complex. This complex contains all
  proper simplices (in their original order) followed by all improper
  ones.
*/

template <class Simplex, class Function> std::pair<topology::SimplicialComplex<Simplex>, std::size_t> partition( const topology::SimplicialComplex<Simplex>& K, Function phi )
{
  topology::SimplicialComplex<Simplex> L;

  for( auto&& simplex : K )
  {
    if( phi(simplex) )
      L.push_back( simplex );
  }

  if( L.empty() )
    return {};

  for( auto&& simplex : K )
  {
    if( !phi(simplex) )
      L.push_back( simplex );
  }

  return std::make_pair( L, L.size() - 1 );
}

} // namespace aleph

#endif
<commit_msg>Fixed incorrect indices<commit_after>#ifndef ALEPH_PERSISTENT_HOMOLOGY_PHI_PERSISTENCE_HH__
#define ALEPH_PERSISTENT_HOMOLOGY_PHI_PERSISTENCE_HH__

#include <aleph/topology/Conversions.hh>
#include <aleph/topology/SimplicialComplex.hh>

#include <utility>

namespace aleph
{

/**
  Partitions a simplicial complex according to its $\phi$-persistence
  values. This follows the persistent intersection homology algorithm
  in:

    Persistent Intersection Homology
    Paul Bendich and John Harer

  The function expects a simplicial complex $K$ and a function $\phi$
  that determines whether a simplex is proper or not. The function is
  going to create a new simplicial complex. This complex contains all
  proper simplices (in their original order) followed by all improper
  ones.
*/

template <class Simplex, class Function> std::pair<topology::SimplicialComplex<Simplex>, std::size_t> partition( const topology::SimplicialComplex<Simplex>& K, Function phi )
{
  topology::SimplicialComplex<Simplex> L;

  for( auto&& simplex : K )
  {
    if( phi(simplex) )
      L.push_back( simplex );
  }

  auto s = L.size();

  for( auto&& simplex : K )
  {
    if( !phi(simplex) )
      L.push_back( simplex );
  }

  return std::make_pair( L, s );
}

} // namespace aleph

#endif
<|endoftext|>


<commit_before>/******************************************************************************
* Copyright (c) 2011, Michael P. Gerlek (mpg@flaxen.com)
*
* 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 Hobu, Inc. or Flaxen Geo Consulting 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.
****************************************************************************/

#include <pdal/PipelineWriter.hpp>

#include <pdal/Filter.hpp>
#include <pdal/MultiFilter.hpp>
#include <pdal/Reader.hpp>
#include <pdal/Writer.hpp>
#include <pdal/PipelineManager.hpp>

#include <boost/property_tree/xml_parser.hpp>
#include <boost/optional.hpp>
#include <boost/foreach.hpp>

namespace pdal
{

PipelineWriter::PipelineWriter(const PipelineManager& manager)
    : m_manager(manager)
{

    return;
}

PipelineWriter::~PipelineWriter()
{
    return;
}

static boost::property_tree::ptree generateTreeFromStageBase(const StageBase& stage)
{
    boost::property_tree::ptree subtree = stage.serializePipeline();

    boost::property_tree::ptree tree;

    boost::property_tree::ptree& attrtree = tree.add_child("Pipeline", subtree);

    attrtree.put("<xmlattr>.version", "1.0");

    return tree;
}

void PipelineWriter::write_option_ptree(boost::property_tree::ptree& tree, const Options& opts)
{
    boost::property_tree::ptree m_tree = opts.toPTree();

    boost::property_tree::ptree::const_iterator iter = m_tree.begin();

    while (iter != m_tree.end())
    {
        if (iter->first != "Option")
            throw pdal_error("malformed Options ptree");
        const boost::property_tree::ptree& optionTree = iter->second;

        // we want to create this:
        //      ...
        //      <Option name="file">foo.las</Option>
        //      ...

        const std::string& name = optionTree.get_child("Name").get_value<std::string>();
        const std::string& value = optionTree.get_child("Value").get_value<std::string>();

        boost::property_tree::ptree& subtree = tree.add("Option", value);
        subtree.put("<xmlattr>.name", name);

        using namespace boost::property_tree;
        using namespace boost;

        optional<ptree const&> moreOptions = optionTree.get_child_optional("Options");

        if (moreOptions)
        {
            ptree newOpts;
            write_option_ptree(newOpts, moreOptions.get());
            subtree.put_child("Options", newOpts);
        }
        ++iter;
    }

    return;
}

void PipelineWriter::write_metadata_ptree(boost::property_tree::ptree& tree, const Metadata& mdata)
{
    using namespace boost;

    property_tree::ptree m_tree = mdata.toPTree();

    property_tree::ptree::const_iterator iter = m_tree.begin();

    property_tree::ptree output;

    while (iter != m_tree.end())
    {
        if (iter->first != "entry")
            throw pdal_error("malformed Metadata ptree");
        const property_tree::ptree& e_tree = iter->second;

        property_tree::ptree entry;

        const std::string& name = e_tree.get_child("name").get_value<std::string>();
        const std::string& value = e_tree.get_child("value").get_value<std::string>();
        const std::string& type = e_tree.get_child("type").get_value<std::string>();

        entry.put_value(value);
        entry.put("<xmlattr>.name", name);
        entry.put("<xmlattr>.type", type);

        std::pair<property_tree::ptree::const_assoc_iterator, property_tree::ptree::const_assoc_iterator> ret = e_tree.equal_range("attribute");

        for (property_tree::ptree::const_assoc_iterator  o = ret.first; o != ret.second; ++o)
        {
            property_tree::ptree const& tree = o->second;
            std::string const& name =  tree.get_child("name").get_value<std::string>();
            std::string const& value =  tree.get_child("value").get_value<std::string>();

            boost::property_tree::ptree a;
            a.put_value(value);
            a.put("<xmlattr>.name", name);
            entry.add_child("attribute", a);
        }

        output.add_child("Entry", entry);
        ++iter;
    }

    tree.add_child("Metadata", output);

    return;
}

void PipelineWriter::writePipeline(const std::string& filename) const
{
    const StageBase* stage = m_manager.isWriterPipeline() ? (StageBase*)m_manager.getWriter() : (StageBase*)m_manager.getStage();

    boost::property_tree::ptree tree = generateTreeFromStageBase(*stage);

    const boost::property_tree::xml_parser::xml_writer_settings<char> settings(' ', 4);
    boost::property_tree::xml_parser::write_xml(filename, tree, std::locale(), settings);

    return;
}

} // namespace pdal
<commit_msg>support writePipeline transparently writing to stdout via special filename<commit_after>/******************************************************************************
* Copyright (c) 2011, Michael P. Gerlek (mpg@flaxen.com)
*
* 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 Hobu, Inc. or Flaxen Geo Consulting 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.
****************************************************************************/

#include <pdal/PipelineWriter.hpp>

#include <pdal/Filter.hpp>
#include <pdal/MultiFilter.hpp>
#include <pdal/Reader.hpp>
#include <pdal/Writer.hpp>
#include <pdal/PipelineManager.hpp>

#include <boost/property_tree/xml_parser.hpp>
#include <boost/optional.hpp>
#include <boost/foreach.hpp>
#include <boost/algorithm/string.hpp>

namespace pdal
{

PipelineWriter::PipelineWriter(const PipelineManager& manager)
    : m_manager(manager)
{

    return;
}

PipelineWriter::~PipelineWriter()
{
    return;
}

static boost::property_tree::ptree generateTreeFromStageBase(const StageBase& stage)
{
    boost::property_tree::ptree subtree = stage.serializePipeline();

    boost::property_tree::ptree tree;

    boost::property_tree::ptree& attrtree = tree.add_child("Pipeline", subtree);

    attrtree.put("<xmlattr>.version", "1.0");

    return tree;
}

void PipelineWriter::write_option_ptree(boost::property_tree::ptree& tree, const Options& opts)
{
    boost::property_tree::ptree m_tree = opts.toPTree();

    boost::property_tree::ptree::const_iterator iter = m_tree.begin();

    while (iter != m_tree.end())
    {
        if (iter->first != "Option")
            throw pdal_error("malformed Options ptree");
        const boost::property_tree::ptree& optionTree = iter->second;

        // we want to create this:
        //      ...
        //      <Option name="file">foo.las</Option>
        //      ...

        const std::string& name = optionTree.get_child("Name").get_value<std::string>();
        const std::string& value = optionTree.get_child("Value").get_value<std::string>();

        boost::property_tree::ptree& subtree = tree.add("Option", value);
        subtree.put("<xmlattr>.name", name);

        using namespace boost::property_tree;
        using namespace boost;

        optional<ptree const&> moreOptions = optionTree.get_child_optional("Options");

        if (moreOptions)
        {
            ptree newOpts;
            write_option_ptree(newOpts, moreOptions.get());
            subtree.put_child("Options", newOpts);
        }
        ++iter;
    }

    return;
}

void PipelineWriter::write_metadata_ptree(boost::property_tree::ptree& tree, const Metadata& mdata)
{
    using namespace boost;

    property_tree::ptree m_tree = mdata.toPTree();

    property_tree::ptree::const_iterator iter = m_tree.begin();

    property_tree::ptree output;

    while (iter != m_tree.end())
    {
        if (iter->first != "entry")
            throw pdal_error("malformed Metadata ptree");
        const property_tree::ptree& e_tree = iter->second;

        property_tree::ptree entry;

        const std::string& name = e_tree.get_child("name").get_value<std::string>();
        const std::string& value = e_tree.get_child("value").get_value<std::string>();
        const std::string& type = e_tree.get_child("type").get_value<std::string>();

        entry.put_value(value);
        entry.put("<xmlattr>.name", name);
        entry.put("<xmlattr>.type", type);

        std::pair<property_tree::ptree::const_assoc_iterator, property_tree::ptree::const_assoc_iterator> ret = e_tree.equal_range("attribute");

        for (property_tree::ptree::const_assoc_iterator  o = ret.first; o != ret.second; ++o)
        {
            property_tree::ptree const& tree = o->second;
            std::string const& name =  tree.get_child("name").get_value<std::string>();
            std::string const& value =  tree.get_child("value").get_value<std::string>();

            boost::property_tree::ptree a;
            a.put_value(value);
            a.put("<xmlattr>.name", name);
            entry.add_child("attribute", a);
        }

        output.add_child("Entry", entry);
        ++iter;
    }

    tree.add_child("Metadata", output);

    return;
}

void PipelineWriter::writePipeline(const std::string& filename) const
{
    const StageBase* stage = m_manager.isWriterPipeline() ? (StageBase*)m_manager.getWriter() : (StageBase*)m_manager.getStage();

    boost::property_tree::ptree tree = generateTreeFromStageBase(*stage);

    const boost::property_tree::xml_parser::xml_writer_settings<char> settings(' ', 4);

    if (boost::iequals(filename, "STDOUT"))
        boost::property_tree::xml_parser::write_xml(std::cout, tree);
    else
        boost::property_tree::xml_parser::write_xml(filename, tree, std::locale(), settings);

    return;
}

} // namespace pdal
<|endoftext|>