#include <math.h>
|
#include "linear.h"
|
|
Linear::Linear(vector<vector<int>> cam_vec_ray, int center_x,int center_y)
|
{
|
this->points_list = cam_vec_ray;
|
this->center_x = center_x;
|
this->center_y = center_y;
|
|
Linear::bottom_line();
|
Linear::third_line();
|
|
}
|
|
// 直线法表示底边直线
|
void Linear::bottom_line() {
|
int x1 = this->points_list[0][0];
|
int y1 = this->points_list[0][1];
|
|
int x2 = this->points_list[1][0];
|
int y2 = this->points_list[1][1];
|
|
if(x1 == x2) {
|
this->flag = 1;
|
}else if(y2 == y1) {
|
this->flag = 2;
|
} else{
|
this->flag = 0;
|
this->k = (float)(y2-y1)/(x2-x1);
|
}
|
// printf("%d,%d\n",x1,y1);
|
// printf("%d,%d\n",x2,y2);
|
// cout << this->flag << endl;
|
// cout << "sssss" << endl;
|
// cout << this->k <<endl;
|
}
|
|
void Linear::third_line()
|
{
|
if(this->flag == 0)
|
{
|
this->b = this->center_y - this->k * this->center_x;
|
}
|
// printf("%f,%f",this->b,this->k);
|
}
|
|
|
vector<float> Linear::inter_points(vector<int> point_m, vector<int> point_n)
|
{
|
int xm = point_m[0];
|
int ym = point_m[1];
|
int xn = point_n[0];
|
int yn = point_n[1];
|
vector<float> inter_point_vec;
|
float inter_x, inter_y;
|
if(xm == xn) {
|
inter_x = (float) xm;
|
if (this->flag == 2)
|
inter_y = (float) this->center_y;
|
else
|
inter_y = (float) (this->k * inter_x + this->b);
|
}else if(ym == yn){
|
inter_y = (float)ym;
|
if(this->flag == 1)
|
inter_x = (float) this->center_x;
|
else
|
inter_x = (inter_y - this->b)/this->k;
|
}else
|
{
|
float k1 = (float)(ym - yn)/(xm - xn);
|
float b1 = ym - k1*xm;
|
if(this->flag == 1)
|
{
|
inter_x = (float) this->center_x;
|
inter_y = k1*inter_x + b1;
|
} else if (this->flag == 2)
|
{
|
inter_y = (float) this->center_y;
|
inter_x = (inter_y - b1)/k1;
|
} else
|
{
|
inter_x = (this->b - b1)/(k1 - this->k);
|
inter_y = k1*inter_x + b1;
|
}
|
}
|
|
inter_point_vec.push_back(inter_x);
|
inter_point_vec.push_back(inter_y);
|
|
return inter_point_vec;
|
|
}
|
|
|
double Linear::cal_distance()
|
{
|
printf("cal distance");
|
vector<float> s_point_vec = Linear::inter_points(this->points_list[0],this->points_list[3]);
|
vector<float> e_point_vec = Linear::inter_points(this->points_list[1],this->points_list[2]);
|
printf("%f,%f,other,%f,%f",s_point_vec[0],s_point_vec[1],e_point_vec[0],e_point_vec[1]);
|
|
float pow_x = pow((s_point_vec[0] - e_point_vec[0]),2);
|
float pow_y = pow((s_point_vec[1] - e_point_vec[1]),2);
|
double distance = pow((pow_x + pow_y),0.5);
|
|
return distance;
|
}
|
|
|
double Linear::adjust_distance(int base_distance,int bottom_distance, double center_distance,int angle)
|
{
|
MathUtils mm;
|
double rate = cos(mm.angle_to_radian((double)angle));
|
if(rate == 0)
|
{
|
rate = 1e-5;
|
}
|
|
double result_distance = base_distance * bottom_distance / (center_distance * rate);
|
printf("\n%f::%f\n",rate,result_distance);
|
// printf("\na%d%d%f%daa%fhe%f",ragresult_distance);
|
return result_distance;
|
}
|
