diff --git a/README.md b/README.md index 6218dad..33c11e6 100644 --- a/README.md +++ b/README.md @@ -16,6 +16,7 @@ A rewrite of the original CAD package [`sdfx`](https://github.com/deadsy/sdfx) f * End-to-end testing using image comparison. * `must` and `form` packages provide panicking and normal error handling basic shape generation APIs for different scenarios. * Dead-simple, single method `Renderer` interface. +* **Import mesh files**: Edit STL and 3MF files as if they were native SDFs using [`sdfexp.ImportModel`](./helpers/sdfexp/import.go) ## Examples diff --git a/helpers/sdfexp/import.go b/helpers/sdfexp/import.go new file mode 100644 index 0000000..0ec7630 --- /dev/null +++ b/helpers/sdfexp/import.go @@ -0,0 +1,202 @@ +package sdfexp + +import ( + "errors" + "fmt" + "math" + + "github.com/soypat/sdf/internal/d3" + "github.com/soypat/sdf/render" + "gonum.org/v1/gonum/spatial/kdtree" + "gonum.org/v1/gonum/spatial/r3" +) + +// ImportModel instantiates an SDF3 from a set of triangles defining +// a manifold surface. It can be used to import SDFs from triangle files +// such as STL and 3MF files. It will choose shared vertices among triangles +// using vertexTol. +// vertexTol should be of the order of 1/1000th of the size of the smallest +// triangle in the model. If set to 0 then it is inferred automatically. +func ImportModel(model []render.Triangle3, vertexTolOrZero float64) (ImportedSDF3, error) { + m, err := newMesh(model, vertexTolOrZero) + if err != nil { + return ImportedSDF3{}, err + } + tree := kdtree.New(m, true) + return ImportedSDF3{tree: *tree, mesh: m}, nil +} + +type ImportedSDF3 struct { + tree kdtree.Tree + mesh *mesh +} + +func (s ImportedSDF3) Evaluate(q r3.Vec) float64 { + tri, dist2 := s.tree.Nearest(&meshTriangle{C: q}) + kd := tri.(*meshTriangle) + return kd.CopySign(q, math.Sqrt(dist2)) +} + +func (s ImportedSDF3) Bounds() r3.Box { + return r3.Box{ + Min: s.mesh.bb.Min, + Max: s.mesh.bb.Max, + } +} + +type mesh struct { + // bb is the bounding box of the whole mesh. + bb d3.Box + vertices []pseudoVertex + triangles []meshTriangle + // access to edge pseudo normals using vertex index. + // Stored with lower index first. + pseudoEdgeN map[[2]int]r3.Vec +} + +type pseudoVertex struct { + V r3.Vec + // N is the weighted pseudo normal where the weights + // are the opening angle formed by edges for the triangle. + N r3.Vec // Vertex Normal +} + +func newMesh(triangles []render.Triangle3, tol float64) (*mesh, error) { + bb := d3.Box{d3.Elem(math.MaxFloat64), d3.Elem(-math.MaxFloat64)} + minDist2 := math.MaxFloat64 + maxDist2 := -math.MaxFloat64 + for i := range triangles { + for j, vert := range triangles[i] { + // Calculate bounding box + bb.Min = d3.MinElem(bb.Min, vert) + bb.Max = d3.MaxElem(bb.Max, vert) + // Calculate minimum side + vert2 := triangles[i][(j+1)%3] + side2 := r3.Norm2(r3.Sub(vert2, vert)) + minDist2 = math.Min(minDist2, side2) + maxDist2 = math.Max(maxDist2, side2) + } + } + m := &mesh{ + bb: bb, + triangles: make([]meshTriangle, len(triangles)), + pseudoEdgeN: make(map[[2]int]r3.Vec), + } + suggested := math.Sqrt(minDist2) / 256 + if tol > math.Sqrt(maxDist2)/2 { + return nil, fmt.Errorf("vertex tolerance is too large to generate appropiate mesh, suggested tolerance: %g", suggested) + } + if tol == 0 { + tol = suggested + } + size := bb.Size() + maxDim := math.Max(size.X, math.Max(size.Y, size.Z)) + div := int64(maxDim/tol + 1e-12) + if div <= 0 { + return nil, errors.New("tolerance larger than model size") + } + if div > math.MaxInt64/2 { + return nil, errors.New("tolerance too small. overflowed int64") + } + //vertex index cache + cache := make(map[[3]int64]int) + ri := 1 / tol + for i, tri := range triangles { + norm := tri.Normal() + Tform := canalisTransform(tri) + InvT := Tform.Inv() + sdfT := meshTriangle{ + N: r3.Scale(2*math.Pi, norm), + C: centroid(tri), + T: Tform, + InvT: InvT, + m: m, + } + for j, vert := range triangles[i] { + // Scale vert to be integer in resolution-space. + v := r3.Scale(ri, vert) + vi := [3]int64{int64(v.X), int64(v.Y), int64(v.Z)} + vertexIdx, ok := cache[vi] + if !ok { + // Initialize the vertex if not in cache. + vertexIdx = len(m.vertices) + cache[vi] = vertexIdx + m.vertices = append(m.vertices, pseudoVertex{ + V: vert, + }) + } + // Calculate vertex pseudo normal + s1, s2 := r3.Sub(vert, tri[(j+1)%3]), r3.Sub(vert, tri[(j+2)%3]) + alpha := math.Acos(r3.Cos(s1, s2)) + m.vertices[vertexIdx].N = r3.Add(m.vertices[vertexIdx].N, r3.Scale(alpha, norm)) + sdfT.Vertices[j] = vertexIdx + } + m.triangles[i] = sdfT + // Calculate edge pseudo normals. + for j := range sdfT.Vertices { + edge := [2]int{sdfT.Vertices[j], sdfT.Vertices[(j+1)%3]} + if edge[0] > edge[1] { + edge[0], edge[1] = edge[1], edge[0] + } + m.pseudoEdgeN[edge] = r3.Add(m.pseudoEdgeN[edge], r3.Scale(math.Pi, norm)) + } + } + return m, nil +} + +// Index returns the ith element of the list of points. +func (tr *mesh) Index(i int) kdtree.Comparable { return &tr.triangles[i] } + +// Len returns the length of the list. +func (tr *mesh) Len() int { return len(tr.triangles) } + +// Pivot partitions the list based on the dimension specified. +func (tr *mesh) Pivot(d kdtree.Dim) int { + p := kdPlane{dim: int(d), triangles: tr.triangles} + return kdtree.Partition(p, kdtree.MedianOfMedians(p)) +} + +// Slice returns a slice of the list using zero-based half +// open indexing equivalent to built-in slice indexing. +func (tr *mesh) Slice(start, end int) kdtree.Interface { + newmesh := *tr + newmesh.triangles = newmesh.triangles[start:end] + return &newmesh +} + +// Bounds implements the kdtree.Bounder interface and expects +// a calculation based on current triangles which may be modified +// by kdtree.New() +func (tr *mesh) Bounds() *kdtree.Bounding { + min := meshTriangle{C: r3.Vec{X: math.MaxFloat64, Y: math.MaxFloat64, Z: math.MaxFloat64}} + max := meshTriangle{C: r3.Vec{X: -math.MaxFloat64, Y: -math.MaxFloat64, Z: -math.MaxFloat64}} + for _, t := range tr.triangles { + min.C = d3.MinElem(min.C, t.C) + max.C = d3.MaxElem(max.C, t.C) + } + return &kdtree.Bounding{ + Min: &min, + Max: &max, + } +} + +type kdPlane struct { + dim int + triangles []meshTriangle +} + +func (p kdPlane) Less(i, j int) bool { + ti := &p.triangles[i] + tj := &p.triangles[j] + return ti.Compare(tj, kdtree.Dim(p.dim)) < 0 +} +func (p kdPlane) Swap(i, j int) { + p.triangles[i], p.triangles[j] = p.triangles[j], p.triangles[i] +} +func (p kdPlane) Len() int { + return len(p.triangles) +} +func (p kdPlane) Slice(start, end int) kdtree.SortSlicer { + p.triangles = p.triangles[start:end] + return p +} diff --git a/helpers/sdfexp/import_test.go b/helpers/sdfexp/import_test.go new file mode 100644 index 0000000..5339fbd --- /dev/null +++ b/helpers/sdfexp/import_test.go @@ -0,0 +1,32 @@ +package sdfexp_test + +import ( + "testing" + + "github.com/soypat/sdf/form3" + "github.com/soypat/sdf/form3/obj3/thread" + "github.com/soypat/sdf/helpers/sdfexp" + "github.com/soypat/sdf/render" +) + +func TestImportModel(t *testing.T) { + const quality = 80 + s, _ := form3.Sphere(5) + s, _ = thread.Bolt(thread.BoltParms{ + Thread: thread.ISO{D: 16, P: 2}, + Style: thread.NutHex, + TotalLength: 20, + }) + model, err := render.RenderAll(render.NewOctreeRenderer(s, quality)) + if err != nil { + t.Fatal(err) + } + sdf, err := sdfexp.ImportModel(model, 0) + if err != nil { + t.Fatal(err) + } + err = render.CreateSTL("imported.stl", render.NewOctreeRenderer(sdf, quality)) + if err != nil { + t.Fatal(err) + } +} diff --git a/helpers/sdfexp/spatial2.go b/helpers/sdfexp/spatial2.go new file mode 100644 index 0000000..f6c8459 --- /dev/null +++ b/helpers/sdfexp/spatial2.go @@ -0,0 +1,98 @@ +package sdfexp + +import ( + "math" + + "gonum.org/v1/gonum/spatial/r2" +) + +// General purpose 2D spatial functions. + +type triangleFeature int + +const ( + featureV0 triangleFeature = iota + featureV1 + featureV2 + featureE0 + featureE1 + featureE2 + featureFace +) + +func closestOnTriangle2(p r2.Vec, tri [3]r2.Vec) (pointOnTriangle r2.Vec, feature triangleFeature) { + if inTriangle(p, tri) { + return p, featureFace + } + minDist := math.MaxFloat64 + for j := range tri { + edge := [2]r2.Vec{{X: tri[j].X, Y: tri[j].Y}, {X: tri[(j+1)%3].X, Y: tri[(j+1)%3].Y}} + distance, gotFeat := distToLine(p, edge) + d2 := r2.Norm2(distance) + if d2 < minDist { + if gotFeat < 2 { + feature = triangleFeature(j+gotFeat) % 3 + } else { + feature = featureE0 + triangleFeature(j)%3 + } + minDist = d2 + pointOnTriangle = r2.Sub(p, distance) + } + } + return pointOnTriangle, feature +} + +// inTriangle returns true if pt is contained in bounds +// defined by triangle vertices tri. +func inTriangle(pt r2.Vec, tri [3]r2.Vec) bool { + d1 := d2Sign(pt, tri[0], tri[1]) + d2 := d2Sign(pt, tri[1], tri[2]) + d3 := d2Sign(pt, tri[2], tri[0]) + has_neg := (d1 < 0) || (d2 < 0) || (d3 < 0) + has_pos := (d1 > 0) || (d2 > 0) || (d3 > 0) + return !(has_neg && has_pos) +} + +func d2Sign(p1, p2, p3 r2.Vec) float64 { + return (p1.X-p3.X)*(p2.Y-p3.Y) - (p2.X-p3.X)*(p1.Y-p3.Y) +} + +// distToLine returns distance vector from point to line. +// The integer returns 0 if closest to first vertex, 1 if closest +// to second vertex and 2 if closest to the line edge between vertices. +func distToLine(p r2.Vec, ln [2]r2.Vec) (r2.Vec, int) { + lineDir := r2.Sub(ln[1], ln[0]) + perpendicular := r2.Vec{-lineDir.Y, lineDir.X} + perpend2 := r2.Add(ln[1], perpendicular) + e2 := edgeEquation(p, [2]r2.Vec{ln[1], perpend2}) + if e2 > 0 { + return r2.Sub(p, ln[1]), 0 + } + perpend1 := r2.Add(ln[0], perpendicular) + e1 := edgeEquation(p, [2]r2.Vec{ln[0], perpend1}) + if e1 < 0 { + return r2.Sub(p, ln[0]), 1 + } + e3 := distToLineInfinite(p, ln) //edgeEquation(p, line) + return r2.Scale(-e3, r2.Unit(perpendicular)), 2 +} + +// line passes through two points P1 = (x1, y1) and P2 = (x2, y2) +// then the distance of (x0, y0) +func distToLineInfinite(p r2.Vec, line [2]r2.Vec) float64 { + // https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line + p1 := line[0] + p2 := line[1] + num := math.Abs((p2.X-p1.X)*(p1.Y-p.Y) - (p1.X-p.X)*(p2.Y-p1.Y)) + return num / math.Hypot(p2.X-p1.X, p2.Y-p1.Y) +} + +// edgeEquation returns a signed distance of a point to +// an infinite line defined by two points +// Edge equation for a line passing through (X,Y) +// with gradient dY/dX +// E ( x; y ) =(x-X)*dY - (y-Y)*dX +func edgeEquation(p r2.Vec, line [2]r2.Vec) float64 { + dxy := r2.Sub(line[1], line[0]) + return (p.X-line[0].X)*dxy.Y - (p.Y-line[0].Y)*dxy.X +} diff --git a/helpers/sdfexp/spatial3.go b/helpers/sdfexp/spatial3.go new file mode 100644 index 0000000..c19632e --- /dev/null +++ b/helpers/sdfexp/spatial3.go @@ -0,0 +1,129 @@ +package sdfexp + +import ( + "math" + + "github.com/soypat/sdf/internal/d3" + "github.com/soypat/sdf/render" + "gonum.org/v1/gonum/spatial/kdtree" + "gonum.org/v1/gonum/spatial/r2" + "gonum.org/v1/gonum/spatial/r3" +) + +// General purpose 3D spatial functions and types +// with special focus on kdTriangle type. + +type meshTriangle struct { + C r3.Vec // Centroid + lastFeature triangleFeature // result from last distance calculation + lastClosest r3.Vec + Vertices [3]int + m *mesh // to be able to construct triangle geometry. + N r3.Vec // Pseudo Face normal (scaled by 2*pi) + T d3.Transform // Canalis transformation matrix. + InvT d3.Transform // inverse of T +} + +func (t *meshTriangle) Compare(c kdtree.Comparable, d kdtree.Dim) float64 { + q := c.(*meshTriangle) + switch d { + case 0: + return t.C.X - q.C.X + case 1: + return t.C.Y - q.C.Y + case 2: + return t.C.Z - q.C.Z + } + panic("unreachable") +} + +func (t *meshTriangle) Dims() int { return 3 } + +func (t *meshTriangle) Distance(c kdtree.Comparable) float64 { + point := c.(*meshTriangle) + if t.isPoint() { + if point.isPoint() { + return r3.Norm2(r3.Sub(t.C, point.C)) + } + point, t = t, point // make sure `t` is the triangle. + } + pxy := t.T.Transform(point.C) + txy := t.triangle() + for i := range txy { + txy[i] = t.T.Transform(txy[i]) + } + // We find the closest point to the transformed triangle + // in 2D space and then transform the results back to 3D space + onTriangle, feat := closestOnTriangle2(lowerVec(pxy), [3]r2.Vec{lowerVec(txy[0]), lowerVec(txy[1]), lowerVec(txy[2])}) + t.lastFeature = feat + t.lastClosest = t.InvT.Transform(r3.Vec{X: onTriangle.X, Y: onTriangle.Y}) + return r3.Norm2(r3.Sub(point.C, t.lastClosest)) +} + +// CopySign returns a value with the magnitude of dist +// and the sign depending on whether the last call to Distance was +// inside or outside the solid defined by the mesh (SDF). +// Copysign expects p to be the same vector as last call to Distance. +func (t *meshTriangle) CopySign(p r3.Vec, dist float64) (signed float64) { + if t.lastFeature <= featureV2 { + // Distance last called nearest to triangle vertex. + vertex := t.m.vertices[t.Vertices[t.lastFeature]] + signed = r3.Dot(vertex.N, r3.Sub(p, vertex.V)) + } else if t.lastFeature <= featureE2 { + vertex1 := t.lastFeature - 3 + edge := [2]int{t.Vertices[vertex1], t.Vertices[(vertex1+1)%3]} + if edge[0] > edge[1] { + edge[0], edge[1] = edge[1], edge[0] + } + norm := t.m.pseudoEdgeN[edge] + signed = r3.Dot(norm, r3.Sub(p, t.lastClosest)) + } else { + signed = r3.Dot(t.N, r3.Sub(p, t.lastClosest)) + } + return math.Copysign(dist, signed) +} + +func (t *meshTriangle) triangle() render.Triangle3 { + return render.Triangle3{ + t.m.vertices[t.Vertices[0]].V, + t.m.vertices[t.Vertices[1]].V, + t.m.vertices[t.Vertices[2]].V, + } +} + +// canalisTransform courtesy of Agustin Canalis (acanalis). +// Returns a transformation for a triangle so that: +// - the triangle's first edge (t_0,t_1) is on the X axis +// - the triangle's first vertex t_0 is at the origin +// - the triangle's last vertex t_2 is in the XY plane. +func canalisTransform(t render.Triangle3) d3.Transform { + u2 := r3.Sub(t[1], t[0]) + u3 := r3.Sub(t[2], t[0]) + + xc := r3.Unit(u2) + yc := r3.Sub(u3, r3.Scale(r3.Dot(xc, u3), xc)) // t[2] but no X component + yc = r3.Unit(yc) + zc := r3.Cross(xc, yc) + + // Create rotation transform. + T := d3.NewTransform([]float64{ + xc.X, xc.Y, xc.Z, 0, + yc.X, yc.Y, yc.Z, 0, + zc.X, zc.Y, zc.Z, 0, + 0, 0, 0, 1, + }) + t0T := T.Transform(t[0]) + return T.Translate(r3.Scale(-1, t0T)) // add offset. +} + +func (t *meshTriangle) isPoint() bool { + return t.N == (r3.Vec{}) // uninitialized fields. +} + +func lowerVec(v r3.Vec) r2.Vec { + return r2.Vec{X: v.X, Y: v.Y} +} + +func centroid(t render.Triangle3) r3.Vec { + return r3.Scale(1./3., r3.Add(r3.Add(t[0], t[1]), t[2])) +} diff --git a/internal/d3/transform.go b/internal/d3/transform.go new file mode 100644 index 0000000..a3d4e11 --- /dev/null +++ b/internal/d3/transform.go @@ -0,0 +1,264 @@ +package d3 + +import ( + "math" + + "gonum.org/v1/gonum/spatial/r3" +) + +// Transform represents a 3D spatial transformation. +// The zero value of Transform is the identity transform. +type Transform struct { + // in order to make the zero value of Transform represent the identity + // transform we store it with the identity matrix subtracted. + // These diagonal elements are subtracted such that + // d00 = x00-1, d11 = x11-1, d22 = x22-1, d33 = x33-1 + // where x00, x11, x22, x33 are the matrix diagonal elements. + // We can then check for identity in if blocks like so: + // if T == (Transform{}) + d00, x01, x02, x03 float64 + x10, d11, x12, x13 float64 + x20, x21, d22, x23 float64 + x30, x31, x32, d33 float64 +} + +// Transform applies the Transform to the argument vector +// and returns the result. +func (t Transform) Transform(v r3.Vec) r3.Vec { + // https://github.com/mrdoob/three.js/blob/dev/src/math/Vector3.js#L262 + w := 1 / (t.x30*v.X + t.x31*v.Y + t.x32*v.Z + t.d33 + 1) + return r3.Vec{ + X: ((t.d00+1)*v.X + t.x01*v.Y + t.x02*v.Z + t.x03) * w, + Y: (t.x10*v.X + (t.d11+1)*v.Y + t.x12*v.Z + t.x13) * w, + Z: (t.x20*v.X + t.x21*v.Y + (t.d22+1)*v.Z + t.x23) * w, + } +} + +// zeroTransform is the Transform that returns zeroTransform when multiplied by any Transform. +var zeroTransform = Transform{d00: -1, d11: -1, d22: -1, d33: -1} + +// NewTransform returns a new Transform type and populates its elements +// with values passed in row-major form. If val is nil then NewTransform +// returns a Transform filled with zeros. +func NewTransform(a []float64) Transform { + if a == nil { + return zeroTransform + } + if len(a) != 16 { + panic("Transform is initialized with 16 values") + } + return Transform{ + d00: a[0] - 1, x01: a[1], x02: a[2], x03: a[3], + x10: a[4], d11: a[5] - 1, x12: a[6], x13: a[7], + x20: a[8], x21: a[9], d22: a[10] - 1, x23: a[11], + x30: a[12], x31: a[13], x32: a[14], d33: a[15] - 1, + } +} + +// ComposeTransform creates a new transform for a given translation to +// positon, scaling vector scale and quaternion rotation. +// The identity Transform is constructed with +// ComposeTransform(Vec{}, Vec{1,1,1}, Rotation{}) +func ComposeTransform(position, scale r3.Vec, q r3.Rotation) Transform { + x2 := q.Imag + q.Imag + y2 := q.Jmag + q.Jmag + z2 := q.Kmag + q.Kmag + xx := q.Imag * x2 + yy := q.Jmag * y2 + zz := q.Kmag * z2 + xy := q.Imag * y2 + xz := q.Imag * z2 + yz := q.Jmag * z2 + wx := q.Real * x2 + wy := q.Real * y2 + wz := q.Real * z2 + + var t Transform + t.d00 = (1-(yy+zz))*scale.X - 1 + t.x10 = (xy + wz) * scale.X + t.x20 = (xz - wy) * scale.X + + t.x01 = (xy - wz) * scale.Y + t.d11 = (1-(xx+zz))*scale.Y - 1 + t.x21 = (yz + wx) * scale.Y + + t.x02 = (xz + wy) * scale.Z + t.x12 = (yz - wx) * scale.Z + t.d22 = (1-(xx+yy))*scale.Z - 1 + + t.x03 = position.X + t.x13 = position.Y + t.x23 = position.Z + return t +} + +// Translate adds Vec to the positional Transform. +func (t Transform) Translate(v r3.Vec) Transform { + t.x03 += v.X + t.x13 += v.Y + t.x23 += v.Z + return t +} + +// Scale returns the transform with scaling added around +// the argumnt origin. +func (t Transform) Scale(origin, factor r3.Vec) Transform { + if origin == (r3.Vec{}) { + return t.scale(factor) + } + t = t.Translate(r3.Scale(-1, origin)) + t = t.scale(factor) + return t.Translate(origin) +} + +func (t Transform) scale(factor r3.Vec) Transform { + t.d00 = (t.d00+1)*factor.X - 1 + t.x10 *= factor.X + t.x20 *= factor.X + t.x30 *= factor.X + + t.x01 *= factor.Y + t.d11 = (t.d11+1)*factor.Y - 1 + t.x21 *= factor.Y + t.x31 *= factor.Y + + t.x02 *= factor.Z + t.x12 *= factor.Z + t.d22 = (t.d22+1)*factor.Z - 1 + t.x32 *= factor.Z + return t +} + +// Mul multiplies the Transforms a and b and returns the result. +// This is the equivalent of combining two transforms in one. +func (t Transform) Mul(b Transform) Transform { + if t == (Transform{}) { + return b + } + if b == (Transform{}) { + return t + } + x00 := t.d00 + 1 + x11 := t.d11 + 1 + x22 := t.d22 + 1 + x33 := t.d33 + 1 + y00 := b.d00 + 1 + y11 := b.d11 + 1 + y22 := b.d22 + 1 + y33 := b.d33 + 1 + var m Transform + m.d00 = x00*y00 + t.x01*b.x10 + t.x02*b.x20 + t.x03*b.x30 - 1 + m.x10 = t.x10*y00 + x11*b.x10 + t.x12*b.x20 + t.x13*b.x30 + m.x20 = t.x20*y00 + t.x21*b.x10 + x22*b.x20 + t.x23*b.x30 + m.x30 = t.x30*y00 + t.x31*b.x10 + t.x32*b.x20 + x33*b.x30 + m.x01 = x00*b.x01 + t.x01*y11 + t.x02*b.x21 + t.x03*b.x31 + m.d11 = t.x10*b.x01 + x11*y11 + t.x12*b.x21 + t.x13*b.x31 - 1 + m.x21 = t.x20*b.x01 + t.x21*y11 + x22*b.x21 + t.x23*b.x31 + m.x31 = t.x30*b.x01 + t.x31*y11 + t.x32*b.x21 + x33*b.x31 + m.x02 = x00*b.x02 + t.x01*b.x12 + t.x02*y22 + t.x03*b.x32 + m.x12 = t.x10*b.x02 + x11*b.x12 + t.x12*y22 + t.x13*b.x32 + m.d22 = t.x20*b.x02 + t.x21*b.x12 + x22*y22 + t.x23*b.x32 - 1 + m.x32 = t.x30*b.x02 + t.x31*b.x12 + t.x32*y22 + x33*b.x32 + m.x03 = x00*b.x03 + t.x01*b.x13 + t.x02*b.x23 + t.x03*y33 + m.x13 = t.x10*b.x03 + x11*b.x13 + t.x12*b.x23 + t.x13*y33 + m.x23 = t.x20*b.x03 + t.x21*b.x13 + x22*b.x23 + t.x23*y33 + m.d33 = t.x30*b.x03 + t.x31*b.x13 + t.x32*b.x23 + x33*y33 - 1 + return m +} + +// Det returns the determinant of the Transform. +func (t Transform) Det() float64 { + x00 := t.d00 + 1 + x11 := t.d11 + 1 + x22 := t.d22 + 1 + x33 := t.d33 + 1 + return x00*x11*x22*x33 - x00*x11*t.x23*t.x32 + + x00*t.x12*t.x23*t.x31 - x00*t.x12*t.x21*x33 + + x00*t.x13*t.x21*t.x32 - x00*t.x13*x22*t.x31 - + t.x01*t.x12*t.x23*t.x30 + t.x01*t.x12*t.x20*x33 - + t.x01*t.x13*t.x20*t.x32 + t.x01*t.x13*x22*t.x30 - + t.x01*t.x10*x22*x33 + t.x01*t.x10*t.x23*t.x32 + + t.x02*t.x13*t.x20*t.x31 - t.x02*t.x13*t.x21*t.x30 + + t.x02*t.x10*t.x21*x33 - t.x02*t.x10*t.x23*t.x31 + + t.x02*x11*t.x23*t.x30 - t.x02*x11*t.x20*x33 - + t.x03*t.x10*t.x21*t.x32 + t.x03*t.x10*x22*t.x31 - + t.x03*x11*x22*t.x30 + t.x03*x11*t.x20*t.x32 - + t.x03*t.x12*t.x20*t.x31 + t.x03*t.x12*t.x21*t.x30 +} + +// Inv returns the inverse of the transform such that +// t.Inv() * t is the identity Transform. +// If matrix is singular then Inv() returns the zero transform. +func (t Transform) Inv() Transform { + if t == (Transform{}) { + return t + } + det := t.Det() + if math.Abs(det) < 1e-16 { + return zeroTransform + } + // Do something if singular? + d := 1 / det + x00 := t.d00 + 1 + x11 := t.d11 + 1 + x22 := t.d22 + 1 + x33 := t.d33 + 1 + var m Transform + m.d00 = (t.x12*t.x23*t.x31-t.x13*x22*t.x31+t.x13*t.x21*t.x32-x11*t.x23*t.x32-t.x12*t.x21*x33+x11*x22*x33)*d - 1 + m.x01 = (t.x03*x22*t.x31 - t.x02*t.x23*t.x31 - t.x03*t.x21*t.x32 + t.x01*t.x23*t.x32 + t.x02*t.x21*x33 - t.x01*x22*x33) * d + m.x02 = (t.x02*t.x13*t.x31 - t.x03*t.x12*t.x31 + t.x03*x11*t.x32 - t.x01*t.x13*t.x32 - t.x02*x11*x33 + t.x01*t.x12*x33) * d + m.x03 = (t.x03*t.x12*t.x21 - t.x02*t.x13*t.x21 - t.x03*x11*x22 + t.x01*t.x13*x22 + t.x02*x11*t.x23 - t.x01*t.x12*t.x23) * d + m.x10 = (t.x13*x22*t.x30 - t.x12*t.x23*t.x30 - t.x13*t.x20*t.x32 + t.x10*t.x23*t.x32 + t.x12*t.x20*x33 - t.x10*x22*x33) * d + m.d11 = (t.x02*t.x23*t.x30-t.x03*x22*t.x30+t.x03*t.x20*t.x32-x00*t.x23*t.x32-t.x02*t.x20*x33+x00*x22*x33)*d - 1 + m.x12 = (t.x03*t.x12*t.x30 - t.x02*t.x13*t.x30 - t.x03*t.x10*t.x32 + x00*t.x13*t.x32 + t.x02*t.x10*x33 - x00*t.x12*x33) * d + m.x13 = (t.x02*t.x13*t.x20 - t.x03*t.x12*t.x20 + t.x03*t.x10*x22 - x00*t.x13*x22 - t.x02*t.x10*t.x23 + x00*t.x12*t.x23) * d + m.x20 = (x11*t.x23*t.x30 - t.x13*t.x21*t.x30 + t.x13*t.x20*t.x31 - t.x10*t.x23*t.x31 - x11*t.x20*x33 + t.x10*t.x21*x33) * d + m.x21 = (t.x03*t.x21*t.x30 - t.x01*t.x23*t.x30 - t.x03*t.x20*t.x31 + x00*t.x23*t.x31 + t.x01*t.x20*x33 - x00*t.x21*x33) * d + m.d22 = (t.x01*t.x13*t.x30-t.x03*x11*t.x30+t.x03*t.x10*t.x31-x00*t.x13*t.x31-t.x01*t.x10*x33+x00*x11*x33)*d - 1 + m.x23 = (t.x03*x11*t.x20 - t.x01*t.x13*t.x20 - t.x03*t.x10*t.x21 + x00*t.x13*t.x21 + t.x01*t.x10*t.x23 - x00*x11*t.x23) * d + m.x30 = (t.x12*t.x21*t.x30 - x11*x22*t.x30 - t.x12*t.x20*t.x31 + t.x10*x22*t.x31 + x11*t.x20*t.x32 - t.x10*t.x21*t.x32) * d + m.x31 = (t.x01*x22*t.x30 - t.x02*t.x21*t.x30 + t.x02*t.x20*t.x31 - x00*x22*t.x31 - t.x01*t.x20*t.x32 + x00*t.x21*t.x32) * d + m.x32 = (t.x02*x11*t.x30 - t.x01*t.x12*t.x30 - t.x02*t.x10*t.x31 + x00*t.x12*t.x31 + t.x01*t.x10*t.x32 - x00*x11*t.x32) * d + m.d33 = (t.x01*t.x12*t.x20-t.x02*x11*t.x20+t.x02*t.x10*t.x21-x00*t.x12*t.x21-t.x01*t.x10*x22+x00*x11*x22)*d - 1 + return m +} + +func (t Transform) transpose() Transform { + return Transform{ + d00: t.d00, x01: t.x10, x02: t.x20, x03: t.x30, + x10: t.x01, d11: t.d11, x12: t.x21, x13: t.x31, + x20: t.x02, x21: t.x12, d22: t.d22, x23: t.x32, + x30: t.x03, x31: t.x13, x32: t.x23, d33: t.d33, + } +} + +// equals tests the equality of the Transforms to within a tolerance. +func (t Transform) equals(b Transform, tolerance float64) bool { + return math.Abs(t.d00-b.d00) < tolerance && + math.Abs(t.x01-b.x01) < tolerance && + math.Abs(t.x02-b.x02) < tolerance && + math.Abs(t.x03-b.x03) < tolerance && + math.Abs(t.x10-b.x10) < tolerance && + math.Abs(t.d11-b.d11) < tolerance && + math.Abs(t.x12-b.x12) < tolerance && + math.Abs(t.x13-b.x13) < tolerance && + math.Abs(t.x20-b.x20) < tolerance && + math.Abs(t.x21-b.x21) < tolerance && + math.Abs(t.d22-b.d22) < tolerance && + math.Abs(t.x23-b.x23) < tolerance && + math.Abs(t.x30-b.x30) < tolerance && + math.Abs(t.x31-b.x31) < tolerance && + math.Abs(t.x32-b.x32) < tolerance && + math.Abs(t.d33-b.d33) < tolerance +} + +// SliceCopy returns a copy of the Transform's data +// in row major storage format. It returns 16 elements. +func (t Transform) SliceCopy() []float64 { + return []float64{ + t.d00 + 1, t.x01, t.x02, t.x03, + t.x10, t.d11 + 1, t.x12, t.x13, + t.x20, t.x21, t.d22 + 1, t.x23, + t.x30, t.x31, t.x32, t.d33 + 1, + } +} diff --git a/internal/d3/triangle.go b/internal/d3/triangle.go deleted file mode 100644 index 9d4d3fc..0000000 --- a/internal/d3/triangle.go +++ /dev/null @@ -1,11 +0,0 @@ -package d3 - -import "gonum.org/v1/gonum/spatial/r3" - -type r3Triangle [3]r3.Vec - -// Closest returns closest point on the triangle to argument point p. -func (t r3Triangle) Closest(p r3.Vec) r3.Vec { - // Calculate transformation matrix so that - return r3.Vec{} -}