<commit_before>/*
 * (C) Copyright 2015 Kurento (http://kurento.org/)
 *
 * All rights reserved. This program and the accompanying materials
 * are made available under the terms of the GNU Lesser General Public License
 * (LGPL) version 2.1 which accompanies this distribution, and is available at
 * http://www.gnu.org/licenses/lgpl-2.1.html
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
 * Lesser General Public License for more details.
 *
 */

#include "Statistics.hpp"
#include "RTCStatsType.hpp"
#include "RTCInboundRTPStreamStats.hpp"
#include "RTCOutboundRTPStreamStats.hpp"

#define GST_CAT_DEFAULT kurento_statistics
GST_DEBUG_CATEGORY_STATIC (GST_CAT_DEFAULT);
#define GST_DEFAULT_NAME "KurentoStatistics"

#define UUID_STR_SIZE 37 /* 36-byte string (plus tailing '\0') */
#define KMS_STATISTIC_FIELD_PREFIX_SESSION "session-"
#define KMS_STATISTIC_FIELD_PREFIX_SSRC "ssrc-"

namespace kurento
{

namespace stats
{

static std::shared_ptr<RTCInboundRTPStreamStats>
createRTCInboundRTPStreamStats (const GstStructure *stats)
{
  guint64 bytesReceived, packetsReceived;
  guint jitter, fractionLost, pliCount, firCount, remb;
  gint packetLost;

  packetLost = jitter = fractionLost = pliCount = firCount = remb = 0;
  bytesReceived = packetsReceived = G_GUINT64_CONSTANT (0);

  gst_structure_get (stats, "packets-received", G_TYPE_UINT64, &packetsReceived,
                     "octets-received", G_TYPE_UINT64, &bytesReceived,
                     "rb-packetslost", G_TYPE_INT, &packetLost,
                     "rb-fractionlost", G_TYPE_UINT, &fractionLost,
                     "rb-jitter", G_TYPE_UINT, &jitter, NULL);

  /* Next fields are only available with PLI and FIR statistics patches so */
  /* hey are prone to fail if these patches are not applied in Gstreamer */
  if (!gst_structure_get (stats, "recv-pli-count", G_TYPE_UINT, &pliCount,
                          "recv-fir-count", G_TYPE_UINT, &firCount, NULL) ) {
    GST_WARNING ("Current version of gstreamer has neither PLI nor FIR statistics patches applied.");
  }

  if (!gst_structure_get (stats, "remb", G_TYPE_UINT, &remb, NULL) ) {
    GST_TRACE ("No remb stats collected");
  }

  return std::make_shared <RTCInboundRTPStreamStats> ("",
         std::make_shared <RTCStatsType> (RTCStatsType::inboundrtp), 0.0, "",
         "", false, "", "", "", firCount, pliCount, 0, 0, remb,
         packetsReceived, bytesReceived, packetLost, (float) jitter,
         (float) fractionLost);
}

static std::shared_ptr<RTCOutboundRTPStreamStats>
createRTCOutboundRTPStreamStats (const GstStructure *stats)
{
  guint64 bytesSent, packetsSent, bitRate, roundTripTime;
  guint pliCount, firCount, remb;

  bytesSent = packetsSent = bitRate = roundTripTime = G_GUINT64_CONSTANT (0);
  pliCount = firCount = remb = 0;

  gst_structure_get (stats, "packets-sent", G_TYPE_UINT64, &packetsSent,
                     "octets-sent", G_TYPE_UINT64, &bytesSent, "bitrate",
                     G_TYPE_UINT64, &bitRate, "rb-round-trip", G_TYPE_UINT64,
                     &roundTripTime, NULL);

  /* Next fields are only available with PLI and FIR statistics patches so */
  /* hey are prone to fail if these patches are not applied in Gstreamer */
  if (!gst_structure_get (stats, "sent-pli-count", G_TYPE_UINT, &pliCount,
                          "sent-fir-count", G_TYPE_UINT, &firCount, NULL) ) {
    GST_WARNING ("Current version of gstreamer has neither PLI nor FIR statistics patches applied.");
  }

  if (!gst_structure_get (stats, "remb", G_TYPE_UINT, &remb, NULL) ) {
    GST_TRACE ("No remb stats collected");
  }

  return std::make_shared <RTCOutboundRTPStreamStats> ("",
         std::make_shared <RTCStatsType> (RTCStatsType::outboundrtp), 0.0, "",
         "", false, "", "", "", firCount, pliCount, 0, 0, remb,
         packetsSent, bytesSent, (float) bitRate, (float) roundTripTime);
}

static std::shared_ptr<RTCRTPStreamStats>
createRTCRTPStreamStats (uint nackCount, const GstStructure *stats)
{
  std::shared_ptr<RTCRTPStreamStats> rtcStats;
  gboolean isInternal;
  gchar *ssrcStr, *id;
  uint ssrc;

  gst_structure_get (stats, "ssrc", G_TYPE_UINT, &ssrc, "internal",
                     G_TYPE_BOOLEAN, &isInternal, "id", G_TYPE_STRING, &id, NULL);

  ssrcStr = g_strdup_printf ("%u", ssrc);

  if (isInternal) {
    /* Local SSRC */
    rtcStats = createRTCOutboundRTPStreamStats (stats);
  } else {
    /* Remote SSRC */
    rtcStats = createRTCInboundRTPStreamStats (stats);
  }

  rtcStats->setNackCount (nackCount);
  rtcStats->setSsrc (ssrcStr);
  rtcStats->setId (id);

  g_free (ssrcStr);
  g_free (id);

  return rtcStats;
}

static void
collectRTCRTPStreamStats (std::map <std::string, std::shared_ptr<RTCStats>>
                          &rtcStatsReport, double timestamp, const GstStructure *stats)
{
  guint nackCount = 0;
  gint i, n;

  gst_structure_get (stats, "sent-nack-count", G_TYPE_UINT, &nackCount, NULL);

  n = gst_structure_n_fields (stats);

  for (i = 0; i < n; i++) {
    std::shared_ptr<RTCStats> rtcStats;
    const GValue *value;
    const gchar *name;

    name = gst_structure_nth_field_name (stats, i);

    if (!g_str_has_prefix (name, KMS_STATISTIC_FIELD_PREFIX_SSRC) ) {
      continue;
    }

    value = gst_structure_get_value (stats, name);

    if (!GST_VALUE_HOLDS_STRUCTURE (value) ) {
      gchar *str_val;

      str_val = g_strdup_value_contents (value);
      GST_WARNING ("Unexpected field type (%s) = %s", name, str_val);
      g_free (str_val);

      continue;
    }

    rtcStats = createRTCRTPStreamStats (nackCount,
                                        gst_value_get_structure (value) );

    rtcStats->setTimestamp (timestamp);

    rtcStatsReport[rtcStats->getId ()] = rtcStats;
  }
}

std::map <std::string, std::shared_ptr<RTCStats>> createRTCStatsReport (
      double timestamp, const GstStructure *stats)
{
  std::map <std::string, std::shared_ptr<RTCStats>> rtcStatsReport;
  gint i, n;

  n = gst_structure_n_fields (stats);

  for (i = 0; i < n; i++) {
    std::shared_ptr<RTCStats> rtcStats;
    const GValue *value;
    const gchar *name;

    name = gst_structure_nth_field_name (stats, i);

    if (!g_str_has_prefix (name, KMS_STATISTIC_FIELD_PREFIX_SESSION) ) {
      GST_DEBUG ("Ignoring field %s", name);
      continue;
    }

    value = gst_structure_get_value (stats, name);

    if (!GST_VALUE_HOLDS_STRUCTURE (value) ) {
      gchar *str_val;

      str_val = g_strdup_value_contents (value);
      GST_WARNING ("Unexpected field type (%s) = %s", name, str_val);
      g_free (str_val);

      continue;
    }

    collectRTCRTPStreamStats (rtcStatsReport, timestamp,
                              gst_value_get_structure (value) );
  }

  return rtcStatsReport;
}

} /* statistics */

} /* kurento */

static void init_debug (void) __attribute__ ( (constructor) );

static void
init_debug (void)
{
  GST_DEBUG_CATEGORY_INIT (GST_CAT_DEFAULT, GST_DEFAULT_NAME, 0,
                           GST_DEFAULT_NAME);
}
<commit_msg>Statistics: Fix how statistics are collected<commit_after>/*
 * (C) Copyright 2015 Kurento (http://kurento.org/)
 *
 * All rights reserved. This program and the accompanying materials
 * are made available under the terms of the GNU Lesser General Public License
 * (LGPL) version 2.1 which accompanies this distribution, and is available at
 * http://www.gnu.org/licenses/lgpl-2.1.html
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
 * Lesser General Public License for more details.
 *
 */

#include "Statistics.hpp"
#include "RTCStatsType.hpp"
#include "RTCInboundRTPStreamStats.hpp"
#include "RTCOutboundRTPStreamStats.hpp"

#define GST_CAT_DEFAULT kurento_statistics
GST_DEBUG_CATEGORY_STATIC (GST_CAT_DEFAULT);
#define GST_DEFAULT_NAME "KurentoStatistics"

#define UUID_STR_SIZE 37 /* 36-byte string (plus tailing '\0') */
#define KMS_STATISTIC_FIELD_PREFIX_SESSION "session-"
#define KMS_STATISTIC_FIELD_PREFIX_SSRC "ssrc-"

namespace kurento
{

namespace stats
{

static std::shared_ptr<RTCInboundRTPStreamStats>
createRTCInboundRTPStreamStats (const GstStructure *stats)
{
  guint64 bytesReceived, packetsReceived;
  guint jitter, fractionLost, pliCount, firCount, remb;
  gint packetLost;

  packetLost = jitter = fractionLost = pliCount = firCount = remb = 0;
  bytesReceived = packetsReceived = G_GUINT64_CONSTANT (0);

  gst_structure_get (stats, "packets-received", G_TYPE_UINT64, &packetsReceived,
                     "octets-received", G_TYPE_UINT64, &bytesReceived,
                     "rb-packetslost", G_TYPE_INT, &packetLost,
                     "rb-fractionlost", G_TYPE_UINT, &fractionLost,
                     "rb-jitter", G_TYPE_UINT, &jitter, NULL);

  /* Next fields are only available with PLI and FIR statistics patches so */
  /* hey are prone to fail if these patches are not applied in Gstreamer */
  if (!gst_structure_get (stats, "sent-pli-count", G_TYPE_UINT, &pliCount,
                          "sent-fir-count", G_TYPE_UINT, &firCount, NULL) ) {
    GST_WARNING ("Current version of gstreamer has neither PLI nor FIR statistics patches applied.");
  }

  if (!gst_structure_get (stats, "remb", G_TYPE_UINT, &remb, NULL) ) {
    GST_TRACE ("No remb stats collected");
  }

  return std::make_shared <RTCInboundRTPStreamStats> ("",
         std::make_shared <RTCStatsType> (RTCStatsType::inboundrtp), 0.0, "",
         "", false, "", "", "", firCount, pliCount, 0, 0, remb,
         packetsReceived, bytesReceived, packetLost, (float) jitter,
         (float) fractionLost);
}

static std::shared_ptr<RTCOutboundRTPStreamStats>
createRTCOutboundRTPStreamStats (const GstStructure *stats)
{
  guint64 bytesSent, packetsSent, bitRate, roundTripTime;
  guint pliCount, firCount, remb;

  bytesSent = packetsSent = bitRate = roundTripTime = G_GUINT64_CONSTANT (0);
  pliCount = firCount = remb = 0;

  gst_structure_get (stats, "packets-sent", G_TYPE_UINT64, &packetsSent,
                     "octets-sent", G_TYPE_UINT64, &bytesSent, "bitrate",
                     G_TYPE_UINT64, &bitRate, "rb-round-trip", G_TYPE_UINT64,
                     &roundTripTime, NULL);

  /* Next fields are only available with PLI and FIR statistics patches so */
  /* hey are prone to fail if these patches are not applied in Gstreamer */
  if (!gst_structure_get (stats, "recv-pli-count", G_TYPE_UINT, &pliCount,
                          "recv-fir-count", G_TYPE_UINT, &firCount, NULL) ) {
    GST_WARNING ("Current version of gstreamer has neither PLI nor FIR statistics patches applied.");
  }

  if (!gst_structure_get (stats, "remb", G_TYPE_UINT, &remb, NULL) ) {
    GST_TRACE ("No remb stats collected");
  }

  return std::make_shared <RTCOutboundRTPStreamStats> ("",
         std::make_shared <RTCStatsType> (RTCStatsType::outboundrtp), 0.0, "",
         "", false, "", "", "", firCount, pliCount, 0, 0, remb,
         packetsSent, bytesSent, (float) bitRate, (float) roundTripTime);
}

static std::shared_ptr<RTCRTPStreamStats>
createRTCRTPStreamStats (uint nackCount, const GstStructure *stats)
{
  std::shared_ptr<RTCRTPStreamStats> rtcStats;
  gboolean isInternal;
  gchar *ssrcStr, *id;
  uint ssrc;

  gst_structure_get (stats, "ssrc", G_TYPE_UINT, &ssrc, "internal",
                     G_TYPE_BOOLEAN, &isInternal, "id", G_TYPE_STRING, &id, NULL);

  ssrcStr = g_strdup_printf ("%u", ssrc);

  if (isInternal) {
    /* Local SSRC */
    rtcStats = createRTCOutboundRTPStreamStats (stats);
  } else {
    /* Remote SSRC */
    rtcStats = createRTCInboundRTPStreamStats (stats);
  }

  rtcStats->setNackCount (nackCount);
  rtcStats->setSsrc (ssrcStr);
  rtcStats->setId (id);

  g_free (ssrcStr);
  g_free (id);

  return rtcStats;
}

static void
collectRTCRTPStreamStats (std::map <std::string, std::shared_ptr<RTCStats>>
                          &rtcStatsReport, double timestamp, const GstStructure *stats)
{
  guint nackCount = 0;
  gint i, n;

  gst_structure_get (stats, "sent-nack-count", G_TYPE_UINT, &nackCount, NULL);

  n = gst_structure_n_fields (stats);

  for (i = 0; i < n; i++) {
    std::shared_ptr<RTCStats> rtcStats;
    const GValue *value;
    const gchar *name;

    name = gst_structure_nth_field_name (stats, i);

    if (!g_str_has_prefix (name, KMS_STATISTIC_FIELD_PREFIX_SSRC) ) {
      continue;
    }

    value = gst_structure_get_value (stats, name);

    if (!GST_VALUE_HOLDS_STRUCTURE (value) ) {
      gchar *str_val;

      str_val = g_strdup_value_contents (value);
      GST_WARNING ("Unexpected field type (%s) = %s", name, str_val);
      g_free (str_val);

      continue;
    }

    rtcStats = createRTCRTPStreamStats (nackCount,
                                        gst_value_get_structure (value) );

    rtcStats->setTimestamp (timestamp);

    rtcStatsReport[rtcStats->getId ()] = rtcStats;
  }
}

std::map <std::string, std::shared_ptr<RTCStats>> createRTCStatsReport (
      double timestamp, const GstStructure *stats)
{
  std::map <std::string, std::shared_ptr<RTCStats>> rtcStatsReport;
  gint i, n;

  n = gst_structure_n_fields (stats);

  for (i = 0; i < n; i++) {
    std::shared_ptr<RTCStats> rtcStats;
    const GValue *value;
    const gchar *name;

    name = gst_structure_nth_field_name (stats, i);

    if (!g_str_has_prefix (name, KMS_STATISTIC_FIELD_PREFIX_SESSION) ) {
      GST_DEBUG ("Ignoring field %s", name);
      continue;
    }

    value = gst_structure_get_value (stats, name);

    if (!GST_VALUE_HOLDS_STRUCTURE (value) ) {
      gchar *str_val;

      str_val = g_strdup_value_contents (value);
      GST_WARNING ("Unexpected field type (%s) = %s", name, str_val);
      g_free (str_val);

      continue;
    }

    collectRTCRTPStreamStats (rtcStatsReport, timestamp,
                              gst_value_get_structure (value) );
  }

  return rtcStatsReport;
}

} /* statistics */

} /* kurento */

static void init_debug (void) __attribute__ ( (constructor) );

static void
init_debug (void)
{
  GST_DEBUG_CATEGORY_INIT (GST_CAT_DEFAULT, GST_DEFAULT_NAME, 0,
                           GST_DEFAULT_NAME);
}
<|endoftext|>


<commit_before>/****************************************************************************
 *
 *   Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. 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.
 * 3. Neither the name PX4 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 OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file VtolLandDetector.cpp
 * Land detection algorithm for VTOL
 *
 * @author Roman Bapst <bapstroma@gmail.com>
 * @author Julian Oes <julian@oes.ch>
 */

#include <drivers/drv_hrt.h>

#include "VtolLandDetector.h"

namespace land_detector
{

VtolLandDetector::VtolLandDetector()
{
	_paramHandle.maxAirSpeed = param_find("LNDFW_AIRSPD_MAX");
}

void VtolLandDetector::_initialize_topics()
{
	MulticopterLandDetector::_initialize_topics();

	_airspeedSub = orb_subscribe(ORB_ID(airspeed));
	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
}

void VtolLandDetector::_update_topics()
{
	MulticopterLandDetector::_update_topics();

	_orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed);
	_orb_update(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
}

bool VtolLandDetector::_get_maybe_landed_state()
{
	// Only trigger in RW mode
	if (!_vehicle_status.is_rotary_wing) {
		return false;
	}

	return MulticopterLandDetector::_get_maybe_landed_state();
}

bool VtolLandDetector::_get_landed_state()
{
	// Only trigger in RW mode
	if (!_vehicle_status.is_rotary_wing) {
		return false;
	}

	// this is returned from the mutlicopter land detector
	bool landed = MulticopterLandDetector::_get_landed_state();

	// for vtol we additionally consider airspeed
	if (hrt_elapsed_time(&_airspeed.timestamp) < 500 * 1000) {
		_airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s;

	} else {
		// if airspeed does not update, set it to zero and rely on multicopter land detector
		_airspeed_filtered = 0.0f;
	}

	// only consider airspeed if we have been in air before to avoid false
	// detections in the case of wind on the ground
	if (_was_in_air && _airspeed_filtered > _params.maxAirSpeed) {
		landed = false;
	}

	_was_in_air = !landed;

	return landed;
}

void VtolLandDetector::_update_params()
{
	MulticopterLandDetector::_update_params();

	param_get(_paramHandle.maxAirSpeed, &_params.maxAirSpeed);
}

} // namespace land_detector
<commit_msg>VtolLandDetector require vehicle_status timestamp<commit_after>/****************************************************************************
 *
 *   Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. 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.
 * 3. Neither the name PX4 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 OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/**
 * @file VtolLandDetector.cpp
 * Land detection algorithm for VTOL
 *
 * @author Roman Bapst <bapstroma@gmail.com>
 * @author Julian Oes <julian@oes.ch>
 */

#include <drivers/drv_hrt.h>

#include "VtolLandDetector.h"

namespace land_detector
{

VtolLandDetector::VtolLandDetector()
{
	_paramHandle.maxAirSpeed = param_find("LNDFW_AIRSPD_MAX");
}

void VtolLandDetector::_initialize_topics()
{
	MulticopterLandDetector::_initialize_topics();

	_airspeedSub = orb_subscribe(ORB_ID(airspeed));
	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
}

void VtolLandDetector::_update_topics()
{
	MulticopterLandDetector::_update_topics();

	_orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed);
	_orb_update(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
}

bool VtolLandDetector::_get_maybe_landed_state()
{
	// Only trigger in RW mode
	if ((_vehicle_status.timestamp != 0) && !_vehicle_status.is_rotary_wing) {
		return false;
	}

	return MulticopterLandDetector::_get_maybe_landed_state();
}

bool VtolLandDetector::_get_landed_state()
{
	// Only trigger in RW mode
	if ((_vehicle_status.timestamp != 0) && !_vehicle_status.is_rotary_wing) {
		return false;
	}

	// this is returned from the mutlicopter land detector
	bool landed = MulticopterLandDetector::_get_landed_state();

	// for vtol we additionally consider airspeed
	if (hrt_elapsed_time(&_airspeed.timestamp) < 500 * 1000) {
		_airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s;

	} else {
		// if airspeed does not update, set it to zero and rely on multicopter land detector
		_airspeed_filtered = 0.0f;
	}

	// only consider airspeed if we have been in air before to avoid false
	// detections in the case of wind on the ground
	if (_was_in_air && _airspeed_filtered > _params.maxAirSpeed) {
		landed = false;
	}

	_was_in_air = !landed;

	return landed;
}

void VtolLandDetector::_update_params()
{
	MulticopterLandDetector::_update_params();

	param_get(_paramHandle.maxAirSpeed, &_params.maxAirSpeed);
}

} // namespace land_detector
<|endoftext|>


<commit_before>#include "LadderQueue.hpp"
#include <cassert>

namespace warped {

LadderQueue::LadderQueue() {

    /* Reserve the size of Top */
    top_.buffer_.reserve(2*THRESHOLD);

    /* Create and Initialize Rungs */
    for (unsigned int i = 0; i < MAX_RUNG_CNT; i++) {

        /* NOTE: First rung's bucket count equals 2*THRESHOLD,
                 Other rungs' bucket count equals THRESHOLD
         */
        unsigned int bucket_cnt = i ? THRESHOLD : 2*THRESHOLD;
        for (unsigned int j = 0; j < bucket_cnt; j++) {
            bucket temp;
            temp.reserve(THRESHOLD);
            rung_[i].buffer_.push_back(temp);
        }
    }

#if defined(PARTIALLY_SORTED_LADDER_QUEUE)
    /* Initialize Bottom capacity */
    bottom_.reserve(THRESHOLD);
#endif
}

std::shared_ptr<Event> LadderQueue::dequeue() {

    // Remove from bottom if not empty
    if ( !bottom_.empty() ) {
        auto e = *bottom_.begin();
        bottom_.erase( bottom_.begin() );
        return e;
    }

    // If there are no rungs, pull from Top
    if (!rung_cnt_) {
        createNewRung();

        // Transfer events from Top to 1st rung of Ladder
        rung_[0].first_nonempty_bucket_ts_ = (unsigned int)-1;
        rung_[0].last_nonempty_bucket_ts_ = 0;
        for (auto event : top_.buffer_) {
            unsigned int index = (event->timestamp()-rung_[0].start_ts_) / rung_[0].bucket_width_;
            rung_[0].buffer_[index].push_back(event);

            rung_[0].first_nonempty_bucket_ts_ =  std::min( rung_[0].first_nonempty_bucket_ts_ ,
                                                            rung_[0].start_ts_ +
                                                                index*rung_[0].bucket_width_    );
            rung_[0].last_nonempty_bucket_ts_ =
                                    std::max(   rung_[0].last_nonempty_bucket_ts_ ,
                                                rung_[0].start_ts_ +
                                                    index*rung_[0].bucket_width_    );
        }
        top_.buffer_.clear();
        top_.start_ts_ = top_.max_ts_ + 1;

        recurseRung();
    }

    // Move the first non-empty bucket of last rung to bottom
    unsigned int bucket_index =
        (rung_[rung_cnt_-1].first_nonempty_bucket_ts_ - rung_[rung_cnt_-1].start_ts_) /
                                                rung_[rung_cnt_-1].bucket_width_;

#if defined(PARTIALLY_SORTED_LADDER_QUEUE)
    bottom_start_ = rung_[rung_cnt_-1].first_nonempty_bucket_ts_;
    bottom_.swap(rung_[rung_cnt_-1].buffer_[bucket_index]);

#else
    for (auto event : rung_[rung_cnt_-1].buffer_[bucket_index]) {
        bottom_.insert(event);
    }
#endif

    rung_[rung_cnt_-1].buffer_[bucket_index].clear();

    // If last rung is empty now
    if (rung_[rung_cnt_-1].first_nonempty_bucket_ts_ ==
                                rung_[rung_cnt_-1].last_nonempty_bucket_ts_) {
        rung_cnt_--;

    } else { // Another non-empty bucket is present on the last rung
        do {
            rung_[rung_cnt_-1].first_nonempty_bucket_ts_ += rung_[rung_cnt_-1].bucket_width_;
        } while( rung_[rung_cnt_-1].buffer_[++bucket_index].empty() );
    }
    recurseRung();

    // Check and erase from bottom, if present
    if (bottom_.empty()) return nullptr;

    auto e = *bottom_.begin();
    bottom_.erase( bottom_.begin() );
    return e;
}

void LadderQueue::insert(std::shared_ptr<Event> event) {

    assert(event);
    auto ts = event->timestamp();

    // Insert into top, if valid
    if (ts >= top_.start_ts_) {
        if(top_.buffer_.empty()) {
            top_.max_ts_ = ts;
        }
        top_.max_ts_ = std::max( ts, top_.max_ts_ );
        top_.buffer_.push_back(event);
        return;
    }

    // Step through rungs
    unsigned int rung_index = 0;
    while ( (rung_index < rung_cnt_) && (ts < rung_[rung_index].first_nonempty_bucket_ts_) ) {
        rung_index++;
    }

    // If a valid rung was found, insert the event
    if (rung_index < rung_cnt_) {
        unsigned int bucket_index = (ts - rung_[rung_index].start_ts_) /
                                                            rung_[rung_index].bucket_width_;

        rung_[rung_index].last_nonempty_bucket_ts_ =
                    std::max (  rung_[rung_index].last_nonempty_bucket_ts_,
                                rung_[rung_index].start_ts_ +
                                    bucket_index*rung_[rung_index].bucket_width_    );

        rung_[rung_index].buffer_[bucket_index].push_back(event);
        recurseRung();
        return;
    }

    // Insert into Bottom
#if defined(PARTIALLY_SORTED_LADDER_QUEUE)
    bottom_.push_back(event);
    bottom_start_ = std::min(bottom_start_, ts);
#else
    bottom_.insert(event);
#endif
}

bool LadderQueue::erase(std::shared_ptr<Event> event) {

    assert(event);
    auto ts = event->timestamp();

    // Erase from top, if found there
    if (ts >= top_.start_ts_) {
        if(top_.buffer_.empty()) return false;

        top_.max_ts_ = 0;
        bool status = false;
        for (auto it = top_.buffer_.begin(); it != top_.buffer_.end();) {
            if (*it == event) {
                status = true;
                it = top_.buffer_.erase(it);
            } else {
                top_.max_ts_ = std::max( (*it)->timestamp(), top_.max_ts_ );
                it++;
            }
        }
        return status;
    }

    // Step through rungs
    unsigned int rung_index = 0;
    while ( (rung_index < rung_cnt_) && (ts < rung_[rung_index].first_nonempty_bucket_ts_) ) {
        rung_index++;
    }

    // Delete the element if found inside a valid rung
    if (rung_index < rung_cnt_) {
        unsigned int bucket_index = (ts - rung_[rung_index].start_ts_) /
                                                    rung_[rung_index].bucket_width_;
        unsigned int current_index =
                (rung_[rung_index].first_nonempty_bucket_ts_ - rung_[rung_index].start_ts_) /
                                                    rung_[rung_index].bucket_width_;
        unsigned int last_valid_index =
                (rung_[rung_index].last_nonempty_bucket_ts_ - rung_[rung_index].start_ts_) /
                                                    rung_[rung_index].bucket_width_;

        // NOTE: Don't delete event if it is the only event in the
        //       first or last non-empty bucket of a inner rung
        if (rung_[rung_index].buffer_[bucket_index].size() <= 1) {

            // If chosen rung is an inner one
            if (rung_index < rung_cnt_-1) {

                // If the chosen bucket is the first or the last non-empty bucket
                if ( bucket_index == current_index || bucket_index == last_valid_index ) {
                    return false;
                }
            }
        }

        // Delete the event, if found
        bool status = false;
        for (auto it = rung_[rung_index].buffer_[bucket_index].begin();
                                it != rung_[rung_index].buffer_[bucket_index].end(); it++) {
            if (*it == event) {
                (void) rung_[rung_index].buffer_[bucket_index].erase(it);
                status = true;
                break;
            }
        }

        // If erase successful and chosen bucket in the last rung is empty after erase
        if (status && rung_index == rung_cnt_-1 &&
                            !rung_[rung_index].buffer_[bucket_index].size()) {

            // If that bucket was the only one in the last rung
            if (current_index == last_valid_index) {
                rung_cnt_--;
                recurseRung();

            } else if (bucket_index == current_index) { // first non-empty bucket
                // Update the first nonempty ts for last rung
                do {
                    rung_[rung_index].first_nonempty_bucket_ts_ += rung_[rung_index].bucket_width_;
                } while( rung_[rung_index].buffer_[++bucket_index].empty() );
                recurseRung();

            } else if (bucket_index == last_valid_index) { // last non-empty bucket
                // Update the last nonempty ts for last rung
                do {
                    rung_[rung_index].last_nonempty_bucket_ts_ -= rung_[rung_index].bucket_width_;
                } while( rung_[rung_index].buffer_[--bucket_index].empty() );
            }
        }
        return status;
    }

    // Check and erase from bottom, if present
    if (bottom_.empty()) return false;

    bool status = false;

#if defined(PARTIALLY_SORTED_LADDER_QUEUE)
    for (auto it = bottom_.begin(); it != bottom_.end(); it++) {
        if (*it == event) {
            (void) bottom_.erase(it);
            status = true;
            break;
        }
    }
#else
    status = (bottom_.erase(event) >= 1) ? true : false;
#endif

    return status;
}

void LadderQueue::createNewRung() {

    /* Check if this is the first rung creation */
    if (!rung_cnt_) {
        rung_cnt_++;
        rung_[0].bucket_width_ = (top_.max_ts_ - top_.start_ts_ + rung_[0].buffer_.size()) /
                                                                            rung_[0].buffer_.size();
        rung_[0].start_ts_  = top_.start_ts_;

    } else { // When rungs already exist
        rung_cnt_++;
        rung_[rung_cnt_-1].bucket_width_ =
            (rung_[rung_cnt_-2].bucket_width_ + rung_[rung_cnt_-1].buffer_.size()) /
                                                        rung_[rung_cnt_-1].buffer_.size();
        rung_[rung_cnt_-1].start_ts_  = rung_[rung_cnt_-2].first_nonempty_bucket_ts_;
    }
}

void LadderQueue::recurseRung() {

    // If there are no rungs
    if (!rung_cnt_) return;

    // If rung count has reached upper threshold
    if (rung_cnt_ >= MAX_RUNG_CNT) return;

    // If the last rung has only one non-empty bucket
    if (rung_[rung_cnt_-1].last_nonempty_bucket_ts_ ==
                    rung_[rung_cnt_-1].first_nonempty_bucket_ts_) return;

    // If the last rung has minimum bucket width
    if (rung_[rung_cnt_-1].bucket_width_ == MIN_BUCKET_WIDTH) return;

    unsigned int bucket_index =
        (rung_[rung_cnt_-1].first_nonempty_bucket_ts_ - rung_[rung_cnt_-1].start_ts_) /
                                                rung_[rung_cnt_-1].bucket_width_;

    // If event count inside the first non-empty bucket of last rung is within threshold
    if (rung_[rung_cnt_-1].buffer_[bucket_index].size() <= THRESHOLD) return;

    // Create a new rung if event count exceeds threshold
    createNewRung();

    // Move events from the bucket to the new rung
    rung_[rung_cnt_-1].first_nonempty_bucket_ts_ = (unsigned int)-1;
    rung_[rung_cnt_-1].last_nonempty_bucket_ts_ = 0;
    for (auto event : rung_[rung_cnt_-2].buffer_[bucket_index]) {
        unsigned int index =
            (event->timestamp() - rung_[rung_cnt_-1].start_ts_) / rung_[rung_cnt_-1].bucket_width_;
        rung_[rung_cnt_-1].buffer_[index].push_back(event);
        rung_[rung_cnt_-1].first_nonempty_bucket_ts_ =
                        std::min(   rung_[rung_cnt_-1].first_nonempty_bucket_ts_ ,
                                    rung_[rung_cnt_-1].start_ts_ +
                                            index*rung_[rung_cnt_-1].bucket_width_  );
        rung_[rung_cnt_-1].last_nonempty_bucket_ts_ =
                        std::max(   rung_[rung_cnt_-1].last_nonempty_bucket_ts_ ,
                                    rung_[rung_cnt_-1].start_ts_ +
                                            index*rung_[rung_cnt_-1].bucket_width_  );
    }
    rung_[rung_cnt_-2].buffer_[bucket_index].clear();

    // Update the current ts for penultimate rung
    do {
        rung_[rung_cnt_-2].first_nonempty_bucket_ts_ += rung_[rung_cnt_-2].bucket_width_;
    } while( rung_[rung_cnt_-2].buffer_[++bucket_index].empty() );

    // Repeat the recursive process
    recurseRung();
}

} // namespace warped
<commit_msg>added enhancements to the Ladder Queue library to make unsorted bottom work faster<commit_after>#include "LadderQueue.hpp"
#include <cassert>

namespace warped {

LadderQueue::LadderQueue() {

    /* Reserve the size of Top */
    top_.buffer_.reserve(2*THRESHOLD);

    /* Create and Initialize Rungs */
    for (unsigned int i = 0; i < MAX_RUNG_CNT; i++) {

        /* NOTE: First rung's bucket count equals 2*THRESHOLD,
                 Other rungs' bucket count equals THRESHOLD
         */
        unsigned int bucket_cnt = i ? THRESHOLD : 2*THRESHOLD;
        for (unsigned int j = 0; j < bucket_cnt; j++) {
            bucket temp;
            temp.reserve(THRESHOLD);
            rung_[i].buffer_.push_back(temp);
        }
    }

#ifdef PARTIALLY_SORTED_LADDER_QUEUE
    /* Initialize Bottom capacity */
    bottom_.reserve(THRESHOLD);
#endif
}

std::shared_ptr<Event> LadderQueue::dequeue() {

    // Remove from bottom if not empty
    if ( !bottom_.empty() ) {

#ifdef PARTIALLY_SORTED_LADDER_QUEUE
        auto e = bottom_.back();
        bottom_.pop_back();
        return e;
#else
        auto e = *bottom_.begin();
        bottom_.erase( bottom_.begin() );
        return e;
#endif
    }

    // If there are no rungs, pull from Top
    if (!rung_cnt_) {
        createNewRung();

        // Transfer events from Top to 1st rung of Ladder
        rung_[0].first_nonempty_bucket_ts_ = (unsigned int)-1;
        rung_[0].last_nonempty_bucket_ts_ = 0;
        for (auto event : top_.buffer_) {
            unsigned int index = (event->timestamp()-rung_[0].start_ts_) / rung_[0].bucket_width_;
            rung_[0].buffer_[index].push_back(event);

            rung_[0].first_nonempty_bucket_ts_ =  std::min( rung_[0].first_nonempty_bucket_ts_ ,
                                                            rung_[0].start_ts_ +
                                                                index*rung_[0].bucket_width_    );
            rung_[0].last_nonempty_bucket_ts_ =
                                    std::max(   rung_[0].last_nonempty_bucket_ts_ ,
                                                rung_[0].start_ts_ +
                                                    index*rung_[0].bucket_width_    );
        }
        top_.buffer_.clear();
        top_.start_ts_ = top_.max_ts_ + 1;

        recurseRung();
    }

    // Move the first non-empty bucket of last rung to bottom
    unsigned int bucket_index =
        (rung_[rung_cnt_-1].first_nonempty_bucket_ts_ - rung_[rung_cnt_-1].start_ts_) /
                                                rung_[rung_cnt_-1].bucket_width_;

#ifdef PARTIALLY_SORTED_LADDER_QUEUE
    bottom_start_ = rung_[rung_cnt_-1].first_nonempty_bucket_ts_;
    for (auto event : rung_[rung_cnt_-1].buffer_[bucket_index]) {
        bottom_.push_back(event);
    }
    rung_[rung_cnt_-1].buffer_[bucket_index].clear();
#else
    for (auto event : rung_[rung_cnt_-1].buffer_[bucket_index]) {
        bottom_.insert(event);
    }
    rung_[rung_cnt_-1].buffer_[bucket_index].clear();
#endif

    // If last rung is empty now
    if (rung_[rung_cnt_-1].first_nonempty_bucket_ts_ ==
                                rung_[rung_cnt_-1].last_nonempty_bucket_ts_) {
        rung_cnt_--;

    } else { // Another non-empty bucket is present on the last rung
        do {
            rung_[rung_cnt_-1].first_nonempty_bucket_ts_ += rung_[rung_cnt_-1].bucket_width_;
        } while( rung_[rung_cnt_-1].buffer_[++bucket_index].empty() );
    }
    recurseRung();

    // Check and erase from bottom, if present
    if (bottom_.empty()) return nullptr;

#ifdef PARTIALLY_SORTED_LADDER_QUEUE
    auto e = bottom_.back();
    bottom_.pop_back();
    return e;
#else
    auto e = *bottom_.begin();
    bottom_.erase( bottom_.begin() );
    return e;
#endif
}

void LadderQueue::insert(std::shared_ptr<Event> event) {

    assert(event);
    auto ts = event->timestamp();

    // Insert into top, if valid
    if (ts >= top_.start_ts_) {
        if(top_.buffer_.empty()) {
            top_.max_ts_ = ts;
        }
        top_.max_ts_ = std::max( ts, top_.max_ts_ );
        top_.buffer_.push_back(event);
        return;
    }

    // Step through rungs
    unsigned int rung_index = 0;
    while ( (rung_index < rung_cnt_) && (ts < rung_[rung_index].first_nonempty_bucket_ts_) ) {
        rung_index++;
    }

    // If a valid rung was found, insert the event
    if (rung_index < rung_cnt_) {
        unsigned int bucket_index = (ts - rung_[rung_index].start_ts_) /
                                                            rung_[rung_index].bucket_width_;

        rung_[rung_index].last_nonempty_bucket_ts_ =
                    std::max (  rung_[rung_index].last_nonempty_bucket_ts_,
                                rung_[rung_index].start_ts_ +
                                    bucket_index*rung_[rung_index].bucket_width_    );

        rung_[rung_index].buffer_[bucket_index].push_back(event);
        recurseRung();
        return;
    }

    // Insert into Bottom
#ifdef PARTIALLY_SORTED_LADDER_QUEUE
    bottom_.push_back(event);
    bottom_start_ = std::min(bottom_start_, ts);
#else
    bottom_.insert(event);
#endif
}

bool LadderQueue::erase(std::shared_ptr<Event> event) {

    assert(event);
    auto ts = event->timestamp();

    // Erase from top, if found there
    if (ts >= top_.start_ts_) {
        if(top_.buffer_.empty()) return false;

        top_.max_ts_ = 0;
        bool status = false;
        for (auto it = top_.buffer_.begin(); it != top_.buffer_.end();) {
            if (*it == event) {
                status = true;
                it = top_.buffer_.erase(it);
            } else {
                top_.max_ts_ = std::max( (*it)->timestamp(), top_.max_ts_ );
                it++;
            }
        }
        return status;
    }

    // Step through rungs
    unsigned int rung_index = 0;
    while ( (rung_index < rung_cnt_) && (ts < rung_[rung_index].first_nonempty_bucket_ts_) ) {
        rung_index++;
    }

    // Delete the element if found inside a valid rung
    if (rung_index < rung_cnt_) {
        unsigned int bucket_index = (ts - rung_[rung_index].start_ts_) /
                                                    rung_[rung_index].bucket_width_;
        unsigned int current_index =
                (rung_[rung_index].first_nonempty_bucket_ts_ - rung_[rung_index].start_ts_) /
                                                    rung_[rung_index].bucket_width_;
        unsigned int last_valid_index =
                (rung_[rung_index].last_nonempty_bucket_ts_ - rung_[rung_index].start_ts_) /
                                                    rung_[rung_index].bucket_width_;

        // NOTE: Don't delete event if it is the only event in the
        //       first or last non-empty bucket of a inner rung
        if (rung_[rung_index].buffer_[bucket_index].size() <= 1) {

            // If chosen rung is an inner one
            if (rung_index < rung_cnt_-1) {

                // If the chosen bucket is the first or the last non-empty bucket
                if ( bucket_index == current_index || bucket_index == last_valid_index ) {
                    return false;
                }
            }
        }

        // Delete the event, if found
        bool status = false;
        for (auto it = rung_[rung_index].buffer_[bucket_index].begin();
                                it != rung_[rung_index].buffer_[bucket_index].end(); it++) {
            if (*it == event) {
                (void) rung_[rung_index].buffer_[bucket_index].erase(it);
                status = true;
                break;
            }
        }

        // If erase successful and chosen bucket in the last rung is empty after erase
        if (status && rung_index == rung_cnt_-1 &&
                            !rung_[rung_index].buffer_[bucket_index].size()) {

            // If that bucket was the only one in the last rung
            if (current_index == last_valid_index) {
                rung_cnt_--;
                recurseRung();

            } else if (bucket_index == current_index) { // first non-empty bucket
                // Update the first nonempty ts for last rung
                do {
                    rung_[rung_index].first_nonempty_bucket_ts_ += rung_[rung_index].bucket_width_;
                } while( rung_[rung_index].buffer_[++bucket_index].empty() );
                recurseRung();

            } else if (bucket_index == last_valid_index) { // last non-empty bucket
                // Update the last nonempty ts for last rung
                do {
                    rung_[rung_index].last_nonempty_bucket_ts_ -= rung_[rung_index].bucket_width_;
                } while( rung_[rung_index].buffer_[--bucket_index].empty() );
            }
        }
        return status;
    }

    // Check and erase from bottom, if present
    if (bottom_.empty()) return false;

    bool status = false;

#ifdef PARTIALLY_SORTED_LADDER_QUEUE
    for (auto it = bottom_.begin(); it != bottom_.end(); it++) {
        if (*it == event) {
            (void) bottom_.erase(it);
            status = true;
            break;
        }
    }
#else
    status = (bottom_.erase(event) >= 1) ? true : false;
#endif

    return status;
}

void LadderQueue::createNewRung() {

    /* Check if this is the first rung creation */
    if (!rung_cnt_) {
        rung_cnt_++;
        rung_[0].bucket_width_ = (top_.max_ts_ - top_.start_ts_ + rung_[0].buffer_.size()) /
                                                                            rung_[0].buffer_.size();
        rung_[0].start_ts_  = top_.start_ts_;

    } else { // When rungs already exist
        rung_cnt_++;
        rung_[rung_cnt_-1].bucket_width_ =
            (rung_[rung_cnt_-2].bucket_width_ + rung_[rung_cnt_-1].buffer_.size()) /
                                                        rung_[rung_cnt_-1].buffer_.size();
        rung_[rung_cnt_-1].start_ts_  = rung_[rung_cnt_-2].first_nonempty_bucket_ts_;
    }
}

void LadderQueue::recurseRung() {

    // If there are no rungs
    if (!rung_cnt_) return;

    // If rung count has reached upper threshold
    if (rung_cnt_ >= MAX_RUNG_CNT) return;

    // If the last rung has only one non-empty bucket
    if (rung_[rung_cnt_-1].last_nonempty_bucket_ts_ ==
                    rung_[rung_cnt_-1].first_nonempty_bucket_ts_) return;

    // If the last rung has minimum bucket width
    if (rung_[rung_cnt_-1].bucket_width_ == MIN_BUCKET_WIDTH) return;

    unsigned int bucket_index =
        (rung_[rung_cnt_-1].first_nonempty_bucket_ts_ - rung_[rung_cnt_-1].start_ts_) /
                                                rung_[rung_cnt_-1].bucket_width_;

    // If event count inside the first non-empty bucket of last rung is within threshold
    if (rung_[rung_cnt_-1].buffer_[bucket_index].size() <= THRESHOLD) return;

    // Create a new rung if event count exceeds threshold
    createNewRung();

    // Move events from the bucket to the new rung
    rung_[rung_cnt_-1].first_nonempty_bucket_ts_ = (unsigned int)-1;
    rung_[rung_cnt_-1].last_nonempty_bucket_ts_ = 0;
    for (auto event : rung_[rung_cnt_-2].buffer_[bucket_index]) {
        unsigned int index =
            (event->timestamp() - rung_[rung_cnt_-1].start_ts_) / rung_[rung_cnt_-1].bucket_width_;
        rung_[rung_cnt_-1].buffer_[index].push_back(event);
        rung_[rung_cnt_-1].first_nonempty_bucket_ts_ =
                        std::min(   rung_[rung_cnt_-1].first_nonempty_bucket_ts_ ,
                                    rung_[rung_cnt_-1].start_ts_ +
                                            index*rung_[rung_cnt_-1].bucket_width_  );
        rung_[rung_cnt_-1].last_nonempty_bucket_ts_ =
                        std::max(   rung_[rung_cnt_-1].last_nonempty_bucket_ts_ ,
                                    rung_[rung_cnt_-1].start_ts_ +
                                            index*rung_[rung_cnt_-1].bucket_width_  );
    }
    rung_[rung_cnt_-2].buffer_[bucket_index].clear();

    // Update the current ts for penultimate rung
    do {
        rung_[rung_cnt_-2].first_nonempty_bucket_ts_ += rung_[rung_cnt_-2].bucket_width_;
    } while( rung_[rung_cnt_-2].buffer_[++bucket_index].empty() );

    // Repeat the recursive process
    recurseRung();
}

} // namespace warped
<|endoftext|>


<commit_before>/*
 *  Copyright (c) 2012 The WebRTC project authors. All Rights Reserved.
 *
 *  Use of this source code is governed by a BSD-style license
 *  that can be found in the LICENSE file in the root of the source
 *  tree. An additional intellectual property rights grant can be found
 *  in the file PATENTS.  All contributing project authors may
 *  be found in the AUTHORS file in the root of the source tree.
 */

#include "webrtc/modules/video_capture/android/device_info_android.h"

#include <algorithm>
#include <sstream>
#include <vector>

#include "json/json.h"
#include "third_party/icu/source/common/unicode/unistr.h"
#include "webrtc/modules/video_capture/android/video_capture_android.h"
#include "webrtc/system_wrappers/interface/logging.h"
#include "webrtc/system_wrappers/interface/ref_count.h"
#include "webrtc/system_wrappers/interface/trace.h"

namespace webrtc {

namespace videocapturemodule {

// Helper for storing lists of pairs of ints.  Used e.g. for resolutions & FPS
// ranges.
typedef std::pair<int, int> IntPair;
typedef std::vector<IntPair> IntPairs;

static std::string IntPairsToString(const IntPairs& pairs, char separator) {
  std::stringstream stream;
  for (size_t i = 0; i < pairs.size(); ++i) {
    if (i > 0)
      stream << ", ";
    stream << "(" << pairs[i].first << separator << pairs[i].second << ")";
  }
  return stream.str();
}

struct AndroidCameraInfo {
  std::string name;
  bool front_facing;
  int orientation;
  IntPairs resolutions;  // Pairs are: (width,height).
  // Pairs are (min,max) in units of FPS*1000 ("milli-frame-per-second").
  IntPairs mfpsRanges;

  std::string ToString() {
    std::stringstream stream;
    stream << "Name: [" << name << "], MFPS ranges: ["
           << IntPairsToString(mfpsRanges, ':')
           << "], front_facing: " << front_facing
           << ", orientation: " << orientation << ", resolutions: ["
           << IntPairsToString(resolutions, 'x') << "]";
    return stream.str();
  }
};

// Camera info; populated during DeviceInfoAndroid::Initialize() and immutable
// thereafter.
static std::vector<AndroidCameraInfo>* g_camera_info = NULL;

// Set |*index| to the index of |name| in g_camera_info or return false if no
// match found.
static bool FindCameraIndexByName(const std::string& name, size_t* index) {
  for (size_t i = 0; i < g_camera_info->size(); ++i) {
    if (g_camera_info->at(i).name == name) {
      *index = i;
      return true;
    }
  }
  return false;
}

// Returns a pointer to the named member of g_camera_info, or NULL if no match
// is found.
static AndroidCameraInfo* FindCameraInfoByName(const std::string& name) {
  size_t index = 0;
  if (FindCameraIndexByName(name, &index))
    return &g_camera_info->at(index);
  return NULL;
}

// static
void DeviceInfoAndroid::Initialize(JNIEnv* jni) {
  // TODO(henrike): this "if" would make a lot more sense as an assert, but
  // Java_org_webrtc_videoengineapp_ViEAndroidJavaAPI_GetVideoEngine() and
  // Java_org_webrtc_videoengineapp_ViEAndroidJavaAPI_Terminate() conspire to
  // prevent this.  Once that code is made to only
  // VideoEngine::SetAndroidObjects() once per process, this can turn into an
  // assert.
  if (g_camera_info)
    return;

  g_camera_info = new std::vector<AndroidCameraInfo>();
  jclass j_info_class =
      jni->FindClass("org/webrtc/videoengine/VideoCaptureDeviceInfoAndroid");
  assert(j_info_class);
  jmethodID j_initialize = jni->GetStaticMethodID(
      j_info_class, "getDeviceInfo", "()Ljava/lang/String;");
  jstring j_json_info = static_cast<jstring>(
      jni->CallStaticObjectMethod(j_info_class, j_initialize));

  const jchar* jchars = jni->GetStringChars(j_json_info, NULL);
  icu::UnicodeString ustr(jchars, jni->GetStringLength(j_json_info));
  jni->ReleaseStringChars(j_json_info, jchars);
  std::string json_info;
  ustr.toUTF8String(json_info);

  Json::Value cameras;
  Json::Reader reader(Json::Features::strictMode());
  bool parsed = reader.parse(json_info, cameras);
  if (!parsed) {
    std::stringstream stream;
    stream << "Failed to parse configuration:\n"
           << reader.getFormattedErrorMessages();
    assert(false);
    return;
  }
  for (Json::ArrayIndex i = 0; i < cameras.size(); ++i) {
    const Json::Value& camera = cameras[i];
    AndroidCameraInfo info;
    info.name = camera["name"].asString();
    info.front_facing = camera["front_facing"].asBool();
    info.orientation = camera["orientation"].asInt();
    Json::Value sizes = camera["sizes"];
    for (Json::ArrayIndex j = 0; j < sizes.size(); ++j) {
      const Json::Value& size = sizes[j];
      info.resolutions.push_back(std::make_pair(
          size["width"].asInt(), size["height"].asInt()));
    }
    Json::Value mfpsRanges = camera["mfpsRanges"];
    for (Json::ArrayIndex j = 0; j < mfpsRanges.size(); ++j) {
      const Json::Value& mfpsRange = mfpsRanges[j];
      info.mfpsRanges.push_back(std::make_pair(mfpsRange["min_mfps"].asInt(),
                                               mfpsRange["max_mfps"].asInt()));
    }
    g_camera_info->push_back(info);
  }
}

void DeviceInfoAndroid::DeInitialize() {
  if (g_camera_info) {
    delete g_camera_info;
    g_camera_info = NULL;
  }
}

VideoCaptureModule::DeviceInfo* VideoCaptureImpl::CreateDeviceInfo(
    const int32_t id) {
  return new videocapturemodule::DeviceInfoAndroid(id);
}

DeviceInfoAndroid::DeviceInfoAndroid(const int32_t id) :
    DeviceInfoImpl(id) {
}

DeviceInfoAndroid::~DeviceInfoAndroid() {
}

bool DeviceInfoAndroid::FindCameraIndex(const char* deviceUniqueIdUTF8,
                                        size_t* index) {
  return FindCameraIndexByName(deviceUniqueIdUTF8, index);
}

int32_t DeviceInfoAndroid::Init() {
  return 0;
}

uint32_t DeviceInfoAndroid::NumberOfDevices() {
  return g_camera_info->size();
}

int32_t DeviceInfoAndroid::GetDeviceName(
    uint32_t deviceNumber,
    char* deviceNameUTF8,
    uint32_t deviceNameLength,
    char* deviceUniqueIdUTF8,
    uint32_t deviceUniqueIdUTF8Length,
    char* /*productUniqueIdUTF8*/,
    uint32_t /*productUniqueIdUTF8Length*/) {
  if (deviceNumber >= g_camera_info->size())
    return -1;
  const AndroidCameraInfo& info = g_camera_info->at(deviceNumber);
  if (info.name.length() + 1 > deviceNameLength ||
      info.name.length() + 1 > deviceUniqueIdUTF8Length) {
    return -1;
  }
  memcpy(deviceNameUTF8, info.name.c_str(), info.name.length() + 1);
  memcpy(deviceUniqueIdUTF8, info.name.c_str(), info.name.length() + 1);
  return 0;
}

int32_t DeviceInfoAndroid::CreateCapabilityMap(
    const char* deviceUniqueIdUTF8) {
  _captureCapabilities.clear();
  const AndroidCameraInfo* info = FindCameraInfoByName(deviceUniqueIdUTF8);
  if (info == NULL)
    return -1;

  for (size_t i = 0; i < info->resolutions.size(); ++i) {
    for (size_t j = 0; j < info->mfpsRanges.size(); ++j) {
      const IntPair& size = info->resolutions[i];
      const IntPair& mfpsRange = info->mfpsRanges[j];
      VideoCaptureCapability cap;
      cap.width = size.first;
      cap.height = size.second;
      cap.maxFPS = mfpsRange.second / 1000;
      cap.expectedCaptureDelay = kExpectedCaptureDelay;
      cap.rawType = kVideoNV21;
      _captureCapabilities.push_back(cap);
    }
  }
  return _captureCapabilities.size();
}

int32_t DeviceInfoAndroid::GetOrientation(
    const char* deviceUniqueIdUTF8,
    VideoCaptureRotation& orientation) {
  const AndroidCameraInfo* info = FindCameraInfoByName(deviceUniqueIdUTF8);
  if (info == NULL ||
      !VideoCaptureImpl::RotationFromDegrees(info->orientation, &orientation)) {
    return -1;
  }
  return 0;
}

void DeviceInfoAndroid::GetMFpsRange(const char* deviceUniqueIdUTF8,
                                     int max_fps_to_match,
                                     int* min_mfps, int* max_mfps) {
  const AndroidCameraInfo* info = FindCameraInfoByName(deviceUniqueIdUTF8);
  if (info == NULL)
    return;
  int desired_mfps = max_fps_to_match * 1000;
  int best_diff_mfps = 0;
  LOG(LS_INFO) << "Search for best target mfps " << desired_mfps;
  // Search for best fps range with preference shifted to constant fps modes.
  for (size_t i = 0; i < info->mfpsRanges.size(); ++i) {
    int diff_mfps = abs(info->mfpsRanges[i].first - desired_mfps) +
        abs(info->mfpsRanges[i].second - desired_mfps) +
        (info->mfpsRanges[i].second - info->mfpsRanges[i].first) / 2;
    LOG(LS_INFO) << "Fps range " << info->mfpsRanges[i].first << ":" <<
        info->mfpsRanges[i].second << ". Distance: " << diff_mfps;
    if (i == 0 || diff_mfps < best_diff_mfps) {
      best_diff_mfps = diff_mfps;
      *min_mfps = info->mfpsRanges[i].first;
      *max_mfps = info->mfpsRanges[i].second;
    }
  }
}

}  // namespace videocapturemodule
}  // namespace webrtc
<commit_msg>Getting orientation is not working properly. VideoCaptureImpl::RotationFromDegrees returns -1 in case fails not 0. So we need to change the if statement.<commit_after>/*
 *  Copyright (c) 2012 The WebRTC project authors. All Rights Reserved.
 *
 *  Use of this source code is governed by a BSD-style license
 *  that can be found in the LICENSE file in the root of the source
 *  tree. An additional intellectual property rights grant can be found
 *  in the file PATENTS.  All contributing project authors may
 *  be found in the AUTHORS file in the root of the source tree.
 */

#include "webrtc/modules/video_capture/android/device_info_android.h"

#include <algorithm>
#include <sstream>
#include <vector>

#include "json/json.h"
#include "third_party/icu/source/common/unicode/unistr.h"
#include "webrtc/modules/video_capture/android/video_capture_android.h"
#include "webrtc/system_wrappers/interface/logging.h"
#include "webrtc/system_wrappers/interface/ref_count.h"
#include "webrtc/system_wrappers/interface/trace.h"

namespace webrtc {

namespace videocapturemodule {

// Helper for storing lists of pairs of ints.  Used e.g. for resolutions & FPS
// ranges.
typedef std::pair<int, int> IntPair;
typedef std::vector<IntPair> IntPairs;

static std::string IntPairsToString(const IntPairs& pairs, char separator) {
  std::stringstream stream;
  for (size_t i = 0; i < pairs.size(); ++i) {
    if (i > 0)
      stream << ", ";
    stream << "(" << pairs[i].first << separator << pairs[i].second << ")";
  }
  return stream.str();
}

struct AndroidCameraInfo {
  std::string name;
  bool front_facing;
  int orientation;
  IntPairs resolutions;  // Pairs are: (width,height).
  // Pairs are (min,max) in units of FPS*1000 ("milli-frame-per-second").
  IntPairs mfpsRanges;

  std::string ToString() {
    std::stringstream stream;
    stream << "Name: [" << name << "], MFPS ranges: ["
           << IntPairsToString(mfpsRanges, ':')
           << "], front_facing: " << front_facing
           << ", orientation: " << orientation << ", resolutions: ["
           << IntPairsToString(resolutions, 'x') << "]";
    return stream.str();
  }
};

// Camera info; populated during DeviceInfoAndroid::Initialize() and immutable
// thereafter.
static std::vector<AndroidCameraInfo>* g_camera_info = NULL;

// Set |*index| to the index of |name| in g_camera_info or return false if no
// match found.
static bool FindCameraIndexByName(const std::string& name, size_t* index) {
  for (size_t i = 0; i < g_camera_info->size(); ++i) {
    if (g_camera_info->at(i).name == name) {
      *index = i;
      return true;
    }
  }
  return false;
}

// Returns a pointer to the named member of g_camera_info, or NULL if no match
// is found.
static AndroidCameraInfo* FindCameraInfoByName(const std::string& name) {
  size_t index = 0;
  if (FindCameraIndexByName(name, &index))
    return &g_camera_info->at(index);
  return NULL;
}

// static
void DeviceInfoAndroid::Initialize(JNIEnv* jni) {
  // TODO(henrike): this "if" would make a lot more sense as an assert, but
  // Java_org_webrtc_videoengineapp_ViEAndroidJavaAPI_GetVideoEngine() and
  // Java_org_webrtc_videoengineapp_ViEAndroidJavaAPI_Terminate() conspire to
  // prevent this.  Once that code is made to only
  // VideoEngine::SetAndroidObjects() once per process, this can turn into an
  // assert.
  if (g_camera_info)
    return;

  g_camera_info = new std::vector<AndroidCameraInfo>();
  jclass j_info_class =
      jni->FindClass("org/webrtc/videoengine/VideoCaptureDeviceInfoAndroid");
  assert(j_info_class);
  jmethodID j_initialize = jni->GetStaticMethodID(
      j_info_class, "getDeviceInfo", "()Ljava/lang/String;");
  jstring j_json_info = static_cast<jstring>(
      jni->CallStaticObjectMethod(j_info_class, j_initialize));

  const jchar* jchars = jni->GetStringChars(j_json_info, NULL);
  icu::UnicodeString ustr(jchars, jni->GetStringLength(j_json_info));
  jni->ReleaseStringChars(j_json_info, jchars);
  std::string json_info;
  ustr.toUTF8String(json_info);

  Json::Value cameras;
  Json::Reader reader(Json::Features::strictMode());
  bool parsed = reader.parse(json_info, cameras);
  if (!parsed) {
    std::stringstream stream;
    stream << "Failed to parse configuration:\n"
           << reader.getFormattedErrorMessages();
    assert(false);
    return;
  }
  for (Json::ArrayIndex i = 0; i < cameras.size(); ++i) {
    const Json::Value& camera = cameras[i];
    AndroidCameraInfo info;
    info.name = camera["name"].asString();
    info.front_facing = camera["front_facing"].asBool();
    info.orientation = camera["orientation"].asInt();
    Json::Value sizes = camera["sizes"];
    for (Json::ArrayIndex j = 0; j < sizes.size(); ++j) {
      const Json::Value& size = sizes[j];
      info.resolutions.push_back(std::make_pair(
          size["width"].asInt(), size["height"].asInt()));
    }
    Json::Value mfpsRanges = camera["mfpsRanges"];
    for (Json::ArrayIndex j = 0; j < mfpsRanges.size(); ++j) {
      const Json::Value& mfpsRange = mfpsRanges[j];
      info.mfpsRanges.push_back(std::make_pair(mfpsRange["min_mfps"].asInt(),
                                               mfpsRange["max_mfps"].asInt()));
    }
    g_camera_info->push_back(info);
  }
}

void DeviceInfoAndroid::DeInitialize() {
  if (g_camera_info) {
    delete g_camera_info;
    g_camera_info = NULL;
  }
}

VideoCaptureModule::DeviceInfo* VideoCaptureImpl::CreateDeviceInfo(
    const int32_t id) {
  return new videocapturemodule::DeviceInfoAndroid(id);
}

DeviceInfoAndroid::DeviceInfoAndroid(const int32_t id) :
    DeviceInfoImpl(id) {
}

DeviceInfoAndroid::~DeviceInfoAndroid() {
}

bool DeviceInfoAndroid::FindCameraIndex(const char* deviceUniqueIdUTF8,
                                        size_t* index) {
  return FindCameraIndexByName(deviceUniqueIdUTF8, index);
}

int32_t DeviceInfoAndroid::Init() {
  return 0;
}

uint32_t DeviceInfoAndroid::NumberOfDevices() {
  return g_camera_info->size();
}

int32_t DeviceInfoAndroid::GetDeviceName(
    uint32_t deviceNumber,
    char* deviceNameUTF8,
    uint32_t deviceNameLength,
    char* deviceUniqueIdUTF8,
    uint32_t deviceUniqueIdUTF8Length,
    char* /*productUniqueIdUTF8*/,
    uint32_t /*productUniqueIdUTF8Length*/) {
  if (deviceNumber >= g_camera_info->size())
    return -1;
  const AndroidCameraInfo& info = g_camera_info->at(deviceNumber);
  if (info.name.length() + 1 > deviceNameLength ||
      info.name.length() + 1 > deviceUniqueIdUTF8Length) {
    return -1;
  }
  memcpy(deviceNameUTF8, info.name.c_str(), info.name.length() + 1);
  memcpy(deviceUniqueIdUTF8, info.name.c_str(), info.name.length() + 1);
  return 0;
}

int32_t DeviceInfoAndroid::CreateCapabilityMap(
    const char* deviceUniqueIdUTF8) {
  _captureCapabilities.clear();
  const AndroidCameraInfo* info = FindCameraInfoByName(deviceUniqueIdUTF8);
  if (info == NULL)
    return -1;

  for (size_t i = 0; i < info->resolutions.size(); ++i) {
    for (size_t j = 0; j < info->mfpsRanges.size(); ++j) {
      const IntPair& size = info->resolutions[i];
      const IntPair& mfpsRange = info->mfpsRanges[j];
      VideoCaptureCapability cap;
      cap.width = size.first;
      cap.height = size.second;
      cap.maxFPS = mfpsRange.second / 1000;
      cap.expectedCaptureDelay = kExpectedCaptureDelay;
      cap.rawType = kVideoNV21;
      _captureCapabilities.push_back(cap);
    }
  }
  return _captureCapabilities.size();
}

int32_t DeviceInfoAndroid::GetOrientation(
    const char* deviceUniqueIdUTF8,
    VideoCaptureRotation& orientation) {
  const AndroidCameraInfo* info = FindCameraInfoByName(deviceUniqueIdUTF8);
  if (info == NULL ||
      VideoCaptureImpl::RotationFromDegrees(info->orientation,
                                            &orientation) != 0) {
    return -1;
  }
  return 0;
}

void DeviceInfoAndroid::GetMFpsRange(const char* deviceUniqueIdUTF8,
                                     int max_fps_to_match,
                                     int* min_mfps, int* max_mfps) {
  const AndroidCameraInfo* info = FindCameraInfoByName(deviceUniqueIdUTF8);
  if (info == NULL)
    return;
  int desired_mfps = max_fps_to_match * 1000;
  int best_diff_mfps = 0;
  LOG(LS_INFO) << "Search for best target mfps " << desired_mfps;
  // Search for best fps range with preference shifted to constant fps modes.
  for (size_t i = 0; i < info->mfpsRanges.size(); ++i) {
    int diff_mfps = abs(info->mfpsRanges[i].first - desired_mfps) +
        abs(info->mfpsRanges[i].second - desired_mfps) +
        (info->mfpsRanges[i].second - info->mfpsRanges[i].first) / 2;
    LOG(LS_INFO) << "Fps range " << info->mfpsRanges[i].first << ":" <<
        info->mfpsRanges[i].second << ". Distance: " << diff_mfps;
    if (i == 0 || diff_mfps < best_diff_mfps) {
      best_diff_mfps = diff_mfps;
      *min_mfps = info->mfpsRanges[i].first;
      *max_mfps = info->mfpsRanges[i].second;
    }
  }
}

}  // namespace videocapturemodule
}  // namespace webrtc
<|endoftext|>


<commit_before>/*****************************************************************************************
 *                                                                                *
 * OpenSpace                                                                             *
 *                                                                                *
 * Copyright (c) 2014-2020                                                               *
 *                                                                                *
 * Permission is hereby granted, free of charge, to any person obtaining a copy of this  *
 * software and associated documentation files (the "Software"), to deal in the Software *
 * without restriction, including without limitation the rights to use, copy, modify,    *
 * merge, publish, distribute, sublicense, and/or sell copies of the Software, and to    *
 * permit persons to whom the Software is furnished to do so, subject to the following   *
 * conditions:                                                                           *
 *                                                                                *
 * The above copyright notice and this permission notice shall be included in all copies *
 * or substantial portions of the Software.                                              *
 *                                                                                *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,   *
 * INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A         *
 * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT    *
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF  *
 * CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE  *
 * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                                         *
 ****************************************************************************************/

#include <modules/vislab/rendering/renderabledistancelabel.h>

#include <modules/base/rendering/renderablenodeline.h>
#include <openspace/documentation/documentation.h>
#include <openspace/documentation/verifier.h>
#include <openspace/engine/globals.h>
#include <openspace/rendering/renderengine.h>
#include <openspace/scene/scene.h>
#include <ghoul/logging/logmanager.h>
#include <string>

namespace {
    constexpr const char* _loggerCat = "RenderableDistanceLabel";

    constexpr openspace::properties::Property::PropertyInfo NodeLineInfo = {
        "NodeLine",
        "Node Line",
        "Property to track a nodeline. When tracking the label text will be updating the "
        "distance from the nodeline start and end."
    };

    constexpr openspace::properties::Property::PropertyInfo DistanceUnitInfo = {
        "DistanceUnit",
        "Distance Unit",
        "Property to define the unit in which the distance should be displayed."
        "Defaults to 'km' if not specified."
    };

    constexpr openspace::properties::Property::PropertyInfo CustomUnitDescriptorInfo = {
        "CustomUnitDescriptor",
        "Custom Unit Descriptor",
        "Property to define a custom unit descriptor to use to describe the distance value."
        "Defaults to the units SI descriptor if not specified."
    };
}

namespace openspace {

documentation::Documentation RenderableDistanceLabel::Documentation() {
    using namespace documentation;
    return {
        "Renderable Distance Label",
        "vislab_renderable_distance_label",
        {
            {
                NodeLineInfo.identifier,
                new StringVerifier,
                Optional::No,
                NodeLineInfo.description
            },
            {
                DistanceUnitInfo.identifier,
                new IntVerifier,
                Optional::Yes,
                DistanceUnitInfo.description
            },
            {
                CustomUnitDescriptorInfo.identifier,
                new StringVerifier,
                Optional::Yes,
                CustomUnitDescriptorInfo.description
            }
        }
    };
}

RenderableDistanceLabel::RenderableDistanceLabel(const ghoul::Dictionary& dictionary)
    : RenderableLabels(dictionary)
    , _nodelineId(NodeLineInfo)
    , _distanceUnit(DistanceUnitInfo, 1, 0, 11)
    , _customUnitDescriptor(CustomUnitDescriptorInfo)
{
    documentation::testSpecificationAndThrow(
        Documentation(),
        dictionary,
        "RenderableDistanceLabel"
    );

    if (dictionary.hasKey(NodeLineInfo.identifier)) {
        _nodelineId = dictionary.value<std::string>(NodeLineInfo.identifier);
        addProperty(_nodelineId);
    }
    if (dictionary.hasKey(DistanceUnitInfo.identifier)) {
        _distanceUnit = static_cast<int>(
            dictionary.value<double>(DistanceUnitInfo.identifier)
            );
        addProperty(_distanceUnit);
    }
    if (dictionary.hasKey(CustomUnitDescriptorInfo.identifier)) {
        _customUnitDescriptor = dictionary.value<std::string>(CustomUnitDescriptorInfo.identifier);
        addProperty(_customUnitDescriptor);
    }
}

void RenderableDistanceLabel::update(const UpdateData&) {
    if (_errorThrown) {
        return;
    }

    RenderEngine& RE = global::renderEngine;

    SceneGraphNode* nodelineNode = RE.scene()->sceneGraphNode(_nodelineId);
    if (nodelineNode) {
        RenderableNodeLine* nodeline = dynamic_cast<RenderableNodeLine*>(
            nodelineNode->renderable()
        );
        if (!nodeline) {
            LERROR("Expected renderable to be of type 'RenderableNodeLine'");
            _errorThrown = true;
            return;
        }

        // Get used unit scale
        const float scale = unit(_distanceUnit);

        // Get unit descriptor text
        std::string unitDescriptor = toString(_distanceUnit);
        if (!_customUnitDescriptor.value().empty()) {
            unitDescriptor = _customUnitDescriptor.value();
        }

        // Get distance as string and remove fractional part
        std::string distanceText = std::to_string(std::round(nodeline->distance() / scale));
        int pos = static_cast<int>(distanceText.find("."));
        std::string subStr = distanceText.substr(pos);
        distanceText.erase(pos, subStr.size());

        // Create final label text and set it
        const std::string finalText = distanceText + " " + unitDescriptor;
        setLabelText(finalText);

        // Update placement of label with transformation matrix
        SceneGraphNode* startNode = RE.scene()->sceneGraphNode(nodeline->start());
        glm::dvec3 start = startNode->worldPosition();
        SceneGraphNode* endNode = RE.scene()->sceneGraphNode(nodeline->end());
        glm::dvec3 end = endNode->worldPosition();
        glm::dvec3 goalPos = start + (end - start) / 2.0;
        _transformationMatrix = glm::translate(glm::dmat4(1.0), goalPos);
    }
    else {
        LERROR(fmt::format("There is no scenegraph node with id {}", _nodelineId));
        _errorThrown = true;
    }
}

} // namespace openspace

<commit_msg>style change<commit_after>/*****************************************************************************************
 *                                                                                *
 * OpenSpace                                                                             *
 *                                                                                *
 * Copyright (c) 2014-2020                                                               *
 *                                                                                *
 * Permission is hereby granted, free of charge, to any person obtaining a copy of this  *
 * software and associated documentation files (the "Software"), to deal in the Software *
 * without restriction, including without limitation the rights to use, copy, modify,    *
 * merge, publish, distribute, sublicense, and/or sell copies of the Software, and to    *
 * permit persons to whom the Software is furnished to do so, subject to the following   *
 * conditions:                                                                           *
 *                                                                                *
 * The above copyright notice and this permission notice shall be included in all copies *
 * or substantial portions of the Software.                                              *
 *                                                                                *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,   *
 * INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A         *
 * PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT    *
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF  *
 * CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE  *
 * OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                                         *
 ****************************************************************************************/

#include <modules/vislab/rendering/renderabledistancelabel.h>

#include <modules/base/rendering/renderablenodeline.h>
#include <openspace/documentation/documentation.h>
#include <openspace/documentation/verifier.h>
#include <openspace/engine/globals.h>
#include <openspace/rendering/renderengine.h>
#include <openspace/scene/scene.h>
#include <ghoul/logging/logmanager.h>
#include <string>

namespace {
    constexpr const char* _loggerCat = "RenderableDistanceLabel";

    constexpr openspace::properties::Property::PropertyInfo NodeLineInfo = {
        "NodeLine",
        "Node Line",
        "Property to track a nodeline. When tracking the label text will be updating the "
        "distance from the nodeline start and end."
    };

    constexpr openspace::properties::Property::PropertyInfo DistanceUnitInfo = {
        "DistanceUnit",
        "Distance Unit",
        "Property to define the unit in which the distance should be displayed."
        "Defaults to 'km' if not specified."
    };

    constexpr openspace::properties::Property::PropertyInfo CustomUnitDescriptorInfo = {
        "CustomUnitDescriptor",
        "Custom Unit Descriptor",
        "Property to define a custom unit descriptor to use to describe the distance value."
        "Defaults to the units SI descriptor if not specified."
    };
}

namespace openspace {

documentation::Documentation RenderableDistanceLabel::Documentation() {
    using namespace documentation;
    return {
        "Renderable Distance Label",
        "vislab_renderable_distance_label",
        {
            {
                NodeLineInfo.identifier,
                new StringVerifier,
                Optional::No,
                NodeLineInfo.description
            },
            {
                DistanceUnitInfo.identifier,
                new IntVerifier,
                Optional::Yes,
                DistanceUnitInfo.description
            },
            {
                CustomUnitDescriptorInfo.identifier,
                new StringVerifier,
                Optional::Yes,
                CustomUnitDescriptorInfo.description
            }
        }
    };
}

RenderableDistanceLabel::RenderableDistanceLabel(const ghoul::Dictionary& dictionary)
    : RenderableLabels(dictionary)
    , _nodelineId(NodeLineInfo)
    , _distanceUnit(DistanceUnitInfo, 1, 0, 11)
    , _customUnitDescriptor(CustomUnitDescriptorInfo)
{
    documentation::testSpecificationAndThrow(
        Documentation(),
        dictionary,
        "RenderableDistanceLabel"
    );

    if (dictionary.hasKey(NodeLineInfo.identifier)) {
        _nodelineId = dictionary.value<std::string>(NodeLineInfo.identifier);
        addProperty(_nodelineId);
    }
    if (dictionary.hasKey(DistanceUnitInfo.identifier)) {
        _distanceUnit = static_cast<int>(
            dictionary.value<double>(DistanceUnitInfo.identifier)
        );
        addProperty(_distanceUnit);
    }
    if (dictionary.hasKey(CustomUnitDescriptorInfo.identifier)) {
        _customUnitDescriptor = dictionary.value<std::string>(CustomUnitDescriptorInfo.identifier);
        addProperty(_customUnitDescriptor);
    }
}

void RenderableDistanceLabel::update(const UpdateData&) {
    if (_errorThrown) {
        return;
    }

    RenderEngine& RE = global::renderEngine;

    SceneGraphNode* nodelineNode = RE.scene()->sceneGraphNode(_nodelineId);
    if (nodelineNode) {
        RenderableNodeLine* nodeline = dynamic_cast<RenderableNodeLine*>(
            nodelineNode->renderable()
        );
        if (!nodeline) {
            LERROR("Expected renderable to be of type 'RenderableNodeLine'");
            _errorThrown = true;
            return;
        }

        // Get used unit scale
        const float scale = unit(_distanceUnit);

        // Get unit descriptor text
        std::string unitDescriptor = toString(_distanceUnit);
        if (!_customUnitDescriptor.value().empty()) {
            unitDescriptor = _customUnitDescriptor.value();
        }

        // Get distance as string and remove fractional part
        std::string distanceText = std::to_string(std::round(nodeline->distance() / scale));
        int pos = static_cast<int>(distanceText.find("."));
        std::string subStr = distanceText.substr(pos);
        distanceText.erase(pos, subStr.size());

        // Create final label text and set it
        const std::string finalText = distanceText + " " + unitDescriptor;
        setLabelText(finalText);

        // Update placement of label with transformation matrix
        SceneGraphNode* startNode = RE.scene()->sceneGraphNode(nodeline->start());
        glm::dvec3 start = startNode->worldPosition();
        SceneGraphNode* endNode = RE.scene()->sceneGraphNode(nodeline->end());
        glm::dvec3 end = endNode->worldPosition();
        glm::dvec3 goalPos = start + (end - start) / 2.0;
        _transformationMatrix = glm::translate(glm::dmat4(1.0), goalPos);
    }
    else {
        LERROR(fmt::format("There is no scenegraph node with id {}", _nodelineId));
        _errorThrown = true;
    }
}

} // namespace openspace

<|endoftext|>


<commit_before>#include "complex.hpp"
#include <iostream>

using namespace std;

Complex Complex::add(const Complex b) const
{
	return Complex(rel + b.rel, img + b.img);
}
Complex Complex::sub(const Complex b) const
{
	return Complex(rel - b.rel, img - b.img);
}
Complex Complex::mult(const int i) const
{
	return Complex(rel * i , img * i);
}
Complex Complex::divd(const int j) const
{
	return Complex(rel / j, img / j);
}

Complex::Complex()
{
	rel = 0;
	img = 0;
}

Complex::Complex(double real, double imag)
{
	rel = real;
	img = imag;
}

double Complex::real()
{
	return rel;
}

double Complex::imag()
{
	return img;
}

Complex::Complex(const Complex& cc)
{
	rel = cc.rel;
	img = cc.img;
}

Complex Complex::operator*(Complex &c2)
{
	Complex res;
	res.rel = ((rel * c2.rel)-(img*c2.img));
	res.img = ((img * c2.rel)+(rel*c2.img));
	return res;
}

Complex Complex::operator/(Complex &c2)
{
	Complex res;
	res.rel = ((rel * c2.rel)+(img*c2.img))/((c2.rel*c2.rel)+(c2.img*c2.img));
	res.img = ((img * c2.rel)-(rel*c2.img))/((c2.rel*c2.rel)+(c2.img*c2.img));
	return res;
}

bool Complex::operator==(Complex &cc)
{
	bool r=false;
	if ((rel == cc.rel) && (img == cc.img))
		r=true;
	return r;
}

Complex Complex::operator=(Complex&cc)
{
	rel = cc.rel;
	img = cc.img;
	return *this;
}

Complex Complex::operator+=(Complex &c2)
{
	rel += c2.rel;
	img += c2.imag;
	return *this;
}

Complex Complex::operator-=(Complex &c2)
{
	rel -= c2.rel;
	img -= c2.img;
	return *this;
}

Complex Complex::operator*=(Complex &c2)
{
 Complexres;
	res.rel = ((rel * c2.rel)-(img*c2.img));
	res.img = ((img * c2.rel)+(rel*c2.img));
  rel=res.rel;
  img=res.img;
	return *this;
}

Complex Complex::operator/=(Complex &copy)
{
Complex res;
	res.rel = ((rel * c2.rel)+(img*c2.img))/((c2.rel*c2.rel)+(c2.img*c2.img));
	res.img = ((img * c2.rel)-(rel*c2.img))/((c2.rel*c2.rel)+(c2.img*c2.img));
  rel=res.rel;
  img=res.img;
	return *this
}

ostream& operator <<(ostream& res, Complex& c2)
{
		res << c2.rel << " + " << c2.img << "*i" << endl;
}

<commit_msg>Update complex.cpp<commit_after>#include "complex.hpp"
#include <iostream>

using namespace std;

Complex Complex::add(const Complex b) const
{
	return Complex(rel + b.rel, img + b.img);
}
Complex Complex::sub(const Complex b) const
{
	return Complex(rel - b.rel, img - b.img);
}
Complex Complex::mult(const int i) const
{
	return Complex(rel * i , img * i);
}
Complex Complex::divd(const int j) const
{
	return Complex(rel / j, img / j);
}

Complex::Complex()
{
	rel = 0;
	img = 0;
}

Complex::Complex(double real, double imag)
{
	rel = real;
	img = imag;
}

double Complex::real()
{
	return rel;
}

double Complex::imag()
{
	return img;
}

Complex::Complex(const Complex& cc)
{
	rel = cc.rel;
	img = cc.img;
}

Complex Complex::operator*(Complex &c2)
{
	Complex res;
	res.rel = ((rel * c2.rel)-(img*c2.img));
	res.img = ((img * c2.rel)+(rel*c2.img));
	return res;
}

Complex Complex::operator/(Complex &c2)
{
	Complex res;
	res.rel = ((rel * c2.rel)+(img*c2.img))/((c2.rel*c2.rel)+(c2.img*c2.img));
	res.img = ((img * c2.rel)-(rel*c2.img))/((c2.rel*c2.rel)+(c2.img*c2.img));
	return res;
}

bool Complex::operator==(Complex &cc)
{
	bool r=true;
	if ((rel != cc.rel) || (img != cc.img))
		r=false;
	return r;
}

Complex Complex::operator=(Complex&cc)
{
	rel = cc.rel;
	img = cc.img;
	return *this;
}

Complex Complex::operator+=(Complex &c2)
{
	rel += c2.rel;
	img += c2.imag;
	return *this;
}

Complex Complex::operator-=(Complex &c2)
{
	rel -= c2.rel;
	img -= c2.img;
	return *this;
}

Complex Complex::operator*=(Complex &c2)
{
 Complexres;
	res.rel = ((rel * c2.rel)-(img*c2.img));
	res.img = ((img * c2.rel)+(rel*c2.img));
  rel=res.rel;
  img=res.img;
	return *this;
}

Complex Complex::operator/=(Complex &copy)
{
Complex res;
	res.rel = ((rel * c2.rel)+(img*c2.img))/((c2.rel*c2.rel)+(c2.img*c2.img));
	res.img = ((img * c2.rel)-(rel*c2.img))/((c2.rel*c2.rel)+(c2.img*c2.img));
  rel=res.rel;
  img=res.img;
	return *this
}

ostream& operator <<(ostream& res, Complex& c2)
{
		res << c2.rel << " + " << c2.img << "*i" << endl;
}

<|endoftext|>