@Override
void computeTangent(KeyFrame prev, KeyFrame next) {
tgPVec = Vec.multiply(Vec.subtract(next.position(), prev.position()), 0.5f);
tgQuat =
Quat.squadTangent(
(Quat) prev.orientation(), (Quat) orientation(), (Quat) next.orientation());
}
public void setSpeed(float Speed) {
this.mSpeed = Speed;
float multiplyer = Speed / mDuration;
for (KeyFrame k : mKeyFrames) {
k.mTime *= multiplyer;
}
}
/**
* Internal use. Calls {@link #invalidateValues()} if a keyFrame (frame) defining the path was
* recently modified.
*/
protected void checkValidity() {
boolean flag = false;
for (KeyFrame element : keyFrameList) {
if (element.frame().lastUpdate() > lastUpdate()) {
flag = true;
break;
}
}
if (flag) {
this.invalidateValues();
this.checked();
}
}
private KeyFrame interpolateRotation(KeyFrame start, KeyFrame end, double timestamp) {
double startAngle = start.getOrientation();
double endAngle = end.getOrientation();
double theta = Angle.getRotationAngle(startAngle, endAngle);
if (end.useRelativeAngle()) {
theta = endAngle - startAngle;
}
double ds = start.getTimestamp();
double duration = end.getTimestamp() - ds;
double d = (timestamp - ds) / duration;
double angle = startAngle + d * theta;
Point2D p = start.getPosition();
KeyFrame frame =
new KeyFrame(
TrajectoryMovement.ROTATION,
0,
angle,
0,
start.getRotationSpeed(),
p,
timestamp,
start.getSourceLine());
return frame;
}
private KeyFrame getKeyFrame(double timestamp) {
if (keyFrame == null || keyFrame.getTimestamp() != timestamp) {
KeyFrameInterpolator interpolator = new KeyFrameInterpolator();
keyFrame = interpolator.interpolate(frames, timestamp);
}
return keyFrame;
}
protected void updateModifiedFrameValues() {
KeyFrame kf;
KeyFrame prev = keyFrameList.get(0);
kf = keyFrameList.get(0);
int index = 1;
while (kf != null) {
KeyFrame next = (index < keyFrameList.size()) ? keyFrameList.get(index) : null;
index++;
if (next != null) kf.computeTangent(prev, next);
else kf.computeTangent(prev, kf);
prev = kf;
kf = next;
}
valuesAreValid = true;
}
private KeyFrame interpolateStraightLine(KeyFrame start, KeyFrame end, double timestamp) {
Point2D startPoint = start.getPosition();
Point2D endPoint = end.getPosition();
double ds = start.getTimestamp();
double duration = end.getTimestamp() - ds;
double d = (timestamp - ds) / duration;
double x1 = startPoint.getX();
double y1 = startPoint.getY();
double x2 = endPoint.getX();
double y2 = endPoint.getY();
double x = x1 + d * (x2 - x1);
double y = y1 + d * (y2 - y1);
Double p = new Point2D.Double(x, y);
KeyFrame frame =
new KeyFrame(
TrajectoryMovement.LINE,
start.getMovementSpeed(),
end.getOrientation(),
0,
0,
p,
timestamp,
start.getSourceLine());
return frame;
}
protected KeyFrameInterpolator(KeyFrameInterpolator otherKFI) {
this.gScene = otherKFI.gScene;
this.path = new ArrayList<Frame>();
ListIterator<Frame> frameIt = otherKFI.path.listIterator();
while (frameIt.hasNext()) {
this.path.add(frameIt.next().get());
}
this.setFrame(otherKFI.frame());
this.period = otherKFI.period;
this.interpolationTm = otherKFI.interpolationTm;
this.interpolationSpd = otherKFI.interpolationSpd;
this.interpolationStrt = otherKFI.interpolationStrt;
this.lpInterpolation = otherKFI.lpInterpolation;
this.pathIsValid = otherKFI.pathIsValid;
this.valuesAreValid = otherKFI.valuesAreValid;
this.currentFrmValid = otherKFI.currentFrmValid;
this.keyFrameList = new ArrayList<KeyFrame>();
for (KeyFrame element : otherKFI.keyFrameList) {
KeyFrame kf = (KeyFrame) element.get();
this.keyFrameList.add(kf);
}
this.currentFrame0 = keyFrameList.listIterator(otherKFI.currentFrame0.nextIndex());
this.currentFrame1 = keyFrameList.listIterator(otherKFI.currentFrame1.nextIndex());
this.currentFrame2 = keyFrameList.listIterator(otherKFI.currentFrame2.nextIndex());
this.currentFrame3 = keyFrameList.listIterator(otherKFI.currentFrame3.nextIndex());
this.interpolationTimerTask =
new TimingTask() {
public void execute() {
update();
}
};
gScene.registerTimingTask(interpolationTimerTask);
this.invalidateValues();
}
private KeyFrame interpolate(KeyFrame start, KeyFrame end, double timestamp) {
switch (end.getMovement()) {
case ROTATION:
return interpolateRotation(start, end, timestamp);
case LINE:
return interpolateStraightLine(start, end, timestamp);
case BEZIER:
return interpolateBezier(start, end, timestamp);
default:
return start;
}
}
public void update() {
if (!isPlaying || !updateVertices) return;
currentTime = System.currentTimeMillis();
KeyFrame currentFrame = frames[currentFrameIndex];
KeyFrame nextFrame = frames[(currentFrameIndex + 1) % numFrames];
if (currentFrameName != null && !currentFrameName.equals(currentFrame.getName())) {
if (!loop) stop();
else currentFrameIndex = loopStartIndex;
return;
}
float[] currentVerts = currentFrame.getVertices();
float[] nextVerts = nextFrame.getVertices();
float[] currentNormals = currentFrame.getNormals();
float[] nextNormals = nextFrame.getNormals();
int numVerts = currentVerts.length;
float[] interPolatedVerts = new float[numVerts];
float[] interPolatedNormals = new float[numVerts];
for (int i = 0; i < numVerts; i += 3) {
interPolatedVerts[i] = currentVerts[i] + interpolation * (nextVerts[i] - currentVerts[i]);
interPolatedVerts[i + 1] =
currentVerts[i + 1] + interpolation * (nextVerts[i + 1] - currentVerts[i + 1]);
interPolatedVerts[i + 2] =
currentVerts[i + 2] + interpolation * (nextVerts[i + 2] - currentVerts[i + 2]);
interPolatedNormals[i] =
currentNormals[i] + interpolation * (nextNormals[i] - currentNormals[i]);
interPolatedNormals[i + 1] =
currentNormals[i + 1] + interpolation * (nextNormals[i + 1] - currentNormals[i + 1]);
interPolatedNormals[i + 2] =
currentNormals[i + 2] + interpolation * (nextNormals[i + 2] - currentNormals[i + 2]);
}
interpolation += fps * (currentTime - startTime) / 1000;
vertices().overwriteNormals(interPolatedNormals);
vertices().overwriteVerts(interPolatedVerts);
if (interpolation > 1) {
interpolation = 0;
currentFrameIndex++;
if (currentFrameIndex >= numFrames) currentFrameIndex = 0;
}
startTime = System.currentTimeMillis();
}
private KeyFrame interpolateBezier(KeyFrame start, KeyFrame end, double timestamp) {
double ds = start.getTimestamp();
double duration = end.getTimestamp() - ds;
double d = (timestamp - ds) / duration;
Point2D[] points = end.getControlPoints();
Point2D s = start.getPosition();
Point2D e = end.getPosition();
Point2D p = Bezier.getPoint(d, s, points[0], points[1], e);
double angle = Bezier.getAngle(d, s, points[0], points[1], e);
KeyFrame frame =
new KeyFrame(
TrajectoryMovement.BEZIER,
start.getMovementSpeed(),
angle,
0,
0,
p,
timestamp,
start.getSourceLine(),
points);
return frame;
}
public StraightLine(
String name, double initialAngle, double finalAngle, ArrayList<KeyFrame> frames) {
super(name, new Point2D.Double(), null, null, initialAngle, finalAngle);
this.frames = frames;
order = 1000;
int n = frames.size();
start = frames.get(0).getPosition();
end = frames.get(n - 1).getPosition();
boolean forward = true;
path = new Path2D.Double();
path.moveTo(start.getX(), start.getY());
for (int i = 1; i < n; i++) {
KeyFrame frame = frames.get(i);
TrajectoryMovement movement = frame.getMovement();
switch (movement) {
case LINE:
// handles backward moves by adding an extra moveto instruction
if (forward == frame.getMovementSpeed() < 0) {
Point2D p = path.getCurrentPoint();
path.moveTo(p.getX(), p.getY());
forward = !forward;
}
Point2D p = frame.getPosition();
path.lineTo(p.getX(), p.getY());
break;
case BEZIER:
p = frame.getPosition();
Point2D[] cp = frame.getControlPoints();
path.curveTo(cp[0].getX(), cp[0].getY(), cp[1].getX(), cp[1].getY(), p.getX(), p.getY());
break;
default:
break;
}
}
String description = getTrajectoryDescription();
setProperty(KEY_DESCRIPTION, description);
}
private void writeLine(StringBuilder sb, KeyFrame frame) {
int line = frame.getSourceLine();
sb.append(String.format("// line %d\r\n", line));
}
@Override
public String getTrajectoryDescription() {
StringBuilder sb = new StringBuilder();
Point2D last = null;
double lastOrientation = 0;
double lastTimestamp = 0;
Map<String, String> params = new HashMap<String, String>();
for (KeyFrame frame : frames) {
writeLine(sb, frame);
if (frame.hasComments()) {
ArrayList<String> comments = frame.getComments();
for (String s : comments) {
addParameters(params, s);
}
}
TrajectoryMovement movement = frame.getMovement();
switch (movement) {
case START:
last = frame.getPosition();
lastOrientation = frame.getOrientation();
sb.append(String.format("// start at %s\r\n", last.toString()));
break;
case BEZIER:
break;
case CLOTHOID:
break;
case LINE:
Point2D p = frame.getPosition();
double distance = p.distance(last);
double speed = frame.getMovementSpeed();
String direction = speed > 0 ? "forward" : "backward";
addComments(
params,
String.format(
"// move %s of %.0f mm (%.0f)",
direction, distance, 9.557 * distance * Math.signum(speed)),
true);
addValue(params, KEY_DISTANCE, 9.557 * distance * Math.signum(speed));
last = p;
break;
case NONE:
// double delay = frame.getTimestamp() - lastTimestamp;
// addComments(params, String.format("delay_ms(%.0f);", delay * 1000), true);
break;
case ROTATION:
double o = frame.getOrientation();
double angle = Angle.getRotationAngle(lastOrientation, o);
if (frame.useRelativeAngle()) {
// the angle is a relative angle that can be > 180°
angle = frame.getRelativeAngle();
}
angle = Math.toDegrees(angle);
addComments(
params,
String.format("// rotation of %.0f° (%.0f)", angle, 22527.5d / 360d * angle),
true);
addValue(params, KEY_ANGLE, 22527.5d / 360d * angle);
if (params.containsKey(KEY_ORIENTATION)) {
double opposite = getDoubleValue(params, KEY_ORIENTATION);
opposite = Math.toDegrees(opposite);
addValue(params, KEY_ORIENTATION, 22527.5d / 360d * opposite);
}
lastOrientation = o;
break;
default:
break;
}
lastTimestamp = frame.getTimestamp();
writeCommands(sb, params);
}
return sb.toString();
}
@Override
public double getSourceLine(double timestamp) {
KeyFrame frame = getKeyFrame(timestamp);
return frame.getSourceLine();
}
@Override
public Point2D getPosition(double timestamp) {
KeyFrame frame = getKeyFrame(timestamp);
return frame.getPosition();
}
@Override
public double getOrientation(double timestamp) {
KeyFrame frame = getKeyFrame(timestamp);
return frame.getOrientation();
}
@Override
void computeTangent(KeyFrame prev, KeyFrame next) {
tgPVec = Vec.multiply(Vec.subtract(next.position(), prev.position()), 0.5f);
}