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