text: /*
   *  ConfigEnvironment.cpp
   *  hog2
   *
   *  Created by Nathan Sturtevant on 1/9/09.
   *  Copyright 2009 NS Software. All rights reserved.
   *
   */
  
  #include "ConfigEnvironment.h"
  #include "FPUtil.h"
  #include <iostream>
  #include <math.h>
  
  ConfigEnvironment::ConfigEnvironment()
  {
  	goal_stored = false;
  }
  
  ConfigEnvironment::~ConfigEnvironment()
  {
  }
  
  void ConfigEnvironment::GetSuccessors(const recVec &nodeID, std::vector<recVec> &neighbors) const
  {
  	//[(-0.300000, -0.320000) to (0.000000, 0.000000)] does not cross [(-0.320000, -0.300000) to (0.320000, -0.300000)]
  //	line2d a(recVec(-0.300000, -0.320000, 0), recVec(0, -0.4, 0));
  //	line2d b(recVec(-0.320000, -0.300000, 0), recVec(0.320000, -0.300000, 0));
  //	line2d c(recVec(-0.300000, -0.320000, 0), recVec(-0.300000, 0.320000, 0));
  //	if (a.crosses(b))
  //		printf("1 They Cross!\n");
  //	else
  //		printf("1 No crossing here!\n");
  //	if (b.crosses(a))
  //		printf("2 They Cross!\n");
  //	else
  //		printf("2 No crossing here!\n");
  //	if (a.crosses(c))
  //		printf("3 They Cross!\n");
  //	else
  //		printf("3 No crossing here!\n");
  //	if (c.crosses(a))
  //		printf("4 They Cross!\n");
  //	else
  //		printf("4 No crossing here!\n");
  
  	neighbors.resize(0);
  	neighbors.push_back(goal);
  	for (unsigned int x = 0; x < obstacles.size(); x++)
  	{
  		neighbors.push_back(obstacles[x].start);
  		neighbors.push_back(obstacles[x].end);
  	}
  	for (unsigned int x = 0; x < neighbors.size(); x++)
  	{
  		if (!Legal(nodeID, neighbors[x]))
  		{
  			neighbors[x] = neighbors.back();
  			neighbors.pop_back();
  			x--;
  		}
  	}
  //	printf("Returning:\n");
  //	for (unsigned int x = 0; x < neighbors.size(); x++)
  //		std::cout << neighbors[x] << " ";
  //	std::cout << std::endl;
  }
  
  void ConfigEnvironment::GetActions(const recVec &nodeID, std::vector<line2d> &actions) const
  {
  	actions.resize(0);
  	actions.push_back(line2d(nodeID, goal));
  	for (unsigned int x = 0; x < obstacles.size(); x++)
  	{
  		actions.push_back(line2d(nodeID, obstacles[x].start));
  		actions.push_back(line2d(nodeID, obstacles[x].end));
  	}
  	for (unsigned int x = 0; x < actions.size(); x++)
  	{
  		if (!Legal(actions[x].start, actions[x].end))
  		{
  			actions[x] = actions.back();
  			actions.pop_back();
  			x--;
  		}
  	}
  }
  
  bool ConfigEnvironment::Legal(const recVec &a, const recVec &b) const
  {
  	line2d l(a, b);
  	for (unsigned int x = 0; x < obstacles.size(); x++)
  	{
  		if (obstacles[x].crosses(l))
  		{
  //			printf("[(%f, %f) to (%f, %f)] CROSSES [(%f, %f) to (%f, %f)]\n",
  //				   a.x, a.y, b.x, b.y,
  //				   obstacles[x].start.x, obstacles[x].start.y,
  //				   obstacles[x].end.x, obstacles[x].end.y);
  			return false;
  		}
  //		printf("[(%f, %f) to (%f, %f)] does not cross [(%f, %f) to (%f, %f)]\n",
  //			   a.x, a.y, b.x, b.y,
  //			   obstacles[x].start.x, obstacles[x].start.y,
  //			   obstacles[x].end.x, obstacles[x].end.y);
  	}
  	return true;
  }
  
  line2d ConfigEnvironment::GetAction(const recVec &s1, const recVec &s2) const
  {
  	line2d d;
  	d.start = s1;
  	d.end = s2;
  	return d;
  }
  
  void ConfigEnvironment::ApplyAction(recVec &s, line2d dir) const
  {
  	s = dir.end;
  }
  
  
  bool ConfigEnvironment::InvertAction(line2d &a) const
  {
  	line2d b = a;
  	a.start = b.end;
  	a.end = b.start;
  	return true;
  }
  
  
  double ConfigEnvironment::HCost(const recVec &node1, const recVec &node2)
  {
  	return sqrt((node1.x-node2.x)*(node1.x-node2.x) +
  				(node1.y-node2.y)*(node1.y-node2.y) +
  				(node1.z-node2.z)*(node1.z-node2.z));
  }
  
  double ConfigEnvironment::GCost(const recVec &node1, const  recVec &node2)
  {
  	return sqrt((node1.x-node2.x)*(node1.x-node2.x) +
  				(node1.y-node2.y)*(node1.y-node2.y) +
  				(node1.z-node2.z)*(node1.z-node2.z));
  }
  
  double ConfigEnvironment::GCost(const recVec &node1, const line2d &act)
  {
  	return sqrt((node1.x-act.end.x)*(node1.x-act.end.x) +
  				(node1.y-act.end.y)*(node1.y-act.end.y) +
  				(node1.z-act.end.z)*(node1.z-act.end.z));
  }
  
  bool ConfigEnvironment::GoalTest(const recVec &node, const recVec &theGoal)
  {
  	return (fequal(node.x, theGoal.x) && fequal(node.y, theGoal.y) && fequal(node.z, theGoal.z));
  }
  
  uint64_t ConfigEnvironment::GetStateHash(const recVec &node) const
  {
  	int x, y;
  	bool nx = node.x<0, ny = node.y<0;
  	x = (int)((double)fabs(node.x)*10000.0);
  	y = (int)((double)fabs(node.y)*10000.0);
  	uint64_t val = 0;
  	val = x;
  	val = (val<<14)|y;
  	val = (val<<1)|(nx?1:0);
  	val = (val<<1)|(ny?1:0);
  	return val;
  }
  
  uint64_t ConfigEnvironment::GetActionHash(line2d act) const
  {
  	return (GetStateHash(act.start)^GetStateHash(act.end));
  }
  
  
  void ConfigEnvironment::OpenGLDraw() const
  {
  	glColor3f(0, 0, 1.0);
  	for (unsigned int x = 0; x < obstacles.size(); x++)
  		DrawLine(obstacles[x]);
  }
  
  void ConfigEnvironment::OpenGLDraw(const recVec &l) const
  {
  	glBegin(GL_POINT);
  	glVertex3f(l.x, l.y, l.z);
  	glEnd();
  }
  
  void ConfigEnvironment::OpenGLDraw(const recVec &, const line2d &) const
  {
  }
  
  //void ConfigEnvironment::OpenGLDraw(const recVec &, const line2d &, GLfloat r, GLfloat g, GLfloat b) const
  //{
  //}
  //
  //void ConfigEnvironment::OpenGLDraw(const recVec &l, GLfloat r, GLfloat g, GLfloat b) const
  //{
  //}
  
  
  void ConfigEnvironment::GetNextState(const recVec &, line2d dir, recVec &news) const
  {
  	news = dir.end;
  }
  
  void ConfigEnvironment::DrawLine(line2d l) const
  {
  	glLineWidth(3);
  	glBegin(GL_LINES);
  	glVertex3f(l.start.x, l.start.y, 0);
  	glVertex3f(l.end.x, l.end.y, 0);
  	glEnd();
  	glLineWidth(1);
  }
  