基于韦尔莱算法的可撕扯的幕布

题记：写这个程序是因为偶然看到这个链接里的幕布模拟效果，虽然作者公布了代码，但是笔者没有参考，根据标题<基于韦尔莱算法>自己写了下面这个程序，权当练习。

    正如题记所言，该程序的意义就是模拟一个可以撕扯的幕布，在显示上使用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

CurtainGrain.cpp

#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;
}