<commit_before>#include "catch/catch.hpp"
#include "RaZ/Utils/Ray.hpp"

namespace {

// Declaring rays to be tested
//
//        Ray 1      |       Ray 2       |      Ray 3
//                   |                   |
//          ^        |           ^       |      [ 1; 1 ]
//          |        |          /        |         /
//          |        |         /         |        /
//      [ 0; 0 ]     |    [ -1; -1 ]     |       v

const Raz::Ray ray1(Raz::Vec3f({ 0.f, 0.f, 0.f }), Raz::Axis::Y);
const Raz::Ray ray2(Raz::Vec3f({ -1.f, -1.f, 0.f }), Raz::Vec3f({ 1.f, 1.f, 0.f }).normalize());
const Raz::Ray ray3(Raz::Vec3f({ 1.f, 1.f, 0.f }), Raz::Vec3f({ -1.f, -1.f, 0.f }).normalize());

} // namespace

TEST_CASE("Ray-point intersection checks") {
  REQUIRE(ray1.intersects(ray1.getOrigin()));

  const Raz::Vec3f topPoint({ 0.f, 2.f, 0.f });
  const Raz::Vec3f topRightPoint({ 2.f, 2.f, 0.f });

  //     topPoint  topRightPoint
  //     [ 0; 2 ]    [ 2; 2 ]
  //
  //        ^
  //        |
  //        x < [ 0; 0 ]
  REQUIRE(ray1.intersects(topPoint));
  REQUIRE_FALSE(ray1.intersects(topRightPoint));

  //     topPoint  topRightPoint
  //     [ 0; 2 ]    [ 2; 2 ]
  //
  //          ^
  //         /
  //        x < [ 0; 0 ]
  REQUIRE_FALSE(ray2.intersects(topPoint));
  REQUIRE(ray2.intersects(topRightPoint));

  //     topPoint  topRightPoint
  //     [ 0; 2 ]    [ 2; 2 ]
  //
  //             x < [ 1; 1 ]
  //            /
  //           v
  REQUIRE_FALSE(ray3.intersects(topPoint));
  REQUIRE_FALSE(ray3.intersects(topRightPoint));
}

TEST_CASE("Point projection checks") {
  const Raz::Vec3f topPoint({ 0.f, 2.f, 0.f });
  const Raz::Vec3f topRightPoint({ 2.f, 2.f, 0.f });

  //     topPoint  topRightPoint
  //     [ 0; 2 ]    [ 2; 2 ]
  //
  //        ^
  //        |
  //        x < [ 0; 0 ]
  REQUIRE(ray1.computeProjection(topPoint) == topPoint);
  REQUIRE(ray1.computeProjection(topRightPoint) == topPoint);

  //     topPoint  topRightPoint
  //     [ 0; 2 ]    [ 2; 2 ]
  //
  //          ^
  //         /
  //        x < [ 0; 0 ]
  REQUIRE(ray2.computeProjection(topPoint) == Raz::Vec3f({ 1.f, 1.f, 0.f }));
  REQUIRE(ray2.computeProjection(topRightPoint) == topRightPoint);

  //     topPoint  topRightPoint
  //     [ 0; 2 ]    [ 2; 2 ]
  //
  //             x < [ 1; 1 ]
  //            /
  //           v
  REQUIRE(ray3.computeProjection(topPoint) == ray3.getOrigin());
  REQUIRE(ray3.computeProjection(topRightPoint) == ray3.getOrigin());
}
<commit_msg>[Tests/Utils/Ray] Added a ray-sphere intersection unit test<commit_after>#include "catch/catch.hpp"
#include "RaZ/Utils/Ray.hpp"

namespace {

// Declaring rays to be tested
//
//        Ray 1      |       Ray 2       |      Ray 3
//                   |                   |
//          ^        |           ^       |      [ 1; 1 ]
//          |        |          /        |         /
//          |        |         /         |        /
//      [ 0; 0 ]     |    [ -1; -1 ]     |       v

const Raz::Ray ray1(Raz::Vec3f({ 0.f, 0.f, 0.f }), Raz::Axis::Y);
const Raz::Ray ray2(Raz::Vec3f({ -1.f, -1.f, 0.f }), Raz::Vec3f({ 1.f, 1.f, 0.f }).normalize());
const Raz::Ray ray3(Raz::Vec3f({ 1.f, 1.f, 0.f }), Raz::Vec3f({ -1.f, -1.f, 0.f }).normalize());

} // namespace

TEST_CASE("Ray-point intersection checks") {
  REQUIRE(ray1.intersects(ray1.getOrigin()));

  const Raz::Vec3f topPoint({ 0.f, 2.f, 0.f });
  const Raz::Vec3f topRightPoint({ 2.f, 2.f, 0.f });

  //     topPoint  topRightPoint
  //     [ 0; 2 ]    [ 2; 2 ]
  //
  //        ^
  //        |
  //        x < [ 0; 0 ]
  REQUIRE(ray1.intersects(topPoint));
  REQUIRE_FALSE(ray1.intersects(topRightPoint));

  //     topPoint  topRightPoint
  //     [ 0; 2 ]    [ 2; 2 ]
  //
  //          ^
  //         /
  //        x < [ 0; 0 ]
  REQUIRE_FALSE(ray2.intersects(topPoint));
  REQUIRE(ray2.intersects(topRightPoint));

  //     topPoint  topRightPoint
  //     [ 0; 2 ]    [ 2; 2 ]
  //
  //             x < [ 1; 1 ]
  //            /
  //           v
  REQUIRE_FALSE(ray3.intersects(topPoint));
  REQUIRE_FALSE(ray3.intersects(topRightPoint));
}

TEST_CASE("Ray-sphere intersection") {
  const Raz::Sphere sphere1(Raz::Vec3f(0.f), 1.f);
  const Raz::Sphere sphere2(Raz::Vec3f({ 5.f, 10.f, 0.f }), 5.f);
  const Raz::Sphere sphere3(Raz::Vec3f({ -10.f, -10.f, 0.f }), 1.f);

  REQUIRE(ray1.intersects(sphere1));
  REQUIRE(ray2.intersects(sphere1));
  REQUIRE(ray3.intersects(sphere1));

  REQUIRE(ray1.intersects(sphere2));
  REQUIRE(ray2.intersects(sphere2));
  REQUIRE_FALSE(ray3.intersects(sphere2));

  REQUIRE_FALSE(ray1.intersects(sphere3));
  REQUIRE_FALSE(ray2.intersects(sphere3));
  REQUIRE(ray3.intersects(sphere3));
}

TEST_CASE("Point projection checks") {
  const Raz::Vec3f topPoint({ 0.f, 2.f, 0.f });
  const Raz::Vec3f topRightPoint({ 2.f, 2.f, 0.f });

  //     topPoint  topRightPoint
  //     [ 0; 2 ]    [ 2; 2 ]
  //
  //        ^
  //        |
  //        x < [ 0; 0 ]
  REQUIRE(ray1.computeProjection(topPoint) == topPoint);
  REQUIRE(ray1.computeProjection(topRightPoint) == topPoint);

  //     topPoint  topRightPoint
  //     [ 0; 2 ]    [ 2; 2 ]
  //
  //          ^
  //         /
  //        x < [ 0; 0 ]
  REQUIRE(ray2.computeProjection(topPoint) == Raz::Vec3f({ 1.f, 1.f, 0.f }));
  REQUIRE(ray2.computeProjection(topRightPoint) == topRightPoint);

  //     topPoint  topRightPoint
  //     [ 0; 2 ]    [ 2; 2 ]
  //
  //             x < [ 1; 1 ]
  //            /
  //           v
  REQUIRE(ray3.computeProjection(topPoint) == ray3.getOrigin());
  REQUIRE(ray3.computeProjection(topRightPoint) == ray3.getOrigin());
}
<|endoftext|>


<commit_before>#include <stdlib.h>
#include <stdio.h>

#include "simple_paint_util.h"

#include <iup.h>
#include <iupim.h>

static int SplashTimer_CB(Ihandle* timer)
{
  if (IupGetAttribute(timer, "_FIRST_STAGE"))
  {
    IupSetAttribute(timer, "RUN", "NO");
    IupSetAttribute(timer,"TIME", "1000");
    IupSetAttribute(timer, "RUN", "YES");
    IupSetAttribute(timer, "_FIRST_STAGE", NULL);
  }
  else
  {
    Ihandle* dlg = (Ihandle*)IupGetAttribute(timer, "_DIALOG");
    IupSetAttribute(timer, "RUN", "NO");
    IupDestroy(dlg);
    IupDestroy(timer);
  }
  return IUP_DEFAULT;
}

static void SplashShowDlg(const char* image)
{
  Ihandle *dlg, *timer;

  Ihandle* lbl = IupLabel(NULL);
  IupSetStrAttribute(lbl, "IMAGE", image);

  dlg = IupDialog(lbl);

  IupSetAttribute(dlg,"BORDER", "NO");
  IupSetAttribute(dlg,"RESIZE", "NO");
  IupSetAttribute(dlg,"MINBOX", "NO");
  IupSetAttribute(dlg,"MAXBOX", "NO");
  IupSetAttribute(dlg,"MENUBOX", "NO");
  IupSetAttribute(dlg,"TOPMOST", "YES");
  IupSetStrAttribute(dlg, "OPACITYIMAGE", image);

  /* show the splash for 1 second without other windows,
     then start to show the main window. */

  timer = IupTimer();
  IupSetAttribute(timer,"TIME", "1000");
  IupSetCallback(timer, "ACTION_CB", SplashTimer_CB);
  IupSetAttribute(timer, "_DIALOG", (char*)dlg);
  IupSetAttribute(timer, "_FIRST_STAGE", "YES");
  IupSetAttribute(timer, "RUN", "YES");

  IupShowXY(dlg, IUP_CENTER, IUP_CENTER);

  while(IupGetAttribute(timer, "_FIRST_STAGE"))
    IupLoopStep();
}

void SimplePaintSplash(const char* argv0)
{
  /* Look for the image file on the same folder of the executable
     or in the folder above. */
  char* file_path = str_filepath(argv0);
  char filename[10240];
  Ihandle* image;

  if (file_path)
    sprintf(filename, "%s/%s", file_path, "TecgrafLogo.png");
  else
    sprintf(filename, "%s", "TecgrafLogo.png");

  image = IupLoadImage(filename);
  if (!image && file_path)
  {
    char* up_file_path = str_filepath(file_path);
    if (up_file_path)
    {
      sprintf(filename, "%s/%s", up_file_path, "TecgrafLogo.png");

      image = IupLoadImage(filename);
      delete up_file_path;
    }
  }

  if (file_path) delete file_path;

  if (image)
  {
    IupSetAttribute(image, "AUTOSCALE", "1");  /* do not autoscale the splash image */
    IupSetHandle("SPLASH", image);
    SplashShowDlg("SPLASH");
  }
}
<commit_msg><commit_after>#include <stdlib.h>
#include <stdio.h>

#include "simple_paint_util.h"

#include <iup.h>
#include <iupim.h>

static int SplashTimer_CB(Ihandle* timer)
{
  if (IupGetAttribute(timer, "_FIRST_STAGE"))
  {
    IupSetAttribute(timer, "RUN", "NO");
    IupSetAttribute(timer, "_FIRST_STAGE", NULL);

    IupSetAttribute(timer, "TIME", "500");
    IupSetAttribute(timer, "RUN", "YES");
  }
  else
  {
    Ihandle* dlg = (Ihandle*)IupGetAttribute(timer, "_DIALOG");
    IupSetAttribute(timer, "RUN", "NO");
    IupDestroy(dlg);
    IupDestroy(timer);
  }
  return IUP_DEFAULT;
}

static void SplashShowDlg(const char* image)
{
  Ihandle *dlg, *timer;

  Ihandle* lbl = IupLabel(NULL);
  IupSetStrAttribute(lbl, "IMAGE", image);

  dlg = IupDialog(lbl);

  IupSetAttribute(dlg,"BORDER", "NO");
  IupSetAttribute(dlg,"RESIZE", "NO");
  IupSetAttribute(dlg,"MINBOX", "NO");
  IupSetAttribute(dlg,"MAXBOX", "NO");
  IupSetAttribute(dlg,"MENUBOX", "NO");
  IupSetStrAttribute(dlg, "OPACITYIMAGE", image);

  /* show the splash for 1 second without other windows,
     then start to show the main window. */

  timer = IupTimer();
  IupSetAttribute(timer,"TIME", "1000");
  IupSetCallback(timer, "ACTION_CB", SplashTimer_CB);
  IupSetAttribute(timer, "_DIALOG", (char*)dlg);
  IupSetAttribute(timer, "_FIRST_STAGE", "YES");

  IupSetAttribute(timer, "RUN", "YES");

  IupShowXY(dlg, IUP_CENTER, IUP_CENTER);

  while(IupGetAttribute(timer, "_FIRST_STAGE"))
    IupLoopStep();
}

void SimplePaintSplash(const char* argv0)
{
  /* Look for the image file on the same folder of the executable
     or in the folder above. */
  char* file_path = str_filepath(argv0);
  char filename[10240];
  Ihandle* image;

  if (file_path)
    sprintf(filename, "%s/%s", file_path, "TecgrafLogo.png");
  else
    sprintf(filename, "%s", "TecgrafLogo.png");

  image = IupLoadImage(filename);
  if (!image && file_path)
  {
    char* up_file_path = str_filepath(file_path);
    if (up_file_path)
    {
      sprintf(filename, "%s/%s", up_file_path, "TecgrafLogo.png");

      image = IupLoadImage(filename);
      delete up_file_path;
    }
  }

  if (file_path) delete file_path;

  if (image)
  {
    IupSetAttribute(image, "AUTOSCALE", "1");  /* do not autoscale the splash image */
    IupSetHandle("SPLASH", image);
    SplashShowDlg("SPLASH");
  }
}
<|endoftext|>


<commit_before>/**********************************************************************
 *
 * GEOS - Geometry Engine Open Source
 * http://geos.osgeo.org
 *
 * Copyright (C) 2019 Paul Ramsey <pramsey@cleverelephant.ca>
 *
 * This is free software; you can redistribute and/or modify it under
 * the terms of the GNU Lesser General Public Licence as published
 * by the Free Software Foundation.
 * See the COPYING file for more information.
 *
 **********************************************************************/

#include <geos/operation/union/OverlapUnion.h>

#include <geos/geom/Coordinate.h>
#include <geos/geom/CoordinateSequence.h>
#include <geos/geom/CoordinateSequenceFilter.h>
#include <geos/geom/Envelope.h>
#include <geos/geom/GeometryFactory.h>
#include <geos/geom/LineSegment.h>
#include <geos/geom/util/GeometryCombiner.h>
#include <geos/util/TopologyException.h>

namespace geos {
namespace operation {
namespace geounion {

// https://github.com/locationtech/jts/blob/master/modules/core/src/main/java/org/locationtech/jts/operation/union/OverlapUnion.java

using namespace geom;
using namespace geom::util;

/* public */
std::unique_ptr<Geometry>
OverlapUnion::doUnion()
{
    Envelope overlapEnv = overlapEnvelope(g0, g1);
    /*
     * If no overlap, can just combine the geometries
     */
    if (overlapEnv.isNull()) {
        // Geometry* g0Copy = g0->clone().get();
        // Geometry* g1Copy = g1->clone().get();
        return GeometryCombiner::combine(g0, g1);
    }

    std::vector<std::unique_ptr<Geometry>> disjointPolys;

    std::unique_ptr<Geometry> g0Overlap = extractByEnvelope(overlapEnv, g0, disjointPolys);
    std::unique_ptr<Geometry> g1Overlap = extractByEnvelope(overlapEnv, g1, disjointPolys);

    // std::out << "# geoms in common: " << intersectingPolys.size() << std::endl;
    std::unique_ptr<Geometry> theUnion(unionFull(g0Overlap.get(), g1Overlap.get()));
    isUnionSafe = isBorderSegmentsSame(theUnion.get(), overlapEnv);
    if (!isUnionSafe) {
        // overlap union changed border segments... need to do full union
        // std::out <<  "OverlapUnion: Falling back to full union" << std::endl;
        return unionFull(g0, g1);
    }
    else {
        // std::out << "OverlapUnion: fast path" << std::endl;
        return combine(theUnion, disjointPolys);
    }
}

/* private */
Envelope
OverlapUnion::overlapEnvelope(const Geometry* geom0, const Geometry* geom1)
{
    const Envelope* g0Env = geom0->getEnvelopeInternal();
    const Envelope* g1Env = geom1->getEnvelopeInternal();
    Envelope overlapEnv;
    g0Env->intersection(*g1Env, overlapEnv);
    return overlapEnv;
}

/* private */
std::unique_ptr<Geometry>
OverlapUnion::combine(std::unique_ptr<Geometry>& unionGeom, std::vector<std::unique_ptr<Geometry>>& disjointPolys)
{
    if (disjointPolys.empty())
        return std::move(unionGeom);

    disjointPolys.push_back(std::move(unionGeom));
    return GeometryCombiner::combine(std::move(disjointPolys));
}

/* private */
std::unique_ptr<Geometry>
OverlapUnion::extractByEnvelope(const Envelope& env, const Geometry* geom, std::vector<std::unique_ptr<Geometry>>& disjointGeoms)
{
    std::vector<const Geometry*> intersectingGeoms;
    for (std::size_t i = 0; i < geom->getNumGeometries(); i++) {
        const Geometry* elem = geom->getGeometryN(i);
        if (elem->getEnvelopeInternal()->intersects(env)) {
            intersectingGeoms.push_back(elem);
        }
        else {
            disjointGeoms.push_back(elem->clone());
        }
    }
    return std::unique_ptr<Geometry>(geomFactory->buildGeometry(intersectingGeoms));
}

/* private */
std::unique_ptr<Geometry>
OverlapUnion::unionFull(const Geometry* geom0, const Geometry* geom1)
{
    // try {
    //     return geom0->Union(geom1);
    // }
    // catch (geos::util::TopologyException &) {
    //      // If the overlay union fails,
    //      // try a buffer union, which often succeeds
    //     return unionBuffer(geom0, geom1);
    // }
    if (geom0->getNumGeometries() == 0 && geom1->getNumGeometries() == 0) {
        return geom0->clone();
    }

    return unionFunction->Union(geom0, geom1);
}

/* private */
std::unique_ptr<Geometry>
OverlapUnion::unionBuffer(const Geometry* geom0, const Geometry* geom1)
{
    const GeometryFactory* factory = geom0->getFactory();
    std::unique_ptr<Geometry> copy0 = geom0->clone();
    std::unique_ptr<Geometry> copy1 = geom1->clone();
    std::vector<std::unique_ptr<Geometry>> geoms;
    geoms.push_back(std::move(copy0));
    geoms.push_back(std::move(copy1));
    std::unique_ptr<GeometryCollection> gColl(factory->createGeometryCollection(std::move(geoms)));
    return gColl->buffer(0.0);
}

/* private */
bool
OverlapUnion::isBorderSegmentsSame(const Geometry* result, const Envelope& env)
{
    std::vector<LineSegment> segsBefore = extractBorderSegments(g0, g1, env);
    std::vector<LineSegment> segsAfter;
    extractBorderSegments(result, env, segsAfter);
    bool eq = isEqual(segsBefore, segsAfter);

    return eq;
}

static bool lineSegmentPtrCmp(const LineSegment& a, const LineSegment& b)
{
    return a.compareTo(b) < 0;
}

/* private */
bool
OverlapUnion::isEqual(std::vector<LineSegment>& segs0, std::vector<LineSegment>& segs1)
{
    if (segs0.size() != segs1.size())
        return false;

    std::sort(segs0.begin(), segs0.end(), lineSegmentPtrCmp);
    std::sort(segs1.begin(), segs1.end(), lineSegmentPtrCmp);

    std::size_t sz = segs0.size();
    for (std::size_t i = 0; i < sz; i++) {
        if (segs0[i].p0.x != segs1[i].p0.x ||
            segs0[i].p0.y != segs1[i].p0.y ||
            segs0[i].p1.x != segs1[i].p1.x ||
            segs0[i].p1.y != segs1[i].p1.y)
        {
            return false;
        }
    }

    return true;
}

/* private */
std::vector<LineSegment>
OverlapUnion::extractBorderSegments(const Geometry* geom0, const Geometry* geom1, const Envelope& env)
{
    std::vector<LineSegment> segs;
    extractBorderSegments(geom0, env, segs);
    if (geom1 != nullptr)
        extractBorderSegments(geom1, env, segs);
    return segs;
}

/* static */
static bool
intersects(const Envelope& env, const Coordinate& p0, const Coordinate& p1)
{
    return env.intersects(p0) || env.intersects(p1);
}

/* static */
static bool
containsProperly(const Envelope& env, const Coordinate& p)
{
    if (env.isNull()) return false;
    return p.x > env.getMinX() &&
           p.x < env.getMaxX() &&
           p.y > env.getMinY() &&
           p.y < env.getMaxY();
}

/* static */
static bool
containsProperly(const Envelope& env, const Coordinate& p0, const Coordinate& p1)
{
    return containsProperly(env, p0) && containsProperly(env, p1);
}

/* private */
void
OverlapUnion::extractBorderSegments(const Geometry* geom, const Envelope& penv, std::vector<LineSegment>& psegs)
{
    class BorderSegmentFilter : public CoordinateSequenceFilter {

    private:
        const Envelope m_env;
        std::vector<LineSegment>* m_segs;

    public:

        BorderSegmentFilter(const Envelope& env, std::vector<LineSegment>* segs)
            : m_env(env),
              m_segs(segs) {};

        bool
        isDone() const override { return false; }

        bool
        isGeometryChanged() const override  { return false; }

        void
        filter_ro(const CoordinateSequence& seq, std::size_t i) override
        {
            if (i <= 0) return;

            // extract LineSegment
            const Coordinate& p0 = seq.getAt(i-1);
            const Coordinate& p1 = seq.getAt(i  );
            bool isBorder = intersects(m_env, p0, p1) && ! containsProperly(m_env, p0, p1);
            if (isBorder) {
                m_segs->emplace_back(p0, p1);
            }
        };

    };

    BorderSegmentFilter bsf(penv, &psegs);
    geom->apply_ro(bsf);

}

}
}
}
<commit_msg>Do not test unsigned variable against being negative<commit_after>/**********************************************************************
 *
 * GEOS - Geometry Engine Open Source
 * http://geos.osgeo.org
 *
 * Copyright (C) 2019 Paul Ramsey <pramsey@cleverelephant.ca>
 *
 * This is free software; you can redistribute and/or modify it under
 * the terms of the GNU Lesser General Public Licence as published
 * by the Free Software Foundation.
 * See the COPYING file for more information.
 *
 **********************************************************************/

#include <geos/operation/union/OverlapUnion.h>

#include <geos/geom/Coordinate.h>
#include <geos/geom/CoordinateSequence.h>
#include <geos/geom/CoordinateSequenceFilter.h>
#include <geos/geom/Envelope.h>
#include <geos/geom/GeometryFactory.h>
#include <geos/geom/LineSegment.h>
#include <geos/geom/util/GeometryCombiner.h>
#include <geos/util/TopologyException.h>

namespace geos {
namespace operation {
namespace geounion {

// https://github.com/locationtech/jts/blob/master/modules/core/src/main/java/org/locationtech/jts/operation/union/OverlapUnion.java

using namespace geom;
using namespace geom::util;

/* public */
std::unique_ptr<Geometry>
OverlapUnion::doUnion()
{
    Envelope overlapEnv = overlapEnvelope(g0, g1);
    /*
     * If no overlap, can just combine the geometries
     */
    if (overlapEnv.isNull()) {
        // Geometry* g0Copy = g0->clone().get();
        // Geometry* g1Copy = g1->clone().get();
        return GeometryCombiner::combine(g0, g1);
    }

    std::vector<std::unique_ptr<Geometry>> disjointPolys;

    std::unique_ptr<Geometry> g0Overlap = extractByEnvelope(overlapEnv, g0, disjointPolys);
    std::unique_ptr<Geometry> g1Overlap = extractByEnvelope(overlapEnv, g1, disjointPolys);

    // std::out << "# geoms in common: " << intersectingPolys.size() << std::endl;
    std::unique_ptr<Geometry> theUnion(unionFull(g0Overlap.get(), g1Overlap.get()));
    isUnionSafe = isBorderSegmentsSame(theUnion.get(), overlapEnv);
    if (!isUnionSafe) {
        // overlap union changed border segments... need to do full union
        // std::out <<  "OverlapUnion: Falling back to full union" << std::endl;
        return unionFull(g0, g1);
    }
    else {
        // std::out << "OverlapUnion: fast path" << std::endl;
        return combine(theUnion, disjointPolys);
    }
}

/* private */
Envelope
OverlapUnion::overlapEnvelope(const Geometry* geom0, const Geometry* geom1)
{
    const Envelope* g0Env = geom0->getEnvelopeInternal();
    const Envelope* g1Env = geom1->getEnvelopeInternal();
    Envelope overlapEnv;
    g0Env->intersection(*g1Env, overlapEnv);
    return overlapEnv;
}

/* private */
std::unique_ptr<Geometry>
OverlapUnion::combine(std::unique_ptr<Geometry>& unionGeom, std::vector<std::unique_ptr<Geometry>>& disjointPolys)
{
    if (disjointPolys.empty())
        return std::move(unionGeom);

    disjointPolys.push_back(std::move(unionGeom));
    return GeometryCombiner::combine(std::move(disjointPolys));
}

/* private */
std::unique_ptr<Geometry>
OverlapUnion::extractByEnvelope(const Envelope& env, const Geometry* geom, std::vector<std::unique_ptr<Geometry>>& disjointGeoms)
{
    std::vector<const Geometry*> intersectingGeoms;
    for (std::size_t i = 0; i < geom->getNumGeometries(); i++) {
        const Geometry* elem = geom->getGeometryN(i);
        if (elem->getEnvelopeInternal()->intersects(env)) {
            intersectingGeoms.push_back(elem);
        }
        else {
            disjointGeoms.push_back(elem->clone());
        }
    }
    return std::unique_ptr<Geometry>(geomFactory->buildGeometry(intersectingGeoms));
}

/* private */
std::unique_ptr<Geometry>
OverlapUnion::unionFull(const Geometry* geom0, const Geometry* geom1)
{
    // try {
    //     return geom0->Union(geom1);
    // }
    // catch (geos::util::TopologyException &) {
    //      // If the overlay union fails,
    //      // try a buffer union, which often succeeds
    //     return unionBuffer(geom0, geom1);
    // }
    if (geom0->getNumGeometries() == 0 && geom1->getNumGeometries() == 0) {
        return geom0->clone();
    }

    return unionFunction->Union(geom0, geom1);
}

/* private */
std::unique_ptr<Geometry>
OverlapUnion::unionBuffer(const Geometry* geom0, const Geometry* geom1)
{
    const GeometryFactory* factory = geom0->getFactory();
    std::unique_ptr<Geometry> copy0 = geom0->clone();
    std::unique_ptr<Geometry> copy1 = geom1->clone();
    std::vector<std::unique_ptr<Geometry>> geoms;
    geoms.push_back(std::move(copy0));
    geoms.push_back(std::move(copy1));
    std::unique_ptr<GeometryCollection> gColl(factory->createGeometryCollection(std::move(geoms)));
    return gColl->buffer(0.0);
}

/* private */
bool
OverlapUnion::isBorderSegmentsSame(const Geometry* result, const Envelope& env)
{
    std::vector<LineSegment> segsBefore = extractBorderSegments(g0, g1, env);
    std::vector<LineSegment> segsAfter;
    extractBorderSegments(result, env, segsAfter);
    bool eq = isEqual(segsBefore, segsAfter);

    return eq;
}

static bool lineSegmentPtrCmp(const LineSegment& a, const LineSegment& b)
{
    return a.compareTo(b) < 0;
}

/* private */
bool
OverlapUnion::isEqual(std::vector<LineSegment>& segs0, std::vector<LineSegment>& segs1)
{
    if (segs0.size() != segs1.size())
        return false;

    std::sort(segs0.begin(), segs0.end(), lineSegmentPtrCmp);
    std::sort(segs1.begin(), segs1.end(), lineSegmentPtrCmp);

    std::size_t sz = segs0.size();
    for (std::size_t i = 0; i < sz; i++) {
        if (segs0[i].p0.x != segs1[i].p0.x ||
            segs0[i].p0.y != segs1[i].p0.y ||
            segs0[i].p1.x != segs1[i].p1.x ||
            segs0[i].p1.y != segs1[i].p1.y)
        {
            return false;
        }
    }

    return true;
}

/* private */
std::vector<LineSegment>
OverlapUnion::extractBorderSegments(const Geometry* geom0, const Geometry* geom1, const Envelope& env)
{
    std::vector<LineSegment> segs;
    extractBorderSegments(geom0, env, segs);
    if (geom1 != nullptr)
        extractBorderSegments(geom1, env, segs);
    return segs;
}

/* static */
static bool
intersects(const Envelope& env, const Coordinate& p0, const Coordinate& p1)
{
    return env.intersects(p0) || env.intersects(p1);
}

/* static */
static bool
containsProperly(const Envelope& env, const Coordinate& p)
{
    if (env.isNull()) return false;
    return p.x > env.getMinX() &&
           p.x < env.getMaxX() &&
           p.y > env.getMinY() &&
           p.y < env.getMaxY();
}

/* static */
static bool
containsProperly(const Envelope& env, const Coordinate& p0, const Coordinate& p1)
{
    return containsProperly(env, p0) && containsProperly(env, p1);
}

/* private */
void
OverlapUnion::extractBorderSegments(const Geometry* geom, const Envelope& penv, std::vector<LineSegment>& psegs)
{
    class BorderSegmentFilter : public CoordinateSequenceFilter {

    private:
        const Envelope m_env;
        std::vector<LineSegment>* m_segs;

    public:

        BorderSegmentFilter(const Envelope& env, std::vector<LineSegment>* segs)
            : m_env(env),
              m_segs(segs) {};

        bool
        isDone() const override { return false; }

        bool
        isGeometryChanged() const override  { return false; }

        void
        filter_ro(const CoordinateSequence& seq, std::size_t i) override
        {
            if (i == 0) return;

            // extract LineSegment
            const Coordinate& p0 = seq.getAt(i-1);
            const Coordinate& p1 = seq.getAt(i  );
            bool isBorder = intersects(m_env, p0, p1) && ! containsProperly(m_env, p0, p1);
            if (isBorder) {
                m_segs->emplace_back(p0, p1);
            }
        };

    };

    BorderSegmentFilter bsf(penv, &psegs);
    geom->apply_ro(bsf);

}

}
}
}
<|endoftext|>


<commit_before>/*******************************************************************************
 * Copyright (c) 2013, Art Clarke.  All rights reserved.
 *
 * This file is part of Humble-Video.
 *
 * Humble-Video is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Humble-Video is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with Humble-Video.  If not, see <http://www.gnu.org/licenses/>.
 *******************************************************************************/
/*
 * Encoder.cpp
 *
 *  Created on: Jul 23, 2013
 *      Author: aclarke
 */

#include "Encoder.h"
#include <io/humble/ferry/Logger.h>
#include <io/humble/ferry/HumbleException.h>
#include <io/humble/video/MediaPacketImpl.h>

using namespace io::humble::ferry;

VS_LOG_SETUP(VS_CPP_PACKAGE);

namespace io {
namespace humble {
namespace
video {

#if 0
int32_t
Encoder::acquire()
{
  VS_LOG_DEBUG("encoder acquire: %p", this);
  return RefCounted::acquire();
}
int32_t
Encoder::release()
{
  VS_LOG_DEBUG("encoder release: %p", this);
  return RefCounted::release();
}
#endif // 0

Encoder::Encoder(Codec* codec, AVCodecContext* src, bool copySrc) : Coder(codec, src, copySrc) {
  if (!codec)
    throw HumbleInvalidArgument("no codec passed in");

  if (!codec->canEncode())
    throw HumbleInvalidArgument("passed in codec cannot encode");

  mNumDroppedFrames = 0;
  mLastPtsEncoded = Global::NO_PTS;

  VS_LOG_TRACE("Created encoder");
}

Encoder::~Encoder() {
}

Encoder*
Encoder::make(Codec* codec)
{
  RefPointer<Encoder> r;
  if (!codec) {
    VS_THROW(HumbleInvalidArgument("no codec passed in"));
  }

  if (!codec->canEncode()) {
    VS_THROW(HumbleInvalidArgument("passed in codec cannot encode"));
  }

  r.reset(new Encoder(codec, 0, false), true);

  return r.get();
}

Encoder*
Encoder::make(Coder* src)
{
  if (!src) {
    VS_THROW(HumbleInvalidArgument("no coder to copy"));
  }

  RefPointer<Encoder> r;
  RefPointer<Codec> c = src->getCodec();
  if (!c) {
    VS_THROW(HumbleRuntimeError("coder has no codec"));
  }
  if (!c->canEncode()) {
    // this codec cannot encode, so we try to find a new codec that can
    // of the same type.
    Codec::ID id = c->getID();
    c = Codec::findEncodingCodec(id);
    if (!c) {
      RefPointer<CodecDescriptor> cd = CodecDescriptor::make(id);
      VS_THROW(HumbleRuntimeError::make("cannot find encoder for id: %s", cd ? cd->getName() : "unknown"));
    }
  }
  r.reset(new Encoder(c.value(), src->getCodecCtx(), true), true);
  return r.get();
}

Encoder*
Encoder::make(Codec* codec, AVCodecContext* src) {
  if (!src)
    throw HumbleInvalidArgument("no Encoder to copy");
  if (!src->codec_id)
    throw HumbleRuntimeError("No codec set on coder???");

  RefPointer<Encoder> retval;
  // new Encoder DOES NOT increment refcount but the reset should catch it.
  retval.reset(new Encoder(codec, src, false), true);
  return retval.get();
}

void
Encoder::open(KeyValueBag * inputOptions, KeyValueBag* unsetOptions) {
  Coder::open(inputOptions, unsetOptions);
  // now check to see if we require an audio resampler.
  RefPointer<Codec> codec = getCodec();
  if (codec->getType() == MediaDescriptor::MEDIA_AUDIO &&
      !(codec->getCapabilities() & Codec::CAP_VARIABLE_FRAME_SIZE)) {
    // yes, we do. Let's create one. This looks odd, as it's converting
    // the same params into the same params, but it's really just being
    // used as an audio buffer for us.
    mAResampler = MediaAudioResampler::make(
        getChannelLayout(),
        getSampleRate(),
        getSampleFormat(),
        getChannelLayout(),
        getSampleRate(),
        getSampleFormat()
        );
    mAResampler->open();
  }
}
void
Encoder::encodeVideo(MediaPacket* aOutput, MediaPicture* frame) {
  MediaPacketImpl* output = dynamic_cast<MediaPacketImpl*>(aOutput);

  if (getState() != STATE_OPENED) {
    VS_THROW(HumbleRuntimeError("Cannot encode with unopened encoder"));
  }
  if (getCodecType() != MediaDescriptor::MEDIA_VIDEO) {
    VS_THROW(HumbleRuntimeError("Attempting to encode picture on non-video encoder"));
  }
  if (!output) {
    VS_THROW(HumbleInvalidArgument("output cannot be null"));
  }
  // let's check the picture parameters.
  ensurePictureParamsMatch(frame);

  if (frame && !frame->isComplete()) {
    VS_THROW(HumbleInvalidArgument("Can only pass complete media to encode"));
  }

  if (frame && frame->getPts() == Global::NO_PTS) {
    VS_THROW(HumbleInvalidArgument("Passed in media must have a valid time stamp"));
  }

  /**
   * Ah, this is naive. Here's what we actually need to do:
   *
   * 1) Copy the video frame into a new frame since some codecs will
   * need byte aligned frames (and if the frame came from a Java user, we
   * can't be guaranteed it'll be aligned on the right boundaries).
   *
   * 2) check time stamps and drop frames when needed if the time
   * base of the input frame cannot be converted to the encoder
   * time base without rounding.
   *
   */
  AVFrame* in = frame ? frame->getCtx() : 0;
  AVPacket* out = output->getCtx();

  RefPointer<Rational> coderTb = this->getTimeBase();

  if (in) {
    RefPointer<Rational> frameTb = frame->getTimeBase();
    bool dropFrame = false;

    int64_t inTs = coderTb->rescale(in->pts, frameTb.value());
    if (mLastPtsEncoded != Global::NO_PTS) {
      if (inTs < mLastPtsEncoded) {
        VS_LOG_DEBUG(
            "Dropping frame with timestamp %lld (if coder supports higher time-base use that instead)",
            frame->getPts());
        dropFrame = true;
      }
    } else {
      if (!dropFrame)
        mLastPtsEncoded = inTs;
    }
  }
  int got_frame = 0;

  // set the packet to be the max size if can be
  output->setComplete(false, 0);
  if (out->buf && out->data)
    out->size = out->buf->size;
  else
    out->size = 0;

  int e = avcodec_encode_video2(getCodecCtx(), out, in, &got_frame);
  if (got_frame) {
    output->setTimeBase(coderTb.value());
    output->setComplete(out->size > 0, out->size);
  }
  FfmpegException::check(e, "could not encode video ");
}

void
Encoder::encode(MediaPacket* output, MediaSampled* media) {
  MediaDescriptor::Type type = getCodecType();
  switch(type) {
  case MediaDescriptor::MEDIA_AUDIO: {
    MediaAudio* audio = dynamic_cast<MediaAudio*>(media);
    if (!audio && media)
      VS_THROW(HumbleInvalidArgument("passed non-audio Media to an audio encoder"));
    encodeAudio(output, audio);
  }
  break;
  case MediaDescriptor::MEDIA_VIDEO: {
    MediaPicture* picture = dynamic_cast<MediaPicture*>(media);
    if (!picture && media)
      VS_THROW(HumbleInvalidArgument("passed non-video Media to an video encoder"));
    encodeVideo(output, picture);
  }
  break;
  default:
    VS_THROW(HumbleInvalidArgument("passed a media type that is not compatible with this encoder"));
  }
}
void
Encoder::encodeAudio(MediaPacket* aOutput, MediaAudio* samples) {
  MediaPacketImpl* output = dynamic_cast<MediaPacketImpl*>(aOutput);

  if (getState() != STATE_OPENED) {
    VS_THROW(HumbleRuntimeError("Cannot encode with unopened encoder"));
  }
  if (getCodecType() != MediaDescriptor::MEDIA_AUDIO) {
    VS_THROW(HumbleRuntimeError("Attempting to encode audio on non-audio encoder"));
  }
  if (!output) {
    VS_THROW(HumbleInvalidArgument("output cannot be null"));
  }
  // let's check the audio parameters.
  ensureAudioParamsMatch(samples);

  if (samples && !samples->isComplete()) {
    VS_THROW(HumbleInvalidArgument("Can only pass complete media to encode"));
  }

  RefPointer<Codec> codec = getCodec();
  RefPointer<MediaAudio> inputAudio;

  inputAudio.reset(samples, true);
  if (!(codec->getCapabilities() & Codec::CAP_VARIABLE_FRAME_SIZE)) {
    // this codec requires that the right number of audio samples
    // gets passed in each call.
    if (!mResampledAudio) {
      if (samples)
        mResampledAudio = MediaAudio::make(getFrameSize(),
            samples->getSampleRate(), samples->getChannels(),
            samples->getChannelLayout(), samples->getFormat());
    }
    if (mResampledAudio) {
      mAResampler->resample(mResampledAudio.value(), samples);
      inputAudio = mResampledAudio.get();
    }
  }
  RefPointer<Rational> coderTb = this->getTimeBase();

  AVFrame* in = inputAudio ? inputAudio->getCtx() : 0;
  AVPacket* out = output->getCtx();

  int got_frame = 0;
  int e = avcodec_encode_audio2(getCodecCtx(), out, in, &got_frame);
  if (got_frame) {
    output->setTimeBase(coderTb.value());
    output->setComplete(true, out->size);
  } else {
    output->setComplete(false, 0);
  }

  FfmpegException::check(e, "could not encode audio ");
}

} /* namespace video */
} /* namespace humble */
} /* namespace io */
<commit_msg>fix bug where encoders sometimes need larger packets by reseting the output packet each time through the loop. This will result in the Ffmpeg encode allocating a new packet of the correct size.<commit_after>/*******************************************************************************
 * Copyright (c) 2013, Art Clarke.  All rights reserved.
 *
 * This file is part of Humble-Video.
 *
 * Humble-Video is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Humble-Video is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with Humble-Video.  If not, see <http://www.gnu.org/licenses/>.
 *******************************************************************************/
/*
 * Encoder.cpp
 *
 *  Created on: Jul 23, 2013
 *      Author: aclarke
 */

#include "Encoder.h"
#include <io/humble/ferry/Logger.h>
#include <io/humble/ferry/HumbleException.h>
#include <io/humble/video/MediaPacketImpl.h>

using namespace io::humble::ferry;

VS_LOG_SETUP(VS_CPP_PACKAGE);

namespace io {
namespace humble {
namespace
video {

#if 0
int32_t
Encoder::acquire()
{
  VS_LOG_DEBUG("encoder acquire: %p", this);
  return RefCounted::acquire();
}
int32_t
Encoder::release()
{
  VS_LOG_DEBUG("encoder release: %p", this);
  return RefCounted::release();
}
#endif // 0

Encoder::Encoder(Codec* codec, AVCodecContext* src, bool copySrc) : Coder(codec, src, copySrc) {
  if (!codec)
    throw HumbleInvalidArgument("no codec passed in");

  if (!codec->canEncode())
    throw HumbleInvalidArgument("passed in codec cannot encode");

  mNumDroppedFrames = 0;
  mLastPtsEncoded = Global::NO_PTS;

  VS_LOG_TRACE("Created encoder");
}

Encoder::~Encoder() {
}

Encoder*
Encoder::make(Codec* codec)
{
  RefPointer<Encoder> r;
  if (!codec) {
    VS_THROW(HumbleInvalidArgument("no codec passed in"));
  }

  if (!codec->canEncode()) {
    VS_THROW(HumbleInvalidArgument("passed in codec cannot encode"));
  }

  r.reset(new Encoder(codec, 0, false), true);

  return r.get();
}

Encoder*
Encoder::make(Coder* src)
{
  if (!src) {
    VS_THROW(HumbleInvalidArgument("no coder to copy"));
  }

  RefPointer<Encoder> r;
  RefPointer<Codec> c = src->getCodec();
  if (!c) {
    VS_THROW(HumbleRuntimeError("coder has no codec"));
  }
  if (!c->canEncode()) {
    // this codec cannot encode, so we try to find a new codec that can
    // of the same type.
    Codec::ID id = c->getID();
    c = Codec::findEncodingCodec(id);
    if (!c) {
      RefPointer<CodecDescriptor> cd = CodecDescriptor::make(id);
      VS_THROW(HumbleRuntimeError::make("cannot find encoder for id: %s", cd ? cd->getName() : "unknown"));
    }
  }
  r.reset(new Encoder(c.value(), src->getCodecCtx(), true), true);
  return r.get();
}

Encoder*
Encoder::make(Codec* codec, AVCodecContext* src) {
  if (!src)
    throw HumbleInvalidArgument("no Encoder to copy");
  if (!src->codec_id)
    throw HumbleRuntimeError("No codec set on coder???");

  RefPointer<Encoder> retval;
  // new Encoder DOES NOT increment refcount but the reset should catch it.
  retval.reset(new Encoder(codec, src, false), true);
  return retval.get();
}

void
Encoder::open(KeyValueBag * inputOptions, KeyValueBag* unsetOptions) {
  Coder::open(inputOptions, unsetOptions);
  // now check to see if we require an audio resampler.
  RefPointer<Codec> codec = getCodec();
  if (codec->getType() == MediaDescriptor::MEDIA_AUDIO &&
      !(codec->getCapabilities() & Codec::CAP_VARIABLE_FRAME_SIZE)) {
    // yes, we do. Let's create one. This looks odd, as it's converting
    // the same params into the same params, but it's really just being
    // used as an audio buffer for us.
    mAResampler = MediaAudioResampler::make(
        getChannelLayout(),
        getSampleRate(),
        getSampleFormat(),
        getChannelLayout(),
        getSampleRate(),
        getSampleFormat()
        );
    mAResampler->open();
  }
}
void
Encoder::encodeVideo(MediaPacket* aOutput, MediaPicture* frame) {
  MediaPacketImpl* output = dynamic_cast<MediaPacketImpl*>(aOutput);

  if (getState() != STATE_OPENED) {
    VS_THROW(HumbleRuntimeError("Cannot encode with unopened encoder"));
  }
  if (getCodecType() != MediaDescriptor::MEDIA_VIDEO) {
    VS_THROW(HumbleRuntimeError("Attempting to encode picture on non-video encoder"));
  }
  if (!output) {
    VS_THROW(HumbleInvalidArgument("output cannot be null"));
  }
  // now reset the packet so we allocate new memory (because encoders sometimes change packet sizes).
  output->reset(0);

  // let's check the picture parameters.
  ensurePictureParamsMatch(frame);

  if (frame && !frame->isComplete()) {
    VS_THROW(HumbleInvalidArgument("Can only pass complete media to encode"));
  }

  if (frame && frame->getPts() == Global::NO_PTS) {
    VS_THROW(HumbleInvalidArgument("Passed in media must have a valid time stamp"));
  }

  /**
   * Ah, this is naive. Here's what we actually need to do:
   *
   * 1) Copy the video frame into a new frame since some codecs will
   * need byte aligned frames (and if the frame came from a Java user, we
   * can't be guaranteed it'll be aligned on the right boundaries).
   *
   * 2) check time stamps and drop frames when needed if the time
   * base of the input frame cannot be converted to the encoder
   * time base without rounding.
   *
   */
  AVFrame* in = frame ? frame->getCtx() : 0;
  AVPacket* out = output->getCtx();

  RefPointer<Rational> coderTb = this->getTimeBase();

  if (in) {
    RefPointer<Rational> frameTb = frame->getTimeBase();
    bool dropFrame = false;

    int64_t inTs = coderTb->rescale(in->pts, frameTb.value());
    if (mLastPtsEncoded != Global::NO_PTS) {
      if (inTs < mLastPtsEncoded) {
        VS_LOG_DEBUG(
            "Dropping frame with timestamp %lld (if coder supports higher time-base use that instead)",
            frame->getPts());
        dropFrame = true;
      }
    } else {
      if (!dropFrame)
        mLastPtsEncoded = inTs;
    }
  }
  int got_frame = 0;

  // set the packet to be the max size if can be
  output->setComplete(false, 0);
  if (out->buf && out->data)
    out->size = out->buf->size;
  else
    out->size = 0;

  int e = avcodec_encode_video2(getCodecCtx(), out, in, &got_frame);
  if (got_frame) {
    output->setTimeBase(coderTb.value());
    output->setComplete(out->size > 0, out->size);
  }
  FfmpegException::check(e, "could not encode video ");
}

void
Encoder::encode(MediaPacket* output, MediaSampled* media) {
  MediaDescriptor::Type type = getCodecType();
  switch(type) {
  case MediaDescriptor::MEDIA_AUDIO: {
    MediaAudio* audio = dynamic_cast<MediaAudio*>(media);
    if (!audio && media)
      VS_THROW(HumbleInvalidArgument("passed non-audio Media to an audio encoder"));
    encodeAudio(output, audio);
  }
  break;
  case MediaDescriptor::MEDIA_VIDEO: {
    MediaPicture* picture = dynamic_cast<MediaPicture*>(media);
    if (!picture && media)
      VS_THROW(HumbleInvalidArgument("passed non-video Media to an video encoder"));
    encodeVideo(output, picture);
  }
  break;
  default:
    VS_THROW(HumbleInvalidArgument("passed a media type that is not compatible with this encoder"));
  }
}
void
Encoder::encodeAudio(MediaPacket* aOutput, MediaAudio* samples) {
  MediaPacketImpl* output = dynamic_cast<MediaPacketImpl*>(aOutput);

  if (getState() != STATE_OPENED) {
    VS_THROW(HumbleRuntimeError("Cannot encode with unopened encoder"));
  }
  if (getCodecType() != MediaDescriptor::MEDIA_AUDIO) {
    VS_THROW(HumbleRuntimeError("Attempting to encode audio on non-audio encoder"));
  }
  if (!output) {
    VS_THROW(HumbleInvalidArgument("output cannot be null"));
  }
  // now reset the packet so we allocate new memory (because encoders sometimes change packet sizes).
  output->reset(0);

  // let's check the audio parameters.
  ensureAudioParamsMatch(samples);

  if (samples && !samples->isComplete()) {
    VS_THROW(HumbleInvalidArgument("Can only pass complete media to encode"));
  }

  RefPointer<Codec> codec = getCodec();
  RefPointer<MediaAudio> inputAudio;

  inputAudio.reset(samples, true);
  if (!(codec->getCapabilities() & Codec::CAP_VARIABLE_FRAME_SIZE)) {
    // this codec requires that the right number of audio samples
    // gets passed in each call.
    if (!mResampledAudio) {
      if (samples)
        mResampledAudio = MediaAudio::make(getFrameSize(),
            samples->getSampleRate(), samples->getChannels(),
            samples->getChannelLayout(), samples->getFormat());
    }
    if (mResampledAudio) {
      mAResampler->resample(mResampledAudio.value(), samples);
      inputAudio = mResampledAudio.get();
    }
  }
  RefPointer<Rational> coderTb = this->getTimeBase();

  AVFrame* in = inputAudio ? inputAudio->getCtx() : 0;
  AVPacket* out = output->getCtx();

  int got_frame = 0;
  int e = avcodec_encode_audio2(getCodecCtx(), out, in, &got_frame);
  if (got_frame) {
    output->setTimeBase(coderTb.value());
    output->setComplete(true, out->size);
  } else {
    output->setComplete(false, 0);
  }

  FfmpegException::check(e, "could not encode audio ");
}

} /* namespace video */
} /* namespace humble */
} /* namespace io */
<|endoftext|>


<commit_before>//
//  LayerTilesRenderParameters.cpp
//  G3MiOSSDK
//
//  Created by Diego Gomez Deck on 2/25/13.
//
//

#include "LayerTilesRenderParameters.hpp"

/*
 return ( topSectorSplitsByLatitude, topSectorSplitsByLongitude )
 */
const Vector2I LayerTilesRenderParameters::calculateTopSectorSplitsParametersWGS84(const Sector& topSector) {
  IMathUtils* math = IMathUtils::instance();
  const double maxTile = 90;
  double sLat;
  double sLon;

  const double ratio = topSector._deltaLatitude.div(topSector._deltaLongitude);
  if (ratio > 1) {
    sLat = ratio;
    sLon = 1;
  }
  else {
    sLat = 1;
    sLon = 1 / ratio;
  }

  const double tileDeltaLat = topSector._deltaLatitude.div(sLat)._degrees;
  const double factorLat = tileDeltaLat / maxTile;
  double factor = math->max(factorLat, 1L);

  return Vector2I((int) math->round(sLat * factor), (int) math->round(sLon * factor));
}

bool LayerTilesRenderParameters::isEquals(const LayerTilesRenderParameters* that) const {
  if (that == NULL) {
    return false;
  }

  if (!(_topSector.isEquals(that->_topSector))) {
    return false;
  }

  if (_topSectorSplitsByLatitude != that->_topSectorSplitsByLatitude) {
    return false;
  }

  if (_topSectorSplitsByLongitude != that->_topSectorSplitsByLongitude) {
    return false;
  }

  if (_firstLevel != that->_firstLevel) {
    return false;
  }

  if (_maxLevel != that->_maxLevel) {
    return false;
  }

  if (_maxLevelForPoles != that->_maxLevelForPoles) {
    return false;
  }

  if (!_tileTextureResolution.isEquals(that->_tileTextureResolution)) {
    return false;
  }

  if (!_tileMeshResolution.isEquals(that->_tileMeshResolution)) {
    return false;
  }

  if (_mercator != that->_mercator) {
    return false;
  }

  return true;
}

LayerTilesRenderParameters* LayerTilesRenderParameters::copy() const {
  return new LayerTilesRenderParameters(_topSector,
                                        _topSectorSplitsByLatitude,
                                        _topSectorSplitsByLongitude,
                                        _firstLevel,
                                        _maxLevel,
                                        _tileTextureResolution,
                                        _tileMeshResolution,
                                        _mercator);
}
<commit_msg>Tile's image creation refactoring - NOT YET USABLE<commit_after>//
//  LayerTilesRenderParameters.cpp
//  G3MiOSSDK
//
//  Created by Diego Gomez Deck on 2/25/13.
//
//

#include "LayerTilesRenderParameters.hpp"

/*
 return ( topSectorSplitsByLatitude, topSectorSplitsByLongitude )
 */
const Vector2I LayerTilesRenderParameters::calculateTopSectorSplitsParametersWGS84(const Sector& topSector) {
//  IMathUtils* math = IMathUtils::instance();
  const double maxTile = 90;
  double sLat;
  double sLon;

  const double ratio = topSector._deltaLatitude.div(topSector._deltaLongitude);
  if (ratio > 1) {
    sLat = ratio;
    sLon = 1;
  }
  else {
    sLat = 1;
    sLon = 1 / ratio;
  }

  const double tileDeltaLat = topSector._deltaLatitude.div(sLat)._degrees;
  const double factorLat = tileDeltaLat / maxTile;
//  double factor = math->max(factorLat, 1L);
//  return Vector2I((int) math->round(sLat * factor), (int) math->round(sLon * factor));

  const double factor = (factorLat < 1) ? 1 : factorLat;
  return Vector2I((int) ((sLat * factor) + 0.5),
                  (int) ((sLon * factor) + 0.5));
}

bool LayerTilesRenderParameters::isEquals(const LayerTilesRenderParameters* that) const {
  if (that == NULL) {
    return false;
  }

  if (!(_topSector.isEquals(that->_topSector))) {
    return false;
  }

  if (_topSectorSplitsByLatitude != that->_topSectorSplitsByLatitude) {
    return false;
  }

  if (_topSectorSplitsByLongitude != that->_topSectorSplitsByLongitude) {
    return false;
  }

  if (_firstLevel != that->_firstLevel) {
    return false;
  }

  if (_maxLevel != that->_maxLevel) {
    return false;
  }

  if (_maxLevelForPoles != that->_maxLevelForPoles) {
    return false;
  }

  if (!_tileTextureResolution.isEquals(that->_tileTextureResolution)) {
    return false;
  }

  if (!_tileMeshResolution.isEquals(that->_tileMeshResolution)) {
    return false;
  }

  if (_mercator != that->_mercator) {
    return false;
  }

  return true;
}

LayerTilesRenderParameters* LayerTilesRenderParameters::copy() const {
  return new LayerTilesRenderParameters(_topSector,
                                        _topSectorSplitsByLatitude,
                                        _topSectorSplitsByLongitude,
                                        _firstLevel,
                                        _maxLevel,
                                        _tileTextureResolution,
                                        _tileMeshResolution,
                                        _mercator);
}
<|endoftext|>


<commit_before>/**
 * Clever programming language
 * Copyright (c) 2011-2012 Clever Team
 *
 * Permission is hereby granted, free of charge, to any person
 * obtaining a copy of this software and associated documentation
 * files (the "Software"), to deal in the Software without
 * restriction, including without limitation the rights to use,
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the
 * Software is furnished to do so, subject to the following
 * conditions:
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 *  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
 * OTHER DEALINGS IN THE SOFTWARE.
 */

#include "compiler/value.h"
#include "modules/std/net/net.h"
#include "modules/std/net/tcpsocket.h"
#include "modules/std/rpc/rpc.h"
#include "modules/std/rpc/rpcclass.h"
#include "modules/std/rpc/rpcobject.h"
#include "compiler/compiler.h"
#include "compiler/cstring.h"
#include "types/nativetypes.h"

namespace clever { namespace packages { namespace std { namespace rpc {

/**
 * RPC::constructor()
 */
CLEVER_METHOD(RPC::constructor) {
}

/**
 * Void RPC::operator=(RPC object)
 */
CLEVER_METHOD(RPC::do_assign) {

}

CLEVER_METHOD(RPC::client) {
	RPCValue* rv = CLEVER_GET_VALUE(RPCValue*, value);

	CLEVER_RETURN_BOOL(rv->createClient(CLEVER_ARG_STR(0).c_str(), CLEVER_ARG_INT(1), CLEVER_ARG_INT(2)));
}

CLEVER_METHOD(RPC::server) {
	RPCValue* rv = CLEVER_GET_VALUE(RPCValue*, value);

	rv->createServer(CLEVER_ARG_INT(0), CLEVER_ARG_INT(1));
}

CLEVER_METHOD(RPC::sendString) {
	RPCValue* rv = CLEVER_GET_VALUE(RPCValue*, value);

	rv->sendString(CLEVER_ARG_STR(0).c_str(), CLEVER_ARG_STR(0).size());
}

CLEVER_METHOD(RPC::loadLibrary) {
	RPCValue* rv = CLEVER_GET_VALUE(RPCValue*, value);

	rv->loadLibrary(CLEVER_ARG_STR(0).c_str());
}

CLEVER_METHOD(RPC::addFunction) {
	RPCValue* rv = CLEVER_GET_VALUE(RPCValue*, value);

	rv->addFunction(CLEVER_ARG_STR(0).c_str(), CLEVER_ARG_STR(1).c_str(), CLEVER_ARG_STR(2).c_str());
}

CLEVER_METHOD(RPC::sendInteger) {
	RPCValue* rv = CLEVER_GET_VALUE(RPCValue*, value);

	rv->sendInteger(CLEVER_ARG_INT(0));
}

CLEVER_METHOD(RPC::sendKill) {
	RPCValue* rv = CLEVER_GET_VALUE(RPCValue*, value);

	rv->sendKill();
}

CLEVER_METHOD(RPC::sendInit) {
	RPCValue* rv = CLEVER_GET_VALUE(RPCValue*, value);

	CLEVER_RETURN_BOOL(rv->sendInit());
}

CLEVER_METHOD(RPC::callFunction) {
	RPCValue* rv = CLEVER_GET_VALUE(RPCValue*, value);
	size_t size = CLEVER_NUM_ARGS();
	const char* fname = CLEVER_ARG_STR(0).c_str();
	int len_fname = CLEVER_ARG_STR(0).size();
	int n_args = size-1;
	int len_args = 0;

	for(size_t i=1;i<size;++i){
		len_args+=sizeof(char);
		if (CLEVER_ARG_IS_INT(i)) {
			len_args+=sizeof(int);
		} else if (CLEVER_ARG_IS_BOOL(i)) {
			len_args+=sizeof(char);
		} else if (CLEVER_ARG_IS_STR(i)) {
			len_args+=sizeof(int);
			len_args+=CLEVER_ARG_STR(i).size();
		} else if (CLEVER_ARG_IS_BYTE(i)) {
			len_args+=sizeof(char);
		} else if (CLEVER_ARG_IS_DOUBLE(i)) {
			len_args+=sizeof(double);
		} else if ( CLEVER_ARG_IS_USER(i) ) {
		}
	}

	char* buffer = static_cast<char*>(malloc(len_args+1));

	len_args=0;
	for(size_t i=1;i<size;++i){
		if (CLEVER_ARG_IS_INT(i)) {
			buffer[len_args]='i';
			len_args+=sizeof(char);

			int vi = CLEVER_ARG_INT(i);

			memcpy(buffer+len_args,&vi,sizeof(int));
			len_args+=sizeof(int);

		} else if (CLEVER_ARG_IS_BOOL(i)) {
			buffer[len_args]='b';
			len_args+=sizeof(char);

			char vb = CLEVER_ARG_BOOL(i);

			memcpy(buffer+len_args,&vb,sizeof(char));
			len_args+=sizeof(char);

		} else if (CLEVER_ARG_IS_STR(i)) {
			buffer[len_args]='s';
			len_args+=sizeof(char);

			int len=(int)(CLEVER_ARG_STR(i).size());
			const char* s=CLEVER_ARG_STR(i).c_str();

			memcpy(buffer+len_args,&len,sizeof(int));

			len_args+=sizeof(int);

			memcpy(buffer+len_args,s,len);
			len_args+=len;

		} else if (CLEVER_ARG_IS_BYTE(i)) {
			buffer[len_args]='c';
			len_args+=sizeof(char);

			char vb = CLEVER_ARG_BOOL(i);

			memcpy(buffer+len_args,&vb,sizeof(char));
			len_args+=sizeof(char);
		} else if (CLEVER_ARG_IS_DOUBLE(i)) {
			buffer[len_args]='d';
			len_args+=sizeof(char);

			double vd = CLEVER_ARG_DOUBLE(i);

			memcpy(buffer+len_args,&vd,sizeof(double));
			len_args+=sizeof(double);
		} else if ( CLEVER_ARG_IS_USER(i) ) {
		}
	}

	rv->sendFunctionCall(fname,buffer,len_fname,n_args,len_args);
	free(buffer);

	CLEVER_RETURN_DATA_VALUE(rv->receiveObject());
}

CLEVER_METHOD(RPC::callProcess) {
	RPCValue* rv = CLEVER_GET_VALUE(RPCValue*, value);
	size_t size = CLEVER_NUM_ARGS();
	int id_process = CLEVER_ARG_INT(0);
	const char* fname = CLEVER_ARG_STR(1).c_str();
	int len_fname = CLEVER_ARG_STR(1).size();
	int n_args = size-2;
	int len_args = 0;

	for(size_t i=2;i<size;++i){
		len_args+=sizeof(char);
		if (CLEVER_ARG_IS_INT(i)) {
			len_args+=sizeof(int);
		} else if (CLEVER_ARG_IS_BOOL(i)) {
			len_args+=sizeof(char);
		} else if (CLEVER_ARG_IS_STR(i)) {
			len_args+=sizeof(int);
			len_args+=CLEVER_ARG_STR(i).size();
		} else if (CLEVER_ARG_IS_BYTE(i)) {
			len_args+=sizeof(char);
		} else if (CLEVER_ARG_IS_DOUBLE(i)) {
			len_args+=sizeof(double);
		} else if ( CLEVER_ARG_IS_USER(i) ) {
		}
	}

	char* buffer = static_cast<char*>(malloc(len_args+1));

	len_args=0;
	for(size_t i=2;i<size;++i){
		if (CLEVER_ARG_IS_INT(i)) {
			buffer[len_args]='i';
			len_args+=sizeof(char);

			int vi = CLEVER_ARG_INT(i);

			memcpy(buffer+len_args,&vi,sizeof(int));
			len_args+=sizeof(int);

		} else if (CLEVER_ARG_IS_BOOL(i)) {
			buffer[len_args]='b';
			len_args+=sizeof(char);

			char vb = CLEVER_ARG_BOOL(i);

			memcpy(buffer+len_args,&vb,sizeof(char));
			len_args+=sizeof(char);

		} else if (CLEVER_ARG_IS_STR(i)) {
			buffer[len_args]='s';
			len_args+=sizeof(char);

			int len=(int)(CLEVER_ARG_STR(i).size());
			const char* s=CLEVER_ARG_STR(i).c_str();

			memcpy(buffer+len_args,&len,sizeof(int));

			len_args+=sizeof(int);

			memcpy(buffer+len_args,s,len);
			len_args+=len;

		} else if (CLEVER_ARG_IS_BYTE(i)) {
			buffer[len_args]='c';
			len_args+=sizeof(char);

			char vb = CLEVER_ARG_BOOL(i);

			memcpy(buffer+len_args,&vb,sizeof(char));
			len_args+=sizeof(char);
		} else if (CLEVER_ARG_IS_DOUBLE(i)) {
			buffer[len_args]='d';
			len_args+=sizeof(char);

			double vd = CLEVER_ARG_DOUBLE(i);

			memcpy(buffer+len_args,&vd,sizeof(double));
			len_args+=sizeof(double);
		} else if ( CLEVER_ARG_IS_USER(i) ) {
		}
	}

	rv->sendProcessCall(id_process,fname,buffer,len_fname,n_args,len_args);
	free(buffer);
}

CLEVER_METHOD(RPC::waitResult) {
	RPCValue* rv = CLEVER_GET_VALUE(RPCValue*, value);
	int id_process = CLEVER_ARG_INT(0);
	double time_sleep = CLEVER_ARG_DOUBLE(1);

	CLEVER_RETURN_DATA_VALUE(rv->getResultProcess(id_process, time_sleep));
}

void RPC::init() {
	const Type* rpcobj = CLEVER_TYPE("RPCClass");
	const Type* rpcobjvalue = CLEVER_TYPE("RPCObject");

	addMethod(new Method(CLEVER_CTOR_NAME,
		(MethodPtr)&RPC::constructor, rpcobj));

	addMethod(
		(new Method(CLEVER_OPERATOR_ASSIGN, (MethodPtr)&RPC::do_assign,
			rpcobj, false))
			->addArg("rvalue", rpcobj)
	);

	addMethod(
		(new Method("client", (MethodPtr)&RPC::client, CLEVER_BOOL))
			->addArg("host", CLEVER_STR)
			->addArg("port", CLEVER_INT)
			->addArg("time", CLEVER_INT)
	);

	addMethod(
		(new Method("server", (MethodPtr)&RPC::server, CLEVER_VOID))
			->addArg("port", CLEVER_INT)
			->addArg("nconnections", CLEVER_INT)
	);

	addMethod(
		(new Method("loadLibrary", (MethodPtr)&RPC::loadLibrary, CLEVER_VOID))
			->addArg("libname", CLEVER_STR)
	);

	addMethod(
		(new Method("addFunction", (MethodPtr)&RPC::addFunction, CLEVER_VOID))
			->addArg("libname", CLEVER_STR)
			->addArg("funcname", CLEVER_STR)
			->addArg("rettype", CLEVER_STR)
	);

	addMethod(
		(new Method("sendString", (MethodPtr)&RPC::sendString, CLEVER_VOID))
			->addArg("value", CLEVER_STR)
	);

	addMethod(
		(new Method("sendInteger", (MethodPtr)&RPC::sendInteger, CLEVER_VOID))
			->addArg("value", CLEVER_INT)
	);

	addMethod(
		(new Method("sendKill", (MethodPtr)&RPC::sendKill, CLEVER_VOID))
	);

	addMethod(
		(new Method("sendInit", (MethodPtr)&RPC::sendInit, CLEVER_BOOL))
	);

	addMethod(
		(new Method("callFunction", (MethodPtr)&RPC::callFunction, rpcobjvalue))
			->setVariadic()
			->setMinNumArgs(1)
	);

	addMethod(
		(new Method("callProcess", (MethodPtr)&RPC::callProcess, CLEVER_VOID))
			->setVariadic()
			->setMinNumArgs(2)
	);

	addMethod(
		(new Method("waitResult", (MethodPtr)&RPC::waitResult, rpcobjvalue))
			->addArg("id", CLEVER_INT)
			->addArg("time_sleep", CLEVER_DOUBLE)
	);

}

DataValue* RPC::allocateValue() const {
	return new RPCValue;
}

void RPC::destructor(Value* value) const {
}

}}}} // clever::packages::std::rpc

<commit_msg>Fixed race condition RPC server<commit_after>/**
 * Clever programming language
 * Copyright (c) 2011-2012 Clever Team
 *
 * Permission is hereby granted, free of charge, to any person
 * obtaining a copy of this software and associated documentation
 * files (the "Software"), to deal in the Software without
 * restriction, including without limitation the rights to use,
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the
 * Software is furnished to do so, subject to the following
 * conditions:
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 *  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
 * OTHER DEALINGS IN THE SOFTWARE.
 */

#include "compiler/value.h"
#include "modules/std/net/net.h"
#include "modules/std/net/tcpsocket.h"
#include "modules/std/rpc/rpc.h"
#include "modules/std/rpc/rpcclass.h"
#include "modules/std/rpc/rpcobject.h"
#include "compiler/compiler.h"
#include "compiler/cstring.h"
#include "types/nativetypes.h"

namespace clever { namespace packages { namespace std { namespace rpc {

/**
 * RPC::constructor()
 */
CLEVER_METHOD(RPC::constructor) {
	RPCValue* sv = new RPCValue;

	CLEVER_RETURN_DATA_VALUE(sv);
}

/**
 * Void RPC::operator=(RPC object)
 */
CLEVER_METHOD(RPC::do_assign) {
	value->setDataValue(CLEVER_ARG_DATA_VALUE(0));
}

CLEVER_METHOD(RPC::client) {
	RPCValue* rv = CLEVER_GET_VALUE(RPCValue*, value);

	CLEVER_RETURN_BOOL(rv->createClient(CLEVER_ARG_STR(0).c_str(), CLEVER_ARG_INT(1), CLEVER_ARG_INT(2)));
}

CLEVER_METHOD(RPC::server) {
	RPCValue* rv = CLEVER_GET_VALUE(RPCValue*, value);

	rv->createServer(CLEVER_ARG_INT(0), CLEVER_ARG_INT(1));
}

CLEVER_METHOD(RPC::sendString) {
	RPCValue* rv = CLEVER_GET_VALUE(RPCValue*, value);

	rv->sendString(CLEVER_ARG_STR(0).c_str(), CLEVER_ARG_STR(0).size());
}

CLEVER_METHOD(RPC::loadLibrary) {
	RPCValue* rv = CLEVER_GET_VALUE(RPCValue*, value);

	rv->loadLibrary(CLEVER_ARG_STR(0).c_str());
}

CLEVER_METHOD(RPC::addFunction) {
	RPCValue* rv = CLEVER_GET_VALUE(RPCValue*, value);

	rv->addFunction(CLEVER_ARG_STR(0).c_str(), CLEVER_ARG_STR(1).c_str(), CLEVER_ARG_STR(2).c_str());
}

CLEVER_METHOD(RPC::sendInteger) {
	RPCValue* rv = CLEVER_GET_VALUE(RPCValue*, value);

	rv->sendInteger(CLEVER_ARG_INT(0));
}

CLEVER_METHOD(RPC::sendKill) {
	RPCValue* rv = CLEVER_GET_VALUE(RPCValue*, value);

	rv->sendKill();
}

CLEVER_METHOD(RPC::sendInit) {
	RPCValue* rv = CLEVER_GET_VALUE(RPCValue*, value);

	CLEVER_RETURN_BOOL(rv->sendInit());
}

CLEVER_METHOD(RPC::callFunction) {
	RPCValue* rv = CLEVER_GET_VALUE(RPCValue*, value);
	size_t size = CLEVER_NUM_ARGS();
	const char* fname = CLEVER_ARG_STR(0).c_str();
	int len_fname = CLEVER_ARG_STR(0).size();
	int n_args = size-1;
	int len_args = 0;

	for(size_t i=1;i<size;++i){
		len_args+=sizeof(char);
		if (CLEVER_ARG_IS_INT(i)) {
			len_args+=sizeof(int);
		} else if (CLEVER_ARG_IS_BOOL(i)) {
			len_args+=sizeof(char);
		} else if (CLEVER_ARG_IS_STR(i)) {
			len_args+=sizeof(int);
			len_args+=CLEVER_ARG_STR(i).size();
		} else if (CLEVER_ARG_IS_BYTE(i)) {
			len_args+=sizeof(char);
		} else if (CLEVER_ARG_IS_DOUBLE(i)) {
			len_args+=sizeof(double);
		} else if ( CLEVER_ARG_IS_USER(i) ) {
		}
	}

	char* buffer = static_cast<char*>(malloc(len_args+1));

	len_args=0;
	for(size_t i=1;i<size;++i){
		if (CLEVER_ARG_IS_INT(i)) {
			buffer[len_args]='i';
			len_args+=sizeof(char);

			int vi = CLEVER_ARG_INT(i);

			memcpy(buffer+len_args,&vi,sizeof(int));
			len_args+=sizeof(int);

		} else if (CLEVER_ARG_IS_BOOL(i)) {
			buffer[len_args]='b';
			len_args+=sizeof(char);

			char vb = CLEVER_ARG_BOOL(i);

			memcpy(buffer+len_args,&vb,sizeof(char));
			len_args+=sizeof(char);

		} else if (CLEVER_ARG_IS_STR(i)) {
			buffer[len_args]='s';
			len_args+=sizeof(char);

			int len=(int)(CLEVER_ARG_STR(i).size());
			const char* s=CLEVER_ARG_STR(i).c_str();

			memcpy(buffer+len_args,&len,sizeof(int));

			len_args+=sizeof(int);

			memcpy(buffer+len_args,s,len);
			len_args+=len;

		} else if (CLEVER_ARG_IS_BYTE(i)) {
			buffer[len_args]='c';
			len_args+=sizeof(char);

			char vb = CLEVER_ARG_BOOL(i);

			memcpy(buffer+len_args,&vb,sizeof(char));
			len_args+=sizeof(char);
		} else if (CLEVER_ARG_IS_DOUBLE(i)) {
			buffer[len_args]='d';
			len_args+=sizeof(char);

			double vd = CLEVER_ARG_DOUBLE(i);

			memcpy(buffer+len_args,&vd,sizeof(double));
			len_args+=sizeof(double);
		} else if ( CLEVER_ARG_IS_USER(i) ) {
		}
	}

	rv->sendFunctionCall(fname,buffer,len_fname,n_args,len_args);
	free(buffer);

	CLEVER_RETURN_DATA_VALUE(rv->receiveObject());
}

CLEVER_METHOD(RPC::callProcess) {
	RPCValue* rv = CLEVER_GET_VALUE(RPCValue*, value);
	size_t size = CLEVER_NUM_ARGS();
	int id_process = CLEVER_ARG_INT(0);
	const char* fname = CLEVER_ARG_STR(1).c_str();
	int len_fname = CLEVER_ARG_STR(1).size();
	int n_args = size-2;
	int len_args = 0;

	for(size_t i=2;i<size;++i){
		len_args+=sizeof(char);
		if (CLEVER_ARG_IS_INT(i)) {
			len_args+=sizeof(int);
		} else if (CLEVER_ARG_IS_BOOL(i)) {
			len_args+=sizeof(char);
		} else if (CLEVER_ARG_IS_STR(i)) {
			len_args+=sizeof(int);
			len_args+=CLEVER_ARG_STR(i).size();
		} else if (CLEVER_ARG_IS_BYTE(i)) {
			len_args+=sizeof(char);
		} else if (CLEVER_ARG_IS_DOUBLE(i)) {
			len_args+=sizeof(double);
		} else if ( CLEVER_ARG_IS_USER(i) ) {
		}
	}

	char* buffer = static_cast<char*>(malloc(len_args+1));

	len_args=0;
	for(size_t i=2;i<size;++i){
		if (CLEVER_ARG_IS_INT(i)) {
			buffer[len_args]='i';
			len_args+=sizeof(char);

			int vi = CLEVER_ARG_INT(i);

			memcpy(buffer+len_args,&vi,sizeof(int));
			len_args+=sizeof(int);

		} else if (CLEVER_ARG_IS_BOOL(i)) {
			buffer[len_args]='b';
			len_args+=sizeof(char);

			char vb = CLEVER_ARG_BOOL(i);

			memcpy(buffer+len_args,&vb,sizeof(char));
			len_args+=sizeof(char);

		} else if (CLEVER_ARG_IS_STR(i)) {
			buffer[len_args]='s';
			len_args+=sizeof(char);

			int len=(int)(CLEVER_ARG_STR(i).size());
			const char* s=CLEVER_ARG_STR(i).c_str();

			memcpy(buffer+len_args,&len,sizeof(int));

			len_args+=sizeof(int);

			memcpy(buffer+len_args,s,len);
			len_args+=len;

		} else if (CLEVER_ARG_IS_BYTE(i)) {
			buffer[len_args]='c';
			len_args+=sizeof(char);

			char vb = CLEVER_ARG_BOOL(i);

			memcpy(buffer+len_args,&vb,sizeof(char));
			len_args+=sizeof(char);
		} else if (CLEVER_ARG_IS_DOUBLE(i)) {
			buffer[len_args]='d';
			len_args+=sizeof(char);

			double vd = CLEVER_ARG_DOUBLE(i);

			memcpy(buffer+len_args,&vd,sizeof(double));
			len_args+=sizeof(double);
		} else if ( CLEVER_ARG_IS_USER(i) ) {
		}
	}

	rv->sendProcessCall(id_process,fname,buffer,len_fname,n_args,len_args);
	free(buffer);
}

CLEVER_METHOD(RPC::waitResult) {
	RPCValue* rv = CLEVER_GET_VALUE(RPCValue*, value);
	int id_process = CLEVER_ARG_INT(0);
	double time_sleep = CLEVER_ARG_DOUBLE(1);

	CLEVER_RETURN_DATA_VALUE(rv->getResultProcess(id_process, time_sleep));
}

void RPC::init() {
	const Type* rpcobj = CLEVER_TYPE("RPCClass");
	const Type* rpcobjvalue = CLEVER_TYPE("RPCObject");

	addMethod(new Method(CLEVER_CTOR_NAME,
		(MethodPtr)&RPC::constructor, rpcobj));

	addMethod(
		(new Method(CLEVER_OPERATOR_ASSIGN, (MethodPtr)&RPC::do_assign,
			rpcobj, false))
			->addArg("rvalue", rpcobj)
	);

	addMethod(
		(new Method("client", (MethodPtr)&RPC::client, CLEVER_BOOL))
			->addArg("host", CLEVER_STR)
			->addArg("port", CLEVER_INT)
			->addArg("time", CLEVER_INT)
	);

	addMethod(
		(new Method("server", (MethodPtr)&RPC::server, CLEVER_VOID))
			->addArg("port", CLEVER_INT)
			->addArg("nconnections", CLEVER_INT)
	);

	addMethod(
		(new Method("loadLibrary", (MethodPtr)&RPC::loadLibrary, CLEVER_VOID))
			->addArg("libname", CLEVER_STR)
	);

	addMethod(
		(new Method("addFunction", (MethodPtr)&RPC::addFunction, CLEVER_VOID))
			->addArg("libname", CLEVER_STR)
			->addArg("funcname", CLEVER_STR)
			->addArg("rettype", CLEVER_STR)
	);

	addMethod(
		(new Method("sendString", (MethodPtr)&RPC::sendString, CLEVER_VOID))
			->addArg("value", CLEVER_STR)
	);

	addMethod(
		(new Method("sendInteger", (MethodPtr)&RPC::sendInteger, CLEVER_VOID))
			->addArg("value", CLEVER_INT)
	);

	addMethod(
		(new Method("sendKill", (MethodPtr)&RPC::sendKill, CLEVER_VOID))
	);

	addMethod(
		(new Method("sendInit", (MethodPtr)&RPC::sendInit, CLEVER_BOOL))
	);

	addMethod(
		(new Method("callFunction", (MethodPtr)&RPC::callFunction, rpcobjvalue))
			->setVariadic()
			->setMinNumArgs(1)
	);

	addMethod(
		(new Method("callProcess", (MethodPtr)&RPC::callProcess, CLEVER_VOID))
			->setVariadic()
			->setMinNumArgs(2)
	);

	addMethod(
		(new Method("waitResult", (MethodPtr)&RPC::waitResult, rpcobjvalue))
			->addArg("id", CLEVER_INT)
			->addArg("time_sleep", CLEVER_DOUBLE)
	);

}

DataValue* RPC::allocateValue() const {
	return new RPCValue;
}

void RPC::destructor(Value* value) const {
}

}}}} // clever::packages::std::rpc

<|endoftext|>


<commit_before>namespace mant {
  namespace robotic {
    class ParallelKinematicMachine3PRPR {
      public:
        inline explicit ParallelKinematicMachine3PRPR() noexcept;

        inline arma::Row<double>::fixed<3> getMinimalActiveJointActuations() const noexcept;

        inline void setMinimalActiveJointActuations(
            const arma::Row<double>::fixed<3>& minimalActiveJointActuations) noexcept;

        inline arma::Row<double>::fixed<3> getMaximalActiveJointActuations() const noexcept;

        inline void setMaximalActiveJointActuations(
            const arma::Row<double>::fixed<3>& maximalActiveJointActuations) noexcept;

        inline arma::Mat<double>::fixed<2, 3> getEndEffectorJointPositions() const noexcept;

        inline void setEndEffectorJointPositions(
            const arma::Mat<double>::fixed<2, 3>& endEffectorJointPositions) noexcept;

        inline arma::Mat<double>::fixed<2, 3> getRedundantJointPositionStarts() const noexcept;

        inline void setRedundantJointPositionStarts(
            const arma::Mat<double>::fixed<2, 3>& redundantJointPositionStarts) noexcept;

        inline arma::Mat<double>::fixed<2, 3> getRedundantJointPositionEnds() const noexcept;

        inline void setRedundantJointPositionEnds(
            const arma::Mat<double>::fixed<2, 3>& redundantJointPositionEnds) noexcept;

        inline std::vector<arma::Mat<double>::fixed<2, 3>> getModel(
            const arma::Row<double>::fixed<3>& endEffectorPose,
            const arma::Col<double>& redundantJointActuations) const;

        inline arma::Row<double>::fixed<3> getActuation(
            const arma::Col<double>::fixed<3>& endEffectorPose,
            const arma::Row<double>& redundantJointActuations) const;

        inline arma::Col<double>::fixed<3> getEndEffectorPose(
            const arma::Row<double>::fixed<3>& actuations,
            const arma::Row<double>& redundantJointActuations) const;

        inline double getEndEffectorPoseAccuracy(
            const arma::Col<double>::fixed<3>& endEffectorPose,
            const arma::Row<double>& redundantJointActuations) const;

      protected:
        arma::Mat<double>::fixed<2, 3> endEffectorJointPositions_;

        arma::Mat<double>::fixed<2, 3> redundantJointStartPositions_;
        arma::Mat<double>::fixed<2, 3> redundantJointEndPositions_;

        arma::Row<double>::fixed<3> minimalActiveJointActuations_;
        arma::Row<double>::fixed<3> maximalActiveJointActuations_;

        arma::Mat<double>::fixed<2, 3> redundantJointStartToEndPositions_;

        arma::Col<unsigned int> redundantJointIndicies_;
        arma::Row<double> redundantJointAngleSines_;
        arma::Row<double> redundantJointAngleCosines_;
    };

    //
    // Implementation
    //

    inline ParallelKinematicMachine3PRPR::ParallelKinematicMachine3PRPR() noexcept {
      setMinimalActiveJointActuations({0.1, 0.1, 0.1});
      setMinimalActiveJointActuations({1.2, 1.2, 1.2});

      setEndEffectorJointPositions({
        -0.000066580445834, 0.106954081945581,
        -0.092751709777083, -0.053477040972790,
        0.092818290222917, -0.053477040972790});

      setRedundantJointPositionStarts({
        0.1, 1.0392,
        0.0, 0.8,
        1.2, 0.8});

      setRedundantJointPositionEnds({
        1.1, 1.0392,
        0.0, -0.2,
        1.2, -0.2});

      redundantJointStartToEndPositions_ = redundantJointEndPositions_ - redundantJointStartPositions_;
      redundantJointIndicies_ = arma::find(arma::any(redundantJointStartToEndPositions_));

      redundantJointAngleSines_.set_size(redundantJointIndicies_.n_elem);
      redundantJointAngleCosines_.set_size(redundantJointIndicies_.n_elem);

      for (std::size_t n = 0; n < redundantJointIndicies_.n_elem; ++n) {
        const double redundantJointAngle = std::atan2(redundantJointStartToEndPositions_.at(1, n), redundantJointStartToEndPositions_.at(0, n));
        redundantJointAngleSines_.at(n) = std::sin(redundantJointAngle);
        redundantJointAngleCosines_.at(n) = std::cos(redundantJointAngle);
      }
    }

    inline arma::Row<double>::fixed<3> ParallelKinematicMachine3PRPR::getMinimalActiveJointActuations() const noexcept {
      return minimalActiveJointActuations_;
    }

    inline void ParallelKinematicMachine3PRPR::setMinimalActiveJointActuations(
        const arma::Row<double>::fixed<3>& minimalActiveJointActuations) noexcept {
      minimalActiveJointActuations_ = minimalActiveJointActuations;
    }

    inline arma::Row<double>::fixed<3> ParallelKinematicMachine3PRPR::getMaximalActiveJointActuations() const noexcept {
      return maximalActiveJointActuations_;
    }

    inline void ParallelKinematicMachine3PRPR::setMaximalActiveJointActuations(
        const arma::Row<double>::fixed<3>& maximalActiveJointActuations) noexcept {
      maximalActiveJointActuations_ = maximalActiveJointActuations;
    }

    inline arma::Mat<double>::fixed<2, 3> ParallelKinematicMachine3PRPR::getEndEffectorJointPositions() const noexcept {
      return endEffectorJointPositions_;
    }

    inline void ParallelKinematicMachine3PRPR::setEndEffectorJointPositions(
        const arma::Mat<double>::fixed<2, 3>& endEffectorJointPositions) noexcept {
      endEffectorJointPositions_ = endEffectorJointPositions;
    }

    inline arma::Mat<double>::fixed<2, 3> ParallelKinematicMachine3PRPR::getRedundantJointPositionStarts() const noexcept {
      return redundantJointStartPositions_;
    }

    inline void ParallelKinematicMachine3PRPR::setRedundantJointPositionStarts(
        const arma::Mat<double>::fixed<2, 3>& redundantJointStartPositions) noexcept {
      redundantJointStartPositions_ = redundantJointStartPositions;
    }

    inline arma::Mat<double>::fixed<2, 3> ParallelKinematicMachine3PRPR::getRedundantJointPositionEnds() const noexcept {
      return redundantJointEndPositions_;
    }

    inline void ParallelKinematicMachine3PRPR::setRedundantJointPositionEnds(
        const arma::Mat<double>::fixed<2, 3>& redundantJointEndPositions) noexcept {
      redundantJointEndPositions_ = redundantJointEndPositions;
    }

    inline std::vector<arma::Mat<double>::fixed<2, 3>> ParallelKinematicMachine3PRPR::getModel(
        const arma::Row<double>::fixed<3>& endEffectorPose,
        const arma::Col<double>& redundantJointActuations) const {

      if (arma::any(arma::vectorise(redundantJointActuations < 0)) || arma::any(arma::vectorise(redundantJointActuations > 1))) {
        throw std::logic_error("All values for the actuation of redundantion joints must be between [0, 1].");
      }

      const arma::Col<double>::fixed<2>& endEffectorPosition = endEffectorPose.subvec(0, 1);
      const double& endEffectorAngle = endEffectorPose.at(2);

      arma::Mat<double>::fixed<2, 3> baseJointPositions = redundantJointStartPositions_;
      for (std::size_t n = 0; n < redundantJointIndicies_.n_elem; n++) {
        const unsigned int& redundantJointIndex = redundantJointIndicies_.at(n);
        baseJointPositions.col(redundantJointIndex) += redundantJointActuations.at(redundantJointIndex) * redundantJointStartToEndPositions_.col(redundantJointIndex);
      }

      arma::Mat<double>::fixed<2, 3> endEffectorJointPositions = get2DRotationMatrix(endEffectorAngle) * endEffectorJointPositions_;
      endEffectorJointPositions.each_col() += endEffectorPosition;

      std::vector<arma::Mat<double>::fixed<2, 3>> model;

      model.push_back(baseJointPositions);
      model.push_back(endEffectorJointPositions);

      return model;
    }

    inline arma::Row<double>::fixed<3> ParallelKinematicMachine3PRPR::getActuation(
        const arma::Col<double>::fixed<3>& endEffectorPose,
        const arma::Row<double>& redundantJointActuations) const {
      const std::vector<arma::Mat<double>::fixed<2, 3>>& model = getModel(endEffectorPose, redundantJointActuations);

      const arma::Mat<double>::fixed<2, 3>& baseJoints = model.at(0);
      const arma::Mat<double>::fixed<2, 3>& endEffectorJoints = model.at(1);

      return arma::sqrt(arma::sum(arma::square(endEffectorJoints - baseJoints)));
    }

    inline arma::Col<double>::fixed<3> ParallelKinematicMachine3PRPR::getEndEffectorPose(
        const arma::Row<double>::fixed<3>& actuations,
        const arma::Row<double>& redundantJointActuations) const {
      // TODO Direct kinematic (estimate position, using a simple HillCLimber algorithm)
      return {0, 0, 0};
    }

    inline double ParallelKinematicMachine3PRPR::getEndEffectorPoseAccuracy(
        const arma::Col<double>::fixed<3>& endEffectorPose,
        const arma::Row<double>& redundantJointActuations) const {
      const std::vector<arma::Mat<double>::fixed<2, 3>>& model = getModel(endEffectorPose, redundantJointActuations);

      const arma::Mat<double>::fixed<2, 3>& baseJointPositions = model.at(1);

      const arma::Mat<double>::fixed<2, 3>& endEffectorJointPositions = model.at(1);
      arma::Mat<double>::fixed<2, 3> endEffectorJointPositionsRotated = endEffectorJointPositions;
      endEffectorJointPositionsRotated.each_col() -= endEffectorPose.subvec(0, 1);

      const arma::Mat<double>::fixed<2, 3>& baseToEndEffectorJointPositions = endEffectorJointPositions - baseJointPositions;
      const arma::Row<double>::fixed<3>& baseToEndEffectorJointActuations = arma::sqrt(arma::sum(arma::square(baseToEndEffectorJointPositions)));

      if (arma::any(baseToEndEffectorJointActuations < minimalActiveJointActuations_) || arma::any(baseToEndEffectorJointActuations > maximalActiveJointActuations_)) {
        return 0.0;
      }

      arma::Mat<double>::fixed<3, 3> forwardKinematic;
      forwardKinematic.rows(0, 1) = baseToEndEffectorJointPositions;
      forwardKinematic.row(2) = -forwardKinematic.row(0) % endEffectorJointPositionsRotated.row(1) + forwardKinematic.row(1) % endEffectorJointPositionsRotated.row(0);

      arma::Mat<double> inverseKinematic(3, 3 + redundantJointIndicies_.n_elem, arma::fill::zeros);
      inverseKinematic.diag() = -baseToEndEffectorJointActuations;
      for (std::size_t n = 0; n < redundantJointIndicies_.n_elem; ++n) {
        const unsigned int& redundantJointIndex = redundantJointIndicies_.at(n);
        inverseKinematic.at(n, 3 + n) = forwardKinematic.at(redundantJointIndex, 0) * redundantJointAngleCosines_.at(n) + forwardKinematic.at(redundantJointIndex, 1) * redundantJointAngleSines_.at(n);
      }

      return -1.0 / arma::cond(arma::solve(forwardKinematic.t(), inverseKinematic));
    }
  }
}
<commit_msg>devel: Fixed some Col/Row issues<commit_after>namespace mant {
  namespace robotic {
    class ParallelKinematicMachine3PRPR {
      public:
        inline explicit ParallelKinematicMachine3PRPR() noexcept;

        inline arma::Row<double>::fixed<3> getMinimalActiveJointActuations() const noexcept;

        inline void setMinimalActiveJointActuations(
            const arma::Row<double>::fixed<3>& minimalActiveJointActuations) noexcept;

        inline arma::Row<double>::fixed<3> getMaximalActiveJointActuations() const noexcept;

        inline void setMaximalActiveJointActuations(
            const arma::Row<double>::fixed<3>& maximalActiveJointActuations) noexcept;

        inline arma::Mat<double>::fixed<2, 3> getEndEffectorJointPositions() const noexcept;

        inline void setEndEffectorJointPositions(
            const arma::Mat<double>::fixed<2, 3>& endEffectorJointPositions) noexcept;

        inline arma::Mat<double>::fixed<2, 3> getRedundantJointPositionStarts() const noexcept;

        inline void setRedundantJointPositionStarts(
            const arma::Mat<double>::fixed<2, 3>& redundantJointPositionStarts) noexcept;

        inline arma::Mat<double>::fixed<2, 3> getRedundantJointPositionEnds() const noexcept;

        inline void setRedundantJointPositionEnds(
            const arma::Mat<double>::fixed<2, 3>& redundantJointPositionEnds) noexcept;

        inline std::vector<arma::Mat<double>::fixed<2, 3>> getModel(
            const arma::Col<double>::fixed<3>& endEffectorPose,
            const arma::Row<double>& redundantJointActuations) const;

        inline arma::Row<double>::fixed<3> getActuation(
            const arma::Col<double>::fixed<3>& endEffectorPose,
            const arma::Row<double>& redundantJointActuations) const;

        inline arma::Col<double>::fixed<3> getEndEffectorPose(
            const arma::Row<double>::fixed<3>& actuations,
            const arma::Row<double>& redundantJointActuations) const;

        inline double getEndEffectorPoseAccuracy(
            const arma::Col<double>::fixed<3>& endEffectorPose,
            const arma::Row<double>& redundantJointActuations) const;

      protected:
        arma::Row<double>::fixed<3> minimalActiveJointActuations_;
        arma::Row<double>::fixed<3> maximalActiveJointActuations_;

        arma::Mat<double>::fixed<2, 3> endEffectorJointPositions_;

        arma::Mat<double>::fixed<2, 3> redundantJointStartPositions_;
        arma::Mat<double>::fixed<2, 3> redundantJointEndPositions_;

        arma::Mat<double>::fixed<2, 3> redundantJointStartToEndPositions_;

        arma::Col<unsigned int> redundantJointIndicies_;
        arma::Row<double> redundantJointAngleSines_;
        arma::Row<double> redundantJointAngleCosines_;
    };

    //
    // Implementation
    //

    inline ParallelKinematicMachine3PRPR::ParallelKinematicMachine3PRPR() noexcept {
      setMinimalActiveJointActuations({0.1, 0.1, 0.1});
      setMinimalActiveJointActuations({1.2, 1.2, 1.2});

      setEndEffectorJointPositions({
        -0.000066580445834, 0.106954081945581,
        -0.092751709777083, -0.053477040972790,
        0.092818290222917, -0.053477040972790});

      setRedundantJointPositionStarts({
        0.1, 1.0392,
        0.0, 0.8,
        1.2, 0.8});

      setRedundantJointPositionEnds({
        1.1, 1.0392,
        0.0, -0.2,
        1.2, -0.2});

      redundantJointStartToEndPositions_ = redundantJointEndPositions_ - redundantJointStartPositions_;
      redundantJointIndicies_ = arma::find(arma::any(redundantJointStartToEndPositions_));

      redundantJointAngleSines_.set_size(redundantJointIndicies_.n_elem);
      redundantJointAngleCosines_.set_size(redundantJointIndicies_.n_elem);

      for (std::size_t n = 0; n < redundantJointIndicies_.n_elem; ++n) {
        const double redundantJointAngle = std::atan2(redundantJointStartToEndPositions_.at(1, n), redundantJointStartToEndPositions_.at(0, n));
        redundantJointAngleSines_.at(n) = std::sin(redundantJointAngle);
        redundantJointAngleCosines_.at(n) = std::cos(redundantJointAngle);
      }
    }

    inline arma::Row<double>::fixed<3> ParallelKinematicMachine3PRPR::getMinimalActiveJointActuations() const noexcept {
      return minimalActiveJointActuations_;
    }

    inline void ParallelKinematicMachine3PRPR::setMinimalActiveJointActuations(
        const arma::Row<double>::fixed<3>& minimalActiveJointActuations) noexcept {
      minimalActiveJointActuations_ = minimalActiveJointActuations;
    }

    inline arma::Row<double>::fixed<3> ParallelKinematicMachine3PRPR::getMaximalActiveJointActuations() const noexcept {
      return maximalActiveJointActuations_;
    }

    inline void ParallelKinematicMachine3PRPR::setMaximalActiveJointActuations(
        const arma::Row<double>::fixed<3>& maximalActiveJointActuations) noexcept {
      maximalActiveJointActuations_ = maximalActiveJointActuations;
    }

    inline arma::Mat<double>::fixed<2, 3> ParallelKinematicMachine3PRPR::getEndEffectorJointPositions() const noexcept {
      return endEffectorJointPositions_;
    }

    inline void ParallelKinematicMachine3PRPR::setEndEffectorJointPositions(
        const arma::Mat<double>::fixed<2, 3>& endEffectorJointPositions) noexcept {
      endEffectorJointPositions_ = endEffectorJointPositions;
    }

    inline arma::Mat<double>::fixed<2, 3> ParallelKinematicMachine3PRPR::getRedundantJointPositionStarts() const noexcept {
      return redundantJointStartPositions_;
    }

    inline void ParallelKinematicMachine3PRPR::setRedundantJointPositionStarts(
        const arma::Mat<double>::fixed<2, 3>& redundantJointStartPositions) noexcept {
      redundantJointStartPositions_ = redundantJointStartPositions;
    }

    inline arma::Mat<double>::fixed<2, 3> ParallelKinematicMachine3PRPR::getRedundantJointPositionEnds() const noexcept {
      return redundantJointEndPositions_;
    }

    inline void ParallelKinematicMachine3PRPR::setRedundantJointPositionEnds(
        const arma::Mat<double>::fixed<2, 3>& redundantJointEndPositions) noexcept {
      redundantJointEndPositions_ = redundantJointEndPositions;
    }

    inline std::vector<arma::Mat<double>::fixed<2, 3>> ParallelKinematicMachine3PRPR::getModel(
        const arma::Col<double>::fixed<3>& endEffectorPose,
        const arma::Row<double>& redundantJointActuations) const {

      if (arma::any(arma::vectorise(redundantJointActuations < 0)) || arma::any(arma::vectorise(redundantJointActuations > 1))) {
        throw std::logic_error("All values for the actuation of redundantion joints must be between [0, 1].");
      }

      const arma::Col<double>::fixed<2>& endEffectorPosition = endEffectorPose.subvec(0, 1);
      const double& endEffectorAngle = endEffectorPose.at(2);

      arma::Mat<double>::fixed<2, 3> baseJointPositions = redundantJointStartPositions_;
      for (std::size_t n = 0; n < redundantJointIndicies_.n_elem; n++) {
        const unsigned int& redundantJointIndex = redundantJointIndicies_.at(n);
        baseJointPositions.col(redundantJointIndex) += redundantJointActuations.at(redundantJointIndex) * redundantJointStartToEndPositions_.col(redundantJointIndex);
      }

      arma::Mat<double>::fixed<2, 3> endEffectorJointPositions = get2DRotationMatrix(endEffectorAngle) * endEffectorJointPositions_;
      endEffectorJointPositions.each_col() += endEffectorPosition;

      std::vector<arma::Mat<double>::fixed<2, 3>> model;

      model.push_back(baseJointPositions);
      model.push_back(endEffectorJointPositions);

      return model;
    }

    inline arma::Row<double>::fixed<3> ParallelKinematicMachine3PRPR::getActuation(
        const arma::Col<double>::fixed<3>& endEffectorPose,
        const arma::Row<double>& redundantJointActuations) const {
      const std::vector<arma::Mat<double>::fixed<2, 3>>& model = getModel(endEffectorPose, redundantJointActuations);

      const arma::Mat<double>::fixed<2, 3>& baseJoints = model.at(0);
      const arma::Mat<double>::fixed<2, 3>& endEffectorJoints = model.at(1);

      return arma::sqrt(arma::sum(arma::square(endEffectorJoints - baseJoints)));
    }

    inline arma::Col<double>::fixed<3> ParallelKinematicMachine3PRPR::getEndEffectorPose(
        const arma::Row<double>::fixed<3>& actuations,
        const arma::Row<double>& redundantJointActuations) const {
      // TODO Direct kinematic (estimate position, using a simple HillCLimber algorithm)
      return {0, 0, 0};
    }

    inline double ParallelKinematicMachine3PRPR::getEndEffectorPoseAccuracy(
        const arma::Col<double>::fixed<3>& endEffectorPose,
        const arma::Row<double>& redundantJointActuations) const {
      const std::vector<arma::Mat<double>::fixed<2, 3>>& model = getModel(endEffectorPose, redundantJointActuations);

      const arma::Mat<double>::fixed<2, 3>& baseJointPositions = model.at(1);

      const arma::Mat<double>::fixed<2, 3>& endEffectorJointPositions = model.at(1);
      arma::Mat<double>::fixed<2, 3> endEffectorJointPositionsRotated = endEffectorJointPositions;
      endEffectorJointPositionsRotated.each_col() -= endEffectorPose.subvec(0, 1);

      const arma::Mat<double>::fixed<2, 3>& baseToEndEffectorJointPositions = endEffectorJointPositions - baseJointPositions;
      const arma::Row<double>::fixed<3>& baseToEndEffectorJointActuations = arma::sqrt(arma::sum(arma::square(baseToEndEffectorJointPositions)));

      if (arma::any(baseToEndEffectorJointActuations < minimalActiveJointActuations_) || arma::any(baseToEndEffectorJointActuations > maximalActiveJointActuations_)) {
        return 0.0;
      }

      arma::Mat<double>::fixed<3, 3> forwardKinematic;
      forwardKinematic.rows(0, 1) = baseToEndEffectorJointPositions;
      forwardKinematic.row(2) = -forwardKinematic.row(0) % endEffectorJointPositionsRotated.row(1) + forwardKinematic.row(1) % endEffectorJointPositionsRotated.row(0);

      arma::Mat<double> inverseKinematic(3, 3 + redundantJointIndicies_.n_elem, arma::fill::zeros);
      inverseKinematic.diag() = -baseToEndEffectorJointActuations;
      for (std::size_t n = 0; n < redundantJointIndicies_.n_elem; ++n) {
        const unsigned int& redundantJointIndex = redundantJointIndicies_.at(n);
        inverseKinematic.at(n, 3 + n) = forwardKinematic.at(redundantJointIndex, 0) * redundantJointAngleCosines_.at(n) + forwardKinematic.at(redundantJointIndex, 1) * redundantJointAngleSines_.at(n);
      }

      return -1.0 / arma::cond(arma::solve(forwardKinematic.t(), inverseKinematic));
    }
  }
}
<|endoftext|>