// 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); }