mirror of
https://github.com/plexusorg/Plex-FAWE.git
synced 2025-06-11 20:13:55 +00:00
Added /curve.
This commit is contained in:
orthoplex64
TomyLobo
@ -0,0 +1,68 @@
|
||||
// $Id$
|
||||
/*
|
||||
* WorldEditLibrary
|
||||
* Copyright (C) 2010 sk89q <http://www.sk89q.com> and contributors
|
||||
*
|
||||
* This program is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
package com.sk89q.worldedit.interpolation;
|
||||
|
||||
import java.util.List;
|
||||
|
||||
import com.sk89q.worldedit.Vector;
|
||||
|
||||
/**
|
||||
* Represents an arbitrary function in ℝ → ℝ<sup>3</sup>
|
||||
*
|
||||
* @author TomyLobo
|
||||
*
|
||||
*/
|
||||
public interface Interpolation {
|
||||
/**
|
||||
* Sets nodes to be used by subsequent calls to
|
||||
* {@link #getPosition(double)} and the other methods.
|
||||
*
|
||||
* @param nodes
|
||||
*/
|
||||
public void setNodes(List<Node> nodes);
|
||||
|
||||
/**
|
||||
* Gets the result of f(position)
|
||||
*
|
||||
* @param position
|
||||
* @return
|
||||
*/
|
||||
public Vector getPosition(double position);
|
||||
|
||||
/**
|
||||
* Gets the result of f'(position).
|
||||
*
|
||||
* @param position
|
||||
* @return
|
||||
*/
|
||||
public Vector get1stDerivative(double position);
|
||||
|
||||
/**
|
||||
* Gets the result of ∫<sub>a</sub><sup style="position: relative; left: -1ex">b</sup>|f'(t)| dt.<br />
|
||||
* That means it calculates the arc length (in meters) between positionA
|
||||
* and positionB.
|
||||
*
|
||||
* @param positionA lower limit
|
||||
* @param positionB upper limit
|
||||
* @return
|
||||
*/
|
||||
double arcLength(double positionA, double positionB);
|
||||
|
||||
int getSegment(double position);
|
||||
}
|
||||
@ -0,0 +1,253 @@
|
||||
// $Id$
|
||||
/*
|
||||
* WorldEditLibrary
|
||||
* Copyright (C) 2010 sk89q <http://www.sk89q.com> and contributors
|
||||
*
|
||||
* This program is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
package com.sk89q.worldedit.interpolation;
|
||||
|
||||
import java.util.Collections;
|
||||
import java.util.List;
|
||||
|
||||
import com.sk89q.worldedit.Vector;
|
||||
|
||||
/**
|
||||
* Kochanek-Bartels interpolation.<br />
|
||||
* Continuous in the 2nd derivative.<br />
|
||||
* Supports {@link Node#tension tension}, {@link Node#bias bias} and
|
||||
* {@link Node#continuity continuity} parameters per {@link Node}
|
||||
*
|
||||
* @author TomyLobo
|
||||
*
|
||||
*/
|
||||
public class KochanekBartelsInterpolation implements Interpolation {
|
||||
private List<Node> nodes;
|
||||
private Vector[] coeffA;
|
||||
private Vector[] coeffB;
|
||||
private Vector[] coeffC;
|
||||
private Vector[] coeffD;
|
||||
private double scaling;
|
||||
|
||||
public KochanekBartelsInterpolation() {
|
||||
setNodes(Collections.<Node>emptyList());
|
||||
}
|
||||
|
||||
@Override
|
||||
public void setNodes(List<Node> nodes) {
|
||||
this.nodes = nodes;
|
||||
recalc();
|
||||
}
|
||||
|
||||
private void recalc() {
|
||||
final int nNodes = nodes.size();
|
||||
coeffA = new Vector[nNodes];
|
||||
coeffB = new Vector[nNodes];
|
||||
coeffC = new Vector[nNodes];
|
||||
coeffD = new Vector[nNodes];
|
||||
|
||||
if (nNodes == 0)
|
||||
return;
|
||||
|
||||
Node nodeB = nodes.get(0);
|
||||
double tensionB = nodeB.getTension();
|
||||
double biasB = nodeB.getBias();
|
||||
double continuityB = nodeB.getContinuity();
|
||||
for (int i = 0; i < nNodes; ++i) {
|
||||
final double tensionA = tensionB;
|
||||
final double biasA = biasB;
|
||||
final double continuityA = continuityB;
|
||||
|
||||
if (i + 1 < nNodes) {
|
||||
nodeB = nodes.get(i + 1);
|
||||
tensionB = nodeB.getTension();
|
||||
biasB = nodeB.getBias();
|
||||
continuityB = nodeB.getContinuity();
|
||||
}
|
||||
|
||||
// Kochanek-Bartels tangent coefficients
|
||||
final double ta = (1-tensionA)*(1+biasA)*(1+continuityA)/2; // Factor for lhs of d[i]
|
||||
final double tb = (1-tensionA)*(1-biasA)*(1-continuityA)/2; // Factor for rhs of d[i]
|
||||
final double tc = (1-tensionB)*(1+biasB)*(1-continuityB)/2; // Factor for lhs of d[i+1]
|
||||
final double td = (1-tensionB)*(1-biasB)*(1+continuityB)/2; // Factor for rhs of d[i+1]
|
||||
|
||||
coeffA[i] = linearCombination(i, -ta, ta- tb-tc+2, tb+tc-td-2, td);
|
||||
coeffB[i] = linearCombination(i, 2*ta, -2*ta+2*tb+tc-3, -2*tb-tc+td+3, -td);
|
||||
coeffC[i] = linearCombination(i, -ta, ta- tb , tb , 0);
|
||||
//coeffD[i] = linearCombination(i, 0, 1, 0, 0);
|
||||
coeffD[i] = retrieve(i); // this is an optimization
|
||||
}
|
||||
|
||||
scaling = nodes.size() - 1;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the linear combination of the given coefficients with the nodes adjacent to baseIndex.
|
||||
*
|
||||
* @param baseIndex node index
|
||||
* @param f1 coefficient for baseIndex-1
|
||||
* @param f2 coefficient for baseIndex
|
||||
* @param f3 coefficient for baseIndex+1
|
||||
* @param f4 coefficient for baseIndex+2
|
||||
* @return linear combination of nodes[n-1..n+2] with f1..4
|
||||
*/
|
||||
private Vector linearCombination(int baseIndex, double f1, double f2, double f3, double f4) {
|
||||
final Vector r1 = retrieve(baseIndex - 1).multiply(f1);
|
||||
final Vector r2 = retrieve(baseIndex ).multiply(f2);
|
||||
final Vector r3 = retrieve(baseIndex + 1).multiply(f3);
|
||||
final Vector r4 = retrieve(baseIndex + 2).multiply(f4);
|
||||
|
||||
return r1.add(r2).add(r3).add(r4);
|
||||
}
|
||||
|
||||
/**
|
||||
* Retrieves a node. Indexes are clamped to the valid range.
|
||||
*
|
||||
* @param index node index to retrieve
|
||||
* @return nodes[clamp(0, nodes.length-1)]
|
||||
*/
|
||||
private Vector retrieve(int index) {
|
||||
if (index < 0)
|
||||
return fastRetrieve(0);
|
||||
|
||||
if (index >= nodes.size())
|
||||
return fastRetrieve(nodes.size()-1);
|
||||
|
||||
return fastRetrieve(index);
|
||||
}
|
||||
|
||||
private Vector fastRetrieve(int index) {
|
||||
return nodes.get(index).getPosition();
|
||||
}
|
||||
|
||||
@Override
|
||||
public Vector getPosition(double position) {
|
||||
if (coeffA == null)
|
||||
throw new IllegalStateException("Must call setNodes first.");
|
||||
|
||||
if (position > 1)
|
||||
return null;
|
||||
|
||||
position *= scaling;
|
||||
|
||||
final int index = (int) Math.floor(position);
|
||||
final double remainder = position - index;
|
||||
|
||||
final Vector a = coeffA[index];
|
||||
final Vector b = coeffB[index];
|
||||
final Vector c = coeffC[index];
|
||||
final Vector d = coeffD[index];
|
||||
|
||||
return a.multiply(remainder).add(b).multiply(remainder).add(c).multiply(remainder).add(d);
|
||||
}
|
||||
|
||||
@Override
|
||||
public Vector get1stDerivative(double position) {
|
||||
if (coeffA == null)
|
||||
throw new IllegalStateException("Must call setNodes first.");
|
||||
|
||||
if (position > 1)
|
||||
return null;
|
||||
|
||||
position *= scaling;
|
||||
|
||||
final int index = (int) Math.floor(position);
|
||||
//final double remainder = position - index;
|
||||
|
||||
final Vector a = coeffA[index];
|
||||
final Vector b = coeffB[index];
|
||||
final Vector c = coeffC[index];
|
||||
|
||||
return a.multiply(1.5*position - 3.0*index).add(b).multiply(2.0*position).add(a.multiply(1.5*index).subtract(b).multiply(2.0*index)).add(c).multiply(scaling);
|
||||
}
|
||||
|
||||
@Override
|
||||
public double arcLength(double positionA, double positionB) {
|
||||
if (coeffA == null)
|
||||
throw new IllegalStateException("Must call setNodes first.");
|
||||
|
||||
if (positionA > positionB)
|
||||
return arcLength(positionB, positionA);
|
||||
|
||||
positionA *= scaling;
|
||||
positionB *= scaling;
|
||||
|
||||
final int indexA = (int) Math.floor(positionA);
|
||||
final double remainderA = positionA - indexA;
|
||||
|
||||
final int indexB = (int) Math.floor(positionB);
|
||||
final double remainderB = positionB - indexB;
|
||||
|
||||
return arcLengthRecursive(indexA, remainderA, indexB, remainderB);
|
||||
}
|
||||
|
||||
/**
|
||||
* Assumes a < b
|
||||
*
|
||||
* @param indexLeft
|
||||
* @param remainderLeft
|
||||
* @param indexRight
|
||||
* @param remainderRight
|
||||
* @return
|
||||
*/
|
||||
private double arcLengthRecursive(int indexLeft, double remainderLeft, int indexRight, double remainderRight) {
|
||||
switch (indexRight - indexLeft) {
|
||||
case 0:
|
||||
return arcLengthRecursive(indexLeft, remainderLeft, remainderRight);
|
||||
|
||||
case 1:
|
||||
// This case is merely a speed-up for a very common case
|
||||
return
|
||||
arcLengthRecursive(indexLeft, remainderLeft, 1.0) +
|
||||
arcLengthRecursive(indexRight, 0.0, remainderRight);
|
||||
|
||||
default:
|
||||
return
|
||||
arcLengthRecursive(indexLeft, remainderLeft, indexRight - 1, 1.0) +
|
||||
arcLengthRecursive(indexRight, 0.0, remainderRight);
|
||||
}
|
||||
}
|
||||
|
||||
private double arcLengthRecursive(int index, double remainderLeft, double remainderRight) {
|
||||
final Vector a = coeffA[index].multiply(3.0);
|
||||
final Vector b = coeffB[index].multiply(2.0);
|
||||
final Vector c = coeffC[index];
|
||||
|
||||
final int nPoints = 8;
|
||||
|
||||
double accum = a.multiply(remainderLeft).add(b).multiply(remainderLeft).add(c).length() / 2.0;
|
||||
for (int i = 1; i < nPoints-1; ++i) {
|
||||
double t = ((double) i) / nPoints;
|
||||
t = (remainderRight-remainderLeft)*t + remainderLeft;
|
||||
accum += a.multiply(t).add(b).multiply(t).add(c).length();
|
||||
}
|
||||
|
||||
accum += a.multiply(remainderRight).add(b).multiply(remainderRight).add(c).length() / 2.0;
|
||||
return accum * (remainderRight - remainderLeft) / nPoints;
|
||||
}
|
||||
|
||||
@Override
|
||||
public int getSegment(double position) {
|
||||
if (coeffA == null)
|
||||
throw new IllegalStateException("Must call setNodes first.");
|
||||
|
||||
if (position > 1)
|
||||
return Integer.MAX_VALUE;
|
||||
|
||||
position *= scaling;
|
||||
|
||||
final int index = (int) Math.floor(position);
|
||||
return index;
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,158 @@
|
||||
// $Id$
|
||||
/*
|
||||
* WorldEditLibrary
|
||||
* Copyright (C) 2010 sk89q <http://www.sk89q.com> and contributors
|
||||
*
|
||||
* This program is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
package com.sk89q.worldedit.interpolation;
|
||||
|
||||
import java.util.List;
|
||||
|
||||
import com.sk89q.worldedit.Vector;
|
||||
|
||||
/**
|
||||
* Simple linear interpolation. Mainly used for testing.
|
||||
*
|
||||
* @author TomyLobo
|
||||
*
|
||||
*/
|
||||
public class LinearInterpolation implements Interpolation {
|
||||
private List<Node> nodes;
|
||||
|
||||
@Override
|
||||
public void setNodes(List<Node> nodes) {
|
||||
this.nodes = nodes;
|
||||
}
|
||||
|
||||
@Override
|
||||
public Vector getPosition(double position) {
|
||||
if (nodes == null)
|
||||
throw new IllegalStateException("Must call setNodes first.");
|
||||
|
||||
if (position > 1)
|
||||
return null;
|
||||
|
||||
position *= nodes.size() - 1;
|
||||
|
||||
final int index1 = (int) Math.floor(position);
|
||||
final double remainder = position - index1;
|
||||
|
||||
final Vector position1 = nodes.get(index1).getPosition();
|
||||
final Vector position2 = nodes.get(index1 + 1).getPosition();
|
||||
|
||||
return position1.multiply(1.0 - remainder).add(position2.multiply(remainder));
|
||||
}
|
||||
|
||||
/*
|
||||
Formula for position:
|
||||
p1*(1-t) + p2*t
|
||||
Formula for position in Horner/monomial form:
|
||||
(p2-p1)*t + p1
|
||||
1st Derivative:
|
||||
p2-p1
|
||||
2nd Derivative:
|
||||
0
|
||||
Integral:
|
||||
(p2-p1)/2*t^2 + p1*t + constant
|
||||
Integral in Horner form:
|
||||
((p2-p1)/2*t + p1)*t + constant
|
||||
*/
|
||||
|
||||
@Override
|
||||
public Vector get1stDerivative(double position) {
|
||||
if (nodes == null)
|
||||
throw new IllegalStateException("Must call setNodes first.");
|
||||
|
||||
if (position > 1)
|
||||
return null;
|
||||
|
||||
position *= nodes.size() - 1;
|
||||
|
||||
final int index1 = (int) Math.floor(position);
|
||||
|
||||
final Vector position1 = nodes.get(index1).getPosition();
|
||||
final Vector position2 = nodes.get(index1 + 1).getPosition();
|
||||
|
||||
return position2.subtract(position1);
|
||||
}
|
||||
|
||||
@Override
|
||||
public double arcLength(double positionA, double positionB) {
|
||||
if (nodes == null)
|
||||
throw new IllegalStateException("Must call setNodes first.");
|
||||
|
||||
if (positionA > positionB)
|
||||
return arcLength(positionB, positionA);
|
||||
|
||||
positionA *= nodes.size() - 1;
|
||||
positionB *= nodes.size() - 1;
|
||||
|
||||
final int indexA = (int) Math.floor(positionA);
|
||||
final double remainderA = positionA - indexA;
|
||||
|
||||
final int indexB = (int) Math.floor(positionB);
|
||||
final double remainderB = positionB - indexB;
|
||||
|
||||
return arcLengthRecursive(indexA, remainderA, indexB, remainderB);
|
||||
}
|
||||
|
||||
/**
|
||||
* Assumes a < b
|
||||
*
|
||||
* @param indexA
|
||||
* @param remainderA
|
||||
* @param indexB
|
||||
* @param remainderB
|
||||
* @return
|
||||
*/
|
||||
private double arcLengthRecursive(int indexA, double remainderA, int indexB, double remainderB) {
|
||||
switch (indexB - indexA) {
|
||||
case 0:
|
||||
return arcLengthRecursive(indexA, remainderA, remainderB);
|
||||
|
||||
case 1:
|
||||
// This case is merely a speed-up for a very common case
|
||||
return
|
||||
arcLengthRecursive(indexA, remainderA, 1.0) +
|
||||
arcLengthRecursive(indexB, 0.0, remainderB);
|
||||
|
||||
default:
|
||||
return
|
||||
arcLengthRecursive(indexA, remainderA, indexB - 1, 1.0) +
|
||||
arcLengthRecursive(indexB, 0.0, remainderB);
|
||||
}
|
||||
}
|
||||
|
||||
private double arcLengthRecursive(int index, double remainderA, double remainderB) {
|
||||
final Vector position1 = nodes.get(index).getPosition();
|
||||
final Vector position2 = nodes.get(index + 1).getPosition();
|
||||
|
||||
return position1.distance(position2) * (remainderB - remainderA);
|
||||
}
|
||||
|
||||
@Override
|
||||
public int getSegment(double position) {
|
||||
if (nodes == null)
|
||||
throw new IllegalStateException("Must call setNodes first.");
|
||||
|
||||
if (position > 1)
|
||||
return Integer.MAX_VALUE;
|
||||
|
||||
position *= nodes.size() - 1;
|
||||
|
||||
final int index = (int) Math.floor(position);
|
||||
return index;
|
||||
}
|
||||
}
|
||||
86
src/main/java/com/sk89q/worldedit/interpolation/Node.java
Normal file
86
src/main/java/com/sk89q/worldedit/interpolation/Node.java
Normal file
@ -0,0 +1,86 @@
|
||||
// $Id$
|
||||
/*
|
||||
* WorldEditLibrary
|
||||
* Copyright (C) 2010 sk89q <http://www.sk89q.com> and contributors
|
||||
*
|
||||
* This program is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
package com.sk89q.worldedit.interpolation;
|
||||
|
||||
import com.sk89q.worldedit.Vector;
|
||||
|
||||
/**
|
||||
* Represents a node for interpolation.<br />
|
||||
* The {@link #tension}, {@link #bias} and {@link #continuity} fields
|
||||
* are parameters for the Kochanek-Bartels interpolation algorithm.
|
||||
*
|
||||
* @author TomyLobo
|
||||
*
|
||||
*/
|
||||
public class Node {
|
||||
private Vector position;
|
||||
|
||||
private double tension;
|
||||
private double bias;
|
||||
private double continuity;
|
||||
|
||||
public Node() {
|
||||
this(new Vector(0, 0, 0));
|
||||
}
|
||||
|
||||
public Node(Node other) {
|
||||
this.position = other.position;
|
||||
|
||||
this.tension = other.tension;
|
||||
this.bias = other.bias;
|
||||
this.continuity = other.continuity;
|
||||
}
|
||||
|
||||
public Node(Vector position) {
|
||||
this.position = position;
|
||||
}
|
||||
|
||||
|
||||
public Vector getPosition() {
|
||||
return position;
|
||||
}
|
||||
|
||||
public void setPosition(Vector position) {
|
||||
this.position = position;
|
||||
}
|
||||
|
||||
public double getTension() {
|
||||
return tension;
|
||||
}
|
||||
|
||||
public void setTension(double tension) {
|
||||
this.tension = tension;
|
||||
}
|
||||
|
||||
public double getBias() {
|
||||
return bias;
|
||||
}
|
||||
|
||||
public void setBias(double bias) {
|
||||
this.bias = bias;
|
||||
}
|
||||
|
||||
public double getContinuity() {
|
||||
return continuity;
|
||||
}
|
||||
|
||||
public void setContinuity(double continuity) {
|
||||
this.continuity = continuity;
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,148 @@
|
||||
// $Id$
|
||||
/*
|
||||
* WorldEditLibrary
|
||||
* Copyright (C) 2010 sk89q <http://www.sk89q.com> and contributors
|
||||
*
|
||||
* This program is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
package com.sk89q.worldedit.interpolation;
|
||||
|
||||
import java.util.List;
|
||||
import java.util.Map.Entry;
|
||||
import java.util.TreeMap;
|
||||
|
||||
import com.sk89q.worldedit.Vector;
|
||||
|
||||
/**
|
||||
* Reparametrises another interpolation function by arc length.<br />
|
||||
* This is done so entities travel at roughly the same speed across
|
||||
* the whole route.
|
||||
*
|
||||
* @author TomyLobo
|
||||
*
|
||||
*/
|
||||
public class ReparametrisingInterpolation implements Interpolation {
|
||||
private final Interpolation baseInterpolation;
|
||||
private double totalArcLength;
|
||||
private final TreeMap<Double, Double> cache = new TreeMap<Double, Double>();
|
||||
|
||||
public ReparametrisingInterpolation(Interpolation baseInterpolation) {
|
||||
this.baseInterpolation = baseInterpolation;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void setNodes(List<Node> nodes) {
|
||||
baseInterpolation.setNodes(nodes);
|
||||
cache.clear();
|
||||
cache.put(0.0, 0.0);
|
||||
cache.put(totalArcLength = baseInterpolation.arcLength(0.0, 1.0), 1.0);
|
||||
}
|
||||
|
||||
public Interpolation getBaseInterpolation() {
|
||||
return baseInterpolation;
|
||||
}
|
||||
|
||||
@Override
|
||||
public Vector getPosition(double position) {
|
||||
if (position > 1)
|
||||
return null;
|
||||
|
||||
return baseInterpolation.getPosition(arcToParameter(position));
|
||||
}
|
||||
|
||||
@Override
|
||||
public Vector get1stDerivative(double position) {
|
||||
if (position > 1)
|
||||
return null;
|
||||
|
||||
return baseInterpolation.get1stDerivative(arcToParameter(position)).normalize().multiply(totalArcLength);
|
||||
}
|
||||
|
||||
@Override
|
||||
public double arcLength(double positionA, double positionB) {
|
||||
return baseInterpolation.arcLength(arcToParameter(positionA), arcToParameter(positionB));
|
||||
}
|
||||
|
||||
private double arcToParameter(double arc) {
|
||||
if (cache.isEmpty())
|
||||
throw new IllegalStateException("Must call setNodes first.");
|
||||
|
||||
if (arc > 1) arc = 1;
|
||||
arc *= totalArcLength;
|
||||
|
||||
Entry<Double, Double> floorEntry = cache.floorEntry(arc);
|
||||
final double leftArc = floorEntry.getKey();
|
||||
final double leftParameter = floorEntry.getValue();
|
||||
|
||||
if (leftArc == arc) {
|
||||
return leftParameter;
|
||||
}
|
||||
|
||||
Entry<Double, Double> ceilingEntry = cache.ceilingEntry(arc);
|
||||
if (ceilingEntry == null) {
|
||||
System.out.println("Error in arcToParameter: no ceiling entry for "+arc+" found!");
|
||||
return 0;
|
||||
}
|
||||
final double rightArc = ceilingEntry.getKey();
|
||||
final double rightParameter = ceilingEntry.getValue();
|
||||
|
||||
if (rightArc == arc) {
|
||||
return rightParameter;
|
||||
}
|
||||
|
||||
return evaluate(arc, leftArc, leftParameter, rightArc, rightParameter);
|
||||
}
|
||||
|
||||
private double evaluate(double arc, double leftArc, double leftParameter, double rightArc, double rightParameter) {
|
||||
double midParameter = 0;
|
||||
for (int i = 0; i < 10; ++i) {
|
||||
midParameter = (leftParameter + rightParameter) * 0.5;
|
||||
//final double midArc = leftArc + baseInterpolation.arcLength(leftParameter, midParameter);
|
||||
final double midArc = baseInterpolation.arcLength(0, midParameter);
|
||||
cache.put(midArc, midParameter);
|
||||
|
||||
if (midArc < leftArc) {
|
||||
return leftParameter;
|
||||
}
|
||||
|
||||
if (midArc > rightArc) {
|
||||
return rightParameter;
|
||||
}
|
||||
|
||||
if (Math.abs(midArc - arc) < 0.01) {
|
||||
return midParameter;
|
||||
}
|
||||
|
||||
if (arc < midArc) {
|
||||
// search between left and mid
|
||||
rightArc = midArc;
|
||||
rightParameter = midParameter;
|
||||
}
|
||||
else {
|
||||
// search between mid and right
|
||||
leftArc = midArc;
|
||||
leftParameter = midParameter;
|
||||
}
|
||||
}
|
||||
return midParameter;
|
||||
}
|
||||
|
||||
@Override
|
||||
public int getSegment(double position) {
|
||||
if (position > 1)
|
||||
return Integer.MAX_VALUE;
|
||||
|
||||
return baseInterpolation.getSegment(arcToParameter(position));
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user