/// <summary>Return the closest point on 'rect' to 'pt'</summary>
public static v2 ClosestPoint(BRect rect, v2 pt)
{
v2 lower = rect.Lower;
v2 upper = rect.Upper;
v2 closest;
if (rect.IsWithin(pt))
{
// if pt.x/pt.y > rect.sizeX/rect.sizeY then the point
// is closer to the Y edge of the rectangle
if (Math_.Abs(pt.x * rect.SizeY) > Math_.Abs(pt.y * rect.SizeX))
{
closest = new v2(pt.x, Math_.Sign(pt.y) * rect.SizeY);
}
else
{
closest = new v2(Math_.Sign(pt.x) * rect.SizeX, pt.y);
}
}
else
{
closest = new v2(
Math_.Clamp(pt.x, lower.x, upper.x),
Math_.Clamp(pt.y, lower.y, upper.y));
}
return(closest);
}
public v4(v2 xy_, float z_, float w_)
: this()
{
xy = xy_;
z = z_;
w = w_;
}
/// <summary>Create a Monic from 2 points</summary>
public static Monic FromPoints(v2 a, v2 b)
{
var A = (b.y - a.y) / (b.x - a.x);
var B = a.y - A * a.x;
return(new Monic(A, B));
}
// Make an orientation matrix from a direction.
public static m2x2 OrientationFromDirection(v2 direction, int axis)
{
var ans = m2x2.Identity;
ans.x = Normalise(direction);
ans.y = new v2(ans.x.y, -ans.x.x);
return(PermuteRotation(ans, axis));
}
/// <summary>Return the closest point between two line segments</summary>
public static void ClosestPoint(v2 s0, v2 e0, v2 s1, v2 e1, out float t0, out float t1)
{
v2 line0 = e0 - s0;
v2 line1 = e1 - s1;
v2 separation = s0 - s1;
float f = Math_.Dot(line1, separation);
float c = Math_.Dot(line0, separation);
float line0_length_sq = line0.LengthSq;
float line1_length_sq = line1.LengthSq;
// Check if either or both segments are degenerate
if (Math_.FEql(line0_length_sq, 0f) && Math_.FEql(line1_length_sq, 0f))
{
t0 = 0.0f; t1 = 0.0f; return;
}
if (Math_.FEql(line0_length_sq, 0f))
{
t0 = 0.0f; t1 = Math_.Clamp(f / line1_length_sq, 0.0f, 1.0f); return;
}
if (Math_.FEql(line1_length_sq, 0f))
{
t1 = 0.0f; t0 = Math_.Clamp(-c / line0_length_sq, 0.0f, 1.0f); return;
}
// The general non-degenerate case starts here
float b = Math_.Dot(line0, line1);
float denom = line0_length_sq * line1_length_sq - b * b; // Always non-negative
// If segments not parallel, calculate closest point on infinite line 'line0'
// to infinite line 'line1', and clamp to segment 1. Otherwise pick arbitrary t0
t0 = denom != 0.0f ? Math_.Clamp((b * f - c * line1_length_sq) / denom, 0.0f, 1.0f) : 0.0f;
// Calculate point on infinite line 'line1' closest to segment 'line0' at t0
// using t1 = Dot3(pt0 - s1, line1) / line1_length_sq = (b*t0 + f) / line1_length_sq
t1 = (b * t0 + f) / line1_length_sq;
// If t1 in [0,1] then done. Otherwise, clamp t1, recompute t0 for the new value
// of t1 using t0 = Dot3(pt1 - s0, line0) / line0_length_sq = (b*t1 - c) / line0_length_sq
// and clamped t0 to [0, 1]
if (t1 < 0.0f)
{
t1 = 0.0f; t0 = Math_.Clamp((-c) / line0_length_sq, 0.0f, 1.0f);
}
else if (t1 > 1.0f)
{
t1 = 1.0f; t0 = Math_.Clamp((b - c) / line0_length_sq, 0.0f, 1.0f);
}
}
/// <summary>
/// Return the intercept between a 2d line that passes through 'a' and 'b' and another
/// that passes through 'c' and 'd'. Returns true if the lines intersect, false if they don't.
/// Returns the point of intersect in </summary>
public static bool Intersect(v2 a, v2 b, v2 c, v2 d, out v2 intersect)
{
v2 ab = b - a;
v2 cd = d - c;
float denom = ab.x * cd.y - ab.y * cd.x;
if (Math_.FEql(denom, 0.0f))
{
intersect = v2.Zero;
return(false);
}
float e = b.x * a.y - b.y * a.x;
float f = d.x * c.y - d.y * c.x;
intersect.x = (cd.x * e - ab.x * f) / denom;
intersect.y = (cd.y * e - ab.y * f) / denom;
return(true);
}
/// <summary>Finds the closest point to the 2d line segment a->b returning the parametric value</summary>
public static float ClosestPoint(v2 a, v2 b, v2 pt)
{
v2 ab = b - a;
// Project 'pt' onto 'ab', but defer divide by 'ab.Length2Sq'
float t = Math_.Dot(pt - a, ab);
if (t <= 0.0f)
{
return(0.0f); // 'point' projects outside 'line', clamp to 0.0f
}
float denom = ab.LengthSq;
if (t >= denom)
{
return(1.0f); // 'point' projects outside 'line', clamp to 1.0f
}
return(t / denom); // 'point' projects inside 'line', do deferred divide now
}
/// <summary>Create a quadratic from 3 points</summary>
public static Quadratic FromPoints(v2 a, v2 b, v2 c)
{
//' Aa.x2 + Ba.x + C = a.y
//' Ab.x2 + Bb.x + C = a.y
//' Ac.x2 + Bc.x + C = a.y
//' => Ax = y
//' A = |a.x² a.x 1| x = |A| y = |a.y|
//' |b.x² b.x 1| |B| |b.y|
//' |c.x² c.x 1| |C| |c.y|
var M = Math_.Transpose(new m3x4(
new v4(a.x * a.x, a.x, 1, 0),
new v4(b.x * b.x, b.x, 1, 0),
new v4(c.x * c.x, c.x, 1, 0)));
var y = new v4(a.y, b.y, c.y, 0);
var x = Math_.Invert(M) * y;
return(new Quadratic(x.x, x.y, x.z));
}
/// <summary>
/// Returns the closest points between 'lhs' and 'rhs'.
/// If 'lhs' and 'rhs' overlap, returns the points of deepest penetration.</summary>
public static void ClosestPoint(BRect lhs, BRect rhs, out v2 pt0, out v2 pt1)
{
pt0 = lhs.Centre;
pt1 = rhs.Centre;
if (rhs.Centre.x > lhs.Centre.x)
{
pt0.x += lhs.Radius.x; pt1.x += rhs.Radius.x;
}
if (rhs.Centre.x < lhs.Centre.x)
{
pt0.x -= lhs.Radius.x; pt1.x -= rhs.Radius.x;
}
if (rhs.Centre.y > lhs.Centre.y)
{
pt0.y += lhs.Radius.y; pt1.y += rhs.Radius.y;
}
if (rhs.Centre.y < lhs.Centre.y)
{
pt0.y -= lhs.Radius.y; pt1.y -= rhs.Radius.y;
}
}
// Create a cubic from 4 points
public static Cubic FromPoints(v2 a, v2 b, v2 c, v2 d)
{
//' Aa.x³ + Ba.x² + Ca.x + D = a.y
//' Ab.x³ + Bb.x² + Cb.x + D = a.y
//' Ac.x³ + Bc.x² + Cc.x + D = a.y
//' Ad.x³ + Bd.x² + Cd.x + D = a.y
//' => Ax = y
//' A = |a.x² a.x 1| x = |A| y = |a.y|
//' |b.x² b.x 1| |B| |b.y|
//' |c.x² c.x 1| |C| |c.y|
var M = Math_.Transpose(new m4x4(
new v4(a.x * a.x * a.x, a.x * a.x, a.x, 1),
new v4(b.x * b.x * b.x, b.x * b.x, b.x, 1),
new v4(c.x * c.x * c.x, c.x * c.x, c.x, 1),
new v4(d.x * d.x * d.x, d.x * d.x, d.x, 1)));
var y = new v4(a.y, b.y, c.y, d.y);
var x = Math_.Invert(M) * y;
return(new Cubic(x.x, x.y, x.z, x.w));
}
//public m2x2(v2 axis_norm, v4 axis_sine_angle, float cos_angle) :this() { set(axis_norm, axis_sine_angle, cos_angle); }
//public m2x2(v2 axis_norm, float angle) :this() { set(axis_norm, angle); }
//public m2x2(v2 from, v4 to) :this() { set(from, to); }
/// <summary>Get/Set columns by index</summary>
public v2 this[int c]
{
get
{
switch (c)
{
case 0: return(x);
case 1: return(y);
}
throw new ArgumentException("index out of range", "i");
}
set
{
switch (c)
{
case 0: x = value; return;
case 1: y = value; return;
}
throw new ArgumentException("index out of range", "i");
}
}
/// <summary>Returns the squared distance from 'point' to 'brect'</summary>
public static float DistanceSq(v2 point, BRect brect)
{
float dist_sq = 0.0f;
v2 lower = brect.Lower;
v2 upper = brect.Upper;
if (point.x < lower.x)
{
dist_sq += Math_.Sqr(lower.x - point.x);
}
else if (point.x > upper.x)
{
dist_sq += Math_.Sqr(point.x - upper.x);
}
if (point.y < lower.y)
{
dist_sq += Math_.Sqr(lower.y - point.y);
}
else if (point.y > upper.y)
{
dist_sq += Math_.Sqr(point.y - upper.y);
}
return(dist_sq);
}
public static m4x4 Translation(v2 dxy, float dz)
{
return(Translation(new v4(dxy, dz, 1f)));
}
public m2x2(v2 x, v2 y) : this()
{
this.x = x;
this.y = y;
}
public v4(v2 xy_, v2 zw_)
: this()
{
xy = xy_;
zw = zw_;
}
/// <summary>Increase the size of the bounding rect by 'delta'. i.e. grows by half 'delta' in each direction</summary>
public BRect Inflate(v2 delta)
{
return(new BRect(m_centre, m_radius + delta / 2f));
}
/// <summary>Returns the bounding rectangle shifted by [dx,dy]</summary>
public BRect Shifted(v2 offset)
{
return(new BRect(m_centre + offset, m_radius));
}
/// <summary>Returns true if 'point' is within this bounding rectangle (within 'tol'erance)</summary>
public bool IsWithin(v2 point, float tol = 0f)
{
return
(Math.Abs(point.x - m_centre.x) <= m_radius.x + tol &&
Math.Abs(point.y - m_centre.y) <= m_radius.y + tol);
}
public void set(v2 centre, v2 radius)
{
m_centre = centre; m_radius = radius;
}
// Constructors
public BRect(v2 centre, v2 radius)
{
m_centre = centre; m_radius = radius;
}
public static bool Intersect(v2 a, v2 b, v2 c, v2 d)
{
v2 intersect;
return(Intersect(a, b, c, d, out intersect));
}
public v3(v2 xy_, float z_)
: this()
{
xy = xy_;
z = z_;
}
