442 lines
10 KiB
C#
442 lines
10 KiB
C#
namespace Oriels;
|
|
public static class PullRequest {
|
|
public static void BoundsDraw(Bounds b, float thickness, Color color) {
|
|
Vec3 c = Vec3.One / 2;
|
|
Vec3 ds = b.dimensions;
|
|
for (int i = 0; i < 4; i++) {
|
|
Quat q = Quat.FromAngles(i * 90, 0, 0);
|
|
Lines.Add(q * (new Vec3(0, 0, 0) - c) * ds, q * (new Vec3(0, 1, 0) - c) * ds, color, color, thickness);
|
|
Lines.Add(q * (new Vec3(0, 1, 0) - c) * ds, q * (new Vec3(1, 1, 0) - c) * ds, color, color, thickness);
|
|
Lines.Add(q * (new Vec3(1, 1, 0) - c) * ds, q * (new Vec3(1, 0, 0) - c) * ds, color, color, thickness);
|
|
|
|
// convert to linepoints
|
|
}
|
|
}
|
|
|
|
public static Vec3 Slerp(Vec3 a, Vec3 b, float t) {
|
|
float dot = Vec3.Dot(a, b);
|
|
dot = Clamp(dot, -1f, 1f);
|
|
|
|
float theta = MathF.Acos(dot) * t;
|
|
Vec3 relativeVec = b - a * dot;
|
|
relativeVec.Normalize();
|
|
|
|
return (a * MathF.Cos(theta)) + (relativeVec * MathF.Sin(theta));
|
|
}
|
|
|
|
// amplify quaternions (q * q * lerp(q.i, q, %))
|
|
|
|
public static Vec3 AngularDisplacement(Quat q) {
|
|
float angle; Vec3 axis;
|
|
ToAxisAngle(q, out axis, out angle);
|
|
return axis * angle;
|
|
// * (float)(Math.PI / 180); // radians -> degrees
|
|
// / Time.Elapsedf; // delta -> velocity
|
|
}
|
|
|
|
public static void ToAxisAngle(this Quat q, out Vec3 axis, out float angle) {
|
|
q = q.Normalized; // q.Normalize(); ?
|
|
angle = 2 * MathF.Acos(q.w);
|
|
float s = MathF.Sqrt(1 - q.w * q.w);
|
|
// float s = 2 * MathF.Asin(angle * 0.5f);
|
|
axis = Vec3.Right;
|
|
// avoid divide by zero
|
|
// + if s is close to zero then direction of axis not important
|
|
if (s > 0.001) {
|
|
axis.x = q.x / s;
|
|
axis.y = q.y / s;
|
|
axis.z = q.z / s;
|
|
}
|
|
}
|
|
|
|
// construct the quaternion that rotates one vector to another
|
|
// uses the usual trick to get the half angle
|
|
public static Quat Delta(Vec3 to, Vec3 from) {
|
|
Vec3 vec = Vec3.Cross(from, to);
|
|
return new Quat(
|
|
vec.x,
|
|
vec.y,
|
|
vec.z,
|
|
1 + Vec3.Dot(to, from )
|
|
).Normalized;
|
|
}
|
|
|
|
// Quat q;
|
|
// public static void Relative(Quat to) => q = to * q * to.Inverse;
|
|
public static Quat Relative(Quat to, Quat delta) {
|
|
return (to * delta * to.Inverse).Normalized;
|
|
}
|
|
|
|
// ?
|
|
public static Vec3 Relative(Quat to, Vec3 delta) {
|
|
return to * delta * to.Inverse;
|
|
}
|
|
|
|
// public static void LookDirection(this ref Quat q, Vec3 dir) {
|
|
// Vec3 up = Vec3.Up;
|
|
|
|
// // using AxisAngle
|
|
// Vec3 axis = Vec3.Cross(up, dir);
|
|
// float angle = MathF.Atan2(Vec3.Dot(up, dir), axis.Length);
|
|
// q = FromAxisAngle(axis.Normalized, angle);
|
|
// }
|
|
|
|
public static Quat FromAxisAngle(Vec3 axis, float angle) {
|
|
float halfAngle = angle * 0.5f;
|
|
float sin = (float)Math.Sin(halfAngle);
|
|
float cos = (float)Math.Cos(halfAngle);
|
|
return new Quat(axis.x * sin, axis.y * sin, axis.z * sin, cos).Normalized;
|
|
}
|
|
|
|
static Random r = new Random();
|
|
public static int RandomRange(int min, int max) {
|
|
return r.Next(min, max);
|
|
}
|
|
|
|
public static Vec3 Direction(Vec3 to, Vec3 from) {
|
|
return (to - from).Normalized;
|
|
}
|
|
|
|
public static Vec3 SnapToLine(this Vec3 v, Vec3 a, Vec3 b, bool clamp, out float t, float tMin = 0, float tMax = 1) {
|
|
Quat q = Quat.LookDir(Direction(b, a));
|
|
Vec3 lv = q.Inverse * (v - a);
|
|
lv.x = lv.y = 0;
|
|
Vec3 r = q * lv + a;
|
|
|
|
float d = (b - a).Length;
|
|
t = (r - a).Length / d;
|
|
if (clamp) {
|
|
t = t < tMin ? tMin : (t > tMax ? tMax : t);
|
|
r = a + (b - a) * t;
|
|
}
|
|
return r;
|
|
}
|
|
|
|
/*
|
|
// turn this into a function
|
|
Vec3 vA = new Vec3(-1, 0, 0);
|
|
Vec3 vB = new Vec3(1, 1, 1);
|
|
|
|
Vec3 vC = Input.Hand(Handed.Right).palm.position;
|
|
|
|
Quat q = Quat.LookDir((vB - vA).Normalized);
|
|
|
|
// snap vC to line vA-vB
|
|
Vec3 local = q.Inverse * (vC - vA);
|
|
local.x = 0;
|
|
local.y = 0;
|
|
vC = q * local + vA;
|
|
|
|
Lines.Add(vA, vB, new Color(1, 1, 1), 0.002f);
|
|
Mesh.Cube.Draw(matDev, Matrix.TRS(vC, q, 0.04f));
|
|
|
|
*/
|
|
|
|
// swizzle
|
|
public static Vec3 JustX(this Vec3 v) {
|
|
return new Vec3(v.x, 0, 0);
|
|
}
|
|
public static Vec3 JustY(this Vec3 v) {
|
|
return new Vec3(0, v.y, 0);
|
|
}
|
|
public static Vec3 JustZ(this Vec3 v) {
|
|
return new Vec3(0, 0, v.z);
|
|
}
|
|
|
|
// public static Vec3 Down {
|
|
// get { return new Vec3(0, -1, 0); }
|
|
// }
|
|
|
|
public static Vec3 Abs(this Vec3 v) {
|
|
return new Vec3(
|
|
MathF.Abs(v.x),
|
|
MathF.Abs(v.y),
|
|
MathF.Abs(v.z)
|
|
);
|
|
}
|
|
|
|
public static Vec3 Sign(this Vec3 v) {
|
|
return new Vec3(
|
|
MathF.Sign(v.x),
|
|
MathF.Sign(v.y),
|
|
MathF.Sign(v.z)
|
|
);
|
|
}
|
|
|
|
/// <summary>
|
|
/// a(1,1,1) b(2,2,2) t(0,1,0.5) return(1,2,1.5)
|
|
/// </summary>
|
|
public static Vec3 Splice(this Vec3 a, Vec3 b, Vec3 t, bool nor = false) {
|
|
return new Vec3(
|
|
Lerp(a.x, b.x, nor ? MathF.Sign(t.x) : t.x),
|
|
Lerp(a.y, b.y, nor ? MathF.Sign(t.y) : t.y),
|
|
Lerp(a.z, b.z, nor ? MathF.Sign(t.z) : t.z)
|
|
);
|
|
}
|
|
|
|
static Mesh meshCube = Default.MeshCube;
|
|
static Material matCube = Default.Material;
|
|
public static void BlockOut(Matrix m, Color color, Material mat = null) {
|
|
if (mat == null) {
|
|
mat = matCube;
|
|
mat.FaceCull = Cull.None;
|
|
}
|
|
meshCube.Draw(mat, m, color);
|
|
}
|
|
|
|
public static Mesh GetMesh(this Model model, string name) {
|
|
for (int i = 0; i < model.Nodes.Count; i++) {
|
|
if (model.Nodes[i].Name == name) {
|
|
return model.Nodes[i].Mesh;
|
|
}
|
|
}
|
|
Console.WriteLine("Mesh not found: " + name);
|
|
return Mesh.Quad;
|
|
}
|
|
|
|
public static void SetMat(this Material mat, int offset, Cull cull, bool depthWrite) {
|
|
mat.QueueOffset = offset;
|
|
mat.FaceCull = cull;
|
|
mat.DepthWrite = depthWrite;
|
|
}
|
|
|
|
public static Vec3 RandomInCube(Vec3 center, float size) {
|
|
Random r = new Random();
|
|
return center + new Vec3(
|
|
(r.NextSingle() - 0.5f) * size,
|
|
(r.NextSingle() - 0.5f) * size,
|
|
(r.NextSingle() - 0.5f) * size
|
|
);
|
|
}
|
|
|
|
public static float Lerp(float a, float b, float t) {
|
|
return a + (b - a) * t;
|
|
}
|
|
|
|
// public static Vec3 Slerp(Vec3 a, Vec3 b, float t) {
|
|
// // spherical linear interpolation
|
|
// float dot = Vec3.Dot(a, b);
|
|
// if (dot > 0.9995f) {
|
|
// return Vec3.Lerp(a, b, t);
|
|
// }
|
|
// float theta = (float)Math.Acos(dot);
|
|
// float sinTheta = (float)Math.Sin(theta);
|
|
// return Vec3.Lerp(a * (float)Math.Sin(theta - theta * t) / sinTheta, b * (float)Math.Sin(theta * t) / sinTheta, t);
|
|
// }
|
|
|
|
public class Noise {
|
|
const uint CAP = 4294967295;
|
|
const uint BIT_NOISE1 = 0xB5297A4D;
|
|
const uint BIT_NOISE2 = 0x68E31DA4;
|
|
const uint BIT_NOISE3 = 0x1B56C4E9;
|
|
|
|
public uint seed;
|
|
|
|
public Noise(uint seed) {
|
|
this.seed = seed;
|
|
}
|
|
|
|
int position;
|
|
public float uvalue {
|
|
get {
|
|
float v = RNG(position, seed) / (float)CAP;
|
|
position++;
|
|
return v;
|
|
}
|
|
}
|
|
|
|
public float value { // not ideal *loss of precision*
|
|
get {
|
|
return uvalue * 2 - 1;
|
|
}
|
|
}
|
|
|
|
public float D1(int pos) {
|
|
return RNG(pos, seed) / (float)CAP;
|
|
}
|
|
|
|
public float D2(int x, int y) {
|
|
// large prime number with non-boring bits
|
|
const int PRIME = 198491317;
|
|
return RNG(x + (PRIME * y), seed) / (float)CAP;
|
|
}
|
|
|
|
public float D3(int x, int y, int z) {
|
|
// large prime number with non-boring bits
|
|
const int PRIME1 = 198491317;
|
|
const int PRIME2 = 6542989;
|
|
return RNG(x + (PRIME1 * y) + (PRIME2 * z), seed) / (float)CAP;
|
|
}
|
|
|
|
public uint RNG(int pos, uint seed) {
|
|
uint mangled = (uint)pos;
|
|
mangled *= BIT_NOISE1;
|
|
mangled += seed;
|
|
mangled ^= mangled >> 8;
|
|
mangled += BIT_NOISE2;
|
|
mangled ^= mangled << 8;
|
|
mangled *= BIT_NOISE3;
|
|
mangled ^= mangled >> 8;
|
|
return mangled;
|
|
}
|
|
}
|
|
|
|
public static float Clamp01(float v) {
|
|
return MathF.Max(0, MathF.Min(1, v));
|
|
}
|
|
|
|
public static float Clamp(float v, float min, float max) {
|
|
return MathF.Max(min, MathF.Min(max, v));
|
|
}
|
|
|
|
public static float ToFloat(
|
|
ref string s,
|
|
float min = float.NegativeInfinity,
|
|
float max = float.PositiveInfinity
|
|
) {
|
|
try {
|
|
float value = Clamp(float.Parse(s), min, max);
|
|
// if clamped, update string
|
|
if (value != float.Parse(s)) {
|
|
s = value.ToString();
|
|
}
|
|
return value;
|
|
} catch {
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
public class PID {
|
|
public float p, i;
|
|
public float value;
|
|
float integral;
|
|
// float scalar = 1f;
|
|
|
|
public PID(float p = 1, float i = 0.1f) {
|
|
this.p = p;
|
|
this.i = i;
|
|
}
|
|
|
|
public float Update(float target) {
|
|
float error = value - target;
|
|
integral += error;
|
|
float delta = ((p * error) + (i * integral));
|
|
return value -= delta * Time.Elapsedf;
|
|
}
|
|
}
|
|
|
|
public class Vec3PID {
|
|
public Vec3 value, integral;
|
|
// float scalar = 1f;
|
|
|
|
public Vec3 Update(Vec3 target, float p = 1, float i = 0.1f) {
|
|
Vec3 error = value - target;
|
|
integral += error;
|
|
Vec3 delta = ((p * error) + (i * integral));
|
|
return value -= delta * Time.Elapsedf;
|
|
}
|
|
}
|
|
|
|
public class Lerper {
|
|
public float t = 0;
|
|
public float spring = 1;
|
|
public float dampen = 1;
|
|
float vel;
|
|
|
|
public void Step(float to = 1, bool bounce = false) {
|
|
float dir = to - t;
|
|
vel += dir * spring * Time.Elapsedf;
|
|
|
|
if (Math.Sign(vel) != Math.Sign(dir)) {
|
|
vel *= 1 - (dampen * Time.Elapsedf);
|
|
} else {
|
|
vel *= 1 - (dampen * 0.33f * Time.Elapsedf);
|
|
}
|
|
|
|
float newt = t + vel * Time.Elapsedf;
|
|
if (bounce && (newt < 0 || newt > 1)) {
|
|
vel *= -0.5f;
|
|
newt = Math.Clamp(newt, 0, 1);
|
|
}
|
|
|
|
t = newt;
|
|
}
|
|
|
|
public void Reset() {
|
|
t = vel = 0;
|
|
}
|
|
}
|
|
|
|
public class OneEuroFilter {
|
|
public OneEuroFilter(double minCutoff, double beta) {
|
|
firstTime = true;
|
|
this.minCutoff = minCutoff;
|
|
this.beta = beta;
|
|
|
|
xFilt = new LowpassFilter();
|
|
dxFilt = new LowpassFilter();
|
|
dcutoff = 1;
|
|
}
|
|
|
|
protected bool firstTime;
|
|
protected double minCutoff;
|
|
protected double beta;
|
|
protected LowpassFilter xFilt;
|
|
protected LowpassFilter dxFilt;
|
|
protected double dcutoff;
|
|
|
|
public double MinCutoff {
|
|
get { return minCutoff; }
|
|
set { minCutoff = value; }
|
|
}
|
|
|
|
public double Beta {
|
|
get { return beta; }
|
|
set { beta = value; }
|
|
}
|
|
|
|
public double Filter(double x, double rate) {
|
|
double dx = firstTime ? 0 : (x - xFilt.Last()) * rate;
|
|
if (firstTime) {
|
|
firstTime = false;
|
|
}
|
|
|
|
var edx = dxFilt.Filter(dx, Alpha(rate, dcutoff));
|
|
var cutoff = minCutoff + beta * Math.Abs(edx);
|
|
|
|
return xFilt.Filter(x, Alpha(rate, cutoff));
|
|
}
|
|
|
|
protected double Alpha(double rate, double cutoff) {
|
|
var tau = 1.0 / (2 * Math.PI * cutoff);
|
|
var te = 1.0 / rate;
|
|
return 1.0 / (1.0 + tau / te);
|
|
}
|
|
}
|
|
|
|
public class LowpassFilter {
|
|
public LowpassFilter() {
|
|
firstTime = true;
|
|
}
|
|
|
|
protected bool firstTime;
|
|
protected double hatXPrev;
|
|
|
|
public double Last() {
|
|
return hatXPrev;
|
|
}
|
|
|
|
public double Filter(double x, double alpha) {
|
|
double hatX = 0;
|
|
if (firstTime) {
|
|
firstTime = false;
|
|
hatX = x;
|
|
} else
|
|
hatX = alpha * x + (1 - alpha) * hatXPrev;
|
|
|
|
hatXPrev = hatX;
|
|
|
|
return hatX;
|
|
}
|
|
}
|
|
}
|