void CalculatePoints(Fix64 h, Fix64 r, ref Fix64Vec3 point1, ref Fix64Vec3 point2)
{
point1 = Fix64Vec3.zero;
point2 = Fix64Vec3.zero;
Fix64 pointDistance = h - Fix64.FromDivision(2, 1) * r;
if (pointDistance <= Fix64.zero)
{
Debug.LogError("Invalid size");
return;
}
if (Direction == ParallelCapsuleDirection3D.XAxis)
{
point1 = new Fix64Vec3(Fix64.one, Fix64.zero, Fix64.zero);
point2 = new Fix64Vec3(-Fix64.one, Fix64.zero, Fix64.zero);
}
else if (Direction == ParallelCapsuleDirection3D.YAxis)
{
point1 = new Fix64Vec3(Fix64.zero, Fix64.one, Fix64.zero);
point2 = new Fix64Vec3(Fix64.zero, -Fix64.one, Fix64.zero);
}
else
{
point1 = new Fix64Vec3(Fix64.zero, Fix64.zero, Fix64.one);
point2 = new Fix64Vec3(Fix64.zero, Fix64.zero, -Fix64.one);
}
point1 = point1 * (pointDistance / Fix64.FromDivision(2, 1));
point2 = point2 * (pointDistance / Fix64.FromDivision(2, 1));
}
void CalculatePoints(Fix64 height, Fix64 radius, ref Fix64Vec2 point1, ref Fix64Vec2 point2)
{
point1 = Fix64Vec2.zero;
point2 = Fix64Vec2.zero;
Fix64 pointDistance = height - Fix64.FromDivision(2, 1) * radius;
if (pointDistance <= Fix64.zero)
{
Debug.LogError("Invalid size");
return;
}
if (Direction == PCapsuleDirection2D.Horizontal)
{
point1 = new Fix64Vec2(Fix64.one, Fix64.zero);
point2 = new Fix64Vec2(-Fix64.one, Fix64.zero);
}
else
{
point1 = new Fix64Vec2(Fix64.zero, Fix64.one);
point2 = new Fix64Vec2(Fix64.zero, -Fix64.one);
}
point1 = point1 * (pointDistance / Fix64.FromDivision(2, 1));
point2 = point2 * (pointDistance / Fix64.FromDivision(2, 1));
}
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)));
}
//Fix64Vec2
public static Fix64Vec2 FindNearestPointOnLine(Fix64Vec2 p, Fix64Vec2 a, Fix64Vec2 b)
{
Fix64Vec2 ba = b - a;
Fix64Vec2 pa = p - a;
Fix64 d = pa.Length();
Fix64 angle = Fix64Vec2.Angle(ba, pa);
if (angle > Fix64.FromDivision(90, 1))
{
angle = Fix64.FromDivision(90, 1);
}
d = d * Fix64.Cos(angle * Fix64.DegreeToRad);
return(a + ba.normalized * d);
}
//https://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/
public Fix64Vec3 EulerAngles1()
{
Fix64 sqw = w * w;
Fix64 sqx = x * x;
Fix64 sqy = y * y;
Fix64 sqz = z * z;
Fix64 unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
Fix64 test = x * y + z * w;
Fix64 _x = Fix64.zero;
Fix64 _y = Fix64.zero;
Fix64 _z = Fix64.zero;
Fix64 cutoff = Fix64.FromDivision(499, 1000) * unit;
_z = Fix64.Atan2((Fix64)2 * y * w - (Fix64)2 * x * z, sqx - sqy - sqz + sqw);
_y = Fix64.Asin((Fix64)2 * test / unit);
_x = Fix64.Atan2((Fix64)2 * x * w - (Fix64)2 * y * z, -sqx + sqy - sqz + sqw);
//if (test >= cutoff)
//{ // singularity at north pole
// _z = (Fix64)2 * Fix64.Atan2(x, w);
// _y = Fix64.halfPi;
// _x = Fix64.zero;
//}
//else if (test < -cutoff)
//{ // singularity at south pole
// _z = -(Fix64)2 * Fix64.Atan2(x, w);
// _y = -Fix64.halfPi;
// _x = Fix64.zero;
//}
//else
//{
// _z = Fix64.Atan2((Fix64)2 * y * w - (Fix64)2 * x * z, sqx - sqy - sqz + sqw);
// _y = Fix64.Asin((Fix64)2 * test / unit);
// _x = Fix64.Atan2((Fix64)2 * x * w - (Fix64)2 * y * z, -sqx + sqy - sqz + sqw);
//}
Fix64 xDegree = Fix64.RadToDeg(_x);
Fix64 yDegree = Fix64.RadToDeg(_y);
Fix64 zDegree = Fix64.RadToDeg(_z);
return(new Fix64Vec3(xDegree, zDegree, yDegree));
}
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));
}
