package edu.stanford.rsl.conrad.geometry.motion;
import java.io.Serializable;
import java.util.ArrayList;
import edu.stanford.rsl.conrad.geometry.motion.timewarp.IdentityTimeWarper;
import edu.stanford.rsl.conrad.geometry.motion.timewarp.TimeWarper;
import edu.stanford.rsl.conrad.geometry.shapes.simple.PointND;
import edu.stanford.rsl.conrad.geometry.splines.TimeVariantSurfaceBSpline;
import edu.stanford.rsl.conrad.numerics.SimpleOperators;
import edu.stanford.rsl.conrad.numerics.SimpleVector;
/**
* Implements a linear interpolating motion field based on points over time.
*
* @author akmaier
*
*/
public class PointBasedMotionField implements MotionField, Serializable {
/**
*
*/
private static final long serialVersionUID = 1286210731201948669L;
protected TimeWarper warp;
protected ArrayList<ArrayList<PointND>> timePoints = new ArrayList<ArrayList<PointND>>();
private int context;
protected int timeSamplePoints = 10;
public PointBasedMotionField(TimeVariantSurfaceBSpline [] variants, int context){
this.context = context;
warp = new IdentityTimeWarper();
for (int i = 0; i < variants.length; i++){
if (variants[i].getNumberOfTimePoints() > timeSamplePoints) timeSamplePoints = variants[i].getNumberOfTimePoints();
}
for (int t = 0; t < timeSamplePoints; t++) {
ArrayList<PointND> list = new ArrayList<PointND>();
for (int i = 0; i < variants.length; i++){
list.addAll(variants[i].getControlPoints(t));
}
timePoints.add(list);
}
}
public PointBasedMotionField(ArrayList<TimeVariantSurfaceBSpline> variants, int context){
this.context = context;
warp = new IdentityTimeWarper();
for (int i = 0; i < variants.size(); i++){
if (variants.get(i).getNumberOfTimePoints() > timeSamplePoints) timeSamplePoints = variants.get(i).getNumberOfTimePoints();
}
for (int t = 0; t <timeSamplePoints; t++) {
timePoints.add(new ArrayList<PointND>());
for (int i = 0; i < variants.size(); i++){
timePoints.get(t).addAll(variants.get(i).getControlPoints(t));
}
}
}
public PointBasedMotionField(PointND[][] variants, int context){
this.context = context;
warp = new IdentityTimeWarper();
timeSamplePoints = variants.length;
for (int t = 0; t < timeSamplePoints; t++) {
timePoints.add(new ArrayList<PointND>());
for (int i = 0; i < variants[t].length; i++){
timePoints.get(t).add(variants[t][i]);
}
}
}
private int [] findCloseIndices(PointND position, int t0, int context){
int [] closeIndex = new int[context];
double [] dist = new double [context];
for(int j =0; j<context; j++){
dist[j] = (1.0 / (double)(context-j)) * Double.MAX_VALUE;
}
double maxDistance = Double.MAX_VALUE;
for (int k = 0; k < timePoints.get(t0).size(); k++){
PointND test = timePoints.get(t0).get(k);
double distance = position.euclideanDistance(test);
//System.out.println(k + " " + distance);
if (distance < maxDistance) {
int maxIndex = -1;
double max = -Double.MAX_VALUE;
double lastMax = -Double.MAX_VALUE;
for (int j =0; j<context; j++){
if (dist[j] > max){
lastMax = max;
max = dist[j];
maxIndex = j;
}
}
maxDistance = (lastMax > distance)? lastMax : distance;
dist[maxIndex] = distance;
// pointIndex
closeIndex[maxIndex] = k;
}
}
return closeIndex;
}
public PointND getPosition(PointND initialPosition, double initialTime, double time) {
double t0 = (warp.warpTime(initialTime) * (double)(timeSamplePoints-1));
double t1 = (warp.warpTime(time) * (double)(timeSamplePoints-1));
PointND revan = null;
if ((Math.floor(t0) == t0)&&(Math.floor(t1) == t1)) {
//simple case both values are extact hits in the time frames:
int [] closeIndex1 = findCloseIndices(initialPosition, (int)Math.floor(t0), context);
revan = getInterpolatedPoint(initialPosition, (int)t0, (int)t1, closeIndex1);
} else {
if ((Math.floor(t0) == t0)){ // at least the initial time is a direct hit (e.g. 0)
int [] closeIndex1 = findCloseIndices(initialPosition, (int)Math.floor(t0), context);
revan = getInterpolatedPoint(initialPosition, (int)Math.floor(t0), t1, closeIndex1);
} else { // Most difficult case: no exact hit for t0 and t1
// really inefficient
int [] closeIndex1 = findCloseIndices(initialPosition, (int)Math.floor(t0), context);
int [] closeIndex2 = findCloseIndices(initialPosition, (int)Math.ceil(t0), context);
revan = getInterpolatedPoint(initialPosition, t0, t1, closeIndex1, closeIndex2);
}
}
return revan;
}
/**
* Method to interpolate a point if the initial Position is a direct hit on the interpolation grid of time t0, i.e. t0 is of type int.
* @param initialPosition the initalPosition
* @param t0 the initial time
* @param t1 the target time as double. will be interpolated by linear interpolation
* @param closeIndex the indices of the closest points in the interpolation grid at time t0
* @return the linear interpolated point
*/
private PointND getInterpolatedPoint(PointND initialPosition, int t0, double t1, int [] closeIndex){
PointND one = getInterpolatedPoint(initialPosition, (int)t0, (int)Math.floor(t1), closeIndex);
PointND two = getInterpolatedPoint(initialPosition, (int)t0, (int)Math.ceil(t1), closeIndex);
one.getAbstractVector().multiplyBy(1.0-(t1-Math.floor(t1)));
two.getAbstractVector().multiplyBy(1.0-(Math.ceil(t1) - t1));
one.getAbstractVector().add(two.getAbstractVector());
return one;
}
/**
* Method to interpolate a point if the initial position is not a direct hit on the interpolation grid, i.e. it is of type double.
* The target point will be determined by bilinear interpolation.
* @param initialPosition the initial position
* @param t0 the initial time
* @param t1 the target time
* @param closeIndex1 the close neighbors at time Math.floor(t0)
* @param closeIndex2 the close neighbors at time Math.ceil(t0)
* @return the bilinear interpolated point
*/
private PointND getInterpolatedPoint(PointND initialPosition, double t0, double t1, int [] closeIndex1, int []closeIndex2){
PointND one = getInterpolatedPoint(initialPosition, (int)Math.floor(t0), (int)Math.floor(t1), closeIndex1);
PointND two = getInterpolatedPoint(initialPosition, (int)Math.floor(t0), (int)Math.ceil(t1), closeIndex1);
PointND three = getInterpolatedPoint(initialPosition, (int)Math.ceil(t0), (int)Math.floor(t1), closeIndex2);
PointND four = getInterpolatedPoint(initialPosition, (int)Math.ceil(t0), (int)Math.ceil(t1), closeIndex2);
one.getAbstractVector().multiplyBy(1.0-(t0 - Math.floor(t0)));
two.getAbstractVector().multiplyBy(1.0-(t0 - Math.floor(t0)));
three.getAbstractVector().multiplyBy(1.0-(Math.ceil(t0)-t0));
four.getAbstractVector().multiplyBy(1.0-(Math.ceil(t0)-t0));
one.getAbstractVector().multiplyBy(1.0-(t1-Math.floor(t1)));
two.getAbstractVector().multiplyBy(1.0-(Math.ceil(t1) - t1));
three.getAbstractVector().multiplyBy(1.0-(t1-Math.floor(t1)));
four.getAbstractVector().multiplyBy(1.0-(Math.ceil(t1) - t1));
one.getAbstractVector().add(two.getAbstractVector());
one.getAbstractVector().add(three.getAbstractVector());
one.getAbstractVector().add(four.getAbstractVector());
return one;
}
private PointND getInterpolatedPoint(PointND initialPosition, int t0, int t1, int [] closeIndex){
PointND p = initialPosition.clone();
PointND [] close = new PointND[context];
PointND [] dest = new PointND[context];
double [] dist = new double [context];
for (int i=0; i < context; i++){
close[i] = timePoints.get(t0).get(closeIndex[i]);
dest[i] = timePoints.get(t1).get(closeIndex[i]);
dist[i] = p.euclideanDistance(close[i]);
}
SimpleVector shift = new SimpleVector(3);
double sum = 0;
for (int j =0; j < context; j++){
double weight = 1000000;
if (dist[j] != 0){
weight = 1.0 / dist[j];
}
sum += weight;
SimpleVector partialStep = SimpleOperators.subtract(dest[j].getAbstractVector(), close[j].getAbstractVector()).multipliedBy(weight);
shift.add(partialStep);
}
p.getAbstractVector().add(shift.multipliedBy(1.0/sum));
return p;
}
public ArrayList<PointND> getPositions(double initialTime,
double time, PointND ... initialPositions) {
ArrayList<PointND> list = new ArrayList<PointND>();
for (int i=0; i< initialPositions.length; i++){
list.add(getPosition(initialPositions[i], initialTime, time));
}
return list;
}
public ArrayList<PointND> getPositions(PointND initialPoint, double initialTime, double ... times){
ArrayList<PointND> points = new ArrayList<PointND>();
double t0 = (warp.warpTime(initialTime) * (double)(timeSamplePoints-1));
if ((Math.floor(t0) == t0)) {
int [] closeIndex = findCloseIndices(initialPoint, (int)Math.floor(t0), context);
for (double t:times) {
double t1 = (warp.warpTime(t) * (double)(timeSamplePoints-1));
if (Math.floor(t1) == t1) {
points.add(getInterpolatedPoint(initialPoint, (int)Math.floor(t0), (int)Math.floor(t1), closeIndex));
} else {
points.add(getInterpolatedPoint(initialPoint, (int)Math.floor(t0), t1, closeIndex));
}
}
} else {
int [] closeIndex1 = findCloseIndices(initialPoint, (int)Math.floor(t0), context);
int [] closeIndex2 = findCloseIndices(initialPoint, (int)Math.ceil(t0), context);
for (double t:times) {
double t1 = (warp.warpTime(t) * (double)(timeSamplePoints-1));
points.add(getInterpolatedPoint(initialPoint, t0, t1, closeIndex1, closeIndex2));
}
}
return points;
}
public TimeWarper getTimeWarper() {
return warp;
}
public void setTimeWarper(TimeWarper warp) {
this.warp = warp;
}
}
/*
* Copyright (C) 2010-2014 Andreas Maier
* CONRAD is developed as an Open Source project under the GNU General Public License (GPL).
*/