// Kills redundant partial moves.
// Calculates the unit-vector, and kills all incremental moves between A and B.
// Deprecated by OptVector.
func OptBogusMoves(machine *vm.Machine) {
var (
lastvec vector.Vector
state vector.Vector
npos []vm.Position = make([]vm.Position, 0)
)
for _, m := range machine.Positions {
d := m.Vector().Diff(state)
state = m.Vector()
if m.State.MoveMode != vm.MoveModeRapid && m.State.MoveMode != vm.MoveModeLinear {
lastvec = vector.Vector{}
continue
}
if d.X == 0 && d.Y == 0 && d.Z == 0 {
// Why are we doing this again?!
continue
}
norm := d.Norm()
vec := d.Divide(norm)
if vec == lastvec {
npos[len(npos)-1] = m
} else {
npos = append(npos, m)
lastvec = vec
}
}
machine.Positions = npos
}
// Kills redundant partial moves.
// Calculates the unit-vector, and kills all incremental moves between A and B.
func OptVector(machine *vm.Machine, tolerance float64) {
var (
vec1, vec2, vec3 vector.Vector
ready int
length1, length2 float64
lastMoveMode int
npos []vm.Position = make([]vm.Position, 0)
)
for _, m := range machine.Positions {
if m.State.MoveMode != vm.MoveModeLinear && m.State.MoveMode != vm.MoveModeRapid {
ready = 0
goto appendpos
}
if m.State.MoveMode != lastMoveMode {
lastMoveMode = m.State.MoveMode
ready = 0
}
if ready == 0 {
vec1 = m.Vector()
ready++
goto appendpos
} else if ready == 1 {
vec2 = m.Vector()
ready++
goto appendpos
} else if ready == 2 {
vec3 = m.Vector()
ready++
} else {
vec1 = vec2
vec2 = vec3
vec3 = m.Vector()
}
length1 = vec1.Diff(vec2).Norm() + vec2.Diff(vec3).Norm()
length2 = vec1.Diff(vec3).Norm()
if length1-length2 < tolerance {
npos[len(npos)-1] = m
vec2 = vec1
continue
}
appendpos:
npos = append(npos, m)
}
machine.Positions = npos
}
// Detects a previous drill, and uses rapid move to the previous known depth.
// Scans through all Z-descent moves, logs its height, and ensures that any future move
// at that location will use vm.MoveModeRapid to go to the deepest previous known Z-height.
func OptDrillSpeed(machine *vm.Machine) {
var (
last vector.Vector
npos []vm.Position = make([]vm.Position, 0)
drillStack []vm.Position = make([]vm.Position, 0)
)
fastDrill := func(pos vm.Position) (vm.Position, vm.Position, bool) {
var depth float64
var found bool
for _, m := range drillStack {
if m.X == pos.X && m.Y == pos.Y {
if m.Z < depth {
depth = m.Z
found = true
}
}
}
drillStack = append(drillStack, pos)
if found {
if pos.Z >= depth { // We have drilled all of it, so just rapid all the way
pos.State.MoveMode = vm.MoveModeRapid
return pos, pos, false
} else { // Can only rapid some of the way
p := pos
p.Z = depth
p.State.MoveMode = vm.MoveModeRapid
return p, pos, true
}
} else {
return pos, pos, false
}
}
for _, m := range machine.Positions {
if m.X == last.X && m.Y == last.Y && m.Z < last.Z && m.State.MoveMode == vm.MoveModeLinear {
posn, poso, shouldinsert := fastDrill(m)
if shouldinsert {
npos = append(npos, posn)
}
npos = append(npos, poso)
} else {
npos = append(npos, m)
}
last = m.Vector()
}
machine.Positions = npos
}
// Eliminates any bogus moves above Z0
func OptFloatingZ(machine *vm.Machine) {
var last vm.Position
npos := make([]vm.Position, 0)
for _, m := range machine.Positions {
if last.Z > 0 && m.Z > 0 {
if m.Z > npos[len(npos)-1].Z {
npos[len(npos)-1].Z = m.Z
}
} else if last.Z > 0 && m.Z < 0 {
npos = append(npos, last, m)
} else {
npos = append(npos, m)
}
last = m
}
machine.Positions = npos
}
// Reduces moves between paths.
// It does this by scanning through position stack, grouping moves that move from >= Z0 to < Z0.
// These moves are then sorted after closest to previous position, starting at X0 Y0,
// and moves to groups recalculated as they are inserted in a new stack.
// This optimization pass bails if the Z axis is moved simultaneously with any other axis,
// or the input ends with the drill below Z0, in order to play it safe.
// This pass is new, and therefore slightly experimental.
func OptPathGrouping(machine *vm.Machine, tolerance float64) (err error) {
defer func() {
if r := recover(); r != nil {
err = errors.New(fmt.Sprintf("%s", r))
}
}()
type Set []vm.Position
var (
lastx, lasty, lastz float64
sets []Set = make([]Set, 0)
curSet Set = make(Set, 0)
safetyHeight float64
drillSpeed float64
sequenceStarted bool = false
)
// Find grouped drills
for _, m := range machine.Positions {
if m.Z != lastz && (m.X != lastx || m.Y != lasty) {
panic("Complex z-motion detected")
}
if m.X == lastx && m.Y == lasty {
if lastz >= 0 && m.Z < 0 {
// Down move
sequenceStarted = true
// Set drill feedrate
if m.State.MoveMode == vm.MoveModeLinear && m.State.Feedrate > drillSpeed {
if drillSpeed != 0 {
panic("Multiple drill feedrates detected")
}
drillSpeed = m.State.Feedrate
}
} else if lastz < 0 && m.Z >= 0 {
// Up move - ignored in set
//curSet = append(curSet, m)
if sequenceStarted {
sets = append(sets, curSet)
}
sequenceStarted = false
curSet = make(Set, 0)
goto updateLast // Skip append
}
} else {
if m.Z < 0 && m.State.MoveMode == vm.MoveModeRapid {
panic("Rapid move in stock detected")
}
}
if sequenceStarted {
// Regular move
if m.Z > 0 {
panic("Move above stock detected")
}
curSet = append(curSet, m)
}
updateLast:
if m.Z > safetyHeight {
safetyHeight = m.Z
}
lastx, lasty, lastz = m.X, m.Y, m.Z
}
if safetyHeight == 0 {
panic("Unable to detect safety height")
} else if drillSpeed == 0 {
panic("Unable to detect drill feedrate")
}
// If there was a final set without a proper lift
if len(curSet) == 1 {
p := curSet[0]
if p.Z != safetyHeight || lastz != safetyHeight || p.X != 0 || p.Y != 0 {
panic("Incomplete final drill set")
}
} else if len(curSet) > 0 {
panic("Incomplete final drill set")
}
var (
curVec vector.Vector
sortedSets []Set = make([]Set, 0)
selectedSet int
)
// Stupid difference calculator
xyDiff := func(pos vector.Vector, cur vector.Vector) float64 {
j := cur.Diff(pos)
j.Z = 0
return j.Norm()
}
// Sort the sets after distance from current position
for len(sets) > 0 {
for idx, _ := range sets {
if selectedSet == -1 {
selectedSet = idx
} else {
np := sets[idx][0]
pp := sets[selectedSet][0]
diff := xyDiff(np.Vector(), curVec)
other := xyDiff(pp.Vector(), curVec)
if diff < other {
selectedSet = idx
} else if np.Z > pp.Z {
selectedSet = idx
}
}
}
curVec = sets[selectedSet][0].Vector()
sortedSets = append(sortedSets, sets[selectedSet])
sets = append(sets[0:selectedSet], sets[selectedSet+1:]...)
selectedSet = -1
}
// Reconstruct new position stack from sorted sections
newPos := []vm.Position{machine.Positions[0]} // Origin
addPos := func(pos vm.Position) {
newPos = append(newPos, pos)
}
moveTo := func(pos vm.Position) {
curPos := newPos[len(newPos)-1]
// Check if we should go to safety-height before moving
if xyDiff(curPos.Vector(), pos.Vector()) < tolerance {
if curPos.X != pos.X || curPos.Y != pos.Y {
// If we're not 100% precise...
step1 := curPos
step1.State.MoveMode = vm.MoveModeLinear
step1.X = pos.X
step1.Y = pos.Y
addPos(step1)
}
addPos(pos)
} else {
step1 := curPos
step1.Z = safetyHeight
step1.State.MoveMode = vm.MoveModeRapid
step2 := step1
step2.X, step2.Y = pos.X, pos.Y
step3 := step2
step3.Z = pos.Z
step3.State.MoveMode = vm.MoveModeLinear
step3.State.Feedrate = drillSpeed
addPos(step1)
addPos(step2)
addPos(step3)
}
}
for _, m := range sortedSets {
for idx, p := range m {
if idx == 0 {
moveTo(p)
} else {
addPos(p)
}
}
}
machine.Positions = newPos
return nil
}