<commit_before>#include "problem.h"

#include <complex>
#include <vector>
#include <fstream>

#include "base/base.h"
#include "boost/multiprecision/cpp_int.hpp"
#include "boost/rational.hpp"
#include "polygon.h"

DEFINE_bool(expand_viewbox, true,
            "Expand viewbox to covert the entire silhouette.");
DEFINE_bool(shrink_viewbox, true,
            "Shrink viewbox to fit silhouette and hide the original rect.");
DEFINE_string(input, "/dev/stdin", "input problem file");

using boost::rational;
using boost::rational_cast;
using namespace std;

int main(int argc, char** argv) {
  ParseCommandLineFlags(&argc, &argv);

  std::ifstream ifs(FLAGS_input);
  Problem problem;
  ReadProblem(ifs, &problem);

  // viewbox size
  Q min_x = 0, min_y = 0, max_x = 1, max_y = 1;
  if (FLAGS_shrink_viewbox) {
    min_x = max_x = problem.polygons[0][0].x;
    min_y = max_y = problem.polygons[0][0].y;
  }
  if (FLAGS_expand_viewbox) {
    for (int i = 0; i < problem.polygons.size(); ++i) {
      for (const auto& v : problem.polygons[i]) {
        if (v.x < min_x) min_x = v.x;
        if (max_x < v.x) max_x = v.x;
        if (v.y < min_y) min_y = v.y;
        if (max_y < v.y) max_y = v.y;
      }
    }
  }
  if (FLAGS_shrink_viewbox) {
    // Translate to reasonable coordinates
    for (auto& p : problem.polygons) {
      for (auto& v : p) {
        v.x -= min_x;
        v.y -= min_y;
      }
    }
    for (auto& e : problem.skelton) {
      e.first.x -= min_x;
      e.first.y -= min_y;
      e.second.x -= min_x;
      e.second.y -= min_y;
    }
    LOG(INFO) << "Translate " << min_x << "," << min_y;
    max_x -= min_x;
    max_y -= min_y;
    min_x = min_y = 0;
  }
  printf(
      R"(<svg xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" width="400px" height="400px" viewBox="%.3f %.3f %.3f %.3f" stroke-linejoin="round" stroke-linecap="round">)",
      min_x.convert_to<double>() - 0.0025, min_y.convert_to<double>() - 0.0025,
      Q(max_x - min_x).convert_to<double>() + 0.005,
      Q(max_y - min_y).convert_to<double>() + 0.005);
  if (!FLAGS_shrink_viewbox) {
    printf(
        R"q(<g transform="translate(0,1) scale(1,-1)"><rect x="0" y="0" width="1" height="1" fill="none" stroke="blue" stroke-width="0.005"/>)q");
  }
  printf(
      R"(<path fill="silver" stroke="gray" stroke-width="0.005" fill-rule="nonzero" d=")");
  for (int i = 0; i < problem.polygons.size(); ++i) {
    for (int j = 0; j < problem.polygons[i].size(); ++j) {
      printf("%c%.3f %.3f", j == 0 ? 'M' : 'L',
             problem.polygons[i][j].x.convert_to<double>(),
             problem.polygons[i][j].y.convert_to<double>());
    }
    printf("Z");
  }
  printf(
      R"("/><g fill="none" stroke="purple" stroke-width="0.003">)");
  for (const auto& e : problem.skelton) {
    printf(
        R"(<path d="M%.3f %.3fL%.3f %.3f"/>)", e.first.x.convert_to<double>(),
        e.first.y.convert_to<double>(), e.second.x.convert_to<double>(),
        e.second.y.convert_to<double>());
  }
  printf("</g></svg>");

  return 0;
}
<commit_msg>fix flipping svg for problems<commit_after>#include "problem.h"

#include <complex>
#include <vector>
#include <fstream>

#include "base/base.h"
#include "boost/multiprecision/cpp_int.hpp"
#include "boost/rational.hpp"
#include "polygon.h"

DEFINE_bool(expand_viewbox, true,
            "Expand viewbox to covert the entire silhouette.");
DEFINE_bool(shrink_viewbox, true,
            "Shrink viewbox to fit silhouette and hide the original rect.");
DEFINE_string(input, "/dev/stdin", "input problem file");

using boost::rational;
using boost::rational_cast;
using namespace std;

int main(int argc, char** argv) {
  ParseCommandLineFlags(&argc, &argv);

  std::ifstream ifs(FLAGS_input);
  Problem problem;
  ReadProblem(ifs, &problem);

  // viewbox size
  Q min_x = 0, min_y = 0, max_x = 1, max_y = 1;
  if (FLAGS_shrink_viewbox) {
    min_x = max_x = problem.polygons[0][0].x;
    min_y = max_y = problem.polygons[0][0].y;
  }
  if (FLAGS_expand_viewbox) {
    for (int i = 0; i < problem.polygons.size(); ++i) {
      for (const auto& v : problem.polygons[i]) {
        if (v.x < min_x) min_x = v.x;
        if (max_x < v.x) max_x = v.x;
        if (v.y < min_y) min_y = v.y;
        if (max_y < v.y) max_y = v.y;
      }
    }
  }
  if (FLAGS_shrink_viewbox) {
    // Translate to reasonable coordinates
    for (auto& p : problem.polygons) {
      for (auto& v : p) {
        v.x -= min_x;
        v.y -= min_y;
      }
    }
    for (auto& e : problem.skelton) {
      e.first.x -= min_x;
      e.first.y -= min_y;
      e.second.x -= min_x;
      e.second.y -= min_y;
    }
    LOG(INFO) << "Translate " << min_x << "," << min_y;
    max_x -= min_x;
    max_y -= min_y;
    min_x = min_y = 0;
  }
  printf(
      R"q(<svg xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" width="400px" height="400px" viewBox="%.3f %.3f %.3f %.3f" stroke-linejoin="round" stroke-linecap="round"><g transform="translate(0,%.3f) scale(1,-1)">)q",
      min_x.convert_to<double>() - 0.0025, min_y.convert_to<double>() - 0.0025,
      Q(max_x - min_x).convert_to<double>() + 0.005,
      Q(max_y - min_y).convert_to<double>() + 0.005,
      Q(max_y - min_y).convert_to<double>() + 0.005);
  if (!FLAGS_shrink_viewbox) {
    printf(
        R"(<rect x="0" y="0" width="1" height="1" fill="none" stroke="blue" stroke-width="0.005"/>)");
  }
  printf(
      R"(<path fill="silver" stroke="gray" stroke-width="0.005" fill-rule="nonzero" d=")");
  for (int i = 0; i < problem.polygons.size(); ++i) {
    for (int j = 0; j < problem.polygons[i].size(); ++j) {
      printf("%c%.3f %.3f", j == 0 ? 'M' : 'L',
             problem.polygons[i][j].x.convert_to<double>(),
             problem.polygons[i][j].y.convert_to<double>());
    }
    printf("Z");
  }
  printf(
      R"("/><g fill="none" stroke="purple" stroke-width="0.003">)");
  for (const auto& e : problem.skelton) {
    printf(
        R"(<path d="M%.3f %.3fL%.3f %.3f"/>)", e.first.x.convert_to<double>(),
        e.first.y.convert_to<double>(), e.second.x.convert_to<double>(),
        e.second.y.convert_to<double>());
  }
  printf("</g></svg>");

  return 0;
}
<|endoftext|>


<commit_before><commit_msg>Disable checking for duplicate wishbone devices.<commit_after><|endoftext|>


<commit_before>/*
 *
 * Copyright 2015, Google 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 Google 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.
 *
 */

#include <limits>
#include <cstdint>

#include "grpc/grpc.h"
#include "grpc/support/time.h"
#include "timeval.h"

namespace grpc {
namespace node {

gpr_timespec MillisecondsToTimespec(double millis) {
  if (millis == std::numeric_limits<double>::infinity()) {
    return gpr_inf_future(GPR_CLOCK_REALTIME);
  } else if (millis == -std::numeric_limits<double>::infinity()) {
    return gpr_inf_past(GPR_CLOCK_REALTIME);
  } else {
    return gpr_time_from_micros(static_cast<int64_t>(millis * 1000),
                                GPR_CLOCK_REALTIME);
  }
}

double TimespecToMilliseconds(gpr_timespec timespec) {
  timespec = gpr_convert_clock_type(timespec, GPR_CLOCK_REALTIME);
  if (gpr_time_cmp(timespec, gpr_inf_future(GPR_CLOCK_REALTIME)) == 0) {
    return std::numeric_limits<double>::infinity();
  } else if (gpr_time_cmp(timespec, gpr_inf_past(GPR_CLOCK_REALTIME)) == 0) {
    return -std::numeric_limits<double>::infinity();
  } else {
    return (static_cast<double>(timespec.tv_sec) * 1000 +
            static_cast<double>(timespec.tv_nsec) / 1000000);
  }
}

}  // namespace node
}  // namespace grpc
<commit_msg>Clang format and fix copyrights<commit_after>/*
 *
 * Copyright 2015-2016, Google 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 Google 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.
 *
 */

#include <limits>
#include <cstdint>

#include "grpc/grpc.h"
#include "grpc/support/time.h"
#include "timeval.h"

namespace grpc {
namespace node {

gpr_timespec MillisecondsToTimespec(double millis) {
  if (millis == std::numeric_limits<double>::infinity()) {
    return gpr_inf_future(GPR_CLOCK_REALTIME);
  } else if (millis == -std::numeric_limits<double>::infinity()) {
    return gpr_inf_past(GPR_CLOCK_REALTIME);
  } else {
    return gpr_time_from_micros(static_cast<int64_t>(millis * 1000),
                                GPR_CLOCK_REALTIME);
  }
}

double TimespecToMilliseconds(gpr_timespec timespec) {
  timespec = gpr_convert_clock_type(timespec, GPR_CLOCK_REALTIME);
  if (gpr_time_cmp(timespec, gpr_inf_future(GPR_CLOCK_REALTIME)) == 0) {
    return std::numeric_limits<double>::infinity();
  } else if (gpr_time_cmp(timespec, gpr_inf_past(GPR_CLOCK_REALTIME)) == 0) {
    return -std::numeric_limits<double>::infinity();
  } else {
    return (static_cast<double>(timespec.tv_sec) * 1000 +
            static_cast<double>(timespec.tv_nsec) / 1000000);
  }
}

}  // namespace node
}  // namespace grpc
<|endoftext|>


<commit_before>/*!
 * \file rRendererBasic.cpp
 * \brief \b Classes : \a rRendererBasic
 */
/*
 * Copyright (C) 2016 EEnginE project
 *
 * 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 "defines.hpp"
#include "rRendererBasic.hpp"
#include "uConfig.hpp"
#include "uEnum2Str.hpp"
#include "uLog.hpp"
#include "rObjectBase.hpp"
#include "rPipeline.hpp"
#include "rWorld.hpp"

using namespace e_engine;

VkResult rRendererBasic::initRenderer(std::vector<VkImageView> _images, VkSurfaceFormatKHR _surfaceFormat) {
  vFbData.resize(_images.size());
  vRenderPass.setup(getRenderPassDescription(_surfaceFormat));

  auto lRes = vRenderPass.init(vDevice);
  if (lRes != VK_SUCCESS) {
    eLOG(L"Failed to init render pass. Can not initialize renderer");
    return lRes;
  }

  assert(vFbData.size() == _images.size());
  for (size_t i = 0; i < vFbData.size(); ++i) {
    vFbData[i].frameBuffer.setup(vRenderPass);
    lRes = vFbData[i].frameBuffer.reCreateFrameBuffers(getFrameBufferDescription(_images[i]));
  }

  return lRes;
}

void rRendererBasic::destroyRenderer() {
  vRenderPass.destroy();
  vFbData.clear();
}

void rRendererBasic::initCmdBuffers(vkuCommandPool *_pool) {
  for (auto i : vObjects) {
    if (i.get() == nullptr) {
      eLOG("FATAL ERROR: nullptr in object list!");
      return;
    }

    if (i->isMesh()) {
      vRenderObjects.emplace_back(i);
    }
  }

  for (auto &i : vFbData) {
    i.buffers.resize(vRenderObjects.size());
    for (auto &j : i.buffers) {
      j.init(_pool, VK_COMMAND_BUFFER_LEVEL_SECONDARY);
    }
  }
}

void rRendererBasic::freeCmdBuffers() {
  for (auto &i : vFbData)
    i.buffers.clear();

  vRenderObjects.clear();
}

VkImageView rRendererBasic::getAttachmentView(internal::rRendererBase::ATTACHMENT_ROLE _role) {
  switch (_role) {
    case DEPTH_STENCIL: return VK_NULL_HANDLE;
    default: return VK_NULL_HANDLE;
  }
}

/*!
 * \brief Records the Vulkan command buffers, for a framebuffer
 * \note _toRender.size() MUST BE EQUAL TO _fb.secondary.size()
 * Elements in _toRender can be skipped by setting them to nullptr
 */
void rRendererBasic::recordCmdBuffers(Framebuffer_vk &_fb, RECORD_TARGET _toRender) {
  _fb.render.begin();

  vkCmdBeginRenderPass(*_fb.render, &vCmdRecordInfo.lRPInfo, VK_SUBPASS_CONTENTS_SECONDARY_COMMAND_BUFFERS);

  for (uint32_t i = 0; i < vRenderObjects.size(); i++) {
    if (_toRender == RECORD_PUSH_CONST_ONLY)
      if (!vRenderObjects[i]->supportsPushConstants())
        continue;

    auto *lPipe = vRenderObjects[i]->getPipeline();
    if (!lPipe) {
      eLOG("Object ", vRenderObjects[i]->getName(), " has no pipeline!");
      continue;
    }

    vFbData[_fb.index].buffers[i].begin(VK_COMMAND_BUFFER_USAGE_RENDER_PASS_CONTINUE_BIT, &vCmdRecordInfo.lInherit);

    if (lPipe->getNumViewpors() > 0)
      vkCmdSetViewport(*vFbData[_fb.index].buffers[i], 0, 1, &vCmdRecordInfo.lViewPort);

    if (lPipe->getNumScissors() > 0)
      vkCmdSetScissor(*vFbData[_fb.index].buffers[i], 0, 1, &vCmdRecordInfo.lScissors);

    vRenderObjects[i]->record(*vFbData[_fb.index].buffers[i]);
    vFbData[_fb.index].buffers[i].end();
  }

  for (auto &i : vFbData[_fb.index].buffers) {
    vkCmdExecuteCommands(*_fb.render, 1, &i.get());
  }

  vkCmdEndRenderPass(*_fb.render);

  auto lRes = vkEndCommandBuffer(*_fb.render);
  if (lRes) {
    eLOG("'vkEndCommandBuffer' returned ", uEnum2Str::toStr(lRes));
    //! \todo Handle this somehow (practically this code must not execute)
  }
}
<commit_msg>Fixed an other warning<commit_after>/*!
 * \file rRendererBasic.cpp
 * \brief \b Classes : \a rRendererBasic
 */
/*
 * Copyright (C) 2016 EEnginE project
 *
 * 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 "defines.hpp"
#include "rRendererBasic.hpp"
#include "uConfig.hpp"
#include "uEnum2Str.hpp"
#include "uLog.hpp"
#include "rObjectBase.hpp"
#include "rPipeline.hpp"
#include "rWorld.hpp"

using namespace e_engine;

VkResult rRendererBasic::initRenderer(std::vector<VkImageView> _images, VkSurfaceFormatKHR _surfaceFormat) {
  vFbData.resize(_images.size());
  vRenderPass.setup(getRenderPassDescription(_surfaceFormat));

  auto lRes = vRenderPass.init(vDevice);
  if (lRes != VK_SUCCESS) {
    eLOG(L"Failed to init render pass. Can not initialize renderer");
    return lRes;
  }

  for (size_t i = 0; i < vFbData.size(); ++i) {
    vFbData[i].frameBuffer.setup(vRenderPass);
    lRes = vFbData[i].frameBuffer.reCreateFrameBuffers(getFrameBufferDescription(_images[i]));
  }

  return lRes;
}

void rRendererBasic::destroyRenderer() {
  vRenderPass.destroy();
  vFbData.clear();
}

void rRendererBasic::initCmdBuffers(vkuCommandPool *_pool) {
  for (auto i : vObjects) {
    if (i.get() == nullptr) {
      eLOG("FATAL ERROR: nullptr in object list!");
      return;
    }

    if (i->isMesh()) {
      vRenderObjects.emplace_back(i);
    }
  }

  for (auto &i : vFbData) {
    i.buffers.resize(vRenderObjects.size());
    for (auto &j : i.buffers) {
      j.init(_pool, VK_COMMAND_BUFFER_LEVEL_SECONDARY);
    }
  }
}

void rRendererBasic::freeCmdBuffers() {
  for (auto &i : vFbData)
    i.buffers.clear();

  vRenderObjects.clear();
}

VkImageView rRendererBasic::getAttachmentView(internal::rRendererBase::ATTACHMENT_ROLE _role) {
  switch (_role) {
    case DEPTH_STENCIL: return VK_NULL_HANDLE;
    default: return VK_NULL_HANDLE;
  }
}

/*!
 * \brief Records the Vulkan command buffers, for a framebuffer
 * \note _toRender.size() MUST BE EQUAL TO _fb.secondary.size()
 * Elements in _toRender can be skipped by setting them to nullptr
 */
void rRendererBasic::recordCmdBuffers(Framebuffer_vk &_fb, RECORD_TARGET _toRender) {
  _fb.render.begin();

  vkCmdBeginRenderPass(*_fb.render, &vCmdRecordInfo.lRPInfo, VK_SUBPASS_CONTENTS_SECONDARY_COMMAND_BUFFERS);

  for (uint32_t i = 0; i < vRenderObjects.size(); i++) {
    if (_toRender == RECORD_PUSH_CONST_ONLY)
      if (!vRenderObjects[i]->supportsPushConstants())
        continue;

    auto *lPipe = vRenderObjects[i]->getPipeline();
    if (!lPipe) {
      eLOG("Object ", vRenderObjects[i]->getName(), " has no pipeline!");
      continue;
    }

    vFbData[_fb.index].buffers[i].begin(VK_COMMAND_BUFFER_USAGE_RENDER_PASS_CONTINUE_BIT, &vCmdRecordInfo.lInherit);

    if (lPipe->getNumViewpors() > 0)
      vkCmdSetViewport(*vFbData[_fb.index].buffers[i], 0, 1, &vCmdRecordInfo.lViewPort);

    if (lPipe->getNumScissors() > 0)
      vkCmdSetScissor(*vFbData[_fb.index].buffers[i], 0, 1, &vCmdRecordInfo.lScissors);

    vRenderObjects[i]->record(*vFbData[_fb.index].buffers[i]);
    vFbData[_fb.index].buffers[i].end();
  }

  for (auto &i : vFbData[_fb.index].buffers) {
    vkCmdExecuteCommands(*_fb.render, 1, &i.get());
  }

  vkCmdEndRenderPass(*_fb.render);

  auto lRes = vkEndCommandBuffer(*_fb.render);
  if (lRes) {
    eLOG("'vkEndCommandBuffer' returned ", uEnum2Str::toStr(lRes));
    //! \todo Handle this somehow (practically this code must not execute)
  }
}
<|endoftext|>


<commit_before>/*
 *  world_node.cpp
 *
 *
 *  Created on: Aug 25, 2014
 *  Authors:   Rares Ambrus
 *            raambrus <at> kth.se
 */

/* Copyright (c) 2014, Rares Ambrus, CVAP, KTH
   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 KTH 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 KTH 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.
*/

// ROS includes.
#include <ros/ros.h>
#include <ros/time.h>
#include <visualization_msgs/Marker.h>
#include <geometry_msgs/Point.h>
#include <kdl/frames.hpp>
#include <kdl_conversions/kdl_msg.h>

// Boost includes
#include <stdio.h>
#include <stdlib.h>

// Random seed
#include <ctime>

int main(int argc, char **argv)
{
    // Set up ROS.
    ros::init(argc, argv, "world_node");
    ros::NodeHandle n;
    ros::Rate r(10); // 50 Hz

    // Create random number generators
    srand(time(NULL));
    double angle_z = ((double)(rand()%40)-20)*M_PI/180.0;
    if(angle_z <= 5*M_PI/180.0) angle_z += 5*M_PI/180.0;

    KDL::Frame pose;
    pose.M = KDL::Rotation::RotZ(angle_z);
    pose.p = KDL::Vector(0.0, 0.4+0.23/2, 0.1);

    ros::Publisher vis_pub = n.advertise<visualization_msgs::Marker>( "wall_marker", 0 );
    visualization_msgs::Marker wall_marker;
    wall_marker.header.frame_id = "odom";
    wall_marker.header.stamp = ros::Time();
    wall_marker.ns = "world";
    wall_marker.id = 0;
    wall_marker.type = visualization_msgs::Marker::CUBE;
    wall_marker.action = visualization_msgs::Marker::ADD;
    wall_marker.scale.x = 500.0;
    wall_marker.scale.y = 0.01;
    wall_marker.scale.z = 0.2;
    wall_marker.color.a = 1.0;
    wall_marker.color.r = (255.0/255.0);
    wall_marker.color.g = (0.0/255.0);
    wall_marker.color.b = (0.0/255.0);

    tf::poseKDLToMsg(pose, wall_marker.pose);

    // Main loop.
    while (n.ok())
    {
        vis_pub.publish(wall_marker);
        ros::spinOnce();
        r.sleep();
    }

    return 0;
}
<commit_msg>fixed wall positioning<commit_after>/*
 *  world_node.cpp
 *
 *
 *  Created on: Aug 25, 2014
 *  Authors:   Rares Ambrus
 *            raambrus <at> kth.se
 */

/* Copyright (c) 2014, Rares Ambrus, CVAP, KTH
   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 KTH 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 KTH 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.
*/

// ROS includes.
#include <ros/ros.h>
#include <ros/time.h>
#include <visualization_msgs/Marker.h>
#include <geometry_msgs/Point.h>
#include <kdl/frames.hpp>
#include <kdl_conversions/kdl_msg.h>

// Boost includes
#include <stdio.h>
#include <stdlib.h>

// Random seed
#include <ctime>

int main(int argc, char **argv)
{
    // Set up ROS.
    ros::init(argc, argv, "world_node");
    ros::NodeHandle n;
    ros::Rate r(10); // 50 Hz

    // Create random number generators
    srand(time(NULL));
    double angle_z = ((double)(rand()%40)-20)*M_PI/180.0;
    if(angle_z <= 5*M_PI/180.0 && angle_z >= 0.0) angle_z += 5*M_PI/180.0;
    if(angle_z >= -5*M_PI/180.0 && angle_z <= 0.0) angle_z -= 5*M_PI/180.0;

    KDL::Frame pose;
    pose.M = KDL::Rotation::RotZ(angle_z);
    pose.p = KDL::Vector(0.0, 0.4+0.23/2, 0.1);

    ros::Publisher vis_pub = n.advertise<visualization_msgs::Marker>( "wall_marker", 0 );
    visualization_msgs::Marker wall_marker;
    wall_marker.header.frame_id = "odom";
    wall_marker.header.stamp = ros::Time();
    wall_marker.ns = "world";
    wall_marker.id = 0;
    wall_marker.type = visualization_msgs::Marker::CUBE;
    wall_marker.action = visualization_msgs::Marker::ADD;
    wall_marker.scale.x = 500.0;
    wall_marker.scale.y = 0.01;
    wall_marker.scale.z = 0.2;
    wall_marker.color.a = 1.0;
    wall_marker.color.r = (255.0/255.0);
    wall_marker.color.g = (0.0/255.0);
    wall_marker.color.b = (0.0/255.0);

    tf::poseKDLToMsg(pose, wall_marker.pose);

    // Main loop.
    while (n.ok())
    {
        vis_pub.publish(wall_marker);
        ros::spinOnce();
        r.sleep();
    }

    return 0;
}
<|endoftext|>


<commit_before>/*
 * SegmentInformation.cpp
 *****************************************************************************
 * Copyright (C) 2014 - VideoLAN and VLC Authors
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms of the GNU Lesser General Public License as published
 * by the Free Software Foundation; either version 2.1 of the License, or
 * (at your option) any later version.
 *
 * This program 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 program; if not, write to the Free Software Foundation,
 * Inc., 51 Franklin Street, Fifth Floor, Boston MA 02110-1301, USA.
 *****************************************************************************/
#include "SegmentInformation.hpp"

#include "Segment.h"
#include "SegmentBase.h"
#include "SegmentList.h"
#include "SegmentTemplate.h"
#include "SegmentTimeline.h"
#include "AbstractPlaylist.hpp"

#include <algorithm>

using namespace adaptative::playlist;

SegmentInformation::SegmentInformation(SegmentInformation *parent_) :
    ICanonicalUrl( parent_ ),
    TimescaleAble( parent_ )
{
    parent = parent_;
    init();
}

SegmentInformation::SegmentInformation(AbstractPlaylist * parent_) :
    ICanonicalUrl(parent_),
    TimescaleAble()
{
    parent = NULL;
    init();
}

void SegmentInformation::init()
{
    baseUrl.Set(NULL);
    segmentBase = NULL;
    segmentList = NULL;
    mediaSegmentTemplate = NULL;
    switchpolicy = SWITCH_UNKNOWN;
}

SegmentInformation::~SegmentInformation()
{
    delete segmentBase;
    delete segmentList;
    delete mediaSegmentTemplate;
}

AbstractPlaylist * SegmentInformation::getPlaylist() const
{
    if(parent)
        return parent->getPlaylist();
    else
        return NULL;
}

std::size_t SegmentInformation::getSegments(SegmentInfoType type, std::vector<ISegment *> &retSegments) const
{
    switch (type)
    {
        case INFOTYPE_INIT:
        {
            /* init segments are always single segment */
            if( segmentBase && segmentBase->initialisationSegment.Get() )
            {
                retSegments.push_back( segmentBase->initialisationSegment.Get() );
            }
            else if( segmentList && segmentList->initialisationSegment.Get() )
            {
                retSegments.push_back( segmentList->initialisationSegment.Get() );
            }
            else if( mediaSegmentTemplate && mediaSegmentTemplate->initialisationSegment.Get() )
            {
                retSegments.push_back( mediaSegmentTemplate->initialisationSegment.Get() );
            }
        }
        break;

        case INFOTYPE_MEDIA:
        {
            if( mediaSegmentTemplate )
            {
                retSegments.push_back( mediaSegmentTemplate );
            }
            else if ( segmentList && !segmentList->getSegments().empty() )
            {
                std::vector<ISegment *>::const_iterator it;
                for(it=segmentList->getSegments().begin();
                    it!=segmentList->getSegments().end(); ++it)
                {
                    std::vector<ISegment *> list = (*it)->subSegments();
                    retSegments.insert( retSegments.end(), list.begin(), list.end() );
                }
            }
            else if( segmentBase )
            {
                std::vector<ISegment *> list = segmentBase->subSegments();
                retSegments.insert( retSegments.end(), list.begin(), list.end() );
            }
        }
        break;

        case INFOTYPE_INDEX:
        {
            /* index segments are always single segment */
            if( segmentBase && segmentBase->indexSegment.Get() )
            {
                retSegments.push_back( segmentBase->indexSegment.Get() );
            }
            else if( segmentList && segmentList->indexSegment.Get() )
            {
                retSegments.push_back( segmentList->indexSegment.Get() );
            }
            // templated index ?
        }

        default:
        break;
    }

    if( retSegments.empty() && parent )
    {
        return parent->getSegments( type, retSegments );
    }
    else
    {
        return retSegments.size();
    }
}

std::size_t SegmentInformation::getAllSegments(std::vector<ISegment *> &retSegments) const
{
    for(int i=0; i<InfoTypeCount; i++)
    {
        std::vector<ISegment *> segs;
        if( getSegments(static_cast<SegmentInfoType>(i), segs) )
            retSegments.insert( retSegments.end(), segs.begin(), segs.end() );
    }
    return retSegments.size();
}

/* Returns wanted segment, or next in sequence if not found */
ISegment * SegmentInformation::getNextSegment(SegmentInfoType type, uint64_t i_pos,
                                              uint64_t *pi_newpos, bool *pb_gap) const
{
    *pb_gap = false;
    *pi_newpos = i_pos;
    if( type != INFOTYPE_MEDIA )
        return NULL;

    std::vector<ISegment *> retSegments;
    const size_t size = getSegments( type, retSegments );
    if( size )
    {
        std::vector<ISegment *>::const_iterator it;
        for(it = retSegments.begin(); it != retSegments.end(); ++it)
        {
            ISegment *seg = *it;
            if(seg->isTemplate()) /* we don't care about seq number */
            {
                /* Check if we don't exceed timeline */
                MediaSegmentTemplate *templ = dynamic_cast<MediaSegmentTemplate*>(retSegments[0]);
                SegmentTimeline *timeline = (templ) ? templ->segmentTimeline.Get() : NULL;
                if(timeline)
                {
                    *pi_newpos = std::max(timeline->minElementNumber(), i_pos);
                    if(timeline->maxElementNumber() < i_pos)
                        return NULL;
                }
                else
                {
                    *pi_newpos = i_pos;
                    /* start number */
                    *pi_newpos = std::max((uint64_t)templ->startNumber.Get(), i_pos);
                }
                return seg;
            }
            else if(seg->getSequenceNumber() >= i_pos)
            {
                *pi_newpos = seg->getSequenceNumber();
                *pb_gap = (*pi_newpos != i_pos);
                return seg;
            }
        }
    }

    return NULL;
}

ISegment * SegmentInformation::getSegment(SegmentInfoType type, uint64_t pos) const
{
    std::vector<ISegment *> retSegments;
    const size_t size = getSegments( type, retSegments );
    if( size )
    {
        if(size == 1 && retSegments[0]->isTemplate())
        {
            MediaSegmentTemplate *templ = dynamic_cast<MediaSegmentTemplate*>(retSegments[0]);
            if(!templ || templ->segmentTimeline.Get() == NULL ||
               templ->segmentTimeline.Get()->maxElementNumber() > pos)
                return templ;
        }
        else
        {
            std::vector<ISegment *>::const_iterator it;
            for(it = retSegments.begin(); it != retSegments.end(); ++it)
            {
                ISegment *seg = *it;
                if(seg->getSequenceNumber() >= pos)
                {
                    if(seg->getSequenceNumber() == pos)
                        return seg;
                    else
                        return NULL;
                }
            }
        }
    }

    return NULL;
}

bool SegmentInformation::getSegmentNumberByTime(mtime_t time, uint64_t *ret) const
{
    if( mediaSegmentTemplate )
    {
        const uint64_t timescale = mediaSegmentTemplate->inheritTimescale();

        SegmentTimeline *timeline = mediaSegmentTemplate->segmentTimeline.Get();
        if(timeline)
        {
            time = time * timescale / CLOCK_FREQ;
            *ret = timeline->getElementNumberByScaledPlaybackTime(time);
            return true;
        }

        const mtime_t duration = mediaSegmentTemplate->duration.Get();
        *ret = mediaSegmentTemplate->startNumber.Get();
        if(duration)
        {
            *ret += time / (CLOCK_FREQ * duration / timescale);
            return true;
        }
    }
    else if ( segmentList && !segmentList->getSegments().empty() )
    {
        const uint64_t timescale = segmentList->inheritTimescale();
        time = time * timescale / CLOCK_FREQ;
        return segmentList->getSegmentNumberByScaledTime(time, ret);
    }
    else if( segmentBase )
    {
        const uint64_t timescale = inheritTimescale();
        time = time * timescale / CLOCK_FREQ;
        *ret = 0;
        const std::vector<ISegment *> list = segmentBase->subSegments();
        return SegmentInfoCommon::getSegmentNumberByScaledTime(list, time, ret);
    }

    if(parent)
        return parent->getSegmentNumberByTime(time, ret);
    else
        return false;
}

mtime_t SegmentInformation::getPlaybackTimeBySegmentNumber(uint64_t number) const
{
    SegmentList *segList;
    MediaSegmentTemplate *mediaTemplate;
    mtime_t time = 0;
    if( (mediaTemplate = inheritSegmentTemplate()) )
    {
        uint64_t timescale = mediaTemplate->inheritTimescale();
        if(mediaTemplate->segmentTimeline.Get())
        {
            time = mediaTemplate->segmentTimeline.Get()->
                    getScaledPlaybackTimeByElementNumber(number);
        }
        else
        {
            time = number * mediaTemplate->duration.Get();
        }
        time = CLOCK_FREQ * time / timescale;
    }
    else if ( (segList = inheritSegmentList()) )
    {
        time = segList->getPlaybackTimeBySegmentNumber(number);
    }

    return time;
}

void SegmentInformation::getDurationsRange(mtime_t *min, mtime_t *max) const
{
    /* FIXME: cache stuff in segment holders */
    std::vector<ISegment *> seglist;
    getSegments(INFOTYPE_MEDIA, seglist);
    std::vector<ISegment *>::const_iterator it;
    mtime_t total = 0;
    for(it = seglist.begin(); it != seglist.end(); ++it)
    {
        const mtime_t duration = (*it)->duration.Get() * CLOCK_FREQ / inheritTimescale();
        if(duration)
        {
            total += duration;

            if (!*min || duration < *min)
                *min = duration;
        }
    }

    if(total > *max)
        *max = total;

    if(mediaSegmentTemplate && mediaSegmentTemplate->segmentTimeline.Get())
    {
        const mtime_t duration = mediaSegmentTemplate->segmentTimeline.Get()->end() -
                                 mediaSegmentTemplate->segmentTimeline.Get()->start();
        *min = 0; /* fixme: get minimum ? */

        if(duration > *max)
            *max = duration;
    }

    for(size_t i=0; i<childs.size(); i++)
        childs.at(i)->getDurationsRange(min, max);
}

SegmentInformation * SegmentInformation::getChildByID(const ID &id)
{
    std::vector<SegmentInformation *>::const_iterator it;
    for(it=childs.begin(); it!=childs.end(); ++it)
    {
        if( (*it)->getID() == id )
            return *it;
    }
    return NULL;
}

void SegmentInformation::mergeWith(SegmentInformation *updated, mtime_t prunetime)
{
    /* Support Segment List for now */
    if(segmentList && updated->segmentList)
        segmentList->mergeWith(updated->segmentList);

    if(mediaSegmentTemplate && updated->mediaSegmentTemplate)
        mediaSegmentTemplate->mergeWith(updated->mediaSegmentTemplate, prunetime);

    std::vector<SegmentInformation *>::const_iterator it;
    for(it=childs.begin(); it!=childs.end(); ++it)
    {
        SegmentInformation *child = *it;
        SegmentInformation *updatedChild = updated->getChildByID(child->getID());
        if(updatedChild)
            child->mergeWith(updatedChild, prunetime);
    }
    /* FIXME: handle difference */
}

void SegmentInformation::pruneBySegmentNumber(uint64_t num)
{
    if(segmentList)
        segmentList->pruneBySegmentNumber(num);

    for(size_t i=0; i<childs.size(); i++)
        childs.at(i)->pruneBySegmentNumber(num);
}

void SegmentInformation::runLocalUpdates(mtime_t, uint64_t)
{

}

SegmentInformation::SwitchPolicy SegmentInformation::getSwitchPolicy() const
{
    if(switchpolicy == SWITCH_UNKNOWN)
        return (parent) ? parent->getSwitchPolicy() : SWITCH_UNAVAILABLE;
    else
        return switchpolicy;
}

mtime_t SegmentInformation::getPeriodStart() const
{
    if(parent)
        return parent->getPeriodStart();
    else
        return 0;
}

void SegmentInformation::setSegmentList(SegmentList *list)
{
    if(segmentList)
    {
        segmentList->mergeWith(list);
        delete list;
    }
    else
    {
        segmentList = list;
    }
}

void SegmentInformation::setSegmentBase(SegmentBase *base)
{
    if(segmentBase)
        delete segmentBase;
    segmentBase = base;
}

void SegmentInformation::setSegmentTemplate(MediaSegmentTemplate *templ)
{
    if(mediaSegmentTemplate)
    {
        mediaSegmentTemplate->mergeWith(templ, 0);
        delete templ;
    }
    mediaSegmentTemplate = templ;
}

static void insertIntoSegment(std::vector<ISegment *> &seglist, size_t start,
                              size_t end, stime_t time)
{
    std::vector<ISegment *>::iterator segIt;
    for(segIt = seglist.begin(); segIt < seglist.end(); ++segIt)
    {
        ISegment *segment = *segIt;
        if(segment->getClassId() == Segment::CLASSID_SEGMENT &&
           segment->contains(end + segment->getOffset()))
        {
            SubSegment *subsegment = new SubSegment(segment,
                                                    start + segment->getOffset(),
                                                    (end != 0) ? end + segment->getOffset() : 0);
            subsegment->startTime.Set(time);
            segment->addSubSegment(subsegment);
            break;
        }
    }
}

void SegmentInformation::SplitUsingIndex(std::vector<SplitPoint> &splitlist)
{
    std::vector<ISegment *> seglist;
    getSegments(INFOTYPE_MEDIA, seglist);
    std::vector<SplitPoint>::const_iterator splitIt;
    size_t start = 0, end = 0;
    mtime_t time = 0;
    const uint64_t i_timescale = inheritTimescale();

    for(splitIt = splitlist.begin(); splitIt < splitlist.end(); ++splitIt)
    {
        start = end;
        SplitPoint split = *splitIt;
        end = split.offset;
        if(splitIt == splitlist.begin() && split.offset == 0)
            continue;
        time = split.time;
        insertIntoSegment(seglist, start, end - 1, time * i_timescale / CLOCK_FREQ);
    }

    if(start != 0)
    {
        start = end;
        end = 0;
        insertIntoSegment(seglist, start, end, time * i_timescale / CLOCK_FREQ);
    }
}

void SegmentInformation::setSwitchPolicy(SegmentInformation::SwitchPolicy policy)
{
    switchpolicy = policy;
}

Url SegmentInformation::getUrlSegment() const
{
    if(baseUrl.Get() && baseUrl.Get()->hasScheme())
    {
        return *(baseUrl.Get());
    }
    else
    {
        Url ret = getParentUrlSegment();
        if (baseUrl.Get())
            ret.append(*(baseUrl.Get()));
        return ret;
    }
}

SegmentBase * SegmentInformation::inheritSegmentBase() const
{
    if(segmentBase)
        return segmentBase;
    else if (parent)
        return parent->inheritSegmentBase();
    else
        return NULL;
}

SegmentList * SegmentInformation::inheritSegmentList() const
{
    if(segmentList)
        return segmentList;
    else if (parent)
        return parent->inheritSegmentList();
    else
        return NULL;
}

MediaSegmentTemplate * SegmentInformation::inheritSegmentTemplate() const
{
    if(mediaSegmentTemplate)
        return mediaSegmentTemplate;
    else if (parent)
        return parent->inheritSegmentTemplate();
    else
        return NULL;
}
<commit_msg>demux: adaptative: add timeline pruning by number<commit_after>/*
 * SegmentInformation.cpp
 *****************************************************************************
 * Copyright (C) 2014 - VideoLAN and VLC Authors
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms of the GNU Lesser General Public License as published
 * by the Free Software Foundation; either version 2.1 of the License, or
 * (at your option) any later version.
 *
 * This program 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 program; if not, write to the Free Software Foundation,
 * Inc., 51 Franklin Street, Fifth Floor, Boston MA 02110-1301, USA.
 *****************************************************************************/
#include "SegmentInformation.hpp"

#include "Segment.h"
#include "SegmentBase.h"
#include "SegmentList.h"
#include "SegmentTemplate.h"
#include "SegmentTimeline.h"
#include "AbstractPlaylist.hpp"

#include <algorithm>

using namespace adaptative::playlist;

SegmentInformation::SegmentInformation(SegmentInformation *parent_) :
    ICanonicalUrl( parent_ ),
    TimescaleAble( parent_ )
{
    parent = parent_;
    init();
}

SegmentInformation::SegmentInformation(AbstractPlaylist * parent_) :
    ICanonicalUrl(parent_),
    TimescaleAble()
{
    parent = NULL;
    init();
}

void SegmentInformation::init()
{
    baseUrl.Set(NULL);
    segmentBase = NULL;
    segmentList = NULL;
    mediaSegmentTemplate = NULL;
    switchpolicy = SWITCH_UNKNOWN;
}

SegmentInformation::~SegmentInformation()
{
    delete segmentBase;
    delete segmentList;
    delete mediaSegmentTemplate;
}

AbstractPlaylist * SegmentInformation::getPlaylist() const
{
    if(parent)
        return parent->getPlaylist();
    else
        return NULL;
}

std::size_t SegmentInformation::getSegments(SegmentInfoType type, std::vector<ISegment *> &retSegments) const
{
    switch (type)
    {
        case INFOTYPE_INIT:
        {
            /* init segments are always single segment */
            if( segmentBase && segmentBase->initialisationSegment.Get() )
            {
                retSegments.push_back( segmentBase->initialisationSegment.Get() );
            }
            else if( segmentList && segmentList->initialisationSegment.Get() )
            {
                retSegments.push_back( segmentList->initialisationSegment.Get() );
            }
            else if( mediaSegmentTemplate && mediaSegmentTemplate->initialisationSegment.Get() )
            {
                retSegments.push_back( mediaSegmentTemplate->initialisationSegment.Get() );
            }
        }
        break;

        case INFOTYPE_MEDIA:
        {
            if( mediaSegmentTemplate )
            {
                retSegments.push_back( mediaSegmentTemplate );
            }
            else if ( segmentList && !segmentList->getSegments().empty() )
            {
                std::vector<ISegment *>::const_iterator it;
                for(it=segmentList->getSegments().begin();
                    it!=segmentList->getSegments().end(); ++it)
                {
                    std::vector<ISegment *> list = (*it)->subSegments();
                    retSegments.insert( retSegments.end(), list.begin(), list.end() );
                }
            }
            else if( segmentBase )
            {
                std::vector<ISegment *> list = segmentBase->subSegments();
                retSegments.insert( retSegments.end(), list.begin(), list.end() );
            }
        }
        break;

        case INFOTYPE_INDEX:
        {
            /* index segments are always single segment */
            if( segmentBase && segmentBase->indexSegment.Get() )
            {
                retSegments.push_back( segmentBase->indexSegment.Get() );
            }
            else if( segmentList && segmentList->indexSegment.Get() )
            {
                retSegments.push_back( segmentList->indexSegment.Get() );
            }
            // templated index ?
        }

        default:
        break;
    }

    if( retSegments.empty() && parent )
    {
        return parent->getSegments( type, retSegments );
    }
    else
    {
        return retSegments.size();
    }
}

std::size_t SegmentInformation::getAllSegments(std::vector<ISegment *> &retSegments) const
{
    for(int i=0; i<InfoTypeCount; i++)
    {
        std::vector<ISegment *> segs;
        if( getSegments(static_cast<SegmentInfoType>(i), segs) )
            retSegments.insert( retSegments.end(), segs.begin(), segs.end() );
    }
    return retSegments.size();
}

/* Returns wanted segment, or next in sequence if not found */
ISegment * SegmentInformation::getNextSegment(SegmentInfoType type, uint64_t i_pos,
                                              uint64_t *pi_newpos, bool *pb_gap) const
{
    *pb_gap = false;
    *pi_newpos = i_pos;
    if( type != INFOTYPE_MEDIA )
        return NULL;

    std::vector<ISegment *> retSegments;
    const size_t size = getSegments( type, retSegments );
    if( size )
    {
        std::vector<ISegment *>::const_iterator it;
        for(it = retSegments.begin(); it != retSegments.end(); ++it)
        {
            ISegment *seg = *it;
            if(seg->isTemplate()) /* we don't care about seq number */
            {
                /* Check if we don't exceed timeline */
                MediaSegmentTemplate *templ = dynamic_cast<MediaSegmentTemplate*>(retSegments[0]);
                SegmentTimeline *timeline = (templ) ? templ->segmentTimeline.Get() : NULL;
                if(timeline)
                {
                    *pi_newpos = std::max(timeline->minElementNumber(), i_pos);
                    if(timeline->maxElementNumber() < i_pos)
                        return NULL;
                }
                else
                {
                    *pi_newpos = i_pos;
                    /* start number */
                    *pi_newpos = std::max((uint64_t)templ->startNumber.Get(), i_pos);
                }
                return seg;
            }
            else if(seg->getSequenceNumber() >= i_pos)
            {
                *pi_newpos = seg->getSequenceNumber();
                *pb_gap = (*pi_newpos != i_pos);
                return seg;
            }
        }
    }

    return NULL;
}

ISegment * SegmentInformation::getSegment(SegmentInfoType type, uint64_t pos) const
{
    std::vector<ISegment *> retSegments;
    const size_t size = getSegments( type, retSegments );
    if( size )
    {
        if(size == 1 && retSegments[0]->isTemplate())
        {
            MediaSegmentTemplate *templ = dynamic_cast<MediaSegmentTemplate*>(retSegments[0]);
            if(!templ || templ->segmentTimeline.Get() == NULL ||
               templ->segmentTimeline.Get()->maxElementNumber() > pos)
                return templ;
        }
        else
        {
            std::vector<ISegment *>::const_iterator it;
            for(it = retSegments.begin(); it != retSegments.end(); ++it)
            {
                ISegment *seg = *it;
                if(seg->getSequenceNumber() >= pos)
                {
                    if(seg->getSequenceNumber() == pos)
                        return seg;
                    else
                        return NULL;
                }
            }
        }
    }

    return NULL;
}

bool SegmentInformation::getSegmentNumberByTime(mtime_t time, uint64_t *ret) const
{
    if( mediaSegmentTemplate )
    {
        const uint64_t timescale = mediaSegmentTemplate->inheritTimescale();

        SegmentTimeline *timeline = mediaSegmentTemplate->segmentTimeline.Get();
        if(timeline)
        {
            time = time * timescale / CLOCK_FREQ;
            *ret = timeline->getElementNumberByScaledPlaybackTime(time);
            return true;
        }

        const mtime_t duration = mediaSegmentTemplate->duration.Get();
        *ret = mediaSegmentTemplate->startNumber.Get();
        if(duration)
        {
            *ret += time / (CLOCK_FREQ * duration / timescale);
            return true;
        }
    }
    else if ( segmentList && !segmentList->getSegments().empty() )
    {
        const uint64_t timescale = segmentList->inheritTimescale();
        time = time * timescale / CLOCK_FREQ;
        return segmentList->getSegmentNumberByScaledTime(time, ret);
    }
    else if( segmentBase )
    {
        const uint64_t timescale = inheritTimescale();
        time = time * timescale / CLOCK_FREQ;
        *ret = 0;
        const std::vector<ISegment *> list = segmentBase->subSegments();
        return SegmentInfoCommon::getSegmentNumberByScaledTime(list, time, ret);
    }

    if(parent)
        return parent->getSegmentNumberByTime(time, ret);
    else
        return false;
}

mtime_t SegmentInformation::getPlaybackTimeBySegmentNumber(uint64_t number) const
{
    SegmentList *segList;
    MediaSegmentTemplate *mediaTemplate;
    mtime_t time = 0;
    if( (mediaTemplate = inheritSegmentTemplate()) )
    {
        uint64_t timescale = mediaTemplate->inheritTimescale();
        if(mediaTemplate->segmentTimeline.Get())
        {
            time = mediaTemplate->segmentTimeline.Get()->
                    getScaledPlaybackTimeByElementNumber(number);
        }
        else
        {
            time = number * mediaTemplate->duration.Get();
        }
        time = CLOCK_FREQ * time / timescale;
    }
    else if ( (segList = inheritSegmentList()) )
    {
        time = segList->getPlaybackTimeBySegmentNumber(number);
    }

    return time;
}

void SegmentInformation::getDurationsRange(mtime_t *min, mtime_t *max) const
{
    /* FIXME: cache stuff in segment holders */
    std::vector<ISegment *> seglist;
    getSegments(INFOTYPE_MEDIA, seglist);
    std::vector<ISegment *>::const_iterator it;
    mtime_t total = 0;
    for(it = seglist.begin(); it != seglist.end(); ++it)
    {
        const mtime_t duration = (*it)->duration.Get() * CLOCK_FREQ / inheritTimescale();
        if(duration)
        {
            total += duration;

            if (!*min || duration < *min)
                *min = duration;
        }
    }

    if(total > *max)
        *max = total;

    if(mediaSegmentTemplate && mediaSegmentTemplate->segmentTimeline.Get())
    {
        const mtime_t duration = mediaSegmentTemplate->segmentTimeline.Get()->end() -
                                 mediaSegmentTemplate->segmentTimeline.Get()->start();
        *min = 0; /* fixme: get minimum ? */

        if(duration > *max)
            *max = duration;
    }

    for(size_t i=0; i<childs.size(); i++)
        childs.at(i)->getDurationsRange(min, max);
}

SegmentInformation * SegmentInformation::getChildByID(const ID &id)
{
    std::vector<SegmentInformation *>::const_iterator it;
    for(it=childs.begin(); it!=childs.end(); ++it)
    {
        if( (*it)->getID() == id )
            return *it;
    }
    return NULL;
}

void SegmentInformation::mergeWith(SegmentInformation *updated, mtime_t prunetime)
{
    /* Support Segment List for now */
    if(segmentList && updated->segmentList)
        segmentList->mergeWith(updated->segmentList);

    if(mediaSegmentTemplate && updated->mediaSegmentTemplate)
        mediaSegmentTemplate->mergeWith(updated->mediaSegmentTemplate, prunetime);

    std::vector<SegmentInformation *>::const_iterator it;
    for(it=childs.begin(); it!=childs.end(); ++it)
    {
        SegmentInformation *child = *it;
        SegmentInformation *updatedChild = updated->getChildByID(child->getID());
        if(updatedChild)
            child->mergeWith(updatedChild, prunetime);
    }
    /* FIXME: handle difference */
}

void SegmentInformation::pruneBySegmentNumber(uint64_t num)
{
    if(segmentList)
        segmentList->pruneBySegmentNumber(num);

    if(mediaSegmentTemplate && mediaSegmentTemplate->segmentTimeline.Get())
         mediaSegmentTemplate->segmentTimeline.Get()->pruneBySequenceNumber(num);

    for(size_t i=0; i<childs.size(); i++)
        childs.at(i)->pruneBySegmentNumber(num);
}

void SegmentInformation::runLocalUpdates(mtime_t, uint64_t)
{

}

SegmentInformation::SwitchPolicy SegmentInformation::getSwitchPolicy() const
{
    if(switchpolicy == SWITCH_UNKNOWN)
        return (parent) ? parent->getSwitchPolicy() : SWITCH_UNAVAILABLE;
    else
        return switchpolicy;
}

mtime_t SegmentInformation::getPeriodStart() const
{
    if(parent)
        return parent->getPeriodStart();
    else
        return 0;
}

void SegmentInformation::setSegmentList(SegmentList *list)
{
    if(segmentList)
    {
        segmentList->mergeWith(list);
        delete list;
    }
    else
    {
        segmentList = list;
    }
}

void SegmentInformation::setSegmentBase(SegmentBase *base)
{
    if(segmentBase)
        delete segmentBase;
    segmentBase = base;
}

void SegmentInformation::setSegmentTemplate(MediaSegmentTemplate *templ)
{
    if(mediaSegmentTemplate)
    {
        mediaSegmentTemplate->mergeWith(templ, 0);
        delete templ;
    }
    mediaSegmentTemplate = templ;
}

static void insertIntoSegment(std::vector<ISegment *> &seglist, size_t start,
                              size_t end, stime_t time)
{
    std::vector<ISegment *>::iterator segIt;
    for(segIt = seglist.begin(); segIt < seglist.end(); ++segIt)
    {
        ISegment *segment = *segIt;
        if(segment->getClassId() == Segment::CLASSID_SEGMENT &&
           segment->contains(end + segment->getOffset()))
        {
            SubSegment *subsegment = new SubSegment(segment,
                                                    start + segment->getOffset(),
                                                    (end != 0) ? end + segment->getOffset() : 0);
            subsegment->startTime.Set(time);
            segment->addSubSegment(subsegment);
            break;
        }
    }
}

void SegmentInformation::SplitUsingIndex(std::vector<SplitPoint> &splitlist)
{
    std::vector<ISegment *> seglist;
    getSegments(INFOTYPE_MEDIA, seglist);
    std::vector<SplitPoint>::const_iterator splitIt;
    size_t start = 0, end = 0;
    mtime_t time = 0;
    const uint64_t i_timescale = inheritTimescale();

    for(splitIt = splitlist.begin(); splitIt < splitlist.end(); ++splitIt)
    {
        start = end;
        SplitPoint split = *splitIt;
        end = split.offset;
        if(splitIt == splitlist.begin() && split.offset == 0)
            continue;
        time = split.time;
        insertIntoSegment(seglist, start, end - 1, time * i_timescale / CLOCK_FREQ);
    }

    if(start != 0)
    {
        start = end;
        end = 0;
        insertIntoSegment(seglist, start, end, time * i_timescale / CLOCK_FREQ);
    }
}

void SegmentInformation::setSwitchPolicy(SegmentInformation::SwitchPolicy policy)
{
    switchpolicy = policy;
}

Url SegmentInformation::getUrlSegment() const
{
    if(baseUrl.Get() && baseUrl.Get()->hasScheme())
    {
        return *(baseUrl.Get());
    }
    else
    {
        Url ret = getParentUrlSegment();
        if (baseUrl.Get())
            ret.append(*(baseUrl.Get()));
        return ret;
    }
}

SegmentBase * SegmentInformation::inheritSegmentBase() const
{
    if(segmentBase)
        return segmentBase;
    else if (parent)
        return parent->inheritSegmentBase();
    else
        return NULL;
}

SegmentList * SegmentInformation::inheritSegmentList() const
{
    if(segmentList)
        return segmentList;
    else if (parent)
        return parent->inheritSegmentList();
    else
        return NULL;
}

MediaSegmentTemplate * SegmentInformation::inheritSegmentTemplate() const
{
    if(mediaSegmentTemplate)
        return mediaSegmentTemplate;
    else if (parent)
        return parent->inheritSegmentTemplate();
    else
        return NULL;
}
<|endoftext|>


<commit_before><commit_msg>Perception: lint<commit_after><|endoftext|>


<commit_before>/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * 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
 */

#include "modules/planning/open_space/distance_approach_problem.h"

namespace apollo {
namespace planning {

using apollo::common::time::Clock;

DistanceApproachProblem::DistanceApproachProblem(
    Eigen::MatrixXd x0, Eigen::MatrixXd xF, Eigen::MatrixXd last_time_u,
    std::size_t horizon, float ts, Eigen::MatrixXd ego, Eigen::MatrixXd xWS,
    Eigen::MatrixXd uWS, Eigen::MatrixXd XYbounds, std::size_t obstacles_num,
    Eigen::MatrixXd obstacles_edges_num, Eigen::MatrixXd obstacles_A,
    Eigen::MatrixXd obstacles_b)
    : x0_(x0),
      xF_(xF),
      last_time_u_(last_time_u),
      horizon_(horizon),
      ts_(ts),
      ego_(ego),
      xWS_(xWS),
      uWS_(uWS),
      XYbounds_(XYbounds),
      obstacles_num_(obstacles_num),
      obstacles_edges_num_(obstacles_edges_num),
      obstacles_A_(obstacles_A),
      obstacles_b_(obstacles_b) {}

bool DistanceApproachProblem::Solve(Eigen::MatrixXd* state_result,
                                    Eigen::MatrixXd* control_result,
                                    Eigen::MatrixXd* time_result) {
  // TODO(QiL) : set up number of variables and number of constaints, and rego
  // so constants do not get set repeatedly

  // n1 : states variables, 4 * (N+1)
  int n1 = 4 * (horizon_ + 1);

  // n2 : control inputs variables
  int n2 = 2 * horizon_;

  // n3 : sampling time variables
  int n3 = horizon_ + 1;

  // n4 : dual multiplier associated with obstacleShape
  int n4 = obstacles_edges_num_.sum() * (horizon_ + 1);

  // n5 : dual multipier associated with car shape, obstacles_num*4 * (N+1)
  int n5 = obstacles_num_ * 4 * (horizon_ + 1);

  // m1 : dynamics constatins
  int m1 = 4 * horizon_;

  // m2 : control rate constraints (only steering)
  int m2 = horizon_;

  // m3 : sampling time equality constraints
  int m3 = horizon_;

  // m4 : obstacle constraints
  int m4 = 4 * obstacles_num_ * (horizon_ + 1);

  int num_of_variables = n1 + n2 + n3 + n4 + n5;
  int num_of_constraints = m1 + m2 + m3 + m4;

  // TODO(QiL) : evaluate whether need to new it everytime
  bool use_fix_time_ = false;

  DistanceApproachIPOPTInterface* ptop = new DistanceApproachIPOPTInterface(
      num_of_variables, num_of_constraints, horizon_, ts_, ego_, xWS_, uWS_,
      timeWS_, x0_, xF_, last_time_u_, XYbounds_, obstacles_edges_num_,
      obstacles_num_, obstacles_A_, obstacles_b_, use_fix_time_);

  Ipopt::SmartPtr<Ipopt::TNLP> problem = ptop;

  // Create an instance of the IpoptApplication
  Ipopt::SmartPtr<Ipopt::IpoptApplication> app = IpoptApplicationFactory();

  app->Options()->SetStringValue("hessian_approximation", "limited-memory");
  // app->Options()->SetStringValue("jacobian_approximation",
  // "finite-difference-values");
  // app->Options()->SetStringValue("derivative_test", "first-order");
  // app->Options()->SetNumericValue("derivative_test_tol", 1.0e-3);
  // TODO(QiL) : Change IPOPT settings to flag or configs
  app->Options()->SetIntegerValue("print_level", 5);
  app->Options()->SetIntegerValue("mumps_mem_percent", 6000);
    app->Options()->SetNumericValue("mumps_pivotol", 1e-6);
  app->Options()->SetIntegerValue("max_iter", 1000);
  app->Options()->SetNumericValue("tol", 1e-4);
  app->Options()->SetNumericValue("min_hessian_perturbation", 1e-12);
  app->Options()->SetNumericValue("jacobian_regularization_value", 1e-7);

  Ipopt::ApplicationReturnStatus status = app->Initialize();
  if (status != Ipopt::Solve_Succeeded) {
    AINFO << "*** Distiance Approach problem error during initialization!";
    return false;
  }

  status = app->OptimizeTNLP(problem);

  if (status == Ipopt::Solve_Succeeded ||
      status == Ipopt::Solved_To_Acceptable_Level) {
    // Retrieve some statistics about the solve
    Ipopt::Index iter_count = app->Statistics()->IterationCount();
    AINFO << "*** The problem solved in " << iter_count << " iterations!";

    Ipopt::Number final_obj = app->Statistics()->FinalObjective();
    AINFO << "*** The final value of the objective function is " << final_obj
          << '.';
  } else {
    AINFO << "Return status: " << int(status);
  }

  ptop->get_optimization_results(state_result, control_result, time_result);

  return status == Ipopt::Solve_Succeeded ||
         status == Ipopt::Solved_To_Acceptable_Level;
}

}  // namespace planning
}  // namespace apollo
<commit_msg>Planning : add caculation log time in distance approach problem<commit_after>/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * 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
 */

#include "modules/planning/open_space/distance_approach_problem.h"

namespace apollo {
namespace planning {

using apollo::common::time::Clock;

DistanceApproachProblem::DistanceApproachProblem(
    Eigen::MatrixXd x0, Eigen::MatrixXd xF, Eigen::MatrixXd last_time_u,
    std::size_t horizon, float ts, Eigen::MatrixXd ego, Eigen::MatrixXd xWS,
    Eigen::MatrixXd uWS, Eigen::MatrixXd XYbounds, std::size_t obstacles_num,
    Eigen::MatrixXd obstacles_edges_num, Eigen::MatrixXd obstacles_A,
    Eigen::MatrixXd obstacles_b)
    : x0_(x0),
      xF_(xF),
      last_time_u_(last_time_u),
      horizon_(horizon),
      ts_(ts),
      ego_(ego),
      xWS_(xWS),
      uWS_(uWS),
      XYbounds_(XYbounds),
      obstacles_num_(obstacles_num),
      obstacles_edges_num_(obstacles_edges_num),
      obstacles_A_(obstacles_A),
      obstacles_b_(obstacles_b) {}

bool DistanceApproachProblem::Solve(Eigen::MatrixXd* state_result,
                                    Eigen::MatrixXd* control_result,
                                    Eigen::MatrixXd* time_result) {
  // TODO(QiL) : set up number of variables and number of constaints, and rego
  // so constants do not get set repeatedly

  // n1 : states variables, 4 * (N+1)
  int n1 = 4 * (horizon_ + 1);

  // n2 : control inputs variables
  int n2 = 2 * horizon_;

  // n3 : sampling time variables
  int n3 = horizon_ + 1;

  // n4 : dual multiplier associated with obstacleShape
  int n4 = obstacles_edges_num_.sum() * (horizon_ + 1);

  // n5 : dual multipier associated with car shape, obstacles_num*4 * (N+1)
  int n5 = obstacles_num_ * 4 * (horizon_ + 1);

  // m1 : dynamics constatins
  int m1 = 4 * horizon_;

  // m2 : control rate constraints (only steering)
  int m2 = horizon_;

  // m3 : sampling time equality constraints
  int m3 = horizon_;

  // m4 : obstacle constraints
  int m4 = 4 * obstacles_num_ * (horizon_ + 1);

  int num_of_variables = n1 + n2 + n3 + n4 + n5;
  int num_of_constraints = m1 + m2 + m3 + m4;

  // TODO(QiL) : evaluate whether need to new it everytime
  bool use_fix_time_ = false;

  auto t_start = cyber::Time::Now().ToSecond();
  DistanceApproachIPOPTInterface* ptop = new DistanceApproachIPOPTInterface(
      num_of_variables, num_of_constraints, horizon_, ts_, ego_, xWS_, uWS_,
      timeWS_, x0_, xF_, last_time_u_, XYbounds_, obstacles_edges_num_,
      obstacles_num_, obstacles_A_, obstacles_b_, use_fix_time_);

  Ipopt::SmartPtr<Ipopt::TNLP> problem = ptop;

  // Create an instance of the IpoptApplication
  Ipopt::SmartPtr<Ipopt::IpoptApplication> app = IpoptApplicationFactory();

  app->Options()->SetStringValue("hessian_approximation", "limited-memory");
  // app->Options()->SetStringValue("jacobian_approximation",
  // "finite-difference-values");
  // app->Options()->SetStringValue("derivative_test", "first-order");
  // app->Options()->SetNumericValue("derivative_test_tol", 1.0e-3);
  // TODO(QiL) : Change IPOPT settings to flag or configs
  app->Options()->SetIntegerValue("print_level", 0);
  app->Options()->SetIntegerValue("mumps_mem_percent", 6000);
  app->Options()->SetNumericValue("mumps_pivotol", 1e-6);
  app->Options()->SetIntegerValue("max_iter", 1000);
  app->Options()->SetNumericValue("tol", 1e-4);
  app->Options()->SetNumericValue("min_hessian_perturbation", 1e-12);
  app->Options()->SetNumericValue("jacobian_regularization_value", 1e-7);

  Ipopt::ApplicationReturnStatus status = app->Initialize();
  if (status != Ipopt::Solve_Succeeded) {
    AINFO << "*** Distiance Approach problem error during initialization!";
    return false;
  }

  status = app->OptimizeTNLP(problem);

  if (status == Ipopt::Solve_Succeeded ||
      status == Ipopt::Solved_To_Acceptable_Level) {
    // Retrieve some statistics about the solve
    Ipopt::Index iter_count = app->Statistics()->IterationCount();
    AINFO << "*** The problem solved in " << iter_count << " iterations!";

    Ipopt::Number final_obj = app->Statistics()->FinalObjective();
    AINFO << "*** The final value of the objective function is " << final_obj
          << '.';

    auto t_end = cyber::Time::Now().ToSecond();

    AINFO << "DistanceApproachProblem solving time in second : "
          << t_end - t_start;
  } else {
    AINFO << "Return status: " << int(status);
  }

  ptop->get_optimization_results(state_result, control_result, time_result);

  return status == Ipopt::Solve_Succeeded ||
         status == Ipopt::Solved_To_Acceptable_Level;
}

}  // namespace planning
}  // namespace apollo
<|endoftext|>


<commit_before>/******************************************************************************
*       SOFA, Simulation Open-Framework Architecture, version 1.0 RC 1        *
*                (c) 2006-2011 MGH, INRIA, USTL, UJF, CNRS                    *
*                                                                             *
* This library is free software; you can redistribute it and/or modify it     *
* under the terms of the GNU Lesser General Public License as published by    *
* the Free Software Foundation; either version 2.1 of the License, or (at     *
* your option) any later version.                                             *
*                                                                             *
* 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., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA.          *
*******************************************************************************
*                               SOFA :: Modules                               *
*                                                                             *
* Authors: The SOFA Team and external contributors (see Authors.txt)          *
*                                                                             *
* Contact information: contact@sofa-framework.org                             *
******************************************************************************/
#ifndef SOFA_COMPONENT_FORCEFIELD_CONSTANTFORCEFIELD_INL
#define SOFA_COMPONENT_FORCEFIELD_CONSTANTFORCEFIELD_INL

#include <sofa/component/forcefield/ConstantForceField.h>
#include <sofa/helper/system/config.h>
#include <sofa/helper/gl/template.h>
#include <assert.h>
#include <iostream>
//#include <sofa/helper/gl/BasicShapes.h>
#include <sofa/simulation/common/Simulation.h>
#include <sofa/core/behavior/ForceField.inl>
#include <sofa/core/visual/VisualParams.h>
#include <sofa/component/topology/TopologySubsetData.inl>

namespace sofa
{

namespace component
{

namespace forcefield
{

template<class DataTypes>
ConstantForceField<DataTypes>::ConstantForceField()
    : points(initData(&points, "points", "points where the forces are applied"))
    , forces(initData(&forces, "forces", "applied forces at each point"))
    , force(initData(&force, "force", "applied force to all points if forces attribute is not specified"))
    , totalForce(initData(&totalForce, "totalForce", "total force for all points, will be distributed uniformly over points"))
    , arrowSizeCoef(initData(&arrowSizeCoef,0.0, "arrowSizeCoef", "Size of the drawn arrows (0->no arrows, sign->direction of drawing"))
    , d_color(initData(&d_color, defaulttype::Vec4f(0.2f,0.9f,0.3f,1.0f), "showColor", "Color for object display"))
    , indexFromEnd(initData(&indexFromEnd,(bool)false,"indexFromEnd", "Concerned DOFs indices are numbered from the end of the MState DOFs vector"))
{
    arrowSizeCoef.setGroup("Visualization");
    d_color.setGroup("Visualization");

}

template<class DataTypes>
void ConstantForceField<DataTypes>::init()
{
    topology = this->getContext()->getMeshTopology();

    // Initialize functions and parameters for topology data and handler
    points.createTopologicalEngine(topology);
    points.registerTopologicalData();

    Inherit::init();
}

template<class DataTypes>
void ConstantForceField<DataTypes>::addForce(const core::MechanicalParams* /*params*/ /* PARAMS FIRST */, DataVecDeriv& f1, const DataVecCoord& p1, const DataVecDeriv&)
{
    sofa::helper::WriteAccessor< core::objectmodel::Data< VecDeriv > > _f1 = f1;
    _f1.resize(p1.getValue().size());

    Deriv singleForce;
    const Deriv& forceVal = force.getValue();
    const Deriv& totalForceVal = totalForce.getValue();
    const VecIndex& indices = points.getValue();
    const VecDeriv& f = forces.getValue();
    unsigned int i = 0, nbForcesIn = f.size(), nbForcesOut = _f1.size();

    if (totalForceVal * totalForceVal > 0)
    {
        unsigned int nbForces = indices.empty() ? nbForcesOut : indices.size();
        singleForce = totalForceVal / (Real)nbForces;
    }
    else if (forceVal * forceVal > 0.0)
        singleForce = forceVal;

    const Deriv f_end = (f.empty() ? singleForce : f.back());

    // When no indices are given, copy the forces from the start
    if (indices.empty())
    {
        unsigned int nbCopy = std::min(nbForcesIn, nbForcesOut);
        for (; i < nbCopy; ++i) // Copy from the forces list
            _f1[i] += f[i];
        for (; i < nbForcesOut; ++i) // Copy from the single value or the last value of the forces list
            _f1[i] += f_end;
    }
    else
    {
        unsigned int nbIndices = indices.size();
        unsigned int nbCopy = std::min(nbForcesIn, nbIndices); // forces & points are not garanteed to be of the same size
        if (!indexFromEnd.getValue())
        {
            for (; i < nbCopy; ++i)
                _f1[indices[i]] += f[i];
            for (; i < nbIndices; ++i)
                _f1[indices[i]] += f_end;
        }
        else
        {
            for (; i < nbCopy; ++i)
                _f1[nbForcesOut - indices[i] - 1] += f[i];
            for (; i < nbIndices; ++i)
                _f1[nbForcesOut - indices[i] - 1] += f_end;
        }
    }
}

template<class DataTypes>
void ConstantForceField<DataTypes>::addKToMatrix(sofa::defaulttype::BaseMatrix * /* mat */, SReal /* k */, unsigned int & /* offset */)
{
}

template <class DataTypes>
double ConstantForceField<DataTypes>::getPotentialEnergy(const core::MechanicalParams* /*params*/ /* PARAMS FIRST */, const DataVecCoord& x) const
{
    const VecIndex& indices = points.getValue();
    const VecDeriv& f = forces.getValue();
    const VecCoord& _x = x.getValue();
    const Deriv f_end = (f.empty()? force.getValue() : f[f.size()-1]);
    double e = 0;
    unsigned int i = 0;

    if (!indexFromEnd.getValue())
    {
        for (; i<f.size(); i++)
        {
            e -= f[i] * _x[indices[i]];
        }
        for (; i<indices.size(); i++)
        {
            e -= f_end * _x[indices[i]];
        }
    }
    else
    {
        for (; i < f.size(); i++)
        {
            e -= f[i] * _x[_x.size() - indices[i] -1];
        }
        for (; i < indices.size(); i++)
        {
            e -= f_end * _x[_x.size() - indices[i] -1];
        }
    }

    return e;
}

template <class DataTypes>
void ConstantForceField<DataTypes>::setForce(unsigned i, const Deriv& force)
{
    VecIndex& indices = *points.beginEdit();
    VecDeriv& f = *forces.beginEdit();
    indices.push_back(i);
    f.push_back( force );
    points.endEdit();
    forces.endEdit();
}

template<class DataTypes>
void ConstantForceField<DataTypes>::draw(const core::visual::VisualParams* vparams)
{
#ifndef SOFA_NO_OPENGL
    double aSC = arrowSizeCoef.getValue();

    Deriv singleForce;
    if (totalForce.getValue()*totalForce.getValue() > 0.0)
    {
        for (unsigned comp = 0; comp < totalForce.getValue().size(); comp++)
            singleForce[comp] = (totalForce.getValue()[comp])/(Real(points.getValue().size()));
    }
    else if (force.getValue() * force.getValue() > 0.0)
    {
        singleForce = force.getValue();
    }

    if ((!vparams->displayFlags().getShowForceFields() && (aSC==0)) || (aSC < 0.0)) return;
    const VecIndex& indices = points.getValue();
    const VecDeriv& f = forces.getValue();
    const Deriv f_end = (f.empty()? singleForce : f[f.size()-1]);
    const VecCoord& x = *this->mstate->getX();

    if( fabs(aSC)<1.0e-10 )
    {
        std::vector<defaulttype::Vector3> points;
        for (unsigned int i=0; i<indices.size(); i++)
        {
            Real xx,xy,xz,fx,fy,fz;

            if (!indexFromEnd.getValue())
            {
                DataTypes::get(xx,xy,xz,x[indices[i]]);
            }
            else
            {
                DataTypes::get(xx,xy,xz,x[x.size() - indices[i] - 1]);
            }

            DataTypes::get(fx,fy,fz,(i<f.size())? f[i] : f_end);
            points.push_back(defaulttype::Vector3(xx, xy, xz ));
            points.push_back(defaulttype::Vector3(xx+fx, xy+fy, xz+fz ));
        }
        vparams->drawTool()->drawLines(points, 2, defaulttype::Vec<4,float>(0,1,0,1));
    }
    else
    {
        glPushAttrib(GL_LIGHTING_BIT);
        glEnable(GL_LIGHTING);
        for (unsigned int i=0; i<indices.size(); i++)
        {
            Real xx,xy,xz,fx,fy,fz;

            if (!indexFromEnd.getValue())
            {
                DataTypes::get(xx,xy,xz,x[indices[i]]);
            }
            else
            {
                DataTypes::get(xx,xy,xz,x[x.size() - indices[i] - 1]);
            }

            DataTypes::get(fx,fy,fz,(i<f.size())? f[i] : f_end);

            defaulttype::Vector3 p1( xx, xy, xz);
            defaulttype::Vector3 p2( aSC*fx+xx, aSC*fy+xy, aSC*fz+xz );

            float norm = (float)(p2-p1).norm();

            if( aSC > 0)
            {
                //helper::gl::drawArrow(p1,p2, norm/20.0);
                vparams->drawTool()->drawArrow(p1,p2, norm/20.0f, d_color.getValue());
            }
            else
            {
                //helper::gl::drawArrow(p2,p1, norm/20.0);
                vparams->drawTool()->drawArrow(p2,p1, norm/20.0f, d_color.getValue());
            }
        }
        glPopAttrib();
    }

#endif /* SOFA_NO_OPENGL */
}

} // namespace forcefield

} // namespace component

} // namespace sofa

#endif // SOFA_COMPONENT_FORCEFIELD_CONSTANTFORCEFIELD_INL

<commit_msg>Fix cuda compilation for ConstantForceField<commit_after>/******************************************************************************
*       SOFA, Simulation Open-Framework Architecture, version 1.0 RC 1        *
*                (c) 2006-2011 MGH, INRIA, USTL, UJF, CNRS                    *
*                                                                             *
* This library is free software; you can redistribute it and/or modify it     *
* under the terms of the GNU Lesser General Public License as published by    *
* the Free Software Foundation; either version 2.1 of the License, or (at     *
* your option) any later version.                                             *
*                                                                             *
* 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., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA.          *
*******************************************************************************
*                               SOFA :: Modules                               *
*                                                                             *
* Authors: The SOFA Team and external contributors (see Authors.txt)          *
*                                                                             *
* Contact information: contact@sofa-framework.org                             *
******************************************************************************/
#ifndef SOFA_COMPONENT_FORCEFIELD_CONSTANTFORCEFIELD_INL
#define SOFA_COMPONENT_FORCEFIELD_CONSTANTFORCEFIELD_INL

#include <sofa/component/forcefield/ConstantForceField.h>
#include <sofa/helper/system/config.h>
#include <sofa/helper/gl/template.h>
#include <assert.h>
#include <iostream>
//#include <sofa/helper/gl/BasicShapes.h>
#include <sofa/simulation/common/Simulation.h>
#include <sofa/core/behavior/ForceField.inl>
#include <sofa/core/visual/VisualParams.h>
#include <sofa/component/topology/TopologySubsetData.inl>

namespace sofa
{

namespace component
{

namespace forcefield
{

template<class DataTypes>
ConstantForceField<DataTypes>::ConstantForceField()
    : points(initData(&points, "points", "points where the forces are applied"))
    , forces(initData(&forces, "forces", "applied forces at each point"))
    , force(initData(&force, "force", "applied force to all points if forces attribute is not specified"))
    , totalForce(initData(&totalForce, "totalForce", "total force for all points, will be distributed uniformly over points"))
    , arrowSizeCoef(initData(&arrowSizeCoef,0.0, "arrowSizeCoef", "Size of the drawn arrows (0->no arrows, sign->direction of drawing"))
    , d_color(initData(&d_color, defaulttype::Vec4f(0.2f,0.9f,0.3f,1.0f), "showColor", "Color for object display"))
    , indexFromEnd(initData(&indexFromEnd,(bool)false,"indexFromEnd", "Concerned DOFs indices are numbered from the end of the MState DOFs vector"))
{
    arrowSizeCoef.setGroup("Visualization");
    d_color.setGroup("Visualization");

}

template<class DataTypes>
void ConstantForceField<DataTypes>::init()
{
    topology = this->getContext()->getMeshTopology();

    // Initialize functions and parameters for topology data and handler
    points.createTopologicalEngine(topology);
    points.registerTopologicalData();

    Inherit::init();
}

template<class DataTypes>
void ConstantForceField<DataTypes>::addForce(const core::MechanicalParams* /*params*/ /* PARAMS FIRST */, DataVecDeriv& f1, const DataVecCoord& p1, const DataVecDeriv&)
{
    sofa::helper::WriteAccessor< core::objectmodel::Data< VecDeriv > > _f1 = f1;
    _f1.resize(p1.getValue().size());

    Deriv singleForce;
    const Deriv& forceVal = force.getValue();
    const Deriv& totalForceVal = totalForce.getValue();
    const VecIndex& indices = points.getValue();
    const VecDeriv& f = forces.getValue();
    unsigned int i = 0, nbForcesIn = f.size(), nbForcesOut = _f1.size();

    if (totalForceVal * totalForceVal > 0)
    {
        unsigned int nbForces = indices.empty() ? nbForcesOut : indices.size();
        singleForce = totalForceVal / (Real)nbForces;
    }
    else if (forceVal * forceVal > 0.0)
        singleForce = forceVal;

    const Deriv f_end = (f.empty() ? singleForce : f[f.size()-1]);

    // When no indices are given, copy the forces from the start
    if (indices.empty())
    {
        unsigned int nbCopy = std::min(nbForcesIn, nbForcesOut);
        for (; i < nbCopy; ++i) // Copy from the forces list
            _f1[i] += f[i];
        for (; i < nbForcesOut; ++i) // Copy from the single value or the last value of the forces list
            _f1[i] += f_end;
    }
    else
    {
        unsigned int nbIndices = indices.size();
        unsigned int nbCopy = std::min(nbForcesIn, nbIndices); // forces & points are not garanteed to be of the same size
        if (!indexFromEnd.getValue())
        {
            for (; i < nbCopy; ++i)
                _f1[indices[i]] += f[i];
            for (; i < nbIndices; ++i)
                _f1[indices[i]] += f_end;
        }
        else
        {
            for (; i < nbCopy; ++i)
                _f1[nbForcesOut - indices[i] - 1] += f[i];
            for (; i < nbIndices; ++i)
                _f1[nbForcesOut - indices[i] - 1] += f_end;
        }
    }
}

template<class DataTypes>
void ConstantForceField<DataTypes>::addKToMatrix(sofa::defaulttype::BaseMatrix * /* mat */, SReal /* k */, unsigned int & /* offset */)
{
}

template <class DataTypes>
double ConstantForceField<DataTypes>::getPotentialEnergy(const core::MechanicalParams* /*params*/ /* PARAMS FIRST */, const DataVecCoord& x) const
{
    const VecIndex& indices = points.getValue();
    const VecDeriv& f = forces.getValue();
    const VecCoord& _x = x.getValue();
    const Deriv f_end = (f.empty()? force.getValue() : f[f.size()-1]);
    double e = 0;
    unsigned int i = 0;

    if (!indexFromEnd.getValue())
    {
        for (; i<f.size(); i++)
        {
            e -= f[i] * _x[indices[i]];
        }
        for (; i<indices.size(); i++)
        {
            e -= f_end * _x[indices[i]];
        }
    }
    else
    {
        for (; i < f.size(); i++)
        {
            e -= f[i] * _x[_x.size() - indices[i] -1];
        }
        for (; i < indices.size(); i++)
        {
            e -= f_end * _x[_x.size() - indices[i] -1];
        }
    }

    return e;
}

template <class DataTypes>
void ConstantForceField<DataTypes>::setForce(unsigned i, const Deriv& force)
{
    VecIndex& indices = *points.beginEdit();
    VecDeriv& f = *forces.beginEdit();
    indices.push_back(i);
    f.push_back( force );
    points.endEdit();
    forces.endEdit();
}

template<class DataTypes>
void ConstantForceField<DataTypes>::draw(const core::visual::VisualParams* vparams)
{
#ifndef SOFA_NO_OPENGL
    double aSC = arrowSizeCoef.getValue();

    Deriv singleForce;
    if (totalForce.getValue()*totalForce.getValue() > 0.0)
    {
        for (unsigned comp = 0; comp < totalForce.getValue().size(); comp++)
            singleForce[comp] = (totalForce.getValue()[comp])/(Real(points.getValue().size()));
    }
    else if (force.getValue() * force.getValue() > 0.0)
    {
        singleForce = force.getValue();
    }

    if ((!vparams->displayFlags().getShowForceFields() && (aSC==0)) || (aSC < 0.0)) return;
    const VecIndex& indices = points.getValue();
    const VecDeriv& f = forces.getValue();
    const Deriv f_end = (f.empty()? singleForce : f[f.size()-1]);
    const VecCoord& x = *this->mstate->getX();

    if( fabs(aSC)<1.0e-10 )
    {
        std::vector<defaulttype::Vector3> points;
        for (unsigned int i=0; i<indices.size(); i++)
        {
            Real xx,xy,xz,fx,fy,fz;

            if (!indexFromEnd.getValue())
            {
                DataTypes::get(xx,xy,xz,x[indices[i]]);
            }
            else
            {
                DataTypes::get(xx,xy,xz,x[x.size() - indices[i] - 1]);
            }

            DataTypes::get(fx,fy,fz,(i<f.size())? f[i] : f_end);
            points.push_back(defaulttype::Vector3(xx, xy, xz ));
            points.push_back(defaulttype::Vector3(xx+fx, xy+fy, xz+fz ));
        }
        vparams->drawTool()->drawLines(points, 2, defaulttype::Vec<4,float>(0,1,0,1));
    }
    else
    {
        glPushAttrib(GL_LIGHTING_BIT);
        glEnable(GL_LIGHTING);
        for (unsigned int i=0; i<indices.size(); i++)
        {
            Real xx,xy,xz,fx,fy,fz;

            if (!indexFromEnd.getValue())
            {
                DataTypes::get(xx,xy,xz,x[indices[i]]);
            }
            else
            {
                DataTypes::get(xx,xy,xz,x[x.size() - indices[i] - 1]);
            }

            DataTypes::get(fx,fy,fz,(i<f.size())? f[i] : f_end);

            defaulttype::Vector3 p1( xx, xy, xz);
            defaulttype::Vector3 p2( aSC*fx+xx, aSC*fy+xy, aSC*fz+xz );

            float norm = (float)(p2-p1).norm();

            if( aSC > 0)
            {
                //helper::gl::drawArrow(p1,p2, norm/20.0);
                vparams->drawTool()->drawArrow(p1,p2, norm/20.0f, d_color.getValue());
            }
            else
            {
                //helper::gl::drawArrow(p2,p1, norm/20.0);
                vparams->drawTool()->drawArrow(p2,p1, norm/20.0f, d_color.getValue());
            }
        }
        glPopAttrib();
    }

#endif /* SOFA_NO_OPENGL */
}

} // namespace forcefield

} // namespace component

} // namespace sofa

#endif // SOFA_COMPONENT_FORCEFIELD_CONSTANTFORCEFIELD_INL

<|endoftext|>