static int axisOrdering(vector a, vector b, int axis)
{
var a1 = a[axis];
var b1 = b[axis];
if (a1 < b1)
{
return(-1);
}
if (a1 > b1)
{
return(1);
}
var a2 = a[1 - axis];
var b2 = b[1 - axis];
if (a2 < b2)
{
return(-1);
}
if (a2 > b2)
{
return(1);
}
return(0);
}
/**
*
* @param _project A functor which converts vertices to a 2d
* projection.
* @param _loop The polygon loop which indices address.
* @param _vert The vertex from which distance is measured.
*
*/
public heap_ordering(Project <T> _project, FList <T> _loop, T _vert, int _axis)
{
project = _project;
loop = _loop;
p = _project(_vert);
axis = _axis;
}
public Obb2f(Obb2d v)
{
Center = new vector(v.Center);
Extents = new vector(v.Extents);
Ax = new vector(v.Ax);
Ay = new vector(v.Ay);
}
//public aabb<ndim> getAABB() {
//}
public LineSegment2(vector _v1, vector _v2)
{
v1 = _v1;
v2 = _v2;
midpoint = (v2 + v1) / 2;
half_length = (v2 - v1) / 2;
}
static int windingNumber(vertex_info begin, vector point)
{
int wn = 0;
vertex_info v = begin;
do
{
if (v.p.Y <= point.Y)
{
if (v.next.p.Y > point.Y && orient2d(v.p, v.next.p, point) > 0)
{
++wn;
}
}
else
{
if (v.next.p.Y <= point.Y && orient2d(v.p, v.next.p, point) < 0)
{
--wn;
}
}
v = v.next;
} while (v != begin);
return(wn);
}
public Obb2f(aabb aabb)
{
Center = aabb.Center;
Extents = aabb.Extents;
Ax = vector.AxisX;
Ay = vector.AxisY;
}
public Obb2f(vector center, vector extents, vector ax, vector ay)
{
Center = center;
Extents = extents;
Ax = ax;
Ay = ay;
}
public void flip()
{
var t = v1;
v1 = v2;
v2 = t;
half_length = (v2 - v1) / 2;
}
/// <summary>
/// ３次ベジェ曲線での補間を微分する
/// </summary>
/// <param name="t">パラメータ</param>
/// <param name="p0">コントロールポイント０</param>
/// <param name="p1">コントロールポイント１</param>
/// <param name="p2">コントロールポイント２</param>
/// <param name="p3">コントロールポイント３</param>
/// <returns>微分結果のベクトル</returns>
public static vector DiffInterpolate3(element t, vector p0, vector p1, vector p2, vector p3)
{
var t2 = t * t;
var ti = 1 - t;
var ti2 = ti * ti;
return(p0 * (-3 * ti2) + p1 * (9 * t2 - 12 * t + 3) + p2 * (-9 * t2 + 6 * t) + p3 * (3 * t2));
}
static element orient2d(vector a, vector b, vector c)
{
var acx = a.X - c.X;
var bcx = b.X - c.X;
var acy = a.Y - c.Y;
var bcy = b.Y - c.Y;
return(acy * bcx - acx * bcy);
}
/// <summary>
/// ３次ベジェ曲線で補間する
/// </summary>
/// <param name="t">パラメータ</param>
/// <param name="p0">コントロールポイント０</param>
/// <param name="p1">コントロールポイント１</param>
/// <param name="p2">コントロールポイント２</param>
/// <param name="p3">コントロールポイント３</param>
/// <returns>補間された座標</returns>
public static vector Interpolate3(element t, vector p0, vector p1, vector p2, vector p3)
{
var t2 = t * t;
var t3 = t2 * t;
var ti = 1 - t;
var ti2 = ti * ti;
var ti3 = ti2 * ti;
return(p0 * ti3 + p1 * (3 * ti2 * t) + p2 * (3 * ti * t2) + p3 * t3);
}
public bool Intersects(volume obb2)
{
volume obb1;
if (this.IsAabb)
{
obb1 = obb2;
obb2 = this;
}
else
{
obb1 = this;
}
var d = new vector();
var v = obb1.Center - obb2.Center;
var eax = obb2.Ax * obb2.Extents.X;
var eay = obb2.Ay * obb2.Extents.Y;
var e1 = eax + eay;
var e2 = eay - eax;
d.X = Math.Abs(obb1.Ax.Dot(e1));
d.Y = Math.Abs(obb1.Ax.Dot(e2));
if (obb1.Extents.X + d.Max() < Math.Abs(obb1.Ax.Dot(v)))
{
return(false);
}
d.X = Math.Abs(obb1.Ay.Dot(e1));
d.Y = Math.Abs(obb1.Ay.Dot(e2));
if (obb1.Extents.Y + d.Max() < Math.Abs(obb1.Ay.Dot(v)))
{
return(false);
}
eax = obb1.Ax * obb1.Extents.X;
eay = obb1.Ay * obb1.Extents.Y;
e1 = eax + eay;
e2 = eay - eax;
d.X = Math.Abs(obb2.Ax.Dot(e1));
d.Y = Math.Abs(obb2.Ax.Dot(e2));
if (obb2.Extents.X + d.Max() < Math.Abs(obb2.Ax.Dot(v)))
{
return(false);
}
d.X = Math.Abs(obb2.Ay.Dot(e1));
d.Y = Math.Abs(obb2.Ay.Dot(e2));
if (obb2.Extents.Y + d.Max() < Math.Abs(obb2.Ay.Dot(v)))
{
return(false);
}
return(true);
}
public void ElementWiseMaxSelf(vector v)
{
if (v.X > X)
{
X = v.X;
}
if (v.Y > Y)
{
Y = v.Y;
}
}
public void ElementWiseMinSelf(vector v)
{
if (v.X < X)
{
X = v.X;
}
if (v.Y < Y)
{
Y = v.Y;
}
}
public Range2f(vector min, vector max, bool normalize)
{
if (normalize)
{
vector.ElementWiseMinMax(min, max, out Min, out Max);
}
else
{
Min = min;
Max = max;
}
}
public Range2f(IEnumerable <vector> positions)
{
vector min = vector.MaxValue, max = vector.MinValue;
foreach (var p in positions)
{
min.ElementWiseMinSelf(p);
max.ElementWiseMaxSelf(p);
}
Min = min;
Max = max;
}
static public vector ElementWiseMax(vector v1, vector v2)
{
if (v2.X < v1.X)
{
v2.X = v1.X;
}
if (v2.Y < v1.Y)
{
v2.Y = v1.Y;
}
return(v2);
}
public Range2f(IEnumerable <volume> volumes)
{
vector min = vector.MaxValue, max = vector.MinValue;
foreach (var v in volumes)
{
min.ElementWiseMinSelf(v.Min);
max.ElementWiseMaxSelf(v.Max);
}
Min = min;
Max = max;
}
/**
* \brief Determine whether p is internal to the anticlockwise
* angle abc, where b is the apex of the angle.
*
* @param[in] a
* @param[in] b
* @param[in] c
* @param[in] p
*
* @return true, if p is contained in the anticlockwise angle from
* b->a to b->c. Reflex angles contain p if p lies
* on b->a or on b->c. Acute angles do not contain p
* if p lies on b->a or on b->c. This is so that
* internalToAngle(a,b,c,p) = !internalToAngle(c,b,a,p)
*/
static bool internalToAngle(vector a, vector b, vector c, vector p)
{
bool reflex = a.LessIdThan(c) ? orient2d(b, a, c) <= 0.0 : orient2d(b, c, a) > 0.0;
var d1 = orient2d(b, a, p);
var d2 = orient2d(b, c, p);
if (reflex)
{
return(d1 >= 0.0 || d2 <= 0.0);
}
else
{
return(d1 > 0.0 && d2 < 0.0);
}
}
public static void ElementWiseMinMax(vector v1, vector v2, out vector min, out vector max)
{
if (v2.X < v1.X)
{
var t = v1.X; v1.X = v2.X; v2.X = t;
}
;
if (v2.Y < v1.Y)
{
var t = v1.Y; v1.Y = v2.Y; v2.Y = t;
}
;
min = v1;
max = v2;
}
/// <summary>
/// 指定されたノード列の境界ボリューム中心座標により配列を２つに分ける
/// </summary>
/// <param name="leaves">[in] 元のノード列</param>
/// <param name="left">[out] org を原点としてノードの境界ボリューム中心座標が axis 軸方向に－側のものが追加される</param>
/// <param name="right">[out] org を原点としてノードの境界ボリューム中心座標が axis 軸方向に＋側のものが追加される</param>
/// <param name="org">[in] 原点とする座標</param>
/// <param name="axis">[in] 軸のベクトル</param>
static void Split(List <Node> leaves, List <Node> left, List <Node> right, vector org, vector axis)
{
left.Clear();
right.Clear();
for (int i = 0, n = leaves.Count; i < n; ++i)
{
if (axis.Dot(leaves[i].Volume.Center - org) <= 0)
{
left.Add(leaves[i]);
}
else
{
right.Add(leaves[i]);
}
}
}
static bool lineSegmentIntersection_simple(vector s1, vector e1, vector s2, vector e2)
{
var v = s1 - e1;
var ox = s2.Y - s1.Y;
var oy = s1.X - s2.X;
if (0 <= (v.X * ox + v.Y * oy) * (v.X * (e2.Y - s1.Y) + v.Y * (s1.X - e2.X)))
{
return(false);
}
v = s2 - e2;
if (0 <= -(v.X * ox + v.Y * oy) * (v.X * (e1.Y - s2.Y) + v.Y * (s2.X - e1.X)))
{
return(false);
}
return(true);
}
/// <summary>
/// ２次元線分同士が交差しているか調べる、交点のパラメータは計算しない（整数ベクトル使用可）
/// </summary>
/// <param name="s1">[in] 線分１の開始点</param>
/// <param name="e1">[in] 線分１の終了点</param>
/// <param name="s2">[in] 線分２の開始点</param>
/// <param name="e2">[in] 線分２の終了点</param>
/// <returns>交差しているなら true が返る</returns>
public static bool LineIntersect(vector s1, vector e1, vector s2, vector e2)
{
var v = s1 - e1;
var ox = s2.Y - s1.Y;
var oy = s1.X - s2.X;
if (0 <= (v.X * ox + v.Y * oy) * (v.X * (e2.Y - s1.Y) + v.Y * (s1.X - e2.X)))
{
return(false);
}
v = s2 - e2;
if (0 <= -(v.X * ox + v.Y * oy) * (v.X * (e1.Y - s2.Y) + v.Y * (s2.X - e1.X)))
{
return(false);
}
return(true);
}
public vector this[int index] {
get {
switch (index)
{
case 0:
return(P0);
case 1:
return(P1);
case 2:
return(P2);
case 3:
return(P3);
default:
throw new NotImplementedException();
}
}
set {
switch (index)
{
case 0:
P0 = value;
break;
case 1:
P1 = value;
break;
case 2:
P2 = value;
break;
case 3:
P3 = value;
break;
default:
throw new NotImplementedException();
}
}
}
public void ClipSelf(vector min, vector max)
{
if (X < min.X)
{
X = min.X;
}
else if (max.X < X)
{
X = max.X;
}
if (Y < min.Y)
{
Y = min.Y;
}
else if (max.Y < Y)
{
Y = max.Y;
}
}
public bool LessIdThan(vector v)
{
if (X < v.X)
{
return(true);
}
if (X > v.X)
{
return(false);
}
if (Y < v.Y)
{
return(true);
}
if (Y > v.Y)
{
return(false);
}
return(false);
}
public vector Clip(vector min, vector max)
{
vector v = this;
if (v.X < min.X)
{
v.X = min.X;
}
else if (max.X < v.X)
{
v.X = max.X;
}
if (v.Y < min.Y)
{
v.Y = min.Y;
}
else if (max.Y < v.Y)
{
v.Y = max.Y;
}
return(v);
}
/// <summary>
/// Use Newton-Raphson iteration to find better root.
/// </summary>
/// <param name="Q">Current fitted curve</param>
/// <param name="P">Digitized point</param>
/// <param name="u">Parameter value for <see cref="P"/></param>
/// <returns>パラメータ</returns>
static element NewtonRaphsonRootFind(cubicbezier Q, vector P, element u)
{
/* Compute Q(u) */
var Q_u = Q.Interpolate(u);
/* Generate control vertices for Q' */
var Q1 = new vector[3]; /* Q' and Q'' */
for (int i = 0; i <= 2; i++)
{
Q1[i] = (Q[i + 1] - Q[i]) * 3;
}
/* Generate control vertices for Q'' */
var Q2 = new vector[2];
for (int i = 0; i <= 1; i++)
{
Q2[i] = (Q1[i + 1] - Q1[i]) * 2;
}
/* Compute Q'(u) and Q''(u) */
var Q1_u = Interpolate2(u, Q1[0], Q1[1], Q1[2]);
var Q2_u = Interpolate1(u, Q2[0], Q2[1]);
/* Compute f(u)/f'(u) */
var Q_u_P = Q_u - P;
var numerator = Q_u_P.Dot(Q1_u);
var denominator = Q1_u.LengthSquare + Q_u_P.Dot(Q2_u);
if (denominator == 0)
{
return(u);
}
/* u = u - f(u)/f'(u) */
return(u - numerator / denominator);
}
/// <summary>
/// コンストラクタ、管理する範囲と分割数を指定して初期化する
/// </summary>
/// <param name="range">管理する範囲</param>
/// <param name="division">分割数</param>
public GridSpace2f(range range, vectori division)
{
var size = range.Size;
if (size.HasZero)
{
throw new InvalidOperationException("\"range\" must be non zero size.");
}
if (division.HasZero)
{
throw new InvalidOperationException("\"division\" must be non zero.");
}
var divisionf = new vector(division);
_Range = range;
_Division = division;
_CellMax = division - 1;
_Cells = new List <T> [division.Y * division.X];
_CellExtent = size / (divisionf * 2);
_TransformScale = divisionf / size;
_TransformTranslate = -range.Min;
_InvTransformScale = size / divisionf;
}
public bool Contains(vector v)
{
v.SubSelf(Center);
v = (Ax * v.X) + (Ay * v.Y);
return(Math.Abs(v.X) <= Extents.X && Math.Abs(v.Y) <= Extents.Y);
}
