Esempio n. 1
0
void JMXStatusDCmd::execute(DCmdSource source, TRAPS) {
  ResourceMark rm(THREAD);
  HandleMark hm(THREAD);

  // Load and initialize the sun.management.Agent class
  // invoke getManagementAgentStatus() method to generate the status info
  // throw java.lang.NoSuchMethodError if method doesn't exist

  Handle loader = Handle(THREAD, SystemDictionary::java_system_loader());
  Klass* k = SystemDictionary::resolve_or_fail(vmSymbols::sun_management_Agent(), loader, Handle(), true, CHECK);
  instanceKlassHandle ik (THREAD, k);

  JavaValue result(T_OBJECT);
  JavaCalls::call_static(&result, ik, vmSymbols::getAgentStatus_name(), vmSymbols::void_string_signature(), CHECK);

  jvalue* jv = (jvalue*) result.get_value_addr();
  oop str = (oop) jv->l;
  if (str != NULL) {
      char* out = java_lang_String::as_utf8_string(str);
      if (out) {
          output()->print_cr("%s", out);
          return;
      }
  }
  output()->print_cr("Error obtaining management agent status");
}
// Write the method information portion of ClassFile structure
// JVMSpec|     u2 methods_count;
// JVMSpec|     method_info methods[methods_count];
void JvmtiClassFileReconstituter::write_method_infos() {
  HandleMark hm(thread());
  objArrayHandle methods(thread(), ikh()->methods());
  int num_methods = methods->length();

  write_u2(num_methods);
  if (JvmtiExport::can_maintain_original_method_order()) {
    int index;
    int original_index;
    int* method_order = NEW_RESOURCE_ARRAY(int, num_methods);

    // invert the method order mapping
    for (index = 0; index < num_methods; index++) {
      original_index = ikh()->method_ordering()->int_at(index);
      assert(original_index >= 0 && original_index < num_methods,
             "invalid original method index");
      method_order[original_index] = index;
    }

    // write in original order
    for (original_index = 0; original_index < num_methods; original_index++) {
      index = method_order[original_index];
      methodHandle method(thread(), (methodOop)(ikh()->methods()->obj_at(index)));
      write_method_info(method);
    }
  } else {
    // method order not preserved just dump the method infos
    for (int index = 0; index < num_methods; index++) {
void JMXStartRemoteDCmd::execute(TRAPS) {
    ResourceMark rm(THREAD);
    HandleMark hm(THREAD);

    // Load and initialize the sun.management.Agent class
    // invoke startRemoteManagementAgent(string) method to start
    // the remote management server.
    // throw java.lang.NoSuchMethodError if the method doesn't exist

    Handle loader = Handle(THREAD, SystemDictionary::java_system_loader());
    klassOop k = SystemDictionary::resolve_or_fail(vmSymbols::sun_management_Agent(), loader, Handle(), true, CHECK);
    instanceKlassHandle ik (THREAD, k);

    JavaValue result(T_VOID);

    // Pass all command line arguments to java as key=value,...
    // All checks are done on java side

    int len = 0;
    stringStream options;
    char comma[2] = {0,0};

    // Leave default values on Agent.class side and pass only
    // agruments explicitly set by user. All arguments passed
    // to jcmd override properties with the same name set by
    // command line with -D or by managmenent.properties
    // file.
#define PUT_OPTION(a) \
    if ( (a).is_set() ){ \
        options.print("%scom.sun.management.%s=%s", comma, (a).name(), (a).value()); \
        comma[0] = ','; \
    }

    PUT_OPTION(_config_file);
    PUT_OPTION(_jmxremote_port);
    PUT_OPTION(_jmxremote_rmi_port);
    PUT_OPTION(_jmxremote_ssl);
    PUT_OPTION(_jmxremote_registry_ssl);
    PUT_OPTION(_jmxremote_authenticate);
    PUT_OPTION(_jmxremote_password_file);
    PUT_OPTION(_jmxremote_access_file);
    PUT_OPTION(_jmxremote_login_config);
    PUT_OPTION(_jmxremote_ssl_enabled_cipher_suites);
    PUT_OPTION(_jmxremote_ssl_enabled_protocols);
    PUT_OPTION(_jmxremote_ssl_need_client_auth);
    PUT_OPTION(_jmxremote_ssl_config_file);

#undef PUT_OPTION

    Handle str = java_lang_String::create_from_str(options.as_string(), CHECK);
    JavaCalls::call_static(&result, ik, vmSymbols::startRemoteAgent_name(), vmSymbols::string_void_signature(), str, CHECK);
}
void JvmtiClassFileReconstituter::write_class_file_format() {
  ReallocMark();

  // JVMSpec|   ClassFile {
  // JVMSpec|           u4 magic;
  write_u4(0xCAFEBABE);

  // JVMSpec|           u2 minor_version;
  // JVMSpec|           u2 major_version;
  write_u2(ikh()->minor_version());
  u2 major = ikh()->major_version();
  write_u2(major);

  // JVMSpec|           u2 constant_pool_count;
  // JVMSpec|           cp_info constant_pool[constant_pool_count-1];
  write_u2(cpool()->length());
  copy_cpool_bytes(writeable_address(cpool_size()));

  // JVMSpec|           u2 access_flags;
  write_u2(ikh()->access_flags().get_flags() & JVM_RECOGNIZED_CLASS_MODIFIERS);

  // JVMSpec|           u2 this_class;
  // JVMSpec|           u2 super_class;
  write_u2(class_symbol_to_cpool_index(ikh()->name()));
  Klass* super_class = ikh()->super();
  write_u2(super_class == NULL? 0 :  // zero for java.lang.Object
                class_symbol_to_cpool_index(super_class->name()));

  // JVMSpec|           u2 interfaces_count;
  // JVMSpec|           u2 interfaces[interfaces_count];
  Array<Klass*>* interfaces =  ikh()->local_interfaces();
  int num_interfaces = interfaces->length();
  write_u2(num_interfaces);
  for (int index = 0; index < num_interfaces; index++) {
    HandleMark hm(thread());
    instanceKlassHandle iikh(thread(), interfaces->at(index));
    write_u2(class_symbol_to_cpool_index(iikh->name()));
  }

  // JVMSpec|           u2 fields_count;
  // JVMSpec|           field_info fields[fields_count];
  write_field_infos();

  // JVMSpec|           u2 methods_count;
  // JVMSpec|           method_info methods[methods_count];
  write_method_infos();

  // JVMSpec|           u2 attributes_count;
  // JVMSpec|           attribute_info attributes[attributes_count];
  // JVMSpec|   } /* end ClassFile 8?
  write_class_attributes();
}
Esempio n. 5
0
static void loadAgentModule(TRAPS) {
  ResourceMark rm(THREAD);
  HandleMark hm(THREAD);

  JavaValue result(T_OBJECT);
  Handle h_module_name = java_lang_String::create_from_str("jdk.management.agent", CHECK);
  JavaCalls::call_static(&result,
                         SystemDictionary::module_Modules_klass(),
                         vmSymbols::loadModule_name(),
                         vmSymbols::loadModule_signature(),
                         h_module_name,
                         THREAD);
}
/** Main entry point. */
int main(int argc, char **argv)
{
  ros::init(argc, argv, "heightmap_node");
  ros::NodeHandle node;
  ros::NodeHandle priv_nh("~");

  // create height map class, which subscribes to velodyne_points
  velodyne_height_map::HeightMap hm(node, priv_nh);

  // handle callbacks until shut down
  ros::spin();

  return 0;
}
Esempio n. 7
0
cv::Mat pointsToMat(std::vector<cv::Point2d>& pts)
{
    // each column of hm is a homogeneous coordinate of a point.
    int c=pts.size(); // amount of points
    cv::Mat hm(3,c,CV_64FC1,cv::Scalar(1.0));

    cv::Mat m(pts);
    m=m.reshape(1,c);
    m=m.t();

    cv::Mat roi(hm, cv::Rect(0,0,c,2));
    m.copyTo(roi);

    return hm;
}
Esempio n. 8
0
void testMultiA(){
	/**
	 * 多维数组数组第一维的元素实际上是另一个数组
	 * int matrix[3][0]可以看作是一个一维数组,包含三个元素,只是每个元素恰好是包含10个整型元素的数组。
	 * matrix这个名字的值是一个指向它的第一个元素的指针,所以matrix是一个指向一个包含10个整数元素的数组的指针。
	 * */
	short m[2][3]={100,101,102,110,111,112};
	short (*p)[3]=m;
	printf("p:sizeof(p):%d,sizeof(p[0]):%d,sizeof(p[1]:%d\n",sizeof(p),sizeof(p[0]),sizeof(p[1]));

	printf("sizeof(m):%d,sizeof(m[0]):%d,sizeof(m[1]:%d\n",sizeof(m),sizeof(m[0]),sizeof(m[1]));
	printf("*((m+1)+1):%d,*(m[1]+1):%d,m[1][1]:%d\n",*(*(m+1)+1),*(m[1]+1),m[1][1]);
	hm(m);
	hm1(m);
}
Esempio n. 9
0
void JMXStopRemoteDCmd::execute(DCmdSource source, TRAPS) {
    ResourceMark rm(THREAD);
    HandleMark hm(THREAD);

    // Load and initialize the sun.management.Agent class
    // invoke stopRemoteManagementAgent method to stop the
    // management server
    // throw java.lang.NoSuchMethodError if method doesn't exist

    Handle loader = Handle(THREAD, SystemDictionary::java_system_loader());
    Klass* k = SystemDictionary::resolve_or_fail(vmSymbols::sun_management_Agent(), loader, Handle(), true, CHECK);
    instanceKlassHandle ik (THREAD, k);

    JavaValue result(T_VOID);
    JavaCalls::call_static(&result, ik, vmSymbols::stopRemoteAgent_name(), vmSymbols::void_method_signature(), CHECK);
}
Esempio n. 10
0
double *iter2(double *y0, double x0, double h, double *y, int cnt)
{
	int i;
	double k1,k2,k3,k4;
	double *tmp = malloc (cnt * sizeof(double));
	for (i = 0; i < cnt; i++)
	{
		k1 = func[i](x0, y0);
		k2 = func[i](x0 + h/2, hn(y0, cnt, h, k1, tmp));
		k3 = func[i](x0 + h/2, hn(y0, cnt, h, k2, tmp));
		k4 = func[i](x0 + h, hm(y0, cnt, h, k3, tmp));
		y[i] = y0[i] + (k1 + 2 * k2 + 2 * k3 + k4) * h / 6;
	}
	free(tmp);
	return y;
}
void LowMemoryDetector::process_sensor_changes(TRAPS) {
  ResourceMark rm(THREAD);
  HandleMark hm(THREAD);

  // No need to hold Service_lock to call out to Java
  int num_memory_pools = MemoryService::num_memory_pools();
  for (int i = 0; i < num_memory_pools; i++) {
    MemoryPool* pool = MemoryService::get_memory_pool(i);
    SensorInfo* sensor = pool->usage_sensor();
    SensorInfo* gc_sensor = pool->gc_usage_sensor();
    if (sensor != NULL && sensor->has_pending_requests()) {
      sensor->process_pending_requests(CHECK);
    }
    if (gc_sensor != NULL && gc_sensor->has_pending_requests()) {
      gc_sensor->process_pending_requests(CHECK);
    }
  }
}
void LowMemoryDetector::low_memory_detector_thread_entry(JavaThread* jt, TRAPS) {
  while (true) {
    bool   sensors_changed = false;

    {
      // _no_safepoint_check_flag is used here as LowMemory_lock is a
      // special lock and the VMThread may acquire this lock at safepoint.
      // Need state transition ThreadBlockInVM so that this thread
      // will be handled by safepoint correctly when this thread is
      // notified at a safepoint.

      // This ThreadBlockInVM object is not also considered to be
      // suspend-equivalent because LowMemoryDetector threads are
      // not visible to external suspension.

      ThreadBlockInVM tbivm(jt);

      MutexLockerEx ml(LowMemory_lock, Mutex::_no_safepoint_check_flag);
      while (!(sensors_changed = has_pending_requests())) {
        // wait until one of the sensors has pending requests
        LowMemory_lock->wait(Mutex::_no_safepoint_check_flag);
      }
    }

    {
      ResourceMark rm(THREAD);
      HandleMark hm(THREAD);

      // No need to hold LowMemory_lock to call out to Java
      int num_memory_pools = MemoryService::num_memory_pools();
      for (int i = 0; i < num_memory_pools; i++) {
        MemoryPool* pool = MemoryService::get_memory_pool(i);
        SensorInfo* sensor = pool->usage_sensor();
        SensorInfo* gc_sensor = pool->gc_usage_sensor();
        if (sensor != NULL && sensor->has_pending_requests()) {
          sensor->process_pending_requests(CHECK);
        }
        if (gc_sensor != NULL && gc_sensor->has_pending_requests()) {
          gc_sensor->process_pending_requests(CHECK);
        }
      }
    }
  }
}
// Write the method information portion of ClassFile structure
// JVMSpec|     u2 methods_count;
// JVMSpec|     method_info methods[methods_count];
void JvmtiClassFileReconstituter::write_method_infos() {
  HandleMark hm(thread());
  Array<Method*>* methods = ikh()->methods();
  int num_methods = methods->length();
  int num_overpass = 0;

  // count the generated default interface methods
  // these will not be re-created by write_method_info
  // and should not be included in the total count
  for (int index = 0; index < num_methods; index++) {
    Method* method = methods->at(index);
    if (method->is_overpass()) {
      num_overpass++;
    }
  }

  write_u2(num_methods - num_overpass);
  if (JvmtiExport::can_maintain_original_method_order()) {
    int index;
    int original_index;
    intArray method_order(num_methods, 0);

    // invert the method order mapping
    for (index = 0; index < num_methods; index++) {
      original_index = ikh()->method_ordering()->at(index);
      assert(original_index >= 0 && original_index < num_methods,
             "invalid original method index");
      method_order.at_put(original_index, index);
    }

    // write in original order
    for (original_index = 0; original_index < num_methods; original_index++) {
      index = method_order.at(original_index);
      methodHandle method(thread(), methods->at(index));
      write_method_info(method);
    }
  } else {
    // method order not preserved just dump the method infos
    for (int index = 0; index < num_methods; index++) {
      methodHandle method(thread(), methods->at(index));
      write_method_info(method);
    }
  }
}
Esempio n. 14
0
void connection_handler::handle_accepts()
{
    detail::handling_messages hm(handling_accepts_);       // reset on exit
    std::size_t k = 64;
    while(!stopped_)
    {
        hpx::util::high_resolution_timer t;
        boost::shared_ptr<receiver> rcv = acceptor_.accept(*this, memory_pool_, boost::system::throws);
        if(rcv)
        {
            rcv->async_read(boost::system::throws);
            {
                hpx::lcos::local::spinlock::scoped_lock l(receivers_mtx_);
                receivers_.push_back(rcv);
            }
        }
        time_acct += t.elapsed();

        hpx::lcos::local::spinlock::yield(k);
    }
}
Esempio n. 15
0
jvmtiError
JvmtiEnvBase::get_frame_location(JavaThread *java_thread, jint depth,
                                 jmethodID* method_ptr, jlocation* location_ptr) {
#ifdef ASSERT
  uint32_t debug_bits = 0;
#endif
  assert((SafepointSynchronize::is_at_safepoint() ||
          is_thread_fully_suspended(java_thread, false, &debug_bits)),
         "at safepoint or target thread is suspended");
  Thread* current_thread = Thread::current();
  ResourceMark rm(current_thread);

  vframe *vf = vframeFor(java_thread, depth);
  if (vf == NULL) {
    return JVMTI_ERROR_NO_MORE_FRAMES;
  }

  // vframeFor should return a java frame. If it doesn't
  // it means we've got an internal error and we return the
  // error in product mode. In debug mode we will instead
  // attempt to cast the vframe to a javaVFrame and will
  // cause an assertion/crash to allow further diagnosis.
#ifdef PRODUCT
  if (!vf->is_java_frame()) {
    return JVMTI_ERROR_INTERNAL;
  }
#endif

  HandleMark hm(current_thread);
  javaVFrame *jvf = javaVFrame::cast(vf);
  Method* method = jvf->method();
  if (method->is_native()) {
    *location_ptr = -1;
  } else {
    *location_ptr = jvf->bci();
  }
  *method_ptr = method->jmethod_id();

  return JVMTI_ERROR_NONE;
}
Esempio n. 16
0
void connection_handler::handle_messages()
{
    detail::handling_messages hm(handling_messages_);       // reset on exit

    bool bootstrapping = hpx::is_starting();
    bool has_work = true;
    std::size_t k = 0;

    hpx::util::high_resolution_timer t;
    // We let the message handling loop spin for another 2 seconds to avoid the
    // costs involved with posting it to asio
    while(bootstrapping || (!stopped_ && has_work) || (!has_work && t.elapsed() < 2.0))
    {
        // handle all sends ...
        has_work = do_sends();
        // handle all receives ...
        if(do_receives())
        {
            has_work = true;
        }

        if (bootstrapping)
            bootstrapping = hpx::is_starting();

        if(has_work)
        {
            t.restart();
            k = 0;
        }
        else
        {
            if(enable_parcel_handling_)
            {
                hpx::lcos::local::spinlock::yield(k);
                ++k;
            }
        }
    }
}
Esempio n. 17
0
void GCNotifier::sendNotificationInternal(TRAPS) {
  ResourceMark rm(THREAD);
  HandleMark hm(THREAD);
  GCNotificationRequest *request = getRequest();
  if (request != NULL) {
    NotificationMark nm(request);
    Handle objGcInfo = createGcInfo(request->gcManager, request->gcStatInfo, THREAD);

    Handle objName = java_lang_String::create_from_platform_dependent_str(request->gcManager->name(), CHECK);
    Handle objAction = java_lang_String::create_from_platform_dependent_str(request->gcAction, CHECK);
    Handle objCause = java_lang_String::create_from_platform_dependent_str(request->gcCause, CHECK);

    Klass* k = Management::sun_management_GarbageCollectorImpl_klass(CHECK);
    instanceKlassHandle gc_mbean_klass(THREAD, k);

    instanceOop gc_mbean = request->gcManager->get_memory_manager_instance(THREAD);
    instanceHandle gc_mbean_h(THREAD, gc_mbean);
    if (!gc_mbean_h->is_a(k)) {
      THROW_MSG(vmSymbols::java_lang_IllegalArgumentException(),
                "This GCMemoryManager doesn't have a GarbageCollectorMXBean");
    }

    JavaValue result(T_VOID);
    JavaCallArguments args(gc_mbean_h);
    args.push_long(request->timestamp);
    args.push_oop(objName);
    args.push_oop(objAction);
    args.push_oop(objCause);
    args.push_oop(objGcInfo);

    JavaCalls::call_virtual(&result,
                            gc_mbean_klass,
                            vmSymbols::createGCNotification_name(),
                            vmSymbols::createGCNotification_signature(),
                            &args,
                            CHECK);
  }
}
Esempio n. 18
0
void buf_flush()
{
    int i;
    if (strcmp(text[0], "") == 0) return;
    switch (buf_type) {
        case TYPE_TEXT:
            printf("<p>\n");
            break;
        case TYPE_PRE:
            printf("<pre>\n");
            break;
        case TYPE_LI:
            printf("<ul>\n");
            break;
    }
    for (i = 0; i < 100; i++) {
        if (strcmp(text[i], "") == 0) break;
        if (buf_type == TYPE_LI)
            printf("<li>");
        hm(text[i]);
        o("\n");
        if (buf_type == TYPE_LI)
            printf("</li>");
    }
    switch (buf_type) {
        case TYPE_TEXT:
            printf("</p>\n");
            break;
        case TYPE_PRE:
            printf("</pre>\n");
            break;
        case TYPE_LI:
            printf("</ul>\n");
            break;
    }
    buf_clear();
}
Esempio n. 19
0
static void
ngx_nats_process_msg(ngx_nats_connection_t *nc, ngx_nats_buf_t *buf,
                ngx_nats_msg_t *msg)
{
    ngx_nats_subscription_t    *sub;
    ngx_nats_handle_msg_pt      hm;
    void                       *sub_data;
    ngx_nats_client_t          *client;
    ngx_str_t                  *r = NULL;
    ngx_int_t                   sid;
    ngx_pool_t                 *pool;
    ngx_nats_message_t         *m;

    sid  = msg->sid;

    sub = ngx_nats_get_subscription(nc, sid);
    if (sub == NULL) {
        return;
    }

    if (msg->replyto.len > 0) {
        r = &msg->replyto;
    }

    /* we may remove subscription so cache these */
    hm = sub->handle_msg;
    sub_data = sub->client_subscription_data;
    client = sub->client;

    if (sub->max > 0) {
        sub->recv++;
        if (sub->recv >= sub->max) {
            ngx_nats_remove_subscription(nc, sub->sid);
        }
    }

    pool = ngx_create_pool(NGX_DEFAULT_POOL_SIZE, nc->nd->log);
    if (pool == NULL) {
        ngx_log_error(NGX_LOG_CRIT, nc->nd->log, 0,
                "%s: ngx_create_pool failed", __func__);
        return;
    }

    m = ngx_pcalloc(pool, sizeof(ngx_nats_message_t));
    if (m == NULL) {
        ngx_log_error(NGX_LOG_CRIT, nc->nd->log, 0,
                "%s: ngx_pcalloc failed", __func__);
        goto DONE;
    }

    m->client                   = client;
    m->sid                      = sid;
    m->subject                  = &msg->subject;
    m->replyto                  = r;
    m->pool                     = pool;
    m->client_subscription_data = sub_data;
    m->data.data                = (u_char *) (buf->buf + buf->pos + msg->bstart);
    m->data.len                 = msg->bend - msg->bstart;

    hm(m);

DONE:
    ngx_destroy_pool(pool);
}
Esempio n. 20
0
static void find(intptr_t x, bool print_pc) {
  address addr = (address)x;

  CodeBlob* b = CodeCache::find_blob_unsafe(addr);
  if (b != NULL) {
    if (b->is_buffer_blob()) {
      // the interpreter is generated into a buffer blob
      InterpreterCodelet* i = Interpreter::codelet_containing(addr);
      if (i != NULL) {
        i->print();
        return;
      }
      if (Interpreter::contains(addr)) {
        tty->print_cr(INTPTR_FORMAT " is pointing into interpreter code (not bytecode specific)", addr);
        return;
      }
      //
      if (AdapterHandlerLibrary::contains(b)) {
        AdapterHandlerLibrary::print_handler(b);
      }
      // the stubroutines are generated into a buffer blob
      StubCodeDesc* d = StubCodeDesc::desc_for(addr);
      if (d != NULL) {
        d->print();
        if (print_pc) tty->cr();
        return;
      }
      if (StubRoutines::contains(addr)) {
        tty->print_cr(INTPTR_FORMAT " is pointing to an (unnamed) stub routine", addr);
        return;
      }
      // the InlineCacheBuffer is using stubs generated into a buffer blob
      if (InlineCacheBuffer::contains(addr)) {
        tty->print_cr(INTPTR_FORMAT " is pointing into InlineCacheBuffer", addr);
        return;
      }
      VtableStub* v = VtableStubs::stub_containing(addr);
      if (v != NULL) {
        v->print();
        return;
      }
    }
    if (print_pc && b->is_nmethod()) {
      ResourceMark rm;
      tty->print("%#p: Compiled ", addr);
      ((nmethod*)b)->method()->print_value_on(tty);
      tty->print("  = (CodeBlob*)" INTPTR_FORMAT, b);
      tty->cr();
      return;
    }
    if ( b->is_nmethod()) {
      if (b->is_zombie()) {
        tty->print_cr(INTPTR_FORMAT " is zombie nmethod", b);
      } else if (b->is_not_entrant()) {
        tty->print_cr(INTPTR_FORMAT " is non-entrant nmethod", b);
      }
    }
    b->print();
    return;
  }

  if (Universe::heap()->is_in(addr)) {
    HeapWord* p = Universe::heap()->block_start(addr);
    bool print = false;
    // If we couldn't find it it just may mean that heap wasn't parseable
    // See if we were just given an oop directly
    if (p != NULL && Universe::heap()->block_is_obj(p)) {
      print = true;
    } else if (p == NULL && ((oopDesc*)addr)->is_oop()) {
      p = (HeapWord*) addr;
      print = true;
    }
    if (print) {
      oop(p)->print();
      if (p != (HeapWord*)x && oop(p)->is_constMethod() &&
          constMethodOop(p)->contains(addr)) {
        Thread *thread = Thread::current();
        HandleMark hm(thread);
        methodHandle mh (thread, constMethodOop(p)->method());
        if (!mh->is_native()) {
          tty->print_cr("bci_from(%p) = %d; print_codes():",
                        addr, mh->bci_from(address(x)));
          mh->print_codes();
        }
      }
      return;
    }
  } else if (Universe::heap()->is_in_reserved(addr)) {
    tty->print_cr(INTPTR_FORMAT " is an unallocated location in the heap", addr);
    return;
  }

  if (JNIHandles::is_global_handle((jobject) addr)) {
    tty->print_cr(INTPTR_FORMAT " is a global jni handle", addr);
    return;
  }
  if (JNIHandles::is_weak_global_handle((jobject) addr)) {
    tty->print_cr(INTPTR_FORMAT " is a weak global jni handle", addr);
    return;
  }
  if (JNIHandleBlock::any_contains((jobject) addr)) {
    tty->print_cr(INTPTR_FORMAT " is a local jni handle", addr);
    return;
  }

  for(JavaThread *thread = Threads::first(); thread; thread = thread->next()) {
    // Check for privilege stack
    if (thread->privileged_stack_top() != NULL && thread->privileged_stack_top()->contains(addr)) {
      tty->print_cr(INTPTR_FORMAT " is pointing into the privilege stack for thread: " INTPTR_FORMAT, addr, thread);
      return;
    }
    // If the addr is a java thread print information about that.
    if (addr == (address)thread) {
       thread->print();
       return;
    }
  }

  // Try an OS specific find
  if (os::find(addr)) {
    return;
  }

  if (print_pc) {
    tty->print_cr(INTPTR_FORMAT ": probably in C++ code; check debugger", addr);
    Disassembler::decode(same_page(addr-40,addr),same_page(addr+40,addr));
    return;
  }

  tty->print_cr(INTPTR_FORMAT " is pointing to unknown location", addr);
}
Esempio n. 21
0
int CppInterpreter::native_entry(methodOop method, intptr_t UNUSED, TRAPS) {
  // Make sure method is native and not abstract
  assert(method->is_native() && !method->is_abstract(), "should be");

  JavaThread *thread = (JavaThread *) THREAD;
  ZeroStack *stack = thread->zero_stack();

  // Allocate and initialize our frame
  InterpreterFrame *frame = InterpreterFrame::build(method, CHECK_0);
  thread->push_zero_frame(frame);
  interpreterState istate = frame->interpreter_state();
  intptr_t *locals = istate->locals();

  // Update the invocation counter
  if ((UseCompiler || CountCompiledCalls) && !method->is_synchronized()) {
    InvocationCounter *counter = method->invocation_counter();
    counter->increment();
    if (counter->reached_InvocationLimit()) {
      CALL_VM_NOCHECK(
        InterpreterRuntime::frequency_counter_overflow(thread, NULL));
      if (HAS_PENDING_EXCEPTION)
        goto unwind_and_return;
    }
  }

  // Lock if necessary
  BasicObjectLock *monitor;
  monitor = NULL;
  if (method->is_synchronized()) {
    monitor = (BasicObjectLock*) istate->stack_base();
    oop lockee = monitor->obj();
    markOop disp = lockee->mark()->set_unlocked();

    monitor->lock()->set_displaced_header(disp);
    if (Atomic::cmpxchg_ptr(monitor, lockee->mark_addr(), disp) != disp) {
      if (thread->is_lock_owned((address) disp->clear_lock_bits())) {
        monitor->lock()->set_displaced_header(NULL);
      }
      else {
        CALL_VM_NOCHECK(InterpreterRuntime::monitorenter(thread, monitor));
        if (HAS_PENDING_EXCEPTION)
          goto unwind_and_return;
      }
    }
  }

  // Get the signature handler
  InterpreterRuntime::SignatureHandler *handler; {
    address handlerAddr = method->signature_handler();
    if (handlerAddr == NULL) {
      CALL_VM_NOCHECK(InterpreterRuntime::prepare_native_call(thread, method));
      if (HAS_PENDING_EXCEPTION)
        goto unlock_unwind_and_return;

      handlerAddr = method->signature_handler();
      assert(handlerAddr != NULL, "eh?");
    }
    if (handlerAddr == (address) InterpreterRuntime::slow_signature_handler) {
      CALL_VM_NOCHECK(handlerAddr =
        InterpreterRuntime::slow_signature_handler(thread, method, NULL,NULL));
      if (HAS_PENDING_EXCEPTION)
        goto unlock_unwind_and_return;
    }
    handler = \
      InterpreterRuntime::SignatureHandler::from_handlerAddr(handlerAddr);
  }

  // Get the native function entry point
  address function;
  function = method->native_function();
  assert(function != NULL, "should be set if signature handler is");

  // Build the argument list
  stack->overflow_check(handler->argument_count() * 2, THREAD);
  if (HAS_PENDING_EXCEPTION)
    goto unlock_unwind_and_return;

  void **arguments;
  void *mirror; {
    arguments =
      (void **) stack->alloc(handler->argument_count() * sizeof(void **));
    void **dst = arguments;

    void *env = thread->jni_environment();
    *(dst++) = &env;

    if (method->is_static()) {
      istate->set_oop_temp(
        method->constants()->pool_holder()->java_mirror());
      mirror = istate->oop_temp_addr();
      *(dst++) = &mirror;
    }

    intptr_t *src = locals;
    for (int i = dst - arguments; i < handler->argument_count(); i++) {
      ffi_type *type = handler->argument_type(i);
      if (type == &ffi_type_pointer) {
        if (*src) {
          stack->push((intptr_t) src);
          *(dst++) = stack->sp();
        }
        else {
          *(dst++) = src;
        }
        src--;
      }
      else if (type->size == 4) {
        *(dst++) = src--;
      }
      else if (type->size == 8) {
        src--;
        *(dst++) = src--;
      }
      else {
        ShouldNotReachHere();
      }
    }
  }

  // Set up the Java frame anchor
  thread->set_last_Java_frame();

  // Change the thread state to _thread_in_native
  ThreadStateTransition::transition_from_java(thread, _thread_in_native);

  // Make the call
  intptr_t result[4 - LogBytesPerWord];
  ffi_call(handler->cif(), (void (*)()) function, result, arguments);

  // Change the thread state back to _thread_in_Java.
  // ThreadStateTransition::transition_from_native() cannot be used
  // here because it does not check for asynchronous exceptions.
  // We have to manage the transition ourself.
  thread->set_thread_state(_thread_in_native_trans);

  // Make sure new state is visible in the GC thread
  if (os::is_MP()) {
    if (UseMembar) {
      OrderAccess::fence();
    }
    else {
      InterfaceSupport::serialize_memory(thread);
    }
  }

  // Handle safepoint operations, pending suspend requests,
  // and pending asynchronous exceptions.
  if (SafepointSynchronize::do_call_back() ||
      thread->has_special_condition_for_native_trans()) {
    JavaThread::check_special_condition_for_native_trans(thread);
    CHECK_UNHANDLED_OOPS_ONLY(thread->clear_unhandled_oops());
  }

  // Finally we can change the thread state to _thread_in_Java.
  thread->set_thread_state(_thread_in_Java);
  fixup_after_potential_safepoint();

  // Clear the frame anchor
  thread->reset_last_Java_frame();

  // If the result was an oop then unbox it and store it in
  // oop_temp where the garbage collector can see it before
  // we release the handle it might be protected by.
  if (handler->result_type() == &ffi_type_pointer) {
    if (result[0])
      istate->set_oop_temp(*(oop *) result[0]);
    else
      istate->set_oop_temp(NULL);
  }

  // Reset handle block
  thread->active_handles()->clear();

 unlock_unwind_and_return:

  // Unlock if necessary
  if (monitor) {
    BasicLock *lock = monitor->lock();
    markOop header = lock->displaced_header();
    oop rcvr = monitor->obj();
    monitor->set_obj(NULL);

    if (header != NULL) {
      if (Atomic::cmpxchg_ptr(header, rcvr->mark_addr(), lock) != lock) {
        monitor->set_obj(rcvr); {
          HandleMark hm(thread);
          CALL_VM_NOCHECK(InterpreterRuntime::monitorexit(thread, monitor));
        }
      }
    }
  }

 unwind_and_return:

  // Unwind the current activation
  thread->pop_zero_frame();

  // Pop our parameters
  stack->set_sp(stack->sp() + method->size_of_parameters());

  // Push our result
  if (!HAS_PENDING_EXCEPTION) {
    BasicType type = result_type_of(method);
    stack->set_sp(stack->sp() - type2size[type]);

    switch (type) {
    case T_VOID:
      break;

    case T_BOOLEAN:
#ifndef VM_LITTLE_ENDIAN
      result[0] <<= (BitsPerWord - BitsPerByte);
#endif
      SET_LOCALS_INT(*(jboolean *) result != 0, 0);
      break;

    case T_CHAR:
#ifndef VM_LITTLE_ENDIAN
      result[0] <<= (BitsPerWord - BitsPerShort);
#endif
      SET_LOCALS_INT(*(jchar *) result, 0);
      break;

    case T_BYTE:
#ifndef VM_LITTLE_ENDIAN
      result[0] <<= (BitsPerWord - BitsPerByte);
#endif
      SET_LOCALS_INT(*(jbyte *) result, 0);
      break;

    case T_SHORT:
#ifndef VM_LITTLE_ENDIAN
      result[0] <<= (BitsPerWord - BitsPerShort);
#endif
      SET_LOCALS_INT(*(jshort *) result, 0);
      break;

    case T_INT:
#ifndef VM_LITTLE_ENDIAN
      result[0] <<= (BitsPerWord - BitsPerInt);
#endif
      SET_LOCALS_INT(*(jint *) result, 0);
      break;

    case T_LONG:
      SET_LOCALS_LONG(*(jlong *) result, 0);
      break;

    case T_FLOAT:
      SET_LOCALS_FLOAT(*(jfloat *) result, 0);
      break;

    case T_DOUBLE:
      SET_LOCALS_DOUBLE(*(jdouble *) result, 0);
      break;

    case T_OBJECT:
    case T_ARRAY:
      SET_LOCALS_OBJECT(istate->oop_temp(), 0);
      break;

    default:
      ShouldNotReachHere();
    }
  }

  // No deoptimized frames on the stack
  return 0;
}
Esempio n. 22
0
    void connection_handler::handle_messages()
    {
        detail::handling_messages hm(handling_messages_);       // reset on exit

        bool bootstrapping = hpx::is_starting();
        bool has_work = true;
        std::size_t k = 0;

        hpx::util::high_resolution_timer t;
        std::list<std::pair<int, MPI_Request> > close_requests;

        // We let the message handling loop spin for another 2 seconds to avoid the
        // costs involved with posting it to asio
        while(bootstrapping || has_work || (!has_work && t.elapsed() < 2.0))
        {
            if(stopped_) break;

            // break the loop if someone requested to pause the parcelport
            if(!enable_parcel_handling_) break;

            // handle all send requests
            {
                hpx::lcos::local::spinlock::scoped_lock l(senders_mtx_);
                for(
                    senders_type::iterator it = senders_.begin();
                    !stopped_ && enable_parcel_handling_ && it != senders_.end();
                    /**/)
                {
                    if((*it)->done())
                    {
                        it = senders_.erase(it);
                    }
                    else
                    {
                        ++it;
                    }
                }
                has_work = !senders_.empty();
            }

            // Send the pending close requests
            {
                hpx::lcos::local::spinlock::scoped_lock l(close_mtx_);
                typedef std::pair<int, int> pair_type;

                BOOST_FOREACH(pair_type p, pending_close_requests_)
                {
                    header close_request = header::close(p.first, p.second);
                    close_requests.push_back(std::make_pair(p.first, MPI_Request()));
                    MPI_Isend(
                        close_request.data(),         // Data pointer
                        close_request.data_size_,     // Size
                        close_request.type(),         // MPI Datatype
                        close_request.rank(),         // Destination
                        0,                            // Tag
                        communicator_,                // Communicator
                        &close_requests.back().second
                    );
                }
                pending_close_requests_.clear();
            }

            // add new receive requests
            std::pair<bool, header> next(acceptor_.next_header());
            if(next.first)
            {
                boost::shared_ptr<receiver> rcv;
                header h = next.second;

                receivers_tag_map_type & tag_map = receivers_map_[h.rank()];

                receivers_tag_map_type::iterator jt = tag_map.find(h.tag());

                if(jt != tag_map.end())
                {
                    rcv = jt->second;
                }
                else
                {
                    rcv = boost::make_shared<receiver>(
                        communicator_
                      , get_next_tag()
                      , h.tag()
                      , h.rank()
                      , *this);
                    tag_map.insert(std::make_pair(h.tag(), rcv));
                }

                if(h.close_request())
                {
                    rcv->close();
                }
                else
                {
                    h.assert_valid();
                    if (static_cast<std::size_t>(h.size()) > this->get_max_message_size())
                    {
                        // report this problem ...
                        HPX_THROW_EXCEPTION(boost::asio::error::operation_not_supported,
                            "mpi::connection_handler::handle_messages",
                            "The size of this message exceeds the maximum inbound data size");
                        return;
                    }
                    if(rcv->async_read(h))
                    {
#ifdef HPX_DEBUG
                        receivers_type::iterator it = std::find(receivers_.begin(), receivers_.end(), rcv);
                        HPX_ASSERT(it == receivers_.end());

#endif
                        receivers_.push_back(rcv);
                    }
                }
            }

            // handle all receive requests
            for(receivers_type::iterator it = receivers_.begin();
                it != receivers_.end(); /**/)
            {
                boost::shared_ptr<receiver> rcv = *it;
                if(rcv->done())
                {
                    HPX_ASSERT(rcv->sender_tag() != -1);
                    if(rcv->closing())
                    {
                        receivers_tag_map_type & tag_map = receivers_map_[rcv->rank()];

                        receivers_tag_map_type::iterator jt = tag_map.find(rcv->sender_tag());
                        HPX_ASSERT(jt != tag_map.end());
                        tag_map.erase(jt);
                        {
                            hpx::lcos::local::spinlock::scoped_lock l(tag_mtx_);
                            free_tags_.push_back(rcv->tag());
                        }
                    }
                    it = receivers_.erase(it);
                }
                else
                {
                    ++it;
                }
            }
            if(!has_work) has_work = !receivers_.empty();

            // handle completed close requests
            for(
                std::list<std::pair<int, MPI_Request> >::iterator it = close_requests.begin();
                !stopped_ && enable_parcel_handling_ && it != close_requests.end();
            )
            {
                int completed = 0;
                MPI_Status status;
                int ret = 0;
                ret = MPI_Test(&it->second, &completed, &status);
                HPX_ASSERT(ret == MPI_SUCCESS);
                if(completed && status.MPI_ERROR != MPI_ERR_PENDING)
                {
                    hpx::lcos::local::spinlock::scoped_lock l(tag_mtx_);
                    free_tags_.push_back(it->first);
                    it = close_requests.erase(it);
                }
                else
                {
                    ++it;
                }
            }
            if(!has_work)
                has_work = !close_requests.empty();

            if (bootstrapping)
                bootstrapping = hpx::is_starting();

            if(has_work)
            {
                t.restart();
                k = 0;
            }
            else
            {
                if(enable_parcel_handling_)
                {
                    hpx::lcos::local::spinlock::yield(k);
                    ++k;
                }
            }
        }
    void GravityColumnSolverPolymer<FluxModel, Model>::solveSingleColumn(const std::vector<int>& column_cells,
                                                              const double dt,
                                                              std::vector<double>& s,
                                                              std::vector<double>& c,
                                                              std::vector<double>& cmax,
                                                              std::vector<double>& sol_vec)
    {
	// This is written only to work with SinglePointUpwindTwoPhase,
	// not with arbitrary problem models.
        int col_size = column_cells.size();

        // if (col_size == 1) {
	//     sol_vec[2*column_cells[0] + 0] = 0.0;
	//     sol_vec[2*column_cells[0] + 1] = 0.0;
        //     return;
        // }

	StateWithZeroFlux state(s, c, cmax); // This holds s by reference.

	// Assemble.
        const int kl = 3;
        const int ku = 3;
        const int nrow = 2*kl + ku + 1;
        const int N = 2*col_size; // N unknowns: s and c for each cell.
	std::vector<double> hm(nrow*N, 0.0); // band matrix with 3 upper and 3 lower diagonals.
	std::vector<double> rhs(N, 0.0);
        const BandMatrixCoeff bmc(N, ku, kl);


	for (int ci = 0; ci < col_size; ++ci) {
	    std::vector<double> F(2, 0.);
	    std::vector<double> dFd1(4, 0.);
	    std::vector<double> dFd2(4, 0.);
	    std::vector<double> dF(4, 0.);
	    const int cell = column_cells[ci];
	    const int prev_cell = (ci == 0) ? -999 : column_cells[ci - 1];
	    const int next_cell = (ci == col_size - 1) ? -999 : column_cells[ci + 1];
	    // model_.initResidual(cell, F);
	    for (int j = grid_.cell_facepos[cell]; j < grid_.cell_facepos[cell+1]; ++j) {
		const int face = grid_.cell_faces[j];
		const int c1 = grid_.face_cells[2*face + 0];
                const int c2 = grid_.face_cells[2*face + 1];
		if (c1 == prev_cell || c2 == prev_cell || c1 == next_cell || c2 == next_cell) {
                    F.assign(2, 0.);
                    dFd1.assign(4, 0.);
                    dFd2.assign(4, 0.);
		    fmodel_.fluxConnection(state, grid_, dt, cell, face, &F[0], &dFd1[0], &dFd2[0]);
		    if (c1 == prev_cell || c2 == prev_cell) {
                        hm[bmc(2*ci + 0, 2*(ci - 1) + 0)] += dFd2[0];
                        hm[bmc(2*ci + 0, 2*(ci - 1) + 1)] += dFd2[1];
                        hm[bmc(2*ci + 1, 2*(ci - 1) + 0)] += dFd2[2];
                        hm[bmc(2*ci + 1, 2*(ci - 1) + 1)] += dFd2[3];
		    } else {
			ASSERT(c1 == next_cell || c2 == next_cell);
                        hm[bmc(2*ci + 0, 2*(ci + 1) + 0)] += dFd2[0];
                        hm[bmc(2*ci + 0, 2*(ci + 1) + 1)] += dFd2[1];
                        hm[bmc(2*ci + 1, 2*(ci + 1) + 0)] += dFd2[2];
                        hm[bmc(2*ci + 1, 2*(ci + 1) + 1)] += dFd2[3];
		    }
                    hm[bmc(2*ci + 0, 2*ci + 0)] += dFd1[0];
                    hm[bmc(2*ci + 0, 2*ci + 1)] += dFd1[1];
                    hm[bmc(2*ci + 1, 2*ci + 0)] += dFd1[2];
                    hm[bmc(2*ci + 1, 2*ci + 1)] += dFd1[3];

		    rhs[2*ci + 0] += F[0];
		    rhs[2*ci + 1] += F[1];
		}
	    }
	    F.assign(2, 0.);
            dF.assign(4, 0.);
	    fmodel_.accumulation(grid_, cell, &F[0], &dF[0]);
            hm[bmc(2*ci + 0, 2*ci + 0)] += dF[0];
            hm[bmc(2*ci + 0, 2*ci + 1)] += dF[1];
            hm[bmc(2*ci + 1, 2*ci + 0)] += dF[2];
            if (std::abs(dF[3]) < 1e-12) {
                hm[bmc(2*ci + 1, 2*ci + 1)] += 1e-12;
            } else {
                hm[bmc(2*ci + 1, 2*ci + 1)] += dF[3];
            }

            rhs[2*ci + 0] += F[0];
            rhs[2*ci + 1] += F[1];

	}
	// model_.sourceTerms(); // Not needed
	// Solve.
	const int num_rhs = 1;
	int info = 0;
        std::vector<int> ipiv(N, 0);
	// Solution will be written to rhs.
        dgbsv_(&N, &kl, &ku, &num_rhs, &hm[0], &nrow, &ipiv[0], &rhs[0], &N, &info);
	if (info != 0) {
            std::cerr << "Failed column cells: ";
            std::copy(column_cells.begin(), column_cells.end(), std::ostream_iterator<int>(std::cerr, " "));
            std::cerr << "\n";
	    THROW("Lapack reported error in dgtsv: " << info);
	}
	for (int ci = 0; ci < col_size; ++ci) {
	    sol_vec[2*column_cells[ci] + 0] = -rhs[2*ci + 0];
	    sol_vec[2*column_cells[ci] + 1] = -rhs[2*ci + 1];
	}
    }
int main(int argc, char** argv) {
    const double PI(3.141592653589793);

    if (argc != 4) {
        std::cout << "usage:" << std::endl;
        std::cout << "package_scene path_scene output" << std::endl;
        return 0;
    }
    ros::init(argc, argv, "dart_test");

    ros::NodeHandle nh_;

    std::string package_name( argv[1] );
    std::string scene_urdf( argv[2] );

    ros::Rate loop_rate(400);

    ros::Publisher joint_state_pub_;
    joint_state_pub_ = nh_.advertise<sensor_msgs::JointState>("/joint_states", 10);
    tf::TransformBroadcaster br;
    MarkerPublisher markers_pub(nh_);

    std::string package_path_barrett = ros::package::getPath("barrett_hand_defs");
    std::string package_path = ros::package::getPath(package_name);

    // Load the Skeleton from a file
    dart::utils::DartLoader loader;
    loader.addPackageDirectory("barrett_hand_defs", package_path_barrett);
    loader.addPackageDirectory("barrett_hand_sim_dart", package_path);

    boost::shared_ptr<GraspSpecification > gspec = GraspSpecification::readFromUrdf(package_path + scene_urdf);

    dart::dynamics::SkeletonPtr scene( loader.parseSkeleton(package_path + scene_urdf) );
    scene->enableSelfCollision(true);

    dart::dynamics::SkeletonPtr bh( loader.parseSkeleton(package_path_barrett + "/robots/barrett_hand.urdf") );
    Eigen::Isometry3d tf;
    tf = scene->getBodyNode("gripper_mount_link")->getRelativeTransform();
    bh->getJoint(0)->setTransformFromParentBodyNode(tf);

    dart::simulation::World* world = new dart::simulation::World();

    world->addSkeleton(scene);
    world->addSkeleton(bh);

    Eigen::Vector3d grav(0,0,-1);
    world->setGravity(grav);

    GripperController gc;

    double Kc = 400.0;
    double KcDivTi = Kc / 1.0;
    gc.addJoint("right_HandFingerOneKnuckleOneJoint", Kc, KcDivTi, 0.0, 0.001, 50.0, true, false);
    gc.addJoint("right_HandFingerOneKnuckleTwoJoint", Kc, KcDivTi, 0.0, 0.001, 50.0, false, true);
    gc.addJoint("right_HandFingerTwoKnuckleTwoJoint", Kc, KcDivTi, 0.0, 0.001, 50.0, false, true);
    gc.addJoint("right_HandFingerThreeKnuckleTwoJoint", Kc, KcDivTi, 0.0, 0.001, 50.0, false, true);

    gc.addJointMimic("right_HandFingerTwoKnuckleOneJoint", 2.0*Kc, KcDivTi, 0.0, 0.001, 50.0, true, "right_HandFingerOneKnuckleOneJoint", 1.0, 0.0);
    gc.addJointMimic("right_HandFingerOneKnuckleThreeJoint", 2.0*Kc, KcDivTi, 0.0, 0.001, 50.0, false, "right_HandFingerOneKnuckleTwoJoint", 0.333333, 0.0);
    gc.addJointMimic("right_HandFingerTwoKnuckleThreeJoint", 2.0*Kc, KcDivTi, 0.0, 0.001, 50.0, false, "right_HandFingerTwoKnuckleTwoJoint", 0.333333, 0.0);
    gc.addJointMimic("right_HandFingerThreeKnuckleThreeJoint", 2.0*Kc, KcDivTi, 0.0, 0.001, 50.0, false, "right_HandFingerThreeKnuckleTwoJoint", 0.333333, 0.0);

    gc.setGoalPosition("right_HandFingerOneKnuckleOneJoint", gspec->getGoalPosition("right_HandFingerOneKnuckleOneJoint"));
    gc.setGoalPosition("right_HandFingerOneKnuckleTwoJoint", gspec->getGoalPosition("right_HandFingerOneKnuckleTwoJoint"));
    gc.setGoalPosition("right_HandFingerTwoKnuckleTwoJoint", gspec->getGoalPosition("right_HandFingerTwoKnuckleTwoJoint"));
    gc.setGoalPosition("right_HandFingerThreeKnuckleTwoJoint", gspec->getGoalPosition("right_HandFingerThreeKnuckleTwoJoint"));

    std::map<std::string, double> joint_q_map;
    joint_q_map["right_HandFingerOneKnuckleOneJoint"] = gspec->getInitPosition("right_HandFingerOneKnuckleOneJoint");
    joint_q_map["right_HandFingerTwoKnuckleOneJoint"] = gspec->getInitPosition("right_HandFingerOneKnuckleOneJoint");
    joint_q_map["right_HandFingerOneKnuckleTwoJoint"] = gspec->getInitPosition("right_HandFingerOneKnuckleTwoJoint");
    joint_q_map["right_HandFingerOneKnuckleThreeJoint"] = 0.333333 * gspec->getInitPosition("right_HandFingerOneKnuckleTwoJoint");
    joint_q_map["right_HandFingerTwoKnuckleTwoJoint"] = gspec->getInitPosition("right_HandFingerTwoKnuckleTwoJoint");
    joint_q_map["right_HandFingerTwoKnuckleThreeJoint"] = 0.333333 * gspec->getInitPosition("right_HandFingerTwoKnuckleTwoJoint");
    joint_q_map["right_HandFingerThreeKnuckleTwoJoint"] = gspec->getInitPosition("right_HandFingerThreeKnuckleTwoJoint");
    joint_q_map["right_HandFingerThreeKnuckleThreeJoint"] = 0.333333 * gspec->getInitPosition("right_HandFingerThreeKnuckleTwoJoint");

    for (std::vector<std::string >::const_iterator it = gc.getJointNames().begin(); it != gc.getJointNames().end(); it++) {
        dart::dynamics::Joint *j = bh->getJoint((*it));
        j->setActuatorType(dart::dynamics::Joint::FORCE);
     	j->setPositionLimited(true);
        j->setPosition(0, joint_q_map[(*it)]);
    }

    int counter = 0;

    while (ros::ok()) {
        world->step(false);

        for (std::map<std::string, double>::iterator it = joint_q_map.begin(); it != joint_q_map.end(); it++) {
            dart::dynamics::Joint *j = bh->getJoint(it->first);
            it->second = j->getPosition(0);
        }

        gc.controlStep(joint_q_map);

        // Compute the joint forces needed to compensate for Coriolis forces and
        // gravity
        const Eigen::VectorXd& Cg = bh->getCoriolisAndGravityForces();

        for (std::map<std::string, double>::iterator it = joint_q_map.begin(); it != joint_q_map.end(); it++) {
            dart::dynamics::Joint *j = bh->getJoint(it->first);
            int qidx = j->getIndexInSkeleton(0);
            double u = gc.getControl(it->first);
            double dq = j->getVelocity(0);
            if (!gc.isBackdrivable(it->first)) {
                j->setPositionLowerLimit(0, std::max(j->getPositionLowerLimit(0), it->second-0.01));
            }

            if (gc.isStopped(it->first)) {
                j->setPositionLowerLimit(0, std::max(j->getPositionLowerLimit(0), it->second-0.01));
                j->setPositionUpperLimit(0, std::min(j->getPositionUpperLimit(0), it->second+0.01));
//                std::cout << it->first << " " << "stopped" << std::endl;
            }
            j->setForce(0, 0.02*(u-dq) + Cg(qidx));
        }

        for (int bidx = 0; bidx < bh->getNumBodyNodes(); bidx++) {
            dart::dynamics::BodyNode *b = bh->getBodyNode(bidx);
            const Eigen::Isometry3d &tf = b->getTransform();
            KDL::Frame T_W_L;
            EigenTfToKDL(tf, T_W_L);
//            std::cout << b->getName() << std::endl;
            publishTransform(br, T_W_L, b->getName(), "world");
        }

        int m_id = 0;
        for (int bidx = 0; bidx < scene->getNumBodyNodes(); bidx++) {
                dart::dynamics::BodyNode *b = scene->getBodyNode(bidx);

                const Eigen::Isometry3d &tf = b->getTransform();
                KDL::Frame T_W_L;
                EigenTfToKDL(tf, T_W_L);
                publishTransform(br, T_W_L, b->getName(), "world");
                for (int cidx = 0; cidx < b->getNumCollisionShapes(); cidx++) {
                    dart::dynamics::ConstShapePtr sh = b->getCollisionShape(cidx);
                    if (sh->getShapeType() == dart::dynamics::Shape::MESH) {
                        std::shared_ptr<const dart::dynamics::MeshShape > msh = std::static_pointer_cast<const dart::dynamics::MeshShape >(sh);
                        m_id = markers_pub.addMeshMarker(m_id, KDL::Vector(), 0, 1, 0, 1, 1, 1, 1, msh->getMeshUri(), b->getName());
                    }
                }
        }

        markers_pub.publish();

        ros::spinOnce();
        loop_rate.sleep();

        counter++;
        if (counter < 3000) {
        }
        else if (counter == 3000) {
            dart::dynamics::Joint::Properties prop = bh->getJoint(0)->getJointProperties();
            dart::dynamics::FreeJoint::Properties prop_free;
            prop_free.mName = prop_free.mName;
            prop_free.mT_ParentBodyToJoint = prop.mT_ParentBodyToJoint;
            prop_free.mT_ChildBodyToJoint = prop.mT_ChildBodyToJoint;
            prop_free.mIsPositionLimited = false;
            prop_free.mActuatorType = dart::dynamics::Joint::VELOCITY;
            bh->getRootBodyNode()->changeParentJointType<dart::dynamics::FreeJoint >(prop_free);
        }
        else if (counter < 4000) {
            bh->getDof("Joint_pos_z")->setVelocity(-0.1);
        }
        else {
            break;
        }
    }

    //
    // generate models
    //

    const std::string ob_name( "graspable" );

    scene->getBodyNode(ob_name)->setFrictionCoeff(0.001);

    // calculate point clouds for all links and for the grasped object
    std::map<std::string, pcl::PointCloud<pcl::PointNormal>::Ptr > point_clouds_map;
    std::map<std::string, pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr > point_pc_clouds_map;
    std::map<std::string, KDL::Frame > frames_map;
    std::map<std::string, boost::shared_ptr<std::vector<KDL::Frame > > > features_map;
    std::map<std::string, boost::shared_ptr<pcl::VoxelGrid<pcl::PointNormal> > > grids_map;
    for (int skidx = 0; skidx < world->getNumSkeletons(); skidx++) {
        dart::dynamics::SkeletonPtr sk = world->getSkeleton(skidx);

        for (int bidx = 0; bidx < sk->getNumBodyNodes(); bidx++) {
            dart::dynamics::BodyNode *b = sk->getBodyNode(bidx);
            const Eigen::Isometry3d &tf = b->getTransform();
            const std::string &body_name = b->getName();
            if (body_name.find("right_Hand") != 0 && body_name != ob_name) {
                continue;
            }
            KDL::Frame T_W_L;
            EigenTfToKDL(tf, T_W_L);
            std::cout << body_name << "   " << b->getNumCollisionShapes() << std::endl;
            for (int cidx = 0; cidx < b->getNumCollisionShapes(); cidx++) {
                dart::dynamics::ConstShapePtr sh = b->getCollisionShape(cidx);
                if (sh->getShapeType() == dart::dynamics::Shape::MESH) {
                    std::shared_ptr<const dart::dynamics::MeshShape > msh = std::static_pointer_cast<const dart::dynamics::MeshShape >(sh);
                    std::cout << "mesh path: " << msh->getMeshPath() << std::endl;
                    std::cout << "mesh uri: " << msh->getMeshUri() << std::endl;
                    const Eigen::Isometry3d &tf = sh->getLocalTransform();
                    KDL::Frame T_L_S;
                    EigenTfToKDL(tf, T_L_S);
                    KDL::Frame T_S_L = T_L_S.Inverse();

                    const aiScene *sc = msh->getMesh();
                    if (sc->mNumMeshes != 1) {
                        std::cout << "ERROR: sc->mNumMeshes = " << sc->mNumMeshes << std::endl;
                    }
                    int midx = 0;
//                    std::cout << "v: " << sc->mMeshes[midx]->mNumVertices << "   f: " << sc->mMeshes[midx]->mNumFaces << std::endl;
                    pcl::PointCloud<pcl::PointNormal>::Ptr cloud_1 (new pcl::PointCloud<pcl::PointNormal>);
                    uniform_sampling(sc->mMeshes[midx], 1000000, *cloud_1);
                    for (int pidx = 0; pidx < cloud_1->points.size(); pidx++) {
                        KDL::Vector pt_L = T_L_S * KDL::Vector(cloud_1->points[pidx].x, cloud_1->points[pidx].y, cloud_1->points[pidx].z);
                        cloud_1->points[pidx].x = pt_L.x();
                        cloud_1->points[pidx].y = pt_L.y();
                        cloud_1->points[pidx].z = pt_L.z();
                    }
                    // Voxelgrid
                    boost::shared_ptr<pcl::VoxelGrid<pcl::PointNormal> > grid_(new pcl::VoxelGrid<pcl::PointNormal>);
                    pcl::PointCloud<pcl::PointNormal>::Ptr res(new pcl::PointCloud<pcl::PointNormal>);
                    grid_->setDownsampleAllData(true);
                    grid_->setSaveLeafLayout(true);
                    grid_->setInputCloud(cloud_1);
                    grid_->setLeafSize(0.004, 0.004, 0.004);
                    grid_->filter (*res);
                    point_clouds_map[body_name] = res;
                    frames_map[body_name] = T_W_L;
                    grids_map[body_name] = grid_;

                    std::cout << "res->points.size(): " << res->points.size() << std::endl;

                    pcl::search::KdTree<pcl::PointNormal>::Ptr tree (new pcl::search::KdTree<pcl::PointNormal>);

                    // Setup the principal curvatures computation
                    pcl::PrincipalCurvaturesEstimation<pcl::PointNormal, pcl::PointNormal, pcl::PrincipalCurvatures> principalCurvaturesEstimation;

                    // Provide the original point cloud (without normals)
                    principalCurvaturesEstimation.setInputCloud (res);

                    // Provide the point cloud with normals
                    principalCurvaturesEstimation.setInputNormals(res);

                    // Use the same KdTree from the normal estimation
                    principalCurvaturesEstimation.setSearchMethod (tree);
                    principalCurvaturesEstimation.setRadiusSearch(0.02);

                    // Actually compute the principal curvatures
                    pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr principalCurvatures (new pcl::PointCloud<pcl::PrincipalCurvatures> ());
                    principalCurvaturesEstimation.compute (*principalCurvatures);
                    point_pc_clouds_map[body_name] = principalCurvatures;

                    features_map[body_name].reset( new std::vector<KDL::Frame >(res->points.size()) );
                    for (int pidx = 0; pidx < res->points.size(); pidx++) {
                        KDL::Vector nx, ny, nz(res->points[pidx].normal[0], res->points[pidx].normal[1], res->points[pidx].normal[2]);
                        if ( std::fabs( principalCurvatures->points[pidx].pc1 - principalCurvatures->points[pidx].pc2 ) > 0.001) {
                            nx = KDL::Vector(principalCurvatures->points[pidx].principal_curvature[0], principalCurvatures->points[pidx].principal_curvature[1], principalCurvatures->points[pidx].principal_curvature[2]);
                        }
                        else {
                            if (std::fabs(nz.z()) < 0.7) {
                                nx = KDL::Vector(0, 0, 1);
                            }
                            else {
                                nx = KDL::Vector(1, 0, 0);
                            }
                        }
                        ny = nz * nx;
                        nx = ny * nz;
                        nx.Normalize();
                        ny.Normalize();
                        nz.Normalize();
                        (*features_map[body_name])[pidx] = KDL::Frame( KDL::Rotation(nx, ny, nz), KDL::Vector(res->points[pidx].x, res->points[pidx].y, res->points[pidx].z) );
                    }
                }
            }
        }
    }

    const double sigma_p = 0.01;//05;
    const double sigma_q = 10.0/180.0*PI;//100.0;
    const double sigma_r = 0.2;//05;
    double sigma_c = 5.0/180.0*PI;

    int m_id = 101;

    // generate object model
    boost::shared_ptr<ObjectModel > om(new ObjectModel);
    for (int pidx = 0; pidx < point_clouds_map[ob_name]->points.size(); pidx++) {
        if (point_pc_clouds_map[ob_name]->points[pidx].pc1 > 1.1 * point_pc_clouds_map[ob_name]->points[pidx].pc2) {
            // e.g. pc1=1, pc2=0
            // edge
            om->addPointFeature((*features_map[ob_name])[pidx] * KDL::Frame(KDL::Rotation::RotZ(PI)), point_pc_clouds_map[ob_name]->points[pidx].pc1, point_pc_clouds_map[ob_name]->points[pidx].pc2);
            om->addPointFeature((*features_map[ob_name])[pidx], point_pc_clouds_map[ob_name]->points[pidx].pc1, point_pc_clouds_map[ob_name]->points[pidx].pc2);
        }
        else {
            for (double angle = 0.0; angle < 359.0/180.0*PI; angle += 20.0/180.0*PI) {
                om->addPointFeature((*features_map[ob_name])[pidx] * KDL::Frame(KDL::Rotation::RotZ(angle)), point_pc_clouds_map[ob_name]->points[pidx].pc1, point_pc_clouds_map[ob_name]->points[pidx].pc2);
            }
        }
    }


    std::cout << "om.getPointFeatures().size(): " << om->getPointFeatures().size() << std::endl;
    KDL::Frame T_W_O = frames_map[ob_name];

    // generate collision model
    std::map<std::string, std::list<std::pair<int, double> > > link_pt_map;
    boost::shared_ptr<CollisionModel > cm(new CollisionModel);
    cm->setSamplerParameters(sigma_p, sigma_q, sigma_r);

    std::list<std::string > gripper_link_names;
    for (int bidx = 0; bidx < bh->getNumBodyNodes(); bidx++) {
        const std::string &link_name = bh->getBodyNode(bidx)->getName();
        gripper_link_names.push_back(link_name);
    }

    double dist_range = 0.01;
    for (std::list<std::string >::const_iterator nit = gripper_link_names.begin(); nit != gripper_link_names.end(); nit++) {
        const std::string &link_name = (*nit);
        if (point_clouds_map.find( link_name ) == point_clouds_map.end()) {
            continue;
        }
        cm->addLinkContacts(dist_range, link_name, point_clouds_map[link_name], frames_map[link_name],
                            om->getPointFeatures(), T_W_O);
    }

    // generate hand configuration model
    boost::shared_ptr<HandConfigurationModel > hm(new HandConfigurationModel);
    std::map<std::string, double> joint_q_map_before( joint_q_map );

    double angleDiffKnuckleTwo = 15.0/180.0*PI;
    joint_q_map_before["right_HandFingerOneKnuckleTwoJoint"] -= angleDiffKnuckleTwo;
    joint_q_map_before["right_HandFingerTwoKnuckleTwoJoint"] -= angleDiffKnuckleTwo;
    joint_q_map_before["right_HandFingerThreeKnuckleTwoJoint"] -= angleDiffKnuckleTwo;
    joint_q_map_before["right_HandFingerOneKnuckleThreeJoint"] -= angleDiffKnuckleTwo*0.333333;
    joint_q_map_before["right_HandFingerTwoKnuckleThreeJoint"] -= angleDiffKnuckleTwo*0.333333;
    joint_q_map_before["right_HandFingerThreeKnuckleThreeJoint"] -= angleDiffKnuckleTwo*0.333333;

    hm->generateModel(joint_q_map_before, joint_q_map, 1.0, 10, sigma_c);

    writeToXml(argv[3], cm, hm);

    return 0;
}
Esempio n. 25
0
jvmtiError
JvmtiEnvBase::get_stack_trace(JavaThread *java_thread,
                              jint start_depth, jint max_count,
                              jvmtiFrameInfo* frame_buffer, jint* count_ptr) {
#ifdef ASSERT
  uint32_t debug_bits = 0;
#endif
  assert((SafepointSynchronize::is_at_safepoint() ||
          is_thread_fully_suspended(java_thread, false, &debug_bits)),
         "at safepoint or target thread is suspended");
  int count = 0;
  if (java_thread->has_last_Java_frame()) {
    RegisterMap reg_map(java_thread);
    Thread* current_thread = Thread::current();
    ResourceMark rm(current_thread);
    javaVFrame *jvf = java_thread->last_java_vframe(&reg_map);
    HandleMark hm(current_thread);
    if (start_depth != 0) {
      if (start_depth > 0) {
        for (int j = 0; j < start_depth && jvf != NULL; j++) {
          jvf = jvf->java_sender();
        }
        if (jvf == NULL) {
          // start_depth is deeper than the stack depth
          return JVMTI_ERROR_ILLEGAL_ARGUMENT;
        }
      } else { // start_depth < 0
        // we are referencing the starting depth based on the oldest
        // part of the stack.
        // optimize to limit the number of times that java_sender() is called
        javaVFrame *jvf_cursor = jvf;
        javaVFrame *jvf_prev = NULL;
        javaVFrame *jvf_prev_prev;
        int j = 0;
        while (jvf_cursor != NULL) {
          jvf_prev_prev = jvf_prev;
          jvf_prev = jvf_cursor;
          for (j = 0; j > start_depth && jvf_cursor != NULL; j--) {
            jvf_cursor = jvf_cursor->java_sender();
          }
        }
        if (j == start_depth) {
          // previous pointer is exactly where we want to start
          jvf = jvf_prev;
        } else {
          // we need to back up further to get to the right place
          if (jvf_prev_prev == NULL) {
            // the -start_depth is greater than the stack depth
            return JVMTI_ERROR_ILLEGAL_ARGUMENT;
          }
          // j now is the number of frames on the stack starting with
          // jvf_prev, we start from jvf_prev_prev and move older on
          // the stack that many, the result is -start_depth frames
          // remaining.
          jvf = jvf_prev_prev;
          for (; j < 0; j++) {
            jvf = jvf->java_sender();
          }
        }
      }
    }
    for (; count < max_count && jvf != NULL; count++) {
      frame_buffer[count].method = jvf->method()->jmethod_id();
      frame_buffer[count].location = (jvf->method()->is_native() ? -1 : jvf->bci());
      jvf = jvf->java_sender();
    }
  } else {
    if (start_depth != 0) {
      // no frames and there is a starting depth
      return JVMTI_ERROR_ILLEGAL_ARGUMENT;
    }
  }
  *count_ptr = count;
  return JVMTI_ERROR_NONE;
}
// Write the field information portion of ClassFile structure
// JVMSpec|   	u2 fields_count;
// JVMSpec|   	field_info fields[fields_count];
void JvmtiClassFileReconstituter::write_field_infos() {  
  HandleMark hm(thread());
  typeArrayHandle fields(thread(), ikh()->fields());
  int fields_length = fields->length();
  int num_fields = fields_length / instanceKlass::next_offset;
  objArrayHandle fields_anno(thread(), ikh()->fields_annotations());

  write_u2(num_fields);  
  for (int index = 0; index < fields_length; index += instanceKlass::next_offset) {
    AccessFlags access_flags;
    int flags = fields->ushort_at(index + instanceKlass::access_flags_offset);
    access_flags.set_flags(flags);
    int name_index = fields->ushort_at(index + instanceKlass::name_index_offset);
    int signature_index = fields->ushort_at(index + instanceKlass::signature_index_offset);
    int initial_value_index = fields->ushort_at(index + instanceKlass::initval_index_offset);
    guarantee(name_index != 0 && signature_index != 0, "bad constant pool index for field");
    int offset = ikh()->offset_from_fields( index );
    int generic_signature_index = 
                        fields->ushort_at(index + instanceKlass::generic_signature_offset);
    typeArrayHandle anno(thread(), fields_anno.not_null() ? 
                                 (typeArrayOop)(fields_anno->obj_at(index / instanceKlass::next_offset)) :
                                 (typeArrayOop)NULL);

    // JVMSpec|   field_info {
    // JVMSpec|   	u2 access_flags;
    // JVMSpec|   	u2 name_index;
    // JVMSpec|   	u2 descriptor_index;
    // JVMSpec|   	u2 attributes_count;
    // JVMSpec|   	attribute_info attributes[attributes_count];
    // JVMSpec|   }

    write_u2(flags & JVM_RECOGNIZED_FIELD_MODIFIERS); 
    write_u2(name_index); 
    write_u2(signature_index); 
    int attr_count = 0;
    if (initial_value_index != 0) {
      ++attr_count;
    }
    if (access_flags.is_synthetic()) {
      // ++attr_count;
    }
    if (generic_signature_index != 0) {
      ++attr_count;
    }
    if (anno.not_null()) {
      ++attr_count;     // has RuntimeVisibleAnnotations attribute
    }

    write_u2(attr_count); 

    if (initial_value_index != 0) {
      write_attribute_name_index("ConstantValue");
      write_u4(2); //length always 2
      write_u2(initial_value_index);
    }
    if (access_flags.is_synthetic()) {
      // write_synthetic_attribute();
    }
    if (generic_signature_index != 0) {
      write_signature_attribute(generic_signature_index);
    }
    if (anno.not_null()) {
      write_annotations_attribute("RuntimeVisibleAnnotations", anno);
    }
  }
}
Esempio n. 27
0
// Revised lookup semantics   introduced 1.3 (Kestral beta)
void klassVtable::initialize_vtable(bool checkconstraints, TRAPS) {

  // Note:  Arrays can have intermediate array supers.  Use java_super to skip them.
  KlassHandle super (THREAD, klass()->java_super());
  int nofNewEntries = 0;


  if (PrintVtables && !klass()->oop_is_array()) {
    ResourceMark rm(THREAD);
    tty->print_cr("Initializing: %s", _klass->name()->as_C_string());
  }

#ifdef ASSERT
  oop* end_of_obj = (oop*)_klass() + _klass()->size();
  oop* end_of_vtable = (oop*)&table()[_length];
  assert(end_of_vtable <= end_of_obj, "vtable extends beyond end");
#endif

  if (Universe::is_bootstrapping()) {
    // just clear everything
    for (int i = 0; i < _length; i++) table()[i].clear();
    return;
  }

  int super_vtable_len = initialize_from_super(super);
  if (klass()->oop_is_array()) {
    assert(super_vtable_len == _length, "arrays shouldn't introduce new methods");
  } else {
    assert(_klass->oop_is_instance(), "must be instanceKlass");

    objArrayHandle methods(THREAD, ik()->methods());
    int len = methods()->length();
    int initialized = super_vtable_len;

    // update_inherited_vtable can stop for gc - ensure using handles
    for (int i = 0; i < len; i++) {
      HandleMark hm(THREAD);
      assert(methods()->obj_at(i)->is_method(), "must be a methodOop");
      methodHandle mh(THREAD, (methodOop)methods()->obj_at(i));

      bool needs_new_entry = update_inherited_vtable(ik(), mh, super_vtable_len, checkconstraints, CHECK);

      if (needs_new_entry) {
        put_method_at(mh(), initialized);
        mh()->set_vtable_index(initialized); // set primary vtable index
        initialized++;
      }
    }

    // add miranda methods; it will also update the value of initialized
    fill_in_mirandas(initialized);

    // In class hierarchies where the accessibility is not increasing (i.e., going from private ->
    // package_private -> publicprotected), the vtable might actually be smaller than our initial
    // calculation.
    assert(initialized <= _length, "vtable initialization failed");
    for(;initialized < _length; initialized++) {
      put_method_at(NULL, initialized);
    }
    NOT_PRODUCT(verify(tty, true));
  }
}
Esempio n. 28
0
    void connection_handler::handle_messages()
    {
        detail::handling_messages hm(handling_messages_);       // reset on exit

        bool bootstrapping = hpx::is_starting();
        bool has_work = true;
        std::size_t k = 0;

        hpx::util::high_resolution_timer t;
        std::list<std::pair<int, MPI_Request> > close_requests;

        // We let the message handling loop spin for another 2 seconds to avoid the
        // costs involved with posting it to asio
        while(bootstrapping || (!stopped_ && has_work) || (!has_work && t.elapsed() < 2.0))
        {
            // break the loop if someone requested to pause the parcelport
            if(!enable_parcel_handling_) break;

            // handle all send requests
            {
                hpx::lcos::local::spinlock::scoped_lock l(senders_mtx_);
                for(
                    senders_type::iterator it = senders_.begin();
                    !stopped_ && enable_parcel_handling_ && it != senders_.end();
                    /**/)
                {
                    if((*it)->done())
                    {
                        it = senders_.erase(it);
                    }
                    else
                    {
                        ++it;
                    }
                }
                has_work = !senders_.empty();
            }

            // Send the pending close requests
            {
                hpx::lcos::local::spinlock::scoped_lock l(close_mtx_);
                typedef std::pair<int, int> pair_type;

                BOOST_FOREACH(pair_type p, pending_close_requests_)
                {
                    header close_request = header::close(p.first, p.second);
                    close_requests.push_back(std::make_pair(p.first, MPI_Request()));
                    MPI_Isend(
                        close_request.data(),         // Data pointer
                        close_request.data_size_,     // Size
                        close_request.type(),         // MPI Datatype
                        close_request.rank(),         // Destination
                        0,                            // Tag
                        communicator_,                // Communicator
                        &close_requests.back().second
                    );
                }
                pending_close_requests_.clear();
            }

            // add new receive requests
            std::pair<bool, header> next(acceptor_.next_header());
            if(next.first)
            {
                boost::shared_ptr<receiver> rcv;
                receivers_rank_map_type::iterator jt = receivers_map_.find(next.second.rank());
                if(jt != receivers_map_.end())
                {
                    receivers_tag_map_type::iterator kt = jt->second.find(next.second.tag());
                    if(kt != jt->second.end())
                    {
                        if(next.second.close_request())
                        {
                            hpx::lcos::local::spinlock::scoped_lock l(tag_mtx_);
                            free_tags_.push_back(kt->second->tag());
                            jt->second.erase(kt);
                            if(jt->second.empty())
                            {
                                receivers_map_.erase(jt);
                            }
                        }
                        else
                        {
                            rcv = kt->second;
                        }
                    }
                }
                if(!next.second.close_request())
                {
                    next.second.assert_valid();
                    if(!rcv)
                    {
                        rcv = boost::make_shared<receiver>(communicator_, get_next_tag());
                    }
                    rcv->async_read(next.second, *this);
                    receivers_.push_back(rcv);
                }
            }

            // handle all receive requests
            for(
                receivers_type::iterator it = receivers_.begin();
                !stopped_ && enable_parcel_handling_ && it != receivers_.end();
                /**/)
            {
                if((*it)->done(*this))
                {
                    HPX_ASSERT(
                        !receivers_map_[(*it)->rank()][(*it)->sender_tag()]
                     || receivers_map_[(*it)->rank()][(*it)->sender_tag()].get() == it->get()
                    );
                    receivers_map_[(*it)->rank()][(*it)->sender_tag()] = *it;
                    it = receivers_.erase(it);
                }
                else
                {
                    ++it;
                }
            }

            if(!has_work) has_work = !receivers_.empty();

            // handle completed close requests
            for(
                std::list<std::pair<int, MPI_Request> >::iterator it = close_requests.begin();
                !stopped_ && enable_parcel_handling_ && it != close_requests.end();
            )
            {
                int completed = 0;
                MPI_Status status;
                int ret = 0;
                ret = MPI_Test(&it->second, &completed, &status);
                HPX_ASSERT(ret == MPI_SUCCESS);
                if(completed && status.MPI_ERROR != MPI_ERR_PENDING)
                {
                    hpx::lcos::local::spinlock::scoped_lock l(tag_mtx_);
                    free_tags_.push_back(it->first);
                    it = close_requests.erase(it);
                }
                else
                {
                    ++it;
                }
            }
            if(!has_work)
                has_work = !close_requests.empty();

            if (bootstrapping)
                bootstrapping = hpx::is_starting();

            if(has_work)
            {
                t.restart();
                k = 0;
            }
            else
            {
                if(enable_parcel_handling_)
                {
                    hpx::lcos::local::spinlock::yield(k);
                    ++k;
                }
            }
        }
void calibrate(const std::vector<TiePoint>& tiePoints,const int gridSize[2],const double tileSize[2],const unsigned int depthFrameSize[2],const unsigned int colorFrameSize[2])
{
    /* Initialize the depth camera's intrinsic parameter matrix: */
    Math::Matrix depthV(6,6,0.0);

    /* Process all tie points: */
    for(std::vector<TiePoint>::const_iterator tpIt=tiePoints.begin(); tpIt!=tiePoints.end(); ++tpIt)
    {
        /* Enter the tie point's depth homography into the intrinsic parameter matrix: */
        Homography::Matrix hm(1.0);
        hm(0,2)=double(depthFrameSize[0]);
        hm*=tpIt->depthHom.getMatrix();
        Homography::Matrix scale(1.0);
        scale(0,0)=1.0/tileSize[0];
        scale(1,1)=1.0/tileSize[1];
        hm*=scale;
        double row[3][6];
        static const int is[3]= {0,0,1};
        static const int js[3]= {1,0,1};
        for(int r=0; r<3; ++r)
        {
            int i=is[r];
            int j=js[r];
            row[r][0]=hm(0,i)*hm(0,j);
            row[r][1]=hm(0,i)*hm(1,j)+hm(1,i)*hm(0,j);
            row[r][2]=hm(0,i)*hm(2,j)+hm(2,i)*hm(0,j);
            row[r][3]=hm(1,i)*hm(1,j);
            row[r][4]=hm(1,i)*hm(2,j)+hm(2,i)*hm(1,j);
            row[r][5]=hm(2,i)*hm(2,j);
        }
        for(int i=0; i<6; ++i)
            row[1][i]-=row[2][i];
        for(int r=0; r<2; ++r)
        {
            for(unsigned int i=0; i<6; ++i)
                for(unsigned int j=0; j<6; ++j)
                    depthV(i,j)+=row[r][i]*row[r][j];
        }
    }

    /* Find the intrinsic parameter linear system's smallest eigenvalue: */
    std::pair<Math::Matrix,Math::Matrix> depthQe=depthV.jacobiIteration();
    unsigned int minEIndex=0;
    double minE=Math::abs(depthQe.second(0));
    for(unsigned int i=1; i<6; ++i)
    {
        if(minE>Math::abs(depthQe.second(i)))
        {
            minEIndex=i;
            minE=Math::abs(depthQe.second(i));
        }
    }
    std::cout<<"Smallest eigenvalue of v = "<<depthQe.second(minEIndex)<<std::endl;

    /* Calculate the intrinsic parameters: */
    Math::Matrix b=depthQe.first.getColumn(minEIndex);
    std::cout<<b(0)<<", "<<b(1)<<", "<<b(2)<<", "<<b(3)<<", "<<b(4)<<", "<<b(5)<<std::endl;
    double v0=(b(1)*b(2)-b(0)*b(4))/(b(0)*b(3)-Math::sqr(b(1)));
    double lambda=b(5)-(Math::sqr(b(2))+v0*(b(1)*b(2)-b(0)*b(4)))/b(0);
    double alpha=Math::sqrt(lambda/b(0));
    double beta=Math::sqrt(lambda*b(0)/(b(0)*b(3)-Math::sqr(b(1))));
    double gamma=-b(1)*Math::sqr(alpha)*beta/lambda;
    double u0=gamma*v0/beta-b(2)*Math::sqr(alpha)/lambda;

    std::cout<<"Intrinsic camera parameters:"<<std::endl;
    std::cout<<alpha<<" "<<gamma<<" "<<u0<<std::endl;
    std::cout<<0.0<<" "<<beta<<" "<<v0<<std::endl;
    std::cout<<0.0<<" "<<0.0<<" "<<1.0<<std::endl;

    /* Create the intrinsic camera parameter matrix: */
    Math::Matrix a(3,3,1.0);
    a.set(0,0,alpha);
    a.set(0,1,gamma);
    a.set(0,2,u0);
    a.set(1,1,beta);
    a.set(1,2,v0);
    Math::Matrix aInv=a.inverse();

    /* Calculate extrinsic parameters for each tie point to get measurements for the depth formula regression: */
    Math::Matrix depthAta(2,2,0.0);
    Math::Matrix depthAtb(2,1,0.0);
    for(std::vector<TiePoint>::const_iterator tpIt=tiePoints.begin(); tpIt!=tiePoints.end(); ++tpIt)
    {
        /* Convert the tie point's depth homography to a matrix: */
        Homography::Matrix hm(1.0);
        hm(0,2)=double(depthFrameSize[0]);
        hm*=tpIt->depthHom.getMatrix();
        Homography::Matrix scale(1.0);
        scale(0,0)=1.0/tileSize[0];
        scale(1,1)=1.0/tileSize[1];
        hm*=scale;
        Math::Matrix h(3,3);
        for(unsigned int i=0; i<3; ++i)
            for(unsigned int j=0; j<3; ++j)
                h(i,j)=hm(i,j);

        /* Calculate the extrinsic parameters: */
        double lambda=0.5/(aInv*h.getColumn(0)).mag()+0.5/(aInv*h.getColumn(1)).mag();
        Math::Matrix r1=lambda*aInv*h.getColumn(0);
        Math::Matrix r2=lambda*aInv*h.getColumn(1);
        Math::Matrix r3(3,1);
        for(unsigned int i=0; i<3; ++i)
            r3.set(i,r1((i+1)%3)*r2((i+2)%3)-r1((i+2)%3)*r2((i+1)%3)); // 'Tis a cross product, in case you're wondering
        Math::Matrix t=lambda*aInv*h.getColumn(2);

        /* Create the extrinsic parameter matrix: */
        Math::Matrix rt(3,4);
        rt.setColumn(0,r1);
        rt.setColumn(1,r2);
        rt.setColumn(2,r3);
        rt.setColumn(3,t);

        Math::Matrix wgc(4,1);
        wgc(0)=tileSize[0]*double(gridSize[0])*0.5;
        wgc(1)=tileSize[1]*double(gridSize[1])*0.5;
        wgc(2)=0.0;
        wgc(3)=1.0;
        Math::Matrix cgc=rt*wgc;
        if(cgc(2)<0.0)
        {
            /* Flip the extrinsic matrix to move the grid to positive z: */
            Math::Matrix flip(3,3,-1.0);
            rt=flip*rt;
            cgc=rt*wgc;
        }
        std::cout<<"Grid center: "<<cgc(0)<<", "<<cgc(1)<<", "<<cgc(2)<<std::endl;

        /* Transform all world grid points with the extrinsic matrix to get their camera-space z values: */
        for(int y=0; y<gridSize[1]; ++y)
            for(int x=0; x<gridSize[0]; ++x)
            {
                /* Create the world point: */
                Math::Matrix wp(4,1);
                wp(0)=tileSize[0]*double(x);
                wp(1)=tileSize[1]*double(y);
                wp(2)=0.0;
                wp(3)=1.0;

                /* Transform the world point to camera space: */
                Math::Matrix cp=rt*wp;
                double dist=cp(2);

                /* Get the depth frame value from the grid's plane in depth image space: */
                Point dip=tpIt->depthHom.transform(Point(x,y));
                const Plane::Vector& n=tpIt->gridPlane.getNormal();
                double o=tpIt->gridPlane.getOffset();
                double depth=(o-dip[0]*n[0]-dip[1]*n[1])/n[2];

                /* Enter the depth / z pair into the depth formula accumulator: */
                depthAta(0,0)+=1.0;
                depthAta(0,1)+=-dist;
                depthAta(1,0)+=-dist;
                depthAta(1,1)+=dist*dist;
                depthAtb(0)+=-dist*depth;
                depthAtb(1)+=dist*dist*depth;
            }
    }

    /* Solve the depth formula least-squares system: */
    Math::Matrix depthX=depthAtb.divideFullPivot(depthAta);
    std::cout<<"Depth conversion formula: dist = "<<depthX(0)<<" / ("<<depthX(1)<<" - depth)"<<std::endl;

    /* Calculate the full depth unprojection matrix: */
    Math::Matrix depthProj(4,4,0.0);
    depthProj(0,0)=1.0/alpha;
    depthProj(0,1)=-gamma/(alpha*beta);
    depthProj(0,3)=-u0/alpha+v0*gamma/(alpha*beta);
    depthProj(1,1)=1.0/beta;
    depthProj(1,3)=-v0/beta;
    depthProj(2,3)=-1.0;
    depthProj(3,2)=-1.0/depthX(0);
    depthProj(3,3)=depthX(1)/depthX(0);

    Math::Matrix colorAta(12,12,0.0);
    for(std::vector<TiePoint>::const_iterator tpIt=tiePoints.begin(); tpIt!=tiePoints.end(); ++tpIt)
    {
        for(int y=0; y<gridSize[1]; ++y)
            for(int x=0; x<gridSize[0]; ++x)
            {
                /* Enter the tie point into the color calibration matrix linear system: */
                Point dip=tpIt->depthHom.transform(Point(x,y));
                const Plane::Vector& n=tpIt->gridPlane.getNormal();
                double o=tpIt->gridPlane.getOffset();
                double depth=(o-dip[0]*n[0]-dip[1]*n[1])/n[2];
                Math::Matrix dwp(4,1);
                dwp(0)=dip[0]+double(depthFrameSize[0]);
                dwp(1)=dip[1];
                dwp(2)=depth;
                dwp(3)=1.0;
                dwp=depthProj*dwp;
                for(int i=0; i<3; ++i)
                    dwp(i)/=dwp(3);
                Point cip=tpIt->colorHom.transform(Point(x,y));
                cip[0]/=double(colorFrameSize[0]);
                cip[1]/=double(colorFrameSize[1]);

                double eq[2][12];
                eq[0][0]=dwp(0);
                eq[0][1]=dwp(1);
                eq[0][2]=dwp(2);
                eq[0][3]=1.0;
                eq[0][4]=0.0;
                eq[0][5]=0.0;
                eq[0][6]=0.0;
                eq[0][7]=0.0;
                eq[0][8]=-cip[0]*dwp(0);
                eq[0][9]=-cip[0]*dwp(1);
                eq[0][10]=-cip[0]*dwp(2);
                eq[0][11]=-cip[0];

                eq[1][0]=0.0;
                eq[1][1]=0.0;
                eq[1][2]=0.0;
                eq[1][3]=0.0;
                eq[1][4]=dwp(0);
                eq[1][5]=dwp(1);
                eq[1][6]=dwp(2);
                eq[1][7]=1.0;
                eq[1][8]=-cip[1]*dwp(0);
                eq[1][9]=-cip[1]*dwp(1);
                eq[1][10]=-cip[1]*dwp(2);
                eq[1][11]=-cip[1];

                for(int row=0; row<2; ++row)
                {
                    for(unsigned int i=0; i<12; ++i)
                        for(unsigned int j=0; j<12; ++j)
                            colorAta(i,j)+=eq[row][i]*eq[row][j];
                }
            }
    }

    /* Find the color calibration system's smallest eigenvalue: */
    std::pair<Math::Matrix,Math::Matrix> colorQe=colorAta.jacobiIteration();
    minEIndex=0;
    minE=Math::abs(colorQe.second(0,0));
    for(unsigned int i=1; i<12; ++i)
    {
        if(minE>Math::abs(colorQe.second(i,0)))
        {
            minEIndex=i;
            minE=Math::abs(colorQe.second(i,0));
        }
    }

    /* Create the normalized color homography: */
    Math::Matrix colorHom(3,4);
    double scale=colorQe.first(11,minEIndex);
    for(int i=0; i<3; ++i)
        for(int j=0; j<4; ++j)
            colorHom(i,j)=colorQe.first(i*4+j,minEIndex)/scale;

    /* Create the full color unprojection matrix by extending the homography: */
    Math::Matrix colorProj(4,4);
    for(unsigned int i=0; i<2; ++i)
        for(unsigned int j=0; j<4; ++j)
            colorProj(i,j)=colorHom(i,j);
    for(unsigned int j=0; j<4; ++j)
        colorProj(2,j)=j==2?1.0:0.0;
    for(unsigned int j=0; j<4; ++j)
        colorProj(3,j)=colorHom(2,j);

    /* Modify the color unprojection matrix by the depth projection matrix: */
    colorProj*=depthProj;

    /* Write the calibration file: */
    IO::FilePtr calibFile(IO::openFile("CameraCalibrationMatrices.dat",IO::File::WriteOnly));
    calibFile->setEndianness(Misc::LittleEndian);
    for(int i=0; i<4; ++i)
        for(int j=0; j<4; ++j)
            calibFile->write<double>(depthProj(i,j));
    for(int i=0; i<4; ++i)
        for(int j=0; j<4; ++j)
            calibFile->write<double>(colorProj(i,j));
}
// Write the field information portion of ClassFile structure
// JVMSpec|     u2 fields_count;
// JVMSpec|     field_info fields[fields_count];
void JvmtiClassFileReconstituter::write_field_infos() {
  HandleMark hm(thread());
  Array<AnnotationArray*>* fields_anno = ikh()->fields_annotations();
  Array<AnnotationArray*>* fields_type_anno = ikh()->fields_type_annotations();

  // Compute the real number of Java fields
  int java_fields = ikh()->java_fields_count();

  write_u2(java_fields);
  for (JavaFieldStream fs(ikh()); !fs.done(); fs.next()) {
    AccessFlags access_flags = fs.access_flags();
    int name_index = fs.name_index();
    int signature_index = fs.signature_index();
    int initial_value_index = fs.initval_index();
    guarantee(name_index != 0 && signature_index != 0, "bad constant pool index for field");
    // int offset = ikh()->field_offset( index );
    int generic_signature_index = fs.generic_signature_index();
    AnnotationArray* anno = fields_anno == NULL ? NULL : fields_anno->at(fs.index());
    AnnotationArray* type_anno = fields_type_anno == NULL ? NULL : fields_type_anno->at(fs.index());

    // JVMSpec|   field_info {
    // JVMSpec|         u2 access_flags;
    // JVMSpec|         u2 name_index;
    // JVMSpec|         u2 descriptor_index;
    // JVMSpec|         u2 attributes_count;
    // JVMSpec|         attribute_info attributes[attributes_count];
    // JVMSpec|   }

    write_u2(access_flags.as_int() & JVM_RECOGNIZED_FIELD_MODIFIERS);
    write_u2(name_index);
    write_u2(signature_index);
    int attr_count = 0;
    if (initial_value_index != 0) {
      ++attr_count;
    }
    if (access_flags.is_synthetic()) {
      // ++attr_count;
    }
    if (generic_signature_index != 0) {
      ++attr_count;
    }
    if (anno != NULL) {
      ++attr_count;     // has RuntimeVisibleAnnotations attribute
    }
    if (type_anno != NULL) {
      ++attr_count;     // has RuntimeVisibleTypeAnnotations attribute
    }

    write_u2(attr_count);

    if (initial_value_index != 0) {
      write_attribute_name_index("ConstantValue");
      write_u4(2); //length always 2
      write_u2(initial_value_index);
    }
    if (access_flags.is_synthetic()) {
      // write_synthetic_attribute();
    }
    if (generic_signature_index != 0) {
      write_signature_attribute(generic_signature_index);
    }
    if (anno != NULL) {
      write_annotations_attribute("RuntimeVisibleAnnotations", anno);
    }
    if (type_anno != NULL) {
      write_annotations_attribute("RuntimeVisibleTypeAnnotations", type_anno);
    }
  }
}