基于韦尔莱算法的可撕扯的幕布
题记：写这个程序是因为偶然看到这个链接里的幕布模拟效果，虽然作者公布了代码，但是笔者没有参考，根据标题<基于韦尔莱算法>自己写了下面这个程序，权当练习。
正如题记所言，该程序的意义就是模拟一个可以撕扯的幕布，在显示上使用OpenCV3.0的库来显示，并且响应鼠标消息。顺便提一句，OpenCV3.0相比之前的2.4.x而言，终于把多个lib合并到一个了，现在只要添加一个world就都解决了，真好啊！
读者可以参考维基百科对韦尔莱算法的介绍，就能想清楚这个程序的实现。不过这里最复杂的还是各种参数的调整，否则会出现不少bug。
首先给出我编译后的程序：http://download.csdn.net/detail/delltdk/8691797
下面是主要代码：
CurtainGrain：
CurtainGrain.h
#ifndef _CURTAINGRAIN_H_
#define _CURTAINGRAIN_H_
#include "stdafx.h"
#include <math.h>
#define MAX(x,y) (x)>(y)?(x):(y)
#define MIN(x,y) (x)<(y)?(x):(y)
#define BreakLink 0
#define Gravity_g 9.8f
enum NeighborType
{
upNeibor = 0x01,
downNeibor,
leftNeibor,
rightNeibor
};
struct Acc{
float ax;
float ay;
float az;
};
struct Power
{
float px;
float py;
float pz;
void Add( const Power f ){
px += f.px;
py += f.py;
pz += f.pz;
}
Acc GetAcc( const float m ){
Acc acc;
acc.ax = px/m;
acc.ay = py/m;
acc.az = pz/m;
}
};
struct Pos
{
float x;
float y;
float z;
};
struct Velocity
{
float vx;
float vy;
float vz;
};
class CurtainGrain
{
public:
CurtainGrain();
CurtainGrain(int x, int y);
~CurtainGrain();
int colId, rowId;
bool isVisited;
void SetFixed();
void SetPos();
void SetGround(float _ground);
void SetPos(const float scale);
void GetPos( Pos &_pos );
bool GetNeiborVisitStatus(NeighborType type);
bool NeiborExist(NeighborType type);
void GetUpPos( Pos &_pos );
void GetDownPos( Pos &_pos );
void GetLeftPos( Pos &_pos );
void GetRightPos( Pos &_pos );
void SetNeighbor( CurtainGrain* pNeibor, NeighborType type );
void RemoveNeighbor( NeighborType type );
void ClearOutsidePower();
void SetOutsidePower( const Power out_f );
void UpdatePosAfterPower(const float deltaT);
private:
void CalcSelfOtherPower( Power &selfPower, Power &otherPower, const Pos otherPos );
float iCalcSelfPower( const float dis );
// These are state properties of Curtain_Grain
bool isFixed;
Pos pos, prevPos;
Velocity vt, prevVt;
Power prevF;
float ground;
CurtainGrain* left;
CurtainGrain* right;
CurtainGrain* up;
CurtainGrain* down;
// These are inner properties of Curtain_Grain
float k; // the factor in equation f = k*x^2 when it's drawed
float l; // the factor in equation f = l*x^2 when it's clustered
float rho; // the density in equation m = rho*Volume
float s; // the length of Curtain_Grain, so Volume = s^2
float m; // the weight of one Curtain_Grain
float wind_fac; // the factor of wind strength
float linkPower; // max power the link can bear
Power f; // the sum power which is measured in N(Newton)
};
inline float redef_abs(float x)
{
return x>0?x:-x;
}
#endif
#include "stdafx.h"
#include "CurtainGrain.h"
CurtainGrain::CurtainGrain()
{
isVisited = false;
k = 2.0f; // N/m^2
l = 2.0f;
rho = 0.25f; // kg/m^3
s = 0.05f; // m
m = rho*s*s; // kg
wind_fac = 0.005f;
linkPower = 0.03f;
f.py = m*Gravity_g;
f.px = f.pz = 0.0f;
prevF = f;
vt.vx = vt.vy = vt.vz = 0.0f;
prevVt = vt;
left = right = up = down = BreakLink;
}
CurtainGrain::CurtainGrain( int x, int y )
{
colId = x;
rowId = y;
isVisited = false;
isFixed = false;
k = 2.0f; // N/m^2
l = 2.0f;
rho = 0.25f; // kg/m^3
s = 0.05f; // m
m = rho*s*s*0.02f; // kg
wind_fac = 0.005f;
linkPower = MAX(0.03f,0.06f-y*0.003f);
f.py = m*Gravity_g;
f.px = f.pz = 0.0f;
prevF = f;
vt.vx = vt.vy = vt.vz = 0.0f;
prevVt = vt;
left = right = up = down = BreakLink;
}
CurtainGrain::~CurtainGrain()
{
}
void CurtainGrain::SetFixed()
{
isFixed = true;
}
void CurtainGrain::SetPos()
{
prevPos.x = pos.x = colId*s;
prevPos.y = pos.y = rowId*s;
prevPos.z = pos.z = 0.0f;
}
void CurtainGrain::SetGround(float _ground)
{
ground = _ground;
}
void CurtainGrain::SetPos(const float scale)
{
prevPos.y *= scale;
pos.y *= scale;
}
void CurtainGrain::GetPos( Pos &_pos )
{
if( rowId==2 && colId== 0 && pos.y > 100 )
int tempInt = 0;
_pos = pos;
}
bool CurtainGrain::GetNeiborVisitStatus(NeighborType type)
{
switch (type)
{
case upNeibor:
return up->isVisited;
case downNeibor:
return down->isVisited;
case leftNeibor:
return left->isVisited;
case rightNeibor:
return right->isVisited;
default:
break;
}
return false;
}
bool CurtainGrain::NeiborExist(NeighborType type)
{
switch(type)
{
case upNeibor:
return up != BreakLink;
case downNeibor:
return down != BreakLink;
case leftNeibor:
return left != BreakLink;
case rightNeibor:
return right != BreakLink;
default:
return false;
}
}
void CurtainGrain::GetUpPos( Pos &_pos )
{
if(up) up->GetPos(_pos);
}
void CurtainGrain::GetDownPos( Pos &_pos )
{
if(down) down->GetPos(_pos);
}
void CurtainGrain::GetLeftPos( Pos &_pos )
{
if(left) left->GetPos(_pos);
}
void CurtainGrain::GetRightPos( Pos &_pos )
{
if(right) right->GetPos(_pos);
}
void CurtainGrain::SetNeighbor( CurtainGrain* pNeibor, NeighborType type )
{
if ( pNeibor )
{
switch (type)
{
case upNeibor:
{
up = pNeibor;
break;
}
case downNeibor:
{
down = pNeibor;
break;
}
case leftNeibor:
{
left = pNeibor;
break;
}
case rightNeibor:
{
right = pNeibor;
break;
}
default:
break;
}
}
}
void CurtainGrain::RemoveNeighbor( NeighborType type )
{
switch (type)
{
case upNeibor:
{
up = BreakLink;
break;
}
case downNeibor:
{
down = BreakLink;
break;
}
case leftNeibor:
{
left = BreakLink;
break;
}
case rightNeibor:
{
right = BreakLink;
break;
}
default:
break;
}
}
void CurtainGrain::ClearOutsidePower()
{
prevF = f;
f.px = f.pz = 0.0f;
f.py = m*Gravity_g;
}
void CurtainGrain::SetOutsidePower( const Power out_f )
{
f.Add(out_f);
Power selfPower,otherPower;
Pos tmpPos;
if (up)
{
if(!up->isVisited)
{
up->GetPos( tmpPos );
CalcSelfOtherPower( selfPower, otherPower, tmpPos );
float p = selfPower.px + selfPower.py + selfPower.pz;
if (p > linkPower)
{
up->down = BreakLink;
up = BreakLink;
}
else
{
f.Add( selfPower );
up->f.Add( otherPower );
}
}
}
if (down)
{
if(!down->isVisited)
{
down->GetPos( tmpPos );
CalcSelfOtherPower( selfPower, otherPower, tmpPos );
float p = selfPower.px + selfPower.py + selfPower.pz;
if (p > linkPower)
{
down->up = BreakLink;
down = BreakLink;
}
else
{
f.Add( selfPower );
down->f.Add( otherPower );
}
}
}
if (left)
{
if(!left->isVisited)
{
left->GetPos( tmpPos );
CalcSelfOtherPower( selfPower, otherPower, tmpPos );
float p = selfPower.px + selfPower.py + selfPower.pz;
if (p > linkPower)
{
left->right = BreakLink;
left = BreakLink;
}
else
{
f.Add( selfPower );
left->f.Add( otherPower );
}
}
}
if (right)
{
if(!right->isVisited)
{
right->GetPos( tmpPos );
CalcSelfOtherPower( selfPower, otherPower, tmpPos );
float p = selfPower.px + selfPower.py + selfPower.pz;
if (p > linkPower)
{
right->left = BreakLink;
right = BreakLink;
}
else
{
f.Add( selfPower );
right->f.Add( otherPower );
}
}
}
// Add the wind power
Power wind;
wind.px = -wind_fac*(pos.x - prevPos.x);
wind.py = -wind_fac*(pos.y - prevPos.y);
wind.pz = -wind_fac*(pos.z - prevPos.z);
f.Add(wind);
isVisited = true;
}
void CurtainGrain::UpdatePosAfterPower( const float deltaT )
{
if(isFixed) return ;
float deltaTInS = deltaT*0.001f;
Pos temp = pos;
Velocity tmpVt = vt;
Power tmpF = f;
tmpF.Add(prevF);
//vt.vx = vt.vx + 0.5*tmpF.px/m*deltaTInS;
//vt.vy = vt.vy + 0.5*tmpF.py/m*deltaTInS;
//vt.vz = vt.vz + 0.5*tmpF.pz/m*deltaTInS;
//pos.x = pos.x + (redef_abs(vt.vx*deltaTInS)<0.05?vt.vx*deltaTInS:(vt.vx<0?-0.05:0.05));
//pos.y = pos.y + (redef_abs(vt.vy*deltaTInS)<0.05?vt.vy*deltaTInS:(vt.vy<0?-0.05:0.05));
//pos.z = pos.z + (redef_abs(vt.vz*deltaTInS)<0.05?vt.vz*deltaTInS:(vt.vz<0?-0.05:0.05));
pos.x = 2*pos.x - prevPos.x + f.px/m*deltaTInS*deltaTInS;
pos.y = 2*pos.y - prevPos.y + f.py/m*deltaTInS*deltaTInS;
pos.z = 2*pos.z - prevPos.z + f.pz/m*deltaTInS*deltaTInS;
// the grain must be above the ground
pos.y = MIN(ground,pos.y);
prevVt = tmpVt;
prevPos = temp;
}
void CurtainGrain::CalcSelfOtherPower(
Power &selfPower, Power &otherPower,
const Pos otherPos )
{
float disx = otherPos.x - pos.x;
float disy = otherPos.y - pos.y;
float disz = otherPos.z - pos.z;
float dis = sqrt(disx*disx + disy*disy + disz*disz);
float strength = iCalcSelfPower( dis );
float selfPx = disx/dis*strength;
float selfPy = disy/dis*strength;
float selfPz = disz/dis*strength;
selfPower.px = selfPx;
selfPower.py = selfPy;
selfPower.pz = selfPz;
otherPower.px = -selfPx;
otherPower.py = -selfPy;
otherPower.pz = -selfPz;
}
float CurtainGrain::iCalcSelfPower( const float dis )
{
if( dis > 0 )
{
float tmp = redef_abs(dis)-s;
if(tmp > 0)
{
return k*tmp*tmp; // positive means "pull" power
}
else
{
return -l*tmp*tmp; // negative means "push" power
}
}
else
{
return 0.0f;
}
}
Curtain:
Curtain.h
#ifndef _CURTAIN_H_
#define _CURTAIN_H_
#include "stdafx.h"
#include "CurtainGrain.h"
#include "cv.h"
#include "highgui.h"
#include <string.h>
class Curtain
{
public:
Curtain(int _w, int _h);
~Curtain();
void InitAllPos(const float scale);
void ReceivePower( const Pos _pos, const Power _f );
void ReceivePower( const CvPoint _pt, const Power _f );
void ShowThePosImage( const std::string winName );
float fistRange; // the range affected by the fist measured in m
private:
// These are the inner properties
int w, h; // the curtain has w*h grains
CurtainGrain* pFirst; // the first curtain grain in the curtain
// These are the operate params
float deltaT; // the time interval for update, it's measured in ms
IplImage* backImg; // a white image to plaint on
float meterPerPixel; // how long(in m) one pixel distance denotes
CurtainGrain* GetGrainById(int x, int y);
CvPoint MapPosToPoint(const Pos _pos);
Pos MapPointToPos(const CvPoint _pt);
double t;
};
#endif
Curtain.cpp
#include "stdafx.h"
#include "Curtain.h"
Curtain
::Curtain(int _w, int _h)
{
w = _w;
h = _h;
deltaT = 4.0f;
fistRange = 0.2f;
meterPerPixel = 5.0f/7.0f*0.01f;
pFirst = new CurtainGrain[w*h];
for (int i = 0; i < w*h; i++)
{
int x = i%w, y = i/w;
CurtainGrain* curGrain = pFirst+i;
memcpy(curGrain, &CurtainGrain(x,y), sizeof(CurtainGrain));
if(y==0) curGrain->SetFixed();
//set position
curGrain->SetPos();
//set ground
float temp = MapPointToPos(cvPoint(0,610)).y;
curGrain->SetGround(temp);
//setup the link
if(x!=0)
curGrain->SetNeighbor(GetGrainById(x-1,y), leftNeibor);
if(x!=w-1)
curGrain->SetNeighbor(GetGrainById(x+1,y), rightNeibor);
if(y!=0)
curGrain->SetNeighbor(GetGrainById(x,y-1), upNeibor);
if(y!=h-1)
curGrain->SetNeighbor(GetGrainById(x,y+1), downNeibor);
}
backImg = cvCreateImage(cvSize(800,640),IPL_DEPTH_8U, 3);
cvSet(backImg,cvScalarAll(255));
cvLine(backImg,cvPoint(5,610),cvPoint(795,610),CV_RGB(0,0,0),2);
}
Curtain
::~Curtain()
{
delete[] pFirst;
}
CurtainGrain* Curtain::GetGrainById(int x, int y)
{
return (CurtainGrain*)(pFirst + y*w + x);
}
CvPoint Curtain::MapPosToPoint(const Pos _pos)
{
return cvPoint(cvRound(_pos.x/meterPerPixel)+136,
cvRound(_pos.y/meterPerPixel)+41);
}
Pos Curtain::MapPointToPos(const CvPoint _pt)
{
Pos temp;
temp.z = 0.0f;
temp.x = (_pt.x-136)*meterPerPixel;
temp.y = (_pt.y-41)*meterPerPixel;
return temp;
}
void Curtain::InitAllPos(const float scale)
{
for (int x = 0; x < w; x ++)
{
for (int y = 0; y < h; y++)
{
CurtainGrain* curGrain = GetGrainById(x,y);
curGrain->SetPos(scale);
}
}
}
void Curtain::ReceivePower( const Pos _pos, const Power _f )
{
t = (double)cvGetTickCount();
int id = 0;
while (id<w*h)
{
(pFirst+id)->ClearOutsidePower();
id ++;
}
Pos tmpPos;
Power tmpPower;
for( int y = 0; y < h; y ++ )
{
for (int x = 0; x < w; x++)
{
CurtainGrain* curGrain = GetGrainById(x,y);
curGrain->GetPos(tmpPos);
if (abs(tmpPos.x-_pos.x) < fistRange &&
abs(tmpPos.y-_pos.y) < fistRange)
{
float diff = sqrt((tmpPos.x-_pos.x)*(tmpPos.x-_pos.x) +
(tmpPos.y-_pos.y)*(tmpPos.y-_pos.y));
tmpPower.px = _f.px*exp(-diff);
tmpPower.py = _f.py*exp(-diff);
tmpPower.pz = _f.pz*exp(-diff);
curGrain->SetOutsidePower( tmpPower );
curGrain->UpdatePosAfterPower(deltaT);
}
else
{
tmpPower.px = tmpPower.py = tmpPower.pz = 0.0f;
curGrain->SetOutsidePower( tmpPower );
curGrain->UpdatePosAfterPower(deltaT);
}
}
}
}
void Curtain::ReceivePower( const CvPoint _pt, const Power _f )
{
Pos temp = MapPointToPos(_pt);
ReceivePower(temp,_f);
}
void Curtain::ShowThePosImage( const std::string winName )
{
IplImage* showImg = cvCloneImage(backImg);
Pos tmpPos;
for (int y = 0; y < h; y++)
{
for (int x = 0; x < w; x++)
{
CurtainGrain* curGrain = GetGrainById(x,y);
curGrain->GetPos(tmpPos);
CvPoint curPt = MapPosToPoint(tmpPos);
if(curGrain->isVisited)
{
if(curGrain->NeiborExist(upNeibor) &&
curGrain->GetNeiborVisitStatus(upNeibor))
{
curGrain->GetUpPos(tmpPos);
cvLine(showImg,curPt,MapPosToPoint(tmpPos),cvScalarAll(0));
}
if(curGrain->NeiborExist(downNeibor) &&
curGrain->GetNeiborVisitStatus(downNeibor))
{
curGrain->GetDownPos(tmpPos);
cvLine(showImg,curPt,MapPosToPoint(tmpPos),cvScalarAll(0));
}
if(curGrain->NeiborExist(leftNeibor) &&
curGrain->GetNeiborVisitStatus(leftNeibor))
{
curGrain->GetLeftPos(tmpPos);
cvLine(showImg,curPt,MapPosToPoint(tmpPos),cvScalarAll(0));
}
if(curGrain->NeiborExist(rightNeibor) &&
curGrain->GetNeiborVisitStatus(rightNeibor))
{
curGrain->GetRightPos(tmpPos);
cvLine(showImg,curPt,MapPosToPoint(tmpPos),cvScalarAll(0));
}
curGrain->isVisited = false;
}
}
}
t = ((double)cvGetTickCount() - t)/(1000.0*cvGetTickFrequency());
cvShowImage( winName.c_str(), showImg );
cvWaitKey(/*MAX(cvRound(deltaT-t),*/1);
cvReleaseImage( &showImg );
}
Main.cpp
#include "stdafx.h"
#include "Curtain.h"
#include "cv.h"
#include "highgui.h"
#define EPS_FLOAT 0.000000001f
CvPoint g_cur,g_prev;
Power g_power;
float g_powerBase = 0.001f;
float g_powerScale = 0.0004f;
void OnMouse(int Event,int x,int y,int flags,void* param)
{
switch (Event)
{
case CV_EVENT_LBUTTONDOWN:
{
g_power.pz = -g_powerBase;
}
break;
case CV_EVENT_MOUSEMOVE:
{
if (flags&CV_EVENT_FLAG_LBUTTON)
{
g_power.px = (abs(x-g_prev.x)*g_powerScale + g_powerBase)*
(x>g_prev.x?1.0f:-1.0f);
g_power.py = (abs(y-g_prev.y)*g_powerScale + g_powerBase)*
(y>g_prev.y?1.0f:-1.0f);
g_power.pz = -g_powerBase;
}
else
{
g_power.px = g_power.py = g_power.pz = 0.0f;
}
}
break;
default:
g_power.px = g_power.py = g_power.pz = 0.0f;
}
g_prev.x = g_cur.x = x;
g_prev.y = g_cur.y = y;
}
int _tmain(int argc, _TCHAR* argv[])
{
g_power.px = g_power.py = g_power.pz = 0.0f;
g_prev.x = g_prev.y = g_cur.x = g_cur.y = 0;
std::string winName = "display";
cvNamedWindow(winName.c_str());
cvSetMouseCallback(winName.c_str(),OnMouse,NULL);
Curtain curtain(70,30);
curtain.InitAllPos(1.0);
int tdk = 0;
while (true)
{
//printf("tdk=%d\n",tdk++);
curtain.ReceivePower(g_cur,g_power);
curtain.ShowThePosImage("display");
//char c = cvWaitKey(1);
//if(c==27) break;
}
return 0;
}