Update quaternion.h

typo fix for quaternion multiplication

DirectXTK/Src/Geometry.cpp

@@ -14,6 +14,7 @@
 #include "pch.h"
 #include "Geometry.h"
 #include "Bezier.h"
+#include <stdexcept>
 
 using namespace DirectX;
 

Simulations/DiffusionSimulator.cpp

@@ -0,0 +1,159 @@
+#include "DiffusionSimulator.h"
+#include "pcgsolver.h"
+using namespace std;
+
+Grid::Grid() {
+}
+
+
+DiffusionSimulator::DiffusionSimulator()
+{
+	m_iTestCase = 0;
+	m_vfMovableObjectPos = Vec3();
+	m_vfMovableObjectFinalPos = Vec3();
+	m_vfRotate = Vec3();
+	// to be implemented
+}
+
+const char * DiffusionSimulator::getTestCasesStr(){
+	return "Explicit_solver, Implicit_solver";
+}
+
+void DiffusionSimulator::reset(){
+		m_mouse.x = m_mouse.y = 0;
+		m_trackmouse.x = m_trackmouse.y = 0;
+		m_oldtrackmouse.x = m_oldtrackmouse.y = 0;
+
+}
+
+void DiffusionSimulator::initUI(DrawingUtilitiesClass * DUC)
+{
+	this->DUC = DUC;
+	// to be implemented
+}
+
+void DiffusionSimulator::notifyCaseChanged(int testCase)
+{
+	m_iTestCase = testCase;
+	m_vfMovableObjectPos = Vec3(0, 0, 0);
+	m_vfRotate = Vec3(0, 0, 0);
+	//
+	//to be implemented
+	//
+	switch (m_iTestCase)
+	{
+	case 0:
+		cout << "Explicit solver!\n";
+		break;
+	case 1:
+		cout << "Implicit solver!\n";
+		break;
+	default:
+		cout << "Empty Test!\n";
+		break;
+	}
+}
+
+Grid* DiffusionSimulator::diffuseTemperatureExplicit() {//add your own parameters
+	Grid* newT = new Grid();
+	// to be implemented
+	//make sure that the temperature in boundary cells stays zero
+	return newT;
+}
+
+void setupB(std::vector<Real>& b) {//add your own parameters
+	// to be implemented
+	//set vector B[sizeX*sizeY]
+	for (int i = 0; i < 25; i++) {
+		b.at(i) = 0;
+	}
+}
+
+void fillT() {//add your own parameters
+	// to be implemented
+	//fill T with solved vector x
+	//make sure that the temperature in boundary cells stays zero
+}
+
+void setupA(SparseMatrix<Real>& A, double factor) {//add your own parameters
+	// to be implemented
+	//setup Matrix A[sizeX*sizeY*sizeZ, sizeX*sizeY*sizeZ]
+	// set with:  A.set_element( index1, index2 , value );
+	// if needed, read with: A(index1, index2);
+	// avoid zero rows in A -> set the diagonal value for boundary cells to 1.0
+	for (int i = 0; i < 25; i++) {
+			A.set_element(i, i, 1); // set diagonal
+	}
+}
+
+
+void DiffusionSimulator::diffuseTemperatureImplicit() {//add your own parameters
+	// solve A T = b
+	// to be implemented
+	const int N = 25;//N = sizeX*sizeY*sizeZ
+	SparseMatrix<Real> *A = new SparseMatrix<Real> (N);
+	std::vector<Real> *b = new std::vector<Real>(N);
+
+	setupA(*A, 0.1);
+	setupB(*b);
+
+	// perform solve
+	Real pcg_target_residual = 1e-05;
+	Real pcg_max_iterations = 1000;
+	Real ret_pcg_residual = 1e10;
+	int  ret_pcg_iterations = -1;
+
+	SparsePCGSolver<Real> solver;
+	solver.set_solver_parameters(pcg_target_residual, pcg_max_iterations, 0.97, 0.25);
+
+	std::vector<Real> x(N);
+	for (int j = 0; j < N; ++j) { x[j] = 0.; }
+
+	// preconditioners: 0 off, 1 diagonal, 2 incomplete cholesky
+	solver.solve(*A, *b, x, ret_pcg_residual, ret_pcg_iterations, 0);
+	// x contains the new temperature values
+	fillT();//copy x to T
+}
+
+
+
+void DiffusionSimulator::simulateTimestep(float timeStep)
+{
+	// to be implemented
+	// update current setup for each frame
+	switch (m_iTestCase)
+	{
+	case 0:
+		T = diffuseTemperatureExplicit();
+		break;
+	case 1:
+		diffuseTemperatureImplicit();
+		break;
+	}
+}
+
+void DiffusionSimulator::drawObjects()
+{
+	// to be implemented
+	//visualization
+}
+
+
+void DiffusionSimulator::drawFrame(ID3D11DeviceContext* pd3dImmediateContext)
+{
+	drawObjects();
+}
+
+void DiffusionSimulator::onClick(int x, int y)
+{
+	m_trackmouse.x = x;
+	m_trackmouse.y = y;
+}
+
+void DiffusionSimulator::onMouse(int x, int y)
+{
+	m_oldtrackmouse.x = x;
+	m_oldtrackmouse.y = y;
+	m_trackmouse.x = x;
+	m_trackmouse.y = y;
+}

Simulations/DiffusionSimulator.h

@@ -0,0 +1,51 @@
+#ifndef DIFFUSIONSIMULATOR_h
+#define DIFFUSIONSIMULATOR_h
+
+#include "Simulator.h"
+#include "vectorbase.h"
+
+//impement your own grid class for saving grid data
+class Grid {
+public:
+	// Construtors
+	Grid();
+
+
+private:
+	// Attributes
+};
+
+
+
+class DiffusionSimulator:public Simulator{
+public:
+	// Construtors
+	DiffusionSimulator();
+
+	// Functions
+	const char * getTestCasesStr();
+	void initUI(DrawingUtilitiesClass * DUC);
+	void reset();
+	void drawFrame(ID3D11DeviceContext* pd3dImmediateContext);
+	void notifyCaseChanged(int testCase);
+	void simulateTimestep(float timeStep);
+	void externalForcesCalculations(float timeElapsed) {};
+	void onClick(int x, int y);
+	void onMouse(int x, int y);
+	// Specific Functions
+	void drawObjects();
+	Grid* diffuseTemperatureExplicit();
+	void diffuseTemperatureImplicit();
+
+private:
+	// Attributes
+	Vec3  m_vfMovableObjectPos;
+	Vec3  m_vfMovableObjectFinalPos;
+	Vec3  m_vfRotate;
+	Point2D m_mouse;
+	Point2D m_trackmouse;
+	Point2D m_oldtrackmouse;
+	Grid *T; //save results of every time step
+};
+
+#endif

Simulations/SphereSystemSimulator.cpp

@@ -0,0 +1,11 @@
+#include "SphereSystemSimulator.h"
+
+std::function<float(float)> SphereSystemSimulator::m_Kernels[5] = {
+	[](float x) {return 1.0f; },              // Constant, m_iKernel = 0
+	[](float x) {return 1.0f - x; },          // Linear, m_iKernel = 1, as given in the exercise Sheet, x = d/2r
+	[](float x) {return (1.0f - x)*(1.0f - x); }, // Quadratic, m_iKernel = 2
+	[](float x) {return 1.0f / (x)-1.0f; },     // Weak Electric Charge, m_iKernel = 3
+	[](float x) {return 1.0f / (x*x) - 1.0f; },   // Electric Charge, m_iKernel = 4
+};
+
+// SphereSystemSimulator member functions

Simulations/SphereSystemSimulator.h

@@ -0,0 +1,49 @@
+#ifndef SPHSYSTEMSIMULATOR_h
+#define SPHSYSTEMSIMULATOR_h
+#include "Simulator.h"
+//#include "spheresystem.h", add your sphere system header file
+
+#define NAIVEACC 0
+#define GRIDACC 1
+
+class SphereSystemSimulator:public Simulator{
+public:
+	// Construtors
+	SphereSystemSimulator();
+	// Functions
+	const char * getTestCasesStr();
+	void initUI(DrawingUtilitiesClass * DUC);
+	void reset();
+	void drawFrame(ID3D11DeviceContext* pd3dImmediateContext);
+	void notifyCaseChanged(int testCase);
+	void externalForcesCalculations(float timeElapsed);
+	void simulateTimestep(float timeStep);
+	void onClick(int x, int y);
+	void onMouse(int x, int y);
+	
+protected:
+	// Attributes
+	Vec3 externalForce;
+	Point2D m_mouse;
+	Point2D m_trackmouse;
+	Point2D m_oldtrackmouse;
+	float m_fMass;
+	float m_fRadius;
+	float m_fForceScaling;
+	float m_fDamping;
+	int   m_iNumSpheres;
+	
+	int   m_iKernel; // index of the m_Kernels[5], more detials in SphereSystemSimulator.cpp
+	static std::function<float(float)> m_Kernels[5];
+	
+	int   m_iAccelerator; // switch between NAIVEACC and GRIDACC, (optionally, KDTREEACC, 2)
+	
+	//SphereSystem * m_pSphereSystem; // add your own sphere system member!
+	// for Demo 3 only:
+	// you will need multiple SphereSystem objects to do comparisons in Demo 3
+	// m_iAccelerator should be ignored.
+	// SphereSystem * m_pSphereSystemGrid; 
+
+};
+
+#endif

Simulations/general.h

@@ -0,0 +1,160 @@
+/******************************************************************************
+ *
+ * MantaFlow fluid solver framework
+ * Copyright 2011 Tobias Pfaff, Nils Thuerey 
+ *
+ * This program is free software, distributed under the terms of the
+ * GNU General Public License (GPL) 
+ * http://www.gnu.org/licenses
+ *
+ * Globally used macros and functions
+ *
+ ******************************************************************************/
+
+#ifndef _GENERAL_H
+#define _GENERAL_H
+
+#include <iostream>
+#include <sstream>
+#include <cmath>
+#include <algorithm>
+
+namespace Manta {
+
+// ui data exchange
+#ifdef GUI
+	// defined in qtmain.cpp
+	extern void updateQtGui(bool full, int frame, float time, const std::string& curPlugin);
+#else
+	// dummy function if GUI is not enabled
+	inline void updateQtGui(bool full, int frame, float time, const std::string& curPlugin) {}
+#endif
+
+
+// activate debug mode if _DEBUG is defined (eg for windows)
+#ifndef DEBUG
+#ifdef _DEBUG
+#define DEBUG 1
+#endif // _DEBUG
+#endif // DEBUG
+
+// Standard exception
+class Error : public std::exception
+{
+public:
+   Error(const std::string& s) : mS(s) {
+#		ifdef DEBUG
+		// print error
+		std::cerr << "Aborting: "<< s <<" \n";
+		// then force immedieate crash in debug mode
+		*(volatile int*)(0) = 1; 
+#		endif
+   }
+   virtual ~Error() throw() {}
+   virtual const char* what() const throw() { return mS.c_str(); }
+private:
+   std::string mS;   
+};
+
+// mark unused parameter variables
+#define unusedParameter(x) ((void)x)
+
+// Debug output functions and macros
+extern int gDebugLevel;
+
+#define MSGSTREAM std::ostringstream msg; msg.precision(7); msg.width(9);
+#define debMsg(mStr, level)     if (_chklevel(level)) { MSGSTREAM; msg << mStr; std::cout << msg.str()  << std::endl; }
+inline bool _chklevel(int level=0) { return gDebugLevel >= level; }
+
+// error and assertation macros
+#ifdef DEBUG
+#   define DEBUG_ONLY(a) a
+#else
+#   define DEBUG_ONLY(a)
+#endif
+#define throwError(msg)      { std::ostringstream __s; __s << msg << std::endl << "Error raised in " << __FILE__ << ":" << __LINE__; throw Manta::Error(__s.str()); }
+#define errMsg(msg)          throwError(msg);
+#define assertMsg(cond,msg)  if(!(cond)) throwError(msg)
+#define assertDeb(cond,msg)  DEBUG_ONLY( assertMsg(cond,msg) )
+
+// for compatibility with blender, blender only defines WITH_MANTA, make sure we have "BLENDER"
+#ifndef BLENDER
+#ifdef WITH_MANTA
+#define BLENDER 1
+#endif
+#endif
+
+// common type for indexing large grids
+typedef long long IndexInt;
+
+// template tricks
+template<typename T>
+struct remove_pointers {
+	typedef T type;
+};
+
+template<typename T>
+struct remove_pointers<T*> {
+	typedef T type;
+};
+
+template<typename T>
+struct remove_pointers<T&> {
+	typedef T type;
+};
+
+// Commonly used enums and types
+//! Timing class for preformance measuring
+struct MuTime {
+	MuTime() { get(); }
+	MuTime operator-(const MuTime& a) { MuTime b; b.time = time - a.time; return b; };
+	MuTime operator+(const MuTime& a) { MuTime b; b.time = time + a.time; return b; };
+	MuTime operator/(unsigned long a) { MuTime b; b.time = time / a; return b; };
+	MuTime& operator+=(const MuTime& a) { time += a.time; return *this; }
+	MuTime& operator-=(const MuTime& a) { time -= a.time; return *this; }
+	MuTime& operator/=(unsigned long a) { time /= a; return *this; }
+	std::string toString();
+	
+    void clear() { time = 0; }
+	void get();
+	MuTime update();
+	
+	unsigned long time;
+};
+std::ostream& operator<< (std::ostream& os, const MuTime& t);
+	
+//! generate a string with infos about the current mantaflow build
+std::string buildInfoString();
+
+// Some commonly used math helpers
+template<class T> inline T square(T a) {
+	return a*a;
+}
+template<class T> inline T cubed(T a) {
+	return a*a*a;
+}
+
+template<class T> inline T clamp(const T& val, const T& vmin, const T& vmax) {
+	if (val < vmin) return vmin;
+	if (val > vmax) return vmax;
+	return val;
+}
+
+template<class T> inline T      nmod(const T& a, const T& b);
+template<>        inline int    nmod(const int& a, const int& b) { int c=a%b; return (c<0) ? (c+b) : c; }
+template<>        inline float  nmod(const float& a, const float& b) { float c=std::fmod(a,b); return (c<0) ? (c+b) : c; }
+template<>        inline double nmod(const double& a, const double& b) { double c=std::fmod(a,b); return (c<0) ? (c+b) : c; }
+
+template<class T> inline T      safeDivide(const T& a, const T& b);
+template<>        inline int    safeDivide<int>(const int &a, const int& b) { return (b) ? (a/b) : a; }
+template<>        inline float  safeDivide<float>(const float &a, const float& b) { return (b) ? (a/b) : a; }
+template<>        inline double safeDivide<double>(const double &a, const double& b) { return (b) ? (a/b) : a; }
+
+inline bool c_isnan(float c) { 
+	volatile float d=c;
+	return d != d;
+}
+
+} // namespace
+
+#endif

Simulations/main.cpp

@@ -20,10 +20,11 @@ using namespace GamePhysics;
 
 //#define ADAPTIVESTEP
 
-#define TEMPLATE_DEMO
+//#define TEMPLATE_DEMO
 //#define MASS_SPRING_SYSTEM
 //#define RIGID_BODY_SYSTEM
 //#define SPH_SYSTEM
+#define DIFFUSION_SYSTEM
 
 #ifdef TEMPLATE_DEMO
 #include "TemplateSimulator.h"
@@ -38,6 +39,10 @@ using namespace GamePhysics;
 //#include "SPHSystemSimulator.h"
 #endif
 
+#ifdef DIFFUSION_SYSTEM
+#include "DiffusionSimulator.h"
+#endif
+
 DrawingUtilitiesClass * g_pDUC;
 Simulator * g_pSimulator;
 float 	g_fTimestep = 0.001;
@@ -369,6 +374,9 @@ int main(int argc, char* argv[])
 #endif
 #ifdef SPH_SYSTEM
 	//g_pSimulator= new SPHSystemSimulator();
+#endif
+#ifdef DIFFUSION_SYSTEM
+	g_pSimulator= new DiffusionSimulator();
 #endif
 	g_pSimulator->reset();
 

Simulations/pcgsolver.h

@@ -0,0 +1,750 @@
+//
+//  Preconditioned conjugate gradient solver
+//
+//  Created by Robert Bridson, Ryoichi Ando and Nils Thuerey
+//
+
+#ifndef RCMATRIX3_H
+#define RCMATRIX3_H
+
+#include <iterator>
+#include <cassert>
+#include <vector>
+#include <fstream>
+#include <cmath>
+#include <functional>
+
+// index type 
+#define int_index long long
+
+// parallelization disabled
+
+#define parallel_for(size)  { int thread_number = 0; int_index parallel_index=0; for( int_index parallel_index=0; parallel_index<(int_index)size; parallel_index++ ) {
+#define parallel_end 	} thread_number=parallel_index=0; }
+
+#define parallel_block 
+#define do_parallel 
+#define do_end
+#define block_end
+
+#include "vectorbase.h"
+
+// note - "Int" instead of "N" here, the latter is size!
+template<class Int, class T>
+struct InstantBLAS {
+	static inline Int offset(Int N, Int incX) { return ((incX) > 0 ?  0 : ((N) - 1) * (-(incX))); }
+	static T cblas_ddot( const Int N, const T *X, const Int incX, const T *Y, const Int incY) {
+		double r  = 0.0; // always use double precision internally here...
+		Int    i;
+		Int    ix = offset(N,incX);
+		Int    iy = offset(N,incY);
+		for (i = 0; i < N; i++) {
+			r  += X[ix] * Y[iy];
+			ix += incX;
+			iy += incY;
+		}
+		return (T)r;
+	}
+	static void cblas_daxpy( const Int N, const T alpha, const T *X, const Int incX, T *Y, const Int incY) {
+		Int i;
+		if (N     <= 0  ) return;
+		if (alpha == 0.0) return;
+		if (incX == 1 && incY == 1) {
+			const Int m = N % 4;
+			for (i = 0; i < m; i++)
+				Y[i] += alpha * X[i];
+			for (i = m; i + 3 < N; i += 4) {
+				Y[i    ] += alpha * X[i    ];
+				Y[i + 1] += alpha * X[i + 1];
+				Y[i + 2] += alpha * X[i + 2];
+				Y[i + 3] += alpha * X[i + 3];
+			}
+		} else {
+			Int ix = offset(N, incX);
+			Int iy = offset(N, incY);
+			for (i = 0; i < N; i++) {
+				Y[iy] += alpha * X[ix];
+				ix    += incX;
+				iy    += incY;
+			}
+		}
+	}
+	// dot products ==============================================================
+	static inline T dot(const std::vector<T> &x, const std::vector<T> &y) {
+		return cblas_ddot((int)x.size(), &x[0], 1, &y[0], 1);
+	}
+	
+	// inf-norm (maximum absolute value: index of max returned) ==================
+	static inline Int index_abs_max(const std::vector<T> &x) {
+		int maxind = 0;
+		T maxvalue = 0;
+		for(Int i = 0; i < (Int)x.size(); ++i) {
+			if(std::abs(x[i]) > maxvalue) {
+				maxvalue = fabs(x[i]);
+				maxind = i;
+			}
+		}
+		return maxind;
+	}
+	
+	// inf-norm (maximum absolute value) =========================================
+	// technically not part of BLAS, but useful
+	static inline T abs_max(const std::vector<T> &x)
+	{ return std::abs(x[index_abs_max(x)]); }
+	
+	// saxpy (y=alpha*x+y) =======================================================
+	static inline void add_scaled(T alpha, const std::vector<T> &x, std::vector<T> &y) {
+		cblas_daxpy((Int)x.size(), alpha, &x[0], 1, &y[0], 1);
+	}
+};
+
+
+
+
+template<class T>
+void zero(std::vector<T> &v)
+{ for(int i=(int)v.size()-1; i>=0; --i) v[i]=0; }
+
+template<class T>
+void insert(std::vector<T> &a, unsigned int index, T e)
+{
+   a.push_back(a.back());
+   for(unsigned int i=(unsigned int)a.size()-1; i>index; --i)
+      a[i]=a[i-1];
+   a[index]=e;
+}
+
+template<class T>
+void erase(std::vector<T> &a, unsigned int index)
+{
+   for(unsigned int i=index; i<a.size()-1; ++i)
+      a[i]=a[i+1];
+   a.pop_back();
+}
+
+//============================================================================
+// Dynamic compressed sparse row matrix.
+
+template<class T>
+struct SparseMatrix
+{
+   int n; // dimension
+   std::vector<std::vector<int> > index; // for each row, a list of all column indices (sorted)
+   std::vector<std::vector<T> > value; // values corresponding to index
+
+   explicit SparseMatrix(int n_=0, int expected_nonzeros_per_row=7)
+      : n(n_), index(n_), value(n_)
+   {
+      for(int i=0; i<n; ++i){
+         index[i].reserve(expected_nonzeros_per_row);
+         value[i].reserve(expected_nonzeros_per_row);
+      }
+   }
+
+   void clear(void)
+   {
+      n=0;
+      index.clear();
+      value.clear();
+   }
+
+   void zero(void)
+   {
+      for(int i=0; i<n; ++i){
+         index[i].resize(0);
+         value[i].resize(0);
+      }
+   }
+
+   void resize(int n_)
+   {
+      n=n_;
+      index.resize(n);
+      value.resize(n);
+   }
+
+   T operator()(int i, int j) const
+   {
+      for(int k=0; k<(int)index[i].size(); ++k){
+         if(index[i][k]==j) return value[i][k];
+         else if(index[i][k]>j) return 0;
+      }
+      return 0;
+   }
+
+   void set_element(int i, int j, T new_value)
+   {
+      int k=0;
+      for(; k<(int)index[i].size(); ++k){
+         if(index[i][k]==j){
+            value[i][k]=new_value;
+            return;
+         }else if(index[i][k]>j){
+            insert(index[i], k, j);
+            insert(value[i], k, new_value);
+            return;
+         }
+      }
+      index[i].push_back(j);
+      value[i].push_back(new_value);
+   }
+
+   void add_to_element(int i, int j, T increment_value)
+   {
+      int k=0;
+      for(; k<(int)index[i].size(); ++k){
+         if(index[i][k]==j){
+            value[i][k]+=increment_value;
+            return;
+         }else if(index[i][k]>j){
+            insert(index[i], k, j);
+            insert(value[i], k, increment_value);
+            return;
+         }
+      }
+      index[i].push_back(j);
+      value[i].push_back(increment_value);
+   }
+
+   // assumes indices is already sorted
+   void add_sparse_row(int i, const std::vector<int> &indices, const std::vector<T> &values)
+   {
+      int j=0, k=0;
+      while(j<indices.size() && k<(int)index[i].size()){
+         if(index[i][k]<indices[j]){
+            ++k;
+         }else if(index[i][k]>indices[j]){
+            insert(index[i], k, indices[j]);
+            insert(value[i], k, values[j]);
+            ++j;
+         }else{
+            value[i][k]+=values[j];
+            ++j;
+            ++k;
+         }
+      }
+      for(;j<indices.size(); ++j){
+         index[i].push_back(indices[j]);
+         value[i].push_back(values[j]);
+      }
+   }
+
+   // assumes matrix has symmetric structure - so the indices in row i tell us which columns to delete i from
+   void symmetric_remove_row_and_column(int i)
+   {
+      for(int a=0; a<index[i].size(); ++a){
+         int j=index[i][a]; // 
+         for(int b=0; b<index[j].size(); ++b){
+            if(index[j][b]==i){
+               erase(index[j], b);
+               erase(value[j], b);
+               break;
+            }
+         }
+      }
+      index[i].resize(0);
+      value[i].resize(0);
+   }
+
+   void write_matlab(std::ostream &output, const char *variable_name)
+   {
+      output<<variable_name<<"=sparse([";
+      for(int i=0; i<n; ++i){
+         for(int j=0; j<index[i].size(); ++j){
+            output<<i+1<<" ";
+         }
+      }
+      output<<"],...\n  [";
+      for(int i=0; i<n; ++i){
+         for(int j=0; j<index[i].size(); ++j){
+            output<<index[i][j]+1<<" ";
+         }
+      }
+      output<<"],...\n  [";
+      for(int i=0; i<n; ++i){
+         for(int j=0; j<value[i].size(); ++j){
+            output<<value[i][j]<<" ";
+         }
+      }
+      output<<"], "<<n<<", "<<n<<");"<<std::endl;
+   }
+};
+
+typedef SparseMatrix<float> SparseMatrixf;
+typedef SparseMatrix<double> SparseMatrixd;
+
+// perform result=matrix*x
+template<class T>
+void multiply(const SparseMatrix<T> &matrix, const std::vector<T> &x, std::vector<T> &result)
+{
+   assert(matrix.n==x.size());
+   result.resize(matrix.n);
+   //for(int i=0; i<matrix.n; ++i)
+	parallel_for(matrix.n) {
+		unsigned i (parallel_index);
+		T value=0;
+		for(int j=0; j<(int)matrix.index[i].size(); ++j){
+			value+=matrix.value[i][j]*x[matrix.index[i][j]];
+		}
+		result[i]=value;
+	} parallel_end
+}
+
+// perform result=result-matrix*x
+template<class T>
+void multiply_and_subtract(const SparseMatrix<T> &matrix, const std::vector<T> &x, std::vector<T> &result)
+{
+   assert(matrix.n==x.size());
+   result.resize(matrix.n);
+   for(int i=0; i<(int)matrix.n; ++i){
+      for(int j=0; j<(int)matrix.index[i].size(); ++j){
+         result[i]-=matrix.value[i][j]*x[matrix.index[i][j]];
+      }
+   }
+}
+
+//============================================================================
+// Fixed version of SparseMatrix. This is not a good structure for dynamically
+// modifying the matrix, but can be significantly faster for matrix-vector
+// multiplies due to better data locality.
+
+template<class T>
+struct FixedSparseMatrix
+{
+   int n; // dimension
+   std::vector<T> value; // nonzero values row by row
+   std::vector<int> colindex; // corresponding column indices
+   std::vector<int> rowstart; // where each row starts in value and colindex (and last entry is one past the end, the number of nonzeros)
+
+   explicit FixedSparseMatrix(int n_=0)
+      : n(n_), value(0), colindex(0), rowstart(n_+1)
+   {}
+
+   void clear(void)
+   {
+      n=0;
+      value.clear();
+      colindex.clear();
+      rowstart.clear();
+   }
+
+   void resize(int n_)
+   {
+      n=n_;
+      rowstart.resize(n+1);
+   }
+
+   void construct_from_matrix(const SparseMatrix<T> &matrix)
+   {
+      resize(matrix.n);
+      rowstart[0]=0;
+      for(int i=0; i<n; ++i){
+         rowstart[i+1]=rowstart[i]+matrix.index[i].size();
+      }
+      value.resize(rowstart[n]);
+      colindex.resize(rowstart[n]);
+      int j=0;
+      for(int i=0; i<n; ++i){
+         for(int k=0; k<(int)matrix.index[i].size(); ++k){
+            value[j]=matrix.value[i][k];
+            colindex[j]=matrix.index[i][k];
+            ++j;
+         }
+      }
+   }
+
+   void write_matlab(std::ostream &output, const char *variable_name)
+   {
+      output<<variable_name<<"=sparse([";
+      for(int i=0; i<n; ++i){
+         for(int j=rowstart[i]; j<rowstart[i+1]; ++j){
+            output<<i+1<<" ";
+         }
+      }
+      output<<"],...\n  [";
+      for(int i=0; i<n; ++i){
+         for(int j=rowstart[i]; j<rowstart[i+1]; ++j){
+            output<<colindex[j]+1<<" ";
+         }
+      }
+      output<<"],...\n  [";
+      for(int i=0; i<n; ++i){
+         for(int j=rowstart[i]; j<rowstart[i+1]; ++j){
+            output<<value[j]<<" ";
+         }
+      }
+      output<<"], "<<n<<", "<<n<<");"<<std::endl;
+   }
+};
+
+
+// perform result=matrix*x
+template<class T>
+void multiply(const FixedSparseMatrix<T> &matrix, const std::vector<T> &x, std::vector<T> &result)
+{
+   assert(matrix.n==x.size());
+   result.resize(matrix.n);
+   //for(int i=0; i<matrix.n; ++i)
+	parallel_for(matrix.n) {
+		unsigned i (parallel_index);
+		T value=0;
+		for(int j=matrix.rowstart[i]; j<matrix.rowstart[i+1]; ++j){
+			value+=matrix.value[j]*x[matrix.colindex[j]];
+		}
+		result[i]=value;
+	} parallel_end
+}
+
+// perform result=result-matrix*x
+template<class T>
+void multiply_and_subtract(const FixedSparseMatrix<T> &matrix, const std::vector<T> &x, std::vector<T> &result)
+{
+   assert(matrix.n==x.size());
+   result.resize(matrix.n);
+   for(int i=0; i<matrix.n; ++i){
+      for(int j=matrix.rowstart[i]; j<matrix.rowstart[i+1]; ++j){
+         result[i]-=matrix.value[j]*x[matrix.colindex[j]];
+      }
+   }
+}
+
+//============================================================================
+// A simple compressed sparse column data structure (with separate diagonal)
+// for lower triangular matrices
+
+template<class T>
+struct SparseColumnLowerFactor
+{
+   int n;
+   std::vector<T> invdiag; // reciprocals of diagonal elements
+   std::vector<T> value; // values below the diagonal, listed column by column
+   std::vector<int> rowindex; // a list of all row indices, for each column in turn
+   std::vector<int> colstart; // where each column begins in rowindex (plus an extra entry at the end, of #nonzeros)
+   std::vector<T> adiag; // just used in factorization: minimum "safe" diagonal entry allowed
+
+   explicit SparseColumnLowerFactor(int n_=0)
+      : n(n_), invdiag(n_), colstart(n_+1), adiag(n_)
+   {}
+
+   void clear(void)
+   {
+      n=0;
+      invdiag.clear();
+      value.clear();
+      rowindex.clear();
+      colstart.clear();
+      adiag.clear();
+   }
+
+   void resize(int n_)
+   {
+      n=n_;
+      invdiag.resize(n);
+      colstart.resize(n+1);
+      adiag.resize(n);
+   }
+
+   void write_matlab(std::ostream &output, const char *variable_name)
+   {
+      output<<variable_name<<"=sparse([";
+      for(int i=0; i<n; ++i){
+         output<<" "<<i+1;
+         for(int j=colstart[i]; j<colstart[i+1]; ++j){
+            output<<" "<<rowindex[j]+1;
+         }
+      }
+      output<<"],...\n  [";
+      for(int i=0; i<n; ++i){
+         output<<" "<<i+1;
+         for(int j=colstart[i]; j<colstart[i+1]; ++j){
+            output<<" "<<i+1;
+         }
+      }
+      output<<"],...\n  [";
+      for(int i=0; i<n; ++i){
+         output<<" "<<(invdiag[i]!=0 ? 1/invdiag[i] : 0);
+         for(int j=colstart[i]; j<colstart[i+1]; ++j){
+            output<<" "<<value[j];
+         }
+      }
+      output<<"], "<<n<<", "<<n<<");"<<std::endl;
+   }
+};
+
+//============================================================================
+// Incomplete Cholesky factorization, level zero, with option for modified version.
+// Set modification_parameter between zero (regular incomplete Cholesky) and
+// one (fully modified version), with values close to one usually giving the best
+// results. The min_diagonal_ratio parameter is used to detect and correct
+// problems in factorization: if a pivot is this much less than the diagonal
+// entry from the original matrix, the original matrix entry is used instead.
+
+template<class T>
+void factor_modified_incomplete_cholesky0(const SparseMatrix<T> &matrix, SparseColumnLowerFactor<T> &factor,
+                                          T modification_parameter=0.97, T min_diagonal_ratio=0.25)
+{
+   // first copy lower triangle of matrix into factor (Note: assuming A is symmetric of course!)
+   factor.resize(matrix.n);
+   zero(factor.invdiag); // important: eliminate old values from previous solves!
+   factor.value.resize(0);
+   factor.rowindex.resize(0);
+   zero(factor.adiag);
+   for(int i=0; i<matrix.n; ++i){
+      factor.colstart[i]=(int)factor.rowindex.size();
+      for(int j=0; j<(int)matrix.index[i].size(); ++j){
+         if(matrix.index[i][j]>i){
+            factor.rowindex.push_back(matrix.index[i][j]);
+            factor.value.push_back(matrix.value[i][j]);
+         }else if(matrix.index[i][j]==i){
+            factor.invdiag[i]=factor.adiag[i]=matrix.value[i][j];
+         }
+      }
+   }
+   factor.colstart[matrix.n]=(int)factor.rowindex.size();
+   // now do the incomplete factorization (figure out numerical values)
+
+   // MATLAB code:
+   // L=tril(A);
+   // for k=1:size(L,2)
+   //   L(k,k)=sqrt(L(k,k));
+   //   L(k+1:end,k)=L(k+1:end,k)/L(k,k);
+   //   for j=find(L(:,k))'
+   //     if j>k
+   //       fullupdate=L(:,k)*L(j,k);
+   //       incompleteupdate=fullupdate.*(A(:,j)~=0);
+   //       missing=sum(fullupdate-incompleteupdate);
+   //       L(j:end,j)=L(j:end,j)-incompleteupdate(j:end);
+   //       L(j,j)=L(j,j)-omega*missing;
+   //     end
+   //   end
+   // end
+
+   for(int k=0; k<matrix.n; ++k){
+      if(factor.adiag[k]==0) continue; // null row/column
+      // figure out the final L(k,k) entry
+      if(factor.invdiag[k]<min_diagonal_ratio*factor.adiag[k])
+         factor.invdiag[k]=1/sqrt(factor.adiag[k]); // drop to Gauss-Seidel here if the pivot looks dangerously small
+      else
+         factor.invdiag[k]=1/sqrt(factor.invdiag[k]);
+      // finalize the k'th column L(:,k)
+      for(int p=factor.colstart[k]; p<factor.colstart[k+1]; ++p){
+         factor.value[p]*=factor.invdiag[k];
+      }
+      // incompletely eliminate L(:,k) from future columns, modifying diagonals
+      for(int p=factor.colstart[k]; p<factor.colstart[k+1]; ++p){
+         int j=factor.rowindex[p]; // work on column j
+         T multiplier=factor.value[p];
+         T missing=0;
+         int a=factor.colstart[k];
+         // first look for contributions to missing from dropped entries above the diagonal in column j
+         int b=0;
+         while(a<factor.colstart[k+1] && factor.rowindex[a]<j){
+            // look for factor.rowindex[a] in matrix.index[j] starting at b
+            while(b<(int)matrix.index[j].size()){
+               if(matrix.index[j][b]<factor.rowindex[a])
+                  ++b;
+               else if(matrix.index[j][b]==factor.rowindex[a])
+                  break;
+               else{
+                  missing+=factor.value[a];
+                  break;
+               }
+            }
+            ++a;
+         }
+         // adjust the diagonal j,j entry
+         if(a<factor.colstart[k+1] && factor.rowindex[a]==j){
+            factor.invdiag[j]-=multiplier*factor.value[a];
+         }
+         ++a;
+         // and now eliminate from the nonzero entries below the diagonal in column j (or add to missing if we can't)
+         b=factor.colstart[j];
+         while(a<factor.colstart[k+1] && b<factor.colstart[j+1]){
+            if(factor.rowindex[b]<factor.rowindex[a])
+               ++b;
+            else if(factor.rowindex[b]==factor.rowindex[a]){
+               factor.value[b]-=multiplier*factor.value[a];
+               ++a;
+               ++b;
+            }else{
+               missing+=factor.value[a];
+               ++a;
+            }
+         }
+         // and if there's anything left to do, add it to missing
+         while(a<factor.colstart[k+1]){
+            missing+=factor.value[a];
+            ++a;
+         }
+         // and do the final diagonal adjustment from the missing entries
+         factor.invdiag[j]-=modification_parameter*multiplier*missing;
+      }
+   }
+}
+
+//============================================================================
+// Solution routines with lower triangular matrix.
+
+// solve L*result=rhs
+template<class T>
+void solve_lower(const SparseColumnLowerFactor<T> &factor, const std::vector<T> &rhs, std::vector<T> &result)
+{
+   assert(factor.n==rhs.size());
+   assert(factor.n==result.size());
+   result=rhs;
+   for(int i=0; i<factor.n; ++i){
+      result[i]*=factor.invdiag[i];
+      for(int j=factor.colstart[i]; j<factor.colstart[i+1]; ++j){
+         result[factor.rowindex[j]]-=factor.value[j]*result[i];
+      }
+   }
+}
+
+// solve L^T*result=rhs
+template<class T>
+void solve_lower_transpose_in_place(const SparseColumnLowerFactor<T> &factor, std::vector<T> &x)
+{
+   assert(factor.n==(int)x.size());
+   assert(factor.n>0);
+   int i=factor.n;
+   do{
+      --i;
+      for(int j=factor.colstart[i]; j<factor.colstart[i+1]; ++j){
+         x[i]-=factor.value[j]*x[factor.rowindex[j]];
+      }
+      x[i]*=factor.invdiag[i];
+   }while(i!=0);
+}
+
+//============================================================================
+// Encapsulates the Conjugate Gradient algorithm with incomplete Cholesky
+// factorization preconditioner.
+
+template <class T>
+struct SparsePCGSolver
+{
+   SparsePCGSolver(void)
+   {
+      set_solver_parameters(1e-5, 100, 0.97, 0.25);
+   }
+
+   void set_solver_parameters(T tolerance_factor_, int max_iterations_, T modified_incomplete_cholesky_parameter_=0.97, T min_diagonal_ratio_=0.25)
+   {
+      tolerance_factor=tolerance_factor_;
+      if(tolerance_factor<1e-30) tolerance_factor=1e-30;
+      max_iterations=max_iterations_;
+      modified_incomplete_cholesky_parameter=modified_incomplete_cholesky_parameter_;
+      min_diagonal_ratio=min_diagonal_ratio_;
+   }
+
+   bool solve(const SparseMatrix<T> &matrix, const std::vector<T> &rhs, std::vector<T> &result, T &relative_residual_out, int &iterations_out, int precondition=2)
+   {
+      int n=matrix.n;
+      if((int)m.size()!=n){ m.resize(n); s.resize(n); z.resize(n); r.resize(n); }
+      zero(result);
+      r=rhs;
+      double residual_out=InstantBLAS<int,T>::abs_max(r);
+      if(residual_out==0) {
+         iterations_out=0;
+         return true;
+      }
+      //double tol=tolerance_factor*residual_out; // relative residual
+      double tol=tolerance_factor;
+	  double residual_0 = residual_out;
+
+      form_preconditioner(matrix, precondition);
+      apply_preconditioner( r, z, precondition);
+      double rho=InstantBLAS<int,T>::dot(z, r);
+      if(rho==0 || rho!=rho) {
+         iterations_out=0;
+         return false;
+      }
+
+      s=z;
+      fixed_matrix.construct_from_matrix(matrix);
+      int iteration;
+      for(iteration=0; iteration<max_iterations; ++iteration){
+         multiply(fixed_matrix, s, z);
+         double alpha=rho/InstantBLAS<int,T>::dot(s, z);
+         InstantBLAS<int,T>::add_scaled(alpha, s, result);
+         InstantBLAS<int,T>::add_scaled(-alpha, z, r);
+         residual_out=InstantBLAS<int,T>::abs_max(r);
+		 relative_residual_out = residual_out / residual_0;
+         if(residual_out<=tol) {
+            iterations_out=iteration+1;
+            return true; 
+         }
+         apply_preconditioner(r, z, precondition);
+         double rho_new=InstantBLAS<int,T>::dot(z, r);
+         double beta=rho_new/rho;
+         InstantBLAS<int,T>::add_scaled(beta, s, z); s.swap(z); // s=beta*s+z
+         rho=rho_new;
+      }
+      iterations_out=iteration;
+	  relative_residual_out = residual_out / residual_0;
+      return false;
+   }
+
+   protected:
+
+   // internal structures
+   SparseColumnLowerFactor<T> ic_factor; // modified incomplete cholesky factor
+   std::vector<T> m, z, s, r; // temporary vectors for PCG
+   FixedSparseMatrix<T> fixed_matrix; // used within loop
+
+   // parameters
+   T tolerance_factor;
+   int max_iterations;
+   T modified_incomplete_cholesky_parameter;
+   T min_diagonal_ratio;
+
+   void form_preconditioner(const SparseMatrix<T>& matrix, int precondition=2)
+   {
+		if(precondition==2) {
+			// incomplete cholesky
+			factor_modified_incomplete_cholesky0(matrix, ic_factor, modified_incomplete_cholesky_parameter, min_diagonal_ratio);
+		
+		} else if(precondition==1) {
+			// diagonal
+		    ic_factor.resize(matrix.n);
+		    zero(ic_factor.invdiag);
+			for(int i=0; i<matrix.n; ++i) {
+				  for(int j=0; j<(int)matrix.index[i].size(); ++j){
+					 if(matrix.index[i][j]==i){
+						ic_factor.invdiag[i] = 1./matrix.value[i][j];
+					 }
+				  }
+			}
+		}
+   }
+
+   void apply_preconditioner(const std::vector<T> &x, std::vector<T> &result, int precondition=2)
+   {
+		if (precondition==2) {
+			// incomplete cholesky
+			solve_lower(ic_factor, x, result);
+			solve_lower_transpose_in_place(ic_factor,result);
+		} else if(precondition==1) {
+			// diagonal
+			for(int_index i=0; i<(int_index)result.size(); ++i) {
+				result[i] = x[i] * ic_factor.invdiag[i];
+		    }
+		} else {
+			// off
+			result = x;
+		}
+   }
+};
+
+
+
+#undef parallel_for
+#undef parallel_end             
+#undef int_index
+
+#undef parallel_block 
+#undef do_parallel 
+#undef do_end
+#undef block_end
+
+#endif

Simulations/util/quaternion.h

@@ -83,7 +83,7 @@ public:
 	inline const Quaternion operator*=(const Quaternion &q) 
 	{
 		vector3Dim<Scalar> v1(x,y,z), v2(q.x,q.y,q.z);
-		vector3Dim<Scalar> nv = v1*q.w + v2*w + cross(v2,v1);
+		vector3Dim<Scalar> nv = v1*q.w + v2*w + cross(v1,v2);
 		Scalar nw = w*q.w - (v1.x*v2.x+v1.y*v2.y+v1.z*v2.z);
 		x = nv.x;
 		y = nv.y;
@@ -106,7 +106,7 @@ public:
 	inline const Quaternion operator*(const Quaternion &q) const
 	{
 		vector3Dim<Scalar> v1(x,y,z), v2(q.x,q.y,q.z);
-		vector3Dim<Scalar> nv = v1*q.w + v2*w + cross(v2,v1);
+		vector3Dim<Scalar> nv = v1*q.w + v2*w + cross(v1,v2);
 		Scalar nw = w*q.w - (v1.x*v2.x+v1.y*v2.y+v1.z*v2.z);
 
 		return Quaternion(nv.x,nv.y,nv.z,nw);

Simulations/util/vector4d.h

@@ -14,29 +14,29 @@ namespace GamePhysics
 
 // basic inlined vector class
 template<class Scalar>
-class ntlVector4Dim
+class vector4Dim
 {
 public:
 	//! Constructor
-	inline ntlVector4Dim() : x(0),y(0),z(0),t(0) {}
+	inline vector4Dim() : x(0),y(0),z(0),t(0) {}
 	
 	//! Copy-Constructor
-	inline ntlVector4Dim ( const ntlVector4Dim<Scalar> &v ) : x(v.x), y(v.y), z(v.z),t(v.t) {}
+	inline vector4Dim ( const vector4Dim<Scalar> &v ) : x(v.x), y(v.y), z(v.z),t(v.t) {}
 
 	//! Copy-Constructor
-	inline ntlVector4Dim ( const float * v) : x((Scalar)v[0]), y((Scalar)v[1]), z((Scalar)v[2]), t((Scalar)v[3]) {}
+	inline vector4Dim ( const float * v) : x((Scalar)v[0]), y((Scalar)v[1]), z((Scalar)v[2]), t((Scalar)v[3]) {}
 
 	//! Copy-Constructor
-	inline ntlVector4Dim ( const double * v) : x((Scalar)v[0]), y((Scalar)v[1]), z((Scalar)v[2]), t((Scalar)v[3]) {}
+	inline vector4Dim ( const double * v) : x((Scalar)v[0]), y((Scalar)v[1]), z((Scalar)v[2]), t((Scalar)v[3]) {}
 	
 	//! Construct a vector from one Scalar
-	inline ntlVector4Dim ( Scalar v) : x(v), y(v), z(v), t(v) {}
+	inline vector4Dim ( Scalar v) : x(v), y(v), z(v), t(v) {}
 		
 	//! Construct a vector from four Ss
-	inline ntlVector4Dim ( Scalar vx, Scalar vy, Scalar vz, Scalar vw) : x(vx), y(vy), z(vz), t(vw) {}
+	inline vector4Dim ( Scalar vx, Scalar vy, Scalar vz, Scalar vw) : x(vx), y(vy), z(vz), t(vw) {}
 
 	//! Construct a vector from four Ss
-	//inline ntlVector4Dim(DirectX::XMVECTOR &v ); // TODO CHECK!
+	//inline vector4Dim(DirectX::XMVECTOR &v ); // TODO CHECK!
 	
 	// get address of array for OpenGL
 	Scalar *getAddress() { return value; }
@@ -44,7 +44,7 @@ public:
 	// Operators
 	
 	//! Assignment operator
-	inline const ntlVector4Dim<Scalar>& operator= ( const ntlVector4Dim<Scalar>& v ) {
+	inline const vector4Dim<Scalar>& operator= ( const vector4Dim<Scalar>& v ) {
 		x = v.x;
 		y = v.y;
 		z = v.z;
@@ -52,12 +52,12 @@ public:
 		return *this;
 	}
 	//! Assignment operator
-	inline const ntlVector4Dim<Scalar>& operator= ( Scalar s ) {
+	inline const vector4Dim<Scalar>& operator= ( Scalar s ) {
 		x = y = z = t = s;
 		return *this;
 	}
 	//! Assign and add operator
-	inline const ntlVector4Dim<Scalar>& operator+= ( const ntlVector4Dim<Scalar>& v ) {
+	inline const vector4Dim<Scalar>& operator+= ( const vector4Dim<Scalar>& v ) {
 		x += v.x;
 		y += v.y;
 		z += v.z;
@@ -65,7 +65,7 @@ public:
 		return *this;
 	}
 	//! Assign and add operator
-	inline const ntlVector4Dim<Scalar>& operator+= ( Scalar s ) {
+	inline const vector4Dim<Scalar>& operator+= ( Scalar s ) {
 		x += s;
 		y += s;
 		z += s;
@@ -73,7 +73,7 @@ public:
 		return *this;
 	}
 	//! Assign and sub operator
-	inline const ntlVector4Dim<Scalar>& operator-= ( const ntlVector4Dim<Scalar>& v ) {
+	inline const vector4Dim<Scalar>& operator-= ( const vector4Dim<Scalar>& v ) {
 		x -= v.x;
 		y -= v.y;
 		z -= v.z;
@@ -81,7 +81,7 @@ public:
 		return *this;
 	}
 	//! Assign and sub operator
-	inline const ntlVector4Dim<Scalar>& operator-= ( Scalar s ) {
+	inline const vector4Dim<Scalar>& operator-= ( Scalar s ) {
 		x -= s;
 		y -= s;
 		z -= s;
@@ -89,7 +89,7 @@ public:
 		return *this;
 	}
 	//! Assign and mult operator
-	inline const ntlVector4Dim<Scalar>& operator*= ( const ntlVector4Dim<Scalar>& v ) {
+	inline const vector4Dim<Scalar>& operator*= ( const vector4Dim<Scalar>& v ) {
 		x *= v.x;
 		y *= v.y;
 		z *= v.z;
@@ -97,7 +97,7 @@ public:
 		return *this;
 	}
 	//! Assign and mult operator
-	inline const ntlVector4Dim<Scalar>& operator*= ( Scalar s ) {
+	inline const vector4Dim<Scalar>& operator*= ( Scalar s ) {
 		x *= s;
 		y *= s;
 		z *= s;
@@ -105,7 +105,7 @@ public:
 		return *this;
 	}
 	//! Assign and div operator
-	inline const ntlVector4Dim<Scalar>& operator/= ( const ntlVector4Dim<Scalar>& v ) {
+	inline const vector4Dim<Scalar>& operator/= ( const vector4Dim<Scalar>& v ) {
 		x /= v.x;
 		y /= v.y;
 		z /= v.z;
@@ -113,7 +113,7 @@ public:
 		return *this;        
 	}
 	//! Assign and div operator
-	inline const ntlVector4Dim<Scalar>& operator/= ( Scalar s ) {        
+	inline const vector4Dim<Scalar>& operator/= ( Scalar s ) {        
 		x /= s;
 		y /= s;
 		z /= s;
@@ -121,29 +121,29 @@ public:
 		return *this;        
 	}
 	
-	inline void safeDivide (const ntlVector4Dim<Scalar>& v);
+	inline void safeDivide (const vector4Dim<Scalar>& v);
 	
 	//! Negation operator
-	inline ntlVector4Dim<Scalar> operator- () const {
+	inline vector4Dim<Scalar> operator- () const {
-		return ntlVector4Dim<Scalar> (-x, -y, -z, -t);        
+		return vector4Dim<Scalar> (-x, -y, -z, -t);        
 	}
 	
 	// binary operator add
-	inline ntlVector4Dim<Scalar> operator+ (const ntlVector4Dim<Scalar>&) const;
+	inline vector4Dim<Scalar> operator+ (const vector4Dim<Scalar>&) const;
 	// binary operator add
-	inline ntlVector4Dim<Scalar> operator+ (Scalar) const;
+	inline vector4Dim<Scalar> operator+ (Scalar) const;
 	// binary operator sub
-	inline ntlVector4Dim<Scalar> operator- (const ntlVector4Dim<Scalar>&) const;
+	inline vector4Dim<Scalar> operator- (const vector4Dim<Scalar>&) const;
 	// binary operator sub
-	inline ntlVector4Dim<Scalar> operator- (Scalar) const;
+	inline vector4Dim<Scalar> operator- (Scalar) const;
 	// binary operator mult
-	inline ntlVector4Dim<Scalar> operator* (const ntlVector4Dim<Scalar>&) const;
+	inline vector4Dim<Scalar> operator* (const vector4Dim<Scalar>&) const;
 	// binary operator mult
-	inline ntlVector4Dim<Scalar> operator* (Scalar) const;
+	inline vector4Dim<Scalar> operator* (Scalar) const;
 	// binary operator div
-	inline ntlVector4Dim<Scalar> operator/ (const ntlVector4Dim<Scalar>&) const;
+	inline vector4Dim<Scalar> operator/ (const vector4Dim<Scalar>&) const;
 	// binary operator div
-	inline ntlVector4Dim<Scalar> operator/ (Scalar) const;
+	inline vector4Dim<Scalar> operator/ (Scalar) const;
 	
 	//! Get smallest component 
 	//inline Scalar min() const { return ( x<y ) ? ( ( x<z ) ? x:z ) : ( ( y<z ) ? y:z ); // todo t!!}
@@ -185,7 +185,7 @@ public:
 	};
 
 	// zero element
-	static const ntlVector4Dim<Scalar> ZERO;
+	static const vector4Dim<Scalar> ZERO;
 	
 protected:
 
@@ -196,9 +196,9 @@ protected:
 //************************************************************************
 //! Addition operator
 template<class Scalar> 
-inline ntlVector4Dim<Scalar> ntlVector4Dim<Scalar>::operator+ ( const ntlVector4Dim<Scalar> &v) const
+inline vector4Dim<Scalar> vector4Dim<Scalar>::operator+ ( const vector4Dim<Scalar> &v) const
 {
-	return ntlVector4Dim<Scalar> (value[0]+v.value[0],
+	return vector4Dim<Scalar> (value[0]+v.value[0],
 			value[1]+v.value[1],
 			value[2]+v.value[2],
 			value[3]+v.value[3]);
@@ -206,42 +206,42 @@ inline ntlVector4Dim<Scalar> ntlVector4Dim<Scalar>::operator+ ( const ntlVector4
 
 //! Addition operator
 template<class Scalar>
-inline ntlVector4Dim<Scalar>
+inline vector4Dim<Scalar>
-ntlVector4Dim<Scalar>::operator+(Scalar s) const
+vector4Dim<Scalar>::operator+(Scalar s) const
 {
-  return ntlVector4Dim<Scalar>(value[0]+s,
+  return vector4Dim<Scalar>(value[0]+s,
 			value[1]+s,
 			value[2]+s,
 			value[3]+s);
 }
 
 template<class Scalar>
-inline ntlVector4Dim<Scalar> 
+inline vector4Dim<Scalar> 
-operator+(float s, ntlVector4Dim<Scalar> v) 
+operator+(float s, vector4Dim<Scalar> v) 
 {
   return v + s;
 }
 
 template<class Scalar>
-inline ntlVector4Dim<Scalar> 
+inline vector4Dim<Scalar> 
-operator+(double s, ntlVector4Dim<Scalar> v) 
+operator+(double s, vector4Dim<Scalar> v) 
 {
   return v + s;
 }
 
 template<class Scalar>
-inline ntlVector4Dim<Scalar> 
+inline vector4Dim<Scalar> 
-operator+(int s, ntlVector4Dim<Scalar> v) 
+operator+(int s, vector4Dim<Scalar> v) 
 {
   return v + s;
 }
 
 //! Subtraction operator
 template<class Scalar>
-inline ntlVector4Dim<Scalar>
+inline vector4Dim<Scalar>
-ntlVector4Dim<Scalar>::operator-( const ntlVector4Dim<Scalar> &v ) const
+vector4Dim<Scalar>::operator-( const vector4Dim<Scalar> &v ) const
 {
-  return ntlVector4Dim<Scalar>(value[0]-v.value[0],
+  return vector4Dim<Scalar>(value[0]-v.value[0],
 			value[1]-v.value[1],
 			value[2]-v.value[2],
 			value[3]-v.value[3]);
@@ -249,10 +249,10 @@ ntlVector4Dim<Scalar>::operator-( const ntlVector4Dim<Scalar> &v ) const
 
 //! Subtraction operator
 template<class Scalar>
-inline ntlVector4Dim<Scalar>
+inline vector4Dim<Scalar>
-ntlVector4Dim<Scalar>::operator-(Scalar s ) const
+vector4Dim<Scalar>::operator-(Scalar s ) const
 {
-  return ntlVector4Dim<Scalar>(value[0]-s,
+  return vector4Dim<Scalar>(value[0]-s,
 			value[1]-s,
 			value[2]-s,
 			value[3]-s,);
@@ -260,50 +260,50 @@ ntlVector4Dim<Scalar>::operator-(Scalar s ) const
 
 //! Multiplication operator
 template<class Scalar>
-inline ntlVector4Dim<Scalar> 
+inline vector4Dim<Scalar> 
-ntlVector4Dim<Scalar>::operator* ( const ntlVector4Dim<Scalar>& v ) const
+vector4Dim<Scalar>::operator* ( const vector4Dim<Scalar>& v ) const
 {
-	 return ntlVector4Dim<Scalar>(value[0]*v.value[0],
+	 return vector4Dim<Scalar>(value[0]*v.value[0],
 			value[1]*v.value[1],
 			value[2]*v.value[2],
 			value[3]*v.value[3]);
 }
 //! Multiplication operator
 template<class Scalar>
-inline ntlVector4Dim<Scalar>
+inline vector4Dim<Scalar>
-ntlVector4Dim<Scalar>::operator* (Scalar s) const
+vector4Dim<Scalar>::operator* (Scalar s) const
 {
-	return ntlVector4Dim<Scalar>(value[0]*s, value[1]*s, value[2]*s, value[3]*s);
+	return vector4Dim<Scalar>(value[0]*s, value[1]*s, value[2]*s, value[3]*s);
 }
 
 //! Multiplication operator
 template<class Scalar> 
-inline ntlVector4Dim<Scalar>
+inline vector4Dim<Scalar>
-operator* (float s, ntlVector4Dim<Scalar> v) 
+operator* (float s, vector4Dim<Scalar> v) 
 {
   return v * s;
 }
 
 template<class Scalar>
-inline ntlVector4Dim<Scalar> 
+inline vector4Dim<Scalar> 
-operator*(double s, ntlVector4Dim<Scalar> v) 
+operator*(double s, vector4Dim<Scalar> v) 
 {
   return v * s;
 }
 
 template<class Scalar>
-inline ntlVector4Dim<Scalar> 
+inline vector4Dim<Scalar> 
-operator*(int s, ntlVector4Dim<Scalar> v) 
+operator*(int s, vector4Dim<Scalar> v) 
 {
   return v * s;
 }
 
 //! Division operator
 template<class Scalar>
-inline ntlVector4Dim<Scalar>
+inline vector4Dim<Scalar>
-ntlVector4Dim<Scalar>::operator/ (const ntlVector4Dim<Scalar> & v) const
+vector4Dim<Scalar>::operator/ (const vector4Dim<Scalar> & v) const
 {
-	return ntlVector4Dim<Scalar> (value[0]/v.value[0],
+	return vector4Dim<Scalar> (value[0]/v.value[0],
 			value[1]/v.value[1],
 			value[2]/v.value[2],
 			value[3]/v.value[3]);
@@ -311,10 +311,10 @@ ntlVector4Dim<Scalar>::operator/ (const ntlVector4Dim<Scalar> & v) const
 
 //! Division operator
 template<class Scalar>
-inline ntlVector4Dim<Scalar>
+inline vector4Dim<Scalar>
-ntlVector4Dim<Scalar>::operator / (Scalar s) const
+vector4Dim<Scalar>::operator / (Scalar s) const
 {
-	return ntlVector4Dim<Scalar> (value[0]/s,
+	return vector4Dim<Scalar> (value[0]/s,
 			value[1]/s,
 			value[2]/s,
 			value[3]/s);
@@ -322,7 +322,7 @@ ntlVector4Dim<Scalar>::operator / (Scalar s) const
 
 //! Safe divide
 template<class Scalar>
-inline void ntlVector4Dim<Scalar>::safeDivide( const ntlVector4Dim<Scalar> &v )
+inline void vector4Dim<Scalar>::safeDivide( const vector4Dim<Scalar> &v )
 {
   value[0] = (v.value[0]!=0) ? (value[0] / v.value[0]) : 0;
   value[1] = (v.value[1]!=0) ? (value[1] / v.value[1]) : 0;
@@ -336,14 +336,14 @@ inline void ntlVector4Dim<Scalar>::safeDivide( const ntlVector4Dim<Scalar> &v )
 
 //! Dot product
 template<class Scalar>
-inline Scalar dot ( const ntlVector4Dim<Scalar> &t, const ntlVector4Dim<Scalar> &v ) {
+inline Scalar dot ( const vector4Dim<Scalar> &t, const vector4Dim<Scalar> &v ) {
 	return t.x*v.x + t.y*v.y + t.z*v.z + t.t*v.t;    
 }
 
 //! Cross product
 /*template<class Scalar>
-inline ntlVector4Dim<Scalar> cross ( const ntlVector4Dim<Scalar> &t, const ntlVector4Dim<Scalar> &v ) {
+inline vector4Dim<Scalar> cross ( const vector4Dim<Scalar> &t, const vector4Dim<Scalar> &v ) {
-	NYI ntlVector4Dim<Scalar> cp (
+	NYI vector4Dim<Scalar> cp (
 		( ( t.y*v.z ) - ( t.z*v.y ) ),
 		( ( t.z*v.x ) - ( t.x*v.z ) ),
 		( ( t.x*v.y ) - ( t.y*v.x ) ) );
@@ -353,36 +353,36 @@ inline ntlVector4Dim<Scalar> cross ( const ntlVector4Dim<Scalar> &t, const ntlVe
 
 //! Compute the magnitude (length) of the vector
 template<class Scalar>
-inline Scalar norm ( const ntlVector4Dim<Scalar>& v ) {
+inline Scalar norm ( const vector4Dim<Scalar>& v ) {
 	Scalar l = v.x*v.x + v.y*v.y + v.z*v.z + v.t*v.t;
 	return ( fabs ( l-1. ) < VECTOR_EPSILON*VECTOR_EPSILON ) ? 1. : sqrt ( l );
 }
 
 //! Compute squared magnitude
 template<class Scalar>
-inline Scalar normSquare ( const ntlVector4Dim<Scalar>& v ) {
+inline Scalar normSquare ( const vector4Dim<Scalar>& v ) {
 	return v.x*v.x + v.y*v.y + v.z*v.z + v.t*v.t;
 }
 
 //! Returns a normalized vector
 template<class Scalar>
-inline ntlVector4Dim<Scalar> getNormalized ( const ntlVector4Dim<Scalar>& v ) {
+inline vector4Dim<Scalar> getNormalized ( const vector4Dim<Scalar>& v ) {
 	Scalar l = v.x*v.x + v.y*v.y + v.z*v.z + v.t*v.t;
 	if ( fabs ( l-1. ) < VECTOR_EPSILON*VECTOR_EPSILON )
 		return v; /* normalized "enough"... */
 	else if ( l > VECTOR_EPSILON*VECTOR_EPSILON )
 	{
 		Scalar fac = 1./sqrt ( l );
-		return ntlVector4Dim<Scalar> ( v.x*fac, v.y*fac, v.z*fac , v.t*fac );
+		return vector4Dim<Scalar> ( v.x*fac, v.y*fac, v.z*fac , v.t*fac );
 	}
 	else
-		return ntlVector4Dim<Scalar> ( ( Scalar ) 0 );
+		return vector4Dim<Scalar> ( ( Scalar ) 0 );
 }
 
 //! Compute the norm of the vector and normalize it.
 /*! \return The value of the norm */
 template<class Scalar>
-inline Scalar normalize ( ntlVector4Dim<Scalar> &v ) {
+inline Scalar normalize ( vector4Dim<Scalar> &v ) {
 	Scalar norm;
 	Scalar l = v.x*v.x + v.y*v.y + v.z*v.z + v.t*v.t;
 	if ( fabs ( l-1. ) < VECTOR_EPSILON*VECTOR_EPSILON ) {
@@ -391,14 +391,14 @@ inline Scalar normalize ( ntlVector4Dim<Scalar> &v ) {
 		norm = sqrt ( l );
 		v *= 1./norm;        
 	} else {
-		v = ntlVector4Dim<Scalar>::ZERO;
+		v = vector4Dim<Scalar>::ZERO;
 		norm = 0.;
 	}
 	return ( Scalar ) norm;
 }
 
 template<class Scalar>
-inline bool equal(const ntlVector4Dim<Scalar> &v, const ntlVector4Dim<Scalar> &c)
+inline bool equal(const vector4Dim<Scalar> &v, const vector4Dim<Scalar> &c)
 {
   return (ABS(v[0]-c[0]) + 
 	  ABS(v[1]-c[1]) + 
@@ -407,7 +407,7 @@ inline bool equal(const ntlVector4Dim<Scalar> &v, const ntlVector4Dim<Scalar> &c
 }
 
 //! Outputs the object in human readable form as string
-template<class Scalar> std::string ntlVector4Dim<Scalar>::toString() const {
+template<class Scalar> std::string vector4Dim<Scalar>::toString() const {
 	char buf[256];
 	snprintf ( buf,256,"<%f,%f,%f,%f>", ( double ) ( *this ) [0], ( double ) ( *this ) [1], ( double ) ( *this ) [2] , ( double ) ( *this ) [3] );
 	return std::string ( buf );
@@ -415,7 +415,7 @@ template<class Scalar> std::string ntlVector4Dim<Scalar>::toString() const {
 
 //! Outputs the object in human readable form to stream
 template<class Scalar>
-std::ostream& operator<< ( std::ostream& os, const ntlVector4Dim<Scalar>& i ) {
+std::ostream& operator<< ( std::ostream& os, const vector4Dim<Scalar>& i ) {
 	char buf[256];
 	snprintf ( buf,256,"[%d,%d,%d,%d]", (double) i[0], (double) i[1], (double) i[2] , (double) i[3] );
 	os << std::string ( buf );
@@ -424,7 +424,7 @@ std::ostream& operator<< ( std::ostream& os, const ntlVector4Dim<Scalar>& i ) {
 
 //! Reads the contents of the object from a stream 
 template<class Scalar>
-std::istream& operator>> ( std::istream& is, ntlVector4Dim<Scalar>& i ) {
+std::istream& operator>> ( std::istream& is, vector4Dim<Scalar>& i ) {
 	char c;
 	char dummy[4];
 	is >> c >> i[0] >> dummy >> i[1] >> dummy >> i[2] >> dummy >> i[3] >> c;
@@ -436,16 +436,16 @@ std::istream& operator>> ( std::istream& is, ntlVector4Dim<Scalar>& i ) {
 /**************************************************************************/
 
 //! 3D vector class of type Real (typically float)
-typedef ntlVector4Dim<Real>  Vec4;
+typedef vector4Dim<Real>  Vec4;
 
 // a 3D vector with double precision
-typedef ntlVector4Dim<double>  nVec4d; 
+typedef vector4Dim<double>  nVec4d; 
 
 // a 3D vector with single precision
-typedef ntlVector4Dim<float>   nVec4f; 
+typedef vector4Dim<float>   nVec4f; 
 
 //! 3D vector class of type int
-typedef ntlVector4Dim<int>  nVec4i;
+typedef vector4Dim<int>  nVec4i;
 
 /* convert int,float and double vectors */
 template<class T> inline nVec4i vec42I(T v) { return nVec4i((int)v[0],(int)v[1],(int)v[2],(int)v[3]); }

Simulations/vectorbase.h

@@ -0,0 +1,611 @@
+/******************************************************************************
+ *
+ * MantaFlow fluid solver framework
+ * Copyright 2011-2016 Tobias Pfaff, Nils Thuerey 
+ *
+ * This program is free software, distributed under the terms of the
+ * GNU General Public License (GPL) 
+ * http://www.gnu.org/licenses
+ *
+ * Basic vector class
+ *
+ ******************************************************************************/
+
+#ifndef _VECTORBASE_H
+#define _VECTORBASE_H
+
+// get rid of windos min/max defines
+#if defined(WIN32) || defined(_WIN32)
+#	define NOMINMAX
+#endif
+
+#include <stdio.h>
+#include <string>
+#include <limits>
+#include <iostream>
+#include "general.h"
+
+// if min/max are still around...
+#if defined(WIN32) || defined(_WIN32)
+#   undef min
+#   undef max
+#endif
+
+// redefine usage of some windows functions
+#if defined(WIN32) || defined(_WIN32)
+#	ifndef snprintf 
+#	define snprintf _snprintf
+#	endif
+#endif
+
+// use which fp-precision? 1=float, 2=double
+#ifndef FLOATINGPOINT_PRECISION
+#   define FLOATINGPOINT_PRECISION 1
+#endif
+
+// VECTOR_EPSILON is the minimal vector length
+// In order to be able to discriminate floating point values near zero, and
+// to be sure not to fail a comparison because of roundoff errors, use this
+// value as a threshold.
+#if FLOATINGPOINT_PRECISION==1
+	typedef float Real;
+#   define VECTOR_EPSILON (1e-6f)
+#else
+	typedef double Real;
+#   define VECTOR_EPSILON (1e-10)
+#endif
+
+#ifndef M_PI
+#   define M_PI 3.1415926536
+#endif
+#ifndef M_E
+#   define M_E  2.7182818284
+#endif
+
+namespace Manta
+{
+
+//! Basic inlined vector class
+template<class S>
+class Vector3D
+{
+public:
+	//! Constructor
+	inline Vector3D() : x(0),y(0),z(0) {}
+	
+	//! Copy-Constructor
+	inline Vector3D ( const Vector3D<S> &v ) : x(v.x), y(v.y), z(v.z) {}
+
+	//! Copy-Constructor
+	inline Vector3D ( const float * v) : x((S)v[0]), y((S)v[1]), z((S)v[2]) {}
+
+	//! Copy-Constructor
+	inline Vector3D ( const double * v) : x((S)v[0]), y((S)v[1]), z((S)v[2]) {}
+	
+	//! Construct a vector from one S
+	inline Vector3D ( S v) : x(v), y(v), z(v) {}
+		
+	//! Construct a vector from three Ss
+	inline Vector3D ( S vx, S vy, S vz) : x(vx), y(vy), z(vz) {}
+
+	// Operators
+	
+	//! Assignment operator
+	inline const Vector3D<S>& operator= ( const Vector3D<S>& v ) {
+		x = v.x;
+		y = v.y;
+		z = v.z;
+		return *this;
+	}
+	//! Assignment operator
+	inline const Vector3D<S>& operator= ( S s ) {
+		x = y = z = s;
+		return *this;
+	}
+	//! Assign and add operator
+	inline const Vector3D<S>& operator+= ( const Vector3D<S>& v ) {
+		x += v.x;
+		y += v.y;
+		z += v.z;
+		return *this;
+	}
+	//! Assign and add operator
+	inline const Vector3D<S>& operator+= ( S s ) {
+		x += s;
+		y += s;
+		z += s;
+		return *this;
+	}
+	//! Assign and sub operator
+	inline const Vector3D<S>& operator-= ( const Vector3D<S>& v ) {
+		x -= v.x;
+		y -= v.y;
+		z -= v.z;
+		return *this;
+	}
+	//! Assign and sub operator
+	inline const Vector3D<S>& operator-= ( S s ) {
+		x -= s;
+		y -= s;
+		z -= s;
+		return *this;
+	}
+	//! Assign and mult operator
+	inline const Vector3D<S>& operator*= ( const Vector3D<S>& v ) {
+		x *= v.x;
+		y *= v.y;
+		z *= v.z;
+		return *this;
+	}
+	//! Assign and mult operator
+	inline const Vector3D<S>& operator*= ( S s ) {
+		x *= s;
+		y *= s;
+		z *= s;
+		return *this;
+	}
+	//! Assign and div operator
+	inline const Vector3D<S>& operator/= ( const Vector3D<S>& v ) {
+		x /= v.x;
+		y /= v.y;
+		z /= v.z;
+		return *this;        
+	}
+	//! Assign and div operator
+	inline const Vector3D<S>& operator/= ( S s ) {        
+		x /= s;
+		y /= s;
+		z /= s;
+		return *this;        
+	}
+	//! Negation operator
+	inline Vector3D<S> operator- () const {
+		return Vector3D<S> (-x, -y, -z);        
+	}
+	
+	//! Get smallest component 
+	inline S min() const {
+	return ( x<y ) ? ( ( x<z ) ? x:z ) : ( ( y<z ) ? y:z );
+	}
+	//! Get biggest component
+	inline S max() const {
+		return ( x>y ) ? ( ( x>z ) ? x:z ) : ( ( y>z ) ? y:z );
+	}
+		
+	//! Test if all components are zero
+	inline bool empty() {
+		return x==0 && y==0 && z==0;
+	}
+
+	//! access operator
+	inline S& operator[] ( unsigned int i ) { 
+		return value[i];
+	}
+	//! constant access operator
+	inline const S& operator[] ( unsigned int i ) const {
+		return value[i];
+	}
+
+	//! debug output vector to a string
+	std::string toString() const;
+	
+	//! test if nans are present
+	bool isValid() const; 
+
+	//! actual values
+	union {
+		S value[3];
+		struct {
+			S x;
+			S y;
+			S z;
+		};
+		struct {
+			S X;
+			S Y;
+			S Z;
+		};
+	};
+
+	//! zero element
+	static const Vector3D<S> Zero, Invalid;
+		
+	//! For compatibility with 4d vectors (discards 4th comp)
+	inline Vector3D ( S vx, S vy, S vz, S vDummy) : x(vx), y(vy), z(vz) {}
+	
+protected:
+
+};
+
+//************************************************************************
+// Additional operators
+//************************************************************************
+
+//! Addition operator
+template<class S> 
+inline Vector3D<S> operator+ ( const Vector3D<S> &v1, const Vector3D<S> &v2 ) {
+	return Vector3D<S> ( v1.x+v2.x, v1.y+v2.y, v1.z+v2.z );
+}
+//! Addition operator
+template<class S, class S2> 
+inline Vector3D<S> operator+ ( const Vector3D<S>& v, S2 s ) {
+	return Vector3D<S> ( v.x+s, v.y+s, v.z+s );
+}
+//! Addition operator
+template<class S, class S2> 
+inline Vector3D<S> operator+ ( S2 s, const Vector3D<S>& v ) {
+	return Vector3D<S> ( v.x+s, v.y+s, v.z+s );
+}
+
+//! Subtraction operator
+template<class S> 
+inline Vector3D<S> operator- ( const Vector3D<S> &v1, const Vector3D<S> &v2 ) {
+	return Vector3D<S> ( v1.x-v2.x, v1.y-v2.y, v1.z-v2.z );
+}
+//! Subtraction operator
+template<class S, class S2> 
+inline Vector3D<S> operator- ( const Vector3D<S>& v, S2 s ) {
+	return Vector3D<S> ( v.x-s, v.y-s, v.z-s );
+}
+//! Subtraction operator
+template<class S, class S2>
+inline Vector3D<S> operator- ( S2 s, const Vector3D<S>& v ) {
+	return Vector3D<S> ( s-v.x, s-v.y, s-v.z );
+}
+
+//! Multiplication operator
+template<class S>
+inline Vector3D<S> operator* ( const Vector3D<S> &v1, const Vector3D<S> &v2 ) {
+	return Vector3D<S> ( v1.x*v2.x, v1.y*v2.y, v1.z*v2.z );
+}
+//! Multiplication operator
+template<class S, class S2>
+inline Vector3D<S> operator* ( const Vector3D<S>& v, S2 s ) {
+	return Vector3D<S> ( v.x*s, v.y*s, v.z*s );
+}
+//! Multiplication operator
+template<class S, class S2> 
+inline Vector3D<S> operator* ( S2 s, const Vector3D<S>& v ) {
+	return Vector3D<S> ( s*v.x, s*v.y, s*v.z );
+}
+
+//! Division operator
+template<class S>
+inline Vector3D<S> operator/ ( const Vector3D<S> &v1, const Vector3D<S> &v2 ) {
+	return Vector3D<S> ( v1.x/v2.x, v1.y/v2.y, v1.z/v2.z );
+}
+//! Division operator
+template<class S, class S2>
+inline Vector3D<S> operator/ ( const Vector3D<S>& v, S2 s ) {
+	return Vector3D<S> ( v.x/s, v.y/s, v.z/s );
+}
+//! Division operator
+template<class S, class S2> 
+inline Vector3D<S> operator/ ( S2 s, const Vector3D<S>& v ) {
+	return Vector3D<S> ( s/v.x, s/v.y, s/v.z );
+}
+
+//! Comparison operator
+template<class S>
+inline bool operator== (const Vector3D<S>& s1, const Vector3D<S>& s2) {
+	return s1.x == s2.x && s1.y == s2.y && s1.z == s2.z;
+}
+
+//! Comparison operator
+template<class S>
+inline bool operator!= (const Vector3D<S>& s1, const Vector3D<S>& s2) {
+	return s1.x != s2.x || s1.y != s2.y || s1.z != s2.z;
+}
+
+//************************************************************************
+// External functions
+//************************************************************************
+
+//! Min operator
+template<class S>
+inline Vector3D<S> vmin (const Vector3D<S>& s1, const Vector3D<S>& s2) {
+	return Vector3D<S>(std::min(s1.x,s2.x), std::min(s1.y,s2.y), std::min(s1.z,s2.z));
+}
+
+//! Min operator
+template<class S, class S2>
+inline Vector3D<S> vmin (const Vector3D<S>& s1, S2 s2) {
+	return Vector3D<S>(std::min(s1.x,s2), std::min(s1.y,s2), std::min(s1.z,s2));
+}
+
+//! Min operator
+template<class S1, class S>
+inline Vector3D<S> vmin (S1 s1, const Vector3D<S>& s2) {
+	return Vector3D<S>(std::min(s1,s2.x), std::min(s1,s2.y), std::min(s1,s2.z));
+}
+
+//! Max operator
+template<class S>
+inline Vector3D<S> vmax (const Vector3D<S>& s1, const Vector3D<S>& s2) {
+	return Vector3D<S>(std::max(s1.x,s2.x), std::max(s1.y,s2.y), std::max(s1.z,s2.z));
+}
+
+//! Max operator
+template<class S, class S2>
+inline Vector3D<S> vmax (const Vector3D<S>& s1, S2 s2) {
+	return Vector3D<S>(std::max(s1.x,s2), std::max(s1.y,s2), std::max(s1.z,s2));
+}
+
+//! Max operator
+template<class S1, class S>
+inline Vector3D<S> vmax (S1 s1, const Vector3D<S>& s2) {
+	return Vector3D<S>(std::max(s1,s2.x), std::max(s1,s2.y), std::max(s1,s2.z));
+}
+
+//! Dot product
+template<class S>
+inline S dot ( const Vector3D<S> &t, const Vector3D<S> &v ) {
+	return t.x*v.x + t.y*v.y + t.z*v.z;    
+}
+
+//! Cross product
+template<class S>
+inline Vector3D<S> cross ( const Vector3D<S> &t, const Vector3D<S> &v ) {
+	Vector3D<S> cp (
+		( ( t.y*v.z ) - ( t.z*v.y ) ),
+		( ( t.z*v.x ) - ( t.x*v.z ) ),
+		( ( t.x*v.y ) - ( t.y*v.x ) ) );
+	return cp;
+}
+
+//! Project a vector into a plane, defined by its normal
+/*! Projects a vector into a plane normal to the given vector, which must
+  have unit length. Self is modified.
+  \param v The vector to project
+  \param n The plane normal
+  \return The projected vector */
+template<class S>
+inline const Vector3D<S>& projectNormalTo ( const Vector3D<S>& v, const Vector3D<S> &n) {
+	S sprod = dot (v, n);
+	return v - n * dot(v, n);
+}
+
+//! Compute the magnitude (length) of the vector
+//! (clamps to 0 and 1 with VECTOR_EPSILON)
+template<class S>
+inline S norm ( const Vector3D<S>& v ) {
+	S l = v.x*v.x + v.y*v.y + v.z*v.z;
+	if     (        l      <= VECTOR_EPSILON*VECTOR_EPSILON ) return(0.);
+	return ( fabs ( l-1. ) <  VECTOR_EPSILON*VECTOR_EPSILON ) ? 1. : sqrt ( l );
+}
+
+//! Compute squared magnitude
+template<class S>
+inline S normSquare ( const Vector3D<S>& v ) {
+	return v.x*v.x + v.y*v.y + v.z*v.z;
+}
+
+//! compatibility, allow use of int, Real and Vec inputs with norm/normSquare
+inline Real norm(const Real v) { return fabs(v); }
+inline Real normSquare(const Real v) { return square(v); }
+inline Real norm(const int v) { return abs(v); }
+inline Real normSquare(const int v) { return square(v); }
+
+//! Returns a normalized vector
+template<class S>
+inline Vector3D<S> getNormalized ( const Vector3D<S>& v ) {
+	S l = v.x*v.x + v.y*v.y + v.z*v.z;
+	if ( fabs ( l-1. ) < VECTOR_EPSILON*VECTOR_EPSILON )
+		return v; /* normalized "enough"... */
+	else if ( l > VECTOR_EPSILON*VECTOR_EPSILON )
+	{
+		S fac = 1./sqrt ( l );
+		return Vector3D<S> ( v.x*fac, v.y*fac, v.z*fac );
+	}
+	else
+		return Vector3D<S> ( ( S ) 0 );
+}
+
+//! Compute the norm of the vector and normalize it.
+/*! \return The value of the norm */
+template<class S>
+inline S normalize ( Vector3D<S> &v ) {
+	S norm;
+	S l = v.x*v.x + v.y*v.y + v.z*v.z;
+	if ( fabs ( l-1. ) < VECTOR_EPSILON*VECTOR_EPSILON ) {
+		norm = 1.;
+	} else if ( l > VECTOR_EPSILON*VECTOR_EPSILON ) {
+		norm = sqrt ( l );
+		v *= 1./norm;        
+	} else {
+		v = Vector3D<S>::Zero;
+		norm = 0.;
+	}
+	return ( S ) norm;
+}
+
+//! Obtain an orthogonal vector
+/*! Compute a vector that is orthonormal to the given vector.
+ *  Nothing else can be assumed for the direction of the new vector.
+ *  \return The orthonormal vector */
+template<class S> 
+Vector3D<S> getOrthogonalVector(const Vector3D<S>& v) {
+	// Determine the  component with max. absolute value
+	int maxIndex= ( fabs ( v.x ) > fabs ( v.y ) ) ? 0 : 1;
+	maxIndex= ( fabs ( v[maxIndex] ) > fabs ( v.z ) ) ? maxIndex : 2;
+
+	// Choose another axis than the one with max. component and project
+	// orthogonal to self
+	Vector3D<S> o ( 0.0 );
+	o[ ( maxIndex+1 ) %3]= 1;
+	
+	Vector3D<S> c = cross(v, o);
+	normalize(c);
+	return c;    
+}
+
+//! Convert vector to polar coordinates
+/*! Stable vector to angle conversion
+ *\param v vector to convert
+  \param phi unique angle [0,2PI]
+  \param theta unique angle [0,PI]
+ */
+template<class S>
+inline void vecToAngle ( const Vector3D<S>& v, S& phi, S& theta )
+{
+	if ( fabs ( v.y ) < VECTOR_EPSILON )
+		theta = M_PI/2;
+	else if ( fabs ( v.x ) < VECTOR_EPSILON && fabs ( v.z ) < VECTOR_EPSILON )
+		theta = ( v.y>=0 ) ? 0:M_PI;
+	else
+		theta = atan ( sqrt ( v.x*v.x+v.z*v.z ) /v.y );
+	if ( theta<0 ) theta+=M_PI;
+
+	if ( fabs ( v.x ) < VECTOR_EPSILON )
+		phi = M_PI/2;
+	else
+		phi = atan ( v.z/v.x );
+	if ( phi<0 ) phi+=M_PI;
+	if ( fabs ( v.z ) < VECTOR_EPSILON )
+		phi = ( v.x>=0 ) ? 0 : M_PI;
+	else if ( v.z < 0 )
+		phi += M_PI;
+}
+
+//! Compute vector reflected at a surface
+/*! Compute a vector, that is self (as an incoming vector) 
+ * reflected at a surface with a distinct normal vector. 
+ * Note that the normal is reversed, if the scalar product with it is positive.
+  \param t The incoming vector
+  \param n The surface normal
+  \return The new reflected vector
+  */
+template<class S>
+inline Vector3D<S> reflectVector ( const Vector3D<S>& t, const Vector3D<S>& n ) {
+	Vector3D<S> nn= ( dot ( t, n ) > 0.0 ) ? ( n*-1.0 ) : n;
+	return ( t - nn * ( 2.0 * dot ( nn, t ) ) );
+}
+
+//! Compute vector refracted at a surface
+/*! \param t The incoming vector
+ *  \param n The surface normal
+ *  \param nt The "inside" refraction index
+ *  \param nair The "outside" refraction index
+ *  \param refRefl Set to 1 on total reflection
+ *  \return The refracted vector
+*/
+template<class S>
+inline Vector3D<S> refractVector ( const Vector3D<S> &t, const Vector3D<S> &normal, S nt, S nair, int &refRefl ) {
+	// from Glassner's book, section 5.2 (Heckberts method)
+	S eta = nair / nt;
+	S n = -dot ( t, normal );
+	S tt = 1.0 + eta*eta* ( n*n-1.0 );
+	if ( tt<0.0 ) {
+		// we have total reflection!
+		refRefl = 1;
+	} else {
+		// normal reflection
+		tt = eta*n - sqrt ( tt );
+		return ( t*eta + normal*tt );
+	}
+	return t;
+}
+
+//! Outputs the object in human readable form as string
+template<class S> std::string Vector3D<S>::toString() const {
+	char buf[256];
+	snprintf ( buf,256,"[%+4.6f,%+4.6f,%+4.6f]", ( double ) ( *this ) [0], ( double ) ( *this ) [1], ( double ) ( *this ) [2] );
+	// for debugging, optionally increase precision:
+	//snprintf ( buf,256,"[%+4.16f,%+4.16f,%+4.16f]", ( double ) ( *this ) [0], ( double ) ( *this ) [1], ( double ) ( *this ) [2] );
+	return std::string ( buf );
+}
+
+template<> std::string Vector3D<int>::toString() const;
+
+
+//! Outputs the object in human readable form to stream
+/*! Output format [x,y,z] */
+template<class S>
+std::ostream& operator<< ( std::ostream& os, const Vector3D<S>& i ) {
+	os << i.toString();
+	return os;
+}
+
+//! Reads the contents of the object from a stream 
+/*! Input format [x,y,z] */
+template<class S>
+std::istream& operator>> ( std::istream& is, Vector3D<S>& i ) {
+	char c;
+	char dummy[3];
+	is >> c >> i[0] >> dummy >> i[1] >> dummy >> i[2] >> c;
+	return is;
+}
+
+/**************************************************************************/
+// Define default vector alias
+/**************************************************************************/
+
+//! 3D vector class of type Real (typically float)
+typedef Vector3D<Real>  Vec3;
+
+//! 3D vector class of type int
+typedef Vector3D<int>  Vec3i;
+
+//! convert to Real Vector
+template<class T> inline Vec3 toVec3 ( T v ) {
+	return Vec3 ( v[0],v[1],v[2] );
+}
+
+//! convert to int Vector
+template<class T> inline Vec3i toVec3i ( T v ) {
+	return Vec3i ( ( int ) v[0], ( int ) v[1], ( int ) v[2] );
+}
+
+//! convert to int Vector
+template<class T> inline Vec3i toVec3i ( T v0, T v1, T v2 ) {
+	return Vec3i ( ( int ) v0, ( int ) v1, ( int ) v2 );
+}
+
+//! round, and convert to int Vector
+template<class T> inline Vec3i toVec3iRound ( T v ) {
+	return Vec3i ( ( int ) round ( v[0] ), ( int ) round ( v[1] ), ( int ) round ( v[2] ) );
+}
+
+//! convert to int Vector if values are close enough to an int
+template<class T> inline Vec3i toVec3iChecked ( T v ) {    
+	Vec3i ret;
+	for (size_t i=0; i<3; i++) {
+		Real a = v[i];
+		if (fabs(a-floor(a+0.5)) > 1e-5)
+			errMsg("argument is not an int, cannot convert");    
+		ret[i] = (int) (a+0.5);
+	}
+	return ret;
+}
+
+//! convert to double Vector
+template<class T> inline Vector3D<double> toVec3d ( T v ) {
+	return Vector3D<double> ( v[0], v[1], v[2] );
+}
+
+//! convert to float Vector
+template<class T> inline Vector3D<float> toVec3f ( T v ) {
+	return Vector3D<float> ( v[0], v[1], v[2] );
+}
+
+
+/**************************************************************************/
+// Specializations for common math functions
+/**************************************************************************/
+
+template<> inline Vec3 clamp<Vec3>(const Vec3& a, const Vec3& b, const Vec3& c) {
+	return Vec3 ( clamp(a.x, b.x, c.x),
+				  clamp(a.y, b.y, c.y),
+				  clamp(a.z, b.z, c.z) );    
+}
+template<> inline Vec3 safeDivide<Vec3>(const Vec3 &a, const Vec3& b) { 
+	return Vec3(safeDivide(a.x,b.x), safeDivide(a.y,b.y), safeDivide(a.z,b.z));
+}
+template<> inline Vec3 nmod<Vec3>(const Vec3& a, const Vec3& b) {
+	return Vec3(nmod(a.x,b.x),nmod(a.y,b.y),nmod(a.z,b.z));
+}
+
+}; // namespace
+
+
+#endif

readme.txt

@@ -4,6 +4,7 @@
 --- Updated: Florian Ferstl, Sept 2014 ----------------------------------------
 --- Updated: Mina Saad Aziz, May 2016 -----------------------------------------
 --- Updated: Mengyu Chu, Nov 2017    ------------------------------------------
+--- Updated: You Xie, Nov 2019    ---------------------------------------------
 -------------------------------------------------------------------------------
 
 This solution contains the following components:
@@ -45,4 +46,4 @@ Further Note:
    DXUT are both based on the new DirectXMath API for linear algebra that comes
    with the Windows 8.* SDKs (replacing the old D3DXMath). You can use it for
    all of your linear algebra tasks.
-   Documentation: https://msdn.microsoft.com/en-us/library/windows/desktop/hh437833(v=vs.85).aspx
+   Documentation: https://msdn.microsoft.com/en-us/library/windows/desktop/hh437833(v=vs.85).aspx