2 replies to this topic

[gretty]

Hello

I am following the tutorial/article 2d Rotated Rectangle Collision algorithm and I have attempted to implement the algorithm in C++.

My Problem: the collision function always returns true, ie, always thinks 2 rectangles collide even they do not.

What do you think I have done wrong? I have a feeling it has to do with my projectOntoAxis() function but to be honest I dont know. Any advice about where my function's are wrong would be extremely helpful

Does anyone know of an existing C/C++ function/library that can tell me whether 2 rotated rectangles collide where I dont have to specify the 4 corners of the rectangle? Ie, I just specify the 2d centre point of the rectangle, the width, height and rotation.

The small  TESTRectCollide2.txt   9.26K   12 downloads.cpp file attached compiles and demonstrates how the function fails.

bool Rectangle::collides(Rectangle rect)
{
   // Post: Returns true if this(Rectangle A) collides with rect(Rectangle B).
   //	   This function is able to detect collisions between rotated rectangles
  
   // Step 1: Calculate the 4 axis' for each rectangle
   Vector2 axis1, axis2, axis3, axis4;
   axis1.x = this->UR.x - this->UL.x;
   axis1.y = this->UR.y - this->UL.y;
   axis2.x = this->UR.x - this->LR.x;
   axis2.y = this->UR.y - this->LR.y;
   axis3.x = rect.UL.x  - rect.LL.x;
   axis3.y = rect.UL.y  - rect.LL.y;
   axis4.x = rect.UL.x  - rect.UR.x;
   axis4.y = rect.UL.y  - rect.UR.y;
  
   // Step 2: Project the vectors representing the four corners of each rectangle onto each of the axes
   Vector2 axis1Proj1A = projectOntoAxis(UL, axis1);
   Vector2 axis1Proj2A = projectOntoAxis(UR, axis1);
   Vector2 axis1Proj3A = projectOntoAxis(LL, axis1);
   Vector2 axis1Proj4A = projectOntoAxis(LR, axis1);
   Vector2 axis1Proj1B = projectOntoAxis(rect.UL, axis1);
   Vector2 axis1Proj2B = projectOntoAxis(rect.UR, axis1);
   Vector2 axis1Proj3B = projectOntoAxis(rect.LL, axis1);
   Vector2 axis1Proj4B = projectOntoAxis(rect.LR, axis1);
  
   Vector2 axis2Proj1A = projectOntoAxis(UL, axis2);
   Vector2 axis2Proj2A = projectOntoAxis(UR, axis2);
   Vector2 axis2Proj3A = projectOntoAxis(LL, axis2);
   Vector2 axis2Proj4A = projectOntoAxis(LR, axis2);
   Vector2 axis2Proj1B = projectOntoAxis(rect.UL, axis2);
   Vector2 axis2Proj2B = projectOntoAxis(rect.UR, axis2);
   Vector2 axis2Proj3B = projectOntoAxis(rect.LL, axis2);
   Vector2 axis2Proj4B = projectOntoAxis(rect.LR, axis2);
  
   Vector2 axis3Proj1A = projectOntoAxis(UL, axis3);
   Vector2 axis3Proj2A = projectOntoAxis(UR, axis3);
   Vector2 axis3Proj3A = projectOntoAxis(LL, axis3);
   Vector2 axis3Proj4A = projectOntoAxis(LR, axis3);
   Vector2 axis3Proj1B = projectOntoAxis(rect.UL, axis3);
   Vector2 axis3Proj2B = projectOntoAxis(rect.UR, axis3);
   Vector2 axis3Proj3B = projectOntoAxis(rect.LL, axis3);
   Vector2 axis3Proj4B = projectOntoAxis(rect.LR, axis3);
  
   Vector2 axis4Proj1A = projectOntoAxis(UL, axis4);
   Vector2 axis4Proj2A = projectOntoAxis(UR, axis4);
   Vector2 axis4Proj3A = projectOntoAxis(LL, axis4);
   Vector2 axis4Proj4A = projectOntoAxis(LR, axis4);
   Vector2 axis4Proj1B = projectOntoAxis(rect.UL, axis4);
   Vector2 axis4Proj2B = projectOntoAxis(rect.UR, axis4);
   Vector2 axis4Proj3B = projectOntoAxis(rect.LL, axis4);
   Vector2 axis4Proj4B = projectOntoAxis(rect.LR, axis4);
  
   // Step 3: Identify the max & min projected vectors for each of the rectangles
   float axis1MinA, axis1MaxA, axis2MinA, axis2MaxA, axis3MinA, axis3MaxA, axis4MinA, axis4MaxA,
		 axis1MinB, axis1MaxB, axis2MinB, axis2MaxB, axis3MinB, axis3MaxB, axis4MinB, axis4MaxB;
   float axis1Val1A  = (axis1Proj1A.x * axis1.x) + (axis1Proj1A.y * axis1.y);
   float axis1Val2A  = (axis1Proj2A.x * axis1.x) + (axis1Proj2A.y * axis1.y);
   float axis1Val3A  = (axis1Proj3A.x * axis1.x) + (axis1Proj3A.y * axis1.y);
   float axis1Val4A  = (axis1Proj4A.x * axis1.x) + (axis1Proj4A.y * axis1.y);
   float axis1Val1B  = (axis1Proj1B.x * axis1.x) + (axis1Proj1B.y * axis1.y);
   float axis1Val2B  = (axis1Proj2B.x * axis1.x) + (axis1Proj2B.y * axis1.y);
   float axis1Val3B  = (axis1Proj3B.x * axis1.x) + (axis1Proj3B.y * axis1.y);
   float axis1Val4B  = (axis1Proj4B.x * axis1.x) + (axis1Proj4B.y * axis1.y);
   findMinMax(axis1Val1A, axis1Val2A, axis1Val3A, axis1Val4A, axis1MinA, axis1MaxA);
   findMinMax(axis1Val1B, axis1Val2B, axis1Val3B, axis1Val4B, axis1MinB, axis1MaxB);
  
   float axis2Val1A  = (axis2Proj1A.x * axis2.x) + (axis2Proj1A.y * axis2.y);
   float axis2Val2A  = (axis2Proj2A.x * axis2.x) + (axis2Proj2A.y * axis2.y);
   float axis2Val3A  = (axis2Proj3A.x * axis2.x) + (axis2Proj3A.y * axis2.y);
   float axis2Val4A  = (axis2Proj4A.x * axis2.x) + (axis2Proj4A.y * axis2.y);
   float axis2Val1B  = (axis2Proj1B.x * axis2.x) + (axis2Proj1B.y * axis2.y);
   float axis2Val2B  = (axis2Proj2B.x * axis2.x) + (axis2Proj2B.y * axis2.y);
   float axis2Val3B  = (axis2Proj3B.x * axis2.x) + (axis2Proj3B.y * axis2.y);
   float axis2Val4B  = (axis2Proj4B.x * axis2.x) + (axis2Proj4B.y * axis2.y);
   findMinMax(axis2Val1A, axis2Val2A, axis2Val3A, axis2Val4A, axis2MinA, axis2MaxA);
   findMinMax(axis2Val1B, axis2Val2B, axis2Val3B, axis2Val4B, axis2MinB, axis2MaxB);
  
   float axis3Val1A  = (axis3Proj1A.x * axis3.x) + (axis3Proj1A.y * axis3.y);
   float axis3Val2A  = (axis3Proj2A.x * axis3.x) + (axis3Proj2A.y * axis3.y);
   float axis3Val3A  = (axis3Proj3A.x * axis3.x) + (axis3Proj3A.y * axis3.y);
   float axis3Val4A  = (axis3Proj4A.x * axis3.x) + (axis3Proj4A.y * axis3.y);
   float axis3Val1B  = (axis3Proj1B.x * axis3.x) + (axis3Proj1B.y * axis3.y);
   float axis3Val2B  = (axis3Proj2B.x * axis3.x) + (axis3Proj2B.y * axis3.y);
   float axis3Val3B  = (axis3Proj3B.x * axis3.x) + (axis3Proj3B.y * axis3.y);
   float axis3Val4B  = (axis3Proj4B.x * axis3.x) + (axis3Proj4B.y * axis3.y);
   findMinMax(axis3Val1A, axis3Val2A, axis3Val3A, axis3Val4A, axis3MinA, axis3MaxA);
   findMinMax(axis3Val1B, axis3Val2B, axis3Val3B, axis3Val4B, axis3MinB, axis3MaxB);
  
   float axis4Val1A  = (axis4Proj1A.x * axis4.x) + (axis4Proj1A.y * axis4.y);
   float axis4Val2A  = (axis4Proj2A.x * axis4.x) + (axis4Proj2A.y * axis4.y);
   float axis4Val3A  = (axis4Proj3A.x * axis4.x) + (axis4Proj3A.y * axis4.y);
   float axis4Val4A  = (axis4Proj4A.x * axis4.x) + (axis4Proj4A.y * axis4.y);
   float axis4Val1B  = (axis4Proj1B.x * axis4.x) + (axis4Proj1B.y * axis4.y);
   float axis4Val2B  = (axis4Proj2B.x * axis4.x) + (axis4Proj2B.y * axis4.y);
   float axis4Val3B  = (axis4Proj3B.x * axis4.x) + (axis4Proj3B.y * axis4.y);
   float axis4Val4B  = (axis4Proj4B.x * axis4.x) + (axis4Proj4B.y * axis4.y);
   findMinMax(axis4Val1A, axis4Val2A, axis4Val3A, axis4Val4A, axis4MinA, axis4MaxA);
   findMinMax(axis4Val1B, axis4Val2B, axis4Val3B, axis4Val4B, axis4MinB, axis4MaxB);
  
   // Step 4: Check for overlapping(collisions) min/max values
   if (axis1MinB <= axis1MinA || axis1MaxB >= axis1MinA)
	  return true;
   if (axis2MinB <= axis2MinA || axis2MaxB >= axis2MinA)
	  return true;
   if (axis3MinB <= axis3MinA || axis3MaxB >= axis3MinA)
	  return true;
   if (axis4MinB <= axis4MinA || axis4MaxB >= axis4MinA)
	  return true;
	
   return false;
}
Vector2 Rectangle::projectOntoAxis(Vector2 rectPnt, Vector2 axis)
{
   Vector2 res;
   res.x = ((rectPnt.x*axis.x + rectPnt.y*axis.y) / (pow(axis.x,2) + pow(axis.y,2))) * axis.x;
   res.y = ((rectPnt.x*axis.x + rectPnt.y*axis.y) / (pow(axis.x,2) + pow(axis.y,2))) * axis.y;
   return res;
}
void Rectangle::findMinMax(float val1, float val2, float val3, float val4, float& min, float& max)
{
	float values[4] = {val1, val2, val3, val4};
	min			 = *min_element(values, values+4);
	max			 = *max_element(values, values+4);
}

class Vector2
{
   public:
   static const float NULL_VALUE = -999999;
   float x;
   float y;
};
class Rectangle
{
   public:
   Vector2 UL;
   Vector2 UR;
   Vector2 LL;
   Vector2 LR;
   float rotation;
};

[slayemin]

I've written code to detect collisions between polygons, so I can generalize/abstract the rectangle out as a polygon.
So, what I do is treat every non-axis aligned rectangle as a polygon, and let my rectangles inherit from the polygon class.
One thing to note: Doing axis-aligned collision detection with rectangles is probably faster and more efficient, mathematically speaking. So, if you can get away with it, prefer to use axis aligned bounding boxes.

Anyways, I also wanted to share this code because I think it is sexy & beautiful (from a logical/mathematical standpoint, not aesthetic). Studying it is like looking at the fine brush strokes on an oil painting. There are probably nuanced flaws and edge cases where it may not work properly, but that's just me being a mediocre code artist. I'm curious to know if anyone finds better methods for doing what I do so I can get better.

This is my "Calc.cs" file

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;

namespace EricsLib
{
	/// <summary>
	/// This is an abstract mathematical representation of a euclidean line.
	/// </summary>
	public class Line
	{
		public Vector2 p1, p2;

		public Line()
		{
			p1 = new Vector2();
			p2 = new Vector2();
		}

		public Line(Vector2 pt1, Vector2 pt2)
		{
			p1 = pt1;
			p2 = pt2;
		}

		public void Translate(Vector2 translation)
		{
			p1 += translation;
			p2 += translation;
		}

		public float Length()
		{
			return (p2 - p1).Length();
		}

		public float LengthSquared()
		{
			return (p2 - p1).LengthSquared();
		}
	}

	/// <summary>
	/// This is a singleton class which exposes and wraps some commonly used calculations
	/// </summary>
	public class Calc
	{
		Random rand;
		
		static Calc instance;
		private Calc()
		{
			rand = new Random();
		}

		private static Calc Instance
		{
			get
			{
				if (instance == null)
					instance = new Calc();
				return instance;
			}
		}

		public static Random random
		{
			get
			{
				return Instance.rand;
			}
		}

		public static Color RandomColor()
		{
			return new Color((byte)Calc.random.Next(255), (byte)Calc.random.Next(255), (byte)Calc.random.Next(255));
		}

		/// <summary>
		/// This gives you the Y value of a guassian distribution curve, provided you have an X value.
		/// Let X be [0, 100] -> Y = [0,1]
		/// </summary>
		/// <param name="X">Percentage value</param>
		/// <returns>Gaussian Y value</returns>
		public static double Gaussian(float X)
		{
			float x = X;
			
			//check for values above 100
			if (x > 100)
				x = 100;

			//converts [0,100] range to [-3,3] range
			float d = (6.0f * (x / 100.0f)) - 3.0f;
			d *= d;
			return Math.Exp(-d);
		}

		/// <summary>
		/// Dumb function: This gives you a percentage value for x.
		/// </summary>
		/// <param name="X"></param>
		/// <returns></returns>
		public static double Linear(float X)
		{
			float x = X;
			if (x > 100)
				x = 100;
			return x / 100.0f;
		}

		/// <summary>
		/// Gives Y value which has a decaying exponential growth function (looks like Quadrant 2 of unit circle)
		/// </summary>
		/// <param name="X">[0,100]</param>
		/// <returns>[0,1)</returns>
		public static double Exponential(float X)
		{
			//exponential decay function, where X is between [0,100] and Y is [0,1]
			//Y value asymptote is at 1.
			//the rate of growth is an exponential decay function.
			return (1.0f + -Math.Exp(-X / 25.0f));
		}

		/// <summary>
		/// Rotates a 2D point around a pivot point
		/// </summary>
		/// <param name="Pivot">The location of the pivot point</param>
		/// <param name="Point">The point to rotate</param>
		/// <param name="Radians">How many radians you want to rotate around pivot point</param>
		/// <returns>The rotated position of the point around the pivot</returns>
		public static Vector2 RotatePoint(Vector2 Pivot, Vector2 Point, float Radians)
		{
			return Vector2.Transform(Point - Pivot, Matrix.CreateRotationZ(Radians)) + Pivot;
		}

		/// <summary>
		/// Tells you if a line intersects with a ray (line with infinite length and origin)
		/// </summary>
		/// <param name="Line1">The line to test</param>
		/// <param name="ray1">the ray to test against</param>
		/// <returns></returns>
		public static bool DoIntersect(Line Line1, Ray ray1)
		{
			//a ray is a line with a known point and the other point is infinitely far away in a given direction.
			Line Line2 = new Line();
			Line2.p1 = new Vector2(ray1.Position.X, ray1.Position.Y);

			Line2.p2 = new Vector2(ray1.Direction.X * 1000000, ray1.Direction.Y * 1000000);

			return DoIntersect(Line1, Line2);
		}

		/// <summary>
		/// Tells you if two lines are intersecting each other
		/// </summary>
		/// <param name="Line1">The first line to test</param>
		/// <param name="Line2">The second line to test</param>
		/// <returns>boolean result</returns>
		public static bool DoIntersect(Line Line1, Line Line2)
		{
			return DoIntersect(Line1.p1, Line1.p2, Line2.p1, Line2.p2);
		}

		/// <summary>
		/// Converts a radian angle to its equivalent rectangular coordinate (normalized)
		/// </summary>
		/// <param name="radians">angle in radians</param>
		/// <returns>Vector2 value containing rectangular coordinate position</returns>
		public static Vector2 UnitVector(float radians)
		{
			return new Vector2((float)Math.Cos(radians), (float)Math.Sin(radians));
		}

		/// <summary>
		/// Converts a vector into a radian angle between zero and two pi
		/// </summary>
		/// <param name="vector">vector direction to convert to radians</param>
		/// <returns>radian value of vector direction</returns>
		public static float RadianVector(Vector2 vector)
		{
			Vector2 B = vector;
			B.Normalize();

			Vector2 A = new Vector2(1, 0);

			float dot = Vector2.Dot(B, A);

			//make sure that we're within the allowable bounds for arccos() by clamping values to -1 and 1.
			if (dot < -1)
				dot = -1;
			if (dot > 1)
				dot = 1;

			float acos = (float)Math.Acos(dot);

			if (B.Y < 0)
			{
				acos = (MathHelper.Pi - acos) + MathHelper.Pi;
			}

			return acos;
		}

		/// <summary>
		/// Determines if a line intersects with a bounding sphere
		/// </summary>
		/// <param name="line">The line segment to test</param>
		/// <param name="circle">The circle to test against</param>
		/// <returns>true/false value indicating intersection</returns>
		public static bool LineCircleIntersect(Line line, BoundingSphere circle)
		{
			/*
			 * Description: Determines if a line intersects with a bounding sphere
			 * Test Status: tested and works. No bugs found.
			 */

			Vector2 d = line.p2 - line.p1;
			Vector2 f = line.p1 - new Vector2(circle.Center.X, circle.Center.Y);

			float a = Vector2.Dot(d, d);
			float b = 2 * Vector2.Dot(f, d);
			float c = Vector2.Dot(f, f) - (circle.Radius * circle.Radius);

			float discriminant = b * b - 4 * a * c;

			if (discriminant < 0)
				return false;
			else
			{
				discriminant = (float)Math.Sqrt(discriminant);

				float t1 = (-b + discriminant) / (2 * a);
				float t2 = (-b - discriminant) / (2 * a);

				if (t1 >= 0 && t1 <= 1)
				{
					//solution is ON the line (tangent)
					return true;
				}
				else
				{
					//solution is out of range of the line
					return false;
				}
			}


			#region wolfram version commented out
			/* //Wolfram version (sucks)
			//the subsequent formula relies on the bounding sphere to be centered on the origin, so
			//we have to translate our line to be relative to the origin too.
			line.Translate(new Vector2(0 - circle.Center.X, 0 - circle.Center.Y));
			
			float dx = line.p2.X - line.p1.X;	   //change in X
			float dy = line.p2.Y - line.p1.Y;	   //change in Y

			float r2 = circle.Radius * circle.Radius;   //radius squared
			float dr2 = line.LengthSquared();		   //distance squared
			float r2dr2 = r2 * dr2;					  //radius squared times distance squared
			

			float D = (line.p1.X * line.p2.Y) - (line.p2.X * line.p1.Y);	//discriminate
			float delta = r2dr2 - (D * D);
			

			//there's an intersection if the discriminate is not negative
			if (delta >= 0)
			{
				//we have a possible intersection, so now we have to make sure that the point of intersection lies between the
				//endpoints of the given line.
				float Q = (float)(Math.Sqrt(delta));

				float x1 = (D * dy + ((dy < 0) ? -1 : 1) * dx * Q) / dr2;
				float y1 = ((-1 * D * dx) + Math.Abs(dy) * Q) / dr2;

				float x2 = (D * dy - ((dy < 0) ? -1 : 1) * dx * Q) / dr2;
				float y2 = ((-1 * D * dx) - Math.Abs(dy) * Q) / dr2;

				//if we have an intersection of the secant line with the test line, we have a hit.
				return DoIntersect(new Line(new Vector2(x1, y1), new Vector2(x2, y2)), line);
				
			}

			return false;
			*/
			#endregion

		}

		/// <summary>
		/// Given the end points for two line segments, this will tell you if those two line segments intersect each other.
		/// </summary>
		/// <param name="A1">First endpoint for Line A</param>
		/// <param name="A2">Second endpoint for Line A</param>
		/// <param name="B1">First endpoint for Line B</param>
		/// <param name="B2">Second endpoint for line B</param>
		/// <returns>true or false depending on if they intersect.</returns>
		public static bool DoIntersect(Vector2 A1, Vector2 A2, Vector2 B1, Vector2 B2)
		{

			//NOTE: Floating point precision errors can cause bugs here. This can result in false-positives or true-negatives.

			//Based off of the Y = mx + b formula for two lines.

			//calculate the slopes for Line A and Line B
			double mx1 = (double)(A2.Y - A1.Y) / (double)(A2.X - A1.X);
			double mx2 = (double)(B2.Y - B1.Y) / (double)(B2.X - B1.X);  

			//calculate the y-intercepts for Line A and Line B
			double b1 = (double)(-mx1 * A2.X) + A2.Y;
			double b2 = (double)(-mx2 * B2.X) + B2.Y;

			//calculate the point of intercept for Line A and Line B
			double x = (b2 - b1) / (mx1 - mx2);
			double y = mx1 * x + b1;

			if (double.IsInfinity(mx1) && double.IsInfinity(mx2))
			{
				//we're dealing with two vertical lines. If both lines share an X value, then we just have to compare
				//y values to see if they intersect.
				return (A1.X == B1.X) && ((B1.Y <= A1.Y && A1.Y <= B2.Y) || (B1.Y <= A2.Y && A2.Y <= B2.Y));
			}
			else if (double.IsInfinity(mx1) && !double.IsInfinity(mx2))
			{
				//Line A is a vertical line but Line B is not.
				x = A1.X;
				y = mx2 * x + b2;

				return (
					((A1.Y <= A2.Y && LTE(A1.Y , y) && GTE(A2.Y, y)) || (A1.Y >= A2.Y && GTE(A1.Y, y) && LTE(A2.Y, y))) &&
					((B1.X <= B2.X && LTE(B1.X, x) && GTE(B2.X, x)) || (B1.X >= B2.X && GTE(B1.X, x) && LTE(B2.X, x))) &&
					((B1.Y <= B2.Y && LTE(B1.Y, y) && GTE(B2.Y, y)) || (B1.Y >= B2.Y && GTE(B1.Y, y) && LTE(B2.Y, y))));
			}
			else if (double.IsInfinity(mx2) && !double.IsInfinity(mx1))
			{
				//Line B is a vertical line but line A is not.
				x = B1.X;
				y = mx1 * x + b1;

				return (
				((A1.X <= A2.X && LTE(A1.X, x) && GTE(A2.X, x)) || (A1.X >= A2.X && GTE(A1.X, x) && LTE(A2.X, x))) &&
				((A1.Y <= A2.Y && LTE(A1.Y, y) && GTE(A2.Y, y)) || (A1.Y >= A2.Y && GTE(A1.Y, y) && LTE(A2.Y, y))) &&
				((B1.Y <= B2.Y && LTE(B1.Y, y) && GTE(B2.Y, y)) || (B1.Y >= B2.Y && GTE(B1.Y, y) && LTE(B2.Y, y))));
			}

			//figure out if the point of interception is between all the given points
			return (
				((A1.X <= A2.X && LTE(A1.X, x) && GTE(A2.X, x)) || (A1.X >= A2.X && GTE(A1.X, x) && LTE(A2.X, x))) &&
				((A1.Y <= A2.Y && LTE(A1.Y, y) && GTE(A2.Y, y)) || (A1.Y >= A2.Y && GTE(A1.Y, y) && LTE(A2.Y, y))) &&
				((B1.X <= B2.X && LTE(B1.X, x) && GTE(B2.X, x)) || (B1.X >= B2.X && GTE(B1.X, x) && LTE(B2.X, x))) &&
				((B1.Y <= B2.Y && LTE(B1.Y, y) && GTE(B2.Y, y)) || (B1.Y >= B2.Y && GTE(B1.Y, y) && LTE(B2.Y, y))));
		}

		/// <summary>
		/// Equal-Equal: Tells you if two doubles are equivalent even with floating point precision errors
		/// </summary>
		/// <param name="Val1">First double value</param>
		/// <param name="Val2">Second double value</param>
		/// <returns>true if they are within 0.000001 of each other, false otherwise.</returns>
		public static bool EE(double Val1, double Val2)
		{
			return (Math.Abs(Val1 - Val2) < 0.000001f);
		}

		/// <summary>
		/// Equal-Equal: Tells you if two doubles are equivalent even with floating point precision errors
		/// </summary>
		/// <param name="Val1">First double value</param>
		/// <param name="Val2">Second double value</param>
		/// <param name="Epsilon">The delta value the two doubles need to be within to be considered equal</param>
		/// <returns>true if they are within the epsilon value of each other, false otherwise.</returns>
		public static bool EE(double Val1, double Val2, double Epsilon)
		{
			return (Math.Abs(Val1 - Val2) < Epsilon);
		}

		/// <summary>
		/// Less Than or Equal: Tells you if the left value is less than or equal to the right value
		/// with floating point precision error taken into account.
		/// </summary>
		/// <param name="leftVal">The value on the left side of comparison operator</param>
		/// <param name="rightVal">The value on the right side of comparison operator</param>
		/// <returns>True if the left value and right value are within 0.000001 of each other, or if leftVal is less than rightVal</returns>
		public static bool LTE(double leftVal, double rightVal)
		{
			return (EE(leftVal, rightVal) || leftVal < rightVal);

		}

		/// <summary>
		/// Greather Than or Equal: Tells you if the left value is greater than or equal to the right value
		/// with floating point precision error taken into account.
		/// </summary>
		/// <param name="leftVal">The value on the left side of comparison operator</param>
		/// <param name="rightVal">The value on the right side of comparison operator</param>
		/// <returns>True if the left value and right value are within 0.000001 of each other, or if leftVal is greater than rightVal</returns>
		public static bool GTE(double leftVal, double rightVal)
		{
			return (EE(leftVal, rightVal) || leftVal > rightVal);
		}

		/// <summary>
		/// 2D VERSION:
		/// Gives you a relative difference in angle from your own position to a destination position. This would be how much
		/// you would need to turn in order to face your target destination.
		/// </summary>
		/// <param name="Position">Your current coordinate position</param>
		/// <param name="Destination">Where you want to go</param>
		/// <param name="CurrentOrienation">Your current facing direction</param>
		/// <returns>Radian angle indicating which direction you need to turn and how much</returns>
		public static float Target_Direction(Vector3 Position, Vector3 Destination, float CurrentOrienation)
		{
			if (Destination == Position)
			{
				//throw new Exception("Destination is the same as the current position!");
				return 0;
			}

			//a line indicating our orientation
			Vector3 orientationVec = new Vector3((float)Math.Cos(CurrentOrienation), (float)Math.Sin(CurrentOrienation), 0);
			Vector3 rightVec = Vector3.Cross(orientationVec, Vector3.Forward);

			//a line towards the target
			Vector3 TgtVec = Vector3.Normalize(Destination - Position);

			float dot = Vector3.Dot(TgtVec, orientationVec);

			//make sure that we're within the allowable bounds for arccos() by clamping values to -1 and 1.
			if (dot < -1)
				dot = -1;
			if (dot > 1)
				dot = 1;

			float acos = (float)Math.Acos(dot);

			float vecDot = Vector3.Dot(TgtVec, rightVec);
			//do we turn left or right?
			if (vecDot < 0)
				return -acos;

			return acos;
		}
	}
}

This is my "Polygon.cs" file

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;

namespace EricsLib
{
	/// <summary>
	/// In the most abstract sense, a polygon is a series of connected points.
	/// In some cases, you may want to rotate the polygon around a pivot point.
	/// </summary>
	public class Polygon
	{
		//the points in a polygon are stored in sequential order.
		protected List<Vector2> m_points = new List<Vector2>();
		protected Vector2 m_pivot;
		
		public Polygon()
		{
			m_pivot = new Vector2(0);
		}

		public Polygon(List<Vector2> verticies)
		{
			foreach (Vector2 v in verticies)
			{
				m_points.Add(v);
			}
			m_pivot = new Vector2(0);
		}

		public List<Vector2> Verticies
		{
			get
			{
				return m_points;
			}
			set
			{
				m_points = value;
			}
		}

		public void AddPoint(Vector2 pt)
		{
			m_points.Add(pt);
		}

		public void AddPoint(float x, float y)
		{
			Vector2 v = new Vector2(x, y);
			m_points.Add(v);
		}


		/// <summary>
		/// Move the polygon on the X/Y axis
		/// </summary>
		/// <param name="offset">The offset vector for the translation (direction and magnitude)</param>
		public void Translate(Vector2 offset)
		{
			for(int a=0;a<m_points.Count;a++)
			{
				Vector2 cur = m_points[a];
				cur.X += offset.X;
				cur.Y += offset.Y;
				m_points[a] = cur;
			}

			m_pivot.X += offset.X;
			m_pivot.Y += offset.Y;
		}

		public void Translate(float x, float y)
		{
			Translate(new Vector2(x, y));
		}


		/// <summary>
		/// This rotates the polygon about the pivot point by the given radian angle
		/// </summary>
		/// <param name="radians">radians to rotate around the pivot point</param>
		public void Rotate(float radians)
		{
			for(int a=0;a<m_points.Count;a++)
			{
				m_points[a] = Calc.RotatePoint(m_pivot, m_points[a], radians);
			}
		}

		/// <summary>
		/// Universally scales the polygon by multiplying the points by an amount
		/// </summary>
		/// <param name="amount"></param>
		public void Scale(float amount)
		{
			for (int a = 0; a < m_points.Count; a++)
			{
				Vector2 cur = m_points[a];
				cur.X *= amount;
				cur.Y *= amount;
				m_points[a] = cur;
			}
		}

		public Vector2 Pivot
		{
			get
			{
				return m_pivot;
			}
			set
			{
				m_pivot = value;
			}
		}

		public Vector2 Position
		{
			get
			{
				return m_pivot;
			}
			set
			{
				CenterOn(value);
			}
		}

		/// <summary>
		/// This finds the mathematical average given all the points in the polygon
		/// </summary>
		/// <returns>A point indicating the average center</returns>
		public Vector2 FindCenter()
		{
			if (m_points.Count == 0)
				return new Vector2();

			Vector2 result = new Vector2();
			foreach (Vector2 v in m_points)
			{
				result.X += v.X;
				result.Y += v.Y;
			}
			result.Y /= m_points.Count;
			result.X /= m_points.Count;

			return result;
		}

		/// <summary>
		/// This centers the polygon on the target point
		/// </summary>
		/// <param name="tgt">The point you want to center the polygon on</param>
		private void CenterOn(Vector2 tgt)
		{

			for (int a = 0; a < m_points.Count; a++)
			{
				//get this points distance from center
				Vector2 delta = m_points[a] - m_pivot;

				m_points[a] = tgt + delta;
			}

			m_pivot = tgt;
		}

		/// <summary>
		/// Determines if this polygon intersects with the bounding sphere
		/// </summary>
		/// <param name="other">The bounding sphere to test against</param>
		/// <returns></returns>
		public bool Intersects(BoundingSphere other)
		{
			//Method: This calculates a line-sphere intersection with every line in the polygon
			//First, see if the polygon contains the point given by the center of the circle
			Vector2 center = new Vector2(other.Center.X, other.Center.Y);

			if (Contains(center))
				return true;

			//next, do a bunch of line/circle intersection tests against each line segment in the polygon
			for (int a = 0; a < m_points.Count; )
			{
				Line l1 = new Line(m_points[a], m_points[++a % (m_points.Count)]);

				if (Calc.LineCircleIntersect(l1, other))
					return true;
			}

			return false;
		}

		/// <summary>
		/// Tells you whether or not a point lies within a polygon
		/// </summary>
		/// <param name="point">Point to test</param>
		/// <returns>true if the point lies inside polygon, false otherwise.</returns>
		public bool Contains(Vector2 point)
		{
			//Testing Status: Quite confident

			//BUG NOTE: If the point is positioned such that the test ray perfectly passes through a point shared by
			//two line segments, this will get an even count which misses an existing collision. For now, I'm picking
			//angles which are irrationally random and hoping I don't get extremely lucky. Don't use this algorithm for nuclear weapons [img]http://public.gamedev.net//public/style_emoticons/default/tongue.png[/img]
			
			//Method: I'm using the Even/Odd rule

			float randDir = (float)(Calc.random.NextDouble() * MathHelper.TwoPi);
			Vector2 dirVec = Calc.UnitVector(randDir);

			//create a 45 degree ray centered on the point
			Ray testRay = new Ray(new Vector3(point, 0), new Vector3(dirVec, 0));

			//Now, loop through every line in the polygon and test for intersection while counting results.
			int Count = 0;
			for (int a = 0; a < m_points.Count;)
			{
				Line line1 = new Line(m_points[a], m_points[++a % (m_points.Count)]);

				if (Calc.DoIntersect(line1, testRay))
					Count++;
			}

			return (Count % 2 != 0);
		}

		/// <summary>
		/// Tells you if the given line passes through this polygon
		/// </summary>
		/// <param name="line">Line to test</param>
		/// <returns></returns>
		public bool Intersects(Line TestLine)
		{

			//We go through every line segment in the polygon and see if the line we're testing
			//intersects any of the line segments.
			for (int a = 0; a < m_points.Count; )
			{
				//create a line connecting the two points in this polygon
				Line line1 = new Line(m_points[a], m_points[++a % (m_points.Count)]);

				if (Calc.DoIntersect(TestLine, line1))
				   return true;
			}

			return false;
		}

		public bool Intersects(Polygon other)
		{
			/*Algorithm: This is the ray casting algorithm using the even-odd rule.
			 If a point is within a polygon, there is an odd number of intersections.
			 Otherwise, there's an even number of intersections and the point is outside the polygon.*/

			//For every point in this polygon, create a ray in any direction
			//loop through every line in the other polygon and count the number of intersections this ray has.
			for (int a = 0; a < m_points.Count; a++)
			{
				int intersectionCount = 0;
				Ray ray1 = new Ray(new Vector3(m_points[a], 0), new Vector3(1, 1, 0));

				for (int b = 0; b < other.m_points.Count;)
				{
					Line line1 = new Line(other.m_points[b], other.m_points[++b % (other.m_points.Count)]);

					if (Calc.DoIntersect(line1, ray1))
						intersectionCount++;

				}

				//if we got odd number of intersections, point lies inside polygon.
				if (intersectionCount % 2 != 0)
					return true;
				
			}


			//Now, do line intersection tests. We have to do this because a polygon could
			//have no points inside the other polygon, yet still be overlapping.
			for (int a = 0; a < m_points.Count;)
			{
				Line line1 = new Line(m_points[a], m_points[++a % (m_points.Count)]);

				for (int b = 0; b < other.m_points.Count;)
				{
					Line line2 = new Line(other.m_points[b], other.m_points[++b % (other.m_points.Count)]);

					if (Calc.DoIntersect(line1, line2))
						return true;
				}
			}


			return false;
		}

		/// <summary>
		/// Writes XML to save the current state of the polygon at the file pointer
		/// </summary>
		/// <param name="writer">A reference to the XML file writer</param>
		public void ExportXML(CommandFileWriter writer)
		{
			writer.PushNode("Polygon");

			writer.PushNode("Pivot");
			writer.WriteVector2(m_pivot);
			writer.PopNode();

			foreach (Vector2 v in m_points)
			{
				writer.PushNode("Vertex");
				writer.WriteVector2(v);
				writer.PopNode();
			}
			writer.PopNode();
		}

		/// <summary>
		/// Reads in XML to load this instance of the polygon
		/// </summary>
		/// <param name="parser"></param>
		public void ImportXML(CommandFileParser parser)
		{
			while (!parser.IsEndElement("Polygon"))
			{
				switch (parser.GetElementName())
				{
					case "Pivot":
						{
							m_pivot = parser.ReadVector2();
							break;
						}
					case "Vertex":
						{
							Vector2 v = parser.ReadVector2();
							m_points.Add(v);
							break;
						}
					default:
						{
							parser.ParserRead();
							break;
						}
				}
			}
			parser.ParserRead();
		}
	}
}

Edit Note: using the "source" tags causes the embedded code to be truncated, so I have to use the "code" tag. Unfortunately, this makes the embedded code slightly less readable.

Edited by slayemin, 02 September 2012 - 05:24 AM.

[serumas]

Your directed article i think is separating axis algorithm, but so so purly written and named very strange.
I will suggest to find realy good article about SAT algorithm with working code, for example
http://www.gamedev.net/page/resources/_/technical/math-and-physics/a-verlet-based-approach-for-2d-game-physics-r2714

About your case:
why use sqrt in projection point to axis? its can by found with dot(). And you must make projection to perpendicular axis...

sorry im bad english

Edited by serumas, 02 September 2012 - 11:54 AM.