377 lines
12 KiB
C#
377 lines
12 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Linq;
|
|
using System.Text;
|
|
using System.Threading.Tasks;
|
|
using System.Numerics;
|
|
|
|
|
|
|
|
namespace ServerCore;
|
|
|
|
public static class Vector3Helper
|
|
{
|
|
public const float Infinity = float.MaxValue;
|
|
public const float kEpsilon = 0.00001F;
|
|
public const float kEpsilonNormalSqrt = 1e-15F;
|
|
|
|
private static readonly Vector3 zeroVector = new Vector3(0F, 0F, 0F);
|
|
private static readonly Vector3 oneVector = new Vector3(1F, 1F, 1F);
|
|
private static readonly Vector3 upVector = new Vector3(0F, 1F, 0F);
|
|
private static readonly Vector3 downVector = new Vector3(0F, -1F, 0F);
|
|
private static readonly Vector3 leftVector = new Vector3(-1F, 0F, 0F);
|
|
private static readonly Vector3 rightVector = new Vector3(1F, 0F, 0F);
|
|
private static readonly Vector3 forwardVector = new Vector3(0F, 0F, 1F);
|
|
private static readonly Vector3 backwardVector = new Vector3(0F, 0F, -1F);
|
|
private static readonly Vector3 positiveInfinityVector = new Vector3(float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity);
|
|
private static readonly Vector3 negativeInfinityVector = new Vector3(float.NegativeInfinity, float.NegativeInfinity, float.NegativeInfinity);
|
|
|
|
public static Vector3 zero { get { return zeroVector; } }
|
|
|
|
public static Vector3 normalize(this Vector3 _this)
|
|
{
|
|
return normalizeEx(_this);
|
|
}
|
|
|
|
public static Vector3 multipleEx(this Vector3 _this, float b)
|
|
{
|
|
return multiple(_this, b);
|
|
}
|
|
|
|
public static bool isZero(Vector3 v)
|
|
{
|
|
return Equals(v, zeroVector);
|
|
}
|
|
|
|
public static Vector3 deepClone(Vector3 from)
|
|
{
|
|
return new Vector3(from.X, from.Y, from.Z);
|
|
}
|
|
|
|
public static void deepCopy(Vector3 from, ref Vector3 to)
|
|
{
|
|
to.X = from.X;
|
|
to.Y = from.Y;
|
|
to.Z = from.Z;
|
|
}
|
|
|
|
public static Vector3 normalizeEx(Vector3 value)
|
|
{
|
|
float mag = magnitude(value);
|
|
if (mag > kEpsilon)
|
|
{
|
|
return new Vector3(value.X / mag, value.Y / mag, value.Z / mag);
|
|
}
|
|
else
|
|
{
|
|
return zero;
|
|
}
|
|
}
|
|
|
|
// 외적
|
|
public static Vector3 cross(Vector3 lhs, Vector3 rhs)
|
|
{
|
|
return new Vector3 (
|
|
lhs.Y * rhs.Z - lhs.Z * rhs.Y,
|
|
lhs.Z * rhs.X - lhs.X * rhs.Z,
|
|
lhs.X * rhs.Y - lhs.Y * rhs.X
|
|
);
|
|
}
|
|
|
|
// 외적을 이용한 a 점과 (from, to)방향 벡터의 수직 거리(수선의 발)
|
|
public static float pointToVectorDistance(Vector3 a, Vector3 from, Vector3 to)
|
|
{
|
|
// from->to 방향벡터
|
|
Vector3 line_vector = dir(to, from);
|
|
// from->a 방향벡터
|
|
Vector3 from_to_a_vector = dir(a, from);
|
|
// 외적 from에서 a로의 벡터와 from to 벡터의 외적
|
|
Vector3 crossed = cross(from_to_a_vector, line_vector);
|
|
// 외적 결과 벡터의 크기를 라인 백터의 크기로 나누면 직선 from->to와 a점 사이의 거리가 나온다.
|
|
return magnitude(crossed) / magnitude(line_vector);
|
|
}
|
|
// 외적을 이용한 (a, b)방향 벡터 점 p의 수직 거리(수선의 발)
|
|
public static float PointToDirDistance(Vector3 dir_a_b, Vector3 dir_a_p)
|
|
{
|
|
// 외적 a에서 p로의 방향 벡터와 a b 방향 벡터의 외적
|
|
Vector3 crossed = cross(dir_a_p, dir_a_b);
|
|
// 외적 결과 벡터의 크기를 라인 백터의 크기로 나누면 직선 a->b와 c점 사이의 거리가 나온다.
|
|
return magnitude(crossed) / magnitude(dir_a_b);
|
|
}
|
|
|
|
// 점과 선분사이의 최단 거리
|
|
public static bool pointToLineDistance(Vector3 p, Vector3 from, Vector3 to, bool is_capsule, out float dist)
|
|
{
|
|
dist = 0;
|
|
|
|
float from_to_distance = distance(from, to);
|
|
// from == to
|
|
if (0 == from_to_distance)
|
|
{
|
|
dist = distance(from, p);
|
|
|
|
return true;
|
|
}
|
|
|
|
Vector3 from_to_vector = dir(to, from);
|
|
Vector3 from_p_vector = dir(p, from);
|
|
Vector3 to_p_vector = dir(p, to);
|
|
|
|
// 수선의 발이 선분 위에 있는 경우
|
|
if (dot(from_p_vector, from_to_vector) * dot(to_p_vector, from_to_vector) <= 0)
|
|
{
|
|
// 외적을 이용한 방향 벡터와 점 사이의 수직거리를 얻는다.
|
|
dist = pointToVectorDistance(p, from, to);
|
|
|
|
return true;
|
|
}
|
|
// 수선의 발이 선분 위에 없으면 양끝점과 p의 거리 중 최단 거리를 리턴
|
|
if (is_capsule)
|
|
{
|
|
dist = Math.Min(distance(from, p), distance(to, p));
|
|
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
//내적
|
|
public static float dot(Vector3 lhs, Vector3 rhs) { return lhs.X * rhs.X + lhs.Y * rhs.Y + lhs.Z * rhs.Z; }
|
|
|
|
// 내적을 이용한 두 방향 벡터 사이의 각도
|
|
public static float angle(Vector3 a_dir, Vector3 b_dir)
|
|
{
|
|
// sqrt(a) * sqrt(b) = sqrt(a * b) -- valid for real numbers
|
|
float denominator = MathHelper.sqrt(sqrMagnitude(a_dir) * sqrMagnitude(b_dir));
|
|
if (denominator < kEpsilonNormalSqrt)
|
|
return 0F;
|
|
|
|
float dotted = MathHelper.clamp(dot(a_dir, b_dir) / denominator, -1F, 1F);
|
|
float angle = MathHelper.acos(dotted);
|
|
|
|
return angle;
|
|
}
|
|
|
|
public static Vector3 dir(Vector3 to, Vector3 from) { return new Vector3(to.X - from.X, to.Y - from.Y, to.Z - from.Z); }
|
|
|
|
public static float magnitude(Vector3 vector) { return MathHelper.sqrt(vector.X * vector.X + vector.Y * vector.Y + vector.Z * vector.Z); }
|
|
|
|
public static float sqrMagnitude(Vector3 vector) { return vector.X * vector.X + vector.Y * vector.Y + vector.Z * vector.Z; }
|
|
|
|
public static float distance(Vector3 a, Vector3 b)
|
|
{
|
|
Vector3 vec = new Vector3(a.X - b.X, a.Y - b.Y, a.Z - b.Z);
|
|
return MathHelper.sqrt(vec.X * vec.X + vec.Y * vec.Y + vec.Z * vec.Z);
|
|
}
|
|
|
|
public static float distance(Vector3 a, Vector3 b, float radius_a, float radius_b)
|
|
{
|
|
Vector3 vec = new Vector3(a.X - b.X, a.Y - b.Y, a.Z - b.Z);
|
|
float distance = MathHelper.sqrt(vec.X * vec.X + vec.Y * vec.Y + vec.Z * vec.Z);
|
|
|
|
distance = distance - (radius_a + radius_b);
|
|
distance = Math.Max(0, distance);
|
|
|
|
return distance;
|
|
}
|
|
|
|
public static bool checkDistanceXZ(Vector3 sourcePos, Vector3 targetPos, float maxDiffDist, out float distance)
|
|
{
|
|
distance = 0;
|
|
|
|
var source_pos = sourcePos;
|
|
var target_pos = targetPos;
|
|
source_pos.Y = target_pos.Y;
|
|
|
|
var diff_distance = Vector3Helper.distance(source_pos, target_pos);
|
|
if (maxDiffDist < diff_distance)
|
|
{
|
|
distance = diff_distance;
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
public static bool equals(Vector3 a, Vector3 b)
|
|
{
|
|
return a.X.Equals(b.X) && a.Y.Equals(b.Y) && a.Z.Equals(b.Z);
|
|
}
|
|
|
|
// Adds two vectors.
|
|
public static Vector3 add(Vector3 a, Vector3 b)
|
|
{
|
|
return new Vector3(a.X + b.X, a.Y + b.Y, a.Z + b.Z);
|
|
}
|
|
|
|
public static Vector3 sub(Vector3 a, Vector3 b)
|
|
{
|
|
return new Vector3(a.X - b.X, a.Y - b.Y, a.Z - b.Z);
|
|
}
|
|
|
|
public static Vector3 multiple(Vector3 a, float b)
|
|
{
|
|
return new Vector3(a.X * b, a.Y * b, a.Z * b);
|
|
}
|
|
|
|
public static Vector3 division(Vector3 a, float b)
|
|
{
|
|
return new Vector3(a.X / b, a.Y / b, a.Z / b);
|
|
}
|
|
|
|
public static void dirVectorToDegree(Vector3 dir, out float degree, Vector3 toAxis)
|
|
{
|
|
degree = 0;
|
|
|
|
if (true == Vector3Helper.equals(Vector3Helper.forward, toAxis))
|
|
{
|
|
degree = MathHelper.atan2(dir.X, dir.Z) * (180f / MathHelper.PI);
|
|
}
|
|
else if (true == Vector3Helper.equals(Vector3Helper.right, toAxis))
|
|
{
|
|
degree = MathHelper.atan2(dir.Z, dir.X) * (180f / MathHelper.PI);
|
|
}
|
|
}
|
|
|
|
public static void degreeToDirVectorWithXY(float degree, out Vector3 dir, Vector3 toAxis)
|
|
{
|
|
dir = Vector3Helper.zero;
|
|
|
|
float rad = degree * MathHelper.Deg2Rad;
|
|
|
|
if (true == Vector3Helper.Equals(Vector3Helper.forward, toAxis))
|
|
{
|
|
float x = MathHelper.sin(rad);
|
|
float y = MathHelper.cos(rad);
|
|
|
|
dir = new Vector3(x, y, 0);
|
|
}
|
|
else if (true == Vector3Helper.Equals(Vector3Helper.right, toAxis))
|
|
{
|
|
float x = MathHelper.cos(rad);
|
|
float y = MathHelper.sin(rad);
|
|
|
|
dir = new Vector3(x, y, 0);
|
|
}
|
|
}
|
|
|
|
public static void degreeToDirVectorWithXZ(float degree, out Vector3 dir, Vector3 toAxis)
|
|
{
|
|
dir = Vector3Helper.zero;
|
|
|
|
float rad = degree * MathHelper.Deg2Rad;
|
|
|
|
if (true == Vector3Helper.Equals(Vector3Helper.forward, toAxis))
|
|
{
|
|
float x = MathHelper.sin(rad);
|
|
float z = MathHelper.cos(rad);
|
|
|
|
dir = new Vector3(x, 0, z);
|
|
}
|
|
else if (true == Vector3Helper.Equals(Vector3Helper.right, toAxis))
|
|
{
|
|
float x = MathHelper.cos(rad);
|
|
float z = MathHelper.sin(rad);
|
|
|
|
dir = new Vector3(x, 0, z);
|
|
}
|
|
}
|
|
|
|
public static Vector3 rotateVectorByDegree(float degree, Vector3 baseVector, Vector3 toAxis)
|
|
{
|
|
float rad = degree * MathHelper.Deg2Rad;
|
|
|
|
if (true == Vector3Helper.Equals(Vector3Helper.forward, toAxis))
|
|
{
|
|
return rotateVectorZForwardByRadian(rad, baseVector);
|
|
}
|
|
else if (true == Vector3Helper.Equals(Vector3Helper.right, toAxis))
|
|
{
|
|
return rotateVectorXRightByRadian(rad, baseVector);
|
|
}
|
|
|
|
return Vector3Helper.zero;
|
|
}
|
|
|
|
public static Vector3 rotateVectorZForwardByRadian(float radian, Vector3 baseVector)
|
|
{
|
|
//일단 2d x, z 값만 활용 한다.
|
|
float ca = MathHelper.cos(radian);
|
|
float sa = MathHelper.sin(radian);
|
|
|
|
// z축이 forward 이므로 x,z를 바꾼다.
|
|
return new Vector3( (baseVector.Z * sa + baseVector.X * ca)
|
|
, backwardVector.Y
|
|
, (baseVector.Z * ca - baseVector.X * sa));
|
|
}
|
|
|
|
public static Vector3 rotateVectorXRightByRadian(float radian, Vector3 baseVector)
|
|
{
|
|
// 일단 2d x, z 값만 활용 한다.
|
|
float ca = MathHelper.cos(radian);
|
|
float sa = MathHelper.sin(radian);
|
|
|
|
return new Vector3( (baseVector.X * ca - baseVector.Z * sa)
|
|
, backwardVector.Y
|
|
, (baseVector.X * sa + baseVector.Z * ca));
|
|
}
|
|
|
|
// Shorthand for writing @@Vector3(0, 0, 0)@@
|
|
// Shorthand for writing @@Vector3(1, 1, 1)@@
|
|
public static Vector3 one { get { return oneVector; } }
|
|
|
|
// Shorthand for writing @@Vector3(0, 0, 1)@@
|
|
public static Vector3 forward { get { return forwardVector; } }
|
|
|
|
public static Vector3 back { get { return backwardVector; } }
|
|
|
|
// Shorthand for writing @@Vector3(0, 1, 0)@@
|
|
public static Vector3 up { get { return upVector; } }
|
|
|
|
public static Vector3 down { get { return downVector; } }
|
|
|
|
public static Vector3 left { get { return leftVector; } }
|
|
|
|
// Shorthand for writing @@Vector3(1, 0, 0)@@
|
|
public static Vector3 right { get { return rightVector; } }
|
|
|
|
public static Vector3 angleOnAxisX(float angle)
|
|
{
|
|
return new Vector3(angle, 0.0f, 0.0f);
|
|
}
|
|
|
|
public static Vector3 angleOnAxisY(float angle)
|
|
{
|
|
return new Vector3(0.0f, angle, 0.0f);
|
|
}
|
|
|
|
public static Vector3 angleOnAxisZ(float angle)
|
|
{
|
|
return new Vector3(0.0f, 0.0f, angle);
|
|
}
|
|
|
|
// Shorthand for writing @@Vector3(float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity)@@
|
|
public static Vector3 positiveInfinity { get { return positiveInfinityVector; } }
|
|
|
|
// Shorthand for writing @@Vector3(float.NegativeInfinity, float.NegativeInfinity, float.NegativeInfinity)@@
|
|
public static Vector3 negativeInfinity { get { return negativeInfinityVector; } }
|
|
|
|
public static string toString(Vector3 a)
|
|
{
|
|
return string.Format("({0:F1}, {1:F1}, {2:F1})", a.X, a.Y, a.Z);
|
|
}
|
|
|
|
// Returns a vector that is made from the smallest components of two vectors.
|
|
public static Vector3 min(Vector3 lhs, Vector3 rhs)
|
|
{
|
|
return new Vector3(MathHelper.min(lhs.X, rhs.X), MathHelper.min(lhs.Y, rhs.Y), MathHelper.min(lhs.Z, rhs.Z));
|
|
}
|
|
|
|
// Returns a vector that is made from the largest components of two vectors.
|
|
public static Vector3 max(Vector3 lhs, Vector3 rhs)
|
|
{
|
|
return new Vector3(MathHelper.max(lhs.X, rhs.X), MathHelper.max(lhs.Y, rhs.Y), MathHelper.max(lhs.Z, rhs.Z));
|
|
}
|
|
}
|