/// <summary>
/// Returns true if the ray hits the other ray before the parameter
/// value contained in the supplied hit. Detailed information about
/// the hit is returned in the supplied hit.
/// </summary>
public bool HitsRay(
Ray3d ray,
double tmin, double tmax,
ref RayHit3d hit
)
{
V3d d = Origin - ray.Origin;
V3d u = Direction;
V3d v = ray.Direction;
V3d n = u.Cross(v);
if (Fun.IsTiny(d.Length))
{
return(true);
}
else if (Fun.IsTiny(u.Cross(v).Length))
{
return(false);
}
else
{
//-t0*u + t1*v + t2*n == d
//M = {-u,v,n}
//M*{t0,t1,t2}T == d
//{t0,t1,t2}T == M^-1 * d
M33d M = new M33d();
M.C0 = -u;
M.C1 = v;
M.C2 = n;
if (M.Invertible)
{
V3d t = M.Inverse * d;
if (Fun.IsTiny(t.Z))
{
ProcessHits(t.X, double.MaxValue, tmin, tmax, ref hit);
return(true);
}
else
{
return(false);
}
}
else
{
return(false);
}
}
}
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);
}
/// <summary>
/// The geometric center of a 3-dimensional, flat polygon.
/// WARNING: UNTESTED!
/// </summary>
public static V3d ComputeCentroid(this Polygon3d polygon)
{
var pc = polygon.PointCount;
if (pc < 3)
{
return(V3d.Zero);
}
V3d p0 = polygon[0], p1 = polygon[1];
V3d e0 = p1 - p0;
var p2 = polygon[2];
var e1 = p2 - p0;
var normal = e0.Cross(e1);
var area2 = normal.Length;
var centroid = area2 * (p0 + p1 + p2);
p1 = p2; e0 = e1;
for (int pi = 3; pi < pc; pi++)
{
p2 = polygon[pi]; e1 = p2 - p0;
var n = e0.Cross(e1);
var a2 = Fun.Sign(normal.Dot(n)) * n.Length;
area2 += a2;
centroid += a2 * (p0 + p1 + p2);
p1 = p2; e0 = e1;
}
if (area2 > Constant <double> .PositiveTinyValue)
{
return(centroid * (Constant.OneThird / area2));
}
else if (area2 < Constant <double> .NegativeTinyValue)
{
return(centroid * (-Constant.OneThird / area2));
}
else
{
return(V3d.Zero);
}
}
/// <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);
}
internal static void AddTriangle(
BspTreeBuilder builder, int tiMul3, ref BspNode node)
{
Triangle3d tr;
builder.GetTriangleVertexPositions(tiMul3, out tr.P0, out tr.P1, out tr.P2);
V3d e0 = tr.P1 - tr.P0;
V3d e1 = tr.P2 - tr.P0;
V3d n = V3d.Cross(e0, e1);
double len2 = n.LengthSquared;
if (len2 > 0.0)
{
AddTriangle(builder, tiMul3, ref tr,
n * (1.0 / Math.Sqrt(len2)),
ref node);
}
}
public void ObjectHitInfo(
ObjectRayHit hit,
ref ObjectHitInfo info
)
{
List <V3d> pl = Position3dList;
int pi = hit.SetObject.Index * 3;
info.Points = new V3d[3] {
pl[pi], pl[pi + 1], pl[pi + 2]
};
V3d e01 = info.Points[1] - info.Points[0];
V3d e02 = info.Points[2] - info.Points[0];
info.Edges = new V3d[2] {
e01, e02
};
info.Normal = V3d.Cross(e01, e02).Normalized;
}
/// <summary>
/// Computes the normal with the length of twice the polygon area as
/// the sum of the simple triangulation cross-product normals.
/// NOTE: This has been tested to be slightly faster and slightly more
/// accurate than the computation via the 3d Newell normal
/// (see Math.Tests/GeometryTests, rft 2013-05-04).
/// </summary>
public static V3d ComputeDoubleAreaNormal(this Polygon3d polygon)
{
var pc = polygon.PointCount;
if (pc < 3)
{
return(V3d.Zero);
}
V3d p0 = polygon[0];
V3d e0 = polygon[1] - p0;
V3d normal = V3d.Zero;
for (int pi = 2; pi < pc; pi++)
{
var e1 = polygon[pi] - p0;
normal += e0.Cross(e1);
e0 = e1;
}
return(normal);
}
public static bool IsLinearCombinationOf(this V3d x, V3d u, V3d v, out double t0, out double t1)
{
//x == t2*u + t1*v
V3d n = u.Cross(v);
double[,] mat = new double[3, 3]
{
{ u.X, v.X, n.X },
{ u.Y, v.Y, n.Y },
{ u.Z, v.Z, n.Z }
};
double[] result = new double[3] {
x.X, x.Y, x.Z
};
int[] perm = mat.LuFactorize();
V3d t = new V3d(mat.LuSolve(perm, result));
if (Fun.IsTiny(t.Z))
{
t0 = t.X;
t1 = t.Y;
return(true);
}
else
{
t0 = double.NaN;
t1 = double.NaN;
return(false);
}
//x ==
}
/// <summary>
/// Creates a left-handed view trafo, where z-positive points into the scene.
/// </summary>
public static Trafo3d ViewTrafoLH(V3d location, V3d up, V3d forward)
{
return(Trafo3d.ViewTrafo(location, up.Cross(forward), up, forward));
}
/// <summary>
/// Creates a right-handed view trafo, where z-negative points into the scene.
/// </summary>
public static Trafo3d ViewTrafoRH(V3d location, V3d up, V3d forward)
{
return(Trafo3d.ViewTrafo(location, forward.Cross(up), up, -forward));
}
// 3-Dimensional
#region V3d - V3d
public static bool IsParallelTo(this V3d u, V3d v)
=> Fun.IsTiny(u.Cross(v).Norm1);
public static bool IsLinearCombinationOf(this V3d x, V3d u, V3d v)
{
V3d n = u.Cross(v);
return(n.IsOrthogonalTo(x));
}
/// <summary> /// Creates a right-handed view trafo, where z-negative points into the scene. /// </summary> public static Trafo3d ViewTrafoRH(V3d location, V3d up, V3d forward) => ViewTrafo(location, forward.Cross(up), up, -forward);
public bool HitsCylinder(Cylinder3d cylinder,
double tmin, double tmax,
ref RayHit3d hit)
{
var axisDir = cylinder.Axis.Direction.Normalized;
// Vector Cyl.P0 -> Ray.Origin
var op = Origin - cylinder.P0;
// normal RayDirection - CylinderAxis
var normal = Direction.Cross(axisDir);
var unitNormal = normal.Normalized;
// normal (Vec Cyl.P0 -> Ray.Origin) - CylinderAxis
var normal2 = op.Cross(axisDir);
var t = -normal2.Dot(unitNormal) / normal.Length;
var radius = cylinder.Radius;
if (cylinder.DistanceScale != 0)
{ // cylinder gets bigger, the further away it is
var pnt = GetPointOnRay(t);
var dis = V3d.Distance(pnt, this.Origin);
radius = ((cylinder.Radius / cylinder.DistanceScale) * dis) * 2;
}
// between enitre rays (caps are ignored)
var shortestDistance = Fun.Abs(op.Dot(unitNormal));
if (shortestDistance <= radius)
{
var s = Fun.Abs(Fun.Sqrt(radius.Square() - shortestDistance.Square()) / Direction.Length);
var t1 = t - s; // first hit of Cylinder shell
var t2 = t + s; // second hit of Cylinder shell
if (t1 > tmin && t1 < tmax)
{
tmin = t1;
}
if (t2 < tmax && t2 > tmin)
{
tmax = t2;
}
hit.T = t1;
hit.Point = GetPointOnRay(t1);
// check if found point is outside of Cylinder Caps
var bottomPlane = new Plane3d(cylinder.Circle0.Normal, cylinder.Circle0.Center);
var topPlane = new Plane3d(cylinder.Circle1.Normal, cylinder.Circle1.Center);
var heightBottom = bottomPlane.Height(hit.Point);
var heightTop = topPlane.Height(hit.Point);
// t1 lies outside of caps => find closest cap hit
if (heightBottom > 0 || heightTop > 0)
{
hit.T = tmax;
// intersect with bottom Cylinder Cap
var bottomHit = HitsPlane(bottomPlane, tmin, tmax, ref hit);
// intersect with top Cylinder Cap
var topHit = HitsPlane(topPlane, tmin, tmax, ref hit);
// hit still close enough to cylinder axis?
var distance = cylinder.Axis.Ray3d.GetMinimalDistanceTo(hit.Point);
if (distance <= radius && (bottomHit || topHit))
{
return(true);
}
}
else
{
return(true);
}
}
hit.T = tmax;
hit.Point = V3d.NaN;
return(false);
}
// 3-Dimensional
#region V3d - V3d
public static bool IsParallelTo(this V3d u, V3d v)
{
return(Fun.IsTiny(u.Cross(v).Norm1));
}
/// <summary> /// Creates a left-handed view trafo, where z-positive points into the scene. /// </summary> public static Trafo3d ViewTrafoLH(V3d location, V3d up, V3d forward) => ViewTrafo(location, up.Cross(forward), up, forward);
/// <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);
}
