internal static PointSampleGlobal SampleCenter(Road road, SamplingStateRoad state, float sRoad)
{
var offset = state.GetLaneOffset(road, sRoad);
var geometrySample = state.GetGeometrySample(road, sRoad);
var sampleLocal = new PointSampleLocal(0, offset, 0);
return(FromLocalToGlobal(geometrySample.pose, sampleLocal));
}
/// <summary>
/// Converts a point sample from inertial coordinates to global (Unity) coordinates
/// </summary>
/// <param name="poseLocalInGlobal">Transform defining the conversion from local to global coordinates.
/// Equivalent to the geometry segments pose in global space</param>
/// <param name="pointLocal"></param>
/// <returns></returns>
internal static PointSampleGlobal FromLocalToGlobal(
RigidTransform poseLocalInGlobal, PointSampleLocal pointLocal)
{
// NOTE: In converting the forward vector from right-handed inertial with u forward to left-handed
// 3D space with z forward, we must negate the inertial y axis
var positionLocal = new float3(-pointLocal.position.y, 0, pointLocal.position.x);
var positionGlobal = math.transform(poseLocalInGlobal, positionLocal);
var yawLocal = quaternion.RotateY(-pointLocal.headingRadians);
var rotationGlobal = math.mul(poseLocalInGlobal.rot, yawLocal);
return(new PointSampleGlobal(new RigidTransform(rotationGlobal,
new float3(positionGlobal.x, positionGlobal.y, positionGlobal.z))));
}
