From 9d3f8803b8cdfaf53896123ed991f34942cdc17d Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 30 Apr 2015 13:36:39 -0700 Subject: [PATCH] add Stan Melax' ImplicitCloth demo --- examples/ExampleBrowser/CMakeLists.txt | 5 + examples/ExampleBrowser/ExampleEntries.cpp | 11 +- .../ExampleBrowser/OpenGLExampleBrowser.cpp | 10 +- examples/ExampleBrowser/premake4.lua | 2 + .../ImplicitCloth/ImplicitClothExample.cpp | 119 ++ .../ImplicitCloth/ImplicitClothExample.h | 8 + .../Experiments/ImplicitCloth/stan/Cloth.cpp | 96 ++ .../Experiments/ImplicitCloth/stan/Cloth.h | 18 + .../ImplicitCloth/stan/SpringNetwork.cpp | 198 +++ .../ImplicitCloth/stan/SpringNetwork.h | 63 + .../Experiments/ImplicitCloth/stan/array.h | 284 ++++ .../Experiments/ImplicitCloth/stan/vec3n.cpp | 152 +++ .../Experiments/ImplicitCloth/stan/vec3n.h | 340 +++++ .../ImplicitCloth/stan/vecmath.cpp | 1183 +++++++++++++++++ .../Experiments/ImplicitCloth/stan/vecmath.h | 466 +++++++ 15 files changed, 2949 insertions(+), 6 deletions(-) create mode 100644 examples/Experiments/ImplicitCloth/ImplicitClothExample.cpp create mode 100644 examples/Experiments/ImplicitCloth/ImplicitClothExample.h create mode 100644 examples/Experiments/ImplicitCloth/stan/Cloth.cpp create mode 100644 examples/Experiments/ImplicitCloth/stan/Cloth.h create mode 100644 examples/Experiments/ImplicitCloth/stan/SpringNetwork.cpp create mode 100644 examples/Experiments/ImplicitCloth/stan/SpringNetwork.h create mode 100644 examples/Experiments/ImplicitCloth/stan/array.h create mode 100644 examples/Experiments/ImplicitCloth/stan/vec3n.cpp create mode 100644 examples/Experiments/ImplicitCloth/stan/vec3n.h create mode 100644 examples/Experiments/ImplicitCloth/stan/vecmath.cpp create mode 100644 examples/Experiments/ImplicitCloth/stan/vecmath.h diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index ecf803d47..c7e0553be 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -80,6 +80,11 @@ SET(App_ExampleBrowser_SRCS ../Constraints/Dof6Spring2Setup.h ../Constraints/ConstraintPhysicsSetup.cpp ../Constraints/ConstraintPhysicsSetup.h + ../Experiments/ImplicitCloth/ImplicitClothExample.cpp + ../Experiments/ImplicitCloth/stan/Cloth.cpp + ../Experiments/ImplicitCloth/stan/SpringNetwork.cpp + ../Experiments/ImplicitCloth/stan/vec3n.cpp + ../Experiments/ImplicitCloth/stan/vecmath.cpp ../ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index 65cc9a449..050ac92ab 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -25,6 +25,8 @@ #include "../SoftDemo/SoftDemo.h" #include "../Constraints/ConstraintDemo.h" #include "../Vehicles/Hinge2Vehicle.h" +#include "../Experiments/ImplicitCloth/ImplicitClothExample.h" + struct ExampleEntry { @@ -143,10 +145,15 @@ static ExampleEntry gDefaultExamples[]= "The demo implementation allows to choose various MLCP constraint solvers.", ForkLiftCreateFunc), - ExampleEntry(1,"Advanced"), + + + ExampleEntry(0,"Experiments"), ExampleEntry(1,"Voronoi Fracture", "Automatically create a compound rigid body using voronoi tesselation. Individual parts are modeled as rigid bodies using a btConvexHullShape.", - VoronoiFractureCreateFunc), + VoronoiFractureCreateFunc), + + ExampleEntry(1,"Implicit Cloth", "Cloth simulation using implicit integration, by Stan Melax. The cloth is only attached at the corners. Note the stability using a large time step even with high stiffness.", + ImplicitClothCreateFunc), ExampleEntry(0,"Rendering"), diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index c75aeb97b..d56a7a835 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp @@ -80,15 +80,17 @@ void MyKeyboardCallback(int key, int state) //printf("key=%d, state=%d\n", key, state); bool handled = false; + if (gui && !handled ) + { + handled = gui->keyboardCallback(key, state); + } + if (!handled && sCurrentDemo) { handled = sCurrentDemo->keyboardCallback(key,state); } - if (gui && !handled ) - { - handled = gui->keyboardCallback(key, state); - } + //checkout: is it desired to ignore keys, if the demo already handles them? diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua index e6bc81ce7..769527caf 100644 --- a/examples/ExampleBrowser/premake4.lua +++ b/examples/ExampleBrowser/premake4.lua @@ -39,6 +39,8 @@ "../Utils/b3Clock.*", "../GyroscopicDemo/GyroscopicSetup.cpp", "../GyroscopicDemo/GyroscopicSetup.h", + "../Experiments/ImplicitCloth/**.cpp", + "../Experiments/ImplicitCloth/**.h", "../ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/pose.cpp", "../ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/model.cpp", "../ThirdPartyLibs/urdf/urdfdom/urdf_parser/src/link.cpp", diff --git a/examples/Experiments/ImplicitCloth/ImplicitClothExample.cpp b/examples/Experiments/ImplicitCloth/ImplicitClothExample.cpp new file mode 100644 index 000000000..7a3df9dd6 --- /dev/null +++ b/examples/Experiments/ImplicitCloth/ImplicitClothExample.cpp @@ -0,0 +1,119 @@ +#include "ImplicitClothExample.h" + + +#include "../CommonInterfaces/CommonExampleInterface.h" +#include "../CommonInterfaces/CommonGUIHelperInterface.h" +#include "../CommonInterfaces/CommonRenderInterface.h" +#include "../CommonInterfaces/CommonCameraInterface.h" +#include "../CommonInterfaces/CommonGraphicsAppInterface.h" +#include "../CommonInterfaces/CommonWindowInterface.h" +#include "stan/vecmath.h" +#include "stan/Cloth.h" +#include "Bullet3Common/b3Vector3.h" +#include "Bullet3Common/b3AlignedObjectArray.h" + + +#ifdef _DEBUG +int numX = 20, numY=20; +#else +int numX = 60, numY=60; +#endif +const size_t total_points = (numX)*(numY); + + +struct ImplicitClothExample : public CommonExampleInterface +{ + struct GUIHelperInterface* m_guiHelper; + int m_option; + + Cloth* m_cloth = 0; + + + + +public: + ImplicitClothExample(struct GUIHelperInterface* helper, int option) + :m_guiHelper(helper), + m_option(option) + { + } + virtual void initPhysics(); + virtual void exitPhysics(); + virtual void stepSimulation(float deltaTime); + virtual void renderScene(); + virtual void physicsDebugDraw(int debugFlags);//for now we reuse the flags in Bullet/src/LinearMath/btIDebugDraw.h + virtual bool mouseMoveCallback(float x,float y) + { + return false; + } + virtual bool mouseButtonCallback(int button, int state, float x, float y) + { + return false; + } + virtual bool keyboardCallback(int key, int state) + { + return false; + } + + + +}; + + +void ImplicitClothExample::initPhysics() +{ + float size=10; + m_cloth = ClothCreate(numX,numY,size); + +} +void ImplicitClothExample::exitPhysics() +{ + delete m_cloth; + m_cloth=0; +} +void ImplicitClothExample::stepSimulation(float deltaTime) +{ + m_cloth->Simulate(deltaTime); + m_cloth->cloth_gravity.y = -9.8;//-9.8;//-9.8;//-9.8;//0;//-9.8;//0;//-9.8;//0;//-9.8; + m_cloth->cloth_gravity.z =-9.8;//0;//-9.8;//0;//-9.8; + + m_cloth->spring_struct=10000000.0f; + m_cloth->spring_shear=10000000.0f; + + //m_cloth->spring_struct=1000000.0f; + //m_cloth->spring_shear=1000000.0f; + + m_cloth->spring_damp = 0;//100; + + +} +void ImplicitClothExample::renderScene() +{ +} +void ImplicitClothExample::physicsDebugDraw(int debugFlags) +{ + + CommonRenderInterface* renderer = m_guiHelper->getRenderInterface(); + + b3AlignedObjectArray indices; + + for (int i=0;isprings.count;i++) + { + indices.push_back(m_cloth->springs[i].a); + indices.push_back(m_cloth->springs[i].b); + } + float lineColor[4]={0.4,0.4,1.0,1}; + renderer->drawLines(&m_cloth->X[0].x,lineColor,total_points,sizeof(float3),&indices[0],indices.size(),1); + + float pointColor[4]={1,0.4,0.4,1}; + +// renderer->drawPoints(&m_cloth->X[0].x,pointColor,total_points,sizeof(float3),3); + + +} + + +class CommonExampleInterface* ImplicitClothCreateFunc(struct PhysicsInterface* pint, struct GUIHelperInterface* helper, int option) +{ + return new ImplicitClothExample(helper, option); +} diff --git a/examples/Experiments/ImplicitCloth/ImplicitClothExample.h b/examples/Experiments/ImplicitCloth/ImplicitClothExample.h new file mode 100644 index 000000000..1856a5ed1 --- /dev/null +++ b/examples/Experiments/ImplicitCloth/ImplicitClothExample.h @@ -0,0 +1,8 @@ +#ifndef IMPLICIT_CLOTH_EXAMPLE_H +#define IMPLICIT_CLOTH_EXAMPLE_H + +class CommonExampleInterface* ImplicitClothCreateFunc(struct PhysicsInterface* pint, struct GUIHelperInterface* helper, int option); + + +#endif //IMPLICIT_CLOTH_EXAMPLE_H + diff --git a/examples/Experiments/ImplicitCloth/stan/Cloth.cpp b/examples/Experiments/ImplicitCloth/stan/Cloth.cpp new file mode 100644 index 000000000..35859e686 --- /dev/null +++ b/examples/Experiments/ImplicitCloth/stan/Cloth.cpp @@ -0,0 +1,96 @@ + +//----------------------------------------------------------------------------------------------- +// +// The remainder of this file shows how to use the spring network class with backward integration +// in order to implement a cloth system within a 3D game environment. +// The cloth class extends the springnetwork class in order to provide +// import/export, rendering support, and hooks into the game. +// + +#include "Cloth.h" + +Array cloths; + +Cloth::Cloth(const char *_name,int _n):SpringNetwork(_n), + color(0,0.5f,1.0f) +{ + + cloths.Add(this); +} +Cloth::~Cloth() +{ + cloths.Remove(this); +} + +// +// I/O support for serialization of our springnetwork and cloth objects. +// + + + + +int cloth_showbbox = 0; // for debug visualization shows bounding box. +float cloth_showvert = 0.025f; // size of box to put around current vert selected, 0 turns off + + + +Cloth *ClothCreate(int w,int h,float size) +{ + // simple cloth generation routine that creates a typical square cloth. + // better to use a real pipeline to generate these, this is just for testing. + int i,j; + Cloth *cloth = new Cloth("cloth",w*h); + cloth->w=w; + cloth->h=h; // later for rendering. + for(i=0;iX[i*w+j] = (float3(-0.5f,-0.5f,0)+float3((float)j/(w-1.0f),1.0f-(float)i/(h-1.0f),0)) * size; + } + for(i=0;iCreateSpring(SPRING_STRUCT,i*w+j,(i+1)*w+j); // structural + if(jCreateSpring(SPRING_STRUCT,i*w+j,i*w+(j+1)); // structural + if(jCreateSpring(SPRING_SHEAR ,i*w+j,(i+1)*w+(j+1)); // shear + if(j>0 &&iCreateSpring(SPRING_SHEAR ,i*w+j,(i+1)*w+(j-1)); // shear + if(iCreateSpring(SPRING_BEND ,i*w+j,(i+2)*w+j); // benders + if(jCreateSpring(SPRING_BEND ,i*w+j,i*w+(j+2)); // benders + } + cloth->UpdateLimits(); + return cloth; +} + + +int cloth_tess = 20; +float3 cloth_spawnpoint(0,3,5.0f); + + +/* +static void ClothDrawSprings(Cloth *cloth) +{ + static const float3 color[3]={float3(1,1,0),float3(1,0,1),float3(0,1,1)}; + float3N &X = cloth->X; + for(int i=0;isprings.count;i++) + { + SpringNetwork::Spring &s = cloth->springs[i]; + extern void Line(const float3 &,const float3 &,const float3 &color_rgb); + Line(X[s.a],X[s.b],color[s.type]); + } +} +*/ +int cloth_showsprings=0; + +void DoCloths() +{ + int i; + for(i=0;iSimulate((cloth->cloth_step<0)?DeltaT:cloth->cloth_step); + //if(cloth_showsprings) + // ClothDrawSprings(cloth); // debug visualization + + } +} diff --git a/examples/Experiments/ImplicitCloth/stan/Cloth.h b/examples/Experiments/ImplicitCloth/stan/Cloth.h new file mode 100644 index 000000000..4a4ec9166 --- /dev/null +++ b/examples/Experiments/ImplicitCloth/stan/Cloth.h @@ -0,0 +1,18 @@ +#ifndef STAN_CLOTH_H +#define STAN_CLOTH_H + +#include "SpringNetwork.h" + +class Cloth : public SpringNetwork +{ + public: + int w,h; + + float3 color; // for debug rendering + Cloth(const char* _name,int _n); + ~Cloth(); +}; + +Cloth *ClothCreate(int w,int h,float size); + +#endif //STAN_CLOTH_H diff --git a/examples/Experiments/ImplicitCloth/stan/SpringNetwork.cpp b/examples/Experiments/ImplicitCloth/stan/SpringNetwork.cpp new file mode 100644 index 000000000..d9b2b29c4 --- /dev/null +++ b/examples/Experiments/ImplicitCloth/stan/SpringNetwork.cpp @@ -0,0 +1,198 @@ +#include "vec3n.h" +//#include "console.h" + + +extern int numX; + + +// +// Cloth - Backward Integrated Spring Network +// +// (c) Stan Melax 2006 +// http://www.melax.com/cloth +// freeware demo and source +// Although its free software, I'll gaurantee and support this software as much as is reasonable. +// However, if you choose to use any of this code, then you agree that +// I assume no financial liability should the software not meet your expectations. +// But do feel free to send any feedback. +// +// The core backward integration functionality has all been extracted into the SpringNetwork class. +// This makes it easy for you if you just want to look at or use the math and the algorithms. +// The remainder of the code builds a cloth system with basic render support, I/O, and manipulators, +// so its possible to make use of the technology within a 3D application. +// This code is separated from the SpringNetwork class in order to avoid pushing a particular style +// and prevent any dependancies of the algorithms onto unrelated systems. +// Feel free to adapt any of this into your own 3D engine/environment. +// +// Instead of having unique Hooke force and damping coefficients on each spring, the SpringNetwork +// code uses a spring 'type' that indexes a short list of shared named coefficients. +// This was just more practical for the typical application of this technology. +// Over-designed systems that are too general can be slower for +// the next guy to understand and more painful to use. +// Editing/creation is easier when only 1 number needs to be changed. +// Nonetheless, feel free to adapt to your own needs. +// + +#include +#include + +#include "vec3n.h" +//#include "console.h" +//#include "manipulatori.h" +//#include "object.h" +//#include "xmlparse.h" + + + + +static const float3x3 I(1,0,0,0,1,0,0,0,1); + +inline float3x3 dfdx_spring(const float3 &dir,float length,float rest,float k) +{ + // dir is unit length direction, rest is spring's restlength, k is spring constant. + return ( (I-outerprod(dir,dir))*Min(1.0f,rest/length) - I) * -k; +} +inline float3x3 dfdx_damp(const float3 &dir,float length,const float3& vel,float rest,float damping) +{ + // inner spring damping vel is the relative velocity of the endpoints. + return (I-outerprod(dir,dir)) * (-damping * -(dot(dir,vel)/Max(length,rest))); +} +inline float3x3 dfdv_damp(const float3 &dir,float damping) +{ + // derivative of force wrt velocity. + return outerprod(dir,dir) * damping; +} + + + +#include "SpringNetwork.h" + + +SpringNetwork::SpringNetwork(int _n):X(_n),V(_n),F(_n),dV(_n),A(_n),dFdX(_n),dFdV(_n) +{ + assert(SPRING_STRUCT==0); + assert(&spring_shear == &spring_struct +SPRING_SHEAR); + assert(&spring_bend == &spring_struct +SPRING_BEND); + assert(&spring_struct== &spring_k[SPRING_STRUCT]); + assert(&spring_shear == &spring_k[SPRING_SHEAR ]); + assert(&spring_bend == &spring_k[SPRING_BEND ]); + // spring_struct=1000000.0f; + // spring_shear=1000000.0f; + + spring_struct=1000.0f; + spring_shear=100.0f; + + spring_bend=25.0f; + spring_damp=5.0f; + spring_air=1.0f; + spring_air=1.0f; + cloth_step = 0.25f; // delta time for cloth + cloth_gravity=float3(0,-10,0); + cloth_sleepthreshold = 0.001f; + cloth_sleepcount = 100; + awake = cloth_sleepcount; + + //fix/pin two points in worldspace + float3Nx3N::Block zero; + zero.m = float3x3(0,0,0,0,0,0,0,0,0); + zero.c = 0; + zero.r = 0; + S.blocks.Add(zero); + zero.r = numX-1; + S.blocks.Add(zero); + + + +} + + +SpringNetwork::Spring &SpringNetwork::AddBlocks(Spring &s) +{ + // Called during initial creation of springs in our spring network. + // Sets up the sparse matrices corresponding to connections. + // Note the indices (s.iab,s.iba) are also stored with spring to avoid looking them up each time a spring is applied + // All 3 matrices A,dFdX, and dFdV are contstructed identically so the block array layout will be the same for each. + s.iab = A.blocks.count; // added 'ab' blocks will have this index. + A.blocks.Add(float3Nx3N::Block(s.a,s.b)); + dFdX.blocks.Add(float3Nx3N::Block(s.a,s.b)); + dFdV.blocks.Add(float3Nx3N::Block(s.a,s.b)); + s.iba = A.blocks.count; // added 'ba' blocks will have this index. + A.blocks.Add(float3Nx3N::Block(s.b,s.a)); + dFdX.blocks.Add(float3Nx3N::Block(s.b,s.a)); + dFdV.blocks.Add(float3Nx3N::Block(s.b,s.a)); + return s; +} + +void SpringNetwork::PreSolveSpring(const SpringNetwork::Spring &s) +{ + // Adds this spring's contribution into force vector F and force derivitves dFdX and dFdV + // One optimization would be premultiply dfdx by dt*dt and F and dFdV by dt right here in this function. + // However, for educational purposes we wont do that now and intead just follow the paper directly. + //assert(dFdX.blocks[s.a].c==s.a); // delete this assert, no bugs here + //assert(dFdX.blocks[s.a].r==s.a); + float3 extent = X[s.b] - X[s.a]; + float length = magnitude(extent); + float3 dir = (length==0)?float3(0,0,0): extent * 1.0f/length; + float3 vel = V[s.b] - V[s.a]; + float k = spring_k[s.type]; + float3 f = dir * ((k * (length-s.restlen) ) + spring_damp * dot(vel,dir)); // spring force + damping force + F[s.a] += f; + F[s.b] -= f; + float3x3 dfdx = dfdx_spring(dir,length,s.restlen,k) + dfdx_damp(dir,length,vel,s.restlen,spring_damp); + dFdX.blocks[s.a].m -= dfdx; // diagonal chunk dFdX[a,a] + dFdX.blocks[s.b].m -= dfdx; // diagonal chunk dFdX[b,b] + dFdX.blocks[s.iab].m += dfdx; // off-diag chunk dFdX[a,b] + dFdX.blocks[s.iba].m += dfdx; // off-diag chunk dFdX[b,a] + float3x3 dfdv = dfdv_damp(dir,spring_damp); + dFdV.blocks[s.a].m -= dfdv; // diagonal chunk dFdV[a,a] + dFdV.blocks[s.b].m -= dfdv; // diagonal chunk dFdV[b,b] + dFdV.blocks[s.iab].m += dfdv; // off-diag chunk dFdV[a,b] + dFdV.blocks[s.iba].m += dfdv; // off-diag chunk dFdV[b,a] +} + + + + +void SpringNetwork::CalcForces() +{ + // Collect forces and derivatives: F,dFdX,dFdV + dFdX.Zero(); + dFdV.InitDiagonal(-spring_air); + F.Init(cloth_gravity); + + F.element[0]=float3(0,0,0); + F.element[numX-1]=float3(0,0,0); + + F -= V * spring_air; + for(int i=0;i springs; + float3N X; // positions of all points + float3N V; // velocities + float3N F; // force on each point + float3N dV; // change in velocity + float3Nx3N A; // big matrix we solve system with + float3Nx3N dFdX; // big matrix of derivative of force wrt position + float3Nx3N dFdV; // big matrix of derivative of force wrt velocity + float3Nx3N S; // used for our constraints - contains only some diagonal blocks as needed S[i,i] + int awake; + float3 bmin,bmax; + union + { + struct + { + float spring_struct; + float spring_shear; + float spring_bend; + }; + float spring_k[3]; + }; + float spring_damp; + float spring_air; + float cloth_step; // delta time for cloth + float3 cloth_gravity; + float cloth_sleepthreshold; + int cloth_sleepcount; + + SpringNetwork(int _n); + Spring &AddBlocks(Spring &s); + Spring &CreateSpring(int type,int a,int b,float restlen){return AddBlocks(springs.Add(Spring(type,a,b,restlen)));} + Spring &CreateSpring(int type,int a,int b){return CreateSpring(type,a,b,magnitude(X[b]-X[a]));} + void UpdateLimits() { BoxLimits(X.element,X.count,bmin,bmax);} + void Wake(){awake=cloth_sleepcount;} + void Simulate(float dt); + void PreSolveSpring(const Spring &s); + void CalcForces(); +}; + +#endif //STAN_SPRING_NETWORK_H diff --git a/examples/Experiments/ImplicitCloth/stan/array.h b/examples/Experiments/ImplicitCloth/stan/array.h new file mode 100644 index 000000000..d55714698 --- /dev/null +++ b/examples/Experiments/ImplicitCloth/stan/array.h @@ -0,0 +1,284 @@ +// +// Typical template dynamic array container class. +// By S Melax 1998 +// +// anyone is free to use, inspect, learn from, or ignore +// the code here as they see fit. +// +// A very simple template array class. +// Its easiest to understand this array +// class by seeing how it is used in code. +// +// For example: +// for(i=0;i +#include + +template class Array { + public: + Array(int s=0); + Array(Array &array); + ~Array(); + void allocate(int s); + void SetSize(int s); + void Pack(); + Type& Add(Type); + void AddUnique(Type); + int Contains(Type); + void Insert(Type,int); + int IndexOf(Type); + void Remove(Type); + void DelIndex(int i); + Type& DelIndexWithLast(int i); + Type * element; + int count; + int array_size; + const Type &operator[](int i) const { assert(i>=0 && i=0 && i ©(const Array &array); + Array &operator=(Array &array); +}; + + +template Array::Array(int s) +{ + if(s==-1) return; + count=0; + array_size = 0; + element = NULL; + if(s) + { + allocate(s); + } +} + + +template Array::Array(Array &array) +{ + count=0; + array_size = 0; + element = NULL; + *this = array; +} + + + + +template Array &Array::copy(const Array &array) +{ + assert(array.array_size>=0); + count=0; + for(int i=0;i Array &Array::operator=( Array &array) +{ + if(array.array_size<0) // negative number means steal the data buffer instead of copying + { + delete[] element; + element = array.element; + array_size = -array.array_size; + count = array.count; + array.count =array.array_size = 0; + array.element = NULL; + return *this; + } + count=0; + for(int i=0;i Array::~Array() +{ + if (element != NULL && array_size!=0) + { + delete[] element; + } + count=0;array_size=0;element=NULL; +} + +template void Array::allocate(int s) +{ + assert(s>0); + assert(s>=count); + if(s==array_size) return; + Type *old = element; + array_size =s; + element = new Type[array_size]; + assert(element); + for(int i=0;i void Array::SetSize(int s) +{ + if(s==0) + { + if(element) + { + delete[] element; + element = NULL; + } + array_size = s; + } + else + { + allocate(s); + } + count=s; +} + +template void Array::Pack() +{ + allocate(count); +} + +template Type& Array::Add(Type t) +{ + assert(count<=array_size); + if(count==array_size) + { + allocate((array_size)?array_size *2:16); + } + //int i; + //for(i=0;i int Array::Contains(Type t) +{ + int i; + int found=0; + for(i=0;i void Array::AddUnique(Type t) +{ + if(!Contains(t)) Add(t); +} + + +template void Array::DelIndex(int i) +{ + assert(i Type& Array::DelIndexWithLast(int i) +{ + assert(i void Array::Remove(Type t) +{ + int i; + for(i=0;i void Array::Insert(Type t,int k) +{ + int i=count; + Add(t); // to allocate space + while(i>k) + { + element[i]=element[i-1]; + i--; + } + assert(i==k); + element[k]=t; +} + + +template int Array::IndexOf(Type t) +{ + int i; + for(i=0;i + +#include "vec3n.h" + + +float conjgrad_lasterror; +float conjgrad_epsilon = 0.1f; +int conjgrad_loopcount; +int conjgrad_looplimit = 100; + +/*EXPORTVAR(conjgrad_lasterror); +EXPORTVAR(conjgrad_epsilon ); +EXPORTVAR(conjgrad_loopcount); +EXPORTVAR(conjgrad_looplimit); +*/ + +int ConjGradient(float3N &X, float3Nx3N &A, float3N &B) +{ + // Solves for unknown X in equation AX=B + conjgrad_loopcount=0; + int n=B.count; + float3N q(n),d(n),tmp(n),r(n); + r = B - Mul(tmp,A,X); // just use B if X known to be zero + d = r; + float s = dot(r,r); + float starget = s * squared(conjgrad_epsilon); + while( s>starget && conjgrad_loopcount++ < conjgrad_looplimit) + { + Mul(q,A,d); // q = A*d; + float a = s/dot(d,q); + X = X + d*a; + if(conjgrad_loopcount%50==0) + { + r = B - Mul(tmp,A,X); + } + else + { + r = r - q*a; + } + float s_prev = s; + s = dot(r,r); + d = r+d*(s/s_prev); + } + conjgrad_lasterror = s; + return conjgrad_loopcountstarget && conjgrad_loopcount++ < conjgrad_looplimit) + { + Mul(q,A,d); // q = A*d; + q[hack[0]] = q[hack[1]] = q[hack[2]] = float3(0,0,0); + float a = s/dot(d,q); + X = X + d*a; + if(conjgrad_loopcount%50==0) + { + r = B - Mul(tmp,A,X); + r[hack[0]] = r[hack[1]] = r[hack[2]] = float3(0,0,0); + } + else + { + r = r - q*a; + } + float s_prev = s; + s = dot(r,r); + d = r+d*(s/s_prev); + d[hack[0]] = d[hack[1]] = d[hack[2]] = float3(0,0,0); + } + conjgrad_lasterror = s; + return conjgrad_loopcountstarget && conjgrad_loopcount++ < conjgrad_looplimit) + { + Mul(q,A,d); // q = A*d; + filter(q,S); + float a = s/dot(d,q); + X = X + d*a; + if(conjgrad_loopcount%50==0) + { + r = B - Mul(tmp,A,X); + filter(r,S); + } + else + { + r = r - q*a; + } + float s_prev = s; + s = dot(r,r); + d = r+d*(s/s_prev); + filter(d,S); + } + conjgrad_lasterror = s; + return conjgrad_loopcount +//template void * vec4::operator new[](size_t n){ return _mm_malloc(n,64); } +//template void vec4::operator delete[](void *a) { _mm_free(a); } + + +struct HalfConstraint { + float3 n;int vi; + float s,t; + HalfConstraint(const float3& _n,int _vi,float _t):n(_n),vi(_vi),s(0),t(_t){} + HalfConstraint():vi(-1){} +}; + +class float3Nx3N +{ + public: + class Block + { + public: + + float3x3 m; + int r,c; + float unused[16]; + + + Block(){} + Block(short _r,short _c):r(_r),c(_c){m.x=m.y=m.z=float3(0,0,0);} + }; + Array blocks; // the first n blocks use as the diagonal. + int n; + void Zero(); + void InitDiagonal(float d); + void Identity(){InitDiagonal(1.0f);} + float3Nx3N():n(0){} + float3Nx3N(int _n):n(_n) {for(int i=0;i float3Nx3N &operator= (const E& expression) {expression.evalequals(*this);return *this;} + template float3Nx3N &operator+=(const E& expression) {expression.evalpluseq(*this);return *this;} + template float3Nx3N &operator-=(const E& expression) {expression.evalmnuseq(*this);return *this;} +}; + +class float3N: public Array +{ +public: + float3N(int _count=0) + { + SetSize(_count); + } + void Zero(); + void Init(const float3 &v); // sets each subvector to v + template float3N &operator= (const E& expression) {expression.evalequals(*this);return *this;} + template float3N &operator+=(const E& expression) {expression.evalpluseq(*this);return *this;} + template float3N &operator-=(const E& expression) {expression.evalmnuseq(*this);return *this;} + float3N &operator=( const float3N &V) { this->copy(V); return *this;} +}; + +int ConjGradient(float3N &X, float3Nx3N &A, float3N &B); +int ConjGradientFiltered(float3N &X, const float3Nx3N &A, const float3N &B,const float3Nx3N &S,Array &H); +int ConjGradientFiltered(float3N &X, const float3Nx3N &A, const float3N &B,const float3Nx3N &S); + +inline float3N& Mul(float3N &r,const float3Nx3N &m, const float3N &v) +{ + int i; + for(i=0;i // for memcpy +#include + +float squared(float a){return a*a;} +float clamp(float a,const float minval, const float maxval) {return Min(maxval,Max(minval,a));} +int clamp(int a,const int minval, const int maxval) {return Min(maxval,Max(minval,a));} + + +float Round(float a,float precision) +{ + return floorf(0.5f+a/precision)*precision; +} + + +float Interpolate(const float &f0,const float &f1,float alpha) +{ + return f0*(1-alpha) + f1*alpha; +} + + +int argmin(const float a[],int n) +{ + int r=0; + for(int i=1;ia[r]) + { + r = i; + } + } + return r; +} + + + +//------------ float3 (3D) -------------- + + + +float3 vabs(const float3 &v) +{ + return float3(fabsf(v.x),fabsf(v.y),fabsf(v.z)); +} + + + +float3 safenormalize(const float3 &v) +{ + if(magnitude(v)<=0.0f) + { + return float3(1,0,0); + } + return normalize(v); +} + +float3 Round(const float3 &a,float precision) +{ + return float3(Round(a.x,precision),Round(a.y,precision),Round(a.z,precision)); +} + + +float3 Interpolate(const float3 &v0,const float3 &v1,float alpha) +{ + return v0*(1-alpha) + v1*alpha; +} + +float3 Orth(const float3& v) +{ + float3 absv=vabs(v); + float3 u(1,1,1); + u[argmax(&absv[0],3)] =0.0f; + return normalize(cross(u,v)); +} + +void BoxLimits(const float3 *verts,int verts_count, float3 &bmin,float3 &bmax) +{ + bmin=float3( FLT_MAX, FLT_MAX, FLT_MAX); + bmax=float3(-FLT_MAX,-FLT_MAX,-FLT_MAX); + for(int i=0;ibmaxb[j]) return 0; + if(bminb[j]>bmaxa[j]) return 0; + } + return 1; +} + +// the statement v1*v2 is ambiguous since there are 3 types +// of vector multiplication +// - componantwise (for example combining colors) +// - dot product +// - cross product +// Therefore we never declare/implement this function. +// So we will never see: float3 operator*(float3 a,float3 b) + + + + +//------------ float3x3 --------------- +float Determinant(const float3x3 &m) +{ + return m.x.x*m.y.y*m.z.z + m.y.x*m.z.y*m.x.z + m.z.x*m.x.y*m.y.z + -m.x.x*m.z.y*m.y.z - m.y.x*m.x.y*m.z.z - m.z.x*m.y.y*m.x.z ; +} + +float3x3 Inverse(const float3x3 &a) +{ + float3x3 b; + float d=Determinant(a); + assert(d!=0); + for(int i=0;i<3;i++) + { + for(int j=0;j<3;j++) + { + int i1=(i+1)%3; + int i2=(i+2)%3; + int j1=(j+1)%3; + int j2=(j+2)%3; + // reverse indexs i&j to take transpose + b[j][i] = (a[i1][j1]*a[i2][j2]-a[i1][j2]*a[i2][j1])/d; + } + } + // Matrix check=a*b; // Matrix 'check' should be the identity (or close to it) + return b; +} + + +float3x3 Transpose( const float3x3& m ) +{ + return float3x3( float3(m.x.x,m.y.x,m.z.x), + float3(m.x.y,m.y.y,m.z.y), + float3(m.x.z,m.y.z,m.z.z)); +} + + +float3 operator*(const float3& v , const float3x3 &m ) { + return float3((m.x.x*v.x + m.y.x*v.y + m.z.x*v.z), + (m.x.y*v.x + m.y.y*v.y + m.z.y*v.z), + (m.x.z*v.x + m.y.z*v.y + m.z.z*v.z)); +} +float3 operator*(const float3x3 &m,const float3& v ) { + return float3(dot(m.x,v),dot(m.y,v),dot(m.z,v)); +} + + +float3x3 operator*( const float3x3& a, const float3x3& b ) +{ + return float3x3(a.x*b,a.y*b,a.z*b); +} + +float3x3 operator*( const float3x3& a, const float& s ) +{ + return float3x3(a.x*s, a.y*s ,a.z*s); +} +float3x3 operator/( const float3x3& a, const float& s ) +{ + float t=1/s; + return float3x3(a.x*t, a.y*t ,a.z*t); +} +float3x3 operator+( const float3x3& a, const float3x3& b ) +{ + return float3x3(a.x+b.x, a.y+b.y, a.z+b.z); +} +float3x3 operator-( const float3x3& a, const float3x3& b ) +{ + return float3x3(a.x-b.x, a.y-b.y, a.z-b.z); +} +float3x3 &operator+=( float3x3& a, const float3x3& b ) +{ + a.x+=b.x; + a.y+=b.y; + a.z+=b.z; + return a; +} +float3x3 &operator-=( float3x3& a, const float3x3& b ) +{ + a.x-=b.x; + a.y-=b.y; + a.z-=b.z; + return a; +} +float3x3 &operator*=( float3x3& a, const float& s ) +{ + a.x*=s; + a.y*=s; + a.z*=s; + return a; +} + +float3x3 outerprod(const float3& a,const float3& b) +{ + return float3x3(a.x*b,a.y*b,a.z*b); // a is a column vector b is a row vector +} + +//--------------- 4D ---------------- + +float4 operator*( const float4& v, const float4x4& m ) +{ + return v.x*m.x + v.y*m.y + v.z*m.z + v.w*m.w; // yes this actually works +} + + +// Dont implement m*v for now, since that might confuse us +// All our transforms are based on multiplying the "row" vector on the left +//float4 operator*(const float4x4& m , const float4& v ) +//{ +// return float4(dot(v,m.x),dot(v,m.y),dot(v,m.z),dot(v,m.w)); +//} + + +float4x4 operator*( const float4x4& a, const float4x4& b ) +{ + return float4x4(a.x*b,a.y*b,a.z*b,a.w*b); +} + +float4x4 MatrixTranspose(const float4x4 &m) +{ + return float4x4( + m.x.x, m.y.x, m.z.x, m.w.x, + m.x.y, m.y.y, m.z.y, m.w.y, + m.x.z, m.y.z, m.z.z, m.w.z, + m.x.w, m.y.w, m.z.w, m.w.w ); +} + +float4x4 MatrixRigidInverse(const float4x4 &m) +{ + float4x4 trans_inverse = MatrixTranslation(-m.w.xyz()); + float4x4 rot = m; + rot.w = float4(0,0,0,1); + return trans_inverse * MatrixTranspose(rot); +} + + +float4x4 MatrixPerspectiveFov(float fovy, float aspect, float zn, float zf ) +{ + float h = 1.0f/tanf(fovy/2.0f); // view space height + float w = h / aspect ; // view space width + return float4x4( + w, 0, 0 , 0, + 0, h, 0 , 0, + 0, 0, zf/(zn-zf) , -1, + 0, 0, zn*zf/(zn-zf) , 0 ); +} + + + +float4x4 MatrixLookAt(const float3& eye, const float3& at, const float3& up) +{ + float4x4 m; + m.w.w = 1.0f; + m.w.xyz() = eye; + m.z.xyz() = normalize(eye-at); + m.x.xyz() = normalize(cross(up,m.z.xyz())); + m.y.xyz() = cross(m.z.xyz(),m.x.xyz()); + return MatrixRigidInverse(m); +} + + +float4x4 MatrixTranslation(const float3 &t) +{ + return float4x4( + 1, 0, 0, 0, + 0, 1, 0, 0, + 0, 0, 1, 0, + t.x,t.y,t.z,1 ); +} + + +float4x4 MatrixRotationZ(const float angle_radians) +{ + float s = sinf(angle_radians); + float c = cosf(angle_radians); + return float4x4( + c, s, 0, 0, + -s, c, 0, 0, + 0, 0, 1, 0, + 0, 0, 0, 1 ); +} + + + +int operator==( const float4x4 &a, const float4x4 &b ) +{ + return (a.x==b.x && a.y==b.y && a.z==b.z && a.w==b.w); +} + + +float4x4 Inverse(const float4x4 &m) +{ + float4x4 d; + float *dst = &d.x.x; + float tmp[12]; /* temp array for pairs */ + float src[16]; /* array of transpose source matrix */ + float det; /* determinant */ + /* transpose matrix */ + for ( int i = 0; i < 4; i++) { + src[i] = m(i,0) ; + src[i + 4] = m(i,1); + src[i + 8] = m(i,2); + src[i + 12] = m(i,3); + } + /* calculate pairs for first 8 elements (cofactors) */ + tmp[0] = src[10] * src[15]; + tmp[1] = src[11] * src[14]; + tmp[2] = src[9] * src[15]; + tmp[3] = src[11] * src[13]; + tmp[4] = src[9] * src[14]; + tmp[5] = src[10] * src[13]; + tmp[6] = src[8] * src[15]; + tmp[7] = src[11] * src[12]; + tmp[8] = src[8] * src[14]; + tmp[9] = src[10] * src[12]; + tmp[10] = src[8] * src[13]; + tmp[11] = src[9] * src[12]; + /* calculate first 8 elements (cofactors) */ + dst[0] = tmp[0]*src[5] + tmp[3]*src[6] + tmp[4]*src[7]; + dst[0] -= tmp[1]*src[5] + tmp[2]*src[6] + tmp[5]*src[7]; + dst[1] = tmp[1]*src[4] + tmp[6]*src[6] + tmp[9]*src[7]; + dst[1] -= tmp[0]*src[4] + tmp[7]*src[6] + tmp[8]*src[7]; + dst[2] = tmp[2]*src[4] + tmp[7]*src[5] + tmp[10]*src[7]; + dst[2] -= tmp[3]*src[4] + tmp[6]*src[5] + tmp[11]*src[7]; + dst[3] = tmp[5]*src[4] + tmp[8]*src[5] + tmp[11]*src[6]; + dst[3] -= tmp[4]*src[4] + tmp[9]*src[5] + tmp[10]*src[6]; + dst[4] = tmp[1]*src[1] + tmp[2]*src[2] + tmp[5]*src[3]; + dst[4] -= tmp[0]*src[1] + tmp[3]*src[2] + tmp[4]*src[3]; + dst[5] = tmp[0]*src[0] + tmp[7]*src[2] + tmp[8]*src[3]; + dst[5] -= tmp[1]*src[0] + tmp[6]*src[2] + tmp[9]*src[3]; + dst[6] = tmp[3]*src[0] + tmp[6]*src[1] + tmp[11]*src[3]; + dst[6] -= tmp[2]*src[0] + tmp[7]*src[1] + tmp[10]*src[3]; + dst[7] = tmp[4]*src[0] + tmp[9]*src[1] + tmp[10]*src[2]; + dst[7] -= tmp[5]*src[0] + tmp[8]*src[1] + tmp[11]*src[2]; + /* calculate pairs for second 8 elements (cofactors) */ + tmp[0] = src[2]*src[7]; + tmp[1] = src[3]*src[6]; + tmp[2] = src[1]*src[7]; + tmp[3] = src[3]*src[5]; + tmp[4] = src[1]*src[6]; + tmp[5] = src[2]*src[5]; + tmp[6] = src[0]*src[7]; + tmp[7] = src[3]*src[4]; + tmp[8] = src[0]*src[6]; + tmp[9] = src[2]*src[4]; + tmp[10] = src[0]*src[5]; + tmp[11] = src[1]*src[4]; + /* calculate second 8 elements (cofactors) */ + dst[8] = tmp[0]*src[13] + tmp[3]*src[14] + tmp[4]*src[15]; + dst[8] -= tmp[1]*src[13] + tmp[2]*src[14] + tmp[5]*src[15]; + dst[9] = tmp[1]*src[12] + tmp[6]*src[14] + tmp[9]*src[15]; + dst[9] -= tmp[0]*src[12] + tmp[7]*src[14] + tmp[8]*src[15]; + dst[10] = tmp[2]*src[12] + tmp[7]*src[13] + tmp[10]*src[15]; + dst[10]-= tmp[3]*src[12] + tmp[6]*src[13] + tmp[11]*src[15]; + dst[11] = tmp[5]*src[12] + tmp[8]*src[13] + tmp[11]*src[14]; + dst[11]-= tmp[4]*src[12] + tmp[9]*src[13] + tmp[10]*src[14]; + dst[12] = tmp[2]*src[10] + tmp[5]*src[11] + tmp[1]*src[9]; + dst[12]-= tmp[4]*src[11] + tmp[0]*src[9] + tmp[3]*src[10]; + dst[13] = tmp[8]*src[11] + tmp[0]*src[8] + tmp[7]*src[10]; + dst[13]-= tmp[6]*src[10] + tmp[9]*src[11] + tmp[1]*src[8]; + dst[14] = tmp[6]*src[9] + tmp[11]*src[11] + tmp[3]*src[8]; + dst[14]-= tmp[10]*src[11] + tmp[2]*src[8] + tmp[7]*src[9]; + dst[15] = tmp[10]*src[10] + tmp[4]*src[8] + tmp[9]*src[9]; + dst[15]-= tmp[8]*src[9] + tmp[11]*src[10] + tmp[5]*src[8]; + /* calculate determinant */ + det=src[0]*dst[0]+src[1]*dst[1]+src[2]*dst[2]+src[3]*dst[3]; + /* calculate matrix inverse */ + det = 1/det; + for ( int j = 0; j < 16; j++) + dst[j] *= det; + return d; +} + + +//--------- Quaternion -------------- + +template<> void Quaternion::Normalize() +{ + float m = sqrtf(squared(w)+squared(x)+squared(y)+squared(z)); + if(m<0.000000001f) { + w=1.0f; + x=y=z=0.0f; + return; + } + (*this) *= (1.0f/m); +} + +float3 rotate( const Quaternion& q, const float3& v ) +{ + // The following is equivalent to: + //return (q.getmatrix() * v); + float qx2 = q.x*q.x; + float qy2 = q.y*q.y; + float qz2 = q.z*q.z; + + float qxqy = q.x*q.y; + float qxqz = q.x*q.z; + float qxqw = q.x*q.w; + float qyqz = q.y*q.z; + float qyqw = q.y*q.w; + float qzqw = q.z*q.w; + return float3( + (1-2*(qy2+qz2))*v.x + (2*(qxqy-qzqw))*v.y + (2*(qxqz+qyqw))*v.z , + (2*(qxqy+qzqw))*v.x + (1-2*(qx2+qz2))*v.y + (2*(qyqz-qxqw))*v.z , + (2*(qxqz-qyqw))*v.x + (2*(qyqz+qxqw))*v.y + (1-2*(qx2+qy2))*v.z ); +} + + +Quaternion slerp(const Quaternion &_a, const Quaternion& b, float interp ) +{ + Quaternion a=_a; + if(dot(a,b) <0.0) + { + a.w=-a.w; + a.x=-a.x; + a.y=-a.y; + a.z=-a.z; + } + float d = dot(a,b); + if(d>=1.0) { + return a; + } + float theta = acosf(d); + if(theta==0.0f) { return(a);} + return a*(sinf(theta-interp*theta)/sinf(theta)) + b*(sinf(interp*theta)/sinf(theta)); +} + + +Quaternion Interpolate(const Quaternion &q0,const Quaternion &q1,float alpha) { + return slerp(q0,q1,alpha); +} + + +Quaternion YawPitchRoll( float yaw, float pitch, float roll ) +{ + return QuatFromAxisAngle(float3(0.0f,0.0f,1.0f),DegToRad(yaw )) + * QuatFromAxisAngle(float3(1.0f,0.0f,0.0f),DegToRad(pitch)) + * QuatFromAxisAngle(float3(0.0f,1.0f,0.0f),DegToRad(roll )); +} + +float Yaw( const Quaternion& q ) +{ + static float3 v; + v=q.ydir(); + return (v.y==0.0&&v.x==0.0) ? 0.0f: RadToDeg(atan2f(-v.x,v.y)); +} + +float Pitch( const Quaternion& q ) +{ + static float3 v; + v=q.ydir(); + return RadToDeg(atan2f(v.z,sqrtf(squared(v.x)+squared(v.y)))); +} + +float Roll( const Quaternion &_q ) +{ + Quaternion q=_q; + q = QuatFromAxisAngle(float3(0.0f,0.0f,1.0f),-DegToRad(Yaw(q))) *q; + q = QuatFromAxisAngle(float3(1.0f,0.0f,0.0f),-DegToRad(Pitch(q))) *q; + return RadToDeg(atan2f(-q.xdir().z,q.xdir().x)); +} + +float Yaw( const float3& v ) +{ + return (v.y==0.0&&v.x==0.0) ? 0.0f: RadToDeg(atan2f(-v.x,v.y)); +} + +float Pitch( const float3& v ) +{ + return RadToDeg(atan2f(v.z,sqrtf(squared(v.x)+squared(v.y)))); +} + + + + + + +//--------- utility functions ------------- + +// RotationArc() +// Given two vectors v0 and v1 this function +// returns quaternion q where q*v0==v1. +// Routine taken from game programming gems. +Quaternion RotationArc(float3 v0,float3 v1){ + static Quaternion q; + v0 = normalize(v0); // Comment these two lines out if you know its not needed. + v1 = normalize(v1); // If vector is already unit length then why do it again? + float3 c = cross(v0,v1); + float d = dot(v0,v1); + if(d<=-1.0f) { float3 a=Orth(v0); return Quaternion(a.x,a.y,a.z,0);} // 180 about any orthogonal axis axis + float s = sqrtf((1+d)*2); + q.x = c.x / s; + q.y = c.y / s; + q.z = c.z / s; + q.w = s /2.0f; + return q; +} + + +float4x4 MatrixFromQuatVec(const Quaternion &q, const float3 &v) +{ + // builds a 4x4 transformation matrix based on orientation q and translation v + float qx2 = q.x*q.x; + float qy2 = q.y*q.y; + float qz2 = q.z*q.z; + + float qxqy = q.x*q.y; + float qxqz = q.x*q.z; + float qxqw = q.x*q.w; + float qyqz = q.y*q.z; + float qyqw = q.y*q.w; + float qzqw = q.z*q.w; + + return float4x4( + 1-2*(qy2+qz2), + 2*(qxqy+qzqw), + 2*(qxqz-qyqw), + 0 , + 2*(qxqy-qzqw), + 1-2*(qx2+qz2), + 2*(qyqz+qxqw), + 0 , + 2*(qxqz+qyqw), + 2*(qyqz-qxqw), + 1-2*(qx2+qy2), + 0 , + v.x , + v.y , + v.z , + 1.0f ); +} + + + +float3 PlaneLineIntersection(const float3 &normal,const float dist, const float3 &p0, const float3 &p1) +{ + // returns the point where the line p0-p1 intersects the plane n&d + float3 dif; + dif = p1-p0; + float dn= dot(normal,dif); + float t = -(dist+dot(normal,p0) )/dn; + return p0 + (dif*t); +} + +float3 LineProject(const float3 &p0, const float3 &p1, const float3 &a) +{ + // project point a on segment [p0,p1] + float3 d= p1-p0; + float t= dot(d,(a-p0)) / dot(d,d); + return p0+ d*t; +} + + +float LineProjectTime(const float3 &p0, const float3 &p1, const float3 &a) +{ + // project point a on segment [p0,p1] + float3 d= p1-p0; + float t= dot(d,(a-p0)) / dot(d,d); + return t; +} + + + +float3 TriNormal(const float3 &v0, const float3 &v1, const float3 &v2) +{ + // return the normal of the triangle + // inscribed by v0, v1, and v2 + float3 cp=cross(v1-v0,v2-v1); + float m=magnitude(cp); + if(m==0) return float3(1,0,0); + return cp*(1.0f/m); +} + + + +int BoxInside(const float3 &p, const float3 &bmin, const float3 &bmax) +{ + return (p.x >= bmin.x && p.x <=bmax.x && + p.y >= bmin.y && p.y <=bmax.y && + p.z >= bmin.z && p.z <=bmax.z ); +} + + +int BoxIntersect(const float3 &v0, const float3 &v1, const float3 &bmin, const float3 &bmax,float3 *impact) +{ + if(BoxInside(v0,bmin,bmax)) + { + *impact=v0; + return 1; + } + if(v0.x<=bmin.x && v1.x>=bmin.x) + { + float a = (bmin.x-v0.x)/(v1.x-v0.x); + //v.x = bmin.x; + float vy = (1-a) *v0.y + a*v1.y; + float vz = (1-a) *v0.z + a*v1.z; + if(vy>=bmin.y && vy<=bmax.y && vz>=bmin.z && vz<=bmax.z) + { + impact->x = bmin.x; + impact->y = vy; + impact->z = vz; + return 1; + } + } + else if(v0.x >= bmax.x && v1.x <= bmax.x) + { + float a = (bmax.x-v0.x)/(v1.x-v0.x); + //v.x = bmax.x; + float vy = (1-a) *v0.y + a*v1.y; + float vz = (1-a) *v0.z + a*v1.z; + if(vy>=bmin.y && vy<=bmax.y && vz>=bmin.z && vz<=bmax.z) + { + impact->x = bmax.x; + impact->y = vy; + impact->z = vz; + return 1; + } + } + if(v0.y<=bmin.y && v1.y>=bmin.y) + { + float a = (bmin.y-v0.y)/(v1.y-v0.y); + float vx = (1-a) *v0.x + a*v1.x; + //v.y = bmin.y; + float vz = (1-a) *v0.z + a*v1.z; + if(vx>=bmin.x && vx<=bmax.x && vz>=bmin.z && vz<=bmax.z) + { + impact->x = vx; + impact->y = bmin.y; + impact->z = vz; + return 1; + } + } + else if(v0.y >= bmax.y && v1.y <= bmax.y) + { + float a = (bmax.y-v0.y)/(v1.y-v0.y); + float vx = (1-a) *v0.x + a*v1.x; + // vy = bmax.y; + float vz = (1-a) *v0.z + a*v1.z; + if(vx>=bmin.x && vx<=bmax.x && vz>=bmin.z && vz<=bmax.z) + { + impact->x = vx; + impact->y = bmax.y; + impact->z = vz; + return 1; + } + } + if(v0.z<=bmin.z && v1.z>=bmin.z) + { + float a = (bmin.z-v0.z)/(v1.z-v0.z); + float vx = (1-a) *v0.x + a*v1.x; + float vy = (1-a) *v0.y + a*v1.y; + // v.z = bmin.z; + if(vy>=bmin.y && vy<=bmax.y && vx>=bmin.x && vx<=bmax.x) + { + impact->x = vx; + impact->y = vy; + impact->z = bmin.z; + return 1; + } + } + else if(v0.z >= bmax.z && v1.z <= bmax.z) + { + float a = (bmax.z-v0.z)/(v1.z-v0.z); + float vx = (1-a) *v0.x + a*v1.x; + float vy = (1-a) *v0.y + a*v1.y; + // v.z = bmax.z; + if(vy>=bmin.y && vy<=bmax.y && vx>=bmin.x && vx<=bmax.x) + { + impact->x = vx; + impact->y = vy; + impact->z = bmax.z; + return 1; + } + } + return 0; +} + + +float DistanceBetweenLines(const float3 &ustart, const float3 &udir, const float3 &vstart, const float3 &vdir, float3 *upoint, float3 *vpoint) +{ + static float3 cp; + cp = normalize(cross(udir,vdir)); + + float distu = -dot(cp,ustart); + float distv = -dot(cp,vstart); + float dist = (float)fabs(distu-distv); + if(upoint) + { + float3 normal = normalize(cross(vdir,cp)); + *upoint = PlaneLineIntersection(normal,-dot(normal,vstart),ustart,ustart+udir); + } + if(vpoint) + { + float3 normal = normalize(cross(udir,cp)); + *vpoint = PlaneLineIntersection(normal,-dot(normal,ustart),vstart,vstart+vdir); + } + return dist; +} + + +Quaternion VirtualTrackBall(const float3 &cop, const float3 &cor, const float3 &dir1, const float3 &dir2) +{ + // routine taken from game programming gems. + // Implement track ball functionality to spin stuf on the screen + // cop center of projection + // cor center of rotation + // dir1 old mouse direction + // dir2 new mouse direction + // pretend there is a sphere around cor. Then find the points + // where dir1 and dir2 intersect that sphere. Find the + // rotation that takes the first point to the second. + float m; + // compute plane + float3 nrml = cor - cop; + float fudgefactor = 1.0f/(magnitude(nrml) * 0.25f); // since trackball proportional to distance from cop + nrml = normalize(nrml); + float dist = -dot(nrml,cor); + float3 u= PlaneLineIntersection(nrml,dist,cop,cop+dir1); + u=u-cor; + u=u*fudgefactor; + m= magnitude(u); + if(m>1) + { + u/=m; + } + else + { + u=u - (nrml * sqrtf(1-m*m)); + } + float3 v= PlaneLineIntersection(nrml,dist,cop,cop+dir2); + v=v-cor; + v=v*fudgefactor; + m= magnitude(v); + if(m>1) + { + v/=m; + } + else + { + v=v - (nrml * sqrtf(1-m*m)); + } + return RotationArc(u,v); +} + + +int countpolyhit=0; +int HitCheckPoly(const float3 *vert, const int n, const float3 &v0, const float3 &v1, float3 *impact, float3 *normal) +{ + countpolyhit++; + int i; + float3 nrml(0,0,0); + for(i=0;i0) + { + return 0; + } + + static float3 the_point; + // By using the cached plane distances d0 and d1 + // we can optimize the following: + // the_point = planelineintersection(nrml,dist,v0,v1); + float a = d0/(d0-d1); + the_point = v0*(1-a) + v1*a; + + + int inside=1; + for(int j=0;inside && j= 0.0); + } + if(inside) + { + if(normal){*normal=nrml;} + if(impact){*impact=the_point;} + } + return inside; +} + +int SolveQuadratic(float a,float b,float c,float *ta,float *tb) // if true returns roots ta,tb where ta<=tb +{ + assert(ta); + assert(tb); + float d = b*b-4.0f*a*c; // discriminant + if(d<0.0f) return 0; + float sqd = sqrtf(d); + *ta = (-b-sqd) / (2.0f * a); + *tb = (-b+sqd) / (2.0f * a); + return 1; +} + +int HitCheckRaySphere(const float3& sphereposition,float radius, const float3& _v0, const float3& _v1, float3 *impact,float3 *normal) +{ + assert(impact); + assert(normal); + float3 dv = _v1-_v0; + float3 v0 = _v0 - sphereposition; // solve in coord system of the sphere + if(radius<=0.0f || _v0==_v1) return 0; // only true if point moves from outside to inside sphere. + float a = dot(dv,dv); + float b = 2.0f * dot(dv,v0); + float c = dot(v0,v0) - radius*radius; + if(c<0.0f) return 0; // we are already inside the sphere. + + float ta, tb; + int doesIntersect = SolveQuadratic(a, b, c, &ta, &tb); + + if (!doesIntersect) return 0; + + if (ta >= 0.0f && ta <= 1.0f && (ta <= tb || tb<=0.0f)) + { + *impact = _v0 + dv * ta; + *normal = (v0 + dv*ta)/radius; + return 1; + } + if (tb >= 0.0f && tb <= 1.0f) + { + assert(tb <= ta || ta <=0.0f); // tb must be better than ta + *impact = _v0 + dv * tb; + *normal = (v0 + dv*tb)/radius; + return 1; + } + return 0; +} + +int HitCheckRayCylinder(const float3 &p0,const float3 &p1,float radius,const float3& _v0,const float3& _v1, float3 *impact,float3 *normal) +{ + assert(impact); + assert(normal); + // only concerned about hitting the sides, not the caps for now + float3x3 m=RotationArc(p1-p0,float3(0,0,1.0f)).getmatrix(); + float h = ((p1-p0)*m ).z; + float3 v0 = (_v0-p0) *m; + float3 v1 = (_v1-p0) *m; + if(v0.z <= 0.0f && v1.z <= 0.0f) return 0; // entirely below cylinder + if(v0.z >= h && v1.z >= h ) return 0; // ray is above cylinder + if(v0.z <0.0f ) v0 = PlaneLineIntersection(float3(0,0,1.0f), 0,v0,v1); // crop to cylinder range + if(v1.z <0.0f ) v1 = PlaneLineIntersection(float3(0,0,1.0f), 0,v0,v1); + if(v0.z > h ) v0 = PlaneLineIntersection(float3(0,0,1.0f),-h,v0,v1); + if(v1.z > h ) v1 = PlaneLineIntersection(float3(0,0,1.0f),-h,v0,v1); + if(v0.x==v1.x && v0.y==v1.y) return 0; + float3 dv = v1-v0; + + float a = dv.x*dv.x+dv.y*dv.y; + float b = 2.0f * (dv.x*v0.x+dv.y*v0.y); + float c = (v0.x*v0.x+v0.y*v0.y) - radius*radius; + if(c<0.0f) return 0; // we are already inside the cylinder . + + float ta, tb; + int doesIntersect = SolveQuadratic(a, b, c, &ta, &tb); + + if (!doesIntersect) return 0; + + if (ta >= 0.0f && ta <= 1.0f && (ta <= tb || tb<=0.0f)) + { + *impact = (v0 + dv * ta)*Transpose(m) + p0; + *normal = (float3(v0.x,v0.y,0.0f) + float3(dv.x,dv.y,0) * ta) /radius * Transpose(m); + return 1; + } + if (tb >= 0.0f && tb <= 1.0f) + { + assert(tb <= ta || ta <=0.0f); // tb must be better than ta + *impact = (v0 + dv * tb)*Transpose(m) + p0; // compute intersection in original space + *normal = (float3(v0.x,v0.y,0.0f) + float3(dv.x,dv.y,0) * tb) /radius * Transpose(m); + return 1; + } + return 0; +} + +int HitCheckSweptSphereTri(const float3 &p0,const float3 &p1,const float3 &p2,float radius, const float3& v0,const float3& _v1, float3 *impact,float3 *normal) +{ + float3 unused; + if(!normal) normal=&unused; + float3 v1=_v1; // so we can update v1 after each sub intersection test if necessary + int hit=0; + float3 cp = cross(p1-p0,p2-p0); + if(dot(cp,v1-v0)>=0.0f) return 0; // coming from behind and/or moving away + float3 n = normalize(cp); + float3 tv[3]; + tv[0] = p0 + n*radius; + tv[1] = p1 + n*radius; + tv[2] = p2 + n*radius; + hit += HitCheckPoly(tv,3,v0,v1,&v1,normal); + hit += HitCheckRayCylinder(p0,p1,radius,v0,v1,&v1,normal); + hit += HitCheckRayCylinder(p1,p2,radius,v0,v1,&v1,normal); + hit += HitCheckRayCylinder(p2,p0,radius,v0,v1,&v1,normal); + hit += HitCheckRaySphere(p0,radius,v0,v1,&v1,normal); + hit += HitCheckRaySphere(p1,radius,v0,v1,&v1,normal); + hit += HitCheckRaySphere(p2,radius,v0,v1,&v1,normal); + if(hit && impact) *impact = v1 + *normal * 0.001f; + return hit; +} + + +float3 PlanesIntersection(const Plane &p0,const Plane &p1, const Plane &p2) +{ + float3x3 mp =Transpose(float3x3(p0.normal(),p1.normal(),p2.normal())); + float3x3 mi = Inverse(mp); + float3 b(p0.dist(),p1.dist(),p2.dist()); + return -b * mi; +} + + +float3 PlanesIntersection(const Plane *planes,int planes_count,const float3 &seed) +{ + int i; + float3x3 A; // gets initilized to 0 matrix + float3 b(0,0,0); + for(i=0;i= count+1 + assert(verts_out); + int n=0; + int prev_status = (dot(plane_normal,verts_in[count_in-1])+plane_dist > 0) ; + for(int i=0;i 0) ; + if(status != prev_status) + { + verts_out[n++] = PlaneLineIntersection(plane_normal,plane_dist,verts_in[(i==0)?count_in-1:i-1],verts_in[i]); + } + if(status==0) // under + { + verts_out[n++] = verts_in[i]; + } + } + assert(n<=count_in+1); // remove if intention to use this routine on convex polygons + return n; +} + +int ClipPolyPoly(const float3 &normal,const float3 *clipper,int clipper_count,const float3 *verts_in, int in_count,float3 *scratch) +{ + // clips polys against each other. + // requires sufficiently allocated temporary memory in scratch buffer + // function returns final number of vertices in clipped polygon. + // Resulting vertices are returned in the scratch buffer. + // if the arrays are the same &verts_in==&scratch the routine should still work anyways. + // the first argument (normal) is the normal of polygon clipper. + // its generally assumed both are convex polygons. + assert(scratch); // size should be >= 2*(clipper_count+in_count) + int i; + int bsize = clipper_count+in_count; + int count = in_count; + for(i=0;iom.y&&om.x>om.z)?0: (om.y>om.z)? 1 : 2; // index of largest element of offdiag + int k1 = (k+1)%3; + int k2 = (k+2)%3; + if(offdiag[k]==0.0f) break; // diagonal already + float thet = (D[k2][k2]-D[k1][k1])/(2.0f*offdiag[k]); + float sgn = (thet>0.0f)?1.0f:-1.0f; + thet *= sgn; // make it positive + float t = sgn /(thet +((thet<1.E6f)?sqrtf(squared(thet)+1.0f):thet)) ; // sign(T)/(|T|+sqrt(T^2+1)) + float c = 1.0f/sqrtf(squared(t)+1.0f); // c= 1/(t^2+1) , t=s/c + if(c==1.0f) break; // no room for improvement - reached machine precision. + Quaternion jr(0,0,0,0); // jacobi rotation for this iteration. + jr[k] = sgn*sqrtf((1.0f-c)/2.0f); // using 1/2 angle identity sin(a/2) = sqrt((1-cos(a))/2) + jr[k] *= -1.0f; // since our quat-to-matrix convention was for v*M instead of M*v + jr.w = sqrtf(1.0f - squared(jr[k])); + if(jr.w==1.0f) break; // reached limits of floating point precision + q = q*jr; + q.Normalize(); + } + return q; +} + +float3 Diagonal(const float3x3 &M) +{ + return float3(M[0][0],M[1][1],M[2][2]); +} diff --git a/examples/Experiments/ImplicitCloth/stan/vecmath.h b/examples/Experiments/ImplicitCloth/stan/vecmath.h new file mode 100644 index 000000000..a83ed8cfb --- /dev/null +++ b/examples/Experiments/ImplicitCloth/stan/vecmath.h @@ -0,0 +1,466 @@ +// +// +// Typical 3d vector math code. +// By S Melax 1998-2008 +// +// + +#ifndef SM_VEC_MATH_H +#define SM_VEC_MATH_H + +#include +#include +#include +#include + +#define M_PIf (3.1415926535897932384626433832795f) + +inline float DegToRad(float angle_degrees) { return angle_degrees * M_PIf / 180.0f; } // returns Radians. +inline float RadToDeg(float angle_radians) { return angle_radians * 180.0f / M_PIf; } // returns Degrees. + +#define OFFSET(Class,Member) (((char*) (&(((Class*)NULL)-> Member )))- ((char*)NULL)) + + + + +int argmin(const float a[],int n); +int argmax(const float a[],int n); +float squared(float a); +float clamp(float a,const float minval=0.0f, const float maxval=1.0f); +int clamp(int a,const int minval,const int maxval) ; +float Round(float a,float precision); +float Interpolate(const float &f0,const float &f1,float alpha) ; + +template +void Swap(T &a,T &b) +{ + T tmp = a; + a=b; + b=tmp; +} + + + +template +T Max(const T &a,const T &b) +{ + return (a>b)?a:b; +} + +template +T Min(const T &a,const T &b) +{ + return (a +class vec2 +{ + public: + T x,y; + inline vec2(){x=0;y=0;} + inline vec2(const T &_x, const T &_y){x=_x;y=_y;} + inline T& operator[](int i) {return ((T*)this)[i];} + inline const T& operator[](int i) const {return ((T*)this)[i];} +}; + +typedef vec2 int2; +typedef vec2 float2; + + +template inline int operator ==(const vec2 &a,const vec2 &b) {return (a.x==b.x && a.y==b.y);} +template inline vec2 operator-( const vec2& a, const vec2& b ){return vec2(a.x-b.x,a.y-b.y);} +template inline vec2 operator+( const vec2& a, const vec2& b ){return float2(a.x+b.x,a.y+b.y);} + +//--------- 3D --------- + + + +template +class vec3 +{ + public: + T x,y,z; + inline vec3(){x=0;y=0;z=0;}; + inline vec3(const T &_x,const T &_y,const T &_z){x=_x;y=_y;z=_z;}; + inline T& operator[](int i) {return ((T*)this)[i];} + inline const T& operator[](int i) const {return ((T*)this)[i];} +}; + + +typedef vec3 int3; +typedef vec3 short3; +typedef vec3 float3; + +// due to ambiguity there is no overloaded operators for v3*v3 use dot,cross,outerprod,cmul +template inline int operator==(const vec3 &a,const vec3 &b) {return (a.x==b.x && a.y==b.y && a.z==b.z);} +template inline int operator!=(const vec3 &a,const vec3 &b) {return !(a==b);} +template inline vec3 operator+(const vec3& a, const vec3& b ){return vec3(a.x+b.x, a.y+b.y, a.z+b.z);} +template inline vec3 operator-(const vec3& a, const vec3& b ){return vec3(a.x-b.x, a.y-b.y, a.z-b.z);} +template inline vec3 operator-(const vec3& v){return vec3(-v.x,-v.y,-v.z );} +template inline vec3 operator*(const vec3& v, const T &s ){ return vec3( v.x*s, v.y*s, v.z*s );} +template inline vec3 operator*(T s, const vec3& v ){return v*s;} +template inline vec3 operator/(const vec3& v, T s ){return vec3( v.x/s, v.y/s, v.z/s );} +template inline T dot (const vec3& a, const vec3& b){return a.x*b.x + a.y*b.y + a.z*b.z;} +template inline vec3 cmul (const vec3& a, const vec3& b){return vec3(a.x*b.x, a.y*b.y, a.z*b.z);} +template inline vec3 cross(const vec3& a, const vec3& b){return vec3(a.y*b.z-a.z*b.y, a.z*b.x-a.x*b.z, a.x*b.y-a.y*b.x);} +template inline T magnitude( const vec3& v ){return squareroot(dot(v,v));} +template inline vec3 normalize( const vec3& v ){return v/magnitude(v);} +template inline vec3& operator+=(vec3& a, const vec3& b){a.x+=b.x;a.y+=b.y;a.z+=b.z;return a;} +template inline vec3& operator-=(vec3& a, const vec3& b){a.x-=b.x;a.y-=b.y;a.z-=b.z;return a;} +template inline vec3& operator*=(vec3& v, T s){v.x*=s;v.y*=s;v.z*= s;return v;} +template inline vec3& operator/=(vec3& v, T s){v.x/=s;v.y/=s;v.z/=s;return v;} + + +float3 safenormalize(const float3 &v); +float3 vabs(const float3 &v); +float3 Interpolate(const float3 &v0,const float3 &v1,float alpha); +float3 Round(const float3& a,float precision); +template inline vec3VectorMin(const vec3 &a,const vec3 &b) {return vec3(Min(a.x,b.x),Min(a.y,b.y),Min(a.z,b.z));} +template inline vec3VectorMax(const vec3 &a,const vec3 &b) {return vec3(Max(a.x,b.x),Max(a.y,b.y),Max(a.z,b.z));} +int overlap(const float3 &bmina,const float3 &bmaxa,const float3 &bminb,const float3 &bmaxb); + +template +class mat3x3 +{ + public: + vec3 x,y,z; // the 3 rows of the Matrix + inline mat3x3(){} + inline mat3x3(const T &xx,const T &xy,const T &xz,const T &yx,const T &yy,const T &yz,const T &zx,const T &zy,const T &zz):x(xx,xy,xz),y(yx,yy,yz),z(zx,zy,zz){} + inline mat3x3(const vec3 &_x,const vec3 &_y,const vec3 &_z):x(_x),y(_y),z(_z){} + inline vec3& operator[](int i) {return (&x)[i];} + inline const vec3& operator[](int i) const {return (&x)[i];} + inline T& operator()(int r, int c) {return ((&x)[r])[c];} + inline const T& operator()(int r, int c) const {return ((&x)[r])[c];} +}; +typedef mat3x3 float3x3; + +float3x3 Transpose( const float3x3& m ); +template vec3 operator*( const vec3& v , const mat3x3& m ) +{ + return vec3((m.x.x*v.x + m.y.x*v.y + m.z.x*v.z), + (m.x.y*v.x + m.y.y*v.y + m.z.y*v.z), + (m.x.z*v.x + m.y.z*v.y + m.z.z*v.z)); +} + +float3 operator*( const float3x3& m , const float3& v ); +float3x3 operator*( const float3x3& m , const float& s ); +float3x3 operator*( const float3x3& ma, const float3x3& mb ); +float3x3 operator/( const float3x3& a, const float& s ) ; +float3x3 operator+( const float3x3& a, const float3x3& b ); +float3x3 operator-( const float3x3& a, const float3x3& b ); +float3x3 &operator+=( float3x3& a, const float3x3& b ); +float3x3 &operator-=( float3x3& a, const float3x3& b ); +float3x3 &operator*=( float3x3& a, const float& s ); +float Determinant(const float3x3& m ); +float3x3 Inverse(const float3x3& a); // its just 3x3 so we simply do that cofactor method +float3x3 outerprod(const float3& a,const float3& b); + +//-------- 4D Math -------- + +template +class vec4 +{ +public: + T x,y,z,w; + inline vec4(){x=0;y=0;z=0;w=0;}; + inline vec4(const T &_x, const T &_y, const T &_z, const T &_w){x=_x;y=_y;z=_z;w=_w;} + inline vec4(const vec3 &v,const T &_w){x=v.x;y=v.y;z=v.z;w=_w;} + //operator float *() { return &x;}; + T& operator[](int i) {return ((T*)this)[i];} + const T& operator[](int i) const {return ((T*)this)[i];} + inline const vec3& xyz() const { return *((vec3*)this);} + inline vec3& xyz() { return *((vec3*)this);} +}; + + +typedef vec4 float4; +typedef vec4 int4; +typedef vec4 byte4; + + +template inline int operator==(const vec4 &a,const vec4 &b) {return (a.x==b.x && a.y==b.y && a.z==b.z && a.w==b.w);} +template inline int operator!=(const vec4 &a,const vec4 &b) {return !(a==b);} +template inline vec4 operator+(const vec4& a, const vec4& b ){return vec4(a.x+b.x,a.y+b.y,a.z+b.z,a.w+b.w);} +template inline vec4 operator-(const vec4& a, const vec4& b ){return vec4(a.x-b.x,a.y-b.y,a.z-b.z,a.w-b.w);} +template inline vec4 operator-(const vec4& v){return vec4(-v.x,-v.y,-v.z,-v.w);} +template inline vec4 operator*(const vec4& v, const T &s ){ return vec4( v.x*s, v.y*s, v.z*s,v.w*s);} +template inline vec4 operator*(T s, const vec4& v ){return v*s;} +template inline vec4 operator/(const vec4& v, T s ){return vec4( v.x/s, v.y/s, v.z/s,v.w/s );} +template inline T dot(const vec4& a, const vec4& b ){return a.x*b.x + a.y*b.y + a.z*b.z+a.w*b.w;} +template inline vec4 cmul(const vec4 &a, const vec4 &b) {return vec4(a.x*b.x, a.y*b.y, a.z*b.z,a.w*b.w);} +template inline vec4& operator+=(vec4& a, const vec4& b ){a.x+=b.x;a.y+=b.y;a.z+=b.z;a.w+=b.w;return a;} +template inline vec4& operator-=(vec4& a, const vec4& b ){a.x-=b.x;a.y-=b.y;a.z-=b.z;a.w-=b.w;return a;} +template inline vec4& operator*=(vec4& v, T s){v.x*=s;v.y*=s;v.z*=s;v.w*=s;return v;} +template inline vec4& operator/=(vec4& v, T s){v.x/=s;v.y/=s;v.z/=s;v.w/=s;return v;} +template inline T magnitude( const vec4& v ){return squareroot(dot(v,v));} +template inline vec4 normalize( const vec4& v ){return v/magnitude(v);} + + + +struct D3DXMATRIX; + +template +class mat4x4 +{ + public: + vec4 x,y,z,w; // the 4 rows + inline mat4x4(){} + inline mat4x4(const vec4 &_x, const vec4 &_y, const vec4 &_z, const vec4 &_w):x(_x),y(_y),z(_z),w(_w){} + inline mat4x4(const T& m00, const T& m01, const T& m02, const T& m03, + const T& m10, const T& m11, const T& m12, const T& m13, + const T& m20, const T& m21, const T& m22, const T& m23, + const T& m30, const T& m31, const T& m32, const T& m33 ) + :x(m00,m01,m02,m03),y(m10,m11,m12,m13),z(m20,m21,m22,m23),w(m30,m31,m32,m33){} + inline vec4& operator[](int i) {assert(i>=0&&i<4);return (&x)[i];} + inline const vec4& operator[](int i) const {assert(i>=0&&i<4);return (&x)[i];} + inline T& operator()(int r, int c) {assert(r>=0&&r<4&&c>=0&&c<4);return ((&x)[r])[c];} + inline const T& operator()(int r, int c) const {assert(r>=0&&r<4&&c>=0&&c<4);return ((&x)[r])[c];} + inline operator T* () {return &x.x;} + inline operator const T* () const {return &x.x;} + operator struct D3DXMATRIX* () { return (struct D3DXMATRIX*) this;} + operator const struct D3DXMATRIX* () const { return (struct D3DXMATRIX*) this;} +}; + +typedef mat4x4 float4x4; + + +float4x4 operator*( const float4x4& a, const float4x4& b ); +float4 operator*( const float4& v, const float4x4& m ); +float4x4 Inverse(const float4x4 &m); +float4x4 MatrixRigidInverse(const float4x4 &m); +float4x4 MatrixTranspose(const float4x4 &m); +float4x4 MatrixPerspectiveFov(float fovy, float Aspect, float zn, float zf ); +float4x4 MatrixTranslation(const float3 &t); +float4x4 MatrixRotationZ(const float angle_radians); +float4x4 MatrixLookAt(const float3& eye, const float3& at, const float3& up); +int operator==( const float4x4 &a, const float4x4 &b ); + + +//-------- Quaternion ------------ + +template +class quaternion : public vec4 +{ + public: + inline quaternion() { this->x = this->y = this->z = 0.0f; this->w = 1.0f; } + inline quaternion(const T &_x, const T &_y, const T &_z, const T &_w){this->x=_x;this->y=_y;this->z=_z;this->w=_w;} + inline explicit quaternion(const vec4 &v):vec4(v){} + T angle() const { return acosf(this->w)*2.0f; } + vec3 axis() const { vec3 a(this->x,this->y,this->z); if(fabsf(angle())<0.0000001f) return vec3(1,0,0); return a*(1/sinf(angle()/2.0f)); } + inline vec3 xdir() const { return vec3( 1-2*(this->y*this->y+this->z*this->z), 2*(this->x*this->y+this->w*this->z), + 2*(this->x*this->z-this->w*this->y) ); } + inline vec3 ydir() const { return vec3( 2*(this->x*this->y-this->w*this->z),1-2*(this->x*this->x+this->z*this->z), 2*(this->y*this->z+this->w*this->x) ); } + inline vec3 zdir() const { return vec3( 2*(this->x*this->z+this->w*this->y), + 2*(this->y*this->z-this->w*this->x),1- + 2*(this->x*this->x+this->y*this->y) ); } + inline mat3x3 getmatrix() const { return mat3x3( xdir(), ydir(), zdir() ); } + //operator float3x3() { return getmatrix(); } + void Normalize(); +}; + +template +inline quaternion quatfrommat(const mat3x3 &m) +{ + T magw = m[0 ][ 0] + m[1 ][ 1] + m[2 ][ 2]; + T magxy; + T magzw; + vec3 pre; + vec3 prexy; + vec3 prezw; + quaternion postxy; + quaternion postzw; + quaternion post; + int wvsz = (magw > m[2][2] ) ; + magzw = (wvsz) ? magw : m[2][2]; + prezw = (wvsz) ? vec3(1.0f,1.0f,1.0f) : vec3(-1.0f,-1.0f,1.0f) ; + postzw = (wvsz) ? quaternion(0.0f,0.0f,0.0f,1.0f): quaternion(0.0f,0.0f,1.0f,0.0f); + int xvsy = (m[0][0]>m[1][1]); + magxy = (xvsy) ? m[0][0] : m[1][1]; + prexy = (xvsy) ? vec3(1.0f,-1.0f,-1.0f) : vec3(-1.0f,1.0f,-1.0f) ; + postxy = (xvsy) ? quaternion(1.0f,0.0f,0.0f,0.0f): quaternion(0.0f,1.0f,0.0f,0.0f); + int zwvsxy = (magzw > magxy); + pre = (zwvsxy) ? prezw : prexy ; + post = (zwvsxy) ? postzw : postxy; + + T t = pre.x * m[0 ][ 0] + pre.y * m[1 ][ 1] + pre.z * m[2 ][ 2] + 1.0f; + T s = 1/sqrt(t) * 0.5f; + quaternion qp; + qp.x = ( pre.y * m[1][2] - pre.z * m[2][1] ) * s; + qp.y = ( pre.z * m[2][0] - pre.x * m[0][2] ) * s; + qp.z = ( pre.x * m[0][1] - pre.y * m[1][0] ) * s; + qp.w = t * s ; + return qp * post ; +} + +typedef quaternion Quaternion; + +inline Quaternion QuatFromAxisAngle(const float3 &_v, float angle_radians ) +{ + float3 v = normalize(_v)*sinf(angle_radians/2.0f); + return Quaternion(v.x,v.y,v.z,cosf(angle_radians/2.0f)); +} + +template inline quaternion Conjugate(const quaternion &q){return quaternion(-q.x,-q.y,-q.z,q.w);} +template inline quaternion Inverse(const quaternion &q){return Conjugate(q);} +template inline quaternion normalize( const quaternion & a ){return quaternion (normalize((vec4&)a));} +template inline quaternion& operator*=(quaternion& a, T s ){return (quaternion&)((vec4&)a *=s);} +template inline quaternion operator*( const quaternion& a, float s ){return quaternion((vec4&)a*s);} +template inline quaternion operator+( const quaternion& a, const quaternion& b){return quaternion((vec4&)a+(vec4&)b);} +template inline quaternion operator-( const quaternion& a, const quaternion& b){return quaternion((vec4&)a-(vec4&)b);} +template inline quaternion operator-( const quaternion& b){return quaternion(-(vec4&)b);} +template inline quaternion operator*( const quaternion& a, const quaternion& b) +{ + return quaternion( + a.w*b.x + a.x*b.w + a.y*b.z - a.z*b.y, //x + a.w*b.y - a.x*b.z + a.y*b.w + a.z*b.x, //y + a.w*b.z + a.x*b.y - a.y*b.x + a.z*b.w, //z + a.w*b.w - a.x*b.x - a.y*b.y - a.z*b.z ); //w +} + + +float3 rotate( const Quaternion& q, const float3& v ); +//float3 operator*( const Quaternion& q, const float3& v ); +//float3 operator*( const float3& v, const Quaternion& q ); + +Quaternion slerp(const Quaternion &a, const Quaternion& b, float t ); +Quaternion Interpolate(const Quaternion &q0,const Quaternion &q1,float t); +Quaternion RotationArc(float3 v0, float3 v1 ); // returns quat q where q*v0*q^-1=v1 +float4x4 MatrixFromQuatVec(const Quaternion &q, const float3 &v); + +inline Quaternion QuatFromMat(const float3 &t, const float3 &b, const float3 &n) +{ + return normalize(quatfrommat(float3x3(t,b,n))); +} + + +//---------------- Pose ------------------ + +class Pose +{ +public: + float3 position; + Quaternion orientation; + Pose(){} + Pose(const float3 &p,const Quaternion &q):position(p),orientation(q){} + Pose &pose(){return *this;} + const Pose &pose() const {return *this;} +}; + +inline float3 operator*(const Pose &a,const float3 &v) +{ + return a.position + rotate(a.orientation,v); +} + +inline Pose operator*(const Pose &a,const Pose &b) +{ + return Pose(a.position + rotate(a.orientation,b.position),a.orientation*b.orientation); +} + +inline Pose Inverse(const Pose &a) +{ + Quaternion q = Inverse(a.orientation); + return Pose(rotate(q,-a.position),q); +} + +inline Pose slerp(const Pose &p0,const Pose &p1,float t) +{ + return Pose(p0.position * (1.0f-t) + p1.position * t,slerp(p0.orientation,p1.orientation,t)); +} + +inline float4x4 MatrixFromPose(const Pose &pose) +{ + return MatrixFromQuatVec(pose.orientation,pose.position); +} + +//------ Euler Angle ----- + +Quaternion YawPitchRoll( float yaw, float pitch, float roll ); +float Yaw( const Quaternion& q ); +float Pitch( const Quaternion& q ); +float Roll( const Quaternion &q ); +float Yaw( const float3& v ); +float Pitch( const float3& v ); + +//------- Plane ---------- +class Plane : public float4 +{ + public: + float3& normal(){ return xyz(); } + const float3& normal() const { return xyz(); } + float& dist(){return w;} // distance below origin - the D from plane equasion Ax+By+Cz+D=0 + const float& dist() const{return w;} // distance below origin - the D from plane equasion Ax+By+Cz+D=0 + Plane(const float3 &n,float d):float4(n,d){} + Plane(){dist()=0;} + explicit Plane(const float4 &v):float4(v){} +}; + +Plane Transform(const Plane &p, const float3 &translation, const Quaternion &rotation); + +inline Plane PlaneFlip(const Plane &p){return Plane(-p.normal(),-p.dist());} +inline int operator==( const Plane &a, const Plane &b ) { return (a.normal()==b.normal() && a.dist()==b.dist()); } +inline int coplanar( const Plane &a, const Plane &b ) { return (a==b || a==PlaneFlip(b)); } + +float3 PlaneLineIntersection(const Plane &plane, const float3 &p0, const float3 &p1); +float3 PlaneProject(const Plane &plane, const float3 &point); +float3 PlanesIntersection(const Plane &p0,const Plane &p1, const Plane &p2); +float3 PlanesIntersection(const Plane *planes,int planes_count,const float3 &seed=float3(0,0,0)); + +int Clip(const Plane &p,const float3 *verts_in,int count,float* verts_out); // verts_out must be preallocated with sufficient size >= count+1 or more if concave +int ClipPolyPoly(const float3 &normal,const float3 *clipper,int clipper_count,const float3 *verts_in, int in_count,float3 *scratch); //scratch must be preallocated + + +//--------- Utility Functions ------ + +float3 PlaneLineIntersection(const float3 &normal,const float dist, const float3 &p0, const float3 &p1); +float3 LineProject(const float3 &p0, const float3 &p1, const float3 &a); // projects a onto infinite line p0p1 +float LineProjectTime(const float3 &p0, const float3 &p1, const float3 &a); +int BoxInside(const float3 &p,const float3 &bmin, const float3 &bmax) ; +int BoxIntersect(const float3 &v0, const float3 &v1, const float3 &bmin, const float3 &bmax, float3 *impact); +float DistanceBetweenLines(const float3 &ustart, const float3 &udir, const float3 &vstart, const float3 &vdir, float3 *upoint=NULL, float3 *vpoint=NULL); +float3 TriNormal(const float3 &v0, const float3 &v1, const float3 &v2); +float3 NormalOf(const float3 *vert, const int n); +Quaternion VirtualTrackBall(const float3 &cop, const float3 &cor, const float3 &dir0, const float3 &dir1); +int Clip(const float3 &plane_normal,float plane_dist,const float3 *verts_in,int count,float* verts_out); // verts_out must be preallocated with sufficient size >= count+1 or more if concave +int ClipPolyPoly(const float3 &normal,const float3 *clipper,int clipper_count,const float3 *verts_in, int in_count,float3 *scratch); //scratch must be preallocated +float3 Diagonal(const float3x3 &M); +Quaternion Diagonalizer(const float3x3 &A); +float3 Orth(const float3& v); +int SolveQuadratic(float a,float b,float c,float *ta,float *tb); // if true returns roots ta,tb where ta<=tb +int HitCheckPoly(const float3 *vert,const int n,const float3 &v0, const float3 &v1, float3 *impact=NULL, float3 *normal=NULL); +int HitCheckRaySphere(const float3& sphereposition,float radius, const float3& _v0, const float3& _v1, float3 *impact,float3 *normal); +int HitCheckRayCylinder(const float3 &p0,const float3 &p1,float radius,const float3& _v0,const float3& _v1, float3 *impact,float3 *normal); +int HitCheckSweptSphereTri(const float3 &p0,const float3 &p1,const float3 &p2,float radius, const float3& v0,const float3& _v1, float3 *impact,float3 *normal); +void BoxLimits(const float3 *verts,int verts_count, float3 &bmin_out,float3 &bmax_out); +void BoxLimits(const float4 *verts,int verts_count, float3 &bmin_out,float3 &bmax_out); + + +template +inline int maxdir(const T *p,int count,const T &dir) +{ + assert(count); + int m=0; + for(int i=1;idot(p[m],dir)) m=i; + } + return m; +} + +float3 CenterOfMass(const float3 *vertices, const int3 *tris, const int count) ; +float3x3 Inertia(const float3 *vertices, const int3 *tris, const int count, const float3& com=float3(0,0,0)) ; +float Volume(const float3 *vertices, const int3 *tris, const int count) ; +int calchull(float3 *verts,int verts_count, int3 *&tris_out, int &tris_count,int vlimit); // computes convex hull see hull.cpp + +#endif // VEC_MATH_H