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;
}
if (Z < min.Z)
{
Z = min.Z;
}
else if (max.Z < Z)
{
Z = max.Z;
}
if (W < min.W)
{
W = min.W;
}
else if (max.W < W)
{
W = max.W;
}
}
public void MulSelf(vector v)
{
X *= v.X;
Y *= v.Y;
Z *= v.Z;
W *= v.W;
}
public void DivSelf(vector v)
{
X /= v.X;
Y /= v.Y;
Z /= v.Z;
W /= v.W;
}
public void SubSelf(vector v)
{
X -= v.X;
Y -= v.Y;
Z -= v.Z;
W -= v.W;
}
public void AddSelf(vector v)
{
X += v.X;
Y += v.Y;
Z += v.Z;
W += v.W;
}
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;
}
;
if (v2.Z < v1.Z)
{
var t = v1.Z; v1.Z = v2.Z; v2.Z = t;
}
;
if (v2.W < v1.W)
{
var t = v1.W; v1.W = v2.W; v2.W = t;
}
;
min = v1;
max = v2;
}
/// <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));
}
public Obb4f(aabb aabb)
{
Center = aabb.Center;
Extents = aabb.Extents;
Ax = vector.AxisX;
Ay = vector.AxisY;
Az = vector.AxisZ;
Aw = vector.AxisW;
}
public Obb4f(vector center, vector extents, vector ax, vector ay, vector az, vector aw)
{
Center = center;
Extents = extents;
Ax = ax;
Ay = ay;
Az = az;
Aw = aw;
}
public Obb4f(Obb4d v)
{
Center = new vector(v.Center);
Extents = new vector(v.Extents);
Ax = new vector(v.Ax);
Ay = new vector(v.Ay);
Az = new vector(v.Az);
Aw = new vector(v.Aw);
}
/// <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 Range4f(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;
}
public Range4f(vector min, vector max, bool normalize)
{
if (normalize)
{
vector.ElementWiseMinMax(min, max, out Min, out Max);
}
else
{
Min = min;
Max = max;
}
}
public Range4f(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;
}
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 ElementWiseMinSelf(vector v)
{
if (v.X < X)
{
X = v.X;
}
if (v.Y < Y)
{
Y = v.Y;
}
if (v.Z < Z)
{
Z = v.Z;
}
if (v.W < W)
{
W = v.W;
}
}
public void ElementWiseMaxSelf(vector v)
{
if (v.X > X)
{
X = v.X;
}
if (v.Y > Y)
{
Y = v.Y;
}
if (v.Z > Z)
{
Z = v.Z;
}
if (v.W > W)
{
W = v.W;
}
}
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;
}
if (v2.Z < v1.Z)
{
v2.Z = v1.Z;
}
if (v2.W < v1.W)
{
v2.W = v1.W;
}
return(v2);
}
/// <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);
}
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;
}
if (v.Z < min.Z)
{
v.Z = min.Z;
}
else if (max.Z < v.Z)
{
v.Z = max.Z;
}
if (v.W < min.W)
{
v.W = min.W;
}
else if (max.W < v.W)
{
v.W = max.W;
}
return(v);
}
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);
}
if (Z < v.Z)
{
return(true);
}
if (Z > v.Z)
{
return(false);
}
if (W < v.W)
{
return(true);
}
if (W > v.W)
{
return(false);
}
return(false);
}
public Range4f(vector min, vector max)
{
Min = min;
Max = max;
}
public Range4f(vector position)
{
Min = position;
Max = position;
}
public void ExpandSelf(vector v)
{
Min.SubSelf(v);
Max.AddSelf(v);
}
public volume Expand(vector v)
{
return(new volume(Min - v, Max + v));
}
public void MergeSelf(vector v)
{
Min.ElementWiseMinSelf(v);
Max.ElementWiseMaxSelf(v);
}
public volume Merge(vector v)
{
return(new volume(vector.ElementWiseMin(Min, v), vector.ElementWiseMax(Max, v)));
}
/// <summary>
/// 最小二乗法を用いて指定範囲のベジェコントロールポイントを探す
/// </summary>
/// <param name="d">頂点列</param>
/// <param name="first">範囲開始インデックス</param>
/// <param name="last">範囲終了インデックス</param>
/// <param name="uPrime">指定範囲内のパラメータ</param>
/// <param name="tHat1">範囲開始部分のベクトル</param>
/// <param name="tHat2">範囲終了部分のベクトル</param>
/// <returns>３次ベジェ曲線</returns>
static cubicbezier GenerateBezier(vector[] d, int first, int last, element[] uPrime, vector tHat1, vector tHat2)
{
var nPts = last - first + 1;
var tHat1LenDiv = tHat1.Length;
var tHat2LenDiv = tHat2.Length;
if (tHat1LenDiv != 0)
{
tHat1LenDiv = 1 / tHat1LenDiv;
}
if (tHat2LenDiv != 0)
{
tHat2LenDiv = 1 / tHat2LenDiv;
}
/* Compute the A's */
var A = new vector[nPts, 2]; /* Precomputed rhs for eqn */
for (int i = 0; i < nPts; i++)
{
var u = uPrime[i];
var ui = 1 - u;
var b1 = 3 * u * ui * ui;
var b2 = 3 * u * u * ui;
A[i, 0] = tHat1 * (b1 * tHat1LenDiv);
A[i, 1] = tHat2 * (b2 * tHat2LenDiv);
}
/* Create the C and X matrices */
var C = new element[2, 2]; /* Matrix C */
var X = new element[2]; /* Matrix X */
vector tmp; /* Utility variable */
for (int i = 0; i < nPts; i++)
{
C[0, 0] += A[i, 0].Dot(A[i, 0]);
C[0, 1] += A[i, 0].Dot(A[i, 1]);
C[1, 0] = C[0, 1];
C[1, 1] += A[i, 1].Dot(A[i, 1]);
var df = d[first];
var dl = d[last];
tmp = d[first + i] - Interpolate3(uPrime[i], df, df, dl, dl);
X[0] += A[i, 0].Dot(tmp);
X[1] += A[i, 1].Dot(tmp);
}
/* Compute the determinants of C and X */
var det_C0_C1 = C[0, 0] * C[1, 1] - C[1, 0] * C[0, 1];
var det_C0_X = C[0, 0] * X[1] - C[1, 0] * X[0];
var det_X_C1 = X[0] * C[1, 1] - X[1] * C[0, 1];
/* Finally, derive alpha values */
var alpha_l = det_C0_C1 == 0 ? 0 : det_X_C1 / det_C0_C1;
var alpha_r = det_C0_C1 == 0 ? 0 : det_C0_X / det_C0_C1;
/* If alpha negative, use the Wu/Barsky heuristic (see text) */
/* (if alpha is 0, you get coincident control points that lead to
* divide by zero in any subsequent NewtonRaphsonRootFind() call. */
var bezCurve = new cubicbezier();
var segLength = (d[last] - d[first]).Length;
var epsilon = (element)1.0e-6 * segLength;
if (alpha_l < epsilon || alpha_r < epsilon)
{
/* fall back on standard (probably inaccurate) formula, and subdivide further if needed. */
element dist = segLength / 3;
bezCurve.P0 = d[first];
bezCurve.P3 = d[last];
bezCurve.P1 = bezCurve.P0 + tHat1 * (dist * tHat1LenDiv);
bezCurve.P2 = bezCurve.P3 + tHat2 * (dist * tHat2LenDiv);
return(bezCurve);
}
/* First and last control points of the Bezier curve are */
/* positioned exactly at the first and last data points */
/* Control points 1 and 2 are positioned an alpha distance out */
/* on the tangent vectors, left and right, respectively */
bezCurve.P0 = d[first];
bezCurve.P3 = d[last];
bezCurve.P1 = bezCurve.P0 + tHat1 * (alpha_l * tHat1LenDiv);
bezCurve.P2 = bezCurve.P3 + tHat2 * (alpha_r * tHat2LenDiv);
return(bezCurve);
}
public Range4f(Range4d v)
{
Min = new vector(v.Min);
Max = new vector(v.Max);
}
public bool Contains(vector v)
{
return(Min <= v && v <= Max);
}
