c++实现 给定直角停车位两个点，求取剩余两点坐标。

//2018-09-08-fourmi


/*************************include head files************************************************/
#include<iostream>
#include<math.h>
#include<vector>
#include<cmath>
#include<iomanip>
#include<cstdlib>
#include<cstring>
#include<fstream>
/*******************************************************************************************/




/**********************Define variables*****************************************************/
#define pi 3.1415926
#define POINT   pair<double,double>  
#define VECT vector<vector<double> >
const int MAX_ITER=100000;
const double eps=0.0000001;
/*******************************************************************************************/




using namespace std;





/*****************************Vector_Transformation****************************************/
POINT Vector_Transformation(POINT &pt,double num1,double num2,double num3,double num4)
{
   POINT result;
   result.first=pt.first*num1+pt.second*num3;
   result.second=pt.first*num2+pt.second*num4;
   
   return result;
   
}
/*******************************************************************************************/




/***************************FUNCTIONS_ABOUT_SVD*********************************************/
double get_norm(double *x, int n){
    double r=0;

    for(int i=0;i<n;i++)
        r+=x[i]*x[i];
    return sqrt(r);
}

double normalize(double *x, int n){
    double r=get_norm(x,n);

    if(r<eps)
        return 0;
    for(int i=0;i<n;i++)
        x[i]/=r;
    return r;
}

inline double product(double*a, double *b,int n){
    double r=0;

    for(int i=0;i<n;i++)
        r+=a[i]*b[i];
    return r;
}

void orth(double *a, double *b, int n){//|a|=1
    double r=product(a,b,n);
 
    for(int i=0;i<n;i++)
        b[i]-=r*a[i];
    
}

bool svd(VECT A, int K, VECT &U, vector<double> &S, VECT &V){
    int M=A.size();
    int N=A[0].size();
    double *left_vector=new double[M];
    double *next_left_vector=new double[M];
    double *right_vector=new double[N];
    double *next_right_vector=new double[N];
    double diff=1;
    double r=-1;
    int col=0;

    U.clear();
    V.clear();
    S.clear();
    S.resize(K,0);
    U.resize(K);
    for(int i=0;i<K;i++)
        U[i].resize(M,0);
    V.resize(K);
    for(int i=0;i<K;i++)
        V[i].resize(N,0);    

    for(int col=0;col<K;col++){

        while(1){
            for(int i=0;i<M;i++)
                left_vector[i]= (float)rand() / RAND_MAX;
            if(normalize(left_vector, M)>eps)
                break;
                }
 
        for(int iter=0;diff>=eps && iter<MAX_ITER;iter++){
            memset(next_left_vector,0,sizeof(double)*M);
            memset(next_right_vector,0,sizeof(double)*N);
            for(int i=0;i<M;i++)
                for(int j=0;j<N;j++)
                    next_right_vector[j]+=left_vector[i]*A[i][j];
 
            r=normalize(next_right_vector,N);
            if(r<eps) break;
            for(int i=0;i<col;i++)
                orth(&V[i][0],next_right_vector,N);
            normalize(next_right_vector,N);
 
            for(int i=0;i<M;i++)
                for(int j=0;j<N;j++)
                    next_left_vector[i]+=next_right_vector[j]*A[i][j];
            r=normalize(next_left_vector,M);
            if(r<eps) break;
            for(int i=0;i<col;i++)
                orth(&U[i][0],next_left_vector,M);
            normalize(next_left_vector,M);
            diff=0;
            
            for(int i=0;i<M;i++){
                double d=next_left_vector[i]-left_vector[i];
                diff+=d*d;
            }
 
            memcpy(left_vector,next_left_vector,sizeof(double)*M);
            memcpy(right_vector,next_right_vector,sizeof(double)*N);
        }
        
        if(r>=eps){
            S[col]=r;
            memcpy((char *)&U[col][0],left_vector,sizeof(double)*M);
            memcpy((char *)&V[col][0],right_vector,sizeof(double)*N);
        }else{
            cout<<r<<endl;
            break;
        }
    }
   
    delete [] next_left_vector;
    delete [] next_right_vector;
    delete [] left_vector;
    delete [] right_vector;
 
    return true;
}
/*******************************************************************************************/




/**********************GET_THE_BIGGEST_SINGULAR_VALUE***************************************/
vector<double> GET_THE_BIGGEST_SINGULAR_VALUE(POINT vec,int m,int n,int k)
{
    //分解一个1*2的矩阵A，求其前1个奇异值和奇异向量
    
    VECT A;
    A.resize(m);
    
    for(int i=0;i<m;i++)
      {
        A[i].resize(n);
      }
    A[0][0]=vec.first;
    A[0][1]=vec.second; 

    VECT U;
    vector<double> S;
    VECT V;
    svd(A,k,U,S,V);
   return S;
}
/*******************************************************************************************/




/************************CAL_THIRD_AND_FORTH_POINTS*****************************************/
POINT * cal_reset_TWO_points(POINT &pt1,POINT &pt2)
{
   static POINT arr[2];
   POINT vecFromSecToFirst,vec,pt3,pt4;
   int x1,y1,x2,y2,exemplarStart,exemplarEnd;
   double distance,angle,s,sideLength;
   double slantAngleInRadians = (double(90)/180)*pi;
   double LongSideMin=47.9042;
   double verticalPSSideLength=195;
   double parallelPSSideLength=83;
   int matrix_rows=1;
   int matrix_cols=2;
   int top_k_max=1;
  
   x1=pt1.first;
   y1=pt1.second;
   x2=pt2.first;
   y2=pt2.second;

   distance = sqrt(pow((x1-x2),2)+pow((y1-y2),2));

   if (distance < LongSideMin) //说明是短库位
      sideLength = verticalPSSideLength;
   else //说明是平行长库位
      sideLength = parallelPSSideLength;
   
   vecFromSecToFirst.first=pt1.first-pt2.first;
   vecFromSecToFirst.second=pt1.second-pt2.second;
   vec=Vector_Transformation(vecFromSecToFirst,cos(slantAngleInRadians),-sin(slantAngleInRadians), sin(slantAngleInRadians), cos(slantAngleInRadians));


   if ((pt2.first-pt1.first)==0)
    {
       angle=pi/2;
    }
    else
    {
       angle=abs(atan2(vecFromSecToFirst.second,vecFromSecToFirst.first));      
    }

    s=GET_THE_BIGGEST_SINGULAR_VALUE(vec,matrix_rows,matrix_cols,top_k_max)[0];   
    vec.first=vec.first/s;
    vec.second=vec.second/s;

    pt3.first=pt2.first+vec.first*sideLength;
    pt3.second=pt2.second+vec.second*sideLength;
    pt4.first=pt1.first+vec.first*sideLength;
    pt4.second=pt1.second+vec.second*sideLength;
    arr[0]=pt3;
    arr[1]=pt4;


   return arr;
}
/*******************************************************************************************/




/*****************************************MAIN()********************************************/
int main()
{

  POINT * arr;
  POINT pt1(171,145);
  POINT pt2(171,213);
  arr=cal_reset_TWO_points(pt1,pt2);
  std::cout<<arr[0].first<<"  "<<arr[0].second<<std::endl;
  std::cout<<arr[1].first<<"  "<<arr[1].second<<std::endl;

  return 0;
  
}
/*******************************************************************************************/