us.ihmc.utilities.math.geometry
Class FrameVector

java.lang.Object
  extended by us.ihmc.utilities.math.geometry.FrameVector
All Implemented Interfaces:
java.io.Serializable, ReferenceFrameHolder

public class FrameVector
extends java.lang.Object
implements ReferenceFrameHolder, java.io.Serializable

One of the main goals of this class is to check, at runtime, that operations on vectors occur within the same Frame. This method checks for one Vector argument.

Version:
2.0
Author:
Learning Locomotion Team
See Also:
Serialized Form

Field Summary
static boolean LOUD_WARNING
           
 
Constructor Summary
FrameVector()
           
FrameVector(FramePoint framePoint)
          FrameVector Turns a FramePoint into a FrameVector.
FrameVector(FramePoint framePoint, java.lang.String name)
           
FrameVector(FrameVector frameVector)
          FrameVector A normal vector associated with a specific reference frame
FrameVector(FrameVector frameVector, java.lang.String name)
           
FrameVector(ReferenceFrame referenceFrame)
          FrameVector A normal vector associated with a specific reference frame
FrameVector(ReferenceFrame referenceFrame, double[] vector)
           
FrameVector(ReferenceFrame referenceFrame, double[] vector, java.lang.String name)
           
FrameVector(ReferenceFrame referenceFrame, double x, double y, double z)
          FrameVector A normal vector associated with a specific reference frame.
FrameVector(ReferenceFrame referenceFrame, double x, double y, double z, java.lang.String name)
           
FrameVector(ReferenceFrame referenceFrame, java.lang.String name)
           
FrameVector(ReferenceFrame referenceFrame, javax.vecmath.Tuple3d tuple)
          FrameVector A normal vector associated with a specific reference frame
FrameVector(ReferenceFrame referenceFrame, javax.vecmath.Tuple3d tuple, java.lang.String name)
           
 
Method Summary
 void add(FramePoint framePoint)
           
 void add(FrameVector frameVector)
           
 void add(FrameVector vector1, FrameVector vector2)
           
 double angle(FrameVector frameVector)
           
 void applyTransform(javax.media.j3d.Transform3D transform)
           
 FrameVector applyTransformCopy(javax.media.j3d.Transform3D transform)
           
 void changeFrame(ReferenceFrame desiredFrame)
          Changes frame of this FrameVector to the given ReferenceFrame.
 FrameVector changeFrameCopy(ReferenceFrame desiredFrame)
          Changes frame of this FrameVector to the given ReferenceFrame and returns a copy.
 void changeFrameUsingTransform(ReferenceFrame desiredFrame, javax.media.j3d.Transform3D transformToNewFrame)
          Changes frame of this FramePoint to the given ReferenceFrame, using the given Transform3D.
 void checkForNaN()
           
 void checkReferenceFrameMatch(ReferenceFrame frame)
           
 void checkReferenceFrameMatch(ReferenceFrameHolder referenceFrameHolder)
          Makes sure that the FrameVector argument has the same Frame as the current FrameVector.
 boolean containsNaN()
           
 void cross(FramePoint f1, FramePoint f2)
           
 void cross(FramePoint f1, FrameVector f2)
           
 void cross(FrameVector f1, FramePoint f2)
           
 void cross(FrameVector f1, FrameVector f2)
           
 double dot(FrameVector frameVector)
           
 boolean epsilonEquals(FramePoint framePoint, double threshold)
           
 boolean epsilonEquals(FrameVector frameVector, double threshold)
           
 double get(Direction direction)
           
 com.mathworks.jama.Matrix getMatrix()
           
 java.lang.String getName()
           
 ReferenceFrame getReferenceFrame()
          Returns this FrameVectors ReferenceFrame.
 javax.vecmath.Vector3d getVector()
          Retrieves the vector inside this FrameVector
 void getVector(javax.vecmath.Tuple3d tuple3dToPack)
           
 javax.vecmath.Vector3d getVectorCopy()
          Returns a deep copy of the vector inside this FrameVector.
 double getX()
           
 double getY()
           
 double getZ()
           
 double length()
           
 double lengthSquared()
           
 void negate()
           
 void normalize()
           
 void packMatrix(org.ejml.data.DenseMatrix64F matrix, int startRow)
           
 void printOutFrameVector(java.lang.String varaibleName)
           
 void scale(double scaleFactor)
           
 void scale(double scaleFactor, FramePoint point1)
          Sets the value of this tuple to the scalar multiplication of vertor1 (this = s * t1).
 void scale(double scaleFactor, FrameVector vector1)
          Sets the value of this tuple to the scalar multiplication of vertor1 (this = s * t1).
 void scaleAdd(double scaleFactor, FramePoint point1)
          Sets the value of this tuple to the scalar multiplication of itself and then adds tuple t1 (this = s*this + t1).
 void scaleAdd(double scaleFactor, FramePoint point1, FramePoint point2)
          Sets the value of this tuple to the scalar multiplication of tuple t1 and then adds tuple t2 (this = s*v1 + t2).
 void scaleAdd(double scaleFactor, FramePoint point1, FrameVector vector2)
          Sets the value of this tuple to the scalar multiplication of tuple t1 and then adds tuple t2 (this = s*v1 + t2).
 void scaleAdd(double scaleFactor, FrameVector vector1)
          Sets the value of this tuple to the scalar multiplication of itself and then adds tuple t1 (this = s*this + t1).
 void scaleAdd(double scaleFactor, FrameVector vector1, FramePoint point2)
          Sets the value of this tuple to the scalar multiplication of tuple t1 and then adds tuple t2 (this = s*v1 + t2).
 void scaleAdd(double scaleFactor, FrameVector vector1, FrameVector vector2)
          Sets the value of this tuple to the scalar multiplication of tuple t1 and then adds tuple t2 (this = s*v1 + t2)
 void set(Direction direction, double value)
           
 void set(double x, double y, double z)
           
 void set(FramePoint framePoint)
           
 void set(FrameVector frameVector)
           
 void set(ReferenceFrame referenceFrame, double x, double y, double z)
           
 void set(ReferenceFrame referenceFrame, javax.vecmath.Vector3d vector)
           
 void set(javax.vecmath.Vector3d vector)
           
 void setAndChangeFrame(FramePoint framePoint)
           
 void setAndChangeFrame(FrameVector frameVector)
           
 void setAndChangeFrame(ReferenceFrame referenceFrame, javax.vecmath.Vector3d vector)
           
 void setName(java.lang.String name)
           
 void setToZero(ReferenceFrame referenceFrame)
           
 void setX(double x)
           
 void setY(double y)
           
 void setZ(double z)
           
 void sub(FramePoint framePoint)
           
 void sub(FramePoint point1, FramePoint point2)
           
 void sub(FrameVector frameVector)
           
 void sub(FrameVector vector1, FrameVector vector2)
           
 double[] toArray()
           
static double[] toArray(FrameVector[] frameVectors)
           
 FrameVector2d toFrameVector2d()
          Creates a new FrameVector2d based on the x and y components of this FrameVector
 java.lang.String toString()
          toString String representation of a FrameVector (x,y,z):reference frame name
 void weightedAverage(FrameVector vector1, FrameVector vector2, double weightedAverage)
          Sets this vector to be the weighted average of the two input vectors.
 
Methods inherited from class java.lang.Object
equals, getClass, hashCode, notify, notifyAll, wait, wait, wait
 

Field Detail

LOUD_WARNING

public static boolean LOUD_WARNING
Constructor Detail

FrameVector

public FrameVector(ReferenceFrame referenceFrame,
                   javax.vecmath.Tuple3d tuple)
FrameVector A normal vector associated with a specific reference frame

Parameters:
referenceFrame - Frame
vector - Vector3d

FrameVector

public FrameVector(ReferenceFrame referenceFrame,
                   javax.vecmath.Tuple3d tuple,
                   java.lang.String name)

FrameVector

public FrameVector(ReferenceFrame referenceFrame,
                   double[] vector)

FrameVector

public FrameVector(ReferenceFrame referenceFrame,
                   double[] vector,
                   java.lang.String name)

FrameVector

public FrameVector(ReferenceFrame referenceFrame)
FrameVector A normal vector associated with a specific reference frame

Parameters:
referenceFrame - Frame

FrameVector

public FrameVector(ReferenceFrame referenceFrame,
                   java.lang.String name)

FrameVector

public FrameVector(FrameVector frameVector)
FrameVector A normal vector associated with a specific reference frame

Parameters:
referenceFrame - Frame
vector - Vector3d

FrameVector

public FrameVector(FrameVector frameVector,
                   java.lang.String name)

FrameVector

public FrameVector(FramePoint framePoint)
FrameVector Turns a FramePoint into a FrameVector.

Parameters:
referenceFrame - Frame
vector - Vector3d

FrameVector

public FrameVector(FramePoint framePoint,
                   java.lang.String name)

FrameVector

public FrameVector(ReferenceFrame referenceFrame,
                   double x,
                   double y,
                   double z)
FrameVector A normal vector associated with a specific reference frame. The x, y, and z are used to create the vector

Parameters:
referenceFrame - Frame
x - double
y - double
z - double

FrameVector

public FrameVector(ReferenceFrame referenceFrame,
                   double x,
                   double y,
                   double z,
                   java.lang.String name)

FrameVector

public FrameVector()
Method Detail

setName

public void setName(java.lang.String name)

getName

public java.lang.String getName()

changeFrameUsingTransform

public void changeFrameUsingTransform(ReferenceFrame desiredFrame,
                                      javax.media.j3d.Transform3D transformToNewFrame)
Changes frame of this FramePoint to the given ReferenceFrame, using the given Transform3D.

Parameters:
desiredFrame - ReferenceFrame to change the FramePoint into.
transformToNewFrame - Transform3D from the current frame to the new desiredFrame

getReferenceFrame

public ReferenceFrame getReferenceFrame()
Returns this FrameVectors ReferenceFrame.

Specified by:
getReferenceFrame in interface ReferenceFrameHolder
Returns:
ReferenceFrame

getVector

public javax.vecmath.Vector3d getVector()
Retrieves the vector inside this FrameVector

Returns:
Vector3d

getVector

public void getVector(javax.vecmath.Tuple3d tuple3dToPack)

getVectorCopy

public javax.vecmath.Vector3d getVectorCopy()
Returns a deep copy of the vector inside this FrameVector.

Returns:
Vector3d

changeFrameCopy

public FrameVector changeFrameCopy(ReferenceFrame desiredFrame)
Changes frame of this FrameVector to the given ReferenceFrame and returns a copy.

Specified by:
changeFrameCopy in interface ReferenceFrameHolder
Parameters:
desiredFrame - ReferenceFrame to change the FrameVector into.
Returns:
Copied FrameVector in the new reference frame.

changeFrame

public void changeFrame(ReferenceFrame desiredFrame)
Changes frame of this FrameVector to the given ReferenceFrame.

Parameters:
desiredFrame - ReferenceFrame to change the FrameVector into.

negate

public void negate()

getX

public double getX()

getY

public double getY()

getZ

public double getZ()

get

public double get(Direction direction)

set

public void set(ReferenceFrame referenceFrame,
                double x,
                double y,
                double z)

set

public void set(double x,
                double y,
                double z)

setX

public void setX(double x)

setY

public void setY(double y)

setZ

public void setZ(double z)

set

public void set(Direction direction,
                double value)

scale

public void scale(double scaleFactor)

toArray

public double[] toArray()

toFrameVector2d

public FrameVector2d toFrameVector2d()
Creates a new FrameVector2d based on the x and y components of this FrameVector


scale

public void scale(double scaleFactor,
                  FrameVector vector1)
Sets the value of this tuple to the scalar multiplication of vertor1 (this = s * t1).

Parameters:
scaleFactor - double
vector1 - FrameVector

scale

public void scale(double scaleFactor,
                  FramePoint point1)
Sets the value of this tuple to the scalar multiplication of vertor1 (this = s * t1).

Parameters:
scaleFactor - double
point1 - FramePoint

scaleAdd

public void scaleAdd(double scaleFactor,
                     FrameVector vector1,
                     FrameVector vector2)
Sets the value of this tuple to the scalar multiplication of tuple t1 and then adds tuple t2 (this = s*v1 + t2)

Parameters:
scaleFactor - double
vector1 - FrameVector
vector2 - FrameVector

scaleAdd

public void scaleAdd(double scaleFactor,
                     FrameVector vector1,
                     FramePoint point2)
Sets the value of this tuple to the scalar multiplication of tuple t1 and then adds tuple t2 (this = s*v1 + t2).

Parameters:
scaleFactor - double
vector1 - FrameVector
point2 - FramePoint

scaleAdd

public void scaleAdd(double scaleFactor,
                     FramePoint point1,
                     FrameVector vector2)
Sets the value of this tuple to the scalar multiplication of tuple t1 and then adds tuple t2 (this = s*v1 + t2).

Parameters:
scaleFactor - double
point1 - FramePoint
vector2 - FrameVector

scaleAdd

public void scaleAdd(double scaleFactor,
                     FramePoint point1,
                     FramePoint point2)
Sets the value of this tuple to the scalar multiplication of tuple t1 and then adds tuple t2 (this = s*v1 + t2).

Parameters:
scaleFactor - double
point1 - FramePoint
point2 - FramePoint

scaleAdd

public void scaleAdd(double scaleFactor,
                     FrameVector vector1)
Sets the value of this tuple to the scalar multiplication of itself and then adds tuple t1 (this = s*this + t1).

Parameters:
scaleFactor - double
vector1 - FrameVector

scaleAdd

public void scaleAdd(double scaleFactor,
                     FramePoint point1)
Sets the value of this tuple to the scalar multiplication of itself and then adds tuple t1 (this = s*this + t1).

Parameters:
scaleFactor - double
point1 - FramePoint

checkReferenceFrameMatch

public void checkReferenceFrameMatch(ReferenceFrameHolder referenceFrameHolder)
Makes sure that the FrameVector argument has the same Frame as the current FrameVector.

Specified by:
checkReferenceFrameMatch in interface ReferenceFrameHolder
Parameters:
frameVector - FrameVector
Throws:
ReferenceFrameMismatchException

checkReferenceFrameMatch

public void checkReferenceFrameMatch(ReferenceFrame frame)
                              throws ReferenceFrameMismatchException
Specified by:
checkReferenceFrameMatch in interface ReferenceFrameHolder
Throws:
ReferenceFrameMismatchException

checkForNaN

public void checkForNaN()

containsNaN

public boolean containsNaN()

add

public void add(FrameVector frameVector)

add

public void add(FramePoint framePoint)

add

public void add(FrameVector vector1,
                FrameVector vector2)

setToZero

public void setToZero(ReferenceFrame referenceFrame)

sub

public void sub(FrameVector frameVector)

sub

public void sub(FramePoint framePoint)

sub

public void sub(FramePoint point1,
                FramePoint point2)

sub

public void sub(FrameVector vector1,
                FrameVector vector2)

dot

public double dot(FrameVector frameVector)

angle

public double angle(FrameVector frameVector)

cross

public void cross(FrameVector f1,
                  FrameVector f2)

cross

public void cross(FrameVector f1,
                  FramePoint f2)

cross

public void cross(FramePoint f1,
                  FrameVector f2)

cross

public void cross(FramePoint f1,
                  FramePoint f2)

setAndChangeFrame

public void setAndChangeFrame(FrameVector frameVector)

set

public void set(FrameVector frameVector)

set

public void set(javax.vecmath.Vector3d vector)

set

public void set(ReferenceFrame referenceFrame,
                javax.vecmath.Vector3d vector)

setAndChangeFrame

public void setAndChangeFrame(FramePoint framePoint)

set

public void set(FramePoint framePoint)

normalize

public void normalize()

length

public double length()

lengthSquared

public double lengthSquared()

applyTransform

public void applyTransform(javax.media.j3d.Transform3D transform)

applyTransformCopy

public FrameVector applyTransformCopy(javax.media.j3d.Transform3D transform)

toArray

public static double[] toArray(FrameVector[] frameVectors)

weightedAverage

public void weightedAverage(FrameVector vector1,
                            FrameVector vector2,
                            double weightedAverage)
Sets this vector to be the weighted average of the two input vectors. v = (1.0 - weightedAverage) * vector1 + weightedAverage * vector2

Parameters:
vector1 - FramePoint
vector2 - FramePoint
weightedAverage - double
Throws:
ReferenceFrameMismatchException

toString

public java.lang.String toString()
toString String representation of a FrameVector (x,y,z):reference frame name

Overrides:
toString in class java.lang.Object
Returns:
String

printOutFrameVector

public void printOutFrameVector(java.lang.String varaibleName)

epsilonEquals

public boolean epsilonEquals(FramePoint framePoint,
                             double threshold)

epsilonEquals

public boolean epsilonEquals(FrameVector frameVector,
                             double threshold)

getMatrix

public com.mathworks.jama.Matrix getMatrix()

packMatrix

public void packMatrix(org.ejml.data.DenseMatrix64F matrix,
                       int startRow)

setAndChangeFrame

public void setAndChangeFrame(ReferenceFrame referenceFrame,
                              javax.vecmath.Vector3d vector)