<commit_before>/*
* PKCS#11 Mechanism
* (C) 2016 Daniel Neus, Sirrix AG
* (C) 2016 Philipp Weber, Sirrix AG
*
* Botan is released under the Simplified BSD License (see license.txt)
*/

#include <botan/internal/p11_mechanism.h>
#include <botan/scan_name.h>
#include <botan/parsing.h>
#include <botan/emsa.h>

#include <tuple>

namespace Botan {
namespace PKCS11 {

namespace {
using PSS_Params = std::tuple<size_t, MechanismType, MGF>;

// maps a PSS mechanism type to the number of bytes used for the salt, the mechanism type of the underlying hash algorithm and the MGF
static const std::map<MechanismType, PSS_Params> PssOptions =
   {
      { MechanismType::RsaPkcsPss, PSS_Params(0, MechanismType::Sha1, MGF::Mgf1Sha1) },
      { MechanismType::Sha1RsaPkcsPss, PSS_Params(20, MechanismType::Sha1, MGF::Mgf1Sha1) },
      { MechanismType::Sha224RsaPkcsPss, PSS_Params(28, MechanismType::Sha224, MGF::Mgf1Sha224) },
      { MechanismType::Sha256RsaPkcsPss, PSS_Params(32, MechanismType::Sha256, MGF::Mgf1Sha256) },
      { MechanismType::Sha384RsaPkcsPss, PSS_Params(48, MechanismType::Sha384, MGF::Mgf1Sha384) },
      { MechanismType::Sha512RsaPkcsPss, PSS_Params(64, MechanismType::Sha512, MGF::Mgf1Sha512) }
   };

struct MechanismData
   {
   explicit MechanismData(MechanismType _type)
      : type(_type)
      {}

   MechanismData(MechanismData const&) = default;
   MechanismData& operator=(MechanismData const&) = default;
   virtual ~MechanismData() = default;

   // the mechanism to perform
   MechanismType type;
   };

struct RSA_SignMechanism final : public MechanismData
   {
   explicit RSA_SignMechanism(MechanismType _type)
      : MechanismData(_type), hash(static_cast<MechanismType>(0)), mgf(static_cast<MGF>(0)), salt_size(0)
      {
      auto pss_option = PssOptions.find(type);
      if(pss_option != PssOptions.end())
         {
         hash = std::get<1>(pss_option->second);
         mgf = std::get<2>(pss_option->second);
         salt_size = std::get<0>(pss_option->second);
         }
      }

   // hash algorithm used in the PSS encoding; if the signature mechanism does not include message hashing,
   // then this value must be the mechanism used by the application to generate the message hash;
   // if the signature mechanism includes hashing, then this value must match the hash algorithm indicated by the signature mechanism
   MechanismType hash;

   // mask generation function to use on the encoded block
   MGF mgf;

   // length, in bytes, of the salt value used in the PSS encoding; typical values are the length of the message hash and zero
   size_t salt_size;
   };

// note: when updating this map, update the documentation for `MechanismWrapper::create_rsa_sign_mechanism`
static std::map<std::string, RSA_SignMechanism> SignMechanisms =
   {
      { "Raw", RSA_SignMechanism(MechanismType::RsaX509) },

      { "EMSA2(Raw)", RSA_SignMechanism(MechanismType::RsaX931) },
      { "EMSA2(SHA-1)", RSA_SignMechanism(MechanismType::Sha1RsaX931) },

      // RSASSA PKCS#1 v1.5
      { "EMSA3(Raw)", RSA_SignMechanism(MechanismType::RsaPkcs) },
      { "EMSA3(SHA-1)", RSA_SignMechanism(MechanismType::Sha1RsaPkcs) },
      { "EMSA3(SHA-224)", RSA_SignMechanism(MechanismType::Sha224RsaPkcs) },
      { "EMSA3(SHA-256)", RSA_SignMechanism(MechanismType::Sha256RsaPkcs) },
      { "EMSA3(SHA-384)", RSA_SignMechanism(MechanismType::Sha384RsaPkcs) },
      { "EMSA3(SHA-512)", RSA_SignMechanism(MechanismType::Sha512RsaPkcs) },

      // RSASSA PKCS#1 PSS
      { "EMSA4(Raw)", RSA_SignMechanism(MechanismType::RsaPkcsPss) },
      { "EMSA4(SHA-1)", RSA_SignMechanism(MechanismType::Sha1RsaPkcsPss) },
      { "EMSA4(SHA-224)", RSA_SignMechanism(MechanismType::Sha224RsaPkcsPss) },
      { "EMSA4(SHA-256)", RSA_SignMechanism(MechanismType::Sha256RsaPkcsPss) },
      { "EMSA4(SHA-384)", RSA_SignMechanism(MechanismType::Sha384RsaPkcsPss) },
      { "EMSA4(SHA-512)", RSA_SignMechanism(MechanismType::Sha512RsaPkcsPss) },

      { "ISO9796", RSA_SignMechanism(MechanismType::Rsa9796) }
   };

struct RSA_CryptMechanism final : public MechanismData
   {
   RSA_CryptMechanism(MechanismType _type, size_t _padding_size, MechanismType _hash, MGF _mgf)
      : MechanismData(_type), hash(_hash), mgf(_mgf), padding_size(_padding_size)
      {}

   RSA_CryptMechanism(MechanismType _type, size_t _padding_size)
      : RSA_CryptMechanism(_type, _padding_size, static_cast<MechanismType>(0), static_cast<MGF>(0))
      {}

   // mechanism ID of the message digest algorithm used to calculate the digest of the encoding parameter
   MechanismType hash;

   // mask generation function to use on the encoded block
   MGF mgf;

   // number of bytes required for the padding
   size_t padding_size;
   };

// note: when updating this map, update the documentation for `MechanismWrapper::create_rsa_crypt_mechanism`
static const std::map<std::string, RSA_CryptMechanism> CryptMechanisms =
   {
      { "Raw", RSA_CryptMechanism(MechanismType::RsaX509, 0) },
      { "EME-PKCS1-v1_5", RSA_CryptMechanism(MechanismType::RsaPkcs, 11) },
      { "OAEP(SHA-1)", RSA_CryptMechanism(MechanismType::RsaPkcsOaep, 2 + 2 * 20, MechanismType::Sha1, MGF::Mgf1Sha1) },
      { "OAEP(SHA-224)", RSA_CryptMechanism(MechanismType::RsaPkcsOaep, 2 + 2 * 28, MechanismType::Sha224, MGF::Mgf1Sha224) },
      { "OAEP(SHA-256)", RSA_CryptMechanism(MechanismType::RsaPkcsOaep, 2 + 2 * 32, MechanismType::Sha256, MGF::Mgf1Sha256) },
      { "OAEP(SHA-384)", RSA_CryptMechanism(MechanismType::RsaPkcsOaep, 2 + 2 * 48, MechanismType::Sha384, MGF::Mgf1Sha384) },
      { "OAEP(SHA-512)", RSA_CryptMechanism(MechanismType::RsaPkcsOaep, 2 + 2 * 64, MechanismType::Sha512, MGF::Mgf1Sha512) }
   };

// note: when updating this map, update the documentation for `MechanismWrapper::create_ecdsa_mechanism`
static std::map<std::string, MechanismType> EcdsaHash =
   {
      { "Raw", MechanismType::Ecdsa },
      { "SHA-160", MechanismType::EcdsaSha1 },
      { "SHA-224", MechanismType::EcdsaSha224 },
      { "SHA-256", MechanismType::EcdsaSha256 },
      { "SHA-384", MechanismType::EcdsaSha384 },
      { "SHA-512", MechanismType::EcdsaSha512 }
   };

// note: when updating this map, update the documentation for `MechanismWrapper::create_ecdh_mechanism`
static std::map<std::string, KeyDerivation> EcdhHash =
   {
      { "Raw", KeyDerivation::Null },
      { "SHA-160", KeyDerivation::Sha1Kdf },
      { "SHA-224", KeyDerivation::Sha224Kdf },
      { "SHA-256", KeyDerivation::Sha256Kdf },
      { "SHA-384", KeyDerivation::Sha384Kdf },
      { "SHA-512", KeyDerivation::Sha512Kdf }
   };
}

MechanismWrapper::MechanismWrapper(MechanismType mechanism_type)
   : m_mechanism( { static_cast<CK_MECHANISM_TYPE>(mechanism_type), nullptr, 0 }), m_parameters(nullptr)
   {}

MechanismWrapper MechanismWrapper::create_rsa_crypt_mechanism(const std::string& padding)
   {
   auto mechanism_info_it = CryptMechanisms.find(padding);
   if(mechanism_info_it == CryptMechanisms.end())
      {
      // at this point it would be possible to support additional configurations that are not predefined above by parsing `padding`
      throw Lookup_Error("PKCS#11 RSA encrypt/decrypt does not support EME " + padding);
      }
   RSA_CryptMechanism mechanism_info = mechanism_info_it->second;

   MechanismWrapper mech(mechanism_info.type);
   if(mechanism_info.type == MechanismType::RsaPkcsOaep)
      {
      mech.m_parameters = std::make_shared<MechanismParameters>();
      mech.m_parameters->oaep_params.hashAlg = static_cast<CK_MECHANISM_TYPE>(mechanism_info.hash);
      mech.m_parameters->oaep_params.mgf = static_cast<CK_RSA_PKCS_MGF_TYPE>(mechanism_info.mgf);
      mech.m_parameters->oaep_params.source = CKZ_DATA_SPECIFIED;
      mech.m_parameters->oaep_params.pSourceData = nullptr;
      mech.m_parameters->oaep_params.ulSourceDataLen = 0;
      mech.m_mechanism.pParameter = mech.m_parameters.get();
      mech.m_mechanism.ulParameterLen = sizeof(RsaPkcsOaepParams);
      }
   mech.m_padding_size = mechanism_info.padding_size;
   return mech;
   }

MechanismWrapper MechanismWrapper::create_rsa_sign_mechanism(const std::string& padding)
   {
   auto mechanism_info_it = SignMechanisms.find(padding);
   if(mechanism_info_it == SignMechanisms.end())
      {
      // at this point it would be possible to support additional configurations that are not predefined above by parsing `padding`
      throw Lookup_Error("PKCS#11 RSA sign/verify does not support EMSA " + padding);
      }
   RSA_SignMechanism mechanism_info = mechanism_info_it->second;

   MechanismWrapper mech(mechanism_info.type);
   if(PssOptions.find(mechanism_info.type) != PssOptions.end())
      {
      mech.m_parameters = std::make_shared<MechanismParameters>();
      mech.m_parameters->pss_params.hashAlg = static_cast<CK_MECHANISM_TYPE>(mechanism_info.hash);
      mech.m_parameters->pss_params.mgf = static_cast<CK_RSA_PKCS_MGF_TYPE>(mechanism_info.mgf);
      mech.m_parameters->pss_params.sLen = mechanism_info.salt_size;
      mech.m_mechanism.pParameter = mech.m_parameters.get();
      mech.m_mechanism.ulParameterLen = sizeof(RsaPkcsPssParams);
      }
   return mech;
   }

MechanismWrapper MechanismWrapper::create_ecdsa_mechanism(const std::string& hash)
   {
   std::string hash_name = hash;

   if(hash_name != "Raw")
      {
      hash_name = hash_for_emsa(hash);
      }

   auto mechanism_type = EcdsaHash.find(hash_name);
   if(mechanism_type == EcdsaHash.end())
      {
      throw Lookup_Error("PKCS#11 ECDSA sign/verify does not support " + hash);
      }
   return MechanismWrapper(mechanism_type->second);
   }

MechanismWrapper MechanismWrapper::create_ecdh_mechanism(const std::string& params)
   {
   std::vector<std::string> param_parts = split_on(params, ',');

   if(param_parts.empty() || param_parts.size() > 2)
      throw Invalid_Argument("PKCS #11 ECDH key derivation bad params " + params);

   const bool use_cofactor =
      (param_parts[0] == "Cofactor") ||
      (param_parts.size() == 2 && param_parts[1] == "Cofactor");

   std::string kdf_name = (param_parts[0] == "Cofactor" ? param_parts[1] : param_parts[0]);
   std::string hash = kdf_name;

   if(kdf_name != "Raw")
      {
      SCAN_Name kdf_hash(kdf_name);

      if(kdf_hash.arg_count() > 0)
         {
         hash = kdf_hash.arg(0);
         }
      }

   auto kdf = EcdhHash.find(hash);
   if(kdf == EcdhHash.end())
      {
      throw Lookup_Error("PKCS#11 ECDH key derivation does not support KDF " + kdf_name);
      }
   MechanismWrapper mech(use_cofactor ? MechanismType::Ecdh1CofactorDerive : MechanismType::Ecdh1Derive);
   mech.m_parameters = std::make_shared<MechanismParameters>();
   mech.m_parameters->ecdh_params.kdf = static_cast<CK_EC_KDF_TYPE>(kdf->second);
   mech.m_mechanism.pParameter = mech.m_parameters.get();
   mech.m_mechanism.ulParameterLen = sizeof(Ecdh1DeriveParams);
   return mech;
   }

}
}
<commit_msg>Extend the SignMechanisms map for Signature_Scheme padding strings<commit_after>/*
* PKCS#11 Mechanism
* (C) 2016 Daniel Neus, Sirrix AG
* (C) 2016 Philipp Weber, Sirrix AG
*
* Botan is released under the Simplified BSD License (see license.txt)
*/

#include <botan/internal/p11_mechanism.h>
#include <botan/scan_name.h>
#include <botan/parsing.h>
#include <botan/emsa.h>

#include <tuple>

namespace Botan {
namespace PKCS11 {

namespace {
using PSS_Params = std::tuple<size_t, MechanismType, MGF>;

// maps a PSS mechanism type to the number of bytes used for the salt, the mechanism type of the underlying hash algorithm and the MGF
static const std::map<MechanismType, PSS_Params> PssOptions =
   {
      { MechanismType::RsaPkcsPss, PSS_Params(0, MechanismType::Sha1, MGF::Mgf1Sha1) },
      { MechanismType::Sha1RsaPkcsPss, PSS_Params(20, MechanismType::Sha1, MGF::Mgf1Sha1) },
      { MechanismType::Sha224RsaPkcsPss, PSS_Params(28, MechanismType::Sha224, MGF::Mgf1Sha224) },
      { MechanismType::Sha256RsaPkcsPss, PSS_Params(32, MechanismType::Sha256, MGF::Mgf1Sha256) },
      { MechanismType::Sha384RsaPkcsPss, PSS_Params(48, MechanismType::Sha384, MGF::Mgf1Sha384) },
      { MechanismType::Sha512RsaPkcsPss, PSS_Params(64, MechanismType::Sha512, MGF::Mgf1Sha512) }
   };

struct MechanismData
   {
   explicit MechanismData(MechanismType _type)
      : type(_type)
      {}

   MechanismData(MechanismData const&) = default;
   MechanismData& operator=(MechanismData const&) = default;
   virtual ~MechanismData() = default;

   // the mechanism to perform
   MechanismType type;
   };

struct RSA_SignMechanism final : public MechanismData
   {
   explicit RSA_SignMechanism(MechanismType _type)
      : MechanismData(_type), hash(static_cast<MechanismType>(0)), mgf(static_cast<MGF>(0)), salt_size(0)
      {
      auto pss_option = PssOptions.find(type);
      if(pss_option != PssOptions.end())
         {
         hash = std::get<1>(pss_option->second);
         mgf = std::get<2>(pss_option->second);
         salt_size = std::get<0>(pss_option->second);
         }
      }

   // hash algorithm used in the PSS encoding; if the signature mechanism does not include message hashing,
   // then this value must be the mechanism used by the application to generate the message hash;
   // if the signature mechanism includes hashing, then this value must match the hash algorithm indicated by the signature mechanism
   MechanismType hash;

   // mask generation function to use on the encoded block
   MGF mgf;

   // length, in bytes, of the salt value used in the PSS encoding; typical values are the length of the message hash and zero
   size_t salt_size;
   };

// note: when updating this map, update the documentation for `MechanismWrapper::create_rsa_sign_mechanism`
static std::map<std::string, RSA_SignMechanism> SignMechanisms =
   {
      { "Raw", RSA_SignMechanism(MechanismType::RsaX509) },

      { "EMSA1(SHA-1)", RSA_SignMechanism(MechanismType::DsaSha1) },
      { "EMSA1(SHA-256)", RSA_SignMechanism(MechanismType::DsaSha256) },
      { "EMSA1(SHA-384)", RSA_SignMechanism(MechanismType::DsaSha384) },
      { "EMSA1(SHA-512)", RSA_SignMechanism(MechanismType::DsaSha512) },

      { "EMSA2(Raw)", RSA_SignMechanism(MechanismType::RsaX931) },
      { "EMSA2(SHA-1)", RSA_SignMechanism(MechanismType::Sha1RsaX931) },

      // RSASSA PKCS#1 v1.5
      { "EMSA3(Raw)", RSA_SignMechanism(MechanismType::RsaPkcs) },
      { "EMSA3(SHA-1)", RSA_SignMechanism(MechanismType::Sha1RsaPkcs) },
      { "EMSA3(SHA-224)", RSA_SignMechanism(MechanismType::Sha224RsaPkcs) },
      { "EMSA3(SHA-256)", RSA_SignMechanism(MechanismType::Sha256RsaPkcs) },
      { "EMSA3(SHA-384)", RSA_SignMechanism(MechanismType::Sha384RsaPkcs) },
      { "EMSA3(SHA-512)", RSA_SignMechanism(MechanismType::Sha512RsaPkcs) },

      { "EMSA_PKCS1(SHA-1)", RSA_SignMechanism(MechanismType::Sha1RsaPkcs) },
      { "EMSA_PKCS1(SHA-224)", RSA_SignMechanism(MechanismType::Sha224RsaPkcs) },
      { "EMSA_PKCS1(SHA-256)", RSA_SignMechanism(MechanismType::Sha256RsaPkcs) },
      { "EMSA_PKCS1(SHA-384)", RSA_SignMechanism(MechanismType::Sha384RsaPkcs) },
      { "EMSA_PKCS1(SHA-512)", RSA_SignMechanism(MechanismType::Sha512RsaPkcs) },

      // RSASSA PKCS#1 PSS
      { "EMSA4(Raw)", RSA_SignMechanism(MechanismType::RsaPkcsPss) },
      { "EMSA4(SHA-1)", RSA_SignMechanism(MechanismType::Sha1RsaPkcsPss) },
      { "EMSA4(SHA-224)", RSA_SignMechanism(MechanismType::Sha224RsaPkcsPss) },
      { "EMSA4(SHA-256)", RSA_SignMechanism(MechanismType::Sha256RsaPkcsPss) },
      { "EMSA4(SHA-384)", RSA_SignMechanism(MechanismType::Sha384RsaPkcsPss) },
      { "EMSA4(SHA-512)", RSA_SignMechanism(MechanismType::Sha512RsaPkcsPss) },

      { "PSSR(SHA-256,MGF1,32)", RSA_SignMechanism(MechanismType::Sha256RsaPkcsPss) },
      { "PSSR(SHA-384,MGF1,48)", RSA_SignMechanism(MechanismType::Sha384RsaPkcsPss) },
      { "PSSR(SHA-512,MGF1,64)", RSA_SignMechanism(MechanismType::Sha512RsaPkcsPss) },

      { "ISO9796", RSA_SignMechanism(MechanismType::Rsa9796) }
   };

struct RSA_CryptMechanism final : public MechanismData
   {
   RSA_CryptMechanism(MechanismType _type, size_t _padding_size, MechanismType _hash, MGF _mgf)
      : MechanismData(_type), hash(_hash), mgf(_mgf), padding_size(_padding_size)
      {}

   RSA_CryptMechanism(MechanismType _type, size_t _padding_size)
      : RSA_CryptMechanism(_type, _padding_size, static_cast<MechanismType>(0), static_cast<MGF>(0))
      {}

   // mechanism ID of the message digest algorithm used to calculate the digest of the encoding parameter
   MechanismType hash;

   // mask generation function to use on the encoded block
   MGF mgf;

   // number of bytes required for the padding
   size_t padding_size;
   };

// note: when updating this map, update the documentation for `MechanismWrapper::create_rsa_crypt_mechanism`
static const std::map<std::string, RSA_CryptMechanism> CryptMechanisms =
   {
      { "Raw", RSA_CryptMechanism(MechanismType::RsaX509, 0) },
      { "EME-PKCS1-v1_5", RSA_CryptMechanism(MechanismType::RsaPkcs, 11) },
      { "OAEP(SHA-1)", RSA_CryptMechanism(MechanismType::RsaPkcsOaep, 2 + 2 * 20, MechanismType::Sha1, MGF::Mgf1Sha1) },
      { "OAEP(SHA-224)", RSA_CryptMechanism(MechanismType::RsaPkcsOaep, 2 + 2 * 28, MechanismType::Sha224, MGF::Mgf1Sha224) },
      { "OAEP(SHA-256)", RSA_CryptMechanism(MechanismType::RsaPkcsOaep, 2 + 2 * 32, MechanismType::Sha256, MGF::Mgf1Sha256) },
      { "OAEP(SHA-384)", RSA_CryptMechanism(MechanismType::RsaPkcsOaep, 2 + 2 * 48, MechanismType::Sha384, MGF::Mgf1Sha384) },
      { "OAEP(SHA-512)", RSA_CryptMechanism(MechanismType::RsaPkcsOaep, 2 + 2 * 64, MechanismType::Sha512, MGF::Mgf1Sha512) }
   };

// note: when updating this map, update the documentation for `MechanismWrapper::create_ecdsa_mechanism`
static std::map<std::string, MechanismType> EcdsaHash =
   {
      { "Raw", MechanismType::Ecdsa },
      { "SHA-160", MechanismType::EcdsaSha1 },
      { "SHA-224", MechanismType::EcdsaSha224 },
      { "SHA-256", MechanismType::EcdsaSha256 },
      { "SHA-384", MechanismType::EcdsaSha384 },
      { "SHA-512", MechanismType::EcdsaSha512 }
   };

// note: when updating this map, update the documentation for `MechanismWrapper::create_ecdh_mechanism`
static std::map<std::string, KeyDerivation> EcdhHash =
   {
      { "Raw", KeyDerivation::Null },
      { "SHA-160", KeyDerivation::Sha1Kdf },
      { "SHA-224", KeyDerivation::Sha224Kdf },
      { "SHA-256", KeyDerivation::Sha256Kdf },
      { "SHA-384", KeyDerivation::Sha384Kdf },
      { "SHA-512", KeyDerivation::Sha512Kdf }
   };
}

MechanismWrapper::MechanismWrapper(MechanismType mechanism_type)
   : m_mechanism( { static_cast<CK_MECHANISM_TYPE>(mechanism_type), nullptr, 0 }), m_parameters(nullptr)
   {}

MechanismWrapper MechanismWrapper::create_rsa_crypt_mechanism(const std::string& padding)
   {
   auto mechanism_info_it = CryptMechanisms.find(padding);
   if(mechanism_info_it == CryptMechanisms.end())
      {
      // at this point it would be possible to support additional configurations that are not predefined above by parsing `padding`
      throw Lookup_Error("PKCS#11 RSA encrypt/decrypt does not support EME " + padding);
      }
   RSA_CryptMechanism mechanism_info = mechanism_info_it->second;

   MechanismWrapper mech(mechanism_info.type);
   if(mechanism_info.type == MechanismType::RsaPkcsOaep)
      {
      mech.m_parameters = std::make_shared<MechanismParameters>();
      mech.m_parameters->oaep_params.hashAlg = static_cast<CK_MECHANISM_TYPE>(mechanism_info.hash);
      mech.m_parameters->oaep_params.mgf = static_cast<CK_RSA_PKCS_MGF_TYPE>(mechanism_info.mgf);
      mech.m_parameters->oaep_params.source = CKZ_DATA_SPECIFIED;
      mech.m_parameters->oaep_params.pSourceData = nullptr;
      mech.m_parameters->oaep_params.ulSourceDataLen = 0;
      mech.m_mechanism.pParameter = mech.m_parameters.get();
      mech.m_mechanism.ulParameterLen = sizeof(RsaPkcsOaepParams);
      }
   mech.m_padding_size = mechanism_info.padding_size;
   return mech;
   }

MechanismWrapper MechanismWrapper::create_rsa_sign_mechanism(const std::string& padding)
   {
   auto mechanism_info_it = SignMechanisms.find(padding);
   if(mechanism_info_it == SignMechanisms.end())
      {
      // at this point it would be possible to support additional configurations that are not predefined above by parsing `padding`
      throw Lookup_Error("PKCS#11 RSA sign/verify does not support EMSA " + padding);
      }
   RSA_SignMechanism mechanism_info = mechanism_info_it->second;

   MechanismWrapper mech(mechanism_info.type);
   if(PssOptions.find(mechanism_info.type) != PssOptions.end())
      {
      mech.m_parameters = std::make_shared<MechanismParameters>();
      mech.m_parameters->pss_params.hashAlg = static_cast<CK_MECHANISM_TYPE>(mechanism_info.hash);
      mech.m_parameters->pss_params.mgf = static_cast<CK_RSA_PKCS_MGF_TYPE>(mechanism_info.mgf);
      mech.m_parameters->pss_params.sLen = mechanism_info.salt_size;
      mech.m_mechanism.pParameter = mech.m_parameters.get();
      mech.m_mechanism.ulParameterLen = sizeof(RsaPkcsPssParams);
      }
   return mech;
   }

MechanismWrapper MechanismWrapper::create_ecdsa_mechanism(const std::string& hash)
   {
   std::string hash_name = hash;

   if(hash_name != "Raw")
      {
      hash_name = hash_for_emsa(hash);
      }

   auto mechanism_type = EcdsaHash.find(hash_name);
   if(mechanism_type == EcdsaHash.end())
      {
      throw Lookup_Error("PKCS#11 ECDSA sign/verify does not support " + hash);
      }
   return MechanismWrapper(mechanism_type->second);
   }

MechanismWrapper MechanismWrapper::create_ecdh_mechanism(const std::string& params)
   {
   std::vector<std::string> param_parts = split_on(params, ',');

   if(param_parts.empty() || param_parts.size() > 2)
      throw Invalid_Argument("PKCS #11 ECDH key derivation bad params " + params);

   const bool use_cofactor =
      (param_parts[0] == "Cofactor") ||
      (param_parts.size() == 2 && param_parts[1] == "Cofactor");

   std::string kdf_name = (param_parts[0] == "Cofactor" ? param_parts[1] : param_parts[0]);
   std::string hash = kdf_name;

   if(kdf_name != "Raw")
      {
      SCAN_Name kdf_hash(kdf_name);

      if(kdf_hash.arg_count() > 0)
         {
         hash = kdf_hash.arg(0);
         }
      }

   auto kdf = EcdhHash.find(hash);
   if(kdf == EcdhHash.end())
      {
      throw Lookup_Error("PKCS#11 ECDH key derivation does not support KDF " + kdf_name);
      }
   MechanismWrapper mech(use_cofactor ? MechanismType::Ecdh1CofactorDerive : MechanismType::Ecdh1Derive);
   mech.m_parameters = std::make_shared<MechanismParameters>();
   mech.m_parameters->ecdh_params.kdf = static_cast<CK_EC_KDF_TYPE>(kdf->second);
   mech.m_mechanism.pParameter = mech.m_parameters.get();
   mech.m_mechanism.ulParameterLen = sizeof(Ecdh1DeriveParams);
   return mech;
   }

}
}
<|endoftext|>


<commit_before>/* This code is subject to the terms of the Mozilla Public License, v.2.0. http://mozilla.org/MPL/2.0/. */
#include "udt_socket_set.h"

#include "udt/udt.h"
#include <iostream>

namespace
{
	int _epollReadFlag = EPOLLOpt::UDT_EPOLL_IN ^ EPOLLOpt::UDT_EPOLL_ERR;
	int _epollWriteFlag = EPOLLOpt::UDT_EPOLL_OUT ^ EPOLLOpt::UDT_EPOLL_ERR;
}

udt_socket_set::udt_socket_set(int flags)
	: _pollFlags(flags & WRITES? _epollWriteFlag : _epollReadFlag)
{
	_pollSet = UDT::epoll_create();
	if (_pollSet < 0)
		fatalError("couldn't get udt epoll to work: " + std::string(UDT::getlasterror().getErrorMessage()));
}

udt_socket_set::~udt_socket_set()
{
	release();
}

void udt_socket_set::release()
{
	if (_pollSet >= 0)
	{
		UDT::epoll_release(_pollSet);
		_pollSet = -1;
	}
}

bool udt_socket_set::good() const
{
	return _lastError.empty();
}

std::set<int> udt_socket_set::wait() const
{
	std::set<int> reads;
	std::set<int> writeReady;
	int waiters;
	if ((waiters = UDT::epoll_wait(_pollSet, &reads, &writeReady, 5000)) < 0)
	{
		std::cout << "epoll_wait did an error! :( " << waiters << std::endl;
		return reads;
	}

	if (_pollFlags == _epollWriteFlag)
		return writeReady;
	return reads;
}

bool udt_socket_set::add(int sock)
{
	return UDT::epoll_add_usock(_pollSet, sock, &_pollFlags) >= 0;
}

bool udt_socket_set::remove(int sock)
{
	return UDT::epoll_remove_usock(_pollSet, sock) >= 0;
}

std::string udt_socket_set::lastError() const
{
	return _lastError;
}

void udt_socket_set::fatalError(const std::string& error)
{
	_lastError = error;
}
<commit_msg>Don't be so noisy...<commit_after>/* This code is subject to the terms of the Mozilla Public License, v.2.0. http://mozilla.org/MPL/2.0/. */
#include "udt_socket_set.h"

#include "udt/udt.h"
#include <iostream>

namespace
{
	int _epollReadFlag = EPOLLOpt::UDT_EPOLL_IN ^ EPOLLOpt::UDT_EPOLL_ERR;
	int _epollWriteFlag = EPOLLOpt::UDT_EPOLL_OUT ^ EPOLLOpt::UDT_EPOLL_ERR;
}

udt_socket_set::udt_socket_set(int flags)
	: _pollFlags(flags & WRITES? _epollWriteFlag : _epollReadFlag)
{
	_pollSet = UDT::epoll_create();
	if (_pollSet < 0)
		fatalError("couldn't get udt epoll to work: " + std::string(UDT::getlasterror().getErrorMessage()));
}

udt_socket_set::~udt_socket_set()
{
	release();
}

void udt_socket_set::release()
{
	if (_pollSet >= 0)
	{
		UDT::epoll_release(_pollSet);
		_pollSet = -1;
	}
}

bool udt_socket_set::good() const
{
	return _lastError.empty();
}

std::set<int> udt_socket_set::wait() const
{
	std::set<int> reads;
	std::set<int> writeReady;
	int waiters;
	if ((waiters = UDT::epoll_wait(_pollSet, &reads, &writeReady, 5000)) < 0)
	{
		//std::cout << "epoll_wait did an error! :( " << waiters << std::endl;
		return reads;
	}

	if (_pollFlags == _epollWriteFlag)
		return writeReady;
	return reads;
}

bool udt_socket_set::add(int sock)
{
	return UDT::epoll_add_usock(_pollSet, sock, &_pollFlags) >= 0;
}

bool udt_socket_set::remove(int sock)
{
	return UDT::epoll_remove_usock(_pollSet, sock) >= 0;
}

std::string udt_socket_set::lastError() const
{
	return _lastError;
}

void udt_socket_set::fatalError(const std::string& error)
{
	_lastError = error;
}
<|endoftext|>


<commit_before>#include <iostream>
#include <thread>
#include <atomic>
#include <chrono>
#include <vector>

// from here [C++11 Implementation of Spinlock using <atomic>](http://stackoverflow.com/questions/26583433/c11-implementation-of-spinlock-using-atomic)
// see also [C++11 spinlock](http://anki3d.org/spinlock/)

class SpinLock {
    std::atomic_flag locked = ATOMIC_FLAG_INIT ;
public:
    void lock() {
        while (locked.test_and_set(std::memory_order_acquire)) { ; }
    }
    void unlock() {
        locked.clear(std::memory_order_release);
    }
};

SpinLock lock;
//std::mutex lock;
int count[8] = {0, 0, 0, 0, 0, 0, 0, 0};

void foo(unsigned n)
{
    const unsigned cnt = 10000000;
    for(unsigned i = 0; i < cnt; i++) {
        lock.lock();
        ++count[n];
        lock.unlock();
    }
}

int main()
{
    using namespace std::literals::chrono_literals;

    // warming up
    SpinLock spinlock;
    spinlock.lock();
    std::this_thread::sleep_for(0.2s);
    spinlock.unlock();
    std::cout << "spinlock unlocked\n";

    // benchmark
    std::vector<std::thread> v;
    for(int n = 0; n < 8; ++n) {
        v.emplace_back(foo, n);
    }

    for (auto& t : v) {
        t.join();
    }

    return count[0] + count[1] + count[2] + count[3]
        + count[4] + count[5] + count[6] + count[7];

}
<commit_msg>in comments: link to recursive spinlock.<commit_after>#include <iostream>
#include <thread>
#include <atomic>
#include <chrono>
#include <vector>

// from here [C++11 Implementation of Spinlock using <atomic>](http://stackoverflow.com/questions/26583433/c11-implementation-of-spinlock-using-atomic)
// see also [C++11 spinlock](http://anki3d.org/spinlock/)
// and [Code Review: ++11 recursive atomic spinlock](http://codereview.stackexchange.com/questions/95590/c11-recursive-atomic-spinlock)

class SpinLock {
    std::atomic_flag locked = ATOMIC_FLAG_INIT ;
public:
    void lock() {
        while (locked.test_and_set(std::memory_order_acquire)) { ; }
    }
    void unlock() {
        locked.clear(std::memory_order_release);
    }
};

SpinLock lock;
//std::mutex lock;
int count[8] = {0, 0, 0, 0, 0, 0, 0, 0};

void foo(unsigned n)
{
    const unsigned cnt = 10000000;
    for(unsigned i = 0; i < cnt; i++) {
        lock.lock();
        ++count[n];
        lock.unlock();
    }
}

int main()
{
    using namespace std::literals::chrono_literals;

    // warming up
    SpinLock spinlock;
    spinlock.lock();
    std::this_thread::sleep_for(0.2s);
    spinlock.unlock();
    std::cout << "spinlock unlocked\n";

    // benchmark
    std::vector<std::thread> v;
    for(int n = 0; n < 8; ++n) {
        v.emplace_back(foo, n);
    }

    for (auto& t : v) {
        t.join();
    }

    return count[0] + count[1] + count[2] + count[3]
        + count[4] + count[5] + count[6] + count[7];

}
<|endoftext|>


<commit_before>// ----------------------------------------------------------------------------
// ------------- AI Battleground, Copyright(C) Maciej Pryc, 2016 --------------
// ----------------------------------------------------------------------------

#define _USE_MATH_DEFINES
#include <math.h>
#include <iostream>

#include "Actor.h"
#include "TextureManager.h"
#include "LevelInfo.h"
#include "AISystemFSM.h"
#include "AISystemBT.h"

Actor::Actor(class LevelInfo* argLevelInfo, TextureManager* TexManager, const std::string& TexName, const ETeam argTeam, const sf::Vector2f& InitialPosition) :
LevelInfo(argLevelInfo), AISystem(nullptr), NearestEnemy(nullptr), LastQuadTreePosition(InitialPosition), Position(InitialPosition), DesiredMovementDirection(0.0f, 0.0f),
ActualMovementDirection(0.0f, 0.0f), VectorTowardsEnemy(0.0f, 0.0f), ShotDist(75.0f * (1.0f - GetRandomFloat(0.4f))), MovementSpeed(50.0f), DirectionChangeSpeed(5.0f),
MaxHP(100.0f), HP(MaxHP), Damage(10.0f), Team(argTeam), MovementDirectionInterpStart(0.0f, 0.0f), bInterpolateMovementDirection(false),
MovementDirectionInterpAlpha(0.0f), bShouldDrawLaser(false), ShotInterval(sf::seconds(0.5f)), ShotTimeCounter(ShotInterval),
QuadTreeUpdateInterval(sf::seconds(0.2f)), QuadTreeUpdateCounter(sf::seconds(GetRandomFloat(QuadTreeUpdateInterval.asSeconds())))
{
	for (int i = 0; i < ROBOT_SPRITES_AMOUNT; ++i)
	{
		sf::Vector2u TexSize = TexManager->InitTexture(&RobotSprite[i], TexName + std::to_string(i + 1));

		if (i == 0)
			Size = TexSize;
		else if (TexSize != Size)
			std::cout << "Actor construction: sprite textures in different sizes! " << TexName << " " << i << std::endl;

		RobotSprite[i].setOrigin(Size.x / 2.0f, Size.y / 2.0f);
	}

	BeamTexSize = TexManager->InitTexture(&LaserBeamSprite, "LaserBeam");
	LaserBeamSprite.setOrigin(0.0f, BeamTexSize.y / 2.0f);

	sf::Vector2u BurstTexSize = TexManager->InitTexture(&LaserBurstSprite, "LaserBurst");
	LaserBurstSprite.setOrigin(BurstTexSize.x / 2.0f, BurstTexSize.y / 2.0f);

	MovementDirectionOffset.x = GetRandomFloat(ShotDist * 1.5f) - ShotDist * 0.75f;
	MovementDirectionOffset.y = GetRandomFloat(ShotDist * 1.5f) - ShotDist * 0.75f;

	//AISystem = new AISystemFSM(this, &Blackboard);
	AISystem = new AISystemBT(this, &Blackboard);

	Blackboard.SetMaxHP(MaxHP);
	Blackboard.SetHP(HP);
}

Actor::~Actor()
{
	delete AISystem;
}

void Actor::SyncDrawData()
{
	DrawData_Position = Position;
	DrawData_HP = HP;
	DrawData_bShouldDrawLaser = bShouldDrawLaser;
	DrawData_VectorTowardsEnemy = VectorTowardsEnemy;
}

void Actor::DrawRobot(sf::RenderWindow* Window) const
{
	DrawLaserBurst(Window);

	sf::Transform CurrTransform;
	CurrTransform.translate(DrawData_Position);

	Window->draw(GetRobotSprite(), CurrTransform);
}

const sf::Sprite& Actor::GetRobotSprite() const
{
	const float HPRatio = DrawData_HP / MaxHP;
	return RobotSprite[(int)((1.0f - HPRatio) * ROBOT_SPRITES_AMOUNT)];
}

void Actor::DrawLaserBeam(sf::RenderWindow* Window) const
{
	if (DrawData_bShouldDrawLaser)
	{
		sf::Vector2f Diff = DrawData_VectorTowardsEnemy;
		NormalizeVector2f(Diff);
		DrawData_AngleToEnemy = acos(Diff.x);
		DrawData_AngleToEnemy *= (float)M_1_PI * 180.0f;

		if (Diff.y < 0.0f)
			DrawData_AngleToEnemy = 360.0f - DrawData_AngleToEnemy;

		sf::Transform CurrTransform;
		CurrTransform.translate(DrawData_Position);
		CurrTransform.rotate(DrawData_AngleToEnemy);
		CurrTransform.scale(GetLength(DrawData_VectorTowardsEnemy) / BeamTexSize.x, 1.0f);

		Window->draw(LaserBeamSprite, CurrTransform);
	}
}

void Actor::DrawLaserBurst(sf::RenderWindow* Window) const
{
	if (DrawData_bShouldDrawLaser)
	{
		sf::Transform CurrTransform;
		CurrTransform.translate(DrawData_Position);
		CurrTransform.rotate(DrawData_AngleToEnemy);
		CurrTransform.translate(sf::Vector2f(Size.x / 2.0f + 1.0f, 0.0f));
		Window->draw(LaserBurstSprite, CurrTransform);
	}
}

void Actor::Update(const float DeltaTime)
{
	bShouldDrawLaser = false;
	ShotTimeCounter += sf::seconds(DeltaTime);

	ProcessMovement(DeltaTime);
	UpdatePositionInQuadTree(DeltaTime);

	const bool bReached = GetSquaredDist(LevelInfo->GetHealZonePosition(Team) + MovementDirectionOffset, GetPosition()) < 100.0f;
	Blackboard.SetBHealthZoneDestReached(bReached);

	if (NearestEnemy)
	{
		VectorTowardsEnemy = NearestEnemy->GetPosition() - GetPosition();
		Blackboard.SetBEnemyInRange(GetLength(VectorTowardsEnemy) <= ShotDist);
	}
}

void Actor::UpdateAISystem()
{
	AISystem->Update();
}

void Actor::ProcessMovement(const float DeltaTime)
{
	if (bInterpolateMovementDirection)
	{
		if (MovementDirectionInterpAlpha >= 1.0f)
		{
			ActualMovementDirection = DesiredMovementDirection;
			bInterpolateMovementDirection = false;
		}
		else
		{
			MovementDirectionInterpAlpha += DeltaTime * DirectionChangeSpeed;
			ActualMovementDirection = (1.0f - MovementDirectionInterpAlpha) * MovementDirectionInterpStart + MovementDirectionInterpAlpha * DesiredMovementDirection;
		}
	}

	if (ActualMovementDirection != sf::Vector2f(0.0f, 0.0f))
	{
		Position += ActualMovementDirection * MovementSpeed * DeltaTime;
		ClampVector2f(Position, LevelInfo->Boundaries);
	}
}

void Actor::UpdatePositionInQuadTree(const float DeltaTime)
{
	QuadTreeUpdateCounter += sf::seconds(DeltaTime);

	if (QuadTreeUpdateCounter >= QuadTreeUpdateInterval)
	{
		LevelInfo->UpdatePositionInQuadTree(this);
		QuadTreeUpdateCounter -= QuadTreeUpdateInterval;
	}
}

void Actor::RetreatToHealZone()
{
	sf::Vector2f NewDesiredMovementDirection = LevelInfo->GetHealZonePosition(Team) - GetPosition() + MovementDirectionOffset;
	NormalizeVector2f(NewDesiredMovementDirection);
	SetDesiredMovementDirection(NewDesiredMovementDirection);
}

void Actor::StopMovement()
{
	if (DesiredMovementDirection != sf::Vector2f(0.0f, 0.0f))
		SetDesiredMovementDirection(sf::Vector2f(0.0f, 0.0f));
}

void Actor::GoTowardsNearestEnemy()
{
	sf::Vector2f NewDesiredMovementDirection = VectorTowardsEnemy + MovementDirectionOffset;
	NormalizeVector2f(NewDesiredMovementDirection);
	SetDesiredMovementDirection(NewDesiredMovementDirection);
}

void Actor::SetDesiredMovementDirection(const sf::Vector2f& NewDesiredMovementDireciton)
{
	DesiredMovementDirection = NewDesiredMovementDireciton;
	MovementDirectionInterpStart = ActualMovementDirection;
	MovementDirectionInterpAlpha = 0.0f;
	bInterpolateMovementDirection = true;
}

const sf::Vector2f& Actor::GetPosition() const
{
	return Position;
}

ETeam Actor::GetTeam() const
{
	return Team;
}

void Actor::SetNearestEnemy(Actor* NewNearestEnemy)
{
	NearestEnemy = NewNearestEnemy;
	Blackboard.SetNearestEnemy(NearestEnemy);
}

Actor* Actor::GetNearestEnemy() const
{
	return NearestEnemy;
}

void Actor::UpdateLastQuadTreePosition(const sf::Vector2f& NewPositionInQuadTree)
{
	LastQuadTreePosition = NewPositionInQuadTree;
}

const sf::Vector2f& Actor::GetLastQuadTreePosition() const
{
	return LastQuadTreePosition;
}

void Actor::Heal(float HPToHeal)
{
	HP = std::min(HP + HPToHeal, MaxHP);
	Blackboard.SetHP(HP);
}

void Actor::TryToShoot()
{
	if (ShotTimeCounter >= ShotInterval)
	{
		bShouldDrawLaser = true;
		NearestEnemy->TakeDamage(Damage);
		ShotTimeCounter = sf::Time::Zero;
	}
}

void Actor::TakeDamage(float DamageAmount)
{
	HP -= DamageAmount;
	Blackboard.SetHP(HP);

	if (HP <= 0.0f)
		LevelInfo->DestroyActor(this);
}

void Actor::GenerateRandomMovementDirection(EDirection DirectionToAvoid)
{
	switch (DirectionToAvoid)
	{
	case EDirection::UP:
		DesiredMovementDirection.x = GetRandomFloat(2.0f) - 1.0f;
		DesiredMovementDirection.y = GetRandomFloat();
		break;
	case EDirection::RIGHT:
		DesiredMovementDirection.x = GetRandomFloat() - 1.0f;
		DesiredMovementDirection.y = GetRandomFloat(2.0f) - 1.0f;
		break;
	case EDirection::DOWN:
		DesiredMovementDirection.x = GetRandomFloat(2.0f) - 1.0f;
		DesiredMovementDirection.y = GetRandomFloat() - 1.0f;
		break;
	case EDirection::LEFT:
		DesiredMovementDirection.x = GetRandomFloat();
		DesiredMovementDirection.y = GetRandomFloat(2.0f) - 1.0f;
		break;
	case EDirection::NONE:
		DesiredMovementDirection.x = GetRandomFloat(2.0f) - 1.0f;
		DesiredMovementDirection.y = GetRandomFloat(2.0f) - 1.0f;
		break;
	}

	NormalizeVector2f(DesiredMovementDirection);
}
<commit_msg>Actor's movement speed reverted to 100.0f.<commit_after>// ----------------------------------------------------------------------------
// ------------- AI Battleground, Copyright(C) Maciej Pryc, 2016 --------------
// ----------------------------------------------------------------------------

#define _USE_MATH_DEFINES
#include <math.h>
#include <iostream>

#include "Actor.h"
#include "TextureManager.h"
#include "LevelInfo.h"
#include "AISystemFSM.h"
#include "AISystemBT.h"

Actor::Actor(class LevelInfo* argLevelInfo, TextureManager* TexManager, const std::string& TexName, const ETeam argTeam, const sf::Vector2f& InitialPosition) :
LevelInfo(argLevelInfo), AISystem(nullptr), NearestEnemy(nullptr), LastQuadTreePosition(InitialPosition), Position(InitialPosition), DesiredMovementDirection(0.0f, 0.0f),
ActualMovementDirection(0.0f, 0.0f), VectorTowardsEnemy(0.0f, 0.0f), ShotDist(75.0f * (1.0f - GetRandomFloat(0.4f))), MovementSpeed(100.0f), DirectionChangeSpeed(5.0f),
MaxHP(100.0f), HP(MaxHP), Damage(10.0f), Team(argTeam), MovementDirectionInterpStart(0.0f, 0.0f), bInterpolateMovementDirection(false),
MovementDirectionInterpAlpha(0.0f), bShouldDrawLaser(false), ShotInterval(sf::seconds(0.5f)), ShotTimeCounter(ShotInterval),
QuadTreeUpdateInterval(sf::seconds(0.2f)), QuadTreeUpdateCounter(sf::seconds(GetRandomFloat(QuadTreeUpdateInterval.asSeconds())))
{
	for (int i = 0; i < ROBOT_SPRITES_AMOUNT; ++i)
	{
		sf::Vector2u TexSize = TexManager->InitTexture(&RobotSprite[i], TexName + std::to_string(i + 1));

		if (i == 0)
			Size = TexSize;
		else if (TexSize != Size)
			std::cout << "Actor construction: sprite textures in different sizes! " << TexName << " " << i << std::endl;

		RobotSprite[i].setOrigin(Size.x / 2.0f, Size.y / 2.0f);
	}

	BeamTexSize = TexManager->InitTexture(&LaserBeamSprite, "LaserBeam");
	LaserBeamSprite.setOrigin(0.0f, BeamTexSize.y / 2.0f);

	sf::Vector2u BurstTexSize = TexManager->InitTexture(&LaserBurstSprite, "LaserBurst");
	LaserBurstSprite.setOrigin(BurstTexSize.x / 2.0f, BurstTexSize.y / 2.0f);

	MovementDirectionOffset.x = GetRandomFloat(ShotDist * 1.5f) - ShotDist * 0.75f;
	MovementDirectionOffset.y = GetRandomFloat(ShotDist * 1.5f) - ShotDist * 0.75f;

	//AISystem = new AISystemFSM(this, &Blackboard);
	AISystem = new AISystemBT(this, &Blackboard);

	Blackboard.SetMaxHP(MaxHP);
	Blackboard.SetHP(HP);
}

Actor::~Actor()
{
	delete AISystem;
}

void Actor::SyncDrawData()
{
	DrawData_Position = Position;
	DrawData_HP = HP;
	DrawData_bShouldDrawLaser = bShouldDrawLaser;
	DrawData_VectorTowardsEnemy = VectorTowardsEnemy;
}

void Actor::DrawRobot(sf::RenderWindow* Window) const
{
	DrawLaserBurst(Window);

	sf::Transform CurrTransform;
	CurrTransform.translate(DrawData_Position);

	Window->draw(GetRobotSprite(), CurrTransform);
}

const sf::Sprite& Actor::GetRobotSprite() const
{
	const float HPRatio = DrawData_HP / MaxHP;
	return RobotSprite[(int)((1.0f - HPRatio) * ROBOT_SPRITES_AMOUNT)];
}

void Actor::DrawLaserBeam(sf::RenderWindow* Window) const
{
	if (DrawData_bShouldDrawLaser)
	{
		sf::Vector2f Diff = DrawData_VectorTowardsEnemy;
		NormalizeVector2f(Diff);
		DrawData_AngleToEnemy = acos(Diff.x);
		DrawData_AngleToEnemy *= (float)M_1_PI * 180.0f;

		if (Diff.y < 0.0f)
			DrawData_AngleToEnemy = 360.0f - DrawData_AngleToEnemy;

		sf::Transform CurrTransform;
		CurrTransform.translate(DrawData_Position);
		CurrTransform.rotate(DrawData_AngleToEnemy);
		CurrTransform.scale(GetLength(DrawData_VectorTowardsEnemy) / BeamTexSize.x, 1.0f);

		Window->draw(LaserBeamSprite, CurrTransform);
	}
}

void Actor::DrawLaserBurst(sf::RenderWindow* Window) const
{
	if (DrawData_bShouldDrawLaser)
	{
		sf::Transform CurrTransform;
		CurrTransform.translate(DrawData_Position);
		CurrTransform.rotate(DrawData_AngleToEnemy);
		CurrTransform.translate(sf::Vector2f(Size.x / 2.0f + 1.0f, 0.0f));
		Window->draw(LaserBurstSprite, CurrTransform);
	}
}

void Actor::Update(const float DeltaTime)
{
	bShouldDrawLaser = false;
	ShotTimeCounter += sf::seconds(DeltaTime);

	ProcessMovement(DeltaTime);
	UpdatePositionInQuadTree(DeltaTime);

	const bool bReached = GetSquaredDist(LevelInfo->GetHealZonePosition(Team) + MovementDirectionOffset, GetPosition()) < 100.0f;
	Blackboard.SetBHealthZoneDestReached(bReached);

	if (NearestEnemy)
	{
		VectorTowardsEnemy = NearestEnemy->GetPosition() - GetPosition();
		Blackboard.SetBEnemyInRange(GetLength(VectorTowardsEnemy) <= ShotDist);
	}
}

void Actor::UpdateAISystem()
{
	AISystem->Update();
}

void Actor::ProcessMovement(const float DeltaTime)
{
	if (bInterpolateMovementDirection)
	{
		if (MovementDirectionInterpAlpha >= 1.0f)
		{
			ActualMovementDirection = DesiredMovementDirection;
			bInterpolateMovementDirection = false;
		}
		else
		{
			MovementDirectionInterpAlpha += DeltaTime * DirectionChangeSpeed;
			ActualMovementDirection = (1.0f - MovementDirectionInterpAlpha) * MovementDirectionInterpStart + MovementDirectionInterpAlpha * DesiredMovementDirection;
		}
	}

	if (ActualMovementDirection != sf::Vector2f(0.0f, 0.0f))
	{
		Position += ActualMovementDirection * MovementSpeed * DeltaTime;
		ClampVector2f(Position, LevelInfo->Boundaries);
	}
}

void Actor::UpdatePositionInQuadTree(const float DeltaTime)
{
	QuadTreeUpdateCounter += sf::seconds(DeltaTime);

	if (QuadTreeUpdateCounter >= QuadTreeUpdateInterval)
	{
		LevelInfo->UpdatePositionInQuadTree(this);
		QuadTreeUpdateCounter -= QuadTreeUpdateInterval;
	}
}

void Actor::RetreatToHealZone()
{
	sf::Vector2f NewDesiredMovementDirection = LevelInfo->GetHealZonePosition(Team) - GetPosition() + MovementDirectionOffset;
	NormalizeVector2f(NewDesiredMovementDirection);
	SetDesiredMovementDirection(NewDesiredMovementDirection);
}

void Actor::StopMovement()
{
	if (DesiredMovementDirection != sf::Vector2f(0.0f, 0.0f))
		SetDesiredMovementDirection(sf::Vector2f(0.0f, 0.0f));
}

void Actor::GoTowardsNearestEnemy()
{
	sf::Vector2f NewDesiredMovementDirection = VectorTowardsEnemy + MovementDirectionOffset;
	NormalizeVector2f(NewDesiredMovementDirection);
	SetDesiredMovementDirection(NewDesiredMovementDirection);
}

void Actor::SetDesiredMovementDirection(const sf::Vector2f& NewDesiredMovementDireciton)
{
	DesiredMovementDirection = NewDesiredMovementDireciton;
	MovementDirectionInterpStart = ActualMovementDirection;
	MovementDirectionInterpAlpha = 0.0f;
	bInterpolateMovementDirection = true;
}

const sf::Vector2f& Actor::GetPosition() const
{
	return Position;
}

ETeam Actor::GetTeam() const
{
	return Team;
}

void Actor::SetNearestEnemy(Actor* NewNearestEnemy)
{
	NearestEnemy = NewNearestEnemy;
	Blackboard.SetNearestEnemy(NearestEnemy);
}

Actor* Actor::GetNearestEnemy() const
{
	return NearestEnemy;
}

void Actor::UpdateLastQuadTreePosition(const sf::Vector2f& NewPositionInQuadTree)
{
	LastQuadTreePosition = NewPositionInQuadTree;
}

const sf::Vector2f& Actor::GetLastQuadTreePosition() const
{
	return LastQuadTreePosition;
}

void Actor::Heal(float HPToHeal)
{
	HP = std::min(HP + HPToHeal, MaxHP);
	Blackboard.SetHP(HP);
}

void Actor::TryToShoot()
{
	if (ShotTimeCounter >= ShotInterval)
	{
		bShouldDrawLaser = true;
		NearestEnemy->TakeDamage(Damage);
		ShotTimeCounter = sf::Time::Zero;
	}
}

void Actor::TakeDamage(float DamageAmount)
{
	HP -= DamageAmount;
	Blackboard.SetHP(HP);

	if (HP <= 0.0f)
		LevelInfo->DestroyActor(this);
}

void Actor::GenerateRandomMovementDirection(EDirection DirectionToAvoid)
{
	switch (DirectionToAvoid)
	{
	case EDirection::UP:
		DesiredMovementDirection.x = GetRandomFloat(2.0f) - 1.0f;
		DesiredMovementDirection.y = GetRandomFloat();
		break;
	case EDirection::RIGHT:
		DesiredMovementDirection.x = GetRandomFloat() - 1.0f;
		DesiredMovementDirection.y = GetRandomFloat(2.0f) - 1.0f;
		break;
	case EDirection::DOWN:
		DesiredMovementDirection.x = GetRandomFloat(2.0f) - 1.0f;
		DesiredMovementDirection.y = GetRandomFloat() - 1.0f;
		break;
	case EDirection::LEFT:
		DesiredMovementDirection.x = GetRandomFloat();
		DesiredMovementDirection.y = GetRandomFloat(2.0f) - 1.0f;
		break;
	case EDirection::NONE:
		DesiredMovementDirection.x = GetRandomFloat(2.0f) - 1.0f;
		DesiredMovementDirection.y = GetRandomFloat(2.0f) - 1.0f;
		break;
	}

	NormalizeVector2f(DesiredMovementDirection);
}
<|endoftext|>


<commit_before>/**
 * \file TypedBaseFilter.hxx
 */

#include <ATK/Core/TypedBaseFilter.h>
#include <ATK/Core/Utilities.h>

#include <complex>
#include <cstdint>
#include <iostream>
#include <type_traits>

#include <boost/mpl/contains.hpp>
#include <boost/mpl/distance.hpp>
#include <boost/mpl/empty.hpp>
#include <boost/mpl/find.hpp>
#include <boost/mpl/front.hpp>
#include <boost/mpl/pop_front.hpp>
#include <boost/mpl/vector.hpp>
#include <boost/type_traits.hpp>
#include <boost/utility/enable_if.hpp>

#if USE_SIMD
#include <simdpp/simd.h>
#endif

namespace ATK
{
namespace Utilities
{
  typedef boost::mpl::vector<std::int16_t, std::int32_t, int64_t, float, double, std::complex<float>, std::complex<double> > ConversionTypes;

  template<typename Vector, typename DataType>
  typename boost::enable_if<typename boost::mpl::empty<Vector>::type, void>::type
    convert_scalar_array(ATK::BaseFilter* filter, unsigned int port, DataType* converted_input, gsl::index size, int type)
  {
    throw RuntimeError("Cannot convert types for these filters");
  }

  template<typename Vector, typename DataType>
  typename boost::disable_if<typename boost::is_arithmetic<typename boost::mpl::front<Vector>::type>::type, typename boost::disable_if<typename boost::mpl::empty<Vector>::type, void>::type>::type
  convert_scalar_array(ATK::BaseFilter* filter, unsigned int port, DataType* converted_input, gsl::index size, int type)
  {
    convert_scalar_array<typename boost::mpl::pop_front<Vector>::type, DataType>(filter, port, converted_input, size, type - 1);
  }

  template<typename Vector, typename DataType>
  typename boost::enable_if<typename boost::is_arithmetic<typename boost::mpl::front<Vector>::type>::type, typename boost::disable_if<typename boost::mpl::empty<Vector>::type, void>::type>::type
    convert_scalar_array(ATK::BaseFilter* filter, unsigned int port, DataType* converted_input, gsl::index size, int type)
  {
    if(type != 0)
    {
      convert_scalar_array<typename boost::mpl::pop_front<Vector>::type, DataType>(filter, port, converted_input, size, type - 1);
    }
    else
    {
      typedef typename boost::mpl::front<Vector>::type InputOriginalType;
      InputOriginalType* original_input_array = static_cast<ATK::TypedBaseFilter<InputOriginalType>*>(filter)->get_output_array(port);
      ATK::ConversionUtilities<InputOriginalType, DataType>::convert_array(original_input_array, converted_input, size);
    }
  }

  template<typename Vector, typename DataType>
  typename boost::enable_if<typename boost::mpl::empty<Vector>::type, void>::type
    convert_complex_array(ATK::BaseFilter* filter, unsigned int port, DataType* converted_input, gsl::index size, int type)
  {
    throw RuntimeError("Can't convert types");
  }

  template<typename Vector, typename DataType>
  typename boost::disable_if<typename boost::mpl::empty<Vector>::type, void>::type
    convert_complex_array(ATK::BaseFilter* filter, unsigned int port, DataType* converted_input, gsl::index size, int type)
  {
    assert(type >= 0);
    if (type != 0)
    {
      convert_complex_array<typename boost::mpl::pop_front<Vector>::type, DataType>(filter, port, converted_input, size, type - 1);
    }
    else
    {
      typedef typename boost::mpl::front<Vector>::type InputOriginalType;
      InputOriginalType* original_input_array = static_cast<ATK::TypedBaseFilter<InputOriginalType>*>(filter)->get_output_array(port);
      ATK::ConversionUtilities<InputOriginalType, DataType>::convert_array(original_input_array, converted_input, size);
    }
  }

  /// Conversion function for arithmetic types
  template<typename Vector, typename DataType>
  typename boost::enable_if<typename boost::is_arithmetic<DataType>::type, void>::type convert_array(ATK::BaseFilter* filter, unsigned int port, DataType* converted_input, gsl::index size, int type)
  {
    convert_scalar_array<Vector, DataType>(filter, port, converted_input, size, type);
  }

  /// Conversion function for other types not contained in ConversionTypes (no conversion in that case, just copy)
  template<typename Vector, typename DataType>
  typename boost::disable_if<typename boost::is_arithmetic<DataType>::type, typename boost::disable_if<typename boost::mpl::contains<Vector, DataType>::type, void>::type>::type convert_array(ATK::BaseFilter* filter, unsigned int port, DataType* converted_input, gsl::index size, int type)
  {
    assert(dynamic_cast<ATK::OutputArrayInterface<DataType>*>(filter));
     // For SIMD, you shouldn't call this, but adapt input/output delays so that there is no copy from one filter to another.
    DataType* original_input_array = dynamic_cast<ATK::TypedBaseFilter<DataType>*>(filter)->get_output_array(port);
    ATK::ConversionUtilities<DataType, DataType>::convert_array(original_input_array, converted_input, size);
  }

  /// Conversion function for std::complex contained in ConversionTypes
  template<typename Vector, typename DataType>
  typename boost::disable_if<typename boost::is_arithmetic<DataType>::type, typename boost::enable_if<typename boost::mpl::contains<Vector, DataType>::type, void>::type>::type convert_array(ATK::BaseFilter* filter, unsigned int port, DataType* converted_input, gsl::index size, int type)
  {
    convert_complex_array<Vector, DataType>(filter, port, converted_input, size, type);
  }

  template<typename Vector, typename DataType>
  typename boost::enable_if<typename boost::mpl::contains<Vector, DataType>::type, int>::type get_type()
  {
    return boost::mpl::distance<boost::mpl::begin<ConversionTypes>::type, typename boost::mpl::find<ConversionTypes, DataType>::type >::value;
  }

  template<typename Vector, typename DataType>
  typename boost::disable_if<typename boost::mpl::contains<Vector, DataType>::type, int>::type get_type()
  {
    return -1;
  }
}

  template<typename DataType>
  OutputArrayInterface<DataType>::~OutputArrayInterface()
  {
  }

  template<typename DataType_, typename DataType__>
  TypedBaseFilter<DataType_, DataType__>::TypedBaseFilter(gsl::index nb_input_ports, gsl::index nb_output_ports)
  :Parent(nb_input_ports, nb_output_ports), converted_inputs_delay(nb_input_ports), converted_inputs(nb_input_ports, nullptr), converted_inputs_size(nb_input_ports, 0), converted_in_delays(nb_input_ports, 0), direct_filters(nb_input_ports, nullptr), outputs_delay(nb_output_ports), outputs(nb_output_ports, nullptr), outputs_size(nb_output_ports, 0), out_delays(nb_output_ports, 0), default_input(nb_input_ports, TypeTraits<DataType_>::Zero()), default_output(nb_output_ports, TypeTraits<DataType__>::Zero())
  {
  }

  template<typename DataType_, typename DataType__>
  TypedBaseFilter<DataType_, DataType__>::TypedBaseFilter(TypedBaseFilter&& other)
  : Parent(std::move(other)), converted_inputs_delay(std::move(other.converted_inputs_delay)), converted_inputs(std::move(other.converted_inputs)), converted_inputs_size(std::move(other.converted_inputs_size)), converted_in_delays(std::move(other.converted_in_delays)), direct_filters(std::move(other.direct_filters)), outputs_delay(std::move(other.outputs_delay)), outputs(std::move(other.outputs)), outputs_size(std::move(other.outputs_size)), default_input(std::move(other.default_input)), default_output(std::move(other.default_output))
  {
  }

  template<typename DataType_, typename DataType__>
  TypedBaseFilter<DataType_, DataType__>::~TypedBaseFilter()
  {
  }

  template<typename DataType_, typename DataType__>
  void TypedBaseFilter<DataType_, DataType__>::set_nb_input_ports(gsl::index nb_ports)
  {
    if(nb_ports == nb_input_ports)
      return;
    Parent::set_nb_input_ports(nb_ports);
    converted_inputs_delay = std::vector<AlignedVector>(nb_ports);
    converted_inputs.assign(nb_ports, nullptr);
    converted_inputs_size.assign(nb_ports, 0);
    converted_in_delays.assign(nb_ports, 0);
    direct_filters.assign(nb_ports, nullptr);
    default_input.assign(nb_ports, TypeTraits<DataTypeInput>::Zero());
  }

  template<typename DataType_, typename DataType__>
  void TypedBaseFilter<DataType_, DataType__>::set_nb_output_ports(gsl::index nb_ports)
  {
    if(nb_ports == nb_output_ports)
      return;
    Parent::set_nb_output_ports(nb_ports);
    outputs_delay = std::vector<AlignedOutVector>(nb_ports);
    outputs.assign(nb_ports, nullptr);
    outputs_size.assign(nb_ports, 0);
    out_delays.assign(nb_ports, 0);
    default_output.assign(nb_ports, TypeTraits<DataTypeOutput>::Zero());
  }

  template<typename DataType_, typename DataType__>
  void TypedBaseFilter<DataType_, DataType__>::process_impl(gsl::index size) const
  {
    // Nothing to do by default
  }

  template<typename DataType_, typename DataType__>
  void TypedBaseFilter<DataType_, DataType__>::prepare_process(gsl::index size)
  {
    convert_inputs(size);
  }

  template<typename DataType_, typename DataType__>
  int TypedBaseFilter<DataType_, DataType__>::get_type() const
  {
    return Utilities::get_type<Utilities::ConversionTypes, DataType__>();
  }

  template<typename DataType_, typename DataType__>
  DataType__* TypedBaseFilter<DataType_, DataType__>::get_output_array(gsl::index port) const
  {
    return outputs[port];
  }

  template<typename DataType_, typename DataType__>
  gsl::index TypedBaseFilter<DataType_, DataType__>::get_output_array_size() const
  {
    return outputs_size.front();
  }

  template<typename DataType_, typename DataType__>
  void TypedBaseFilter<DataType_, DataType__>::convert_inputs(gsl::index size)
  {
    for(gsl::index i = 0; i < nb_input_ports; ++i)
    {
      // if the input delay is smaller than the preceding filter output delay, we may have overlap
      // if the types are identical and if the type is not -1 (an unknown type)
      // if we have overlap, don't copy anything at all
      if((input_delay <= connections[i].second->get_output_delay()) && (direct_filters[i] != nullptr))
      {
        converted_inputs[i] = direct_filters[i]->get_output_array(connections[i].first);
        converted_inputs_size[i] = size;
        converted_in_delays[i] = input_delay;
        continue;
      }
      auto input_size = converted_inputs_size[i];
      auto in_delay = converted_in_delays[i];
      if(input_size < size || in_delay < input_delay)
      {
        // TODO Properly align the beginning of the data, not depending on input delay
        AlignedVector temp(input_delay + size, TypeTraits<DataTypeInput>::Zero());
        if(input_size == 0)
        {
          for(unsigned int j = 0; j < input_delay; ++j)
          {
            temp[j] = default_input[i];
          }
        }
        else
        {
          const auto input_ptr = converted_inputs[i];
          for(gsl::index j = 0; j < in_delay; ++j)
          {
            temp[j] = input_ptr[last_size + j - in_delay];
          }
        }

        converted_inputs_delay[i] = std::move(temp);
        converted_inputs[i] = converted_inputs_delay[i].data() + input_delay;
        converted_inputs_size[i] = size;
        converted_in_delays[i] = input_delay;
      }
      else
      {
        auto my_last_size = static_cast<int64_t>(last_size) * input_sampling_rate / output_sampling_rate;
        const auto input_ptr = converted_inputs[i];
        for(gsl::index j = 0; j < input_delay; ++j)
        {
          input_ptr[j - input_delay] = input_ptr[my_last_size + j - input_delay];
        }
      }
      Utilities::convert_array<Utilities::ConversionTypes, DataTypeInput>(connections[i].second, connections[i].first, converted_inputs[i], size, connections[i].second->get_type());
    }
  }

  template<typename DataType_, typename DataType__>
  void TypedBaseFilter<DataType_, DataType__>::prepare_outputs(gsl::index size)
  {
    for(gsl::index i = 0; i < nb_output_ports; ++i)
    {
      auto output_size = outputs_size[i];
      auto out_delay = out_delays[i];
      if(output_size < size || out_delay < output_delay)
      {
        // TODO Properly align the beginning of the data, not depending on output delay
        AlignedOutVector temp(output_delay + size, TypeTraits<DataTypeOutput>::Zero());
        if(output_size == 0)
        {
          for(gsl::index j = 0; j < output_delay; ++j)
          {
            temp[j] = default_output[i];
          }
        }
        else
        {
          const auto output_ptr = outputs[i];
          for(gsl::index j = 0; j < static_cast<int>(out_delay); ++j)
          {
            temp[j] = output_ptr[last_size + j - out_delay];
          }
        }

        outputs_delay[i] = std::move(temp);
        outputs[i] = outputs_delay[i].data() + output_delay;
        outputs_size[i] = size;
        out_delays[i] = output_delay;
      }
      else
      {
        const auto output_ptr = outputs[i];
        for(gsl::index j = 0; j < static_cast<int>(output_delay); ++j)
        {
          output_ptr[j - output_delay] = output_ptr[last_size + j - output_delay];
        }
      }
    }
  }

  template<typename DataType_, typename DataType__>
  void TypedBaseFilter<DataType_, DataType__>::full_setup()
  {
    // Reset input arrays
    converted_inputs_delay = std::vector<AlignedVector>(nb_input_ports);
    converted_inputs.assign(nb_input_ports, nullptr);
    converted_inputs_size.assign(nb_input_ports, 0);
    converted_in_delays.assign(nb_input_ports, 0);

    // Reset output arrays
    outputs_delay = std::vector<AlignedOutVector>(nb_output_ports);
    outputs.assign(nb_output_ports, nullptr);
    outputs_size.assign(nb_output_ports, 0);
    out_delays.assign(nb_output_ports, 0);

    Parent::full_setup();
  }

  template<typename DataType_, typename DataType__>
  void TypedBaseFilter<DataType_, DataType__>::set_input_port(gsl::index input_port, gsl::not_null<BaseFilter*> filter, gsl::index output_port)
  {
    set_input_port(input_port, *filter, output_port);
  }

  template<typename DataType_, typename DataType__>
  void TypedBaseFilter<DataType_, DataType__>::set_input_port(gsl::index input_port, BaseFilter& filter, gsl::index output_port)
  {
    Parent::set_input_port(input_port, filter, output_port);
    converted_inputs_size[input_port] = 0;
    converted_in_delays[input_port] = 0;
    direct_filters[input_port] = dynamic_cast<OutputArrayInterface<DataType_>*>(&filter);
  }
}
<commit_msg>Start using C++17 constexpr and std type_traits<commit_after>/**
 * \file TypedBaseFilter.hxx
 */

#include <ATK/Core/TypedBaseFilter.h>
#include <ATK/Core/Utilities.h>

#include <complex>
#include <cstdint>
#include <iostream>
#include <type_traits>

#include <boost/mpl/contains.hpp>
#include <boost/mpl/distance.hpp>
#include <boost/mpl/empty.hpp>
#include <boost/mpl/find.hpp>
#include <boost/mpl/front.hpp>
#include <boost/mpl/pop_front.hpp>
#include <boost/mpl/vector.hpp>
#include <boost/type_traits.hpp>
#include <boost/utility/enable_if.hpp>

#if USE_SIMD
#include <simdpp/simd.h>
#endif

namespace ATK
{
namespace Utilities
{
  typedef boost::mpl::vector<std::int16_t, std::int32_t, int64_t, float, double, std::complex<float>, std::complex<double> > ConversionTypes;

  template<typename Vector, typename DataType>
  typename boost::enable_if<typename boost::mpl::empty<Vector>::type, void>::type
    convert_scalar_array(ATK::BaseFilter* filter, unsigned int port, DataType* converted_input, gsl::index size, int type)
  {
    throw RuntimeError("Cannot convert types for these filters");
  }

  template<typename Vector, typename DataType>
  typename boost::disable_if<typename std::is_arithmetic<typename boost::mpl::front<Vector>::type>::type, typename boost::disable_if<typename boost::mpl::empty<Vector>::type, void>::type>::type
  convert_scalar_array(ATK::BaseFilter* filter, unsigned int port, DataType* converted_input, gsl::index size, int type)
  {
    convert_scalar_array<typename boost::mpl::pop_front<Vector>::type, DataType>(filter, port, converted_input, size, type - 1);
  }

  template<typename Vector, typename DataType>
  typename boost::enable_if<typename std::is_arithmetic<typename boost::mpl::front<Vector>::type>::type, typename boost::disable_if<typename boost::mpl::empty<Vector>::type, void>::type>::type
    convert_scalar_array(ATK::BaseFilter* filter, unsigned int port, DataType* converted_input, gsl::index size, int type)
  {
    if(type != 0)
    {
      convert_scalar_array<typename boost::mpl::pop_front<Vector>::type, DataType>(filter, port, converted_input, size, type - 1);
    }
    else
    {
      typedef typename boost::mpl::front<Vector>::type InputOriginalType;
      InputOriginalType* original_input_array = static_cast<ATK::TypedBaseFilter<InputOriginalType>*>(filter)->get_output_array(port);
      ATK::ConversionUtilities<InputOriginalType, DataType>::convert_array(original_input_array, converted_input, size);
    }
  }

  template<typename Vector, typename DataType>
  void convert_complex_array(ATK::BaseFilter* filter, unsigned int port, DataType* converted_input, gsl::index size, int type)
  {
    if constexpr(boost::mpl::empty<Vector>::value)
    {
      throw RuntimeError("Can't convert types");
    }
    else
    {
      assert(type >= 0);
      if (type != 0)
      {
        convert_complex_array<typename boost::mpl::pop_front<Vector>::type, DataType>(filter, port, converted_input, size, type - 1);
      }
      else
      {
        typedef typename boost::mpl::front<Vector>::type InputOriginalType;
        InputOriginalType* original_input_array = static_cast<ATK::TypedBaseFilter<InputOriginalType>*>(filter)->get_output_array(port);
        ATK::ConversionUtilities<InputOriginalType, DataType>::convert_array(original_input_array, converted_input, size);
      }
    }
  }

  /// Conversion function for arithmetic types
  template<typename Vector, typename DataType>
  typename boost::enable_if<typename boost::is_arithmetic<DataType>::type, void>::type convert_array(ATK::BaseFilter* filter, unsigned int port, DataType* converted_input, gsl::index size, int type)
  {
    convert_scalar_array<Vector, DataType>(filter, port, converted_input, size, type);
  }

  /// Conversion function for other types not contained in ConversionTypes (no conversion in that case, just copy)
  template<typename Vector, typename DataType>
  typename boost::disable_if<typename boost::is_arithmetic<DataType>::type, typename boost::disable_if<typename boost::mpl::contains<Vector, DataType>::type, void>::type>::type convert_array(ATK::BaseFilter* filter, unsigned int port, DataType* converted_input, gsl::index size, int type)
  {
    assert(dynamic_cast<ATK::OutputArrayInterface<DataType>*>(filter));
     // For SIMD, you shouldn't call this, but adapt input/output delays so that there is no copy from one filter to another.
    DataType* original_input_array = dynamic_cast<ATK::TypedBaseFilter<DataType>*>(filter)->get_output_array(port);
    ATK::ConversionUtilities<DataType, DataType>::convert_array(original_input_array, converted_input, size);
  }

  /// Conversion function for std::complex contained in ConversionTypes
  template<typename Vector, typename DataType>
  typename boost::disable_if<typename boost::is_arithmetic<DataType>::type, typename boost::enable_if<typename boost::mpl::contains<Vector, DataType>::type, void>::type>::type convert_array(ATK::BaseFilter* filter, unsigned int port, DataType* converted_input, gsl::index size, int type)
  {
    convert_complex_array<Vector, DataType>(filter, port, converted_input, size, type);
  }

  template<typename Vector, typename DataType>
  int get_type()
  {
    if constexpr(boost::mpl::contains<Vector, DataType>::value)
    {
      return boost::mpl::distance<boost::mpl::begin<ConversionTypes>::type, typename boost::mpl::find<ConversionTypes, DataType>::type >::value;
    }
    else
    {
      return -1;
    }
  }
}

  template<typename DataType>
  OutputArrayInterface<DataType>::~OutputArrayInterface()
  {
  }

  template<typename DataType_, typename DataType__>
  TypedBaseFilter<DataType_, DataType__>::TypedBaseFilter(gsl::index nb_input_ports, gsl::index nb_output_ports)
  :Parent(nb_input_ports, nb_output_ports), converted_inputs_delay(nb_input_ports), converted_inputs(nb_input_ports, nullptr), converted_inputs_size(nb_input_ports, 0), converted_in_delays(nb_input_ports, 0), direct_filters(nb_input_ports, nullptr), outputs_delay(nb_output_ports), outputs(nb_output_ports, nullptr), outputs_size(nb_output_ports, 0), out_delays(nb_output_ports, 0), default_input(nb_input_ports, TypeTraits<DataType_>::Zero()), default_output(nb_output_ports, TypeTraits<DataType__>::Zero())
  {
  }

  template<typename DataType_, typename DataType__>
  TypedBaseFilter<DataType_, DataType__>::TypedBaseFilter(TypedBaseFilter&& other)
  : Parent(std::move(other)), converted_inputs_delay(std::move(other.converted_inputs_delay)), converted_inputs(std::move(other.converted_inputs)), converted_inputs_size(std::move(other.converted_inputs_size)), converted_in_delays(std::move(other.converted_in_delays)), direct_filters(std::move(other.direct_filters)), outputs_delay(std::move(other.outputs_delay)), outputs(std::move(other.outputs)), outputs_size(std::move(other.outputs_size)), default_input(std::move(other.default_input)), default_output(std::move(other.default_output))
  {
  }

  template<typename DataType_, typename DataType__>
  TypedBaseFilter<DataType_, DataType__>::~TypedBaseFilter()
  {
  }

  template<typename DataType_, typename DataType__>
  void TypedBaseFilter<DataType_, DataType__>::set_nb_input_ports(gsl::index nb_ports)
  {
    if(nb_ports == nb_input_ports)
      return;
    Parent::set_nb_input_ports(nb_ports);
    converted_inputs_delay = std::vector<AlignedVector>(nb_ports);
    converted_inputs.assign(nb_ports, nullptr);
    converted_inputs_size.assign(nb_ports, 0);
    converted_in_delays.assign(nb_ports, 0);
    direct_filters.assign(nb_ports, nullptr);
    default_input.assign(nb_ports, TypeTraits<DataTypeInput>::Zero());
  }

  template<typename DataType_, typename DataType__>
  void TypedBaseFilter<DataType_, DataType__>::set_nb_output_ports(gsl::index nb_ports)
  {
    if(nb_ports == nb_output_ports)
      return;
    Parent::set_nb_output_ports(nb_ports);
    outputs_delay = std::vector<AlignedOutVector>(nb_ports);
    outputs.assign(nb_ports, nullptr);
    outputs_size.assign(nb_ports, 0);
    out_delays.assign(nb_ports, 0);
    default_output.assign(nb_ports, TypeTraits<DataTypeOutput>::Zero());
  }

  template<typename DataType_, typename DataType__>
  void TypedBaseFilter<DataType_, DataType__>::process_impl(gsl::index size) const
  {
    // Nothing to do by default
  }

  template<typename DataType_, typename DataType__>
  void TypedBaseFilter<DataType_, DataType__>::prepare_process(gsl::index size)
  {
    convert_inputs(size);
  }

  template<typename DataType_, typename DataType__>
  int TypedBaseFilter<DataType_, DataType__>::get_type() const
  {
    return Utilities::get_type<Utilities::ConversionTypes, DataType__>();
  }

  template<typename DataType_, typename DataType__>
  DataType__* TypedBaseFilter<DataType_, DataType__>::get_output_array(gsl::index port) const
  {
    return outputs[port];
  }

  template<typename DataType_, typename DataType__>
  gsl::index TypedBaseFilter<DataType_, DataType__>::get_output_array_size() const
  {
    return outputs_size.front();
  }

  template<typename DataType_, typename DataType__>
  void TypedBaseFilter<DataType_, DataType__>::convert_inputs(gsl::index size)
  {
    for(gsl::index i = 0; i < nb_input_ports; ++i)
    {
      // if the input delay is smaller than the preceding filter output delay, we may have overlap
      // if the types are identical and if the type is not -1 (an unknown type)
      // if we have overlap, don't copy anything at all
      if((input_delay <= connections[i].second->get_output_delay()) && (direct_filters[i] != nullptr))
      {
        converted_inputs[i] = direct_filters[i]->get_output_array(connections[i].first);
        converted_inputs_size[i] = size;
        converted_in_delays[i] = input_delay;
        continue;
      }
      auto input_size = converted_inputs_size[i];
      auto in_delay = converted_in_delays[i];
      if(input_size < size || in_delay < input_delay)
      {
        // TODO Properly align the beginning of the data, not depending on input delay
        AlignedVector temp(input_delay + size, TypeTraits<DataTypeInput>::Zero());
        if(input_size == 0)
        {
          for(unsigned int j = 0; j < input_delay; ++j)
          {
            temp[j] = default_input[i];
          }
        }
        else
        {
          const auto input_ptr = converted_inputs[i];
          for(gsl::index j = 0; j < in_delay; ++j)
          {
            temp[j] = input_ptr[last_size + j - in_delay];
          }
        }

        converted_inputs_delay[i] = std::move(temp);
        converted_inputs[i] = converted_inputs_delay[i].data() + input_delay;
        converted_inputs_size[i] = size;
        converted_in_delays[i] = input_delay;
      }
      else
      {
        auto my_last_size = static_cast<int64_t>(last_size) * input_sampling_rate / output_sampling_rate;
        const auto input_ptr = converted_inputs[i];
        for(gsl::index j = 0; j < input_delay; ++j)
        {
          input_ptr[j - input_delay] = input_ptr[my_last_size + j - input_delay];
        }
      }
      Utilities::convert_array<Utilities::ConversionTypes, DataTypeInput>(connections[i].second, connections[i].first, converted_inputs[i], size, connections[i].second->get_type());
    }
  }

  template<typename DataType_, typename DataType__>
  void TypedBaseFilter<DataType_, DataType__>::prepare_outputs(gsl::index size)
  {
    for(gsl::index i = 0; i < nb_output_ports; ++i)
    {
      auto output_size = outputs_size[i];
      auto out_delay = out_delays[i];
      if(output_size < size || out_delay < output_delay)
      {
        // TODO Properly align the beginning of the data, not depending on output delay
        AlignedOutVector temp(output_delay + size, TypeTraits<DataTypeOutput>::Zero());
        if(output_size == 0)
        {
          for(gsl::index j = 0; j < output_delay; ++j)
          {
            temp[j] = default_output[i];
          }
        }
        else
        {
          const auto output_ptr = outputs[i];
          for(gsl::index j = 0; j < static_cast<int>(out_delay); ++j)
          {
            temp[j] = output_ptr[last_size + j - out_delay];
          }
        }

        outputs_delay[i] = std::move(temp);
        outputs[i] = outputs_delay[i].data() + output_delay;
        outputs_size[i] = size;
        out_delays[i] = output_delay;
      }
      else
      {
        const auto output_ptr = outputs[i];
        for(gsl::index j = 0; j < static_cast<int>(output_delay); ++j)
        {
          output_ptr[j - output_delay] = output_ptr[last_size + j - output_delay];
        }
      }
    }
  }

  template<typename DataType_, typename DataType__>
  void TypedBaseFilter<DataType_, DataType__>::full_setup()
  {
    // Reset input arrays
    converted_inputs_delay = std::vector<AlignedVector>(nb_input_ports);
    converted_inputs.assign(nb_input_ports, nullptr);
    converted_inputs_size.assign(nb_input_ports, 0);
    converted_in_delays.assign(nb_input_ports, 0);

    // Reset output arrays
    outputs_delay = std::vector<AlignedOutVector>(nb_output_ports);
    outputs.assign(nb_output_ports, nullptr);
    outputs_size.assign(nb_output_ports, 0);
    out_delays.assign(nb_output_ports, 0);

    Parent::full_setup();
  }

  template<typename DataType_, typename DataType__>
  void TypedBaseFilter<DataType_, DataType__>::set_input_port(gsl::index input_port, gsl::not_null<BaseFilter*> filter, gsl::index output_port)
  {
    set_input_port(input_port, *filter, output_port);
  }

  template<typename DataType_, typename DataType__>
  void TypedBaseFilter<DataType_, DataType__>::set_input_port(gsl::index input_port, BaseFilter& filter, gsl::index output_port)
  {
    Parent::set_input_port(input_port, filter, output_port);
    converted_inputs_size[input_port] = 0;
    converted_in_delays[input_port] = 0;
    direct_filters[input_port] = dynamic_cast<OutputArrayInterface<DataType_>*>(&filter);
  }
}
<|endoftext|>


<commit_before>#include "deformable/deformable.h"
#include "deformable/DeformAnimation.h"
#include "misc/stream.h"
#include "visualizer/shader.h"
#include "visualizer/atengl.h"
#include "texture/texture.h"
#include "camera/camera.h"

#include <algorithm>
#include <iterator>

namespace aten
{
	class DeformMeshReadHelper : public IDeformMeshReadHelper {
	public:
		DeformMeshReadHelper() {}
		virtual ~DeformMeshReadHelper() {}

	public:
		virtual void createVAO(
			GeomVertexBuffer* vb,
			const VertexAttrib* attribs,
			uint32_t attribNum) override final
		{
			AT_ASSERT(m_shd);
			vb->createVAOByAttribName(m_shd, attribs, attribNum);
		}

		shader* m_shd{ nullptr };
	};

	bool deformable::read(const char* path)
	{
		// TODO...
		DeformMeshReadHelper helper;
		DeformableRenderer::initDeformMeshReadHelper(&helper);

		FileInputStream file;
		AT_VRETURN_FALSE(file.open(path, "rb"));

		FileInputStream* stream = &file;

		MdlHeader header;
		AT_VRETURN_FALSE(AT_STREAM_READ(stream, &header, sizeof(header)));

		// Mesh.
		{
			MdlChunkHeader meshChunkHeader;
			AT_VRETURN_FALSE(AT_STREAM_READ(stream, &meshChunkHeader, sizeof(meshChunkHeader)));

			if (meshChunkHeader.magicChunk == MdlChunkMagic::Mesh) {
				AT_VRETURN_FALSE(m_mesh.read(stream, &helper));
			}
			else {
				AT_VRETURN_FALSE(false);
			}
		}

		auto pos = stream->curPos();

		// Skeleton.
		{
			MdlChunkHeader sklChunkHeader;
			AT_VRETURN_FALSE(AT_STREAM_READ(stream, &sklChunkHeader, sizeof(sklChunkHeader)));

			if (sklChunkHeader.magicChunk == MdlChunkMagic::Joint) {
				AT_VRETURN_FALSE(m_skl.read(stream));
			}
			else {
				AT_VRETURN_FALSE(false);
			}

			m_sklController.init(&m_skl);
		}

		return true;
	}

	void deformable::release()
	{
		m_mesh.release();
		m_skl.release();
		m_sklController.release();
	}

	class DeformMeshRenderHelper : public IDeformMeshRenderHelper {
	public:
		DeformMeshRenderHelper(shader* s) : m_shd(s) {}
		virtual ~DeformMeshRenderHelper() {}

		virtual void applyMatrix(uint32_t idx, const mat4& mtx) override final
		{
			if (m_handleMtxJoint < 0) {
				m_handleMtxJoint = m_shd->getHandle("mtxJoint");
				m_mtxs.reserve(4);
			}

			m_mtxs.push_back(mtx);
		}

		virtual void applyMaterial(const MeshMaterial& mtrlDesc) override final
		{
			const auto& mtrls = material::getMaterials();

			// TODO
			// Find material.
			auto found = std::find_if(
				mtrls.begin(), mtrls.end(),
				[&](const material* mtrl)->bool
			{
				if (mtrl->nameString() == mtrlDesc.name) {
					return true;
				}

				return false;
			});

			if (found != mtrls.end()) {
				const auto mtrl = *found;
				const auto& mtrlParam = mtrl->param();

				auto albedo = texture::getTexture(mtrlParam.albedoMap);
				if (albedo) {
					albedo->bindAsGLTexture(0, m_shd);
				}
			}
		}

		virtual void commitChanges() override final
		{
			AT_ASSERT(m_handleMtxJoint >= 0);

			uint32_t mtxNum = (uint32_t)m_mtxs.size();

			CALL_GL_API(::glUniformMatrix4fv(m_handleMtxJoint, mtxNum, GL_TRUE, (const GLfloat*)&m_mtxs[0]));

			m_mtxs.clear();
		}

		shader* m_shd{ nullptr };
		int m_handleMtxJoint{ -1 };
		std::vector<mat4> m_mtxs;
	};

	void deformable::render(shader* shd)
	{
		if (isEnabledForGPUSkinning()) {
			AT_ASSERT_LOG(false, "Not support for gpu skinning format data.");
			return;
		}

		AT_ASSERT(shd);

		DeformMeshRenderHelper helper(shd);

		m_mesh.render(m_sklController, &helper);
	}

	void deformable::update(const mat4& mtxL2W)
	{
		m_sklController.buildPose(mtxL2W);
	}

	void deformable::update(
		const mat4& mtxL2W,
		DeformAnimation* anm,
		real time)
	{
		anm->applyAnimation(&m_sklController, time);
		m_sklController.buildPose(mtxL2W);
	}

	void deformable::getGeometryData(
		std::vector<SkinningVertex>& vtx,
		std::vector<uint32_t>& idx) const
	{
		m_mesh.getGeometryData(vtx, idx);
	}

	const std::vector<mat4>& deformable::getMatrices() const
	{
		return m_sklController.getMatrices();
	}

	//////////////////////////////////////////////////////////////

	shader DeformableRenderer::s_shd;

	bool DeformableRenderer::init(
		int width, int height,
		const char* pathVS,
		const char* pathFS)
	{
		return s_shd.init(width, height, pathVS, pathFS);
	}

	void DeformableRenderer::render(
		const camera* cam,
		deformable* mdl)
	{
		CALL_GL_API(::glClearColor(0, 0.5f, 1.0f, 1.0f));
		CALL_GL_API(::glClearDepthf(1.0f));
		CALL_GL_API(::glClearStencil(0));
		CALL_GL_API(::glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT | GL_STENCIL_BUFFER_BIT));

		CALL_GL_API(::glEnable(GL_DEPTH_TEST));

		// For Alpha Blend.
		CALL_GL_API(::glEnable(GL_BLEND));
		CALL_GL_API(::glBlendEquation(GL_FUNC_ADD));
		CALL_GL_API(::glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA));

		s_shd.prepareRender(nullptr, false);

		{
			auto camparam = cam->param();

			// TODO
			camparam.znear = real(0.1);
			camparam.zfar = real(10000.0);

			mat4 mtxW2V;
			mat4 mtxV2C;

			mtxW2V.lookat(
				camparam.origin,
				camparam.center,
				camparam.up);

			mtxV2C.perspective(
				camparam.znear,
				camparam.zfar,
				camparam.vfov,
				camparam.aspect);

			aten::mat4 mtxW2C = mtxV2C * mtxW2V;

			auto hMtxW2C = s_shd.getHandle("mtxW2C");
			CALL_GL_API(::glUniformMatrix4fv(hMtxW2C, 1, GL_TRUE, &mtxW2C.a[0]));

			// NOTE
			// O[o}gNXvZɃ[g local to world }gNXZς.
			// ̂߁AVF[_ł͌vZKvȂ̂ŁAVF[_ɓnȂ.
		}

		mdl->render(&s_shd);
	}

	void DeformableRenderer::initDeformMeshReadHelper(DeformMeshReadHelper* helper)
	{
		AT_ASSERT(s_shd.isValid());
		helper->m_shd = &s_shd;
	}
}
<commit_msg>Fix bugs.<commit_after>#include "deformable/deformable.h"
#include "deformable/DeformAnimation.h"
#include "misc/stream.h"
#include "visualizer/shader.h"
#include "visualizer/atengl.h"
#include "texture/texture.h"
#include "camera/camera.h"

#include <algorithm>
#include <iterator>

namespace aten
{
	class DeformMeshReadHelper : public IDeformMeshReadHelper {
	public:
		DeformMeshReadHelper() {}
		virtual ~DeformMeshReadHelper() {}

	public:
		virtual void createVAO(
			GeomVertexBuffer* vb,
			const VertexAttrib* attribs,
			uint32_t attribNum) override final
		{
			AT_ASSERT(m_shd);
			vb->createVAOByAttribName(m_shd, attribs, attribNum);
		}

		shader* m_shd{ nullptr };
	};

	bool deformable::read(const char* path)
	{
		// TODO...
		DeformMeshReadHelper helper;
		DeformableRenderer::initDeformMeshReadHelper(&helper);

		FileInputStream file;
		AT_VRETURN_FALSE(file.open(path, "rb"));

		FileInputStream* stream = &file;

		MdlHeader header;
		AT_VRETURN_FALSE(AT_STREAM_READ(stream, &header, sizeof(header)));

		// Mesh.
		{
			MdlChunkHeader meshChunkHeader;
			AT_VRETURN_FALSE(AT_STREAM_READ(stream, &meshChunkHeader, sizeof(meshChunkHeader)));

			if (meshChunkHeader.magicChunk == MdlChunkMagic::Mesh) {
				AT_VRETURN_FALSE(m_mesh.read(stream, &helper));
			}
			else {
				AT_VRETURN_FALSE(false);
			}
		}

		auto pos = stream->curPos();

		// Skeleton.
		{
			MdlChunkHeader sklChunkHeader;
			AT_VRETURN_FALSE(AT_STREAM_READ(stream, &sklChunkHeader, sizeof(sklChunkHeader)));

			if (sklChunkHeader.magicChunk == MdlChunkMagic::Joint) {
				AT_VRETURN_FALSE(m_skl.read(stream));
			}
			else {
				AT_VRETURN_FALSE(false);
			}

			m_sklController.init(&m_skl);
		}

		return true;
	}

	void deformable::release()
	{
		m_mesh.release();
		m_skl.release();
		m_sklController.release();
	}

	class DeformMeshRenderHelper : public IDeformMeshRenderHelper {
	public:
		DeformMeshRenderHelper(shader* s) : m_shd(s) {}
		virtual ~DeformMeshRenderHelper() {}

		virtual void applyMatrix(uint32_t idx, const mat4& mtx) override final
		{
			if (m_handleMtxJoint < 0) {
				m_handleMtxJoint = m_shd->getHandle("mtxJoint");
				m_mtxs.reserve(4);
			}

			m_mtxs.push_back(mtx);
		}

		virtual void applyMaterial(const MeshMaterial& mtrlDesc) override final
		{
			const auto& mtrls = material::getMaterials();

			// TODO
			// Find material.
			auto found = std::find_if(
				mtrls.begin(), mtrls.end(),
				[&](const material* mtrl)->bool
			{
				if (mtrl->nameString() == mtrlDesc.name) {
					return true;
				}

				return false;
			});

			if (found != mtrls.end()) {
				const auto mtrl = *found;
				const auto& mtrlParam = mtrl->param();

				auto albedo = texture::getTexture(mtrlParam.albedoMap);
				if (albedo) {
					albedo->bindAsGLTexture(0, m_shd);
				}
			}
		}

		virtual void commitChanges() override final
		{
			AT_ASSERT(m_handleMtxJoint >= 0);

			uint32_t mtxNum = (uint32_t)m_mtxs.size();

			CALL_GL_API(::glUniformMatrix4fv(m_handleMtxJoint, mtxNum, GL_TRUE, (const GLfloat*)&m_mtxs[0]));

			m_mtxs.clear();
		}

		shader* m_shd{ nullptr };
		int m_handleMtxJoint{ -1 };
		std::vector<mat4> m_mtxs;
	};

	void deformable::render(shader* shd)
	{
		AT_ASSERT(shd);

		DeformMeshRenderHelper helper(shd);

		m_mesh.render(m_sklController, &helper);
	}

	void deformable::update(const mat4& mtxL2W)
	{
		m_sklController.buildPose(mtxL2W);
	}

	void deformable::update(
		const mat4& mtxL2W,
		DeformAnimation* anm,
		real time)
	{
		if (anm) {
			anm->applyAnimation(&m_sklController, time);
		}
		m_sklController.buildPose(mtxL2W);
	}

	void deformable::getGeometryData(
		std::vector<SkinningVertex>& vtx,
		std::vector<uint32_t>& idx) const
	{
		m_mesh.getGeometryData(vtx, idx);
	}

	const std::vector<mat4>& deformable::getMatrices() const
	{
		return m_sklController.getMatrices();
	}

	//////////////////////////////////////////////////////////////

	shader DeformableRenderer::s_shd;

	bool DeformableRenderer::init(
		int width, int height,
		const char* pathVS,
		const char* pathFS)
	{
		return s_shd.init(width, height, pathVS, pathFS);
	}

	void DeformableRenderer::render(
		const camera* cam,
		deformable* mdl)
	{
		CALL_GL_API(::glClearColor(0, 0.5f, 1.0f, 1.0f));
		CALL_GL_API(::glClearDepthf(1.0f));
		CALL_GL_API(::glClearStencil(0));
		CALL_GL_API(::glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT | GL_STENCIL_BUFFER_BIT));

		CALL_GL_API(::glEnable(GL_DEPTH_TEST));

		// For Alpha Blend.
		CALL_GL_API(::glEnable(GL_BLEND));
		CALL_GL_API(::glBlendEquation(GL_FUNC_ADD));
		CALL_GL_API(::glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA));

		s_shd.prepareRender(nullptr, false);

		{
			auto camparam = cam->param();

			// TODO
			camparam.znear = real(0.1);
			camparam.zfar = real(10000.0);

			mat4 mtxW2V;
			mat4 mtxV2C;

			mtxW2V.lookat(
				camparam.origin,
				camparam.center,
				camparam.up);

			mtxV2C.perspective(
				camparam.znear,
				camparam.zfar,
				camparam.vfov,
				camparam.aspect);

			aten::mat4 mtxW2C = mtxV2C * mtxW2V;

			auto hMtxW2C = s_shd.getHandle("mtxW2C");
			CALL_GL_API(::glUniformMatrix4fv(hMtxW2C, 1, GL_TRUE, &mtxW2C.a[0]));

			// NOTE
			// O[o}gNXvZɃ[g local to world }gNXZς.
			// ̂߁AVF[_ł͌vZKvȂ̂ŁAVF[_ɓnȂ.
		}

		mdl->render(&s_shd);
	}

	void DeformableRenderer::initDeformMeshReadHelper(DeformMeshReadHelper* helper)
	{
		AT_ASSERT(s_shd.isValid());
		helper->m_shd = &s_shd;
	}
}
<|endoftext|>


<commit_before>#include "obstacle_sim.h"

namespace astar_planner
{

void convertToPointCloud2(const std::vector<geometry_msgs::Point>& points, sensor_msgs::PointCloud2* ros_pointcloud)
{
  pcl::PointCloud<pcl::PointXYZ> pcl_points;

  for (const auto& p : points)
  {
    pcl::PointXYZ pcl_p;
    pcl_p.x = p.x;
    pcl_p.y = p.y;
    pcl_p.z = p.z;

    pcl_points.push_back(pcl_p);
  }

  //sensor_msgs::PointCloud2 ros_pointcloud;
  pcl::toROSMsg(pcl_points, *ros_pointcloud);
}

geometry_msgs::Point transformPoint(const geometry_msgs::Point& p, const tf::Transform& tf)
{
  // convert to tf msg
  tf::Point tf_point;
  tf::pointMsgToTF(p, tf_point);

  // apply transform
  tf_point = tf * tf_point;

  // convert to ros msg
  geometry_msgs::Point ros_point;
  tf::pointTFToMsg(tf_point, ros_point);

  return ros_point;
}

ObstacleSim::ObstacleSim()
    : private_nh_("~"),
      sub_navgoal(false)
{
  initForROS();
}

ObstacleSim::~ObstacleSim()
{
}

void ObstacleSim::initForROS()
{
  // ros parameter settings
  private_nh_.param<double>("obstacle_height", obstacle_height_, 1.0);
  private_nh_.param<double>("obstacle_width", obstacle_width_, 1.0);
  private_nh_.param<double>("points_interval", points_interval_, 0.1);
  private_nh_.param<std::string>("obstacle_frame", obstacle_frame_, "/velodyne");

  // setup subscriber
  nav_goal_sub_ = nh_.subscribe("move_base_simple/goal", 1, &ObstacleSim::callbackFromNavGoal, this);

  // setup publisher
  obstacle_sim_points_pub_ = nh_.advertise<visualization_msgs::Marker>("obstacle_sim_points", 1, true);
  obstacle_sim_pointcloud_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("obstacle_sim_pointcloud", 1, true);
  obstacle_marker_pub_ = nh_.advertise<visualization_msgs::Marker>("astar_sim_obstacle", 1, true);
}

void ObstacleSim::callbackFromNavGoal(const geometry_msgs::PoseStampedConstPtr& msg)
{
  sub_navgoal = true;

  // frame of using in RViz
  world_frame_ = msg->header.frame_id;

  // reset points
  obstacle_points_.clear();

  // points at each vertex of rectangle
  geometry_msgs::Point upper_left, upper_right, lower_left, lower_right;
  upper_left.x  = msg->pose.position.x - obstacle_width_ / 2.0;
  upper_left.y  = msg->pose.position.y + obstacle_height_ / 2.0;
  upper_right.x = upper_left.x + obstacle_width_;
  upper_right.y = upper_left.y;
  lower_left.x  = upper_left.x;
  lower_left.y  = upper_left.y - obstacle_height_;
  lower_right.x = upper_right.x;
  lower_right.y = lower_left.y;

  // points along each side
  for (double x = upper_left.x; x < upper_right.x; x += points_interval_)
  {
    geometry_msgs::Point up, down;
    up.x = x;
    up.y = upper_left.y;
    down.x = x;
    down.y = lower_left.y;

    obstacle_points_.emplace_back(up);
    obstacle_points_.emplace_back(down);
  }
  for (double y = upper_left.y; y > lower_left.y; y -= points_interval_)
  {
    geometry_msgs::Point left, right;
    left.x = upper_left.x;
    left.y = y;
    right.x = upper_right.x;
    right.y = y;

    obstacle_points_.emplace_back(left);
    obstacle_points_.emplace_back(right);
  }

  displayObstacleMarker(upper_left, upper_right, lower_left, lower_right, world_frame_);

  // debug
  //displayObstaclePoints();
}

// debug
void ObstacleSim::displayObstaclePoints()
{
  visualization_msgs::Marker marker;
  marker.header.frame_id = "/world";
  marker.header.stamp = ros::Time();
  marker.ns = "obstacle_sim";
  marker.id = 0;
  marker.type = visualization_msgs::Marker::POINTS;
  marker.action = visualization_msgs::Marker::ADD;
  for (const auto &p : obstacle_points_)
  {
    marker.points.push_back(p);
  }

  marker.scale.x = 0.05;
  marker.scale.y = 0.05;
  marker.color.a = 1.0;
  marker.color.r = 0.0;
  marker.color.g = 1.0;
  marker.color.b = 0.0;
  //marker.lifetime = ros::Duration(0.1);
  marker.frame_locked = true;

  obstacle_sim_points_pub_.publish(marker);
}

void ObstacleSim::displayObstacleMarker(const geometry_msgs::Point& p1, const geometry_msgs::Point& p2, const geometry_msgs::Point& p3, const geometry_msgs::Point& p4, const std::string& frame)
{
  visualization_msgs::Marker marker;
  marker.header.frame_id = frame;
  marker.header.stamp = ros::Time();
  marker.ns = "obstacle_sim";
  marker.id = 0;
  marker.type = visualization_msgs::Marker::LINE_STRIP;
  marker.action = visualization_msgs::Marker::ADD;

  // 1 -- 2
  // |    |
  // 3 -- 4
  marker.points.push_back(p1);
  marker.points.push_back(p2);
  marker.points.push_back(p4);
  marker.points.push_back(p3);
  marker.points.push_back(p1);
  marker.points.push_back(p4);
  marker.points.push_back(p3);
  marker.points.push_back(p2);

  marker.scale.x = 0.1;
  marker.color.a = 1.0;
  marker.color.r = 0.0;
  marker.color.g = 1.0;
  marker.color.b = 0.0;
  //marker.lifetime = ros::Duration(0.1);
  marker.frame_locked = true;

  obstacle_marker_pub_.publish(marker);
}

void ObstacleSim::publishPoints()
{
  // transform points to sensor frame to be detected as an obstacle
  tf::StampedTransform world2sensor;
  try
  {
    tf_listener_.lookupTransform(world_frame_, obstacle_frame_, ros::Time(0), world2sensor);
  }
  catch (tf::TransformException ex)
  {
    ROS_ERROR("%s", ex.what());
    return;
  }

  tf::Transform sensor2world = world2sensor.inverse();

  std::vector<geometry_msgs::Point> obstacle_points_tf;
  for (const auto& p : obstacle_points_)
  {
    geometry_msgs::Point tf_p = transformPoint(p, sensor2world);
    tf_p.z = 0.0;

    obstacle_points_tf.emplace_back(tf_p);
  }

  sensor_msgs::PointCloud2 ros_pointcloud;
  convertToPointCloud2(obstacle_points_tf, &ros_pointcloud);
  ros_pointcloud.header.frame_id = obstacle_frame_;
  ros_pointcloud.header.stamp = ros::Time();

  obstacle_sim_pointcloud_pub_.publish(ros_pointcloud);
}

void ObstacleSim::run()
{
  ros::Rate loop_rate(10);
  while (ros::ok())
  {
    ros::spinOnce();

    // not subscribed obstacle pose yet
    if (!sub_navgoal)
    {
      loop_rate.sleep();
      continue;
    }

    // publish obstacle points
    publishPoints();

    loop_rate.sleep();
  }
}

} // namespace astar_planner
<commit_msg>Change color of a simulated obstacle<commit_after>#include "obstacle_sim.h"

namespace astar_planner
{

void convertToPointCloud2(const std::vector<geometry_msgs::Point>& points, sensor_msgs::PointCloud2* ros_pointcloud)
{
  pcl::PointCloud<pcl::PointXYZ> pcl_points;

  for (const auto& p : points)
  {
    pcl::PointXYZ pcl_p;
    pcl_p.x = p.x;
    pcl_p.y = p.y;
    pcl_p.z = p.z;

    pcl_points.push_back(pcl_p);
  }

  //sensor_msgs::PointCloud2 ros_pointcloud;
  pcl::toROSMsg(pcl_points, *ros_pointcloud);
}

geometry_msgs::Point transformPoint(const geometry_msgs::Point& p, const tf::Transform& tf)
{
  // convert to tf msg
  tf::Point tf_point;
  tf::pointMsgToTF(p, tf_point);

  // apply transform
  tf_point = tf * tf_point;

  // convert to ros msg
  geometry_msgs::Point ros_point;
  tf::pointTFToMsg(tf_point, ros_point);

  return ros_point;
}

ObstacleSim::ObstacleSim()
    : private_nh_("~"),
      sub_navgoal(false)
{
  initForROS();
}

ObstacleSim::~ObstacleSim()
{
}

void ObstacleSim::initForROS()
{
  // ros parameter settings
  private_nh_.param<double>("obstacle_height", obstacle_height_, 1.0);
  private_nh_.param<double>("obstacle_width", obstacle_width_, 1.0);
  private_nh_.param<double>("points_interval", points_interval_, 0.1);
  private_nh_.param<std::string>("obstacle_frame", obstacle_frame_, "/velodyne");

  // setup subscriber
  nav_goal_sub_ = nh_.subscribe("move_base_simple/goal", 1, &ObstacleSim::callbackFromNavGoal, this);

  // setup publisher
  obstacle_sim_points_pub_ = nh_.advertise<visualization_msgs::Marker>("obstacle_sim_points", 1, true);
  obstacle_sim_pointcloud_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("obstacle_sim_pointcloud", 1, true);
  obstacle_marker_pub_ = nh_.advertise<visualization_msgs::Marker>("astar_sim_obstacle", 1, true);
}

void ObstacleSim::callbackFromNavGoal(const geometry_msgs::PoseStampedConstPtr& msg)
{
  sub_navgoal = true;

  // frame of using in RViz
  world_frame_ = msg->header.frame_id;

  // reset points
  obstacle_points_.clear();

  // points at each vertex of rectangle
  geometry_msgs::Point upper_left, upper_right, lower_left, lower_right;
  upper_left.x  = msg->pose.position.x - obstacle_width_ / 2.0;
  upper_left.y  = msg->pose.position.y + obstacle_height_ / 2.0;
  upper_right.x = upper_left.x + obstacle_width_;
  upper_right.y = upper_left.y;
  lower_left.x  = upper_left.x;
  lower_left.y  = upper_left.y - obstacle_height_;
  lower_right.x = upper_right.x;
  lower_right.y = lower_left.y;

  // points along each side
  for (double x = upper_left.x; x < upper_right.x; x += points_interval_)
  {
    geometry_msgs::Point up, down;
    up.x = x;
    up.y = upper_left.y;
    down.x = x;
    down.y = lower_left.y;

    obstacle_points_.emplace_back(up);
    obstacle_points_.emplace_back(down);
  }
  for (double y = upper_left.y; y > lower_left.y; y -= points_interval_)
  {
    geometry_msgs::Point left, right;
    left.x = upper_left.x;
    left.y = y;
    right.x = upper_right.x;
    right.y = y;

    obstacle_points_.emplace_back(left);
    obstacle_points_.emplace_back(right);
  }

  displayObstacleMarker(upper_left, upper_right, lower_left, lower_right, world_frame_);

  // debug
  //displayObstaclePoints();
}

// debug
void ObstacleSim::displayObstaclePoints()
{
  visualization_msgs::Marker marker;
  marker.header.frame_id = "/world";
  marker.header.stamp = ros::Time();
  marker.ns = "obstacle_sim";
  marker.id = 0;
  marker.type = visualization_msgs::Marker::POINTS;
  marker.action = visualization_msgs::Marker::ADD;
  for (const auto &p : obstacle_points_)
  {
    marker.points.push_back(p);
  }

  marker.scale.x = 0.05;
  marker.scale.y = 0.05;
  marker.color.a = 1.0;
  marker.color.r = 0.0;
  marker.color.g = 1.0;
  marker.color.b = 0.0;
  //marker.lifetime = ros::Duration(0.1);
  marker.frame_locked = true;

  obstacle_sim_points_pub_.publish(marker);
}

void ObstacleSim::displayObstacleMarker(const geometry_msgs::Point& p1, const geometry_msgs::Point& p2, const geometry_msgs::Point& p3, const geometry_msgs::Point& p4, const std::string& frame)
{
  visualization_msgs::Marker marker;
  marker.header.frame_id = frame;
  marker.header.stamp = ros::Time();
  marker.ns = "obstacle_sim";
  marker.id = 0;
  marker.type = visualization_msgs::Marker::LINE_STRIP;
  marker.action = visualization_msgs::Marker::ADD;

  // 1 -- 2
  // |    |
  // 3 -- 4
  marker.points.push_back(p1);
  marker.points.push_back(p2);
  marker.points.push_back(p4);
  marker.points.push_back(p3);
  marker.points.push_back(p1);
  marker.points.push_back(p4);
  marker.points.push_back(p3);
  marker.points.push_back(p2);

  marker.scale.x = 0.1;
  marker.color.a = 1.0;
  marker.color.r = 1.0;
  marker.color.g = 1.0;
  marker.color.b = 0.0;
  //marker.lifetime = ros::Duration(0.1);
  marker.frame_locked = true;

  obstacle_marker_pub_.publish(marker);
}

void ObstacleSim::publishPoints()
{
  // transform points to sensor frame to be detected as an obstacle
  tf::StampedTransform world2sensor;
  try
  {
    tf_listener_.lookupTransform(world_frame_, obstacle_frame_, ros::Time(0), world2sensor);
  }
  catch (tf::TransformException ex)
  {
    ROS_ERROR("%s", ex.what());
    return;
  }

  tf::Transform sensor2world = world2sensor.inverse();

  std::vector<geometry_msgs::Point> obstacle_points_tf;
  for (const auto& p : obstacle_points_)
  {
    geometry_msgs::Point tf_p = transformPoint(p, sensor2world);
    tf_p.z = 0.0;

    obstacle_points_tf.emplace_back(tf_p);
  }

  sensor_msgs::PointCloud2 ros_pointcloud;
  convertToPointCloud2(obstacle_points_tf, &ros_pointcloud);
  ros_pointcloud.header.frame_id = obstacle_frame_;
  ros_pointcloud.header.stamp = ros::Time();

  obstacle_sim_pointcloud_pub_.publish(ros_pointcloud);
}

void ObstacleSim::run()
{
  ros::Rate loop_rate(10);
  while (ros::ok())
  {
    ros::spinOnce();

    // not subscribed obstacle pose yet
    if (!sub_navgoal)
    {
      loop_rate.sleep();
      continue;
    }

    // publish obstacle points
    publishPoints();

    loop_rate.sleep();
  }
}

} // namespace astar_planner
<|endoftext|>