|
/*
CUDAfy.NET - LGPL 2.1 License
Please consider purchasing a commerical license - it helps development, frees you from LGPL restrictions
and provides you with support. Thank you!
Copyright (C) 2011 Hybrid DSP Systems
http://www.hybriddsp.com
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
*/
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Runtime.InteropServices;
using Cudafy;
namespace Hybrid.DSP.GPS
{
/// <summary>
/// A GPS Point structure that can be used on CPU and GPU. Use the Cudafy attribute.
/// </summary>
[Cudafy]
[StructLayout(LayoutKind.Sequential)]
public struct TrackPoint
{
/// <summary>
/// Initializes a new instance of the <see cref="TrackPoint"/> struct.
/// </summary>
/// <param name="lon">The longitude.</param>
/// <param name="lat">The latitude.</param>
/// <param name="timeStamp">The time stamp.</param>
public TrackPoint(double lon, double lat, long timeStamp)
{
Longitude = lon;
Latitude = lat;
TimeStamp = timeStamp;
}
/// <summary>
/// Longitude in degrees.
/// </summary>
public double Longitude;
/// <summary>
/// Latitude in degrees.
/// </summary>
public double Latitude;
/// <summary>
/// Timestamp in ticks
/// </summary>
public long TimeStamp;
/// <summary>
/// Gets or sets the time. Since the GPU does not understand DateTime or properties
/// we flag this to be ignored by using CudafyIgnore attribute.
/// </summary>
/// <value>
/// The time.
/// </value>
[CudafyIgnore]
public DateTime Time
{
get { return new DateTime(TimeStamp); }
set { TimeStamp = value.Ticks; }
}
/// <summary>
/// All members of a struct are translated to CUDA C by default.
/// This method calculates the distance between two GPS points.
/// </summary>
/// <param name="B">The second point.</param>
/// <returns>Distance in metres.</returns>
public double DistanceTo(TrackPoint B)
{
double dDistance = Single.MinValue;
double dLat1InRad = Latitude * CONSTS.PI2;
double dLong1InRad = Longitude * CONSTS.PI2;
double dLat2InRad = B.Latitude * CONSTS.PI2;
double dLong2InRad = B.Longitude * CONSTS.PI2;
double dLongitude = dLong2InRad - dLong1InRad;
double dLatitude = dLat2InRad - dLat1InRad;
// Intermediate result a.
double a = Math.Pow(Math.Sin(dLatitude / 2.0), 2.0) +
Math.Cos(dLat1InRad) * Math.Cos(dLat2InRad) *
Math.Pow(Math.Sin(dLongitude / 2.0), 2.0);
// Intermediate result c (great circle distance in Radians).
double c = 2.0 * Math.Atan2(Math.Sqrt(a), Math.Sqrt(1.0 - a));
// Distance
dDistance = CONSTS.EARTHRADIUS * c;
return dDistance * 1000;
}
/// <summary>
/// Gets the bearing in degrees between two points.
/// </summary>
/// <param name="B">The second point.</param>
/// <returns>Bearing in degrees.</returns>
public double BearingTo(TrackPoint B)
{
double lat1 = Latitude * CONSTS.PI2;
double lat2 = B.Latitude * CONSTS.PI2;
double dLon = (B.Longitude - Longitude) * CONSTS.PI2;
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);
return (180 * Math.Atan2(y, x)) / Math.PI;
}
/// <summary>
/// Gets the great circle coordinate at the specified bearing and at a distance calculated from
/// the given speed and time span.
/// </summary>
/// <param name="bearing">The bearing in degrees.</param>
/// <param name="speed">The speed in metres/second.</param>
/// <param name="dTime">The time in ticks.</param>
/// <returns></returns>
public TrackPoint GetGreatCircleCoordinateAt(double bearing, double speed, long dTime)
{
double R = CONSTS.EARTHRADIUS;
double distance = speed * (dTime / 10000000);
double d = (distance / 1000);
double brng = ToRad(bearing);
double lon1 = ToRad(Longitude);
double lat1 = ToRad(Latitude);
double lat2 = Math.Asin(Math.Sin(lat1) * Math.Cos(d / R) +
Math.Cos(lat1) * Math.Sin(d / R) * Math.Cos(brng));
double lon2 = lon1 + Math.Atan2(Math.Sin(brng) * Math.Sin(d / R) * Math.Cos(lat1),
Math.Cos(d / R) - Math.Sin(lat1) * Math.Sin(lat2));
if (lon2 > Math.PI)
lon2 -= Math.PI * 2;
else if (lon2 <= -Math.PI)
lon2 += Math.PI * 2;
lon2 = FromRad(lon2);
lat2 = FromRad(lat2);
long newTime = TimeStamp + dTime;
return new TrackPoint(lon2, lat2, newTime);
}
/// <summary>
/// Converts to radians.
/// </summary>
/// <param name="x">Angle in degrees.</param>
/// <returns>Angle in radians.</returns>
private double ToRad(double x)
{
return x / 180 * Math.PI;
}
/// <summary>
/// Converts from radians.
/// </summary>
/// <param name="x">Angle in radians.</param>
/// <returns>Angle in radians.</returns>
private double FromRad(double x)
{
return x / Math.PI * 180;
}
}
/// <summary>
/// Some useful constants we can freely use on CPU and GPU.
/// </summary>
public static class CONSTS
{
/// <summary>
/// Radius of the earth in kilometres.
/// </summary>
public const double EARTHRADIUS = 6376.5;
/// <summary>
/// PI / 180.
/// </summary>
public const double PI2 = Math.PI / 180.0;
}
}
|
By viewing downloads associated with this article you agree to the Terms of Service and the article's licence.
If a file you wish to view isn't highlighted, and is a text file (not binary), please
let us know and we'll add colourisation support for it.