Bart Janssens
Simple Vector Library 1.5 http://www.cs.cmu.edu/~ajw/doc/svl.html
Revision 0:785cff1e5a7c, committed 2016-01-04
- Comitter:
- BartJanssens
- Date:
- Mon Jan 04 15:19:10 2016 +0000
- Child:
- 1:e25ff4b06ed2
- Commit message:
- svl-1.5
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Basics.cpp Mon Jan 04 15:19:10 2016 +0000
@@ -0,0 +1,78 @@
+/*
+ File: Basics.cpp
+
+ Function: Implements Basics.h
+
+ Author(s): Andrew Willmott
+
+ Copyright: (c) 1995-2001, Andrew Willmott
+
+ Notes:
+
+*/
+
+#include "Basics.h"
+//#include <cstdio>
+//#include <cstdlib>
+//#include <iostream>
+
+
+//using namespace std;
+
+
+// --- Error functions for range and routine checking -------------------------
+
+
+static Void DebuggerBreak()
+{
+ abort();
+}
+
+Void _Assert(Int condition, const Char *errorMessage, const Char *file, Int line)
+{
+ if (!condition)
+ {
+ //Char reply;
+
+ //cerr << "\n*** Assert failed (line " << line << " in " <<
+ // file << "): " << errorMessage << endl;
+ printf("\r\n*** Assert failed (line \"%d \" in \" %s \"): \"%s", line, file, errorMessage);
+ //cerr << " Continue? [y/n] ";
+ //cin >> reply;
+
+ //if (reply != 'y')
+ //{
+ DebuggerBreak();
+ exit(1);
+ //}
+ }
+}
+
+Void _Expect(Int condition, const Char *warningMessage, const Char *file, Int line)
+{
+ if (!condition)
+ //cerr << "\n*** Warning (line " << line << " in " << file << "): " <<
+ // warningMessage << endl;
+ printf("\r\n*** Warning (line \"%d \" in \" %s \"): \"%s", line, file, warningMessage);
+}
+
+Void _CheckRange(Int i, Int lowerBound, Int upperBound,
+ const Char *rangeMessage, const Char *file, Int line)
+{
+ if (i < lowerBound || i >= upperBound)
+ {
+ //Char reply;
+
+ //cerr << "\n*** Range Error (line " << line << " in " << file <<
+ // "): " << rangeMessage << endl;
+ printf("\r\n*** Range Error (line \"%d \" in \" %s \"): \"%s", line, file, rangeMessage);
+ //cerr << " Continue? [y/n] ";
+ //cin >> reply;
+
+ //if (reply != 'y')
+ //{
+ DebuggerBreak();
+ exit(1);
+ //}
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Basics.h Mon Jan 04 15:19:10 2016 +0000
@@ -0,0 +1,162 @@
+/*
+ File: Basics.h
+
+ Function: Basic definitions for all files. Contains type
+ definitions, assertion and debugging facilities, and
+ miscellaneous useful template functions.
+
+ This is a cut-down version for SVL.
+
+ Author(s): Andrew Willmott
+
+ Copyright: (c) 1995-2001, Andrew Willmott
+
+ Notes: This header is affected by the following defines:
+
+ VL_CHECKING - Include code for assertions,
+ range errors and warnings.
+ VL_FLOAT - Use floats for real numbers. (Doubles
+ are the default.)
+ VL_NO_BOOL - There is no bool type.
+ VL_NO_TF - true and false are not predefined.
+*/
+
+#ifndef __Basics__
+#define __Basics__
+
+#include "mbed.h"
+//#include <iostream>
+//#include <cmath>
+
+
+// --- Basic types -------------------------------------------------------------
+
+typedef void Void;
+typedef float Float;
+typedef double Double;
+typedef char Char;
+typedef int Short;
+typedef int Int;
+typedef long Long;
+typedef unsigned char Byte;
+typedef unsigned int UInt;
+
+#ifndef VL_FLOAT
+typedef Double Real;
+#else
+typedef Float Real;
+#endif
+
+#define SELF (*this) // A syntactic convenience.
+
+
+// --- Boolean type ------------------------------------------------------------
+
+// X11 #defines 'Bool' -- typical.
+
+#ifdef Bool
+#undef Bool
+#endif
+
+#ifndef VL_NO_BOOL
+// if the compiler implements the bool type...
+typedef bool Bool;
+#else
+// if not, make up our own.
+class Bool
+{
+public:
+
+ Bool() : val(0) {};
+ Bool(Int b) : val(b) {};
+
+ operator Int() { return val; };
+
+private:
+ Int val;
+};
+#ifdef VL_NO_TF
+enum {false, true};
+#endif
+#endif
+
+
+// --- Assertions and Range checking -------------------------------------------
+
+#define _Error(e) _Assert(false, e, __FILE__, __LINE__)
+#define _Warning(w) _Expect(false, w, __FILE__, __LINE__)
+
+#if defined(VL_CHECKING)
+#define Assert(b, e) _Assert(b, e, __FILE__, __LINE__)
+ // Assert that b is true. e is an error message to be printed if b
+ // is false.
+#define Expect(b, w) _Expect(b, w, __FILE__, __LINE__)
+ // Prints warning w if b is false
+#define CheckRange(i, l, u, r) _CheckRange(i, l, u, r, __FILE__, __LINE__)
+ // Checks whether i is in the range [lowerBound, upperBound).
+#else
+#define Assert(b, e)
+#define Expect(b, w)
+#define CheckRange(a, l, u, r)
+#endif
+
+Void _Assert(Int condition, const Char *errorMessage, const Char *file, Int line);
+Void _Expect(Int condition, const Char *warningMessage, const Char *file, Int line);
+Void _CheckRange(Int i, Int lowerBound, Int upperBound, const Char *rangeMessage,
+ const Char *file, Int line);
+
+
+// --- Inlines -----------------------------------------------------------------
+
+template<class Value>
+ inline Value Min(Value x, Value y)
+ {
+ if (x <= y)
+ return(x);
+ else
+ return(y);
+ };
+
+template<class Value>
+ inline Value Max(Value x, Value y)
+ {
+ if (x >= y)
+ return(x);
+ else
+ return(y);
+ };
+
+template<class Value>
+ inline Void Swap(Value &x, Value &y)
+ {
+ Value t;
+
+ t = x;
+ x = y;
+ y = t;
+ };
+
+template<class Value>
+ inline Value Mix(Value x, Value y, Real s)
+ {
+ return(x + (y - x) * s);
+ };
+
+template<class Value>
+ inline Value Clip(Value x, Value min, Value max)
+ {
+ if (x < min)
+ return(min);
+ else if (x > max)
+ return(max);
+ else
+ return(x);
+ };
+
+template<class Value>
+ inline Value sqr(Value x)
+ {
+ return(x * x);
+ };
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Constants.h Mon Jan 04 15:19:10 2016 +0000
@@ -0,0 +1,44 @@
+/*
+ File: Constants.h
+
+ Function: Contains various constants for VL.
+
+ Author: Andrew Willmott
+
+ Copyright: (c) 1999-2001, Andrew Willmott
+*/
+
+#ifndef __VLConstants__
+#define __VLConstants__
+
+#include <cmath>
+#include "Basics.h"
+
+
+// --- Mathematical constants -------------------------------------------------
+
+
+#ifdef M_PI
+const double vl_pi = M_PI;
+const double vl_halfPi = M_PI_2;
+#elif defined(_PI)
+const double vl_pi = _PI;
+const double vl_halfPi = vl_pi / 2.0;
+#else
+const double vl_pi = 3.14159265358979323846;
+const double vl_halfPi = vl_pi / 2.0;
+#endif
+
+#ifdef HUGE_VAL
+const double vl_inf = HUGE_VAL;
+#endif
+
+enum ZeroOrOne { vl_zero = 0, vl_0 = 0, vl_one = 1, vl_I = 1, vl_1 = 1 };
+enum Block { vl_Z = 0, vl_B = 1, vl_block = 1 };
+enum Axis { vl_x, vl_y, vl_z, vl_w };
+typedef Axis vl_axis;
+
+const UInt VL_REF_FLAG = UInt(1) << (sizeof(UInt) * 8 - 1);
+const UInt VL_REF_MASK = (~VL_REF_FLAG);
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Mat.cpp Mon Jan 04 15:19:10 2016 +0000
@@ -0,0 +1,676 @@
+/*
+ File: Mat.cpp
+
+ Function: Implements Mat.h
+
+ Author(s): Andrew Willmott
+
+ Copyright: (c) 1995-2001, Andrew Willmott
+
+*/
+
+#include "Mat.h"
+
+//#include <cctype>
+//#include <cstring>
+//#include <cstdarg>
+//#include <iomanip>
+
+
+
+// --- Mat Constructors & Destructors -----------------------------------------
+
+
+Mat::Mat(Int rows, Int cols, ZeroOrOne k) : rows(rows), cols(cols)
+{
+ Assert(rows > 0 && cols > 0, "(Mat) illegal matrix size");
+
+ data = new Real[rows * cols];
+
+ MakeDiag(k);
+}
+
+Mat::Mat(Int rows, Int cols, Block k) : rows(rows), cols(cols)
+{
+ Assert(rows > 0 && cols > 0, "(Mat) illegal matrix size");
+
+ data = new Real[rows * cols];
+
+ MakeBlock(k);
+}
+
+Mat::Mat(Int rows, Int cols, double elt0, ...) : rows(rows), cols(cols)
+// The double is hardwired here because it is the only type that will work
+// with var args and C++ real numbers.
+{
+ Assert(rows > 0 && cols > 0, "(Mat) illegal matrix size");
+
+ va_list ap;
+ Int i, j;
+
+ data = new Real[rows * cols];
+ va_start(ap, elt0);
+
+ SetReal(data[0], elt0);
+
+ for (i = 1; i < cols; i++)
+ SetReal(Elt(0, i), va_arg(ap, double));
+
+ for (i = 1; i < rows; i++)
+ for (j = 0; j < cols; j++)
+ SetReal(Elt(i, j), va_arg(ap, double));
+
+ va_end(ap);
+}
+
+Mat::Mat(const Mat &m) : cols(m.cols)
+{
+ Assert(m.data != 0, "(Mat) Can't construct from null matrix");
+ rows = m.Rows();
+
+ UInt elts = rows * cols;
+
+ data = new Real[elts];
+#ifdef VL_USE_MEMCPY
+ memcpy(data, m.data, elts * sizeof(Real));
+#else
+ for (UInt i = 0; i < elts; i++)
+ data[i] = m.data[i];
+#endif
+}
+
+Mat::Mat(const Mat2 &m) : data(m.Ref()), rows(2 | VL_REF_FLAG), cols(2)
+{
+}
+
+Mat::Mat(const Mat3 &m) : data(m.Ref()), rows(3 | VL_REF_FLAG), cols(3)
+{
+}
+
+Mat::Mat(const Mat4 &m) : data(m.Ref()), rows(4 | VL_REF_FLAG), cols(4)
+{
+}
+
+
+// --- Mat Assignment Operators -----------------------------------------------
+
+
+Mat &Mat::operator = (const Mat &m)
+{
+ if (!IsRef())
+ SetSize(m.Rows(), m.Cols());
+ else
+ Assert(Rows() == m.Rows(), "(Mat::=) Matrix rows don't match");
+ for (Int i = 0; i < Rows(); i++)
+ SELF[i] = m[i];
+
+ return(SELF);
+}
+
+Mat &Mat::operator = (const Mat2 &m)
+{
+ if (!IsRef())
+ SetSize(m.Rows(), m.Cols());
+ else
+ Assert(Rows() == m.Rows(), "(Mat::=) Matrix rows don't match");
+ for (Int i = 0; i < Rows(); i++)
+ SELF[i] = m[i];
+
+ return(SELF);
+}
+
+Mat &Mat::operator = (const Mat3 &m)
+{
+ if (!IsRef())
+ SetSize(m.Rows(), m.Cols());
+ else
+ Assert(Rows() == m.Rows(), "(Mat::=) Matrix rows don't match");
+ for (Int i = 0; i < Rows(); i++)
+ SELF[i] = m[i];
+
+ return(SELF);
+}
+
+Mat &Mat::operator = (const Mat4 &m)
+{
+ if (!IsRef())
+ SetSize(m.Rows(), m.Cols());
+ else
+ Assert(Rows() == m.Rows(), "(Mat::=) Matrix rows don't match");
+
+ for (Int i = 0; i < Rows(); i++)
+ SELF[i] = m[i];
+
+ return(SELF);
+}
+
+Void Mat::SetSize(Int nrows, Int ncols)
+{
+ UInt elts = nrows * ncols;
+ Assert(nrows > 0 && ncols > 0, "(Mat::SetSize) Illegal matrix size.");
+ UInt oldElts = Rows() * Cols();
+
+ if (IsRef())
+ {
+ // Abort! We don't allow this operation on references.
+ _Error("(Mat::SetSize) Trying to resize a matrix reference");
+ }
+
+ rows = nrows;
+ cols = ncols;
+
+ // Don't reallocate if we already have enough storage
+ if (elts <= oldElts)
+ return;
+
+ // Otherwise, delete old storage and reallocate
+ delete[] data;
+ data = 0;
+ data = new Real[elts]; // may throw an exception
+}
+
+Void Mat::SetSize(const Mat &m)
+{
+ SetSize(m.Rows(), m.Cols());
+}
+
+Void Mat::MakeZero()
+{
+#ifdef VL_USE_MEMCPY
+ memset(data, 0, sizeof(Real) * Rows() * Cols());
+#else
+ Int i;
+
+ for (i = 0; i < Rows(); i++)
+ SELF[i] = vl_zero;
+#endif
+}
+
+Void Mat::MakeDiag(Real k)
+{
+ Int i, j;
+
+ for (i = 0; i < Rows(); i++)
+ for (j = 0; j < Cols(); j++)
+ if (i == j)
+ Elt(i,j) = k;
+ else
+ Elt(i,j) = vl_zero;
+}
+
+Void Mat::MakeDiag()
+{
+ Int i, j;
+
+ for (i = 0; i < Rows(); i++)
+ for (j = 0; j < Cols(); j++)
+ Elt(i,j) = (i == j) ? vl_one : vl_zero;
+}
+
+Void Mat::MakeBlock(Real k)
+{
+ Int i;
+
+ for (i = 0; i < Rows(); i++)
+ SELF[i].MakeBlock(k);
+}
+
+Void Mat::MakeBlock()
+{
+ Int i, j;
+
+ for (i = 0; i < Rows(); i++)
+ for (j = 0; j < Cols(); j++)
+ Elt(i,j) = vl_one;
+}
+
+
+// --- Mat Assignment Operators -----------------------------------------------
+
+
+Mat &Mat::operator += (const Mat &m)
+{
+ Assert(Rows() == m.Rows(), "(Mat::+=) matrix rows don't match");
+
+ Int i;
+
+ for (i = 0; i < Rows(); i++)
+ SELF[i] += m[i];
+
+ return(SELF);
+}
+
+Mat &Mat::operator -= (const Mat &m)
+{
+ Assert(Rows() == m.Rows(), "(Mat::-=) matrix rows don't match");
+
+ Int i;
+
+ for (i = 0; i < Rows(); i++)
+ SELF[i] -= m[i];
+
+ return(SELF);
+}
+
+Mat &Mat::operator *= (const Mat &m)
+{
+ Assert(Cols() == m.Cols(), "(Mat::*=) matrix columns don't match");
+
+ Int i;
+
+ for (i = 0; i < Rows(); i++)
+ SELF[i] = SELF[i] * m;
+
+ return(SELF);
+}
+
+Mat &Mat::operator *= (Real s)
+{
+ Int i;
+
+ for (i = 0; i < Rows(); i++)
+ SELF[i] *= s;
+
+ return(SELF);
+}
+
+Mat &Mat::operator /= (Real s)
+{
+ Int i;
+
+ for (i = 0; i < Rows(); i++)
+ SELF[i] /= s;
+
+ return(SELF);
+}
+
+
+// --- Mat Comparison Operators -----------------------------------------------
+
+
+Bool operator == (const Mat &m, const Mat &n)
+{
+ Assert(n.Rows() == m.Rows(), "(Mat::==) matrix rows don't match");
+
+ Int i;
+
+ for (i = 0; i < m.Rows(); i++)
+ if (m[i] != n[i])
+ return(0);
+
+ return(1);
+}
+
+Bool operator != (const Mat &m, const Mat &n)
+{
+ Assert(n.Rows() == m.Rows(), "(Mat::!=) matrix rows don't match");
+
+ Int i;
+
+ for (i = 0; i < m.Rows(); i++)
+ if (m[i] != n[i])
+ return(1);
+
+ return(0);
+}
+
+
+// --- Mat Arithmetic Operators -----------------------------------------------
+
+
+Mat operator + (const Mat &m, const Mat &n)
+{
+ Assert(n.Rows() == m.Rows(), "(Mat::+) matrix rows don't match");
+
+ Mat result(m.Rows(), m.Cols());
+ Int i;
+
+ for (i = 0; i < m.Rows(); i++)
+ result[i] = m[i] + n[i];
+
+ return(result);
+}
+
+Mat operator - (const Mat &m, const Mat &n)
+{
+ Assert(n.Rows() == m.Rows(), "(Mat::-) matrix rows don't match");
+
+ Mat result(m.Rows(), m.Cols());
+ Int i;
+
+ for (i = 0; i < m.Rows(); i++)
+ result[i] = m[i] - n[i];
+
+ return(result);
+}
+
+Mat operator - (const Mat &m)
+{
+ Mat result(m.Rows(), m.Cols());
+ Int i;
+
+ for (i = 0; i < m.Rows(); i++)
+ result[i] = -m[i];
+
+ return(result);
+}
+
+Mat operator * (const Mat &m, const Mat &n)
+{
+ Assert(m.Cols() == n.Rows(), "(Mat::*m) matrix cols don't match");
+
+ Mat result(m.Rows(), n.Cols());
+ Int i;
+
+ for (i = 0; i < m.Rows(); i++)
+ result[i] = m[i] * n;
+
+ return(result);
+}
+
+Vec operator * (const Mat &m, const Vec &v)
+{
+ Assert(m.Cols() == v.Elts(), "(Mat::*v) matrix and vector sizes don't match");
+
+ Int i;
+ Vec result(m.Rows());
+
+ for (i = 0; i < m.Rows(); i++)
+ result[i] = dot(v, m[i]);
+
+ return(result);
+}
+
+Mat operator * (const Mat &m, Real s)
+{
+ Int i;
+ Mat result(m.Rows(), m.Cols());
+
+ for (i = 0; i < m.Rows(); i++)
+ result[i] = m[i] * s;
+
+ return(result);
+}
+
+Mat operator / (const Mat &m, Real s)
+{
+ Int i;
+ Mat result(m.Rows(), m.Cols());
+
+ for (i = 0; i < m.Rows(); i++)
+ result[i] = m[i] / s;
+
+ return(result);
+}
+
+
+// --- Mat Mat-Vec Functions --------------------------------------------------
+
+
+Vec operator * (const Vec &v, const Mat &m) // v * m
+{
+ Assert(v.Elts() == m.Rows(), "(Mat::v*m) vector/matrix sizes don't match");
+
+ Vec result(m.Cols(), vl_zero);
+ Int i;
+
+ for (i = 0; i < m.Rows(); i++)
+ result += m[i] * v[i];
+
+ return(result);
+}
+
+
+// --- Mat Special Functions --------------------------------------------------
+
+
+Mat trans(const Mat &m)
+{
+ Int i,j;
+ Mat result(m.Cols(), m.Rows());
+
+ for (i = 0; i < m.Rows(); i++)
+ for (j = 0; j < m.Cols(); j++)
+ result.Elt(j,i) = m.Elt(i,j);
+
+ return(result);
+}
+
+Real trace(const Mat &m)
+{
+ Int i;
+ Real result = vl_0;
+
+ for (i = 0; i < m.Rows(); i++)
+ result += m.Elt(i,i);
+
+ return(result);
+}
+
+Mat &Mat::Clamp(Real fuzz)
+// clamps all values of the matrix with a magnitude
+// smaller than fuzz to zero.
+{
+ Int i;
+
+ for (i = 0; i < Rows(); i++)
+ SELF[i].Clamp(fuzz);
+
+ return(SELF);
+}
+
+Mat &Mat::Clamp()
+{
+ return(Clamp(1e-7));
+}
+
+Mat clamped(const Mat &m, Real fuzz)
+// clamps all values of the matrix with a magnitude
+// smaller than fuzz to zero.
+{
+ Mat result(m);
+
+ return(result.Clamp(fuzz));
+}
+
+Mat clamped(const Mat &m)
+{
+ return(clamped(m, 1e-7));
+}
+
+Mat oprod(const Vec &a, const Vec &b)
+// returns outerproduct of a and b: a * trans(b)
+{
+ Mat result;
+ Int i;
+
+ result.SetSize(a.Elts(), b.Elts());
+ for (i = 0; i < a.Elts(); i++)
+ result[i] = a[i] * b;
+
+ return(result);
+}
+
+
+// --- Mat Input & Output -----------------------------------------------------
+
+void printMat(const Mat &m)
+{
+ Int i;
+
+ printf("[");
+ for (i = 0; i < m.Rows() - 1; i++){
+ printVec(m[i]);
+ printf("\r\n");
+ }
+ printVec(m[i]);
+ printf("]\r\n");
+}
+
+/*
+ostream &operator << (ostream &s, const Mat &m)
+{
+ Int i, w = s.width();
+
+ s << '[';
+ for (i = 0; i < m.Rows() - 1; i++)
+ s << setw(w) << m[i] << "\r\n";
+ s << setw(w) << m[i] << ']' << "\r\n";
+ return(s);
+}
+*/
+
+inline Void CopyPartialMat(const Mat &m, Mat &n, Int numRows)
+{
+ for (Int i = 0; i < numRows; i++)
+ n[i] = m[i];
+}
+
+/*
+istream &operator >> (istream &s, Mat &m)
+{
+ Int size = 1;
+ Char c;
+ Mat inMat;
+
+ // Expected format: [row0 row1 row2 ...]
+
+ while (isspace(s.peek())) // chomp white space
+ s.get(c);
+
+ if (s.peek() == '[')
+ {
+ Vec row;
+
+ s.get(c);
+
+ s >> row;
+ inMat.SetSize(2 * row.Elts(), row.Elts());
+ inMat[0] = row;
+
+ while (isspace(s.peek())) // chomp white space
+ s.get(c);
+
+ while (s.peek() != ']') // resize if needed
+ {
+ if (size == inMat.Rows())
+ {
+ Mat holdMat(inMat);
+
+ inMat.SetSize(size * 2, inMat.Cols());
+ CopyPartialMat(holdMat, inMat, size);
+ }
+
+ s >> row; // read a row
+ inMat[size++] = row;
+
+ if (!s)
+ {
+ _Warning("Couldn't read matrix row");
+ return(s);
+ }
+
+ while (isspace(s.peek())) // chomp white space
+ s.get(c);
+ }
+ s.get(c);
+ }
+ else
+ {
+ s.clear(ios::failbit);
+ _Warning("Error: Expected '[' while reading matrix");
+ return(s);
+ }
+
+ m.SetSize(size, inMat.Cols());
+ CopyPartialMat(inMat, m, size);
+
+ return(s);
+}
+*/
+
+// --- Matrix Inversion -------------------------------------------------------
+
+#if !defined(CL_CHECKING) && !defined(VL_CHECKING)
+// we #define away pAssertEps if it is not used, to avoid
+// compiler warnings.
+#define pAssertEps
+#endif
+
+Mat inv(const Mat &m, Real *determinant, Real pAssertEps)
+// matrix inversion using Gaussian pivoting
+{
+ Assert(m.IsSquare(), "(inv) Matrix not square");
+
+ Int i, j, k;
+ Int n = m.Rows();
+ Real t, pivot, det;
+ Real max;
+ Mat A(m);
+ Mat B(n, n, vl_I);
+
+ // ---------- Forward elimination ---------- ------------------------------
+
+ det = vl_1;
+
+ for (i = 0; i < n; i++) // Eliminate in column i, below diag
+ {
+ max = -1.0;
+
+ for (k = i; k < n; k++) // Find a pivot for column i
+ if (len(A[k][i]) > max)
+ {
+ max = len(A[k][i]);
+ j = k;
+ }
+
+ Assert(max > pAssertEps, "(inv) Matrix not invertible");
+
+ if (j != i) // Swap rows i and j
+ {
+ for (k = i; k < n; k++)
+ Swap(A.Elt(i, k), A.Elt(j, k));
+ for (k = 0; k < n; k++)
+ Swap(B.Elt(i, k), B.Elt(j, k));
+
+ det = -det;
+ }
+
+ pivot = A.Elt(i, i);
+ Assert(abs(pivot) > pAssertEps, "(inv) Matrix not invertible");
+ det *= pivot;
+
+ for (k = i + 1; k < n; k++) // Only do elements to the right of the pivot
+ A.Elt(i, k) /= pivot;
+
+ for (k = 0; k < n; k++)
+ B.Elt(i, k) /= pivot;
+
+ // We know that A(i, i) will be set to 1, so don't bother to do it
+
+ for (j = i + 1; j < n; j++)
+ { // Eliminate in rows below i
+ t = A.Elt(j, i); // We're gonna zero this guy
+ for (k = i + 1; k < n; k++) // Subtract scaled row i from row j
+ A.Elt(j, k) -= A.Elt(i, k) * t; // (Ignore k <= i, we know they're 0)
+ for (k = 0; k < n; k++)
+ B.Elt(j, k) -= B.Elt(i, k) * t;
+ }
+ }
+
+ // ---------- Backward elimination ---------- -----------------------------
+
+ for (i = n - 1; i > 0; i--) // Eliminate in column i, above diag
+ {
+ for (j = 0; j < i; j++) // Eliminate in rows above i
+ {
+ t = A.Elt(j, i); // We're gonna zero this guy
+ for (k = 0; k < n; k++) // Subtract scaled row i from row j
+ B.Elt(j, k) -= B.Elt(i, k) * t;
+ }
+ }
+ if (determinant)
+ *determinant = det;
+ return(B);
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Mat.h Mon Jan 04 15:19:10 2016 +0000
@@ -0,0 +1,218 @@
+/*
+ File: Mat.h
+
+ Function: Defines a generic resizeable matrix.
+
+ Author(s): Andrew Willmott
+
+ Copyright: (c) 1995-2001, Andrew Willmott
+ */
+
+#ifndef __Mat__
+#define __Mat__
+
+#include "Vec.h"
+#include "Mat2.h"
+#include "Mat3.h"
+#include "Mat4.h"
+
+class Mat2;
+class Mat3;
+class Mat4;
+
+
+// --- Mat Class --------------------------------------------------------------
+
+class Mat
+{
+public:
+
+ // Constructors
+
+ Mat(); // Null matrix
+ Mat(Int rows, Int cols); // Uninitialised matrix
+ Mat(Int rows, Int cols, Double elt0 ...); // Mat(2, 2, 1.0, 2.0, 3.0, 4.0)
+ Mat(Int nrows, Int ncols, Real *ndata); // Create reference matrix
+ Mat(const Mat &m); // Copy constructor
+ Mat(const Mat2 &m); // reference to a Mat2
+ Mat(const Mat3 &m); // reference to a Mat3
+ Mat(const Mat4 &m); // reference to a Mat4
+ Mat(Int rows, Int cols, ZeroOrOne k); // I * k
+ Mat(Int rows, Int cols, Block k); // block matrix (m[i][j] = k)
+
+ ~Mat();
+
+ // Accessor methods
+
+ Int Rows() const { return(rows & VL_REF_MASK); };
+ Int Cols() const { return(cols); };
+
+ Vec operator [] (Int i); // Indexing by row
+ Vec operator [] (Int i) const; // Indexing by row
+
+ Real &Elt(Int i, Int j); // Indexing by elt
+ Real Elt(Int i, Int j) const;
+
+ Void SetSize(Int nrows, Int ncols);
+ Void SetSize(const Mat &m);
+ Bool IsSquare() const
+ { return((rows & VL_REF_MASK) == cols); };
+
+ Real *Ref() const; // Return pointer to data
+
+ // Assignment operators
+
+ Mat &operator = (const Mat &m); // Assignment of a matrix
+ Mat &operator = (ZeroOrOne k); // Set to k * I...
+ Mat &operator = (Block k); // Set to a block matrix...
+ Mat &operator = (const Mat2 &m);
+ Mat &operator = (const Mat3 &m);
+ Mat &operator = (const Mat4 &m);
+
+ // In-Place Operators
+
+ Mat &operator += (const Mat &m);
+ Mat &operator -= (const Mat &m);
+ Mat &operator *= (const Mat &m);
+ Mat &operator *= (Real s);
+ Mat &operator /= (Real s);
+
+ // Matrix initialisers
+
+ Void MakeZero();
+ Void MakeDiag(Real k);
+ Void MakeDiag();
+ Void MakeBlock(Real k);
+ Void MakeBlock();
+
+ Mat &Clamp(Real fuzz);
+ Mat &Clamp();
+
+ // Private...
+
+protected:
+
+ Real *data;
+ UInt rows;
+ UInt cols;
+
+ Bool IsRef() { return((rows & VL_REF_FLAG) != 0); };
+};
+
+
+// --- Mat Comparison Operators -----------------------------------------------
+
+Bool operator == (const Mat &m, const Mat &n);
+Bool operator != (const Mat &m, const Mat &n);
+
+
+// --- Mat Arithmetic Operators -----------------------------------------------
+
+Mat operator + (const Mat &m, const Mat &n);
+Mat operator - (const Mat &m, const Mat &n);
+Mat operator - (const Mat &m);
+Mat operator * (const Mat &m, const Mat &n);
+Mat operator * (const Mat &m, Real s);
+inline Mat operator * (Real s, const Mat &m);
+Mat operator / (const Mat &m, Real s);
+
+Vec operator * (const Mat &m, const Vec &v);
+Vec operator * (const Vec &v, const Mat &m);
+
+Mat trans(const Mat &m); // Transpose
+Real trace(const Mat &m); // Trace
+Mat inv(const Mat &m, Real *determinant = 0, Real pEps = 1e-20);
+ // Inverse
+Mat oprod(const Vec &a, const Vec &b); // Outer product
+
+Mat clamped(const Mat &m, Real fuzz);
+Mat clamped(const Mat &m);
+
+
+// --- Mat Input & Output -----------------------------------------------------
+
+//std::ostream &operator << (std::ostream &s, const Mat &m);
+//std::istream &operator >> (std::istream &s, Mat &m);
+
+void printMat(const Mat &m);
+
+
+// --- Mat Inlines ------------------------------------------------------------
+
+inline Mat::Mat() : data(0), rows(0), cols(0)
+{
+}
+
+inline Mat::Mat(Int rows, Int cols) : rows(rows), cols(cols)
+{
+ Assert(rows > 0 && cols > 0, "(Mat) illegal matrix size");
+
+ data = new Real[rows * cols];
+}
+
+inline Mat::Mat(Int nrows, Int ncols, Real *ndata) :
+ data(ndata), rows(nrows | VL_REF_FLAG), cols(ncols)
+{
+}
+
+inline Vec Mat::operator [] (Int i)
+{
+ CheckRange(i, 0, Rows(), "(Mat::[i]) i index out of range");
+
+ return(Vec(cols, data + i * cols));
+}
+
+inline Vec Mat::operator [] (Int i) const
+{
+ CheckRange(i, 0, Rows(), "(Mat::[i]) i index out of range");
+
+ return(Vec(cols, data + i * cols));
+}
+
+inline Real &Mat::Elt(Int i, Int j)
+{
+ CheckRange(i, 0, Rows(), "(Mat::e(i,j)) i index out of range");
+ CheckRange(j, 0, Cols(), "(Mat::e(i,j)) j index out of range");
+
+ return(data[i * cols + j]);
+}
+
+inline Real Mat::Elt(Int i, Int j) const
+{
+ CheckRange(i, 0, Rows(), "(Mat::e(i,j)) i index out of range");
+ CheckRange(j, 0, Cols(), "(Mat::e(i,j)) j index out of range");
+
+ return(data[i * cols + j]);
+}
+
+inline Real *Mat::Ref() const
+{
+ return(data);
+}
+
+inline Mat operator * (Real s, const Mat &m)
+{
+ return(m * s);
+}
+
+inline Mat &Mat::operator = (ZeroOrOne k)
+{
+ MakeDiag(k);
+
+ return(SELF);
+}
+
+inline Mat &Mat::operator = (Block k)
+{
+ MakeBlock((ZeroOrOne) k);
+
+ return(SELF);
+}
+
+inline Mat::~Mat()
+{
+ if (!IsRef())
+ delete[] data;
+}
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Mat2.cpp Mon Jan 04 15:19:10 2016 +0000
@@ -0,0 +1,146 @@
+/*
+ File: Mat2.cpp
+
+ Function: Implements Mat2.h
+
+ Author(s): Andrew Willmott
+
+ Copyright: (c) 1995-2001, Andrew Willmott
+
+*/
+
+#include "Mat2.h"
+//#include <cctype>
+//#include <iomanip>
+#include "Utils.h"
+
+
+Bool Mat2::operator == (const Mat2 &m) const
+{
+ return(row[0] == m[0] && row[1] == m[1]);
+}
+
+Bool Mat2::operator != (const Mat2 &m) const
+{
+ return(row[0] != m[0] || row[1] != m[1]);
+}
+
+
+Real det(const Mat2 &m)
+{
+ return(dot(m[0], cross(m[1])));
+}
+
+Mat2 inv(const Mat2 &m)
+{
+ Real mDet;
+ Mat2 result;
+
+ result[0][0] = m[1][1]; result[0][1] = -m[0][1];
+ result[1][0] = -m[1][0]; result[1][1] = m[0][0];
+
+ mDet = m[0][0] * result[0][0] + m[0][1] * result[1][0];
+ Assert(mDet != 0.0, "(Mat2::inv) matrix is non-singular");
+ result /= mDet;
+
+ return(result);
+}
+
+Mat2 oprod(const Vec2 &a, const Vec2 &b)
+// returns outerproduct of a and b: a * trans(b)
+{
+ Mat2 result;
+
+ result[0] = a[0] * b;
+ result[1] = a[1] * b;
+
+ return(result);
+}
+
+void printMat2(const Mat2 &m)
+{
+ printf("[");
+ printVec2(m[0]);
+ printf("\r\n");
+ printVec2(m[1]);
+ printf("]\r\n");
+}
+
+/*
+ostream &operator << (ostream &s, const Mat2 &m)
+{
+ Int w = s.width();
+
+ return(s << '[' << m[0] << "\r\n" << setw(w) << m[1] << ']' << "\r\n");
+}
+
+istream &operator >> (istream &s, Mat2 &m)
+{
+ Mat2 result;
+ Char c;
+
+ // Expected format: [[1 2] [3 4]]
+ // Each vector is a row of the row matrix.
+
+ while (s >> c && isspace(c)) // ignore leading white space
+ ;
+
+ if (c == '[')
+ {
+ s >> result[0] >> result[1];
+
+ if (!s)
+ {
+ cerr << "Expected number while reading matrix\n";
+ return(s);
+ }
+
+ while (s >> c && isspace(c))
+ ;
+
+ if (c != ']')
+ {
+ s.clear(ios::failbit);
+ cerr << "Expected ']' while reading matrix\n";
+ return(s);
+ }
+ }
+ else
+ {
+ s.clear(ios::failbit);
+ cerr << "Expected '[' while reading matrix\n";
+ return(s);
+ }
+
+ m = result;
+ return(s);
+}
+*/
+
+
+Mat2 &Mat2::MakeRot(Real theta)
+{
+ Real c, s;
+
+ SetReal(s, sin(theta));
+ SetReal(c, cos(theta));
+
+#ifdef VL_ROW_ORIENT
+ row[0][0] = c; row[0][1] = s;
+ row[1][0] = -s; row[1][1] = c;
+#else
+ row[0][0] = c; row[0][1] = -s;
+ row[1][0] = s; row[1][1] = c;
+#endif
+
+ return(SELF);
+}
+
+Mat2 &Mat2::MakeScale(const Vec2 &s)
+{
+ row[0][0] = s[0]; row[0][1] = vl_0;
+ row[1][0] = vl_0; row[1][1] = s[1];
+
+ return(SELF);
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Mat2.h Mon Jan 04 15:19:10 2016 +0000
@@ -0,0 +1,381 @@
+/*
+ File: Mat2.h
+
+ Function: Defines a 2 x 2 matrix.
+
+ Author(s): Andrew Willmott
+
+ Copyright: (c) 1995-2001, Andrew Willmott
+ */
+
+#ifndef __Mat2__
+#define __Mat2__
+
+#include "Vec2.h"
+
+
+// --- Mat2 Class -------------------------------------------------------------
+
+class Mat2
+{
+public:
+
+ // Constructors
+
+ Mat2();
+ Mat2(Real a, Real b, Real c, Real d); // Create from rows
+ Mat2(const Mat2 &m); // Copy constructor
+ Mat2(ZeroOrOne k);
+ Mat2(Block k);
+
+ // Accessor functions
+
+ Int Rows() const { return(2); };
+ Int Cols() const { return(2); };
+
+ Vec2 &operator [] (Int i);
+ const Vec2 &operator [] (Int i) const;
+
+ Real *Ref() const; // Return pointer to data
+
+ // Assignment operators
+
+ Mat2 &operator = (const Mat2 &m);
+ Mat2 &operator = (ZeroOrOne k);
+ Mat2 &operator = (Block k);
+ Mat2 &operator += (const Mat2 &m);
+ Mat2 &operator -= (const Mat2 &m);
+ Mat2 &operator *= (const Mat2 &m);
+ Mat2 &operator *= (Real s);
+ Mat2 &operator /= (Real s);
+
+ // Comparison operators
+
+ Bool operator == (const Mat2 &m) const; // M == N?
+ Bool operator != (const Mat2 &m) const; // M != N?
+
+ // Arithmetic operators
+
+ Mat2 operator + (const Mat2 &m) const; // M + N
+ Mat2 operator - (const Mat2 &m) const; // M - N
+ Mat2 operator - () const; // -M
+ Mat2 operator * (const Mat2 &m) const; // M * N
+ Mat2 operator * (Real s) const; // M * s
+ Mat2 operator / (Real s) const; // M / s
+
+ // Initialisers
+
+ Void MakeZero(); // Zero matrix
+ Void MakeDiag(Real k = vl_one); // I
+ Void MakeBlock(Real k = vl_one); // all elts=k
+
+ // Vector Transformations
+
+ Mat2& MakeRot(Real theta);
+ Mat2& MakeScale(const Vec2 &s);
+
+ // Private...
+
+protected:
+
+ Vec2 row[2]; // Rows of the matrix
+};
+
+
+// --- Matrix operators -------------------------------------------------------
+
+inline Vec2 &operator *= (Vec2 &v, const Mat2 &m); // v *= m
+inline Vec2 operator * (const Mat2 &m, const Vec2 &v); // m * v
+inline Vec2 operator * (const Vec2 &v, const Mat2 &m); // v * m
+inline Mat2 operator * (Real s, const Mat2 &m); // s * m
+
+inline Mat2 trans(const Mat2 &m); // Transpose
+inline Real trace(const Mat2 &m); // Trace
+inline Mat2 adj(const Mat2 &m); // Adjoint
+Real det(const Mat2 &m); // Determinant
+Mat2 inv(const Mat2 &m); // Inverse
+Mat2 oprod(const Vec2 &a, const Vec2 &b);
+ // Outer product
+
+// The xform functions help avoid dependence on whether row or column
+// vectors are used to represent points and vectors.
+inline Vec2 xform(const Mat2 &m, const Vec2 &v); // Transform of v by m
+inline Mat2 xform(const Mat2 &m, const Mat2 &n); // xform v -> m(n(v))
+
+//std::ostream &operator << (std::ostream &s, const Mat2 &m);
+//std::istream &operator >> (std::istream &s, Mat2 &m);
+
+void printMat2(const Mat2 &m);
+
+
+// --- Inlines ----------------------------------------------------------------
+
+inline Vec2 &Mat2::operator [] (Int i)
+{
+ CheckRange(i, 0, 2, "(Mat2::[i]) index out of range");
+ return(row[i]);
+}
+
+inline const Vec2 &Mat2::operator [] (Int i) const
+{
+ CheckRange(i, 0, 2, "(Mat2::[i]) index out of range");
+ return(row[i]);
+}
+
+inline Real *Mat2::Ref() const
+{
+ return((Real*) row);
+}
+
+inline Mat2::Mat2()
+{
+}
+
+inline Mat2::Mat2(Real a, Real b, Real c, Real d)
+{
+ row[0][0] = a; row[0][1] = b;
+ row[1][0] = c; row[1][1] = d;
+}
+
+inline Mat2::Mat2(const Mat2 &m)
+{
+ row[0] = m[0];
+ row[1] = m[1];
+}
+
+
+inline Void Mat2::MakeZero()
+{
+ row[0][0] = vl_zero; row[0][1] = vl_zero;
+ row[1][0] = vl_zero; row[1][1] = vl_zero;
+}
+
+inline Void Mat2::MakeDiag(Real k)
+{
+ row[0][0] = k; row[0][1] = vl_zero;
+ row[1][0] = vl_zero; row[1][1] = k;
+}
+
+inline Void Mat2::MakeBlock(Real k)
+{
+ row[0][0] = k; row[0][1] = k;
+ row[1][0] = k; row[1][1] = k;
+}
+
+inline Mat2::Mat2(ZeroOrOne k)
+{
+ MakeDiag(k);
+}
+
+inline Mat2::Mat2(Block k)
+{
+ MakeBlock((ZeroOrOne) k);
+}
+
+inline Mat2 &Mat2::operator = (ZeroOrOne k)
+{
+ MakeDiag(k);
+
+ return(SELF);
+}
+
+inline Mat2 &Mat2::operator = (Block k)
+{
+ MakeBlock((ZeroOrOne) k);
+
+ return(SELF);
+}
+
+inline Mat2 &Mat2::operator = (const Mat2 &m)
+{
+ row[0] = m[0];
+ row[1] = m[1];
+
+ return(SELF);
+}
+
+inline Mat2 &Mat2::operator += (const Mat2 &m)
+{
+ row[0] += m[0];
+ row[1] += m[1];
+
+ return(SELF);
+}
+
+inline Mat2 &Mat2::operator -= (const Mat2 &m)
+{
+ row[0] -= m[0];
+ row[1] -= m[1];
+
+ return(SELF);
+}
+
+inline Mat2 &Mat2::operator *= (const Mat2 &m)
+{
+ SELF = SELF * m;
+
+ return(SELF);
+}
+
+inline Mat2 &Mat2::operator *= (Real s)
+{
+ row[0] *= s;
+ row[1] *= s;
+
+ return(SELF);
+}
+
+inline Mat2 &Mat2::operator /= (Real s)
+{
+ row[0] /= s;
+ row[1] /= s;
+
+ return(SELF);
+}
+
+
+inline Mat2 Mat2::operator + (const Mat2 &m) const
+{
+ Mat2 result;
+
+ result[0] = row[0] + m[0];
+ result[1] = row[1] + m[1];
+
+ return(result);
+}
+
+inline Mat2 Mat2::operator - (const Mat2 &m) const
+{
+ Mat2 result;
+
+ result[0] = row[0] - m[0];
+ result[1] = row[1] - m[1];
+
+ return(result);
+}
+
+inline Mat2 Mat2::operator - () const
+{
+ Mat2 result;
+
+ result[0] = -row[0];
+ result[1] = -row[1];
+
+ return(result);
+}
+
+inline Mat2 Mat2::operator * (const Mat2 &m) const
+{
+#define N(x,y) row[x][y]
+#define M(x,y) m.row[x][y]
+#define R(x,y) result[x][y]
+
+ Mat2 result;
+
+ R(0,0) = N(0,0) * M(0,0) + N(0,1) * M(1,0);
+ R(0,1) = N(0,0) * M(0,1) + N(0,1) * M(1,1);
+ R(1,0) = N(1,0) * M(0,0) + N(1,1) * M(1,0);
+ R(1,1) = N(1,0) * M(0,1) + N(1,1) * M(1,1);
+
+ return(result);
+
+#undef N
+#undef M
+#undef R
+}
+
+inline Mat2 Mat2::operator * (Real s) const
+{
+ Mat2 result;
+
+ result[0] = row[0] * s;
+ result[1] = row[1] * s;
+
+ return(result);
+}
+
+inline Mat2 Mat2::operator / (Real s) const
+{
+ Mat2 result;
+
+ result[0] = row[0] / s;
+ result[1] = row[1] / s;
+
+ return(result);
+}
+
+inline Mat2 operator * (Real s, const Mat2 &m)
+{
+ return(m * s);
+}
+
+inline Vec2 operator * (const Mat2 &m, const Vec2 &v)
+{
+ Vec2 result;
+
+ result[0] = m[0][0] * v[0] + m[0][1] * v[1];
+ result[1] = m[1][0] * v[0] + m[1][1] * v[1];
+
+ return(result);
+}
+
+inline Vec2 operator * (const Vec2 &v, const Mat2 &m)
+{
+ Vec2 result;
+
+ result[0] = v[0] * m[0][0] + v[1] * m[1][0];
+ result[1] = v[0] * m[0][1] + v[1] * m[1][1];
+
+ return(result);
+}
+
+inline Vec2 &operator *= (Vec2 &v, const Mat2 &m)
+{
+ Real t;
+
+ t = v[0] * m[0][0] + v[1] * m[1][0];
+ v[1] = v[0] * m[0][1] + v[1] * m[1][1];
+ v[0] = t;
+
+ return(v);
+}
+
+
+inline Mat2 trans(const Mat2 &m)
+{
+ Mat2 result;
+
+ result[0][0] = m[0][0]; result[0][1] = m[1][0];
+ result[1][0] = m[0][1]; result[1][1] = m[1][1];
+
+ return(result);
+}
+
+inline Real trace(const Mat2 &m)
+{
+ return(m[0][0] + m[1][1]);
+}
+
+inline Mat2 adj(const Mat2 &m)
+{
+ Mat2 result;
+
+ result[0] = cross(m[1]);
+ result[1] = -cross(m[0]);
+
+ return(result);
+}
+
+#ifdef VL_ROW_ORIENT
+inline Vec2 xform(const Mat2 &m, const Vec2 &v)
+{ return(v * m); }
+inline Mat2 xform(const Mat2 &m, const Mat2 &n)
+{ return(n * m); }
+#else
+inline Vec2 xform(const Mat2 &m, const Vec2 &v)
+{ return(m * v); }
+inline Mat2 xform(const Mat2 &m, const Mat2 &n)
+{ return(m * n); }
+#endif
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Mat3.cpp Mon Jan 04 15:19:10 2016 +0000
@@ -0,0 +1,442 @@
+/*
+ File: Mat3.cpp
+
+ Function: Implements Mat3.h
+
+ Author(s): Andrew Willmott
+
+ Copyright: (c) 1995-2001, Andrew Willmott
+
+*/
+#include "Mat3.h"
+#include "Vec4.h"
+//#include <cctype>
+//#include <iomanip>
+
+
+Mat3::Mat3(Real a, Real b, Real c,
+ Real d, Real e, Real f,
+ Real g, Real h, Real i)
+{
+ row[0][0] = a; row[0][1] = b; row[0][2] = c;
+ row[1][0] = d; row[1][1] = e; row[1][2] = f;
+ row[2][0] = g; row[2][1] = h; row[2][2] = i;
+}
+
+Mat3::Mat3(const Mat3 &m)
+{
+ row[0] = m[0];
+ row[1] = m[1];
+ row[2] = m[2];
+}
+
+Mat3 &Mat3::operator = (const Mat3 &m)
+{
+ row[0] = m[0];
+ row[1] = m[1];
+ row[2] = m[2];
+
+ return(SELF);
+}
+
+Mat3 &Mat3::operator += (const Mat3 &m)
+{
+ row[0] += m[0];
+ row[1] += m[1];
+ row[2] += m[2];
+
+ return(SELF);
+}
+
+Mat3 &Mat3::operator -= (const Mat3 &m)
+{
+ row[0] -= m[0];
+ row[1] -= m[1];
+ row[2] -= m[2];
+
+ return(SELF);
+}
+
+Mat3 &Mat3::operator *= (const Mat3 &m)
+{
+ SELF = SELF * m;
+
+ return(SELF);
+}
+
+Mat3 &Mat3::operator *= (Real s)
+{
+ row[0] *= s;
+ row[1] *= s;
+ row[2] *= s;
+
+ return(SELF);
+}
+
+Mat3 &Mat3::operator /= (Real s)
+{
+ row[0] /= s;
+ row[1] /= s;
+ row[2] /= s;
+
+ return(SELF);
+}
+
+
+Bool Mat3::operator == (const Mat3 &m) const
+{
+ return(row[0] == m[0] && row[1] == m[1] && row[2] == m[2]);
+}
+
+Bool Mat3::operator != (const Mat3 &m) const
+{
+ return(row[0] != m[0] || row[1] != m[1] || row[2] != m[2]);
+}
+
+
+Mat3 Mat3::operator + (const Mat3 &m) const
+{
+ Mat3 result;
+
+ result[0] = row[0] + m[0];
+ result[1] = row[1] + m[1];
+ result[2] = row[2] + m[2];
+
+ return(result);
+}
+
+Mat3 Mat3::operator - (const Mat3 &m) const
+{
+ Mat3 result;
+
+ result[0] = row[0] - m[0];
+ result[1] = row[1] - m[1];
+ result[2] = row[2] - m[2];
+
+ return(result);
+}
+
+Mat3 Mat3::operator - () const
+{
+ Mat3 result;
+
+ result[0] = -row[0];
+ result[1] = -row[1];
+ result[2] = -row[2];
+
+ return(result);
+}
+
+Mat3 Mat3::operator * (const Mat3 &m) const
+{
+#define N(x,y) row[x][y]
+#define M(x,y) m[x][y]
+#define R(x,y) result[x][y]
+
+ Mat3 result;
+
+ R(0,0) = N(0,0) * M(0,0) + N(0,1) * M(1,0) + N(0,2) * M(2,0);
+ R(0,1) = N(0,0) * M(0,1) + N(0,1) * M(1,1) + N(0,2) * M(2,1);
+ R(0,2) = N(0,0) * M(0,2) + N(0,1) * M(1,2) + N(0,2) * M(2,2);
+
+ R(1,0) = N(1,0) * M(0,0) + N(1,1) * M(1,0) + N(1,2) * M(2,0);
+ R(1,1) = N(1,0) * M(0,1) + N(1,1) * M(1,1) + N(1,2) * M(2,1);
+ R(1,2) = N(1,0) * M(0,2) + N(1,1) * M(1,2) + N(1,2) * M(2,2);
+
+ R(2,0) = N(2,0) * M(0,0) + N(2,1) * M(1,0) + N(2,2) * M(2,0);
+ R(2,1) = N(2,0) * M(0,1) + N(2,1) * M(1,1) + N(2,2) * M(2,1);
+ R(2,2) = N(2,0) * M(0,2) + N(2,1) * M(1,2) + N(2,2) * M(2,2);
+
+ return(result);
+
+#undef N
+#undef M
+#undef R
+}
+
+Mat3 Mat3::operator * (Real s) const
+{
+ Mat3 result;
+
+ result[0] = row[0] * s;
+ result[1] = row[1] * s;
+ result[2] = row[2] * s;
+
+ return(result);
+}
+
+Mat3 Mat3::operator / (Real s) const
+{
+ Mat3 result;
+
+ result[0] = row[0] / s;
+ result[1] = row[1] / s;
+ result[2] = row[2] / s;
+
+ return(result);
+}
+
+Mat3 trans(const Mat3 &m)
+{
+#define M(x,y) m[x][y]
+#define R(x,y) result[x][y]
+
+ Mat3 result;
+
+ R(0,0) = M(0,0); R(0,1) = M(1,0); R(0,2) = M(2,0);
+ R(1,0) = M(0,1); R(1,1) = M(1,1); R(1,2) = M(2,1);
+ R(2,0) = M(0,2); R(2,1) = M(1,2); R(2,2) = M(2,2);
+
+ return(result);
+
+#undef M
+#undef R
+}
+
+Mat3 adj(const Mat3 &m)
+{
+ Mat3 result;
+
+ result[0] = cross(m[1], m[2]);
+ result[1] = cross(m[2], m[0]);
+ result[2] = cross(m[0], m[1]);
+
+ return(result);
+}
+
+
+Real trace(const Mat3 &m)
+{
+ return(m[0][0] + m[1][1] + m[2][2]);
+}
+
+Real det(const Mat3 &m)
+{
+ return(dot(m[0], cross(m[1], m[2])));
+}
+
+Mat3 inv(const Mat3 &m)
+{
+ Real mDet;
+ Mat3 adjoint;
+ Mat3 result;
+
+ adjoint = adj(m);
+ mDet = dot(adjoint[0], m[0]);
+
+ Assert(mDet != 0, "(Mat3::inv) matrix is non-singular");
+
+ result = trans(adjoint);
+ result /= mDet;
+
+ return(result);
+}
+
+Mat3 oprod(const Vec3 &a, const Vec3 &b)
+// returns outerproduct of a and b: a * trans(b)
+{
+ Mat3 result;
+
+ result[0] = a[0] * b;
+ result[1] = a[1] * b;
+ result[2] = a[2] * b;
+
+ return(result);
+}
+
+Void Mat3::MakeZero()
+{
+ Int i;
+
+ for (i = 0; i < 9; i++)
+ ((Real*) row)[i] = vl_zero;
+}
+
+Void Mat3::MakeDiag(Real k)
+{
+ Int i, j;
+
+ for (i = 0; i < 3; i++)
+ for (j = 0; j < 3; j++)
+ if (i == j)
+ row[i][j] = k;
+ else
+ row[i][j] = vl_zero;
+}
+
+Void Mat3::MakeBlock(Real k)
+{
+ Int i;
+
+ for (i = 0; i < 9; i++)
+ ((Real *) row)[i] = k;
+}
+
+void printMat3(const Mat3 &m)
+{
+ printf("[");
+ printVec3(m[0]);
+ printf("\r\n");
+ printVec3(m[1]);
+ printf("\r\n");
+ printVec3(m[2]);
+ printf("]\r\n");
+}
+
+/*
+ostream &operator << (ostream &s, const Mat3 &m)
+{
+ Int w = s.width();
+
+ return(s << '[' << m[0] << "\r\n" << setw(w) << m[1] << "\r\n" << setw(w)
+ << m[2] << ']' << "\r\n");
+}
+
+istream &operator >> (istream &s, Mat3 &m)
+{
+ Mat3 result;
+ Char c;
+
+ // Expected format: [[1 2 3] [4 5 6] [7 8 9]]
+ // Each vector is a column of the matrix.
+
+ while (s >> c && isspace(c)) // ignore leading white space
+ ;
+
+ if (c == '[')
+ {
+ s >> result[0] >> result[1] >> result[2];
+
+ if (!s)
+ {
+ cerr << "Expected number while reading matrix\n";
+ return(s);
+ }
+
+ while (s >> c && isspace(c))
+ ;
+
+ if (c != ']')
+ {
+ s.clear(ios::failbit);
+ cerr << "Expected ']' while reading matrix\n";
+ return(s);
+ }
+ }
+ else
+ {
+ s.clear(ios::failbit);
+ cerr << "Expected '[' while reading matrix\n";
+ return(s);
+ }
+
+ m = result;
+ return(s);
+}
+*/
+
+
+Mat3 &Mat3::MakeRot(const Vec4 &q)
+// modify to the new quat format [q0, (q1,q2,q3)]
+{
+ Real i2 = 2 * q[1],
+ j2 = 2 * q[2],
+ k2 = 2 * q[3],
+ ij = i2 * q[2],
+ ik = i2 * q[3],
+ jk = j2 * q[3],
+ ri = i2 * q[0],
+ rj = j2 * q[0],
+ rk = k2 * q[0];
+
+ i2 *= q[1];
+ j2 *= q[2];
+ k2 *= q[3];
+
+#if VL_ROW_ORIENT // off by default
+ row[0][0] = 1 - j2 - k2; row[0][1] = ij + rk ; row[0][2] = ik - rj;
+ row[1][0] = ij - rk ; row[1][1] = 1 - i2 - k2; row[1][2] = jk + ri;
+ row[2][0] = ik + rj ; row[2][1] = jk - ri ; row[2][2] = 1 - i2 - j2;
+#else
+ row[0][0] = 1 - j2 - k2; row[0][1] = ij - rk ; row[0][2] = ik + rj;
+ row[1][0] = ij + rk ; row[1][1] = 1 - i2 - k2; row[1][2] = jk - ri;
+ row[2][0] = ik - rj ; row[2][1] = jk + ri ; row[2][2] = 1 - i2 - j2;
+#endif
+
+ return(SELF);
+}
+
+Mat3 &Mat3::MakeRot(const Vec3 &axis, Real theta)
+// modify to the new quat format [q0,(q1,q2,q3)]
+{
+ Real s;
+ Vec4 q;
+
+ theta /= 2.0;
+ s = sin(theta);
+
+ q[1] = s * axis[0];
+ q[2] = s * axis[1];
+ q[3] = s * axis[2];
+ q[0] = cos(theta);
+
+ MakeRot(q);
+
+ return(SELF);
+}
+
+Mat3 &Mat3::MakeScale(const Vec3 &s)
+{
+ MakeZero();
+
+ row[0][0] = s[0];
+ row[1][1] = s[1];
+ row[2][2] = s[2];
+
+ return(SELF);
+}
+
+Mat3 &Mat3::MakeHRot(Real theta)
+{
+ Real c, s;
+
+ MakeDiag();
+
+ s = sin(theta);
+ c = cos(theta);
+
+#ifdef VL_ROW_ORIENT
+ row[0][0] = c; row[0][1] = s;
+ row[1][0] = -s; row[1][1] = c;
+#else
+ row[0][0] = c; row[0][1] = -s;
+ row[1][0] = s; row[1][1] = c;
+#endif
+
+ return(SELF);
+}
+
+Mat3 &Mat3::MakeHScale(const Vec2 &s)
+{
+ MakeDiag();
+
+ row[0][0] = s[0];
+ row[1][1] = s[1];
+
+ return(SELF);
+}
+
+Mat3 &Mat3::MakeHTrans(const Vec2 &t)
+{
+ MakeDiag();
+
+#ifdef VL_ROW_ORIENT
+ row[2][0] = t[0];
+ row[2][1] = t[1];
+#else
+ row[0][2] = t[0];
+ row[1][2] = t[1];
+#endif
+
+ return(SELF);
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Mat3.h Mon Jan 04 15:19:10 2016 +0000
@@ -0,0 +1,226 @@
+/*
+ File: Mat3.h
+
+ Function: Defines a 3 x 3 matrix.
+
+ Author(s): Andrew Willmott
+
+ Copyright: (c) 1995-2001, Andrew Willmott
+*/
+
+#ifndef __Mat3__
+#define __Mat3__
+
+#include "Vec3.h"
+
+
+// --- Mat3 Class -------------------------------------------------------------
+
+
+class Vec4;
+
+class Mat3
+{
+public:
+
+ // Constructors
+
+ Mat3();
+ Mat3(Real a, Real b, Real c,
+ Real d, Real e, Real f,
+ Real g, Real h, Real i);
+ Mat3(const Mat3 &m);
+ Mat3(ZeroOrOne k);
+ Mat3(Block k);
+
+ // Accessor functions
+
+ Int Rows() const { return(3); };
+ Int Cols() const { return(3); };
+
+ Vec3 &operator [] (Int i);
+ const Vec3 &operator [] (Int i) const;
+
+ Real *Ref() const; // Return pointer to data
+
+ // Assignment operators
+
+ Mat3 &operator = (const Mat3 &m);
+ Mat3 &operator = (ZeroOrOne k);
+ Mat3 &operator = (Block k);
+ Mat3 &operator += (const Mat3 &m);
+ Mat3 &operator -= (const Mat3 &m);
+ Mat3 &operator *= (const Mat3 &m);
+ Mat3 &operator *= (Real s);
+ Mat3 &operator /= (Real s);
+
+ // Comparison operators
+
+ Bool operator == (const Mat3 &m) const; // M == N?
+ Bool operator != (const Mat3 &m) const; // M != N?
+
+ // Arithmetic operators
+
+ Mat3 operator + (const Mat3 &m) const; // M + N
+ Mat3 operator - (const Mat3 &m) const; // M - N
+ Mat3 operator - () const; // -M
+ Mat3 operator * (const Mat3 &m) const; // M * N
+ Mat3 operator * (Real s) const; // M * s
+ Mat3 operator / (Real s) const; // M / s
+
+ // Initialisers
+
+ Void MakeZero(); // Zero matrix
+ Void MakeDiag(Real k = vl_one); // I
+ Void MakeBlock(Real k = vl_one); // all elts = k
+
+ // Vector Transforms
+
+ Mat3& MakeRot(const Vec3 &axis, Real theta);
+ Mat3& MakeRot(const Vec4 &q); // Rotate by quaternion
+ Mat3& MakeScale(const Vec3 &s);
+
+ // Homogeneous Transforms
+
+ Mat3& MakeHRot(Real theta); // Rotate by theta rads
+ Mat3& MakeHScale(const Vec2 &s); // Scale by s
+ Mat3& MakeHTrans(const Vec2 &t); // Translation by t
+
+ // Private...
+
+protected:
+
+ Vec3 row[3];
+};
+
+
+// --- Matrix operators -------------------------------------------------------
+
+inline Vec3 &operator *= (Vec3 &v, const Mat3 &m); // v *= m
+inline Vec3 operator * (const Mat3 &m, const Vec3 &v); // m * v
+inline Vec3 operator * (const Vec3 &v, const Mat3 &m); // v * m
+inline Mat3 operator * (const Real s, const Mat3 &m); // s * m
+
+Mat3 trans(const Mat3 &m); // Transpose
+Real trace(const Mat3 &m); // Trace
+Mat3 adj(const Mat3 &m); // Adjoint
+Real det(const Mat3 &m); // Determinant
+Mat3 inv(const Mat3 &m); // Inverse
+Mat3 oprod(const Vec3 &a, const Vec3 &b); // Outer product
+
+// The xform functions help avoid dependence on whether row or column
+// vectors are used to represent points and vectors.
+inline Vec3 xform(const Mat3 &m, const Vec3 &v); // Transform of v by m
+inline Vec2 xform(const Mat3 &m, const Vec2 &v); // Hom. xform of v by m
+inline Mat3 xform(const Mat3 &m, const Mat3 &n); // Xform v -> m(n(v))
+
+//std::ostream &operator << (std::ostream &s, const Mat3 &m);
+//std::istream &operator >> (std::istream &s, Mat3 &m);
+
+void printMat3(const Mat3 &m);
+
+
+// --- Inlines ----------------------------------------------------------------
+
+inline Mat3::Mat3()
+{
+}
+
+inline Vec3 &Mat3::operator [] (Int i)
+{
+ CheckRange(i, 0, 3, "(Mat3::[i]) index out of range");
+ return(row[i]);
+}
+
+inline const Vec3 &Mat3::operator [] (Int i) const
+{
+ CheckRange(i, 0, 3, "(Mat3::[i]) index out of range");
+ return(row[i]);
+}
+
+inline Real *Mat3::Ref() const
+{
+ return((Real *) row);
+}
+
+inline Mat3::Mat3(ZeroOrOne k)
+{
+ MakeDiag(k);
+}
+
+inline Mat3::Mat3(Block k)
+{
+ MakeBlock((ZeroOrOne) k);
+}
+
+inline Mat3 &Mat3::operator = (ZeroOrOne k)
+{
+ MakeDiag(k);
+
+ return(SELF);
+}
+
+inline Mat3 &Mat3::operator = (Block k)
+{
+ MakeBlock((ZeroOrOne) k);
+
+ return(SELF);
+}
+
+inline Mat3 operator * (const Real s, const Mat3 &m)
+{
+ return(m * s);
+}
+
+inline Vec3 operator * (const Mat3 &m, const Vec3 &v)
+{
+ Vec3 result;
+
+ result[0] = v[0] * m[0][0] + v[1] * m[0][1] + v[2] * m[0][2];
+ result[1] = v[0] * m[1][0] + v[1] * m[1][1] + v[2] * m[1][2];
+ result[2] = v[0] * m[2][0] + v[1] * m[2][1] + v[2] * m[2][2];
+
+ return(result);
+}
+
+inline Vec3 operator * (const Vec3 &v, const Mat3 &m)
+{
+ Vec3 result;
+
+ result[0] = v[0] * m[0][0] + v[1] * m[1][0] + v[2] * m[2][0];
+ result[1] = v[0] * m[0][1] + v[1] * m[1][1] + v[2] * m[2][1];
+ result[2] = v[0] * m[0][2] + v[1] * m[1][2] + v[2] * m[2][2];
+
+ return(result);
+}
+
+inline Vec3 &operator *= (Vec3 &v, const Mat3 &m)
+{
+ Real t0, t1;
+
+ t0 = v[0] * m[0][0] + v[1] * m[1][0] + v[2] * m[2][0];
+ t1 = v[0] * m[0][1] + v[1] * m[1][1] + v[2] * m[2][1];
+ v[2] = v[0] * m[0][2] + v[1] * m[1][2] + v[2] * m[2][2];
+ v[0] = t0;
+ v[1] = t1;
+
+ return(v);
+}
+
+#ifdef VL_ROW_ORIENT
+inline Vec2 xform(const Mat3 &m, const Vec2 &v)
+{ return(proj(Vec3(v, 1.0) * m)); }
+inline Vec3 xform(const Mat3 &m, const Vec3 &v)
+{ return(v * m); }
+inline Mat3 xform(const Mat3 &m, const Mat3 &n)
+{ return(n * m); }
+#else
+inline Vec2 xform(const Mat3 &m, const Vec2 &v)
+{ return(proj(m * Vec3(v, 1.0))); }
+inline Vec3 xform(const Mat3 &m, const Vec3 &v)
+{ return(m * v); }
+inline Mat3 xform(const Mat3 &m, const Mat3 &n)
+{ return(m * n); }
+#endif
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Mat4.cpp Mon Jan 04 15:19:10 2016 +0000
@@ -0,0 +1,507 @@
+/*
+ File: Mat4.cpp
+
+ Function: Implements Mat4.h
+
+ Author(s): Andrew Willmott
+
+ Copyright: (c) 1995-2001, Andrew Willmott
+
+*/
+
+#include "Mat4.h"
+//#include <cctype>
+//#include <iomanip>
+
+
+Mat4::Mat4(Real a, Real b, Real c, Real d,
+ Real e, Real f, Real g, Real h,
+ Real i, Real j, Real k, Real l,
+ Real m, Real n, Real o, Real p)
+{
+ row[0][0] = a; row[0][1] = b; row[0][2] = c; row[0][3] = d;
+ row[1][0] = e; row[1][1] = f; row[1][2] = g; row[1][3] = h;
+ row[2][0] = i; row[2][1] = j; row[2][2] = k; row[2][3] = l;
+ row[3][0] = m; row[3][1] = n; row[3][2] = o; row[3][3] = p;
+}
+
+Mat4::Mat4(const Mat4 &m)
+{
+ row[0] = m[0];
+ row[1] = m[1];
+ row[2] = m[2];
+ row[3] = m[3];
+}
+
+
+Mat4 &Mat4::operator = (const Mat4 &m)
+{
+ row[0] = m[0];
+ row[1] = m[1];
+ row[2] = m[2];
+ row[3] = m[3];
+
+ return(SELF);
+}
+
+Mat4 &Mat4::operator += (const Mat4 &m)
+{
+ row[0] += m[0];
+ row[1] += m[1];
+ row[2] += m[2];
+ row[3] += m[3];
+
+ return(SELF);
+}
+
+Mat4 &Mat4::operator -= (const Mat4 &m)
+{
+ row[0] -= m[0];
+ row[1] -= m[1];
+ row[2] -= m[2];
+ row[3] -= m[3];
+
+ return(SELF);
+}
+
+Mat4 &Mat4::operator *= (const Mat4 &m)
+{
+ SELF = SELF * m;
+
+ return(SELF);
+}
+
+Mat4 &Mat4::operator *= (Real s)
+{
+ row[0] *= s;
+ row[1] *= s;
+ row[2] *= s;
+ row[3] *= s;
+
+ return(SELF);
+}
+
+Mat4 &Mat4::operator /= (Real s)
+{
+ row[0] /= s;
+ row[1] /= s;
+ row[2] /= s;
+ row[3] /= s;
+
+ return(SELF);
+}
+
+
+Bool Mat4::operator == (const Mat4 &m) const
+{
+ return(row[0] == m[0] && row[1] == m[1] && row[2] == m[2] && row[3] == m[3]);
+}
+
+Bool Mat4::operator != (const Mat4 &m) const
+{
+ return(row[0] != m[0] || row[1] != m[1] || row[2] != m[2] || row[3] != m[3]);
+}
+
+
+Mat4 Mat4::operator + (const Mat4 &m) const
+{
+ Mat4 result;
+
+ result[0] = row[0] + m[0];
+ result[1] = row[1] + m[1];
+ result[2] = row[2] + m[2];
+ result[3] = row[3] + m[3];
+
+ return(result);
+}
+
+Mat4 Mat4::operator - (const Mat4 &m) const
+{
+ Mat4 result;
+
+ result[0] = row[0] - m[0];
+ result[1] = row[1] - m[1];
+ result[2] = row[2] - m[2];
+ result[3] = row[3] - m[3];
+
+ return(result);
+}
+
+Mat4 Mat4::operator - () const
+{
+ Mat4 result;
+
+ result[0] = -row[0];
+ result[1] = -row[1];
+ result[2] = -row[2];
+ result[3] = -row[3];
+
+ return(result);
+}
+
+Mat4 Mat4::operator * (const Mat4 &m) const
+{
+#define N(x,y) row[x][y]
+#define M(x,y) m[x][y]
+#define R(x,y) result[x][y]
+
+ Mat4 result;
+
+ R(0,0) = N(0,0) * M(0,0) + N(0,1) * M(1,0) + N(0,2) * M(2,0) + N(0,3) * M(3,0);
+ R(0,1) = N(0,0) * M(0,1) + N(0,1) * M(1,1) + N(0,2) * M(2,1) + N(0,3) * M(3,1);
+ R(0,2) = N(0,0) * M(0,2) + N(0,1) * M(1,2) + N(0,2) * M(2,2) + N(0,3) * M(3,2);
+ R(0,3) = N(0,0) * M(0,3) + N(0,1) * M(1,3) + N(0,2) * M(2,3) + N(0,3) * M(3,3);
+
+ R(1,0) = N(1,0) * M(0,0) + N(1,1) * M(1,0) + N(1,2) * M(2,0) + N(1,3) * M(3,0);
+ R(1,1) = N(1,0) * M(0,1) + N(1,1) * M(1,1) + N(1,2) * M(2,1) + N(1,3) * M(3,1);
+ R(1,2) = N(1,0) * M(0,2) + N(1,1) * M(1,2) + N(1,2) * M(2,2) + N(1,3) * M(3,2);
+ R(1,3) = N(1,0) * M(0,3) + N(1,1) * M(1,3) + N(1,2) * M(2,3) + N(1,3) * M(3,3);
+
+ R(2,0) = N(2,0) * M(0,0) + N(2,1) * M(1,0) + N(2,2) * M(2,0) + N(2,3) * M(3,0);
+ R(2,1) = N(2,0) * M(0,1) + N(2,1) * M(1,1) + N(2,2) * M(2,1) + N(2,3) * M(3,1);
+ R(2,2) = N(2,0) * M(0,2) + N(2,1) * M(1,2) + N(2,2) * M(2,2) + N(2,3) * M(3,2);
+ R(2,3) = N(2,0) * M(0,3) + N(2,1) * M(1,3) + N(2,2) * M(2,3) + N(2,3) * M(3,3);
+
+ R(3,0) = N(3,0) * M(0,0) + N(3,1) * M(1,0) + N(3,2) * M(2,0) + N(3,3) * M(3,0);
+ R(3,1) = N(3,0) * M(0,1) + N(3,1) * M(1,1) + N(3,2) * M(2,1) + N(3,3) * M(3,1);
+ R(3,2) = N(3,0) * M(0,2) + N(3,1) * M(1,2) + N(3,2) * M(2,2) + N(3,3) * M(3,2);
+ R(3,3) = N(3,0) * M(0,3) + N(3,1) * M(1,3) + N(3,2) * M(2,3) + N(3,3) * M(3,3);
+
+ return(result);
+
+#undef N
+#undef M
+#undef R
+}
+
+Mat4 Mat4::operator * (Real s) const
+{
+ Mat4 result;
+
+ result[0] = row[0] * s;
+ result[1] = row[1] * s;
+ result[2] = row[2] * s;
+ result[3] = row[3] * s;
+
+ return(result);
+}
+
+Mat4 Mat4::operator / (Real s) const
+{
+ Mat4 result;
+
+ result[0] = row[0] / s;
+ result[1] = row[1] / s;
+ result[2] = row[2] / s;
+ result[3] = row[3] / s;
+
+ return(result);
+}
+
+Vec4 operator * (const Mat4 &m, const Vec4 &v) // m * v
+{
+ Vec4 result;
+
+ result[0] = v[0] * m[0][0] + v[1] * m[0][1] + v[2] * m[0][2] + v[3] * m[0][3];
+ result[1] = v[0] * m[1][0] + v[1] * m[1][1] + v[2] * m[1][2] + v[3] * m[1][3];
+ result[2] = v[0] * m[2][0] + v[1] * m[2][1] + v[2] * m[2][2] + v[3] * m[2][3];
+ result[3] = v[0] * m[3][0] + v[1] * m[3][1] + v[2] * m[3][2] + v[3] * m[3][3];
+
+ return(result);
+}
+
+Vec4 operator * (const Vec4 &v, const Mat4 &m) // v * m
+{
+ Vec4 result;
+
+ result[0] = v[0] * m[0][0] + v[1] * m[1][0] + v[2] * m[2][0] + v[3] * m[3][0];
+ result[1] = v[0] * m[0][1] + v[1] * m[1][1] + v[2] * m[2][1] + v[3] * m[3][1];
+ result[2] = v[0] * m[0][2] + v[1] * m[1][2] + v[2] * m[2][2] + v[3] * m[3][2];
+ result[3] = v[0] * m[0][3] + v[1] * m[1][3] + v[2] * m[2][3] + v[3] * m[3][3];
+
+ return(result);
+}
+
+Vec4 &operator *= (Vec4 &v, const Mat4 &m) // v *= m
+{
+ Real t0, t1, t2;
+
+ t0 = v[0] * m[0][0] + v[1] * m[1][0] + v[2] * m[2][0] + v[3] * m[3][0];
+ t1 = v[0] * m[0][1] + v[1] * m[1][1] + v[2] * m[2][1] + v[3] * m[3][1];
+ t2 = v[0] * m[0][2] + v[1] * m[1][2] + v[2] * m[2][2] + v[3] * m[3][2];
+ v[3] = v[0] * m[0][3] + v[1] * m[1][3] + v[2] * m[2][3] + v[3] * m[3][3];
+ v[0] = t0;
+ v[1] = t1;
+ v[2] = t2;
+
+ return(v);
+}
+
+Mat4 trans(const Mat4 &m)
+{
+#define M(x,y) m[x][y]
+#define R(x,y) result[x][y]
+
+ Mat4 result;
+
+ R(0,0) = M(0,0); R(0,1) = M(1,0); R(0,2) = M(2,0); R(0,3) = M(3,0);
+ R(1,0) = M(0,1); R(1,1) = M(1,1); R(1,2) = M(2,1); R(1,3) = M(3,1);
+ R(2,0) = M(0,2); R(2,1) = M(1,2); R(2,2) = M(2,2); R(2,3) = M(3,2);
+ R(3,0) = M(0,3); R(3,1) = M(1,3); R(3,2) = M(2,3); R(3,3) = M(3,3);
+
+ return(result);
+
+#undef M
+#undef R
+}
+
+Mat4 adj(const Mat4 &m)
+{
+ Mat4 result;
+
+ result[0] = cross(m[1], m[2], m[3]);
+ result[1] = -cross(m[0], m[2], m[3]);
+ result[2] = cross(m[0], m[1], m[3]);
+ result[3] = -cross(m[0], m[1], m[2]);
+
+ return(result);
+}
+
+Real trace(const Mat4 &m)
+{
+ return(m[0][0] + m[1][1] + m[2][2] + m[3][3]);
+}
+
+Real det(const Mat4 &m)
+{
+ return(dot(m[0], cross(m[1], m[2], m[3])));
+}
+
+Mat4 inv(const Mat4 &m)
+{
+ Real mDet;
+ Mat4 adjoint;
+ Mat4 result;
+
+ adjoint = adj(m); // Find the adjoint
+ mDet = dot(adjoint[0], m[0]);
+
+ Assert(mDet != 0, "(Mat4::inv) matrix is non-singular");
+
+ result = trans(adjoint);
+ result /= mDet;
+
+ return(result);
+}
+
+Mat4 oprod(const Vec4 &a, const Vec4 &b)
+// returns outerproduct of a and b: a * trans(b)
+{
+ Mat4 result;
+
+ result[0] = a[0] * b;
+ result[1] = a[1] * b;
+ result[2] = a[2] * b;
+ result[3] = a[3] * b;
+
+ return(result);
+}
+
+Void Mat4::MakeZero()
+{
+ Int i;
+
+ for (i = 0; i < 16; i++)
+ ((Real *) row)[i] = vl_zero;
+}
+
+Void Mat4::MakeDiag(Real k)
+// default argument is vl_one
+{
+ Int i, j;
+
+ for (i = 0; i < 4; i++)
+ for (j = 0; j < 4; j++)
+ if (i == j)
+ row[i][j] = k;
+ else
+ row[i][j] = vl_zero;
+}
+
+Void Mat4::MakeBlock(Real k)
+{
+ Int i;
+
+ for (i = 0; i < 16; i++) {
+ //printf("i = %d k = %f\r\n",i,k);
+ ((Real *) row)[i] = k;
+ }
+}
+
+void printMat4(const Mat4 &m)
+{
+ printf("[");
+ printVec4(m[0]);
+ printf("\r\n");
+ printVec4(m[1]);
+ printf("\r\n");
+ printVec4(m[2]);
+ printf("\r\n");
+ printVec4(m[3]);
+ printf("]\r\n");
+}
+
+/*
+ostream &operator << (ostream &s, const Mat4 &m)
+{
+ Int w = s.width();
+
+ return(s << '[' << m[0] << "\r\n" << setw(w) << m[1] << "\r\n"
+ << setw(w) << m[2] << "\r\n" << setw(w) << m[3] << ']' << "\r\n");
+}
+
+istream &operator >> (istream &s, Mat4 &m)
+{
+ Mat4 result;
+ Char c;
+
+ // Expected format: [[1 2 3] [4 5 6] [7 8 9]]
+ // Each vector is a column of the matrix.
+
+ while (s >> c && isspace(c)) // ignore leading white space
+ ;
+
+ if (c == '[')
+ {
+ s >> result[0] >> result[1] >> result[2] >> result[3];
+
+ if (!s)
+ {
+ cerr << "Expected number while reading matrix\n";
+ return(s);
+ }
+
+ while (s >> c && isspace(c))
+ ;
+
+ if (c != ']')
+ {
+ s.clear(ios::failbit);
+ cerr << "Expected ']' while reading matrix\n";
+ return(s);
+ }
+ }
+ else
+ {
+ s.clear(ios::failbit);
+ cerr << "Expected '[' while reading matrix\n";
+ return(s);
+ }
+
+ m = result;
+ return(s);
+}
+*/
+
+
+Mat4& Mat4::MakeHRot(const Vec4 &q)
+{
+ Real i2 = 2 * q[1],
+ j2 = 2 * q[2],
+ k2 = 2 * q[3],
+ ij = i2 * q[2],
+ ik = i2 * q[3],
+ jk = j2 * q[3],
+ ri = i2 * q[0],
+ rj = j2 * q[0],
+ rk = k2 * q[0];
+
+ MakeDiag();
+
+ i2 *= q[1];
+ j2 *= q[2];
+ k2 *= q[3];
+
+#if VL_ROW_ORIENT
+ row[0][0] = 1 - j2 - k2; row[0][1] = ij + rk ; row[0][2] = ik - rj;
+ row[1][0] = ij - rk ; row[1][1] = 1 - i2- k2; row[1][2] = jk + ri;
+ row[2][0] = ik + rj ; row[2][1] = jk - ri ; row[2][2] = 1 - i2 - j2;
+#else
+ row[0][0] = 1 - j2 - k2; row[0][1] = ij - rk ; row[0][2] = ik + rj;
+ row[1][0] = ij + rk ; row[1][1] = 1 - i2- k2; row[1][2] = jk - ri;
+ row[2][0] = ik - rj ; row[2][1] = jk + ri ; row[2][2] = 1 - i2 - j2;
+#endif
+
+ return(SELF);
+}
+
+Mat4& Mat4::MakeHRot(const Vec3 &axis, Real theta)
+{
+ Real s;
+ Vec4 q;
+
+ theta /= 2.0;
+ s = sin(theta);
+
+ q[1] = s * axis[0];
+ q[2] = s * axis[1];
+ q[3] = s * axis[2];
+ q[0] = cos(theta);
+
+ MakeHRot(q);
+
+ return(SELF);
+}
+
+Mat4& Mat4::MakeHScale(const Vec3 &s)
+{
+ MakeDiag();
+
+ row[0][0] = s[0];
+ row[1][1] = s[1];
+ row[2][2] = s[2];
+
+ return(SELF);
+}
+
+Mat4& Mat4::MakeHTrans(const Vec3 &t)
+{
+ MakeDiag();
+
+#ifdef VL_ROW_ORIENT
+ row[3][0] = t[0];
+ row[3][1] = t[1];
+ row[3][2] = t[2];
+#else
+ row[0][3] = t[0];
+ row[1][3] = t[1];
+ row[2][3] = t[2];
+#endif
+
+ return(SELF);
+}
+
+Mat4& Mat4::Transpose()
+{
+ row[0][1] = row[1][0]; row[0][2] = row[2][0]; row[0][3] = row[3][0];
+ row[1][0] = row[0][1]; row[1][2] = row[2][1]; row[1][3] = row[3][1];
+ row[2][0] = row[0][2]; row[2][1] = row[1][2]; row[2][3] = row[3][2];
+ row[3][0] = row[0][3]; row[3][1] = row[1][3]; row[3][2] = row[2][3];
+
+ return(SELF);
+}
+
+Mat4& Mat4::AddShift(const Vec3 &t)
+{
+#ifdef VL_ROW_ORIENT
+ row[3][0] += t[0];
+ row[3][1] += t[1];
+ row[3][2] += t[2];
+#else
+ row[0][3] += t[0];
+ row[1][3] += t[1];
+ row[2][3] += t[2];
+#endif
+
+ return(SELF);
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Mat4.h Mon Jan 04 15:19:10 2016 +0000
@@ -0,0 +1,190 @@
+/*
+ File: Mat4.h
+
+ Function: Defines a 4 x 4 matrix.
+
+ Author(s): Andrew Willmott
+
+ Copyright: (c) 1995-2001, Andrew Willmott
+ */
+
+#ifndef __Mat4__
+#define __Mat4__
+
+#include "Vec3.h"
+#include "Vec4.h"
+
+
+// --- Mat4 Class -------------------------------------------------------------
+
+class Mat4
+{
+public:
+
+ // Constructors
+
+ Mat4();
+ Mat4(Real a, Real b, Real c, Real d,
+ Real e, Real f, Real g, Real h,
+ Real i, Real j, Real k, Real l,
+ Real m, Real n, Real o, Real p);
+ Mat4(const Mat4 &m);
+ Mat4(ZeroOrOne k);
+ Mat4(Block k);
+
+ // Accessor functions
+
+ Int Rows() const { return(4); };
+ Int Cols() const { return(4); };
+
+ Vec4 &operator [] (Int i);
+ const Vec4 &operator [] (Int i) const;
+
+ Real *Ref() const;
+
+ // Assignment operators
+
+ Mat4 &operator = (const Mat4 &m);
+ Mat4 &operator = (ZeroOrOne k);
+ Mat4 &operator = (Block k);
+ Mat4 &operator += (const Mat4 &m);
+ Mat4 &operator -= (const Mat4 &m);
+ Mat4 &operator *= (const Mat4 &m);
+ Mat4 &operator *= (Real s);
+ Mat4 &operator /= (Real s);
+
+ // Comparison operators
+
+ Bool operator == (const Mat4 &m) const; // M == N?
+ Bool operator != (const Mat4 &m) const; // M != N?
+
+ // Arithmetic operators
+
+ Mat4 operator + (const Mat4 &m) const; // M + N
+ Mat4 operator - (const Mat4 &m) const; // M - N
+ Mat4 operator - () const; // -M
+ Mat4 operator * (const Mat4 &m) const; // M * N
+ Mat4 operator * (Real s) const; // M * s
+ Mat4 operator / (Real s) const; // M / s
+
+ // Initialisers
+
+ Void MakeZero(); // Zero matrix
+ Void MakeDiag(Real k = vl_one); // I
+ Void MakeBlock(Real k = vl_one); // all elts = k
+
+ // Homogeneous Transforms
+
+ Mat4& MakeHRot(const Vec3 &axis, Real theta);
+ // Rotate by theta radians about axis
+ Mat4& MakeHRot(const Vec4 &q); // Rotate by quaternion
+ Mat4& MakeHScale(const Vec3 &s); // Scale by components of s
+
+ Mat4& MakeHTrans(const Vec3 &t); // Translation by t
+
+ Mat4& Transpose(); // transpose in place
+ Mat4& AddShift(const Vec3 &t); // Concatenate shift
+
+ // Private...
+
+protected:
+
+ Vec4 row[4];
+};
+
+
+// --- Matrix operators -------------------------------------------------------
+
+Vec4 operator * (const Mat4 &m, const Vec4 &v); // m * v
+Vec4 operator * (const Vec4 &v, const Mat4 &m); // v * m
+Vec4 &operator *= (Vec4 &a, const Mat4 &m); // v *= m
+inline Mat4 operator * (Real s, const Mat4 &m); // s * m
+
+Mat4 trans(const Mat4 &m); // Transpose
+Real trace(const Mat4 &m); // Trace
+Mat4 adj(const Mat4 &m); // Adjoint
+Real det(const Mat4 &m); // Determinant
+Mat4 inv(const Mat4 &m); // Inverse
+Mat4 oprod(const Vec4 &a, const Vec4 &b);
+ // Outer product
+
+// The xform functions help avoid dependence on whether row or column
+// vectors are used to represent points and vectors.
+inline Vec4 xform(const Mat4 &m, const Vec4 &v); // Transform of v by m
+inline Vec3 xform(const Mat4 &m, const Vec3 &v); // Hom. xform of v by m
+inline Mat4 xform(const Mat4 &m, const Mat4 &n); // Xform v -> m(n(v))
+
+//std::ostream &operator << (std::ostream &s, const Mat4 &m);
+//std::istream &operator >> (std::istream &s, Mat4 &m);
+
+void printMat4(const Mat4 &m);
+
+// --- Inlines ----------------------------------------------------------------
+
+inline Mat4::Mat4()
+{
+}
+
+inline Vec4 &Mat4::operator [] (Int i)
+{
+ CheckRange(i, 0, 4, "(Mat4::[i]) index out of range");
+ return(row[i]);
+}
+
+inline const Vec4 &Mat4::operator [] (Int i) const
+{
+ CheckRange(i, 0, 4, "(Mat4::[i]) index out of range");
+ return(row[i]);
+}
+
+inline Real *Mat4::Ref() const
+{
+ return((Real *) row);
+}
+
+inline Mat4::Mat4(ZeroOrOne k)
+{
+ MakeDiag(k);
+}
+
+inline Mat4::Mat4(Block k)
+{
+ MakeBlock((ZeroOrOne) k);
+}
+
+inline Mat4 &Mat4::operator = (ZeroOrOne k)
+{
+ MakeDiag(k);
+
+ return(SELF);
+}
+
+inline Mat4 &Mat4::operator = (Block k)
+{
+ MakeBlock((ZeroOrOne) k);
+
+ return(SELF);
+}
+
+inline Mat4 operator * (Real s, const Mat4 &m)
+{
+ return(m * s);
+}
+
+#ifdef VL_ROW_ORIENT
+inline Vec3 xform(const Mat4 &m, const Vec3 &v)
+{ return(proj(Vec4(v, 1.0) * m)); }
+inline Vec4 xform(const Mat4 &m, const Vec4 &v)
+{ return(v * m); }
+inline Mat4 xform(const Mat4 &m, const Mat4 &n)
+{ return(n * m); }
+#else
+inline Vec3 xform(const Mat4 &m, const Vec3 &v)
+{ return(proj(m * Vec4(v, 1.0))); }
+inline Vec4 xform(const Mat4 &m, const Vec4 &v)
+{ return(m * v); }
+inline Mat4 xform(const Mat4 &m, const Mat4 &n)
+{ return(m * n); }
+#endif
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Quat.cpp Mon Jan 04 15:19:10 2016 +0000
@@ -0,0 +1,146 @@
+#include "Quat.h"
+//#include "svl/Mat3.h"
+#include <cctype>
+#include <iomanip>
+
+//
+// Quat in SVL: [(q0,q1,q2),q3]
+// update:
+// Modify to conform with most literature:
+// Quat in SVL [q0, (q1,q2,q3)]
+// (may need to modify other SVL files relating to quat!!!
+//
+// implement:
+//
+// Quaterion multiplication
+// 3x3 -> mQuat;
+// mQuat->3x3 (NY) .... already available
+// slerp
+//
+// find application that demonstrate the needs ...
+//
+
+Mat3 Rot3 (const Quat &q)
+{
+ Mat3 result;
+ Vec4 v(q[0],q[1],q[2],q[3]);
+ result.MakeRot(v);
+ return(result);
+}
+
+Mat4 HRot4 (const Quat &q)
+{
+ Mat4 result;
+ Vec4 v(q[0],q[1],q[2],q[3]);
+ result.MakeHRot(v);
+ return(result);
+}
+
+Quat::Quat(const Mat3 & m)
+{
+ Real compare[4], max;
+ compare[0] = 1 + m[0][0] + m[1][1] + m[2][2];
+ compare[1] = 1 + m[0][0] - m[1][1] - m[2][2];
+ compare[2] = 1 - m[0][0] + m[1][1] - m[2][2];
+ compare[3] = 1 - m[0][0] - m[1][1] + m[2][2];
+
+ int i, which;
+ for (i = 0, which = -1, max = 0; i < 4; i++)
+ if (fabs(compare[i]) > max)
+ which = i, max = fabs(compare[i]);
+
+ Real q0,q1,q2,q3;
+ switch (which) {
+ case 0:
+ q0 = 0.5*sqrt (compare[which]);
+ q1 = 0.25*(m[2][1] - m[1][2])/q0;
+ q2 = 0.25*(m[0][2] - m[2][0])/q0;
+ q3 = 0.25*(m[1][0] - m[0][1])/q0;
+ break;
+
+ case 1:
+ q1 = 0.5*sqrt (compare[which]);
+ q0 = 0.25*(m[2][1] - m[1][2])/q1;
+ q2 = 0.25*(m[0][1] + m[1][0])/q1;
+ q3 = 0.25*(m[0][2] + m[2][0])/q1;
+ break;
+
+ case 2:
+ q2 = 0.5*sqrt (compare[which]);
+ q0 = 0.25*(m[0][2] - m[2][0])/q2;
+ q1 = 0.25*(m[0][1] + m[1][0])/q2;
+ q3 = 0.25*(m[1][2] + m[2][1])/q2;
+ break;
+
+ case 3:
+ q3 = 0.5*sqrt (compare[which]);
+ q0 = 0.25*(m[1][0] - m[0][1])/q3;
+ q1 = 0.25*(m[0][2] + m[2][0])/q3;
+ q2 = 0.25*(m[1][2] + m[2][1])/q3;
+ break;
+ }
+ elt[1] = q1, elt[2] = q2, elt[3] = q3, elt[0] = q0;
+}
+
+void printQuat(const Quat &v)
+{
+ Int i;
+
+ printf("[");
+ for (i = 0; i < 3; i++){
+ printf("%f", v[i]);
+ printf(" ");
+ }
+ printf("%f ]/r/n", v[3]);
+}
+
+/*
+ostream &operator << (ostream &s, const Quat &v)
+{
+ Int w = s.width();
+
+ return(s << '[' << v[0] << ' ' << setw(w) << v[1] << ' '
+ << setw(w) << v[2] << ' ' << setw(w) << v[3] << ']');
+}
+
+istream &operator >> (istream &s, Quat &v)
+{
+ Quat result;
+ Char c;
+
+ // Expected format: [1 2 3 4]
+
+ while (s >> c && isspace(c))
+ ;
+
+ if (c == '[')
+ {
+ s >> result[0] >> result[1] >> result[2] >> result[3];
+
+ if (!s)
+ {
+ cerr << "Error: Expected number while reading vector\n";
+ return(s);
+ }
+
+ while (s >> c && isspace(c))
+ ;
+
+ if (c != ']')
+ {
+ s.clear(ios::failbit);
+ cerr << "Error: Expected ']' while reading vector\n";
+ return(s);
+ }
+ }
+ else
+ {
+ s.clear(ios::failbit);
+ cerr << "Error: Expected '[' while reading vector\n";
+ return(s);
+ }
+
+ v = result;
+ return(s);
+}
+*/
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Quat.h Mon Jan 04 15:19:10 2016 +0000
@@ -0,0 +1,296 @@
+#ifndef __Quat__
+#define __Quat__
+#include "Vec3.h"
+#include "Mat3.h"
+#include "Vec4.h"
+#include "Mat4.h"
+
+class Quat
+{
+public:
+ // constructors
+ Quat();
+ Quat(Real q0, Real q1, Real q2, Real q3); // [q0,(q1,q2,q3)]
+ Quat (const Vec3 &axis, Real angle);
+ Quat (const Mat3 &m);
+
+ Int Elts() const { return (4); };
+
+ Real &operator [] (Int i);
+ const Real &operator [] (Int i) const;
+
+ // Assignment operators
+
+ Quat &operator = (const Quat &a);
+ Quat &operator += (const Quat &a);
+ Quat &operator -= (const Quat &a);
+ Quat &operator *= (const Quat &a);
+ Quat &operator *= (Real s);
+ Quat &operator /= (Real s);
+
+ // Arithmetic operators
+
+ Quat operator + (const Quat &a) const; // v + a
+ Quat operator - (const Quat &a) const; // v - a
+ Quat operator - () const; // -v
+ Quat operator * (const Quat &a) const; // v * a (vx * ax, ...)
+ Quat operator * (Real s) const; // v * s
+ Quat operator / (Real s) const; // v / s
+
+
+ Quat &Normalise(); // normalise vector
+
+protected:
+ Real elt[4];
+};
+
+inline Quat operator * (Real s, const Quat &v); // Left mult. by s
+inline Real dot(const Quat &a, const Quat &b); // v . a
+inline Real len(const Quat &v); // || v ||
+inline Real sqrlen(const Quat &v); // v . v
+inline Quat norm(const Quat &v); // v / || v ||
+inline Void normalise(Quat &v); // v = norm(v)
+inline Quat slerp(const Quat &q1, const Quat &q2, Real t);
+inline Quat conjugate(const Quat &q);
+
+Mat3 Rot3(const Quat &q);
+Mat4 HRot4(const Quat &q);
+
+
+//std::ostream &operator << (std::ostream &s, const Quat &v);
+//std::istream &operator >> (std::istream &s, Quat &v);
+
+void printQuat(const Quat &v);
+
+inline Real &Quat::operator [] (Int i)
+{
+ CheckRange(i, 0, 4, "(Quat::[i]) index out of range");
+ return(elt[i]);
+}
+
+inline const Real &Quat::operator [] (Int i) const
+{
+ CheckRange(i, 0, 4, "(Quat::[i]) index out of range");
+ return(elt[i]);
+}
+
+inline Quat::Quat()
+{
+}
+
+inline Quat::Quat(Real q0, Real q1, Real q2, Real q3)
+{
+ elt[0] = q0;
+ elt[1] = q1;
+ elt[2] = q2;
+ elt[3] = q3;
+}
+
+inline Quat::Quat(const Vec3 &axis, Real angle)
+{
+ Vec3 n = norm(axis);
+ Real sinhalf = sin(angle/2);
+ elt[1] = sinhalf*n[0];
+ elt[2] = sinhalf*n[1];
+ elt[3] = sinhalf*n[2];
+
+ elt[0] = cos(angle/2);
+}
+
+
+inline Quat &Quat::operator = (const Quat &v)
+{
+ elt[0] = v[0];
+ elt[1] = v[1];
+ elt[2] = v[2];
+ elt[3] = v[3];
+
+ return(SELF);
+}
+
+inline Quat &Quat::operator += (const Quat &v)
+{
+ elt[0] += v[0];
+ elt[1] += v[1];
+ elt[2] += v[2];
+ elt[3] += v[3];
+
+ return(SELF);
+}
+
+inline Quat &Quat::operator -= (const Quat &v)
+{
+ elt[0] -= v[0];
+ elt[1] -= v[1];
+ elt[2] -= v[2];
+ elt[3] -= v[3];
+
+ return(SELF);
+}
+
+inline Quat &Quat::operator *= (const Quat &v)
+{
+ Quat tmp(elt[0],elt[1],elt[2],elt[3]);
+ tmp = tmp * v;
+
+ elt[0] = tmp[0];
+ elt[1] = tmp[1];
+ elt[2] = tmp[2];
+ elt[3] = tmp[3];
+
+ return(SELF);
+}
+
+inline Quat &Quat::operator *= (Real s)
+{
+ elt[0] *= s;
+ elt[1] *= s;
+ elt[2] *= s;
+ elt[3] *= s;
+
+ return(SELF);
+}
+
+inline Quat &Quat::operator /= (Real s)
+{
+ elt[0] /= s;
+ elt[1] /= s;
+ elt[2] /= s;
+ elt[3] /= s;
+
+ return(SELF);
+}
+
+
+inline Quat Quat::operator + (const Quat &a) const
+{
+ Quat result;
+
+ result[0] = elt[0] + a[0];
+ result[1] = elt[1] + a[1];
+ result[2] = elt[2] + a[2];
+ result[3] = elt[3] + a[3];
+
+ return(result);
+}
+
+inline Quat Quat::operator - (const Quat &a) const
+{
+ Quat result;
+
+ result[0] = elt[0] - a[0];
+ result[1] = elt[1] - a[1];
+ result[2] = elt[2] - a[2];
+ result[3] = elt[3] - a[3];
+
+ return(result);
+}
+
+inline Quat Quat::operator - () const
+{
+ Quat result;
+
+ result[0] = -elt[0];
+ result[1] = -elt[1];
+ result[2] = -elt[2];
+ result[3] = -elt[3];
+
+ return(result);
+}
+
+inline Quat Quat::operator * (const Quat &a) const
+{
+ Quat result;
+
+ Vec3 qv(elt[1],elt[2],elt[3]); Real qs = elt[0];
+ Vec3 av(a[1],a[2],a[3]); Real as = a[0];
+
+ Vec3 rv = qs*av + as*qv + cross(qv,av);
+ Real rs = qs*as - dot(qv,av);
+
+ result[1] = rv[0];
+ result[2] = rv[1];
+ result[3] = rv[2];
+ result[0] = rs;
+
+ return(result);
+}
+
+inline Quat Quat::operator * (Real s) const
+{
+ Quat result;
+
+ result[0] = elt[0] * s;
+ result[1] = elt[1] * s;
+ result[2] = elt[2] * s;
+ result[3] = elt[3] * s;
+
+ return(result);
+}
+
+inline Quat Quat::operator / (Real s) const
+{
+ Quat result;
+
+ result[0] = elt[0] / s;
+ result[1] = elt[1] / s;
+ result[2] = elt[2] / s;
+ result[3] = elt[3] / s;
+
+ return(result);
+}
+
+inline Quat operator * (Real s, const Quat &v)
+{
+ return(v * s);
+}
+
+// for convenience. Quat has no dot operation.
+inline Real dot(const Quat &a, const Quat &b)
+{
+ return(a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + a[3] * b[3]);
+}
+
+inline Real len(const Quat &v)
+{
+ return(sqrt(dot(v, v)));
+}
+
+inline Real sqrlen(const Quat &v)
+{
+ return(dot(v, v));
+}
+
+inline Quat norm(const Quat &v)
+{
+ Assert(sqrlen(v) > 0.0, "normalising length-zero vector");
+ return(v / len(v));
+}
+
+inline Void normalise(Quat &v)
+{
+ v /= len(v);
+}
+
+inline Quat slerp (const Quat& q1, const Quat& q2, Real t)
+{
+ Quat result;
+ Quat qq = q1;
+
+ if (dot(qq,q2) < 0)
+ qq = -q1;
+
+ Real phi = acos(dot (qq, q2));
+ Real denom = sin(phi);
+
+ result = sin(phi*(1-t))/denom * qq + sin(phi*t)/denom * q2;
+
+ return result;
+}
+
+inline Quat conjugate(const Quat &q)
+{
+ return Quat (q[0], -q[1], -q[2], -q[3]);
+}
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/SVL.h Mon Jan 04 15:19:10 2016 +0000
@@ -0,0 +1,53 @@
+/*
+ File: SVL.h
+
+ Function: Master header for a simple version of the VL library.
+ The various classes are named Vec2, Mat3, Vec, etc.
+ Link with -lsvl, or define the symbol VL_DEBUG and
+ link with -lsvl.dbg for the debugging version.
+
+ Author(s): Andrew Willmott
+
+ Copyright: (c) 1995-2001, Andrew Willmott
+ */
+
+ // http://www.cs.cmu.edu/~ajw/doc/svl.html
+#ifndef __SVL__
+#define __SVL__
+
+#define SVL_VERSION "1.5"
+#define SVL_VER_NUM 10500
+
+//#pragma comment (lib, "svl-dbg.lib")
+
+
+
+
+#ifdef VL_DEBUG
+#define VL_CHECKING
+#endif
+
+//#include <iostream>
+//namespace svl {
+#include "Basics.h"
+#include "Constants.h"
+#include "Utils.h"
+
+#include "Vec2.h"
+#include "Vec3.h"
+#include "Vec4.h"
+#include "Vec.h"
+
+#include "Mat2.h"
+#include "Mat3.h"
+#include "Mat4.h"
+#include "Mat.h"
+
+#include "Transform.h"
+
+#include "Quat.h"
+
+//}
+//using namespace svl;
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Transform.h Mon Jan 04 15:19:10 2016 +0000
@@ -0,0 +1,41 @@
+/*
+ File: Transform.h
+
+ Function: Provides transformation constructors.
+
+ Author(s): Andrew Willmott
+
+ Copyright: (c) 1995-2001, Andrew Willmott
+*/
+
+#ifndef __SVL_TRANSFORM__
+#define __SVL_TRANSFORM__
+
+inline Mat2 Rot2(Real theta)
+ { Mat2 result; result.MakeRot(theta); return(result); }
+inline Mat2 Scale2(const Vec2 &s)
+ { Mat2 result; result.MakeScale(s); return(result); }
+
+inline Mat3 Rot3(const Vec3 &axis, Real theta)
+ { Mat3 result; result.MakeRot(axis, theta); return(result); }
+inline Mat3 Rot3(const Vec4 &q)
+ { Mat3 result; result.MakeRot(q); return(result); }
+inline Mat3 Scale3(const Vec3 &s)
+ { Mat3 result; result.MakeScale(s); return(result); }
+inline Mat3 HRot3(Real theta)
+ { Mat3 result; result.MakeHRot(theta); return(result); }
+inline Mat3 HScale3(const Vec2 &s)
+ { Mat3 result; result.MakeHScale(s); return(result); }
+inline Mat3 HTrans3(const Vec2 &t)
+ { Mat3 result; result.MakeHTrans(t); return(result); }
+
+inline Mat4 HRot4(const Vec3 &axis, Real theta)
+ { Mat4 result; result.MakeHRot(axis, theta); return(result); }
+inline Mat4 HRot4(const Vec4 &q)
+ { Mat4 result; result.MakeHRot(q); return(result); }
+inline Mat4 HScale4(const Vec3 &s)
+ { Mat4 result; result.MakeHScale(s); return(result); }
+inline Mat4 HTrans4(const Vec3 &t)
+ { Mat4 result; result.MakeHTrans(t); return(result); }
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Utils.h Mon Jan 04 15:19:10 2016 +0000
@@ -0,0 +1,86 @@
+/*
+ File: Utils.h
+
+ Function: Various math definitions for VL
+
+ Author(s): Andrew Willmott
+
+ Copyright: (c) 1995-2001, Andrew Willmott
+ */
+
+#ifndef __VL_MATH__
+#define __VL_MATH__
+
+//#include <cstdlib>
+
+// --- Inlines ----------------------------------------------------------------
+
+// additions to arithmetic functions
+
+#ifdef VL_HAS_IEEEFP
+#include <ieeefp.h>
+#define vl_is_finite(X) finite(X)
+#elif defined (__GNUC__) && defined(__USE_MISC)
+#define vl_is_finite(X) finite(X)
+#else
+#define vl_is_finite(X) (1)
+#endif
+
+#ifdef VL_HAS_DRAND
+inline Double vl_rand()
+{ return(drand48()); }
+#else
+#ifndef RAND_MAX
+// we're on something totally sucky, like SunOS
+#define RAND_MAX (Double(1 << 30) * 4.0 - 1.0)
+#endif
+inline Double vl_rand()
+{ return(rand() / (RAND_MAX + 1.0)); }
+#endif
+
+#ifndef __CMATH__
+// GNU's complex.h defines its own abs(double)
+#ifdef VL_HAS_ABSF
+inline Float abs(Float x)
+{ return (fabsf(x)); }
+#endif
+// jmc ......... 7/19
+//inline Double abs(Double x)
+//{ return (fabs(x)); }
+#endif
+#ifdef VL_HAS_ABSF
+inline Float len(Float x)
+{ return (fabsf(x)); }
+#endif
+inline Double len(Double x)
+{ return (fabs(x)); }
+
+inline Float sqrlen(Float r)
+{ return(sqr(r)); }
+inline Double sqrlen(Double r)
+{ return(sqr(r)); }
+
+inline Float mix(Float a, Float b, Float s)
+{ return((1.0f - s) * a + s * b); }
+inline Double mix(Double a, Double b, Double s)
+{ return((1.0 - s) * a + s * b); }
+
+inline Double sign(Double d)
+{
+ if (d < 0)
+ return(-1.0);
+ else
+ return(1.0);
+}
+
+// useful routines
+
+inline Bool IsPowerOfTwo(Int a)
+{ return((a & -a) == a); };
+
+inline Void SetReal(Float &a, Double b)
+{ a = Float(b); }
+inline Void SetReal(Double &a, Double b)
+{ a = b; }
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Vec.cpp Mon Jan 04 15:19:10 2016 +0000
@@ -0,0 +1,548 @@
+/*
+ File: Vec.cpp
+
+ Function: Implements Vec.h
+
+ Author(s): Andrew Willmott
+
+ Copyright: (c) 1995-2001, Andrew Willmott
+
+*/
+
+
+#include "Vec.h"
+
+//#include <cctype>
+//#include <cstring>
+//#include <cstdarg>
+//#include <iomanip>
+#include "Vec2.h"
+#include "Vec3.h"
+#include "Vec4.h"
+
+// --- Vec Constructors -------------------------------------------------------
+
+
+Vec::Vec(Int n, ZeroOrOne k) : elts(n)
+{
+ Assert(n > 0,"(Vec) illegal vector size");
+
+ data = new Real[n];
+
+ MakeBlock(k);
+}
+
+Vec::Vec(Int n, Axis a) : elts(n)
+{
+ Assert(n > 0,"(Vec) illegal vector size");
+
+ data = new Real[n];
+
+ MakeUnit(a);
+}
+
+Vec::Vec(const Vec &v)
+{
+ Assert(v.data != 0, "(Vec) Can't construct from a null vector");
+
+ elts = v.Elts();
+ data = new Real[elts];
+
+#ifdef VL_USE_MEMCPY
+ memcpy(data, v.Ref(), sizeof(Real) * Elts());
+#else
+ for (Int i = 0; i < Elts(); i++)
+ data[i] = v[i];
+#endif
+}
+
+Vec::Vec(const Vec2 &v) : data(v.Ref()), elts(v.Elts() | VL_REF_FLAG)
+{
+}
+
+Vec::Vec(const Vec3 &v) : data(v.Ref()), elts(v.Elts() | VL_REF_FLAG)
+{
+}
+
+Vec::Vec(const Vec4 &v) : data(v.Ref()), elts(v.Elts() | VL_REF_FLAG)
+{
+}
+
+Vec::Vec(Int n, double elt0, ...) : elts(n)
+{
+ Assert(n > 0,"(Vec) illegal vector size");
+
+ va_list ap;
+ Int i = 1;
+
+ data = new Real[n];
+ va_start(ap, elt0);
+
+ SetReal(data[0], elt0);
+
+ while (--n)
+ SetReal(data[i++], va_arg(ap, double));
+
+ va_end(ap);
+}
+
+Vec::~Vec()
+{
+ Assert(elts != 0,"(Vec) illegal vector size");
+
+ if (!IsRef())
+ delete[] data;
+}
+
+
+// --- Vec Assignment Operators -----------------------------------------------
+
+
+Vec &Vec::operator = (const Vec &v)
+{
+ if (!IsRef())
+ SetSize(v.Elts());
+ else
+ Assert(Elts() == v.Elts(), "(Vec::=) Vector sizes don't match");
+
+#ifdef VL_USE_MEMCPY
+ memcpy(data, v.data, sizeof(Real) * Elts());
+#else
+ for (Int i = 0; i < Elts(); i++)
+ data[i] = v[i];
+#endif
+
+ return(SELF);
+}
+
+Vec &Vec::operator = (const Vec2 &v)
+{
+ if (!IsRef())
+ SetSize(v.Elts());
+ else
+ Assert(Elts() == v.Elts(), "(Vec::=) Vector sizes don't match");
+
+ data[0] = v[0];
+ data[1] = v[1];
+
+ return(SELF);
+}
+
+Vec &Vec::operator = (const Vec3 &v)
+{
+ if (!IsRef())
+ SetSize(v.Elts());
+ else
+ Assert(Elts() == v.Elts(), "(Vec::=) Vector sizes don't match");
+
+ data[0] = v[0];
+ data[1] = v[1];
+ data[2] = v[2];
+
+ return(SELF);
+}
+
+Vec &Vec::operator = (const Vec4 &v)
+{
+ if (!IsRef())
+ SetSize(v.Elts());
+ else
+ Assert(Elts() == v.Elts(), "(Vec::=) Vector sizes don't match");
+
+ data[0] = v[0];
+ data[1] = v[1];
+ data[2] = v[2];
+ data[3] = v[3];
+
+ return(SELF);
+}
+
+Void Vec::SetSize(Int ni)
+{
+ Assert(ni > 0, "(Vec::SetSize) Illegal vector size");
+ UInt n = UInt(ni);
+
+ if (!IsRef())
+ {
+ // Don't reallocate if we already have enough storage
+
+ if (n <= elts)
+ {
+ elts = n;
+ return;
+ }
+
+ // Otherwise, delete old storage
+
+ delete[] data;
+
+ elts = n;
+ data = new Real[elts];
+ }
+ else
+ Assert(false, "(Vec::SetSize) Can't resize a vector reference");
+}
+
+Vec &Vec::MakeZero()
+{
+#ifdef VL_USE_MEMCPY
+ memset(data, 0, sizeof(Real) * Elts());
+#else
+ Int j;
+
+ for (j = 0; j < Elts(); j++)
+ data[j] = vl_zero;
+#endif
+
+ return(SELF);
+}
+
+Vec &Vec::MakeUnit(Int i, Real k)
+{
+ MakeZero();
+ data[i] = k;
+
+ return(SELF);
+}
+
+Vec &Vec::MakeBlock(Real k)
+{
+ Int i;
+
+ for (i = 0; i < Elts(); i++)
+ data[i] = k;
+
+ return(SELF);
+}
+
+
+// --- Vec In-Place operators -------------------------------------------------
+
+
+Vec &Vec::operator += (const Vec &b)
+{
+ Assert(Elts() == b.Elts(), "(Vec::+=) vector sizes don't match");
+
+ Int i;
+
+ for (i = 0; i < Elts(); i++)
+ data[i] += b[i];
+
+ return(SELF);
+}
+
+Vec &Vec::operator -= (const Vec &b)
+{
+ Assert(Elts() == b.Elts(), "(Vec::-=) vector sizes don't match");
+
+ Int i;
+
+ for (i = 0; i < Elts(); i++)
+ data[i] -= b[i];
+
+ return(SELF);
+}
+
+Vec &Vec::operator *= (const Vec &b)
+{
+ Assert(Elts() == b.Elts(), "(Vec::*=) Vec sizes don't match");
+
+ Int i;
+
+ for (i = 0; i < Elts(); i++)
+ data[i] *= b[i];
+
+ return(SELF);
+}
+
+Vec &Vec::operator *= (Real s)
+{
+ Int i;
+
+ for (i = 0; i < Elts(); i++)
+ data[i] *= s;
+
+ return(SELF);
+}
+
+Vec &Vec::operator /= (const Vec &b)
+{
+ Assert(Elts() == b.Elts(), "(Vec::/=) Vec sizes don't match");
+
+ Int i;
+
+ for (i = 0; i < Elts(); i++)
+ data[i] /= b[i];
+
+ return(SELF);
+}
+
+Vec &Vec::operator /= (Real s)
+{
+ Int i;
+
+ for (i = 0; i < Elts(); i++)
+ data[i] /= s;
+
+ return(SELF);
+}
+
+
+// --- Vec Comparison Operators -----------------------------------------------
+
+
+Bool operator == (const Vec &a, const Vec &b)
+{
+ Int i;
+
+ for (i = 0; i < a.Elts(); i++)
+ if (a[i] != b[i])
+ return(0);
+
+ return(1);
+}
+
+Bool operator != (const Vec &a, const Vec &b)
+{
+ Int i;
+
+ for (i = 0; i < a.Elts(); i++)
+ if (a[i] != b[i])
+ return(1);
+
+ return(0);
+}
+
+
+// --- Vec Arithmetic Operators -----------------------------------------------
+
+
+Vec operator + (const Vec &a, const Vec &b)
+{
+ Assert(a.Elts() == b.Elts(), "(Vec::+) Vec sizes don't match");
+
+ Vec result(a.Elts());
+ Int i;
+
+ for (i = 0; i < a.Elts(); i++)
+ result[i] = a[i] + b[i];
+
+ return(result);
+}
+
+Vec operator - (const Vec &a, const Vec &b)
+{
+ Assert(a.Elts() == b.Elts(), "(Vec::-) Vec sizes don't match");
+
+ Vec result(a.Elts());
+ Int i;
+
+ for (i = 0; i < a.Elts(); i++)
+ result[i] = a[i] - b[i];
+
+ return(result);
+}
+
+Vec operator - (const Vec &v)
+{
+ Vec result(v.Elts());
+ Int i;
+
+ for (i = 0; i < v.Elts(); i++)
+ result[i] = - v[i];
+
+ return(result);
+}
+
+Vec operator * (const Vec &a, const Vec &b)
+{
+ Assert(a.Elts() == b.Elts(), "(Vec::*) Vec sizes don't match");
+
+ Vec result(a.Elts());
+ Int i;
+
+ for (i = 0; i < a.Elts(); i++)
+ result[i] = a[i] * b[i];
+
+ return(result);
+}
+
+Vec operator * (const Vec &v, Real s)
+{
+ Vec result(v.Elts());
+ Int i;
+
+ for (i = 0; i < v.Elts(); i++)
+ result[i] = v[i] * s;
+
+ return(result);
+}
+
+Vec operator / (const Vec &a, const Vec &b)
+{
+ Assert(a.Elts() == b.Elts(), "(Vec::/) Vec sizes don't match");
+
+ Vec result(a.Elts());
+ Int i;
+
+ for (i = 0; i < a.Elts(); i++)
+ result[i] = a[i] / b[i];
+
+ return(result);
+}
+
+Vec operator / (const Vec &v, Real s)
+{
+ Vec result(v.Elts());
+ Int i;
+
+ for (i = 0; i < v.Elts(); i++)
+ result[i] = v[i] / s;
+
+ return(result);
+}
+
+Real dot(const Vec &a, const Vec &b)
+{
+ Assert(a.Elts() == b.Elts(), "(Vec::dot) Vec sizes don't match");
+
+ Real sum = vl_zero;
+ Int i;
+
+ for (i = 0; i < a.Elts(); i++)
+ sum += a[i] * b[i];
+
+ return(sum);
+}
+
+Vec operator * (Real s, const Vec &v)
+{
+ Vec result(v.Elts());
+ Int i;
+
+ for (i = 0; i < v.Elts(); i++)
+ result[i] = v[i] * s;
+
+ return(result);
+}
+
+Vec &Vec::Clamp(Real fuzz)
+// clamps all values of the matrix with a magnitude
+// smaller than fuzz to zero.
+{
+ Int i;
+
+ for (i = 0; i < Elts(); i++)
+ if (len(SELF[i]) < fuzz)
+ SELF[i] = vl_zero;
+
+ return(SELF);
+}
+
+Vec &Vec::Clamp()
+{
+ return(Clamp(1e-7));
+}
+
+Vec clamped(const Vec &v, Real fuzz)
+// clamps all values of the matrix with a magnitude
+// smaller than fuzz to zero.
+{
+ Vec result(v);
+
+ return(result.Clamp(fuzz));
+}
+
+Vec clamped(const Vec &v)
+{
+ return(clamped(v, 1e-7));
+}
+
+
+// --- Vec Input & Output -----------------------------------------------------
+
+
+/*
+ostream &operator << (ostream &s, const Vec &v)
+{
+ Int i, w;
+
+ s << '[';
+
+ if (v.Elts() > 0)
+ {
+ w = s.width();
+ s << v[0];
+
+ for (i = 1; i < v.Elts(); i++)
+ s << ' ' << setw(w) << v[i];
+ }
+
+ s << ']';
+
+ return(s);
+}
+*/
+
+inline Void CopyPartialVec(const Vec &u, Vec &v, Int numElts)
+{
+ for (Int i = 0; i < numElts; i++)
+ v[i] = u[i];
+}
+
+/*
+istream &operator >> (istream &s, Vec &v)
+{
+ Int size = 0;
+ Vec inVec(16);
+ Char c;
+
+ // Expected format: [a b c d ...]
+
+ while (isspace(s.peek())) // chomp white space
+ s.get(c);
+
+ if (s.peek() == '[')
+ {
+ s.get(c);
+
+
+ while (isspace(s.peek())) // chomp white space
+ s.get(c);
+
+ while (s.peek() != ']') // resize if needed
+ {
+ if (size == inVec.Elts())
+ {
+ Vec holdVec(inVec);
+
+ inVec.SetSize(size * 2);
+ CopyPartialVec(holdVec, inVec, size);
+ }
+
+ s >> inVec[size++]; // read an item
+
+ if (!s)
+ {
+ _Warning("Couldn't read vector element");
+ return(s);
+ }
+
+ while (isspace(s.peek())) // chomp white space
+ s.get(c);
+ }
+ s.get(c);
+ }
+ else
+ {
+ s.clear(ios::failbit);
+ _Warning("Error: Expected '[' while reading vector");
+ return(s);
+ }
+
+ v.SetSize(size);
+ CopyPartialVec(inVec, v, size);
+
+ return(s);
+}
+*/
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Vec.h Mon Jan 04 15:19:10 2016 +0000
@@ -0,0 +1,255 @@
+/*
+ File: Vec.h
+
+ Function: Defines a generic resizeable vector.
+
+ Author(s): Andrew Willmott
+
+ Copyright: (c) 1995-2001, Andrew Willmott
+ */
+
+#ifndef __Vec__
+#define __Vec__
+#include "Basics.h"
+#include "Constants.h"
+#include "Utils.h"
+
+class Vec2;
+class Vec3;
+class Vec4;
+
+
+// --- Vec Class --------------------------------------------------------------
+
+class Vec
+{
+public:
+
+ // Constructors
+
+ Vec(); // Null vector
+ explicit Vec(Int n); // n-element vector
+ Vec(Int n, double elt0, ...); // e.g. Vec(3, 1.1, 2.0, 3.4)
+ Vec(Int n, Real *data); // Vector reference
+ Vec(const Vec &v); // Copy constructor
+ Vec(const Vec2 &v); // reference to a Vec2
+ Vec(const Vec3 &v); // reference to a Vec3
+ Vec(const Vec4 &v); // reference to a Vec4
+ Vec(Int n, ZeroOrOne); // Zero or all-ones vector
+ Vec(Int n, Axis a); // Unit vector
+ ~Vec(); // Destructor
+
+ // Accessor functions
+
+ Int Elts() const;
+
+ Real &operator [] (Int i);
+ Real operator [] (Int i) const;
+
+ Void SetSize(Int n); // Resize the vector
+ Real *Ref() const; // Return pointer to data
+
+ // Assignment operators
+
+ Vec &operator = (const Vec &v); // v = a etc.
+ Vec &operator = (ZeroOrOne k);
+ Vec &operator = (Axis a);
+ Vec &operator = (const Vec2 &v);
+ Vec &operator = (const Vec3 &v);
+ Vec &operator = (const Vec4 &v);
+
+ // In-Place operators
+
+ Vec &operator += (const Vec &v);
+ Vec &operator -= (const Vec &v);
+ Vec &operator *= (const Vec &v);
+ Vec &operator *= (Real s);
+ Vec &operator /= (const Vec &v);
+ Vec &operator /= (Real s);
+
+ // Vector initialisers
+
+ Vec &MakeZero();
+ Vec &MakeUnit(Int i, Real k = vl_one);
+ Vec &MakeBlock(Real k = vl_one);
+
+ Vec &Normalise(); // Normalise vector
+ Vec &Clamp(Real fuzz);
+ Vec &Clamp();
+
+ Bool IsRef() const { return((elts & VL_REF_FLAG) != 0); };
+
+ // Private...
+
+protected:
+
+ Real *data;
+ UInt elts;
+};
+
+
+// --- Vec Comparison Operators -----------------------------------------------
+
+Bool operator == (const Vec &a, const Vec &b);
+Bool operator != (const Vec &a, const Vec &b);
+
+
+// --- Vec Arithmetic Operators -----------------------------------------------
+
+Vec operator + (const Vec &a, const Vec &b);
+Vec operator - (const Vec &a, const Vec &b);
+Vec operator - (const Vec &v);
+Vec operator * (const Vec &a, const Vec &b);
+Vec operator * (const Vec &v, Real s);
+Vec operator / (const Vec &a, const Vec &b);
+Vec operator / (const Vec &v, Real s);
+Vec operator * (Real s, const Vec &v);
+
+Real dot(const Vec &a, const Vec &b);// v . a
+inline Real len(const Vec &v); // || v ||
+inline Real sqrlen(const Vec &v); // v . v
+inline Vec norm(const Vec &v); // v / || v ||
+inline Void normalise(Vec &v); // v = norm(v)
+
+Vec clamped(const Vec &v, Real fuzz);
+Vec clamped(const Vec &v);
+
+
+// --- Vec Input & Output -----------------------------------------------------
+
+//std::ostream &operator << (std::ostream &s, const Vec &v);
+//std::istream &operator >> (std::istream &s, Vec &v);
+
+inline void printVec(const Vec &v);
+
+// --- Sub-vector functions ---------------------------------------------------
+
+inline Vec sub(const Vec &v, Int start, Int length);
+inline Vec first(const Vec &v, Int length);
+inline Vec last(const Vec &v, Int length);
+
+
+// --- Vec inlines ------------------------------------------------------------
+
+inline Vec::Vec() : data(0), elts(0)
+{
+}
+
+inline Vec::Vec(Int n) : elts(n)
+{
+ Assert(n > 0,"(Vec) illegal vector size");
+
+ data = new Real[n];
+}
+
+inline Vec::Vec(Int n, Real *data) : data(data), elts(n | VL_REF_FLAG)
+{
+}
+
+inline Int Vec::Elts() const
+{
+ return(elts & VL_REF_MASK);
+}
+
+inline Real &Vec::operator [] (Int i)
+{
+ CheckRange(i, 0, Elts(), "Vec::[i]");
+
+ return(data[i]);
+}
+
+inline Real Vec::operator [] (Int i) const
+{
+ CheckRange(i, 0, Elts(), "Vec::[i]");
+
+ return(data[i]);
+}
+
+inline Real *Vec::Ref() const
+{
+ return(data);
+}
+
+inline Vec &Vec::operator = (ZeroOrOne k)
+{
+ MakeBlock(k);
+
+ return(SELF);
+}
+
+inline Vec &Vec::operator = (Axis a)
+{
+ MakeUnit(a);
+
+ return(SELF);
+}
+
+inline Real len(const Vec &v)
+{
+ return(sqrt(dot(v, v)));
+}
+
+inline Real sqrlen(const Vec &v)
+{
+ return(dot(v, v));
+}
+
+inline Vec norm(const Vec &v)
+{
+ Assert(sqrlen(v) > 0.0, "normalising length-zero vector");
+ return(v / len(v));
+}
+
+inline Void normalise(Vec &v)
+{
+ v /= len(v);
+}
+
+inline Vec sub(const Vec &v, Int start, Int length)
+{
+ Assert(start >= 0 && length > 0 && start + length <= v.Elts(),
+ "(sub(Vec)) illegal subset of vector");
+
+ return(Vec(length, v.Ref() + start));
+}
+
+inline Vec first(const Vec &v, Int length)
+{
+ Assert(length > 0 && length <= v.Elts(),
+ "(first(Vec)) illegal subset of vector");
+
+ return(Vec(length, v.Ref()));
+}
+
+inline Vec last(const Vec &v, Int length)
+{
+ Assert(length > 0 && length <= v.Elts(),
+ "(last(Vec)) illegal subset of vector");
+
+ return(Vec(length, v.Ref() + v.Elts() - length));
+}
+
+inline Vec &Vec::Normalise()
+{
+ Assert(sqrlen(SELF) > 0.0, "normalising length-zero vector");
+ SELF /= len(SELF);
+ return(SELF);
+}
+
+inline void printVec(const Vec &v)
+{
+ Int i;
+ printf("[");
+ if (v.Elts() > 0)
+ {
+ printf("%10f",v[0]);
+
+ for (i = 1; i < v.Elts(); i++)
+ printf(" %10f", v[i]);
+ }
+ printf("]");
+}
+
+
+#endif
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Vec2.cpp Mon Jan 04 15:19:10 2016 +0000
@@ -0,0 +1,66 @@
+/*
+ File: Vec2.cpp
+
+ Function: Implements Vec2.h
+
+ Author(s): Andrew Willmott
+
+ Copyright: (c) 1995-2001, Andrew Willmott
+
+*/
+
+
+#include "Vec2.h"
+//#include <cctype>
+//#include <iomanip>
+
+
+/*
+ostream &operator << (ostream &s, const Vec2 &v)
+{
+ uint16_t w = s.width();
+
+ return(s << '[' << v[0] << ' ' << setw(w) << v[1] << ']');
+}
+
+istream &operator >> (istream &s, Vec2 &v)
+{
+ Vec2 result;
+ Char c;
+
+ // Expected format: [1 2]
+
+ while (s >> c && isspace(c))
+ ;
+
+ if (c == '[')
+ {
+ s >> result[0] >> result[1];
+
+ if (!s)
+ {
+ cerr << "Error: Expected number while reading vector\n";
+ return(s);
+ }
+
+ while (s >> c && isspace(c))
+ ;
+
+ if (c != ']')
+ {
+ s.clear(ios::failbit);
+ cerr << "Error: Expected ']' while reading vector\n";
+ return(s);
+ }
+ }
+ else
+ {
+ s.clear(ios::failbit);
+ cerr << "Error: Expected '[' while reading vector\n";
+ return(s);
+ }
+
+ v = result;
+ return(s);
+}
+*/
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Vec2.h Mon Jan 04 15:19:10 2016 +0000
@@ -0,0 +1,373 @@
+/*
+ File: Vec2.h
+
+ Function: Defines a length-2 vector.
+
+ Author(s): Andrew Willmott
+
+ Copyright: (c) 1995-2001, Andrew Willmott
+ */
+
+#ifndef __Vec2__
+#define __Vec2__
+#include "Constants.h"
+#include "Basics.h"
+
+// --- Vec2 Class -------------------------------------------------------------
+
+
+class Vec2
+{
+public:
+
+ // Constructors
+
+ Vec2();
+ Vec2(double x, double y); // (x, y)
+ Vec2(const Vec2 &v); // Copy constructor
+ Vec2(ZeroOrOne k); // v[i] = vl_zero
+ Vec2(Axis k); // v[k] = 1
+
+ // Accessor functions
+
+ double &operator [] (Int i);
+ const double &operator [] (Int i) const;
+
+ Int Elts() const { return(2); };
+ double *Ref() const; // Return ptr to data
+
+ // Assignment operators
+
+ Vec2 &operator = (const Vec2 &a);
+ Vec2 &operator = (ZeroOrOne k);
+ Vec2 &operator = (Axis k);
+
+ Vec2 &operator += (const Vec2 &a);
+ Vec2 &operator -= (const Vec2 &a);
+ Vec2 &operator *= (const Vec2 &a);
+ Vec2 &operator *= (double s);
+ Vec2 &operator /= (const Vec2 &a);
+ Vec2 &operator /= (double s);
+
+ // Comparison operators
+
+ bool operator == (const Vec2 &a) const; // v == a?
+ bool operator != (const Vec2 &a) const; // v != a?
+
+ // Arithmetic operators
+
+ Vec2 operator + (const Vec2 &a) const; // v + a
+ Vec2 operator - (const Vec2 &a) const; // v - a
+ Vec2 operator - () const; // -v
+ Vec2 operator * (const Vec2 &a) const; // v * a (vx * ax, ...)
+ Vec2 operator * (double s) const; // v * s
+ Vec2 operator / (const Vec2 &a) const; // v / a (vx / ax, ...)
+ Vec2 operator / (double s) const; // v / s
+
+ // Initialisers
+
+ Vec2 &MakeZero(); // Zero vector
+ Vec2 &MakeUnit(Int i, double k = vl_one); // I[i]
+ Vec2 &MakeBlock(double k = vl_one); // All-k vector
+
+ Vec2 &Normalise(); // normalise vector
+
+ // Private...
+
+protected:
+
+ double elt[2];
+};
+
+
+// --- Vec operators ----------------------------------------------------------
+
+inline Vec2 operator * (double s, const Vec2 &v); // s * v
+inline double dot(const Vec2 &a, const Vec2 &b); // v . a
+inline double len(const Vec2 &v); // || v ||
+inline double sqrlen(const Vec2 &v); // v . v
+inline Vec2 norm(const Vec2 &v); // v / || v ||
+inline void normalise(Vec2 &v); // v = norm(v)
+inline Vec2 cross(const Vec2 &v); // cross prod.
+
+//std::ostream &operator << (std::ostream &s, const Vec2 &v);
+//std::istream &operator >> (std::istream &s, Vec2 &v);
+
+inline void printVec2(const Vec2 &v);
+
+
+// --- Inlines ----------------------------------------------------------------
+
+inline double &Vec2::operator [] (Int i)
+{
+ CheckRange(i, 0, 2, "(Vec2::[i]) index out of range");
+ return(elt[i]);
+}
+
+inline const double &Vec2::operator [] (Int i) const
+{
+ CheckRange(i, 0, 2, "(Vec2::[i]) index out of range");
+ return(elt[i]);
+}
+
+inline Vec2::Vec2()
+{
+}
+
+inline Vec2::Vec2(double x, double y)
+{
+ elt[0] = x;
+ elt[1] = y;
+}
+
+inline Vec2::Vec2(const Vec2 &v)
+{
+ elt[0] = v[0];
+ elt[1] = v[1];
+}
+
+inline double *Vec2::Ref() const
+{
+ return((double *) elt);
+}
+
+inline Vec2 &Vec2::operator = (const Vec2 &v)
+{
+ elt[0] = v[0];
+ elt[1] = v[1];
+
+ return(SELF);
+}
+
+inline Vec2 &Vec2::operator += (const Vec2 &v)
+{
+ elt[0] += v[0];
+ elt[1] += v[1];
+
+ return(SELF);
+}
+
+inline Vec2 &Vec2::operator -= (const Vec2 &v)
+{
+ elt[0] -= v[0];
+ elt[1] -= v[1];
+
+ return(SELF);
+}
+
+inline Vec2 &Vec2::operator *= (const Vec2 &v)
+{
+ elt[0] *= v[0];
+ elt[1] *= v[1];
+
+ return(SELF);
+}
+
+inline Vec2 &Vec2::operator *= (double s)
+{
+ elt[0] *= s;
+ elt[1] *= s;
+
+ return(SELF);
+}
+
+inline Vec2 &Vec2::operator /= (const Vec2 &v)
+{
+ elt[0] /= v[0];
+ elt[1] /= v[1];
+
+ return(SELF);
+}
+
+inline Vec2 &Vec2::operator /= (double s)
+{
+ elt[0] /= s;
+ elt[1] /= s;
+
+ return(SELF);
+}
+
+inline Vec2 Vec2::operator + (const Vec2 &a) const
+{
+ Vec2 result;
+
+ result[0] = elt[0] + a[0];
+ result[1] = elt[1] + a[1];
+
+ return(result);
+}
+
+inline Vec2 Vec2::operator - (const Vec2 &a) const
+{
+ Vec2 result;
+
+ result[0] = elt[0] - a[0];
+ result[1] = elt[1] - a[1];
+
+ return(result);
+}
+
+inline Vec2 Vec2::operator - () const
+{
+ Vec2 result;
+
+ result[0] = -elt[0];
+ result[1] = -elt[1];
+
+ return(result);
+}
+
+inline Vec2 Vec2::operator * (const Vec2 &a) const
+{
+ Vec2 result;
+
+ result[0] = elt[0] * a[0];
+ result[1] = elt[1] * a[1];
+
+ return(result);
+}
+
+inline Vec2 Vec2::operator * (double s) const
+{
+ Vec2 result;
+
+ result[0] = elt[0] * s;
+ result[1] = elt[1] * s;
+
+ return(result);
+}
+
+inline Vec2 operator * (double s, const Vec2 &v)
+{
+ return(v * s);
+}
+
+inline Vec2 Vec2::operator / (const Vec2 &a) const
+{
+ Vec2 result;
+
+ result[0] = elt[0] / a[0];
+ result[1] = elt[1] / a[1];
+
+ return(result);
+}
+
+inline Vec2 Vec2::operator / (double s) const
+{
+ Vec2 result;
+
+ result[0] = elt[0] / s;
+ result[1] = elt[1] / s;
+
+ return(result);
+}
+
+inline double dot(const Vec2 &a, const Vec2 &b)
+{
+ return(a[0] * b[0] + a[1] * b[1]);
+}
+
+inline Vec2 cross(const Vec2 &a)
+{
+ Vec2 result;
+
+ result[0] = a[1];
+ result[1] = -a[0];
+
+ return(result);
+}
+
+inline double len(const Vec2 &v)
+{
+ return(sqrt(dot(v, v)));
+}
+
+inline double sqrlen(const Vec2 &v)
+{
+ return(dot(v, v));
+}
+
+inline Vec2 norm(const Vec2 &v)
+{
+ Assert(sqrlen(v) > 0.0, "normalising length-zero vector");
+ return(v / len(v));
+}
+
+inline void normalise(Vec2 &v)
+{
+ v /= len(v);
+}
+
+inline Vec2 &Vec2::MakeUnit(Int i, double k)
+{
+ if (i == 0)
+ { elt[0] = k; elt[1] = vl_zero; }
+ else if (i == 1)
+ { elt[0] = vl_zero; elt[1] = k; }
+ else
+ _Error("(Vec2::Unit) illegal unit vector");
+ return(SELF);
+}
+
+inline Vec2 &Vec2::MakeZero()
+{
+ elt[0] = vl_zero; elt[1] = vl_zero;
+ return(SELF);
+}
+
+inline Vec2 &Vec2::MakeBlock(double k)
+{
+ elt[0] = k; elt[1] = k;
+ return(SELF);
+}
+
+inline Vec2 &Vec2::Normalise()
+{
+ Assert(sqrlen(SELF) > 0.0, "normalising length-zero vector");
+ SELF /= len(SELF);
+ return(SELF);
+}
+
+
+inline Vec2::Vec2(ZeroOrOne k)
+{
+ elt[0] = k;
+ elt[1] = k;
+}
+
+inline Vec2::Vec2(Axis k)
+{
+ MakeUnit(k, vl_one);
+}
+
+inline Vec2 &Vec2::operator = (ZeroOrOne k)
+{
+ elt[0] = k; elt[1] = k;
+
+ return(SELF);
+}
+
+inline Vec2 &Vec2::operator = (Axis k)
+{
+ MakeUnit(k, vl_1);
+
+ return(SELF);
+}
+
+inline bool Vec2::operator == (const Vec2 &a) const
+{
+ return(elt[0] == a[0] && elt[1] == a[1]);
+}
+
+inline bool Vec2::operator != (const Vec2 &a) const
+{
+ return(elt[0] != a[0] || elt[1] != a[1]);
+}
+
+inline void printVec2(const Vec2 &v)
+{
+ printf("[%10f %10f]",v[0],v[1]);
+}
+
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Vec3.cpp Mon Jan 04 15:19:10 2016 +0000
@@ -0,0 +1,65 @@
+/*
+ File: Vec3.cpp
+
+ Function: Implements Vec3.h
+
+ Author(s): Andrew Willmott
+
+ Copyright: (c) 1995-2001, Andrew Willmott
+
+*/
+
+
+#include "Vec3.h"
+//#include <cctype>
+//#include <iomanip>
+
+/*
+ostream &operator << (ostream &s, const Vec3 &v)
+{
+ Int w = s.width();
+
+ return(s << '[' << v[0] << ' ' << setw(w) << v[1] << ' ' << setw(w) << v[2] << ']');
+}
+
+istream &operator >> (istream &s, Vec3 &v)
+{
+ Vec3 result;
+ Char c;
+
+ // Expected format: [1 2 3]
+
+ while (s >> c && isspace(c))
+ ;
+
+ if (c == '[')
+ {
+ s >> result[0] >> result[1] >> result[2];
+
+ if (!s)
+ {
+ cerr << "Error: Expected number while reading vector\n";
+ return(s);
+ }
+
+ while (s >> c && isspace(c))
+ ;
+
+ if (c != ']')
+ {
+ s.clear(ios::failbit);
+ cerr << "Error: Expected ']' while reading vector\n";
+ return(s);
+ }
+ }
+ else
+ {
+ s.clear(ios::failbit);
+ cerr << "Error: Expected '[' while reading vector\n";
+ return(s);
+ }
+
+ v = result;
+ return(s);
+}
+*/
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Vec3.h Mon Jan 04 15:19:10 2016 +0000
@@ -0,0 +1,421 @@
+/*
+ File: Vec3.h
+
+ Function: Defines a length-3 vector.
+
+ Author(s): Andrew Willmott
+
+ Copyright: (c) 1995-2001, Andrew Willmott
+ */
+
+#ifndef __Vec3__
+#define __Vec3__
+
+#include "Vec2.h"
+
+
+// --- Vec3 Class -------------------------------------------------------------
+
+class Vec3
+{
+public:
+
+ // Constructors
+
+ Vec3();
+ Vec3(double x, double y, double z); // [x, y, z]
+ Vec3(const Vec3 &v); // Copy constructor
+ Vec3(const Vec2 &v, double w); // Hom. 2D vector
+ Vec3(ZeroOrOne k);
+ Vec3(Axis a);
+
+ // Accessor functions
+
+ Int Elts() const { return(3); };
+
+ double &operator [] (Int i);
+ const double &operator [] (Int i) const;
+
+ double *Ref() const; // Return pointer to data
+
+ // Assignment operators
+
+ Vec3 &operator = (const Vec3 &a);
+ Vec3 &operator = (ZeroOrOne k);
+ Vec3 &operator += (const Vec3 &a);
+ Vec3 &operator -= (const Vec3 &a);
+ Vec3 &operator *= (const Vec3 &a);
+ Vec3 &operator *= (double s);
+ Vec3 &operator /= (const Vec3 &a);
+ Vec3 &operator /= (double s);
+
+ // Comparison operators
+
+ Bool operator == (const Vec3 &a) const; // v == a?
+ Bool operator != (const Vec3 &a) const; // v != a?
+ Bool operator < (const Vec3 &a) const; // v < a?
+ Bool operator >= (const Vec3 &a) const; // v >= a?
+
+ // Arithmetic operators
+
+ Vec3 operator + (const Vec3 &a) const; // v + a
+ Vec3 operator - (const Vec3 &a) const; // v - a
+ Vec3 operator - () const; // -v
+ Vec3 operator * (const Vec3 &a) const; // v * a (vx * ax, ...)
+ Vec3 operator * (double s) const; // v * s
+ Vec3 operator / (const Vec3 &a) const; // v / a (vx / ax, ...)
+ Vec3 operator / (double s) const; // v / s
+
+ // Initialisers
+
+ Vec3 &MakeZero(); // Zero vector
+ Vec3 &MakeUnit(Int i, double k = vl_one); // I[i]
+ Vec3 &MakeBlock(double k = vl_one); // All-k vector
+
+ Vec3 &Normalise(); // normalise vector
+
+ // Private...
+
+protected:
+
+ double elt[3];
+};
+
+
+// --- Vec operators ----------------------------------------------------------
+
+inline Vec3 operator * (double s, const Vec3 &v); // s * v
+inline double dot(const Vec3 &a, const Vec3 &b); // v . a
+inline double len(const Vec3 &v); // || v ||
+inline double sqrlen(const Vec3 &v); // v . v
+inline Vec3 norm(const Vec3 &v); // v / || v ||
+inline Void normalise(Vec3 &v); // v = norm(v)
+inline Vec3 cross(const Vec3 &a, const Vec3 &b);// a x b
+inline Vec2 proj(const Vec3 &v); // hom. projection
+
+//std::ostream &operator << (std::ostream &s, const Vec3 &v);
+//std::istream &operator >> (std::istream &s, Vec3 &v);
+
+inline void printVec3(const Vec3 &v);
+
+
+// --- Inlines ----------------------------------------------------------------
+
+inline double &Vec3::operator [] (Int i)
+{
+ CheckRange(i, 0, 3, "(Vec3::[i]) index out of range");
+ return(elt[i]);
+}
+
+inline const double &Vec3::operator [] (Int i) const
+{
+ CheckRange(i, 0, 3, "(Vec3::[i]) index out of range");
+ return(elt[i]);
+}
+
+inline Vec3::Vec3()
+{
+}
+
+inline Vec3::Vec3(double x, double y, double z)
+{
+ elt[0] = x;
+ elt[1] = y;
+ elt[2] = z;
+}
+
+inline Vec3::Vec3(const Vec3 &v)
+{
+ elt[0] = v[0];
+ elt[1] = v[1];
+ elt[2] = v[2];
+}
+
+inline Vec3::Vec3(const Vec2 &v, double w)
+{
+ elt[0] = v[0];
+ elt[1] = v[1];
+ elt[2] = w;
+}
+
+inline double *Vec3::Ref() const
+{
+ return((double *) elt);
+}
+
+inline Vec3 &Vec3::operator = (const Vec3 &v)
+{
+ elt[0] = v[0];
+ elt[1] = v[1];
+ elt[2] = v[2];
+
+ return(SELF);
+}
+
+inline Vec3 &Vec3::operator += (const Vec3 &v)
+{
+ elt[0] += v[0];
+ elt[1] += v[1];
+ elt[2] += v[2];
+
+ return(SELF);
+}
+
+inline Vec3 &Vec3::operator -= (const Vec3 &v)
+{
+ elt[0] -= v[0];
+ elt[1] -= v[1];
+ elt[2] -= v[2];
+
+ return(SELF);
+}
+
+inline Vec3 &Vec3::operator *= (const Vec3 &a)
+{
+ elt[0] *= a[0];
+ elt[1] *= a[1];
+ elt[2] *= a[2];
+
+ return(SELF);
+}
+
+inline Vec3 &Vec3::operator *= (double s)
+{
+ elt[0] *= s;
+ elt[1] *= s;
+ elt[2] *= s;
+
+ return(SELF);
+}
+
+inline Vec3 &Vec3::operator /= (const Vec3 &a)
+{
+ elt[0] /= a[0];
+ elt[1] /= a[1];
+ elt[2] /= a[2];
+
+ return(SELF);
+}
+
+inline Vec3 &Vec3::operator /= (double s)
+{
+ elt[0] /= s;
+ elt[1] /= s;
+ elt[2] /= s;
+
+ return(SELF);
+}
+
+inline Vec3 Vec3::operator + (const Vec3 &a) const
+{
+ Vec3 result;
+
+ result[0] = elt[0] + a[0];
+ result[1] = elt[1] + a[1];
+ result[2] = elt[2] + a[2];
+
+ return(result);
+}
+
+inline Vec3 Vec3::operator - (const Vec3 &a) const
+{
+ Vec3 result;
+
+ result[0] = elt[0] - a[0];
+ result[1] = elt[1] - a[1];
+ result[2] = elt[2] - a[2];
+
+ return(result);
+}
+
+inline Vec3 Vec3::operator - () const
+{
+ Vec3 result;
+
+ result[0] = -elt[0];
+ result[1] = -elt[1];
+ result[2] = -elt[2];
+
+ return(result);
+}
+
+inline Vec3 Vec3::operator * (const Vec3 &a) const
+{
+ Vec3 result;
+
+ result[0] = elt[0] * a[0];
+ result[1] = elt[1] * a[1];
+ result[2] = elt[2] * a[2];
+
+ return(result);
+}
+
+inline Vec3 Vec3::operator * (double s) const
+{
+ Vec3 result;
+
+ result[0] = elt[0] * s;
+ result[1] = elt[1] * s;
+ result[2] = elt[2] * s;
+
+ return(result);
+}
+
+inline Vec3 Vec3::operator / (const Vec3 &a) const
+{
+ Vec3 result;
+
+ result[0] = elt[0] / a[0];
+ result[1] = elt[1] / a[1];
+ result[2] = elt[2] / a[2];
+
+ return(result);
+}
+
+inline Vec3 Vec3::operator / (double s) const
+{
+ Vec3 result;
+
+ result[0] = elt[0] / s;
+ result[1] = elt[1] / s;
+ result[2] = elt[2] / s;
+
+ return(result);
+}
+
+inline Vec3 operator * (double s, const Vec3 &v)
+{
+ return(v * s);
+}
+
+inline Vec3 &Vec3::MakeUnit(Int n, double k)
+{
+ if (n == 0)
+ { elt[0] = k; elt[1] = vl_zero; elt[2] = vl_zero; }
+ else if (n == 1)
+ { elt[0] = vl_zero; elt[1] = k; elt[2] = vl_zero; }
+ else if (n == 2)
+ { elt[0] = vl_zero; elt[1] = vl_zero; elt[2] = k; }
+ else
+ _Error("(Vec3::Unit) illegal unit vector");
+
+ return(SELF);
+}
+
+inline Vec3 &Vec3::MakeZero()
+{
+ elt[0] = vl_zero; elt[1] = vl_zero; elt[2] = vl_zero;
+
+ return(SELF);
+}
+
+inline Vec3 &Vec3::MakeBlock(double k)
+{
+ elt[0] = k; elt[1] = k; elt[2] = k;
+
+ return(SELF);
+}
+
+inline Vec3 &Vec3::Normalise()
+{
+ Assert(sqrlen(SELF) > 0.0, "normalising length-zero vector");
+ SELF /= len(SELF);
+
+ return(SELF);
+}
+
+
+inline Vec3::Vec3(ZeroOrOne k)
+{
+ elt[0] = k; elt[1] = k; elt[2] = k;
+}
+
+inline Vec3 &Vec3::operator = (ZeroOrOne k)
+{
+ elt[0] = k; elt[1] = k; elt[2] = k;
+
+ return(SELF);
+}
+
+inline Vec3::Vec3(Axis a)
+{
+ MakeUnit(a, vl_one);
+}
+
+
+inline Bool Vec3::operator == (const Vec3 &a) const
+{
+ return(elt[0] == a[0] && elt[1] == a[1] && elt[2] == a[2]);
+}
+
+inline Bool Vec3::operator != (const Vec3 &a) const
+{
+ return(elt[0] != a[0] || elt[1] != a[1] || elt[2] != a[2]);
+}
+
+inline Bool Vec3::operator < (const Vec3 &a) const
+{
+ return(elt[0] < a[0] && elt[1] < a[1] && elt[2] < a[2]);
+}
+
+inline Bool Vec3::operator >= (const Vec3 &a) const
+{
+ return(elt[0] >= a[0] && elt[1] >= a[1] && elt[2] >= a[2]);
+}
+
+
+inline double dot(const Vec3 &a, const Vec3 &b)
+{
+ return(a[0] * b[0] + a[1] * b[1] + a[2] * b[2]);
+}
+
+inline double len(const Vec3 &v)
+{
+ return(sqrt(dot(v, v)));
+}
+
+inline double sqrlen(const Vec3 &v)
+{
+ return(dot(v, v));
+}
+
+inline Vec3 norm(const Vec3 &v)
+{
+ Assert(sqrlen(v) > 0.0, "normalising length-zero vector");
+ return(v / len(v));
+}
+
+inline Void normalise(Vec3 &v)
+{
+ v /= len(v);
+}
+
+inline Vec3 cross(const Vec3 &a, const Vec3 &b)
+{
+ Vec3 result;
+
+ result[0] = a[1] * b[2] - a[2] * b[1];
+ result[1] = a[2] * b[0] - a[0] * b[2];
+ result[2] = a[0] * b[1] - a[1] * b[0];
+
+ return(result);
+}
+
+inline Vec2 proj(const Vec3 &v)
+{
+ Vec2 result;
+
+ Assert(v[2] != 0, "(Vec3/proj) last elt. is zero");
+
+ result[0] = v[0] / v[2];
+ result[1] = v[1] / v[2];
+
+ return(result);
+}
+
+inline void printVec3(const Vec3 &v)
+{
+ printf("[%10f %10f %10f]",v[0],v[1],v[2]);
+}
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Vec4.cpp Mon Jan 04 15:19:10 2016 +0000
@@ -0,0 +1,124 @@
+/*
+ File: Vec4.cpp
+
+ Function: Implements Vec4.h
+
+ Author(s): Andrew Willmott
+
+ Copyright: (c) 1995-2001, Andrew Willmott
+
+*/
+
+
+#include "Vec4.h"
+//#include <cctype>
+//#include <iomanip>
+
+
+Vec4 &Vec4::MakeUnit(Int n, Real k)
+{
+ if (n == 0)
+ { elt[0] = k; elt[1] = vl_zero; elt[2] = vl_zero; elt[3] = vl_zero; }
+ else if (n == 1)
+ { elt[0] = vl_zero; elt[1] = k; elt[2] = vl_zero; elt[3] = vl_zero; }
+ else if (n == 2)
+ { elt[0] = vl_zero; elt[1] = vl_zero; elt[2] = k; elt[3] = vl_zero; }
+ else if (n == 3)
+ { elt[0] = vl_zero; elt[1] = vl_zero; elt[2] = vl_zero; elt[3] = k; }
+ else
+ _Error("(Vec4::MakeUnit) illegal unit vector");
+
+ return(SELF);
+}
+
+bool Vec4::operator == (const Vec4 &a) const
+{
+ return(elt[0] == a[0] && elt[1] == a[1] && elt[2] == a[2] && elt[3] == a[3]);
+}
+
+bool Vec4::operator != (const Vec4 &a) const
+{
+ return(elt[0] != a[0] || elt[1] != a[1] || elt[2] != a[2] || elt[3] != a[3]);
+}
+
+Vec4 cross(const Vec4 &a, const Vec4 &b, const Vec4 &c)
+{
+ Vec4 result;
+ // XXX can this be improved? Look at assembly.
+#define ROW(i) a[i], b[i], c[i]
+#define DET(i,j,k) dot(Vec3(ROW(i)), cross(Vec3(ROW(j)), Vec3(ROW(k))))
+
+ result[0] = DET(1,2,3);
+ result[1] = -DET(0,2,3);
+ result[2] = DET(0,1,3);
+ result[3] = -DET(0,1,2);
+
+ return(result);
+
+#undef ROW
+#undef DET
+}
+
+Vec3 proj(const Vec4 &v)
+{
+ Vec3 result;
+
+ Assert(v[3] != 0, "(Vec4/proj) last elt. is zero");
+
+ result[0] = v[0] / v[3];
+ result[1] = v[1] / v[3];
+ result[2] = v[2] / v[3];
+
+ return(result);
+}
+
+/*
+ostream &operator << (ostream &s, const Vec4 &v)
+{
+ Int w = s.width();
+
+ return(s << '[' << v[0] << ' ' << setw(w) << v[1] << ' '
+ << setw(w) << v[2] << ' ' << setw(w) << v[3] << ']');
+}
+
+istream &operator >> (istream &s, Vec4 &v)
+{
+ Vec4 result;
+ Char c;
+
+ // Expected format: [1 2 3 4]
+
+ while (s >> c && isspace(c))
+ ;
+
+ if (c == '[')
+ {
+ s >> result[0] >> result[1] >> result[2] >> result[3];
+
+ if (!s)
+ {
+ cerr << "Error: Expected number while reading vector\n";
+ return(s);
+ }
+
+ while (s >> c && isspace(c))
+ ;
+
+ if (c != ']')
+ {
+ s.clear(ios::failbit);
+ cerr << "Error: Expected ']' while reading vector\n";
+ return(s);
+ }
+ }
+ else
+ {
+ s.clear(ios::failbit);
+ cerr << "Error: Expected '[' while reading vector\n";
+ return(s);
+ }
+
+ v = result;
+ return(s);
+}
+*/
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Vec4.h Mon Jan 04 15:19:10 2016 +0000
@@ -0,0 +1,386 @@
+/*
+ File: Vec4.h
+
+ Function: Defines a length-4 vector.
+
+ Author(s): Andrew Willmott
+
+ Copyright: (c) 1995-2001, Andrew Willmott
+ */
+
+#ifndef __Vec4__
+#define __Vec4__
+
+#include "Vec3.h"
+
+
+// --- Vec4 Class -------------------------------------------------------------
+
+class Vec4
+{
+public:
+
+ // Constructors
+
+ Vec4();
+ Vec4(Real x, Real y, Real z, Real w); // [x, y, z, w]
+ Vec4(const Vec4 &v); // Copy constructor
+ Vec4(const Vec3 &v, Real w); // Hom. 3D vector
+ Vec4(ZeroOrOne k);
+ Vec4(Axis k);
+
+ // Accessor functions
+
+ Int Elts() const { return(4); };
+
+ Real &operator [] (Int i);
+ const Real &operator [] (Int i) const;
+
+ Real *Ref() const; // Return pointer to data
+
+ // Assignment operators
+
+ Vec4 &operator = (const Vec4 &a);
+ Vec4 &operator = (ZeroOrOne k);
+ Vec4 &operator = (Axis k);
+ Vec4 &operator += (const Vec4 &a);
+ Vec4 &operator -= (const Vec4 &a);
+ Vec4 &operator *= (const Vec4 &a);
+ Vec4 &operator *= (Real s);
+ Vec4 &operator /= (const Vec4 &a);
+ Vec4 &operator /= (Real s);
+
+ // Comparison operators
+
+ Bool operator == (const Vec4 &a) const; // v == a ?
+ Bool operator != (const Vec4 &a) const; // v != a ?
+
+ // Arithmetic operators
+
+ Vec4 operator + (const Vec4 &a) const; // v + a
+ Vec4 operator - (const Vec4 &a) const; // v - a
+ Vec4 operator - () const; // -v
+ Vec4 operator * (const Vec4 &a) const; // v * a (vx * ax, ...)
+ Vec4 operator * (Real s) const; // v * s
+ Vec4 operator / (const Vec4 &a) const; // v / a (vx / ax, ...)
+ Vec4 operator / (Real s) const; // v / s
+
+
+ // Initialisers
+
+ Vec4 &MakeZero(); // Zero vector
+ Vec4 &MakeUnit(Int i, Real k = vl_one); // kI[i]
+ Vec4 &MakeBlock(Real k = vl_one); // All-k vector
+
+ Vec4 &Normalise(); // normalise vector
+
+ // Private...
+
+protected:
+
+ Real elt[4];
+};
+
+
+// --- Vec operators ----------------------------------------------------------
+
+inline Vec4 operator * (Real s, const Vec4 &v); // Left mult. by s
+inline Real dot(const Vec4 &a, const Vec4 &b); // v . a
+inline Real len(const Vec4 &v); // || v ||
+inline Real sqrlen(const Vec4 &v); // v . v
+inline Vec4 norm(const Vec4 &v); // v / || v ||
+inline Void normalise(Vec4 &v); // v = norm(v)
+Vec4 cross(const Vec4 &a, const Vec4 &b, const Vec4 &c);
+ // 4D cross prod.
+Vec3 proj(const Vec4 &v); // hom. projection
+
+//std::ostream &operator << (std::ostream &s, const Vec4 &v);
+//std::istream &operator >> (std::istream &s, Vec4 &v);
+
+inline void printVec4(const Vec4 &v);
+
+
+// --- Inlines ----------------------------------------------------------------
+
+inline Real &Vec4::operator [] (Int i)
+{
+ CheckRange(i, 0, 4, "(Vec4::[i]) index out of range");
+ return(elt[i]);
+}
+
+inline const Real &Vec4::operator [] (Int i) const
+{
+ CheckRange(i, 0, 4, "(Vec4::[i]) index out of range");
+ return(elt[i]);
+}
+
+
+inline Vec4::Vec4()
+{
+}
+
+inline Vec4::Vec4(Real x, Real y, Real z, Real w)
+{
+ elt[0] = x;
+ elt[1] = y;
+ elt[2] = z;
+ elt[3] = w;
+}
+
+inline Vec4::Vec4(const Vec4 &v)
+{
+ elt[0] = v[0];
+ elt[1] = v[1];
+ elt[2] = v[2];
+ elt[3] = v[3];
+}
+
+inline Vec4::Vec4(const Vec3 &v, Real w)
+{
+ elt[0] = v[0];
+ elt[1] = v[1];
+ elt[2] = v[2];
+ elt[3] = w;
+}
+
+inline Real *Vec4::Ref() const
+{
+ return((Real *) elt);
+}
+
+inline Vec4 &Vec4::operator = (const Vec4 &v)
+{
+ elt[0] = v[0];
+ elt[1] = v[1];
+ elt[2] = v[2];
+ elt[3] = v[3];
+
+ return(SELF);
+}
+
+inline Vec4 &Vec4::operator += (const Vec4 &v)
+{
+ elt[0] += v[0];
+ elt[1] += v[1];
+ elt[2] += v[2];
+ elt[3] += v[3];
+
+ return(SELF);
+}
+
+inline Vec4 &Vec4::operator -= (const Vec4 &v)
+{
+ elt[0] -= v[0];
+ elt[1] -= v[1];
+ elt[2] -= v[2];
+ elt[3] -= v[3];
+
+ return(SELF);
+}
+
+inline Vec4 &Vec4::operator *= (const Vec4 &v)
+{
+ elt[0] *= v[0];
+ elt[1] *= v[1];
+ elt[2] *= v[2];
+ elt[3] *= v[3];
+
+ return(SELF);
+}
+
+inline Vec4 &Vec4::operator *= (Real s)
+{
+ elt[0] *= s;
+ elt[1] *= s;
+ elt[2] *= s;
+ elt[3] *= s;
+
+ return(SELF);
+}
+
+inline Vec4 &Vec4::operator /= (const Vec4 &v)
+{
+ elt[0] /= v[0];
+ elt[1] /= v[1];
+ elt[2] /= v[2];
+ elt[3] /= v[3];
+
+ return(SELF);
+}
+
+inline Vec4 &Vec4::operator /= (Real s)
+{
+ elt[0] /= s;
+ elt[1] /= s;
+ elt[2] /= s;
+ elt[3] /= s;
+
+ return(SELF);
+}
+
+
+inline Vec4 Vec4::operator + (const Vec4 &a) const
+{
+ Vec4 result;
+
+ result[0] = elt[0] + a[0];
+ result[1] = elt[1] + a[1];
+ result[2] = elt[2] + a[2];
+ result[3] = elt[3] + a[3];
+
+ return(result);
+}
+
+inline Vec4 Vec4::operator - (const Vec4 &a) const
+{
+ Vec4 result;
+
+ result[0] = elt[0] - a[0];
+ result[1] = elt[1] - a[1];
+ result[2] = elt[2] - a[2];
+ result[3] = elt[3] - a[3];
+
+ return(result);
+}
+
+inline Vec4 Vec4::operator - () const
+{
+ Vec4 result;
+
+ result[0] = -elt[0];
+ result[1] = -elt[1];
+ result[2] = -elt[2];
+ result[3] = -elt[3];
+
+ return(result);
+}
+
+inline Vec4 Vec4::operator * (const Vec4 &a) const
+{
+ Vec4 result;
+
+ result[0] = elt[0] * a[0];
+ result[1] = elt[1] * a[1];
+ result[2] = elt[2] * a[2];
+ result[3] = elt[3] * a[3];
+
+ return(result);
+}
+
+inline Vec4 Vec4::operator * (Real s) const
+{
+ Vec4 result;
+
+ result[0] = elt[0] * s;
+ result[1] = elt[1] * s;
+ result[2] = elt[2] * s;
+ result[3] = elt[3] * s;
+
+ return(result);
+}
+
+inline Vec4 Vec4::operator / (const Vec4 &a) const
+{
+ Vec4 result;
+
+ result[0] = elt[0] / a[0];
+ result[1] = elt[1] / a[1];
+ result[2] = elt[2] / a[2];
+ result[3] = elt[3] / a[3];
+
+ return(result);
+}
+
+inline Vec4 Vec4::operator / (Real s) const
+{
+ Vec4 result;
+
+ result[0] = elt[0] / s;
+ result[1] = elt[1] / s;
+ result[2] = elt[2] / s;
+ result[3] = elt[3] / s;
+
+ return(result);
+}
+
+inline Vec4 operator * (Real s, const Vec4 &v)
+{
+ return(v * s);
+}
+
+inline Vec4 &Vec4::MakeZero()
+{
+ elt[0] = vl_zero; elt[1] = vl_zero; elt[2] = vl_zero; elt[3] = vl_zero;
+ return(SELF);
+}
+
+inline Vec4 &Vec4::MakeBlock(Real k)
+{
+ elt[0] = k; elt[1] = k; elt[2] = k; elt[3] = k;
+ return(SELF);
+}
+
+inline Vec4 &Vec4::Normalise()
+{
+ Assert(sqrlen(SELF) > 0.0, "normalising length-zero vector");
+ SELF /= len(SELF);
+ return(SELF);
+}
+
+inline Vec4::Vec4(ZeroOrOne k)
+{
+ MakeBlock(k);
+}
+
+inline Vec4::Vec4(Axis k)
+{
+ MakeUnit(k, vl_1);
+}
+
+inline Vec4 &Vec4::operator = (ZeroOrOne k)
+{
+ MakeBlock(k);
+
+ return(SELF);
+}
+
+inline Vec4 &Vec4::operator = (Axis k)
+{
+ MakeUnit(k, vl_1);
+
+ return(SELF);
+}
+
+
+inline Real dot(const Vec4 &a, const Vec4 &b)
+{
+ return(a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + a[3] * b[3]);
+}
+
+inline Real len(const Vec4 &v)
+{
+ return(sqrt(dot(v, v)));
+}
+
+inline Real sqrlen(const Vec4 &v)
+{
+ return(dot(v, v));
+}
+
+inline Vec4 norm(const Vec4 &v)
+{
+ Assert(sqrlen(v) > 0.0, "normalising length-zero vector");
+ return(v / len(v));
+}
+
+inline Void normalise(Vec4 &v)
+{
+ v /= len(v);
+}
+
+inline void printVec4(const Vec4 &v)
+{
+ printf("[%10f %10f %10f %10f]",v[0],v[1],v[2],v[3]);
+}
+
+#endif