6334d8139ff626e21842a59d7a88800c1f9f38d1
[occt.git] / src / math / math_Vector.cxx
1 // Copyright (c) 1997-1999 Matra Datavision
2 // Copyright (c) 1999-2014 OPEN CASCADE SAS
3 //
4 // This file is part of Open CASCADE Technology software library.
5 //
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
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.
11 //
12 // Alternatively, this file may be used under the terms of Open CASCADE
13 // commercial license or contractual agreement.
14
15 #include <stdio.h>
16
17 #include <math_Vector.hxx>
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
25 math_Vector::math_Vector(const Standard_Integer theLower, const Standard_Integer theUpper) :
26   LowerIndex(theLower),
27   UpperIndex(theUpper),
28   Array(theLower,theUpper)
29 {
30   Standard_RangeError_Raise_if(theLower > theUpper, "");
31 }
32
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)
39 {
40   Standard_RangeError_Raise_if(theLower > theUpper, "");
41   Array.Init(theInitialValue);
42 }
43
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) , "");
52 }
53
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
73 void math_Vector::Init(const Standard_Real theInitialValue)
74 {
75   Array.Init(theInitialValue);
76 }
77
78 math_Vector::math_Vector(const math_Vector& theOther) :
79   LowerIndex(theOther.LowerIndex),
80   UpperIndex(theOther.UpperIndex),
81   Array(theOther.Array)
82 {
83 }
84
85 void math_Vector::SetLower(const Standard_Integer theLower)
86 {
87   Array.SetLower(theLower);
88   UpperIndex = UpperIndex - LowerIndex + theLower;
89   LowerIndex = theLower;
90 }
91
92 Standard_Real math_Vector::Norm() const
93 {
94   Standard_Real Result = 0;
95
96   for(Standard_Integer Index = LowerIndex; Index <= UpperIndex; Index++)
97   {
98     Result = Result + Array(Index) * Array(Index);
99   }
100   return Sqrt(Result);
101 }
102
103 Standard_Real math_Vector::Norm2() const
104 {
105   Standard_Real Result = 0;
106
107   for(Standard_Integer Index = LowerIndex; Index <= UpperIndex; Index++)
108   {
109     Result = Result + Array(Index) * Array(Index);
110   }
111   return Result;
112 }
113
114 Standard_Integer math_Vector::Max() const
115 {
116   Standard_Integer I=0;
117   Standard_Real X = RealFirst();
118
119   for(Standard_Integer Index = LowerIndex; Index <= UpperIndex; Index++)
120   {
121     if(Array(Index) > X)
122     {
123       X = Array(Index);
124       I = Index;
125     }
126   }
127   return I;
128 }
129
130 Standard_Integer math_Vector::Min() const
131 {
132   Standard_Integer I=0;
133   Standard_Real X = RealLast();
134
135   for(Standard_Integer Index = LowerIndex; Index <= UpperIndex; Index++)
136   {
137     if(Array(Index) < X)
138     {
139       X = Array(Index);
140       I = Index;
141     }
142   }
143   return I;
144 }
145
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()), "");
152
153   Standard_Integer I = theV.Lower();
154   for(Standard_Integer Index = theI1; Index <= theI2; Index++)
155   {
156     Array(Index) = theV.Array(I);
157     I++;
158   }
159 }
160
161 void math_Vector::Normalize()
162 {
163   Standard_Real Result = Norm();
164   Standard_NullValue_Raise_if((Result <= RealEpsilon()), "");
165   for(Standard_Integer Index = LowerIndex; Index <= UpperIndex; Index++)
166   {
167     Array(Index) = Array(Index) / Result;
168   }
169 }
170
171 math_Vector math_Vector::Normalized() const
172 {
173   math_Vector Result = *this;
174
175   Result.Normalize();
176   return Result;
177 }
178
179 void math_Vector::Invert()
180 {
181   Standard_Integer J;
182   Standard_Real Temp;
183   for(Standard_Integer Index = LowerIndex; Index <= (LowerIndex + Length()) >> 1 ; Index++)
184   {
185     J = UpperIndex + LowerIndex - Index;
186     Temp = Array(Index);
187     Array(Index) = Array(J);
188     Array(J) = Temp;
189   }
190 }
191
192 math_Vector math_Vector::Inverse() const
193 {
194   math_Vector Result = *this;
195   Result.Invert();
196   return Result;
197 }
198
199 math_Vector math_Vector::Multiplied(const Standard_Real theRight) const
200 {
201   math_Vector Result (LowerIndex, UpperIndex);
202
203   for(Standard_Integer Index = LowerIndex; Index <= UpperIndex; Index++)
204   {
205     Result.Array(Index) = Array(Index) * theRight;
206   }
207   return Result;
208 }
209
210 math_Vector math_Vector::TMultiplied(const Standard_Real theRight) const
211 {
212   math_Vector Result (LowerIndex, UpperIndex);
213
214   for(Standard_Integer Index = LowerIndex; Index <= UpperIndex; Index++)
215   {
216     Result.Array(Index) = Array(Index) * theRight;
217   }
218   return Result;
219 }
220
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;
226   }
227 }
228
229 void math_Vector::Divide(const Standard_Real theRight)
230 {
231   Standard_DivideByZero_Raise_if(Abs(theRight) <= RealEpsilon(), "");
232
233   for(Standard_Integer Index =LowerIndex; Index <=UpperIndex; Index++)
234   {
235     Array(Index) = Array(Index) / theRight;
236   }
237 }
238
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);
243   return temp;
244 }
245
246 void math_Vector::Add(const math_Vector& theRight)
247 {
248   Standard_DimensionError_Raise_if(Length() != theRight.Length(), "");
249
250   Standard_Integer I = theRight.LowerIndex;
251   for(Standard_Integer Index = LowerIndex; Index <= UpperIndex; Index++)
252   {
253     Array(Index) = Array(Index) + theRight.Array(I);
254     I++;
255   }
256 }
257
258 math_Vector math_Vector::Added(const math_Vector& theRight) const
259 {
260   Standard_DimensionError_Raise_if(Length() != theRight.Length(), "");
261
262   math_Vector Result(LowerIndex, UpperIndex);
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);
268     I++;
269   }
270   return Result;
271 }
272
273 void math_Vector::Subtract(const math_Vector& theRight)
274 {
275   Standard_DimensionError_Raise_if(Length() != theRight.Length(), "");
276
277   Standard_Integer I = theRight.LowerIndex;
278   for(Standard_Integer Index = LowerIndex; Index <= UpperIndex; Index++)
279   {
280     Array(Index) = Array(Index) - theRight.Array(I);
281     I++;
282   }
283 }
284
285 math_Vector math_Vector::Subtracted (const math_Vector& theRight) const
286 {
287   Standard_DimensionError_Raise_if(Length() != theRight.Length(), "");
288
289   math_Vector Result(LowerIndex, UpperIndex);
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);
295     I++;
296   }
297   return Result;
298 }
299
300 math_Vector math_Vector::Slice(const Standard_Integer theI1, const Standard_Integer theI2) const
301 {
302   Standard_RangeError_Raise_if((theI1 < LowerIndex) || (theI1 > UpperIndex) || (theI2 < LowerIndex) || (theI2 > UpperIndex) , "");
303
304   if(theI2 >= theI1)
305   {
306     math_Vector Result(theI1, theI2);
307     for(Standard_Integer Index = theI1; Index <= theI2; Index++)
308     {
309       Result.Array(Index) = Array(Index);
310     }
311     return Result;
312   }
313   else
314   {
315     math_Vector Result(theI2, theI1);
316     for(Standard_Integer Index = theI1; Index >= theI2; Index--)
317     {
318       Result.Array(Index) = Array(Index);
319     }
320     return Result;
321   }
322 }
323
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()), "");
327
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);
333     I++;
334     J++;
335   }
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);
347     I++;
348     J++;
349   }
350 }
351
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()), "");
356
357   Standard_Integer Index = LowerIndex;
358   for(Standard_Integer I = theLeft.LowerRowIndex; I <= theLeft.UpperRowIndex; I++)
359   {
360     Array(Index) = 0.0;
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);
365       K++;
366     }
367     Index++;
368   }
369 }
370
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()), "");
375
376   Standard_Integer Index = LowerIndex;
377   for(Standard_Integer J = theRight.LowerColIndex; J <= theRight.UpperColIndex; J++)
378   {
379     Array(Index) = 0.0;
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);
384       K++;
385     }
386     Index++;
387   }
388 }
389
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()), "");
394
395   Standard_Integer Index = LowerIndex;
396   for(Standard_Integer I = theTLeft.LowerColIndex; I <= theTLeft.UpperColIndex; I++)
397   {
398     Array(Index) = 0.0;
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);
403       K++;
404     }
405     Index++;
406   }
407 }
408
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()), "");
413
414   Standard_Integer Index = LowerIndex;
415   for(Standard_Integer J = theTRight.LowerRowIndex; J <= theTRight.UpperRowIndex; J++)
416   {
417     Array(Index) = 0.0;
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++;
424     }
425     Index++;
426   }
427 }
428
429 Standard_Real math_Vector::Multiplied(const math_Vector& theRight) const
430 {
431   Standard_Real Result = 0;
432
433   Standard_DimensionError_Raise_if(Length() != theRight.Length(), "");
434
435   Standard_Integer I = theRight.LowerIndex;
436   for(Standard_Integer Index = LowerIndex; Index <= UpperIndex; Index++)
437   {
438     Result = Result + Array(Index) * theRight.Array(I);
439     I++;
440   }
441   return Result;
442 }
443
444 math_Vector math_Vector::Opposite()
445 {
446   math_Vector Result(LowerIndex, UpperIndex);
447
448   for(Standard_Integer Index = LowerIndex; Index <= UpperIndex; Index++)
449   {
450     Result.Array(Index) = - Array(Index);
451   }
452   return Result;
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       }
469   }
470   return Result;
471 }
472
473 void math_Vector::Multiply(const Standard_Real theLeft, const math_Vector& theRight)
474 {
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);
479   }
480 }
481
482 math_Vector& math_Vector::Initialized(const math_Vector& theOther)
483 {
484   Standard_DimensionError_Raise_if(Length() != theOther.Length(), "");
485
486   (theOther.Array).Copy(Array);
487   return *this;
488 }
489
490 void math_Vector::Dump(Standard_OStream& theO) const
491 {
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";
496   }
497 }
498