<commit_before>//
// TcpSocket.cpp for TcpSocket in
//
// Made by mognetworkhrabi Alexandre
// Login   <alexmog@epitech.net>
//
// Started on  Thu Jun  5 20:09:34 2014 mognetworkhrabi Alexandre
// Last update Mon Mar  9 18:50:19 2015 Moghrabi Alexandre
//

#include "mognetwork/OS.hh"

#include <sys/types.h>
#ifndef OS_WINDOWS
#include <sys/socket.h>
#include <netdb.h>
#else
#include <winsock2.h>
#include <Ws2tcpip.h>
#endif // !OS_WINDOWS
#include <algorithm>
#include <cstring>
#include <iostream>
#include "mognetwork/TcpSocket.hh"
#include "mognetwork/OsSocket.hh"
#include "mognetwork/Packet.hh"

namespace
{
#ifdef OS_LINUX
  const int flags = MSG_NOSIGNAL;
#else
  const int flags = 0;
#endif
}

// ! Every packets sends a header (who contains the size of the packet) BEFORE its content.
namespace mognetwork
{
  bool deleteAll(TcpSocket::Data* elem) {delete elem; return true;}

  TcpSocket::TcpSocket() :
    Socket(Tcp), m_userData(NULL)
  {
    // Create the socket, and init it
    create();
  }

  TcpSocket::TcpSocket(SocketFD fd) :
    Socket(Tcp), m_userData(NULL)
  {
    create(fd);
  }

  Socket::Status TcpSocket::connect(const IpAddress& rAddress, unsigned short port)
  {
    // Create the sockaddr by the OS
    sockaddr_in address = OsSocket::createAddress(rAddress.getInt(), port);

    // connect the socket to the address
    return (::connect(getSocketFD(), reinterpret_cast<sockaddr*>(&address), sizeof(address))  == -1 ? OsSocket::getErrorStatus() : Ok);
  }

  void TcpSocket::disconnect()
  {
    close();
    m_pendingDatas.remove_if(deleteAll);
    m_pendingRDatas = ReadedDatas();
  }

  Socket::Status TcpSocket::receive(char* data, std::size_t size, std::size_t& received, int _flags)
  {
    received = 0;
    if (!data)
      {
	std::cerr << "Cannot receive datas if the buffer is null." << std::endl;
	return (Error);
      }
    int readed = recv(getSocketFD(), data, static_cast<int>(size), flags | _flags);
    if (readed > 0)
      {
	received = static_cast<std::size_t>(readed);
	return (Ok);
      }
    else if (readed == 0)
      return (Disconnected);
    else
      return (OsSocket::getErrorStatus());
  }

  Socket::Status TcpSocket::receiveAll(Data& data)
  {
    std::size_t readed = 0;
    char buffer[1024]; // Buffer to get the datas
    std::size_t numReaded = 0;

    ReadedDatas _data = ReadedDatas();
    data.clear();
    // Read the size of the pending packet
    while (readed < sizeof(std::size_t))
      {
	char* d = reinterpret_cast<char*>(&_data.totalSize) + numReaded;
	Socket::Status status = receive(d, sizeof(std::size_t) - _data.readed, readed, 0);
	numReaded += readed;
	if (status != Ok)
	  return (status);
      }
    // Size is set, let's read the content!
    if (numReaded >= sizeof(std::size_t))
      {
	while (_data.readed < _data.totalSize)
	  {
	    std::size_t toGet = std::min(static_cast<std::size_t>(_data.totalSize - _data.readed), sizeof(buffer));
	    Socket::Status status = receive(buffer, toGet, readed, 0);
	    if (status != Ok)
	      return (status);
	    _data.readed += readed;
	    if (readed > 0)
	      {
		data.resize(data.size() + readed);
		char* begin = &data[0] + data.size() - readed;
		std::memcpy(begin, buffer, readed);
	      }
	    if (_data.readed >= _data.totalSize)
	      return (Ok);
	  }
      }
    return (Error);
  }

  Socket::Status TcpSocket::asyncSend(const char* data, std::size_t size)
  {
    if (!data || (size == 0))
      {
	std::cerr << "Cannot send null data" << std::endl;
	return (Error);
      }
    Data* _data = new Data;
    _data->resize(size + sizeof(std::size_t));
    std::memcpy(&_data->front(), &size, sizeof(std::size_t));
    std::memcpy(&_data->front() + sizeof(std::size_t), data, size);
    m_mutex.lock();
    m_pendingDatas.push_back(_data);
    m_mutex.unlock();
    return (Ok);
  }

  bool TcpSocket::havingPendingDatas()
  {
    m_mutex.lock();
    bool status = m_pendingDatas.empty();
    m_mutex.unlock();
    return (status);
  }

  Socket::Status TcpSocket::sendPendingDatas()
  {
    m_mutex.lock();
    if (!m_pendingDatas.empty())
      {
	int sended;
	DataList::iterator it = m_pendingDatas.begin();
	sended = ::send(getSocketFD(), &(*it)->front(), (*it)->size(), flags | MSG_DONTWAIT);
	if (sended < 0) {
	  m_mutex.unlock();
	  return (OsSocket::getErrorStatus());
	}
	if (sended == 0) {
	  m_mutex.unlock();
	  return (Disconnected);
	}
	if ((std::size_t)sended == (*it)->size())
	  {
	    delete *it;
	    m_pendingDatas.pop_front();
	    m_mutex.unlock();
	    return (sendPendingDatas());
	  }
	Data temp;
	temp.resize((*it)->size());
	std::memcpy(&temp[0], &(*it)->front(), (*it)->size());
	std::size_t waiting = (*it)->size() - sended;
	(*it)->resize(waiting);
	std::memcpy(&(*it)->front(), &temp[0], waiting);
	m_mutex.unlock();
	return (Ok);
      }
    m_mutex.unlock();
    return (Nok);
  }

  // Normal send method, waits until the server has received everything.
  Socket::Status TcpSocket::send(const char* data, std::size_t size)
  {
    int sent;

    if (!data || (size == 0))
      {
	std::cerr << "Cannot send null data" << std::endl;
	return (Error);
      }
    Data dat;
    dat.resize(size + sizeof(std::size_t));
    std::memcpy(&dat[0], &size, sizeof(std::size_t));
    std::memcpy(&dat[0] + sizeof(std::size_t), data, size);
    sent = ::send(getSocketFD(), &dat[0], dat.size(), 0);
    if (sent < 0)
      return (OsSocket::getErrorStatus());
    if (sent == 0)
      return (Disconnected);
    return (Ok);
  }

  void TcpSocket::setUserData(void* userData)
  {
    m_mutex.lock();
    m_userData = userData;
    m_mutex.unlock();
  }

  void* TcpSocket::getUserData() const
  {
    return (m_userData);
  }

  Socket::Status TcpSocket::readPendingDatas()
  {
    std::size_t readed;
    char buffer[1024]; // Buffer to get the datas

    // Read the size of the pending packet
    if (m_pendingRDatas.readed < sizeof(std::size_t))
      {
	char* data = reinterpret_cast<char*>(&m_pendingRDatas.totalSize) + m_pendingRDatas.readed;
	Socket::Status status = receive(data, sizeof(std::size_t) - m_pendingRDatas.readed, readed, MSG_DONTWAIT);
	m_pendingRDatas.readed += readed;
	if (status != Ok)
	  return (status);
      }
    // Size is set, let's read the content!
    if (m_pendingRDatas.readed >= sizeof(std::size_t))
      {
	std::size_t toGet = std::min(static_cast<std::size_t>(m_pendingRDatas.totalSize - m_pendingRDatas.datas.size()), sizeof(buffer));

	Socket::Status status = receive(buffer, toGet, readed, MSG_DONTWAIT);
	if (status != Ok)
	  return (status);
	m_pendingRDatas.readed += readed;
	if (readed > 0)
	  {
	    m_pendingRDatas.datas.resize(m_pendingRDatas.datas.size() + readed);
	    char* begin = &m_pendingRDatas.datas[0] + m_pendingRDatas.datas.size() - readed;
	    std::memcpy(begin, buffer, readed);
	  }
	if (m_pendingRDatas.readed >= m_pendingRDatas.totalSize)
	  {
	    m_allDataReaded = new ReadedDatas();
	    m_allDataReaded->datas = m_pendingRDatas.datas;
	    m_allDataReaded->readed = m_pendingRDatas.readed;
	    m_allDataReaded->totalSize = m_pendingRDatas.totalSize;
	    m_pendingRDatas = ReadedDatas();
	    return (Ok);
	  }
      }
    return (Waiting);
  }

  TcpSocket::ReadedDatas* TcpSocket::getDatasReaded() const
  {
    return (m_allDataReaded);
  }

  void TcpSocket::setServer(TcpASIOServer* server)
  {
    m_mutex.lock();
    m_server = server;
    m_mutex.unlock();
  }

  TcpASIOServer* TcpSocket::getServer() const
  {
    return m_server;
  }

  Packet* TcpSocket::getPacketReaded()
  {
    return new Packet(m_allDataReaded);
  }

  TcpSocket::ReadedDatas::ReadedDatas() :
    readed(0),
    totalSize(0),
    datas()
  {}
} // namespace mognetwork
<commit_msg>Removing some mutex... dafuq...<commit_after>//
// TcpSocket.cpp for TcpSocket in
//
// Made by mognetworkhrabi Alexandre
// Login   <alexmog@epitech.net>
//
// Started on  Thu Jun  5 20:09:34 2014 mognetworkhrabi Alexandre
// Last update Mon Mar 23 10:27:39 2015 Moghrabi Alexandre
//

#include "mognetwork/OS.hh"

#include <sys/types.h>
#ifndef OS_WINDOWS
#include <sys/socket.h>
#include <netdb.h>
#else
#include <winsock2.h>
#include <Ws2tcpip.h>
#endif // !OS_WINDOWS
#include <algorithm>
#include <cstring>
#include <iostream>
#include "mognetwork/TcpSocket.hh"
#include "mognetwork/OsSocket.hh"
#include "mognetwork/Packet.hh"

namespace
{
#ifdef OS_LINUX
  const int flags = MSG_NOSIGNAL;
#else
  const int flags = 0;
#endif
}

// ! Every packets sends a header (who contains the size of the packet) BEFORE its content.
namespace mognetwork
{
  bool deleteAll(TcpSocket::Data* elem) {delete elem; return true;}

  TcpSocket::TcpSocket() :
    Socket(Tcp), m_userData(NULL)
  {
    // Create the socket, and init it
    create();
  }

  TcpSocket::TcpSocket(SocketFD fd) :
    Socket(Tcp), m_userData(NULL)
  {
    create(fd);
  }

  Socket::Status TcpSocket::connect(const IpAddress& rAddress, unsigned short port)
  {
    // Create the sockaddr by the OS
    sockaddr_in address = OsSocket::createAddress(rAddress.getInt(), port);

    // connect the socket to the address
    return (::connect(getSocketFD(), reinterpret_cast<sockaddr*>(&address), sizeof(address))  == -1 ? OsSocket::getErrorStatus() : Ok);
  }

  void TcpSocket::disconnect()
  {
    close();
    m_pendingDatas.remove_if(deleteAll);
    m_pendingRDatas = ReadedDatas();
  }

  Socket::Status TcpSocket::receive(char* data, std::size_t size, std::size_t& received, int _flags)
  {
    received = 0;
    if (!data)
      {
	std::cerr << "Cannot receive datas if the buffer is null." << std::endl;
	return (Error);
      }
    int readed = recv(getSocketFD(), data, static_cast<int>(size), flags | _flags);
    if (readed > 0)
      {
	received = static_cast<std::size_t>(readed);
	return (Ok);
      }
    else if (readed == 0)
      return (Disconnected);
    else
      return (OsSocket::getErrorStatus());
  }

  Socket::Status TcpSocket::receiveAll(Data& data)
  {
    std::size_t readed = 0;
    char buffer[1024]; // Buffer to get the datas
    std::size_t numReaded = 0;

    ReadedDatas _data = ReadedDatas();
    data.clear();
    // Read the size of the pending packet
    while (readed < sizeof(std::size_t))
      {
	char* d = reinterpret_cast<char*>(&_data.totalSize) + numReaded;
	Socket::Status status = receive(d, sizeof(std::size_t) - _data.readed, readed, 0);
	numReaded += readed;
	if (status != Ok)
	  return (status);
      }
    // Size is set, let's read the content!
    if (numReaded >= sizeof(std::size_t))
      {
	while (_data.readed < _data.totalSize)
	  {
	    std::size_t toGet = std::min(static_cast<std::size_t>(_data.totalSize - _data.readed), sizeof(buffer));
	    Socket::Status status = receive(buffer, toGet, readed, 0);
	    if (status != Ok)
	      return (status);
	    _data.readed += readed;
	    if (readed > 0)
	      {
		data.resize(data.size() + readed);
		char* begin = &data[0] + data.size() - readed;
		std::memcpy(begin, buffer, readed);
	      }
	    if (_data.readed >= _data.totalSize)
	      return (Ok);
	  }
      }
    return (Error);
  }

  Socket::Status TcpSocket::asyncSend(const char* data, std::size_t size)
  {
    if (!data || (size == 0))
      {
	std::cerr << "Cannot send null data" << std::endl;
	return (Error);
      }
    Data* _data = new Data;
    _data->resize(size + sizeof(std::size_t));
    std::memcpy(&_data->front(), &size, sizeof(std::size_t));
    std::memcpy(&_data->front() + sizeof(std::size_t), data, size);
    m_mutex.lock();
    m_pendingDatas.push_back(_data);
    m_mutex.unlock();
    return (Ok);
  }

  bool TcpSocket::havingPendingDatas()
  {
    m_mutex.lock();
    bool status = m_pendingDatas.empty();
    m_mutex.unlock();
    return (status);
  }

  Socket::Status TcpSocket::sendPendingDatas()
  {
    //    m_mutex.lock();
    if (!m_pendingDatas.empty())
      {
	int sended;
	DataList::iterator it = m_pendingDatas.begin();
	sended = ::send(getSocketFD(), &(*it)->front(), (*it)->size(), flags | MSG_DONTWAIT);
	if (sended < 0) {
	  //	  m_mutex.unlock();
	  return (OsSocket::getErrorStatus());
	}
	if (sended == 0) {
	  //m_mutex.unlock();
	  return (Disconnected);
	}
	if ((std::size_t)sended == (*it)->size())
	  {
	    delete *it;
	    m_pendingDatas.pop_front();
	    //m_mutex.unlock();
	    return (sendPendingDatas());
	  }
	Data temp;
	temp.resize((*it)->size());
	std::memcpy(&temp[0], &(*it)->front(), (*it)->size());
	std::size_t waiting = (*it)->size() - sended;
	(*it)->resize(waiting);
	std::memcpy(&(*it)->front(), &temp[0], waiting);
	//	m_mutex.unlock();
	return (Ok);
      }
    //    m_mutex.unlock();
    return (Nok);
  }

  // Normal send method, waits until the server has received everything.
  Socket::Status TcpSocket::send(const char* data, std::size_t size)
  {
    int sent;

    if (!data || (size == 0))
      {
	std::cerr << "Cannot send null data" << std::endl;
	return (Error);
      }
    Data dat;
    dat.resize(size + sizeof(std::size_t));
    std::memcpy(&dat[0], &size, sizeof(std::size_t));
    std::memcpy(&dat[0] + sizeof(std::size_t), data, size);
    sent = ::send(getSocketFD(), &dat[0], dat.size(), 0);
    if (sent < 0)
      return (OsSocket::getErrorStatus());
    if (sent == 0)
      return (Disconnected);
    return (Ok);
  }

  void TcpSocket::setUserData(void* userData)
  {
    m_mutex.lock();
    m_userData = userData;
    m_mutex.unlock();
  }

  void* TcpSocket::getUserData() const
  {
    return (m_userData);
  }

  Socket::Status TcpSocket::readPendingDatas()
  {
    std::size_t readed;
    char buffer[1024]; // Buffer to get the datas

    // Read the size of the pending packet
    if (m_pendingRDatas.readed < sizeof(std::size_t))
      {
	char* data = reinterpret_cast<char*>(&m_pendingRDatas.totalSize) + m_pendingRDatas.readed;
	Socket::Status status = receive(data, sizeof(std::size_t) - m_pendingRDatas.readed, readed, MSG_DONTWAIT);
	m_pendingRDatas.readed += readed;
	if (status != Ok)
	  return (status);
      }
    // Size is set, let's read the content!
    if (m_pendingRDatas.readed >= sizeof(std::size_t))
      {
	std::size_t toGet = std::min(static_cast<std::size_t>(m_pendingRDatas.totalSize - m_pendingRDatas.datas.size()), sizeof(buffer));

	Socket::Status status = receive(buffer, toGet, readed, MSG_DONTWAIT);
	if (status != Ok)
	  return (status);
	m_pendingRDatas.readed += readed;
	if (readed > 0)
	  {
	    m_pendingRDatas.datas.resize(m_pendingRDatas.datas.size() + readed);
	    char* begin = &m_pendingRDatas.datas[0] + m_pendingRDatas.datas.size() - readed;
	    std::memcpy(begin, buffer, readed);
	  }
	if (m_pendingRDatas.readed >= m_pendingRDatas.totalSize)
	  {
	    m_allDataReaded = new ReadedDatas();
	    m_allDataReaded->datas = m_pendingRDatas.datas;
	    m_allDataReaded->readed = m_pendingRDatas.readed;
	    m_allDataReaded->totalSize = m_pendingRDatas.totalSize;
	    m_pendingRDatas = ReadedDatas();
	    return (Ok);
	  }
      }
    return (Waiting);
  }

  TcpSocket::ReadedDatas* TcpSocket::getDatasReaded() const
  {
    return (m_allDataReaded);
  }

  void TcpSocket::setServer(TcpASIOServer* server)
  {
    m_mutex.lock();
    m_server = server;
    m_mutex.unlock();
  }

  TcpASIOServer* TcpSocket::getServer() const
  {
    return m_server;
  }

  Packet* TcpSocket::getPacketReaded()
  {
    return new Packet(m_allDataReaded);
  }

  TcpSocket::ReadedDatas::ReadedDatas() :
    readed(0),
    totalSize(0),
    datas()
  {}
} // namespace mognetwork
<|endoftext|>


<commit_before>/* Copyright (c) 2016-2016, Vasil Dimov

Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
the Software, and to permit persons to whom the Software is furnished to do so,
subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */

#include <curl/curl.h>
#include <iostream>
#include <memory>
#include <sstream>
#include <stdexcept>
#include <string>
#include <vector>

#include <ipfs/http/transport-curl.h>
#include <ipfs/test/utils.h>

namespace ipfs {

namespace http {

/** CURL global initialization. `curl_global_init()` needs to be called before
 * any other threads have started executing because it is not thread safe and
 * may call other non-thread safe functions. Thus we use a global variable and
 * call `curl_global_init()` in its constructor. */
class CurlGlobal {
 public:
  /** Constructor that calls curl_global_init(). */
  CurlGlobal();

  /** Destructor that calls curl_global_cleanup(). */
  ~CurlGlobal();

  /** The result from curl_global_init() for checking the outcome later. */
  CURLcode result_;
};

CurlGlobal::CurlGlobal() { result_ = curl_global_init(CURL_GLOBAL_ALL); }
CurlGlobal::~CurlGlobal() { curl_global_cleanup(); }

/** Call curl_global_init() before any other threads have started executing.
 * This does not work with DLLs on Windows,
 * @see https://github.com/curl/curl/issues/586. */
static const CurlGlobal curl_global;

/** Flag to simulate a failure of curl_global_init(). */
static bool curl_global_injected_failure = false;

#ifndef NDEBUG
/** Placeholder to inject a fake http response during testing. */
std::string replace_body;
#endif /* NDEBUG */

/** Check if a HTTP status code is 2xx Success.
 * @return true if 2xx */
inline bool status_is_success(long code) { return code >= 200 && code <= 299; }

/** CURL callback for writing the result to a stream. */
static size_t curl_cb_stream(
    /** [in] Pointer to the result. */
    char* ptr,
    /** [in] Size each chunk of the result. */
    size_t size,
    /** [in] Number of chunks in the result. */
    size_t nmemb,
    /** [out] Response (a pointer to `std::iostream`). */
    void* response_void) {
  std::iostream* response = static_cast<std::iostream*>(response_void);

  const size_t n = size * nmemb;

  response->write(ptr, n);

  return n;
}

TransportCurl::TransportCurl() : curl_is_setup_(false) {
  if (curl_global.result_ != CURLE_OK || curl_global_injected_failure) {
    throw std::runtime_error("curl_global_init() failed");
  }
  static_assert(sizeof(curl_error_) >= CURL_ERROR_SIZE,
                "The size of curl_error_ is too small");
}

TransportCurl::~TransportCurl() { HandleDestroy(); }

void TransportCurl::Fetch(const std::string& url,
                          const std::vector<FileUpload>& files,
                          std::iostream* response) {
  HandleSetup();

  /* https://curl.haxx.se/libcurl/c/CURLOPT_POST.html */
  curl_easy_setopt(curl_, CURLOPT_POST, 1);

  curl_httppost* form_parts = NULL;
  curl_httppost* form_parts_end = NULL;

  for (size_t i = 0; i < files.size(); ++i) {
    const FileUpload& file = files[i];
    const std::string name("file" + std::to_string(i));
    static const char* content_type = "application/octet-stream";

    switch (file.type) {
      case FileUpload::Type::kFileContents:
        /* https://curl.haxx.se/libcurl/c/curl_formadd.html */
        curl_formadd(&form_parts, &form_parts_end,
                     /* name="..."; */
                     CURLFORM_COPYNAME, name.c_str(),
                     /* filename="..."; */
                     CURLFORM_BUFFER, file.path.c_str(),
                     /* File contents. */
                     CURLFORM_BUFFERPTR, file.data.c_str(),
                     /* File contents length. */
                     CURLFORM_BUFFERLENGTH, file.data.length(),
                     /* Content-Type: */
                     CURLFORM_CONTENTTYPE, content_type, CURLFORM_END);
        break;
      case FileUpload::Type::kFileName:
        /* https://curl.haxx.se/libcurl/c/curl_formadd.html */
        curl_formadd(&form_parts, &form_parts_end,
                     /* name="..."; */
                     CURLFORM_COPYNAME, name.c_str(),
                     /* filename="..."; */
                     CURLFORM_FILENAME, file.path.c_str(),
                     /* Read the data from this file. */
                     CURLFORM_FILE, file.data.c_str(),
                     /* Content-Type: */
                     CURLFORM_CONTENTTYPE, content_type, CURLFORM_END);
        break;
    }
  }

  /* Auto free the resources occupied by `form_parts`. */
  std::unique_ptr<curl_httppost, void (*)(curl_httppost*)> form_parts_deleter(
      form_parts, [](curl_httppost* d) {
        /* https://curl.haxx.se/libcurl/c/curl_formfree.html */
        curl_formfree(d);
      });

  /* https://curl.haxx.se/libcurl/c/CURLOPT_HTTPPOST.html */
  curl_easy_setopt(curl_, CURLOPT_HTTPPOST, form_parts);

  curl_slist* headers = NULL;
  /* https://curl.haxx.se/libcurl/c/curl_slist_append.html */
  headers = curl_slist_append(headers, "Expect:");

  /* Auto free the resources occupied by `headers`. */
  std::unique_ptr<curl_slist, void (*)(curl_slist*)> headers_deleter(
      headers, [](curl_slist* d) {
        /* https://curl.haxx.se/libcurl/c/curl_slist_free_all.html */
        curl_slist_free_all(d);
      });

  /* https://curl.haxx.se/libcurl/c/CURLOPT_HTTPHEADER.html */
  curl_easy_setopt(curl_, CURLOPT_HTTPHEADER, headers);

#ifndef NDEBUG
  if (!replace_body.empty()) {
    *response << replace_body;
    return;
  }
#endif /* NDEBUG */

  Perform(url, response);
}

void TransportCurl::UrlEncode(const std::string& raw, std::string* encoded) {
  HandleSetup();

  char* encoded_c = curl_easy_escape(curl_, raw.c_str(), 0);
  if (encoded_c == NULL || url_encode_injected_failure) {
    throw std::runtime_error("curl_easy_escape() failed on \"" + raw + "\"");
  }
  std::unique_ptr<char, void (*)(char*)> encoded_c_deleter(
      encoded_c, [](char* e) { curl_free(e); });

  encoded->assign(encoded_c);
}

void TransportCurl::HandleSetup() {
  if (curl_is_setup_) {
    return;
  }

  curl_ = curl_easy_init();
  if (curl_ == NULL || handle_setup_injected_failure) {
    throw std::runtime_error("curl_easy_init() failed");
  }

  /* https://curl.haxx.se/libcurl/c/CURLOPT_ERRORBUFFER.html */
  curl_easy_setopt(curl_, CURLOPT_ERRORBUFFER, curl_error_);

  /* Enable TCP keepalive.
   * https://curl.haxx.se/libcurl/c/CURLOPT_TCP_KEEPALIVE.html */
  curl_easy_setopt(curl_, CURLOPT_TCP_KEEPALIVE, 1);

  /* Seconds to wait before sending keep-alive packets.
   * https://curl.haxx.se/libcurl/c/CURLOPT_TCP_KEEPIDLE.html */
  curl_easy_setopt(curl_, CURLOPT_TCP_KEEPIDLE, 30);

  /* Seconds between keep-alive probes.
   * https://curl.haxx.se/libcurl/c/CURLOPT_TCP_KEEPINTVL.html */
  curl_easy_setopt(curl_, CURLOPT_TCP_KEEPINTVL, 10);

  /* https://curl.haxx.se/libcurl/c/CURLOPT_USERAGENT.html */
  curl_easy_setopt(curl_, CURLOPT_USERAGENT, "cpp-ipfs-api");

  curl_is_setup_ = true;
}

void TransportCurl::Perform(const std::string& url, std::iostream* response) {
  /* https://curl.haxx.se/libcurl/c/CURLOPT_URL.html */
  curl_easy_setopt(curl_, CURLOPT_URL, url.c_str());

  /* https://curl.haxx.se/libcurl/c/CURLOPT_WRITEFUNCTION.html */
  curl_easy_setopt(curl_, CURLOPT_WRITEFUNCTION, curl_cb_stream);

  /* https://curl.haxx.se/libcurl/c/CURLOPT_WRITEDATA.html */
  curl_easy_setopt(curl_, CURLOPT_WRITEDATA, response);

  CURLcode res = curl_easy_perform(curl_);
  if (res != CURLE_OK) {
    const std::string generic_error(curl_easy_strerror(res));
    throw std::runtime_error(generic_error + std::string(": ") + curl_error_);
  }

  long status_code;

  res = curl_easy_getinfo(curl_, CURLINFO_RESPONSE_CODE, &status_code);
  if (res != CURLE_OK || perform_injected_failure) {
    throw std::runtime_error(
        std::string("Can't get the HTTP status code from CURL: ") +
        curl_easy_strerror(res));
  }

  if (!status_is_success(status_code)) {
    std::streambuf* b = response->rdbuf();
    throw std::runtime_error(
        "HTTP request failed with status code " + std::to_string(status_code) +
        ". Response body:\n" +
        /* Read the whole body back from the stream where we wrote it and
         * append it to this error message string. Usually the bodies of
         * HTTP error responses represent a short HTML or JSON that
         * describes the error. */
        static_cast<const std::stringstream&>(std::stringstream() << b).str());
  }
}

void TransportCurl::HandleDestroy() {
  if (!curl_is_setup_) {
    return;
  }

  curl_is_setup_ = false;

  curl_easy_cleanup(curl_);
}

void TransportCurl::Test() {
  curl_global_injected_failure = true;
  test::must_fail("TransportCurl::TransportCurl()",
                  []() { TransportCurl transport_curl; });
  curl_global_injected_failure = false;

  test::must_fail("TransportCurl::UrlEncode()", []() {
    TransportCurl c;
    std::string encoded;
    c.url_encode_injected_failure = true;
    c.UrlEncode("nobody can encode me", &encoded);
  });

  test::must_fail("TransportCurl::HandleSetup()", []() {
    TransportCurl c;
    c.handle_setup_injected_failure = true;
    c.Fetch("http://localhost:1234", {}, nullptr);
  });

#ifndef NDEBUG
  test::must_fail("TransportCurl::Perform()", []() {
    TransportCurl c;
    replace_body = "";
    c.perform_injected_failure = true;
    std::stringstream response;
    c.Fetch("http://google.com", {}, &response);
  });
#endif /* NDEBUG */

  TransportCurl c;
  c.HandleSetup();
  c.HandleSetup();
}

} /* namespace http */
} /* namespace ipfs */
<commit_msg>Don't print curl_error_ if curl_easy_perform() did not modify it<commit_after>/* Copyright (c) 2016-2016, Vasil Dimov

Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
the Software, and to permit persons to whom the Software is furnished to do so,
subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */

#include <curl/curl.h>
#include <iostream>
#include <memory>
#include <sstream>
#include <stdexcept>
#include <string>
#include <vector>

#include <ipfs/http/transport-curl.h>
#include <ipfs/test/utils.h>

namespace ipfs {

namespace http {

/** CURL global initialization. `curl_global_init()` needs to be called before
 * any other threads have started executing because it is not thread safe and
 * may call other non-thread safe functions. Thus we use a global variable and
 * call `curl_global_init()` in its constructor. */
class CurlGlobal {
 public:
  /** Constructor that calls curl_global_init(). */
  CurlGlobal();

  /** Destructor that calls curl_global_cleanup(). */
  ~CurlGlobal();

  /** The result from curl_global_init() for checking the outcome later. */
  CURLcode result_;
};

CurlGlobal::CurlGlobal() { result_ = curl_global_init(CURL_GLOBAL_ALL); }
CurlGlobal::~CurlGlobal() { curl_global_cleanup(); }

/** Call curl_global_init() before any other threads have started executing.
 * This does not work with DLLs on Windows,
 * @see https://github.com/curl/curl/issues/586. */
static const CurlGlobal curl_global;

/** Flag to simulate a failure of curl_global_init(). */
static bool curl_global_injected_failure = false;

#ifndef NDEBUG
/** Placeholder to inject a fake http response during testing. */
std::string replace_body;
#endif /* NDEBUG */

/** Check if a HTTP status code is 2xx Success.
 * @return true if 2xx */
inline bool status_is_success(long code) { return code >= 200 && code <= 299; }

/** CURL callback for writing the result to a stream. */
static size_t curl_cb_stream(
    /** [in] Pointer to the result. */
    char* ptr,
    /** [in] Size each chunk of the result. */
    size_t size,
    /** [in] Number of chunks in the result. */
    size_t nmemb,
    /** [out] Response (a pointer to `std::iostream`). */
    void* response_void) {
  std::iostream* response = static_cast<std::iostream*>(response_void);

  const size_t n = size * nmemb;

  response->write(ptr, n);

  return n;
}

TransportCurl::TransportCurl() : curl_is_setup_(false) {
  if (curl_global.result_ != CURLE_OK || curl_global_injected_failure) {
    throw std::runtime_error("curl_global_init() failed");
  }
  static_assert(sizeof(curl_error_) >= CURL_ERROR_SIZE,
                "The size of curl_error_ is too small");
}

TransportCurl::~TransportCurl() { HandleDestroy(); }

void TransportCurl::Fetch(const std::string& url,
                          const std::vector<FileUpload>& files,
                          std::iostream* response) {
  HandleSetup();

  /* https://curl.haxx.se/libcurl/c/CURLOPT_POST.html */
  curl_easy_setopt(curl_, CURLOPT_POST, 1);

  curl_httppost* form_parts = NULL;
  curl_httppost* form_parts_end = NULL;

  for (size_t i = 0; i < files.size(); ++i) {
    const FileUpload& file = files[i];
    const std::string name("file" + std::to_string(i));
    static const char* content_type = "application/octet-stream";

    switch (file.type) {
      case FileUpload::Type::kFileContents:
        /* https://curl.haxx.se/libcurl/c/curl_formadd.html */
        curl_formadd(&form_parts, &form_parts_end,
                     /* name="..."; */
                     CURLFORM_COPYNAME, name.c_str(),
                     /* filename="..."; */
                     CURLFORM_BUFFER, file.path.c_str(),
                     /* File contents. */
                     CURLFORM_BUFFERPTR, file.data.c_str(),
                     /* File contents length. */
                     CURLFORM_BUFFERLENGTH, file.data.length(),
                     /* Content-Type: */
                     CURLFORM_CONTENTTYPE, content_type, CURLFORM_END);
        break;
      case FileUpload::Type::kFileName:
        /* https://curl.haxx.se/libcurl/c/curl_formadd.html */
        curl_formadd(&form_parts, &form_parts_end,
                     /* name="..."; */
                     CURLFORM_COPYNAME, name.c_str(),
                     /* filename="..."; */
                     CURLFORM_FILENAME, file.path.c_str(),
                     /* Read the data from this file. */
                     CURLFORM_FILE, file.data.c_str(),
                     /* Content-Type: */
                     CURLFORM_CONTENTTYPE, content_type, CURLFORM_END);
        break;
    }
  }

  /* Auto free the resources occupied by `form_parts`. */
  std::unique_ptr<curl_httppost, void (*)(curl_httppost*)> form_parts_deleter(
      form_parts, [](curl_httppost* d) {
        /* https://curl.haxx.se/libcurl/c/curl_formfree.html */
        curl_formfree(d);
      });

  /* https://curl.haxx.se/libcurl/c/CURLOPT_HTTPPOST.html */
  curl_easy_setopt(curl_, CURLOPT_HTTPPOST, form_parts);

  curl_slist* headers = NULL;
  /* https://curl.haxx.se/libcurl/c/curl_slist_append.html */
  headers = curl_slist_append(headers, "Expect:");

  /* Auto free the resources occupied by `headers`. */
  std::unique_ptr<curl_slist, void (*)(curl_slist*)> headers_deleter(
      headers, [](curl_slist* d) {
        /* https://curl.haxx.se/libcurl/c/curl_slist_free_all.html */
        curl_slist_free_all(d);
      });

  /* https://curl.haxx.se/libcurl/c/CURLOPT_HTTPHEADER.html */
  curl_easy_setopt(curl_, CURLOPT_HTTPHEADER, headers);

#ifndef NDEBUG
  if (!replace_body.empty()) {
    *response << replace_body;
    return;
  }
#endif /* NDEBUG */

  Perform(url, response);
}

void TransportCurl::UrlEncode(const std::string& raw, std::string* encoded) {
  HandleSetup();

  char* encoded_c = curl_easy_escape(curl_, raw.c_str(), 0);
  if (encoded_c == NULL || url_encode_injected_failure) {
    throw std::runtime_error("curl_easy_escape() failed on \"" + raw + "\"");
  }
  std::unique_ptr<char, void (*)(char*)> encoded_c_deleter(
      encoded_c, [](char* e) { curl_free(e); });

  encoded->assign(encoded_c);
}

void TransportCurl::HandleSetup() {
  if (curl_is_setup_) {
    return;
  }

  curl_ = curl_easy_init();
  if (curl_ == NULL || handle_setup_injected_failure) {
    throw std::runtime_error("curl_easy_init() failed");
  }

  /* https://curl.haxx.se/libcurl/c/CURLOPT_ERRORBUFFER.html */
  curl_easy_setopt(curl_, CURLOPT_ERRORBUFFER, curl_error_);

  /* Enable TCP keepalive.
   * https://curl.haxx.se/libcurl/c/CURLOPT_TCP_KEEPALIVE.html */
  curl_easy_setopt(curl_, CURLOPT_TCP_KEEPALIVE, 1);

  /* Seconds to wait before sending keep-alive packets.
   * https://curl.haxx.se/libcurl/c/CURLOPT_TCP_KEEPIDLE.html */
  curl_easy_setopt(curl_, CURLOPT_TCP_KEEPIDLE, 30);

  /* Seconds between keep-alive probes.
   * https://curl.haxx.se/libcurl/c/CURLOPT_TCP_KEEPINTVL.html */
  curl_easy_setopt(curl_, CURLOPT_TCP_KEEPINTVL, 10);

  /* https://curl.haxx.se/libcurl/c/CURLOPT_USERAGENT.html */
  curl_easy_setopt(curl_, CURLOPT_USERAGENT, "cpp-ipfs-api");

  curl_is_setup_ = true;
}

void TransportCurl::Perform(const std::string& url, std::iostream* response) {
  /* https://curl.haxx.se/libcurl/c/CURLOPT_URL.html */
  curl_easy_setopt(curl_, CURLOPT_URL, url.c_str());

  /* https://curl.haxx.se/libcurl/c/CURLOPT_WRITEFUNCTION.html */
  curl_easy_setopt(curl_, CURLOPT_WRITEFUNCTION, curl_cb_stream);

  /* https://curl.haxx.se/libcurl/c/CURLOPT_WRITEDATA.html */
  curl_easy_setopt(curl_, CURLOPT_WRITEDATA, response);

  curl_error_[0] = '\0';

  CURLcode res = curl_easy_perform(curl_);
  if (res != CURLE_OK) {
    const std::string generic_error(curl_easy_strerror(res));
    throw std::runtime_error(
        generic_error +
        (curl_error_[0] != '\0' ? std::string(": ") + curl_error_ : ""));
  }

  long status_code;

  res = curl_easy_getinfo(curl_, CURLINFO_RESPONSE_CODE, &status_code);
  if (res != CURLE_OK || perform_injected_failure) {
    throw std::runtime_error(
        std::string("Can't get the HTTP status code from CURL: ") +
        curl_easy_strerror(res));
  }

  if (!status_is_success(status_code)) {
    std::streambuf* b = response->rdbuf();
    throw std::runtime_error(
        "HTTP request failed with status code " + std::to_string(status_code) +
        ". Response body:\n" +
        /* Read the whole body back from the stream where we wrote it and
         * append it to this error message string. Usually the bodies of
         * HTTP error responses represent a short HTML or JSON that
         * describes the error. */
        static_cast<const std::stringstream&>(std::stringstream() << b).str());
  }
}

void TransportCurl::HandleDestroy() {
  if (!curl_is_setup_) {
    return;
  }

  curl_is_setup_ = false;

  curl_easy_cleanup(curl_);
}

void TransportCurl::Test() {
  curl_global_injected_failure = true;
  test::must_fail("TransportCurl::TransportCurl()",
                  []() { TransportCurl transport_curl; });
  curl_global_injected_failure = false;

  test::must_fail("TransportCurl::UrlEncode()", []() {
    TransportCurl c;
    std::string encoded;
    c.url_encode_injected_failure = true;
    c.UrlEncode("nobody can encode me", &encoded);
  });

  test::must_fail("TransportCurl::HandleSetup()", []() {
    TransportCurl c;
    c.handle_setup_injected_failure = true;
    c.Fetch("http://localhost:1234", {}, nullptr);
  });

#ifndef NDEBUG
  test::must_fail("TransportCurl::Perform()", []() {
    TransportCurl c;
    replace_body = "";
    c.perform_injected_failure = true;
    std::stringstream response;
    c.Fetch("http://google.com", {}, &response);
  });
#endif /* NDEBUG */

  TransportCurl c;
  c.HandleSetup();
  c.HandleSetup();
}

} /* namespace http */
} /* namespace ipfs */
<|endoftext|>


<commit_before>#include "drake/systems/plants/inverseKinBackend.h"

#include <stdexcept>
#include <vector>

#include <Eigen/Core>
#include <Eigen/Sparse>

#include "drake/core/Function.h"
#include "drake/core/Gradient.h"
#include "drake/solvers/Optimization.h"
#include "drake/systems/plants/constraint/RigidBodyConstraint.h"
#include "drake/systems/plants/ConstraintWrappers.h"
#include "drake/systems/plants/IKoptions.h"
#include "drake/systems/plants/RigidBodyTree.h"

using Eigen::Map;
using Eigen::MatrixBase;
using Eigen::MatrixXd;
using Eigen::VectorXd;
using Eigen::VectorXi;

using drake::solvers::SolutionResult;

namespace Drake {
namespace systems {
namespace plants {
namespace {

class Mode1Objective : public Constraint {
 public:
  // All references are aliased for the life of the objective.  It
  // should be short.
  Mode1Objective(RigidBodyTree* model, const MatrixXd& Q)
      : Constraint(model->num_positions),
        model_(model),
        Q_(Q) {}

  virtual void eval(const Eigen::Ref<const Eigen::VectorXd>& x,
                    Eigen::VectorXd& y) const override {
    VectorXd q_err = x - q_nom_i_;
    y(0) = q_err.transpose() * Q_ * q_err;
  }

  virtual void eval(const Eigen::Ref<const TaylorVecXd>& x,
                    TaylorVecXd& y) const override {
    VectorXd x_val = autoDiffToValueMatrix(x);
    VectorXd q_err = x_val - q_nom_i_;
    VectorXd y_val = q_err.transpose() * Q_ * q_err;
    MatrixXd dy_vec = 2 * q_err.transpose() * Q_;
    auto gradient_mat = autoDiffToGradientMatrix(x);
    initializeAutoDiffGivenGradientMatrix(
        y_val, (dy_vec * gradient_mat).eval(), y);
  }

  void set_q_nom(const VectorXd& q_nom_i) {
    q_nom_i_ = q_nom_i;
  }

 private:
  RigidBodyTree* model_;
  const MatrixXd& Q_;
  VectorXd q_nom_i_;
};

int GetSolverInfo(const OptimizationProblem& prog, SolutionResult result) {
  std::string solver_name;
  int solver_result = 0;
  prog.GetSolverResult(&solver_name, &solver_result);

  if (solver_name == "SNOPT") {
    // We can return SNOPT results directly.
    return solver_result;
  }

  // Make a SNOPT-like return code out of the generic result.
  switch (result) {
    case SolutionResult::kSolutionFound: {
      return 1;
    }
    case SolutionResult::kInvalidInput: {
      return 91;
    }
    case SolutionResult::kInfeasibleConstraints: {
      return 13;
    }
    case SolutionResult::kUnknownError: {
      return 100; // Not a real SNOPT error.
    }
  }

  return -1;
}

template <typename DerivedA, typename DerivedB, typename DerivedC,
          typename DerivedD, typename DerivedE>
void inverseKinMode1(
    RigidBodyTree* model, const int mode, const int nT,
    const double* t, const MatrixBase<DerivedA>& q_seed,
    const MatrixBase<DerivedB>& q_nom, const int num_constraints,
    RigidBodyConstraint** const constraint_array,
    const IKoptions& ikoptions, MatrixBase<DerivedC>* q_sol,
    MatrixBase<DerivedD>* qdot_sol, MatrixBase<DerivedE>* qddot_sol, int* INFO,
    std::vector<std::string>* infeasible_constraint) {

  OptimizationProblem prog;
  prog.SetSolverOption("SNOPT", "Major optimality tolerance",
                       ikoptions.getMajorOptimalityTolerance());
  prog.SetSolverOption("SNOPT", "Major feasibility tolerance",
                       ikoptions.getMajorFeasibilityTolerance());
  prog.SetSolverOption("SNOPT", "Superbasics limit",
                       ikoptions.getSuperbasicsLimit());
  prog.SetSolverOption("SNOPT", "Major iterations limit",
                       ikoptions.getMajorIterationsLimit());
  prog.SetSolverOption("SNOPT", "Iterations limit",
                       ikoptions.getIterationsLimit());

  DecisionVariableView vars = prog.AddContinuousVariables(model->num_positions);

  MatrixXd Q;
  ikoptions.getQ(Q);
  auto objective = std::make_shared<Mode1Objective>(model, Q);
  prog.AddCost(objective, {vars});

  KinematicsCacheHelper<double> kin_helper(model->bodies);

  bool qsc_active = false;
  for (int i = 0; i < num_constraints; i++) {
    RigidBodyConstraint* constraint = constraint_array[i];
    const int constraint_category = constraint->getCategory();
    if (constraint_category ==
        RigidBodyConstraint::SingleTimeKinematicConstraintCategory) {
      auto wrapper = std::make_shared<SingleTimeKinematicConstraintWrapper>(
          static_cast<SingleTimeKinematicConstraint*>(constraint), &kin_helper);
      // TODO(sam.creasey) I believe that this should have the bounds
      // updated with changes in time steps.
      prog.AddGenericConstraint(wrapper, {vars});
    } else if (constraint_category ==
               RigidBodyConstraint::PostureConstraintCategory) {
      PostureConstraint* pc = static_cast<PostureConstraint*>(constraint);
      VectorXd lb;
      VectorXd ub;
      // TODO(sam.creasey) This needs to support updating the
      // posture constraints across time steps.  Possibly store an
      // array of PostureConstraint + BoundingBoxConstraint pairs and
      // add an update method to BoundingBoxConstraint.
      pc->bounds(&t[0], lb, ub);
      prog.AddBoundingBoxConstraint(lb, ub, {vars});
    } else if (constraint_category ==
               RigidBodyConstraint::SingleTimeLinearPostureConstraintCategory) {
      SingleTimeLinearPostureConstraint* st_lpc =
          static_cast<SingleTimeLinearPostureConstraint*>(constraint);
      // TODO(sam.creasey) This should have the constraints updated
      // across time steps.
      VectorXd lb;
      VectorXd ub;
      st_lpc->bounds(&t[0], lb, ub);

      VectorXi iAfun;
      VectorXi jAvar;
      VectorXd A;
      st_lpc->geval(&t[0], iAfun, jAvar, A);

      assert(iAfun.size() == jAvar.size());
      assert(iAfun.size() != A.size());

      typedef Eigen::Triplet<double> T;
      std::vector<T> triplet_list;
      for (size_t i = 0; i < iAfun.size(); i++) {
        triplet_list.push_back(T(iAfun[i], jAvar[i], A[i]));
      }

      Eigen::SparseMatrix<double> A_sparse(st_lpc->getNumConstraint(&t[0]),
                                           model->num_positions);
      A_sparse.setFromTriplets(triplet_list.begin(), triplet_list.end());
      prog.AddLinearConstraint(MatrixXd(A_sparse), lb, ub, {vars});
    } else if (constraint_category ==
               RigidBodyConstraint::QuasiStaticConstraintCategory) {
      if (qsc_active) {
        throw std::runtime_error(
            "Drake::inverseKinBackend: current implementation supports at "
            "most one QuasiStaticConstraint");
      }

      QuasiStaticConstraint* qsc =
          static_cast<QuasiStaticConstraint*>(constraint);
      int num_vars = qsc->getNumWeights();
      DecisionVariableView qsc_vars =
          prog.AddContinuousVariables(num_vars, "qsc");
      auto wrapper = std::make_shared<QuasiStaticConstraintWrapper>(
          qsc, &kin_helper);
      prog.AddGenericConstraint(wrapper, {vars, qsc_vars});
      prog.AddBoundingBoxConstraint(VectorXd::Constant(num_vars, 0.),
                                    VectorXd::Constant(num_vars, 1.), {qsc_vars});
      VectorXd constraint(num_vars);
      constraint.fill(1.);
      prog.AddLinearEqualityConstraint(constraint.transpose(),
                                       Vector1d::Constant(1.), {qsc_vars});
      prog.SetInitialGuess(qsc_vars,
                           VectorXd::Constant(num_vars, 1.0 / num_vars));
    } else if (constraint_category ==
               RigidBodyConstraint::MultipleTimeKinematicConstraintCategory) {
      throw std::runtime_error("MultipleTimeKinematicConstraint is not supported"
                               " in pointwise mode.");
    } else if (constraint_category ==
               RigidBodyConstraint::MultipleTimeLinearPostureConstraintCategory) {
      throw std::runtime_error(
          "MultipleTimeLinearPostureConstraint is not supported"
          " in pointwise mode.");
    }
  }

  // Add a bounding box constraint from them model.
  prog.AddBoundingBoxConstraint(model->joint_limit_min, model->joint_limit_max,
                                {vars});

  for (int t = 0; t < nT; t++) {
    // TODO(sam.creasey) would this be faster if we stored the view
    // instead of copying into a VectorXd?
    objective->set_q_nom(q_nom.col(t));
    if (!ikoptions.getSequentialSeedFlag() || (t == 0)) {
      prog.SetInitialGuess(vars, q_seed.col(t));
    } else {
      prog.SetInitialGuess(vars, q_sol->col(t - 1));
    }

    SolutionResult result = prog.Solve();
    prog.PrintSolution();
    q_sol->col(t) = vars.value();
    INFO[t] = GetSolverInfo(prog, result);
  }
}

} // namespace (anon)

template <typename DerivedA, typename DerivedB, typename DerivedC,
          typename DerivedD, typename DerivedE>
void inverseKinBackend(
    RigidBodyTree* model, const int mode, const int nT,
    const double* t, const MatrixBase<DerivedA>& q_seed,
    const MatrixBase<DerivedB>& q_nom, const int num_constraints,
    RigidBodyConstraint** const constraint_array,
    const IKoptions& ikoptions, MatrixBase<DerivedC>* q_sol,
    MatrixBase<DerivedD>* qdot_sol, MatrixBase<DerivedE>* qddot_sol,
    int* INFO, std::vector<std::string>* infeasible_constraint) {

  // Validate some basic parameters of the input.
  if (q_seed.rows() != model->num_positions || q_seed.cols() != nT ||
      q_nom.rows() != model->num_positions || q_nom.cols() != nT) {
    throw std::runtime_error(
        "Drake::inverseKinBackend: q_seed and q_nom must be of size "
        "nq x nT");
  }

  // Check to see if the generic version supports the constraint types
  // we're using.
  bool constraints_supported = true;
  for (int i = 0; i < num_constraints; i++) {
    if ((constraint_array[i]->getCategory() !=
         RigidBodyConstraint::SingleTimeKinematicConstraintCategory) &&
        (constraint_array[i]->getCategory() !=
         RigidBodyConstraint::PostureConstraintCategory) &&
        (constraint_array[i]->getCategory() !=
         RigidBodyConstraint::SingleTimeLinearPostureConstraintCategory) &&
        (constraint_array[i]->getCategory() !=
         RigidBodyConstraint::QuasiStaticConstraintCategory)) {
      constraints_supported = false;
      break;
    }
  }

  // Fall through to SNOPT implementation if needed.
  if (!constraints_supported || (mode != 1) || (nT != 1)) {
    inverseKinSnoptBackend(model, mode, nT, t, q_seed,
                           q_nom, num_constraints, constraint_array,
                           ikoptions, q_sol, qdot_sol, qddot_sol, INFO,
                           infeasible_constraint);
    return;
  }

  // TODO(sam.creasey) This will need to be conditional when more than
  // one mode is supported...
  inverseKinMode1(model, mode, nT, t, q_seed,
                  q_nom, num_constraints, constraint_array,
                  ikoptions, q_sol, qdot_sol, qddot_sol, INFO,
                  infeasible_constraint);
  return;
}

template void inverseKinBackend(
    RigidBodyTree* model, const int mode, const int nT,
    const double* t, const MatrixBase<Map<MatrixXd>>& q_seed,
    const MatrixBase<Map<MatrixXd>>& q_nom, const int num_constraints,
    RigidBodyConstraint** const constraint_array,
    const IKoptions& ikoptions, MatrixBase<Map<MatrixXd>>* q_sol,
    MatrixBase<Map<MatrixXd>>* qdot_sol, MatrixBase<Map<MatrixXd>>* qddot_sol,
    int* INFO, std::vector<std::string>* infeasible_constraint);
template void inverseKinBackend(
    RigidBodyTree* model, const int mode, const int nT,
    const double* t, const MatrixBase<MatrixXd>& q_seed,
    const MatrixBase<MatrixXd>& q_nom, const int num_constraints,
    RigidBodyConstraint** const constraint_array,
    const IKoptions& ikoptions, MatrixBase<MatrixXd>* q_sol,
    MatrixBase<MatrixXd>* qdot_sol, MatrixBase<MatrixXd>* qddot_sol,
    int* INFO, std::vector<std::string>* infeasible_constraint);
template void inverseKinBackend(
    RigidBodyTree* model, const int mode, const int nT,
    const double* t, const MatrixBase<Map<MatrixXd>>& q_seed,
    const MatrixBase<Map<MatrixXd>>& q_nom, const int num_constraints,
    RigidBodyConstraint** const constraint_array,
    const IKoptions& ikoptions, MatrixBase<Map<MatrixXd>>* q_sol,
    MatrixBase<MatrixXd>* qdot_sol, MatrixBase<MatrixXd>* qddot_sol,
    int* INFO, std::vector<std::string>* infeasible_constraint);
template void inverseKinBackend(
    RigidBodyTree* model, const int mode, const int nT,
    const double* t, const MatrixBase<Map<VectorXd>>& q_seed,
    const MatrixBase<Map<VectorXd>>& q_nom, const int num_constraints,
    RigidBodyConstraint** const constraint_array,
    const IKoptions& ikoptions, MatrixBase<Map<VectorXd>>* q_sol,
    MatrixBase<Map<VectorXd>>* qdot_sol, MatrixBase<Map<VectorXd>>* qddot_sol,
    int* INFO, std::vector<std::string>* infeasible_constraint);
template void inverseKinBackend(
    RigidBodyTree* model, const int mode, const int nT,
    const double* t, const MatrixBase<VectorXd>& q_seed,
    const MatrixBase<VectorXd>& q_nom, const int num_constraints,
    RigidBodyConstraint** const constraint_array,
    const IKoptions& ikoptions, MatrixBase<VectorXd>* q_sol,
    MatrixBase<VectorXd>* qdot_sol, MatrixBase<VectorXd>* qddot_sol,
    int* INFO, std::vector<std::string>* infeasible_constraint);
template void inverseKinBackend(
    RigidBodyTree* model, const int mode, const int nT,
    const double* t, const MatrixBase<Map<VectorXd>>& q_seed,
    const MatrixBase<Map<VectorXd>>& q_nom, const int num_constraints,
    RigidBodyConstraint** const constraint_array,
    const IKoptions& ikoptions, MatrixBase<Map<VectorXd>>* q_sol,
    MatrixBase<VectorXd>* qdot_sol, MatrixBase<VectorXd>* qddot_sol,
    int* INFO, std::vector<std::string>* infeasible_constraint);
}
}
}
<commit_msg>Implement pointwise solution (suboptimally, at least in terms of runtime)<commit_after>#include "drake/systems/plants/inverseKinBackend.h"

#include <stdexcept>
#include <vector>

#include <Eigen/Core>
#include <Eigen/Sparse>

#include "drake/core/Function.h"
#include "drake/core/Gradient.h"
#include "drake/solvers/Optimization.h"
#include "drake/systems/plants/constraint/RigidBodyConstraint.h"
#include "drake/systems/plants/ConstraintWrappers.h"
#include "drake/systems/plants/IKoptions.h"
#include "drake/systems/plants/RigidBodyTree.h"

using Eigen::Map;
using Eigen::MatrixBase;
using Eigen::MatrixXd;
using Eigen::VectorXd;
using Eigen::VectorXi;

using drake::solvers::SolutionResult;

namespace Drake {
namespace systems {
namespace plants {
namespace {

class Mode1Objective : public Constraint {
 public:
  // All references are aliased for the life of the objective.  It
  // should be short.
  Mode1Objective(RigidBodyTree* model, const MatrixXd& Q)
      : Constraint(model->num_positions),
        model_(model),
        Q_(Q) {}

  virtual void eval(const Eigen::Ref<const Eigen::VectorXd>& x,
                    Eigen::VectorXd& y) const override {
    VectorXd q_err = x - q_nom_i_;
    y(0) = q_err.transpose() * Q_ * q_err;
  }

  virtual void eval(const Eigen::Ref<const TaylorVecXd>& x,
                    TaylorVecXd& y) const override {
    VectorXd x_val = autoDiffToValueMatrix(x);
    VectorXd q_err = x_val - q_nom_i_;
    VectorXd y_val = q_err.transpose() * Q_ * q_err;
    MatrixXd dy_vec = 2 * q_err.transpose() * Q_;
    auto gradient_mat = autoDiffToGradientMatrix(x);
    initializeAutoDiffGivenGradientMatrix(
        y_val, (dy_vec * gradient_mat).eval(), y);
  }

  void set_q_nom(const VectorXd& q_nom_i) {
    q_nom_i_ = q_nom_i;
  }

 private:
  RigidBodyTree* model_;
  const MatrixXd& Q_;
  VectorXd q_nom_i_;
};

int GetSolverInfo(const OptimizationProblem& prog, SolutionResult result) {
  std::string solver_name;
  int solver_result = 0;
  prog.GetSolverResult(&solver_name, &solver_result);

  if (solver_name == "SNOPT") {
    // We can return SNOPT results directly.
    return solver_result;
  }

  // Make a SNOPT-like return code out of the generic result.
  switch (result) {
    case SolutionResult::kSolutionFound: {
      return 1;
    }
    case SolutionResult::kInvalidInput: {
      return 91;
    }
    case SolutionResult::kInfeasibleConstraints: {
      return 13;
    }
    case SolutionResult::kUnknownError: {
      return 100; // Not a real SNOPT error.
    }
  }

  return -1;
}

template <typename DerivedA, typename DerivedB, typename DerivedC,
          typename DerivedD, typename DerivedE>
void inverseKinMode1(
    RigidBodyTree* model, const int mode, const int nT,
    const double* t, const MatrixBase<DerivedA>& q_seed,
    const MatrixBase<DerivedB>& q_nom, const int num_constraints,
    RigidBodyConstraint** const constraint_array,
    const IKoptions& ikoptions, MatrixBase<DerivedC>* q_sol,
    MatrixBase<DerivedD>* qdot_sol, MatrixBase<DerivedE>* qddot_sol, int* INFO,
    std::vector<std::string>* infeasible_constraint) {

  KinematicsCacheHelper<double> kin_helper(model->bodies);

  // TODO(sam.creasey) I really don't like rebuilding the
  // OptimizationProblem for every timestep, but it's not possible to
  // enable/disable (or even remove) a constraint from an
  // OptimizationProblem between calls to Solve() currently, so
  // there's not actually another way.
  for (int t_index = 0; t_index < nT; t_index++) {
    OptimizationProblem prog;
    prog.SetSolverOption("SNOPT", "Major optimality tolerance",
                         ikoptions.getMajorOptimalityTolerance());
    prog.SetSolverOption("SNOPT", "Major feasibility tolerance",
                         ikoptions.getMajorFeasibilityTolerance());
    prog.SetSolverOption("SNOPT", "Superbasics limit",
                         ikoptions.getSuperbasicsLimit());
    prog.SetSolverOption("SNOPT", "Major iterations limit",
                         ikoptions.getMajorIterationsLimit());
    prog.SetSolverOption("SNOPT", "Iterations limit",
                         ikoptions.getIterationsLimit());

    DecisionVariableView vars = prog.AddContinuousVariables(model->num_positions);

    MatrixXd Q;
    ikoptions.getQ(Q);
    auto objective = std::make_shared<Mode1Objective>(model, Q);
    prog.AddCost(objective, {vars});

    bool qsc_active = false;
    for (int i = 0; i < num_constraints; i++) {
      RigidBodyConstraint* constraint = constraint_array[i];
      const int constraint_category = constraint->getCategory();
      if (constraint_category ==
          RigidBodyConstraint::SingleTimeKinematicConstraintCategory) {
        SingleTimeKinematicConstraint* stc =
            static_cast<SingleTimeKinematicConstraint*>(constraint);
        if (!stc->isTimeValid(&t[t_index])) { continue; }
        auto wrapper = std::make_shared<SingleTimeKinematicConstraintWrapper>(
            stc, &kin_helper);
        prog.AddGenericConstraint(wrapper, {vars});
      } else if (constraint_category ==
                 RigidBodyConstraint::PostureConstraintCategory) {
        PostureConstraint* pc = static_cast<PostureConstraint*>(constraint);
        if (!pc->isTimeValid(&t[t_index])) { continue; }
        VectorXd lb;
        VectorXd ub;
        pc->bounds(&t[t_index], lb, ub);
        prog.AddBoundingBoxConstraint(lb, ub, {vars});
      } else if (constraint_category ==
                 RigidBodyConstraint::SingleTimeLinearPostureConstraintCategory) {
        SingleTimeLinearPostureConstraint* st_lpc =
            static_cast<SingleTimeLinearPostureConstraint*>(constraint);
        if (!st_lpc->isTimeValid(&t[t_index])) { continue; }
        VectorXd lb;
        VectorXd ub;
        st_lpc->bounds(&t[t_index], lb, ub);

        VectorXi iAfun;
        VectorXi jAvar;
        VectorXd A;
        st_lpc->geval(&t[t_index], iAfun, jAvar, A);

        assert(iAfun.size() == jAvar.size());
        assert(iAfun.size() != A.size());

        typedef Eigen::Triplet<double> T;
        std::vector<T> triplet_list;
        for (size_t i = 0; i < iAfun.size(); i++) {
          triplet_list.push_back(T(iAfun[i], jAvar[i], A[i]));
        }

        Eigen::SparseMatrix<double> A_sparse(st_lpc->getNumConstraint(&t[t_index]),
                                             model->num_positions);
        A_sparse.setFromTriplets(triplet_list.begin(), triplet_list.end());
        prog.AddLinearConstraint(MatrixXd(A_sparse), lb, ub, {vars});
      } else if (constraint_category ==
                 RigidBodyConstraint::QuasiStaticConstraintCategory) {
        if (qsc_active) {
          throw std::runtime_error(
              "Drake::inverseKinBackend: current implementation supports at "
              "most one QuasiStaticConstraint");
        }

        QuasiStaticConstraint* qsc =
            static_cast<QuasiStaticConstraint*>(constraint);
        if (!qsc->isTimeValid(&t[t_index])) { continue; }
        int num_vars = qsc->getNumWeights();
        DecisionVariableView qsc_vars =
            prog.AddContinuousVariables(num_vars, "qsc");
        auto wrapper = std::make_shared<QuasiStaticConstraintWrapper>(
            qsc, &kin_helper);
        prog.AddGenericConstraint(wrapper, {vars, qsc_vars});
        prog.AddBoundingBoxConstraint(VectorXd::Constant(num_vars, 0.),
                                      VectorXd::Constant(num_vars, 1.), {qsc_vars});
        VectorXd constraint(num_vars);
        constraint.fill(1.);
        prog.AddLinearEqualityConstraint(constraint.transpose(),
                                         Vector1d::Constant(1.), {qsc_vars});
        prog.SetInitialGuess(qsc_vars,
                             VectorXd::Constant(num_vars, 1.0 / num_vars));
      } else if (constraint_category ==
                 RigidBodyConstraint::MultipleTimeKinematicConstraintCategory) {
        throw std::runtime_error("MultipleTimeKinematicConstraint is not supported"
                                 " in pointwise mode.");
      } else if (constraint_category ==
                 RigidBodyConstraint::MultipleTimeLinearPostureConstraintCategory) {
        throw std::runtime_error(
            "MultipleTimeLinearPostureConstraint is not supported"
            " in pointwise mode.");
      }
    }

    // Add a bounding box constraint from them model.
    prog.AddBoundingBoxConstraint(model->joint_limit_min, model->joint_limit_max,
                                  {vars});

    // TODO(sam.creasey) would this be faster if we stored the view
    // instead of copying into a VectorXd?
    objective->set_q_nom(q_nom.col(t_index));
    if (!ikoptions.getSequentialSeedFlag() || (t_index == 0)) {
      prog.SetInitialGuess(vars, q_seed.col(t_index));
    } else {
      prog.SetInitialGuess(vars, q_sol->col(t_index - 1));
    }

    SolutionResult result = prog.Solve();
    prog.PrintSolution();
    q_sol->col(t_index) = vars.value();
    INFO[t_index] = GetSolverInfo(prog, result);
  }
}

} // namespace (anon)

template <typename DerivedA, typename DerivedB, typename DerivedC,
          typename DerivedD, typename DerivedE>
void inverseKinBackend(
    RigidBodyTree* model, const int mode, const int nT,
    const double* t, const MatrixBase<DerivedA>& q_seed,
    const MatrixBase<DerivedB>& q_nom, const int num_constraints,
    RigidBodyConstraint** const constraint_array,
    const IKoptions& ikoptions, MatrixBase<DerivedC>* q_sol,
    MatrixBase<DerivedD>* qdot_sol, MatrixBase<DerivedE>* qddot_sol,
    int* INFO, std::vector<std::string>* infeasible_constraint) {

  // Validate some basic parameters of the input.
  if (q_seed.rows() != model->num_positions || q_seed.cols() != nT ||
      q_nom.rows() != model->num_positions || q_nom.cols() != nT) {
    throw std::runtime_error(
        "Drake::inverseKinBackend: q_seed and q_nom must be of size "
        "nq x nT");
  }

  // Check to see if the generic version supports the constraint types
  // we're using.
  bool constraints_supported = true;
  for (int i = 0; i < num_constraints; i++) {
    if ((constraint_array[i]->getCategory() !=
         RigidBodyConstraint::SingleTimeKinematicConstraintCategory) &&
        (constraint_array[i]->getCategory() !=
         RigidBodyConstraint::PostureConstraintCategory) &&
        (constraint_array[i]->getCategory() !=
         RigidBodyConstraint::SingleTimeLinearPostureConstraintCategory) &&
        (constraint_array[i]->getCategory() !=
         RigidBodyConstraint::QuasiStaticConstraintCategory)) {
      constraints_supported = false;
      break;
    }
  }

  // Fall through to SNOPT implementation if needed.
  if (!constraints_supported || (mode != 1)) {
    inverseKinSnoptBackend(model, mode, nT, t, q_seed,
                           q_nom, num_constraints, constraint_array,
                           ikoptions, q_sol, qdot_sol, qddot_sol, INFO,
                           infeasible_constraint);
    return;
  }

  // TODO(sam.creasey) This will need to be conditional when more than
  // one mode is supported...
  inverseKinMode1(model, mode, nT, t, q_seed,
                  q_nom, num_constraints, constraint_array,
                  ikoptions, q_sol, qdot_sol, qddot_sol, INFO,
                  infeasible_constraint);
  return;
}

template void inverseKinBackend(
    RigidBodyTree* model, const int mode, const int nT,
    const double* t, const MatrixBase<Map<MatrixXd>>& q_seed,
    const MatrixBase<Map<MatrixXd>>& q_nom, const int num_constraints,
    RigidBodyConstraint** const constraint_array,
    const IKoptions& ikoptions, MatrixBase<Map<MatrixXd>>* q_sol,
    MatrixBase<Map<MatrixXd>>* qdot_sol, MatrixBase<Map<MatrixXd>>* qddot_sol,
    int* INFO, std::vector<std::string>* infeasible_constraint);
template void inverseKinBackend(
    RigidBodyTree* model, const int mode, const int nT,
    const double* t, const MatrixBase<MatrixXd>& q_seed,
    const MatrixBase<MatrixXd>& q_nom, const int num_constraints,
    RigidBodyConstraint** const constraint_array,
    const IKoptions& ikoptions, MatrixBase<MatrixXd>* q_sol,
    MatrixBase<MatrixXd>* qdot_sol, MatrixBase<MatrixXd>* qddot_sol,
    int* INFO, std::vector<std::string>* infeasible_constraint);
template void inverseKinBackend(
    RigidBodyTree* model, const int mode, const int nT,
    const double* t, const MatrixBase<Map<MatrixXd>>& q_seed,
    const MatrixBase<Map<MatrixXd>>& q_nom, const int num_constraints,
    RigidBodyConstraint** const constraint_array,
    const IKoptions& ikoptions, MatrixBase<Map<MatrixXd>>* q_sol,
    MatrixBase<MatrixXd>* qdot_sol, MatrixBase<MatrixXd>* qddot_sol,
    int* INFO, std::vector<std::string>* infeasible_constraint);
template void inverseKinBackend(
    RigidBodyTree* model, const int mode, const int nT,
    const double* t, const MatrixBase<Map<VectorXd>>& q_seed,
    const MatrixBase<Map<VectorXd>>& q_nom, const int num_constraints,
    RigidBodyConstraint** const constraint_array,
    const IKoptions& ikoptions, MatrixBase<Map<VectorXd>>* q_sol,
    MatrixBase<Map<VectorXd>>* qdot_sol, MatrixBase<Map<VectorXd>>* qddot_sol,
    int* INFO, std::vector<std::string>* infeasible_constraint);
template void inverseKinBackend(
    RigidBodyTree* model, const int mode, const int nT,
    const double* t, const MatrixBase<VectorXd>& q_seed,
    const MatrixBase<VectorXd>& q_nom, const int num_constraints,
    RigidBodyConstraint** const constraint_array,
    const IKoptions& ikoptions, MatrixBase<VectorXd>* q_sol,
    MatrixBase<VectorXd>* qdot_sol, MatrixBase<VectorXd>* qddot_sol,
    int* INFO, std::vector<std::string>* infeasible_constraint);
template void inverseKinBackend(
    RigidBodyTree* model, const int mode, const int nT,
    const double* t, const MatrixBase<Map<VectorXd>>& q_seed,
    const MatrixBase<Map<VectorXd>>& q_nom, const int num_constraints,
    RigidBodyConstraint** const constraint_array,
    const IKoptions& ikoptions, MatrixBase<Map<VectorXd>>* q_sol,
    MatrixBase<VectorXd>* qdot_sol, MatrixBase<VectorXd>* qddot_sol,
    int* INFO, std::vector<std::string>* infeasible_constraint);
}
}
}
<|endoftext|>


<commit_before>/*
    Copyright (c) 2009, Robin RUAUX
    All rights reserved.
    Redistribution and use in source and binary forms, with or without
    modification, are permitted provided that the following conditions are met:

        * Redistributions of source code must retain the above copyright
          notice, this list of conditions and the following disclaimer.
        * Redistributions in binary form must reproduce the above copyright
          notice, this list of conditions and the following disclaimer in the
          documentation and/or other materials provided with the distribution.
        * Neither the name of the University of California, Berkeley nor the
          names of its contributors may be used to endorse or promote products
          derived from this software without specific prior written permission.

    THIS SOFTWARE IS PROVIDED BY THE REGENTS 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 REGENTS AND 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.

*/

#include "TextInput.hpp"
#include <SFML/Window/Event.hpp>
#include <SFML/Graphics/RenderTarget.hpp>
#include <SFML/Window/OpenGL.hpp>

namespace sf
{
    namespace ui
    {
        float    TextInput::mStringOffset = 4.f;

        TextInput::TextInput(const Unicode::Text& string)
            :   Widget(),
                mString(string),
                mMaxLength(0),
                mEditable(true),
                mCursorPosition(0),
                mCursorOffset(0),
                mSelectionDragged(false),
                mSelectionShifted(false),
                mSelectionStart(0)
        {
            SetDefaultStyle("BI_TextInput");
            LoadStyle(GetDefaultStyle());

            Add(&mString);
            AdjustRect();

            AddKeyListener(this);
            AddMouseListener(this);
        }

        void    TextInput::SetText(const Unicode::Text& text)
        {
            mString.SetText(text);
        }

        const Unicode::Text& TextInput::GetText() const
        {
            return mString.GetText();
        }

        bool    TextInput::IsEmpty() const
        {
            return (Unicode::UTF16String(mString.GetText()).size() == 0);
        }

        void    TextInput::LoadStyle(const std::string& nameStyle)
        {
            Widget::LoadStyle(nameStyle);

            ResourceManager* rm = ResourceManager::Get();
            StyleProperties& properties = rm->GetStyle(nameStyle);

            SetMaxLength(rm->GetValue(properties["maxLength"], GetMaxLength()));
            SetEditable(rm->GetValue(properties["editable"], IsEditable()));

            SetSelectionColor(rm->GetColorValue(properties["selectionColor"], GetSelectionColor()));

            SetTextColor(rm->GetColorValue(properties["textColor"], GetTextColor()));
            SetTextSize(rm->GetValue(properties["textSize"], GetTextSize()));

            Font* newFont = rm->GetFont(properties["font"], GetTextSize());
            if (newFont)
            {
                SetFont(*newFont);
            }

            mString.SetY((GetHeight() - mString.GetString().GetSize()) / 2 - 2);
        }

        void    TextInput::SetMaxLength(unsigned int maxLength)
        {
            mMaxLength = maxLength;
        }

        unsigned int    TextInput::GetMaxLength() const
        {
            return mMaxLength;
        }

        void            TextInput::SetEditable(bool editable)
        {
            mEditable = editable;
        }

        bool            TextInput::IsEditable() const
        {
            return mEditable;
        }

        void            TextInput::SetTextColor(const Color& color)
        {
            mString.SetTextColor(color);
        }

        const Color&    TextInput::GetTextColor() const
        {
            return mString.GetTextColor();
        }

        void            TextInput::SetTextSize(float size)
        {
            mString.SetTextSize(size);
        }

        float           TextInput::GetTextSize() const
        {
            return mString.GetTextSize();
        }

        void            TextInput::SetFont(const Font& font)
        {
            mString.SetFont(font);
        }

        const Font&     TextInput::GetFont() const
        {
            return mString.GetFont();
        }

        void            TextInput::ClearSelection()
        {
            mSelectionStart = mCursorPosition;
        }

        bool            TextInput::EraseSelection()
        {
            if (mSelectionStart == mCursorPosition)
                return false;

            Unicode::UTF16String currentText = GetText();
            unsigned int newPos = std::min(mSelectionStart, mCursorPosition);

            currentText.erase(newPos, GetSelectionSize());
            SetText(currentText);

            mSelectionStart = mCursorPosition = newPos;
            return true;
        }

        void            TextInput::SetSelection(unsigned int start, unsigned int cursorPosition)
        {
            const Unicode::UTF16String& text = GetText();

            mSelectionStart = start;
            mCursorPosition = (cursorPosition > text.length()) ? text.length() : cursorPosition;
        }

        void            TextInput::SetSelectionColor(const Color& color)
        {
            mSelectionColor = color;
        }

        const Color&    TextInput::GetSelectionColor() const
        {
            return mSelectionColor;
        }

        Unicode::Text   TextInput::GetSelection() const
        {
            const Unicode::UTF16String& text = GetText();
            return text.substr(std::min(mSelectionStart, mCursorPosition), GetSelectionSize());
        }

        unsigned int    TextInput::GetSelectionSize() const
        {
            if (mSelectionStart < mCursorPosition)
                return mCursorPosition - mSelectionStart;
            return mSelectionStart - mCursorPosition;
        }

        unsigned int    TextInput::GetCharacterAtPos(float xOffset)
        {
            const Unicode::UTF16String& text = GetText();

            for (unsigned int i = 0; i < text.length(); ++i)
            {
                mString.SetText(text.substr(0, i));
                const FloatRect& rect = mString.GetString().GetRect();
                mString.SetText(text);

                if (rect.GetWidth() > xOffset)
                    return i;
            }
            return text.length();
        }

        void    TextInput::OnMousePressed(const Event::MouseButtonEvent& button)
        {
            if (button.Button != Mouse::Left)
                return;

            mCursorPosition = GetCharacterAtPos(button.X - GetAbsolutePosition().x - mStringOffset + mCursorOffset);
            mSelectionDragged = true;
            ClearSelection();

            AdjustRect();
        }

        void    TextInput::OnMouseReleased(const Event::MouseButtonEvent& button)
        {
            if (button.Button != Mouse::Left)
                return;

            if (mSelectionDragged)
                mSelectionDragged = false;
        }

        void    TextInput::OnMouseMoved(const Event::MouseMoveEvent& mouse)
        {
            if (mSelectionDragged)
            {
                mCursorPosition = GetCharacterAtPos(mouse.X - GetAbsolutePosition().x - mStringOffset + mCursorOffset);
                AdjustRect();
            }
        }

        void    TextInput::OnKeyPressed(const Event::KeyEvent& key)
        {
            Unicode::UTF16String text = GetText();

            if (key.Code == Key::Left)
            {
                if (mCursorPosition)
                    --mCursorPosition;

                if (!mSelectionShifted)
                    ClearSelection();
            }
            else if (key.Code == Key::Right)
            {
                if (mCursorPosition < text.length())
                    ++mCursorPosition;

                if (!mSelectionShifted)
                    ClearSelection();
            }
            else if (key.Code == Key::Home)
            {
                if (mSelectionShifted)
                {
                    mSelectionStart = mCursorPosition;
                    mCursorPosition = 0;
                }
                else
                {
                    mCursorPosition = 0;
                    ClearSelection();
                }
            }
            else if (key.Code == Key::End)
            {
                if (mSelectionShifted)
                {
                    mSelectionStart = mCursorPosition;
                    mCursorPosition = text.length();
                }
                else
                {
                    mCursorPosition = text.length();
                    ClearSelection();
                }
            }
            else if (key.Code == Key::LShift || key.Code == Key::RShift)
            {
                mSelectionShifted = true;
            }
            else if ((key.Code == Key::Delete || key.Code == Key::Back) && mEditable)
            {
                if (!EraseSelection())
                {
                    if (key.Code == Key::Back && !text.empty() && mCursorPosition)
                    {
                        text.erase(--mCursorPosition, 1);
                    }
                    else if (key.Code == Key::Delete && mCursorPosition < text.length())
                    {
                        text.erase(mCursorPosition, 1);
                    }
                    ClearSelection();
                    SetText(text);
                }
            }
            AdjustRect();
        }

        void    TextInput::OnKeyReleased(const Event::KeyEvent& key)
        {
            if (key.Code == Key::LShift || key.Code == Key::RShift)
            {
                mSelectionShifted = false;
            }
        }

        void    TextInput::OnTextEntered(const Event::TextEvent& text)
        {
            // ASCII non printable caracters have to be ignored.
            if (text.Unicode <= 30 || (text.Unicode >= 127 && text.Unicode <= 159) || !mEditable)
                return;

            EraseSelection();

            Unicode::UTF16String currentText = GetText();

            if (currentText.length() < mMaxLength || !mMaxLength)
            {
                Unicode::UTF16String c;
                c += text.Unicode;
                currentText.insert(mCursorPosition, c);
                ++mCursorPosition;
            }

            SetText(currentText);
            ClearSelection();
            AdjustRect();
        }

        void    TextInput::AdjustRect()
        {
            const Unicode::UTF16String& text = GetText();

            mString.SetText(text.substr(0, mCursorPosition));
            const FloatRect& rect = mString.GetString().GetRect();
            mString.SetText(text);

            if (rect.GetWidth() - mCursorOffset >= GetWidth() - mStringOffset)
            {
                mCursorOffset = rect.GetWidth() - GetWidth() + mStringOffset;
            }
            else if (rect.GetWidth() - mCursorOffset <= 0)
            {
                mCursorOffset = rect.GetWidth() - GetWidth() / (mStringOffset / 2);
            }

            if (mCursorOffset < 0)
                mCursorOffset = 0;

            mString.SetX(mStringOffset - mCursorOffset);
        }

        void    TextInput::OnPaint(RenderTarget& target) const
        {
            Widget::OnPaint(target);

            // Draws the cursor with real position
            if (HasFocus())
            {
                const String& rStr = mString.GetString();
                float factor = rStr.GetSize() / rStr.GetFont().GetCharacterSize();
                float yPos = 2.f / factor;
                glScalef(factor, factor, 1.f);

                Vector2f realPos = rStr.GetCharacterPos(mCursorPosition);
                realPos.x = (realPos.x - mCursorOffset + mStringOffset) / factor;
                realPos.y = (GetHeight() - 2.f) / factor;

                glDisable(GL_TEXTURE_2D);

                glBegin(GL_LINES);
                    glVertex2f(realPos.x, yPos);
                    glVertex2f(realPos.x, realPos.y);
                glEnd();

                // Draws the current selection
                if (mSelectionStart != mCursorPosition)
                {
                    Vector2f selectionPos(rStr.GetCharacterPos(mSelectionStart).x, rStr.GetCharacterPos(mCursorPosition).x);
                    selectionPos.x = (selectionPos.x - mCursorOffset + mStringOffset) / factor;
                    selectionPos.y = (selectionPos.y - mCursorOffset + mStringOffset) / factor;

                    const Color& col = mSelectionColor;
                    glColor4ub(col.r , col.g , col.b, col.a);

                    glBegin(GL_QUADS);
                        glVertex2f(selectionPos.x, yPos);
                        glVertex2f(selectionPos.y, yPos);
                        glVertex2f(selectionPos.y, realPos.y);
                        glVertex2f(selectionPos.x, realPos.y);
                    glEnd();
                }
            }

        }

    }

}
<commit_msg>Resolved a bug with TextInput cursor color.<commit_after>/*
    Copyright (c) 2009, Robin RUAUX
    All rights reserved.
    Redistribution and use in source and binary forms, with or without
    modification, are permitted provided that the following conditions are met:

        * Redistributions of source code must retain the above copyright
          notice, this list of conditions and the following disclaimer.
        * Redistributions in binary form must reproduce the above copyright
          notice, this list of conditions and the following disclaimer in the
          documentation and/or other materials provided with the distribution.
        * Neither the name of the University of California, Berkeley nor the
          names of its contributors may be used to endorse or promote products
          derived from this software without specific prior written permission.

    THIS SOFTWARE IS PROVIDED BY THE REGENTS 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 REGENTS AND 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.

*/

#include "TextInput.hpp"
#include <SFML/Window/Event.hpp>
#include <SFML/Graphics/RenderTarget.hpp>
#include <SFML/Window/OpenGL.hpp>

namespace sf
{
    namespace ui
    {
        float    TextInput::mStringOffset = 4.f;

        TextInput::TextInput(const Unicode::Text& string)
            :   Widget(),
                mString(string),
                mMaxLength(0),
                mEditable(true),
                mCursorPosition(0),
                mCursorOffset(0),
                mSelectionDragged(false),
                mSelectionShifted(false),
                mSelectionStart(0)
        {
            SetDefaultStyle("BI_TextInput");
            LoadStyle(GetDefaultStyle());

            Add(&mString);
            AdjustRect();

            AddKeyListener(this);
            AddMouseListener(this);
        }

        void    TextInput::SetText(const Unicode::Text& text)
        {
            mString.SetText(text);
        }

        const Unicode::Text& TextInput::GetText() const
        {
            return mString.GetText();
        }

        bool    TextInput::IsEmpty() const
        {
            return (Unicode::UTF16String(mString.GetText()).size() == 0);
        }

        void    TextInput::LoadStyle(const std::string& nameStyle)
        {
            Widget::LoadStyle(nameStyle);

            ResourceManager* rm = ResourceManager::Get();
            StyleProperties& properties = rm->GetStyle(nameStyle);

            SetMaxLength(rm->GetValue(properties["maxLength"], GetMaxLength()));
            SetEditable(rm->GetValue(properties["editable"], IsEditable()));

            SetSelectionColor(rm->GetColorValue(properties["selectionColor"], GetSelectionColor()));

            SetTextColor(rm->GetColorValue(properties["textColor"], GetTextColor()));
            SetTextSize(rm->GetValue(properties["textSize"], GetTextSize()));

            Font* newFont = rm->GetFont(properties["font"], GetTextSize());
            if (newFont)
            {
                SetFont(*newFont);
            }

            mString.SetY((GetHeight() - mString.GetString().GetSize()) / 2 - 2);
        }

        void    TextInput::SetMaxLength(unsigned int maxLength)
        {
            mMaxLength = maxLength;
        }

        unsigned int    TextInput::GetMaxLength() const
        {
            return mMaxLength;
        }

        void            TextInput::SetEditable(bool editable)
        {
            mEditable = editable;
        }

        bool            TextInput::IsEditable() const
        {
            return mEditable;
        }

        void            TextInput::SetTextColor(const Color& color)
        {
            mString.SetTextColor(color);
        }

        const Color&    TextInput::GetTextColor() const
        {
            return mString.GetTextColor();
        }

        void            TextInput::SetTextSize(float size)
        {
            mString.SetTextSize(size);
        }

        float           TextInput::GetTextSize() const
        {
            return mString.GetTextSize();
        }

        void            TextInput::SetFont(const Font& font)
        {
            mString.SetFont(font);
        }

        const Font&     TextInput::GetFont() const
        {
            return mString.GetFont();
        }

        void            TextInput::ClearSelection()
        {
            mSelectionStart = mCursorPosition;
        }

        bool            TextInput::EraseSelection()
        {
            if (mSelectionStart == mCursorPosition)
                return false;

            Unicode::UTF16String currentText = GetText();
            unsigned int newPos = std::min(mSelectionStart, mCursorPosition);

            currentText.erase(newPos, GetSelectionSize());
            SetText(currentText);

            mSelectionStart = mCursorPosition = newPos;
            return true;
        }

        void            TextInput::SetSelection(unsigned int start, unsigned int cursorPosition)
        {
            const Unicode::UTF16String& text = GetText();

            mSelectionStart = start;
            mCursorPosition = (cursorPosition > text.length()) ? text.length() : cursorPosition;
        }

        void            TextInput::SetSelectionColor(const Color& color)
        {
            mSelectionColor = color;
        }

        const Color&    TextInput::GetSelectionColor() const
        {
            return mSelectionColor;
        }

        Unicode::Text   TextInput::GetSelection() const
        {
            const Unicode::UTF16String& text = GetText();
            return text.substr(std::min(mSelectionStart, mCursorPosition), GetSelectionSize());
        }

        unsigned int    TextInput::GetSelectionSize() const
        {
            if (mSelectionStart < mCursorPosition)
                return mCursorPosition - mSelectionStart;
            return mSelectionStart - mCursorPosition;
        }

        unsigned int    TextInput::GetCharacterAtPos(float xOffset)
        {
            const Unicode::UTF16String& text = GetText();

            for (unsigned int i = 0; i < text.length(); ++i)
            {
                mString.SetText(text.substr(0, i));
                const FloatRect& rect = mString.GetString().GetRect();
                mString.SetText(text);

                if (rect.GetWidth() > xOffset)
                    return i;
            }
            return text.length();
        }

        void    TextInput::OnMousePressed(const Event::MouseButtonEvent& button)
        {
            if (button.Button != Mouse::Left)
                return;

            mCursorPosition = GetCharacterAtPos(button.X - GetAbsolutePosition().x - mStringOffset + mCursorOffset);
            mSelectionDragged = true;
            ClearSelection();

            AdjustRect();
        }

        void    TextInput::OnMouseReleased(const Event::MouseButtonEvent& button)
        {
            if (button.Button != Mouse::Left)
                return;

            if (mSelectionDragged)
                mSelectionDragged = false;
        }

        void    TextInput::OnMouseMoved(const Event::MouseMoveEvent& mouse)
        {
            if (mSelectionDragged)
            {
                mCursorPosition = GetCharacterAtPos(mouse.X - GetAbsolutePosition().x - mStringOffset + mCursorOffset);
                AdjustRect();
            }
        }

        void    TextInput::OnKeyPressed(const Event::KeyEvent& key)
        {
            Unicode::UTF16String text = GetText();

            if (key.Code == Key::Left)
            {
                if (mCursorPosition)
                    --mCursorPosition;

                if (!mSelectionShifted)
                    ClearSelection();
            }
            else if (key.Code == Key::Right)
            {
                if (mCursorPosition < text.length())
                    ++mCursorPosition;

                if (!mSelectionShifted)
                    ClearSelection();
            }
            else if (key.Code == Key::Home)
            {
                if (mSelectionShifted)
                {
                    mSelectionStart = mCursorPosition;
                    mCursorPosition = 0;
                }
                else
                {
                    mCursorPosition = 0;
                    ClearSelection();
                }
            }
            else if (key.Code == Key::End)
            {
                if (mSelectionShifted)
                {
                    mSelectionStart = mCursorPosition;
                    mCursorPosition = text.length();
                }
                else
                {
                    mCursorPosition = text.length();
                    ClearSelection();
                }
            }
            else if (key.Code == Key::LShift || key.Code == Key::RShift)
            {
                mSelectionShifted = true;
            }
            else if ((key.Code == Key::Delete || key.Code == Key::Back) && mEditable)
            {
                if (!EraseSelection())
                {
                    if (key.Code == Key::Back && !text.empty() && mCursorPosition)
                    {
                        text.erase(--mCursorPosition, 1);
                    }
                    else if (key.Code == Key::Delete && mCursorPosition < text.length())
                    {
                        text.erase(mCursorPosition, 1);
                    }
                    ClearSelection();
                    SetText(text);
                }
            }
            AdjustRect();
        }

        void    TextInput::OnKeyReleased(const Event::KeyEvent& key)
        {
            if (key.Code == Key::LShift || key.Code == Key::RShift)
            {
                mSelectionShifted = false;
            }
        }

        void    TextInput::OnTextEntered(const Event::TextEvent& text)
        {
            // ASCII non printable caracters have to be ignored.
            if (text.Unicode <= 30 || (text.Unicode >= 127 && text.Unicode <= 159) || !mEditable)
                return;

            EraseSelection();

            Unicode::UTF16String currentText = GetText();

            if (currentText.length() < mMaxLength || !mMaxLength)
            {
                Unicode::UTF16String c;
                c += text.Unicode;
                currentText.insert(mCursorPosition, c);
                ++mCursorPosition;
            }

            SetText(currentText);
            ClearSelection();
            AdjustRect();
        }

        void    TextInput::AdjustRect()
        {
            const Unicode::UTF16String& text = GetText();

            mString.SetText(text.substr(0, mCursorPosition));
            const FloatRect& rect = mString.GetString().GetRect();
            mString.SetText(text);

            if (rect.GetWidth() - mCursorOffset >= GetWidth() - mStringOffset)
            {
                mCursorOffset = rect.GetWidth() - GetWidth() + mStringOffset;
            }
            else if (rect.GetWidth() - mCursorOffset <= 0)
            {
                mCursorOffset = rect.GetWidth() - GetWidth() / (mStringOffset / 2);
            }

            if (mCursorOffset < 0)
                mCursorOffset = 0;

            mString.SetX(mStringOffset - mCursorOffset);
        }

        void    TextInput::OnPaint(RenderTarget& target) const
        {
            Widget::OnPaint(target);

            // Draws the cursor with real position
            if (HasFocus())
            {
                const String& rStr = mString.GetString();
                float factor = rStr.GetSize() / rStr.GetFont().GetCharacterSize();
                float yPos = 2.f / factor;
                glScalef(factor, factor, 1.f);

                Vector2f realPos = rStr.GetCharacterPos(mCursorPosition);
                realPos.x = (realPos.x - mCursorOffset + mStringOffset) / factor;
                realPos.y = (GetHeight() - 2.f) / factor;

                glDisable(GL_TEXTURE_2D);

                const Color& colCursor = mString.GetTextColor();
                glColor4ub(colCursor.r, colCursor.g, colCursor.b, colCursor.a);

                glBegin(GL_LINES);
                    glVertex2f(realPos.x, yPos);
                    glVertex2f(realPos.x, realPos.y);
                glEnd();

                // Draws the current selection
                if (mSelectionStart != mCursorPosition)
                {
                    Vector2f selectionPos(rStr.GetCharacterPos(mSelectionStart).x, rStr.GetCharacterPos(mCursorPosition).x);
                    selectionPos.x = (selectionPos.x - mCursorOffset + mStringOffset) / factor;
                    selectionPos.y = (selectionPos.y - mCursorOffset + mStringOffset) / factor;

                    const Color& col = mSelectionColor;
                    glColor4ub(col.r , col.g , col.b, col.a);

                    glBegin(GL_QUADS);
                        glVertex2f(selectionPos.x, yPos);
                        glVertex2f(selectionPos.y, yPos);
                        glVertex2f(selectionPos.y, realPos.y);
                        glVertex2f(selectionPos.x, realPos.y);
                    glEnd();
                }
            }

        }

    }

}
<|endoftext|>