<commit_before>/*
 * Copyright (c) 2013, Oracle and/or its affiliates. All rights reserved.
 * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
 *
 * This code is free software; you can redistribute it and/or modify it
 * under the terms of the GNU General Public License version 2 only, as
 * published by the Free Software Foundation.
 *
 * This code 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
 * version 2 for more details (a copy is included in the LICENSE file that
 * accompanied this code).
 *
 * You should have received a copy of the GNU General Public License version
 * 2 along with this work; if not, write to the Free Software Foundation,
 * Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA.
 *
 * Please contact Oracle, 500 Oracle Parkway, Redwood Shores, CA 94065 USA
 * or visit www.oracle.com if you need additional information or have any
 * questions.
 *
 */

#include "precompiled.hpp"
#include "runtime/javaCalls.hpp"
#include "runtime/gpu.hpp"
#include "utilities/globalDefinitions.hpp"
#include "utilities/ostream.hpp"
#include "memory/allocation.hpp"
#include "memory/allocation.inline.hpp"
#include "graal/graalEnv.hpp"
#include "graal/graalCompiler.hpp"
#include "graal/graalJavaAccess.hpp"
#include "hsailKernelArguments.hpp"

// Entry to GPU native method implementation that transitions current thread to '_thread_in_vm'.
#define GPU_VMENTRY(result_type, name, signature) \
  JNIEXPORT result_type JNICALL name signature { \
  GRAAL_VM_ENTRY_MARK; \

// Entry to GPU native method implementation that calls a JNI function
// and hence cannot transition current thread to '_thread_in_vm'.
#define GPU_ENTRY(result_type, name, signature) \
  JNIEXPORT result_type JNICALL name signature { \

#define GPU_END }

#define CC (char*)  /*cast a literal from (const char*)*/
#define FN_PTR(f) CAST_FROM_FN_PTR(void*, &(f))

#define OBJECT                "Ljava/lang/Object;"
#define STRING                "Ljava/lang/String;"
#define HS_INSTALLED_CODE     "Lcom/oracle/graal/hotspot/meta/HotSpotInstalledCode;"

//  public native void executeKernel(HotSpotNmethod kernel, int jobSize, int i, int j, Object[] args) throws InvalidInstalledCodeException;

JNINativeMethod gpu::Hsail::HSAIL_methods[] = {
  {CC"initialize",       CC"()Z",                               FN_PTR(gpu::Hsail::initialize)},
  {CC"generateKernel",   CC"([B" STRING ")J",                   FN_PTR(gpu::Hsail::generate_kernel)},
  {CC"executeKernel0",   CC"("HS_INSTALLED_CODE"I["OBJECT")Z",  FN_PTR(gpu::Hsail::execute_kernel_void_1d)},
};

void * gpu::Hsail::_device_context = NULL;

gpu::Hsail::okra_create_context_func_t  gpu::Hsail::_okra_create_context;
gpu::Hsail::okra_create_kernel_func_t   gpu::Hsail::_okra_create_kernel;
gpu::Hsail::okra_push_object_func_t     gpu::Hsail::_okra_push_object;
gpu::Hsail::okra_push_boolean_func_t    gpu::Hsail::_okra_push_boolean;
gpu::Hsail::okra_push_byte_func_t       gpu::Hsail::_okra_push_byte;
gpu::Hsail::okra_push_double_func_t     gpu::Hsail::_okra_push_double;
gpu::Hsail::okra_push_float_func_t      gpu::Hsail::_okra_push_float;
gpu::Hsail::okra_push_int_func_t        gpu::Hsail::_okra_push_int;
gpu::Hsail::okra_push_long_func_t       gpu::Hsail::_okra_push_long;
gpu::Hsail::okra_execute_with_range_func_t    gpu::Hsail::_okra_execute_with_range;
gpu::Hsail::okra_clearargs_func_t       gpu::Hsail::_okra_clearargs;
gpu::Hsail::okra_register_heap_func_t   gpu::Hsail::_okra_register_heap;

void gpu::Hsail::register_heap() {
  // After the okra functions are set up and the heap is initialized, register the java heap with HSA
  guarantee(Universe::heap() != NULL, "heap should be there by now.");
  if (TraceGPUInteraction) {
    tty->print_cr("[HSAIL] heap=" PTR_FORMAT, Universe::heap());
    tty->print_cr("[HSAIL] base=0x%08x, capacity=%ld", Universe::heap()->base(), Universe::heap()->capacity());
  }
  _okra_register_heap(Universe::heap()->base(), Universe::heap()->capacity());
}

GPU_VMENTRY(jboolean, gpu::Hsail::execute_kernel_void_1d, (JNIEnv* env, jclass, jobject kernel_handle, jint dimX, jobject args_handle))

  ResourceMark rm;
  jlong nmethodValue = HotSpotInstalledCode::codeBlob(kernel_handle);
  if (nmethodValue == 0) {
    SharedRuntime::throw_and_post_jvmti_exception(JavaThread::current(), vmSymbols::com_oracle_graal_api_code_InvalidInstalledCodeException(), NULL);
  }
  nmethod* nm = (nmethod*) (address) nmethodValue;
  methodHandle mh = nm->method();
  Symbol* signature = mh->signature();

  void* kernel = (void*) HotSpotInstalledCode::codeStart(kernel_handle);
  if (kernel == NULL) {
    SharedRuntime::throw_and_post_jvmti_exception(JavaThread::current(), vmSymbols::com_oracle_graal_api_code_InvalidInstalledCodeException(), NULL);
  }

  objArrayOop args = (objArrayOop) JNIHandles::resolve(args_handle);

  // Reset the kernel arguments
  _okra_clearargs(kernel);

  // This object sets up the kernel arguments
  HSAILKernelArguments hka((address) kernel, mh->signature(), args, mh->is_static());

  // Run the kernel
  return _okra_execute_with_range(kernel, dimX);
GPU_END

GPU_ENTRY(jlong, gpu::Hsail::generate_kernel, (JNIEnv *env, jclass, jbyteArray code_handle, jstring name_handle))
  guarantee(_okra_create_kernel != NULL, "[HSAIL] Okra not linked");
  ResourceMark rm;
  jsize name_len = env->GetStringLength(name_handle);
  jsize code_len = env->GetArrayLength(code_handle);

  char* name = NEW_RESOURCE_ARRAY(char, name_len + 1);
  unsigned char *code = NEW_RESOURCE_ARRAY(unsigned char, code_len + 1);

  code[code_len] = 0;
  name[name_len] = 0;

  env->GetByteArrayRegion(code_handle, 0, code_len, (jbyte*) code);
  env->GetStringUTFRegion(name_handle, 0, name_len, name);

  register_heap();

  // The kernel entrypoint is always run for the time being
  const char* entryPointName = "&run";

  _device_context = _okra_create_context();

  return (jlong) _okra_create_kernel(_device_context, code, entryPointName);
GPU_END

#if defined(LINUX)
static const char* okra_library_name = "libokra_x86_64.so";
#else
static char const* okra_library_name = NULL;
#endif

#define STRINGIFY(x)     #x

#define LOOKUP_OKRA_FUNCTION(name, alias)  \
  _##alias =                               \
    CAST_TO_FN_PTR(alias##_func_t, os::dll_lookup(handle, STRINGIFY(name))); \
  if (_##alias == NULL) {      \
  tty->print_cr("[HSAIL] ***** Error: Failed to lookup %s in %s, wrong version of OKRA?", STRINGIFY(name), okra_library_name); \
        return false; \
  } \

GPU_ENTRY(jboolean, gpu::Hsail::initialize, (JNIEnv *env, jclass))
  if (okra_library_name == NULL) {
    if (TraceGPUInteraction) {
      tty->print_cr("Unsupported HSAIL platform");
    }
    return false;
  }

  // here we know we have a valid okra_library_name to try to load
  char ebuf[O_BUFLEN];
  if (TraceGPUInteraction) {
      tty->print_cr("[HSAIL] library is %s", okra_library_name);
  }
  void *handle = os::dll_load(okra_library_name, ebuf, O_BUFLEN);
  // try alternate location if env variable set
  char *okra_lib_name_from_env_var = getenv("_OKRA_SIM_LIB_PATH_");
  if ((handle == NULL) && (okra_lib_name_from_env_var != NULL)) {
    handle = os::dll_load(okra_lib_name_from_env_var, ebuf, O_BUFLEN);
    if ((handle != NULL) && TraceGPUInteraction) {
      tty->print_cr("[HSAIL] using _OKRA_SIM_LIB_PATH_=%s", getenv("_OKRA_SIM_LIB_PATH_"));
    }
  }

  if (handle == NULL) {
    // Unable to dlopen okra
    if (TraceGPUInteraction) {
      tty->print_cr("[HSAIL] library load failed.");
    }
    return false;
  }

  guarantee(_okra_create_context == NULL, "cannot repeat GPU initialization");

  // at this point we know handle is valid and we can lookup the functions
  LOOKUP_OKRA_FUNCTION(okra_create_context, okra_create_context);
  LOOKUP_OKRA_FUNCTION(okra_create_kernel, okra_create_kernel);
  LOOKUP_OKRA_FUNCTION(okra_push_object, okra_push_object);
  LOOKUP_OKRA_FUNCTION(okra_push_boolean, okra_push_boolean);
  LOOKUP_OKRA_FUNCTION(okra_push_byte, okra_push_byte);
  LOOKUP_OKRA_FUNCTION(okra_push_double, okra_push_double);
  LOOKUP_OKRA_FUNCTION(okra_push_float, okra_push_float);
  LOOKUP_OKRA_FUNCTION(okra_push_int, okra_push_int);
  LOOKUP_OKRA_FUNCTION(okra_push_long, okra_push_long);
  LOOKUP_OKRA_FUNCTION(okra_execute_with_range, okra_execute_with_range);
  LOOKUP_OKRA_FUNCTION(okra_clearargs, okra_clearargs);
  LOOKUP_OKRA_FUNCTION(okra_register_heap, okra_register_heap);
  // if we made it this far, real success

  gpu::initialized_gpu("Okra");

  return true;
GPU_END

bool gpu::Hsail::register_natives(JNIEnv* env) {
  jclass klass = env->FindClass("com/oracle/graal/hotspot/hsail/HSAILHotSpotBackend");
  if (klass == NULL) {
    if (TraceGPUInteraction) {
      tty->print_cr("HSAILHotSpotBackend class not found");
    }
    return false;
  }
  jint status = env->RegisterNatives(klass, HSAIL_methods, sizeof(HSAIL_methods) / sizeof(JNINativeMethod));
  if (status != JNI_OK) {
    if (TraceGPUInteraction) {
      tty->print_cr("Error registering natives for HSAILHotSpotBackend: %d", status);
    }
    return false;
  }
  return true;
}
<commit_msg>reverted removal of Okra Windows DLL name<commit_after>/*
 * Copyright (c) 2013, Oracle and/or its affiliates. All rights reserved.
 * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
 *
 * This code is free software; you can redistribute it and/or modify it
 * under the terms of the GNU General Public License version 2 only, as
 * published by the Free Software Foundation.
 *
 * This code 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
 * version 2 for more details (a copy is included in the LICENSE file that
 * accompanied this code).
 *
 * You should have received a copy of the GNU General Public License version
 * 2 along with this work; if not, write to the Free Software Foundation,
 * Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA.
 *
 * Please contact Oracle, 500 Oracle Parkway, Redwood Shores, CA 94065 USA
 * or visit www.oracle.com if you need additional information or have any
 * questions.
 *
 */

#include "precompiled.hpp"
#include "runtime/javaCalls.hpp"
#include "runtime/gpu.hpp"
#include "utilities/globalDefinitions.hpp"
#include "utilities/ostream.hpp"
#include "memory/allocation.hpp"
#include "memory/allocation.inline.hpp"
#include "graal/graalEnv.hpp"
#include "graal/graalCompiler.hpp"
#include "graal/graalJavaAccess.hpp"
#include "hsailKernelArguments.hpp"

// Entry to GPU native method implementation that transitions current thread to '_thread_in_vm'.
#define GPU_VMENTRY(result_type, name, signature) \
  JNIEXPORT result_type JNICALL name signature { \
  GRAAL_VM_ENTRY_MARK; \

// Entry to GPU native method implementation that calls a JNI function
// and hence cannot transition current thread to '_thread_in_vm'.
#define GPU_ENTRY(result_type, name, signature) \
  JNIEXPORT result_type JNICALL name signature { \

#define GPU_END }

#define CC (char*)  /*cast a literal from (const char*)*/
#define FN_PTR(f) CAST_FROM_FN_PTR(void*, &(f))

#define OBJECT                "Ljava/lang/Object;"
#define STRING                "Ljava/lang/String;"
#define HS_INSTALLED_CODE     "Lcom/oracle/graal/hotspot/meta/HotSpotInstalledCode;"

//  public native void executeKernel(HotSpotNmethod kernel, int jobSize, int i, int j, Object[] args) throws InvalidInstalledCodeException;

JNINativeMethod gpu::Hsail::HSAIL_methods[] = {
  {CC"initialize",       CC"()Z",                               FN_PTR(gpu::Hsail::initialize)},
  {CC"generateKernel",   CC"([B" STRING ")J",                   FN_PTR(gpu::Hsail::generate_kernel)},
  {CC"executeKernel0",   CC"("HS_INSTALLED_CODE"I["OBJECT")Z",  FN_PTR(gpu::Hsail::execute_kernel_void_1d)},
};

void * gpu::Hsail::_device_context = NULL;

gpu::Hsail::okra_create_context_func_t  gpu::Hsail::_okra_create_context;
gpu::Hsail::okra_create_kernel_func_t   gpu::Hsail::_okra_create_kernel;
gpu::Hsail::okra_push_object_func_t     gpu::Hsail::_okra_push_object;
gpu::Hsail::okra_push_boolean_func_t    gpu::Hsail::_okra_push_boolean;
gpu::Hsail::okra_push_byte_func_t       gpu::Hsail::_okra_push_byte;
gpu::Hsail::okra_push_double_func_t     gpu::Hsail::_okra_push_double;
gpu::Hsail::okra_push_float_func_t      gpu::Hsail::_okra_push_float;
gpu::Hsail::okra_push_int_func_t        gpu::Hsail::_okra_push_int;
gpu::Hsail::okra_push_long_func_t       gpu::Hsail::_okra_push_long;
gpu::Hsail::okra_execute_with_range_func_t    gpu::Hsail::_okra_execute_with_range;
gpu::Hsail::okra_clearargs_func_t       gpu::Hsail::_okra_clearargs;
gpu::Hsail::okra_register_heap_func_t   gpu::Hsail::_okra_register_heap;

void gpu::Hsail::register_heap() {
  // After the okra functions are set up and the heap is initialized, register the java heap with HSA
  guarantee(Universe::heap() != NULL, "heap should be there by now.");
  if (TraceGPUInteraction) {
    tty->print_cr("[HSAIL] heap=" PTR_FORMAT, Universe::heap());
    tty->print_cr("[HSAIL] base=0x%08x, capacity=%ld", Universe::heap()->base(), Universe::heap()->capacity());
  }
  _okra_register_heap(Universe::heap()->base(), Universe::heap()->capacity());
}

GPU_VMENTRY(jboolean, gpu::Hsail::execute_kernel_void_1d, (JNIEnv* env, jclass, jobject kernel_handle, jint dimX, jobject args_handle))

  ResourceMark rm;
  jlong nmethodValue = HotSpotInstalledCode::codeBlob(kernel_handle);
  if (nmethodValue == 0) {
    SharedRuntime::throw_and_post_jvmti_exception(JavaThread::current(), vmSymbols::com_oracle_graal_api_code_InvalidInstalledCodeException(), NULL);
  }
  nmethod* nm = (nmethod*) (address) nmethodValue;
  methodHandle mh = nm->method();
  Symbol* signature = mh->signature();

  void* kernel = (void*) HotSpotInstalledCode::codeStart(kernel_handle);
  if (kernel == NULL) {
    SharedRuntime::throw_and_post_jvmti_exception(JavaThread::current(), vmSymbols::com_oracle_graal_api_code_InvalidInstalledCodeException(), NULL);
  }

  objArrayOop args = (objArrayOop) JNIHandles::resolve(args_handle);

  // Reset the kernel arguments
  _okra_clearargs(kernel);

  // This object sets up the kernel arguments
  HSAILKernelArguments hka((address) kernel, mh->signature(), args, mh->is_static());

  // Run the kernel
  return _okra_execute_with_range(kernel, dimX);
GPU_END

GPU_ENTRY(jlong, gpu::Hsail::generate_kernel, (JNIEnv *env, jclass, jbyteArray code_handle, jstring name_handle))
  guarantee(_okra_create_kernel != NULL, "[HSAIL] Okra not linked");
  ResourceMark rm;
  jsize name_len = env->GetStringLength(name_handle);
  jsize code_len = env->GetArrayLength(code_handle);

  char* name = NEW_RESOURCE_ARRAY(char, name_len + 1);
  unsigned char *code = NEW_RESOURCE_ARRAY(unsigned char, code_len + 1);

  code[code_len] = 0;
  name[name_len] = 0;

  env->GetByteArrayRegion(code_handle, 0, code_len, (jbyte*) code);
  env->GetStringUTFRegion(name_handle, 0, name_len, name);

  register_heap();

  // The kernel entrypoint is always run for the time being
  const char* entryPointName = "&run";

  _device_context = _okra_create_context();

  return (jlong) _okra_create_kernel(_device_context, code, entryPointName);
GPU_END

#if defined(LINUX)
static const char* okra_library_name = "libokra_x86_64.so";
#elif defined(_WINDOWS)
static char const* okra_library_name = "okra_x86_64.dll";
#else
static char const* okra_library_name = NULL;
#endif

#define STRINGIFY(x)     #x

#define LOOKUP_OKRA_FUNCTION(name, alias)  \
  _##alias =                               \
    CAST_TO_FN_PTR(alias##_func_t, os::dll_lookup(handle, STRINGIFY(name))); \
  if (_##alias == NULL) {      \
  tty->print_cr("[HSAIL] ***** Error: Failed to lookup %s in %s, wrong version of OKRA?", STRINGIFY(name), okra_library_name); \
        return false; \
  } \

GPU_ENTRY(jboolean, gpu::Hsail::initialize, (JNIEnv *env, jclass))
  if (okra_library_name == NULL) {
    if (TraceGPUInteraction) {
      tty->print_cr("Unsupported HSAIL platform");
    }
    return false;
  }

  // here we know we have a valid okra_library_name to try to load
  char ebuf[O_BUFLEN];
  if (TraceGPUInteraction) {
      tty->print_cr("[HSAIL] library is %s", okra_library_name);
  }
  void *handle = os::dll_load(okra_library_name, ebuf, O_BUFLEN);
  // try alternate location if env variable set
  char *okra_lib_name_from_env_var = getenv("_OKRA_SIM_LIB_PATH_");
  if ((handle == NULL) && (okra_lib_name_from_env_var != NULL)) {
    handle = os::dll_load(okra_lib_name_from_env_var, ebuf, O_BUFLEN);
    if ((handle != NULL) && TraceGPUInteraction) {
      tty->print_cr("[HSAIL] using _OKRA_SIM_LIB_PATH_=%s", getenv("_OKRA_SIM_LIB_PATH_"));
    }
  }

  if (handle == NULL) {
    // Unable to dlopen okra
    if (TraceGPUInteraction) {
      tty->print_cr("[HSAIL] library load failed.");
    }
    return false;
  }

  guarantee(_okra_create_context == NULL, "cannot repeat GPU initialization");

  // at this point we know handle is valid and we can lookup the functions
  LOOKUP_OKRA_FUNCTION(okra_create_context, okra_create_context);
  LOOKUP_OKRA_FUNCTION(okra_create_kernel, okra_create_kernel);
  LOOKUP_OKRA_FUNCTION(okra_push_object, okra_push_object);
  LOOKUP_OKRA_FUNCTION(okra_push_boolean, okra_push_boolean);
  LOOKUP_OKRA_FUNCTION(okra_push_byte, okra_push_byte);
  LOOKUP_OKRA_FUNCTION(okra_push_double, okra_push_double);
  LOOKUP_OKRA_FUNCTION(okra_push_float, okra_push_float);
  LOOKUP_OKRA_FUNCTION(okra_push_int, okra_push_int);
  LOOKUP_OKRA_FUNCTION(okra_push_long, okra_push_long);
  LOOKUP_OKRA_FUNCTION(okra_execute_with_range, okra_execute_with_range);
  LOOKUP_OKRA_FUNCTION(okra_clearargs, okra_clearargs);
  LOOKUP_OKRA_FUNCTION(okra_register_heap, okra_register_heap);
  // if we made it this far, real success

  gpu::initialized_gpu("Okra");

  return true;
GPU_END

bool gpu::Hsail::register_natives(JNIEnv* env) {
  jclass klass = env->FindClass("com/oracle/graal/hotspot/hsail/HSAILHotSpotBackend");
  if (klass == NULL) {
    if (TraceGPUInteraction) {
      tty->print_cr("HSAILHotSpotBackend class not found");
    }
    return false;
  }
  jint status = env->RegisterNatives(klass, HSAIL_methods, sizeof(HSAIL_methods) / sizeof(JNINativeMethod));
  if (status != JNI_OK) {
    if (TraceGPUInteraction) {
      tty->print_cr("Error registering natives for HSAILHotSpotBackend: %d", status);
    }
    return false;
  }
  return true;
}
<|endoftext|>


<commit_before>/* -*-c++-*- OpenSceneGraph - Copyright (C) 1998-2006 Robert Osfield
 *
 * This library is open source and may be redistributed and/or modified under
 * the terms of the OpenSceneGraph Public License (OSGPL) version 0.0 or
 * (at your option) any later version.  The full license is in LICENSE file
 * included with this distribution, and on the openscenegraph.org website.
 *
 * 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
 * OpenSceneGraph Public License for more details.
*/

#include <osgGA/GUIEventAdapter>

using namespace osgGA;

GUIEventAdapter::GUIEventAdapter():
    _eventType(NONE),
    _time(0.0),
    _key(0),
    _button(0),
    _Xmin(0.0),
    _Xmax(1.0),
    _Ymin(0.0),
    _Ymax(1.0),
    _mx(0.5),
    _my(0.5),
    _pressure(0.0),
    _buttonMask(0),
    _modKeyMask(0),
    _scrollingMotion(SCROLL_NONE),
    _scrollingDeltaX(0),
    _scrollingDeltaY(0),
    _mouseYOrientation(Y_INCREASING_DOWNWARDS),
    _tabletPointerType(UNKNOWN)
{}

GUIEventAdapter::GUIEventAdapter(const GUIEventAdapter& rhs):
    Referenced(),
    _eventType(rhs._eventType),
    _time(rhs._time),
    _key(rhs._key),
    _button(rhs._button),
    _Xmin(rhs._Xmin),
    _Xmax(rhs._Xmax),
    _Ymin(rhs._Ymin),
    _Ymax(rhs._Ymax),
    _mx(rhs._mx),
    _my(rhs._my),
    _pressure(rhs._pressure),
    _buttonMask(rhs._buttonMask),
    _modKeyMask(rhs._modKeyMask),
    _scrollingMotion(rhs._scrollingMotion),
    _scrollingDeltaX(rhs._scrollingDeltaX),
    _scrollingDeltaY(rhs._scrollingDeltaY),
    _mouseYOrientation(rhs._mouseYOrientation),
    _tabletPointerType(rhs._tabletPointerType)
{}

GUIEventAdapter::~GUIEventAdapter()
{
}

void GUIEventAdapter::setWindowSize(float Xmin, float Ymin, float Xmax, float Ymax)
{
    _Xmin = Xmin;
    _Ymin = Ymin;
    _Xmax = Xmax;
    _Ymax = Ymax;
}
<commit_msg>Added osg:: to Referenced() to fix IRIX build.<commit_after>/* -*-c++-*- OpenSceneGraph - Copyright (C) 1998-2006 Robert Osfield
 *
 * This library is open source and may be redistributed and/or modified under
 * the terms of the OpenSceneGraph Public License (OSGPL) version 0.0 or
 * (at your option) any later version.  The full license is in LICENSE file
 * included with this distribution, and on the openscenegraph.org website.
 *
 * 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
 * OpenSceneGraph Public License for more details.
*/

#include <osgGA/GUIEventAdapter>

using namespace osgGA;

GUIEventAdapter::GUIEventAdapter():
    _eventType(NONE),
    _time(0.0),
    _key(0),
    _button(0),
    _Xmin(0.0),
    _Xmax(1.0),
    _Ymin(0.0),
    _Ymax(1.0),
    _mx(0.5),
    _my(0.5),
    _pressure(0.0),
    _buttonMask(0),
    _modKeyMask(0),
    _scrollingMotion(SCROLL_NONE),
    _scrollingDeltaX(0),
    _scrollingDeltaY(0),
    _mouseYOrientation(Y_INCREASING_DOWNWARDS),
    _tabletPointerType(UNKNOWN)
{}

GUIEventAdapter::GUIEventAdapter(const GUIEventAdapter& rhs):
    osg::Referenced(),
    _eventType(rhs._eventType),
    _time(rhs._time),
    _key(rhs._key),
    _button(rhs._button),
    _Xmin(rhs._Xmin),
    _Xmax(rhs._Xmax),
    _Ymin(rhs._Ymin),
    _Ymax(rhs._Ymax),
    _mx(rhs._mx),
    _my(rhs._my),
    _pressure(rhs._pressure),
    _buttonMask(rhs._buttonMask),
    _modKeyMask(rhs._modKeyMask),
    _scrollingMotion(rhs._scrollingMotion),
    _scrollingDeltaX(rhs._scrollingDeltaX),
    _scrollingDeltaY(rhs._scrollingDeltaY),
    _mouseYOrientation(rhs._mouseYOrientation),
    _tabletPointerType(rhs._tabletPointerType)
{}

GUIEventAdapter::~GUIEventAdapter()
{
}

void GUIEventAdapter::setWindowSize(float Xmin, float Ymin, float Xmax, float Ymax)
{
    _Xmin = Xmin;
    _Ymin = Ymin;
    _Xmax = Xmax;
    _Ymax = Ymax;
}
<|endoftext|>


<commit_before>#pragma once

#include <algorithm>
#include <cstdint>
#include <string>

#include "hecl/CVarManager.hpp"

#undef min
#undef max

namespace hecl {

using namespace std::literals;

#ifdef _WIN32
#define DEFAULT_GRAPHICS_API "D3D11"sv
#elif defined(__APPLE__)
#define DEFAULT_GRAPHICS_API "Metal"sv
#else
#define DEFAULT_GRAPHICS_API "Vulkan"sv
#endif

struct CVarCommons {
  CVarManager& m_mgr;
  CVar* m_graphicsApi = nullptr;
  CVar* m_drawSamples = nullptr;
  CVar* m_texAnisotropy = nullptr;
  CVar* m_deepColor = nullptr;
  CVar* m_variableDt = nullptr;

  CVar* m_debugOverlayPlayerInfo = nullptr;
  CVar* m_debugOverlayWorldInfo = nullptr;
  CVar* m_debugOverlayAreaInfo = nullptr;
  CVar* m_debugOverlayShowFrameCounter = nullptr;
  CVar* m_debugOverlayShowInGameTime = nullptr;
  CVar* m_debugOverlayShowResourceStats = nullptr;

  CVarCommons(CVarManager& manager) : m_mgr(manager) {
    m_graphicsApi = m_mgr.findOrMakeCVar("graphicsApi"sv, "API to use for rendering graphics"sv, DEFAULT_GRAPHICS_API,
                                         hecl::CVar::EFlags::System | hecl::CVar::EFlags::Archive |
                                             hecl::CVar::EFlags::ModifyRestart);
    m_drawSamples = m_mgr.findOrMakeCVar("drawSamples"sv, "Number of MSAA samples to use for render targets"sv, 1,
                                         hecl::CVar::EFlags::System | hecl::CVar::EFlags::Archive |
                                             hecl::CVar::EFlags::ModifyRestart);
    m_texAnisotropy = m_mgr.findOrMakeCVar(
        "texAnisotropy"sv, "Number of anisotropic samples to use for sampling textures"sv, 1,
        hecl::CVar::EFlags::System | hecl::CVar::EFlags::Archive | hecl::CVar::EFlags::ModifyRestart);
    m_deepColor = m_mgr.findOrMakeCVar(
        "deepColor"sv, "Allow framebuffer with color depth greater-then 24-bits"sv, false,
        hecl::CVar::EFlags::System | hecl::CVar::EFlags::Archive | hecl::CVar::EFlags::ModifyRestart);
    m_variableDt =
        m_mgr.findOrMakeCVar("variableDt", "Enable variable delta time (experimental)", false,
                             (CVar::EFlags::Game | CVar::EFlags::ReadOnly | CVar::EFlags::InternalArchivable));

    m_debugOverlayPlayerInfo = m_mgr.findOrMakeCVar(
        "debugOverlay.playerInfo"sv, "Displays information about the player, such as location and orientation"sv, false,
        hecl::CVar::EFlags::Game | hecl::CVar::EFlags::Archive | hecl::CVar::EFlags::ReadOnly);
    m_debugOverlayWorldInfo = m_mgr.findOrMakeCVar(
        "debugOverlay.worldInfo"sv,
        "Displays information about the current world, such as world asset ID, and areaId"sv, false,
        hecl::CVar::EFlags::Game | hecl::CVar::EFlags::Archive | hecl::CVar::EFlags::ReadOnly);
    m_debugOverlayAreaInfo = m_mgr.findOrMakeCVar(
        "debugOverlay.areaInfo"sv,
        "Displays information about the current area, such as asset ID, object/layer counts, and active layer bits"sv,
        false, hecl::CVar::EFlags::Game | hecl::CVar::EFlags::Archive | hecl::CVar::EFlags::ReadOnly);
    m_debugOverlayShowFrameCounter =
        m_mgr.findOrMakeCVar("debugOverlay.showFrameCounter"sv, "Displays the current frame index"sv, false,
                             hecl::CVar::EFlags::Game | hecl::CVar::EFlags::Archive | hecl::CVar::EFlags::ReadOnly);
    m_debugOverlayShowInGameTime =
        m_mgr.findOrMakeCVar("debugOverlay.showInGameTime"sv, "Displays the current in game time"sv, false,
                             hecl::CVar::EFlags::Game | hecl::CVar::EFlags::Archive | hecl::CVar::EFlags::ReadOnly);
    m_debugOverlayShowResourceStats = m_mgr.findOrMakeCVar(
        "debugOverlay.showResourceStats"sv, "Displays the current live resource object and token counts"sv, false,
        hecl::CVar::EFlags::Game | hecl::CVar::EFlags::Archive | hecl::CVar::EFlags::ReadOnly);
  }

  std::string getGraphicsApi() const { return m_graphicsApi->toLiteral(); }

  void setGraphicsApi(std::string_view api) { m_graphicsApi->fromLiteral(api); }

  uint32_t getSamples() const { return std::max(1u, m_drawSamples->toUnsigned()); }

  void setSamples(uint32_t v) { m_drawSamples->fromInteger(std::max(uint32_t(1), v)); }

  uint32_t getAnisotropy() const { return std::max(1u, uint32_t(m_texAnisotropy->toUnsigned())); }

  void setAnisotropy(uint32_t v) { m_texAnisotropy->fromInteger(std::max(1u, v)); }

  bool getDeepColor() const { return m_deepColor->toBoolean(); }

  void setDeepColor(bool b) { m_deepColor->fromBoolean(b); }

  void serialize() { m_mgr.serialize(); }
};

} // namespace hecl
<commit_msg>CVarCommons: Fix variableDt cvar type<commit_after>#pragma once

#include <algorithm>
#include <cstdint>
#include <string>

#include "hecl/CVarManager.hpp"

#undef min
#undef max

namespace hecl {

using namespace std::literals;

#ifdef _WIN32
#define DEFAULT_GRAPHICS_API "D3D11"sv
#elif defined(__APPLE__)
#define DEFAULT_GRAPHICS_API "Metal"sv
#else
#define DEFAULT_GRAPHICS_API "Vulkan"sv
#endif

struct CVarCommons {
  CVarManager& m_mgr;
  CVar* m_graphicsApi = nullptr;
  CVar* m_drawSamples = nullptr;
  CVar* m_texAnisotropy = nullptr;
  CVar* m_deepColor = nullptr;
  CVar* m_variableDt = nullptr;

  CVar* m_debugOverlayPlayerInfo = nullptr;
  CVar* m_debugOverlayWorldInfo = nullptr;
  CVar* m_debugOverlayAreaInfo = nullptr;
  CVar* m_debugOverlayShowFrameCounter = nullptr;
  CVar* m_debugOverlayShowInGameTime = nullptr;
  CVar* m_debugOverlayShowResourceStats = nullptr;

  CVarCommons(CVarManager& manager) : m_mgr(manager) {
    m_graphicsApi = m_mgr.findOrMakeCVar("graphicsApi"sv, "API to use for rendering graphics"sv, DEFAULT_GRAPHICS_API,
                                         hecl::CVar::EFlags::System | hecl::CVar::EFlags::Archive |
                                             hecl::CVar::EFlags::ModifyRestart);
    m_drawSamples = m_mgr.findOrMakeCVar("drawSamples"sv, "Number of MSAA samples to use for render targets"sv, 1,
                                         hecl::CVar::EFlags::System | hecl::CVar::EFlags::Archive |
                                             hecl::CVar::EFlags::ModifyRestart);
    m_texAnisotropy = m_mgr.findOrMakeCVar(
        "texAnisotropy"sv, "Number of anisotropic samples to use for sampling textures"sv, 1,
        hecl::CVar::EFlags::System | hecl::CVar::EFlags::Archive | hecl::CVar::EFlags::ModifyRestart);
    m_deepColor = m_mgr.findOrMakeCVar(
        "deepColor"sv, "Allow framebuffer with color depth greater-then 24-bits"sv, false,
        hecl::CVar::EFlags::System | hecl::CVar::EFlags::Archive | hecl::CVar::EFlags::ModifyRestart);
    m_variableDt =
        m_mgr.findOrMakeCVar("variableDt", "Enable variable delta time (experimental)", false,
                             (hecl::CVar::EFlags::System | hecl::CVar::EFlags::Archive |
                              hecl::CVar::EFlags::ModifyRestart));

    m_debugOverlayPlayerInfo = m_mgr.findOrMakeCVar(
        "debugOverlay.playerInfo"sv, "Displays information about the player, such as location and orientation"sv, false,
        hecl::CVar::EFlags::Game | hecl::CVar::EFlags::Archive | hecl::CVar::EFlags::ReadOnly);
    m_debugOverlayWorldInfo = m_mgr.findOrMakeCVar(
        "debugOverlay.worldInfo"sv,
        "Displays information about the current world, such as world asset ID, and areaId"sv, false,
        hecl::CVar::EFlags::Game | hecl::CVar::EFlags::Archive | hecl::CVar::EFlags::ReadOnly);
    m_debugOverlayAreaInfo = m_mgr.findOrMakeCVar(
        "debugOverlay.areaInfo"sv,
        "Displays information about the current area, such as asset ID, object/layer counts, and active layer bits"sv,
        false, hecl::CVar::EFlags::Game | hecl::CVar::EFlags::Archive | hecl::CVar::EFlags::ReadOnly);
    m_debugOverlayShowFrameCounter =
        m_mgr.findOrMakeCVar("debugOverlay.showFrameCounter"sv, "Displays the current frame index"sv, false,
                             hecl::CVar::EFlags::Game | hecl::CVar::EFlags::Archive | hecl::CVar::EFlags::ReadOnly);
    m_debugOverlayShowInGameTime =
        m_mgr.findOrMakeCVar("debugOverlay.showInGameTime"sv, "Displays the current in game time"sv, false,
                             hecl::CVar::EFlags::Game | hecl::CVar::EFlags::Archive | hecl::CVar::EFlags::ReadOnly);
    m_debugOverlayShowResourceStats = m_mgr.findOrMakeCVar(
        "debugOverlay.showResourceStats"sv, "Displays the current live resource object and token counts"sv, false,
        hecl::CVar::EFlags::Game | hecl::CVar::EFlags::Archive | hecl::CVar::EFlags::ReadOnly);
  }

  std::string getGraphicsApi() const { return m_graphicsApi->toLiteral(); }

  void setGraphicsApi(std::string_view api) { m_graphicsApi->fromLiteral(api); }

  uint32_t getSamples() const { return std::max(1u, m_drawSamples->toUnsigned()); }

  void setSamples(uint32_t v) { m_drawSamples->fromInteger(std::max(uint32_t(1), v)); }

  uint32_t getAnisotropy() const { return std::max(1u, uint32_t(m_texAnisotropy->toUnsigned())); }

  void setAnisotropy(uint32_t v) { m_texAnisotropy->fromInteger(std::max(1u, v)); }

  bool getDeepColor() const { return m_deepColor->toBoolean(); }

  void setDeepColor(bool b) { m_deepColor->fromBoolean(b); }

  void serialize() { m_mgr.serialize(); }
};

} // namespace hecl
<|endoftext|>


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

/*      Title     : collision_check.cpp
 *      Project   : moveit_jog_arm
 *      Created   : 1/11/2019
 *      Author    : Brian O'Neil, Andy Zelenak, Blake Anderson
 */

#include <std_msgs/Float64.h>

#include <moveit_jog_arm/collision_check.h>
#include <moveit_jog_arm/make_shared_from_pool.h>

static const char LOGNAME[] = "collision_check";
static const double MIN_RECOMMENDED_COLLISION_RATE = 10;
constexpr double EPSILON = 1e-6;  // For very small numeric comparisons

namespace moveit_jog_arm
{
// Constructor for the class that handles collision checking
CollisionCheck::CollisionCheck(ros::NodeHandle& nh, const moveit_jog_arm::JogArmParameters& parameters,
                               const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
                               const std::shared_ptr<JointStateSubscriber>& joint_state_subscriber)
  : nh_(nh)
  , parameters_(parameters)
  , planning_scene_monitor_(planning_scene_monitor)
  , joint_state_subscriber_(joint_state_subscriber)
  , self_velocity_scale_coefficient_(-log(0.001) / parameters.self_collision_proximity_threshold)
  , scene_velocity_scale_coefficient_(-log(0.001) / parameters.scene_collision_proximity_threshold)
  , period_(1. / parameters_.collision_check_rate)
{
  // Init collision request
  collision_request_.group_name = parameters_.move_group_name;
  collision_request_.distance = true;  // enable distance-based collision checking

  if (parameters_.collision_check_rate < MIN_RECOMMENDED_COLLISION_RATE)
    ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME,
                                   "Collision check rate is low, increase it in yaml file if CPU allows");

  collision_check_type_ =
      (parameters_.collision_check_type == "threshold_distance" ? K_THRESHOLD_DISTANCE : K_STOP_DISTANCE);
  safety_factor_ = parameters_.collision_distance_safety_factor;

  // Internal namespace
  ros::NodeHandle internal_nh("~internal");
  collision_velocity_scale_pub_ = internal_nh.advertise<std_msgs::Float64>("collision_velocity_scale", ROS_QUEUE_SIZE);
  collision_velocity_scale_pub_ = internal_nh.advertise<std_msgs::Float64>("collision_velocity_scale", 1);
  worst_case_stop_time_sub_ =
      internal_nh.subscribe("worst_case_stop_time", ROS_QUEUE_SIZE, &CollisionCheck::worstCaseStopTimeCB, this);

  current_state_ = std::make_unique<moveit::core::RobotState>(getLockedPlanningSceneRO()->getCurrentState());
  acm_ = getLockedPlanningSceneRO()->getAllowedCollisionMatrix();
}

planning_scene_monitor::LockedPlanningSceneRO CollisionCheck::getLockedPlanningSceneRO() const
{
  return planning_scene_monitor::LockedPlanningSceneRO(planning_scene_monitor_);
}

void CollisionCheck::start()
{
  timer_ = nh_.createTimer(period_, &CollisionCheck::run, this);
}

void CollisionCheck::stop()
{
  timer_.stop();
}

void CollisionCheck::run(const ros::TimerEvent& timer_event)
{
  // Log warning when the last loop duration was longer than the period
  if (timer_event.profile.last_duration.toSec() > period_.toSec())
  {
    ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME,
                                   "last_duration: " << timer_event.profile.last_duration.toSec() << " ("
                                                     << period_.toSec() << ")");
  }

  if (paused_)
  {
    return;
  }

  // Copy the latest joint state
  auto latest_joint_state = joint_state_subscriber_->getLatest();
  for (std::size_t i = 0; i < latest_joint_state->position.size(); ++i)
    current_state_->setJointPositions(latest_joint_state->name[i], &latest_joint_state->position[i]);

  current_state_->updateCollisionBodyTransforms();
  collision_detected_ = false;

  // Do a timer-safe distance-based collision detection
  collision_result_.clear();
  getLockedPlanningSceneRO()->getCollisionEnv()->checkRobotCollision(collision_request_, collision_result_,
                                                                     *current_state_);
  scene_collision_distance_ = collision_result_.distance;
  collision_detected_ |= collision_result_.collision;

  collision_result_.clear();
  getLockedPlanningSceneRO()->getCollisionEnvUnpadded()->checkSelfCollision(collision_request_, collision_result_,
                                                                            *current_state_, acm_);
  self_collision_distance_ = collision_result_.distance;
  collision_detected_ |= collision_result_.collision;

  velocity_scale_ = 1;
  // If we're definitely in collision, stop immediately
  if (collision_detected_)
  {
    velocity_scale_ = 0;
  }
  // If threshold distances were specified
  else if (collision_check_type_ == K_THRESHOLD_DISTANCE)
  {
    // If we are far from a collision, velocity_scale should be 1.
    // If we are very close to a collision, velocity_scale should be ~zero.
    // When scene_collision_proximity_threshold is breached, start decelerating exponentially.
    if (scene_collision_distance_ < parameters_.scene_collision_proximity_threshold)
    {
      // velocity_scale = e ^ k * (collision_distance - threshold)
      // k = - ln(0.001) / collision_proximity_threshold
      // velocity_scale should equal one when collision_distance is at collision_proximity_threshold.
      // velocity_scale should equal 0.001 when collision_distance is at zero.
      velocity_scale_ =
          std::min(velocity_scale_, exp(scene_velocity_scale_coefficient_ *
                                        (scene_collision_distance_ - parameters_.scene_collision_proximity_threshold)));
    }

    if (self_collision_distance_ < parameters_.self_collision_proximity_threshold)
    {
      velocity_scale_ =
          std::min(velocity_scale_, exp(self_velocity_scale_coefficient_ *
                                        (self_collision_distance_ - parameters_.self_collision_proximity_threshold)));
    }
  }
  // Else, we stop based on worst-case stopping distance
  else
  {
    // Retrieve the worst-case time to stop, based on current joint velocities

    // Calculate rate of change of distance to nearest collision
    current_collision_distance_ = std::min(scene_collision_distance_, self_collision_distance_);
    derivative_of_collision_distance_ = (current_collision_distance_ - prev_collision_distance_) / period_.toSec();

    if (current_collision_distance_ < parameters_.min_allowable_collision_distance &&
        derivative_of_collision_distance_ <= 0)
    {
      velocity_scale_ = 0;
    }
    // Only bother doing calculations if we are moving toward the nearest collision
    else if (derivative_of_collision_distance_ < -EPSILON)
    {
      // At the rate the collision distance is decreasing, how long until we collide?
      est_time_to_collision_ = fabs(current_collision_distance_ / derivative_of_collision_distance_);

      // halt if we can't stop fast enough (including the safety factor)
      if (est_time_to_collision_ < (safety_factor_ * worst_case_stop_time_))
      {
        velocity_scale_ = 0;
      }
    }

    // Update for the next iteration
    prev_collision_distance_ = current_collision_distance_;
  }

  // publish message
  {
    auto msg = moveit::util::make_shared_from_pool<std_msgs::Float64>();
    msg->data = velocity_scale_;
    collision_velocity_scale_pub_.publish(msg);
  }
}

void CollisionCheck::worstCaseStopTimeCB(const std_msgs::Float64ConstPtr& msg)
{
  worst_case_stop_time_ = msg->data;
}

void CollisionCheck::setPaused(bool paused)
{
  paused_ = paused;
}

}  // namespace moveit_jog_arm
<commit_msg>[jog_arm] remove duplicate line<commit_after>/*******************************************************************************
 * BSD 3-Clause License
 *
 * Copyright (c) 2019, Los Alamos National Security, LLC
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * * Redistributions of source code must retain the above copyright notice, this
 *   list of conditions and the following disclaimer.
 *
 * * Redistributions in binary form must reproduce the above copyright notice,
 *   this list of conditions and the following disclaimer in the documentation
 *   and/or other materials provided with the distribution.
 *
 * * Neither the name of the copyright holder nor the names of its
 *   contributors may be used to endorse or promote products derived from
 *   this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*******************************************************************************/

/*      Title     : collision_check.cpp
 *      Project   : moveit_jog_arm
 *      Created   : 1/11/2019
 *      Author    : Brian O'Neil, Andy Zelenak, Blake Anderson
 */

#include <std_msgs/Float64.h>

#include <moveit_jog_arm/collision_check.h>
#include <moveit_jog_arm/make_shared_from_pool.h>

static const char LOGNAME[] = "collision_check";
static const double MIN_RECOMMENDED_COLLISION_RATE = 10;
constexpr double EPSILON = 1e-6;  // For very small numeric comparisons

namespace moveit_jog_arm
{
// Constructor for the class that handles collision checking
CollisionCheck::CollisionCheck(ros::NodeHandle& nh, const moveit_jog_arm::JogArmParameters& parameters,
                               const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor,
                               const std::shared_ptr<JointStateSubscriber>& joint_state_subscriber)
  : nh_(nh)
  , parameters_(parameters)
  , planning_scene_monitor_(planning_scene_monitor)
  , joint_state_subscriber_(joint_state_subscriber)
  , self_velocity_scale_coefficient_(-log(0.001) / parameters.self_collision_proximity_threshold)
  , scene_velocity_scale_coefficient_(-log(0.001) / parameters.scene_collision_proximity_threshold)
  , period_(1. / parameters_.collision_check_rate)
{
  // Init collision request
  collision_request_.group_name = parameters_.move_group_name;
  collision_request_.distance = true;  // enable distance-based collision checking

  if (parameters_.collision_check_rate < MIN_RECOMMENDED_COLLISION_RATE)
    ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME,
                                   "Collision check rate is low, increase it in yaml file if CPU allows");

  collision_check_type_ =
      (parameters_.collision_check_type == "threshold_distance" ? K_THRESHOLD_DISTANCE : K_STOP_DISTANCE);
  safety_factor_ = parameters_.collision_distance_safety_factor;

  // Internal namespace
  ros::NodeHandle internal_nh("~internal");
  collision_velocity_scale_pub_ = internal_nh.advertise<std_msgs::Float64>("collision_velocity_scale", ROS_QUEUE_SIZE);
  worst_case_stop_time_sub_ =
      internal_nh.subscribe("worst_case_stop_time", ROS_QUEUE_SIZE, &CollisionCheck::worstCaseStopTimeCB, this);

  current_state_ = std::make_unique<moveit::core::RobotState>(getLockedPlanningSceneRO()->getCurrentState());
  acm_ = getLockedPlanningSceneRO()->getAllowedCollisionMatrix();
}

planning_scene_monitor::LockedPlanningSceneRO CollisionCheck::getLockedPlanningSceneRO() const
{
  return planning_scene_monitor::LockedPlanningSceneRO(planning_scene_monitor_);
}

void CollisionCheck::start()
{
  timer_ = nh_.createTimer(period_, &CollisionCheck::run, this);
}

void CollisionCheck::stop()
{
  timer_.stop();
}

void CollisionCheck::run(const ros::TimerEvent& timer_event)
{
  // Log warning when the last loop duration was longer than the period
  if (timer_event.profile.last_duration.toSec() > period_.toSec())
  {
    ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME,
                                   "last_duration: " << timer_event.profile.last_duration.toSec() << " ("
                                                     << period_.toSec() << ")");
  }

  if (paused_)
  {
    return;
  }

  // Copy the latest joint state
  auto latest_joint_state = joint_state_subscriber_->getLatest();
  for (std::size_t i = 0; i < latest_joint_state->position.size(); ++i)
    current_state_->setJointPositions(latest_joint_state->name[i], &latest_joint_state->position[i]);

  current_state_->updateCollisionBodyTransforms();
  collision_detected_ = false;

  // Do a timer-safe distance-based collision detection
  collision_result_.clear();
  getLockedPlanningSceneRO()->getCollisionEnv()->checkRobotCollision(collision_request_, collision_result_,
                                                                     *current_state_);
  scene_collision_distance_ = collision_result_.distance;
  collision_detected_ |= collision_result_.collision;

  collision_result_.clear();
  getLockedPlanningSceneRO()->getCollisionEnvUnpadded()->checkSelfCollision(collision_request_, collision_result_,
                                                                            *current_state_, acm_);
  self_collision_distance_ = collision_result_.distance;
  collision_detected_ |= collision_result_.collision;

  velocity_scale_ = 1;
  // If we're definitely in collision, stop immediately
  if (collision_detected_)
  {
    velocity_scale_ = 0;
  }
  // If threshold distances were specified
  else if (collision_check_type_ == K_THRESHOLD_DISTANCE)
  {
    // If we are far from a collision, velocity_scale should be 1.
    // If we are very close to a collision, velocity_scale should be ~zero.
    // When scene_collision_proximity_threshold is breached, start decelerating exponentially.
    if (scene_collision_distance_ < parameters_.scene_collision_proximity_threshold)
    {
      // velocity_scale = e ^ k * (collision_distance - threshold)
      // k = - ln(0.001) / collision_proximity_threshold
      // velocity_scale should equal one when collision_distance is at collision_proximity_threshold.
      // velocity_scale should equal 0.001 when collision_distance is at zero.
      velocity_scale_ =
          std::min(velocity_scale_, exp(scene_velocity_scale_coefficient_ *
                                        (scene_collision_distance_ - parameters_.scene_collision_proximity_threshold)));
    }

    if (self_collision_distance_ < parameters_.self_collision_proximity_threshold)
    {
      velocity_scale_ =
          std::min(velocity_scale_, exp(self_velocity_scale_coefficient_ *
                                        (self_collision_distance_ - parameters_.self_collision_proximity_threshold)));
    }
  }
  // Else, we stop based on worst-case stopping distance
  else
  {
    // Retrieve the worst-case time to stop, based on current joint velocities

    // Calculate rate of change of distance to nearest collision
    current_collision_distance_ = std::min(scene_collision_distance_, self_collision_distance_);
    derivative_of_collision_distance_ = (current_collision_distance_ - prev_collision_distance_) / period_.toSec();

    if (current_collision_distance_ < parameters_.min_allowable_collision_distance &&
        derivative_of_collision_distance_ <= 0)
    {
      velocity_scale_ = 0;
    }
    // Only bother doing calculations if we are moving toward the nearest collision
    else if (derivative_of_collision_distance_ < -EPSILON)
    {
      // At the rate the collision distance is decreasing, how long until we collide?
      est_time_to_collision_ = fabs(current_collision_distance_ / derivative_of_collision_distance_);

      // halt if we can't stop fast enough (including the safety factor)
      if (est_time_to_collision_ < (safety_factor_ * worst_case_stop_time_))
      {
        velocity_scale_ = 0;
      }
    }

    // Update for the next iteration
    prev_collision_distance_ = current_collision_distance_;
  }

  // publish message
  {
    auto msg = moveit::util::make_shared_from_pool<std_msgs::Float64>();
    msg->data = velocity_scale_;
    collision_velocity_scale_pub_.publish(msg);
  }
}

void CollisionCheck::worstCaseStopTimeCB(const std_msgs::Float64ConstPtr& msg)
{
  worst_case_stop_time_ = msg->data;
}

void CollisionCheck::setPaused(bool paused)
{
  paused_ = paused;
}

}  // namespace moveit_jog_arm
<|endoftext|>


<commit_before>#include <iostream>

using namespace std;

int main()
{
int Num1;
int Num2;
int Duv;
int Sum;

cout << "Pleas Enter A Number...\n";
cout << endl;
cin >> Num1;
cout << "Pleas Enter A Symbole Ex: +, *, - \n";
cin >> Duv;
cout << endl;
cout << "Pleas Enter A Number...\n";
cin >> Num2;
cout << endl;

Sum = Num1, Duv, Num2;

cout << "Processing...\n";
cout << endl;
cout << "Processing Complete....";
cout << endl;
cout << Num1 << Duv << Num2 << " = " << Sum << endl; // Calclats the problem!

return 0;
}
<commit_msg>Added BING! :D<commit_after>#include <iostream>

using namespace std;

int main()
{
int Num1;
int Num2;
int Duv;
int Sum;

cout << "Pleas Enter A Number...\n";
cout << endl;
cin >> Num1;
cout << "Pleas Enter A Symbole Ex: +, *, - \n";
cin >> Duv;
cout << endl;
cout << "Pleas Enter A Number...\n";
cin >> Num2;
cout << endl;

Sum = Num1, Duv, Num2;

cout << "Processing ...\n";
cout << endl;
cout << "Processing Complete ....";
cout << endl;
cout << Num1 << Duv << Num2 << " = \a" << Sum << endl; // Calclats the problem!

return 0;
}
<|endoftext|>


<commit_before>/*
 * Main_Window.cpp
 *
 *  Created on: 30 avr. 2017
 *      Author: guill
 */

#include "Application/Windows/Main_Window.h"
#include "Application/Events.h"
#include <iostream>

Main_Window::Main_Window(Picaso::Serial_Commander& lcd) : Touchable_Window(lcd),
button_moving(m_lcd, "Moving Window", Picaso::Color::RED, Picaso::Color::BLACK, 10, 70, 450/2, 90),
button_monitor(m_lcd, "Process Monitor", Picaso::Color::RED, Picaso::Color::BLACK, 10, 70+100, 450/2, 90),
button_spreading(m_lcd, "Spreading Window", Picaso::Color::RED, Picaso::Color::BLACK, 20 + 450/2, 70, 450/2, 90),
button_tilt(m_lcd, "Tilt Window", Picaso::Color::RED, Picaso::Color::BLACK, 20 + 450/2, 70+100, 450/2, 90),
m_title(m_lcd, "BE POO 2017", Picaso::Color::WHITE, 10, 10, 320, 60, 3, Picaso::Alignment_H::CENTER, Picaso::Alignment_V::CENTER, false),
m_names(m_lcd, "Combattelli Julien \n\rSarthou Guillaume", Picaso::Color::WHITESMOKE, 320, 10, 480-320, 60, 1, Picaso::Alignment_H::CENTER, Picaso::Alignment_V::CENTER, false)
{
	m_lcd = lcd;
	button_moving.add_receiver(*this);
	button_monitor.add_receiver(*this);
	button_spreading.add_receiver(*this);
	button_tilt.add_receiver(*this);
	m_touch_dispatcher.register_widget(button_moving);
	m_touch_dispatcher.register_widget(button_monitor);
	m_touch_dispatcher.register_widget(button_spreading);
	m_touch_dispatcher.register_widget(button_tilt);

	add_event_handler<Picaso::Button_Pressed>(&Main_Window::button_pressed_handler, this);
}

Main_Window::~Main_Window()
{

}

void Main_Window::show()
{
	m_lcd.gfx_clear_screen();
	button_moving.show();
	button_monitor.show();
	button_spreading.show();
	button_tilt.show();
	m_title.show();
	m_names.show();
}

void Main_Window::button_pressed_handler(Sender& s, const Event& event)
{
	std::cout << "touche" << std::endl;

	unsigned int id = ((Picaso::Widget&)s).get_id();

	if(id == button_moving.get_id())
		button_moving_pressed();
	else if (id == button_monitor.get_id())
		button_monitor_pressed();
	else if(id == button_spreading.get_id())
		button_spreading_pressed();
	else if(id == button_tilt.get_id())
		button_tilt_pressed();
}

void Main_Window::button_moving_pressed()
{
	raise(Navigation_choise_Pressed(1));
}

void Main_Window::button_monitor_pressed()
{
	raise(Navigation_choise_Pressed(2));
}

void Main_Window::button_spreading_pressed()
{
	raise(Navigation_choise_Pressed(3));
}

void Main_Window::button_tilt_pressed()
{
	raise(Navigation_choise_Pressed(4));
}
<commit_msg>Fix UI bug<commit_after>/*
 * Main_Window.cpp
 *
 *  Created on: 30 avr. 2017
 *      Author: guill
 */

#include "Application/Windows/Main_Window.h"
#include "Application/Events.h"
#include <iostream>

Main_Window::Main_Window(Picaso::Serial_Commander& lcd) : Touchable_Window(lcd),
button_moving(m_lcd, "Moving Window", Picaso::Color::RED, Picaso::Color::BLACK, 10, 70, 450/2, 90),
button_monitor(m_lcd, "Process Monitor", Picaso::Color::RED, Picaso::Color::BLACK, 10, 70+100, 450/2, 90),
button_spreading(m_lcd, "Spreading Window", Picaso::Color::RED, Picaso::Color::BLACK, 20 + 450/2, 70, 450/2, 90),
button_tilt(m_lcd, "Tilt Window", Picaso::Color::RED, Picaso::Color::BLACK, 20 + 450/2, 70+100, 450/2, 90),
m_title(m_lcd, "BE POO 2017", Picaso::Color::WHITE, 10, 10, 320, 60, 3, Picaso::Alignment_H::CENTER, Picaso::Alignment_V::CENTER, false),
m_names(m_lcd, "Combattelli Julien \n\rSarthou Guillaume", Picaso::Color::WHITESMOKE, 320, 10, 480-320, 60, 1, Picaso::Alignment_H::CENTER, Picaso::Alignment_V::CENTER, false)
{
	m_lcd = lcd;
	button_moving.add_receiver(*this);
	button_monitor.add_receiver(*this);
	button_spreading.add_receiver(*this);
	button_tilt.add_receiver(*this);
	m_touch_dispatcher.register_widget(button_moving);
	m_touch_dispatcher.register_widget(button_monitor);
	m_touch_dispatcher.register_widget(button_spreading);
	m_touch_dispatcher.register_widget(button_tilt);

	add_event_handler<Picaso::Button_Pressed>(&Main_Window::button_pressed_handler, this);
}

Main_Window::~Main_Window()
{

}

void Main_Window::show()
{
	m_lcd.gfx_clear_screen();
	button_moving.show();
	button_monitor.show();
	button_spreading.show();
	button_tilt.show();
	m_title.show();
	m_names.show();
}

void Main_Window::button_pressed_handler(Sender& s, const Event& event)
{
	std::cout << "touche" << std::endl;

	unsigned int id = ((Picaso::Widget&)s).get_id();

	if(id == button_moving.get_id())
		button_moving_pressed();
	else if (id == button_monitor.get_id())
		button_monitor_pressed();
	else if(id == button_spreading.get_id())
		button_spreading_pressed();
	else if(id == button_tilt.get_id())
		button_tilt_pressed();
}

void Main_Window::button_moving_pressed()
{
	raise(Navigation_choise_Pressed(1));
}

void Main_Window::button_monitor_pressed()
{
	raise(Navigation_choise_Pressed(4));
}

void Main_Window::button_spreading_pressed()
{
	raise(Navigation_choise_Pressed(3));
}

void Main_Window::button_tilt_pressed()
{
	raise(Navigation_choise_Pressed(2));
}
<|endoftext|>


<commit_before>#include "AudioData.h"

#include "Messages/AudioData.pb.h"
#include <google/protobuf/io/zero_copy_stream_impl.h>

#include <algorithm>

using namespace naoth;

AudioData::AudioData():
  sampleRate(8000),
  numChannels(2),
  timestamp(0)
{

}

void AudioData::print(std::ostream& stream) const
{
  stream << "sampleRate: " << sampleRate << std::endl;
  stream << "numChannels: " << numChannels << std::endl;
  stream << "Number of Samples: " << samples.size() << std::endl;
  stream << "timestamp: " << timestamp << std::endl;
}

AudioData::~AudioData()
{
}

void Serializer<AudioData>::serialize(const AudioData& representation, std::ostream& stream)
{
  // the data has to be converted to a YUV (1 byte for each) array. no interlacing
  naothmessages::AudioData msg;

  msg.set_samplerate(representation.sampleRate);
  msg.set_numchannels(representation.numChannels);
  msg.set_samplesize(sizeof(short));
  msg.set_timestamp(representation.timestamp);
  msg.set_samples((unsigned char*)representation.samples.data(), representation.samples.size()*sizeof(short));
  google::protobuf::io::OstreamOutputStream buf(&stream);
  msg.SerializeToZeroCopyStream(&buf);
}

void Serializer<AudioData>::deserialize(std::istream& stream, AudioData& representation)
{
  naothmessages::AudioData msg;

  google::protobuf::io::IstreamInputStream buf(&stream);
  msg.ParseFromZeroCopyStream(&buf);

  representation.sampleRate = msg.samplerate();
  representation.numChannels = msg.numchannels();
  representation.timestamp = msg.timestamp();

  assert(msg.samplesize() == sizeof(short));
  representation.samples.resize(msg.samples().size() / sizeof(short));
  std::copy_n(msg.samples().data(), msg.samples().size(), (unsigned char*)representation.samples.data());
}<commit_msg>small cleanup use fixed size type for samples<commit_after>#include "AudioData.h"

#include "Messages/AudioData.pb.h"
#include <google/protobuf/io/zero_copy_stream_impl.h>

#include <algorithm>

using namespace naoth;

AudioData::AudioData():
  sampleRate(8000),
  numChannels(2),
  timestamp(0)
{
}

void AudioData::print(std::ostream& stream) const
{
  stream << "sampleRate: " << sampleRate << std::endl;
  stream << "numChannels: " << numChannels << std::endl;
  stream << "Number of Samples: " << samples.size() << std::endl;
  stream << "timestamp: " << timestamp << std::endl;
}

void Serializer<AudioData>::serialize(const AudioData& representation, std::ostream& stream)
{
  // the data has to be converted to a YUV (1 byte for each) array. no interlacing
  naothmessages::AudioData msg;

  msg.set_samplerate(representation.sampleRate);
  msg.set_numchannels(representation.numChannels);
  msg.set_samplesize(sizeof(short));
  msg.set_timestamp(representation.timestamp);
  msg.set_samples((unsigned char*)representation.samples.data(), representation.samples.size()*sizeof(short));
  google::protobuf::io::OstreamOutputStream buf(&stream);
  msg.SerializeToZeroCopyStream(&buf);
}

void Serializer<AudioData>::deserialize(std::istream& stream, AudioData& representation)
{
  naothmessages::AudioData msg;

  google::protobuf::io::IstreamInputStream buf(&stream);
  msg.ParseFromZeroCopyStream(&buf);

  representation.sampleRate = msg.samplerate();
  representation.numChannels = msg.numchannels();
  representation.timestamp = msg.timestamp();

  assert(msg.samplesize() == sizeof(uint16_t));
  representation.samples.resize(msg.samples().size() / sizeof(uint16_t));
  std::copy_n(msg.samples().data(), msg.samples().size(), (unsigned char*)representation.samples.data());
}<|endoftext|>


<commit_before>#include "dreal/solver/assertion_filter.h"

#include <cmath>

#include "dreal/util/box.h"
#include "dreal/util/exception.h"

namespace dreal {
namespace {

using std::nextafter;

// Constrains the @p box with ` box[var].lb() == v`.
FilterAssertionResult UpdateBoundsViaEquality(const Variable& var,
                                              const double v, Box* const box) {
  Box::Interval& intv{(*box)[var]};
  if (intv.lb() == intv.ub() && intv.lb() == v) {
    return FilterAssertionResult::FilteredWithoutChange;
  }
  if (intv.contains(v)) {
    intv = v;
  } else {
    box->set_empty();
  }
  return FilterAssertionResult::FilteredWithChange;
}

// Constrains the @p box with ` box[var].lb() >= v`.
FilterAssertionResult UpdateLowerBound(const Variable& var, const double v,
                                       Box* const box) {
  Box::Interval& iv{(*box)[var]};
  if (v <= iv.lb()) {
    return FilterAssertionResult::FilteredWithoutChange;
  }
  if (v <= iv.ub()) {
    iv = Box::Interval(v, iv.ub());
  } else {
    box->set_empty();
  }
  return FilterAssertionResult::FilteredWithChange;
}

// Constrains the @p box with `box[var].lb() > v`. It turns the strict
// inequality into a non-strict one, `box[var].lb() >= v + ε` where `v
// + ε` is the smallest representable floating-point number bigger than
// `v`.
FilterAssertionResult UpdateStrictLowerBound(const Variable& var,
                                             const double v, Box* const box) {
  return UpdateLowerBound(var, nextafter(v, DBL_MAX), box);
}

// Constrains the @p box with ` box[var].lb() <= v`.
FilterAssertionResult UpdateUpperBound(const Variable& var, const double v,
                                       Box* const box) {
  Box::Interval& iv{(*box)[var]};
  if (v >= iv.ub()) {
    return FilterAssertionResult::FilteredWithoutChange;
  }
  if (v >= iv.lb()) {
    iv = Box::Interval(iv.lb(), v);
  } else {
    box->set_empty();
  }
  return FilterAssertionResult::FilteredWithChange;
}

// Constrains the @p box with `box[var].lb() < v`. It turns the strict
// inequality into a non-strict one, `box[var].lb() <= v - ε` where `v
// - ε` is the largest representable floating-point number smaller
// than `v`.
FilterAssertionResult UpdateStrictUpperBound(const Variable& var,
                                             const double v, Box* const box) {
  return UpdateUpperBound(var, nextafter(v, DBL_MIN), box);
}

class AssertionFilter {
 public:
  FilterAssertionResult Process(const Formula& f, Box* const box) const {
    return Visit(f, box, true);
  }

 public:
  FilterAssertionResult Visit(const Formula& f, Box* const box,
                              const bool polarity) const {
    return VisitFormula<FilterAssertionResult>(this, f, box, polarity);
  }
  FilterAssertionResult VisitFalse(const Formula&, Box* const,
                                   const bool) const {
    return FilterAssertionResult::NotFiltered;
  }
  FilterAssertionResult VisitTrue(const Formula&, Box* const,
                                  const bool) const {
    return FilterAssertionResult::NotFiltered;
  }
  FilterAssertionResult VisitVariable(const Formula&, Box* const,
                                      const bool) const {
    return FilterAssertionResult::NotFiltered;
  }
  FilterAssertionResult VisitEqualTo(const Formula& f, Box* const box,
                                     const bool polarity) const {
    if (!polarity) {
      return FilterAssertionResult::NotFiltered;
    }
    const Expression& lhs{get_lhs_expression(f)};
    const Expression& rhs{get_rhs_expression(f)};
    if (is_variable(lhs) && is_constant(rhs)) {
      // var = v
      const Variable& var{get_variable(lhs)};
      const double v{get_constant_value(rhs)};
      return UpdateBoundsViaEquality(var, v, box);
    }
    if (is_constant(lhs) && is_variable(rhs)) {
      // v = var
      const double v{get_constant_value(lhs)};
      const Variable& var{get_variable(rhs)};
      return UpdateBoundsViaEquality(var, v, box);
    }
    return FilterAssertionResult::NotFiltered;
  }
  FilterAssertionResult VisitNotEqualTo(const Formula& f, Box* const box,
                                        const bool polarity) const {
    // Because (x != y) is !(x == y).
    return VisitEqualTo(f, box, !polarity);
  }
  FilterAssertionResult VisitGreaterThan(const Formula& f, Box* const box,
                                         const bool polarity) const {
    const Expression& lhs{get_lhs_expression(f)};
    const Expression& rhs{get_rhs_expression(f)};
    if (is_variable(lhs) && is_constant(rhs)) {
      const Variable& var{get_variable(lhs)};
      const double v{get_constant_value(rhs)};
      if (polarity) {
        // var > v
        return UpdateStrictLowerBound(var, v, box);
      } else {
        // var <= v
        return UpdateUpperBound(var, v, box);
      }
    }
    if (is_constant(lhs) && is_variable(rhs)) {
      const double v{get_constant_value(lhs)};
      const Variable& var{get_variable(rhs)};
      if (polarity) {
        // v > var
        return UpdateStrictUpperBound(var, v, box);
      } else {
        // v <= var
        return UpdateLowerBound(var, v, box);
      }
    }
    return FilterAssertionResult::NotFiltered;
  }
  FilterAssertionResult VisitGreaterThanOrEqualTo(const Formula& f,
                                                  Box* const box,
                                                  const bool polarity) const {
    const Expression& lhs{get_lhs_expression(f)};
    const Expression& rhs{get_rhs_expression(f)};
    if (is_variable(lhs) && is_constant(rhs)) {
      const Variable& var{get_variable(lhs)};
      const double v{get_constant_value(rhs)};
      if (polarity) {
        // var >= v
        return UpdateLowerBound(var, v, box);
      } else {
        // var < v
        return UpdateStrictUpperBound(var, v, box);
      }
    }
    if (is_constant(lhs) && is_variable(rhs)) {
      const double v{get_constant_value(lhs)};
      const Variable& var{get_variable(rhs)};
      if (polarity) {
        // v >= var
        return UpdateUpperBound(var, v, box);
      } else {
        // v < var
        return UpdateStrictLowerBound(var, v, box);
      }
    }
    return FilterAssertionResult::NotFiltered;
  }
  FilterAssertionResult VisitLessThan(const Formula& f, Box* const box,
                                      const bool polarity) const {
    // Because x < y is !(x >= y).
    return VisitGreaterThanOrEqualTo(f, box, !polarity);
  }
  FilterAssertionResult VisitLessThanOrEqualTo(const Formula& f, Box* const box,
                                               const bool polarity) const {
    // Because x <= y is !(x > y).
    return VisitGreaterThan(f, box, !polarity);
  }
  FilterAssertionResult VisitConjunction(const Formula&, Box* const,
                                         const bool) const {
    DREAL_UNREACHABLE();
  }
  FilterAssertionResult VisitDisjunction(const Formula&, Box* const,
                                         const bool) const {
    DREAL_UNREACHABLE();
  }
  FilterAssertionResult VisitNegation(const Formula& f, Box* const box,
                                      const bool polarity) const {
    return Visit(get_operand(f), box, !polarity);
  }
  FilterAssertionResult VisitForall(const Formula&, Box* const,
                                    const bool) const {
    return FilterAssertionResult::NotFiltered;
  }

  // Makes VisitFormula a friend of this class so that it can use private
  // operator()s.
  friend FilterAssertionResult
  drake::symbolic::VisitFormula<FilterAssertionResult>(AssertionFilter*,
                                                       const Formula&,
                                                       dreal::Box* const&,
                                                       const bool&);
};
}  // namespace

FilterAssertionResult FilterAssertion(const Formula& assertion,
                                      Box* const box) {
  return AssertionFilter{}.Process(assertion, box);
}
}  // namespace dreal
<commit_msg>refactor(solver/assertion_filter.cc): Rename variables / Add doc<commit_after>#include "dreal/solver/assertion_filter.h"

#include <cmath>

#include "dreal/util/box.h"
#include "dreal/util/exception.h"

namespace dreal {
namespace {

using std::nextafter;

// Constrains the @p box with ` box[var].lb() == v`.
FilterAssertionResult UpdateBoundsViaEquality(const Variable& var,
                                              const double v, Box* const box) {
  Box::Interval& intv{(*box)[var]};
  const double lb{intv.lb()};
  const double ub{intv.ub()};
  if (lb == ub && lb == v) {
    return FilterAssertionResult::FilteredWithoutChange;
  }
  if (intv.contains(v)) {
    intv = v;
  } else {
    box->set_empty();
  }
  return FilterAssertionResult::FilteredWithChange;
}

// Constrains the @p box with ` box[var].lb() >= v`.
FilterAssertionResult UpdateLowerBound(const Variable& var, const double new_lb,
                                       Box* const box) {
  Box::Interval& intv{(*box)[var]};
  const double lb{intv.lb()};
  const double ub{intv.ub()};
  if (new_lb <= lb) {
    return FilterAssertionResult::FilteredWithoutChange;
  }
  if (new_lb <= ub) {
    intv = Box::Interval(new_lb, ub);
  } else {
    box->set_empty();
  }
  return FilterAssertionResult::FilteredWithChange;
}

// Constrains the @p box with `box[var].lb() > v`. It turns the strict
// inequality into a non-strict one, `box[var].lb() >= v + ε` where `v
// + ε` is the smallest representable floating-point number bigger than
// `v`.
FilterAssertionResult UpdateStrictLowerBound(const Variable& var,
                                             const double lb, Box* const box) {
  return UpdateLowerBound(var, nextafter(lb, DBL_MAX), box);
}

// Constrains the @p box with ` box[var].ub() <= v`.
FilterAssertionResult UpdateUpperBound(const Variable& var, const double new_ub,
                                       Box* const box) {
  Box::Interval& intv{(*box)[var]};
  const double lb{intv.lb()};
  const double ub{intv.ub()};
  if (new_ub >= ub) {
    return FilterAssertionResult::FilteredWithoutChange;
  }
  if (new_ub >= lb) {
    intv = Box::Interval(lb, new_ub);
  } else {
    box->set_empty();
  }
  return FilterAssertionResult::FilteredWithChange;
}

// Constrains the @p box with `box[var].lb() < v`. It turns the strict
// inequality into a non-strict one, `box[var].lb() <= v - ε` where `v
// - ε` is the largest representable floating-point number smaller
// than `v`.
FilterAssertionResult UpdateStrictUpperBound(const Variable& var,
                                             const double ub, Box* const box) {
  return UpdateUpperBound(var, nextafter(ub, DBL_MIN), box);
}

class AssertionFilter {
 public:
  FilterAssertionResult Process(const Formula& f, Box* const box) const {
    return Visit(f, box, true);
  }

 public:
  FilterAssertionResult Visit(const Formula& f, Box* const box,
                              const bool polarity) const {
    return VisitFormula<FilterAssertionResult>(this, f, box, polarity);
  }
  FilterAssertionResult VisitFalse(const Formula&, Box* const,
                                   const bool) const {
    return FilterAssertionResult::NotFiltered;
  }
  FilterAssertionResult VisitTrue(const Formula&, Box* const,
                                  const bool) const {
    return FilterAssertionResult::NotFiltered;
  }
  FilterAssertionResult VisitVariable(const Formula&, Box* const,
                                      const bool) const {
    return FilterAssertionResult::NotFiltered;
  }
  FilterAssertionResult VisitEqualTo(const Formula& f, Box* const box,
                                     const bool polarity) const {
    if (!polarity) {
      return FilterAssertionResult::NotFiltered;
    }
    const Expression& lhs{get_lhs_expression(f)};
    const Expression& rhs{get_rhs_expression(f)};
    if (is_variable(lhs) && is_constant(rhs)) {
      // var = v
      const Variable& var{get_variable(lhs)};
      const double v{get_constant_value(rhs)};
      return UpdateBoundsViaEquality(var, v, box);
    }
    if (is_constant(lhs) && is_variable(rhs)) {
      // v = var
      const double v{get_constant_value(lhs)};
      const Variable& var{get_variable(rhs)};
      return UpdateBoundsViaEquality(var, v, box);
    }
    return FilterAssertionResult::NotFiltered;
  }
  FilterAssertionResult VisitNotEqualTo(const Formula& f, Box* const box,
                                        const bool polarity) const {
    // Because (x != y) is !(x == y).
    return VisitEqualTo(f, box, !polarity);
  }
  FilterAssertionResult VisitGreaterThan(const Formula& f, Box* const box,
                                         const bool polarity) const {
    const Expression& lhs{get_lhs_expression(f)};
    const Expression& rhs{get_rhs_expression(f)};
    if (is_variable(lhs) && is_constant(rhs)) {
      const Variable& var{get_variable(lhs)};
      const double v{get_constant_value(rhs)};
      if (polarity) {
        // var > v
        return UpdateStrictLowerBound(var, v, box);
      } else {
        // !(var > v) => (var <= v)
        return UpdateUpperBound(var, v, box);
      }
    }
    if (is_constant(lhs) && is_variable(rhs)) {
      const double v{get_constant_value(lhs)};
      const Variable& var{get_variable(rhs)};
      if (polarity) {
        // v > var
        return UpdateStrictUpperBound(var, v, box);
      } else {
        // !(v > var) => (v <= var)
        return UpdateLowerBound(var, v, box);
      }
    }
    return FilterAssertionResult::NotFiltered;
  }
  FilterAssertionResult VisitGreaterThanOrEqualTo(const Formula& f,
                                                  Box* const box,
                                                  const bool polarity) const {
    const Expression& lhs{get_lhs_expression(f)};
    const Expression& rhs{get_rhs_expression(f)};
    if (is_variable(lhs) && is_constant(rhs)) {
      const Variable& var{get_variable(lhs)};
      const double v{get_constant_value(rhs)};
      if (polarity) {
        // var >= v
        return UpdateLowerBound(var, v, box);
      } else {
        // !(var >= v) => (var < v)
        return UpdateStrictUpperBound(var, v, box);
      }
    }
    if (is_constant(lhs) && is_variable(rhs)) {
      const double v{get_constant_value(lhs)};
      const Variable& var{get_variable(rhs)};
      if (polarity) {
        // v >= var
        return UpdateUpperBound(var, v, box);
      } else {
        // !(v >= var) => (v < var)
        return UpdateStrictLowerBound(var, v, box);
      }
    }
    return FilterAssertionResult::NotFiltered;
  }
  FilterAssertionResult VisitLessThan(const Formula& f, Box* const box,
                                      const bool polarity) const {
    // Because x < y is !(x >= y).
    return VisitGreaterThanOrEqualTo(f, box, !polarity);
  }
  FilterAssertionResult VisitLessThanOrEqualTo(const Formula& f, Box* const box,
                                               const bool polarity) const {
    // Because x <= y is !(x > y).
    return VisitGreaterThan(f, box, !polarity);
  }
  FilterAssertionResult VisitConjunction(const Formula&, Box* const,
                                         const bool) const {
    DREAL_UNREACHABLE();
  }
  FilterAssertionResult VisitDisjunction(const Formula&, Box* const,
                                         const bool) const {
    DREAL_UNREACHABLE();
  }
  FilterAssertionResult VisitNegation(const Formula& f, Box* const box,
                                      const bool polarity) const {
    return Visit(get_operand(f), box, !polarity);
  }
  FilterAssertionResult VisitForall(const Formula&, Box* const,
                                    const bool) const {
    return FilterAssertionResult::NotFiltered;
  }

  // Makes VisitFormula a friend of this class so that it can use private
  // operator()s.
  friend FilterAssertionResult
  drake::symbolic::VisitFormula<FilterAssertionResult>(AssertionFilter*,
                                                       const Formula&,
                                                       dreal::Box* const&,
                                                       const bool&);
};
}  // namespace

FilterAssertionResult FilterAssertion(const Formula& assertion,
                                      Box* const box) {
  return AssertionFilter{}.Process(assertion, box);
}
}  // namespace dreal
<|endoftext|>


<commit_before>/***
 * Infiniband maximum achievable bandwidth test.
 */

#include <stdio.h>

#include <vector>
#include <iostream>
#include <cmath>

#include "Maxfiles.h"
#include "MaxSLiCInterface.h"

using namespace std;

const double GB = pow(1024, 3) * 8;
const double MB = pow(1024, 2) * 8;

/** Maxmimum possible bandwidth at given clock speed*/
void printMaxInfinbandBandwidth() {
    double clock = 200.0;
    double inBitsPerCycle = 32 * 2;
    double outBitsPerCycle = 32;
    double max = (inBitsPerCycle + outBitsPerCycle) * clock * pow10(6) / GB;
    cout << "Max possible bandwidth " << max << " GB/s" << endl;
}

double measuredInifibandBandwidth(long sizeInBits, double runtimeS) {
    return 3 * size / MB / runtimeS;
}

int main(void)
{

    const long inSize = 2 * 384 * 1E6;

    std::vector<int> a(inSize), b(inSize), expected(inSize), out(inSize, 0);

    for(long i = 0; i < inSize; ++i) {
        a[i] = i + 1;
        b[i] = i - 1;
        expected[i] = 2 * i;
    }

    printMaxInfinbandBandwidth();
    cout << "Running on DFE." << endl;
    struct timeval tv1, tv2;
    gettimeofday(&tv1, NULL);
    InfinibandBenchmark(inSize, &a[0], &b[0], &out[0]);
    gettimeofday(&tv2, NULL);

    cout << "Data size " << inSize * sizeof(int) / GB  << " GBs" << endl;
    double runtimeS = ((tv2.tv_sec-tv1.tv_sec) * (double)1E6 +
                       (tv2.tv_usec-tv1.tv_usec)) / (double)1E6;
    cout << "Runtime (s) " << runtimeS << endl;
    cout << "Bandwidth " << measuredInifibandBandwidth(inSize * sizeof(int),  runtimeS) << " MB/s " << endl;

    for (int i = 0; i < inSize; i++)
        if (out[i] != expected[i]) {
            printf("Output from DFE did not match CPU: %d : %d != %d\n",
                   i, out[i], expected[i]);
            return 1;
        }

    std::cout << "Test passed!" << std::endl;
    return 0;
}
<commit_msg>Change some UoM.<commit_after>/***
 * Infiniband maximum achievable bandwidth test.
 */

#include <stdio.h>

#include <vector>
#include <iostream>
#include <cmath>

#include "Maxfiles.h"
#include "MaxSLiCInterface.h"

using namespace std;

// All measurements should be in bytes
const double GB = pow(1024, 3);
const double MB = pow(1024, 2);

/** Maxmimum possible bandwidth at given clock speed*/
void printMaxInfinbandBandwidth() {
    double clock = 150.0;
    double inBytesPerCycle = sizeof(int) * 2;
    double outBytesPerCycle = sizeof(int);
    double max = (inBytesPerCycle + outBytesPerCycle) * clock * 1E6 / GB;
    cout << "Max possible bandwidth (@150 MHz Stream Clock): " << max << " GB/s" << endl;
}

double measuredInifibandBandwidth(long sizeInBytes, double runtimeS) {
    return sizeInBytes / MB / runtimeS;
}

int main(void)
{

    const long inSize = 2 * 384 * 1E6;
    const long dataSizeBytes = 3 * inSize * sizeof(int);

    std::vector<int> a(inSize), b(inSize), expected(inSize), out(inSize, 0);

    for(long i = 0; i < inSize; ++i) {
        a[i] = i + 1;
        b[i] = i - 1;
        expected[i] = 2 * i;
    }

    printMaxInfinbandBandwidth();
    cout << "Running on DFE. Data size: " << dataSizeBytes / GB << " GB" << endl;

    struct timeval tv1, tv2;
    gettimeofday(&tv1, NULL);
    InfinibandBenchmark(inSize, &a[0], &b[0], &out[0]);
    gettimeofday(&tv2, NULL);

    double runtimeS = ((tv2.tv_sec-tv1.tv_sec) * (double)1E6 +
                       (tv2.tv_usec-tv1.tv_usec)) / (double)1E6;

    cout << "Runtime (s) " << runtimeS << endl;
    cout << "Bandwidth " << measuredInifibandBandwidth(dataSizeBytes,  runtimeS);
    cout << " MB/s " << endl;

    for (int i = 0; i < inSize; i++)
        if (out[i] != expected[i]) {
            printf("Output from DFE did not match CPU: %d : %d != %d\n",
                   i, out[i], expected[i]);
            return 1;
        }

    std::cout << "Test passed!" << std::endl;
    return 0;
}
<|endoftext|>