<commit_before>#ifndef _SDD_MEM_PTR_HH_
#define _SDD_MEM_PTR_HH_

#include <cassert>
#include <functional>  // hash, function
#include <type_traits> // remove_const

#include "sdd/mem/unique_table.hh"

namespace sdd { namespace mem {

/*------------------------------------------------------------------------------------------------*/

/// @internal
/// @brief Define the type of a deletion handler.
///
/// A deletion handler is called by ptr whenever a data is no longer referenced.
template <typename Unique>
using handler_type = std::function<void (const Unique&)>;

/// @internal
/// @brief Get the deletion handler for a given Unique type.
template <typename Unique>
handler_type<Unique>&
deletion_handler()
{
  static handler_type<Unique> handler = [](const Unique&){assert(false && "Unset handler");};
  return handler;
}

/// @internal
/// @brief Reset the deletion handler for a given Unique type.
template <typename Unique>
void
reset_deletion_handler()
{
  deletion_handler<Unique>() = [](const Unique&){assert(false && "Reset handler");};
}

/// @internal
/// @brief Set the deletion handler for a given Unique type.
template <typename Unique>
void
set_deletion_handler(const handler_type<Unique>& h)
{
  deletion_handler<Unique>() = h;
}

/*------------------------------------------------------------------------------------------------*/

/// @internal
/// @brief A smart pointer to manage unified ressources.
/// @tparam Unique the type of the unified ressource.
///
/// Unified ressources are ref_counted elements constructed with an unique_table.
template <typename Unique>
class ptr
{
  // Can't default construct a ptr.
  ptr() = delete;

private:

  /// @brief Pointer to the managed ressource, a unified data.
  const Unique* x_;

public:

  /// @brief Constructor with a unified data.
  explicit
  ptr(const Unique& p)
  noexcept
  	: x_(&p)
  {
    x_->increment_reference_counter();
  }

  /// @brief Copy constructor.
  ptr(const ptr& other)
  noexcept
    : x_(other.x_)
  {
    x_->increment_reference_counter();
  }

  /// @brief Copy operator.
  ptr&
  operator=(const ptr& other)
  noexcept
  {
    other.x_->increment_reference_counter();
    x_->decrement_reference_counter();
    erase_if_necessary(x_);
    x_ = other.x_;
    return *this;
  }

  /// @brief Destructor.
  ~ptr()
  {
    x_->decrement_reference_counter();
    erase_if_necessary(x_);
  }

  /// @brief Get a reference to the unified data.
  const Unique&
  operator*()
  const noexcept
  {
    return *x_;
  }

  /// @brief Get a pointer to the unified data.
  const Unique*
  operator->()
  const noexcept
  {
    return x_;
  }

  /// @brief Swap.
  friend void
  swap(ptr& lhs, ptr& rhs)
  noexcept
  {
    using std::swap;
    swap(lhs.x_, rhs.x_);
  }

private:

  /// @brief If the managed data is no longer referenced, erase it.
  static
  void
  erase_if_necessary(const Unique* x)
  noexcept
  {
    static void* table[2] = {&&noop, &&erase};
    goto *table[x->is_not_referenced()];
    erase: deletion_handler<Unique>()(*x);
    noop:;
  }
};

/*------------------------------------------------------------------------------------------------*/

/// @internal
/// @related ptr
///
/// O(1).
template <typename Unique>
inline
bool
operator==(const ptr<Unique>& lhs, const ptr<Unique>& rhs)
noexcept
{
  return lhs.operator->() == rhs.operator->();
}

/// @internal
/// @related ptr
///
/// O(1).
template <typename Unique>
inline
bool
operator<(const ptr<Unique>& lhs, const ptr<Unique>& rhs)
noexcept
{
  return lhs.operator->() < rhs.operator->();
}

/*------------------------------------------------------------------------------------------------*/

}} // namespace sdd::mem

namespace std {

/*------------------------------------------------------------------------------------------------*/

/// @internal
/// @brief Hash specialization for sdd::mem::ptr
template <typename Unique>
struct hash<sdd::mem::ptr<Unique>>
{
  std::size_t
  operator()(const sdd::mem::ptr<Unique>& x)
  const noexcept
  {
    return std::hash<decltype(x.operator->())>()(x.operator->());
  }
};

/*------------------------------------------------------------------------------------------------*/

} // namespace std

#endif // _SDD_MEM_PTR_HH_
<commit_msg>Remove useless jump table.<commit_after>#ifndef _SDD_MEM_PTR_HH_
#define _SDD_MEM_PTR_HH_

#include <cassert>
#include <functional>  // hash, function
#include <type_traits> // remove_const

#include "sdd/mem/unique_table.hh"

namespace sdd { namespace mem {

/*------------------------------------------------------------------------------------------------*/

/// @internal
/// @brief Define the type of a deletion handler.
///
/// A deletion handler is called by ptr whenever a data is no longer referenced.
template <typename Unique>
using handler_type = std::function<void (const Unique&)>;

/// @internal
/// @brief Get the deletion handler for a given Unique type.
template <typename Unique>
handler_type<Unique>&
deletion_handler()
{
  static handler_type<Unique> handler = [](const Unique&){assert(false && "Unset handler");};
  return handler;
}

/// @internal
/// @brief Reset the deletion handler for a given Unique type.
template <typename Unique>
void
reset_deletion_handler()
{
  deletion_handler<Unique>() = [](const Unique&){assert(false && "Reset handler");};
}

/// @internal
/// @brief Set the deletion handler for a given Unique type.
template <typename Unique>
void
set_deletion_handler(const handler_type<Unique>& h)
{
  deletion_handler<Unique>() = h;
}

/*------------------------------------------------------------------------------------------------*/

/// @internal
/// @brief A smart pointer to manage unified ressources.
/// @tparam Unique the type of the unified ressource.
///
/// Unified ressources are ref_counted elements constructed with an unique_table.
template <typename Unique>
class ptr
{
  // Can't default construct a ptr.
  ptr() = delete;

private:

  /// @brief Pointer to the managed ressource, a unified data.
  const Unique* x_;

public:

  /// @brief Constructor with a unified data.
  explicit
  ptr(const Unique& p)
  noexcept
  	: x_(&p)
  {
    x_->increment_reference_counter();
  }

  /// @brief Copy constructor.
  ptr(const ptr& other)
  noexcept
    : x_(other.x_)
  {
    x_->increment_reference_counter();
  }

  /// @brief Copy operator.
  ptr&
  operator=(const ptr& other)
  noexcept
  {
    other.x_->increment_reference_counter();
    x_->decrement_reference_counter();
    erase_if_necessary(x_);
    x_ = other.x_;
    return *this;
  }

  /// @brief Destructor.
  ~ptr()
  {
    x_->decrement_reference_counter();
    erase_if_necessary(x_);
  }

  /// @brief Get a reference to the unified data.
  const Unique&
  operator*()
  const noexcept
  {
    return *x_;
  }

  /// @brief Get a pointer to the unified data.
  const Unique*
  operator->()
  const noexcept
  {
    return x_;
  }

  /// @brief Swap.
  friend void
  swap(ptr& lhs, ptr& rhs)
  noexcept
  {
    using std::swap;
    swap(lhs.x_, rhs.x_);
  }

private:

  /// @brief If the managed data is no longer referenced, erase it.
  static
  void
  erase_if_necessary(const Unique* x)
  noexcept
  {
    if (x->is_not_referenced())
    {
      deletion_handler<Unique>()(*x);
    }
  }
};

/*------------------------------------------------------------------------------------------------*/

/// @internal
/// @related ptr
///
/// O(1).
template <typename Unique>
inline
bool
operator==(const ptr<Unique>& lhs, const ptr<Unique>& rhs)
noexcept
{
  return lhs.operator->() == rhs.operator->();
}

/// @internal
/// @related ptr
///
/// O(1).
template <typename Unique>
inline
bool
operator<(const ptr<Unique>& lhs, const ptr<Unique>& rhs)
noexcept
{
  return lhs.operator->() < rhs.operator->();
}

/*------------------------------------------------------------------------------------------------*/

}} // namespace sdd::mem

namespace std {

/*------------------------------------------------------------------------------------------------*/

/// @internal
/// @brief Hash specialization for sdd::mem::ptr
template <typename Unique>
struct hash<sdd::mem::ptr<Unique>>
{
  std::size_t
  operator()(const sdd::mem::ptr<Unique>& x)
  const noexcept
  {
    return std::hash<decltype(x.operator->())>()(x.operator->());
  }
};

/*------------------------------------------------------------------------------------------------*/

} // namespace std

#endif // _SDD_MEM_PTR_HH_
<|endoftext|>


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

#include "include/XID_map.hpp"

using namespace std;

std::string getPureXidmap(const char *filename) {

	char s[5000];

	FILE *fd;
	if ((fd = fopen(filename, "r"))==NULL) {
		fprintf(stderr, "error: the file %s doesn't exist\n", filename);
		exit(-1);
	}
	fscanf(fd, "%s", s);
	fclose(fd);
	std::string xidmap(s);
	unsigned int pos = xidmap.find('|',0);
	if (pos == std::string::npos) {
		fprintf(stderr, "error: not a valid xidmap file");
		exit(-1);
	}
	//cout << xidmap << endl << "pos=" << pos << endl;
	std::string pureXidmap = xidmap.substr(0,pos) + ")";
	return pureXidmap;
}

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

	bool argsOk = false;
	if (argc==3) argsOk=true;
	if ((argc==4)&&(strcmp(argv[1], "-n")==0)) argsOk=true;
	if (!argsOk) {
		fprintf(stderr, "usage: XyCmpXidmap [-n] file1.xidmap file2.xidmap \n");
		fprintf(stderr, "             -n : only compare the number of nodes, not the values of the identifiers\n");
		exit(-1);
	}

	const char *file1 = argv[argc-2];
	const char *file2 = argv[argc-1];

	std::string xidmap1 = getPureXidmap(file1);
	std::string xidmap2 = getPureXidmap(file2);

	if (argc==3) {
		if (xidmap1==xidmap2) {
			std::cout << "The xidmaps from files <" << file1 << "> and <" << file2 << "> are identical" << endl ;
			return 0;
		} else {
			std::cout << "The xidmaps from files <" << file1 << "> and <" << file2 << ">  are different" << endl;
			std::cout << xidmap1 << endl << xidmap2 << endl;
			exit(-1);
		}
	} else {
		XidMap_Parser p1(xidmap1.c_str());
		XidMap_Parser p2(xidmap2.c_str());
		int n1=0;
		while (!p1.isListEmpty()) {
			(void)p1.getNextXID();
			n1++;
		}
		int n2=0;
		while (!p2.isListEmpty()) {
			(void)p2.getNextXID();
			n2++;
		}
		if (n1==n2) {
			std::cout << "The xidmaps from files <" << file1 << "> and <" << file2 << "> have the same number of nodes ("<<n1<<")"<< endl ;
			return 0;
		} else {
			std::cout << "The xidmaps from files <" << file1 << "> and <" << file2 << "> do not have the same number of nodes ("<<n1<<" and "<<n2<<")"<< endl ;
			exit(-1);
		}
	}
	std::cerr << "Internal error - unreachable code" << endl;
	return(-1);
}
<commit_msg>Removed conditional that always evaluates false<commit_after>#include <stdio.h>
#include <stdlib.h>
#include <iostream>
#include <string>

#include "include/XID_map.hpp"

using namespace std;

std::string getPureXidmap(const char *filename) {

	char s[5000];

	FILE *fd;
	if ((fd = fopen(filename, "r"))==NULL) {
		fprintf(stderr, "error: the file %s doesn't exist\n", filename);
		exit(-1);
	}
	fscanf(fd, "%s", s);
	fclose(fd);
	std::string xidmap(s);
	unsigned int pos = xidmap.find('|',0);

	//cout << xidmap << endl << "pos=" << pos << endl;
	std::string pureXidmap = xidmap.substr(0,pos) + ")";
	return pureXidmap;
}

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

	bool argsOk = false;
	if (argc==3) argsOk=true;
	if ((argc==4)&&(strcmp(argv[1], "-n")==0)) argsOk=true;
	if (!argsOk) {
		fprintf(stderr, "usage: XyCmpXidmap [-n] file1.xidmap file2.xidmap \n");
		fprintf(stderr, "             -n : only compare the number of nodes, not the values of the identifiers\n");
		exit(-1);
	}

	const char *file1 = argv[argc-2];
	const char *file2 = argv[argc-1];

	std::string xidmap1 = getPureXidmap(file1);
	std::string xidmap2 = getPureXidmap(file2);

	if (argc==3) {
		if (xidmap1==xidmap2) {
			std::cout << "The xidmaps from files <" << file1 << "> and <" << file2 << "> are identical" << endl ;
			return 0;
		} else {
			std::cout << "The xidmaps from files <" << file1 << "> and <" << file2 << ">  are different" << endl;
			std::cout << xidmap1 << endl << xidmap2 << endl;
			exit(-1);
		}
	} else {
		XidMap_Parser p1(xidmap1.c_str());
		XidMap_Parser p2(xidmap2.c_str());
		int n1=0;
		while (!p1.isListEmpty()) {
			(void)p1.getNextXID();
			n1++;
		}
		int n2=0;
		while (!p2.isListEmpty()) {
			(void)p2.getNextXID();
			n2++;
		}
		if (n1==n2) {
			std::cout << "The xidmaps from files <" << file1 << "> and <" << file2 << "> have the same number of nodes ("<<n1<<")"<< endl ;
			return 0;
		} else {
			std::cout << "The xidmaps from files <" << file1 << "> and <" << file2 << "> do not have the same number of nodes ("<<n1<<" and "<<n2<<")"<< endl ;
			exit(-1);
		}
	}
	std::cerr << "Internal error - unreachable code" << endl;
	return(-1);
}
<|endoftext|>


<commit_before>/*
 * TestSpheresDataSource.cpp
 *
 * Copyright (C) 2011 by Universitaet Stuttgart (VISUS).
 * Alle Rechte vorbehalten.
 */

#include "stdafx.h"
#define _USE_MATH_DEFINES
#include "TestSpheresDataSource.h"
#include "cmath"
#include "geometry_calls/MultiParticleDataCall.h"
#include "mmcore/param/BoolParam.h"
#include "mmcore/param/IntParam.h"
#include "mmcore/param/StringParam.h"
#include "vislib/math/Quaternion.h"
#include "vislib/math/ShallowVector.h"
#include "vislib/math/Vector.h"

namespace megamol::moldyn::io {

/*
 * TestSpheresDataSource::TestSpheresDataSource
 */
TestSpheresDataSource::TestSpheresDataSource(void)
        : AnimDataModule()
        , getDataSlot("getData", "Gets the data from the data source")
        , numSpheresSlot("numSpheres", "number of spheres to generate")
        , numFramesSlot("numFrames", "number of frames to generate")
#ifdef MMCORE_TEST_DYN_PARAM_SLOTS
        , p1("p1", "Test slot for dynamic parameter slots")
        , p2("p2", "Test slot for dynamic parameter slots")
#endif
{

    this->getDataSlot.SetCallback(geocalls::MultiParticleDataCall::ClassName(),
        geocalls::MultiParticleDataCall::FunctionName(0), &TestSpheresDataSource::getDataCallback);
    this->getDataSlot.SetCallback(geocalls::MultiParticleDataCall::ClassName(),
        geocalls::MultiParticleDataCall::FunctionName(1), &TestSpheresDataSource::getExtentCallback);
    this->MakeSlotAvailable(&this->getDataSlot);

    numSpheresSlot << new core::param::IntParam(15);
    this->MakeSlotAvailable(&numSpheresSlot);

    numFramesSlot << new core::param::IntParam(100);
    this->MakeSlotAvailable(&numFramesSlot);

#ifdef MMCORE_TEST_DYN_PARAM_SLOTS
    p1.SetParameter(new param::BoolParam(false));
    MakeSlotAvailable(&p1);
    p1.ForceSetDirty();

    p2.SetParameter(new param::StringParam("Hugo"));
#endif
}

/*
 * TestSpheresDataSource::~TestSpheresDataSource
 */
TestSpheresDataSource::~TestSpheresDataSource(void) {
    this->Release();
}

/*
 * TestSpheresDataSource::constructFrame
 */
core::view::AnimDataModule::Frame* TestSpheresDataSource::constructFrame(void) const {
    return new Frame(const_cast<TestSpheresDataSource&>(*this));
}

/*
 * BezierDataSource::create
 */
bool TestSpheresDataSource::create(void) {
    auto f = this->numFramesSlot.Param<core::param::IntParam>()->Value();
    AnimDataModule::setFrameCount(f);
    AnimDataModule::initFrameCache(f);
    return true;
}

/*
 * TestSpheresDataSource::loadFrame
 */
void TestSpheresDataSource::loadFrame(AnimDataModule::Frame* frame, unsigned int idx) {
    Frame* frm = dynamic_cast<Frame*>(frame);
    if (frm == NULL)
        return;
    frm->SetFrameNumber(idx);
    auto frameCount = this->numFramesSlot.Param<core::param::IntParam>()->Value();
    auto sphereCount = this->numSpheresSlot.Param<core::param::IntParam>()->Value();
    frm->data = new float[8 * sphereCount];
    for (unsigned int i = 0; i < sphereCount; i++) {
        vislib::math::ShallowVector<float, 3> pos(&frm->data[i * 8]);
        ::srand(i); // stablize values for particles
        float& r = frm->data[i * 8 + 3];
        float& cr = frm->data[i * 8 + 4];
        float& cg = frm->data[i * 8 + 5];
        float& cb = frm->data[i * 8 + 6];
        float& ca = frm->data[i * 8 + 7];
        vislib::math::Vector<float, 3> X(static_cast<float>((::rand() % 2) * 2 - 1), 0.0f, 0.0f);
        vislib::math::Vector<float, 3> Y(0.0f, static_cast<float>((::rand() % 2) * 2 - 1), 0.0f);
        vislib::math::Vector<float, 3> Z(static_cast<float>(1000 - ::rand() % 2001) * 0.001f,
            static_cast<float>(1000 - ::rand() % 2001) * 0.001f, static_cast<float>(1000 - ::rand() % 2001) * 0.001f);
        switch (::rand() % 6) {
        case 0:
            Z.SetX(1.0f);
            break;
        case 1:
            Z.SetX(-1.0f);
            break;
        case 2:
            Z.SetY(1.0f);
            break;
        case 3:
            Z.SetY(-1.0f);
            break;
        case 4:
            Z.SetZ(1.0f);
            break;
        case 5:
            Z.SetZ(-1.0f);
            break;
        }
        Z.Normalise();
        vislib::math::Quaternion<float> rot(static_cast<float>((::rand() % 2) * 2 - 1) * static_cast<float>(M_PI) *
                                                static_cast<float>(::rand() % 2000) * 0.001f,
            Z);
        float dist = static_cast<float>(::rand() % 1001) * 0.001f;
        dist = ::pow(dist, 0.333f) * 0.9f;
        float a = (static_cast<float>(2 * idx) / static_cast<float>(frameCount)) * static_cast<float>(M_PI);
        X = rot * X;
        Y = rot * Y;

        X *= sin(a) * dist;
        Y *= cos(a) * dist;
        pos = X;
        pos += Y;

        r = 0.05f + static_cast<float>(::rand() % 501) * 0.0001f;

        Z.Set(static_cast<float>(1000 - ::rand() % 2001) * 0.001f, static_cast<float>(1000 - ::rand() % 2001) * 0.001f,
            static_cast<float>(1000 - ::rand() % 2001) * 0.001f);
        switch (::rand() % 6) {
        case 0:
            Z.SetX(1.0f);
            break;
        case 1:
            Z.SetX(-1.0f);
            break;
        case 2:
            Z.SetY(1.0f);
            break;
        case 3:
            Z.SetY(-1.0f);
            break;
        case 4:
            Z.SetZ(1.0f);
            break;
        case 5:
            Z.SetZ(-1.0f);
            break;
        }
        Z.Normalise();

        cr = vislib::math::Abs(Z.X());
        cg = vislib::math::Abs(Z.Y());
        cb = vislib::math::Abs(Z.Z());
        ca = 1.0f;
    }
}

/*
 * TestSpheresDataSource::release
 */
void TestSpheresDataSource::release(void) {
    // intentionally empty
}

/*
 * TestSpheresDataSource::getDataCallback
 */
bool TestSpheresDataSource::getDataCallback(core::Call& caller) {
    geocalls::MultiParticleDataCall* mpdc = dynamic_cast<geocalls::MultiParticleDataCall*>(&caller);
    if (mpdc == NULL)
        return false;

    auto frameCount = this->numFramesSlot.Param<core::param::IntParam>()->Value();
    auto sphereCount = this->numSpheresSlot.Param<core::param::IntParam>()->Value();

    if (this->numFramesSlot.IsDirty() || this->numSpheresSlot.IsDirty()) {
        this->resetFrameCache();
        AnimDataModule::setFrameCount(frameCount);
        AnimDataModule::initFrameCache(frameCount);
        this->numFramesSlot.ResetDirty();
        this->numSpheresSlot.ResetDirty();
    }

    AnimDataModule::Frame* f = this->requestLockedFrame(mpdc->FrameID());
    if (f == NULL)
        return false;
    f->Unlock(); // because I know that this data source is simple enough that no locking is required
    Frame* frm = dynamic_cast<Frame*>(f);
    if (frm == NULL)
        return false;

#ifdef MMCORE_TEST_DYN_PARAM_SLOTS
    if (p1.IsDirty()) {
        p1.ResetDirty();
        if (p1.Param<param::BoolParam>()->Value()) {
            if (p2.GetStatus() == AbstractSlot::STATUS_UNAVAILABLE) {
                MakeSlotAvailable(&p2);
            }
        } else {
            if (p2.GetStatus() != AbstractSlot::STATUS_UNAVAILABLE) {
                SetSlotUnavailable(&p2);
            }
        }
    }
#endif

    mpdc->SetFrameID(f->FrameNumber());
    mpdc->SetDataHash(1);
    mpdc->SetExtent(frameCount, -1.0f, -1.0f, -1.0f, 1.0f, 1.0f, 1.0f);
    mpdc->SetParticleListCount(1);
    mpdc->AccessParticles(0).SetCount(sphereCount);
    mpdc->AccessParticles(0).SetVertexData(
        geocalls::SimpleSphericalParticles::VERTDATA_FLOAT_XYZR, frm->data, sizeof(float) * 8);
    mpdc->AccessParticles(0).SetColourData(
        geocalls::SimpleSphericalParticles::COLDATA_FLOAT_RGBA, frm->data + 4, sizeof(float) * 8);
    mpdc->SetUnlocker(NULL);

    return true;
}

/*
 * TestSpheresDataSource::getExtentCallback
 */
bool TestSpheresDataSource::getExtentCallback(core::Call& caller) {
    geocalls::MultiParticleDataCall* mpdc = dynamic_cast<geocalls::MultiParticleDataCall*>(&caller);
    if (mpdc == NULL)
        return false;

    auto frameCount = this->numFramesSlot.Param<core::param::IntParam>()->Value();

    mpdc->SetDataHash(1);
    mpdc->SetExtent(frameCount, -1.0f, -1.0f, -1.0f, 1.0f, 1.0f, 1.0f);

    return true;
}
} // namespace megamol::moldyn::io
<commit_msg>changed TestSpheres to use mersenne twister<commit_after>/*
 * TestSpheresDataSource.cpp
 *
 * Copyright (C) 2011 by Universitaet Stuttgart (VISUS).
 * Alle Rechte vorbehalten.
 */

#include "stdafx.h"
#define _USE_MATH_DEFINES
#include "TestSpheresDataSource.h"
#include "cmath"
#include "geometry_calls/MultiParticleDataCall.h"
#include "mmcore/param/BoolParam.h"
#include "mmcore/param/IntParam.h"
#include "mmcore/param/StringParam.h"
#include "vislib/math/Quaternion.h"
#include "vislib/math/ShallowVector.h"
#include "vislib/math/Vector.h"
#define USE_MERSENNE_TWISTER
#ifdef USE_MERSENNE_TWISTER
#include <random>
#endif

namespace megamol::moldyn::io {

/*
 * TestSpheresDataSource::TestSpheresDataSource
 */
TestSpheresDataSource::TestSpheresDataSource(void)
        : AnimDataModule()
        , getDataSlot("getData", "Gets the data from the data source")
        , numSpheresSlot("numSpheres", "number of spheres to generate")
        , numFramesSlot("numFrames", "number of frames to generate")
#ifdef MMCORE_TEST_DYN_PARAM_SLOTS
        , p1("p1", "Test slot for dynamic parameter slots")
        , p2("p2", "Test slot for dynamic parameter slots")
#endif
{

    this->getDataSlot.SetCallback(geocalls::MultiParticleDataCall::ClassName(),
        geocalls::MultiParticleDataCall::FunctionName(0), &TestSpheresDataSource::getDataCallback);
    this->getDataSlot.SetCallback(geocalls::MultiParticleDataCall::ClassName(),
        geocalls::MultiParticleDataCall::FunctionName(1), &TestSpheresDataSource::getExtentCallback);
    this->MakeSlotAvailable(&this->getDataSlot);

    numSpheresSlot << new core::param::IntParam(15);
    this->MakeSlotAvailable(&numSpheresSlot);

    numFramesSlot << new core::param::IntParam(100);
    this->MakeSlotAvailable(&numFramesSlot);

#ifdef MMCORE_TEST_DYN_PARAM_SLOTS
    p1.SetParameter(new param::BoolParam(false));
    MakeSlotAvailable(&p1);
    p1.ForceSetDirty();

    p2.SetParameter(new param::StringParam("Hugo"));
#endif
}

/*
 * TestSpheresDataSource::~TestSpheresDataSource
 */
TestSpheresDataSource::~TestSpheresDataSource(void) {
    this->Release();
}

/*
 * TestSpheresDataSource::constructFrame
 */
core::view::AnimDataModule::Frame* TestSpheresDataSource::constructFrame(void) const {
    return new Frame(const_cast<TestSpheresDataSource&>(*this));
}

/*
 * BezierDataSource::create
 */
bool TestSpheresDataSource::create(void) {
    auto f = this->numFramesSlot.Param<core::param::IntParam>()->Value();
    AnimDataModule::setFrameCount(f);
    AnimDataModule::initFrameCache(f);
    return true;
}

/*
 * TestSpheresDataSource::loadFrame
 */
void TestSpheresDataSource::loadFrame(AnimDataModule::Frame* frame, unsigned int idx) {
    Frame* frm = dynamic_cast<Frame*>(frame);
    if (frm == NULL)
        return;
    frm->SetFrameNumber(idx);
    auto frameCount = this->numFramesSlot.Param<core::param::IntParam>()->Value();
    auto sphereCount = this->numSpheresSlot.Param<core::param::IntParam>()->Value();
    frm->data = new float[8 * sphereCount];
#ifdef USE_MERSENNE_TWISTER
    std::mt19937 twister;
    auto new_rand = [&twister]() -> int32_t { return twister() & RAND_MAX; };
    auto new_seed = [&twister](uint32_t i) { twister.seed(i); };
#else
    auto new_rand = []() -> int32_t { return ::rand(); };
    auto new_seed = [](uint32_t i) { ::srand(i); };
#endif
    for (unsigned int i = 0; i < sphereCount; i++) {
        vislib::math::ShallowVector<float, 3> pos(&frm->data[i * 8]);
        new_seed(i); // stablize values for particles
        float& r = frm->data[i * 8 + 3];
        float& cr = frm->data[i * 8 + 4];
        float& cg = frm->data[i * 8 + 5];
        float& cb = frm->data[i * 8 + 6];
        float& ca = frm->data[i * 8 + 7];
        vislib::math::Vector<float, 3> X(static_cast<float>((new_rand() % 2) * 2 - 1), 0.0f, 0.0f);
        vislib::math::Vector<float, 3> Y(0.0f, static_cast<float>((new_rand() % 2) * 2 - 1), 0.0f);
        vislib::math::Vector<float, 3> Z(static_cast<float>(1000 - new_rand() % 2001) * 0.001f,
            static_cast<float>(1000 - new_rand() % 2001) * 0.001f, static_cast<float>(1000 - new_rand() % 2001) * 0.001f);
        switch (new_rand() % 6) {
        case 0:
            Z.SetX(1.0f);
            break;
        case 1:
            Z.SetX(-1.0f);
            break;
        case 2:
            Z.SetY(1.0f);
            break;
        case 3:
            Z.SetY(-1.0f);
            break;
        case 4:
            Z.SetZ(1.0f);
            break;
        case 5:
            Z.SetZ(-1.0f);
            break;
        }
        Z.Normalise();
        vislib::math::Quaternion<float> rot(static_cast<float>((new_rand() % 2) * 2 - 1) * static_cast<float>(M_PI) *
                                                static_cast<float>(new_rand() % 2000) * 0.001f,
            Z);
        float dist = static_cast<float>(new_rand() % 1001) * 0.001f;
        dist = ::pow(dist, 0.333f) * 0.9f;
        float a = (static_cast<float>(2 * idx) / static_cast<float>(frameCount)) * static_cast<float>(M_PI);
        X = rot * X;
        Y = rot * Y;

        X *= sin(a) * dist;
        Y *= cos(a) * dist;
        pos = X;
        pos += Y;

        r = 0.05f + static_cast<float>(new_rand() % 501) * 0.0001f;

        Z.Set(static_cast<float>(1000 - new_rand() % 2001) * 0.001f, static_cast<float>(1000 - new_rand() % 2001) * 0.001f,
            static_cast<float>(1000 - new_rand() % 2001) * 0.001f);
        switch (new_rand() % 6) {
        case 0:
            Z.SetX(1.0f);
            break;
        case 1:
            Z.SetX(-1.0f);
            break;
        case 2:
            Z.SetY(1.0f);
            break;
        case 3:
            Z.SetY(-1.0f);
            break;
        case 4:
            Z.SetZ(1.0f);
            break;
        case 5:
            Z.SetZ(-1.0f);
            break;
        }
        Z.Normalise();

        cr = vislib::math::Abs(Z.X());
        cg = vislib::math::Abs(Z.Y());
        cb = vislib::math::Abs(Z.Z());
        ca = 1.0f;
    }
}

/*
 * TestSpheresDataSource::release
 */
void TestSpheresDataSource::release(void) {
    // intentionally empty
}

/*
 * TestSpheresDataSource::getDataCallback
 */
bool TestSpheresDataSource::getDataCallback(core::Call& caller) {
    geocalls::MultiParticleDataCall* mpdc = dynamic_cast<geocalls::MultiParticleDataCall*>(&caller);
    if (mpdc == NULL)
        return false;

    auto frameCount = this->numFramesSlot.Param<core::param::IntParam>()->Value();
    auto sphereCount = this->numSpheresSlot.Param<core::param::IntParam>()->Value();

    if (this->numFramesSlot.IsDirty() || this->numSpheresSlot.IsDirty()) {
        this->resetFrameCache();
        AnimDataModule::setFrameCount(frameCount);
        AnimDataModule::initFrameCache(frameCount);
        this->numFramesSlot.ResetDirty();
        this->numSpheresSlot.ResetDirty();
    }

    AnimDataModule::Frame* f = this->requestLockedFrame(mpdc->FrameID());
    if (f == NULL)
        return false;
    f->Unlock(); // because I know that this data source is simple enough that no locking is required
    Frame* frm = dynamic_cast<Frame*>(f);
    if (frm == NULL)
        return false;

#ifdef MMCORE_TEST_DYN_PARAM_SLOTS
    if (p1.IsDirty()) {
        p1.ResetDirty();
        if (p1.Param<param::BoolParam>()->Value()) {
            if (p2.GetStatus() == AbstractSlot::STATUS_UNAVAILABLE) {
                MakeSlotAvailable(&p2);
            }
        } else {
            if (p2.GetStatus() != AbstractSlot::STATUS_UNAVAILABLE) {
                SetSlotUnavailable(&p2);
            }
        }
    }
#endif

    mpdc->SetFrameID(f->FrameNumber());
    mpdc->SetDataHash(1);
    mpdc->SetExtent(frameCount, -1.0f, -1.0f, -1.0f, 1.0f, 1.0f, 1.0f);
    mpdc->SetParticleListCount(1);
    mpdc->AccessParticles(0).SetCount(sphereCount);
    mpdc->AccessParticles(0).SetVertexData(
        geocalls::SimpleSphericalParticles::VERTDATA_FLOAT_XYZR, frm->data, sizeof(float) * 8);
    mpdc->AccessParticles(0).SetColourData(
        geocalls::SimpleSphericalParticles::COLDATA_FLOAT_RGBA, frm->data + 4, sizeof(float) * 8);
    mpdc->SetUnlocker(NULL);

    return true;
}

/*
 * TestSpheresDataSource::getExtentCallback
 */
bool TestSpheresDataSource::getExtentCallback(core::Call& caller) {
    geocalls::MultiParticleDataCall* mpdc = dynamic_cast<geocalls::MultiParticleDataCall*>(&caller);
    if (mpdc == NULL)
        return false;

    auto frameCount = this->numFramesSlot.Param<core::param::IntParam>()->Value();

    mpdc->SetDataHash(1);
    mpdc->SetExtent(frameCount, -1.0f, -1.0f, -1.0f, 1.0f, 1.0f, 1.0f);

    return true;
}
} // namespace megamol::moldyn::io
<|endoftext|>


<commit_before>// TabletWindow.cpp

#include "TabletWindow.h"

#include "DataDirectoryLocation.h"
#include "AndroidTouchEnums.h"
#include "FontMgr.h"
#include "FontRenderer.h"
#include "MatrixMath.h"
#include "VectorMath.h"
#include "Logging.h"
#include <sstream>
#include <fstream>

TabletWindow::TabletWindow()
: g_si()
, m_luaScene()
, m_fps()
, m_logDumpTimer()
, m_iconx(20)
, m_icony(240)
, m_iconScale(1.f)
, m_glVersion()
, m_glRenderer()
, m_glSLVersion()
, m_holding(false)
, m_holdingMask(0)
, m_pointerStates(8)
, m_scaleAtPinchStart(1.f)
, m_lastTouchPoint(0,0)
, m_chassisYaw(0.f)
, m_chassisYawAtTouch(0.f)
{
    m_chassisPos.x = 0.f;
    m_chassisPos.y = -.6f;
    m_chassisPos.z = -5.f;
}

TabletWindow::~TabletWindow()
{
}

void FileWriteTest()
{
    const std::string dataHome = APP_DATA_DIRECTORY;
    const std::string outFilename = dataHome + "writtenFromApp.txt";

    LOG_INFO("Attempting to open file %s", outFilename.c_str());
    std::ofstream ofs;
    ofs.open(outFilename.c_str(), std::ios::out);
    if (ofs.is_open())
    {
        LOG_INFO("Open successful.");
        ofs << "Writing text..." << std::endl;
        ofs.close();
    }
    else
    {
        LOG_ERROR("Could not open");
    }
}

void TabletWindow::initGL()
{
    const std::string v(reinterpret_cast<const char*>(glGetString(GL_VERSION)));
    m_glVersion = v;

    const std::string r(reinterpret_cast<const char*>(glGetString(GL_VENDOR)));
    m_glRenderer = r;

    const std::string s(reinterpret_cast<const char*>(glGetString(GL_RENDERER)));
    m_glSLVersion = s;

    g_si.initGL();
    m_tp.initGL();

    const Language lang = USEnglish;
    FontMgr::Instance().LoadLanguageFonts(lang);

    m_luaScene.m_pLoaderFunc = m_pLoaderFunc;
    m_luaScene.initGL();
}

void TabletWindow::exitGL()
{
    m_luaScene.exitGL();
    g_si.exitGL();
    m_tp.exitGL();
}

void TabletWindow::setWindowSize(int w, int h)
{
    m_winw = w;
    m_winh = h;
    m_luaScene.setWindowSize(w, h);
}

void TabletWindow::_DrawIcon(int winw, int winh)
{
    float mview[16];
    float proj[16];
    // Set up projection matrix for Pixel coordinates - lower left origin
    glhOrtho(proj,
        0.f, static_cast<float>(winw),
        static_cast<float>(winh),0.f,
        -1.f, 1.f);
    MakeIdentityMatrix(mview);
    glhTranslate(mview, m_iconx, m_icony, 0);
    const float s = m_iconScale * m_scaleAtPinchStart;
    glhScale(mview, s, s, s);

    g_si.display(mview, proj);
}

void TabletWindow::_DrawText(int winw, int winh)
{
    float mview[16];
    float proj[16];

    MakeIdentityMatrix(mview);
    // Flip window coordinates vertically so origin is upper-left
    glhOrtho(proj,
        0.f, static_cast<float>(winw),
        static_cast<float>(winh), 0.f,
        -1.f, 1.f);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

    const FontRenderer* pFont24 = FontMgr::Instance().GetFontOfSize(24);
    if (pFont24 != NULL)
    {
        const int lineh = 40;
        int y = 10 - lineh;
        const float3 col = {.5f, 1.f, .5f};
        const bool doKerning = true;
        pFont24->DrawWString(
#ifdef __ANDROID__
            L"Flickercladding - Android",
#else
            L"Flickercladding - Desktop",
#endif
            10,
            y += lineh,
            col,
            proj,
            doKerning);

        pFont24->DrawString(
            m_glVersion.c_str(),
            10,
            y += lineh,
            col,
            proj,
            doKerning);

        pFont24->DrawString(
            m_glRenderer.c_str(),
            10,
            y += lineh,
            col,
            proj,
            doKerning);

        pFont24->DrawString(
            m_glSLVersion.c_str(),
            10,
            y += lineh,
            col,
            proj,
            doKerning);

        std::ostringstream oss;
        oss << m_fps.GetFPS() << " fps";
        pFont24->DrawString(
            oss.str().c_str(),
            10,
            y += lineh,
            col,
            proj,
            doKerning);

        const float3 wh = { 1.f, 1.f, 1.f };
        const std::string& err = m_luaScene.ErrorText();
        pFont24->DrawString(
            err.c_str(),
            10,
            y += lineh,
            wh,
            proj,
            doKerning);
    }
    glDisable(GL_BLEND);
}

void TabletWindow::_DisplayOverlay(int winw, int winh)
{
    _DrawIcon(winw, winh);
    _DrawText(winw, winh);

#if 0 //ndef __ANDROID__
    // Draw pointer states
    glPointSize(10.f);
    glDisable(GL_DEPTH_TEST);
    float mview[16];
    float proj[16];
    MakeIdentityMatrix(mview);
    // Flip window coordinates vertically so origin is upper-left
    glhOrtho(proj,
        0.f, static_cast<float>(winw),
        static_cast<float>(winh), 0.f,
        -1.f, 1.f);

    m_tp.display(mview, proj, m_pointerStates);
#endif
}

///@brief draws a 3D scene from a camera location
void TabletWindow::_DisplayScene(int winw, int winh)
{
    float mvmtx[16];
    float prmtx[16];
    MakeIdentityMatrix(mvmtx);

    glhTranslate(mvmtx, m_chassisPos.x, m_chassisPos.y, m_chassisPos.z);

    glhRotate(mvmtx, m_chassisYaw, 0.f, 1.f, 0.f);

    glhPerspectivef2(prmtx,
        80.f,
        static_cast<float>(winw) / static_cast<float>(winh),
        .1f, 100.f);

    m_luaScene.RenderForOneEye(mvmtx, prmtx);
}

void TabletWindow::display(int winw, int winh)
{
    glViewport(0, 0, winw, winh);
    glClearColor(0.f, 0.f, 0.f, 0.f);
    glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);

    glEnable(GL_DEPTH_TEST);
    _DisplayScene(winw, winh);

    glDisable(GL_DEPTH_TEST);
    _DisplayOverlay(winw, winh);
}

void TabletWindow::timestep(double absT, double dt)
{
    m_fps.OnFrame();

#if 1
    // Log fps at regular intervals
    const float dumpInterval = 1.f;
    if (m_logDumpTimer.seconds() > dumpInterval)
    {
        LOG_INFO("Frame rate: %d fps", static_cast<int>(m_fps.GetFPS()));
        m_logDumpTimer.reset();
}
#endif

    m_luaScene.timestep(absT, dt);
}

int getNumPointersDown(int mask)
{
    int num = 0;
    const int maxPointers = 10;
    for (int i=0; i<maxPointers; ++i)
    {
        const int flag = 1 << i;
        if ((mask & flag) != 0)
            ++num;
    }
    return num;
}

void TabletWindow::OnSingleTouch(int pointerid, int action, int x, int y)
{
    m_luaScene.onSingleTouch(pointerid, action, x, y);

    const int pointerflag = 1 << pointerid;
    const int actionflag = action & 0xff;

    if (pointerid >= m_pointerStates.size())
        return;

    touchState& ts = m_pointerStates[pointerid];
    ts.state = actionflag;
    ts.x = x;
    ts.y = y;

    if (actionflag == ActionDown) {
        m_holdingMask |= pointerflag;
        m_chassisPosAtTouch = m_chassisPos;
        m_chassisYawAtTouch = m_chassisYaw;
        m_lastTouchPoint = int2( x,y );
    }
    else if (actionflag == ActionPointerDown) {
        m_holdingMask |= pointerflag;
        // Get current pinch size
        m_chassisPosAtTouch = m_chassisPos;
    }
    else if (actionflag == ActionUp) {
        m_holdingMask &= ~pointerflag;
    }
    else if (actionflag == ActionPointerUp) {
        m_holdingMask &= ~pointerflag;
        if (getNumPointersDown(m_holdingMask) == 1)
        {
            m_scaleAtPinchStart *= m_iconScale;
            m_iconScale = 1.f;
        }
    }

    // Handle a pinch event
    if (actionflag == ActionPointerDown)
    {
        if (getNumPointersDown(m_holdingMask) == 2)
        {
            m_pinchStart.first = m_pointerStates[0];
            m_pinchStart.second = m_pointerStates[1];
        }
    }
    else if (actionflag == ActionMove)
    {
        if (getNumPointersDown(m_holdingMask) == 2)
        {
            const touchState& t0 = m_pinchStart.first;
            const touchState& t1 = m_pinchStart.second;
            const float3 a0 = {static_cast<float>(t0.x),static_cast<float>(t0.y),0};
            const float3 a1 = {static_cast<float>(t1.x),static_cast<float>(t1.y),0};
            const float initialDist = length(a1 - a0);
            const touchState& u0 = m_pointerStates[0];
            const touchState& u1 = m_pointerStates[1];
            const float3 b0 = {static_cast<float>(u0.x),static_cast<float>(u0.y),0};
            const float3 b1 = {static_cast<float>(u1.x),static_cast<float>(u1.y),0};
            const float curDist = length(b1 - b0);
            if (initialDist != 0.f)
            {
                //m_iconScale = curDist / initialDist;
                const float dz = .02f * (curDist - initialDist);
                m_chassisPos.z = m_chassisPosAtTouch.z + dz;
            }
        }
    }

    if (m_holdingMask == 1)
    {
        if (getNumPointersDown(m_holdingMask) == 1)
        {
            if ((actionflag == ActionDown) || (actionflag == ActionMove))
            {
                //m_iconx = x;
                //m_icony = y;
                const int dx = x - m_lastTouchPoint.x;
                const int dy = y - m_lastTouchPoint.y;
                //m_chassisPos.x = m_chassisPosAtTouch.x + (.01f * (float)dx);
                m_chassisPos.y = m_chassisPosAtTouch.y - (.01f * (float)dy);

                m_chassisYaw = m_chassisYawAtTouch + (.1f * (float)dx);
            }
        }
    }

}

void TabletWindow::OnWheelEvent(double dx, double dy)
{
    m_chassisPos.z += .4f * dy;
}

void TabletWindow::onKeyEvent(int key, int codes, int action, int mods)
{
    m_luaScene.keypressed(key);

    switch (key)
    {
    default: break;

    case 258: // Tab in GLFW3
    case 9: // Tab in SDL2
        m_luaScene.ChangeScene(1);
        break;
    }
}
<commit_msg>Pass masked action flag to luajit.<commit_after>// TabletWindow.cpp

#include "TabletWindow.h"

#include "DataDirectoryLocation.h"
#include "AndroidTouchEnums.h"
#include "FontMgr.h"
#include "FontRenderer.h"
#include "MatrixMath.h"
#include "VectorMath.h"
#include "Logging.h"
#include <sstream>
#include <fstream>

TabletWindow::TabletWindow()
: g_si()
, m_luaScene()
, m_fps()
, m_logDumpTimer()
, m_iconx(20)
, m_icony(240)
, m_iconScale(1.f)
, m_glVersion()
, m_glRenderer()
, m_glSLVersion()
, m_holding(false)
, m_holdingMask(0)
, m_pointerStates(8)
, m_scaleAtPinchStart(1.f)
, m_lastTouchPoint(0,0)
, m_chassisYaw(0.f)
, m_chassisYawAtTouch(0.f)
{
    m_chassisPos.x = 0.f;
    m_chassisPos.y = -.6f;
    m_chassisPos.z = -5.f;
}

TabletWindow::~TabletWindow()
{
}

void FileWriteTest()
{
    const std::string dataHome = APP_DATA_DIRECTORY;
    const std::string outFilename = dataHome + "writtenFromApp.txt";

    LOG_INFO("Attempting to open file %s", outFilename.c_str());
    std::ofstream ofs;
    ofs.open(outFilename.c_str(), std::ios::out);
    if (ofs.is_open())
    {
        LOG_INFO("Open successful.");
        ofs << "Writing text..." << std::endl;
        ofs.close();
    }
    else
    {
        LOG_ERROR("Could not open");
    }
}

void TabletWindow::initGL()
{
    const std::string v(reinterpret_cast<const char*>(glGetString(GL_VERSION)));
    m_glVersion = v;

    const std::string r(reinterpret_cast<const char*>(glGetString(GL_VENDOR)));
    m_glRenderer = r;

    const std::string s(reinterpret_cast<const char*>(glGetString(GL_RENDERER)));
    m_glSLVersion = s;

    g_si.initGL();
    m_tp.initGL();

    const Language lang = USEnglish;
    FontMgr::Instance().LoadLanguageFonts(lang);

    m_luaScene.m_pLoaderFunc = m_pLoaderFunc;
    m_luaScene.initGL();
}

void TabletWindow::exitGL()
{
    m_luaScene.exitGL();
    g_si.exitGL();
    m_tp.exitGL();
}

void TabletWindow::setWindowSize(int w, int h)
{
    m_winw = w;
    m_winh = h;
    m_luaScene.setWindowSize(w, h);
}

void TabletWindow::_DrawIcon(int winw, int winh)
{
    float mview[16];
    float proj[16];
    // Set up projection matrix for Pixel coordinates - lower left origin
    glhOrtho(proj,
        0.f, static_cast<float>(winw),
        static_cast<float>(winh),0.f,
        -1.f, 1.f);
    MakeIdentityMatrix(mview);
    glhTranslate(mview, m_iconx, m_icony, 0);
    const float s = m_iconScale * m_scaleAtPinchStart;
    glhScale(mview, s, s, s);

    g_si.display(mview, proj);
}

void TabletWindow::_DrawText(int winw, int winh)
{
    float mview[16];
    float proj[16];

    MakeIdentityMatrix(mview);
    // Flip window coordinates vertically so origin is upper-left
    glhOrtho(proj,
        0.f, static_cast<float>(winw),
        static_cast<float>(winh), 0.f,
        -1.f, 1.f);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

    const FontRenderer* pFont24 = FontMgr::Instance().GetFontOfSize(24);
    if (pFont24 != NULL)
    {
        const int lineh = 40;
        int y = 10 - lineh;
        const float3 col = {.5f, 1.f, .5f};
        const bool doKerning = true;
        pFont24->DrawWString(
#ifdef __ANDROID__
            L"Flickercladding - Android",
#else
            L"Flickercladding - Desktop",
#endif
            10,
            y += lineh,
            col,
            proj,
            doKerning);

        pFont24->DrawString(
            m_glVersion.c_str(),
            10,
            y += lineh,
            col,
            proj,
            doKerning);

        pFont24->DrawString(
            m_glRenderer.c_str(),
            10,
            y += lineh,
            col,
            proj,
            doKerning);

        pFont24->DrawString(
            m_glSLVersion.c_str(),
            10,
            y += lineh,
            col,
            proj,
            doKerning);

        std::ostringstream oss;
        oss << m_fps.GetFPS() << " fps";
        pFont24->DrawString(
            oss.str().c_str(),
            10,
            y += lineh,
            col,
            proj,
            doKerning);

        const float3 wh = { 1.f, 1.f, 1.f };
        const std::string& err = m_luaScene.ErrorText();
        pFont24->DrawString(
            err.c_str(),
            10,
            y += lineh,
            wh,
            proj,
            doKerning);
    }
    glDisable(GL_BLEND);
}

void TabletWindow::_DisplayOverlay(int winw, int winh)
{
    _DrawIcon(winw, winh);
    _DrawText(winw, winh);

#if 0 //ndef __ANDROID__
    // Draw pointer states
    glPointSize(10.f);
    glDisable(GL_DEPTH_TEST);
    float mview[16];
    float proj[16];
    MakeIdentityMatrix(mview);
    // Flip window coordinates vertically so origin is upper-left
    glhOrtho(proj,
        0.f, static_cast<float>(winw),
        static_cast<float>(winh), 0.f,
        -1.f, 1.f);

    m_tp.display(mview, proj, m_pointerStates);
#endif
}

///@brief draws a 3D scene from a camera location
void TabletWindow::_DisplayScene(int winw, int winh)
{
    float mvmtx[16];
    float prmtx[16];
    MakeIdentityMatrix(mvmtx);

    glhTranslate(mvmtx, m_chassisPos.x, m_chassisPos.y, m_chassisPos.z);

    glhRotate(mvmtx, m_chassisYaw, 0.f, 1.f, 0.f);

    glhPerspectivef2(prmtx,
        80.f,
        static_cast<float>(winw) / static_cast<float>(winh),
        .1f, 100.f);

    m_luaScene.RenderForOneEye(mvmtx, prmtx);
}

void TabletWindow::display(int winw, int winh)
{
    glViewport(0, 0, winw, winh);
    glClearColor(0.f, 0.f, 0.f, 0.f);
    glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);

    glEnable(GL_DEPTH_TEST);
    _DisplayScene(winw, winh);

    glDisable(GL_DEPTH_TEST);
    _DisplayOverlay(winw, winh);
}

void TabletWindow::timestep(double absT, double dt)
{
    m_fps.OnFrame();

#if 1
    // Log fps at regular intervals
    const float dumpInterval = 1.f;
    if (m_logDumpTimer.seconds() > dumpInterval)
    {
        LOG_INFO("Frame rate: %d fps", static_cast<int>(m_fps.GetFPS()));
        m_logDumpTimer.reset();
}
#endif

    m_luaScene.timestep(absT, dt);
}

int getNumPointersDown(int mask)
{
    int num = 0;
    const int maxPointers = 10;
    for (int i=0; i<maxPointers; ++i)
    {
        const int flag = 1 << i;
        if ((mask & flag) != 0)
            ++num;
    }
    return num;
}

void TabletWindow::OnSingleTouch(int pointerid, int action, int x, int y)
{
    const int actionflag = action & 0xff;
    m_luaScene.onSingleTouch(pointerid, actionflag, x, y);

    const int pointerflag = 1 << pointerid;

    if (pointerid >= m_pointerStates.size())
        return;

    touchState& ts = m_pointerStates[pointerid];
    ts.state = actionflag;
    ts.x = x;
    ts.y = y;

    if (actionflag == ActionDown) {
        m_holdingMask |= pointerflag;
        m_chassisPosAtTouch = m_chassisPos;
        m_chassisYawAtTouch = m_chassisYaw;
        m_lastTouchPoint = int2( x,y );
    }
    else if (actionflag == ActionPointerDown) {
        m_holdingMask |= pointerflag;
        // Get current pinch size
        m_chassisPosAtTouch = m_chassisPos;
    }
    else if (actionflag == ActionUp) {
        m_holdingMask &= ~pointerflag;
    }
    else if (actionflag == ActionPointerUp) {
        m_holdingMask &= ~pointerflag;
        if (getNumPointersDown(m_holdingMask) == 1)
        {
            m_scaleAtPinchStart *= m_iconScale;
            m_iconScale = 1.f;
        }
    }

    // Handle a pinch event
    if (actionflag == ActionPointerDown)
    {
        if (getNumPointersDown(m_holdingMask) == 2)
        {
            m_pinchStart.first = m_pointerStates[0];
            m_pinchStart.second = m_pointerStates[1];
        }
    }
    else if (actionflag == ActionMove)
    {
        if (getNumPointersDown(m_holdingMask) == 2)
        {
            const touchState& t0 = m_pinchStart.first;
            const touchState& t1 = m_pinchStart.second;
            const float3 a0 = {static_cast<float>(t0.x),static_cast<float>(t0.y),0};
            const float3 a1 = {static_cast<float>(t1.x),static_cast<float>(t1.y),0};
            const float initialDist = length(a1 - a0);
            const touchState& u0 = m_pointerStates[0];
            const touchState& u1 = m_pointerStates[1];
            const float3 b0 = {static_cast<float>(u0.x),static_cast<float>(u0.y),0};
            const float3 b1 = {static_cast<float>(u1.x),static_cast<float>(u1.y),0};
            const float curDist = length(b1 - b0);
            if (initialDist != 0.f)
            {
                //m_iconScale = curDist / initialDist;
                const float dz = .02f * (curDist - initialDist);
                m_chassisPos.z = m_chassisPosAtTouch.z + dz;
            }
        }
    }

    if (m_holdingMask == 1)
    {
        if (getNumPointersDown(m_holdingMask) == 1)
        {
            if ((actionflag == ActionDown) || (actionflag == ActionMove))
            {
                //m_iconx = x;
                //m_icony = y;
                const int dx = x - m_lastTouchPoint.x;
                const int dy = y - m_lastTouchPoint.y;
                //m_chassisPos.x = m_chassisPosAtTouch.x + (.01f * (float)dx);
                m_chassisPos.y = m_chassisPosAtTouch.y - (.01f * (float)dy);

                m_chassisYaw = m_chassisYawAtTouch + (.1f * (float)dx);
            }
        }
    }

}

void TabletWindow::OnWheelEvent(double dx, double dy)
{
    m_chassisPos.z += .4f * dy;
}

void TabletWindow::onKeyEvent(int key, int codes, int action, int mods)
{
    m_luaScene.keypressed(key);

    switch (key)
    {
    default: break;

    case 258: // Tab in GLFW3
    case 9: // Tab in SDL2
        m_luaScene.ChangeScene(1);
        break;
    }
}
<|endoftext|>


<commit_before>/**
* @file ParticleFilterBallLocator.cpp
*
* @author Heinrich Mellmann
* Implementation of class ParticleFilterBallLocator
*/

#include "ParticleFilterBallLocator.h"

// Debug
#include "Tools/Debug/DebugRequest.h"
#include "Tools/Debug/DebugDrawings.h"
#include "Tools/Debug/DebugModify.h"
#include "Tools/Debug/DebugBufferedOutput.h"
#include "Tools/Debug/DebugDrawings3D.h"
#include "Tools/Debug/DebugImageDrawings.h"

// Tools
#include "Tools/Math/Probabilistics.h"
#include "Tools/Math/Geometry.h"

ParticleFilterBallLocator::ParticleFilterBallLocator()
{
  DEBUG_REQUEST_REGISTER("ParticleFilterBallLocator:draw_ball_on_field", "Draw the ball model on the field. Orange: Ball was seen.", false);
  DEBUG_REQUEST_REGISTER("ParticleFilterBallLocator:draw_samples", "Draw the sample set. Orange: Sample is moving.", false);
  DEBUG_REQUEST_REGISTER("ParticleFilterBallLocator:drawMotionUpdateBySpeed", "", false);
}

void ParticleFilterBallLocator::execute()
{
  getBallModel().reset();

  // update by motion model
  motionUpdate(theSampleSet, true);

  if(getBallPercept().ballWasSeen)
  {
    if(theSampleSet.size() > 0)
    {
      //
      updateByBallPercept(theSampleSet);

      //
      resampleGT07(theSampleSet, false);
    }//end if

    // replace a random particle
    if(perceptBuffer.isFull())
    {
      if(theSampleSet.size() < 20)
      {
        theSampleSet.push_back(generateNewSample());
      }
      else
      {
        int k = Math::random(theSampleSet.size());
        theSampleSet[k] = generateNewSample();
      }
    }
  }//end if

  // calculate the model
  if(!theSampleSet.empty())
  {
    Vector2<double> mean;
    Vector2<double> meanSpeed;
    double numberOfMovingBalls = 0;
    for (unsigned int i = 0; i < theSampleSet.size(); i++)
    {
      mean += theSampleSet[i].position;

      if(theSampleSet[i].moving)
      {
        meanSpeed += theSampleSet[i].speed;
        numberOfMovingBalls++;
      }
    }//end for
    mean /= theSampleSet.size();
    if(numberOfMovingBalls > 0)
      meanSpeed /= numberOfMovingBalls;

    // set the ball model
    if(getBallPercept().ballWasSeen)
      getBallModel().setFrameInfoWhenBallWasSeen(getFrameInfo());

    getBallModel().position = mean;
    //TODO change 13.03
    //getBallModel().speed = Vector2<double>();
    getBallModel().speed = meanSpeed;
    getBallModel().valid = true;

    updatePreviewModel();
  }//end if

  DEBUG_REQUEST("ParticleFilterBallLocator:draw_ball_on_field", drawBallModel(getBallModel()); );
  DEBUG_REQUEST("ParticleFilterBallLocator:draw_samples", drawSamples(theSampleSet); );

  // remember some data
  lastFrameInfo = getFrameInfo();
  lastRobotOdometry = getOdometryData();

  if (getBallPercept().ballWasSeen)
    perceptBuffer.add(getBallPercept());
}//end execute

void ParticleFilterBallLocator::updateByBallPercept(SampleSet& sampleSet)
{
  //unsigned int maxNumberOfParticles = 20;

  const double seenDistance = getBallPercept().bearingBasedOffsetOnField.abs();
  const double seenAngle = getBallPercept().bearingBasedOffsetOnField.angle();
  const double cameraHeight  = getCameraMatrix().translation.z;

  for (unsigned int i = 0; i < sampleSet.size(); i++)
  {
    Sample& sample(sampleSet[i]);

    const double expectedDistance = sample.position.abs();
    const double expectedAngle = sample.position.angle();

    sample.likelihood *= computeDistanceWeighting(seenDistance, expectedDistance, cameraHeight, parameters.sigmaDistance, 1.0);
    sample.likelihood *= computeAngleWeighting(seenAngle, expectedAngle, parameters.sigmaAngle, 1.0);
  }//end for
}//end updateByBallPercept

ParticleFilterBallLocator::Sample ParticleFilterBallLocator::generateNewSample()
{
  Sample newSample;

  if(perceptBuffer.getNumberOfEntries() > 1)
  {
    int a = Math::random((int)(perceptBuffer.getNumberOfEntries()));
    int b = Math::random((int)(perceptBuffer.getNumberOfEntries()));
    // a and b must be different, of course:
    if(a == b)
    {
      if(a != 0)
        a--;
      else
        a++;
    }

    const double timeDelta = perceptBuffer[b].frameInfoWhenBallWasSeen.getTimeInSeconds() - perceptBuffer[a].frameInfoWhenBallWasSeen.getTimeInSeconds();
    // a must be older than b:
    if(timeDelta < 0)
    {
      swap(a,b);
    }

    newSample.position = perceptBuffer[b].bearingBasedOffsetOnField;

    Vector2<double> speed = (perceptBuffer[b].bearingBasedOffsetOnField - perceptBuffer[a].bearingBasedOffsetOnField)*
            fabs(timeDelta);

    //newSample.moving = Math::random() > 0.5;

    if(speed.abs() > parameters.speedWehnParticleIsMoving)
    {
      newSample.moving = true;
      newSample.speed  = speed;

    }
  }//end if

  return newSample;
}//end generateNewSample

void ParticleFilterBallLocator::motionUpdate(SampleSet& sampleSet, bool noise)
{
  //double motionNoiseDistance = 10;
  const Pose2D odometryDelta = lastRobotOdometry - getOdometryData();
  // time elapsed sinse last execution
  const double timeDelta = getFrameInfo().getTimeInSeconds() - lastFrameInfo.getTimeInSeconds();
  ASSERT(timeDelta > 0);

  // update the buffer by odometry
  for (int i = 0; i < perceptBuffer.getNumberOfEntries(); i++)
  {
    BallPercept& ball = perceptBuffer[i];
    ball.bearingBasedOffsetOnField = odometryDelta*ball.bearingBasedOffsetOnField;
  }//end for

  for (unsigned int i = 0; i < sampleSet.size(); i++)
  {
    Sample& sample(sampleSet[i]);

    if(sample.moving)
    {

      Vector2<double> position = sample.position;

      // simple motion model
      sample.position += sample.speed*timeDelta; //TODO: add some noise

      Vector2<double> positionWithSpeed = sample.position+sample.speed;
      DEBUG_REQUEST("ParticleFilterBallLocator:drawMotionUpdateBySpeed", drawMotionUpdateBySpeed(position, positionWithSpeed););

      // apply odometry to the speed of the ball
      sample.speed = sample.speed.rotate(odometryDelta.rotation);

      // add some noise to the speed
      if(noise)
      {
        const double velocityVariance = sample.speed.abs()*parameters.velocityNoiseFactor;
        sample.speed.x += Math::sampleTriangularDistribution(velocityVariance);
        sample.speed.y += Math::sampleTriangularDistribution(velocityVariance);
      }

      // add some linear friction
      sample.speed *= parameters.frictionCoefficiant*timeDelta;
    }//end if

    // apply odometry to the position
    sample.position = odometryDelta * sample.position;

    if(noise)
    {
      const double odometryVariance = odometryDelta.translation.abs()*parameters.odometryNoiseFactor;

      sample.position.x += Math::sampleTriangularDistribution(odometryVariance);
      sample.position.y += Math::sampleTriangularDistribution(odometryVariance);
    }
  }//end for
}//end updateByOdometry

void ParticleFilterBallLocator::resampleGT07(SampleSet& sampleSet, bool noise)
{
  // parameters
  double processNoiseDistance = 5;
  MODIFY("ParticleFilterBallLocator:processNoiseDistance",processNoiseDistance);
  double resamplingThreshhold = 0.0;
  MODIFY("ParticleFilterBallLocator:resamplingThreshhold",resamplingThreshhold);

  //
  double totalWeighting = 0;
  for(unsigned int i = 0; i < sampleSet.size(); i++)
  {
    totalWeighting += sampleSet[i].likelihood;
  }//end for
  const double averageWeighting = totalWeighting / (double)sampleSet.size();

  if (averageWeighting > 0) //wrong parameters
    PLOT("ParticleFilterBallLocator:averageWeighting", averageWeighting);

  // copy the samples
  // TODO: use memcopy?
  std::vector<Sample> oldSampleSet = sampleSet;

  totalWeighting += resamplingThreshhold*oldSampleSet.size();
  for(unsigned int i = 0; i < oldSampleSet.size(); i++)
  {
    oldSampleSet[i].likelihood += resamplingThreshhold;
    oldSampleSet[i].likelihood /= totalWeighting; // normalize
  }//end for

  // resample 10% of particles
  //int numberOfPartiklesToResample = (int)(((double)sampleSet.size())*0.1+0.5);

  double sum = -Math::random();
  unsigned int count = 0;

  //unsigned int m = 0;  // Zaehler durchs Ausgangs-Set
  unsigned int n = 0;  // Zaehler durchs Ziel-Set

  for(unsigned int m = 0; m < sampleSet.size(); m++)
  {
    sum += oldSampleSet[m].likelihood * oldSampleSet.size();

    // select the particle to copy
    while(count < sum && count < oldSampleSet.size())
    {
      if (n >= oldSampleSet.size()) break;

      // copy the selected particle
      sampleSet[n] = oldSampleSet[m];
      n++;
      count++;
    }//end while
  }//end for
}//end resampleGT07

inline double ParticleFilterBallLocator::computeAngleWeighting(
                                double measuredAngle,
                                double expectedAngle,
                                double standardDeviation,
                                double bestPossibleWeighting) const
{
  // TODO: normalize?
  double angleDif = Math::normalize(expectedAngle - measuredAngle);
  return Math::gaussianProbability(angleDif, standardDeviation) / bestPossibleWeighting;
}//end computeAngleWeighting

inline double ParticleFilterBallLocator::computeDistanceWeighting(
                                double measuredDistance,
                                double expectedDistance,
                                double cameraZ,
                                double standardDeviation,
                                double bestPossibleWeighting) const
{
  const double measuredDistanceAsAngle = atan2(measuredDistance, cameraZ);
  const double expectedDistanceAsAngle = atan2(expectedDistance, cameraZ);

  // TODO: normalize?
  double angleDif = Math::normalize(expectedDistanceAsAngle - measuredDistanceAsAngle);
  return Math::gaussianProbability(angleDif, standardDeviation) / bestPossibleWeighting;
}//end computeDistanceWeighting

void ParticleFilterBallLocator::updatePreviewModel()
{
  const Pose3D& lFoot = getKinematicChain().theLinks[KinematicChain::LFoot].M;
  const Pose3D& rFoot = getKinematicChain().theLinks[KinematicChain::RFoot].M;

  Pose2D lFootPose(lFoot.rotation.getZAngle(), lFoot.translation.x, lFoot.translation.y);
  Pose2D rFootPose(rFoot.rotation.getZAngle(), rFoot.translation.x, rFoot.translation.y);

  Vector2<double> ballLeftFoot  = lFootPose/getBallModel().position;
  Vector2<double> ballRightFoot = rFootPose/getBallModel().position;

  getBallModel().positionPreview = getMotionStatus().plannedMotion.hip / getBallModel().position;
  getBallModel().positionPreviewInLFoot = getMotionStatus().plannedMotion.lFoot / ballLeftFoot;
  getBallModel().positionPreviewInRFoot = getMotionStatus().plannedMotion.rFoot / ballRightFoot;
}//end updatePreviewModel

void ParticleFilterBallLocator::drawSamples(const SampleSet& sampleSet) const
{
  FIELD_DRAWING_CONTEXT;

  // plot samples
  for (unsigned int i = 0; i < sampleSet.size(); i++)
  {
    if(sampleSet[i].moving)
      PEN("FF9900", 5); //orange
    else
      PEN("0099FF", 5); //blue

    const Vector2<double>& pos = sampleSet[i].position;
    FILLOVAL( pos.x, pos.y, 10, 10);

    if(sampleSet[i].moving)
    {
      const Vector2<double>& speed = sampleSet[i].speed;
      ARROW( pos.x, pos.y, pos.x + speed.x, pos.y + speed.y);
    }//end if
  }//end for
}//end drawSamples

void ParticleFilterBallLocator::drawBallModel(const BallModel& ballModel) const
{

  const double timeDelta = getFrameInfo().getTimeInSeconds() - lastFrameInfo.getTimeInSeconds();
  ASSERT(timeDelta > 0);

  FIELD_DRAWING_CONTEXT;

  if(ballModel.valid)
  {
    if(getBallPercept().ballWasSeen)
      PEN("FF9900", 20); //orange
    else
      PEN("0099FF", 20); //blue
  }
  else
    PEN("999999", 20); //grey

  CIRCLE( ballModel.position.x, ballModel.position.y, getFieldInfo().ballRadius-10);
  ARROW( ballModel.position.x, ballModel.position.y,
         ballModel.position.x+ballModel.speed.x*timeDelta,
         ballModel.position.y+ballModel.speed.y*timeDelta);

  // draw the distribution
  /*
  PEN("00FFFF", 20);
  OVAL(getBallModel().position.x, getBallModel().position.y, Px[0][0], Py[0][0]);
  PEN("FF00FF", 20);
  OVAL(getBallModel().position.x, getBallModel().position.y, Px[1][1], Py[1][1]);
  */

  PEN("999999", 10); //grey
  for(int i=1; i <= BALLMODEL_MAX_FUTURE_SECONDS; i++)
  {
    CIRCLE(ballModel.futurePosition[i].x, ballModel.futurePosition[i].y, getFieldInfo().ballRadius-5);
  }
}//end drawBallModel

void ParticleFilterBallLocator::drawMotionUpdateBySpeed(const Vector2<double> position, const Vector2<double> positionWithSpeed) const
{

    FIELD_DRAWING_CONTEXT;

    PEN("999999", 8); //grey

    CIRCLE( position.x, position.y, getFieldInfo().ballRadius-10);
    LINE( position.x, position.y,
           positionWithSpeed.x,
           positionWithSpeed.y);
    CIRCLE( positionWithSpeed.x, positionWithSpeed.y, getFieldInfo().ballRadius-10);

}//end drawMotionUpdateBySpeed
<commit_msg>- some other fixes<commit_after>/**
* @file ParticleFilterBallLocator.cpp
*
* @author Heinrich Mellmann
* Implementation of class ParticleFilterBallLocator
*/

#include "ParticleFilterBallLocator.h"

// Debug
#include "Tools/Debug/DebugRequest.h"
#include "Tools/Debug/DebugDrawings.h"
#include "Tools/Debug/DebugModify.h"
#include "Tools/Debug/DebugBufferedOutput.h"
#include "Tools/Debug/DebugDrawings3D.h"
#include "Tools/Debug/DebugImageDrawings.h"

// Tools
#include "Tools/Math/Probabilistics.h"
#include "Tools/Math/Geometry.h"

ParticleFilterBallLocator::ParticleFilterBallLocator()
{
  DEBUG_REQUEST_REGISTER("ParticleFilterBallLocator:draw_ball_on_field", "Draw the ball model on the field. Orange: Ball was seen.", false);
  DEBUG_REQUEST_REGISTER("ParticleFilterBallLocator:draw_samples", "Draw the sample set. Orange: Sample is moving.", false);
  DEBUG_REQUEST_REGISTER("ParticleFilterBallLocator:drawMotionUpdateBySpeed", "", false);
}

void ParticleFilterBallLocator::execute()
{
  getBallModel().reset();

  // update by motion model
  motionUpdate(theSampleSet, true);

  if(getBallPercept().ballWasSeen)
  {
    if(theSampleSet.size() > 0)
    {
      //
      updateByBallPercept(theSampleSet);

      //
      resampleGT07(theSampleSet, false);
    }//end if

    // replace a random particle
    if(perceptBuffer.isFull())
    {
      if(theSampleSet.size() < 20)
      {
        theSampleSet.push_back(generateNewSample());
      }
      else
      {
        int k = Math::random(theSampleSet.size());
        theSampleSet[k] = generateNewSample();
      }
    }
  }//end if

  // calculate the model
  if(!theSampleSet.empty())
  {
    Vector2<double> mean;
    Vector2<double> meanSpeed;
    double numberOfMovingSamples = 0;
    for (unsigned int i = 0; i < theSampleSet.size(); i++)
    {
      mean += theSampleSet[i].position;

      if(theSampleSet[i].moving)
      {
        meanSpeed += theSampleSet[i].speed;
        numberOfMovingSamples++;
      }

    }//end for

    double meanSpeedAbs = meanSpeed.abs();
    MODIFY("ParticleFilterBallLocator:meanSpeadAbs", meanSpeedAbs);
    MODIFY("ParticleFilterBallLocator:numberOfMovingSamples", numberOfMovingSamples);

    mean /= theSampleSet.size();
    if(numberOfMovingSamples > 1)
      meanSpeed /= numberOfMovingSamples;

    // set the ball model
    if(getBallPercept().ballWasSeen)
      getBallModel().setFrameInfoWhenBallWasSeen(getFrameInfo());

    getBallModel().position = mean;
    //TODO change 13.03
    //getBallModel().speed = Vector2<double>();
    getBallModel().speed = meanSpeed*100;
    getBallModel().valid = true;

    updatePreviewModel();
  }//end if

  DEBUG_REQUEST("ParticleFilterBallLocator:draw_ball_on_field", drawBallModel(getBallModel()); );
  DEBUG_REQUEST("ParticleFilterBallLocator:draw_samples", drawSamples(theSampleSet); );

  // remember some data
  lastFrameInfo = getFrameInfo();
  lastRobotOdometry = getOdometryData();

  if (getBallPercept().ballWasSeen)
    perceptBuffer.add(getBallPercept());
}//end execute

void ParticleFilterBallLocator::updateByBallPercept(SampleSet& sampleSet)
{
  //unsigned int maxNumberOfParticles = 20;

  const double seenDistance = getBallPercept().bearingBasedOffsetOnField.abs();
  const double seenAngle = getBallPercept().bearingBasedOffsetOnField.angle();
  const double cameraHeight  = getCameraMatrix().translation.z;

  for (unsigned int i = 0; i < sampleSet.size(); i++)
  {
    Sample& sample(sampleSet[i]);

    const double expectedDistance = sample.position.abs();
    const double expectedAngle = sample.position.angle();

    sample.likelihood *= computeDistanceWeighting(seenDistance, expectedDistance, cameraHeight, parameters.sigmaDistance, 1.0);
    sample.likelihood *= computeAngleWeighting(seenAngle, expectedAngle, parameters.sigmaAngle, 1.0);
  }//end for
}//end updateByBallPercept

ParticleFilterBallLocator::Sample ParticleFilterBallLocator::generateNewSample()
{
  Sample newSample;

  if(perceptBuffer.getNumberOfEntries() > 1)
  {
    int a = Math::random((int)(perceptBuffer.getNumberOfEntries()));
    int b = Math::random((int)(perceptBuffer.getNumberOfEntries()));
    // a and b must be different, of course:
    if(a == b)
    {
      if(a != 0)
        a--;
      else
        a++;
    }

    const double timeDelta = perceptBuffer[b].frameInfoWhenBallWasSeen.getTimeInSeconds() - perceptBuffer[a].frameInfoWhenBallWasSeen.getTimeInSeconds();
    // a must be older than b:
    if(timeDelta < 0)
    {
      swap(a,b);
    }

    newSample.position = perceptBuffer[b].bearingBasedOffsetOnField;

    Vector2<double> speed = (perceptBuffer[b].bearingBasedOffsetOnField - perceptBuffer[a].bearingBasedOffsetOnField)*
            fabs(timeDelta);

    //newSample.moving = Math::random() > 0.5;

    if(speed.abs() > parameters.speedWehnParticleIsMoving)
    {
      newSample.moving = true;
      newSample.speed  = speed;
    } else {
      newSample.moving = false;
    }
  }//end if

  return newSample;
}//end generateNewSample

void ParticleFilterBallLocator::motionUpdate(SampleSet& sampleSet, bool noise)
{
  //double motionNoiseDistance = 10;
  const Pose2D odometryDelta = lastRobotOdometry - getOdometryData();
  // time elapsed sinse last execution
  const double timeDelta = getFrameInfo().getTimeInSeconds() - lastFrameInfo.getTimeInSeconds();
  ASSERT(timeDelta > 0);

  // update the buffer by odometry
  for (int i = 0; i < perceptBuffer.getNumberOfEntries(); i++)
  {
    BallPercept& ball = perceptBuffer[i];
    ball.bearingBasedOffsetOnField = odometryDelta*ball.bearingBasedOffsetOnField;
  }//end for

  for (unsigned int i = 0; i < sampleSet.size(); i++)
  {
    Sample& sample(sampleSet[i]);

    if(sample.moving)
    {

      Vector2<double> position = sample.position;

      // simple motion model
      sample.position += sample.speed*timeDelta; //TODO: add some noise

      Vector2<double> positionWithSpeed = sample.position+sample.speed;
      DEBUG_REQUEST("ParticleFilterBallLocator:drawMotionUpdateBySpeed", drawMotionUpdateBySpeed(position, positionWithSpeed););

      // apply odometry to the speed of the ball
      sample.speed = sample.speed.rotate(odometryDelta.rotation);

      // add some noise to the speed
      if(noise)
      {
        const double velocityVariance = sample.speed.abs()*parameters.velocityNoiseFactor;
        sample.speed.x += Math::sampleTriangularDistribution(velocityVariance);
        sample.speed.y += Math::sampleTriangularDistribution(velocityVariance);
      }

      // add some linear friction
      sample.speed *= parameters.frictionCoefficiant*timeDelta;
    }//end if

    // apply odometry to the position
    sample.position = odometryDelta * sample.position;

    if(noise)
    {
      const double odometryVariance = odometryDelta.translation.abs()*parameters.odometryNoiseFactor;

      sample.position.x += Math::sampleTriangularDistribution(odometryVariance);
      sample.position.y += Math::sampleTriangularDistribution(odometryVariance);
    }
  }//end for
}//end updateByOdometry

void ParticleFilterBallLocator::resampleGT07(SampleSet& sampleSet, bool noise)
{
  // parameters
  double processNoiseDistance = 5;
  MODIFY("ParticleFilterBallLocator:processNoiseDistance",processNoiseDistance);
  double resamplingThreshhold = 0.0;
  MODIFY("ParticleFilterBallLocator:resamplingThreshhold",resamplingThreshhold);

  //
  double totalWeighting = 0;
  for(unsigned int i = 0; i < sampleSet.size(); i++)
  {
    totalWeighting += sampleSet[i].likelihood;
  }//end for
  const double averageWeighting = totalWeighting / (double)sampleSet.size();

  if (averageWeighting > 0) //wrong parameters
    PLOT("ParticleFilterBallLocator:averageWeighting", averageWeighting);

  // copy the samples
  // TODO: use memcopy?
  std::vector<Sample> oldSampleSet = sampleSet;

  totalWeighting += resamplingThreshhold*oldSampleSet.size();
  for(unsigned int i = 0; i < oldSampleSet.size(); i++)
  {
    oldSampleSet[i].likelihood += resamplingThreshhold;
    oldSampleSet[i].likelihood /= totalWeighting; // normalize
  }//end for

  // resample 10% of particles
  //int numberOfPartiklesToResample = (int)(((double)sampleSet.size())*0.1+0.5);

  double sum = -Math::random();
  unsigned int count = 0;

  //unsigned int m = 0;  // Zaehler durchs Ausgangs-Set
  unsigned int n = 0;  // Zaehler durchs Ziel-Set

  for(unsigned int m = 0; m < sampleSet.size(); m++)
  {
    sum += oldSampleSet[m].likelihood * oldSampleSet.size();

    // select the particle to copy
    while(count < sum && count < oldSampleSet.size())
    {
      if (n >= oldSampleSet.size()) break;

      // copy the selected particle
      sampleSet[n] = oldSampleSet[m];
      n++;
      count++;
    }//end while
  }//end for
}//end resampleGT07

inline double ParticleFilterBallLocator::computeAngleWeighting(
                                double measuredAngle,
                                double expectedAngle,
                                double standardDeviation,
                                double bestPossibleWeighting) const
{
  // TODO: normalize?
  double angleDif = Math::normalize(expectedAngle - measuredAngle);
  return Math::gaussianProbability(angleDif, standardDeviation) / bestPossibleWeighting;
}//end computeAngleWeighting

inline double ParticleFilterBallLocator::computeDistanceWeighting(
                                double measuredDistance,
                                double expectedDistance,
                                double cameraZ,
                                double standardDeviation,
                                double bestPossibleWeighting) const
{
  const double measuredDistanceAsAngle = atan2(measuredDistance, cameraZ);
  const double expectedDistanceAsAngle = atan2(expectedDistance, cameraZ);

  // TODO: normalize?
  double angleDif = Math::normalize(expectedDistanceAsAngle - measuredDistanceAsAngle);
  return Math::gaussianProbability(angleDif, standardDeviation) / bestPossibleWeighting;
}//end computeDistanceWeighting

void ParticleFilterBallLocator::updatePreviewModel()
{
  const Pose3D& lFoot = getKinematicChain().theLinks[KinematicChain::LFoot].M;
  const Pose3D& rFoot = getKinematicChain().theLinks[KinematicChain::RFoot].M;

  Pose2D lFootPose(lFoot.rotation.getZAngle(), lFoot.translation.x, lFoot.translation.y);
  Pose2D rFootPose(rFoot.rotation.getZAngle(), rFoot.translation.x, rFoot.translation.y);

  Vector2<double> ballLeftFoot  = lFootPose/getBallModel().position;
  Vector2<double> ballRightFoot = rFootPose/getBallModel().position;

  getBallModel().positionPreview = getMotionStatus().plannedMotion.hip / getBallModel().position;
  getBallModel().positionPreviewInLFoot = getMotionStatus().plannedMotion.lFoot / ballLeftFoot;
  getBallModel().positionPreviewInRFoot = getMotionStatus().plannedMotion.rFoot / ballRightFoot;
}//end updatePreviewModel

void ParticleFilterBallLocator::drawSamples(const SampleSet& sampleSet) const
{
  FIELD_DRAWING_CONTEXT;

  // plot samples
  for (unsigned int i = 0; i < sampleSet.size(); i++)
  {
    if(sampleSet[i].moving)
      PEN("FF9900", 5); //orange
    else
      PEN("0099FF", 5); //blue

    const Vector2<double>& pos = sampleSet[i].position;
    FILLOVAL( pos.x, pos.y, 10, 10);

    if(sampleSet[i].moving)
    {
      const Vector2<double>& speed = sampleSet[i].speed;
      ARROW( pos.x, pos.y, pos.x + speed.x, pos.y + speed.y);
    }//end if
  }//end for
}//end drawSamples

void ParticleFilterBallLocator::drawBallModel(const BallModel& ballModel) const
{

  const double timeDelta = getFrameInfo().getTimeInSeconds() - lastFrameInfo.getTimeInSeconds();
  ASSERT(timeDelta > 0);

  FIELD_DRAWING_CONTEXT;

  if(ballModel.valid)
  {
    if(getBallPercept().ballWasSeen)
      PEN("FF9900", 20); //orange
    else
      PEN("0099FF", 20); //blue
  }
  else
    PEN("999999", 20); //grey

  CIRCLE( ballModel.position.x, ballModel.position.y, getFieldInfo().ballRadius-10);
  ARROW( ballModel.position.x, ballModel.position.y,
         ballModel.position.x+ballModel.speed.x*timeDelta,
         ballModel.position.y+ballModel.speed.y*timeDelta);

  // draw the distribution
  /*
  PEN("00FFFF", 20);
  OVAL(getBallModel().position.x, getBallModel().position.y, Px[0][0], Py[0][0]);
  PEN("FF00FF", 20);
  OVAL(getBallModel().position.x, getBallModel().position.y, Px[1][1], Py[1][1]);
  */

  PEN("999999", 10); //grey
  for(int i=1; i <= BALLMODEL_MAX_FUTURE_SECONDS; i++)
  {
    CIRCLE(ballModel.futurePosition[i].x, ballModel.futurePosition[i].y, getFieldInfo().ballRadius-5);
  }
}//end drawBallModel

void ParticleFilterBallLocator::drawMotionUpdateBySpeed(const Vector2<double> position, const Vector2<double> positionWithSpeed) const
{

    FIELD_DRAWING_CONTEXT;

    PEN("999999", 8); //grey

    CIRCLE( position.x, position.y, getFieldInfo().ballRadius-10);
    LINE( position.x, position.y,
           positionWithSpeed.x,
           positionWithSpeed.y);
    CIRCLE( positionWithSpeed.x, positionWithSpeed.y, getFieldInfo().ballRadius-10);

}//end drawMotionUpdateBySpeed
<|endoftext|>


<commit_before>#include <albert/scraper.hpp>

#include <boost/program_options.hpp>

#include <vector>
#include <iostream>

#include <supermarx/api/client.hpp>

struct cli_options
{
	std::string api_host;
	unsigned int ratelimit;
};

int read_options(cli_options& opt, int argc, char** argv)
{
	boost::program_options::options_description o_general("General options");
	o_general.add_options()
			("help,h", "display this message")
			("api,a", boost::program_options::value(&opt.api_host), "api host to send results to")
			("ratelimit,r", boost::program_options::value(&opt.ratelimit), "minimal time between each request in milliseconds (default: 5000)");

	boost::program_options::variables_map vm;
	boost::program_options::positional_options_description pos;

	boost::program_options::options_description options("Allowed options");
	options.add(o_general);

	try
	{
		boost::program_options::store(boost::program_options::command_line_parser(argc, argv).options(options).positional(pos).run(), vm);
	} catch(boost::program_options::unknown_option &e)
	{
		std::cerr << "Unknown option --" << e.get_option_name() << ", see --help." << std::endl;
		return EXIT_FAILURE;
	}

	try
	{
		boost::program_options::notify(vm);
	} catch(const boost::program_options::required_option &e)
	{
		std::cerr << "You forgot this: " << e.what() << std::endl;
		return EXIT_FAILURE;
	}

	if(vm.count("help"))
	{
		std::cout
				<< "SuperMarx scraper for the Albert Heijn inventory. [https://github.com/SuperMarx/albert]" << std::endl
				<< "Usage: ./albert [options]" << std::endl
				<< std::endl
				<< o_general;

		return EXIT_FAILURE;
	}

	if(!vm.count("api"))
		opt.api_host = "https://api.supermarx.nl";

	if(!vm.count("ratelimit"))
		opt.ratelimit = 5000;

	return EXIT_SUCCESS;
}

int main(int argc, char** argv)
{
	cli_options opt;

	int result = read_options(opt, argc, argv);
	if(result != EXIT_SUCCESS)
		return result;

	supermarx::api::client api(opt.api_host, "albert (libsupermarx-api)");

	supermarx::scraper s([&](supermarx::product const& product, supermarx::datetime retrieved_on, supermarx::confidence c) {
		std::cerr << "Product '" << product.name << "' [" << product.identifier << "] ";

		if(product.price == product.orig_price)
			std::cerr << product.price/100.0f << " EUR";
		else
			std::cerr << product.orig_price/100.0f << " EUR -> " << product.price/100.0f << " EUR";

		if(product.discount_amount > 1)
			std::cerr << " (at " << product.discount_amount << ')';

		std::cerr << std::endl;
		api.add_product(product, 1, retrieved_on, c);
	}, opt.ratelimit);

	s.scrape();

	return EXIT_SUCCESS;
}
<commit_msg>Added dry-run option<commit_after>#include <albert/scraper.hpp>

#include <boost/program_options.hpp>

#include <vector>
#include <iostream>

#include <supermarx/api/client.hpp>

struct cli_options
{
	std::string api_host;
	bool dry_run;
	unsigned int ratelimit;
};

int read_options(cli_options& opt, int argc, char** argv)
{
	boost::program_options::options_description o_general("General options");
	o_general.add_options()
			("help,h", "display this message")
			("api,a", boost::program_options::value(&opt.api_host), "api host to send results to")
			("dry-run,d", "does not send products to api when set")
			("ratelimit,r", boost::program_options::value(&opt.ratelimit), "minimal time between each request in milliseconds (default: 5000)");

	boost::program_options::variables_map vm;
	boost::program_options::positional_options_description pos;

	boost::program_options::options_description options("Allowed options");
	options.add(o_general);

	try
	{
		boost::program_options::store(boost::program_options::command_line_parser(argc, argv).options(options).positional(pos).run(), vm);
	} catch(boost::program_options::unknown_option &e)
	{
		std::cerr << "Unknown option --" << e.get_option_name() << ", see --help." << std::endl;
		return EXIT_FAILURE;
	}

	try
	{
		boost::program_options::notify(vm);
	} catch(const boost::program_options::required_option &e)
	{
		std::cerr << "You forgot this: " << e.what() << std::endl;
		return EXIT_FAILURE;
	}

	if(vm.count("help"))
	{
		std::cout
				<< "SuperMarx scraper for the Albert Heijn inventory. [https://github.com/SuperMarx/albert]" << std::endl
				<< "Usage: ./albert [options]" << std::endl
				<< std::endl
				<< o_general;

		return EXIT_FAILURE;
	}

	if(!vm.count("api"))
		opt.api_host = "https://api.supermarx.nl";

	opt.dry_run = vm.count("dry-run");

	if(!vm.count("ratelimit"))
		opt.ratelimit = 5000;

	return EXIT_SUCCESS;
}

int main(int argc, char** argv)
{
	cli_options opt;

	int result = read_options(opt, argc, argv);
	if(result != EXIT_SUCCESS)
		return result;

	supermarx::api::client api(opt.api_host, "albert (libsupermarx-api)");

	supermarx::scraper s([&](supermarx::product const& product, supermarx::datetime retrieved_on, supermarx::confidence c) {
		std::cerr << "Product '" << product.name << "' [" << product.identifier << "] ";

		if(product.price == product.orig_price)
			std::cerr << product.price/100.0f << " EUR";
		else
			std::cerr << product.orig_price/100.0f << " EUR -> " << product.price/100.0f << " EUR";

		if(product.discount_amount > 1)
			std::cerr << " (at " << product.discount_amount << ')';

		std::cerr << std::endl;

		if(!opt.dry_run)
			api.add_product(product, 1, retrieved_on, c);
	}, opt.ratelimit);

	s.scrape();

	return EXIT_SUCCESS;
}
<|endoftext|>