b311480e |
1 | // Copyright (c) 1995-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 | // 10/09/97 : PMN : Correction BUC40192 (pb avec les matrices negatives) |
16 | |
0797d9d3 |
17 | #ifndef OCCT_DEBUG |
7fd59977 |
18 | #define No_Standard_OutOfRange |
19 | #define No_Standard_ConstructionError |
20 | #endif |
21 | |
42cf5bc1 |
22 | |
23 | #include <gp_GTrsf.hxx> |
24 | #include <gp_Mat.hxx> |
25 | #include <gp_Trsf.hxx> |
26 | #include <gp_XYZ.hxx> |
27 | #include <Standard_ConstructionError.hxx> |
28 | #include <Standard_OutOfRange.hxx> |
0904aa63 |
29 | #include <Standard_Dump.hxx> |
7fd59977 |
30 | |
31 | #define M00 ((Standard_Real*)M)[0] |
32 | #define M01 ((Standard_Real*)M)[1] |
33 | #define M02 ((Standard_Real*)M)[2] |
34 | #define M10 ((Standard_Real*)M)[3] |
35 | #define M11 ((Standard_Real*)M)[4] |
36 | #define M12 ((Standard_Real*)M)[5] |
37 | #define M20 ((Standard_Real*)M)[6] |
38 | #define M21 ((Standard_Real*)M)[7] |
39 | #define M22 ((Standard_Real*)M)[8] |
40 | |
41 | #define N00 ((Standard_Real*)N)[0] |
42 | #define N01 ((Standard_Real*)N)[1] |
43 | #define N02 ((Standard_Real*)N)[2] |
44 | #define N10 ((Standard_Real*)N)[3] |
45 | #define N11 ((Standard_Real*)N)[4] |
46 | #define N12 ((Standard_Real*)N)[5] |
47 | #define N20 ((Standard_Real*)N)[6] |
48 | #define N21 ((Standard_Real*)N)[7] |
49 | #define N22 ((Standard_Real*)N)[8] |
50 | |
51 | gp_Mat::gp_Mat (const gp_XYZ& Col1, |
52 | const gp_XYZ& Col2, |
53 | const gp_XYZ& Col3) |
54 | { |
55 | const Standard_Address M = (Standard_Address)&(matrix[0][0]); |
56 | M00 = Col1.X(); M10 = Col1.Y(); M20 = Col1.Z(); |
57 | M01 = Col2.X(); M11 = Col2.Y(); M21 = Col2.Z(); |
58 | M02 = Col3.X(); M12 = Col3.Y(); M22 = Col3.Z(); |
59 | } |
60 | |
61 | void gp_Mat::SetCol (const Standard_Integer Col, |
62 | const gp_XYZ& Value) { |
63 | |
64 | Standard_OutOfRange_Raise_if (Col < 1 || Col > 3, " "); |
65 | const Standard_Address M = (Standard_Address)&(matrix[0][0]); |
66 | if (Col == 1) { |
67 | M00 = Value.X(); M10 = Value.Y(); M20 = Value.Z(); |
68 | } |
69 | else if (Col == 2) { |
70 | M01 = Value.X(); M11 = Value.Y(); M21 = Value.Z(); |
71 | } |
72 | else { |
73 | M02 = Value.X(); M12 = Value.Y(); M22 = Value.Z(); |
74 | } |
75 | } |
76 | |
77 | void gp_Mat::SetCols (const gp_XYZ& Col1, |
78 | const gp_XYZ& Col2, |
79 | const gp_XYZ& Col3) |
80 | { |
7fd59977 |
81 | Mat00 = Col1.X(); Mat10 = Col1.Y(); Mat20 = Col1.Z(); |
82 | Mat01 = Col2.X(); Mat11 = Col2.Y(); Mat21 = Col2.Z(); |
83 | Mat02 = Col3.X(); Mat12 = Col3.Y(); Mat22 = Col3.Z(); |
84 | } |
85 | |
86 | void gp_Mat::SetCross (const gp_XYZ& Ref) |
87 | { |
88 | const Standard_Address M = (Standard_Address)&(matrix[0][0]); |
89 | Standard_Real X = Ref.X(); |
90 | Standard_Real Y = Ref.Y(); |
91 | Standard_Real Z = Ref.Z(); |
92 | M00 = M11 = M22 = 0.0; |
93 | M01 = - Z; |
94 | M02 = Y; |
95 | M12 = - X; |
96 | M10 = Z; |
97 | M20 = - Y; |
98 | M21 = X; |
99 | } |
100 | |
101 | void gp_Mat::SetDot (const gp_XYZ& Ref) |
102 | { |
103 | const Standard_Address M = (Standard_Address)&(matrix[0][0]); |
104 | Standard_Real X = Ref.X(); |
105 | Standard_Real Y = Ref.Y(); |
106 | Standard_Real Z = Ref.Z(); |
107 | M00 = X * X; |
108 | M11 = Y * Y; |
109 | M22 = Z * Z; |
110 | M01 = X * Y; |
111 | M02 = X * Z; |
112 | M12 = Y * Z; |
113 | M10 = M01; |
114 | M20 = M02; |
115 | M21 = M12; |
116 | } |
117 | |
118 | void gp_Mat::SetRotation (const gp_XYZ& Axis, |
119 | const Standard_Real Ang) |
120 | { |
121 | // Rot = I + sin(Ang) * M + (1. - cos(Ang)) * M*M |
122 | // avec M . XYZ = Axis ^ XYZ |
123 | |
124 | // const Standard_Address M = (Standard_Address)&(matrix[0][0]); |
125 | gp_XYZ V = Axis.Normalized(); |
126 | SetCross (V); |
127 | Multiply (sin(Ang)); |
128 | gp_Mat Temp; |
129 | Temp.SetScale (1.0); |
130 | Add (Temp); |
131 | Standard_Real A = V.X(); |
132 | Standard_Real B = V.Y(); |
133 | Standard_Real C = V.Z(); |
134 | Temp.SetRow (1, gp_XYZ(- C*C - B*B, A*B, A*C )); |
135 | Temp.SetRow (2, gp_XYZ( A*B, -A*A - C*C, B*C )); |
136 | Temp.SetRow (3, gp_XYZ( A*C, B*C, - A*A - B*B)); |
137 | Temp.Multiply (1.0 - cos(Ang)); |
138 | Add (Temp); |
139 | } |
140 | |
141 | void gp_Mat::SetRow (const Standard_Integer Row, |
142 | const gp_XYZ& Value) |
143 | { |
144 | Standard_OutOfRange_Raise_if (Row < 1 || Row > 3, " "); |
145 | const Standard_Address M = (Standard_Address)&(matrix[0][0]); |
146 | if (Row == 1) { |
147 | M00 = Value.X(); M01 = Value.Y(); M02 = Value.Z(); |
148 | } |
149 | else if (Row == 2) { |
150 | M10 = Value.X(); M11 = Value.Y(); M12 = Value.Z(); |
151 | } |
152 | else { |
153 | M20 = Value.X(); M21 = Value.Y(); M22 = Value.Z(); |
154 | } |
155 | } |
156 | |
157 | void gp_Mat::SetRows (const gp_XYZ& Row1, |
158 | const gp_XYZ& Row2, |
159 | const gp_XYZ& Row3) |
160 | { |
161 | const Standard_Address M = (Standard_Address)&(matrix[0][0]); |
162 | M00 = Row1.X(); M01 = Row1.Y(); M02 = Row1.Z(); |
163 | M10 = Row2.X(); M11 = Row2.Y(); M12 = Row2.Z(); |
164 | M20 = Row3.X(); M21 = Row3.Y(); M22 = Row3.Z(); |
165 | } |
166 | |
167 | gp_XYZ gp_Mat::Column (const Standard_Integer Col) const |
168 | { |
2d2b3d53 |
169 | Standard_OutOfRange_Raise_if (Col < 1 || Col > 3, "gp_Mat::Column() - wrong index"); |
7fd59977 |
170 | const Standard_Address M = (Standard_Address)&(matrix[0][0]); |
171 | if (Col == 1) return gp_XYZ (M00,M10,M20); |
172 | if (Col == 2) return gp_XYZ (M01,M11,M21); |
173 | return gp_XYZ (M02,M12,M22); |
174 | } |
175 | |
176 | gp_XYZ gp_Mat::Diagonal () const |
177 | { |
178 | const Standard_Address M = (Standard_Address)&(matrix[0][0]); |
179 | return gp_XYZ (M00, M11, M22); |
180 | } |
181 | |
182 | gp_XYZ gp_Mat::Row (const Standard_Integer Row) const |
183 | { |
2d2b3d53 |
184 | Standard_OutOfRange_Raise_if (Row < 1 || Row > 3, "gp_Mat::Row() - wrong index"); |
7fd59977 |
185 | const Standard_Address M = (Standard_Address)&(matrix[0][0]); |
186 | if (Row == 1) return gp_XYZ (M00,M01,M02); |
187 | if (Row == 2) return gp_XYZ (M10,M11,M12); |
188 | return gp_XYZ (M20,M21,M22); |
189 | } |
190 | |
191 | void gp_Mat::Invert () |
192 | { |
193 | Standard_Real new_array[3][3] ; |
194 | const Standard_Address M = (Standard_Address)&( matrix[0][0]); |
195 | const Standard_Address N = (Standard_Address)&(new_array[0][0]); |
196 | |
197 | // |
198 | // calcul de la transposee de la commatrice |
199 | // |
200 | N00 = M11 * M22 - M12 * M21 ; |
201 | N10 = -(M10 * M22 - M20 * M12) ; |
202 | N20 = M10 * M21 - M20 * M11 ; |
203 | N01 = - (M01 * M22 - M21 * M02) ; |
204 | N11 = M00 * M22 - M20 * M02 ; |
205 | N21 = -(M00 * M21 - M20 * M01) ; |
206 | N02 = M01 * M12 - M11 * M02 ; |
207 | N12 = -(M00 * M12 - M10 * M02) ; |
208 | N22 = M00 * M11 - M01 * M10 ; |
209 | Standard_Real det = M00 * N00 + M01* N10 + M02 * N20 ; |
210 | Standard_Real val = det; |
211 | if (val < 0) val = - val; |
2d2b3d53 |
212 | Standard_ConstructionError_Raise_if (val <= gp::Resolution(), "gp_Mat::Invert() - matrix has zero determinant"); |
7fd59977 |
213 | det = 1.0e0 / det ; |
214 | M00 = N00; |
215 | M10 = N10; |
216 | M20 = N20; |
217 | M01 = N01; |
218 | M11 = N11; |
219 | M21 = N21; |
220 | M02 = N02; |
221 | M12 = N12; |
222 | M22 = N22; |
223 | Multiply(det) ; |
224 | } |
225 | |
226 | gp_Mat gp_Mat::Inverted () const |
227 | { |
228 | gp_Mat NewMat; |
229 | const Standard_Address M = (Standard_Address)&( matrix[0][0]); |
230 | const Standard_Address N = (Standard_Address)&(NewMat.matrix[0][0]); |
231 | // |
232 | // calcul de la transposee de la commatrice |
233 | // |
234 | N00 = M11 * M22 - M12 * M21 ; |
235 | N10 = -(M10 * M22 - M20 * M12) ; |
236 | N20 = M10 * M21 - M20 * M11 ; |
237 | N01 = - (M01 * M22 - M21 * M02) ; |
238 | N11 = M00 * M22 - M20 * M02 ; |
239 | N21 = -(M00 * M21 - M20 * M01) ; |
240 | N02 = M01 * M12 - M11 * M02 ; |
241 | N12 = -(M00 * M12 - M10 * M02) ; |
242 | N22 = M00 * M11 - M01 * M10 ; |
243 | Standard_Real det = M00 * N00 + M01* N10 + M02 * N20 ; |
244 | Standard_Real val = det; |
245 | if (val < 0) val = - val; |
2d2b3d53 |
246 | Standard_ConstructionError_Raise_if (val <= gp::Resolution(), "gp_Mat::Inverted() - matrix has zero determinant"); |
7fd59977 |
247 | det = 1.0e0 / det ; |
248 | NewMat.Multiply(det) ; |
249 | return NewMat; |
250 | } |
251 | |
252 | void gp_Mat::Power (const Standard_Integer N) |
253 | { |
254 | if (N == 1) { } |
255 | else if (N == 0) { SetIdentity() ; } |
256 | else if (N == -1) { Invert(); } |
257 | else { |
258 | if (N < 0) { Invert(); } |
259 | Standard_Integer Npower = N; |
260 | if (Npower < 0) Npower = - Npower; |
261 | Npower--; |
262 | gp_Mat Temp = *this; |
302f96fb |
263 | for(;;) { |
7fd59977 |
264 | if (IsOdd(Npower)) Multiply (Temp); |
265 | if (Npower == 1) break; |
266 | Temp.Multiply (Temp); |
267 | Npower>>=1; |
268 | } |
269 | } |
270 | } |
271 | |
0904aa63 |
272 | //======================================================================= |
273 | //function : DumpJson |
274 | //purpose : |
275 | //======================================================================= |
276 | void gp_Mat::DumpJson (Standard_OStream& theOStream, const Standard_Integer) const |
277 | { |
278 | DUMP_VECTOR_CLASS (theOStream, gp_Mat, 9, Mat00, Mat01, Mat02, Mat10, Mat11, Mat12, Mat20, Mat21, Mat22); |
279 | } |