121 lines
4.5 KiB
C#
121 lines
4.5 KiB
C#
using System;
|
|
using System.Drawing;
|
|
using System.Numerics; // Vector3 & Quaternion
|
|
|
|
namespace MosswartMassacre
|
|
{
|
|
public static class Geometry
|
|
{
|
|
/* ---------- heading / quaternion helpers ---------- */
|
|
|
|
public static Quaternion HeadingToQuaternion(float deg) =>
|
|
ToQuaternion((float)Math.PI * -deg / 180.0f, 0, 0);
|
|
|
|
public static Quaternion RadiansToQuaternion(float rad) =>
|
|
ToQuaternion(rad, 0, 0);
|
|
|
|
public static double QuaternionToHeading(Quaternion q)
|
|
{
|
|
double sinr = 2 * (q.W * q.X + q.Y * q.Z);
|
|
double cosr = 1 - 2 * (q.X * q.X + q.Y * q.Y);
|
|
return Math.Atan2(sinr, cosr);
|
|
}
|
|
|
|
public static double CalculateHeading(Vector3 start, Vector3 target)
|
|
{
|
|
var dy = target.Y - start.Y;
|
|
var dx = target.X - start.X;
|
|
return (360 - (Math.Atan2(dy, dx) * 180 / Math.PI) + 90) % 360;
|
|
}
|
|
|
|
// yaw (Z) - pitch (Y) - roll (X)
|
|
public static Quaternion ToQuaternion(float yaw, float pitch, float roll)
|
|
{
|
|
float cy = (float)Math.Cos(yaw * 0.5f); float sy = (float)Math.Sin(yaw * 0.5f);
|
|
float cp = (float)Math.Cos(pitch * 0.5f); float sp = (float)Math.Sin(pitch * 0.5f);
|
|
float cr = (float)Math.Cos(roll * 0.5f); float sr = (float)Math.Sin(roll * 0.5f);
|
|
|
|
return new Quaternion(
|
|
cy * cp * sr - sy * sp * cr, // X
|
|
sy * cp * sr + cy * sp * cr, // Y
|
|
sy * cp * cr - cy * sp * sr, // Z
|
|
cy * cp * cr + sy * sp * sr // W
|
|
);
|
|
}
|
|
|
|
/* ---------- landblock / coordinate helpers ---------- */
|
|
|
|
public static uint GetLandblockFromCoordinates(double ew, double ns)
|
|
{
|
|
ns = (ns - 0.5) * 10.0;
|
|
ew = (ew - 0.5) * 10.0;
|
|
|
|
uint basex = (uint)(ew + 0x400);
|
|
uint basey = (uint)(ns + 0x400);
|
|
|
|
byte bx = (byte)(basex >> 3);
|
|
byte by = (byte)(basey >> 3);
|
|
byte cx = (byte)(basex & 7);
|
|
byte cy = (byte)(basey & 7);
|
|
|
|
int block = (bx << 8) | by;
|
|
int cell = (cx << 3) | cy;
|
|
|
|
return (uint)((block << 16) | (cell + 1));
|
|
}
|
|
|
|
public static float LandblockToEW(uint landcell, float xOff)
|
|
{
|
|
uint l = (landcell & 0xFF000000) / 0x200000;
|
|
return (float)(((xOff / 24) + l - 1019.5) / 10);
|
|
}
|
|
|
|
public static float LandblockToNS(uint landcell, float yOff)
|
|
{
|
|
uint l = (landcell & 0x00FF0000) / 0x2000;
|
|
return (float)(((yOff / 24) + l - 1019.5) / 10);
|
|
}
|
|
|
|
public static float EWToLandblock(uint landcell, float ew)
|
|
{
|
|
uint l = (landcell & 0xFF000000) / 0x200000;
|
|
return (float)(((ew * 10) - l + 1019.5) * 24);
|
|
}
|
|
|
|
public static float NSToLandblock(uint landcell, float ns)
|
|
{
|
|
uint l = (landcell & 0x00FF0000) / 0x2000;
|
|
return (float)(((ns * 10) - l + 1019.5) * 24);
|
|
}
|
|
|
|
public static int LandblockXDifference(uint orig, uint dest) =>
|
|
(int)(((dest >> 24) - (orig >> 24)) * 192);
|
|
|
|
public static int LandblockYDifference(uint orig, uint dest) =>
|
|
(int)((((dest << 8) >> 24) - ((orig << 8) >> 24)) * 192);
|
|
|
|
/* ---------- misc helpers ---------- */
|
|
|
|
public static float Distance2d(float x1, float y1, float x2, float y2) =>
|
|
(float)Math.Sqrt(Math.Pow(x2 - x1, 2) + Math.Pow(y2 - y1, 2));
|
|
|
|
public static bool LineIntersectsRect(Point p1, Point p2, Rectangle r) =>
|
|
LineIntersectsLine(p1, p2, new Point(r.X, r.Y), new Point(r.Right, r.Y)) ||
|
|
LineIntersectsLine(p1, p2, new Point(r.Right, r.Y), new Point(r.Right, r.Bottom)) ||
|
|
LineIntersectsLine(p1, p2, new Point(r.Right, r.Bottom), new Point(r.X, r.Bottom)) ||
|
|
LineIntersectsLine(p1, p2, new Point(r.X, r.Bottom), new Point(r.X, r.Y)) ||
|
|
(r.Contains(p1) && r.Contains(p2));
|
|
|
|
public static bool LineIntersectsLine(Point a1, Point a2, Point b1, Point b2)
|
|
{
|
|
float q = (a1.Y - b1.Y) * (b2.X - b1.X) - (a1.X - b1.X) * (b2.Y - b1.Y);
|
|
float d = (a2.X - a1.X) * (b2.Y - b1.Y) - (a2.Y - a1.Y) * (b2.X - b1.X);
|
|
if (d == 0) return false;
|
|
|
|
float r = q / d;
|
|
q = (a1.Y - b1.Y) * (a2.X - a1.X) - (a1.X - b1.X) * (a2.Y - a1.Y);
|
|
float s = q / d;
|
|
return (r >= 0 && r <= 1 && s >= 0 && s <= 1);
|
|
}
|
|
}
|
|
}
|