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

我会带着你远行 2022-08-13 05:29 109阅读 0赞

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

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

[Link 1]: http://top.jobbole.com/1950/