TrinityCore
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Modules Pages
G3D::Matrix Class Reference

#include <Matrix.h>

Classes

class  Impl
 
class  SortRank
 

Public Types

typedef float T
 

Public Member Functions

 Matrix ()
 
 Matrix (const Matrix3 &M)
 
 Matrix (const Matrix4 &M)
 
 Matrix (int R, int C)
 
int rows () const
 
int cols () const
 
Matrixoperator*= (const T &B)
 
Matrixoperator/= (const T &B)
 
Matrixoperator+= (const T &B)
 
Matrixoperator-= (const T &B)
 
Matrixoperator*= (const Matrix &B)
 
Matrixoperator+= (const Matrix &B)
 
Matrixoperator-= (const Matrix &B)
 
Matrix subMatrix (int r1, int r2, int c1, int c2) const
 
Matrix operator* (const Matrix &B) const
 
Matrix operator* (const T &B) const
 
Matrix operator+ (const Matrix &B) const
 
Matrix operator- (const Matrix &B) const
 
Matrix operator+ (const T &v) const
 
Matrix operator- (const T &v) const
 
Matrix operator> (const T &scalar) const
 
Matrix operator< (const T &scalar) const
 
Matrix operator>= (const T &scalar) const
 
Matrix operator<= (const T &scalar) const
 
Matrix operator== (const T &scalar) const
 
Matrix operator!= (const T &scalar) const
 
Matrix lsub (const T &B) const
 
Matrix arrayMul (const Matrix &B) const
 
Matrix3 toMatrix3 () const
 
Matrix4 toMatrix4 () const
 
Vector2 toVector2 () const
 
Vector3 toVector3 () const
 
Vector4 toVector4 () const
 
void arrayMulInPlace (const Matrix &B)
 
void arrayDivInPlace (const Matrix &B)
 
Matrix operator- () const
 
Matrix inverse () const
 
T determinant () const
 
Matrix transpose () const
 
void transpose (Matrix &out) const
 
Matrix adjoint () const
 
Matrix pseudoInverse (float tolerance=-1) const
 Computes the Moore-Penrose pseudo inverse, equivalent to (ATA)-1AT). The SVD method is used for performance when the matrix has more than four rows or columns. More...
 
Matrix svdPseudoInverse (float tolerance=-1) const
 
Matrix gaussJordanPseudoInverse () const
 
void svd (Matrix &U, Array< T > &d, Matrix &V, bool sort=true) const
 
void set (int r, int c, T v)
 
void setCol (int c, const Matrix &vec)
 
void setRow (int r, const Matrix &vec)
 
Matrix col (int c) const
 
Matrix row (int r) const
 
T get (int r, int c) const
 
Vector2int16 size () const
 
int numElements () const
 
void swapRows (int r0, int r1)
 
void swapAndNegateCols (int c0, int c1)
 
void mulRow (int r, const T &v)
 
bool anyNonZero () const
 
bool allNonZero () const
 
bool allZero () const
 
bool anyZero () const
 
void serialize (TextOutput &t) const
 
std::string toString (const std::string &name) const
 
std::string toString () const
 
double normSquared () const
 
double norm () const
 

Static Public Member Functions

template<class S >
static Matrix fromDiagonal (const Array< S > &d)
 
static Matrix fromDiagonal (const Matrix &d)
 
static Matrix zero (int R, int C)
 
static Matrix one (int R, int C)
 
static Matrix identity (int N)
 
static Matrix random (int R, int C)
 
static const char * svdCore (float **U, int rows, int cols, float *D, float **V)
 

Static Public Attributes

static int debugNumCopyOps = 0
 
static int debugNumAllocOps = 0
 

Private Types

typedef shared_ptr< ImplImplRef
 

Private Member Functions

 Matrix (ImplRef i)
 
 Matrix (Impl *i)
 
Matrix vectorPseudoInverse () const
 
Matrix partitionPseudoInverse () const
 
Matrix colPartPseudoInverse () const
 
Matrix rowPartPseudoInverse () const
 
Matrix col2PseudoInverse (const Matrix &B) const
 
Matrix col3PseudoInverse (const Matrix &B) const
 
Matrix col4PseudoInverse (const Matrix &B) const
 
Matrix row2PseudoInverse (const Matrix &B) const
 
Matrix row3PseudoInverse (const Matrix &B) const
 
Matrix row4PseudoInverse (const Matrix &B) const
 

Private Attributes

ImplRef impl
 

Detailed Description

N x M matrix.

The actual data is tracked internally by a reference counted pointer; it is efficient to pass and assign Matrix objects because no data is actually copied. This avoids the headache of pointers and allows natural math notation:

   Matrix A, B, C;
   // ...
   C = A * f(B);
   C = C.inverse();
   A = Matrix::identity(4);
   C = A;
   C.set(0, 0, 2.0); // Triggers a copy of the data so that A remains unchanged.
   // etc.
 

The Matrix::debugNumCopyOps and Matrix::debugNumAllocOps counters increment every time an operation forces the copy and allocation of matrices. You can use these to detect slow operations when efficiency is a major concern.

Some methods accept an output argument instead of returning a value. For example, A = B.transpose() can also be invoked as B.transpose(A). The latter may be more efficient, since Matrix may be able to re-use the storage of A (if it has approximatly the right size and isn't currently shared with another matrix).

See also
G3D::Matrix3, G3D::Matrix4, G3D::Vector2, G3D::Vector3, G3D::Vector4, G3D::CoordinateFrame

Member Typedef Documentation

typedef shared_ptr<Impl> G3D::Matrix::ImplRef
private
typedef float G3D::Matrix::T

Internal precision. Currently float, but this may become a templated class in the future to allow operations like Matrix<double> and Matrix<ComplexFloat>.

Not necessarily a plain-old-data type (e.g., could ComplexFloat), but must be something with no constructor, that can be safely memcpyd, and that has a bit pattern of all zeros when zero.

Constructor & Destructor Documentation

G3D::Matrix::Matrix ( ImplRef  i)
inlineprivate
241 : impl(i) {}
ImplRef impl
Definition: Matrix.h:239
G3D::Matrix::Matrix ( Impl i)
inlineprivate
242 : impl(ImplRef(i)) {}
ImplRef impl
Definition: Matrix.h:239
shared_ptr< Impl > ImplRef
Definition: Matrix.h:237
G3D::Matrix::Matrix ( )
inline
289 : impl(new Impl(0, 0)) {}
ImplRef impl
Definition: Matrix.h:239

+ Here is the caller graph for this function:

G3D::Matrix::Matrix ( const Matrix3 M)
inline
291 : impl(new Impl(M)) {}
ImplRef impl
Definition: Matrix.h:239
G3D::Matrix::Matrix ( const Matrix4 M)
inline
293 : impl(new Impl(M)) {}
ImplRef impl
Definition: Matrix.h:239
G3D::Matrix::Matrix ( int  R,
int  C 
)
inline

Returns a new matrix that is all zero.

307  : impl(new Impl(R, C)) {
308  impl->setZero();
309  }
ImplRef impl
Definition: Matrix.h:239

Member Function Documentation

Matrix G3D::Matrix::adjoint ( ) const
inline
497  {
498  Impl* A = new Impl(cols(), rows());
499  impl->adjoint(*A);
500  return Matrix(A);
501  }
int rows() const
Definition: Matrix.h:324
ImplRef impl
Definition: Matrix.h:239
int cols() const
Definition: Matrix.h:329
Matrix()
Definition: Matrix.h:289

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

bool G3D::Matrix::allNonZero ( ) const

Returns true if all elements are non-zero

420  {
421  return impl->allNonZero();
422 }
ImplRef impl
Definition: Matrix.h:239

+ Here is the caller graph for this function:

bool G3D::Matrix::allZero ( ) const
inline
571  {
572  return !anyNonZero();
573  }
bool anyNonZero() const
Definition: Matrix.cpp:415

+ Here is the call graph for this function:

bool G3D::Matrix::anyNonZero ( ) const

Returns true if any element is non-zero

415  {
416  return impl->anyNonZero();
417 }
ImplRef impl
Definition: Matrix.h:239

+ Here is the caller graph for this function:

bool G3D::Matrix::anyZero ( ) const
inline
575  {
576  return !allNonZero();
577  }
bool allNonZero() const
Definition: Matrix.cpp:420

+ Here is the call graph for this function:

void G3D::Matrix::arrayDivInPlace ( const Matrix B)

Mutates this

140  {
141  const Impl& B = *_B.impl;
142  INPLACE(arrayDiv)
143 }
#define INPLACE(OP)
Definition: Matrix.cpp:80
Matrix G3D::Matrix::arrayMul ( const Matrix B) const
inline
423  {
424  Matrix C(impl->R, impl->C);
425  impl->arrayMul(*B.impl, *C.impl);
426  return C;
427  }
ImplRef impl
Definition: Matrix.h:239
Matrix()
Definition: Matrix.h:289

+ Here is the caller graph for this function:

void G3D::Matrix::arrayMulInPlace ( const Matrix B)

Mutates this

134  {
135  const Impl& B = *_B.impl;
137 }
#define INPLACE(OP)
Definition: Matrix.cpp:80
Matrix arrayMul(const Matrix &B) const
Definition: Matrix.h:423

+ Here is the call graph for this function:

Matrix G3D::Matrix::col ( int  c) const
217  {
218  debugAssert(c >= 0);
219  debugAssert(c < cols());
220  Matrix out(rows(), 1);
221 
222  T* outData = out.impl->data;
223  // Get a pointer to the first element in the column
224  const T* inElt = &(impl->elt[0][c]);
225  int R = rows();
226  int C = cols();
227  for (int r = 0; r < R; ++r) {
228  outData[r] = *inElt;
229  // Skip around to the next row
230  inElt += C;
231  }
232 
233  return out;
234 }
int rows() const
Definition: Matrix.h:324
ImplRef impl
Definition: Matrix.h:239
#define debugAssert(exp)
Definition: debugAssert.h:160
int cols() const
Definition: Matrix.h:329
float T
Definition: Matrix.h:65
Matrix()
Definition: Matrix.h:289

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

Matrix G3D::Matrix::col2PseudoInverse ( const Matrix B) const
private
1111  {
1112 
1113  Matrix A = *this;
1114  int m = rows();
1115  int n = cols();
1116  (void)n;
1117 
1118  // Row-major 2x2 matrix
1119  const float B2[2][2] =
1120  {{B.get(0,0), B.get(0,1)},
1121  {B.get(1,0), B.get(1,1)}};
1122 
1123  float det = (B2[0][0]*B2[1][1]) - (B2[0][1]*B2[1][0]);
1124 
1125  if (fuzzyEq(det, T(0))) {
1126 
1127  // Matrix was singular; the block matrix pseudo-inverse can't
1128  // handle that, so fall back to the old case
1129  return svdPseudoInverse();
1130 
1131  } else {
1132  // invert using formula at http://www.netsoc.tcd.ie/~jgilbert/maths_site/applets/algebra/matrix_inversion.html
1133 
1134  // Multiply by Binv * A'
1135  Matrix X(cols(), rows());
1136 
1137  T** Xelt = X.impl->elt;
1138  T** Aelt = A.impl->elt;
1139  float binv00 = B2[1][1]/det, binv01 = -B2[1][0]/det;
1140  float binv10 = -B2[0][1]/det, binv11 = B2[0][0]/det;
1141  for (int j = 0; j < m; ++j) {
1142  const T* Arow = Aelt[j];
1143  float a0 = Arow[0];
1144  float a1 = Arow[1];
1145  Xelt[0][j] = binv00 * a0 + binv01 * a1;
1146  Xelt[1][j] = binv10 * a0 + binv11 * a1;
1147  }
1148  return X;
1149  }
1150 }
int rows() const
Definition: Matrix.h:324
#define X
Definition: CollisionDetection.cpp:2281
int cols() const
Definition: Matrix.h:329
float T
Definition: Matrix.h:65
Matrix()
Definition: Matrix.h:289
bool fuzzyEq(double a, double b)
Definition: g3dmath.h:857
Matrix svdPseudoInverse(float tolerance=-1) const
Definition: Matrix.cpp:900

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

Matrix G3D::Matrix::col3PseudoInverse ( const Matrix B) const
private
1152  {
1153  Matrix A = *this;
1154  int m = rows();
1155  int n = cols();
1156 
1157  Matrix3 B3 = B.toMatrix3();
1158  if (fuzzyEq(B3.determinant(), (T)0.0)) {
1159 
1160  // Matrix was singular; the block matrix pseudo-inverse can't
1161  // handle that, so fall back to the old case
1162  return svdPseudoInverse();
1163 
1164  } else {
1165  Matrix3 B3inv = B3.inverse();
1166 
1167  // Multiply by Binv * A'
1168  Matrix X(cols(), rows());
1169 
1170  T** Xelt = X.impl->elt;
1171  T** Aelt = A.impl->elt;
1172  for (int i = 0; i < n; ++i) {
1173  T* Xrow = Xelt[i];
1174  for (int j = 0; j < m; ++j) {
1175  const T* Arow = Aelt[j];
1176  T sum = 0;
1177  const float* Binvrow = B3inv[i];
1178  for (int k = 0; k < n; ++k) {
1179  sum += Binvrow[k] * Arow[k];
1180  }
1181  Xrow[j] = sum;
1182  }
1183  }
1184  return X;
1185  }
1186 }
int rows() const
Definition: Matrix.h:324
#define X
Definition: CollisionDetection.cpp:2281
int cols() const
Definition: Matrix.h:329
float T
Definition: Matrix.h:65
Matrix()
Definition: Matrix.h:289
bool fuzzyEq(double a, double b)
Definition: g3dmath.h:857
Matrix svdPseudoInverse(float tolerance=-1) const
Definition: Matrix.cpp:900

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

Matrix G3D::Matrix::col4PseudoInverse ( const Matrix B) const
private
1188  {
1189  Matrix A = *this;
1190  int m = rows();
1191  int n = cols();
1192 
1193  Matrix4 B4 = B.toMatrix4();
1194  if (fuzzyEq(B4.determinant(), (T)0.0)) {
1195 
1196  // Matrix was singular; the block matrix pseudo-inverse can't
1197  // handle that, so fall back to the old case
1198  return svdPseudoInverse();
1199 
1200  } else {
1201  Matrix4 B4inv = B4.inverse();
1202 
1203  // Multiply by Binv * A'
1204  Matrix X(cols(), rows());
1205 
1206  T** Xelt = X.impl->elt;
1207  T** Aelt = A.impl->elt;
1208  for (int i = 0; i < n; ++i) {
1209  T* Xrow = Xelt[i];
1210  for (int j = 0; j < m; ++j) {
1211  const T* Arow = Aelt[j];
1212  T sum = 0;
1213  const float* Binvrow = B4inv[i];
1214  for (int k = 0; k < n; ++k) {
1215  sum += Binvrow[k] * Arow[k];
1216  }
1217  Xrow[j] = sum;
1218  }
1219  }
1220  return X;
1221  }
1222 }
int rows() const
Definition: Matrix.h:324
#define X
Definition: CollisionDetection.cpp:2281
int cols() const
Definition: Matrix.h:329
float T
Definition: Matrix.h:65
Matrix()
Definition: Matrix.h:289
bool fuzzyEq(double a, double b)
Definition: g3dmath.h:857
Matrix svdPseudoInverse(float tolerance=-1) const
Definition: Matrix.cpp:900

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

Matrix G3D::Matrix::colPartPseudoInverse ( ) const
private
1073  {
1074  int m = rows();
1075  int n = cols();
1076  alwaysAssertM((m>=n),"Column-partitioned block matrix pseudoinverse requires R>C");
1077  // TODO: Put each of the individual cases in its own helper function
1078  // TODO: Push the B computation down into the individual cases
1079  // B = A' * A
1080  Matrix A = *this;
1081  Matrix B = Matrix(n, n);
1082  T** Aelt = A.impl->elt;
1083  T** Belt = B.impl->elt;
1084  for (int i = 0; i < n; ++i) {
1085  for (int j = 0; j < n; ++j) {
1086  T sum = 0;
1087  for (int k = 0; k < m; ++k) {
1088  sum += Aelt[k][i] * Aelt[k][j];
1089  }
1090  Belt[i][j] = sum;
1091  }
1092  }
1093 
1094  // B has size n x n
1095  switch (n) {
1096  case 2:
1097  return col2PseudoInverse(B);
1098 
1099  case 3:
1100  return col3PseudoInverse(B);
1101 
1102  case 4:
1103  return col4PseudoInverse(B);
1104 
1105  default:
1106  alwaysAssertM(false, "G3D internal error: Should have used the vector or general case!");
1107  return Matrix();
1108  }
1109 }
int rows() const
Definition: Matrix.h:324
Matrix col4PseudoInverse(const Matrix &B) const
Definition: Matrix.cpp:1188
Matrix col2PseudoInverse(const Matrix &B) const
Definition: Matrix.cpp:1111
int cols() const
Definition: Matrix.h:329
float T
Definition: Matrix.h:65
Matrix()
Definition: Matrix.h:289
#define alwaysAssertM(exp, message)
Definition: debugAssert.h:165
Matrix col3PseudoInverse(const Matrix &B) const
Definition: Matrix.cpp:1152

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

int G3D::Matrix::cols ( ) const
inline

Number of columns

329  {
330  return impl->C;
331  }
ImplRef impl
Definition: Matrix.h:239

+ Here is the caller graph for this function:

T G3D::Matrix::determinant ( ) const
inline
481  {
482  return impl->determinant();
483  }
ImplRef impl
Definition: Matrix.h:239

+ Here is the caller graph for this function:

template<class S >
static Matrix G3D::Matrix::fromDiagonal ( const Array< S > &  d)
inlinestatic
296  {
297  Matrix D = zero(d.length(), d.length());
298  for (int i = 0; i < d.length(); ++i) {
299  D.set(i, i, d[i]);
300  }
301  return D;
302  }
void set(int r, int c, T v)
Definition: Matrix.cpp:159
Matrix()
Definition: Matrix.h:289
static Matrix zero(int R, int C)
Definition: Matrix.cpp:237

+ Here is the call graph for this function:

Matrix G3D::Matrix::fromDiagonal ( const Matrix d)
static
147  {
148  debugAssert((d.rows() == 1) || (d.cols() == 1));
149 
150  int n = d.numElements();
151  Matrix D = zero(n, n);
152  for (int i = 0; i < n; ++i) {
153  D.set(i, i, d.impl->data[i]);
154  }
155 
156  return D;
157 }
#define debugAssert(exp)
Definition: debugAssert.h:160
Matrix()
Definition: Matrix.h:289
static Matrix zero(int R, int C)
Definition: Matrix.cpp:237

+ Here is the call graph for this function:

Matrix G3D::Matrix::gaussJordanPseudoInverse ( ) const
inline

(ATA)-1AT) computed using Gauss-Jordan elimination.

521  {
522  Matrix trans = transpose();
523  return (trans * (*this)).inverse() * trans;
524  }
Matrix()
Definition: Matrix.h:289
Matrix transpose() const
Definition: Matrix.h:488

+ Here is the call graph for this function:

Matrix::T G3D::Matrix::get ( int  r,
int  c 
) const
203  {
204  return impl->get(r, c);
205 }
ImplRef impl
Definition: Matrix.h:239

+ Here is the caller graph for this function:

Matrix G3D::Matrix::identity ( int  N)
static

Returns a new identity matrix

262  {
263  Impl* m = new Impl(N, N);
264  m->setZero();
265  for (int i = 0; i < N; ++i) {
266  m->elt[i][i] = 1.0;
267  }
268  return Matrix(m);
269 }
Matrix()
Definition: Matrix.h:289

+ Here is the call graph for this function:

Matrix G3D::Matrix::inverse ( ) const
inline

A-1 computed using the Gauss-Jordan algorithm, for square matrices. Run time is O(R3), where R is the number of rows.

475  {
476  Impl* A = new Impl(*impl);
477  A->inverseInPlaceGaussJordan();
478  return Matrix(A);
479  }
ImplRef impl
Definition: Matrix.h:239
Matrix()
Definition: Matrix.h:289

+ Here is the call graph for this function:

Matrix G3D::Matrix::lsub ( const T B) const
inline

scalar B - this

417  {
418  Matrix C(impl->R, impl->C);
419  impl->lsub(B, *C.impl);
420  return C;
421  }
ImplRef impl
Definition: Matrix.h:239
Matrix()
Definition: Matrix.h:289

+ Here is the caller graph for this function:

void G3D::Matrix::mulRow ( int  r,
const T v 
)
289  {
290  debugAssert(r >= 0 && r < rows());
291 
292  if (! impl.unique()) {
293  impl.reset(new Impl(*impl));
294  }
295 
296  impl->mulRow(r, v);
297 }
int rows() const
Definition: Matrix.h:324
ImplRef impl
Definition: Matrix.h:239
#define debugAssert(exp)
Definition: debugAssert.h:160
double G3D::Matrix::norm ( ) const

2-norm (sqrt(sum(squares))

523  {
524  return sqrt(normSquared());
525 }
double normSquared() const
Definition: Matrix.cpp:508

+ Here is the call graph for this function:

double G3D::Matrix::normSquared ( ) const

2-norm squared: sum(squares). (i.e., dot product with itself)

508  {
509  int R = rows();
510  int C = cols();
511  int N = R * C;
512 
513  double sum = 0.0;
514 
515  const T* raw = impl->data;
516  for (int i = 0; i < N; ++i) {
517  sum += square(raw[i]);
518  }
519 
520  return sum;
521 }
int rows() const
Definition: Matrix.h:324
ImplRef impl
Definition: Matrix.h:239
double square(double fValue)
Definition: g3dmath.h:698
int cols() const
Definition: Matrix.h:329
float T
Definition: Matrix.h:65

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

int G3D::Matrix::numElements ( ) const
inline
554  {
555  return rows() * cols();
556  }
int rows() const
Definition: Matrix.h:324
int cols() const
Definition: Matrix.h:329

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

Matrix G3D::Matrix::one ( int  R,
int  C 
)
static

Returns a new matrix that is all one.

244  {
245  Impl* A = new Impl(R, C);
246  for (int i = R * C - 1; i >= 0; --i) {
247  A->data[i] = 1.0;
248  }
249  return Matrix(A);
250 }
Matrix()
Definition: Matrix.h:289

+ Here is the call graph for this function:

Matrix G3D::Matrix::operator!= ( const T scalar) const
Matrix G3D::Matrix::operator* ( const Matrix B) const
inline

Matrix multiplication. To perform element-by-element multiplication, see arrayMul.

362  {
363  Matrix C(impl->R, B.impl->C);
364  impl->mul(*B.impl, *C.impl);
365  return C;
366  }
ImplRef impl
Definition: Matrix.h:239
Matrix()
Definition: Matrix.h:289
Matrix G3D::Matrix::operator* ( const T B) const
inline

See also A *= B, which is more efficient in many cases

369  {
370  Matrix C(impl->R, impl->C);
371  impl->mul(B, *C.impl);
372  return C;
373  }
ImplRef impl
Definition: Matrix.h:239
Matrix()
Definition: Matrix.h:289
Matrix & G3D::Matrix::operator*= ( const T B)

Generally more efficient than A * B

89  {
90  INPLACE(mul)
91  return *this;
92 }
float mul(float a, float b)
Definition: g3dmath.h:453
#define INPLACE(OP)
Definition: Matrix.cpp:80

+ Here is the call graph for this function:

Matrix & G3D::Matrix::operator*= ( const Matrix B)

No performance advantage over A * B because matrix multiplication requires intermediate storage.

113  {
114  // We can't optimize this one
115  *this = *this * B;
116  return *this;
117 }
Matrix G3D::Matrix::operator+ ( const Matrix B) const
inline

See also A += B, which is more efficient in many cases

376  {
377  Matrix C(impl->R, impl->C);
378  impl->add(*B.impl, *C.impl);
379  return C;
380  }
ImplRef impl
Definition: Matrix.h:239
Matrix()
Definition: Matrix.h:289
Matrix G3D::Matrix::operator+ ( const T v) const
inline

See also A += B, which is more efficient in many cases

390  {
391  Matrix C(impl->R, impl->C);
392  impl->add(v, *C.impl);
393  return C;
394  }
ImplRef impl
Definition: Matrix.h:239
Matrix()
Definition: Matrix.h:289
Matrix & G3D::Matrix::operator+= ( const T B)

Generally more efficient than A + B

101  {
102  INPLACE(add)
103  return *this;
104 }
#define INPLACE(OP)
Definition: Matrix.cpp:80
Matrix & G3D::Matrix::operator+= ( const Matrix B)

Generally more efficient than A + B

127  {
128  const Impl& B = *_B.impl;
129  INPLACE(add)
130  return *this;
131 }
#define INPLACE(OP)
Definition: Matrix.cpp:80
Matrix G3D::Matrix::operator- ( const Matrix B) const
inline

See also A -= B, which is more efficient in many cases

383  {
384  Matrix C(impl->R, impl->C);
385  impl->sub(*B.impl, *C.impl);
386  return C;
387  }
ImplRef impl
Definition: Matrix.h:239
Matrix()
Definition: Matrix.h:289
Matrix G3D::Matrix::operator- ( const T v) const
inline

See also A -= B, which is more efficient in many cases

397  {
398  Matrix C(impl->R, impl->C);
399  impl->sub(v, *C.impl);
400  return C;
401  }
ImplRef impl
Definition: Matrix.h:239
Matrix()
Definition: Matrix.h:289
Matrix G3D::Matrix::operator- ( ) const
inline
465  {
466  return negate();
467  }
static G3D::Matrix::T negate(G3D::Matrix::T x)
Definition: Matrix.cpp:8

+ Here is the call graph for this function:

Matrix & G3D::Matrix::operator-= ( const T B)

Generally more efficient than A - B

95  {
96  INPLACE(sub)
97  return *this;
98 }
#define INPLACE(OP)
Definition: Matrix.cpp:80
Matrix & G3D::Matrix::operator-= ( const Matrix B)

Generally more efficient than A - B

120  {
121  const Impl& B = *_B.impl;
122  INPLACE(sub)
123  return *this;
124 }
#define INPLACE(OP)
Definition: Matrix.cpp:80
Matrix & G3D::Matrix::operator/= ( const T B)

Generally more efficient than A / B

107  {
108  INPLACE(div)
109  return *this;
110 }
#define INPLACE(OP)
Definition: Matrix.cpp:80
Matrix G3D::Matrix::operator< ( const T scalar) const
Matrix G3D::Matrix::operator<= ( const T scalar) const
Matrix G3D::Matrix::operator== ( const T scalar) const
Matrix G3D::Matrix::operator> ( const T scalar) const
Matrix G3D::Matrix::operator>= ( const T scalar) const
Matrix G3D::Matrix::partitionPseudoInverse ( ) const
private
1334  {
1335 
1336  // Logic:
1337  // A^-1 = (A'A)^-1 A'
1338  // A has few (n) columns, so A'A is small (n x n) and fast to invert
1339 
1340  int m = rows();
1341  int n = cols();
1342 
1343  if (m < n) {
1344  // TODO: optimize by pushing through the transpose
1345  //return transpose().partitionPseudoInverse().transpose();
1346  return rowPartPseudoInverse();
1347 
1348  } else {
1349  return colPartPseudoInverse();
1350  }
1351 }
int rows() const
Definition: Matrix.h:324
Matrix colPartPseudoInverse() const
Definition: Matrix.cpp:1073
int cols() const
Definition: Matrix.h:329
Matrix rowPartPseudoInverse() const
Definition: Matrix.cpp:1033

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

Matrix G3D::Matrix::pseudoInverse ( float  tolerance = -1) const

Computes the Moore-Penrose pseudo inverse, equivalent to (ATA)-1AT). The SVD method is used for performance when the matrix has more than four rows or columns.

[http://en.wikipedia.org/wiki/Moore]E2%80%93Penrose_pseudoinverse

Parameters
toleranceUse -1 for automatic tolerance.
885  {
886  if ((cols() == 1) || (rows() == 1)) {
887  return vectorPseudoInverse();
888  } else if ((cols() <= 4) || (rows() <= 4)) {
889  return partitionPseudoInverse();
890  } else {
891  return svdPseudoInverse(tolerance);
892  }
893 }
Matrix vectorPseudoInverse() const
Definition: Matrix.cpp:1012
int rows() const
Definition: Matrix.h:324
Matrix partitionPseudoInverse() const
Definition: Matrix.cpp:1334
int cols() const
Definition: Matrix.h:329
Matrix svdPseudoInverse(float tolerance=-1) const
Definition: Matrix.cpp:900

+ Here is the call graph for this function:

Matrix G3D::Matrix::random ( int  R,
int  C 
)
static

Uniformly distributed values between zero and one.

253  {
254  Impl* A = new Impl(R, C);
255  for (int i = R * C - 1; i >= 0; --i) {
256  A->data[i] = G3D::uniformRandom(0.0, 1.0);
257  }
258  return Matrix(A);
259 }
float uniformRandom(float low=0.0f, float hi=1.0f)
Definition: g3dmath.h:694
Matrix()
Definition: Matrix.h:289

+ Here is the call graph for this function:

Matrix G3D::Matrix::row ( int  r) const
208  {
209  debugAssert(r >= 0);
210  debugAssert(r < rows());
211  Matrix out(1, cols());
212  out.impl->setRow(1, impl->elt[r]);
213  return out;
214 }
int rows() const
Definition: Matrix.h:324
ImplRef impl
Definition: Matrix.h:239
#define debugAssert(exp)
Definition: debugAssert.h:160
int cols() const
Definition: Matrix.h:329
Matrix()
Definition: Matrix.h:289

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

Matrix G3D::Matrix::row2PseudoInverse ( const Matrix B) const
private
1224  {
1225 
1226  Matrix A = *this;
1227  int m = rows();
1228  int n = cols();
1229  (void)m;
1230 
1231  // Row-major 2x2 matrix
1232  const float B2[2][2] =
1233  {{B.get(0,0), B.get(0,1)},
1234  {B.get(1,0), B.get(1,1)}};
1235 
1236  float det = (B2[0][0]*B2[1][1]) - (B2[0][1]*B2[1][0]);
1237 
1238  if (fuzzyEq(det, T(0))) {
1239 
1240  // Matrix was singular; the block matrix pseudo-inverse can't
1241  // handle that, so fall back to the old case
1242  return svdPseudoInverse();
1243 
1244  } else {
1245  // invert using formula at http://www.netsoc.tcd.ie/~jgilbert/maths_site/applets/algebra/matrix_inversion.html
1246 
1247  // Multiply by Binv * A'
1248  Matrix X(cols(), rows());
1249 
1250  T** Xelt = X.impl->elt;
1251  T** Aelt = A.impl->elt;
1252  float binv00 = B2[1][1]/det, binv01 = -B2[1][0]/det;
1253  float binv10 = -B2[0][1]/det, binv11 = B2[0][0]/det;
1254  for (int j = 0; j < n; ++j) {
1255  Xelt[j][0] = Aelt[0][j] * binv00 + Aelt[1][j] * binv10;
1256  Xelt[j][1] = Aelt[0][j] * binv01 + Aelt[1][j] * binv11;
1257  }
1258  return X;
1259  }
1260 }
int rows() const
Definition: Matrix.h:324
#define X
Definition: CollisionDetection.cpp:2281
int cols() const
Definition: Matrix.h:329
float T
Definition: Matrix.h:65
Matrix()
Definition: Matrix.h:289
bool fuzzyEq(double a, double b)
Definition: g3dmath.h:857
Matrix svdPseudoInverse(float tolerance=-1) const
Definition: Matrix.cpp:900

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

Matrix G3D::Matrix::row3PseudoInverse ( const Matrix B) const
private
1262  {
1263 
1264  Matrix A = *this;
1265  int m = rows();
1266  int n = cols();
1267 
1268  Matrix3 B3 = B.toMatrix3();
1269  if (fuzzyEq(B3.determinant(), (T)0.0)) {
1270 
1271  // Matrix was singular; the block matrix pseudo-inverse can't
1272  // handle that, so fall back to the old case
1273  return svdPseudoInverse();
1274 
1275  } else {
1276  Matrix3 B3inv = B3.inverse();
1277 
1278  // Multiply by Binv * A'
1279  Matrix X(cols(), rows());
1280 
1281  T** Xelt = X.impl->elt;
1282  T** Aelt = A.impl->elt;
1283  for (int i = 0; i < n; ++i) {
1284  T* Xrow = Xelt[i];
1285  for (int j = 0; j < m; ++j) {
1286  T sum = 0;
1287  for (int k = 0; k < m; ++k) {
1288  sum += Aelt[k][i] * B3inv[j][k];
1289  }
1290  Xrow[j] = sum;
1291  }
1292  }
1293  return X;
1294  }
1295 }
int rows() const
Definition: Matrix.h:324
#define X
Definition: CollisionDetection.cpp:2281
int cols() const
Definition: Matrix.h:329
float T
Definition: Matrix.h:65
Matrix()
Definition: Matrix.h:289
bool fuzzyEq(double a, double b)
Definition: g3dmath.h:857
Matrix svdPseudoInverse(float tolerance=-1) const
Definition: Matrix.cpp:900

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

Matrix G3D::Matrix::row4PseudoInverse ( const Matrix B) const
private
1297  {
1298 
1299  Matrix A = *this;
1300  int m = rows();
1301  int n = cols();
1302 
1303  Matrix4 B4 = B.toMatrix4();
1304  if (fuzzyEq(B4.determinant(), (T)0.0)) {
1305 
1306  // Matrix was singular; the block matrix pseudo-inverse can't
1307  // handle that, so fall back to the old case
1308  return svdPseudoInverse();
1309 
1310  } else {
1311  Matrix4 B4inv = B4.inverse();
1312 
1313  // Multiply by Binv * A'
1314  Matrix X(cols(), rows());
1315 
1316  T** Xelt = X.impl->elt;
1317  T** Aelt = A.impl->elt;
1318  for (int i = 0; i < n; ++i) {
1319  T* Xrow = Xelt[i];
1320  for (int j = 0; j < m; ++j) {
1321  T sum = 0;
1322  for (int k = 0; k < m; ++k) {
1323  sum += Aelt[k][i] * B4inv[j][k];
1324  }
1325  Xrow[j] = sum;
1326  }
1327  }
1328  return X;
1329  }
1330 }
int rows() const
Definition: Matrix.h:324
#define X
Definition: CollisionDetection.cpp:2281
int cols() const
Definition: Matrix.h:329
float T
Definition: Matrix.h:65
Matrix()
Definition: Matrix.h:289
bool fuzzyEq(double a, double b)
Definition: g3dmath.h:857
Matrix svdPseudoInverse(float tolerance=-1) const
Definition: Matrix.cpp:900

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

Matrix G3D::Matrix::rowPartPseudoInverse ( ) const
private
1033  {
1034  int m = rows();
1035  int n = cols();
1036  alwaysAssertM((m<=n),"Row-partitioned block matrix pseudoinverse requires R<C");
1037 
1038  // B = A * A'
1039  Matrix A = *this;
1040  Matrix B = Matrix(m,m);
1041 
1042  T** Aelt = A.impl->elt;
1043  T** Belt = B.impl->elt;
1044  for (int i = 0; i < m; ++i) {
1045  const T* Arow = Aelt[i];
1046  for (int j = 0; j < m; ++j) {
1047  const T* Brow = Aelt[j];
1048  T sum = 0;
1049  for (int k = 0; k < n; ++k) {
1050  sum += Arow[k] * Brow[k];
1051  }
1052  Belt[i][j] = sum;
1053  }
1054  }
1055 
1056  // B has size m x m
1057  switch (m) {
1058  case 2:
1059  return row2PseudoInverse(B);
1060 
1061  case 3:
1062  return row3PseudoInverse(B);
1063 
1064  case 4:
1065  return row4PseudoInverse(B);
1066 
1067  default:
1068  alwaysAssertM(false, "G3D internal error: Should have used the vector or general case!");
1069  return Matrix();
1070  }
1071 }
Matrix row3PseudoInverse(const Matrix &B) const
Definition: Matrix.cpp:1262
int rows() const
Definition: Matrix.h:324
int cols() const
Definition: Matrix.h:329
float T
Definition: Matrix.h:65
Matrix row4PseudoInverse(const Matrix &B) const
Definition: Matrix.cpp:1297
Matrix()
Definition: Matrix.h:289
Matrix row2PseudoInverse(const Matrix &B) const
Definition: Matrix.cpp:1224
#define alwaysAssertM(exp, message)
Definition: debugAssert.h:165

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

int G3D::Matrix::rows ( ) const
inline

The number of rows

324  {
325  return impl->R;
326  }
ImplRef impl
Definition: Matrix.h:239

+ Here is the caller graph for this function:

void G3D::Matrix::serialize ( TextOutput t) const

Serializes in Matlab source format

17  {
18  t.writeSymbol("%");
19  t.writeNumber(rows());
20  t.writeSymbol("x");
21  t.writeNumber(cols());
22  t.pushIndent();
23  t.writeNewline();
24 
25  t.writeSymbol("[");
26  for (int r = 0; r < rows(); ++r) {
27  for (int c = 0; c < cols(); ++c) {
28  t.writeNumber(impl->get(r, c));
29  if (c < cols() - 1) {
30  t.writeSymbol(",");
31  } else {
32  if (r < rows() - 1) {
33  t.writeSymbol(";");
34  t.writeNewline();
35  }
36  }
37  }
38  }
39  t.writeSymbol("]");
40  t.popIndent();
41  t.writeNewline();
42 }
int rows() const
Definition: Matrix.h:324
ImplRef impl
Definition: Matrix.h:239
int cols() const
Definition: Matrix.h:329

+ Here is the call graph for this function:

void G3D::Matrix::set ( int  r,
int  c,
T  v 
)
159  {
160  if (! impl.unique()) {
161  // Copy the data before mutating; this object is shared
162  impl.reset(new Impl(*impl));
163  }
164  impl->set(r, c, v);
165 }
ImplRef impl
Definition: Matrix.h:239

+ Here is the caller graph for this function:

void G3D::Matrix::setCol ( int  c,
const Matrix vec 
)
185  {
186  debugAssertM(vec.rows() == rows(),
187  "A column must be set to a vector of the same size.");
188  debugAssertM(vec.cols() == 1,
189  "A column must be set to a column vector.");
190 
191  debugAssert(c >= 0);
192 
193  debugAssert(c < cols());
194 
195  if (! impl.unique()) {
196  // Copy the data before mutating; this object is shared
197  impl.reset(new Impl(*impl));
198  }
199  impl->setCol(c, vec.impl->data);
200 }
int rows() const
Definition: Matrix.h:324
#define debugAssertM(exp, message)
Definition: debugAssert.h:161
ImplRef impl
Definition: Matrix.h:239
Definition: adtfile.h:39
#define debugAssert(exp)
Definition: debugAssert.h:160
int cols() const
Definition: Matrix.h:329

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void G3D::Matrix::setRow ( int  r,
const Matrix vec 
)
168  {
169  debugAssertM(vec.cols() == cols(),
170  "A row must be set to a vector of the same size.");
171  debugAssertM(vec.rows() == 1,
172  "A row must be set to a row vector.");
173 
174  debugAssert(r >= 0);
175  debugAssert(r < rows());
176 
177  if (! impl.unique()) {
178  // Copy the data before mutating; this object is shared
179  impl.reset(new Impl(*impl));
180  }
181  impl->setRow(r, vec.impl->data);
182 }
int rows() const
Definition: Matrix.h:324
#define debugAssertM(exp, message)
Definition: debugAssert.h:161
ImplRef impl
Definition: Matrix.h:239
Definition: adtfile.h:39
#define debugAssert(exp)
Definition: debugAssert.h:160
int cols() const
Definition: Matrix.h:329

+ Here is the call graph for this function:

Vector2int16 G3D::Matrix::size ( ) const
inline
550  {
551  return Vector2int16(rows(), cols());
552  }
Vector2int16
Definition: Vector2int16.h:28
int rows() const
Definition: Matrix.h:324
int cols() const
Definition: Matrix.h:329

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

Matrix G3D::Matrix::subMatrix ( int  r1,
int  r2,
int  c1,
int  c2 
) const

Returns a new matrix that is a subset of this one, from r1:r2 to c1:c2, inclusive.

395  {
396  debugAssert(r2>=r1);
397  debugAssert(c2>=c1);
398  debugAssert(c2<cols());
399  debugAssert(r2<rows());
400  debugAssert(r1>=0);
401  debugAssert(c1>=0);
402 
403  Matrix X(r2 - r1 + 1, c2 - c1 + 1);
404 
405  for (int r = 0; r < X.rows(); ++r) {
406  for (int c = 0; c < X.cols(); ++c) {
407  X.set(r, c, get(r + r1, c + c1));
408  }
409  }
410 
411  return X;
412 }
int rows() const
Definition: Matrix.h:324
#define X
Definition: CollisionDetection.cpp:2281
#define debugAssert(exp)
Definition: debugAssert.h:160
int cols() const
Definition: Matrix.h:329
Matrix()
Definition: Matrix.h:289

+ Here is the call graph for this function:

void G3D::Matrix::svd ( Matrix U,
Array< T > &  d,
Matrix V,
bool  sort = true 
) const

Singular value decomposition. Factors into three matrices such that this = U * fromDiagonal(d) * V.transpose().

The matrix must have at least as many rows as columns.

Run time is O(C2*R).

Parameters
sortIf true (default), the singular values are arranged so that D is sorted from largest to smallest.
425  {
426  debugAssert(rows() >= cols());
427  debugAssertM(&U != &V, "Arguments to SVD must be different matrices");
428  debugAssertM(&U != this, "Arguments to SVD must be different matrices");
429  debugAssertM(&V != this, "Arguments to SVD must be different matrices");
430 
431  int R = rows();
432  int C = cols();
433 
434  // Make sure we don't overwrite a shared matrix
435  if (! V.impl.unique()) {
436  V = Matrix::zero(C, C);
437  } else {
438  V.impl->setSize(C, C);
439  }
440 
441  if (&U != this || ! impl.unique()) {
442  // Make a copy of this for in-place SVD
443  U.impl.reset(new Impl(*impl));
444  }
445 
446  d.resize(C);
447  const char* ret = svdCore(U.impl->elt, R, C, d.getCArray(), V.impl->elt);
448 
449  debugAssertM(ret == NULL, ret);
450  (void)ret;
451 
452  if (sort) {
453  // Sort the singular values from greatest to least
454 
455  Array<SortRank> rank;
456  rank.resize(C);
457  for (int c = 0; c < C; ++c) {
458  rank[c].col = c;
459  rank[c].value = d[c];
460  }
461 
462  rank.sort(SORT_INCREASING);
463 
464  Matrix Uold = U;
465  Matrix Vold = V;
466 
467  U = Matrix(U.rows(), U.cols());
468  V = Matrix(V.rows(), V.cols());
469 
470  // Now permute U, d, and V appropriately
471  for (int c0 = 0; c0 < C; ++c0) {
472  const int c1 = rank[c0].col;
473 
474  d[c0] = rank[c0].value;
475  U.setCol(c0, Uold.col(c1));
476  V.setCol(c0, Vold.col(c1));
477  }
478 
479  }
480 }
static const char * svdCore(float **U, int rows, int cols, float *D, float **V)
Definition: Matrix.cpp:1505
void resize(size_t n, bool shrinkIfNecessary=true)
Definition: Array.h:490
int rows() const
Definition: Matrix.h:324
void sort(const LessThan &lessThan)
Definition: Array.h:1109
T * getCArray()
Definition: Array.h:256
arena_t NULL
Definition: jemalloc_internal.h:624
#define debugAssertM(exp, message)
Definition: debugAssert.h:161
ImplRef impl
Definition: Matrix.h:239
#define debugAssert(exp)
Definition: debugAssert.h:160
int cols() const
Definition: Matrix.h:329
const int SORT_INCREASING
Definition: Array.h:46
Matrix()
Definition: Matrix.h:289
static Matrix zero(int R, int C)
Definition: Matrix.cpp:237

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

const char * G3D::Matrix::svdCore ( float **  U,
int  rows,
int  cols,
float *  D,
float **  V 
)
static

Low-level SVD functionality. Useful for applications that do not want to construct a Matrix but need to perform the SVD operation.

this = U * D * V'

Assumes that rows >= cols

Returns
NULL on success, a string describing the error on failure.
Parameters
Urows x cols matrix to be decomposed, gets overwritten with U, a rows x cols matrix with orthogonal columns.
Dvector of singular values of a (diagonal of the D matrix). Length cols.
Vreturns the right orthonormal transformation matrix, size cols x cols

[Based] on Dianne Cook's implementation, which is adapted from svdecomp.c in XLISP-STAT 2.1, which is code from Numerical Recipes adapted by Luke Tierney and David Betz. The Numerical Recipes code is adapted from Forsythe et al, who based their code on Golub and Reinsch's original implementation.

1505  {
1506  const int MAX_ITERATIONS = 30;
1507 
1508  int flag, i, its, j, jj, k, l = 0, nm = 0;
1509  double c, f, h, s, x, y, z;
1510  double anorm = 0.0, g = 0.0, scale = 0.0;
1511 
1512  // Temp row vector
1513  double* rv1;
1514 
1515  debugAssertM(rows >= cols, "Must have more rows than columns");
1516 
1517  rv1 = (double*)System::alignedMalloc(cols * sizeof(double), 16);
1518  debugAssert(rv1);
1519 
1520  // Householder reduction to bidiagonal form
1521  for (i = 0; i < cols; ++i) {
1522 
1523  // Left-hand reduction
1524  l = i + 1;
1525  rv1[i] = scale * g;
1526  g = s = scale = 0.0;
1527 
1528  if (i < rows) {
1529 
1530  for (k = i; k < rows; ++k) {
1531  scale += fabs((double)U[k][i]);
1532  }
1533 
1534  if (scale) {
1535  for (k = i; k < rows; ++k) {
1536  U[k][i] = (float)((double)U[k][i]/scale);
1537  s += ((double)U[k][i] * (double)U[k][i]);
1538  }
1539 
1540  f = (double)U[i][i];
1541 
1542  g = -sign(f)*(sqrt(s));
1543  h = f * g - s;
1544  U[i][i] = (float)(f - g);
1545 
1546  if (i != cols - 1) {
1547  for (j = l; j < cols; ++j) {
1548 
1549  for (s = 0.0, k = i; k < rows; ++k) {
1550  s += ((double)U[k][i] * (double)U[k][j]);
1551  }
1552 
1553  f = s / h;
1554  for (k = i; k < rows; ++k) {
1555  U[k][j] += (float)(f * (double)U[k][i]);
1556  }
1557  }
1558  }
1559  for (k = i; k < rows; ++k) {
1560  U[k][i] = (float)((double)U[k][i]*scale);
1561  }
1562  }
1563  }
1564  D[i] = (float)(scale * g);
1565 
1566  // right-hand reduction
1567  g = s = scale = 0.0;
1568  if (i < rows && i != cols - 1) {
1569  for (k = l; k < cols; ++k) {
1570  scale += fabs((double)U[i][k]);
1571  }
1572 
1573  if (scale) {
1574  for (k = l; k < cols; ++k) {
1575  U[i][k] = (float)((double)U[i][k]/scale);
1576  s += ((double)U[i][k] * (double)U[i][k]);
1577  }
1578 
1579  f = (double)U[i][l];
1580  g = -SIGN(sqrt(s), f);
1581  h = f * g - s;
1582  U[i][l] = (float)(f - g);
1583 
1584  for (k = l; k < cols; ++k) {
1585  rv1[k] = (double)U[i][k] / h;
1586  }
1587 
1588  if (i != rows - 1) {
1589 
1590  for (j = l; j < rows; ++j) {
1591  for (s = 0.0, k = l; k < cols; ++k) {
1592  s += ((double)U[j][k] * (double)U[i][k]);
1593  }
1594 
1595  for (k = l; k < cols; ++k) {
1596  U[j][k] += (float)(s * rv1[k]);
1597  }
1598  }
1599  }
1600 
1601  for (k = l; k < cols; ++k) {
1602  U[i][k] = (float)((double)U[i][k]*scale);
1603  }
1604  }
1605  }
1606 
1607  anorm = max(anorm, fabs((double)D[i]) + fabs(rv1[i]));
1608  }
1609 
1610  // accumulate the right-hand transformation
1611  for (i = cols - 1; i >= 0; --i) {
1612  if (i < cols - 1) {
1613  if (g) {
1614  for (j = l; j < cols; ++j) {
1615  V[j][i] = (float)(((double)U[i][j] / (double)U[i][l]) / g);
1616  }
1617 
1618  // double division to avoid underflow
1619  for (j = l; j < cols; ++j) {
1620  for (s = 0.0, k = l; k < cols; ++k) {
1621  s += ((double)U[i][k] * (double)V[k][j]);
1622  }
1623 
1624  for (k = l; k < cols; ++k) {
1625  V[k][j] += (float)(s * (double)V[k][i]);
1626  }
1627  }
1628  }
1629 
1630  for (j = l; j < cols; ++j) {
1631  V[i][j] = V[j][i] = 0.0;
1632  }
1633  }
1634 
1635  V[i][i] = 1.0;
1636  g = rv1[i];
1637  l = i;
1638  }
1639 
1640  // accumulate the left-hand transformation
1641  for (i = cols - 1; i >= 0; --i) {
1642  l = i + 1;
1643  g = (double)D[i];
1644  if (i < cols - 1) {
1645  for (j = l; j < cols; ++j) {
1646  U[i][j] = 0.0;
1647  }
1648  }
1649 
1650  if (g) {
1651  g = 1.0 / g;
1652  if (i != cols - 1) {
1653  for (j = l; j < cols; ++j) {
1654  for (s = 0.0, k = l; k < rows; ++k) {
1655  s += ((double)U[k][i] * (double)U[k][j]);
1656  }
1657 
1658  f = (s / (double)U[i][i]) * g;
1659 
1660  for (k = i; k < rows; ++k) {
1661  U[k][j] += (float)(f * (double)U[k][i]);
1662  }
1663  }
1664  }
1665 
1666  for (j = i; j < rows; ++j) {
1667  U[j][i] = (float)((double)U[j][i]*g);
1668  }
1669 
1670  } else {
1671  for (j = i; j < rows; ++j) {
1672  U[j][i] = 0.0;
1673  }
1674  }
1675  ++U[i][i];
1676  }
1677 
1678  // diagonalize the bidiagonal form
1679  for (k = cols - 1; k >= 0; --k) {
1680  // loop over singular values
1681  for (its = 0; its < MAX_ITERATIONS; ++its) {
1682  // loop over allowed iterations
1683  flag = 1;
1684 
1685  for (l = k; l >= 0; --l) {
1686  // test for splitting
1687  nm = l - 1;
1688  if (fabs(rv1[l]) + anorm == anorm) {
1689  flag = 0;
1690  break;
1691  }
1692 
1693  if (fabs((double)D[nm]) + anorm == anorm) {
1694  break;
1695  }
1696  }
1697 
1698  if (flag) {
1699  c = 0.0;
1700  s = 1.0;
1701  for (i = l; i <= k; ++i) {
1702  f = s * rv1[i];
1703  if (fabs(f) + anorm != anorm) {
1704  g = (double)D[i];
1705  h = pythag(f, g);
1706  D[i] = (float)h;
1707  h = 1.0 / h;
1708  c = g * h;
1709  s = (- f * h);
1710  for (j = 0; j < rows; ++j) {
1711  y = (double)U[j][nm];
1712  z = (double)U[j][i];
1713  U[j][nm] = (float)(y * c + z * s);
1714  U[j][i] = (float)(z * c - y * s);
1715  }
1716  }
1717  }
1718  }
1719 
1720  z = (double)D[k];
1721  if (l == k) {
1722  // convergence
1723  if (z < 0.0) {
1724  // make singular value nonnegative
1725  D[k] = (float)(-z);
1726 
1727  for (j = 0; j < cols; ++j) {
1728  V[j][k] = (-V[j][k]);
1729  }
1730  }
1731  break;
1732  }
1733 
1734  if (its >= MAX_ITERATIONS) {
1735  free(rv1);
1736  rv1 = NULL;
1737  return "Failed to converge.";
1738  }
1739 
1740  // shift from bottom 2 x 2 minor
1741  x = (double)D[l];
1742  nm = k - 1;
1743  y = (double)D[nm];
1744  g = rv1[nm];
1745  h = rv1[k];
1746  f = ((y - z) * (y + z) + (g - h) * (g + h)) / (2.0 * h * y);
1747  g = pythag(f, 1.0);
1748  f = ((x - z) * (x + z) + h * ((y / (f + SIGN(g, f))) - h)) / x;
1749 
1750  // next QR transformation
1751  c = s = 1.0;
1752  for (j = l; j <= nm; ++j) {
1753  i = j + 1;
1754  g = rv1[i];
1755  y = (double)D[i];
1756  h = s * g;
1757  g = c * g;
1758  z = pythag(f, h);
1759  rv1[j] = z;
1760  c = f / z;
1761  s = h / z;
1762  f = x * c + g * s;
1763  g = g * c - x * s;
1764  h = y * s;
1765  y = y * c;
1766 
1767  for (jj = 0; jj < cols; ++jj) {
1768  x = (double)V[jj][j];
1769  z = (double)V[jj][i];
1770  V[jj][j] = (float)(x * c + z * s);
1771  V[jj][i] = (float)(z * c - x * s);
1772  }
1773  z = pythag(f, h);
1774  D[j] = (float)z;
1775  if (z) {
1776  z = 1.0 / z;
1777  c = f * z;
1778  s = h * z;
1779  }
1780  f = (c * g) + (s * y);
1781  x = (c * y) - (s * g);
1782  for (jj = 0; jj < rows; ++jj) {
1783  y = (double)U[jj][j];
1784  z = (double)U[jj][i];
1785  U[jj][j] = (float)(y * c + z * s);
1786  U[jj][i] = (float)(z * c - y * s);
1787  }
1788  }
1789  rv1[l] = 0.0;
1790  rv1[k] = f;
1791  D[k] = (float)x;
1792  }
1793  }
1794 
1795  System::alignedFree(rv1);
1796  rv1 = NULL;
1797 
1798  return NULL;
1799 }
int rows() const
Definition: Matrix.h:324
arena_t NULL
Definition: jemalloc_internal.h:624
static void alignedFree(void *ptr)
Definition: System.cpp:1546
#define debugAssertM(exp, message)
Definition: debugAssert.h:161
T max(const T &x, const T &y)
Definition: g3dmath.h:320
#define debugAssert(exp)
Definition: debugAssert.h:160
G3D::int16 z
Definition: Vector3int16.h:46
G3D::int16 y
Definition: Vector2int16.h:38
static void * alignedMalloc(size_t bytes, size_t alignment)
Definition: System.cpp:1482
int cols() const
Definition: Matrix.h:329
#define SIGN(a, b)
Definition: Matrix.cpp:1503
G3D::int16 x
Definition: Vector2int16.h:37
double sign(double fValue)
Definition: g3dmath.h:669
static double pythag(double a, double b)
Definition: Matrix.cpp:1486

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

Matrix G3D::Matrix::svdPseudoInverse ( float  tolerance = -1) const

Called from pseudoInverse when the matrix has size > 4 along some dimension.

900  {
901  if (cols() > rows()) {
902  return transpose().svdPseudoInverse(tolerance).transpose();
903  }
904 
905  // Matrices from SVD
906  Matrix U, V;
907 
908  // Diagonal elements
909  Array<T> d;
910 
911  svd(U, d, V);
912 
913  if (rows() == 1) {
914  d.resize(1, false);
915  }
916 
917  if (tolerance < 0) {
918  // TODO: Should be eps(d[0]), which is the largest diagonal
919  tolerance = G3D::max(rows(), cols()) * 0.0001f;
920  }
921 
922  Matrix X;
923 
924  int r = 0;
925  for (int i = 0; i < d.size(); ++i) {
926  if (d[i] > tolerance) {
927  d[i] = Matrix::T(1) / d[i];
928  ++r;
929  }
930  }
931 
932  if (r == 0) {
933  // There were no non-zero elements
934  X = zero(cols(), rows());
935  } else {
936  // Use the first r columns
937 
938  // Test code (the rest is below)
939  /*
940  d.resize(r);
941  Matrix testU = U.subMatrix(0, U.rows() - 1, 0, r - 1);
942  Matrix testV = V.subMatrix(0, V.rows() - 1, 0, r - 1);
943  Matrix testX = testV * Matrix::fromDiagonal(d) * testU.transpose();
944  X = testX;
945  */
946 
947 
948  // We want to do this:
949  //
950  // d.resize(r);
951  // U = U.subMatrix(0, U.rows() - 1, 0, r - 1);
952  // X = V * Matrix::fromDiagonal(d) * U.transpose();
953  //
954  // but creating a large diagonal matrix and then
955  // multiplying by it is wasteful. So we instead
956  // explicitly perform A = (D * U')' = U * D, and
957  // then multiply X = V * A'.
958 
959  Matrix A = Matrix(U.rows(), r);
960 
961  const T* dPtr = d.getCArray();
962  for (int i = 0; i < A.rows(); ++i) {
963  const T* Urow = U.impl->elt[i];
964  T* Arow = A.impl->elt[i];
965  const int Acols = A.cols();
966  for (int j = 0; j < Acols; ++j) {
967  // A(i,j) = U(i,:) * D(:,j)
968  // This is non-zero only at j = i because D is diagonal
969  // A(i,j) = U(i,j) * D(j,j)
970  Arow[j] = Urow[j] * dPtr[j];
971  }
972  }
973 
974  //
975  // Compute X = V.subMatrix(0, V.rows() - 1, 0, r - 1) * A.transpose()
976  //
977  // Avoid the explicit subMatrix call, and by storing A' instead of A, avoid
978  // both the transpose and the memory incoherence of striding across memory
979  // in big steps.
980 
981  alwaysAssertM(A.cols() == r,
982  "Internal dimension mismatch during pseudoInverse()");
983  alwaysAssertM(V.cols() >= r,
984  "Internal dimension mismatch during pseudoInverse()");
985 
986  X = Matrix(V.rows(), A.rows());
987  T** Xelt = X.impl->elt;
988  for (int i = 0; i < X.rows(); ++i) {
989  const T* Vrow = V.impl->elt[i];
990  for (int j = 0; j < X.cols(); ++j) {
991  const T* Arow = A.impl->elt[j];
992  T sum = 0;
993  for (int k = 0; k < r; ++k) {
994  sum += Vrow[k] * Arow[k];
995  }
996  Xelt[i][j] = sum;
997  }
998  }
999 
1000  /*
1001  // Test that results are the same after optimizations:
1002  Matrix diff = X - testX;
1003  T n = diff.norm();
1004  debugAssert(n < 0.0001);
1005  */
1006  }
1007 
1008  return X;
1009 }
int rows() const
Definition: Matrix.h:324
#define X
Definition: CollisionDetection.cpp:2281
T max(const T &x, const T &y)
Definition: g3dmath.h:320
int cols() const
Definition: Matrix.h:329
float T
Definition: Matrix.h:65
Matrix()
Definition: Matrix.h:289
void svd(Matrix &U, Array< T > &d, Matrix &V, bool sort=true) const
Definition: Matrix.cpp:425
static Matrix zero(int R, int C)
Definition: Matrix.cpp:237
#define alwaysAssertM(exp, message)
Definition: debugAssert.h:165
Matrix svdPseudoInverse(float tolerance=-1) const
Definition: Matrix.cpp:900
Matrix transpose() const
Definition: Matrix.h:488

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void G3D::Matrix::swapAndNegateCols ( int  c0,
int  c1 
)

Swaps columns c0 and c1 and negates both

380  {
381  debugAssert(c0 >= 0 && c0 < cols());
382  debugAssert(c1 >= 0 && c1 < cols());
383 
384  if (c0 == c1) {
385  return;
386  }
387 
388  if (! impl.unique()) {
389  impl.reset(new Impl(*impl));
390  }
391 
392  impl->swapAndNegateCols(c0, c1);
393 }
ImplRef impl
Definition: Matrix.h:239
#define debugAssert(exp)
Definition: debugAssert.h:160
int cols() const
Definition: Matrix.h:329

+ Here is the call graph for this function:

void G3D::Matrix::swapRows ( int  r0,
int  r1 
)
364  {
365  debugAssert(r0 >= 0 && r0 < rows());
366  debugAssert(r1 >= 0 && r1 < rows());
367 
368  if (r0 == r1) {
369  return;
370  }
371 
372  if (! impl.unique()) {
373  impl.reset(new Impl(*impl));
374  }
375 
376  impl->swapRows(r0, r1);
377 }
int rows() const
Definition: Matrix.h:324
ImplRef impl
Definition: Matrix.h:239
#define debugAssert(exp)
Definition: debugAssert.h:160

+ Here is the call graph for this function:

Matrix3 G3D::Matrix::toMatrix3 ( ) const
310  {
311  debugAssert(impl->R == 3);
312  debugAssert(impl->C == 3);
313  return Matrix3(
314  impl->get(0,0), impl->get(0,1), impl->get(0,2),
315  impl->get(1,0), impl->get(1,1), impl->get(1,2),
316  impl->get(2,0), impl->get(2,1), impl->get(2,2));
317 }
ImplRef impl
Definition: Matrix.h:239
#define debugAssert(exp)
Definition: debugAssert.h:160

+ Here is the caller graph for this function:

Matrix4 G3D::Matrix::toMatrix4 ( ) const
320  {
321  debugAssert(impl->R == 4);
322  debugAssert(impl->C == 4);
323  return Matrix4(
324  impl->get(0,0), impl->get(0,1), impl->get(0,2), impl->get(0,3),
325  impl->get(1,0), impl->get(1,1), impl->get(1,2), impl->get(1,3),
326  impl->get(2,0), impl->get(2,1), impl->get(2,2), impl->get(2,3),
327  impl->get(3,0), impl->get(3,1), impl->get(3,2), impl->get(3,3));
328 }
ImplRef impl
Definition: Matrix.h:239
#define debugAssert(exp)
Definition: debugAssert.h:160

+ Here is the caller graph for this function:

std::string G3D::Matrix::toString ( const std::string &  name) const
45  {
46  std::string s;
47 
48  if (name != "") {
49  s += format("%s = \n", name.c_str());
50  }
51 
52  s += "[";
53  for (int r = 0; r < rows(); ++r) {
54  for (int c = 0; c < cols(); ++c) {
55  double v = impl->get(r, c);
56 
57  if (::fabs(v) < 0.00001) {
58  // Don't print "negative zero"
59  s += format("% 10.04g", 0.0);
60  } else if (v == iRound(v)) {
61  // Print integers nicely
62  s += format("% 10.04g", v);
63  } else {
64  s += format("% 10.04f", v);
65  }
66 
67  if (c < cols() - 1) {
68  s += ",";
69  } else if (r < rows() - 1) {
70  s += ";\n ";
71  } else {
72  s += "]\n";
73  }
74  }
75  }
76  return s;
77 }
int rows() const
Definition: Matrix.h:324
int iRound(double fValue)
Definition: g3dmath.h:226
ImplRef impl
Definition: Matrix.h:239
std::string __cdecl format(const char *fmt...) G3D_CHECK_PRINTF_ARGS
int cols() const
Definition: Matrix.h:329

+ Here is the call graph for this function:

std::string G3D::Matrix::toString ( ) const
inline
584  {
585  static const std::string name = "";
586  return toString(name);
587  }
std::string toString() const
Definition: Matrix.h:584
Vector2 G3D::Matrix::toVector2 ( ) const
331  {
332  debugAssert(impl->R * impl->C == 2);
333  if (impl->R > impl->C) {
334  return Vector2(impl->get(0,0), impl->get(1,0));
335  } else {
336  return Vector2(impl->get(0,0), impl->get(0,1));
337  }
338 }
ImplRef impl
Definition: Matrix.h:239
#define debugAssert(exp)
Definition: debugAssert.h:160
Vector3 G3D::Matrix::toVector3 ( ) const
341  {
342  debugAssert(impl->R * impl->C == 3);
343  if (impl->R > impl->C) {
344  return Vector3(impl->get(0,0), impl->get(1,0), impl->get(2, 0));
345  } else {
346  return Vector3(impl->get(0,0), impl->get(0,1), impl->get(0, 2));
347  }
348 }
ImplRef impl
Definition: Matrix.h:239
#define debugAssert(exp)
Definition: debugAssert.h:160
Vector4 G3D::Matrix::toVector4 ( ) const
351  {
352  debugAssert(
353  ((impl->R == 4) && (impl->C == 1)) ||
354  ((impl->R == 1) && (impl->C == 4)));
355 
356  if (impl->R > impl->C) {
357  return Vector4(impl->get(0,0), impl->get(1,0), impl->get(2, 0), impl->get(3,0));
358  } else {
359  return Vector4(impl->get(0,0), impl->get(0,1), impl->get(0, 2), impl->get(0,3));
360  }
361 }
ImplRef impl
Definition: Matrix.h:239
#define debugAssert(exp)
Definition: debugAssert.h:160
Matrix G3D::Matrix::transpose ( ) const
inline

AT

488  {
489  Impl* A = new Impl(cols(), rows());
490  impl->transpose(*A);
491  return Matrix(A);
492  }
int rows() const
Definition: Matrix.h:324
ImplRef impl
Definition: Matrix.h:239
int cols() const
Definition: Matrix.h:329
Matrix()
Definition: Matrix.h:289

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void G3D::Matrix::transpose ( Matrix out) const

Transpose in place; more efficient than transpose

300  {
301  if ((out.impl == impl) && impl.unique() && (impl->R == impl->C)) {
302  // In place
303  impl->transpose(*out.impl);
304  } else {
305  out = this->transpose();
306  }
307 }
ImplRef impl
Definition: Matrix.h:239
Matrix transpose() const
Definition: Matrix.h:488

+ Here is the call graph for this function:

Matrix G3D::Matrix::vectorPseudoInverse ( ) const
private
1012  {
1013  // If vector A has nonzero elements: transpose A, then divide each elt. by the squared norm
1014  // If A is zero vector: transpose A
1015  double x = 0.0;
1016 
1017  if (anyNonZero()) {
1018  x = 1.0 / normSquared();
1019  }
1020 
1021  Matrix A(cols(), rows());
1022  T** Aelt = A.impl->elt;
1023  for (int r = 0; r < rows(); ++r) {
1024  const T* MyRow = impl->elt[r];
1025  for (int c = 0; c < cols(); ++c) {
1026  Aelt[c][r] = T(MyRow[c] * x);
1027  }
1028  }
1029  return Matrix(A);
1030 }
int rows() const
Definition: Matrix.h:324
ImplRef impl
Definition: Matrix.h:239
int cols() const
Definition: Matrix.h:329
float T
Definition: Matrix.h:65
bool anyNonZero() const
Definition: Matrix.cpp:415
Matrix()
Definition: Matrix.h:289
double normSquared() const
Definition: Matrix.cpp:508
G3D::int16 x
Definition: Vector2int16.h:37

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

Matrix G3D::Matrix::zero ( int  R,
int  C 
)
static

Returns a new matrix that is all zero.

237  {
238  Impl* A = new Impl(R, C);
239  A->setZero();
240  return Matrix(A);
241 }
Matrix()
Definition: Matrix.h:289

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

Member Data Documentation

int G3D::Matrix::debugNumAllocOps = 0
static

Incremented every time a new matrix object is allocated. Useful for profiling your own code that uses Matrix to determine when it is slow due to allocation.

int G3D::Matrix::debugNumCopyOps = 0
static

Incremented every time the elements of a matrix are copied. Useful for profiling your own code that uses Matrix to determine when it is slow due to copying.

ImplRef G3D::Matrix::impl
private

The documentation for this class was generated from the following files: