# Finding an Affine Transform using three 2D point correspondences using Simplex Affine Mapping (SAM) in Swift

`import CoreGraphicsimport Foundation       //for NumberFormatter, used in debug functions// MARK: - Simple Affine Map function for 2D and supporting types/// Calculate the affine transform from one set of three 2D points (the "from" triangle) to another set of three 2D points (the "to" triangle)./// Throws an error if one of the sets of points is colinear./// The transform is calculated using the 2D expression of the Simplex Affine Map [SAM] by V. B. Tymchyshyn and A. V. Khlevniuk./// https://www.researchgate.net/publication/332410209_Beginner%27s_guide_to_mapping_simplexes_affinely////// The workbook from the authors provides numerous examples showing step-by-step calculation for simplexes in N dimensions./// https://www.researchgate.net/publication/332971934_Workbook_on_mapping_simplexes_affinely\////// A triangle is a simplex in 2D space, and hence works using the SAM scheme./// https://en.wikipedia.org/wiki/Simplex////// The matrix elements include a mix of vectors and scalars. For this code, use letter names corresponding to the vectors and scalars in the SAM paper./// a, b, c are 2D points in the "from" triangle/// d, e, f are 2D points in the "to" triangle//////          |0  d   e   f   |///          |p  ax  bx  cx  |       // p called "x1" in the SAM paper///      det |q  ay  by  cy  |       // q called "x2" in the SAM paper///          |1  1   1   1   |/// (-1) ----------------------///             |ax  bx  cx  |      // ax = point A, x component///         det    |ay  by  cy  |///             |1   1   1   |////// For image processing applications this technique may not be appropriate if point finding is not sufficiently repeatable./// This mapping will be sensitive to noise / inaccuracy in finding any of the "from" or "to" points: any triangle can map to any other triangle!/// For point registration, typically more than 3 point correspondences are used. The transform may be found/// using a least squares fitting, and often by eliminating outlier points. Related topics include Singular Value Decomposition (SVD) and/// Random Sampling and Consensus (RANSAC).func sam(from: Triangle, to: Triangle) throws -> CGAffineTransform {    if from.colinear() {        throw SamError.colinearPoints(description: "Points in 'from' triangle are colinear. Can not calculate SAM transform.")    }        if to.colinear() {        throw SamError.colinearPoints(description: "Points in 'to' triangle are colinear. Can not calculate SAM transform.")    }        let a = from.vector1    let b = from.vector2    let c = from.vector3        let d = to.vector1    let e = to.vector2    let f = to.vector3    // Determinant of 4x4 in numerator of SAM formula    // Reduce to p, q, and translation. (p, q) here are (x1, x2) in the SAM paper.    // We can find the determinant starting with any row or any column, so we make it easy to isolate p ("x1") and q ("x2")        // +0 * (...)        // -p ( [d] (by - cy) - [e] (ay - cy) + [f] (ay - by) )    let p = CGFloat(-1.0) * (d * (b.dy - c.dy) - e * (a.dy - c.dy) + f * (a.dy - b.dy))        // +q ( [d] (bx - cx) - [e] (ax - cx) + [f] (ax - bx) )    let q = d * (b.dx - c.dx) - e * (a.dx - c.dx) + f * (a.dx - b.dx)        // -1  ( [d] (bx * cy - by * cx) - [e] (ax * cy - ay * cx) + [f] * (ax * by - ay * bx) )    let translation =  CGFloat(-1.0) * (d * (b.dx * c.dy - b.dy * c.dx) - e * (a.dx * c.dy - a.dy * c.dx) + f * (a.dx * b.dy - a.dy * b.dx))        // Determinant of 3x3 used in denominator of SAM formula    let denominator = determinant(from)        let pd = CGFloat(-1.0) * p / denominator    let qd = CGFloat(-1.0) * q / denominator    let td = CGFloat(-1.0) * translation / denominator        // plug values into CoreGraphics affine transform    //  |a  b   0|   =  |pd.x   pd.y    0|    //  |c  d   0|      |qd.x   qd.y    0|    //  |tx ty  1|      |td.x   td.y    1|        return CGAffineTransform(a: pd.dx, b: pd.dy, c: qd.dx, d: qd.dy, tx: td.dx, ty: td.dy)}enum SamError: Error {    case colinearPoints(description: String)}/// Three points nominally defining a triangle, but possibly colinear.struct Triangle {    var point1: CGPoint    var point2: CGPoint    var point3: CGPoint        var x1: CGFloat { point1.x }    var y1: CGFloat { point1.y }    var x2: CGFloat { point2.x }    var y2: CGFloat { point2.y }    var x3: CGFloat { point3.x }    var y3: CGFloat { point3.y }        /// Point1 as a 2D vector    var vector1: CGVector { CGVector(point1) }        /// Point2 as a 2D vector    var vector2: CGVector { CGVector(point2) }        /// Point3 as a 2D vector    var vector3: CGVector { CGVector(point3) }        /// Return a Triangle after applying an affine transform to self.    func applying(_ t: CGAffineTransform) -> Triangle {        Triangle(            point1: self.point1.applying(t),            point2: self.point2.applying(t),            point3: self.point3.applying(t)        )    }        init(point1: CGPoint, point2: CGPoint, point3: CGPoint) {        self.point1 = point1        self.point2 = point2        self.point3 = point3    }        init(x1: CGFloat, y1: CGFloat, x2: CGFloat, y2: CGFloat, x3: CGFloat, y3: CGFloat) {        point1 = CGPoint(x: x1, y: y1)        point2 = CGPoint(x: x2, y: y2)        point3 = CGPoint(x: x3, y: y3)    }        /// Returns a (Bool, CGFloat) tuple indicating whether the points in the Triangle are colinear, and the angle between vectors tested.    func colinear(degreesTolerance: CGFloat = 0.5) -> Bool {        let v1 = vector2 - vector1        let v2 = vector3 - vector2        let radians = CGVector.angleBetweenVectors(v1: v1, v2: v2)                if radians.isNaN {            return true        }                var degrees = radiansToDegrees(radians)                if degrees > 90 {           degrees = 180 - degrees        }        // code to get around parsing error        return 0 > degrees - degreesTolerance    }}func degreesToRadians(_ degrees: CGFloat) -> CGFloat {    degrees * CGFloat.pi / 180.0}func radiansToDegrees(_ radians: CGFloat) -> CGFloat {    180.0 * radians / CGFloat.pi}extension CGVector {    init(_ point: CGPoint) {        self.init(dx: point.x, dy: point.y)    }        static func + (lhs: CGVector, rhs: CGVector) -> CGVector {        CGVector(dx: lhs.dx + rhs.dx, dy: lhs.dy + rhs.dy)    }    static func - (lhs: CGVector, rhs: CGVector) -> CGVector {        CGVector(dx: lhs.dx - rhs.dx, dy: lhs.dy - rhs.dy)    }    static func * (_ vector: CGVector, _ scalar: CGFloat) -> CGVector {        CGVector(dx: vector.dx * scalar, dy: vector.dy * scalar)    }        static func * (_ scalar: CGFloat, _ vector: CGVector) -> CGVector {        CGVector(dx: vector.dx * scalar, dy: vector.dy * scalar)    }    static func * (lhs: CGVector, rhs: CGVector) -> CGFloat {        lhs.dx * rhs.dx + lhs.dy * rhs.dy    }        static func dotProduct(v1: CGVector, v2: CGVector) -> CGFloat {        v1.dx * v2.dx + v1.dy * v2.dy    }        static func / (_ vector: CGVector, _ scalar: CGFloat) -> CGVector {        CGVector(dx: vector.dx / scalar, dy: vector.dy / scalar)    }    static func / (_ scalar: CGFloat, _ vector: CGVector) -> CGVector {        CGVector(dx: vector.dx / scalar, dy: vector.dy / scalar)    }        // Returns again between vectors in range 0 to 2 * pi [positive]    // a * b = ||a|| ||b|| cos(theta)    // theta = arc cos (a * b / ||a|| ||b||)    static func angleBetweenVectors(v1: CGVector, v2: CGVector) -> CGFloat {        acos( v1 * v2 / (v1.length() * v2.length()) )    }        func length() -> CGFloat {        sqrt(self.dx * self.dx + self.dy * self.dy)    }}/// Determinant of three points used in the denominator of the SAM formula./// x1   x2  x3/// y1   y2  y3/// 1    1   1func determinant(_ t: Triangle) -> CGFloat {    t.x1 * (t.y2 - t.y3) - t.x2 * (t.y1 - t.y3) + t.x3 * (t.y1 - t.y2)}//MARK: - Debugextension NumberFormatter {    func string(_ value: CGFloat, digits: Int, failText: String = "[?]") -> String {        minimumFractionDigits = max(0, digits)        maximumFractionDigits = minimumFractionDigits                guard let s = string(from: NSNumber(value: Double(value))) else {            return failText        }                return s    }        func string(_ point: CGPoint, digits: Int = 1, failText: String = "[?]") -> String {        let sx = string(point.x, digits: digits, failText: failText)        let sy = string(point.y, digits: digits, failText: failText)        return "(\(sx), \(sy))"    }        func string(_ vector: CGVector, digits: Int = 1, failText: String = "[?]") -> String {        let sdx = string(vector.dx, digits: digits, failText: failText)        let sdy = string(vector.dy, digits: digits, failText: failText)        return "(\(sdx), \(sdy))"    }        func string(_ transform: CGAffineTransform, rotationDigits: Int = 2, translationDigits: Int = 1, failText: String = "[?]") -> String {        let sa = string(transform.a, digits: rotationDigits)        let sb = string(transform.b, digits: rotationDigits)        let sc = string(transform.c, digits: rotationDigits)        let sd = string(transform.d, digits: rotationDigits)        let stx = string(transform.tx, digits: translationDigits)        let sty = string(transform.ty, digits: translationDigits)        var s = "a:  \(sa)   b: \(sb)   0"        s += "\nc:  \(sc)   d: \(sd)   0"        s += "\ntx: \(stx)   ty: \(sty)   1"        return s    }}let formatter = NumberFormatter()/// Checks whether the three points in the Triangle are colinear./// Returns a test message spanning multiple lines.func testColinearity(_ t: Triangle) -> String {    let co = t.colinear()    let st1 = formatter.string(t.point1, digits: 2)    let st2 = formatter.string(t.point2, digits: 2)    let st3 = formatter.string(t.point3, digits: 2)        var s = "\(st1), \(st2), \(st3): points in triangle"    s += "\n\(co): colinear"    return s}/// Checks how close two points are. Used to test whether a transformed point (actual) is equivalent to the expected point./// Returns a test message spanning multiple lines.func testCoincidence(actual: CGPoint, expected: CGPoint, pointTolerance: CGFloat = 0.1) -> String {    let vector = CGVector(actual) - CGVector(expected)    let distance = vector.length()    let sd = formatter.string(distance, digits: 2)    let sa = formatter.string(actual, digits: 2)    let se = formatter.string(expected, digits: 2)        return "\(sd) offset  from actual \(sa) to expected \(se)"}/// Calculates the affine transform and checks its validity./// Returns a test message spanning multiple lines.func testSAM(from t1: Triangle, to t2: Triangle, pointTolerance: CGFloat = 0.1) -> String {    var s = "from \(t1) to \(t2)"    do {        let transform = try sam(from: t1, to: t2)        s += "\n" + formatter.string(transform)        // apply transform to points in t1        let a = t1.applying(transform)        // get difference between transformed points (actual) and t2 points (expected)        s += "\n" + testCoincidence(actual: a.point1, expected: t2.point1, pointTolerance: pointTolerance)        s += "\n" + testCoincidence(actual: a.point2, expected: t2.point2, pointTolerance: pointTolerance)        s += "\n" + testCoincidence(actual: a.point3, expected: t2.point3, pointTolerance: pointTolerance)        return s    }    catch SamError.colinearPoints(let description) {        return description    }    catch {        return error.localizedDescription    }}//MARK: - Tests for Playgroundprint()print("** Colinearity tests **")let c1 = Triangle(x1: 1, y1: 1, x2: 2, y2: 2, x3: 3, y3: 3)let sc1 = testColinearity(c1)print(sc1)print()let c2 = Triangle(x1: 0, y1: 0, x2: 5, y2: 5, x3: 0.01, y3: 0.02)let sc2 = testColinearity(c2)print(sc2)print()let c3 = Triangle(x1: 1, y1: 1, x2: 2, y2: 3, x3: 3, y3: 2)let sc3 = testColinearity(c3)print(sc3)print()print("** Arbitrary translation example from SAM workbook **")let a1 = Triangle(x1: 1, y1: 1, x2: 2, y2: 3, x3: 3, y3: 2)let a2 = Triangle(x1: 3, y1: 2, x2: 1, y2: 5, x3: -2, y3: 1)print(testColinearity(a1))print()print(testColinearity(a2))print()print(testSAM(from: a1, to: a2))print()print("** Pure Translation **")let t1 = Triangle(x1: 0, y1: 0, x2: -2, y2: 3, x3: -5, y3: 3)let t2 = Triangle(x1: 3, y1: -2, x2: 1, y2: 1, x3: -2, y3: 1)print(testSAM(from: t1, to: t2))print()print("** Rotation and Translation **")let r1 = Triangle(x1: 0, y1: 0, x2: 1, y2: 0, x3: 0, y3: 1)let r2 = Triangle(x1: 6, y1: 5, x2: 5, y2: 5, x3: 6, y3: 4)print(testSAM(from: r1, to: r2))print()print("** Pure Scaling **")let s1 = Triangle(x1: 0, y1: 0, x2: 1, y2: 0, x3: 0, y3: 1)let s2 = Triangle(x1: 0, y1: 0, x2: 5, y2: 0, x3: 0, y3: 5)print(testSAM(from: s1, to: s2))print()print("** Test in which a triangle has colinear points")let k1 = Triangle(x1: 1, y1: 1, x2: -2, y2: -2, x3: 5, y3: 5)let k2 = Triangle(x1: -5, y1: 3, x2: 2, y2: 8, x3: -3, y3: 1)print()print(testColinearity(k1))print()print(testColinearity(k2))print()print(testSAM(from: k1, to: k2))`

--

--

Founder of Echobatix, developing assistive technology for the blind. echobatix@gmail.com

Love podcasts or audiobooks? Learn on the go with our new app.

## Gary Bartos

Founder of Echobatix, developing assistive technology for the blind. echobatix@gmail.com