/// <summary>
/// Creates a view tranformation from the given vectors.
/// Transformation from world- into view-space.
/// </summary>
/// <param name="location">Origin of the view</param>
/// <param name="right">Right vector of the view-plane</param>
/// <param name="up">Up vector of the view-plane</param>
/// <param name="normal">Normal vector of the view-plane. This vector is suppsoed to point in view-direction for a left-handed view transformation and in opposit direction in the right-handed case.</param>
/// <returns>The view transformation</returns>
public static M44d ViewTrafo(V3d location, V3d right, V3d up, V3d normal)
{
return(new M44d(
right.X, right.Y, right.Z, -location.Dot(right),
up.X, up.Y, up.Z, -location.Dot(up),
normal.X, normal.Y, normal.Z, -location.Dot(normal),
0, 0, 0, 1
));
}
/// <summary>
/// Returns true if the ray hits the sphere given by center and
/// radius within the supplied parameter interval and before the
/// parameter value contained in the supplied hit. Note that a
/// hit is only registered if the front or the backsurface is
/// encountered within the interval.
/// </summary>
public bool HitsSphere(
V3d center, double radius,
double tmin, double tmax,
ref RayHit3d hit)
{
V3d originSubCenter = Origin - center;
double a = Direction.LengthSquared;
double b = Direction.Dot(originSubCenter);
double c = originSubCenter.LengthSquared - radius * radius;
// --------------------- quadric equation : a t^2 + 2b t + c = 0
double d = b * b - a * c; // factor 2 was eliminated
if (d < Constant <double> .PositiveTinyValue) // no root ?
{
return(false); // then exit
}
if (b > 0.0) // stable way to calculate
{
d = -Fun.Sqrt(d) - b; // the roots of a quadratic
}
else // equation
{
d = Fun.Sqrt(d) - b;
}
double t1 = d / a;
double t2 = c / d; // Vieta : t1 * t2 == c/a
return(t1 < t2
? ProcessHits(t1, t2, tmin, tmax, ref hit)
: ProcessHits(t2, t1, tmin, tmax, ref hit));
}
/// <summary>
/// Enumerates all vertex indexes at which
/// both outgoing edges meet at an angle that
/// is less than the given threshold.
/// </summary>
public static IEnumerable <int> GetSpikes(
this Polygon3d self, double toleranceInDegrees = 0.1)
{
if (toleranceInDegrees < 0.0)
{
throw new ArgumentOutOfRangeException();
}
var threshold = Conversion.RadiansFromDegrees(toleranceInDegrees).Cos();
var edges = self.GetEdgeArray();
edges.Apply(e => e.Normalized);
var ec = edges.Length;
if (V3d.Dot(-edges[ec - 1], edges[0]) > threshold)
{
yield return(0);
}
for (int i = 1; i < ec; i++)
{
if (V3d.Dot(-edges[i - 1], edges[i]) > threshold)
{
yield return(i);
}
}
}
public static double ComputeUnscaledFormFactor(
this Polygon3d polygon,
V3d p, V3d n,
double eps = 1e-6)
{
var vc = polygon.PointCount;
V3d[] cpa = new V3d[vc + 1];
var cc = 0;
var pb = polygon[0] - p;
var hb = V3d.Dot(pb, n); bool hbp = hb > eps, hbn = hb < -eps;
if (hb >= -eps)
{
cpa[cc++] = pb;
}
var p0 = pb; var h0 = hb; var h0p = hbp; var h0n = hbn;
for (int vi = 1; vi < vc; vi++)
{
var p1 = polygon[vi] - p; var h1 = V3d.Dot(p1, n);
bool h1p = h1 > eps, h1n = h1 < -eps;
if (h0p && h1n || h0n && h1p)
{
cpa[cc++] = p0 + (p1 - p0) * (h0 / (h0 - h1));
}
if (h1 >= -eps)
{
cpa[cc++] = p1;
}
p0 = p1; h0 = h1; h0p = h1p; h0n = h1n;
}
if (h0p && hbn || h0n && hbp)
{
cpa[cc++] = p0 + (pb - p0) * (h0 / (h0 - hb));
}
var cpr = cpa.Map(cc, v => v.Length);
var cv = V3d.Cross(cpa[0], cpa[cc - 1]);
double ff = V3d.Dot(n, cv)
* Fun.AcosC(V3d.Dot(cpa[0], cpa[cc - 1])
/ (cpr[0] * cpr[cc - 1]))
/ cv.Length;
for (int ci = 0; ci < cc - 1; ci++)
{
cv = V3d.Cross(cpa[ci + 1], cpa[ci]);
ff += V3d.Dot(n, cv)
* Fun.AcosC(V3d.Dot(cpa[ci + 1], cpa[ci])
/ (cpr[ci + 1] * cpr[ci]))
/ cv.Length;
}
return(ff);
}
public static Trafo3d ViewTrafo(V3d location, V3d u, V3d v, V3d z)
{
return(new Trafo3d(
new M44d(
u.X, u.Y, u.Z, -V3d.Dot(u, location),
v.X, v.Y, v.Z, -V3d.Dot(v, location),
z.X, z.Y, z.Z, -V3d.Dot(z, location),
0, 0, 0, 1
),
new M44d(
u.X, v.X, z.X, location.X,
u.Y, v.Y, z.Y, location.Y,
u.Z, v.Z, z.Z, location.Z,
0, 0, 0, 1
)));
}
/// <summary>
/// Returns true if the ray hits the triangle within the supplied
/// parameter interval and before the parameter value contained
/// in the supplied hit. Detailed information about the hit is
/// returned in the supplied hit. In order to obtain all potential
/// hits, the supplied hit can be initialized with RayHit3d.MaxRange.
/// </summary>
public bool HitsTriangle(
V3d p0, V3d p1, V3d p2,
double tmin, double tmax,
ref RayHit3d hit
)
{
V3d edge01 = p1 - p0;
V3d edge02 = p2 - p0;
V3d plane = V3d.Cross(Direction, edge02);
double det = V3d.Dot(edge01, plane);
if (det > -0.0000001 && det < 0.0000001)
{
return(false);
}
// ray ~= paralell / Triangle
V3d tv = Origin - p0;
det = 1.0 / det; // det is now inverse det
double u = V3d.Dot(tv, plane) * det;
if (u < 0.0 || u > 1.0)
{
return(false);
}
plane = V3d.Cross(tv, edge01); // plane is now qv
double v = V3d.Dot(Direction, plane) * det;
if (v < 0.0 || u + v > 1.0)
{
return(false);
}
double t = V3d.Dot(edge02, plane) * det;
if (t < tmin || t >= tmax || t >= hit.T)
{
return(false);
}
hit.T = t;
hit.Point = Origin + t * Direction;
hit.Coord.X = u; hit.Coord.Y = v;
hit.BackSide = (det < 0.0);
return(true);
}
/// <summary>
/// Note, that the parallel implementation of this method could be
/// optimized, by providing separate node types for parallel and non
/// parallel execution.
/// </summary>
internal void SortBackToFront(
BspTree t, TargetArrays target,
int ti, V3d eye, bool mainTask, Action <Action> runChild)
{
int nti = ti;
double height = V3d.Dot(m_normal, eye - m_point);
if (height >= 0.0)
{
ti += m_negativeCount;
var ti3 = ti * 3;
foreach (int oti in m_zeroList)
{
t.GetTriangleVertexIndices(oti * 3,
out target.Via[ti3],
out target.Via[ti3 + 1],
out target.Via[ti3 + 2]);
ti3 += 3;
target.Aia[ti] = (t.m_triangleAttributeIndexArray != null) ? t.m_triangleAttributeIndexArray[oti] : 0;
ti++;
}
}
else
{
nti += m_positiveCount;
var nti3 = nti * 3;
foreach (int oti in m_zeroList)
{
t.GetTriangleVertexIndices(oti * 3,
out target.Via[nti3],
out target.Via[nti3 + 1],
out target.Via[nti3 + 2]);
nti3 += 3;
target.Aia[nti] = (t.m_triangleAttributeIndexArray != null) ? t.m_triangleAttributeIndexArray[oti] : 0;
nti++;
}
}
if (m_negativeTree != null)
{
if (mainTask && m_negativeCount < c_taskChunkSize)
{
target.Finished.AddCount(1);
runChild(() =>
{
m_negativeTree.SortBackToFront(
t, target, nti, eye, false, runChild);
target.Finished.Signal();
});
}
else
{
m_negativeTree.SortBackToFront(
t, target, nti, eye, mainTask, runChild);
}
}
if (m_positiveTree != null)
{
if (mainTask && m_positiveCount < c_taskChunkSize)
{
target.Finished.AddCount(1);
runChild(() =>
{
m_positiveTree.SortBackToFront(
t, target, ti, eye, false, runChild);
target.Finished.Signal();
});
}
else
{
m_positiveTree.SortBackToFront(
t, target, ti, eye, mainTask, runChild);
}
}
}
/// <summary>
/// The signed height of the supplied point over the plane.
/// </summary>
public double Height(V3d p)
{
return(V3d.Dot(Normal, p) - Distance);
}
// 3-Dimensional
#region V3d - V3d
public static bool IsOrthogonalTo(this V3d u, V3d v) => Fun.IsTiny(u.Dot(v));
/// <summary>
/// The signed height of the supplied point over the plane.
/// </summary>
public double Height(V3d p)
{
return(V3d.Dot(Normal, p - Point));
}
public NormalsClustering(V3d[] normalArray, double delta)
{
var count = normalArray.Length;
Alloc(count);
var ca = m_indexArray;
var sa = new int[count].Set(1);
var suma = SumArray;
var kdTree = normalArray.CreateRkdTreeDistDotProduct(0);
var query = kdTree.CreateClosestToPointQuery(delta, 0);
for (int i = 0; i < count; i++)
{
int ci = ca[i]; if (ca[ci] != ci)
{
do
{
ci = ca[ci];
} while (ca[ci] != ci); ca[i] = ci;
}
int si = sa[ci];
V3d avgNormali = suma[ci].Normalized;
kdTree.GetClosest(query, avgNormali);
kdTree.GetClosest(query, avgNormali.Negated);
foreach (var id in query.List)
{
int j = (int)id.Index;
int cj = ca[j]; if (ca[cj] != cj)
{
do
{
cj = ca[cj];
} while (ca[cj] != cj); ca[j] = cj;
}
if (ci == cj)
{
continue;
}
int sj = sa[cj];
V3d avgNormalj = suma[cj].Normalized;
double avgDot = avgNormali.Dot(avgNormalj);
if (avgDot.Abs() < 1.0 - 2.0 * delta)
{
continue;
}
V3d sum = suma[ci] + (avgDot > 0 ? suma[cj] : suma[cj].Negated);
if (si < sj)
{
ca[ci] = cj; ca[i] = cj; ci = cj;
}
else
{
ca[cj] = ci; ca[j] = ci;
}
si += sj; sa[ci] = si; suma[ci] = sum;
}
query.Clear();
}
Init();
}
/// <summary>
/// Projets the given point x perpendicular on the plane
/// and returns the nearest point on the plane.
/// </summary>
public V3d NearestPoint(V3d x)
{
var p = Point;
return(x - Normal.Dot(x - p) * Normal);
}
/// <summary> /// The signed height of the supplied point over the plane. /// </summary> public double Height(V3d p) => V3d.Dot(Normal, p - Point);
/// <summary>
/// Creates plane from point and normal vector. IMPORTANT: The
/// supplied vector has to be normalized in order for all methods
/// to work correctly, however if only relative height computations
/// using the <see cref="Height"/> method are necessary, the normal
/// vector need not be normalized.
/// </summary>
public Plane3d(V3d normalizedNormal, V3d point)
{
Normal = normalizedNormal;
Distance = V3d.Dot(normalizedNormal, point);
}
internal void SortFrontToBack(
BspTree t, TargetArray via,
int ti3, V3d eye, bool mainTask, Action <Action> runChild)
{
int nti3 = ti3;
double height = V3d.Dot(m_normal, eye - m_point);
if (height < 0.0)
{
ti3 += m_negativeCount;
foreach (int tiMul3 in m_zeroList)
{
t.GetTriangleVertexIndices(tiMul3,
out via.Via[ti3],
out via.Via[ti3 + 1],
out via.Via[ti3 + 2]);
ti3 += 3;
}
}
else
{
nti3 += m_positiveCount;
foreach (int tiMul3 in m_zeroList)
{
t.GetTriangleVertexIndices(tiMul3,
out via.Via[nti3],
out via.Via[nti3 + 1],
out via.Via[nti3 + 2]);
nti3 += 3;
}
}
if (m_positiveTree != null)
{
if (mainTask && m_positiveCount < c_taskChunkSize)
{
via.Finished.AddCount(1);
runChild(() =>
{
m_positiveTree.SortFrontToBack(
t, via, ti3, eye, false, runChild);
via.Finished.Signal();
});
}
else
{
m_positiveTree.SortFrontToBack(
t, via, ti3, eye, mainTask, runChild);
}
}
if (m_negativeTree != null)
{
if (mainTask && m_negativeCount < c_taskChunkSize)
{
via.Finished.AddCount(1);
runChild(() =>
{
m_negativeTree.SortFrontToBack(
t, via, nti3, eye, false, runChild);
via.Finished.Signal();
});
}
else
{
m_negativeTree.SortFrontToBack(
t, via, nti3, eye, mainTask, runChild);
}
}
}
internal void AddTriangle(
BspTreeBuilder builder, int tiMul3, ref Triangle3d tr, V3d normal)
{
var htr = (V3d.Dot(m_normal, tr.P0 - m_point),
V3d.Dot(m_normal, tr.P1 - m_point),
V3d.Dot(m_normal, tr.P2 - m_point));
var signs = new[] { htr.Item1, htr.Item2, htr.Item3 }.AggregateSigns(builder.m_absoluteEpsilon);
if (signs == Signs.Zero)
{
m_zeroList.Add(tiMul3);
}
else if ((signs & Signs.Negative) == Signs.None)
{
AddTriangle(builder, tiMul3, ref tr, normal, ref m_positiveTree);
}
else if ((signs & Signs.Positive) == Signs.None)
{
AddTriangle(builder, tiMul3, ref tr, normal, ref m_negativeTree);
}
else
{
// the triangle straddles the separating plane
var positivePoints = new List <BspSplitPoint>(4);
var negativePoints = new List <BspSplitPoint>(4);
V3d firstPoint = tr.P0;
double firstHeight = htr.Item1;
bool firstPositive = firstHeight > 0.0;
if (firstPositive)
{
positivePoints.Add(new BspSplitPoint(firstPoint, 0, 0.0));
}
else
{
negativePoints.Add(new BspSplitPoint(firstPoint, 0, 0.0));
}
V3d startPoint = firstPoint;
double startHeight = firstHeight;
bool startPositive = firstPositive;
int start = 0;
int end = 1;
while (end < 3)
{
V3d endPoint = tr[end];
double endHeight = htr.Get(end);
bool endPositive = endHeight > 0.0;
if (startPositive != endPositive)
{
V3d direction = endPoint - startPoint;
double t = -startHeight / V3d.Dot(m_normal,
direction);
V3d newPoint = startPoint + t * direction;
// note, that the same split point (reference!) is
// added to both lists!
var sp = new BspSplitPoint(newPoint, start, t);
positivePoints.Add(sp);
negativePoints.Add(sp);
}
if (endPositive)
{
positivePoints.Add(new BspSplitPoint(endPoint, end, 0.0));
}
else
{
negativePoints.Add(new BspSplitPoint(endPoint, end, 0.0));
}
start = end;
startPoint = endPoint;
startHeight = endHeight;
startPositive = endPositive;
end++;
}
if (startPositive != firstPositive)
{
V3d direction = firstPoint - startPoint;
double t = -startHeight / V3d.Dot(m_normal,
direction);
V3d newPoint = startPoint + t * direction;
var sp = new BspSplitPoint(newPoint, start, t);
positivePoints.Add(sp);
negativePoints.Add(sp);
}
// in order to ensure that all fragments of a triangle are
// consecutively stored, we walk through the two point lists
// twice. for this we need a store of the triangle indices
int[] positiveIndices = new int[2];
int[] negativeIndices = new int[2];
// first pass: generate the cloned triangles (fragments) and
// the resulting triangle indices
if (positivePoints.Count > 2)
{
for (int i = 1; i < positivePoints.Count - 1; i++)
{
positiveIndices[i - 1] = builder.AddClonedTriangle(tiMul3,
positivePoints[0],
positivePoints[i],
positivePoints[i + 1]);
}
}
if (negativePoints.Count > 2)
{
for (int i = 1; i < negativePoints.Count - 1; i++)
{
negativeIndices[i - 1] = builder.AddClonedTriangle(tiMul3,
negativePoints[0],
negativePoints[i],
negativePoints[i + 1]);
}
}
// second pass: add the fragments (with the triangle
// indices) to the BSP-tree
if (positivePoints.Count > 2)
{
for (int i = 0; i < positivePoints.Count - 2; i++)
{
AddTriangle(builder, positiveIndices[i], ref m_positiveTree);
}
}
if (negativePoints.Count > 2)
{
for (int i = 0; i < negativePoints.Count - 2; i++)
{
AddTriangle(builder, negativeIndices[i], ref m_negativeTree);
}
}
}
}
/// <summary>
/// Creates a plane from 3 independent points. A normalized normal
/// vector is computed and stored.
/// </summary>
public Plane3d(V3d p0, V3d p1, V3d p2)
{
Normal = V3d.Cross(p1 - p0, p2 - p0).Normalized;
Distance = V3d.Dot(Normal, p0);
}
// 3-Dimensional
#region V3d - V3d
public static bool IsOrthogonalTo(this V3d u, V3d v)
{
return(Fun.IsTiny(u.Dot(v)));
}
/// <summary> /// The signed height of the supplied point over the plane. /// </summary> public double Height(V3d p) => V3d.Dot(Normal, p) - Distance;