utils.c (5561B)
1 #include <stdio.h> 2 #include <stdlib.h> 3 #include <math.h> 4 #include "utils.h" 5 6 /* Factorial for Taylor series */ 7 static double factorial(int n) { 8 if (n == 0 || n == 1) return 1; 9 return n * factorial(n - 1); 10 } 11 12 /* Sine function using Taylor series */ 13 double Sin 14 (double x) 15 { 16 double sum = 0; 17 int n; 18 x = fmod(x, 2 * PI); /* Reduce x to [0, 2π) */ 19 for (n = 0; n < 10; ++n) { 20 sum += pow(-1, n) * pow(x, 2*n+1) / factorial(2*n+1); 21 } 22 return sum; 23 } 24 25 /* Cosine function using Taylor series */ 26 double 27 Cos(double x) { 28 double sum = 0; 29 int n; 30 x = fmod(x, 2 * PI); /* Reduce x to [0, 2π) */ 31 for (n = 0; n < 10; ++n) { 32 sum += pow(-1, n) * pow(x, 2*n) / factorial(2*n); 33 } 34 return sum; 35 } 36 37 /* Matrix multiplication */ 38 void 39 matmul(double A[3][3], double B[3][3], double result[3][3]) 40 { 41 for (int i = 0; i < 3; i++) { 42 for (int j = 0; j < 3; j++) { 43 result[i][j] = 0; 44 for (int k = 0; k < 3; k++) { 45 result[i][j] += A[i][k] * B[k][j]; 46 } 47 } 48 } 49 } 50 51 /* Vector-matrix multiplication */ 52 void 53 dot_product(double result[3], double A[3][3], double B[3]) 54 { 55 for(int i = 0; i < 3; i++) { 56 result[i] = 0; 57 for(int j = 0; j < 3; j++) { 58 result[i] += A[i][j] * B[j]; 59 } 60 } 61 } 62 63 /* Matrix transposition */ 64 void 65 transpose(double A[3][3], double Ai[3][3]) 66 { 67 for (int j = 0; j < 3; j++) { 68 for (int k = 0; k < 3; k++) { 69 Ai[j][k] = A[k][j]; 70 } 71 } 72 } 73 74 /* Display matrix in row or column major order */ 75 void 76 dispmat(double *A, int rows, int cols, bool transpose) 77 { 78 if (transpose) { 79 for (int j = 0; j < cols; j++) { 80 printf("%4d ", j + 1); 81 for (int i = 0; i < rows; i++) { 82 printf("%10.2e", *(A + i * cols + j)); 83 } 84 printf("\n"); 85 } 86 } else { 87 for (int i = 0; i < rows; i++) { 88 for (int j = 0; j < cols; j++) { 89 printf("%10.2e", *(A + i * cols + j)); 90 } 91 printf("\n"); 92 } 93 } 94 } 95 96 /* Static variables for pivot */ 97 static int *pivot = NULL; 98 99 /* LU Decomposition */ 100 int LUDecompose(double *amat, int n, int numcols) { 101 if (pivot != NULL) free(pivot); 102 pivot = malloc(n * sizeof(int)); 103 if (!pivot) { 104 fprintf(stderr, "Error in LUDecompose - malloc\n"); 105 return -2; 106 } 107 108 for (int i = 0; i < n; i++) { 109 pivot[i] = i; 110 } 111 112 int sign = 1; 113 double *dptr1, *dptr2, dtmp1; 114 115 for (int i = 0; i < n-1; i++) { 116 dptr1 = amat + i * numcols + i; 117 int k = i; 118 for (int j = i + 1; j < n; j++) { 119 dptr2 = amat + j * numcols + i; 120 if (fabs(*dptr2) > fabs(*dptr1)) { 121 dptr1 = dptr2; 122 k = j; 123 } 124 } 125 126 if (fabs(*dptr1) < SMALLEST_PIVOT) { 127 fprintf(stderr, "Error in LUDecompose - matrix is singular\n"); 128 return -1; 129 } 130 131 if (k != i) { 132 int temp = pivot[i]; pivot[i] = pivot[k]; pivot[k] = temp; 133 sign *= -1; 134 for (int j = 0; j < n; j++) { 135 dtmp1 = amat[i * numcols + j]; 136 amat[i * numcols + j] = amat[k * numcols + j]; 137 amat[k * numcols + j] = dtmp1; 138 } 139 } 140 141 for (int j = i+1; j < n; j++) { 142 dtmp1 = amat[j * numcols + i] / *dptr1; 143 amat[j * numcols + i] = dtmp1; 144 for (int k = i+1; k < n; k++) { 145 amat[j * numcols + k] -= dtmp1 * amat[i * numcols + k]; 146 } 147 } 148 } 149 /*pivot[n-1] = sign;*/ 150 *(pivot+n-1) = sign; 151 return 1; 152 } 153 154 /* Solve system with LU decomposition */ 155 void LUSolve(double *amat, double *b, int n, int numcols) { 156 double dtmp1; 157 int i, j; 158 159 for (i = 0; i < n; i++) { 160 j = pivot[i]; 161 if (j != i) { 162 dtmp1 = b[i]; b[i] = b[j]; b[j] = dtmp1; 163 } 164 } 165 166 for (i = 0; i < n; i++) { 167 dtmp1 = b[i]; 168 for (j = 0; j < i; j++) { 169 dtmp1 -= amat[i * numcols + j] * b[j]; 170 } 171 b[i] = dtmp1; 172 } 173 174 for (i = n-1; i >= 0; i--) { 175 dtmp1 = b[i]; 176 for (j = n-1; j > i; j--) { 177 dtmp1 -= amat[i * numcols + j] * b[j]; 178 } 179 b[i] = dtmp1 / amat[i * numcols + i]; 180 } 181 } 182 183 /* Compute determinant from LU decomposition */ 184 double LUDeterminant(double *amat, int n, int numcols) { 185 double det = 1.0; 186 for (int i = 0; i < n; i++) { 187 det *= amat[i * (numcols + 1)]; 188 } 189 return det * pivot[n - 1]; 190 }