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)));
}
