<commit_before>// @(#)root/xml:$Name:  $:$Id: TXMLEngine.cxx,v 1.4 2004/05/11 18:52:17 brun Exp $
// Author: Sergey Linev  10.05.2004

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

//________________________________________________________________________
//
//  The aim of TXMLEngine class is to provide interface to libxml2 library.
//  It defines only used in ROOT functions of libxml2
//  There is a problem to parse libxml2 include files through CINT,
//  therefore several wrappers for libxml2 types are defined.
//  More information about libxml2 itself can be found in http://xmlsoft.org
//________________________________________________________________________

#include "TXMLEngine.h"

#include "Riostream.h"
#include "libxml/tree.h"

ClassImp(TXMLEngine);

//______________________________________________________________________________
TXMLEngine::TXMLEngine()
{

};

//______________________________________________________________________________
TXMLEngine::~TXMLEngine()
{
}

//______________________________________________________________________________
Bool_t TXMLEngine::HasAttr(xmlNodePointer node, const char* name) {

   if ((node==0) || (name==0)) return kFALSE;
   return xmlHasProp((xmlNodePtr) node, (const xmlChar*) name) != 0;
}

//______________________________________________________________________________
const char* TXMLEngine::GetAttr(xmlNodePointer node, const char* name) {
   if (node==0) return 0;
   xmlChar* prop = xmlGetProp((xmlNodePtr) node, (const xmlChar*) name);
   if (prop) {
      fStrBuf = (const char*) prop;
      xmlFree(prop);
      return fStrBuf.Data();
   }
   return 0;
}

//______________________________________________________________________________
Int_t TXMLEngine::GetIntAttr(xmlNodePointer node, const char* name) {
   if (node==0) return 0;

   Int_t res = 0;

   xmlChar* prop = xmlGetProp((xmlNodePtr) node, (const xmlChar*) name);
   if (prop) {
      sscanf((const char*) prop,"%d", &res);
      xmlFree(prop);
   }
   return res;
}

//______________________________________________________________________________
xmlAttrPointer TXMLEngine::NewAttr(xmlNodePointer node,
                                   xmlNsPointer ns,
                                   const char* name,
                                   const char* value) {
   if (ns==0)
      return (xmlAttrPointer) xmlNewProp((xmlNodePtr) node,
                                         (const xmlChar*) name,
                                         (const xmlChar*) value);
   else
      return (xmlAttrPointer) xmlNewNsProp((xmlNodePtr) node,
                                           (xmlNsPtr) ns,
                                           (const xmlChar*) name,
                                           (const xmlChar*) value);
}

//______________________________________________________________________________
xmlAttrPointer TXMLEngine::NewIntAttr(xmlNodePointer node,
                                      const char* name,
                                      Int_t value)
{
  char sbuf[30];
  sprintf(sbuf,"%d",value);
  return NewAttr(node, 0, name, sbuf);
}

//______________________________________________________________________________
void TXMLEngine::FreeAttr(xmlNodePointer node, const char* name)
{
   if ((node==0) || (name==0)) return;
   xmlAttrPtr attr = xmlHasProp((xmlNodePtr) node, (const xmlChar*) name);
   if (attr!=0) xmlRemoveProp(attr);
}

//______________________________________________________________________________
xmlNodePointer TXMLEngine::NewChild(xmlNodePointer parent,
                                    xmlNsPointer ns,
                                    const char* name,
                                    const char* content) {
   xmlNodePointer node = xmlNewNode((xmlNsPtr) ns, (const xmlChar*) name);
   if (parent!=0)
     xmlAddChild((xmlNodePtr) parent, (xmlNodePtr) node);
   if (content!=0)
     xmlNodeAddContent((xmlNodePtr) node, (const xmlChar*) content);
   return node;
}

//______________________________________________________________________________
xmlNsPointer TXMLEngine::NewNS(xmlNodePointer node,
                               const char* reference,
                               const char* name) {
   return (xmlNsPointer) xmlNewNs((xmlNodePtr) node, (const xmlChar*) reference, (const xmlChar*) name);
}

//______________________________________________________________________________
void TXMLEngine::AddChild(xmlNodePointer parent, xmlNodePointer child) {
   xmlAddChild((xmlNodePtr) parent, (xmlNodePtr) child);
}

//______________________________________________________________________________
void TXMLEngine::UnlinkNode(xmlNodePointer node) {
   if (node!=0)
     xmlUnlinkNode((xmlNodePtr) node);
}

//______________________________________________________________________________
void TXMLEngine::FreeNode(xmlNodePointer node) {
   xmlFreeNode((xmlNodePtr) node);
}

//______________________________________________________________________________
const char* TXMLEngine::GetNodeName(xmlNodePointer node) {
   if (node==0) return 0;
   return (const char*) ((xmlNodePtr) node)->name;
}

//______________________________________________________________________________
const char* TXMLEngine::GetNodeContent(xmlNodePointer node) {
   if (node==0) return 0;
   xmlChar* cont = xmlNodeGetContent((xmlNodePtr) node);
   if (cont) {
      fStrBuf = (const char*) cont;
      xmlFree(cont);
      return fStrBuf.Data();
   }
   return 0;
}

//______________________________________________________________________________
xmlNodePointer TXMLEngine::GetChild(xmlNodePointer node) {
   if (node==0) return 0;
   return ((xmlNodePtr) node)->xmlChildrenNode;
}

//______________________________________________________________________________
xmlNodePointer TXMLEngine::GetParent(xmlNodePointer node) {
   if (node==0) return 0;
   return ((xmlNodePtr) node)->parent;
}

//______________________________________________________________________________
xmlNodePointer TXMLEngine::GetNext(xmlNodePointer node) {
   if (node==0) return 0;
   return ((xmlNodePtr) node)->next;
}

//______________________________________________________________________________
Bool_t TXMLEngine::IsEmptyNode(xmlNodePointer node)
{
// checks if provided node is empty
  if (node==0) return kTRUE;
  return xmlIsBlankNode((xmlNodePtr) node);
}

//______________________________________________________________________________
void TXMLEngine::CleanNode(xmlNodePointer node) {
   if (node==0) return;
   xmlNodePointer child = GetChild(node);
   while (child!=0) {
      xmlNodePointer next = GetNext(child);
      UnlinkNode(child);
      FreeNode(child);
      child = next;
   }
}

//______________________________________________________________________________
void TXMLEngine::ShiftToNext(xmlNodePointer &node, Bool_t skipempty) {
   if (node==0) return;
   node = ((xmlNodePtr) node)->next;

   if (skipempty)
     SkipEmpty(node);
}

//______________________________________________________________________________
void TXMLEngine::SkipEmpty(xmlNodePointer &node) {
   while (node && IsEmptyNode(node))
     node = ((xmlNodePtr) node) ->next;
}

//______________________________________________________________________________
xmlDocPointer TXMLEngine::NewDoc(const char* version) {
   return xmlNewDoc((const xmlChar*)version);
}

//______________________________________________________________________________
void TXMLEngine::AssignDtd(xmlDocPointer doc, const char* dtdname, const char* rootname) {
   xmlCreateIntSubset((xmlDocPtr) doc,
                      (const xmlChar*) rootname,
                      (const xmlChar*) "-//CERN//ROOT//v 1.0//EN",
                      (const xmlChar*) dtdname);
}

//______________________________________________________________________________
void TXMLEngine::FreeDoc(xmlDocPointer doc) {
   xmlFreeDoc((xmlDocPtr) doc);
}

//______________________________________________________________________________
void TXMLEngine::SaveDoc(xmlDocPointer doc, const char* filename, Int_t layout) {
   xmlSaveFormatFile(filename, (xmlDocPtr) doc, layout);
}

//______________________________________________________________________________
void TXMLEngine::DocSetRootElement(xmlDocPointer doc, xmlNodePointer node) {
   xmlDocSetRootElement((xmlDocPtr) doc, (xmlNodePtr) node);
}

//______________________________________________________________________________
xmlNodePointer TXMLEngine::DocGetRootElement(xmlDocPointer doc) {
   return (xmlNodePointer) xmlDocGetRootElement((xmlDocPtr) doc);
}

//______________________________________________________________________________
xmlDocPointer TXMLEngine::ParseFile(const char* filename) {
   return (xmlDocPointer) xmlParseFile(filename);
}

//______________________________________________________________________________
Bool_t TXMLEngine::ValidateDocument(xmlDocPointer doc, Bool_t doout) {
	xmlValidCtxt cvp;
	cvp.userData = doout ? (void *) stderr : 0;
//	cvp.error    = doout ? (xmlValidityErrorFunc) fprintf : 0;
//	cvp.warning  = doout ? (xmlValidityWarningFunc) fprintf : 0;
// debug output excluded to solve problem for Solaris compilation
	cvp.error    = 0;
	cvp.warning  = 0;

    xmlDocPtr docptr = (xmlDocPtr) doc;

    int res = xmlValidateDocument(&cvp, docptr);

    if ((docptr->intSubset!=0) || (docptr->extSubset!=0)) {
       xmlDtdPtr i = docptr->intSubset;
       xmlDtdPtr e = docptr->extSubset;
       if (i==e) e = 0;
       if (e!=0) {
         xmlUnlinkNode((xmlNodePtr) docptr->extSubset);
         docptr->extSubset = 0;
         xmlFreeDtd(e);
       }
       if (i!=0) {
         xmlUnlinkNode((xmlNodePtr) docptr->intSubset);
         docptr->intSubset = 0;
         xmlFreeDtd(i);
       }
    }

    if (doout)
      if (res==1) cout << "Validation done" << endl;
             else cout << "Validation failed" << endl;

    return (res == 1);
}

<commit_msg>From Bertrand Bellenot -In case of old VC++6 add declaration of external functions _ftol and _ftol2<commit_after>// @(#)root/xml:$Name:  $:$Id: TXMLEngine.cxx,v 1.6 2004/05/14 14:30:46 brun Exp $
// Author: Sergey Linev  10.05.2004

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

//________________________________________________________________________
//
//  The aim of TXMLEngine class is to provide interface to libxml2 library.
//  It defines only used in ROOT functions of libxml2
//  There is a problem to parse libxml2 include files through CINT,
//  therefore several wrappers for libxml2 types are defined.
//  More information about libxml2 itself can be found in http://xmlsoft.org
//________________________________________________________________________

#include "TXMLEngine.h"

#include "Riostream.h"
#include "libxml/tree.h"

ClassImp(TXMLEngine);

#if (_MSC_VER == 1200) && (WINVER < 0x0500)
extern "C" long _ftol( double ); //defined by VC6 C libs
extern "C" long _ftol2( double dblSource ) { return _ftol( dblSource ); }
#endif

//______________________________________________________________________________
TXMLEngine::TXMLEngine()
{

};

//______________________________________________________________________________
TXMLEngine::~TXMLEngine()
{
}

//______________________________________________________________________________
Bool_t TXMLEngine::HasAttr(xmlNodePointer node, const char* name) {

   if ((node==0) || (name==0)) return kFALSE;
   return xmlHasProp((xmlNodePtr) node, (const xmlChar*) name) != 0;
}

//______________________________________________________________________________
const char* TXMLEngine::GetAttr(xmlNodePointer node, const char* name) {
   if (node==0) return 0;
   xmlChar* prop = xmlGetProp((xmlNodePtr) node, (const xmlChar*) name);
   if (prop) {
      fStrBuf = (const char*) prop;
      xmlFree(prop);
      return fStrBuf.Data();
   }
   return 0;
}

//______________________________________________________________________________
Int_t TXMLEngine::GetIntAttr(xmlNodePointer node, const char* name) {
   if (node==0) return 0;

   Int_t res = 0;

   xmlChar* prop = xmlGetProp((xmlNodePtr) node, (const xmlChar*) name);
   if (prop) {
      sscanf((const char*) prop,"%d", &res);
      xmlFree(prop);
   }
   return res;
}

//______________________________________________________________________________
xmlAttrPointer TXMLEngine::NewAttr(xmlNodePointer node,
                                   xmlNsPointer ns,
                                   const char* name,
                                   const char* value) {
   if (ns==0)
      return (xmlAttrPointer) xmlNewProp((xmlNodePtr) node,
                                         (const xmlChar*) name,
                                         (const xmlChar*) value);
   else
      return (xmlAttrPointer) xmlNewNsProp((xmlNodePtr) node,
                                           (xmlNsPtr) ns,
                                           (const xmlChar*) name,
                                           (const xmlChar*) value);
}

//______________________________________________________________________________
xmlAttrPointer TXMLEngine::NewIntAttr(xmlNodePointer node,
                                      const char* name,
                                      Int_t value)
{
  char sbuf[30];
  sprintf(sbuf,"%d",value);
  return NewAttr(node, 0, name, sbuf);
}

//______________________________________________________________________________
void TXMLEngine::FreeAttr(xmlNodePointer node, const char* name)
{
   if ((node==0) || (name==0)) return;
   xmlAttrPtr attr = xmlHasProp((xmlNodePtr) node, (const xmlChar*) name);
   if (attr!=0) xmlRemoveProp(attr);
}

//______________________________________________________________________________
xmlNodePointer TXMLEngine::NewChild(xmlNodePointer parent,
                                    xmlNsPointer ns,
                                    const char* name,
                                    const char* content) {
   xmlNodePointer node = xmlNewNode((xmlNsPtr) ns, (const xmlChar*) name);
   if (parent!=0)
     xmlAddChild((xmlNodePtr) parent, (xmlNodePtr) node);
   if (content!=0)
     xmlNodeAddContent((xmlNodePtr) node, (const xmlChar*) content);
   return node;
}

//______________________________________________________________________________
xmlNsPointer TXMLEngine::NewNS(xmlNodePointer node,
                               const char* reference,
                               const char* name) {
   return (xmlNsPointer) xmlNewNs((xmlNodePtr) node, (const xmlChar*) reference, (const xmlChar*) name);
}

//______________________________________________________________________________
void TXMLEngine::AddChild(xmlNodePointer parent, xmlNodePointer child) {
   xmlAddChild((xmlNodePtr) parent, (xmlNodePtr) child);
}

//______________________________________________________________________________
void TXMLEngine::UnlinkNode(xmlNodePointer node) {
   if (node!=0)
     xmlUnlinkNode((xmlNodePtr) node);
}

//______________________________________________________________________________
void TXMLEngine::FreeNode(xmlNodePointer node) {
   xmlFreeNode((xmlNodePtr) node);
}

//______________________________________________________________________________
const char* TXMLEngine::GetNodeName(xmlNodePointer node) {
   if (node==0) return 0;
   return (const char*) ((xmlNodePtr) node)->name;
}

//______________________________________________________________________________
const char* TXMLEngine::GetNodeContent(xmlNodePointer node) {
   if (node==0) return 0;
   xmlChar* cont = xmlNodeGetContent((xmlNodePtr) node);
   if (cont) {
      fStrBuf = (const char*) cont;
      xmlFree(cont);
      return fStrBuf.Data();
   }
   return 0;
}

//______________________________________________________________________________
xmlNodePointer TXMLEngine::GetChild(xmlNodePointer node) {
   if (node==0) return 0;
   return ((xmlNodePtr) node)->xmlChildrenNode;
}

//______________________________________________________________________________
xmlNodePointer TXMLEngine::GetParent(xmlNodePointer node) {
   if (node==0) return 0;
   return ((xmlNodePtr) node)->parent;
}

//______________________________________________________________________________
xmlNodePointer TXMLEngine::GetNext(xmlNodePointer node) {
   if (node==0) return 0;
   return ((xmlNodePtr) node)->next;
}

//______________________________________________________________________________
Bool_t TXMLEngine::IsEmptyNode(xmlNodePointer node)
{
// checks if provided node is empty
  if (node==0) return kTRUE;
  return xmlIsBlankNode((xmlNodePtr) node);
}

//______________________________________________________________________________
void TXMLEngine::CleanNode(xmlNodePointer node) {
   if (node==0) return;
   xmlNodePointer child = GetChild(node);
   while (child!=0) {
      xmlNodePointer next = GetNext(child);
      UnlinkNode(child);
      FreeNode(child);
      child = next;
   }
}

//______________________________________________________________________________
void TXMLEngine::ShiftToNext(xmlNodePointer &node, Bool_t skipempty) {
   if (node==0) return;
   node = ((xmlNodePtr) node)->next;

   if (skipempty)
     SkipEmpty(node);
}

//______________________________________________________________________________
void TXMLEngine::SkipEmpty(xmlNodePointer &node) {
   while (node && IsEmptyNode(node))
     node = ((xmlNodePtr) node) ->next;
}

//______________________________________________________________________________
xmlDocPointer TXMLEngine::NewDoc(const char* version) {
   return xmlNewDoc((const xmlChar*)version);
}

//______________________________________________________________________________
void TXMLEngine::AssignDtd(xmlDocPointer doc, const char* dtdname, const char* rootname) {
   xmlCreateIntSubset((xmlDocPtr) doc,
                      (const xmlChar*) rootname,
                      (const xmlChar*) "-//CERN//ROOT//v 1.0//EN",
                      (const xmlChar*) dtdname);
}

//______________________________________________________________________________
void TXMLEngine::FreeDoc(xmlDocPointer doc) {
   xmlFreeDoc((xmlDocPtr) doc);
}

//______________________________________________________________________________
void TXMLEngine::SaveDoc(xmlDocPointer doc, const char* filename, Int_t layout) {
   xmlSaveFormatFile(filename, (xmlDocPtr) doc, layout);
}

//______________________________________________________________________________
void TXMLEngine::DocSetRootElement(xmlDocPointer doc, xmlNodePointer node) {
   xmlDocSetRootElement((xmlDocPtr) doc, (xmlNodePtr) node);
}

//______________________________________________________________________________
xmlNodePointer TXMLEngine::DocGetRootElement(xmlDocPointer doc) {
   return (xmlNodePointer) xmlDocGetRootElement((xmlDocPtr) doc);
}

//______________________________________________________________________________
xmlDocPointer TXMLEngine::ParseFile(const char* filename) {
   return (xmlDocPointer) xmlParseFile(filename);
}

//______________________________________________________________________________
Bool_t TXMLEngine::ValidateDocument(xmlDocPointer doc, Bool_t doout) {
	xmlValidCtxt cvp;
	cvp.userData = doout ? (void *) stderr : 0;
//	cvp.error    = doout ? (xmlValidityErrorFunc) fprintf : 0;
//	cvp.warning  = doout ? (xmlValidityWarningFunc) fprintf : 0;
// debug output excluded to solve problem for Solaris compilation
	cvp.error    = 0;
	cvp.warning  = 0;

    xmlDocPtr docptr = (xmlDocPtr) doc;

    int res = xmlValidateDocument(&cvp, docptr);

    if ((docptr->intSubset!=0) || (docptr->extSubset!=0)) {
       xmlDtdPtr i = docptr->intSubset;
       xmlDtdPtr e = docptr->extSubset;
       if (i==e) e = 0;
       if (e!=0) {
         xmlUnlinkNode((xmlNodePtr) docptr->extSubset);
         docptr->extSubset = 0;
         xmlFreeDtd(e);
       }
       if (i!=0) {
         xmlUnlinkNode((xmlNodePtr) docptr->intSubset);
         docptr->intSubset = 0;
         xmlFreeDtd(i);
       }
    }

    if (doout)
      if (res==1) cout << "Validation done" << endl;
             else cout << "Validation failed" << endl;

    return (res == 1);
}

<|endoftext|>


<commit_before>#include "stdafx.h"
#include "Pool.h"
#include "Arm.h"
#include "Core/Common.h"
#include "Core/FileManager.h"

ArmStateCommand::ArmStateCommand(bool state)
{
	armstate = state;
}

bool ArmStateCommand::Validate()
{
	RamPos = g_fileManager->getVirtualAddress();
	return false;
}

void ArmStateCommand::writeSymData(SymbolData& symData) const
{
	// TODO: find a less ugly way to check for undefined memory positions
	if (RamPos == -1)
		return;

	if (armstate == true)
	{
		symData.addLabel(RamPos,L".arm");
	} else {
		symData.addLabel(RamPos,L".thumb");
	}
}

ArmPoolCommand::ArmPoolCommand()
{

}

bool ArmPoolCommand::Validate()
{
	position = g_fileManager->getVirtualAddress();

	size_t oldSize = values.size();
	values.clear();

	for (ArmPoolEntry& entry: Arm.getPoolContent())
	{
		size_t index = values.size();

		// try to filter redundant values, but only if
		// we aren't in an unordinarily long validation loop
		if (Global.validationPasses < 10)
		{
			for (size_t i = 0; i < values.size(); i++)
			{
				if (values[i] == entry.value)
				{
					index = i;
					break;
				}
			}
		}

		if (index == values.size())
			values.push_back(entry.value);

		entry.command->applyFileInfo();
		entry.command->setPoolAddress(position+index*4);
	}

	Arm.clearPoolContent();
	g_fileManager->advanceMemory(values.size()*4);

	return oldSize != values.size();
}

void ArmPoolCommand::Encode() const
{
	for (size_t i = 0; i < values.size(); i++)
	{
		int32_t value = values[i];
		g_fileManager->writeU32(value);
	}
}

void ArmPoolCommand::writeTempData(TempData& tempData) const
{
	for (size_t i = 0; i < values.size(); i++)
	{
		int32_t value = values[i];
		tempData.writeLine(position+i*4,formatString(L".word 0x%08X",value));
	}
}

void ArmPoolCommand::writeSymData(SymbolData& symData) const
{
	if (values.size() != 0)
	{
		symData.addLabel(position,L".pool");
		symData.addData(position,values.size()*4,SymbolData::Data32);
	}
}
<commit_msg>Optimize duplicate pool value checks.<commit_after>#include "stdafx.h"
#include <unordered_map>
#include "Pool.h"
#include "Arm.h"
#include "Core/Common.h"
#include "Core/FileManager.h"

ArmStateCommand::ArmStateCommand(bool state)
{
	armstate = state;
}

bool ArmStateCommand::Validate()
{
	RamPos = g_fileManager->getVirtualAddress();
	return false;
}

void ArmStateCommand::writeSymData(SymbolData& symData) const
{
	// TODO: find a less ugly way to check for undefined memory positions
	if (RamPos == -1)
		return;

	if (armstate == true)
	{
		symData.addLabel(RamPos,L".arm");
	} else {
		symData.addLabel(RamPos,L".thumb");
	}
}

ArmPoolCommand::ArmPoolCommand()
{

}

bool ArmPoolCommand::Validate()
{
	position = g_fileManager->getVirtualAddress();

	size_t oldSize = values.size();
	values.clear();

	std::unordered_map<int32_t, size_t> usedValues;
	for (ArmPoolEntry& entry: Arm.getPoolContent())
	{
		size_t index = values.size();

		// try to filter redundant values, but only if
		// we aren't in an unordinarily long validation loop
		if (Global.validationPasses < 10)
		{
			auto it = usedValues.find(entry.value);
			if (it != usedValues.end())
				index = it->second;
		}

		if (index == values.size())
		{
			usedValues[entry.value] = index;
			values.push_back(entry.value);
		}

		entry.command->applyFileInfo();
		entry.command->setPoolAddress(position+index*4);
	}

	Arm.clearPoolContent();
	g_fileManager->advanceMemory(values.size()*4);

	return oldSize != values.size();
}

void ArmPoolCommand::Encode() const
{
	for (size_t i = 0; i < values.size(); i++)
	{
		int32_t value = values[i];
		g_fileManager->writeU32(value);
	}
}

void ArmPoolCommand::writeTempData(TempData& tempData) const
{
	for (size_t i = 0; i < values.size(); i++)
	{
		int32_t value = values[i];
		tempData.writeLine(position+i*4,formatString(L".word 0x%08X",value));
	}
}

void ArmPoolCommand::writeSymData(SymbolData& symData) const
{
	if (values.size() != 0)
	{
		symData.addLabel(position,L".pool");
		symData.addData(position,values.size()*4,SymbolData::Data32);
	}
}
<|endoftext|>


<commit_before>#include "MovingEntity.h"

MovingEntity::MovingEntity(const sf::Vector2f &center, const sf::Vector2f &size, const sf::Color &color, const sf::Vector2f &velocity) :
    Entity(center, size, color), velocity(velocity)
{
}

void MovingEntity::render(sf::RenderTarget *render_target)
{
    if (true) {
        // draw motion bounding polygon
        sf::Shape shape = sf::Shape();
        for (int i = 0; i < (int)motion_bounding_polygon.size(); i++)
            shape.AddPoint(motion_bounding_polygon[i], collisions.empty() ? sf::Color::Green : sf::Color::Magenta);
        render_target->Draw(shape);
    }

    Entity::render(render_target);
}

void MovingEntity::getMotionBoundingPolygon(std::vector<sf::Vector2f> *polygon)
{
    *polygon = motion_bounding_polygon;
}

void MovingEntity::resetTemporaryState()
{
    is_front_edge.clear();
    motion_bounding_polygon.clear();
    collisions.clear();
}

void MovingEntity::calculateMotionBoundingPolygon()
{
    float velocity_angle = Util::angleOfVector(velocity);
    std::vector<sf::Vector2f> here;
    getBoundingPolygon(&here);
    // determine which edges face which ways
    calculateEdgesFacingAngle(here, &is_front_edge, velocity_angle);

    bool previously_front_edge = is_front_edge[is_front_edge.size() - 1];
    for (int i = 0; i < (int)here.size(); i++) {
        sf::Vector2f point = here[i];
        bool this_is_front_edge = is_front_edge[i];
        if (!this_is_front_edge) {
            if (previously_front_edge) {
                // insert backward pointing edge
                motion_bounding_polygon.push_back(point + velocity);
            }
            motion_bounding_polygon.push_back(point);
        } else {
            if (!previously_front_edge) {
                // insert forward pointing edge
                motion_bounding_polygon.push_back(point);
            }
            motion_bounding_polygon.push_back(point + velocity);
        }
        previously_front_edge = this_is_front_edge;
    }
}

void MovingEntity::calculateEdgesFacingAngle(const std::vector<sf::Vector2f> &polygon, std::vector<bool> *is_facing_edge, float facing_angle)
{
    for (int i = 0; i < (int)polygon.size(); i++) {
        sf::Vector2f point1 = polygon[i];
        sf::Vector2f point2 = polygon[(i + 1) % polygon.size()];
        sf::Vector2f edge = point2 - point1;
        float edge_angle = std::atan2(edge.y, edge.x);
        float relative_angle = Util::euclideanMod(facing_angle - edge_angle, Util::two_pi);
        bool is_front = relative_angle > Util::pi;
        is_facing_edge->push_back(is_front);
    }
}

void MovingEntity::detectCollision(Entity *other)
{
    std::vector<sf::Vector2f> here;
    getBoundingPolygon(&here);
    std::vector<sf::Vector2f> other_motion_bounding_polygon;
    other->getMotionBoundingPolygon(&other_motion_bounding_polygon);
    float velocity_magnitude = Util::magnitude(velocity);
    float velocity_angle = Util::angleOfVector(velocity);
    float opposite_velocity_angle = velocity_angle + Util::pi;
    std::vector<bool> is_other_back_edge;
    calculateEdgesFacingAngle(other_motion_bounding_polygon, &is_other_back_edge, opposite_velocity_angle);
    for (int i = 0; i < (int)here.size(); i++) {
        if (!(is_front_edge[i] || is_front_edge[Util::euclideanMod(i - 1, (int)is_front_edge.size())]))
            continue;
        // check this point against those edges
        sf::Vector2f this1 = here[i];
        for (int j = 0; j < (int)other_motion_bounding_polygon.size(); j++) {
            if (!is_other_back_edge[j])
                continue;
            sf::Vector2f other1 = other_motion_bounding_polygon[j];
            sf::Vector2f other2 = other_motion_bounding_polygon[(j + 1) % other_motion_bounding_polygon.size()];
            sf::Vector2f segment_direction = other2 - other1;
            float distance_to_collision = distanceFromPointToSegment(this1, velocity, other1, segment_direction);
            if (distance_to_collision <= velocity_magnitude) {
                // collision
                Util::insert(&collisions, distance_to_collision, Collision(other, Util::perp(Util::normalized(segment_direction))));
            }
        }
        if (is_front_edge[i]) {
            // check this edge against those points
            sf::Vector2f this2 = here[(i + 1) % here.size()];
            for (int j = 0; j < (int)other_motion_bounding_polygon.size(); j++) {
                if (!(is_other_back_edge[j] || is_other_back_edge[Util::euclideanMod(j - 1, (int)is_other_back_edge.size())]))
                    continue;
                sf::Vector2f other1 = other_motion_bounding_polygon[j];
                sf::Vector2f segment_direction = this2 - this1;
                float distance_to_collision = distanceFromPointToSegment(other1, -velocity, this1, segment_direction);
                if (distance_to_collision <= velocity_magnitude) {
                    // collision
                    Util::insert(&collisions, distance_to_collision, Collision(other, -Util::perp(Util::normalized(segment_direction))));
                }
            }
        }
    }
}

float MovingEntity::distanceFromPointToSegment(const sf::Vector2f &point, const sf::Vector2f &direction, const sf::Vector2f &segment_start, const sf::Vector2f &segment_direction)
{
    // http://objectmix.com/graphics/132701-ray-line-segment-intersection-2d.html#post460607
    float denominator = Util::dot(Util::perp(segment_direction), direction);
    if (std::fabs(denominator) < std::numeric_limits<float>::epsilon())
        return std::numeric_limits<float>::infinity(); // parallel
    float t = Util::dot(Util::perp(direction), segment_start - point) / denominator;
    if (!(0 <= t && t <= 1))
        return std::numeric_limits<float>::infinity(); // miss
    float s = Util::dot(Util::perp(segment_direction), segment_start - point) / denominator;
    if (s < 0)
        return std::numeric_limits<float>::infinity(); // behind
    return s * Util::magnitude(direction);
}

void MovingEntity::resolveCollisionsAndApplyVelocity()
{
    if (collisions.empty()) {
        center += velocity;
        return;
    }
    std::multimap<float, Collision>::iterator iterator = collisions.begin();
    float closest_collision = iterator->first;
    center += closest_collision * Util::normalized(velocity);
    velocity = sf::Vector2f(0, 0);
    for (; iterator != collisions.end(); iterator++) {
        sf::Vector2f normal = iterator->second.normal;
        Entity * blah = iterator->second.entity;
    }
}

void MovingEntity::serialize(std::vector<byte> *buffer)
{
    Util::serialize(buffer, center);
    Util::serialize(buffer, size);
    Util::serialize(buffer, color);
    Util::serialize(buffer, velocity);
}

MovingEntity * MovingEntity::deserialize(std::vector<byte>::const_iterator* buffer)
{
    sf::Vector2f center = Util::deserialize<sf::Vector2f>(buffer);
    sf::Vector2f size = Util::deserialize<sf::Vector2f>(buffer);
    sf::Color color = Util::deserialize<sf::Color>(buffer);
    sf::Vector2f velocity = Util::deserialize<sf::Vector2f>(buffer);
    return new MovingEntity(center, size, color, velocity);
}
<commit_msg>perfectly elastic collisions<commit_after>#include "MovingEntity.h"

MovingEntity::MovingEntity(const sf::Vector2f &center, const sf::Vector2f &size, const sf::Color &color, const sf::Vector2f &velocity) :
    Entity(center, size, color), velocity(velocity)
{
}

void MovingEntity::render(sf::RenderTarget *render_target)
{
    if (true) {
        // draw motion bounding polygon
        sf::Shape shape = sf::Shape();
        for (int i = 0; i < (int)motion_bounding_polygon.size(); i++)
            shape.AddPoint(motion_bounding_polygon[i], collisions.empty() ? sf::Color::Green : sf::Color::Magenta);
        render_target->Draw(shape);
    }

    Entity::render(render_target);
}

void MovingEntity::getMotionBoundingPolygon(std::vector<sf::Vector2f> *polygon)
{
    *polygon = motion_bounding_polygon;
}

void MovingEntity::resetTemporaryState()
{
    is_front_edge.clear();
    motion_bounding_polygon.clear();
    collisions.clear();
}

void MovingEntity::calculateMotionBoundingPolygon()
{
    float velocity_angle = Util::angleOfVector(velocity);
    std::vector<sf::Vector2f> here;
    getBoundingPolygon(&here);
    // determine which edges face which ways
    calculateEdgesFacingAngle(here, &is_front_edge, velocity_angle);

    bool previously_front_edge = is_front_edge[is_front_edge.size() - 1];
    for (int i = 0; i < (int)here.size(); i++) {
        sf::Vector2f point = here[i];
        bool this_is_front_edge = is_front_edge[i];
        if (!this_is_front_edge) {
            if (previously_front_edge) {
                // insert backward pointing edge
                motion_bounding_polygon.push_back(point + velocity);
            }
            motion_bounding_polygon.push_back(point);
        } else {
            if (!previously_front_edge) {
                // insert forward pointing edge
                motion_bounding_polygon.push_back(point);
            }
            motion_bounding_polygon.push_back(point + velocity);
        }
        previously_front_edge = this_is_front_edge;
    }
}

void MovingEntity::calculateEdgesFacingAngle(const std::vector<sf::Vector2f> &polygon, std::vector<bool> *is_facing_edge, float facing_angle)
{
    for (int i = 0; i < (int)polygon.size(); i++) {
        sf::Vector2f point1 = polygon[i];
        sf::Vector2f point2 = polygon[(i + 1) % polygon.size()];
        sf::Vector2f edge = point2 - point1;
        float edge_angle = std::atan2(edge.y, edge.x);
        float relative_angle = Util::euclideanMod(facing_angle - edge_angle, Util::two_pi);
        bool is_front = relative_angle > Util::pi;
        is_facing_edge->push_back(is_front);
    }
}

void MovingEntity::detectCollision(Entity *other)
{
    std::vector<sf::Vector2f> here;
    getBoundingPolygon(&here);
    std::vector<sf::Vector2f> other_motion_bounding_polygon;
    other->getMotionBoundingPolygon(&other_motion_bounding_polygon);
    float velocity_magnitude = Util::magnitude(velocity);
    float velocity_angle = Util::angleOfVector(velocity);
    float opposite_velocity_angle = velocity_angle + Util::pi;
    std::vector<bool> is_other_back_edge;
    calculateEdgesFacingAngle(other_motion_bounding_polygon, &is_other_back_edge, opposite_velocity_angle);
    for (int i = 0; i < (int)here.size(); i++) {
        if (!(is_front_edge[i] || is_front_edge[Util::euclideanMod(i - 1, (int)is_front_edge.size())]))
            continue;
        // check this point against those edges
        sf::Vector2f this1 = here[i];
        for (int j = 0; j < (int)other_motion_bounding_polygon.size(); j++) {
            if (!is_other_back_edge[j])
                continue;
            sf::Vector2f other1 = other_motion_bounding_polygon[j];
            sf::Vector2f other2 = other_motion_bounding_polygon[(j + 1) % other_motion_bounding_polygon.size()];
            sf::Vector2f segment_direction = other2 - other1;
            float distance_to_collision = distanceFromPointToSegment(this1, velocity, other1, segment_direction);
            if (distance_to_collision <= velocity_magnitude) {
                // collision
                Util::insert(&collisions, distance_to_collision, Collision(other, Util::perp(Util::normalized(segment_direction))));
            }
        }
        if (is_front_edge[i]) {
            // check this edge against those points
            sf::Vector2f this2 = here[(i + 1) % here.size()];
            for (int j = 0; j < (int)other_motion_bounding_polygon.size(); j++) {
                if (!(is_other_back_edge[j] || is_other_back_edge[Util::euclideanMod(j - 1, (int)is_other_back_edge.size())]))
                    continue;
                sf::Vector2f other1 = other_motion_bounding_polygon[j];
                sf::Vector2f segment_direction = this2 - this1;
                float distance_to_collision = distanceFromPointToSegment(other1, -velocity, this1, segment_direction);
                if (distance_to_collision <= velocity_magnitude) {
                    // collision
                    Util::insert(&collisions, distance_to_collision, Collision(other, -Util::perp(Util::normalized(segment_direction))));
                }
            }
        }
    }
}

float MovingEntity::distanceFromPointToSegment(const sf::Vector2f &point, const sf::Vector2f &direction, const sf::Vector2f &segment_start, const sf::Vector2f &segment_direction)
{
    // http://objectmix.com/graphics/132701-ray-line-segment-intersection-2d.html#post460607
    float denominator = Util::dot(Util::perp(segment_direction), direction);
    if (std::fabs(denominator) < std::numeric_limits<float>::epsilon())
        return std::numeric_limits<float>::infinity(); // parallel
    float t = Util::dot(Util::perp(direction), segment_start - point) / denominator;
    if (!(0 <= t && t <= 1))
        return std::numeric_limits<float>::infinity(); // miss
    float s = Util::dot(Util::perp(segment_direction), segment_start - point) / denominator;
    if (s < 0)
        return std::numeric_limits<float>::infinity(); // behind
    return s * Util::magnitude(direction);
}

void MovingEntity::resolveCollisionsAndApplyVelocity()
{
    if (collisions.empty()) {
        center += velocity;
        return;
    }
    sf::Vector2f normalized_velocity = Util::normalized(velocity);
    std::multimap<float, Collision>::iterator iterator = collisions.begin();
    float closest_collision = iterator->first;
    // the biggest dot product means the steepest collision angle
    sf::Vector2f normal_with_biggest_dot_product = iterator->second.normal;
    float biggest_dot_product = Util::dot(normalized_velocity, normal_with_biggest_dot_product);
    iterator++;
    for (; iterator != collisions.end(); iterator++) {
        // accept any collision at pretty much the same place as the first one
        float distance_to_collision = iterator->first;
        if (distance_to_collision - closest_collision > std::numeric_limits<float>::epsilon())
            break; // too far away
        sf::Vector2f normal = iterator->second.normal;
        float dot_product = Util::dot(normalized_velocity, normal);
        if (dot_product > biggest_dot_product) {
            // found a steeper collision
            biggest_dot_product = dot_product;
            normal_with_biggest_dot_product = normal;
        }
    }

    // snap to the collision point
    center += closest_collision * normalized_velocity;

    // bounce
    velocity = velocity - 2 * Util::dot(velocity, normal_with_biggest_dot_product) * normal_with_biggest_dot_product;
}

void MovingEntity::serialize(std::vector<byte> *buffer)
{
    Util::serialize(buffer, center);
    Util::serialize(buffer, size);
    Util::serialize(buffer, color);
    Util::serialize(buffer, velocity);
}

MovingEntity * MovingEntity::deserialize(std::vector<byte>::const_iterator* buffer)
{
    sf::Vector2f center = Util::deserialize<sf::Vector2f>(buffer);
    sf::Vector2f size = Util::deserialize<sf::Vector2f>(buffer);
    sf::Color color = Util::deserialize<sf::Color>(buffer);
    sf::Vector2f velocity = Util::deserialize<sf::Vector2f>(buffer);
    return new MovingEntity(center, size, color, velocity);
}
<|endoftext|>


<commit_before>/// @author Michał Orynicz

#include "VisualOdometer.hpp"
#include "BirdsEyeTranslationReader.hpp"
#include "TangentRotationReader.hpp"
#include "SmoothFilter.hpp"
#include "TrimHistoryFilter.hpp"
#include "LucasKanadePyramidTracker.hpp"
#include "ShiThomasiFeatureExtractor.hpp"
#include "PreparationFunctions.hpp"
#include "ConvenienceFunctions.hpp"
#include "ErrorCodes.hpp"
#include "DrawingFunctions.hpp"
#include "Catcher.hpp"
#include "CvTypesIo.hpp"
#include "CommandlineParameters.hpp"

#include "opencv2/core/core.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/calib3d/calib3d.hpp"

#include <boost/program_options.hpp>
#include <boost/date_time.hpp>

#include <string>
#include <vector>
#include <ctime>

using std::cerr;
using std::endl;

void processInput(const cv::Mat &input,
        const std::vector<cv::Mat> &rectifyMaps,
        std::list<cv::Point3f> &positions, VisualOdometer &odo,
        std::list<long long int> &timestamps,
        const boost::posix_time::ptime &milenium,
        const int &verbosity, const CameraSpatialParameters &spatial,
        const cv::Size &imageSize, const std::string &mainWindowName,
        const std::string &mapWindowName);

VisualOdometer getVisualOdometer(cv::VideoCapture &capture,
        const ShiThomasiParameters &shiThomasi,
        const LucasKanadeParameters &lucasKanade,
        const ChessboardParameters &chessboard,
        CameraSpatialParameters &spatial,
        const std::string &cameraParametersFilename,
        const std::string &cameraSpatialFilename,
        const int &maxHistoryLength, const int &maxDeviation,
        const int &maxDeviants, const int &minLength,
        const int &maxLength, const int &upperMargin,
        const int &lowerMargin, const int & maxFeaturesUpper,
        const int & maxFeaturesLower,
        std::vector<cv::Mat> &rectifyMaps, cv::Size &imageSize,
        std::list<cv::Point3f> &positions,
        const cv::Mat &cameraMatrix,
        const cv::Mat &distortionCoefficients,
        void (*parametersCalibration)(cv::VideoCapture &capture,
                std::vector<cv::Mat> rectifyMaps, int &horizon,
                int &deadZone, const cv::Size &imageSize));

void printTrajectory(const std::string &matlabFilename,
        const std::list<long long int> &timestamps,
        const std::list<cv::Point3f> &positions);

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

    ShiThomasiParameters shiThomasi;
    LucasKanadeParameters lucasKanade;
    ChessboardParameters chessboard;
    CameraSpatialParameters spatial;

    int maxFeaturesUpper = 100;
    int maxFeaturesLower = 100;
    int maxHistoryLength = 8;

    double maxDeviation = CV_PI / 16;
    double maxDeviants = 0.9;
    double minLength = 10;
    double maxLength = 50;

    double upperMargin = 30;
    double lowerMargin = 30;

    spatial.horizon = -1;
    spatial.deadZone = -1;

    int captureDeviceNumber = 1;
    int verbosity = 0;
    std::string cameraParametersFilename;
    std::string matlabFilename, cameraSpatialFilename;

    std::vector<cv::Mat> rectifyMaps;
    cv::Size imageSize;

    std::list<cv::Point3f> positions;
    std::list<long long int> timestamps;
    cv::Point3f currentPosition(0, 0, 0);
    Catcher capture;
    cv::Mat cameraMatrix, distortionCoefficients;

    try {
        readParametersCommandLine(argc, argv, shiThomasi, lucasKanade,
                chessboard, spatial, maxFeaturesUpper,
                maxFeaturesLower, maxHistoryLength, upperMargin,
                lowerMargin, captureDeviceNumber, verbosity,
                cameraParametersFilename, matlabFilename,
                cameraSpatialFilename);

        capture.open(captureDeviceNumber);
        getCameraParameters(cameraParametersFilename, rectifyMaps,
                cameraMatrix, distortionCoefficients, imageSize);

        capture.set(CV_CAP_PROP_FRAME_WIDTH, imageSize.width);
        capture.set(CV_CAP_PROP_FRAME_HEIGHT, imageSize.height);
        std::cerr << imageSize << std::endl;
    } catch (cv::Exception &ex) {
        if (USER_TRIGGERED_EXIT == ex.code) {
            return 0;
        } else {
            std::cerr << "EXCEPTION!!!!!!!!!!!" << std::endl
                    << ex.what() << std::endl;
            return 1;
        }
    }
    try {
        VisualOdometer odo = getVisualOdometer(capture, shiThomasi,
                lucasKanade, chessboard, spatial,
                cameraParametersFilename, cameraSpatialFilename,
                maxHistoryLength, maxDeviation, maxDeviants,
                minLength, maxLength, upperMargin, lowerMargin,
                maxFeaturesUpper, maxFeaturesLower, rectifyMaps,
                imageSize, positions, cameraMatrix,
                distortionCoefficients, calibrateParameters);
        char control = ' ';
        cv::Mat input;
        cv::Mat undistorted;
        cv::Mat grey;
        cv::Mat map;
        if (verbosity > 1) {
            cv::namedWindow("main", CV_WINDOW_KEEPRATIO);
        }
        if (verbosity > 0) {
            cv::namedWindow("map", CV_WINDOW_KEEPRATIO);
        }
        if (0 == verbosity) {
            cv::namedWindow("kill switch", CV_WINDOW_NORMAL);
        }
        std::vector<std::list<cv::Point2f> > featuresRot;
        std::vector<std::list<cv::Point2f> > featuresGround;
        boost::posix_time::ptime milenium(
                boost::gregorian::date(1970, 1, 1));

        for (int i = 0; i < 5; ++i) { //a couple of dry cycles to warm up
            cv::Point3f displacement;
            capture >> input;
            if (input.empty()) {
                continue;
            }

            cv::remap(input, undistorted, rectifyMaps[0],
                    rectifyMaps[1], cv::INTER_LINEAR);
            cv::cvtColor(undistorted, grey, CV_RGB2GRAY);
            displacement = odo.calculateDisplacement(grey);
        }

        do {
            cv::Point3f displacement;
            capture >> input;
            if (input.empty()) {
                continue;
            }
            processInput(input, rectifyMaps, positions, odo,
                    timestamps, milenium, verbosity, spatial,
                    imageSize,"main","map");

            if (1 < verbosity && 'p' == control) {
                std::string fileName;
                std::stringstream buff;
                buff << "screen" << timestamps.front() << ".png";
                buff >> fileName;
                cv::imwrite(fileName, undistorted);

            }
            control = cv::waitKey(1);
        } while ('q' != control);
        cv::waitKey(0);
        printTrajectory(matlabFilename, timestamps, positions);

    } catch (cv::Exception &ex) {
        if (USER_TRIGGERED_EXIT == ex.code) {
            return 0;
        } else {
            std::cerr << "EXCEPTION!!!!!!!!!!!" << std::endl
                    << ex.what() << std::endl;
            return 1;
        }
    }
    return 0;
}

void printTrajectory(const std::string &matlabFilename,
        const std::list<long long int> &timestamps,
        const std::list<cv::Point3f> &positions) {
    std::list<long long int>::const_iterator iTime =
            timestamps.begin();
    std::ofstream out;
    if (!matlabFilename.empty()) {
        out.open(matlabFilename.c_str(), std::ofstream::out);
        if (!out.is_open()) {
            std::cerr << "WARNING: could not open file "
                    << matlabFilename << " for write" << std::endl;
        } else {
            out << "t=[" << std::endl;
        }
    }
    for (std::list<cv::Point3f>::const_iterator iPos =
            positions.begin();
            positions.end() != iPos && timestamps.end() != iTime;
            ++iPos, ++iTime) {
        std::cerr << *iPos << std::endl;
        if (!matlabFilename.empty()) {
            out << iPos->x << "," << iPos->y << "," << iPos->z << ","
                    << *iTime << std::endl;
        }
    }
    out << "];" << std::endl;
    out.close();
}

void processInput(const cv::Mat &input,
        const std::vector<cv::Mat> &rectifyMaps,
        std::list<cv::Point3f> &positions, VisualOdometer &odo,
        std::list<long long int> &timestamps,
        const boost::posix_time::ptime &milenium,
        const int &verbosity, const CameraSpatialParameters &spatial,
        const cv::Size &imageSize, const std::string &mainWindowName,
        const std::string &mapWindowName) {
    cv::Mat undistorted;
    cv::Mat grey;
    cv::Point3d currentPosition = positions.back();
    cv::Point3d displacement;
    cv::Mat map;
    try {
        cv::remap(input, undistorted, rectifyMaps[0], rectifyMaps[1],
                cv::INTER_LINEAR);
        cv::cvtColor(undistorted, grey, CV_RGB2GRAY);
        displacement = odo.calculateDisplacement(grey);
        cv::Point3d globalDisplacement(
                displacement.x
                        * cos(currentPosition.z + displacement.z)
                        + displacement.y
                                * sin(
                                        currentPosition.z
                                                + displacement.z),
                displacement.x
                        * sin(currentPosition.z + displacement.z)
                        + displacement.y
                                * cos(
                                        currentPosition.z
                                                + displacement.z),
                displacement.z);
        currentPosition = currentPosition + globalDisplacement;
        positions.push_back(currentPosition);

        boost::posix_time::ptime pTime =
                boost::posix_time::microsec_clock::universal_time();
        boost::posix_time::time_duration duration(pTime - milenium);
        long long int miliseconds = duration.total_milliseconds();
        timestamps.push_back(miliseconds);

        if (verbosity > 0) {
            map = drawTraveledRoute(positions);
            cv::imshow(mapWindowName, map);
        }
        if (verbosity > 1) {
            std::vector<std::list<cv::Point2f> > featuresRot =
                    odo.getRotationFeatures();
            std::vector<std::list<cv::Point2f> > featuresGround =
                    odo.getTranslationFeatures();

            drawFeaturesUpperAndLower(undistorted, featuresRot,
                    cv::Mat(), featuresGround, spatial.homography,
                    spatial.horizon, spatial.deadZone);
            cv::line(undistorted, cv::Point(imageSize.width / 2, 0),
                    cv::Point(imageSize.width / 2, imageSize.height),
                    CV_RGB(255,0,0), 1, 8, 0);
            drawDeadZoneHorizon(undistorted, spatial.horizon,
                    spatial.deadZone);
            cv::imshow(mainWindowName, undistorted);

        }
    } catch (const cv::Exception &ex) {

        std::cerr << "EXCEPTION!!!!!!!!!!!" << std::endl << ex.what()
                << std::endl;
        cv::Exception newEx(ex.code, "Exception caught", __func__,
                __FILE__, __LINE__);
        throw newEx;
    }
}

VisualOdometer getVisualOdometer(cv::VideoCapture &capture,
        const ShiThomasiParameters &shiThomasi,
        const LucasKanadeParameters &lucasKanade,
        const ChessboardParameters &chessboard,
        CameraSpatialParameters &spatial,
        const std::string &cameraParametersFilename,
        const std::string &cameraSpatialFilename,
        const int &maxHistoryLength, const int &maxDeviation,
        const int &maxDeviants, const int &minLength,
        const int &maxLength, const int &upperMargin,
        const int &lowerMargin, const int & maxFeaturesUpper,
        const int & maxFeaturesLower,
        std::vector<cv::Mat> &rectifyMaps, cv::Size &imageSize,
        std::list<cv::Point3f> &positions,
        const cv::Mat &cameraMatrix,
        const cv::Mat &distortionCoefficients,
        void (*parametersCalibration)(cv::VideoCapture &capture,
                std::vector<cv::Mat> rectifyMaps, int &horizon,
                int &deadZone, const cv::Size &imageSize)) {

    ShiThomasiFeatureExtractor extractor(shiThomasi.qualityLevel,
            shiThomasi.minDistance, shiThomasi.blockSize,
            shiThomasi.winSize, shiThomasi.zeroZone,
            shiThomasi.termCrit);
    LucasKanadePyramidTracker tracker(lucasKanade.winSize,
            lucasKanade.maxLevel, lucasKanade.flags,
            lucasKanade.termCrit, lucasKanade.minEigenvalueThresh,
            lucasKanade.maxErrorValue);

    try {
        std::list<FeatureFilter*> filters;

        filters.push_back(new TrimHistoryFilter(maxHistoryLength));
        filters.push_back(
                new SmoothFilter(maxDeviation, maxDeviants, minLength,
                        maxLength));

        cv::Point3f currentPosition(0, 0, 0);

        positions.push_back(currentPosition);

        if (0 >= spatial.horizon || 0 > spatial.deadZone) {
            spatial.horizon = imageSize.height / 2;
            spatial.deadZone = 0;
            parametersCalibration(capture, rectifyMaps,
                    spatial.horizon, spatial.deadZone, imageSize);

            std::vector<cv::Point2f> corners = getChessboardCorners(
                    capture, rectifyMaps, spatial.horizon,
                    spatial.deadZone, chessboard.size, imageSize,
                    chessboard.winSize, chessboard.zeroZone,
                    chessboard.termCrit);
            cerr << "got corners" << endl;
            getHomographyRtMatrixAndRotationCenter(corners, imageSize,
                    chessboard.size, chessboard.squareSize,
                    spatial.horizon, spatial.deadZone, cameraMatrix,
                    distortionCoefficients, chessboard.height,
                    spatial.homography, spatial.rotationCenter,
                    spatial.rtMatrix);
            if (!cameraSpatialFilename.empty()) {
                cv::FileStorage fs(cameraSpatialFilename,
                        cv::FileStorage::WRITE);
                if (!fs.isOpened()) {
                    cv::Exception ex(-1, "Could not open FileStorage",
                            __func__, __FILE__, __LINE__);
                    throw ex;
                }
                fs << "cameraSpatialParameters" << spatial;
                fs.release();
            }

        }
        TangentRotationReader rotationReader(tracker, extractor,
                filters, maxFeaturesUpper,
                cameraMatrix.at<double>(0, 0),
                cv::Size(imageSize.width,
                        spatial.horizon - spatial.deadZone),
                upperMargin);
        BirdsEyeTranslationReader transReader(spatial.homography,
                extractor, tracker, maxFeaturesLower, filters,
                spatial.rotationCenter,
                cv::Size(imageSize.width,
                        imageSize.height - spatial.horizon
                                - spatial.deadZone), lowerMargin);
        VisualOdometer odo(rotationReader, transReader,
                spatial.horizon, spatial.deadZone);
        return odo;
    } catch (const cv::Exception &ex) {
        if (USER_TRIGGERED_EXIT != ex.code) {
            std::cerr << "EXCEPTION!!!!!!!!!!!" << std::endl
                    << ex.what() << std::endl;
        }
        cv::Exception newEx(ex.code, "Exception caught", __func__,
                __FILE__, __LINE__);
        throw newEx;
    }

}
<commit_msg>Cosmetic changes and formattig in OdoMain<commit_after>/// @author Michał Orynicz

#include "VisualOdometer.hpp"
#include "BirdsEyeTranslationReader.hpp"
#include "TangentRotationReader.hpp"
#include "SmoothFilter.hpp"
#include "TrimHistoryFilter.hpp"
#include "LucasKanadePyramidTracker.hpp"
#include "ShiThomasiFeatureExtractor.hpp"
#include "PreparationFunctions.hpp"
#include "ConvenienceFunctions.hpp"
#include "ErrorCodes.hpp"
#include "DrawingFunctions.hpp"
#include "Catcher.hpp"
#include "CvTypesIo.hpp"
#include "CommandlineParameters.hpp"

#include "opencv2/core/core.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/calib3d/calib3d.hpp"

#include <boost/program_options.hpp>
#include <boost/date_time.hpp>

#include <string>
#include <vector>
#include <ctime>

using std::cerr;
using std::endl;

void processInput(const cv::Mat &input,
        const std::vector<cv::Mat> &rectifyMaps,
        std::list<cv::Point3f> &positions, VisualOdometer &odo,
        std::list<long long int> &timestamps,
        const boost::posix_time::ptime &milenium,
        const int &verbosity, const CameraSpatialParameters &spatial,
        const cv::Size &imageSize, const std::string &mainWindowName,
        const std::string &mapWindowName);

VisualOdometer getVisualOdometer(cv::VideoCapture &capture,
        const ShiThomasiParameters &shiThomasi,
        const LucasKanadeParameters &lucasKanade,
        const ChessboardParameters &chessboard,
        CameraSpatialParameters &spatial,
        const std::string &cameraParametersFilename,
        const std::string &cameraSpatialFilename,
        const int &maxHistoryLength, const int &maxDeviation,
        const int &maxDeviants, const int &minLength,
        const int &maxLength, const int &upperMargin,
        const int &lowerMargin, const int & maxFeaturesUpper,
        const int & maxFeaturesLower,
        std::vector<cv::Mat> &rectifyMaps, cv::Size &imageSize,
        std::list<cv::Point3f> &positions,
        const cv::Mat &cameraMatrix,
        const cv::Mat &distortionCoefficients,
        void (*parametersCalibration)(cv::VideoCapture &capture,
                std::vector<cv::Mat> rectifyMaps, int &horizon,
                int &deadZone, const cv::Size &imageSize));

void printTrajectory(const std::string &matlabFilename,
        const std::list<long long int> &timestamps,
        const std::list<cv::Point3f> &positions);

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

    ShiThomasiParameters shiThomasi;
    LucasKanadeParameters lucasKanade;
    ChessboardParameters chessboard;
    CameraSpatialParameters spatial;

    int maxFeaturesUpper = 100;
    int maxFeaturesLower = 100;
    int maxHistoryLength = 8;

    double maxDeviation = CV_PI / 16;
    double maxDeviants = 0.9;
    double minLength = 10;
    double maxLength = 50;

    double upperMargin = 30;
    double lowerMargin = 30;

    spatial.horizon = -1;
    spatial.deadZone = -1;

    int captureDeviceNumber = 1;
    int verbosity = 0;
    std::string cameraParametersFilename;
    std::string matlabFilename, cameraSpatialFilename;

    std::vector<cv::Mat> rectifyMaps;
    cv::Size imageSize;

    std::list<cv::Point3f> positions;
    std::list<long long int> timestamps;
    cv::Point3f currentPosition(0, 0, 0);
    Catcher capture;
    cv::Mat cameraMatrix, distortionCoefficients;

    try {
        readParametersCommandLine(argc, argv, shiThomasi, lucasKanade,
                chessboard, spatial, maxFeaturesUpper,
                maxFeaturesLower, maxHistoryLength, upperMargin,
                lowerMargin, captureDeviceNumber, verbosity,
                cameraParametersFilename, matlabFilename,
                cameraSpatialFilename);

        capture.open(captureDeviceNumber);
        getCameraParameters(cameraParametersFilename, rectifyMaps,
                cameraMatrix, distortionCoefficients, imageSize);

        capture.set(CV_CAP_PROP_FRAME_WIDTH, imageSize.width);
        capture.set(CV_CAP_PROP_FRAME_HEIGHT, imageSize.height);
        std::cerr << imageSize << std::endl;
    } catch (cv::Exception &ex) {
        if (USER_TRIGGERED_EXIT == ex.code) {
            return 0;
        } else {
            std::cerr << "EXCEPTION!!!!!!!!!!!" << std::endl
                    << ex.what() << std::endl;
            return 1;
        }
    }
    try {
        VisualOdometer odo = getVisualOdometer(capture, shiThomasi,
                lucasKanade, chessboard, spatial,
                cameraParametersFilename, cameraSpatialFilename,
                maxHistoryLength, maxDeviation, maxDeviants,
                minLength, maxLength, upperMargin, lowerMargin,
                maxFeaturesUpper, maxFeaturesLower, rectifyMaps,
                imageSize, positions, cameraMatrix,
                distortionCoefficients, calibrateParameters);
        char control = ' ';
        cv::Mat input;
        cv::Mat undistorted;
        cv::Mat grey;
        cv::Mat map;
        if (verbosity > 1) {
            cv::namedWindow("main", CV_WINDOW_KEEPRATIO);
        }
        if (verbosity > 0) {
            cv::namedWindow("map", CV_WINDOW_KEEPRATIO);
        }
        if (0 == verbosity) {
            cv::namedWindow("kill switch", CV_WINDOW_NORMAL);
        }
        std::vector<std::list<cv::Point2f> > featuresRot;
        std::vector<std::list<cv::Point2f> > featuresGround;
        boost::posix_time::ptime milenium(
                boost::gregorian::date(1970, 1, 1));

        for (int i = 0; i < 5; ++i) { //a couple of dry cycles to warm up
            cv::Point3f displacement;
            capture >> input;
            if (input.empty()) {
                continue;
            }

            cv::remap(input, undistorted, rectifyMaps[0],
                    rectifyMaps[1], cv::INTER_LINEAR);
            cv::cvtColor(undistorted, grey, CV_RGB2GRAY);
            displacement = odo.calculateDisplacement(grey);
        }

        do {
            cv::Point3f displacement;
            capture >> input;
            if (input.empty()) {
                continue;
            }
            processInput(input, rectifyMaps, positions, odo,
                    timestamps, milenium, verbosity, spatial,
                    imageSize, "main", "map");

            if (1 < verbosity && 'p' == control) {
                std::string fileName;
                std::stringstream buff;
                buff << "screen" << timestamps.front() << ".png";
                buff >> fileName;
                cv::imwrite(fileName, undistorted);

            }
            control = cv::waitKey(1);
        } while ('q' != control);
        cv::waitKey(0);
        printTrajectory(matlabFilename, timestamps, positions);

    } catch (cv::Exception &ex) {
        if (USER_TRIGGERED_EXIT == ex.code) {
            return 0;
        } else {
            std::cerr << "EXCEPTION!!!!!!!!!!!" << std::endl
                    << ex.what() << std::endl;
            return 1;
        }
    }
    return 0;
}

void printTrajectory(const std::string &matlabFilename,
        const std::list<long long int> &timestamps,
        const std::list<cv::Point3f> &positions) {
    std::list<long long int>::const_iterator iTime =
            timestamps.begin();
    std::ofstream out;
    if (!matlabFilename.empty()) {
        out.open(matlabFilename.c_str(), std::ofstream::out);
        if (!out.is_open()) {
            std::cerr << "WARNING: could not open file "
                    << matlabFilename << " for write" << std::endl;
        } else {
            out << "t=[" << std::endl;
        }
    }
    for (std::list<cv::Point3f>::const_iterator iPos =
            positions.begin();
            positions.end() != iPos && timestamps.end() != iTime;
            ++iPos, ++iTime) {
        std::cerr << *iPos << " " << *iTime << std::endl;
        if (!matlabFilename.empty()) {
            out << iPos->x << "," << iPos->y << "," << iPos->z << ","
                    << *iTime << std::endl;
        }
    }
    out << "];" << std::endl;
    out.close();
}

void processInput(const cv::Mat &input,
        const std::vector<cv::Mat> &rectifyMaps,
        std::list<cv::Point3f> &positions, VisualOdometer &odo,
        std::list<long long int> &timestamps,
        const boost::posix_time::ptime &milenium,
        const int &verbosity, const CameraSpatialParameters &spatial,
        const cv::Size &imageSize, const std::string &mainWindowName,
        const std::string &mapWindowName) {
    cv::Mat undistorted;
    cv::Mat grey;
    cv::Point3d currentPosition = positions.back();
    cv::Point3d displacement;
    cv::Mat map;
    try {
        cv::remap(input, undistorted, rectifyMaps[0], rectifyMaps[1],
                cv::INTER_LINEAR);
        cv::cvtColor(undistorted, grey, CV_RGB2GRAY);
        displacement = odo.calculateDisplacement(grey);
        cv::Point3d globalDisplacement(
                displacement.x
                        * cos(currentPosition.z + displacement.z)
                        + displacement.y
                                * sin(
                                        currentPosition.z
                                                + displacement.z),
                displacement.x
                        * sin(currentPosition.z + displacement.z)
                        + displacement.y
                                * cos(
                                        currentPosition.z
                                                + displacement.z),
                displacement.z);
        currentPosition = currentPosition + globalDisplacement;
        positions.push_back(currentPosition);

        boost::posix_time::ptime pTime =
                boost::posix_time::microsec_clock::universal_time();
        boost::posix_time::time_duration duration(pTime - milenium);
        long long int miliseconds = duration.total_milliseconds();
        timestamps.push_back(miliseconds);

        if (verbosity > 0) {
            map = drawTraveledRoute(positions);
            cv::imshow(mapWindowName, map);
        }
        if (verbosity > 1) {
            std::vector<std::list<cv::Point2f> > featuresRot =
                    odo.getRotationFeatures();
            std::vector<std::list<cv::Point2f> > featuresGround =
                    odo.getTranslationFeatures();

            drawFeaturesUpperAndLower(undistorted, featuresRot,
                    cv::Mat(), featuresGround, spatial.homography,
                    spatial.horizon, spatial.deadZone);
            cv::line(undistorted, cv::Point(imageSize.width / 2, 0),
                    cv::Point(imageSize.width / 2, imageSize.height),
                    CV_RGB(255,0,0), 1, 8, 0);
            drawDeadZoneHorizon(undistorted, spatial.horizon,
                    spatial.deadZone);
            cv::imshow(mainWindowName, undistorted);

        }
    } catch (const cv::Exception &ex) {

        std::cerr << "EXCEPTION!!!!!!!!!!!" << std::endl << ex.what()
                << std::endl;
        cv::Exception newEx(ex.code, "Exception caught", __func__,
                __FILE__, __LINE__);
        throw newEx;
    }
}

VisualOdometer getVisualOdometer(cv::VideoCapture &capture,
        const ShiThomasiParameters &shiThomasi,
        const LucasKanadeParameters &lucasKanade,
        const ChessboardParameters &chessboard,
        CameraSpatialParameters &spatial,
        const std::string &cameraParametersFilename,
        const std::string &cameraSpatialFilename,
        const int &maxHistoryLength, const int &maxDeviation,
        const int &maxDeviants, const int &minLength,
        const int &maxLength, const int &upperMargin,
        const int &lowerMargin, const int & maxFeaturesUpper,
        const int & maxFeaturesLower,
        std::vector<cv::Mat> &rectifyMaps, cv::Size &imageSize,
        std::list<cv::Point3f> &positions,
        const cv::Mat &cameraMatrix,
        const cv::Mat &distortionCoefficients,
        void (*parametersCalibration)(cv::VideoCapture &capture,
                std::vector<cv::Mat> rectifyMaps, int &horizon,
                int &deadZone, const cv::Size &imageSize)) {

    ShiThomasiFeatureExtractor extractor(shiThomasi.qualityLevel,
            shiThomasi.minDistance, shiThomasi.blockSize,
            shiThomasi.winSize, shiThomasi.zeroZone,
            shiThomasi.termCrit);
    LucasKanadePyramidTracker tracker(lucasKanade.winSize,
            lucasKanade.maxLevel, lucasKanade.flags,
            lucasKanade.termCrit, lucasKanade.minEigenvalueThresh,
            lucasKanade.maxErrorValue);

    try {
        std::list<FeatureFilter*> filters;

        filters.push_back(new TrimHistoryFilter(maxHistoryLength));
        filters.push_back(
                new SmoothFilter(maxDeviation, maxDeviants, minLength,
                        maxLength));

        cv::Point3f currentPosition(0, 0, 0);

        positions.push_back(currentPosition);

        if (0 >= spatial.horizon || 0 > spatial.deadZone) {
            spatial.horizon = imageSize.height / 2;
            spatial.deadZone = 0;
            parametersCalibration(capture, rectifyMaps,
                    spatial.horizon, spatial.deadZone, imageSize);

            std::vector<cv::Point2f> corners = getChessboardCorners(
                    capture, rectifyMaps, spatial.horizon,
                    spatial.deadZone, chessboard.size, imageSize,
                    chessboard.winSize, chessboard.zeroZone,
                    chessboard.termCrit);
            cerr << "got corners" << endl;
            getHomographyRtMatrixAndRotationCenter(corners, imageSize,
                    chessboard.size, chessboard.squareSize,
                    spatial.horizon, spatial.deadZone, cameraMatrix,
                    distortionCoefficients, chessboard.height,
                    spatial.homography, spatial.rotationCenter,
                    spatial.rtMatrix);
            if (!cameraSpatialFilename.empty()) {
                cv::FileStorage fs(cameraSpatialFilename,
                        cv::FileStorage::WRITE);
                if (!fs.isOpened()) {
                    cv::Exception ex(-1, "Could not open FileStorage",
                            __func__, __FILE__, __LINE__);
                    throw ex;
                }
                fs << "cameraSpatialParameters" << spatial;
                fs.release();
            }

        }
        TangentRotationReader rotationReader(tracker, extractor,
                filters, maxFeaturesUpper,
                cameraMatrix.at<double>(0, 0),
                cv::Size(imageSize.width,
                        spatial.horizon - spatial.deadZone),
                upperMargin);
        BirdsEyeTranslationReader transReader(spatial.homography,
                extractor, tracker, maxFeaturesLower, filters,
                spatial.rotationCenter,
                cv::Size(imageSize.width,
                        imageSize.height - spatial.horizon
                                - spatial.deadZone), lowerMargin);
        VisualOdometer odo(rotationReader, transReader,
                spatial.horizon, spatial.deadZone);
        return odo;
    } catch (const cv::Exception &ex) {
        if (USER_TRIGGERED_EXIT != ex.code) {
            std::cerr << "EXCEPTION!!!!!!!!!!!" << std::endl
                    << ex.what() << std::endl;
        }
        cv::Exception newEx(ex.code, "Exception caught", __func__,
                __FILE__, __LINE__);
        throw newEx;
    }

}
<|endoftext|>


<commit_before>#include <cstdio>
#include <cstdlib>
#include <cmath>
#include "iRRAM.h"
#include "iRRAM/core.h"

using namespace iRRAM;

// tests of alternative implemenations for sine and cosine

REAL one_minus_cos_approx (int prec, int wd, const REAL& x){
precision_policy(RELATIVE);
  REAL x2=square(x);
  REAL xpow[wd],xf=power(x2,wd),z;
  xpow[0]=-x2/2;
  z=xpow[0];
  for (int i=1; i<wd; i++) {
     xpow[i]=xpow[i-1]*x2/(-(2*i+1)*(2*i+2));
     z+=xpow[i];
  }
  REAL xp=1,e=z;
  INTEGER f=1;
  int i=1;
  while ( upperbound(xpow[wd-1])+upperbound(xp)> prec || bool(! bound(e,-1)) ) {
    xp*=xf; e=0;
    for (int j=0; j<wd; j++) {
      f=1;
      for (int k=i;k<i+wd;k++) f*=(-(2*k+2)*(2*k+1));
      xpow[j]=xpow[j]/REAL(f);
      i=i+1;
      e+=xpow[j];
    }
    e*=xp;     z+=e;
  }
precision_policy(ABSOLUTE);
  return z;
}

REAL cos_new_2 (int prec,const REAL& x){
  int it;
  int wd;
  if ( prec > -100 ) {it =10; wd= 1;}
  else if ( prec > -400 ) {it =15; wd= 2;}
  else if (prec > -800 ) {it=20; wd =2;}
  else if (prec > -1600 ) {it=24; wd =2;}
  else if (prec > -3200 ) {it=40; wd =3;}
  else if (prec > -6400 ) {it=20; wd =4;}
  else if (prec > -12000 ) {it=60; wd =5;}
  else if (prec > -24000 ) {it=80; wd =8;}
  else if (prec > -48000 ) {it=100; wd =12;}
  else if (prec > -100000 ) {it=120; wd =18;}
  else if (prec > -200000 ) {it=140; wd =26;}
  else {
     it=(int)(log(double(-prec))*8+6);
     wd=int(std::sqrt(double(-prec)/18));
  }
  REAL y=x;

precision_policy(RELATIVE);
  y=scale(y,-it);

  y=one_minus_cos_approx(prec,wd,y);
precision_policy(RELATIVE);

  for (int i=1;i<=it;i+=1) y=scale(y,2)+scale(square(y),1);
precision_policy(ABSOLUTE);

  return 1+y;
}

REAL cos_new_1 (int prec,const REAL& x){
  if (prec >= 0 ) return 0;
  REAL m=modulo(x,2*pi());
  return cos_new_2(prec,m-2*pi());
}

REAL trig_norm(int& position,const REAL& x){
  REAL m=modulo(x,2*pi());
  int i=round(m*100);
  if ( i <  0 ) {i=i+628;m=m+2*pi();}
  if ( i> 550 ) {
    position=1;
    return m-2*pi();
  } else if ( i> 392) {
    position=1;return m-2*pi();
  } else if ( i> 235) {
    position=1;return m-2*pi();
  } else if ( i> 80) {
    position=1;return m-2*pi();
  } else {
    position=1;return m-2*pi();
  }
}

//------------------------------------------------------------------------------------
REAL cos_new_alternative_2 (int prec,const REAL& x){
  int it;
  int wd;
  if ( prec > -100 ) {it =8; wd= 1;}
  else if ( prec > -400 ) {it =16; wd= 2;}
  else if (prec > -800 ) {it=20; wd =2;}
  else if (prec > -1600 ) {it=24; wd =2;}
  else if (prec > -3200 ) {it=40; wd =3;}
  else if (prec > -6400 ) {it=50; wd =3;}
  else if (prec > -12000 ) {it=60; wd =5;}
  else if (prec > -24000 ) {it=80; wd =8;}
  else if (prec > -48000 ) {it=100; wd =12;}
  else if (prec > -100000 ) {it=120; wd =18;}
  else if (prec > -200000 ) {it=140; wd =26;}
  else {
     it=(int)(log(double(-prec))*8+6);
     wd=int(std::sqrt(double(-prec)/18));
  }
  REAL y=x;

precision_policy(RELATIVE);
  y=scale(y,-it);

  y=one_minus_cos_approx(prec,wd,y)+1;
precision_policy(RELATIVE);

  for (int i=1;i<=it;i+=1) y=scale(square(y),1)-1;
precision_policy(ABSOLUTE);

  return y;
}

REAL cos_new_alternative (int prec,const REAL& x){
  if (prec >= 0 ) return 0;
  REAL m=modulo(x,2*pi());
  return cos_new_alternative_2(prec,m-2*pi());
}

bool total_domain(const REAL& x){ return true;}

REAL cos_new2 (const REAL& x){ return  limit_lip(cos_new_alternative,0,total_domain,x);}

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

REAL cos_new (const REAL& x){ return  limit_lip(cos_new_1,0,total_domain,x);}

REAL sin_new (const REAL& x){ return  cos_new(x-pi()/2);}

void sincos_new (REAL& s,REAL& c,const REAL& x){
  REAL arg=x;
  s=sin_new(arg);
  c=cos_new(arg);
}

/*
void compute(){

  int deci_places;
  int n;
  rprintf("Desired Decimals: ");  rscanf("%d",&deci_places);

  do {
    rprintf("\n cos(0.5)=");
    rwrite(cos_new(REAL(0.5)),deci_places+8);
    rprintf("\n cos2(0.5)=");
    rwrite(cos_new2(REAL(0.5)),deci_places+8);
    rprintf("\n sin(0.5)=");
    rwrite(sin_new(REAL(0.5)),deci_places+8);
    rprintf("\n\nAnother try? ");
    rscanf("%d",&deci_places);
  } while ( deci_places > 0 );
};

*/
#include <sys/time.h>
#include <sys/resource.h>
#include <unistd.h>

double cputime(){
 struct rusage r;
 getrusage (RUSAGE_SELF,&r);
 return r.ru_utime.tv_sec+0.000001*r.ru_utime.tv_usec;
 }

void printval(std::string s,int DP, const REAL& y)
{
cout << s <<"\n" <<setRwidth(DP+8)<<y<<"\n\n";
}

void printval(std::string s,int DP, const DYADIC& y)
{
cout << s <<"\n" <<setRwidth(DP+8)<<y<<"\n\n";
}

void compute(){
DYADIC value[100];

int DP;
cout << "Length of mantissa: ";
cin  >> DP;
cout <<"\n";

REAL x=sqrt(REAL(3));
REAL y=sqrt(REAL(5));
REAL PI =pi();
int prec=-10*DP/3;
int min_fkt=1;
int max_fkt=10;

for (int fkt=min_fkt; fkt<=max_fkt; fkt++) {
if (fkt ==1)  value[1]=approx(x*y,prec);
if (fkt ==2)  value[1]=approx(x/y,prec);
if (fkt ==3)  value[1]=approx(sqrt(x),prec);
if (fkt ==4)  value[1]=approx(exp(x),prec);
if (fkt ==5)  value[1]=approx(log(x),prec);
if (fkt ==6)  value[1]=approx(sin(x),prec);
if (fkt ==7)  value[1]=approx(cos(x),prec);
if (fkt ==8)  value[1]=approx(sin_new(x),prec);
if (fkt ==9)  value[1]=approx(cos_new(x),prec);
if (fkt ==10)  value[1]=approx(cos_new2(x),prec);
}

for (int fkt=min_fkt; fkt<=max_fkt; fkt++) {

double s1=cputime(),s2=s1;
int   count=1;
{
  single_valued code;
  while (s2-s1 < 1 ) {
  if (fkt ==1) for (int i=1; i<=count;i++) value[1]=approx(x*y,prec);
  if (fkt ==2) for (int i=1; i<=count;i++) value[1]=approx(x/y,prec);
  if (fkt ==3) for (int i=1; i<=count;i++) value[1]=approx(sqrt(x),prec);
  if (fkt ==4) for (int i=1; i<=count;i++) value[1]=approx(exp(x),prec);
  if (fkt ==5) for (int i=1; i<=count;i++) value[1]=approx(log(x),prec);
  if (fkt ==6) for (int i=1; i<=count;i++) value[1]=approx(sin(x),prec);
  if (fkt ==7) for (int i=1; i<=count;i++) value[1]=approx(cos(x),prec);
  if (fkt ==8) for (int i=1; i<=count;i++) value[1]=approx(sin_new(x),prec);
  if (fkt ==9) for (int i=1; i<=count;i++) value[1]=approx(cos_new(x),prec);
  if (fkt ==10) for (int i=1; i<=count;i++) value[1]=approx(cos_new2(x),prec);
  s2=cputime();
  count=2*count;
  }
}
if (fkt ==1) cout << "x*y     took ";
if (fkt ==2) cout << "x/y     took ";
if (fkt ==3) cout << "sqrt(x) took ";
if (fkt ==4) cout << "exp(x)  took ";
if (fkt ==5) cout << "log(x)  took ";
if (fkt ==6) cout << "sin(x)  took ";
if (fkt ==7) cout << "cos(x)  took ";
if (fkt ==8) cout << "sin_n(x)took ";
if (fkt ==9) cout << "cos_n(x)took ";
if (fkt ==10) cout << "cos_n2(x)took ";
cout <<1000*(s2-s1)/(count-1)<<" ms  ("
     <<count-1<<" tries in "
     << s2-s1 << " s)\n";
}

}

<commit_msg>Fix undefined behaviour: Variable Length Arrays cannot be of non-POD type (replaced by std::vector<REAL>).<commit_after>#include <cstdio>
#include <cstdlib>
#include <cmath>
#include "iRRAM.h"
#include "iRRAM/core.h"

using namespace iRRAM;

// tests of alternative implemenations for sine and cosine

REAL one_minus_cos_approx (int prec, int wd, const REAL& x){
precision_policy(RELATIVE);
  REAL x2=square(x);
  std::vector<REAL> xpow(wd);
  REAL xf=power(x2,wd),z;
  xpow[0]=-x2/2;
  z=xpow[0];
  for (int i=1; i<wd; i++) {
     xpow[i]=xpow[i-1]*x2/(-(2*i+1)*(2*i+2));
     z+=xpow[i];
  }
  REAL xp=1,e=z;
  INTEGER f=1;
  int i=1;
  while ( upperbound(xpow[wd-1])+upperbound(xp)> prec || bool(! bound(e,-1)) ) {
    xp*=xf; e=0;
    for (int j=0; j<wd; j++) {
      f=1;
      for (int k=i;k<i+wd;k++) f*=(-(2*k+2)*(2*k+1));
      xpow[j]=xpow[j]/REAL(f);
      i=i+1;
      e+=xpow[j];
    }
    e*=xp;     z+=e;
  }
precision_policy(ABSOLUTE);
  return z;
}

REAL cos_new_2 (int prec,const REAL& x){
  int it;
  int wd;
  if ( prec > -100 ) {it =10; wd= 1;}
  else if ( prec > -400 ) {it =15; wd= 2;}
  else if (prec > -800 ) {it=20; wd =2;}
  else if (prec > -1600 ) {it=24; wd =2;}
  else if (prec > -3200 ) {it=40; wd =3;}
  else if (prec > -6400 ) {it=20; wd =4;}
  else if (prec > -12000 ) {it=60; wd =5;}
  else if (prec > -24000 ) {it=80; wd =8;}
  else if (prec > -48000 ) {it=100; wd =12;}
  else if (prec > -100000 ) {it=120; wd =18;}
  else if (prec > -200000 ) {it=140; wd =26;}
  else {
     it=(int)(log(double(-prec))*8+6);
     wd=int(std::sqrt(double(-prec)/18));
  }
  REAL y=x;

precision_policy(RELATIVE);
  y=scale(y,-it);

  y=one_minus_cos_approx(prec,wd,y);
precision_policy(RELATIVE);

  for (int i=1;i<=it;i+=1) y=scale(y,2)+scale(square(y),1);
precision_policy(ABSOLUTE);

  return 1+y;
}

REAL cos_new_1 (int prec,const REAL& x){
  if (prec >= 0 ) return 0;
  REAL m=modulo(x,2*pi());
  return cos_new_2(prec,m-2*pi());
}

REAL trig_norm(int& position,const REAL& x){
  REAL m=modulo(x,2*pi());
  int i=round(m*100);
  if ( i <  0 ) {i=i+628;m=m+2*pi();}
  if ( i> 550 ) {
    position=1;
    return m-2*pi();
  } else if ( i> 392) {
    position=1;return m-2*pi();
  } else if ( i> 235) {
    position=1;return m-2*pi();
  } else if ( i> 80) {
    position=1;return m-2*pi();
  } else {
    position=1;return m-2*pi();
  }
}

//------------------------------------------------------------------------------------
REAL cos_new_alternative_2 (int prec,const REAL& x){
  int it;
  int wd;
  if ( prec > -100 ) {it =8; wd= 1;}
  else if ( prec > -400 ) {it =16; wd= 2;}
  else if (prec > -800 ) {it=20; wd =2;}
  else if (prec > -1600 ) {it=24; wd =2;}
  else if (prec > -3200 ) {it=40; wd =3;}
  else if (prec > -6400 ) {it=50; wd =3;}
  else if (prec > -12000 ) {it=60; wd =5;}
  else if (prec > -24000 ) {it=80; wd =8;}
  else if (prec > -48000 ) {it=100; wd =12;}
  else if (prec > -100000 ) {it=120; wd =18;}
  else if (prec > -200000 ) {it=140; wd =26;}
  else {
     it=(int)(log(double(-prec))*8+6);
     wd=int(std::sqrt(double(-prec)/18));
  }
  REAL y=x;

precision_policy(RELATIVE);
  y=scale(y,-it);

  y=one_minus_cos_approx(prec,wd,y)+1;
precision_policy(RELATIVE);

  for (int i=1;i<=it;i+=1) y=scale(square(y),1)-1;
precision_policy(ABSOLUTE);

  return y;
}

REAL cos_new_alternative (int prec,const REAL& x){
  if (prec >= 0 ) return 0;
  REAL m=modulo(x,2*pi());
  return cos_new_alternative_2(prec,m-2*pi());
}

bool total_domain(const REAL& x){ return true;}

REAL cos_new2 (const REAL& x){ return  limit_lip(cos_new_alternative,0,total_domain,x);}

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

REAL cos_new (const REAL& x){ return  limit_lip(cos_new_1,0,total_domain,x);}

REAL sin_new (const REAL& x){ return  cos_new(x-pi()/2);}

void sincos_new (REAL& s,REAL& c,const REAL& x){
  REAL arg=x;
  s=sin_new(arg);
  c=cos_new(arg);
}

/*
void compute(){

  int deci_places;
  int n;
  rprintf("Desired Decimals: ");  rscanf("%d",&deci_places);

  do {
    rprintf("\n cos(0.5)=");
    rwrite(cos_new(REAL(0.5)),deci_places+8);
    rprintf("\n cos2(0.5)=");
    rwrite(cos_new2(REAL(0.5)),deci_places+8);
    rprintf("\n sin(0.5)=");
    rwrite(sin_new(REAL(0.5)),deci_places+8);
    rprintf("\n\nAnother try? ");
    rscanf("%d",&deci_places);
  } while ( deci_places > 0 );
};

*/
#include <sys/time.h>
#include <sys/resource.h>
#include <unistd.h>

double cputime(){
 struct rusage r;
 getrusage (RUSAGE_SELF,&r);
 return r.ru_utime.tv_sec+0.000001*r.ru_utime.tv_usec;
 }

void printval(std::string s,int DP, const REAL& y)
{
cout << s <<"\n" <<setRwidth(DP+8)<<y<<"\n\n";
}

void printval(std::string s,int DP, const DYADIC& y)
{
cout << s <<"\n" <<setRwidth(DP+8)<<y<<"\n\n";
}

void compute(){
DYADIC value[100];

int DP;
cout << "Length of mantissa: ";
cin  >> DP;
cout <<"\n";

REAL x=sqrt(REAL(3));
REAL y=sqrt(REAL(5));
REAL PI =pi();
int prec=-10*DP/3;
int min_fkt=1;
int max_fkt=10;

for (int fkt=min_fkt; fkt<=max_fkt; fkt++) {
if (fkt ==1)  value[1]=approx(x*y,prec);
if (fkt ==2)  value[1]=approx(x/y,prec);
if (fkt ==3)  value[1]=approx(sqrt(x),prec);
if (fkt ==4)  value[1]=approx(exp(x),prec);
if (fkt ==5)  value[1]=approx(log(x),prec);
if (fkt ==6)  value[1]=approx(sin(x),prec);
if (fkt ==7)  value[1]=approx(cos(x),prec);
if (fkt ==8)  value[1]=approx(sin_new(x),prec);
if (fkt ==9)  value[1]=approx(cos_new(x),prec);
if (fkt ==10)  value[1]=approx(cos_new2(x),prec);
}

for (int fkt=min_fkt; fkt<=max_fkt; fkt++) {

double s1=cputime(),s2=s1;
int   count=1;
{
  single_valued code;
  while (s2-s1 < 1 ) {
  if (fkt ==1) for (int i=1; i<=count;i++) value[1]=approx(x*y,prec);
  if (fkt ==2) for (int i=1; i<=count;i++) value[1]=approx(x/y,prec);
  if (fkt ==3) for (int i=1; i<=count;i++) value[1]=approx(sqrt(x),prec);
  if (fkt ==4) for (int i=1; i<=count;i++) value[1]=approx(exp(x),prec);
  if (fkt ==5) for (int i=1; i<=count;i++) value[1]=approx(log(x),prec);
  if (fkt ==6) for (int i=1; i<=count;i++) value[1]=approx(sin(x),prec);
  if (fkt ==7) for (int i=1; i<=count;i++) value[1]=approx(cos(x),prec);
  if (fkt ==8) for (int i=1; i<=count;i++) value[1]=approx(sin_new(x),prec);
  if (fkt ==9) for (int i=1; i<=count;i++) value[1]=approx(cos_new(x),prec);
  if (fkt ==10) for (int i=1; i<=count;i++) value[1]=approx(cos_new2(x),prec);
  s2=cputime();
  count=2*count;
  }
}
if (fkt ==1) cout << "x*y     took ";
if (fkt ==2) cout << "x/y     took ";
if (fkt ==3) cout << "sqrt(x) took ";
if (fkt ==4) cout << "exp(x)  took ";
if (fkt ==5) cout << "log(x)  took ";
if (fkt ==6) cout << "sin(x)  took ";
if (fkt ==7) cout << "cos(x)  took ";
if (fkt ==8) cout << "sin_n(x)took ";
if (fkt ==9) cout << "cos_n(x)took ";
if (fkt ==10) cout << "cos_n2(x)took ";
cout <<1000*(s2-s1)/(count-1)<<" ms  ("
     <<count-1<<" tries in "
     << s2-s1 << " s)\n";
}

}

<|endoftext|>


<commit_before>/*
 *  This file is part of qtzeroconf. (c) 2012 Johannes Hilden
 *  https://github.com/johanneshilden/qtzeroconf
 *
 *  qtzeroconf is free software; you can redistribute it and/or modify it
 *  under the terms of the GNU Lesser General Public License as
 *  published by the Free Software Foundation; either version 2.1 of the
 *  License, or (at your option) any later version.
 *
 *  qtzeroconf 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 qtzeroconf; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
 */

#include <QDebug>
#include "zconfbrowserwidget.h"
#include "zconfservicebrowser.h"

class ZConfBrowserWidgetPrivate
{
public:
    ZConfBrowserWidgetPrivate(ZConfBrowserWidget *widget)
        : q(widget),
          browser(0)
    {
        q->setColumnCount(6);
        QStringList headers;
        q->setHeaderLabels(headers << QObject::tr("Service")
                                   << QObject::tr("Domain")
                                   << QObject::tr("Host")
                                   << QObject::tr("Protocol")
                                   << QObject::tr("IP")
                                   << QObject::tr("Port"));

        q->setColumnWidth(0, 170);
    }

    void init(QString serviceType)
    {
        delete browser;
        browser = new ZConfServiceBrowser(q);
        type = serviceType;
        QObject::connect(browser, SIGNAL(serviceEntryAdded(QString)), q, SLOT(addService(QString)));
        QObject::connect(browser, SIGNAL(serviceEntryRemoved(QString)), q, SLOT(removeService(QString)));
        q->clear();
        browser->browse(type);
    }

    ZConfBrowserWidget  *const q;
    ZConfServiceBrowser *browser;
    QString              type;
};

/*!
    \class ZConfBrowserWidget

    \brief QTreeWidget-based widget for browsing and displaying available
    Zeroconf services on the local network.
 */

/*!
    Creates a ZConfBrowserWidget object using the provided service type.
 */
ZConfBrowserWidget::ZConfBrowserWidget(QString serviceType, QWidget *parent)
    : QTreeWidget(parent),
      d_ptr(new ZConfBrowserWidgetPrivate(this))
{
    d_ptr->init(serviceType);
}

/*!
    Convenience constructor that uses "_http._tcp" as service type.
 */
ZConfBrowserWidget::ZConfBrowserWidget(QWidget *parent)
    : QTreeWidget(parent),
      d_ptr(new ZConfBrowserWidgetPrivate(this))
{
    d_ptr->init("_http._tcp");
}

/*!
    Destroys the ZConfBrowserWidget object.
 */
ZConfBrowserWidget::~ZConfBrowserWidget()
{
    delete d_ptr;
}

/*!
    Enables/disables condensed list mode. In condensed mode, only service name,
    domain and host name is shown in the list.
 */
void ZConfBrowserWidget::setCondensed(bool enabled)
{
    for (int i = 3; i < 7; i++) setColumnHidden(i, enabled);
    setRootIsDecorated(!enabled);
}

void ZConfBrowserWidget::addService(QString service)
{
    QTreeWidgetItem *parent = 0;
    for (int i = 0; i < topLevelItemCount(); i++) {
        QTreeWidgetItem *item = topLevelItem(i);
        if (service == item->text(0)) {
            parent = item;
            break;
        }
    }
    ZConfServiceEntry serviceEntry = d_ptr->browser->serviceEntry(service);
    if (!serviceEntry.isValid())
        return;         // No service entry with that name found.
    if (!parent) {
        parent = new QTreeWidgetItem(this);
        parent->setText(0, service);
        parent->setText(1, serviceEntry.domain);
        parent->setText(2, serviceEntry.host);
        parent->setText(5, QString::number(serviceEntry.port));
    }

    QTreeWidgetItem *entry = new QTreeWidgetItem(parent);
    entry->setText(0, service);
    entry->setText(1, serviceEntry.domain);
    entry->setText(3, serviceEntry.protocolName());
    entry->setText(4, serviceEntry.ip);
    entry->setText(5, QString::number(serviceEntry.port));
}

void ZConfBrowserWidget::removeService(QString service)
{
    int count = topLevelItemCount();
    int i = 0;
    while (i < count) {
        QTreeWidgetItem *item = topLevelItem(i);
        if (service == item->text(0)) {
            delete item;
            --count;
        } else {
            ++i;
        }
    }
}
<commit_msg>minor fixes<commit_after>/*
 *  This file is part of qtzeroconf. (c) 2012 Johannes Hilden
 *  https://github.com/johanneshilden/qtzeroconf
 *
 *  qtzeroconf is free software; you can redistribute it and/or modify it
 *  under the terms of the GNU Lesser General Public License as
 *  published by the Free Software Foundation; either version 2.1 of the
 *  License, or (at your option) any later version.
 *
 *  qtzeroconf 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 qtzeroconf; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
 */

#include <QDebug>
#include "zconfbrowserwidget.h"
#include "zconfservicebrowser.h"

class ZConfBrowserWidgetPrivate
{
public:
    ZConfBrowserWidgetPrivate(ZConfBrowserWidget *widget)
        : q(widget),
          browser(0)
    {
        q->setColumnCount(6);
        QStringList headers;
        q->setHeaderLabels(headers << QObject::tr("Service")
                                   << QObject::tr("Domain")
                                   << QObject::tr("Host")
                                   << QObject::tr("Protocol")
                                   << QObject::tr("IP")
                                   << QObject::tr("Port"));

        q->setColumnWidth(0, 170);
    }

    void init(QString serviceType)
    {
        delete browser;
        browser = new ZConfServiceBrowser(q);
        type = serviceType;
        QObject::connect(browser, SIGNAL(serviceEntryAdded(QString)), q, SLOT(addService(QString)));
        QObject::connect(browser, SIGNAL(serviceEntryRemoved(QString)), q, SLOT(removeService(QString)));
        q->clear();
        browser->browse(type);
    }

    ZConfBrowserWidget  *const q;
    ZConfServiceBrowser *browser;
    QString              type;
};

/*!
    \class ZConfBrowserWidget

    \brief QTreeWidget-based widget for browsing and displaying Zeroconf
    services available on the local network.
 */

/*!
    Creates a ZConfBrowserWidget object using the provided service type.
 */
ZConfBrowserWidget::ZConfBrowserWidget(QString serviceType, QWidget *parent)
    : QTreeWidget(parent),
      d_ptr(new ZConfBrowserWidgetPrivate(this))
{
    d_ptr->init(serviceType);
}

/*!
    Convenience constructor that uses "_http._tcp" as service type.
 */
ZConfBrowserWidget::ZConfBrowserWidget(QWidget *parent)
    : QTreeWidget(parent),
      d_ptr(new ZConfBrowserWidgetPrivate(this))
{
    d_ptr->init("_http._tcp");
}

/*!
    Destroys the ZConfBrowserWidget object.
 */
ZConfBrowserWidget::~ZConfBrowserWidget()
{
    delete d_ptr;
}

/*!
    Enables/disables condensed list mode. In condensed mode, only service name,
    domain and host name is shown in the list.
 */
void ZConfBrowserWidget::setCondensed(bool enabled)
{
    for (int i = 3; i < 7; i++) setColumnHidden(i, enabled);
    setRootIsDecorated(!enabled);
}

void ZConfBrowserWidget::addService(QString service)
{
    QTreeWidgetItem *parent = 0;
    for (int i = 0; i < topLevelItemCount(); i++) {
        QTreeWidgetItem *item = topLevelItem(i);
        if (service == item->text(0)) {
            parent = item;
            break;
        }
    }
    ZConfServiceEntry serviceEntry = d_ptr->browser->serviceEntry(service);
    if (!serviceEntry.isValid())
        return;         // No service entry with that name found.
    if (!parent) {
        parent = new QTreeWidgetItem(this);
        parent->setText(0, service);
        parent->setText(1, serviceEntry.domain);
        parent->setText(2, serviceEntry.host);
        parent->setText(5, QString::number(serviceEntry.port));
    }

    QTreeWidgetItem *entry = new QTreeWidgetItem(parent);
    entry->setText(0, service);
    entry->setText(1, serviceEntry.domain);
    entry->setText(3, serviceEntry.protocolName());
    entry->setText(4, serviceEntry.ip);
    entry->setText(5, QString::number(serviceEntry.port));
}

void ZConfBrowserWidget::removeService(QString service)
{
    int count = topLevelItemCount();
    int i = 0;
    while (i < count) {
        QTreeWidgetItem *item = topLevelItem(i);
        if (service == item->text(0)) {
            delete item;
            --count;
        } else {
            ++i;
        }
    }
}
<|endoftext|>