Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

add Stan Melax' ImplicitCloth demo

Browse Source
This commit is contained in:
Erwin Coumans
2015-04-30 13:36:39 -07:00
parent 49a71a9296
commit 9d3f8803b8
15 changed files with 2949 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
examples/ExampleBrowser/CMakeLists.txt
View File

@@ -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

9
examples/ExampleBrowser/ExampleEntries.cpp
View File

@@ -25,6 +25,8 @@
#include "../SoftDemo/SoftDemo.h"
#include "../Constraints/ConstraintDemo.h"
#include "../Vehicles/Hinge2Vehicle.h"
#include "../Experiments/ImplicitCloth/ImplicitClothExample.h"
struct ExampleEntry
{
@@ -143,11 +145,16 @@ 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),
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"),
ExampleEntry(1,"Instanced Rendering", "Simple example of fast instanced rendering, only active when using OpenGL3+.",RenderInstancingCreateFunc),

10
examples/ExampleBrowser/OpenGLExampleBrowser.cpp
View File

@@ -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?

2
examples/ExampleBrowser/premake4.lua
View File

@@ -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",

119
examples/Experiments/ImplicitCloth/ImplicitClothExample.cpp Normal file
View File

@@ -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<unsigned int> indices;
for (int i=0;i<m_cloth->springs.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);
}

8
examples/Experiments/ImplicitCloth/ImplicitClothExample.h Normal file
View File

@@ -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

96
examples/Experiments/ImplicitCloth/stan/Cloth.cpp Normal file
View File

@@ -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<Cloth*> 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;i<h;i++)
for(j=0;j<w;j++)
{
cloth->X[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;i<h;i++)
for(j=0;j<w;j++)
{
if(i<h-1) cloth->CreateSpring(SPRING_STRUCT,i*w+j,(i+1)*w+j); // structural
if(j<w-1) cloth->CreateSpring(SPRING_STRUCT,i*w+j,i*w+(j+1)); // structural
if(j<w-1&&i<h-1) cloth->CreateSpring(SPRING_SHEAR ,i*w+j,(i+1)*w+(j+1)); // shear
if(j>0 &&i<h-1) cloth->CreateSpring(SPRING_SHEAR ,i*w+j,(i+1)*w+(j-1)); // shear
if(i<h-2) cloth->CreateSpring(SPRING_BEND ,i*w+j,(i+2)*w+j); // benders
if(j<w-2) cloth->CreateSpring(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;i<cloth->springs.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;i<cloths.count;i++)
{
Cloth *cloth=cloths[i];
// cloth->Simulate((cloth->cloth_step<0)?DeltaT:cloth->cloth_step);
//if(cloth_showsprings)
// ClothDrawSprings(cloth); // debug visualization
}
}

18
examples/Experiments/ImplicitCloth/stan/Cloth.h Normal file
View File

@@ -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

198
examples/Experiments/ImplicitCloth/stan/SpringNetwork.cpp Normal file
View File

@@ -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 <stdio.h>
#include <float.h>
#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.count;i++)
{
PreSolveSpring(springs[i]); // will add to F,dFdX,dFdV
}
}
void SpringNetwork::Simulate(float dt)
{
// Get ready for conjugate gradient iterative solver step.
// Initialize operands.
if(!awake) return;
CalcForces();
int n=X.count; // all our big vectors are of this size
float3N dFdXmV(n); // temp to store result of matrix multiply
float3N B(n);
dV.Zero();
A.Identity(); // build up the big matrix we feed to solver
A -= dFdV * dt + dFdX * (dt*dt) ;
dFdXmV = dFdX * V;
B = F * dt + dFdXmV * (dt*dt);
ConjGradientFiltered(dV,A,B,S);
V = V + dV;
// V.element[0] = float3(0,0,0);
// V.element[numX-1] = float3(0,0,0);
X = X + V*dt;
UpdateLimits();
awake = (dot(V,V)<cloth_sleepthreshold)?awake-1:awake=cloth_sleepcount;
}

63
examples/Experiments/ImplicitCloth/stan/SpringNetwork.h Normal file
View File

@@ -0,0 +1,63 @@
#ifndef STAN_SPRING_NETWORK_H
#define STAN_SPRING_NETWORK_H
#include "vec3n.h"
#define SPRING_STRUCT (0)
#define SPRING_SHEAR (1)
#define SPRING_BEND (2)
class SpringNetwork
{
public:
class Spring
{
public:
int type; // index into coefficients spring_k[]
float restlen;
int a,b; // spring endpoints vector indices
int iab,iba; // indices into off-diagonal blocks of sparse matrix
Spring(){}
Spring(int _type,int _a,int _b,float _restlen):type(_type),a(_a),b(_b),restlen(_restlen){iab=iba=-1;}
};
Array<Spring> 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

284
examples/Experiments/ImplicitCloth/stan/array.h Normal file
View File

@@ -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<myarray.count;i++)
// myarray[i] = somefunction(i);
//
// When the array runs out of room, it
// reallocates memory and doubles the size of its
// storage buffer. The reason for *doubleing* the amount of
// memory is so the order of any algorithm using this class
// is the same as it would be had you used a regular C array.
// The penalty for reallocating and copying
// For example consider adding n elements to a list.
// Lets sum the number of times elements are "copied".
// The worst case occurs when n=2^k+1 where k is integer.
// In this case we do a big reallocation when we add the last element.
// n elements are copied once, n/2 elements are copied twice,
// n/4 elements are copied 3 times, and so on ...
// total == n* (1+1/2 + 1/4 + 1/8 + ...) == n * 2
// So we do n*2 copies. Therefore adding n
// elements to an Array is still O(n).
// The memory usage is also of the same order as if a C array was used.
// An Array uses less than double the minimum needed space. Again, we
// see that we are within a small constant multiple.
//
// Why no "realloc" to avoid the copy when reallocating memory?
// You have a choice to either use malloc/free and friends
// or to use new/delete. Its bad mojo to mix these. new/delete was
// chosen to be C++ish and have the array elements constructors/destructors
// invoked as expected.
//
//
#ifndef SM_ARRAY_H
#define SM_ARRAY_H
#include <assert.h>
#include <stdio.h>
template <class Type> class Array {
public:
Array(int s=0);
Array(Array<Type> &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<count); return element[i]; }
Type &operator[](int i) { assert(i>=0 && i<count); return element[i]; }
Type &Pop() { assert(count); count--; return element[count]; }
Array<Type> &copy(const Array<Type> &array);
Array<Type> &operator=(Array<Type> &array);
};
template <class Type> Array<Type>::Array(int s)
{
if(s==-1) return;
count=0;
array_size = 0;
element = NULL;
if(s)
{
allocate(s);
}
}
template <class Type> Array<Type>::Array(Array<Type> &array)
{
count=0;
array_size = 0;
element = NULL;
*this = array;
}
template <class Type> Array<Type> &Array<Type>::copy(const Array<Type> &array)
{
assert(array.array_size>=0);
count=0;
for(int i=0;i<array.count;i++)
{
Add(array[i]);
}
return *this;
}
template <class Type> Array<Type> &Array<Type>::operator=( Array<Type> &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.count;i++)
{
Add(array[i]);
}
return *this;
}
template <class Type> Array<Type>::~Array()
{
if (element != NULL && array_size!=0)
{
delete[] element;
}
count=0;array_size=0;element=NULL;
}
template <class Type> void Array<Type>::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<count;i++)
{
element[i]=old[i];
}
if(old) delete[] old;
}
template <class Type> void Array<Type>::SetSize(int s)
{
if(s==0)
{
if(element)
{
delete[] element;
element = NULL;
}
array_size = s;
}
else
{
allocate(s);
}
count=s;
}
template <class Type> void Array<Type>::Pack()
{
allocate(count);
}
template <class Type> Type& Array<Type>::Add(Type t)
{
assert(count<=array_size);
if(count==array_size)
{
allocate((array_size)?array_size *2:16);
}
//int i;
//for(i=0;i<count;i++) {
// dissallow duplicates
// assert(element[i] != t);
//}
element[count++] = t;
return element[count-1];
}
template <class Type> int Array<Type>::Contains(Type t)
{
int i;
int found=0;
for(i=0;i<count;i++)
{
if(element[i] == t) found++;
}
return found;
}
template <class Type> void Array<Type>::AddUnique(Type t)
{
if(!Contains(t)) Add(t);
}
template <class Type> void Array<Type>::DelIndex(int i)
{
assert(i<count);
count--;
while(i<count)
{
element[i] = element[i+1];
i++;
}
}
template <class Type> Type& Array<Type>::DelIndexWithLast(int i)
{
assert(i<count);
count--;
if(i<count)
{
Type r=element[i];
element[i] = element[count];
element[count]=r;
}
return element[count];
}
template <class Type> void Array<Type>::Remove(Type t)
{
int i;
for(i=0;i<count;i++)
{
if(element[i] == t)
{
break;
}
}
assert(i<count); // assert object t is in the array.
DelIndex(i);
for(i=0;i<count;i++)
{
assert(element[i] != t);
}
}
template <class Type> void Array<Type>::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 <class Type> int Array<Type>::IndexOf(Type t)
{
int i;
for(i=0;i<count;i++)
{
if(element[i] == t)
{
return i;
}
}
assert(0);
return -1;
}
#endif

152
examples/Experiments/ImplicitCloth/stan/vec3n.cpp Normal file
View File

@@ -0,0 +1,152 @@
//
// Big Vector and Sparse Matrix Classes
//
#include <float.h>
#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_loopcount<conjgrad_looplimit; // true means we reached desired accuracy in given time - ie stable
}
int ConjGradientMod(float3N &X, float3Nx3N &A, float3N &B,int3 hack)
{
// obsolete!!!
// 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
r[hack[0]] = r[hack[1]] = r[hack[2]] = float3(0,0,0);
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;
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_loopcount<conjgrad_looplimit; // true means we reached desired accuracy in given time - ie stable
}
static inline void filter(float3N &V,const float3Nx3N &S)
{
for(int i=0;i<S.blocks.count;i++)
{
V[S.blocks[i].r] = V[S.blocks[i].r]*S.blocks[i].m;
}
}
int ConjGradientFiltered(float3N &X, const float3Nx3N &A, const float3N &B,const float3Nx3N &S)
{
// 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
filter(r,S);
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;
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<conjgrad_looplimit; // true means we reached desired accuracy in given time - ie stable
}
// test big vector math library:
static void testfloat3N()
{
float3N a(2),b(2),c(2);
a[0] = float3(1,2,3);
a[1] = float3(4,5,6);
b[0] = float3(10,20,30);
b[1] = float3(40,50,60);
// c = a+b+b * 10.0f;
// float d = dot(a+b,-b);
int k;
k=0;
}
class dotest{public:dotest(){testfloat3N();}}do_test_at_program_startup;

340
examples/Experiments/ImplicitCloth/stan/vec3n.h Normal file
View File

@@ -0,0 +1,340 @@
//
// Big Vector and Sparse Matrix Classes
//
// (c) S Melax 2006
//
// The focus is on 3D applications, so
// the big vector is an array of float3s
// and the matrix class uses 3x3 blocks.
//
// This file includes both:
// - basic non-optimized version
// - an expression optimized version
//
// Optimized Expressions
//
// We want to write sweet looking code such as V=As+Bt with big vectors.
// However, we dont want the extra overheads with allocating memory for temps and excessing copying.
// Instead of a full Template Metaprogramming approach, we explicitly write
// classes to specifically handle all the expressions we are likely to use.
// Most applicable lines of code will be of the same handful of basic forms,
// but with different parameters for the operands.
// In the future, if we ever need a longer expression with more operands,
// then we will just add whatever additional building blocks that are necessary - not a big deal.
// This approach is much simpler to develop, debug and optimize (restrict keyword, simd etc)
// than template metaprogramming is. We do not rely on the implementation
// of a particular compiler to be able to expand extensive nested inline codes.
// Additionally, we reliably get our optimizations even within a debug build.
// Therefore we believe that our Optimized Expressions
// are a good compromise that give us the best of both worlds.
// The code within those important algorithms, which use this library,
// can now remain clean and readable yet still execute quickly.
//
#ifndef SM_VEC3N_H
#define SM_VEC3N_H
#include "vecmath.h"
#include "array.h"
//#include <malloc.h>
//template <class T> void * vec4<T>::operator new[](size_t n){ return _mm_malloc(n,64); }
//template <class T> void vec4<T>::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<Block> 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<n;i++) blocks.Add(Block((short)i,(short)i));}
template<class E> float3Nx3N &operator= (const E& expression) {expression.evalequals(*this);return *this;}
template<class E> float3Nx3N &operator+=(const E& expression) {expression.evalpluseq(*this);return *this;}
template<class E> float3Nx3N &operator-=(const E& expression) {expression.evalmnuseq(*this);return *this;}
};
class float3N: public Array<float3>
{
public:
float3N(int _count=0)
{
SetSize(_count);
}
void Zero();
void Init(const float3 &v); // sets each subvector to v
template<class E> float3N &operator= (const E& expression) {expression.evalequals(*this);return *this;}
template<class E> float3N &operator+=(const E& expression) {expression.evalpluseq(*this);return *this;}
template<class E> 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<HalfConstraint> &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<r.count;i++) r[i]=float3(0,0,0);
for(i=0;i<m.blocks.count;i++)
{
r[m.blocks[i].r] += m.blocks[i].m * v[m.blocks[i].c];
}
return r;
}
inline float dot(const float3N &a,const float3N &b)
{
float d=0;
for(int i=0;i<a.count;i++)
{
d+= dot(a[i],b[i]);
}
return d;
}
inline void float3Nx3N::Zero()
{
for(int i=0;i<blocks.count;i++)
{
blocks[i].m = float3x3(0,0,0,0,0,0,0,0,0);
}
}
inline void float3Nx3N::InitDiagonal(float d)
{
for(int i=0;i<blocks.count;i++)
{
blocks[i].m = (blocks[i].c==blocks[i].r) ? float3x3(d,0,0,0,d,0,0,0,d) : float3x3(0,0,0,0,0,0,0,0,0);
}
}
inline void float3N::Zero()
{
for(int i=0;i<count;i++)
{
element[i] = float3(0,0,0);
}
}
inline void float3N::Init(const float3 &v)
{
for(int i=0;i<count;i++)
{
element[i] = v;
}
}
#ifdef WE_LIKE_SLOW_CODE
// Unoptimized Slow Basic Version of big vector operators.
// Uses typical implmentation for operators +/-*=
// These operators cause lots of unnecessary construction, memory allocation, and copying.
inline float3N operator +(const float3N &a,const float3N &b)
{
float3N r(a.count);
for(int i=0;i<a.count;i++) r[i]=a[i]+b[i];
return r;
}
inline float3N operator *(const float3N &a,const float &s)
{
float3N r(a.count);
for(int i=0;i<a.count;i++) r[i]=a[i]*s;
return r;
}
inline float3N operator /(const float3N &a,const float &s)
{
float3N r(a.count);
return Mul(r,a, 1.0f/s );
}
inline float3N operator -(const float3N &a,const float3N &b)
{
float3N r(a.count);
for(int i=0;i<a.count;i++) r[i]=a[i]-b[i];
return r;
}
inline float3N operator -(const float3N &a)
{
float3N r(a.count);
for(int i=0;i<a.count;i++) r[i]=-a[i];
return r;
}
inline float3N operator *(const float3Nx3N &m,const float3N &v)
{
float3N r(v.count);
return Mul(r,m,v);
}
inline float3N &operator-=(float3N &A, const float3N &B)
{
assert(A.count==B.count);
for(int i=0;i<A.count;i++) A[i] -= B[i];
return A;
}
inline float3N &operator+=(float3N &A, const float3N &B)
{
assert(A.count==B.count);
for(int i=0;i<A.count;i++) A[i] += B[i];
return A;
}
#else
// Optimized Expressions
class exVneg
{
public:
const float3N &v;
exVneg(const float3N &_v): v(_v){}
void evalequals(float3N &r)const { for(int i=0;i<v.count;i++) r[i] =-v[i];}
void evalpluseq(float3N &r)const { for(int i=0;i<v.count;i++) r[i]+=-v[i];}
void evalmnuseq(float3N &r)const { for(int i=0;i<v.count;i++) r[i]-=-v[i];}
};
class exVaddV
{
public:
const float3N &a;
const float3N &b;
exVaddV(const float3N &_a,const float3N &_b): a(_a),b(_b){}
void evalequals(float3N &r)const { for(int i=0;i<a.count;i++) r[i] =a[i]+b[i];}
void evalpluseq(float3N &r)const { for(int i=0;i<a.count;i++) r[i]+=a[i]+b[i];}
void evalmnuseq(float3N &r)const { for(int i=0;i<a.count;i++) r[i]-=a[i]+b[i];}
};
class exVsubV
{
public:
const float3N &a;
const float3N &b;
exVsubV(const float3N &_a,const float3N &_b): a(_a),b(_b){}
void evalequals(float3N &r)const { for(int i=0;i<a.count;i++) r[i] =a[i]-b[i];}
void evalpluseq(float3N &r)const { for(int i=0;i<a.count;i++) r[i]+=a[i]-b[i];}
void evalmnuseq(float3N &r)const { for(int i=0;i<a.count;i++) r[i]-=a[i]-b[i];}
};
class exVs
{
public:
const float3N &v;
const float s;
exVs(const float3N &_v,const float &_s): v(_v),s(_s){}
void evalequals(float3N &r)const { for(int i=0;i<v.count;i++) r[i] =v[i]*s;}
void evalpluseq(float3N &r)const { for(int i=0;i<v.count;i++) r[i]+=v[i]*s;}
void evalmnuseq(float3N &r)const { for(int i=0;i<v.count;i++) r[i]-=v[i]*s;}
};
class exAsaddB
{
public:
const float3N &a;
const float3N &b;
const float s;
exAsaddB(const float3N &_a,const float &_s,const float3N &_b): a(_a),s(_s),b(_b){}
void evalequals(float3N &r)const { for(int i=0;i<a.count;i++) r[i] =a[i]*s+b[i];}
void evalpluseq(float3N &r)const { for(int i=0;i<a.count;i++) r[i]+=a[i]*s+b[i];}
void evalmnuseq(float3N &r)const { for(int i=0;i<a.count;i++) r[i]-=a[i]*s+b[i];}
};
class exAsaddBt
{
public:
const float3N &a;
const float3N &b;
const float s;
const float t;
exAsaddBt(const float3N &_a,const float &_s,const float3N &_b,const float &_t): a(_a),s(_s),b(_b),t(_t){}
void evalequals(float3N &r)const { for(int i=0;i<a.count;i++) r[i] =a[i]*s+b[i]*t;}
void evalpluseq(float3N &r)const { for(int i=0;i<a.count;i++) r[i]+=a[i]*s+b[i]*t;}
void evalmnuseq(float3N &r)const { for(int i=0;i<a.count;i++) r[i]-=a[i]*s+b[i]*t;}
};
class exMv
{
public:
const float3Nx3N &m;
const float3N &v;
exMv(const float3Nx3N &_m,const float3N &_v): m(_m),v(_v){}
void evalequals(float3N &r)const { Mul(r,m,v);}
};
class exMs
{
public:
const float3Nx3N &m;
const float s;
exMs(const float3Nx3N &_m,const float &_s): m(_m),s(_s){}
void evalequals(float3Nx3N &r)const { for(int i=0;i<r.blocks.count;i++) r.blocks[i].m = m.blocks[i].m*s;}
void evalpluseq(float3Nx3N &r)const { for(int i=0;i<r.blocks.count;i++) r.blocks[i].m += m.blocks[i].m*s;}
void evalmnuseq(float3Nx3N &r)const { for(int i=0;i<r.blocks.count;i++) r.blocks[i].m -= m.blocks[i].m*s;}
};
class exMAsMBt
{
public:
const float3Nx3N &a;
const float s;
const float3Nx3N &b;
const float t;
exMAsMBt(const float3Nx3N &_a,const float &_s,const float3Nx3N &_b,const float &_t): a(_a),s(_s),b(_b),t(_t){}
void evalequals(float3Nx3N &r)const { for(int i=0;i<r.blocks.count;i++) r.blocks[i].m = a.blocks[i].m*s + b.blocks[i].m*t;}
void evalpluseq(float3Nx3N &r)const { for(int i=0;i<r.blocks.count;i++) r.blocks[i].m += a.blocks[i].m*s + b.blocks[i].m*t;}
void evalmnuseq(float3Nx3N &r)const { for(int i=0;i<r.blocks.count;i++) r.blocks[i].m -= a.blocks[i].m*s + b.blocks[i].m*t;}
};
inline exVaddV operator +(const float3N &a,const float3N &b) {return exVaddV(a,b);}
inline exVsubV operator +(const exVneg &E,const float3N &b) {return exVsubV(b,E.v);}
inline exVsubV operator -(const float3N &a,const float3N &b) {return exVsubV(a,b);}
inline exVs operator *(const float3N &V,const float &s) {return exVs(V,s); }
inline exVs operator *(const exVs &E,const float &s) {return exVs(E.v,E.s*s); }
inline exAsaddB operator +(const exVs &E,const float3N &b) {return exAsaddB(E.v, E.s,b);}
inline exAsaddB operator +(const float3N &b,const exVs &E) {return exAsaddB(E.v, E.s,b);}
inline exAsaddB operator -(const float3N &b,const exVs &E) {return exAsaddB(E.v,-E.s,b);}
inline exAsaddBt operator +(const exVs &Ea,const exVs &Eb) {return exAsaddBt(Ea.v,Ea.s,Eb.v, Eb.s);}
inline exAsaddBt operator -(const exVs &Ea,const exVs &Eb) {return exAsaddBt(Ea.v,Ea.s,Eb.v,-Eb.s);}
inline exMv operator *(const float3Nx3N &m,const float3N &v) {return exMv(m,v); }
inline exMs operator *(const exMs &E,const float &s) {return exMs(E.m,E.s*s); }
inline exMs operator *(const float3Nx3N &m,const float &s) {return exMs(m,s); }
inline exMAsMBt operator +(const exMs &Ea,const exMs &Eb) {return exMAsMBt(Ea.m,Ea.s, Eb.m,Eb.s);}
inline exMAsMBt operator -(const exMs &Ea,const exMs &Eb) {return exMAsMBt(Ea.m,Ea.s, Eb.m,-Eb.s);}
#endif
#endif

1183
examples/Experiments/ImplicitCloth/stan/vecmath.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

466
examples/Experiments/ImplicitCloth/stan/vecmath.h Normal file
View File

@@ -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 <stdio.h>
#include <math.h>
#include <assert.h>
#include <xmmintrin.h>
#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 <class T>
void Swap(T &a,T &b)
{
T tmp = a;
a=b;
b=tmp;
}
template <class T>
T Max(const T &a,const T &b)
{
return (a>b)?a:b;
}
template <class T>
T Min(const T &a,const T &b)
{
return (a<b)?a:b;
}
//for template normalize functions:
inline float squareroot(float a){return sqrtf(a);}
inline double squareroot(double a){return sqrt(a); }
//----------------------------------
//-------- 2D --------
template<class T>
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<int> int2;
typedef vec2<float> float2;
template<class T> inline int operator ==(const vec2<T> &a,const vec2<T> &b) {return (a.x==b.x && a.y==b.y);}
template<class T> inline vec2<T> operator-( const vec2<T>& a, const vec2<T>& b ){return vec2<T>(a.x-b.x,a.y-b.y);}
template<class T> inline vec2<T> operator+( const vec2<T>& a, const vec2<T>& b ){return float2(a.x+b.x,a.y+b.y);}
//--------- 3D ---------
template<class T>
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<int> int3;
typedef vec3<short> short3;
typedef vec3<float> float3;
// due to ambiguity there is no overloaded operators for v3*v3 use dot,cross,outerprod,cmul
template<class T> inline int operator==(const vec3<T> &a,const vec3<T> &b) {return (a.x==b.x && a.y==b.y && a.z==b.z);}
template<class T> inline int operator!=(const vec3<T> &a,const vec3<T> &b) {return !(a==b);}
template<class T> inline vec3<T> operator+(const vec3<T>& a, const vec3<T>& b ){return vec3<T>(a.x+b.x, a.y+b.y, a.z+b.z);}
template<class T> inline vec3<T> operator-(const vec3<T>& a, const vec3<T>& b ){return vec3<T>(a.x-b.x, a.y-b.y, a.z-b.z);}
template<class T> inline vec3<T> operator-(const vec3<T>& v){return vec3<T>(-v.x,-v.y,-v.z );}
template<class T> inline vec3<T> operator*(const vec3<T>& v, const T &s ){ return vec3<T>( v.x*s, v.y*s, v.z*s );}
template<class T> inline vec3<T> operator*(T s, const vec3<T>& v ){return v*s;}
template<class T> inline vec3<T> operator/(const vec3<T>& v, T s ){return vec3<T>( v.x/s, v.y/s, v.z/s );}
template<class T> inline T dot (const vec3<T>& a, const vec3<T>& b){return a.x*b.x + a.y*b.y + a.z*b.z;}
template<class T> inline vec3<T> cmul (const vec3<T>& a, const vec3<T>& b){return vec3<T>(a.x*b.x, a.y*b.y, a.z*b.z);}
template<class T> inline vec3<T> cross(const vec3<T>& a, const vec3<T>& b){return vec3<T>(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<class T> inline T magnitude( const vec3<T>& v ){return squareroot(dot(v,v));}
template<class T> inline vec3<T> normalize( const vec3<T>& v ){return v/magnitude(v);}
template<class T> inline vec3<T>& operator+=(vec3<T>& a, const vec3<T>& b){a.x+=b.x;a.y+=b.y;a.z+=b.z;return a;}
template<class T> inline vec3<T>& operator-=(vec3<T>& a, const vec3<T>& b){a.x-=b.x;a.y-=b.y;a.z-=b.z;return a;}
template<class T> inline vec3<T>& operator*=(vec3<T>& v, T s){v.x*=s;v.y*=s;v.z*= s;return v;}
template<class T> inline vec3<T>& operator/=(vec3<T>& 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<class T> inline vec3<T>VectorMin(const vec3<T> &a,const vec3<T> &b) {return vec3<T>(Min(a.x,b.x),Min(a.y,b.y),Min(a.z,b.z));}
template<class T> inline vec3<T>VectorMax(const vec3<T> &a,const vec3<T> &b) {return vec3<T>(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 T>
class mat3x3
{
public:
vec3<T> 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<T> &_x,const vec3<T> &_y,const vec3<T> &_z):x(_x),y(_y),z(_z){}
inline vec3<T>& operator[](int i) {return (&x)[i];}
inline const vec3<T>& 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<float> float3x3;
float3x3 Transpose( const float3x3& m );
template<class T> vec3<T> operator*( const vec3<T>& v , const mat3x3<T>& m )
{
return vec3<T>((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 T>
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<T> &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<T>& xyz() const { return *((vec3<T>*)this);}
inline vec3<T>& xyz() { return *((vec3<T>*)this);}
};
typedef vec4<float> float4;
typedef vec4<int> int4;
typedef vec4<unsigned char> byte4;
template<class T> inline int operator==(const vec4<T> &a,const vec4<T> &b) {return (a.x==b.x && a.y==b.y && a.z==b.z && a.w==b.w);}
template<class T> inline int operator!=(const vec4<T> &a,const vec4<T> &b) {return !(a==b);}
template<class T> inline vec4<T> operator+(const vec4<T>& a, const vec4<T>& b ){return vec4<T>(a.x+b.x,a.y+b.y,a.z+b.z,a.w+b.w);}
template<class T> inline vec4<T> operator-(const vec4<T>& a, const vec4<T>& b ){return vec4<T>(a.x-b.x,a.y-b.y,a.z-b.z,a.w-b.w);}
template<class T> inline vec4<T> operator-(const vec4<T>& v){return vec4<T>(-v.x,-v.y,-v.z,-v.w);}
template<class T> inline vec4<T> operator*(const vec4<T>& v, const T &s ){ return vec4<T>( v.x*s, v.y*s, v.z*s,v.w*s);}
template<class T> inline vec4<T> operator*(T s, const vec4<T>& v ){return v*s;}
template<class T> inline vec4<T> operator/(const vec4<T>& v, T s ){return vec4<T>( v.x/s, v.y/s, v.z/s,v.w/s );}
template<class T> inline T dot(const vec4<T>& a, const vec4<T>& b ){return a.x*b.x + a.y*b.y + a.z*b.z+a.w*b.w;}
template<class T> inline vec4<T> cmul(const vec4<T> &a, const vec4<T> &b) {return vec4<T>(a.x*b.x, a.y*b.y, a.z*b.z,a.w*b.w);}
template<class T> inline vec4<T>& operator+=(vec4<T>& a, const vec4<T>& b ){a.x+=b.x;a.y+=b.y;a.z+=b.z;a.w+=b.w;return a;}
template<class T> inline vec4<T>& operator-=(vec4<T>& a, const vec4<T>& b ){a.x-=b.x;a.y-=b.y;a.z-=b.z;a.w-=b.w;return a;}
template<class T> inline vec4<T>& operator*=(vec4<T>& v, T s){v.x*=s;v.y*=s;v.z*=s;v.w*=s;return v;}
template<class T> inline vec4<T>& operator/=(vec4<T>& v, T s){v.x/=s;v.y/=s;v.z/=s;v.w/=s;return v;}
template<class T> inline T magnitude( const vec4<T>& v ){return squareroot(dot(v,v));}
template<class T> inline vec4<T> normalize( const vec4<T>& v ){return v/magnitude(v);}
struct D3DXMATRIX;
template<class T>
class mat4x4
{
public:
vec4<T> x,y,z,w; // the 4 rows
inline mat4x4(){}
inline mat4x4(const vec4<T> &_x, const vec4<T> &_y, const vec4<T> &_z, const vec4<T> &_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<T>& operator[](int i) {assert(i>=0&&i<4);return (&x)[i];}
inline const vec4<T>& 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<float> 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 T>
class quaternion : public vec4<T>
{
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<T> &v):vec4<T>(v){}
T angle() const { return acosf(this->w)*2.0f; }
vec3<T> axis() const { vec3<T> a(this->x,this->y,this->z); if(fabsf(angle())<0.0000001f) return vec3<T>(1,0,0); return a*(1/sinf(angle()/2.0f)); }
inline vec3<T> xdir() const { return vec3<T>( 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<T> ydir() const { return vec3<T>( 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<T> zdir() const { return vec3<T>( 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<T> getmatrix() const { return mat3x3<T>( xdir(), ydir(), zdir() ); }
//operator float3x3() { return getmatrix(); }
void Normalize();
};
template<class T>
inline quaternion<T> quatfrommat(const mat3x3<T> &m)
{
T magw = m[0 ][ 0] + m[1 ][ 1] + m[2 ][ 2];
T magxy;
T magzw;
vec3<T> pre;
vec3<T> prexy;
vec3<T> prezw;
quaternion<T> postxy;
quaternion<T> postzw;
quaternion<T> post;
int wvsz = (magw > m[2][2] ) ;
magzw = (wvsz) ? magw : m[2][2];
prezw = (wvsz) ? vec3<T>(1.0f,1.0f,1.0f) : vec3<T>(-1.0f,-1.0f,1.0f) ;
postzw = (wvsz) ? quaternion<T>(0.0f,0.0f,0.0f,1.0f): quaternion<T>(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<T>(1.0f,-1.0f,-1.0f) : vec3<T>(-1.0f,1.0f,-1.0f) ;
postxy = (xvsy) ? quaternion<T>(1.0f,0.0f,0.0f,0.0f): quaternion<T>(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<T> 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<float> 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<class T> inline quaternion<T> Conjugate(const quaternion<T> &q){return quaternion<T>(-q.x,-q.y,-q.z,q.w);}
template<class T> inline quaternion<T> Inverse(const quaternion<T> &q){return Conjugate(q);}
template<class T> inline quaternion<T> normalize( const quaternion<T> & a ){return quaternion<T> (normalize((vec4<T>&)a));}
template<class T> inline quaternion<T>& operator*=(quaternion<T>& a, T s ){return (quaternion<T>&)((vec4<T>&)a *=s);}
template<class T> inline quaternion<T> operator*( const quaternion<T>& a, float s ){return quaternion<T>((vec4<T>&)a*s);}
template<class T> inline quaternion<T> operator+( const quaternion<T>& a, const quaternion<T>& b){return quaternion<T>((vec4<T>&)a+(vec4<T>&)b);}
template<class T> inline quaternion<T> operator-( const quaternion<T>& a, const quaternion<T>& b){return quaternion<T>((vec4<T>&)a-(vec4<T>&)b);}
template<class T> inline quaternion<T> operator-( const quaternion<T>& b){return quaternion<T>(-(vec4<T>&)b);}
template<class T> inline quaternion<T> operator*( const quaternion<T>& a, const quaternion<T>& b)
{
return quaternion<T>(
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<float>(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<class T>
inline int maxdir(const T *p,int count,const T &dir)
{
assert(count);
int m=0;
for(int i=1;i<count;i++)
{
if(dot(p[i],dir)>dot(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