<commit_before>#include "ssdt.h"
#include "undocumented.h"
#include "pe.h"
#include "log.h"
#include "ntdll.h"

//structures
struct SSDTStruct
{
    LONG* pServiceTable;
    PVOID pCounterTable;
#ifdef _WIN64
    ULONGLONG NumberOfServices;
#else
    ULONG NumberOfServices;
#endif
    PCHAR pArgumentTable;
};

//Based on: https://github.com/hfiref0x/WinObjEx64
static SSDTStruct* SSDTfind()
{
    static SSDTStruct* SSDT = 0;
    if(!SSDT)
    {
#ifndef _WIN64
        //x86 code
        UNICODE_STRING routineName;
        RtlInitUnicodeString(&routineName, L"KeServiceDescriptorTable");
        SSDT = (SSDTStruct*)MmGetSystemRoutineAddress(&routineName);
#else
        //x64 code
        ULONG kernelSize;
        ULONG_PTR kernelBase = (ULONG_PTR)Undocumented::GetKernelBase(&kernelSize);
        if(kernelBase == 0 || kernelSize == 0)
            return NULL;

        // Find KiSystemServiceStart
        const unsigned char KiSystemServiceStartPattern[] = { 0x8B, 0xF8, 0xC1, 0xEF, 0x07, 0x83, 0xE7, 0x20, 0x25, 0xFF, 0x0F, 0x00, 0x00 };
        const ULONG signatureSize = sizeof(KiSystemServiceStartPattern);
        bool found = false;
        ULONG KiSSSOffset;
        for(KiSSSOffset = 0; KiSSSOffset < kernelSize - signatureSize; KiSSSOffset++)
        {
            if(RtlCompareMemory(((unsigned char*)kernelBase + KiSSSOffset), KiSystemServiceStartPattern, signatureSize) == signatureSize)
            {
                found = true;
                break;
            }
        }
        if(!found)
            return NULL;

        // lea r10, KeServiceDescriptorTable
        ULONG_PTR address = kernelBase + KiSSSOffset + signatureSize;
        LONG relativeOffset = 0;
        if((*(unsigned char*)address == 0x4c) &&
                (*(unsigned char*)(address + 1) == 0x8d) &&
                (*(unsigned char*)(address + 2) == 0x15))
        {
            relativeOffset = *(LONG*)(address + 3);
        }
        if(relativeOffset == 0)
            return NULL;

        SSDT = (SSDTStruct*)(address + relativeOffset + 7);
#endif
    }
    return SSDT;
}

PVOID SSDT::GetFunctionAddress(const char* apiname)
{
    //read address from SSDT
    SSDTStruct* SSDT = SSDTfind();
    if(!SSDT)
    {
        Log("[TITANHIDE] SSDT not found...\r\n");
        return 0;
    }
    ULONG_PTR SSDTbase = (ULONG_PTR)SSDT->pServiceTable;
    if(!SSDTbase)
    {
        Log("[TITANHIDE] ServiceTable not found...\r\n");
        return 0;
    }
    ULONG readOffset = NTDLL::GetExportSsdtIndex(apiname);
    if(readOffset == -1)
        return 0;
    if(readOffset >= SSDT->NumberOfServices)
    {
        Log("[TITANHIDE] Invalid read offset...\r\n");
        return 0;
    }
#ifdef _WIN64
    return (PVOID)((SSDT->pServiceTable[readOffset] >> 4) + SSDTbase);
#else
    return (PVOID)SSDT->pServiceTable[readOffset];
#endif
}

static void InterlockedSet(LONG* Destination, LONG Source)
{
    //Change memory properties.
    PMDL g_pmdl = IoAllocateMdl(Destination, sizeof(LONG), 0, 0, NULL);
    if(!g_pmdl)
        return;
    MmBuildMdlForNonPagedPool(g_pmdl);
    LONG* Mapped = (LONG*)MmMapLockedPages(g_pmdl, KernelMode);
    if(!Mapped)
    {
        IoFreeMdl(g_pmdl);
        return;
    }
    InterlockedExchange(Mapped, Source);
    //Restore memory properties.
    MmUnmapLockedPages((PVOID)Mapped, g_pmdl);
    IoFreeMdl(g_pmdl);
}

#ifdef _WIN64
static PVOID FindCaveAddress(PVOID CodeStart, ULONG CodeSize, ULONG CaveSize)
{
    unsigned char* Code = (unsigned char*)CodeStart;

    for(unsigned int i = 0, j = 0; i < CodeSize; i++)
    {
        if(Code[i] == 0x90 || Code[i] == 0xCC)  //NOP or INT3
            j++;
        else
            j = 0;
        if(j == CaveSize)
            return (PVOID)((ULONG_PTR)CodeStart + i - CaveSize + 1);
    }
    return 0;
}
#endif //_WIN64

HOOK SSDT::Hook(const char* apiname, void* newfunc)
{
    SSDTStruct* SSDT = SSDTfind();
    if(!SSDT)
    {
        Log("[TITANHIDE] SSDT not found...\r\n");
        return 0;
    }
    ULONG_PTR SSDTbase = (ULONG_PTR)SSDT->pServiceTable;
    if(!SSDTbase)
    {
        Log("[TITANHIDE] ServiceTable not found...\r\n");
        return 0;
    }
    int FunctionIndex = NTDLL::GetExportSsdtIndex(apiname);
    if(FunctionIndex == -1)
        return 0;
    if(FunctionIndex >= SSDT->NumberOfServices)
    {
        Log("[TITANHIDE] Invalid API offset...\r\n");
        return 0;
    }

    HOOK hHook = 0;
    LONG oldValue = SSDT->pServiceTable[FunctionIndex];
    LONG newValue;

#ifdef _WIN64
    /*
    x64 SSDT Hook;
    1) find API addr
    2) get code page+size
    3) find cave address
    4) hook cave address (using hooklib)
    5) change SSDT value
    */

    static ULONG CodeSize = 0;
    static PVOID CodeStart = 0;
    if(!CodeStart)
    {
        ULONG_PTR Lowest = SSDTbase;
        ULONG_PTR Highest = Lowest + 0x0FFFFFFF;
        Log("[TITANHIDE] Range: 0x%p-0x%p\r\n", Lowest, Highest);
        CodeSize = 0;
        CodeStart = PE::GetPageBase(Undocumented::GetKernelBase(), &CodeSize, (PVOID)((oldValue >> 4) + SSDTbase));
        if(!CodeStart || !CodeSize)
        {
            Log("[TITANHIDE] PeGetPageBase failed...\r\n");
            return 0;
        }
        Log("[TITANHIDE] CodeStart: 0x%p, CodeSize: 0x%X\r\n", CodeStart, CodeSize);
        if((ULONG_PTR)CodeStart < Lowest)  //start of the page is out of range (impossible, but whatever)
        {
            CodeSize -= (ULONG)(Lowest - (ULONG_PTR)CodeStart);
            CodeStart = (PVOID)Lowest;
            Log("[TITANHIDE] CodeStart: 0x%p, CodeSize: 0x%X\r\n", CodeStart, CodeSize);
        }
        Log("[TITANHIDE] Range: 0x%p-0x%p\r\n", CodeStart, (ULONG_PTR)CodeStart + CodeSize);
    }

    PVOID CaveAddress = FindCaveAddress(CodeStart, CodeSize, sizeof(HOOKOPCODES));
    if(!CaveAddress)
    {
        Log("[TITANHIDE] FindCaveAddress failed...\r\n");
        return 0;
    }
    Log("[TITANHIDE] CaveAddress: 0x%p\r\n", CaveAddress);

    hHook = Hooklib::Hook(CaveAddress, (void*)newfunc);
    if(!hHook)
        return 0;

    newValue = (LONG)((ULONG_PTR)CaveAddress - SSDTbase);
    newValue = (newValue << 4) | oldValue & 0xF;

    //update HOOK structure
    hHook->SSDTindex = FunctionIndex;
    hHook->SSDTold = oldValue;
    hHook->SSDTnew = newValue;
    hHook->SSDTaddress = (oldValue >> 4) + SSDTbase;

#else
    /*
    x86 SSDT Hook:
    1) change SSDT value
    */
    newValue = (ULONG)newfunc;

    hHook = (HOOK)RtlAllocateMemory(true, sizeof(HOOKSTRUCT));

    //update HOOK structure
    hHook->SSDTindex = FunctionIndex;
    hHook->SSDTold = oldValue;
    hHook->SSDTnew = newValue;
    hHook->SSDTaddress = oldValue;

#endif

    InterlockedSet(&SSDT->pServiceTable[FunctionIndex], newValue);

    Log("[TITANHIDE] SSDThook(%s:0x%p, 0x%p)\r\n", apiname, hHook->SSDTold, hHook->SSDTnew);

    return hHook;
}

void SSDT::Hook(HOOK hHook)
{
    if(!hHook)
        return;
    SSDTStruct* SSDT = SSDTfind();
    if(!SSDT)
    {
        Log("[TITANHIDE] SSDT not found...\r\n");
        return;
    }
    LONG* SSDT_Table = SSDT->pServiceTable;
    if(!SSDT_Table)
    {
        Log("[TITANHIDE] ServiceTable not found...\r\n");
        return;
    }
    InterlockedSet(&SSDT_Table[hHook->SSDTindex], hHook->SSDTnew);
}

void SSDT::Unhook(HOOK hHook, bool free)
{
    if(!hHook)
        return;
    SSDTStruct* SSDT = SSDTfind();
    if(!SSDT)
    {
        Log("[TITANHIDE] SSDT not found...\r\n");
        return;
    }
    LONG* SSDT_Table = SSDT->pServiceTable;
    if(!SSDT_Table)
    {
        Log("[TITANHIDE] ServiceTable not found...\r\n");
        return;
    }
    InterlockedSet(&SSDT_Table[hHook->SSDTindex], hHook->SSDTold);
#ifdef _WIN64
    if(free)
        Hooklib::Unhook(hHook, true);
#else
    if(free)
        RtlFreeMemory(hHook);
#endif
}<commit_msg>fixed compile error<commit_after>#include "ssdt.h"
#include "undocumented.h"
#include "pe.h"
#include "log.h"
#include "ntdll.h"

//structures
struct SSDTStruct
{
    LONG* pServiceTable;
    PVOID pCounterTable;
#ifdef _WIN64
    ULONGLONG NumberOfServices;
#else
    ULONG NumberOfServices;
#endif
    PCHAR pArgumentTable;
};

//Based on: https://github.com/hfiref0x/WinObjEx64
static SSDTStruct* SSDTfind()
{
    static SSDTStruct* SSDT = 0;
    if(!SSDT)
    {
#ifndef _WIN64
        //x86 code
        UNICODE_STRING routineName;
        RtlInitUnicodeString(&routineName, L"KeServiceDescriptorTable");
        SSDT = (SSDTStruct*)MmGetSystemRoutineAddress(&routineName);
#else
        //x64 code
        ULONG kernelSize;
        ULONG_PTR kernelBase = (ULONG_PTR)Undocumented::GetKernelBase(&kernelSize);
        if(kernelBase == 0 || kernelSize == 0)
            return NULL;

        // Find KiSystemServiceStart
        const unsigned char KiSystemServiceStartPattern[] = { 0x8B, 0xF8, 0xC1, 0xEF, 0x07, 0x83, 0xE7, 0x20, 0x25, 0xFF, 0x0F, 0x00, 0x00 };
        const ULONG signatureSize = sizeof(KiSystemServiceStartPattern);
        bool found = false;
        ULONG KiSSSOffset;
        for(KiSSSOffset = 0; KiSSSOffset < kernelSize - signatureSize; KiSSSOffset++)
        {
            if(RtlCompareMemory(((unsigned char*)kernelBase + KiSSSOffset), KiSystemServiceStartPattern, signatureSize) == signatureSize)
            {
                found = true;
                break;
            }
        }
        if(!found)
            return NULL;

        // lea r10, KeServiceDescriptorTable
        ULONG_PTR address = kernelBase + KiSSSOffset + signatureSize;
        LONG relativeOffset = 0;
        if((*(unsigned char*)address == 0x4c) &&
                (*(unsigned char*)(address + 1) == 0x8d) &&
                (*(unsigned char*)(address + 2) == 0x15))
        {
            relativeOffset = *(LONG*)(address + 3);
        }
        if(relativeOffset == 0)
            return NULL;

        SSDT = (SSDTStruct*)(address + relativeOffset + 7);
#endif
    }
    return SSDT;
}

PVOID SSDT::GetFunctionAddress(const char* apiname)
{
    //read address from SSDT
    SSDTStruct* SSDT = SSDTfind();
    if(!SSDT)
    {
        Log("[TITANHIDE] SSDT not found...\r\n");
        return 0;
    }
    ULONG_PTR SSDTbase = (ULONG_PTR)SSDT->pServiceTable;
    if(!SSDTbase)
    {
        Log("[TITANHIDE] ServiceTable not found...\r\n");
        return 0;
    }
    ULONG readOffset = NTDLL::GetExportSsdtIndex(apiname);
    if(readOffset == -1)
        return 0;
    if(readOffset >= SSDT->NumberOfServices)
    {
        Log("[TITANHIDE] Invalid read offset...\r\n");
        return 0;
    }
#ifdef _WIN64
    return (PVOID)((SSDT->pServiceTable[readOffset] >> 4) + SSDTbase);
#else
    return (PVOID)SSDT->pServiceTable[readOffset];
#endif
}

static void InterlockedSet(LONG* Destination, LONG Source)
{
    //Change memory properties.
    PMDL g_pmdl = IoAllocateMdl(Destination, sizeof(LONG), 0, 0, NULL);
    if(!g_pmdl)
        return;
    MmBuildMdlForNonPagedPool(g_pmdl);
    LONG* Mapped = (LONG*)MmMapLockedPages(g_pmdl, KernelMode);
    if(!Mapped)
    {
        IoFreeMdl(g_pmdl);
        return;
    }
    InterlockedExchange(Mapped, Source);
    //Restore memory properties.
    MmUnmapLockedPages((PVOID)Mapped, g_pmdl);
    IoFreeMdl(g_pmdl);
}

#ifdef _WIN64
static PVOID FindCaveAddress(PVOID CodeStart, ULONG CodeSize, ULONG CaveSize)
{
    unsigned char* Code = (unsigned char*)CodeStart;

    for(unsigned int i = 0, j = 0; i < CodeSize; i++)
    {
        if(Code[i] == 0x90 || Code[i] == 0xCC)  //NOP or INT3
            j++;
        else
            j = 0;
        if(j == CaveSize)
            return (PVOID)((ULONG_PTR)CodeStart + i - CaveSize + 1);
    }
    return 0;
}
#endif //_WIN64

HOOK SSDT::Hook(const char* apiname, void* newfunc)
{
    SSDTStruct* SSDT = SSDTfind();
    if(!SSDT)
    {
        Log("[TITANHIDE] SSDT not found...\r\n");
        return 0;
    }
    ULONG_PTR SSDTbase = (ULONG_PTR)SSDT->pServiceTable;
    if(!SSDTbase)
    {
        Log("[TITANHIDE] ServiceTable not found...\r\n");
        return 0;
    }
    int FunctionIndex = NTDLL::GetExportSsdtIndex(apiname);
    if(FunctionIndex == -1)
        return 0;
    if((ULONGLONG)FunctionIndex >= SSDT->NumberOfServices)
    {
        Log("[TITANHIDE] Invalid API offset...\r\n");
        return 0;
    }

    HOOK hHook = 0;
    LONG oldValue = SSDT->pServiceTable[FunctionIndex];
    LONG newValue;

#ifdef _WIN64
    /*
    x64 SSDT Hook;
    1) find API addr
    2) get code page+size
    3) find cave address
    4) hook cave address (using hooklib)
    5) change SSDT value
    */

    static ULONG CodeSize = 0;
    static PVOID CodeStart = 0;
    if(!CodeStart)
    {
        ULONG_PTR Lowest = SSDTbase;
        ULONG_PTR Highest = Lowest + 0x0FFFFFFF;
        Log("[TITANHIDE] Range: 0x%p-0x%p\r\n", Lowest, Highest);
        CodeSize = 0;
        CodeStart = PE::GetPageBase(Undocumented::GetKernelBase(), &CodeSize, (PVOID)((oldValue >> 4) + SSDTbase));
        if(!CodeStart || !CodeSize)
        {
            Log("[TITANHIDE] PeGetPageBase failed...\r\n");
            return 0;
        }
        Log("[TITANHIDE] CodeStart: 0x%p, CodeSize: 0x%X\r\n", CodeStart, CodeSize);
        if((ULONG_PTR)CodeStart < Lowest)  //start of the page is out of range (impossible, but whatever)
        {
            CodeSize -= (ULONG)(Lowest - (ULONG_PTR)CodeStart);
            CodeStart = (PVOID)Lowest;
            Log("[TITANHIDE] CodeStart: 0x%p, CodeSize: 0x%X\r\n", CodeStart, CodeSize);
        }
        Log("[TITANHIDE] Range: 0x%p-0x%p\r\n", CodeStart, (ULONG_PTR)CodeStart + CodeSize);
    }

    PVOID CaveAddress = FindCaveAddress(CodeStart, CodeSize, sizeof(HOOKOPCODES));
    if(!CaveAddress)
    {
        Log("[TITANHIDE] FindCaveAddress failed...\r\n");
        return 0;
    }
    Log("[TITANHIDE] CaveAddress: 0x%p\r\n", CaveAddress);

    hHook = Hooklib::Hook(CaveAddress, (void*)newfunc);
    if(!hHook)
        return 0;

    newValue = (LONG)((ULONG_PTR)CaveAddress - SSDTbase);
    newValue = (newValue << 4) | oldValue & 0xF;

    //update HOOK structure
    hHook->SSDTindex = FunctionIndex;
    hHook->SSDTold = oldValue;
    hHook->SSDTnew = newValue;
    hHook->SSDTaddress = (oldValue >> 4) + SSDTbase;

#else
    /*
    x86 SSDT Hook:
    1) change SSDT value
    */
    newValue = (ULONG)newfunc;

    hHook = (HOOK)RtlAllocateMemory(true, sizeof(HOOKSTRUCT));

    //update HOOK structure
    hHook->SSDTindex = FunctionIndex;
    hHook->SSDTold = oldValue;
    hHook->SSDTnew = newValue;
    hHook->SSDTaddress = oldValue;

#endif

    InterlockedSet(&SSDT->pServiceTable[FunctionIndex], newValue);

    Log("[TITANHIDE] SSDThook(%s:0x%p, 0x%p)\r\n", apiname, hHook->SSDTold, hHook->SSDTnew);

    return hHook;
}

void SSDT::Hook(HOOK hHook)
{
    if(!hHook)
        return;
    SSDTStruct* SSDT = SSDTfind();
    if(!SSDT)
    {
        Log("[TITANHIDE] SSDT not found...\r\n");
        return;
    }
    LONG* SSDT_Table = SSDT->pServiceTable;
    if(!SSDT_Table)
    {
        Log("[TITANHIDE] ServiceTable not found...\r\n");
        return;
    }
    InterlockedSet(&SSDT_Table[hHook->SSDTindex], hHook->SSDTnew);
}

void SSDT::Unhook(HOOK hHook, bool free)
{
    if(!hHook)
        return;
    SSDTStruct* SSDT = SSDTfind();
    if(!SSDT)
    {
        Log("[TITANHIDE] SSDT not found...\r\n");
        return;
    }
    LONG* SSDT_Table = SSDT->pServiceTable;
    if(!SSDT_Table)
    {
        Log("[TITANHIDE] ServiceTable not found...\r\n");
        return;
    }
    InterlockedSet(&SSDT_Table[hHook->SSDTindex], hHook->SSDTold);
#ifdef _WIN64
    if(free)
        Hooklib::Unhook(hHook, true);
#else
    if(free)
        RtlFreeMemory(hHook);
#endif
}<|endoftext|>


<commit_before>/*! \file generate_roadmap.cpp
 * \author Chris Dellin <cdellin@gmail.com>
 * \copyright 2015 Carnegie Mellon University
 * \copyright License: BSD
 */

#include <algorithm>
#include <fstream>

#include <boost/property_map/property_map.hpp>
#include <boost/property_map/dynamic_property_map.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graphml.hpp>
#include <boost/program_options.hpp>
#include <ompl/base/StateSpace.h>
#include <ompl/base/ScopedState.h>
#include <ompl/base/spaces/RealVectorStateSpace.h>
#include <ompl/datastructures/NearestNeighbors.h>

#include <pr_bgl/graph_io.h>
#include <pr_bgl/string_map.h>
#include <pr_bgl/vector_ref_property_map.h>
#include <pr_bgl/edge_indexed_graph.h>

#include <ompl_lemur/util.h>
#include <ompl_lemur/rvstate_map_string_adaptor.h>
#include <ompl_lemur/FnString.h>
#include <ompl_lemur/SamplerGenMonkeyPatch.h>
#include <ompl_lemur/NearestNeighborsLinearBGL.h>
#include <ompl_lemur/Roadmap.h>
#include <ompl_lemur/RoadmapAAGrid.h>
#include <ompl_lemur/RoadmapFromFile.h>
#include <ompl_lemur/RoadmapHalton.h>
#include <ompl_lemur/RoadmapHaltonDens.h>
#include <ompl_lemur/RoadmapHaltonOffDens.h>
#include <ompl_lemur/RoadmapRGG.h>
#include <ompl_lemur/RoadmapRGGDens.h>
#include <ompl_lemur/RoadmapRGGDensConst.h>

struct VertexProperties
{
   ompl::base::State * state;
   int batch;
   bool is_shadow;
};
struct EdgeProperties
{
   std::size_t index;
   double distance;
   int batch;
};

typedef boost::adjacency_list<
   boost::vecS, // Edgelist ds, for per-vertex out-edges
   boost::vecS, // VertexList ds, for vertex set
   boost::undirectedS, // type of graph
   VertexProperties, // internal (bundled) vertex properties
   EdgeProperties // internal (bundled) edge properties
   > Graph;

typedef boost::graph_traits<Graph>::edge_descriptor Edge;

typedef boost::property_map<Graph, boost::vertex_index_t>::type VertexIndexMap;
typedef boost::property_map<Graph, std::size_t EdgeProperties::*>::type EdgeIndexMap;

typedef boost::property_map<Graph, ompl::base::State * VertexProperties::*>::type StateMap;
typedef boost::property_map<Graph, int VertexProperties::*>::type VertexBatchMap;
typedef boost::property_map<Graph, int EdgeProperties::*>::type EdgeBatchMap;
typedef boost::property_map<Graph, bool VertexProperties::*>::type IsShadowMap;
typedef boost::property_map<Graph, double EdgeProperties::*>::type DistanceMap;

typedef pr_bgl::edge_indexed_graph<Graph, EdgeIndexMap> EdgeIndexedGraph;
typedef ompl_lemur::NearestNeighborsLinearBGL<EdgeIndexedGraph,StateMap> NN;

typedef ompl_lemur::RoadmapArgs<EdgeIndexedGraph,StateMap,DistanceMap,VertexBatchMap,EdgeBatchMap,IsShadowMap,EdgeIndexedGraph::EdgeVectorMap,NN> RoadmapArgs;
typedef boost::shared_ptr< ompl_lemur::Roadmap<RoadmapArgs> > RoadmapPtr;

int main(int argc, char **argv)
{
   boost::program_options::options_description desc("Allowed options");
   desc.add_options()
      ("help", "produce help message")
      ("dim", boost::program_options::value<int>(), "hypercube dimension (e.g. 2)")
      ("bounds", boost::program_options::value< std::vector<std::string> >(), "(e.g. 0:-2:2)")
      ("roadmap-type", boost::program_options::value<std::string>(), "(e.g. Halton)")
      ("roadmap-param", boost::program_options::value< std::vector<std::string> >(), "(e.g. num=30)")
      ("num-batches", boost::program_options::value<std::size_t>(), "number of batches (e.g. 1)")
      ("out-file", boost::program_options::value<std::string>(), "output file (can be - for stdout)")
      ("out-format", boost::program_options::value<std::string>(), "output format (graphml or graphio)")
   ;

   boost::program_options::variables_map args;
   boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), args);
   boost::program_options::notify(args);

   if (args.count("help"))
   {
      std::cout << desc << std::endl;
      return 0;
   }

   const char * reqs[] = {"dim", "roadmap-type", "num-batches", "out-file", "out-format"};
   for (unsigned int ui=0; ui<sizeof(reqs)/sizeof(reqs[0]); ui++)
   {
      if (args.count(reqs[ui]) != 1)
      {
         OMPL_ERROR("--%s must be passed.", reqs[ui]);
         return 1;
      }
   }

   int dim = args["dim"].as<int>();
   OMPL_INFORM("Creating unit ompl space of dimension %d ...", dim);
   ompl::base::StateSpacePtr space(new ompl::base::RealVectorStateSpace(dim));
   ompl::base::RealVectorBounds space_bounds(dim);
   space_bounds.setLow(0.0);
   space_bounds.setHigh(1.0);

   const std::vector<std::string> & bounds = args["bounds"].as< std::vector<std::string> >();
   for (unsigned int ui=0; ui<bounds.size(); ui++)
   {
      int idof;
      double lower;
      double upper;
      int n;
      int ret;
      ret = sscanf(bounds[ui].c_str(), "%d:%lf,%lf%n", &idof, &lower, &upper, &n);
      if (ret != 3 || n != (int)bounds[ui].size() || idof<0 || dim<=idof)
      {
         OMPL_ERROR("--bounds %s not properly formatted.", bounds[ui].c_str());
         return 1;
      }
      space_bounds.setLow(idof, lower);
      space_bounds.setHigh(idof, upper);
   }

   space->as<ompl::base::RealVectorStateSpace>()->setBounds(space_bounds);

   RoadmapPtr p_mygen;

   Graph g;

   pr_bgl::edge_indexed_graph<Graph, EdgeIndexMap>
      eig(g, get(&EdgeProperties::index, g));

   NN nnlin(eig, get(&VertexProperties::state,g), space);

   // construct roadmap
   RoadmapArgs rmargs(space, eig,
      get(&VertexProperties::state, g),
      get(&EdgeProperties::distance, g),
      get(&VertexProperties::batch, g),
      get(&EdgeProperties::batch, g),
      get(&VertexProperties::is_shadow, g),
      eig.edge_vector_map,
      &nnlin);

   if (args["roadmap-type"].as<std::string>() == "AAGrid")
      p_mygen.reset(new ompl_lemur::RoadmapAAGrid<RoadmapArgs>(rmargs));
   else if (args["roadmap-type"].as<std::string>() == "FromFile")
      p_mygen.reset(new ompl_lemur::RoadmapFromFile<RoadmapArgs>(rmargs));
   else if (args["roadmap-type"].as<std::string>() == "RGG")
      p_mygen.reset(new ompl_lemur::RoadmapRGG<RoadmapArgs>(rmargs));
   else if (args["roadmap-type"].as<std::string>() == "RGGDens")
      p_mygen.reset(new ompl_lemur::RoadmapRGGDens<RoadmapArgs>(rmargs));
   else if (args["roadmap-type"].as<std::string>() == "RGGDensConst")
      p_mygen.reset(new ompl_lemur::RoadmapRGGDensConst<RoadmapArgs>(rmargs));
   else if (args["roadmap-type"].as<std::string>() == "Halton")
      p_mygen.reset(new ompl_lemur::RoadmapHalton<RoadmapArgs>(rmargs));
   else if (args["roadmap-type"].as<std::string>() == "HaltonDens")
      p_mygen.reset(new ompl_lemur::RoadmapHaltonDens<RoadmapArgs>(rmargs));
   else if (args["roadmap-type"].as<std::string>() == "HaltonOffDens")
      p_mygen.reset(new ompl_lemur::RoadmapHaltonOffDens<RoadmapArgs>(rmargs));
   else
   {
      OMPL_ERROR("--roadmap-type unknown!");
      return 1;
   }

   const std::vector<std::string> & params = args["roadmap-param"].as< std::vector<std::string> >();
   for (unsigned int ui=0; ui<params.size(); ui++)
   {
      size_t eq = params[ui].find('=');
      if (eq == params[ui].npos)
      {
         OMPL_ERROR("--roadmap-param has bad format!");
         return 1;
      }
      p_mygen->params.setParam(params[ui].substr(0,eq), params[ui].substr(eq+1));
   }

   std::size_t num_batches = args["num-batches"].as<std::size_t>();
   OMPL_INFORM("Generating %lu batch%s ...", num_batches, num_batches==1?"":"es");

   p_mygen->initialize();
   while (p_mygen->num_batches_generated < num_batches)
   {
      // generate a graph
      p_mygen->generate();
   }

   OMPL_INFORM("Generated graph has %lu vertices and %lu edges.", num_vertices(g), num_edges(g));

   // write it out to file
   boost::dynamic_properties props;
   props.property("state", ompl_lemur::make_rvstate_map_string_adaptor(
      get(&VertexProperties::state,g),
      space->as<ompl::base::RealVectorStateSpace>()));
   props.property("batch", pr_bgl::make_string_map(get(&VertexProperties::batch,g)));
   props.property("batch", pr_bgl::make_string_map(get(&EdgeProperties::batch,g)));
   props.property("is_shadow", pr_bgl::make_string_map(get(&VertexProperties::is_shadow,g)));
   props.property("distance", pr_bgl::make_string_map(get(&EdgeProperties::distance,g)));

   std::ostream * outp;
   std::ofstream fp;
   std::string out_file = args["out-file"].as<std::string>();
   if (out_file == "-")
      outp = &std::cout;
   else
   {
      fp.open(out_file.c_str());
      assert(fp.is_open());
      outp = &fp;
   }

   std::string out_format = args["out-format"].as<std::string>();
   if (out_format == "graphio")
   {
      OMPL_INFORM("Writing to graphio file ...");
      pr_bgl::write_graphio_graph(*outp, g,
         get(boost::vertex_index,g), get(&EdgeProperties::index,g));
      pr_bgl::write_graphio_properties(*outp, g,
         get(boost::vertex_index,g), get(&EdgeProperties::index,g),
         props);
   }
   else if (out_format == "graphml")
   {
      OMPL_INFORM("Writing to graphml file ...");
      boost::write_graphml(*outp, g, get(boost::vertex_index,g), props, true); // ordered_vertices
   }
   else
   {
      OMPL_ERROR("--out-format must be graphio or graphml!");
      return 1;
   }

   if (fp.is_open())
      fp.close();

   return 0;
}
<commit_msg>fix generate-roadmap handing of optional --bounds and --roadmap-param args, handle outfile writing errors<commit_after>/*! \file generate_roadmap.cpp
 * \author Chris Dellin <cdellin@gmail.com>
 * \copyright 2015 Carnegie Mellon University
 * \copyright License: BSD
 */

#include <algorithm>
#include <fstream>

#include <boost/property_map/property_map.hpp>
#include <boost/property_map/dynamic_property_map.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graphml.hpp>
#include <boost/program_options.hpp>
#include <ompl/base/StateSpace.h>
#include <ompl/base/ScopedState.h>
#include <ompl/base/spaces/RealVectorStateSpace.h>
#include <ompl/datastructures/NearestNeighbors.h>

#include <pr_bgl/graph_io.h>
#include <pr_bgl/string_map.h>
#include <pr_bgl/vector_ref_property_map.h>
#include <pr_bgl/edge_indexed_graph.h>

#include <ompl_lemur/util.h>
#include <ompl_lemur/rvstate_map_string_adaptor.h>
#include <ompl_lemur/FnString.h>
#include <ompl_lemur/SamplerGenMonkeyPatch.h>
#include <ompl_lemur/NearestNeighborsLinearBGL.h>
#include <ompl_lemur/Roadmap.h>
#include <ompl_lemur/RoadmapAAGrid.h>
#include <ompl_lemur/RoadmapFromFile.h>
#include <ompl_lemur/RoadmapHalton.h>
#include <ompl_lemur/RoadmapHaltonDens.h>
#include <ompl_lemur/RoadmapHaltonOffDens.h>
#include <ompl_lemur/RoadmapRGG.h>
#include <ompl_lemur/RoadmapRGGDens.h>
#include <ompl_lemur/RoadmapRGGDensConst.h>

struct VertexProperties
{
   ompl::base::State * state;
   int batch;
   bool is_shadow;
};
struct EdgeProperties
{
   std::size_t index;
   double distance;
   int batch;
};

typedef boost::adjacency_list<
   boost::vecS, // Edgelist ds, for per-vertex out-edges
   boost::vecS, // VertexList ds, for vertex set
   boost::undirectedS, // type of graph
   VertexProperties, // internal (bundled) vertex properties
   EdgeProperties // internal (bundled) edge properties
   > Graph;

typedef boost::graph_traits<Graph>::edge_descriptor Edge;

typedef boost::property_map<Graph, boost::vertex_index_t>::type VertexIndexMap;
typedef boost::property_map<Graph, std::size_t EdgeProperties::*>::type EdgeIndexMap;

typedef boost::property_map<Graph, ompl::base::State * VertexProperties::*>::type StateMap;
typedef boost::property_map<Graph, int VertexProperties::*>::type VertexBatchMap;
typedef boost::property_map<Graph, int EdgeProperties::*>::type EdgeBatchMap;
typedef boost::property_map<Graph, bool VertexProperties::*>::type IsShadowMap;
typedef boost::property_map<Graph, double EdgeProperties::*>::type DistanceMap;

typedef pr_bgl::edge_indexed_graph<Graph, EdgeIndexMap> EdgeIndexedGraph;
typedef ompl_lemur::NearestNeighborsLinearBGL<EdgeIndexedGraph,StateMap> NN;

typedef ompl_lemur::RoadmapArgs<EdgeIndexedGraph,StateMap,DistanceMap,VertexBatchMap,EdgeBatchMap,IsShadowMap,EdgeIndexedGraph::EdgeVectorMap,NN> RoadmapArgs;
typedef boost::shared_ptr< ompl_lemur::Roadmap<RoadmapArgs> > RoadmapPtr;

int main(int argc, char **argv)
{
   boost::program_options::options_description desc("Allowed options");
   desc.add_options()
      ("help", "produce help message")
      ("dim", boost::program_options::value<int>(), "hypercube dimension (e.g. 2)")
      ("bounds", boost::program_options::value< std::vector<std::string> >(), "(e.g. 0:-2:2)")
      ("roadmap-type", boost::program_options::value<std::string>(), "(e.g. Halton)")
      ("roadmap-param", boost::program_options::value< std::vector<std::string> >(), "(e.g. num=30)")
      ("num-batches", boost::program_options::value<std::size_t>(), "number of batches (e.g. 1)")
      ("out-file", boost::program_options::value<std::string>(), "output file (can be - for stdout)")
      ("out-format", boost::program_options::value<std::string>(), "output format (graphml or graphio)")
   ;

   boost::program_options::variables_map args;
   boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), args);
   boost::program_options::notify(args);

   if (args.count("help"))
   {
      std::cout << desc << std::endl;
      return 0;
   }

   const char * reqs[] = {"dim", "roadmap-type", "num-batches", "out-file", "out-format"};
   for (unsigned int ui=0; ui<sizeof(reqs)/sizeof(reqs[0]); ui++)
   {
      if (args.count(reqs[ui]) != 1)
      {
         OMPL_ERROR("--%s must be passed.", reqs[ui]);
         return 1;
      }
   }

   int dim = args["dim"].as<int>();
   OMPL_INFORM("Creating unit ompl space of dimension %d ...", dim);
   ompl::base::StateSpacePtr space(new ompl::base::RealVectorStateSpace(dim));
   ompl::base::RealVectorBounds space_bounds(dim);
   space_bounds.setLow(0.0);
   space_bounds.setHigh(1.0);

   if (args.count("bounds"))
   {
      const std::vector<std::string> & bounds = args["bounds"].as< std::vector<std::string> >();

      for (unsigned int ui=0; ui<bounds.size(); ui++)
      {
         int idof;
         double lower;
         double upper;
         int n;
         int ret;
         ret = sscanf(bounds[ui].c_str(), "%d:%lf,%lf%n", &idof, &lower, &upper, &n);
         if (ret != 3 || n != (int)bounds[ui].size() || idof<0 || dim<=idof)
         {
            OMPL_ERROR("--bounds %s not properly formatted.", bounds[ui].c_str());
            return 1;
         }
         space_bounds.setLow(idof, lower);
         space_bounds.setHigh(idof, upper);
      }
   }

   space->as<ompl::base::RealVectorStateSpace>()->setBounds(space_bounds);

   RoadmapPtr p_mygen;

   Graph g;

   pr_bgl::edge_indexed_graph<Graph, EdgeIndexMap>
      eig(g, get(&EdgeProperties::index, g));

   NN nnlin(eig, get(&VertexProperties::state,g), space);

   // construct roadmap
   RoadmapArgs rmargs(space, eig,
      get(&VertexProperties::state, g),
      get(&EdgeProperties::distance, g),
      get(&VertexProperties::batch, g),
      get(&EdgeProperties::batch, g),
      get(&VertexProperties::is_shadow, g),
      eig.edge_vector_map,
      &nnlin);

   if (args["roadmap-type"].as<std::string>() == "AAGrid")
      p_mygen.reset(new ompl_lemur::RoadmapAAGrid<RoadmapArgs>(rmargs));
   else if (args["roadmap-type"].as<std::string>() == "FromFile")
      p_mygen.reset(new ompl_lemur::RoadmapFromFile<RoadmapArgs>(rmargs));
   else if (args["roadmap-type"].as<std::string>() == "RGG")
      p_mygen.reset(new ompl_lemur::RoadmapRGG<RoadmapArgs>(rmargs));
   else if (args["roadmap-type"].as<std::string>() == "RGGDens")
      p_mygen.reset(new ompl_lemur::RoadmapRGGDens<RoadmapArgs>(rmargs));
   else if (args["roadmap-type"].as<std::string>() == "RGGDensConst")
      p_mygen.reset(new ompl_lemur::RoadmapRGGDensConst<RoadmapArgs>(rmargs));
   else if (args["roadmap-type"].as<std::string>() == "Halton")
      p_mygen.reset(new ompl_lemur::RoadmapHalton<RoadmapArgs>(rmargs));
   else if (args["roadmap-type"].as<std::string>() == "HaltonDens")
      p_mygen.reset(new ompl_lemur::RoadmapHaltonDens<RoadmapArgs>(rmargs));
   else if (args["roadmap-type"].as<std::string>() == "HaltonOffDens")
      p_mygen.reset(new ompl_lemur::RoadmapHaltonOffDens<RoadmapArgs>(rmargs));
   else
   {
      OMPL_ERROR("--roadmap-type unknown!");
      return 1;
   }

   if (args.count("roadmap-param"))
   {
      const std::vector<std::string> & params = args["roadmap-param"].as< std::vector<std::string> >();
      for (unsigned int ui=0; ui<params.size(); ui++)
      {
         size_t eq = params[ui].find('=');
         if (eq == params[ui].npos)
         {
            OMPL_ERROR("--roadmap-param has bad format!");
            return 1;
         }
         p_mygen->params.setParam(params[ui].substr(0,eq), params[ui].substr(eq+1));
      }
   }

   std::size_t num_batches = args["num-batches"].as<std::size_t>();
   OMPL_INFORM("Generating %lu batch%s ...", num_batches, num_batches==1?"":"es");

   p_mygen->initialize();
   while (p_mygen->num_batches_generated < num_batches)
   {
      // generate a graph
      p_mygen->generate();
   }

   OMPL_INFORM("Generated graph has %lu vertices and %lu edges.", num_vertices(g), num_edges(g));

   // write it out to file
   boost::dynamic_properties props;
   props.property("state", ompl_lemur::make_rvstate_map_string_adaptor(
      get(&VertexProperties::state,g),
      space->as<ompl::base::RealVectorStateSpace>()));
   props.property("batch", pr_bgl::make_string_map(get(&VertexProperties::batch,g)));
   props.property("batch", pr_bgl::make_string_map(get(&EdgeProperties::batch,g)));
   props.property("is_shadow", pr_bgl::make_string_map(get(&VertexProperties::is_shadow,g)));
   props.property("distance", pr_bgl::make_string_map(get(&EdgeProperties::distance,g)));

   std::ostream * outp;
   std::ofstream fp;
   std::string out_file = args["out-file"].as<std::string>();
   if (out_file == "-")
      outp = &std::cout;
   else
   {
      fp.open(out_file.c_str());
      if (!fp.is_open())
      {
         OMPL_ERROR("could not open file!");
         return 1;
      }
      outp = &fp;
   }

   std::string out_format = args["out-format"].as<std::string>();
   if (out_format == "graphio")
   {
      OMPL_INFORM("Writing to graphio file ...");
      pr_bgl::write_graphio_graph(*outp, g,
         get(boost::vertex_index,g), get(&EdgeProperties::index,g));
      pr_bgl::write_graphio_properties(*outp, g,
         get(boost::vertex_index,g), get(&EdgeProperties::index,g),
         props);
   }
   else if (out_format == "graphml")
   {
      OMPL_INFORM("Writing to graphml file ...");
      boost::write_graphml(*outp, g, get(boost::vertex_index,g), props, true); // ordered_vertices
   }
   else
   {
      OMPL_ERROR("--out-format must be graphio or graphml!");
      return 1;
   }

   if (fp.is_open())
      fp.close();

   return 0;
}
<|endoftext|>


<commit_before>#include <cstddef>
#include <cstddef>
#include <fstream>
#include <iomanip>

#define WIN32_LEAN_AND_MEAN
#include <Windows.h>
#include <ShlObj.h>

#include <windows.storage.h>
#include <windows.system.h>
#include <wrl.h>

#define WIDEIFYIMP(x) L##x
#define WIDEIFY(x) WIDEIFYIMP(x)

#include <queue>

#include <experimental/filesystem>
namespace fs = std::experimental::filesystem;

#include <UWP/UWP.hpp>

#include <UWP/DumperIPC.hpp>

void OpenTempState()
{
	Microsoft::WRL::ComPtr<ABI::Windows::Storage::IApplicationDataStatics> AppDataStatics;

	if(
		RoGetActivationFactory(
			Microsoft::WRL::Wrappers::HStringReference(RuntimeClass_Windows_Storage_ApplicationData).Get(),
			__uuidof(AppDataStatics), &AppDataStatics
		) < 0
		)
	{
		// Error getting ApplicationData statics
	}

	Microsoft::WRL::ComPtr<ABI::Windows::Storage::IApplicationData> AppData;
	if( AppDataStatics->get_Current(&AppData) < 0 )
	{
		// Error getting current IApplicationData
	}

	Microsoft::WRL::ComPtr<ABI::Windows::System::ILauncherStatics3> LauncherStatics;
	if(
		RoGetActivationFactory(
			Microsoft::WRL::Wrappers::HStringReference(RuntimeClass_Windows_System_Launcher).Get(),
			__uuidof(LauncherStatics), &LauncherStatics
		) < 0
		)
	{
		// Error getting Launcher statics
	}

	Microsoft::WRL::ComPtr<ABI::Windows::Storage::IStorageFolder> TemporaryFolder;

	if( AppData->get_TemporaryFolder(&TemporaryFolder) < 0 )
	{
		// Failed to get folder
		return;
	}

	Microsoft::WRL::ComPtr<ABI::Windows::Foundation::IAsyncOperation<bool>> Result;
	LauncherStatics->LaunchFolderAsync(
		TemporaryFolder.Get(),
		&Result
	);
}

std::uint32_t __stdcall DumperThread(void* DLLHandle)
{
	std::wstring DumpPath = fs::path(UWP::Current::Storage::GetTemporaryPath()) / L"DUMP";

	IPC::SetTargetThread(GetCurrentThreadId());

	IPC::PushMessage(L"UWPDumper Build date(%ls : %ls)\n", WIDEIFY(__DATE__), WIDEIFY(__TIME__));
	IPC::PushMessage(L"\t-https://github.com/Wunkolo/UWPDumper\n");
	IPC::PushMessage(L"Publisher:\n\t%s\n", UWP::Current::GetPublisher().c_str());
	IPC::PushMessage(L"Publisher ID:\n\t%s\n", UWP::Current::GetPublisherID().c_str());
	IPC::PushMessage(L"Publisher Path:\n\t%s\n", UWP::Current::Storage::GetPublisherPath().c_str());
	IPC::PushMessage(L"Package Path:\n\t%s\n", UWP::Current::GetPackagePath().c_str());
	IPC::PushMessage(L"Package Name:\n\t%s\n", UWP::Current::GetFullName().c_str());
	IPC::PushMessage(L"Family Name:\n\t%s\n", UWP::Current::GetFamilyName().c_str());

	IPC::PushMessage(L"Dump Path:\n\t%s\n", DumpPath.c_str());

	std::vector<fs::directory_entry> FileList;

	for( auto& Entry : fs::recursive_directory_iterator(".") )
	{
		if( fs::is_regular_file(Entry.path()) )
		{
			FileList.push_back(Entry);
		}
	}

	IPC::PushMessage(L"\tDumping %zu files\n", FileList.size());

	std::size_t i = 0;
	for( const auto& File : FileList )
	{
		const fs::path WritePath = DumpPath + File.path().wstring().substr(1);
		const std::wstring ReadPath = File.path().wstring();
		IPC::PushMessage(
			L"%*.*s %*.u bytes %*zu/%zu\n",
			60, 60,
			ReadPath.c_str() + (ReadPath.length() > 60 ? (ReadPath.length() - (60)) : 0),
			15,
			fs::file_size(File),
			20,
			++i,
			FileList.size()
		);

		std::error_code ErrorCode;
		if( fs::create_directories(WritePath.parent_path(), ErrorCode) )
		{
			IPC::PushMessage(
				L"Error creating subfolder: %s (%s)\n",
				WritePath.parent_path().c_str(),
				ErrorCode.message().c_str()
			);
			continue;
		}

		std::ifstream SourceFile(ReadPath, std::ios::binary);
		if( !SourceFile.is_open() )
		{
			IPC::PushMessage(
				L"Error opening %s for reading\n",
				ReadPath.c_str()
			);
			continue;
		}

		std::ofstream DestFile(WritePath, std::ios::binary);
		if( !DestFile.is_open() )
		{
			IPC::PushMessage(
				L"Error opening %s for writing\n",
				WritePath.c_str()
			);
			continue;
		}

		if( SourceFile && DestFile )
		{
			DestFile << SourceFile.rdbuf();
		}
		else
		{
			IPC::PushMessage(
				L"Error copying:\n"
				"\t%s\n"
				"\tto\n"
				"\t%s\n",
				File.path().c_str(),
				WritePath.c_str()
			);
		}
	}

	IPC::PushMessage(L"Dump complete!\n\tPath:\n\t%s\n", DumpPath.c_str());
	OpenTempState();
	IPC::ClearTargetThread();

	FreeLibraryAndExitThread(
		reinterpret_cast<HMODULE>(DLLHandle),
		EXIT_SUCCESS
	);
}

std::int32_t __stdcall DllMain(HINSTANCE hDLL, std::uint32_t Reason, void* Reserved)
{
	switch( Reason )
	{
	case DLL_PROCESS_ATTACH:
	{
		if( IPC::GetTargetProcess() == GetCurrentProcessId() )
		{
			// We are the target process to be dumped
			CreateThread(
				nullptr,
				0,
				reinterpret_cast<unsigned long(__stdcall*)(void*)>(&DumperThread),
				hDLL,
				0,
				nullptr
			);
		}
	}
	case DLL_PROCESS_DETACH:
	case DLL_THREAD_ATTACH:
	case DLL_THREAD_DETACH:
	default:
	{
		return true;
	}
	}

	return false;
}
<commit_msg>Fix subfolder creation<commit_after>#include <cstddef>
#include <cstddef>
#include <fstream>
#include <iomanip>

#define WIN32_LEAN_AND_MEAN
#include <Windows.h>
#include <ShlObj.h>

#include <windows.storage.h>
#include <windows.system.h>
#include <wrl.h>

#define WIDEIFYIMP(x) L##x
#define WIDEIFY(x) WIDEIFYIMP(x)

#include <queue>

#include <experimental/filesystem>
namespace fs = std::experimental::filesystem;

#include <UWP/UWP.hpp>

#include <UWP/DumperIPC.hpp>

void OpenTempState()
{
	Microsoft::WRL::ComPtr<ABI::Windows::Storage::IApplicationDataStatics> AppDataStatics;

	if(
		RoGetActivationFactory(
			Microsoft::WRL::Wrappers::HStringReference(RuntimeClass_Windows_Storage_ApplicationData).Get(),
			__uuidof(AppDataStatics), &AppDataStatics
		) < 0
	)
	{
		// Error getting ApplicationData statics
	}

	Microsoft::WRL::ComPtr<ABI::Windows::Storage::IApplicationData> AppData;
	if( AppDataStatics->get_Current(&AppData) < 0 )
	{
		// Error getting current IApplicationData
	}

	Microsoft::WRL::ComPtr<ABI::Windows::System::ILauncherStatics3> LauncherStatics;
	if(
		RoGetActivationFactory(
			Microsoft::WRL::Wrappers::HStringReference(RuntimeClass_Windows_System_Launcher).Get(),
			__uuidof(LauncherStatics), &LauncherStatics
		) < 0
	)
	{
		// Error getting Launcher statics
	}

	Microsoft::WRL::ComPtr<ABI::Windows::Storage::IStorageFolder> TemporaryFolder;

	if( AppData->get_TemporaryFolder(&TemporaryFolder) < 0 )
	{
		// Failed to get folder
		return;
	}

	Microsoft::WRL::ComPtr<ABI::Windows::Foundation::IAsyncOperation<bool>> Result;
	LauncherStatics->LaunchFolderAsync(
		TemporaryFolder.Get(),
		&Result
	);
}

std::uint32_t __stdcall DumperThread(void* DLLHandle)
{
	std::wstring DumpPath = fs::path(UWP::Current::Storage::GetTemporaryPath()) / L"DUMP";

	IPC::SetTargetThread(GetCurrentThreadId());

	IPC::PushMessage(L"UWPDumper Build date(%ls : %ls)\n", WIDEIFY(__DATE__), WIDEIFY(__TIME__));
	IPC::PushMessage(L"\t-https://github.com/Wunkolo/UWPDumper\n");
	IPC::PushMessage(L"Publisher:\n\t%s\n", UWP::Current::GetPublisher().c_str());
	IPC::PushMessage(L"Publisher ID:\n\t%s\n", UWP::Current::GetPublisherID().c_str());
	IPC::PushMessage(L"Publisher Path:\n\t%s\n", UWP::Current::Storage::GetPublisherPath().c_str());
	IPC::PushMessage(L"Package Path:\n\t%s\n", UWP::Current::GetPackagePath().c_str());
	IPC::PushMessage(L"Package Name:\n\t%s\n", UWP::Current::GetFullName().c_str());
	IPC::PushMessage(L"Family Name:\n\t%s\n", UWP::Current::GetFamilyName().c_str());

	IPC::PushMessage(L"Dump Path:\n\t%s\n", DumpPath.c_str());

	std::vector<fs::directory_entry> FileList;

	for( auto& Entry : fs::recursive_directory_iterator(".") )
	{
		if( fs::is_regular_file(Entry.path()) )
		{
			FileList.push_back(Entry);
		}
	}

	IPC::PushMessage(L"\tDumping %zu files\n", FileList.size());

	std::size_t i = 0;
	for( const auto& File : FileList )
	{
		const fs::path WritePath = DumpPath + File.path().wstring().substr(1);
		const std::wstring ReadPath = File.path().wstring();
		IPC::PushMessage(
			L"%*.*s %*.u bytes %*zu/%zu\n",
			60, 60,
			ReadPath.c_str() + (ReadPath.length() > 60 ? (ReadPath.length() - (60)) : 0),
			15,
			fs::file_size(File),
			20,
			++i,
			FileList.size()
		);

		std::error_code ErrorCode;
		if( fs::create_directories(WritePath.parent_path(), ErrorCode) == false && ErrorCode )
		{
			const std::string ErrorMessage(ErrorCode.message());
			std::wstring WErrorMessage;
			WErrorMessage.assign(ErrorMessage.begin(), ErrorMessage.end());
			IPC::PushMessage(
				L"Error creating subfolder: %s\n\t%s\n",
				WritePath.parent_path().c_str(),
				WErrorMessage.c_str()
			);
			continue;
		}

		std::ifstream SourceFile(ReadPath, std::ios::binary);
		if( !SourceFile.is_open() )
		{
			IPC::PushMessage(
				L"Error opening %s for reading\n",
				ReadPath.c_str()
			);
			continue;
		}

		std::ofstream DestFile(WritePath, std::ios::binary);
		if( !DestFile.is_open() )
		{
			IPC::PushMessage(
				L"Error opening %s for writing\n",
				WritePath.c_str()
			);
			continue;
		}

		if( SourceFile && DestFile )
		{
			DestFile << SourceFile.rdbuf();
		}
		else
		{
			IPC::PushMessage(
				L"Error copying:\n"
				"\t%s\n"
				"\tto\n"
				"\t%s\n",
				File.path().c_str(),
				WritePath.c_str()
			);
		}
	}

	IPC::PushMessage(L"Dump complete!\n\tPath:\n\t%s\n", DumpPath.c_str());
	OpenTempState();
	IPC::ClearTargetThread();

	FreeLibraryAndExitThread(
		reinterpret_cast<HMODULE>(DLLHandle),
		EXIT_SUCCESS
	);
}

std::int32_t __stdcall DllMain(HINSTANCE hDLL, std::uint32_t Reason, void* Reserved)
{
	switch( Reason )
	{
	case DLL_PROCESS_ATTACH:
	{
		if( IPC::GetTargetProcess() == GetCurrentProcessId() )
		{
			// We are the target process to be dumped
			CreateThread(
				nullptr,
				0,
				reinterpret_cast<unsigned long(__stdcall*)(void*)>(&DumperThread),
				hDLL,
				0,
				nullptr
			);
		}
	}
	case DLL_PROCESS_DETACH:
	case DLL_THREAD_ATTACH:
	case DLL_THREAD_DETACH:
	default:
	{
		return true;
	}
	}

	return false;
}
<|endoftext|>


<commit_before>//---
// Copyright (C) 2000 ImageLinks Inc.
//
// License: MIT
//
// Author: Garrett Potts
//
//---
// $Id$

#include <ossim/base/ossimStdOutProgress.h>
#include <ossim/base/ossimPreferences.h>
#include <iomanip>

#if defined(WIN32) || defined(_MSC_VER) && !defined(__CYGWIN__) && !defined(__MWERKS__)
   #include <io.h>
   #define ISATTY _isatty
   #define FILENO _fileno
#else
   #include <unistd.h>
   #define ISATTY isatty
   #define FILENO fileno
#endif

RTTI_DEF1(ossimStdOutProgress, "ossimStdOutProgress", ossimProcessListener);

ossimStdOutProgress theStdOutProgress;

ossimStdOutProgress::ossimStdOutProgress(ossim_uint32 precision,
                                         bool flushStream)
   :
      ossimProcessListener(),
      thePrecision(precision),
      theFlushStreamFlag(flushStream),
      theRunningInConsoleFlag(true)
{
   // Determine if running in a terminal window. Progress reports are only written to console if
   // running in one.

   bool prefsFlag = theRunningInConsoleFlag;
   ossimString stdOutConsole;
   stdOutConsole.string() = ossimPreferences::instance()->
      preferencesKWL().findKey(std::string("ossim.std.out.progress"));

   if ( stdOutConsole.size() )
   {
      prefsFlag = stdOutConsole.toBool();
   }

   if ( !ISATTY(FILENO(stdout)) || !prefsFlag )
   {
      theRunningInConsoleFlag = false;
   }
}

void ossimStdOutProgress::processProgressEvent(ossimProcessProgressEvent& event)
{
   if (!theRunningInConsoleFlag)
      return;

   if (event.getOutputMessageFlag())
   {
      ossimString s;
      event.getMessage(s);
      if (!s.empty())
      {
         ossimNotify(ossimNotifyLevel_NOTICE) << s.c_str() << std::endl;
      }
      return; // Don't output percentage on a message update.
   }

   double p = event.getPercentComplete();
   ossimNotify(ossimNotifyLevel_NOTICE)
      << std::setiosflags(std::ios::fixed)
      << std::setprecision(thePrecision)
      << p << "%\r";

   if(theFlushStreamFlag)
   {
      (p != 100.0) ?
         ossimNotify(ossimNotifyLevel_NOTICE).flush() :
         ossimNotify(ossimNotifyLevel_NOTICE) << "\n";
   }
}

void ossimStdOutProgress::setFlushStreamFlag(bool flag)
{
   theFlushStreamFlag = flag;
}

<commit_msg>Fixed logic of prefs override.<commit_after>//---
// Copyright (C) 2000 ImageLinks Inc.
//
// License: MIT
//
// Author: Garrett Potts
//
//---
// $Id$

#include <ossim/base/ossimStdOutProgress.h>
#include <ossim/base/ossimPreferences.h>
#include <iomanip>

#if defined(WIN32) || defined(_MSC_VER) && !defined(__CYGWIN__) && !defined(__MWERKS__)
   #include <io.h>
   #define ISATTY _isatty
   #define FILENO _fileno
#else
   #include <unistd.h>
   #define ISATTY isatty
   #define FILENO fileno
#endif

RTTI_DEF1(ossimStdOutProgress, "ossimStdOutProgress", ossimProcessListener);

ossimStdOutProgress theStdOutProgress;

ossimStdOutProgress::ossimStdOutProgress(ossim_uint32 precision,
                                         bool flushStream)
   :
      ossimProcessListener(),
      thePrecision(precision),
      theFlushStreamFlag(flushStream),
      theRunningInConsoleFlag(true)
{
   // Determine if running in a terminal window. Progress reports are only written to console if
   // running in one.

   bool prefsFlag = theRunningInConsoleFlag;
   ossimString stdOutConsole;
   stdOutConsole.string() = ossimPreferences::instance()->
      preferencesKWL().findKey(std::string("ossim.std.out.progress"));

   if ( stdOutConsole.size() )
   {
      // Override auto detected console.
      theRunningInConsoleFlag = stdOutConsole.toBool();
   }
   else if ( !ISATTY(FILENO(stdout) ) )
   {
      theRunningInConsoleFlag = false;
   }
}

void ossimStdOutProgress::processProgressEvent(ossimProcessProgressEvent& event)
{
   if (!theRunningInConsoleFlag)
      return;

   if (event.getOutputMessageFlag())
   {
      ossimString s;
      event.getMessage(s);
      if (!s.empty())
      {
         ossimNotify(ossimNotifyLevel_NOTICE) << s.c_str() << std::endl;
      }
      return; // Don't output percentage on a message update.
   }

   double p = event.getPercentComplete();
   ossimNotify(ossimNotifyLevel_NOTICE)
      << std::setiosflags(std::ios::fixed)
      << std::setprecision(thePrecision)
      << p << "%\r";

   if(theFlushStreamFlag)
   {
      (p != 100.0) ?
         ossimNotify(ossimNotifyLevel_NOTICE).flush() :
         ossimNotify(ossimNotifyLevel_NOTICE) << "\n";
   }
}

void ossimStdOutProgress::setFlushStreamFlag(bool flag)
{
   theFlushStreamFlag = flag;
}

<|endoftext|>


<commit_before>// ======================================================================== //
// Copyright 2009-2015 Intel Corporation                                    //
//                                                                          //
// Licensed under the Apache License, Version 2.0 (the "License");          //
// you may not use this file except in compliance with the License.         //
// You may obtain a copy of the License at                                  //
//                                                                          //
//     http://www.apache.org/licenses/LICENSE-2.0                           //
//                                                                          //
// Unless required by applicable law or agreed to in writing, software      //
// distributed under the License is distributed on an "AS IS" BASIS,        //
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. //
// See the License for the specific language governing permissions and      //
// limitations under the License.                                           //
// ======================================================================== //

/* This is a small example tutorial how to use OSPRay in an application.
 *
 * Build it in the build_directory with
 *   g++ ../apps/ospTutorial.cpp -I ../ospray/include -I .. -I ../ospray/embree/common  ./libospray.so -o ospTutorial
 */

#include "ospray/ospray.h"

// helper function to write the rendered image as PPM file
void writePPM(const char *fileName,
              const int sizeX, const int sizeY,
              const uint32 *pixel)
{
  FILE *file = fopen(fileName, "wb");
  fprintf(file, "P6\n%i %i\n255\n", sizeX, sizeY);
  unsigned char out[3*sizeX];
  for (int y = 0; y < sizeY; y++) {
    const unsigned char *in = (const unsigned char *)&pixel[(sizeY-1-y)*sizeX];
    for (int x = 0; x < sizeX; x++) {
      out[3*x + 0] = in[4*x + 0];
      out[3*x + 1] = in[4*x + 1];
      out[3*x + 2] = in[4*x +2 ];
    }
    fwrite(&out, 3*sizeX, sizeof(char), file);
  }
  fprintf(file, "\n");
  fclose(file);
}

int main(int ac, const char **av) {
  // image size
  int width = 1024;
  int height = 768;

  // camera
  osp::vec3f cam_pos(0.f);
  osp::vec3f cam_up(0.f, 1.f, 0.f);
  osp::vec3f cam_view(0.1f, 0.f, 1.f);

  // triangle mesh data
  float vertex[] = { -1.0f, -1.0f, 3.0f, 0.f,
                     -1.0f,  1.0f, 3.0f, 0.f,
                      1.0f, -1.0f, 3.0f, 0.f,
                      0.1f,  0.1f, 0.3f, 0.f };
  float color[] =  { 0.9f, 0.5f, 0.5f, 1.0f,
                     0.8f, 0.8f, 0.8f, 1.0f,
                     0.8f, 0.8f, 0.8f, 1.0f,
                     0.5f, 0.9f, 0.5f, 1.0f };
  int32 index[] = { 0, 1, 2,
                    1, 2, 3 };

  // initialize OSPRay; OSPRay parses (and removes) its commandline parameters, e.g. "--osp:debug"
  ospInit(&ac, av);

  // create and setup camera
  OSPCamera camera = ospNewCamera("perspective");
  ospSetf(camera, "aspect", width/(float)height);
  ospSetVec3f(camera, "pos", cam_pos);
  ospSetVec3f(camera, "dir", cam_view);
  ospSetVec3f(camera, "up",  cam_up);
  ospCommit(camera); // commit each object to indicate modifications are done

  // create and setup model and mesh
  OSPGeometry mesh = ospNewTriangleMesh();
  OSPData data = ospNewData(4, OSP_FLOAT3A, vertex); // OSP_FLOAT3 format is also supported for vertex positions (currently not on MIC)
  ospCommit(data);
  ospSetData(mesh, "vertex", data);

  data = ospNewData(4, OSP_FLOAT4, color);
  ospCommit(data);
  ospSetData(mesh, "vertex.color", data);

  data = ospNewData(2, OSP_INT3, index); // OSP_INT4 format is also supported for triangle indices
  ospCommit(data);
  ospSetData(mesh, "index", data);

  ospCommit(mesh);

  OSPModel world = ospNewModel();
  ospAddGeometry(world, mesh);
  ospCommit(world);

  // create and setup renderer
  OSPRenderer renderer = ospNewRenderer("ao4"); // choose Ambient Occlusion renderer
  ospSetObject(renderer, "model",  world);
  ospSetObject(renderer, "camera", camera);
  ospCommit(renderer);

  // create and setup framebuffer
  OSPFrameBuffer framebuffer = ospNewFrameBuffer(osp::vec2i(width, height), OSP_RGBA_I8, OSP_FB_COLOR | /*OSP_FB_DEPTH |*/ OSP_FB_ACCUM);
  ospFrameBufferClear(framebuffer, OSP_FB_COLOR | OSP_FB_ACCUM);

  // render one frame
  ospRenderFrame(framebuffer, renderer, OSP_FB_COLOR | OSP_FB_ACCUM);

  // access framebuffer and write its content as PPM file
  const uint32 * fb = (uint32*)ospMapFrameBuffer(framebuffer, OSP_FB_COLOR);
  writePPM("firstFrame.ppm", width, height, fb);
  ospUnmapFrameBuffer(fb, framebuffer);

  // render 10 more frames, which are accumulated to result in a better converged image
  for (int frames = 0; frames < 10; frames++)
    ospRenderFrame(framebuffer, renderer, OSP_FB_COLOR | OSP_FB_ACCUM);

  fb = (uint32*)ospMapFrameBuffer(framebuffer, OSP_FB_COLOR);
  writePPM("accumulatedFrame.ppm", width, height, fb);
  ospUnmapFrameBuffer(fb, framebuffer);

  return 0;
}
<commit_msg>Adapt ospTutorial to Windows<commit_after>// ======================================================================== //
// Copyright 2009-2015 Intel Corporation                                    //
//                                                                          //
// Licensed under the Apache License, Version 2.0 (the "License");          //
// you may not use this file except in compliance with the License.         //
// You may obtain a copy of the License at                                  //
//                                                                          //
//     http://www.apache.org/licenses/LICENSE-2.0                           //
//                                                                          //
// Unless required by applicable law or agreed to in writing, software      //
// distributed under the License is distributed on an "AS IS" BASIS,        //
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. //
// See the License for the specific language governing permissions and      //
// limitations under the License.                                           //
// ======================================================================== //

/* This is a small example tutorial how to use OSPRay in an application.
 *
 * On Linux build it in the build_directory with
 *   g++ ../apps/ospTutorial.cpp -I ../ospray/include -I .. -I ../ospray/embree/common  ./libospray.so -o ospTutorial
 * On Windows build it in the build_directory\$Configuration with
 *   cl ..\..\apps\ospTutorial.cpp /EHsc -I ..\..\ospray\include -I ..\.. -I ..\..\ospray\embree\common ospray.lib
 */

#include "ospray/ospray.h"

// helper function to write the rendered image as PPM file
void writePPM(const char *fileName,
              const int sizeX, const int sizeY,
              const uint32 *pixel)
{
  FILE *file = fopen(fileName, "wb");
  fprintf(file, "P6\n%i %i\n255\n", sizeX, sizeY);
  unsigned char *out = (unsigned char *)alloca(3*sizeX);
  for (int y = 0; y < sizeY; y++) {
    const unsigned char *in = (const unsigned char *)&pixel[(sizeY-1-y)*sizeX];
    for (int x = 0; x < sizeX; x++) {
      out[3*x + 0] = in[4*x + 0];
      out[3*x + 1] = in[4*x + 1];
      out[3*x + 2] = in[4*x +2 ];
    }
    fwrite(out, 3*sizeX, sizeof(char), file);
  }
  fprintf(file, "\n");
  fclose(file);
}

int main(int ac, const char **av) {
  // image size
  int width = 1024;
  int height = 768;

  // camera
  osp::vec3f cam_pos(0.f);
  osp::vec3f cam_up(0.f, 1.f, 0.f);
  osp::vec3f cam_view(0.1f, 0.f, 1.f);

  // triangle mesh data
  float vertex[] = { -1.0f, -1.0f, 3.0f, 0.f,
                     -1.0f,  1.0f, 3.0f, 0.f,
                      1.0f, -1.0f, 3.0f, 0.f,
                      0.1f,  0.1f, 0.3f, 0.f };
  float color[] =  { 0.9f, 0.5f, 0.5f, 1.0f,
                     0.8f, 0.8f, 0.8f, 1.0f,
                     0.8f, 0.8f, 0.8f, 1.0f,
                     0.5f, 0.9f, 0.5f, 1.0f };
  int32 index[] = { 0, 1, 2,
                    1, 2, 3 };

  // initialize OSPRay; OSPRay parses (and removes) its commandline parameters, e.g. "--osp:debug"
  ospInit(&ac, av);

  // create and setup camera
  OSPCamera camera = ospNewCamera("perspective");
  ospSetf(camera, "aspect", width/(float)height);
  ospSetVec3f(camera, "pos", cam_pos);
  ospSetVec3f(camera, "dir", cam_view);
  ospSetVec3f(camera, "up",  cam_up);
  ospCommit(camera); // commit each object to indicate modifications are done

  // create and setup model and mesh
  OSPGeometry mesh = ospNewTriangleMesh();
  OSPData data = ospNewData(4, OSP_FLOAT3A, vertex); // OSP_FLOAT3 format is also supported for vertex positions (currently not on MIC)
  ospCommit(data);
  ospSetData(mesh, "vertex", data);

  data = ospNewData(4, OSP_FLOAT4, color);
  ospCommit(data);
  ospSetData(mesh, "vertex.color", data);

  data = ospNewData(2, OSP_INT3, index); // OSP_INT4 format is also supported for triangle indices
  ospCommit(data);
  ospSetData(mesh, "index", data);

  ospCommit(mesh);

  OSPModel world = ospNewModel();
  ospAddGeometry(world, mesh);
  ospCommit(world);

  // create and setup renderer
  OSPRenderer renderer = ospNewRenderer("ao4"); // choose Ambient Occlusion renderer
  ospSetObject(renderer, "model",  world);
  ospSetObject(renderer, "camera", camera);
  ospCommit(renderer);

  // create and setup framebuffer
  OSPFrameBuffer framebuffer = ospNewFrameBuffer(osp::vec2i(width, height), OSP_RGBA_I8, OSP_FB_COLOR | /*OSP_FB_DEPTH |*/ OSP_FB_ACCUM);
  ospFrameBufferClear(framebuffer, OSP_FB_COLOR | OSP_FB_ACCUM);

  // render one frame
  ospRenderFrame(framebuffer, renderer, OSP_FB_COLOR | OSP_FB_ACCUM);

  // access framebuffer and write its content as PPM file
  const uint32 * fb = (uint32*)ospMapFrameBuffer(framebuffer, OSP_FB_COLOR);
  writePPM("firstFrame.ppm", width, height, fb);
  ospUnmapFrameBuffer(fb, framebuffer);

  // render 10 more frames, which are accumulated to result in a better converged image
  for (int frames = 0; frames < 10; frames++)
    ospRenderFrame(framebuffer, renderer, OSP_FB_COLOR | OSP_FB_ACCUM);

  fb = (uint32*)ospMapFrameBuffer(framebuffer, OSP_FB_COLOR);
  writePPM("accumulatedFrame.ppm", width, height, fb);
  ospUnmapFrameBuffer(fb, framebuffer);

  return 0;
}
<|endoftext|>


<commit_before>#include "SyntaxTree.h"

#include <list>
#include <vector>
#include <sstream>
#include <iostream>
#include <stdexcept>
#include <assert.h>

#include <boost/format.hpp>

#include "template.common.h"
#include "template.cs.h"

using namespace std;

// class Node
// {
// public:
    Node::~Node()
    {
    }

    int Node::getType() { return 0; } //TODO: remove getType() if unnecessary
    int Node::getHash(vector<Node*>* referencingStack) { return 0; }
    string* Node::getParsed(int as) { return 0; }

// };

// class NodeKst : public Node
// {
    // list<Node*>* commands;
    // string* fileName;
    // public:
    NodeKst::NodeKst(list<Node*>* _commands, string* _fileName) : Node()
    {
        commands = _commands;
        fileName = _fileName;
    }

    int NodeKst::getType()
    {
       return CsNodeType::kst;
    }

    int NodeKst::getHash(vector<Node*>* referencingStack)
    {
        return 0;
    }

    string* NodeKst::getParsed(int as)
    {
        stringstream parsed;

        parsed << boost::format(TCS_HEADER) % *fileName;
        parsed << TCS_USINGS;

        string namespaceByFileName = fileName->substr(0, fileName->find("."));

        parsed << boost::format(TCS_NAMESPACE_BEGIN) % namespaceByFileName;

        list<Node*>::iterator i = commands->begin();
        list<Node*>::iterator end = commands->end();

        string* temp;

        for (; i != end; ++i)
        {
            temp = (*i)->getParsed(CsParseAs::Default);
            temp = indent(temp);
            parsed << *temp;
            parsed << "\n";
        }

        parsed << TCS_NAMESPACE_END;

        parsed << "\n\n";

        return new string(parsed.str());
    }
// };

// class NodeInclude : public Node
// {
//     string* value;
//     public:
    NodeInclude::NodeInclude(string* _value) : Node()
    {
        value = _value;
    }

    int NodeInclude::getType()
    {
       return CsNodeType::include;
    }

    int NodeInclude::getHash(vector<Node*>* referencingStack)
    {
        return 0;
    }

    string* NodeInclude::getParsed(int as)
    {
        stringstream parsed;

        parsed << "#include \"";
        parsed << *(value);
        parsed << "\"";

        return new string(parsed.str());
    }
// };

// class NodePacket : public Node
// {
//     string* packetName;
//     list<Node*>* packetMembers;
//     public:
    NodePacket::NodePacket(string* _packetName, list<Node*>* _packetMembers) : Node()
    {
        packetName = _packetName;
        packetMembers = _packetMembers;
    }

    int NodePacket::getType()
    {
       return CsNodeType::packet;
    }

    int NodePacket::getHash(vector<Node*>* referencingStack)
    {
        if (referencingStack == NULL)
        {
            referencingStack = new vector<Node*>();
        }

        referencingStack->push_back(this);

        size_t packetHash = getHashCode(packetName);

        list<Node*>::iterator i = packetMembers->begin();
        list<Node*>::iterator end = packetMembers->end();
        for (; i != end; ++i)
        {
            combineHashCode(packetHash, (*i)->getHash(referencingStack));
        }

        return (int)packetHash;
    }

    string* NodePacket::getParsed(int as)
    {
        stringstream parsed;

        switch (as)
        {
            case CsParseAs::Default:
            {
                parsed << boost::format(TCS_PACKET_BEGIN) % *packetName;
                parsed << "\t<temp> packet hash: " << getHash(NULL) << "\n";
                list<Node*>::iterator i = packetMembers->begin();
                list<Node*>::iterator end = packetMembers->end();
                for (; i != end; ++i)
                {
                    parsed << "\t" << *((*i)->getParsed(CsParseAs::Default));
                }
                parsed << TCS_PACKET_END;
            }
            break;
        }

        return new string(parsed.str());
    }
// };

// class NodePacketMember : public Node
// {
//     Node* memberType;
//     Node* memberName;
//     public:
    NodePacketMember::NodePacketMember(Node* _memberType, Node* _memberName) : Node()
    {
        memberType = _memberType;
        memberName = _memberName;
    }

    int NodePacketMember::getType()
    {
       return CsNodeType::packetMember;
    }

    int NodePacketMember::getHash(vector<Node*>* referencingStack)
    {
        assert(referencingStack != NULL);

        return 0;
    }

    string* NodePacketMember::getParsed(int as)
    {
        stringstream parsed;

        switch (as)
        {
            case CsParseAs::Default:
            {
                parsed << boost::format(TCS_PACKET_MEMBER_AS_DEFAULT)
                    % *(memberType->getParsed(CsParseAs::Default))
                    % *(memberName->getParsed(CsParseAs::Default));
            }
            break;
        }

        return new string(parsed.str());
    }
// };

// class NodePacketMemberType : public Node
// {
//     int typeType; // one of PRIMITIVE_DATA_TYPE, REFERENCE_DATA_TYPE, MAP, LIST
//     string* value; // "int", "bool", ..., "MyPacket", "Skill" or NULL when type is MAP or LIST
//     Node* generic1; // LIST<generic1>
//     Node* generic2; // MAP <generic1, generic2>
//     Node* generic3; // reserved
//     public:
    NodePacketMemberType::NodePacketMemberType(int _type, string* _value) : Node()
    {
        typeType = _type;
        value = _value;
        generic1 = NULL;
        generic2 = NULL;
        generic3 = NULL;
    }

    NodePacketMemberType::NodePacketMemberType(int _type, Node* _generic1) : Node()
    {
        typeType = _type;
        value = NULL;
        generic1 = _generic1;
        generic2 = NULL;
        generic3 = NULL;
    }

    NodePacketMemberType::NodePacketMemberType(int _type, Node* _generic1, Node* _generic2) : Node()
    {
        typeType = _type;
        value = NULL;
        generic1 = _generic1;
        generic2 = _generic2;
        generic3 = NULL;
    }

    NodePacketMemberType::NodePacketMemberType(int _type, Node* _generic1, Node* _generic2, Node* _generic3) : Node()
    {
        typeType = _type;
        value = NULL;
        generic1 = _generic1;
        generic2 = _generic2;
        generic3 = _generic3;
    }

    int NodePacketMemberType::getType()
    {
       return CsNodeType::packetMemberType;
    }

    int NodePacketMemberType::getHash(vector<Node*>* referencingStack)
    {
        return 0;
    }

    string* NodePacketMemberType::getParsed(int as)
    {
        stringstream parsed;

        switch (typeType) {

            case Rakjin::Krystal::Parser::token::PRIMITIVE_DATA_TYPE:
            parsed << *value;
            break;

            case Rakjin::Krystal::Parser::token::REFERENCE_DATA_TYPE:
            parsed << *value;
            break;

            case Rakjin::Krystal::Parser::token::MAP:
            parsed << "Dictionary";
            parsed << "<" << *(generic1->getParsed(CsParseAs::Default)) << ", " << *(generic2->getParsed(CsParseAs::Default)) << ">";
            break;

            case Rakjin::Krystal::Parser::token::LIST:
            parsed << "List";
            parsed << "<" << *(generic1->getParsed(CsParseAs::Default)) << ">";
            break;

            default:
            throw(runtime_error("Unknown NodePacketMemberType type."));
            break;
        }

        return new string(parsed.str());
    }
// };

// class NodePacketMemberName : public Node
// {
//     string* value;
//     public:
    NodePacketMemberName::NodePacketMemberName(string* _value) : Node()
    {
        value = _value;
    }

    int NodePacketMemberName::getType()
    {
       return CsNodeType::packetMemberName;
    }

    int NodePacketMemberName::getHash(vector<Node*>* referencingStack)
    {
        return 0;
    }

    string* NodePacketMemberName::getParsed(int as)
    {
       return value;
    }
// };

<commit_msg>NodePacketMember::getHash returns hash<commit_after>#include "SyntaxTree.h"

#include <list>
#include <vector>
#include <sstream>
#include <iostream>
#include <stdexcept>
#include <assert.h>

#include <boost/format.hpp>

#include "template.common.h"
#include "template.cs.h"

using namespace std;

// class Node
// {
// public:
    Node::~Node()
    {
    }

    int Node::getType() { return 0; } //TODO: remove getType() if unnecessary
    int Node::getHash(vector<Node*>* referencingStack) { return 0; }
    string* Node::getParsed(int as) { return 0; }

// };

// class NodeKst : public Node
// {
    // list<Node*>* commands;
    // string* fileName;
    // public:
    NodeKst::NodeKst(list<Node*>* _commands, string* _fileName) : Node()
    {
        commands = _commands;
        fileName = _fileName;
    }

    int NodeKst::getType()
    {
       return CsNodeType::kst;
    }

    int NodeKst::getHash(vector<Node*>* referencingStack)
    {
        return 0;
    }

    string* NodeKst::getParsed(int as)
    {
        stringstream parsed;

        parsed << boost::format(TCS_HEADER) % *fileName;
        parsed << TCS_USINGS;

        string namespaceByFileName = fileName->substr(0, fileName->find("."));

        parsed << boost::format(TCS_NAMESPACE_BEGIN) % namespaceByFileName;

        list<Node*>::iterator i = commands->begin();
        list<Node*>::iterator end = commands->end();

        string* temp;

        for (; i != end; ++i)
        {
            temp = (*i)->getParsed(CsParseAs::Default);
            temp = indent(temp);
            parsed << *temp;
            parsed << "\n";
        }

        parsed << TCS_NAMESPACE_END;

        parsed << "\n\n";

        return new string(parsed.str());
    }
// };

// class NodeInclude : public Node
// {
//     string* value;
//     public:
    NodeInclude::NodeInclude(string* _value) : Node()
    {
        value = _value;
    }

    int NodeInclude::getType()
    {
       return CsNodeType::include;
    }

    int NodeInclude::getHash(vector<Node*>* referencingStack)
    {
        return 0;
    }

    string* NodeInclude::getParsed(int as)
    {
        stringstream parsed;

        parsed << "#include \"";
        parsed << *(value);
        parsed << "\"";

        return new string(parsed.str());
    }
// };

// class NodePacket : public Node
// {
//     string* packetName;
//     list<Node*>* packetMembers;
//     public:
    NodePacket::NodePacket(string* _packetName, list<Node*>* _packetMembers) : Node()
    {
        packetName = _packetName;
        packetMembers = _packetMembers;
    }

    int NodePacket::getType()
    {
       return CsNodeType::packet;
    }

    int NodePacket::getHash(vector<Node*>* referencingStack)
    {
        if (referencingStack == NULL)
        {
            referencingStack = new vector<Node*>();
        }

        referencingStack->push_back(this);

        size_t packetHash = getHashCode(packetName);

        list<Node*>::iterator i = packetMembers->begin();
        list<Node*>::iterator end = packetMembers->end();
        for (; i != end; ++i)
        {
            combineHashCode(packetHash, (*i)->getHash(referencingStack));
        }

        return (int) packetHash;
    }

    string* NodePacket::getParsed(int as)
    {
        stringstream parsed;

        switch (as)
        {
            case CsParseAs::Default:
            {
                parsed << boost::format(TCS_PACKET_BEGIN) % *packetName;
                parsed << "\t<temp> packet hash: " << getHash(NULL) << "\n";
                list<Node*>::iterator i = packetMembers->begin();
                list<Node*>::iterator end = packetMembers->end();
                for (; i != end; ++i)
                {
                    parsed << "\t" << *((*i)->getParsed(CsParseAs::Default));
                }
                parsed << TCS_PACKET_END;
            }
            break;
        }

        return new string(parsed.str());
    }
// };

// class NodePacketMember : public Node
// {
//     Node* memberType;
//     Node* memberName;
//     public:
    NodePacketMember::NodePacketMember(Node* _memberType, Node* _memberName) : Node()
    {
        memberType = _memberType;
        memberName = _memberName;
    }

    int NodePacketMember::getType()
    {
       return CsNodeType::packetMember;
    }

    int NodePacketMember::getHash(vector<Node*>* referencingStack)
    {
        assert(referencingStack != NULL);

        size_t packetMemberHash = memberType->getHash(referencingStack);
        combineHashCode(packetMemberHash, memberName->getHash(referencingStack));

        return (int) packetMemberHash;
    }

    string* NodePacketMember::getParsed(int as)
    {
        stringstream parsed;

        switch (as)
        {
            case CsParseAs::Default:
            {
                parsed << boost::format(TCS_PACKET_MEMBER_AS_DEFAULT)
                    % *(memberType->getParsed(CsParseAs::Default))
                    % *(memberName->getParsed(CsParseAs::Default));
            }
            break;
        }

        return new string(parsed.str());
    }
// };

// class NodePacketMemberType : public Node
// {
//     int typeType; // one of PRIMITIVE_DATA_TYPE, REFERENCE_DATA_TYPE, MAP, LIST
//     string* value; // "int", "bool", ..., "MyPacket", "Skill" or NULL when type is MAP or LIST
//     Node* generic1; // LIST<generic1>
//     Node* generic2; // MAP <generic1, generic2>
//     Node* generic3; // reserved
//     public:
    NodePacketMemberType::NodePacketMemberType(int _type, string* _value) : Node()
    {
        typeType = _type;
        value = _value;
        generic1 = NULL;
        generic2 = NULL;
        generic3 = NULL;
    }

    NodePacketMemberType::NodePacketMemberType(int _type, Node* _generic1) : Node()
    {
        typeType = _type;
        value = NULL;
        generic1 = _generic1;
        generic2 = NULL;
        generic3 = NULL;
    }

    NodePacketMemberType::NodePacketMemberType(int _type, Node* _generic1, Node* _generic2) : Node()
    {
        typeType = _type;
        value = NULL;
        generic1 = _generic1;
        generic2 = _generic2;
        generic3 = NULL;
    }

    NodePacketMemberType::NodePacketMemberType(int _type, Node* _generic1, Node* _generic2, Node* _generic3) : Node()
    {
        typeType = _type;
        value = NULL;
        generic1 = _generic1;
        generic2 = _generic2;
        generic3 = _generic3;
    }

    int NodePacketMemberType::getType()
    {
       return CsNodeType::packetMemberType;
    }

    int NodePacketMemberType::getHash(vector<Node*>* referencingStack)
    {
        return 0;
    }

    string* NodePacketMemberType::getParsed(int as)
    {
        stringstream parsed;

        switch (typeType) {

            case Rakjin::Krystal::Parser::token::PRIMITIVE_DATA_TYPE:
            parsed << *value;
            break;

            case Rakjin::Krystal::Parser::token::REFERENCE_DATA_TYPE:
            parsed << *value;
            break;

            case Rakjin::Krystal::Parser::token::MAP:
            parsed << "Dictionary";
            parsed << "<" << *(generic1->getParsed(CsParseAs::Default)) << ", " << *(generic2->getParsed(CsParseAs::Default)) << ">";
            break;

            case Rakjin::Krystal::Parser::token::LIST:
            parsed << "List";
            parsed << "<" << *(generic1->getParsed(CsParseAs::Default)) << ">";
            break;

            default:
            throw(runtime_error("Unknown NodePacketMemberType type."));
            break;
        }

        return new string(parsed.str());
    }
// };

// class NodePacketMemberName : public Node
// {
//     string* value;
//     public:
    NodePacketMemberName::NodePacketMemberName(string* _value) : Node()
    {
        value = _value;
    }

    int NodePacketMemberName::getType()
    {
       return CsNodeType::packetMemberName;
    }

    int NodePacketMemberName::getHash(vector<Node*>* referencingStack)
    {
        return 0;
    }

    string* NodePacketMemberName::getParsed(int as)
    {
       return value;
    }
// };

<|endoftext|>


<commit_before>#include "src/bitmap.h"
#include "src/camera.h"
#include "src/math.h"
#include "src/timer.h"
#include "src/triangle.h"
#include <stdio.h>

typedef struct
{
    gfx_Triangle tris[2];
} TexQuad;

// helper functions
void setupTexQuad(TexQuad *q, int qx, int qy, int qw, int qh, gfx_Bitmap *texture);
void drawTexQuad(const TexQuad *q, const mth_Matrix4 *mvp, gfx_drawBuffer *buffer);

#define ROTATE_QUAD(delta, x, y, z) {\
            for(k = 0; k < 2; ++k) \
            { \
                for(i = 0; i < 3; ++i) \
                { \
                    mth_rotateVecAxisAngle(&quad.tris[k].vertices[i].position, delta*dt, x, y, z); \
                } \
            } \
        }

// Texture mapping test
void testTextureMapping()
{
    unsigned long int now, last = 0;
    const unsigned short *keysPressed;
    TexQuad quad;
    int texMapFlipped = 0, depthFuncFlipped = 0;
    gfx_Camera cam;
    mth_Matrix4 modelViewProj;
    gfx_Bitmap bmp = gfx_loadBitmap("images/quake.bmp");
    gfx_drawBuffer buffer;

    ALLOC_DRAWBUFFER(buffer, SCREEN_WIDTH, SCREEN_HEIGHT, DB_COLOR | DB_DEPTH);
    buffer.drawOpts.depthFunc = DF_ALWAYS;

    if(!DRAWBUFFER_VALID(buffer, DB_COLOR | DB_DEPTH))
    {
        printf("Out of memory!");
        exit(1);
    }

    gfx_setPalette(bmp.palette);

    tmr_start();

    mth_matIdentity(&modelViewProj);

    // setup camera
    VEC4(cam.position, 0, 0, 60);
    VEC4(cam.up, 0, 1, 0);
    VEC4(cam.right, 1, 0, 0);
    VEC4(cam.target, 0, 0, -1);

    // setup textured quad
    setupTexQuad(&quad, -25, -25, 25, 25, &bmp);

    mth_matPerspective(&cam.projection, 75.f * M_PI /180.f, (float)buffer.width / (float)buffer.height, 0.1f, 500.f);
    mth_matView(&cam.view, &cam.position, &cam.target, &cam.up);
    modelViewProj = mth_matMul(&cam.view, &cam.projection);

    do
    {
        int i, k;
        float dt;
        now = tmr_getMs();
        dt = (float)(now - last);
        keysPressed = kbd_getInput();

        if(keysPressed[KEY_RIGHT])
            ROTATE_QUAD(0.002f, 0.f, 1.f, 0.f);

        if(keysPressed[KEY_LEFT])
            ROTATE_QUAD(-0.002f, 0.f, 1.f, 0.f);

        if(keysPressed[KEY_UP])
            ROTATE_QUAD(0.002f, 1.f, 0.f, 0.f);

        if(keysPressed[KEY_DOWN])
            ROTATE_QUAD(-0.002f, 1.f, 0.f, 0.f);

        if(keysPressed[KEY_T] && !texMapFlipped)
        {
            buffer.drawOpts.drawMode = buffer.drawOpts.drawMode == DM_AFFINE ? DM_PERSPECTIVE : DM_AFFINE;
            texMapFlipped = 1;

        }
        else if(!keysPressed[KEY_T]) texMapFlipped = 0;

        if(keysPressed[KEY_D] && !depthFuncFlipped)
        {
            buffer.drawOpts.depthFunc = buffer.drawOpts.depthFunc == DF_ALWAYS ? DF_LESS : DF_ALWAYS;
            depthFuncFlipped = 1;

        }
        else if(!keysPressed[KEY_D]) depthFuncFlipped = 0;

        // clear buffers and draw the quad!
        gfx_clrBuffer(&buffer, DB_COLOR | DB_DEPTH);
        drawTexQuad(&quad, &modelViewProj, &buffer);
        gfx_updateScreen(&buffer);

        fprintf(stdout, buffer.drawOpts.drawMode == DM_AFFINE ? "[T]exmapping: Affine\r\n" : "[T]exmapping: Perspective\r\n");
        fprintf(stdout, buffer.drawOpts.depthFunc == DF_ALWAYS  ? "[D]epth test: OFF\r" : "[D]epth test: ON\r");
        // reset carriage to top left corner of the screen
        fprintf(stdout, "%c[%d;%df", 0x1B, 0, 0);
        gfx_vSync();

        keysPressed = kbd_getInput();
        fflush(stdout);
        last = now;
    } while(!keysPressed[KEY_ESC]);

    tmr_finish();
    FREE_DRAWBUFFER(buffer);
    gfx_freeBitmap(&bmp);
}

/* ***** */
void setupTexQuad(TexQuad *q, int qx, int qy, int qw, int qh, gfx_Bitmap *texture)
{
    q->tris[0].color = 1;
    q->tris[0].texture = texture;
    VEC4(q->tris[0].vertices[0].position, qx, qh, 0);
    q->tris[0].vertices[0].uv.u = 0;
    q->tris[0].vertices[0].uv.v = 1;
    VEC4(q->tris[0].vertices[1].position, qw, qy, 0);
    q->tris[0].vertices[1].uv.u = 1;
    q->tris[0].vertices[1].uv.v = 0;
    VEC4(q->tris[0].vertices[2].position, qx, qy, 0);
    q->tris[0].vertices[2].uv.u = 0;
    q->tris[0].vertices[2].uv.v = 0;

    q->tris[1].color = 1;
    q->tris[1].texture = texture;
    VEC4(q->tris[1].vertices[0].position, qx, qh, 0);
    q->tris[1].vertices[0].uv.u = 0;
    q->tris[1].vertices[0].uv.v = 1;
    VEC4(q->tris[1].vertices[1].position, qw, qh, 0);
    q->tris[1].vertices[1].uv.u = 1;
    q->tris[1].vertices[1].uv.v = 1;
    VEC4(q->tris[1].vertices[2].position, qw, qy, 0);
    q->tris[1].vertices[2].uv.u = 1;
    q->tris[1].vertices[2].uv.v = 0;
}

/* ***** */
void drawTexQuad(const TexQuad *q, const mth_Matrix4 *mvp, gfx_drawBuffer *buffer)
{
    gfx_drawTriangle(&q->tris[0], mvp, buffer);
    gfx_drawTriangle(&q->tris[1], mvp, buffer);
}
<commit_msg>use different aspect for tex mapping sample<commit_after>#include "src/bitmap.h"
#include "src/camera.h"
#include "src/math.h"
#include "src/timer.h"
#include "src/triangle.h"
#include <stdio.h>

typedef struct
{
    gfx_Triangle tris[2];
} TexQuad;

// helper functions
void setupTexQuad(TexQuad *q, int qx, int qy, int qw, int qh, gfx_Bitmap *texture);
void drawTexQuad(const TexQuad *q, const mth_Matrix4 *mvp, gfx_drawBuffer *buffer);

#define ROTATE_QUAD(delta, x, y, z) {\
            for(k = 0; k < 2; ++k) \
            { \
                for(i = 0; i < 3; ++i) \
                { \
                    mth_rotateVecAxisAngle(&quad.tris[k].vertices[i].position, delta*dt, x, y, z); \
                } \
            } \
        }

// Texture mapping test
void testTextureMapping()
{
    unsigned long int now, last = 0;
    const unsigned short *keysPressed;
    TexQuad quad;
    int texMapFlipped = 0, depthFuncFlipped = 0;
    gfx_Camera cam;
    mth_Matrix4 modelViewProj;
    gfx_Bitmap bmp = gfx_loadBitmap("images/quake.bmp");
    gfx_drawBuffer buffer;

    ALLOC_DRAWBUFFER(buffer, SCREEN_WIDTH, SCREEN_HEIGHT, DB_COLOR | DB_DEPTH);
    buffer.drawOpts.depthFunc = DF_ALWAYS;

    if(!DRAWBUFFER_VALID(buffer, DB_COLOR | DB_DEPTH))
    {
        printf("Out of memory!");
        exit(1);
    }

    gfx_setPalette(bmp.palette);

    tmr_start();

    mth_matIdentity(&modelViewProj);

    // setup camera
    VEC4(cam.position, 0, 0, 60);
    VEC4(cam.up, 0, 1, 0);
    VEC4(cam.right, 1, 0, 0);
    VEC4(cam.target, 0, 0, -1);

    // setup textured quad
    setupTexQuad(&quad, -20, -20, 25, 25, &bmp);

    mth_matPerspective(&cam.projection, 75.f * M_PI /180.f, (float)buffer.width / (float)buffer.height, 0.1f, 500.f);
    mth_matView(&cam.view, &cam.position, &cam.target, &cam.up);
    modelViewProj = mth_matMul(&cam.view, &cam.projection);

    do
    {
        int i, k;
        float dt;
        now = tmr_getMs();
        dt = (float)(now - last);
        keysPressed = kbd_getInput();

        if(keysPressed[KEY_RIGHT])
            ROTATE_QUAD(0.002f, 0.f, 1.f, 0.f);

        if(keysPressed[KEY_LEFT])
            ROTATE_QUAD(-0.002f, 0.f, 1.f, 0.f);

        if(keysPressed[KEY_UP])
            ROTATE_QUAD(0.002f, 1.f, 0.f, 0.f);

        if(keysPressed[KEY_DOWN])
            ROTATE_QUAD(-0.002f, 1.f, 0.f, 0.f);

        if(keysPressed[KEY_T] && !texMapFlipped)
        {
            buffer.drawOpts.drawMode = buffer.drawOpts.drawMode == DM_AFFINE ? DM_PERSPECTIVE : DM_AFFINE;
            texMapFlipped = 1;

        }
        else if(!keysPressed[KEY_T]) texMapFlipped = 0;

        if(keysPressed[KEY_D] && !depthFuncFlipped)
        {
            buffer.drawOpts.depthFunc = buffer.drawOpts.depthFunc == DF_ALWAYS ? DF_LESS : DF_ALWAYS;
            depthFuncFlipped = 1;

        }
        else if(!keysPressed[KEY_D]) depthFuncFlipped = 0;

        // clear buffers and draw the quad!
        gfx_clrBuffer(&buffer, DB_COLOR | DB_DEPTH);
        drawTexQuad(&quad, &modelViewProj, &buffer);
        gfx_updateScreen(&buffer);

        fprintf(stdout, buffer.drawOpts.drawMode == DM_AFFINE ? "[T]exmapping: Affine\r\n" : "[T]exmapping: Perspective\r\n");
        fprintf(stdout, buffer.drawOpts.depthFunc == DF_ALWAYS  ? "[D]epth test: OFF\r" : "[D]epth test: ON\r");
        // reset carriage to top left corner of the screen
        fprintf(stdout, "%c[%d;%df", 0x1B, 0, 0);
        gfx_vSync();

        keysPressed = kbd_getInput();
        fflush(stdout);
        last = now;
    } while(!keysPressed[KEY_ESC]);

    tmr_finish();
    FREE_DRAWBUFFER(buffer);
    gfx_freeBitmap(&bmp);
}

/* ***** */
void setupTexQuad(TexQuad *q, int qx, int qy, int qw, int qh, gfx_Bitmap *texture)
{
    q->tris[0].color = 1;
    q->tris[0].texture = texture;
    VEC4(q->tris[0].vertices[0].position, qx, qh, 0);
    q->tris[0].vertices[0].uv.u = 0;
    q->tris[0].vertices[0].uv.v = 1;
    VEC4(q->tris[0].vertices[1].position, qw, qy, 0);
    q->tris[0].vertices[1].uv.u = 1;
    q->tris[0].vertices[1].uv.v = 0;
    VEC4(q->tris[0].vertices[2].position, qx, qy, 0);
    q->tris[0].vertices[2].uv.u = 0;
    q->tris[0].vertices[2].uv.v = 0;

    q->tris[1].color = 1;
    q->tris[1].texture = texture;
    VEC4(q->tris[1].vertices[0].position, qx, qh, 0);
    q->tris[1].vertices[0].uv.u = 0;
    q->tris[1].vertices[0].uv.v = 1;
    VEC4(q->tris[1].vertices[1].position, qw, qh, 0);
    q->tris[1].vertices[1].uv.u = 1;
    q->tris[1].vertices[1].uv.v = 1;
    VEC4(q->tris[1].vertices[2].position, qw, qy, 0);
    q->tris[1].vertices[2].uv.u = 1;
    q->tris[1].vertices[2].uv.v = 0;
}

/* ***** */
void drawTexQuad(const TexQuad *q, const mth_Matrix4 *mvp, gfx_drawBuffer *buffer)
{
    gfx_drawTriangle(&q->tris[0], mvp, buffer);
    gfx_drawTriangle(&q->tris[1], mvp, buffer);
}
<|endoftext|>


<commit_before><commit_msg>base: Make Named::name() non-reference<commit_after><|endoftext|>


<commit_before>#include "rapid_pbd/visualizer.h"

#include <map>
#include <sstream>
#include <string>
#include <utility>
#include <vector>

#include "geometry_msgs/TransformStamped.h"
#include "pcl/point_cloud.h"
#include "pcl/point_types.h"
#include "pcl_conversions/pcl_conversions.h"
#include "rapid_pbd/joint_state.h"
#include "rapid_pbd_msgs/EditorEvent.h"
#include "rapid_pbd_msgs/Landmark.h"
#include "rapid_pbd_msgs/Program.h"
#include "robot_markers/builder.h"
#include "sensor_msgs/PointCloud2.h"
#include "surface_perception/visualization.h"
#include "visualization_msgs/Marker.h"
#include "visualization_msgs/MarkerArray.h"

#include "rapid_pbd/robot_config.h"
#include "rapid_pbd/world.h"

namespace msgs = rapid_pbd_msgs;
using sensor_msgs::PointCloud2;
using visualization_msgs::Marker;
using visualization_msgs::MarkerArray;

namespace rapid {
namespace pbd {
Visualizer::Visualizer(const SceneDb& scene_db,
                       const robot_markers::Builder& marker_builder,
                       const RobotConfig& robot_config)
    : scene_db_(scene_db),
      marker_builder_(marker_builder),
      robot_config_(robot_config),
      step_vizs_(),
      nh_() {}

void Visualizer::Init() {
  marker_builder_.Init();
  marker_builder_.SetNamespace("robot");
  marker_builder_.SetFrameId(robot_config_.base_link());
}

void Visualizer::Publish(const std::string& program_id, const World& world) {
  CreateStepVizIfNotExists(program_id);

  // Publish the robot visualization
  MarkerArray robot_markers;
  std::map<std::string, double> joint_positions;
  world.joint_state.ToMap(&joint_positions);
  marker_builder_.SetJointPositions(joint_positions);
  marker_builder_.Build(&robot_markers);
  step_vizs_[program_id].robot_pub.publish(robot_markers);

  std::string base_link(robot_config_.base_link());

  // Publish the scene
  PointCloud2 scene;
  if (world.scene_id != "" && scene_db_.Get(world.scene_id, &scene)) {
    if (world.scene_id != step_vizs_[program_id].last_scene_id) {
      step_vizs_[program_id].scene_pub.publish(scene);
      step_vizs_[program_id].last_scene_id = world.scene_id;
    }
  } else {
    pcl::PointCloud<pcl::PointXYZRGB> blank;
    pcl::PointXYZRGB pt;
    blank.points.push_back(pt);
    pcl::toROSMsg(blank, scene);
    scene.header.frame_id = base_link;
    step_vizs_[program_id].scene_pub.publish(scene);
  }

  // Publish landmark markers
  MarkerArray scene_markers;
  GetSegmentationMarker(world.surface_box_landmarks, robot_config_,
                        &scene_markers);
  if (scene_markers.markers.size() > 0) {
    step_vizs_[program_id].surface_seg_pub.publish(scene_markers);
  } else {
    for (size_t i = 0; i < 100; ++i) {
      Marker blank;
      blank.ns = "segmentation";
      blank.id = i;
      blank.header.frame_id = base_link;
      blank.type = Marker::CUBE;
      blank.pose.orientation.w = 1;
      blank.scale.x = 0.05;
      blank.scale.y = 0.05;
      blank.scale.z = 0.05;
      scene_markers.markers.push_back(blank);
    }
    step_vizs_[program_id].surface_seg_pub.publish(scene_markers);
  }
}

void Visualizer::StopPublishing(const std::string& program_id) {
  if (step_vizs_.find(program_id) != step_vizs_.end()) {
    step_vizs_.erase(program_id);
  }
}

void Visualizer::CreateStepVizIfNotExists(const std::string& program_id) {
  // Create the publisher if it doesn't exist.
  if (step_vizs_.find(program_id) == step_vizs_.end()) {
    step_vizs_[program_id].robot_pub =
        nh_.advertise<MarkerArray>("robot/" + program_id, 10, true);
    step_vizs_[program_id].scene_pub =
        nh_.advertise<PointCloud2>("scene/" + program_id, 10, true);
    step_vizs_[program_id].surface_seg_pub = nh_.advertise<MarkerArray>(
        "surface_segmentation/" + program_id, 10, true);
    step_vizs_[program_id].last_scene_id = "";
  }
}

RuntimeVisualizer::RuntimeVisualizer(const RobotConfig& robot_config,
                                     const ros::Publisher& surface_box_pub)
    : robot_config_(robot_config), surface_box_pub_(surface_box_pub) {}

void RuntimeVisualizer::PublishSurfaceBoxes(
    const std::vector<rapid_pbd_msgs::Landmark>& box_landmarks) const {
  MarkerArray scene_markers;
  GetSegmentationMarker(box_landmarks, robot_config_, &scene_markers);
  surface_box_pub_.publish(scene_markers);
}

void GetSegmentationMarker(const std::vector<msgs::Landmark>& landmarks,
                           const RobotConfig& robot_config,
                           visualization_msgs::MarkerArray* scene_markers) {
  std::vector<surface_perception::Object> objects;
  for (size_t li = 0; li < landmarks.size(); ++li) {
    const msgs::Landmark& landmark = landmarks[li];
    surface_perception::Object object;
    object.pose_stamped = landmark.pose_stamped;
    object.dimensions = landmark.surface_box_dims;
    objects.push_back(object);
  }
  surface_perception::ObjectMarkers(objects, &scene_markers->markers);

  std::string base_link(robot_config.base_link());

  for (size_t i = 0; i < objects.size(); ++i) {
    scene_markers->markers[i].ns = "segmentation";
    scene_markers->markers[i].id = i;
  }
  for (size_t i = 0; i < objects.size(); ++i) {
    Marker marker = scene_markers->markers[i];
    marker.type = Marker::TEXT_VIEW_FACING;
    marker.ns = "segmentation_names";
    marker.text = landmarks[i].name;
    marker.pose.position.z += 0.15;
    marker.scale.x = 0.1;
    marker.scale.y = 0.1;
    marker.scale.z = 0.1;
    marker.color.r = 1;
    marker.color.g = 0;
    marker.color.b = 0;
    marker.color.a = 1;
    scene_markers->markers.push_back(marker);
  }
  int num_objects = objects.size();
  for (size_t i = num_objects; i < 100; ++i) {
    Marker blank;
    blank.ns = "segmentation";
    blank.id = i;
    blank.header.frame_id = base_link;
    blank.type = Marker::CUBE;
    blank.pose.orientation.w = 1;
    blank.scale.x = 0.05;
    blank.scale.y = 0.05;
    blank.scale.z = 0.05;
    scene_markers->markers.push_back(blank);

    blank.ns = "segmentation_names";
    scene_markers->markers.push_back(blank);
  }
}
}  // namespace pbd
}  // namespace rapid
<commit_msg>Fixed scene disappearing after it's first displayed.<commit_after>#include "rapid_pbd/visualizer.h"

#include <map>
#include <sstream>
#include <string>
#include <utility>
#include <vector>

#include "geometry_msgs/TransformStamped.h"
#include "pcl/point_cloud.h"
#include "pcl/point_types.h"
#include "pcl_conversions/pcl_conversions.h"
#include "rapid_pbd/joint_state.h"
#include "rapid_pbd_msgs/EditorEvent.h"
#include "rapid_pbd_msgs/Landmark.h"
#include "rapid_pbd_msgs/Program.h"
#include "robot_markers/builder.h"
#include "sensor_msgs/PointCloud2.h"
#include "surface_perception/visualization.h"
#include "visualization_msgs/Marker.h"
#include "visualization_msgs/MarkerArray.h"

#include "rapid_pbd/robot_config.h"
#include "rapid_pbd/world.h"

namespace msgs = rapid_pbd_msgs;
using sensor_msgs::PointCloud2;
using visualization_msgs::Marker;
using visualization_msgs::MarkerArray;

namespace rapid {
namespace pbd {
Visualizer::Visualizer(const SceneDb& scene_db,
                       const robot_markers::Builder& marker_builder,
                       const RobotConfig& robot_config)
    : scene_db_(scene_db),
      marker_builder_(marker_builder),
      robot_config_(robot_config),
      step_vizs_(),
      nh_() {}

void Visualizer::Init() {
  marker_builder_.Init();
  marker_builder_.SetNamespace("robot");
  marker_builder_.SetFrameId(robot_config_.base_link());
}

void Visualizer::Publish(const std::string& program_id, const World& world) {
  CreateStepVizIfNotExists(program_id);

  // Publish the robot visualization
  MarkerArray robot_markers;
  std::map<std::string, double> joint_positions;
  world.joint_state.ToMap(&joint_positions);
  marker_builder_.SetJointPositions(joint_positions);
  marker_builder_.Build(&robot_markers);
  step_vizs_[program_id].robot_pub.publish(robot_markers);

  std::string base_link(robot_config_.base_link());

  // Publish the scene
  PointCloud2 scene;
  if (world.scene_id != "" && scene_db_.Get(world.scene_id, &scene)) {
    if (world.scene_id != step_vizs_[program_id].last_scene_id) {
      step_vizs_[program_id].scene_pub.publish(scene);
    }
  } else {
    pcl::PointCloud<pcl::PointXYZRGB> blank;
    pcl::PointXYZRGB pt;
    blank.points.push_back(pt);
    pcl::toROSMsg(blank, scene);
    scene.header.frame_id = base_link;
    step_vizs_[program_id].scene_pub.publish(scene);
  }
  step_vizs_[program_id].last_scene_id = world.scene_id;

  // Publish landmark markers
  MarkerArray scene_markers;
  GetSegmentationMarker(world.surface_box_landmarks, robot_config_,
                        &scene_markers);
  if (scene_markers.markers.size() > 0) {
    step_vizs_[program_id].surface_seg_pub.publish(scene_markers);
  } else {
    for (size_t i = 0; i < 100; ++i) {
      Marker blank;
      blank.ns = "segmentation";
      blank.id = i;
      blank.header.frame_id = base_link;
      blank.type = Marker::CUBE;
      blank.pose.orientation.w = 1;
      blank.scale.x = 0.05;
      blank.scale.y = 0.05;
      blank.scale.z = 0.05;
      scene_markers.markers.push_back(blank);
    }
    step_vizs_[program_id].surface_seg_pub.publish(scene_markers);
  }
}

void Visualizer::StopPublishing(const std::string& program_id) {
  if (step_vizs_.find(program_id) != step_vizs_.end()) {
    step_vizs_.erase(program_id);
  }
}

void Visualizer::CreateStepVizIfNotExists(const std::string& program_id) {
  // Create the publisher if it doesn't exist.
  if (step_vizs_.find(program_id) == step_vizs_.end()) {
    step_vizs_[program_id].robot_pub =
        nh_.advertise<MarkerArray>("robot/" + program_id, 10, true);
    step_vizs_[program_id].scene_pub =
        nh_.advertise<PointCloud2>("scene/" + program_id, 10, true);
    step_vizs_[program_id].surface_seg_pub = nh_.advertise<MarkerArray>(
        "surface_segmentation/" + program_id, 10, true);
    step_vizs_[program_id].last_scene_id = "";
  }
}

RuntimeVisualizer::RuntimeVisualizer(const RobotConfig& robot_config,
                                     const ros::Publisher& surface_box_pub)
    : robot_config_(robot_config), surface_box_pub_(surface_box_pub) {}

void RuntimeVisualizer::PublishSurfaceBoxes(
    const std::vector<rapid_pbd_msgs::Landmark>& box_landmarks) const {
  MarkerArray scene_markers;
  GetSegmentationMarker(box_landmarks, robot_config_, &scene_markers);
  surface_box_pub_.publish(scene_markers);
}

void GetSegmentationMarker(const std::vector<msgs::Landmark>& landmarks,
                           const RobotConfig& robot_config,
                           visualization_msgs::MarkerArray* scene_markers) {
  std::vector<surface_perception::Object> objects;
  for (size_t li = 0; li < landmarks.size(); ++li) {
    const msgs::Landmark& landmark = landmarks[li];
    surface_perception::Object object;
    object.pose_stamped = landmark.pose_stamped;
    object.dimensions = landmark.surface_box_dims;
    objects.push_back(object);
  }
  surface_perception::ObjectMarkers(objects, &scene_markers->markers);

  std::string base_link(robot_config.base_link());

  for (size_t i = 0; i < objects.size(); ++i) {
    scene_markers->markers[i].ns = "segmentation";
    scene_markers->markers[i].id = i;
  }
  for (size_t i = 0; i < objects.size(); ++i) {
    Marker marker = scene_markers->markers[i];
    marker.type = Marker::TEXT_VIEW_FACING;
    marker.ns = "segmentation_names";
    marker.text = landmarks[i].name;
    marker.pose.position.z += 0.15;
    marker.scale.x = 0.1;
    marker.scale.y = 0.1;
    marker.scale.z = 0.1;
    marker.color.r = 1;
    marker.color.g = 0;
    marker.color.b = 0;
    marker.color.a = 1;
    scene_markers->markers.push_back(marker);
  }
  int num_objects = objects.size();
  for (size_t i = num_objects; i < 100; ++i) {
    Marker blank;
    blank.ns = "segmentation";
    blank.id = i;
    blank.header.frame_id = base_link;
    blank.type = Marker::CUBE;
    blank.pose.orientation.w = 1;
    blank.scale.x = 0.05;
    blank.scale.y = 0.05;
    blank.scale.z = 0.05;
    scene_markers->markers.push_back(blank);

    blank.ns = "segmentation_names";
    scene_markers->markers.push_back(blank);
  }
}
}  // namespace pbd
}  // namespace rapid
<|endoftext|>