// Because all of these tests were written assuming open drive coordinates, but our geometry is in
// Unity coordinates, rather than re-write all the test cases, we just convert from OpenDRIVE to Unity here
static Geometry ConstructGeometryFromOpenDriveCoordinates(
GeometryKind geometryKind, float headingDegrees, float length, float sAbsolute, float x, float z,
ArcData arcData = default, SpiralData spiralData = default,
Poly3Data poly3Data = default, ParamPoly3Data paramPoly3Data = default)
{
var pose = OpenDriveMapElementFactory.FromOpenDriveGlobalToUnity(math.radians(headingDegrees), x, z);
return(ConstructGeometry(geometryKind, pose, length, sAbsolute,
arcData, spiralData, poly3Data, paramPoly3Data));
}
static Geometry ConstructGeometry(
GeometryKind geometryKind, RigidTransform pose, float length, float sAbsolute,
ArcData arcData = default, SpiralData spiralData = default,
Poly3Data poly3Data = default, ParamPoly3Data paramPoly3Data = default)
{
return(new Geometry()
{
length = length,
sRoad = sAbsolute,
startPose = pose,
geometryKind = geometryKind,
arcData = arcData,
spiralData = spiralData,
poly3Data = poly3Data,
paramPoly3Data = paramPoly3Data
});
}
// Because all of these tests were written assuming open drive coordinates, but our geometry is in
// Unity coordinates, rather than re-write all the test cases, we just convert from OpenDRIVE to Unity here
static Geometry ConstructGeometryFromOpenDriveCoordinates(
float headingDegrees, float length, float sAbsolute, float x, float z,
GeometryKind geometryKind, ArcData arcData = default, SpiralData spiralData = default,
Poly3Data poly3Data = default, ParamPoly3Data paramPoly3Data = default)
{
var pose = OpenDriveMapElementFactory.FromOpenDriveGlobalToUnity(math.radians(headingDegrees), x, z);
return(new Geometry()
{
length = length,
sRoad = sAbsolute,
startPose = pose,
geometryKind = geometryKind,
arcData = arcData,
spiralData = spiralData,
poly3Data = poly3Data,
paramPoly3Data = paramPoly3Data
});
}
/* IEnumerator ArcSequence(float arc)
* {
* float start = (180 - Mathf.CeilToInt(180 / arc) * arc) * 0.5f - 90;
* for (int i = 0; i < 3; i++)
* {
* float angle = start;
* for (int j = 0; j < 2; j++)
* {
* for (; angle <= 90; angle += 2 * arc)
* ShootArc(angle, arc);
* angle = start + arc;
* yield return waveTime;
* }
* }
* } */
IEnumerator ArcSequence(ArcData data)
{
float halfSpan = data.span * 0.5f;
for (int i = 0; i < waves; i++)
{
float angle = data.start;
for (int j = 0; j < 2; j++)
{
for (; angle <= 90; angle += data.span)
{
ShootArc(angle, data.arc);
}
angle = data.start + halfSpan;
while (angle > data.minStart)
{
angle -= data.span;
}
yield return(waveTime);
}
}
}
private static void DrawCornerArc(Graphics g,ArcData arc,eStyleBorderType border,int lineWidth, Color color)
{
using(Pen pen=CreatePen(border,lineWidth,color))
{
g.DrawArc(pen,arc.X,arc.Y,arc.Width,arc.Height,arc.StartAngle,arc.SweepAngle);
}
}
internal static ArcData GetCornerArc(Rectangle bounds, int cornerDiameter, eCornerArc corner)
{
ArcData a;
int diameter=cornerDiameter*2;
switch(corner)
{
case eCornerArc.TopLeft:
a=new ArcData(bounds.X,bounds.Y,diameter,diameter,180,90);
break;
case eCornerArc.TopRight:
a=new ArcData(bounds.Right-diameter,bounds.Y,diameter,diameter,270,90);
break;
case eCornerArc.BottomLeft:
a=new ArcData(bounds.X,bounds.Bottom-diameter,diameter,diameter,90,90);
break;
default: // eCornerArc.BottomRight:
a=new ArcData(bounds.Right-diameter,bounds.Bottom-diameter,diameter,diameter,0,90);
break;
}
return a;
}
private static void DrawCornerArc(Graphics g, ArcData arc, eStyleBorderType border, int lineWidth, Color color)
{
SmoothingMode sm = g.SmoothingMode;
if (sm == SmoothingMode.None) g.SmoothingMode = SmoothingMode.HighQuality;
using (Pen pen = CreatePen(border, lineWidth, color))
{
g.DrawArc(pen, arc.X, arc.Y, arc.Width, arc.Height, arc.StartAngle, arc.SweepAngle);
}
if (sm == SmoothingMode.None) g.SmoothingMode = sm;
}
public static Vector3 ArcNormalised(out ArcData data, Vector3 startPosition, Vector3 startNormal, Vector3 endPosition, float t)
{
//Vector3 currentNormal;
Vector3 toEndNormal;
//Vector3 toStartNormal;
Vector3 endNormal;
Vector3 startInsideNormal;
Vector3 endInsideNormal;
//Vector3 startPoint;
//Vector3 endPoint;
//Vector3 anglePoint;
Vector3 circleCentre = Vector3.zero;
Vector3 intersect = Vector3.zero;
float radius;
float angle = 0;
float properAngle = 0;
float angleToStart;
//float angleToEnd;
float worldAngle;
float worldAngleTwo;
startPosition.y = 0;
endPosition.y = 0;
startNormal.y = 0;
toEndNormal = Vector3.Normalize(endPosition - startPosition);
properAngle = AngleHalf(startNormal, toEndNormal, Vector3.up);
worldAngle = Vector3.Angle(Vector3.forward, startNormal);
worldAngleTwo = AngleHalf(Vector3.forward, startNormal, Vector3.up);
if (properAngle > 0)
{
//anglePoint = startNormal * (Vector3.Distance(startPosition, endPosition) * 0.5f);
//toStartNormal = Vector3.Normalize(startPosition - endPosition);
angle = Vector3.Angle(startNormal, toEndNormal);
endNormal = Quaternion.Euler(new Vector3(0, angle, 0)) * -toEndNormal;
endInsideNormal = Quaternion.Euler(new Vector3(0, -90, 0)) * endNormal;
startInsideNormal = Quaternion.Euler(new Vector3(0, 90, 0)) * startNormal;
angleToStart = AngleFull(startInsideNormal, endInsideNormal, Vector3.up);
}
else
{
//anglePoint = startNormal * (Vector3.Distance(startPosition, endPosition) * 0.5f);
//toStartNormal = Vector3.Normalize(startPosition - endPosition);
angle = Vector3.Angle(startNormal, toEndNormal);
endNormal = Quaternion.Euler(new Vector3(0, -angle, 0)) * -toEndNormal;
endInsideNormal = Quaternion.Euler(new Vector3(0, 90, 0)) * endNormal;
startInsideNormal = Quaternion.Euler(new Vector3(0, -90, 0)) * startNormal;
angleToStart = AngleFull(endInsideNormal, startInsideNormal, Vector3.up);
}
if (LineLineIntersection(out intersect, startPosition, startInsideNormal * 100000, endPosition, endInsideNormal * 100000))
{
circleCentre = intersect;
}
else
{
}
radius = Vector3.Distance(circleCentre, startPosition);
Vector3 newPos = Vector3.zero;
if (angleToStart < 1)
{
}
else
{
}
Vector3 tNormal;
if (LineLineIntersection(out intersect, startPosition, startInsideNormal * 100000, endPosition, endInsideNormal * 100000))
{
if (properAngle > 0)
{
if (worldAngleTwo >= 0)
{
newPos = Orbit(circleCentre, radius, -90 + t * (angleToStart) + worldAngle);
}
else
{
newPos = Orbit(circleCentre, radius, -90 + t * (angleToStart) - worldAngle);
}
}
else
{
if (worldAngleTwo >= 0)
{
newPos = Orbit2(circleCentre, radius, -90 + t * (angleToStart) - worldAngle);
}
else
{
newPos = Orbit2(circleCentre, radius, -90 + t * (angleToStart) + worldAngle);
}
}
tNormal = Vector3.Normalize(circleCentre - newPos);
}
else
{
newPos = Vector3.Lerp(startPosition, endPosition, t);
tNormal = Quaternion.Euler(0, 90, 0) * Vector3.Normalize(endPosition - startPosition);
endNormal = Vector3.Normalize(startPosition - endPosition);
}
//float newAngle =AngleFull(startNormal, endNormal, Vector3.up);
data = new ArcData(startPosition, startNormal, endPosition, -endNormal, radius, 2 * radius * Mathf.PI, angleToStart, circleCentre, ArcFunctions.ArcLength(startPosition, startNormal, endPosition), tNormal);
return(newPos);
}
