bool IsOriented(Vector3 normal, PointNormal normal0, PointNormal normal1, PointNormal normal2)
{
int count = 0;
count = normal.NormalDot(normal0) < 0 ? count + 1 : count;
count = normal.NormalDot(normal1) < 0 ? count + 1 : count;
count = normal.NormalDot(normal2) < 0 ? count + 1 : count;
return(count <= 1);
}
public static extern void std_vector_pointnormal_insert(IntPtr ptr, IntPtr idx, PointNormal value);
public static extern void std_vector_pointnormal_add(IntPtr ptr, PointNormal value);
public static extern void std_vector_pointnormal_at(IntPtr ptr, UIntPtr idx, ref PointNormal value);
internal Tuple <int, Triangle> Pivot(Edge e)
{
Tuple <PointNormal, int> v0 = e.First;
Tuple <PointNormal, int> v1 = e.Second;
Tuple <PointNormal, int> op = e.OppositeVertex;
//Vector3 diff1 = 100 * (v0.Item1.AsVector3() - middle);
//Vector3 diff2 = 100 * (e.BallCenter.AsVector3() - middle);
//Vector3 y = Vector3.Cross(diff1, diff2).normalized;
//Vector3 normal = Vector3.Cross(diff2, y).normalized;
Vector3 normal = (v0.Item1 - e.MiddlePoint).normalized;
HyperPlane plane = new HyperPlane(normal, e.MiddlePoint);
Vector3 zeroAngle = op.Item1 - e.MiddlePoint;
zeroAngle = plane.Projection(zeroAngle).normalized;
float currentAngle = Mathf.PI;
Tuple <int, Triangle> output = null;
int[] indices = GetNeighbors(e.MiddlePoint, ballRadius * 2).ToArray();
for (int t = 0; t < indices.Length; t++)
{
int index = indices[t];
if (v0.Item2 == index || v1.Item2 == index || op.Item2 == index)
{
continue;
}
PointNormal point = cloud[index];
if (plane.AbsDistance(point) <= ballRadius)
{
Vector3 center;
if (GetBallCenter(v0.Item2, v1.Item2, index, out center, out _))
{
List <int> neighborhood = GetNeighbors(center, ballRadius);
if (!isEmpty(neighborhood, v0.Item2, v1.Item2, index, center))
{
continue;
}
Vector3 Vij = v1.Item1.AsVector3() - v0.Item1.AsVector3();
Vector3 Vik = point - v0.Item1.AsVector3();
Vector3 faceNormal = Vector3.Cross(Vik, Vij).normalized;
if (!IsOriented(faceNormal, v0.Item1, v1.Item1, cloud[index]))
{
continue;
}
float cosAngle = zeroAngle.Dot(plane.Projection(center).normalized);
if (Mathf.Abs(cosAngle) > 1.0f)
{
cosAngle = Mathf.Sign(cosAngle);
}
float angle = Mathf.Acos(cosAngle);
if (output == null || currentAngle > angle)
{
currentAngle = angle;
output = new Tuple <int, Triangle>(index, new Triangle(v0.Item1, cloud[index], v1.Item1, v0.Item2, index, v1.Item2, center, ballRadius));
}
}
}
}
return(output);
}