hmi.math
Class SpatialTransform

java.lang.Object
  hmi.math.SpatialTransform

public final class SpatialTransform
extends Object

Efficient implementation of the 6x6 spatial transform matrix As defined in Rigid Body Dynamics Algorithms Roy Featherstone 2007 using a 12-element float array

Author:

Herwin van Welbergen

Field Summary

static float[]	ID The identity matrix.
static int	R
static int	SPATIALTRANSFORM_SIZE Length of SpatialTransform arrays is 12

Constructor Summary

private

SpatialTransform()

Method Summary

static boolean	epsilonEquals(float[] a, float[] b, float epsilon)
static float[]	getSpatialTransform() Returns a new float[12] array with zero components
static void	mul(float[] dest, float[] a, float[] b) Multiplies spatial transforms a and b and stores the result in dest dest is not allowed to be aliased with a or b
static void	mul(float[] dest, int dIndex, float[] a, int aIndex, float[] b, int bIndex) Multiplies spatial transforms a and b and stores the result in dest dest is not allowed to be aliased with a or b
static void	set(float[] dst, float[] src) Sets the spatial transform from another spatial transform
static void	set(float[] dst, int dIndex, float[] src, int srcIndex) Sets the spatial transform from another spatial transform
static void	setFromMat3fVec3f(float[] dst, float[] m, float[] tr) Sets the spatial transform
static void	setFromQuat4fVec3f(float[] dst, float[] q, float[] tr) Sets the spatial transform
static void	setFromQuat4fVec3f(float[] dst, int dstIndex, float[] q, int qIndex, float[] tr, int trIndex) Sets the spatial transform
static String	toString(float[] a)
static String	toString(float[] a, int aIndex)
static void	transformForce(float[] dest, float[] trans, float[] src) Transforms a spatial force vector Vec6 src, Vec6 dest, 12 float spatial transform trans.
static void	transformForce(float[] dest, int destIndex, float[] trans, int transIndex, float[] src, int srcIndex) Transforms a spatial force vector
static void	transformForceTranspose(float[] dest, float[] trans, float[] src) Transforms a spatial force vector with the transpose
static void	transformForceTranspose(float[] dest, int destIndex, float[] trans, int transIndex, float[] src, int srcIndex) Transforms a spatial force vector with the transpose
static void	transformMotion(float[] dest, float[] trans, float[] src) Transforms a vec6
static void	transformMotion(float[] dest, int destI, float[] trans, int transI, float[] src, int srcI) Transforms a spatial motion vector
static void	transformMotionTranspose(float[] dest, float[] trans, float[] src) Transforms a spatial motion vector with the transpose
static void	transformMotionTranspose(float[] dest, int destI, float[] trans, int transI, float[] src, int srcI) Transforms a spatial motion vector with the transpose
static void	transpose(float[] dst) Sets the transpose of dst in dst
static void	transpose(float[] dest, float[] a) Sets the transpose of a in dest, dest can not be aliased in a

Methods inherited from class java.lang.Object

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

Field Detail

R

public static final int R

See Also:

Constant Field Values

SPATIALTRANSFORM_SIZE

public static final int SPATIALTRANSFORM_SIZE

Length of SpatialTransform arrays is 12

See Also:

Constant Field Values

ID

public static final float[] ID

The identity matrix.

Constructor Detail

SpatialTransform

private SpatialTransform()

Method Detail

getSpatialTransform

public static float[] getSpatialTransform()

Returns a new float[12] array with zero components

setFromMat3fVec3f

public static void setFromMat3fVec3f(float[] dst,
                                     float[] m,
                                     float[] tr)

Sets the spatial transform

Parameters:

dst - the spatial transform (lenght 12 float array)

m - 3x3 rotation matrix, specified as length 9 float array

tr - 3x1 translation vector

setFromQuat4fVec3f

public static void setFromQuat4fVec3f(float[] dst,
                                      float[] q,
                                      float[] tr)

Sets the spatial transform

Parameters:

dst - the spatial transform (lenght 12 float array)

q - quaternion

tr - 3x1 translation vector

set

public static void set(float[] dst,
                       float[] src)

Sets the spatial transform from another spatial transform

set

public static void set(float[] dst,
                       int dIndex,
                       float[] src,
                       int srcIndex)

Sets the spatial transform from another spatial transform

setFromQuat4fVec3f

public static void setFromQuat4fVec3f(float[] dst,
                                      int dstIndex,
                                      float[] q,
                                      int qIndex,
                                      float[] tr,
                                      int trIndex)

Sets the spatial transform

Parameters:

dst - the spatial transform (lenght 12 float array)

q - quaternion

tr - 3x1 translation vector

transformMotion

public static void transformMotion(float[] dest,
                                   float[] trans,
                                   float[] src)

Transforms a vec6

Parameters:

trans - the spatial transform

src - the vec6 dest = spx(E,r)spv(v,v0)

transformMotion

public static void transformMotion(float[] dest,
                                   int destI,
                                   float[] trans,
                                   int transI,
                                   float[] src,
                                   int srcI)

Transforms a spatial motion vector

Parameters:

trans - the spatial transform

src - the vec6 dest = spx(E,r)spv(v,v0)

transformForce

public static void transformForce(float[] dest,
                                  float[] trans,
                                  float[] src)

Transforms a spatial force vector Vec6 src, Vec6 dest, 12 float spatial transform trans.

Parameters:

trans - the spatial transform

transformForce

public static void transformForce(float[] dest,
                                  int destIndex,
                                  float[] trans,
                                  int transIndex,
                                  float[] src,
                                  int srcIndex)

Transforms a spatial force vector

Parameters:

trans - the spatial transform

transformMotionTranspose

public static void transformMotionTranspose(float[] dest,
                                            float[] trans,
                                            float[] src)

Transforms a spatial motion vector with the transpose

Parameters:

trans - the spatial transform

src - the vec6

transformMotionTranspose

public static void transformMotionTranspose(float[] dest,
                                            int destI,
                                            float[] trans,
                                            int transI,
                                            float[] src,
                                            int srcI)

Transforms a spatial motion vector with the transpose

Parameters:

trans - the spatial transform

src - the vec6

transformForceTranspose

public static void transformForceTranspose(float[] dest,
                                           float[] trans,
                                           float[] src)

Transforms a spatial force vector with the transpose

transformForceTranspose

public static void transformForceTranspose(float[] dest,
                                           int destIndex,
                                           float[] trans,
                                           int transIndex,
                                           float[] src,
                                           int srcIndex)

Transforms a spatial force vector with the transpose

mul

public static void mul(float[] dest,
                       float[] a,
                       float[] b)

Multiplies spatial transforms a and b and stores the result in dest dest is not allowed to be aliased with a or b

mul

public static void mul(float[] dest,
                       int dIndex,
                       float[] a,
                       int aIndex,
                       float[] b,
                       int bIndex)

Multiplies spatial transforms a and b and stores the result in dest dest is not allowed to be aliased with a or b

transpose

public static void transpose(float[] dst)

Sets the transpose of dst in dst

transpose

public static void transpose(float[] dest,
                             float[] a)

Sets the transpose of a in dest, dest can not be aliased in a

epsilonEquals

public static boolean epsilonEquals(float[] a,
                                    float[] b,
                                    float epsilon)

toString

public static String toString(float[] a)

toString

public static String toString(float[] a,
                              int aIndex)