// N is specified, c = Dot(N,P) where P is a point on the plane.
public Plane3d(Vector3d normal, Vector3d point)
{
Normal = normal;
Constant = Normal.Dot(point);
}
public Vector3d Project(Vector3d vPoint, int identifier = -1)
{
Vector3d d = vPoint - Origin;
return(Origin + (d - d.Dot(Normal) * Normal));
}
public bool Test()
{
Vector3d WdU = Vector3d.Zero;
Vector3d AWdU = Vector3d.Zero;
Vector3d DdU = Vector3d.Zero;
Vector3d ADdU = Vector3d.Zero;
Vector3d AWxDdU = Vector3d.Zero;
double RHS;
Vector3d diff = ray.Origin - box.Center;
WdU[0] = ray.Direction.Dot(box.AxisX);
AWdU[0] = Math.Abs(WdU[0]);
DdU[0] = diff.Dot(box.AxisX);
ADdU[0] = Math.Abs(DdU[0]);
if (ADdU[0] > box.Extent.x && DdU[0] * WdU[0] >= (double)0)
{
return(false);
}
WdU[1] = ray.Direction.Dot(box.AxisY);
AWdU[1] = Math.Abs(WdU[1]);
DdU[1] = diff.Dot(box.AxisY);
ADdU[1] = Math.Abs(DdU[1]);
if (ADdU[1] > box.Extent.y && DdU[1] * WdU[1] >= (double)0)
{
return(false);
}
WdU[2] = ray.Direction.Dot(box.AxisZ);
AWdU[2] = Math.Abs(WdU[2]);
DdU[2] = diff.Dot(box.AxisZ);
ADdU[2] = Math.Abs(DdU[2]);
if (ADdU[2] > box.Extent.z && DdU[2] * WdU[2] >= (double)0)
{
return(false);
}
Vector3d WxD = ray.Direction.Cross(diff);
AWxDdU[0] = Math.Abs(WxD.Dot(box.AxisX));
RHS = box.Extent.y * AWdU[2] + box.Extent.z * AWdU[1];
if (AWxDdU[0] > RHS)
{
return(false);
}
AWxDdU[1] = Math.Abs(WxD.Dot(box.AxisY));
RHS = box.Extent.x * AWdU[2] + box.Extent.z * AWdU[0];
if (AWxDdU[1] > RHS)
{
return(false);
}
AWxDdU[2] = Math.Abs(WxD.Dot(box.AxisZ));
RHS = box.Extent.x * AWdU[1] + box.Extent.y * AWdU[0];
if (AWxDdU[2] > RHS)
{
return(false);
}
return(true);
}
public double GetSquared()
{
if (DistanceSquared > 0)
return DistanceSquared;
Vector3d diff = ray.Origin - segment.Center;
double a01 = -ray.Direction.Dot(segment.Direction);
double b0 = diff.Dot(ray.Direction);
double b1 = -diff.Dot(segment.Direction);
double c = diff.LengthSquared;
double det = Math.Abs(1 - a01 * a01);
double s0, s1, sqrDist, extDet;
if (det >= MathUtil.ZeroTolerance) {
// The Ray and Segment are not parallel.
s0 = a01 * b1 - b0;
s1 = a01 * b0 - b1;
extDet = segment.Extent * det;
if (s0 >= 0) {
if (s1 >= -extDet) {
if (s1 <= extDet) // region 0
{
// Minimum at interior points of Ray and Segment.
double invDet = (1) / det;
s0 *= invDet;
s1 *= invDet;
sqrDist = s0 * (s0 + a01 * s1 + (2) * b0) +
s1 * (a01 * s0 + s1 + (2) * b1) + c;
} else // region 1
{
s1 = segment.Extent;
s0 = -(a01 * s1 + b0);
if (s0 > 0) {
sqrDist = -s0 * s0 + s1 * (s1 + (2) * b1) + c;
} else {
s0 = 0;
sqrDist = s1 * (s1 + (2) * b1) + c;
}
}
} else // region 5
{
s1 = -segment.Extent;
s0 = -(a01 * s1 + b0);
if (s0 > 0) {
sqrDist = -s0 * s0 + s1 * (s1 + (2) * b1) + c;
} else {
s0 = 0;
sqrDist = s1 * (s1 + (2) * b1) + c;
}
}
} else {
if (s1 <= -extDet) // region 4
{
s0 = -(-a01 * segment.Extent + b0);
if (s0 > 0) {
s1 = -segment.Extent;
sqrDist = -s0 * s0 + s1 * (s1 + (2) * b1) + c;
} else {
s0 = 0;
s1 = -b1;
if (s1 < -segment.Extent) {
s1 = -segment.Extent;
} else if (s1 > segment.Extent) {
s1 = segment.Extent;
}
sqrDist = s1 * (s1 + (2) * b1) + c;
}
} else if (s1 <= extDet) // region 3
{
s0 = 0;
s1 = -b1;
if (s1 < -segment.Extent) {
s1 = -segment.Extent;
} else if (s1 > segment.Extent) {
s1 = segment.Extent;
}
sqrDist = s1 * (s1 + (2) * b1) + c;
} else // region 2
{
s0 = -(a01 * segment.Extent + b0);
if (s0 > 0) {
s1 = segment.Extent;
sqrDist = -s0 * s0 + s1 * (s1 + (2) * b1) + c;
} else {
s0 = 0;
s1 = -b1;
if (s1 < -segment.Extent) {
s1 = -segment.Extent;
} else if (s1 > segment.Extent) {
s1 = segment.Extent;
}
sqrDist = s1 * (s1 + (2) * b1) + c;
}
}
}
} else {
// Ray and Segment are parallel.
if (a01 > 0) {
// Opposite direction vectors.
s1 = -segment.Extent;
} else {
// Same direction vectors.
s1 = segment.Extent;
}
s0 = -(a01 * s1 + b0);
if (s0 > 0) {
sqrDist = -s0 * s0 + s1 * (s1 + (2) * b1) + c;
} else {
s0 = 0;
sqrDist = s1 * (s1 + (2) * b1) + c;
}
}
RayClosest = ray.Origin + s0 * ray.Direction;
SegmentClosest = segment.Center + s1 * segment.Direction;
RayParameter = s0;
SegmentParameter = s1;
// Account for numerical round-off errors.
if (sqrDist < 0) {
sqrDist = 0;
}
DistanceSquared = sqrDist;
return DistanceSquared;
}
static double edge_flip_metric(ref Vector3d n0, ref Vector3d n1, double flip_dot_tol)
{
return((flip_dot_tol == 0) ? n0.Dot(n1) : n0.Normalized.Dot(n1.Normalized));
}
/// <summary>
/// Compute distance from point to triangle in mesh, with minimal extra objects/etc
/// </summary>
public static double TriDistanceSqr(DMesh3 mesh, int ti, Vector3d point)
{
Vector3d V0 = Vector3d.Zero, V1 = Vector3d.Zero, V2 = Vector3d.Zero;
mesh.GetTriVertices(ti, ref V0, ref V1, ref V2);
Vector3d diff = V0 - point;
Vector3d edge0 = V1 - V0;
Vector3d edge1 = V2 - V0;
double a00 = edge0.LengthSquared;
double a01 = edge0.Dot(ref edge1);
double a11 = edge1.LengthSquared;
double b0 = diff.Dot(ref edge0);
double b1 = diff.Dot(ref edge1);
double c = diff.LengthSquared;
double det = Math.Abs(a00 * a11 - a01 * a01);
double s = a01 * b1 - a11 * b0;
double t = a01 * b0 - a00 * b1;
double sqrDistance;
if (s + t <= det)
{
if (s < 0)
{
if (t < 0) // region 4
{
if (b0 < 0)
{
t = 0;
if (-b0 >= a00)
{
s = 1;
sqrDistance = a00 + (2) * b0 + c;
}
else
{
s = -b0 / a00;
sqrDistance = b0 * s + c;
}
}
else
{
s = 0;
if (b1 >= 0)
{
t = 0;
sqrDistance = c;
}
else if (-b1 >= a11)
{
t = 1;
sqrDistance = a11 + (2) * b1 + c;
}
else
{
t = -b1 / a11;
sqrDistance = b1 * t + c;
}
}
}
else // region 3
{
s = 0;
if (b1 >= 0)
{
t = 0;
sqrDistance = c;
}
else if (-b1 >= a11)
{
t = 1;
sqrDistance = a11 + (2) * b1 + c;
}
else
{
t = -b1 / a11;
sqrDistance = b1 * t + c;
}
}
}
else if (t < 0) // region 5
{
t = 0;
if (b0 >= 0)
{
s = 0;
sqrDistance = c;
}
else if (-b0 >= a00)
{
s = 1;
sqrDistance = a00 + (2) * b0 + c;
}
else
{
s = -b0 / a00;
sqrDistance = b0 * s + c;
}
}
else // region 0
// minimum at interior point
{
double invDet = (1) / det;
s *= invDet;
t *= invDet;
sqrDistance = s * (a00 * s + a01 * t + (2) * b0) +
t * (a01 * s + a11 * t + (2) * b1) + c;
}
}
else
{
double tmp0, tmp1, numer, denom;
if (s < 0) // region 2
{
tmp0 = a01 + b0;
tmp1 = a11 + b1;
if (tmp1 > tmp0)
{
numer = tmp1 - tmp0;
denom = a00 - (2) * a01 + a11;
if (numer >= denom)
{
s = 1;
t = 0;
sqrDistance = a00 + (2) * b0 + c;
}
else
{
s = numer / denom;
t = 1 - s;
sqrDistance = s * (a00 * s + a01 * t + (2) * b0) +
t * (a01 * s + a11 * t + (2) * b1) + c;
}
}
else
{
s = 0;
if (tmp1 <= 0)
{
t = 1;
sqrDistance = a11 + (2) * b1 + c;
}
else if (b1 >= 0)
{
t = 0;
sqrDistance = c;
}
else
{
t = -b1 / a11;
sqrDistance = b1 * t + c;
}
}
}
else if (t < 0) // region 6
{
tmp0 = a01 + b1;
tmp1 = a00 + b0;
if (tmp1 > tmp0)
{
numer = tmp1 - tmp0;
denom = a00 - (2) * a01 + a11;
if (numer >= denom)
{
t = 1;
s = 0;
sqrDistance = a11 + (2) * b1 + c;
}
else
{
t = numer / denom;
s = 1 - t;
sqrDistance = s * (a00 * s + a01 * t + (2) * b0) +
t * (a01 * s + a11 * t + (2) * b1) + c;
}
}
else
{
t = 0;
if (tmp1 <= 0)
{
s = 1;
sqrDistance = a00 + (2) * b0 + c;
}
else if (b0 >= 0)
{
s = 0;
sqrDistance = c;
}
else
{
s = -b0 / a00;
sqrDistance = b0 * s + c;
}
}
}
else // region 1
{
numer = a11 + b1 - a01 - b0;
if (numer <= 0)
{
s = 0;
t = 1;
sqrDistance = a11 + (2) * b1 + c;
}
else
{
denom = a00 - (2) * a01 + a11;
if (numer >= denom)
{
s = 1;
t = 0;
sqrDistance = a00 + (2) * b0 + c;
}
else
{
s = numer / denom;
t = 1 - s;
sqrDistance = s * (a00 * s + a01 * t + (2) * b0) +
t * (a01 * s + a11 * t + (2) * b1) + c;
}
}
}
}
if (sqrDistance < 0)
{
sqrDistance = 0;
}
return(sqrDistance);
}
void constrained_smooth(DGraph3 graph, double surfDist, double dotThresh, double alpha, int rounds)
{
int NV = graph.MaxVertexID;
Vector3d[] pos = new Vector3d[NV];
for (int ri = 0; ri < rounds; ++ri) {
gParallel.ForEach(graph.VertexIndices(), (vid) => {
Vector3d v = graph.GetVertex(vid);
if ( GroundVertices.Contains(vid) || TipVertices.Contains(vid) ) {
pos[vid] = v;
return;
}
// for tip base vertices, we could allow them to move down and away within angle cone...
if (TipBaseVertices.Contains(vid)) {
pos[vid] = v;
return;
}
// compute smoothed position of vtx
Vector3d centroid = Vector3d.Zero; int nbr_count = 0;
foreach (int nbr_vid in graph.VtxVerticesItr(vid)) {
centroid += graph.GetVertex(nbr_vid);
nbr_count++;
}
if (nbr_count == 1) {
pos[vid] = v;
return;
}
centroid /= nbr_count;
Vector3d vnew = (1 - alpha) * v + (alpha) * centroid;
// make sure we don't violate angle constraint to any nbrs
int attempt = 0;
try_again:
foreach ( int nbr_vid in graph.VtxVerticesItr(vid)) {
Vector3d dv = graph.GetVertex(nbr_vid) - vnew;
dv.Normalize();
double dot = dv.Dot(Vector3d.AxisY);
if ( Math.Abs(dot) < dotThresh ) {
if (attempt++ < 3) {
vnew = Vector3d.Lerp(v, vnew, 0.66);
goto try_again;
} else {
pos[vid] = v;
return;
}
}
}
// offset from nearest point on surface
Frame3f fNearest = MeshQueries.NearestPointFrame(Mesh, MeshSpatial, vnew, true);
Vector3d vNearest = fNearest.Origin;
double dist = vnew.Distance(vNearest);
bool inside = MeshSpatial.IsInside(vnew);
if (inside || dist < surfDist) {
Vector3d normal = fNearest.Z;
// don't push down?
if (normal.Dot(Vector3d.AxisY) < 0) {
normal.y = 0; normal.Normalize();
}
vnew = fNearest.Origin + surfDist * normal;
}
pos[vid] = vnew;
});
foreach (int vid in graph.VertexIndices())
graph.SetVertex(vid, pos[vid]);
}
}
private static bool Intersects(ref Triangle3d triangle0, ref Triangle3d triangle1, ref IntersectionType type)
{
// Get edge vectors for triangle0.
Vector3dTuple3 E0;
E0.V0 = triangle0.V1 - triangle0.V0;
E0.V1 = triangle0.V2 - triangle0.V1;
E0.V2 = triangle0.V0 - triangle0.V2;
// Get normal vector of triangle0.
Vector3d N0 = E0.V0.UnitCross(ref E0.V1);
// Project triangle1 onto normal line of triangle0, test for separation.
double N0dT0V0 = N0.Dot(ref triangle0.V0);
double min1, max1;
ProjectOntoAxis(ref triangle1, ref N0, out min1, out max1);
if (N0dT0V0 < min1 || N0dT0V0 > max1)
{
return(false);
}
// Get edge vectors for triangle1.
Vector3dTuple3 E1;
E1.V0 = triangle1.V1 - triangle1.V0;
E1.V1 = triangle1.V2 - triangle1.V1;
E1.V2 = triangle1.V0 - triangle1.V2;
// Get normal vector of triangle1.
Vector3d N1 = E1.V0.UnitCross(ref E1.V1);
Vector3d dir;
double min0, max0;
int i0, i1;
Vector3d N0xN1 = N0.UnitCross(ref N1);
if (N0xN1.Dot(ref N0xN1) >= MathUtil.ZeroTolerance)
{
// Triangles are not parallel.
// Project triangle0 onto normal line of triangle1, test for
// separation.
double N1dT1V0 = N1.Dot(ref triangle1.V0);
ProjectOntoAxis(ref triangle0, ref N1, out min0, out max0);
if (N1dT1V0 < min0 || N1dT1V0 > max0)
{
return(false);
}
// Directions E0[i0]xE1[i1].
for (i1 = 0; i1 < 3; ++i1)
{
for (i0 = 0; i0 < 3; ++i0)
{
dir = E0[i0].UnitCross(E1[i1]); // could pass ref if we reversed these...need to negate?
ProjectOntoAxis(ref triangle0, ref dir, out min0, out max0);
ProjectOntoAxis(ref triangle1, ref dir, out min1, out max1);
if (max0 < min1 || max1 < min0)
{
return(false);
}
}
}
// The test query does not know the intersection set.
type = IntersectionType.Unknown;
}
else // Triangles are parallel (and, in fact, coplanar).
// Directions N0xE0[i0].
{
for (i0 = 0; i0 < 3; ++i0)
{
dir = N0.UnitCross(E0[i0]);
ProjectOntoAxis(ref triangle0, ref dir, out min0, out max0);
ProjectOntoAxis(ref triangle1, ref dir, out min1, out max1);
if (max0 < min1 || max1 < min0)
{
return(false);
}
}
// Directions N1xE1[i1].
for (i1 = 0; i1 < 3; ++i1)
{
dir = N1.UnitCross(E1[i1]);
ProjectOntoAxis(ref triangle0, ref dir, out min0, out max0);
ProjectOntoAxis(ref triangle1, ref dir, out min1, out max1);
if (max0 < min1 || max1 < min0)
{
return(false);
}
}
// The test query does not know the intersection set.
type = IntersectionType.Plane;
}
return(true);
}
public double GetSquared()
{
if (DistanceSquared >= 0)
{
return(DistanceSquared);
}
if (cylinder.Height >= double.MaxValue)
{
return(get_squared_infinite());
}
// Convert the point to the cylinder coordinate system. In this system,
// the point believes (0,0,0) is the cylinder axis origin and (0,0,1) is
// the cylinder axis direction.
Vector3d basis0 = cylinder.Axis.Direction;
Vector3d basis1 = Vector3d.Zero, basis2 = Vector3d.Zero;
Vector3d.ComputeOrthogonalComplement(1, basis0, ref basis1, ref basis2);
double height = Cylinder.Height / 2.0;
Vector3d delta = point - cylinder.Axis.Origin;
var P = new Vector3d(basis1.Dot(delta), basis2.Dot(delta), basis0.Dot(delta));
double result_distance = 0; // signed!
Vector3d result_closest = Vector3d.Zero;
double sqrRadius = cylinder.Radius * cylinder.Radius;
double sqrDistance = P[0] * P[0] + P[1] * P[1];
// The point is outside the infinite cylinder, or on the cylinder wall.
double distance = Math.Sqrt(sqrDistance);
double inf_distance = distance - Cylinder.Radius;
double temp = Cylinder.Radius / distance;
var inf_closest = new Vector3d(temp * P.x, temp * P.y, P.z);
bool bOutside = (sqrDistance >= sqrRadius);
result_closest = inf_closest;
result_distance = inf_distance;
if (inf_closest.z >= height)
{
result_closest = (bOutside) ? inf_closest : P;
result_closest.z = height;
result_distance = result_closest.Distance(P); // TODO: only compute sqr here
bOutside = true;
}
else if (inf_closest.z <= -height)
{
result_closest = (bOutside) ? inf_closest : P;
result_closest.z = -height;
result_distance = result_closest.Distance(P); // TODO: only compute sqr here
bOutside = true;
}
else if (bOutside == false)
{
if (inf_closest.z > 0 && Math.Abs(inf_closest.z - height) < Math.Abs(inf_distance))
{
result_closest = P;
result_closest.z = height;
result_distance = result_closest.Distance(P); // TODO: only compute sqr here
}
else if (inf_closest.z < 0 && Math.Abs(inf_closest.z - -height) < Math.Abs(inf_distance))
{
result_closest = P;
result_closest.z = -height;
result_distance = result_closest.Distance(P); // TODO: only compute sqr here
}
}
SignedDistance = (bOutside) ? Math.Abs(result_distance) : -Math.Abs(result_distance);
// Convert the closest point from the cylinder coordinate system to the
// original coordinate system.
CylinderClosest = cylinder.Axis.Origin +
result_closest.x * basis1 +
result_closest.y * basis2 +
result_closest.z * basis0;
DistanceSquared = result_distance * result_distance;
return(DistanceSquared);
}
/// <summary>
/// minimal intersection test, computes ray-t
/// </summary>
public static bool Intersects(ref Ray3d ray, ref Vector3d V0, ref Vector3d V1, ref Vector3d V2, out double rayT)
{
// Compute the offset origin, edges, and normal.
Vector3d diff = ray.Origin - V0;
Vector3d edge1 = V1 - V0;
Vector3d edge2 = V2 - V0;
Vector3d normal = edge1.Cross(ref edge2);
rayT = double.MaxValue;
// Solve Q + t*D = b1*E1 + b2*E2 (Q = kDiff, D = ray direction,
// E1 = kEdge1, E2 = kEdge2, N = Cross(E1,E2)) by
// |Dot(D,N)|*b1 = sign(Dot(D,N))*Dot(D,Cross(Q,E2))
// |Dot(D,N)|*b2 = sign(Dot(D,N))*Dot(D,Cross(E1,Q))
// |Dot(D,N)|*t = -sign(Dot(D,N))*Dot(Q,N)
double DdN = ray.Direction.Dot(ref normal);
double sign;
if (DdN > MathUtil.ZeroTolerance)
{
sign = 1;
}
else if (DdN < -MathUtil.ZeroTolerance)
{
sign = -1;
DdN = -DdN;
}
else
{
// Ray and triangle are parallel, call it a "no intersection"
// even if the ray does intersect.
return(false);
}
Vector3d cross = diff.Cross(ref edge2);
double DdQxE2 = sign * ray.Direction.Dot(ref cross);
if (DdQxE2 >= 0)
{
cross = edge1.Cross(ref diff);
double DdE1xQ = sign * ray.Direction.Dot(ref cross);
if (DdE1xQ >= 0)
{
if (DdQxE2 + DdE1xQ <= DdN)
{
// Line intersects triangle, check if ray does.
double QdN = -sign *diff.Dot(ref normal);
if (QdN >= 0)
{
// Ray intersects triangle.
double inv = (1) / DdN;
rayT = QdN * inv;
return(true);
}
// else: t < 0, no intersection
}
// else: b1+b2 > 1, no intersection
}
// else: b2 < 0, no intersection
}
// else: b1 < 0, no intersection
return(false);
}
public bool Find()
{
if (Result != IntersectionResult.NotComputed)
{
return(Result != g3.IntersectionResult.NoIntersection);
}
// Compute the offset origin, edges, and normal.
Vector3d diff = ray.Origin - triangle.V0;
Vector3d edge1 = triangle.V1 - triangle.V0;
Vector3d edge2 = triangle.V2 - triangle.V0;
Vector3d normal = edge1.Cross(edge2);
// Solve Q + t*D = b1*E1 + b2*E2 (Q = kDiff, D = ray direction,
// E1 = kEdge1, E2 = kEdge2, N = Cross(E1,E2)) by
// |Dot(D,N)|*b1 = sign(Dot(D,N))*Dot(D,Cross(Q,E2))
// |Dot(D,N)|*b2 = sign(Dot(D,N))*Dot(D,Cross(E1,Q))
// |Dot(D,N)|*t = -sign(Dot(D,N))*Dot(Q,N)
double DdN = ray.Direction.Dot(normal);
double sign;
if (DdN > MathUtil.ZeroTolerance)
{
sign = 1;
}
else if (DdN < -MathUtil.ZeroTolerance)
{
sign = -1;
DdN = -DdN;
}
else
{
// Ray and triangle are parallel, call it a "no intersection"
// even if the ray does intersect.
Result = IntersectionResult.NoIntersection;
return(false);
}
double DdQxE2 = sign * ray.Direction.Dot(diff.Cross(edge2));
if (DdQxE2 >= 0)
{
double DdE1xQ = sign * ray.Direction.Dot(edge1.Cross(diff));
if (DdE1xQ >= 0)
{
if (DdQxE2 + DdE1xQ <= DdN)
{
// Line intersects triangle, check if ray does.
double QdN = -sign *diff.Dot(normal);
if (QdN >= 0)
{
// Ray intersects triangle.
double inv = (1) / DdN;
RayParameter = QdN * inv;
double mTriBary1 = DdQxE2 * inv;
double mTriBary2 = DdE1xQ * inv;
TriangleBaryCoords = new Vector3d(1 - mTriBary1 - mTriBary2, mTriBary1, mTriBary2);
Type = IntersectionType.Point;
Quantity = 1;
Result = IntersectionResult.Intersects;
return(true);
}
// else: t < 0, no intersection
}
// else: b1+b2 > 1, no intersection
}
// else: b2 < 0, no intersection
}
// else: b1 < 0, no intersection
Result = IntersectionResult.NoIntersection;
return(false);
}
public static double DistanceSqr(ref Vector3d point, ref Triangle3d triangle, out Vector3d closestPoint, out Vector3d baryCoords)
{
Vector3d diff = triangle.V0 - point;
Vector3d edge0 = triangle.V1 - triangle.V0;
Vector3d edge1 = triangle.V2 - triangle.V0;
double a00 = edge0.LengthSquared;
double a01 = edge0.Dot(ref edge1);
double a11 = edge1.LengthSquared;
double b0 = diff.Dot(ref edge0);
double b1 = diff.Dot(ref edge1);
double c = diff.LengthSquared;
double det = Math.Abs(a00 * a11 - a01 * a01);
double s = a01 * b1 - a11 * b0;
double t = a01 * b0 - a00 * b1;
double sqrDistance;
if (s + t <= det)
{
if (s < 0)
{
if (t < 0) // region 4
{
if (b0 < 0)
{
t = 0;
if (-b0 >= a00)
{
s = 1;
sqrDistance = a00 + (2) * b0 + c;
}
else
{
s = -b0 / a00;
sqrDistance = b0 * s + c;
}
}
else
{
s = 0;
if (b1 >= 0)
{
t = 0;
sqrDistance = c;
}
else if (-b1 >= a11)
{
t = 1;
sqrDistance = a11 + (2) * b1 + c;
}
else
{
t = -b1 / a11;
sqrDistance = b1 * t + c;
}
}
}
else // region 3
{
s = 0;
if (b1 >= 0)
{
t = 0;
sqrDistance = c;
}
else if (-b1 >= a11)
{
t = 1;
sqrDistance = a11 + (2) * b1 + c;
}
else
{
t = -b1 / a11;
sqrDistance = b1 * t + c;
}
}
}
else if (t < 0) // region 5
{
t = 0;
if (b0 >= 0)
{
s = 0;
sqrDistance = c;
}
else if (-b0 >= a00)
{
s = 1;
sqrDistance = a00 + (2) * b0 + c;
}
else
{
s = -b0 / a00;
sqrDistance = b0 * s + c;
}
}
else // region 0
{
// minimum at interior point
double invDet = (1) / det;
s *= invDet;
t *= invDet;
sqrDistance = s * (a00 * s + a01 * t + (2) * b0) +
t * (a01 * s + a11 * t + (2) * b1) + c;
}
}
else
{
double tmp0, tmp1, numer, denom;
if (s < 0) // region 2
{
tmp0 = a01 + b0;
tmp1 = a11 + b1;
if (tmp1 > tmp0)
{
numer = tmp1 - tmp0;
denom = a00 - (2) * a01 + a11;
if (numer >= denom)
{
s = 1;
t = 0;
sqrDistance = a00 + (2) * b0 + c;
}
else
{
s = numer / denom;
t = 1 - s;
sqrDistance = s * (a00 * s + a01 * t + (2) * b0) +
t * (a01 * s + a11 * t + (2) * b1) + c;
}
}
else
{
s = 0;
if (tmp1 <= 0)
{
t = 1;
sqrDistance = a11 + (2) * b1 + c;
}
else if (b1 >= 0)
{
t = 0;
sqrDistance = c;
}
else
{
t = -b1 / a11;
sqrDistance = b1 * t + c;
}
}
}
else if (t < 0) // region 6
{
tmp0 = a01 + b1;
tmp1 = a00 + b0;
if (tmp1 > tmp0)
{
numer = tmp1 - tmp0;
denom = a00 - (2) * a01 + a11;
if (numer >= denom)
{
t = 1;
s = 0;
sqrDistance = a11 + (2) * b1 + c;
}
else
{
t = numer / denom;
s = 1 - t;
sqrDistance = s * (a00 * s + a01 * t + (2) * b0) +
t * (a01 * s + a11 * t + (2) * b1) + c;
}
}
else
{
t = 0;
if (tmp1 <= 0)
{
s = 1;
sqrDistance = a00 + (2) * b0 + c;
}
else if (b0 >= 0)
{
s = 0;
sqrDistance = c;
}
else
{
s = -b0 / a00;
sqrDistance = b0 * s + c;
}
}
}
else // region 1
{
numer = a11 + b1 - a01 - b0;
if (numer <= 0)
{
s = 0;
t = 1;
sqrDistance = a11 + (2) * b1 + c;
}
else
{
denom = a00 - (2) * a01 + a11;
if (numer >= denom)
{
s = 1;
t = 0;
sqrDistance = a00 + (2) * b0 + c;
}
else
{
s = numer / denom;
t = 1 - s;
sqrDistance = s * (a00 * s + a01 * t + (2) * b0) +
t * (a01 * s + a11 * t + (2) * b1) + c;
}
}
}
}
closestPoint = triangle.V0 + s * edge0 + t * edge1;
baryCoords = new Vector3d(1 - s - t, s, t);
// Account for numerical round-off error.
return(Math.Max(sqrDistance, 0));
}
public double GetSquared()
{
if (DistanceSquared >= 0)
{
return(DistanceSquared);
}
// Test if line intersects triangle. If so, the squared distance is zero.
Vector3d edge0 = triangle.V1 - triangle.V0;
Vector3d edge1 = triangle.V2 - triangle.V0;
Vector3d normal = edge0.UnitCross(edge1);
double NdD = normal.Dot(line.Direction);
if (Math.Abs(NdD) > MathUtil.ZeroTolerance)
{
// The line and triangle are not parallel, so the line intersects
// the plane of the triangle.
Vector3d diff = line.Origin - triangle.V0;
Vector3d U = Vector3d.Zero, V = Vector3d.Zero;
Vector3d.GenerateComplementBasis(ref U, ref V, line.Direction);
double UdE0 = U.Dot(edge0);
double UdE1 = U.Dot(edge1);
double UdDiff = U.Dot(diff);
double VdE0 = V.Dot(edge0);
double VdE1 = V.Dot(edge1);
double VdDiff = V.Dot(diff);
double invDet = (1) / (UdE0 * VdE1 - UdE1 * VdE0);
// Barycentric coordinates for the point of intersection.
double b1 = (VdE1 * UdDiff - UdE1 * VdDiff) * invDet;
double b2 = (UdE0 * VdDiff - VdE0 * UdDiff) * invDet;
double b0 = 1 - b1 - b2;
if (b0 >= 0 && b1 >= 0 && b2 >= 0)
{
// Line parameter for the point of intersection.
double DdE0 = line.Direction.Dot(edge0);
double DdE1 = line.Direction.Dot(edge1);
double DdDiff = line.Direction.Dot(diff);
LineParam = b1 * DdE0 + b2 * DdE1 - DdDiff;
// Barycentric coordinates for the point of intersection.
TriangleBaryCoords = new Vector3d(b0, b1, b2);
// The intersection point is inside or on the triangle.
LineClosest = line.Origin + LineParam * line.Direction;
TriangleClosest = triangle.V0 + b1 * edge0 + b2 * edge1;
DistanceSquared = 0;
return(0);
}
}
// Either (1) the line is not parallel to the triangle and the point of
// intersection of the line and the plane of the triangle is outside the
// triangle or (2) the line and triangle are parallel. Regardless, the
// closest point on the triangle is on an edge of the triangle. Compare
// the line to all three edges of the triangle.
double sqrDist = double.MaxValue;
for (int i0 = 2, i1 = 0; i1 < 3; i0 = i1++)
{
var segment = new Segment3d(triangle[i0], triangle[i1]);
var queryLS = new DistLine3Segment3(line, segment);
double sqrDistTmp = queryLS.GetSquared();
if (sqrDistTmp < sqrDist)
{
LineClosest = queryLS.LineClosest;
TriangleClosest = queryLS.SegmentClosest;
sqrDist = sqrDistTmp;
LineParam = queryLS.LineParameter;
double ratio = queryLS.SegmentParameter / segment.Extent;
TriangleBaryCoords = Vector3d.Zero;
TriangleBaryCoords[i0] = (0.5) * (1 - ratio);
TriangleBaryCoords[i1] = 1 - TriangleBaryCoords[i0];
TriangleBaryCoords[3 - i0 - i1] = 0;
}
}
DistanceSquared = sqrDist;
return(DistanceSquared);
}
void generate_support(Vector3f origin, float dx,
int ni, int nj, int nk,
DenseGrid3f supportGrid)
{
supportGrid.resize(ni, nj, nk);
supportGrid.assign(1); // sentinel
if (DebugPrint) System.Console.WriteLine("start");
bool CHECKERBOARD = false;
System.Console.WriteLine("Computing SDF");
// compute unsigned SDF
MeshSignedDistanceGrid sdf = new MeshSignedDistanceGrid(Mesh, CellSize) {
ComputeSigns = true, ExactBandWidth = 3,
/*,ComputeMode = MeshSignedDistanceGrid.ComputeModes.FullGrid*/ };
sdf.CancelF = Cancelled;
sdf.Compute();
if (Cancelled())
return;
var distanceField = new DenseGridTrilinearImplicit(sdf.Grid, sdf.GridOrigin, sdf.CellSize);
double angle = MathUtil.Clamp(OverhangAngleDeg, 0.01, 89.99);
double cos_thresh = Math.Cos(angle * MathUtil.Deg2Rad);
System.Console.WriteLine("Marking overhangs");
// Compute narrow-band distances. For each triangle, we find its grid-coord-bbox,
// and compute exact distances within that box. The intersection_count grid
// is also filled in this computation
double ddx = (double)dx;
double ox = (double)origin[0], oy = (double)origin[1], oz = (double)origin[2];
Vector3d va = Vector3d.Zero, vb = Vector3d.Zero, vc = Vector3d.Zero;
foreach (int tid in Mesh.TriangleIndices()) {
if (tid % 100 == 0 && Cancelled())
break;
Mesh.GetTriVertices(tid, ref va, ref vb, ref vc);
Vector3d normal = MathUtil.Normal(ref va, ref vb, ref vc);
if (normal.Dot(-Vector3d.AxisY) < cos_thresh)
continue;
// real ijk coordinates of va/vb/vc
double fip = (va[0] - ox) / ddx, fjp = (va[1] - oy) / ddx, fkp = (va[2] - oz) / ddx;
double fiq = (vb[0] - ox) / ddx, fjq = (vb[1] - oy) / ddx, fkq = (vb[2] - oz) / ddx;
double fir = (vc[0] - ox) / ddx, fjr = (vc[1] - oy) / ddx, fkr = (vc[2] - oz) / ddx;
// clamped integer bounding box of triangle plus exact-band
int exact_band = 0;
int i0 = MathUtil.Clamp(((int)MathUtil.Min(fip, fiq, fir)) - exact_band, 0, ni - 1);
int i1 = MathUtil.Clamp(((int)MathUtil.Max(fip, fiq, fir)) + exact_band + 1, 0, ni - 1);
int j0 = MathUtil.Clamp(((int)MathUtil.Min(fjp, fjq, fjr)) - exact_band, 0, nj - 1);
int j1 = MathUtil.Clamp(((int)MathUtil.Max(fjp, fjq, fjr)) + exact_band + 1, 0, nj - 1);
int k0 = MathUtil.Clamp(((int)MathUtil.Min(fkp, fkq, fkr)) - exact_band, 0, nk - 1);
int k1 = MathUtil.Clamp(((int)MathUtil.Max(fkp, fkq, fkr)) + exact_band + 1, 0, nk - 1);
// don't put into y=0 plane
if (j0 == 0)
j0 = 1;
// compute distance for each tri inside this bounding box
// note: this can be very conservative if the triangle is large and on diagonal to grid axes
for (int k = k0; k <= k1; ++k) {
for (int j = j0; j <= j1; ++j) {
for (int i = i0; i <= i1; ++i) {
Vector3d gx = new Vector3d((float)i * dx + origin[0], (float)j * dx + origin[1], (float)k * dx + origin[2]);
float d = (float)MeshSignedDistanceGrid.point_triangle_distance(ref gx, ref va, ref vb, ref vc);
// vertical checkerboard pattern (eg 'tips')
if (CHECKERBOARD) {
int zz = (k % 2 == 0) ? 1 : 0;
if (i % 2 == zz)
continue;
}
if (d < dx / 2) {
if (j > 1) {
supportGrid[i, j, k] = SUPPORT_TIP_TOP;
supportGrid[i, j - 1, k] = SUPPORT_TIP_BASE;
} else {
supportGrid[i, j, k] = SUPPORT_TIP_BASE;
}
}
}
}
}
}
if (Cancelled())
return;
//process_version1(supportGrid, distanceField);
//process_version2(supportGrid, distanceField);
generate_graph(supportGrid, distanceField);
//Util.WriteDebugMesh(MakeDebugGraphMesh(), "c:\\scratch\\__LAST_GRAPH_INIT.obj");
postprocess_graph();
//Util.WriteDebugMesh(MakeDebugGraphMesh(), "c:\\scratch\\__LAST_GRAPH_OPT.obj");
}
// Compute d = Dot(N,P)-c where N is the plane normal and c is the plane
// constant. This is a signed distance. The sign of the return value is
// positive if the point is on the positive side of the plane, negative if
// the point is on the negative side, and zero if the point is on the
// plane.
public double DistanceTo(Vector3d p)
{
return(Normal.Dot(p) - Constant);
}
public double GetSquared()
{
if (DistanceSquared >= 0)
{
return(DistanceSquared);
}
Vector3d diff = ray1.Origin - ray2.Origin;
double a01 = -ray1.Direction.Dot(ray2.Direction);
double b0 = diff.Dot(ray1.Direction);
double c = diff.LengthSquared;
double det = Math.Abs(1.0 - a01 * a01);
double b1, s0, s1, sqrDist;
if (det >= MathUtil.ZeroTolerance)
{
// Rays are not parallel.
b1 = -diff.Dot(ray2.Direction);
s0 = a01 * b1 - b0;
s1 = a01 * b0 - b1;
if (s0 >= 0)
{
if (s1 >= 0)
{ // region 0 (interior)
// Minimum at two interior points of rays.
double invDet = (1.0) / det;
s0 *= invDet;
s1 *= invDet;
sqrDist = s0 * (s0 + a01 * s1 + (2.0) * b0) +
s1 * (a01 * s0 + s1 + (2.0) * b1) + c;
}
else
{ // region 3 (side)
s1 = 0;
if (b0 >= 0)
{
s0 = 0;
sqrDist = c;
}
else
{
s0 = -b0;
sqrDist = b0 * s0 + c;
}
}
}
else
{
if (s1 >= 0)
{ // region 1 (side)
s0 = 0;
if (b1 >= 0)
{
s1 = 0;
sqrDist = c;
}
else
{
s1 = -b1;
sqrDist = b1 * s1 + c;
}
}
else
{ // region 2 (corner)
if (b0 < 0)
{
s0 = -b0;
s1 = 0;
sqrDist = b0 * s0 + c;
}
else
{
s0 = 0;
if (b1 >= 0)
{
s1 = 0;
sqrDist = c;
}
else
{
s1 = -b1;
sqrDist = b1 * s1 + c;
}
}
}
}
}
else
{
// Rays are parallel.
if (a01 > 0)
{
// Opposite direction vectors.
s1 = 0;
if (b0 >= 0)
{
s0 = 0;
sqrDist = c;
}
else
{
s0 = -b0;
sqrDist = b0 * s0 + c;
}
}
else
{
// Same direction vectors.
if (b0 >= 0)
{
b1 = -diff.Dot(ray2.Direction);
s0 = 0;
s1 = -b1;
sqrDist = b1 * s1 + c;
}
else
{
s0 = -b0;
s1 = 0;
sqrDist = b0 * s0 + c;
}
}
}
Ray1Closest = ray1.Origin + s0 * ray1.Direction;
Ray2Closest = ray2.Origin + s1 * ray2.Direction;
Ray1Parameter = s0;
Ray2Parameter = s1;
// Account for numerical round-off errors.
if (sqrDist < 0)
{
sqrDist = 0;
}
DistanceSquared = sqrDist;
return(sqrDist);
}
public double GetSquared()
{
if (DistanceSquared >= 0)
{
return(DistanceSquared);
}
Vector3d diff = line.Origin - segment.Center;
double a01 = -line.Direction.Dot(segment.Direction);
double b0 = diff.Dot(line.Direction);
double c = diff.LengthSquared;
double det = Math.Abs(1 - a01 * a01);
double b1, s0, s1, sqrDist, extDet;
if (det >= MathUtil.ZeroTolerance)
{
// The line and segment are not parallel.
b1 = -diff.Dot(segment.Direction);
s1 = a01 * b0 - b1;
extDet = segment.Extent * det;
if (s1 >= -extDet)
{
if (s1 <= extDet)
{
// Two interior points are closest, one on the line and one
// on the segment.
double invDet = (1) / det;
s0 = (a01 * b1 - b0) * invDet;
s1 *= invDet;
sqrDist = s0 * (s0 + a01 * s1 + (2) * b0) +
s1 * (a01 * s0 + s1 + (2) * b1) + c;
}
else
{
// The endpoint e1 of the segment and an interior point of
// the line are closest.
s1 = segment.Extent;
s0 = -(a01 * s1 + b0);
sqrDist = -s0 * s0 + s1 * (s1 + (2) * b1) + c;
}
}
else
{
// The end point e0 of the segment and an interior point of the
// line are closest.
s1 = -segment.Extent;
s0 = -(a01 * s1 + b0);
sqrDist = -s0 * s0 + s1 * (s1 + (2) * b1) + c;
}
}
else
{
// The line and segment are parallel. Choose the closest pair so that
// one point is at segment center.
s1 = 0;
s0 = -b0;
sqrDist = b0 * s0 + c;
}
LineClosest = line.Origin + s0 * line.Direction;
SegmentClosest = segment.Center + s1 * segment.Direction;
LineParameter = s0;
SegmentParameter = s1;
// Account for numerical round-off errors.
if (sqrDist < 0)
{
sqrDist = 0;
}
DistanceSquared = sqrDist;
return(sqrDist);
}
public double InnerProduct(ref Matrix3d m2)
{
return(Row0.Dot(ref m2.Row0) + Row1.Dot(ref m2.Row1) + Row2.Dot(ref m2.Row2));
}
/// <summary>
/// test if ray intersects box.
/// expandExtents allows you to scale box for hit-testing purposes.
/// </summary>
public static bool Intersects(ref Ray3d ray, ref Box3d box, double expandExtents = 0)
{
Vector3d WdU = Vector3d.Zero;
Vector3d AWdU = Vector3d.Zero;
Vector3d DdU = Vector3d.Zero;
Vector3d ADdU = Vector3d.Zero;
Vector3d AWxDdU = Vector3d.Zero;
double RHS;
Vector3d diff = ray.Origin - box.Center;
Vector3d extent = box.Extent + expandExtents;
WdU[0] = ray.Direction.Dot(ref box.AxisX);
AWdU[0] = Math.Abs(WdU[0]);
DdU[0] = diff.Dot(ref box.AxisX);
ADdU[0] = Math.Abs(DdU[0]);
if (ADdU[0] > extent.x && DdU[0] * WdU[0] >= (double)0)
{
return(false);
}
WdU[1] = ray.Direction.Dot(ref box.AxisY);
AWdU[1] = Math.Abs(WdU[1]);
DdU[1] = diff.Dot(ref box.AxisY);
ADdU[1] = Math.Abs(DdU[1]);
if (ADdU[1] > extent.y && DdU[1] * WdU[1] >= (double)0)
{
return(false);
}
WdU[2] = ray.Direction.Dot(ref box.AxisZ);
AWdU[2] = Math.Abs(WdU[2]);
DdU[2] = diff.Dot(ref box.AxisZ);
ADdU[2] = Math.Abs(DdU[2]);
if (ADdU[2] > extent.z && DdU[2] * WdU[2] >= (double)0)
{
return(false);
}
Vector3d WxD = ray.Direction.Cross(diff);
AWxDdU[0] = Math.Abs(WxD.Dot(ref box.AxisX));
RHS = extent.y * AWdU[2] + extent.z * AWdU[1];
if (AWxDdU[0] > RHS)
{
return(false);
}
AWxDdU[1] = Math.Abs(WxD.Dot(ref box.AxisY));
RHS = extent.x * AWdU[2] + extent.z * AWdU[0];
if (AWxDdU[1] > RHS)
{
return(false);
}
AWxDdU[2] = Math.Abs(WxD.Dot(ref box.AxisZ));
RHS = extent.x * AWdU[1] + extent.y * AWdU[0];
if (AWxDdU[2] > RHS)
{
return(false);
}
return(true);
}
void generate_support(Vector3f origin, float dx,
int ni, int nj, int nk,
DenseGrid3f supportGrid)
{
supportGrid.resize(ni, nj, nk);
supportGrid.assign(1); // sentinel
bool CHECKERBOARD = false;
// compute unsigned SDF
int exact_band = 1;
if (SubtractMesh && SubtractMeshOffset > 0)
{
int offset_band = (int)(SubtractMeshOffset / CellSize) + 1;
exact_band = Math.Max(exact_band, offset_band);
}
sdf = new MeshSignedDistanceGrid(Mesh, CellSize)
{
ComputeSigns = true, ExactBandWidth = exact_band
};
sdf.CancelF = this.CancelF;
sdf.Compute();
if (CancelF())
{
return;
}
var distanceField = new DenseGridTrilinearImplicit(sdf.Grid, sdf.GridOrigin, sdf.CellSize);
double angle = MathUtil.Clamp(OverhangAngleDeg, 0.01, 89.99);
double cos_thresh = Math.Cos(angle * MathUtil.Deg2Rad);
// Compute narrow-band distances. For each triangle, we find its grid-coord-bbox,
// and compute exact distances within that box. The intersection_count grid
// is also filled in this computation
double ddx = (double)dx;
double ox = (double)origin[0], oy = (double)origin[1], oz = (double)origin[2];
Vector3d va = Vector3d.Zero, vb = Vector3d.Zero, vc = Vector3d.Zero;
foreach (int tid in Mesh.TriangleIndices())
{
if (tid % 100 == 0 && CancelF())
{
break;
}
Mesh.GetTriVertices(tid, ref va, ref vb, ref vc);
Vector3d normal = MathUtil.Normal(ref va, ref vb, ref vc);
if (normal.Dot(-Vector3d.AxisY) < cos_thresh)
{
continue;
}
// real ijk coordinates of va/vb/vc
double fip = (va[0] - ox) / ddx, fjp = (va[1] - oy) / ddx, fkp = (va[2] - oz) / ddx;
double fiq = (vb[0] - ox) / ddx, fjq = (vb[1] - oy) / ddx, fkq = (vb[2] - oz) / ddx;
double fir = (vc[0] - ox) / ddx, fjr = (vc[1] - oy) / ddx, fkr = (vc[2] - oz) / ddx;
// clamped integer bounding box of triangle plus exact-band
int extra_band = 0;
int i0 = MathUtil.Clamp(((int)MathUtil.Min(fip, fiq, fir)) - extra_band, 0, ni - 1);
int i1 = MathUtil.Clamp(((int)MathUtil.Max(fip, fiq, fir)) + extra_band + 1, 0, ni - 1);
int j0 = MathUtil.Clamp(((int)MathUtil.Min(fjp, fjq, fjr)) - extra_band, 0, nj - 1);
int j1 = MathUtil.Clamp(((int)MathUtil.Max(fjp, fjq, fjr)) + extra_band + 1, 0, nj - 1);
int k0 = MathUtil.Clamp(((int)MathUtil.Min(fkp, fkq, fkr)) - extra_band, 0, nk - 1);
int k1 = MathUtil.Clamp(((int)MathUtil.Max(fkp, fkq, fkr)) + extra_band + 1, 0, nk - 1);
// don't put into y=0 plane
//if (j0 == 0)
// j0 = 1;
// compute distance for each tri inside this bounding box
// note: this can be very conservative if the triangle is large and on diagonal to grid axes
for (int k = k0; k <= k1; ++k)
{
for (int j = j0; j <= j1; ++j)
{
for (int i = i0; i <= i1; ++i)
{
Vector3d gx = new Vector3d((float)i * dx + origin[0], (float)j * dx + origin[1], (float)k * dx + origin[2]);
float d = (float)MeshSignedDistanceGrid.point_triangle_distance(ref gx, ref va, ref vb, ref vc);
// vertical checkerboard pattern (eg 'tips')
if (CHECKERBOARD)
{
int zz = (k % 2 == 0) ? 1 : 0;
if (i % 2 == zz)
{
continue;
}
}
if (d < dx / 2)
{
supportGrid[i, j, k] = SUPPORT_TIP_TOP;
}
}
}
}
}
if (CancelF())
{
return;
}
fill_vertical_spans(supportGrid, distanceField);
generate_mesh(supportGrid, distanceField);
}
