public static void ClosestPointOnPolygon(RenderVertex3D[] rgv, Vertex2D pvin, bool fClosed, out Vertex2D pvOut, out int piSeg) { var count = rgv.Length; var minDist = Constants.FloatMax; piSeg = -1; // in case we are not next to the line pvOut = new Vertex2D(); var loopCount = count; if (!fClosed) { --loopCount; // Don"t check segment running from the end point to the beginning point } // Go through line segment, calculate distance from point to the line // then pick the shortest distance for (var i = 0; i < loopCount; ++i) { var p2 = i < count - 1 ? i + 1 : 0; var rgvi = new RenderVertex3D(); rgvi.Set(rgv[i].X, rgv[i].Y, rgv[i].Z); var rgvp2 = new RenderVertex3D(); rgvp2.Set(rgv[p2].X, rgv[p2].Y, rgv[p2].Z); var a = rgvi.Y - rgvp2.Y; var b = rgvp2.X - rgvi.X; var c = -(a * rgvi.X + b * rgvi.Y); var dist = MathF.Abs(a * pvin.X + b * pvin.Y + c) / MathF.Sqrt(a * a + b * b); if (dist < minDist) { // Assuming we got a segment that we are closet to, calculate the intersection // of the line with the perpendicular line projected from the point, // to find the closest point on the line var d = -b; var f = -(d * pvin.X + a * pvin.Y); var det = a * a - b * d; var invDet = det != 0.0f ? 1.0f / det : 0.0f; var intersectX = (b * f - a * c) * invDet; var intersectY = (c * d - a * f) * invDet; // If the intersect point lies on the polygon segment // (not out in space), then make this the closest known point if (intersectX >= MathF.Min(rgvi.X, rgvp2.X) - 0.1 && intersectX <= MathF.Max(rgvi.X, rgvp2.X) + 0.1 && intersectY >= MathF.Min(rgvi.Y, rgvp2.Y) - 0.1 && intersectY <= MathF.Max(rgvi.Y, rgvp2.Y) + 0.1) { minDist = dist; var seg = i; pvOut.X = intersectX; pvOut.Y = intersectY; piSeg = seg; } } } }
public RampVertex GetRampVertex(float tableHeight, float accuracy, bool incWidth) { var result = new RampVertex(); // vvertex are the 2D vertices forming the central curve of the ramp as seen from above var vertex = GetCentralCurve(accuracy); var numVertices = vertex.Length; result.VertexCount = numVertices; result.PointHeights = new float[numVertices]; result.Cross = new bool[numVertices]; result.PointRatios = new float[numVertices]; result.MiddlePoints = new Vertex2D[numVertices]; result.RgvLocal = new Vertex2D[_data.Type != RampType.RampTypeFlat ? (numVertices + 1) * 2 : numVertices * 2]; // Compute an approximation to the length of the central curve // by adding up the lengths of the line segments. var totalLength = 0f; var bottomHeight = _data.HeightBottom + tableHeight; var topHeight = _data.HeightTop + tableHeight; for (var i = 0; i < numVertices - 1; i++) { var v1 = vertex[i]; var v2 = vertex[i + 1]; var dx = v1.X - v2.X; var dy = v1.Y - v2.Y; var length = MathF.Sqrt(dx * dx + dy * dy); totalLength += length; } var currentLength = 0f; for (var i = 0; i < numVertices; i++) { // clamp next and prev as ramps do not loop var prev = vertex[i > 0 ? i - 1 : i]; var next = vertex[i < numVertices - 1 ? i + 1 : i]; var middle = vertex[i]; result.Cross[i] = middle.IsControlPoint; var normal = new Vertex2D(); // Get normal at this point // Notice that these values equal the ones in the line // equation and could probably be substituted by them. var v1Normal = new Vertex2D(prev.Y - middle.Y, middle.X - prev.X); // vector vmiddle-vprev rotated RIGHT var v2Normal = new Vertex2D(middle.Y - next.Y, next.X - middle.X); // vector vnext-vmiddle rotated RIGHT // special handling for beginning and end of the ramp, as ramps do not loop if (i == numVertices - 1) { v1Normal.Normalize(); normal = v1Normal; } else if (i == 0) { v2Normal.Normalize(); normal = v2Normal; } else { v1Normal.Normalize(); v2Normal.Normalize(); if (MathF.Abs(v1Normal.X - v2Normal.X) < 0.0001 && MathF.Abs(v1Normal.Y - v2Normal.Y) < 0.0001) { // Two parallel segments normal = v1Normal; } else { // Find intersection of the two edges meeting this points, but // shift those lines outwards along their normals // First line var a = prev.Y - middle.Y; var b = middle.X - prev.X; // Shift line along the normal var c = -(a * (prev.X - v1Normal.X) + b * (prev.Y - v1Normal.Y)); // Second line var d = next.Y - middle.Y; var e = middle.X - next.X; // Shift line along the normal var f = -(d * (next.X - v2Normal.X) + e * (next.Y - v2Normal.Y)); var det = a * e - b * d; var invDet = det != 0.0 ? 1.0f / det : 0.0f; var intersectX = (b * f - e * c) * invDet; var intersectY = (c * d - a * f) * invDet; normal.X = middle.X - intersectX; normal.Y = middle.Y - intersectY; } } // Update current length along the ramp. var dx = prev.X - middle.X; var dy = prev.Y - middle.Y; var length = MathF.Sqrt(dx * dx + dy * dy); currentLength += length; var percentage = currentLength / totalLength; var currentWidth = percentage * (_data.WidthTop - _data.WidthBottom) + _data.WidthBottom; result.PointHeights[i] = middle.Z + percentage * (topHeight - bottomHeight) + bottomHeight; AssignHeightToControlPoint(new Vertex2D(vertex[i].X, vertex[i].Y), middle.Z + percentage * (topHeight - bottomHeight) + bottomHeight); result.PointRatios[i] = 1.0f - percentage; // only change the width if we want to create vertices for rendering or for the editor // the collision engine uses flat type ramps if (IsHabitrail() && _data.Type != RampType.RampType1Wire) { currentWidth = _data.WireDistanceX; if (incWidth) { currentWidth += 20.0f; } } else if (_data.Type == RampType.RampType1Wire) { currentWidth = _data.WireDiameter; } result.MiddlePoints[i] = new Vertex2D(middle.X, middle.Y) + normal; result.RgvLocal[i] = new Vertex2D(middle.X, middle.Y) + currentWidth * 0.5f * normal; result.RgvLocal[numVertices * 2 - i - 1] = new Vertex2D(middle.X, middle.Y) - currentWidth * 0.5f * normal; } return(result); }
public bool IsZero() { return(MathF.Abs(X) < Constants.FloatMin && MathF.Abs(Y) < Constants.FloatMin && MathF.Abs(Z) < Constants.FloatMin); }