/// <summary> /// Calculates the rotational inertia for a polygon with a given mass /// Passed in polygon does not need to be in local space /// </summary> /// <param name="polygon"></param> /// <param name="mass"></param> /// <returns></returns> public static float GetInertiaForPolygon(gxtPolygon polygon, float mass) { gxtDebug.Assert(polygon.NumVertices >= 3); // perform a deep copy so we don't corrupt the values of the polygon Vector2[] vertices = new Vector2[polygon.NumVertices]; for (int i = 0; i < vertices.Length; ++i) { vertices[i] = polygon.v[i]; } Vector2 centroid = polygon.GetCentroid(); if (centroid != Vector2.Zero) { for (int i = 0; i < vertices.Length; ++i) { vertices[i] -= centroid; } //polygon.Translate(-centroid); } float denom = 0.0f; float numer = 0.0f; for (int j = vertices.Length - 1, i = 0; i < vertices.Length; j = i, i++) { float a = vertices[i].LengthSquared(); float b = Vector2.Dot(vertices[i], vertices[j]); float c = vertices[j].LengthSquared(); float d = gxtMath.Abs(gxtMath.Cross2D(vertices[j], vertices[i])); numer += d; denom += (a + b + c) * d; } return denom / (numer * 6.0f); }
public void CalculateUVCoords(gxtPolygon polygon) { Vector2 topLeft = new Vector2(-brickTexture.Width * 0.5f, -brickTexture.Height * 0.5f); Vector2 oneOverSizeVector = new Vector2(1.0f / brickTexture.Width, 1.0f / brickTexture.Height); for (int i = 0; i < vertices.Length; i++) { vertices[i].TextureCoordinate = Vector2.Multiply(polygon.v[i] - topLeft, oneOverSizeVector); vertices[i].TextureCoordinate = new Vector2(gxtMath.Clamp(vertices[i].TextureCoordinate.X, 0.0f, 1.0f), gxtMath.Clamp(vertices[i].TextureCoordinate.Y, 0.0f, 1.0f)); } vertexBuffer.SetData<VertexPositionColorTexture>(vertices); }
/// <summary> /// An efficient contains point function whoich uses a binary search /// and assumes the input of a counter clockwise polygon /// </summary> /// <param name="polygon">Polygon</param> /// <param name="pt">Pt</param> /// <returns>If Inside</returns> public virtual bool Contains(ref gxtPolygon polygon, Vector2 pt) { int low = 0, high = polygon.v.Length; do { int mid = (low + high) / 2; if (gxtMath.IsCCWTriangle(polygon.v[0], polygon.v[mid], pt)) low = mid; else high = mid; } while (low + 1 < high); if (low == 0 || high == polygon.v.Length) return false; return gxtMath.IsCCWTriangle(polygon.v[low], polygon.v[high], pt); }
/// <summary> /// Finds the farthest point in a given direction /// Search direction does not need to be normalized /// </summary> /// <param name="polygon"></param> /// <param name="nd"></param> /// <returns></returns> public Vector2 FarthestPointInDirection(ref gxtPolygon polygon, Vector2 nd) { int farthestIndex = 0; float farthestDistance = Vector2.Dot(polygon.v[0], nd); float tmpDistance; // finds largest scalar porjection onto nd for (int i = 1; i < polygon.NumVertices; i++) { tmpDistance = Vector2.Dot(polygon.v[i], nd); if (tmpDistance > farthestDistance) { farthestDistance = tmpDistance; farthestIndex = i; } } return polygon.v[farthestIndex]; }
/// <summary> /// Finds the farthest point in a given direction /// Search direction does not need to be normalized /// </summary> /// <param name="polygon"></param> /// <param name="nd"></param> /// <returns></returns> public static Vector2 FarthestPointInDirection(ref gxtPolygon polygon, Vector2 nd) { //gxtDebug.Assert(gxtMath.Equals(nd.Length(), 1.0f, float.Epsilon), "Direction vector must be normalized"); int farthestIndex = 0; float farthestDistance = Vector2.Dot(polygon.v[0], nd); float tmpDistance; // info could be calculated here to affect search direction for (int i = 1; i < polygon.NumVertices; i++) { tmpDistance = Vector2.Dot(polygon.v[i], nd); if (tmpDistance > farthestDistance) { farthestDistance = tmpDistance; farthestIndex = i; } } return polygon.v[farthestIndex]; }
/// <summary> /// Performs a ray cast on the polygon given an optional tmax value /// Only process the information in the rayHit structure if Intersection = true, /// which is specified in the gxtRayHit instance and the return value. If the ray origin is /// inside the Polygon it is not considered an intersection /// </summary> /// <param name="ray">Ray</param> /// <param name="polygon">Polygon</param> /// <param name="rayHit">Ray Hit Info</param> /// <param name="tmax">Max T Value</param> /// <returns>If Intersecting</returns> public virtual bool RayCast(gxtRay ray, ref gxtPolygon polygon, out gxtRayHit rayHit, float tmax = float.MaxValue) { rayHit = new gxtRayHit(); rayHit.Distance = tmax; // if a crossing is within tmax bool intersection = false; // temp holder for segment distance float distance; // number of crossings, regardless of tmax int crossings = 0; // log a message when we run a raycast on a very detailed polygon // to indicate that the results may not be perfect int testIterations = polygon.NumVertices; if (MaxIterations < polygon.NumVertices) { gxtLog.WriteLineV(gxtVerbosityLevel.WARNING, "Polygon vertices exceeds max collider iterations. Not all segments will be tested!"); testIterations = MaxIterations; } for (int j = polygon.NumVertices - 1, i = 0; i < testIterations; j = i, i++) { if (RayIntersectsSegment(ray, polygon.v[j], polygon.v[i], out distance)) { crossings++; if (distance <= rayHit.Distance) { intersection = true; rayHit.Distance = distance; rayHit.Point = ray.GetPoint(distance); // right perp assumes CCW polygon winding Vector2 edge = polygon.v[j] - polygon.v[i]; rayHit.Normal = Vector2.Normalize(gxtMath.RightPerp(edge)); } } } // raycast algorithm rayHit.Intersection = intersection && crossings > 0 && crossings % 2 == 0; return rayHit.Intersection; }
public static bool RayCast(gxtRay ray, gxtPolygon polygon, float tmax, out float t) { t = float.MaxValue; bool intersection = false; int crossings = 0; float distance; for (int j = polygon.NumVertices - 1, i = 0; i < polygon.NumVertices; j = i, i++) { if (RayIntersectsSegment(ray, polygon.v[j], polygon.v[i], float.MaxValue, out distance)) { crossings++; if (distance < tmax) intersection = true; if (distance < t) t = distance; } } return intersection && crossings > 0 && crossings % 2 == 0; }
/// <summary> /// Gets the farthest points in one call /// </summary> /// <param name="polyA"></param> /// <param name="polyAPt"></param> /// <param name="polyB"></param> /// <param name="polyBPt"></param> /// <param name="nd"></param> /// <returns></returns> public Vector2 SupportPt(ref gxtPolygon polyA, out Vector2 polyAPt, ref gxtPolygon polyB, out Vector2 polyBPt, Vector2 nd) { polyAPt = FarthestPointInDirection(ref polyA, nd); polyBPt = FarthestPointInDirection(ref polyB, -nd); return polyAPt - polyBPt; }
public override bool Collide(ref gxtPolygon polygonA, Vector2 centroidA, ref gxtPolygon polygonB, out gxtCollisionResult collisionResult) { throw new NotImplementedException(); }
public static void DebugDrawSimplex(List<Vector2> simplex, Color color, float depth) { Vector2[] vertices = simplex.ToArray(); #if GXT_USE_PHYSICS_SCALING for (int i = 0; i < vertices.Length; i++) { vertices[i] *= gxtPhysicsWorld.PHYSICS_SCALE; } #endif gxtPolygon poly = new gxtPolygon(vertices); gxtDebugDrawer.Singleton.AddPolygon(poly, color, 0.0f); for (int i = 0; i < poly.NumVertices; i++) { gxtDebugDrawer.Singleton.AddPt(poly.v[i], Color.WhiteSmoke, 0.0f); gxtDebugDrawer.Singleton.AddString(i.ToString(), poly.v[i], Color.White, 0.0f); } }
public void Initialize(Vector2 initPos, float initRot = 0.0f) { hashedString = new gxtHashedString("player_actor"); this.position = initPos; this.rotation = gxtMath.WrapAngle(initRot); // in physics world units playerPolygon = gxtGeometry.CreateRectanglePolygon(2, 5.5f); }
public abstract bool Collide(ref gxtPolygon polygonA, Vector2 centroidA, ref gxtPolygon polygonB, out gxtCollisionResult collisionResult);
public abstract bool Intersects(ref gxtPolygon polygonA, Vector2 centroidA, ref gxtPolygon polygonB, Vector2 centroidB);
public static bool RayCast(gxtRay ray, gxtPolygon polygon, float tmax, out float t, out Vector2 pt, out Vector2 normal) { t = float.MaxValue; pt = ray.Origin; normal = ray.Direction; bool intersection = false; // temp holder for segment distance float distance; int crossings = 0; for (int j = polygon.NumVertices - 1, i = 0; i < polygon.NumVertices; j = i, i++) { if (RayIntersectsSegment(ray, polygon.v[j], polygon.v[i], float.MaxValue, out distance)) { crossings++; if (distance < t && distance <= tmax) { intersection = true; t = distance; pt = ray.GetPoint(t); Vector2 edge = polygon.v[i] - polygon.v[j]; normal = Vector2.Normalize(gxtMath.RightPerp(edge)); //normal = gxtMath.GetReflection(ray.Direction, edgeNormal); } } } return intersection && crossings > 0 && crossings % 2 == 0; }
public void Initialize(Vector2 initPos, float speed = 3.0f, float maxSpeed = 500.0f) { hashedString = new gxtHashedString("player_actor"); this.position = initPos; this.rotation = 0.0f; // if we were to take a rotation be sure to use gxtMath.WrapAngle(initRot) MoveSpeed = speed; MaxSpeed = maxSpeed; this.clipMode = asgClipMode.NORMAL; // in physics world units // setup body playerBody = new gxtRigidBody(); playerBody.Mass = 2.0f; playerBody.CanSleep = false; // should NEVER go to sleep playerBody.Awake = true; playerBody.FixedRotation = true; playerBody.IgnoreGravity = false; playerBody.Position = position; playerBody.Rotation = rotation; world.AddRigidBody(playerBody); // setup geom //playerPolygon = gxtGeometry.CreateRectanglePolygon(2, 3.5f); playerPolygon = gxtGeometry.CreateRoundedRectanglePolygon(2.0f, 3.5f, 0.45f, 0.05f); //playerPolygon = gxtGeometry.CreateEllipsePolygon(1.0f, 1.75f, 20); //playerPolygon = gxtGeometry.CreateCapsulePolygon(3.0f, 1.0f, 8); //playerPolygon = gxtGeometry.CreateCirclePolygon(3.0f, 3); playerGeom = new gxtGeom(playerPolygon, position, rotation); playerGeom.Tag = this; playerGeom.CollidesWithGroups = world.PhysicsWorld.GetCollisionGroup("traversable_world_geometry"); playerGeom.CollisionGroups = world.PhysicsWorld.GetCollisionGroup("player"); playerGeom.RigidBody = playerBody; playerGeom.OnCollision += OnCollision; playerGeom.OnSeperation += OnSeperation; world.PhysicsWorld.AddGeom(playerGeom); // setup scene node // for now we'll just use a line loop but programming it // this way makes it easy to swap out for something like a skeleton // or a texture later down the road scenePoly = gxtPolygon.Copy(playerPolygon); scenePoly.Scale(gxtPhysicsWorld.PHYSICS_SCALE); //playerEntity = new gxtLineLoop(scenePoly.v); // setup drawable //playerDrawable = new gxtDrawable(playerEntity, Color.Yellow, true, 0.1f); playerSceneNode = new gxtSceneNode(); playerSceneNode.Position = playerBody.Position; //playerSceneNode.AttachDrawable(playerDrawable); //world.AddSceneNode(playerSceneNode); // setup raycatsing logic rayCastTimer = new gxtStopWatch(true); world.AddProcess(rayCastTimer); raycasts = new List<gxtRayHit>(); clipMode = asgClipMode.NORMAL; this.halfHeight = playerGeom.LocalAABB.Height * 0.5f; }
public static bool Intersects(ref gxtPolygon polyA, Vector2 centroidA, ref gxtPolygon polyB, Vector2 centroidB) { Vector2 d = centroidB - centroidA; return Intersects(ref polyA, ref polyB, d); }
public static bool Intersects(ref gxtPolygon polyA, ref gxtPolygon polyB) { // arbitrary search direction // strictly placeholder for now Vector2 d = Vector2.One; return Intersects(ref polyA, ref polyB, d); }
public static bool BuildSimplex(ref gxtPolygon polyA, ref gxtPolygon polyB, Vector2 nd, out List<Vector2> simplex, out List<Vector2> pointsOnA, out List<Vector2> pointsOnB) { simplex = new List<Vector2>(); pointsOnA = new List<Vector2>(); pointsOnB = new List<Vector2>(); Vector2 ptA; Vector2 ptB; Vector2 supportPt = SupportPt(ref polyA, out ptA, ref polyB, out ptB, nd); simplex.Add(supportPt); pointsOnA.Add(ptA); pointsOnB.Add(ptB); nd = -nd; int iterations = 0; while (iterations < MAX_ITERATIONS) { supportPt = SupportPt(ref polyA, out ptA, ref polyB, out ptB, nd); simplex.Add(supportPt); pointsOnA.Add(ptA); pointsOnB.Add(ptB); // if worse we know they aren't colliding, so don't even bother // finding closest points if (Vector2.Dot(simplex[simplex.Count - 1], nd) <= 0.0f) return false; // if true, we have an intersection // if not the search direction will be modified if (SimplexContainsOrigin(simplex, pointsOnA, pointsOnB, ref nd)) return true; iterations++; } return false; }
/// <summary> /// Builds a simplex as part of GJK, determines collision between the two polygons /// Termination simplex can be used by EPA /// </summary> /// <param name="polyA"></param> /// <param name="polyB"></param> /// <param name="nd"></param> /// <param name="simplex"></param> /// <returns>If intersecting</returns> public static bool BuildSimplex(ref gxtPolygon polyA, ref gxtPolygon polyB, Vector2 nd, out List<Vector2> simplex) { simplex = new List<Vector2>(); //add a support point simplex.Add(SupportPt(ref polyA, ref polyB, nd)); nd = -nd; int iterations = 0; // gjk loop while (iterations < MAX_ITERATIONS) { // add a new support point simplex.Add(SupportPt(ref polyA, ref polyB, nd)); // if worse we know they aren't colliding if (Vector2.Dot(simplex[simplex.Count - 1], nd) <= 0.0f) return false; else if (SimplexContainsOrigin(simplex, ref nd)) return true; iterations++; } return false; }
public static bool Intersects(ref gxtPolygon polyA, ref gxtPolygon polyB, Vector2 nd) { List<Vector2> simplex = new List<Vector2>(); simplex.Add(SupportPt(ref polyA, ref polyB, nd)); nd = -nd; int iterations = 0; while (iterations < MAX_ITERATIONS) { simplex.Add(SupportPt(ref polyA, ref polyB, nd)); if (Vector2.Dot(simplex[simplex.Count - 1], nd) <= 0.0f) return false; else if (SimplexContainsOrigin(simplex, ref nd)) return true; iterations++; } return false; }
public override bool Intersects(ref gxtPolygon polygonA, Vector2 centroidA, ref gxtPolygon polygonB, Vector2 centroidB) { throw new NotImplementedException(); }
private void Triangulate(gxtPolygon polygon) { // setup vertex buffer vertices = new VertexPositionColorTexture[polygon.NumVertices]; for (int i = 0; i < vertices.Length; i++) { vertices[i] = new VertexPositionColorTexture(new Vector3(polygon.v[i].X, polygon.v[i].Y, 0.0f), Color.White, new Vector2(0.0f, 0.0f)); // coords will be figured out later } vertexBuffer.SetData<VertexPositionColorTexture>(vertices); // setup index buffer, uses proper triangulation List<int> indices = new List<int>(3 + ((vertices.Length - 3) * 3)); for (int i = 1, j = 2; j < vertices.Length; i = j, j++) { indices.Add(0); indices.Add(i); indices.Add(j); } indicesArray = indices.ToArray(); indexBuffer.SetData<int>(indicesArray); }
/// <summary> /// Creates a support point for the simplex given the polygons and a given search direction /// Used to build the simplex as part of the GJK and EPA algorithms /// </summary> /// <param name="polyA"></param> /// <param name="polyB"></param> /// <param name="nd"></param> /// <returns></returns> public Vector2 SupportPt(ref gxtPolygon polyA, ref gxtPolygon polyB, Vector2 nd) { Vector2 p1 = FarthestPointInDirection(ref polyA, nd); Vector2 p2 = FarthestPointInDirection(ref polyB, -nd); return p1 - p2; }
/// <summary> /// Returns extensive collision information about the given polygons /// Will determine if intersecting by running the GJK algorithm /// It saves the terminating simplex and further uses it for the /// EPA algorithm. Returns the intersection depth and collision normal as /// part of the gxtCollisionResult return struct. The normal and depth will /// not be calculated, and thus such information is not realiable, if the intersection /// is false /// </summary> /// <param name="polyA">Polygon A</param> /// <param name="centroidA">Centroid of polygon A</param> /// <param name="polyB">Polygon B</param> /// <param name="centroidB">Centroid of polygon B</param> /// <returns>Collision Result</returns> public static gxtCollisionResult CollideOld(gxtPolygon polyA, Vector2 centroidA, gxtPolygon polyB, Vector2 centroidB) { // optimal search direction Vector2 direction = centroidB - centroidA; // simplex vector2 collection List<Vector2> simplex; // the result structure for the collision gxtCollisionResult result = new gxtCollisionResult(); // finishes with terminating simplex from gjk // boolean return for intersection if (!BuildSimplex(ref polyA, ref polyB, direction, out simplex)) { result.Intersection = false; return result; } else { result.Intersection = true; } // iterations int iterations = 0; // holder edge information Vector2 normal; float distance; int index; // calculate the ordering (CW/CCW) // of the simplex. only calc once // reusable in loop int winding = GetWinding(simplex); // epa loop while (iterations < MAX_ITERATIONS) { // find closest edge to origin info //FindClosestEdge(simplex, out index, out distance, out normal); if (FindClosestFeature(simplex, winding, out index, out distance, out normal)) { //gxtLog.WriteLineV(gxtVerbosityLevel.WARNING, "vertex collision"); result.Normal = normal; result.Depth = distance; //result.EdgeIndex = index; //DebugDrawSimplex(simplex, Color.Blue, 0.0f); return result; } // TODO: COMPARE TO VERTEX VALUES // REALLY LOOKING FOR CLOSEST *FEATURE* // support point to (possibly) add to the simplex Vector2 pt = SupportPt(ref polyA, ref polyB, normal); float d = Vector2.Dot(pt, normal); // if less than our tolerance // will return this information if (d - distance < EPA_TOLERANCE) { result.Normal = normal; result.Depth = distance; //result.EdgeIndex = index; // TEMPORARY //DebugDrawSimplex(simplex, Color.Green, 0.0f); //gxtDebugDrawer.Singleton.AddLine(Vector2.Zero, result.Normal * 30, Color.Gray, 0.0f); return result; } // otherwise add to the simplex else { simplex.Insert(index, pt); iterations++; } } return result; }
public static bool RayCast(gxtRay ray, gxtPolygon polygon) { //if (polygon.Contains(ray.Origin)) // return false; int crossings = 0; for (int j = polygon.NumVertices - 1, i = 0; i < polygon.NumVertices; j = i, i++) { if (RayIntersectsSegment(ray, polygon.v[j], polygon.v[i])) crossings++; } return crossings > 0 && crossings % 2 == 0; }
/// <summary> /// Returns extensive collision information about the given polygons /// Will determine if intersecting by running the GJK algorithm /// It saves the terminating simplex and further uses it for the /// EPA algorithm. Returns the intersection depth and collision normal as /// part of the gxtCollisionResult return struct. The normal and depth will /// not be calculated, and thus such information is not realiable, if the intersection /// is false /// </summary> /// <param name="polyA">Polygon A</param> /// <param name="centroidA">Centroid of polygon A</param> /// <param name="polyB">Polygon B</param> /// <param name="centroidB">Centroid of polygon B</param> /// <returns>Collision Result</returns> public static gxtCollisionResult Collide(gxtPolygon polyA, Vector2 centroidA, gxtPolygon polyB, Vector2 centroidB) { // optimal search direction Vector2 direction = centroidB - centroidA; // simplex vector2 collection List<Vector2> simplex; List<Vector2> pointsOnA; List<Vector2> pointsOnB; // the result structure for the collision gxtCollisionResult result = new gxtCollisionResult(); // finishes with terminating simplex from gjk // boolean return for intersection if (!BuildSimplex(ref polyA, ref polyB, direction, out simplex, out pointsOnA, out pointsOnB)) { result.Intersection = false; return result; } else { result.Intersection = true; } // iterations int iterations = 0; // holder edge information Vector2 normal; float distance; int index; Vector2 cpA, cpB; /* FindClosestPointsSpecial(simplex, pointsOnA, pointsOnB, out cpA, out cpB); #if (GXT_DEBUG_DRAW_CONTACTS) gxtDebugDrawer.Singleton.AddPt(cpA * gxtPhysicsWorld.PHYSICS_SCALE, Color.GreenYellow, 0.0f); gxtDebugDrawer.Singleton.AddPt(cpB * gxtPhysicsWorld.PHYSICS_SCALE, Color.GreenYellow, 0.0f); #endif result.ContactPointA = cpA; result.ContactPointB = cpB; */ // calc ordering once, reusable in loop int winding = GetWinding(simplex); // epa loop while (iterations < MAX_ITERATIONS) { // find closest edge to origin info // returns true if it is a vertex FindClosestFeature(simplex, winding, out index, out distance, out normal); /* if (FindClosestFeature(simplex, winding, out index, out distance, out normal)) { result.Normal = normal; result.Depth = distance; int nextIndex = (index + 1) % simplex.Count; FindClosestPointsHelper(simplex[index], simplex[nextIndex], pointsOnA[index], pointsOnA[nextIndex], pointsOnB[index], pointsOnB[nextIndex], out cpA, out cpB); result.ContactPointA = cpA; result.ContactPointB = cpB; #if (GXT_DEBUG_DRAW_CONTACTS) gxtDebugDrawer.Singleton.AddPt(cpA * gxtPhysicsWorld.PHYSICS_SCALE, Color.Red, 0.0f); gxtDebugDrawer.Singleton.AddPt(cpB * gxtPhysicsWorld.PHYSICS_SCALE, Color.Red, 0.0f); #endif #if GXT_DEBUG_DRAW_SIMPLEX DebugDrawSimplex(simplex, Color.Blue, 0.0f); #endif return result; } */ // TODO: COMPARE TO VERTEX VALUES // REALLY LOOKING FOR CLOSEST *FEATURE* // support point to (possibly) add to the simplex //Vector2 pt = SupportPt(ref polyA, ref polyB, normal); Vector2 ptA, ptB; Vector2 pt = SupportPt(ref polyA, out ptA, ref polyB, out ptB, normal); float d = Vector2.Dot(pt, normal); // if less than our tolerance // will return this information if (d - distance < EPA_TOLERANCE) { result.Normal = normal; result.Depth = distance; FindClosestPointsSpecial2(simplex, pointsOnA, pointsOnB, out cpA, out cpB); result.ContactPointA = cpA; result.ContactPointB = cpB; //result.EdgeIndex = index; #if GXT_DEBUG_DRAW_SIMPLEX DebugDrawSimplex(simplex, Color.Green, 0.0f); #endif #if (GXT_DEBUG_DRAW_CONTACTS) gxtDebugDrawer.Singleton.AddPt(cpA * gxtPhysicsWorld.PHYSICS_SCALE, Color.Red, 0.0f); gxtDebugDrawer.Singleton.AddPt(cpB * gxtPhysicsWorld.PHYSICS_SCALE, Color.Red, 0.0f); #endif return result; } // otherwise add to the simplex else { simplex.Insert(index, pt); // could remove pointsOnA.Insert(index, ptA); pointsOnB.Insert(index, ptB); iterations++; } } return result; }
public static float Distance(ref gxtPolygon polyA, ref gxtPolygon polyB, Vector2 nd, out Vector2 cpA, out Vector2 cpB) { cpA = Vector2.Zero; cpB = Vector2.Zero; //int icpA = 0; //int icpB = 0; List<Vector2> simplex = new List<Vector2>(); List<Vector2> pointsOnA = new List<Vector2>(); List<Vector2> pointsOnB = new List<Vector2>(); Vector2 ptA; Vector2 ptB; // first search direction Vector2 supportPt = SupportPt(ref polyA, out ptA, ref polyB, out ptB, nd); simplex.Add(supportPt); pointsOnA.Add(ptA); pointsOnB.Add(ptB); // reverse search direction nd = -nd; Vector2 supportPt2 = SupportPt(ref polyA, out ptA, ref polyB, out ptB, nd); simplex.Add(supportPt2); pointsOnA.Add(ptA); pointsOnB.Add(ptB); //nd = ClosestPointOnSegment(simplex[0], simplex[1], Vector2.Zero); int iterations = 0; while (iterations < MAX_ITERATIONS) { nd = ClosestPointOnSegment(simplex[0], simplex[1], Vector2.Zero); nd = -nd; nd.Normalize(); if (nd == Vector2.Zero) return 0.0f; Vector2 supportPt3 = SupportPt(ref polyA, out ptA, ref polyB, out ptB, nd); simplex.Add(supportPt3); pointsOnA.Add(ptA); pointsOnB.Add(ptB); if (TriangleContainsOrigin(simplex[0], simplex[1], simplex[2])) return 0.0f; float proj = Vector2.Dot(simplex[2], nd); if (proj - Vector2.Dot(simplex[0], nd) < float.Epsilon) { FindClosestPoints(simplex, pointsOnA, pointsOnB, out cpA, out cpB); return -proj; } // TODO: FINISH ALGORITHM if (simplex[0].LengthSquared() < simplex[1].LengthSquared()) { simplex[1] = simplex[2]; } else { simplex[0] = simplex[2]; } iterations++; } return 0.0f; }
public static bool Intersects(ref gxtPolygon polygon, Vector2 centroid, gxtSphere sphere) { if (Contains(polygon, sphere.Position)) return true; float r2 = sphere.Radius * sphere.Radius; for (int j = polygon.NumVertices - 1, i = 0; i < polygon.NumVertices; j = i, i++) { Vector2 edgeVector = polygon.v[i] - polygon.v[j]; Vector2 edgeNormal = Vector2.Normalize(gxtMath.RightPerp(edgeVector)); float edgeDistance = gxtMath.Abs(Vector2.Dot(polygon.v[j], edgeNormal)); // void PointEdgeDistance(const Vector& P, const Vector& E0, const Vector& E1, Vector& Q, float& dist2){ Vector D = P - E0; Vector E = E1 - E0; float e2 = E * E; float ed = E * D; float t = (ed / e2); t = (t < 0.0)? 0.0f : (t > 1.0f)? : 1.0f : t; Q = E0 + t * E; Vector PQ = Q - P; dist2 = PQ * PQ;} if (edgeDistance <= r2) return true; } return false; }
private gxtGeom CreatePlatformGeom(gxtPolygon polygon, Vector2 position) { gxtGeom platGeom = new gxtGeom(polygon, true); gxtRigidBody platBody = new gxtRigidBody(); platBody.MotionType = gxtRigidyBodyMotion.FIXED; platBody.CanSleep = false; platBody.Awake = true; platGeom.RigidBody = platBody; gxtPhysicsMaterial mat = new gxtPhysicsMaterial(0.6f, 0.3f); platGeom.CollisionGroups = world.PhysicsWorld.GetCollisionGroup("traversable_world_geometry"); platGeom.CollidesWithGroups = world.PhysicsWorld.GetCollisionGroup("player"); platGeom.Material = mat; platGeom.SetPosition(new Vector2(0.0f, 8.5f)); world.AddGeom(platGeom); return platGeom; }
/* public float Distance(ref gxtPolygon polyA, ref gxtPolygon polyB) { Vector2 d = Vector2.One; return Distance(ref polyA, ref polyB, d); } */ public static float Distance(ref gxtPolygon polyA, Vector2 centroidA, ref gxtPolygon polyB, Vector2 centroidB, out Vector2 cpA, out Vector2 cpB) { Vector2 d = centroidB - centroidA; return Distance(ref polyA, ref polyB, d, out cpA, out cpB); }