Swift Rope Verlets - swift

Question

I'm trying to implement a verlet rope in swift. I'm running into the problem where when trying to fix the position of the point masses from constraints, they very rapidly grow apart and then the coordinates become NaNs. Any idea what could be wrong in my code?
import Foundation
private let DEF_POINT_COUNT = 10
private let CONST_ITERATIONS = 5
#objc class PointMass: DebugPrintable {
var point: NSPoint
private var oldPoint: NSPoint
var x: Double {
get { return Double(point.x) }
}
var y: Double {
get { return Double(point.y) }
}
var debugDescription: String {
get { return "\(point)" }
}
init(_ point: NSPoint) {
self.point = point
self.oldPoint = point
}
func updatePosition() {
let dx = point.x - oldPoint.x
let dy = (point.y - oldPoint.y)
oldPoint = point
point = NSPoint(x: point.x + dx, y: point.y + dy)
}
func updatePosition(point: NSPoint) {
let dx = point.x - self.point.x
let dy = point.y - self.point.y
self.oldPoint = NSPoint(x: oldPoint.x + dx, y: oldPoint.y + dy)
self.point = point
}
}
struct Constraint {
var p1: PointMass
var p2: PointMass
var len: Double
func fixPoints() {
let dx = p2.x - p1.x
let dy = p2.y - p1.y
let dist = sqrt(dx*dx + dy*dy)
let diff = (dist - len)/len
p2.updatePosition(NSPoint(x: p2.x - diff*dx*0.5, y: p2.y - diff*dy*0.5))
p1.updatePosition(NSPoint(x: p1.x + diff*dx*0.5, y: p1.y + diff*dy*0.5))
}
}
#objc class Rope: NSObject {
let points: [PointMass]
let constraints: [Constraint]
init(anchor: NSPoint, end: NSPoint, length: Double, count: Int = DEF_POINT_COUNT) {
let anchorPoint = PointMass(anchor)
let endPoint = PointMass(end)
let dx = (anchorPoint.x - endPoint.x)/Double(count)
let dy = (anchorPoint.y - endPoint.y)/Double(count)
let constraintLength = length/Double(count)
var points = [endPoint]
var constraints: [Constraint] = []
for i in 1...count {
let prevPoint = points[i-1]
let newPoint = PointMass(NSPoint(x: prevPoint.x + dx, y: prevPoint.y + dy))
let constraint = Constraint(p1: prevPoint, p2: newPoint, len: constraintLength)
points.append(newPoint)
constraints.append(constraint)
}
self.points = points
self.constraints = constraints
}
func update(anchor: NSPoint, endPoint: NSPoint) {
points.first?.updatePosition(endPoint)
points.last?.updatePosition(anchor)
for point in points {
point.updatePosition()
}
for i in 0...CONST_ITERATIONS {
for constraint in constraints {
constraint.fixPoints()
}
}
}
}

Answer 1

Found it. The problem was in the fixPoints() method, as I suspected.
The line
let diff = (dist - len)/len
should be instead
let diff = (len - dist)/dist