private static int FixFunnel(ref Vector2[] left, ref Vector2[] right)
{
if (left.Length != right.Length)
{
throw new ArgumentException("left and right lists must have equal length");
}
if (left.Length < 3)
{
return(-1);
}
int num = 0;
while (left[1] == left[2] && right[1] == right[2])
{
left[1] = left[0];
right[1] = right[0];
num++;
if (left.Length - num < 3)
{
return(-1);
}
}
Vector2 vector = left[num + 2];
if (vector == left[num + 1])
{
vector = right[num + 2];
}
while (VectorMath.IsColinear(left[num], left[num + 1], right[num + 1]) || VectorMath.RightOrColinear(left[num + 1], right[num + 1], vector) == VectorMath.RightOrColinear(left[num + 1], right[num + 1], left[num]))
{
left[num + 1] = left[num];
right[num + 1] = right[num];
num++;
if (left.Length - num < 3)
{
return(-1);
}
vector = left[num + 2];
if (vector == left[num + 1])
{
vector = right[num + 2];
}
}
if (!VectorMath.RightOrColinear(left[num], left[num + 1], right[num + 1]) && !VectorMath.IsColinear(left[num], left[num + 1], right[num + 1]))
{
Vector2[] array = left;
left = right;
right = array;
}
return(num);
}
private static bool UnwrapHelper(Vector3 portalStart, Vector3 portalEnd, Vector3 prevPoint, Vector3 nextPoint, ref Quaternion mRot, ref Vector3 mOffset)
{
if (VectorMath.IsColinear(portalStart, portalEnd, nextPoint))
{
return(false);
}
Vector3 vector = portalEnd - portalStart;
float sqrMagnitude = vector.sqrMagnitude;
prevPoint -= Vector3.Dot(prevPoint - portalStart, vector) / sqrMagnitude * vector;
nextPoint -= Vector3.Dot(nextPoint - portalStart, vector) / sqrMagnitude * vector;
Quaternion quaternion = Quaternion.FromToRotation(nextPoint - portalStart, portalStart - prevPoint);
mOffset += mRot * (portalStart - quaternion * portalStart);
mRot *= quaternion;
return(true);
}
static bool UnwrapHelper(Vector3 portalStart, Vector3 portalEnd, Vector3 prevPoint, Vector3 nextPoint, ref Quaternion mRot, ref Vector3 mOffset)
{
// Skip the point if it was on the rotation axis
if (VectorMath.IsColinear(portalStart, portalEnd, nextPoint))
{
return(false);
}
var axis = portalEnd - portalStart;
var sqrMagn = axis.sqrMagnitude;
prevPoint -= Vector3.Dot(prevPoint - portalStart, axis) / sqrMagn * axis;
nextPoint -= Vector3.Dot(nextPoint - portalStart, axis) / sqrMagn * axis;
var rot = Quaternion.FromToRotation(nextPoint - portalStart, portalStart - prevPoint);
// The code below is equivalent to these matrix operations (but a lot faster)
// This represents a rotation around a line in 3D space
//mat = mat * Matrix4x4.TRS(portalStart, rot, Vector3.one) * Matrix4x4.TRS(-portalStart, Quaternion.identity, Vector3.one);
mOffset += mRot * (portalStart - rot * portalStart);
mRot *= rot;
return(true);
}
/** Try to fix degenerate or invalid funnels.
* \returns The number of vertices at the start of both arrays that should be ignored or -1 if the algorithm failed.
*/
static int FixFunnel(ref Vector2[] left, ref Vector2[] right)
{
if (left.Length != right.Length)
{
throw new System.ArgumentException("left and right lists must have equal length");
}
if (left.Length < 3)
{
return(-1);
}
int removed = 0;
// Remove identical vertices
while (left[1] == left[2] && right[1] == right[2])
{
// Equivalent to RemoveAt(1) if they would have been lists
left[1] = left[0];
right[1] = right[0];
removed++;
if (left.Length - removed < 3)
{
return(-1);
}
}
Vector2 swPoint = left[removed + 2];
if (swPoint == left[removed + 1])
{
swPoint = right[removed + 2];
}
//Test
while (VectorMath.IsColinear(left[removed + 0], left[removed + 1], right[removed + 1]) || VectorMath.RightOrColinear(left[removed + 1], right[removed + 1], swPoint) == VectorMath.RightOrColinear(left[removed + 1], right[removed + 1], left[removed + 0]))
{
// Equivalent to RemoveAt(1) if they would have been lists
left[removed + 1] = left[removed + 0];
right[removed + 1] = right[removed + 0];
removed++;
if (left.Length - removed < 3)
{
return(-1);
}
swPoint = left[removed + 2];
if (swPoint == left[removed + 1])
{
swPoint = right[removed + 2];
}
}
// Switch left and right to really be on the "left" and "right" sides
if (!VectorMath.RightOrColinear(left[removed + 0], left[removed + 1], right[removed + 1]) && !VectorMath.IsColinear(left[removed + 0], left[removed + 1], right[removed + 1]))
{
var tmp = left;
left = right;
right = tmp;
}
return(removed);
}
