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

Code-style consistency improvement:

Browse Source 
Apply clang-format-all.sh using the _clang-format file through all the cpp/.h files.
make sure not to apply it to certain serialization structures, since some parser expects the * as part of the name, instead of type.
This commit contains no other changes aside from adding and applying clang-format-all.sh
This commit is contained in:
erwincoumans
2018-09-23 14:17:31 -07:00
parent b73b05e9fb
commit ab8f16961e
1773 changed files with 1081087 additions and 474249 deletions
Download Patch File Download Diff File Expand all files Collapse all files

95
examples/Experiments/ImplicitCloth/stan/Cloth.cpp
View File

@@ -9,62 +9,54 @@
#include "Cloth.h"
Array<Cloth*> cloths;
Array<Cloth *> cloths;
Cloth::Cloth(const char *_name,int _n):SpringNetwork(_n),
color(0,0.5f,1.0f)
Cloth::Cloth(const char *_name, int _n) : SpringNetwork(_n),
color(0, 0.5f, 1.0f)
{
cloths.Add(this);
cloths.Add(this);
}
Cloth::~Cloth()
{
cloths.Remove(this);
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
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)
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;
// 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);
int cloth_tess = 20;
float3 cloth_spawnpoint(0, 3, 5.0f);
/*
static void ClothDrawSprings(Cloth *cloth)
@@ -79,18 +71,17 @@ static void ClothDrawSprings(Cloth *cloth)
}
}
*/
int cloth_showsprings=0;
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
}
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
}
}

16
examples/Experiments/ImplicitCloth/stan/Cloth.h
View File

@@ -5,14 +5,14 @@
class Cloth : public SpringNetwork
{
public:
int w,h;
float3 color; // for debug rendering
Cloth(const char* _name,int _n);
~Cloth();
public:
int w, h;
float3 color; // for debug rendering
Cloth(const char* _name, int _n);
~Cloth();
};
Cloth *ClothCreate(int w,int h,float size);
Cloth* ClothCreate(int w, int h, float size);
#endif //STAN_CLOTH_H
#endif //STAN_CLOTH_H

229
examples/Experiments/ImplicitCloth/stan/SpringNetwork.cpp
View File

@@ -1,10 +1,8 @@
#include "vec3n.h"
//#include "console.h"
extern int numX;
//
// Cloth - Backward Integrated Spring Network
//
@@ -42,157 +40,144 @@ extern int numX;
//#include "object.h"
//#include "xmlparse.h"
static const float3x3 I(1, 0, 0, 0, 1, 0, 0, 0, 1);
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)
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;
// 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)
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)));
// 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)
inline float3x3 dfdv_damp(const float3 &dir, float damping)
{
// derivative of force wrt velocity.
return outerprod(dir,dir) * 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)
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);
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;
// 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]
// 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);
// 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
}
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);
// 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);
X = X + V*dt;
dFdXmV = dFdX * V;
B = F * dt + dFdXmV * (dt * dt);
UpdateLimits();
awake = (dot(V,V)<cloth_sleepthreshold)?awake-1:awake=cloth_sleepcount;
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;
}

100
examples/Experiments/ImplicitCloth/stan/SpringNetwork.h
View File

@@ -4,60 +4,58 @@
#include "vec3n.h"
#define SPRING_STRUCT (0)
#define SPRING_SHEAR (1)
#define SPRING_BEND (2)
#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;
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();
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
#endif //STAN_SPRING_NETWORK_H

303
examples/Experiments/ImplicitCloth/stan/array.h
View File

@@ -1,41 +1,41 @@
//
// 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.
// 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++)
// for(i=0;i<myarray.count;i++)
// myarray[i] = somefunction(i);
//
// When the array runs out of room, it
//
// 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
// 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.
// 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
// 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
// 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
//
// 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
// chosen to be C++ish and have the array elements constructors/destructors
// invoked as expected.
//
//
@@ -46,231 +46,257 @@
#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]; }
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)
template <class Type>
Array<Type>::Array(int s)
{
if(s==-1) return;
count=0;
if (s == -1) return;
count = 0;
array_size = 0;
element = NULL;
if(s)
{
if (s)
{
allocate(s);
}
}
template <class Type> Array<Type>::Array(Array<Type> &array)
template <class Type>
Array<Type>::Array(Array<Type> &array)
{
count=0;
count = 0;
array_size = 0;
element = NULL;
*this = array;
}
template <class Type> Array<Type> &Array<Type>::copy(const Array<Type> &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++)
{
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)
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
if (array.array_size < 0) // negative number means steal the data buffer instead of copying
{
delete[] element;
delete[] element;
element = array.element;
array_size = -array.array_size;
count = array.count;
array.count =array.array_size = 0;
array.count = array.array_size = 0;
array.element = NULL;
return *this;
}
count=0;
for(int i=0;i<array.count;i++)
{
count = 0;
for (int i = 0; i < array.count; i++)
{
Add(array[i]);
}
return *this;
}
template <class Type> Array<Type>::~Array()
template <class Type>
Array<Type>::~Array()
{
if (element != NULL && array_size!=0)
{
delete[] element;
}
count=0;array_size=0;element=NULL;
if (element != NULL && array_size != 0)
{
delete[] element;
}
count = 0;
array_size = 0;
element = NULL;
}
template <class Type> void Array<Type>::allocate(int s)
template <class Type>
void Array<Type>::allocate(int s)
{
assert(s>0);
assert(s>=count);
if(s==array_size) return;
assert(s > 0);
assert(s >= count);
if (s == array_size) return;
Type *old = element;
array_size =s;
array_size = s;
element = new Type[array_size];
assert(element);
for(int i=0;i<count;i++)
{
element[i]=old[i];
for (int i = 0; i < count; i++)
{
element[i] = old[i];
}
if(old) delete[] old;
if (old) delete[] old;
}
template <class Type> void Array<Type>::SetSize(int s)
template <class Type>
void Array<Type>::SetSize(int s)
{
if(s==0)
{
if(element)
{
delete[] element;
element = NULL;
}
if (s == 0)
{
if (element)
{
delete[] element;
element = NULL;
}
array_size = s;
}
else
{
allocate(s);
}
count=s;
}
else
{
allocate(s);
}
count = s;
}
template <class Type> void Array<Type>::Pack()
template <class Type>
void Array<Type>::Pack()
{
allocate(count);
}
template <class Type> Type& Array<Type>::Add(Type t)
template <class Type>
Type &Array<Type>::Add(Type t)
{
assert(count<=array_size);
if(count==array_size)
{
allocate((array_size)?array_size *2:16);
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
// dissallow duplicates
// assert(element[i] != t);
//}
element[count++] = t;
return element[count-1];
return element[count - 1];
}
template <class Type> int Array<Type>::Contains(Type t)
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++;
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)
template <class Type>
void Array<Type>::AddUnique(Type t)
{
if(!Contains(t)) Add(t);
if (!Contains(t)) Add(t);
}
template <class Type> void Array<Type>::DelIndex(int i)
template <class Type>
void Array<Type>::DelIndex(int i)
{
assert(i<count);
assert(i < count);
count--;
while(i<count)
{
element[i] = element[i+1];
while (i < count)
{
element[i] = element[i + 1];
i++;
}
}
template <class Type> Type& Array<Type>::DelIndexWithLast(int i)
template <class Type>
Type &Array<Type>::DelIndexWithLast(int i)
{
assert(i<count);
assert(i < count);
count--;
if(i<count)
{
Type r=element[i];
if (i < count)
{
Type r = element[i];
element[i] = element[count];
element[count]=r;
element[count] = r;
}
return element[count];
}
template <class Type> void Array<Type>::Remove(Type t)
template <class Type>
void Array<Type>::Remove(Type t)
{
int i;
for(i=0;i<count;i++)
{
if(element[i] == t)
{
for (i = 0; i < count; i++)
{
if (element[i] == t)
{
break;
}
}
assert(i<count); // assert object t is in the array.
assert(i < count); // assert object t is in the array.
DelIndex(i);
for(i=0;i<count;i++)
{
for (i = 0; i < count; i++)
{
assert(element[i] != t);
}
}
template <class Type> void Array<Type>::Insert(Type t,int k)
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];
int i = count;
Add(t); // to allocate space
while (i > k)
{
element[i] = element[i - 1];
i--;
}
assert(i==k);
element[k]=t;
assert(i == k);
element[k] = t;
}
template <class Type> int Array<Type>::IndexOf(Type t)
template <class Type>
int Array<Type>::IndexOf(Type t)
{
int i;
for(i=0;i<count;i++)
{
if(element[i] == t)
{
for (i = 0; i < count; i++)
{
if (element[i] == t)
{
return i;
}
}
@@ -278,7 +304,4 @@ template <class Type> int Array<Type>::IndexOf(Type t)
return -1;
}
#endif

235
examples/Experiments/ImplicitCloth/stan/vec3n.cpp
View File

@@ -1,16 +1,15 @@
//
// 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;
int conjgrad_loopcount;
int conjgrad_looplimit = 100;
/*EXPORTVAR(conjgrad_lasterror);
EXPORTVAR(conjgrad_epsilon );
@@ -18,135 +17,131 @@ EXPORTVAR(conjgrad_loopcount);
EXPORTVAR(conjgrad_looplimit);
*/
int ConjGradient(float3N &X, float3Nx3N &A, float3N &B)
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
// 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)
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
// 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)
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;
}
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)
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
// 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;
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;
class dotest
{
public:
dotest() { testfloat3N(); }
} do_test_at_program_startup;

387
examples/Experiments/ImplicitCloth/stan/vec3n.h
View File

@@ -1,33 +1,33 @@
//
// Big Vector and Sparse Matrix Classes
//
//
// (c) S Melax 2006
//
// The focus is on 3D applications, so
// 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
// - 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
// 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,
// 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,
// 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
// 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,
// The code within those important algorithms, which use this library,
// can now remain clean and readable yet still execute quickly.
//
@@ -41,111 +41,144 @@
//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){}
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:
public:
class Block
{
public:
public:
float3x3 m;
int r,c;
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);}
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;
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;}
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>
class float3N : public Array<float3>
{
public:
float3N(int _count=0)
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;}
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);
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)
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++)
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)
inline float dot(const float3N &a, const float3N &b)
{
float d=0;
for(int i=0;i<a.count;i++)
float d = 0;
for (int i = 0; i < a.count; i++)
{
d+= dot(a[i],b[i]);
d += dot(a[i], b[i]);
}
return d;
}
inline void float3Nx3N::Zero()
{
for(int i=0;i<blocks.count;i++)
for (int i = 0; i < blocks.count; i++)
{
blocks[i].m = float3x3(0,0,0,0,0,0,0,0,0);
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++)
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);
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++)
for (int i = 0; i < count; i++)
{
element[i] = float3(0,0,0);
element[i] = float3(0, 0, 0);
}
}
inline void float3N::Init(const float3 &v)
inline void float3N::Init(const float3 &v)
{
for(int i=0;i<count;i++)
for (int i = 0; i < count; i++)
{
element[i] = v;
}
@@ -157,56 +190,55 @@ inline void float3N::Init(const float3 &v)
// 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)
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];
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)
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;
for (int i = 0; i < a.count; i++) r[i] = a[i] * s;
return r;
}
inline float3N operator /(const float3N &a,const float &s)
inline float3N operator/(const float3N &a, const float &s)
{
float3N r(a.count);
return Mul(r,a, 1.0f/s );
return Mul(r, a, 1.0f / s);
}
inline float3N operator -(const float3N &a,const float3N &b)
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];
for (int i = 0; i < a.count; i++) r[i] = a[i] - b[i];
return r;
}
inline float3N operator -(const float3N &a)
inline float3N operator-(const float3N &a)
{
float3N r(a.count);
for(int i=0;i<a.count;i++) r[i]=-a[i];
for (int i = 0; i < a.count; i++) r[i] = -a[i];
return r;
}
inline float3N operator *(const float3Nx3N &m,const float3N &v)
inline float3N operator*(const float3Nx3N &m, const float3N &v)
{
float3N r(v.count);
return Mul(r,m,v);
return Mul(r, m, v);
}
inline float3N &operator-=(float3N &A, const float3N &B)
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];
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)
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];
assert(A.count == B.count);
for (int i = 0; i < A.count; i++) A[i] += B[i];
return A;
}
#else
// Optimized Expressions
@@ -215,10 +247,19 @@ 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];}
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
@@ -226,10 +267,19 @@ 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];}
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
@@ -237,104 +287,149 @@ 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];}
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;}
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];}
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;}
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);}
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;}
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 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;}
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);}
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

1316
examples/Experiments/ImplicitCloth/stan/vecmath.cpp
View File

File diff suppressed because it is too large Load Diff

835
examples/Experiments/ImplicitCloth/stan/vecmath.h
View File

File diff suppressed because it is too large Load Diff