b311480e |
1 | // Copyright (c) 1995-1999 Matra Datavision |
2 | // Copyright (c) 1999-2012 OPEN CASCADE SAS |
3 | // |
4 | // The content of this file is subject to the Open CASCADE Technology Public |
5 | // License Version 6.5 (the "License"). You may not use the content of this file |
6 | // except in compliance with the License. Please obtain a copy of the License |
7 | // at http://www.opencascade.org and read it completely before using this file. |
8 | // |
9 | // The Initial Developer of the Original Code is Open CASCADE S.A.S., having its |
10 | // main offices at: 1, place des Freres Montgolfier, 78280 Guyancourt, France. |
11 | // |
12 | // The Original Code and all software distributed under the License is |
13 | // distributed on an "AS IS" basis, without warranty of any kind, and the |
14 | // Initial Developer hereby disclaims all such warranties, including without |
15 | // limitation, any warranties of merchantability, fitness for a particular |
16 | // purpose or non-infringement. Please see the License for the specific terms |
17 | // and conditions governing the rights and limitations under the License. |
18 | |
7fd59977 |
19 | // 10/09/97 : PMN : Correction BUC40192 (pb avec les matrices negatives) |
20 | |
21 | #ifndef DEB |
22 | #define No_Standard_OutOfRange |
23 | #define No_Standard_ConstructionError |
24 | #endif |
25 | |
26 | #include <gp_Mat.ixx> |
27 | |
28 | #define M00 ((Standard_Real*)M)[0] |
29 | #define M01 ((Standard_Real*)M)[1] |
30 | #define M02 ((Standard_Real*)M)[2] |
31 | #define M10 ((Standard_Real*)M)[3] |
32 | #define M11 ((Standard_Real*)M)[4] |
33 | #define M12 ((Standard_Real*)M)[5] |
34 | #define M20 ((Standard_Real*)M)[6] |
35 | #define M21 ((Standard_Real*)M)[7] |
36 | #define M22 ((Standard_Real*)M)[8] |
37 | |
38 | #define N00 ((Standard_Real*)N)[0] |
39 | #define N01 ((Standard_Real*)N)[1] |
40 | #define N02 ((Standard_Real*)N)[2] |
41 | #define N10 ((Standard_Real*)N)[3] |
42 | #define N11 ((Standard_Real*)N)[4] |
43 | #define N12 ((Standard_Real*)N)[5] |
44 | #define N20 ((Standard_Real*)N)[6] |
45 | #define N21 ((Standard_Real*)N)[7] |
46 | #define N22 ((Standard_Real*)N)[8] |
47 | |
48 | gp_Mat::gp_Mat (const gp_XYZ& Col1, |
49 | const gp_XYZ& Col2, |
50 | const gp_XYZ& Col3) |
51 | { |
52 | const Standard_Address M = (Standard_Address)&(matrix[0][0]); |
53 | M00 = Col1.X(); M10 = Col1.Y(); M20 = Col1.Z(); |
54 | M01 = Col2.X(); M11 = Col2.Y(); M21 = Col2.Z(); |
55 | M02 = Col3.X(); M12 = Col3.Y(); M22 = Col3.Z(); |
56 | } |
57 | |
58 | void gp_Mat::SetCol (const Standard_Integer Col, |
59 | const gp_XYZ& Value) { |
60 | |
61 | Standard_OutOfRange_Raise_if (Col < 1 || Col > 3, " "); |
62 | const Standard_Address M = (Standard_Address)&(matrix[0][0]); |
63 | if (Col == 1) { |
64 | M00 = Value.X(); M10 = Value.Y(); M20 = Value.Z(); |
65 | } |
66 | else if (Col == 2) { |
67 | M01 = Value.X(); M11 = Value.Y(); M21 = Value.Z(); |
68 | } |
69 | else { |
70 | M02 = Value.X(); M12 = Value.Y(); M22 = Value.Z(); |
71 | } |
72 | } |
73 | |
74 | void gp_Mat::SetCols (const gp_XYZ& Col1, |
75 | const gp_XYZ& Col2, |
76 | const gp_XYZ& Col3) |
77 | { |
7fd59977 |
78 | Mat00 = Col1.X(); Mat10 = Col1.Y(); Mat20 = Col1.Z(); |
79 | Mat01 = Col2.X(); Mat11 = Col2.Y(); Mat21 = Col2.Z(); |
80 | Mat02 = Col3.X(); Mat12 = Col3.Y(); Mat22 = Col3.Z(); |
81 | } |
82 | |
83 | void gp_Mat::SetCross (const gp_XYZ& Ref) |
84 | { |
85 | const Standard_Address M = (Standard_Address)&(matrix[0][0]); |
86 | Standard_Real X = Ref.X(); |
87 | Standard_Real Y = Ref.Y(); |
88 | Standard_Real Z = Ref.Z(); |
89 | M00 = M11 = M22 = 0.0; |
90 | M01 = - Z; |
91 | M02 = Y; |
92 | M12 = - X; |
93 | M10 = Z; |
94 | M20 = - Y; |
95 | M21 = X; |
96 | } |
97 | |
98 | void gp_Mat::SetDot (const gp_XYZ& Ref) |
99 | { |
100 | const Standard_Address M = (Standard_Address)&(matrix[0][0]); |
101 | Standard_Real X = Ref.X(); |
102 | Standard_Real Y = Ref.Y(); |
103 | Standard_Real Z = Ref.Z(); |
104 | M00 = X * X; |
105 | M11 = Y * Y; |
106 | M22 = Z * Z; |
107 | M01 = X * Y; |
108 | M02 = X * Z; |
109 | M12 = Y * Z; |
110 | M10 = M01; |
111 | M20 = M02; |
112 | M21 = M12; |
113 | } |
114 | |
115 | void gp_Mat::SetRotation (const gp_XYZ& Axis, |
116 | const Standard_Real Ang) |
117 | { |
118 | // Rot = I + sin(Ang) * M + (1. - cos(Ang)) * M*M |
119 | // avec M . XYZ = Axis ^ XYZ |
120 | |
121 | // const Standard_Address M = (Standard_Address)&(matrix[0][0]); |
122 | gp_XYZ V = Axis.Normalized(); |
123 | SetCross (V); |
124 | Multiply (sin(Ang)); |
125 | gp_Mat Temp; |
126 | Temp.SetScale (1.0); |
127 | Add (Temp); |
128 | Standard_Real A = V.X(); |
129 | Standard_Real B = V.Y(); |
130 | Standard_Real C = V.Z(); |
131 | Temp.SetRow (1, gp_XYZ(- C*C - B*B, A*B, A*C )); |
132 | Temp.SetRow (2, gp_XYZ( A*B, -A*A - C*C, B*C )); |
133 | Temp.SetRow (3, gp_XYZ( A*C, B*C, - A*A - B*B)); |
134 | Temp.Multiply (1.0 - cos(Ang)); |
135 | Add (Temp); |
136 | } |
137 | |
138 | void gp_Mat::SetRow (const Standard_Integer Row, |
139 | const gp_XYZ& Value) |
140 | { |
141 | Standard_OutOfRange_Raise_if (Row < 1 || Row > 3, " "); |
142 | const Standard_Address M = (Standard_Address)&(matrix[0][0]); |
143 | if (Row == 1) { |
144 | M00 = Value.X(); M01 = Value.Y(); M02 = Value.Z(); |
145 | } |
146 | else if (Row == 2) { |
147 | M10 = Value.X(); M11 = Value.Y(); M12 = Value.Z(); |
148 | } |
149 | else { |
150 | M20 = Value.X(); M21 = Value.Y(); M22 = Value.Z(); |
151 | } |
152 | } |
153 | |
154 | void gp_Mat::SetRows (const gp_XYZ& Row1, |
155 | const gp_XYZ& Row2, |
156 | const gp_XYZ& Row3) |
157 | { |
158 | const Standard_Address M = (Standard_Address)&(matrix[0][0]); |
159 | M00 = Row1.X(); M01 = Row1.Y(); M02 = Row1.Z(); |
160 | M10 = Row2.X(); M11 = Row2.Y(); M12 = Row2.Z(); |
161 | M20 = Row3.X(); M21 = Row3.Y(); M22 = Row3.Z(); |
162 | } |
163 | |
164 | gp_XYZ gp_Mat::Column (const Standard_Integer Col) const |
165 | { |
166 | Standard_OutOfRange_Raise_if (Col < 1 || Col > 3, ""); |
167 | const Standard_Address M = (Standard_Address)&(matrix[0][0]); |
168 | if (Col == 1) return gp_XYZ (M00,M10,M20); |
169 | if (Col == 2) return gp_XYZ (M01,M11,M21); |
170 | return gp_XYZ (M02,M12,M22); |
171 | } |
172 | |
173 | gp_XYZ gp_Mat::Diagonal () const |
174 | { |
175 | const Standard_Address M = (Standard_Address)&(matrix[0][0]); |
176 | return gp_XYZ (M00, M11, M22); |
177 | } |
178 | |
179 | gp_XYZ gp_Mat::Row (const Standard_Integer Row) const |
180 | { |
181 | Standard_OutOfRange_Raise_if (Row < 1 || Row > 3, ""); |
182 | const Standard_Address M = (Standard_Address)&(matrix[0][0]); |
183 | if (Row == 1) return gp_XYZ (M00,M01,M02); |
184 | if (Row == 2) return gp_XYZ (M10,M11,M12); |
185 | return gp_XYZ (M20,M21,M22); |
186 | } |
187 | |
188 | void gp_Mat::Invert () |
189 | { |
190 | Standard_Real new_array[3][3] ; |
191 | const Standard_Address M = (Standard_Address)&( matrix[0][0]); |
192 | const Standard_Address N = (Standard_Address)&(new_array[0][0]); |
193 | |
194 | // |
195 | // calcul de la transposee de la commatrice |
196 | // |
197 | N00 = M11 * M22 - M12 * M21 ; |
198 | N10 = -(M10 * M22 - M20 * M12) ; |
199 | N20 = M10 * M21 - M20 * M11 ; |
200 | N01 = - (M01 * M22 - M21 * M02) ; |
201 | N11 = M00 * M22 - M20 * M02 ; |
202 | N21 = -(M00 * M21 - M20 * M01) ; |
203 | N02 = M01 * M12 - M11 * M02 ; |
204 | N12 = -(M00 * M12 - M10 * M02) ; |
205 | N22 = M00 * M11 - M01 * M10 ; |
206 | Standard_Real det = M00 * N00 + M01* N10 + M02 * N20 ; |
207 | Standard_Real val = det; |
208 | if (val < 0) val = - val; |
209 | Standard_ConstructionError_Raise_if |
210 | (val <= gp::Resolution(),""); |
211 | det = 1.0e0 / det ; |
212 | M00 = N00; |
213 | M10 = N10; |
214 | M20 = N20; |
215 | M01 = N01; |
216 | M11 = N11; |
217 | M21 = N21; |
218 | M02 = N02; |
219 | M12 = N12; |
220 | M22 = N22; |
221 | Multiply(det) ; |
222 | } |
223 | |
224 | gp_Mat gp_Mat::Inverted () const |
225 | { |
226 | gp_Mat NewMat; |
227 | const Standard_Address M = (Standard_Address)&( matrix[0][0]); |
228 | const Standard_Address N = (Standard_Address)&(NewMat.matrix[0][0]); |
229 | // |
230 | // calcul de la transposee de la commatrice |
231 | // |
232 | N00 = M11 * M22 - M12 * M21 ; |
233 | N10 = -(M10 * M22 - M20 * M12) ; |
234 | N20 = M10 * M21 - M20 * M11 ; |
235 | N01 = - (M01 * M22 - M21 * M02) ; |
236 | N11 = M00 * M22 - M20 * M02 ; |
237 | N21 = -(M00 * M21 - M20 * M01) ; |
238 | N02 = M01 * M12 - M11 * M02 ; |
239 | N12 = -(M00 * M12 - M10 * M02) ; |
240 | N22 = M00 * M11 - M01 * M10 ; |
241 | Standard_Real det = M00 * N00 + M01* N10 + M02 * N20 ; |
242 | Standard_Real val = det; |
243 | if (val < 0) val = - val; |
244 | Standard_ConstructionError_Raise_if |
245 | (val <= gp::Resolution(),""); |
246 | det = 1.0e0 / det ; |
247 | NewMat.Multiply(det) ; |
248 | return NewMat; |
249 | } |
250 | |
251 | void gp_Mat::Power (const Standard_Integer N) |
252 | { |
253 | if (N == 1) { } |
254 | else if (N == 0) { SetIdentity() ; } |
255 | else if (N == -1) { Invert(); } |
256 | else { |
257 | if (N < 0) { Invert(); } |
258 | Standard_Integer Npower = N; |
259 | if (Npower < 0) Npower = - Npower; |
260 | Npower--; |
261 | gp_Mat Temp = *this; |
302f96fb |
262 | for(;;) { |
7fd59977 |
263 | if (IsOdd(Npower)) Multiply (Temp); |
264 | if (Npower == 1) break; |
265 | Temp.Multiply (Temp); |
266 | Npower>>=1; |
267 | } |
268 | } |
269 | } |
270 | |