Cod sursa(job #680887)

Utilizator claudiumihailClaudiu Mihail claudiumihail Data 16 februarie 2012 08:02:17
Problema Car Scor 20
Compilator cpp Status done
Runda Arhiva de probleme Marime 5.95 kb
#define _USE_MATH_DEFINES
#include <fstream>
#include <iostream>
#include <cstdlib>
#include <cstring>
#include <queue>
#include <cmath>

#define MAX_SIZE        501
#define NUM_DIRECTIONS  8
#define INF             0x3f3f3f3f
#define ANGLE_INC       M_PI_4

using namespace std;

struct Node
{
    Node() :
        bIsTraversable(true)
    {
        memset(dirCost, 0x3f, NUM_DIRECTIONS*sizeof(int));
    }
    
    operator bool() const
    {
        return bIsTraversable;
    }

    bool bIsTraversable;
    int dirCost[NUM_DIRECTIONS];
};

struct Vector2D
{
    Vector2D() :
        x(0),
        y(0)
    {}
    
    Vector2D(const short xx, const short yy) :
        x(xx),
        y(yy)
    {}
    
    bool operator == (const Vector2D& v) const
    {
        return (x == v.x && y == v.y);
    }
	
	bool operator != (const Vector2D& v) const
    {
        return (x != v.x || y != v.y);
    }

    short x, y;
};

typedef Vector2D Coord2D;

template<typename T>
class CQueue
{
public:
	CQueue(const size_t cap) :
		current(0),
		capacity(cap),
		size(0)
	{
		Q = (T*)malloc((cap+1)*sizeof(T));
	}
	
	inline T front() const
	{
		return Q[current];
	}
	
	inline void push(const T& elem)
	{
		const size_t pos = (current+size) % capacity;
		size++;
		Q[pos] = elem;
	}
	
	inline void pop()
	{
		if (size != 0)
		{
			size--;
			current++;
			current %= capacity;
		}
	}
	
	inline bool empty() const
	{
		return (size == 0);
	}
	
	inline void clear()
	{
		current = 0;
		size = 0;
	}
	
	~CQueue()
	{
		free(Q);
	}

private:
	T* Q;
	size_t current;
	size_t capacity;
	size_t size;
};

Vector2D vDirections[] =
{
    Vector2D(1,0),
    Vector2D(1,1),
    Vector2D(0,1),
    Vector2D(-1,1),
    Vector2D(-1,0),
    Vector2D(-1,-1),
    Vector2D(0,-1),
    Vector2D(1,-1)
};

Node matDrivingField[MAX_SIZE][MAX_SIZE];

float GetUnitVectorAngle(const Vector2D& v1, const Vector2D& v2)
{
    float magnitude = 2.0f;
    const bool bAllNon0 = (v1.x && v1.y) && (v2.x && v2.y);
    
    if (!bAllNon0)
    {
        const float magnitude1 = (v1.x && v1.y) ? 1.41421356f : 1.0f;
        const float magnitude2 = (v2.x && v2.y) ? 1.41421356f : 1.0f;
        magnitude = magnitude1 * magnitude2;
    }
    
    return acos( ((float)(v2.x*v1.x + v2.y*v1.y)) / (magnitude));
}

int GetCostFromDirDiff(const float fDirDiff)
{
    return (int)(fDirDiff / ANGLE_INC);
}

struct Pass
{
	Pass() :
		dir(0)
	{}
	
	Pass(const Coord2D& n, const Coord2D p, const char d) :
		dir(d),
		node(n),
		parent(p)
	{}

	char dir;
	Coord2D node;
	Coord2D parent;
};

int BFS(const Coord2D& st, const Coord2D& end, const int n, const int m)
{
    int cost = -1;
	queue<Pass> Q;
    
    for (int i=0; i<NUM_DIRECTIONS; ++i)
    {
		matDrivingField[st.x][st.y].dirCost[i] = 0;

		const Coord2D pt(st.x + vDirections[i].x, st.y + vDirections[i].y);

		if (pt.x >=0 && pt.x <n && pt.y >=0 && pt.y <n)
		{
			if (matDrivingField[pt.x][pt.y].bIsTraversable == true)
			{
				matDrivingField[pt.x][pt.y].dirCost[i] = 0;
				Q.push(Pass(pt, st, i));
			}
		}
	}
	
	while (!Q.empty())
	{
		const Pass current = Q.front();
		Q.pop();
		
		if (current.node == end)
		{
			break;
		}
		
		for (int i=0; i<NUM_DIRECTIONS; ++i)
		{
			const Coord2D pt(current.node.x + vDirections[i].x, current.node.y + vDirections[i].y);
			
			if (pt.x >=0 && pt.x <n && pt.y >=0 && pt.y <n)
			{
				if (pt != current.parent && matDrivingField[pt.x][pt.y].bIsTraversable == true && pt != st)
				{
					if (matDrivingField[current.node.x][current.node.y].dirCost[i] == INF)
					{
						matDrivingField[current.node.x][current.node.y].dirCost[i] = matDrivingField[current.node.x][current.node.y].dirCost[current.dir];
					}

					const int curCost = matDrivingField[current.node.x][current.node.y].dirCost[current.dir] +
										//matDrivingField[current.parent.x][current.parent.y].dirCost[current.dir] +
										GetCostFromDirDiff(GetUnitVectorAngle(vDirections[current.dir], vDirections[i]));
					
					if (matDrivingField[pt.x][pt.y].dirCost[i] > curCost)
					{
						matDrivingField[pt.x][pt.y].dirCost[i] = curCost;
						Q.push(Pass(pt, current.node, i));
						
						//cout << "Processed " << pt.x << " " << pt.y << " " << curCost << "\n";
					}
				}
			}
		}
	}
	
    // get the lowest cost for teh final destination point
    cost = matDrivingField[end.x][end.y].dirCost[0];
    for (int i=1; i<NUM_DIRECTIONS; ++i)
    {
        cost = min(cost, matDrivingField[end.x][end.y].dirCost[i]);
    }

    if (cost < INF)
    {
        return cost;
    }

    return -1;
}

int main()
{
    int n, m;
    Coord2D st, end;
    fstream fin("car.in", fstream::in);
    fstream fout("car.out", fstream::out);
    
    fin >> n >> m;
    fin >> st.x >> st.y >> end.x >> end.y;
    
    st.x--, st.y--;
    end.x--, end.y--;
	
	if (st == end)
	{
		fout << 0 << "\n";
		return 0;
	}
    
    /*cout << n << " " << m << "\n";
    cout << st.x << " " << st.y << "\n";
    cout << end.x << " " << end.y << "\n";*/
    
    for (int i=0; i<n; ++i)
    {
        for (int j=0; j<m; ++j)
        {
            int x;
            fin >> x;
            if (x)
            {
                matDrivingField[i][j].bIsTraversable = false;
            }
        }
    }
    
    /*for (int i=0; i<n; ++i)
    {
        for (int j=0; j<m; ++j)
        {
            cout << !matDrivingField[i][j] << " ";
        }
        cout << "\n";
    }*/

    /*cout << 180*GetUnitVectorAngle(Vector2D(-1,1), Vector2D(1,0))/M_PI << endl;
    
    for (int i=0; i< NUM_DIRECTIONS; ++i)
    {
        const float angleDiff = GetUnitVectorAngle(vDirections[i], Vector2D(1,0));
        cout << GetCostFromDirDiff(angleDiff) << " ";
    }
    cout << "\n";*/
    
    fout << BFS(st, end, n, m) << "\n";
    
    fin.close();
    fout.close();
    return 0;
}