public Vertex2 CalcIntersection(Vertex2 p1, Vertex2 p2, Vertex2 v1, Vertex2 v2)
{
//float s1 = (p2.Y - p1.Y) / (p2.X - p1.X);
//float s2 = (v2.Y - v1.Y) / (v2.X - v1.X);
//float x = (s1 * p1.X - s2 * v1.X - p1.Y + v1.Y) / (s1 - s2);
//float y = s1 * (x - p1.X) + p1.Y;
//return new Vertex2(x, y);
// Step Three - Check for error conditions. If none, compute the intersection
float a1 = p2.Y - p1.Y;
float b1 = p1.X - p2.X;
float c1 = p2.X * p1.Y - p1.X * p2.Y; // a1*x + b1*y + c1 = 0 is line 1
float a2 = v2.Y - v1.Y;
float b2 = v1.X - v2.X;
float c2 = v2.X * v1.Y - v1.X * v2.Y; // a2*x + b2*y + c2 = 0 is line 2
float denom = a1 * b2 - a2 * b1;
if (denom == 0f) return Vertex2.Zero;
float x = (b1 * c2 - b2 * c1) / denom; // (x,y) is the intersection
float y = (a2 * c1 - a1 * c2) / denom;
Vertex2 neu = new Vertex2(x, y);
//Console.WriteLine(((velocity)-neu).ToString());
return neu;
//return 1;
}
public Polygon(List<Vertex2> vertices)
{
st = new StringBuilder();
GhostVertices = new List<Vertex2>();
Vertices = new List<Vertex2>();
DebugVertices = new List<Vertex2>();
DebugNormals = new List<Vertex2>();
nearPolygons = new List<Polygon>();
Velocity = Vertex2.Zero;
CheckedVelocity = Vertex2.Zero;
foreach (Vertex2 v in vertices)
Vertices.Add(v);
foreach (Vertex2 v in vertices)
{
GhostVertices.Add(v);
}
CalcOrigin();
}
public bool BetweenPoints(Vertex2 v1, Vertex2 v2)
{
float length = Vertex2.Distance(v1, v2);
float lengthtoCheckv1 = Vertex2.Distance(v1, this);
float lengthtoCheckv2 = Vertex2.Distance(v2, this);
float test = length - lengthtoCheckv1;
float test2 = length - lengthtoCheckv2;
// Console.WriteLine("Test1:"+test.ToString()+"Test2: "+test2.ToString());
if (test >= 0 && test2 >= 0) return true;
return false;
}
public static Vertex2 Normalize(Vertex2 v1)
{
float length = 1f / Vertex2.Length(v1);
return v1 * length;
}
public static Vertex2 NormalVector(Vertex2 ortVertex, Vertex2 directionVertex, bool up)
{
Vertex2 v = directionVertex - ortVertex;
if (up) return new Vertex2(v.Y, -v.X);
return new Vertex2(-v.Y, v.X);
}
public static Vertex2 hessischeNormal(Vertex2 ortsVertex, Vertex2 richtungsVertex, Vertex2 checkVertex)
{
Vertex2 normal = Vertex2.NormalVector(ortsVertex, richtungsVertex, true);
Vertex2 normalizedNormal = Vertex2.Normalize(normal);
return (checkVertex - ortsVertex) * normalizedNormal;
}
public static float Length(Vertex2 v1, Vertex2 v2)
{
return (float)Math.Sqrt((v1.X * v1.X) + (v1.Y * v1.Y));
}
public Vertex2 calcLineAlignedForce(Vertex2 attackPoint, Vertex2 Velocity, Vertex2 v1, Vertex2 v2)
{
//Vertex2 directionVertex = v2 - v1;
Vertex2 normalAligned = Vertex2.NormalVector(v1, v2, true);
Vertex2 coll = CalcIntersection(attackPoint, attackPoint + CheckedVelocity, v1, v2) - attackPoint;
return Velocity - coll;
}
public static float Distance(Vertex2 from, Vertex2 to)
{
Vertex2 v = to - from;
return (float)Math.Sqrt((v.X * v.X) + (v.Y * v.Y));
}
/// <summary>
/// Gibt den Normalen Vertex(Normalen Vektor) von der gerade zurück
/// </summary>
/// <param name="directionVertex"></param>
/// <returns></returns>
public Vertex2 NormalVector(Vertex2 directionVertex)
{
Vertex2 v = new Vertex2(directionVertex.X - x, directionVertex.Y - y);
return new Vertex2(v.Y, -v.X);
}
public Vertex2 CalcOrthogonalIntersection(Vertex2 p1, Vertex2 v1, Vertex2 v2)
{
Vertex2 normalP = CalcIntersection(p1, p1 + Vertex2.NormalVector(v1, v2, true),v1,v2);
return normalP;
}
public void Move()
{
for (int i = 0; i < Vertices.Count; i++)
{
Vertices[i] += Velocity;
}
Velocity = Vertex2.Zero;
}
public void PerformMovement()
{
bool collide = false;
//CheckedVelocity = Vertex2.Zero;
foreach (Vertex2 vertexToCheck in this.Vertices)
{
goTop = vertexToCheck + Velocity;
foreach (Polygon polygonToCheck in this.nearPolygons)
{
for (int i = 1; i < polygonToCheck.Vertices.Count; i++)
{
Vertex2 beforeFromPolygonToCheck = polygonToCheck.Vertices[i - 1];
Vertex2 currentFromPolygonToCheck = polygonToCheck.Vertices[i];
DebugVertices.Add(CalcIntersection(vertexToCheck, vertexToCheck + Velocity, beforeFromPolygonToCheck, currentFromPolygonToCheck));
Vertex2 oldVel = Velocity;
Vertex2 oben = Vertex2.NormalVector(beforeFromPolygonToCheck, currentFromPolygonToCheck, true);
Vertex2 unten = Vertex2.NormalVector(beforeFromPolygonToCheck, currentFromPolygonToCheck, false);
float distOben = Vertex2.Distance(vertexToCheck, beforeFromPolygonToCheck + oben);
float distUnten = Vertex2.Distance(vertexToCheck, beforeFromPolygonToCheck + unten);
//Console.WriteLine(checkCollision(vertexToCheck, Velocity, beforeFromPolygonToCheck, currentFromPolygonToCheck));
if (checkCollision(vertexToCheck, Velocity, beforeFromPolygonToCheck, currentFromPolygonToCheck))
{
Vertex2 p = CalcIntersection(vertexToCheck, vertexToCheck + Velocity, beforeFromPolygonToCheck, currentFromPolygonToCheck);
//DebugCollPoints.Add(vertexToCheck+(p-vertexToCheck));
DebugCollPoints.Add((p+ LineAlignedForce(p, Velocity, beforeFromPolygonToCheck, currentFromPolygonToCheck)) - vertexToCheck);
//Console.WriteLine("coll");
if (distUnten+1 < distOben ) Velocity = oldVel;
else Velocity = (p + LineAlignedForce(p, Velocity, beforeFromPolygonToCheck, currentFromPolygonToCheck)) - vertexToCheck;
//Velocity = new Vertex2((int)Velocity.X, (int)Velocity.Y);
}
//Velocity = CheckedVelocity < Velocity ? CheckedVelocity : Velocity;
}
}
}
// if (collide) PerformMovement();
this.Move();
}
public Vertex2 LineAlignedForce(Vertex2 p, Vertex2 Velocity, Vertex2 v1, Vertex2 v2)
{
Vertex2 normalAligned = Vertex2.NormalVector(v1, v2, true);
Vertex2 goTo = CalcIntersection(p + Velocity, p + Velocity + normalAligned, v1, v2);
return goTo - p;
}
public void drawPartCircel(float radius, float startAngel, float endAngel, Vertex2 pos, SpriteBatch batch, Texture2D texture)
{
for (float i = startAngel; i <= endAngel; i += 0.1f)
{
float x = (float)(pos.X + Math.Cos(i * Math.PI / 180) * radius);
float y = (float)(pos.Y - Math.Sin(i * Math.PI / 180) * radius);
Vertex2 pixelpos = new Vertex2(x, y);
batch.Draw(texture,new Vector2( pixelpos.X,pixelpos.Y), Color.White);
}
}
public void drawLine(Vertex2 from, Vertex2 to, Texture2D texture, float size, Color color, SpriteBatch spriteBatch)
{
double degress = Math.Atan2((to.Y - from.Y), (to.X - from.X));
float length = Vertex2.Distance(from, to);
spriteBatch.Draw(texture, new Vector2( from.X,from.Y), new Rectangle(0, 0, 1, 1), color, (float)degress, Vector2.Zero, new Vector2(length, size), SpriteEffects.None, 0);
}
public void drawDebugNormal(SpriteBatch batch, Texture2D pixel, Vertex2 v1, Vertex2 v2, Color r)
{
Vertex2 middel = v1 + ((v2 - v1) / 2f);
// Console.WriteLine("Vertex 1:" + v1.ToString() + " Vertex2: " + v2.ToString());
// Console.WriteLine("middel" + middel.ToString());
batch.Draw(pixel, new Vector2(middel.X,middel.Y), new Rectangle(0, 0, 5, 5), Color.White);
Vertex2 normal = Vertex2.NormalVector(v1, v2, true) / 2f;
//Console.WriteLine(normal.ToString());
drawLine(middel, middel + normal, pixel, 1f, Color.White, batch);
}
public bool checkCollision(Vertex2 from, Vertex2 Velocity, Vertex2 v1, Vertex2 v2)
{
Vertex2 p = CalcIntersection(from, from + Velocity, v1, v2);
if (p.BetweenPoints(v1, v2))
{
Vertex2 normalp = CalcOrthogonalIntersection(from+Velocity, v1, v2);
DebugNormals.Add(normalp);
float stepOne = Vertex2.Distance(normalp, from +Velocity+ Vertex2.NormalVector(v1, v2, true));
float stepTwo = Vertex2.Length(Vertex2.NormalVector(v1, v2, true));
float result = stepOne - stepTwo;
if (result <= 0) return true;
}
return false;
}
/// <summary>
/// Gibt den Normalen Vertex(Normalen Vektor) von der gerade zurück
/// </summary>
/// <param name="directionVector"></param>
/// <returns></returns>
public Vertex2 NormalVector(Vertex2 directionVector, Vertex2 originVector)
{
return new Vertex2();
}
public void CalcOrigin()
{
Vertex2 temp = Vertex2.Zero;
foreach (Vertex2 v in this.Vertices)
{
temp += v;
}
temp /= this.Vertices.Count();
Origin = temp;
}