1 //
2 // Copyright 2016 Pixar
3 //
4 // Licensed under the Apache License, Version 2.0 (the "Apache License")
5 // with the following modification; you may not use this file except in
6 // compliance with the Apache License and the following modification to it:
7 // Section 6. Trademarks. is deleted and replaced with:
8 //
9 // 6. Trademarks. This License does not grant permission to use the trade
10 // names, trademarks, service marks, or product names of the Licensor
11 // and its affiliates, except as required to comply with Section 4(c) of
12 // the License and to reproduce the content of the NOTICE file.
13 //
14 // You may obtain a copy of the Apache License at
15 //
16 // http://www.apache.org/licenses/LICENSE-2.0
17 //
18 // Unless required by applicable law or agreed to in writing, software
19 // distributed under the Apache License with the above modification is
20 // distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
21 // KIND, either express or implied. See the Apache License for the specific
22 // language governing permissions and limitations under the Apache License.
23 //
24 ////////////////////////////////////////////////////////////////////////
25 // This file is generated by a script. Do not edit directly. Edit the
26 // matrix3.template.cpp file to make changes.
27
28
29 #include "pxr/pxr.h"
30 #include "pxr/base/gf/matrix3d.h"
31 #include "pxr/base/gf/matrix3f.h"
32
33 #include "pxr/base/gf/math.h"
34 #include "pxr/base/gf/ostreamHelpers.h"
35 #include "pxr/base/tf/type.h"
36
37 #include "pxr/base/gf/quatd.h"
38 #include "pxr/base/gf/rotation.h"
39 #include <float.h>
40 #include <ostream>
41
42 PXR_NAMESPACE_OPEN_SCOPE
43
TF_REGISTRY_FUNCTION(TfType)44 TF_REGISTRY_FUNCTION(TfType) {
45 TfType::Define<GfMatrix3d>();
46 }
47
48 std::ostream&
operator <<(std::ostream & out,const GfMatrix3d & m)49 operator<<(std::ostream& out, const GfMatrix3d& m)
50 {
51 return out
52 << "( ("
53 << Gf_OstreamHelperP(m[0][0]) << ", "
54 << Gf_OstreamHelperP(m[0][1]) << ", "
55 << Gf_OstreamHelperP(m[0][2])
56 << "), ("
57 << Gf_OstreamHelperP(m[1][0]) << ", "
58 << Gf_OstreamHelperP(m[1][1]) << ", "
59 << Gf_OstreamHelperP(m[1][2])
60 << "), ("
61 << Gf_OstreamHelperP(m[2][0]) << ", "
62 << Gf_OstreamHelperP(m[2][1]) << ", "
63 << Gf_OstreamHelperP(m[2][2])
64 << ") )";
65 }
66
GfMatrix3d(const GfMatrix3f & m)67 GfMatrix3d::GfMatrix3d(const GfMatrix3f& m)
68 {
69 Set(m[0][0], m[0][1], m[0][2],
70 m[1][0], m[1][1], m[1][2],
71 m[2][0], m[2][1], m[2][2]);
72 }
73
GfMatrix3d(const std::vector<std::vector<double>> & v)74 GfMatrix3d::GfMatrix3d(const std::vector< std::vector<double> >& v)
75 {
76 double m[3][3] = {{1.0, 0.0, 0.0},
77 {0.0, 1.0, 0.0},
78 {0.0, 0.0, 1.0}};
79 for(size_t row = 0; row < 3 && row < v.size(); ++row) {
80 for (size_t col = 0; col < 3 && col < v[row].size(); ++col) {
81 m[row][col] = v[row][col];
82 }
83 }
84 Set(m);
85 }
86
GfMatrix3d(const std::vector<std::vector<float>> & v)87 GfMatrix3d::GfMatrix3d(const std::vector< std::vector<float> >& v)
88 {
89 double m[3][3] = {{1.0, 0.0, 0.0},
90 {0.0, 1.0, 0.0},
91 {0.0, 0.0, 1.0}};
92 for(size_t row = 0; row < 3 && row < v.size(); ++row) {
93 for (size_t col = 0; col < 3 && col < v[row].size(); ++col) {
94 m[row][col] = v[row][col];
95 }
96 }
97 Set(m);
98 }
99
GfMatrix3d(const GfRotation & rot)100 GfMatrix3d::GfMatrix3d(const GfRotation &rot)
101 {
102 SetRotate(rot);
103 }
104
GfMatrix3d(const GfQuatd & rot)105 GfMatrix3d::GfMatrix3d(const GfQuatd &rot)
106 {
107 SetRotate(rot);
108 }
109
110 GfMatrix3d &
SetDiagonal(double s)111 GfMatrix3d::SetDiagonal(double s)
112 {
113 _mtx[0][0] = s;
114 _mtx[0][1] = 0.0;
115 _mtx[0][2] = 0.0;
116 _mtx[1][0] = 0.0;
117 _mtx[1][1] = s;
118 _mtx[1][2] = 0.0;
119 _mtx[2][0] = 0.0;
120 _mtx[2][1] = 0.0;
121 _mtx[2][2] = s;
122 return *this;
123 }
124
125 GfMatrix3d &
SetDiagonal(const GfVec3d & v)126 GfMatrix3d::SetDiagonal(const GfVec3d& v)
127 {
128 _mtx[0][0] = v[0]; _mtx[0][1] = 0.0; _mtx[0][2] = 0.0;
129 _mtx[1][0] = 0.0; _mtx[1][1] = v[1]; _mtx[1][2] = 0.0;
130 _mtx[2][0] = 0.0; _mtx[2][1] = 0.0; _mtx[2][2] = v[2];
131 return *this;
132 }
133
134 double *
Get(double m[3][3]) const135 GfMatrix3d::Get(double m[3][3]) const
136 {
137 m[0][0] = _mtx[0][0];
138 m[0][1] = _mtx[0][1];
139 m[0][2] = _mtx[0][2];
140 m[1][0] = _mtx[1][0];
141 m[1][1] = _mtx[1][1];
142 m[1][2] = _mtx[1][2];
143 m[2][0] = _mtx[2][0];
144 m[2][1] = _mtx[2][1];
145 m[2][2] = _mtx[2][2];
146 return &m[0][0];
147 }
148
149 bool
operator ==(const GfMatrix3d & m) const150 GfMatrix3d::operator ==(const GfMatrix3d &m) const
151 {
152 return (_mtx[0][0] == m._mtx[0][0] &&
153 _mtx[0][1] == m._mtx[0][1] &&
154 _mtx[0][2] == m._mtx[0][2] &&
155 _mtx[1][0] == m._mtx[1][0] &&
156 _mtx[1][1] == m._mtx[1][1] &&
157 _mtx[1][2] == m._mtx[1][2] &&
158 _mtx[2][0] == m._mtx[2][0] &&
159 _mtx[2][1] == m._mtx[2][1] &&
160 _mtx[2][2] == m._mtx[2][2]);
161 }
162
163 bool
operator ==(const GfMatrix3f & m) const164 GfMatrix3d::operator ==(const GfMatrix3f &m) const
165 {
166 return (_mtx[0][0] == m._mtx[0][0] &&
167 _mtx[0][1] == m._mtx[0][1] &&
168 _mtx[0][2] == m._mtx[0][2] &&
169 _mtx[1][0] == m._mtx[1][0] &&
170 _mtx[1][1] == m._mtx[1][1] &&
171 _mtx[1][2] == m._mtx[1][2] &&
172 _mtx[2][0] == m._mtx[2][0] &&
173 _mtx[2][1] == m._mtx[2][1] &&
174 _mtx[2][2] == m._mtx[2][2]);
175 }
176
177
178 GfMatrix3d
GetTranspose() const179 GfMatrix3d::GetTranspose() const
180 {
181 GfMatrix3d transpose;
182 transpose._mtx[0][0] = _mtx[0][0];
183 transpose._mtx[1][0] = _mtx[0][1];
184 transpose._mtx[2][0] = _mtx[0][2];
185 transpose._mtx[0][1] = _mtx[1][0];
186 transpose._mtx[1][1] = _mtx[1][1];
187 transpose._mtx[2][1] = _mtx[1][2];
188 transpose._mtx[0][2] = _mtx[2][0];
189 transpose._mtx[1][2] = _mtx[2][1];
190 transpose._mtx[2][2] = _mtx[2][2];
191
192 return transpose;
193 }
194
195 GfMatrix3d
GetInverse(double * detPtr,double eps) const196 GfMatrix3d::GetInverse(double *detPtr, double eps) const
197 {
198 double a00,a01,a02,a10,a11,a12,a20,a21,a22;
199 double det, rcp;
200
201 a00 = _mtx[0][0];
202 a01 = _mtx[0][1];
203 a02 = _mtx[0][2];
204 a10 = _mtx[1][0];
205 a11 = _mtx[1][1];
206 a12 = _mtx[1][2];
207 a20 = _mtx[2][0];
208 a21 = _mtx[2][1];
209 a22 = _mtx[2][2];
210 det = -(a02*a11*a20) + a01*a12*a20 + a02*a10*a21 -
211 a00*a12*a21 - a01*a10*a22 + a00*a11*a22;
212
213 if (detPtr) {
214 *detPtr = det;
215 }
216
217 GfMatrix3d inverse;
218
219 if (GfAbs(det) > eps) {
220 rcp = 1.0 / det;
221 inverse._mtx[0][0] = (-(a12*a21) + a11*a22)*rcp;
222 inverse._mtx[0][1] = (a02*a21 - a01*a22)*rcp;
223 inverse._mtx[0][2] = (-(a02*a11) + a01*a12)*rcp;
224 inverse._mtx[1][0] = (a12*a20 - a10*a22)*rcp;
225 inverse._mtx[1][1] = (-(a02*a20) + a00*a22)*rcp;
226 inverse._mtx[1][2] = (a02*a10 - a00*a12)*rcp;
227 inverse._mtx[2][0] = (-(a11*a20) + a10*a21)*rcp;
228 inverse._mtx[2][1] = (a01*a20 - a00*a21)*rcp;
229 inverse._mtx[2][2] = (-(a01*a10) + a00*a11)*rcp;
230
231 }
232 else {
233 inverse.SetScale(FLT_MAX);
234 }
235
236 return inverse;
237 }
238
239 double
GetDeterminant() const240 GfMatrix3d::GetDeterminant() const
241 {
242 return (_mtx[0][0] * _mtx[1][1] * _mtx[2][2] +
243 _mtx[0][1] * _mtx[1][2] * _mtx[2][0] +
244 _mtx[0][2] * _mtx[1][0] * _mtx[2][1] -
245 _mtx[0][0] * _mtx[1][2] * _mtx[2][1] -
246 _mtx[0][1] * _mtx[1][0] * _mtx[2][2] -
247 _mtx[0][2] * _mtx[1][1] * _mtx[2][0]);
248 }
249
250 double
GetHandedness() const251 GfMatrix3d::GetHandedness() const
252 {
253 // Note: This can be computed with fewer arithmetic operations using a
254 // cross and dot product, but it is more important that the result
255 // is consistent with the way the determinant is computed.
256 return GfSgn(GetDeterminant());
257 }
258
259 /* Make the matrix orthonormal in place using an iterative method.
260 * It is potentially slower if the matrix is far from orthonormal (i.e. if
261 * the row basis vectors are close to colinear) but in the common case
262 * of near-orthonormality it should be just as fast. */
263 bool
Orthonormalize(bool issueWarning)264 GfMatrix3d::Orthonormalize(bool issueWarning)
265 {
266 // orthogonalize and normalize row vectors
267 GfVec3d r0(_mtx[0][0],_mtx[0][1],_mtx[0][2]);
268 GfVec3d r1(_mtx[1][0],_mtx[1][1],_mtx[1][2]);
269 GfVec3d r2(_mtx[2][0],_mtx[2][1],_mtx[2][2]);
270 bool result = GfVec3d::OrthogonalizeBasis(&r0, &r1, &r2, true);
271 _mtx[0][0] = r0[0];
272 _mtx[0][1] = r0[1];
273 _mtx[0][2] = r0[2];
274 _mtx[1][0] = r1[0];
275 _mtx[1][1] = r1[1];
276 _mtx[1][2] = r1[2];
277 _mtx[2][0] = r2[0];
278 _mtx[2][1] = r2[1];
279 _mtx[2][2] = r2[2];
280 if (!result && issueWarning)
281 TF_WARN("OrthogonalizeBasis did not converge, matrix may not be "
282 "orthonormal.");
283 return result;
284 }
285
286 GfMatrix3d
GetOrthonormalized(bool issueWarning) const287 GfMatrix3d::GetOrthonormalized(bool issueWarning) const
288 {
289 GfMatrix3d result = *this;
290 result.Orthonormalize(issueWarning);
291 return result;
292 }
293
294 /*
295 ** Scaling
296 */
297 GfMatrix3d&
operator *=(double d)298 GfMatrix3d::operator*=(double d)
299 {
300 _mtx[0][0] *= d; _mtx[0][1] *= d; _mtx[0][2] *= d;
301 _mtx[1][0] *= d; _mtx[1][1] *= d; _mtx[1][2] *= d;
302 _mtx[2][0] *= d; _mtx[2][1] *= d; _mtx[2][2] *= d;
303 return *this;
304 }
305
306 /*
307 ** Addition
308 */
309 GfMatrix3d &
operator +=(const GfMatrix3d & m)310 GfMatrix3d::operator+=(const GfMatrix3d &m)
311 {
312 _mtx[0][0] += m._mtx[0][0];
313 _mtx[0][1] += m._mtx[0][1];
314 _mtx[0][2] += m._mtx[0][2];
315 _mtx[1][0] += m._mtx[1][0];
316 _mtx[1][1] += m._mtx[1][1];
317 _mtx[1][2] += m._mtx[1][2];
318 _mtx[2][0] += m._mtx[2][0];
319 _mtx[2][1] += m._mtx[2][1];
320 _mtx[2][2] += m._mtx[2][2];
321 return *this;
322 }
323
324 /*
325 ** Subtraction
326 */
327 GfMatrix3d &
operator -=(const GfMatrix3d & m)328 GfMatrix3d::operator-=(const GfMatrix3d &m)
329 {
330 _mtx[0][0] -= m._mtx[0][0];
331 _mtx[0][1] -= m._mtx[0][1];
332 _mtx[0][2] -= m._mtx[0][2];
333 _mtx[1][0] -= m._mtx[1][0];
334 _mtx[1][1] -= m._mtx[1][1];
335 _mtx[1][2] -= m._mtx[1][2];
336 _mtx[2][0] -= m._mtx[2][0];
337 _mtx[2][1] -= m._mtx[2][1];
338 _mtx[2][2] -= m._mtx[2][2];
339 return *this;
340 }
341
342 /*
343 ** Negation
344 */
345 GfMatrix3d
operator -(const GfMatrix3d & m)346 operator -(const GfMatrix3d& m)
347 {
348 return
349 GfMatrix3d(-m._mtx[0][0], -m._mtx[0][1], -m._mtx[0][2],
350 -m._mtx[1][0], -m._mtx[1][1], -m._mtx[1][2],
351 -m._mtx[2][0], -m._mtx[2][1], -m._mtx[2][2]);
352 }
353
354 GfMatrix3d &
operator *=(const GfMatrix3d & m)355 GfMatrix3d::operator*=(const GfMatrix3d &m)
356 {
357 // Save current values before they are overwritten
358 GfMatrix3d tmp = *this;
359
360 _mtx[0][0] = tmp._mtx[0][0] * m._mtx[0][0] +
361 tmp._mtx[0][1] * m._mtx[1][0] +
362 tmp._mtx[0][2] * m._mtx[2][0];
363
364 _mtx[0][1] = tmp._mtx[0][0] * m._mtx[0][1] +
365 tmp._mtx[0][1] * m._mtx[1][1] +
366 tmp._mtx[0][2] * m._mtx[2][1];
367
368 _mtx[0][2] = tmp._mtx[0][0] * m._mtx[0][2] +
369 tmp._mtx[0][1] * m._mtx[1][2] +
370 tmp._mtx[0][2] * m._mtx[2][2];
371
372 _mtx[1][0] = tmp._mtx[1][0] * m._mtx[0][0] +
373 tmp._mtx[1][1] * m._mtx[1][0] +
374 tmp._mtx[1][2] * m._mtx[2][0];
375
376 _mtx[1][1] = tmp._mtx[1][0] * m._mtx[0][1] +
377 tmp._mtx[1][1] * m._mtx[1][1] +
378 tmp._mtx[1][2] * m._mtx[2][1];
379
380 _mtx[1][2] = tmp._mtx[1][0] * m._mtx[0][2] +
381 tmp._mtx[1][1] * m._mtx[1][2] +
382 tmp._mtx[1][2] * m._mtx[2][2];
383
384 _mtx[2][0] = tmp._mtx[2][0] * m._mtx[0][0] +
385 tmp._mtx[2][1] * m._mtx[1][0] +
386 tmp._mtx[2][2] * m._mtx[2][0];
387
388 _mtx[2][1] = tmp._mtx[2][0] * m._mtx[0][1] +
389 tmp._mtx[2][1] * m._mtx[1][1] +
390 tmp._mtx[2][2] * m._mtx[2][1];
391
392 _mtx[2][2] = tmp._mtx[2][0] * m._mtx[0][2] +
393 tmp._mtx[2][1] * m._mtx[1][2] +
394 tmp._mtx[2][2] * m._mtx[2][2];
395
396 return *this;
397 }
398
399 /*
400 * Define multiplication between floating vector and double matrix.
401 */
402 GfVec3f
operator *(const GfVec3f & vec,const GfMatrix3d & m)403 operator *(const GfVec3f &vec, const GfMatrix3d &m)
404 {
405 return GfVec3f(
406 float(vec[0] * m._mtx[0][0] + vec[1] * m._mtx[1][0] + vec[2] * m._mtx[2][0]),
407 float(vec[0] * m._mtx[0][1] + vec[1] * m._mtx[1][1] + vec[2] * m._mtx[2][1]),
408 float(vec[0] * m._mtx[0][2] + vec[1] * m._mtx[1][2] + vec[2] * m._mtx[2][2]));
409 }
410
411 GfVec3f
operator *(const GfMatrix3d & m,const GfVec3f & vec)412 operator *(const GfMatrix3d& m, const GfVec3f &vec)
413 {
414 return GfVec3f(
415 float(vec[0] * m._mtx[0][0] + vec[1] * m._mtx[0][1] + vec[2] * m._mtx[0][2]),
416 float(vec[0] * m._mtx[1][0] + vec[1] * m._mtx[1][1] + vec[2] * m._mtx[1][2]),
417 float(vec[0] * m._mtx[2][0] + vec[1] * m._mtx[2][1] + vec[2] * m._mtx[2][2]));
418 }
419 GfMatrix3d &
SetScale(double s)420 GfMatrix3d::SetScale(double s)
421 {
422 _mtx[0][0] = s; _mtx[0][1] = 0.0; _mtx[0][2] = 0.0;
423 _mtx[1][0] = 0.0; _mtx[1][1] = s; _mtx[1][2] = 0.0;
424 _mtx[2][0] = 0.0; _mtx[2][1] = 0.0; _mtx[2][2] = s;
425
426 return *this;
427 }
428
429 GfMatrix3d &
SetRotate(const GfQuatd & rot)430 GfMatrix3d::SetRotate(const GfQuatd &rot)
431 {
432 _SetRotateFromQuat(rot.GetReal(), rot.GetImaginary());
433 return *this;
434 }
435
436 GfMatrix3d &
SetRotate(const GfRotation & rot)437 GfMatrix3d::SetRotate(const GfRotation &rot)
438 {
439 GfQuaternion quat = rot.GetQuaternion();
440 _SetRotateFromQuat(quat.GetReal(), GfVec3d(quat.GetImaginary()));
441 return *this;
442 }
443
444 void
_SetRotateFromQuat(double r,const GfVec3d & i)445 GfMatrix3d::_SetRotateFromQuat(double r, const GfVec3d& i)
446 {
447 _mtx[0][0] = 1.0 - 2.0 * (i[1] * i[1] + i[2] * i[2]);
448 _mtx[0][1] = 2.0 * (i[0] * i[1] + i[2] * r);
449 _mtx[0][2] = 2.0 * (i[2] * i[0] - i[1] * r);
450
451 _mtx[1][0] = 2.0 * (i[0] * i[1] - i[2] * r);
452 _mtx[1][1] = 1.0 - 2.0 * (i[2] * i[2] + i[0] * i[0]);
453 _mtx[1][2] = 2.0 * (i[1] * i[2] + i[0] * r);
454
455 _mtx[2][0] = 2.0 * (i[2] * i[0] + i[1] * r);
456 _mtx[2][1] = 2.0 * (i[1] * i[2] - i[0] * r);
457 _mtx[2][2] = 1.0 - 2.0 * (i[1] * i[1] + i[0] * i[0]);
458 }
459
460
461 GfMatrix3d &
SetScale(const GfVec3d & s)462 GfMatrix3d::SetScale(const GfVec3d &s)
463 {
464 _mtx[0][0] = s[0]; _mtx[0][1] = 0.0; _mtx[0][2] = 0.0;
465 _mtx[1][0] = 0.0; _mtx[1][1] = s[1]; _mtx[1][2] = 0.0;
466 _mtx[2][0] = 0.0; _mtx[2][1] = 0.0; _mtx[2][2] = s[2];
467
468 return *this;
469 }
470
471 GfQuaternion
ExtractRotationQuaternion() const472 GfMatrix3d::ExtractRotationQuaternion() const
473 {
474 // This was adapted from the (open source) Open Inventor
475 // SbRotation::SetValue(const SbMatrix &m)
476
477 int i;
478
479 // First, find largest diagonal in matrix:
480 if (_mtx[0][0] > _mtx[1][1])
481 i = (_mtx[0][0] > _mtx[2][2] ? 0 : 2);
482 else
483 i = (_mtx[1][1] > _mtx[2][2] ? 1 : 2);
484
485 GfVec3d im;
486 double r;
487
488 if (_mtx[0][0] + _mtx[1][1] + _mtx[2][2] > _mtx[i][i]) {
489 r = 0.5 * sqrt(_mtx[0][0] + _mtx[1][1] +
490 _mtx[2][2] + 1);
491 im.Set((_mtx[1][2] - _mtx[2][1]) / (4.0 * r),
492 (_mtx[2][0] - _mtx[0][2]) / (4.0 * r),
493 (_mtx[0][1] - _mtx[1][0]) / (4.0 * r));
494 }
495 else {
496 int j = (i + 1) % 3;
497 int k = (i + 2) % 3;
498 double q = 0.5 * sqrt(_mtx[i][i] - _mtx[j][j] -
499 _mtx[k][k] + 1);
500
501 im[i] = q;
502 im[j] = (_mtx[i][j] + _mtx[j][i]) / (4 * q);
503 im[k] = (_mtx[k][i] + _mtx[i][k]) / (4 * q);
504 r = (_mtx[j][k] - _mtx[k][j]) / (4 * q);
505 }
506
507 return GfQuaternion(GfClamp(r, -1.0, 1.0), im);
508 }
509
510 GfRotation
ExtractRotation() const511 GfMatrix3d::ExtractRotation() const
512 {
513 return GfRotation( ExtractRotationQuaternion() );
514 }
515
516 GfVec3d
DecomposeRotation(const GfVec3d & axis0,const GfVec3d & axis1,const GfVec3d & axis2) const517 GfMatrix3d::DecomposeRotation(const GfVec3d &axis0,
518 const GfVec3d &axis1,
519 const GfVec3d &axis2) const
520 {
521 return (ExtractRotation().Decompose(axis0, axis1, axis2));
522 }
523
524
525 bool
GfIsClose(GfMatrix3d const & m1,GfMatrix3d const & m2,double tolerance)526 GfIsClose(GfMatrix3d const &m1, GfMatrix3d const &m2, double tolerance)
527 {
528 for(size_t row = 0; row < 3; ++row) {
529 for(size_t col = 0; col < 3; ++col) {
530 if(!GfIsClose(m1[row][col], m2[row][col], tolerance))
531 return false;
532 }
533 }
534 return true;
535 }
536
537
538 PXR_NAMESPACE_CLOSE_SCOPE
539