private void DrawShape(tShape shape, Point point1, Point point2, Pen pen) { using (var g = this.CreateGraphics()) { this.GetDrawAction(shape, g)(pen, Math.Min(point1.X, point2.X), Math.Min(point1.Y, point2.Y), Math.Abs(point1.X - point2.X), Math.Abs(point1.Y - point2.Y)); } }
public static IShape MakeShape(string shapeType) { IShape shape = null; switch (shapeType.ToLower()) { case "i": shape = new iShape(); break; case "j": shape = new jShape(); break; case "l": shape = new lShape(); break; case "o": shape = new oShape(); break; case "s": shape = new sShape(); break; case "t": shape = new tShape(); break; case "z": shape = new zShape(); break; case "test": shape = new testShape(); break; default: throw new ArgumentException("Invalid shape name"); } return(shape); }
private void EraseShape(tShape shape, Point point1, Point point2) { this.DrawShape(shape, point1, point2, Pens.White); }
private void DrawShape(tShape shape, Point point1, Point point2) { this.DrawShape(shape, point1, point2, Pens.MidnightBlue); }
private Action<Pen, float, float, float, float> GetDrawAction(tShape shape, Graphics g) { switch (shape) { case tShape.Rect: return g.DrawRectangle; case tShape.Ellipse: return g.DrawEllipse; default: throw new Exception(""); } }
// public static bool Penetration( btConvexShape shape0, ref btTransform wtrs0, btConvexShape shape1, ref btTransform wtrs1, ref btVector3 guess, out sResults results, bool usemargins = false ) { tShape shape = new tShape(); Initialize( shape0, ref wtrs0, shape1, ref wtrs1, out results, shape, usemargins ); GJK gjk = new GJK(); btVector3 tmp; guess.Invert( out tmp ); GJK.eStatus._ gjk_status = gjk.Evaluate( shape, ref tmp ); switch( gjk_status ) { case GJK.eStatus._.Inside: { EPA epa = new EPA(); EPA.eStatus._ epa_status = epa.Evaluate( gjk, ref tmp ); if( epa_status != EPA.eStatus._.Failed ) { btVector3 w0 = btVector3.Zero; for( uint i = 0; i < epa.m_result.rank; ++i ) { shape.Support( ref epa.m_result.c[i].d, 0, out tmp ); w0.AddScale( ref tmp, epa.m_result.p[i], out w0 ); } results.status = sResults.eStatus.Penetrating; wtrs0.Apply( ref w0, out results.witness0 ); w0.SubScale( ref epa.m_normal, epa.m_depth, out tmp ); wtrs0.Apply( ref tmp, out results.witness1 ); epa.m_normal.Invert( out results.normal ); results.distance = -epa.m_depth; return ( true ); } else results.status = sResults.eStatus.EPA_Failed; } break; case GJK.eStatus._.Failed: results.status = sResults.eStatus.GJK_Failed; break; default: break; } return ( false ); }
// // Api // // // public static bool Distance( btConvexShape shape0, ref btTransform wtrs0, btConvexShape shape1, ref btTransform wtrs1, ref btVector3 guess, out sResults results ) { tShape shape = new tShape(); results.witness0 = results.witness1 = btVector3.Zero; results.status = btGjkEpaSolver2.sResults.eStatus.Separated; Initialize( shape0, ref wtrs0, shape1, ref wtrs1, out results, shape, false ); GJK gjk = new GJK(); GJK.eStatus._ gjk_status = gjk.Evaluate( shape, ref guess ); if( gjk_status == GJK.eStatus._.Valid ) { btVector3 w0 = btVector3.Zero; btVector3 w1 = btVector3.Zero; for( uint i = 0; i < gjk.m_simplex.rank; ++i ) { double p = gjk.m_simplex.p[i]; btVector3 tmp; shape.Support( ref gjk.m_simplex.c[i].d, 0, out tmp ); w0.AddScale( ref tmp, p, out w0 ); gjk.m_simplex.c[i].d.Invert( out tmp ); shape.Support( ref tmp, 1, out tmp ); w1.AddScale( ref tmp, p, out w1 ); } wtrs0.Apply( ref w0, out results.witness0 ); wtrs0.Apply( ref w1, out results.witness1 ); w0.Sub( ref w1, out results.normal ); results.distance = results.normal.length(); results.normal.Div( ( results.distance > GJK_MIN_DISTANCE ) ? results.distance : 1, out results.normal ); return ( true ); } else { results.status = gjk_status == GJK.eStatus._.Inside ? sResults.eStatus.Penetrating : sResults.eStatus.GJK_Failed; return ( false ); } }
// static void Initialize( btConvexShape shape0, ref btTransform wtrs0, btConvexShape shape1, ref btTransform wtrs1, out btGjkEpaSolver2.sResults results, tShape shape, bool withmargins ) { /* Results */ results.normal = btVector3.xAxis; results.witness0 = results.witness1 = btVector3.Zero; results.status = btGjkEpaSolver2.sResults.eStatus.Separated; results.distance = 0; /* Shape */ shape.m_shape0 = shape0; shape.m_shape1 = shape1; wtrs1.m_basis.transposeTimes( ref wtrs0.m_basis, out shape.m_toshape1 ); wtrs0.inverseTimes( ref wtrs1, out shape.m_toshape0 ); shape.EnableMargin( withmargins ); }
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 ); }
// public static double SignedDistance( ref btVector3 position, double margin, btConvexShape shape0, ref btTransform wtrs0, sResults results ) { tShape shape = new tShape(); using( btSphereShape shape1 = BulletGlobals.SphereShapePool.Get() ) { shape1.Initialize( margin ); btTransform wtrs1 = new btTransform( ref btQuaternion.Zero, ref position ); Initialize( shape0, ref wtrs0, shape1, ref wtrs1, out results, shape, false ); GJK gjk = new GJK(); GJK.eStatus._ gjk_status = gjk.Evaluate( shape, ref btVector3.One ); if( gjk_status == GJK.eStatus._.Valid ) { btVector3 w0 = btVector3.Zero; btVector3 w1 = btVector3.Zero; for( uint i = 0; i < gjk.m_simplex.rank; ++i ) { double p = gjk.m_simplex.p[i]; btVector3 tmp; shape.Support( ref gjk.m_simplex.c[i].d, 0, out tmp ); w0.AddScale( ref tmp, p, out w0 ); btVector3 tmp2; gjk.m_simplex.c[i].d.Invert( out tmp2 ); shape.Support( ref tmp2, 1, out tmp ); w1.AddScale( ref tmp, p, out w1 ); } wtrs0.Apply( ref w0, out results.witness0 ); wtrs0.Apply( ref w1, out results.witness1 ); btVector3 delta; results.witness1.Sub( ref results.witness0, out delta ); margin = shape0.getMarginNonVirtual() + shape1.getMarginNonVirtual(); double length = delta.length(); delta.Div( length, out results.normal ); results.witness0.AddScale( ref results.normal, margin, out results.witness0 ); return ( length - margin ); } else { if( gjk_status == GJK.eStatus._.Inside ) { if( Penetration( shape0, ref wtrs0, shape1, ref wtrs1, ref gjk.m_ray, out results ) ) { btVector3 delta; results.witness0.Sub( ref results.witness1, out delta ); double length = delta.length(); if( length >= btScalar.SIMD_EPSILON ) delta.Div( length, out results.normal ); return ( -length ); } } } } return ( btScalar.SIMD_INFINITY ); }