/// <summary>
/// Attempts to compare two floats.
/// </summary>
/// <param name="a">The first number.</param>
/// <param name="b">The second number.</param>
/// <returns>Returns zero if the floats are equal, a positive value if the first float is greater, and a negative value
/// if the second number is greater.</returns>
public static int Compare(float a, float b)
{
if (a == b)
{
return(0);
}
float diff = FloatHelper.Abs(a - b);
if (diff < Constants.Epsilon)
{
return(0);
}
if (b != 0 && diff / FloatHelper.Abs(b) < Constants.Epsilon)
{
return(0);
}
return(a > b ? 1 : -1);
}
/// <summary>
/// Gets the polyhedron feature that is the most extreme, or furthest along, the specified direction.
/// </summary>
/// <remarks>
/// If two vertices are equally far along the given direction, then an edge feature is returned. If more
/// than two vertices are equally far, then the extreme feature is a face.
/// </remarks>
/// <param name="d">The normalized direction vector along which to search for an extreme feature.</param>
/// <returns>Returns the extreme feature that is furthest along the specified direction.</returns>
public PolyhedronFeature ExtremeFeature(ref Vector3 d)
{
PolyhedronFeature e = ExtremeVertex(ref d);
Vector3 v;
int vIndex = e.Index;
for (int i = 0; i < _compiled.Neighbors[vIndex].Length; i++)
{
float x;
int newIndex = _compiled.Neighbors[vIndex][i];
v = _world[newIndex];
Vector3.Dot(ref d, ref v, out x);
if (FloatHelper.Equals(x, e.X))
{
if (e.Type == PolyhedronFeatureType.Vertex)
{
for (int j = 0; j < _compiled.Edges.Length; j++)
{
if ((_compiled.Edges[j][0] == vIndex && _compiled.Edges[j][1] == newIndex) ||
(_compiled.Edges[j][0] == newIndex && _compiled.Edges[j][1] == vIndex))
{
e.Type = PolyhedronFeatureType.Edge;
e.Index = j;
break;
}
}
}
else if (e.Type == PolyhedronFeatureType.Edge)
{
for (int j = 0; j < _compiled.Faces.Length; j++)
{
int count = 0;
for (int k = 0; k < _compiled.Faces[j].Length; k++)
{
if (_compiled.Faces[j][k] == _compiled.Edges[e.Index][0] ||
_compiled.Faces[j][k] == _compiled.Edges[e.Index][1] ||
_compiled.Faces[j][k] == newIndex)
{
count++;
}
if (count > 2)
{
break;
}
}
if (count > 2)
{
e.Type = PolyhedronFeatureType.Face;
e.Index = j;
break;
}
}
}
else
{
break;
}
}
}
return(e);
}
/// <summary>
/// Determines whether two floats are equal, or within a certain narrow range of each other.
/// </summary>
/// <param name="a">The first float.</param>
/// <param name="b">The second float.</param>
/// <returns>Returns a value indicating whether the numbers are nearly equal.</returns>
public static bool Equals(float a, float b)
{
return(FloatHelper.Compare(a, b) == 0);
}
public CompiledPolyhedron(Vector3[] vertices, int[][] faces)
{
this._body = vertices;
this._faces = faces;
Vector3 v1, v2, v3;
_neighbors = new int[vertices.Length][];
_faceNormals = new Vector3[faces.Length];
var v2v = new List <int> [vertices.Length];
for (int i = 0; i < vertices.Length; i++)
{
v2v[i] = new List <int>();
}
var e = new List <int[]>();
var ev = new List <Vector3>();
for (int i = 0; i < faces.Length; i++)
{
int c = faces[i].Length;
v1 = vertices[faces[i][0]];
v2 = vertices[faces[i][1]];
v3 = vertices[faces[i][2]];
Vector3.Subtract(ref v2, ref v1, out v2);
Vector3.Subtract(ref v3, ref v1, out v3);
Vector3.Cross(ref v2, ref v3, out v1);
v1.Normalize();
_faceNormals[i] = v1;
for (int j = 0; j < c; j++)
{
int n = j == c - 1 ? 0 : j + 1;
int p = j == 0 ? c - 1 : j - 1;
v1 = _body[faces[i][j]];
v2 = _body[faces[i][n]];
Vector3.Subtract(ref v2, ref v1, out v1);
v1.Normalize();
bool isUnique = true;
for (int k = 0; k < ev.Count; k++)
{
v2 = ev[k];
float dot;
Vector3.Dot(ref v1, ref v2, out dot);
if (dot < 0f)
{
dot *= -1f;
}
if (FloatHelper.Abs(dot - 1f) < Constants.Epsilon)
{
isUnique = false;
break;
}
}
if (isUnique)
{
ev.Add(v1);
}
if (!v2v[faces[i][j]].Contains(faces[i][n]) &&
!v2v[faces[i][n]].Contains(faces[i][j]))
{
e.Add(new int[] { faces[i][j], faces[i][n] });
}
if (j == 0 && !v2v[faces[i][j]].Contains(faces[i][p]) &&
!v2v[faces[i][p]].Contains(faces[i][j]))
{
e.Add(new int[] { faces[i][p], faces[i][j] });
}
v2v[faces[i][j]].Add(faces[i][n]);
v2v[faces[i][j]].Add(faces[i][p]);
}
}
for (int i = 0; i < v2v.Length; i++)
{
_neighbors[i] = v2v[i].Distinct().ToArray();
}
_edges = e.ToArray();
_edgeVectors = ev.ToArray();
}