<commit_before>#include "primops.hh"
#include "eval-inline.hh"
#include "download.hh"
#include "store-api.hh"
#include "pathlocks.hh"

#include <sys/time.h>

#include <regex>

#include <nlohmann/json.hpp>

using namespace std::string_literals;

namespace nix {

struct GitInfo
{
    Path storePath;
    std::string rev;
    std::string shortRev;
    uint64_t revCount = 0;
};

std::regex revRegex("^[0-9a-fA-F]{40}$");

GitInfo exportGit(ref<Store> store, const std::string & uri,
    std::experimental::optional<std::string> ref, std::string rev,
    const std::string & name)
{
    if (settings.pureEval && rev == "")
        throw Error("in pure evaluation mode, 'fetchGit' requires a Git revision");

    if (!ref && rev == "" && hasPrefix(uri, "/") && pathExists(uri + "/.git")) {

        bool clean = true;

        try {
            runProgram("git", true, { "-C", uri, "diff-index", "--quiet", "HEAD", "--" });
        } catch (ExecError e) {
            if (!WIFEXITED(e.status) || WEXITSTATUS(e.status) != 1) throw;
            clean = false;
        }

        if (!clean) {

            /* This is an unclean working tree. So copy all tracked
               files. */

            GitInfo gitInfo;
            gitInfo.rev = "0000000000000000000000000000000000000000";
            gitInfo.shortRev = std::string(gitInfo.rev, 0, 7);

            auto files = tokenizeString<std::set<std::string>>(
                runProgram("git", true, { "-C", uri, "ls-files", "-z" }), "\0"s);

            PathFilter filter = [&](const Path & p) -> bool {
                assert(hasPrefix(p, uri));
                std::string file(p, uri.size() + 1);

                auto st = lstat(p);

                if (S_ISDIR(st.st_mode)) {
                    auto prefix = file + "/";
                    auto i = files.lower_bound(prefix);
                    return i != files.end() && hasPrefix(*i, prefix);
                }

                return files.count(file);
            };

            gitInfo.storePath = store->addToStore("source", uri, true, htSHA256, filter);

            return gitInfo;
        }

        // clean working tree, but no ref or rev specified.  Use 'HEAD'.
        rev = chomp(runProgram("git", true, { "-C", uri, "rev-parse", "HEAD" }));
        ref = "HEAD"s;
    }

    if (!ref) ref = "master"s;

    if (rev != "" && !std::regex_match(rev, revRegex))
        throw Error("invalid Git revision '%s'", rev);

    Path cacheDir = getCacheDir() + "/nix/git";

    if (!pathExists(cacheDir)) {
        runProgram("git", true, { "init", "--bare", cacheDir });
    }

    std::string localRef = hashString(htSHA256, fmt("%s-%s", uri, *ref)).to_string(Base32, false);

    Path localRefFile = cacheDir + "/refs/heads/" + localRef;

    bool doFetch;
    time_t now = time(0);
    /* If a rev was specified, we need to fetch if it's not in the
       repo. */
    if (rev != "") {
        try {
            runProgram("git", true, { "-C", cacheDir, "cat-file", "-e", rev });
            doFetch = false;
        } catch (ExecError & e) {
            if (WIFEXITED(e.status)) {
                doFetch = true;
            } else {
                throw;
            }
        }
    } else {
        /* If the local ref is older than ‘tarball-ttl’ seconds, do a
           git fetch to update the local ref to the remote ref. */
        struct stat st;
        doFetch = stat(localRefFile.c_str(), &st) != 0 ||
            st.st_mtime <= now - settings.tarballTtl;
    }
    if (doFetch)
    {
        Activity act(*logger, lvlTalkative, actUnknown, fmt("fetching Git repository '%s'", uri));

        // FIXME: git stderr messes up our progress indicator, so
        // we're using --quiet for now. Should process its stderr.
        runProgram("git", true, { "-C", cacheDir, "fetch", "--quiet", "--force", "--", uri, *ref + ":" + localRef });

        struct timeval times[2];
        times[0].tv_sec = now;
        times[0].tv_usec = 0;
        times[1].tv_sec = now;
        times[1].tv_usec = 0;

        utimes(localRefFile.c_str(), times);
    }

    // FIXME: check whether rev is an ancestor of ref.
    GitInfo gitInfo;
    gitInfo.rev = rev != "" ? rev : chomp(readFile(localRefFile));
    gitInfo.shortRev = std::string(gitInfo.rev, 0, 7);

    printTalkative("using revision %s of repo '%s'", uri, gitInfo.rev);

    std::string storeLinkName = hashString(htSHA512, name + std::string("\0"s) + gitInfo.rev).to_string(Base32, false);
    Path storeLink = cacheDir + "/" + storeLinkName + ".link";
    PathLocks storeLinkLock({storeLink}, fmt("waiting for lock on '%1%'...", storeLink)); // FIXME: broken

    try {
        auto json = nlohmann::json::parse(readFile(storeLink));

        assert(json["name"] == name && json["rev"] == gitInfo.rev);

        gitInfo.storePath = json["storePath"];

        if (store->isValidPath(gitInfo.storePath)) {
            gitInfo.revCount = json["revCount"];
            return gitInfo;
        }

    } catch (SysError & e) {
        if (e.errNo != ENOENT) throw;
    }

    // FIXME: should pipe this, or find some better way to extract a
    // revision.
    auto tar = runProgram("git", true, { "-C", cacheDir, "archive", gitInfo.rev });

    Path tmpDir = createTempDir();
    AutoDelete delTmpDir(tmpDir, true);

    runProgram("tar", true, { "x", "-C", tmpDir }, tar);

    gitInfo.storePath = store->addToStore(name, tmpDir);

    gitInfo.revCount = std::stoull(runProgram("git", true, { "-C", cacheDir, "rev-list", "--count", gitInfo.rev }));

    nlohmann::json json;
    json["storePath"] = gitInfo.storePath;
    json["uri"] = uri;
    json["name"] = name;
    json["rev"] = gitInfo.rev;
    json["revCount"] = gitInfo.revCount;

    writeFile(storeLink, json.dump());

    return gitInfo;
}

static void prim_fetchGit(EvalState & state, const Pos & pos, Value * * args, Value & v)
{
    std::string url;
    std::experimental::optional<std::string> ref;
    std::string rev;
    std::string name = "source";
    PathSet context;

    state.forceValue(*args[0]);

    if (args[0]->type == tAttrs) {

        state.forceAttrs(*args[0], pos);

        for (auto & attr : *args[0]->attrs) {
            string n(attr.name);
            if (n == "url")
                url = state.coerceToString(*attr.pos, *attr.value, context, false, false);
            else if (n == "ref")
                ref = state.forceStringNoCtx(*attr.value, *attr.pos);
            else if (n == "rev")
                rev = state.forceStringNoCtx(*attr.value, *attr.pos);
            else if (n == "name")
                name = state.forceStringNoCtx(*attr.value, *attr.pos);
            else
                throw EvalError("unsupported argument '%s' to 'fetchGit', at %s", attr.name, *attr.pos);
        }

        if (url.empty())
            throw EvalError(format("'url' argument required, at %1%") % pos);

    } else
        url = state.coerceToString(pos, *args[0], context, false, false);

    if (!isUri(url)) url = absPath(url);

    // FIXME: git externals probably can be used to bypass the URI
    // whitelist. Ah well.
    state.checkURI(url);

    auto gitInfo = exportGit(state.store, url, ref, rev, name);

    state.mkAttrs(v, 8);
    mkString(*state.allocAttr(v, state.sOutPath), gitInfo.storePath, PathSet({gitInfo.storePath}));
    mkString(*state.allocAttr(v, state.symbols.create("rev")), gitInfo.rev);
    mkString(*state.allocAttr(v, state.symbols.create("shortRev")), gitInfo.shortRev);
    mkInt(*state.allocAttr(v, state.symbols.create("revCount")), gitInfo.revCount);
    v.attrs->sort();

    if (state.allowedPaths)
        state.allowedPaths->insert(gitInfo.storePath);
}

static RegisterPrimOp r("fetchGit", 1, prim_fetchGit);

}
<commit_msg>fetchGit: use "HEAD" as default ref<commit_after>#include "primops.hh"
#include "eval-inline.hh"
#include "download.hh"
#include "store-api.hh"
#include "pathlocks.hh"

#include <sys/time.h>

#include <regex>

#include <nlohmann/json.hpp>

using namespace std::string_literals;

namespace nix {

struct GitInfo
{
    Path storePath;
    std::string rev;
    std::string shortRev;
    uint64_t revCount = 0;
};

std::regex revRegex("^[0-9a-fA-F]{40}$");

GitInfo exportGit(ref<Store> store, const std::string & uri,
    std::experimental::optional<std::string> ref, std::string rev,
    const std::string & name)
{
    if (settings.pureEval && rev == "")
        throw Error("in pure evaluation mode, 'fetchGit' requires a Git revision");

    if (!ref && rev == "" && hasPrefix(uri, "/") && pathExists(uri + "/.git")) {

        bool clean = true;

        try {
            runProgram("git", true, { "-C", uri, "diff-index", "--quiet", "HEAD", "--" });
        } catch (ExecError e) {
            if (!WIFEXITED(e.status) || WEXITSTATUS(e.status) != 1) throw;
            clean = false;
        }

        if (!clean) {

            /* This is an unclean working tree. So copy all tracked
               files. */

            GitInfo gitInfo;
            gitInfo.rev = "0000000000000000000000000000000000000000";
            gitInfo.shortRev = std::string(gitInfo.rev, 0, 7);

            auto files = tokenizeString<std::set<std::string>>(
                runProgram("git", true, { "-C", uri, "ls-files", "-z" }), "\0"s);

            PathFilter filter = [&](const Path & p) -> bool {
                assert(hasPrefix(p, uri));
                std::string file(p, uri.size() + 1);

                auto st = lstat(p);

                if (S_ISDIR(st.st_mode)) {
                    auto prefix = file + "/";
                    auto i = files.lower_bound(prefix);
                    return i != files.end() && hasPrefix(*i, prefix);
                }

                return files.count(file);
            };

            gitInfo.storePath = store->addToStore("source", uri, true, htSHA256, filter);

            return gitInfo;
        }

        // clean working tree, but no ref or rev specified.  Use 'HEAD'.
        rev = chomp(runProgram("git", true, { "-C", uri, "rev-parse", "HEAD" }));
        ref = "HEAD"s;
    }

    if (!ref) ref = "HEAD"s;

    if (rev != "" && !std::regex_match(rev, revRegex))
        throw Error("invalid Git revision '%s'", rev);

    Path cacheDir = getCacheDir() + "/nix/git";

    if (!pathExists(cacheDir)) {
        runProgram("git", true, { "init", "--bare", cacheDir });
    }

    std::string localRef = hashString(htSHA256, fmt("%s-%s", uri, *ref)).to_string(Base32, false);

    Path localRefFile = cacheDir + "/refs/heads/" + localRef;

    bool doFetch;
    time_t now = time(0);
    /* If a rev was specified, we need to fetch if it's not in the
       repo. */
    if (rev != "") {
        try {
            runProgram("git", true, { "-C", cacheDir, "cat-file", "-e", rev });
            doFetch = false;
        } catch (ExecError & e) {
            if (WIFEXITED(e.status)) {
                doFetch = true;
            } else {
                throw;
            }
        }
    } else {
        /* If the local ref is older than ‘tarball-ttl’ seconds, do a
           git fetch to update the local ref to the remote ref. */
        struct stat st;
        doFetch = stat(localRefFile.c_str(), &st) != 0 ||
            st.st_mtime <= now - settings.tarballTtl;
    }
    if (doFetch)
    {
        Activity act(*logger, lvlTalkative, actUnknown, fmt("fetching Git repository '%s'", uri));

        // FIXME: git stderr messes up our progress indicator, so
        // we're using --quiet for now. Should process its stderr.
        runProgram("git", true, { "-C", cacheDir, "fetch", "--quiet", "--force", "--", uri, *ref + ":" + localRef });

        struct timeval times[2];
        times[0].tv_sec = now;
        times[0].tv_usec = 0;
        times[1].tv_sec = now;
        times[1].tv_usec = 0;

        utimes(localRefFile.c_str(), times);
    }

    // FIXME: check whether rev is an ancestor of ref.
    GitInfo gitInfo;
    gitInfo.rev = rev != "" ? rev : chomp(readFile(localRefFile));
    gitInfo.shortRev = std::string(gitInfo.rev, 0, 7);

    printTalkative("using revision %s of repo '%s'", uri, gitInfo.rev);

    std::string storeLinkName = hashString(htSHA512, name + std::string("\0"s) + gitInfo.rev).to_string(Base32, false);
    Path storeLink = cacheDir + "/" + storeLinkName + ".link";
    PathLocks storeLinkLock({storeLink}, fmt("waiting for lock on '%1%'...", storeLink)); // FIXME: broken

    try {
        auto json = nlohmann::json::parse(readFile(storeLink));

        assert(json["name"] == name && json["rev"] == gitInfo.rev);

        gitInfo.storePath = json["storePath"];

        if (store->isValidPath(gitInfo.storePath)) {
            gitInfo.revCount = json["revCount"];
            return gitInfo;
        }

    } catch (SysError & e) {
        if (e.errNo != ENOENT) throw;
    }

    // FIXME: should pipe this, or find some better way to extract a
    // revision.
    auto tar = runProgram("git", true, { "-C", cacheDir, "archive", gitInfo.rev });

    Path tmpDir = createTempDir();
    AutoDelete delTmpDir(tmpDir, true);

    runProgram("tar", true, { "x", "-C", tmpDir }, tar);

    gitInfo.storePath = store->addToStore(name, tmpDir);

    gitInfo.revCount = std::stoull(runProgram("git", true, { "-C", cacheDir, "rev-list", "--count", gitInfo.rev }));

    nlohmann::json json;
    json["storePath"] = gitInfo.storePath;
    json["uri"] = uri;
    json["name"] = name;
    json["rev"] = gitInfo.rev;
    json["revCount"] = gitInfo.revCount;

    writeFile(storeLink, json.dump());

    return gitInfo;
}

static void prim_fetchGit(EvalState & state, const Pos & pos, Value * * args, Value & v)
{
    std::string url;
    std::experimental::optional<std::string> ref;
    std::string rev;
    std::string name = "source";
    PathSet context;

    state.forceValue(*args[0]);

    if (args[0]->type == tAttrs) {

        state.forceAttrs(*args[0], pos);

        for (auto & attr : *args[0]->attrs) {
            string n(attr.name);
            if (n == "url")
                url = state.coerceToString(*attr.pos, *attr.value, context, false, false);
            else if (n == "ref")
                ref = state.forceStringNoCtx(*attr.value, *attr.pos);
            else if (n == "rev")
                rev = state.forceStringNoCtx(*attr.value, *attr.pos);
            else if (n == "name")
                name = state.forceStringNoCtx(*attr.value, *attr.pos);
            else
                throw EvalError("unsupported argument '%s' to 'fetchGit', at %s", attr.name, *attr.pos);
        }

        if (url.empty())
            throw EvalError(format("'url' argument required, at %1%") % pos);

    } else
        url = state.coerceToString(pos, *args[0], context, false, false);

    if (!isUri(url)) url = absPath(url);

    // FIXME: git externals probably can be used to bypass the URI
    // whitelist. Ah well.
    state.checkURI(url);

    auto gitInfo = exportGit(state.store, url, ref, rev, name);

    state.mkAttrs(v, 8);
    mkString(*state.allocAttr(v, state.sOutPath), gitInfo.storePath, PathSet({gitInfo.storePath}));
    mkString(*state.allocAttr(v, state.symbols.create("rev")), gitInfo.rev);
    mkString(*state.allocAttr(v, state.symbols.create("shortRev")), gitInfo.shortRev);
    mkInt(*state.allocAttr(v, state.symbols.create("revCount")), gitInfo.revCount);
    v.attrs->sort();

    if (state.allowedPaths)
        state.allowedPaths->insert(gitInfo.storePath);
}

static RegisterPrimOp r("fetchGit", 1, prim_fetchGit);

}
<|endoftext|>


<commit_before>#ifndef AGGRASTER_HPP
#define AGGRASTER_HPP

#include <libslic3r/SLA/RasterBase.hpp>
#include "libslic3r/ExPolygon.hpp"
#include "libslic3r/MTUtils.hpp"
#include <libnest2d/backends/clipper/clipper_polygon.hpp>

// For rasterizing
#include <agg/agg_basics.h>
#include <agg/agg_rendering_buffer.h>
#include <agg/agg_pixfmt_gray.h>
#include <agg/agg_pixfmt_rgb.h>
#include <agg/agg_renderer_base.h>
#include <agg/agg_renderer_scanline.h>

#include <agg/agg_scanline_p.h>
#include <agg/agg_rasterizer_scanline_aa.h>
#include <agg/agg_path_storage.h>

namespace Slic3r {

inline const Polygon& contour(const ExPolygon& p) { return p.contour; }
inline const ClipperLib::Path& contour(const ClipperLib::Polygon& p) { return p.Contour; }

inline const Polygons& holes(const ExPolygon& p) { return p.holes; }
inline const ClipperLib::Paths& holes(const ClipperLib::Polygon& p) { return p.Holes; }

namespace sla {

template<class Color> struct Colors {
    static const Color White;
    static const Color Black;
};

template<class Color> const Color Colors<Color>::White = Color{255};
template<class Color> const Color Colors<Color>::Black = Color{0};

template<class PixelRenderer,
         template<class /*agg::renderer_base<PixelRenderer>*/> class Renderer,
         class Rasterizer = agg::rasterizer_scanline_aa<>,
         class Scanline   = agg::scanline_p8>
class AGGRaster: public RasterBase {
public:
    using TColor = typename PixelRenderer::color_type;
    using TValue = typename TColor::value_type;
    using TPixel = typename PixelRenderer::pixel_type;
    using TRawBuffer = agg::rendering_buffer;

protected:

    Resolution m_resolution;
    PixelDim m_pxdim_scaled;    // used for scaled coordinate polygons

    std::vector<TPixel> m_buf;
    agg::rendering_buffer m_rbuf;

    PixelRenderer m_pixrenderer;

    agg::renderer_base<PixelRenderer> m_raw_renderer;
    Renderer<agg::renderer_base<PixelRenderer>> m_renderer;

    Trafo m_trafo;
    Scanline m_scanlines;
    Rasterizer m_rasterizer;

    void flipy(agg::path_storage &path) const
    {
        path.flip_y(0, double(m_resolution.height_px));
    }

    void flipx(agg::path_storage &path) const
    {
        path.flip_x(0, double(m_resolution.width_px));
    }

    double getPx(const Point &p) { return p(0) * m_pxdim_scaled.w_mm; }
    double getPy(const Point &p) { return p(1) * m_pxdim_scaled.h_mm; }
    agg::path_storage to_path(const Polygon &poly) { return to_path(poly.points); }
    double getPx(const ClipperLib::IntPoint &p) { return p.X * m_pxdim_scaled.w_mm; }
    double getPy(const ClipperLib::IntPoint& p) { return p.Y * m_pxdim_scaled.h_mm; }

    template<class PointVec> agg::path_storage _to_path(const PointVec& v)
    {
        agg::path_storage path;

        auto it = v.begin();
        path.move_to(getPx(*it), getPy(*it));
        while(++it != v.end()) path.line_to(getPx(*it), getPy(*it));
        path.line_to(getPx(v.front()), getPy(v.front()));

        return path;
    }

    template<class PointVec> agg::path_storage _to_path_flpxy(const PointVec& v)
    {
        agg::path_storage path;

        auto it = v.begin();
        path.move_to(getPy(*it), getPx(*it));
        while(++it != v.end()) path.line_to(getPy(*it), getPx(*it));
        path.line_to(getPy(v.front()), getPx(v.front()));

        return path;
    }

    template<class PointVec> agg::path_storage to_path(const PointVec &v)
    {
        auto path = m_trafo.flipXY ? _to_path_flpxy(v) : _to_path(v);

        path.translate_all_paths(m_trafo.center_x * m_pxdim_scaled.w_mm,
                                 m_trafo.center_y * m_pxdim_scaled.h_mm);

        if(m_trafo.mirror_x) flipx(path);
        if(m_trafo.mirror_y) flipy(path);

        return path;
    }

    template<class P> void _draw(const P &poly)
    {
        m_rasterizer.reset();

        m_rasterizer.add_path(to_path(contour(poly)));
        for(auto& h : holes(poly)) m_rasterizer.add_path(to_path(h));

        agg::render_scanlines(m_rasterizer, m_scanlines, m_renderer);
    }

public:
    template<class GammaFn>
    AGGRaster(const Resolution &res,
              const PixelDim &  pd,
              const Trafo &     trafo,
              const TColor &    foreground,
              const TColor &    background,
              GammaFn &&        gammafn)
        : m_resolution(res)
        , m_pxdim_scaled(SCALING_FACTOR, SCALING_FACTOR)
        , m_buf(res.pixels())
        , m_rbuf(reinterpret_cast<TValue *>(m_buf.data()),
                 unsigned(res.width_px),
                 unsigned(res.height_px),
                 int(res.width_px *PixelRenderer::num_components))
        , m_pixrenderer(m_rbuf)
        , m_raw_renderer(m_pixrenderer)
        , m_renderer(m_raw_renderer)
        , m_trafo(trafo)
    {
        m_pxdim_scaled.w_mm /= pd.w_mm;
        m_pxdim_scaled.h_mm /= pd.h_mm;
        m_renderer.color(foreground);
        clear(background);

        m_rasterizer.gamma(gammafn);
    }

    Trafo trafo() const override { return m_trafo; }
    Resolution resolution() const override { return m_resolution; }
    PixelDim   pixel_dimensions() const override
    {
        return {SCALING_FACTOR / m_pxdim_scaled.w_mm,
                SCALING_FACTOR / m_pxdim_scaled.h_mm};
    }

    void draw(const ExPolygon &poly) override { _draw(poly); }
    void draw(const ClipperLib::Polygon &poly) override { _draw(poly); }

    EncodedRaster encode(RasterEncoder encoder) const override
    {
        return encoder(m_buf.data(), m_resolution.width_px, m_resolution.height_px, 1);
    }

    void clear(const TColor color) { m_raw_renderer.clear(color); }
};

/*
 * Captures an anti-aliased monochrome canvas where vectorial
 * polygons can be rasterized. Fill color is always white and the background is
 * black. Contours are anti-aliased.
 *
 * A gamma function can be specified at compile time to make it more flexible.
 */
using _RasterGrayscaleAA =
    AGGRaster<agg::pixfmt_gray8, agg::renderer_scanline_aa_solid>;

class RasterGrayscaleAA : public _RasterGrayscaleAA {
    using Base = _RasterGrayscaleAA;
    using typename Base::TColor;
    using typename Base::TValue;
public:
    template<class GammaFn>
    RasterGrayscaleAA(const RasterBase::Resolution &res,
                      const RasterBase::PixelDim &  pd,
                      const RasterBase::Trafo &     trafo,
                      GammaFn &&                    fn)
        : Base(res, pd, trafo, Colors<TColor>::White, Colors<TColor>::Black,
               std::forward<GammaFn>(fn))
    {}

    uint8_t read_pixel(size_t col, size_t row) const
    {
        static_assert(std::is_same<TValue, uint8_t>::value, "Not grayscale pix");

        uint8_t px;
        Base::m_buf[row * Base::resolution().width_px + col].get(px);
        return px;
    }

    void clear() { Base::clear(Colors<TColor>::Black); }
};

class RasterGrayscaleAAGammaPower: public RasterGrayscaleAA {
public:
    RasterGrayscaleAAGammaPower(const RasterBase::Resolution &res,
                                const RasterBase::PixelDim &  pd,
                                const RasterBase::Trafo &     trafo,
                                double                        gamma = 1.)
        : RasterGrayscaleAA(res, pd, trafo, agg::gamma_power(gamma))
    {}
};

}} // namespace Slic3r::sla

#endif // AGGRASTER_HPP
<commit_msg>Silencing compiler warning in AGGRaster on Visual Studio<commit_after>#ifndef AGGRASTER_HPP
#define AGGRASTER_HPP

#include <libslic3r/SLA/RasterBase.hpp>
#include "libslic3r/ExPolygon.hpp"
#include "libslic3r/MTUtils.hpp"
#include <libnest2d/backends/clipper/clipper_polygon.hpp>

// For rasterizing
#include <agg/agg_basics.h>
#include <agg/agg_rendering_buffer.h>
#include <agg/agg_pixfmt_gray.h>
#include <agg/agg_pixfmt_rgb.h>
#include <agg/agg_renderer_base.h>
#include <agg/agg_renderer_scanline.h>

#include <agg/agg_scanline_p.h>
#include <agg/agg_rasterizer_scanline_aa.h>
#include <agg/agg_path_storage.h>

namespace Slic3r {

inline const Polygon& contour(const ExPolygon& p) { return p.contour; }
inline const ClipperLib::Path& contour(const ClipperLib::Polygon& p) { return p.Contour; }

inline const Polygons& holes(const ExPolygon& p) { return p.holes; }
inline const ClipperLib::Paths& holes(const ClipperLib::Polygon& p) { return p.Holes; }

namespace sla {

template<class Color> struct Colors {
    static const Color White;
    static const Color Black;
};

template<class Color> const Color Colors<Color>::White = Color{255};
template<class Color> const Color Colors<Color>::Black = Color{0};

template<class PixelRenderer,
         template<class /*agg::renderer_base<PixelRenderer>*/> class Renderer,
         class Rasterizer = agg::rasterizer_scanline_aa<>,
         class Scanline   = agg::scanline_p8>
class AGGRaster: public RasterBase {
public:
    using TColor = typename PixelRenderer::color_type;
    using TValue = typename TColor::value_type;
    using TPixel = typename PixelRenderer::pixel_type;
    using TRawBuffer = agg::rendering_buffer;

protected:

    Resolution m_resolution;
    PixelDim m_pxdim_scaled;    // used for scaled coordinate polygons

    std::vector<TPixel> m_buf;
    agg::rendering_buffer m_rbuf;

    PixelRenderer m_pixrenderer;

    agg::renderer_base<PixelRenderer> m_raw_renderer;
    Renderer<agg::renderer_base<PixelRenderer>> m_renderer;

    Trafo m_trafo;
    Scanline m_scanlines;
    Rasterizer m_rasterizer;

    void flipy(agg::path_storage &path) const
    {
        path.flip_y(0, double(m_resolution.height_px));
    }

    void flipx(agg::path_storage &path) const
    {
        path.flip_x(0, double(m_resolution.width_px));
    }

    double getPx(const Point &p) { return p(0) * m_pxdim_scaled.w_mm; }
    double getPy(const Point &p) { return p(1) * m_pxdim_scaled.h_mm; }
    agg::path_storage to_path(const Polygon &poly) { return to_path(poly.points); }
    double getPx(const ClipperLib::IntPoint &p) { return p.X * m_pxdim_scaled.w_mm; }
    double getPy(const ClipperLib::IntPoint& p) { return p.Y * m_pxdim_scaled.h_mm; }

    template<class PointVec> agg::path_storage _to_path(const PointVec& v)
    {
        agg::path_storage path;

        auto it = v.begin();
        path.move_to(getPx(*it), getPy(*it));
        while(++it != v.end()) path.line_to(getPx(*it), getPy(*it));
        path.line_to(getPx(v.front()), getPy(v.front()));

        return path;
    }

    template<class PointVec> agg::path_storage _to_path_flpxy(const PointVec& v)
    {
        agg::path_storage path;

        auto it = v.begin();
        path.move_to(getPy(*it), getPx(*it));
        while(++it != v.end()) path.line_to(getPy(*it), getPx(*it));
        path.line_to(getPy(v.front()), getPx(v.front()));

        return path;
    }

    template<class PointVec> agg::path_storage to_path(const PointVec &v)
    {
        auto path = m_trafo.flipXY ? _to_path_flpxy(v) : _to_path(v);

        path.translate_all_paths(m_trafo.center_x * m_pxdim_scaled.w_mm,
                                 m_trafo.center_y * m_pxdim_scaled.h_mm);

        if(m_trafo.mirror_x) flipx(path);
        if(m_trafo.mirror_y) flipy(path);

        return path;
    }

    template<class P> void _draw(const P &poly)
    {
        m_rasterizer.reset();

        m_rasterizer.add_path(to_path(contour(poly)));
        for(auto& h : holes(poly)) m_rasterizer.add_path(to_path(h));

        agg::render_scanlines(m_rasterizer, m_scanlines, m_renderer);
    }

public:
    template<class GammaFn>
    AGGRaster(const Resolution &res,
              const PixelDim &  pd,
              const Trafo &     trafo,
              const TColor &    foreground,
              const TColor &    background,
              GammaFn &&        gammafn)
        : m_resolution(res)
        , m_pxdim_scaled(SCALING_FACTOR, SCALING_FACTOR)
        , m_buf(res.pixels())
        , m_rbuf(reinterpret_cast<TValue *>(m_buf.data()),
                 unsigned(res.width_px),
                 unsigned(res.height_px),
                 int(res.width_px *PixelRenderer::num_components))
        , m_pixrenderer(m_rbuf)
        , m_raw_renderer(m_pixrenderer)
        , m_renderer(m_raw_renderer)
        , m_trafo(trafo)
    {
        // Visual Studio compiler gives warnings about possible division by zero.
        assert(pd.w_mm != 0 && pd.h_mm != 0);
        if (pd.w_mm != 0 && pd.h_mm != 0) {
            m_pxdim_scaled.w_mm /= pd.w_mm;
            m_pxdim_scaled.h_mm /= pd.h_mm;
        }
        m_renderer.color(foreground);
        clear(background);

        m_rasterizer.gamma(gammafn);
    }

    Trafo trafo() const override { return m_trafo; }
    Resolution resolution() const override { return m_resolution; }
    PixelDim   pixel_dimensions() const override
    {
        return {SCALING_FACTOR / m_pxdim_scaled.w_mm,
                SCALING_FACTOR / m_pxdim_scaled.h_mm};
    }

    void draw(const ExPolygon &poly) override { _draw(poly); }
    void draw(const ClipperLib::Polygon &poly) override { _draw(poly); }

    EncodedRaster encode(RasterEncoder encoder) const override
    {
        return encoder(m_buf.data(), m_resolution.width_px, m_resolution.height_px, 1);
    }

    void clear(const TColor color) { m_raw_renderer.clear(color); }
};

/*
 * Captures an anti-aliased monochrome canvas where vectorial
 * polygons can be rasterized. Fill color is always white and the background is
 * black. Contours are anti-aliased.
 *
 * A gamma function can be specified at compile time to make it more flexible.
 */
using _RasterGrayscaleAA =
    AGGRaster<agg::pixfmt_gray8, agg::renderer_scanline_aa_solid>;

class RasterGrayscaleAA : public _RasterGrayscaleAA {
    using Base = _RasterGrayscaleAA;
    using typename Base::TColor;
    using typename Base::TValue;
public:
    template<class GammaFn>
    RasterGrayscaleAA(const RasterBase::Resolution &res,
                      const RasterBase::PixelDim &  pd,
                      const RasterBase::Trafo &     trafo,
                      GammaFn &&                    fn)
        : Base(res, pd, trafo, Colors<TColor>::White, Colors<TColor>::Black,
               std::forward<GammaFn>(fn))
    {}

    uint8_t read_pixel(size_t col, size_t row) const
    {
        static_assert(std::is_same<TValue, uint8_t>::value, "Not grayscale pix");

        uint8_t px;
        Base::m_buf[row * Base::resolution().width_px + col].get(px);
        return px;
    }

    void clear() { Base::clear(Colors<TColor>::Black); }
};

class RasterGrayscaleAAGammaPower: public RasterGrayscaleAA {
public:
    RasterGrayscaleAAGammaPower(const RasterBase::Resolution &res,
                                const RasterBase::PixelDim &  pd,
                                const RasterBase::Trafo &     trafo,
                                double                        gamma = 1.)
        : RasterGrayscaleAA(res, pd, trafo, agg::gamma_power(gamma))
    {}
};

}} // namespace Slic3r::sla

#endif // AGGRASTER_HPP
<|endoftext|>


<commit_before>

struct tile_face {
  tile_location tile;
  cardinal_direction which_face;
}

struct tile_edge {
  tile_location tile;
  something_or_other which_edge;
}

struct tile_face_shadow_info {
  collection<tile_edge> edges_directly_shadowing_this; // TODO do we need this? redundant with lit_shape

  // dunno if these will actually use the moving_bbox_collision_detector type
  moving_bbox_collision_detector<1> x_axis_edges;
  moving_bbox_collision_detector<1> y_axis_edges;
  moving_bbox_collision_detector<1> sun_angle_edges;

  // This structure is one of the cruces of the shadow edges algorithm.
  // It is a collection of loops of shadow edges or the edges of this face,
  //     the interior of which is an illuminated area.
  // However, because the visual positions of the edges are continuously changing,
  //     we don't actually record the visual positions of the edges on the face!
  // "What?" you say?
  // The key is that the TOPOLOGICAL structure does NOT change except at certain identifiable moments,
  //     which are handled by the shadow_changes system.
  // The lists need to be doubly linked, and the collection needs to have a way to look up
  //    the instances of a particular edge in log-or-better time
  //    (note that there can be more than one instance of an edge, although it's rare,
  //      as it only happens in a subset of the times when there is more than one loop,
  //      which is rare.)
  // They probably will be something fancier than just doubly linked list structures.
  (collection of doubly linked lists (maybe) of tile_edges) lit_shape;

  TODO we need to handle the case where e.g. when a moving diagonal line is pinched off by two axis aligned lines.

  [a bunch of insertion and deletion functions]
    most of which need to have access to the shadow_changes heap so that they can record the next-collisions of anything that was inserted,
    and faces_by_shadow_edge so they can record... hmm, maybe these should be private functions of lighting_distribution instead.

  tile_face adj_faces[4]; // not sure how these should be arranged / how we can make use of them
}

enum shadow_change_type {
  COLLISION, // shadow edges facing opposite directions collide
  SUBSUMATION, // a shadow edge catches up to another facing the same direction.
  PROCESSION, // a shadow edge hits the opposite edge of the tile_face
  RECESSION, // a shadow edge recedes past an edge of the tile face
  // shadow edges will probably be recorded in a way that has direction, so COLLISION/SUBSUMATION and PROCESSION/RECESSION could be combined into each other.
  // Actually we could just make the edge of the tile be the second tile_edge recorded in shadow_change, eliminating the need for types at all.
}

struct shadow_change {
  tile_face face;
  shadow_change_type type;
  tile_edge shadow1;
  tile_edge shadow2; // if any.
    // Standardization: SUBSUMATION is standardized as "shadow2, moving slower, subsumes shadow1, which is moving faster"
    // A faster-moving shadow is also always closer to the sun;
    // the standard is that shadow1 is closer to the sun.

  // Unnecessary because we now use tile_edges, which include dimension.
  // dimension_identifier which_dimension (maybe?);
}

struct lighting_distribution {
public:
  void do_shadow_change(time t, shadow_change c) {
    tile_face_shadow_info& inf = shadow_info[c.face];
    if (c.shadow1 is no longer in inf)) return;

    if (c.type == COLLISION) {
      if (c.shadow2 is no longer in inf)) return;
      assert(c.shadow1 and c.shadow2 are parallel);
      // Assume that shadow1 and shadow2 are at the same (direction-perpendicular-to-them) position.
      // If the time units are exact, this will be true, but I think we're okay with them being inexact (TODO check that mathematically)
      if (c.shadow1 and c.shadow2 don't overlap in the direction parallel to them) return;

      inf.lit_shape.do something at the point of collision (TODO)
      and update other inf members in accordance with that
      it's possible that both edges disappear, or that one of them does, or that neither does.

      // shadow2 is further from the sun, so shadow1 is now cast there (but not vice versa)
      c.shadow2 (considered as an edge in real space) is an edge of two faces, one of which is lit and the other of which is not.
      tile_face_shadow_info inf2 = shadow_info[the lit face];
      inf2.insert (as a dark area) c.shadow1 and any necessary adjacent edges
    }
    else if (c.type == SUBSUMATION) {
      if (c.shadow2 is no longer in inf)) return;
      if (c.shadow1 is not subsumed at time t) return; // TODO how do I compute this? and do I need times to be exact in order to do so?

      inf.subsume c.shadow1 into c.shadow2;
      inf.record_next_possible_change(c.shadow2);

      // A subsumation is always when a closer-to-the-sun object (shadow1's) stops shadowing
      //    anything beyond what a farther-from-the-sun object (shadow2's) is shadowing.
      // Hence, it's accompanied by (an infinitesimal duration later)
      //    shadow1's object ceasing to shadow some part of shadow2's object.
      c.shadow2 (considered as an edge in real space) is an edge of two faces, one of which is at an angle which can be lit by the sun and the other of which is not.
      tile_face_shadow_info inf2 = shadow_info[the potentially-lit face] (construct it (as totally-dark) if necessary);
      inf2.insert (as a lit area) c.shadow1 and any necessary adjacent edges
    }
    else if (c.type == PROCESSION) {
      // No extra checks - procession only fails if the shadow is no longer on the face at all.
      tile_edge e2 = the edge we're walking over;

      if (e2 joins this face with a parallel face or with one that folds back *towards* the sun) {
        tile_face_shadow_info* inf2 = find_as_pointer(shadow_info, that next face);
        inf2.insert (as a dark area) c.shadow1 and any necessary adjacent edges
      }
      else {
        for (each face f2 with e2 casting an edge of its lit area) {
          shadow_info[f2].insert (as a dark area) c.shadow1 and any necessary adjacent edges
        }
      }
    }
    else if (c.type == RECESSION) {
      // No extra checks - recession only fails if the shadow is no longer on the face at all.
      tile_edge e2 = the edge we're walking over;

      inf.subsume shadow1 into e2;

      if (e2 joins this face with a parallel face or with one that folds back *away from* the sun) {
        tile_face_shadow_info& inf2 = shadow_info[the potentially-lit face] (construct it (as totally-dark) if necessary);
        inf2.insert (as a lit area) c.shadow1 and any necessary adjacent edges
      }
      else {
        // e2 is at least partially in shadow, but it might be nonfully in shadow and still shadowing something else.
        // If it is, then that shadow is currently forming a corner with something where, once shadow1 recedes off the edge,
        // there will be a pathway of light going into the corner.
        for (each face f2 with e2 casting an edge of its lit area) {
          handle that
        }

        if (the above process didn't handle the full length of (c.shadow1 u e2)) {
          do a laser-like search (extrusion of the unhandled pieces) for face(s) on which light should now fall
            ignoring, I think, ones with an infinitesimal overlap
          for (each found face f2) {
            assert(e2 isn't casting any of the edges of f2's lit area)
            tile_face_shadow_info& inf2 = shadow_info[the newly-lit face] (construct it (as totally-dark) if necessary);
            inf2.insert (as a lit area) c.shadow1 and any necessary adjacent edges
          }
        }
      }
    }
    else { assert(false); }

    // logically we only need to do this check for COLLISION and PROCESSION
    if (inf.is_completely_dark()) shadow_info.erase(c.face);
  }
  void advance_to_time(time t) {
    while (the first of shadow_changes is before t) {
      do_shadow_change (pop the first of shadow_changes)
    }
  }
private:
  heap<time, shadow_change> shadow_changes;
  unordered_map<tile_face, tile_shadow_info> shadow_info;
  unordered_multimap<tile_edge, tile_face> faces_by_shadow_edge; // we need to do this reverse lookup sometimes
}
<commit_msg>half an attempt at redoing the pseudocode<commit_after>

struct edge {
  references to two corners, maybe
};
struct corner {
  references to two edges, possibly adjoining ("real corner"), possibly not ("virtual corner")
};

struct face_info {
  which face
  any number of outer edge loops identifying lit areas (some are shadows from above, some are the edges of the face)
  any number of inner edge loops identifying isolated shadows within those lit areas
  (note that edge loops and corner loops are the same thing, do it as you will)
  Canonical ordering: Loops with light on the inside are clockwise when viewed from the outer side of the face; loops with dark on the inside are counterclockwise.
};

/* Lit areas move around and change proportions. (not considered a change)
 Changes:
 outer loops' convex corners run into the edges of the faces they're on (or if faces are not convex, outer loops' edges with the concave corners of the face)
 corners/edges of a lit area run into each other.

 I think noticing those things about every lit area notices all shadow transitions.
 If any shadow intrudes from the outside, it intruded through an edge; therefore, it crossed the physical edge of a face above.
 */

struct shadow_change {
  face reference f
  edge reference e
  corner reference c
  maybe an invalidatable record of when/how it was computed
};

struct lighting_distribution {
public:
  void do_shadow_change(time t, shadow_change c) {
    If it's not still correct, bail

    // Edge is closer to the sun than corner, or further away
    // Corner edges span edge, or are on the same side

    On c.f:
      // Nothing new is added because both c.c and c.e were here already.
      // SO this isn't too hard.
      The old connections are: c.e to its neighbors, c.c.e1 to c.c.e2
      Switch to: c.e's previous neighbor -> c.e -> c.c.e1, c.c.e2 -> c.e -> its next neighbor

    // Shadow changes will also be caused either above here or below here

    if (c.c or c.e is a part of the face boundary) {
      if (it's c.e, the more common case) {
        Get whatever face that part of c.e was touching, f2:
        if (c.c crosses the boundary) {
          On c.f:
            c.c.e1 -> c.c.e2 -> c.e
            becomes
            c.c.e1 -> c.e
          On f2:
            c.e -> c.c.e2
            becomes
            c.e -> c.c.e1 -> c.c.e2
          (Or all of that but with c.e on the other side)
        }
        else {

          On c.f:
            c.c.e1 -> c.c.e2
            becomes
            c.c.e1 -> c.e -> c.c.e2
          On f2:
            c.e
            becomes
            c.e -> c.c.e1 -> c.c.e2 -> c.e
        }
      }
      else (it's c.c) {
        On c.f:
          The edge loop was: c.e
          Now it is: c.e -> c.c.e1 -> c.c.e2 -> c.e
      }
    }
  }

  void advance_to_time(time t) {
    while (the first of shadow_changes is before t) {
      do_shadow_change (pop the first of shadow_changes)
    }
  }
private:
  heap<time, shadow_change> shadow_changes;
}

#if 0
struct tile_face {
  tile_location tile;
  cardinal_direction which_face;
}

struct tile_edge {
  tile_location tile;
  something_or_other which_edge;
}

struct tile_face_shadow_info {
  collection<tile_edge> edges_directly_shadowing_this; // TODO do we need this? redundant with lit_shape

  // dunno if these will actually use the moving_bbox_collision_detector type
  moving_bbox_collision_detector<1> x_axis_edges;
  moving_bbox_collision_detector<1> y_axis_edges;
  moving_bbox_collision_detector<1> sun_angle_edges;

  // This structure is one of the cruces of the shadow edges algorithm.
  // It is a collection of loops of shadow edges or the edges of this face,
  //     the interior of which is an illuminated area.
  // However, because the visual positions of the edges are continuously changing,
  //     we don't actually record the visual positions of the edges on the face!
  // "What?" you say?
  // The key is that the TOPOLOGICAL structure does NOT change except at certain identifiable moments,
  //     which are handled by the shadow_changes system.
  // The lists need to be doubly linked, and the collection needs to have a way to look up
  //    the instances of a particular edge in log-or-better time
  //    (note that there can be more than one instance of an edge, although it's rare,
  //      as it only happens in a subset of the times when there is more than one loop,
  //      which is rare.)
  // They probably will be something fancier than just doubly linked list structures.
  (collection of doubly linked lists (maybe) of tile_edges) lit_shape;

  TODO we need to handle the case where e.g. when a moving diagonal line is pinched off by two axis aligned lines.

  [a bunch of insertion and deletion functions]
    most of which need to have access to the shadow_changes heap so that they can record the next-collisions of anything that was inserted,
    and faces_by_shadow_edge so they can record... hmm, maybe these should be private functions of lighting_distribution instead.

  tile_face adj_faces[4]; // not sure how these should be arranged / how we can make use of them
}

enum shadow_change_type {
  COLLISION, // shadow edges facing opposite directions collide
  SUBSUMATION, // a shadow edge catches up to another facing the same direction.
  PROCESSION, // a shadow edge hits the opposite edge of the tile_face
  RECESSION, // a shadow edge recedes past an edge of the tile face
  // shadow edges will probably be recorded in a way that has direction, so COLLISION/SUBSUMATION and PROCESSION/RECESSION could be combined into each other.
  // Actually we could just make the edge of the tile be the second tile_edge recorded in shadow_change, eliminating the need for types at all.
}

struct shadow_change {
  tile_face face;
  shadow_change_type type;
  tile_edge shadow1;
  tile_edge shadow2; // if any.
    // Standardization: SUBSUMATION is standardized as "shadow2, moving slower, subsumes shadow1, which is moving faster"
    // A faster-moving shadow is also always closer to the sun;
    // the standard is that shadow1 is closer to the sun.

  // Unnecessary because we now use tile_edges, which include dimension.
  // dimension_identifier which_dimension (maybe?);
}

struct lighting_distribution {
public:
  void do_shadow_change(time t, shadow_change c) {
    tile_face_shadow_info& inf = shadow_info[c.face];
    if (c.shadow1 is no longer in inf)) return;

    if (c.type == COLLISION) {
      if (c.shadow2 is no longer in inf)) return;
      assert(c.shadow1 and c.shadow2 are parallel);
      // Assume that shadow1 and shadow2 are at the same (direction-perpendicular-to-them) position.
      // If the time units are exact, this will be true, but I think we're okay with them being inexact (TODO check that mathematically)
      if (c.shadow1 and c.shadow2 don't overlap in the direction parallel to them) return;

      inf.lit_shape.do something at the point of collision (TODO)
      and update other inf members in accordance with that
      it's possible that both edges disappear, or that one of them does, or that neither does.

      // shadow2 is further from the sun, so shadow1 is now cast there (but not vice versa)
      c.shadow2 (considered as an edge in real space) is an edge of two faces, one of which is lit and the other of which is not.
      tile_face_shadow_info inf2 = shadow_info[the lit face];
      inf2.insert (as a dark area) c.shadow1 and any necessary adjacent edges
    }
    else if (c.type == SUBSUMATION) {
      if (c.shadow2 is no longer in inf)) return;
      if (c.shadow1 is not subsumed at time t) return; // TODO how do I compute this? and do I need times to be exact in order to do so?

      inf.subsume c.shadow1 into c.shadow2;
      inf.record_next_possible_change(c.shadow2);

      // A subsumation is always when a closer-to-the-sun object (shadow1's) stops shadowing
      //    anything beyond what a farther-from-the-sun object (shadow2's) is shadowing.
      // Hence, it's accompanied by (an infinitesimal duration later)
      //    shadow1's object ceasing to shadow some part of shadow2's object.
      c.shadow2 (considered as an edge in real space) is an edge of two faces, one of which is at an angle which can be lit by the sun and the other of which is not.
      tile_face_shadow_info inf2 = shadow_info[the potentially-lit face] (construct it (as totally-dark) if necessary);
      inf2.insert (as a lit area) c.shadow1 and any necessary adjacent edges
    }
    else if (c.type == PROCESSION) {
      // No extra checks - procession only fails if the shadow is no longer on the face at all.
      tile_edge e2 = the edge we're walking over;

      if (e2 joins this face with a parallel face or with one that folds back *towards* the sun) {
        tile_face_shadow_info* inf2 = find_as_pointer(shadow_info, that next face);
        inf2.insert (as a dark area) c.shadow1 and any necessary adjacent edges
      }
      else {
        for (each face f2 with e2 casting an edge of its lit area) {
          shadow_info[f2].insert (as a dark area) c.shadow1 and any necessary adjacent edges
        }
      }
    }
    else if (c.type == RECESSION) {
      // No extra checks - recession only fails if the shadow is no longer on the face at all.
      tile_edge e2 = the edge we're walking over;

      inf.subsume shadow1 into e2;

      if (e2 joins this face with a parallel face or with one that folds back *away from* the sun) {
        tile_face_shadow_info& inf2 = shadow_info[the potentially-lit face] (construct it (as totally-dark) if necessary);
        inf2.insert (as a lit area) c.shadow1 and any necessary adjacent edges
      }
      else {
        // e2 is at least partially in shadow, but it might be nonfully in shadow and still shadowing something else.
        // If it is, then that shadow is currently forming a corner with something where, once shadow1 recedes off the edge,
        // there will be a pathway of light going into the corner.
        for (each face f2 with e2 casting an edge of its lit area) {
          handle that
        }

        if (the above process didn't handle the full length of (c.shadow1 u e2)) {
          do a laser-like search (extrusion of the unhandled pieces) for face(s) on which light should now fall
            ignoring, I think, ones with an infinitesimal overlap
          for (each found face f2) {
            assert(e2 isn't casting any of the edges of f2's lit area)
            tile_face_shadow_info& inf2 = shadow_info[the newly-lit face] (construct it (as totally-dark) if necessary);
            inf2.insert (as a lit area) c.shadow1 and any necessary adjacent edges
          }
        }
      }
    }
    else { assert(false); }

    // logically we only need to do this check for COLLISION and PROCESSION
    if (inf.is_completely_dark()) shadow_info.erase(c.face);
  }
  void advance_to_time(time t) {
    while (the first of shadow_changes is before t) {
      do_shadow_change (pop the first of shadow_changes)
    }
  }
private:
  heap<time, shadow_change> shadow_changes;
  unordered_map<tile_face, tile_shadow_info> shadow_info;
  unordered_multimap<tile_edge, tile_face> faces_by_shadow_edge; // we need to do this reverse lookup sometimes
}
#endif
<|endoftext|>


<commit_before>/**
 * @file TextSummarizationSubManager.cpp
 * @brief header file of TextSummarizationSubManager class
 * @author MyungHyun (Kent)
 * @date 2008-06-05
 * @details
 * - Log
 * @author Deepesh Shrestha
 *   Modified interfaces to generate sentence blocks for
 *   snippet and summarization
 */

#include "TextSummarizationSubManager.h"
#include "text-summarization/TextSummarization.h"

using namespace std;
using namespace izenelib::util;     //for UString
using namespace sf1r::text_summarization;

namespace sf1r
{

TextSummarizationSubManager::TextSummarizationSubManager()
{}

TextSummarizationSubManager::~TextSummarizationSubManager()
{}

void TextSummarizationSubManager::init(
    ilplib::langid::Analyzer* langIdAnalyzer,
    boost::shared_ptr<izenelib::ir::idmanager::IDManager> idManager
)
{
    langIdAnalyzer_ = langIdAnalyzer;
    idManager_ = idManager;
}

////@brief get requests for getting summary, snippet offsets, when given a text
bool TextSummarizationSubManager::getOffsetPairs(
    const izenelib::util::UString& textBody,
    const unsigned int maxDisplayLength,
    const unsigned int numOfSentence,
    std::vector<CharacterOffset>& offsetPairs
)
{
    //clear out structure
    offsetPairs.clear();

    std::string utf8_text;
    textBody.convertString(utf8_text, izenelib::util::UString::UTF_8);
    const char* p = utf8_text.c_str();

    std::vector<std::vector<TermId> > sentenceListInTermId;
    std::vector<std::pair<CharacterOffset, CharacterOffset> > sentencesOffsetPairs;

    CharacterOffset startPos = 0;
    while (int len = langIdAnalyzer_->sentenceLength(p))
    {
        UString sentence;
        /// Add an efficient UString initializer for it. - Wei, 2010.08.25
        sentence.assign(p, len, izenelib::util::UString::UTF_8);

        /// Replace the old inefficient API to the new one. - Wei, 2010.08.25
        TermId id;
        sentenceListInTermId.push_back(vector<TermId>());
        vector<TermId>& sentenceIds = sentenceListInTermId.back();

        tokenizer_.tokenize(sentence);
        while (tokenizer_.nextToken())
        {
            if (!tokenizer_.isDelimiter())
            {
                idManager_->getTermIdByTermString(tokenizer_.getToken(), tokenizer_.getLength(), id);
                sentenceIds.push_back(id);
            }
        }

        sentencesOffsetPairs.push_back(make_pair(startPos, startPos+sentence.length()));
        startPos+=sentence.length();
        p += len;				 // move to the begining of next sentence
    }

    //initialize first value to be 0 by default
    offsetPairs.push_back(0);

    //append summary offset
    vector<unsigned int> sentenceOrder;
    if (numOfSentence > 0)
    {
        TextSummarization ts( numOfSentence );
        ts.computeSummaryRM(numOfSentence, sentenceListInTermId, sentenceOrder);

        //adding summary sentence Offset pair
        unsigned int i=0;
        while ( ( i < sentenceOrder.size()) && (i < numOfSentence))
        {
            if (sentenceOrder[i] < sentencesOffsetPairs.size())
            {
                offsetPairs.push_back(sentencesOffsetPairs[sentenceOrder[i]].first);
                offsetPairs.push_back(sentencesOffsetPairs[sentenceOrder[i]].second);
            }
            i++;
        }

        if (numOfSentence < sentenceOrder.size())
            offsetPairs[0] = numOfSentence;
        else
            offsetPairs[0] = sentenceOrder.size();

        sentenceOrder.clear();
    }

    //adding snippet sentence offset pair
    unsigned int j = 0;
    while ( j < sentencesOffsetPairs.size())
    {
        offsetPairs.push_back(sentencesOffsetPairs[j].first);
        offsetPairs.push_back(sentencesOffsetPairs[j].second);
        j++;
    }

    if (offsetPairs.size() > 0)
        return true;
    else
        return false;
}

}

<commit_msg>as langid interface supports UString parameter now, in TextSummarizationSubManager::getOffsetPairs(), pass UString to langid interface directly to avoid conversion from UString to utf8 string as before.<commit_after>/**
 * @file TextSummarizationSubManager.cpp
 * @brief header file of TextSummarizationSubManager class
 * @author MyungHyun (Kent)
 * @date 2008-06-05
 * @details
 * - Log
 * @author Deepesh Shrestha
 *   Modified interfaces to generate sentence blocks for
 *   snippet and summarization
 */

#include "TextSummarizationSubManager.h"
#include "text-summarization/TextSummarization.h"

using namespace std;
using namespace izenelib::util;     //for UString
using namespace sf1r::text_summarization;

namespace sf1r
{

TextSummarizationSubManager::TextSummarizationSubManager()
{}

TextSummarizationSubManager::~TextSummarizationSubManager()
{}

void TextSummarizationSubManager::init(
    ilplib::langid::Analyzer* langIdAnalyzer,
    boost::shared_ptr<izenelib::ir::idmanager::IDManager> idManager
)
{
    langIdAnalyzer_ = langIdAnalyzer;
    idManager_ = idManager;
}

////@brief get requests for getting summary, snippet offsets, when given a text
bool TextSummarizationSubManager::getOffsetPairs(
    const izenelib::util::UString& textBody,
    const unsigned int maxDisplayLength,
    const unsigned int numOfSentence,
    std::vector<CharacterOffset>& offsetPairs
)
{
    //clear out structure
    offsetPairs.clear();

    std::vector<std::vector<TermId> > sentenceListInTermId;
    std::vector<std::pair<CharacterOffset, CharacterOffset> > sentencesOffsetPairs;

    UString sentence;
    CharacterOffset startPos = 0;
    while (std::size_t len = langIdAnalyzer_->sentenceLength(textBody, startPos))
    {
        sentence.assign(textBody, startPos, len);

        /// Replace the old inefficient API to the new one. - Wei, 2010.08.25
        TermId id;
        sentenceListInTermId.push_back(vector<TermId>());
        vector<TermId>& sentenceIds = sentenceListInTermId.back();

        tokenizer_.tokenize(sentence);
        while (tokenizer_.nextToken())
        {
            if (!tokenizer_.isDelimiter())
            {
                idManager_->getTermIdByTermString(tokenizer_.getToken(), tokenizer_.getLength(), id);
                sentenceIds.push_back(id);
            }
        }

        sentencesOffsetPairs.push_back(make_pair(startPos, startPos+len));
        startPos += len;
    }

    //initialize first value to be 0 by default
    offsetPairs.push_back(0);

    //append summary offset
    vector<unsigned int> sentenceOrder;
    if (numOfSentence > 0)
    {
        TextSummarization ts( numOfSentence );
        ts.computeSummaryRM(numOfSentence, sentenceListInTermId, sentenceOrder);

        //adding summary sentence Offset pair
        unsigned int i=0;
        while ( ( i < sentenceOrder.size()) && (i < numOfSentence))
        {
            if (sentenceOrder[i] < sentencesOffsetPairs.size())
            {
                offsetPairs.push_back(sentencesOffsetPairs[sentenceOrder[i]].first);
                offsetPairs.push_back(sentencesOffsetPairs[sentenceOrder[i]].second);
            }
            i++;
        }

        if (numOfSentence < sentenceOrder.size())
            offsetPairs[0] = numOfSentence;
        else
            offsetPairs[0] = sentenceOrder.size();

        sentenceOrder.clear();
    }

    //adding snippet sentence offset pair
    unsigned int j = 0;
    while ( j < sentencesOffsetPairs.size())
    {
        offsetPairs.push_back(sentencesOffsetPairs[j].first);
        offsetPairs.push_back(sentencesOffsetPairs[j].second);
        j++;
    }

    if (offsetPairs.size() > 0)
        return true;
    else
        return false;
}

}

<|endoftext|>


<commit_before>/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2012, Willow Garage, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Willow Garage nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

/* Author: Ioan Sucan */

#include <moveit/ompl_interface/parameterization/model_based_joint_state_space.h>

ompl_interface::ModelBasedJointStateSpace::ModelBasedJointStateSpace(const kinematic_model::JointModel *joint_model,
                                                                     const kinematic_model::JointModel::Bounds &joint_bounds) :
  ompl::base::StateSpace(), joint_model_(joint_model), joint_bounds_(joint_bounds)
{
  // set the state space name
  setName(joint_model_->getName());

  if (joint_bounds_.size() != joint_model_->getVariableBounds().size())
  {
    logError("Joint '%s' from group has incorrect bounds specified. Using the default bounds instead.",  joint_model_->getName().c_str());
    joint_bounds_ = joint_model_->getVariableBounds();
  }
}

ompl_interface::ModelBasedJointStateSpace::ModelBasedJointStateSpace(const kinematic_model::JointModel *joint_model) :
  ompl::base::StateSpace(), joint_model_(joint_model)
{
  // set the state space name
  setName(joint_model_->getName());
  joint_bounds_ = joint_model_->getVariableBounds();
}

ompl_interface::ModelBasedJointStateSpace::~ModelBasedJointStateSpace(void)
{
}

ompl::base::State* ompl_interface::ModelBasedJointStateSpace::allocState(void) const
{
  StateType *st = new StateType();
  st->joint_state = new kinematic_state::JointState(joint_model_);
  return st;
}

void ompl_interface::ModelBasedJointStateSpace::freeState(ompl::base::State *state) const
{
  delete state->as<StateType>()->joint_state;
  delete state->as<StateType>();
}

unsigned int ompl_interface::ModelBasedJointStateSpace::getDimension(void) const
{
  return joint_model_->getStateSpaceDimension();
}

double ompl_interface::ModelBasedJointStateSpace::getMaximumExtent(void) const
{
  return joint_model_->getMaximumExtent();
}

void ompl_interface::ModelBasedJointStateSpace::enforceBounds(ompl::base::State *state) const
{
  joint_model_->enforceBounds(state->as<StateType>()->joint_state->getVariableValues(), joint_bounds_);
}

bool ompl_interface::ModelBasedJointStateSpace::satisfiesBounds(const ompl::base::State *state) const
{
  return joint_model_->satisfiesBounds(state->as<StateType>()->joint_state->getVariableValues(), joint_bounds_, std::numeric_limits<double>::epsilon());
}

void ompl_interface::ModelBasedJointStateSpace::copyState(ompl::base::State *destination, const ompl::base::State *source) const
{
  *destination->as<StateType>()->joint_state = *source->as<StateType>()->joint_state;
}

double ompl_interface::ModelBasedJointStateSpace::distance(const ompl::base::State *state1, const ompl::base::State *state2) const
{
  return state1->as<StateType>()->joint_state->distance(state2->as<StateType>()->joint_state);
}

bool ompl_interface::ModelBasedJointStateSpace::equalStates(const ompl::base::State *state1, const ompl::base::State *state2) const
{
  return state1->as<StateType>()->joint_state->getVariableValues() == state2->as<StateType>()->joint_state->getVariableValues();
}

void ompl_interface::ModelBasedJointStateSpace::interpolate(const ompl::base::State *from, const ompl::base::State *to, const double t, ompl::base::State *state) const
{
  from->as<StateType>()->joint_state->interpolate(to->as<StateType>()->joint_state, t, state->as<StateType>()->joint_state);
  propagateJointStateUpdate(state);
}

unsigned int ompl_interface::ModelBasedJointStateSpace::getSerializationLength(void) const
{
  return sizeof(double) * joint_model_->getVariableCount();
}

void ompl_interface::ModelBasedJointStateSpace::serialize(void *serialization, const ompl::base::State *state) const
{
  memcpy(serialization, &state->as<StateType>()->joint_state->getVariableValues()[0], joint_model_->getVariableCount() * sizeof(double));
}

void ompl_interface::ModelBasedJointStateSpace::deserialize(ompl::base::State *state, const void *serialization) const
{
  memcpy(&state->as<StateType>()->joint_state->getVariableValues()[0], serialization, joint_model_->getVariableCount() * sizeof(double));
}

void ompl_interface::ModelBasedJointStateSpace::setBounds(double minX, double maxX, double minY, double maxY, double minZ, double maxZ)
{
  if (joint_model_->getType() == kinematic_model::JointModel::PLANAR)
  {
    joint_bounds_[0].first = minX;
    joint_bounds_[0].second = maxX;
    joint_bounds_[1].first = minY;
    joint_bounds_[1].second = maxY;
  }
  else
    if (joint_model_->getType() == kinematic_model::JointModel::FLOATING)
    {
      joint_bounds_[0].first = minX;
      joint_bounds_[0].second = maxX;
      joint_bounds_[1].first = minY;
      joint_bounds_[1].second = maxY;
      joint_bounds_[2].first = minZ;
      joint_bounds_[2].second = maxZ;
    }
}

double* ompl_interface::ModelBasedJointStateSpace::getValueAddressAtIndex(ompl::base::State *state, const unsigned int index) const
{
  if (index >= joint_model_->getVariableCount())
    return NULL;
  return &state->as<StateType>()->joint_state->getVariableValues()[index];
}

void ompl_interface::ModelBasedJointStateSpace::printState(const ompl::base::State *state, std::ostream &out) const
{
  out << "JointState(" << joint_model_->getName() << ") = [ ";
  for (std::size_t j = 0 ; j < state->as<StateType>()->joint_state->getVariableValues().size() ; ++j)
    out << state->as<StateType>()->joint_state->getVariableValues()[j] << " ";
  out << "]" << std::endl;
}

void ompl_interface::ModelBasedJointStateSpace::printSettings(std::ostream &out) const
{
  out << "ModelBasedJointStateSpace '" << getName() << "' at " << this << std::endl;
}

void ompl_interface::ModelBasedJointStateSpace::propagateJointStateUpdate(ompl::base::State *state) const
{
  // update the transform the joint applies
  joint_model_->updateTransform(state->as<StateType>()->joint_state->getVariableValues(), state->as<StateType>()->joint_state->getVariableTransform());

  // propagate updates to mimic joints
  state->as<StateType>()->joint_state->updateMimicJoints();
}

ompl::base::StateSamplerPtr ompl_interface::ModelBasedJointStateSpace::allocDefaultStateSampler(void) const
{
  class DefaultStateSampler : public ompl::base::StateSampler
  {
  public:

    DefaultStateSampler(const ompl::base::StateSpace *space) : ompl::base::StateSampler(space),
                                                               values_(space->as<ModelBasedJointStateSpace>()->getJointModel()->getVariableCount())
    {
    }

    virtual void sampleUniform(ompl::base::State *state)
    {
      std::vector<double> &values = state->as<ModelBasedJointStateSpace::StateType>()->joint_state->getVariableValues();
      values.clear(); // clear first since getRandomValues() does .push_back()

      // generate random values accorrding to JointModel
      space_->as<ModelBasedJointStateSpace>()->getJointModel()->getVariableRandomValues(moveit_rng_, values, space_->as<ModelBasedJointStateSpace>()->getJointBounds());

      // propagate transforms
      space_->as<ModelBasedJointStateSpace>()->propagateJointStateUpdate(state);
    }

    virtual void sampleUniformNear(ompl::base::State *state, const ompl::base::State *near, const double distance)
    {
      std::vector<double> &values = state->as<ModelBasedJointStateSpace::StateType>()->joint_state->getVariableValues();
      values.clear(); // clear first since getRandomValues() does .push_back()

      // generate random values accorrding to JointModel
      space_->as<ModelBasedJointStateSpace>()->getJointModel()->getVariableRandomValuesNearBy(moveit_rng_, values, space_->as<ModelBasedJointStateSpace>()->getJointBounds(),
                                                                                near->as<ModelBasedJointStateSpace::StateType>()->joint_state->getVariableValues(),
                                                                                distance);
      // propagate transforms
      space_->as<ModelBasedJointStateSpace>()->propagateJointStateUpdate(state);
    }

    virtual void sampleGaussian(ompl::base::State *state, const ompl::base::State *mean, const double stdDev)
    {
      sampleUniformNear(state, mean, rng_.gaussian(0.0, stdDev));
    }

  protected:
    std::vector<double> values_;
    random_numbers::RandomNumberGenerator moveit_rng_;
  };

  return ompl::base::StateSamplerPtr(static_cast<ompl::base::StateSampler*>(new DefaultStateSampler(this)));
}
<commit_msg>use generalized version of getMaximumExtent()<commit_after>/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2012, Willow Garage, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Willow Garage nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

/* Author: Ioan Sucan */

#include <moveit/ompl_interface/parameterization/model_based_joint_state_space.h>

ompl_interface::ModelBasedJointStateSpace::ModelBasedJointStateSpace(const kinematic_model::JointModel *joint_model,
                                                                     const kinematic_model::JointModel::Bounds &joint_bounds) :
  ompl::base::StateSpace(), joint_model_(joint_model), joint_bounds_(joint_bounds)
{
  // set the state space name
  setName(joint_model_->getName());

  if (joint_bounds_.size() != joint_model_->getVariableBounds().size())
  {
    logError("Joint '%s' from group has incorrect bounds specified. Using the default bounds instead.",  joint_model_->getName().c_str());
    joint_bounds_ = joint_model_->getVariableBounds();
  }
}

ompl_interface::ModelBasedJointStateSpace::ModelBasedJointStateSpace(const kinematic_model::JointModel *joint_model) :
  ompl::base::StateSpace(), joint_model_(joint_model)
{
  // set the state space name
  setName(joint_model_->getName());
  joint_bounds_ = joint_model_->getVariableBounds();
}

ompl_interface::ModelBasedJointStateSpace::~ModelBasedJointStateSpace(void)
{
}

ompl::base::State* ompl_interface::ModelBasedJointStateSpace::allocState(void) const
{
  StateType *st = new StateType();
  st->joint_state = new kinematic_state::JointState(joint_model_);
  return st;
}

void ompl_interface::ModelBasedJointStateSpace::freeState(ompl::base::State *state) const
{
  delete state->as<StateType>()->joint_state;
  delete state->as<StateType>();
}

unsigned int ompl_interface::ModelBasedJointStateSpace::getDimension(void) const
{
  return joint_model_->getStateSpaceDimension();
}

double ompl_interface::ModelBasedJointStateSpace::getMaximumExtent(void) const
{
  return joint_model_->getMaximumExtent(joint_bounds_);
}

void ompl_interface::ModelBasedJointStateSpace::enforceBounds(ompl::base::State *state) const
{
  joint_model_->enforceBounds(state->as<StateType>()->joint_state->getVariableValues(), joint_bounds_);
}

bool ompl_interface::ModelBasedJointStateSpace::satisfiesBounds(const ompl::base::State *state) const
{
  return joint_model_->satisfiesBounds(state->as<StateType>()->joint_state->getVariableValues(), joint_bounds_, std::numeric_limits<double>::epsilon());
}

void ompl_interface::ModelBasedJointStateSpace::copyState(ompl::base::State *destination, const ompl::base::State *source) const
{
  *destination->as<StateType>()->joint_state = *source->as<StateType>()->joint_state;
}

double ompl_interface::ModelBasedJointStateSpace::distance(const ompl::base::State *state1, const ompl::base::State *state2) const
{
  return state1->as<StateType>()->joint_state->distance(state2->as<StateType>()->joint_state);
}

bool ompl_interface::ModelBasedJointStateSpace::equalStates(const ompl::base::State *state1, const ompl::base::State *state2) const
{
  return state1->as<StateType>()->joint_state->getVariableValues() == state2->as<StateType>()->joint_state->getVariableValues();
}

void ompl_interface::ModelBasedJointStateSpace::interpolate(const ompl::base::State *from, const ompl::base::State *to, const double t, ompl::base::State *state) const
{
  from->as<StateType>()->joint_state->interpolate(to->as<StateType>()->joint_state, t, state->as<StateType>()->joint_state);
  propagateJointStateUpdate(state);
}

unsigned int ompl_interface::ModelBasedJointStateSpace::getSerializationLength(void) const
{
  return sizeof(double) * joint_model_->getVariableCount();
}

void ompl_interface::ModelBasedJointStateSpace::serialize(void *serialization, const ompl::base::State *state) const
{
  memcpy(serialization, &state->as<StateType>()->joint_state->getVariableValues()[0], joint_model_->getVariableCount() * sizeof(double));
}

void ompl_interface::ModelBasedJointStateSpace::deserialize(ompl::base::State *state, const void *serialization) const
{
  memcpy(&state->as<StateType>()->joint_state->getVariableValues()[0], serialization, joint_model_->getVariableCount() * sizeof(double));
}

void ompl_interface::ModelBasedJointStateSpace::setBounds(double minX, double maxX, double minY, double maxY, double minZ, double maxZ)
{
  if (joint_model_->getType() == kinematic_model::JointModel::PLANAR)
  {
    joint_bounds_[0].first = minX;
    joint_bounds_[0].second = maxX;
    joint_bounds_[1].first = minY;
    joint_bounds_[1].second = maxY;
  }
  else
    if (joint_model_->getType() == kinematic_model::JointModel::FLOATING)
    {
      joint_bounds_[0].first = minX;
      joint_bounds_[0].second = maxX;
      joint_bounds_[1].first = minY;
      joint_bounds_[1].second = maxY;
      joint_bounds_[2].first = minZ;
      joint_bounds_[2].second = maxZ;
    }
}

double* ompl_interface::ModelBasedJointStateSpace::getValueAddressAtIndex(ompl::base::State *state, const unsigned int index) const
{
  if (index >= joint_model_->getVariableCount())
    return NULL;
  return &state->as<StateType>()->joint_state->getVariableValues()[index];
}

void ompl_interface::ModelBasedJointStateSpace::printState(const ompl::base::State *state, std::ostream &out) const
{
  out << "JointState(" << joint_model_->getName() << ") = [ ";
  for (std::size_t j = 0 ; j < state->as<StateType>()->joint_state->getVariableValues().size() ; ++j)
    out << state->as<StateType>()->joint_state->getVariableValues()[j] << " ";
  out << "]" << std::endl;
}

void ompl_interface::ModelBasedJointStateSpace::printSettings(std::ostream &out) const
{
  out << "ModelBasedJointStateSpace '" << getName() << "' at " << this << std::endl;
}

void ompl_interface::ModelBasedJointStateSpace::propagateJointStateUpdate(ompl::base::State *state) const
{
  // update the transform the joint applies
  joint_model_->updateTransform(state->as<StateType>()->joint_state->getVariableValues(), state->as<StateType>()->joint_state->getVariableTransform());

  // propagate updates to mimic joints
  state->as<StateType>()->joint_state->updateMimicJoints();
}

ompl::base::StateSamplerPtr ompl_interface::ModelBasedJointStateSpace::allocDefaultStateSampler(void) const
{
  class DefaultStateSampler : public ompl::base::StateSampler
  {
  public:

    DefaultStateSampler(const ompl::base::StateSpace *space) : ompl::base::StateSampler(space),
                                                               values_(space->as<ModelBasedJointStateSpace>()->getJointModel()->getVariableCount())
    {
    }

    virtual void sampleUniform(ompl::base::State *state)
    {
      std::vector<double> &values = state->as<ModelBasedJointStateSpace::StateType>()->joint_state->getVariableValues();
      values.clear(); // clear first since getRandomValues() does .push_back()

      // generate random values accorrding to JointModel
      space_->as<ModelBasedJointStateSpace>()->getJointModel()->getVariableRandomValues(moveit_rng_, values, space_->as<ModelBasedJointStateSpace>()->getJointBounds());

      // propagate transforms
      space_->as<ModelBasedJointStateSpace>()->propagateJointStateUpdate(state);
    }

    virtual void sampleUniformNear(ompl::base::State *state, const ompl::base::State *near, const double distance)
    {
      std::vector<double> &values = state->as<ModelBasedJointStateSpace::StateType>()->joint_state->getVariableValues();
      values.clear(); // clear first since getRandomValues() does .push_back()

      // generate random values accorrding to JointModel
      space_->as<ModelBasedJointStateSpace>()->getJointModel()->getVariableRandomValuesNearBy(moveit_rng_, values, space_->as<ModelBasedJointStateSpace>()->getJointBounds(),
                                                                                near->as<ModelBasedJointStateSpace::StateType>()->joint_state->getVariableValues(),
                                                                                distance);
      // propagate transforms
      space_->as<ModelBasedJointStateSpace>()->propagateJointStateUpdate(state);
    }

    virtual void sampleGaussian(ompl::base::State *state, const ompl::base::State *mean, const double stdDev)
    {
      sampleUniformNear(state, mean, rng_.gaussian(0.0, stdDev));
    }

  protected:
    std::vector<double> values_;
    random_numbers::RandomNumberGenerator moveit_rng_;
  };

  return ompl::base::StateSamplerPtr(static_cast<ompl::base::StateSampler*>(new DefaultStateSampler(this)));
}
<|endoftext|>