public static GpVector3 SmoothDamp(GpVector3 current, GpVector3 target, ref GpVector3 currentVelocity, float smoothTime, float maxSpeed, float deltaTime)
{
smoothTime = Mathf.Max(0.0001f, smoothTime);
var num = 2f / smoothTime;
var num2 = num * deltaTime;
var d = 1f / (1f + num2 + 0.48f * num2 * num2 + 0.235f * num2 * num2 * num2);
var vector = current - target;
var vector2 = target;
var maxLength = maxSpeed * smoothTime;
vector = ClampMagnitude(vector, maxLength);
target = current - vector;
var vector3 = (currentVelocity + num * vector) * deltaTime;
currentVelocity = (currentVelocity - num * vector3) * d;
var vector4 = target + (vector + vector3) * d;
if (Dot(vector2 - current, vector4 - vector2) > 0f)
{
vector4 = vector2;
currentVelocity = (vector4 - vector2) / deltaTime;
}
return(vector4);
}
public GpNode(int id, GpVector3 postition)
{
Id = id;
Position = postition;
Neighbors = new List <GpNode>();
}
// Note, this ignores the triangles Y component and may yield odd results
public bool IsPointInsideXZ(GpVector3 p)
{
var a = 1f / 2f * (-V2.Z * V3.X + V1.Z * (-V2.X + V3.X) + V1.X * (V2.Z - V3.Z) + V2.X * V3.Z);
var sign = a < 0f ? -1f : 1f;
var s = (V1.Z * V3.X - V1.X * V3.Z + (V3.Z - V1.Z) * p.X + (V1.X - V3.X) * p.Z) * sign;
var t = (V1.X * V2.Z - V1.Z * V2.X + (V1.Z - V2.Z) * p.X + (V2.X - V1.X) * p.Z) * sign;
return(s > 0 && t > 0 && (s + t) < 2 * a * sign);
}
public GpVector3[] GetVectorField(GpVector3 destination)
{
var vectorField = new GpVector3[Triangles.Length];
for (int i = 0; i < Triangles.Length; i++)
{
var origin = Triangles[i].Centroid();
vectorField[i] = destination - origin;
}
return(vectorField);
}
public static GpVector3 ClampMagnitude(GpVector3 vector, float maxLength)
{
GpVector3 result;
if (SqrMagnitude(vector) > maxLength * maxLength)
{
result = Normalize(vector) * maxLength;
}
else
{
result = vector;
}
return(result);
}
public static GpVector3 Project(GpVector3 vector, GpVector3 onNormal)
{
var num = Dot(onNormal, onNormal);
GpVector3 result;
if (num < float.Epsilon)
{
result = Zero;
}
else
{
result = onNormal * Dot(vector, onNormal) / num;
}
return(result);
}
public static GpVector3 Normalize(GpVector3 value)
{
var num = Magnitude(value);
GpVector3 result;
if (num > EPSILON)
{
result = value / num;
}
else
{
result = Zero;
}
return(result);
}
public static GpVector3 MoveTowards(GpVector3 current, GpVector3 target, float maxDistanceDelta)
{
var a = target - current;
var magnitude = Magnitude(a);
GpVector3 result;
if (magnitude <= maxDistanceDelta || magnitude == 0f)
{
result = target;
}
else
{
result = current + a / magnitude * maxDistanceDelta;
}
return(result);
}
public void Scale(GpVector3 scale)
{
X *= scale.X;
Y *= scale.Y;
Z *= scale.Z;
}
public static GpVector3 Max(GpVector3 lhs, GpVector3 rhs)
{
return(new GpVector3(Mathf.Max(lhs.X, rhs.X), Mathf.Max(lhs.Y, rhs.Y), Mathf.Max(lhs.Z, rhs.Z)));
}
public static float SqrMagnitude(GpVector3 a)
{
return(a.X * a.X + a.Y * a.Y + a.Z * a.Z);
}
public static float Magnitude(GpVector3 a)
{
return(Mathf.Sqrt(a.X * a.X + a.Y * a.Y + a.Z * a.Z));
}
public static float DistanceSqr(GpVector3 a, GpVector3 b)
{
var vector = new GpVector3(a.X - b.X, a.Y - b.Y, a.Z - b.Z);
return(vector.X * vector.X + vector.Y * vector.Y + vector.Z * vector.Z);
}
public static float Distance(GpVector3 a, GpVector3 b)
{
return(Mathf.Sqrt(DistanceSqr(a, b)));
}
public static GpVector3 LerpUnclamped(GpVector3 a, GpVector3 b, float t)
{
return(new GpVector3(a.X + (b.X - a.X) * t, a.Y + (b.Y - a.Y) * t, a.Z + (b.Z - a.Z) * t));
}
public static GpVector3 Scale(GpVector3 a, GpVector3 b)
{
return(new GpVector3(a.X * b.X, a.Y * b.Y, a.Z * b.Z));
}
public static GpVector3 Cross(GpVector3 lhs, GpVector3 rhs)
{
return(new GpVector3(lhs.Y * rhs.Z - lhs.Z * rhs.Y, lhs.Z * rhs.X - lhs.X * rhs.Z, lhs.X * rhs.Y - lhs.Y * rhs.X));
}
public static float Dot(GpVector3 lhs, GpVector3 rhs)
{
return(lhs.X * rhs.X + lhs.Y * rhs.Y + lhs.Z * rhs.Z);
}
public static GpVector3 Reflect(GpVector3 inDirection, GpVector3 inNormal)
{
return(-2f * Dot(inNormal, inDirection) * inNormal + inDirection);
}
public GpTriangle(GpVector3 v1, GpVector3 v2, GpVector3 v3)
{
V1 = v1;
V2 = v2;
V3 = v3;
}
public static GpVector3 ProjectOnPlane(GpVector3 vector, GpVector3 planeNormal)
{
return(vector - Project(vector, planeNormal));
}
public static float Angle(GpVector3 from, GpVector3 to)
{
return(Mathf.Acos(Mathf.Clamp(Dot(Normalize(from), Normalize(to)), -1f, 1f)) * 57.29578f);
}
public static GpVector3 Lerp(GpVector3 a, GpVector3 b, float t)
{
t = Mathf.Clamp01(t);
return(new GpVector3(a.X + (b.X - a.X) * t, a.Y + (b.Y - a.Y) * t, a.Z + (b.Z - a.Z) * t));
}