<commit_before>{

  cout << "Loading libraries ..." << endl;
  gROOT->LoadMacro("${ALICE_ROOT}/MUON/loadlibs.C");
  gInterpreter->ProcessLine("loadlibs()");

  TString includePath = "-I${ROOTSYS}/include ";
  includePath        += "-I${ALICE_ROOT}/STEER/STEER ";
  includePath        += "-I${ALICE_ROOT}/STEER/STEERBase ";
  includePath        += "-I${ALICE_BUILD}/include ";
  includePath        += "-I${ALICE_ROOT}/RAW ";
  includePath        += "-I${ALICE_ROOT}/FASTSIM ";
  includePath        += "-I${ALICE_ROOT}/EVGEN ";
  includePath        += "-I${ALICE_ROOT}/SHUTTLE/TestShuttle ";
  includePath        += "-I${ALICE_ROOT}/ITS ";
  includePath        += "-I${ALICE_ROOT}/MUON ";
  includePath        += "-I${ALICE_ROOT}/MFT ";
  includePath        += "-I${ALICE_ROOT}/MUON/mapping ";
  includePath        += "-I${ALICE_ROOT}/RAW ";
  includePath        += "-I${ALICE_ROOT}/CORRFW ";
  includePath        += "-I${ROOTSYS}/net/alien/inc ";
  gSystem->SetIncludePath(includePath.Data());

  gSystem->Load("libCore.so");
  gSystem->Load("libTree.so");
  gSystem->Load("libGeom.so");
  gSystem->Load("libVMC.so");
  gSystem->Load("libPhysics.so");
  gSystem->Load("libSTEERBase");
  gSystem->Load("libESD");
  gSystem->Load("libAOD");
  gSystem->Load("libANALYSIS") ;
  gSystem->Load("libANALYSISalice") ;
  gSystem->Load("libCORRFW") ;
  gSystem->Load("libMFTbase") ;
  gSystem->Load("libMFTsim") ;
  gSystem->Load("libMFTrec") ;

  //================= define the style =================== //

  // divisions: axis marks outside the figure (minus sign)
  gStyle->SetTickLength(-0.03,"X");  gStyle->SetTickLength(-0.03,"Y");
  gStyle->SetNdivisions(508,"X");    gStyle->SetNdivisions(506,"Y"); // set ndvx (y)

  // dimensions and position of fonts connected to axes
  gStyle->SetLabelSize(0.05,"X");    gStyle->SetLabelSize(0.05,"Y");
  gStyle->SetLabelOffset(0.02,"X");  gStyle->SetLabelOffset(0.04,"Y");

  // dimension of the canvas and of the figure (pad) inside it
  gStyle->SetCanvasDefW(600);        gStyle->SetCanvasDefH(600);
  gStyle->SetPadBottomMargin(0.16);  gStyle->SetPadLeftMargin(0.18);
  gStyle->SetPadTopMargin(0.08);     gStyle->SetPadRightMargin(0.06);
  gStyle->SetTitleXSize(0.05);       gStyle->SetTitleYSize(0.05);
  gStyle->SetTitleXOffset(1.5);      gStyle->SetTitleYOffset(1.9);
  gStyle->SetStatW(0.16);            gStyle->SetStatH(0.16);
  gStyle->SetFitFormat("9.6g");
  gStyle->SetCanvasColor(0);
  gStyle->SetCanvasBorderMode(0);
  gStyle->SetPadBorderMode(0);
  gStyle->SetOptTitle(1);
  gStyle->SetOptStat(1110);
  gStyle->SetOptFit(1);

  gStyle->SetPalette(1,0);  // rainbow colors
  gStyle->SetHistLineWidth(2);

  gStyle->SetFrameBorderMode(0);
  gROOT->ForceStyle();

}

<commit_msg>rootlogon.C updated<commit_after>{

  TString includePath = "-I${ROOTSYS}/include ";

  printf("\nLoading ALICE settings...\n\n");
  includePath        += "-I${ALICE_ROOT}/STEER ";
  includePath        += "-I${ALICE_ROOT}/STEER/STEERBase ";
  includePath        += "-I${ALICE_BUILD}/include ";
  includePath        += "-I${ALICE_ROOT}/RAW ";
  includePath        += "-I${ALICE_ROOT}/FASTSIM ";
  includePath        += "-I${ALICE_ROOT}/EVGEN ";
  includePath        += "-I${ALICE_ROOT}/SHUTTLE/TestShuttle ";
  includePath        += "-I${ALICE_ROOT}/ITS ";
  includePath        += "-I${ALICE_ROOT}/MFT ";
  includePath        += "-I${ALICE_ROOT}/MUON ";
  includePath        += "-I${ALICE_ROOT}/MUON/mapping ";
  includePath        += "-I${ALICE_ROOT}/RAW ";
  includePath        += "-I${ALICE_ROOT}/CORRFW ";
  includePath        += "-I${ROOTSYS}/net/alien/inc ";

  gSystem->SetIncludePath(includePath.Data());

  gSystem->Load("libCore.so");
  gSystem->Load("libTree.so");
  gSystem->Load("libGeom.so");
  gSystem->Load("libVMC.so");
  gSystem->Load("libPhysics.so");
  gSystem->Load("libSTEERBase");
  gSystem->Load("libESD");
  gSystem->Load("libAOD");
  gSystem->Load("libANALYSIS") ;
  gSystem->Load("libANALYSISalice") ;
  gSystem->Load("libCORRFW") ;
  gSystem->Load("libMFTbase") ;
  gSystem->Load("libMFTsim") ;
  gSystem->Load("libMFTrec") ;

  //================= define the style =================== //

  // divisions: axis marks outside the figure (minus sign)
  gStyle->SetTickLength(-0.03,"X");  gStyle->SetTickLength(-0.03,"Y");
  gStyle->SetNdivisions(508,"X");    gStyle->SetNdivisions(506,"Y"); // set ndvx (y)

  // dimensions and position of fonts connected to axes
  gStyle->SetLabelSize(0.05,"X");    gStyle->SetLabelSize(0.05,"Y");
  gStyle->SetLabelOffset(0.02,"X");  gStyle->SetLabelOffset(0.04,"Y");

  // dimension of the canvas and of the figure (pad) inside it
  gStyle->SetCanvasDefW(600);        gStyle->SetCanvasDefH(600);
  gStyle->SetPadBottomMargin(0.16);  gStyle->SetPadLeftMargin(0.18);
  gStyle->SetPadTopMargin(0.08);     gStyle->SetPadRightMargin(0.06);
  gStyle->SetTitleXSize(0.05);       gStyle->SetTitleYSize(0.05);
  gStyle->SetTitleXOffset(1.5);      gStyle->SetTitleYOffset(1.9);
  gStyle->SetStatW(0.16);            gStyle->SetStatH(0.16);
  gStyle->SetFitFormat("9.6g");
  gStyle->SetCanvasColor(0);
  gStyle->SetCanvasBorderMode(0);
  gStyle->SetPadBorderMode(0);
  gStyle->SetOptTitle(1);
  gStyle->SetOptStat(1110);
  gStyle->SetOptFit(1);

  gStyle->SetPalette(1,0);  // rainbow colors
  gStyle->SetHistLineWidth(2);

  gStyle->SetFrameBorderMode(0);
  gROOT->ForceStyle();

}

<|endoftext|>


<commit_before>#include "Enemies.hpp"
#include <Plinth/Generic.hpp>

Enemies::Enemies(sf::RenderWindow& window)
	: m_speedIncreaseMultiplier(0.9)
	, m_dropSpeedIncreaseMultiplier(1.1)
	, m_speed(40.0)
	, m_dropSpeed(0.05)
	, m_enemies()
	, m_view(window.getView())
	, m_reachedBottom(false)
{
	priv_addEnemies();
}

void Enemies::reset()
{
	m_speed = 40.0;
	m_dropSpeed = 0.05;
	m_reachedBottom = false;
	m_enemies.clear();
	priv_addEnemies();
}

void Enemies::update(double dt)
{
	if (m_reachedBottom)
		return;

	bool requiresDirectionFlipping = false;
	for (auto& enemy : m_enemies)
	{
		if (!enemy.isAlive())
			continue;

		if (enemy.isMovingRight())
			enemy.move({ m_speed * dt, m_speed * m_dropSpeed * dt });
		else
			enemy.move({ -m_speed * dt, m_speed * m_dropSpeed * dt });
		if (enemy.requiresFlipping())
			requiresDirectionFlipping = true;
		if (enemy.reachedBottom())
			m_reachedBottom = true;
	}
	if (requiresDirectionFlipping)
		toggleDirection();
}

void Enemies::killEnemy(const unsigned int enemyIndex)
{
	if (enemyIndex < m_enemies.size() && m_enemies[enemyIndex].isAlive())
	{
		m_enemies[enemyIndex].die();

		// adjust speed following a kill
		if (getNumberOfEnemiesAlive() > 0)
			m_speed *= m_speedIncreaseMultiplier / getNumberOfEnemiesAlive() + 1;
	}
}

void Enemies::toggleDirection()
{
	for (auto& enemy : m_enemies)
		enemy.flipDirection();
	m_dropSpeed *= m_dropSpeedIncreaseMultiplier;
}

unsigned int Enemies::getNumberOfEnemiesAlive() const
{
	unsigned int total{ 0u };
	for (auto& enemy : m_enemies)
	{
		if (enemy.isAlive())
			++total;
	}
	return total;
}

bool Enemies::getReachedBottom() const
{
	return m_reachedBottom;
}

void Enemies::priv_addEnemies()
{
	const unsigned int numberOfEnemies = 40u;
	const unsigned int enemiesPerRow = 8u;
	const double horizontalSpacing = 65.0;
	const double verticalSpacing = 65.0;
	const pl::Vector2d positionOfTopLeft = { 120.0, 180.0 };
	for (unsigned int i = 0; i < numberOfEnemies; ++i)
		m_enemies.emplace_back(Enemy(m_view, positionOfTopLeft + pl::Vector2d{ horizontalSpacing * (i % enemiesPerRow), verticalSpacing * (i / enemiesPerRow) }));
}
<commit_msg>correct Enemy initial position<commit_after>#include "Enemies.hpp"
#include <Plinth/Generic.hpp>

Enemies::Enemies(sf::RenderWindow& window)
	: m_speedIncreaseMultiplier(0.9)
	, m_dropSpeedIncreaseMultiplier(1.1)
	, m_speed(40.0)
	, m_dropSpeed(0.05)
	, m_enemies()
	, m_view(window.getView())
	, m_reachedBottom(false)
{
	priv_addEnemies();
}

void Enemies::reset()
{
	m_speed = 40.0;
	m_dropSpeed = 0.05;
	m_reachedBottom = false;
	m_enemies.clear();
	priv_addEnemies();
}

void Enemies::update(double dt)
{
	if (m_reachedBottom)
		return;

	bool requiresDirectionFlipping = false;
	for (auto& enemy : m_enemies)
	{
		if (!enemy.isAlive())
			continue;

		if (enemy.isMovingRight())
			enemy.move({ m_speed * dt, m_speed * m_dropSpeed * dt });
		else
			enemy.move({ -m_speed * dt, m_speed * m_dropSpeed * dt });
		if (enemy.requiresFlipping())
			requiresDirectionFlipping = true;
		if (enemy.reachedBottom())
			m_reachedBottom = true;
	}
	if (requiresDirectionFlipping)
		toggleDirection();
}

void Enemies::killEnemy(const unsigned int enemyIndex)
{
	if (enemyIndex < m_enemies.size() && m_enemies[enemyIndex].isAlive())
	{
		m_enemies[enemyIndex].die();

		// adjust speed following a kill
		if (getNumberOfEnemiesAlive() > 0)
			m_speed *= m_speedIncreaseMultiplier / getNumberOfEnemiesAlive() + 1;
	}
}

void Enemies::toggleDirection()
{
	for (auto& enemy : m_enemies)
		enemy.flipDirection();
	m_dropSpeed *= m_dropSpeedIncreaseMultiplier;
}

unsigned int Enemies::getNumberOfEnemiesAlive() const
{
	unsigned int total{ 0u };
	for (auto& enemy : m_enemies)
	{
		if (enemy.isAlive())
			++total;
	}
	return total;
}

bool Enemies::getReachedBottom() const
{
	return m_reachedBottom;
}

void Enemies::priv_addEnemies()
{
	const unsigned int numberOfEnemies = 40u;
	const unsigned int enemiesPerRow = 8u;
	const double horizontalSpacing = 65.0;
	const double verticalSpacing = 65.0;
	const pl::Vector2d positionOfTopLeft = { 120.0, 80.0 };
	for (unsigned int i = 0; i < numberOfEnemies; ++i)
		m_enemies.emplace_back(Enemy(m_view, positionOfTopLeft + pl::Vector2d{ horizontalSpacing * (i % enemiesPerRow), verticalSpacing * (i / enemiesPerRow) }));
}
<|endoftext|>


<commit_before>#include <dlfcn.h>
#include <mach/mach.h>

extern "C" {
#include <mach-o/nlist.h>
}

#include <cstdio>
#include <pthread.h>
#include <unistd.h>

#include "Baton.hpp"
#include "Exception.hpp"
#include "Pooling.hpp"
#include "Trampoline.t.hpp"

extern "C" void __pthread_set_self(pthread_t);

template <typename Type_>
static void nlset(Type_ &function, struct nlist *nl, size_t index) {
    struct nlist &name(nl[index]);
    uintptr_t value(name.n_value);
    _assert(value != 0);
    if ((name.n_desc & N_ARM_THUMB_DEF) != 0)
        value |= 0x00000001;
    function = value;
}

void InjectLibrary(pid_t pid) {
    // XXX: break this into the build environment
    const char *library("/usr/lib/libcycript.dylib");

    static const size_t Stack_(8 * 1024);
    size_t length(strlen(library) + 1), depth(sizeof(Baton) + length);
    depth = (depth + sizeof(uintptr_t) + 1) / sizeof(uintptr_t) * sizeof(uintptr_t);

    CYPool pool;
    uint8_t *local(reinterpret_cast<uint8_t *>(apr_palloc(pool, depth)));
    Baton *baton(reinterpret_cast<Baton *>(local));

    uintptr_t set_self_internal;
    uintptr_t set_self_external;

    struct nlist nl[3];
    memset(nl, 0, sizeof(nl));
    nl[0].n_un.n_name = (char *) "__pthread_set_self";
    nl[1].n_un.n_name = (char *) "___pthread_set_self";
    nlist("/usr/lib/libSystem.B.dylib", nl);
    nlset(set_self_internal, nl, 0);
    nlset(set_self_external, nl, 1);

    baton->_pthread_set_self = reinterpret_cast<void (*)(pthread_t)>(reinterpret_cast<uintptr_t>(&__pthread_set_self) - set_self_external + set_self_internal);

    baton->pthread_create = &pthread_create;
    baton->pthread_join = &pthread_join;

    baton->dlopen = &dlopen;
    baton->dlsym = &dlsym;

    baton->mach_thread_self = &mach_thread_self;
    baton->thread_terminate = &thread_terminate;

    baton->pid = getpid();
    memcpy(baton->library, library, length);

    vm_size_t size(depth + Stack_);

    mach_port_t self(mach_task_self()), task;
    _krncall(task_for_pid(self, pid, &task));

    vm_address_t stack;
    _krncall(vm_allocate(task, &stack, size, true));
    vm_address_t data(stack + Stack_);

    vm_write(task, data, reinterpret_cast<vm_address_t>(baton), depth);

    vm_address_t code;
    _krncall(vm_allocate(task, &code, sizeof(Trampoline_), true));
    vm_write(task, code, reinterpret_cast<vm_address_t>(Trampoline_), sizeof(Trampoline_));
    _krncall(vm_protect(task, code, sizeof(Trampoline_), false, VM_PROT_READ | VM_PROT_EXECUTE));

    thread_act_t thread;
    _krncall(thread_create(task, &thread));

    thread_state_flavor_t flavor;
    mach_msg_type_number_t count;
    size_t push;

#if defined(__arm__)
    arm_thread_state_t state;
    flavor = ARM_THREAD_STATE;
    count = ARM_THREAD_STATE_COUNT;
    push = 0;
#elif defined(__i386__)
    i386_thread_state_t state;
    flavor = i386_THREAD_STATE;
    count = i386_THREAD_STATE_COUNT;
    push = 5;
#else
    #error XXX: implement
#endif

    uintptr_t frame[push];
    if (sizeof(frame) != 0)
        memset(frame, 0, sizeof(frame));

    memset(&state, 0, sizeof(state));

    mach_msg_type_number_t read(count);
    _krncall(thread_get_state(thread, flavor, reinterpret_cast<thread_state_t>(&state), &read));
    _assert(count == count);

#if defined(__arm__)
    state.r[0] = data;
    state.sp = stack + Stack_;
    state.pc = code;

    if ((state.pc & 0x1) != 0) {
        state.pc &= ~0x1;
        state.cpsr |= 0x20;
    }
#elif defined(__i386__)
    frame[0] = 0;
    frame[1] = data;

    state.__eip = code;
    state.__esp = stack + Stack_ - sizeof(frame);
#else
    #error XXX: implement
#endif

    if (sizeof(frame) != 0)
        vm_write(task, stack + Stack_ - sizeof(frame), reinterpret_cast<vm_address_t>(frame), sizeof(frame));

    _krncall(thread_set_state(thread, flavor, reinterpret_cast<thread_state_t>(&state), count));
    _krncall(thread_resume(thread));

    _krncall(mach_port_deallocate(self, task));
}
<commit_msg>Attempting a silly x64 fix.<commit_after>#include <dlfcn.h>
#include <mach/mach.h>

extern "C" {
#include <mach-o/nlist.h>
}

#include <cstdio>
#include <pthread.h>
#include <unistd.h>

#include "Baton.hpp"
#include "Exception.hpp"
#include "Pooling.hpp"
#include "Trampoline.t.hpp"

extern "C" void __pthread_set_self(pthread_t);

template <typename Type_>
static void nlset(Type_ &function, struct nlist *nl, size_t index) {
    struct nlist &name(nl[index]);
    uintptr_t value(name.n_value);
    _assert(value != 0);
    if ((name.n_desc & N_ARM_THUMB_DEF) != 0)
        value |= 0x00000001;
    function = value;
}

void InjectLibrary(pid_t pid) {
    // XXX: break this into the build environment
    const char *library("/usr/lib/libcycript.dylib");

    static const size_t Stack_(8 * 1024);
    size_t length(strlen(library) + 1), depth(sizeof(Baton) + length);
    depth = (depth + sizeof(uintptr_t) + 1) / sizeof(uintptr_t) * sizeof(uintptr_t);

    CYPool pool;
    uint8_t *local(reinterpret_cast<uint8_t *>(apr_palloc(pool, depth)));
    Baton *baton(reinterpret_cast<Baton *>(local));

    uintptr_t set_self_internal;
    uintptr_t set_self_external;

    struct nlist nl[3];
    memset(nl, 0, sizeof(nl));
    nl[0].n_un.n_name = (char *) "__pthread_set_self";
    nl[1].n_un.n_name = (char *) "___pthread_set_self";
    nlist("/usr/lib/libSystem.B.dylib", nl);
    nlset(set_self_internal, nl, 0);
    nlset(set_self_external, nl, 1);

    baton->_pthread_set_self = reinterpret_cast<void (*)(pthread_t)>(reinterpret_cast<uintptr_t>(&__pthread_set_self) - set_self_external + set_self_internal);

    baton->pthread_create = &pthread_create;
    baton->pthread_join = &pthread_join;

    baton->dlopen = &dlopen;
    baton->dlsym = &dlsym;

    baton->mach_thread_self = &mach_thread_self;
    baton->thread_terminate = &thread_terminate;

    baton->pid = getpid();
    memcpy(baton->library, library, length);

    vm_size_t size(depth + Stack_);

    mach_port_t self(mach_task_self()), task;
    _krncall(task_for_pid(self, pid, &task));

    vm_address_t stack;
    _krncall(vm_allocate(task, &stack, size, true));
    vm_address_t data(stack + Stack_);

    vm_write(task, data, reinterpret_cast<vm_address_t>(baton), depth);

    vm_address_t code;
    _krncall(vm_allocate(task, &code, sizeof(Trampoline_), true));
    vm_write(task, code, reinterpret_cast<vm_address_t>(Trampoline_), sizeof(Trampoline_));
    _krncall(vm_protect(task, code, sizeof(Trampoline_), false, VM_PROT_READ | VM_PROT_EXECUTE));

    thread_act_t thread;
    _krncall(thread_create(task, &thread));

    thread_state_flavor_t flavor;
    mach_msg_type_number_t count;
    size_t push;

#if defined(__arm__)
    arm_thread_state_t state;
    flavor = ARM_THREAD_STATE;
    count = ARM_THREAD_STATE_COUNT;
    push = 0;
#elif defined(__i386__) || defined(__x86_64__)
    i386_thread_state_t state;
    flavor = i386_THREAD_STATE;
    count = i386_THREAD_STATE_COUNT;
    push = 5;
#else
    #error XXX: implement
#endif

    uintptr_t frame[push];
    if (sizeof(frame) != 0)
        memset(frame, 0, sizeof(frame));

    memset(&state, 0, sizeof(state));

    mach_msg_type_number_t read(count);
    _krncall(thread_get_state(thread, flavor, reinterpret_cast<thread_state_t>(&state), &read));
    _assert(count == count);

#if defined(__arm__)
    state.r[0] = data;
    state.sp = stack + Stack_;
    state.pc = code;

    if ((state.pc & 0x1) != 0) {
        state.pc &= ~0x1;
        state.cpsr |= 0x20;
    }
#elif defined(__i386__) || defined(__x86_64__)
    frame[0] = 0;
    frame[1] = data;

    state.__eip = code;
    state.__esp = stack + Stack_ - sizeof(frame);
#else
    #error XXX: implement
#endif

    if (sizeof(frame) != 0)
        vm_write(task, stack + Stack_ - sizeof(frame), reinterpret_cast<vm_address_t>(frame), sizeof(frame));

    _krncall(thread_set_state(thread, flavor, reinterpret_cast<thread_state_t>(&state), count));
    _krncall(thread_resume(thread));

    _krncall(mach_port_deallocate(self, task));
}
<|endoftext|>


<commit_before>/******************************************************************************
 * Copyright 2018 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/scenarios/side_pass/side_pass_stop_on_wait_point.h"

#include <algorithm>
#include <vector>

#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/proto/pnc_point.pb.h"
#include "modules/planning/common/frame.h"
#include "modules/planning/common/speed_profile_generator.h"

namespace apollo {
namespace planning {
namespace scenario {
namespace side_pass {

using apollo::common::TrajectoryPoint;
using apollo::common::PathPoint;
using apollo::common::math::Vec2d;

constexpr double kExtraMarginforStopOnWaitPointStage = 3.0;

Stage::StageStatus SidePassStopOnWaitPoint::Process(
    const TrajectoryPoint& planning_start_point, Frame* frame) {
  const ReferenceLineInfo& reference_line_info =
      frame->reference_line_info().front();
  const ReferenceLine& reference_line = reference_line_info.reference_line();
  const PathDecision& path_decision = reference_line_info.path_decision();

  if (GetContext()->path_data_.discretized_path().path_points().empty()) {
    AERROR << "path data is empty.";
    return Stage::ERROR;
  }

  if (!GetContext()->path_data_.UpdateFrenetFramePath(&reference_line)) {
    return Stage::ERROR;
  }

  const auto adc_frenet_frame_point_ =
      reference_line.GetFrenetPoint(frame->PlanningStartPoint());

  if (!GetContext()->path_data_.LeftTrimWithRefS(adc_frenet_frame_point_.s(),
                                                 adc_frenet_frame_point_.l())) {
    return Stage::ERROR;
  }

  PathPoint first_path_point =
      GetContext()->path_data_.discretized_path().path_points().front();

  PathPoint last_path_point;
  if (!GetMoveForwardLastPathPoint(reference_line, &last_path_point)) {
    AERROR << "Fail to get move forward last path point.";
    return Stage::ERROR;
  }
  ADEBUG << "first_path_point: " << first_path_point.ShortDebugString();
  ADEBUG << "last_path_point : " << first_path_point.ShortDebugString();

  double move_forward_distance = last_path_point.s() - first_path_point.s();
  ADEBUG << "move_forward_distance: " << move_forward_distance;

  if (!IsFarAwayFromObstacles(reference_line, path_decision.obstacles(),
                              first_path_point, last_path_point)) {
    // wait here, do nothing this cycle.
    auto& rfl_info = frame->mutable_reference_line_info()->front();
    *(rfl_info.mutable_speed_data()) =
      SpeedProfileGenerator::GenerateFallbackSpeedProfile();
    rfl_info.set_trajectory_type(ADCTrajectory::NORMAL);
    DiscretizedTrajectory trajectory;
    if (!rfl_info.CombinePathAndSpeedProfile(
            frame->PlanningStartPoint().relative_time(),
            frame->PlanningStartPoint().path_point().s(), &trajectory)) {
      AERROR << "Fail to aggregate planning trajectory.";
      return Stage::RUNNING;
    }
    rfl_info.SetTrajectory(trajectory);
    rfl_info.SetDrivable(true);

    AINFO << "waiting until obstacles are far away.";
    return Stage::RUNNING;
  }

  // (1) call proceed with cautious
  constexpr double kSidePassCreepSpeed = 2.33;  // m/s
  auto& rfl_info = frame->mutable_reference_line_info()->front();
  *(rfl_info.mutable_speed_data()) =
      SpeedProfileGenerator::GenerateFixedDistanceCreepProfile(
          move_forward_distance, kSidePassCreepSpeed);

  // (2) combine path and speed.
  *(rfl_info.mutable_path_data()) = GetContext()->path_data_;

  rfl_info.set_trajectory_type(ADCTrajectory::NORMAL);
  DiscretizedTrajectory trajectory;
  if (!rfl_info.CombinePathAndSpeedProfile(
          frame->PlanningStartPoint().relative_time(),
          frame->PlanningStartPoint().path_point().s(), &trajectory)) {
    AERROR << "Fail to aggregate planning trajectory.";
    return Stage::RUNNING;
  }
  rfl_info.SetTrajectory(trajectory);
  rfl_info.SetDrivable(true);

  constexpr double kBuffer = 0.3;
  if (move_forward_distance < kBuffer) {
    next_stage_ = ScenarioConfig::SIDE_PASS_DETECT_SAFETY;
  }
  return Stage::FINISHED;
}

bool SidePassStopOnWaitPoint::IsFarAwayFromObstacles(
    const ReferenceLine& reference_line,
    const IndexedList<std::string, Obstacle>& indexed_obstacle_list,
    const PathPoint& first_path_point, const PathPoint& last_path_point) {
  common::SLPoint first_sl_point;
  if (!reference_line.XYToSL(Vec2d(first_path_point.x(), first_path_point.y()),
                             &first_sl_point)) {
    AERROR << "Failed to get the projection from TrajectoryPoint onto "
              "reference_line";
    return false;
  }
  common::SLPoint last_sl_point;
  if (!reference_line.XYToSL(Vec2d(last_path_point.x(), last_path_point.y()),
                             &last_sl_point)) {
    AERROR << "Failed to get the projection from TrajectoryPoint onto "
              "reference_line";
    return false;
  }

  // Go through every obstacle, check if there is any in the no_obs_zone,
  // which will used by the proceed_with_caution movement.
  for (const auto* obstacle : indexed_obstacle_list.Items()) {
    if (obstacle->IsVirtual()) {
      continue;
    }
    // Check the s-direction.
    double obs_start_s = obstacle->PerceptionSLBoundary().start_s();
    double obs_end_s = obstacle->PerceptionSLBoundary().end_s();
    if (obs_end_s < first_sl_point.s() ||
        obs_start_s > last_sl_point.s() + kExtraMarginforStopOnWaitPointStage) {
      continue;
    }
    // Check the l-direction.
    double lane_left_width_at_start_s = 0.0;
    double lane_left_width_at_end_s = 0.0;
    double lane_right_width_at_start_s = 0.0;
    double lane_right_width_at_end_s = 0.0;
    reference_line.GetLaneWidth(obs_start_s, &lane_left_width_at_start_s,
                                &lane_right_width_at_start_s);
    reference_line.GetLaneWidth(obs_end_s, &lane_left_width_at_end_s,
                                &lane_right_width_at_end_s);
    double lane_left_width = std::min(std::abs(lane_left_width_at_start_s),
                                      std::abs(lane_left_width_at_end_s));
    double lane_right_width = std::min(std::abs(lane_right_width_at_start_s),
                                       std::abs(lane_right_width_at_end_s));
    double obs_start_l = obstacle->PerceptionSLBoundary().start_l();
    double obs_end_l = obstacle->PerceptionSLBoundary().end_l();
    if (obs_start_l < lane_left_width && -obs_end_l < lane_right_width) {
      return false;
    }
  }
  return true;
}

bool SidePassStopOnWaitPoint::GetMoveForwardLastPathPoint(
    const ReferenceLine& reference_line,
    common::PathPoint* const last_path_point) {
  int count = 0;
  for (const auto& path_point :
       GetContext()->path_data_.discretized_path().path_points()) {
    // Get the four corner points ABCD of ADC at every path point,
    // and check if that's within the current lane until it reaches
    // out of current lane.
    const auto& vehicle_box =
        common::VehicleConfigHelper::Instance()->GetBoundingBox(path_point);
    std::vector<Vec2d> ABCDpoints = vehicle_box.GetAllCorners();
    bool is_out_of_curr_lane = false;
    for (size_t i = 0; i < ABCDpoints.size(); i++) {
      // For each corner point, project it onto reference_line
      common::SLPoint curr_point_sl;
      if (!reference_line.XYToSL(ABCDpoints[i], &curr_point_sl)) {
        AERROR << "Failed to get the projection from point onto "
                  "reference_line";
        return false;
      }
      // Get the lane width at the current s indicated by path_point
      double curr_point_left_width = 0.0;
      double curr_point_right_width = 0.0;
      reference_line.GetLaneWidth(curr_point_sl.s(), &curr_point_left_width,
                                  &curr_point_right_width);
      // Check if this corner point is within the lane:
      if (curr_point_sl.l() > std::abs(curr_point_left_width) ||
          curr_point_sl.l() < -std::abs(curr_point_right_width)) {
        is_out_of_curr_lane = true;
        break;
      }
    }
    if (is_out_of_curr_lane) {
      if (count == 0) {
        // The current ADC, without moving at all, is already at least
        // partially out of the current lane.
        return Stage::FINISHED;
      }
      break;
    } else {
      *last_path_point = path_point;
    }
    // check if the ego car on path_point will partially go into the
    // neighbor lane, and retain only those within-lane path-points.
    CHECK_GE(path_point.s(), 0.0);
    ++count;
  }
  return true;
}

}  // namespace side_pass
}  // namespace scenario
}  // namespace planning
}  // namespace apollo
<commit_msg>planning: added path check in side pass stop on wait point.<commit_after>/******************************************************************************
 * Copyright 2018 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/scenarios/side_pass/side_pass_stop_on_wait_point.h"

#include <algorithm>
#include <vector>

#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/proto/pnc_point.pb.h"
#include "modules/planning/common/frame.h"
#include "modules/planning/common/speed_profile_generator.h"

namespace apollo {
namespace planning {
namespace scenario {
namespace side_pass {

using apollo::common::TrajectoryPoint;
using apollo::common::PathPoint;
using apollo::common::math::Vec2d;

constexpr double kExtraMarginforStopOnWaitPointStage = 3.0;

Stage::StageStatus SidePassStopOnWaitPoint::Process(
    const TrajectoryPoint& planning_start_point, Frame* frame) {
  const ReferenceLineInfo& reference_line_info =
      frame->reference_line_info().front();
  const ReferenceLine& reference_line = reference_line_info.reference_line();
  const PathDecision& path_decision = reference_line_info.path_decision();

  if (GetContext()->path_data_.discretized_path().path_points().empty()) {
    AERROR << "path data is empty.";
    return Stage::ERROR;
  }

  if (!GetContext()->path_data_.UpdateFrenetFramePath(&reference_line)) {
    return Stage::ERROR;
  }

  const auto adc_frenet_frame_point_ =
      reference_line.GetFrenetPoint(frame->PlanningStartPoint());

  if (!GetContext()->path_data_.LeftTrimWithRefS(adc_frenet_frame_point_.s(),
                                                 adc_frenet_frame_point_.l())) {
    return Stage::ERROR;
  }
  if (GetContext()->path_data_.discretized_path().path_points().empty()) {
    AERROR << "path data is empty after trim.";
    return Stage::ERROR;
  }

  PathPoint first_path_point =
      GetContext()->path_data_.discretized_path().path_points().front();

  PathPoint last_path_point;
  if (!GetMoveForwardLastPathPoint(reference_line, &last_path_point)) {
    AERROR << "Fail to get move forward last path point.";
    return Stage::ERROR;
  }
  ADEBUG << "first_path_point: " << first_path_point.ShortDebugString();
  ADEBUG << "last_path_point : " << first_path_point.ShortDebugString();

  double move_forward_distance = last_path_point.s() - first_path_point.s();
  ADEBUG << "move_forward_distance: " << move_forward_distance;

  if (!IsFarAwayFromObstacles(reference_line, path_decision.obstacles(),
                              first_path_point, last_path_point)) {
    // wait here, do nothing this cycle.
    auto& rfl_info = frame->mutable_reference_line_info()->front();
    *(rfl_info.mutable_path_data()) = GetContext()->path_data_;
    *(rfl_info.mutable_speed_data()) =
      SpeedProfileGenerator::GenerateFallbackSpeedProfile();
    rfl_info.set_trajectory_type(ADCTrajectory::NORMAL);
    DiscretizedTrajectory trajectory;
    if (!rfl_info.CombinePathAndSpeedProfile(
            frame->PlanningStartPoint().relative_time(),
            frame->PlanningStartPoint().path_point().s(), &trajectory)) {
      AERROR << "Fail to aggregate planning trajectory.";
      return Stage::RUNNING;
    }
    rfl_info.SetTrajectory(trajectory);
    rfl_info.SetDrivable(true);

    AINFO << "waiting until obstacles are far away.";
    return Stage::RUNNING;
  }

  // (1) call proceed with cautious
  constexpr double kSidePassCreepSpeed = 2.33;  // m/s
  auto& rfl_info = frame->mutable_reference_line_info()->front();
  *(rfl_info.mutable_speed_data()) =
      SpeedProfileGenerator::GenerateFixedDistanceCreepProfile(
          move_forward_distance, kSidePassCreepSpeed);

  // (2) combine path and speed.
  *(rfl_info.mutable_path_data()) = GetContext()->path_data_;

  rfl_info.set_trajectory_type(ADCTrajectory::NORMAL);
  DiscretizedTrajectory trajectory;
  if (!rfl_info.CombinePathAndSpeedProfile(
          frame->PlanningStartPoint().relative_time(),
          frame->PlanningStartPoint().path_point().s(), &trajectory)) {
    AERROR << "Fail to aggregate planning trajectory.";
    return Stage::RUNNING;
  }
  rfl_info.SetTrajectory(trajectory);
  rfl_info.SetDrivable(true);

  constexpr double kBuffer = 0.3;
  if (move_forward_distance < kBuffer) {
    next_stage_ = ScenarioConfig::SIDE_PASS_DETECT_SAFETY;
  }
  return Stage::FINISHED;
}

bool SidePassStopOnWaitPoint::IsFarAwayFromObstacles(
    const ReferenceLine& reference_line,
    const IndexedList<std::string, Obstacle>& indexed_obstacle_list,
    const PathPoint& first_path_point, const PathPoint& last_path_point) {
  common::SLPoint first_sl_point;
  if (!reference_line.XYToSL(Vec2d(first_path_point.x(), first_path_point.y()),
                             &first_sl_point)) {
    AERROR << "Failed to get the projection from TrajectoryPoint onto "
              "reference_line";
    return false;
  }
  common::SLPoint last_sl_point;
  if (!reference_line.XYToSL(Vec2d(last_path_point.x(), last_path_point.y()),
                             &last_sl_point)) {
    AERROR << "Failed to get the projection from TrajectoryPoint onto "
              "reference_line";
    return false;
  }

  // Go through every obstacle, check if there is any in the no_obs_zone,
  // which will used by the proceed_with_caution movement.
  for (const auto* obstacle : indexed_obstacle_list.Items()) {
    if (obstacle->IsVirtual()) {
      continue;
    }
    // Check the s-direction.
    double obs_start_s = obstacle->PerceptionSLBoundary().start_s();
    double obs_end_s = obstacle->PerceptionSLBoundary().end_s();
    if (obs_end_s < first_sl_point.s() ||
        obs_start_s > last_sl_point.s() + kExtraMarginforStopOnWaitPointStage) {
      continue;
    }
    // Check the l-direction.
    double lane_left_width_at_start_s = 0.0;
    double lane_left_width_at_end_s = 0.0;
    double lane_right_width_at_start_s = 0.0;
    double lane_right_width_at_end_s = 0.0;
    reference_line.GetLaneWidth(obs_start_s, &lane_left_width_at_start_s,
                                &lane_right_width_at_start_s);
    reference_line.GetLaneWidth(obs_end_s, &lane_left_width_at_end_s,
                                &lane_right_width_at_end_s);
    double lane_left_width = std::min(std::abs(lane_left_width_at_start_s),
                                      std::abs(lane_left_width_at_end_s));
    double lane_right_width = std::min(std::abs(lane_right_width_at_start_s),
                                       std::abs(lane_right_width_at_end_s));
    double obs_start_l = obstacle->PerceptionSLBoundary().start_l();
    double obs_end_l = obstacle->PerceptionSLBoundary().end_l();
    if (obs_start_l < lane_left_width && -obs_end_l < lane_right_width) {
      return false;
    }
  }
  return true;
}

bool SidePassStopOnWaitPoint::GetMoveForwardLastPathPoint(
    const ReferenceLine& reference_line,
    common::PathPoint* const last_path_point) {
  int count = 0;
  for (const auto& path_point :
       GetContext()->path_data_.discretized_path().path_points()) {
    // Get the four corner points ABCD of ADC at every path point,
    // and check if that's within the current lane until it reaches
    // out of current lane.
    const auto& vehicle_box =
        common::VehicleConfigHelper::Instance()->GetBoundingBox(path_point);
    std::vector<Vec2d> ABCDpoints = vehicle_box.GetAllCorners();
    bool is_out_of_curr_lane = false;
    for (size_t i = 0; i < ABCDpoints.size(); i++) {
      // For each corner point, project it onto reference_line
      common::SLPoint curr_point_sl;
      if (!reference_line.XYToSL(ABCDpoints[i], &curr_point_sl)) {
        AERROR << "Failed to get the projection from point onto "
                  "reference_line";
        return false;
      }
      // Get the lane width at the current s indicated by path_point
      double curr_point_left_width = 0.0;
      double curr_point_right_width = 0.0;
      reference_line.GetLaneWidth(curr_point_sl.s(), &curr_point_left_width,
                                  &curr_point_right_width);
      // Check if this corner point is within the lane:
      if (curr_point_sl.l() > std::abs(curr_point_left_width) ||
          curr_point_sl.l() < -std::abs(curr_point_right_width)) {
        is_out_of_curr_lane = true;
        break;
      }
    }
    if (is_out_of_curr_lane) {
      if (count == 0) {
        // The current ADC, without moving at all, is already at least
        // partially out of the current lane.
        return Stage::FINISHED;
      }
      break;
    } else {
      *last_path_point = path_point;
    }
    // check if the ego car on path_point will partially go into the
    // neighbor lane, and retain only those within-lane path-points.
    CHECK_GE(path_point.s(), 0.0);
    ++count;
  }
  return true;
}

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


<commit_before>/* Siconos-Examples version 3.0.0, Copyright INRIA 2005-2008.
 * Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 * Siconos is a free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * Siconos is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Siconos; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 * Contact: Vincent ACARY vincent.acary@inrialpes.fr
*/

/*!\file CamFollowerNoXML.cpp
\brief \ref EMCamFollower - C++ input file version - M. di Bernardo, G. Osorio, S. Santini.
*/

#include "SiconosKernel.hpp"
#include "CamState.h"

using namespace std;

int main(int argc, char* argv[])
{
  double rpm = 358;
  try
  {

    // ================= Creation of the model =======================

    // User-defined main parameters
    unsigned int dsNumber = 1;       // the Follower and the ground
    unsigned int nDof = 1;           // degrees of freedom for the ball
    double t0 = 0;                   // initial computation time
    double T = 1;                   // final computation time
    double h = 0.0001;                // time step
    double position_init = 0.;//;40;      // initial position for lowest bead.
    double velocity_init = 0.;//4;      // initial velocity for lowest bead.
    double theta = 0.5;              // theta for Moreau integrator
    // -------------------------
    // --- Dynamical systems ---
    // -------------------------

    SP::SimpleMatrix Mass(new SimpleMatrix(nDof, nDof));
    SP::SimpleMatrix K(new SimpleMatrix(nDof, nDof));
    SP::SimpleMatrix C(new SimpleMatrix(nDof, nDof));       // mass/rigidity/viscosity
    (*Mass)(0, 0) = 1.221;
    (*K)(0, 0) = 1430.8;

    // -- Initial positions and velocities --
    vector<SP::SimpleVector> q0;
    vector<SP::SimpleVector> velocity0;
    q0.resize(dsNumber);
    velocity0.resize(dsNumber);
    q0[0].reset(new SimpleVector(nDof));
    velocity0[0].reset(new SimpleVector(nDof));
    (*(q0[0]))(0) = position_init;
    (*(velocity0[0]))(0) = velocity_init;
    SP::LagrangianLinearTIDS lds(new LagrangianLinearTIDS(q0[0], velocity0[0], Mass, K, C));
    lds->setComputeFExtFunction("FollowerPlugin.so", "FollowerFExtR");

    // Example to set a list of parameters in FExt function.
    // 1 - Create a simple vector that contains the required parameters.
    SP::SiconosVector param(new SimpleVector(1)); // Here we only set one parameter, the DS number.
    //    (*param)(0) = vectorDS[0]->getNumber();
    (*param)(0) = rpm;
    // 2 - Assign this param to the function FExt
    lds->setZPtr(param);
    // 2 corresponds to the position of FExt in the stl vector of possible parameters. 0 is mass, 1 FInt and so on.
    // Now the DS number will be available in FExt plugin.

    DynamicalSystemsSet allDS;
    allDS.insert(lds);

    // --------------------
    // --- Interactions ---
    // --------------------

    // -- nslaw --
    double e = 0.8;

    // Interaction Follower-floor
    //
    SimpleMatrix H(1, nDof);
    H(0, 0) = 1.0;
    SP::NonSmoothLaw nslaw0(new NewtonImpactNSL(e));
    SP::Relation relation0(new LagrangianLinearTIR(H));
    DynamicalSystemsSet dsConcerned;
    dsConcerned.insert(lds);

    SP::Interaction inter(new Interaction("Follower-Ground", dsConcerned, 0, 1, nslaw0, relation0));
    InteractionsSet allInteractions;
    allInteractions.insert(inter);

    // -------------
    // --- Model ---
    // -------------

    SP::Model Follower(new Model(t0, T, allDS, allInteractions));

    // ----------------
    // --- Simulation ---
    // ----------------

    // -- Time discretisation --
    SP::TimeDiscretisation t(new TimeDiscretisation(t0, h));

    SP::TimeStepping S(new TimeStepping(t));

    // -- OneStepIntegrator --
    SP::OneStepIntegrator OSI(new Moreau(lds, theta));
    S->insertIntegrator(OSI);

    // -- OneStepNsProblem --
    IntParameters iparam(5);
    iparam[0] = 101; // Max number of iteration
    DoubleParameters dparam(5);
    dparam[0] = 1e-5; // Tolerance
    string solverName = "QP" ;
    SP::NonSmoothSolver mySolver(new NonSmoothSolver(solverName, iparam, dparam));
    SP::OneStepNSProblem osnspb(new LCP(mySolver));
    S->insertNonSmoothProblem(osnspb);

    cout << "=== End of model loading === " << endl;
    // =========================== End of model definition ===========================

    // ================================= Computation =================================

    // --- Simulation initialization ---
    Follower->initialize(S);
    cout << "End of model initialisation" << endl;

    int k = 0;
    int N = (int)((T - t0) / h); // Number of time steps

    // --- Get the values to be plotted ---
    // -> saved in a matrix dataPlot
    unsigned int outputSize = 8;
    SimpleMatrix DataPlot(N + 1, outputSize);
    // For the initial time step:
    // time
    DataPlot(k, 0) = t0;
    DataPlot(k, 1) = (*lds->q())(0);
    DataPlot(k, 2) = (*lds->velocity())(0);
    DataPlot(k, 3) = (*Follower->nonSmoothDynamicalSystem()->topology()->interactions()->getPtr(0)->lambda(1))(0);
    DataPlot(k, 4) = (*lds->fExt())(0);

    // State of the Cam
    //    double rpm=358;
    double CamEqForce, CamPosition, CamVelocity, CamAcceleration;

    CamEqForce = CamState(t0, rpm, CamPosition, CamVelocity, CamAcceleration);
    // Position of the Cam
    DataPlot(k, 5) = CamPosition;
    // Velocity of the Cam
    DataPlot(k, 6) = CamVelocity;
    // Acceleration of the Cam
    DataPlot(k, 7) = CamPosition + (*lds->q())(0);
    boost::timer tt;
    tt.restart();
    // --- Time loop ---
    cout << "Start computation ... " << endl;
    while (k < N)
    {
      // get current time step
      k++;
      // solve ...
      S->computeOneStep();

      // --- Get values to be plotted ---

      DataPlot(k, 0) = S->nextTime();
      DataPlot(k, 1) = (*lds->q())(0);
      DataPlot(k, 2) = (*lds->velocity())(0);
      DataPlot(k, 3) = (*Follower->nonSmoothDynamicalSystem()->topology()->interactions()->getPtr(0)->lambda(1))(0);
      DataPlot(k, 4) = (*lds->fExt())(0);

      CamEqForce = CamState(S->nextTime(), rpm, CamPosition, CamVelocity, CamAcceleration);
      DataPlot(k, 5) = CamPosition;
      DataPlot(k, 6) = CamVelocity;
      DataPlot(k, 7) = CamPosition + (*lds->q())(0);
      // transfer of state i+1 into state i and time incrementation
      S->nextStep();
    }
    // --- Output files ---
    ioMatrix io("result.dat", "ascii");
    io.write(DataPlot, "noDim");
    cout << "time = " << tt.elapsed() << endl;
    cout << "End of computation - Number of iterations done: " << k << endl;
  }

  catch (SiconosException e)
  {
    cout << e.report() << endl;
  }
  catch (...)
  {
    cout << "Exception caught in \'sample/CamFollower\'" << endl;
  }
}
<commit_msg>update as the bouncing ball<commit_after>/* Siconos-Examples version 3.0.0, Copyright INRIA 2005-2008.
 * Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 * Siconos is a free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * Siconos is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Siconos; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 * Contact: Vincent ACARY vincent.acary@inrialpes.fr
*/

/*!\file CamFollowerNoXML.cpp
\brief \ref EMCamFollower - C++ input file version - M. di Bernardo, G. Osorio, S. Santini.
*/

#include "SiconosKernel.hpp"
#include "CamState.h"

using namespace std;

int main(int argc, char* argv[])
{
  double rpm = 358;
  try
  {

    // ================= Creation of the model =======================

    // User-defined main parameters
    unsigned int dsNumber = 1;       // the Follower and the ground
    unsigned int nDof = 1;           // degrees of freedom for the ball
    double t0 = 0;                   // initial computation time
    double T = 1;                   // final computation time
    double h = 0.0001;                // time step
    double position_init = 0.;//;40;      // initial position for lowest bead.
    double velocity_init = 0.;//4;      // initial velocity for lowest bead.
    double theta = 0.5;              // theta for Moreau integrator
    // -------------------------
    // --- Dynamical systems ---
    // -------------------------

    SP::SimpleMatrix Mass(new SimpleMatrix(nDof, nDof));
    SP::SimpleMatrix K(new SimpleMatrix(nDof, nDof));
    SP::SimpleMatrix C(new SimpleMatrix(nDof, nDof));       // mass/rigidity/viscosity
    (*Mass)(0, 0) = 1.221;
    (*K)(0, 0) = 1430.8;

    // -- Initial positions and velocities --
    vector<SP::SimpleVector> q0;
    vector<SP::SimpleVector> velocity0;
    q0.resize(dsNumber);
    velocity0.resize(dsNumber);
    q0[0].reset(new SimpleVector(nDof));
    velocity0[0].reset(new SimpleVector(nDof));
    (*(q0[0]))(0) = position_init;
    (*(velocity0[0]))(0) = velocity_init;
    SP::LagrangianLinearTIDS lds(new LagrangianLinearTIDS(q0[0], velocity0[0], Mass, K, C));
    lds->setComputeFExtFunction("FollowerPlugin.so", "FollowerFExtR");

    // Example to set a list of parameters in FExt function.
    // 1 - Create a simple vector that contains the required parameters.
    SP::SiconosVector param(new SimpleVector(1)); // Here we only set one parameter, the DS number.
    //    (*param)(0) = vectorDS[0]->getNumber();
    (*param)(0) = rpm;
    // 2 - Assign this param to the function FExt
    lds->setZPtr(param);
    // 2 corresponds to the position of FExt in the stl vector of possible parameters. 0 is mass, 1 FInt and so on.
    // Now the DS number will be available in FExt plugin.

    // --------------------
    // --- Interactions ---
    // --------------------

    // -- nslaw --
    double e = 0.8;

    // Interaction Follower-floor
    //
    SP::SiconosMatrix H(new SimpleMatrix(1, nDof));
    (*H)(0, 0) = 1.0;
    SP::NonSmoothLaw nslaw0(new NewtonImpactNSL(e));
    SP::Relation relation0(new LagrangianLinearTIR(H));
    SP::Interaction inter(new Interaction(1, nslaw0, relation0));

    // -------------
    // --- Model ---
    // -------------

    SP::Model Follower(new Model(t0, T));

    Follower->nonSmoothDynamicalSystem()->insertDynamicalSystem(lds);
    Follower->nonSmoothDynamicalSystem()->link(inter, lds);

    // ----------------
    // --- Simulation ---
    // ----------------

    // -- Time discretisation --
    SP::TimeDiscretisation t(new TimeDiscretisation(t0, h));

    // -- OneStepIntegrator --
    SP::OneStepIntegrator OSI(new Moreau(lds, theta));

    // -- OneStepNsProblem --
    IntParameters iparam(5);
    iparam[0] = 101; // Max number of iteration
    DoubleParameters dparam(5);
    dparam[0] = 1e-5; // Tolerance
    string solverName = "QP" ;
    SP::NonSmoothSolver mySolver(new NonSmoothSolver(solverName, iparam, dparam));
    SP::OneStepNSProblem osnspb(new LCP(mySolver));

    SP::TimeStepping S(new TimeStepping(t));
    S->insertIntegrator(OSI);
    S->insertNonSmoothProblem(osnspb);

    cout << "=== End of model loading === " << endl;
    // =========================== End of model definition ===========================

    // ================================= Computation =================================

    // --- Simulation initialization ---
    Follower->initialize(S);
    cout << "End of model initialisation" << endl;

    int k = 0;
    int N = (int)((T - t0) / h); // Number of time steps

    // --- Get the values to be plotted ---
    // -> saved in a matrix dataPlot
    unsigned int outputSize = 8;
    SimpleMatrix DataPlot(N + 1, outputSize);
    // For the initial time step:
    // time
    DataPlot(k, 0) = t0;
    DataPlot(k, 1) = (*lds->q())(0);
    DataPlot(k, 2) = (*lds->velocity())(0);
    DataPlot(k, 3) = (*Follower->nonSmoothDynamicalSystem()->topology()->interactions()->getPtr(0)->lambda(1))(0);
    DataPlot(k, 4) = (*lds->fExt())(0);

    // State of the Cam
    //    double rpm=358;
    double CamEqForce, CamPosition, CamVelocity, CamAcceleration;

    CamEqForce = CamState(t0, rpm, CamPosition, CamVelocity, CamAcceleration);
    // Position of the Cam
    DataPlot(k, 5) = CamPosition;
    // Velocity of the Cam
    DataPlot(k, 6) = CamVelocity;
    // Acceleration of the Cam
    DataPlot(k, 7) = CamPosition + (*lds->q())(0);
    boost::timer tt;
    tt.restart();
    // --- Time loop ---
    cout << "Start computation ... " << endl;
    while (k < N)
    {
      // get current time step
      k++;
      // solve ...
      S->computeOneStep();

      // --- Get values to be plotted ---

      DataPlot(k, 0) = S->nextTime();
      DataPlot(k, 1) = (*lds->q())(0);
      DataPlot(k, 2) = (*lds->velocity())(0);
      DataPlot(k, 3) = (*Follower->nonSmoothDynamicalSystem()->topology()->interactions()->getPtr(0)->lambda(1))(0);
      DataPlot(k, 4) = (*lds->fExt())(0);

      CamEqForce = CamState(S->nextTime(), rpm, CamPosition, CamVelocity, CamAcceleration);
      DataPlot(k, 5) = CamPosition;
      DataPlot(k, 6) = CamVelocity;
      DataPlot(k, 7) = CamPosition + (*lds->q())(0);
      // transfer of state i+1 into state i and time incrementation
      S->nextStep();
    }
    // --- Output files ---
    ioMatrix io("result.dat", "ascii");
    io.write(DataPlot, "noDim");
    cout << "time = " << tt.elapsed() << endl;
    cout << "End of computation - Number of iterations done: " << k << endl;
  }

  catch (SiconosException e)
  {
    cout << e.report() << endl;
  }
  catch (...)
  {
    cout << "Exception caught in \'sample/CamFollower\'" << endl;
  }
}
<|endoftext|>


<commit_before>/* Siconos-sample , Copyright INRIA 2005-2011.
 * Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 * Siconos is a free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * Siconos is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Siconos; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 * Contact: Vincent ACARY vincent.acary@inrialpes.fr
 */

/*!\file BouncingBallNETS.cpp
  \brief \ref EMBouncingBall - C++ input file, Time-Stepping version -
  V. Acary, O. Bonnefon.

  A Ball bouncing on the ground.
  Direct description of the model without XML input.
  Simulation with a Time-Stepping scheme.
*/
#include "SphereNEDSPlanR.hpp"
#include "SiconosKernel.hpp"

//#define WITH_PROJ
#define WITH_FC3D
using namespace std;
#ifdef WITH_FC3D
#define R_CLASS NewtonEulerFrom3DLocalFrameR
#else
#define R_CLASS NewtonEulerFrom1DLocalFrameR
#endif

class my_NewtonEulerR : public R_CLASS
{

  double _sBallRadius ;

public:

  my_NewtonEulerR(double radius): R_CLASS(), _sBallRadius(radius) { };

  virtual void computeh(double t, Interaction& inter)
  {
    SP::SiconosVector y = inter.y(0);
    double height = fabs(inter.data(q0)->getValue(0)) - _sBallRadius;
    // std::cout <<"my_NewtonEulerR:: computeh _jachq" << std:: endl;
    // _jachq->display();
    y->setValue(0, height);
    _Nc->setValue(0, 1);
    _Nc->setValue(1, 0);
    _Nc->setValue(2, 0);
    _Pc1->setValue(0, height);
    _Pc1->setValue(1, inter.data(q0)->getValue(1));
    _Pc1->setValue(2, inter.data(q0)->getValue(2));

    //_Pc2->setValue(0,hpc);
    //_Pc2->setValue(1,data[q0]->getValue(1));
    //_Pc2->setValue(2,data[q0]->getValue(2));
    //printf("my_NewtonEulerR N, Pc\n");
    //_Nc->display();
    //_Pc1->display();
  }
  //ACCEPT_VISITORS();
};
TYPEDEF_SPTR(my_NewtonEulerR);

int main(int argc, char* argv[])
{
  try
  {

    // ================= Creation of the model =======================

    // User-defined main parameters
    unsigned int nDof = 3;           // degrees of freedom for the ball
    unsigned int qDim = 7;           // degrees of freedom for the ball
    unsigned int nDim = 6;           // degrees of freedom for the ball
    double t0 = 0;                   // initial computation time
    double T = 10.0;                  // final computation time
    double h = 0.005;                // time step
    double position_init = 1.0;      // initial position for lowest bead.
    double velocity_init = 2.0;      // initial velocity for lowest bead.
    double omega_initx = 0.0;
    double omega_initz = 0.0;// initial velocity for lowest bead.
    double theta = 0.5;              // theta for Moreau integrator
    double m = 1; // Ball mass
    double g = 9.81; // Gravity
    double radius = 0.1;
    // -------------------------
    // --- Dynamical systems ---
    // -------------------------

    cout << "====> Model loading ..." << endl << endl;

    // -- Initial positions and velocities --
    SP::SiconosVector q0(new SiconosVector(qDim));
    SP::SiconosVector v0(new SiconosVector(nDim));
    SP::SimpleMatrix I(new SimpleMatrix(3, 3));
    v0->zero();
    q0->zero();
    I->eye();
    (*q0)(0) = position_init;
    /*initial quaternion equal to (1,0,0,0)*/
    (*q0)(3) = 1.0;

    (*v0)(0) = velocity_init;
    (*v0)(3) = omega_initx;
    (*v0)(5) = omega_initz;
    // -- The dynamical system --
    SP::NewtonEulerDS ball(new NewtonEulerDS(q0, v0, m, I));

    // -- Set external forces (weight) --
    SP::SiconosVector weight(new SiconosVector(nDof));
    (*weight)(0) = -m * g;
    ball->setFExtPtr(weight);

    // --------------------
    // --- Interactions ---
    // --------------------

    // -- nslaw --
    double e = 0.9;

    // Interaction ball-floor
    //

    //     vector<SP::SiconosMatrix> vecMatrix1;
    //     vecMatrix1.push_back(H);
    //     SP::BlockMatrix H_block(new BlockMatrix(vecMatrix1,1,1));

    //     SP::SiconosMatrix HT(new SimpleMatrix(1,nDim));
    //     vector<SP::SiconosMatrix> vecMatrix2;
    //     vecMatrix2.push_back(HT);
    //     SP::BlockMatrix HT_block(new BlockMatrix(vecMatrix2,1,1));

#ifdef WITH_FC3D
    int nslawsize = 3;
    SP::NonSmoothLaw nslaw0(new NewtonImpactFrictionNSL(e, e, 0.6, 3));
#else
    int nslawsize = 1;
    SP::NonSmoothLaw nslaw0(new NewtonImpactNSL(e));
#endif

    //     Version with NewtonEulerR()
    //
    //     SP::SimpleMatrix H(new SimpleMatrix(nslawsize,qDim));
    //     H->zero();
    //     (*H)(0,0) = 1.0;
    // #ifdef WITH_FC3D
    //     (*H)(1,1) = 1.0;
    //     (*H)(2,2) = 1.0;
    // #endif
    //     //SP::NewtonEulerR relation0(new SphereNEDSPlanR(0.1,1.0,0.0,0.0,0.0));
    //     //SP::NewtonEulerR relation0(new NewtonEulerR());
    //     //relation0->setJachq(H);
    //     //    relation0->setJacQH(H_block);
    //     //    relation0->setJacQHT(HT_block);
    //     //cout<<"main jacQH"<<endl;
    //     //relation0->jachq()->display();

    // Version with my_NewtonEulerR()
    SP::NewtonEulerR relation0(new my_NewtonEulerR(radius));
    SP::Interaction inter(new Interaction(nslawsize, nslaw0, relation0));

    // -------------
    // --- Model ---
    // -------------
    SP::Model bouncingBall(new Model(t0, T));
    // add the dynamical system in the non smooth dynamical system
    bouncingBall->nonSmoothDynamicalSystem()->insertDynamicalSystem(ball);

    // link the interaction and the dynamical system
    bouncingBall->nonSmoothDynamicalSystem()->link(inter, ball);

    // ------------------
    // --- Simulation ---
    // ------------------

    // -- (1) OneStepIntegrators --
#ifdef WITH_PROJ
    SP::MoreauProjectOnConstraintsOSI OSI(new MoreauProjectOnConstraintsOSI(ball, theta));
#else
    SP::Moreau OSI(new Moreau(ball, theta));
#endif
    // -- (2) Time discretisation --
    SP::TimeDiscretisation t(new TimeDiscretisation(t0, h));

    // -- (3) one step non smooth problem
    SP::OneStepNSProblem osnspb(new GenericMechanical());
#ifdef WITH_PROJ
    SP::OneStepNSProblem osnspb_pos(new MLCPProjectOnConstraints(SICONOS_MLCP_ENUM, 1.0));
#endif
    // -- (4) Simulation setup with (1) (2) (3)
#ifdef WITH_PROJ
    SP::TimeSteppingProjectOnConstraints s(new TimeSteppingProjectOnConstraints(t, OSI, osnspb, osnspb_pos));
    s->setProjectionMaxIteration(20);
    s->setConstraintTolUnilateral(1e-08);
    s->setConstraintTol(1e-08);
#else
    SP::TimeStepping s(new TimeStepping(t, OSI, osnspb));
#endif
    s->setNewtonTolerance(1e-4);
    s->setNewtonMaxIteration(10);

    // =========================== End of model definition ===========================

    // ================================= Computation =================================

    // --- Simulation initialization ---

    cout << "====> Initialisation ..." << endl << endl;
    bouncingBall->initialize(s);
    int N = ceil((T - t0) / h); // Number of time steps

    // --- Get the values to be plotted ---
    // -> saved in a matrix dataPlot
    unsigned int outputSize = 16;
    SimpleMatrix dataPlot(N + 1, outputSize);

    SP::SiconosVector q = ball->q();
    SP::SiconosVector v = ball->velocity();
    SP::SiconosVector p = ball->p(1);
    SP::SiconosVector lambda = inter->lambda(1);

    dataPlot(0, 0) = bouncingBall->t0();
    dataPlot(0, 1) = (*q)(0);
    dataPlot(0, 2) = (*v)(0);
    dataPlot(0, 3) = (*p)(0);
    dataPlot(0, 4) = (*lambda)(0);
    dataPlot(0, 5) = acos((*q)(3));
    dataPlot(0, 6) = relation0->contactForce()->norm2();
    dataPlot(0, 7) = (*q)(0);
    dataPlot(0, 8) = (*q)(1);
    dataPlot(0, 9) = (*q)(2);
    dataPlot(0, 10) = (*q)(3);
    dataPlot(0, 11) = (*q)(4);
    dataPlot(0, 12) = (*q)(5);
    dataPlot(0, 13) = (*q)(6);
    dataPlot(0, 14) = (*v)(1);
    dataPlot(0, 15) = (*v)(2);

    // --- Time loop ---
    cout << "====> Start computation ... " << endl << endl;
    // ==== Simulation loop - Writing without explicit event handling =====
    int k = 1;
    boost::progress_display show_progress(N);

    boost::timer time;
    time.restart();
    dataPlot(k, 6) = relation0->contactForce()->norm2();
    while (s->hasNextEvent())
    {
      //      s->computeOneStep();
      s->advanceToEvent();
      // --- Get values to be plotted ---
      dataPlot(k, 0) =  s->nextTime();
      dataPlot(k, 1) = (*q)(0);
      dataPlot(k, 2) = (*v)(0);
      dataPlot(k, 3) = (*p)(0);
      dataPlot(k, 4) = (*lambda)(0);
      dataPlot(k, 5) = acos((*q)(3));
      dataPlot(k, 6) = relation0->contactForce()->norm2();
      dataPlot(k, 7) = (*q)(0);
      dataPlot(k, 8) = (*q)(1);
      dataPlot(k, 9) = (*q)(2);
      dataPlot(k, 10) = (*q)(3);
      dataPlot(k, 11) = (*q)(4);
      dataPlot(k, 12) = (*q)(5);
      dataPlot(k, 13) = (*q)(6);
      dataPlot(k, 14) = (*v)(1);
      dataPlot(k, 15) = (*v)(2);
      s->nextStep();
      ++show_progress;
      k++;
    }
    cout << endl << "End of computation - Number of iterations done: " << k - 1 << endl;
    cout << "Computation Time " << time.elapsed()  << endl;

    // --- Output files ---
    cout << "====> Output file writing ..." << endl;
    dataPlot.resize(k, outputSize);
    ioMatrix::write("result.dat", "ascii", dataPlot, "noDim");

    // Comparison with a reference file
    cout << "====> Comparison with a reference file ..." << endl;
    SimpleMatrix dataPlotRef(dataPlot);
    dataPlotRef.zero();
#ifdef WITH_PROJ
    ioMatrix::read("resultNETS-WITHPROJ.ref", "ascii", dataPlotRef);
#else
    ioMatrix::read("resultNETS.ref", "ascii", dataPlotRef);
#endif
    std::cout << "Error w.r.t reference file = " << (dataPlot - dataPlotRef).normInf() << std::endl;

    if ((dataPlot - dataPlotRef).normInf() > 1e-10)
    {
      std::cout << "Warning. The results is rather different from the reference file. err = " << (dataPlot - dataPlotRef).normInf() << std::endl;
      //(dataPlot-dataPlotRef).display();

      double maxerror = -1e+24;
      unsigned int imax = -1;
      unsigned int jmax = -1;
      double error;
      for (unsigned int ii = 0;  ii < dataPlot.size(0); ii++)
      {
        for (unsigned int jj = 0;  jj < dataPlot.size(1); jj++)
        {
          error = std::abs(dataPlot.getValue(ii, jj) - dataPlotRef.getValue(ii, jj)) ;
          if (error > 1e-12)
          {

            std::cout << "error = " << error << std::endl;
            std::cout << "ii  = " << ii << "  jj  = " << jmax << std::endl;
            std::cout << "dataPlot.getValue(ii,jj) = " <<  dataPlot.getValue(ii, jj) << std::endl;
            std::cout << "dataPlotRef.getValue(ii,jj) =" <<  dataPlotRef.getValue(ii, jj) << std::endl;
          }

          if (error > maxerror)
          {
            maxerror = error ;
            imax = ii;
            jmax = jj;
          }
        }
      }
      std::cout << "max error = " << maxerror << std::endl;
      std::cout << "imax  = " << imax << "  jmax  = " << jmax << std::endl;
      std::cout << "dataPlot.getValue(imax,jmax) = " <<  dataPlot.getValue(imax, jmax) << std::endl;
      std::cout << "dataPlotRef.getValue(imax,jmax) =" <<  dataPlotRef.getValue(imax, jmax) << std::endl;

      return 1;
    }
  }

  catch (SiconosException e)
  {
    cout << e.report() << endl;
  }
  catch (...)
  {
    cout << "Exception caught in BouncingBallNETS.cpp" << endl;
  }

}
<commit_msg>fix ref files<commit_after>/* Siconos-sample , Copyright INRIA 2005-2011.
 * Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 * Siconos is a free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * Siconos is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Siconos; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 * Contact: Vincent ACARY vincent.acary@inrialpes.fr
 */

/*!\file BouncingBallNETS.cpp
  \brief \ref EMBouncingBall - C++ input file, Time-Stepping version -
  V. Acary, O. Bonnefon.

  A Ball bouncing on the ground.
  Direct description of the model without XML input.
  Simulation with a Time-Stepping scheme.
*/
#include "SphereNEDSPlanR.hpp"
#include "SiconosKernel.hpp"

#define WITH_PROJ
#define WITH_FC3D
using namespace std;
#ifdef WITH_FC3D
#define R_CLASS NewtonEulerFrom3DLocalFrameR
#else
#define R_CLASS NewtonEulerFrom1DLocalFrameR
#endif

class my_NewtonEulerR : public R_CLASS
{

  double _sBallRadius ;

public:

  my_NewtonEulerR(double radius): R_CLASS(), _sBallRadius(radius) { };

  virtual void computeh(double t, Interaction& inter)
  {
    SP::SiconosVector y = inter.y(0);
    double height = fabs(inter.data(q0)->getValue(0)) - _sBallRadius;
    // std::cout <<"my_NewtonEulerR:: computeh _jachq" << std:: endl;
    // _jachq->display();
    y->setValue(0, height);
    _Nc->setValue(0, 1);
    _Nc->setValue(1, 0);
    _Nc->setValue(2, 0);
    _Pc1->setValue(0, height);
    _Pc1->setValue(1, inter.data(q0)->getValue(1));
    _Pc1->setValue(2, inter.data(q0)->getValue(2));

    //_Pc2->setValue(0,hpc);
    //_Pc2->setValue(1,data[q0]->getValue(1));
    //_Pc2->setValue(2,data[q0]->getValue(2));
    //printf("my_NewtonEulerR N, Pc\n");
    //_Nc->display();
    //_Pc1->display();
  }
  //ACCEPT_VISITORS();
};
TYPEDEF_SPTR(my_NewtonEulerR);

int main(int argc, char* argv[])
{
  try
  {

    // ================= Creation of the model =======================

    // User-defined main parameters
    unsigned int nDof = 3;           // degrees of freedom for the ball
    unsigned int qDim = 7;           // degrees of freedom for the ball
    unsigned int nDim = 6;           // degrees of freedom for the ball
    double t0 = 0;                   // initial computation time
    double T = 10.0;                  // final computation time
    double h = 0.005;                // time step
    double position_init = 1.0;      // initial position for lowest bead.
    double velocity_init = 2.0;      // initial velocity for lowest bead.
    double omega_initx = 0.0;
    double omega_initz = 0.0;// initial velocity for lowest bead.
    double theta = 0.5;              // theta for Moreau integrator
    double m = 1; // Ball mass
    double g = 9.81; // Gravity
    double radius = 0.1;
    // -------------------------
    // --- Dynamical systems ---
    // -------------------------

    cout << "====> Model loading ..." << endl << endl;

    // -- Initial positions and velocities --
    SP::SiconosVector q0(new SiconosVector(qDim));
    SP::SiconosVector v0(new SiconosVector(nDim));
    SP::SimpleMatrix I(new SimpleMatrix(3, 3));
    v0->zero();
    q0->zero();
    I->eye();
    (*q0)(0) = position_init;
    /*initial quaternion equal to (1,0,0,0)*/
    (*q0)(3) = 1.0;

    (*v0)(0) = velocity_init;
    (*v0)(3) = omega_initx;
    (*v0)(5) = omega_initz;
    // -- The dynamical system --
    SP::NewtonEulerDS ball(new NewtonEulerDS(q0, v0, m, I));

    // -- Set external forces (weight) --
    SP::SiconosVector weight(new SiconosVector(nDof));
    (*weight)(0) = -m * g;
    ball->setFExtPtr(weight);

    // --------------------
    // --- Interactions ---
    // --------------------

    // -- nslaw --
    double e = 0.9;

    // Interaction ball-floor
    //

    //     vector<SP::SiconosMatrix> vecMatrix1;
    //     vecMatrix1.push_back(H);
    //     SP::BlockMatrix H_block(new BlockMatrix(vecMatrix1,1,1));

    //     SP::SiconosMatrix HT(new SimpleMatrix(1,nDim));
    //     vector<SP::SiconosMatrix> vecMatrix2;
    //     vecMatrix2.push_back(HT);
    //     SP::BlockMatrix HT_block(new BlockMatrix(vecMatrix2,1,1));

#ifdef WITH_FC3D
    int nslawsize = 3;
    SP::NonSmoothLaw nslaw0(new NewtonImpactFrictionNSL(e, e, 0.6, 3));
#else
    int nslawsize = 1;
    SP::NonSmoothLaw nslaw0(new NewtonImpactNSL(e));
#endif

    //     Version with NewtonEulerR()
    //
    //     SP::SimpleMatrix H(new SimpleMatrix(nslawsize,qDim));
    //     H->zero();
    //     (*H)(0,0) = 1.0;
    // #ifdef WITH_FC3D
    //     (*H)(1,1) = 1.0;
    //     (*H)(2,2) = 1.0;
    // #endif
    //     //SP::NewtonEulerR relation0(new SphereNEDSPlanR(0.1,1.0,0.0,0.0,0.0));
    //     //SP::NewtonEulerR relation0(new NewtonEulerR());
    //     //relation0->setJachq(H);
    //     //    relation0->setJacQH(H_block);
    //     //    relation0->setJacQHT(HT_block);
    //     //cout<<"main jacQH"<<endl;
    //     //relation0->jachq()->display();

    // Version with my_NewtonEulerR()
    SP::NewtonEulerR relation0(new my_NewtonEulerR(radius));
    SP::Interaction inter(new Interaction(nslawsize, nslaw0, relation0));

    // -------------
    // --- Model ---
    // -------------
    SP::Model bouncingBall(new Model(t0, T));
    // add the dynamical system in the non smooth dynamical system
    bouncingBall->nonSmoothDynamicalSystem()->insertDynamicalSystem(ball);

    // link the interaction and the dynamical system
    bouncingBall->nonSmoothDynamicalSystem()->link(inter, ball);

    // ------------------
    // --- Simulation ---
    // ------------------

    // -- (1) OneStepIntegrators --
#ifdef WITH_PROJ
    SP::MoreauProjectOnConstraintsOSI OSI(new MoreauProjectOnConstraintsOSI(ball, theta));
#else
    SP::Moreau OSI(new Moreau(ball, theta));
#endif
    // -- (2) Time discretisation --
    SP::TimeDiscretisation t(new TimeDiscretisation(t0, h));

    // -- (3) one step non smooth problem
    SP::OneStepNSProblem osnspb(new GenericMechanical());
#ifdef WITH_PROJ
    SP::OneStepNSProblem osnspb_pos(new MLCPProjectOnConstraints(SICONOS_MLCP_ENUM, 1.0));
#endif
    // -- (4) Simulation setup with (1) (2) (3)
#ifdef WITH_PROJ
    SP::TimeSteppingProjectOnConstraints s(new TimeSteppingProjectOnConstraints(t, OSI, osnspb, osnspb_pos));
    s->setProjectionMaxIteration(20);
    s->setConstraintTolUnilateral(1e-08);
    s->setConstraintTol(1e-08);
#else
    SP::TimeStepping s(new TimeStepping(t, OSI, osnspb));
#endif
    s->setNewtonTolerance(1e-4);
    s->setNewtonMaxIteration(10);

    // =========================== End of model definition ===========================

    // ================================= Computation =================================

    // --- Simulation initialization ---

    cout << "====> Initialisation ..." << endl << endl;
    bouncingBall->initialize(s);
    int N = ceil((T - t0) / h); // Number of time steps

    // --- Get the values to be plotted ---
    // -> saved in a matrix dataPlot
    unsigned int outputSize = 16;
    SimpleMatrix dataPlot(N + 1, outputSize);

    SP::SiconosVector q = ball->q();
    SP::SiconosVector v = ball->velocity();
    SP::SiconosVector p = ball->p(1);
    SP::SiconosVector lambda = inter->lambda(1);

    dataPlot(0, 0) = bouncingBall->t0();
    dataPlot(0, 1) = (*q)(0);
    dataPlot(0, 2) = (*v)(0);
    dataPlot(0, 3) = (*p)(0);
    dataPlot(0, 4) = (*lambda)(0);
    dataPlot(0, 5) = acos((*q)(3));
    dataPlot(0, 6) = relation0->contactForce()->norm2();
    dataPlot(0, 7) = (*q)(0);
    dataPlot(0, 8) = (*q)(1);
    dataPlot(0, 9) = (*q)(2);
    dataPlot(0, 10) = (*q)(3);
    dataPlot(0, 11) = (*q)(4);
    dataPlot(0, 12) = (*q)(5);
    dataPlot(0, 13) = (*q)(6);
    dataPlot(0, 14) = (*v)(1);
    dataPlot(0, 15) = (*v)(2);

    // --- Time loop ---
    cout << "====> Start computation ... " << endl << endl;
    // ==== Simulation loop - Writing without explicit event handling =====
    int k = 1;
    boost::progress_display show_progress(N);

    boost::timer time;
    time.restart();
    dataPlot(k, 6) = relation0->contactForce()->norm2();
    while (s->hasNextEvent())
    {
      //      s->computeOneStep();
      s->advanceToEvent();
      // --- Get values to be plotted ---
      dataPlot(k, 0) =  s->nextTime();
      dataPlot(k, 1) = (*q)(0);
      dataPlot(k, 2) = (*v)(0);
      dataPlot(k, 3) = (*p)(0);
      dataPlot(k, 4) = (*lambda)(0);
      dataPlot(k, 5) = acos((*q)(3));
      dataPlot(k, 6) = relation0->contactForce()->norm2();
      dataPlot(k, 7) = (*q)(0);
      dataPlot(k, 8) = (*q)(1);
      dataPlot(k, 9) = (*q)(2);
      dataPlot(k, 10) = (*q)(3);
      dataPlot(k, 11) = (*q)(4);
      dataPlot(k, 12) = (*q)(5);
      dataPlot(k, 13) = (*q)(6);
      dataPlot(k, 14) = (*v)(1);
      dataPlot(k, 15) = (*v)(2);
      s->nextStep();
      ++show_progress;
      k++;
    }
    cout << endl << "End of computation - Number of iterations done: " << k - 1 << endl;
    cout << "Computation Time " << time.elapsed()  << endl;

    // --- Output files ---
    cout << "====> Output file writing ..." << endl;
    dataPlot.resize(k, outputSize);
    ioMatrix::write("result.dat", "ascii", dataPlot, "noDim");

    // Comparison with a reference file
    cout << "====> Comparison with a reference file ..." << endl;
    SimpleMatrix dataPlotRef(dataPlot);
    dataPlotRef.zero();
#ifdef WITH_PROJ
    ioMatrix::read("resultNETS-WITHPROJ.ref", "ascii", dataPlotRef);
#else
    ioMatrix::read("resultNETS.ref", "ascii", dataPlotRef);
#endif
    std::cout << "Error w.r.t reference file = " << (dataPlot - dataPlotRef).normInf() << std::endl;

    if ((dataPlot - dataPlotRef).normInf() > 1e-10)
    {
      std::cout << "Warning. The results is rather different from the reference file. err = " << (dataPlot - dataPlotRef).normInf() << std::endl;
      //(dataPlot-dataPlotRef).display();

      double maxerror = -1e+24;
      unsigned int imax = -1;
      unsigned int jmax = -1;
      double error;
      for (unsigned int ii = 0;  ii < dataPlot.size(0); ii++)
      {
        for (unsigned int jj = 0;  jj < dataPlot.size(1); jj++)
        {
          error = std::abs(dataPlot.getValue(ii, jj) - dataPlotRef.getValue(ii, jj)) ;
          if (error > 1e-12)
          {

            std::cout << "error = " << error << std::endl;
            std::cout << "ii  = " << ii << "  jj  = " << jmax << std::endl;
            std::cout << "dataPlot.getValue(ii,jj) = " <<  dataPlot.getValue(ii, jj) << std::endl;
            std::cout << "dataPlotRef.getValue(ii,jj) =" <<  dataPlotRef.getValue(ii, jj) << std::endl;
          }

          if (error > maxerror)
          {
            maxerror = error ;
            imax = ii;
            jmax = jj;
          }
        }
      }
      std::cout << "max error = " << maxerror << std::endl;
      std::cout << "imax  = " << imax << "  jmax  = " << jmax << std::endl;
      std::cout << "dataPlot.getValue(imax,jmax) = " <<  dataPlot.getValue(imax, jmax) << std::endl;
      std::cout << "dataPlotRef.getValue(imax,jmax) =" <<  dataPlotRef.getValue(imax, jmax) << std::endl;

      return 1;
    }
  }

  catch (SiconosException e)
  {
    cout << e.report() << endl;
  }
  catch (...)
  {
    cout << "Exception caught in BouncingBallNETS.cpp" << endl;
  }

}
<|endoftext|>


<commit_before>// Copyright (c) 2015 GitHub, Inc.
// Use of this source code is governed by the MIT license that can be
// found in the LICENSE file.

#include "atom/browser/atom_resource_dispatcher_host_delegate.h"

#include "atom/browser/login_handler.h"
#include "atom/browser/web_contents_permission_helper.h"
#include "atom/common/platform_util.h"
#include "content/public/browser/browser_thread.h"
#include "net/base/escape.h"
#include "url/gurl.h"

using content::BrowserThread;

namespace atom {

namespace {

void OnOpenExternal(const GURL& escaped_url,
                    bool allowed) {
  if (allowed)
    platform_util::OpenExternal(escaped_url, true);
}

void HandleExternalProtocolInUI(
    const GURL& url,
    const content::ResourceRequestInfo::WebContentsGetter& web_contents_getter,
    bool has_user_gesture) {
  content::WebContents* web_contents = web_contents_getter.Run();
  if (!web_contents)
    return;

  GURL escaped_url(net::EscapeExternalHandlerValue(url.spec()));
  auto callback = base::Bind(&OnOpenExternal, escaped_url);
  auto permission_helper =
      WebContentsPermissionHelper::FromWebContents(web_contents);
  permission_helper->RequestOpenExternalPermission(callback, has_user_gesture);
}

}  // namespace

AtomResourceDispatcherHostDelegate::AtomResourceDispatcherHostDelegate() {
}

bool AtomResourceDispatcherHostDelegate::HandleExternalProtocol(
    const GURL& url,
    int child_id,
    const content::ResourceRequestInfo::WebContentsGetter& web_contents_getter,
    bool is_main_frame,
    ui::PageTransition transition,
    bool has_user_gesture) {
  BrowserThread::PostTask(BrowserThread::UI, FROM_HERE,
                          base::Bind(&HandleExternalProtocolInUI,
                                     url,
                                     web_contents_getter,
                                     has_user_gesture));
  return true;
}

content::ResourceDispatcherHostLoginDelegate*
AtomResourceDispatcherHostDelegate::CreateLoginDelegate(
    net::AuthChallengeInfo* auth_info,
    net::URLRequest* request) {
  return new LoginHandler(auth_info, request);
}

}  // namespace atom
<commit_msg>fix dereferencing null pointer<commit_after>// Copyright (c) 2015 GitHub, Inc.
// Use of this source code is governed by the MIT license that can be
// found in the LICENSE file.

#include "atom/browser/atom_resource_dispatcher_host_delegate.h"

#include "atom/browser/login_handler.h"
#include "atom/browser/web_contents_permission_helper.h"
#include "atom/common/platform_util.h"
#include "content/public/browser/browser_thread.h"
#include "net/base/escape.h"
#include "url/gurl.h"

using content::BrowserThread;

namespace atom {

namespace {

void OnOpenExternal(const GURL& escaped_url,
                    bool allowed) {
  if (allowed)
    platform_util::OpenExternal(escaped_url, true);
}

void HandleExternalProtocolInUI(
    const GURL& url,
    const content::ResourceRequestInfo::WebContentsGetter& web_contents_getter,
    bool has_user_gesture) {
  content::WebContents* web_contents = web_contents_getter.Run();
  if (!web_contents)
    return;

  auto permission_helper =
      WebContentsPermissionHelper::FromWebContents(web_contents);
  if (!permission_helper)
    return;

  GURL escaped_url(net::EscapeExternalHandlerValue(url.spec()));
  auto callback = base::Bind(&OnOpenExternal, escaped_url);
  permission_helper->RequestOpenExternalPermission(callback, has_user_gesture);
}

}  // namespace

AtomResourceDispatcherHostDelegate::AtomResourceDispatcherHostDelegate() {
}

bool AtomResourceDispatcherHostDelegate::HandleExternalProtocol(
    const GURL& url,
    int child_id,
    const content::ResourceRequestInfo::WebContentsGetter& web_contents_getter,
    bool is_main_frame,
    ui::PageTransition transition,
    bool has_user_gesture) {
  BrowserThread::PostTask(BrowserThread::UI, FROM_HERE,
                          base::Bind(&HandleExternalProtocolInUI,
                                     url,
                                     web_contents_getter,
                                     has_user_gesture));
  return true;
}

content::ResourceDispatcherHostLoginDelegate*
AtomResourceDispatcherHostDelegate::CreateLoginDelegate(
    net::AuthChallengeInfo* auth_info,
    net::URLRequest* request) {
  return new LoginHandler(auth_info, request);
}

}  // namespace atom
<|endoftext|>


<commit_before>#include <fstream>
#include <pcl/common/angles.h>
#include <pcl/console/parse.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/point_representation.h>

#include <pcl/io/pcd_io.h>
#include <pcl/keypoints/uniform_sampling.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/fpfh.h>
#include <pcl/registration/correspondence_estimation.h>
#include <pcl/registration/correspondence_estimation_normal_shooting.h>
#include <pcl/registration/correspondence_estimation_backprojection.h>
#include <pcl/registration/correspondence_rejection_median_distance.h>
#include <pcl/registration/correspondence_rejection_surface_normal.h>
#include <pcl/registration/transformation_estimation_point_to_plane_lls.h>
#include <pcl/registration/default_convergence_criteria.h>

#include <pcl/visualization/pcl_visualizer.h>

using namespace std;
using namespace sensor_msgs;
using namespace pcl;
using namespace pcl::io;
using namespace pcl::console;
using namespace pcl::registration;
using namespace pcl::visualization;

typedef PointNormal PointT;
typedef PointCloud<PointT> Cloud;
typedef Cloud::Ptr CloudPtr;
typedef Cloud::ConstPtr CloudConstPtr;

CloudPtr src, tgt;

bool rejection = true;
bool visualize = false;

boost::shared_ptr<PCLVisualizer> vis;

////////////////////////////////////////////////////////////////////////////////
void
findCorrespondences (const CloudPtr &src,
                     const CloudPtr &tgt,
                     Correspondences &all_correspondences)
{
  //CorrespondenceEstimationNormalShooting<PointT, PointT, PointT> est;
  //CorrespondenceEstimation<PointT, PointT> est;
  CorrespondenceEstimationBackProjection<PointT, PointT, PointT> est;
  est.setInputSource (src);
  est.setInputTarget (tgt);

  est.setSourceNormals (src);
  est.setTargetNormals (tgt);
  est.setKSearch (10);
  est.determineCorrespondences (all_correspondences);
  //est.determineReciprocalCorrespondences (all_correspondences);
}

////////////////////////////////////////////////////////////////////////////////
void
rejectBadCorrespondences (const CorrespondencesPtr &all_correspondences,
                          const CloudPtr &src,
                          const CloudPtr &tgt,
                          Correspondences &remaining_correspondences)
{
  CorrespondenceRejectorMedianDistance rej;
  rej.setMedianFactor (8.79241104);
  rej.setInputCorrespondences (all_correspondences);

  rej.getCorrespondences (remaining_correspondences);
  return;

  CorrespondencesPtr remaining_correspondences_temp (new Correspondences);
  rej.getCorrespondences (*remaining_correspondences_temp);
  PCL_DEBUG ("[rejectBadCorrespondences] Number of correspondences remaining after rejection: %d\n", remaining_correspondences_temp->size ());

  // Reject if the angle between the normals is really off
  CorrespondenceRejectorSurfaceNormal rej_normals;
  rej_normals.setThreshold (acos (deg2rad (45.0)));
  rej_normals.initializeDataContainer<PointT, PointT> ();
  rej_normals.setInputCloud<PointT> (src);
  rej_normals.setInputNormals<PointT, PointT> (src);
  rej_normals.setInputTarget<PointT> (tgt);
  rej_normals.setTargetNormals<PointT, PointT> (tgt);
  rej_normals.setInputCorrespondences (remaining_correspondences_temp);
  rej_normals.getCorrespondences (remaining_correspondences);
}

////////////////////////////////////////////////////////////////////////////////
void
findTransformation (const CloudPtr &src,
                    const CloudPtr &tgt,
                    const CorrespondencesPtr &correspondences,
                    Eigen::Matrix4d &transform)
{
  TransformationEstimationPointToPlaneLLS<PointT, PointT, double> trans_est;
  trans_est.estimateRigidTransformation (*src, *tgt, *correspondences, transform);
}

////////////////////////////////////////////////////////////////////////////////
void
view (const CloudConstPtr &src, const CloudConstPtr &tgt, const CorrespondencesPtr &correspondences)
{
  if (!visualize || !vis) return;
  PointCloudColorHandlerCustom<PointT> green (tgt, 0, 255, 0);
  if (!vis->updatePointCloud<PointT> (src, "source"))
  {
    vis->addPointCloud<PointT> (src, "source");
    vis->resetCameraViewpoint ("source");
  }
  if (!vis->updatePointCloud<PointT> (tgt, green, "target")) vis->addPointCloud<PointT> (tgt, green, "target");
  vis->setPointCloudRenderingProperties (PCL_VISUALIZER_OPACITY, 0.5, "source");
  vis->setPointCloudRenderingProperties (PCL_VISUALIZER_OPACITY, 0.7, "target");
  vis->setPointCloudRenderingProperties (PCL_VISUALIZER_POINT_SIZE, 6, "source");
  TicToc tt;
  tt.tic ();
  if (!vis->updateCorrespondences<PointT> (src, tgt, *correspondences, 1))
    vis->addCorrespondences<PointT> (src, tgt, *correspondences, 1, "correspondences");
  tt.toc_print ();
  vis->setShapeRenderingProperties (PCL_VISUALIZER_LINE_WIDTH, 5, "correspondences");
  //vis->setShapeRenderingProperties (PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "correspondences");
  vis->spin ();
}

////////////////////////////////////////////////////////////////////////////////
void
icp (const PointCloud<PointT>::Ptr &src,
     const PointCloud<PointT>::Ptr &tgt,
     Eigen::Matrix4d &transform)
{
  CorrespondencesPtr all_correspondences (new Correspondences),
                     good_correspondences (new Correspondences);

  PointCloud<PointT>::Ptr output (new PointCloud<PointT>);
  *output = *src;

  Eigen::Matrix4d final_transform (Eigen::Matrix4d::Identity ());

  int iterations = 0;
  DefaultConvergenceCriteria<double> converged (iterations, transform, *good_correspondences);

  // ICP loop
  do
  {
    // Find correspondences
    findCorrespondences (output, tgt, *all_correspondences);
    PCL_DEBUG ("Number of correspondences found: %d\n", all_correspondences->size ());

    if (rejection)
    {
      // Reject correspondences
      rejectBadCorrespondences (all_correspondences, output, tgt, *good_correspondences);
      PCL_DEBUG ("Number of correspondences remaining after rejection: %d\n", good_correspondences->size ());
    }
    else
      *good_correspondences = *all_correspondences;

    // Find transformation
    findTransformation (output, tgt, good_correspondences, transform);

    // Obtain the final transformation
    final_transform = transform * final_transform;

    // Transform the data
    transformPointCloudWithNormals (*src, *output, final_transform.cast<float> ());

    // Check if convergence has been reached
    ++iterations;

    // Visualize the results
    view (output, tgt, good_correspondences);
  }
  while (!converged);
  transform = final_transform;
}

////////////////////////////////////////////////////////////////////////////////
void
saveTransform (const std::string &file, const Eigen::Matrix4d &transform)
{
  ofstream ofs;
  ofs.open (file.c_str (), ios::trunc | ios::binary);
  for (int i = 0; i < 4; ++i)
    for (int j = 0; j < 4; ++j)
      ofs.write (reinterpret_cast<const char*>(&transform (i, j)), sizeof (double));
  ofs.close ();
}

/* ---[ */
int
main (int argc, char** argv)
{
  // Check whether we want to enable debug mode
  bool debug = false;
  parse_argument (argc, argv, "-debug", debug);
  if (debug)
    setVerbosityLevel (L_DEBUG);

  parse_argument (argc, argv, "-rejection", rejection);
  parse_argument (argc, argv, "-visualization", visualize);
  if (visualize)
    vis.reset (new PCLVisualizer ("Registration example"));

  // Parse the command line arguments for .pcd and .transform files
  std::vector<int> p_pcd_file_indices, p_tr_file_indices;
  p_pcd_file_indices = parse_file_extension_argument (argc, argv, ".pcd");
  if (p_pcd_file_indices.size () != 2)
  {
    print_error ("Need one input source PCD file and one input target PCD file to continue.\n");
    print_error ("Example: %s source.pcd target.pcd output.transform\n", argv[0]);
    return (-1);
  }
  p_tr_file_indices = parse_file_extension_argument (argc, argv, ".transform");
  if (p_tr_file_indices.size () != 1)
  {
    print_error ("Need one output transform file to continue.\n");
    print_error ("Example: %s source.pcd target.pcd output.transform\n", argv[0]);
    return (-1);
  }

  // Load the files
  print_info ("Loading %s as source and %s as target...\n", argv[p_pcd_file_indices[0]], argv[p_pcd_file_indices[1]]);
  src.reset (new PointCloud<PointT>);
  tgt.reset (new PointCloud<PointT>);
  if (loadPCDFile (argv[p_pcd_file_indices[0]], *src) == -1 || loadPCDFile (argv[p_pcd_file_indices[1]], *tgt) == -1)
  {
    print_error ("Error reading the input files!\n");
    return (-1);
  }

  // Compute the best transformtion
  Eigen::Matrix4d transform;
  icp (src, tgt, transform);

  saveTransform (argv[p_tr_file_indices[0]], transform);

  cerr.precision (15);
  std::cerr << transform << std::endl;
}
/* ]--- */
<commit_msg>#include <fstream> #include <pcl/common/angles.h> #include <pcl/console/parse.h> #include <pcl/point_types.h> #include <pcl/point_cloud.h> #include <pcl/point_representation.h><commit_after>#include <fstream>
#include <pcl/common/angles.h>
#include <pcl/console/parse.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/point_representation.h>

#include <pcl/io/pcd_io.h>
#include <pcl/console/time.h>
#include <pcl/keypoints/uniform_sampling.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/fpfh.h>
#include <pcl/registration/correspondence_estimation.h>
#include <pcl/registration/correspondence_estimation_normal_shooting.h>
#include <pcl/registration/correspondence_estimation_backprojection.h>
#include <pcl/registration/correspondence_rejection_median_distance.h>
#include <pcl/registration/correspondence_rejection_surface_normal.h>
#include <pcl/registration/transformation_estimation_point_to_plane_lls.h>
#include <pcl/registration/default_convergence_criteria.h>

#include <pcl/visualization/pcl_visualizer.h>

using namespace std;
using namespace sensor_msgs;
using namespace pcl;
using namespace pcl::io;
using namespace pcl::console;
using namespace pcl::registration;
using namespace pcl::visualization;

typedef PointNormal PointT;
typedef PointCloud<PointT> Cloud;
typedef Cloud::Ptr CloudPtr;
typedef Cloud::ConstPtr CloudConstPtr;

CloudPtr src, tgt;

bool rejection = true;
bool visualize = false;

boost::shared_ptr<PCLVisualizer> vis;

////////////////////////////////////////////////////////////////////////////////
void
findCorrespondences (const CloudPtr &src,
                     const CloudPtr &tgt,
                     Correspondences &all_correspondences)
{
  //CorrespondenceEstimationNormalShooting<PointT, PointT, PointT> est;
  //CorrespondenceEstimation<PointT, PointT> est;
  CorrespondenceEstimationBackProjection<PointT, PointT, PointT> est;
  est.setInputSource (src);
  est.setInputTarget (tgt);

  est.setSourceNormals (src);
  est.setTargetNormals (tgt);
  est.setKSearch (10);
  est.determineCorrespondences (all_correspondences);
  //est.determineReciprocalCorrespondences (all_correspondences);
}

////////////////////////////////////////////////////////////////////////////////
void
rejectBadCorrespondences (const CorrespondencesPtr &all_correspondences,
                          const CloudPtr &src,
                          const CloudPtr &tgt,
                          Correspondences &remaining_correspondences)
{
  CorrespondenceRejectorMedianDistance rej;
  rej.setMedianFactor (8.79241104);
  rej.setInputCorrespondences (all_correspondences);

  rej.getCorrespondences (remaining_correspondences);
  return;

  CorrespondencesPtr remaining_correspondences_temp (new Correspondences);
  rej.getCorrespondences (*remaining_correspondences_temp);
  PCL_DEBUG ("[rejectBadCorrespondences] Number of correspondences remaining after rejection: %d\n", remaining_correspondences_temp->size ());

  // Reject if the angle between the normals is really off
  CorrespondenceRejectorSurfaceNormal rej_normals;
  rej_normals.setThreshold (acos (deg2rad (45.0)));
  rej_normals.initializeDataContainer<PointT, PointT> ();
  rej_normals.setInputCloud<PointT> (src);
  rej_normals.setInputNormals<PointT, PointT> (src);
  rej_normals.setInputTarget<PointT> (tgt);
  rej_normals.setTargetNormals<PointT, PointT> (tgt);
  rej_normals.setInputCorrespondences (remaining_correspondences_temp);
  rej_normals.getCorrespondences (remaining_correspondences);
}

////////////////////////////////////////////////////////////////////////////////
void
findTransformation (const CloudPtr &src,
                    const CloudPtr &tgt,
                    const CorrespondencesPtr &correspondences,
                    Eigen::Matrix4d &transform)
{
  TransformationEstimationPointToPlaneLLS<PointT, PointT, double> trans_est;
  trans_est.estimateRigidTransformation (*src, *tgt, *correspondences, transform);
}

////////////////////////////////////////////////////////////////////////////////
void
view (const CloudConstPtr &src, const CloudConstPtr &tgt, const CorrespondencesPtr &correspondences)
{
  if (!visualize || !vis) return;
  PointCloudColorHandlerCustom<PointT> green (tgt, 0, 255, 0);
  if (!vis->updatePointCloud<PointT> (src, "source"))
  {
    vis->addPointCloud<PointT> (src, "source");
    vis->resetCameraViewpoint ("source");
  }
  if (!vis->updatePointCloud<PointT> (tgt, green, "target")) vis->addPointCloud<PointT> (tgt, green, "target");
  vis->setPointCloudRenderingProperties (PCL_VISUALIZER_OPACITY, 0.5, "source");
  vis->setPointCloudRenderingProperties (PCL_VISUALIZER_OPACITY, 0.7, "target");
  vis->setPointCloudRenderingProperties (PCL_VISUALIZER_POINT_SIZE, 6, "source");
  pcl::console::TicToc tt;
  tt.tic ();
  if (!vis->updateCorrespondences<PointT> (src, tgt, *correspondences, 1))
    vis->addCorrespondences<PointT> (src, tgt, *correspondences, 1, "correspondences");
  tt.toc_print ();
  vis->setShapeRenderingProperties (PCL_VISUALIZER_LINE_WIDTH, 5, "correspondences");
  //vis->setShapeRenderingProperties (PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "correspondences");
  vis->spin ();
}

////////////////////////////////////////////////////////////////////////////////
void
icp (const PointCloud<PointT>::Ptr &src,
     const PointCloud<PointT>::Ptr &tgt,
     Eigen::Matrix4d &transform)
{
  CorrespondencesPtr all_correspondences (new Correspondences),
                     good_correspondences (new Correspondences);

  PointCloud<PointT>::Ptr output (new PointCloud<PointT>);
  *output = *src;

  Eigen::Matrix4d final_transform (Eigen::Matrix4d::Identity ());

  int iterations = 0;
  DefaultConvergenceCriteria<double> converged (iterations, transform, *good_correspondences);

  // ICP loop
  do
  {
    // Find correspondences
    findCorrespondences (output, tgt, *all_correspondences);
    PCL_DEBUG ("Number of correspondences found: %d\n", all_correspondences->size ());

    if (rejection)
    {
      // Reject correspondences
      rejectBadCorrespondences (all_correspondences, output, tgt, *good_correspondences);
      PCL_DEBUG ("Number of correspondences remaining after rejection: %d\n", good_correspondences->size ());
    }
    else
      *good_correspondences = *all_correspondences;

    // Find transformation
    findTransformation (output, tgt, good_correspondences, transform);

    // Obtain the final transformation
    final_transform = transform * final_transform;

    // Transform the data
    transformPointCloudWithNormals (*src, *output, final_transform.cast<float> ());

    // Check if convergence has been reached
    ++iterations;

    // Visualize the results
    view (output, tgt, good_correspondences);
  }
  while (!converged);
  transform = final_transform;
}

////////////////////////////////////////////////////////////////////////////////
void
saveTransform (const std::string &file, const Eigen::Matrix4d &transform)
{
  ofstream ofs;
  ofs.open (file.c_str (), ios::trunc | ios::binary);
  for (int i = 0; i < 4; ++i)
    for (int j = 0; j < 4; ++j)
      ofs.write (reinterpret_cast<const char*>(&transform (i, j)), sizeof (double));
  ofs.close ();
}

/* ---[ */
int
main (int argc, char** argv)
{
  // Check whether we want to enable debug mode
  bool debug = false;
  parse_argument (argc, argv, "-debug", debug);
  if (debug)
    setVerbosityLevel (L_DEBUG);

  parse_argument (argc, argv, "-rejection", rejection);
  parse_argument (argc, argv, "-visualization", visualize);
  if (visualize)
    vis.reset (new PCLVisualizer ("Registration example"));

  // Parse the command line arguments for .pcd and .transform files
  std::vector<int> p_pcd_file_indices, p_tr_file_indices;
  p_pcd_file_indices = parse_file_extension_argument (argc, argv, ".pcd");
  if (p_pcd_file_indices.size () != 2)
  {
    print_error ("Need one input source PCD file and one input target PCD file to continue.\n");
    print_error ("Example: %s source.pcd target.pcd output.transform\n", argv[0]);
    return (-1);
  }
  p_tr_file_indices = parse_file_extension_argument (argc, argv, ".transform");
  if (p_tr_file_indices.size () != 1)
  {
    print_error ("Need one output transform file to continue.\n");
    print_error ("Example: %s source.pcd target.pcd output.transform\n", argv[0]);
    return (-1);
  }

  // Load the files
  print_info ("Loading %s as source and %s as target...\n", argv[p_pcd_file_indices[0]], argv[p_pcd_file_indices[1]]);
  src.reset (new PointCloud<PointT>);
  tgt.reset (new PointCloud<PointT>);
  if (loadPCDFile (argv[p_pcd_file_indices[0]], *src) == -1 || loadPCDFile (argv[p_pcd_file_indices[1]], *tgt) == -1)
  {
    print_error ("Error reading the input files!\n");
    return (-1);
  }

  // Compute the best transformtion
  Eigen::Matrix4d transform;
  icp (src, tgt, transform);

  saveTransform (argv[p_tr_file_indices[0]], transform);

  cerr.precision (15);
  std::cerr << transform << std::endl;
}
/* ]--- */
<|endoftext|>