示例#1
0
文件: EPA.cs 项目: HaKDMoDz/InVision
		public eStatus Evaluate(GJK gjk,ref Vector3 guess)
		{
			sSimplex simplex=gjk.m_simplex;
			if((simplex.rank>1)&&gjk.EncloseOrigin())
			{
				/* Clean up				*/ 
				while(m_hull.Count > 0)
				{
					sFace	f = m_hull[0];
					Remove(m_hull,f);
					Append(m_stock,f);
				}

				m_status = eStatus.Valid;
				m_nextsv = 0;
				/* Orient simplex		*/ 
				if(GJK.Det(	simplex.c[0].w-simplex.c[3].w,
				           	simplex.c[1].w-simplex.c[3].w,
				           	simplex.c[2].w-simplex.c[3].w)<0)
				{
					SwapSv(simplex.c,0,1);
					SwapFloat(simplex.p,0,1);
				}
				/* Build initial hull	*/ 
				sFace[]	tetra ={NewFace(simplex.c[0],simplex.c[1],simplex.c[2],true),
				       	        NewFace(simplex.c[1],simplex.c[0],simplex.c[3],true),
				       	        NewFace(simplex.c[2],simplex.c[1],simplex.c[3],true),
				       	        NewFace(simplex.c[0],simplex.c[2],simplex.c[3],true)};
				if(m_hull.Count==4)
				{
					sFace best=FindBest();
					sFace outer = best;
					uint pass=0;
					uint iterations=0;
					Bind(tetra[0],0,tetra[1],0);
					Bind(tetra[0],1,tetra[2],0);
					Bind(tetra[0],2,tetra[3],0);
					Bind(tetra[1],1,tetra[3],2);
					Bind(tetra[1],2,tetra[2],1);
					Bind(tetra[2],2,tetra[3],1);
					m_status=eStatus.Valid;
					for (; iterations < GjkEpaSolver2.EPA_MAX_ITERATIONS; ++iterations)
					{
						if (m_nextsv < GjkEpaSolver2.EPA_MAX_VERTICES)
						{
							sHorizon horizon = new sHorizon() ;
							sSV	w = m_sv_store[m_nextsv++];
							bool valid = true;					
							best.pass =	(uint)(++pass);
							gjk.GetSupport(ref best.n,ref w);
							float wdist=Vector3.Dot(best.n,w.w)-best.d;
							if (wdist > GjkEpaSolver2.EPA_ACCURACY)
							{
								for(int j=0;(j<3)&&valid;++j)
								{
									valid&=Expand(	pass,w,
									              	best.f[j],best.e[j],
									              	horizon);
								}
								if(valid&&(horizon.nf>=3))
								{
									Bind(horizon.cf,1,horizon.ff,2);
									Remove(m_hull,best);
									Append(m_stock,best);
									best=FindBest();
									if (best.p >= outer.p)
									{
										outer = best;
									}
								} 
								else 
								{ 
									m_status=eStatus.InvalidHull;
									break; 
								}
							} 
							else 
							{ 
								m_status=eStatus.AccuraryReached;
								break; 
							}
						} 
						else 
						{ 
							m_status=eStatus.OutOfVertices;
							break; 
						}
					}
					Vector3	projection=outer.n*outer.d;
					m_normal	=	outer.n;
					m_depth		=	outer.d;
					m_result.rank	=	3;
					m_result.c[0]	=	outer.c[0];
					m_result.c[1]	=	outer.c[1];
					m_result.c[2]	=	outer.c[2];
					m_result.p[0]	=	Vector3.Cross(	outer.c[1].w-projection,
					             	 	              	outer.c[2].w-projection).Length();
					m_result.p[1] = Vector3.Cross(outer.c[2].w - projection,
					                              outer.c[0].w-projection).Length();
					m_result.p[2] = Vector3.Cross(outer.c[0].w - projection,
					                              outer.c[1].w-projection).Length();
					float sum=m_result.p[0]+m_result.p[1]+m_result.p[2];
					m_result.p[0]	/=	sum;
					m_result.p[1]	/=	sum;
					m_result.p[2]	/=	sum;
					return(m_status);
				}
			}
			/* Fallback		*/ 
			m_status	=	eStatus.FallBack;
			m_normal	=	-guess;
			float nl=m_normal.LengthSquared();
			if(nl>0)
			{
				m_normal.Normalize();
			}
			else
			{
				m_normal = new Vector3(1,0,0);
			}

			m_depth	=	0;
			m_result.rank=1;
			m_result.c[0]=simplex.c[0];
			m_result.p[0]=1;	
			return(m_status);
		}
示例#2
0
文件: EPA.cs 项目: HaKDMoDz/InVision
		public bool	Expand(uint pass,sSV w,sFace f,uint e,sHorizon horizon)
		{
			uint[]	i1m3 = {1,2,0};
			uint[]  i2m3 = {2,0,1};
			if(f.pass!=pass)
			{
				uint e1 = i1m3[e];
				if ((Vector3.Dot(f.n, w.w) - f.d) < -GjkEpaSolver2.EPA_PLANE_EPS)
				{
					sFace nf = NewFace(f.c[e1],f.c[e],w,false);
					if(nf != null)
					{
						Bind(nf,0,f,e);
						if(horizon.cf != null)
						{
							Bind(horizon.cf,1,nf,2);
						}
						else 
						{
							horizon.ff=nf;
						}
						horizon.cf=nf;
						++horizon.nf;
						return(true);
					}
				}
				else
				{
					uint e2=i2m3[e];
					f.pass = (uint)pass;
					if(	Expand(pass,w,f.f[e1],f.e[e1],horizon)&&
					   	Expand(pass,w,f.f[e2],f.e[e2],horizon))
					{
						Remove(m_hull,f);
						Append(m_stock,f);
						return(true);
					}
				}
			}
			return(false);
		}