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)))); }