<commit_before>// =============================================================================
// PROJECT CHRONO - http://projectchrono.org
//
// Copyright (c) 2014 projectchrono.org
// All rights reserved.
//
// Use of this source code is governed by a BSD-style license that can be found
// in the LICENSE file at the top level of the distribution and at
// http://projectchrono.org/license-chrono.txt.
//
// =============================================================================
// Authors: Alessandro Tasora
// =============================================================================
//
// FEA for 3D beams
//
// =============================================================================

#include "chrono/physics/ChSystemNSC.h"
#include "chrono/physics/ChSystemSMC.h"
#include "chrono/physics/ChLinkMate.h"
#include "chrono/physics/ChBodyEasy.h"
#include "chrono/physics/ChLinkMotorRotationSpeed.h"
#include "chrono/timestepper/ChTimestepper.h"
#include "chrono/solver/ChSolverMINRES.h"
#include "chrono/utils/ChUtilsCreators.h"
#include "chrono/collision/ChCCollisionSystemBullet.h"

#include "chrono_fea/ChElementBeamEuler.h"
#include "chrono_fea/ChBuilderBeam.h"
#include "chrono_fea/ChMesh.h"
#include "chrono_fea/ChVisualizationFEAmesh.h"
#include "chrono_fea/ChLinkPointFrame.h"
#include "chrono_fea/ChLinkDirFrame.h"
#include "chrono_fea/ChContactSurfaceMesh.h"
#include "chrono_fea/ChContactSurfaceNodeCloud.h"
#include "chrono_irrlicht/ChIrrApp.h"

#include "chrono_mkl/ChSolverMKL.h"

using namespace chrono;
using namespace chrono::fea;
using namespace chrono::irrlicht;

using namespace irr;

// A helper function that creates a'lobed gear', almost a macro user in main()
// to quickly create one or two rotating obstacles for the extruding beam

std::shared_ptr<ChBody> CreateLobedGear (
            ChVector<> gear_center,
            int    lobe_copies,
            double lobe_width,
            double lobe_primitive_rad,
            double lobe_inner_rad,
            double lobe_outer_rad,
            double lobe_thickness,
            ChSystem& my_system,
            std::shared_ptr<ChMaterialSurface> mysurfmaterial
            )  {

    auto mgear = std::make_shared<ChBody>();
    mgear->SetMaterialSurface(mysurfmaterial);
    mgear->SetPos(gear_center);
    my_system.Add(mgear);

    // add cylindrical lobes
    mgear->GetCollisionModel()->ClearModel();
    for (int i = 0; i< lobe_copies; ++i) {
        double phase = CH_C_2PI * ((double)i/(double)lobe_copies);
        // this is a quick shortcut from ChUtilsCreators.h,
        // it both adds the collision shape and the visualization asset:
        /*
        utils::AddCylinderGeometry(
            mgear.get(),
            lobe_width*0.5,
            lobe_thickness*0.5,
            ChVector<>(lobe_primitive_rad*sin(phase), lobe_primitive_rad*cos(phase),0),
            Q_from_AngAxis(CH_C_PI_2, VECT_X), // rotate cylinder axis: from default on Y axis, to Z axis
            true);
        */
        utils::AddBoxGeometry(
            mgear.get(),
            ChVector<>(lobe_width, lobe_outer_rad-lobe_inner_rad, lobe_thickness)*0.5, // half size used in this function
            ChVector<>(0.5*(lobe_outer_rad+lobe_inner_rad)*sin(phase), 0.5*(lobe_outer_rad+lobe_inner_rad)*cos(phase),0),
            Q_from_AngAxis(-phase, VECT_Z), // rotate cylinder axis: from default on Y axis, to Z axis
            true);
    }
    utils::AddCylinderGeometry( mgear.get(), lobe_inner_rad, lobe_thickness*0.5,  ChVector<>(0,0,0), Q_from_AngAxis(CH_C_PI_2, VECT_X), true);
    mgear->GetCollisionModel()->BuildModel();
    mgear->SetCollide(true);

    return mgear;
}

int main(int argc, char* argv[]) {
    GetLog() << "Copyright (c) 2017 projectchrono.org\nChrono version: " << CHRONO_VERSION << "\n\n";

    // Create a Chrono::Engine physical system
    ChSystemSMC my_system;

    // Here set the inward-outward margins for collision shapes: should make sense in the scale of the model
    collision::ChCollisionModel::SetDefaultSuggestedEnvelope(0.001);
    collision::ChCollisionModel::SetDefaultSuggestedMargin(0.002);
    collision::ChCollisionSystemBullet::SetContactBreakingThreshold(0.0001);

    // Create a ground object, useful reference for connecting constraints etc.
    auto mground = std::make_shared<ChBody>();
    mground->SetBodyFixed(true);
    my_system.Add(mground);

    // Create a mesh, that is a container for groups
    // of elements and their referenced nodes.

    auto my_mesh = std::make_shared<ChMesh>();
    my_system.Add(my_mesh);

    // Create a section, i.e. thickness and material properties
    // for beams. This will be shared among some beams.

    auto msection = std::make_shared<ChBeamSectionAdvanced>();

    double wire_diameter = 0.012;
    msection->SetAsCircularSection(wire_diameter);
    msection->SetYoungModulus(0.01e9);  // not exactly a steel wire...
    msection->SetGshearModulus(0.01e9 * 0.3);
    msection->SetBeamRaleyghDamping(0.1);

    // Create the surface material for the contacts; this contains information about friction etc.
    // It is a SMC (penalty) material: interpenetration might happen for low Young stiffness,
    // but unstable simulation might happen for high stiffness, requiring smaller timesteps.

    // option A: Hertz contact force model
    my_system.SetContactForceModel(ChSystemSMC::ContactForceModel::Hertz);
    auto mysurfmaterial = std::make_shared<ChMaterialSurfaceSMC>();
    mysurfmaterial->SetYoungModulus(12e3);  // to adjust heuristically..
    mysurfmaterial->SetRestitution(0.1f);
    mysurfmaterial->SetFriction(0.2f);
    /*
    // Option B: Hooke force model
    my_system.SetContactForceModel(ChSystemSMC::ContactForceModel::Hooke);
    //my_system.UseMaterialProperties(false);
    auto mysurfmaterial = std::make_shared<ChMaterialSurfaceSMC>();
    mysurfmaterial->SetKn(100); // contact normal stiffness
    mysurfmaterial->SetKt(100); // contact tangential stiffness
    mysurfmaterial->SetGn(100);   // contact normal damping
    mysurfmaterial->SetGt(100);   // contact tangential damping
    mysurfmaterial->SetFriction(0.2f);
    */

    //
    // Add the EXTRUDER
    //

    auto extruder = std::make_shared<ChExtruderBeamEuler>(
            &my_system,                 // the physical system
            my_mesh,                    // the mesh where to add the beams
            msection,                   // section for created beam
            0.020,                        // beam element length (size of discretization: the smaller, the more precise)
            ChCoordsys<>(ChVector<>(0,0,0)), // outlet coordinate system (x axis is the extrusion dir)
            0.04                         // the extrusion speed
            );

    // Enable collision for extruded beam
    extruder->SetContact( mysurfmaterial,  // the NSC material for contact surfaces
                          1.15*wire_diameter*0.5  // the radius of the collision spheres at the nodes, (enlarge 15%)
                          );

    //
    // Add some other beams
    //
    // ***NOTE: hack! this is needed because if the extruder starts with 0 beams in the scene, the
    //    ChVisualizationFEAmesh does not visualize any of the newly generated beams by extrusion. Must be fixed.

    double beam_L = 0.1;

    auto hnode1 = std::make_shared<ChNodeFEAxyzrot>(ChFrame<>(ChVector<>(-0.4, 0, 0)));
    auto hnode2 = std::make_shared<ChNodeFEAxyzrot>(ChFrame<>(ChVector<>(-0.4+beam_L, 0, 0)));
    my_mesh->AddNode(hnode1);
    my_mesh->AddNode(hnode2);

    auto belement1 = std::make_shared<ChElementBeamEuler>();
    belement1->SetNodes(hnode1, hnode2);
    belement1->SetSection(msection);

    my_mesh->AddElement(belement1);
    // Fix a node to ground - the easy way, without constraints
    hnode1->SetFixed(true);

    //
    // Attach a visualization of the FEM mesh.
    //

    auto mvisualizebeamA = std::make_shared<ChVisualizationFEAmesh>(*(my_mesh.get()));
    mvisualizebeamA->SetFEMdataType(ChVisualizationFEAmesh::E_PLOT_ELEM_BEAM_MZ);
    mvisualizebeamA->SetColorscaleMinMax(-0.4, 0.4);
    mvisualizebeamA->SetSmoothFaces(true);
    mvisualizebeamA->SetWireframe(false);
    my_mesh->AddAsset(mvisualizebeamA);

    auto mvisualizebeamC = std::make_shared<ChVisualizationFEAmesh>(*(my_mesh.get()));
    mvisualizebeamC->SetFEMglyphType(ChVisualizationFEAmesh::E_GLYPH_NODE_CSYS);
    mvisualizebeamC->SetFEMdataType(ChVisualizationFEAmesh::E_PLOT_NONE);
    mvisualizebeamC->SetSymbolsThickness(0.006);
    mvisualizebeamC->SetSymbolsScale(0.01);
    mvisualizebeamC->SetZbufferHide(false);
    my_mesh->AddAsset(mvisualizebeamC);

    //
    // Add some obstacles. two rotating lobed gears.
    //
    // Here create two rotating lobed gears, just for fun, that wil trap the
    // extruded beam. To quickly create them, use the CreateLobedGear() function
    // implemented at the top of this file.
    // Also, create two simple constant speed motors to rotate the lobed gears.

    int    lobe_copies = 8;
    double lobe_width = 0.03;
    double lobe_primitive_rad = 0.3;
    double lobe_inner_rad = 0.23;
    double lobe_outer_rad = 0.34;
    double lobe_thickness = 0.08;
    ChVector<> gear_centerLOW(0.4,-lobe_primitive_rad,0);
    ChVector<> gear_centerHI (0.4, lobe_primitive_rad,0);

    auto gearLOW =  CreateLobedGear (gear_centerLOW, lobe_copies, lobe_width, lobe_primitive_rad,
                    lobe_inner_rad, lobe_outer_rad, lobe_thickness, my_system, mysurfmaterial);

    auto mgear_motorLOW = std::make_shared<ChLinkMotorRotationSpeed>();
    mgear_motorLOW->Initialize(gearLOW, mground, ChFrame<>(gear_centerLOW));
    my_system.Add(mgear_motorLOW);

    auto mgear_speedLOW = std::make_shared<ChFunction_Const>(-0.1); // [rad/s]
    mgear_motorLOW->SetSpeedFunction(mgear_speedLOW);

    auto gearHI =  CreateLobedGear (gear_centerHI, lobe_copies, lobe_width, lobe_primitive_rad,
                    lobe_inner_rad, lobe_outer_rad, lobe_thickness, my_system, mysurfmaterial);
    gearHI->SetRot(Q_from_AngZ(0.5*CH_C_2PI/lobe_copies)); // to phase half step respect to other gear

    auto mgear_motorHI = std::make_shared<ChLinkMotorRotationSpeed>();
    mgear_motorHI->Initialize(gearHI, mground, ChFrame<>(gear_centerHI));
    my_system.Add(mgear_motorHI);

    auto mgear_speedHI = std::make_shared<ChFunction_Const>( 0.1); // [rad/s]
    mgear_motorHI->SetSpeedFunction(mgear_speedHI);

    // Create the Irrlicht visualization (open the Irrlicht device,
    // bind a simple user interface, etc. etc.)
    ChIrrApp application(&my_system, L"Beam continuous extrusion and FEA contacts", core::dimension2d<u32>(800, 600),
                         false, true);

    // Easy shortcuts to add camera, lights, logo and sky in Irrlicht scene:
    application.AddTypicalLogo();
    application.AddTypicalSky();
    application.AddTypicalLights();
    application.AddTypicalCamera(core::vector3df(-0.1f, 0.2f, -0.2f));

    // ==IMPORTANT!== Use this function for adding a ChIrrNodeAsset to all items
    // in the system. These ChIrrNodeAsset assets are 'proxies' to the Irrlicht meshes.
    // If you need a finer control on which item really needs a visualization proxy in
    // Irrlicht, just use application.AssetBind(myitem); on a per-item basis.

    application.AssetBindAll();

    // ==IMPORTANT!== Use this function for 'converting' into Irrlicht meshes the assets
    // that you added to the bodies into 3D shapes, they can be visualized by Irrlicht!

    application.AssetUpdateAll();

    // Mark completion of system construction
    my_system.SetupInitial();

    //
    // THE SOFT-REAL-TIME CYCLE
    //

    my_system.SetSolverType(ChSolver::Type::MINRES);
    my_system.SetSolverWarmStarting(true);  // this helps a lot to speedup convergence in this class of problems
    my_system.SetMaxItersSolverSpeed(460);
    my_system.SetMaxItersSolverStab(460);
    my_system.SetTolForce(1e-13);
    auto msolver = std::static_pointer_cast<ChSolverMINRES>(my_system.GetSolver());
    msolver->SetVerbose(false);
    msolver->SetDiagonalPreconditioning(true);

    auto mkl_solver = std::make_shared<ChSolverMKL<>>();
    my_system.SetSolver(mkl_solver);

    application.SetTimestep(0.001);
    application.SetVideoframeSaveInterval(20);
    while (application.GetDevice()->run()) {
        application.BeginScene();

        application.DrawAll();
        ChIrrTools::drawGrid(application.GetVideoDriver(), 0.1, 0.1,20,20,CSYSNORM, irr::video::SColor(255,100,100,100),true);

        application.DoStep();

        extruder->Update();    //***REMEMBER*** to do this to update the extrusion

        application.EndScene();
    }

    return 0;
}

<commit_msg>test extrusion of IGA<commit_after>// =============================================================================
// PROJECT CHRONO - http://projectchrono.org
//
// Copyright (c) 2014 projectchrono.org
// All rights reserved.
//
// Use of this source code is governed by a BSD-style license that can be found
// in the LICENSE file at the top level of the distribution and at
// http://projectchrono.org/license-chrono.txt.
//
// =============================================================================
// Authors: Alessandro Tasora
// =============================================================================
//
// FEA for 3D beams
//
// =============================================================================

#include "chrono/physics/ChSystemNSC.h"
#include "chrono/physics/ChSystemSMC.h"
#include "chrono/physics/ChLinkMate.h"
#include "chrono/physics/ChBodyEasy.h"
#include "chrono/physics/ChLinkMotorRotationSpeed.h"
#include "chrono/timestepper/ChTimestepper.h"
#include "chrono/solver/ChSolverMINRES.h"
#include "chrono/utils/ChUtilsCreators.h"
#include "chrono/collision/ChCCollisionSystemBullet.h"

#include "chrono_fea/ChElementBeamEuler.h"
#include "chrono_fea/ChBuilderBeam.h"
#include "chrono_fea/ChMesh.h"
#include "chrono_fea/ChVisualizationFEAmesh.h"
#include "chrono_fea/ChLinkPointFrame.h"
#include "chrono_fea/ChLinkDirFrame.h"
#include "chrono_fea/ChContactSurfaceMesh.h"
#include "chrono_fea/ChContactSurfaceNodeCloud.h"
#include "chrono_irrlicht/ChIrrApp.h"

#include "chrono_mkl/ChSolverMKL.h"

using namespace chrono;
using namespace chrono::fea;
using namespace chrono::irrlicht;

using namespace irr;

// A helper function that creates a'lobed gear', almost a macro user in main()
// to quickly create one or two rotating obstacles for the extruding beam

std::shared_ptr<ChBody> CreateLobedGear (
            ChVector<> gear_center,
            int    lobe_copies,
            double lobe_width,
            double lobe_primitive_rad,
            double lobe_inner_rad,
            double lobe_outer_rad,
            double lobe_thickness,
            ChSystem& my_system,
            std::shared_ptr<ChMaterialSurface> mysurfmaterial
            )  {

    auto mgear = std::make_shared<ChBody>();
    mgear->SetMaterialSurface(mysurfmaterial);
    mgear->SetPos(gear_center);
    my_system.Add(mgear);

    // add cylindrical lobes
    mgear->GetCollisionModel()->ClearModel();
    for (int i = 0; i< lobe_copies; ++i) {
        double phase = CH_C_2PI * ((double)i/(double)lobe_copies);
        // this is a quick shortcut from ChUtilsCreators.h,
        // it both adds the collision shape and the visualization asset:
        /*
        utils::AddCylinderGeometry(
            mgear.get(),
            lobe_width*0.5,
            lobe_thickness*0.5,
            ChVector<>(lobe_primitive_rad*sin(phase), lobe_primitive_rad*cos(phase),0),
            Q_from_AngAxis(CH_C_PI_2, VECT_X), // rotate cylinder axis: from default on Y axis, to Z axis
            true);
        */
        utils::AddBoxGeometry(
            mgear.get(),
            ChVector<>(lobe_width, lobe_outer_rad-lobe_inner_rad, lobe_thickness)*0.5, // half size used in this function
            ChVector<>(0.5*(lobe_outer_rad+lobe_inner_rad)*sin(phase), 0.5*(lobe_outer_rad+lobe_inner_rad)*cos(phase),0),
            Q_from_AngAxis(-phase, VECT_Z), // rotate cylinder axis: from default on Y axis, to Z axis
            true);
    }
    utils::AddCylinderGeometry( mgear.get(), lobe_inner_rad, lobe_thickness*0.5,  ChVector<>(0,0,0), Q_from_AngAxis(CH_C_PI_2, VECT_X), true);
    mgear->GetCollisionModel()->BuildModel();
    mgear->SetCollide(true);

    return mgear;
}

int main(int argc, char* argv[]) {
    GetLog() << "Copyright (c) 2017 projectchrono.org\nChrono version: " << CHRONO_VERSION << "\n\n";

    // Create a Chrono::Engine physical system
    ChSystemSMC my_system;

    // Here set the inward-outward margins for collision shapes: should make sense in the scale of the model
    collision::ChCollisionModel::SetDefaultSuggestedEnvelope(0.001);
    collision::ChCollisionModel::SetDefaultSuggestedMargin(0.002);
    collision::ChCollisionSystemBullet::SetContactBreakingThreshold(0.0001);

    // Create a ground object, useful reference for connecting constraints etc.
    auto mground = std::make_shared<ChBody>();
    mground->SetBodyFixed(true);
    my_system.Add(mground);

    // Create a mesh, that is a container for groups
    // of elements and their referenced nodes.

    auto my_mesh = std::make_shared<ChMesh>();
    my_system.Add(my_mesh);

    // Create a section, i.e. thickness and material properties
    // for beams. This will be shared among some beams.

    auto msection = std::make_shared<ChBeamSectionAdvanced>();

    double wire_diameter = 0.012;
    msection->SetAsCircularSection(wire_diameter);
    msection->SetYoungModulus(0.01e9);  // not exactly a steel wire...
    msection->SetGshearModulus(0.01e9 * 0.3);
    msection->SetBeamRaleyghDamping(0.1);

    // Create the surface material for the contacts; this contains information about friction etc.
    // It is a SMC (penalty) material: interpenetration might happen for low Young stiffness,
    // but unstable simulation might happen for high stiffness, requiring smaller timesteps.

    // option A: Hertz contact force model
    my_system.SetContactForceModel(ChSystemSMC::ContactForceModel::Hertz);
    auto mysurfmaterial = std::make_shared<ChMaterialSurfaceSMC>();
    mysurfmaterial->SetYoungModulus(12e3);  // to adjust heuristically..
    mysurfmaterial->SetRestitution(0.1f);
    mysurfmaterial->SetFriction(0.2f);
    /*
    // Option B: Hooke force model
    my_system.SetContactForceModel(ChSystemSMC::ContactForceModel::Hooke);
    //my_system.UseMaterialProperties(false);
    auto mysurfmaterial = std::make_shared<ChMaterialSurfaceSMC>();
    mysurfmaterial->SetKn(100); // contact normal stiffness
    mysurfmaterial->SetKt(100); // contact tangential stiffness
    mysurfmaterial->SetGn(100);   // contact normal damping
    mysurfmaterial->SetGt(100);   // contact tangential damping
    mysurfmaterial->SetFriction(0.2f);
    */

    //
    // Add the EXTRUDER
    //

    auto extruder = std::make_shared<ChExtruderBeamEuler>(
            &my_system,                 // the physical system
            my_mesh,                    // the mesh where to add the beams
            msection,                   // section for created beam
            0.020,                        // beam element length (size of discretization: the smaller, the more precise)
            ChCoordsys<>(ChVector<>(0,0,0)), // outlet coordinate system (x axis is the extrusion dir)
            0.04                         // the extrusion speed
            );

    // Enable collision for extruded beam
    extruder->SetContact( mysurfmaterial,  // the NSC material for contact surfaces
                          1.15*wire_diameter*0.5  // the radius of the collision spheres at the nodes, (enlarge 15%)
                          );
/*
    auto extruder2 = std::make_shared<ChExtruderBeamIGA>(
            &my_system,                 // the physical system
            my_mesh,                    // the mesh where to add the beams
            msection,                   // section for created beam
            0.020,                        // beam element length (size of discretization: the smaller, the more precise)
            ChCoordsys<>(ChVector<>(0,0,0.1)), // outlet coordinate system (x axis is the extrusion dir)
            0.04,                        // the extrusion speed
            3                            // the order of beams
            );

    // Enable collision for extruded beam
    extruder2->SetContact( mysurfmaterial,  // the NSC material for contact surfaces
                          1.15*wire_diameter*0.5  // the radius of the collision spheres at the nodes, (enlarge 15%)
                          );
*/
    //
    // Add some other beams
    //
    // ***NOTE: hack! this is needed because if the extruder starts with 0 beams in the scene, the
    //    ChVisualizationFEAmesh does not visualize any of the newly generated beams by extrusion. Must be fixed.

    double beam_L = 0.1;

    auto hnode1 = std::make_shared<ChNodeFEAxyzrot>(ChFrame<>(ChVector<>(-0.4, 0, 0)));
    auto hnode2 = std::make_shared<ChNodeFEAxyzrot>(ChFrame<>(ChVector<>(-0.4+beam_L, 0, 0)));
    my_mesh->AddNode(hnode1);
    my_mesh->AddNode(hnode2);

    auto belement1 = std::make_shared<ChElementBeamEuler>();
    belement1->SetNodes(hnode1, hnode2);
    belement1->SetSection(msection);

    my_mesh->AddElement(belement1);
    // Fix a node to ground - the easy way, without constraints
    hnode1->SetFixed(true);

    //
    // Attach a visualization of the FEM mesh.
    //

    auto mvisualizebeamA = std::make_shared<ChVisualizationFEAmesh>(*(my_mesh.get()));
    mvisualizebeamA->SetFEMdataType(ChVisualizationFEAmesh::E_PLOT_ELEM_BEAM_MZ);
    mvisualizebeamA->SetColorscaleMinMax(-0.4, 0.4);
    mvisualizebeamA->SetSmoothFaces(true);
    mvisualizebeamA->SetWireframe(false);
    my_mesh->AddAsset(mvisualizebeamA);

    auto mvisualizebeamC = std::make_shared<ChVisualizationFEAmesh>(*(my_mesh.get()));
    mvisualizebeamC->SetFEMglyphType(ChVisualizationFEAmesh::E_GLYPH_NODE_CSYS);
    mvisualizebeamC->SetFEMdataType(ChVisualizationFEAmesh::E_PLOT_NONE);
    mvisualizebeamC->SetSymbolsThickness(0.006);
    mvisualizebeamC->SetSymbolsScale(0.01);
    mvisualizebeamC->SetZbufferHide(false);
    my_mesh->AddAsset(mvisualizebeamC);

    //
    // Add some obstacles. two rotating lobed gears.
    //
    // Here create two rotating lobed gears, just for fun, that wil trap the
    // extruded beam. To quickly create them, use the CreateLobedGear() function
    // implemented at the top of this file.
    // Also, create two simple constant speed motors to rotate the lobed gears.

    int    lobe_copies = 8;
    double lobe_width = 0.03;
    double lobe_primitive_rad = 0.3;
    double lobe_inner_rad = 0.23;
    double lobe_outer_rad = 0.34;
    double lobe_thickness = 0.08;
    ChVector<> gear_centerLOW(0.4,-lobe_primitive_rad,0);
    ChVector<> gear_centerHI (0.4, lobe_primitive_rad,0);

    auto gearLOW =  CreateLobedGear (gear_centerLOW, lobe_copies, lobe_width, lobe_primitive_rad,
                    lobe_inner_rad, lobe_outer_rad, lobe_thickness, my_system, mysurfmaterial);

    auto mgear_motorLOW = std::make_shared<ChLinkMotorRotationSpeed>();
    mgear_motorLOW->Initialize(gearLOW, mground, ChFrame<>(gear_centerLOW));
    my_system.Add(mgear_motorLOW);

    auto mgear_speedLOW = std::make_shared<ChFunction_Const>(-0.1); // [rad/s]
    mgear_motorLOW->SetSpeedFunction(mgear_speedLOW);

    auto gearHI =  CreateLobedGear (gear_centerHI, lobe_copies, lobe_width, lobe_primitive_rad,
                    lobe_inner_rad, lobe_outer_rad, lobe_thickness, my_system, mysurfmaterial);
    gearHI->SetRot(Q_from_AngZ(0.5*CH_C_2PI/lobe_copies)); // to phase half step respect to other gear

    auto mgear_motorHI = std::make_shared<ChLinkMotorRotationSpeed>();
    mgear_motorHI->Initialize(gearHI, mground, ChFrame<>(gear_centerHI));
    my_system.Add(mgear_motorHI);

    auto mgear_speedHI = std::make_shared<ChFunction_Const>( 0.1); // [rad/s]
    mgear_motorHI->SetSpeedFunction(mgear_speedHI);

    // Create the Irrlicht visualization (open the Irrlicht device,
    // bind a simple user interface, etc. etc.)
    ChIrrApp application(&my_system, L"Beam continuous extrusion and FEA contacts", core::dimension2d<u32>(800, 600),
                         false, true);

    // Easy shortcuts to add camera, lights, logo and sky in Irrlicht scene:
    application.AddTypicalLogo();
    application.AddTypicalSky();
    application.AddTypicalLights();
    application.AddTypicalCamera(core::vector3df(-0.1f, 0.2f, -0.2f));

    // ==IMPORTANT!== Use this function for adding a ChIrrNodeAsset to all items
    // in the system. These ChIrrNodeAsset assets are 'proxies' to the Irrlicht meshes.
    // If you need a finer control on which item really needs a visualization proxy in
    // Irrlicht, just use application.AssetBind(myitem); on a per-item basis.

    application.AssetBindAll();

    // ==IMPORTANT!== Use this function for 'converting' into Irrlicht meshes the assets
    // that you added to the bodies into 3D shapes, they can be visualized by Irrlicht!

    application.AssetUpdateAll();

    // Mark completion of system construction
    my_system.SetupInitial();

    //
    // THE SOFT-REAL-TIME CYCLE
    //

    my_system.SetSolverType(ChSolver::Type::MINRES);
    my_system.SetSolverWarmStarting(true);  // this helps a lot to speedup convergence in this class of problems
    my_system.SetMaxItersSolverSpeed(460);
    my_system.SetMaxItersSolverStab(460);
    my_system.SetTolForce(1e-13);
    auto msolver = std::static_pointer_cast<ChSolverMINRES>(my_system.GetSolver());
    msolver->SetVerbose(false);
    msolver->SetDiagonalPreconditioning(true);

    auto mkl_solver = std::make_shared<ChSolverMKL<>>();
    my_system.SetSolver(mkl_solver);

    application.SetTimestep(0.001);
    application.SetVideoframeSaveInterval(20);
    while (application.GetDevice()->run()) {
        application.BeginScene();

        application.DrawAll();
        ChIrrTools::drawGrid(application.GetVideoDriver(), 0.1, 0.1,20,20,CSYSNORM, irr::video::SColor(255,100,100,100),true);

        application.DoStep();

        extruder->Update();    //***REMEMBER*** to do this to update the extrusion
        //extruder2->Update();    //***REMEMBER*** to do this to update the extrusion

        application.EndScene();
    }

    return 0;
}

<|endoftext|>


<commit_before>#ifndef _FREERTOS_DRIVERS_COMMON_WIFIDEFS_HXX_
#define _FREERTOS_DRIVERS_COMMON_WIFIDEFS_HXX_

#include <stdint.h>

/// Wifi not associated to access point: continuous short blinks.
#define WIFI_BLINK_NOTASSOCIATED  0b1010
/// Waiting for IP address: double short blink, pause, double short blink, ...
#define WIFI_BLINK_ASSOC_NOIP  0b101000
/// Connecting to hub: long blinks
#define WIFI_BLINK_CONNECTING  0b1100
/// Connecting to hub: long blinks
#define WIFI_BLINK_FAILED  0b10101100

enum class WlanState : uint8_t
{
    OK = 0,
    NOT_ASSOCIATED = 1,
    NO_IP,
    NO_CONNECTION,
    CONNECTING,
    MDNS_LOOKUP = 5,
    CONNECT_MDNS,
    CONNECT_STATIC,
    CONNECT_FAILED,
    CONNECTION_LOST,
    WRONG_PASSWORD,
    UPDATE_DISPLAY = 20,
};

/** Operating Role.
 */
enum class WlanRole : uint8_t
{
    UNKNOWN = 0,       /**< Default mode (from stored configuration) */
    DEFAULT = UNKNOWN, /**< Default mode (from stored configuration) */
    STA,               /**< Wi-Fi station mode */
    AP                 /**< Wi-Fi access point mode */
};

enum class CountryCode : uint8_t
{
    US, ///< United States
    EU, ///< European Union
    JP, ///< Japan
    UNKNOWN, ///< unknown country code
};

enum class WlanConnectResult
{
    CONNECT_OK = 0, ///< success
    PASSWORD_INVALID, /// password privided is invalid
};

extern "C" {
/// Name of wifi accesspoint to connect to.
extern char WIFI_SSID[];
/// Password of wifi connection. If empty, use no encryption.
extern char WIFI_PASS[];
/// Hostname at which the OpenLCB hub is at.
extern char WIFI_HUB_HOSTNAME[];
/// Port number of the OpenLCB hub.
extern int WIFI_HUB_PORT;
}

#endif // _FREERTOS_DRIVERS_COMMON_WIFIDEFS_HXX_
<commit_msg>Fix OpenMRNLite problem with DEFAULT symbol (#462)<commit_after>#ifndef _FREERTOS_DRIVERS_COMMON_WIFIDEFS_HXX_
#define _FREERTOS_DRIVERS_COMMON_WIFIDEFS_HXX_

#include <stdint.h>

/// Wifi not associated to access point: continuous short blinks.
#define WIFI_BLINK_NOTASSOCIATED  0b1010
/// Waiting for IP address: double short blink, pause, double short blink, ...
#define WIFI_BLINK_ASSOC_NOIP  0b101000
/// Connecting to hub: long blinks
#define WIFI_BLINK_CONNECTING  0b1100
/// Connecting to hub: long blinks
#define WIFI_BLINK_FAILED  0b10101100

enum class WlanState : uint8_t
{
    OK = 0,
    NOT_ASSOCIATED = 1,
    NO_IP,
    NO_CONNECTION,
    CONNECTING,
    MDNS_LOOKUP = 5,
    CONNECT_MDNS,
    CONNECT_STATIC,
    CONNECT_FAILED,
    CONNECTION_LOST,
    WRONG_PASSWORD,
    UPDATE_DISPLAY = 20,
};

/** Operating Role.
 */
enum class WlanRole : uint8_t
{
    UNKNOWN = 0,       /**< Default mode (from stored configuration) */
    DEFAULT_ROLE = UNKNOWN, /**< Default mode (from stored configuration) */
    STA,               /**< Wi-Fi station mode */
    AP                 /**< Wi-Fi access point mode */
};

enum class CountryCode : uint8_t
{
    US, ///< United States
    EU, ///< European Union
    JP, ///< Japan
    UNKNOWN, ///< unknown country code
};

enum class WlanConnectResult
{
    CONNECT_OK = 0, ///< success
    PASSWORD_INVALID, /// password privided is invalid
};

extern "C" {
/// Name of wifi accesspoint to connect to.
extern char WIFI_SSID[];
/// Password of wifi connection. If empty, use no encryption.
extern char WIFI_PASS[];
/// Hostname at which the OpenLCB hub is at.
extern char WIFI_HUB_HOSTNAME[];
/// Port number of the OpenLCB hub.
extern int WIFI_HUB_PORT;
}

#endif // _FREERTOS_DRIVERS_COMMON_WIFIDEFS_HXX_
<|endoftext|>


<commit_before>
#include <iostream>

#include "SDL2/SDL.h"

#include "sdl/error.h"
#include "sdl/window.h"
#include "sdl/renderer.h"
#include "sdl/texture.h"
#include "sdl/sdl_init.h"
#include "sdl/color.h"
#include "sdl/timer.h"

#include "phys/system.h"
#include "phys/basic_material.h"

#include "draw/sys_view.h"

#include "fuel.h"

using sdl::Color;

int main(int argc, char** argv) {
  std::ranlux48_base rand_gen;
  std::uniform_real_distribution<> uniform01;

  try {
    sdl::SDLinit init(SDL_INIT_EVERYTHING);

    // create window and renderer
    int w = 850;
    int h = 650;
    sdl::Window win("Reactor", w, h, 0);
    win.Center();
    win.Show();
    sdl::Renderer ren(win, SDL_RENDERER_ACCELERATED);

    // create material geometry
    phys::BasicMaterial m1{0, 0, .1, 1, 0};
    phys::Object::Rect r1{w/2 - 40, h/2 - 40, 120, 120};
    phys::Object reflector(&m1, r1, sdl::Color::white());
    phys::Object reflector2(reflector);
    phys::Object reflector3(reflector);
    phys::Object reflector4(reflector);

    phys::BasicMaterial m2{.1, 0, 0, 1, 0};
    phys::Object::Rect r2{w/2 - 130, h/2 - 40, 80, 80};
    phys::Object absorber(&m2, r2, sdl::Color::blue());
    phys::Object absorber2(absorber);

    phys::BasicMaterial m3{0, 0, .03, .3, 0};
    phys::Object::Rect r3{w/2 + 50, h/2 - 40, 80, 80};
    phys::Object moderator(&m3, r3, sdl::Color::green());
    phys::Object moderator2(moderator);
    phys::Object moderator3(moderator);
    phys::Object moderator4(moderator);

    Fuel m4(0, 0.03, 2);
    phys::Object::Rect r4{w/2 - 20, h/2 + 50, 40, 40};
    phys::Object fuel(&m4, r4, sdl::Color::purple());
    phys::Object fuel2(fuel);
    phys::Object fuel3(fuel);
    phys::Object fuel4(fuel);

    // create system and a view for drawing it
    phys::System sys(w, h);
    sys.AddObject(&reflector);
    sys.AddObject(&reflector2);
    sys.AddObject(&reflector3);
    sys.AddObject(&reflector4);
    sys.AddObject(&absorber);
    sys.AddObject(&absorber2);
    sys.AddObject(&moderator);
    sys.AddObject(&moderator2);
    sys.AddObject(&moderator3);
    sys.AddObject(&moderator4);
    sys.AddObject(&fuel);
    sys.AddObject(&fuel2);
    sys.AddObject(&fuel3);
    sys.AddObject(&fuel4);
    draw::SysView view(&sys, &ren);

    // start up the main loop
    SDL_Event ev;
    sdl::Timer timer;
    timer.Start();
    bool done = false;
    bool dragging = false;
    phys::Object* dragged = nullptr;
    while(!done) {
      // process events
      while(SDL_PollEvent(&ev)) {
        if (ev.type == SDL_QUIT) {
          done = true;
        } else if (ev.type == SDL_MOUSEBUTTONDOWN && ev.button.button == SDL_BUTTON_RIGHT) {
          // create some neutrons
          phys::Neutron::Pop ns;
          for (int i = 0; i < 5000; ++i) {
            double theta = uniform01(rand_gen) * 2 * 3.141592654;
            int vx = std::cos(theta) * phys::Neutron::kNomSpeed;
            int vy = std::sin(theta) * phys::Neutron::kNomSpeed;
            ns.push_back(phys::Neutron(ev.button.x, ev.button.y, vx, vy));
          }
          sys.AddNeutrons(ns);
        } else if (ev.type == SDL_MOUSEBUTTONDOWN && ev.button.button == SDL_BUTTON_LEFT) {
          dragged = sys.ObjectFor(ev.button.x, ev.button.y);
          sys.MoveTop(dragged);
          dragging = true;
        } else if (ev.type == SDL_MOUSEBUTTONUP && ev.button.button == SDL_BUTTON_LEFT) {
          dragging = false;
        } else if (ev.type == SDL_MOUSEMOTION && dragging) {
          dragged->Shift(ev.motion.xrel, ev.motion.yrel);
        }
      }

      double dt = (double)timer.Mark();
      sys.Tick(dt / 1000);
      view.Render(((double)1000) / dt);
    }

    return 0;
  } catch (std::exception err) {
    std::cout << "ERROR: " << err.what() << "\n";
    return 1;
  }
}

<commit_msg>added 2 more absorbers<commit_after>
#include <iostream>

#include "SDL2/SDL.h"

#include "sdl/error.h"
#include "sdl/window.h"
#include "sdl/renderer.h"
#include "sdl/texture.h"
#include "sdl/sdl_init.h"
#include "sdl/color.h"
#include "sdl/timer.h"

#include "phys/system.h"
#include "phys/basic_material.h"

#include "draw/sys_view.h"

#include "fuel.h"

using sdl::Color;

int main(int argc, char** argv) {
  std::ranlux48_base rand_gen;
  std::uniform_real_distribution<> uniform01;

  try {
    sdl::SDLinit init(SDL_INIT_EVERYTHING);

    // create window and renderer
    int w = 850;
    int h = 650;
    sdl::Window win("Reactor", w, h, 0);
    win.Center();
    win.Show();
    sdl::Renderer ren(win, SDL_RENDERER_ACCELERATED);

    // create material geometry
    phys::BasicMaterial m1{0, 0, .1, 1, 0};
    phys::Object::Rect r1{w/2 - 40, h/2 - 40, 120, 120};
    phys::Object reflector(&m1, r1, sdl::Color::white());
    phys::Object reflector2(reflector);
    phys::Object reflector3(reflector);
    phys::Object reflector4(reflector);

    phys::BasicMaterial m2{.1, 0, 0, 1, 0};
    phys::Object::Rect r2{w/2 - 130, h/2 - 40, 80, 80};
    phys::Object absorber(&m2, r2, sdl::Color::blue());
    phys::Object absorber2(absorber);
    phys::Object absorber3(absorber);
    phys::Object absorber4(absorber);

    phys::BasicMaterial m3{0, 0, .03, .3, 0};
    phys::Object::Rect r3{w/2 + 50, h/2 - 40, 80, 80};
    phys::Object moderator(&m3, r3, sdl::Color::green());
    phys::Object moderator2(moderator);
    phys::Object moderator3(moderator);
    phys::Object moderator4(moderator);

    Fuel m4(0, 0.03, 2);
    phys::Object::Rect r4{w/2 - 20, h/2 + 50, 40, 40};
    phys::Object fuel(&m4, r4, sdl::Color::purple());
    phys::Object fuel2(fuel);
    phys::Object fuel3(fuel);
    phys::Object fuel4(fuel);

    // create system and a view for drawing it
    phys::System sys(w, h);
    sys.AddObject(&reflector);
    sys.AddObject(&reflector2);
    sys.AddObject(&reflector3);
    sys.AddObject(&reflector4);
    sys.AddObject(&absorber);
    sys.AddObject(&absorber2);
    sys.AddObject(&absorber3);
    sys.AddObject(&absorber4);
    sys.AddObject(&moderator);
    sys.AddObject(&moderator2);
    sys.AddObject(&moderator3);
    sys.AddObject(&moderator4);
    sys.AddObject(&fuel);
    sys.AddObject(&fuel2);
    sys.AddObject(&fuel3);
    sys.AddObject(&fuel4);
    draw::SysView view(&sys, &ren);

    // start up the main loop
    SDL_Event ev;
    sdl::Timer timer;
    timer.Start();
    bool done = false;
    bool dragging = false;
    phys::Object* dragged = nullptr;
    while(!done) {
      // process events
      while(SDL_PollEvent(&ev)) {
        if (ev.type == SDL_QUIT) {
          done = true;
        } else if (ev.type == SDL_MOUSEBUTTONDOWN && ev.button.button == SDL_BUTTON_RIGHT) {
          // create some neutrons
          phys::Neutron::Pop ns;
          for (int i = 0; i < 5000; ++i) {
            double theta = uniform01(rand_gen) * 2 * 3.141592654;
            int vx = std::cos(theta) * phys::Neutron::kNomSpeed;
            int vy = std::sin(theta) * phys::Neutron::kNomSpeed;
            ns.push_back(phys::Neutron(ev.button.x, ev.button.y, vx, vy));
          }
          sys.AddNeutrons(ns);
        } else if (ev.type == SDL_MOUSEBUTTONDOWN && ev.button.button == SDL_BUTTON_LEFT) {
          dragged = sys.ObjectFor(ev.button.x, ev.button.y);
          sys.MoveTop(dragged);
          dragging = true;
        } else if (ev.type == SDL_MOUSEBUTTONUP && ev.button.button == SDL_BUTTON_LEFT) {
          dragging = false;
        } else if (ev.type == SDL_MOUSEMOTION && dragging) {
          dragged->Shift(ev.motion.xrel, ev.motion.yrel);
        }
      }

      double dt = (double)timer.Mark();
      sys.Tick(dt / 1000);
      view.Render(((double)1000) / dt);
    }

    return 0;
  } catch (std::exception err) {
    std::cout << "ERROR: " << err.what() << "\n";
    return 1;
  }
}

<|endoftext|>


<commit_before>#include <cstdlib>

#include <iostream>
#include <fstream>
#include <vector>
#include <algorithm>
#include <string>

#include <cmath>

#include "fasta_parser.h"
#include "sa_is.h"
#include "search.h"

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

  if(argc != 5) {
    std::cerr << "Usage: " << argv[0] << " <file containing reference string> <file containing query string> <index level of sparseSA, K-SA> <size of minimal match>" << std::endl;
    exit(-1);
  }

  std::ifstream ref_file(argv[1]);
	std::ifstream query_file(argv[2]);

  if(!ref_file.is_open()) {
    std::cerr << "There was a problem openning the file \"" << argv[1] << "\"!\nAborting." << std::endl;
    exit(-2);
  }

  if(!query_file.is_open()) {
    std::cerr << "There was a problem openning the file \"" << argv[2] << "\"!\nAborting." << std::endl;
    exit(-2);
  }

  // Read in reference string (fasta format)
	std::string ref_string;
	std::vector<string> refdescr;
	std::vector<long> startpos;
	fasta_parser(argv[1], ref_string, refdescr, startpos);

  // Read in query string (fasta format)
	std::string query_string;
	std::vector<string> querydescr;
	std::vector<long> q_startpos;
  fasta_parser(argv[2], query_string, querydescr, q_startpos);

  int K = atoi(argv[3]);
  int L = atoi(argv[4]);

  // pad string with $
  int pad_length = K - (ref_string.size() % K);
  ref_string.append(1, TERMINATION_CHAR);

  int N = ref_string.length();

  std::cout << ref_string << std::endl;

  std::vector<int> sa;
  sa.reserve(N);
  bool *types = new bool[N];

  int *SA = new int[N];
  int *sparseSA = new int[N / K];
  int *sparseISA = new int[N / K];
  int *sparseLCP = new int[N / K];

  // Creates Suffix Array using SA_IS algorithm

  sa_is(ref_string.c_str(), SA, N, 256, sizeof(char));

  int j = 0;
  for (int i = 0; i < N / K; i++){
	  sparseSA[i] = SA[i * K];
  }

  for (unsigned int i = 0; i < j; ++i) {
    std::cout << "[" << i << "]\t" << sparseSA[i] << (types[sparseSA[i]] ? "\tS\t" : "\tL\t")
    << ref_string.substr(sparseSA[i]) << std::endl;
  }

  cout << endl;

  // Generate ISA A
  for (int i = 0; i < N; i++){
    if (SA[i] % K == 0){
      sparseSA[j++] = (int) SA[i];
    }
  }
  // Generate LCP
  int h = 0;
  for(int i = 0; i < N/K  ; i+=K) {
  int m = (int) sparseISA[i];
    if(m==0) {
      sparseLCP[m] = -1;
    }
    else {
      int j = sparseSA[m-1];
      while(i+h < N && j+h < N && ref_string[i+h] == ref_string[j+h]) {
        h++;
      }
      sparseLCP[m] = (int) h;
    }
    h = std::max(0, h - K);
  }

  cout << endl << "Sparse SA: ";
  for (int i = 0; i < ceil(N/K); ++i)
    cout << sparseSA[i] << "  ";

  cout << endl << "Sparse ISA: ";
  for (int i = 0; i < ceil(N/K) ; ++i)
    cout << sparseISA[i] << "  ";

  cout << endl << "Sparse LCP: ";
  for (int i = 0; i < ceil(N/K); ++i)
    cout << sparseLCP[i] << "  ";

  // Search for MEMs:
  cout << endl << "\tRef.\tQuery\tLength" << endl;

  int p0 = 0;
  MEM(p0, ref_string, sparseISA, sparseLCP, sparseSA, query_string, K, N, L);

  return 0;
}

<commit_msg>Revert "Fixed sparseSA generation"<commit_after>#include <cstdlib>

#include <iostream>
#include <fstream>
#include <vector>
#include <algorithm>
#include <string>

#include <cmath>

#include "fasta_parser.h"
#include "sa_is.h"
#include "search.h"

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

  if(argc != 5) {
    std::cerr << "Usage: " << argv[0] << " <file containing reference string> <file containing query string> <index level of sparseSA, K-SA> <size of minimal match>" << std::endl;
    exit(-1);
  }

  std::ifstream ref_file(argv[1]);
	std::ifstream query_file(argv[2]);

  if(!ref_file.is_open()) {
    std::cerr << "There was a problem openning the file \"" << argv[1] << "\"!\nAborting." << std::endl;
    exit(-2);
  }

  if(!query_file.is_open()) {
    std::cerr << "There was a problem openning the file \"" << argv[2] << "\"!\nAborting." << std::endl;
    exit(-2);
  }

  // Read in reference string (fasta format)
	std::string ref_string;
	std::vector<string> refdescr;
	std::vector<long> startpos;
	fasta_parser(argv[1], ref_string, refdescr, startpos);

  // Read in query string (fasta format)
	std::string query_string;
	std::vector<string> querydescr;
	std::vector<long> q_startpos;
  fasta_parser(argv[2], query_string, querydescr, q_startpos);

  int K = atoi(argv[3]);
  int L = atoi(argv[4]);

  // pad string with $
  int pad_length = K - (ref_string.size() % K);
  ref_string.append(1, TERMINATION_CHAR);

  int N = ref_string.length();

  std::cout << ref_string << std::endl;

  std::vector<int> sa;
  sa.reserve(N);
  bool *types = new bool[N];

  int *SA = new int[N];
  int *sparseSA = new int[N / K];
  int *sparseISA = new int[N / K];
  int *sparseLCP = new int[N / K];

  // Creates Suffix Array using SA_IS algorithm

  sa_is(ref_string.c_str(), SA, N, 256, sizeof(char));

  int j = 0;
  for (int i = 0; i < N / K; i++){
	  sparseSA[i] = SA[i * K];
  }

  for (unsigned int i = 0; i < j; ++i) {
    std::cout << "[" << i << "]\t" << sparseSA[i] << (types[sparseSA[i]] ? "\tS\t" : "\tL\t")
    << ref_string.substr(sparseSA[i]) << std::endl;
  }

  cout << endl;

  // Generate ISA A
  for(int i = 0; i < N/K  ; i++) {
	  sparseISA[i] = sparseSA[i] / K;
  }

  // Generate LCP
  int h = 0;
  for(int i = 0; i < N/K  ; i+=K) {
  int m = (int) sparseISA[i];
    if(m==0) {
      sparseLCP[m] = -1;
    }
    else {
      int j = sparseSA[m-1];
      while(i+h < N && j+h < N && ref_string[i+h] == ref_string[j+h]) {
        h++;
      }
      sparseLCP[m] = (int) h;
    }
    h = std::max(0, h - K);
  }

  cout << endl << "Sparse SA: ";
  for (int i = 0; i < ceil(N/K); ++i)
    cout << sparseSA[i] << "  ";

  cout << endl << "Sparse ISA: ";
  for (int i = 0; i < ceil(N/K) ; ++i)
    cout << sparseISA[i] << "  ";

  cout << endl << "Sparse LCP: ";
  for (int i = 0; i < ceil(N/K); ++i)
    cout << sparseLCP[i] << "  ";

  // Search for MEMs:
  cout << endl << "\tRef.\tQuery\tLength" << endl;

  int p0 = 0;
  MEM(p0, ref_string, sparseISA, sparseLCP, sparseSA, query_string, K, N, L);

  return 0;
}

<|endoftext|>


<commit_before>#include "../Outer_Parameters.hxx"
#include "setup_constraints.hxx"

#include "../../ostream_set.hxx"
#include "../../ostream_map.hxx"
#include "../../ostream_vector.hxx"
#include "../../set_stream_precision.hxx"

void compute_y_transform(
  const std::vector<std::vector<std::vector<std::vector<Function>>>>
    &function_blocks,
  const std::vector<std::set<El::BigFloat>> &points,
  const std::vector<El::BigFloat> &objectives,
  const std::vector<El::BigFloat> &normalization,
  const Outer_Parameters &parameters, const size_t &max_index,
  const El::Grid &global_grid,
  El::DistMatrix<El::BigFloat, El::STAR, El::STAR> &yp_to_y,
  El::DistMatrix<El::BigFloat, El::STAR, El::STAR> &dual_objective_b_star,
  El::BigFloat &primal_c_scale);

boost::optional<int64_t> load_checkpoint(
  const boost::filesystem::path &checkpoint_directory,
  const Verbosity &verbosity, boost::optional<int64_t> &backup_generation,
  int64_t &current_generation,
  El::DistMatrix<El::BigFloat, El::STAR, El::STAR> &yp_to_y_star,
  El::DistMatrix<El::BigFloat, El::STAR, El::STAR> &dual_objective_b_star,
  El::Matrix<El::BigFloat> &y, std::vector<std::set<El::BigFloat>> &points,
  El::BigFloat &threshold, El::BigFloat &primal_c_scale);

void copy_matrix(const El::Matrix<El::BigFloat> &source,
                 El::DistMatrix<El::BigFloat> &destination);
void copy_matrix(const El::DistMatrix<El::BigFloat, El::STAR, El::STAR> &source,
                 El::Matrix<El::BigFloat> &destination);

void fill_weights(const El::Matrix<El::BigFloat> &y, const size_t &max_index,
                  const std::vector<El::BigFloat> &normalization,
                  std::vector<El::BigFloat> &weights);

void find_new_points(
  const size_t &num_blocks, const size_t &rank, const size_t &num_procs,
  const El::BigFloat &infinity,
  const std::vector<std::vector<std::vector<std::vector<Function>>>>
    &function_blocks,
  const std::vector<El::BigFloat> &weights,
  const std::vector<std::set<El::BigFloat>> &points,
  std::vector<std::vector<El::BigFloat>> &new_points, bool &has_new_points);

void save_checkpoint(
  const boost::filesystem::path &checkpoint_directory,
  const Verbosity &verbosity,
  const El::DistMatrix<El::BigFloat, El::STAR, El::STAR> &yp_to_y_star,
  const El::DistMatrix<El::BigFloat, El::STAR, El::STAR> &dual_objective_b_star,
  const El::Matrix<El::BigFloat> &y,
  const std::vector<std::set<El::BigFloat>> &points,
  const El::BigFloat &infinity, const El::BigFloat &threshold,
  const El::BigFloat &primal_c_scale,
  boost::optional<int64_t> &backup_generation, int64_t &current_generation);

std::vector<El::BigFloat> compute_optimal(
  const std::vector<std::vector<std::vector<std::vector<Function>>>>
    &function_blocks,
  const std::vector<std::vector<El::BigFloat>> &initial_points,
  const std::vector<El::BigFloat> &objectives,
  const std::vector<El::BigFloat> &normalization,
  const Outer_Parameters &parameters_in)
{
  if(initial_points.size() != function_blocks.size())
    {
      throw std::runtime_error(
        "Size are different: Positive_Matrix_With_Prefactor: "
        + std::to_string(function_blocks.size())
        + ", initial points: " + std::to_string(initial_points.size()));
    }
  Outer_Parameters parameters(parameters_in);
  if(parameters.verbosity != Verbosity::debug)
    {
      parameters.verbosity = Verbosity::none;
    }

  const size_t rank(El::mpi::Rank()), num_procs(El::mpi::Size()),
    num_weights(normalization.size());

  const size_t num_blocks(initial_points.size());
  std::vector<El::BigFloat> weights(num_weights, 0);
  std::vector<std::set<El::BigFloat>> points(num_blocks);
  std::vector<std::vector<El::BigFloat>> new_points(num_blocks);

  // GMP does not have a special infinity value, so we use max double.
  const El::BigFloat infinity(std::numeric_limits<double>::max());
  // Use the input points and add inifinty
  for(size_t block(0); block < num_blocks; ++block)
    {
      for(auto &point : initial_points.at(block))
        {
          points.at(block).emplace(point);
        }
      points.at(block).emplace(infinity);
    }

  // TODO: This is duplicated from sdp2input/write_output/write_output.cxx
  size_t max_index(0);
  El::BigFloat max_normalization(0);
  for(size_t index(0); index != normalization.size(); ++index)
    {
      const El::BigFloat element(Abs(normalization[index]));
      if(element > max_normalization)
        {
          max_normalization = element;
          max_index = index;
        }
    }

  const El::Grid global_grid;
  El::DistMatrix<El::BigFloat, El::STAR, El::STAR> yp_to_y_star(
    normalization.size() - 1, normalization.size() - 1, global_grid),
    dual_objective_b_star(global_grid);
  El::BigFloat primal_c_scale;

  parameters.solver.duality_gap_threshold = 1.1;
  El::Matrix<El::BigFloat> yp_saved(yp_to_y_star.Height(), 1);
  El::Zero(yp_saved);

  int64_t current_generation(0);
  boost::optional<int64_t> backup_generation;
  load_checkpoint(parameters.solver.checkpoint_in, parameters_in.verbosity,
                  backup_generation, current_generation, yp_to_y_star,
                  dual_objective_b_star, yp_saved, points,
                  parameters.solver.duality_gap_threshold, primal_c_scale);
  if(backup_generation)
    {
      El::Matrix<El::BigFloat> y(yp_saved.Height(), 1);

      El::Gemv(El::Orientation::NORMAL, El::BigFloat(1.0),
               yp_to_y_star.LockedMatrix(), yp_saved, El::BigFloat(0.0), y);
      fill_weights(y, max_index, normalization, weights);
    }
  else
    {
      compute_y_transform(function_blocks, points, objectives, normalization,
                          parameters, max_index, global_grid, yp_to_y_star,
                          dual_objective_b_star, primal_c_scale);
    }

  while(parameters.solver.duality_gap_threshold
        >= parameters_in.solver.duality_gap_threshold)
    {
      std::map<size_t, size_t> new_to_old;
      size_t num_constraints(0), old_index(0);
      std::vector<size_t> matrix_dimensions;
      for(size_t block(0); block != num_blocks; ++block)
        {
          for(size_t offset(0); offset != points.at(block).size(); ++offset)
            {
              new_to_old.emplace(num_constraints + offset, old_index + offset);
            }
          old_index += points.at(block).size();
          for(auto &point : new_points.at(block))
            {
              points.at(block).emplace(point);
            }
          num_constraints += points.at(block).size();
          matrix_dimensions.insert(matrix_dimensions.end(),
                                   points.at(block).size(),
                                   function_blocks[block].size());
          if(rank == 0 && parameters_in.verbosity >= Verbosity::debug)
            {
              std::cout << "points: " << block << " " << points.at(block)
                        << "\n";
            }
        }
      if(rank == 0 && parameters_in.verbosity >= Verbosity::regular)
        {
          std::cout << "num_constraints: " << num_constraints << "\n";
        }

      std::vector<std::vector<El::BigFloat>> primal_objective_c;
      primal_objective_c.reserve(num_constraints);
      std::vector<El::Matrix<El::BigFloat>> free_var_matrix;
      free_var_matrix.reserve(num_constraints);

      setup_constraints(max_index, num_blocks, infinity, function_blocks,
                        normalization, points, primal_objective_c,
                        free_var_matrix);

      const El::BigFloat objective_const(objectives.at(max_index)
                                         / normalization.at(max_index));

      Block_Info block_info(matrix_dimensions, parameters.verbosity);

      El::Grid grid(block_info.mpi_comm.value);

      SDP sdp(objective_const, primal_objective_c, free_var_matrix,
              yp_to_y_star, dual_objective_b_star, primal_c_scale, block_info,
              grid);

      SDP_Solver solver(parameters.solver, parameters.verbosity,
                        parameters.require_initial_checkpoint, block_info,
                        grid, sdp.dual_objective_b.Height());

      for(auto &y_block : solver.y.blocks)
        {
          copy_matrix(yp_saved, y_block);
        }

      boost::property_tree::ptree parameter_properties(
        to_property_tree(parameters));
      bool has_new_points(false);
      while(!has_new_points
            && parameters.solver.duality_gap_threshold
                 >= parameters_in.solver.duality_gap_threshold)
        {
          if(rank == 0 && parameters_in.verbosity >= Verbosity::regular)
            {
              std::cout << "Threshold: "
                        << parameters.solver.duality_gap_threshold << "\n";
            }

          Timers timers(parameters.verbosity >= Verbosity::debug);
          SDP_Solver_Terminate_Reason reason
            = solver.run(parameters.solver, parameters.verbosity,
                         parameter_properties, block_info, sdp, grid, timers);

          if(rank == 0 && parameters_in.verbosity >= Verbosity::debug)
            {
              set_stream_precision(std::cout);
              std::cout << "-----" << reason << "-----\n"
                        << '\n'
                        << "primalObjective = " << solver.primal_objective
                        << '\n'
                        << "dualObjective   = " << solver.dual_objective
                        << '\n'
                        << "dualityGap      = " << solver.duality_gap << '\n'
                        << "primalError     = " << solver.primal_error()
                        << '\n'
                        << "dualError       = " << solver.dual_error << '\n'
                        << '\n';
            }

          if(reason == SDP_Solver_Terminate_Reason::MaxComplementarityExceeded
             || reason == SDP_Solver_Terminate_Reason::MaxIterationsExceeded
             || reason == SDP_Solver_Terminate_Reason::MaxRuntimeExceeded
             || reason == SDP_Solver_Terminate_Reason::PrimalStepTooSmall
             || reason == SDP_Solver_Terminate_Reason::DualStepTooSmall)
            {
              std::stringstream ss;
              ss << "Can not find solution: " << reason;
              throw std::runtime_error(ss.str());
            }

          El::Matrix<El::BigFloat> y(dual_objective_b_star.Height(), 1);
          El::DistMatrix<El::BigFloat, El::STAR, El::STAR> yp_star(
            solver.y.blocks.at(0));

          El::Gemv(El::Orientation::NORMAL, El::BigFloat(1.0),
                   yp_to_y_star.LockedMatrix(), yp_star.LockedMatrix(),
                   El::BigFloat(0.0), y);

          fill_weights(y, max_index, normalization, weights);
          if(rank == 0 && parameters_in.verbosity >= Verbosity::regular)
            {
              set_stream_precision(std::cout);
              std::cout << "weight: " << weights << "\n";

              El::BigFloat optimal(0);
              for(size_t index(0); index < objectives.size(); ++index)
                {
                  optimal += objectives[index] * weights[index];
                }
              std::cout << "optimal: " << optimal << "\n";
            }
          find_new_points(num_blocks, rank, num_procs, infinity,
                          function_blocks, weights, points, new_points,
                          has_new_points);
          if(!has_new_points)
            {
              if(parameters.solver.duality_gap_threshold==parameters_in.solver.duality_gap_threshold)
                {
                  parameters.solver.duality_gap_threshold=0;
                }
              else
                {
                  parameters.solver.duality_gap_threshold
                    = std::max(parameters.solver.duality_gap_threshold/parameters.duality_gap_reduction,
                               parameters_in.solver.duality_gap_threshold);
                }
            }
        }
      El::DistMatrix<El::BigFloat, El::STAR, El::STAR> yp_star(
        solver.y.blocks.front());
      copy_matrix(yp_star, yp_saved);
      save_checkpoint(parameters.solver.checkpoint_out,
                      parameters_in.verbosity, yp_to_y_star,
                      dual_objective_b_star, yp_saved, points, infinity,
                      parameters.solver.duality_gap_threshold, primal_c_scale,
                      backup_generation, current_generation);
    }
  return weights;
}
<commit_msg>Clang-format<commit_after>#include "../Outer_Parameters.hxx"
#include "setup_constraints.hxx"

#include "../../ostream_set.hxx"
#include "../../ostream_map.hxx"
#include "../../ostream_vector.hxx"
#include "../../set_stream_precision.hxx"

void compute_y_transform(
  const std::vector<std::vector<std::vector<std::vector<Function>>>>
    &function_blocks,
  const std::vector<std::set<El::BigFloat>> &points,
  const std::vector<El::BigFloat> &objectives,
  const std::vector<El::BigFloat> &normalization,
  const Outer_Parameters &parameters, const size_t &max_index,
  const El::Grid &global_grid,
  El::DistMatrix<El::BigFloat, El::STAR, El::STAR> &yp_to_y,
  El::DistMatrix<El::BigFloat, El::STAR, El::STAR> &dual_objective_b_star,
  El::BigFloat &primal_c_scale);

boost::optional<int64_t> load_checkpoint(
  const boost::filesystem::path &checkpoint_directory,
  const Verbosity &verbosity, boost::optional<int64_t> &backup_generation,
  int64_t &current_generation,
  El::DistMatrix<El::BigFloat, El::STAR, El::STAR> &yp_to_y_star,
  El::DistMatrix<El::BigFloat, El::STAR, El::STAR> &dual_objective_b_star,
  El::Matrix<El::BigFloat> &y, std::vector<std::set<El::BigFloat>> &points,
  El::BigFloat &threshold, El::BigFloat &primal_c_scale);

void copy_matrix(const El::Matrix<El::BigFloat> &source,
                 El::DistMatrix<El::BigFloat> &destination);
void copy_matrix(const El::DistMatrix<El::BigFloat, El::STAR, El::STAR> &source,
                 El::Matrix<El::BigFloat> &destination);

void fill_weights(const El::Matrix<El::BigFloat> &y, const size_t &max_index,
                  const std::vector<El::BigFloat> &normalization,
                  std::vector<El::BigFloat> &weights);

void find_new_points(
  const size_t &num_blocks, const size_t &rank, const size_t &num_procs,
  const El::BigFloat &infinity,
  const std::vector<std::vector<std::vector<std::vector<Function>>>>
    &function_blocks,
  const std::vector<El::BigFloat> &weights,
  const std::vector<std::set<El::BigFloat>> &points,
  std::vector<std::vector<El::BigFloat>> &new_points, bool &has_new_points);

void save_checkpoint(
  const boost::filesystem::path &checkpoint_directory,
  const Verbosity &verbosity,
  const El::DistMatrix<El::BigFloat, El::STAR, El::STAR> &yp_to_y_star,
  const El::DistMatrix<El::BigFloat, El::STAR, El::STAR> &dual_objective_b_star,
  const El::Matrix<El::BigFloat> &y,
  const std::vector<std::set<El::BigFloat>> &points,
  const El::BigFloat &infinity, const El::BigFloat &threshold,
  const El::BigFloat &primal_c_scale,
  boost::optional<int64_t> &backup_generation, int64_t &current_generation);

std::vector<El::BigFloat> compute_optimal(
  const std::vector<std::vector<std::vector<std::vector<Function>>>>
    &function_blocks,
  const std::vector<std::vector<El::BigFloat>> &initial_points,
  const std::vector<El::BigFloat> &objectives,
  const std::vector<El::BigFloat> &normalization,
  const Outer_Parameters &parameters_in)
{
  if(initial_points.size() != function_blocks.size())
    {
      throw std::runtime_error(
        "Size are different: Positive_Matrix_With_Prefactor: "
        + std::to_string(function_blocks.size())
        + ", initial points: " + std::to_string(initial_points.size()));
    }
  Outer_Parameters parameters(parameters_in);
  if(parameters.verbosity != Verbosity::debug)
    {
      parameters.verbosity = Verbosity::none;
    }

  const size_t rank(El::mpi::Rank()), num_procs(El::mpi::Size()),
    num_weights(normalization.size());

  const size_t num_blocks(initial_points.size());
  std::vector<El::BigFloat> weights(num_weights, 0);
  std::vector<std::set<El::BigFloat>> points(num_blocks);
  std::vector<std::vector<El::BigFloat>> new_points(num_blocks);

  // GMP does not have a special infinity value, so we use max double.
  const El::BigFloat infinity(std::numeric_limits<double>::max());
  // Use the input points and add inifinty
  for(size_t block(0); block < num_blocks; ++block)
    {
      for(auto &point : initial_points.at(block))
        {
          points.at(block).emplace(point);
        }
      points.at(block).emplace(infinity);
    }

  // TODO: This is duplicated from sdp2input/write_output/write_output.cxx
  size_t max_index(0);
  El::BigFloat max_normalization(0);
  for(size_t index(0); index != normalization.size(); ++index)
    {
      const El::BigFloat element(Abs(normalization[index]));
      if(element > max_normalization)
        {
          max_normalization = element;
          max_index = index;
        }
    }

  const El::Grid global_grid;
  El::DistMatrix<El::BigFloat, El::STAR, El::STAR> yp_to_y_star(
    normalization.size() - 1, normalization.size() - 1, global_grid),
    dual_objective_b_star(global_grid);
  El::BigFloat primal_c_scale;

  parameters.solver.duality_gap_threshold = 1.1;
  El::Matrix<El::BigFloat> yp_saved(yp_to_y_star.Height(), 1);
  El::Zero(yp_saved);

  int64_t current_generation(0);
  boost::optional<int64_t> backup_generation;
  load_checkpoint(parameters.solver.checkpoint_in, parameters_in.verbosity,
                  backup_generation, current_generation, yp_to_y_star,
                  dual_objective_b_star, yp_saved, points,
                  parameters.solver.duality_gap_threshold, primal_c_scale);
  if(backup_generation)
    {
      El::Matrix<El::BigFloat> y(yp_saved.Height(), 1);

      El::Gemv(El::Orientation::NORMAL, El::BigFloat(1.0),
               yp_to_y_star.LockedMatrix(), yp_saved, El::BigFloat(0.0), y);
      fill_weights(y, max_index, normalization, weights);
    }
  else
    {
      compute_y_transform(function_blocks, points, objectives, normalization,
                          parameters, max_index, global_grid, yp_to_y_star,
                          dual_objective_b_star, primal_c_scale);
    }

  while(parameters.solver.duality_gap_threshold
        >= parameters_in.solver.duality_gap_threshold)
    {
      std::map<size_t, size_t> new_to_old;
      size_t num_constraints(0), old_index(0);
      std::vector<size_t> matrix_dimensions;
      for(size_t block(0); block != num_blocks; ++block)
        {
          for(size_t offset(0); offset != points.at(block).size(); ++offset)
            {
              new_to_old.emplace(num_constraints + offset, old_index + offset);
            }
          old_index += points.at(block).size();
          for(auto &point : new_points.at(block))
            {
              points.at(block).emplace(point);
            }
          num_constraints += points.at(block).size();
          matrix_dimensions.insert(matrix_dimensions.end(),
                                   points.at(block).size(),
                                   function_blocks[block].size());
          if(rank == 0 && parameters_in.verbosity >= Verbosity::debug)
            {
              std::cout << "points: " << block << " " << points.at(block)
                        << "\n";
            }
        }
      if(rank == 0 && parameters_in.verbosity >= Verbosity::regular)
        {
          std::cout << "num_constraints: " << num_constraints << "\n";
        }

      std::vector<std::vector<El::BigFloat>> primal_objective_c;
      primal_objective_c.reserve(num_constraints);
      std::vector<El::Matrix<El::BigFloat>> free_var_matrix;
      free_var_matrix.reserve(num_constraints);

      setup_constraints(max_index, num_blocks, infinity, function_blocks,
                        normalization, points, primal_objective_c,
                        free_var_matrix);

      const El::BigFloat objective_const(objectives.at(max_index)
                                         / normalization.at(max_index));

      Block_Info block_info(matrix_dimensions, parameters.verbosity);

      El::Grid grid(block_info.mpi_comm.value);

      SDP sdp(objective_const, primal_objective_c, free_var_matrix,
              yp_to_y_star, dual_objective_b_star, primal_c_scale, block_info,
              grid);

      SDP_Solver solver(parameters.solver, parameters.verbosity,
                        parameters.require_initial_checkpoint, block_info,
                        grid, sdp.dual_objective_b.Height());

      for(auto &y_block : solver.y.blocks)
        {
          copy_matrix(yp_saved, y_block);
        }

      boost::property_tree::ptree parameter_properties(
        to_property_tree(parameters));
      bool has_new_points(false);
      while(!has_new_points
            && parameters.solver.duality_gap_threshold
                 >= parameters_in.solver.duality_gap_threshold)
        {
          if(rank == 0 && parameters_in.verbosity >= Verbosity::regular)
            {
              std::cout << "Threshold: "
                        << parameters.solver.duality_gap_threshold << "\n";
            }

          Timers timers(parameters.verbosity >= Verbosity::debug);
          SDP_Solver_Terminate_Reason reason
            = solver.run(parameters.solver, parameters.verbosity,
                         parameter_properties, block_info, sdp, grid, timers);

          if(rank == 0 && parameters_in.verbosity >= Verbosity::debug)
            {
              set_stream_precision(std::cout);
              std::cout << "-----" << reason << "-----\n"
                        << '\n'
                        << "primalObjective = " << solver.primal_objective
                        << '\n'
                        << "dualObjective   = " << solver.dual_objective
                        << '\n'
                        << "dualityGap      = " << solver.duality_gap << '\n'
                        << "primalError     = " << solver.primal_error()
                        << '\n'
                        << "dualError       = " << solver.dual_error << '\n'
                        << '\n';
            }

          if(reason == SDP_Solver_Terminate_Reason::MaxComplementarityExceeded
             || reason == SDP_Solver_Terminate_Reason::MaxIterationsExceeded
             || reason == SDP_Solver_Terminate_Reason::MaxRuntimeExceeded
             || reason == SDP_Solver_Terminate_Reason::PrimalStepTooSmall
             || reason == SDP_Solver_Terminate_Reason::DualStepTooSmall)
            {
              std::stringstream ss;
              ss << "Can not find solution: " << reason;
              throw std::runtime_error(ss.str());
            }

          El::Matrix<El::BigFloat> y(dual_objective_b_star.Height(), 1);
          El::DistMatrix<El::BigFloat, El::STAR, El::STAR> yp_star(
            solver.y.blocks.at(0));

          El::Gemv(El::Orientation::NORMAL, El::BigFloat(1.0),
                   yp_to_y_star.LockedMatrix(), yp_star.LockedMatrix(),
                   El::BigFloat(0.0), y);

          fill_weights(y, max_index, normalization, weights);
          if(rank == 0 && parameters_in.verbosity >= Verbosity::regular)
            {
              set_stream_precision(std::cout);
              std::cout << "weight: " << weights << "\n";

              El::BigFloat optimal(0);
              for(size_t index(0); index < objectives.size(); ++index)
                {
                  optimal += objectives[index] * weights[index];
                }
              std::cout << "optimal: " << optimal << "\n";
            }
          find_new_points(num_blocks, rank, num_procs, infinity,
                          function_blocks, weights, points, new_points,
                          has_new_points);
          if(!has_new_points)
            {
              if(parameters.solver.duality_gap_threshold
                 == parameters_in.solver.duality_gap_threshold)
                {
                  parameters.solver.duality_gap_threshold = 0;
                }
              else
                {
                  parameters.solver.duality_gap_threshold
                    = std::max(parameters.solver.duality_gap_threshold
                                 / parameters.duality_gap_reduction,
                               parameters_in.solver.duality_gap_threshold);
                }
            }
        }
      El::DistMatrix<El::BigFloat, El::STAR, El::STAR> yp_star(
        solver.y.blocks.front());
      copy_matrix(yp_star, yp_saved);
      save_checkpoint(parameters.solver.checkpoint_out,
                      parameters_in.verbosity, yp_to_y_star,
                      dual_objective_b_star, yp_saved, points, infinity,
                      parameters.solver.duality_gap_threshold, primal_c_scale,
                      backup_generation, current_generation);
    }
  return weights;
}
<|endoftext|>


<commit_before>#include <stdexcept>
#include <string>

#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/TransformStamped.h>
#include <goal_sender_msgs/ApplyGoals.h>
#include <goal_sender_msgs/GoalSequence.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <move_base_msgs/MoveBaseGoal.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>

inline double squaring_distance(const geometry_msgs::Point& a, const geometry_msgs::Point& b) {
  const auto x {a.x - b.x};
  const auto y {a.y - b.y};
  return x*x + y*y;
}

/**
 * Waypoint update service manager
 */
class WaypointManager
{
public:
  WaypointManager()
    : sequence_ {},
      now_goal_ {sequence_.waypoints.end()}
  {
  }

  bool operator()(goal_sender_msgs::ApplyGoals::Request& req,
                  goal_sender_msgs::ApplyGoals::Response& res){
    sequence_ = req.goal_sequence;
    now_goal_ = sequence_.waypoints.begin();
    res.success = true;
    res.message = "update waypoints";
    return true;
  }

  const geometry_msgs::Point& point() const
  {
    if (is_end()) throw std::logic_error {"range error: Please check is_end() before point()"};
    return now_goal_->position;
  }

  const geometry_msgs::Quaternion& quaternion() const
  {
    if (is_end()) throw std::logic_error {"range error: Please check is_end() before quaternion()"};
    return now_goal_->orientation;
  }

  double radius() const
  {
    if (is_end()) throw std::logic_error {"range error: Please check is_end() before radius()"};
    return now_goal_->radius;
  }

  bool next()
  {
    if (is_end()) throw std::logic_error {"range error: Please check is_end() before next()"}; // wrong way.
    if (++now_goal_ == sequence_.waypoints.end())
      return false; // correct way: this is last one.
    return true;
  }

  bool is_end() const noexcept
  {
    return sequence_.waypoints.end() == now_goal_;
  }

  explicit operator bool() noexcept
  {
    return !is_end();
  }

  [[deprecated]]
  const goal_sender_msgs::Waypoint& get() const
  {
    if (is_end()) throw std::logic_error {"range error: Please check is_end() before get()"};
    return *now_goal_;
  }

private:
  goal_sender_msgs::GoalSequence sequence_;
  decltype(sequence_.waypoints)::iterator now_goal_;
};

/**
 * Tf lookup API
 */
class TfPositionManager
{
public:
  explicit TfPositionManager(tf2_ros::Buffer& tfBuffer) // buffer is lvalue
    : buffer_ {tfBuffer}
  {
  }

  geometry_msgs::Point operator()(std::string parent, std::string child) const
  {
    const auto ts {buffer_.lookupTransform(parent, child, ros::Time(0))};
    geometry_msgs::Point point;
    point.x = ts.transform.translation.x;
    point.y = ts.transform.translation.y;
    return point;
  }

private:
  const tf2_ros::Buffer& buffer_;
};

class GoalSender
{
public:
  using MoveBaseActionClient = actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction>;

  GoalSender(WaypointManager& point_manager,
             MoveBaseActionClient& move_base_client,
             TfPositionManager lookupper)
    : point_manager_ {point_manager},
      move_base_client_ {move_base_client},
      lookupper_ {lookupper}
  {
  }

  void once()
  {
    if (!point_manager_)
      return; // no work
    if (is_reach()) {
      point_manager_.next();
      send_goal();
    }
  }

private:
  bool is_reach() const
  {
    const auto robot_point {lookupper_("/map", "/base_link")};
    const auto waypoint_point {point_manager_.point()};
    const auto sqr_distance {squaring_distance(robot_point, waypoint_point)};

    const auto radius {point_manager_.radius()};
    const auto sqr_radius {radius * radius};

    if (sqr_distance < sqr_radius) // into valid range
      return true;
    return false;
  }

  void send_goal() const
  {
    if (!point_manager_) { // finish waypoint
      move_base_client_.cancelGoal(); // cancel moveing
      ROS_INFO("Finish waypoints");
      return;
    }

    move_base_msgs::MoveBaseGoal goal;
    goal.target_pose.pose.position = point_manager_.point();
    goal.target_pose.pose.orientation = point_manager_.quaternion();
    goal.target_pose.header.frame_id = "/map";
    goal.target_pose.header.stamp = ros::Time::now();
    move_base_client_.sendGoal(goal); // send waypoint
  }

  WaypointManager& point_manager_;
  MoveBaseActionClient& move_base_client_;
  TfPositionManager lookupper_;
};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "goal_sender");
  ros::NodeHandle nh {};

  WaypointManager point_manager {};
  ros::ServiceServer srv {
      nh.advertiseService<goal_sender_msgs::ApplyGoals::Request,
                          goal_sender_msgs::ApplyGoals::Response>(
          "apply_goals", point_manager)};

  tf2_ros::Buffer tfBuffer;
  tf2_ros::TransformListener tfListener {tfBuffer};
  TfPositionManager lookupper {tfBuffer};

  GoalSender::MoveBaseActionClient move_base_client {"move_base", true};
  move_base_client.waitForServer();

  GoalSender goal_sender {point_manager, move_base_client, lookupper};

  ros::Rate rate {10};
  while (ros::ok()) {
    ros::spinOnce();
    goal_sender.once();
  }
}
<commit_msg>Fix indentation<commit_after>#include <stdexcept>
#include <string>

#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/TransformStamped.h>
#include <goal_sender_msgs/ApplyGoals.h>
#include <goal_sender_msgs/GoalSequence.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <move_base_msgs/MoveBaseGoal.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>

inline double squaring_distance(const geometry_msgs::Point& a, const geometry_msgs::Point& b) {
  const auto x {a.x - b.x};
  const auto y {a.y - b.y};
  return x*x + y*y;
}

/**
 * Waypoint update service manager
 */
class WaypointManager
{
public:
  WaypointManager()
    : sequence_ {},
      now_goal_ {sequence_.waypoints.end()}
  {
  }

  bool operator()(goal_sender_msgs::ApplyGoals::Request& req,
                  goal_sender_msgs::ApplyGoals::Response& res){
    sequence_ = req.goal_sequence;
    now_goal_ = sequence_.waypoints.begin();
    res.success = true;
    res.message = "update waypoints";
    return true;
  }

  const geometry_msgs::Point& point() const
  {
    if (is_end()) throw std::logic_error {"range error: Please check is_end() before point()"};
    return now_goal_->position;
  }

  const geometry_msgs::Quaternion& quaternion() const
  {
    if (is_end()) throw std::logic_error {"range error: Please check is_end() before quaternion()"};
    return now_goal_->orientation;
  }

  double radius() const
  {
    if (is_end()) throw std::logic_error {"range error: Please check is_end() before radius()"};
    return now_goal_->radius;
  }

  bool next()
  {
    if (is_end()) throw std::logic_error {"range error: Please check is_end() before next()"}; // wrong way.
    if (++now_goal_ == sequence_.waypoints.end())
      return false; // correct way: this is last one.
    return true;
  }

  bool is_end() const noexcept
  {
    return sequence_.waypoints.end() == now_goal_;
  }

  explicit operator bool() noexcept
  {
    return !is_end();
  }

  [[deprecated]]
  const goal_sender_msgs::Waypoint& get() const
  {
    if (is_end()) throw std::logic_error {"range error: Please check is_end() before get()"};
    return *now_goal_;
  }

private:
  goal_sender_msgs::GoalSequence sequence_;
  decltype(sequence_.waypoints)::iterator now_goal_;
};

/**
 * Tf lookup API
 */
class TfPositionManager
{
public:
  explicit TfPositionManager(tf2_ros::Buffer& tfBuffer) // buffer is lvalue
    : buffer_ {tfBuffer}
  {
  }

  geometry_msgs::Point operator()(std::string parent, std::string child) const
  {
    const auto ts {buffer_.lookupTransform(parent, child, ros::Time(0))};
    geometry_msgs::Point point;
    point.x = ts.transform.translation.x;
    point.y = ts.transform.translation.y;
    return point;
  }

private:
  const tf2_ros::Buffer& buffer_;
};

class GoalSender
{
public:
  using MoveBaseActionClient = actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction>;

  GoalSender(WaypointManager& point_manager,
             MoveBaseActionClient& move_base_client,
             TfPositionManager lookupper)
    : point_manager_ {point_manager},
      move_base_client_ {move_base_client},
      lookupper_ {lookupper}
  {
  }

  void once()
  {
    if (!point_manager_)
      return; // no work
    if (is_reach()) {
      point_manager_.next();
      send_goal();
    }
  }

private:
  bool is_reach() const
  {
    const auto robot_point {lookupper_("/map", "/base_link")};
    const auto waypoint_point {point_manager_.point()};
    const auto sqr_distance {squaring_distance(robot_point, waypoint_point)};

    const auto radius {point_manager_.radius()};
    const auto sqr_radius {radius * radius};

    if (sqr_distance < sqr_radius) // into valid range
      return true;
    return false;
  }

  void send_goal() const
  {
    if (!point_manager_) { // finish waypoint
      move_base_client_.cancelGoal(); // cancel moveing
      ROS_INFO("Finish waypoints");
      return;
    }

    move_base_msgs::MoveBaseGoal goal;
    goal.target_pose.pose.position = point_manager_.point();
    goal.target_pose.pose.orientation = point_manager_.quaternion();
    goal.target_pose.header.frame_id = "/map";
    goal.target_pose.header.stamp = ros::Time::now();
    move_base_client_.sendGoal(goal); // send waypoint
  }

  WaypointManager& point_manager_;
  MoveBaseActionClient& move_base_client_;
  TfPositionManager lookupper_;
};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "goal_sender");
  ros::NodeHandle nh {};

  WaypointManager point_manager {};
  ros::ServiceServer srv {
      nh.advertiseService<
          goal_sender_msgs::ApplyGoals::Request,
          goal_sender_msgs::ApplyGoals::Response>(
              "apply_goals", point_manager)};

  tf2_ros::Buffer tfBuffer;
  tf2_ros::TransformListener tfListener {tfBuffer};
  TfPositionManager lookupper {tfBuffer};

  GoalSender::MoveBaseActionClient move_base_client {"move_base", true};
  move_base_client.waitForServer();

  GoalSender goal_sender {point_manager, move_base_client, lookupper};

  ros::Rate rate {10};
  while (ros::ok()) {
    ros::spinOnce();
    goal_sender.once();
  }
}
<|endoftext|>


<commit_before>/************************************************************************************\
This source file is part of the APRIL Utility library                                *
For latest info, see http://libapril.sourceforge.net/                                *
**************************************************************************************
Copyright (c) 2010 Kresimir Spes                                                     *
*                                                                                *
* This program is free software; you can redistribute it and/or modify it under      *
* the terms of the BSD license: http://www.opensource.org/licenses/bsd-license.php   *
\************************************************************************************/
#include <april/RenderSystem.h>
#include <gtypes/Matrix4.h>
#include "BlurManager.h"
#include <memory.h>

namespace april
{
	BlurManager::BlurManager()
	{
		mTex1=rendersys->createEmptyTexture(1024,1024,AT_XRGB,AT_RENDER_TARGET);
		mTex2=rendersys->createEmptyTexture(1024,1024,AT_XRGB,AT_RENDER_TARGET);
		mNumLevels=0;
		mEnable=1;
		mInc=1;
	}

	BlurManager::~BlurManager()
	{
		delete mTex1;
		delete mTex2;
	}

	void BlurManager::begin(int nLevels,float inc)
	{
		if (nLevels != mNumLevels && nLevels > 0)
		{
			rendersys->setRenderTarget(mTex1);
			mEnable=1;
		}
		else
		{
			mEnable=0;
		}
		mInc=inc;
		mNumLevels=nLevels;
	}

	void BlurManager::end()
	{
		if (mEnable)
		{
			int i;
			float inc;
			april::Texture* temp;
			april::ColoredTexturedVertex av[6*4];
			gtypes::Matrix4 mat=rendersys->getProjectionMatrix();
			rendersys->setOrthoProjection(1024,1024);
			float k=0.5/1024.0f;
			av[0].x=0;    av[0].y=0;     av[0].u=k;   av[0].v=k;
			av[1].x=1024; av[1].y=0;     av[1].u=1+k; av[1].v=k;
			av[2].x=0;    av[2].y=1024;  av[2].u=k;   av[2].v=1+k;
			av[3].x=1024; av[3].y=0;     av[3].u=1+k; av[3].v=k;
			av[4].x=1024; av[4].y=1024;  av[4].u=1+k; av[4].v=1+k;
			av[5].x=0;    av[5].y=1024;  av[5].u=k;   av[5].v=1+k;
			av[0].color=av[1].color=av[2].color=av[3].color=av[4].color=av[5].color=0xFF404040;
			memcpy(&av[ 6],&av[0],6*sizeof(april::ColoredTexturedVertex));
			memcpy(&av[12],&av[0],6*sizeof(april::ColoredTexturedVertex));
			memcpy(&av[18],&av[0],6*sizeof(april::ColoredTexturedVertex));

			rendersys->setBlendMode(april::ADD);
			for (int j=0;j<mNumLevels;j++)
			{
				rendersys->setRenderTarget(mTex2);
				rendersys->setTexture(mTex1);
				rendersys->clear();
				temp=mTex1; mTex1=mTex2; mTex2=temp;

				inc=mInc;
				for (i= 0;i< 6;i++) av[i].v-=inc*k;
				for (i= 6;i<12;i++) av[i].v+=inc*k;
				for (i=12;i<18;i++) av[i].u-=inc*k;
				for (i=18;i<24;i++) av[i].u+=inc*k;
				rendersys->render(april::TriangleList,av,24);
			}
			rendersys->setBlendMode(april::ALPHA_BLEND);
			rendersys->setRenderTarget(0);
			rendersys->setProjectionMatrix(mat);
		}
	}

	void BlurManager::draw(int w,int h)
	{
		if (mNumLevels)
		{
			float k=0.5/1024.0f;
			rendersys->setTexture(mTex1);
			rendersys->drawTexturedQuad(grect(0,0,w,h),grect(k,k,1,1));
		}
	}
}
<commit_msg>- fixed a bug<commit_after>/************************************************************************************\
This source file is part of the APRIL Utility library                                *
For latest info, see http://libapril.sourceforge.net/                                *
**************************************************************************************
Copyright (c) 2010 Kresimir Spes                                                     *
*                                                                                *
* This program is free software; you can redistribute it and/or modify it under      *
* the terms of the BSD license: http://www.opensource.org/licenses/bsd-license.php   *
\************************************************************************************/
#include <april/RenderSystem.h>
#include <gtypes/Matrix4.h>
#include "BlurManager.h"
#include <memory.h>

namespace april
{
	BlurManager::BlurManager()
	{
		mTex1=rendersys->createEmptyTexture(1024,1024,AT_XRGB,AT_RENDER_TARGET);
		mTex2=rendersys->createEmptyTexture(1024,1024,AT_XRGB,AT_RENDER_TARGET);
		mNumLevels=0;
		mEnable=1;
		mInc=1;
	}

	BlurManager::~BlurManager()
	{
		delete mTex1;
		delete mTex2;
	}

	void BlurManager::begin(int nLevels,float inc)
	{
		if (nLevels != mNumLevels && nLevels > 0)
		{
			rendersys->setRenderTarget(mTex1);
			mEnable=1;
		}
		else
		{
			mEnable=0;
		}
		mInc=inc;
		mNumLevels=nLevels;
	}

	void BlurManager::end()
	{
		if (mEnable)
		{
			int i;
			float inc;
			april::Texture* temp;
			april::ColoredTexturedVertex av[6*4];
			gtypes::Matrix4 mat=rendersys->getProjectionMatrix();
			rendersys->setOrthoProjection(gvec2(1024,1024));
			float k=0.5/1024.0f;
			av[0].x=0;    av[0].y=0;     av[0].u=k;   av[0].v=k;
			av[1].x=1024; av[1].y=0;     av[1].u=1+k; av[1].v=k;
			av[2].x=0;    av[2].y=1024;  av[2].u=k;   av[2].v=1+k;
			av[3].x=1024; av[3].y=0;     av[3].u=1+k; av[3].v=k;
			av[4].x=1024; av[4].y=1024;  av[4].u=1+k; av[4].v=1+k;
			av[5].x=0;    av[5].y=1024;  av[5].u=k;   av[5].v=1+k;
			av[0].color=av[1].color=av[2].color=av[3].color=av[4].color=av[5].color=0xFF404040;
			memcpy(&av[ 6],&av[0],6*sizeof(april::ColoredTexturedVertex));
			memcpy(&av[12],&av[0],6*sizeof(april::ColoredTexturedVertex));
			memcpy(&av[18],&av[0],6*sizeof(april::ColoredTexturedVertex));

			rendersys->setBlendMode(april::ADD);
			for (int j=0;j<mNumLevels;j++)
			{
				rendersys->setRenderTarget(mTex2);
				rendersys->setTexture(mTex1);
				rendersys->clear();
				temp=mTex1; mTex1=mTex2; mTex2=temp;

				inc=mInc;
				for (i= 0;i< 6;i++) av[i].v-=inc*k;
				for (i= 6;i<12;i++) av[i].v+=inc*k;
				for (i=12;i<18;i++) av[i].u-=inc*k;
				for (i=18;i<24;i++) av[i].u+=inc*k;
				rendersys->render(april::TriangleList,av,24);
			}
			rendersys->setBlendMode(april::ALPHA_BLEND);
			rendersys->setRenderTarget(0);
			rendersys->setProjectionMatrix(mat);
		}
	}

	void BlurManager::draw(int w,int h)
	{
		if (mNumLevels)
		{
			float k=0.5/1024.0f;
			rendersys->setTexture(mTex1);
			rendersys->drawTexturedQuad(grect(0,0,w,h),grect(k,k,1,1));
		}
	}
}
<|endoftext|>