<commit_before>/************************************************************************/
/*                                                                      */
/*               Copyright 2008-2009 by Ullrich Koethe                  */
/*       Cognitive Systems Group, University of Hamburg, Germany        */
/*                                                                      */
/*    This file is part of the VIGRA computer vision library.           */
/*    The VIGRA Website is                                              */
/*        http://kogs-www.informatik.uni-hamburg.de/~koethe/vigra/      */
/*    Please direct questions, bug reports, and contributions to        */
/*        ullrich.koethe@iwr.uni-heidelberg.de    or                    */
/*        vigra@informatik.uni-hamburg.de                               */
/*                                                                      */
/*    Permission is hereby granted, free of charge, to any person       */
/*    obtaining a copy of this software and associated documentation    */
/*    files (the "Software"), to deal in the Software without           */
/*    restriction, including without limitation the rights to use,      */
/*    copy, modify, merge, publish, distribute, sublicense, and/or      */
/*    sell copies of the Software, and to permit persons to whom the    */
/*    Software is furnished to do so, subject to the following          */
/*    conditions:                                                       */
/*                                                                      */
/*    The above copyright notice and this permission notice shall be    */
/*    included in all copies or substantial portions of the             */
/*    Software.                                                         */
/*                                                                      */
/*    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND    */
/*    EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES   */
/*    OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND          */
/*    NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT       */
/*    HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,      */
/*    WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING      */
/*    FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR     */
/*    OTHER DEALINGS IN THE SOFTWARE.                                   */
/*                                                                      */
/************************************************************************/

#ifndef VIGRA_TIMING_HXX
#define VIGRA_TIMING_HXX
#ifndef VIGRA_NO_TIMING

#include <iostream>
#include <sstream>
#ifdef MULTI_TICTOC
    #include <vector>
#endif
// usage:
// void time_it()
// {
//     USETICTOC;
//     TIC;
//      ...
//     std::cerr << TOC << " for time_it\n";
// }

#ifdef WIN32

    #include "windows.h"

    namespace {

    inline double queryTimerUnit()
    {
        LARGE_INTEGER frequency;
        QueryPerformanceFrequency(&frequency);
        return 1000.0 / frequency.QuadPart;
    }

    inline double tic_toc_diff_num(LARGE_INTEGER const & tic)
    {
        LARGE_INTEGER toc;
        QueryPerformanceCounter(&toc);
        static double unit = queryTimerUnit();
        return ((toc.QuadPart - tic.QuadPart) * unit);
    }

    inline std::string tic_toc_diff_string(LARGE_INTEGER const & tic)
    {
        double diff = tic_toc_diff_num(tic);
        std::stringstream s;
        s << diff << " msec";
        return s.str();
    }

    inline void tic_toc_diff(LARGE_INTEGER const & tic)
    {
        std::cerr << tic_toc_diff_string(tic) <<std::endl;
    }

    } // unnamed namespace

#ifndef MULTI_TICTOC
    #define USETICTOC LARGE_INTEGER tic_timer;
    #define TIC QueryPerformanceCounter(&tic_timer);
    #define TOC  tic_toc_diff       (tic_timer);
    #define TOCN tic_toc_diff_num   (tic_timer);
    #define TOCS tic_toc_diff_string(tic_timer);
#else
    #define USETICTOC std::vector<LARGE_INTEGER> tic_timer;
    #define TIC tic_timer.push_back(LARGE_INTEGER());\
                QueryPerformanceCounter(&(tic_timer.back()));
    #define TOC  tic_toc_diff       (tic_timer.back());\
                 tic_timer.pop_back();
    #define TOCN tic_toc_diff_num   (tic_timer.back());\
                 tic_timer.pop_back();
    #define TOCS tic_toc_diff_string(tic_timer.back());\
                 tic_timer.pop_back();
#endif

#else

    #if defined(VIGRA_HIRES_TIMING) && !defined(__CYGWIN__)
        // requires linking against librt

        #include <time.h>

        namespace {

        inline double tic_toc_diff_num(timespec const & tic)
        {
            timespec toc;
            clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &toc);
            return ((toc.tv_sec*1000.0 + toc.tv_nsec/1000000.0) -
                  (tic.tv_sec*1000.0 + tic.tv_nsec/1000000.0));
        }

        inline std::string tic_toc_diff_string(timeval const & tic)
        {
            double diff = tic_toc_diff_num(tic);
            std::stringstream s;
            s << diff << " msec";
            return s.str();
        }

        inline void tic_toc_diff(timeval const & tic)
        {
            std::cerr << tic_toc_diff_string(tic) << std::endl;
        }
        } // unnamed namespace

#ifndef MULTI_TICTOC
        #define USETICTOC timespec tic_timer;
        #define TIC clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &tic_timer);
        #define TOC  tic_toc_diff       (tic_timer);
        #define TOCN tic_toc_diff_num   (tic_timer);
        #define TOCS tic_toc_diff_string(tic_timer);
#else

        #define USETICTOC std::vector<timespec> tic_timer;
        #define TIC tic_timer.push_back(timespec());\
                    clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &(tic_timer.back()));
        #define TOC  tic_toc_diff       (tic_timer.back());\
                     tic_timer.pop_back();
        #define TOCN tic_toc_diff_num   (tic_timer.back());\
                     tic_timer.pop_back();
        #define TOCS tic_toc_diff_string(tic_timer.back());\
                     tic_timer.pop_back();
#endif
    #else

        #include <sys/time.h>

        namespace {

        inline double tic_toc_diff_num(timeval const & tic)
        {
            timeval toc;
            gettimeofday(&toc, NULL);
            return  ((toc.tv_sec*1000.0 + toc.tv_usec/1000.0) -
                        (tic.tv_sec*1000.0 + tic.tv_usec/1000.0));
        }
        inline std::string tic_toc_diff_string(timeval const & tic)
        {
            double diff = tic_toc_diff_num(tic);
            std::stringstream s;
            s << diff << " msec";
            return s.str();
        }
        inline void tic_toc_diff(timeval const & tic)
        {
            std::cerr << tic_toc_diff_string(tic)<< std::endl;
        }

        } // unnamed namespace

#ifndef MULTI_TICTOC
        #define USETICTOC timeval tic_timer;
        #define TIC  gettimeofday       (&tic_timer, NULL);
        #define TOC  tic_toc_diff       (tic_timer);
        #define TOCN tic_toc_diff_num   (tic_timer);
        #define TOCS tic_toc_diff_string(tic_timer);
#else

        #define USETICTOC std::vector<timeval> tic_timer;
        #define TIC tic_timer.push_back(timeval());\
                    gettimeofday(&(tic_timer.back()), NULL);
        #define TOC  tic_toc_diff       (tic_timer.back());\
                     tic_timer.pop_back();
        #define TOCN tic_toc_diff_num   (tic_timer.back());\
                     tic_timer.pop_back();
        #define TOCS tic_toc_diff_string(tic_timer.back());\
                     tic_timer.pop_back();
#endif

    #endif // VIGRA_HIRES_TIMING

#endif // WIN32

#else // NDEBUG

#define USETICTOC
#define TIC
#define TOC
#define TOCN
#define TICS
#endif // NDEBUG

#endif // VIGRA_TIMING_HXX
<commit_msg>improved timing.hxx docu<commit_after>/************************************************************************/
/*                                                                      */
/*               Copyright 2008-2009 by Ullrich Koethe                  */
/*       Cognitive Systems Group, University of Hamburg, Germany        */
/*                                                                      */
/*    This file is part of the VIGRA computer vision library.           */
/*    The VIGRA Website is                                              */
/*        http://kogs-www.informatik.uni-hamburg.de/~koethe/vigra/      */
/*    Please direct questions, bug reports, and contributions to        */
/*        ullrich.koethe@iwr.uni-heidelberg.de    or                    */
/*        vigra@informatik.uni-hamburg.de                               */
/*                                                                      */
/*    Permission is hereby granted, free of charge, to any person       */
/*    obtaining a copy of this software and associated documentation    */
/*    files (the "Software"), to deal in the Software without           */
/*    restriction, including without limitation the rights to use,      */
/*    copy, modify, merge, publish, distribute, sublicense, and/or      */
/*    sell copies of the Software, and to permit persons to whom the    */
/*    Software is furnished to do so, subject to the following          */
/*    conditions:                                                       */
/*                                                                      */
/*    The above copyright notice and this permission notice shall be    */
/*    included in all copies or substantial portions of the             */
/*    Software.                                                         */
/*                                                                      */
/*    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND    */
/*    EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES   */
/*    OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND          */
/*    NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT       */
/*    HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,      */
/*    WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING      */
/*    FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR     */
/*    OTHER DEALINGS IN THE SOFTWARE.                                   */
/*                                                                      */
/************************************************************************/

#ifndef VIGRA_TIMING_HXX
#define VIGRA_TIMING_HXX
#ifndef VIGRA_NO_TIMING

#include <iostream>
#include <sstream>
#ifdef MULTI_TICTOC
    #include <vector>
#endif

// usage:
// void time_it()
// {
//     USETICTOC;
//     TIC;
//      ... // code to be timed
//     TOC;
//      ... // untimed code
//     TIC;
//      ... // other code to be timed
//     TOC;
// }
//
// Intead of TOC which outputs the time difference to std::cerr,
// you may use TOCN (the time difference in msec as a double)
// or TOCS (the time difference as a std::string). If MULTI_TICTOC
// is defined, TIC/TOC pairs can be nested.
//
// Timings below 1 msec are generally subject to round-off errors. Under
// LINUX, you can #define VIGRA_HIRES_TIMING to get better
// accuracy, but this requires linking against librt.

#ifdef WIN32

    #include "windows.h"

    namespace {

    inline double queryTimerUnit()
    {
        LARGE_INTEGER frequency;
        QueryPerformanceFrequency(&frequency);
        return 1000.0 / frequency.QuadPart;
    }

    inline double tic_toc_diff_num(LARGE_INTEGER const & tic)
    {
        LARGE_INTEGER toc;
        QueryPerformanceCounter(&toc);
        static double unit = queryTimerUnit();
        return ((toc.QuadPart - tic.QuadPart) * unit);
    }

    inline std::string tic_toc_diff_string(LARGE_INTEGER const & tic)
    {
        double diff = tic_toc_diff_num(tic);
        std::stringstream s;
        s << diff << " msec";
        return s.str();
    }

    inline void tic_toc_diff(LARGE_INTEGER const & tic)
    {
        std::cerr << tic_toc_diff_string(tic) <<std::endl;
    }

    } // unnamed namespace

#ifndef MULTI_TICTOC
    #define USETICTOC LARGE_INTEGER tic_timer;
    #define TIC QueryPerformanceCounter(&tic_timer);
    #define TOC  tic_toc_diff       (tic_timer);
    #define TOCN tic_toc_diff_num   (tic_timer);
    #define TOCS tic_toc_diff_string(tic_timer);
#else
    #define USETICTOC std::vector<LARGE_INTEGER> tic_timer;
    #define TIC tic_timer.push_back(LARGE_INTEGER());\
                QueryPerformanceCounter(&(tic_timer.back()));
    #define TOC  tic_toc_diff       (tic_timer.back());\
                 tic_timer.pop_back();
    #define TOCN tic_toc_diff_num   (tic_timer.back());\
                 tic_timer.pop_back();
    #define TOCS tic_toc_diff_string(tic_timer.back());\
                 tic_timer.pop_back();
#endif

#else

    #if defined(VIGRA_HIRES_TIMING) && !defined(__CYGWIN__)
        // requires linking against librt

        #include <time.h>

        namespace {

        inline double tic_toc_diff_num(timespec const & tic)
        {
            timespec toc;
            clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &toc);
            return ((toc.tv_sec*1000.0 + toc.tv_nsec/1000000.0) -
                  (tic.tv_sec*1000.0 + tic.tv_nsec/1000000.0));
        }

        inline std::string tic_toc_diff_string(timespec const & tic)
        {
            double diff = tic_toc_diff_num(tic);
            std::stringstream s;
            s << diff << " msec";
            return s.str();
        }

        inline void tic_toc_diff(timespec const & tic)
        {
            std::cerr << tic_toc_diff_string(tic) << std::endl;
        }

        } // unnamed namespace

#ifndef MULTI_TICTOC
        #define USETICTOC timespec tic_timer;
        #define TIC clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &tic_timer);
        #define TOC  tic_toc_diff       (tic_timer);
        #define TOCN tic_toc_diff_num   (tic_timer);
        #define TOCS tic_toc_diff_string(tic_timer);
#else

        #define USETICTOC std::vector<timespec> tic_timer;
        #define TIC tic_timer.push_back(timespec());\
                    clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &(tic_timer.back()));
        #define TOC  tic_toc_diff       (tic_timer.back());\
                     tic_timer.pop_back();
        #define TOCN tic_toc_diff_num   (tic_timer.back());\
                     tic_timer.pop_back();
        #define TOCS tic_toc_diff_string(tic_timer.back());\
                     tic_timer.pop_back();
#endif
    #else

        #include <sys/time.h>

        namespace {

        inline double tic_toc_diff_num(timeval const & tic)
        {
            timeval toc;
            gettimeofday(&toc, NULL);
            return  ((toc.tv_sec*1000.0 + toc.tv_usec/1000.0) -
                        (tic.tv_sec*1000.0 + tic.tv_usec/1000.0));
        }
        inline std::string tic_toc_diff_string(timeval const & tic)
        {
            double diff = tic_toc_diff_num(tic);
            std::stringstream s;
            s << diff << " msec";
            return s.str();
        }
        inline void tic_toc_diff(timeval const & tic)
        {
            std::cerr << tic_toc_diff_string(tic)<< std::endl;
        }

        } // unnamed namespace

#ifndef MULTI_TICTOC
        #define USETICTOC timeval tic_timer;
        #define TIC  gettimeofday       (&tic_timer, NULL);
        #define TOC  tic_toc_diff       (tic_timer);
        #define TOCN tic_toc_diff_num   (tic_timer);
        #define TOCS tic_toc_diff_string(tic_timer);
#else

        #define USETICTOC std::vector<timeval> tic_timer;
        #define TIC tic_timer.push_back(timeval());\
                    gettimeofday(&(tic_timer.back()), NULL);
        #define TOC  tic_toc_diff       (tic_timer.back());\
                     tic_timer.pop_back();
        #define TOCN tic_toc_diff_num   (tic_timer.back());\
                     tic_timer.pop_back();
        #define TOCS tic_toc_diff_string(tic_timer.back());\
                     tic_timer.pop_back();
#endif

    #endif // VIGRA_HIRES_TIMING

#endif // WIN32

#else // NDEBUG

#define USETICTOC
#define TIC
#define TOC
#define TOCN
#define TICS
#endif // NDEBUG

#endif // VIGRA_TIMING_HXX
<|endoftext|>


<commit_before>/*
* Cryptobox Message Routines
* (C) 2009 Jack Lloyd
*
* Botan is released under the Simplified BSD License (see license.txt)
*/

#include <botan/cryptobox.h>
#include <botan/cipher_mode.h>
#include <botan/mac.h>
#include <botan/pbkdf.h>
#include <botan/data_src.h>
#include <botan/pem.h>
#include <botan/loadstor.h>
#include <botan/mem_ops.h>

namespace Botan {

namespace CryptoBox {

namespace {

/*
First 24 bits of SHA-256("Botan Cryptobox"), followed by 8 0 bits
for later use as flags, etc if needed
*/
const uint32_t CRYPTOBOX_VERSION_CODE = 0xEFC22400;

const size_t VERSION_CODE_LEN = 4;
const size_t CIPHER_KEY_LEN = 32;
const size_t CIPHER_IV_LEN = 16;
const size_t MAC_KEY_LEN = 32;
const size_t MAC_OUTPUT_LEN = 20;
const size_t PBKDF_SALT_LEN = 10;
const size_t PBKDF_ITERATIONS = 8 * 1024;

const size_t PBKDF_OUTPUT_LEN = CIPHER_KEY_LEN + MAC_KEY_LEN + CIPHER_IV_LEN;
const size_t CRYPTOBOX_HEADER_LEN = VERSION_CODE_LEN + PBKDF_SALT_LEN + MAC_OUTPUT_LEN;

}

std::string encrypt(const uint8_t input[], size_t input_len,
                    const std::string& passphrase,
                    RandomNumberGenerator& rng)
   {
   /*
   Output format is:
      version # (4 bytes)
      salt (10 bytes)
      mac (20 bytes)
      ciphertext
   */
   secure_vector<uint8_t> out_buf(CRYPTOBOX_HEADER_LEN + input_len);
   for(size_t i = 0; i != VERSION_CODE_LEN; ++i)
     out_buf[i] = get_byte(i, CRYPTOBOX_VERSION_CODE);
   rng.randomize(&out_buf[VERSION_CODE_LEN], PBKDF_SALT_LEN);
   // space left for MAC here
   copy_mem(&out_buf[CRYPTOBOX_HEADER_LEN], input, input_len);

   // Generate the keys and IV

   std::unique_ptr<PBKDF> pbkdf(PBKDF::create_or_throw("PBKDF2(HMAC(SHA-512))"));

   OctetString master_key = pbkdf->derive_key(
      CIPHER_KEY_LEN + MAC_KEY_LEN + CIPHER_IV_LEN,
      passphrase,
      &out_buf[VERSION_CODE_LEN],
      PBKDF_SALT_LEN,
      PBKDF_ITERATIONS);

   const uint8_t* mk = master_key.begin();
   const uint8_t* cipher_key = mk;
   const uint8_t* mac_key = mk + CIPHER_KEY_LEN;
   const uint8_t* iv = mk + CIPHER_KEY_LEN + MAC_KEY_LEN;

   // Now encrypt and authenticate
   std::unique_ptr<Cipher_Mode> ctr(get_cipher_mode("Serpent/CTR-BE", ENCRYPTION));
   ctr->set_key(cipher_key, CIPHER_KEY_LEN);
   ctr->start(iv, CIPHER_IV_LEN);
   ctr->finish(out_buf, CRYPTOBOX_HEADER_LEN);

   std::unique_ptr<MessageAuthenticationCode> hmac =
      MessageAuthenticationCode::create_or_throw("HMAC(SHA-512)");
   hmac->set_key(mac_key, MAC_KEY_LEN);
   hmac->update(&out_buf[CRYPTOBOX_HEADER_LEN], input_len);

   // Can't write directly because of MAC truncation
   secure_vector<uint8_t> mac = hmac->final();
   copy_mem(&out_buf[VERSION_CODE_LEN + PBKDF_SALT_LEN], mac.data(), MAC_OUTPUT_LEN);

   return PEM_Code::encode(out_buf, "BOTAN CRYPTOBOX MESSAGE");
   }

secure_vector<uint8_t>
decrypt_bin(const uint8_t input[], size_t input_len,
            const std::string& passphrase)
   {
   DataSource_Memory input_src(input, input_len);
   secure_vector<uint8_t> ciphertext =
      PEM_Code::decode_check_label(input_src,
                                   "BOTAN CRYPTOBOX MESSAGE");

   if(ciphertext.size() < CRYPTOBOX_HEADER_LEN)
      throw Decoding_Error("Invalid CryptoBox input");

   for(size_t i = 0; i != VERSION_CODE_LEN; ++i)
      if(ciphertext[i] != get_byte(i, CRYPTOBOX_VERSION_CODE))
         throw Decoding_Error("Bad CryptoBox version");

   const uint8_t* pbkdf_salt = &ciphertext[VERSION_CODE_LEN];
   const uint8_t* box_mac = &ciphertext[VERSION_CODE_LEN + PBKDF_SALT_LEN];

   std::unique_ptr<PBKDF> pbkdf(PBKDF::create_or_throw("PBKDF2(HMAC(SHA-512))"));

   OctetString master_key = pbkdf->derive_key(
      PBKDF_OUTPUT_LEN,
      passphrase,
      pbkdf_salt,
      PBKDF_SALT_LEN,
      PBKDF_ITERATIONS);

   const uint8_t* mk = master_key.begin();
   const uint8_t* cipher_key = mk;
   const uint8_t* mac_key = mk + CIPHER_KEY_LEN;
   const uint8_t* iv = mk + CIPHER_KEY_LEN + MAC_KEY_LEN;

   // Now authenticate and decrypt
   std::unique_ptr<MessageAuthenticationCode> hmac =
      MessageAuthenticationCode::create_or_throw("HMAC(SHA-512)");
   hmac->set_key(mac_key, MAC_KEY_LEN);
   hmac->update(&ciphertext[CRYPTOBOX_HEADER_LEN],
                ciphertext.size() - CRYPTOBOX_HEADER_LEN);
   secure_vector<uint8_t> computed_mac = hmac->final();

   if(!constant_time_compare(computed_mac.data(), box_mac, MAC_OUTPUT_LEN))
      throw Decoding_Error("CryptoBox integrity failure");

   std::unique_ptr<Cipher_Mode> ctr(get_cipher_mode("Serpent/CTR-BE", DECRYPTION));
   ctr->set_key(cipher_key, CIPHER_KEY_LEN);
   ctr->start(iv, CIPHER_IV_LEN);
   ctr->finish(ciphertext, CRYPTOBOX_HEADER_LEN);

   ciphertext.erase(ciphertext.begin(), ciphertext.begin() + CRYPTOBOX_HEADER_LEN);
   return ciphertext;
   }

secure_vector<uint8_t> decrypt_bin(const std::string& input,
                                   const std::string& passphrase)
   {
   return decrypt_bin(reinterpret_cast<const uint8_t*>(input.data()),
                      input.size(),
                      passphrase);
   }

std::string decrypt(const uint8_t input[], size_t input_len,
                    const std::string& passphrase)
   {
   const secure_vector<uint8_t> bin = decrypt_bin(input, input_len, passphrase);

   return std::string(reinterpret_cast<const char*>(&bin[0]),
                      bin.size());
   }

std::string decrypt(const std::string& input,
                    const std::string& passphrase)
   {
   return decrypt(reinterpret_cast<const uint8_t*>(input.data()),
                  input.size(), passphrase);
   }

}

}
<commit_msg>Avoid creating invalid iterator references when input_len == 0<commit_after>/*
* Cryptobox Message Routines
* (C) 2009 Jack Lloyd
*
* Botan is released under the Simplified BSD License (see license.txt)
*/

#include <botan/cryptobox.h>
#include <botan/cipher_mode.h>
#include <botan/mac.h>
#include <botan/pbkdf.h>
#include <botan/data_src.h>
#include <botan/pem.h>
#include <botan/loadstor.h>
#include <botan/mem_ops.h>

namespace Botan {

namespace CryptoBox {

namespace {

/*
First 24 bits of SHA-256("Botan Cryptobox"), followed by 8 0 bits
for later use as flags, etc if needed
*/
const uint32_t CRYPTOBOX_VERSION_CODE = 0xEFC22400;

const size_t VERSION_CODE_LEN = 4;
const size_t CIPHER_KEY_LEN = 32;
const size_t CIPHER_IV_LEN = 16;
const size_t MAC_KEY_LEN = 32;
const size_t MAC_OUTPUT_LEN = 20;
const size_t PBKDF_SALT_LEN = 10;
const size_t PBKDF_ITERATIONS = 8 * 1024;

const size_t PBKDF_OUTPUT_LEN = CIPHER_KEY_LEN + MAC_KEY_LEN + CIPHER_IV_LEN;
const size_t CRYPTOBOX_HEADER_LEN = VERSION_CODE_LEN + PBKDF_SALT_LEN + MAC_OUTPUT_LEN;

}

std::string encrypt(const uint8_t input[], size_t input_len,
                    const std::string& passphrase,
                    RandomNumberGenerator& rng)
   {
   /*
   Output format is:
      version # (4 bytes)
      salt (10 bytes)
      mac (20 bytes)
      ciphertext
   */
   secure_vector<uint8_t> out_buf(CRYPTOBOX_HEADER_LEN + input_len);
   for(size_t i = 0; i != VERSION_CODE_LEN; ++i)
     out_buf[i] = get_byte(i, CRYPTOBOX_VERSION_CODE);
   rng.randomize(&out_buf[VERSION_CODE_LEN], PBKDF_SALT_LEN);
   // space left for MAC here
   if(input_len > 0)
      copy_mem(&out_buf[CRYPTOBOX_HEADER_LEN], input, input_len);

   // Generate the keys and IV

   std::unique_ptr<PBKDF> pbkdf(PBKDF::create_or_throw("PBKDF2(HMAC(SHA-512))"));

   OctetString master_key = pbkdf->derive_key(
      CIPHER_KEY_LEN + MAC_KEY_LEN + CIPHER_IV_LEN,
      passphrase,
      &out_buf[VERSION_CODE_LEN],
      PBKDF_SALT_LEN,
      PBKDF_ITERATIONS);

   const uint8_t* mk = master_key.begin();
   const uint8_t* cipher_key = mk;
   const uint8_t* mac_key = mk + CIPHER_KEY_LEN;
   const uint8_t* iv = mk + CIPHER_KEY_LEN + MAC_KEY_LEN;

   // Now encrypt and authenticate
   std::unique_ptr<Cipher_Mode> ctr(get_cipher_mode("Serpent/CTR-BE", ENCRYPTION));
   ctr->set_key(cipher_key, CIPHER_KEY_LEN);
   ctr->start(iv, CIPHER_IV_LEN);
   ctr->finish(out_buf, CRYPTOBOX_HEADER_LEN);

   std::unique_ptr<MessageAuthenticationCode> hmac =
      MessageAuthenticationCode::create_or_throw("HMAC(SHA-512)");
   hmac->set_key(mac_key, MAC_KEY_LEN);
   if(input_len > 0)
      hmac->update(&out_buf[CRYPTOBOX_HEADER_LEN], input_len);

   // Can't write directly because of MAC truncation
   secure_vector<uint8_t> mac = hmac->final();
   copy_mem(&out_buf[VERSION_CODE_LEN + PBKDF_SALT_LEN], mac.data(), MAC_OUTPUT_LEN);

   return PEM_Code::encode(out_buf, "BOTAN CRYPTOBOX MESSAGE");
   }

secure_vector<uint8_t>
decrypt_bin(const uint8_t input[], size_t input_len,
            const std::string& passphrase)
   {
   DataSource_Memory input_src(input, input_len);
   secure_vector<uint8_t> ciphertext =
      PEM_Code::decode_check_label(input_src,
                                   "BOTAN CRYPTOBOX MESSAGE");

   if(ciphertext.size() < CRYPTOBOX_HEADER_LEN)
      throw Decoding_Error("Invalid CryptoBox input");

   for(size_t i = 0; i != VERSION_CODE_LEN; ++i)
      if(ciphertext[i] != get_byte(i, CRYPTOBOX_VERSION_CODE))
         throw Decoding_Error("Bad CryptoBox version");

   const uint8_t* pbkdf_salt = &ciphertext[VERSION_CODE_LEN];
   const uint8_t* box_mac = &ciphertext[VERSION_CODE_LEN + PBKDF_SALT_LEN];

   std::unique_ptr<PBKDF> pbkdf(PBKDF::create_or_throw("PBKDF2(HMAC(SHA-512))"));

   OctetString master_key = pbkdf->derive_key(
      PBKDF_OUTPUT_LEN,
      passphrase,
      pbkdf_salt,
      PBKDF_SALT_LEN,
      PBKDF_ITERATIONS);

   const uint8_t* mk = master_key.begin();
   const uint8_t* cipher_key = mk;
   const uint8_t* mac_key = mk + CIPHER_KEY_LEN;
   const uint8_t* iv = mk + CIPHER_KEY_LEN + MAC_KEY_LEN;

   // Now authenticate and decrypt
   std::unique_ptr<MessageAuthenticationCode> hmac =
      MessageAuthenticationCode::create_or_throw("HMAC(SHA-512)");
   hmac->set_key(mac_key, MAC_KEY_LEN);

   if(ciphertext.size() > CRYPTOBOX_HEADER_LEN)
      {
      hmac->update(&ciphertext[CRYPTOBOX_HEADER_LEN],
                   ciphertext.size() - CRYPTOBOX_HEADER_LEN);
      }
   secure_vector<uint8_t> computed_mac = hmac->final();

   if(!constant_time_compare(computed_mac.data(), box_mac, MAC_OUTPUT_LEN))
      throw Decoding_Error("CryptoBox integrity failure");

   std::unique_ptr<Cipher_Mode> ctr(get_cipher_mode("Serpent/CTR-BE", DECRYPTION));
   ctr->set_key(cipher_key, CIPHER_KEY_LEN);
   ctr->start(iv, CIPHER_IV_LEN);
   ctr->finish(ciphertext, CRYPTOBOX_HEADER_LEN);

   ciphertext.erase(ciphertext.begin(), ciphertext.begin() + CRYPTOBOX_HEADER_LEN);
   return ciphertext;
   }

secure_vector<uint8_t> decrypt_bin(const std::string& input,
                                   const std::string& passphrase)
   {
   return decrypt_bin(reinterpret_cast<const uint8_t*>(input.data()),
                      input.size(),
                      passphrase);
   }

std::string decrypt(const uint8_t input[], size_t input_len,
                    const std::string& passphrase)
   {
   const secure_vector<uint8_t> bin = decrypt_bin(input, input_len, passphrase);

   return std::string(reinterpret_cast<const char*>(&bin[0]),
                      bin.size());
   }

std::string decrypt(const std::string& input,
                    const std::string& passphrase)
   {
   return decrypt(reinterpret_cast<const uint8_t*>(input.data()),
                  input.size(), passphrase);
   }

}

}
<|endoftext|>


<commit_before>/*************************************************************************
 *
 *  OpenOffice.org - a multi-platform office productivity suite
 *
 *  $RCSfile: ftpcontentcaps.cxx,v $
 *
 *  $Revision: 1.7 $
 *
 *  last change: $Author: rt $ $Date: 2005-09-09 15:32:22 $
 *
 *  The Contents of this file are made available subject to
 *  the terms of GNU Lesser General Public License Version 2.1.
 *
 *
 *    GNU Lesser General Public License Version 2.1
 *    =============================================
 *    Copyright 2005 by Sun Microsystems, Inc.
 *    901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *    This library is free software; you can redistribute it and/or
 *    modify it under the terms of the GNU Lesser General Public
 *    License version 2.1, as published by the Free Software Foundation.
 *
 *    This library is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *    Lesser General Public License for more details.
 *
 *    You should have received a copy of the GNU Lesser General Public
 *    License along with this library; if not, write to the Free Software
 *    Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *    MA  02111-1307  USA
 *
 ************************************************************************/

#include <com/sun/star/beans/Property.hpp>
#include <com/sun/star/beans/PropertyAttribute.hpp>
#include <com/sun/star/beans/PropertyValue.hpp>
#include <com/sun/star/ucb/CommandInfo.hpp>
#include <com/sun/star/ucb/OpenCommandArgument2.hpp>
#include <com/sun/star/ucb/InsertCommandArgument.hpp>
#include <com/sun/star/util/DateTime.hpp>
#include <com/sun/star/uno/Sequence.hxx>

#include "ftpcontent.hxx"

using namespace com::sun::star::beans;
using namespace com::sun::star::uno;
using namespace com::sun::star::ucb;
using namespace com::sun::star::util;
using namespace rtl;

using namespace ftp;

// virtual
Sequence< Property > FTPContent::getProperties(
    const Reference< XCommandEnvironment > & xEnv
)
{
    Sequence< Property > props(7);

    props[0] =
        Property(
            OUString( RTL_CONSTASCII_USTRINGPARAM( "ContentType" ) ),
            -1,
            getCppuType( static_cast< const OUString * >( 0 ) ),
            PropertyAttribute::BOUND | PropertyAttribute::READONLY );

    props[1] =
        Property(
            OUString( RTL_CONSTASCII_USTRINGPARAM( "IsDocument" ) ),
            -1,
            getCppuBooleanType(),
            PropertyAttribute::BOUND | PropertyAttribute::READONLY );

    props[2] =
        Property(
            OUString( RTL_CONSTASCII_USTRINGPARAM( "IsFolder" ) ),
            -1,
            getCppuBooleanType(),
            PropertyAttribute::BOUND | PropertyAttribute::READONLY );

    props[3] =
        Property(
            OUString( RTL_CONSTASCII_USTRINGPARAM( "Title" ) ),
            -1,
            getCppuType( static_cast< const OUString * >( 0 ) ),
            PropertyAttribute::BOUND );//  | PropertyAttribute::READONLY );

    props[4] =
        Property(
            OUString( RTL_CONSTASCII_USTRINGPARAM( "Size" ) ),
            -1,
            getCppuType( static_cast< const sal_Int64 * >( 0 ) ),
            PropertyAttribute::BOUND  | PropertyAttribute::READONLY );

    props[5] =
        Property(
            OUString( RTL_CONSTASCII_USTRINGPARAM( "DateCreated" ) ),
            -1,
            getCppuType( static_cast< DateTime* >( 0 ) ),
            PropertyAttribute::BOUND  | PropertyAttribute::READONLY );

    props[6] =
        Property(
            OUString( RTL_CONSTASCII_USTRINGPARAM( "IsReadOnly" ) ),
            -1,
            getCppuBooleanType(),
            PropertyAttribute::BOUND | PropertyAttribute::READONLY );

    return props;
}

//=========================================================================
// virtual
Sequence< CommandInfo > FTPContent::getCommands(
    const Reference< XCommandEnvironment > & xEnv )
{
//  osl::MutexGuard aGuard( m_aMutex );

    //=================================================================
    //
    // Supported commands
    //
    //=================================================================

#define COMMAND_COUNT 7

    static CommandInfo aCommandInfoTable[] =
    {
        ///////////////////////////////////////////////////////////////
        // Required commands
        ///////////////////////////////////////////////////////////////
        CommandInfo(
            OUString( RTL_CONSTASCII_USTRINGPARAM( "getCommandInfo" ) ),
            -1,
            getCppuVoidType()
        ),
        CommandInfo(
            OUString( RTL_CONSTASCII_USTRINGPARAM( "getPropertySetInfo" ) ),
            -1,
            getCppuVoidType()
        ),
        CommandInfo(
            OUString( RTL_CONSTASCII_USTRINGPARAM( "getPropertyValues" ) ),
            -1,
            getCppuType( static_cast< Sequence< Property > * >( 0 ) )
        ),
        CommandInfo(
            OUString( RTL_CONSTASCII_USTRINGPARAM( "setPropertyValues" ) ),
            -1,
            getCppuType( static_cast< Sequence< PropertyValue > * >( 0 ) )
        ),
        CommandInfo(
            OUString( RTL_CONSTASCII_USTRINGPARAM( "open" ) ),
            -1,
            getCppuType( static_cast< OpenCommandArgument2 * >( 0 ) )
        ),
        CommandInfo(
            OUString( RTL_CONSTASCII_USTRINGPARAM( "insert" ) ),
            -1,
            getCppuType( static_cast< InsertCommandArgument * >( 0 ) )
        ),
        CommandInfo(
            OUString( RTL_CONSTASCII_USTRINGPARAM( "delete" ) ),
            -1,
            getCppuType( static_cast< sal_Bool * >( 0 ) )
        )
    };

    return Sequence<CommandInfo>(aCommandInfoTable,COMMAND_COUNT);
}

<commit_msg>INTEGRATION: CWS warnings01 (1.7.10); FILE MERGED 2005/11/10 15:32:57 pl 1.7.10.1: #i55991# removed warnings<commit_after>/*************************************************************************
 *
 *  OpenOffice.org - a multi-platform office productivity suite
 *
 *  $RCSfile: ftpcontentcaps.cxx,v $
 *
 *  $Revision: 1.8 $
 *
 *  last change: $Author: hr $ $Date: 2006-06-20 05:23:24 $
 *
 *  The Contents of this file are made available subject to
 *  the terms of GNU Lesser General Public License Version 2.1.
 *
 *
 *    GNU Lesser General Public License Version 2.1
 *    =============================================
 *    Copyright 2005 by Sun Microsystems, Inc.
 *    901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *    This library is free software; you can redistribute it and/or
 *    modify it under the terms of the GNU Lesser General Public
 *    License version 2.1, as published by the Free Software Foundation.
 *
 *    This library is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *    Lesser General Public License for more details.
 *
 *    You should have received a copy of the GNU Lesser General Public
 *    License along with this library; if not, write to the Free Software
 *    Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *    MA  02111-1307  USA
 *
 ************************************************************************/

#include <com/sun/star/beans/Property.hpp>
#include <com/sun/star/beans/PropertyAttribute.hpp>
#include <com/sun/star/beans/PropertyValue.hpp>
#include <com/sun/star/ucb/CommandInfo.hpp>
#include <com/sun/star/ucb/OpenCommandArgument2.hpp>
#include <com/sun/star/ucb/InsertCommandArgument.hpp>
#include <com/sun/star/util/DateTime.hpp>
#include <com/sun/star/uno/Sequence.hxx>

#include "ftpcontent.hxx"

using namespace com::sun::star::beans;
using namespace com::sun::star::uno;
using namespace com::sun::star::ucb;
using namespace com::sun::star::util;
using namespace rtl;

using namespace ftp;

// virtual
Sequence< Property > FTPContent::getProperties(
    const Reference< XCommandEnvironment > & /*xEnv*/
)
{
    Sequence< Property > props(7);

    props[0] =
        Property(
            OUString( RTL_CONSTASCII_USTRINGPARAM( "ContentType" ) ),
            -1,
            getCppuType( static_cast< const OUString * >( 0 ) ),
            PropertyAttribute::BOUND | PropertyAttribute::READONLY );

    props[1] =
        Property(
            OUString( RTL_CONSTASCII_USTRINGPARAM( "IsDocument" ) ),
            -1,
            getCppuBooleanType(),
            PropertyAttribute::BOUND | PropertyAttribute::READONLY );

    props[2] =
        Property(
            OUString( RTL_CONSTASCII_USTRINGPARAM( "IsFolder" ) ),
            -1,
            getCppuBooleanType(),
            PropertyAttribute::BOUND | PropertyAttribute::READONLY );

    props[3] =
        Property(
            OUString( RTL_CONSTASCII_USTRINGPARAM( "Title" ) ),
            -1,
            getCppuType( static_cast< const OUString * >( 0 ) ),
            PropertyAttribute::BOUND );//  | PropertyAttribute::READONLY );

    props[4] =
        Property(
            OUString( RTL_CONSTASCII_USTRINGPARAM( "Size" ) ),
            -1,
            getCppuType( static_cast< const sal_Int64 * >( 0 ) ),
            PropertyAttribute::BOUND  | PropertyAttribute::READONLY );

    props[5] =
        Property(
            OUString( RTL_CONSTASCII_USTRINGPARAM( "DateCreated" ) ),
            -1,
            getCppuType( static_cast< DateTime* >( 0 ) ),
            PropertyAttribute::BOUND  | PropertyAttribute::READONLY );

    props[6] =
        Property(
            OUString( RTL_CONSTASCII_USTRINGPARAM( "IsReadOnly" ) ),
            -1,
            getCppuBooleanType(),
            PropertyAttribute::BOUND | PropertyAttribute::READONLY );

    return props;
}

//=========================================================================
// virtual
Sequence< CommandInfo > FTPContent::getCommands(
    const Reference< XCommandEnvironment > & /*xEnv*/ )
{
//  osl::MutexGuard aGuard( m_aMutex );

    //=================================================================
    //
    // Supported commands
    //
    //=================================================================

#define COMMAND_COUNT 7

    static CommandInfo aCommandInfoTable[] =
    {
        ///////////////////////////////////////////////////////////////
        // Required commands
        ///////////////////////////////////////////////////////////////
        CommandInfo(
            OUString( RTL_CONSTASCII_USTRINGPARAM( "getCommandInfo" ) ),
            -1,
            getCppuVoidType()
        ),
        CommandInfo(
            OUString( RTL_CONSTASCII_USTRINGPARAM( "getPropertySetInfo" ) ),
            -1,
            getCppuVoidType()
        ),
        CommandInfo(
            OUString( RTL_CONSTASCII_USTRINGPARAM( "getPropertyValues" ) ),
            -1,
            getCppuType( static_cast< Sequence< Property > * >( 0 ) )
        ),
        CommandInfo(
            OUString( RTL_CONSTASCII_USTRINGPARAM( "setPropertyValues" ) ),
            -1,
            getCppuType( static_cast< Sequence< PropertyValue > * >( 0 ) )
        ),
        CommandInfo(
            OUString( RTL_CONSTASCII_USTRINGPARAM( "open" ) ),
            -1,
            getCppuType( static_cast< OpenCommandArgument2 * >( 0 ) )
        ),
        CommandInfo(
            OUString( RTL_CONSTASCII_USTRINGPARAM( "insert" ) ),
            -1,
            getCppuType( static_cast< InsertCommandArgument * >( 0 ) )
        ),
        CommandInfo(
            OUString( RTL_CONSTASCII_USTRINGPARAM( "delete" ) ),
            -1,
            getCppuType( static_cast< sal_Bool * >( 0 ) )
        )
    };

    return Sequence<CommandInfo>(aCommandInfoTable,COMMAND_COUNT);
}

<|endoftext|>


<commit_before>//////////////////////////////////////////////////////////////////////////
//
//  Copyright 2010 Dr D Studios Pty Limited (ACN 127 184 954) (Dr. D Studios),
//  its affiliates and/or its licensors.
//
//  Redistribution and use in source and binary forms, with or without
//  modification, are permitted provided that the following conditions are
//  met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//
//     * Neither the name of Image Engine Design nor the names of any
//       other contributors to this software may be used to endorse or
//       promote products derived from this software without specific prior
//       written permission.
//
//  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
//  IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
//  THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
//  PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
//  CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
//  EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
//  PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
//  PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
//  LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
//  NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
//  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
//////////////////////////////////////////////////////////////////////////

// OpenGL
#include <glew.h>

// Houdini
#include <UT/UT_Version.h>
#include <RE/RE_Render.h>
#include <UT/UT_Interrupt.h>

// Cortex
#include <IECoreGL/Scene.h>
#include <IECoreGL/State.h>
#include <IECoreGL/StateComponent.h>
#include <IECoreGL/TypedStateComponent.h>
#include <IECoreGL/NameStateComponent.h>
#include <IECoreGL/BoxPrimitive.h>
#include <IECoreGL/Exception.h>
#include <IECoreGL/Group.h>
#include <IECoreGL/Camera.h>
#include <IECore/SimpleTypedData.h>

// IECoreHoudini
#include "GR_Procedural.h"
#include "SOP_ProceduralHolder.h"
using namespace IECoreHoudini;

// ctor
GR_Procedural::GR_Procedural()
{
	IECoreGL::init( true );
}
// dtor
GR_Procedural::~GR_Procedural()
{
}

// Tell Houdini to only render GU_ProceduralDetails with this
// render hook.
int GR_Procedural::getWireMask( GU_Detail *gdp,
		const GR_DisplayOption *dopt
		) const
{
	if ( gdp->attribs().find("IECoreHoudini::SOP_ProceduralHolder", GB_ATTRIB_MIXED) )
		return 0;
	else
    	return GEOPRIMALL;
}

// Tell Houdini to only render GU_ProceduralDetails with this
// render hook.
int GR_Procedural::getShadedMask( GU_Detail *gdp,
		const GR_DisplayOption *dopt
		) const
{
	if ( gdp->attribs().find("IECoreHoudini::SOP_ProceduralHolder", GB_ATTRIB_MIXED) )
        return 0;
    else
    	return GEOPRIMALL;
}

// Get a Cortex display state based on the Houdini display options
IECoreGL::ConstStatePtr GR_Procedural::getDisplayState(
		const GR_DisplayOption *dopt,
		bool wireframe
		)
{
	// default is good for shaded
	IECoreGL::StatePtr state = new IECoreGL::State( true );

	// add some properties for wireframe rendering
	if ( wireframe )
	{
		state->add( new IECoreGL::Primitive::DrawSolid( false ) );
		state->add( new IECoreGL::Primitive::DrawWireframe( true ) );
		UT_Color wire_col;
		wire_col = dopt->wireColor();
		float r,g,b;
		wire_col.getValue( r, g, b );
		state->add( new IECoreGL::WireframeColorStateComponent( Imath::Color4f( r, g, b, 1 ) ) );
	}
	return state;
}

// Render our ParameterisedProcedural in wireframe
void GR_Procedural::renderWire( GU_Detail *gdp,
    RE_Render &ren,
    const GR_AttribOffset &ptinfo,
    const GR_DisplayOption *dopt,
    float lod,
    const GU_PrimGroupClosure *hidden_geometry
    )
{
	if ( !gdp->attribs().find("IECoreHoudini::SOP_ProceduralHolder", GB_ATTRIB_MIXED) )
		return;

#if UT_MAJOR_VERSION_INT >= 11
	GB_AttributeRef attr_offset = gdp->attribs().getOffset( "IECoreHoudini::SOP_ProceduralHolder", GB_ATTRIB_MIXED );
	SOP_ProceduralPassStruct *sop = gdp->attribs().castAttribData<SOP_ProceduralPassStruct>(attr_offset);
#else
	int attr_offset = gdp->attribs().getOffset( "IECoreHoudini::SOP_ProceduralHolder", GB_ATTRIB_MIXED );
	SOP_ProceduralPassStruct *sop = gdp->attribs().castAttribData<SOP_ProceduralPassStruct>(attr_offset);
#endif
	if ( !sop )
		return;

    // our render state
    IECoreGL::ConstStatePtr displayState = getDisplayState( dopt, true );

	// our render scene
	IECoreGL::ConstScenePtr scene = sop->ptr()->scene();
    if ( !scene )
    	return;

    // render our scene
	GLint prevProgram;
	glGetIntegerv( GL_CURRENT_PROGRAM, &prevProgram );
	scene->root()->render( displayState );
	glUseProgram( prevProgram );
}

// Render our ParameterisedProcedural in shaded
void GR_Procedural::renderShaded( GU_Detail *gdp,
		RE_Render &ren,
		const GR_AttribOffset &ptinfo,
		const GR_DisplayOption *dopt,
		float lod,
		const GU_PrimGroupClosure *hidden_geometry
		)
{
	if ( !gdp->attribs().find("IECoreHoudini::SOP_ProceduralHolder", GB_ATTRIB_MIXED) )
		return;

#if UT_MAJOR_VERSION_INT >= 11
	GB_AttributeRef attr_offset = gdp->attribs().getOffset( "IECoreHoudini::SOP_ProceduralHolder", GB_ATTRIB_MIXED );
	SOP_ProceduralPassStruct *sop = gdp->attribs().castAttribData<SOP_ProceduralPassStruct>(attr_offset);
#else
	int attr_offset = gdp->attribs().getOffset( "IECoreHoudini::SOP_ProceduralHolder", GB_ATTRIB_MIXED );
	SOP_ProceduralPassStruct *sop = gdp->attribs().castAttribData<SOP_ProceduralPassStruct>(attr_offset);
#endif
	if ( !sop )
		return;

    // our render state
    IECoreGL::ConstStatePtr displayState = getDisplayState( dopt, false );

	// our render scene
	IECoreGL::ConstScenePtr scene = sop->ptr()->scene();
    if ( !scene )
    	return;

    // render our scene
	GLint prevProgram;
	glGetIntegerv( GL_CURRENT_PROGRAM, &prevProgram );
	scene->root()->render( displayState );
	glUseProgram( prevProgram );
}
<commit_msg>including GB_AttributeRef and avoiding UT_MAJOR_VERSION_INT checks<commit_after>//////////////////////////////////////////////////////////////////////////
//
//  Copyright 2010 Dr D Studios Pty Limited (ACN 127 184 954) (Dr. D Studios),
//  its affiliates and/or its licensors.
//
//  Redistribution and use in source and binary forms, with or without
//  modification, are permitted provided that the following conditions are
//  met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//
//     * Neither the name of Image Engine Design nor the names of any
//       other contributors to this software may be used to endorse or
//       promote products derived from this software without specific prior
//       written permission.
//
//  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
//  IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
//  THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
//  PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
//  CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
//  EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
//  PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
//  PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
//  LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
//  NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
//  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
//////////////////////////////////////////////////////////////////////////

// OpenGL
#include <glew.h>

// Houdini
#include <UT/UT_Version.h>
#include <RE/RE_Render.h>
#include <UT/UT_Interrupt.h>
#include "GB/GB_AttributeRef.h"

// Cortex
#include <IECoreGL/Scene.h>
#include <IECoreGL/State.h>
#include <IECoreGL/StateComponent.h>
#include <IECoreGL/TypedStateComponent.h>
#include <IECoreGL/NameStateComponent.h>
#include <IECoreGL/BoxPrimitive.h>
#include <IECoreGL/Exception.h>
#include <IECoreGL/Group.h>
#include <IECoreGL/Camera.h>
#include <IECore/SimpleTypedData.h>

// IECoreHoudini
#include "GR_Procedural.h"
#include "SOP_ProceduralHolder.h"
using namespace IECoreHoudini;

// ctor
GR_Procedural::GR_Procedural()
{
	IECoreGL::init( true );
}
// dtor
GR_Procedural::~GR_Procedural()
{
}

// Tell Houdini to only render GU_ProceduralDetails with this
// render hook.
int GR_Procedural::getWireMask( GU_Detail *gdp,
		const GR_DisplayOption *dopt
		) const
{
	if ( gdp->attribs().find("IECoreHoudini::SOP_ProceduralHolder", GB_ATTRIB_MIXED) )
		return 0;
	else
    	return GEOPRIMALL;
}

// Tell Houdini to only render GU_ProceduralDetails with this
// render hook.
int GR_Procedural::getShadedMask( GU_Detail *gdp,
		const GR_DisplayOption *dopt
		) const
{
	if ( gdp->attribs().find("IECoreHoudini::SOP_ProceduralHolder", GB_ATTRIB_MIXED) )
        return 0;
    else
    	return GEOPRIMALL;
}

// Get a Cortex display state based on the Houdini display options
IECoreGL::ConstStatePtr GR_Procedural::getDisplayState(
		const GR_DisplayOption *dopt,
		bool wireframe
		)
{
	// default is good for shaded
	IECoreGL::StatePtr state = new IECoreGL::State( true );

	// add some properties for wireframe rendering
	if ( wireframe )
	{
		state->add( new IECoreGL::Primitive::DrawSolid( false ) );
		state->add( new IECoreGL::Primitive::DrawWireframe( true ) );
		UT_Color wire_col;
		wire_col = dopt->wireColor();
		float r,g,b;
		wire_col.getValue( r, g, b );
		state->add( new IECoreGL::WireframeColorStateComponent( Imath::Color4f( r, g, b, 1 ) ) );
	}
	return state;
}

// Render our ParameterisedProcedural in wireframe
void GR_Procedural::renderWire( GU_Detail *gdp,
    RE_Render &ren,
    const GR_AttribOffset &ptinfo,
    const GR_DisplayOption *dopt,
    float lod,
    const GU_PrimGroupClosure *hidden_geometry
    )
{
	if ( !gdp->attribs().find("IECoreHoudini::SOP_ProceduralHolder", GB_ATTRIB_MIXED) )
		return;

	GB_AttributeRef attrOffset = gdp->attribs().getOffset( "IECoreHoudini::SOP_ProceduralHolder", GB_ATTRIB_MIXED );
	SOP_ProceduralPassStruct *sop = gdp->attribs().castAttribData<SOP_ProceduralPassStruct>( attrOffset );

	if ( !sop )
		return;

    // our render state
    IECoreGL::ConstStatePtr displayState = getDisplayState( dopt, true );

	// our render scene
	IECoreGL::ConstScenePtr scene = sop->ptr()->scene();
    if ( !scene )
    	return;

    // render our scene
	GLint prevProgram;
	glGetIntegerv( GL_CURRENT_PROGRAM, &prevProgram );
	scene->root()->render( displayState );
	glUseProgram( prevProgram );
}

// Render our ParameterisedProcedural in shaded
void GR_Procedural::renderShaded( GU_Detail *gdp,
		RE_Render &ren,
		const GR_AttribOffset &ptinfo,
		const GR_DisplayOption *dopt,
		float lod,
		const GU_PrimGroupClosure *hidden_geometry
		)
{
	if ( !gdp->attribs().find("IECoreHoudini::SOP_ProceduralHolder", GB_ATTRIB_MIXED) )
		return;

	GB_AttributeRef attrOffset = gdp->attribs().getOffset( "IECoreHoudini::SOP_ProceduralHolder", GB_ATTRIB_MIXED );
	SOP_ProceduralPassStruct *sop = gdp->attribs().castAttribData<SOP_ProceduralPassStruct>( attrOffset );

	if ( !sop )
		return;

    // our render state
    IECoreGL::ConstStatePtr displayState = getDisplayState( dopt, false );

	// our render scene
	IECoreGL::ConstScenePtr scene = sop->ptr()->scene();
    if ( !scene )
    	return;

    // render our scene
	GLint prevProgram;
	glGetIntegerv( GL_CURRENT_PROGRAM, &prevProgram );
	scene->root()->render( displayState );
	glUseProgram( prevProgram );
}
<|endoftext|>


<commit_before>//
// This file is part of the Marble Virtual Globe.
//
// This program is free software licensed under the GNU LGPL. You can
// find a copy of this license in LICENSE.txt in the top directory of
// the source code.
//
// Copyright 2010      Siddharth Srivastava <akssps011@gmail.com>
//

#include "AdjustNavigation.h"

#include "MarbleWidget.h"
#include "GeoDataCoordinates.h"
#include "PositionTracking.h"
#include "MarbleDebug.h"
#include "MarbleModel.h"
#include "ViewportParams.h"
#include "MarbleMath.h"
#include "global.h"

#include <QtGui/QPixmap>
#include <QtGui/QWidget>
#include <QtCore/QRect>
#include <QtCore/QPointF>
#include <QtCore/QTime>
#include <math.h>

namespace Marble {

class AdjustNavigationPrivate
{
public:

    MarbleWidget        *m_widget;
    PositionTracking    *m_tracking;
    qreal                m_gpsSpeed;
    qreal                m_gpsDirection;
    AdjustNavigation::CenterMode m_recenterMode;
    bool                 m_adjustZoom;
    QTime                m_lastWidgetInteraction;
    bool                 m_selfInteraction;

    /** Constructor */
    AdjustNavigationPrivate( MarbleWidget *widget );

    /**
     * @brief To center on when reaching custom defined border
     * @param position current gps location
     * @param speed optional speed argument
     */
     void moveOnBorderToCenter( const GeoDataCoordinates &position, qreal speed );

    /**
     * For calculating intersection point of projected LineString from
     * current gps location with the map border
     * @param position current gps location
     */
     void findIntersection( const GeoDataCoordinates &position );

    /**
     * @brief Adjust the zoom value of the map
     * @param currentPosition current location of the gps device
     * @param destination geoCoordinates of the point on the screen border where the gps device
     * would reach if allowed to move in that direction
     */
     void adjustZoom( const GeoDataCoordinates &currentPosition, const GeoDataCoordinates &destination );

     /**
       * Center the widget on the given position unless recentering is currently inhibited
       */
     void centerOn( const GeoDataCoordinates &position );
};

AdjustNavigationPrivate::AdjustNavigationPrivate( MarbleWidget *widget ) :
        m_widget( widget ),
        m_tracking( 0 ),
        m_gpsSpeed( 0 ),
        m_gpsDirection( 0 ),
        m_recenterMode( AdjustNavigation::DontRecenter ),
        m_adjustZoom( 0 ),
        m_selfInteraction( false )
{
    m_lastWidgetInteraction.start();
}

void AdjustNavigationPrivate::moveOnBorderToCenter( const GeoDataCoordinates &position, qreal )
{
    qreal lon = 0.0;
    qreal lat = 0.0;

    position.geoCoordinates( lon, lat, GeoDataCoordinates::Degree );

    qreal x = 0.0;
    qreal y = 0.0;
    //recenter if initially the gps location is not visible on the screen
    if(!( m_widget->screenCoordinates( lon, lat, x, y ) ) ) {
         centerOn( position );
    }
    qreal centerLon = m_widget->centerLongitude();
    qreal centerLat = m_widget->centerLatitude();

    qreal centerX = 0.0;
    qreal centerY = 0.0;

    m_widget->screenCoordinates( centerLon, centerLat, centerX, centerY );

    const qreal borderRatio = 0.25;
    //defining the default border distance from map center
    int shiftX = qRound( centerX * borderRatio );
    int shiftY = qRound( centerY * borderRatio );

    QRect recenterBorderBound;
    recenterBorderBound.setCoords( centerX-shiftX, centerY-shiftY, centerX+shiftX,  centerY+shiftY );

    if( !recenterBorderBound.contains( x,y ) ) {
        centerOn( position );
    }
}

void AdjustNavigationPrivate::findIntersection( const GeoDataCoordinates &position )
{
    qreal track = m_gpsDirection;

    if ( track >= 360 ) {
        track = fmod( track,360.0 );
    }

    qreal lon = 0;
    qreal lat = 0;
    position.geoCoordinates( lon, lat, GeoDataCoordinates::Degree );

    qreal currentX = 0;
    qreal currentY = 0;

    if( !m_widget->screenCoordinates( lon, lat, currentX, currentY ) ) {
        centerOn( position );
    }
    else {
        ViewportParams const * const viewparams = m_widget->viewport();

        qreal width = viewparams->width();
        qreal height = viewparams->height();

        QPointF intercept;
        QPointF destinationHorizontal;
        QPointF destinationVertical;
        QPointF destination;

        qreal angle = 0.0;
        bool crossHorizontal =  false;
        bool crossVertical = false;

        //calculation of intersection point
        if( track > 0 && track < 90 ) {
            angle = track;

            //Intersection with line x = width
            intercept.setX( width - currentX );
            intercept.setY( intercept.x() / tan( angle ) );
            destinationVertical.setX( width );
            destinationVertical.setY( currentY-intercept.y() );

            //Intersection with line y = 0
            intercept.setY( currentY );
            intercept.setX( intercept.y() * tan( angle ) );
            destinationHorizontal.setX( currentX + intercept.x() );
            destinationHorizontal.setY( 0 );

            if ( destinationVertical.y() < 0 ) {
                crossHorizontal = true;
            }
            else if( destinationHorizontal.x() > width ) {
                crossVertical = true;
            }

        }
        else if( track > 270 && track < 360 ) {
            angle = track - 270;

            //Intersection with line y = 0
            intercept.setY( currentY );
            intercept.setX( intercept.y() / tan( angle ) );
            destinationHorizontal.setX( currentX - intercept.x() );
            destinationHorizontal.setY( 0 );

            //Intersection with line x = 0
            intercept.setX( currentX );
            intercept.setY( intercept.x() * tan( angle ) );
            destinationVertical.setY( currentY - intercept.y() );
            destinationVertical.setX( 0 );

            if( destinationHorizontal.x() > width ) {
                crossVertical = true;
            }
            else if( destinationVertical.y() < 0 ) {
                crossHorizontal = true;
            }

        }
        else if( track > 180 && track < 270  ) {
            angle = track - 180;

            //Intersection with line x = 0
            intercept.setX( currentX );
            intercept.setY( intercept.x() / tan( angle ) );
            destinationVertical.setY( currentY + intercept.y() );
            destinationVertical.setX( 0 );

            //Intersection with line y = height
            intercept.setY( currentY );
            intercept.setX( intercept.y() * tan( angle ) );
            destinationHorizontal.setX( currentX - intercept.x() );
            destinationHorizontal.setY( height );

            if ( destinationVertical.y() > height ) {
                crossHorizontal = true;
            }
            else if ( destinationHorizontal.x() < 0 ) {
                crossVertical = true;
            }

        }
        else if( track > 90 && track < 180  ) {
            angle = track - 90;

            //Intersection with line y = height
            intercept.setY( height - currentY );
            intercept.setX( intercept.y() / tan( angle ) );
            destinationHorizontal.setX( currentX + intercept.x() );
            destinationHorizontal.setY( height );

            //Intersection with line x = width
            intercept.setX( width - currentX );
            intercept.setY( intercept.x() * tan( angle ) );
            destinationVertical.setX( width );
            destinationVertical.setY( currentY + intercept.y() );

            if ( destinationHorizontal.x() > width ) {
                crossVertical = true;
            }
            else if( destinationVertical.y() > height ) {
                crossHorizontal = true;
            }

        }
        else {
            if( track == 0 ) {
                destinationHorizontal.setX( currentX );
                destinationHorizontal.setY( 0 );
                crossHorizontal = true;
            }
            else if( track == 90 ) {
                destinationVertical.setX( width );
                destinationVertical.setY( currentY );
                crossVertical = true;
            }
            else if( track == 190 ) {
                destinationHorizontal.setX( currentX );
                destinationHorizontal.setY( height );
                crossHorizontal = true;
            }
            else if( track == 270 ) {
                destinationVertical.setX( 0 );
                destinationVertical.setY( currentY );
                crossVertical = true;
            }
        }

        if ( crossHorizontal == true && crossVertical == false ) {
            destination.setX( destinationHorizontal.x() );
            destination.setY( destinationHorizontal.y() );
        }
        else if ( crossVertical == true && crossHorizontal == false ) {
            destination.setX( destinationVertical.x() );
            destination.setY( destinationVertical.y() );
        }

        qreal destinationLon = 0.0;
        qreal destinationLat = 0.0;
        m_widget->geoCoordinates( destination.x(), destination.y(), destinationLon, destinationLat,
                                  GeoDataCoordinates::Radian );
        GeoDataCoordinates destinationCoord( destinationLon, destinationLat, GeoDataCoordinates::Radian );
        adjustZoom( position, destinationCoord );
    }
}

void AdjustNavigationPrivate::adjustZoom( const GeoDataCoordinates &currentPosition, const GeoDataCoordinates &destination )
{
    qreal greatCircleDistance = distanceSphere( currentPosition, destination );
    qreal radius = m_widget->model()->planetRadius();
    qreal distance = greatCircleDistance *  radius;

    if( m_gpsSpeed != 0 ) {
        //time(in minutes) remaining to reach the border of the map
        qreal  remainingTime = ( distance / m_gpsSpeed ) * SEC2MIN;

        //tolerance time limits( in minutes ) before auto zooming
        qreal thresholdVeryLow = 0.5;
        qreal thresholdLow = 2.0 * thresholdVeryLow;
        qreal thresholdHigh = 12.0 * thresholdLow;
        qreal thresholdVeryHigh = 3.0 * thresholdHigh;
        qreal thresholdExtreme = 4.0 * thresholdVeryHigh;

        int zoom = 0;
        m_selfInteraction = true;
        if ( remainingTime <= thresholdVeryLow ) {
            zoom = 3100;
            m_widget->zoomView( zoom );
        }

        if ( remainingTime > thresholdVeryLow && remainingTime < thresholdLow ) {
            m_widget->zoomOut();
        }

        if ( remainingTime >= thresholdHigh && remainingTime < thresholdVeryHigh ) {
            m_widget->zoomIn();
        }

        if ( remainingTime >= thresholdVeryHigh && remainingTime < thresholdExtreme ) {
            int zoomStep = 55;
            m_widget->zoomViewBy( zoomStep );
        }

        if ( remainingTime >= thresholdExtreme ) {
             zoom = 3000;
             m_widget->zoomView( zoom );
        }
        m_selfInteraction = false;
    }
}

void AdjustNavigationPrivate::centerOn( const GeoDataCoordinates &position )
{
    if ( m_widget && m_lastWidgetInteraction.elapsed() > 10 * 1000 ) {
        m_selfInteraction = true;
        m_widget->centerOn( position, false );
        m_selfInteraction = false;
    }
}

AdjustNavigation::AdjustNavigation( MarbleWidget *widget, QObject *parent )
    :QObject( parent ), d( new AdjustNavigationPrivate( widget ) )
{
    connect( widget, SIGNAL( visibleLatLonAltBoxChanged( GeoDataLatLonAltBox ) ),
             this, SLOT( inhibitAutoAdjustments() ) );

    d->m_tracking = widget->model()->positionTracking();
    connect( d->m_tracking, SIGNAL( gpsLocation( GeoDataCoordinates, qreal ) ),
                this, SLOT( adjust( GeoDataCoordinates, qreal ) ) );
}

AdjustNavigation::~AdjustNavigation()
{
    delete d;
}

void AdjustNavigation::adjust( const GeoDataCoordinates &position, qreal speed )
{
    if( !d->m_widget) {
        return;
    }

    d->m_gpsDirection = d->m_tracking->direction();
    d->m_gpsSpeed = speed;
    if( d->m_recenterMode && d->m_adjustZoom ) {
        if( d->m_recenterMode == AlwaysRecenter ) {
            d->centerOn( position );
        }
        else if( d->m_recenterMode == RecenterOnBorder ) {
            d->moveOnBorderToCenter( position, speed );
        }
        d->findIntersection( position );
    }
    else if( d->m_recenterMode == AlwaysRecenter ) {
        d->centerOn( position );
    }
    else if( d->m_recenterMode == RecenterOnBorder ) {
        d->moveOnBorderToCenter( position, speed );
    }
    else if( d->m_adjustZoom ) {
        d->findIntersection( position );
    }
}

void AdjustNavigation::setAutoZoom( bool autoZoom )
{
    d->m_adjustZoom = autoZoom;
    emit autoZoomToggled( autoZoom );
}

void AdjustNavigation::setRecenter( CenterMode recenterMode )
{
    d->m_recenterMode = recenterMode;
    emit recenterModeChanged( recenterMode );
}

void AdjustNavigation::inhibitAutoAdjustments()
{
    if ( !d->m_selfInteraction ) {
        d->m_lastWidgetInteraction.restart();
    }
}

AdjustNavigation::CenterMode AdjustNavigation::recenterMode() const
{
    return d->m_recenterMode;
}

bool AdjustNavigation::autoZoom() const
{
    return d->m_adjustZoom;
}

} // namespace Marble

#include "AdjustNavigation.moc"
<commit_msg>AdjustNavigation: m_widget can't be null<commit_after>//
// This file is part of the Marble Virtual Globe.
//
// This program is free software licensed under the GNU LGPL. You can
// find a copy of this license in LICENSE.txt in the top directory of
// the source code.
//
// Copyright 2010      Siddharth Srivastava <akssps011@gmail.com>
//

#include "AdjustNavigation.h"

#include "MarbleWidget.h"
#include "GeoDataCoordinates.h"
#include "PositionTracking.h"
#include "MarbleDebug.h"
#include "MarbleModel.h"
#include "ViewportParams.h"
#include "MarbleMath.h"
#include "global.h"

#include <QtGui/QPixmap>
#include <QtGui/QWidget>
#include <QtCore/QRect>
#include <QtCore/QPointF>
#include <QtCore/QTime>
#include <math.h>

namespace Marble {

class AdjustNavigationPrivate
{
public:

    MarbleWidget        *const m_widget;
    const PositionTracking *const m_tracking;
    qreal                m_gpsSpeed;
    qreal                m_gpsDirection;
    AdjustNavigation::CenterMode m_recenterMode;
    bool                 m_adjustZoom;
    QTime                m_lastWidgetInteraction;
    bool                 m_selfInteraction;

    /** Constructor */
    AdjustNavigationPrivate( MarbleWidget *widget );

    /**
     * @brief To center on when reaching custom defined border
     * @param position current gps location
     * @param speed optional speed argument
     */
     void moveOnBorderToCenter( const GeoDataCoordinates &position, qreal speed );

    /**
     * For calculating intersection point of projected LineString from
     * current gps location with the map border
     * @param position current gps location
     */
     void findIntersection( const GeoDataCoordinates &position );

    /**
     * @brief Adjust the zoom value of the map
     * @param currentPosition current location of the gps device
     * @param destination geoCoordinates of the point on the screen border where the gps device
     * would reach if allowed to move in that direction
     */
     void adjustZoom( const GeoDataCoordinates &currentPosition, const GeoDataCoordinates &destination );

     /**
       * Center the widget on the given position unless recentering is currently inhibited
       */
     void centerOn( const GeoDataCoordinates &position );
};

AdjustNavigationPrivate::AdjustNavigationPrivate( MarbleWidget *widget ) :
        m_widget( widget ),
        m_tracking( widget->model()->positionTracking() ),
        m_gpsSpeed( 0 ),
        m_gpsDirection( 0 ),
        m_recenterMode( AdjustNavigation::DontRecenter ),
        m_adjustZoom( 0 ),
        m_selfInteraction( false )
{
    m_lastWidgetInteraction.start();
}

void AdjustNavigationPrivate::moveOnBorderToCenter( const GeoDataCoordinates &position, qreal )
{
    qreal lon = 0.0;
    qreal lat = 0.0;

    position.geoCoordinates( lon, lat, GeoDataCoordinates::Degree );

    qreal x = 0.0;
    qreal y = 0.0;
    //recenter if initially the gps location is not visible on the screen
    if(!( m_widget->screenCoordinates( lon, lat, x, y ) ) ) {
         centerOn( position );
    }
    qreal centerLon = m_widget->centerLongitude();
    qreal centerLat = m_widget->centerLatitude();

    qreal centerX = 0.0;
    qreal centerY = 0.0;

    m_widget->screenCoordinates( centerLon, centerLat, centerX, centerY );

    const qreal borderRatio = 0.25;
    //defining the default border distance from map center
    int shiftX = qRound( centerX * borderRatio );
    int shiftY = qRound( centerY * borderRatio );

    QRect recenterBorderBound;
    recenterBorderBound.setCoords( centerX-shiftX, centerY-shiftY, centerX+shiftX,  centerY+shiftY );

    if( !recenterBorderBound.contains( x,y ) ) {
        centerOn( position );
    }
}

void AdjustNavigationPrivate::findIntersection( const GeoDataCoordinates &position )
{
    qreal track = m_gpsDirection;

    if ( track >= 360 ) {
        track = fmod( track,360.0 );
    }

    qreal lon = 0;
    qreal lat = 0;
    position.geoCoordinates( lon, lat, GeoDataCoordinates::Degree );

    qreal currentX = 0;
    qreal currentY = 0;

    if( !m_widget->screenCoordinates( lon, lat, currentX, currentY ) ) {
        centerOn( position );
    }
    else {
        ViewportParams const * const viewparams = m_widget->viewport();

        qreal width = viewparams->width();
        qreal height = viewparams->height();

        QPointF intercept;
        QPointF destinationHorizontal;
        QPointF destinationVertical;
        QPointF destination;

        qreal angle = 0.0;
        bool crossHorizontal =  false;
        bool crossVertical = false;

        //calculation of intersection point
        if( track > 0 && track < 90 ) {
            angle = track;

            //Intersection with line x = width
            intercept.setX( width - currentX );
            intercept.setY( intercept.x() / tan( angle ) );
            destinationVertical.setX( width );
            destinationVertical.setY( currentY-intercept.y() );

            //Intersection with line y = 0
            intercept.setY( currentY );
            intercept.setX( intercept.y() * tan( angle ) );
            destinationHorizontal.setX( currentX + intercept.x() );
            destinationHorizontal.setY( 0 );

            if ( destinationVertical.y() < 0 ) {
                crossHorizontal = true;
            }
            else if( destinationHorizontal.x() > width ) {
                crossVertical = true;
            }

        }
        else if( track > 270 && track < 360 ) {
            angle = track - 270;

            //Intersection with line y = 0
            intercept.setY( currentY );
            intercept.setX( intercept.y() / tan( angle ) );
            destinationHorizontal.setX( currentX - intercept.x() );
            destinationHorizontal.setY( 0 );

            //Intersection with line x = 0
            intercept.setX( currentX );
            intercept.setY( intercept.x() * tan( angle ) );
            destinationVertical.setY( currentY - intercept.y() );
            destinationVertical.setX( 0 );

            if( destinationHorizontal.x() > width ) {
                crossVertical = true;
            }
            else if( destinationVertical.y() < 0 ) {
                crossHorizontal = true;
            }

        }
        else if( track > 180 && track < 270  ) {
            angle = track - 180;

            //Intersection with line x = 0
            intercept.setX( currentX );
            intercept.setY( intercept.x() / tan( angle ) );
            destinationVertical.setY( currentY + intercept.y() );
            destinationVertical.setX( 0 );

            //Intersection with line y = height
            intercept.setY( currentY );
            intercept.setX( intercept.y() * tan( angle ) );
            destinationHorizontal.setX( currentX - intercept.x() );
            destinationHorizontal.setY( height );

            if ( destinationVertical.y() > height ) {
                crossHorizontal = true;
            }
            else if ( destinationHorizontal.x() < 0 ) {
                crossVertical = true;
            }

        }
        else if( track > 90 && track < 180  ) {
            angle = track - 90;

            //Intersection with line y = height
            intercept.setY( height - currentY );
            intercept.setX( intercept.y() / tan( angle ) );
            destinationHorizontal.setX( currentX + intercept.x() );
            destinationHorizontal.setY( height );

            //Intersection with line x = width
            intercept.setX( width - currentX );
            intercept.setY( intercept.x() * tan( angle ) );
            destinationVertical.setX( width );
            destinationVertical.setY( currentY + intercept.y() );

            if ( destinationHorizontal.x() > width ) {
                crossVertical = true;
            }
            else if( destinationVertical.y() > height ) {
                crossHorizontal = true;
            }

        }
        else {
            if( track == 0 ) {
                destinationHorizontal.setX( currentX );
                destinationHorizontal.setY( 0 );
                crossHorizontal = true;
            }
            else if( track == 90 ) {
                destinationVertical.setX( width );
                destinationVertical.setY( currentY );
                crossVertical = true;
            }
            else if( track == 190 ) {
                destinationHorizontal.setX( currentX );
                destinationHorizontal.setY( height );
                crossHorizontal = true;
            }
            else if( track == 270 ) {
                destinationVertical.setX( 0 );
                destinationVertical.setY( currentY );
                crossVertical = true;
            }
        }

        if ( crossHorizontal == true && crossVertical == false ) {
            destination.setX( destinationHorizontal.x() );
            destination.setY( destinationHorizontal.y() );
        }
        else if ( crossVertical == true && crossHorizontal == false ) {
            destination.setX( destinationVertical.x() );
            destination.setY( destinationVertical.y() );
        }

        qreal destinationLon = 0.0;
        qreal destinationLat = 0.0;
        m_widget->geoCoordinates( destination.x(), destination.y(), destinationLon, destinationLat,
                                  GeoDataCoordinates::Radian );
        GeoDataCoordinates destinationCoord( destinationLon, destinationLat, GeoDataCoordinates::Radian );
        adjustZoom( position, destinationCoord );
    }
}

void AdjustNavigationPrivate::adjustZoom( const GeoDataCoordinates &currentPosition, const GeoDataCoordinates &destination )
{
    qreal greatCircleDistance = distanceSphere( currentPosition, destination );
    qreal radius = m_widget->model()->planetRadius();
    qreal distance = greatCircleDistance *  radius;

    if( m_gpsSpeed != 0 ) {
        //time(in minutes) remaining to reach the border of the map
        qreal  remainingTime = ( distance / m_gpsSpeed ) * SEC2MIN;

        //tolerance time limits( in minutes ) before auto zooming
        qreal thresholdVeryLow = 0.5;
        qreal thresholdLow = 2.0 * thresholdVeryLow;
        qreal thresholdHigh = 12.0 * thresholdLow;
        qreal thresholdVeryHigh = 3.0 * thresholdHigh;
        qreal thresholdExtreme = 4.0 * thresholdVeryHigh;

        int zoom = 0;
        m_selfInteraction = true;
        if ( remainingTime <= thresholdVeryLow ) {
            zoom = 3100;
            m_widget->zoomView( zoom );
        }

        if ( remainingTime > thresholdVeryLow && remainingTime < thresholdLow ) {
            m_widget->zoomOut();
        }

        if ( remainingTime >= thresholdHigh && remainingTime < thresholdVeryHigh ) {
            m_widget->zoomIn();
        }

        if ( remainingTime >= thresholdVeryHigh && remainingTime < thresholdExtreme ) {
            int zoomStep = 55;
            m_widget->zoomViewBy( zoomStep );
        }

        if ( remainingTime >= thresholdExtreme ) {
             zoom = 3000;
             m_widget->zoomView( zoom );
        }
        m_selfInteraction = false;
    }
}

void AdjustNavigationPrivate::centerOn( const GeoDataCoordinates &position )
{
    if ( m_lastWidgetInteraction.elapsed() > 10 * 1000 ) {
        m_selfInteraction = true;
        m_widget->centerOn( position, false );
        m_selfInteraction = false;
    }
}

AdjustNavigation::AdjustNavigation( MarbleWidget *widget, QObject *parent )
    :QObject( parent ), d( new AdjustNavigationPrivate( widget ) )
{
    connect( widget, SIGNAL( visibleLatLonAltBoxChanged( GeoDataLatLonAltBox ) ),
             this, SLOT( inhibitAutoAdjustments() ) );

    connect( d->m_tracking, SIGNAL( gpsLocation( GeoDataCoordinates, qreal ) ),
                this, SLOT( adjust( GeoDataCoordinates, qreal ) ) );
}

AdjustNavigation::~AdjustNavigation()
{
    delete d;
}

void AdjustNavigation::adjust( const GeoDataCoordinates &position, qreal speed )
{
    d->m_gpsDirection = d->m_tracking->direction();
    d->m_gpsSpeed = speed;
    if( d->m_recenterMode && d->m_adjustZoom ) {
        if( d->m_recenterMode == AlwaysRecenter ) {
            d->centerOn( position );
        }
        else if( d->m_recenterMode == RecenterOnBorder ) {
            d->moveOnBorderToCenter( position, speed );
        }
        d->findIntersection( position );
    }
    else if( d->m_recenterMode == AlwaysRecenter ) {
        d->centerOn( position );
    }
    else if( d->m_recenterMode == RecenterOnBorder ) {
        d->moveOnBorderToCenter( position, speed );
    }
    else if( d->m_adjustZoom ) {
        d->findIntersection( position );
    }
}

void AdjustNavigation::setAutoZoom( bool autoZoom )
{
    d->m_adjustZoom = autoZoom;
    emit autoZoomToggled( autoZoom );
}

void AdjustNavigation::setRecenter( CenterMode recenterMode )
{
    d->m_recenterMode = recenterMode;
    emit recenterModeChanged( recenterMode );
}

void AdjustNavigation::inhibitAutoAdjustments()
{
    if ( !d->m_selfInteraction ) {
        d->m_lastWidgetInteraction.restart();
    }
}

AdjustNavigation::CenterMode AdjustNavigation::recenterMode() const
{
    return d->m_recenterMode;
}

bool AdjustNavigation::autoZoom() const
{
    return d->m_adjustZoom;
}

} // namespace Marble

#include "AdjustNavigation.moc"
<|endoftext|>


<commit_before>/* Siconos-Kernel, Copyright INRIA 2005-2012.
* Siconos is a program dedicated to modeling, simulation and control
* of non smooth dynamical systems.
* Siconos is a free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
* Siconos is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Siconos; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
*
* Contact: Vincent ACARY, siconos-team@lists.gforge.inria.fr
*/

#include "NewtonEulerFrom3DLocalFrameR.hpp"
#include "NewtonEulerDS.hpp"
#include <boost/math/quaternion.hpp>
#include "Interaction.hpp"
#include "BlockVector.hpp"

#include "op3x3.h"

// #define DEBUG_STDOUT
// #define DEBUG_MESSAGES
#include "debug.h"

//#define NEFC3D_DEBUG
/*
See devNotes.pdf for details. A detailed documentation is available in DevNotes.pdf: chapter 'NewtonEulerR: computation of \nabla q H'. Subsection 'Case FC3D: using the local frame local velocities'
*/
void NewtonEulerFrom3DLocalFrameR::initComponents(Interaction& inter, VectorOfBlockVectors& DSlink, VectorOfVectors& workV, VectorOfSMatrices& workM)
{
  NewtonEulerFrom1DLocalFrameR::initComponents(inter, DSlink, workV, workM);
  unsigned int qSize = 7 * (inter.getSizeOfDS() / 6);
  /*keep only the distance.*/
  _jachq.reset(new SimpleMatrix(3, qSize));

  /* VA 12/04/2016 All of what follows shoudl be put in WorkM*/
  _Mabs_C.reset(new SimpleMatrix(3, 3));
  _MObjToAbs.reset(new SimpleMatrix(3, 3));
  _AUX2.reset(new SimpleMatrix(3, 3));
  //  _isContact=1;
}
void NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts(SP::SiconosVector q1)
{

  double Nx = _Nc->getValue(0);
  double Ny = _Nc->getValue(1);
  double Nz = _Nc->getValue(2);
  double Px = _Pc1->getValue(0);
  double Py = _Pc1->getValue(1);
  double Pz = _Pc1->getValue(2);
  double G1x = q1->getValue(0);
  double G1y = q1->getValue(1);
  double G1z = q1->getValue(2);

#ifdef NEFC3D_DEBUG
  DEBUG_PRINTF("contact normal:\n");
  _Nc->display();
  DEBUG_PRINTF("contact point :\n");
  _Pc1->display();
  DEBUG_PRINTF("center of mass :\n");
  q1->display();
#endif

  assert(_Nc->norm2() >0.0 && "NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts. Normal vector not consistent ") ;

  double t[6];
  double * pt = t;
  orthoBaseFromVector(&Nx, &Ny, &Nz, pt, pt + 1, pt + 2, pt + 3, pt + 4, pt + 5);
  pt = t;
  _Mabs_C->setValue(0, 0, Nx);
  _Mabs_C->setValue(1, 0, *pt);
  _Mabs_C->setValue(2, 0, *(pt + 3));
  _Mabs_C->setValue(0, 1, Ny);
  _Mabs_C->setValue(1, 1, *(pt + 1));
  _Mabs_C->setValue(2, 1, *(pt + 4));
  _Mabs_C->setValue(0, 2, Nz);
  _Mabs_C->setValue(1, 2, *(pt + 2));
  _Mabs_C->setValue(2, 2, *(pt + 5));
#ifdef NEFC3D_DEBUG
  DEBUG_PRINTF("_Mabs_C:\n");
  _Mabs_C->display();
#endif
  _NPG1->zero();

  (*_NPG1)(0, 0) = 0;
  (*_NPG1)(0, 1) = -(G1z - Pz);
  (*_NPG1)(0, 2) = (G1y - Py);
  (*_NPG1)(1, 0) = (G1z - Pz);
  (*_NPG1)(1, 1) = 0;
  (*_NPG1)(1, 2) = -(G1x - Px);
  (*_NPG1)(2, 0) = -(G1y - Py);
  (*_NPG1)(2, 1) = (G1x - Px);
  (*_NPG1)(2, 2) = 0;

//  d1->computeMObjToAbs();
//  SimpleMatrix& Mobj1_abs = *d1->MObjToAbs();

  computeMObjToAbs(q1,_MObjToAbs);

  DEBUG_PRINT("NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts, _MObjToAbs:");
  DEBUG_EXPR(_MObjToAbs->display(););

  prod(*_NPG1, *_MObjToAbs, *_AUX1, true);

  DEBUG_PRINT("NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts, *_MObjToAbs:");
  DEBUG_EXPR_WE(_MObjToAbs->display(););
  DEBUG_PRINT("NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts, AUX1:");
  DEBUG_EXPR_WE(_AUX1->display(););
  DEBUG_PRINT("NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts, AUX2:");
  DEBUG_EXPR(_AUX2->display(););

  prod(*_Mabs_C, *_AUX1, *_AUX2, true);

  DEBUG_PRINT("NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts, Mabs_C:");
  DEBUG_EXPR_WE(_Mabs_C->display(););
  DEBUG_PRINT("NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts, AUX1:");
  DEBUG_EXPR_WE(_AUX1->display(););
  DEBUG_PRINT("NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts, AUX2:");
  DEBUG_EXPR_WE(_AUX2->display(););

  for (unsigned int ii = 0; ii < 3; ii++)
    for (unsigned int jj = 0; jj < 3; jj++)
      _jachqT->setValue(ii, jj, _Mabs_C->getValue(ii, jj));

  for (unsigned int ii = 0; ii < 3; ii++)
    for (unsigned int jj = 3; jj < 6; jj++)
      _jachqT->setValue(ii, jj, _AUX2->getValue(ii, jj - 3));

  DEBUG_PRINT("NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts, _jahcqT:\n");
  DEBUG_EXPR(_jachqT->display(););
  // DEBUG_EXPR_WE(
  //   SP::SimpleMatrix jaux(new SimpleMatrix(*_jachqT));
  //   jaux->trans();
  //   SP::SiconosVector v(new SiconosVector(3));
  //   SP::SiconosVector vRes(new SiconosVector(6));
  //   v->zero();
  //   v->setValue(0, 1);
  //   prod(*jaux, *v, *vRes, true);
  //   vRes->display();
  //   v->zero();
  //   v->setValue(1, 1);
  //   prod(*jaux, *v, *vRes, true);
  //   vRes->display();
  //   v->zero();
  //   v->setValue(2, 1);
  //   prod(*jaux, *v, *vRes, true);
  //   vRes->display();
  //   );
}

void NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts(SP::SiconosVector q1, SP::SiconosVector q2)
{
  double Nx = _Nc->getValue(0);
  double Ny = _Nc->getValue(1);
  double Nz = _Nc->getValue(2);
  double Px = _Pc1->getValue(0);
  double Py = _Pc1->getValue(1);
  double Pz = _Pc1->getValue(2);
  double G1x = q1->getValue(0);
  double G1y = q1->getValue(1);
  double G1z = q1->getValue(2);
  double G2x = q2->getValue(0);
  double G2y = q2->getValue(1);
  double G2z = q2->getValue(2);

  double t[6];
  double * pt = t;
  orthoBaseFromVector(&Nx, &Ny, &Nz, pt, pt + 1, pt + 2, pt + 3, pt + 4, pt + 5);
  pt = t;
  _Mabs_C->setValue(0, 0, Nx);
  _Mabs_C->setValue(1, 0, *pt);
  _Mabs_C->setValue(2, 0, *(pt + 3));
  _Mabs_C->setValue(0, 1, Ny);
  _Mabs_C->setValue(1, 1, *(pt + 1));
  _Mabs_C->setValue(2, 1, *(pt + 4));
  _Mabs_C->setValue(0, 2, Nz);
  _Mabs_C->setValue(1, 2, *(pt + 2));
  _Mabs_C->setValue(2, 2, *(pt + 5));

  _NPG1->zero();

  (*_NPG1)(0, 0) = 0;
  (*_NPG1)(0, 1) = -(G1z - Pz);
  (*_NPG1)(0, 2) = (G1y - Py);
  (*_NPG1)(1, 0) = (G1z - Pz);
  (*_NPG1)(1, 1) = 0;
  (*_NPG1)(1, 2) = -(G1x - Px);
  (*_NPG1)(2, 0) = -(G1y - Py);
  (*_NPG1)(2, 1) = (G1x - Px);
  (*_NPG1)(2, 2) = 0;

  _NPG2->zero();

  (*_NPG2)(0, 0) = 0;
  (*_NPG2)(0, 1) = -(G2z - Pz);
  (*_NPG2)(0, 2) = (G2y - Py);
  (*_NPG2)(1, 0) = (G2z - Pz);
  (*_NPG2)(1, 1) = 0;
  (*_NPG2)(1, 2) = -(G2x - Px);
  (*_NPG2)(2, 0) = -(G2y - Py);
  (*_NPG2)(2, 1) = (G2x - Px);
  (*_NPG2)(2, 2) = 0;

//  d1->computeMObjToAbs();
//  SimpleMatrix& Mobj1_abs = *d1->MObjToAbs();

  computeMObjToAbs(q1,_MObjToAbs);

  prod(*_NPG1, *_MObjToAbs, *_AUX1, true);
  prod(*_Mabs_C, *_AUX1, *_AUX2, true);

  for (unsigned int ii = 0; ii < 3; ii++)
    for (unsigned int jj = 0; jj < 3; jj++)
      _jachqT->setValue(ii, jj, _Mabs_C->getValue(ii, jj));

  for (unsigned int ii = 0; ii < 3; ii++)
    for (unsigned int jj = 3; jj < 6; jj++)
      _jachqT->setValue(ii, jj, _AUX2->getValue(ii, jj - 3));

//  d2->computeMObjToAbs();
//  SimpleMatrix& Mobj2_abs = *d2->MObjToAbs();

  computeMObjToAbs(q2,_MObjToAbs);

  prod(*_NPG2, *_MObjToAbs, *_AUX1, true);
  prod(*_Mabs_C, *_AUX1, *_AUX2, true);

  for (unsigned int ii = 0; ii < 3; ii++)
    for (unsigned int jj = 0; jj < 3; jj++)
      _jachqT->setValue(ii, jj + 6, -_Mabs_C->getValue(ii, jj));

  for (unsigned int ii = 0; ii < 3; ii++)
    for (unsigned int jj = 3; jj < 6; jj++)
      _jachqT->setValue(ii, jj + 6, -_AUX2->getValue(ii, jj - 3));

}

void NewtonEulerFrom3DLocalFrameR::computeJachqT(Interaction& inter, SP::DynamicalSystem ds1, SP::DynamicalSystem ds2)
{

  DEBUG_BEGIN("NewtonEulerFrom3DLocalFrameR::computeJachqT(Interaction& inter, SP::DynamicalSystem ds1, SP::DynamicalSystem ds2)\n");
  SP::NewtonEulerDS d1 =  std11::static_pointer_cast<NewtonEulerDS> (ds1);
  SP::NewtonEulerDS d2 =  std11::static_pointer_cast<NewtonEulerDS> (ds2);

  if(d1 != d2)
    FC3DcomputeJachqTFromContacts(d1->q(), d2->q());

  else
  {
    FC3DcomputeJachqTFromContacts(d1->q());
  }
  DEBUG_END("NewtonEulerFrom3DLocalFrameR::computeJachqT(Interaction& inter, SP::DynamicalSystem ds1, SP::DynamicalSystem ds2)\n");

}

void NewtonEulerFrom3DLocalFrameR::computeJachqT(Interaction& inter, VectorOfBlockVectors& DSlink)
{
  DEBUG_BEGIN("NewtonEulerFrom3DLocalFrameR::computeJachqT(Interaction& inter, VectorOfBlockVectors& DSlink) \n")
  SP::BlockVector BlockX = DSlink[NewtonEulerR::q0];
  if (inter.has2Bodies())
  {
    FC3DcomputeJachqTFromContacts((BlockX->getAllVect())[0], (BlockX->getAllVect())[1]);
  }
  else
  {
    FC3DcomputeJachqTFromContacts((BlockX->getAllVect())[0]);
  }

  DEBUG_END("NewtonEulerFrom3DLocalFrameR::computeJachqT(Interaction& inter, VectorOfBlockVectors& DSlink) \n");

}
<commit_msg>[kernel] update DEBUG_PRINT stuff../kernel/src/modelingTools/NewtonEulerR.cpp<commit_after>/* Siconos-Kernel, Copyright INRIA 2005-2012.
* Siconos is a program dedicated to modeling, simulation and control
* of non smooth dynamical systems.
* Siconos is a free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
* Siconos is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Siconos; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
*
* Contact: Vincent ACARY, siconos-team@lists.gforge.inria.fr
*/

#include "NewtonEulerFrom3DLocalFrameR.hpp"
#include "NewtonEulerDS.hpp"
#include <boost/math/quaternion.hpp>
#include "Interaction.hpp"
#include "BlockVector.hpp"

#include "op3x3.h"

// #define DEBUG_STDOUT
// #define DEBUG_MESSAGES
#include "debug.h"

/*
See devNotes.pdf for details. A detailed documentation is available in DevNotes.pdf: chapter 'NewtonEulerR: computation of \nabla q H'. Subsection 'Case FC3D: using the local frame local velocities'
*/
void NewtonEulerFrom3DLocalFrameR::initComponents(Interaction& inter, VectorOfBlockVectors& DSlink, VectorOfVectors& workV, VectorOfSMatrices& workM)
{
  NewtonEulerFrom1DLocalFrameR::initComponents(inter, DSlink, workV, workM);
  unsigned int qSize = 7 * (inter.getSizeOfDS() / 6);
  /*keep only the distance.*/
  _jachq.reset(new SimpleMatrix(3, qSize));

  /* VA 12/04/2016 All of what follows shoudl be put in WorkM*/
  _Mabs_C.reset(new SimpleMatrix(3, 3));
  _MObjToAbs.reset(new SimpleMatrix(3, 3));
  _AUX2.reset(new SimpleMatrix(3, 3));
  //  _isContact=1;
}
void NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts(SP::SiconosVector q1)
{

  double Nx = _Nc->getValue(0);
  double Ny = _Nc->getValue(1);
  double Nz = _Nc->getValue(2);
  double Px = _Pc1->getValue(0);
  double Py = _Pc1->getValue(1);
  double Pz = _Pc1->getValue(2);
  double G1x = q1->getValue(0);
  double G1y = q1->getValue(1);
  double G1z = q1->getValue(2);

  DEBUG_PRINT("contact normal:\n");
  DEBUG_EXPR(_Nc->display(););
  DEBUG_PRINT("contact point :\n");
  DEBUG_EXPR(_Pc1->display(););
  DEBUG_PRINT("center of mass :\n");
  DEBUG_EXPR(q1->display(););

  assert(_Nc->norm2() >0.0 && "NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts. Normal vector not consistent ") ;

  double t[6];
  double * pt = t;
  orthoBaseFromVector(&Nx, &Ny, &Nz, pt, pt + 1, pt + 2, pt + 3, pt + 4, pt + 5);
  pt = t;
  _Mabs_C->setValue(0, 0, Nx);
  _Mabs_C->setValue(1, 0, *pt);
  _Mabs_C->setValue(2, 0, *(pt + 3));
  _Mabs_C->setValue(0, 1, Ny);
  _Mabs_C->setValue(1, 1, *(pt + 1));
  _Mabs_C->setValue(2, 1, *(pt + 4));
  _Mabs_C->setValue(0, 2, Nz);
  _Mabs_C->setValue(1, 2, *(pt + 2));
  _Mabs_C->setValue(2, 2, *(pt + 5));

  DEBUG_PRINT("_Mabs_C:\n");
  DEBUG_EXPR(_Mabs_C->display(););

  _NPG1->zero();

  (*_NPG1)(0, 0) = 0;
  (*_NPG1)(0, 1) = -(G1z - Pz);
  (*_NPG1)(0, 2) = (G1y - Py);
  (*_NPG1)(1, 0) = (G1z - Pz);
  (*_NPG1)(1, 1) = 0;
  (*_NPG1)(1, 2) = -(G1x - Px);
  (*_NPG1)(2, 0) = -(G1y - Py);
  (*_NPG1)(2, 1) = (G1x - Px);
  (*_NPG1)(2, 2) = 0;

//  d1->computeMObjToAbs();
//  SimpleMatrix& Mobj1_abs = *d1->MObjToAbs();

  computeMObjToAbs(q1,_MObjToAbs);

  DEBUG_PRINT("NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts, _MObjToAbs:");
  DEBUG_EXPR(_MObjToAbs->display(););

  prod(*_NPG1, *_MObjToAbs, *_AUX1, true);

  DEBUG_PRINT("NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts, *_MObjToAbs:");
  DEBUG_EXPR_WE(_MObjToAbs->display(););
  DEBUG_PRINT("NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts, AUX1:");
  DEBUG_EXPR_WE(_AUX1->display(););
  DEBUG_PRINT("NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts, AUX2:");
  DEBUG_EXPR(_AUX2->display(););

  prod(*_Mabs_C, *_AUX1, *_AUX2, true);

  DEBUG_PRINT("NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts, Mabs_C:");
  DEBUG_EXPR_WE(_Mabs_C->display(););
  DEBUG_PRINT("NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts, AUX1:");
  DEBUG_EXPR_WE(_AUX1->display(););
  DEBUG_PRINT("NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts, AUX2:");
  DEBUG_EXPR_WE(_AUX2->display(););

  for (unsigned int ii = 0; ii < 3; ii++)
    for (unsigned int jj = 0; jj < 3; jj++)
      _jachqT->setValue(ii, jj, _Mabs_C->getValue(ii, jj));

  for (unsigned int ii = 0; ii < 3; ii++)
    for (unsigned int jj = 3; jj < 6; jj++)
      _jachqT->setValue(ii, jj, _AUX2->getValue(ii, jj - 3));

  DEBUG_PRINT("NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts, _jahcqT:\n");
  DEBUG_EXPR(_jachqT->display(););
  // DEBUG_EXPR_WE(
  //   SP::SimpleMatrix jaux(new SimpleMatrix(*_jachqT));
  //   jaux->trans();
  //   SP::SiconosVector v(new SiconosVector(3));
  //   SP::SiconosVector vRes(new SiconosVector(6));
  //   v->zero();
  //   v->setValue(0, 1);
  //   prod(*jaux, *v, *vRes, true);
  //   vRes->display();
  //   v->zero();
  //   v->setValue(1, 1);
  //   prod(*jaux, *v, *vRes, true);
  //   vRes->display();
  //   v->zero();
  //   v->setValue(2, 1);
  //   prod(*jaux, *v, *vRes, true);
  //   vRes->display();
  //   );
}

void NewtonEulerFrom3DLocalFrameR::FC3DcomputeJachqTFromContacts(SP::SiconosVector q1, SP::SiconosVector q2)
{
  double Nx = _Nc->getValue(0);
  double Ny = _Nc->getValue(1);
  double Nz = _Nc->getValue(2);
  double Px = _Pc1->getValue(0);
  double Py = _Pc1->getValue(1);
  double Pz = _Pc1->getValue(2);
  double G1x = q1->getValue(0);
  double G1y = q1->getValue(1);
  double G1z = q1->getValue(2);
  double G2x = q2->getValue(0);
  double G2y = q2->getValue(1);
  double G2z = q2->getValue(2);

  DEBUG_PRINT("contact normal:\n");
  DEBUG_EXPR(_Nc->display(););
  DEBUG_PRINT("contact point :\n");
  DEBUG_EXPR(_Pc1->display(););
  DEBUG_PRINT("center of mass :\n");
  DEBUG_EXPR(q1->display(););

  double t[6];
  double * pt = t;
  orthoBaseFromVector(&Nx, &Ny, &Nz, pt, pt + 1, pt + 2, pt + 3, pt + 4, pt + 5);
  pt = t;
  _Mabs_C->setValue(0, 0, Nx);
  _Mabs_C->setValue(1, 0, *pt);
  _Mabs_C->setValue(2, 0, *(pt + 3));
  _Mabs_C->setValue(0, 1, Ny);
  _Mabs_C->setValue(1, 1, *(pt + 1));
  _Mabs_C->setValue(2, 1, *(pt + 4));
  _Mabs_C->setValue(0, 2, Nz);
  _Mabs_C->setValue(1, 2, *(pt + 2));
  _Mabs_C->setValue(2, 2, *(pt + 5));

  _NPG1->zero();

  (*_NPG1)(0, 0) = 0;
  (*_NPG1)(0, 1) = -(G1z - Pz);
  (*_NPG1)(0, 2) = (G1y - Py);
  (*_NPG1)(1, 0) = (G1z - Pz);
  (*_NPG1)(1, 1) = 0;
  (*_NPG1)(1, 2) = -(G1x - Px);
  (*_NPG1)(2, 0) = -(G1y - Py);
  (*_NPG1)(2, 1) = (G1x - Px);
  (*_NPG1)(2, 2) = 0;

  _NPG2->zero();

  (*_NPG2)(0, 0) = 0;
  (*_NPG2)(0, 1) = -(G2z - Pz);
  (*_NPG2)(0, 2) = (G2y - Py);
  (*_NPG2)(1, 0) = (G2z - Pz);
  (*_NPG2)(1, 1) = 0;
  (*_NPG2)(1, 2) = -(G2x - Px);
  (*_NPG2)(2, 0) = -(G2y - Py);
  (*_NPG2)(2, 1) = (G2x - Px);
  (*_NPG2)(2, 2) = 0;

//  d1->computeMObjToAbs();
//  SimpleMatrix& Mobj1_abs = *d1->MObjToAbs();

  computeMObjToAbs(q1,_MObjToAbs);

  prod(*_NPG1, *_MObjToAbs, *_AUX1, true);
  prod(*_Mabs_C, *_AUX1, *_AUX2, true);

  for (unsigned int ii = 0; ii < 3; ii++)
    for (unsigned int jj = 0; jj < 3; jj++)
      _jachqT->setValue(ii, jj, _Mabs_C->getValue(ii, jj));

  for (unsigned int ii = 0; ii < 3; ii++)
    for (unsigned int jj = 3; jj < 6; jj++)
      _jachqT->setValue(ii, jj, _AUX2->getValue(ii, jj - 3));

//  d2->computeMObjToAbs();
//  SimpleMatrix& Mobj2_abs = *d2->MObjToAbs();

  computeMObjToAbs(q2,_MObjToAbs);

  prod(*_NPG2, *_MObjToAbs, *_AUX1, true);
  prod(*_Mabs_C, *_AUX1, *_AUX2, true);

  for (unsigned int ii = 0; ii < 3; ii++)
    for (unsigned int jj = 0; jj < 3; jj++)
      _jachqT->setValue(ii, jj + 6, -_Mabs_C->getValue(ii, jj));

  for (unsigned int ii = 0; ii < 3; ii++)
    for (unsigned int jj = 3; jj < 6; jj++)
      _jachqT->setValue(ii, jj + 6, -_AUX2->getValue(ii, jj - 3));

}

void NewtonEulerFrom3DLocalFrameR::computeJachqT(Interaction& inter, SP::DynamicalSystem ds1, SP::DynamicalSystem ds2)
{

  DEBUG_BEGIN("NewtonEulerFrom3DLocalFrameR::computeJachqT(Interaction& inter, SP::DynamicalSystem ds1, SP::DynamicalSystem ds2)\n");
  SP::NewtonEulerDS d1 =  std11::static_pointer_cast<NewtonEulerDS> (ds1);
  SP::NewtonEulerDS d2 =  std11::static_pointer_cast<NewtonEulerDS> (ds2);

  if(d1 != d2)
    FC3DcomputeJachqTFromContacts(d1->q(), d2->q());

  else
  {
    FC3DcomputeJachqTFromContacts(d1->q());
  }
  DEBUG_END("NewtonEulerFrom3DLocalFrameR::computeJachqT(Interaction& inter, SP::DynamicalSystem ds1, SP::DynamicalSystem ds2)\n");

}

void NewtonEulerFrom3DLocalFrameR::computeJachqT(Interaction& inter, VectorOfBlockVectors& DSlink)
{
  DEBUG_BEGIN("NewtonEulerFrom3DLocalFrameR::computeJachqT(Interaction& inter, VectorOfBlockVectors& DSlink) \n")
  SP::BlockVector BlockX = DSlink[NewtonEulerR::q0];
  if (inter.has2Bodies())
  {
    FC3DcomputeJachqTFromContacts((BlockX->getAllVect())[0], (BlockX->getAllVect())[1]);
  }
  else
  {
    FC3DcomputeJachqTFromContacts((BlockX->getAllVect())[0]);
  }

  DEBUG_END("NewtonEulerFrom3DLocalFrameR::computeJachqT(Interaction& inter, VectorOfBlockVectors& DSlink) \n");

}
<|endoftext|>