Cod sursa(job #681916)

Utilizator claudiumihailClaudiu Mihail claudiumihail Data 18 februarie 2012 06:41:59
Problema Car Scor 100
Compilator cpp Status done
Runda Arhiva de probleme Marime 6.8 kb
#define _USE_MATH_DEFINES
#include <fstream>
#include <iostream>
#include <cstdlib>
#include <cstring>
#include <queue>
#include <cmath>
#include <limits>

#define MAX_SIZE        501
#define MAX_DIRS		8
#define INF             0x3f3f3f3f
#define ANGLE_INC       M_PI_4
#define DIR_MASK		7
#define COORD_MASK		511
#define COORD_SHIFT		9

using namespace std;

struct Vector2D
{
    Vector2D() :
        x(0),
        y(0)
    {}
    
    Vector2D(const short xx, const short yy) :
        x(xx),
        y(yy)
    {}
	
	Vector2D(const int packedVector2D) :
		x((packedVector2D>>COORD_SHIFT) & COORD_MASK),
		y(packedVector2D & COORD_MASK)
	{}
    
    inline bool operator == (const Vector2D& v) const
    {
        return (x == v.x && y == v.y);
    }
	
	inline const Vector2D operator + (const Vector2D& v) const
	{
		return Vector2D(x+v.x, y+v.y);
	}
	
	inline bool operator != (const Vector2D& v) const
    {
        return (x != v.x || y != v.y);
    }
	
	inline operator int() const
	{
		return ((x<<COORD_SHIFT) | y);
	}

    short x, y;
};

inline int PackDir(const int package, const int dir)
{
	return (package | (dir<<(2*COORD_SHIFT)));
}

inline int UnpackDir(const int package)
{
	return ((package >> (2*COORD_SHIFT)) & DIR_MASK);
}

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)
};

#define NUM_DIRECTIONS  8 //(sizeof(vDirections)/sizeof(Vector2D))

short explored[MAX_SIZE][MAX_SIZE];

template<typename T, int defaultMaxCap=0>
class CQueue
{
public:
	CQueue(const size_t cap) :
		current(0),
		capacity(cap),
		size(0)
	{
		Q = (T*)malloc((cap+1)*sizeof(T));
	}
	
	CQueue() :
		current(0),
		capacity(defaultMaxCap),
		size(0)
	{
		Q = (T*)malloc((defaultMaxCap+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;
};

CQueue<int, MAX_SIZE*MAX_SIZE> Q[2];

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

    bool bIsTraversable;
    unsigned short dirCost[MAX_DIRS];
};

typedef Vector2D Coord2D;

Node matDrivingField[MAX_SIZE][MAX_SIZE];

inline bool IsWithinLimits(const Vector2D& p, const int n, const int m)
{
	return (p.x >=0 && p.x <n && p.y >=0 && p.y <m);
}

int BFS(const Coord2D& st, const Coord2D& end, const int n, const int m)
{
    int cost = INF;
	int curQ = 0;
	
	for (int i=0; i<MAX_DIRS; ++i)
	{
		const Vector2D p = st + vDirections[i];
		
		if (IsWithinLimits(p,n,m) && matDrivingField[p.x][p.y])
		{
			int packedState = p;
			packedState = PackDir(p, i);
			
			Q[curQ].push(packedState);
			matDrivingField[p.x][p.y].dirCost[i] = 0;
		}
	}
	
	while (!Q[0].empty() || !Q[1].empty())
	{
		while (!Q[curQ].empty())
		{
			int nextDir, newCost;
			const int packedState = Q[curQ].front();
			Q[curQ].pop();

			const Vector2D v = packedState;
			const int dir = UnpackDir(packedState);
			
			if (v == end)
			{
				return matDrivingField[v.x][v.y].dirCost[dir];
			}
			
			Vector2D pS = v + vDirections[dir];
			if (IsWithinLimits(pS,n,m) && matDrivingField[pS.x][pS.y])
			{
				if (matDrivingField[v.x][v.y].dirCost[dir] < matDrivingField[pS.x][pS.y].dirCost[dir])
				{
					Q[curQ].push(PackDir(pS,dir));
					matDrivingField[pS.x][pS.y].dirCost[dir] = matDrivingField[v.x][v.y].dirCost[dir];
					
					explored[v.x][v.y] = matDrivingField[v.x][v.y].dirCost[dir];
				}
			}
			
			nextDir = (dir+1)%MAX_DIRS;
			pS = v + vDirections[nextDir];
			if (IsWithinLimits(pS,n,m) && matDrivingField[pS.x][pS.y])
			{
				newCost = matDrivingField[v.x][v.y].dirCost[dir]+1;
				
				if (newCost < matDrivingField[pS.x][pS.y].dirCost[nextDir] && newCost < matDrivingField[v.x][v.y].dirCost[nextDir])
				{
					Q[!curQ].push(PackDir(v,nextDir));
					matDrivingField[v.x][v.y].dirCost[nextDir] = newCost;
				}
			}
			
			nextDir = ((dir-1) < 0 ? 7 : (dir-1));
			pS = v + vDirections[nextDir];
			if (IsWithinLimits(pS,n,m) && matDrivingField[pS.x][pS.y])
			{
				newCost = matDrivingField[v.x][v.y].dirCost[dir]+1;
				
				if (newCost < matDrivingField[pS.x][pS.y].dirCost[nextDir] && newCost < matDrivingField[v.x][v.y].dirCost[nextDir])
				{
					Q[!curQ].push(PackDir(v,nextDir));
					matDrivingField[v.x][v.y].dirCost[nextDir] = newCost;
				}
			}
		}
		
		curQ = !curQ;
	}

    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--;
	
	/*Vector2D v;
	GetDestDir(Coord2D(1,1), Coord2D(-2,4), v);
	
	cout << v.x << " " << v.y << "\n";
	
	return 0;*/
	
	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;
            }
        }
    }
	
	/*Vector2D v(501, 501);
	
	int packedV = v;
	
	packedV = PackDir(packedV, 7);
	
	cout << v << endl;
	
	Vector2D v2(packedV);
	
	cout << v2.x << " " << v2.y << " " << UnpackDir(packedV) << endl;*/
    
    /*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";
	
	/*for (int i=0; i<n; ++i)
	{
		for (int j=0; j<m; ++j)
		{
			cout << explored[i][j] << " ";
		}
		cout << "\n";
	}*/
    
    fin.close();
    fout.close();
    return 0;
}