//Convert coordinate to Autoware/Rviz coordinate
public static ApolloCommon.PointENU GetApolloCoordinates(Vector3 unityPos, bool dim3D = true)
{
var pointENU = new ApolloCommon.PointENU()
{
X = unityPos.x,
Y = unityPos.z,
};
if (dim3D)
{
pointENU.Z = unityPos.y;
}
return(pointENU);
}
public static ApolloCommon.PointENU GetApolloCoordinates(Vector3 unityPos, float originEasting, float originNorthing, float altitudeOffset, float angle, bool dim3D = true)
{
unityPos = Quaternion.Euler(0f, angle, 0f) * unityPos;
var pointENU = new ApolloCommon.PointENU()
{
X = unityPos.x + originEasting, Y = unityPos.z + originNorthing
};
if (dim3D)
{
pointENU.Z = unityPos.y + altitudeOffset;
}
return(pointENU);
}