/// <summary>
/// The Inverse of the matrix copied into mInv.
/// Returns false if the matrix has no inverse.
/// A matrix multipled by its inverse is the idenity.
/// Invert a 3x3 using cofactors. This is about 8 times faster than
/// the Numerical Recipes code which uses Gaussian elimination.
/// </summary>
public bool TryInverse(out Matrix3x3f mInv)
{
mInv.m00 = m11 * m22 - m12 * m21;
mInv.m01 = m02 * m21 - m01 * m22;
mInv.m02 = m01 * m12 - m02 * m11;
mInv.m10 = m12 * m20 - m10 * m22;
mInv.m11 = m00 * m22 - m02 * m20;
mInv.m12 = m02 * m10 - m00 * m12;
mInv.m20 = m10 * m21 - m11 * m20;
mInv.m21 = m01 * m20 - m00 * m21;
mInv.m22 = m00 * m11 - m01 * m10;
float det = m00 * mInv.m00 + m01 * mInv.m10 + m02 * mInv.m20;
if (FMath.IsZero(det))
{
mInv = Identity;
return(false);
}
float invDet = 1.0f / det;
mInv.m00 *= invDet; mInv.m01 *= invDet; mInv.m02 *= invDet;
mInv.m10 *= invDet; mInv.m11 *= invDet; mInv.m12 *= invDet;
mInv.m20 *= invDet; mInv.m21 *= invDet; mInv.m22 *= invDet;
return(true);
}
public static bool RayIntersectsSegment(Ray2f ray, Segment2f seg, out float s, out float t)
{
s = t = 0;
float dx = seg.A.x - ray.Position.x;
float dy = seg.A.y - ray.Position.y;
float len = Vector2f.Distance(seg.A, seg.B);
if (FMath.IsZero(len))
{
return(false);
}
Vector2f n1;
n1.x = (seg.B.x - seg.A.x) / len;
n1.y = (seg.B.y - seg.A.y) / len;
float det = n1.x * ray.Direction.y - n1.y * ray.Direction.x;
if (FMath.IsZero(det))
{
return(false);
}
s = (dy * n1.x - dx * n1.y) / det;
t = (dy * ray.Direction.x - dx * ray.Direction.y) / det;
t /= len;
return(s > 0 && t > 0 && t < 1.0f);
}
private static bool RayIntersectsSegment(Vector2f p, Vector2f n, Vector2f a, Vector2f b, out float t)
{
t = 0;
float dx = a.x - p.x;
float dy = a.y - p.y;
float len = Vector2f.Distance(a, b);
if (FMath.IsZero(len))
{
return(false);
}
Vector2f n1;
n1.x = (b.x - a.x) / len;
n1.y = (b.y - a.y) / len;
float det = n1.x * n.y - n1.y * n.x;
if (FMath.IsZero(det))
{
return(false);
}
float s = (dy * n1.x - dx * n1.y) / det;
t = (dy * n.x - dx * n.y) / det;
t /= len;
return(s >= 0 && t >= 0 && t <= 1.0f);
}
/// <summary>
/// The inverse of the matrix.
/// A matrix multipled by its inverse is the idenity.
/// </summary>
public bool TryInverse(ref Matrix4x4f mInv)
{
float det = Determinant;
if (FMath.IsZero(det))
{
return(false);
}
mInv = Adjoint * (1.0f / det);
return(true);
}
/// <summary>
/// Slerp between two vectors arc.
/// </summary>
public static Vector3f Slerp(Vector3f from, Vector3f to, float t)
{
if (t < 0.0f)
{
t = 0.0f;
}
if (t > 1.0f)
{
t = 1.0f;
}
if (t == 0.0f)
{
return(from);
}
if (t == 1.0f)
{
return(to);
}
if (to.x == from.x && to.y == from.y && to.z == from.z)
{
return(to);
}
float m = from.Magnitude * to.Magnitude;
if (FMath.IsZero(m))
{
return(Vector3f.Zero);
}
double theta = Math.Acos(Dot(from, to) / m);
if (theta == 0.0)
{
return(to);
}
double sinTheta = Math.Sin(theta);
float st1 = (float)(Math.Sin((1.0 - t) * theta) / sinTheta);
float st = (float)(Math.Sin(t * theta) / sinTheta);
Vector3f v = new Vector3f();
v.x = from.x * st1 + to.x * st;
v.y = from.y * st1 + to.y * st;
v.z = from.z * st1 + to.z * st;
return(v);
}
/// <summary>
/// The Inverse of the matrix copied into mInv.
/// Returns false if the matrix has no inverse.
/// A matrix multipled by its inverse is the idenity.
/// </summary>
public bool TryInverse(ref Matrix2x2f mInv)
{
float det = Determinant;
if (FMath.IsZero(det))
{
return(false);
}
float invDet = 1.0f / det;
mInv.m00 = m11 * invDet;
mInv.m01 = -m01 * invDet;
mInv.m10 = -m10 * invDet;
mInv.m11 = m00 * invDet;
return(true);
}
public static bool RayIntersectsRay(Ray2f ray0, Ray2f ray1, out float s, out float t)
{
s = t = 0;
float dx = ray1.Position.x - ray0.Position.x;
float dy = ray1.Position.y - ray0.Position.y;
float det = ray1.Direction.x * ray0.Direction.y - ray1.Direction.y * ray0.Direction.x;
if (FMath.IsZero(det))
{
return(false);
}
s = (dy * ray1.Direction.x - dx * ray1.Direction.y) / det;
t = (dy * ray0.Direction.x - dx * ray0.Direction.y) / det;
return(s > 0 && t > 0);
}
public static void ClosestSegmentToSegments(Segment2f seg0, Segment2f seg1, out float s, out float t)
{
Vector2f ab0 = seg0.B - seg0.A;
Vector2f ab1 = seg1.B - seg1.A;
Vector2f a01 = seg0.A - seg1.A;
float d00 = Vector2f.Dot(ab0, ab0);
float d11 = Vector2f.Dot(ab1, ab1);
float d1 = Vector2f.Dot(ab1, a01);
s = 0;
t = 0;
//Check if either or both segments degenerate into points.
if (d00 < FMath.EPS && d11 < FMath.EPS)
{
return;
}
if (d00 < FMath.EPS)
{
//First segment degenerates into a point.
s = 0;
t = FMath.Clamp01(d1 / d11);
}
else
{
float c = Vector2f.Dot(ab0, a01);
if (d11 < FMath.EPS)
{
//Second segment degenerates into a point.
s = FMath.Clamp01(-c / d00);
t = 0;
}
else
{
//The generate non degenerate case starts here.
float d2 = Vector2f.Dot(ab0, ab1);
float denom = d00 * d11 - d2 * d2;
//if segments not parallel compute closest point and clamp to segment.
if (!FMath.IsZero(denom))
{
s = FMath.Clamp01((d2 * d1 - c * d11) / denom);
}
else
{
s = 0;
}
t = (d2 * s + d1) / d11;
if (t < 0.0f)
{
t = 0.0f;
s = FMath.Clamp01(-c / d00);
}
else if (t > 1.0f)
{
t = 1.0f;
s = FMath.Clamp01((d2 - c) / d00);
}
}
}
}
