rewrite EqualOrientation

Author: soypat

ms3/ms3_test.go

@@ -33,15 +33,16 @@ func TestRotationConversions(t *testing.T) {
 		Rotation(2*math.Pi, Vec{X: 1}),
 		Rotation(0, Vec{X: 1}),
 	}
-	tolroot := math.Sqrt(math.Sqrt(tol))
+	maxAngle := 5 * math.Pi / 180
+	angleTol := math.Cos(maxAngle / 2)
 	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, tolroot) {
+		if !rot.EqualOrientation(gotrot, angleTol) {
 			t.Errorf("not equal orientations %v, got %v", rot, gotrot)
 		}
 		m3 := rot.RotationMat3()

ms3/rotation.go

@@ -120,14 +120,14 @@ func RotationBetweenVecs(start, dest Vec) Quat {
 }
 
 // EqualOrientation returns whether the quaternions represents the same orientation with a given tolerence
-func (q1 Quat) EqualOrientation(q2 Quat, tolsq float32) bool {
-	// n1sq, n2sq := q1.Dot(q1), q2.Dot(q2)
-	n1sq, n2sq := q1.Norm(), q2.Norm()
+func (q1 Quat) EqualOrientation(q2 Quat, tol float32) bool {
+	n1sq := q1.Dot(q1)
+	n2sq := q2.Dot(q2)
 	if n1sq == 0 || n2sq == 0 {
 		return false // Degenerate quaternion.
 	}
-	dot := math.Abs(q1.Dot(q2) / (n1sq * n2sq))
-	return dot > 1-tolsq
+	d := q1.Dot(q2)
+	return d*d >= tol*tol*(n1sq*n2sq)
 }
 
 /*