private List <MoveActionChain> SplitChainByDirectionFlip(MoveActionChain origChain) { List <MoveActionChain> chains = new List <MoveActionChain>(); MoveActionChain chain = new MoveActionChain(); var actions = origChain.Actions; IRTMoveCommand prevCmd = null; foreach (var cmd in actions) { if (prevCmd != null) { Vector3 dir_cur = cmd.DirStart; Vector3 dir_prev = prevCmd.DirEnd; if (dir_cur * dir_prev <= 1e-3m) { chains.Add(chain); chain = new MoveActionChain(); } } chain.Actions.Add(cmd); prevCmd = cmd; } if (chain.Actions.Count > 0) { chains.Add(chain); } return(chains); }
public void ProcessProgram(ActionProgram.ActionProgram program) { List <MoveActionChain> chains = new List <MoveActionChain>(); MoveActionChain chain = new MoveActionChain(); var actions = program.Actions; Logger.Instance.Debug(this, "optimize", "begin optimization"); int index = 1; int na = actions.Count; foreach (var action in actions) { Logger.Instance.Debug(this, "optimize", string.Format("Process action #{0} / {1}", index, na)); var ma = action.action as RTAction; if (ma == null || ma.RequireFinish || ma.Command as IRTMoveCommand == null) { if (ma == null || ma.RequireFinish) { if (chain.Actions.Count > 0) { var subc = SplitChainByDirectionFlip(chain); chains.AddRange(subc); chain = new MoveActionChain(); } } } else { var cmd = ma.Command as IRTMoveCommand; chain.Actions.Add(cmd); } index += 1; } if (chain.Actions.Count > 0) { var subc = SplitChainByDirectionFlip(chain); chains.AddRange(subc); } index = 1; int nc = chains.Count; foreach (var c in chains) { Logger.Instance.Debug(this, "optimize", string.Format("Optimize chain #{0} / {1}. It has {2} actions", index, nc, c.Actions.Count)); OptimizeChain(c); index += 1; } Logger.Instance.Debug(this, "optimize", "ready optimization"); }
private void OptimizeChain(MoveActionChain chain) { var actions = chain.Actions; int len = actions.Count; // Maximal feed at segment decimal[] maxFeedAtSegment = new decimal[len]; // Distance of joints from chain begin decimal[] X = new decimal[len + 1]; X[0] = 0; for (int i = 0; i < len; ++i) { maxFeedAtSegment[i] = actions[i].Options.Feed; X[i + 1] = X[i] + actions[i].Length; //Console.WriteLine("X[{0}] = {1}", i+1, X[i+1]); } // Maximal feed at the ends of segments decimal[] maxFeedAtJoint = new decimal[len + 1]; maxFeedAtJoint[0] = 0; maxFeedAtJoint[len] = 0; for (int i = 1; i < len; ++i) { int m1 = i - 1; int m2 = i; Vector3 dirEnd = actions[m1].DirEnd; Vector3 dirStart = actions[m2].DirStart; decimal mf = MaxFeed(dirEnd * dirStart, config.max_movement_leap); maxFeedAtJoint[m2] = Math.Min(mf, Math.Min(maxFeedAtSegment[m1], maxFeedAtSegment[m2])); } // Calculate feeds decimal[] feedAtSegment = new decimal[len]; decimal[] feedAtJoint = new decimal[len + 1]; feedAtJoint[0] = 0; feedAtJoint[len] = 0; // Find minimal intersection point for segments for (int i = 0; i < maxFeedAtSegment.Length; ++i) { decimal xmin = 0; decimal fmin = Decimal.MaxValue; // Find minimum from all pairs of acceleration and deceleration intersections for (int j = 0; j <= i; ++j) { for (int k = i + 1; k < maxFeedAtJoint.Length; ++k) { decimal x0 = X[j]; decimal feed0 = maxFeedAtJoint[j]; decimal x1 = X[k]; decimal feed1 = maxFeedAtJoint[k]; var intersection = AcceletationDecelerationIntersection(x0, feed0, x1, feed1, config.max_acceleration); decimal feed = intersection.feed; if (feed < fmin) { fmin = feed; xmin = intersection.x; } } } // Fill segment begin, end and center feeds decimal segmentFeedStart; decimal segmentFeedEnd; decimal segmentFeed; if (xmin < X[i]) { segmentFeedStart = DecelerationInitialFeed(xmin, fmin, X[i], config.max_acceleration); segmentFeedEnd = DecelerationInitialFeed(xmin, fmin, X[i + 1], config.max_acceleration); segmentFeed = segmentFeedStart; } else if (xmin > X[i + 1]) { segmentFeedStart = AccelerationInitialFeed(xmin, fmin, X[i], config.max_acceleration); segmentFeedEnd = AccelerationInitialFeed(xmin, fmin, X[i + 1], config.max_acceleration); segmentFeed = segmentFeedEnd; } else { segmentFeedStart = AccelerationInitialFeed(xmin, fmin, X[i], config.max_acceleration); segmentFeedEnd = DecelerationInitialFeed(xmin, fmin, X[i + 1], config.max_acceleration); segmentFeed = fmin; } feedAtSegment[i] = Math.Min(segmentFeed, maxFeedAtSegment[i]); feedAtJoint[i] = Math.Min(segmentFeedStart, feedAtSegment[i]); feedAtJoint[i + 1] = Math.Min(segmentFeedEnd, feedAtSegment[i]); } // Fill actions feeds for (int i = 0; i < len; i++) { actions[i].Options.Feed = feedAtSegment[i]; actions[i].Options.FeedStart = feedAtJoint[i]; actions[i].Options.FeedEnd = feedAtJoint[i + 1]; } }