Skip to content

Instantly share code, notes, and snippets.

@SaffronCR
Created May 22, 2017 14:24
Show Gist options
  • Save SaffronCR/b0802d102dd7f262118ac853cd5b4901 to your computer and use it in GitHub Desktop.
Save SaffronCR/b0802d102dd7f262118ac853cd5b4901 to your computer and use it in GitHub Desktop.
[C#, Unity] Math Helper Class.
using System.Runtime.InteropServices;
using UnityEngine;
public class MathUtil
{
// Evil floating point bit level hacking.
[StructLayout(LayoutKind.Explicit)]
private struct FloatIntUnion
{
[FieldOffset(0)]
public float f;
[FieldOffset(0)]
public int tmp;
}
/// <summary>
/// Rotates a point around a pivot
/// </summary>
/// <param name="point">Start position</param>
/// <param name="pivot">Pivot position</param>
/// <param name="rotation">Desired rotation around the pivot</param>
/// <returns>The position after being rotated around the pivot</returns>
public static Vector3 RotatePointAroundPivot(Vector3 point, Vector3 pivot, Quaternion rotation)
{
Vector3 dir = point - pivot;
dir = rotation * dir;
point = dir + pivot;
return point;
}
/// <summary>
/// Sample a parabola trajectory
/// </summary>
/// <param name="start">Start position</param>
/// <param name="end">End position</param>
/// <param name="height">Height of the parabola</param>
/// <param name="t">Time parameter to sample</param>
/// <returns>The position in the parabola at time t</returns>
public static Vector3 SampleParabola(Vector3 start, Vector3 end, float height, float t)
{
if (Mathf.Abs(start.y - end.y) < 0.1f)
{
// Start and end are roughly level, pretend they are - simpler solution with less steps
Vector3 travelDirection = end - start;
Vector3 result = start + t * travelDirection;
result.y += Mathf.Sin(t * Mathf.PI) * height;
return result;
}
else
{
// Start and end are not level, gets more complicated
Vector3 travelDirection = end - start;
Vector3 levelDirecteion = end - new Vector3(start.x, end.y, start.z);
Vector3 right = Vector3.Cross(travelDirection, levelDirecteion);
Vector3 up = Vector3.Cross(right, travelDirection);
if (end.y > start.y) up = -up;
Vector3 result = start + t * travelDirection;
result += (Mathf.Sin(t * Mathf.PI) * height) * up.normalized;
return result;
}
}
/// <summary>
/// Closest point on line
/// </summary>
/// <param name="vA"></param>
/// <param name="vB"></param>
/// <param name="vPoint"></param>
/// <returns></returns>
public static Vector3 ClosestPointOnLine(Vector3 vA, Vector3 vB, Vector3 vPoint)
{
Vector3 vVector1 = vPoint - vA;
Vector3 vVector2 = (vB - vA).normalized;
float d = Vector3.Distance(vA, vB);
float t = Vector3.Dot(vVector2, vVector1);
if (t <= 0)
return vA;
if (t >= d)
return vB;
Vector3 vVector3 = vVector2 * t;
Vector3 vClosestPoint = vA + vVector3;
return vClosestPoint;
}
/// <summary>
/// Determine the signed angle between two vectors, with normal 'n'
/// as the rotation axis.
/// </summary>
public static float AngleSigned(Vector3 v1, Vector3 v2, Vector3 n)
{
return Mathf.Atan2(
Vector3.Dot(n, Vector3.Cross(v1, v2)),
Vector3.Dot(v1, v2)) * Mathf.Rad2Deg;
}
/// <summary>
/// Low accuracy sqrt method for fast calculation.
/// </summary>
public static float FastSqrt(float z)
{
if (z == 0) return 0;
FloatIntUnion u;
u.tmp = 0;
u.f = z;
u.tmp -= 1 << 23; // Subtract 2^m.
u.tmp >>= 1; // Divide by 2.
u.tmp += 1 << 29; // Add ((b + 1) / 2) * 2^m.
return u.f;
}
/// <summary>
/// Returns the distance between a and b (approximately).
/// </summary>
public static float AproxDistance(Vector3 a, Vector3 b)
{
Vector3 vector = new Vector3(a.x - b.x, a.y - b.y, a.z - b.z);
return InvSqrt(vector.x * vector.x + vector.y * vector.y + vector.z * vector.z);
}
/// <summary>
/// The Infamous Unusual Fast Inverse Square Root (TM).
/// </summary>
public static float InvSqrt(float z)
{
if (z == 0) return 0;
FloatIntUnion u;
u.tmp = 0;
float xhalf = 0.5f * z;
u.f = z;
u.tmp = 0x5f375a86 - (u.tmp >> 1);
u.f = u.f * (1.5f - xhalf * u.f * u.f);
return u.f * z;
}
/// <summary>
/// Returns the distance between a and b (fast but very low accuracy).
/// </summary>
public static float FastDistance(Vector3 a, Vector3 b)
{
Vector3 vector = new Vector3(a.x - b.x, a.y - b.y, a.z - b.z);
return FastSqrt(vector.x * vector.x + vector.y * vector.y + vector.z * vector.z);
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment