Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- // line-triangle formula
- public static bool LinePlane2(Vector start, Vector end, Vector[] face, Vector norm)
- {
- //tmp = start.Subtract(end);
- Vector3 lineDirection = -end.Vec3 - -start.Vec3;//end.Subtract(start).Vec3;
- //if ((Vector3.Dot(norm.Vec3, face[0].Vec3) - Vector3.Dot(start.Vec3, norm.Vec3)) / Vector3.Dot(lineDirection, norm.Vec3) < 0) // ray collides behind direction
- //return false;
- //lineDirection.Y = -end.Y - -start.Y;
- // Prepare our barycentric variables
- // UV are our pixel coordinates in a 0 to 1 range
- Vector3 u, v, triangleNormal;
- //tmp = face[1].Subtract(face[0]);
- u = face[1].Subtract(face[0]).Vec3;
- //tmp = face[2].Subtract(face[0]);
- v = face[2].Subtract(face[0]).Vec3;
- triangleNormal = Vector3.Cross(u, v);
- // Paranoid Check for collapsed triangle
- if (triangleNormal == new Vector3(0.0f, 0.0f, 0.0f))
- {
- // triangle is degenerate
- return false;
- }
- //tmp = face[0].Subtract(end);
- Vector3 triangleToLineDirection = face[0].Subtract(end).Vec3;
- // Parameters for ray/plane intersection
- float a, b, r;
- a = Vector3.Dot(triangleNormal, triangleToLineDirection);
- b = Vector3.Dot(triangleNormal, lineDirection);
- if (a < 0.0001f) // parallel check
- {
- if (a == 0)
- {
- // ray lies in triangle plane
- return false;
- }
- else
- {
- // ray is disjointed from triangle's plane
- return false;
- }
- }
- r = a / b;
- if (r < 0.0f)
- {
- // Line points away from triangle
- return false;
- }
- // This is the intersection point of the line direction to the triangles plane
- //tmp = start;
- //Vector3 intersectionPoint = start.Vec3;
- Vector3 intersectionPoint = Vector3.Add(start.Vec3, lineDirection * r);
- // This is crazy vector dot product math that essentially is a set of line to point
- // collision checks
- float uu, uv, vv, wu, wv, D;
- uu = Vector3.Dot(u, u);
- uv = Vector3.Dot(u, v);
- vv = Vector3.Dot(v, v);
- // Now we are getting ray from the intersection point to the refrence
- // point on the triangle
- //tmp = face[0];
- Vector3 refrencePointOnTriangle = face[0].Vec3;
- Vector3 w = intersectionPoint - refrencePointOnTriangle;
- wu = Vector3.Dot(w, u);
- wv = Vector3.Dot(w, v);
- D = uv * uv - uu * vv;
- // Testing the "parametric" coordinates
- float s, t;
- s = (uv * wv - vv * wu) / D;
- if (s < 0.0 || s > 1.0) // I is outside T
- {
- // Line Intersection is outside of the Triangle
- return false;
- }
- t = (uv * wu - uu * wv) / D;
- if (t < 0.0 || (s + t) > 1.0)
- {
- // I is outside T
- return false;
- }
- // The line does intersect
- return true;
- }
- // loop-code
- if (!player.pos.Equals(dest))
- {
- collisions = new Model();
- List<Vector> verts = new List<Vector>();
- List<Vector> norms = new List<Vector>();
- List<Color> colors = new List<Color>();
- Stopwatch stopwatch = new Stopwatch();
- stopwatch.Start();
- //ParallelLoopResult result = Parallel.For(0, World._rawdata._Tri.Count(), i =>
- for (int i = 0; i < World._rawdata._Tri.Count(); i++)
- {
- //Gl.glTranslated(0, -player.pos.Y, 0);
- //Gl.glTranslated(player.pos.X, player.pos.Y, player.pos.Z);
- Vector[] face = { World._rawdata._Vert[World._rawdata._Tri[i].Vertex[0]], World._rawdata._Vert[World._rawdata._Tri[i].Vertex[1]], World._rawdata._Vert[World._rawdata._Tri[i].Vertex[2]] };
- Vector[] norm = { World._rawdata._Norm[World._rawdata._Tri[i].Normal[0]], World._rawdata._Norm[World._rawdata._Tri[i].Normal[1]], World._rawdata._Norm[World._rawdata._Tri[i].Normal[2]] };
- Gl.glLoadIdentity();
- /*if (RayPlane(player.pos, dest, face, norm[0]) > 0)
- {
- verts.Add(face[0]); verts.Add(face[1]); verts.Add(face[2]);
- norms.Add(norm[0]); norms.Add(norm[1]); norms.Add(norm[2]);
- colors.Add(new Color(0, 1, 0, 0.5f)); colors.Add(new Color(0, 1, 0, 0.5f)); colors.Add(new Color(0, 1, 0, 0.5f));
- }*/
- /*if (XNARayPlane(player.pos, dest, face, norm[0]) > 0)
- {
- verts.Add(face[0]); verts.Add(face[1]); verts.Add(face[2]);
- norms.Add(norm[0]); norms.Add(norm[1]); norms.Add(norm[2]);
- colors.Add(new Color(0, 0, 1, 0.5f)); colors.Add(new Color(0, 0, 1, 0.5f)); colors.Add(new Color(0, 0, 1, 0.5f));
- }*/
- int chk = 0;
- //LinePlane(player.pos, dest, face, norm[0]) &&
- if (Collision.LinePlane2(player.pos, dest, face, norm[0]))
- {
- verts.Add(face[0]); verts.Add(face[1]); verts.Add(face[2]);
- norms.Add(norm[0]); norms.Add(norm[1]); norms.Add(norm[2]);
- colors.Add(new Color(1, 0, 0, 0.5f)); colors.Add(new Color(1, 0, 0, 0.5f)); colors.Add(new Color(1, 0, 0, 0.5f));
- //dest = player.pos;
- }
- /*Vector3 intersection;
- if (LinePlane2(out intersection, new Vector3(player.pos.X, player.pos.Y, player.pos.Z), new Vector3(dest.X, dest.Y, dest.Z), new Vector3(norm[0].X, norm[0].Y, norm[0].Z), new Vector3(verts[0].X, verts[0].Y, verts[0].Z)))
- {
- if (intersection.Length() - player.pos.Length() < dest.Subtract(player.pos).Length())
- {
- verts.Add(face[0]); verts.Add(face[1]); verts.Add(face[2]);
- norms.Add(norm[0]); norms.Add(norm[1]); norms.Add(norm[2]);
- colors.Add(new Color(0, 0, 1, 0.5f)); colors.Add(new Color(0, 0, 1, 0.5f)); colors.Add(new Color(0, 0, 1, 0.5f));
- }
- //dest = player.pos;
- }*/
- /*Vector size = player.model.getSize();
- if (XNABoundingBox(new Vector(size.X / 2 - dest.X, size.Y / 2 - dest.Y, size.Z / 2 - dest.Z), size, face, norm[0]) == 0)
- {
- verts.Add(face[0]); verts.Add(face[1]); verts.Add(face[2]);
- norms.Add(norm[0]); norms.Add(norm[1]); norms.Add(norm[2]);
- colors.Add(new Color(0, 1, 0, 0.5f)); colors.Add(new Color(0, 1, 0, 0.5f)); colors.Add(new Color(0, 1, 0, 0.5f));
- }*/
- }//);
- collisions._rawdata._Vert = verts.ToArray();
- collisions._rawdata._Norm = norms.ToArray();
- int a = 0;
- collisions._vertexArray = new float[(collisions._rawdata._Vert.Count()) * 3 * 3];
- foreach (Vector v in collisions._rawdata._Vert)
- {
- collisions._vertexArray[a + 0] = v.X;
- collisions._vertexArray[a + 1] = v.Y;
- collisions._vertexArray[a + 2] = v.Z;
- a += 3;
- }
- a = 0;
- collisions._vertexNormal = new float[(collisions._rawdata._Norm.Count()) * 3 * 3];
- foreach (Vector v in collisions._rawdata._Norm)
- {
- collisions._vertexNormal[a + 0] = v.X;
- collisions._vertexNormal[a + 1] = v.Y;
- collisions._vertexNormal[a + 2] = v.Z;
- a += 3;
- }
- a = 0;
- collisions._vertexColors = new float[(collisions._rawdata._Vert.Count()) * 4 * 3];
- foreach (Color c in colors)
- {
- collisions._vertexColors[a + 0] = c.Red;
- collisions._vertexColors[a + 1] = c.Green;
- collisions._vertexColors[a + 2] = c.Blue;
- collisions._vertexColors[a + 3] = c.Alpha;
- a += 4;
- }
- //while (!result.IsCompleted)
- //;
- stopwatch.Stop();
- //System.Console.WriteLine("[" + DateTime.Now.ToString("HH:mm:ss+ff") + "] <System>: Collision detections took: " + stopwatch.ElapsedMilliseconds + " miliseconds.");
- /*System.Console.WriteLine("[" + DateTime.Now.ToString("HH:mm:ss+ff") + "] <System>: Will collide with "+raycount+" faces,"+
- "\nimmediate collisions "+(allow_move? "detected" : "not detected")+","+
- "\nfound in " + stopwatch.ElapsedMilliseconds + " miliseconds.");*/
- stopwatch.Reset();
- player.pos = dest;
- }
Advertisement
Add Comment
Please, Sign In to add comment