rendered paste body/**
* ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
*
* This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* as published by the Free Software Foundation; either version 2
* of the License, or (at your option) any later version. The Blender
* Foundation also sells licenses for use in proprietary software under
* the Blender License. See http://www.blender.org/BL/ for information
* about this.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software Foundation,
* Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
*
* The Original Code is Copyright (C) 2006 by NaN Holding BV.
* All rights reserved.
*
* The Original Code is: all of this file.
*
* Contributor(s): Daniel Genrich (Genscher)
*
* ***** END GPL/BL DUAL LICENSE BLOCK *****
*/
#include <windows.h>
#include <stdio.h>
#include <float.h>
#include <ctime> // For time()
#include <cstdlib> // For srand() and rand()
#include "cloth_extern.h"
#include "vec3n.h"
#include "vecmath.h"
#include "LinearMath/btVector3.h"
#include "LinearMath/btScalar.h"
#include "LinearMath/btMatrix3x3.h"
#include "LinearMath/btTransform.h"
#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
#include "BulletCollision/CollisionShapes/btTriangleShape.h"
#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
#include "BulletCollision/NarrowPhaseCollision/btPointCollector.h"
#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
#include "BulletCollision/NarrowPhaseCollision/btGjkEpa.h"
#include "BulletCollision/CollisionShapes/btMinkowskiSumShape.h"
#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
#include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h"
#include "BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h"
#include "BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h"
#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"
#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"
#include "LinearMath/btStackAlloc.h"
/*
#include <SOLID.h>
#include "MT_Point3.h"
#include "MT_Vector3.h"
*/
// #include "vclip.h"
#include <string.h>
#include <stdio.h>
#define SPRING_STRUCT (0)
#define SPRING_SHEAR (1)
#define SPRING_BEND (2)
static const float3x3 I(1,0,0,0,1,0,0,0,1);
#define VECADD(v1,v2,v3) {*(v1)= *(v2) + *(v3); *(v1+1)= *(v2+1) + *(v3+1); *(v1+2)= *(v2+2) + *(v3+2);}
#define VECSUB(v1,v2,v3) {*(v1)= *(v2) - *(v3); *(v1+1)= *(v2+1) - *(v3+1); *(v1+2)= *(v2+2) - *(v3+2);}
#define VECADDFAC(v1,v2,v3,fac) {*(v1)= *(v2) + *(v3)*(fac); *(v1+1)= *(v2+1) + *(v3+1)*(fac); *(v1+2)= *(v2+2) + *(v3+2)*(fac);}
#define VECCOPY(v1,v2) {*(v1)= *(v2); *(v1+1)= *(v2+1); *(v1+2)= *(v2+2);}
#define INPR(v1, v2) ( (v1)[0]*(v2)[0] + (v1)[1]*(v2)[1] + (v1)[2]*(v2)[2] )
float Normalise(float *n)
{
float d;
d= n[0]*n[0]+n[1]*n[1]+n[2]*n[2];
/* A larger value causes normalise errors in a scaled down models with camera xtreme close */
if(d>1.0e-35F) {
d= (float)sqrt(d);
n[0]/=d;
n[1]/=d;
n[2]/=d;
} else {
n[0]=n[1]=n[2]= 0.0;
d= 0.0;
}
return d;
}
void VecMulf(float *v1, float f)
{
v1[0]*= f;
v1[1]*= f;
v1[2]*= f;
}
using namespace std;
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;
}
SpringNetwork::SpringNetwork(int _n):X(_n),V(_n),Xold(_n),Vold(_n),Xold2(_n),Vold2(_n),F(_n),dX(_n),z(_n),olddV(_n),A(_n),dFdX(_n),dFdV(_n),M(_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=1000.0f;
spring_shear=100.0f;
spring_bend=25.0f;
spring_damp=5.0f;
spring_air=1.0f;
cb = 200 * spring_bend;
cf = 0.01;
csf = 0.01;
cloth_gravity=float3(0,0,-9.81); // gravity is meters per sec^2
step = 1;
dtold = 0;
}
SpringNetwork::Spring &SpringNetwork::AddBlocks(Spring &s)
{
/*
if(s.type==SPRING_BEND)
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(Block(s.a,s.b));
dFdX.blocks.Add(Block(s.a,s.b));
dFdV.blocks.Add(Block(s.a,s.b));
s.iba = A.blocks.count; // added 'ba' blocks will have this index.
A.blocks.Add(Block(s.b,s.a));
dFdX.blocks.Add(Block(s.b,s.a));
dFdV.blocks.Add(Block(s.b,s.a));
return s;
}
void Zero(float3x3 &m)
{
m.x.x = m.y.y = m.z.z = m.x.y = m.x.z = m.y.x = m.y.z = m.z.x = m.z.y = 0;
}
void SpringNetwork::PreSolveSpring(const SpringNetwork::Spring &s, float3N &cX, float3N &cV)
{
// DO IT HERE
}
void SpringNetwork::CalcForces(ClothVertex *extforce, float3N &cX, float3N &cV)
{
// Collect forces and derivatives: F,dFdX,dFdV
dFdX.Zero();
dFdV.InitDiagonal(-spring_air);
F.Init(cloth_gravity);
if(extforce)
{
for(int i=0;i<F.count;i++)
{
F[i] += float3(extforce[i].extForce[0], extforce[i].extForce[1], extforce[i].extForce[2]);
}
}
F -= cV * spring_air;
for(int i=0;i<springs.count;i++)
{
PreSolveSpring(springs[i], cX, cV); // will add to F,dFdX,dFdV
}
}
void SpringNetwork::BuildM(int n)
{
// float random;
// float signum;
// srand(time(0));
float3x3 temp, temp2;
temp[0][1] = temp[0][2] = temp[1][0] = temp[1][2] = temp[2][0] = temp[2][1] = 0;
for (int h=0; h<n; h++)
{
// for (int w=0; w<n; w++)
{
/*
random = (rand() % 800) + 100; // 100 - 900
random = 1.0f / random;
signum = (rand() %2);
if(!signum)
signum = -1.0f;
random = weight + signum * random * weight;
*/
temp[0][0] = temp[1][1] = temp[2][2] = weight; //random;
/*
if(temp[0][0]==0)
temp2[0][0] = 0;
else
temp2[0][0] = 1 / temp[0][0];
if(temp[1][1]==0)
temp2[1][1] = 0;
else
temp2[1][1] = 1 / temp[1][1];
if(temp[2][2]==0)
temp2[2][2] = 0;
else
temp2[2][2] = 1 / temp[2][2];
*/
// only fill the diagonals
M.blocks[h].m = float3x3(temp[0][0],0,0,0,temp[1][1],0,0,0,temp[2][2]);
}
}
}
void SpringNetwork::Simulate(float3N &cX, float3N &cV, float3N &cXold, float3N &cVold, float3N &Xnew, float3N &Vnew, float dt, float ldtold, ClothVertex *extforce, int update)
{
// Get ready for conjugate gradient iterative solver step.
// Initialize operands.
int n=X.count; // all our big vectors are of this size
float3N temp(n); // temp to store result of matrix multiply
float3N B(n);
float3N dFdXmV(n);
dX.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);
PreConjGradientFiltered(epsilon, dX,A,B,S, dX,z, dt);
Vnew = V + dX;
Xnew = X + Vnew*dt;
}
void SpringNetwork::SimulateCentral(float3N &cX, float3N &cV, float3N &Vnew, float dt, ClothVertex *extforce)
{
int n=X.count; // all our big vectors are of this size
float3N temp(n); // temp to store result of matrix multiply
float3N B(n);
dX.Zero();
A.Identity(); // build up the big matrix we feed to solver
A = M * A;
A -= dFdV*(dt/2.0f);
B.Zero();
B = M * cV;
B += (dt/2.0f)*F;
// solve Ax = b for x (where x equals dX)
PreConjGradientFiltered(epsilon, dX,A,B,S,z, olddV, dt);
// ConjGradientFiltered(dV, A, B, S, z);
Vnew = 1.0f * dX;
}
void SpringNetwork::copyAllFromClothNetwork(ClothVertex *source, int numberV)
{
// 1. copy cloth positions
for(int i = 0; i < numberV; i++)
{
source[i].prevV[0] = source[i].v[0];
source[i].prevV[1] = source[i].v[1];
source[i].prevV[2] = source[i].v[2];
}
}
int SpringNetwork::advanceStep(float dt, ClothVertex *extforce, ClothModifierData *clmd, CM_COLLISION_SELF self_collision, CM_COLLISION_OBJ obj_collision, ClothVertex *source)
{
int n=X.count; // all our big vectors are of this size
float3N Xnew(n);
float3N Vnew(n);
float3N Vcorr(n);
float t = 0.0f;
float tf = 1.0f;
LARGE_INTEGER before, before1, before2, before3;
LARGE_INTEGER after, after1, after2, after3;
LARGE_INTEGER freq;
double milliseconds;
bool timecoll_occured = 0, coll_occured = 0;
int c = 0;
if(dtold == 0)
dtold = dt;
while(t < tf)
{
// clean matrix S to only hold pinned verts
for(int i = S.blocks.count-1; i >=0 ; i--)
{
while((i<S.blocks.count) && (!S.blocks[i].pinned))
{
z[S.blocks[i].c] = float3(0,0,0);
S.blocks.DelIndexWithLast(i);
}
}
CalcForces(extforce, X, V);
Simulate(X, V, Xold, Vold, Xnew, Vnew, dt, dt, extforce, 1);
// update collision positions with suggested future positions
for(int i = 0; i < n; i++)
{
source[i].v[0] = (Xnew[i])[0];
source[i].v[1] = (Xnew[i])[1];
source[i].v[2] = (Xnew[i])[2];
VECSUB(source[i].velocity, source[i].v, source[i].prevV);
VecMulf(source[i].velocity, 1.0f/dt);
}
// get & save all coliding faces
if (obj_collision)
{
// get & save all coliding faces
obj_collision (clmd, step, implicit_collision_response);
// check for near faces and resolve colissions
coll_occured = solve_coherence(dt, source);
}
/*
if (obj_collision)
{
// get & save all coliding faces
obj_collision (clmd, step, implicit_timecollision_response);
// check for near faces and resolve potential collisions
timecoll_occured = solve_timecoherence(dt, source);
}
*/
if(coll_occured)
{
// update the corrected velocity
for(int i = 0; i < n; i++)
{
(Vcorr[i])[0] = source[i].velocity[0];
(Vcorr[i])[1] = source[i].velocity[1];
(Vcorr[i])[2] = source[i].velocity[2];
}
Xnew = X + Vcorr*dt;
CalcForces(extforce, Xnew, Vcorr);
SimulateCentral(X, Vcorr, Vnew, dt, extforce);
olddV = Vnew - Vcorr;
// update the cloth network
Vold = Vcorr;
}
else
Vold = V;
Xold = X;
for(int i = 0; i < n; i++)
{
source[i].prevV[0] = (Xnew[i])[0];
source[i].prevV[1] = (Xnew[i])[1];
source[i].prevV[2] = (Xnew[i])[2];
}
V = Vnew;
X = Xnew;
t = t + dt;
/*
if(!step)
step = true;
*/
c++;
// if(c==4)
// break;
}
return true;
}
extern "C"
SpringNetwork *initClothNetwork(ClothVertex *source, int numberV, ClothSpring *springs, int numberS, SimulationSettings *simsettings, CollisionSettings *collsettings)
{
SpringNetwork *cloth = new SpringNetwork(numberV);
// 1. copy settings
cloth->spring_damp = simsettings->Cdis * 0.01; // mechanical spring damping
cloth->spring_air = simsettings->Cvi * 0.01; // viscous spring damping
cloth->cloth_gravity[0] = simsettings->gravity[0]; cloth->cloth_gravity[1] = simsettings->gravity[1]; cloth->cloth_gravity[2] = simsettings->gravity[2]; // gravity
cloth->spring_struct = simsettings->structural; // spring_struct=1000.0f;
cloth->spring_shear = simsettings->shear; // spring_shear=100.0f;
cloth->spring_bend = simsettings->flexion; // spring_bend=25.0f;
cloth->cb = 20000 * cloth->spring_bend; // bei 2000 * bending => 500 - 1000 struct/shear = viele Falten, 3000 = wenig/keine Falten
cloth->weight = simsettings->mass;
cloth->epsilon = collsettings->epsilon;
cloth->cf = collsettings->friction * 0.01; // friction
cloth->csf = collsettings->self_friction * 0.01; // selffriction
// 2. copy cloth positions
for(int i = 0; i < numberV; i++)
{
cloth->X[i] = float3(source[i].v[0], source[i].v[1], source[i].v[2]);
cloth->Xold[i] = float3(source[i].v[0], source[i].v[1], source[i].v[2]);
if(source [i].flags & CVERT_FLAG_PINNED)
{
cloth->S.blocks.Add(Block(i,i)).m=float3x3(0,0,0,0,0,0,0,0,0);
cloth->S.blocks[cloth->S.blocks.count-1].ndof = 0;
cloth->S.blocks[cloth->S.blocks.count-1].pinned = 1;
}
}
// 3. copy springs
for (int i = 0; i < numberS; i++)
{
switch (springs [i].type)
{
case STRUCTURAL:
cloth->CreateSpring(SPRING_STRUCT,springs [i].ij,springs [i].kl);
break;
case SHEAR:
cloth->CreateSpring(SPRING_SHEAR,springs [i].ij,springs [i].kl);
break;
case FLEXION:
cloth->CreateSpring(SPRING_BEND,springs [i].ij,springs [i].kl);
break;
}
}
cloth-> BuildM(numberV);
return cloth;
}
extern "C"
void setupzOfClothNetwork(SpringNetwork *cloth, ClothVertex *source, int numberV, float dt)
{
// apply velocity of the pinned verteces
for(int i = 0; i < numberV; i++)
{
if(source [i].flags & CVERT_FLAG_PINNED)
{
float3 tempa(source [i].v[0], source [i].v[1], source [i].v[2]);
float3 tempb(source [i].prevV[0], source [i].prevV[1], source [i].prevV[2]);
cloth->z[i]= (tempa - tempb) * dt;
}
}
}
extern "C"
void copyToClothNetwork(SpringNetwork *cloth, ClothVertex *source, int numberV, ClothSpring *springs, int numberS)
{
if(!cloth)
return;
// TODO
// check if width*height == numberV
// 1. copy cloth positions
for(int i = 0; i < numberV; i++)
{
cloth->X[i] = float3(source[i].v[0], source[i].v[1], source[i].v[2]);
cloth->Xold[i] = float3(source[i].v[0], source[i].v[1], source[i].v[2]);
}
// 2. copy springs
for (int i = 0; i < numberS; i++)
{
switch (springs [i].type)
{
case STRUCTURAL:
cloth->CreateSpring(SPRING_STRUCT,springs [i].ij,springs [i].kl);
break;
case SHEAR:
cloth->CreateSpring(SPRING_SHEAR,springs [i].ij,springs [i].kl);
break;
case FLEXION:
cloth->CreateSpring(SPRING_BEND,springs [i].ij,springs [i].kl);
break;
}
}
// 3. copy settings
// TODO
}
extern "C"
void copyFromClothNetwork(SpringNetwork *cloth, ClothVertex *source, int numberV)
{
if(!cloth)
return;
// 1. copy cloth positions
for(int i = 0; i < numberV; i++)
{
source[i].v[0] = (cloth->X[i])[0];
source[i].v[1] = (cloth->X[i])[1];
source[i].v[2] = (cloth->X[i])[2];
}
}
extern "C"
void deleteClothNetwork(SpringNetwork *cloth)
{
if(cloth)
{
delete cloth;
cloth = NULL;
}
}
extern "C"
void simulateClothNetwork(SpringNetwork *clothNetwork, float timestep, ClothVertex *extforce, ClothModifierData *clmd, CM_COLLISION_SELF self_collision, CM_COLLISION_OBJ obj_collision, ClothVertex *source)
{
if(!clothNetwork)
return;
clothNetwork->advanceStep(timestep, extforce, clmd, self_collision, obj_collision, source);
}
extern "C"
int getStepFromClothNetwork(SpringNetwork *cloth)
{
if(!cloth)
return -1;
return cloth->step;
}
extern "C"
void setStepFromClothNetwork(SpringNetwork *cloth, int step)
{
if(!cloth)
return;
cloth->step = step;
}