1 /*
2 *
3 * Mathematics Subpackage (VrMath)
4 *
5 *
6 * Author: Samuel R. Buss, sbuss@ucsd.edu.
7 * Web page: http://math.ucsd.edu/~sbuss/MathCG
8 *
9 *
10 This software is provided 'as-is', without any express or implied warranty.
11 In no event will the authors be held liable for any damages arising from the use of this software.
12 Permission is granted to anyone to use this software for any purpose,
13 including commercial applications, and to alter it and redistribute it freely,
14 subject to the following restrictions:
15
16 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
18 3. This notice may not be removed or altered from any source distribution.
19 *
20 *
21 */
22
23 //
24 // Linear Algebra Classes over R2
25 //
26 //
27 // A. Vector and Position classes
28 //
29 // A.1. VectorR2: a column vector of length 2
30 //
31 // A.2. VectorHgR2 - homogenous vector for R2 (a 3-Vector)
32 //
33 // B. Matrix Classes
34 //
35 // B.1 LinearMapR2 - arbitrary linear map; 2x2 real matrix
36 //
37 // B.2 RotationMapR2 - orthonormal 2x2 matrix
38 //
39
40 #ifndef LINEAR_R2_H
41 #define LINEAR_R2_H
42
43 #include <math.h>
44 #include <assert.h>
45 #include <iostream>
46 #include "MathMisc.h"
47 using namespace std;
48
49 class VectorR2; // R2 Vector
50 class VectorHgR2;
51 class Matrix2x2;
52 class LinearMapR2; // 2x2 real matrix
53 class AffineMapR3; // Affine Map (3x4 Matrix)
54 class RotationMapR2; // 2x2 rotation map
55
56 // **************************************
57 // VectorR2 class *
58 // * * * * * * * * * * * * * * * * * * **
59
60 class VectorR2
61 {
62 public:
63 double x, y; // The x & y coordinates.
64
65 public:
VectorR2()66 VectorR2() : x(0.0), y(0.0) {}
VectorR2(double xVal,double yVal)67 VectorR2(double xVal, double yVal)
68 : x(xVal), y(yVal) {}
69 VectorR2(const VectorHgR2& uH);
70
SetZero()71 VectorR2& SetZero()
72 {
73 x = 0.0;
74 y = 0.0;
75 return *this;
76 }
Set(double xx,double yy)77 VectorR2& Set(double xx, double yy)
78 {
79 x = xx;
80 y = yy;
81 return *this;
82 }
83 VectorR2& Load(const double* v);
84 VectorR2& Load(const float* v);
85 void Dump(double* v) const;
86 void Dump(float* v) const;
87
88 static const VectorR2 Zero;
89 static const VectorR2 UnitX;
90 static const VectorR2 UnitY;
91 static const VectorR2 NegUnitX;
92 static const VectorR2 NegUnitY;
93
94 VectorR2& operator+=(const VectorR2& v)
95 {
96 x += v.x;
97 y += v.y;
98 return (*this);
99 }
100 VectorR2& operator-=(const VectorR2& v)
101 {
102 x -= v.x;
103 y -= v.y;
104 return (*this);
105 }
106 VectorR2& operator*=(double m)
107 {
108 x *= m;
109 y *= m;
110 return (*this);
111 }
112 VectorR2& operator/=(double m)
113 {
114 double mInv = 1.0 / m;
115 x *= mInv;
116 y *= mInv;
117 return (*this);
118 }
119 VectorR2 operator-() const { return (VectorR2(-x, -y)); }
120 VectorR2& ArrayProd(const VectorR2&); // Component-wise product
121
122 VectorR2& AddScaled(const VectorR2& u, double s);
123
Norm()124 double Norm() const { return (sqrt(x * x + y * y)); }
L1Norm()125 double L1Norm() const { return (Max(fabs(x), fabs(y))); }
126 double Dist(const VectorR2& u) const; // Distance from u
127 double DistSq(const VectorR2& u) const; // Distance from u
NormSq()128 double NormSq() const { return (x * x + y * y); }
129 double MaxAbs() const;
Normalize()130 VectorR2& Normalize()
131 {
132 *this /= Norm();
133 return *this;
134 } // No error checking
135 VectorR2& MakeUnit(); // Normalize() with error checking
136 VectorR2& ReNormalize();
137 bool IsUnit(double tolerance = 1.0e-15) const
138 {
139 double norm = Norm();
140 return (1.0 + tolerance >= norm && norm >= 1.0 - tolerance);
141 }
IsZero()142 bool IsZero() const { return (x == 0.0 && y == 0.0); }
NearZero(double tolerance)143 bool NearZero(double tolerance) const { return (MaxAbs() <= tolerance); }
144 // tolerance should be non-negative
145
146 VectorR2& Rotate(double theta); // rotate through angle theta
147 VectorR2& Rotate(double costheta, double sintheta);
148 };
149
150 inline VectorR2 operator+(const VectorR2& u, const VectorR2& v);
151 inline VectorR2 operator-(const VectorR2& u, const VectorR2& v);
152 inline VectorR2 operator*(const VectorR2& u, double m);
153 inline VectorR2 operator*(double m, const VectorR2& u);
154 inline VectorR2 operator/(const VectorR2& u, double m);
155 inline int operator==(const VectorR2& u, const VectorR2& v);
156
157 inline double operator^(const VectorR2& u, const VectorR2& v); // Dot Product
158 inline VectorR2 ArrayProd(const VectorR2& u, const VectorR2& v);
159
Mag(const VectorR2 & u)160 inline double Mag(const VectorR2& u) { return u.Norm(); }
Dist(const VectorR2 & u,const VectorR2 & v)161 inline double Dist(const VectorR2& u, const VectorR2& v) { return u.Dist(v); }
DistSq(const VectorR2 & u,const VectorR2 & v)162 inline double DistSq(const VectorR2& u, const VectorR2& v) { return u.DistSq(v); }
163 inline double NormalizeError(const VectorR2&);
164
165 // ****************************************
166 // VectorHgR2 class *
167 // * * * * * * * * * * * * * * * * * * * **
168
169 class VectorHgR2
170 {
171 public:
172 double x, y, w; // The x & y & w coordinates.
173
174 public:
VectorHgR2()175 VectorHgR2() : x(0.0), y(0.0), w(1.0) {}
VectorHgR2(double xVal,double yVal)176 VectorHgR2(double xVal, double yVal)
177 : x(xVal), y(yVal), w(1.0) {}
VectorHgR2(double xVal,double yVal,double wVal)178 VectorHgR2(double xVal, double yVal, double wVal)
179 : x(xVal), y(yVal), w(wVal) {}
VectorHgR2(const VectorR2 & u)180 VectorHgR2(const VectorR2& u) : x(u.x), y(u.y), w(1.0) {}
181 };
182
183 // ********************************************************************
184 // Matrix2x2 - base class for 2x2 matrices *
185 // * * * * * * * * * * * * * * * * * * * * * **************************
186
187 class Matrix2x2
188 {
189 public:
190 double m11, m12, m21, m22;
191
192 // Implements a 2x2 matrix: m_i_j - row-i and column-j entry
193
194 static const Matrix2x2 Identity;
195
196 public:
197 inline Matrix2x2();
198 inline Matrix2x2(const VectorR2&, const VectorR2&); // Sets by columns!
199 inline Matrix2x2(double, double, double, double); // Sets by columns
200
201 inline void SetIdentity(); // Set to the identity map
202 inline void SetZero(); // Set to the zero map
203 inline void Set(const VectorR2&, const VectorR2&);
204 inline void Set(double, double, double, double);
205 inline void SetByRows(const VectorR2&, const VectorR2&);
206 inline void SetByRows(double, double, double, double);
207 inline void SetColumn1(double, double);
208 inline void SetColumn2(double, double);
209 inline void SetColumn1(const VectorR2&);
210 inline void SetColumn2(const VectorR2&);
211 inline VectorR2 Column1() const;
212 inline VectorR2 Column2() const;
213
214 inline void SetRow1(double, double);
215 inline void SetRow2(double, double);
216 inline void SetRow1(const VectorR2&);
217 inline void SetRow2(const VectorR2&);
218 inline VectorR2 Row1() const;
219 inline VectorR2 Row2() const;
220
221 inline void SetDiagonal(double, double);
222 inline void SetDiagonal(const VectorR2&);
223 inline double Diagonal(int);
224
225 inline void MakeTranspose(); // Transposes it.
226 inline void operator*=(const Matrix2x2& B); // Matrix product
227 inline Matrix2x2& ReNormalize();
228
229 inline void Transform(VectorR2*) const;
230 inline void Transform(const VectorR2& src, VectorR2* dest) const;
231 };
232
233 inline double NormalizeError(const Matrix2x2&);
234 inline VectorR2 operator*(const Matrix2x2&, const VectorR2&);
235
236 ostream& operator<<(ostream& os, const Matrix2x2& A);
237
238 // *****************************************
239 // LinearMapR2 class *
240 // * * * * * * * * * * * * * * * * * * * * *
241
242 class LinearMapR2 : public Matrix2x2
243 {
244 public:
245 LinearMapR2();
246 LinearMapR2(const VectorR2&, const VectorR2&); // Sets by columns!
247 LinearMapR2(double, double, double, double); // Sets by columns
248 LinearMapR2(const Matrix2x2&);
249
250 inline void Negate();
251 inline LinearMapR2& operator+=(const Matrix2x2&);
252 inline LinearMapR2& operator-=(const Matrix2x2&);
253 inline LinearMapR2& operator*=(double);
254 inline LinearMapR2& operator/=(double);
255 inline LinearMapR2& operator*=(const Matrix2x2&); // Matrix product
256
257 inline LinearMapR2 Transpose() const;
258 inline double Determinant() const; // Returns the determinant
259 LinearMapR2 Inverse() const; // Returns inverse
260 LinearMapR2& Invert(); // Converts into inverse.
261 VectorR2 Solve(const VectorR2&) const; // Returns solution
262 LinearMapR2 PseudoInverse() const; // Returns pseudo-inverse TO DO
263 VectorR2 PseudoSolve(const VectorR2&); // Finds least squares solution TO DO
264 };
265
266 inline LinearMapR2 operator+(const LinearMapR2&, const LinearMapR2&);
267 inline LinearMapR2 operator-(const Matrix2x2&);
268 inline LinearMapR2 operator-(const LinearMapR2&, const LinearMapR2&);
269 inline LinearMapR2 operator*(const LinearMapR2&, double);
270 inline LinearMapR2 operator*(double, const LinearMapR2&);
271 inline LinearMapR2 operator/(const LinearMapR2&, double);
272 inline LinearMapR2 operator*(const Matrix2x2&, const LinearMapR2&);
273 inline LinearMapR2 operator*(const LinearMapR2&, const Matrix2x2&);
274 inline LinearMapR2 operator*(const LinearMapR2&, const LinearMapR2&);
275 // Matrix product (composition)
276
277 // *******************************************
278 // RotationMapR2class *
279 // * * * * * * * * * * * * * * * * * * * * * *
280
281 class RotationMapR2 : public Matrix2x2
282 {
283 public:
284 RotationMapR2();
285 RotationMapR2(const VectorR2&, const VectorR2&); // Sets by columns!
286 RotationMapR2(double, double, double, double); // Sets by columns!
287
288 RotationMapR2& SetZero(); // IT IS AN ERROR TO USE THIS FUNCTION!
289
290 inline RotationMapR2& operator*=(const RotationMapR2&); // Matrix product
291
292 inline RotationMapR2 Transpose() const;
Inverse()293 inline RotationMapR2 Inverse() const { return Transpose(); }; // Returns the transpose
Invert()294 inline RotationMapR2& Invert()
295 {
296 MakeTranspose();
297 return *this;
298 }; // Transposes it.
299 inline VectorR2 Invert(const VectorR2&) const; // Returns solution
300 };
301
302 inline RotationMapR2 operator*(const RotationMapR2&, const RotationMapR2&);
303 // Matrix product (composition)
304
305 // ***************************************************************
306 // * 2-space vector and matrix utilities (prototypes) *
307 // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
308
309 // Returns the angle between vectors u and v.
310 // Use AngleUnit if both vectors are unit vectors
311 inline double Angle(const VectorR2& u, const VectorR2& v);
312 inline double AngleUnit(const VectorR2& u, const VectorR2& v);
313
314 // Returns a righthanded orthonormal basis to complement vector u
315 // The vector u must be unit.
316 inline VectorR2 GetOrtho(const VectorR2& u);
317
318 // Projections
319
320 inline VectorR2 ProjectToUnit(const VectorR2& u, const VectorR2& v);
321 // Project u onto v
322 inline VectorR2 ProjectPerpUnit(const VectorR2& u, const VectorR2& v);
323 // Project perp to v
324 // v must be a unit vector.
325
326 // Projection maps (LinearMapR2's)
327
328 inline LinearMapR2 VectorProjectMap(const VectorR2& u);
329
330 inline LinearMapR2 PerpProjectMap(const VectorR2& u);
331 // u - must be unit vector.
332
333 // Rotation Maps
334
335 inline RotationMapR2 RotateToMap(const VectorR2& fromVec, const VectorR2& toVec);
336 // fromVec and toVec should be unit vectors
337
338 // ***************************************************************
339 // * Stream Output Routines (Prototypes) *
340 // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
341
342 ostream& operator<<(ostream& os, const VectorR2& u);
343
344 // *****************************************************
345 // * VectorR2 class - inlined functions *
346 // * * * * * * * * * * * * * * * * * * * * * * * * * * *
347
Load(const double * v)348 inline VectorR2& VectorR2::Load(const double* v)
349 {
350 x = *v;
351 y = *(v + 1);
352 return *this;
353 }
354
Load(const float * v)355 inline VectorR2& VectorR2::Load(const float* v)
356 {
357 x = *v;
358 y = *(v + 1);
359 return *this;
360 }
361
Dump(double * v)362 inline void VectorR2::Dump(double* v) const
363 {
364 *v = x;
365 *(v + 1) = y;
366 }
367
Dump(float * v)368 inline void VectorR2::Dump(float* v) const
369 {
370 *v = (float)x;
371 *(v + 1) = (float)y;
372 }
373
ArrayProd(const VectorR2 & v)374 inline VectorR2& VectorR2::ArrayProd(const VectorR2& v) // Component-wise Product
375 {
376 x *= v.x;
377 y *= v.y;
378 return (*this);
379 }
380
MakeUnit()381 inline VectorR2& VectorR2::MakeUnit() // Convert to unit vector (or leave zero).
382 {
383 double nSq = NormSq();
384 if (nSq != 0.0)
385 {
386 *this /= sqrt(nSq);
387 }
388 return *this;
389 }
390
ReNormalize()391 inline VectorR2& VectorR2::ReNormalize() // Convert near unit back to unit
392 {
393 double nSq = NormSq();
394 double mFact = 1.0 - 0.5 * (nSq - 1.0); // Multiplicative factor
395 *this *= mFact;
396 return *this;
397 }
398
399 // Rotate through angle theta
Rotate(double theta)400 inline VectorR2& VectorR2::Rotate(double theta)
401 {
402 double costheta = cos(theta);
403 double sintheta = sin(theta);
404 double tempx = x * costheta - y * sintheta;
405 y = y * costheta + x * sintheta;
406 x = tempx;
407 return *this;
408 }
409
Rotate(double costheta,double sintheta)410 inline VectorR2& VectorR2::Rotate(double costheta, double sintheta)
411 {
412 double tempx = x * costheta + y * sintheta;
413 y = y * costheta - x * sintheta;
414 x = tempx;
415 return *this;
416 }
417
MaxAbs()418 inline double VectorR2::MaxAbs() const
419 {
420 double m;
421 m = (x >= 0.0) ? x : -x;
422 if (y > m)
423 m = y;
424 else if (-y > m)
425 m = -y;
426 return m;
427 }
428
429 inline VectorR2 operator+(const VectorR2& u, const VectorR2& v)
430 {
431 return VectorR2(u.x + v.x, u.y + v.y);
432 }
433 inline VectorR2 operator-(const VectorR2& u, const VectorR2& v)
434 {
435 return VectorR2(u.x - v.x, u.y - v.y);
436 }
437 inline VectorR2 operator*(const VectorR2& u, double m)
438 {
439 return VectorR2(u.x * m, u.y * m);
440 }
441 inline VectorR2 operator*(double m, const VectorR2& u)
442 {
443 return VectorR2(u.x * m, u.y * m);
444 }
445 inline VectorR2 operator/(const VectorR2& u, double m)
446 {
447 double mInv = 1.0 / m;
448 return VectorR2(u.x * mInv, u.y * mInv);
449 }
450
451 inline int operator==(const VectorR2& u, const VectorR2& v)
452 {
453 return (u.x == v.x && u.y == v.y);
454 }
455
456 inline double operator^(const VectorR2& u, const VectorR2& v) // Dot Product
457 {
458 return (u.x * v.x + u.y * v.y);
459 }
460
ArrayProd(const VectorR2 & u,const VectorR2 & v)461 inline VectorR2 ArrayProd(const VectorR2& u, const VectorR2& v)
462 {
463 return (VectorR2(u.x * v.x, u.y * v.y));
464 }
465
AddScaled(const VectorR2 & u,double s)466 inline VectorR2& VectorR2::AddScaled(const VectorR2& u, double s)
467 {
468 x += s * u.x;
469 y += s * u.y;
470 return (*this);
471 }
472
VectorR2(const VectorHgR2 & uH)473 inline VectorR2::VectorR2(const VectorHgR2& uH)
474 : x(uH.x), y(uH.y)
475 {
476 *this /= uH.w;
477 }
478
NormalizeError(const VectorR2 & u)479 inline double NormalizeError(const VectorR2& u)
480 {
481 double discrepancy;
482 discrepancy = u.x * u.x + u.y * u.y - 1.0;
483 if (discrepancy < 0.0)
484 {
485 discrepancy = -discrepancy;
486 }
487 return discrepancy;
488 }
489
Dist(const VectorR2 & u)490 inline double VectorR2::Dist(const VectorR2& u) const // Distance from u
491 {
492 return sqrt(DistSq(u));
493 }
494
DistSq(const VectorR2 & u)495 inline double VectorR2::DistSq(const VectorR2& u) const // Distance from u
496 {
497 return ((x - u.x) * (x - u.x) + (y - u.y) * (y - u.y));
498 }
499
500 // *********************************************************
501 // * Matrix2x2 class - inlined functions *
502 // * * * * * * * * * * * * * * * * * * * * * * * * * * *****
503
Matrix2x2()504 inline Matrix2x2::Matrix2x2() {}
505
Matrix2x2(const VectorR2 & u,const VectorR2 & v)506 inline Matrix2x2::Matrix2x2(const VectorR2& u, const VectorR2& v)
507 {
508 m11 = u.x; // Column 1
509 m21 = u.y;
510 m12 = v.x; // Column 2
511 m22 = v.y;
512 }
513
Matrix2x2(double a11,double a21,double a12,double a22)514 inline Matrix2x2::Matrix2x2(double a11, double a21, double a12, double a22)
515 // Values specified in column order!!!
516 {
517 m11 = a11; // Row 1
518 m12 = a12;
519 m21 = a21; // Row 2
520 m22 = a22;
521 }
522
SetIdentity()523 inline void Matrix2x2::SetIdentity()
524 {
525 m11 = m22 = 1.0;
526 m12 = m21 = 0.0;
527 }
528
Set(const VectorR2 & u,const VectorR2 & v)529 inline void Matrix2x2::Set(const VectorR2& u, const VectorR2& v)
530 {
531 m11 = u.x; // Column 1
532 m21 = u.y;
533 m12 = v.x; // Column 2
534 m22 = v.y;
535 }
536
Set(double a11,double a21,double a12,double a22)537 inline void Matrix2x2::Set(double a11, double a21, double a12, double a22)
538 // Values specified in column order!!!
539 {
540 m11 = a11; // Row 1
541 m12 = a12;
542 m21 = a21; // Row 2
543 m22 = a22;
544 }
545
SetZero()546 inline void Matrix2x2::SetZero()
547 {
548 m11 = m12 = m21 = m22 = 0.0;
549 }
550
SetByRows(const VectorR2 & u,const VectorR2 & v)551 inline void Matrix2x2::SetByRows(const VectorR2& u, const VectorR2& v)
552 {
553 m11 = u.x; // Row 1
554 m12 = u.y;
555 m21 = v.x; // Row 2
556 m22 = v.y;
557 }
558
SetByRows(double a11,double a12,double a21,double a22)559 inline void Matrix2x2::SetByRows(double a11, double a12, double a21, double a22)
560 // Values specified in row order!!!
561 {
562 m11 = a11; // Row 1
563 m12 = a12;
564 m21 = a21; // Row 2
565 m22 = a22;
566 }
567
SetColumn1(double x,double y)568 inline void Matrix2x2::SetColumn1(double x, double y)
569 {
570 m11 = x;
571 m21 = y;
572 }
573
SetColumn2(double x,double y)574 inline void Matrix2x2::SetColumn2(double x, double y)
575 {
576 m12 = x;
577 m22 = y;
578 }
579
SetColumn1(const VectorR2 & u)580 inline void Matrix2x2::SetColumn1(const VectorR2& u)
581 {
582 m11 = u.x;
583 m21 = u.y;
584 }
585
SetColumn2(const VectorR2 & u)586 inline void Matrix2x2::SetColumn2(const VectorR2& u)
587 {
588 m12 = u.x;
589 m22 = u.y;
590 }
591
Column1()592 VectorR2 Matrix2x2::Column1() const
593 {
594 return (VectorR2(m11, m21));
595 }
596
Column2()597 VectorR2 Matrix2x2::Column2() const
598 {
599 return (VectorR2(m12, m22));
600 }
601
SetRow1(double x,double y)602 inline void Matrix2x2::SetRow1(double x, double y)
603 {
604 m11 = x;
605 m12 = y;
606 }
607
SetRow2(double x,double y)608 inline void Matrix2x2::SetRow2(double x, double y)
609 {
610 m21 = x;
611 m22 = y;
612 }
613
SetRow1(const VectorR2 & u)614 inline void Matrix2x2::SetRow1(const VectorR2& u)
615 {
616 m11 = u.x;
617 m12 = u.y;
618 }
619
SetRow2(const VectorR2 & u)620 inline void Matrix2x2::SetRow2(const VectorR2& u)
621 {
622 m21 = u.x;
623 m22 = u.y;
624 }
625
Row1()626 VectorR2 Matrix2x2::Row1() const
627 {
628 return (VectorR2(m11, m12));
629 }
630
Row2()631 VectorR2 Matrix2x2::Row2() const
632 {
633 return (VectorR2(m21, m22));
634 }
635
SetDiagonal(double x,double y)636 inline void Matrix2x2::SetDiagonal(double x, double y)
637 {
638 m11 = x;
639 m22 = y;
640 }
641
SetDiagonal(const VectorR2 & u)642 inline void Matrix2x2::SetDiagonal(const VectorR2& u)
643 {
644 SetDiagonal(u.x, u.y);
645 }
646
Diagonal(int i)647 inline double Matrix2x2::Diagonal(int i)
648 {
649 switch (i)
650 {
651 case 0:
652 return m11;
653 case 1:
654 return m22;
655 default:
656 assert(0);
657 return 0.0;
658 }
659 }
MakeTranspose()660 inline void Matrix2x2::MakeTranspose() // Transposes it.
661 {
662 double temp;
663 temp = m12;
664 m12 = m21;
665 m21 = temp;
666 }
667
668 inline void Matrix2x2::operator*=(const Matrix2x2& B) // Matrix product
669 {
670 double t1; // temporary value
671
672 t1 = m11 * B.m11 + m12 * B.m21;
673 m12 = m11 * B.m12 + m12 * B.m22;
674 m11 = t1;
675
676 t1 = m21 * B.m11 + m22 * B.m21;
677 m22 = m21 * B.m12 + m22 * B.m22;
678 m21 = t1;
679 }
680
ReNormalize()681 inline Matrix2x2& Matrix2x2::ReNormalize() // Re-normalizes nearly orthonormal matrix
682 {
683 double alpha = m11 * m11 + m21 * m21; // First column's norm squared
684 double beta = m12 * m12 + m22 * m22; // Second column's norm squared
685 alpha = 1.0 - 0.5 * (alpha - 1.0); // Get mult. factor
686 beta = 1.0 - 0.5 * (beta - 1.0);
687 m11 *= alpha; // Renormalize first column
688 m21 *= alpha;
689 m12 *= beta; // Renormalize second column
690 m22 *= beta;
691 alpha = m11 * m12 + m21 * m22; // Columns' inner product
692 alpha *= 0.5; // times 1/2
693 double temp;
694 temp = m11 - alpha * m12; // Subtract alpha times other column
695 m12 -= alpha * m11;
696 m11 = temp;
697 temp = m21 - alpha * m22;
698 m22 -= alpha * m21;
699 m11 = temp;
700 return *this;
701 }
702
703 // Gives a measure of how far the matrix is from being normalized.
704 // Mostly intended for diagnostic purposes.
NormalizeError(const Matrix2x2 & A)705 inline double NormalizeError(const Matrix2x2& A)
706 {
707 double discrepancy;
708 double newdisc;
709 discrepancy = A.m11 * A.m11 + A.m21 * A.m21 - 1.0; // First column - inner product - 1
710 if (discrepancy < 0.0)
711 {
712 discrepancy = -discrepancy;
713 }
714 newdisc = A.m12 * A.m12 + A.m22 * A.m22 - 1.0; // Second column inner product - 1
715 if (newdisc < 0.0)
716 {
717 newdisc = -newdisc;
718 }
719 if (newdisc > discrepancy)
720 {
721 discrepancy = newdisc;
722 }
723 newdisc = A.m11 * A.m12 + A.m21 * A.m22; // Inner product of two columns
724 if (newdisc < 0.0)
725 {
726 newdisc = -newdisc;
727 }
728 if (newdisc > discrepancy)
729 {
730 discrepancy = newdisc;
731 }
732 return discrepancy;
733 }
734
735 inline VectorR2 operator*(const Matrix2x2& A, const VectorR2& u)
736 {
737 return (VectorR2(A.m11 * u.x + A.m12 * u.y,
738 A.m21 * u.x + A.m22 * u.y));
739 }
740
Transform(VectorR2 * u)741 inline void Matrix2x2::Transform(VectorR2* u) const
742 {
743 double newX;
744 newX = m11 * u->x + m12 * u->y;
745 u->y = m21 * u->x + m22 * u->y;
746 u->x = newX;
747 }
748
Transform(const VectorR2 & src,VectorR2 * dest)749 inline void Matrix2x2::Transform(const VectorR2& src, VectorR2* dest) const
750 {
751 dest->x = m11 * src.x + m12 * src.y;
752 dest->y = m21 * src.x + m22 * src.y;
753 }
754
755 // ******************************************************
756 // * LinearMapR2 class - inlined functions *
757 // * * * * * * * * * * * * * * * * * * * * * * * * * * **
758
LinearMapR2()759 inline LinearMapR2::LinearMapR2()
760 {
761 SetZero();
762 return;
763 }
764
LinearMapR2(const VectorR2 & u,const VectorR2 & v)765 inline LinearMapR2::LinearMapR2(const VectorR2& u, const VectorR2& v)
766 : Matrix2x2(u, v)
767 {
768 }
769
LinearMapR2(double a11,double a21,double a12,double a22)770 inline LinearMapR2::LinearMapR2(double a11, double a21, double a12, double a22)
771 // Values specified in column order!!!
772 : Matrix2x2(a11, a21, a12, a22)
773 {
774 }
775
LinearMapR2(const Matrix2x2 & A)776 inline LinearMapR2::LinearMapR2(const Matrix2x2& A)
777 : Matrix2x2(A)
778 {
779 }
780
Negate()781 inline void LinearMapR2::Negate()
782 {
783 m11 = -m11;
784 m12 = -m12;
785 m21 = -m21;
786 m22 = -m22;
787 }
788
789 inline LinearMapR2& LinearMapR2::operator+=(const Matrix2x2& B)
790 {
791 m11 += B.m11;
792 m12 += B.m12;
793 m21 += B.m21;
794 m22 += B.m22;
795 return (*this);
796 }
797
798 inline LinearMapR2& LinearMapR2::operator-=(const Matrix2x2& B)
799 {
800 m11 -= B.m11;
801 m12 -= B.m12;
802 m21 -= B.m21;
803 m22 -= B.m22;
804 return (*this);
805 }
806
807 inline LinearMapR2 operator+(const LinearMapR2& A, const LinearMapR2& B)
808 {
809 return (LinearMapR2(A.m11 + B.m11, A.m21 + B.m21,
810 A.m12 + B.m12, A.m22 + B.m22));
811 }
812
813 inline LinearMapR2 operator-(const Matrix2x2& A)
814 {
815 return (LinearMapR2(-A.m11, -A.m21, -A.m12, -A.m22));
816 }
817
818 inline LinearMapR2 operator-(const LinearMapR2& A, const LinearMapR2& B)
819 {
820 return (LinearMapR2(A.m11 - B.m11, A.m21 - B.m21,
821 A.m12 - B.m12, A.m22 - B.m22));
822 }
823
824 inline LinearMapR2& LinearMapR2::operator*=(double b)
825 {
826 m11 *= b;
827 m12 *= b;
828 m21 *= b;
829 m22 *= b;
830 return (*this);
831 }
832
833 inline LinearMapR2 operator*(const LinearMapR2& A, double b)
834 {
835 return (LinearMapR2(A.m11 * b, A.m21 * b,
836 A.m12 * b, A.m22 * b));
837 }
838
839 inline LinearMapR2 operator*(double b, const LinearMapR2& A)
840 {
841 return (LinearMapR2(A.m11 * b, A.m21 * b,
842 A.m12 * b, A.m22 * b));
843 }
844
845 inline LinearMapR2 operator/(const LinearMapR2& A, double b)
846 {
847 double bInv = 1.0 / b;
848 return (A * bInv);
849 }
850
851 inline LinearMapR2& LinearMapR2::operator/=(double b)
852 {
853 double bInv = 1.0 / b;
854 return (*this *= bInv);
855 }
856
Transpose()857 inline LinearMapR2 LinearMapR2::Transpose() const // Returns the transpose
858 {
859 return (LinearMapR2(m11, m12, m21, m22));
860 }
861
862 inline LinearMapR2& LinearMapR2::operator*=(const Matrix2x2& B) // Matrix product
863 {
864 (*this).Matrix2x2::operator*=(B);
865
866 return (*this);
867 }
868
869 inline LinearMapR2 operator*(const LinearMapR2& A, const Matrix2x2& B)
870 {
871 LinearMapR2 AA(A);
872 AA.Matrix2x2::operator*=(B);
873 return AA;
874 }
875
876 inline LinearMapR2 operator*(const Matrix2x2& A, const LinearMapR2& B)
877 {
878 LinearMapR2 AA(A);
879 AA.Matrix2x2::operator*=(B);
880 return AA;
881 }
882
883 inline LinearMapR2 operator*(const LinearMapR2& A, const LinearMapR2& B)
884 {
885 LinearMapR2 AA(A);
886 AA.Matrix2x2::operator*=(B);
887 return AA;
888 }
889
Determinant()890 inline double LinearMapR2::Determinant() const // Returns the determinant
891 {
892 return (m11 * m22 - m12 * m21);
893 }
894
895 // ******************************************************
896 // * RotationMapR2 class - inlined functions *
897 // * * * * * * * * * * * * * * * * * * * * * * * * * * **
898
RotationMapR2()899 inline RotationMapR2::RotationMapR2()
900 {
901 SetIdentity();
902 return;
903 }
904
RotationMapR2(const VectorR2 & u,const VectorR2 & v)905 inline RotationMapR2::RotationMapR2(const VectorR2& u, const VectorR2& v)
906 : Matrix2x2(u, v)
907 {
908 }
909
RotationMapR2(double a11,double a21,double a12,double a22)910 inline RotationMapR2::RotationMapR2(
911 double a11, double a21, double a12, double a22)
912 // Values specified in column order!!!
913 : Matrix2x2(a11, a21, a12, a22)
914 {
915 }
916
Transpose()917 inline RotationMapR2 RotationMapR2::Transpose() const // Returns the transpose
918 {
919 return (RotationMapR2(m11, m12,
920 m21, m22));
921 }
922
Invert(const VectorR2 & u)923 inline VectorR2 RotationMapR2::Invert(const VectorR2& u) const // Returns solution
924 {
925 return (VectorR2(m11 * u.x + m21 * u.y, // Multiply with Transpose
926 m12 * u.x + m22 * u.y));
927 }
928
929 inline RotationMapR2& RotationMapR2::operator*=(const RotationMapR2& B) // Matrix product
930 {
931 (*this).Matrix2x2::operator*=(B);
932
933 return (*this);
934 }
935
936 inline RotationMapR2 operator*(const RotationMapR2& A, const RotationMapR2& B)
937 {
938 RotationMapR2 AA(A);
939 AA.Matrix2x2::operator*=(B);
940 return AA;
941 }
942
943 // ***************************************************************
944 // * 2-space vector and matrix utilities (inlined functions) *
945 // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
946
947 // Returns a righthanded orthonormal basis to complement vector u
948 // The vector u must be unit.
GetOrtho(const VectorR2 & u)949 inline VectorR2 GetOrtho(const VectorR2& u)
950 {
951 return VectorR2(-u.y, u.x);
952 }
953
954 // Returns the projection of u onto unit v
ProjectToUnit(const VectorR2 & u,const VectorR2 & v)955 inline VectorR2 ProjectToUnit(const VectorR2& u, const VectorR2& v)
956 {
957 return (u ^ v) * v;
958 }
959
960 // Returns the projection of u onto the plane perpindicular to the unit vector v
ProjectPerpUnit(const VectorR2 & u,const VectorR2 & v)961 inline VectorR2 ProjectPerpUnit(const VectorR2& u, const VectorR2& v)
962 {
963 return (u - ((u ^ v) * v));
964 }
965
966 // Returns the projection of u onto the plane perpindicular to the unit vector v
967 // This one is more stable when u and v are nearly equal.
ProjectPerpUnitDiff(const VectorR2 & u,const VectorR2 & v)968 inline VectorR2 ProjectPerpUnitDiff(const VectorR2& u, const VectorR2& v)
969 {
970 VectorR2 ans = u;
971 ans -= v;
972 ans -= ((ans ^ v) * v);
973 return ans; // ans = (u-v) - ((u-v)^v)*v
974 }
975
976 // Returns the solid angle between vectors u and v.
Angle(const VectorR2 & u,const VectorR2 & v)977 inline double Angle(const VectorR2& u, const VectorR2& v)
978 {
979 double nSqU = u.NormSq();
980 double nSqV = v.NormSq();
981 if (nSqU == 0.0 && nSqV == 0.0)
982 {
983 return (0.0);
984 }
985 else
986 {
987 return (AngleUnit(u / sqrt(nSqU), v / sqrt(nSqV)));
988 }
989 }
990
AngleUnit(const VectorR2 & u,const VectorR2 & v)991 inline double AngleUnit(const VectorR2& u, const VectorR2& v)
992 {
993 return (atan2((ProjectPerpUnit(v, u)).Norm(), u ^ v));
994 }
995
996 // Projection maps (LinearMapR2's)
997
998 // VectorProjectMap returns map projecting onto a given vector u.
999 // u should be a unit vector (otherwise the returned map is
1000 // scaled according to the magnitude of u.
VectorProjectMap(const VectorR2 & u)1001 inline LinearMapR2 VectorProjectMap(const VectorR2& u)
1002 {
1003 double xy = u.x * u.y;
1004 return (LinearMapR2(u.x * u.x, xy, xy, u.y * u.y));
1005 }
1006
1007 // PlaneProjectMap returns map projecting onto a given plane.
1008 // The plane is the plane orthognal to u.
1009 // u must be a unit vector (otherwise the returned map is
1010 // garbage).
PerpProjectMap(const VectorR2 & u)1011 inline LinearMapR2 PerpProjectMap(const VectorR2& u)
1012 {
1013 double nxy = -u.x * u.y;
1014 return (LinearMapR2(1.0 - u.x * u.x, nxy, nxy, 1.0 - u.y * u.y));
1015 }
1016
1017 // fromVec and toVec should be unit vectors
RotateToMap(const VectorR2 & fromVec,const VectorR2 & toVec)1018 inline RotationMapR2 RotateToMap(const VectorR2& fromVec, const VectorR2& toVec)
1019 {
1020 double costheta = fromVec.x * toVec.x + fromVec.y * toVec.y;
1021 double sintheta = fromVec.x * toVec.y - fromVec.y * toVec.x;
1022 return (RotationMapR2(costheta, sintheta, -sintheta, costheta));
1023 }
1024
1025 #endif // LINEAR_R2_H
1026