b311480e |
1 | // Copyright (c) 1997-1999 Matra Datavision |
973c2be1 |
2 | // Copyright (c) 1999-2014 OPEN CASCADE SAS |
b311480e |
3 | // |
973c2be1 |
4 | // This file is part of Open CASCADE Technology software library. |
b311480e |
5 | // |
d5f74e42 |
6 | // This library is free software; you can redistribute it and/or modify it under |
7 | // the terms of the GNU Lesser General Public License version 2.1 as published |
973c2be1 |
8 | // by the Free Software Foundation, with special exception defined in the file |
9 | // OCCT_LGPL_EXCEPTION.txt. Consult the file LICENSE_LGPL_21.txt included in OCCT |
10 | // distribution for complete text of the license and disclaimer of any warranty. |
b311480e |
11 | // |
973c2be1 |
12 | // Alternatively, this file may be used under the terms of Open CASCADE |
13 | // commercial license or contractual agreement. |
b311480e |
14 | |
7fd59977 |
15 | #include <stdio.h> |
16 | |
3b010a74 |
17 | #include <math_Vector.hxx> |
7fd59977 |
18 | #include <math_Matrix.hxx> |
19 | |
20 | #include <Standard_DimensionError.hxx> |
21 | #include <Standard_DivideByZero.hxx> |
22 | #include <Standard_RangeError.hxx> |
23 | #include <Standard_NullValue.hxx> |
24 | |
3b010a74 |
25 | math_Vector::math_Vector(const Standard_Integer theLower, const Standard_Integer theUpper) : |
26 | LowerIndex(theLower), |
27 | UpperIndex(theUpper), |
28 | Array(theLower,theUpper) |
7fd59977 |
29 | { |
3b010a74 |
30 | Standard_RangeError_Raise_if(theLower > theUpper, ""); |
7fd59977 |
31 | } |
32 | |
3b010a74 |
33 | math_Vector::math_Vector(const Standard_Integer theLower, |
34 | const Standard_Integer theUpper, |
35 | const Standard_Real theInitialValue): |
36 | LowerIndex(theLower), |
37 | UpperIndex(theUpper), |
38 | Array(theLower,theUpper) |
7fd59977 |
39 | { |
3b010a74 |
40 | Standard_RangeError_Raise_if(theLower > theUpper, ""); |
41 | Array.Init(theInitialValue); |
7fd59977 |
42 | } |
43 | |
3b010a74 |
44 | math_Vector::math_Vector(const Standard_Address theTab, |
45 | const Standard_Integer theLower, |
46 | const Standard_Integer theUpper) : |
47 | LowerIndex(theLower), |
48 | UpperIndex(theUpper), |
49 | Array(theTab, theLower,theUpper) |
50 | { |
51 | Standard_RangeError_Raise_if((theLower > theUpper) , ""); |
7fd59977 |
52 | } |
53 | |
b5ef9d91 |
54 | math_Vector::math_Vector(const gp_XY& theOther): |
55 | LowerIndex(1), |
56 | UpperIndex(2), |
57 | Array(1,2) |
58 | { |
59 | Array(1) = theOther.X(); |
60 | Array(2) = theOther.Y(); |
61 | } |
62 | |
63 | math_Vector::math_Vector(const gp_XYZ& theOther): |
64 | LowerIndex(1), |
65 | UpperIndex(3), |
66 | Array(1, 3) |
67 | { |
68 | Array(1) = theOther.X(); |
69 | Array(2) = theOther.Y(); |
70 | Array(3) = theOther.Z(); |
71 | } |
72 | |
3b010a74 |
73 | void math_Vector::Init(const Standard_Real theInitialValue) |
74 | { |
75 | Array.Init(theInitialValue); |
76 | } |
7fd59977 |
77 | |
3b010a74 |
78 | math_Vector::math_Vector(const math_Vector& theOther) : |
79 | LowerIndex(theOther.LowerIndex), |
80 | UpperIndex(theOther.UpperIndex), |
81 | Array(theOther.Array) |
82 | { |
7fd59977 |
83 | } |
84 | |
3b010a74 |
85 | void math_Vector::SetLower(const Standard_Integer theLower) |
86 | { |
87 | Array.SetLower(theLower); |
88 | UpperIndex = UpperIndex - LowerIndex + theLower; |
89 | LowerIndex = theLower; |
90 | } |
7fd59977 |
91 | |
3b010a74 |
92 | Standard_Real math_Vector::Norm() const |
93 | { |
7fd59977 |
94 | Standard_Real Result = 0; |
95 | |
3b010a74 |
96 | for(Standard_Integer Index = LowerIndex; Index <= UpperIndex; Index++) |
97 | { |
7fd59977 |
98 | Result = Result + Array(Index) * Array(Index); |
99 | } |
100 | return Sqrt(Result); |
101 | } |
102 | |
3b010a74 |
103 | Standard_Real math_Vector::Norm2() const |
104 | { |
7fd59977 |
105 | Standard_Real Result = 0; |
106 | |
3b010a74 |
107 | for(Standard_Integer Index = LowerIndex; Index <= UpperIndex; Index++) |
108 | { |
7fd59977 |
109 | Result = Result + Array(Index) * Array(Index); |
110 | } |
111 | return Result; |
112 | } |
113 | |
3b010a74 |
114 | Standard_Integer math_Vector::Max() const |
115 | { |
7fd59977 |
116 | Standard_Integer I=0; |
117 | Standard_Real X = RealFirst(); |
118 | |
3b010a74 |
119 | for(Standard_Integer Index = LowerIndex; Index <= UpperIndex; Index++) |
120 | { |
121 | if(Array(Index) > X) |
122 | { |
7fd59977 |
123 | X = Array(Index); |
124 | I = Index; |
125 | } |
126 | } |
127 | return I; |
128 | } |
129 | |
3b010a74 |
130 | Standard_Integer math_Vector::Min() const |
131 | { |
7fd59977 |
132 | Standard_Integer I=0; |
133 | Standard_Real X = RealLast(); |
134 | |
3b010a74 |
135 | for(Standard_Integer Index = LowerIndex; Index <= UpperIndex; Index++) |
136 | { |
137 | if(Array(Index) < X) |
138 | { |
7fd59977 |
139 | X = Array(Index); |
140 | I = Index; |
141 | } |
142 | } |
143 | return I; |
144 | } |
145 | |
3b010a74 |
146 | void math_Vector::Set(const Standard_Integer theI1, |
147 | const Standard_Integer theI2, |
148 | const math_Vector &theV) |
149 | { |
150 | Standard_RangeError_Raise_if((theI1 < LowerIndex) || (theI2 > UpperIndex) || |
151 | (theI1 > theI2) || (theI2 - theI1 + 1 != theV.Length()), ""); |
7fd59977 |
152 | |
3b010a74 |
153 | Standard_Integer I = theV.Lower(); |
154 | for(Standard_Integer Index = theI1; Index <= theI2; Index++) |
155 | { |
156 | Array(Index) = theV.Array(I); |
7fd59977 |
157 | I++; |
158 | } |
159 | } |
160 | |
3b010a74 |
161 | void math_Vector::Normalize() |
162 | { |
7fd59977 |
163 | Standard_Real Result = Norm(); |
164 | Standard_NullValue_Raise_if((Result <= RealEpsilon()), ""); |
3b010a74 |
165 | for(Standard_Integer Index = LowerIndex; Index <= UpperIndex; Index++) |
166 | { |
7fd59977 |
167 | Array(Index) = Array(Index) / Result; |
168 | } |
169 | } |
170 | |
3b010a74 |
171 | math_Vector math_Vector::Normalized() const |
172 | { |
7fd59977 |
173 | math_Vector Result = *this; |
174 | |
175 | Result.Normalize(); |
176 | return Result; |
177 | } |
178 | |
3b010a74 |
179 | void math_Vector::Invert() |
180 | { |
7fd59977 |
181 | Standard_Integer J; |
182 | Standard_Real Temp; |
3b010a74 |
183 | for(Standard_Integer Index = LowerIndex; Index <= (LowerIndex + Length()) >> 1 ; Index++) |
184 | { |
7fd59977 |
185 | J = UpperIndex + LowerIndex - Index; |
186 | Temp = Array(Index); |
187 | Array(Index) = Array(J); |
188 | Array(J) = Temp; |
189 | } |
190 | } |
191 | |
3b010a74 |
192 | math_Vector math_Vector::Inverse() const |
193 | { |
7fd59977 |
194 | math_Vector Result = *this; |
195 | Result.Invert(); |
196 | return Result; |
197 | } |
198 | |
3b010a74 |
199 | math_Vector math_Vector::Multiplied(const Standard_Real theRight) const |
200 | { |
7fd59977 |
201 | math_Vector Result (LowerIndex, UpperIndex); |
202 | |
3b010a74 |
203 | for(Standard_Integer Index = LowerIndex; Index <= UpperIndex; Index++) |
204 | { |
205 | Result.Array(Index) = Array(Index) * theRight; |
7fd59977 |
206 | } |
207 | return Result; |
208 | } |
209 | |
3b010a74 |
210 | math_Vector math_Vector::TMultiplied(const Standard_Real theRight) const |
211 | { |
7fd59977 |
212 | math_Vector Result (LowerIndex, UpperIndex); |
213 | |
3b010a74 |
214 | for(Standard_Integer Index = LowerIndex; Index <= UpperIndex; Index++) |
215 | { |
216 | Result.Array(Index) = Array(Index) * theRight; |
7fd59977 |
217 | } |
218 | return Result; |
219 | } |
220 | |
3b010a74 |
221 | void math_Vector::Multiply(const Standard_Real theRight) |
222 | { |
223 | for(Standard_Integer Index = LowerIndex; Index <= UpperIndex; Index++) |
224 | { |
225 | Array(Index) = Array(Index) * theRight; |
7fd59977 |
226 | } |
227 | } |
228 | |
3b010a74 |
229 | void math_Vector::Divide(const Standard_Real theRight) |
230 | { |
231 | Standard_DivideByZero_Raise_if(Abs(theRight) <= RealEpsilon(), ""); |
7fd59977 |
232 | |
3b010a74 |
233 | for(Standard_Integer Index =LowerIndex; Index <=UpperIndex; Index++) |
234 | { |
235 | Array(Index) = Array(Index) / theRight; |
7fd59977 |
236 | } |
237 | } |
238 | |
3b010a74 |
239 | math_Vector math_Vector::Divided (const Standard_Real theRight) const |
240 | { |
241 | Standard_DivideByZero_Raise_if(Abs(theRight) <= RealEpsilon(), ""); |
242 | math_Vector temp = Multiplied(1./theRight); |
7fd59977 |
243 | return temp; |
244 | } |
245 | |
3b010a74 |
246 | void math_Vector::Add(const math_Vector& theRight) |
247 | { |
248 | Standard_DimensionError_Raise_if(Length() != theRight.Length(), ""); |
7fd59977 |
249 | |
3b010a74 |
250 | Standard_Integer I = theRight.LowerIndex; |
251 | for(Standard_Integer Index = LowerIndex; Index <= UpperIndex; Index++) |
252 | { |
253 | Array(Index) = Array(Index) + theRight.Array(I); |
7fd59977 |
254 | I++; |
255 | } |
3b010a74 |
256 | } |
7fd59977 |
257 | |
3b010a74 |
258 | math_Vector math_Vector::Added(const math_Vector& theRight) const |
259 | { |
260 | Standard_DimensionError_Raise_if(Length() != theRight.Length(), ""); |
7fd59977 |
261 | |
262 | math_Vector Result(LowerIndex, UpperIndex); |
3b010a74 |
263 | |
264 | Standard_Integer I = theRight.LowerIndex; |
265 | for(Standard_Integer Index = LowerIndex; Index <= UpperIndex; Index++) |
266 | { |
267 | Result.Array(Index) = Array(Index) + theRight.Array(I); |
7fd59977 |
268 | I++; |
269 | } |
270 | return Result; |
3b010a74 |
271 | } |
7fd59977 |
272 | |
3b010a74 |
273 | void math_Vector::Subtract(const math_Vector& theRight) |
274 | { |
275 | Standard_DimensionError_Raise_if(Length() != theRight.Length(), ""); |
7fd59977 |
276 | |
3b010a74 |
277 | Standard_Integer I = theRight.LowerIndex; |
278 | for(Standard_Integer Index = LowerIndex; Index <= UpperIndex; Index++) |
279 | { |
280 | Array(Index) = Array(Index) - theRight.Array(I); |
7fd59977 |
281 | I++; |
282 | } |
3b010a74 |
283 | } |
7fd59977 |
284 | |
3b010a74 |
285 | math_Vector math_Vector::Subtracted (const math_Vector& theRight) const |
286 | { |
287 | Standard_DimensionError_Raise_if(Length() != theRight.Length(), ""); |
7fd59977 |
288 | |
7fd59977 |
289 | math_Vector Result(LowerIndex, UpperIndex); |
3b010a74 |
290 | |
291 | Standard_Integer I = theRight.LowerIndex; |
292 | for(Standard_Integer Index = LowerIndex; Index <= UpperIndex; Index++) |
293 | { |
294 | Result.Array(Index) = Array(Index) - theRight.Array(I); |
7fd59977 |
295 | I++; |
296 | } |
297 | return Result; |
3b010a74 |
298 | } |
7fd59977 |
299 | |
3b010a74 |
300 | math_Vector math_Vector::Slice(const Standard_Integer theI1, const Standard_Integer theI2) const |
7fd59977 |
301 | { |
3b010a74 |
302 | Standard_RangeError_Raise_if((theI1 < LowerIndex) || (theI1 > UpperIndex) || (theI2 < LowerIndex) || (theI2 > UpperIndex) , ""); |
7fd59977 |
303 | |
3b010a74 |
304 | if(theI2 >= theI1) |
305 | { |
306 | math_Vector Result(theI1, theI2); |
307 | for(Standard_Integer Index = theI1; Index <= theI2; Index++) |
308 | { |
7fd59977 |
309 | Result.Array(Index) = Array(Index); |
3b010a74 |
310 | } |
311 | return Result; |
7fd59977 |
312 | } |
3b010a74 |
313 | else |
314 | { |
315 | math_Vector Result(theI2, theI1); |
316 | for(Standard_Integer Index = theI1; Index >= theI2; Index--) |
317 | { |
7fd59977 |
318 | Result.Array(Index) = Array(Index); |
319 | } |
320 | return Result; |
3b010a74 |
321 | } |
7fd59977 |
322 | } |
323 | |
3b010a74 |
324 | void math_Vector::Add (const math_Vector& theLeft, const math_Vector& theRight) |
325 | { |
326 | Standard_DimensionError_Raise_if((Length() != theRight.Length()) || (theRight.Length() != theLeft.Length()), ""); |
7fd59977 |
327 | |
3b010a74 |
328 | Standard_Integer I = theLeft.LowerIndex; |
329 | Standard_Integer J = theRight.LowerIndex; |
330 | for(Standard_Integer Index = LowerIndex; Index <= UpperIndex; Index++) |
331 | { |
332 | Array(Index) = theLeft.Array(I) + theRight.Array(J); |
7fd59977 |
333 | I++; |
334 | J++; |
335 | } |
3b010a74 |
336 | } |
337 | |
338 | void math_Vector::Subtract (const math_Vector& theLeft, const math_Vector& theRight) |
339 | { |
340 | Standard_DimensionError_Raise_if((Length() != theRight.Length()) || (theRight.Length() != theLeft.Length()), ""); |
341 | |
342 | Standard_Integer I = theLeft.LowerIndex; |
343 | Standard_Integer J = theRight.LowerIndex; |
344 | for(Standard_Integer Index = LowerIndex; Index <= UpperIndex; Index++) |
345 | { |
346 | Array(Index) = theLeft.Array(I) - theRight.Array(J); |
7fd59977 |
347 | I++; |
348 | J++; |
349 | } |
3b010a74 |
350 | } |
7fd59977 |
351 | |
3b010a74 |
352 | void math_Vector::Multiply(const math_Matrix& theLeft, const math_Vector& theRight) |
353 | { |
354 | Standard_DimensionError_Raise_if((Length() != theLeft.RowNumber()) || |
355 | (theLeft.ColNumber() != theRight.Length()), ""); |
7fd59977 |
356 | |
357 | Standard_Integer Index = LowerIndex; |
3b010a74 |
358 | for(Standard_Integer I = theLeft.LowerRowIndex; I <= theLeft.UpperRowIndex; I++) |
359 | { |
7fd59977 |
360 | Array(Index) = 0.0; |
3b010a74 |
361 | Standard_Integer K = theRight.LowerIndex; |
362 | for(Standard_Integer J = theLeft.LowerColIndex; J <= theLeft.UpperColIndex; J++) |
363 | { |
364 | Array(Index) = Array(Index) + theLeft.Array(I, J) * theRight.Array(K); |
7fd59977 |
365 | K++; |
366 | } |
367 | Index++; |
368 | } |
3b010a74 |
369 | } |
7fd59977 |
370 | |
3b010a74 |
371 | void math_Vector::Multiply(const math_Vector& theLeft, const math_Matrix& theRight) |
372 | { |
373 | Standard_DimensionError_Raise_if((Length() != theRight.ColNumber()) || |
374 | (theLeft.Length() != theRight.RowNumber()), ""); |
7fd59977 |
375 | |
376 | Standard_Integer Index = LowerIndex; |
3b010a74 |
377 | for(Standard_Integer J = theRight.LowerColIndex; J <= theRight.UpperColIndex; J++) |
378 | { |
7fd59977 |
379 | Array(Index) = 0.0; |
3b010a74 |
380 | Standard_Integer K = theLeft.LowerIndex; |
381 | for(Standard_Integer I = theRight.LowerRowIndex; I <= theRight.UpperRowIndex; I++) |
382 | { |
383 | Array(Index) = Array(Index) + theLeft.Array(K) * theRight.Array(I, J); |
7fd59977 |
384 | K++; |
385 | } |
386 | Index++; |
387 | } |
3b010a74 |
388 | } |
7fd59977 |
389 | |
3b010a74 |
390 | void math_Vector::TMultiply(const math_Matrix& theTLeft, const math_Vector& theRight) |
391 | { |
392 | Standard_DimensionError_Raise_if((Length() != theTLeft.ColNumber()) || |
393 | (theTLeft.RowNumber() != theRight.Length()), ""); |
7fd59977 |
394 | |
395 | Standard_Integer Index = LowerIndex; |
3b010a74 |
396 | for(Standard_Integer I = theTLeft.LowerColIndex; I <= theTLeft.UpperColIndex; I++) |
397 | { |
7fd59977 |
398 | Array(Index) = 0.0; |
3b010a74 |
399 | Standard_Integer K = theRight.LowerIndex; |
400 | for(Standard_Integer J = theTLeft.LowerRowIndex; J <= theTLeft.UpperRowIndex; J++) |
401 | { |
402 | Array(Index) = Array(Index) + theTLeft.Array(J, I) * theRight.Array(K); |
7fd59977 |
403 | K++; |
404 | } |
405 | Index++; |
406 | } |
3b010a74 |
407 | } |
7fd59977 |
408 | |
3b010a74 |
409 | void math_Vector::TMultiply(const math_Vector& theLeft, const math_Matrix& theTRight) |
410 | { |
411 | Standard_DimensionError_Raise_if((Length() != theTRight.RowNumber()) || |
412 | (theLeft.Length() != theTRight.ColNumber()), ""); |
7fd59977 |
413 | |
414 | Standard_Integer Index = LowerIndex; |
3b010a74 |
415 | for(Standard_Integer J = theTRight.LowerRowIndex; J <= theTRight.UpperRowIndex; J++) |
416 | { |
7fd59977 |
417 | Array(Index) = 0.0; |
3b010a74 |
418 | Standard_Integer K = theLeft.LowerIndex; |
419 | for(Standard_Integer I = theTRight.LowerColIndex; |
420 | I <= theTRight.UpperColIndex; I++) |
421 | { |
422 | Array(Index) = Array(Index) + theLeft.Array(K) * theTRight.Array(J, I); |
423 | K++; |
7fd59977 |
424 | } |
425 | Index++; |
426 | } |
3b010a74 |
427 | } |
7fd59977 |
428 | |
3b010a74 |
429 | Standard_Real math_Vector::Multiplied(const math_Vector& theRight) const |
430 | { |
7fd59977 |
431 | Standard_Real Result = 0; |
432 | |
3b010a74 |
433 | Standard_DimensionError_Raise_if(Length() != theRight.Length(), ""); |
7fd59977 |
434 | |
3b010a74 |
435 | Standard_Integer I = theRight.LowerIndex; |
436 | for(Standard_Integer Index = LowerIndex; Index <= UpperIndex; Index++) |
437 | { |
438 | Result = Result + Array(Index) * theRight.Array(I); |
7fd59977 |
439 | I++; |
440 | } |
441 | return Result; |
3b010a74 |
442 | } |
7fd59977 |
443 | |
3b010a74 |
444 | math_Vector math_Vector::Opposite() |
445 | { |
7fd59977 |
446 | math_Vector Result(LowerIndex, UpperIndex); |
447 | |
3b010a74 |
448 | for(Standard_Integer Index = LowerIndex; Index <= UpperIndex; Index++) |
449 | { |
7fd59977 |
450 | Result.Array(Index) = - Array(Index); |
451 | } |
452 | return Result; |
3b010a74 |
453 | } |
454 | |
455 | math_Vector math_Vector::Multiplied(const math_Matrix& theRight)const |
456 | { |
457 | Standard_DimensionError_Raise_if(Length() != theRight.RowNumber(), ""); |
458 | |
459 | math_Vector Result(theRight.LowerColIndex, theRight.UpperColIndex); |
460 | for(Standard_Integer J2 = theRight.LowerColIndex; J2 <= theRight.UpperColIndex; J2++) |
461 | { |
462 | Array(J2) = 0.0; |
463 | Standard_Integer theI2 = theRight.LowerRowIndex; |
464 | for(Standard_Integer I = LowerIndex; I <= UpperIndex; I++) |
465 | { |
466 | Result.Array(J2) = Result.Array(J2) + Array(I) * theRight.Array(theI2, J2); |
467 | theI2++; |
468 | } |
7fd59977 |
469 | } |
470 | return Result; |
3b010a74 |
471 | } |
7fd59977 |
472 | |
3b010a74 |
473 | void math_Vector::Multiply(const Standard_Real theLeft, const math_Vector& theRight) |
7fd59977 |
474 | { |
3b010a74 |
475 | Standard_DimensionError_Raise_if((Length() != theRight.Length()), ""); |
476 | for(Standard_Integer I = LowerIndex; I <= UpperIndex; I++) |
477 | { |
478 | Array(I) = theLeft * theRight.Array(I); |
7fd59977 |
479 | } |
480 | } |
481 | |
3b010a74 |
482 | math_Vector& math_Vector::Initialized(const math_Vector& theOther) |
483 | { |
484 | Standard_DimensionError_Raise_if(Length() != theOther.Length(), ""); |
7fd59977 |
485 | |
3b010a74 |
486 | (theOther.Array).Copy(Array); |
7fd59977 |
487 | return *this; |
488 | } |
489 | |
3b010a74 |
490 | void math_Vector::Dump(Standard_OStream& theO) const |
7fd59977 |
491 | { |
3b010a74 |
492 | theO << "math_Vector of Length = " << Length() << "\n"; |
493 | for(Standard_Integer Index = LowerIndex; Index <= UpperIndex; Index++) |
494 | { |
495 | theO << "math_Vector(" << Index << ") = " << Array(Index) << "\n"; |
7fd59977 |
496 | } |
497 | } |
3b010a74 |
498 | |