/// <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);
}
