void Matrix3by3Inverse( double** M, double** M_inv ) {
double determinant = 0; /* Variable for the matrix determinant of M */
double C_ji; /* Co-factor C_ji of element M_ji, related to M_inv by: M_inv_ij = C_ji/det(M) */
double det_inv; /* Variable for the inverse of the determinant */
double epsilon = 0.0001; /* Acceptable tolerance for concluding that a determinant equals 0 (and hence that M is non-invertible) */
/* Compute the determinant */
for ( int i = 0; i < 3; i++ ) {
determinant = determinant + M[0][i] * ( M[1][(i+1)%3] * M[2][(i+2)%3] - M[1][(i+2)%3] * M[2][(i+1)%3] );
}
/* If the determinant is too small, abort */
assert( determinant > epsilon );
det_inv = 1.0 / determinant;
/* Find the inverse of the matrix */
for ( int i = 0; i < 3; i++ ) {
for ( int j = 0; j < 3; j++ ) {
C_ji = ( M[(j+1)%3][(i+1)%3] * M[(j+2)%3][(i+2)%3] ) - ( M[(j+1)%3][(i+2)%3] * M[(j+2)%3][(i+1)%3] );
M_inv[i][j] = C_ji * det_inv;
}
}
/* Check that the identity matrix results for M*M^-1 */
double** Product = Make2DDoubleArray(3,3);
MatrixMultiply(M,M_inv,3,3,3,Product);
for ( int i = 0; i < 3; i++ ) {
for ( int j = 0; j < 3; j++ ) {
if (i == j) assert( fabs(Product[i][j] - 1) < epsilon );
else assert( fabs(Product[i][j]) < epsilon );
}
}
free(Product);
}
int main(){
int i,j,rozmiara,rozmiarb;
char a;
printf("Podaj rozmiar tablicy xn: \n");
scanf("%d",&rozmiara);
printf("Podaj rozmiar tablicy yn: \n");
scanf("%d",&rozmiarb);
double** myArray = Make2DDoubleArray(rozmiara, rozmiarb);
for ( i = 0; i < rozmiara; i++ )
{
for ( j = 0; j < rozmiarb; j++ )
{
printf("a[%d][%d] podaj wartosc\n", i,j);
scanf("%f",&myArray[i][j]);
}
}
printf("Podaj rozmiar tablicy yn: %d\n",sizeof(myArray));
scanf("%d",&rozmiarb);
return 0;
}
void FindRotMat( double* v, double* v_out, double** R ) {
double a[3], a_prime[3], b[3], b_prime[3], c[3], c_norm; /* a = v_hat, a' = v_out_hat, c = c' is perp to both, and b, b' each complete the RHR */
double** R1 = Make2DDoubleArray(3,3); /* The matrix R1 rotates from the reference frame to a frame x-aligned with v */
double** R1_inv = Make2DDoubleArray(3,3); /* Inverse of R1 */
double** R3 = Make2DDoubleArray(3,3); /* The matrix R3 rotates from the reference frame to a frame x-aligned with v_out */
double v_norm = Norm(v,3,2.0); /* Magnitude of v */
double v_out_norm = Norm(v_out,3,2.0); /* Magnitude of v_out */
double epsilon = 0.0001; /* Tolerance for testing non-zero values of v and v_out (for testing degenerate cases) */
/* Make sure v and v_out are non-zero vectors */
assert( fabs(v[0]) > epsilon || fabs(v[1]) > epsilon || fabs(v[2]) > epsilon );
assert( fabs(v_out[0]) > epsilon || fabs(v_out[1]) > epsilon || fabs(v_out[2]) > epsilon );
/* Check if v and v_out are parallel. If so, output the identity matrix. If not, continue. */
if ( fabs( (v[0]*v_out[0] + v[1]*v_out[1] + v[2]*v_out[2])/(v_norm*v_out_norm) - 1 ) < epsilon ) {
IdentityMatrix( R, 3 );
}
else {
/* Define a and a' */
for (int i = 0; i < 3; i++) {
a[i] = v[i]/v_norm;
a_prime[i] = v_out[i]/v_out_norm;
}
/* Define c and c' (c' = c). If v and v_out are anti-parallel, use SVD instead of the cross product to find an orthogonal vector. */
if ( fabs( a[0]*a_prime[0] + a[1]*a_prime[1] + a[2]*a_prime[2] + 1 ) < epsilon ) {
if (fabs(a[0]) > epsilon) {
c[2] = pow( 1.0/ (1.0 + (a[2]/a[0])), 0.5 );
c[0] = -(a[2]/a[0])*c[2];
c[1] = 0;
}
else if (fabs(a[1]) > epsilon) {
c[0] = 0;
c[2] = pow( 1.0/ (1.0 + (a[2]/a[1])), 0.5 );
c[1] = -(a[2]/a[1])*c[2];
}
else {
c[0] = 0;
c[1] = pow( 1.0/ (1.0 + (a[1]/a[2])), 0.5 );
c[2] = -(a[1]/a[2])*c[1];
}
}
else {
Cross(a,a_prime,c);
c_norm = Norm(c,3,2.0);
for (int i = 0; i < 3; i++) {
c[i] = c[i]/c_norm;
}
}
/* Define b and b' (no normalization needed since c _|_ a and c' _|_ a' by definition) */
Cross(c,a,b);
Cross(c,a_prime,b_prime);
/* Define R1 = [a, b, c] and R3 = [a', b', c'] */
for (int i = 0; i < 3; i++) {
R1[i][0] = a[i];
R1[i][1] = b[i];
R1[i][2] = c[i];
R3[i][0] = a_prime[i];
R3[i][1] = b_prime[i];
R3[i][2] = c[i];
}
/* Now, R = R3*R1^-1 */
Matrix3by3Inverse(R1, R1_inv);
MatrixMultiply(R3, R1_inv, 3, 3, 3, R);
}
for (int i = 2; i >= 0; i--) {
free(R1[i]); free(R1_inv[i]); free(R3[i]);
}
free(R1); free(R1_inv); free(R3);
}
