//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;
}
/*******************************************************************************************/
