<commit_before>#include <valhalla/baldr/signinfo.h>

#include "test.h"

#include "mjolnir/graphbuilder.h"

using namespace std;
using namespace valhalla::mjolnir;
using valhalla::mjolnir::GraphBuilder;

namespace {

class test_graph_builder : public GraphBuilder {
 public:
  using GraphBuilder::CreateExitSignInfoList;
};

void ExitToTest() {

  uint64_t osmnodeid = 1;
  Node node;
  OSMWay way;
  std::unordered_map<uint64_t, std::string> map_ref;
  std::unordered_map<uint64_t, std::string> map_name;
  std::unordered_map<uint64_t, std::string> map_exit_to;

  node.set_exit_to(true);

  map_exit_to[osmnodeid] = "US 11;To I 81;Carlisle;Harrisburg";

  std::vector<SignInfo> exitsigns;
  exitsigns = test_graph_builder::CreateExitSignInfoList(osmnodeid, node, way, map_ref, map_name, map_exit_to);

  if (exitsigns.size() == 4) {
    for (auto& exitsign : exitsigns) {
      if (exitsign.type() != Sign::Type::kExitToward)
        throw std::runtime_error("US 11;To I 81;Carlisle;Harrisburg types are not all Toward");
    }

    if (exitsigns[0].text() != "US 11" && exitsigns[1].text() != "I 81" &&
        exitsigns[2].text() != "Carlisle" && exitsigns[3].text() != "Harrisburg")
      throw std::runtime_error("Exitsign text is bad for US 11;To I 81;Carlisle;Harrisburg.");
  }
  else throw std::runtime_error("US 11/To I 81/Carlisle/Harrisburg failed to be parsed.  " + exitsigns.size() );

  exitsigns.clear();
  map_exit_to[osmnodeid] = "US 11;Toward I 81;Carlisle;Harrisburg";

  exitsigns = test_graph_builder::CreateExitSignInfoList(osmnodeid, node, way, map_ref, map_name, map_exit_to);

  if (exitsigns.size() == 4) {
    for (auto& exitsign : exitsigns) {
      if (exitsign.type() != Sign::Type::kExitToward)
        throw std::runtime_error("US 11;Toward I 81;Carlisle;Harrisburg types are not all Toward");
    }
    if (exitsigns[0].text() != "US 11" && exitsigns[1].text() != "I 81" &&
        exitsigns[2].text() != "Carlisle" && exitsigns[3].text() != "Harrisburg")
      throw std::runtime_error("Exitsign text is bad for US 11;To I 81;Carlisle;Harrisburg.");
  }
  else throw std::runtime_error("US 11;Toward I 81;Carlisle;Harrisburg failed to be parsed.");

  exitsigns.clear();
  map_exit_to[osmnodeid] = "I 95 To I 695";

  exitsigns = test_graph_builder::CreateExitSignInfoList(osmnodeid, node, way, map_ref, map_name, map_exit_to);

  if (exitsigns.size() == 2) {
     if (exitsigns[0].type() != Sign::Type::kExitBranch)
       throw std::runtime_error("I 95 should be a branch.");

     if (exitsigns[1].type() != Sign::Type::kExitToward)
       throw std::runtime_error("I 695 should be a toward.");

     if (exitsigns[0].text() != "I 95" && exitsigns[1].text() != "I 695")
       throw std::runtime_error("Exitsign text is bad for I 95 To I 695");
  }
  else throw std::runtime_error("I 95 To I 695 failed to be parsed.");

  exitsigns.clear();
  map_exit_to[osmnodeid] = "I 495 Toward I 270";

  exitsigns = test_graph_builder::CreateExitSignInfoList(osmnodeid, node, way, map_ref, map_name, map_exit_to);

  if (exitsigns.size() == 2) {
    if (exitsigns[0].type() != Sign::Type::kExitBranch)
      throw std::runtime_error("I 495 should be a branch.");

    if (exitsigns[1].type() != Sign::Type::kExitToward)
      throw std::runtime_error("I 270 should be a toward.");

    if (exitsigns[0].text() != "I 495" && exitsigns[1].text() != "I 270")
      throw std::runtime_error("Exitsign text is bad for I 495 Toward I 270");
  }
  else throw std::runtime_error("I 495 Toward I 270 failed to be parsed.");

  exitsigns.clear();
  map_exit_to[osmnodeid] = "I 495 Toward I 270 To I 95";//default to toward.  Punt on parsing.

  exitsigns = test_graph_builder::CreateExitSignInfoList(osmnodeid, node, way, map_ref, map_name, map_exit_to);

  if (exitsigns.size() == 1) {
    if (exitsigns[0].type() != Sign::Type::kExitToward)
      throw std::runtime_error("I 495 Toward I 270 To I 95 should be a toward.");

    if (exitsigns[0].text() != "I 495 Toward I 270 To I 95")
      throw std::runtime_error("Exitsign text is bad for I 495 Toward I 270 To I 95");
  }
  else throw std::runtime_error("I 495 Toward I 270 To I 95 failed to be parsed.");

}

}

int main() {
  test::suite suite("signinfo");

  // Test setting and getting on random sizes of bit tables
  suite.test(TEST_CASE(ExitToTest));

  return suite.tear_down();
}
<commit_msg>fixed typo.<commit_after>#include <valhalla/baldr/signinfo.h>

#include "test.h"

#include "mjolnir/graphbuilder.h"

using namespace std;
using namespace valhalla::mjolnir;
using valhalla::mjolnir::GraphBuilder;

namespace {

class test_graph_builder : public GraphBuilder {
 public:
  using GraphBuilder::CreateExitSignInfoList;
};

void ExitToTest() {

  uint64_t osmnodeid = 1;
  Node node;
  OSMWay way;
  std::unordered_map<uint64_t, std::string> map_ref;
  std::unordered_map<uint64_t, std::string> map_name;
  std::unordered_map<uint64_t, std::string> map_exit_to;

  node.set_exit_to(true);

  map_exit_to[osmnodeid] = "US 11;To I 81;Carlisle;Harrisburg";

  std::vector<SignInfo> exitsigns;
  exitsigns = test_graph_builder::CreateExitSignInfoList(osmnodeid, node, way, map_ref, map_name, map_exit_to);

  if (exitsigns.size() == 4) {
    for (auto& exitsign : exitsigns) {
      if (exitsign.type() != Sign::Type::kExitToward)
        throw std::runtime_error("US 11;To I 81;Carlisle;Harrisburg types are not all Toward");
    }

    if (exitsigns[0].text() != "US 11" && exitsigns[1].text() != "I 81" &&
        exitsigns[2].text() != "Carlisle" && exitsigns[3].text() != "Harrisburg")
      throw std::runtime_error("Exitsign text is bad for US 11;To I 81;Carlisle;Harrisburg.");
  }
  else throw std::runtime_error("US 11/To I 81/Carlisle/Harrisburg failed to be parsed.  " + exitsigns.size() );

  exitsigns.clear();
  map_exit_to[osmnodeid] = "US 11;Toward I 81;Carlisle;Harrisburg";

  exitsigns = test_graph_builder::CreateExitSignInfoList(osmnodeid, node, way, map_ref, map_name, map_exit_to);

  if (exitsigns.size() == 4) {
    for (auto& exitsign : exitsigns) {
      if (exitsign.type() != Sign::Type::kExitToward)
        throw std::runtime_error("US 11;Toward I 81;Carlisle;Harrisburg types are not all Toward");
    }
    if (exitsigns[0].text() != "US 11" && exitsigns[1].text() != "I 81" &&
        exitsigns[2].text() != "Carlisle" && exitsigns[3].text() != "Harrisburg")
      throw std::runtime_error("Exitsign text is bad for US 11;To I 81;Carlisle;Harrisburg.");
  }
  else throw std::runtime_error("US 11;Toward I 81;Carlisle;Harrisburg failed to be parsed.");

  exitsigns.clear();
  map_exit_to[osmnodeid] = "I 95 To I 695";

  exitsigns = test_graph_builder::CreateExitSignInfoList(osmnodeid, node, way, map_ref, map_name, map_exit_to);

  if (exitsigns.size() == 2) {
     if (exitsigns[0].type() != Sign::Type::kExitBranch)
       throw std::runtime_error("I 95 should be a branch.");

     if (exitsigns[1].type() != Sign::Type::kExitToward)
       throw std::runtime_error("I 695 should be a toward.");

     if (exitsigns[0].text() != "I 95" && exitsigns[1].text() != "I 695")
       throw std::runtime_error("Exitsign text is bad for I 95 To I 695");
  }
  else throw std::runtime_error("I 95 To I 695 failed to be parsed.");

  exitsigns.clear();
  map_exit_to[osmnodeid] = "I 495 Toward I 270";

  exitsigns = test_graph_builder::CreateExitSignInfoList(osmnodeid, node, way, map_ref, map_name, map_exit_to);

  if (exitsigns.size() == 2) {
    if (exitsigns[0].type() != Sign::Type::kExitBranch)
      throw std::runtime_error("I 495 should be a branch.");

    if (exitsigns[1].type() != Sign::Type::kExitToward)
      throw std::runtime_error("I 270 should be a toward.");

    if (exitsigns[0].text() != "I 495" && exitsigns[1].text() != "I 270")
      throw std::runtime_error("Exitsign text is bad for I 495 Toward I 270");
  }
  else throw std::runtime_error("I 495 Toward I 270 failed to be parsed.");

  exitsigns.clear();
  map_exit_to[osmnodeid] = "I 495 Toward I 270 To I 95";//default to toward.  Punt on parsing.

  exitsigns = test_graph_builder::CreateExitSignInfoList(osmnodeid, node, way, map_ref, map_name, map_exit_to);

  if (exitsigns.size() == 1) {
    if (exitsigns[0].type() != Sign::Type::kExitToward)
      throw std::runtime_error("I 495 Toward I 270 To I 95 should be a toward.");

    if (exitsigns[0].text() != "I 495 Toward I 270 To I 95")
      throw std::runtime_error("Exitsign text is bad for I 495 Toward I 270 To I 95");
  }
  else throw std::runtime_error("I 495 Toward I 270 To I 95 failed to be parsed.");

}

}

int main() {
  test::suite suite("signinfo");

  // Test exit to logic.
  suite.test(TEST_CASE(ExitToTest));

  return suite.tear_down();
}
<|endoftext|>


<commit_before>/*
 * ToolFinder.cpp
 *
 *  Created on: 8 sept. 2009
 *      Author: froy
 */

#ifndef SOFAVRPNCLIENT_TOOLFINDER_INL_
#define SOFAVRPNCLIENT_TOOLFINDER_INL_

#include <ToolFinder.h>

#include <sofa/core/ObjectFactory.h>
#include <sofa/core/componentmodel/topology/BaseMeshTopology.h>

namespace sofavrpn
{

namespace client
{

using namespace sofa;
using namespace sofa::defaulttype;
using namespace sofa::core::componentmodel::topology;

template<class Datatypes>
ToolFinder<Datatypes>::ToolFinder()
    : f_points(initData(&f_points, "points", "Incoming 3D Points"))
    , f_distances(initData(&f_distances, "distances", "Distances between each point"))
    , f_center(initData(&f_center, "center", "Tool's center"))
    , f_orientation(initData(&f_orientation, "orientation", "Tool's orientation"))
    , f_rigidCenter(initData(&f_rigidCenter, "rigidCenter", "Rigid center of the tool"))
{
    addInput(&f_points);
    addInput(&f_distances);

    addOutput(&f_center);
    addOutput(&f_orientation);
    addOutput(&f_rigidCenter);

    setDirtyValue();

}

template<class Datatypes>
ToolFinder<Datatypes>::~ToolFinder()
{
    // TODO Auto-generated destructor stub
}

template<class Datatypes>
void ToolFinder<Datatypes>::update()
{
    cleanDirty();

    typedef BaseMeshTopology::Edge Edge;
    typedef BaseMeshTopology::PointID PointID;

    sofa::helper::ReadAccessor< Data<VecCoord > > inPoints = f_points;
    sofa::helper::ReadAccessor< Data<helper::vector<double> > > realDistances = f_distances;
    //sofa::helper::WriteAccessor< Data<Coord > > centerPoint = f_center;
    Coord centerPoint = *f_center.beginEdit();
    //sofa::helper::WriteAccessor< Data<Quat > > orientation = f_orientation;
    Quat& orientation = *f_orientation.beginEdit();

    //sofa::helper::WriteAccessor< Data<RCoord > > rigidPoint = f_rigidCenter;
    RCoord& rigidPoint = *f_rigidCenter.beginEdit();

    //double & result_angle = *f_angle.beginEdit();
    //const double &distance = f_distance.getValue();

    if ( (inPoints.size() != realDistances.size()) || (inPoints.size () != 3))
    {
        serr << "Tool Finder : not enough given point or distance ..." << sendl;
        rigidPoint = RCoord();
        centerPoint = Coord();
        orientation = Quat();

        f_rigidCenter.endEdit();
        f_orientation.endEdit();
        return;
    }

    //Compute distances from incoming points
    helper::vector<double> computedDistances;
    helper::vector<Edge> computedEdges;
    for (unsigned int i=0 ; i<inPoints.size() ; i++)
    {
        Vec3f v = inPoints[((i+1)%realDistances.size())] - inPoints[i];
        computedDistances.push_back(v.norm());
        computedEdges.push_back(Edge(i, ((i+1)%realDistances.size())));
    }

    //Guess current mapping between incoming edges and real edges
    std::map<Edge, Edge> mapEdges;
    PointID p1, p2;
    double EPSILON = 0.00001;

    bool first = true;
    for (unsigned int i=0 ; i<realDistances.size() ; i++)
    {
        double dist = realDistances[i];
        Edge currentRealEdge;
        currentRealEdge = Edge(i, ((i+1)%realDistances.size()) );

        unsigned int minIndex=0;
        double min = abs(dist - computedDistances[minIndex]);

        //search the nearest distances we have
        for (unsigned int j=1 ; j<computedDistances.size() ; j++)
        {
            if(fabs(dist - computedDistances[j]) - min < EPSILON )
            {
                min = abs(dist - computedDistances[j]);
                minIndex = j;
            }
        }
        mapEdges[currentRealEdge] = computedEdges[minIndex];
        //std::cout << currentRealEdge << " <-->" << computedEdges[minIndex]<< std::endl;
    }

    //Finally, get the real points from the mapping
    std::map<PointID, Coord> mapRealPoints;
    std::map<Edge, Edge>::const_iterator edgeIt = mapEdges.begin();

    Edge oldRealEdge = (*edgeIt).first;
    Edge oldIREdge = (*edgeIt).second;
    //edgeIt++;

    Edge currentRealEdge, currentIREdge;
    PointID commonRealPoint, commonIRPoint;
    unsigned int nb=0;
    for (edgeIt++ ; edgeIt != mapEdges.end(); edgeIt++)
        //for (edgeIt++ ; mapRealPoints.size() < inPoints.size(); edgeIt++)
    {
        currentRealEdge = (*edgeIt).first;
        currentIREdge = (*edgeIt).second;

        //We know that edges are consecutive
        if (currentRealEdge[0] == oldRealEdge[0] || currentRealEdge[0] == oldRealEdge[1])
            commonRealPoint = currentRealEdge[0];
        else
            commonRealPoint = currentRealEdge[1];

        if (currentIREdge[0] == oldIREdge[0] || currentIREdge[0] == oldIREdge[1])
            commonIRPoint = currentIREdge[0];
        else
            commonIRPoint = currentIREdge[1];

        oldRealEdge = currentRealEdge;
        oldIREdge = currentIREdge;

        mapRealPoints[commonRealPoint] = inPoints[commonIRPoint];
        //std::cout << commonRealPoint << " -> " << inPoints[commonIRPoint] << std::endl;
    }
    //get the last point
    edgeIt = mapEdges.begin();
    currentRealEdge = (*edgeIt).first;
    currentIREdge = (*edgeIt).second;

    //We know that edges are consecutive
    if (currentRealEdge[0] == oldRealEdge[0] || currentRealEdge[0] == oldRealEdge[1])
        commonRealPoint = currentRealEdge[0];
    else
        commonRealPoint = currentRealEdge[1];

    if (currentIREdge[0] == oldIREdge[0] || currentIREdge[0] == oldIREdge[1])
        commonIRPoint = currentIREdge[0];
    else
        commonIRPoint = currentIREdge[1];

    oldRealEdge = currentRealEdge;
    oldIREdge = currentIREdge;

    mapRealPoints[commonRealPoint] = inPoints[commonIRPoint];
    //std::cout << commonRealPoint << " -> " << inPoints[commonIRPoint] << std::endl;

    //not generic code ...
    //assume that point :
    //0 is left point from above view of the tool
    //1 is right ...
    //2 is top
    // 0-----1
    //    |
    //    |
    //    2

    //Compute orientation
    Coord leftPoint = mapRealPoints[0];
    Coord rightPoint = mapRealPoints[1];
    Coord topPoint = mapRealPoints[2];

    Coord xAxis = (leftPoint+rightPoint)*0.5 - topPoint;
    xAxis.normalize();
    Coord yAxis = (leftPoint - topPoint).cross(rightPoint - topPoint);
    yAxis.normalize();
    Coord zAxis = xAxis.cross(yAxis);
    zAxis.normalize();

    sofa::defaulttype::Quat q;
    q = q.createQuaterFromFrame(xAxis, yAxis, zAxis);
    q.normalize();
    //Compute center
    centerPoint = topPoint;
    orientation = q;
    rigidPoint.getCenter() = topPoint;
    rigidPoint.getOrientation() = q;

    f_rigidCenter.endEdit();
    f_orientation.endEdit();

    /*

    	//Get the Top Point
    	std::vector<double> lengths;
    	//get the (possible) top point
    	double length01 = (inPoints[1] - inPoints[0]).norm();
    	double length02 = (inPoints[2] - inPoints[0]).norm();
    	double length12 = (inPoints[2] - inPoints[1]).norm();
    	Coord topPoint, leftPoint, rightPoint;

    	if ( (length01 < length02) && (length01 < length12) )
    	{
    		topPoint = inPoints[2];
    		leftPoint = inPoints[0];
    		rightPoint = inPoints[1];
    		if (inPoints[1][0] < inPoints[0][0])
    		{
    			leftPoint = inPoints[1];
    			rightPoint = inPoints[0];
    		}
    	}
    	else
    		if ( (length02 < length01) && (length02 < length12) )
    		{
    			topPoint = inPoints[1];
    			leftPoint = inPoints[0];
    			rightPoint = inPoints[1];
    			if (inPoints[2][0] < inPoints[0][0])
    			{
    				leftPoint = inPoints[2];
    				rightPoint = inPoints[0];
    			}
    		}
    		else
    		{
    			topPoint = inPoints[0];
    			leftPoint = inPoints[1];
    			rightPoint = inPoints[2];
    			if (inPoints[2][0] < inPoints[1][0])
    			{
    				leftPoint = inPoints[2];
    				rightPoint = inPoints[1];
    			}
    		}

    	double topLeftLength = (leftPoint - topPoint).norm();
    	double leftRightLength = (leftPoint - rightPoint).norm();
    	//
    	double angle = atan( (leftRightLength - distance)/topLeftLength );

    	//
    	Coord xAxis = (leftPoint+rightPoint)*0.5 - topPoint;
    	xAxis.normalize();
    	Coord yAxis = (leftPoint - topPoint).cross(rightPoint - topPoint);
    	yAxis.normalize();
    	Coord zAxis = xAxis.cross(yAxis);
    	zAxis.normalize();

    	sofa::defaulttype::Quat q;
    	q = q.createQuaterFromFrame(xAxis, yAxis, zAxis);
    	topPointRigid.getCenter() = topPoint;
    	topPointRigid.getOrientation() = q;

    	leftPointW = leftPoint;
    	rightPointW = rightPoint;
    	angleW = angle;
    	angleArticulatedW.clear();
    	angleArticulatedW.push_back(0);
    	angleArticulatedW.push_back(angle);
    	angleArticulatedW.push_back(angle);

    	f_topPoint.endEdit();
    	f_leftPoint.endEdit();
    	f_rightPoint.endEdit();
    	f_angle.endEdit();
    	f_angleArticulated.endEdit();
    	*/
}

}

}

#endif //SOFAVRPNCLIENT_TOOLFINDER_INL_

<commit_msg>r6211/sofa-dev : UPDATE: replace abs with fabs for approximation<commit_after>/*
 * ToolFinder.cpp
 *
 *  Created on: 8 sept. 2009
 *      Author: froy
 */

#ifndef SOFAVRPNCLIENT_TOOLFINDER_INL_
#define SOFAVRPNCLIENT_TOOLFINDER_INL_

#include <ToolFinder.h>

#include <sofa/core/ObjectFactory.h>
#include <sofa/core/componentmodel/topology/BaseMeshTopology.h>

namespace sofavrpn
{

namespace client
{

using namespace sofa;
using namespace sofa::defaulttype;
using namespace sofa::core::componentmodel::topology;

template<class Datatypes>
ToolFinder<Datatypes>::ToolFinder()
    : f_points(initData(&f_points, "points", "Incoming 3D Points"))
    , f_distances(initData(&f_distances, "distances", "Distances between each point"))
    , f_center(initData(&f_center, "center", "Tool's center"))
    , f_orientation(initData(&f_orientation, "orientation", "Tool's orientation"))
    , f_rigidCenter(initData(&f_rigidCenter, "rigidCenter", "Rigid center of the tool"))
{
    addInput(&f_points);
    addInput(&f_distances);

    addOutput(&f_center);
    addOutput(&f_orientation);
    addOutput(&f_rigidCenter);

    setDirtyValue();

}

template<class Datatypes>
ToolFinder<Datatypes>::~ToolFinder()
{
    // TODO Auto-generated destructor stub
}

template<class Datatypes>
void ToolFinder<Datatypes>::update()
{
    cleanDirty();

    typedef BaseMeshTopology::Edge Edge;
    typedef BaseMeshTopology::PointID PointID;

    sofa::helper::ReadAccessor< Data<VecCoord > > inPoints = f_points;
    sofa::helper::ReadAccessor< Data<helper::vector<double> > > realDistances = f_distances;
    //sofa::helper::WriteAccessor< Data<Coord > > centerPoint = f_center;
    Coord centerPoint = *f_center.beginEdit();
    //sofa::helper::WriteAccessor< Data<Quat > > orientation = f_orientation;
    Quat& orientation = *f_orientation.beginEdit();

    //sofa::helper::WriteAccessor< Data<RCoord > > rigidPoint = f_rigidCenter;
    RCoord& rigidPoint = *f_rigidCenter.beginEdit();

    //double & result_angle = *f_angle.beginEdit();
    //const double &distance = f_distance.getValue();

    if ( (inPoints.size() != realDistances.size()) || (inPoints.size () != 3))
    {
        serr << "Tool Finder : not enough given point or distance ..." << sendl;
        rigidPoint = RCoord();
        centerPoint = Coord();
        orientation = Quat();

        f_rigidCenter.endEdit();
        f_orientation.endEdit();
        return;
    }

    //Compute distances from incoming points
    helper::vector<double> computedDistances;
    helper::vector<Edge> computedEdges;
    for (unsigned int i=0 ; i<inPoints.size() ; i++)
    {
        Vec3f v = inPoints[((i+1)%realDistances.size())] - inPoints[i];
        computedDistances.push_back(v.norm());
        computedEdges.push_back(Edge(i, ((i+1)%realDistances.size())));
    }

    //Guess current mapping between incoming edges and real edges
    std::map<Edge, Edge> mapEdges;
    PointID p1, p2;
    double EPSILON = 0.0000001;

    bool first = true;
    for (unsigned int i=0 ; i<realDistances.size() ; i++)
    {
        double dist = realDistances[i];
        Edge currentRealEdge;
        currentRealEdge = Edge(i, ((i+1)%realDistances.size()) );

        unsigned int minIndex=0;
        double min = fabs(dist - computedDistances[minIndex]);
        std::cout << "? " << 0 << " " << min << std::endl;
        //search the nearest distances we have
        for (unsigned int j=1 ; j<computedDistances.size() ; j++)
        {
            //std::cout << "? " << j << " " << fabs(dist - computedDistances[j]) << std::endl;
            if(fabs(dist - computedDistances[j]) - min < EPSILON )
            {
                min = fabs(dist - computedDistances[j]);
                minIndex = j;
            }
        }
        mapEdges[currentRealEdge] = computedEdges[minIndex];
        //std::cout << currentRealEdge << " <-->" << computedEdges[minIndex]<< std::endl;
    }

    //Finally, get the real points from the mapping
    std::map<PointID, Coord> mapRealPoints;
    std::map<Edge, Edge>::const_iterator edgeIt = mapEdges.begin();

    Edge oldRealEdge = (*edgeIt).first;
    Edge oldIREdge = (*edgeIt).second;
    //edgeIt++;

    Edge currentRealEdge, currentIREdge;
    PointID commonRealPoint, commonIRPoint;
    unsigned int nb=0;
    for (edgeIt++ ; edgeIt != mapEdges.end(); edgeIt++)
        //for (edgeIt++ ; mapRealPoints.size() < inPoints.size(); edgeIt++)
    {
        currentRealEdge = (*edgeIt).first;
        currentIREdge = (*edgeIt).second;

        //We know that edges are consecutive
        if (currentRealEdge[0] == oldRealEdge[0] || currentRealEdge[0] == oldRealEdge[1])
            commonRealPoint = currentRealEdge[0];
        else
            commonRealPoint = currentRealEdge[1];

        if (currentIREdge[0] == oldIREdge[0] || currentIREdge[0] == oldIREdge[1])
            commonIRPoint = currentIREdge[0];
        else
            commonIRPoint = currentIREdge[1];

        oldRealEdge = currentRealEdge;
        oldIREdge = currentIREdge;

        mapRealPoints[commonRealPoint] = inPoints[commonIRPoint];
        //std::cout << commonRealPoint << " -> " << inPoints[commonIRPoint] << std::endl;
    }
    //get the last point
    edgeIt = mapEdges.begin();
    currentRealEdge = (*edgeIt).first;
    currentIREdge = (*edgeIt).second;

    //We know that edges are consecutive
    if (currentRealEdge[0] == oldRealEdge[0] || currentRealEdge[0] == oldRealEdge[1])
        commonRealPoint = currentRealEdge[0];
    else
        commonRealPoint = currentRealEdge[1];

    if (currentIREdge[0] == oldIREdge[0] || currentIREdge[0] == oldIREdge[1])
        commonIRPoint = currentIREdge[0];
    else
        commonIRPoint = currentIREdge[1];

    oldRealEdge = currentRealEdge;
    oldIREdge = currentIREdge;

    mapRealPoints[commonRealPoint] = inPoints[commonIRPoint];
    //std::cout << commonRealPoint << " -> " << inPoints[commonIRPoint] << std::endl;

    //not generic code ...
    //assume that point :
    //0 is left point from above view of the tool
    //1 is right ...
    //2 is top
    // 0-----1
    //    |
    //    |
    //    2

    //Compute orientation
    Coord leftPoint = mapRealPoints[0];
    Coord rightPoint = mapRealPoints[1];
    Coord topPoint = mapRealPoints[2];
    /*
    	Coord xAxis = topPoint - leftPoint;
    	xAxis.normalize();
    	Coord yAxis = (leftPoint - topPoint).cross(rightPoint - topPoint);
    	yAxis.normalize();
    	Coord zAxis = xAxis.cross(yAxis);
    	zAxis.normalize();

    	sofa::defaulttype::Quat q;
    	q = q.createQuaterFromFrame(xAxis, yAxis, zAxis);
    	q.normalize();
    	//Compute center
    	centerPoint = topPoint;
    	orientation = q;
    	rigidPoint.getCenter() = topPoint;
    	rigidPoint.getOrientation() = q;
    */
    Coord centerTool = (leftPoint + rightPoint + topPoint)/3.0;
    centerPoint = centerTool;
    rigidPoint.getCenter() = centerTool;

    Coord xAxis = topPoint - centerTool;
    xAxis.normalize();
    Coord yAxis = (leftPoint - topPoint).cross(rightPoint - topPoint);
    yAxis.normalize();
    Coord zAxis = xAxis.cross(yAxis);
    zAxis.normalize();

    //std::cout << "X: " << xAxis << std::endl;
    //std::cout << "Y: " << yAxis << std::endl;
    //std::cout << "Z: " << zAxis << std::endl;

    sofa::defaulttype::Quat q;
    q = q.createQuaterFromFrame(xAxis, yAxis, zAxis);
    q.normalize();

    orientation = q;
    rigidPoint.getOrientation() = q;

    f_rigidCenter.endEdit();
    f_orientation.endEdit();

    /*

    	//Get the Top Point
    	std::vector<double> lengths;
    	//get the (possible) top point
    	double length01 = (inPoints[1] - inPoints[0]).norm();
    	double length02 = (inPoints[2] - inPoints[0]).norm();
    	double length12 = (inPoints[2] - inPoints[1]).norm();
    	Coord topPoint, leftPoint, rightPoint;

    	if ( (length01 < length02) && (length01 < length12) )
    	{
    		topPoint = inPoints[2];
    		leftPoint = inPoints[0];
    		rightPoint = inPoints[1];
    		if (inPoints[1][0] < inPoints[0][0])
    		{
    			leftPoint = inPoints[1];
    			rightPoint = inPoints[0];
    		}
    	}
    	else
    		if ( (length02 < length01) && (length02 < length12) )
    		{
    			topPoint = inPoints[1];
    			leftPoint = inPoints[0];
    			rightPoint = inPoints[1];
    			if (inPoints[2][0] < inPoints[0][0])
    			{
    				leftPoint = inPoints[2];
    				rightPoint = inPoints[0];
    			}
    		}
    		else
    		{
    			topPoint = inPoints[0];
    			leftPoint = inPoints[1];
    			rightPoint = inPoints[2];
    			if (inPoints[2][0] < inPoints[1][0])
    			{
    				leftPoint = inPoints[2];
    				rightPoint = inPoints[1];
    			}
    		}

    	double topLeftLength = (leftPoint - topPoint).norm();
    	double leftRightLength = (leftPoint - rightPoint).norm();
    	//
    	double angle = atan( (leftRightLength - distance)/topLeftLength );

    	//
    	Coord xAxis = (leftPoint+rightPoint)*0.5 - topPoint;
    	xAxis.normalize();
    	Coord yAxis = (leftPoint - topPoint).cross(rightPoint - topPoint);
    	yAxis.normalize();
    	Coord zAxis = xAxis.cross(yAxis);
    	zAxis.normalize();

    	sofa::defaulttype::Quat q;
    	q = q.createQuaterFromFrame(xAxis, yAxis, zAxis);
    	topPointRigid.getCenter() = topPoint;
    	topPointRigid.getOrientation() = q;

    	leftPointW = leftPoint;
    	rightPointW = rightPoint;
    	angleW = angle;
    	angleArticulatedW.clear();
    	angleArticulatedW.push_back(0);
    	angleArticulatedW.push_back(angle);
    	angleArticulatedW.push_back(angle);

    	f_topPoint.endEdit();
    	f_leftPoint.endEdit();
    	f_rightPoint.endEdit();
    	f_angle.endEdit();
    	f_angleArticulated.endEdit();
    	*/
}

}

}

#endif //SOFAVRPNCLIENT_TOOLFINDER_INL_

<|endoftext|>


<commit_before>#include "MeterWnd.h"
#include <dwmapi.h>
#pragma comment(lib, "dwmapi.lib")
#include <VersionHelpers.h>
#include <sstream>

#include "Animation.h"
#include "AnimationFactory.h"
#include "MeterWndClone.h"

#define TIMER_HIDE 100
#define TIMER_ANIMIN 101
#define TIMER_ANIMOUT 102

MeterWnd::MeterWnd(LPCWSTR className, LPCWSTR title, HINSTANCE hInstance,
    AnimationTypes::HideAnimation hideAnim, int visibleDuration) :
_hInstance(hInstance),
_className(className),
_title(title),
_visibleDuration(visibleDuration),
_visible(false) {
    if (_hInstance == NULL) {
        _hInstance = (HINSTANCE) GetModuleHandle(NULL);
    }

    WNDCLASSEX wcex;
    wcex.cbSize = sizeof(WNDCLASSEX);
    wcex.style = 0;
    wcex.lpfnWndProc = &MeterWnd::StaticWndProc;
    wcex.cbClsExtra = 0;
    wcex.cbWndExtra = 0;
    wcex.hInstance = _hInstance;
    wcex.hIcon = LoadIcon(NULL, IDI_APPLICATION);
    wcex.hCursor = LoadCursor(NULL, IDC_ARROW);
    wcex.hbrBackground = (HBRUSH) (COLOR_WINDOW + 1);
    wcex.lpszMenuName = NULL;
    wcex.lpszClassName = _className;
    wcex.hIconSm = LoadIcon(NULL, IDI_APPLICATION);

    if (!RegisterClassEx(&wcex)) {
        /* throw exception */
    }

    _location.x = 0;
    _location.y = 0;
    _size.cx = 1;
    _size.cy = 1;
    _transparency = 255;

    _hWnd = CreateWindowEx(
        WS_EX_TOOLWINDOW |
        WS_EX_LAYERED |
        WS_EX_NOACTIVATE |
        WS_EX_TOPMOST |
        WS_EX_TRANSPARENT,
        _className, _title,
        WS_POPUP, CW_USEDEFAULT, CW_USEDEFAULT,
        _size.cx, _size.cy,
        NULL, NULL, _hInstance, this);

    if (_hWnd == NULL) {
        /* throw exception */
    }

    HideAnimation(hideAnim, 800);
}

MeterWnd::~MeterWnd() {
    delete _hideAnimation;
    delete _composite;
    DestroyWindow(_hWnd);

    for (MeterWndClone *clone : _clones) {
        delete clone;
    }
}

void MeterWnd::Update()
{
    CLOG(L"Updating meter window");
    using namespace Gdiplus;

    bool dirty = (_composite == NULL);

    for (Meter *meter : _meters) {
        if (meter->Dirty() == true) {
            dirty = true;
            break;
        }
    }

    if (dirty) {
        CLOG(L"Contents have changed; redrawing");

        if (_composite) {
            delete _composite;
        }

        Rect bgRect(0, 0, _background->GetWidth(), _background->GetHeight());
        _composite = _background->Clone(bgRect, PixelFormat32bppARGB);
        Graphics graphics(_composite);

        for (Meter *meter : _meters) {
            CLOG(L"Drawing meter:\n%s", meter->ToString().c_str());
            meter->Draw(_composite, &graphics);
        }
    }

    UpdateLayeredWnd();
    UpdateClones();
}

void MeterWnd::UpdateLayeredWnd() {
    BLENDFUNCTION bFunc;
    bFunc.AlphaFormat = AC_SRC_ALPHA;
    bFunc.BlendFlags = 0;
    bFunc.BlendOp = AC_SRC_OVER;
    bFunc.SourceConstantAlpha = _transparency;

    HDC screenDc = GetDC(GetDesktopWindow());
    HDC sourceDc = CreateCompatibleDC(screenDc);

    HBITMAP hBmp;
    _composite->GetHBITMAP(Gdiplus::Color(0, 0, 0, 0), &hBmp);
    HGDIOBJ hReplaced = SelectObject(sourceDc, hBmp);

    POINT pt = { 0, 0 };
    SIZE size = { _composite->GetWidth(), _composite->GetHeight() };

    UPDATELAYEREDWINDOWINFO lwInfo;
    lwInfo.cbSize = sizeof(UPDATELAYEREDWINDOWINFO);
    lwInfo.crKey = 0;
    lwInfo.dwFlags = ULW_ALPHA;
    lwInfo.hdcDst = screenDc;
    lwInfo.hdcSrc = sourceDc;
    lwInfo.pblend = &bFunc;
    lwInfo.pptDst = &_location;
    lwInfo.pptSrc = &pt;
    lwInfo.prcDirty = _dirtyRect;
    lwInfo.psize = &size;

    UpdateLayeredWindowIndirect(_hWnd, &lwInfo);

    SelectObject(sourceDc, hReplaced);
    DeleteDC(sourceDc);
    DeleteObject(hBmp);
    ReleaseDC(GetDesktopWindow(), screenDc);
}

void MeterWnd::UpdateTransparency() {
    BLENDFUNCTION bFunc;
    bFunc.AlphaFormat = AC_SRC_ALPHA;
    bFunc.BlendFlags = 0;
    bFunc.BlendOp = AC_SRC_OVER;
    bFunc.SourceConstantAlpha = _transparency;

    UPDATELAYEREDWINDOWINFO lwInfo;
    lwInfo.cbSize = sizeof(UPDATELAYEREDWINDOWINFO);
    lwInfo.crKey = 0;
    lwInfo.dwFlags = ULW_ALPHA;
    lwInfo.hdcDst = NULL;
    lwInfo.hdcSrc = NULL;
    lwInfo.pblend = &bFunc;
    lwInfo.pptDst = NULL;
    lwInfo.pptSrc = NULL;
    lwInfo.prcDirty = NULL;
    lwInfo.psize = NULL;

    UpdateLayeredWindowIndirect(_hWnd, &lwInfo);
}

void MeterWnd::ApplyGlass() {
    if (_glassMask == NULL) {
        return;
    }

    /* Disable for Windows 8+ */
    if (IsWindows8OrGreater()) {
        CLOG(L"Glass disabled: Windows 8+ client detected");
        return;
    }

    /* Disable for Windows XP */
    if (IsWindowsXPOrGreater() == true && IsWindowsVistaOrGreater() == false) {
        CLOG(L"Glass disabled: Windows XP client detected");
        return;
    }

    using namespace Gdiplus;
    ARGB searchArgb = 0xFF000000;

    unsigned int height = _glassMask->GetHeight();
    unsigned int width = _glassMask->GetWidth();

    Region reg;
    reg.MakeEmpty();
    bool match = false;

    /* One row of pixels is scanned at a time, so the height is 1. */
    Rect rec(0, 0, 0, 1);

    for (unsigned int y = 0; y < height; ++y) {
        for (unsigned int x = 0; x < width; ++x) {
            Color pixelColor;
            _glassMask->GetPixel(x, y, &pixelColor);
            ARGB pixelArgb = pixelColor.GetValue();

            if (searchArgb == pixelArgb && (x + 1 != width)) {
                if (match) {
                    continue;
                }

                match = true;
                rec.X = x;
                rec.Y = y;
            } else if (match) {
                /* Reached the end of a matching line */
                match = false;
                rec.Width = x - rec.X;
                reg.Union(rec);
            }
        }
    }

    DWM_BLURBEHIND blurBehind = { 0 };
    blurBehind.dwFlags = DWM_BB_ENABLE | DWM_BB_BLURREGION;
    blurBehind.fEnable = TRUE;

    Graphics g(_glassMask);
    HRGN hReg = reg.GetHRGN(&g);
    blurBehind.hRgnBlur = reg.GetHRGN(&g);

    HRESULT hr = DwmEnableBlurBehindWindow(_hWnd, &blurBehind);
    if (SUCCEEDED(hr)) {
        CLOG(L"Applied glass mask");
    }
}

//
// UpdateLocation()
//
// Uses MoveWindow() to set the window's X,Y coordinates and its size.
//
void MeterWnd::UpdateLocation() {
    MoveWindow(_hWnd, _location.x, _location.y, Width(), Height(), FALSE);
}

void MeterWnd::AddMeter(Meter *meter)
{
    _meters.push_back(meter);
}

void MeterWnd::MeterLevels(float value)
{
    for (Meter *meter : _meters) {
        meter->Value(value);
    }
}

void MeterWnd::HideAnimation(AnimationTypes::HideAnimation anim, int speed) {
    delete _hideAnimation;
    _hideAnimation = AnimationFactory::Create(anim, speed);
}

void MeterWnd::VisibleDuration(int duration) {
    _visibleDuration = duration;
}

void MeterWnd::BackgroundImage(Gdiplus::Bitmap *background) {
    _background = background;
    _size.cx = background->GetWidth();
    _size.cy = background->GetHeight();
}

void MeterWnd::GlassMask(Gdiplus::Bitmap *mask) {
    _glassMask = mask;
    ApplyGlass();
    ApplyClonesGlass();
}

void MeterWnd::Show(bool animate) {
    if (_visible == false) {
        UpdateLocation();
        ShowWindow(_hWnd, SW_SHOW);
        _visible = true;
    }

    ShowClones();

    if (_visibleDuration > 0) {
        SetTimer(_hWnd, TIMER_HIDE, _visibleDuration, NULL);
        KillTimer(_hWnd, TIMER_ANIMOUT);

        if (_hideAnimation) {
            _hideAnimation->Reset(this);
        }
    }
}

void MeterWnd::Hide(bool animate) {
    if (_visible == false) {
        return;
    }

    if (animate && _hideAnimation) {
        SetTimer(_hWnd, TIMER_ANIMOUT, _hideAnimation->UpdateInterval(), NULL);
    } else {
        ShowWindow(_hWnd, SW_HIDE);
        _visible = false;
        HideClones();
    }
}

void MeterWnd::AnimateOut() {
    bool animOver = _hideAnimation->Animate(this);
    if (animOver) {
        CLOG(L"Finished hide animation.");
        KillTimer(_hWnd, TIMER_ANIMOUT);
        ShowWindow(_hWnd, SW_HIDE);
        _visible = false;
        HideClones();
    }
}

int MeterWnd::X() const {
    return _location.x;
}

void MeterWnd::X(int x) {
    _location.x = x;
}

int MeterWnd::Y() const {
    return _location.y;
}

void MeterWnd::Y(int y) {
    _location.y = y;
}

int MeterWnd::Width() const {
    return _size.cx;
}

int MeterWnd::Height() const {
    return _size.cy;
}

void MeterWnd::Move(int x, int y) {
    _location.x = x;
    _location.y = y;
    UpdateLocation();
}

byte MeterWnd::Transparency() const {
    return _transparency;
}

void MeterWnd::Transparency(byte transparency) {
    _transparency = transparency;
    UpdateTransparency();
    UpdateClonesTransparency(transparency);
}

MeterWndClone *MeterWnd::Clone() {
    int numClones = _clones.size() + 1;
    std::wstringstream cloneClass;
    cloneClass << _className << L":" << numClones;
    std::wstringstream cloneTitle;
    cloneTitle << _title << L":" << numClones;

    MeterWndClone *mwc = new MeterWndClone(
        cloneClass.str().c_str(),
        cloneTitle.str().c_str(),
        _hInstance);

    ApplyClonesGlass();

    _clones.push_back(mwc);
    CLOG(L"Created meter window clone: %s/%s",
        cloneClass.str().c_str(), cloneTitle.str().c_str());
    return mwc;
}

std::vector<MeterWndClone *> MeterWnd::Clones() {
    return _clones;
}

void MeterWnd::UpdateClones() {
    if (_clones.size() > 0) {
        for (MeterWndClone *mwc : _clones) {
            mwc->Update(_composite);
        }
    }
}

void MeterWnd::UpdateClonesTransparency(byte transparency) {
    if (_clones.size() > 0) {
        for (MeterWndClone *mwc : _clones) {
            mwc->Transparency(transparency);
        }
    }
}

void MeterWnd::ShowClones() {
    if (_clones.size() > 0) {
        for (MeterWndClone *mwc : _clones) {
            mwc->Show();
        }
    }
}

void MeterWnd::HideClones() {
    if (_clones.size() > 0) {
        for (MeterWndClone *mwc : _clones) {
            mwc->Hide();
        }
    }
}

void MeterWnd::ApplyClonesGlass() {
    if (_glassMask == NULL) {
        return;
    }

    if (_clones.size() > 0) {
        for (MeterWndClone *mwc : _clones) {
            mwc->GlassMask(_glassMask);
        }
    }
}

LRESULT CALLBACK
MeterWnd::StaticWndProc(
        HWND hWnd, UINT message, WPARAM wParam, LPARAM lParam) {
    MeterWnd* wnd;

    if (message == WM_CREATE) {
        wnd = (MeterWnd*) ((LPCREATESTRUCT) lParam)->lpCreateParams;
        SetWindowLongPtr(hWnd, GWLP_USERDATA, (LONG_PTR) wnd);
    } else {
        wnd = (MeterWnd*) GetWindowLongPtr(hWnd, GWLP_USERDATA);
        if (!wnd) {
            return DefWindowProc(hWnd, message, wParam, lParam);
        }
    }

    return wnd->WndProc(message, wParam, lParam);
}

LRESULT MeterWnd::WndProc(UINT message, WPARAM wParam, LPARAM lParam) {
    if (message == WM_TIMER) {
        switch (wParam) {
        case TIMER_HIDE:
            CLOG(L"Display duration has elapsed. Hiding window.");
            Hide();
            KillTimer(_hWnd, TIMER_HIDE);
            break;

        case TIMER_ANIMIN:
            break;

        case TIMER_ANIMOUT:
            AnimateOut();
            break;
        }
    } else if (message == WM_DWMCOMPOSITIONCHANGED) {
        ApplyGlass();
    }

    return DefWindowProc(_hWnd, message, wParam, lParam);
}

<commit_msg>Individually update windows/glass for clones as they are created<commit_after>#include "MeterWnd.h"
#include <dwmapi.h>
#pragma comment(lib, "dwmapi.lib")
#include <VersionHelpers.h>
#include <sstream>

#include "Animation.h"
#include "AnimationFactory.h"
#include "MeterWndClone.h"

#define TIMER_HIDE 100
#define TIMER_ANIMIN 101
#define TIMER_ANIMOUT 102

MeterWnd::MeterWnd(LPCWSTR className, LPCWSTR title, HINSTANCE hInstance,
    AnimationTypes::HideAnimation hideAnim, int visibleDuration) :
_hInstance(hInstance),
_className(className),
_title(title),
_visibleDuration(visibleDuration),
_visible(false) {
    if (_hInstance == NULL) {
        _hInstance = (HINSTANCE) GetModuleHandle(NULL);
    }

    WNDCLASSEX wcex;
    wcex.cbSize = sizeof(WNDCLASSEX);
    wcex.style = 0;
    wcex.lpfnWndProc = &MeterWnd::StaticWndProc;
    wcex.cbClsExtra = 0;
    wcex.cbWndExtra = 0;
    wcex.hInstance = _hInstance;
    wcex.hIcon = LoadIcon(NULL, IDI_APPLICATION);
    wcex.hCursor = LoadCursor(NULL, IDC_ARROW);
    wcex.hbrBackground = (HBRUSH) (COLOR_WINDOW + 1);
    wcex.lpszMenuName = NULL;
    wcex.lpszClassName = _className;
    wcex.hIconSm = LoadIcon(NULL, IDI_APPLICATION);

    if (!RegisterClassEx(&wcex)) {
        /* throw exception */
    }

    _location.x = 0;
    _location.y = 0;
    _size.cx = 1;
    _size.cy = 1;
    _transparency = 255;

    _hWnd = CreateWindowEx(
        WS_EX_TOOLWINDOW |
        WS_EX_LAYERED |
        WS_EX_NOACTIVATE |
        WS_EX_TOPMOST |
        WS_EX_TRANSPARENT,
        _className, _title,
        WS_POPUP, CW_USEDEFAULT, CW_USEDEFAULT,
        _size.cx, _size.cy,
        NULL, NULL, _hInstance, this);

    if (_hWnd == NULL) {
        /* throw exception */
    }

    HideAnimation(hideAnim, 800);
}

MeterWnd::~MeterWnd() {
    delete _hideAnimation;
    delete _composite;
    DestroyWindow(_hWnd);

    for (MeterWndClone *clone : _clones) {
        delete clone;
    }
}

void MeterWnd::Update()
{
    CLOG(L"Updating meter window");
    using namespace Gdiplus;

    bool dirty = (_composite == NULL);

    for (Meter *meter : _meters) {
        if (meter->Dirty() == true) {
            dirty = true;
            break;
        }
    }

    if (dirty) {
        CLOG(L"Contents have changed; redrawing");

        if (_composite) {
            delete _composite;
        }

        Rect bgRect(0, 0, _background->GetWidth(), _background->GetHeight());
        _composite = _background->Clone(bgRect, PixelFormat32bppARGB);
        Graphics graphics(_composite);

        for (Meter *meter : _meters) {
            CLOG(L"Drawing meter:\n%s", meter->ToString().c_str());
            meter->Draw(_composite, &graphics);
        }
    }

    UpdateLayeredWnd();
    UpdateClones();
}

void MeterWnd::UpdateLayeredWnd() {
    BLENDFUNCTION bFunc;
    bFunc.AlphaFormat = AC_SRC_ALPHA;
    bFunc.BlendFlags = 0;
    bFunc.BlendOp = AC_SRC_OVER;
    bFunc.SourceConstantAlpha = _transparency;

    HDC screenDc = GetDC(GetDesktopWindow());
    HDC sourceDc = CreateCompatibleDC(screenDc);

    HBITMAP hBmp;
    _composite->GetHBITMAP(Gdiplus::Color(0, 0, 0, 0), &hBmp);
    HGDIOBJ hReplaced = SelectObject(sourceDc, hBmp);

    POINT pt = { 0, 0 };
    SIZE size = { _composite->GetWidth(), _composite->GetHeight() };

    UPDATELAYEREDWINDOWINFO lwInfo;
    lwInfo.cbSize = sizeof(UPDATELAYEREDWINDOWINFO);
    lwInfo.crKey = 0;
    lwInfo.dwFlags = ULW_ALPHA;
    lwInfo.hdcDst = screenDc;
    lwInfo.hdcSrc = sourceDc;
    lwInfo.pblend = &bFunc;
    lwInfo.pptDst = &_location;
    lwInfo.pptSrc = &pt;
    lwInfo.prcDirty = _dirtyRect;
    lwInfo.psize = &size;

    UpdateLayeredWindowIndirect(_hWnd, &lwInfo);

    SelectObject(sourceDc, hReplaced);
    DeleteDC(sourceDc);
    DeleteObject(hBmp);
    ReleaseDC(GetDesktopWindow(), screenDc);
}

void MeterWnd::UpdateTransparency() {
    BLENDFUNCTION bFunc;
    bFunc.AlphaFormat = AC_SRC_ALPHA;
    bFunc.BlendFlags = 0;
    bFunc.BlendOp = AC_SRC_OVER;
    bFunc.SourceConstantAlpha = _transparency;

    UPDATELAYEREDWINDOWINFO lwInfo;
    lwInfo.cbSize = sizeof(UPDATELAYEREDWINDOWINFO);
    lwInfo.crKey = 0;
    lwInfo.dwFlags = ULW_ALPHA;
    lwInfo.hdcDst = NULL;
    lwInfo.hdcSrc = NULL;
    lwInfo.pblend = &bFunc;
    lwInfo.pptDst = NULL;
    lwInfo.pptSrc = NULL;
    lwInfo.prcDirty = NULL;
    lwInfo.psize = NULL;

    UpdateLayeredWindowIndirect(_hWnd, &lwInfo);
}

void MeterWnd::ApplyGlass() {
    if (_glassMask == NULL) {
        return;
    }

    /* Disable for Windows 8+ */
    if (IsWindows8OrGreater()) {
        CLOG(L"Glass disabled: Windows 8+ client detected");
        return;
    }

    /* Disable for Windows XP */
    if (IsWindowsXPOrGreater() == true && IsWindowsVistaOrGreater() == false) {
        CLOG(L"Glass disabled: Windows XP client detected");
        return;
    }

    using namespace Gdiplus;
    ARGB searchArgb = 0xFF000000;

    unsigned int height = _glassMask->GetHeight();
    unsigned int width = _glassMask->GetWidth();

    Region reg;
    reg.MakeEmpty();
    bool match = false;

    /* One row of pixels is scanned at a time, so the height is 1. */
    Rect rec(0, 0, 0, 1);

    for (unsigned int y = 0; y < height; ++y) {
        for (unsigned int x = 0; x < width; ++x) {
            Color pixelColor;
            _glassMask->GetPixel(x, y, &pixelColor);
            ARGB pixelArgb = pixelColor.GetValue();

            if (searchArgb == pixelArgb && (x + 1 != width)) {
                if (match) {
                    continue;
                }

                match = true;
                rec.X = x;
                rec.Y = y;
            } else if (match) {
                /* Reached the end of a matching line */
                match = false;
                rec.Width = x - rec.X;
                reg.Union(rec);
            }
        }
    }

    DWM_BLURBEHIND blurBehind = { 0 };
    blurBehind.dwFlags = DWM_BB_ENABLE | DWM_BB_BLURREGION;
    blurBehind.fEnable = TRUE;

    Graphics g(_glassMask);
    HRGN hReg = reg.GetHRGN(&g);
    blurBehind.hRgnBlur = reg.GetHRGN(&g);

    HRESULT hr = DwmEnableBlurBehindWindow(_hWnd, &blurBehind);
    if (SUCCEEDED(hr)) {
        CLOG(L"Applied glass mask");
    }
}

//
// UpdateLocation()
//
// Uses MoveWindow() to set the window's X,Y coordinates and its size.
//
void MeterWnd::UpdateLocation() {
    MoveWindow(_hWnd, _location.x, _location.y, Width(), Height(), FALSE);
}

void MeterWnd::AddMeter(Meter *meter)
{
    _meters.push_back(meter);
}

void MeterWnd::MeterLevels(float value)
{
    for (Meter *meter : _meters) {
        meter->Value(value);
    }
}

void MeterWnd::HideAnimation(AnimationTypes::HideAnimation anim, int speed) {
    delete _hideAnimation;
    _hideAnimation = AnimationFactory::Create(anim, speed);
}

void MeterWnd::VisibleDuration(int duration) {
    _visibleDuration = duration;
}

void MeterWnd::BackgroundImage(Gdiplus::Bitmap *background) {
    _background = background;
    _size.cx = background->GetWidth();
    _size.cy = background->GetHeight();
}

void MeterWnd::GlassMask(Gdiplus::Bitmap *mask) {
    _glassMask = mask;
    ApplyGlass();
    ApplyClonesGlass();
}

void MeterWnd::Show(bool animate) {
    if (_visible == false) {
        UpdateLocation();
        ShowWindow(_hWnd, SW_SHOW);
        _visible = true;
    }

    ShowClones();

    if (_visibleDuration > 0) {
        SetTimer(_hWnd, TIMER_HIDE, _visibleDuration, NULL);
        KillTimer(_hWnd, TIMER_ANIMOUT);

        if (_hideAnimation) {
            _hideAnimation->Reset(this);
        }
    }
}

void MeterWnd::Hide(bool animate) {
    if (_visible == false) {
        return;
    }

    if (animate && _hideAnimation) {
        SetTimer(_hWnd, TIMER_ANIMOUT, _hideAnimation->UpdateInterval(), NULL);
    } else {
        ShowWindow(_hWnd, SW_HIDE);
        _visible = false;
        HideClones();
    }
}

void MeterWnd::AnimateOut() {
    bool animOver = _hideAnimation->Animate(this);
    if (animOver) {
        CLOG(L"Finished hide animation.");
        KillTimer(_hWnd, TIMER_ANIMOUT);
        ShowWindow(_hWnd, SW_HIDE);
        _visible = false;
        HideClones();
    }
}

int MeterWnd::X() const {
    return _location.x;
}

void MeterWnd::X(int x) {
    _location.x = x;
}

int MeterWnd::Y() const {
    return _location.y;
}

void MeterWnd::Y(int y) {
    _location.y = y;
}

int MeterWnd::Width() const {
    return _size.cx;
}

int MeterWnd::Height() const {
    return _size.cy;
}

void MeterWnd::Move(int x, int y) {
    _location.x = x;
    _location.y = y;
    UpdateLocation();
}

byte MeterWnd::Transparency() const {
    return _transparency;
}

void MeterWnd::Transparency(byte transparency) {
    _transparency = transparency;
    UpdateTransparency();
    UpdateClonesTransparency(transparency);
}

MeterWndClone *MeterWnd::Clone() {
    int numClones = _clones.size() + 1;
    std::wstringstream cloneClass;
    cloneClass << _className << L":" << numClones;
    std::wstringstream cloneTitle;
    cloneTitle << _title << L":" << numClones;

    MeterWndClone *mwc = new MeterWndClone(
        cloneClass.str().c_str(),
        cloneTitle.str().c_str(),
        _hInstance);

    if (_glassMask) {
        mwc->GlassMask(_glassMask);
    }
    mwc->Update(_composite);

    _clones.push_back(mwc);
    CLOG(L"Created meter window clone: %s/%s",
        cloneClass.str().c_str(), cloneTitle.str().c_str());
    return mwc;
}

std::vector<MeterWndClone *> MeterWnd::Clones() {
    return _clones;
}

void MeterWnd::UpdateClones() {
    if (_clones.size() > 0) {
        for (MeterWndClone *mwc : _clones) {
            mwc->Update(_composite);
        }
    }
}

void MeterWnd::UpdateClonesTransparency(byte transparency) {
    if (_clones.size() > 0) {
        for (MeterWndClone *mwc : _clones) {
            mwc->Transparency(transparency);
        }
    }
}

void MeterWnd::ShowClones() {
    if (_clones.size() > 0) {
        for (MeterWndClone *mwc : _clones) {
            mwc->Show();
        }
    }
}

void MeterWnd::HideClones() {
    if (_clones.size() > 0) {
        for (MeterWndClone *mwc : _clones) {
            mwc->Hide();
        }
    }
}

void MeterWnd::ApplyClonesGlass() {
    if (_glassMask == NULL) {
        return;
    }

    if (_clones.size() > 0) {
        for (MeterWndClone *mwc : _clones) {
            mwc->GlassMask(_glassMask);
        }
    }
}

LRESULT CALLBACK
MeterWnd::StaticWndProc(
        HWND hWnd, UINT message, WPARAM wParam, LPARAM lParam) {
    MeterWnd* wnd;

    if (message == WM_CREATE) {
        wnd = (MeterWnd*) ((LPCREATESTRUCT) lParam)->lpCreateParams;
        SetWindowLongPtr(hWnd, GWLP_USERDATA, (LONG_PTR) wnd);
    } else {
        wnd = (MeterWnd*) GetWindowLongPtr(hWnd, GWLP_USERDATA);
        if (!wnd) {
            return DefWindowProc(hWnd, message, wParam, lParam);
        }
    }

    return wnd->WndProc(message, wParam, lParam);
}

LRESULT MeterWnd::WndProc(UINT message, WPARAM wParam, LPARAM lParam) {
    if (message == WM_TIMER) {
        switch (wParam) {
        case TIMER_HIDE:
            CLOG(L"Display duration has elapsed. Hiding window.");
            Hide();
            KillTimer(_hWnd, TIMER_HIDE);
            break;

        case TIMER_ANIMIN:
            break;

        case TIMER_ANIMOUT:
            AnimateOut();
            break;
        }
    } else if (message == WM_DWMCOMPOSITIONCHANGED) {
        ApplyGlass();
    }

    return DefWindowProc(_hWnd, message, wParam, lParam);
}

<|endoftext|>


<commit_before>/*
Copyright (c) 2013 Microsoft Corporation. All rights reserved.
Released under Apache 2.0 license as described in the file LICENSE.

Author: Leonardo de Moura
*/
#include <algorithm>
#include <limits>
#include <unordered_map>
#include "util/list.h"
#include "util/flet.h"
#include "util/freset.h"
#include "util/buffer.h"
#include "util/interrupt.h"
#include "util/sexpr/options.h"
#include "kernel/normalizer.h"
#include "kernel/expr.h"
#include "kernel/context.h"
#include "kernel/environment.h"
#include "kernel/builtin.h"
#include "kernel/metavar.h"
#include "kernel/free_vars.h"
#include "kernel/instantiate.h"
#include "kernel/kernel_exception.h"

#ifndef LEAN_KERNEL_NORMALIZER_MAX_DEPTH
#define LEAN_KERNEL_NORMALIZER_MAX_DEPTH std::numeric_limits<unsigned>::max()
#endif

namespace lean {
static name g_kernel_normalizer_max_depth       {"kernel", "normalizer", "max_depth"};
RegisterUnsignedOption(g_kernel_normalizer_max_depth, LEAN_KERNEL_NORMALIZER_MAX_DEPTH, "(kernel) maximum recursion depth for expression normalizer");
unsigned get_normalizer_max_depth(options const & opts) {
    return opts.get_unsigned(g_kernel_normalizer_max_depth, LEAN_KERNEL_NORMALIZER_MAX_DEPTH);
}

typedef list<expr> value_stack;
value_stack extend(value_stack const & s, expr const & v) {
    lean_assert(!is_lambda(v) && !is_pi(v) && !is_metavar(v) && !is_let(v));
    return cons(v, s);
}

/**
   \brief Internal value used to store closures.
   This is a transient value that is only used during normalization.
*/
class closure : public value {
    expr         m_expr;
    context      m_ctx;
    value_stack  m_stack;
public:
    closure(expr const & e, context const & ctx, value_stack const & s):m_expr(e), m_ctx(ctx), m_stack(s) {}
    virtual ~closure() {}
    virtual expr get_type() const { lean_unreachable(); } // LCOV_EXCL_LINE
    virtual name get_name() const { return name("Closure"); }
    virtual void display(std::ostream & out) const {
        out << "(Closure " << m_expr << " [";
        bool first = true;
        for (auto v : m_stack) {
            if (first) first = false; else out << " ";
            out << v;
        }
        out << "])";
    }
    expr const & get_expr() const { return m_expr; }
    context const & get_context() const { return m_ctx; }
    value_stack const & get_stack() const { return m_stack; }
};

expr mk_closure(expr const & e, context const & ctx, value_stack const & s) { return mk_value(*(new closure(e, ctx, s))); }
bool is_closure(expr const & e) { return is_value(e) && dynamic_cast<closure const *>(&to_value(e)) != nullptr; }
closure const & to_closure(expr const & e) { lean_assert(is_closure(e));   return static_cast<closure const &>(to_value(e)); }

/** \brief Expression normalizer. */
class normalizer::imp {
    typedef std::unordered_map<expr, expr, expr_hash_alloc, expr_eqp> cache;

    ro_environment::weak_ref m_env;
    context                  m_ctx;
    cached_metavar_env       m_menv;
    cache                    m_cache;
    unsigned                 m_max_depth;
    unsigned                 m_depth;

    ro_environment env() const { return ro_environment(m_env); }

    expr instantiate(expr const & e, unsigned n, expr const * s) {
        return ::lean::instantiate(e, n, s, m_menv.to_some_menv());
    }

    expr add_lift(expr const & m, unsigned s, unsigned n) {
        return ::lean::add_lift(m, s, n, m_menv.to_some_menv());
    }

    expr lookup(value_stack const & s, unsigned i) {
        unsigned j = i;
        value_stack const * it1 = &s;
        while (*it1) {
            if (j == 0)
                return head(*it1);
            --j;
            it1 = &tail(*it1);
        }
        auto p = lookup_ext(m_ctx, j);
        context_entry const & entry = p.first;
        context const & entry_c     = p.second;
        if (entry.get_body()) {
            // Save the current context and cache
            freset<cache>    reset(m_cache);
            flet<context>    set(m_ctx, entry_c);
            unsigned k = m_ctx.size();
            return normalize(*(entry.get_body()), value_stack(), k);
        } else {
            return mk_var(entry_c.size());
        }
    }

    /** \brief Convert the value \c v back into an expression in a context that contains \c k binders. */
    expr reify(expr const & v, unsigned k) {
        auto f = [&](expr const & e, unsigned DEBUG_CODE(offset)) {
            lean_assert(offset == 0);
            lean_assert(!is_lambda(e) && !is_pi(e) && !is_metavar(e) && !is_let(e));
            if (is_var(e)) {
                // de Bruijn level --> de Bruijn index
                return mk_var(k - var_idx(e) - 1);
            } else if (is_closure(e)) {
                return reify_closure(to_closure(e), k);
            } else {
                return e;
            }
        };
        return replace_fn<decltype(f)>(f)(v);
    }

    /** \brief Return true iff the value_stack does affect the context of a metavariable */
    bool is_identity_stack(value_stack const & s, unsigned k) {
        unsigned i = 0;
        for (auto e : s) {
            if (!is_var(e) || k - var_idx(e) - 1 != i)
                return false;
            ++i;
        }
        return true;
    }

    /** \brief Convert the closure \c c into an expression in a context that contains \c k binders. */
    expr reify_closure(closure const & c, unsigned k) {
        expr const & e        = c.get_expr();
        context const & ctx   = c.get_context();
        value_stack const & s = c.get_stack();
        freset<cache>    reset(m_cache);
        flet<context>    set(m_ctx, ctx);
        if (is_abstraction(e)) {
            return update_abst(e, [&](expr const & d, expr const & b) {
                    expr new_d = reify(normalize(d, s, k), k);
                    m_cache.clear(); // make sure we do not reuse cached values from the previous call
                    expr new_b = reify(normalize(b, extend(s, mk_var(k)), k+1), k+1);
                    return mk_pair(new_d, new_b);
                });
        } else {
            lean_assert(is_metavar(e));
            if (is_identity_stack(s, k))
                return e; // nothing to be done
            local_context lctx = metavar_lctx(e);
            unsigned len_s     = length(s);
            unsigned len_ctx   = ctx.size();
            lean_assert(k >= len_ctx);
            expr r;
            if (k > len_ctx)
                r = add_lift(e, len_s, k - len_ctx);
            else
                r = e;
            buffer<expr> subst;
            for (auto v : s)
                subst.push_back(reify(v, k));
            std::reverse(subst.begin(), subst.end());
            return instantiate(r, subst.size(), subst.data());
        }
    }

    /** \brief Normalize the expression \c a in a context composed of stack \c s and \c k binders. */
    expr normalize(expr const & a, value_stack const & s, unsigned k) {
        flet<unsigned> l(m_depth, m_depth+1);
        check_system("normalizer");
        if (m_depth > m_max_depth)
            throw kernel_exception(env(), "normalizer maximum recursion depth exceeded");
        bool shared = false;
        if (is_shared(a)) {
            shared = true;
            auto it = m_cache.find(a);
            if (it != m_cache.end())
                return it->second;
        }

        expr r;
        switch (a.kind()) {
        case expr_kind::MetaVar: case expr_kind::Pi: case expr_kind::Lambda:
            r = mk_closure(a, m_ctx, s);
            break;
        case expr_kind::Var:
            r = lookup(s, var_idx(a));
            break;
        case expr_kind::Constant: {
            object const & obj = env()->get_object(const_name(a));
            if (obj.is_definition() && !obj.is_opaque()) {
                freset<cache> reset(m_cache);
                r = normalize(obj.get_value(), value_stack(), 0);
            } else {
                r = a;
            }
            break;
        }
        case expr_kind::Type: case expr_kind::Value:
            r = a;
            break;
        case expr_kind::App: {
            expr f      = normalize(arg(a, 0), s, k);
            unsigned i  = 1;
            unsigned n  = num_args(a);
            while (true) {
                if (is_closure(f) && is_lambda(to_closure(f).get_expr())) {
                    // beta reduction
                    expr const & fv = to_closure(f).get_expr();
                    {
                        freset<cache> reset(m_cache);
                        flet<context> set(m_ctx, to_closure(f).get_context());
                        value_stack new_s = extend(to_closure(f).get_stack(), normalize(arg(a, i), s, k));
                        f = normalize(abst_body(fv), new_s, k);
                    }
                    if (i == n - 1) {
                        r = f;
                        break;
                    }
                    i++;
                } else {
                    buffer<expr> new_args;
                    new_args.push_back(f);
                    for (; i < n; i++)
                        new_args.push_back(normalize(arg(a, i), s, k));
                    if (is_value(f) && !is_closure(f)) {
                        buffer<expr> reified_args;
                        for (auto arg : new_args) reified_args.push_back(reify(arg, k));
                        optional<expr> m = to_value(f).normalize(reified_args.size(), reified_args.data());
                        if (m) {
                            r = normalize(*m, s, k);
                            break;
                        }
                    }
                    r = mk_app(new_args);
                    break;
                }
            }
            break;
        }
        case expr_kind::Eq: {
            expr new_lhs = normalize(eq_lhs(a), s, k);
            expr new_rhs = normalize(eq_rhs(a), s, k);
            if (is_value(new_lhs) && is_value(new_rhs) && !is_closure(new_lhs) && !is_closure(new_rhs)) {
                r = mk_bool_value(new_lhs == new_rhs);
            } else {
                r = mk_eq(new_lhs, new_rhs);
            }
            break;
        }
        case expr_kind::Let: {
            expr v = normalize(let_value(a), s, k);
            {
                freset<cache> reset(m_cache);
                r = normalize(let_body(a), extend(s, v), k);
            }
            break;
        }}
        if (shared) {
            m_cache[a] = r;
        }
        return r;
    }

    void set_ctx(context const & ctx) {
        if (!is_eqp(ctx, m_ctx)) {
            m_ctx = ctx;
            m_cache.clear();
        }
    }

public:
    imp(ro_environment const & env, unsigned max_depth):
        m_env(env) {
        m_max_depth      = max_depth;
        m_depth          = 0;
    }

    expr operator()(expr const & e, context const & ctx, optional<metavar_env> const & menv) {
        set_ctx(ctx);
        if (m_menv.update(menv))
            m_cache.clear();
        unsigned k = m_ctx.size();
        return reify(normalize(e, value_stack(), k), k);
    }

    void clear() { m_ctx = context(); m_cache.clear(); m_menv.clear(); }
};

normalizer::normalizer(ro_environment const & env, unsigned max_depth):m_ptr(new imp(env, max_depth)) {}
normalizer::normalizer(ro_environment const & env):normalizer(env, std::numeric_limits<unsigned>::max()) {}
normalizer::normalizer(ro_environment const & env, options const & opts):normalizer(env, get_normalizer_max_depth(opts)) {}
normalizer::~normalizer() {}
expr normalizer::operator()(expr const & e, context const & ctx, optional<metavar_env> const & menv) { return (*m_ptr)(e, ctx, menv); }
expr normalizer::operator()(expr const & e, context const & ctx, metavar_env const & menv) { return operator()(e, ctx, some_menv(menv)); }
expr normalizer::operator()(expr const & e, context const & ctx) { return operator()(e, ctx, none_menv()); }
void normalizer::clear() { m_ptr->clear(); }

expr normalize(expr const & e, ro_environment const & env, context const & ctx) {
    return normalizer(env)(e, ctx);
}
}

/*
  Remark:

  Eta-reduction + Cumulativity + Set theoretic interpretation is unsound.
  Example:
     f : (Type 2) -> (Type 2)
     (fun (x : (Type 1)) (f x)) : (Type 1) -> (Type 2)
     The domains of these two terms are different. So, they must have different denotations.

     However, by eta-reduction, we have:
     (fun (x : (Type 1)) (f x)) == f

     For now, we will disable it.
     REMARK: we can workaround this problem by applying only when the domain of f is equal
     to the domain of the lambda abstraction.

  Cody Roux suggested we use Eta-expanded normal forms.

  Remark: The source code for eta-reduction can be found in the commit 519a290f320c6a
*/
<commit_msg>feat(kernel/normalizer): avoid unnecessary creation of closures for n-ary functions<commit_after>/*
Copyright (c) 2013 Microsoft Corporation. All rights reserved.
Released under Apache 2.0 license as described in the file LICENSE.

Author: Leonardo de Moura
*/
#include <algorithm>
#include <limits>
#include <unordered_map>
#include "util/list.h"
#include "util/flet.h"
#include "util/freset.h"
#include "util/buffer.h"
#include "util/interrupt.h"
#include "util/sexpr/options.h"
#include "kernel/normalizer.h"
#include "kernel/expr.h"
#include "kernel/context.h"
#include "kernel/environment.h"
#include "kernel/builtin.h"
#include "kernel/metavar.h"
#include "kernel/free_vars.h"
#include "kernel/instantiate.h"
#include "kernel/kernel_exception.h"

#ifndef LEAN_KERNEL_NORMALIZER_MAX_DEPTH
#define LEAN_KERNEL_NORMALIZER_MAX_DEPTH std::numeric_limits<unsigned>::max()
#endif

namespace lean {
static name g_kernel_normalizer_max_depth       {"kernel", "normalizer", "max_depth"};
RegisterUnsignedOption(g_kernel_normalizer_max_depth, LEAN_KERNEL_NORMALIZER_MAX_DEPTH, "(kernel) maximum recursion depth for expression normalizer");
unsigned get_normalizer_max_depth(options const & opts) {
    return opts.get_unsigned(g_kernel_normalizer_max_depth, LEAN_KERNEL_NORMALIZER_MAX_DEPTH);
}

typedef list<expr> value_stack;
value_stack extend(value_stack const & s, expr const & v) {
    lean_assert(!is_lambda(v) && !is_pi(v) && !is_metavar(v) && !is_let(v));
    return cons(v, s);
}

/**
   \brief Internal value used to store closures.
   This is a transient value that is only used during normalization.
*/
class closure : public value {
    expr         m_expr;
    context      m_ctx;
    value_stack  m_stack;
public:
    closure(expr const & e, context const & ctx, value_stack const & s):m_expr(e), m_ctx(ctx), m_stack(s) {}
    virtual ~closure() {}
    virtual expr get_type() const { lean_unreachable(); } // LCOV_EXCL_LINE
    virtual name get_name() const { return name("Closure"); }
    virtual void display(std::ostream & out) const {
        out << "(Closure " << m_expr << " [";
        bool first = true;
        for (auto v : m_stack) {
            if (first) first = false; else out << " ";
            out << v;
        }
        out << "])";
    }
    expr const & get_expr() const { return m_expr; }
    context const & get_context() const { return m_ctx; }
    value_stack const & get_stack() const { return m_stack; }
};

expr mk_closure(expr const & e, context const & ctx, value_stack const & s) { return mk_value(*(new closure(e, ctx, s))); }
bool is_closure(expr const & e) { return is_value(e) && dynamic_cast<closure const *>(&to_value(e)) != nullptr; }
closure const & to_closure(expr const & e) { lean_assert(is_closure(e));   return static_cast<closure const &>(to_value(e)); }

/** \brief Expression normalizer. */
class normalizer::imp {
    typedef std::unordered_map<expr, expr, expr_hash_alloc, expr_eqp> cache;

    ro_environment::weak_ref m_env;
    context                  m_ctx;
    cached_metavar_env       m_menv;
    cache                    m_cache;
    unsigned                 m_max_depth;
    unsigned                 m_depth;

    ro_environment env() const { return ro_environment(m_env); }

    expr instantiate(expr const & e, unsigned n, expr const * s) {
        return ::lean::instantiate(e, n, s, m_menv.to_some_menv());
    }

    expr add_lift(expr const & m, unsigned s, unsigned n) {
        return ::lean::add_lift(m, s, n, m_menv.to_some_menv());
    }

    expr lookup(value_stack const & s, unsigned i) {
        unsigned j = i;
        value_stack const * it1 = &s;
        while (*it1) {
            if (j == 0)
                return head(*it1);
            --j;
            it1 = &tail(*it1);
        }
        auto p = lookup_ext(m_ctx, j);
        context_entry const & entry = p.first;
        context const & entry_c     = p.second;
        if (entry.get_body()) {
            // Save the current context and cache
            freset<cache>    reset(m_cache);
            flet<context>    set(m_ctx, entry_c);
            unsigned k = m_ctx.size();
            return normalize(*(entry.get_body()), value_stack(), k);
        } else {
            return mk_var(entry_c.size());
        }
    }

    /** \brief Convert the value \c v back into an expression in a context that contains \c k binders. */
    expr reify(expr const & v, unsigned k) {
        auto f = [&](expr const & e, unsigned DEBUG_CODE(offset)) {
            lean_assert(offset == 0);
            lean_assert(!is_lambda(e) && !is_pi(e) && !is_metavar(e) && !is_let(e));
            if (is_var(e)) {
                // de Bruijn level --> de Bruijn index
                return mk_var(k - var_idx(e) - 1);
            } else if (is_closure(e)) {
                return reify_closure(to_closure(e), k);
            } else {
                return e;
            }
        };
        return replace_fn<decltype(f)>(f)(v);
    }

    /** \brief Return true iff the value_stack does affect the context of a metavariable */
    bool is_identity_stack(value_stack const & s, unsigned k) {
        unsigned i = 0;
        for (auto e : s) {
            if (!is_var(e) || k - var_idx(e) - 1 != i)
                return false;
            ++i;
        }
        return true;
    }

    /** \brief Convert the closure \c c into an expression in a context that contains \c k binders. */
    expr reify_closure(closure const & c, unsigned k) {
        expr const & e        = c.get_expr();
        context const & ctx   = c.get_context();
        value_stack const & s = c.get_stack();
        freset<cache>    reset(m_cache);
        flet<context>    set(m_ctx, ctx);
        if (is_abstraction(e)) {
            return update_abst(e, [&](expr const & d, expr const & b) {
                    expr new_d = reify(normalize(d, s, k), k);
                    m_cache.clear(); // make sure we do not reuse cached values from the previous call
                    expr new_b = reify(normalize(b, extend(s, mk_var(k)), k+1), k+1);
                    return mk_pair(new_d, new_b);
                });
        } else {
            lean_assert(is_metavar(e));
            if (is_identity_stack(s, k))
                return e; // nothing to be done
            local_context lctx = metavar_lctx(e);
            unsigned len_s     = length(s);
            unsigned len_ctx   = ctx.size();
            lean_assert(k >= len_ctx);
            expr r;
            if (k > len_ctx)
                r = add_lift(e, len_s, k - len_ctx);
            else
                r = e;
            buffer<expr> subst;
            for (auto v : s)
                subst.push_back(reify(v, k));
            std::reverse(subst.begin(), subst.end());
            return instantiate(r, subst.size(), subst.data());
        }
    }

    /** \brief Normalize the expression \c a in a context composed of stack \c s and \c k binders. */
    expr normalize(expr const & a, value_stack const & s, unsigned k) {
        flet<unsigned> l(m_depth, m_depth+1);
        check_system("normalizer");
        if (m_depth > m_max_depth)
            throw kernel_exception(env(), "normalizer maximum recursion depth exceeded");
        bool shared = false;
        if (is_shared(a)) {
            shared = true;
            auto it = m_cache.find(a);
            if (it != m_cache.end())
                return it->second;
        }

        expr r;
        switch (a.kind()) {
        case expr_kind::MetaVar: case expr_kind::Pi: case expr_kind::Lambda:
            r = mk_closure(a, m_ctx, s);
            break;
        case expr_kind::Var:
            r = lookup(s, var_idx(a));
            break;
        case expr_kind::Constant: {
            object const & obj = env()->get_object(const_name(a));
            if (obj.is_definition() && !obj.is_opaque()) {
                freset<cache> reset(m_cache);
                r = normalize(obj.get_value(), value_stack(), 0);
            } else {
                r = a;
            }
            break;
        }
        case expr_kind::Type: case expr_kind::Value:
            r = a;
            break;
        case expr_kind::App: {
            expr f      = normalize(arg(a, 0), s, k);
            unsigned i  = 1;
            unsigned n  = num_args(a);
            while (true) {
                if (is_closure(f) && is_lambda(to_closure(f).get_expr())) {
                    // beta reduction
                    expr fv = to_closure(f).get_expr();
                    value_stack new_s = to_closure(f).get_stack();
                    while (is_lambda(fv) && i < n) {
                        new_s = extend(new_s, normalize(arg(a, i), s, k));
                        i++;
                        fv = abst_body(fv);
                    }
                    {
                        freset<cache> reset(m_cache);
                        flet<context> set(m_ctx, to_closure(f).get_context());
                        f = normalize(fv, new_s, k);
                    }
                    if (i == n) {
                        r = f;
                        break;
                    }
                } else {
                    buffer<expr> new_args;
                    new_args.push_back(f);
                    for (; i < n; i++)
                        new_args.push_back(normalize(arg(a, i), s, k));
                    if (is_value(f) && !is_closure(f)) {
                        buffer<expr> reified_args;
                        for (auto arg : new_args) reified_args.push_back(reify(arg, k));
                        optional<expr> m = to_value(f).normalize(reified_args.size(), reified_args.data());
                        if (m) {
                            r = normalize(*m, s, k);
                            break;
                        }
                    }
                    r = mk_app(new_args);
                    break;
                }
            }
            break;
        }
        case expr_kind::Eq: {
            expr new_lhs = normalize(eq_lhs(a), s, k);
            expr new_rhs = normalize(eq_rhs(a), s, k);
            if (is_value(new_lhs) && is_value(new_rhs) && !is_closure(new_lhs) && !is_closure(new_rhs)) {
                r = mk_bool_value(new_lhs == new_rhs);
            } else {
                r = mk_eq(new_lhs, new_rhs);
            }
            break;
        }
        case expr_kind::Let: {
            expr v = normalize(let_value(a), s, k);
            {
                freset<cache> reset(m_cache);
                r = normalize(let_body(a), extend(s, v), k);
            }
            break;
        }}
        if (shared) {
            m_cache[a] = r;
        }
        return r;
    }

    void set_ctx(context const & ctx) {
        if (!is_eqp(ctx, m_ctx)) {
            m_ctx = ctx;
            m_cache.clear();
        }
    }

public:
    imp(ro_environment const & env, unsigned max_depth):
        m_env(env) {
        m_max_depth      = max_depth;
        m_depth          = 0;
    }

    expr operator()(expr const & e, context const & ctx, optional<metavar_env> const & menv) {
        set_ctx(ctx);
        if (m_menv.update(menv))
            m_cache.clear();
        unsigned k = m_ctx.size();
        return reify(normalize(e, value_stack(), k), k);
    }

    void clear() { m_ctx = context(); m_cache.clear(); m_menv.clear(); }
};

normalizer::normalizer(ro_environment const & env, unsigned max_depth):m_ptr(new imp(env, max_depth)) {}
normalizer::normalizer(ro_environment const & env):normalizer(env, std::numeric_limits<unsigned>::max()) {}
normalizer::normalizer(ro_environment const & env, options const & opts):normalizer(env, get_normalizer_max_depth(opts)) {}
normalizer::~normalizer() {}
expr normalizer::operator()(expr const & e, context const & ctx, optional<metavar_env> const & menv) { return (*m_ptr)(e, ctx, menv); }
expr normalizer::operator()(expr const & e, context const & ctx, metavar_env const & menv) { return operator()(e, ctx, some_menv(menv)); }
expr normalizer::operator()(expr const & e, context const & ctx) { return operator()(e, ctx, none_menv()); }
void normalizer::clear() { m_ptr->clear(); }

expr normalize(expr const & e, ro_environment const & env, context const & ctx) {
    return normalizer(env)(e, ctx);
}
}

/*
  Remark:

  Eta-reduction + Cumulativity + Set theoretic interpretation is unsound.
  Example:
     f : (Type 2) -> (Type 2)
     (fun (x : (Type 1)) (f x)) : (Type 1) -> (Type 2)
     The domains of these two terms are different. So, they must have different denotations.

     However, by eta-reduction, we have:
     (fun (x : (Type 1)) (f x)) == f

     For now, we will disable it.
     REMARK: we can workaround this problem by applying only when the domain of f is equal
     to the domain of the lambda abstraction.

  Cody Roux suggested we use Eta-expanded normal forms.

  Remark: The source code for eta-reduction can be found in the commit 519a290f320c6a
*/
<|endoftext|>


<commit_before>

#include <iostream>
#include <fstream>
#include <unistd.h>
#include <getopt.h>

#include "dogbot/DogBotAPI.hh"
#include "dogbot/Util.hh"
#include "dogbot/LegController.hh"
#include "cxxopts.hpp"

// This provides a network interface for controlling the servos via ZMQ.

int main(int argc,char **argv)
{
  std::string devFilename = "local";
  std::string configFile = DogBotN::DogBotAPIC::DefaultConfigFile();
  std::string jointName = "front_right_knee";
  std::string firmwareFile;
  std::string loadPoseFile;

  auto logger = spdlog::stdout_logger_mt("console");
  bool dumpPose = false;
  bool cycle = false;
  float torque = 1.0;
  float range = 45.0;
  float angle = 0;
  int delay = 5000;
  bool useVirtualJoint = false;
  try
  {
    cxxopts::Options options(argv[0], "DogBot hardware manager");
    options
      .positional_help("[optional args]")
      .show_positional_help();

    options.add_options()
      ("c,config", "Configuration file", cxxopts::value<std::string>(configFile))
      ("d,device", "Device to use from communication. Typically 'local' for local server or 'usb' for direct connection ", cxxopts::value<std::string>(devFilename))
      ("t,torque","Torque to use", cxxopts::value<float>(torque))
      ("j,joint","Joint name", cxxopts::value<std::string>(jointName))
      ("r,range","Motion range", cxxopts::value<float>(range))
      ("a,angle","Centre angle", cxxopts::value<float>(angle))
      ("p,pose","Joint name", cxxopts::value<bool>(dumpPose))
      ("l,load","Load pose", cxxopts::value<std::string>(loadPoseFile))
      ("g,cycle","Cycle up and down",cxxopts::value<bool>(cycle))
      ("v,virtual-joint","Use virtual joint",cxxopts::value<bool>(useVirtualJoint))
      ("o,delay","Delay in cycle, default is 5000",cxxopts::value<int>(delay))
      ("h,help", "Print help")
    ;

    auto result = options.parse(argc, argv);

    if (result.count("help"))
    {
      std::cout << options.help({""}) << std::endl;
      exit(0);
    }

  } catch (const cxxopts::OptionException& e)
  {
    std::cout << "error parsing options: " << e.what() << std::endl;
    exit(1);
  }

  logger->info("Dumping leg coordinates ");
  logger->info("Using config file: '{}'",configFile);
  logger->info("Using communication type: '{}'",devFilename);

  std::shared_ptr<DogBotN::DogBotAPIC> dogbot = std::make_shared<DogBotN::DogBotAPIC>(
      devFilename,
      configFile,
      logger
      );

  // Wait for poses to update and things to settle.
  sleep(1);

  if(dumpPose) {
    std::vector<std::shared_ptr<DogBotN::ServoC> > list = dogbot->ListServos();

    Json::Value poseJSON;
    for(auto &a : list) {
      if(!a || a->Name() == "spare")
        continue;
      poseJSON[a->Name()] =  DogBotN::Rad2Deg(a->Position());
    }
    std::cout << poseJSON << std::endl;
    return 1;
  }

  if(!loadPoseFile.empty()) {

    std::ifstream inFile(loadPoseFile);
    if(!inFile) {
      logger->error("Failed to load {} ",loadPoseFile);
      return 1;
    }
    Json::Value poseJSON;
    inFile >> poseJSON;
    std::vector<std::shared_ptr<DogBotN::JointC> > jnts;
    std::vector<float> position;
    for(auto iter = poseJSON.begin();iter != poseJSON.end();++iter) {
      std::shared_ptr<DogBotN::JointC> jnt = dogbot->GetJointByName(iter.key().asString());
      if(!jnt) {
        logger->error("Failed to find joint {} ",iter.key().asString());
        return 1;
      }
      jnts.push_back(jnt);
      position.push_back(iter->asFloat());
    }

    for(int i = 0;i < jnts.size();i++) {
      jnts[i]->DemandPosition(DogBotN::Deg2Rad(position[i]),torque);
    }
    logger->info("Move done.");
    return 0;
  }

  std::shared_ptr<DogBotN::JointC> joint = dogbot->GetJointByName(jointName);
  if(!joint) {
    logger->error("Failed to find joint {} ",jointName);
    return 1;
  }

  if(cycle) {

    // Lift the speed limit a bit
    dogbot->Connection()->SetParam(0,CPI_VelocityLimit,(float) 300.0);

    std::shared_ptr<DogBotN::LegControllerC> legs[4];
    legs[3] = std::make_shared<DogBotN::LegControllerC>(dogbot,"front_right",useVirtualJoint);
    legs[2] = std::make_shared<DogBotN::LegControllerC>(dogbot,"front_left",useVirtualJoint);
    legs[1] = std::make_shared<DogBotN::LegControllerC>(dogbot,"back_right",useVirtualJoint);
    legs[0] = std::make_shared<DogBotN::LegControllerC>(dogbot,"back_left",useVirtualJoint);

    while(1) {
      for(int i = 0;i < 360;i++) {
        // 0.35 to 0.7
        float z = cos(DogBotN::Deg2Rad(i)) * 0.17 +0.525;

        for(int i = 0;i < 4;i++) {
          legs[i]->Goto(0,0,z,torque);
        }
        usleep(delay);
      }
    }

  }

  enum DogBotN::JointMoveStatusT moveStatus;
  while(true)
  {
    if((moveStatus = joint->MoveWait(DogBotN::Deg2Rad(angle-range/2),torque)) != DogBotN::JMS_Done) {
      logger->warn("Move failed. {} ",(int) moveStatus);
    }
    if((moveStatus =joint->MoveWait(DogBotN::Deg2Rad(angle+range/2),torque))!= DogBotN::JMS_Done) {
      logger->warn("Move failed. {} ",(int) moveStatus);
    }
  }

  return 0;
}
<commit_msg>Fix for new coordinate system<commit_after>

#include <iostream>
#include <fstream>
#include <unistd.h>
#include <getopt.h>

#include "dogbot/DogBotAPI.hh"
#include "dogbot/Util.hh"
#include "dogbot/LegController.hh"
#include "cxxopts.hpp"

// This provides a network interface for controlling the servos via ZMQ.

int main(int argc,char **argv)
{
  std::string devFilename = "local";
  std::string configFile = DogBotN::DogBotAPIC::DefaultConfigFile();
  std::string jointName = "front_right_knee";
  std::string firmwareFile;
  std::string loadPoseFile;

  auto logger = spdlog::stdout_logger_mt("console");
  bool dumpPose = false;
  bool cycle = false;
  float torque = 1.0;
  float range = 45.0;
  float angle = 0;
  int delay = 5000;
  bool useVirtualJoint = false;
  try
  {
    cxxopts::Options options(argv[0], "DogBot hardware manager");
    options
      .positional_help("[optional args]")
      .show_positional_help();

    options.add_options()
      ("c,config", "Configuration file", cxxopts::value<std::string>(configFile))
      ("d,device", "Device to use from communication. Typically 'local' for local server or 'usb' for direct connection ", cxxopts::value<std::string>(devFilename))
      ("t,torque","Torque to use", cxxopts::value<float>(torque))
      ("j,joint","Joint name", cxxopts::value<std::string>(jointName))
      ("r,range","Motion range", cxxopts::value<float>(range))
      ("a,angle","Centre angle", cxxopts::value<float>(angle))
      ("p,pose","Joint name", cxxopts::value<bool>(dumpPose))
      ("l,load","Load pose", cxxopts::value<std::string>(loadPoseFile))
      ("g,cycle","Cycle up and down",cxxopts::value<bool>(cycle))
      ("v,virtual-joint","Use virtual joint",cxxopts::value<bool>(useVirtualJoint))
      ("o,delay","Delay in cycle, default is 5000",cxxopts::value<int>(delay))
      ("h,help", "Print help")
    ;

    auto result = options.parse(argc, argv);

    if (result.count("help"))
    {
      std::cout << options.help({""}) << std::endl;
      exit(0);
    }

  } catch (const cxxopts::OptionException& e)
  {
    std::cout << "error parsing options: " << e.what() << std::endl;
    exit(1);
  }

  logger->info("Dumping leg coordinates ");
  logger->info("Using config file: '{}'",configFile);
  logger->info("Using communication type: '{}'",devFilename);

  std::shared_ptr<DogBotN::DogBotAPIC> dogbot = std::make_shared<DogBotN::DogBotAPIC>(
      devFilename,
      configFile,
      logger
      );

  // Wait for poses to update and things to settle.
  sleep(1);

  if(dumpPose) {
    std::vector<std::shared_ptr<DogBotN::ServoC> > list = dogbot->ListServos();

    Json::Value poseJSON;
    for(auto &a : list) {
      if(!a || a->Name() == "spare")
        continue;
      poseJSON[a->Name()] =  DogBotN::Rad2Deg(a->Position());
    }
    std::cout << poseJSON << std::endl;
    return 1;
  }

  if(!loadPoseFile.empty()) {

    std::ifstream inFile(loadPoseFile);
    if(!inFile) {
      logger->error("Failed to load {} ",loadPoseFile);
      return 1;
    }
    Json::Value poseJSON;
    inFile >> poseJSON;
    std::vector<std::shared_ptr<DogBotN::JointC> > jnts;
    std::vector<float> position;
    for(auto iter = poseJSON.begin();iter != poseJSON.end();++iter) {
      std::shared_ptr<DogBotN::JointC> jnt = dogbot->GetJointByName(iter.key().asString());
      if(!jnt) {
        logger->error("Failed to find joint {} ",iter.key().asString());
        return 1;
      }
      jnts.push_back(jnt);
      position.push_back(iter->asFloat());
    }

    for(int i = 0;i < jnts.size();i++) {
      jnts[i]->DemandPosition(DogBotN::Deg2Rad(position[i]),torque);
    }
    logger->info("Move done.");
    return 0;
  }

  std::shared_ptr<DogBotN::JointC> joint = dogbot->GetJointByName(jointName);
  if(!joint) {
    logger->error("Failed to find joint {} ",jointName);
    return 1;
  }

  if(cycle) {

    // Lift the speed limit a bit
    dogbot->Connection()->SetParam(0,CPI_VelocityLimit,(float) 300.0);

    std::shared_ptr<DogBotN::LegControllerC> legs[4];
    legs[3] = std::make_shared<DogBotN::LegControllerC>(dogbot,"front_right",useVirtualJoint);
    legs[2] = std::make_shared<DogBotN::LegControllerC>(dogbot,"front_left",useVirtualJoint);
    legs[1] = std::make_shared<DogBotN::LegControllerC>(dogbot,"back_right",useVirtualJoint);
    legs[0] = std::make_shared<DogBotN::LegControllerC>(dogbot,"back_left",useVirtualJoint);

    while(1) {
      for(int i = 0;i < 360;i++) {
        // 0.35 to 0.7
        float z = cos(DogBotN::Deg2Rad(i)) * 0.17 +0.525;

        for(int i = 0;i < 4;i++) {
          legs[i]->Goto(0,0,-z,torque);
        }
        usleep(delay);
      }
    }

  }

  enum DogBotN::JointMoveStatusT moveStatus;
  while(true)
  {
    if((moveStatus = joint->MoveWait(DogBotN::Deg2Rad(angle-range/2),torque)) != DogBotN::JMS_Done) {
      logger->warn("Move failed. {} ",(int) moveStatus);
    }
    if((moveStatus =joint->MoveWait(DogBotN::Deg2Rad(angle+range/2),torque))!= DogBotN::JMS_Done) {
      logger->warn("Move failed. {} ",(int) moveStatus);
    }
  }

  return 0;
}
<|endoftext|>