/*! \brief Input: cyphertext as matrix, private key, unimodular, inverse of private key, inverse of unimodular
* Output: Decrypted message as a matrix.
*
* Performs decryption on a message. This is what Alice does to Bob's encrypted message.
*/
gsl_matrix* decryptor(gsl_matrix *c, /*!< Cyphertext as a vector */
gsl_matrix *v, /*!< Private Key*/
gsl_matrix *u, /*!< Unimodular matrix */
gsl_matrix *vinv,/*!< Inverse of private key */
gsl_matrix *uinv) /*!< Inverse of Unimodular matrix */
{
gsl_matrix *decryp = gsl_matrix_alloc(1,SIZE);
gsl_matrix *m = gsl_matrix_alloc(1,SIZE); /*result is stored in here*/
/*Compute c*V^(-1)*/
gsl_linalg_matmult(c,vinv,decryp);
/*Round the elements of decryp*/
roundVector(decryp);
/*Compute decryp*U^(-1) = m*/
gsl_linalg_matmult(decryp,uinv,m);
roundVector(m);
gsl_matrix_free(decryp);
return m;
}
/*! \brief Input: _. Output: A matrix
*
* Generates a unimodular matrix.
*/
gsl_matrix* genU(){
int z, j ,display = 1;
/* Initialize and allocate memory for necessary matrices */
gsl_matrix* result = gsl_matrix_alloc(SIZE,SIZE);
gsl_matrix* result2 = gsl_matrix_alloc(SIZE,SIZE);
gsl_matrix* base = gsl_matrix_alloc(SIZE,SIZE);
gsl_matrix_set_identity(result);
gsl_matrix_set_identity(result2);
gsl_matrix_set_identity(base);
/* Display progress of generating a unimodular to the user */
printf("Generating Unimodular\n");
printf("|..................................................|\r\033[01;35m|");
fflush(stdout);
if(SIZE/50 > 1) display = SIZE/50;
for(z = 0; z<DEPTH; z++){
if(z%2 == 0){
for(j=0; j < SIZE ; j++)
{
if(j%2 == 0){
randomLine(base,j);
gsl_linalg_matmult(result,base,result2);
gsl_matrix_memcpy(result,result2);
clearLine(base,j);
if(j % display == 0){
printf("-");
fflush(stdout);
}
}
}
}else{
printf("| 50%%\r\033[01;32m|");
for(j=SIZE-1; j >= 0 ; j--)
{
if(j%2 == 1){
randomLine(base,j);
gsl_linalg_matmult(result,base,result2);
gsl_matrix_memcpy(result,result2);
clearLine(base,j);
if(j % display == 1){
printf("=");
fflush(stdout);
}
}
}
}
}
printf("| 100%% DONE\rUnimodular Generated\033[0m\n");
free(result2);
free(base);
return result;
}
/*! \brief Input: cyphertext as matrices, public key, inverse of public key. Output: Decrypted message as a matrix.
*
* Attempts to decrypt the message only using the public key, used for testing the security of the encryption.
* Performs decryption on a message. This is what Eve does to Bob's encrypted message.
* This is a measure of the encryption strength, hence a property.
*/
gsl_matrix* baddecryptor( gsl_matrix *c, /*!< Cyphertext as a vector */
gsl_matrix *pub, /*!< Public Key */
gsl_matrix *pubinv) /*!< Inverse of Public Key*/
{
gsl_matrix *decryp = gsl_matrix_alloc(1,SIZE);
gsl_linalg_matmult(c,pubinv,decryp);
roundVector(decryp);
return decryp;
}
// matrix multiplication
Matrix Matrix::operator*( const Matrix& other ) const
{
// check dimensions
assert( nCols() == other.nRows() );
// matrix multiplication
Matrix result( nRows(), other.nCols() );
gsl_linalg_matmult( data, other.data, result.data );
return result;
}
void
CameraConf::calcCameraPos() {
RoboCompJointMotor::MotorStateMap motorsstate;
gsl_matrix * init, * motor_pan, * RT_pan;
gsl_matrix * neck, * RT_neck, * motor_tilt, *RT_tilt;
gsl_matrix * cam_right, * RT_cam_right;
gsl_matrix * foa_rel, * foa;
float pan_value, tilt_value;
init = gsl_matrix_alloc(4,4);
motor_pan = gsl_matrix_alloc(4,4);
RT_pan = gsl_matrix_alloc(4,4);
neck = gsl_matrix_alloc(4,4);
RT_neck = gsl_matrix_alloc(4,4);
motor_tilt = gsl_matrix_alloc(4,4);
RT_tilt = gsl_matrix_alloc(4,4);
cam_right = gsl_matrix_alloc(4,4);
RT_cam_right = gsl_matrix_alloc(4,4);
foa_rel = gsl_matrix_alloc(4,1);
foa = gsl_matrix_alloc(4,1);
/*Get motors state*/
jprx->getAllMotorState(motorsstate);
pan_value = motorsstate["neck"].pos;
tilt_value = -motorsstate["tilt"].pos;
/*Initial matrix*/
gsl_matrix_set(init,0,0,1.0);
gsl_matrix_set(init,0,1,0.0);
gsl_matrix_set(init,0,2,0.0);
gsl_matrix_set(init,0,3,INIT_X); //Profundidad
gsl_matrix_set(init,1,0,0.0);
gsl_matrix_set(init,1,1,1.0);
gsl_matrix_set(init,1,2,0.0);
gsl_matrix_set(init,1,3,INIT_Y); //Lateral (izq positivo)
gsl_matrix_set(init,2,0,0.0);
gsl_matrix_set(init,2,1,0.0);
gsl_matrix_set(init,2,2,1.0);
gsl_matrix_set(init,2,3,INIT_Z); //Altura
gsl_matrix_set(init,3,0,0.0);
gsl_matrix_set(init,3,1,0.0);
gsl_matrix_set(init,3,2,0.0);
gsl_matrix_set(init,3,3,1.0);
/*Pan motor turn in Z axis*/
gsl_matrix_set(motor_pan,0,0,cos(pan_value));
gsl_matrix_set(motor_pan,0,1,-sin(pan_value));
gsl_matrix_set(motor_pan,0,2,0.0);
gsl_matrix_set(motor_pan,0,3,0.0); //Profundidad
gsl_matrix_set(motor_pan,1,0,sin(pan_value));
gsl_matrix_set(motor_pan,1,1,cos(pan_value));
gsl_matrix_set(motor_pan,1,2,0.0);
gsl_matrix_set(motor_pan,1,3,0.0); //Lateral (izq positivo)
gsl_matrix_set(motor_pan,2,0,0.0);
gsl_matrix_set(motor_pan,2,1,0.0);
gsl_matrix_set(motor_pan,2,2,1.0);
gsl_matrix_set(motor_pan,2,3,0.0); //Altura
gsl_matrix_set(motor_pan,3,0,0.0);
gsl_matrix_set(motor_pan,3,1,0.0);
gsl_matrix_set(motor_pan,3,2,0.0);
gsl_matrix_set(motor_pan,3,3,1.0);
gsl_linalg_matmult(init, motor_pan, RT_pan);
/*Neck length*/
gsl_matrix_set(neck,0,0,1.0);
gsl_matrix_set(neck,0,1,0.0);
gsl_matrix_set(neck,0,2,0.0);
gsl_matrix_set(neck,0,3,0.0); //Profundidad
gsl_matrix_set(neck,1,0,0.0);
gsl_matrix_set(neck,1,1,1.0);
gsl_matrix_set(neck,1,2,0.0);
gsl_matrix_set(neck,1,3,0.0); //Lateral (izq positivo)
gsl_matrix_set(neck,2,0,0.0);
gsl_matrix_set(neck,2,1,0.0);
gsl_matrix_set(neck,2,2,1.0);
gsl_matrix_set(neck,2,3,NECK_LENGTH); //Altura
gsl_matrix_set(neck,3,0,0.0);
gsl_matrix_set(neck,3,1,0.0);
gsl_matrix_set(neck,3,2,0.0);
gsl_matrix_set(neck,3,3,1.0);
gsl_linalg_matmult(RT_pan, neck, RT_neck);
/*Tilt motor turn in Y axis*/
gsl_matrix_set(motor_tilt,0,0,cos(tilt_value));
gsl_matrix_set(motor_tilt,0,1,0.0);
gsl_matrix_set(motor_tilt,0,2,sin(tilt_value));
gsl_matrix_set(motor_tilt,0,3,0.0); //Profundidad
gsl_matrix_set(motor_tilt,1,0,0.0);
gsl_matrix_set(motor_tilt,1,1,1.0);
gsl_matrix_set(motor_tilt,1,2,0.0);
gsl_matrix_set(motor_tilt,1,3,0.0); //Lateral (izq positivo)
gsl_matrix_set(motor_tilt,2,0,-sin(tilt_value));
gsl_matrix_set(motor_tilt,2,1,0.0);
gsl_matrix_set(motor_tilt,2,2,cos(tilt_value));
gsl_matrix_set(motor_tilt,2,3,0.0); //Altura
gsl_matrix_set(motor_tilt,3,0,0.0);
gsl_matrix_set(motor_tilt,3,1,0.0);
gsl_matrix_set(motor_tilt,3,2,0.0);
gsl_matrix_set(motor_tilt,3,3,1.0);
gsl_linalg_matmult(RT_neck, motor_tilt, RT_tilt);
/*Camera right position*/
gsl_matrix_set(cam_right,0,0,1.0);
gsl_matrix_set(cam_right,0,1,0.0);
gsl_matrix_set(cam_right,0,2,0.0);
gsl_matrix_set(cam_right,0,3,CAM_RIGHT_X); //Profundidad
gsl_matrix_set(cam_right,1,0,0.0);
gsl_matrix_set(cam_right,1,1,1.0);
gsl_matrix_set(cam_right,1,2,0.0);
gsl_matrix_set(cam_right,1,3,CAM_RIGHT_Y); //Lateral (izq positivo)
gsl_matrix_set(cam_right,2,0,0.0);
gsl_matrix_set(cam_right,2,1,0.0);
gsl_matrix_set(cam_right,2,2,1.0);
gsl_matrix_set(cam_right,2,3,CAM_RIGHT_Z); //Altura
gsl_matrix_set(cam_right,3,0,0.0);
gsl_matrix_set(cam_right,3,1,0.0);
gsl_matrix_set(cam_right,3,2,0.0);
gsl_matrix_set(cam_right,3,3,1.0);
gsl_linalg_matmult(RT_tilt, cam_right, RT_cam_right);
/*Calc foa (1 meter in front of the camera*/
gsl_matrix_set(foa_rel,0,0,1.0);
gsl_matrix_set(foa_rel,1,0,0.0);
gsl_matrix_set(foa_rel,2,0,0.0);
gsl_matrix_set(foa_rel,3,0,1.0);
gsl_linalg_matmult(RT_cam_right,foa_rel,foa);
/*Update extrinsecs camera parameters*/
this->camera.position.X=(float)gsl_matrix_get(RT_cam_right,0,3)*1000;
this->camera.position.Y=(float)gsl_matrix_get(RT_cam_right,1,3)*1000;
this->camera.position.Z=(float)gsl_matrix_get(RT_cam_right,2,3)*1000;
this->camera.position.H=1.0;
this->camera.foa.X=(float)gsl_matrix_get(foa,0,0)*1000;
this->camera.foa.Y=(float)gsl_matrix_get(foa,1,0)*1000;
this->camera.foa.Z=(float)gsl_matrix_get(foa,2,0)*1000;
this->camera.foa.H=1.0;
this->camera.roll=0.0;
update_camera_matrix(&(this->camera));
gsl_matrix_free(init);
gsl_matrix_free(motor_pan);
gsl_matrix_free(RT_pan);
gsl_matrix_free(neck);
gsl_matrix_free(RT_neck);
gsl_matrix_free(motor_tilt);
gsl_matrix_free(RT_tilt);
gsl_matrix_free(cam_right);
gsl_matrix_free(RT_cam_right);
gsl_matrix_free(foa_rel);
gsl_matrix_free(foa);
}
int SolveSVD (double a[], double b[], double x[], int neq, int nvar)
{
// get A
gsl_matrix_view av = gsl_matrix_view_array (a, neq, nvar);
if (neq < nvar) { // M < N ... do the transposed matrix
gsl_matrix *atrans = gsl_matrix_alloc (nvar, neq);
gsl_matrix_transpose_memcpy (atrans, &av.matrix);
gsl_matrix *v = gsl_matrix_alloc (neq, neq);
gsl_vector *s = gsl_vector_alloc (neq);
gsl_vector *work = gsl_vector_alloc (neq);
gsl_linalg_SV_decomp (atrans, v, s, work);
// x = A+ b
gsl_matrix *splus = gsl_matrix_calloc (neq, neq);
// compute sigma_plus
for (int i = 0; i < neq; i++) {
double sigma;
if ((sigma = gsl_vector_get (s,i)) != 0.0)
gsl_matrix_set (splus, i,i, 1.0/sigma);
}
gsl_linalg_matmult (atrans, splus, atrans);
gsl_linalg_matmult_mod (atrans, GSL_LINALG_MOD_NONE, v, GSL_LINALG_MOD_TRANSPOSE, atrans);
gsl_vector_view bv = gsl_vector_view_array (b, neq);
gsl_matrix_view bmv = gsl_matrix_view_vector (&bv.vector, neq, 1);
gsl_matrix *xx = gsl_matrix_alloc (nvar,1);
gsl_linalg_matmult (atrans, &bmv.matrix, xx);
// gsl_matrix_fprintf (stdout, xx, "%g");
for (int i = 0; i < nvar; i++) {
x[i] = gsl_matrix_get(xx,i,0);
}
gsl_matrix_free (splus); gsl_matrix_free (xx);
gsl_matrix_free (atrans);
gsl_matrix_free (v); gsl_vector_free (s); gsl_vector_free (work);
} else { // M >= N
gsl_matrix *v = gsl_matrix_alloc (nvar, nvar);
gsl_vector *s = gsl_vector_alloc (nvar);
gsl_vector *work = gsl_vector_alloc (nvar);
gsl_linalg_SV_decomp (&av.matrix, v, s, work);
// x = A+ b
gsl_vector_view bv = gsl_vector_view_array (b, neq);
gsl_vector *xx = gsl_vector_alloc (nvar);
gsl_linalg_SV_solve (&av.matrix, v, s, &bv.vector, xx);
// gsl_vector_fprintf (stdout, xx, "%g");
for (int i = 0; i < nvar; i++)
x[i] = gsl_vector_get (xx, i);
gsl_vector_free (xx);
gsl_matrix_free (v); gsl_vector_free (s); gsl_vector_free (work);
}
return 1;
}
int
gsl_linalg_matmult_mod (const gsl_matrix * A, gsl_linalg_matrix_mod_t modA,
const gsl_matrix * B, gsl_linalg_matrix_mod_t modB,
gsl_matrix * C)
{
if (modA == GSL_LINALG_MOD_NONE && modB == GSL_LINALG_MOD_NONE)
{
return gsl_linalg_matmult (A, B, C);
}
else
{
size_t dim1_A = A->size1;
size_t dim2_A = A->size2;
size_t dim1_B = B->size1;
size_t dim2_B = B->size2;
size_t dim1_C = C->size1;
size_t dim2_C = C->size2;
if (modA & GSL_LINALG_MOD_TRANSPOSE)
SWAP_SIZE_T (dim1_A, dim2_A);
if (modB & GSL_LINALG_MOD_TRANSPOSE)
SWAP_SIZE_T (dim1_B, dim2_B);
if (dim2_A != dim1_B || dim1_A != dim1_C || dim2_B != dim2_C)
{
GSL_ERROR ("matrix sizes are not conformant", GSL_EBADLEN);
}
else
{
double a, b;
double temp;
size_t i, j, k;
size_t a1, a2, b1, b2;
for (i = 0; i < dim1_C; i++)
{
for (j = 0; j < dim2_C; j++)
{
a1 = i;
a2 = 0;
b1 = 0;
b2 = j;
if (modA & GSL_LINALG_MOD_TRANSPOSE)
SWAP_SIZE_T (a1, a2);
if (modB & GSL_LINALG_MOD_TRANSPOSE)
SWAP_SIZE_T (b1, b2);
a = gsl_matrix_get (A, a1, a2);
b = gsl_matrix_get (B, b1, b2);
temp = a * b;
for (k = 1; k < dim2_A; k++)
{
a1 = i;
a2 = k;
b1 = k;
b2 = j;
if (modA & GSL_LINALG_MOD_TRANSPOSE)
SWAP_SIZE_T (a1, a2);
if (modB & GSL_LINALG_MOD_TRANSPOSE)
SWAP_SIZE_T (b1, b2);
a = gsl_matrix_get (A, a1, a2);
b = gsl_matrix_get (B, b1, b2);
temp += a * b;
}
gsl_matrix_set (C, i, j, temp);
}
}
return GSL_SUCCESS;
}
}
}
void Module_DLT::rq_decomp(double* solucion,
gsl_matrix* R_prima,
gsl_matrix* Q_prima,
gsl_vector* x
){
/*
int i, j, lotkin_signum, frank_signum;
int DIM = 3;
gsl_matrix *lotkin_a, *frank_a;
gsl_vector *x, *lotkin_b, *frank_b, *lotkin_x, *frank_x;
gsl_vector *lotkin_tau, *frank_tau;
/* allocate a, x, b
lotkin_a = gsl_matrix_alloc(DIM, DIM);
frank_a = gsl_matrix_alloc(DIM, DIM);
x = gsl_vector_alloc(DIM);
lotkin_b = gsl_vector_alloc(DIM);
frank_b = gsl_vector_alloc(DIM);
lotkin_x = gsl_vector_alloc(DIM);
frank_x = gsl_vector_alloc(DIM);
/* set x = [1 2 ... DIM]
for(i = 0; i < DIM; i++)
gsl_vector_set(x, i, (double)i);
/* set Lotkin matrix */
/* a_ij = 1 (i = 1) or 1/(i+j-1) (i != 1)
for(i = 0; i < DIM; i++)
gsl_matrix_set(lotkin_a, 0, i, 1.0);
for(i = 1; i < DIM; i++)
for(j = 0; j < DIM; j++)
gsl_matrix_set(lotkin_a, i, j, 1.0 / (double)(i + j + 1));
/* set Frank matrix
/* a_ij = DIM - min(i,j) + 1
for(i = 0; i < DIM; i++)
for(j = 0; j < DIM; j++)
gsl_matrix_set(frank_a, i, j, (double)DIM - (double)GSL_MAX(i, j) );
*/
/* set A matrix
gsl_matrix_set(lotkin_a, 0, 0, 12);
gsl_matrix_set(lotkin_a, 0, 1, 6);
gsl_matrix_set(lotkin_a, 0, 2, -4);
gsl_matrix_set(lotkin_a, 1, 0, -51);
gsl_matrix_set(lotkin_a, 1, 1, 167);
gsl_matrix_set(lotkin_a, 1, 2, 24);
gsl_matrix_set(lotkin_a, 2, 0, 4);
gsl_matrix_set(lotkin_a, 2, 1, -68);
gsl_matrix_set(lotkin_a, 2, 2, -41);
/* Print matrix
for(i = 0; i < DIM; i++)
{
printf("%3d: ", i);
for(j = 0; j < DIM; j++)
printf("%g ", gsl_matrix_get(lotkin_a, i, j));
printf("\n");
}
printf("\n");
/* b = A * x
gsl_blas_dgemv(CblasNoTrans, 1.0, lotkin_a, x, 0.0, lotkin_b);
/* QR decomposition and solve
lotkin_tau = gsl_vector_alloc(DIM);
gsl_linalg_QR_decomp(lotkin_a, lotkin_tau);
gsl_linalg_QR_solve(lotkin_a, lotkin_tau, lotkin_b, lotkin_x);
gsl_vector_free(lotkin_tau);
/* Print solution matrix
for(i = 0; i < DIM; i++)
{
printf("%3d: ", i);
for(j = 0; j < DIM; j++)
printf("%g ", gsl_matrix_get(lotkin_a, i, j));
printf("\n");
}
printf("\n");
for(i = 0; i < DIM; i++)
{
printf("%3d: ", i);
for(j = 0; j < DIM; j++)
//printf("%g ", gsl_vector_get(lotkin_x, i, j));
printf("\n");
}
/* free a, x, b
gsl_matrix_free(lotkin_a);
gsl_vector_free(x);
gsl_vector_free(lotkin_b);
gsl_vector_free(lotkin_x);
*/
/*
gsl_matrix* C = gsl_matrix_alloc(3,3);
/* Compute C = A B
gsl_blas_dgemm (CblasNoTrans, CblasNoTrans,
1.0, R_prima, Q_prima,
0.0, C);
camera->rt11 = gsl_matrix_get(C, 0, 0);
camera->rt12 = gsl_matrix_get(C, 0, 1);
camera->rt13 = gsl_matrix_get(C, 0, 2);
camera->rt21 = gsl_matrix_get(C, 1, 0);
camera->rt22 = gsl_matrix_get(C, 1, 1);
camera->rt23 = gsl_matrix_get(C, 1, 2);
camera->rt31 = gsl_matrix_get(C, 2, 0);
camera->rt32 = gsl_matrix_get(C, 2, 1);
camera->rt33 = gsl_matrix_get(C, 2, 2);
camera->rt41 = 0;
camera->rt42 = 0;
camera->rt43 = 0;
camera->rt44 = 1;
**/
std::cout << "RQ_Decomp" << std::endl;
int n,mm,s,signum ;
gsl_matrix *M,*Q,*R;
gsl_vector* tau;
double tmp,det;
/* para invertir las matriz M,Q,R */
gsl_permutation* p = gsl_permutation_alloc (3);
gsl_permutation* p2 = gsl_permutation_alloc (3);
gsl_permutation* p3 = gsl_permutation_alloc (3);
gsl_matrix* M_prima = gsl_matrix_alloc(3,3);
gsl_matrix* Q_prima_tmp = gsl_matrix_alloc(3,3);
/* para resolver el centro de la camara usando Mx=C
donde C es el verctor p4 de la matriz P */
gsl_vector* p4 = gsl_vector_alloc(3);
gsl_matrix* temp = gsl_matrix_alloc(3,3);
gsl_matrix* I_C = gsl_matrix_alloc(3,4);
gsl_matrix* test = gsl_matrix_alloc(3,4);
M = gsl_matrix_alloc(3,3);
Q = gsl_matrix_alloc(3,3);
R = gsl_matrix_alloc(3,3);
tau = gsl_vector_alloc(3);
/* Copiamos la submatriz 3x3 Izq de la solucion P a la matriz M */
gsl_matrix_set(M,0,0,solucion[0]);
gsl_matrix_set(M,0,1,solucion[1]);
gsl_matrix_set(M,0,2,solucion[2]);
gsl_matrix_set(M,1,0,solucion[4]);
gsl_matrix_set(M,1,1,solucion[5]);
gsl_matrix_set(M,1,2,solucion[6]);
gsl_matrix_set(M,2,0,solucion[8]);
gsl_matrix_set(M,2,1,solucion[9]);
gsl_matrix_set(M,2,2,solucion[10]);
/* Copiamos el vector p4 */
gsl_vector_set(p4,0,solucion[3]);
gsl_vector_set(p4,1,solucion[7]);
gsl_vector_set(p4,2,solucion[11]);
/* invertimos la matriz M */
gsl_linalg_LU_decomp (M, p, &s);
gsl_linalg_LU_solve(M,p,p4,x);
gsl_linalg_LU_invert (M, p, M_prima);
/* Hacemos una descomposicion a la matriz M invertida */
gsl_linalg_QR_decomp (M_prima,tau);
gsl_linalg_QR_unpack (M_prima,tau,Q,R);
/* Invertimos R */
gsl_linalg_LU_decomp (R, p2, &s);
gsl_linalg_LU_invert (R, p2, R_prima);
/* Invertimos Q */
gsl_linalg_LU_decomp (Q, p3, &s);
gsl_linalg_LU_invert (Q, p3, Q_prima);
gsl_matrix_memcpy(Q_prima_tmp, Q_prima);
std::cout << "Calculamos" << std::endl;
if (DEBUG) {
/** checking results:
If the rq decompsition is correct we should obtain
the decomposed matrix:
orig_matrix = K*R*T
where T = (I|C)
*/
gsl_matrix_set(I_C,0,3,gsl_vector_get(x,0));
gsl_matrix_set(I_C,1,3,gsl_vector_get(x,1));
gsl_matrix_set(I_C,2,3,gsl_vector_get(x,2));
gsl_matrix_set(I_C,0,0,1);
gsl_matrix_set(I_C,0,1,0);
gsl_matrix_set(I_C,0,2,0);
gsl_matrix_set(I_C,1,0,0);
gsl_matrix_set(I_C,1,1,1);
gsl_matrix_set(I_C,1,2,0);
gsl_matrix_set(I_C,2,0,0);
gsl_matrix_set(I_C,2,1,0);
gsl_matrix_set(I_C,2,2,1);
gsl_linalg_matmult(R_prima,Q_prima,temp);
gsl_linalg_matmult(temp,I_C,test);
printf(" Result -> \n");
for (n=0; n<3; n++){
// for (mm=0; mm<4; mm++){
for (mm=0; mm<3; mm++){
printf(" %g \t",gsl_matrix_get(temp,n,mm));
// se debe sacar test
}
printf("\n");
}
}
/* El elemento (3,3) de la matriz R tiene que ser 1
para ello tenemos que normalizar la matriz dividiendo
entre este elemento
*/
tmp = gsl_matrix_get(R_prima,2,2);
for (n=0; n<3; n++)
for (mm=0; mm<3; mm++){
gsl_matrix_set(R_prima,n,mm, gsl_matrix_get(R_prima,n,mm)/tmp);
}
/* Si obtenemos valores negativos en la
diagonal de K tenemos que cambiar de signo la columna de K y la fila de Q
correspondiente
*/
if (DEBUG)
print_matrix(R_prima);
if (DEBUG)
print_matrix(Q_prima);
if (gsl_matrix_get(R_prima,0,0)<0){
if (DEBUG) printf(" distancia focat 0,0 negativa\n");
gsl_matrix_set(R_prima,0,0,
abs(gsl_matrix_get(R_prima,0,0))
);
for (n=0;n<3;n++)
gsl_matrix_set(Q_prima,0,n,
gsl_matrix_get(Q_prima,0,n)*-1
);
}
if (DEBUG) printf("R_prima\n");
print_matrix(R_prima);
if (DEBUG) printf("Q_prima\n");
print_matrix(Q_prima);
if (gsl_matrix_get(R_prima,1,1)<0){
if (DEBUG) printf(" distancia focal 1,1 negativa\n");
for (n=0;n<3;n++){
gsl_matrix_set(Q_prima,1,n,
gsl_matrix_get(Q_prima,1,n)*-1
);
gsl_matrix_set(R_prima,n,1,
gsl_matrix_get(R_prima,n,1)*-1
);
}
}
if (DEBUG) printf("R_prima\n");
print_matrix(R_prima);
if (DEBUG) printf("Q_prima\n");
print_matrix(Q_prima);
/*Finalmente, si Q queda con determinante -1 cambiamos de signo
todos sus elementos para obtener una rotación sin "reflexion".
NOTA: Este trozo de codigo lo he desactivado debido a que si lo
hacemos obtenemos una orientacion equivocada a la hora de dibujarla
con OGL
*/
gsl_linalg_LU_decomp (Q_prima_tmp, p3, &s);
signum=1;
det = gsl_linalg_LU_det(Q_prima_tmp,signum);
if (-1 == det && 0){
if (DEBUG) printf("Q has a negatif det");
for (n=0;n<3;n++)
for (mm=0;mm<3;mm++)
gsl_matrix_set(Q_prima,n,mm,gsl_matrix_get(Q_prima,n,mm)*-1);
}
}
