题目地址：https://nanti.jisuanke.com/t/18518

 

人生第一篇博客。。。

 

用两个向量face和head模拟人的方位，向上或向下旋转ang角时，两个向量都旋转ang角，左右旋转时，face旋转ang角，head不变，自己感受一下就知道了。主要就是两个模板，三维空间中点到线段的距离以及向量的旋转。

 

 

#include<bits/stdc++.h>
#define eps 1e-10
#define pi acos(-1.0)
using namespace std;
int dcmp(double x){if (fabs(x)<eps)return 0;else return x<0?-1:1;}
struct Point3
{
    double x,y,z;
    Point3(double x=0,double y=0,double z=0):x(x),y(y),z(z){};
};
typedef Point3 Vector3;

Vector3 operator + (Vector3 a,Vector3 b){return Vector3(a.x+b.x,a.y+b.y,a.z+b.z);}
Vector3 operator - (Vector3 a,Vector3 b){return Vector3(a.x-b.x,a.y-b.y,a.z-b.z);}
Vector3 operator * (Vector3 a,double b){return Vector3(a.x*b,a.y*b,a.z*b);}
Vector3 operator / (Vector3 a,double b){return Vector3(a.x/b,a.y/b,a.z/b);}
bool operator == (Vector3 a,Vector3 b){return a.x==b.x && a.y==b.y && a.z==b.z;}
double Dot3(Vector3 a,Vector3 b){return a.x*b.x+a.y*b.y+a.z*b.z;}  //点积
double Length3(Vector3 a){return sqrt(Dot3(a,a));}
Vector3 Cross3(Vector3 a,Vector3 b) //叉积
{return Vector3(a.y*b.z-a.z*b.y,a.z*b.x-a.x*b.z,a.x*b.y-a.y*b.x);}
double Angle3(Vector3 a,Vector3 b){return acos(Dot3(a,b)/Length3(a)/Length3(b));}// 范围[0,180]

//点到线段的距离
double DistanceToSeg3(Point3 p,Point3 a,Point3 b)
{
    if(a==b) return Length3(p-a);
    Vector3 v1=b-a,v2=p-a,v3=p-b;
    if(dcmp(Dot3(v1,v2))<0) return Length3(v2);
    else if(dcmp(Dot3(v1,v3))>0) return Length3(v3);
    else return Length3(Cross3(v1,v2))/Length3(v1);
}

//点p绕向量ov旋转ang角度，旋转方向是向量ov叉乘向量op
Point3 rotate3(Point3 p,Vector3 v,double ang)
{
    double ret[3][3],a[3];
    v = v / Length3(v);
    ret[0][0] = (1.0 - cos(ang)) * v.x * v.x + cos(ang);
    ret[0][1] = (1.0 - cos(ang)) * v.x * v.y - sin(ang) * v.z;
    ret[0][2] = (1.0 - cos(ang)) * v.x * v.z + sin(ang) * v.y;
    ret[1][0] = (1.0 - cos(ang)) * v.y * v.x + sin(ang) * v.z;
    ret[1][1] = (1.0 - cos(ang)) * v.y * v.y + cos(ang);
    ret[1][2] = (1.0 - cos(ang)) * v.y * v.z - sin(ang) * v.x;
    ret[2][0] = (1.0 - cos(ang)) * v.z * v.x - sin(ang) * v.y;
    ret[2][1] = (1.0 - cos(ang)) * v.z * v.y + sin(ang) * v.x;
    ret[2][2] = (1.0 - cos(ang)) * v.z * v.z + cos(ang);
    for (int i=0;i<3;i++) a[i]=ret[i][0]*p.x+ret[i][1]*p.y+ret[i][2]*p.z;
    return Point3(a[0],a[1],a[2]);
}

Point3 face,head,st,en,a,vx=Point3(1,0,0),vy=Point3(0,1,0),vz=Point3(0,0,1);

int main()
{
    int T,n;
    scanf("%d",&T);
    while (T--)
    {
        double ans=1e8,ang,dx,dy,dz,d; char x[3];
        face=Point3(1,0,0); head=Point3(0,0,1);
        scanf("%lf%lf%lf",&st.x,&st.y,&st.z);
        scanf("%lf%lf%lf",&en.x,&en.y,&en.z);
        scanf("%d",&n);
        while (n--)
        {
            scanf("%lf %s %lf",&d,x,&ang);
            dx=d*cos(Angle3(vx,face));
            dy=d*cos(Angle3(vy,face));
            dz=d*cos(Angle3(vz,face));
            a=st+Point3(dx,dy,dz);
            ans=min(ans,DistanceToSeg3(en,st,a));
            st=a;
            if (x[0]=='U')
            {
                Vector3 v=Cross3(face,head);
                face=rotate3(face,v,ang);
                head=rotate3(head,v,ang);
            }else
            if (x[0]=='D')
            {
                Vector3 v=Cross3(head,face);
                face=rotate3(face,v,ang);
                head=rotate3(head,v,ang);
            }else
            if (x[0]=='L')
            {
                face=rotate3(face,head,ang);
            }else
            if (x[0]=='R')
            {
                Vector3 v=head*(-1);
                face=rotate3(face,v,ang);
            }
        }
        printf("%.2f\n",ans);
    }
    return 0;
}