Skip to content

Commit

Permalink
Merge from 'develop' into 'main' for CLOiSim-4.6.1 (#294)
Browse files Browse the repository at this point in the history
  • Loading branch information
@hyunseok-yang
hyunseok-yang authored Jul 2, 2024
2 parents 25e955e + 41d198d commit 3ad4c75
Show file tree
Hide file tree
Showing 4 changed files with 55 additions and 37 deletions.
2 changes: 1 addition & 1 deletion Assets/Resources/RenderPipelines/UniversalRenderPipelineAsset.asset
View file
Original file line number Diff line number Diff line change
Expand Up @@ -84,7 +84,7 @@ MonoBehaviour:
blueNoise64LTex: {fileID: 2800000, guid: e3d24661c1e055f45a7560c033dbb837, type: 3}
bayerMatrixTex: {fileID: 2800000, guid: f9ee4ed84c1d10c49aabb9b210b0fc44, type: 3}
m_PrefilteringModeMainLightShadows: 3
m_PrefilteringModeAdditionalLight: 4
m_PrefilteringModeAdditionalLight: 0
m_PrefilteringModeAdditionalLightShadows: 2
m_PrefilterXRKeywords: 1
m_PrefilteringModeForwardPlus: 1
Expand Down
45 changes: 31 additions & 14 deletions Assets/Scripts/Core/Modules/SphericalCoordinates.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,8 +22,31 @@ public class WGS84

// if: WGS84 inverse flattening parameter (no units)
public const double EarthFlattening = 1d / 298.257223563d;

// Radius of the Earth (meters).
// public const double EarthRadius = 6371000.0d;
}

// public class MOON_SCS
// {
// // Radius of the Moon (meters).
// // Source: https://lunar.gsfc.nasa.gov/library/451-SCI-000958.pdf
// const double MoonRadius = 1737400.0;

// // a: Equatorial radius of the Moon.
// // Source : https://nssdc.gsfc.nasa.gov/planetary/factsheet/moonfact.html
// const double MoonAxisEquatorial = 1738100.0;

// // b: Polar radius of the Moon.
// // Source : https://nssdc.gsfc.nasa.gov/planetary/factsheet/moonfact.html
// const double MoonAxisPolar = 1736000.0;

// // if: Unitless flattening parameter for the Moon.
// // Source : https://nssdc.gsfc.nasa.gov/planetary/factsheet/moonfact.html
// const double MoonFlattening = 0.0012;
// }


public enum SurfaceType
{
// Model of reference ellipsoid for earth, based on WGS 84 standard.
Expand Down Expand Up @@ -77,8 +100,6 @@ public enum CoordinateType
private double _elevationReference = 0; // in meters
private double _heading = 0; // in radian

private double _headingOrientationOffset = 0; // in degree

public void SetLatitudeReference(in double angle)
{
_latitudeReference = angle * Mathf.Deg2Rad;
Expand All @@ -99,11 +120,11 @@ public void SetElevationReference(in double elevation)

public void SetHeadingOffset(in double angle)
{
_heading = (angle + _headingOrientationOffset) * Mathf.Deg2Rad;
_heading = angle * Mathf.Deg2Rad;
UpdateTransformation();
}

public float HeadingAngle => (float)(_heading * Mathf.Rad2Deg - _headingOrientationOffset);
public float HeadingAngle => (float)(_heading * Mathf.Rad2Deg);

void Awake()
{
Expand Down Expand Up @@ -165,22 +186,19 @@ private void UpdateTransformation()
public void SetWorldOrientation(in string orientation)
{
// world frame: world_orientation="ENU" with heading_deg=-90° == "NWU" with heading of 0°.

switch (orientation)
{
case "NWU":
_headingOrientationOffset = 0;
Debug.Log("Recommend to set ENU");
break;

case "NED":
Debug.LogWarning("need to check NED orientaion");
_headingOrientationOffset = 0;
Debug.Log("Recommend to set ENU");
break;

case "ENU":
case "":
default:
_headingOrientationOffset = -90;
break;
}
}
Expand Down Expand Up @@ -256,7 +274,6 @@ private Vector3d PositionTransform(in Vector3d position, in CoordinateType input
case CoordinateType.LOCAL:
tmpPosition.x = -position.x * cosHea + position.y * sinHea;
tmpPosition.y = -position.x * sinHea - position.y * cosHea;

goto case CoordinateType.GLOBAL;

case CoordinateType.GLOBAL:
Expand Down Expand Up @@ -290,7 +307,8 @@ private Vector3d PositionTransform(in Vector3d position, in CoordinateType input
var theta = Math.Atan((tmpPosition.z * this.ellA) / (p * this.ellB));

// Calculate latitude and longitude
var lat = Math.Atan((tmpPosition.z + Math.Pow(this.ellP, 2) * this.ellB * Math.Pow(Math.Sin(theta), 3)) / (p - Math.Pow(this.ellE, 2) * this.ellA * Math.Pow(Math.Cos(theta), 3)));
var lat = Math.Atan((tmpPosition.z + Math.Pow(this.ellP, 2) * this.ellB * Math.Pow(Math.Sin(theta), 3)) /
(p - Math.Pow(this.ellE, 2) * this.ellA * Math.Pow(Math.Cos(theta), 3)));
var lon = Math.Atan2(tmpPosition.y, tmpPosition.x);

// Recalculate radius of planet curvature at the current latitude.
Expand Down Expand Up @@ -397,7 +415,6 @@ private Vector3d VelocityTransform(in Vector3d velocity, in CoordinateType input
case CoordinateType.LOCAL:

tmpVelocity = matrixECEFToGlobal.MultiplyVector(tmpVelocity);

tmpVelocity.Set(tmpVelocity.x * cosHea - tmpVelocity.y * sinHea,
tmpVelocity.x * sinHea + tmpVelocity.y * cosHea,
tmpVelocity.z);
Expand All @@ -417,7 +434,7 @@ public void SetCoordinatesReference(in double latitudeAngle, in double longitude
_latitudeReference = latitudeAngle * Mathf.Deg2Rad;
_longitudeReference = longitudeAngle * Mathf.Deg2Rad;
_elevationReference = elevation;
_heading = (headingAngle + _headingOrientationOffset) * Mathf.Deg2Rad;
_heading = headingAngle * Mathf.Deg2Rad;

UpdateTransformation();
}
Expand All @@ -432,7 +449,7 @@ public Vector3d SphericalFromLocal(in SDF.Vector3<double> xyz)
return result;
}

/// <summary>based on right handed system</summary>
// <summary>based on right handed system</summary>
public Vector3d LocalFromSpherical(in Vector3 xyz)
{
var convertedXYZ = xyz;
Expand Down
43 changes: 22 additions & 21 deletions Assets/Scripts/Devices/GPS.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,8 +25,8 @@ public class GPS : Device
public Vector3 _sensorVelocity;

private Vector3 _previousSensorPosition;
public Vector3d _gpsCoordinates;
public Vector3d _gpsVelocity;
// public Vector3d _gpsCoordinates;
// public Vector3d _gpsVelocity;

public Dictionary<string, Noise> position_sensing_noises = new Dictionary<string, Noise>()
{
Expand All @@ -48,7 +48,7 @@ protected override void OnAwake()

_sphericalCoordinates = DeviceHelper.GetGlobalSphericalCoordinates();
_worldFrameOrientation = (Vector3.up * _sphericalCoordinates.HeadingAngle);
// Debug.Log("worldFrameOrientation=" + _worldFrameOrientation.ToString("F3"));
Debug.Log("worldFrameOrientation=" + _worldFrameOrientation.ToString("F3"));
}

protected override void OnStart()
Expand Down Expand Up @@ -85,27 +85,26 @@ void Update()
_sensorCurrentRotation = transform.rotation.eulerAngles;
}


private void ApplyNoises()
private void ApplyNoises(ref Vector3d coordinates, ref Vector3d velocity)
{
if (position_sensing_noises["horizontal"] != null)
{
position_sensing_noises["horizontal"].Apply<double>(ref _gpsCoordinates.x);
position_sensing_noises["horizontal"].Apply<double>(ref coordinates.x);
}

if (position_sensing_noises["vertical"] != null)
{
position_sensing_noises["vertical"].Apply<double>(ref _gpsCoordinates.y);
position_sensing_noises["vertical"].Apply<double>(ref coordinates.y);
}

if (velocity_sensing_noises["horizontal"] != null)
{
velocity_sensing_noises["horizontal"].Apply<double>(ref _gpsVelocity.x);
velocity_sensing_noises["horizontal"].Apply<double>(ref velocity.x);
}

if (velocity_sensing_noises["vertical"] != null)
{
velocity_sensing_noises["vertical"].Apply<double>(ref _gpsVelocity.y);
velocity_sensing_noises["vertical"].Apply<double>(ref velocity.y);
}
}

Expand All @@ -115,23 +114,25 @@ private void AssembleGPSMessage()

// Convert to global frames
var convertedPosition = Unity2SDF.Position(_worldPosition);
convertedPosition.X *= -1;
convertedPosition.Y *= -1;
var gpsCoordinates = _sphericalCoordinates.SphericalFromLocal(convertedPosition);

_gpsCoordinates = _sphericalCoordinates.SphericalFromLocal(convertedPosition);

_gps.LatitudeDeg = _gpsCoordinates.x;
_gps.LongitudeDeg = _gpsCoordinates.y;
_gps.Altitude = _gpsCoordinates.z;
_gps.LatitudeDeg = gpsCoordinates.x;
_gps.LongitudeDeg = gpsCoordinates.y;
_gps.Altitude = gpsCoordinates.z;

// Convert to global frame
var convertedVelocity = Unity2SDF.Position(_sensorVelocity);
_gpsVelocity = _sphericalCoordinates.GlobalFromLocal(convertedVelocity);
var velocityRHS = Unity2SDF.Position(_sensorVelocity);
var gpsVelocity = _sphericalCoordinates.GlobalFromLocal(velocityRHS);

// Apply noise after converting to global frame
ApplyNoises();
_gps.VelocityEast = gpsVelocity.x;
_gps.VelocityNorth = -gpsVelocity.y;
_gps.VelocityUp = gpsVelocity.z;
// Debug.Log($"{_gps.VelocityEast} {_gps.VelocityNorth} {_gps.VelocityUp}");

_gps.VelocityNorth = _gpsVelocity.x;
_gps.VelocityEast = _gpsVelocity.y;
_gps.VelocityUp = _gpsVelocity.z;
// Apply noise after converting to global frame
ApplyNoises(ref gpsCoordinates, ref gpsVelocity);
}

public void AssembleHeadingMessage()
Expand Down
2 changes: 1 addition & 1 deletion ProjectSettings/ProjectSettings.asset
View file
Original file line number Diff line number Diff line change
Expand Up @@ -139,7 +139,7 @@ PlayerSettings:
loadStoreDebugModeEnabled: 0
visionOSBundleVersion: 1.0
tvOSBundleVersion: 1.0
bundleVersion: 4.6.0
bundleVersion: 4.6.1
preloadedAssets: []
metroInputSource: 0
wsaTransparentSwapchain: 0
Expand Down

0 comments on commit 3ad4c75

Please sign in to comment.