<commit_before>// @(#)root/base:$Id$
// Author: Fons Rademakers   29/07/95

/*************************************************************************
 * Copyright (C) 1995-2000, Rene Brun and Fons Rademakers.               *
 * All rights reserved.                                                  *
 *                                                                       *
 * For the licensing terms see $ROOTSYS/LICENSE.                         *
 * For the list of contributors see $ROOTSYS/README/CREDITS.             *
 *************************************************************************/

//////////////////////////////////////////////////////////////////////////
//                                                                      //
// Error handling routines.                                             //
//                                                                      //
// This file defines a number of global error handling routines:        //
// Warning(), Error(), SysError() and Fatal(). They all take a          //
// location string (where the error happened) and a printf style format //
// string plus vararg's. In the end these functions call an             //
// errorhandler function. By default DefaultErrorHandler() is used.     //
//                                                                      //
//////////////////////////////////////////////////////////////////////////

#ifdef WIN32
#include <windows.h>
#endif

#include <stdio.h>
#include <stdlib.h>
#include "snprintf.h"
#include "Varargs.h"
#include "Riostream.h"
#include "TError.h"
#include "TSystem.h"
#include "TString.h"
#include "TEnv.h"
#include "TVirtualMutex.h"

// Mutex for error and error format protection
// (exported to be used for similar cases in other classes)

TVirtualMutex *gErrorMutex = 0;

Int_t  gErrorIgnoreLevel     = kUnset;
Int_t  gErrorAbortLevel      = kSysError+1;
Bool_t gPrintViaErrorHandler = kFALSE;

const char *kAssertMsg = "%s violated at line %d of `%s'";
const char *kCheckMsg  = "%s not true at line %d of `%s'";

// Integrate with crash reporter.
#ifdef __APPLE__
extern "C" const char *__crashreporter_info__;
const char *__crashreporter_info__ = 0;
#endif

static ErrorHandlerFunc_t gErrorHandler = DefaultErrorHandler;

//______________________________________________________________________________
static void DebugPrint(const char *fmt, ...)
{
   // Print debugging message to stderr and, on Windows, to the system debugger.

   static Int_t buf_size = 2048;
   static char *buf = 0;

   R__LOCKGUARD2(gErrorMutex);

   va_list ap;
   va_start(ap, fmt);

again:
   if (!buf)
      buf = new char[buf_size];

   Int_t n = vsnprintf(buf, buf_size, fmt, ap);
   // old vsnprintf's return -1 if string is truncated new ones return
   // total number of characters that would have been written
   if (n == -1 || n >= buf_size) {
      if (n == -1)
         buf_size *= 2;
      else
         buf_size = n+1;
      delete [] buf;
      buf = 0;
      va_end(ap);
      va_start(ap, fmt);
      goto again;
   }
   va_end(ap);

   fprintf(stderr, "%s", buf);

#ifdef WIN32
   ::OutputDebugString(buf);
#endif
}

//______________________________________________________________________________
ErrorHandlerFunc_t SetErrorHandler(ErrorHandlerFunc_t newhandler)
{
   // Set an errorhandler function. Returns the old handler.

   ErrorHandlerFunc_t oldhandler = gErrorHandler;
   gErrorHandler = newhandler;
   return oldhandler;
}

//______________________________________________________________________________
ErrorHandlerFunc_t GetErrorHandler()
{
   // Returns the current error handler function.

   return gErrorHandler;
}

//______________________________________________________________________________
void DefaultErrorHandler(Int_t level, Bool_t abort_bool, const char *location, const char *msg)
{
   // The default error handler function. It prints the message on stderr and
   // if abort is set it aborts the application.

   if (gErrorIgnoreLevel == kUnset) {
      R__LOCKGUARD2(gErrorMutex);

      gErrorIgnoreLevel = 0;
      if (gEnv) {
         TString slevel = gEnv->GetValue("Root.ErrorIgnoreLevel", "Print");
         if (!slevel.CompareTo("Print", TString::kIgnoreCase))
            gErrorIgnoreLevel = kPrint;
         else if (!slevel.CompareTo("Info", TString::kIgnoreCase))
            gErrorIgnoreLevel = kInfo;
         else if (!slevel.CompareTo("Warning", TString::kIgnoreCase))
            gErrorIgnoreLevel = kWarning;
         else if (!slevel.CompareTo("Error", TString::kIgnoreCase))
            gErrorIgnoreLevel = kError;
         else if (!slevel.CompareTo("Break", TString::kIgnoreCase))
            gErrorIgnoreLevel = kBreak;
         else if (!slevel.CompareTo("SysError", TString::kIgnoreCase))
            gErrorIgnoreLevel = kSysError;
         else if (!slevel.CompareTo("Fatal", TString::kIgnoreCase))
            gErrorIgnoreLevel = kFatal;
      }
   }

   if (level < gErrorIgnoreLevel)
      return;

   const char *type = 0;

   if (level >= kInfo)
      type = "Info";
   if (level >= kWarning)
      type = "Warning";
   if (level >= kError)
      type = "Error";
   if (level >= kBreak)
      type = "\n *** Break ***";
   if (level >= kSysError)
      type = "SysError";
   if (level >= kFatal)
      type = "Fatal";

   TString smsg;
   if (level >= kPrint && level < kInfo)
      smsg.Form("%s", msg);
   else if (level >= kBreak && level < kSysError)
      smsg.Form("%s %s", type, msg);
   else if (!location || strlen(location) == 0)
      smsg.Form("%s: %s", type, msg);
   else
      smsg.Form("%s in <%s>: %s", type, location, msg);

   DebugPrint("%s\n", smsg.Data());

#ifdef __APPLE__
   if (__crashreporter_info__)
      delete [] __crashreporter_info__;
   __crashreporter_info__ = StrDup(smsg);
#endif

   fflush(stderr);
   if (abort_bool) {
      DebugPrint("aborting\n");
      fflush(stderr);
      if (gSystem) {
         gSystem->StackTrace();
         gSystem->Abort();
      } else
         abort();
   }
}

//______________________________________________________________________________
void ErrorHandler(Int_t level, const char *location, const char *fmt, va_list ap)
{
   // General error handler function. It calls the user set error handler.

   R__LOCKGUARD2(gErrorMutex);

   static Int_t buf_size = 2048;
   static char *buf = 0;

   int vc = 0;
   va_list sap;
   R__VA_COPY(sap, ap);

again:
   if (!buf)
      buf = new char[buf_size];

   if (!fmt)
      fmt = "no error message provided";

   Int_t n = vsnprintf(buf, buf_size, fmt, ap);
   // old vsnprintf's return -1 if string is truncated new ones return
   // total number of characters that would have been written
   if (n == -1 || n >= buf_size) {
      if (n == -1)
         buf_size *= 2;
      else
         buf_size = n+1;
      delete [] buf;
      buf = 0;
      va_end(ap);
      R__VA_COPY(ap, sap);
      vc = 1;
      goto again;
   }
   va_end(sap);
   if (vc)
      va_end(ap);

   char *bp;
   if (level >= kSysError && level < kFatal)
      bp = Form("%s (%s)", buf, gSystem->GetError());
   else
      bp = buf;

   if (level != kFatal)
      gErrorHandler(level, level >= gErrorAbortLevel, location, bp);
   else
      gErrorHandler(level, kTRUE, location, bp);
}

//______________________________________________________________________________
void AbstractMethod(const char *method)
{
   // This function can be used in abstract base classes in case one does
   // not want to make the class a "real" (in C++ sense) ABC. If this
   // function is called it will warn the user that the function should
   // have been overridden.

   Warning(method, "this method must be overridden!");
}

//______________________________________________________________________________
void MayNotUse(const char *method)
{
   // This function can be used in classes that should override a certain
   // function, but in the inherited class the function makes no sense.

   Warning(method, "may not use this method");
}

//______________________________________________________________________________
void Obsolete(const char *function, const char *asOfVers, const char *removedFromVers)
{
   // Use this function to declare a function obsolete. Specify as of which version
   // the method is obsolete and as from which version it will be removed.

   TString mess;
   mess.Form("obsolete as of %s and will be removed from %s", asOfVers, removedFromVers);
   Warning(function, "%s", mess.Data());
}

//______________________________________________________________________________
void Error(const char *location, const char *va_(fmt), ...)
{
   // Use this function in case an error occured.

   va_list ap;
   va_start(ap,va_(fmt));
   ErrorHandler(kError, location, va_(fmt), ap);
   va_end(ap);
}

//______________________________________________________________________________
void SysError(const char *location, const char *va_(fmt), ...)
{
   // Use this function in case a system (OS or GUI) related error occured.

   va_list ap;
   va_start(ap, va_(fmt));
   ErrorHandler(kSysError, location, va_(fmt), ap);
   va_end(ap);
}

//______________________________________________________________________________
void Break(const char *location, const char *va_(fmt), ...)
{
   // Use this function in case an error occured.

   va_list ap;
   va_start(ap,va_(fmt));
   ErrorHandler(kBreak, location, va_(fmt), ap);
   va_end(ap);
}

//______________________________________________________________________________
void Info(const char *location, const char *va_(fmt), ...)
{
   // Use this function for informational messages.

   va_list ap;
   va_start(ap,va_(fmt));
   ErrorHandler(kInfo, location, va_(fmt), ap);
   va_end(ap);
}

//______________________________________________________________________________
void Warning(const char *location, const char *va_(fmt), ...)
{
   // Use this function in warning situations.

   va_list ap;
   va_start(ap,va_(fmt));
   ErrorHandler(kWarning, location, va_(fmt), ap);
   va_end(ap);
}

//______________________________________________________________________________
void Fatal(const char *location, const char *va_(fmt), ...)
{
   // Use this function in case of a fatal error. It will abort the program.

   va_list ap;
   va_start(ap,va_(fmt));
   ErrorHandler(kFatal, location, va_(fmt), ap);
   va_end(ap);
}
<commit_msg>Fix thread-safety issues with TError<commit_after>// @(#)root/base:$Id$
// Author: Fons Rademakers   29/07/95

/*************************************************************************
 * Copyright (C) 1995-2000, Rene Brun and Fons Rademakers.               *
 * All rights reserved.                                                  *
 *                                                                       *
 * For the licensing terms see $ROOTSYS/LICENSE.                         *
 * For the list of contributors see $ROOTSYS/README/CREDITS.             *
 *************************************************************************/

//////////////////////////////////////////////////////////////////////////
//                                                                      //
// Error handling routines.                                             //
//                                                                      //
// This file defines a number of global error handling routines:        //
// Warning(), Error(), SysError() and Fatal(). They all take a          //
// location string (where the error happened) and a printf style format //
// string plus vararg's. In the end these functions call an             //
// errorhandler function. By default DefaultErrorHandler() is used.     //
//                                                                      //
//////////////////////////////////////////////////////////////////////////

#ifdef WIN32
#include <windows.h>
#endif

#include <stdio.h>
#include <stdlib.h>
#include "snprintf.h"
#include "Varargs.h"
#include "Riostream.h"
#include "TError.h"
#include "TSystem.h"
#include "TString.h"
#include "TEnv.h"
#include "TVirtualMutex.h"

// Mutex for error and error format protection
// (exported to be used for similar cases in other classes)

TVirtualMutex *gErrorMutex = 0;

Int_t  gErrorIgnoreLevel     = kUnset;
Int_t  gErrorAbortLevel      = kSysError+1;
Bool_t gPrintViaErrorHandler = kFALSE;

const char *kAssertMsg = "%s violated at line %d of `%s'";
const char *kCheckMsg  = "%s not true at line %d of `%s'";

// Integrate with crash reporter.
#ifdef __APPLE__
extern "C" const char *__crashreporter_info__;
const char *__crashreporter_info__ = 0;
#endif

static ErrorHandlerFunc_t gErrorHandler = DefaultErrorHandler;

//______________________________________________________________________________
static void DebugPrint(const char *fmt, ...)
{
   // Print debugging message to stderr and, on Windows, to the system debugger.

   static thread_local Int_t buf_size = 2048;
   static thread_local char *buf = 0;

   va_list ap;
   va_start(ap, fmt);

again:
   if (!buf)
      buf = new char[buf_size];

   Int_t n = vsnprintf(buf, buf_size, fmt, ap);
   // old vsnprintf's return -1 if string is truncated new ones return
   // total number of characters that would have been written
   if (n == -1 || n >= buf_size) {
      if (n == -1)
         buf_size *= 2;
      else
         buf_size = n+1;
      delete [] buf;
      buf = 0;
      va_end(ap);
      va_start(ap, fmt);
      goto again;
   }
   va_end(ap);

   R__LOCKGUARD2(gErrorMutex);

   fprintf(stderr, "%s", buf);

#ifdef WIN32
   ::OutputDebugString(buf);
#endif
}

//______________________________________________________________________________
ErrorHandlerFunc_t SetErrorHandler(ErrorHandlerFunc_t newhandler)
{
   // Set an errorhandler function. Returns the old handler.

   ErrorHandlerFunc_t oldhandler = gErrorHandler;
   gErrorHandler = newhandler;
   return oldhandler;
}

//______________________________________________________________________________
ErrorHandlerFunc_t GetErrorHandler()
{
   // Returns the current error handler function.

   return gErrorHandler;
}

//______________________________________________________________________________
void DefaultErrorHandler(Int_t level, Bool_t abort_bool, const char *location, const char *msg)
{
   // The default error handler function. It prints the message on stderr and
   // if abort is set it aborts the application.

   if (gErrorIgnoreLevel == kUnset) {
      R__LOCKGUARD2(gErrorMutex);

      gErrorIgnoreLevel = 0;
      if (gEnv) {
         TString slevel = gEnv->GetValue("Root.ErrorIgnoreLevel", "Print");
         if (!slevel.CompareTo("Print", TString::kIgnoreCase))
            gErrorIgnoreLevel = kPrint;
         else if (!slevel.CompareTo("Info", TString::kIgnoreCase))
            gErrorIgnoreLevel = kInfo;
         else if (!slevel.CompareTo("Warning", TString::kIgnoreCase))
            gErrorIgnoreLevel = kWarning;
         else if (!slevel.CompareTo("Error", TString::kIgnoreCase))
            gErrorIgnoreLevel = kError;
         else if (!slevel.CompareTo("Break", TString::kIgnoreCase))
            gErrorIgnoreLevel = kBreak;
         else if (!slevel.CompareTo("SysError", TString::kIgnoreCase))
            gErrorIgnoreLevel = kSysError;
         else if (!slevel.CompareTo("Fatal", TString::kIgnoreCase))
            gErrorIgnoreLevel = kFatal;
      }
   }

   if (level < gErrorIgnoreLevel)
      return;

   const char *type = 0;

   if (level >= kInfo)
      type = "Info";
   if (level >= kWarning)
      type = "Warning";
   if (level >= kError)
      type = "Error";
   if (level >= kBreak)
      type = "\n *** Break ***";
   if (level >= kSysError)
      type = "SysError";
   if (level >= kFatal)
      type = "Fatal";

   TString smsg;
   if (level >= kPrint && level < kInfo)
      smsg.Form("%s", msg);
   else if (level >= kBreak && level < kSysError)
      smsg.Form("%s %s", type, msg);
   else if (!location || strlen(location) == 0)
      smsg.Form("%s: %s", type, msg);
   else
      smsg.Form("%s in <%s>: %s", type, location, msg);

   DebugPrint("%s\n", smsg.Data());

#ifdef __APPLE__
   if (__crashreporter_info__)
      delete [] __crashreporter_info__;
   __crashreporter_info__ = StrDup(smsg);
#endif

   fflush(stderr);
   if (abort_bool) {
      DebugPrint("aborting\n");
      fflush(stderr);
      if (gSystem) {
         gSystem->StackTrace();
         gSystem->Abort();
      } else
         abort();
   }
}

//______________________________________________________________________________
void ErrorHandler(Int_t level, const char *location, const char *fmt, va_list ap)
{
   // General error handler function. It calls the user set error handler.

   static thread_local Int_t buf_size = 2048;
   static thread_local char *buf = 0;

   int vc = 0;
   va_list sap;
   R__VA_COPY(sap, ap);

again:
   if (!buf)
      buf = new char[buf_size];

   if (!fmt)
      fmt = "no error message provided";

   Int_t n = vsnprintf(buf, buf_size, fmt, ap);
   // old vsnprintf's return -1 if string is truncated new ones return
   // total number of characters that would have been written
   if (n == -1 || n >= buf_size) {
      if (n == -1)
         buf_size *= 2;
      else
         buf_size = n+1;
      delete [] buf;
      buf = 0;
      va_end(ap);
      R__VA_COPY(ap, sap);
      vc = 1;
      goto again;
   }
   va_end(sap);
   if (vc)
      va_end(ap);

   char *bp;
   if (level >= kSysError && level < kFatal) {
      R__LOCKGUARD2(gErrorMutex);
      bp = Form("%s (%s)", buf, gSystem->GetError());
   } else
      bp = buf;

   if (level != kFatal)
      gErrorHandler(level, level >= gErrorAbortLevel, location, bp);
   else
      gErrorHandler(level, kTRUE, location, bp);
}

//______________________________________________________________________________
void AbstractMethod(const char *method)
{
   // This function can be used in abstract base classes in case one does
   // not want to make the class a "real" (in C++ sense) ABC. If this
   // function is called it will warn the user that the function should
   // have been overridden.

   Warning(method, "this method must be overridden!");
}

//______________________________________________________________________________
void MayNotUse(const char *method)
{
   // This function can be used in classes that should override a certain
   // function, but in the inherited class the function makes no sense.

   Warning(method, "may not use this method");
}

//______________________________________________________________________________
void Obsolete(const char *function, const char *asOfVers, const char *removedFromVers)
{
   // Use this function to declare a function obsolete. Specify as of which version
   // the method is obsolete and as from which version it will be removed.

   TString mess;
   mess.Form("obsolete as of %s and will be removed from %s", asOfVers, removedFromVers);
   Warning(function, "%s", mess.Data());
}

//______________________________________________________________________________
void Error(const char *location, const char *va_(fmt), ...)
{
   // Use this function in case an error occured.

   va_list ap;
   va_start(ap,va_(fmt));
   ErrorHandler(kError, location, va_(fmt), ap);
   va_end(ap);
}

//______________________________________________________________________________
void SysError(const char *location, const char *va_(fmt), ...)
{
   // Use this function in case a system (OS or GUI) related error occured.

   va_list ap;
   va_start(ap, va_(fmt));
   ErrorHandler(kSysError, location, va_(fmt), ap);
   va_end(ap);
}

//______________________________________________________________________________
void Break(const char *location, const char *va_(fmt), ...)
{
   // Use this function in case an error occured.

   va_list ap;
   va_start(ap,va_(fmt));
   ErrorHandler(kBreak, location, va_(fmt), ap);
   va_end(ap);
}

//______________________________________________________________________________
void Info(const char *location, const char *va_(fmt), ...)
{
   // Use this function for informational messages.

   va_list ap;
   va_start(ap,va_(fmt));
   ErrorHandler(kInfo, location, va_(fmt), ap);
   va_end(ap);
}

//______________________________________________________________________________
void Warning(const char *location, const char *va_(fmt), ...)
{
   // Use this function in warning situations.

   va_list ap;
   va_start(ap,va_(fmt));
   ErrorHandler(kWarning, location, va_(fmt), ap);
   va_end(ap);
}

//______________________________________________________________________________
void Fatal(const char *location, const char *va_(fmt), ...)
{
   // Use this function in case of a fatal error. It will abort the program.

   va_list ap;
   va_start(ap,va_(fmt));
   ErrorHandler(kFatal, location, va_(fmt), ap);
   va_end(ap);
}
<|endoftext|>


<commit_before>/*
 * Copyright (C) 2017 Google Inc.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "core/cc/thread.h"

#include <cstdint>
#include <pthread.h>

namespace core {

AsyncJob::AsyncJob(const std::function<void()>& function) : mFunction(function) {
    _ = malloc(sizeof(pthread_t));
    pthread_t* thread = reinterpret_cast<pthread_t*>(_);

    pthread_create(thread, nullptr, &AsyncJob::RunJob, this);
}

AsyncJob::~AsyncJob() {
    pthread_t* thread = reinterpret_cast<pthread_t*>(_);
    pthread_join(*thread, nullptr);
}

}  // namespace core

<commit_msg>Fix OSX build. (#167)<commit_after>/*
 * Copyright (C) 2017 Google Inc.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "core/cc/thread.h"

#include <cstdint>
#include <cstdlib>
#include <pthread.h>

namespace core {

AsyncJob::AsyncJob(const std::function<void()>& function) : mFunction(function) {
    _ = malloc(sizeof(pthread_t));
    pthread_t* thread = reinterpret_cast<pthread_t*>(_);

    pthread_create(thread, nullptr, &AsyncJob::RunJob, this);
}

AsyncJob::~AsyncJob() {
    pthread_t* thread = reinterpret_cast<pthread_t*>(_);
    pthread_join(*thread, nullptr);
}

}  // namespace core

<|endoftext|>


<commit_before>
// Copyright (c) 2015 Pierre MOULON.

// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.

#ifndef OPENMVG_SFM_DATA_FILTERS_HPP
#define OPENMVG_SFM_DATA_FILTERS_HPP

#include "openMVG/stl/stl.hpp"
#include <iterator>

namespace openMVG {
namespace sfm {

/// List the view indexes that have valid camera intrinsic and pose.
static std::set<IndexT> Get_Valid_Views
(
  const SfM_Data & sfm_data
)
{
  std::set<IndexT> valid_idx;
  for (Views::const_iterator it = sfm_data.GetViews().begin();
    it != sfm_data.GetViews().end(); ++it)
  {
    const View * v = it->second.get();
    if (sfm_data.IsPoseAndIntrinsicDefined(v))
    {
      valid_idx.insert(v->id_view);
    }
  }
  return valid_idx;
}

/// Filter a list of pair: Keep only the pair that are defined in index list
template <typename IterablePairs, typename IterableIndex>
static Pair_Set Pair_filter
(
  const IterablePairs & pairs,
  const IterableIndex & index
)
{
  Pair_Set kept_pairs;
  for (auto& it : pairs)
  {
    if (index.count(it.first) > 0 &&
      index.count(it.second) > 0)
    kept_pairs.insert(it);
  }
  return kept_pairs;
}

// Remove tracks that have a small angle (tracks with tiny angle leads to instable 3D points)
// Return the number of removed tracks
static IndexT RemoveOutliers_PixelResidualError
(
  SfM_Data & sfm_data,
  const double dThresholdPixel,
  const unsigned int minTrackLength = 2
)
{
  IndexT outlier_count = 0;
  Landmarks::iterator iterTracks = sfm_data.structure.begin();
  while (iterTracks != sfm_data.structure.end())
  {
    Observations & obs = iterTracks->second.obs;
    Observations::iterator itObs = obs.begin();
    while (itObs != obs.end())
    {
      const View * view = sfm_data.views.at(itObs->first).get();
      const geometry::Pose3 pose = sfm_data.GetPoseOrDie(view);
      const cameras::IntrinsicBase * intrinsic = sfm_data.intrinsics.at(view->id_intrinsic).get();
      const Vec2 residual = intrinsic->residual(pose, iterTracks->second.X, itObs->second.x);
      if (residual.norm() > dThresholdPixel)
      {
        ++outlier_count;
        itObs = obs.erase(itObs);
      }
      else
        ++itObs;
    }
    if (obs.empty() || obs.size() < minTrackLength)
      iterTracks = sfm_data.structure.erase(iterTracks);
    else
      ++iterTracks;
  }
  return outlier_count;
}

// Remove tracks that have a small angle (tracks with tiny angle leads to instable 3D points)
// Return the number of removed tracks
static IndexT RemoveOutliers_AngleError
(
  SfM_Data & sfm_data,
  const double dMinAcceptedAngle
)
{
  IndexT removedTrack_count = 0;
  Landmarks::iterator iterTracks = sfm_data.structure.begin();
  while (iterTracks != sfm_data.structure.end())
  {
    Observations & obs = iterTracks->second.obs;
    double max_angle = 0.0;
    for (Observations::const_iterator itObs1 = obs.begin();
      itObs1 != obs.end(); ++itObs1)
    {
      const View * view1 = sfm_data.views.at(itObs1->first).get();
      const geometry::Pose3 pose1 = sfm_data.GetPoseOrDie(view1);
      const cameras::IntrinsicBase * intrinsic1 = sfm_data.intrinsics.at(view1->id_intrinsic).get();

      Observations::const_iterator itObs2 = itObs1;
      ++itObs2;
      for (; itObs2 != obs.end(); ++itObs2)
      {
        const View * view2 = sfm_data.views.at(itObs2->first).get();
        const geometry::Pose3 pose2 = sfm_data.GetPoseOrDie(view2);
        const cameras::IntrinsicBase * intrinsic2 = sfm_data.intrinsics.at(view2->id_intrinsic).get();

        const double angle = AngleBetweenRay(
          pose1, intrinsic1, pose2, intrinsic2,
          itObs1->second.x, itObs2->second.x);
        max_angle = std::max(angle, max_angle);
      }
    }
    if (max_angle < dMinAcceptedAngle)
    {
      iterTracks = sfm_data.structure.erase(iterTracks);
      ++removedTrack_count;
    }
    else
      ++iterTracks;
  }
  return removedTrack_count;
}

static bool eraseMissingPoses(SfM_Data & sfm_data, const IndexT min_points_per_pose)
{
  IndexT removed_elements = 0;
  const Landmarks & landmarks = sfm_data.structure;

  // Count the observation poses occurence
  Hash_Map<IndexT, IndexT> map_PoseId_Count;
  // Init with 0 count (in order to be able to remove non referenced elements)
  for (Poses::const_iterator itPoses = sfm_data.GetPoses().begin();
    itPoses != sfm_data.GetPoses().end(); ++itPoses)
  {
    map_PoseId_Count[itPoses->first] = 0;
  }

  // Count occurence of the poses in the Landmark observations
  for (Landmarks::const_iterator itLandmarks = landmarks.begin();
    itLandmarks != landmarks.end(); ++itLandmarks)
  {
    const Observations & obs = itLandmarks->second.obs;
    for (Observations::const_iterator itObs = obs.begin();
      itObs != obs.end(); ++itObs)
    {
      const IndexT ViewId = itObs->first;
      const View * v = sfm_data.GetViews().at(ViewId).get();
      if (map_PoseId_Count.count(v->id_pose))
        map_PoseId_Count.at(v->id_pose) += 1;
      else
        map_PoseId_Count[v->id_pose] = 0;
    }
  }
  // If usage count is smaller than the threshold, remove the Pose
  for (Hash_Map<IndexT, IndexT>::const_iterator it = map_PoseId_Count.begin();
    it != map_PoseId_Count.end(); ++it)
  {
    if (it->second < min_points_per_pose)
    {
      sfm_data.poses.erase(it->first);
      ++removed_elements;
    }
  }
  return removed_elements > 0;
}

static bool eraseObservationsWithMissingPoses(SfM_Data & sfm_data, const IndexT min_points_per_landmark)
{
  IndexT removed_elements = 0;

  std::set<IndexT> pose_Index;
  std::transform(sfm_data.poses.begin(), sfm_data.poses.end(),
    std::inserter(pose_Index, pose_Index.begin()), stl::RetrieveKey());

  // For each landmark:
  //  - Check if we need to keep the observations & the track
  Landmarks::iterator itLandmarks = sfm_data.structure.begin();
  while (itLandmarks != sfm_data.structure.end())
  {
    Observations & obs = itLandmarks->second.obs;
    Observations::iterator itObs = obs.begin();
    while (itObs != obs.end())
    {
      const IndexT ViewId = itObs->first;
      const View * v = sfm_data.GetViews().at(ViewId).get();
      if (pose_Index.count(v->id_pose) == 0)
      {
        itObs = obs.erase(itObs);
        ++removed_elements;
      }
      else
        ++itObs;
    }
    if (obs.empty() || obs.size() < min_points_per_landmark)
      itLandmarks = sfm_data.structure.erase(itLandmarks);
    else
      ++itLandmarks;
  }
  return removed_elements > 0;
}

/// Remove unstable content from analysis of the sfm_data structure
static bool eraseUnstablePosesAndObservations(
  SfM_Data & sfm_data,
  const IndexT min_points_per_pose = 6,
  const IndexT min_points_per_landmark = 2)
{
  IndexT remove_iteration = 0;
  bool bRemovedContent = false;
  do
  {
    bRemovedContent = false;
    if (eraseMissingPoses(sfm_data, min_points_per_pose))
    {
      bRemovedContent = eraseObservationsWithMissingPoses(sfm_data, min_points_per_landmark);
      // Erase some observations can make some Poses index disappear so perform the process in a loop
    }
    remove_iteration += bRemovedContent ? 1 : 0;
  }
  while (bRemovedContent);

  return remove_iteration > 0;
}

} // namespace sfm
} // namespace openMVG

#endif // OPENMVG_SFM_DATA_FILTERS_HPP
<commit_msg>Fix some typo & enhanced filtering of structure data in case of missing camera poses.<commit_after>
// Copyright (c) 2015 Pierre MOULON.

// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.

#ifndef OPENMVG_SFM_DATA_FILTERS_HPP
#define OPENMVG_SFM_DATA_FILTERS_HPP

#include "openMVG/stl/stl.hpp"
#include <iterator>

namespace openMVG {
namespace sfm {

/// List the view indexes that have valid camera intrinsic and pose.
static std::set<IndexT> Get_Valid_Views
(
  const SfM_Data & sfm_data
)
{
  std::set<IndexT> valid_idx;
  for (Views::const_iterator it = sfm_data.GetViews().begin();
    it != sfm_data.GetViews().end(); ++it)
  {
    const View * v = it->second.get();
    if (sfm_data.IsPoseAndIntrinsicDefined(v))
    {
      valid_idx.insert(v->id_view);
    }
  }
  return valid_idx;
}

/// Filter a list of pair: Keep only the pair that are defined in index list
template <typename IterablePairs, typename IterableIndex>
static Pair_Set Pair_filter
(
  const IterablePairs & pairs,
  const IterableIndex & index
)
{
  Pair_Set kept_pairs;
  for (auto& it : pairs)
  {
    if (index.count(it.first) > 0 &&
      index.count(it.second) > 0)
    kept_pairs.insert(it);
  }
  return kept_pairs;
}

// Remove tracks that have a small angle (tracks with tiny angle leads to instable 3D points)
// Return the number of removed tracks
static IndexT RemoveOutliers_PixelResidualError
(
  SfM_Data & sfm_data,
  const double dThresholdPixel,
  const unsigned int minTrackLength = 2
)
{
  IndexT outlier_count = 0;
  Landmarks::iterator iterTracks = sfm_data.structure.begin();
  while (iterTracks != sfm_data.structure.end())
  {
    Observations & obs = iterTracks->second.obs;
    Observations::iterator itObs = obs.begin();
    while (itObs != obs.end())
    {
      const View * view = sfm_data.views.at(itObs->first).get();
      const geometry::Pose3 pose = sfm_data.GetPoseOrDie(view);
      const cameras::IntrinsicBase * intrinsic = sfm_data.intrinsics.at(view->id_intrinsic).get();
      const Vec2 residual = intrinsic->residual(pose, iterTracks->second.X, itObs->second.x);
      if (residual.norm() > dThresholdPixel)
      {
        ++outlier_count;
        itObs = obs.erase(itObs);
      }
      else
        ++itObs;
    }
    if (obs.empty() || obs.size() < minTrackLength)
      iterTracks = sfm_data.structure.erase(iterTracks);
    else
      ++iterTracks;
  }
  return outlier_count;
}

// Remove tracks that have a small angle (tracks with tiny angle leads to instable 3D points)
// Return the number of removed tracks
static IndexT RemoveOutliers_AngleError
(
  SfM_Data & sfm_data,
  const double dMinAcceptedAngle
)
{
  IndexT removedTrack_count = 0;
  Landmarks::iterator iterTracks = sfm_data.structure.begin();
  while (iterTracks != sfm_data.structure.end())
  {
    Observations & obs = iterTracks->second.obs;
    double max_angle = 0.0;
    for (Observations::const_iterator itObs1 = obs.begin();
      itObs1 != obs.end(); ++itObs1)
    {
      const View * view1 = sfm_data.views.at(itObs1->first).get();
      const geometry::Pose3 pose1 = sfm_data.GetPoseOrDie(view1);
      const cameras::IntrinsicBase * intrinsic1 = sfm_data.intrinsics.at(view1->id_intrinsic).get();

      Observations::const_iterator itObs2 = itObs1;
      ++itObs2;
      for (; itObs2 != obs.end(); ++itObs2)
      {
        const View * view2 = sfm_data.views.at(itObs2->first).get();
        const geometry::Pose3 pose2 = sfm_data.GetPoseOrDie(view2);
        const cameras::IntrinsicBase * intrinsic2 = sfm_data.intrinsics.at(view2->id_intrinsic).get();

        const double angle = AngleBetweenRay(
          pose1, intrinsic1, pose2, intrinsic2,
          itObs1->second.x, itObs2->second.x);
        max_angle = std::max(angle, max_angle);
      }
    }
    if (max_angle < dMinAcceptedAngle)
    {
      iterTracks = sfm_data.structure.erase(iterTracks);
      ++removedTrack_count;
    }
    else
      ++iterTracks;
  }
  return removedTrack_count;
}

static bool eraseMissingPoses
(
  SfM_Data & sfm_data,
  const IndexT min_points_per_pose = 6
)
{
  IndexT removed_elements = 0;
  const Landmarks & landmarks = sfm_data.structure;

  // Count the observation poses occurrence
  Hash_Map<IndexT, IndexT> map_PoseId_Count;
  // Init with 0 count (in order to be able to remove non referenced elements)
  for (Poses::const_iterator itPoses = sfm_data.GetPoses().begin();
    itPoses != sfm_data.GetPoses().end(); ++itPoses)
  {
    map_PoseId_Count[itPoses->first] = 0;
  }

  // Count occurrence of the poses in the Landmark observations
  for (Landmarks::const_iterator itLandmarks = landmarks.begin();
    itLandmarks != landmarks.end(); ++itLandmarks)
  {
    const Observations & obs = itLandmarks->second.obs;
    for (Observations::const_iterator itObs = obs.begin();
      itObs != obs.end(); ++itObs)
    {
      const IndexT ViewId = itObs->first;
      const View * v = sfm_data.GetViews().at(ViewId).get();
      if (map_PoseId_Count.count(v->id_pose))
        map_PoseId_Count.at(v->id_pose) += 1;
      else
        map_PoseId_Count[v->id_pose] = 0;
    }
  }
  // If usage count is smaller than the threshold, remove the Pose
  for (Hash_Map<IndexT, IndexT>::const_iterator it = map_PoseId_Count.begin();
    it != map_PoseId_Count.end(); ++it)
  {
    if (it->second < min_points_per_pose)
    {
      sfm_data.poses.erase(it->first);
      ++removed_elements;
    }
  }
  return removed_elements > 0;
}

static bool eraseObservationsWithMissingPoses
(
  SfM_Data & sfm_data,
  const IndexT min_points_per_landmark = 2
)
{
  IndexT removed_elements = 0;

  std::set<IndexT> pose_Index;
  std::transform(sfm_data.poses.begin(), sfm_data.poses.end(),
    std::inserter(pose_Index, pose_Index.begin()), stl::RetrieveKey());

  // For each landmark:
  //  - Check if we need to keep the observations & the track
  Landmarks::iterator itLandmarks = sfm_data.structure.begin();
  while (itLandmarks != sfm_data.structure.end())
  {
    Observations & obs = itLandmarks->second.obs;
    Observations::iterator itObs = obs.begin();
    while (itObs != obs.end())
    {
      const IndexT ViewId = itObs->first;
      const View * v = sfm_data.GetViews().at(ViewId).get();
      if (pose_Index.count(v->id_pose) == 0)
      {
        itObs = obs.erase(itObs);
        ++removed_elements;
      }
      else
        ++itObs;
    }
    if (obs.empty() || obs.size() < min_points_per_landmark)
      itLandmarks = sfm_data.structure.erase(itLandmarks);
    else
      ++itLandmarks;
  }
  return removed_elements > 0;
}

/// Remove unstable content from analysis of the sfm_data structure
static bool eraseUnstablePosesAndObservations
(
  SfM_Data & sfm_data,
  const IndexT min_points_per_pose = 6,
  const IndexT min_points_per_landmark = 2
)
{
  // First remove orphan observation(s) (observation using an undefined pose)
  eraseObservationsWithMissingPoses(sfm_data, min_points_per_landmark);
  // Then iteratively remove orphan poses & observations
  IndexT remove_iteration = 0;
  bool bRemovedContent = false;
  do
  {
    bRemovedContent = false;
    if (eraseMissingPoses(sfm_data, min_points_per_pose))
    {
      bRemovedContent = eraseObservationsWithMissingPoses(sfm_data, min_points_per_landmark);
      // Erase some observations can make some Poses index disappear so perform the process in a loop
    }
    remove_iteration += bRemovedContent ? 1 : 0;
  }
  while (bRemovedContent);

  return remove_iteration > 0;
}

} // namespace sfm
} // namespace openMVG

#endif // OPENMVG_SFM_DATA_FILTERS_HPP
<|endoftext|>


<commit_before>#include <core/stdafx.h>
#include <core/interpret/flags.h>
#include <core/utility/strings.h>
#include <core/addin/addin.h>
#include <core/addin/mfcmapi.h>

namespace flags
{
	// Interprets a flag value according to a flag name and returns a string
	// Will not return a string if the flag name is not recognized
	std::wstring InterpretFlags(ULONG ulFlagName, LONG lFlagValue)
	{
		ULONG ulCurEntry = 0;

		if (FlagArray.empty()) return L"";

		while (ulCurEntry < FlagArray.size() && FlagArray[ulCurEntry].ulFlagName != ulFlagName)
		{
			ulCurEntry++;
		}

		// Don't run off the end of the array
		if (FlagArray.size() == ulCurEntry) return L"";
		if (FlagArray[ulCurEntry].ulFlagName != ulFlagName) return L"";

		// We've matched our flag name to the array - we SHOULD return a string at this point
		auto flags = std::vector<std::wstring>{};
		auto lTempValue = lFlagValue;
		for (; FlagArray[ulCurEntry].ulFlagName == ulFlagName; ulCurEntry++)
		{
			if (flagFLAG == FlagArray[ulCurEntry].ulFlagType)
			{
				if (FlagArray[ulCurEntry].lFlagValue & lTempValue)
				{
					flags.push_back(FlagArray[ulCurEntry].lpszName);
					lTempValue &= ~FlagArray[ulCurEntry].lFlagValue;
				}
			}
			else if (flagVALUE == FlagArray[ulCurEntry].ulFlagType)
			{
				if (FlagArray[ulCurEntry].lFlagValue == lTempValue)
				{
					flags.push_back(FlagArray[ulCurEntry].lpszName);
					lTempValue = 0;
				}
			}
			else if (flagVALUEHIGHBYTES == FlagArray[ulCurEntry].ulFlagType)
			{
				if (FlagArray[ulCurEntry].lFlagValue == (lTempValue >> 16 & 0xFFFF))
				{
					flags.push_back(FlagArray[ulCurEntry].lpszName);
					lTempValue = lTempValue - (FlagArray[ulCurEntry].lFlagValue << 16);
				}
			}
			else if (flagVALUE3RDBYTE == FlagArray[ulCurEntry].ulFlagType)
			{
				if (FlagArray[ulCurEntry].lFlagValue == (lTempValue >> 8 & 0xFF))
				{
					flags.push_back(FlagArray[ulCurEntry].lpszName);
					lTempValue = lTempValue - (FlagArray[ulCurEntry].lFlagValue << 8);
				}
			}
			else if (flagVALUE4THBYTE == FlagArray[ulCurEntry].ulFlagType)
			{
				if (FlagArray[ulCurEntry].lFlagValue == (lTempValue & 0xFF))
				{
					flags.push_back(FlagArray[ulCurEntry].lpszName);
					lTempValue = lTempValue - FlagArray[ulCurEntry].lFlagValue;
				}
			}
			else if (flagVALUELOWERNIBBLE == FlagArray[ulCurEntry].ulFlagType)
			{
				if (FlagArray[ulCurEntry].lFlagValue == (lTempValue & 0x0F))
				{
					flags.push_back(FlagArray[ulCurEntry].lpszName);
					lTempValue = lTempValue - FlagArray[ulCurEntry].lFlagValue;
				}
			}
			else if (flagCLEARBITS == FlagArray[ulCurEntry].ulFlagType)
			{
				// find any bits we need to clear
				const auto lClearedBits = FlagArray[ulCurEntry].lFlagValue & lTempValue;
				// report what we found
				if (0 != lClearedBits)
				{
					flags.push_back(strings::format(L"0x%X", lClearedBits)); // STRING_OK
						// clear the bits out
					lTempValue &= ~FlagArray[ulCurEntry].lFlagValue;
				}
			}
		}

		if (lTempValue || flags.empty())
		{
			flags.push_back(strings::format(L"0x%X", lTempValue)); // STRING_OK
		}

		return strings::join(flags, L" | ");
	}

	// Returns a list of all known flags/values for a flag name.
	// For instance, for flagFuzzyLevel, would return:
	// 0x00000000 FL_FULLSTRING\r\n\
	// 0x00000001 FL_SUBSTRING\r\n\
	// 0x00000002 FL_PREFIX\r\n\
	// 0x00010000 FL_IGNORECASE\r\n\
	// 0x00020000 FL_IGNORENONSPACE\r\n\
	// 0x00040000 FL_LOOSE
	//
	// Since the string is always appended to a prompt we include \r\n at the start
	std::wstring AllFlagsToString(ULONG ulFlagName, bool bHex)
	{
		if (!ulFlagName) return L"";
		if (FlagArray.empty()) return L"";

		ULONG ulCurEntry = 0;

		while (ulCurEntry < FlagArray.size() && FlagArray[ulCurEntry].ulFlagName != ulFlagName)
		{
			ulCurEntry++;
		}

		if (ulCurEntry == FlagArray.size() || FlagArray[ulCurEntry].ulFlagName != ulFlagName) return L"";

		// We've matched our flag name to the array - we SHOULD return a string at this point
		auto flags = std::vector<std::wstring>{};
		for (; FlagArray[ulCurEntry].ulFlagName == ulFlagName; ulCurEntry++)
		{
			if (flagCLEARBITS == FlagArray[ulCurEntry].ulFlagType)
			{
				// keep going
			}
			else
			{
				if (bHex)
				{
					flags.push_back(strings::formatmessage(
						IDS_FLAGTOSTRINGHEX, FlagArray[ulCurEntry].lFlagValue, FlagArray[ulCurEntry].lpszName));
				}
				else
				{
					flags.push_back(strings::formatmessage(
						IDS_FLAGTOSTRINGDEC, FlagArray[ulCurEntry].lFlagValue, FlagArray[ulCurEntry].lpszName));
				}
			}
		}

		return strings::join(flags, L"\r\n");
	}
} // namespace flags
<commit_msg>tighten<commit_after>#include <core/stdafx.h>
#include <core/interpret/flags.h>
#include <core/utility/strings.h>
#include <core/addin/addin.h>
#include <core/addin/mfcmapi.h>

namespace flags
{
	// Interprets a flag value according to a flag name and returns a string
	// Will not return a string if the flag name is not recognized
	std::wstring InterpretFlags(ULONG ulFlagName, LONG lFlagValue)
	{
		ULONG ulCurEntry = 0;

		if (FlagArray.empty()) return L"";

		while (ulCurEntry < FlagArray.size() && FlagArray[ulCurEntry].ulFlagName != ulFlagName)
		{
			ulCurEntry++;
		}

		// Don't run off the end of the array
		if (FlagArray.size() == ulCurEntry) return L"";
		if (FlagArray[ulCurEntry].ulFlagName != ulFlagName) return L"";

		// We've matched our flag name to the array - we SHOULD return a string at this point
		auto flags = std::vector<std::wstring>{};
		auto lTempValue = lFlagValue;
		for (; FlagArray[ulCurEntry].ulFlagName == ulFlagName; ulCurEntry++)
		{
			if (flagFLAG == FlagArray[ulCurEntry].ulFlagType)
			{
				if (FlagArray[ulCurEntry].lFlagValue & lTempValue)
				{
					flags.push_back(FlagArray[ulCurEntry].lpszName);
					lTempValue &= ~FlagArray[ulCurEntry].lFlagValue;
				}
			}
			else if (flagVALUE == FlagArray[ulCurEntry].ulFlagType)
			{
				if (FlagArray[ulCurEntry].lFlagValue == lTempValue)
				{
					flags.push_back(FlagArray[ulCurEntry].lpszName);
					lTempValue = 0;
				}
			}
			else if (flagVALUEHIGHBYTES == FlagArray[ulCurEntry].ulFlagType)
			{
				if (FlagArray[ulCurEntry].lFlagValue == (lTempValue >> 16 & 0xFFFF))
				{
					flags.push_back(FlagArray[ulCurEntry].lpszName);
					lTempValue = lTempValue - (FlagArray[ulCurEntry].lFlagValue << 16);
				}
			}
			else if (flagVALUE3RDBYTE == FlagArray[ulCurEntry].ulFlagType)
			{
				if (FlagArray[ulCurEntry].lFlagValue == (lTempValue >> 8 & 0xFF))
				{
					flags.push_back(FlagArray[ulCurEntry].lpszName);
					lTempValue = lTempValue - (FlagArray[ulCurEntry].lFlagValue << 8);
				}
			}
			else if (flagVALUE4THBYTE == FlagArray[ulCurEntry].ulFlagType)
			{
				if (FlagArray[ulCurEntry].lFlagValue == (lTempValue & 0xFF))
				{
					flags.push_back(FlagArray[ulCurEntry].lpszName);
					lTempValue = lTempValue - FlagArray[ulCurEntry].lFlagValue;
				}
			}
			else if (flagVALUELOWERNIBBLE == FlagArray[ulCurEntry].ulFlagType)
			{
				if (FlagArray[ulCurEntry].lFlagValue == (lTempValue & 0x0F))
				{
					flags.push_back(FlagArray[ulCurEntry].lpszName);
					lTempValue = lTempValue - FlagArray[ulCurEntry].lFlagValue;
				}
			}
			else if (flagCLEARBITS == FlagArray[ulCurEntry].ulFlagType)
			{
				// find any bits we need to clear
				const auto lClearedBits = FlagArray[ulCurEntry].lFlagValue & lTempValue;
				// report what we found
				if (0 != lClearedBits)
				{
					flags.push_back(strings::format(L"0x%X", lClearedBits)); // STRING_OK
						// clear the bits out
					lTempValue &= ~FlagArray[ulCurEntry].lFlagValue;
				}
			}
		}

		if (lTempValue || flags.empty())
		{
			flags.push_back(strings::format(L"0x%X", lTempValue)); // STRING_OK
		}

		return strings::join(flags, L" | ");
	}

	// Returns a list of all known flags/values for a flag name.
	// For instance, for flagFuzzyLevel, would return:
	// 0x00000000 FL_FULLSTRING\r\n\
	// 0x00000001 FL_SUBSTRING\r\n\
	// 0x00000002 FL_PREFIX\r\n\
	// 0x00010000 FL_IGNORECASE\r\n\
	// 0x00020000 FL_IGNORENONSPACE\r\n\
	// 0x00040000 FL_LOOSE
	//
	// Since the string is always appended to a prompt we include \r\n at the start
	std::wstring AllFlagsToString(ULONG ulFlagName, bool bHex)
	{
		if (!ulFlagName) return L"";
		if (FlagArray.empty()) return L"";

		ULONG ulCurEntry = 0;

		while (ulCurEntry < FlagArray.size() && FlagArray[ulCurEntry].ulFlagName != ulFlagName)
		{
			ulCurEntry++;
		}

		if (ulCurEntry == FlagArray.size() || FlagArray[ulCurEntry].ulFlagName != ulFlagName) return L"";

		// We've matched our flag name to the array - we SHOULD return a string at this point
		auto flags = std::vector<std::wstring>{};
		for (; FlagArray[ulCurEntry].ulFlagName == ulFlagName; ulCurEntry++)
		{
			if (flagCLEARBITS == FlagArray[ulCurEntry].ulFlagType)
			{
				// keep going
			}
			else
			{
				flags.push_back(strings::formatmessage(
					bHex ? IDS_FLAGTOSTRINGHEX : IDS_FLAGTOSTRINGDEC,
					FlagArray[ulCurEntry].lFlagValue,
					FlagArray[ulCurEntry].lpszName));
			}
		}

		return strings::join(flags, L"\r\n");
	}
} // namespace flags
<|endoftext|>


<commit_before>#include <osgUtil/TrackballManipulator>
#include <osg/Types>
#include <osg/Notify>

using namespace osg;
using namespace osgUtil;

TrackballManipulator::TrackballManipulator()
{
    _modelScale = 0.01f;
    _minimumZoomScale = 0.05f;
    _thrown = false;
}

TrackballManipulator::~TrackballManipulator()
{
}

void TrackballManipulator::setNode(osg::Node* node)
{
    _node = node;
    if (_node.get())
    {
        const osg::BoundingSphere& boundingSphere=_node->getBound();
        _modelScale = boundingSphere._radius;
    }
}

const osg::Node* TrackballManipulator::getNode() const
{
    return _node.get();
}

                                 /*ea*/
void TrackballManipulator::home(const GUIEventAdapter& ,GUIActionAdapter& us)
{
    if(_node.get() && _camera.get())
    {

        const osg::BoundingSphere& boundingSphere=_node->getBound();

        _camera->setView(boundingSphere._center+osg::Vec3( 0.0,-3.0f * boundingSphere._radius,0.0f),
                        boundingSphere._center,
                        osg::Vec3(0.0f,0.0f,1.0f));

        us.requestRedraw();
    }

}

void TrackballManipulator::init(const GUIEventAdapter& ,GUIActionAdapter& )
{
    flushMouseEventStack();
}

bool TrackballManipulator::handle(const GUIEventAdapter& ea,GUIActionAdapter& us)
{
    if(!_camera.get()) return false;

    switch(ea.getEventType())
    {
        case(GUIEventAdapter::PUSH):
        {
            flushMouseEventStack();
            addMouseEvent(ea);
            if (calcMovement()) us.requestRedraw();
            us.requestContinuousUpdate(false);
            _thrown = false;
            return true;
        }

        case(GUIEventAdapter::RELEASE):
        {
            if (ea.getButtonMask()==0)
            {

                if (isMouseMoving())
                {
                    if (calcMovement())
                    {
                        us.requestRedraw();
                        us.requestContinuousUpdate(true);
                        _thrown = true;
                    }
                }
                else
                {
                    flushMouseEventStack();
                    addMouseEvent(ea);
                    if (calcMovement()) us.requestRedraw();
                    us.requestContinuousUpdate(false);
                    _thrown = false;
                }

            }
            else
            {
                flushMouseEventStack();
                addMouseEvent(ea);
                if (calcMovement()) us.requestRedraw();
                us.requestContinuousUpdate(false);
                _thrown = false;
            }
            return true;
        }

        case(GUIEventAdapter::DRAG):
        {
            addMouseEvent(ea);
            if (calcMovement()) us.requestRedraw();
            us.requestContinuousUpdate(false);
            _thrown = false;
            return true;
        }

        case(GUIEventAdapter::MOVE):
        {
            return false;
        }

        case(GUIEventAdapter::KEYBOARD):
            if (ea.getKey()==' ')
            {
                flushMouseEventStack();
                _thrown = false;
                home(ea,us);
                us.requestRedraw();
                us.requestContinuousUpdate(false);
                return true;
            } else if (ea.getKey()=='+')
            {
                _camera->setFusionDistanceRatio(_camera->getFusionDistanceRatio()*1.25f);
                return true;
            }
            else if (ea.getKey()=='-')
            {
                _camera->setFusionDistanceRatio(_camera->getFusionDistanceRatio()/1.25f);
                return true;
            }
            return false;
        case(GUIEventAdapter::FRAME):
            if (_thrown)
            {
                if (calcMovement()) us.requestRedraw();
                return true;
            }
            return false;
        default:
            return false;
    }
}

bool TrackballManipulator::isMouseMoving()
{
    if (_ga_t0.get()==NULL || _ga_t1.get()==NULL) return false;

    static const float velocity = 100.0f;

    float dx = _ga_t0->getX()-_ga_t1->getX();
    float dy = _ga_t0->getY()-_ga_t1->getY();
    float len = sqrtf(dx*dx+dy*dy);
    float dt = _ga_t0->time()-_ga_t1->time();

    return (len>dt*velocity);
}

void TrackballManipulator::flushMouseEventStack()
{
    _ga_t1 = NULL;
    _ga_t0 = NULL;
}

void TrackballManipulator::addMouseEvent(const GUIEventAdapter& ea)
{
    _ga_t1 = _ga_t0;
    _ga_t0 = &ea;
}

bool TrackballManipulator::calcMovement()
{
    _camera->setFusionDistanceMode(osg::Camera::PROPORTIONAL_TO_LOOK_DISTANCE);

    // return if less then two events have been added.
    if (_ga_t0.get()==NULL || _ga_t1.get()==NULL) return false;

    float dx = _ga_t0->getX()-_ga_t1->getX();
    float dy = _ga_t0->getY()-_ga_t1->getY();

    // return if there is no movement.
    if (dx==0 && dy==0) return false;

    float focalLength = (_camera->getCenterPoint()-_camera->getEyePoint()).length();
    unsigned int buttonMask = _ga_t1->getButtonMask();
    if (buttonMask==GUIEventAdapter::LEFT_BUTTON)
    {

        // rotate camera.

        osg::Vec3 center = _camera->getCenterPoint();
        osg::Vec3 axis;
        float angle;

        float mx0 = (_ga_t0->getXmin()+_ga_t0->getXmax())/2.0f;
        float rx0 = (_ga_t0->getXmax()-_ga_t0->getXmin())/2.0f;

        float my0 = (_ga_t0->getYmin()+_ga_t0->getYmax())/2.0f;
        float ry0 = (_ga_t0->getYmax()-_ga_t0->getYmin())/2.0f;

        float mx1 = (_ga_t0->getXmin()+_ga_t1->getXmax())/2.0f;
        float rx1 = (_ga_t0->getXmax()-_ga_t1->getXmin())/2.0f;

        float my1 = (_ga_t1->getYmin()+_ga_t1->getYmax())/2.0f;
        float ry1 = (_ga_t1->getYmax()-_ga_t1->getYmin())/2.0f;

        float px0 = (_ga_t0->getX()-mx0)/rx0;
        float py0 = (my0-_ga_t0->getY())/ry0;

        float px1 = (_ga_t1->getX()-mx1)/rx1;
        float py1 = (my1-_ga_t1->getY())/ry1;

        trackball(axis,angle,px1,py1,px0,py0);

        osg::Matrix mat;
        mat.makeTranslate(-center.x(),-center.y(),-center.z());
        mat *= Matrix::rotate(angle,axis.x(),axis.y(),axis.z());
        mat *= Matrix::translate(center.x(),center.y(),center.z());

        _camera->transformLookAt(mat);

        return true;

    }
    else if (buttonMask==GUIEventAdapter::MIDDLE_BUTTON ||
        buttonMask==(GUIEventAdapter::LEFT_BUTTON|GUIEventAdapter::RIGHT_BUTTON))
    {

        // pan model.

        float scale = 0.0015f*focalLength;

        osg::Vec3 uv = _camera->getUpVector();
        osg::Vec3 sv = _camera->getSideVector();
        osg::Vec3 dv = uv*(dy*scale)-sv*(dx*scale);

        osg::Matrix mat;
        mat.makeTranslate(dv.x(),dv.y(),dv.z());

        _camera->transformLookAt(mat);

        return true;

    }
    else if (buttonMask==GUIEventAdapter::RIGHT_BUTTON)
    {

        // zoom model.

        float fd = focalLength;
        float scale = 1.0f-dy*0.001f;
        if (fd*scale>_modelScale*_minimumZoomScale)
        {
            // zoom camera in.
            osg::Vec3 center = _camera->getCenterPoint();

            osg::Matrix mat;
            mat.makeTranslate(-center.x(),-center.y(),-center.z());
            mat *= Matrix::scale(scale,scale,scale);
            mat *= Matrix::translate(center.x(),center.y(),center.z());

            _camera->transformLookAt(mat);

        }
        else
        {

            //            notify(DEBUG_INFO) << "Pushing forward"<<std::endl;
            // push the camera forward.
            float scale = 0.0015f*fd;
            osg::Vec3 dv = _camera->getLookVector()*(dy*scale);

            osg::Matrix mat;
            mat.makeTranslate(dv.x(),dv.y(),dv.z());

            _camera->transformLookAt(mat);

        }

        return true;

    }

    return false;
}

/*
 * This size should really be based on the distance from the center of
 * rotation to the point on the object underneath the mouse.  That
 * point would then track the mouse as closely as possible.  This is a
 * simple example, though, so that is left as an Exercise for the
 * Programmer.
 */
const float TRACKBALLSIZE = 0.8f;

/*
 * Ok, simulate a track-ball.  Project the points onto the virtual
 * trackball, then figure out the axis of rotation, which is the cross
 * product of P1 P2 and O P1 (O is the center of the ball, 0,0,0)
 * Note:  This is a deformed trackball-- is a trackball in the center,
 * but is deformed into a hyperbolic sheet of rotation away from the
 * center.  This particular function was chosen after trying out
 * several variations.
 *
 * It is assumed that the arguments to this routine are in the range
 * (-1.0 ... 1.0)
 */
void TrackballManipulator::trackball(osg::Vec3& axis,float& angle, float p1x, float p1y, float p2x, float p2y)
{
    /*
     * First, figure out z-coordinates for projection of P1 and P2 to
     * deformed sphere
     */

    osg::Vec3 uv = _camera->getUpVector();
    osg::Vec3 sv = _camera->getSideVector();
    osg::Vec3 lv = _camera->getLookVector();

    osg::Vec3 p1 = sv*p1x+uv*p1y-lv*tb_project_to_sphere(TRACKBALLSIZE,p1x,p1y);
    osg::Vec3 p2 = sv*p2x+uv*p2y-lv*tb_project_to_sphere(TRACKBALLSIZE,p2x,p2y);

    /*
     *  Now, we want the cross product of P1 and P2
     */

// Robert,
//
// This was the quick 'n' dirty  fix to get the trackball doing the right
// thing after fixing the Quat rotations to be right-handed.  You may want
// to do something more elegant.
//   axis = p1^p2;
axis = p2^p1;
    axis.normalize();

    /*
     *  Figure out how much to rotate around that axis.
     */
    float t = (p2-p1).length() / (2.0*TRACKBALLSIZE);

    /*
     * Avoid problems with out-of-control values...
     */
    if (t > 1.0) t = 1.0;
    if (t < -1.0) t = -1.0;
    angle = inRadians(asin(t));

}

/*
 * Project an x,y pair onto a sphere of radius r OR a hyperbolic sheet
 * if we are away from the center of the sphere.
 */
float TrackballManipulator::tb_project_to_sphere(float r, float x, float y)
{
    float d, t, z;

    d = sqrt(x*x + y*y);
                                 /* Inside sphere */
    if (d < r * 0.70710678118654752440)
    {
        z = sqrt(r*r - d*d);
    }                            /* On hyperbola */
    else
    {
        t = r / 1.41421356237309504880;
        z = t*t / d;
    }
    return z;
}
<commit_msg>Moved the setting of camera fusion distance mode into the handle method so that it is updated on everyframe to ensure that switching between Drive/Flight and Trackball happens seemlessly.<commit_after>#include <osgUtil/TrackballManipulator>
#include <osg/Types>
#include <osg/Notify>

using namespace osg;
using namespace osgUtil;

TrackballManipulator::TrackballManipulator()
{
    _modelScale = 0.01f;
    _minimumZoomScale = 0.05f;
    _thrown = false;
}

TrackballManipulator::~TrackballManipulator()
{
}

void TrackballManipulator::setNode(osg::Node* node)
{
    _node = node;
    if (_node.get())
    {
        const osg::BoundingSphere& boundingSphere=_node->getBound();
        _modelScale = boundingSphere._radius;
    }
}

const osg::Node* TrackballManipulator::getNode() const
{
    return _node.get();
}

                                 /*ea*/
void TrackballManipulator::home(const GUIEventAdapter& ,GUIActionAdapter& us)
{
    if(_node.get() && _camera.get())
    {

        const osg::BoundingSphere& boundingSphere=_node->getBound();

        _camera->setView(boundingSphere._center+osg::Vec3( 0.0,-3.0f * boundingSphere._radius,0.0f),
                        boundingSphere._center,
                        osg::Vec3(0.0f,0.0f,1.0f));

        us.requestRedraw();
    }

}

void TrackballManipulator::init(const GUIEventAdapter& ,GUIActionAdapter& )
{
    flushMouseEventStack();
}

bool TrackballManipulator::handle(const GUIEventAdapter& ea,GUIActionAdapter& us)
{
    if(!_camera.get()) return false;

    switch(ea.getEventType())
    {
        case(GUIEventAdapter::PUSH):
        {
            flushMouseEventStack();
            addMouseEvent(ea);
            if (calcMovement()) us.requestRedraw();
            us.requestContinuousUpdate(false);
            _thrown = false;
            return true;
        }

        case(GUIEventAdapter::RELEASE):
        {
            if (ea.getButtonMask()==0)
            {

                if (isMouseMoving())
                {
                    if (calcMovement())
                    {
                        us.requestRedraw();
                        us.requestContinuousUpdate(true);
                        _thrown = true;
                    }
                }
                else
                {
                    flushMouseEventStack();
                    addMouseEvent(ea);
                    if (calcMovement()) us.requestRedraw();
                    us.requestContinuousUpdate(false);
                    _thrown = false;
                }

            }
            else
            {
                flushMouseEventStack();
                addMouseEvent(ea);
                if (calcMovement()) us.requestRedraw();
                us.requestContinuousUpdate(false);
                _thrown = false;
            }
            return true;
        }

        case(GUIEventAdapter::DRAG):
        {
            addMouseEvent(ea);
            if (calcMovement()) us.requestRedraw();
            us.requestContinuousUpdate(false);
            _thrown = false;
            return true;
        }

        case(GUIEventAdapter::MOVE):
        {
            return false;
        }

        case(GUIEventAdapter::KEYBOARD):
            if (ea.getKey()==' ')
            {
                flushMouseEventStack();
                _thrown = false;
                home(ea,us);
                us.requestRedraw();
                us.requestContinuousUpdate(false);
                return true;
            } else if (ea.getKey()=='+')
            {
                _camera->setFusionDistanceRatio(_camera->getFusionDistanceRatio()*1.25f);
                return true;
            }
            else if (ea.getKey()=='-')
            {
                _camera->setFusionDistanceRatio(_camera->getFusionDistanceRatio()/1.25f);
                return true;
            }
            return false;
        case(GUIEventAdapter::FRAME):
            _camera->setFusionDistanceMode(osg::Camera::PROPORTIONAL_TO_LOOK_DISTANCE);
            if (_thrown)
            {
                if (calcMovement()) us.requestRedraw();
                return true;
            }
            return false;
        default:
            return false;
    }
}

bool TrackballManipulator::isMouseMoving()
{
    if (_ga_t0.get()==NULL || _ga_t1.get()==NULL) return false;

    static const float velocity = 100.0f;

    float dx = _ga_t0->getX()-_ga_t1->getX();
    float dy = _ga_t0->getY()-_ga_t1->getY();
    float len = sqrtf(dx*dx+dy*dy);
    float dt = _ga_t0->time()-_ga_t1->time();

    return (len>dt*velocity);
}

void TrackballManipulator::flushMouseEventStack()
{
    _ga_t1 = NULL;
    _ga_t0 = NULL;
}

void TrackballManipulator::addMouseEvent(const GUIEventAdapter& ea)
{
    _ga_t1 = _ga_t0;
    _ga_t0 = &ea;
}

bool TrackballManipulator::calcMovement()
{

    // return if less then two events have been added.
    if (_ga_t0.get()==NULL || _ga_t1.get()==NULL) return false;

    float dx = _ga_t0->getX()-_ga_t1->getX();
    float dy = _ga_t0->getY()-_ga_t1->getY();

    // return if there is no movement.
    if (dx==0 && dy==0) return false;

    float focalLength = (_camera->getCenterPoint()-_camera->getEyePoint()).length();
    unsigned int buttonMask = _ga_t1->getButtonMask();
    if (buttonMask==GUIEventAdapter::LEFT_BUTTON)
    {

        // rotate camera.

        osg::Vec3 center = _camera->getCenterPoint();
        osg::Vec3 axis;
        float angle;

        float mx0 = (_ga_t0->getXmin()+_ga_t0->getXmax())/2.0f;
        float rx0 = (_ga_t0->getXmax()-_ga_t0->getXmin())/2.0f;

        float my0 = (_ga_t0->getYmin()+_ga_t0->getYmax())/2.0f;
        float ry0 = (_ga_t0->getYmax()-_ga_t0->getYmin())/2.0f;

        float mx1 = (_ga_t0->getXmin()+_ga_t1->getXmax())/2.0f;
        float rx1 = (_ga_t0->getXmax()-_ga_t1->getXmin())/2.0f;

        float my1 = (_ga_t1->getYmin()+_ga_t1->getYmax())/2.0f;
        float ry1 = (_ga_t1->getYmax()-_ga_t1->getYmin())/2.0f;

        float px0 = (_ga_t0->getX()-mx0)/rx0;
        float py0 = (my0-_ga_t0->getY())/ry0;

        float px1 = (_ga_t1->getX()-mx1)/rx1;
        float py1 = (my1-_ga_t1->getY())/ry1;

        trackball(axis,angle,px1,py1,px0,py0);

        osg::Matrix mat;
        mat.makeTranslate(-center.x(),-center.y(),-center.z());
        mat *= Matrix::rotate(angle,axis.x(),axis.y(),axis.z());
        mat *= Matrix::translate(center.x(),center.y(),center.z());

        _camera->transformLookAt(mat);

        return true;

    }
    else if (buttonMask==GUIEventAdapter::MIDDLE_BUTTON ||
        buttonMask==(GUIEventAdapter::LEFT_BUTTON|GUIEventAdapter::RIGHT_BUTTON))
    {

        // pan model.

        float scale = 0.0015f*focalLength;

        osg::Vec3 uv = _camera->getUpVector();
        osg::Vec3 sv = _camera->getSideVector();
        osg::Vec3 dv = uv*(dy*scale)-sv*(dx*scale);

        osg::Matrix mat;
        mat.makeTranslate(dv.x(),dv.y(),dv.z());

        _camera->transformLookAt(mat);

        return true;

    }
    else if (buttonMask==GUIEventAdapter::RIGHT_BUTTON)
    {

        // zoom model.

        float fd = focalLength;
        float scale = 1.0f-dy*0.001f;
        if (fd*scale>_modelScale*_minimumZoomScale)
        {
            // zoom camera in.
            osg::Vec3 center = _camera->getCenterPoint();

            osg::Matrix mat;
            mat.makeTranslate(-center.x(),-center.y(),-center.z());
            mat *= Matrix::scale(scale,scale,scale);
            mat *= Matrix::translate(center.x(),center.y(),center.z());

            _camera->transformLookAt(mat);

        }
        else
        {

            //            notify(DEBUG_INFO) << "Pushing forward"<<std::endl;
            // push the camera forward.
            float scale = 0.0015f*fd;
            osg::Vec3 dv = _camera->getLookVector()*(dy*scale);

            osg::Matrix mat;
            mat.makeTranslate(dv.x(),dv.y(),dv.z());

            _camera->transformLookAt(mat);

        }

        return true;

    }

    return false;
}

/*
 * This size should really be based on the distance from the center of
 * rotation to the point on the object underneath the mouse.  That
 * point would then track the mouse as closely as possible.  This is a
 * simple example, though, so that is left as an Exercise for the
 * Programmer.
 */
const float TRACKBALLSIZE = 0.8f;

/*
 * Ok, simulate a track-ball.  Project the points onto the virtual
 * trackball, then figure out the axis of rotation, which is the cross
 * product of P1 P2 and O P1 (O is the center of the ball, 0,0,0)
 * Note:  This is a deformed trackball-- is a trackball in the center,
 * but is deformed into a hyperbolic sheet of rotation away from the
 * center.  This particular function was chosen after trying out
 * several variations.
 *
 * It is assumed that the arguments to this routine are in the range
 * (-1.0 ... 1.0)
 */
void TrackballManipulator::trackball(osg::Vec3& axis,float& angle, float p1x, float p1y, float p2x, float p2y)
{
    /*
     * First, figure out z-coordinates for projection of P1 and P2 to
     * deformed sphere
     */

    osg::Vec3 uv = _camera->getUpVector();
    osg::Vec3 sv = _camera->getSideVector();
    osg::Vec3 lv = _camera->getLookVector();

    osg::Vec3 p1 = sv*p1x+uv*p1y-lv*tb_project_to_sphere(TRACKBALLSIZE,p1x,p1y);
    osg::Vec3 p2 = sv*p2x+uv*p2y-lv*tb_project_to_sphere(TRACKBALLSIZE,p2x,p2y);

    /*
     *  Now, we want the cross product of P1 and P2
     */

// Robert,
//
// This was the quick 'n' dirty  fix to get the trackball doing the right
// thing after fixing the Quat rotations to be right-handed.  You may want
// to do something more elegant.
//   axis = p1^p2;
axis = p2^p1;
    axis.normalize();

    /*
     *  Figure out how much to rotate around that axis.
     */
    float t = (p2-p1).length() / (2.0*TRACKBALLSIZE);

    /*
     * Avoid problems with out-of-control values...
     */
    if (t > 1.0) t = 1.0;
    if (t < -1.0) t = -1.0;
    angle = inRadians(asin(t));

}

/*
 * Project an x,y pair onto a sphere of radius r OR a hyperbolic sheet
 * if we are away from the center of the sphere.
 */
float TrackballManipulator::tb_project_to_sphere(float r, float x, float y)
{
    float d, t, z;

    d = sqrt(x*x + y*y);
                                 /* Inside sphere */
    if (d < r * 0.70710678118654752440)
    {
        z = sqrt(r*r - d*d);
    }                            /* On hyperbola */
    else
    {
        t = r / 1.41421356237309504880;
        z = t*t / d;
    }
    return z;
}
<|endoftext|>


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

#ifndef _DTRANS_GENERIC_CLIPBOARD_HXX_
#define _DTRANS_GENERIC_CLIPBOARD_HXX_

#ifndef _CPPUHELPER_COMPBASE4_HXX_
#include <cppuhelper/compbase4.hxx>
#endif

#ifndef _COM_SUN_STAR_DATATRANSFER_CLIPBAORD_XCLIPBOARDEX_HPP_
#include <com/sun/star/datatransfer/clipboard/XClipboardEx.hpp>
#endif

#ifndef _COM_SUN_STAR_DATATRANSFER_CLIPBAORD_XCLIPBOARDNOTIFIER_HPP_
#include <com/sun/star/datatransfer/clipboard/XClipboardNotifier.hpp>
#endif

#ifndef _COM_SUN_STAR_LANG_XSERVICEINFO_HPP_
#include <com/sun/star/lang/XServiceInfo.hpp>
#endif

#ifndef _COM_SUN_STAR_LANG_XINITIALIZATION_HPP_
#include <com/sun/star/lang/XInitialization.hpp>
#endif

// ------------------------------------------------------------------------

#define GENERIC_CLIPBOARD_IMPLEMENTATION_NAME "com.sun.star.comp.datatransfer.clipboard.GenericClipboard"

// ------------------------------------------------------------------------

namespace dtrans
{

    class GenericClipboard : public ::cppu::WeakComponentImplHelper4 < \
    ::com::sun::star::datatransfer::clipboard::XClipboardEx, \
    ::com::sun::star::datatransfer::clipboard::XClipboardNotifier, \
    ::com::sun::star::lang::XServiceInfo, \
    ::com::sun::star::lang::XInitialization >
    {
        ::osl::Mutex m_aMutex;
        ::rtl::OUString m_aName;

        ::com::sun::star::uno::Reference< ::com::sun::star::datatransfer::XTransferable > m_aContents;
        ::com::sun::star::uno::Reference< ::com::sun::star::datatransfer::clipboard::XClipboardOwner > m_aOwner;

        sal_Bool m_bInitialized;
        virtual ~GenericClipboard();

    public:

        GenericClipboard();

        /*
         * XInitialization
         */

        virtual void SAL_CALL initialize( const ::com::sun::star::uno::Sequence< ::com::sun::star::uno::Any >& aArguments )
            throw(::com::sun::star::uno::Exception, ::com::sun::star::uno::RuntimeException);

        /*
         * XServiceInfo
         */

        virtual ::rtl::OUString SAL_CALL getImplementationName(  )
            throw(::com::sun::star::uno::RuntimeException);

        virtual sal_Bool SAL_CALL supportsService( const ::rtl::OUString& ServiceName )
            throw(::com::sun::star::uno::RuntimeException);

        virtual ::com::sun::star::uno::Sequence< ::rtl::OUString > SAL_CALL getSupportedServiceNames(  )
            throw(::com::sun::star::uno::RuntimeException);

        /*
         * XClipboard
         */

        virtual ::com::sun::star::uno::Reference< ::com::sun::star::datatransfer::XTransferable > SAL_CALL getContents()
            throw(::com::sun::star::uno::RuntimeException);

        virtual void SAL_CALL setContents(
            const ::com::sun::star::uno::Reference< ::com::sun::star::datatransfer::XTransferable >& xTrans,
            const ::com::sun::star::uno::Reference< ::com::sun::star::datatransfer::clipboard::XClipboardOwner >& xClipboardOwner )
            throw(::com::sun::star::uno::RuntimeException);

        virtual ::rtl::OUString SAL_CALL getName()
            throw(::com::sun::star::uno::RuntimeException);

        /*
         * XClipboardEx
         */

        virtual sal_Int8 SAL_CALL getRenderingCapabilities()
            throw(::com::sun::star::uno::RuntimeException);

        /*
         * XClipboardNotifier
         */

        virtual void SAL_CALL addClipboardListener(
            const ::com::sun::star::uno::Reference< ::com::sun::star::datatransfer::clipboard::XClipboardListener >& listener )
            throw(::com::sun::star::uno::RuntimeException);

        virtual void SAL_CALL removeClipboardListener(
            const ::com::sun::star::uno::Reference< ::com::sun::star::datatransfer::clipboard::XClipboardListener >& listener )
            throw(::com::sun::star::uno::RuntimeException);

    };

}

// ------------------------------------------------------------------------

::com::sun::star::uno::Sequence< ::rtl::OUString > SAL_CALL GenericClipboard_getSupportedServiceNames();
::com::sun::star::uno::Reference< ::com::sun::star::uno::XInterface > SAL_CALL GenericClipboard_createInstance(
    const ::com::sun::star::uno::Reference< ::com::sun::star::lang::XMultiServiceFactory > & xMultiServiceFactory);

#endif
<commit_msg>INTEGRATION: CWS changefileheader (1.4.76); FILE MERGED 2008/04/01 15:13:30 thb 1.4.76.3: #i85898# Stripping all external header guards 2008/04/01 12:28:28 thb 1.4.76.2: #i85898# Stripping all external header guards 2008/03/31 13:08:55 rt 1.4.76.1: #i87441# Change license header to LPGL v3.<commit_after>/*************************************************************************
 *
 * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
 *
 * Copyright 2008 by Sun Microsystems, Inc.
 *
 * OpenOffice.org - a multi-platform office productivity suite
 *
 * $RCSfile: generic_clipboard.hxx,v $
 * $Revision: 1.6 $
 *
 * This file is part of OpenOffice.org.
 *
 * OpenOffice.org is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License version 3
 * only, as published by the Free Software Foundation.
 *
 * OpenOffice.org 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 version 3 for more details
 * (a copy is included in the LICENSE file that accompanied this code).
 *
 * You should have received a copy of the GNU Lesser General Public License
 * version 3 along with OpenOffice.org.  If not, see
 * <http://www.openoffice.org/license.html>
 * for a copy of the LGPLv3 License.
 *
 ************************************************************************/

#ifndef _DTRANS_GENERIC_CLIPBOARD_HXX_
#define _DTRANS_GENERIC_CLIPBOARD_HXX_

#include <cppuhelper/compbase4.hxx>

#ifndef _COM_SUN_STAR_DATATRANSFER_CLIPBAORD_XCLIPBOARDEX_HPP_
#include <com/sun/star/datatransfer/clipboard/XClipboardEx.hpp>
#endif

#ifndef _COM_SUN_STAR_DATATRANSFER_CLIPBAORD_XCLIPBOARDNOTIFIER_HPP_
#include <com/sun/star/datatransfer/clipboard/XClipboardNotifier.hpp>
#endif
#include <com/sun/star/lang/XServiceInfo.hpp>
#include <com/sun/star/lang/XInitialization.hpp>

// ------------------------------------------------------------------------

#define GENERIC_CLIPBOARD_IMPLEMENTATION_NAME "com.sun.star.comp.datatransfer.clipboard.GenericClipboard"

// ------------------------------------------------------------------------

namespace dtrans
{

    class GenericClipboard : public ::cppu::WeakComponentImplHelper4 < \
    ::com::sun::star::datatransfer::clipboard::XClipboardEx, \
    ::com::sun::star::datatransfer::clipboard::XClipboardNotifier, \
    ::com::sun::star::lang::XServiceInfo, \
    ::com::sun::star::lang::XInitialization >
    {
        ::osl::Mutex m_aMutex;
        ::rtl::OUString m_aName;

        ::com::sun::star::uno::Reference< ::com::sun::star::datatransfer::XTransferable > m_aContents;
        ::com::sun::star::uno::Reference< ::com::sun::star::datatransfer::clipboard::XClipboardOwner > m_aOwner;

        sal_Bool m_bInitialized;
        virtual ~GenericClipboard();

    public:

        GenericClipboard();

        /*
         * XInitialization
         */

        virtual void SAL_CALL initialize( const ::com::sun::star::uno::Sequence< ::com::sun::star::uno::Any >& aArguments )
            throw(::com::sun::star::uno::Exception, ::com::sun::star::uno::RuntimeException);

        /*
         * XServiceInfo
         */

        virtual ::rtl::OUString SAL_CALL getImplementationName(  )
            throw(::com::sun::star::uno::RuntimeException);

        virtual sal_Bool SAL_CALL supportsService( const ::rtl::OUString& ServiceName )
            throw(::com::sun::star::uno::RuntimeException);

        virtual ::com::sun::star::uno::Sequence< ::rtl::OUString > SAL_CALL getSupportedServiceNames(  )
            throw(::com::sun::star::uno::RuntimeException);

        /*
         * XClipboard
         */

        virtual ::com::sun::star::uno::Reference< ::com::sun::star::datatransfer::XTransferable > SAL_CALL getContents()
            throw(::com::sun::star::uno::RuntimeException);

        virtual void SAL_CALL setContents(
            const ::com::sun::star::uno::Reference< ::com::sun::star::datatransfer::XTransferable >& xTrans,
            const ::com::sun::star::uno::Reference< ::com::sun::star::datatransfer::clipboard::XClipboardOwner >& xClipboardOwner )
            throw(::com::sun::star::uno::RuntimeException);

        virtual ::rtl::OUString SAL_CALL getName()
            throw(::com::sun::star::uno::RuntimeException);

        /*
         * XClipboardEx
         */

        virtual sal_Int8 SAL_CALL getRenderingCapabilities()
            throw(::com::sun::star::uno::RuntimeException);

        /*
         * XClipboardNotifier
         */

        virtual void SAL_CALL addClipboardListener(
            const ::com::sun::star::uno::Reference< ::com::sun::star::datatransfer::clipboard::XClipboardListener >& listener )
            throw(::com::sun::star::uno::RuntimeException);

        virtual void SAL_CALL removeClipboardListener(
            const ::com::sun::star::uno::Reference< ::com::sun::star::datatransfer::clipboard::XClipboardListener >& listener )
            throw(::com::sun::star::uno::RuntimeException);

    };

}

// ------------------------------------------------------------------------

::com::sun::star::uno::Sequence< ::rtl::OUString > SAL_CALL GenericClipboard_getSupportedServiceNames();
::com::sun::star::uno::Reference< ::com::sun::star::uno::XInterface > SAL_CALL GenericClipboard_createInstance(
    const ::com::sun::star::uno::Reference< ::com::sun::star::lang::XMultiServiceFactory > & xMultiServiceFactory);

#endif
<|endoftext|>


<commit_before>#include <ros/ros.h>
#include <string>

#include "EscortRobot.h"
#include "elderly_care_simulation/EventTrigger.h"
#include "elderly_care_simulation/FindPath.h"
#include "elderly_care_simulation/PerformTask.h"
#include "EventTriggerUtility.h"
#include "nav_msgs/Odometry.h"
#include "Robot.h"
#include "StaticPoiConstants.h"
#include "PerformTaskConstants.h"

EscortRobot::EscortRobot() {
    // No implementation
}

EscortRobot::EscortRobot(int escortEventType, geometry_msgs::Point escortBase, geometry_msgs::Point escortPoi) {
    currentLocationState = AT_BASE;
    eventType = escortEventType;
    base = escortBase;
    poi = escortPoi;
}

EscortRobot::~EscortRobot() {}

void EscortRobot::residentLocationCallback(nav_msgs::Odometry msg) {
    residentLocation = msg.pose.pose.position;
}

void EscortRobot::goToPoi() {
    currentLocationState = GOING_TO_POI;
    goToLocation(poi);
}

void EscortRobot::goToResident() {
    currentLocationState = GOING_TO_POI;
    goToLocation(residentLocation);
}

void EscortRobot::goToBase() {
    currentLocationState = GOING_TO_BASE;
    goToLocation(base);
}

void EscortRobot::eventTriggered(const elderly_care_simulation::EventTrigger msg) {
    if (msg.msg_type == EVENT_TRIGGER_MSG_TYPE_REQUEST) {
        if (msg.event_type == eventType) {
            if (currentLocationState == AT_BASE ||
                currentLocationState == GOING_TO_BASE) {

                currentLocationState = GOING_TO_POI;
                performingTask = true;

            }
        }
    }
}

void EscortRobot::eventFinished() {

    stopSpinning();
    goToBase();

    // create response message
    elderly_care_simulation::EventTrigger msg;
    msg.msg_type = EVENT_TRIGGER_MSG_TYPE_RESPONSE;

    msg.event_type = EVENT_TRIGGER_EVENT_TYPE_COOK;
    msg.event_priority = EVENT_TRIGGER_PRIORITY_UNDEFINED;
    msg.event_weight = getEventWeight(msg.event_type);
    msg.result = EVENT_TRIGGER_RESULT_SUCCESS;

    eventTriggerPub.publish(msg);
}

/**
 * Perform a task on the resident by making a service call to them.
 */
void EscortRobot::performTask() {

    // Generate the service call
    elderly_care_simulation::PerformTask performTaskService;
    performTaskService.request.taskType = eventType;
    performTaskService.request.taskRequiresPoi = true;

    performTaskService.request.taskPoi = poi;

    // Make the call using the client
    if (!performTaskClient.call(performTaskService)) {
        throw std::runtime_error("Service call to the initiate task with Resident failed");
    }

    switch (performTaskService.response.result) {
        case PERFORM_TASK_RESULT_ACCEPTED:
        {
            startSpinning(false);
            ROS_INFO("Resident has accepted the task but says keep going");
            break;
        }
        case PERFORM_TASK_RESULT_TAKE_ME_THERE:
        {
            currentLocationState = GOING_TO_POI;
            goToLocation(poi);
            ROS_INFO("Resident wants to be taken to the POI");
            break;
        }
        case PERFORM_TASK_RESULT_FINISHED:
        {
            performingTask = false;
            currentLocationState = GOING_TO_BASE;

            goToBase();

            stopSpinning();
            notifySchedulerOfTaskCompletion();

            ROS_INFO("Resident has accepted the task and has had enough");

            break;
        }
        case PERFORM_TASK_RESULT_BUSY:
        {
            ROS_INFO("Resident is busy");
            break;
        }
    }
}

void EscortRobot::notifySchedulerOfTaskCompletion() {
    // Create response message
    elderly_care_simulation::EventTrigger msg;
    msg.msg_type = EVENT_TRIGGER_MSG_TYPE_RESPONSE;

    msg.event_type = eventType;
    msg.event_priority = EVENT_TRIGGER_PRIORITY_UNDEFINED;
    msg.event_weight = getEventWeight(msg.event_type);
    msg.result = EVENT_TRIGGER_RESULT_SUCCESS;

    eventTriggerPub.publish(msg);
    ROS_INFO("Task Completion Message Sent");
}

int EscortRobot::execute() {
    ros::Rate loopRate(10);

    while (ros::ok()) {
        if (atDesiredLocation()) {
            switch (currentLocationState) {
                case GOING_TO_BASE:
                    currentLocationState = AT_BASE;
                    break;
                case GOING_TO_POI:
                    currentLocationState = AT_POI;
                    break;
                default:
                    break;
            }
        }

        if (performingTask && currentLocationState == AT_POI) {
            performTask();
        }

        updateCurrentVelocity();

        ros::spinOnce();
        loopRate.sleep();
    }

    return 0;
}

EscortRobot feeder;

void eventTriggeredCallback(elderly_care_simulation::EventTrigger msg) {
    feeder.eventTriggered(msg);
}

void stageCallBack(nav_msgs::Odometry msg) {
    feeder.stage0domCallback(msg);
}

void residentStageCallback(nav_msgs::Odometry msg) {
    feeder.residentLocationCallback(msg);
}

int main(int argc, char **argv) {

    geometry_msgs::Point base;
    base.x = 12.0f;
    base.y = 1.0f;

    geometry_msgs::Point table;
    table.x = ADJACENT_TABLE_X;
    table.y = ADJACENT_TABLE_Y;

    feeder = EscortRobot(EVENT_TRIGGER_EVENT_TYPE_EAT, base, table);

    const std::string rid = "robot_10";

    ros::init(argc, argv, "Feeding_Robot");

    // Node handle
    ros::NodeHandle chefNodeHandle;

    // Will publish geometry_msgs::Twist messages to the cmd_vel topic
    feeder.robotNodeStagePub = chefNodeHandle.advertise<geometry_msgs::Twist>(rid + "/cmd_vel", 1000);
    feeder.eventTriggerPub = chefNodeHandle.advertise<elderly_care_simulation::EventTrigger>("event_trigger", 1000, true);

    feeder.stageOdoSub = chefNodeHandle.subscribe<nav_msgs::Odometry>(rid + "/base_pose_ground_truth", 1000, stageCallBack);
    feeder.eventTriggerSub = chefNodeHandle.subscribe<elderly_care_simulation::EventTrigger>("event_trigger", 1000, eventTriggeredCallback);
    feeder.residentLocationSub = chefNodeHandle.subscribe<nav_msgs::Odometry>("robot_0/base_pose_ground_truth", 1000, residentStageCallback);

    // Service used to find paths
    feeder.pathFinderService = chefNodeHandle.serviceClient<elderly_care_simulation::FindPath>("find_path");

    // Service to perform tasks on the resident
    feeder.performTaskClient = chefNodeHandle.serviceClient<elderly_care_simulation::PerformTask>("perform_task");

    return feeder.execute();
}<commit_msg>Fixed bug which caused the EscortRobot to send tasks too early. [#54] [#56] <@mshafer | @nweatherburn><commit_after>#include <ros/ros.h>
#include <string>

#include "EscortRobot.h"
#include "elderly_care_simulation/EventTrigger.h"
#include "elderly_care_simulation/FindPath.h"
#include "elderly_care_simulation/PerformTask.h"
#include "EventTriggerUtility.h"
#include "nav_msgs/Odometry.h"
#include "Robot.h"
#include "StaticPoiConstants.h"
#include "PerformTaskConstants.h"

EscortRobot::EscortRobot() {
    // No implementation
}

EscortRobot::EscortRobot(int escortEventType, geometry_msgs::Point escortBase, geometry_msgs::Point escortPoi) {
    currentLocationState = AT_BASE;
    eventType = escortEventType;
    base = escortBase;
    poi = escortPoi;
}

EscortRobot::~EscortRobot() {}

void EscortRobot::residentLocationCallback(nav_msgs::Odometry msg) {
    residentLocation = msg.pose.pose.position;
}

void EscortRobot::goToPoi() {
    currentLocationState = GOING_TO_POI;
    goToLocation(poi);
}

void EscortRobot::goToResident() {
    currentLocationState = GOING_TO_POI;
    goToLocation(residentLocation);
}

void EscortRobot::goToBase() {
    currentLocationState = GOING_TO_BASE;
    goToLocation(base);
}

void EscortRobot::eventTriggered(const elderly_care_simulation::EventTrigger msg) {
    if (msg.msg_type == EVENT_TRIGGER_MSG_TYPE_REQUEST) {
        if (msg.event_type == eventType) {
            if (currentLocationState == AT_BASE ||
                currentLocationState == GOING_TO_BASE) {

                currentLocationState = GOING_TO_POI;
                performingTask = true;
                goToResident();

            }
        }
    }
}

void EscortRobot::eventFinished() {

    stopSpinning();
    goToBase();

    // create response message
    elderly_care_simulation::EventTrigger msg;
    msg.msg_type = EVENT_TRIGGER_MSG_TYPE_RESPONSE;

    msg.event_type = EVENT_TRIGGER_EVENT_TYPE_COOK;
    msg.event_priority = EVENT_TRIGGER_PRIORITY_UNDEFINED;
    msg.event_weight = getEventWeight(msg.event_type);
    msg.result = EVENT_TRIGGER_RESULT_SUCCESS;

    eventTriggerPub.publish(msg);
}

/**
 * Perform a task on the resident by making a service call to them.
 */
void EscortRobot::performTask() {

    // Generate the service call
    elderly_care_simulation::PerformTask performTaskService;
    performTaskService.request.taskType = eventType;
    performTaskService.request.taskRequiresPoi = true;

    performTaskService.request.taskPoi = poi;

    // Make the call using the client
    if (!performTaskClient.call(performTaskService)) {
        throw std::runtime_error("Service call to the initiate task with Resident failed");
    }

    switch (performTaskService.response.result) {
        case PERFORM_TASK_RESULT_ACCEPTED:
        {
            startSpinning(false);
            ROS_INFO("Resident has accepted the task but says keep going");
            break;
        }
        case PERFORM_TASK_RESULT_TAKE_ME_THERE:
        {
            currentLocationState = GOING_TO_POI;
            goToLocation(poi);
            ROS_INFO("Resident wants to be taken to the POI");
            break;
        }
        case PERFORM_TASK_RESULT_FINISHED:
        {
            performingTask = false;
            currentLocationState = GOING_TO_BASE;

            goToBase();

            stopSpinning();
            notifySchedulerOfTaskCompletion();

            ROS_INFO("Resident has accepted the task and has had enough");

            break;
        }
        case PERFORM_TASK_RESULT_BUSY:
        {
            ROS_INFO("Resident is busy");
            break;
        }
    }
}

void EscortRobot::notifySchedulerOfTaskCompletion() {
    // Create response message
    elderly_care_simulation::EventTrigger msg;
    msg.msg_type = EVENT_TRIGGER_MSG_TYPE_RESPONSE;

    msg.event_type = eventType;
    msg.event_priority = EVENT_TRIGGER_PRIORITY_UNDEFINED;
    msg.event_weight = getEventWeight(msg.event_type);
    msg.result = EVENT_TRIGGER_RESULT_SUCCESS;

    eventTriggerPub.publish(msg);
    ROS_INFO("Task Completion Message Sent");
}

int EscortRobot::execute() {
    ros::Rate loopRate(10);

    while (ros::ok()) {
        if (atDesiredLocation()) {
            switch (currentLocationState) {
                case GOING_TO_BASE:
                    currentLocationState = AT_BASE;
                    break;
                case GOING_TO_POI:
                    currentLocationState = AT_POI;
                    break;
                default:
                    break;
            }

            if (performingTask && currentLocationState == AT_POI) {
                performTask();
            }
        }

        updateCurrentVelocity();

        ros::spinOnce();
        loopRate.sleep();
    }

    return 0;
}

EscortRobot feeder;

void eventTriggeredCallback(elderly_care_simulation::EventTrigger msg) {
    feeder.eventTriggered(msg);
}

void stageCallBack(nav_msgs::Odometry msg) {
    feeder.stage0domCallback(msg);
}

void residentStageCallback(nav_msgs::Odometry msg) {
    feeder.residentLocationCallback(msg);
}

int main(int argc, char **argv) {

    geometry_msgs::Point base;
    base.x = 12.0f;
    base.y = 1.0f;

    geometry_msgs::Point table;
    table.x = ADJACENT_TABLE_X;
    table.y = ADJACENT_TABLE_Y;

    feeder = EscortRobot(EVENT_TRIGGER_EVENT_TYPE_EAT, base, table);

    const std::string rid = "robot_10";

    ros::init(argc, argv, "Feeding_Robot");

    // Node handle
    ros::NodeHandle chefNodeHandle;

    // Will publish geometry_msgs::Twist messages to the cmd_vel topic
    feeder.robotNodeStagePub = chefNodeHandle.advertise<geometry_msgs::Twist>(rid + "/cmd_vel", 1000);
    feeder.eventTriggerPub = chefNodeHandle.advertise<elderly_care_simulation::EventTrigger>("event_trigger", 1000, true);

    feeder.stageOdoSub = chefNodeHandle.subscribe<nav_msgs::Odometry>(rid + "/base_pose_ground_truth", 1000, stageCallBack);
    feeder.eventTriggerSub = chefNodeHandle.subscribe<elderly_care_simulation::EventTrigger>("event_trigger", 1000, eventTriggeredCallback);
    feeder.residentLocationSub = chefNodeHandle.subscribe<nav_msgs::Odometry>("robot_0/base_pose_ground_truth", 1000, residentStageCallback);

    // Service used to find paths
    feeder.pathFinderService = chefNodeHandle.serviceClient<elderly_care_simulation::FindPath>("find_path");

    // Service to perform tasks on the resident
    feeder.performTaskClient = chefNodeHandle.serviceClient<elderly_care_simulation::PerformTask>("perform_task");

    return feeder.execute();
}<|endoftext|>