/// <summary>
/// Returns the outer product (tensor-product) of v1 * v2^T as a 3x3 Matrix.
/// </summary>
public static M33f OuterProduct(this V3f v1, V3f v2)
{
return(new M33f(
v2.X * v1.X, v2.Y * v1.X, v2.Z * v1.X,
v2.X * v1.Y, v2.Y * v1.Y, v2.Z * v1.Y,
v2.X * v1.Z, v2.Y * v1.Z, v2.Z * v1.Z));
}
/// <summary></summary>
public static V3f[] GetV3fArray(this Storage storage, string key, CancellationToken ct)
{
var data = (V3f[])storage.f_tryGetFromCache(key, ct);
if (data != null)
{
return(data);
}
var buffer = storage.f_get(key, ct);
if (buffer == null)
{
return(null);
}
data = new V3f[buffer.Length / 12];
using (var ms = new MemoryStream(buffer))
using (var br = new BinaryReader(ms))
{
for (var i = 0; i < data.Length; i++)
{
data[i] = new V3f(br.ReadSingle(), br.ReadSingle(), br.ReadSingle());
}
}
storage.f_add(key, data, null, ct);
return(data);
}
/// <summary>
/// Returns the skew-symmetric "cross" matrix (A^T = -A) of the vector v.
/// </summary>
public static M33f CrossMatrix(this V3f v)
{
return(new M33f(
0, -v.Z, +v.Y,
+v.Z, 0, -v.X,
-v.Y, +v.X, 0));
}
public bool Occluded(V3f rayOrigin, V3f rayDirection, float minT = 0.0f, float maxT = float.MaxValue, RTCFilterFunction filter = null)
{
// NOTE: rtcInitIntersectContext is an inline method -> do manually
var ctx = new RTCIntersectContext()
{
instID = unchecked ((uint)-1),
filter = filter != null?Marshal.GetFunctionPointerForDelegate(filter) : IntPtr.Zero,
};
var rtRay = new RTCRay()
{
org = rayOrigin,
dir = rayDirection,
tnear = minT,
tfar = maxT,
flags = 0, // must be initialized with 0
time = 0,
mask = 0,
id = 0,
};
unsafe
{
var ctxPtr = &ctx;
var rayPtr = &rtRay;
EmbreeAPI.rtcOccluded1(Handle, ctxPtr, rayPtr);
}
// tfar set to -inf if intersection is found
return(rtRay.tfar == float.NegativeInfinity);
}
public void Write(V3f value)
{
for (int i = 0; i < 3; i++)
{
base.Write(Conversion.HostToNetworkOrder(value[i]));
}
}
/// <summary>
/// Decode the unsigned integer direction code.
/// </summary>
/// <param name="code"></param>
/// <returns>A normalized direction.</returns>
public V3f Decode(uint code)
{
if (code < m_edgeBasis) // face number is code
{
double u = Unwarp(code % m_r2Sub1);
code /= m_r2Sub1;
double v = Unwarp(code % m_r2Sub1);
code /= m_r2Sub1;
double scale = 1.0 / System.Math.Sqrt(1.0 + u * u + v * v);
switch (code)
{
case 0: return(new V3f(-scale, u * scale, v * scale));
case 1: return(new V3f(v * scale, -scale, u * scale));
case 2: return(new V3f(u * scale, v * scale, -scale));
case 3: return(new V3f(+scale, u * scale, v * scale));
case 4: return(new V3f(v * scale, +scale, u * scale));
case 5: return(new V3f(u * scale, v * scale, +scale));
default: throw new ArgumentException();
}
}
else if (code < m_cornerBasis)
{
code -= m_edgeBasis;
double u = Unwarp(code % m_r2Sub1);
code /= m_r2Sub1; // edge number in code
double scale = 1.0 / System.Math.Sqrt(2.0 + u * u);
uint edge = code & 3; code >>= 2;
V3f dir = new V3f(0, 0, 0); // init to make compiler happy
dir[(int)code] = (float)(u * scale);
dir[s_uAxis[code]] = (float)(s_uSignOfEdge[edge] * scale);
dir[s_vAxis[code]] = (float)(s_vSignOfEdge[edge] * scale);
return(dir);
#if NEVERMORE
float edgeOne = (code & 1) == 0 ? -scale : scale;
float edgeTwo = (code & 2) == 0 ? -scale : scale;
switch (code >> 2)
{
case 0: return(new V3f((float)(u * scale), edgeOne, edgeTwo));
case 1: return(new V3f(edgeOne, (float)(u * scale), edgeTwo));
case 2: return(new V3f(edgeOne, edgeTwo, (float)(u * scale)));
default: throw new ArgumentException();
}
#endif
}
else
{
return(s_vecToCornerTable[code - m_cornerBasis]);
}
}
/// <summary>
/// Returns the matrix that transforms from the coordinate system
/// specified by the basis into the world cordinate system.
/// </summary>
public static M44f FromBasis(V3f xAxis, V3f yAxis, V3f zAxis, V3f origin)
{
return(new M44f(
xAxis.X, yAxis.X, zAxis.X, origin.X,
xAxis.Y, yAxis.Y, zAxis.Y, origin.Y,
xAxis.Z, yAxis.Z, zAxis.Z, origin.Z,
0, 0, 0, 1));
}
/// <summary>
/// Constructs rkd-tree from points and kd-tree data.
/// </summary>
public static PointRkdTreeF <V3f[], V3f> ToKdTree(this V3f[] points, PointRkdTreeFData data)
=> new PointRkdTreeF <V3f[], V3f>(
3, points.Length, points,
(xs, i) => xs[(int)i], (v, i) => (float)v[i],
(a, b) => V3f.Distance(a, b), (i, a, b) => b - a,
(a, b, c) => VecFun.DistanceToLine(a, b, c), VecFun.Lerp, 1e-6f,
data
);
/// <summary></summary>
public FilterNormalDirection(V3f direction, float epsInDegrees)
{
var e = (float)Math.Sin(Conversion.RadiansFromDegrees(Fun.Clamp(epsInDegrees, 0.0, 90.0)));
Direction = direction.Normalized;
EpsInDegrees = epsInDegrees;
m_eps = e * e;
}
public void NormalsFromEmptyArray()
{
var ps = new V3f[0];
var kd = ps.BuildKdTree();
var ns = ps.EstimateNormalsAsync(16, kd).Result;
Assert.IsTrue(ns.Length == 0);
}
public void NeigbourTest(uint raster)
{
var coder = new V3fCoder(raster);
for (uint code = 0; code < coder.Count; code++)
{
uint[] neighbours = new uint[8];
V3f n = coder.DecodeOnCube(code, false);
uint nCount = coder.NeighbourCodes(code, neighbours);
float min = float.MaxValue;
float max = float.MinValue;
for (uint nc = 0; nc < nCount; nc++)
{
V3f nv = coder.DecodeOnCube(neighbours[nc], false);
float diff = (nv - n).Length;
if (diff < min)
{
min = diff;
}
if (diff > max)
{
max = diff;
}
}
if (min < 0.000001)
{
Console.WriteLine("min too small");
}
if (max > 2.0 * min)
{
Console.WriteLine("max too large");
}
if (raster < 3)
{
Console.WriteLine("code {0} min {1} max {2}",
code, min, max);
}
min *= 0.99f;
for (uint nc0 = 0; nc0 < nCount; nc0++)
{
V3f nv0 = coder.DecodeOnCube(neighbours[nc0], false);
for (uint nc1 = nc0 + 1; nc1 < nCount; nc1++)
{
V3f nv1 = coder.DecodeOnCube(neighbours[nc1], false);
float diff = (nv1 - nv0).Length;
if (diff < min)
{
Console.Write("neighbours too close");
}
}
}
}
}
public static V3f Transform(this Rot2f rot, V3f v)
{
float a = Fun.Cos(rot.Angle);
float b = Fun.Sin(rot.Angle);
return(new V3f(a * v.X + -b * v.Y,
b * v.X + a * v.Y,
v.Z));
}
public static V3f Multiply(Rot2f rot, V3f vec)
{
float ca = (float)System.Math.Cos(rot.Angle);
float sa = (float)System.Math.Sin(rot.Angle);
return(new V3f(ca * vec.X + sa * vec.Y,
-sa * vec.X + ca * vec.Y,
vec.Z));
}
/// <summary>
/// Creates point rkd-tree.
/// </summary>
public static PointRkdTreeD <V3f[], V3f> BuildKdTree(this V3f[] self, double kdTreeEps = 1e-6)
{
return(new PointRkdTreeD <V3f[], V3f>(
3, self.Length, self,
(xs, i) => xs[(int)i], (v, i) => (float)v[i],
(a, b) => V3f.Distance(a, b), (i, a, b) => b - a,
(a, b, c) => VecFun.DistanceToLine(a, b, c), VecFun.Lerp, kdTreeEps
));
}
/// <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 M44f ViewTrafo(V3f location, V3f right, V3f up, V3f normal)
{
return(new M44f(
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>
/// Creates a inverse view tranformation from the given vectors.
/// Transformation from view- into world-space.
/// The implementation is the same as FromBasis(right, up, normal, location)
/// </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 inverse view transformation</returns>
public static M44f InvViewTrafo(V3f location, V3f right, V3f up, V3f normal)
{
return(new M44f(
right.X, up.X, normal.X, location.X,
right.Y, up.Y, normal.Y, location.Y,
right.Z, up.Z, normal.Z, location.Z,
0, 0, 0, 1
));
}
/// <summary>
/// Computes from a <see cref="V3f"/> point (origin) and
/// a <see cref="V3f"/> normal the transformation matrix
/// and its inverse.
/// </summary>
/// <param name="origin">The point which will become the new origin.</param>
/// <param name="normal">The normal vector of the new ground plane.</param>
/// <param name="local2global">A <see cref="M44f"/>The trafo from local to global system.</param>
/// <param name="global2local">A <see cref="M44f"/>The trafofrom global to local system.</param>
public static void NormalFrame(V3f origin, V3f normal,
out M44f local2global, out M44f global2local
)
{
V3f min;
float x = Fun.Abs(normal.X);
float y = Fun.Abs(normal.Y);
float z = Fun.Abs(normal.Z);
if (x < y)
{
if (x < z)
{
min = V3f.XAxis;
}
else
{
min = V3f.ZAxis;
}
}
else
{
if (y < z)
{
min = V3f.YAxis;
}
else
{
min = V3f.ZAxis;
}
}
V3f xVec = Vec.Cross(normal, min);
xVec.Normalize(); // this is now guaranteed to be normal to the input normal
V3f yVec = Vec.Cross(normal, xVec);
yVec.Normalize();
V3f zVec = normal;
zVec.Normalize();
local2global = new M44f(xVec.X, yVec.X, zVec.X, origin.X,
xVec.Y, yVec.Y, zVec.Y, origin.Y,
xVec.Z, yVec.Z, zVec.Z, origin.Z,
0, 0, 0, 1);
M44f mat = new M44f(xVec.X, xVec.Y, xVec.Z, 0,
yVec.X, yVec.Y, yVec.Z, 0,
zVec.X, zVec.Y, zVec.Z, 0,
0, 0, 0, 1);
var shift = M44f.Translation(-origin);
global2local = mat * shift;
}
/// <summary>
/// Calculates the sum for a given set of V3fs.
/// </summary>
public static V3f Sum(this IEnumerable <V3f> vectors)
{
V3f sum = V3f.Zero;
foreach (var e in vectors)
{
sum += e;
}
return(sum);
}
/// <summary>
/// Estimates a normal vector for each point by least-squares-fitting a plane through its k nearest neighbours.
/// </summary>
public static V3f[] EstimateNormals(this V3f[] points, int k)
{
var kd = new PointRkdTreeD <V3f[], V3f>(
3, points.Length, points,
(xs, i) => xs[(int)i], (v, i) => (float)v[i],
(a, b) => V3f.Distance(a, b), (i, a, b) => b - a,
(a, b, c) => VecFun.DistanceToLine(a, b, c), VecFun.Lerp, 0
);
return(EstimateNormals(points, kd, k));
}
/// <summary>
/// Calculates the centroid for a given set of V3fs.
/// </summary>
public static V3f ComputeCentroid(this V3f[] vectors, int[] indices)
{
V3f sum = V3f.Zero;
for (var i = 0; i < indices.Length; i++)
{
sum += vectors[indices[i]];
}
return(sum / (float)indices.Length);
}
/// <summary>
/// Calculates the centroid for a given set of V3fs.
/// </summary>
public static V3f ComputeCentroid(this V3f[] vectors)
{
V3f sum = V3f.Zero;
for (var i = 0; i < vectors.Length; i++)
{
sum += vectors[i];
}
return(sum / (float)vectors.Length);
}
/// <summary>
/// Calculates the sum for a given set of V3fs.
/// </summary>
public static V3f Sum(this V3f[] vectors)
{
V3f sum = V3f.Zero;
for (var i = 0; i < vectors.Length; i++)
{
sum += vectors[i];
}
return(sum);
}
/// <summary>
/// Calculate all directions that are exactly encoded.
/// </summary>
/// <returns>An array of directions.</returns>
public V3f[] GenerateTable()
{
uint dirCount = Count;
V3f[] directionTable = new V3f[dirCount];
for (uint dc = 0; dc < dirCount; dc++)
{
directionTable[dc] = Decode(dc);
}
return(directionTable);
}
/// <summary>
/// Calculates the centroid for a given set of V3fs.
/// </summary>
public static V3f ComputeCentroid(this IEnumerable <V3f> vectors)
{
V3f sum = V3f.Zero;
int count = 0;
foreach (var e in vectors)
{
sum += e;
count++;
}
return(sum / (float)count);
}
// https://stackoverflow.com/questions/1171849/finding-quaternion-representing-the-rotation-from-one-vector-to-another
public static Rot3f HalfWayVec(V3f from, V3f into)
{
if (from.Dot(into).ApproximateEquals(-1))
{
return(new Rot3f(0, from.AxisAlignedNormal()));
}
else
{
V3f half = Vec.Normalized(from + into);
QuaternionF q = new QuaternionF(Vec.Dot(from, half), Vec.Cross(from, half));
return(new Rot3f(q.Normalized));
}
}
/// <summary>
/// Calculates a weighted centroid for a given array of V3fs.
/// </summary>
public static V3f ComputeCentroid(this V3f[] vectors, float[] weights)
{
V3f sum = V3f.Zero;
float weightSum = 0;
for (int i = 0; i < vectors.Length; i++)
{
sum += weights[i] * vectors[i];
weightSum += weights[i];
}
return(sum / weightSum);
}
/// <summary>
/// Creates an orthonormal basis from the given normal as z-axis.
/// The resulting matrix transforms from the local to the global coordinate system.
/// The normal is expected to be normalized.
///
/// The implementation is based on:
/// Building an Orthonormal Basis, Revisited, by Duff et al. 2017
/// </summary>
public static M33f NormalFrame(V3f n)
{
var sg = n.Z >= 0 ? 1 : -1; // original uses copysign(1.0, n.Z) -> not the same as sign where 0 -> 0
var a = -1 / (sg + n.Z);
var b = n.X * n.Y * a;
// column 0: [1 + sg * n.X * n.X * a, sg * b, -sg * n.X]
// column 1: [b, sg + n.Y * n.Y * a, -n.Y]
// column 2: n
return(new M33f(1 + sg * n.X * n.X * a, b, n.X,
sg * b, sg + n.Y * n.Y * a, n.Y,
-sg * n.X, -n.Y, n.Z));
}
public void CanEstimateNormalsAsync_FromZeroToThreePoints()
{
var r = new Random();
for (var n = 0; n < 4; n++)
{
var ps = new V3f[n].SetByIndex(_ => new V3f(r.NextDouble(), r.NextDouble(), r.NextDouble()));
var kd = ps.BuildKdTreeAsync().Result;
var ns = Normals.EstimateNormalsAsync(ps, 16, kd).Result;
Assert.IsTrue(ns.Length == n);
}
}
/// <summary>
/// Code generation.
/// </summary>
public void WriteCornerNeighbours(uint corner)
{
V3f v = s_vecToCornerTableNonNormalized[corner];
string f = " neighbourCodes[{0}] = ";
float s = (float)m_invDoubleRaster;
Console.Write(f, 0); WriteCode(Encode(v + s * new V3f(-1, 0, 0)));
Console.Write(f, 1); WriteCode(Encode(v + s * new V3f(0, -1, 0)));
Console.Write(f, 2); WriteCode(Encode(v + s * new V3f(0, 0, -1)));
Console.Write(f, 3); WriteCode(Encode(v + s * new V3f(1, 0, 0)));
Console.Write(f, 4); WriteCode(Encode(v + s * new V3f(0, 1, 0)));
Console.Write(f, 5); WriteCode(Encode(v + s * new V3f(0, 0, 1)));
}
/// <summary>
/// Calculates a weighted centroid for vectors and weights given by indices.
/// Sum(vectors[indices[i]] * weights[indices[i]]) / Sum(weights[indices[i]].
/// </summary>
public static V3f ComputeCentroid(this V3f[] vectors, float[] weights, int[] indices)
{
V3f sum = V3f.Zero;
float weightSum = 0;
for (int i = 0; i < indices.Length; i++)
{
var w = weights[indices[i]];
sum += w * vectors[indices[i]];
weightSum += w;
}
return(sum / weightSum);
}
