public static bool ApproxEqual(Euclidean3d r0, Euclidean3d r1, double angleTol, double posTol)
{
return(V3d.ApproxEqual(r0.Trans, r1.Trans, posTol) && Rot3d.ApproxEqual(r0.Rot, r1.Rot, angleTol));
}
/// <summary>
/// Creates a circle from it's center, a normal vector (normalized) and a radius.
/// </summary>
public Circle3d(V3d center, V3d normal, double radius)
{
Center = center;
Normal = normal;
Radius = radius;
}
/// <summary>
/// Points within given distance of a point.
/// </summary>
public static PointsNearObject <V3d> QueryPointsNearPoint(
this PointSet self, V3d query, double maxDistanceToPoint, int maxCount
)
=> QueryPointsNearPoint(self.Root.Value, query, maxDistanceToPoint, maxCount);
/// <summary>
/// Projects a point from world-space to normalized device coordinates.
/// </summary>
public static Ndc3d Project(this IRenderView rv, V3d point)
{
var viewPoint = rv.View.ViewTrafo.Forward.TransformPos(point);
return(rv.Projection.TransformPos(viewPoint));
}
public OrientedBox3d(Box3d box, Rot3d rot, V3d trans)
{
Box = box;
Trafo = new Euclidean3d(rot, trans);
}
public static CameraExtrinsics FromWorld2Camera(V3d angleAxis, V3d translation) => FromWorld2Camera((M33d)Rot3d.FromAngleAxis(angleAxis), translation);
public void CanAddNormals()
{
var r = new Random();
var storage = PointSetTests.CreateStorage();
var ps = new V3d[10000].SetByIndex(_ => new V3d(r.NextDouble(), r.NextDouble(), r.NextDouble()));
var pointset = PointSet
.Create(storage, "test", ps.ToList(), null, null, null, 5000, false, CancellationToken.None)
.GenerateLod(ImportConfig.Default.WithKey("lod").WithOctreeSplitLimit(5000))
;
storage.Add("pss", pointset, CancellationToken.None);
var withNormals = WithRandomNormals(pointset.Root.Value);
storage.Add("psWithNormals", withNormals, CancellationToken.None);
withNormals.ForEachNode(true, node =>
{
if (node.IsLeaf)
{
Assert.IsTrue(node.HasNormals);
Assert.IsTrue(!node.HasLodNormals);
Assert.IsTrue(node.Normals.Value.Length == node.PointCount);
}
else
{
Assert.IsTrue(!node.HasNormals);
Assert.IsTrue(node.HasLodNormals);
Assert.IsTrue(node.LodNormals.Value.Length == node.LodPointCount);
}
//
var binary = node.ToBinary();
var node2 = PointSetNode.ParseBinary(binary, storage);
Assert.IsTrue(node.HasNormals == node2.HasNormals);
Assert.IsTrue(node.HasLodNormals == node2.HasLodNormals);
Assert.IsTrue(node.Normals?.Value?.Length == node2.Normals?.Value?.Length);
Assert.IsTrue(node.LodNormals?.Value?.Length == node2.LodNormals?.Value?.Length);
});
PointSetNode WithRandomNormals(PointSetNode n)
{
var id = Guid.NewGuid();
var ns = new V3f[n.IsLeaf ? n.PointCount : n.LodPointCount].Set(V3f.OOI);
storage.Add(id, ns, CancellationToken.None);
if (n.IsLeaf)
{
var m = n.WithNormals(id);
return(m);
}
else
{
var subnodes = n.Subnodes.Map(x => x != null ? WithRandomNormals(x.Value) : null);
var m = n.WithLodNormals(id, subnodes);
return(m);
}
}
}
public V3d WorldPointFromCameraPoint(V3d cameraPoint)
{
return(Rotation.Transform(cameraPoint) + Translation);
}
/// <summary>
/// Generates a uniform distributed random sample on the given triangle using
/// two random series (seriesIndex, seriesIndex + 1).
/// </summary>
public static V3d Triangle(V3d p0, V3d p1, V3d p2, IRandomSeries rnd, int seriesIndex)
{
return(Triangle(p0, p1, p2,
rnd.UniformDouble(seriesIndex),
rnd.UniformDouble(seriesIndex + 1)));
}
/// <summary>
/// Build a geometry transformation from the given parameters as specified in Transform
/// http://gun.teipir.gr/VRML-amgem/spec/part1/nodesRef.html#Transform
/// </summary>
public static Trafo3d BuildVrmlGeometryTrafo(V3d center, V4d rotation, V3d scale, V4d scaleOrientation, V3d translation)
{
// create composite trafo (naming taken from vrml97 spec)
M44d C = M44d.Translation(center), Ci = M44d.Translation(-center);
M44d SR = M44d.Rotation(scaleOrientation.XYZ, scaleOrientation.W),
SRi = M44d.Rotation(scaleOrientation.XYZ, -scaleOrientation.W);
M44d T = M44d.Translation(translation), Ti = M44d.Translation(-translation);
//if (m_aveCompatibilityMode) r.W = -r.W;
M44d R = M44d.Rotation(rotation.XYZ, rotation.W),
Ri = M44d.Rotation(rotation.XYZ, -rotation.W);
// in case some axis scales by 0 the best thing for the inverse scale is also 0
var si = new V3d(scale.X.IsTiny() ? 0 : 1 / scale.X,
scale.Y.IsTiny() ? 0 : 1 / scale.Y,
scale.Z.IsTiny() ? 0 : 1 / scale.Z);
M44d S = M44d.Scale(scale), Si = M44d.Scale(si);
return(new Trafo3d(
T * C * R * SR * S * SRi * Ci,
C * SR * Si * SRi * Ri * Ci * Ti));
}
public V3d CameraPointFromWorldPoint(V3d worldPoint)
{
return(Rotation.TransposedTransform(worldPoint - Translation));
}
/// <summary>
/// Creates clusters of planes within a certain epsilon from each other
/// (euclidean distance between normal vectors and between offsets).
/// This algorithm uses a 4d hash-grid and only works fast if the supplied
/// epsilons are small enough that not too many planes fall within each cluster.
/// Thus it is ideal for merging planes with small variations in orientation and
/// offset due to numerical inaccuracies.
/// </summary>
public PlaneEpsilonClustering(
int count, TArray pa,
Func <TArray, int, V3d> getNormal,
Func <TArray, int, double> getDist,
double epsNormal, double epsDist,
double deltaEpsFactor = 0.25, IRandomUniform rnd = null)
{
rnd = rnd ?? new RandomSystem();
Alloc(count);
var ca = m_indexArray;
var dict = new IntDict <int>(count, stackDuplicateKeys: true);
int rndBits = 0; int bit = 0;
var ha = new int[16];
var ne2 = epsNormal * epsNormal;
var de2 = epsDist * epsDist;
var deps = (ne2 + de2) * deltaEpsFactor * deltaEpsFactor;
for (int i = 0; i < count; i++)
{
var ni = getNormal(pa, i); var di = getDist(pa, i);
ni.HashCodes16(di, epsNormal, epsDist, ha);
int ci = ca[i]; if (ca[ci] != ci)
{
do
{
ci = ca[ci];
} while (ca[ci] != ci); ca[i] = ci;
}
double dmin = double.MaxValue;
for (int hi = 0; hi < 16; hi++)
{
foreach (var j in dict.ValuesWithKey(ha[hi]))
{
int cj = ca[j]; if (ca[cj] != cj)
{
do
{
cj = ca[cj];
} while (ca[cj] != cj); ca[j] = cj;
}
if (ci == cj)
{
continue;
}
var dd = Fun.Square(di - getDist(pa, j)); if (dd >= de2)
{
continue;
}
var dn = V3d.DistanceSquared(ni, getNormal(pa, j)); if (dn >= ne2)
{
continue;
}
var d = dn + dd; if (d < dmin)
{
dmin = d;
}
bit >>= 1; if (bit == 0)
{
rnd.UniformInt(); bit = 1 << 30;
}
if ((rndBits & bit) != 0)
{
ca[ci] = cj; ca[i] = cj; ci = cj;
}
else
{
ca[cj] = ci; ca[j] = ci;
}
}
}
if (dmin > deps)
{
dict[ha[0]] = i; // only sparsely populate hashtable for performance reasons
}
}
Init();
}
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();
}
public static Euclidean3d Parse(string s)
{
var x = s.NestedBracketSplitLevelOne().ToArray();
return(new Euclidean3d(Rot3d.Parse(x[0]), V3d.Parse(x[1])));
}
public V3d WorldPointFromCameraPoint(V3d cameraPoint) => Rotation.Transform(cameraPoint) + Translation;
/// <summary>
/// Generates a uniform distributed random sample on the given triangle using
/// two random variables x1 and x2.
/// </summary>
public static V3d Triangle(V3d p0, V3d p1, V3d p2, double x1, double x2)
{
var x1sq = x1.Sqrt();
return((1 - x1sq) * p0 + (x1sq * (1 - x2)) * p1 + (x1sq * x2) * p2);
}
public static CameraExtrinsics FromWorld2Camera(M33d rotation, V3d translation) => new CameraExtrinsics(rotation.Transposed, -rotation.TransposedTransform(translation));
/// <summary>
/// Supplied normal MUST be normalized, uses the 2 random series
/// (seriesIndex, seriesIndex+1).
/// </summary>
public static V3d Lambertian(V3d normal, IRandomSeries rnds, int seriesIndex)
{
return(Lambertian(normal,
rnds.UniformDouble(seriesIndex),
rnds.UniformDouble(seriesIndex + 1)));
}
/// <summary>
/// Sets location and axes in a single transaction.
/// </summary>
public void Set(V3d location, V3d right, V3d up, V3d forward)
{
m_trafo = Trafo3d.ViewTrafo(location, right, up, -forward);
m_trafoChanges.Emit(m_trafo);
}
/// <summary>
/// Initializes a new instance of the <see cref="Sphere3d"/> class using center and radius values.
/// </summary>
public Sphere3d(V3d center, double radius)
{
Center = center;
Radius = radius;
}
public double GetMinimalDistance(V3d p)
{
return(GetMinimalDistance(p, Position, Direction, MajorRadius, MinorRadius));
}
/// <summary>
/// Creates a sphere from its center, and a point on its surface.
/// </summary>
public Sphere3d(V3d center, V3d pointOnSurface)
{
Center = center;
Radius = (pointOnSurface - center).Length;
}
/// <summary>
/// Project a point from world-space to a pixel position.
/// </summary>
public static PixelPosition ProjectPixel(this IRenderView rv, V3d point)
{
var ndcPoint = rv.Project(point);
return(new PixelPosition(ndcPoint, rv.Region));
}
/// <summary>
/// Initializes a new instance of the <see cref="Sphere3d"/> class using values from another sphere instance.
/// </summary>
public Sphere3d(Sphere3d sphere)
{
Center = sphere.Center;
Radius = sphere.Radius;
}
public static Circle3d Parse(string s)
{
var x = s.NestedBracketSplitLevelOne().ToArray();
return(new Circle3d(V3d.Parse(x[0]), V3d.Parse(x[1]), double.Parse(x[2], CultureInfo.InvariantCulture)));
}
public static Sphere3d FromCenterAndRadius(V3d center, double radius) => new Sphere3d(center, radius);
public Circle3d(Circle3d circle)
{
Center = circle.Center;
Normal = circle.Normal;
Radius = circle.Radius;
}
public V3d CameraPointFromWorldPoint(V3d worldPoint) => Rotation.TransposedTransform(worldPoint - Translation);
/// <summary>
/// Points within given distance of a point.
/// </summary>
public static PointsNearObject <V3d> QueryPointsNearPoint(
this IPointCloudNode node, V3d query, double maxDistanceToPoint, int maxCount
)
{
if (node == null)
{
return(PointsNearObject <V3d> .Empty);
}
// if query point is farther from bounding box than maxDistanceToPoint,
// then there cannot be a result and we are done
var eps = node.BoundingBoxExactGlobal.Distance(query);
if (eps > maxDistanceToPoint)
{
return(PointsNearObject <V3d> .Empty);
}
if (node.IsLeaf())
{
var nodePositions = node.Positions;
#if PARANOID
if (nodePositions.Value.Length <= 0)
{
throw new InvalidOperationException();
}
#endif
var center = node.Center;
var ia = node.KdTree.Value.GetClosest((V3f)(query - center), (float)maxDistanceToPoint, maxCount);
if (ia.Count > 0)
{
var ps = new V3d[ia.Count];
var cs = node.HasColors ? new C4b[ia.Count] : null;
var ns = node.HasNormals ? new V3f[ia.Count] : null;
var js = node.HasIntensities ? new int[ia.Count] : null;
var ks = node.HasClassifications ? new byte[ia.Count] : null;
var ds = new double[ia.Count];
for (var i = 0; i < ia.Count; i++)
{
var index = (int)ia[i].Index;
ps[i] = center + (V3d)node.Positions.Value[index];
if (node.HasColors)
{
cs[i] = node.Colors.Value[index];
}
if (node.HasNormals)
{
ns[i] = node.Normals.Value[index];
}
if (node.HasIntensities)
{
js[i] = node.Intensities.Value[index];
}
if (node.HasClassifications)
{
ks[i] = node.Classifications.Value[index];
}
ds[i] = ia[i].Dist;
}
var chunk = new PointsNearObject <V3d>(query, maxDistanceToPoint, ps, cs, ns, js, ks, ds);
return(chunk);
}
else
{
return(PointsNearObject <V3d> .Empty);
}
}
else
{
// first traverse octant containing query point
var index = node.GetSubIndex(query);
var n = node.Subnodes[index];
var result = n != null?n.Value.QueryPointsNearPoint(query, maxDistanceToPoint, maxCount) : PointsNearObject <V3d> .Empty;
if (!result.IsEmpty && result.MaxDistance < maxDistanceToPoint)
{
maxDistanceToPoint = result.MaxDistance;
}
// now traverse other octants
for (var i = 0; i < 8; i++)
{
if (i == index)
{
continue;
}
n = node.Subnodes[i];
if (n == null)
{
continue;
}
var x = n.Value.QueryPointsNearPoint(query, maxDistanceToPoint, maxCount);
result = result.Merge(x, maxCount);
if (!result.IsEmpty && result.MaxDistance < maxDistanceToPoint)
{
maxDistanceToPoint = result.MaxDistance;
}
}
return(result);
}
}
/// <summary>
/// Inverts this rigid transformation (multiplicative inverse).
/// this = [Rot^T,-Rot^T Trans]
/// </summary>
public void Invert()
{
Rot.Invert();
Trans = -Rot.TransformDir(Trans);
}
