nurb_solve.c
Go to the documentation of this file.
1 /* N U R B _ S O L V E . C
3  *
4  * Copyright (c) 1983-2014 United States Government as represented by
5  * the U.S. Army Research Laboratory.
6  *
7  * This library is free software; you can redistribute it and/or
8  * modify it under the terms of the GNU Lesser General Public License
10  *
11  * This library is distributed in the hope that it will be useful, but
12  * WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14  * Lesser General Public License for more details.
15  *
16  * You should have received a copy of the GNU Lesser General Public
17  * License along with this file; see the file named COPYING for more
18  * information.
19  */
21 /** @{ */
22 /** @file primitives/bspline/nurb_solve.c
23  *
24  * Decompose a matrix into its LU decomposition using pivoting.
25  *
26  * These Procedures take a set of matrices of the form Ax = b and
27  * allows one to solve the system by various means. The
28  * rt_nurb_doolittle routine takes the system and creates a lu
29  * decomposition using pivoting to get the system in a desired
30  * form. Forward and backward substitution are then used to solve the
31  * system. All work is done in place.
32  *
33  */
34 /** @} */
35
36 #include "common.h"
37
38 #include <math.h>
39 #include "bio.h"
40
41 #include "vmath.h"
42 #include "raytrace.h"
43 #include "nurb.h"
44
45
46 /**
47  * Solve must be passed two matrices that are of the form of pointer
48  * to double arrays. mat_1 is the n by n matrix that is to be
49  * decomposed and mat_2 is the matrix that contains the left side of
50  * the equation. The variable solution which is double is also to be
51  * created by the user and consisting of n elements of which the
52  * solution set is passed back.
53  *
54  * Arguments mat_1 and mat_2 are modified by this call.
55  * The solution is written into the solution[] array.
56  */
57 void
58 rt_nurb_solve(fastf_t *mat_1, fastf_t *mat_2, fastf_t *solution, int dim, int coords)
59  /* A and b array of the system Ax= b*/
60
61
62  /* dimension of the matrix */
63  /* Number of coordinates for mat_2 and solution */
64 {
65  register int i, k;
66  fastf_t *y;
67  fastf_t * b;
68  fastf_t * s;
69
70  y = (fastf_t *) bu_malloc(sizeof (fastf_t) * dim,
71  "rt_nurb_solve: y");/* Create temp array */
72
73  b = (fastf_t *) bu_malloc(sizeof (fastf_t) * dim,
74  "rt_nurb_solve: b");/* Create temp array */
75
76  s = (fastf_t *) bu_malloc(sizeof (fastf_t) * dim,
77  "rt_nurb_solve: s");/* Create temp array */
78
79  rt_nurb_doolittle (mat_1, mat_2, dim, coords);/* Create LU decomposition */
80
81  for (k =0; k < coords; k++) {
82  fastf_t * ptr;
83
84  ptr = mat_2 + k;
85
86  for (i = 0; i < dim; i++) {
87  b[i] = *ptr;
88  ptr += coords;
89  }
90
91  /* Solve the system Ly =b */
92  rt_nurb_forw_solve (mat_1, b, y, dim);
93
94  /* Solve the system Ux = y */
95  rt_nurb_back_solve (mat_1, y, s, dim);
96
97
98  ptr = solution + k;
99  for (i=0; i < dim; i++) {
100  *ptr = s[i];
101  ptr += coords;
102  }
103  }
104
105  bu_free ((char *)y, "rt_nurb_solve: y"); /* Free up storage */
106  bu_free ((char *)b, "rt_nurb_solve: b"); /* Free up storage */
107  bu_free ((char *)s, "rt_nurb_solve: s"); /* Free up storage */
108 }
109
110
111 /**
112  * Create LU decomposition.
113  * Modifies both mat_1 and mat_2 values.
114  */
115 void
116 rt_nurb_doolittle(fastf_t *mat_1, fastf_t *mat_2, int row, int coords)
117 {
118  register int i;
119  register int j;
120  register int k;
121  register int x;
122  int m;
123  register fastf_t *d; /* Scaling factors */
124  register fastf_t *s; /* vector for swapping if needed */
125  register fastf_t *ds; /* See if swapping is needed */
126  fastf_t maxd;
127  fastf_t tmp;
128
129  int max_pivot;
130
131  d = (fastf_t *) bu_malloc(sizeof (fastf_t) * row,
132  "rt_nurb_doolittle:d"); /* scale factor */
133  s = (fastf_t *) bu_malloc(sizeof (fastf_t) * row * row,
134  "rt_nurb_doolittle:s"); /* vector to check */
135  ds = (fastf_t *) bu_malloc(sizeof (fastf_t) * row,
136  "rt_nurb_doolittle:ds"); /* if rows need to be swapped */
137
138  for (i = 0; i < row; i++) {
139  /* calculate the scaling factors */
140  maxd = 0.0;
141  for (j = 0; j < row; j++) {
142  if (maxd < fabs(mat_1[i * row + j]))
143  maxd = fabs(mat_1[i * row + j]);
144  }
145  d[i] = 1.0 / maxd;
146  }
147
148  for (k = 0; k < row; k++) {
149  for (i = k; i < row; i++) {
150  tmp = 0.0;
151  for (j = 0; j <= k -1; j ++)
152  tmp += mat_1[i * row + j ] * mat_1[j * row + k];
153  s[i * row + k] = mat_1[i * row + k] - tmp;
154  }
155
156  max_pivot = k;
157
158  for (i = k; i < row; i ++) {
159  /* check to see if rows need to be swapped */
160  ds[i] = d[i] * s[ i * row + k];
161  if (ds[max_pivot] < ds[i])
162  max_pivot = i;
163  }
164
165  if (max_pivot != k) {
166  /* yes; swap row k with row max_pivot */
167  for (m = 0; m < row; m++) {
168  tmp = mat_1[k * row + m];
169  mat_1[k * row + m] = mat_1[max_pivot * row + m];
170  mat_1[max_pivot * row + m] = tmp;
171  }
172
173  for (x = 0; x < coords; x++) {
174  tmp = mat_2[k*coords + x]; /* b matrix also */
175  mat_2[k*coords+x] = mat_2[max_pivot*coords+x];
176  mat_2[max_pivot*coords+x] = tmp;
177  }
178
179  tmp = s[k * row + k]; /* swap s vector */
180  s[k * row + k] = s[max_pivot * row + k];
181  s[max_pivot * row + k] = tmp;
182  }
183
184  mat_1[ k * row + k] = s[k * row + k]; /* mat_1[k][k] */
185
186  for (i = k + 1; i < row; i++) /* lower matrix */
187  mat_1[i * row + k] = (float)(s[i* row + k] / s[k* row +k]);
188
189  for (j = k + 1; j < row; j++) {
190  /* upper matrix */
191  tmp = 0;
192  for (i = 0; i <= k - 1; i++)
193  tmp += mat_1[ k * row + i] * mat_1[ i* row + j];
194
195  mat_1[ k * row + j] -= tmp;
196  }
197
198  }
199  bu_free((char *)d, "rt_nurb_doolittle:d"); /* Free up the storage. */
200  bu_free((char *)s, "rt_nurb_doolittle:s");
201  bu_free((char *)ds, "rt_nurb_doolittle:ds");
202 }
203
204
205 void
206 rt_nurb_forw_solve(const fastf_t *lu, const fastf_t *b, fastf_t *y, int n) /* spl_solve lower triangular matrix */
207
208
209 {
210  register int i, j;
211  fastf_t tmp;
212
213  for (i = 0; i < n; i++) {
214  tmp = 0.0;
215  for (j = 0; j <= i - 1; j++)
216  tmp += lu[i*n + j] * y[j];
217  y[i] = b[i] - tmp;
218  }
219 }
220
221
222 void
223 rt_nurb_back_solve(const fastf_t *lu, const fastf_t *y, fastf_t *x, int n) /* spl_solve upper triangular matrix */
224
225
226 {
227  register int i, j;
228  fastf_t tmp;
229
230  for (i = n - 1; i >= 0; i--) {
231  tmp = 0.0;
232  for (j = i + 1; j < n; j++)
233  tmp += lu[i*n + j] * x[j];
234  x[i] = (y[i] - tmp) / lu[i * n + i];
235  }
236
237 }
238
239
240 void
241 rt_nurb_p_mat(const fastf_t *mat, int dim)
242 {
243  int i;
244
245  for (i = 0; i < dim; i++)
246  fprintf(stderr, "%f\n", mat[i]);
247  fprintf(stderr, "\n");
248 }
249
250
251 /*
252  * Local Variables:
253  * mode: C
254  * tab-width: 8
255  * indent-tabs-mode: t
256  * c-file-style: "stroustrup"
257  * End:
258  * ex: shiftwidth=4 tabstop=8
259  */
if lu s
Definition: nmg_mod.c:3860
lu
Definition: nmg_mod.c:3855
void rt_nurb_back_solve(const fastf_t *lu, const fastf_t *y, fastf_t *x, int n)
Definition: nurb_solve.c:223
void * bu_malloc(size_t siz, const char *str)
Definition: malloc.c:314
void rt_nurb_doolittle(fastf_t *mat_1, fastf_t *mat_2, int row, int coords)
Definition: nurb_solve.c:116
void rt_nurb_forw_solve(const fastf_t *lu, const fastf_t *b, fastf_t *y, int n)
Definition: nurb_solve.c:206
void rt_nurb_p_mat(const fastf_t *mat, int dim)
Definition: nurb_solve.c:241
void bu_free(void *ptr, const char *str)
Definition: malloc.c:328
double fastf_t
Definition: defines.h:300
void rt_nurb_solve(fastf_t *mat_1, fastf_t *mat_2, fastf_t *solution, int dim, int coords)
Definition: nurb_solve.c:58