コード例 #1
0
ファイル: GJK.cs プロジェクト: himapo/ccm
 public eStatus Evaluate(tShape shapearg, btVector3 guess)
 {
     U iterations = 0;
     float sqdist = 0;
     float alpha = 0;
     //btVector3	*lastw=stackalloc btVector3[4];
     //btVector3[] lastw = new btVector3[4];
     StackPtr<btVector3> lastw = StackPtr<btVector3>.Allocate(4);
     try
     {
         U clastw = 0;
         /* Initialize solver		*/
         m_free[0] = m_store[0];
         m_free[1] = m_store[1];
         m_free[2] = m_store[2];
         m_free[3] = m_store[3];
         m_nfree = 4;
         m_current = 0;
         m_status = eStatus.Valid;
         m_shape = shapearg;
         m_distance = 0;
         /* Initialize simplex		*/
         m_simplices[0].rank = 0;
         m_ray = guess;
         float sqrl = m_ray.Length2;
         appendvertice(m_simplices[0], sqrl > 0 ? -m_ray : new btVector3(1, 0, 0));
         m_simplices[0].p[0] = 1;
         m_ray = m_simplices[0].c[0].w;
         sqdist = sqrl;
         lastw[0] =
             lastw[1] =
             lastw[2] =
             lastw[3] = m_ray;
         /* Loop						*/
         do
         {
             U next = 1 - m_current;
             sSimplex cs = m_simplices[m_current];
             sSimplex ns = m_simplices[next];
             /* Check zero							*/
             float rl = m_ray.Length;
             if (rl < GJK_MIN_DISTANCE)
             {/* Touching or inside				*/
                 m_status = eStatus.Inside;
                 break;
             }
             /* Append new vertice in -'v' direction	*/
             appendvertice(cs, -m_ray);
             btVector3 w = cs.c[cs.rank - 1].w;
             bool found = false;
             for (U i = 0; i < 4; ++i)
             {
                 if ((w - lastw[i]).Length2 < GJK_DUPLICATED_EPS)
                 { found = true; break; }
             }
             if (found)
             {/* Return old simplex				*/
                 removevertice(m_simplices[m_current]);
                 break;
             }
             else
             {/* Update lastw					*/
                 lastw[clastw = (clastw + 1) & 3] = w;
             }
             /* Check for termination				*/
             float omega = btVector3.dot(m_ray, w) / rl;
             alpha = (float)Math.Max(omega, alpha);
             if (((rl - alpha) - (GJK_ACCURARY * rl)) <= 0)
             {/* Return old simplex				*/
                 removevertice(m_simplices[m_current]);
                 break;
             }
             /* Reduce simplex						*/
             //float* weights = stackalloc float[4];
             StackPtr<float> weights = StackPtr<float>.Allocate(4);
             try
             {
                 U mask = 0;
                 switch (cs.rank)
                 {
                     case 2: sqdist = projectorigin(cs.c[0].w,
                                 cs.c[1].w,
                                 weights, ref mask); break;
                     case 3: sqdist = projectorigin(cs.c[0].w,
                                 cs.c[1].w,
                                 cs.c[2].w,
                                 weights, ref mask); break;
                     case 4: sqdist = projectorigin(cs.c[0].w,
                                 cs.c[1].w,
                                 cs.c[2].w,
                                 cs.c[3].w,
                                 weights, ref mask); break;
                 }
                 if (sqdist >= 0)
                 {/* Valid	*/
                     ns.rank = 0;
                     m_ray = new btVector3(0, 0, 0);
                     m_current = next;
                     for (U i = 0, ni = cs.rank; i < ni; ++i)
                     {
                         if ((mask & (1 << (int)i)) != 0)
                         {
                             ns.c[ns.rank] = cs.c[i];
                             ns.p[ns.rank++] = weights[i];
                             #region m_ray += cs.c[i].w * weights[i];
                             {
                                 btVector3 temp;
                                 btVector3.Multiply(ref cs.c[i].w, weights[i], out temp);
                                 m_ray.Add(ref temp);
                             }
                             #endregion
                         }
                         else
                         {
                             m_free[m_nfree++] = cs.c[i];
                         }
                     }
                     if (mask == 15) m_status = eStatus.Inside;
                 }
                 else
                 {/* Return old simplex				*/
                     removevertice(m_simplices[m_current]);
                     break;
                 }
                 m_status = ((++iterations) < GJK_MAX_ITERATIONS) ? m_status : eStatus.Failed;
             }
             finally
             {
                 weights.Dispose();
             }
         } while (m_status == eStatus.Valid);
         m_simplex = m_simplices[m_current];
         switch (m_status)
         {
             case eStatus.Valid: m_distance = m_ray.Length; break;
             case eStatus.Inside: m_distance = 0; break;
             default:
                 {
                     break;
                 }
         }
         return (m_status);
     }
     finally
     {
         lastw.Dispose();
     }
 }
コード例 #2
0
ファイル: GJK.cs プロジェクト: himapo/ccm
 void removevertice(sSimplex simplex)
 {
     m_free[m_nfree++] = simplex.c[--simplex.rank];
 }
コード例 #3
0
ファイル: GJK.cs プロジェクト: himapo/ccm
 void appendvertice(sSimplex simplex, btVector3 v)
 {
     simplex.p[simplex.rank] = 0;
     simplex.c[simplex.rank] = m_free[--m_nfree];
     getsupport(v, simplex.c[simplex.rank++]);
 }
コード例 #4
0
ファイル: GjkEpa2.h.cs プロジェクト: d3x0r/Voxelarium
			internal eStatus._ Evaluate( tShape shapearg, ref btVector3 guess )
			{
				uint iterations = 0;
				double sqdist = 0;
				double alpha = 0;
				btVector3[] lastw = new btVector3[4];
				uint clastw = 0;
				/* Initialize solver		*/
				m_free[0] = new sSV();
				m_free[1] = new sSV();
				m_free[2] = new sSV();
				m_free[3] = new sSV();
				m_nfree = 4;
				m_current = 0;
				m_status = eStatus._.Valid;
				m_shape = shapearg;
				m_distance = 0;
				/* Initialize simplex		*/
				m_simplices0.rank = 0;
				m_ray = guess;
				double sqrl = m_ray.length2();
				btVector3 tmp;
				if( sqrl > 0 )
					m_ray.Invert( out tmp );
				else
					tmp = btVector3.xAxis;
				appendvertice( m_simplices0, ref tmp );
				m_simplices0.p[0] = 1;
				m_ray = m_simplices0.c[0].w;
				sqdist = sqrl;
				lastw[0] =
					lastw[1] =
					lastw[2] =
					lastw[3] = m_ray;
				/* Loop						*/
				do
				{
					uint next = 1 - m_current;
					sSimplex cs = m_current==0?m_simplices0:m_simplices1;
					sSimplex ns = next==0?m_simplices0:m_simplices1;
					/* Check zero							*/
					double rl = m_ray.length();
					if( rl < GJK_MIN_DISTANCE )
					{/* Touching or inside				*/
						m_status = eStatus._.Inside;
						break;
					}
					/* Append new vertice in -'v' direction	*/
					m_ray.Invert( out tmp );
					appendvertice( cs, ref tmp );
					btVector3 w = cs.c[cs.rank - 1].w;
					bool found = false;
					for( uint i = 0; i < 4; ++i )
					{
						w.Sub( ref lastw[i], out tmp );
						if( tmp.length2() < GJK_DUPLICATED_EPS )
						{ found = true; break; }
					}
					if( found )
					{/* Return old simplex				*/
						removevertice( cs );
						break;
					}
					else
					{/* Update lastw					*/
						lastw[clastw = ( clastw + 1 ) & 3] = w;
					}
					/* Check for termination				*/
					double omega = btVector3.btDot( ref m_ray, ref w ) / rl;
					alpha = btScalar.btMax( omega, alpha );
					if( ( ( rl - alpha ) - ( GJK_ACCURARY * rl ) ) <= 0 )
					{/* Return old simplex				*/
						removevertice( cs );
						break;
					}
					/* Reduce simplex						*/
					double[] weights = new double[4];
					uint mask = 0;
					switch( cs.rank )
					{
						case 2:
							sqdist = projectorigin( ref cs.c[0].w,
								ref cs.c[1].w,
								weights, out mask ); break;
						case 3:
							sqdist = projectorigin( ref cs.c[0].w,
								ref cs.c[1].w,
								ref cs.c[2].w,
								weights, out mask ); break;
						case 4:
							sqdist = projectorigin( ref cs.c[0].w,
								ref cs.c[1].w,
								ref cs.c[2].w,
								ref cs.c[3].w,
								weights, out mask ); break;
					}
					if( sqdist >= 0 )
					{/* Valid	*/
						ns.rank = 0;
						m_ray = btVector3.Zero;
						m_current = next;
						for( int i = 0, ni = (int)cs.rank; i < ni; ++i )
						{
							if( ( mask & ( (uint)1 << i ) ) != 0 )
							{
								ns.c[ns.rank] = cs.c[i];
								ns.p[ns.rank++] = weights[i];
								btVector3 tmp2;
								cs.c[i].w.Mult( weights[i], out tmp2 );
								m_ray.Add( ref tmp2, out m_ray );
							}
							else
							{
								m_free[m_nfree++] = cs.c[i];
							}
						}
						if( mask == 15 ) m_status = eStatus._.Inside;
					}
					else
					{/* Return old simplex				*/
						removevertice( cs );
						break;
					}
					m_status = ( ( ++iterations ) < GJK_MAX_ITERATIONS ) ? m_status : eStatus._.Failed;
				} while( m_status == eStatus._.Valid );
				m_simplex = m_current==0?m_simplices0:m_simplices1;
				switch( m_status )
				{
					case eStatus._.Valid: m_distance = m_ray.length(); break;
					case eStatus._.Inside: m_distance = 0; break;
					default:
						break;
				}
				return ( m_status );
			}