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];
}
}
