155 lines
3.9 KiB
C#
155 lines
3.9 KiB
C#
//#if INTERNAL_TOOL
|
||
|
||
using System;
|
||
using System.Collections.Generic;
|
||
using System.Linq;
|
||
using System.Text;
|
||
//using GMap.NET;
|
||
using TaskTools.Utilities;
|
||
|
||
namespace TaskTools.HIL
|
||
{
|
||
public class Vector3
|
||
{
|
||
Vector3 self;
|
||
public double x;
|
||
public double y;
|
||
public double z;
|
||
|
||
public Vector3(double x = 0, double y = 0, double z = 0)
|
||
{
|
||
self = this;
|
||
this.x = x;
|
||
this.y = y;
|
||
this.z = z;
|
||
}
|
||
|
||
public Vector3(Vector3 copyme)
|
||
{
|
||
self = this;
|
||
this.x = copyme.x;
|
||
this.y = copyme.y;
|
||
this.z = copyme.z;
|
||
}
|
||
|
||
public new string ToString()
|
||
{
|
||
return String.Format("Vector3({0}, {1}, {2})", self.x,
|
||
self.y,
|
||
self.z);
|
||
}
|
||
|
||
// 王海，20150702：删除 MissionPlanner.Utilities.dll。
|
||
//public static implicit operator Vector3(PointLatLngAlt a)
|
||
//{
|
||
// return new Vector3(a.Lat,a.Lng,a.Alt);
|
||
//}
|
||
|
||
// 王海，20150702：删除 GMap.NET.Core.dll（仅此处使用其 PointLatLng）。
|
||
|
||
//public static implicit operator Vector3(PointLatLng a)
|
||
//{
|
||
// return new Vector3(a.Lat, a.Lng, 0);
|
||
//}
|
||
|
||
//public static implicit operator PointLatLng(Vector3 a)
|
||
//{
|
||
// return new PointLatLng(a.x,a.y);
|
||
//}
|
||
|
||
public static Vector3 operator +(Vector3 self, Vector3 v)
|
||
{
|
||
|
||
return new Vector3(self.x + v.x,
|
||
self.y + v.y,
|
||
self.z + v.z);
|
||
}
|
||
|
||
|
||
public static Vector3 operator -(Vector3 self, Vector3 v)
|
||
{
|
||
return new Vector3(self.x - v.x,
|
||
self.y - v.y,
|
||
self.z - v.z);
|
||
}
|
||
|
||
public static Vector3 operator -(Vector3 self)
|
||
{
|
||
return new Vector3(-self.x, -self.y, -self.z);
|
||
}
|
||
|
||
|
||
public static Vector3 operator *(Vector3 self, Vector3 v)
|
||
{
|
||
// '''dot product'''
|
||
return new Vector3(self.x * v.x + self.y * v.y + self.z * v.z);
|
||
|
||
}
|
||
|
||
public static Vector3 operator *(Vector3 self, double v)
|
||
{
|
||
return new Vector3(self.x * v,
|
||
self.y * v,
|
||
self.z * v);
|
||
}
|
||
|
||
public static Vector3 operator *(double v, Vector3 self)
|
||
{
|
||
return (self * v);
|
||
}
|
||
|
||
public static Vector3 operator /(Vector3 self, double v)
|
||
{
|
||
return new Vector3(self.x / v,
|
||
self.y / v,
|
||
self.z / v);
|
||
}
|
||
|
||
public static Vector3 operator %(Vector3 self, Vector3 v)
|
||
{
|
||
// '''cross product'''
|
||
return new Vector3(self.y * v.z - self.z * v.y,
|
||
self.z * v.x - self.x * v.z,
|
||
self.x * v.y - self.y * v.x);
|
||
}
|
||
|
||
public Vector3 copy()
|
||
{
|
||
return new Vector3(self.x, self.y, self.z);
|
||
}
|
||
|
||
|
||
public double length()
|
||
{
|
||
return Math.Sqrt(self.x * self.x + self.y * self.y + self.z * self.z);
|
||
}
|
||
|
||
public void zero()
|
||
{
|
||
self.x = self.y = self.z = 0;
|
||
}
|
||
|
||
//public double angle (Vector3 self, Vector3 v) {
|
||
// '''return the angle between this vector and another vector'''
|
||
// return Math.Acos(self * v) / (self.length() * v.length());
|
||
//}
|
||
|
||
public Vector3 normalized()
|
||
{
|
||
return self / self.length();
|
||
}
|
||
|
||
public void normalize()
|
||
{
|
||
Vector3 v = self.normalized();
|
||
self.x = v.x;
|
||
self.y = v.y;
|
||
self.z = v.z;
|
||
}
|
||
|
||
const double HALF_SQRT_2 = 0.70710678118654757;
|
||
}
|
||
}
|
||
|
||
//#endif
|