Skip to content
Permalink
Browse files

Fix Transform.inverse and fix plane-cursor intersection in AR

The intersection point was off because the Transform.inverse wasn't correct
  • Loading branch information...
endavid committed Mar 23, 2019
1 parent e6a21fb commit 5a68ac09401be04335eb17e02d4d760eee470bc6
@@ -82,6 +82,7 @@
639D070A203B7EF600F9905D /* VolumetricCompute.swift in Sources */ = {isa = PBXBuildFile; fileRef = 639D0709203B7EF600F9905D /* VolumetricCompute.swift */; };
63AA9E6E223EFCCE004B9989 /* squareFrame.png in Resources */ = {isa = PBXBuildFile; fileRef = 63AA9E6D223EFCCE004B9989 /* squareFrame.png */; };
63AA9E702241B0D8004B9989 /* Samplers.swift in Sources */ = {isa = PBXBuildFile; fileRef = 63AA9E6F2241B0D8004B9989 /* Samplers.swift */; };
63AA9E7422466A5B004B9989 /* SVD.swift in Sources */ = {isa = PBXBuildFile; fileRef = 63AA9E7322466A5A004B9989 /* SVD.swift */; };
63BA72112128C05A0023E879 /* MotionController.swift in Sources */ = {isa = PBXBuildFile; fileRef = 63BA72102128C05A0023E879 /* MotionController.swift */; };
63C7A1732231D2DA00DB1439 /* VidController+ARSessionDelegate.swift in Sources */ = {isa = PBXBuildFile; fileRef = 63C7A1722231D2DA00DB1439 /* VidController+ARSessionDelegate.swift */; };
63C7A1752233CCCF00DB1439 /* Triangle.swift in Sources */ = {isa = PBXBuildFile; fileRef = 63C7A1742233CCCF00DB1439 /* Triangle.swift */; };
@@ -171,6 +172,7 @@
639D0709203B7EF600F9905D /* VolumetricCompute.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = VolumetricCompute.swift; sourceTree = "<group>"; };
63AA9E6D223EFCCE004B9989 /* squareFrame.png */ = {isa = PBXFileReference; lastKnownFileType = image.png; path = squareFrame.png; sourceTree = "<group>"; };
63AA9E6F2241B0D8004B9989 /* Samplers.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = Samplers.swift; sourceTree = "<group>"; };
63AA9E7322466A5A004B9989 /* SVD.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = SVD.swift; sourceTree = "<group>"; };
63BA72102128C05A0023E879 /* MotionController.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = MotionController.swift; sourceTree = "<group>"; };
63C7A1722231D2DA00DB1439 /* VidController+ARSessionDelegate.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = "VidController+ARSessionDelegate.swift"; sourceTree = "<group>"; };
63C7A1742233CCCF00DB1439 /* Triangle.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = Triangle.swift; sourceTree = "<group>"; };
@@ -285,6 +287,7 @@
6311EE5A222BF2A900B5A87F /* Line.swift */,
63C7A1742233CCCF00DB1439 /* Triangle.swift */,
63C7A1762233CD9C00DB1439 /* Ray.swift */,
63AA9E7322466A5A004B9989 /* SVD.swift */,
);
path = math;
sourceTree = "<group>";
@@ -480,6 +483,7 @@
buildActionMask = 2147483647;
files = (
632D2812203896C7000562F0 /* Primitive2D.swift in Sources */,
63AA9E7422466A5B004B9989 /* SVD.swift in Sources */,
632D2831203896C7000562F0 /* Texture.swift in Sources */,
6393E7B6205B152A0098F96F /* CiexyY.swift in Sources */,
63C7A1752233CCCF00DB1439 /* Triangle.swift in Sources */,
@@ -13,6 +13,10 @@ public enum SerializationError: Error {
case invalid(String, Any)
}

public enum MathError: Error {
case unsupported(String)
}

public enum FileError: Error {
case missing(String)
case corrupt(String)
@@ -49,7 +49,9 @@ public class Camera {
/// inverse of the transform, to convert to view space
public var viewTransform: Transform {
get {
return transform.inverse()
// it shouldn't throw, as there's no anisotropic scaling
let inv = try? transform.inverse()
return inv!
}
}
public var viewMatrix: float4x4 {
@@ -52,4 +52,10 @@ public extension float4x4 {
bottom: -size / aspectRatio, top: size / aspectRatio,
near: near, far: far)
}
public var upper3x3: float3x3 {
get {
let (c0, c1, c2, _) = columns
return float3x3(c0.xyz, c1.xyz, c2.xyz)
}
}
}
@@ -65,6 +65,11 @@ public struct Quaternion : CustomStringConvertible {
m[3,3] = w2 + x2 + y2 + z2 // = 1 if unit quaternion
return m
}
public func toMatrix3() -> float3x3 {
let m = self.toMatrix4()
let (c0, c1, c2, _) = m.columns
return float3x3(c0.xyz, c1.xyz, c2.xyz)
}

public static func createRotation(start: float3, end: float3) -> Quaternion {
let up = float3(start.x, start.z, start.y)
@@ -81,7 +86,7 @@ public struct Quaternion : CustomStringConvertible {
let axis = normalize(cross(start, end))
return Quaternion(AngleAxis(angle: angle, axis: axis))
}
public static func fromMatrix(_ m: float3x3) -> Quaternion {
public init(_ m: float3x3) {
let (cx, cy, cz) = m.columns
let angle = acos((cx.x + cy.y + cz.z - 1.0) / 2.0)
var axis = float3(0, 0, 1)
@@ -96,7 +101,7 @@ public struct Quaternion : CustomStringConvertible {
axis = normalize(d)
}
}
return Quaternion(AngleAxis(angle: angle, axis: axis))
self.init(AngleAxis(angle: angle, axis: axis))
}
}

@@ -69,3 +69,10 @@ public func * (t: Transform, ray: Ray) -> Ray {
let direction = t.rotate(direction: ray.direction)
return Ray(start: start, direction: direction)
}

public func * (m: float4x4, ray: Ray) -> Ray {
let s = m * float4(ray.start.x, ray.start.y, ray.start.z, 1)
let d = m.upper3x3 * ray.direction
let direction = normalize(d)
return Ray(start: s.xyz, direction: direction)
}
@@ -0,0 +1,60 @@
//
// SVD.swift
// VidFramework
//
// Created by David Gavilan on 2019/03/23.
// Copyright © 2019 David Gavilan. All rights reserved.
//
import Accelerate
import simd

// https://gist.github.com/wannyk/04c1f48161780322c0bb
/// array is a column-wise matrix m * n
func svd(array x:[Double], m:Int, n:Int) -> (u:[Double], s:[Double], v:[Double]) {
var JOBZ = Int8(UnicodeScalar("A").value)
var M = __CLPK_integer(m)
var N = __CLPK_integer(n)
var A = x
var LDA = __CLPK_integer(m)
var S = [__CLPK_doublereal](repeating: 0.0, count: min(m,n))
var U = [__CLPK_doublereal](repeating: 0.0, count: m*m)
var LDU = __CLPK_integer(m)
var VT = [__CLPK_doublereal](repeating: 0.0, count: n*n)
var LDVT = __CLPK_integer(n)
let lwork = min(m,n)*(6+4*min(m,n))+max(m,n)
var WORK = [__CLPK_doublereal](repeating: 0.0, count: lwork)
var LWORK = __CLPK_integer(lwork)
var IWORK = [__CLPK_integer](repeating: 0, count: 8*min(m,n))
var INFO = __CLPK_integer(0)
dgesdd_(&JOBZ, &M, &N, &A, &LDA, &S, &U, &LDU, &VT, &LDVT, &WORK, &LWORK, &IWORK, &INFO)
var s = [Double](repeating: 0.0, count: m*n)
for ni in 0...n-1 {
s[ni*m+ni] = S[ni]
}
var v = [Double](repeating: 0.0, count: n*n)
vDSP_mtransD(VT, 1, &v, 1, vDSP_Length(n), vDSP_Length(n))
return (U, s, v)
}

public func svd(matrix m: float3x3) -> (float3x3, float3, float3x3) {
let (c0, c1, c2) = m.columns
let d0 = c0.map { Double($0) }
let d1 = c1.map { Double($0) }
let d2 = c2.map { Double($0) }
let flatArray = d0 + d1 + d2
let (u, s, v) = svd(array: flatArray, m: 3, n: 3)
let uf = u.map { Float($0) }
let sf = s.map { Float($0) }
let vf = v.map { Float($0) }
let U = float3x3(
float3(uf[0], uf[1], uf[2]),
float3(uf[3], uf[4], uf[5]),
float3(uf[6], uf[7], uf[8]))
let S = float3(sf[0], sf[4], sf[8])
let V = float3x3(
float3(vf[0], vf[1], vf[2]),
float3(vf[3], vf[4], vf[5]),
float3(vf[6], vf[7], vf[8]))
return (U, S, V)
}
@@ -21,13 +21,23 @@ public struct Transform {
let tt = float4(position.x, position.y, position.z, 1.0)
return float4x4([xx, yy, zz, tt])
}
public func inverse() -> Transform {
public func inverse() throws -> Transform {
// M = T * R * S -> M^ = S^ * R^ * T^
// if the scale is not uniform, S^ * R^ will create
// a shear matrix, that can be decomposed using svd,
// S^ * R^ = U * S * V^, but not into rot * scale
if !scale.isClose(scale.x * float3(1,1,1)) {
throw MathError.unsupported("Transforms with anisotropic scaling can't be inverted")
}
let r = rotation.inverse()
let s = scale.inverse()
return Transform(
position: r * (s * -self.position),
scale: s,
rotation: r)
let scaleMatrix = float3x3(diagonal: s)
// verified that SR == (rotation.toMatrix3() * float3x3(diagonal: scale)).inverse
let SR = scaleMatrix * r.toMatrix3()
let ti = -position
// verified that t == SR * ti
let t = s * (r * ti)
return Transform(rotationAndScale: SR, translation: t)
}
public init(position: float3) {
self.position = position
@@ -45,14 +55,28 @@ public struct Transform {
self.scale = scale
self.rotation = rotation
}
public init(matrix: float4x4) {
public init(rotationAndTranslation matrix: float4x4) {
let (c0, c1, c2, c3) = matrix.columns

position = float3(c3.x, c3.y, c3.z)
// assume scale = 1; otherwise we'd need to compute SVD
scale = float3(1, 1, 1)
rotation = Quaternion.fromMatrix(float3x3(c0.xyz, c1.xyz, c2.xyz))
rotation = Quaternion(float3x3(c0.xyz, c1.xyz, c2.xyz))
}
public init(rotationAndScale matrix: float3x3, translation: float3) {
position = translation
// https://math.stackexchange.com/a/1463487
// only works if there's no shear
let (a, b, c) = matrix.columns
scale = float3(length(a), length(b), length(c))
//let (u, s, v) = svd(matrix: matrix)
rotation = Quaternion(float3x3(a/scale.x, b/scale.y, c/scale.z))
}
public init(transform matrix: float4x4) {
let (c0, c1, c2, c3) = matrix.columns
let t = float3(c3.x, c3.y, c3.z)
let RS = float3x3(c0.xyz, c1.xyz, c2.xyz)
self.init(rotationAndScale: RS, translation: t)
}

public init() {
}
public func rotate(direction: float3) -> float3 {
@@ -202,9 +202,12 @@ public class Primitive {
var point = float3(0,0,0)
var triangle: Triangle?
for i in instances {
// convert the ray to model space, less operations
// than converting all the triangles to world space
let modelRay = i.transform.inverse() * ray
// Convert the ray to model space, less operations
// than converting all the triangles to world space.
// And because there may be anisotropic scaling,
// use matrices intead of Transforms.
let toModel = i.transform.toMatrix4().inverse
let modelRay = toModel * ray
for t in triangles {
if let d = modelRay.intersects(triangle: t), d < dist {
dist = d
@@ -24,7 +24,7 @@ extension VidController: ARSessionDelegate {
}
if let plane = anchor as? ARPlaneAnchor {
if let (primitive, instanceIndex) = scene.findPrimitiveInstance(by: plane.identifier), let p = primitive as? PlanePrimitive {
var t = Transform(matrix: plane.transform)
var t = Transform(rotationAndTranslation: plane.transform)
t.scale = float3(plane.extent.x, 1, plane.extent.z)
p.instances[instanceIndex].transform = t
p.instances[instanceIndex].material.uvScale = Vec2(plane.extent.x / p.gridSizeMeters, plane.extent.z / p.gridSizeMeters)
@@ -36,7 +36,7 @@ extension VidController: ARSessionDelegate {
let arPlanesName = Scene.arPlanesPrimitiveName
for anchor in anchors {
if let plane = anchor as? ARPlaneAnchor {
var t = Transform(matrix: plane.transform)
var t = Transform(rotationAndTranslation: plane.transform)
t.scale = float3(plane.extent.x, 1, plane.extent.z)
if let primitive = scene.findPrimitive(by: arPlanesName), let planePrim = primitive as? PlanePrimitive {
scene.dequeue(primitive)
@@ -191,10 +191,10 @@ open class VidController: UIViewController, MTKViewDelegate {
//camera.transform.position = pos.xyz
let orientation = camera.orientation
if orientation == .landscapeRight {
camera.transform = Transform(matrix: frame.camera.transform)
camera.transform = Transform(rotationAndTranslation: frame.camera.transform)
} else {
let viewMatrix = frame.camera.viewMatrix(for: camera.orientation)
camera.transform = Transform(matrix: viewMatrix.inverse)
camera.transform = Transform(rotationAndTranslation: viewMatrix.inverse)
}
camera.projection = frame.camera.projectionMatrix(for: camera.orientation, viewportSize: view.bounds.size, zNear: CGFloat(camera.near), zFar: CGFloat(camera.far))
}
@@ -20,6 +20,7 @@
635EAEE2221398DF00D18734 /* TestHelpers.swift in Sources */ = {isa = PBXBuildFile; fileRef = 635EAEE1221398DF00D18734 /* TestHelpers.swift */; };
63711C95203F961B00D046CF /* ColorTests.swift in Sources */ = {isa = PBXBuildFile; fileRef = 63711C94203F961B00D046CF /* ColorTests.swift */; };
6389DFB8221AAC6A001625B4 /* SphericalHarmonicsTests.swift in Sources */ = {isa = PBXBuildFile; fileRef = 6389DFB7221AAC6A001625B4 /* SphericalHarmonicsTests.swift */; };
63AA9E7622466ED9004B9989 /* MathTests.swift in Sources */ = {isa = PBXBuildFile; fileRef = 63AA9E7522466ED9004B9989 /* MathTests.swift */; };
63C7A17B22355CEB00DB1439 /* RayTests.swift in Sources */ = {isa = PBXBuildFile; fileRef = 63C7A17A22355CEB00DB1439 /* RayTests.swift */; };
/* End PBXBuildFile section */

@@ -72,6 +73,7 @@
635EAEE1221398DF00D18734 /* TestHelpers.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = TestHelpers.swift; sourceTree = "<group>"; };
63711C94203F961B00D046CF /* ColorTests.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = ColorTests.swift; sourceTree = "<group>"; };
6389DFB7221AAC6A001625B4 /* SphericalHarmonicsTests.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = SphericalHarmonicsTests.swift; sourceTree = "<group>"; };
63AA9E7522466ED9004B9989 /* MathTests.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = MathTests.swift; sourceTree = "<group>"; };
63C7A17A22355CEB00DB1439 /* RayTests.swift */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.swift; path = RayTests.swift; sourceTree = "<group>"; };
/* End PBXFileReference section */

@@ -140,6 +142,7 @@
children = (
63711C94203F961B00D046CF /* ColorTests.swift */,
632D28DA203B6661000562F0 /* Info.plist */,
63AA9E7522466ED9004B9989 /* MathTests.swift */,
63C7A17A22355CEB00DB1439 /* RayTests.swift */,
6389DFB7221AAC6A001625B4 /* SphericalHarmonicsTests.swift */,
635EAEE1221398DF00D18734 /* TestHelpers.swift */,
@@ -317,6 +320,7 @@
6389DFB8221AAC6A001625B4 /* SphericalHarmonicsTests.swift in Sources */,
63C7A17B22355CEB00DB1439 /* RayTests.swift in Sources */,
632D28D9203B6661000562F0 /* VidTestsTests.swift in Sources */,
63AA9E7622466ED9004B9989 /* MathTests.swift in Sources */,
63711C95203F961B00D046CF /* ColorTests.swift in Sources */,
635EAEE2221398DF00D18734 /* TestHelpers.swift in Sources */,
);
@@ -0,0 +1,41 @@
//
// MathTests.swift
// VidTestsTests
//
// Created by David Gavilan on 2019/03/23.
// Copyright © 2019 David Gavilan. All rights reserved.
//
import XCTest
import simd
import VidFramework
@testable import VidTests

class MatTests: XCTestCase {
func testSVDMatrix3() {
let m = float3x3(
float3(1, 3, 9),
float3(-3, 8, 7),
float3(0.5, -0.1, -3))
let (u, s, v) = svd(matrix: m)
let reconstructed = u * float3x3(diagonal: s) * v.inverse
assertAlmostEqual(m, reconstructed)
// matrix values obtained with Octave for comparison
// m = [1 3 9; -3 8 7; 0.5 -0.1 -3]'
// [u, s, v] = svd(m)
assertAlmostEqual(float3(14.086166, 4.759304, 1.0903881), s)
let octaveU = float3x3(rows: [
float3(0.12164, 0.53498, -0.83606),
float3(-0.56327, -0.65635, -0.50194),
float3(-0.81727, 0.53198, 0.22150)
])
let octaveV = float3x3(rows: [
float3(-0.633501, 0.704683, -0.319529),
float3(-0.751941, -0.658041, 0.039576),
float3(0.182375, -0.265338, -0.946750)
])
assertAlmostEqual(octaveU, u, epsilon: 1e-5)
assertAlmostEqual(octaveV, v, epsilon: 1e-5)
}
}

Oops, something went wrong.

0 comments on commit 5a68ac0

Please sign in to comment.
You can’t perform that action at this time.