github.com/pavlo67/common@v0.5.3/common/mathlib/plane/transformation.go (about)

     1  package plane
     2  
     3  import (
     4  	"fmt"
     5  	"math"
     6  )
     7  
     8  type Transformation struct {
     9  	From Point2
    10  	To   Point2
    11  }
    12  
    13  const onCalculateRotationAndScale = "on CalculateRotationAndScale()"
    14  
    15  func CalculateRotationAndScale(pts []Transformation, rotDeviationMax, scaleDeviationMax float64) (XToYAngle, float64, error) {
    16  	if len(pts) != 3 {
    17  		return 0, 0, fmt.Errorf("wrong points (%d), must be 3 items exactly / "+onCalculateRotationAndScale, len(pts))
    18  	}
    19  
    20  	cOut, cIn := make(Contour, len(pts)), make(Contour, len(pts))
    21  
    22  	for i, p := range pts {
    23  		if i > 0 && pts[i-1].From.X == p.From.X && pts[i-1].From.Y == p.From.Y {
    24  			return 0, 0, fmt.Errorf("pts[%d] == pts[%d] / "+onCalculateRotationAndScale, i-1, i)
    25  		}
    26  		cOut[i], cIn[i] = p.To, p.From
    27  	}
    28  
    29  	rotInn, distInn := cOut.Rotations(), cOut.Distances()
    30  	rotOut, distOut := cIn.Rotations(), cIn.Distances()
    31  
    32  	//fmt.Printf("rotInn: %v\n", rotInn)
    33  	//fmt.Printf("rotOut: %v\n", rotOut)
    34  
    35  	var rotDeltaSum XToYAngle
    36  	var scalesSum float64
    37  
    38  	rotDeltas := make([]XToYAngle, len(pts))
    39  	scales := make([]float64, len(pts))
    40  
    41  	for i := 0; i < len(cOut); i++ {
    42  		rotDeltas[i] = (rotInn[i] - rotOut[i]).Canon()
    43  		rotDeltaSum += rotDeltas[i]
    44  		scales[i] = distInn[i] / distOut[i]
    45  		scalesSum += scales[i]
    46  	}
    47  
    48  	rotDeltaAvg := (rotDeltaSum / XToYAngle(len(pts))).Canon()
    49  	scaleAvg := scalesSum / float64(len(pts))
    50  
    51  	if scaleAvg <= 0 {
    52  		return 0, 0, fmt.Errorf("wrong scaleAvg (%f) for scales: %v / "+onCalculateRotationAndScale, scaleAvg, scales)
    53  	}
    54  
    55  	rotDeltaDev := math.Abs(float64((rotInn[0] - rotOut[0] - rotDeltaAvg).Canon()))
    56  	scaleDev := math.Abs(1 - scales[0]/scaleAvg)
    57  	for i := 1; i < len(pts); i++ {
    58  		rotDeltaDevI := math.Abs(float64((rotInn[i] - rotOut[i] - rotDeltaAvg).Canon()))
    59  		if rotDeltaDevI > rotDeltaDev {
    60  			rotDeltaDev = rotDeltaDevI
    61  		}
    62  		scaleDevI := math.Abs(1 - scales[i]/scaleAvg)
    63  		if scaleDevI > scaleDev {
    64  			scaleDev = scaleDevI
    65  		}
    66  	}
    67  
    68  	if rotDeltaDev > rotDeviationMax {
    69  		return 0, 0, fmt.Errorf("rotDeltaDev (%f) > rotDeviationMax (%f) for rotation deltas: %v, rotDeltaAvg: %f / "+onCalculateRotationAndScale,
    70  			rotDeltaDev, rotDeviationMax, rotDeltas, rotDeltaAvg)
    71  	}
    72  
    73  	if scaleDev > scaleDeviationMax {
    74  		return 0, 0, fmt.Errorf("scaleDev (%f) > scaleDeviationMax (%f) for scales: %v / "+onCalculateRotationAndScale,
    75  			scaleDev, scaleDeviationMax, scales)
    76  	}
    77  
    78  	return rotDeltaAvg, scaleAvg, nil
    79  }