refactor: rotation API changes; testing; refactoring

Author: soypat

md2/polygon.go

@@ -10,6 +10,7 @@ import (
 
 	math "math"
 	"github.com/soypat/geometry/internal"
+	ms1 "github.com/soypat/geometry/md1"
 )
 
 type cpAtIdxErr struct {
@@ -208,22 +209,24 @@ func appendArcWithCenter(dst []Vec, start, center Vec, arcAngle float64, facets
 
 func arcCenterFrom2points(p1, p2 Vec, r float64) (Vec, float64, error) {
 	rabs := math.Abs(r)
+	semiArcTol := rabs * 1e-3
 	V12 := Sub(p2, p1)
 	chordCenter := Add(p1, Scale(0.5, V12))
 	chordLen := Norm(V12)   // Chord length.
 	maxChordLen := 2 * rabs // Make sure the perimeter of a arc with infinite radius is less than the chord.
+
+	// Theta is the opening angle from the center of the arc circle
+	// to the two chord points.
+	// Due to chord definition theta/2 is the angle formed
+	// by the chord and the tangent to the chord point.
+	sinTheta := chordLen / maxChordLen
 	if chordLen == 0 {
 		return Vec{}, 0, errArcCPEqualToPrev
-	} else if maxChordLen-chordLen < -internal.Smallfloat64 {
+	} else if maxChordLen-chordLen <= -semiArcTol {
 		return Vec{}, 0, errBadArc
-	} else if math.Abs(chordLen/maxChordLen-1) < internal.Smallfloat64 {
+	} else if ms1.EqualWithinAbs(sinTheta, 1, semiArcTol) {
 		return Scale(0.5, Add(p1, p2)), math.Copysign(math.Pi/2, r), nil
 	}
-	// Theta is the opening angle from the center of the arc circle
-	// to the two chord points.
-	// Due to chord definition theta/2 is the angle formed
-	// by the chord and the tangent to the chord point.
-	sinTheta := chordLen / (2 * rabs)
 	chordThetaDiv2 := math.Asin(sinTheta)
 	diffTo90 := chordThetaDiv2 - math.Pi/2
 	if math.Abs(diffTo90) < internal.Smallfloat64/10 {

md2/polygon_test.go

@@ -141,28 +141,30 @@ func TestPolygon_IsClockwise(t *testing.T) {
 func TestArc_invalidArc(t *testing.T) {
 	var cases = []struct {
 		start, end, center Vec
+		isValid            bool
 	}{
-		// 180 degree:
-		// {start: Vec{X: 165.36844, Y: 125.63215}, end: Vec{X: 165.59346, Y: 125.85769}, center: Vec{X: 165.48108, Y: 125.74479}},
-		//
-		{start: Vec{X: 168.19885, Y: 129.9802}, end: Vec{X: 167.97331, Y: 129.75517}, center: Vec{X: 168.08597, Y: 129.86781}},
-		{start: Vec{X: 135.1107, Y: 116.67478}, end: Vec{X: 135.1107, Y: 116.67478}, center: Vec{X: 135.10947, Y: 116.673546}},
-		{start: Vec{X: -1.05, Y: 149.07}, end: Vec{X: -1.12, Y: 148.7}, center: Vec{X: -1.0500132, Y: 149}},
+
+		0: {start: Vec{X: 135.1107, Y: 116.67478}, end: Vec{X: 135.1107, Y: 116.67478}, center: Vec{X: 135.10947, Y: 116.673546}},
+		1: {start: Vec{X: -1.05, Y: 149.07}, end: Vec{X: -1.12, Y: 148.7}, center: Vec{X: -1.0500132, Y: 149}},
+		// Close to 180 Degree arcs:
+		2: {isValid: true, start: Vec{X: -40.52799, Y: -25.628536}, end: Vec{X: -40.88199, Y: -25.628536}, center: Vec{X: -40.704987, Y: -25.628536}},
+		3: {isValid: true, start: Vec{X: 165.36844, Y: 125.63215}, end: Vec{X: 165.59346, Y: 125.85769}, center: Vec{X: 165.48108, Y: 125.74479}},
+		4: {isValid: true, start: Vec{X: 168.19885, Y: 129.9802}, end: Vec{X: 167.97331, Y: 129.75517}, center: Vec{X: 168.08597, Y: 129.86781}},
 	}
 	var poly PolygonBuilder
-	for _, test := range cases {
+	for i, test := range cases {
 		start, end, center := test.start, test.end, test.center
 		radius1 := Norm(Sub(start, center))
 		radius2 := Norm(Sub(end, center))
 		if math.Abs(radius1-radius2)/radius1 > 0.001 {
-			t.Log("start/end not equidistant from center:", radius1, radius2)
+			t.Log("start/end not equidistant from center:", i, radius1, radius2)
 		}
 		poly.Reset()
 		poly.Add(start)
 		poly.Add(end).Arc(radius1, 3)
 		vecs, err := poly.AppendVecs(nil)
-		if err == nil {
-			t.Error("expected invalid input")
+		if err == nil && !test.isValid {
+			t.Error("expected invalid input", i)
 			for _, v := range vecs {
 				if v != v {
 					t.Error("effectively got NaNs")
@@ -173,6 +175,15 @@ func TestArc_invalidArc(t *testing.T) {
 					t.Errorf("bad radius got=%f want=%f", gotRadius, radius1)
 				}
 			}
+		} else if test.isValid {
+			if err != nil {
+				t.Error("error for valid input", i)
+			}
+			for _, v := range vecs {
+				if v != v {
+					t.Error("NaN output for valid input")
+				}
+			}
 		}
 	}
 }

md3/linalg.go

@@ -28,7 +28,7 @@ func (a Mat3) SVD() (U, S, V Mat3) {
 	_, qVr := ATA.jacobiEigenanalysis()
 
 	// Compute B = A * V
-	V = RotatingMat3(qVr)
+	V = qVr.RotationMat3()
 	b := MulMat3(a, V)
 
 	// Sort singular values and adjust V

md3/mat3.go

@@ -69,12 +69,6 @@ func EqualMat3(a, b Mat3, tolerance float64) bool {
 		ms1.EqualWithinAbs(a.x22, b.x22, tolerance)
 }
 
-// MulPosition multiplies a V2 position with a rotate/translate matrix.
-func (a Mat3) mulPosition(x, y float64) (float64, float64) {
-	return a.x00*x + a.x01*y + a.x02,
-		a.x10*x + a.x11*y + a.x12
-}
-
 // MulMat3 multiplies two 3x3 matrices.
 func MulMat3(a, b Mat3) Mat3 {
 	m := Mat3{}
@@ -258,27 +252,6 @@ func (m Mat3) AsMat4() Mat4 {
 	}
 }
 
-// RotatingMat3 returns a 3×3 rotation matrix corresponding to the receiver. It
-// may be used to perform rotations on a 3-vector or to apply the rotation
-// to a 3×n matrix of column vectors. If the receiver is not a unit
-// quaternion, the returned matrix will not be a pure rotation.
-func RotatingMat3(rotationUnit Quat) Mat3 {
-	w, i, j, k := rotationUnit.W, rotationUnit.I, rotationUnit.J, rotationUnit.K
-	ii := 2 * i * i
-	jj := 2 * j * j
-	kk := 2 * k * k
-	wi := 2 * w * i
-	wj := 2 * w * j
-	wk := 2 * w * k
-	ij := 2 * i * j
-	jk := 2 * j * k
-	ki := 2 * k * i
-	return mat3(
-		1-(jj+kk), ij-wk, ki+wj,
-		ij+wk, 1-(ii+kk), jk-wi,
-		ki-wj, jk+wi, 1-(ii+jj))
-}
-
 // Hessian returns the Hessian matrix of the vector field f at point p.
 // step is the step with which the second derivative is calculated.
 func Hessian(p Vec, step float64, f func(Vec) float64) Mat3 {

md3/mat4.go

@@ -64,19 +64,6 @@ func ScalingMat4(v Vec) Mat4 {
 		0, 0, 0, 1}
 }
 
-// RotatingMat4 returns an orthographic 4x4 rotation matrix (right hand rule).
-func RotatingMat4(angleRadians float64, axis Vec) Mat4 {
-	axis = Unit(axis)
-	s, c := math.Sincos(angleRadians)
-	m := 1 - c
-	return Mat4{
-		m*axis.X*axis.X + c, m*axis.X*axis.Y - axis.Z*s, m*axis.Z*axis.X + axis.Y*s, 0,
-		m*axis.X*axis.Y + axis.Z*s, m*axis.Y*axis.Y + c, m*axis.Y*axis.Z - axis.X*s, 0,
-		m*axis.Z*axis.X - axis.Y*s, m*axis.Y*axis.Z + axis.X*s, m*axis.Z*axis.Z + c, 0,
-		0, 0, 0, 1,
-	}
-}
-
 // MulMat4 multiplies two 4x4 matrices and returns the result.
 func MulMat4(a, b Mat4) Mat4 {
 	m := Mat4{}
@@ -222,46 +209,6 @@ func (m Mat4) Array() (rowmajor [16]float64) {
 	return rowmajor
 }
 
-// RotatingBetweenVecsMat4 returns the rotation matrix that transforms "start" onto the same direction as "dest".
-func RotatingBetweenVecsMat4(start, dest Vec) Mat4 {
-	// is either vector == 0?
-	const epsilon = 1e-12
-	if EqualElem(start, Vec{}, epsilon) || EqualElem(dest, Vec{}, epsilon) {
-		return IdentityMat4()
-	}
-	// normalize both vectors
-	start = Unit(start)
-	dest = Unit(dest)
-	// are the vectors the same?
-	if EqualElem(start, dest, epsilon) {
-		return IdentityMat4()
-	}
-
-	// are the vectors opposite (180 degrees apart)?
-	if EqualElem(Scale(-1, start), dest, epsilon) {
-		return Mat4{
-			-1, 0, 0, 0,
-			0, -1, 0, 0,
-			0, 0, -1, 0,
-			0, 0, 0, 1,
-		}
-	}
-	// general case
-	// See:	https://math.stackexchange.com/questions/180418/calculate-rotation-matrix-to-align-vector-a-to-vector-b-in-3d
-	v := Cross(start, dest)
-	vx := Skew(v)
-	k := 1. / (1. + Dot(start, dest))
-
-	vx2 := MulMat3(vx, vx)
-	vx2 = ScaleMat3(vx2, k)
-
-	// Calculate sum of matrices.
-	vx = AddMat3(vx, IdentityMat3())
-	vx = AddMat3(vx, vx2)
-
-	return vx.AsMat4()
-}
-
 // EqualMat4 tests the equality of 4x4 matrices.
 func EqualMat4(a, b Mat4, tolerance float64) bool {
 	return ms1.EqualWithinAbs(a.x00, b.x00, tolerance) &&

md3/md3_test.go

@@ -7,20 +7,71 @@ package md3
 import (
 	"fmt"
 	"math"
+	"math/rand"
 	"testing"
+
+	"github.com/soypat/geometry/internal"
 )
 
 func TestRotation(t *testing.T) {
 	const tol = 1e-7
 	v := Vec{X: 1}
-	y90 := RotatingMat4(math.Pi/2, Vec{Y: 1})
+	y90 := RotationMat4(math.Pi/2, Vec{Y: 1})
 	got := y90.MulPosition(v)
 	want := Vec{Z: -1}
 	if !EqualElem(got, want, tol) {
 		t.Errorf("want %v, got %v", want, got)
 	}
 }
 
+func TestRotationConversions(t *testing.T) {
+	const tol = internal.Smallfloat64 / 10
+	rotations := []Quat{
+		Rotation(1, Vec{X: 1}),
+		Rotation(1, Vec{X: 1, Y: 1}),
+		Rotation(2, Vec{X: 1, Y: 1, Z: 1}),
+		Rotation(math.Pi, Vec{X: 1}),
+		Rotation(math.Pi, Vec{X: 1, Y: 1}),
+		Rotation(math.Pi, Vec{X: 1, Y: 1, Z: 1}),
+		Rotation(2*math.Pi, Vec{X: 1}),
+		Rotation(0, Vec{X: 1}),
+	}
+	rng := newRNG(1)
+	for _, rot := range rotations {
+		angle, axis := rot.Rotation()
+		gotrot := Rotation(angle, axis)
+		if !EqualQuat(rot, gotrot, tol) {
+			t.Errorf("want %v, got %v", rot, gotrot)
+		}
+		if !rot.EqualOrientation(gotrot, tol) {
+			t.Errorf("not equal orientations %v, got %v", rot, gotrot)
+		}
+		m3 := rot.RotationMat3()
+		for i := 0; i < 20; i++ {
+			v := rng.Vec()
+			vgot := MulMatVec(m3, v)
+			vwant := rot.Rotate(v)
+			if !EqualElem(vgot, vwant, tol) {
+				t.Errorf("want %v, got %v", vwant, vgot)
+			}
+		}
+	}
+}
+
+func TestRotationBetweenVecs(t *testing.T) {
+	const tol = internal.Smallfloat64 / 10
+	rng := newRNG(1)
+	for i := 0; i < 80; i++ {
+		start := rng.Vec()
+		dest := rng.Vec()
+		rot := RotationBetweenVecs(start, dest)
+		gotDest := rot.Rotate(start)
+		if !EqualElem(Unit(gotDest), Unit(dest), tol) { // test direction, so use Unit.
+			t.Errorf("want %v, got %v", dest, gotDest)
+		}
+	}
+}
+
 func TestSVD(t *testing.T) {
 	const tol = 1e-6
 	a := mat3(-0.558253, -0.0461681, -0.505735, -0.411397, 0.0365854, 0.199707, 0.285389, -0.313789, 0.200189)
@@ -59,3 +110,27 @@ func printMat(a Mat3) {
 	fmt.Printf("%f %f %f \n", a.x10, a.x11, a.x12)
 	fmt.Printf("%f %f %f \n", a.x20, a.x21, a.x22)
 }
+
+func newRNG(src int) *rngGen {
+	return &rngGen{rng: *rand.New(rand.NewSource(int64(src)))}
+}
+
+type rngGen struct {
+	rng rand.Rand
+}
+
+func (rng *rngGen) Vec() Vec {
+	return Vec{X: rng.Float(), Y: rng.Float(), Z: rng.Float()}
+}
+
+func (rng *rngGen) Float() float64 {
+	return float64(rng.rng.Float64())
+}
+
+func (rng *rngGen) FloatRange(start, end float64) float64 {
+	return float64(rng.rng.Float64())*(end-start) + start
+}
+
+func (rng *rngGen) VecRange(start, end float64) Vec {
+	return Vec{X: rng.FloatRange(start, end), Y: rng.FloatRange(start, end), Z: rng.FloatRange(start, end)}
+}