static void UpdateWithExtensionDistance(float extensionDistance, NativeSlice <PointSampleGlobal> polygon)
{
var pose = polygon[0].pose;
polygon[0] = new PointSampleGlobal(new RigidTransform
{
pos = pose.pos + math.rotate(pose.rot, new float3(0, 0, -extensionDistance)),
rot = pose.rot
});
pose = polygon[polygon.Length - 1].pose;
polygon[polygon.Length - 1] = new PointSampleGlobal(new RigidTransform
{
pos = pose.pos + math.rotate(pose.rot, new float3(0, 0, -extensionDistance)),
rot = pose.rot
});
var middleIndex = polygon.Length / 2;
pose = polygon[middleIndex - 1].pose;
polygon[middleIndex - 1] = new PointSampleGlobal(new RigidTransform
{
pos = pose.pos + math.rotate(pose.rot, new float3(0, 0, extensionDistance)),
rot = pose.rot
});
pose = polygon[middleIndex].pose;
polygon[middleIndex] = new PointSampleGlobal(new RigidTransform
{
pos = pose.pos + math.rotate(pose.rot, new float3(0, 0, extensionDistance)),
rot = pose.rot
});
}
public SamplingStateEcsRoad(EcsRoadData road, float samplesPerMeter)
{
canMoveToNextLaneSection = true;
geometryIdx = 0;
geometryEndS = road.ecsGeometries[0].length;
laneSectionIdx = 0;
laneSectionEndS = road.ecsLaneSections.Length > 1 ? road.ecsLaneSections[1].sRoad : road.ecsRoad.length;
if (road.laneOffsets.Length > 0)
{
laneOffsetIdx = 0;
laneOffsetEndS = road.laneOffsets.Length > 1 ? road.laneOffsets[1].sRoad : road.ecsRoad.length;
}
else
{
laneOffsetIdx = -1;
laneOffsetEndS = float.MaxValue;
}
sRoadCurrent = 0.0f;
sRoadStep = 1.0f / samplesPerMeter;
sRoadLastComputed = -1f;
geometrySample = default;
laneOffset = 0f;
geometryState = new SamplingStateGeometry(road.ecsGeometries[0]);
ComputeSamples(road);
}
internal static PointSampleLocal FromGlobalToLocal(RigidTransform poseLocalInGlobal,
PointSampleGlobal pointGlobal)
{
var tfFromGlobalToLocal = math.inverse(poseLocalInGlobal);
var poseLocal3D = math.mul(tfFromGlobalToLocal, pointGlobal.pose);
// In 3D coordinates, z is forward and x is right; in 2D coordinates, x is forward and y is left
var positionLocal2D = new float2(poseLocal3D.pos.z, -poseLocal3D.pos.x);
var headingLocalRadians = GlobalOrientationToLocalHeadingRadians(poseLocal3D.rot);
return(new PointSampleLocal(positionLocal2D, headingLocalRadians));
}
private void ComputeSamples(EcsRoadData road)
{
if (Mathf.Approximately(sRoadCurrent, sRoadLastComputed))
{
Debug.LogWarning("Compute samples called for the same s-value twice.");
}
sRoadLastComputed = sRoadCurrent;
var geometry = road.ecsGeometries[geometryIdx];
//var sGeometry = sRoadCurrent - geometry.sRoad;
geometrySample = geometry.Sample(sRoadCurrent);
if (road.laneOffsets.Length > 0)
{
var laneOffsetRecord = road.laneOffsets[laneOffsetIdx];
var sLaneOffset = sRoadCurrent - laneOffsetRecord.sRoad;
laneOffset = GeometrySampling.Poly3ComputeV(laneOffsetRecord.poly3, sLaneOffset);
}
}
internal static void BuildSamplesInsideLaneSection(
EcsRoadData road, int laneId, int numSamplesSection, int outputIdx, TraversalDirection samplingDirection,
ref SamplingStateEcsRoad samplingState, ref NativeArray <PointSampleGlobal> samplesOut)
{
var numSamplesOut = samplesOut.Length;
var numEdgesToSample = math.abs(laneId);
var numSamplesEdge = numEdgesToSample == 0 ? numSamplesOut : numSamplesOut * numEdgesToSample;
// We will need to sample the center line as well, if we are not between two lane edges
var shouldSampleCenter = numEdgesToSample <= 1;
var samplesEdges = new NativeArray <PointSampleGlobal>(numSamplesEdge, Allocator.Temp);
var side = ToSide(laneId);
var sectionStart = samplingDirection == TraversalDirection.Forward ? outputIdx : numSamplesSection + outputIdx - 1;
for (var sampleNum = 0; sampleNum < numSamplesSection; sampleNum++)
{
var sampleIdx = sectionStart + sampleNum * (int)samplingDirection;
var sRoad = samplingState.sRoadLastComputed;
SampleLanesOneSide(road, samplingState, side, sRoad, sampleIdx, ref samplesEdges,
numEdgesToSample);
// Lane index is lane ID - 1 because ID's start at 1, not 0
var laneSampleIdx = ComputeLaneSampleIdx(numEdgesToSample - 1, numSamplesOut, sampleIdx);
var poseOuterEdge = samplesEdges[laneSampleIdx].pose;
RigidTransform poseInnerEdge;
if (shouldSampleCenter)
{
poseInnerEdge = SampleCenter(road, samplingState, sRoad).pose;
}
else
{
var innerEdgeIdx = ComputeLaneSampleIdx(numEdgesToSample - 2, numSamplesOut, sampleIdx);
poseInnerEdge = samplesEdges[innerEdgeIdx].pose;
}
var positionMean = math.lerp(poseInnerEdge.pos, poseOuterEdge.pos, 0.5f);
var rotationMean = math.nlerp(poseInnerEdge.rot, poseOuterEdge.rot, 0.5f);
samplesOut[sampleIdx] = new PointSampleGlobal(rotationMean, positionMean);
samplingState.Step(road);
}
samplesEdges.Dispose();
}
public bool Equals(PointSampleGlobal other)
{
return(pose.Equals(other.pose));
}
