public static Fix64Quat FromTwoVectors(Fix64Vec3 a, Fix64Vec3 b) { // From: http://lolengine.net/blog/2014/02/24/quaternion-from-two-vectors-final Fix64 epsilon = Fix64.FromDivision(1, 1000000); Fix64 ab = Fix64Vec3.LengthSqr(a) * Fix64Vec3.LengthSqr(b); Fix64 norm_a_norm_b = Fix64.FromRaw(NativeFixedMath.Sqrt64(ab.Raw)); Fix64 real_part = norm_a_norm_b + Fix64Vec3.Dot(a, b); Fix64Vec3 v; if (real_part < (epsilon * norm_a_norm_b)) { /* If u and v are exactly opposite, rotate 180 degrees * around an arbitrary orthogonal axis. Axis normalization * can happen later, when we normalize the quaternion. */ real_part = Fix64.zero; bool cond = NativeFixedMath.Abs64(a.x.Raw) > NativeFixedMath.Abs64(a.z.Raw); v = cond ? new Fix64Vec3(-a.y, a.x, Fix64.zero) : new Fix64Vec3(Fix64.zero, -a.z, a.y); } else { /* Otherwise, build quaternion the standard way. */ v = Fix64Vec3.Cross(a, b); } return(Normalize(new Fix64Quat(v.x, v.y, v.z, real_part))); }
public static Fix64 LengthSqr(Fix64Quat a) { return(Fix64.FromRaw(NativeFixedMath.Mul64(a.RawX, a.RawX) + NativeFixedMath.Mul64(a.RawY, a.RawY) + NativeFixedMath.Mul64(a.RawZ, a.RawZ) + NativeFixedMath.Mul64(a.RawW, a.RawW))); }
//Angle in degrees between two normalized vectors public static Fix64 Anlge(Fix64Vec3 a, Fix64Vec3 b) { Fix64 dot = Fix64Vec3.Dot(a.normalized, b.normalized); Fix64 clamped = Fix64Math.Clamp(dot, -Fix64.one, Fix64.one); Fix64 rad = Fix64.FromRaw(NativeFixedMath.Acos64(clamped.Raw)); return(rad * Fix64.RadToDegree); }
public static Fix64Vec3 operator /(Fix64Vec3 a, Fix64Vec3 b) { long x = NativeFixedMath.Div64(a.RawX, b.RawX); long y = NativeFixedMath.Div64(a.RawY, b.RawY); long z = NativeFixedMath.Div64(a.RawZ, b.RawZ); Fix64Vec3 r = Make(x, y, z); return(r); }
public static Fix64Vec2 operator *(Fix64Vec2 a, Fix64Vec2 b) { long x = NativeFixedMath.Mul64(a.RawX, b.RawX); long y = NativeFixedMath.Mul64(a.RawY, b.RawY); Fix64Vec2 r = Make(x, y); return(r); }
public static Fix64Quat Inverse(Fix64Quat a) { long inv_norm = NativeFixedMath.Rcp64(LengthSqr(a).Raw); return(Make( -NativeFixedMath.Mul64(a.RawX, inv_norm), -NativeFixedMath.Mul64(a.RawY, inv_norm), -NativeFixedMath.Mul64(a.RawZ, inv_norm), NativeFixedMath.Mul64(a.RawW, inv_norm))); }
public static Fix64Quat Normalize(Fix64Quat a) { long inv_norm = NativeFixedMath.Rcp64(LengthFastest(a).Raw); Fix64 x = Fix64.FromRaw(NativeFixedMath.Mul64(a.RawX, inv_norm)); Fix64 y = Fix64.FromRaw(NativeFixedMath.Mul64(a.RawY, inv_norm)); Fix64 z = Fix64.FromRaw(NativeFixedMath.Mul64(a.RawZ, inv_norm)); Fix64 w = Fix64.FromRaw(NativeFixedMath.Mul64(a.RawW, inv_norm)); return(new Fix64Quat(x, y, z, w)); }
public static Fix64Quat Slerp(Fix64Quat q1, Fix64Quat q2, Fix64 t) { Fix64 epsilon = Fix64.FromDivision(1, 1000000); Fix64 cos_omega = q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w; bool flip = false; if (cos_omega < Fix64.zero) { flip = true; cos_omega = -cos_omega; } Fix64 s1, s2; if (cos_omega > (Fix64.one - epsilon)) { // Too close, do straight linear interpolation. s1 = Fix64.one - t; s2 = (flip) ? -t : t; } else { Fix64 omega = Fix64.FromRaw(NativeFixedMath.Acos64(cos_omega.Raw)); Fix64 inv_sin_omega = Fix64.one / Fix64.FromRaw(NativeFixedMath.Sin64(omega.Raw)); Fix64 v1 = (Fix64.one - t) * omega; Fix64 v2 = t * omega; s1 = Fix64.FromRaw(NativeFixedMath.Sin64(v1.Raw)) * inv_sin_omega; s2 = Fix64.FromRaw(NativeFixedMath.Sin64(v2.Raw)) * inv_sin_omega; s2 = (flip) ? -s2 : s2; } return(new Fix64Quat( s1 * q1.x + s2 * q2.x, s1 * q1.y + s2 * q2.y, s1 * q1.z + s2 * q2.z, s1 * q1.w + s2 * q2.w)); }
public static Fix64 Asin(Fix64 a) { return(FromRaw(NativeFixedMath.Asin64(a.Raw))); }
public static Fix64 Atan2(Fix64 a, Fix64 b) { return(FromRaw(NativeFixedMath.Atan264(a.Raw, b.Raw))); }
public static Fix64 Cos(Fix64 a) { return(FromRaw(NativeFixedMath.Cos64(a.Raw))); }
public static Fix64Vec4 operator /(Fix64Vec4 a, Fix64Vec4 b) { return(Make(NativeFixedMath.Div64(a.RawX, b.RawX), NativeFixedMath.Div64(a.RawY, b.RawY), NativeFixedMath.Div64(a.RawZ, b.RawZ), NativeFixedMath.Div64(a.RawW, b.RawW))); }
public static Fix64 RadToDeg(Fix64 a) { return(FromRaw(NativeFixedMath.Mul64(a.Raw, FixedConstants64.RAD_TO_DEGREE))); }
public static Fix64 LengthFastest(Fix64Quat a) { return(Fix64.FromRaw(NativeFixedMath.Sqrt64(LengthSqr(a).Raw))); }
public static Fix64Vec3 operator /(Fix64 a, Fix64Vec3 b) { return(Make(NativeFixedMath.Div64(a.Raw, b.RawX), NativeFixedMath.Div64(a.Raw, b.RawY), NativeFixedMath.Div64(a.Raw, b.RawZ))); }
public static Fix64Vec3 operator /(Fix64Vec3 a, Fix64 b) { return(Make(NativeFixedMath.Div64(a.RawX, b.Raw), NativeFixedMath.Div64(a.RawY, b.Raw), NativeFixedMath.Div64(a.RawZ, b.Raw))); }
public static Fix64 DegToRad(Fix64 a) { return(FromRaw(NativeFixedMath.Mul64(a.Raw, FixedConstants64.DEGREE_TO_RAD))); }
public static Fix64Vec4 operator *(Fix64Vec4 a, Fix64 b) { return(Make(NativeFixedMath.Mul64(a.RawX, b.Raw), NativeFixedMath.Mul64(a.RawY, b.Raw), NativeFixedMath.Mul64(a.RawZ, b.Raw), NativeFixedMath.Mul64(a.RawW, b.Raw))); }
public static Fix64 operator *(Fix64 a, Fix64 b) { return(FromRaw(NativeFixedMath.Mul64(a.Raw, b.Raw))); }
public static Fix64Vec4 operator *(Fix64 a, Fix64Vec4 b) { return(Make(NativeFixedMath.Mul64(a.Raw, b.RawX), NativeFixedMath.Mul64(a.Raw, b.RawY), NativeFixedMath.Mul64(a.Raw, b.RawZ), NativeFixedMath.Mul64(a.Raw, b.RawW))); }
public static Fix64 Sign(Fix64 a) { return(FromRaw(NativeFixedMath.Sign64(a.Raw))); }