vona.math

Class RXSInterpolator

java.lang.Object

vona.math.RXSInterpolator

@ThreadSafe
public class RXSInterpolator
extends java.lang.Object

Waypoint interpolation for local-to-world rigid-body-plus-scale transforms.

Each waypoint is represented as a TXEMS (translation, exponential map rotation, scale) vector, and the rigid part is also represented as an RX. The exponential map vectors are stored separately from the RX representation and are never canonicalized, which facilitates interpolation through rotations with more than 2pi of twist.

Three waypoint transforms are always maintained: from and to are the initial and final waypoints for the current interpolation; next will be the subsequent waypoint. The user may compute interpolate interpolated transforms between from and to with the interp*() APIs, and switch to the next waypoint with shift().

The next waypoint can be adjusted incrementally and cumulatively by the xformLocal() APIs or via addToNext(double[], double, double...). Or, the values of any waypoint can be directly and absolutely set through the setWaypoint*() APIs.

The localToWorld() and worldToLocal() APIs facilitate transforming vectors by the current next transform.

LOWER and/or UPPER limits may be specified for each TXEMS component. When a limit is set the corresponding waypoint components are always clamped. Transforms interpolated between waypoints are not clamped, but will stay within bounds as long as the interpolation type is set to RXSInterpolator.InterpolationType.LINEAR_TXEM.

Synchronization Policies

This class is designed to be thread-safe with internal synchronization.

Copyright (C) 2007 Marsette A. Vona, III

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 2 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, write to the Free Software Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.

Author:

Marsette (Marty) A. Vona, III

Nested Class Summary

Nested Classes

Modifier and Type	Class and Description
static class	RXSInterpolator.InterpolationProfile interpolation profile options
static class	RXSInterpolator.InterpolationType interpolation type options

Field Summary

Fields

Modifier and Type	Field and Description
private static java.lang.String	cvsid
static double	DEF_ACCEL_FRACTION default RXSInterpolator.InterpolationProfile.TRAPEZOIDAL accel fraction
static RXSInterpolator.InterpolationProfile	DEF_INTERPOLATION_PROFILE default interpolation profile
static RXSInterpolator.InterpolationType	DEF_INTERPOLATION_TYPE default interpolation type
protected RX	fromToX computed as (rxTo)(rxFrom)^{-1} in shift()
protected RXSInterpolator.InterpolationProfile	interpolationProfile current interpolation profile
protected RXSInterpolator.InterpolationType	interpolationType current interpolation type
protected double[][]	limit current TXEMS LOWER and UPPER limits, NaN if unset
static java.lang.String[]	LIMIT_NAME limit category names
static int	LOWER limit categories
protected double[]	maxAccel Current TXEMS max accel and speed in arbitrary, but agreeing, distance and time units, or NaN if unset.
protected double[]	maxSpeed Current TXEMS max accel and speed in arbitrary, but agreeing, distance and time units, or NaN if unset.
protected double	minTrapezoidAccelFraction cached getMinTrapezoidAccelFraction() for current waypoints
protected RX	rxFrom Local-to-world waypoint RX, see class header doc for details.
protected RX	rxNext Local-to-world waypoint RX, see class header doc for details.
protected RX	rxTo Local-to-world waypoint RX, see class header doc for details.
static int	S index of scale component in a TXEMS vector
protected RX	temp2X temp storage
protected double[]	tempTXEMS temp storage
protected RX	tempX temp storage
protected double[]	txemsFrom Local-to-world waypoint TXEMS (translation, exponential map, scale) vectors, see class header doc for details.
protected double[]	txemsNext Local-to-world waypoint TXEMS (translation, exponential map, scale) vectors, see class header doc for details.
protected double[]	txemsTo Local-to-world waypoint TXEMS (translation, exponential map, scale) vectors, see class header doc for details.
static int	UPPER limit categories
protected RX[]	waypointRX local-to-world waypoint RX indexed by waypoint index.
protected double[][]	waypointTXEMS local-to-world waypoint TXEMS indexed by waypoint index.
static int	WP_FROM waypoint index
static java.lang.String[]	WP_NAME waypoint names
static int	WP_NEXT waypoint index
static int	WP_TO waypoint index

Constructor Summary

Constructors

Constructor and Description
RXSInterpolator() sets limits to NaN

Method Summary

Methods

Modifier and Type	Method and Description
protected static double[]	add(double[] sum, double[] a, double[] b, int last) sum = a+b
void	addToNext(double[] dtxem, double ds, double... c) Add a TXEM vector to the next waypoint local-to-world transform, and optionally append a scaling.
protected boolean	clampToEMLimits(int which, boolean updateRX) like clampToTXLimits(int, boolean) but applies to EM components
protected boolean	clampToLimits(int which, int i) Clamp an individual component i of waypoint which to any set limit.
protected boolean	clampToScaleLimits(int which) like clampToTXLimits(int, boolean) but applies to the scale component
protected boolean	clampToTXEMLimits(int which, boolean updateRX) like clampToTXLimits(int, boolean) but applies to all TXEM components
protected boolean	clampToTXEMSLimits(boolean updateRX) clampToTXEMSLimits(int, boolean) all waypoints
protected boolean	clampToTXEMSLimits(int which, boolean updateRX) like clampToTXLimits(int, boolean) but applies to all TXEMS components
protected boolean	clampToTXLimits(int which, boolean updateRX) Clamp the translation components of waypoint which to any set limit.
static double	computeMinTrapezoidAccelFraction(double distance, double maxAccel, double maxSpeed) Parameterized computation for getMinTrapezoidAccelFraction(int).
static double	computeMinTrapezoidDuration(double distance, double maxAccel, double maxSpeed) Parameterized computation for getMinTrapezoidDuration(int).
protected static double[]	diff(double[] diff, double[] a, double[] b, int last) diff = a-b
double[]	diffTXEMFromNext(RX dx) diffTXEMFromNext(RX, double[]), conses
double[]	diffTXEMFromNext(RX dx, double[] dtxem) Compute a differential TXEM vector which when applied in addToNext(double[], double, double...) achieves the equivalent of appending dx to the current value of rxNext.
void	dump() dump(PrintStream) to System.out
void	dump(java.io.PrintStream s) dump state
void	dumpWaypoint(java.io.PrintStream s, int which) Dump state of a particular waypoint.
RXSInterpolator.InterpolationProfile	getInterpolationProfile() get the profile for interpolations
RXSInterpolator.InterpolationType	getInterpolationType() get the type for interpolations
double	getMaxTrapezoidAccel(int which) Like getMaxTrapezoidSpeed(int) but gets max accel.
double	getMaxTrapezoidSpeed(int which) Get the maximum speed, in arbitrary distance units per time unit, for RXSInterpolator.InterpolationProfile.TRAPEZOIDAL interpolation of one DoF.
double	getMinTrapezoidAccelFraction() Similar to getMinTrapezoidDuration() but gets overall min accel fraction.
double	getMinTrapezoidAccelFraction(int which) Compute the minimum accel fraction for RXSInterpolator.InterpolationProfile.TRAPEZOIDAL interpolation of the specified DoF for the current to and from waypoints.
double	getMinTrapezoidDuration() Compute the minimum duration for RXSInterpolator.InterpolationProfile.TRAPEZOIDAL interpolation of all DoF for the current to and from waypoints.
double	getMinTrapezoidDuration(int which) Compute the minimum duration for RXSInterpolator.InterpolationProfile.TRAPEZOIDAL interpolation of the specified DoF for the current to and from waypoints.
protected double	getMinTrapezoidDurationOrAccelFraction(boolean accelFraction) Common impl of getMinTrapezoidDuration() and getMinTrapezoidAccelFraction().
RX	getWaypointRX(int which) getWaypointRX(int, RX), always conses
RX	getWaypointRX(int which, RX x) Get a copy of a waypoint transform.
double	getWaypointScale(int which) Get the scale component of a waypoint transform.
double	getWaypointTransform(int which, RX x) Get a copy of a waypoint transform, and return the waypoint scale.
double[]	getWaypointTXEM(int which, double[] txem) Get a copy of a waypoint TXEM vector.
double[]	getWaypointTXEMS(int which, double[] txems) Get a copy of a waypoint TXEMS (translation, exponential map, scale) vector.
protected static double[]	interpolate(double[] dest, double u, double[] from, double[] to, int last) dest = from*(1-u)+to*u
void	interpolateWaypoint(int which, double u) Set a waypoint to its interpolated value at u in [0.0, 1.0] from the previous waypoint according to interpolationType.
RX	interpRX(double u) covers interpRX(double, RX), conses a new RX
RX	interpRX(double u, RX x) Interpolate x according to interpolationType at u in [0.0, 1.0] between the from and to waypoints.
double	interpS(double u) Interpolate scale at u in [0.0, 1.0] between the from and to waypoints.
double[]	interpTXEM(double u) covers interpTXEM(double, double[]), conses
double[]	interpTXEM(double u, double[] txem) Interpolate txem according to interpolationType at u in [0.0, 1.0] between the from and to waypoints.
double[]	interpTXEMS(double u) covers interpTXEMS(double, double[]), conses
double[]	interpTXEMS(double u, double[] txems) Like interpTXEM(double, double[]) but also interpolates scale at component index S.
protected double[]	interpTXEMSImpl(int from, int to, double u, double[] txems, int last) Common impl of interpTXEM(double, double[]), interpTXEMS(double, double[]), and interpolateWaypoint(int, double).
double	interpU(double u) Map u in [0.0, 1.0] according to current interpolationProfile, interpolationType, and minTrapezoidAccelFraction.
static double	interpU(double u, RXSInterpolator.InterpolationProfile profile, double accelFraction) Parameterized computation for interpU(double).
double[]	localToWorld(boolean doScale, boolean doRotate, boolean doTranslate, double... v) Covers localToWorld(boolean, boolean, boolean, int, double...), starts at 0.
double[]	localToWorld(boolean doScale, boolean doRotate, boolean doTranslate, int start, double... v) Transform a 3D vector v from local frame to world frame using the next waypoint transform and scale.
double[]	localToWorld(double... v) Covers localToWorld(boolean, boolean, boolean, int, double...), always does all three parts of the transformation and starts at index 0.
double[]	localToWorld(int start, double... v) Covers localToWorld(boolean, boolean, boolean, int, double...), always does all three parts of the transformation.
protected boolean	relativeEquals(double[] a, double[] b, int last) Check that each component of a VonaMath.relativeEquals(double, double, double) the corresponding component of b.
void	rotateLocal(double dt, double... d) Covers rotateLocal(double, double[], double[]), rotation axis is always through local frame origin.
void	rotateLocal(double dt, double[] p, double[] d) Covers xformLocal(RX, double, double[]) to rotate the local frame dt radians about axis d through point p, both in world frame.
void	scaleLocal(double ds) Covers scaleLocal(double, double...) with the local frame origin as the scale center.
void	scaleLocal(double ds, double... c) Covers xformLocal(RX, double, double...) to scale the local frame by factor ds about scale center c in world frame.
protected static double[]	set(double[] dest, double[] src, int last) dest = src
void	setInterpolationProfile(RXSInterpolator.InterpolationProfile profile) set the profile for interpolations
void	setInterpolationType(RXSInterpolator.InterpolationType type) set the type for interpolations
boolean	setLimit(int which, int i, double l) Set a particular limit component.
protected boolean	setLimitImpl(int which, int i, double l) Common impl of setLimits(int, double...) and setLimit(int, int, double).
boolean	setLimits(int which, double... l) Set a contiguous range of limit components.
void	setMaxTrapezoidAccel(int which, double accel) Like setMaxTrapezoidSpeed(int, double) but sets max accel.
void	setMaxTrapezoidSpeed(int which, double speed) Set the maximum speed, in arbitrary distance units per time unit, for RXSInterpolator.InterpolationProfile.TRAPEZOIDAL interpolation of one DoF.
void	setWaypointRX(int which, RX x) Set a waypoint transform.
void	setWaypointScale(int which, double scale) Set a waypoint scale.
void	setWaypointTransform(int which, RX x, double scale) setWaypointRX(int, vona.math.RX) and setWaypointScale(int, double)
void	setWaypointTXEM(int which, double... txem) Set a waypoint TXEM vector.
void	setWaypointTXEMS(int which, double... txems) Set a waypoint TXEMS (translation, exponential map, scale) vector.
boolean	shift() Advance to the next waypoint.
boolean	shiftPending() Check whether the next waypoint differs from to.
void	translateLocal(double... dl) Covers xformLocal(RX, double, double...) to translate the local frame by 3D translation vector dl in world frame.
boolean	unsetLimit(int which, int i) Unset a specfic limit component.
boolean	unsetLimits() Unset all components of LOWER and UPPER limits.
boolean	unsetLimits(int which) Unset all LOWER or UPPER limit components.
double[]	worldToLocal(boolean doScale, boolean doRotate, boolean doTranslate, double... v) Covers worldToLocal(boolean, boolean, boolean, int, double...), starts at 0.
double[]	worldToLocal(boolean doScale, boolean doRotate, boolean doTranslate, int start, double... v) Transform a 3D vector v from world frame to local frame using the next waypoint transform and scale.
double[]	worldToLocal(double... v) Covers worldToLocal(boolean, boolean, boolean, int, double...), always does all three parts of the transformation and starts at index 0.
double[]	worldToLocal(int start, double... v) Covers worldToLocal(boolean, boolean, boolean, int, double...), always does all three parts of the transformation.
void	xformLocal(RX dx) Covers xformLocal(RX, double, double...) with no change in scale.
void	xformLocal(RX dx, double ds) Covers xformLocal(RX, double, double...) with the local frame origin as the scale center.
void	xformLocal(RX dx, double ds, double... c) Append a rigid transform dx to the next waypoint local-to-world transform.

Methods inherited from class java.lang.Object

clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

Field Detail

cvsid

private static final java.lang.String cvsid

See Also:

Constant Field Values

DEF_INTERPOLATION_PROFILE

public static final RXSInterpolator.InterpolationProfile DEF_INTERPOLATION_PROFILE

default interpolation profile

interpolationProfile

protected RXSInterpolator.InterpolationProfile interpolationProfile

current interpolation profile

DEF_INTERPOLATION_TYPE

public static final RXSInterpolator.InterpolationType DEF_INTERPOLATION_TYPE

default interpolation type

interpolationType

protected RXSInterpolator.InterpolationType interpolationType

current interpolation type

WP_FROM

public static final int WP_FROM

waypoint index

See Also:

Constant Field Values

WP_TO

public static final int WP_TO

waypoint index

See Also:

Constant Field Values

WP_NEXT

public static final int WP_NEXT

waypoint index

See Also:

Constant Field Values

S

public static final int S

index of scale component in a TXEMS vector

See Also:

Constant Field Values

LOWER

public static final int LOWER

limit categories

See Also:

Constant Field Values

UPPER

public static final int UPPER

limit categories

See Also:

Constant Field Values

WP_NAME

public static final java.lang.String[] WP_NAME

waypoint names

LIMIT_NAME

public static final java.lang.String[] LIMIT_NAME

limit category names

DEF_ACCEL_FRACTION

public static final double DEF_ACCEL_FRACTION

default RXSInterpolator.InterpolationProfile.TRAPEZOIDAL accel fraction

See Also:

Constant Field Values

rxFrom

protected final RX rxFrom

Local-to-world waypoint RX, see class header doc for details.

Initialized to identity.

rxTo

protected final RX rxTo

Local-to-world waypoint RX, see class header doc for details.

Initialized to identity.

rxNext

protected final RX rxNext

Local-to-world waypoint RX, see class header doc for details.

Initialized to identity.

waypointRX

protected final RX[] waypointRX

local-to-world waypoint RX indexed by waypoint index.

txemsFrom

protected final double[] txemsFrom

Local-to-world waypoint TXEMS (translation, exponential map, scale) vectors, see class header doc for details.

The EM vectors may be non-canonical.

Initialized to identity.

txemsTo

protected final double[] txemsTo

Local-to-world waypoint TXEMS (translation, exponential map, scale) vectors, see class header doc for details.

The EM vectors may be non-canonical.

Initialized to identity.

txemsNext

protected final double[] txemsNext

Local-to-world waypoint TXEMS (translation, exponential map, scale) vectors, see class header doc for details.

The EM vectors may be non-canonical.

Initialized to identity.

waypointTXEMS

protected double[][] waypointTXEMS

local-to-world waypoint TXEMS indexed by waypoint index.

fromToX

protected final RX fromToX

computed as (rxTo)(rxFrom)^{-1} in shift()

tempX

protected final RX tempX

temp storage

temp2X

protected final RX temp2X

temp storage

tempTXEMS

protected final double[] tempTXEMS

temp storage

limit

protected final double[][] limit

current TXEMS LOWER and UPPER limits, NaN if unset

maxAccel

protected final double[] maxAccel

Current TXEMS max accel and speed in arbitrary, but agreeing, distance and time units, or NaN if unset.

See setMaxTrapezoidSpeed(int, double) and setMaxTrapezoidAccel(int, double) for more info.

maxSpeed

protected final double[] maxSpeed

Current TXEMS max accel and speed in arbitrary, but agreeing, distance and time units, or NaN if unset.

See setMaxTrapezoidSpeed(int, double) and setMaxTrapezoidAccel(int, double) for more info.

minTrapezoidAccelFraction

protected double minTrapezoidAccelFraction

cached getMinTrapezoidAccelFraction() for current waypoints

Constructor Detail

RXSInterpolator

public RXSInterpolator()

sets limits to NaN

Method Detail

setLimits

public boolean setLimits(int which,
                double... l)

Set a contiguous range of limit components.

If the limits are actually changed then all waypoints are re-clamped.

Parameters:

which - LOWER or UPPER

l - the limit values to set

Returns:

true if any change was made to any waypoint

setLimit

public boolean setLimit(int which,
               int i,
               double l)

Set a particular limit component.

If the limit is actually changed then all waypoints are re-clamped.

Parameters:

which - LOWER or UPPER

i - the component to set

l - the value to set, NaN to unset

Returns:

true if any change was made to any waypoint

Throws:

java.lang.IllegalArgumentException - if he tlimits for any given component must are not properly ordered or if the lower limit for the scale component is less than or equal to 0

setLimitImpl

protected boolean setLimitImpl(int which,
                   int i,
                   double l)

Common impl of setLimits(int, double...) and setLimit(int, int, double).

unsetLimits

public boolean unsetLimits(int which)

Unset all LOWER or UPPER limit components.

Parameters:

which - LOWER or UPPER

Returns:

true if any components were previously set

unsetLimits

public boolean unsetLimits()

Unset all components of LOWER and UPPER limits.

Returns:

true if any components were previously set

unsetLimit

public boolean unsetLimit(int which,
                 int i)

Unset a specfic limit component.

Parameters:

which - LOWER or UPPER

i - the component to unset

Returns:

true if the component was previously set

dump

public void dump(java.io.PrintStream s)

dump state

dump

public void dump()

dump(PrintStream) to System.out

dumpWaypoint

public void dumpWaypoint(java.io.PrintStream s,
                int which)

Dump state of a particular waypoint.

Parameters:

which - one of the WP_* constants

shiftPending

public boolean shiftPending()

Check whether the next waypoint differs from to.

shift

public boolean shift()

Advance to the next waypoint.

The current from waypoint is discarded, and from is assigned to to. to is then assigned to next, which retains its value.

Returns:

true iff a shift was actually pending

getMaxTrapezoidAccel

public double getMaxTrapezoidAccel(int which)

Like getMaxTrapezoidSpeed(int) but gets max accel.

setMaxTrapezoidAccel

public void setMaxTrapezoidAccel(int which,
                        double accel)

Like setMaxTrapezoidSpeed(int, double) but sets max accel.

getMaxTrapezoidSpeed

public double getMaxTrapezoidSpeed(int which)

Get the maximum speed, in arbitrary distance units per time unit, for RXSInterpolator.InterpolationProfile.TRAPEZOIDAL interpolation of one DoF.

setMaxTrapezoidSpeed

public void setMaxTrapezoidSpeed(int which,
                        double speed)

Set the maximum speed, in arbitrary distance units per time unit, for RXSInterpolator.InterpolationProfile.TRAPEZOIDAL interpolation of one DoF.

If either the max speed or accel for a DoF is set NaN then the trapezoid profile is computed only based on DEF_ACCEL_FRACTION.

Parameters:

speed - the speed limit, NaN to set undefined, +inf to set unlimited

Throws:

java.lang.IllegalArgumentException - if speed is not positive

getMinTrapezoidDuration

public double getMinTrapezoidDuration(int which)

Compute the minimum duration for RXSInterpolator.InterpolationProfile.TRAPEZOIDAL interpolation of the specified DoF for the current to and from waypoints.

Time units are whatever is used for the corresponding maxAccel and maxSpeed; if either of those is unset (NaN) then the trapezoid duration is considered arbitrary and this method returns NaN.

The current implementation assumes an interpolation type of RXSInterpolator.InterpolationType.LINEAR_TXEM.

See also getMinTrapezoidDuration().

Calls computeMinTrapezoidDuration(double, double, double) to do the work.

computeMinTrapezoidDuration

public static double computeMinTrapezoidDuration(double distance,
                                 double maxAccel,
                                 double maxSpeed)

Parameterized computation for getMinTrapezoidDuration(int).

getMinTrapezoidAccelFraction

public double getMinTrapezoidAccelFraction(int which)

Compute the minimum accel fraction for RXSInterpolator.InterpolationProfile.TRAPEZOIDAL interpolation of the specified DoF for the current to and from waypoints.

If either the corresponding maxAccel or maxSpeed is unset (NaN) then this method returns DEF_ACCEL_FRACTION.

The current implementation assumes an interpolation type of RXSInterpolator.InterpolationType.LINEAR_TXEM.

See also getMinTrapezoidAccelFraction().

Calls computeMinTrapezoidAccelFraction(double, double, double) to do the work.

computeMinTrapezoidAccelFraction

public static double computeMinTrapezoidAccelFraction(double distance,
                                      double maxAccel,
                                      double maxSpeed)

Parameterized computation for getMinTrapezoidAccelFraction(int).

getMinTrapezoidDuration

public double getMinTrapezoidDuration()

Compute the minimum duration for RXSInterpolator.InterpolationProfile.TRAPEZOIDAL interpolation of all DoF for the current to and from waypoints.

This is the minimum duration such that all defined maxAccel and maxSpeed constraints are satisfied.

The current implementation assumes an interpolation type of RXSInterpolator.InterpolationType.LINEAR_TXEM.

getMinTrapezoidAccelFraction

public double getMinTrapezoidAccelFraction()

Similar to getMinTrapezoidDuration() but gets overall min accel fraction.

getMinTrapezoidDurationOrAccelFraction

protected double getMinTrapezoidDurationOrAccelFraction(boolean accelFraction)

Common impl of getMinTrapezoidDuration() and getMinTrapezoidAccelFraction().

interpU

public double interpU(double u)

Map u in [0.0, 1.0] according to current interpolationProfile, interpolationType, and minTrapezoidAccelFraction.

This is the basis for speed profiling in all of the interpolation calls, including interpRX(double, vona.math.RX).

This calls interpU(double, InterpolationProfile, double) to do the work, with accelFraction minTrapezoidAccelFraction if interpolationType is RXSInterpolator.InterpolationType.LINEAR_TXEM, else DEF_ACCEL_FRACTION.

interpU

public static double interpU(double u,
             RXSInterpolator.InterpolationProfile profile,
             double accelFraction)

Parameterized computation for interpU(double).

setInterpolationProfile

public void setInterpolationProfile(RXSInterpolator.InterpolationProfile profile)

set the profile for interpolations

getInterpolationProfile

public RXSInterpolator.InterpolationProfile getInterpolationProfile()

get the profile for interpolations

setInterpolationType

public void setInterpolationType(RXSInterpolator.InterpolationType type)

set the type for interpolations

getInterpolationType

public RXSInterpolator.InterpolationType getInterpolationType()

get the type for interpolations

interpRX

public RX interpRX(double u,
          RX x)

Interpolate x according to interpolationType at u in [0.0, 1.0] between the from and to waypoints.

The input u domain is always linear. The appropriate mapping is internally applied according to the current interpolationProfile.

interpRX

public RX interpRX(double u)

covers interpRX(double, RX), conses a new RX

interpS

public double interpS(double u)

Interpolate scale at u in [0.0, 1.0] between the from and to waypoints.

The input u domain is always linear. The appropriate mapping is internally applied according to the current interpolationProfile.

interpTXEM

public double[] interpTXEM(double u,
                  double[] txem)

Interpolate txem according to interpolationType at u in [0.0, 1.0] between the from and to waypoints.

The computed EM vector will interpolate the waypoints and so may be non-canonical.

The input u domain is always linear. The appropriate mapping is internally applied according to the current interpolationProfile.

interpTXEM

public double[] interpTXEM(double u)

covers interpTXEM(double, double[]), conses

interpTXEMS

public double[] interpTXEMS(double u,
                   double[] txems)

Like interpTXEM(double, double[]) but also interpolates scale at component index S.

interpTXEMS

public double[] interpTXEMS(double u)

covers interpTXEMS(double, double[]), conses

interpTXEMSImpl

protected double[] interpTXEMSImpl(int from,
                       int to,
                       double u,
                       double[] txems,
                       int last)

Common impl of interpTXEM(double, double[]), interpTXEMS(double, double[]), and interpolateWaypoint(int, double).

getWaypointRX

public RX getWaypointRX(int which,
               RX x)

Get a copy of a waypoint transform.

Parameters:

which - one of the WP_* constants

getWaypointRX

public RX getWaypointRX(int which)

getWaypointRX(int, RX), always conses

getWaypointScale

public double getWaypointScale(int which)

Get the scale component of a waypoint transform.

Parameters:

which - one of the WP_* constants

getWaypointTransform

public double getWaypointTransform(int which,
                          RX x)

Get a copy of a waypoint transform, and return the waypoint scale.

Parameters:

which - one of the WP_* constants

getWaypointTXEM

public double[] getWaypointTXEM(int which,
                       double[] txem)

Get a copy of a waypoint TXEM vector.

The EM vector may be non-canonical.

Parameters:

which - one of the WP_* constants

getWaypointTXEMS

public double[] getWaypointTXEMS(int which,
                        double[] txems)

Get a copy of a waypoint TXEMS (translation, exponential map, scale) vector.

The EM vector may be non-canonical.

Parameters:

which - one of the WP_* constants

setWaypointRX

public void setWaypointRX(int which,
                 RX x)

Set a waypoint transform.

The TXEM vector for the waypoint is set to be the canonical TXEM vector for x. The waypoint scale is not modified.

Parameters:

which - one of the WP_* constants

setWaypointScale

public void setWaypointScale(int which,
                    double scale)

Set a waypoint scale.

The waypoint RX and TXEM are not modified.

Parameters:

which - one of the WP_* constants

setWaypointTransform

public void setWaypointTransform(int which,
                        RX x,
                        double scale)

setWaypointRX(int, vona.math.RX) and setWaypointScale(int, double)

setWaypointTXEM

public void setWaypointTXEM(int which,
                   double... txem)

Set a waypoint TXEM vector.

The RX for the waypoint is also set. The waypoint scale is not modified.

Parameters:

which - one of the WP_* constants

setWaypointTXEMS

public void setWaypointTXEMS(int which,
                    double... txems)

Set a waypoint TXEMS (translation, exponential map, scale) vector.

The RX for the waypoint is also set.

Parameters:

which - one of the WP_* constants

interpolateWaypoint

public void interpolateWaypoint(int which,
                       double u)

Set a waypoint to its interpolated value at u in [0.0, 1.0] from the previous waypoint according to interpolationType.

which = WP_FROM is a special case: from is set to the interpolation at u between from and to.

The input u domain is always linear. The appropriate mapping is internally applied according to the current interpolationProfile.

diffTXEMFromNext

public double[] diffTXEMFromNext(RX dx,
                        double[] dtxem)

Compute a differential TXEM vector which when applied in addToNext(double[], double, double...) achieves the equivalent of appending dx to the current value of rxNext.

This menthod does not mutate any internal state, but the result will depend on the current values in txemsNext.

diffTXEMFromNext

public double[] diffTXEMFromNext(RX dx)

diffTXEMFromNext(RX, double[]), conses

xformLocal

public void xformLocal(RX dx,
              double ds,
              double... c)

Append a rigid transform dx to the next waypoint local-to-world transform.

This is implemented as a call to addToNext(double[], double, double...) by way of diffTXEMFromNext(vona.math.RX, double[]).

Parameters:

dx - the incremental rigid transform to append, if any, else null

ds - the multiplicative change in scale to append, must be positive

c - the scale center in world frame or null to use the local frame origin

xformLocal

public void xformLocal(RX dx,
              double ds)

Covers xformLocal(RX, double, double...) with the local frame origin as the scale center.

xformLocal

public void xformLocal(RX dx)

Covers xformLocal(RX, double, double...) with no change in scale.

translateLocal

public void translateLocal(double... dl)

Covers xformLocal(RX, double, double...) to translate the local frame by 3D translation vector dl in world frame.

rotateLocal

public void rotateLocal(double dt,
               double[] p,
               double[] d)

Covers xformLocal(RX, double, double[]) to rotate the local frame dt radians about axis d through point p, both in world frame.

Parameters:

p - if non-null then a point in world frame on the rotation axis, if null then the rotation axis is taken to be through the local frame origin

rotateLocal

public void rotateLocal(double dt,
               double... d)

Covers rotateLocal(double, double[], double[]), rotation axis is always through local frame origin.

scaleLocal

public void scaleLocal(double ds,
              double... c)

Covers xformLocal(RX, double, double...) to scale the local frame by factor ds about scale center c in world frame.

Parameters:

c - if non-null then the scale center point in world frame, i.e. the unique point of coincidence between local and world frames for any non-unity scaling; if null then the scale center is the local frame origin

scaleLocal

public void scaleLocal(double ds)

Covers scaleLocal(double, double...) with the local frame origin as the scale center.

addToNext

public void addToNext(double[] dtxem,
             double ds,
             double... c)

Add a TXEM vector to the next waypoint local-to-world transform, and optionally append a scaling.

The RX for the waypoint is updated to match the new TXEM vector for the waypoint.

Note that vector addition in TXEM space is not generally the same as appending a rigid body transform corresponding to dtxem; for that use xformLocal(RX, double, double[]).

Parameters:

dtxem - the incremental TXEM to add, if any, else null

ds - the multiplicative change in scale to append, must be positive

c - the scale center in world frame or null to use the local frame origin

localToWorld

public double[] localToWorld(boolean doScale,
                    boolean doRotate,
                    boolean doTranslate,
                    int start,
                    double... v)

Transform a 3D vector v from local frame to world frame using the next waypoint transform and scale.

Parameters:

start - the start index of the vector in v

doScale - whether to apply the scaling

doRotate - whether to apply the rotation

doTranslate - whether to apply the translation

Returns:

a reference to v

localToWorld

public double[] localToWorld(boolean doScale,
                    boolean doRotate,
                    boolean doTranslate,
                    double... v)

Covers localToWorld(boolean, boolean, boolean, int, double...), starts at 0.

localToWorld

public double[] localToWorld(int start,
                    double... v)

Covers localToWorld(boolean, boolean, boolean, int, double...), always does all three parts of the transformation.

localToWorld

public double[] localToWorld(double... v)

Covers localToWorld(boolean, boolean, boolean, int, double...), always does all three parts of the transformation and starts at index 0.

worldToLocal

public double[] worldToLocal(boolean doScale,
                    boolean doRotate,
                    boolean doTranslate,
                    int start,
                    double... v)

Transform a 3D vector v from world frame to local frame using the next waypoint transform and scale.

Parameters:

start - the start index of the vector in v

doScale - whether to apply the scaling

doRotate - whether to apply the rotation

doTranslate - whether to apply the translation

Returns:

a reference to v

worldToLocal

public double[] worldToLocal(boolean doScale,
                    boolean doRotate,
                    boolean doTranslate,
                    double... v)

Covers worldToLocal(boolean, boolean, boolean, int, double...), starts at 0.

worldToLocal

public double[] worldToLocal(int start,
                    double... v)

Covers worldToLocal(boolean, boolean, boolean, int, double...), always does all three parts of the transformation.

worldToLocal

public double[] worldToLocal(double... v)

Covers worldToLocal(boolean, boolean, boolean, int, double...), always does all three parts of the transformation and starts at index 0.

clampToTXLimits

protected boolean clampToTXLimits(int which,
                      boolean updateRX)

Clamp the translation components of waypoint which to any set limit.

Parameters:

updateRX - whether to update waypointRX, if not then only waypointTXEMS is clamped

Returns:

true iff any changes were made

clampToEMLimits

protected boolean clampToEMLimits(int which,
                      boolean updateRX)

like clampToTXLimits(int, boolean) but applies to EM components

clampToScaleLimits

protected boolean clampToScaleLimits(int which)

like clampToTXLimits(int, boolean) but applies to the scale component

clampToTXEMLimits

protected boolean clampToTXEMLimits(int which,
                        boolean updateRX)

like clampToTXLimits(int, boolean) but applies to all TXEM components

clampToTXEMSLimits

protected boolean clampToTXEMSLimits(int which,
                         boolean updateRX)

like clampToTXLimits(int, boolean) but applies to all TXEMS components

clampToTXEMSLimits

protected boolean clampToTXEMSLimits(boolean updateRX)

clampToTXEMSLimits(int, boolean) all waypoints

clampToLimits

protected boolean clampToLimits(int which,
                    int i)

Clamp an individual component i of waypoint which to any set limit.

Returns:

true iff any changes were made

relativeEquals

protected boolean relativeEquals(double[] a,
                     double[] b,
                     int last)

Check that each component of a VonaMath.relativeEquals(double, double, double) the corresponding component of b.

set

protected static double[] set(double[] dest,
           double[] src,
           int last)

dest = src

diff

protected static double[] diff(double[] diff,
            double[] a,
            double[] b,
            int last)

diff = a-b

add

protected static double[] add(double[] sum,
           double[] a,
           double[] b,
           int last)

sum = a+b

interpolate

protected static double[] interpolate(double[] dest,
                   double u,
                   double[] from,
                   double[] to,
                   int last)

dest = from*(1-u)+to*u