diff --git a/Simulations/DiffusionSimulator.cpp b/Simulations/DiffusionSimulator.cpp new file mode 100644 index 0000000..17bda0b --- /dev/null +++ b/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& 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& 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 *A = new SparseMatrix (N); + std::vector *b = new std::vector(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 solver; + solver.set_solver_parameters(pcg_target_residual, pcg_max_iterations, 0.97, 0.25); + + std::vector 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; +} diff --git a/Simulations/DiffusionSimulator.h b/Simulations/DiffusionSimulator.h new file mode 100644 index 0000000..dea0560 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/Simulations/util/pcgsolver.h b/Simulations/util/pcgsolver.h new file mode 100644 index 0000000..f87e6c7 --- /dev/null +++ b/Simulations/util/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 +#include +#include +#include +#include +#include + +// 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 +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 &x, const std::vector &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 &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 &x) + { return std::abs(x[index_abs_max(x)]); } + + // saxpy (y=alpha*x+y) ======================================================= + static inline void add_scaled(T alpha, const std::vector &x, std::vector &y) { + cblas_daxpy((Int)x.size(), alpha, &x[0], 1, &y[0], 1); + } +}; + + + + +template +void zero(std::vector &v) +{ for(int i=(int)v.size()-1; i>=0; --i) v[i]=0; } + +template +void insert(std::vector &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 +void erase(std::vector &a, unsigned int index) +{ + for(unsigned int i=index; i +struct SparseMatrix +{ + int n; // dimension + std::vector > index; // for each row, a list of all column indices (sorted) + std::vector > 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; ij) 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 &indices, const std::vector &values) + { + int j=0, k=0; + while(jindices[j]){ + insert(index[i], k, indices[j]); + insert(value[i], k, values[j]); + ++j; + }else{ + value[i][k]+=values[j]; + ++j; + ++k; + } + } + for(;j SparseMatrixf; +typedef SparseMatrix SparseMatrixd; + +// perform result=matrix*x +template +void multiply(const SparseMatrix &matrix, const std::vector &x, std::vector &result) +{ + assert(matrix.n==x.size()); + result.resize(matrix.n); + //for(int i=0; i +void multiply_and_subtract(const SparseMatrix &matrix, const std::vector &x, std::vector &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 +struct FixedSparseMatrix +{ + int n; // dimension + std::vector value; // nonzero values row by row + std::vector colindex; // corresponding column indices + std::vector 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 &matrix) + { + resize(matrix.n); + rowstart[0]=0; + for(int i=0; i +void multiply(const FixedSparseMatrix &matrix, const std::vector &x, std::vector &result) +{ + assert(matrix.n==x.size()); + result.resize(matrix.n); + //for(int i=0; i +void multiply_and_subtract(const FixedSparseMatrix &matrix, const std::vector &x, std::vector &result) +{ + assert(matrix.n==x.size()); + result.resize(matrix.n); + for(int i=0; i +struct SparseColumnLowerFactor +{ + int n; + std::vector invdiag; // reciprocals of diagonal elements + std::vector value; // values below the diagonal, listed column by column + std::vector rowindex; // a list of all row indices, for each column in turn + std::vector colstart; // where each column begins in rowindex (plus an extra entry at the end, of #nonzeros) + std::vector 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< +void factor_modified_incomplete_cholesky0(const SparseMatrix &matrix, SparseColumnLowerFactor &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; ii){ + 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 +void solve_lower(const SparseColumnLowerFactor &factor, const std::vector &rhs, std::vector &result) +{ + assert(factor.n==rhs.size()); + assert(factor.n==result.size()); + result=rhs; + for(int i=0; i +void solve_lower_transpose_in_place(const SparseColumnLowerFactor &factor, std::vector &x) +{ + assert(factor.n==(int)x.size()); + assert(factor.n>0); + int i=factor.n; + do{ + --i; + for(int j=factor.colstart[i]; j +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 &matrix, const std::vector &rhs, std::vector &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::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::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::dot(s, z); + InstantBLAS::add_scaled(alpha, s, result); + InstantBLAS::add_scaled(-alpha, z, r); + residual_out=InstantBLAS::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::dot(z, r); + double beta=rho_new/rho; + InstantBLAS::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 ic_factor; // modified incomplete cholesky factor + std::vector m, z, s, r; // temporary vectors for PCG + FixedSparseMatrix 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& 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 &x, std::vector &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 \ No newline at end of file diff --git a/Simulations/util/vectorbase.h b/Simulations/util/vectorbase.h index c586c3e..e5e1377 100644 --- a/Simulations/util/vectorbase.h +++ b/Simulations/util/vectorbase.h @@ -1,1260 +1,611 @@ -/****************************************************************************** - * - * Basic vector class - * - *****************************************************************************/ -#ifndef GamePhysics_BASICVEC_H -#define GamePhysics_BASICVEC_H - -// get rid of windos min/max defines -#ifdef WIN32 -#define NOMINMAX -#endif - -#include -#include -#include -#include -#include -#include - -// if min/max are still around... -#ifdef WIN32 -#undef min -#undef max -#endif - -// use which fp-precision? 1=float, 2=double -#ifndef FLOATINGPOINT_PRECISION -#if GamePhysics_DEBUG==0 -#define FLOATINGPOINT_PRECISION 2 -#else // GamePhysics_DEBUG==1 -#define FLOATINGPOINT_PRECISION 1 -#endif // GamePhysics_DEBUG==1 -#endif - - -// windos, hardcoded limits for now... -// for e.g. MSVC compiler... -// some of these defines can be needed -// for linux systems as well (e.g. FLT_MAX) -#ifdef WIN32 - -#ifndef __FLT_MAX__ -# ifdef FLT_MAX // try to use it instead -# define __FLT_MAX__ FLT_MAX -# else // FLT_MAX -# define __FLT_MAX__ 3.402823466e+38f -# endif // FLT_MAX -#endif // __FLT_MAX__ - -#ifndef __DBL_MAX__ -# ifdef DBL_MAX // try to use it instead -# define __DBL_MAX__ DBL_MAX -# else // DBL_MAX -# define __DBL_MAX__ 1.7976931348623158e+308 -# endif // DBL_MAX -#endif // __DBL_MAX__ - -#ifndef M_PI -# define M_PI 3.1415926536 -# define M_E 2.7182818284 -#endif - -#ifndef snprintf -# define snprintf _snprintf -#endif - -#endif - -namespace GamePhysics { - - -// basic inlined vector class -template -class vector3Dim -{ -public: - // Constructor - inline vector3Dim(); - // Copy-Constructor - inline vector3Dim(const vector3Dim &v ); - inline vector3Dim(DirectX::XMVECTOR &v ); - inline vector3Dim(const float *); - inline vector3Dim(const double *); - // construct a vector from one Scalar - inline vector3Dim(Scalar); - // construct a vector from three Scalars - inline vector3Dim(Scalar, Scalar, Scalar); - - // get address of array for OpenGL - Scalar *getAddress() { return value; } - - // Assignment operator - inline const vector3Dim& operator= (const vector3Dim& v); - // Assignment operator - inline const vector3Dim& operator= (Scalar s); - // Assign and add operator - inline const vector3Dim& operator+= (const vector3Dim& v); - // Assign and add operator - inline const vector3Dim& operator+= (Scalar s); - // Assign and sub operator - inline const vector3Dim& operator-= (const vector3Dim& v); - // Assign and sub operator - inline const vector3Dim& operator-= (Scalar s); - // Assign and mult operator - inline const vector3Dim& operator*= (const vector3Dim& v); - // Assign and mult operator - inline const vector3Dim& operator*= (Scalar s); - // Assign and div operator - inline const vector3Dim& operator/= (const vector3Dim& v); - // Assign and div operator - inline const vector3Dim& operator/= (Scalar s); - - inline void safeDivide (const vector3Dim& v); - - - // unary operator - inline vector3Dim operator- () const; - - // binary operator add - inline vector3Dim operator+ (const vector3Dim&) const; - // binary operator add - inline vector3Dim operator+ (Scalar) const; - // binary operator sub - inline vector3Dim operator- (const vector3Dim&) const; - // binary operator sub - inline vector3Dim operator- (Scalar) const; - // binary operator mult - inline vector3Dim operator* (const vector3Dim&) const; - // binary operator mult - inline vector3Dim operator* (Scalar) const; - // binary operator div - inline vector3Dim operator/ (const vector3Dim&) const; - // binary operator div - inline vector3Dim operator/ (Scalar) const; - // Projection normal to a vector - inline vector3Dim getOrthogonalvector3Dim() const; - // Project into a plane - inline const vector3Dim& projectNormalTo(const vector3Dim &v); - - inline Scalar min() const { return (xy) ? ( (x>z) ? x:z ) : ( (y>z) ? y:z); } - - // minimize - inline const vector3Dim &minimize(const vector3Dim &); - // maximize - inline const vector3Dim &maximize(const vector3Dim &); - - // access operator - inline Scalar& operator[](unsigned int i); - // access operator - inline const Scalar& operator[](unsigned int i) const; - - // return absolutes of all components - inline vector3Dim getAbsolutes() const { return - vector3Dim(fabs(value[0]), fabs(value[1]), fabs(value[2]) ); - - }; - - // debug output vector to a string - std::string toString(); - - //! actual values - union { - struct { - Scalar value[3]; - }; - struct { - Scalar x; - Scalar y; - Scalar z; - }; - struct { - Scalar X; - Scalar Y; - Scalar Z; - }; - }; - - // expe compatibility functions - void makeFloor(const vector3Dim& cmp); - void makeCeil(const vector3Dim& cmp); - Scalar squaredDistanceTo(const vector3Dim& vec) const; - - // Returns true if the vector's s components are all greater that the ones of the vector it is compared against. - inline bool operator < ( const vector3Dim& vec ) const; - // Returns true if the vector's s components are all greater or equal that the ones of the vector it is compared against. - inline bool operator <= ( const vector3Dim& vec ) const; - // Returns true if the vector's s components are all smaller that the ones of the vector it is compared against. - inline bool operator > ( const vector3Dim& vec ) const; - // Returns true if the vector's s components are all smaller or equal that the ones of the vector it is compared against. - inline bool operator >= ( const vector3Dim& vec ) const; - - // Return the maximal component value. - inline Scalar maxComponent(void) const; - // Return the minimal component value. - inline Scalar minComponent(void) const; - // Return the index of the maximal coordinate value. - inline int maxComponentId(void) const; - // Return the index of the minimal coordinate value. - inline int minComponentId(void) const; - - inline DirectX::XMVECTOR toDirectXVector() const{ - return DirectX::XMVectorSet(x,y,z,1); - } - - // zero element - static const vector3Dim ZERO; - -protected: - -}; - - - - -// 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 FP_REAL_MAX __FLT_MAX__ -#define VECTOR_EPSILON (1e-5f) -#else -typedef double Real; -#define FP_REAL_MAX __DBL_MAX__ -#define VECTOR_EPSILON (1e-10) -#endif - - -//------------------------------------------------------------------------------ -// VECTOR inline FUNCTIONS -//------------------------------------------------------------------------------ - - - -/************************************************************************* - Constructor. - */ -template -inline vector3Dim::vector3Dim( void ) -{ - value[0] = value[1] = value[2] = 0; -} - - - -/************************************************************************* - Copy-Constructor. - */ -template -inline vector3Dim::vector3Dim( const vector3Dim &v ) -{ - value[0] = v.value[0]; - value[1] = v.value[1]; - value[2] = v.value[2]; -} - -template -inline vector3Dim::vector3Dim( DirectX::XMVECTOR &v ) -{ - value[0] = DirectX::XMVectorGetX(v); - value[1] = DirectX::XMVectorGetY(v); - value[2] = DirectX::XMVectorGetZ(v); -} - template -inline vector3Dim::vector3Dim( const float *fvalue) -{ - value[0] = (Scalar)fvalue[0]; - value[1] = (Scalar)fvalue[1]; - value[2] = (Scalar)fvalue[2]; -} - template -inline vector3Dim::vector3Dim( const double *fvalue) -{ - value[0] = (Scalar)fvalue[0]; - value[1] = (Scalar)fvalue[1]; - value[2] = (Scalar)fvalue[2]; -} - - - -/************************************************************************* - Constructor for a vector from a single Scalar. All components of - the vector get the same value. - \param s The value to set - \return The new vector - */ -template -inline vector3Dim::vector3Dim(Scalar s ) -{ - value[0]= s; - value[1]= s; - value[2]= s; -} - - -/************************************************************************* - Constructor for a vector from three Scalars. - \param s1 The value for the first vector component - \param s2 The value for the second vector component - \param s3 The value for the third vector component - \return The new vector - */ -template -inline vector3Dim::vector3Dim(Scalar s1, Scalar s2, Scalar s3) -{ - value[0]= s1; - value[1]= s2; - value[2]= s3; - -} - - -/************************************************************************* - Compute the vector product of two 3D vectors - \param v Second vector to compute the product with - \return A new vector with the product values - */ -/*template -inline vector3Dim -vector3Dim::operator^( const vector3Dim &v ) const -{ - return vector3Dim(value[1]*v.value[2] - value[2]*v.value[1], - value[2]*v.value[0] - value[0]*v.value[2], - value[0]*v.value[1] - value[1]*v.value[0]); -}*/ - - -/************************************************************************* - Copy a vector3Dim componentwise. - \param v vector with values to be copied - \return Reference to self - */ -template -inline const vector3Dim& -vector3Dim::operator=( const vector3Dim &v ) -{ - value[0] = v.value[0]; - value[1] = v.value[1]; - value[2] = v.value[2]; - return *this; -} - - -/************************************************************************* - Copy a Scalar to each component. - \param s The value to copy - \return Reference to self - */ -template -inline const vector3Dim& -vector3Dim::operator=(Scalar s) -{ - value[0] = s; - value[1] = s; - value[2] = s; - return *this; -} - - -/************************************************************************* - Add another vector3Dim componentwise. - \param v vector with values to be added - \return Reference to self - */ -template -inline const vector3Dim& -vector3Dim::operator+=( const vector3Dim &v ) -{ - value[0] += v.value[0]; - value[1] += v.value[1]; - value[2] += v.value[2]; - return *this; -} - - -/************************************************************************* - Add a Scalar value to each component. - \param s Value to add - \return Reference to self - */ -template -inline const vector3Dim& -vector3Dim::operator+=(Scalar s) -{ - value[0] += s; - value[1] += s; - value[2] += s; - return *this; -} - - -/************************************************************************* - Subtract another vector componentwise. - \param v vector of values to subtract - \return Reference to self - */ -template -inline const vector3Dim& -vector3Dim::operator-=( const vector3Dim &v ) -{ - value[0] -= v.value[0]; - value[1] -= v.value[1]; - value[2] -= v.value[2]; - return *this; -} - - -/************************************************************************* - Subtract a Scalar value from each component. - \param s Value to subtract - \return Reference to self - */ -template -inline const vector3Dim& -vector3Dim::operator-=(Scalar s) -{ - value[0]-= s; - value[1]-= s; - value[2]-= s; - return *this; -} - - -/************************************************************************* - Multiply with another vector componentwise. - \param v vector of values to multiply with - \return Reference to self - */ -template -inline const vector3Dim& -vector3Dim::operator*=( const vector3Dim &v ) -{ - value[0] *= v.value[0]; - value[1] *= v.value[1]; - value[2] *= v.value[2]; - return *this; -} - - -/************************************************************************* - Multiply each component with a Scalar value. - \param s Value to multiply with - \return Reference to self - */ -template -inline const vector3Dim& -vector3Dim::operator*=(Scalar s) -{ - value[0] *= s; - value[1] *= s; - value[2] *= s; - return *this; -} - - -/************************************************************************* - Divide by another vector3Dim componentwise. - \param v vector of values to divide by - \return Reference to self - */ -template -inline const vector3Dim& -vector3Dim::operator/=( const vector3Dim &v ) -{ - value[0] /= v.value[0]; - value[1] /= v.value[1]; - value[2] /= v.value[2]; - return *this; -} - -template -inline void vector3Dim::safeDivide( const vector3Dim &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; - value[2] = (v.value[2]!=0) ? (value[2] / v.value[2]) : 0; -} - - -/************************************************************************* - Divide each component by a Scalar value. - \param s Value to divide by - \return Reference to self - */ -template -inline const vector3Dim& -vector3Dim::operator/=(Scalar s) -{ - value[0] /= s; - value[1] /= s; - value[2] /= s; - return *this; -} - - -//------------------------------------------------------------------------------ -// unary operators -//------------------------------------------------------------------------------ - - -/************************************************************************* - Build componentwise the negative this vector. - \return The new (negative) vector - */ -template -inline vector3Dim -vector3Dim::operator-() const -{ - return vector3Dim(-value[0], -value[1], -value[2]); -} - - - -//------------------------------------------------------------------------------ -// binary operators -//------------------------------------------------------------------------------ - - -/************************************************************************* - Build a vector with another vector added componentwise. - \param v The second vector to add - \return The sum vector - */ -template -inline vector3Dim -vector3Dim::operator+( const vector3Dim &v ) const -{ - return vector3Dim(value[0]+v.value[0], - value[1]+v.value[1], - value[2]+v.value[2]); -} - - -/************************************************************************* - Build a vector with a Scalar value added to each component. - \param s The Scalar value to add - \return The sum vector - */ -template -inline vector3Dim -vector3Dim::operator+(Scalar s) const -{ - return vector3Dim(value[0]+s, - value[1]+s, - value[2]+s); -} - -template -inline vector3Dim -operator+(float s, vector3Dim v) -{ - return v + s; -} - -template -inline vector3Dim -operator+(double s, vector3Dim v) -{ - return v + s; -} - -template -inline vector3Dim -operator+(int s, vector3Dim v) -{ - return v + s; -} - - -/************************************************************************* - Build a vector with another vector subtracted componentwise. - \param v The second vector to subtract - \return The difference vector - */ -template -inline vector3Dim -vector3Dim::operator-( const vector3Dim &v ) const -{ - return vector3Dim(value[0]-v.value[0], - value[1]-v.value[1], - value[2]-v.value[2]); -} - - - - -/************************************************************************* - Build a vector with a Scalar value subtracted componentwise. - \param s The Scalar value to subtract - \return The difference vector - */ -template -inline vector3Dim -vector3Dim::operator-(Scalar s ) const -{ - return vector3Dim(value[0]-s, - value[1]-s, - value[2]-s); -} - -/************************************************************************* - Build a vector with another vector multiplied by componentwise. - \param v The second vector to muliply with - \return The product vector - */ -template -inline vector3Dim -vector3Dim::operator*( const vector3Dim& v) const -{ - return vector3Dim(value[0]*v.value[0], - value[1]*v.value[1], - value[2]*v.value[2]); -} - - -/************************************************************************* - Build a vector3Dim with a Scalar value multiplied to each component. - \param s The Scalar value to multiply with - \return The product vector - */ -template -inline vector3Dim -vector3Dim::operator*(Scalar s) const -{ - return vector3Dim(value[0]*s, value[1]*s, value[2]*s); -} - -// allow multiplications of the form: v2 = 3 * v1 -template -inline vector3Dim -operator*(float s, vector3Dim v) -{ - return v * s; -} - -template -inline vector3Dim -operator*(double s, vector3Dim v) -{ - return v * s; -} - -template -inline vector3Dim -operator*(int s, vector3Dim v) -{ - return v * s; -} - - - - - - -/************************************************************************* - Build a vector divided componentwise by another vector. - \param v The second vector to divide by - \return The ratio vector - */ -template -inline vector3Dim -vector3Dim::operator/(const vector3Dim& v) const -{ - return vector3Dim(value[0]/v.value[0], - value[1]/v.value[1], - value[2]/v.value[2]); -} - - - -/************************************************************************* - Build a vector divided componentwise by a Scalar value. - \param s The Scalar value to divide by - \return The ratio vector - */ -template -inline vector3Dim -vector3Dim::operator/(Scalar s) const -{ - return vector3Dim(value[0]/s, - value[1]/s, - value[2]/s); -} - - - -/************************************************************************* - Get a particular component of the vector. - \param i Number of Scalar to get - \return Reference to the component - */ -template -inline Scalar& -vector3Dim::operator[]( unsigned int i ) -{ - return value[i]; -} - - -/************************************************************************* - Get a particular component of a constant vector. - \param i Number of Scalar to get - \return Reference to the component - */ -template -inline const Scalar& -vector3Dim::operator[]( unsigned int i ) const -{ - return value[i]; -} - - - -//------------------------------------------------------------------------------ -// High level functions -//------------------------------------------------------------------------------ - - - -/************************************************************************* - Compute the scalar product with another vector. - \param v The second vector to work with - \return The value of the scalar product - */ -template -inline Scalar dot(const vector3Dim &t, const vector3Dim &v ) -{ - //return t.value[0]*v.value[0] + t.value[1]*v.value[1] + t.value[2]*v.value[2]; - return ((t[0]*v[0]) + (t[1]*v[1]) + (t[2]*v[2])); -} - - -/************************************************************************* - Calculate the cross product of this and another vector - */ -template -inline vector3Dim cross(const vector3Dim &t, const vector3Dim &v) -{ - vector3Dim cp( - ((t[1]*v[2]) - (t[2]*v[1])), - ((t[2]*v[0]) - (t[0]*v[2])), - ((t[0]*v[1]) - (t[1]*v[0])) ); - return cp; -} - - - - -/************************************************************************* - Compute a vector that is orthonormal to self. Nothing else can be assumed - for the direction of the new vector. - \return The orthonormal vector - */ -template -vector3Dim -vector3Dim::getOrthogonalvector3Dim() const -{ - // Determine the component with max. absolute value - int maxIndex= (fabs(value[0]) > fabs(value[1])) ? 0 : 1; - maxIndex= (fabs(value[maxIndex]) > fabs(value[2])) ? maxIndex : 2; - - /************************************************************************* - Choose another axis than the one with max. component and project - orthogonal to self - */ - vector3Dim vec(0.0); - vec[(maxIndex+1)%3]= 1; - vec.normalize(); - vec.projectNormalTo(this->getNormalized()); - return vec; -} - - -/************************************************************************* - Projects the vector into a plane normal to the given vector, which must - have unit length. Self is modified. - \param v The plane normal - \return The projected vector - */ -template -inline const vector3Dim& -vector3Dim::projectNormalTo(const vector3Dim &v) -{ - Scalar sprod = dot(*this,v); - value[0]= value[0] - v.value[0] * sprod; - value[1]= value[1] - v.value[1] * sprod; - value[2]= value[2] - v.value[2] * sprod; - return *this; -} - - - -//------------------------------------------------------------------------------ -// Other helper functions -//------------------------------------------------------------------------------ - - - -/************************************************************************* - Minimize the vector, i.e. set each entry of the vector to the minimum - of both values. - \param pnt The second vector to compare with - \return Reference to the modified self - */ -template -inline const vector3Dim & -vector3Dim::minimize(const vector3Dim &pnt) -{ - for (unsigned int i = 0; i < 3; i++) - value[i] = MIN(value[i],pnt[i]); - return *this; -} - - - -/************************************************************************* - Maximize the vector, i.e. set each entry of the vector to the maximum - of both values. - \param pnt The second vector to compare with - \return Reference to the modified self - */ -template -inline const vector3Dim & -vector3Dim::maximize(const vector3Dim &pnt) -{ - for (unsigned int i = 0; i < 3; i++) - value[i] = MAX(value[i],pnt[i]); - return *this; -} - - - - - - -/************************************************************************/ -// HELPER FUNCTIONS, independent of implementation -/************************************************************************/ - - - -/************************************************************************* - Compute the length (norm) of the vector. - \return The value of the norm - */ -template -inline Scalar norm( const vector3Dim &v) -{ - Scalar l = v[0]*v[0] + v[1]*v[1] + v[2]*v[2]; - return (fabs(l-1.) < VECTOR_EPSILON*VECTOR_EPSILON) ? 1. : sqrt(l); -} - -// for e.g. min max operator -inline Real normHelper(const vector3Dim &v) { - return norm(v); -} -inline Real normHelper(const Real &v) { - return (0. < v) ? v : -v ; -} -inline Real normHelper(const int &v) { - return (0 < v) ? (Real)(v) : (Real)(-v) ; -} - - -/************************************************************************* - Same as getNorm but doesnt sqrt - */ -template -inline Scalar normNoSqrt( const vector3Dim &v) { - return v[0]*v[0] + v[1]*v[1] + v[2]*v[2]; -} - - -/************************************************************************* - Compute a normalized vector based on this vector. - \return The new normalized vector - */ -template -inline vector3Dim getNormalized( const vector3Dim &v) -{ - Scalar l = v[0]*v[0] + v[1]*v[1] + v[2]*v[2]; - 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 vector3Dim(v[0]*fac, v[1]*fac, v[2]*fac); - } - else - return vector3Dim((Scalar)0); -} - - -/************************************************************************* - Compute the norm of the vector and normalize it. - \return The value of the norm - */ - template -inline Scalar normalize( vector3Dim &v) -{ - Scalar norm; - Scalar l = v[0]*v[0] + v[1]*v[1] + v[2]*v[2]; - if (fabs(l-1.) < VECTOR_EPSILON*VECTOR_EPSILON) { - norm = 1.; - } else if (l > VECTOR_EPSILON*VECTOR_EPSILON) { - norm = sqrt(l); - Scalar fac = 1./norm; - v[0] *= fac; - v[1] *= fac; - v[2] *= fac; - } else { - v[0]= v[1]= v[2]= 0; - norm = 0.; - } - return (Scalar)norm; -} - -/************************************************************************* - Stable vector to angle conversion - \return unique Angles in the of phi=[0,2PI], th=[0,PI] - */ - template -inline void vecToAngle(const vector3Dim &v, Scalar& phi, Scalar& 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 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 n The surface normal - \return The new reflected vector - */ -template -inline vector3Dim reflectVector(const vector3Dim &t, const vector3Dim &n) -{ - vector3Dim nn= (dot(t, n) > 0.0) ? (n*-1.0) : n; - return ( t - nn * (2.0 * dot(nn, t)) ); -} - - - -/************************************************************************* - * My own refraction calculation - * Taken from Glassner's book, section 5.2 (Heckberts method) - */ -template -inline vector3Dim refractVector(const vector3Dim &t, const vector3Dim &normal, Scalar nt, Scalar nair, int &refRefl) -{ - Scalar eta = nair / nt; - Scalar n = -dot(t, normal); - Scalar 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; -} - - -/************************************************************************* - Test two vector3Dims for equality based on the equality of their - values within a small threshold. - \param c The second vector to compare - \return TRUE if both are equal - \sa getEpsilon() - */ -template -inline bool equal(const vector3Dim &v, const vector3Dim &c) -{ - return (ABS(v[0]-c[0]) + - ABS(v[1]-c[1]) + - ABS(v[2]-c[2]) < VECTOR_EPSILON); -} - - -/************************************************************************* - * Assume this vector is an RGB color, and convert it to HSV - */ -template -inline void rgbToHsv( vector3Dim &V ) -{ - Scalar h=0,s=0,v=0; - Scalar maxrgb, minrgb, delta; - // convert to hsv... - maxrgb = V[0]; - int maxindex = 1; - if(V[2] > maxrgb){ maxrgb = V[2]; maxindex = 2; } - if(V[1] > maxrgb){ maxrgb = V[1]; maxindex = 3; } - minrgb = V[0]; - if(V[2] < minrgb) minrgb = V[2]; - if(V[1] < minrgb) minrgb = V[1]; - - v = maxrgb; - delta = maxrgb-minrgb; - - if(maxrgb > 0) s = delta/maxrgb; - else s = 0; - - h = 0; - if(s > 0) { - if(maxindex == 1) { - h = ((V[1]-V[2])/delta) + 0.0; } - if(maxindex == 2) { - h = ((V[2]-V[0])/delta) + 2.0; } - if(maxindex == 3) { - h = ((V[0]-V[1])/delta) + 4.0; } - h *= 60.0; - if(h < 0.0) h += 360.0; - } - - V[0] = h; - V[1] = s; - V[2] = v; -} - -/************************************************************************* - * Assume this vector is HSV and convert to RGB - */ -template -inline void hsvToRgb( vector3Dim &V ) -{ - Scalar h = V[0], s = V[1], v = V[2]; - Scalar r=0,g=0,b=0; - Scalar p,q,t, fracth; - int floorh; - // ...and back to rgb - if(s == 0) { - r = g = b = v; } - else { - h /= 60.0; - floorh = (int)h; - fracth = h - floorh; - p = v * (1.0 - s); - q = v * (1.0 - (s * fracth)); - t = v * (1.0 - (s * (1.0 - fracth))); - switch (floorh) { - case 0: r = v; g = t; b = p; break; - case 1: r = q; g = v; b = p; break; - case 2: r = p; g = v; b = t; break; - case 3: r = p; g = q; b = v; break; - case 4: r = t; g = p; b = v; break; - case 5: r = v; g = p; b = q; break; - } - } - - V[0] = r; - V[1] = g; - V[2] = b; -} - -//------------------------------------------------------------------------------ -// STREAM FUNCTIONS -//------------------------------------------------------------------------------ - -/************************************************************************* - Outputs the object in human readable form using the format - [x,y,z] - */ -template -std::ostream& -operator<<( std::ostream& os, const GamePhysics::vector3Dim& i ) -{ - - char buf[256]; - snprintf(buf,256,"<%g,%g,%g>", (double)i[0],(double)i[1],(double)i[2]); - os << std::string(buf); - //os << '[' << i[0] << ", " << i[1] << ", " << i[2] << ']'; - return os; -} - - - -/************************************************************************* - Reads the contents of the object from a stream using the same format - as the output operator. - */ -template -std::istream& -operator>>( std::istream& is, GamePhysics::vector3Dim& i ) -{ - char c; - char dummy[3]; - is >> c >> i[0] >> dummy >> i[1] >> dummy >> i[2] >> c; - return is; -} - -// helper function for output -template std::string vector3Dim::toString() { - char buf[256]; - snprintf(buf,256,"<%f,%f,%f>", (double)(*this)[0],(double)(*this)[1],(double)(*this)[2]); - return std::string(buf); -} - -// helper function for output -template bool intVecIsEqual(Vec a, Vec b) { - return a[0]==b[0] && a[1]==b[1] && a[2]==b[2]; -} - -/**************************************************************************/ -// typedefs! -/**************************************************************************/ - -// a 3D vector with double precision -typedef vector3Dim nVec3d; - -// a 3D vector with single precision -typedef vector3Dim nVec3f; - -// a 3D integer vector -typedef vector3Dim nVec3i; - -/* convert int,float and double vectors */ -template inline nVec3i vec2I(T v) { return nVec3i((int)v[0],(int)v[1],(int)v[2]); } -template inline nVec3i vec2I(T v0, T v1, T v2) { return nVec3i((int)v0,(int)v1,(int)v2); } -template inline nVec3d vec2D(T v) { return nVec3d(v[0],v[1],v[2]); } -template inline nVec3i vec2D(T v0, T v1, T v2) { return nVec3d((double)v0,(double)v1,(double)v2); } -template inline nVec3f vec2F(T v) { return nVec3f(v[0],v[1],v[2]); } -template inline nVec3i vec2F(T v0, T v1, T v2) { return nVec3f((float)v0,(float)v1,(float)v2); } -template inline nVec3i vecround(T v) { return nVec3i((int)round(v[0]),(int)round(v[1]),(int)round(v[2])); } - - - -/************************************************************************/ -// default vector typing - - -// a 3D vector for graphics output, typically float? -typedef vector3Dim Vec3; - -/* convert to Real vec */ -template inline Vec3 vec2R(T v) { return Vec3(v[0],v[1],v[2]); } - - -/* get minimal vector length value that can be discriminated. */ -inline Real getVecEpsilon() { return (Real)VECTOR_EPSILON; } - - - -//-------------------------------------------------------------------------------- -template -inline Scalar vector3Dim::squaredDistanceTo(const vector3Dim& vec) const -{ - Scalar dx,dy,dz; - dx = x-vec.x; - dy = y-vec.y; - dz = z-vec.z; - return dx*dx + dy*dy + dz*dz; -} -//-------------------------------------------------------------------------------- -template -inline void vector3Dim::makeFloor(const vector3Dim& cmp) -{ - if( cmp.x < x ) x = cmp.x; - if( cmp.y < y ) y = cmp.y; - if( cmp.z < z ) z = cmp.z; -} -//-------------------------------------------------------------------------------- -template -inline void vector3Dim::makeCeil(const vector3Dim& cmp) -{ - if( cmp.x > x ) x = cmp.x; - if( cmp.y > y ) y = cmp.y; - if( cmp.z > z ) z = cmp.z; -} - -//-------------------------------------------------------------------------------- -template -inline bool vector3Dim::operator < ( const vector3Dim& vec ) const -{ - if( x < vec.x && y < vec.y && z < vec.z ) - return true; - return false; -} -//-------------------------------------------------------------------------------- -template -inline bool vector3Dim::operator <= ( const vector3Dim& vec ) const -{ - if( x <= vec.x && y <= vec.y && z <= vec.z ) - return true; - return false; -} -//-------------------------------------------------------------------------------- -template -inline bool vector3Dim::operator > ( const vector3Dim& vec ) const -{ - if( x > vec.x && y > vec.y && z > vec.z ) - return true; - return false; -} -//-------------------------------------------------------------------------------- -template -inline bool vector3Dim::operator >= ( const vector3Dim& vec ) const -{ - if( x >= vec.x && y >= vec.y && z >= vec.z ) - return true; - return false; -} - -//-------------------------------------------------------------------------------- -template -inline Scalar vector3Dim::maxComponent(void) const -{ - return VMAX(*this); -} -//-------------------------------------------------------------------------------- -template -inline Scalar vector3Dim::minComponent(void) const -{ - return VMIN(*this); -} -//-------------------------------------------------------------------------------- -template -inline int vector3Dim::maxComponentId(void) const -{ - if (x -inline int vector3Dim::minComponentId(void) const -{ - if (x +#include +#include +#include +#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 Vector3D +{ +public: + //! Constructor + inline Vector3D() : x(0),y(0),z(0) {} + + //! Copy-Constructor + inline Vector3D ( const Vector3D &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& operator= ( const Vector3D& v ) { + x = v.x; + y = v.y; + z = v.z; + return *this; + } + //! Assignment operator + inline const Vector3D& operator= ( S s ) { + x = y = z = s; + return *this; + } + //! Assign and add operator + inline const Vector3D& operator+= ( const Vector3D& v ) { + x += v.x; + y += v.y; + z += v.z; + return *this; + } + //! Assign and add operator + inline const Vector3D& operator+= ( S s ) { + x += s; + y += s; + z += s; + return *this; + } + //! Assign and sub operator + inline const Vector3D& operator-= ( const Vector3D& v ) { + x -= v.x; + y -= v.y; + z -= v.z; + return *this; + } + //! Assign and sub operator + inline const Vector3D& operator-= ( S s ) { + x -= s; + y -= s; + z -= s; + return *this; + } + //! Assign and mult operator + inline const Vector3D& operator*= ( const Vector3D& v ) { + x *= v.x; + y *= v.y; + z *= v.z; + return *this; + } + //! Assign and mult operator + inline const Vector3D& operator*= ( S s ) { + x *= s; + y *= s; + z *= s; + return *this; + } + //! Assign and div operator + inline const Vector3D& operator/= ( const Vector3D& v ) { + x /= v.x; + y /= v.y; + z /= v.z; + return *this; + } + //! Assign and div operator + inline const Vector3D& operator/= ( S s ) { + x /= s; + y /= s; + z /= s; + return *this; + } + //! Negation operator + inline Vector3D operator- () const { + return Vector3D (-x, -y, -z); + } + + //! Get smallest component + inline S min() const { + return ( xy ) ? ( ( 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 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 +inline Vector3D operator+ ( const Vector3D &v1, const Vector3D &v2 ) { + return Vector3D ( v1.x+v2.x, v1.y+v2.y, v1.z+v2.z ); +} +//! Addition operator +template +inline Vector3D operator+ ( const Vector3D& v, S2 s ) { + return Vector3D ( v.x+s, v.y+s, v.z+s ); +} +//! Addition operator +template +inline Vector3D operator+ ( S2 s, const Vector3D& v ) { + return Vector3D ( v.x+s, v.y+s, v.z+s ); +} + +//! Subtraction operator +template +inline Vector3D operator- ( const Vector3D &v1, const Vector3D &v2 ) { + return Vector3D ( v1.x-v2.x, v1.y-v2.y, v1.z-v2.z ); +} +//! Subtraction operator +template +inline Vector3D operator- ( const Vector3D& v, S2 s ) { + return Vector3D ( v.x-s, v.y-s, v.z-s ); +} +//! Subtraction operator +template +inline Vector3D operator- ( S2 s, const Vector3D& v ) { + return Vector3D ( s-v.x, s-v.y, s-v.z ); +} + +//! Multiplication operator +template +inline Vector3D operator* ( const Vector3D &v1, const Vector3D &v2 ) { + return Vector3D ( v1.x*v2.x, v1.y*v2.y, v1.z*v2.z ); +} +//! Multiplication operator +template +inline Vector3D operator* ( const Vector3D& v, S2 s ) { + return Vector3D ( v.x*s, v.y*s, v.z*s ); +} +//! Multiplication operator +template +inline Vector3D operator* ( S2 s, const Vector3D& v ) { + return Vector3D ( s*v.x, s*v.y, s*v.z ); +} + +//! Division operator +template +inline Vector3D operator/ ( const Vector3D &v1, const Vector3D &v2 ) { + return Vector3D ( v1.x/v2.x, v1.y/v2.y, v1.z/v2.z ); +} +//! Division operator +template +inline Vector3D operator/ ( const Vector3D& v, S2 s ) { + return Vector3D ( v.x/s, v.y/s, v.z/s ); +} +//! Division operator +template +inline Vector3D operator/ ( S2 s, const Vector3D& v ) { + return Vector3D ( s/v.x, s/v.y, s/v.z ); +} + +//! Comparison operator +template +inline bool operator== (const Vector3D& s1, const Vector3D& s2) { + return s1.x == s2.x && s1.y == s2.y && s1.z == s2.z; +} + +//! Comparison operator +template +inline bool operator!= (const Vector3D& s1, const Vector3D& s2) { + return s1.x != s2.x || s1.y != s2.y || s1.z != s2.z; +} + +//************************************************************************ +// External functions +//************************************************************************ + +//! Min operator +template +inline Vector3D vmin (const Vector3D& s1, const Vector3D& s2) { + return Vector3D(std::min(s1.x,s2.x), std::min(s1.y,s2.y), std::min(s1.z,s2.z)); +} + +//! Min operator +template +inline Vector3D vmin (const Vector3D& s1, S2 s2) { + return Vector3D(std::min(s1.x,s2), std::min(s1.y,s2), std::min(s1.z,s2)); +} + +//! Min operator +template +inline Vector3D vmin (S1 s1, const Vector3D& s2) { + return Vector3D(std::min(s1,s2.x), std::min(s1,s2.y), std::min(s1,s2.z)); +} + +//! Max operator +template +inline Vector3D vmax (const Vector3D& s1, const Vector3D& s2) { + return Vector3D(std::max(s1.x,s2.x), std::max(s1.y,s2.y), std::max(s1.z,s2.z)); +} + +//! Max operator +template +inline Vector3D vmax (const Vector3D& s1, S2 s2) { + return Vector3D(std::max(s1.x,s2), std::max(s1.y,s2), std::max(s1.z,s2)); +} + +//! Max operator +template +inline Vector3D vmax (S1 s1, const Vector3D& s2) { + return Vector3D(std::max(s1,s2.x), std::max(s1,s2.y), std::max(s1,s2.z)); +} + +//! Dot product +template +inline S dot ( const Vector3D &t, const Vector3D &v ) { + return t.x*v.x + t.y*v.y + t.z*v.z; +} + +//! Cross product +template +inline Vector3D cross ( const Vector3D &t, const Vector3D &v ) { + Vector3D 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 +inline const Vector3D& projectNormalTo ( const Vector3D& v, const Vector3D &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 +inline S norm ( const Vector3D& 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 +inline S normSquare ( const Vector3D& 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 +inline Vector3D getNormalized ( const Vector3D& 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 ( v.x*fac, v.y*fac, v.z*fac ); + } + else + return Vector3D ( ( S ) 0 ); +} + +//! Compute the norm of the vector and normalize it. +/*! \return The value of the norm */ +template +inline S normalize ( Vector3D &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::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 +Vector3D getOrthogonalVector(const Vector3D& 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 o ( 0.0 ); + o[ ( maxIndex+1 ) %3]= 1; + + Vector3D 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 +inline void vecToAngle ( const Vector3D& 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 +inline Vector3D reflectVector ( const Vector3D& t, const Vector3D& n ) { + Vector3D 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 +inline Vector3D refractVector ( const Vector3D &t, const Vector3D &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 std::string Vector3D::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::toString() const; + + +//! Outputs the object in human readable form to stream +/*! Output format [x,y,z] */ +template +std::ostream& operator<< ( std::ostream& os, const Vector3D& i ) { + os << i.toString(); + return os; +} + +//! Reads the contents of the object from a stream +/*! Input format [x,y,z] */ +template +std::istream& operator>> ( std::istream& is, Vector3D& 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 Vec3; + +//! 3D vector class of type int +typedef Vector3D Vec3i; + +//! convert to Real Vector +template inline Vec3 toVec3 ( T v ) { + return Vec3 ( v[0],v[1],v[2] ); +} + +//! convert to int Vector +template inline Vec3i toVec3i ( T v ) { + return Vec3i ( ( int ) v[0], ( int ) v[1], ( int ) v[2] ); +} + +//! convert to int Vector +template inline Vec3i toVec3i ( T v0, T v1, T v2 ) { + return Vec3i ( ( int ) v0, ( int ) v1, ( int ) v2 ); +} + +//! round, and convert to int Vector +template 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 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 inline Vector3D toVec3d ( T v ) { + return Vector3D ( v[0], v[1], v[2] ); +} + +//! convert to float Vector +template inline Vector3D toVec3f ( T v ) { + return Vector3D ( v[0], v[1], v[2] ); +} + + +/**************************************************************************/ +// Specializations for common math functions +/**************************************************************************/ + +template<> inline Vec3 clamp(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(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(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