1 files changed, 271 insertions, 0 deletions

@@ -0,0 +1,271 @@
+#include "matrix3x3.h"
+#include <iostream>
+#include <cmath>
+#include <vector>
+#include <cassert>
+
+Matrix3x3::Matrix3x3()
+{
+  
+}
+
+Matrix3x3::Matrix3x3(std::initializer_list<Vector3D> vals)
+{
+  std::vector<Vector3D> _entries(vals);
+  assert(_entries.size() == 3);
+  
+  entries[0] = _entries[0];
+  entries[1] = _entries[1];
+  entries[2] = _entries[2];
+}
+
+double& Matrix3x3::operator()(int i, int j)
+{
+  return entries[j][i];
+}
+
+const double& Matrix3x3::operator()(int i, int j) const
+{
+  return entries[j][i];
+}
+
+Vector3D& Matrix3x3::operator[](int j)
+{
+  return entries[j];
+}
+
+const Vector3D& Matrix3x3::operator[](int j) const
+{
+  return entries[j];
+}
+
+void Matrix3x3::zero(double val)
+{
+  // sets all elements to val
+  entries[0] = entries[1] = entries[2] = Vector3D( val, val, val );
+}
+
+double Matrix3x3::det() const
+{
+  const Matrix3x3& A( *this );
+
+  return -A(0,2)*A(1,1)*A(2,0) + A(0,1)*A(1,2)*A(2,0) +
+          A(0,2)*A(1,0)*A(2,1) - A(0,0)*A(1,2)*A(2,1) -
+          A(0,1)*A(1,0)*A(2,2) + A(0,0)*A(1,1)*A(2,2) ;
+}
+
+double Matrix3x3::norm() const
+{
+  return sqrt( entries[0].norm2() +
+               entries[1].norm2() +
+               entries[2].norm2() );
+}
+
+Matrix3x3 Matrix3x3::operator-() const
+{
+  const Matrix3x3& A( *this );
+  Matrix3x3 B;
+
+  B(0,0) = -A(0,0); B(0,1) = -A(0,1); B(0,2) = -A(0,2);
+  B(1,0) = -A(1,0); B(1,1) = -A(1,1); B(1,2) = -A(1,2);
+  B(2,0) = -A(2,0); B(2,1) = -A(2,1); B(2,2) = -A(2,2);
+
+  return B;
+}
+
+void Matrix3x3::operator+=(const Matrix3x3& B)
+{
+  Matrix3x3& A( *this );
+  double* Aij = (double*) &A;
+  const double* Bij = (const double*) &B;
+
+  *Aij++ += *Bij++;
+  *Aij++ += *Bij++;
+  *Aij++ += *Bij++;
+  *Aij++ += *Bij++;
+  *Aij++ += *Bij++;
+  *Aij++ += *Bij++;
+  *Aij++ += *Bij++;
+  *Aij++ += *Bij++;
+  *Aij++ += *Bij++;
+}
+
+Matrix3x3 Matrix3x3::operator-(const Matrix3x3& B) const
+{
+  const Matrix3x3& A( *this );
+  Matrix3x3 C;
+
+  for( int i = 0; i < 3; i++ )
+  for( int j = 0; j < 3; j++ )
+  {
+     C(i,j) = A(i,j) - B(i,j);
+  }
+
+  return C;
+}
+
+Matrix3x3 Matrix3x3::operator*(double c) const
+{
+  const Matrix3x3& A( *this );
+  Matrix3x3 B;
+
+  for( int i = 0; i < 3; i++ )
+  for( int j = 0; j < 3; j++ )
+  {
+     B(i,j) = c*A(i,j);
+  }
+
+  return B;
+}
+
+Matrix3x3 operator*(double c, const Matrix3x3& A)
+{
+  Matrix3x3 cA;
+  const double* Aij = (const double*) &A;
+  double* cAij = (double*) &cA;
+
+  *cAij++ = c * (*Aij++);
+  *cAij++ = c * (*Aij++);
+  *cAij++ = c * (*Aij++);
+  *cAij++ = c * (*Aij++);
+  *cAij++ = c * (*Aij++);
+  *cAij++ = c * (*Aij++);
+  *cAij++ = c * (*Aij++);
+  *cAij++ = c * (*Aij++);
+  *cAij++ = c * (*Aij++);
+
+  return cA;
+}
+
+Matrix3x3 Matrix3x3::operator*(const Matrix3x3& B) const
+{
+  const Matrix3x3& A( *this );
+  Matrix3x3 C;
+
+  for( int i = 0; i < 3; i++ )
+  for( int j = 0; j < 3; j++ )
+  {
+     C(i,j) = 0.;
+
+     for( int k = 0; k < 3; k++ )
+     {
+        C(i,j) += A(i,k)*B(k,j);
+     }
+  }
+
+  return C;
+}
+
+Vector3D Matrix3x3::operator*(const Vector3D& x) const
+{
+  return x[0]*entries[0] +
+         x[1]*entries[1] +
+         x[2]*entries[2] ;
+}
+
+Matrix3x3 Matrix3x3::T() const
+{
+  const Matrix3x3& A( *this );
+  Matrix3x3 B;
+
+  for( int i = 0; i < 3; i++ )
+  for( int j = 0; j < 3; j++ )
+  {
+     B(i,j) = A(j,i);
+  }
+
+  return B;
+}
+
+Matrix3x3 Matrix3x3::inv() const
+{
+  const Matrix3x3& A( *this );
+  Matrix3x3 B;
+
+  B(0,0) = -A(1,2)*A(2,1) + A(1,1)*A(2,2); B(0,1) =  A(0,2)*A(2,1) - A(0,1)*A(2,2); B(0,2) = -A(0,2)*A(1,1) + A(0,1)*A(1,2);
+  B(1,0) =  A(1,2)*A(2,0) - A(1,0)*A(2,2); B(1,1) = -A(0,2)*A(2,0) + A(0,0)*A(2,2); B(1,2) =  A(0,2)*A(1,0) - A(0,0)*A(1,2);
+  B(2,0) = -A(1,1)*A(2,0) + A(1,0)*A(2,1); B(2,1) =  A(0,1)*A(2,0) - A(0,0)*A(2,1); B(2,2) = -A(0,1)*A(1,0) + A(0,0)*A(1,1);
+
+  B /= det();
+
+  return B;
+}
+
+void Matrix3x3::operator/=(double x)
+{
+  Matrix3x3& A( *this );
+  double rx = 1./x;
+
+  for( int i = 0; i < 3; i++ )
+  for( int j = 0; j < 3; j++ )
+  {
+     A( i, j ) *= rx;
+  }
+}
+
+Matrix3x3 Matrix3x3::identity()
+{
+  Matrix3x3 B;
+
+  B(0,0) = 1.; B(0,1) = 0.; B(0,2) = 0.;
+  B(1,0) = 0.; B(1,1) = 1.; B(1,2) = 0.;
+  B(2,0) = 0.; B(2,1) = 0.; B(2,2) = 1.;
+
+  return B;
+}
+
+Matrix3x3 Matrix3x3::crossProduct(const Vector3D& u)
+{
+  Matrix3x3 B;
+
+  B(0,0) =   0.;  B(0,1) = -u.z;  B(0,2) =  u.y;
+  B(1,0) =  u.z;  B(1,1) =   0.;  B(1,2) = -u.x;
+  B(2,0) = -u.y;  B(2,1) =  u.x;  B(2,2) =   0.;
+
+  return B;
+}
+
+Matrix3x3 outer(const Vector3D& u, const Vector3D& v)
+{
+  Matrix3x3 B;
+  double* Bij = (double*) &B;
+
+  *Bij++ = u.x*v.x;
+  *Bij++ = u.y*v.x;
+  *Bij++ = u.z*v.x;
+  *Bij++ = u.x*v.y;
+  *Bij++ = u.y*v.y;
+  *Bij++ = u.z*v.y;
+  *Bij++ = u.x*v.z;
+  *Bij++ = u.y*v.z;
+  *Bij++ = u.z*v.z;
+
+  return B;
+}
+
+std::ostream& operator<<(std::ostream& os, const Matrix3x3& A)
+{
+  for (int i = 0; i < 3; i++)
+  {
+    os << "[ ";
+
+    for (int j = 0; j < 3; j++)
+    {
+      os << A(i,j) << " ";
+    }
+
+    os << "]" << std::endl;
+  }
+
+  return os;
+}
+
+Vector3D& Matrix3x3::column(int i)
+{
+  return entries[i];
+}
+
+const Vector3D& Matrix3x3::column(int i) const
+{
+  return entries[i];
+}