HDU 1435 Stable Match(稳定婚姻问题)

#include<iostream>
#include<queue>
#include<cmath>
using namespace std;
const int maxn=300+5;
const double eps=1e-8;
int pref[maxn][maxn],order[maxn][maxn],Next[maxn];
int future_husband[maxn],future_wife[maxn];
queue<int> q;
struct point{int idx,c;double x,y,z;}p1[maxn],p2[maxn];
struct rank{
    int idx,c;double dis;
    bool operator < (const rank &rhs) const {
        if(fabs(dis-rhs.dis)<eps) return c>rhs.c;
        else return dis<rhs.dis;
    }
}r[maxn];
void engage(int man,int woman)
{
    int m=future_husband[woman];
    if(m)
    {
        future_wife[m]=0;
        q.push(m);
    }
    future_husband[woman]=man;
    future_wife[man]=woman;
}
double dis(point o1,point o2)
{
    double x=o1.x-o2.x;
    double y=o1.y-o2.y;
    double z=o1.z-o2.z;
    return sqrt(x*x+y*y+z*z);
}
int main()
{
    int t;
    scanf("%d",&t);
    while(t--)
    {
        int n;
        scanf("%d",&n);
        for(int i=1;i<=n;i++)
            scanf("%d%d%lf%lf%lf",&p1[i].idx,&p1[i].c,&p1[i].x,&p1[i].y,&p1[i].z);
        for(int i=1;i<=n;i++)
            scanf("%d%d%lf%lf%lf",&p2[i].idx,&p2[i].c,&p2[i].x,&p2[i].y,&p2[i].z);
        for(int i=1;i<=n;i++){
            for(int j=1;j<=n;j++){
                r[j].idx=j;
                r[j].c=p2[j].c;
                r[j].dis=dis(p1[i],p2[j]);
            }
            sort(r+1,r+n+1);
            for(int j=1;j<=n;j++){
                pref[i][j]=r[j].idx;
            }
            Next[i]=1;
            future_wife[i]=0;
            q.push(i);
        }
        for(int i=1;i<=n;i++){
            for(int j=1;j<=n;j++){
                r[j].idx=j;
                r[j].c=p1[j].c;
                r[j].dis=dis(p1[j],p2[i]);
            }
            sort(r+1,r+n+1);
            for(int j=1;j<=n;j++){
                order[i][r[j].idx]=j;
            }
            future_husband[i]=0;
        }
        while(!q.empty())  
        {  
            int man=q.front();q.pop();  
            int woman=pref[man][Next[man]++];  
            if(future_husband[woman]==0) engage(man,woman);  
            else if(order[woman][man]<order[woman][future_husband[woman]]) engage(man,woman);  
            else q.push(man);  
        } 
        while(!q.empty()) q.pop();
        for(int i=1;i<=n;i++) {
            printf("%d %d\n",future_husband[i],i);
        }
    }
    return 0;
}