<commit_before>// __BEGIN_LICENSE__
//
// Copyright (C) 2008 United States Government as represented by the
// Administrator of the National Aeronautics and Space Administration
// (NASA).  All Rights Reserved.
//
// Copyright 2008 Carnegie Mellon University. All rights reserved.
//
// This software is distributed under the NASA Open Source Agreement
// (NOSA), version 1.3.  The NOSA has been approved by the Open Source
// Initiative.  See the file COPYING at the top of the distribution
// directory tree for the complete NOSA document.
//
// THE SUBJECT SOFTWARE IS PROVIDED "AS IS" WITHOUT ANY WARRANTY OF ANY
// KIND, EITHER EXPRESSED, IMPLIED, OR STATUTORY, INCLUDING, BUT NOT
// LIMITED TO, ANY WARRANTY THAT THE SUBJECT SOFTWARE WILL CONFORM TO
// SPECIFICATIONS, ANY IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR
// A PARTICULAR PURPOSE, OR FREEDOM FROM INFRINGEMENT, ANY WARRANTY THAT
// THE SUBJECT SOFTWARE WILL BE ERROR FREE, OR ANY WARRANTY THAT
// DOCUMENTATION, IF PROVIDED, WILL CONFORM TO THE SUBJECT SOFTWARE.
//
// __END_LICENSE__

/// \file ControlNetworkLoader.cc
///
/// Control Network Loader has one function available to the public
/// and that is build_control_network at the bottom. It will load all
/// match files and triangulate all control points for the control
/// network giving by pointer.

/// Control Network Loader aims to be faster than previous method by
/// using an intermediate data struct called the Camera Relations
/// Network. It reduces the search length of inserting a new match
/// file by organizing it's base layer by camera. Compared to the
/// previous method this should reduce the search length 1/(# cameras)
/// * what ever it was before.

#include "ControlNetworkLoader.h"

// Standard
#include <vector>
#include <iostream>
#include <string>

// VW
#include <vw/Camera/ControlNetwork.h>
#include <vw/Camera/CameraModel.h>
#include <vw/Stereo/StereoModel.h>
#include <vw/Cartography.h>
#include <vw/InterestPoint.h>
#include <vw/Math.h>
#include <vw/Core.h>

// Boost (for good measure)
#include "boost/filesystem/operations.hpp"
#include "boost/filesystem/fstream.hpp"
#include <boost/algorithm/string.hpp>
namespace fs = boost::filesystem;

using namespace vw;
using namespace vw::camera;
using namespace vw::stereo;
using namespace vw::ip;

  /////////////////////////////
 // Prototypes and Snippets //
/////////////////////////////

static std::string prefix_from_filename( std::string const& filename ){
  std::string result = filename;
  int index = result.find(".");
  if (index != -1)
    result.erase(index, result.size());
  return result;
}

static std::string remove_path( std::string const& filename ){
  std::string result = filename;
  int index = result.rfind("/");
  if (index != -1)
    result.erase(0,index+1);
  return result;
}

// Secondary Layer of Camera Relations Network. This contains the
// match relations and is doubly linked.
struct InterestPtRelation {
  boost::shared_ptr<InterestPoint> ip;
  typedef boost::shared_ptr<InterestPtRelation> ipr;
  std::list<ipr> matches;
  unsigned camera_id;

  InterestPtRelation ( InterestPoint& mine, unsigned& camera ) : camera_id(camera) { ip = boost::shared_ptr<InterestPoint>(new InterestPoint(mine)); }
  void AttachIpr( ipr& m );
  void RecursiveListing( std::list<ipr>& listing );
};

// Base Layer of Camera Relations Network
struct CameraRelation {
  typedef boost::shared_ptr<InterestPtRelation> ipr;
  int id;
  std::string name;
  std::list<ipr> relations;

  CameraRelation ( int tid, std::string tname ): id(tid), name(tname) {}
  ipr FindMatching( InterestPoint const& key );
  void AttachIpr( boost::shared_ptr<InterestPtRelation>& m );

};

  /////////////////////////////
 // Comparisons             //
/////////////////////////////

inline bool operator==( InterestPtRelation const& ipr1,
			InterestPtRelation const& ipr2 ) {
  if ( ipr1.ip->ix == ipr2.ip->ix &&
       ipr1.ip->iy == ipr2.ip->iy )
    return true;
  return false;
}

inline bool operator==( InterestPtRelation const& ipr1,
			InterestPoint const& ip2 ) {
  if ( ipr1.ip->ix == ip2.ix &&
       ipr1.ip->iy == ip2.iy )
    return true;
  return false;
}

  /////////////////////////////
 // Implementations         //
/////////////////////////////

// Interest Pt Relations
//---------------------------

void InterestPtRelation::AttachIpr( ipr& m ) {
  for ( std::list<ipr>::iterator iter =
	  matches.begin(); iter != matches.end(); iter++ ) {
    if ( **iter == *m )
      return;
  }
  matches.push_back( m );
}

void InterestPtRelation::RecursiveListing( std::list<ipr>& listing ) {

  for ( std::list<ipr>::iterator iter = matches.begin();
	iter != matches.end(); iter++ ) {
    bool contains = false;
    for ( std::list<ipr>::iterator from_list = listing.begin();
	  from_list != listing.end(); from_list++ ) {
      if ( *from_list == *iter ) {
	contains = true;
	break;
      }
    }

    if ( !contains ) {
      listing.push_back( *iter );
      (*iter)->RecursiveListing( listing );
    }
  }
}

// Camera Relations
//---------------------------

boost::shared_ptr<InterestPtRelation> CameraRelation::FindMatching( InterestPoint const& key ) {
  for ( std::list<ipr>::iterator iter = relations.begin();
	iter != relations.end(); iter++ ) {
    if ( **iter == key )
      return *iter;
  }
  return ipr();
}

void CameraRelation::AttachIpr( boost::shared_ptr<InterestPtRelation>& m ) {
  /*
  for ( std::list<ipr>::iterator iter = m_relations.begin();
	iter != m_relations.end(); iter++ ) {
    if ( **iter == *m )
      return;
  }
  */
  relations.push_back( m );
}

// Functions for Users
//----------------------------
void build_control_network( boost::shared_ptr<ControlNetwork> cnet,
			    std::vector<boost::shared_ptr<vw::camera::CameraModel> > const& camera_models,
			    std::vector<std::string> image_files,
			    int min_matches ) {

  cnet->clear();

  // 1.) Build CRN and load matches
  std::vector<CameraRelation> crn;
  for ( unsigned i = 0; i < image_files.size(); i++ )
    crn.push_back( CameraRelation( i,
				   prefix_from_filename( image_files[i] )));
  vw_out(0) << "Loading matches:\n";
  for ( unsigned i = 0; i < image_files.size(); ++i ) {
    for ( unsigned j = i+1; j < image_files.size(); ++j ) {
      std::string match_filename =
	prefix_from_filename( image_files[i] ) + "__" +
	prefix_from_filename( image_files[j] ) + ".match";

      if ( !fs::exists( match_filename ) ) {
	match_filename = remove_path(prefix_from_filename( image_files[i] ) )
	  + "__" + remove_path(prefix_from_filename(image_files[j]))+".match";
	if (!fs::exists( match_filename))
	  continue;
      }

      std::vector<InterestPoint> ip1, ip2;
      read_binary_match_file( match_filename, ip1, ip2 );
      if ( int( ip1.size() ) < min_matches ) {
	vw_out(0) << "\t" << match_filename << "    " << i << " <-> "
		  << j << " : " << ip1.size() << " matches. [rejected]\n";
      } else {
	vw_out(0) << "\t" << match_filename << "    " << i << " <-> "
		  << j << " : " << ip1.size() << " matches.\n";

	// Loading individual matches now
	for ( unsigned k = 0; k < ip1.size(); k++ ) {
	  boost::shared_ptr<InterestPtRelation> ipr1, ipr2;
	  ipr1 = crn[i].FindMatching( ip1[k] );
	  ipr2 = crn[j].FindMatching( ip2[k] );

	  // Match hasn't been previously loaded
	  if ( ipr1 == boost::shared_ptr<InterestPtRelation>() ) {
	    ipr1 = boost::shared_ptr<InterestPtRelation>( new InterestPtRelation( ip1[k], i ) );
	    crn[i].AttachIpr( ipr1 );
	  }
	  if ( ipr2 == boost::shared_ptr<InterestPtRelation>() ) {
	    ipr2 = boost::shared_ptr<InterestPtRelation>( new InterestPtRelation( ip2[k], j ) );
	    crn[j].AttachIpr( ipr2 );
	  }

	  // Doubly linking
	  ipr1->AttachIpr( ipr2 );
	  ipr2->AttachIpr( ipr1 );
	}
      }
    }
  }

  // 2.) Build Control Network finally
  int spiral_error_count = 0;
  vw_out(0) << "Assembling Control Network:\n";
  for ( unsigned i = 0; i < crn.size() -1; ++i ) {
    typedef boost::shared_ptr<InterestPtRelation> ipr;

    // Iterating over match relations and building control points for
    // them.
    for ( std::list<ipr>::iterator iter = crn[i].relations.begin();
	  iter != crn[i].relations.end(); iter++ ) {
      // 2.1) Building a listing of interest points for a control
      // point
      std::list<ipr> interestpts;
      interestpts.push_back(*iter);
      (*iter)->RecursiveListing( interestpts );

      ControlPoint cpoint( ControlPoint::TiePoint );

      // Now removing listed interest points from CRN, and then adding
      // control measure
      // 2.2) Adding this location
      {
	ControlMeasure m( (*iter)->ip->x,
			  (*iter)->ip->y,
			  (*iter)->ip->scale,
			  (*iter)->ip->scale,
			  (*iter)->camera_id );
	cpoint.add_measure( m );
      }
      // 2.3) Adding & Removing all the other locations
      std::list<ipr>::iterator measure = interestpts.begin();
      measure++;
      for ( ; measure != interestpts.end(); measure++ ) {
	crn[(*measure)->camera_id].relations.remove(*measure);
	ControlMeasure m( (*measure)->ip->x,
			  (*measure)->ip->y,
			  (*measure)->ip->scale,
			  (*measure)->ip->scale,
			  (*measure)->camera_id );
	cpoint.add_measure( m );
      }
      // 2.4) Removing this location
      iter = crn[i].relations.erase(iter);
      iter--;
      // 2.5) Checking for a spiral error
      {
	std::list<unsigned> previous_camera;
	bool match = false;
	for ( std::list<ipr>::iterator interest = interestpts.begin();
	      interest != interestpts.end(); interest++ ) {
	  for ( std::list<unsigned>::iterator previous = previous_camera.begin();
		previous != previous_camera.end(); previous++ ) {
	    if ((*previous) == (*interest)->camera_id) {
	      match = true;
	      break;
	    }
	  }
	  previous_camera.push_back( (*interest)->camera_id );
	  if ( match ) {
	    continue;
	  }
	}
	if ( match ) {
	  spiral_error_count++;
	  continue;
	}
      }

      // 2.6) If haven't left for spiral error, add new control point
      if ( cpoint.size() > 1 )
	cnet->add_control_point( cpoint );
    }
  }
  if ( spiral_error_count != 0 )
    vw_out(0) << "\t" << spiral_error_count << " control points removed because of spiral errors.\n";

  VW_ASSERT( cnet->size() != 0, vw::Aborted() << "Failed to load any points, Control Network Empty\n");

  // 3.) Triangulating Positions
  for ( unsigned i = 0; i < cnet->size(); i++ ) {

    std::vector< Vector3 > positions;
    double error = 0, error_sum = 0;

    const double min_convergence_angle = 5.0*3.1415/180.0;

    // 3.1) Building a permutation listing of triangulation
    for ( unsigned j = 0; j < (*cnet)[i].size(); j++ ) {
      for ( unsigned k = j+1; k < (*cnet)[j].size(); k++ ) {

	StereoModel sm( *(camera_models[ (*cnet)[i][j].image_id()]),
			*(camera_models[ (*cnet)[i][k].image_id()]) );

	if ( sm.convergence_angle( (*cnet)[i][j].position(),
				   (*cnet)[i][k].position() ) >
	     min_convergence_angle ) {
	  positions.push_back( sm( (*cnet)[i][j].position(),
				   (*cnet)[i][k].position(),
				   error ) );
	  error_sum += error;
	}
      }
    }

    // 3.2) Summing, Averaging and Storing
    error_sum /= positions.size();
    Vector3 position_avg;
    for ( unsigned j = 0; j < positions.size(); j++ )
      position_avg += positions[j];
    position_avg /= positions.size();

    (*cnet)[i].set_position( position_avg );

  }
}

void add_ground_control_points( boost::shared_ptr<vw::camera::ControlNetwork> cnet,
				std::vector<std::string> image_files ) {
  std::cout << "\nLoading Ground Control Points:\n";
  for ( unsigned i = 0; i < image_files.size(); ++i ) {
    std::string potential_gcp = prefix_from_filename( image_files[i] ) + ".gcp";

    if ( fs::exists( potential_gcp ) ) {
      int count = 0;

      // Now Loading the Ground Control Points
      std::ifstream istr(potential_gcp.c_str());

      while (!istr.eof()) {
	Vector2 pix;
	Vector3 loc;
	Vector3 sigma;

	istr >> pix[0] >> pix[1] >> loc[0] >> loc[1] >> loc[2] >> sigma[0] >> sigma[1] >> sigma[2];
	if (loc[0] !=0) {  // ignore blank lines
	  Vector3 xyz = cartography::lon_lat_radius_to_xyz(loc);
	  std::cout << "GCP: " << xyz << "\n";

	  ControlMeasure m(pix[0], pix[1], 1.0, 1.0, i);
	  ControlPoint cpoint(ControlPoint::GroundControlPoint);
	  cpoint.set_position(xyz[0],xyz[1],xyz[2]);
	  cpoint.set_sigma(sigma[0],sigma[1],sigma[2]);
	  cpoint.add_measure(m);

	  // Deciding where to put it
	  bool just_append = true;
	  for ( unsigned p = 0; p < cnet->size(); ++p) {
	    if ((*cnet)[p].type() == ControlPoint::GroundControlPoint ) {
	      if (((*cnet)[p].position()[0] - xyz[0]) < .001 &&
		  ((*cnet)[p].position()[1] - xyz[1]) < .001 &&
		  ((*cnet)[p].position()[2] - xyz[2]) < .001) {
		// I'm not checking measures because there is only one GCP
		// file per camera
		just_append = false;
		(*cnet)[p].add_measure(m);
		break;
	      }

	    }
	  }

	  if (just_append)
	    cnet->add_control_point(cpoint);

	  ++count;
	}
      }
      istr.close();

      std::cout << "\t" << potential_gcp << "    " << " : "
	      << count << " GCPs.\n";
    }
  }
}
<commit_msg>Typo in the code.<commit_after>// __BEGIN_LICENSE__
//
// Copyright (C) 2008 United States Government as represented by the
// Administrator of the National Aeronautics and Space Administration
// (NASA).  All Rights Reserved.
//
// Copyright 2008 Carnegie Mellon University. All rights reserved.
//
// This software is distributed under the NASA Open Source Agreement
// (NOSA), version 1.3.  The NOSA has been approved by the Open Source
// Initiative.  See the file COPYING at the top of the distribution
// directory tree for the complete NOSA document.
//
// THE SUBJECT SOFTWARE IS PROVIDED "AS IS" WITHOUT ANY WARRANTY OF ANY
// KIND, EITHER EXPRESSED, IMPLIED, OR STATUTORY, INCLUDING, BUT NOT
// LIMITED TO, ANY WARRANTY THAT THE SUBJECT SOFTWARE WILL CONFORM TO
// SPECIFICATIONS, ANY IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR
// A PARTICULAR PURPOSE, OR FREEDOM FROM INFRINGEMENT, ANY WARRANTY THAT
// THE SUBJECT SOFTWARE WILL BE ERROR FREE, OR ANY WARRANTY THAT
// DOCUMENTATION, IF PROVIDED, WILL CONFORM TO THE SUBJECT SOFTWARE.
//
// __END_LICENSE__

/// \file ControlNetworkLoader.cc
///
/// Control Network Loader has one function available to the public
/// and that is build_control_network at the bottom. It will load all
/// match files and triangulate all control points for the control
/// network giving by pointer.

/// Control Network Loader aims to be faster than previous method by
/// using an intermediate data struct called the Camera Relations
/// Network. It reduces the search length of inserting a new match
/// file by organizing it's base layer by camera. Compared to the
/// previous method this should reduce the search length 1/(# cameras)
/// * what ever it was before.

#include "ControlNetworkLoader.h"

// Standard
#include <vector>
#include <iostream>
#include <string>

// VW
#include <vw/Camera/ControlNetwork.h>
#include <vw/Camera/CameraModel.h>
#include <vw/Stereo/StereoModel.h>
#include <vw/Cartography.h>
#include <vw/InterestPoint.h>
#include <vw/Math.h>
#include <vw/Core.h>

// Boost (for good measure)
#include "boost/filesystem/operations.hpp"
#include "boost/filesystem/fstream.hpp"
#include <boost/algorithm/string.hpp>
namespace fs = boost::filesystem;

using namespace vw;
using namespace vw::camera;
using namespace vw::stereo;
using namespace vw::ip;

  /////////////////////////////
 // Prototypes and Snippets //
/////////////////////////////

static std::string prefix_from_filename( std::string const& filename ){
  std::string result = filename;
  int index = result.find(".");
  if (index != -1)
    result.erase(index, result.size());
  return result;
}

static std::string remove_path( std::string const& filename ){
  std::string result = filename;
  int index = result.rfind("/");
  if (index != -1)
    result.erase(0,index+1);
  return result;
}

// Secondary Layer of Camera Relations Network. This contains the
// match relations and is doubly linked.
struct InterestPtRelation {
  boost::shared_ptr<InterestPoint> ip;
  typedef boost::shared_ptr<InterestPtRelation> ipr;
  std::list<ipr> matches;
  unsigned camera_id;

  InterestPtRelation ( InterestPoint& mine, unsigned& camera ) : camera_id(camera) { ip = boost::shared_ptr<InterestPoint>(new InterestPoint(mine)); }
  void AttachIpr( ipr& m );
  void RecursiveListing( std::list<ipr>& listing );
};

// Base Layer of Camera Relations Network
struct CameraRelation {
  typedef boost::shared_ptr<InterestPtRelation> ipr;
  int id;
  std::string name;
  std::list<ipr> relations;

  CameraRelation ( int tid, std::string tname ): id(tid), name(tname) {}
  ipr FindMatching( InterestPoint const& key );
  void AttachIpr( boost::shared_ptr<InterestPtRelation>& m );

};

  /////////////////////////////
 // Comparisons             //
/////////////////////////////

inline bool operator==( InterestPtRelation const& ipr1,
			InterestPtRelation const& ipr2 ) {
  if ( ipr1.ip->ix == ipr2.ip->ix &&
       ipr1.ip->iy == ipr2.ip->iy )
    return true;
  return false;
}

inline bool operator==( InterestPtRelation const& ipr1,
			InterestPoint const& ip2 ) {
  if ( ipr1.ip->ix == ip2.ix &&
       ipr1.ip->iy == ip2.iy )
    return true;
  return false;
}

  /////////////////////////////
 // Implementations         //
/////////////////////////////

// Interest Pt Relations
//---------------------------

void InterestPtRelation::AttachIpr( ipr& m ) {
  for ( std::list<ipr>::iterator iter =
	  matches.begin(); iter != matches.end(); iter++ ) {
    if ( **iter == *m )
      return;
  }
  matches.push_back( m );
}

void InterestPtRelation::RecursiveListing( std::list<ipr>& listing ) {

  for ( std::list<ipr>::iterator iter = matches.begin();
	iter != matches.end(); iter++ ) {
    bool contains = false;
    for ( std::list<ipr>::iterator from_list = listing.begin();
	  from_list != listing.end(); from_list++ ) {
      if ( *from_list == *iter ) {
	contains = true;
	break;
      }
    }

    if ( !contains ) {
      listing.push_back( *iter );
      (*iter)->RecursiveListing( listing );
    }
  }
}

// Camera Relations
//---------------------------

boost::shared_ptr<InterestPtRelation> CameraRelation::FindMatching( InterestPoint const& key ) {
  for ( std::list<ipr>::iterator iter = relations.begin();
	iter != relations.end(); iter++ ) {
    if ( **iter == key )
      return *iter;
  }
  return ipr();
}

void CameraRelation::AttachIpr( boost::shared_ptr<InterestPtRelation>& m ) {
  /*
  for ( std::list<ipr>::iterator iter = m_relations.begin();
	iter != m_relations.end(); iter++ ) {
    if ( **iter == *m )
      return;
  }
  */
  relations.push_back( m );
}

// Functions for Users
//----------------------------
void build_control_network( boost::shared_ptr<ControlNetwork> cnet,
			    std::vector<boost::shared_ptr<vw::camera::CameraModel> > const& camera_models,
			    std::vector<std::string> image_files,
			    int min_matches ) {

  cnet->clear();

  // 1.) Build CRN and load matches
  std::vector<CameraRelation> crn;
  for ( unsigned i = 0; i < image_files.size(); i++ )
    crn.push_back( CameraRelation( i,
				   prefix_from_filename( image_files[i] )));
  vw_out(0) << "Loading matches:\n";
  for ( unsigned i = 0; i < image_files.size(); ++i ) {
    for ( unsigned j = i+1; j < image_files.size(); ++j ) {
      std::string match_filename =
	prefix_from_filename( image_files[i] ) + "__" +
	prefix_from_filename( image_files[j] ) + ".match";

      if ( !fs::exists( match_filename ) ) {
	match_filename = remove_path(prefix_from_filename( image_files[i] ) )
	  + "__" + remove_path(prefix_from_filename(image_files[j]))+".match";
	if (!fs::exists( match_filename))
	  continue;
      }

      std::vector<InterestPoint> ip1, ip2;
      read_binary_match_file( match_filename, ip1, ip2 );
      if ( int( ip1.size() ) < min_matches ) {
	vw_out(0) << "\t" << match_filename << "    " << i << " <-> "
		  << j << " : " << ip1.size() << " matches. [rejected]\n";
      } else {
	vw_out(0) << "\t" << match_filename << "    " << i << " <-> "
		  << j << " : " << ip1.size() << " matches.\n";

	// Loading individual matches now
	for ( unsigned k = 0; k < ip1.size(); k++ ) {
	  boost::shared_ptr<InterestPtRelation> ipr1, ipr2;
	  ipr1 = crn[i].FindMatching( ip1[k] );
	  ipr2 = crn[j].FindMatching( ip2[k] );

	  // Match hasn't been previously loaded
	  if ( ipr1 == boost::shared_ptr<InterestPtRelation>() ) {
	    ipr1 = boost::shared_ptr<InterestPtRelation>( new InterestPtRelation( ip1[k], i ) );
	    crn[i].AttachIpr( ipr1 );
	  }
	  if ( ipr2 == boost::shared_ptr<InterestPtRelation>() ) {
	    ipr2 = boost::shared_ptr<InterestPtRelation>( new InterestPtRelation( ip2[k], j ) );
	    crn[j].AttachIpr( ipr2 );
	  }

	  // Doubly linking
	  ipr1->AttachIpr( ipr2 );
	  ipr2->AttachIpr( ipr1 );
	}
      }
    }
  }

  // 2.) Build Control Network finally
  int spiral_error_count = 0;
  vw_out(0) << "Assembling Control Network:\n";
  for ( unsigned i = 0; i < crn.size() -1; ++i ) {
    typedef boost::shared_ptr<InterestPtRelation> ipr;

    // Iterating over match relations and building control points for
    // them.
    for ( std::list<ipr>::iterator iter = crn[i].relations.begin();
	  iter != crn[i].relations.end(); iter++ ) {
      // 2.1) Building a listing of interest points for a control
      // point
      std::list<ipr> interestpts;
      interestpts.push_back(*iter);
      (*iter)->RecursiveListing( interestpts );

      ControlPoint cpoint( ControlPoint::TiePoint );

      // Now removing listed interest points from CRN, and then adding
      // control measure
      // 2.2) Adding this location
      {
	ControlMeasure m( (*iter)->ip->x,
			  (*iter)->ip->y,
			  (*iter)->ip->scale,
			  (*iter)->ip->scale,
			  (*iter)->camera_id );
	cpoint.add_measure( m );
      }
      // 2.3) Adding & Removing all the other locations
      std::list<ipr>::iterator measure = interestpts.begin();
      measure++;
      for ( ; measure != interestpts.end(); measure++ ) {
	crn[(*measure)->camera_id].relations.remove(*measure);
	ControlMeasure m( (*measure)->ip->x,
			  (*measure)->ip->y,
			  (*measure)->ip->scale,
			  (*measure)->ip->scale,
			  (*measure)->camera_id );
	cpoint.add_measure( m );
      }
      // 2.4) Removing this location
      iter = crn[i].relations.erase(iter);
      iter--;
      // 2.5) Checking for a spiral error
      {
	std::list<unsigned> previous_camera;
	bool match = false;
	for ( std::list<ipr>::iterator interest = interestpts.begin();
	      interest != interestpts.end(); interest++ ) {
	  for ( std::list<unsigned>::iterator previous = previous_camera.begin();
		previous != previous_camera.end(); previous++ ) {
	    if ((*previous) == (*interest)->camera_id) {
	      match = true;
	      break;
	    }
	  }
	  previous_camera.push_back( (*interest)->camera_id );
	  if ( match ) {
	    continue;
	  }
	}
	if ( match ) {
	  spiral_error_count++;
	  continue;
	}
      }

      // 2.6) If haven't left for spiral error, add new control point
      if ( cpoint.size() > 1 )
	cnet->add_control_point( cpoint );
    }
  }
  if ( spiral_error_count != 0 )
    vw_out(0) << "\t" << spiral_error_count << " control points removed because of spiral errors.\n";

  VW_ASSERT( cnet->size() != 0, vw::Aborted() << "Failed to load any points, Control Network Empty\n");

  // 3.) Triangulating Positions
  for ( unsigned i = 0; i < cnet->size(); i++ ) {

    std::vector< Vector3 > positions;
    double error = 0, error_sum = 0;

    const double min_convergence_angle = 5.0*3.1415/180.0;

    // 3.1) Building a permutation listing of triangulation
    for ( unsigned j = 0; j < (*cnet)[i].size(); j++ ) {
      for ( unsigned k = j+1; k < (*cnet)[i].size(); k++ ) {

	StereoModel sm( *(camera_models[ (*cnet)[i][j].image_id()]),
			*(camera_models[ (*cnet)[i][k].image_id()]) );

	if ( sm.convergence_angle( (*cnet)[i][j].position(),
				   (*cnet)[i][k].position() ) >
	     min_convergence_angle ) {
	  positions.push_back( sm( (*cnet)[i][j].position(),
				   (*cnet)[i][k].position(),
				   error ) );
	  error_sum += error;
	}
      }
    }

    // 3.2) Summing, Averaging and Storing
    error_sum /= positions.size();
    Vector3 position_avg;
    for ( unsigned j = 0; j < positions.size(); j++ )
      position_avg += positions[j];
    position_avg /= positions.size();

    (*cnet)[i].set_position( position_avg );

  }
}

void add_ground_control_points( boost::shared_ptr<vw::camera::ControlNetwork> cnet,
				std::vector<std::string> image_files ) {
  std::cout << "\nLoading Ground Control Points:\n";
  for ( unsigned i = 0; i < image_files.size(); ++i ) {
    std::string potential_gcp = prefix_from_filename( image_files[i] ) + ".gcp";

    if ( fs::exists( potential_gcp ) ) {
      int count = 0;

      // Now Loading the Ground Control Points
      std::ifstream istr(potential_gcp.c_str());

      while (!istr.eof()) {
	Vector2 pix;
	Vector3 loc;
	Vector3 sigma;

	istr >> pix[0] >> pix[1] >> loc[0] >> loc[1] >> loc[2] >> sigma[0] >> sigma[1] >> sigma[2];
	if (loc[0] !=0) {  // ignore blank lines
	  Vector3 xyz = cartography::lon_lat_radius_to_xyz(loc);
	  std::cout << "GCP: " << xyz << "\n";

	  ControlMeasure m(pix[0], pix[1], 1.0, 1.0, i);
	  ControlPoint cpoint(ControlPoint::GroundControlPoint);
	  cpoint.set_position(xyz[0],xyz[1],xyz[2]);
	  cpoint.set_sigma(sigma[0],sigma[1],sigma[2]);
	  cpoint.add_measure(m);

	  // Deciding where to put it
	  bool just_append = true;
	  for ( unsigned p = 0; p < cnet->size(); ++p) {
	    if ((*cnet)[p].type() == ControlPoint::GroundControlPoint ) {
	      if (((*cnet)[p].position()[0] - xyz[0]) < .001 &&
		  ((*cnet)[p].position()[1] - xyz[1]) < .001 &&
		  ((*cnet)[p].position()[2] - xyz[2]) < .001) {
		// I'm not checking measures because there is only one GCP
		// file per camera
		just_append = false;
		(*cnet)[p].add_measure(m);
		break;
	      }

	    }
	  }

	  if (just_append)
	    cnet->add_control_point(cpoint);

	  ++count;
	}
      }
      istr.close();

      std::cout << "\t" << potential_gcp << "    " << " : "
	      << count << " GCPs.\n";
    }
  }
}
<|endoftext|>


<commit_before>/* adserver_connector.cc
   Jeremy Barnes, 19 December 2012
   Copyright (c) 2012 Datacratic Inc.  All rights reserved.

   Base class to connect to an ad server.  We also have an http ad server
   connector that builds on top of this.
*/

#include "adserver_connector.h"

#include "rtbkit/common/auction_events.h"
#include <dlfcn.h>

using namespace std;

namespace RTBKIT {

/*****************************************************************************/
/* POST AUCTION PROXY                                                        */
/*****************************************************************************/

AdServerConnector::
AdServerConnector(const string & serviceName,
                  const shared_ptr<Datacratic::ServiceProxies> & proxy)
    : ServiceBase(serviceName, proxy),
      toPostAuctionService_(proxy->zmqContext)
{
}

AdServerConnector::
~AdServerConnector()
{
}

void
AdServerConnector::
init(shared_ptr<ConfigurationService> config)
{
    shared_ptr<ServiceProxies> services = getServices();

    registerServiceProvider(serviceName_, { "adServer" });
    services->config->removePath(serviceName());

    toPostAuctionService_.init(config, ZMQ_XREQ);
    toPostAuctionService_.connectToServiceClass("rtbPostAuctionService",
                                                "events");

#if 0 // later, for when we have multiple

    toPostAuctionServices_.init(getServices()->config, name);
    toPostAuctionServices_.connectHandler = [=] (const string & connectedTo) {
        cerr << "AdServerConnector is connected to post auction service "
        << connectedTo << endl;
    };
    toPostAuctionServices_.connectAllServiceProviders("rtbPostAuctionService",
                                                      "agents");
    toPostAuctionServices_.connectToServiceClass();
#endif
}

void
AdServerConnector::
start()
{
    startTime_ = Date::now();
    recordHit("up");
}

void
AdServerConnector::
shutdown()
{
}

void
AdServerConnector::
recordUptime()
    const
{
    recordLevel(Date::now().secondsSince(startTime_), "uptime");
}

void
AdServerConnector::
publishWin(const Id & auctionId,
           const Id & adSpotId,
           Amount winPrice,
           Date timestamp,
           const JsonHolder & winMeta,
           const UserIds & ids,
           const AccountKey & account,
           Date bidTimestamp)
{
    recordHit("receivedEvent");
    recordHit("event.WIN");

    PostAuctionEvent event;
    event.type = PAE_WIN;
    event.auctionId = auctionId;
    event.adSpotId = adSpotId;
    event.winPrice = winPrice;
    event.timestamp = timestamp;
    event.metadata = winMeta;
    event.uids = ids;
    event.account = account;
    event.bidTimestamp = bidTimestamp;

    string str = ML::DB::serializeToString(event);
    toPostAuctionService_.sendMessage("WIN", str);
}

void
AdServerConnector::
publishLoss(const Id & auctionId,
            const Id & adSpotId,
            Date timestamp,
            const JsonHolder & lossMeta,
            const AccountKey & account,
            Date bidTimestamp)
{
    recordHit("receivedEvent");
    recordHit("event.LOSS");

    PostAuctionEvent event;
    event.type = PAE_LOSS;
    event.auctionId = auctionId;
    event.adSpotId = adSpotId;
    event.timestamp = timestamp;
    event.metadata = lossMeta;
    event.account = account;
    event.bidTimestamp = bidTimestamp;

    string str = ML::DB::serializeToString(event);
    toPostAuctionService_.sendMessage("LOSS", str);
}

void
AdServerConnector::
publishCampaignEvent(const string & label,
                     const Id & auctionId,
                     const Id & adSpotId,
                     Date timestamp,
                     const JsonHolder & impressionMeta,
                     const UserIds & ids)
{
    recordHit("receivedEvent");
    recordHit("event." + label);

    PostAuctionEvent event;
    event.type = PAE_CAMPAIGN_EVENT;
    event.label = label;
    event.auctionId = auctionId;
    event.adSpotId = adSpotId;
    event.timestamp = timestamp;
    event.uids = ids;
    event.metadata = impressionMeta;

    string str = ML::DB::serializeToString(event);
    toPostAuctionService_.sendMessage("EVENT", str);
}

void
AdServerConnector::
publishUserEvent(const string & label,
                 const Id & userId,
                 Date timestamp,
                 const JsonHolder & impressionMeta,
                 const UserIds & ids)
{
    recordHit("receivedEvent");
    recordHit("event." + label);
}

namespace {
    typedef std::lock_guard<ML::Spinlock> Guard;
    static ML::Spinlock lock;
    static std::unordered_map<std::string, AdServerConnector::Factory> factories;
}

AdServerConnector::Factory getFactory(std::string const & name) {
    // see if it's already existing
    {
        Guard guard(lock);
        auto i = factories.find(name);
        if (i != factories.end()) return i->second;
    }

    // else, try to load the exchange library
    std::string path = "lib" + name + "_adserver.so";
    void * handle = dlopen(path.c_str(), RTLD_NOW);
    if (!handle) {
        throw ML::Exception("couldn't find adserver library " + path);
    }

    // if it went well, it should be registered now
    Guard guard(lock);
    auto i = factories.find(name);
    if (i != factories.end()) return i->second;

    throw ML::Exception("couldn't find adserver name " + name);
}

void AdServerConnector::registerFactory(std::string const & name, Factory callback) {
    Guard guard(lock);
    if (!factories.insert(std::make_pair(name, callback)).second)
        throw ML::Exception("already had an adserver factory registered");
}

std::unique_ptr<AdServerConnector> AdServerConnector::create(
    std::shared_ptr<ServiceProxies> const & proxies, Json::Value const & json) {
    auto name = json.get("type", "unknown").asString();
    auto factory = getFactory(name);
    return std::unique_ptr<AdServerConnector>(factory(proxies, json));
}

} // namespace RTBKIT
<commit_msg>added error message when loading shared library fails<commit_after>/* adserver_connector.cc
   Jeremy Barnes, 19 December 2012
   Copyright (c) 2012 Datacratic Inc.  All rights reserved.

   Base class to connect to an ad server.  We also have an http ad server
   connector that builds on top of this.
*/

#include "adserver_connector.h"

#include "rtbkit/common/auction_events.h"
#include <dlfcn.h>

using namespace std;

namespace RTBKIT {

/*****************************************************************************/
/* POST AUCTION PROXY                                                        */
/*****************************************************************************/

AdServerConnector::
AdServerConnector(const string & serviceName,
                  const shared_ptr<Datacratic::ServiceProxies> & proxy)
    : ServiceBase(serviceName, proxy),
      toPostAuctionService_(proxy->zmqContext)
{
}

AdServerConnector::
~AdServerConnector()
{
}

void
AdServerConnector::
init(shared_ptr<ConfigurationService> config)
{
    shared_ptr<ServiceProxies> services = getServices();

    registerServiceProvider(serviceName_, { "adServer" });
    services->config->removePath(serviceName());

    toPostAuctionService_.init(config, ZMQ_XREQ);
    toPostAuctionService_.connectToServiceClass("rtbPostAuctionService",
                                                "events");

#if 0 // later, for when we have multiple

    toPostAuctionServices_.init(getServices()->config, name);
    toPostAuctionServices_.connectHandler = [=] (const string & connectedTo) {
        cerr << "AdServerConnector is connected to post auction service "
        << connectedTo << endl;
    };
    toPostAuctionServices_.connectAllServiceProviders("rtbPostAuctionService",
                                                      "agents");
    toPostAuctionServices_.connectToServiceClass();
#endif
}

void
AdServerConnector::
start()
{
    startTime_ = Date::now();
    recordHit("up");
}

void
AdServerConnector::
shutdown()
{
}

void
AdServerConnector::
recordUptime()
    const
{
    recordLevel(Date::now().secondsSince(startTime_), "uptime");
}

void
AdServerConnector::
publishWin(const Id & auctionId,
           const Id & adSpotId,
           Amount winPrice,
           Date timestamp,
           const JsonHolder & winMeta,
           const UserIds & ids,
           const AccountKey & account,
           Date bidTimestamp)
{
    recordHit("receivedEvent");
    recordHit("event.WIN");

    PostAuctionEvent event;
    event.type = PAE_WIN;
    event.auctionId = auctionId;
    event.adSpotId = adSpotId;
    event.winPrice = winPrice;
    event.timestamp = timestamp;
    event.metadata = winMeta;
    event.uids = ids;
    event.account = account;
    event.bidTimestamp = bidTimestamp;

    string str = ML::DB::serializeToString(event);
    toPostAuctionService_.sendMessage("WIN", str);
}

void
AdServerConnector::
publishLoss(const Id & auctionId,
            const Id & adSpotId,
            Date timestamp,
            const JsonHolder & lossMeta,
            const AccountKey & account,
            Date bidTimestamp)
{
    recordHit("receivedEvent");
    recordHit("event.LOSS");

    PostAuctionEvent event;
    event.type = PAE_LOSS;
    event.auctionId = auctionId;
    event.adSpotId = adSpotId;
    event.timestamp = timestamp;
    event.metadata = lossMeta;
    event.account = account;
    event.bidTimestamp = bidTimestamp;

    string str = ML::DB::serializeToString(event);
    toPostAuctionService_.sendMessage("LOSS", str);
}

void
AdServerConnector::
publishCampaignEvent(const string & label,
                     const Id & auctionId,
                     const Id & adSpotId,
                     Date timestamp,
                     const JsonHolder & impressionMeta,
                     const UserIds & ids)
{
    recordHit("receivedEvent");
    recordHit("event." + label);

    PostAuctionEvent event;
    event.type = PAE_CAMPAIGN_EVENT;
    event.label = label;
    event.auctionId = auctionId;
    event.adSpotId = adSpotId;
    event.timestamp = timestamp;
    event.uids = ids;
    event.metadata = impressionMeta;

    string str = ML::DB::serializeToString(event);
    toPostAuctionService_.sendMessage("EVENT", str);
}

void
AdServerConnector::
publishUserEvent(const string & label,
                 const Id & userId,
                 Date timestamp,
                 const JsonHolder & impressionMeta,
                 const UserIds & ids)
{
    recordHit("receivedEvent");
    recordHit("event." + label);
}

namespace {
    typedef std::lock_guard<ML::Spinlock> Guard;
    static ML::Spinlock lock;
    static std::unordered_map<std::string, AdServerConnector::Factory> factories;
}

AdServerConnector::Factory getFactory(std::string const & name) {
    // see if it's already existing
    {
        Guard guard(lock);
        auto i = factories.find(name);
        if (i != factories.end()) return i->second;
    }

    // else, try to load the exchange library
    std::string path = "lib" + name + "_adserver.so";
    void * handle = dlopen(path.c_str(), RTLD_NOW);
    if (!handle) {
        std::cerr << dlerror() << std::endl;
        throw ML::Exception("couldn't load adserver library " + path);
    }

    // if it went well, it should be registered now
    Guard guard(lock);
    auto i = factories.find(name);
    if (i != factories.end()) return i->second;

    throw ML::Exception("couldn't find adserver name " + name);
}

void AdServerConnector::registerFactory(std::string const & name, Factory callback) {
    Guard guard(lock);
    if (!factories.insert(std::make_pair(name, callback)).second)
        throw ML::Exception("already had an adserver factory registered");
}

std::unique_ptr<AdServerConnector> AdServerConnector::create(
    std::shared_ptr<ServiceProxies> const & proxies, Json::Value const & json) {
    auto name = json.get("type", "unknown").asString();
    auto factory = getFactory(name);
    return std::unique_ptr<AdServerConnector>(factory(proxies, json));
}

} // namespace RTBKIT
<|endoftext|>


<commit_before>/*=====================================================================

PIXHAWK Micro Air Vehicle Flying Robotics Toolkit
Please see our website at <http://pixhawk.ethz.ch>

(c) 2009, 2010 PIXHAWK PROJECT  <http://pixhawk.ethz.ch>

This file is part of the PIXHAWK project

    PIXHAWK is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    PIXHAWK is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with PIXHAWK. If not, see <http://www.gnu.org/licenses/>.

======================================================================*/

/**
 * @file
 *   @brief Implementation of the MAVLink protocol
 *
 *   @author Lorenz Meier <mavteam@student.ethz.ch>
 *
 */

#include <inttypes.h>
#include <iostream>

#include <QDebug>
#include <QTime>

#include "MG.h"
#include "MAVLinkProtocol.h"
#include "UASInterface.h"
#include "UASManager.h"
#include "UASInterface.h"
#include "UAS.h"
#include "configuration.h"
#include "LinkManager.h"
#include <mavlink.h>

/**
 * The default constructor will create a new MAVLink object sending heartbeats at
 * the MAVLINK_HEARTBEAT_DEFAULT_RATE to all connected links.
 */
MAVLinkProtocol::MAVLinkProtocol() :
        heartbeatTimer(new QTimer(this)),
        heartbeatRate(MAVLINK_HEARTBEAT_DEFAULT_RATE),
        m_heartbeatsEnabled(false)
{
    start(QThread::LowPriority);
    // Start heartbeat timer, emitting a heartbeat at the configured rate
    connect(heartbeatTimer, SIGNAL(timeout()), this, SLOT(sendHeartbeat()));
    heartbeatTimer->start(1000/heartbeatRate);
    totalReceiveCounter = 0;
    totalLossCounter = 0;
    currReceiveCounter = 0;
    currLossCounter = 0;
    for (int i = 0; i < 256; i++)
    {
        for (int j = 0; j < 256; j++)
        {
            lastIndex[i][j] = -1;
        }
    }
}

MAVLinkProtocol::~MAVLinkProtocol()
{
}

void MAVLinkProtocol::run()
{
}

/**
 * The bytes are copied by calling the LinkInterface::readBytes() method.
 * This method parses all incoming bytes and constructs a MAVLink packet.
 * It can handle multiple links in parallel, as each link has it's own buffer/
 * parsing state machine.
 * @param link The interface to read from
 * @see LinkInterface
 **/
void MAVLinkProtocol::receiveBytes(LinkInterface* link)
{
    receiveMutex.lock();
    // Prepare buffer
    static const int maxlen = 4096 * 100;
    static char buffer[maxlen];
    qint64 bytesToRead = link->bytesAvailable();

    // Get all data at once, let link read the bytes in the buffer array
    link->readBytes(buffer, maxlen);
    mavlink_message_t message;
    mavlink_status_t status;
    for (int position = 0; position < bytesToRead; position++)
    {
        unsigned int decodeState = mavlink_parse_char(link->getId(), (uint8_t)*(buffer + position), &message, &status);

        if (decodeState == 1)
        {
            // ORDER MATTERS HERE!
            // If the matching UAS object does not yet exist, it has to be created
            // before emitting the packetReceived signal
            UASInterface* uas = UASManager::instance()->getUASForId(message.sysid);

            // Check and (if necessary) create UAS object
            if (uas == NULL)
            {
                // ORDER MATTERS HERE!
                // The UAS object has first to be created and connected,
                // only then the rest of the application can be made aware
                // of its existence, as it only then can send and receive
                // it's first messages.

                // First create new UAS object
                uas = new UAS(this, message.sysid);
                // Make UAS aware that this link can be used to communicate with the actual robot
                uas->addLink(link);
                // Connect this robot to the UAS object
                connect(this, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), uas, SLOT(receiveMessage(LinkInterface*, mavlink_message_t)));
                // Now add UAS to "official" list, which makes the whole application aware of it
                UASManager::instance()->addUAS(uas);
            }
            // Increase receive counter
            totalReceiveCounter++;
            currReceiveCounter++;
            qint64 lastLoss = totalLossCounter;
            // Update last packet index
            if (lastIndex[message.sysid][message.compid] == -1)
            {
                lastIndex[message.sysid][message.compid] = message.seq;
            }
            else
            {
                if (lastIndex[message.sysid][message.compid] == 255)
                {
                    lastIndex[message.sysid][message.compid] = 0;
                }
                else
                {
                    lastIndex[message.sysid][message.compid]++;
                }

                int safeguard = 0;
                //qDebug() << "SYSID" << message.sysid << "COMPID" << message.compid << "MSGID" << message.msgid << "LASTINDEX" << lastIndex[message.sysid][message.compid] << "SEQ" << message.seq;
                while(lastIndex[message.sysid][message.compid] != message.seq && safeguard < 255)
                {
                    if (lastIndex[message.sysid][message.compid] == 255)
                    {
                        lastIndex[message.sysid][message.compid] = 0;
                    }
                    else
                    {
                        lastIndex[message.sysid][message.compid]++;
                    }
                    totalLossCounter++;
                    currLossCounter++;
                    safeguard++;
                }
            }
            //            if (lastIndex.contains(message.sysid))
            //            {
            //                QMap<int, int>* lastCompIndex = lastIndex.value(message.sysid);
            //                if (lastCompIndex->contains(message.compid))
            //                while (lastCompIndex->value(message.compid, 0)+1 )
            //            }
            //if ()

            // If a new loss was detected or we just hit one 128th packet step
            if (lastLoss != totalLossCounter || (totalReceiveCounter & 0b1111111) == 0)
            {
                // Calculate new loss ratio
                // Receive loss
                float receiveLoss = (double)currLossCounter/(double)(currReceiveCounter+currLossCounter);
                receiveLoss *= 100.0f;
               // qDebug() << "LOSSCHANGED" << receiveLoss;
                currLossCounter = 0;
                currReceiveCounter = 0;
                emit receiveLossChanged(receiveLoss);
            }

            // The packet is emitted as a whole, as it is only 255 - 261 bytes short
            // kind of inefficient, but no issue for a groundstation pc.
            // It buys as reentrancy for the whole code over all threads
            emit messageReceived(link, message);
        }
    }
    receiveMutex.unlock();
}

/**
 * @return The name of this protocol
 **/
QString MAVLinkProtocol::getName()
{
    return QString(tr("MAVLink protocol"));
}

/** @return System id of this application */
int MAVLinkProtocol::getSystemId()
{
    return MG::SYSTEM::ID;
}

/** @return Component id of this application */
int MAVLinkProtocol::getComponentId()
{
    return MG::SYSTEM::COMPID;
}

/**
 * @param message message to send
 */
void MAVLinkProtocol::sendMessage(mavlink_message_t message)
{
    // Get all links connected to this unit
    QList<LinkInterface*> links = LinkManager::instance()->getLinksForProtocol(this);

    // Emit message on all links that are currently connected
    QList<LinkInterface*>::iterator i;
    for (i = links.begin(); i != links.end(); ++i)
    {
        sendMessage(*i, message);
    }
}

/**
 * @param link the link to send the message over
 * @param message message to send
 */
void MAVLinkProtocol::sendMessage(LinkInterface* link, mavlink_message_t message)
{
    // Create buffer
    uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
    // Write message into buffer, prepending start sign
    int len = mavlink_msg_to_send_buffer(buffer, &message);
    // If link is connected
    if (link->isConnected())
    {
        // Send the portion of the buffer now occupied by the message
        link->writeBytes((const char*)buffer, len);
    }
}

/**
 * The heartbeat is sent out of order and does not reset the
 * periodic heartbeat emission. It will be just sent in addition.
 * @return mavlink_message_t heartbeat message sent on serial link
 */
void MAVLinkProtocol::sendHeartbeat()
{
    if (m_heartbeatsEnabled)
    {
        mavlink_message_t beat;
        mavlink_msg_heartbeat_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID,&beat, OCU);
        sendMessage(beat);
    }
}

/** @param enabled true to enable heartbeats emission at heartbeatRate, false to disable */
void MAVLinkProtocol::enableHeartbeats(bool enabled)
{
    m_heartbeatsEnabled = enabled;
    emit heartbeatChanged(enabled);
}

bool MAVLinkProtocol::heartbeatsEnabled(void)
{
    return m_heartbeatsEnabled;
}

/**
 * The default rate is 1 Hertz.
 *
 * @param rate heartbeat rate in hertz (times per second)
 */
void MAVLinkProtocol::setHeartbeatRate(int rate)
{
    heartbeatRate = rate;
    heartbeatTimer->setInterval(1000/heartbeatRate);
}

/** @return heartbeat rate in Hertz */
int MAVLinkProtocol::getHeartbeatRate()
{
    return heartbeatRate;
}
<commit_msg>Fixed small compile error with module bit mask<commit_after>/*=====================================================================

PIXHAWK Micro Air Vehicle Flying Robotics Toolkit
Please see our website at <http://pixhawk.ethz.ch>

(c) 2009, 2010 PIXHAWK PROJECT  <http://pixhawk.ethz.ch>

This file is part of the PIXHAWK project

    PIXHAWK is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    PIXHAWK is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with PIXHAWK. If not, see <http://www.gnu.org/licenses/>.

======================================================================*/

/**
 * @file
 *   @brief Implementation of the MAVLink protocol
 *
 *   @author Lorenz Meier <mavteam@student.ethz.ch>
 *
 */

#include <inttypes.h>
#include <iostream>

#include <QDebug>
#include <QTime>

#include "MG.h"
#include "MAVLinkProtocol.h"
#include "UASInterface.h"
#include "UASManager.h"
#include "UASInterface.h"
#include "UAS.h"
#include "configuration.h"
#include "LinkManager.h"
#include <mavlink.h>

/**
 * The default constructor will create a new MAVLink object sending heartbeats at
 * the MAVLINK_HEARTBEAT_DEFAULT_RATE to all connected links.
 */
MAVLinkProtocol::MAVLinkProtocol() :
        heartbeatTimer(new QTimer(this)),
        heartbeatRate(MAVLINK_HEARTBEAT_DEFAULT_RATE),
        m_heartbeatsEnabled(false)
{
    start(QThread::LowPriority);
    // Start heartbeat timer, emitting a heartbeat at the configured rate
    connect(heartbeatTimer, SIGNAL(timeout()), this, SLOT(sendHeartbeat()));
    heartbeatTimer->start(1000/heartbeatRate);
    totalReceiveCounter = 0;
    totalLossCounter = 0;
    currReceiveCounter = 0;
    currLossCounter = 0;
    for (int i = 0; i < 256; i++)
    {
        for (int j = 0; j < 256; j++)
        {
            lastIndex[i][j] = -1;
        }
    }
}

MAVLinkProtocol::~MAVLinkProtocol()
{
}

void MAVLinkProtocol::run()
{
}

/**
 * The bytes are copied by calling the LinkInterface::readBytes() method.
 * This method parses all incoming bytes and constructs a MAVLink packet.
 * It can handle multiple links in parallel, as each link has it's own buffer/
 * parsing state machine.
 * @param link The interface to read from
 * @see LinkInterface
 **/
void MAVLinkProtocol::receiveBytes(LinkInterface* link)
{
    receiveMutex.lock();
    // Prepare buffer
    static const int maxlen = 4096 * 100;
    static char buffer[maxlen];
    qint64 bytesToRead = link->bytesAvailable();

    // Get all data at once, let link read the bytes in the buffer array
    link->readBytes(buffer, maxlen);
    mavlink_message_t message;
    mavlink_status_t status;
    for (int position = 0; position < bytesToRead; position++)
    {
        unsigned int decodeState = mavlink_parse_char(link->getId(), (uint8_t)*(buffer + position), &message, &status);

        if (decodeState == 1)
        {
            // ORDER MATTERS HERE!
            // If the matching UAS object does not yet exist, it has to be created
            // before emitting the packetReceived signal
            UASInterface* uas = UASManager::instance()->getUASForId(message.sysid);

            // Check and (if necessary) create UAS object
            if (uas == NULL)
            {
                // ORDER MATTERS HERE!
                // The UAS object has first to be created and connected,
                // only then the rest of the application can be made aware
                // of its existence, as it only then can send and receive
                // it's first messages.

                // First create new UAS object
                uas = new UAS(this, message.sysid);
                // Make UAS aware that this link can be used to communicate with the actual robot
                uas->addLink(link);
                // Connect this robot to the UAS object
                connect(this, SIGNAL(messageReceived(LinkInterface*, mavlink_message_t)), uas, SLOT(receiveMessage(LinkInterface*, mavlink_message_t)));
                // Now add UAS to "official" list, which makes the whole application aware of it
                UASManager::instance()->addUAS(uas);
            }
            // Increase receive counter
            totalReceiveCounter++;
            currReceiveCounter++;
            qint64 lastLoss = totalLossCounter;
            // Update last packet index
            if (lastIndex[message.sysid][message.compid] == -1)
            {
                lastIndex[message.sysid][message.compid] = message.seq;
            }
            else
            {
                if (lastIndex[message.sysid][message.compid] == 255)
                {
                    lastIndex[message.sysid][message.compid] = 0;
                }
                else
                {
                    lastIndex[message.sysid][message.compid]++;
                }

                int safeguard = 0;
                //qDebug() << "SYSID" << message.sysid << "COMPID" << message.compid << "MSGID" << message.msgid << "LASTINDEX" << lastIndex[message.sysid][message.compid] << "SEQ" << message.seq;
                while(lastIndex[message.sysid][message.compid] != message.seq && safeguard < 255)
                {
                    if (lastIndex[message.sysid][message.compid] == 255)
                    {
                        lastIndex[message.sysid][message.compid] = 0;
                    }
                    else
                    {
                        lastIndex[message.sysid][message.compid]++;
                    }
                    totalLossCounter++;
                    currLossCounter++;
                    safeguard++;
                }
            }
            //            if (lastIndex.contains(message.sysid))
            //            {
            //                QMap<int, int>* lastCompIndex = lastIndex.value(message.sysid);
            //                if (lastCompIndex->contains(message.compid))
            //                while (lastCompIndex->value(message.compid, 0)+1 )
            //            }
            //if ()

            // If a new loss was detected or we just hit one 128th packet step
            if (lastLoss != totalLossCounter || (totalReceiveCounter & 0x7F) == 0)
            {
                // Calculate new loss ratio
                // Receive loss
                float receiveLoss = (double)currLossCounter/(double)(currReceiveCounter+currLossCounter);
                receiveLoss *= 100.0f;
               // qDebug() << "LOSSCHANGED" << receiveLoss;
                currLossCounter = 0;
                currReceiveCounter = 0;
                emit receiveLossChanged(receiveLoss);
            }

            // The packet is emitted as a whole, as it is only 255 - 261 bytes short
            // kind of inefficient, but no issue for a groundstation pc.
            // It buys as reentrancy for the whole code over all threads
            emit messageReceived(link, message);
        }
    }
    receiveMutex.unlock();
}

/**
 * @return The name of this protocol
 **/
QString MAVLinkProtocol::getName()
{
    return QString(tr("MAVLink protocol"));
}

/** @return System id of this application */
int MAVLinkProtocol::getSystemId()
{
    return MG::SYSTEM::ID;
}

/** @return Component id of this application */
int MAVLinkProtocol::getComponentId()
{
    return MG::SYSTEM::COMPID;
}

/**
 * @param message message to send
 */
void MAVLinkProtocol::sendMessage(mavlink_message_t message)
{
    // Get all links connected to this unit
    QList<LinkInterface*> links = LinkManager::instance()->getLinksForProtocol(this);

    // Emit message on all links that are currently connected
    QList<LinkInterface*>::iterator i;
    for (i = links.begin(); i != links.end(); ++i)
    {
        sendMessage(*i, message);
    }
}

/**
 * @param link the link to send the message over
 * @param message message to send
 */
void MAVLinkProtocol::sendMessage(LinkInterface* link, mavlink_message_t message)
{
    // Create buffer
    uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
    // Write message into buffer, prepending start sign
    int len = mavlink_msg_to_send_buffer(buffer, &message);
    // If link is connected
    if (link->isConnected())
    {
        // Send the portion of the buffer now occupied by the message
        link->writeBytes((const char*)buffer, len);
    }
}

/**
 * The heartbeat is sent out of order and does not reset the
 * periodic heartbeat emission. It will be just sent in addition.
 * @return mavlink_message_t heartbeat message sent on serial link
 */
void MAVLinkProtocol::sendHeartbeat()
{
    if (m_heartbeatsEnabled)
    {
        mavlink_message_t beat;
        mavlink_msg_heartbeat_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID,&beat, OCU);
        sendMessage(beat);
    }
}

/** @param enabled true to enable heartbeats emission at heartbeatRate, false to disable */
void MAVLinkProtocol::enableHeartbeats(bool enabled)
{
    m_heartbeatsEnabled = enabled;
    emit heartbeatChanged(enabled);
}

bool MAVLinkProtocol::heartbeatsEnabled(void)
{
    return m_heartbeatsEnabled;
}

/**
 * The default rate is 1 Hertz.
 *
 * @param rate heartbeat rate in hertz (times per second)
 */
void MAVLinkProtocol::setHeartbeatRate(int rate)
{
    heartbeatRate = rate;
    heartbeatTimer->setInterval(1000/heartbeatRate);
}

/** @return heartbeat rate in Hertz */
int MAVLinkProtocol::getHeartbeatRate()
{
    return heartbeatRate;
}
<|endoftext|>


<commit_before>#include <AI/Steering.h>
#include <Graphics/DebugRenderer.h>
#include <imgui.h>
#include <Core/World.h>

using namespace Osm;

Steering::Steering(Entity& entity)
	: Component(entity)
	, _flags(STEERING_NONE)
{
	_physicsManager = GetOwner().GetWorld().GetComponent<PhysicsManager2D>();
	_physicsBody = entity.GetComponent<PhysicsBody2D>();
}

Vector2 Steering::GetSteering()
{
	CalculatePrioritized();
	DebugRender();
	return _current;
}

bool Steering::AccumulateForce(Vector2 add)
{
	// Calculate how much steering force the vehicle has used so far
	float  magnitudeSoFar = _current.Magnitude();

	// Calculate how much steering force remains to be used by this vehicle
	float magnitudeRemaining = MaxForce - magnitudeSoFar;

	// Return false if there is no more force left to use
	if (magnitudeRemaining <= 0.0) return false;

	// Calculate the magnitude of the force we want to add
	double magnitudeToAdd = add.Magnitude();

	// If the magnitude of the sum of ForceToAdd and the running total
	// does not exceed the maximum force available to this vehicle, just
	// add together. Otherwise add as much of the ForceToAdd vector is
	// possible without going over the max.
	if (magnitudeToAdd < magnitudeRemaining)
	{
		_current += add;
	}
	else
	{
		// Add it to the steering force
		add.Normalize();
		_current += add * magnitudeRemaining;
	}

	return true;
}

void Steering::CalculateWeightedSum()
{
}

void Steering::CalculatePrioritized()
{
	_current.Clear();
	vector<PhysicsBody2D*> neighbors;
	if (IsOn(STEERING_SEPARATION) || IsOn(STEERING_ALIGNMENT) || IsOn(STEERING_COHESION))
	{
		Vector2 v = _physicsBody->GetPosition();
		neighbors = GetFlockingNeighbors();
	}

	Vector2 force;

	if (IsOn(STEERING_SEEK))
	{
		force += Seek(Target) * SeekWeight;
		if (!AccumulateForce(force)) return;
	}
	if (IsOn(STEERING_ARRIVE))
	{
		force += Arrive(Target, ArriveAcceleration)* ArriveWeight;
		if (!AccumulateForce(force)) return;
	}
	if (IsOn(STEERING_EVADE))
	{
		force += Evade(Agent);
		if (!AccumulateForce(force)) return;
	}
	if (IsOn(STEERING_OFFSET_PURSUIT))
	{
		force += OffsetPursuit(Agent, Offset) * OffsetPursuitWeight;
		if (!AccumulateForce(force)) return;
	}
	if (IsOn(STEERING_COHESION))
	{
		force += Cohesion(neighbors) * CohesionWeight;
		if (!AccumulateForce(force)) return;
	}
	if (IsOn(STEERING_SEPARATION))
	{
		force += Separation(neighbors) * SeparationWeight;
		if (!AccumulateForce(force)) return;
	}
	if (IsOn(STEERING_ALIGNMENT))
	{
		force += Alignment(neighbors) * AlignmentWeight;
		if (!AccumulateForce(force)) return;
	}
	if (IsOn(STEERING_WANDER))
	{
		force += Wander() * WanderWeight;
		if (!AccumulateForce(force)) return;
	}
}

Vector2 Steering::Seek(Vector2& targetPos)
{
	Vector2 seek = targetPos - _physicsBody->GetPosition();
	seek.Normalize();
	seek *= MaxAccelearation;
	return seek;
}

Vector2 Steering::Flee(Vector2& targetPos)
{
	/*
	Vector2D DesiredVelocity = Vec2DNormalize(m_pVehicle->Pos() - TargetPos)
		* m_pVehicle->MaxSpeed();
	return (DesiredVelocity - m_pVehicle->Velocity());
	*/

	Vector2 flee = targetPos - _physicsBody->GetPosition();
	flee.Normalize();
	flee *= MaxAccelearation;
	flee = flee - _physicsBody->GetVelocity();
	return flee;
}

Vector2 Steering::Arrive(Vector2& targetPos, float deceleration)
{
	Vector2 toTarget = Target - _physicsBody->GetPosition();

	Vector2 arrive;

	// Calculate the distance to the target
	double dist = toTarget.Magnitude();

	if (dist > 0)
	{
		// Because Deceleration is enumerated as an int, this value is required
		// to provide fine tweaking of the deceleration..
		const float DecelerationTweaker = 0.3;

		// Calculate the speed required to reach the target given the desired
		// deceleration
		float speed = dist / ((double)deceleration * DecelerationTweaker);

		// Make sure the velocity does not exceed the max
		speed = min(speed, MaxSpeed);

		// From here proceed just like Seek except we don't need to normalize
		// the ToTarget vector because we have already gone to the trouble
		// of calculating its length: dist.
		Vector2 desiredVelocity = toTarget * speed / dist;

		arrive = (desiredVelocity - _physicsBody->GetVelocity());
	}

	return arrive;
}

Vector2 Steering::OffsetPursuit(const PhysicsBody2D* agent, const Vector2& offset)
{
	if (agent == nullptr)
		return Vector2();

	// Get vehicle coordinate frame
	Vector2 target = agent->GetToWorld(offset);

	// If the evader is ahead and facing the agent then we can just seek
	// for the evader's current position.
	Vector2 toTarget = target - _physicsBody->GetPosition();

	gDebugRenderer.AddLine(ToVector3(target), ToVector3(_physicsBody->GetPosition()));

	// The lookahead time is propotional to the distance between the evader
	// and the pursuer; and is inversely proportional to the sum of the
	// agent's velocities
	float LookAheadTime = toTarget.Magnitude() /
		(MaxSpeed + agent->GetVelocity().Magnitude()) * 0.1f;

	// Now seek to the predicted future position of the evader
	target = target + agent->GetVelocity() * LookAheadTime;

	gDebugRenderer.AddCircle(ToVector3(target));

	return Seek(target);
}

Vector2 Steering::Evade(const PhysicsBody2D* agent)
{
	// Not necessary to include the check for facing direction this time
	Vector2 toPursuer = agent->GetPosition() - _physicsBody->GetPosition();

	//uncomment the following two lines to have Evade only consider pursuers
	//within a 'threat range'
	if (toPursuer.SquareMagnitude() > EvadeDistance * EvadeDistance)
		return Vector2();

	//the lookahead time is propotional to the distance between the pursuer
	//and the pursuer; and is inversely proportional to the sum of the
	//agents' velocities
	float lookAheadTime = toPursuer.Magnitude() / (MaxSpeed + agent->GetVelocity().Magnitude());
	lookAheadTime *= 0.1f;

	//now flee away from predicted future position of the pursuer
	Vector2 target = agent->GetPosition() + agent->GetVelocity() * lookAheadTime;
	return -1.0f * Flee(target);
}

Vector2 Steering::Wander()
{
	Vector2 wander;

	// This behavior is dependent on the update rate, so this line must
	// be included when using time independent framerate.
	float JitterThisTimeSlice = WanderJitter * 0.016f; // m_pVehicle->TimeElapsed();

	// First, add a small random vector to the target's position
	_wanderTarget += Vector2(	RandInRange(-1.0f, 1.0f) * JitterThisTimeSlice,
								RandInRange(-1.0f, 1.0f) * JitterThisTimeSlice);

	// Reproject this new vector back on to a unit circle
	_wanderTarget.Normalize();

	//increase the length of the vector to the same as the radius
	//of the wander circle
	_wanderTarget *= WanderRadius;

	//move the target into a position WanderDist in front of the agent
	Vector2 target = _wanderTarget + Vector2(0, WanderDistance);

	//project the target into world space
	target = _physicsBody->GetToWorld(target);

	Vector2 center = _physicsBody->GetToWorld(Vector2(0, WanderDistance));

	gDebugRenderer.AddCircle(ToVector3(center), WanderRadius);

	gDebugRenderer.AddCircle(ToVector3(target), 0.5f, Color::Yellow);

	// And steer towards it
	wander = target - _physicsBody->GetPosition();

	return wander;
}

vector<PhysicsBody2D*> Steering::GetFlockingNeighbors()
{
	vector<PhysicsBody2D*> neighbors;
	Vector2 v = _physicsBody->GetPosition();
	neighbors = _physicsManager->GetInRadius(v, FlockingRadius);
	if (FlockingTag != 0)
	{
		remove_if(	neighbors.begin(),
					neighbors.end(),
					[this](PhysicsBody2D* b)
		{
			return b->GetOwner().GetTag() != FlockingTag;
		});
	}
	return neighbors;
}

Vector2 Steering::Cohesion(const std::vector<PhysicsBody2D*>& agents)
{
	Vector2 cohesion;

	// First find the center of mass of all the agents
	Vector2 centerOfMass, steeringForce;

	size_t neighborCount = 0;

	// Iterate through the neighbors and sum up all the position vectors
	for(auto a : agents)
	{
		if(a != _physicsBody && a != Agent)
		{
			centerOfMass += a->GetPosition();
			neighborCount++;
		}
	}

	if (neighborCount > 0)
	{
		// The center of mass is the average of the sum of positions
		centerOfMass *= 1.0f / (float)neighborCount;

		//now seek towards that position
		cohesion = Seek(centerOfMass);
	}

	cohesion.Normalize();

	return cohesion;
}

Vector2 Steering::Separation(const std::vector<PhysicsBody2D*>& agents)
{
	Vector2 steeringForce;

	// iterate through the neighbors and sum up all the position vectors
	for (auto a : agents)
	{
		// make sure this agent isn't included in the calculations and that
		// the agent being examined is close enough
		if (a != _physicsBody && a != Agent)
		{
			Vector2 toAgent =_physicsBody->GetPosition() - a->GetPosition();

			// scale the force inversely proportional to the agents distance
			// from its neighbor.
			float d = toAgent.Magnitude();
			if (d > 0.01)
			{
				toAgent.Normalize();
				steeringForce += toAgent / d;
			}
		}

	}

	return steeringForce;
}

Vector2 Steering::Alignment(const std::vector<PhysicsBody2D*>& agents)
{
	//This will record the average heading of the neighbors
	Vector2 averageHeading;

	//This count the number of vehicles in the neighborhood
	float neighborCount = 0.0;

	Vector2 currentHeading = _physicsBody->GetFroward();
	currentHeading.Normalize();

	//iterate through the neighbors and sum up all the position vectors
	for (auto a : agents)
	{
		// make sure this agent isn't included in the calculations and that
		// the agent being examined is close enough
		if (a != _physicsBody && a != Agent)
		{
			Vector2 heading = a->GetFroward();
			heading.Normalize();
			averageHeading += heading;
			neighborCount++;
		}

	}

	// If the neighborhood contained one or more vehicles, average their
	// heading vectors.
	if (neighborCount > 0.0)
	{
		averageHeading *= 1.0f / neighborCount;
		averageHeading -= currentHeading;
	}

	return averageHeading;
}

void Steering::DebugRender()
{
	if (_inspect)
	{
		auto pos = _physicsBody->GetPosition();
		gDebugRenderer.AddCircle(
			ToVector3(pos),
			_physicsBody->GetRaduis(),
			Color::Purple);
		gDebugRenderer.AddLine(
			ToVector3(pos),
			ToVector3(pos + _current),
			Color::Orange);
		gDebugRenderer.AddCircle(
			ToVector3(pos),
			FlockingRadius,
			Color::Purple);
	}
	_inspect = false;
}

#ifdef INSPECTOR
void Steering::Inspect()
{
	_inspect = true;

	ImGui::InputFloat("Max Accelearation", &MaxAccelearation);
	ImGui::InputFloat("Max Force", &MaxForce);
	ImGui::InputFloat("Max Speed", &MaxSpeed);
	ImGui::InputFloat("Wander Jitter", &WanderJitter);
	ImGui::InputFloat("Wander Radius", &WanderRadius);
	ImGui::InputFloat("Wander Distance", &WanderDistance);

	float maxed = _current.Magnitude() / MaxSpeed;
	ImGui::ProgressBar(maxed);
	// ImGui::SameLine();
	//ImGui::LabelText("Total Used ","");
}
#endif

/*
PhysicsBody2D& SteeringBehaviour::GetPhysicsBody() const
{
	auto physics = GetOwner().GetPhysicsBody();
	ASSERT(physics);
	return *physics;
}

SteeringOutput Seek::GetSteering()
{
	SteeringOutput so;

	so.Linear = Target - GetPhysicsBody().GetPosition();
	so.Linear.Normalize();
	so.Linear *= GetOwner().MaxAccelearation;

	return so;
}
*/<commit_msg>Wandering behavior WIP<commit_after>#include <AI/Steering.h>
#include <Graphics/DebugRenderer.h>
#include <imgui.h>
#include <Core/World.h>

using namespace Osm;

Steering::Steering(Entity& entity)
	: Component(entity)
	, _flags(STEERING_NONE)
{
	_physicsManager = GetOwner().GetWorld().GetComponent<PhysicsManager2D>();
	_physicsBody = entity.GetComponent<PhysicsBody2D>();
}

Vector2 Steering::GetSteering()
{
	CalculatePrioritized();
	DebugRender();
	return _current;
}

bool Steering::AccumulateForce(Vector2 add)
{
	// Calculate how much steering force the vehicle has used so far
	float  magnitudeSoFar = _current.Magnitude();

	// Calculate how much steering force remains to be used by this vehicle
	float magnitudeRemaining = MaxForce - magnitudeSoFar;

	// Return false if there is no more force left to use
	if (magnitudeRemaining <= 0.0) return false;

	// Calculate the magnitude of the force we want to add
	double magnitudeToAdd = add.Magnitude();

	// If the magnitude of the sum of ForceToAdd and the running total
	// does not exceed the maximum force available to this vehicle, just
	// add together. Otherwise add as much of the ForceToAdd vector is
	// possible without going over the max.
	if (magnitudeToAdd < magnitudeRemaining)
	{
		_current += add;
	}
	else
	{
		// Add it to the steering force
		add.Normalize();
		_current += add * magnitudeRemaining;
	}

	return true;
}

void Steering::CalculateWeightedSum()
{
}

void Steering::CalculatePrioritized()
{
	_current.Clear();
	vector<PhysicsBody2D*> neighbors;
	if (IsOn(STEERING_SEPARATION) || IsOn(STEERING_ALIGNMENT) || IsOn(STEERING_COHESION))
	{
		Vector2 v = _physicsBody->GetPosition();
		neighbors = GetFlockingNeighbors();
	}

	Vector2 force;

	if (IsOn(STEERING_SEEK))
	{
		force += Seek(Target) * SeekWeight;
		if (!AccumulateForce(force)) return;
	}
	if (IsOn(STEERING_ARRIVE))
	{
		force += Arrive(Target, ArriveAcceleration)* ArriveWeight;
		if (!AccumulateForce(force)) return;
	}
	if (IsOn(STEERING_EVADE))
	{
		force += Evade(Agent);
		if (!AccumulateForce(force)) return;
	}
	if (IsOn(STEERING_OFFSET_PURSUIT))
	{
		force += OffsetPursuit(Agent, Offset) * OffsetPursuitWeight;
		if (!AccumulateForce(force)) return;
	}
	if (IsOn(STEERING_COHESION))
	{
		force += Cohesion(neighbors) * CohesionWeight;
		if (!AccumulateForce(force)) return;
	}
	if (IsOn(STEERING_SEPARATION))
	{
		force += Separation(neighbors) * SeparationWeight;
		if (!AccumulateForce(force)) return;
	}
	if (IsOn(STEERING_ALIGNMENT))
	{
		force += Alignment(neighbors) * AlignmentWeight;
		if (!AccumulateForce(force)) return;
	}
	if (IsOn(STEERING_WANDER))
	{
		force += Wander() * WanderWeight;
		if (!AccumulateForce(force)) return;
	}
}

Vector2 Steering::Seek(Vector2& targetPos)
{
	Vector2 seek = targetPos - _physicsBody->GetPosition();
	seek.Normalize();
	seek *= MaxAccelearation;
	return seek;
}

Vector2 Steering::Flee(Vector2& targetPos)
{
	/*
	Vector2D DesiredVelocity = Vec2DNormalize(m_pVehicle->Pos() - TargetPos)
		* m_pVehicle->MaxSpeed();
	return (DesiredVelocity - m_pVehicle->Velocity());
	*/

	Vector2 flee = targetPos - _physicsBody->GetPosition();
	flee.Normalize();
	flee *= MaxAccelearation;
	flee = flee - _physicsBody->GetVelocity();
	return flee;
}

Vector2 Steering::Arrive(Vector2& targetPos, float deceleration)
{
	Vector2 toTarget = Target - _physicsBody->GetPosition();

	Vector2 arrive;

	// Calculate the distance to the target
	double dist = toTarget.Magnitude();

	if (dist > 0)
	{
		// Because Deceleration is enumerated as an int, this value is required
		// to provide fine tweaking of the deceleration..
		const float DecelerationTweaker = 0.3;

		// Calculate the speed required to reach the target given the desired
		// deceleration
		float speed = dist / ((double)deceleration * DecelerationTweaker);

		// Make sure the velocity does not exceed the max
		speed = min(speed, MaxSpeed);

		// From here proceed just like Seek except we don't need to normalize
		// the ToTarget vector because we have already gone to the trouble
		// of calculating its length: dist.
		Vector2 desiredVelocity = toTarget * speed / dist;

		arrive = (desiredVelocity - _physicsBody->GetVelocity());
	}

	return arrive;
}

Vector2 Steering::OffsetPursuit(const PhysicsBody2D* agent, const Vector2& offset)
{
	if (agent == nullptr)
		return Vector2();

	// Get vehicle coordinate frame
	Vector2 target = agent->GetToWorld(offset);

	// If the evader is ahead and facing the agent then we can just seek
	// for the evader's current position.
	Vector2 toTarget = target - _physicsBody->GetPosition();

	gDebugRenderer.AddLine(ToVector3(target), ToVector3(_physicsBody->GetPosition()));

	// The lookahead time is propotional to the distance between the evader
	// and the pursuer; and is inversely proportional to the sum of the
	// agent's velocities
	float LookAheadTime = toTarget.Magnitude() /
		(MaxSpeed + agent->GetVelocity().Magnitude()) * 0.1f;

	// Now seek to the predicted future position of the evader
	target = target + agent->GetVelocity() * LookAheadTime;

	gDebugRenderer.AddCircle(ToVector3(target));

	return Seek(target);
}

Vector2 Steering::Evade(const PhysicsBody2D* agent)
{
	// Not necessary to include the check for facing direction this time
	Vector2 toPursuer = agent->GetPosition() - _physicsBody->GetPosition();

	//uncomment the following two lines to have Evade only consider pursuers
	//within a 'threat range'
	if (toPursuer.SquareMagnitude() > EvadeDistance * EvadeDistance)
		return Vector2();

	//the lookahead time is propotional to the distance between the pursuer
	//and the pursuer; and is inversely proportional to the sum of the
	//agents' velocities
	float lookAheadTime = toPursuer.Magnitude() / (MaxSpeed + agent->GetVelocity().Magnitude());
	lookAheadTime *= 0.1f;

	//now flee away from predicted future position of the pursuer
	Vector2 target = agent->GetPosition() + agent->GetVelocity() * lookAheadTime;
	return -1.0f * Flee(target);
}

Vector2 Steering::Wander()
{

	Vector2 fwd = _physicsBody->GetVelocity();
	fwd.Normalize();
	Matrix33 transform;
	transform.SetTransform(
		fwd.Perpendicular(),
		Vector2(1.0f, 1.0f),
		_physicsBody->GetPosition());

	Vector2 wander;

	// This behavior is dependent on the update rate, so this line must
	// be included when using time independent framerate.
	float JitterThisTimeSlice = WanderJitter * 0.016f; // m_pVehicle->TimeElapsed();

													   // First, add a small random vector to the target's position
	_wanderTarget += Vector2(RandInRange(-1.0f, 1.0f) * JitterThisTimeSlice,
		RandInRange(-1.0f, 1.0f) * JitterThisTimeSlice);

	// Reproject this new vector back on to a unit circle
	_wanderTarget.Normalize();

	//increase the length of the vector to the same as the radius
	//of the wander circle
	_wanderTarget *= WanderRadius;

	//move the target into a position WanderDist in front of the agent
	//Vector2 target = _wanderTarget + Vector2(0, WanderDistance);
	Vector2 target = _wanderTarget + Vector2(0, WanderDistance);

	//project the target into world space
	target = transform.TransformVector(target); // _physicsBody->GetToWorld(target);

	//Vector2 center = _physicsBody->GetToWorld(Vector2(0, WanderDistance));
	Vector2 center = transform.TransformVector(Vector2(0, WanderDistance));

	gDebugRenderer.AddCircle(ToVector3(center), WanderRadius);

	gDebugRenderer.AddCircle(ToVector3(target), 0.5f, Color::Yellow);

	// And steer towards it
	wander = target - _physicsBody->GetPosition();

	/*
	Vector2 wander;

	// This behavior is dependent on the update rate, so this line must
	// be included when using time independent framerate.
	float JitterThisTimeSlice = WanderJitter * 0.016f; // m_pVehicle->TimeElapsed();

	// First, add a small random vector to the target's position
	_wanderTarget += Vector2(	RandInRange(-1.0f, 1.0f) * JitterThisTimeSlice,
								RandInRange(-1.0f, 1.0f) * JitterThisTimeSlice);

	// Reproject this new vector back on to a unit circle
	_wanderTarget.Normalize();

	//increase the length of the vector to the same as the radius
	//of the wander circle
	_wanderTarget *= WanderRadius;

	//move the target into a position WanderDist in front of the agent
	//Vector2 target = _wanderTarget + Vector2(0, WanderDistance);
	Vector2 target = _wanderTarget + Vector2(0, WanderDistance);

	//project the target into world space
	target = _physicsBody->GetToWorld(target);

	Vector2 center = _physicsBody->GetToWorld(Vector2(0, WanderDistance));

	gDebugRenderer.AddCircle(ToVector3(center), WanderRadius);

	gDebugRenderer.AddCircle(ToVector3(target), 0.5f, Color::Yellow);

	// And steer towards it
	wander = target - _physicsBody->GetPosition();
	*/

	return wander;
}

vector<PhysicsBody2D*> Steering::GetFlockingNeighbors()
{
	vector<PhysicsBody2D*> neighbors;
	Vector2 v = _physicsBody->GetPosition();
	neighbors = _physicsManager->GetInRadius(v, FlockingRadius);
	if (FlockingTag != 0)
	{
		remove_if(	neighbors.begin(),
					neighbors.end(),
					[this](PhysicsBody2D* b)
		{
			return b->GetOwner().GetTag() != FlockingTag;
		});
	}
	return neighbors;
}

Vector2 Steering::Cohesion(const std::vector<PhysicsBody2D*>& agents)
{
	Vector2 cohesion;

	// First find the center of mass of all the agents
	Vector2 centerOfMass, steeringForce;

	size_t neighborCount = 0;

	// Iterate through the neighbors and sum up all the position vectors
	for(auto a : agents)
	{
		if(a != _physicsBody && a != Agent)
		{
			centerOfMass += a->GetPosition();
			neighborCount++;
		}
	}

	if (neighborCount > 0)
	{
		// The center of mass is the average of the sum of positions
		centerOfMass *= 1.0f / (float)neighborCount;

		//now seek towards that position
		cohesion = Seek(centerOfMass);
	}

	cohesion.Normalize();

	return cohesion;
}

Vector2 Steering::Separation(const std::vector<PhysicsBody2D*>& agents)
{
	Vector2 steeringForce;

	// iterate through the neighbors and sum up all the position vectors
	for (auto a : agents)
	{
		// make sure this agent isn't included in the calculations and that
		// the agent being examined is close enough
		if (a != _physicsBody && a != Agent)
		{
			Vector2 toAgent =_physicsBody->GetPosition() - a->GetPosition();

			// scale the force inversely proportional to the agents distance
			// from its neighbor.
			float d = toAgent.Magnitude();
			if (d > 0.01)
			{
				toAgent.Normalize();
				steeringForce += toAgent / d;
			}
		}

	}

	return steeringForce;
}

Vector2 Steering::Alignment(const std::vector<PhysicsBody2D*>& agents)
{
	//This will record the average heading of the neighbors
	Vector2 averageHeading;

	//This count the number of vehicles in the neighborhood
	float neighborCount = 0.0;

	Vector2 currentHeading = _physicsBody->GetFroward();
	currentHeading.Normalize();

	//iterate through the neighbors and sum up all the position vectors
	for (auto a : agents)
	{
		// make sure this agent isn't included in the calculations and that
		// the agent being examined is close enough
		if (a != _physicsBody && a != Agent)
		{
			Vector2 heading = a->GetFroward();
			heading.Normalize();
			averageHeading += heading;
			neighborCount++;
		}

	}

	// If the neighborhood contained one or more vehicles, average their
	// heading vectors.
	if (neighborCount > 0.0)
	{
		averageHeading *= 1.0f / neighborCount;
		averageHeading -= currentHeading;
	}

	return averageHeading;
}

void Steering::DebugRender()
{
	if (_inspect)
	{
		auto pos = _physicsBody->GetPosition();
		gDebugRenderer.AddCircle(
			ToVector3(pos),
			_physicsBody->GetRaduis(),
			Color::Purple);
		gDebugRenderer.AddLine(
			ToVector3(pos),
			ToVector3(pos + _current),
			Color::Orange);
		gDebugRenderer.AddCircle(
			ToVector3(pos),
			FlockingRadius,
			Color::Purple);
	}
	_inspect = false;
}

#ifdef INSPECTOR
void Steering::Inspect()
{
	_inspect = true;

	ImGui::InputFloat("Max Accelearation", &MaxAccelearation);
	ImGui::InputFloat("Max Force", &MaxForce);
	ImGui::InputFloat("Max Speed", &MaxSpeed);
	ImGui::InputFloat("Wander Jitter", &WanderJitter);
	ImGui::InputFloat("Wander Radius", &WanderRadius);
	ImGui::InputFloat("Wander Distance", &WanderDistance);

	float maxed = _current.Magnitude() / MaxSpeed;
	ImGui::ProgressBar(maxed);
	// ImGui::SameLine();
	//ImGui::LabelText("Total Used ","");
}
#endif

/*
PhysicsBody2D& SteeringBehaviour::GetPhysicsBody() const
{
	auto physics = GetOwner().GetPhysicsBody();
	ASSERT(physics);
	return *physics;
}

SteeringOutput Seek::GetSteering()
{
	SteeringOutput so;

	so.Linear = Target - GetPhysicsBody().GetPosition();
	so.Linear.Normalize();
	so.Linear *= GetOwner().MaxAccelearation;

	return so;
}
*/<|endoftext|>


<commit_before>/*************************************************************************
 *
 *  $RCSfile: transliteration_commonclass.cxx,v $
 *
 *  $Revision: 1.4 $
 *
 *  last change: $Author: vg $ $Date: 2003-06-12 10:50:04 $
 *
 *  The Contents of this file are made available subject to the terms of
 *  either of the following licenses
 *
 *         - GNU Lesser General Public License Version 2.1
 *         - Sun Industry Standards Source License Version 1.1
 *
 *  Sun Microsystems Inc., October, 2000
 *
 *  GNU Lesser General Public License Version 2.1
 *  =============================================
 *  Copyright 2000 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
 *
 *
 *  Sun Industry Standards Source License Version 1.1
 *  =================================================
 *  The contents of this file are subject to the Sun Industry Standards
 *  Source License Version 1.1 (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.openoffice.org/license.html.
 *
 *  Software provided under this License is provided on an "AS IS" basis,
 *  WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING,
 *  WITHOUT LIMITATION, WARRANTIES THAT THE SOFTWARE IS FREE OF DEFECTS,
 *  MERCHANTABLE, FIT FOR A PARTICULAR PURPOSE, OR NON-INFRINGING.
 *  See the License for the specific provisions governing your rights and
 *  obligations concerning the Software.
 *
 *  The Initial Developer of the Original Code is: Sun Microsystems, Inc.
 *
 *  Copyright: 2000 by Sun Microsystems, Inc.
 *
 *  All Rights Reserved.
 *
 *  Contributor(s): _______________________________________
 *
 *
 ************************************************************************/

#include <transliteration_commonclass.hxx>
#include <com/sun/star/i18n/CollatorOptions.hpp>

using namespace ::com::sun::star::uno;
using namespace ::com::sun::star::lang;
using namespace ::rtl;

namespace com { namespace sun { namespace star { namespace i18n {

transliteration_commonclass::transliteration_commonclass()
{
        transliterationName = "";
        implementationName = "";
        useOffset = sal_True;
}

OUString SAL_CALL transliteration_commonclass::getName() throw(RuntimeException)
{
        return OUString::createFromAscii(transliterationName);
}

void SAL_CALL transliteration_commonclass::loadModule( TransliterationModules modName, const Locale& rLocale )
        throw(RuntimeException)
{
        aLocale = rLocale;
}

void SAL_CALL
transliteration_commonclass::loadModuleNew( const Sequence < TransliterationModulesNew >& modName, const Locale& rLocale )
        throw(RuntimeException)
{
        throw RuntimeException();
}

void SAL_CALL
transliteration_commonclass::loadModuleByImplName( const OUString& implName, const Locale& rLocale )
        throw(RuntimeException)
{
        throw RuntimeException();
}

void SAL_CALL
transliteration_commonclass::loadModulesByImplNames(const Sequence< OUString >& modNamelist, const Locale& rLocale)
        throw(RuntimeException)
{
        throw RuntimeException();
}

Sequence< OUString > SAL_CALL
transliteration_commonclass::getAvailableModules( const Locale& rLocale, sal_Int16 sType )
        throw(RuntimeException)
{
        throw RuntimeException();
}

sal_Int32 SAL_CALL
transliteration_commonclass::compareSubstring(
        const OUString& str1, sal_Int32 off1, sal_Int32 len1,
        const OUString& str2, sal_Int32 off2, sal_Int32 len2)
        throw(RuntimeException)
{
        const sal_Unicode* unistr1 = NULL;
        const sal_Unicode* unistr2 = NULL;
        sal_uInt32 strlen1;
        sal_uInt32 strlen2;

        Sequence <long> offset1(2*len1);
        Sequence <long> offset2(2*len2);

        OUString in_str1 = this->transliterate(str1, off1, len1, offset1);
        OUString in_str2 = this->transliterate(str2, off2, len2, offset2);
        strlen1 = in_str1.getLength();
        strlen2 = in_str2.getLength();
        unistr1 = in_str1.getStr();
        unistr2 = in_str2.getStr();

        while (strlen1 && strlen2)
        {
            sal_uInt32 ret = *unistr1 - *unistr2;
            if (ret)
               return ret;

            unistr1++;
            unistr2++;
            strlen1--;
            strlen2--;
        }
        return strlen1 - strlen2;
}

sal_Int32 SAL_CALL
transliteration_commonclass::compareString( const OUString& str1, const OUString& str2 ) throw ( RuntimeException)
{
        return( this->compareSubstring(str1, 0, str1.getLength(), str2, 0, str2.getLength()));
}

OUString SAL_CALL
transliteration_commonclass::transliterateString2String( const OUString& inStr, sal_Int32 startPos, sal_Int32 nCount ) throw(RuntimeException)
{
    static Sequence < sal_Int32 > dummy_offset;
    useOffset = sal_False;
    OUString tmpStr = transliterate(inStr, startPos, nCount, dummy_offset);
    useOffset = sal_True;
    return tmpStr;
}

OUString SAL_CALL
transliteration_commonclass::transliterateChar2String( sal_Unicode inChar ) throw(RuntimeException)
{
    return transliteration_commonclass::transliterateString2String(OUString(&inChar, 1), 0, 1);
}

OUString SAL_CALL transliteration_commonclass::getImplementationName() throw( RuntimeException )
{
    return OUString::createFromAscii(implementationName);
}

const sal_Char cTrans[] = "com.sun.star.i18n.Transliteration.l10n";

sal_Bool SAL_CALL transliteration_commonclass::supportsService(const OUString& rServiceName) throw( RuntimeException )
{
        return rServiceName.equalsAscii(cTrans);
}

Sequence< OUString > SAL_CALL transliteration_commonclass::getSupportedServiceNames() throw( RuntimeException )
{
        Sequence< OUString > aRet(1);
        aRet[0] = OUString::createFromAscii(cTrans);
        return aRet;
}

} } } }
<commit_msg>INTEGRATION: CWS ooo64bit01 (1.4.78); FILE MERGED 2004/04/12 17:45:58 fa 1.4.78.3: 64-bit fixes (long->sal_Int32) 2004/03/31 16:48:30 fa 1.4.78.2: Back out of changes since 2004-03-24, focus on smaller, more integratable changes 2004/03/29 00:29:45 fa 1.4.78.1: More 64-bit changes.  'rsc' still broken, but not as much as before.<commit_after>/*************************************************************************
 *
 *  $RCSfile: transliteration_commonclass.cxx,v $
 *
 *  $Revision: 1.5 $
 *
 *  last change: $Author: rt $ $Date: 2004-06-17 12:35:43 $
 *
 *  The Contents of this file are made available subject to the terms of
 *  either of the following licenses
 *
 *         - GNU Lesser General Public License Version 2.1
 *         - Sun Industry Standards Source License Version 1.1
 *
 *  Sun Microsystems Inc., October, 2000
 *
 *  GNU Lesser General Public License Version 2.1
 *  =============================================
 *  Copyright 2000 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
 *
 *
 *  Sun Industry Standards Source License Version 1.1
 *  =================================================
 *  The contents of this file are subject to the Sun Industry Standards
 *  Source License Version 1.1 (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.openoffice.org/license.html.
 *
 *  Software provided under this License is provided on an "AS IS" basis,
 *  WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING,
 *  WITHOUT LIMITATION, WARRANTIES THAT THE SOFTWARE IS FREE OF DEFECTS,
 *  MERCHANTABLE, FIT FOR A PARTICULAR PURPOSE, OR NON-INFRINGING.
 *  See the License for the specific provisions governing your rights and
 *  obligations concerning the Software.
 *
 *  The Initial Developer of the Original Code is: Sun Microsystems, Inc.
 *
 *  Copyright: 2000 by Sun Microsystems, Inc.
 *
 *  All Rights Reserved.
 *
 *  Contributor(s): _______________________________________
 *
 *
 ************************************************************************/

#include <transliteration_commonclass.hxx>
#include <com/sun/star/i18n/CollatorOptions.hpp>

using namespace ::com::sun::star::uno;
using namespace ::com::sun::star::lang;
using namespace ::rtl;

namespace com { namespace sun { namespace star { namespace i18n {

transliteration_commonclass::transliteration_commonclass()
{
        transliterationName = "";
        implementationName = "";
        useOffset = sal_True;
}

OUString SAL_CALL transliteration_commonclass::getName() throw(RuntimeException)
{
        return OUString::createFromAscii(transliterationName);
}

void SAL_CALL transliteration_commonclass::loadModule( TransliterationModules modName, const Locale& rLocale )
        throw(RuntimeException)
{
        aLocale = rLocale;
}

void SAL_CALL
transliteration_commonclass::loadModuleNew( const Sequence < TransliterationModulesNew >& modName, const Locale& rLocale )
        throw(RuntimeException)
{
        throw RuntimeException();
}

void SAL_CALL
transliteration_commonclass::loadModuleByImplName( const OUString& implName, const Locale& rLocale )
        throw(RuntimeException)
{
        throw RuntimeException();
}

void SAL_CALL
transliteration_commonclass::loadModulesByImplNames(const Sequence< OUString >& modNamelist, const Locale& rLocale)
        throw(RuntimeException)
{
        throw RuntimeException();
}

Sequence< OUString > SAL_CALL
transliteration_commonclass::getAvailableModules( const Locale& rLocale, sal_Int16 sType )
        throw(RuntimeException)
{
        throw RuntimeException();
}

sal_Int32 SAL_CALL
transliteration_commonclass::compareSubstring(
        const OUString& str1, sal_Int32 off1, sal_Int32 len1,
        const OUString& str2, sal_Int32 off2, sal_Int32 len2)
        throw(RuntimeException)
{
        const sal_Unicode* unistr1 = NULL;
        const sal_Unicode* unistr2 = NULL;
        sal_uInt32 strlen1;
        sal_uInt32 strlen2;

        Sequence <sal_Int32> offset1(2*len1);
        Sequence <sal_Int32> offset2(2*len2);

        OUString in_str1 = this->transliterate(str1, off1, len1, offset1);
        OUString in_str2 = this->transliterate(str2, off2, len2, offset2);
        strlen1 = in_str1.getLength();
        strlen2 = in_str2.getLength();
        unistr1 = in_str1.getStr();
        unistr2 = in_str2.getStr();

        while (strlen1 && strlen2)
        {
            sal_uInt32 ret = *unistr1 - *unistr2;
            if (ret)
               return ret;

            unistr1++;
            unistr2++;
            strlen1--;
            strlen2--;
        }
        return strlen1 - strlen2;
}

sal_Int32 SAL_CALL
transliteration_commonclass::compareString( const OUString& str1, const OUString& str2 ) throw ( RuntimeException)
{
        return( this->compareSubstring(str1, 0, str1.getLength(), str2, 0, str2.getLength()));
}

OUString SAL_CALL
transliteration_commonclass::transliterateString2String( const OUString& inStr, sal_Int32 startPos, sal_Int32 nCount ) throw(RuntimeException)
{
    static Sequence < sal_Int32 > dummy_offset;
    useOffset = sal_False;
    OUString tmpStr = transliterate(inStr, startPos, nCount, dummy_offset);
    useOffset = sal_True;
    return tmpStr;
}

OUString SAL_CALL
transliteration_commonclass::transliterateChar2String( sal_Unicode inChar ) throw(RuntimeException)
{
    return transliteration_commonclass::transliterateString2String(OUString(&inChar, 1), 0, 1);
}

OUString SAL_CALL transliteration_commonclass::getImplementationName() throw( RuntimeException )
{
    return OUString::createFromAscii(implementationName);
}

const sal_Char cTrans[] = "com.sun.star.i18n.Transliteration.l10n";

sal_Bool SAL_CALL transliteration_commonclass::supportsService(const OUString& rServiceName) throw( RuntimeException )
{
        return rServiceName.equalsAscii(cTrans);
}

Sequence< OUString > SAL_CALL transliteration_commonclass::getSupportedServiceNames() throw( RuntimeException )
{
        Sequence< OUString > aRet(1);
        aRet[0] = OUString::createFromAscii(cTrans);
        return aRet;
}

} } } }
<|endoftext|>