<commit_before>/* ************************************************
 * GEANT4 VCGLIB/CAD INTERFACE
 *
 * File:      CADMesh.hh
 *
 * Author:    Christopher M Poole,
 * Email:     mail@christopherpoole.net
 *
 * Date:      7th March, 2011
 **************************************************/

#ifndef CADMesh_HH
#define CADMesh_HH

// GEANT4 //
#include "G4String.hh"
#include "G4ThreeVector.hh"
#include "G4TessellatedSolid.hh"
#include "G4AssemblyVolume.hh"

#ifndef NOVCGLIB
// VCGLIB //
#include "vcg/simplex/vertex/base.h"
#include "vcg/simplex/vertex/component.h"
#include "vcg/simplex/face/base.h"
#include "vcg/simplex/face/component.h"

#include "vcg/complex/complex.h"
#endif

// TETGEN //
#ifndef NOTET
#include "tetgen.h"
#endif

#ifndef NOVCGLIB

class CADVertex;
class CADFace;

class CADUsedTypes: public vcg::UsedTypes< vcg::Use<CADVertex>::AsVertexType, vcg::Use<CADFace>::AsFaceType>{};

class CADVertex : public vcg::Vertex<CADUsedTypes, vcg::vertex::Coord3d, vcg::vertex::Normal3f> {};
class CADFace : public vcg::Face<CADUsedTypes, vcg::face::VertexRef> {};
class CADTriMesh : public vcg::tri::TriMesh< std::vector<CADVertex>, std::vector<CADFace> > {};
#endif

class CADMesh
{
// METHODS //
public:
    CADMesh(char * file, char * type, double units, G4ThreeVector offset, G4bool reverse);
    CADMesh(char * file, char * type, G4Material * material, double quality);
    CADMesh(char * file, char * type, G4Material * material, double quality, G4ThreeVector offset);
    CADMesh(char * file, char * type);
    CADMesh(char * file, char * type, G4Material * material);
    ~CADMesh();

#ifndef NOTET
public:
    G4AssemblyVolume * TetrahedralMesh();
    G4AssemblyVolume * GetAssembly() { return assembly; };

    int get_input_point_count() { return in.numberofpoints; };
    int get_output_point_count() { return out.numberofpoints; };
    int get_tetrahedron_count() { return out.numberoftetrahedra; };
#endif

#ifndef NOVCGLIB
public:
    G4VSolid* TessellatedMesh();
    G4TessellatedSolid * GetSolid() { return volume_solid; };

    G4String MeshName(){ return file_name_; };
    int MeshVertexNumber(){ return m.VertexNumber(); };
    //float MeshVolume(){ return m.Volume(); };

    void SetVerbose(int v){ verbose_ = v; };
#endif

private:
    G4ThreeVector GetTetPoint(G4int index_offset);

// VARS //
private:
#ifndef NOVCGLIB
    CADTriMesh m;
#endif

#ifndef NOTET
    tetgenio in, out;
    G4AssemblyVolume * assembly;
#endif

    G4TessellatedSolid * volume_solid;
    G4int verbose_;
    G4bool has_mesh_;
    G4bool has_solid_;

    G4String name_;
    G4double units_;
    G4ThreeVector offset_;
    G4bool reverse_;

    G4double quality_;

    G4Material * material_;

    char * file_name_;
    G4String file_type_;
};

#endif // CADMesh_HH

<commit_msg>added include for G4Material.hh<commit_after>/* ************************************************
 * GEANT4 VCGLIB/CAD INTERFACE
 *
 * File:      CADMesh.hh
 *
 * Author:    Christopher M Poole,
 * Email:     mail@christopherpoole.net
 *
 * Date:      7th March, 2011
 **************************************************/

#ifndef CADMesh_HH
#define CADMesh_HH

// GEANT4 //
#include "G4String.hh"
#include "G4ThreeVector.hh"
#include "G4TessellatedSolid.hh"
#include "G4AssemblyVolume.hh"
#include "G4Material.hh"

#ifndef NOVCGLIB
// VCGLIB //
#include "vcg/simplex/vertex/base.h"
#include "vcg/simplex/vertex/component.h"
#include "vcg/simplex/face/base.h"
#include "vcg/simplex/face/component.h"

#include "vcg/complex/complex.h"
#endif

// TETGEN //
#ifndef NOTET
#include "tetgen.h"
#endif

#ifndef NOVCGLIB

class CADVertex;
class CADFace;

class CADUsedTypes: public vcg::UsedTypes< vcg::Use<CADVertex>::AsVertexType, vcg::Use<CADFace>::AsFaceType>{};

class CADVertex : public vcg::Vertex<CADUsedTypes, vcg::vertex::Coord3d, vcg::vertex::Normal3f> {};
class CADFace : public vcg::Face<CADUsedTypes, vcg::face::VertexRef> {};
class CADTriMesh : public vcg::tri::TriMesh< std::vector<CADVertex>, std::vector<CADFace> > {};
#endif

class CADMesh
{
// METHODS //
public:
    CADMesh(char * file, char * type, double units, G4ThreeVector offset, G4bool reverse);
    CADMesh(char * file, char * type, G4Material * material, double quality);
    CADMesh(char * file, char * type, G4Material * material, double quality, G4ThreeVector offset);
    CADMesh(char * file, char * type);
    CADMesh(char * file, char * type, G4Material * material);
    ~CADMesh();

#ifndef NOTET
public:
    G4AssemblyVolume * TetrahedralMesh();
    G4AssemblyVolume * GetAssembly() { return assembly; };

    int get_input_point_count() { return in.numberofpoints; };
    int get_output_point_count() { return out.numberofpoints; };
    int get_tetrahedron_count() { return out.numberoftetrahedra; };
#endif

#ifndef NOVCGLIB
public:
    G4VSolid* TessellatedMesh();
    G4TessellatedSolid * GetSolid() { return volume_solid; };

    G4String MeshName(){ return file_name_; };
    int MeshVertexNumber(){ return m.VertexNumber(); };
    //float MeshVolume(){ return m.Volume(); };

    void SetVerbose(int v){ verbose_ = v; };
#endif

private:
    G4ThreeVector GetTetPoint(G4int index_offset);

// VARS //
private:
#ifndef NOVCGLIB
    CADTriMesh m;
#endif

#ifndef NOTET
    tetgenio in, out;
    G4AssemblyVolume * assembly;
#endif

    G4TessellatedSolid * volume_solid;
    G4int verbose_;
    G4bool has_mesh_;
    G4bool has_solid_;

    G4String name_;
    G4double units_;
    G4ThreeVector offset_;
    G4bool reverse_;

    G4double quality_;

    G4Material * material_;

    char * file_name_;
    G4String file_type_;
};

#endif // CADMesh_HH

<|endoftext|>


<commit_before>#ifndef __DARRAY_HPP__
#define __DARRAY_HPP__

#include "common.hpp"
#include <assert.h>
#include <sys/mman.h>

namespace hft {

/**
 * class DArray - Dinamically-allocated fixed-sized array
 *
 * This class is a wrapper of mmap. It supports hugepages [1], transparent
 * hugepags [2]. It guarantees to return page-aligned pointers. It has four
 * behaviors you can choose by defining their name:
 * - HFT_FORCEHUGE: every request allocs hugepages;
 * - HFT_FORCENOHUGE: every request allocs standard 4k pages (non-transparent)
 * - HFT_HUGE: every request of at least 2MB allocs hugepages while smaller
 *   requests use standard 4k pages;
 * - by default: every request of at least 2MB allocs 4k pages, but they are
 *   transparently defragmented into hugepages by khugepaged thanks to a call to
 *   madvice.
 *
 * [1] https://www.kernel.org/doc/html/latest/admin-guide/mm/hugetlbpage.html
 * [2] https://www.kernel.org/doc/html/latest/admin-guide/mm/transhuge.html
 *
 */
template <typename T> class DArray {
public:
  static constexpr int PROT = PROT_READ | PROT_WRITE;
  static constexpr int FLAGS = MAP_PRIVATE | MAP_ANONYMOUS;

private:
  size_t Size = 0;  // length of the array
  size_t Space = 0; // allocated space in memory
  T *Buffer = nullptr;

public:
  DArray<T>() = default;

  explicit DArray<T>(size_t length)
      : Size(length),
#ifdef HFT_FORCE_HUGETLBPAGE
        Space(((2097152 - 1) | (Size * sizeof(T) - 1)) + 1) {
#elif HFT_DISABLE_TRANSHUGE
        Space(((4096 - 1) | (Size * sizeof(T) - 1)) + 1) {
#else
        Space(((pagesize(Size) - 1) | (Size * sizeof(T) - 1)) + 1) {
#endif
    if (Space) {
#ifdef HFT_FORCE_HUGETLBPAGE
      void *mem = mmap(nullptr, Space, PROT, FLAGS | MAP_HUGETLB, -1, 0);
      assert(mem != MAP_FAILED && "mmap failed");
#elif HFT_DISABLE_TRANSHUGE
      void *mem = mmap(nullptr, Space, PROT, FLAGS, -1, 0);
      assert(mem != MAP_FAILED && "mmap failed");
#elif HFT_HUGETLBPAGE
      void *mem = Space >= 2097152
                      ? mmap(nullptr, Space, PROT, FLAGS | MAP_HUGETLB, -1, 0)
                      : mmap(nullptr, Space, PROT, FLAGS, -1, 0);
      assert(mem != MAP_FAILED && "mmap failed");
#else
    void *mem = mmap(nullptr, Space, PROT, FLAGS, -1, 0);
    assert(mem != MAP_FAILED && "mmap failed");

    if (Size >= 2097152) {
      int adv = madvise(mem, Space, MADV_HUGEPAGE);
      assert(adv == 0 && "madvise failed");
    }
#endif

      Buffer = static_cast<T *>(mem);
    }
  }

  DArray(DArray<T> &&oth)
      : Size(std::exchange(oth.Size, 0)), Space(std::exchange(oth.Space, 0)),
        Buffer(std::exchange(oth.Buffer, nullptr)) {}

  DArray<T> &operator=(DArray<T> &&oth) {
    swap(*this, oth);
    return *this;
  }

  ~DArray<T>() {
    if (Buffer != nullptr) {
      int result = munmap(Buffer, Space);
      assert(result == 0 && "mmunmap failed");
    }
  }

  friend void swap(DArray<T> &first, DArray<T> &second) noexcept {
    std::swap(first.Size, second.Size);
    std::swap(first.Space, second.Space);
    std::swap(first.Buffer, second.Buffer);
  }

  inline T *get() const { return Buffer; }

  inline T &operator[](size_t i) const { return Buffer[i]; };

  inline size_t size() const { return Size; }

  size_t bitCount() const { return sizeof(DArray<T>) * 8 + Space * 8; }

private:
  size_t pagesize(size_t space) {
    return (space >= 2097152 ? 2097152 : 4096) - 1;
  }
}; // namespace hft

} // namespace hft

#endif // __DARRAY_HPP__
<commit_msg>Bugfix<commit_after>#ifndef __DARRAY_HPP__
#define __DARRAY_HPP__

#include "common.hpp"
#include <assert.h>
#include <sys/mman.h>

namespace hft {

/**
 * class DArray - Dinamically-allocated fixed-sized array
 *
 * This class is a wrapper of mmap. It supports hugepages [1], transparent
 * hugepags [2]. It guarantees to return page-aligned pointers. It has four
 * behaviors you can choose by defining their name:
 * - HFT_FORCEHUGE: every request allocs hugepages;
 * - HFT_FORCENOHUGE: every request allocs standard 4k pages (non-transparent)
 * - HFT_HUGE: every request of at least 2MB allocs hugepages while smaller
 *   requests use standard 4k pages;
 * - by default: every request of at least 2MB allocs 4k pages, but they are
 *   transparently defragmented into hugepages by khugepaged thanks to a call to
 *   madvice.
 *
 * [1] https://www.kernel.org/doc/html/latest/admin-guide/mm/hugetlbpage.html
 * [2] https://www.kernel.org/doc/html/latest/admin-guide/mm/transhuge.html
 *
 */
template <typename T> class DArray {
public:
  static constexpr int PROT = PROT_READ | PROT_WRITE;
  static constexpr int FLAGS = MAP_PRIVATE | MAP_ANONYMOUS;

private:
  size_t Size = 0;  // length of the array
  size_t Space = 0; // allocated space in memory
  T *Buffer = nullptr;

public:
  DArray<T>() = default;

  explicit DArray<T>(size_t length)
      : Size(length),
#ifdef HFT_FORCE_HUGETLBPAGE
        Space(((2097152 - 1) | (Size * sizeof(T) - 1)) + 1) {
#elif HFT_DISABLE_TRANSHUGE
        Space(((4096 - 1) | (Size * sizeof(T) - 1)) + 1) {
#else
        Space(((pagesize(Size) - 1) | (Size * sizeof(T) - 1)) + 1) {
#endif
    if (Space) {
#ifdef HFT_FORCE_HUGETLBPAGE
      void *mem = mmap(nullptr, Space, PROT, FLAGS | MAP_HUGETLB, -1, 0);
      assert(mem != MAP_FAILED && "mmap failed");
#elif HFT_DISABLE_TRANSHUGE
      void *mem = mmap(nullptr, Space, PROT, FLAGS, -1, 0);
      assert(mem != MAP_FAILED && "mmap failed");
#elif HFT_HUGETLBPAGE
      void *mem = Space >= 2097152
                      ? mmap(nullptr, Space, PROT, FLAGS | MAP_HUGETLB, -1, 0)
                      : mmap(nullptr, Space, PROT, FLAGS, -1, 0);
      assert(mem != MAP_FAILED && "mmap failed");
#else
    void *mem = mmap(nullptr, Space, PROT, FLAGS, -1, 0);
    assert(mem != MAP_FAILED && "mmap failed");

    int adv = madvise(mem, Space, MADV_HUGEPAGE);
    assert(adv == 0 && "madvise failed");
#endif

      Buffer = static_cast<T *>(mem);
    }
  }

  DArray(DArray<T> &&oth)
      : Size(std::exchange(oth.Size, 0)), Space(std::exchange(oth.Space, 0)),
        Buffer(std::exchange(oth.Buffer, nullptr)) {}

  DArray<T> &operator=(DArray<T> &&oth) {
    swap(*this, oth);
    return *this;
  }

  ~DArray<T>() {
    if (Buffer != nullptr) {
      int result = munmap(Buffer, Space);
      assert(result == 0 && "mmunmap failed");
    }
  }

  friend void swap(DArray<T> &first, DArray<T> &second) noexcept {
    std::swap(first.Size, second.Size);
    std::swap(first.Space, second.Space);
    std::swap(first.Buffer, second.Buffer);
  }

  inline T *get() const { return Buffer; }

  inline T &operator[](size_t i) const { return Buffer[i]; };

  inline size_t size() const { return Size; }

  size_t bitCount() const { return sizeof(DArray<T>) * 8 + Space * 8; }

private:
  size_t pagesize(size_t space) {
    return (space >= 2097152 ? 2097152 : 4096) - 1;
  }
}; // namespace hft

} // namespace hft

#endif // __DARRAY_HPP__
<|endoftext|>


<commit_before>/** \copyright
 * Copyright (c) 2018, Balazss Racz
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are  permitted provided that the following conditions are met:
 *
 *  - Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 *  - Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 * \file Pic32mxUsbCdcDevice.cxx
 *
 * This file implements a USB CDC device driver layer using PIC32 Harmony USB
 * middleware driver.
 *
 * @author Balazs Racz
 * @date 9 Sep 2018
 */

#include "freertos_drivers/common/Serial.hxx"
#include "system_config.h"

extern "C" {
#include "driver/usb/usbfs/drv_usbfs.h"
#include "usb/usb_device.h"
#include "usb/usb_device_cdc.h"

extern const DRV_USBFS_INIT drvUSBFSInit;
extern const USB_DEVICE_INIT usbDevInitData;
} // extern "C"

#include "os/os.h"

/** Device driver for PIC32MX USB virtual serial port. */
class Pic32mxCdc : public Serial
{
public:
    /** Constructor.
     * @param name name of this device instance in the file system
     * @param interrupt interrupt number used by the device
     */
    Pic32mxCdc(const char *name);

    /** Destructor.
     */
    ~Pic32mxCdc()
    {
    }

    /** handle an interrupt.
     */
    void interrupt_handler()
    {
        DRV_USBFS_Tasks_ISR(drvUSBObject);
    }

    /** Background thread for doing USB device driver tasks. Must be called
     * aobut once per second in order to initialize, register with the USB
     * hosts, etc. */
    void device_tasks();

private:

    /** Function to try and transmit a character.  Unused by this device driver.
     */
    void tx_char() override {}

    void enable() override {} /**< function to enable device */
    void disable() override {} /**< function to disable device */

#if 0
    /** Write to a file or device.
     * @param file file reference for this device
     * @param buf location to find write data
     * @param count number of bytes to write
     * @return number of bytes written upon success, -1 upon failure with errno containing the cause
     */
    ssize_t write(File *file, const void *buf, size_t count) OVERRIDE;

    /** Device select method. Default impementation returns true.
     * @param file reference to the file
     * @param mode FREAD for read active, FWRITE for write active, 0 for
     *        exceptions
     * @return true if active, false if inactive
     */
    bool select(File* file, int mode) OVERRIDE;

    /** Handles CDC driver notifications related to control and setup of the
     * device.  This is called from within interrupt context.
     * @param data private data
     * @param event identifies the event we are being notified about
     * @param msg_param event-specific value
     * @param msg_data event-specific data
     * @return return value is event specific
     */
    static uint32_t control_callback(void *data, unsigned long event,
                                     unsigned long msg_param, void *msg_data);

    /** Handles CDC driver notifications related to reception.
     * This is called from within interrupt context.
     * @param data private data
     * @param event identifies the event we are being notified about
     * @param msg_param event-specific value
     * @param msg_data event-specific data
     * @return return value is event specific
     */
    static uint32_t rx_callback(void *data, unsigned long event,
                                unsigned long msg_param, void *msg_data);

    /** Handles CDC driver notifications related to transmission.
     * This is called from within interrupt context.
     * @param data private data
     * @param event identifies the event we are being notified about
     * @param msg_param event-specific value
     * @param msg_data event-specific data
     * @return return value is event specific
     */
    static uint32_t tx_callback(void *data, unsigned long event,
                                unsigned long msg_param, void *msg_data);

#endif

    //tUSBDCDCDevice usbdcdcDevice; /**< CDC serial device instance */
    SYS_MODULE_OBJ  drvUSBObject;
    SYS_MODULE_OBJ  usbDevObject0;
    uint32_t interrupt; /**< interrupt number for device */
    bool connected; /**< connection status */
    bool enabled; /**< enabled status */
    int woken; /**< task woken metadata for ISR */
    bool txPending; /**< true if a transmission is in progress or pending */
    SelectInfo selInfoWr; /**< Metadata for select() logic */

    /** Default constructor.
     */
    Pic32mxCdc();

    DISALLOW_COPY_AND_ASSIGN(Pic32mxCdc);
};

// Instantiates the driver.
Pic32mxCdc usbCdc0("/dev/serUSB0");

void* usb_device_task(void*) {
    while (1) {
        usbCdc0.device_tasks();
        vTaskDelay(configTICK_RATE_HZ / 20);
    }
}

extern "C" {
void usb_interrupt() {
    usbCdc0.interrupt_handler();
}
}

Pic32mxCdc::Pic32mxCdc(const char *name)
    : Serial(name)
{
    drvUSBObject = DRV_USBFS_Initialize(DRV_USBFS_INDEX_0, (SYS_MODULE_INIT *) &drvUSBFSInit);

    /* Set priority of USB interrupt source. This has to be within kernel
     * interrupt priority. */
    SYS_INT_VectorPrioritySet(INT_VECTOR_USB1, INT_PRIORITY_LEVEL2);

    /* Set Sub-priority of USB interrupt source */
    SYS_INT_VectorSubprioritySet(INT_VECTOR_USB1, INT_SUBPRIORITY_LEVEL0);

    /* Initialize Middleware */
    /* Initialize the USB device layer */
    usbDevObject0 = USB_DEVICE_Initialize (USB_DEVICE_INDEX_0 , ( SYS_MODULE_INIT* ) & usbDevInitData);

    os_thread_t tid;
    os_thread_create(&tid, "usb_tasks", 1, 512, &usb_device_task, nullptr);
}

void Pic32mxCdc::device_tasks() {
    DRV_USBFS_Tasks(drvUSBObject);
    USB_DEVICE_Tasks(usbDevObject0);
}
<commit_msg>Implements attach state machine and callback handlers for the USB stack.<commit_after>/** \copyright
 * Copyright (c) 2018, Balazss Racz
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are  permitted provided that the following conditions are met:
 *
 *  - Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 *  - Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 * \file Pic32mxUsbCdcDevice.cxx
 *
 * This file implements a USB CDC device driver layer using PIC32 Harmony USB
 * middleware driver.
 *
 * @author Balazs Racz
 * @date 9 Sep 2018
 */

#include "freertos_drivers/common/Serial.hxx"
#include "system_config.h"

extern "C" {
#include "driver/usb/usbfs/drv_usbfs.h"
#include "usb/usb_device.h"
#include "usb/usb_device_cdc.h"

extern const DRV_USBFS_INIT drvUSBFSInit;
extern const USB_DEVICE_INIT usbDevInitData;
} // extern "C"

#include "os/os.h"

/** Device driver for PIC32MX USB virtual serial port. */
class Pic32mxCdc : public Serial
{
public:
    /** Constructor.
     * @param name name of this device instance in the file system
     * @param interrupt interrupt number used by the device
     */
    Pic32mxCdc(const char *name);

    /** Destructor.
     */
    ~Pic32mxCdc()
    {
    }

    /** handle an interrupt.
     */
    void interrupt_handler()
    {
        DRV_USBFS_Tasks_ISR(drvUSBObject);
    }

    /** Background thread for doing USB device driver tasks. Must be called
     * aobut once per second in order to initialize, register with the USB
     * hosts, etc. */
    void device_tasks();

    /** This function is called from the interrupt context of the USB driver to
     *  notify us about various events. */
    inline void event_hander_from_isr(USB_DEVICE_EVENT event, void *pData);

    /** THis function is called from the interrupt context of the USB driver to
     * notify us about various events. */
    inline USB_DEVICE_CDC_EVENT_RESPONSE cdc_event_hander_from_isr(
        USB_DEVICE_CDC_EVENT event, void *pData);

private:

    /** Function to try and transmit a character.  Unused by this device driver.
     */
    void tx_char() override {}

    void enable() override {} /**< function to enable device */
    void disable() override {} /**< function to disable device */

#if 0
    /** Write to a file or device.
     * @param file file reference for this device
     * @param buf location to find write data
     * @param count number of bytes to write
     * @return number of bytes written upon success, -1 upon failure with errno containing the cause
     */
    ssize_t write(File *file, const void *buf, size_t count) OVERRIDE;

    /** Device select method. Default impementation returns true.
     * @param file reference to the file
     * @param mode FREAD for read active, FWRITE for write active, 0 for
     *        exceptions
     * @return true if active, false if inactive
     */
    bool select(File* file, int mode) OVERRIDE;

    /** Handles CDC driver notifications related to control and setup of the
     * device.  This is called from within interrupt context.
     * @param data private data
     * @param event identifies the event we are being notified about
     * @param msg_param event-specific value
     * @param msg_data event-specific data
     * @return return value is event specific
     */
    static uint32_t control_callback(void *data, unsigned long event,
                                     unsigned long msg_param, void *msg_data);

    /** Handles CDC driver notifications related to reception.
     * This is called from within interrupt context.
     * @param data private data
     * @param event identifies the event we are being notified about
     * @param msg_param event-specific value
     * @param msg_data event-specific data
     * @return return value is event specific
     */
    static uint32_t rx_callback(void *data, unsigned long event,
                                unsigned long msg_param, void *msg_data);

    /** Handles CDC driver notifications related to transmission.
     * This is called from within interrupt context.
     * @param data private data
     * @param event identifies the event we are being notified about
     * @param msg_param event-specific value
     * @param msg_data event-specific data
     * @return return value is event specific
     */
    static uint32_t tx_callback(void *data, unsigned long event,
                                unsigned long msg_param, void *msg_data);

#endif

    //tUSBDCDCDevice usbdcdcDevice; /**< CDC serial device instance */
    SYS_MODULE_OBJ  drvUSBObject;
    SYS_MODULE_OBJ  usbDevObject0;
    USB_DEVICE_HANDLE deviceHandle{USB_DEVICE_HANDLE_INVALID};
    USB_CDC_LINE_CODING lineCodingData;

    unsigned needAttach : 1;
    unsigned isConfigured : 1;
    uint32_t interrupt; /**< interrupt number for device */
#if 0
    bool connected; /**< connection status */
    bool enabled; /**< enabled status */
    int woken; /**< task woken metadata for ISR */
    bool txPending; /**< true if a transmission is in progress or pending */
    SelectInfo selInfoWr; /**< Metadata for select() logic */
#endif

    /** Default constructor.
     */
    Pic32mxCdc();

    DISALLOW_COPY_AND_ASSIGN(Pic32mxCdc);
};

// Instantiates the driver.
Pic32mxCdc usbCdc0("/dev/serUSB0");

void* usb_device_task(void*) {
    while (1) {
        usbCdc0.device_tasks();
        vTaskDelay(configTICK_RATE_HZ / 20);
    }
}

extern "C" {
void usb_interrupt() {
    usbCdc0.interrupt_handler();
}
}

Pic32mxCdc::Pic32mxCdc(const char *name)
    : Serial(name)
    , needAttach(0)
    , isConfigured(0)
{
    drvUSBObject = DRV_USBFS_Initialize(DRV_USBFS_INDEX_0, (SYS_MODULE_INIT *) &drvUSBFSInit);

    /* Set priority of USB interrupt source. This has to be within kernel
     * interrupt priority. */
    SYS_INT_VectorPrioritySet(INT_VECTOR_USB1, INT_PRIORITY_LEVEL2);

    /* Set Sub-priority of USB interrupt source */
    SYS_INT_VectorSubprioritySet(INT_VECTOR_USB1, INT_SUBPRIORITY_LEVEL0);

    /* Initialize Middleware */
    /* Initialize the USB device layer */
    usbDevObject0 = USB_DEVICE_Initialize (USB_DEVICE_INDEX_0 , ( SYS_MODULE_INIT* ) & usbDevInitData);

    os_thread_t tid;
    os_thread_create(&tid, "usb_tasks", 1, 512, &usb_device_task, nullptr);

    lineCodingData.dwDTERate = 115200;
    lineCodingData.bDataBits = 8;
    lineCodingData.bParityType = 0;
    lineCodingData.bCharFormat = 0;
}

static void usb_device_event_handler(
    USB_DEVICE_EVENT event, void *pData, uintptr_t context) {
    usbCdc0.event_hander_from_isr(event, pData);
}

void Pic32mxCdc::device_tasks()
{
    DRV_USBFS_Tasks(drvUSBObject);
    USB_DEVICE_Tasks(usbDevObject0);

    do {
        if (deviceHandle != USB_DEVICE_HANDLE_INVALID) {
            break;
        }
        deviceHandle =
            USB_DEVICE_Open(USB_DEVICE_INDEX_0, DRV_IO_INTENT_READWRITE);
        if (deviceHandle == USB_DEVICE_HANDLE_INVALID) {
            break;
        }
        USB_DEVICE_EventHandlerSet(deviceHandle, APP_USBDeviceEventHandler, 0);
        needAttach = 1;
    } while (0);

    if (needAttach) {
        USB_DEVICE_Attach(deviceHandle);
        needAttach = 0;
    }

}

static USB_DEVICE_CDC_EVENT_RESPONSE cdc_device_event_handler(
    USB_DEVICE_CDC_INDEX index, USB_DEVICE_CDC_EVENT event, void *pData,
    uintptr_t userData)
{
    return usbCdc0.cdc_event_hander_from_isr(event, pData);
}

void Pic32mxCdc::event_hander_from_isr(USB_DEVICE_EVENT event, void *pData) {
    switch( event )
    {
        case USB_DEVICE_EVENT_POWER_REMOVED:
            USB_DEVICE_Detach (appData.deviceHandle);
            break;
        case USB_DEVICE_EVENT_POWER_DETECTED:
            needAttach = 1;
            break;
        case USB_DEVICE_EVENT_RESET:
        case USB_DEVICE_EVENT_DECONFIGURED:
            isConfigured = 0;
            break;
        case USB_DEVICE_EVENT_CONFIGURED:
            auto v = ((USB_DEVICE_EVENT_DATA_CONFIGURED *)pData)->configurationValue;
            if (v != 1) { break; }

            /* Register the CDC Device application event handler here.
             * Note how the appData object pointer is passed as the
             * user data */
            USB_DEVICE_CDC_EventHandlerSet(
                0, APP_USBDeviceCDCEventHandler, 0);
            isConfigured = 1;
            break;
    case USB_DEVICE_EVENT_SUSPENDED:
        break;
        case USB_DEVICE_EVENT_RESUMED:
        break;

    default:
        break;
    }

}

USB_DEVICE_CDC_EVENT_RESPONSE Pic32mxCdc::cdc_event_hander_from_isr(
    USB_DEVICE_CDC_EVENT event, void *pData)
{
    switch (event)
    {
        case USB_DEVICE_CDC_EVENT_GET_LINE_CODING:

            /* This means the host wants to know the current line
             * coding. This is a control transfer request. Use the
             * USB_DEVICE_ControlSend() function to send the data to
             * host.  */

            USB_DEVICE_ControlSend(
                deviceHandle, &lineCodingData, sizeof(USB_CDC_LINE_CODING));

            break;

        case USB_DEVICE_CDC_EVENT_SET_LINE_CODING:

            /* This means the host wants to set the line coding.
             * This is a control transfer request. Use the
             * USB_DEVICE_ControlReceive() function to receive the
             * data from the host */

            USB_DEVICE_ControlReceive(
                deviceHandle, lineCodingData, sizeof(USB_CDC_LINE_CODING));

            break;

        case USB_DEVICE_CDC_EVENT_SET_CONTROL_LINE_STATE:
            // Note: here we could implement fake hardware flow control by
            // looking at the DTR and DCD lines as given by the host. I don't
            // think that's interesting as USB drivers always have builtin
            // hardware flow control using kernel buffers.

            // fall through
        case USB_DEVICE_CDC_EVENT_CONTROL_TRANSFER_DATA_RECEIVED:
            // acknowledge
            USB_DEVICE_ControlStatus(
                deviceHandle, USB_DEVICE_CONTROL_STATUS_OK);
            break;
        case USB_DEVICE_CDC_EVENT_CONTROL_TRANSFER_DATA_SENT:
            break;
        case USB_DEVICE_CDC_EVENT_SEND_BREAK:
            break;
        case USB_DEVICE_CDC_EVENT_READ_COMPLETE:
            // !TODO
            break;
        case USB_DEVICE_CDC_EVENT_WRITE_COMPLETE:
            // !TODO
            break;

        default:
            break;
    }
}
<|endoftext|>


<commit_before>#include "xigua/enviroment.hpp"

namespace xig {

enviroment::enviroment(env_type in_type) {
  my_type = in_type;
  my_relative_path = "";
  my_parent = nullptr;
}

enviroment::enviroment(env_type in_type, enviroment *parent_enviroment) {
  my_type = in_type;
  my_relative_path = "";

  if (in_type != env_type::container) {
    if (parent_enviroment->enviroment_type() != env_type::container) {
      defined_variables = (*parent_enviroment).defined_variables;
      my_parent = parent_enviroment->parent();
    } else {
      my_parent = parent_enviroment;
    }
  } else {
    my_parent = parent_enviroment;
  }
}

data_type enviroment::type() const { return data_type::enviroment; }

const enviroment &enviroment::as_enviroment() const { return *this; }

env_type enviroment::enviroment_type() const {
  return my_type;
};

data_ptr enviroment::find_here(std::string variable_name) const {
	std::string delimiter = "::";
	size_t pos = 0;
	if ((pos = variable_name.find(delimiter)) != std::string::npos) {
		auto var_env = variable_name.substr(0, pos);
		auto new_var_name = variable_name;
		new_var_name.erase(0, pos + delimiter.length());

		auto e = find(var_env);
		if (!e || e->type() != data_type::enviroment) {
			return data_ptr(nullptr);
		}
		return e->as_enviroment().find(new_var_name);
	}

	auto found_position = defined_variables.find(variable_name);
	if (found_position != defined_variables.end())
		return found_position->second;
}

data_ptr enviroment::find(std::string variable_name) const {
  std::string delimiter = "::";
  size_t pos = 0;
  if ((pos = variable_name.find(delimiter)) != std::string::npos) {

    auto var_env = variable_name.substr(0, pos);
    auto new_var_name = variable_name;
    new_var_name.erase(0, pos + delimiter.length());

    auto e = find(var_env);
    if (!e || e->type() != data_type::enviroment) {
      if (parent() != nullptr) {
        auto data = parent()->find(variable_name);
        return data;
      }
      return data_ptr(nullptr);
    }
	return e->as_enviroment().find(new_var_name);
  }

  if (defined_variables.find(variable_name) != defined_variables.end())
	  return defined_variables.find(variable_name)->second;

  if (parent() != nullptr) {
    auto data = parent()->find(variable_name);
    return data;
  }

  return data_ptr(nullptr);
  ;
}

void enviroment::set(std::string name, data_ptr value) {
	if (my_type == env_type::container || my_type == env_type::function) {
		defined_variables[name] = value;
	}
	else if (my_type == env_type::macro || my_type == env_type::let) {
		parent()->set(name, value);
	}
}

void enviroment::set_here(std::string name, data_ptr value) {
	defined_variables[name] = value;
}

enviroment *enviroment::parent() const {
  return my_parent;
};

bool enviroment::has_parent() const { return (parent() != nullptr); }

std::string enviroment::relative_path() const {
  if (has_parent() && (my_relative_path == "")) {
    return parent()->relative_path();
  }
  return my_relative_path;
}

void enviroment::set_relative_path(std::string path) {
  my_relative_path = path;
}

void enviroment::print_all_vars() {
  std::cout << " -------------------- " << std::endl;
  for (auto i : defined_variables) {
    std::cout << i.first << "  =>  " << string_representation(i.second)
              << std::endl;
  }
  std::cout << " -------------------- " << std::endl;
}

bool enviroment::operator<(const data &other) const {
	if (type() == other.type())
		return defined_variables < other.as_enviroment().defined_variables;
	return type() < other.type();
}

bool enviroment::operator==(const data &other) const {
	if (type() == other.type())
		return defined_variables == other.as_enviroment().defined_variables;
	return false;
}

}
<commit_msg>cleaner env find<commit_after>#include "xigua/enviroment.hpp"

namespace xig {

enviroment::enviroment(env_type in_type) {
  my_type = in_type;
  my_relative_path = "";
  my_parent = nullptr;
}

enviroment::enviroment(env_type in_type, enviroment *parent_enviroment) {
  my_type = in_type;
  my_relative_path = "";

  if (in_type != env_type::container) {
    if (parent_enviroment->enviroment_type() != env_type::container) {
      defined_variables = (*parent_enviroment).defined_variables;
      my_parent = parent_enviroment->parent();
    } else {
      my_parent = parent_enviroment;
    }
  } else {
    my_parent = parent_enviroment;
  }
}

data_type enviroment::type() const { return data_type::enviroment; }

const enviroment &enviroment::as_enviroment() const { return *this; }

env_type enviroment::enviroment_type() const {
  return my_type;
};

static std::string delimiter = "::";

data_ptr find_with_delimiter(const enviroment &me, const std::map<std::string, data_ptr> &defined_variables,  const std::string &variable_name) {
	auto delimiter_position = variable_name.find(delimiter);
	if (delimiter_position != std::string::npos) {
		auto env_name = variable_name.substr(delimiter_position + delimiter.length());
		auto var_name = variable_name.substr(0, delimiter_position);
		auto env = me.find(env_name);
		if (env && env->type() == data_type::enviroment) {
			return env->as_enviroment().find(var_name);
		}
		return data_ptr(nullptr);
	}
	return data_ptr(nullptr);
}

data_ptr find_in_map(const std::map<std::string, data_ptr> &defined_variables, const std::string &variable_name) {
	auto found = defined_variables.find(variable_name);
	if (found != defined_variables.end())
		return found->second;
	return data_ptr(nullptr);
}

data_ptr find_in_parent(const enviroment &me, const std::string &variable_name) {
	if (me.has_parent()) {
		return me.parent()->find(variable_name);
	}
}

data_ptr enviroment::find(std::string variable_name) const {
	auto found_delimiter = variable_name.find(delimiter);
	if (found_delimiter != std::string::npos) {
		auto found_var = find_with_delimiter(*this, defined_variables, variable_name);
		if (found_var)
			return found_var;
		parent()->find(variable_name);
	}

	auto found_in_me = find_in_map(defined_variables, variable_name);
	if (found_in_me)
		return found_in_me;

	return find_in_parent(*this, variable_name);
}

data_ptr enviroment::find_here(std::string variable_name) const {
	auto found_delimiter = variable_name.find(delimiter);
	if (found_delimiter != std::string::npos) {
		return find_with_delimiter(*this, defined_variables, variable_name);
	}

	return find_in_map(defined_variables, variable_name);
}

void enviroment::set(std::string name, data_ptr value) {
	if (my_type == env_type::container || my_type == env_type::function) {
		defined_variables[name] = value;
	}
	else if (my_type == env_type::macro || my_type == env_type::let) {
		parent()->set(name, value);
	}
}

void enviroment::set_here(std::string name, data_ptr value) {
	defined_variables[name] = value;
}

enviroment *enviroment::parent() const {
  return my_parent;
};

bool enviroment::has_parent() const { return (parent() != nullptr); }

std::string enviroment::relative_path() const {
  if (has_parent() && (my_relative_path == "")) {
    return parent()->relative_path();
  }
  return my_relative_path;
}

void enviroment::set_relative_path(std::string path) {
  my_relative_path = path;
}

void enviroment::print_all_vars() {
  std::cout << " -------------------- " << std::endl;
  for (auto i : defined_variables) {
    std::cout << i.first << "  =>  " << string_representation(i.second)
              << std::endl;
  }
  std::cout << " -------------------- " << std::endl;
}

bool enviroment::operator<(const data &other) const {
	if (type() == other.type())
		return defined_variables < other.as_enviroment().defined_variables;
	return type() < other.type();
}

bool enviroment::operator==(const data &other) const {
	if (type() == other.type())
		return defined_variables == other.as_enviroment().defined_variables;
	return false;
}

}
<|endoftext|>


<commit_before>
#include "CAssetManager.h"

namespace ion
{

	void CAssetManager::Init(Graphics::IGraphicsAPI * GraphicsAPI)
	{
		this->GraphicsAPI = GraphicsAPI;
	}

	Graphics::IShaderProgram * CAssetManager::LoadShader(string const & Name)
	{
		if (! GraphicsAPI)
		{
			Log::Error("CAssetManager being used without being initialized, Shader '%s' will not be loaded.", Name);
			return nullptr;
		}

		Graphics::IVertexShader * VertexShader = GraphicsAPI->CreateVertexShaderFromFile(AssetPath + ShaderPath + Name + ".vert");
		Graphics::IPixelShader * PixelShader = GraphicsAPI->CreatePixelShaderFromFile(AssetPath + ShaderPath + Name + ".frag");

		if (! VertexShader)
		{
			Log::Error("Failed to compile vertex shader '%s'", Name);
			return nullptr;
		}

		if (! PixelShader)
		{
			Log::Error("Failed to compile pixel shader '%s'", Name);
			return nullptr;
		}

		Graphics::IShaderProgram * ShaderProgram = GraphicsAPI->CreateShaderProgram();
		ShaderProgram->SetVertexStage(VertexShader);
		ShaderProgram->SetPixelStage(PixelShader);

		return ShaderProgram;
	}

	Graphics::ITexture2D * CAssetManager::LoadTexture(string const & FileName)
	{
		if (! GraphicsAPI)
		{
			Log::Error("CAssetManager being used without being initialized, Texture '%s' will not be loaded.", FileName);
			return nullptr;
		}

		CImage * Image = CImage::Load(AssetPath + TexturePath + FileName);
		Graphics::ITexture2D * Texture = GraphicsAPI->CreateTexture2D(
			Image->GetSize(),
			Graphics::ITexture::EMipMaps::True,
			(Image->GetChannels() == 4 ?
				Graphics::ITexture::EFormatComponents::RGBA :
				(Image->GetChannels() == 3 ?
				Graphics::ITexture::EFormatComponents::RGB :
				(Image->GetChannels() == 2 ?
				Graphics::ITexture::EFormatComponents::RG :
				Graphics::ITexture::EFormatComponents::R))),
			Graphics::ITexture::EInternalFormatType::Fix8);
		Texture->Upload(
			Image->GetData(),
			Image->GetSize(),
			Graphics::ITexture::EFormatComponents::RGB,
			Graphics::EScalarType::UnsignedInt8);
		return Texture;
	}

	void CAssetManager::SetAssetPath(string const & Path)
	{
		AssetPath = Path + "/";
	}

	void CAssetManager::SetTexturePath(string const & Path)
	{
		TexturePath = Path + "/";
	}

	void CAssetManager::SetShaderPath(string const & Path)
	{
		ShaderPath = Path + "/";
	}

}
<commit_msg>Prevent crash when using asset manager to load texture from missing image file<commit_after>
#include "CAssetManager.h"

namespace ion
{

	void CAssetManager::Init(Graphics::IGraphicsAPI * GraphicsAPI)
	{
		this->GraphicsAPI = GraphicsAPI;
	}

	Graphics::IShaderProgram * CAssetManager::LoadShader(string const & Name)
	{
		if (! GraphicsAPI)
		{
			Log::Error("CAssetManager being used without being initialized, Shader '%s' will not be loaded.", Name);
			return nullptr;
		}

		Graphics::IVertexShader * VertexShader = GraphicsAPI->CreateVertexShaderFromFile(AssetPath + ShaderPath + Name + ".vert");
		Graphics::IPixelShader * PixelShader = GraphicsAPI->CreatePixelShaderFromFile(AssetPath + ShaderPath + Name + ".frag");

		if (! VertexShader)
		{
			Log::Error("Failed to compile vertex shader '%s'", Name);
			return nullptr;
		}

		if (! PixelShader)
		{
			Log::Error("Failed to compile pixel shader '%s'", Name);
			return nullptr;
		}

		Graphics::IShaderProgram * ShaderProgram = GraphicsAPI->CreateShaderProgram();
		ShaderProgram->SetVertexStage(VertexShader);
		ShaderProgram->SetPixelStage(PixelShader);

		return ShaderProgram;
	}

	Graphics::ITexture2D * CAssetManager::LoadTexture(string const & FileName)
	{
		if (! GraphicsAPI)
		{
			Log::Error("CAssetManager being used without being initialized, Texture '%s' will not be loaded.", FileName);
			return nullptr;
		}

		CImage * Image = CImage::Load(AssetPath + TexturePath + FileName);
		if (Image)
		{
			Graphics::ITexture2D * Texture = GraphicsAPI->CreateTexture2D(
				Image->GetSize(),
				Graphics::ITexture::EMipMaps::True,
				(Image->GetChannels() == 4 ?
					Graphics::ITexture::EFormatComponents::RGBA :
					(Image->GetChannels() == 3 ?
						Graphics::ITexture::EFormatComponents::RGB :
						(Image->GetChannels() == 2 ?
							Graphics::ITexture::EFormatComponents::RG :
							Graphics::ITexture::EFormatComponents::R))),
				Graphics::ITexture::EInternalFormatType::Fix8);
			Texture->Upload(
				Image->GetData(),
				Image->GetSize(),
				Graphics::ITexture::EFormatComponents::RGB,
				Graphics::EScalarType::UnsignedInt8);
			return Texture;
		}
		else
		{
			return nullptr;
		}
	}

	void CAssetManager::SetAssetPath(string const & Path)
	{
		AssetPath = Path + "/";
	}

	void CAssetManager::SetTexturePath(string const & Path)
	{
		TexturePath = Path + "/";
	}

	void CAssetManager::SetShaderPath(string const & Path)
	{
		ShaderPath = Path + "/";
	}

}
<|endoftext|>


<commit_before>#ifndef fakeit_h__
#define fakeit_h__

#include "fakeit/Mock.hpp"
#include "fakeit/WhenFunctor.hpp"
#include "fakeit/FakeFunctor.hpp"
#include "fakeit/UsingFunctor.hpp"
#include "fakeit/VerifyFunctor.hpp"
#include "fakeit/VerifyNoOtherInvocationsFunctor.hpp"
#include "fakeit/SpyFunctor.hpp"

#ifdef _WIN32
#define __func__ __FUNCTION__
#endif

#define CALL( mock , method ) \
	mock[&std::remove_reference<decltype(mock.get())>::type::method].setMethodDetails(#mock,#method)

#define Verify( ... ) \
		Verify( __VA_ARGS__ ).setFileInfo(__FILE__,__LINE__, __func__)

#define Using( ... ) \
		Using( __VA_ARGS__ )

#define VerifyNoOtherInvocations( ... ) \
	VerifyNoOtherInvocations( __VA_ARGS__ ).setFileInfo(__FILE__,__LINE__, __func__)

#define Fake( ... ) \
	Fake( __VA_ARGS__ )

#define When( call ) \
	When(call)

#endif // fakeit_h__
<commit_msg>Use fakeit:: namespace inside macros<commit_after>#ifndef fakeit_h__
#define fakeit_h__

#include "fakeit/Mock.hpp"
#include "fakeit/WhenFunctor.hpp"
#include "fakeit/FakeFunctor.hpp"
#include "fakeit/UsingFunctor.hpp"
#include "fakeit/VerifyFunctor.hpp"
#include "fakeit/VerifyNoOtherInvocationsFunctor.hpp"
#include "fakeit/SpyFunctor.hpp"

#ifdef _WIN32
#define __func__ __FUNCTION__
#endif

#define CALL( mock , method ) \
	mock[&std::remove_reference<decltype(mock.get())>::type::method].setMethodDetails(#mock,#method)

#define Verify( ... ) \
        fakeit::Verify( __VA_ARGS__ ).setFileInfo(__FILE__,__LINE__, __func__)

#define Using( ... ) \
        fakeit::Using( __VA_ARGS__ )

#define VerifyNoOtherInvocations( ... ) \
    fakeit::VerifyNoOtherInvocations( __VA_ARGS__ ).setFileInfo(__FILE__,__LINE__, __func__)

#define Fake( ... ) \
    fakeit::Fake( __VA_ARGS__ )

#define When( call ) \
    fakeit::When(call)

#endif // fakeit_h__
<|endoftext|>


<commit_before>/*!
 * @file InverseKinematicsData.cpp
 * @author Francesco Romano
 * @copyright 2016 iCub Facility - Istituto Italiano di Tecnologia
 *            Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
 * @date 2016
 *
 */

#include "InverseKinematicsData.h"
#include "InverseKinematicsNLP.h"
#include "TransformConstraint.h"
#include <iDynTree/Core/Twist.h>
#include <iDynTree/Core/ClassicalAcc.h>
#include <iDynTree/Core/SpatialAcc.h>
#include <iDynTree/ModelIO/ModelLoader.h>
#include <iDynTree/Core/EigenHelpers.h>

#include <cassert>
#include <private/InverseKinematicsData.h>

namespace internal {
namespace kinematics {

    InverseKinematicsData::InverseKinematicsData(const InverseKinematicsData&) {}
    InverseKinematicsData& InverseKinematicsData::operator=(const InverseKinematicsData&) { return *this; }

    InverseKinematicsData::InverseKinematicsData()
    : m_defaultTargetResolutionMode(iDynTree::InverseKinematicsTreatTargetAsConstraintNone)
    , m_dofs(0)
    , m_rotationParametrization(iDynTree::InverseKinematicsRotationParametrizationQuaternion)
    , m_areBaseInitialConditionsSet(false)
    , m_areJointsInitialConditionsSet(InverseKinematicsInitialConditionNotSet)
    , m_solver(NULL)
    // The default values for the ipopt related parameters are exactly the one of IPOPT,
    // see https://www.coin-or.org/Ipopt/documentation/node41.html
    //     https://www.coin-or.org/Ipopt/documentation/node42.html
    , m_maxIter(3000)
    , m_maxCpuTime(1e6)
    , m_tol(1e-8)
    , m_constrTol(1e-4)
    , m_verbosityLevel(0)
    {
        //These variables are touched only once.
        m_state.worldGravity.zero();

        m_state.baseTwist.zero();

        m_comTarget.isActive = false;
        m_comTarget.weight = 0;
        m_comTarget.desiredPosition.zero();
        m_comTarget.constraintTolerance = 1e-8;
        m_comTarget.isConstraint = false;
    }

    bool InverseKinematicsData::setModel(const iDynTree::Model& model, const std::vector<std::string> &consideredJoints)
    {
        m_dofs = model.getNrOfDOFs();

        m_reducedVariablesInfo.fixedVariables.assign(m_dofs, false);
        m_reducedVariablesInfo.modelJointsToOptimisedJoints.clear();

        if (!consideredJoints.empty()) {

            iDynTree::ModelLoader reducedModelLoader;
            if (!reducedModelLoader.loadReducedModelFromFullModel(model, consideredJoints)){
                std::cerr << "[ERROR] Error loading reduced robot model" << std::endl;
                return false;
            }
            m_reducedVariablesInfo.reducedModel = reducedModelLoader.model();

            for (iDynTree::JointIndex jointIdx = 0; jointIdx < model.getNrOfDOFs(); ++jointIdx) {
                std::string jointName = model.getJointName(jointIdx);
                std::vector<std::string>::const_iterator found = std::find(consideredJoints.begin(), consideredJoints.end(), jointName);
                if (found == consideredJoints.end()) {
                    m_reducedVariablesInfo.fixedVariables[jointIdx] = true;
                    continue;
                }
                // found => get the index in the optimised joints vector
                size_t optimisedIndex = std::distance(consideredJoints.begin(), found);
                m_reducedVariablesInfo.modelJointsToOptimisedJoints.insert(std::unordered_map<int, int>::value_type(optimisedIndex, jointIdx));
            }
        } else {

            m_reducedVariablesInfo.reducedModel = model;

            for (iDynTree::JointIndex jointIdx = 0; jointIdx < model.getNrOfDOFs(); ++jointIdx) {
                m_reducedVariablesInfo.modelJointsToOptimisedJoints.insert(std::unordered_map<int, int>::value_type(jointIdx, jointIdx));
            }
        }

        bool result = m_dynamics.loadRobotModel(model);
        if (!result || !m_dynamics.isValid()) {
            std::cerr << "[ERROR] Error loading robot model" << std::endl;
            return false;
        }

        //prepare joint limits
        m_jointLimits.clear();
        //TODO to be changed to +_ infinity
        //default: no limits
        m_jointLimits.assign(m_dofs, std::pair<double, double>(-2e+19, 2e+19));

        //for each joint, ask the limits
        for (iDynTree::JointIndex jointIdx = 0; jointIdx < model.getNrOfJoints(); ++jointIdx) {
            iDynTree::IJointConstPtr joint = model.getJoint(jointIdx);
            //if the joint does not have limits skip it
            if (!joint->hasPosLimits())
                continue;
            //for each DoF modelled by the joint get the limits
            for (unsigned dof = 0; dof < joint->getNrOfDOFs(); ++dof) {
                if (!joint->getPosLimits(dof,
                                         m_jointLimits[joint->getDOFsOffset() + dof].first,
                                         m_jointLimits[joint->getDOFsOffset() + dof].second))
                    continue;
            }
        }

        //We set a new model, clear the variables
        clearProblem();

        updateRobotConfiguration();

        return true;
    }

    void InverseKinematicsData::clearProblem()
    {
        //resize vectors
        m_jointInitialConditions.resize(m_dofs);
        m_jointInitialConditions.zero();
        m_jointsResults.resize(m_dofs);
        m_jointsResults.zero();
        m_preferredJointsConfiguration.resize(m_dofs);
        m_preferredJointsConfiguration.zero();
        m_preferredJointsWeight = 1e-6;

        m_state.jointsConfiguration.resize(m_dofs);
        m_state.jointsConfiguration.zero();

        m_state.jointsVelocity.resize(m_dofs);
        m_state.jointsVelocity.zero();

        m_state.basePose.setPosition(iDynTree::Position(0, 0, 0));
        m_state.basePose.setRotation(iDynTree::Rotation::Identity());

        m_constraints.clear();
        m_targets.clear();
        m_comHullConstraint.setActive(false);

        m_areBaseInitialConditionsSet = false;
        m_areJointsInitialConditionsSet = internal::kinematics::InverseKinematicsData::InverseKinematicsInitialConditionNotSet;

        m_comTarget.isActive = false;
        m_comTarget.weight = 0;
        m_comTarget.desiredPosition.zero();
        m_comTarget.constraintTolerance = 1e-8;
    }

    bool InverseKinematicsData::addFrameConstraint(const kinematics::TransformConstraint& frameTransformConstraint)
    {
        int frameIndex = m_dynamics.getFrameIndex(frameTransformConstraint.getFrameName());
        if (frameIndex < 0)
            return false;

        //add the constraint to the set
        std::pair<TransformMap::iterator, bool> result = m_constraints.insert(TransformMap::value_type(frameIndex, frameTransformConstraint));
        return result.second;
    }

    bool InverseKinematicsData::addTarget(const kinematics::TransformConstraint& frameTransform)
    {
        int frameIndex = m_dynamics.getFrameIndex(frameTransform.getFrameName());
        if (frameIndex < 0)
            return false;

        std::pair<TransformMap::iterator, bool> result = m_targets.insert(TransformMap::value_type(frameIndex, frameTransform));
        // As the input is const, I can only modify it after insertion
        if (result.second) {
            result.first->second.setTargetResolutionMode(m_defaultTargetResolutionMode);
        }
        return result.second;
    }

    TransformMap::iterator InverseKinematicsData::getTargetRefIfItExists(const std::string targetFrameName)
    {
        // The error for this check is already printed in getFrameIndex
        int frameIndex = m_dynamics.getFrameIndex(targetFrameName);
        if (frameIndex == iDynTree::FRAME_INVALID_INDEX)
            return m_targets.end();

        // Find the target (if this fails, it will return m_targets.end()
        return m_targets.find(frameIndex);
    }

    void InverseKinematicsData::updatePositionTarget(TransformMap::iterator target, iDynTree::Position newPos, double newPosWeight)
    {
        assert(target != m_targets.end());
        target->second.setPosition(newPos);
        target->second.setPositionWeight(newPosWeight);
    }

    void InverseKinematicsData::updateRotationTarget(TransformMap::iterator target, iDynTree::Rotation newRot, double newRotWeight)
    {
        assert(target != m_targets.end());
        target->second.setRotation(newRot);
        target->second.setRotationWeight(newRotWeight);
    }

    iDynTree::KinDynComputations& InverseKinematicsData::dynamics() { return m_dynamics; }

    bool InverseKinematicsData::setRobotConfiguration(const iDynTree::Transform& baseConfiguration,
                                                      const iDynTree::VectorDynSize& jointConfiguration)
    {
        assert(m_state.jointsConfiguration.size() == jointConfiguration.size());
        m_state.jointsConfiguration = jointConfiguration;
        m_state.basePose = baseConfiguration;
        updateRobotConfiguration();
        return true;
    }

    bool InverseKinematicsData::setJointConfiguration(const std::string& jointName, const double jointConfiguration)
    {
        iDynTree::JointIndex jointIndex = m_dynamics.model().getJointIndex(jointName);
        if (jointIndex == iDynTree::JOINT_INVALID_INDEX) return false;
        m_state.jointsConfiguration(jointIndex) = jointConfiguration;
        updateRobotConfiguration();
        return true;
    }

    void InverseKinematicsData::setRotationParametrization(enum iDynTree::InverseKinematicsRotationParametrization parametrization)
    {
        m_rotationParametrization = parametrization;
    }

    enum iDynTree::InverseKinematicsRotationParametrization InverseKinematicsData::rotationParametrization() { return m_rotationParametrization; }

    void InverseKinematicsData::updateRobotConfiguration()
    {
        m_dynamics.setRobotState(m_state.basePose,
                                 m_state.jointsConfiguration,
                                 m_state.baseTwist,
                                 m_state.jointsVelocity,
                                 m_state.worldGravity);
    }

    void InverseKinematicsData::prepareForOptimization()
    {
        //Do all stuff needed before starting an optimization problem
        //1) prepare initial condition if not explicitly set
        if (!m_areBaseInitialConditionsSet) {
            m_baseInitialCondition = m_state.basePose;
        }

        switch (m_areJointsInitialConditionsSet) {
            case InverseKinematicsInitialConditionNotSet:
                m_jointInitialConditions = m_state.jointsConfiguration;
                break;
            case InverseKinematicsInitialConditionPartial:
                // in this case we have to set in m_jointInitialConditions
                // the joints in m_state.jointsConfiguration which are not considered
                // in the reduced variables
                for (size_t i = 0; i < m_reducedVariablesInfo.fixedVariables.size(); ++i) {
                    if (!m_reducedVariablesInfo.fixedVariables[i]) continue;
                    // joint is fixed => set the initial condition
                    m_jointInitialConditions(i) = m_state.jointsConfiguration(i);
                }
                break;
            default:
                break;
        }

        //2) Check joint limits..
        for (size_t i = 0; i < m_jointInitialConditions.size(); ++i) {
            //check joint to be inside limit
            double &jointValue = m_jointInitialConditions(i);
            if (jointValue < m_jointLimits[i].first || jointValue > m_jointLimits[i].second) {
                std::cerr << "[WARNING] InverseKinematics: joint " << m_dynamics.model().getJointName(i) << " (index " << i << ") initial condition is outside the limits " << m_jointLimits[i].first << " " << m_jointLimits[i].second << std::endl;
                //set the initial value to at the limit
                if (jointValue < m_jointLimits[i].first) {
                    jointValue = m_jointLimits[i].first;
                }

                if (jointValue > m_jointLimits[i].second) {
                    jointValue = m_jointLimits[i].second;
                }
            }
        }

    }

    void InverseKinematicsData::setDefaultTargetResolutionMode(iDynTree::InverseKinematicsTreatTargetAsConstraint mode)
    {
        m_defaultTargetResolutionMode = mode;
    }

    enum iDynTree::InverseKinematicsTreatTargetAsConstraint InverseKinematicsData::defaultTargetResolutionMode()
    {
        return m_defaultTargetResolutionMode;
    }

    void InverseKinematicsData::setTargetResolutionMode(TransformMap::iterator target, iDynTree::InverseKinematicsTreatTargetAsConstraint mode)
    {
       assert(target != m_targets.end());
       target->second.setTargetResolutionMode(mode);
    }

    enum iDynTree::InverseKinematicsTreatTargetAsConstraint InverseKinematicsData::targetResolutionMode(TransformMap::iterator target) const
    {
        assert(target != m_targets.end());
        return target->second.targetResolutionMode();
    }

    bool InverseKinematicsData::solveProblem()
    {
        Ipopt::ApplicationReturnStatus solverStatus;

        if (Ipopt::IsNull(m_solver)) {
            m_solver = IpoptApplicationFactory();

            //TODO: set options
            //For example, one needed option is the linear solver type
            //Best thing is to wrap the IPOPT options with new structure so as to abstract them
            m_solver->Options()->SetStringValue("hessian_approximation", "limited-memory");
            m_solver->Options()->SetIntegerValue("print_level",m_verbosityLevel);
            m_solver->Options()->SetIntegerValue("max_iter", m_maxIter);
            m_solver->Options()->SetNumericValue("max_cpu_time", m_maxCpuTime);
            m_solver->Options()->SetNumericValue("tol", m_tol);
            m_solver->Options()->SetNumericValue("constr_viol_tol", m_constrTol);
            m_solver->Options()->SetIntegerValue("acceptable_iter", 5);
            m_solver->Options()->SetStringValue("fixed_variable_treatment", "make_parameter"); //which btw is the default option
#ifndef NDEBUG
            m_solver->Options()->SetStringValue("derivative_test", "first-order");
#endif
            if (!m_solverName.empty()) {
                m_solver->Options()->SetStringValue("linear_solver", m_solverName);
            } else {
                m_solver->Options()->GetStringValue("linear_solver", m_solverName, "");
            }

            solverStatus = m_solver->Initialize();
            if (solverStatus != Ipopt::Solve_Succeeded) {
                return false;
            }
        }

        prepareForOptimization();

        //instantiate the IpOpt problem
        internal::kinematics::InverseKinematicsNLP *iKin = new internal::kinematics::InverseKinematicsNLP(*this);
        //Do something (if necessary)
        Ipopt::SmartPtr<Ipopt::TNLP> problem(iKin);

        // Ask Ipopt to solve the problem
        solverStatus = m_solver->OptimizeTNLP(problem);

        if (solverStatus == Ipopt::Solve_Succeeded || solverStatus == Ipopt::Solved_To_Acceptable_Level ) {
            return true;
        } else {
            return false;
        }
    }

    void InverseKinematicsData::setCoMTarget(iDynTree::Position& desiredPosition, double weight){
        this->m_comTarget.desiredPosition = desiredPosition;

        if (!this->m_comTarget.isActive
            && m_defaultTargetResolutionMode & iDynTree::InverseKinematicsTreatTargetAsConstraintPositionOnly) {
            this->m_comTarget.isConstraint = true;
        }

        if (!(weight < 0)) {
            this->m_comTarget.weight = weight;
        }

        this->m_comTarget.isActive = true;
    }

    void InverseKinematicsData::setCoMasConstraint(bool asConstraint)
    {
        this->m_comTarget.isConstraint = asConstraint;
    }

    bool InverseKinematicsData::isCoMaConstraint()
    {
        return this->m_comTarget.isConstraint;
    }

    void InverseKinematicsData::setCoMasConstraintTolerance(double TOL)
    {
        this->m_comTarget.constraintTolerance = TOL;
    }

    bool InverseKinematicsData::isCoMTargetActive(){
        return this->m_comTarget.isActive;
    }

    void InverseKinematicsData::setCoMTargetInactive()
    {
        this->m_comTarget.isActive = false;
        this->m_comTarget.weight = 0;
        this->m_comTarget.desiredPosition.zero();
    }

}
}
<commit_msg>Improved again the warning message<commit_after>/*!
 * @file InverseKinematicsData.cpp
 * @author Francesco Romano
 * @copyright 2016 iCub Facility - Istituto Italiano di Tecnologia
 *            Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
 * @date 2016
 *
 */

#include "InverseKinematicsData.h"
#include "InverseKinematicsNLP.h"
#include "TransformConstraint.h"
#include <iDynTree/Core/Twist.h>
#include <iDynTree/Core/ClassicalAcc.h>
#include <iDynTree/Core/SpatialAcc.h>
#include <iDynTree/ModelIO/ModelLoader.h>
#include <iDynTree/Core/EigenHelpers.h>

#include <cassert>
#include <private/InverseKinematicsData.h>

namespace internal {
namespace kinematics {

    InverseKinematicsData::InverseKinematicsData(const InverseKinematicsData&) {}
    InverseKinematicsData& InverseKinematicsData::operator=(const InverseKinematicsData&) { return *this; }

    InverseKinematicsData::InverseKinematicsData()
    : m_defaultTargetResolutionMode(iDynTree::InverseKinematicsTreatTargetAsConstraintNone)
    , m_dofs(0)
    , m_rotationParametrization(iDynTree::InverseKinematicsRotationParametrizationQuaternion)
    , m_areBaseInitialConditionsSet(false)
    , m_areJointsInitialConditionsSet(InverseKinematicsInitialConditionNotSet)
    , m_solver(NULL)
    // The default values for the ipopt related parameters are exactly the one of IPOPT,
    // see https://www.coin-or.org/Ipopt/documentation/node41.html
    //     https://www.coin-or.org/Ipopt/documentation/node42.html
    , m_maxIter(3000)
    , m_maxCpuTime(1e6)
    , m_tol(1e-8)
    , m_constrTol(1e-4)
    , m_verbosityLevel(0)
    {
        //These variables are touched only once.
        m_state.worldGravity.zero();

        m_state.baseTwist.zero();

        m_comTarget.isActive = false;
        m_comTarget.weight = 0;
        m_comTarget.desiredPosition.zero();
        m_comTarget.constraintTolerance = 1e-8;
        m_comTarget.isConstraint = false;
    }

    bool InverseKinematicsData::setModel(const iDynTree::Model& model, const std::vector<std::string> &consideredJoints)
    {
        m_dofs = model.getNrOfDOFs();

        m_reducedVariablesInfo.fixedVariables.assign(m_dofs, false);
        m_reducedVariablesInfo.modelJointsToOptimisedJoints.clear();

        if (!consideredJoints.empty()) {

            iDynTree::ModelLoader reducedModelLoader;
            if (!reducedModelLoader.loadReducedModelFromFullModel(model, consideredJoints)){
                std::cerr << "[ERROR] Error loading reduced robot model" << std::endl;
                return false;
            }
            m_reducedVariablesInfo.reducedModel = reducedModelLoader.model();

            for (iDynTree::JointIndex jointIdx = 0; jointIdx < model.getNrOfDOFs(); ++jointIdx) {
                std::string jointName = model.getJointName(jointIdx);
                std::vector<std::string>::const_iterator found = std::find(consideredJoints.begin(), consideredJoints.end(), jointName);
                if (found == consideredJoints.end()) {
                    m_reducedVariablesInfo.fixedVariables[jointIdx] = true;
                    continue;
                }
                // found => get the index in the optimised joints vector
                size_t optimisedIndex = std::distance(consideredJoints.begin(), found);
                m_reducedVariablesInfo.modelJointsToOptimisedJoints.insert(std::unordered_map<int, int>::value_type(optimisedIndex, jointIdx));
            }
        } else {

            m_reducedVariablesInfo.reducedModel = model;

            for (iDynTree::JointIndex jointIdx = 0; jointIdx < model.getNrOfDOFs(); ++jointIdx) {
                m_reducedVariablesInfo.modelJointsToOptimisedJoints.insert(std::unordered_map<int, int>::value_type(jointIdx, jointIdx));
            }
        }

        bool result = m_dynamics.loadRobotModel(model);
        if (!result || !m_dynamics.isValid()) {
            std::cerr << "[ERROR] Error loading robot model" << std::endl;
            return false;
        }

        //prepare joint limits
        m_jointLimits.clear();
        //TODO to be changed to +_ infinity
        //default: no limits
        m_jointLimits.assign(m_dofs, std::pair<double, double>(-2e+19, 2e+19));

        //for each joint, ask the limits
        for (iDynTree::JointIndex jointIdx = 0; jointIdx < model.getNrOfJoints(); ++jointIdx) {
            iDynTree::IJointConstPtr joint = model.getJoint(jointIdx);
            //if the joint does not have limits skip it
            if (!joint->hasPosLimits())
                continue;
            //for each DoF modelled by the joint get the limits
            for (unsigned dof = 0; dof < joint->getNrOfDOFs(); ++dof) {
                if (!joint->getPosLimits(dof,
                                         m_jointLimits[joint->getDOFsOffset() + dof].first,
                                         m_jointLimits[joint->getDOFsOffset() + dof].second))
                    continue;
            }
        }

        //We set a new model, clear the variables
        clearProblem();

        updateRobotConfiguration();

        return true;
    }

    void InverseKinematicsData::clearProblem()
    {
        //resize vectors
        m_jointInitialConditions.resize(m_dofs);
        m_jointInitialConditions.zero();
        m_jointsResults.resize(m_dofs);
        m_jointsResults.zero();
        m_preferredJointsConfiguration.resize(m_dofs);
        m_preferredJointsConfiguration.zero();
        m_preferredJointsWeight = 1e-6;

        m_state.jointsConfiguration.resize(m_dofs);
        m_state.jointsConfiguration.zero();

        m_state.jointsVelocity.resize(m_dofs);
        m_state.jointsVelocity.zero();

        m_state.basePose.setPosition(iDynTree::Position(0, 0, 0));
        m_state.basePose.setRotation(iDynTree::Rotation::Identity());

        m_constraints.clear();
        m_targets.clear();
        m_comHullConstraint.setActive(false);

        m_areBaseInitialConditionsSet = false;
        m_areJointsInitialConditionsSet = internal::kinematics::InverseKinematicsData::InverseKinematicsInitialConditionNotSet;

        m_comTarget.isActive = false;
        m_comTarget.weight = 0;
        m_comTarget.desiredPosition.zero();
        m_comTarget.constraintTolerance = 1e-8;
    }

    bool InverseKinematicsData::addFrameConstraint(const kinematics::TransformConstraint& frameTransformConstraint)
    {
        int frameIndex = m_dynamics.getFrameIndex(frameTransformConstraint.getFrameName());
        if (frameIndex < 0)
            return false;

        //add the constraint to the set
        std::pair<TransformMap::iterator, bool> result = m_constraints.insert(TransformMap::value_type(frameIndex, frameTransformConstraint));
        return result.second;
    }

    bool InverseKinematicsData::addTarget(const kinematics::TransformConstraint& frameTransform)
    {
        int frameIndex = m_dynamics.getFrameIndex(frameTransform.getFrameName());
        if (frameIndex < 0)
            return false;

        std::pair<TransformMap::iterator, bool> result = m_targets.insert(TransformMap::value_type(frameIndex, frameTransform));
        // As the input is const, I can only modify it after insertion
        if (result.second) {
            result.first->second.setTargetResolutionMode(m_defaultTargetResolutionMode);
        }
        return result.second;
    }

    TransformMap::iterator InverseKinematicsData::getTargetRefIfItExists(const std::string targetFrameName)
    {
        // The error for this check is already printed in getFrameIndex
        int frameIndex = m_dynamics.getFrameIndex(targetFrameName);
        if (frameIndex == iDynTree::FRAME_INVALID_INDEX)
            return m_targets.end();

        // Find the target (if this fails, it will return m_targets.end()
        return m_targets.find(frameIndex);
    }

    void InverseKinematicsData::updatePositionTarget(TransformMap::iterator target, iDynTree::Position newPos, double newPosWeight)
    {
        assert(target != m_targets.end());
        target->second.setPosition(newPos);
        target->second.setPositionWeight(newPosWeight);
    }

    void InverseKinematicsData::updateRotationTarget(TransformMap::iterator target, iDynTree::Rotation newRot, double newRotWeight)
    {
        assert(target != m_targets.end());
        target->second.setRotation(newRot);
        target->second.setRotationWeight(newRotWeight);
    }

    iDynTree::KinDynComputations& InverseKinematicsData::dynamics() { return m_dynamics; }

    bool InverseKinematicsData::setRobotConfiguration(const iDynTree::Transform& baseConfiguration,
                                                      const iDynTree::VectorDynSize& jointConfiguration)
    {
        assert(m_state.jointsConfiguration.size() == jointConfiguration.size());
        m_state.jointsConfiguration = jointConfiguration;
        m_state.basePose = baseConfiguration;
        updateRobotConfiguration();
        return true;
    }

    bool InverseKinematicsData::setJointConfiguration(const std::string& jointName, const double jointConfiguration)
    {
        iDynTree::JointIndex jointIndex = m_dynamics.model().getJointIndex(jointName);
        if (jointIndex == iDynTree::JOINT_INVALID_INDEX) return false;
        m_state.jointsConfiguration(jointIndex) = jointConfiguration;
        updateRobotConfiguration();
        return true;
    }

    void InverseKinematicsData::setRotationParametrization(enum iDynTree::InverseKinematicsRotationParametrization parametrization)
    {
        m_rotationParametrization = parametrization;
    }

    enum iDynTree::InverseKinematicsRotationParametrization InverseKinematicsData::rotationParametrization() { return m_rotationParametrization; }

    void InverseKinematicsData::updateRobotConfiguration()
    {
        m_dynamics.setRobotState(m_state.basePose,
                                 m_state.jointsConfiguration,
                                 m_state.baseTwist,
                                 m_state.jointsVelocity,
                                 m_state.worldGravity);
    }

    void InverseKinematicsData::prepareForOptimization()
    {
        //Do all stuff needed before starting an optimization problem
        //1) prepare initial condition if not explicitly set
        if (!m_areBaseInitialConditionsSet) {
            m_baseInitialCondition = m_state.basePose;
        }

        switch (m_areJointsInitialConditionsSet) {
            case InverseKinematicsInitialConditionNotSet:
                m_jointInitialConditions = m_state.jointsConfiguration;
                break;
            case InverseKinematicsInitialConditionPartial:
                // in this case we have to set in m_jointInitialConditions
                // the joints in m_state.jointsConfiguration which are not considered
                // in the reduced variables
                for (size_t i = 0; i < m_reducedVariablesInfo.fixedVariables.size(); ++i) {
                    if (!m_reducedVariablesInfo.fixedVariables[i]) continue;
                    // joint is fixed => set the initial condition
                    m_jointInitialConditions(i) = m_state.jointsConfiguration(i);
                }
                break;
            default:
                break;
        }

        //2) Check joint limits..
        for (size_t i = 0; i < m_jointInitialConditions.size(); ++i) {
            //check joint to be inside limit
            double &jointValue = m_jointInitialConditions(i);
            if (jointValue < m_jointLimits[i].first || jointValue > m_jointLimits[i].second) {
                std::cerr
                << "[WARNING] InverseKinematics: joint " << m_dynamics.model().getJointName(i)
                << " (index " << i << ") initial condition is outside the limits "
                << m_jointLimits[i].first << " " << m_jointLimits[i].second
                << ". Actual value: " << jointValue <<  std::endl;
                //set the initial value to at the limit
                if (jointValue < m_jointLimits[i].first) {
                    jointValue = m_jointLimits[i].first;
                }

                if (jointValue > m_jointLimits[i].second) {
                    jointValue = m_jointLimits[i].second;
                }
            }
        }

    }

    void InverseKinematicsData::setDefaultTargetResolutionMode(iDynTree::InverseKinematicsTreatTargetAsConstraint mode)
    {
        m_defaultTargetResolutionMode = mode;
    }

    enum iDynTree::InverseKinematicsTreatTargetAsConstraint InverseKinematicsData::defaultTargetResolutionMode()
    {
        return m_defaultTargetResolutionMode;
    }

    void InverseKinematicsData::setTargetResolutionMode(TransformMap::iterator target, iDynTree::InverseKinematicsTreatTargetAsConstraint mode)
    {
       assert(target != m_targets.end());
       target->second.setTargetResolutionMode(mode);
    }

    enum iDynTree::InverseKinematicsTreatTargetAsConstraint InverseKinematicsData::targetResolutionMode(TransformMap::iterator target) const
    {
        assert(target != m_targets.end());
        return target->second.targetResolutionMode();
    }

    bool InverseKinematicsData::solveProblem()
    {
        Ipopt::ApplicationReturnStatus solverStatus;

        if (Ipopt::IsNull(m_solver)) {
            m_solver = IpoptApplicationFactory();

            //TODO: set options
            //For example, one needed option is the linear solver type
            //Best thing is to wrap the IPOPT options with new structure so as to abstract them
            m_solver->Options()->SetStringValue("hessian_approximation", "limited-memory");
            m_solver->Options()->SetIntegerValue("print_level",m_verbosityLevel);
            m_solver->Options()->SetIntegerValue("max_iter", m_maxIter);
            m_solver->Options()->SetNumericValue("max_cpu_time", m_maxCpuTime);
            m_solver->Options()->SetNumericValue("tol", m_tol);
            m_solver->Options()->SetNumericValue("constr_viol_tol", m_constrTol);
            m_solver->Options()->SetIntegerValue("acceptable_iter", 5);
            m_solver->Options()->SetStringValue("fixed_variable_treatment", "make_parameter"); //which btw is the default option
#ifndef NDEBUG
            m_solver->Options()->SetStringValue("derivative_test", "first-order");
#endif
            if (!m_solverName.empty()) {
                m_solver->Options()->SetStringValue("linear_solver", m_solverName);
            } else {
                m_solver->Options()->GetStringValue("linear_solver", m_solverName, "");
            }

            solverStatus = m_solver->Initialize();
            if (solverStatus != Ipopt::Solve_Succeeded) {
                return false;
            }
        }

        prepareForOptimization();

        //instantiate the IpOpt problem
        internal::kinematics::InverseKinematicsNLP *iKin = new internal::kinematics::InverseKinematicsNLP(*this);
        //Do something (if necessary)
        Ipopt::SmartPtr<Ipopt::TNLP> problem(iKin);

        // Ask Ipopt to solve the problem
        solverStatus = m_solver->OptimizeTNLP(problem);

        if (solverStatus == Ipopt::Solve_Succeeded || solverStatus == Ipopt::Solved_To_Acceptable_Level ) {
            return true;
        } else {
            return false;
        }
    }

    void InverseKinematicsData::setCoMTarget(iDynTree::Position& desiredPosition, double weight){
        this->m_comTarget.desiredPosition = desiredPosition;

        if (!this->m_comTarget.isActive
            && m_defaultTargetResolutionMode & iDynTree::InverseKinematicsTreatTargetAsConstraintPositionOnly) {
            this->m_comTarget.isConstraint = true;
        }

        if (!(weight < 0)) {
            this->m_comTarget.weight = weight;
        }

        this->m_comTarget.isActive = true;
    }

    void InverseKinematicsData::setCoMasConstraint(bool asConstraint)
    {
        this->m_comTarget.isConstraint = asConstraint;
    }

    bool InverseKinematicsData::isCoMaConstraint()
    {
        return this->m_comTarget.isConstraint;
    }

    void InverseKinematicsData::setCoMasConstraintTolerance(double TOL)
    {
        this->m_comTarget.constraintTolerance = TOL;
    }

    bool InverseKinematicsData::isCoMTargetActive(){
        return this->m_comTarget.isActive;
    }

    void InverseKinematicsData::setCoMTargetInactive()
    {
        this->m_comTarget.isActive = false;
        this->m_comTarget.weight = 0;
        this->m_comTarget.desiredPosition.zero();
    }

}
}
<|endoftext|>


<commit_before>#ifndef PTR_HH
#define PTR_HH

// ======================================================================
//
// ptr: A pointer that is nearly oblivious to its pointee
// (original name: exempt_ptr)
// http://www.open-std.org/JTC1/SC22/WG21/docs/papers/2013/n3514.pdf
// heavily reformatted by Chip
//
// ======================================================================

#include <cstddef>     // nullptr_t
#include <functional>  // less
#include <type_traits> // add_pointer, enable_if, ...
#include <utility>     // swap

namespace ten {

// ======================================================================
// interface:

template <class E> class ptr;

template <class E> void swap(ptr<E> &, ptr<E> &) noexcept;

template <class E> ptr<E> make_ptr(E *) noexcept;

template <class E> bool operator == (ptr<E> const &, ptr<E> const &);
template <class E> bool operator != (ptr<E> const &, ptr<E> const &);
template <class E> bool operator == (ptr<E> const &, std::nullptr_t) noexcept;
template <class E> bool operator != (ptr<E> const &, std::nullptr_t) noexcept;
template <class E> bool operator == (std::nullptr_t, ptr<E> const &) noexcept;
template <class E> bool operator != (std::nullptr_t, ptr<E> const &) noexcept;
template <class E> bool operator <  (ptr<E> const &, ptr<E> const &);
template <class E> bool operator >  (ptr<E> const &, ptr<E> const &);
template <class E> bool operator <= (ptr<E> const &, ptr<E> const &);
template <class E> bool operator >= (ptr<E> const &, ptr<E> const &);

// ======================================================================
// implementation:

template <class E>
class ptr {
  public:
    // publish our template parameter and variations thereof:
    using element_type = E;
    using pointer = typename std::add_pointer<E>::type;
    using reference = typename std::add_lvalue_reference<E>::type;

  private:
    template <class P>
    constexpr bool is_compat() {
        return std::is_convertible< typename std::add_pointer<P>::type
                                    , pointer
                                  >::value;
    }

  public:
//construction:
    constexpr ptr() noexcept : p() {}
    constexpr ptr(std::nullptr_t) noexcept : p() {}
    explicit ptr(pointer other) noexcept : p(other) {}

    template <class E2, class = typename std::enable_if< is_compat<E2>() >::type>
    explicit ptr(E2 *other) noexcept : p(other) {}

    template <class E2, class = typename std::enable_if< is_compat<E2>() >::type>
    ptr(ptr<E2> const &other) noexcept : p(other.get()) {}

//assignment:
    ptr & operator = (std::nullptr_t) noexcept { reset(nullptr); return *this; }

    template <class E2>
    typename std::enable_if< is_compat<E2>(), ptr & >::type
    operator = (E2 *other) noexcept
        { reset(other); return *this; }

    template <class E2>
    typename std::enable_if< is_compat<E2>(), ptr & >::type
    operator = (ptr<E2> const &other) noexcept
        { reset(other.get()); return *this; }

// observers:
    pointer get() const noexcept { return p; }
    reference operator * () const noexcept { return *get(); }
    pointer operator -> () const noexcept { return get(); }
    explicit operator bool () const noexcept { return get(); }
    bool operator ! () const noexcept { return !get(); }
// modifiers:
    pointer release() noexcept { pointer old = get(); reset(); return old; }
    void reset(pointer t = nullptr) noexcept { p = t; }
    void swap(ptr &other) noexcept { std::swap(p, other.p); }
  private:
    pointer p;
};

// -----------------------------------------------
// non-member swap:
template <class E> inline void swap(ptr<E> &x, ptr<E> &y) noexcept { x.swap(y); }

// -----------------------------------------------
// non-member make_ptr:
template <class E> inline ptr<E> make_ptr(E *p) noexcept { return ptr<E>{p}; }

// -----------------------------------------------
// non-member (in)equality comparison:
template <class E> inline bool operator == (ptr<E> const &x, ptr<E> const &y) { return x.get() == y.get(); }
template <class E> inline bool operator != (ptr<E> const &x, ptr<E> const &y) { return x.get() != y.get(); }
template <class E> inline bool operator == (ptr<E> const &x, std::nullptr_t)  noexcept { return !x; }
template <class E> inline bool operator != (ptr<E> const &x, std::nullptr_t)  noexcept { return  x; }
template <class E> inline bool operator == (std::nullptr_t,  ptr<E> const &y) noexcept { return !y; }
template <class E> inline bool operator != (std::nullptr_t,  ptr<E> const &y) noexcept { return  y; }

// -----------------------------------------------
// non-member ordering:
template <class E>
inline bool operator < (ptr<E> const &x, ptr<E> const &y) {
    using PTR = typename ptr<E>::pointer;
    return std::less<PTR>()(x.get(), y.get());
}
template <class E> inline bool operator >  (ptr<E> const &x, ptr<E> const &y) { return y < x; }
template <class E> inline bool operator <= (ptr<E> const &x, ptr<E> const &y) { return !(y < x); }
template <class E> inline bool operator >= (ptr<E> const &x, ptr<E> const &y) { return !(x < y); }

} // end namespace ten

#endif // PTR_HH
<commit_msg>make ptr::is_compat<> static, to fix obscure compilation errs<commit_after>#ifndef PTR_HH
#define PTR_HH

// ======================================================================
//
// ptr: A pointer that is nearly oblivious to its pointee
// (original name: exempt_ptr)
// http://www.open-std.org/JTC1/SC22/WG21/docs/papers/2013/n3514.pdf
// heavily reformatted by Chip
//
// ======================================================================

#include <cstddef>     // nullptr_t
#include <functional>  // less
#include <type_traits> // add_pointer, enable_if, ...
#include <utility>     // swap

namespace ten {

// ======================================================================
// interface:

template <class E> class ptr;

template <class E> void swap(ptr<E> &, ptr<E> &) noexcept;

template <class E> ptr<E> make_ptr(E *) noexcept;

template <class E> bool operator == (ptr<E> const &, ptr<E> const &);
template <class E> bool operator != (ptr<E> const &, ptr<E> const &);
template <class E> bool operator == (ptr<E> const &, std::nullptr_t) noexcept;
template <class E> bool operator != (ptr<E> const &, std::nullptr_t) noexcept;
template <class E> bool operator == (std::nullptr_t, ptr<E> const &) noexcept;
template <class E> bool operator != (std::nullptr_t, ptr<E> const &) noexcept;
template <class E> bool operator <  (ptr<E> const &, ptr<E> const &);
template <class E> bool operator >  (ptr<E> const &, ptr<E> const &);
template <class E> bool operator <= (ptr<E> const &, ptr<E> const &);
template <class E> bool operator >= (ptr<E> const &, ptr<E> const &);

// ======================================================================
// implementation:

template <class E>
class ptr {
  public:
    // publish our template parameter and variations thereof:
    using element_type = E;
    using pointer = typename std::add_pointer<E>::type;
    using reference = typename std::add_lvalue_reference<E>::type;

  private:
    template <class P>
    static constexpr bool is_compat() {
        return std::is_convertible< typename std::add_pointer<P>::type
                                    , pointer
                                  >::value;
    }

  public:
//construction:
    constexpr ptr() noexcept : p() {}
    constexpr ptr(std::nullptr_t) noexcept : p() {}
    explicit ptr(pointer other) noexcept : p(other) {}

    template <class E2, class = typename std::enable_if< is_compat<E2>() >::type>
    explicit ptr(E2 *other) noexcept : p(other) {}

    template <class E2, class = typename std::enable_if< is_compat<E2>() >::type>
    ptr(ptr<E2> const &other) noexcept : p(other.get()) {}

//assignment:
    ptr & operator = (std::nullptr_t) noexcept { reset(nullptr); return *this; }

    template <class E2>
    typename std::enable_if< is_compat<E2>(), ptr & >::type
    operator = (E2 *other) noexcept
        { reset(other); return *this; }

    template <class E2>
    typename std::enable_if< is_compat<E2>(), ptr & >::type
    operator = (ptr<E2> const &other) noexcept
        { reset(other.get()); return *this; }

// observers:
    pointer get() const noexcept { return p; }
    reference operator * () const noexcept { return *get(); }
    pointer operator -> () const noexcept { return get(); }
    explicit operator bool () const noexcept { return get(); }
    bool operator ! () const noexcept { return !get(); }
// modifiers:
    pointer release() noexcept { pointer old = get(); reset(); return old; }
    void reset(pointer t = nullptr) noexcept { p = t; }
    void swap(ptr &other) noexcept { std::swap(p, other.p); }
  private:
    pointer p;
};

// -----------------------------------------------
// non-member swap:
template <class E> inline void swap(ptr<E> &x, ptr<E> &y) noexcept { x.swap(y); }

// -----------------------------------------------
// non-member make_ptr:
template <class E> inline ptr<E> make_ptr(E *p) noexcept { return ptr<E>{p}; }

// -----------------------------------------------
// non-member (in)equality comparison:
template <class E> inline bool operator == (ptr<E> const &x, ptr<E> const &y) { return x.get() == y.get(); }
template <class E> inline bool operator != (ptr<E> const &x, ptr<E> const &y) { return x.get() != y.get(); }
template <class E> inline bool operator == (ptr<E> const &x, std::nullptr_t)  noexcept { return !x; }
template <class E> inline bool operator != (ptr<E> const &x, std::nullptr_t)  noexcept { return  x; }
template <class E> inline bool operator == (std::nullptr_t,  ptr<E> const &y) noexcept { return !y; }
template <class E> inline bool operator != (std::nullptr_t,  ptr<E> const &y) noexcept { return  y; }

// -----------------------------------------------
// non-member ordering:
template <class E>
inline bool operator < (ptr<E> const &x, ptr<E> const &y) {
    using PTR = typename ptr<E>::pointer;
    return std::less<PTR>()(x.get(), y.get());
}
template <class E> inline bool operator >  (ptr<E> const &x, ptr<E> const &y) { return y < x; }
template <class E> inline bool operator <= (ptr<E> const &x, ptr<E> const &y) { return !(y < x); }
template <class E> inline bool operator >= (ptr<E> const &x, ptr<E> const &y) { return !(x < y); }

} // end namespace ten

#endif // PTR_HH
<|endoftext|>


<commit_before>/*
    Copyright (C) 2007, 2008 Nikolas Zimmermann <zimmermann@kde.org>

    This file is part of the KDE project

    This library is free software you can redistribute it and/or
    modify it under the terms of the GNU Library General Public
    License as published by the Free Software Foundation either
    version 2 of the License, or (at your option) any later version.

    This library 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
    Library General Public License for more details.

    You should have received a copy of the GNU Library General Public License
    aint with this library see the file COPYING.LIB.  If not, write to
    the Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor,
    Boston, MA 02110-1301, USA.
*/

#include "KMLFolderTagHandler.h"

#include <QtCore/QDebug>

#include "KMLElementDictionary.h"
#include "GeoDataFolder.h"
#include "GeoDataParser.h"

using namespace GeoDataElementDictionary;

KML_DEFINE_TAG_HANDLER(Folder)

KMLFolderTagHandler::KMLFolderTagHandler()
    : GeoTagHandler()
{
}

KMLFolderTagHandler::~KMLFolderTagHandler()
{
}

GeoNode* KMLFolderTagHandler::parse(GeoParser& parser) const
{
    Q_ASSERT(parser.isStartElement() && parser.isValidElement(kmlTag_Folder));

    GeoDataFolder* folder = new GeoDataFolder;

    GeoStackItem parentItem = parser.parentElement();
    if (parentItem.represents(kmlTag_Folder) || parentItem.represents(kmlTag_Document))
        parentItem.nodeAs<GeoDataContainer>()->addFeature(folder);

    qDebug() << "Parsed <Folder> start! Created GeoDataFolder item: " << folder << " parent item name: " << parentItem.qualifiedName().first << " associated item: " << parentItem.associatedNode();
    return folder;
}
<commit_msg>Break extra long line.<commit_after>/*
    Copyright (C) 2007, 2008 Nikolas Zimmermann <zimmermann@kde.org>

    This file is part of the KDE project

    This library is free software you can redistribute it and/or
    modify it under the terms of the GNU Library General Public
    License as published by the Free Software Foundation either
    version 2 of the License, or (at your option) any later version.

    This library 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
    Library General Public License for more details.

    You should have received a copy of the GNU Library General Public License
    aint with this library see the file COPYING.LIB.  If not, write to
    the Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor,
    Boston, MA 02110-1301, USA.
*/

#include "KMLFolderTagHandler.h"

#include <QtCore/QDebug>

#include "KMLElementDictionary.h"
#include "GeoDataFolder.h"
#include "GeoDataParser.h"

using namespace GeoDataElementDictionary;

KML_DEFINE_TAG_HANDLER(Folder)

KMLFolderTagHandler::KMLFolderTagHandler()
    : GeoTagHandler()
{
}

KMLFolderTagHandler::~KMLFolderTagHandler()
{
}

GeoNode* KMLFolderTagHandler::parse(GeoParser& parser) const
{
    Q_ASSERT(parser.isStartElement() && parser.isValidElement(kmlTag_Folder));

    GeoDataFolder* folder = new GeoDataFolder;

    GeoStackItem parentItem = parser.parentElement();
    if (parentItem.represents(kmlTag_Folder) || parentItem.represents(kmlTag_Document))
        parentItem.nodeAs<GeoDataContainer>()->addFeature(folder);

    qDebug() << "Parsed <Folder> start! Created GeoDataFolder item: " << folder
             << " parent item name: " << parentItem.qualifiedName().first
             << " associated item: " << parentItem.associatedNode();
    return folder;
}
<|endoftext|>