double * multipleLinearRegression(double **X, double *y, int n, int m){
double **A, **XT, **B, *b, *x, **Y;
/* int i,j; */
Y=vectorToMatrix(y,m,1);
x=allocateDoubleVector(n);
XT=trasposeMatrix(X,n,m);
/* XT.X.x = XT.b */
A=multiplyMatrix(XT,X,m,n,n);
B=multiplyMatrix(XT,Y,m,n,1);
b=matrixToVector(B,n,1);
/* for (i=0; i<n; i++){
for (j=0; j<n; j++){
fprintf(stdout,"%f ",A[i][j]);
}
fprintf(stdout,"\n");
}
for(i=0;i<n;i++) fprintf(stdout,"%f ",b[i]);
fprintf(stdout,"\n"); */
x = gaussianElimination (n, A, b);
return x;
}
// Adapted from code found here: http://forum.openframeworks.cc/t/quad-warping-homography-without-opencv/3121/19
mat4 WarpPerspective::getPerspectiveTransform( const vec2 src[4], const vec2 dst[4] ) const
{
float p[8][9] = {
{ -src[0][0], -src[0][1], -1, 0, 0, 0, src[0][0] * dst[0][0], src[0][1] * dst[0][0], -dst[0][0] }, // h11
{ 0, 0, 0, -src[0][0], -src[0][1], -1, src[0][0] * dst[0][1], src[0][1] * dst[0][1], -dst[0][1] }, // h12
{ -src[1][0], -src[1][1], -1, 0, 0, 0, src[1][0] * dst[1][0], src[1][1] * dst[1][0], -dst[1][0] }, // h13
{ 0, 0, 0, -src[1][0], -src[1][1], -1, src[1][0] * dst[1][1], src[1][1] * dst[1][1], -dst[1][1] }, // h21
{ -src[2][0], -src[2][1], -1, 0, 0, 0, src[2][0] * dst[2][0], src[2][1] * dst[2][0], -dst[2][0] }, // h22
{ 0, 0, 0, -src[2][0], -src[2][1], -1, src[2][0] * dst[2][1], src[2][1] * dst[2][1], -dst[2][1] }, // h23
{ -src[3][0], -src[3][1], -1, 0, 0, 0, src[3][0] * dst[3][0], src[3][1] * dst[3][0], -dst[3][0] }, // h31
{ 0, 0, 0, -src[3][0], -src[3][1], -1, src[3][0] * dst[3][1], src[3][1] * dst[3][1], -dst[3][1] }, // h32
};
gaussianElimination( &p[0][0], 9 );
mat4 result = mat4( p[0][8], p[3][8], 0, p[6][8], p[1][8], p[4][8], 0, p[7][8], 0, 0, 1, 0, p[2][8], p[5][8], 0, 1 );
return result;
}
void testGaussianElimination(){
double **A;
int n;
double *x;
n=3;
double b[n];
A=allocateDoubleMatrix(3, 3);
A[0][0]=1.0; A[0][1]=1.0; A[0][2]=-1.0;
A[1][0]=1.0; A[1][1]=2.0; A[1][2]=1.0;
A[2][0]=2.0; A[2][1]=-1.0; A[2][2]=1.0;
b[0]=2.0;
b[1]=6.0;
b[2]=1.0;
x=gaussianElimination (n, A, b);
fprintf(stdout,"Gaussian Elimination\n");
printVector(x,n);
}
M44 computeHomography(const Vec2 *src, const Vec2 *dst)
{
// Adapted from:
// arturo castro - 08/01/2010
//
// create the equation system to be solved
//
// from: Multiple View Geometry in Computer Vision 2ed
// Hartley R. and Zisserman A.
//
// x' = xH
// where H is the homography: a 3 by 3 matrix
// that transformed to inhomogeneous coordinates for each point
// gives the following equations for each point:
//
// x' * (h31*x + h32*y + h33) = h11*x + h12*y + h13
// y' * (h31*x + h32*y + h33) = h21*x + h22*y + h23
//
// as the homography is scale independent we can let h33 be 1 (indeed any of the terms)
// so for 4 points we have 8 equations for 8 terms to solve: h11 - h32
// after ordering the terms it gives the following matrix
// that can be solved with gaussian elimination:
float P[8][9]={
{-src[0].x, -src[0].y, -1, 0, 0, 0, src[0].x*dst[0].x, src[0].y*dst[0].x, -dst[0].x }, // h11
{ 0, 0, 0, -src[0].x, -src[0].y, -1, src[0].x*dst[0].y, src[0].y*dst[0].y, -dst[0].y }, // h12
{-src[1].x, -src[1].y, -1, 0, 0, 0, src[1].x*dst[1].x, src[1].y*dst[1].x, -dst[1].x }, // h13
{ 0, 0, 0, -src[1].x, -src[1].y, -1, src[1].x*dst[1].y, src[1].y*dst[1].y, -dst[1].y }, // h21
{-src[2].x, -src[2].y, -1, 0, 0, 0, src[2].x*dst[2].x, src[2].y*dst[2].x, -dst[2].x }, // h22
{ 0, 0, 0, -src[2].x, -src[2].y, -1, src[2].x*dst[2].y, src[2].y*dst[2].y, -dst[2].y }, // h23
{-src[3].x, -src[3].y, -1, 0, 0, 0, src[3].x*dst[3].x, src[3].y*dst[3].x, -dst[3].x }, // h31
{ 0, 0, 0, -src[3].x, -src[3].y, -1, src[3].x*dst[3].y, src[3].y*dst[3].y, -dst[3].y }, // h32
};
gaussianElimination(&P[0][0],9);
// gaussian elimination gives the results of the equation system
// in the last column of the original matrix.
// fill in openGL compatible 4x4 matrix and return
M44 res;
res.identity();
// gaussian elimination gives the results of the equation system
// in the last column of the original matrix.
// opengl needs the transposed 4x4 matrix:
float aux_H[]={ P[0][8],P[3][8],0,P[6][8], // h11 h21 0 h31
P[1][8],P[4][8],0,P[7][8], // h12 h22 0 h32
0 , 0,1,0, // 0 0 0 0
P[2][8],P[5][8],0,1}; // h13 h23 0 h33
for(int i=0;i<16;i++) res.m[i] = aux_H[i];
/*
res.m11 = P[0][8]; res.m21 = P[3][8]; res.m41 = P[6][8];
res.m12 = P[1][8]; res.m22 = P[4][8]; res.m42 = P[7][8];
res.m14 = P[2][8]; res.m24 = P[5][8]; res.m44 = 1.0;*/
/*
float aux_H[]={ P[0][8],P[3][8],0,P[6][8], // h11 h21 0 h31
P[1][8],P[4][8],0,P[7][8], // h12 h22 0 h32
0 , 0,1,0, // 0 0 0 0
P[2][8],P[5][8],0,1}; // h13 h23 0 h33
*/
return res;
}
ana_uni* delta_move_abs_circle_path(Delta &delta, ana_uni x2,ana_uni y2,ana_uni z2,
ana_uni x3,ana_uni y3,ana_uni z3,
dig_uni axis1,dig_uni axis2,dig_uni axis3,
dig_uni pathNum){
ana_uni* ana_path = new ana_uni[pathNum*3];
delta.set_kin_theta(fromDigCodeToRad(axis1,13),fromDigCodeToRad(axis2,13),fromDigCodeToRad(axis3,13));
delta.kin();
m_uni T[MATRIX_R_4][MATRIX_C_4];
Pos p1(delta.kin_pos);
Pos p2(x2,y2,z2);
Pos p3(x3,y3,z3);
// cout<<"p1----------------"<<endl;
// Pos::pos_print(p1);
Log::log_d("p1",p1);
m_uni A1 = (p1.y-p3.y)*(p2.z-p3.z)-(p2.y-p3.y)*(p1.z-p3.z);
m_uni B1 = (p2.x-p3.x)*(p2.z-p3.z)-(p1.x-p3.x)*(p2.z-p3.z);
m_uni C1 = (p1.x-p3.x)*(p2.y-p3.y)-(p2.x-p3.x)*(p1.y-p3.y);
m_uni D1 = -( ( (p1.y-p3.y)*(p2.z-p3.z)-(p2.y-p3.y)*(p1.z-p3.z) )*p3.x +
( (p2.x-p3.y)*(p2.z-p3.z)-(p1.x-p3.y)*(p2.z-p3.z) )*p3.y +
( (p1.x-p3.x)*(p2.y-p3.y)-(p2.x-p3.x)*(p1.y-p3.y) )*p3.z );
cout<<"A1 : "<<A1<<endl;
cout<<"B1 : "<<B1<<endl;
cout<<"C1 : "<<C1<<endl;
cout<<"D1 : "<<D1<<endl;
m_uni A2 = (p2.x-p1.x);
m_uni B2 = (p2.y-p1.y);
m_uni C2 = (p2.z-p1.z);
m_uni D2 = -(Pos::pos_norm_2(p2)-Pos::pos_norm_2(p1))/2.0;
m_uni A3 = (p3.x-p2.x);
m_uni B3 = (p3.y-p2.y);
m_uni C3 = (p3.z-p2.z);
m_uni D3 = -(Pos::pos_norm_2(p3)-Pos::pos_norm_2(p2))/2.0;
m_uni matrix[MATRIX_R_3][MATRIX_C_4] = {
{A1,B1,C1,-D1},
{A2,B2,C2,-D2},
{A3,B3,C3,-D3},
};
gaussianElimination(matrix,3,4);
matrix_print(matrix,3,4);
coordTransformationMatrix(T,
matrix[0][3],matrix[1][3],matrix[2][3],
p1.x,p1.y,p1.z,
p2.x,p2.y,p2.z,
p3.x,p3.y,p3.z,
A1,B1,C1);
matrix_print(T,MATRIX_R_4,4);
m_uni t03 = T[0][0]*matrix[0][3]+T[1][0]*matrix[1][3]+T[2][0]*matrix[2][3];
m_uni t13 = T[0][1]*matrix[0][3]+T[1][1]*matrix[1][3]+T[2][1]*matrix[2][3];
m_uni t23 = T[0][2]*matrix[0][3]+T[1][2]*matrix[1][3]+T[2][2]*matrix[2][3];
m_uni T_inv[MATRIX_R_4][MATRIX_C_4] = {
{T[0][0],T[1][0],T[2][0],-t03},
{T[0][1],T[1][1],T[2][1],-t13},
{T[0][2],T[1][2],T[2][2],-t23},
{0,0,0,1},
};
matrix_print(T_inv,MATRIX_R_4,4);
m_uni r = Pos::pos_norm(Pos::pos_sub(Pos(matrix[0][3],matrix[1][3],matrix[2][3]),p1));
cout<<"r: "<<r<<endl;
Log::log_d("r",r);
Pos p1_q = TMultiP(T_inv,p1);
Pos p2_q = TMultiP(T_inv,p2);
Pos p3_q = TMultiP(T_inv,p3);
cout<<"p_q-------------------"<<endl;
Pos::pos_print(p1_q);
Pos::pos_print(p2_q);
Pos::pos_print(p3_q);
Log::log_d("p1_q",p1_q);
Log::log_d("p2_q",p2_q);
Log::log_d("p3_q",p3_q);
int pathNum1 = pathNum*(p2_q.x-p1_q.x)/(p3_q.x-p1_q.x)+0.5;
Log::log_d("pathNum1",pathNum1);
int pathNum2 = pathNum-pathNum1;
int dir = (p2_q.y-p1_q.y)>0?1:-1;
ana_uni delta_x = (p2_q.x-p1_q.x)/pathNum1;
Log::log_d("delta_x1",delta_x);
int index = 0;
int cnt = 0;
ana_uni max = p1.y>p2.y?p1.y:p2.y;
ana_uni min = p1.y>p2.y?p2.y:p1.y;
ana_uni y = 0;
for(index = 0;index < pathNum1*3;cnt++){
ana_path[index] = p1_q.x+delta_x*(cnt+1);
y = ana_path[index+1] = sqrt(r*r-ana_path[index]*ana_path[index]);
if(y < min-0.8 || y > max+0.8)
ana_path[index+1] = -y;
ana_path[index+2] = 0;
index+=3;
}
delta_x = (p3_q.x-p2_q.x)/pathNum2;
Log::log_d("delta_x2",delta_x);
dir = (p3_q.y-p2_q.y)>0?1:-1;
max = p3.y>p2.y?p3.y:p2.y;
min = p3.y>p2.y?p2.y:p3.y;
y = 0;
cnt = 0;
for(int i = index;i<pathNum*3;cnt++){
ana_path[i] = p2_q.x+delta_x*(cnt+1);
y = ana_path[i+1] = sqrt(r*r-ana_path[i]*ana_path[i]);
if(y < min-0.8 || y > max+0.8)
ana_path[i+1] = -y;
ana_path[i+2] = 0;
i+=3;
}
Log::log_d("two dimen coord path",ana_path,pathNum);
// Pos p1_q(-400,0,0);
// Pos p2_q(0,400,0);
// Pos p3_q(400,0,0);
// Pos p1_p = getThreeDimensionalPointFromTwoDiment(p1_q,T);
// Pos p2_p = getThreeDimensionalPointFromTwoDiment(p2_q,T);
// Pos p3_p = getThreeDimensionalPointFromTwoDiment(p3_q,T);
// Pos::pos_print(p1_p);
// Pos::pos_print(p2_p);
// Pos::pos_print(p3_p);
return ana_path;
}
