forked from theSpeare/RoverScience-Revisit
-
Notifications
You must be signed in to change notification settings - Fork 2
/
GeomLib.cs
60 lines (49 loc) · 2.45 KB
/
GeomLib.cs
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
using System;
using UnityEngine;
namespace RoverScience
{
// Much of the coordinate work with latitude/longitude in this source is only functional with the work here:
// http://www.movable-type.co.uk/scripts/latlong.html
public static class GeomLib
{
public static double GetDistanceBetweenTwoPoints(CelestialBody cb, Coords _from, Coords _to)
{
double bodyRadius = cb.Radius;
double dLat = (_to.latitude - _from.latitude).ToRadians();
double dLon = (_to.longitude - _from.longitude).ToRadians();
double lat1 = _from.latitude.ToRadians();
double lat2 = _to.latitude.ToRadians();
double a = Math.Sin(dLat / 2) * Math.Sin(dLat / 2) +
Math.Sin(dLon / 2) * Math.Sin(dLon / 2) * Math.Cos(lat1) * Math.Cos(lat2);
double c = 2 * Math.Atan2(Math.Sqrt(a), Math.Sqrt(1 - a));
double d = bodyRadius * c;
return Math.Round(d, 4);
}
public static double GetBearingFromCoords(Coords target, Coords location)
{
// Rover x,y position
double dLat = (target.latitude - location.latitude).ToRadians();
double dLon = (target.longitude - location.longitude).ToRadians();
double lat1 = location.latitude.ToRadians();
double lat2 = target.latitude.ToRadians();
double y = Math.Sin(dLon) * Math.Cos(lat2);
double x = Math.Cos(lat1) * Math.Sin(lat2) -
Math.Sin(lat1) * Math.Cos(lat2) * Math.Cos(dLon);
double bearing = Math.Atan2(y, x).ToDegrees();
//bearing = (bearing + 180) % 360;
//return bearing % 360;
return (bearing + 360) % 360;
}
public static double GetRoverHeading(Vessel vessel)
{
//Vector3d coM = vessel.findLocalCenterOfMass();
Vector3d coM = vessel.localCoM;
Vector3d up = (coM - vessel.mainBody.position).normalized;
Vector3d north = Vector3d.Exclude(up, (vessel.mainBody.position +
(Vector3d)vessel.mainBody.transform.up * vessel.mainBody.Radius) - coM).normalized;
Quaternion rotationSurface = Quaternion.LookRotation(north, up);
Quaternion rotationVesselSurface = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(vessel.GetTransform().rotation) * rotationSurface);
return rotationVesselSurface.eulerAngles.y;
}
}
}