public static float RemapFromSeedDecimal(float min, float max, System.Random randomSeed)
{
float zeroToOneValue = (float)randomSeed.NextDouble();
float minToMaxValue = ExtMathf.Remap(zeroToOneValue, 0, 1, min, max);
return(minToMaxValue);
}
/// <summary>
/// here we have a min & max, we remap the random 0,1 value finded to this min&max
/// warning: we cast it into an int at the end
/// use:
/// System.Random seed = ExtRandom.Seedrandom("hash");
/// int randomInt = ExtRandom.RemapFromSeed(0, 50, seed);
/// </summary>
public static int RemapFromSeed(double min, double max, System.Random randomSeed)
{
double zeroToOneValue = randomSeed.NextDouble();
int minToMaxValue = (int)ExtMathf.Remap(zeroToOneValue, 0, 1, min, max);
return(minToMaxValue);
}
/// <summary>
/// Function returns for given axis min vertex index and breadth along it.
/// </summary>
public int GetAlongAxisData(Vector3 axis, out MinMaxPair breadthData)
{
Pair <int, float> minVertex = new Pair <int, float>(-1, float.MaxValue);
breadthData = new MinMaxPair();
Vector3 centroid = Centroid;
for (int i = 0; i < mDataCloud.Count; i++)
{
Vector3 point = mDataCloud[i];
Vector3 direction = point - centroid;
float distance = Vector3.Dot(direction, axis);
breadthData.CheckAgainst(distance);
if (ExtMathf.Less(distance, minVertex.Second))
{
minVertex.First = i;
minVertex.Second = distance;
}
}
return(minVertex.First);
}
public void AlgorithmBruteDirection()
{
var dataCloud = mConvexHull3.DataCloud;
var points = mConvexHull3.Vertices;
const int X = 128;
const int Y = 128;
mVolume = float.MaxValue;
for (int y = 0; y < Y; y++)
{
for (int x = 0; x < X; x++)
{
float fx = (float)x / (X - 1) * 2f - 1f;
float fy = (float)y / (Y - 1) * 2f - 1f;
float sqLength = fx * fx + fy * fy;
if (sqLength > 1f)
{
continue;
}
float fz = Mathf.Sqrt(1f - sqLength);
Vector3 zAxis = new Vector3(fx, fy, fz);
Vector3 xAxis;
Vector3 yAxis;
ExtMathf.AxesFromAxis(zAxis, out xAxis, out yAxis);
BoxFromNormalAxes(xAxis, yAxis, zAxis);
}
}
}
public static void TestExtMath()
{
float a = 1f;
float b = 1f + 1e-6f;
float c = 1f + 1e-4f;
Debug.Assert(ExtMathf.Equal(a, b));
Debug.Assert(!ExtMathf.Equal(a, c));
Debug.Assert(ExtMathf.Greater(c, a));
Debug.Assert(!ExtMathf.Greater(b, a));
Debug.Assert(ExtMathf.Less(a, c));
Debug.Assert(!ExtMathf.Less(a, b));
Vector3 A = new Vector3(-1, 0, -1);
Vector3 B = new Vector3(1, 0, 1);
Vector3 C = new Vector3(0, 1, 0);
Vector3 D = new Vector3(0, (1f / 3f), 0);
Debug.Assert(ExtMathf.Centroid(A, B, C) == D);
Vector3 xAxis = new Vector3(Random.Range(-1f, 1f), Random.Range(-1f, 1f), Random.Range(-1f, 1f)).normalized;
Vector3 yAxis;
Vector3 zAxis;
ExtMathf.AxesFromAxis(xAxis, out yAxis, out zAxis);
Debug.Assert(ExtMathf.Equal(Vector3.Cross(yAxis, zAxis).normalized, xAxis));
Debug.Assert(ExtMathf.Equal(Vector3.Cross(zAxis, xAxis).normalized, yAxis));
Debug.Assert(ExtMathf.Equal(Vector3.Cross(xAxis, yAxis).normalized, zAxis));
}
public static double RemapFromSeedDecimal(double min, double max, System.Random randomSeed)
{
double zeroToOneValue = randomSeed.NextDouble();
double minToMaxValue = ExtMathf.Remap(zeroToOneValue, 0, 1, min, max);
return(minToMaxValue);
}
private void ComputeCircumcircleInfos()
{
_a = ExtMathf.Determinant3x3(P1.Pos, P2.Pos, P3.Pos)
- ExtMathf.Determinant3x3(P1.Pos, P2.Pos, P4.Pos)
+ ExtMathf.Determinant3x3(P1.Pos, P3.Pos, P4.Pos)
- ExtMathf.Determinant3x3(P2.Pos, P3.Pos, P4.Pos);
float n1 = P1.Pos.sqrMagnitude;
float n2 = P2.Pos.sqrMagnitude;
float n3 = P3.Pos.sqrMagnitude;
float n4 = P4.Pos.sqrMagnitude;
float Dx = ExtMathf.Determinant3x3(new Vector3(n1, P1.Y, P1.Z), new Vector3(n2, P2.Y, P2.Z), new Vector3(n3, P3.Y, P3.Z))
- ExtMathf.Determinant3x3(new Vector3(n1, P1.Y, P1.Z), new Vector3(n2, P2.Y, P2.Z), new Vector3(n4, P4.Y, P4.Z))
+ ExtMathf.Determinant3x3(new Vector3(n1, P1.Y, P1.Z), new Vector3(n3, P3.Y, P3.Z), new Vector3(n4, P4.Y, P4.Z))
- ExtMathf.Determinant3x3(new Vector3(n2, P2.Y, P2.Z), new Vector3(n3, P3.Y, P3.Z), new Vector3(n4, P4.Y, P4.Z));
float Dy = -(ExtMathf.Determinant3x3(new Vector3(n1, P1.X, P1.Z), new Vector3(n2, P2.X, P2.Z), new Vector3(n3, P3.X, P3.Z))
- ExtMathf.Determinant3x3(new Vector3(n1, P1.X, P1.Z), new Vector3(n2, P2.X, P2.Z), new Vector3(n4, P4.X, P4.Z))
+ ExtMathf.Determinant3x3(new Vector3(n1, P1.X, P1.Z), new Vector3(n3, P3.X, P3.Z), new Vector3(n4, P4.X, P4.Z))
- ExtMathf.Determinant3x3(new Vector3(n2, P2.X, P2.Z), new Vector3(n3, P3.X, P3.Z), new Vector3(n4, P4.X, P4.Z)));
float Dz = ExtMathf.Determinant3x3(new Vector3(n1, P1.X, P1.Y), new Vector3(n2, P2.X, P2.Y), new Vector3(n3, P3.X, P3.Y))
- ExtMathf.Determinant3x3(new Vector3(n1, P1.X, P1.Y), new Vector3(n2, P2.X, P2.Y), new Vector3(n4, P4.X, P4.Y))
+ ExtMathf.Determinant3x3(new Vector3(n1, P1.X, P1.Y), new Vector3(n3, P3.X, P3.Y), new Vector3(n4, P4.X, P4.Y))
- ExtMathf.Determinant3x3(new Vector3(n2, P2.X, P2.Y), new Vector3(n3, P3.X, P3.Y), new Vector3(n4, P4.X, P4.Y));
_d = new Vector3(Dx, Dy, Dz);
_c = n1 * ExtMathf.Determinant3x3(P2.Pos, P3.Pos, P4.Pos)
- n2 * ExtMathf.Determinant3x3(P1.Pos, P3.Pos, P4.Pos)
+ n3 * ExtMathf.Determinant3x3(P1.Pos, P2.Pos, P4.Pos)
- n4 * ExtMathf.Determinant3x3(P1.Pos, P2.Pos, P3.Pos);
}
public float GetCurrentSpeedForwardClamped01()
{
float currentVelocity = entityController.GetActualAccelerationForward().magnitude;
float velocityRemapped = ExtMathf.Remap(currentVelocity, 0, 10, 0, 1);
//Debug.Log("velocity forward not mapped: " + currentVelocity);
return(Mathf.Clamp01(velocityRemapped));
}
public float GetCurrentSpeedClamped01()
{
float currentVelocity = entityController.GetActualVelocity();
float velocityRemapped = ExtMathf.Remap(currentVelocity, 0, 10, 0, 1);
//Debug.Log("velocity remapped: " + velocityRemapped + "(actual: " + currentVelocity + ")");
return(Mathf.Clamp01(velocityRemapped));
}
/// <summary>
/// Returns relative position of point 'V' to this Triangle.
/// +1 - point lies in front of the triangle.
/// 0 - point lies on the triangle.
/// -1 - point lies behind the triangle.
/// </summary>
public int CalculateRelativePosition(Vector3 P)
{
float distance = CalculateDistance(P);
return(ExtMathf.Greater(distance, 0f) ?
1 : (ExtMathf.Less(distance, 0f) ?
-1 : 0));
}
/// <summary>
/// Returns relative position of point 'V' to this Edge, assuming space to be 2D.
/// +1 - point lies above the line.
/// 0 - point lies on the line.
/// -1 - point lies below the line.
/// </summary>
public int CalculateRelative2DPosition(Vector3 P)
{
float d = (P.x - V[0].x) * (V[1].y - V[0].y) - (P.y - V[0].y) * (V[1].x - V[0].x);
return(ExtMathf.Greater(d, 0f) ?
1 : (ExtMathf.Less(d, 0f) ?
-1 : 0));
}
public Triangle(Vector3 V, Vector3 N)
{
Vector3 xAxis;
Vector3 yAxis;
ExtMathf.AxesFromAxis(N, out xAxis, out yAxis);
Initialize(V + xAxis, V, V + yAxis);
}
/// <summary>
/// Checks whether point 'V' lies on a line that passes through this Edge.
/// </summary>
public bool CalculateIfIsOnLine(Vector3 P)
{
float dV = (V[0] - V[1]).sqrMagnitude;
float dP1 = (V[0] - P).sqrMagnitude;
float dP2 = (P - V[1]).sqrMagnitude;
float dP = dP1 + dP2;
return(ExtMathf.Equal(dV, dP));
}
/// <summary>
/// move up-down in the dolly
/// </summary>
private void InputDolly()
{
Vector2 dirInput = _cameraInput;
if (Mathf.Abs(dirInput.y) >= _deadZoneVerti)
{
float remapedInput = ExtMathf.Remap(Mathf.Abs(dirInput.y), _deadZoneVerti, 1f, 0f, 1f);
_followPercent -= _speedDolly * remapedInput * Mathf.Sign(dirInput.y) * Time.deltaTime;
_followPercent = Mathf.Clamp(_followPercent, 0, 1f);
}
_follow.SetPercent(_followPercent);
}
/// <summary>
/// Function for each axis returns it's min vertex index and breadth along it.
/// </summary>
public void GetAlongAxesData(
Vector3 xAxis, Vector3 yAxis, Vector3 zAxis,
out Pair <int, MinMaxPair> xAxisData,
out Pair <int, MinMaxPair> yAxisData,
out Pair <int, MinMaxPair> zAxisData
)
{
Pair <int, float> minVertexX = new Pair <int, float>(0, float.MaxValue);
Pair <int, float> minVertexY = new Pair <int, float>(0, float.MaxValue);
Pair <int, float> minVertexZ = new Pair <int, float>(0, float.MaxValue);
MinMaxPair breadthX = new MinMaxPair();
MinMaxPair breadthY = new MinMaxPair();
MinMaxPair breadthZ = new MinMaxPair();
Vector3 centroid = Centroid;
for (int i = 0; i < mDataCloud.Count; i++)
{
Vector3 point = mDataCloud[i];
Vector3 direction = point - centroid;
float xDistance = Vector3.Dot(direction, xAxis);
float yDistance = Vector3.Dot(direction, yAxis);
float zDistance = Vector3.Dot(direction, zAxis);
breadthX.CheckAgainst(xDistance);
breadthY.CheckAgainst(yDistance);
breadthZ.CheckAgainst(zDistance);
if (ExtMathf.Less(xDistance, minVertexX.Second))
{
minVertexX.First = i;
minVertexX.Second = xDistance;
}
if (ExtMathf.Less(yDistance, minVertexY.Second))
{
minVertexY.First = i;
minVertexY.Second = yDistance;
}
if (ExtMathf.Less(zDistance, minVertexZ.Second))
{
minVertexZ.First = i;
minVertexZ.Second = zDistance;
}
}
xAxisData = new Pair <int, MinMaxPair>(minVertexX.First, breadthX);
yAxisData = new Pair <int, MinMaxPair>(minVertexY.First, breadthY);
zAxisData = new Pair <int, MinMaxPair>(minVertexZ.First, breadthZ);
}
private Vector3 GetClosestPoint(Vector3 k)
{
Vector3 vK1 = k - _p1;
float tx = Vector3.Dot(vK1, _v41) / _v41Squared;
float tz = Vector3.Dot(vK1, _v21) / _v21Squared;
tx = ExtMathf.SetBetween(tx, 0, 1);
tz = ExtMathf.SetBetween(tz, 0, 1);
Vector3 closestPoint = tx * _v41 + tz * _v21 + _p1;
return(closestPoint);
}
/// <summary>
/// return a value from 0 to 1, representing the "player input", with rb acceleration
/// </summary>
/// <returns></returns>
public float GetMagnitudeAcceleration()
{
float playerInput = entityAction.GetMagnitudeInput();
//TODO: a super lerp;
//min: minAcceleration
//max: speedMove
//current: currentSpeedMove
float remapCurrentSpeed = ExtMathf.Remap(currentSpeedMove, minAcceleration, speedMove, 0, 1);
return(remapCurrentSpeed * playerInput);
}
/// <summary>
/// tel to zoom
/// </summary>
/// <param name="zoomRatio">between -1 and 1</param>
public void InputZoom(float speedRatio = 1f)
{
if (Mathf.Abs(_trigerZoom) >= _deadZoneZoom)
{
float remapedInput = ExtMathf.Remap(Mathf.Abs(_trigerZoom), _deadZoneVerti, 1f, 0f, 1f);
float localScaleAllAxis = _toScale.localScale.x;
localScaleAllAxis += _speedZoom * remapedInput * speedRatio * Mathf.Sign(_trigerZoom) * Time.deltaTime;
localScaleAllAxis = Mathf.Clamp(localScaleAllAxis, _minMaxZoom.x, _minMaxZoom.y);
_toScale.localScale = new Vector3(localScaleAllAxis, localScaleAllAxis, localScaleAllAxis);
SetAimPosition();
}
}
public void AlgorithmRotatingCalipers()
{
var edges = mConvexHull2.Edges;
var dataCloud = mConvexHull2.DataCloud;
var vertices = mConvexHull2.Vertices;
if (edges.Count < 3)
{
Debugger.Get.Log("Insufficient points to form a triangle.", DebugOption.SBB2);
return;
}
mArea = float.MaxValue;
foreach (Edge edge in edges)
{
Vector3 edgeDirection = edge.V[1] - edge.V[0];
float edgeAngle = Mathf.Atan2(edgeDirection.y, edgeDirection.x);
// x = min, y = max
MinMaxPair X = new MinMaxPair();
MinMaxPair Y = new MinMaxPair();
foreach (int p in vertices)
{
Vector3 rotatedPoint = ExtMathf.Rotation2D(dataCloud[p], -edgeAngle);
X.CheckAgainst(rotatedPoint.x);
Y.CheckAgainst(rotatedPoint.y);
}
float newArea = X.Delta * Y.Delta;
if (newArea < mArea)
{
mArea = newArea;
mRectangle.Extents = new Vector2(X.Delta, Y.Delta);
mRectangle.Corner = ExtMathf.Rotation2D(new Vector3(X.Min, Y.Min), edgeAngle);
mRectangle.X = ExtMathf.Rotation2D(Vector3.right, edgeAngle).normalized;
mRectangle.Y = ExtMathf.Rotation2D(Vector3.up, edgeAngle).normalized;
}
}
}
/// <summary>
/// rotate left-right
/// </summary>
private void InputRotate()
{
//start or continue ease rotate
Vector2 dirInput = _cameraInput;
//if margin turn is ok for HORIZ move
if (Mathf.Abs(dirInput.x) >= _deadZoneHoriz)
{
_easeRotate.StartOrContinue();
float remapedInput = ExtMathf.Remap(Mathf.Abs(dirInput.x), _deadZoneHoriz, 1f, 0f, 1f) * Mathf.Sign(dirInput.x);
_toRotate.Rotate(0, _easeRotate.EvaluateWithDeltaTime() * remapedInput, 0);
}
else
{
_easeRotate.BackToTime();
}
}
public static bool IsCloseXToClampAmount(Quaternion q, float minX, float maxX, float margin = 2)
{
if (q.w == 0)
{
return(true);
}
q.x /= q.w;
q.y /= q.w;
q.z /= q.w;
q.w = 1.0f;
float angleX = 2.0f * Mathf.Rad2Deg * Mathf.Atan(q.x);
if (ExtMathf.IsClose(angleX, minX, margin) ||
ExtMathf.IsClose(angleX, maxX, margin))
{
return(true);
}
return(false);
}
public void CalculateEmptyCellIndexBasedOnPosition(float heightItem, float margin)
{
if (!IsDragging ||
HowManyCellAreDisplayed < 2 ||
FirstItem.y == 0 ||
LastItem.y == 0)
{
return;
}
float min = FirstItem.y + margin;
float max = LastItem.y + heightItem + margin;
float mousePosition = SetBetween(Event.current.mousePosition.y, min, max);
int numberItem = HowManyCellAreDisplayed;
float percent = 1 - ExtMathf.Remap(mousePosition, min, max, 0f, 1f);
float indexPrecise = ExtMathf.Remap(percent, 0, 1, 0, numberItem);
int index = Mathf.RoundToInt(indexPrecise - (1f / numberItem));
index = SetBetween(index, 0, numberItem);
CurrentIndexShouldPointEmptyCell = index;
}
public void DisplayRight()
{
if (!EditorPrefs.GetBool(UnityEssentialsPreferences.SHOW_PEEK_MENU, true))
{
return;
}
float percent = EditorPrefs.GetFloat(UnityEssentialsPreferences.POSITION_IN_TOOLBAR, UnityEssentialsPreferences.DEFAULT_TOOLBAR_POSITION);
if (percent <= 0.5f)
{
return;
}
Rect left = ToolbarExtender.GetRightRect();
percent = ExtMathf.Remap(percent, 0.5f, 1f, 0f, 1f);
float width = (left.width - SIZE_SLIDER) / 1 * percent;
GUILayout.Label("", GUILayout.MinWidth(0), GUILayout.Width(width));
#if UNITY_2018_3_OR_NEWER
Rect finalRect = SetupLocalRect(width);
AddRightClickBehavior(finalRect);
#endif
DisplayPeek();
}
private void FixedUpdate()
{
ChangeState();
ExtMathf.LinearAcceleration(out actualAccelerationVector, rb.position, 4);
actualAcceleration = actualAccelerationVector.magnitude;
}
public void AlgorithmRandomIncremental()
{
ClearIfNecessary();
if (mDataCloud.Count < 4)
{
Debugger.Get.Log("Insufficient points to generate a tetrahedron.", DebugOption.CH3);
return;
}
int index = 0;
// Pick first point.
KeyValuePair <int, Vector3> P1 = new KeyValuePair <int, Vector3>(index, mDataCloud[index]);
KeyValuePair <int, Vector3> P2 = new KeyValuePair <int, Vector3>(-1, Vector3.zero);
while (P2.Key == -1 && ++index < mDataCloud.Count)
{
// Two points form a line if they are not equal.
Vector3 P = mDataCloud[index];
if (!ExtMathf.Equal(P2.Value, P))
{
P2 = new KeyValuePair <int, Vector3>(index, P);
}
}
if (P2.Key == -1)
{
Debugger.Get.Log("Couldn't find two not equal points.", DebugOption.CH3);
return;
}
// Points on line and on the same plane, still should be considered later on. Happens.
List <int> skippedPoints = new List <int>();
Edge line = new Edge(P1.Value, P2.Value);
KeyValuePair <int, Vector3> P3 = new KeyValuePair <int, Vector3>(-1, Vector3.zero);
while (P3.Key == -1 && ++index < mDataCloud.Count)
{
// Three points form a triangle if they are not on the same line.
Vector3 P = mDataCloud[index];
if (!line.CalculateIfIsOnLine(P))
{
P3 = new KeyValuePair <int, Vector3>(index, P);
}
else
{
skippedPoints.Add(index);
}
}
if (P3.Key == -1)
{
Debugger.Get.Log("Couldn't find three not linear points.", DebugOption.CH3);
return;
}
Triangle planeTriangle = new Triangle(P1.Value, P2.Value, P3.Value);
KeyValuePair <int, Vector3> P4 = new KeyValuePair <int, Vector3>(-1, Vector3.zero);
while (P4.Key == -1 && ++index < mDataCloud.Count)
{
// Four points form a tetrahedron if they are not on the same plane.
Vector3 P = mDataCloud[index];
if (planeTriangle.CalculateRelativePosition(P) != 0)
{
P4 = new KeyValuePair <int, Vector3>(index, P);
}
else
{
skippedPoints.Add(index);
}
}
if (P4.Key == -1)
{
Debugger.Get.Log("Couldn't find four not planar points.", DebugOption.CH3);
return;
}
// Calculate reference centroid of ConvexHull.
Vector3 centroid = ExtMathf.Centroid(P1.Value, P2.Value, P3.Value, P4.Value);
List <BasicTriangle> tetrahedronTriangles = new List <BasicTriangle>();
tetrahedronTriangles.Add(new BasicTriangle(P3.Key, P2.Key, P1.Key));
tetrahedronTriangles.Add(new BasicTriangle(P1.Key, P2.Key, P4.Key));
tetrahedronTriangles.Add(new BasicTriangle(P2.Key, P3.Key, P4.Key));
tetrahedronTriangles.Add(new BasicTriangle(P3.Key, P1.Key, P4.Key));
foreach (BasicTriangle basicTriangle in tetrahedronTriangles)
{
Triangle triangle = new Triangle(
mDataCloud[basicTriangle.v[0]],
mDataCloud[basicTriangle.v[1]],
mDataCloud[basicTriangle.v[2]]
);
if (triangle.CalculateRelativePosition(centroid) != -1)
{ // Centroid of ConvexHull should always be inside.
basicTriangle.Reverse();
}
mBasicTriangles.Add(basicTriangle);
}
Dictionary <int, HashSet <BasicTriangle> > pointFacets = new Dictionary <int, HashSet <BasicTriangle> >();
Dictionary <BasicTriangle, HashSet <int> > facetPoints = new Dictionary <BasicTriangle, HashSet <int> >();
foreach (BasicTriangle basicTriangle in mBasicTriangles)
{
Triangle triangle = new Triangle(
mDataCloud[basicTriangle.v[0]],
mDataCloud[basicTriangle.v[1]],
mDataCloud[basicTriangle.v[2]]
);
for (int p = index; p < mDataCloud.Count; p++)
{
if (triangle.CalculateRelativePosition(mDataCloud[p]) == 1)
{
if (!pointFacets.ContainsKey(p))
{
pointFacets.Add(p, new HashSet <BasicTriangle>());
}
pointFacets[p].Add(basicTriangle);
if (!facetPoints.ContainsKey(basicTriangle))
{
facetPoints.Add(basicTriangle, new HashSet <int>());
}
facetPoints[basicTriangle].Add(p);
}
}
foreach (int p in skippedPoints)
{
if (triangle.CalculateRelativePosition(mDataCloud[p]) == 1)
{
if (!pointFacets.ContainsKey(p))
{
pointFacets.Add(p, new HashSet <BasicTriangle>());
}
pointFacets[p].Add(basicTriangle);
facetPoints[basicTriangle].Add(p);
}
}
}
while (pointFacets.Count > 0)
{
var firstPointFacet = pointFacets.First();
if (firstPointFacet.Value.Count > 0)
{
Dictionary <BasicEdge, BasicEdge> horizon = new Dictionary <BasicEdge, BasicEdge>();
foreach (BasicTriangle basicTriangle in firstPointFacet.Value)
{
for (int a = 2, b = 0; b < 3; a = b++)
{
BasicEdge edge = new BasicEdge(basicTriangle.v[a], basicTriangle.v[b]);
BasicEdge edgeUnordered = edge.Unordered();
if (horizon.ContainsKey(edgeUnordered))
{
horizon.Remove(edgeUnordered);
}
else
{
horizon.Add(edgeUnordered, edge);
}
}
}
int pointKey = firstPointFacet.Key;
foreach (BasicTriangle facet in firstPointFacet.Value)
{
foreach (int facetPointKey in facetPoints[facet])
{
if (facetPointKey != pointKey)
{
pointFacets[facetPointKey].Remove(facet);
}
}
facetPoints.Remove(facet);
mBasicTriangles.Remove(facet);
}
pointFacets.Remove(pointKey);
foreach (KeyValuePair <BasicEdge, BasicEdge> edges in horizon)
{
BasicTriangle newBasicTriangle = new BasicTriangle(edges.Value.v[0], edges.Value.v[1], pointKey);
mBasicTriangles.Add(newBasicTriangle);
if (pointFacets.Count > 0)
{
Triangle newTriangle = new Triangle(
mDataCloud[newBasicTriangle.v[0]],
mDataCloud[newBasicTriangle.v[1]],
mDataCloud[newBasicTriangle.v[2]]
);
foreach (var pointFacetsKey in pointFacets.Keys)
{
if (newTriangle.CalculateRelativePosition(mDataCloud[pointFacetsKey]) == 1)
{
pointFacets[pointFacetsKey].Add(newBasicTriangle);
if (!facetPoints.ContainsKey(newBasicTriangle))
{
facetPoints.Add(newBasicTriangle, new HashSet <int>());
}
facetPoints[newBasicTriangle].Add(pointFacetsKey);
}
}
}
}
}
else
{
pointFacets.Remove(firstPointFacet.Key);
}
}
}
/// <summary>
/// get the min lenght of this vector
/// </summary>
/// <param name="vector"></param>
/// <returns>min lenght of x, y or z</returns>
public static float Minimum(this Vector3 vector)
{
return(ExtMathf.Min(vector.x, vector.y, vector.z));
}
public void AlgorithmOptimized()
{
/* Outline of the algorithm:
* 0. Compute the convex hull of the point set (given as input to this function) O(VlogV)
* 1. Compute vertex adjacency data, i.e. given a vertex, return a list of its neighboring vertices. O(V)
* 2. Precompute face normal direction vectors, since these are needed often. (does not affect big-O complexity, just a micro-opt) O(F)
* 3. Compute edge adjacency data, i.e. given an edge, return the two indices of its neighboring faces. O(V)
* 4. Precompute antipodal vertices for each edge. O(A*ElogV), where A is the size of antipodal vertices per edge. A ~ O(1) on average.
* 5. Precompute all sidepodal edges for each edge. O(E*S), where S is the size of sidepodal edges per edge. S ~ O(sqrtE) on average.
* - Sort the sidepodal edges to a linear order so that it's possible to do fast set intersection computations on them. O(E*S*logS), or O(E*sqrtE*logE).
* 6. Test all configurations where all three edges are on adjacent faces. O(E*S^2) = O(E^2) or if smart with graph search, O(ES) = O(E*sqrtE)?
* 7. Test all configurations where two edges are on opposing faces, and the third one is on a face adjacent to the two. O(E*sqrtE*logV)?
* 8. Test all configurations where two edges are on the same face (OBB aligns with a face of the convex hull). O(F*sqrtE*logV).
* 9. Return the best found OBB.
*/
mVolume = float.MaxValue;
var dataCloud = mConvexHull3.DataCloud;
var basicTriangles = mConvexHull3.BasicTriangles;
var edges = mConvexHull3.BasicEdges;
var vertices = mConvexHull3.Vertices;
int verticesCount = vertices.Count;
if (verticesCount < 4)
{
return;
}
// Precomputation: For each vertex in the convex hull, compute their neighboring vertices.
var adjacencyData = mConvexHull3.GetVertexAdjacencyData(); // O(V)
// Precomputation: for each triangle create and store its normal, for later use.
List <Vector3> faceNormals = new List <Vector3>(basicTriangles.Count);
foreach (BasicTriangle basicTriangle in basicTriangles) // O(F)
{
Triangle triangle = Triangle.FromBasicTriangle(basicTriangle, dataCloud);
faceNormals.Add(triangle.N);
}
// Precomputation: For each edge in convex hull, compute both connected faces data.
Dictionary <int, List <int> > facesForEdge = mConvexHull3.GetEdgeFacesData(); // edgeFaces
HashSet <int> internalEdges = new HashSet <int>();
Func <int, bool> IsInternalEdge = (int edgeIndex) => { return(internalEdges.Contains(edgeIndex)); };
for (int edgeIndex = 0; edgeIndex < edges.Count; edgeIndex++)
{
float dot = Vector3.Dot(
faceNormals[facesForEdge[edgeIndex].First()],
faceNormals[facesForEdge[edgeIndex].Second()]
);
bool isInternal = ExtMathf.Equal(dot, 1f, 1e-4f);
if (isInternal)
{
internalEdges.Add(edgeIndex);
}
}
// For each vertex pair (v, u) through which there is an edge, specifies the index i of the edge that passes through them.
// This map contains duplicates, so both (v, u) and (u, v) map to the same edge index.
List <int> vertexPairToEdges = mConvexHull3.GetUniversalEdgesList();
// Throughout the whole algorithm, this array stores an auxiliary structure for performing graph searches
// on the vertices of the convex hull. Conceptually each index of the array stores a boolean whether we
// have visited that vertex or not during the current search. However storing such booleans is slow, since
// we would have to perform a linear-time scan through this array before next search to reset each boolean
// to unvisited false state. Instead, store a number, called a "color" for each vertex to specify whether
// that vertex has been visited, and manage a global color counter floodFillVisitColor that represents the
// visited vertices. At any given time, the vertices that have already been visited have the value
// floodFillVisited[i] == floodFillVisitColor in them. This gives a win that we can perform constant-time
// clears of the floodFillVisited array, by simply incrementing the "color" counter to clear the array.
List <int> floodFillVisited = new List <int>().Populate(-1, verticesCount);
int floodFillVisitColor = 1;
Action ClearGraphSearch = () => { floodFillVisitColor++; };
Action <int> MarkVertexVisited = (int v) => { floodFillVisited[v] = floodFillVisitColor; };
Func <int, bool> HaveVisitedVertex = (int v) => { return(floodFillVisited[v] == floodFillVisitColor); };
List <List <int> > antipodalPointsForEdge = new List <List <int> >().Populate(new List <int>(), edges.Count); // edgeAntipodalVertices
// The currently best variant for establishing a spatially coherent traversal order.
HashSet <int> spatialFaceOrder = new HashSet <int>();
HashSet <int> spatialEdgeOrder = new HashSet <int>();
{ // Explicit scope for variables that are not needed after this.
List <Pair <int, int> > traverseStackEdge = new List <Pair <int, int> >();
List <bool> visitedEdges = new List <bool>().Populate(false, edges.Count);
List <bool> visitedFaces = new List <bool>().Populate(false, basicTriangles.Count);
traverseStackEdge.Add(new Pair <int, int>(0, adjacencyData[0].First()));
while (traverseStackEdge.Count != 0)
{
Pair <int, int> e = traverseStackEdge.Last();
traverseStackEdge.RemoveLast();
int thisEdge = vertexPairToEdges[e.First * verticesCount + e.Second];
if (visitedEdges[thisEdge])
{
continue;
}
visitedEdges[thisEdge] = true;
if (!visitedFaces[facesForEdge[thisEdge].First()])
{
visitedFaces[facesForEdge[thisEdge].First()] = true;
spatialFaceOrder.Add(facesForEdge[thisEdge].First());
}
if (!visitedFaces[facesForEdge[thisEdge].Second()])
{
visitedFaces[facesForEdge[thisEdge].Second()] = true;
spatialFaceOrder.Add(facesForEdge[thisEdge].Second());
}
if (!IsInternalEdge(thisEdge))
{
spatialEdgeOrder.Add(thisEdge);
}
int v0 = e.Second;
int sizeBefore = traverseStackEdge.Count;
foreach (int v1 in adjacencyData[v0])
{
int e1 = vertexPairToEdges[v0 * verticesCount + v1];
if (visitedEdges[e1])
{
continue;
}
traverseStackEdge.Add(new Pair <int, int>(v0, v1));
}
//Take a random adjacent edge
int nNewEdges = (traverseStackEdge.Count - sizeBefore);
if (nNewEdges > 0)
{
int r = UnityEngine.Random.Range(0, nNewEdges - 1);
var swapTemp = traverseStackEdge[traverseStackEdge.Count - 1];
traverseStackEdge[traverseStackEdge.Count - 1] = traverseStackEdge[sizeBefore + r];
traverseStackEdge[sizeBefore + r] = swapTemp;
}
}
}
// Stores a memory of yet unvisited vertices for current graph search.
List <int> traverseStack = new List <int>();
// Since we do several extreme vertex searches, and the search directions have a lot of spatial locality,
// always start the search for the next extreme vertex from the extreme vertex that was found during the
// previous iteration for the previous edge. This has been profiled to improve overall performance by as
// much as 15-25%.
int debugCount = 0;
var startPoint = TimeMeasure.Start();
// Precomputation: for each edge, we need to compute the list of potential antipodal points (points on
// the opposing face of an enclosing OBB of the face that is flush with the given edge of the polyhedron).
// This is O(E*log(V)) ?
foreach (int i in spatialEdgeOrder)
{
Vector3 f1a = faceNormals[facesForEdge[i].First()];
Vector3 f1b = faceNormals[facesForEdge[i].Second()];
MinMaxPair breadthData;
int startingVertex = mConvexHull3.GetAlongAxisData(f1a, out breadthData);
ClearGraphSearch(); // Search through the graph for all adjacent antipodal vertices.
traverseStack.Add(startingVertex);
MarkVertexVisited(startingVertex);
while (!traverseStack.Empty())
{ // In amortized analysis, only a constant number of vertices are antipodal points for any edge?
int v = traverseStack.Last();
traverseStack.RemoveLast();
var neighbours = adjacencyData[v];
if (IsVertexAntipodalToEdge(v, neighbours, f1a, f1b))
{
if (edges[i].ContainsVertex(v))
{
Debug.LogFormat("Edge {0} is antipodal to vertex {1} which is part of the same edge!" +
"This should be possible only if the input is degenerate planar!",
edges[i], v);
return;
}
antipodalPointsForEdge[i].Add(v);
foreach (int neighboursVertex in neighbours)
{
if (!HaveVisitedVertex(neighboursVertex))
{
traverseStack.Add(neighboursVertex);
MarkVertexVisited(neighboursVertex);
}
debugCount++;
}
}
debugCount++;
}
// Robustness: If the above search did not find any antipodal points, add the first found extreme
// point at least, since it is always an antipodal point. This is known to occur very rarely due
// to numerical imprecision in the above loop over adjacent edges.
if (antipodalPointsForEdge[i].Empty())
{
Debug.LogFormat("Got to place which is most likely a bug...");
// Fall back to linear scan, which is very slow.
for (int j = 0; j < verticesCount; j++)
{
if (IsVertexAntipodalToEdge(j, adjacencyData[j], f1a, f1b))
{
antipodalPointsForEdge[i].Add(j);
}
}
}
debugCount++;
}
Debug.Log("@ First loop count: " + debugCount + " in time: " + TimeMeasure.Stop(startPoint));
// Stores for each edge i the list of all sidepodal edge indices j that it can form an OBB with.
List <HashSet <int> > compatibleEdges = new List <HashSet <int> >().Populate(new HashSet <int>(), edges.Count);
// Use a O(E*V) data structure for sidepodal vertices.
bool[] sidepodalVertices = new bool[edges.Count * verticesCount].Populate(false);
// Compute all sidepodal edges for each edge by performing a graph search. The set of sidepodal edges is
// connected in the graph, which lets us avoid having to iterate over each edge pair of the convex hull.
// Total running time is O(E*sqrtE).
debugCount = 0;
startPoint = TimeMeasure.Start();
foreach (int i in spatialEdgeOrder)
{
Vector3 f1a = faceNormals[facesForEdge[i].First()];
Vector3 f1b = faceNormals[facesForEdge[i].Second()];
Vector3 deadDirection = (f1a + f1b) * .5f;
Vector3 basis1, basis2;
deadDirection.PerpendicularBasis(out basis1, out basis2);
Vector3 dir = f1a.Perpendicular();
MinMaxPair breadthData;
int startingVertex = mConvexHull3.GetAlongAxisData(-dir, out breadthData);
ClearGraphSearch();
traverseStack.Add(startingVertex);
while (!traverseStack.Empty()) // O(sqrt(E))
{
int v = traverseStack.Last();
traverseStack.RemoveLast();
if (HaveVisitedVertex(v))
{
continue;
}
MarkVertexVisited(v);
foreach (int vAdj in adjacencyData[v])
{
if (HaveVisitedVertex(vAdj))
{
continue;
}
int edge = vertexPairToEdges[v * verticesCount + vAdj];
if (AreEdgesCompatibleForOBB(f1a, f1b, faceNormals[facesForEdge[edge].First()], faceNormals[facesForEdge[edge].Second()]))
{
if (i <= edge)
{
if (!IsInternalEdge(edge))
{
compatibleEdges[i].Add(edge);
}
sidepodalVertices[i * verticesCount + edges[edge].v[0]] = true;
sidepodalVertices[i * verticesCount + edges[edge].v[1]] = true;
if (i != edge)
{
if (!IsInternalEdge(edge))
{
compatibleEdges[edge].Add(i);
}
sidepodalVertices[edge * verticesCount + edges[i].v[0]] = true;
sidepodalVertices[edge * verticesCount + edges[i].v[1]] = true;
}
}
traverseStack.Add(vAdj);
}
debugCount++;
}
debugCount++;
}
debugCount++;
}
Debug.Log("@ Second loop count: " + debugCount + " in time: " + TimeMeasure.Stop(startPoint));
// Take advantage of spatial locality: start the search for the extreme vertex from the extreme vertex
// that was found during the previous iteration for the previous edge. This speeds up the search since
// edge directions have some amount of spatial locality and the next extreme vertex is often close
// to the previous one. Track two hint variables since we are performing extreme vertex searches to
// two opposing directions at the same time.
// Stores a memory of yet unvisited vertices that are common sidepodal vertices to both currently chosen edges for current graph search.
List <int> traverseStackCommonSidepodals = new List <int>();
Debug.Log("@ Edges count: " + edges.Count);
Debug.Log("@ Vertices count: " + verticesCount);
debugCount = 0;
startPoint = TimeMeasure.Start();
Debug.Log("@ Spatial edges: " + spatialEdgeOrder.Count);
foreach (int i in spatialEdgeOrder)
{
Vector3 f1a = faceNormals[facesForEdge[i].First()];
Vector3 f1b = faceNormals[facesForEdge[i].Second()];
Vector3 deadDirection = (f1a + f1b) * .5f;
Debug.Log("@ Compatible edges: " + compatibleEdges[i].Count);
foreach (int edgeJ in compatibleEdges[i])
{
if (edgeJ <= i) // Remove symmetry.
{
continue;
}
Vector3 f2a = faceNormals[facesForEdge[edgeJ].First()];
Vector3 f2b = faceNormals[facesForEdge[edgeJ].Second()];
Vector3 deadDirection2 = (f2a + f2b) * .5f;
Vector3 searchDir = Vector3.Cross(deadDirection, deadDirection2).normalized;
float length = searchDir.magnitude;
if (ExtMathf.Equal(length, 0f))
{
searchDir = Vector3.Cross(f1a, f2a).normalized;
length = searchDir.magnitude;
if (ExtMathf.Equal(length, 0f))
{
searchDir = f1a.Perpendicular();
}
}
ClearGraphSearch();
MinMaxPair breadthData;
int extremeVertexSearchHint1 = mConvexHull3.GetAlongAxisData(-searchDir, out breadthData);
ClearGraphSearch();
MinMaxPair breadthData2;
int extremeVertexSearchHint2 = mConvexHull3.GetAlongAxisData(searchDir, out breadthData2);
ClearGraphSearch();
int secondSearch = -1;
if (sidepodalVertices[edgeJ * verticesCount + extremeVertexSearchHint1])
{
traverseStackCommonSidepodals.Add(extremeVertexSearchHint1);
}
else
{
traverseStack.Add(extremeVertexSearchHint1);
}
if (sidepodalVertices[edgeJ * verticesCount + extremeVertexSearchHint2])
{
traverseStackCommonSidepodals.Add(extremeVertexSearchHint2);
}
else
{
secondSearch = extremeVertexSearchHint2;
}
// Bootstrap to a good vertex that is sidepodal to both edges.
ClearGraphSearch();
while (!traverseStack.Empty())
{
int v = traverseStack.First();
traverseStack.RemoveFirst();
if (HaveVisitedVertex(v))
{
continue;
}
MarkVertexVisited(v);
foreach (int vAdj in adjacencyData[v])
{
if (!HaveVisitedVertex(vAdj) && sidepodalVertices[i * verticesCount + vAdj])
{
if (sidepodalVertices[edgeJ * verticesCount + vAdj])
{
traverseStack.Clear();
if (secondSearch != -1)
{
traverseStack.Add(secondSearch);
secondSearch = -1;
MarkVertexVisited(vAdj);
}
traverseStackCommonSidepodals.Add(vAdj);
break;
}
else
{
traverseStack.Add(vAdj);
}
}
debugCount++;
}
debugCount++;
}
ClearGraphSearch();
while (!traverseStackCommonSidepodals.Empty())
{
int v = traverseStackCommonSidepodals.Last();
traverseStackCommonSidepodals.RemoveLast();
if (HaveVisitedVertex(v))
{
continue;
}
MarkVertexVisited(v);
foreach (int vAdj in adjacencyData[v])
{
int edgeK = vertexPairToEdges[v * verticesCount + vAdj];
if (IsInternalEdge(edgeK)) // Edges inside faces with 180 degrees dihedral angles can be ignored.
{
continue;
}
if (sidepodalVertices[i * verticesCount + vAdj] && sidepodalVertices[edgeJ * verticesCount + vAdj])
{
if (!HaveVisitedVertex(vAdj))
{
traverseStackCommonSidepodals.Add(vAdj);
}
if (edgeJ < edgeK)
{
Vector3 f3a = faceNormals[facesForEdge[edgeK].First()];
Vector3 f3b = faceNormals[facesForEdge[edgeK].Second()];
Vector3[] n1 = new Vector3[2];
Vector3[] n2 = new Vector3[2];
Vector3[] n3 = new Vector3[2];
int nSolutions = ComputeBasis(f1a, f1b, f2a, f2b, f3a, f3b, ref n1, ref n2, ref n3);
for (int s = 0; s < nSolutions; s++)
{
BoxFromNormalAxes(n1[s], n2[s], n3[s]);
}
}
}
debugCount++;
}
debugCount++;
}
debugCount++;
}
debugCount++;
}
Debug.Log("@ Third loop count: " + debugCount + " in time: " + TimeMeasure.Stop(startPoint));
HashSet <int> antipodalEdges = new HashSet <int>();
List <Vector3> antipodalEdgeNormals = new List <Vector3>();
// Main algorithm body for finding all search directions where the OBB is flush with the edges of the
// convex hull from two opposing faces. This is O(E*sqrtE*logV)?
debugCount = 0;
startPoint = TimeMeasure.Start();
foreach (int i in spatialEdgeOrder)
{
Vector3 f1a = faceNormals[facesForEdge[i].First()];
Vector3 f1b = faceNormals[facesForEdge[i].Second()];
antipodalEdges.Clear();
antipodalEdgeNormals.Clear();
foreach (int antipodalVertex in antipodalPointsForEdge[i])
{
foreach (int vAdj in adjacencyData[antipodalVertex])
{
if (vAdj < antipodalVertex) // We search unordered edges, so no need to process edge (v1, v2) and (v2, v1)
{
continue; // twice - take the canonical order to be antipodalVertex < vAdj
}
int edge = vertexPairToEdges[antipodalVertex * verticesCount + vAdj];
if (i > edge) // We search pairs of edges, so no need to process twice - take the canonical order to be i < edge.
{
continue;
}
if (IsInternalEdge(edge))
{
continue; // Edges inside faces with 180 degrees dihedral angles can be ignored.
}
Vector3 f2a = faceNormals[facesForEdge[edge].First()];
Vector3 f2b = faceNormals[facesForEdge[edge].Second()];
Vector3 normal;
bool success = AreCompatibleOpposingEdges(f1a, f1b, f2a, f2b, out normal);
if (success)
{
antipodalEdges.Add(edge);
antipodalEdgeNormals.Add(normal.normalized);
}
debugCount++;
}
debugCount++;
}
var compatibleEdgesI = compatibleEdges[i];
foreach (int edgeJ in compatibleEdgesI)
{
int k = 0;
foreach (int edgeK in antipodalEdges)
{
Vector3 n1 = antipodalEdgeNormals[k++];
MinMaxPair N1 = new MinMaxPair();
N1.Min = Vector3.Dot(n1, dataCloud[edges[edgeK].v[0]]);
N1.Max = Vector3.Dot(n1, dataCloud[edges[i].v[0]]);
Debug.Assert(N1.Min < N1.Max);
// Test all mutual compatible edges.
Vector3 f3a = faceNormals[facesForEdge[edgeJ].First()];
Vector3 f3b = faceNormals[facesForEdge[edgeJ].Second()];
float num = Vector3.Dot(n1, f3b);
float denom = Vector3.Dot(n1, f3b - f3a);
RefAction <float, float> MoveSign = (ref float dst, ref float src) =>
{
if (src < 0f)
{
dst = -dst;
src = -src;
}
};
MoveSign(ref num, ref denom);
float epsilon = 1e-4f;
if (denom < epsilon)
{
num = (Mathf.Abs(num) == 0f) ? 0f : -1f;
denom = 1f;
}
if (num >= denom * -epsilon && num <= denom * (1f + epsilon))
{
float v = num / denom;
Vector3 n3 = (f3b + (f3a - f3b) * v).normalized;
Vector3 n2 = Vector3.Cross(n3, n1).normalized;
BoxFromNormalAxes(n1, n2, n3);
}
debugCount++;
}
debugCount++;
}
debugCount++;
}
Debug.Log("@ Fourth loop count: " + debugCount + " in time: " + TimeMeasure.Stop(startPoint));
// Main algorithm body for computing all search directions where the OBB touches two edges on the same face.
// This is O(F*sqrtE*logV)?
debugCount = 0;
startPoint = TimeMeasure.Start();
foreach (int i in spatialFaceOrder)
{
Vector3 n1 = faceNormals[i];
// Find two edges on the face. Since we have flexibility to choose from multiple edges of the same face,
// choose two that are possibly most opposing to each other, in the hope that their sets of sidepodal
// edges are most mutually exclusive as possible, speeding up the search below.
int e1 = -1;
int v0 = basicTriangles[i].v[2];
for (int j = 0; j < basicTriangles[i].v.Length; j++)
{
int v1 = basicTriangles[i].v[j];
int e = vertexPairToEdges[v0 * verticesCount + v1];
if (!IsInternalEdge(e))
{
e1 = e;
break;
}
v0 = v1;
}
if (e1 == -1)
{
continue; // All edges of this face were degenerate internal edges! Just skip processing the whole face.
}
var compatibleEdgesI = compatibleEdges[e1];
foreach (int edge3 in compatibleEdgesI)
{
Vector3 f3a = faceNormals[facesForEdge[edge3].First()];
Vector3 f3b = faceNormals[facesForEdge[edge3].Second()];
float num = Vector3.Dot(n1, f3b);
float denom = Vector3.Dot(n1, f3b - f3a);
float v;
if (!ExtMathf.Equal(Mathf.Abs(denom), 0f))
{
v = num / denom;
}
else
{
v = ExtMathf.Equal(Mathf.Abs(num), 0f) ? 0f : -1f;
}
const float epsilon = 1e-4f;
if (v >= 0f - epsilon && v <= 1f + epsilon)
{
Vector3 n3 = (f3b + (f3a - f3b) * v).normalized;
Vector3 n2 = Vector3.Cross(n3, n1).normalized;
BoxFromNormalAxes(n1, n2, n3);
}
debugCount++;
}
debugCount++;
}
Debug.Log("@ Fifth loop count: " + debugCount + " in time: " + TimeMeasure.Stop(startPoint));
}
public static bool SolveAxb(this Matrix4x4 matrix, Vector3 b, ref Vector3 x)
{
float v00 = matrix.m00;
float v10 = matrix.m10;
float v20 = matrix.m20;
float v01 = matrix.m01;
float v11 = matrix.m11;
float v21 = matrix.m21;
float v02 = matrix.m02;
float v12 = matrix.m12;
float v22 = matrix.m22;
float av00 = Mathf.Abs(v00);
float av10 = Mathf.Abs(v10);
float av20 = Mathf.Abs(v20);
// Find which item in first column has largest absolute value.
if (av10 >= av00 && av10 >= av20)
{
Utilities.Swap(ref v00, ref v10);
Utilities.Swap(ref v01, ref v11);
Utilities.Swap(ref v02, ref v12);
Utilities.Swap(ref b.x, ref b.y);
}
else if (av20 >= av00)
{
Utilities.Swap(ref v00, ref v20);
Utilities.Swap(ref v01, ref v21);
Utilities.Swap(ref v02, ref v22);
Utilities.Swap(ref b.x, ref b.z);
}
/* a b c | x
* d e f | y
* g h i | z , where |a| >= |d| && |a| >= |g| */
if (ExtMathf.Equal(av00, 0f))
{
return(false);
}
// Scale row so that leading element is one.
float denom = 1f / v00;
v01 *= denom;
v02 *= denom;
b.x *= denom;
/* 1 b c | x
* d e f | y
* g h i | z */
// Zero first column of second and third rows.
v11 -= v10 * v01;
v12 -= v10 * v02;
b.y -= v10 * b.x;
v21 -= v20 * v01;
v22 -= v20 * v02;
b.z -= v20 * b.x;
/* 1 b c | x
* 0 e f | y
* 0 h i | z */
// Pivotize again.
if (Mathf.Abs(v21) > Mathf.Abs(v11))
{
Utilities.Swap(ref v11, ref v21);
Utilities.Swap(ref v12, ref v22);
Utilities.Swap(ref b.y, ref b.z);
}
if (ExtMathf.Equal(Mathf.Abs(v11), 0f))
{
return(false);
}
/* 1 b c | x
* 0 e f | y
* 0 h i | z, where |e| >= |h| */
denom = 1f / v11;
v12 *= denom;
b.y *= denom;
/* 1 b c | x
* 0 1 f | y
* 0 h i | z */
v22 -= v21 * v12;
b.z -= v21 * b.y;
/* 1 b c | x
* 0 1 f | y
* 0 0 i | z */
if (ExtMathf.Equal(Mathf.Abs(v22), 0f))
{
return(false);
}
x.z = b.z / v22;
x.y = b.y - x.z * v12;
x.x = b.x - x.z * v02 - x.y * v01;
return(true);
}
/// <summary>
/// from a given path (sets of points), get the closest position of pos from this path, and return the
/// current percentage inside
/// </summary>
/// <param name="pos"></param>
/// <param name="chunkPath"></param>
/// <param name="precision"></param>
/// <returns></returns>
public static float GetPercentageFromPosToChunkPath(Vector3 pos, Vector3[] chunkPath, float precision = 1f)
{
if (chunkPath == null || chunkPath.Length == 0)
{
//Debug.LogWarning("out of bounds");
return(0);
}
float percent = 0f;
float lenghtPath = ExtMathf.GetLenghtOfPath(chunkPath);
Vector3[] closestPointInLines = new Vector3[chunkPath.Length];
float[] lenghtAllLine = new float[chunkPath.Length];
for (int i = 0; i < closestPointInLines.Length - 1; i++)
{
ExtLine line = new ExtLine(chunkPath[i], chunkPath[i + 1]);
closestPointInLines[i] = line.ClosestPointTo(pos);
lenghtAllLine[i] = (float)line.GetLenght();
}
int indexFound = -1;
Vector3 closestPoint = ExtMathf.GetClosestPoint(pos, closestPointInLines, ref indexFound);
if (indexFound >= lenghtAllLine.Length)
{
//Debug.LogWarning("out of bounds");
return(0);
}
float lenghtLineClose = lenghtAllLine[indexFound];
Vector3 veccA = chunkPath[indexFound];
int indexPlusOne = indexFound + 1;
if (indexPlusOne >= chunkPath.Length)
{
indexPlusOne = 0;
}
Vector3 veccB = chunkPath[indexPlusOne];
float percentageAlongCLosestLine = ExtMathf.GetPercentageAlong(veccA, veccB, closestPoint);
float lenghtTraveledInThisLine = (percentageAlongCLosestLine * lenghtLineClose) / 1f;
//add lenght, from start to the line found
float lenghtFromZeroToThisPoint = 0f;
for (int i = 0; i < indexFound; i++)
{
lenghtFromZeroToThisPoint += lenghtAllLine[i];
}
lenghtFromZeroToThisPoint += lenghtTraveledInThisLine;
//then add the additionnal percentage
percent = (lenghtFromZeroToThisPoint * 100f) / lenghtPath;
//Debug.Log("ChunkLenght: " + lenghtPath + ", lenght closest line: " + lenghtLineClose + ", percent along this line: " + percentageAlongCLosestLine
// + "lenght From Zero to this point: " + lenghtFromZeroToThisPoint + ", total percent: " + percent);
return(percent);
}
