LinAlg

java.lang.Object
- edu.umich.eecs.april.jmat.LinAlg

```
public final class LinAlg
extends java.lang.Object
```
Simple linear algebra and geometry functions designed to operate on doubles[]. Conventions: We assume points are projected via right multiplication. E.g., p' = Mp. Roll: rotation around X. Pitch: rotation around Y. Yaw: rotation around Z. Roll Pitch Yaw are evaluated in the order: roll, pitch, then yaw. I.e., rollPitchYawToMatrix(rpy) = rotateZ(rpy[2]) * rotateY(rpy[1]) * rotateX(rpy[0])

Constructor Summary

Constructors
Constructor and Description

LinAlg()

Constructors
Constructor and Description
`LinAlg()`

Method Summary

All Methods Static Methods Concrete Methods
Modifier and Type	Method and Description
`static double[]`	`abs(double[] a)` Return absolute value of each element.
`static double[][]`	`abs(double[][] A)` Return absolute value of each element.
`static float[]`	`abs(float[] a)` Return absolute value of each element.
`static double[][]`	`add(double[][] a, double[][] b)`
`static double[][]`	`add(double[][] a, double[][] b, double[][] X)`
`static double[]`	`add(double[] a, double[] b)`
`static double[]`	`add(double[] a, double[] b, double[] r)`
`static double[][]`	`addMany(double[][] a, double[][]... bs)`
`static double[]`	`angleAxisToQuat(double[] angleAxis)`
`static double[]`	`angleAxisToQuat(double theta, double[] axis)`
`static double`	`average(double[] a)` average of elements in a vector
`static float`	`average(float[] a)` average of elements in a vector
`static double[]`	`centroid(java.util.List<double[]> points)`
`static double[][]`	`cholesky22(double[][] S)`
`static double`	`clamp(double v, double min, double max)`
`static int`	`clamp(int v, int min, int max)`
`static void`	`clear(double[] v)`
`static void`	`clear(double[][] v)`
`static void`	`clear(int[] v)`
`static double[]`	`copy(double[] a)`
`static double[][]`	`copy(double[][] a)`
`static double[][]`	`copy(double[][] a, int i0, int j0, int nrows, int ncols)` Copy the submatrix beginning at (i0,j0) with 'nrows' rows and 'ncols' columns
`static void`	`copy(double[] a, double[] r)`
`static double[]`	`copy(double[] a, int len)`
`static double[]`	`copy(double[] a, int a0, int len)`
`static float[]`	`copy(float[] a)`
`static int[]`	`copy(int[] a)`
`static double[]`	`copyDoubles(float[] a)`
`static double[]`	`copyDoubles(int[] a)`
`static float[]`	`copyFloats(double[] a)`
`static double[]`	`crossProduct(double[] a, double[] b)`
`static double[]`	`crossProduct(double[] a, double[] b, double[] r)`
`static float[]`	`crossProduct(float[] a, float[] b)`
`static float[]`	`crossProduct(float[] a, float[] b, float[] r)`
`static double`	`det(double[][] M)`
`static double`	`det22(double[][] A)`
`static double`	`det33(double[][] A)`
`static double[][]`	`diag(double[] d)`
`static double`	`distance(double[] a, double[] b)` Euclidean distance
`static double`	`distance(double[] a, double[] b, int len)` Euclidean distance using first 'len' elements
`static double`	`distance(int[] a, int[] b)`
`static double`	`dotProduct(double[] a, double[] b)`
`static boolean`	`equals(double[][] a, double[][] b, double thresh)`
`static boolean`	`equals(double[] a, double[] b, double thresh)`
`static double[]`	`fitLine(java.util.ArrayList<double[]> xyws)` Fit y = Mx + b using weighted least-squares regression.
`static double[]`	`fitPlaneNormal(java.util.ArrayList<double[]> points)` Find the best plane fit to a set of points using SVD.
`static double[][]`	`identity(int sz)` Return the identity matrix of size 'sz'
`static double[][]`	`inverse(double[][] A)`
`static double[][]`	`inverse(double[][] A, double[][] X)`
`static double`	`linearlyInterpolate(double[] xy0, double[] xy1, double x2)` Given two (x,y) pairs, find the y coordinate for a third x point.
`static double`	`magnitude(double[] a)` length of the vector
`static float`	`magnitude(float[] a)` length of the vector
`static double`	`magnitude(int[] a)` length of the vector
`static void`	`main(java.lang.String[] args)`
`static double[]`	`makePerpendicular(double[] a, double[] b)` return a vector close to a that is perpendicular to b.
`static double`	`manhattenDistance(double[] a, double[] b)` Returns manhatten distance.
`static double[]`	`matrixAB(double[][] A, double[] B)` X = A * B
`static double[][]`	`matrixAB(double[][] A, double[][] B)`
`static double[][]`	`matrixAB(double[][] A, double[][] B, double[][] X)`
`static double[]`	`matrixAB(double[][] A, double[] B, double[] X)`
`static double[]`	`matrixAB(double[] A, double[][] B)` X = A' * B
`static double[][]`	`matrixABC(double[][] A, double[][] B, double[][] C)`
`static double[][]`	`matrixABCt(double[][] A, double[][] B, double[][] C)`
`static double[][]`	`matrixABt(double[][] A, double[][] B)`
`static double[][]`	`matrixABt(double[][] A, double[][] B, double[][] X)`
`static double[]`	`matrixAtB(double[][] A, double[] B)`
`static double[][]`	`matrixAtB(double[][] A, double[][] B)`
`static double[][]`	`matrixAtB(double[][] A, double[][] B, double[][] X)`
`static double[]`	`matrixAtB(double[][] A, double[] B, double[] X)`
`static double[][]`	`matrixAtBC(double[][] A, double[][] B, double[][] C)`
`static double[]`	`matrixToQuat(double[][] R)`
`static double[]`	`matrixToQuat(Matrix R)` Convert the 3x3 rotation matrix to a quaternion.
`static double[]`	`matrixToRollPitchYaw(double[][] M)`
`static double[]`	`matrixToRollPitchYaw(Matrix M)`
`static double[]`	`matrixToXYT(double[][] M)` convert 4x4 matrix to XYT notation.
`static double[]`	`matrixToXyzrpy(double[][] M)`
`static double`	`max(double[] v)`
`static double`	`max(double[][] a)`
`static double[]`	`max(double[] a, double[] b)`
`static float`	`max(float[] v)`
`static int`	`maxIdx(double[] v)`
`static int`	`maxIdx(float[] v)`
`static double`	`maxRadius(java.util.List<double[]> points, double[] center)`
`static double`	`meanRadius(java.util.List<double[]> points, double[] center)`
`static double`	`min(double[] v)`
`static double`	`min(double[][] a)`
`static float`	`min(float[] v)`
`static int`	`min(int[] v)`
`static int`	`minIdx(double[] v)`
`static int`	`minIdx(float[] v)`
`static int`	`minIdx(int[] v)`
`static void`	`minusEquals(double[] a, double[] b)` a -= b
`static void`	`minusEquals(double[] a, double[] b, double scale)`
`static double[][]`	`multiplyMany(double[][] a, double[][]... bs)`
`static double[]`	`normalize(double[] a)`
`static float[]`	`normalize(float[] a)` Rescale so magnitude is 1.
`static void`	`normalizeEquals(double[] a)`
`static double[]`	`normalizeL1(double[] a)`
`static float[]`	`normalizeL1(float[] a)`
`static double`	`normF(double[] a)` sum of squared elements
`static double`	`normL1(double[] a)`
`static float`	`normL1(float[] a)`
`static int`	`nz(double[][] v)` Count the number of non-zero elements.
`static double[][]`	`outerProduct(double[] A, double[] B)`
`static void`	`plusEquals(double[][] a, double[][] b)`
`static void`	`plusEquals(double[] a, double[] b)` a += b
`static void`	`plusEquals(double[] a, double[] b, double scale)`
`static boolean`	`pointInsideTriangle(double[] p0, double[] p1, double[] p2, double[] q)` Given the 2D triangle (p0,p1,p2), where the points are given such that we orbit the triangle clockwise (the interior of the triangle is on the left of each segment), is point q inside the triangle?
`static boolean`	`pointLeftOf(double[] p0, double[] p1, double[] q)` Consider the directed 2D line that travels from p0 to p1. is q on the left (or on the line)?
`static boolean`	`pointsAbovePlane(java.util.ArrayList<double[]> points, double[] plane)` This function implements part of a convex hull collision test: each face of the hull is described in terms of a plane equation: Ax + By + Cz + D = 0 The sign of this equation must be configured so that the value of the expression is negative on the inside of the hull and positive on the other side.
`static int`	`pointsOnWhichSideOfPlane(java.util.ArrayList<double[]> points, double[] plane)` This function analyzes which side of a plane a set of points resides where the plane is defined by the plane equation: Ax + By + Cz + D = 0 The most common use case is for implementing part of a convex hull collision test, where each face of the hull is described in terms of a plane equation.
`static int`	`pointsOnWhichSideOfPlane(java.util.ArrayList<double[]> points, double[] normal, double[] p)` This function analyzes which side of a plane a set of points resides, where the plane is defined by its normal and a point on the plane.
`static void`	`print(byte[] v)`
`static void`	`print(double[] v)`
`static void`	`print(double[][] v)`
`static void`	`print(float[] v)`
`static void`	`print(float[][] v)`
`static void`	`print(int[] v)`
`static void`	`print(int[][] v)`
`static void`	`print(long[] v)`
`static void`	`print(long[][] v)`
`static void`	`printPattern(double[][] v)` print out the sparsity patern of v using "." and "X"
`static void`	`printTranspose(byte[] v)`
`static void`	`printTranspose(double[] v)`
`static void`	`printTranspose(double[][] v)`
`static void`	`printTranspose(float[] v)`
`static void`	`printTranspose(int[] v)`
`static void`	`printTranspose(long[] v)`
`static double[]`	`quatCompute(double[] a, double[] b)` Given two unit vectors 'a' and 'b', what is the minimum-rotation quaternion that projects 'a' onto 'b'?
`static double[]`	`quatInverse(double[] q)`
`static double[]`	`quatMultiply(double[] a, double[] b)` Compose the rotations of quaternion a and quaternion b
`static double[][]`	`quatPosToMatrix(double[] q, double[] pos)`
`static double[]`	`quatPosToXYT(double[] q, double[] pos)`
`static double[]`	`quatPosToXyzrpy(double[] q, double[] pos)`
`static double[]`	`quatRotate(double[] q, double[] v)` Rotate the vector v by the quaternion q
`static double[]`	`quatToAngleAxis(double[] q)` return vector: [rad, x, y, z]
`static double[][]`	`quatToMatrix(double[] q)`
`static double[]`	`quatToRollPitchYaw(double[] q)`
`static double[][]`	`randomMatrix(int rows, int cols, java.util.Random r)` Generate a random matrix with each value drawn from uniform[0,1]
`static double`	`rayCollisionBox(double[] xyz, double[] dir, double[] sxyz)` compute the distance from xyz (in the direction dir) until it collides with an axis-aligned box with dimensions sxyz, centered at the origin.
`static double[][]`	`readMatrix(java.io.BufferedReader ins, int rows, int cols)` Read a matrix; compatible with print method.
`static double[]`	`resize(double[] v, int newlength)`
`static double[][]`	`rollPitchYawToMatrix(double[] rpy)` Returns the equivalent 4x4 transformation matrix.
`static double[]`	`rollPitchYawToQuat(double[] rpy)`
`static double[]`	`rotate2(double[] v, double theta)`
`static double[][]`	`rotateX(double theta)` Returns the 4x4 transformation matrix due to rotate theta degrees around the X axis
`static double[][]`	`rotateY(double theta)` Returns the 4x4 transformation matrix due to rotate theta degrees around the Y axis
`static double[][]`	`rotateZ(double theta)` Returns the 4x4 transformation matrix due to rotate theta degrees around the Z axis
`static double[][]`	`scale(double sxyz)`
`static double[][]`	`scale(double[][] v, double a)`
`static double[]`	`scale(double[] v, double a)` returns v*a, allocating a new result
`static double[]`	`scale(double[] v, double[] s)`
`static double[]`	`scale(double[] v, double[] s, double[] nv)`
`static double[]`	`scale(double[] v, double a, double[] nv)`
`static double[][]`	`scale(double sx, double sy, double sz)` Returns the 4x4 transformation matrix corresponding to scaling the dimensions
`static float[]`	`scale(float[] v, double a)`
`static float[]`	`scale(float[] v, double a, float[] nv)`
`static void`	`scaleEquals(double[][] v, double a)`
`static void`	`scaleEquals(double[] v, double a)` v = v*a, storing result back into v
`static double[][]`	`select(double[][] A, int row0, int row1, int col0, int col1)` Create a new matrix from the indices (inclusive) [row0, row1] and [col0,col1]
`static double[]`	`select(double[] A, int col0, int col1)` Create a new vector from the indices (inclusive) [col0, col1]
`static double[]`	`slerp(double[] q0, double[] q1, double w)` Interpolate quaternions from q0 (w=0) to q1 (w=1).
`static double`	`sq(double v)` Returns the square of v
`static double`	`squaredDistance(double[] a, double[] b)` Squared Euclidean distance
`static double`	`squaredDistance(double[] a, double[] b, int len)` Squared Euclidean distance using first 'len' elements
`static int`	`squaredDistance(int[] a, int[] b)` Squared Euclidean distance
`static int`	`squaredDistance(int[] a, int[] b, int len)` Squared Euclidean distance using first 'len' elements
`static double[][]`	`subtract(double[][] a, double[][] b)`
`static double[][]`	`subtract(double[][] a, double[][] b, double[][] X)`
`static double[]`	`subtract(double[] a, double[] b)`
`static double[]`	`subtract(double[] a, double[] b, double[] r)`
`static void`	`svd22(double[][] A, double[] sv, double[][] U)`
`static void`	`timesEquals(double[][] a, double[][] b)`
`static double`	`trace(double[][] M)`
`static double[]`	`transform(double[][] T, double[] p)`
`static java.util.ArrayList<double[]>`	`transform(double[][] T, java.util.List<double[]> points)`
`static double[]`	`transform(double[] xyt, double[] p)` Transforms 2d or 3d points according the xytheta transform.
`static java.util.ArrayList<double[]>`	`transform(double[] T, java.util.List<double[]> points)`
`static double[]`	`transform(Matrix T, double[] p)`
`static java.util.ArrayList<double[]>`	`transform(Matrix T, java.util.List<double[]> points)`
`static double[]`	`transformInverse(double[][] T, double[] p)`
`static double[]`	`transformInverseRotateOnly(double[][] T, double[] p)`
`static double[]`	`transformPlane(double[][] T, double[] p)` Let nd be the coefficients of the plane equation Ax + By + Cz + D = 0.
`static java.util.ArrayList<double[]>`	`transformPlanes(double[][] T, java.util.ArrayList<double[]> planes)`
`static double[]`	`transformRotateOnly(double[][] T, double[] p)`
`static double[][]`	`translate(double[] xyz)` Return the 4x4 transformation matrix corresponding to translations in x, y, and z
`static double[][]`	`translate(double x, double y)`
`static double[][]`	`translate(double x, double y, double z)` Return the 4x4 transformation matrix corresponding to translations in x, y, and z
`static double[][]`	`transpose(double[][] A)`
`static double[]`	`xytInverse(double[] a)` compute the inverse of a (x,y,theta) transformation
`static double[]`	`xytInvMul31(double[] a, double[] b)` returns xytMultiply(xytInverse(a), b)
`static double[]`	`xytInvMul31(double[] a, double[] b, double[] X)` compute: X = xytMultiply(xytInverse(a), b)
`static double[]`	`xytMultiply(double[] a, double[] b)` multiply two (x,y,theta) 2D rigid-body transformations
`static double[]`	`xytMultiply(double[] a, double[] b, double[] r)` multiply two (x,y,theta) 2D rigid-body transformations
`static double[][]`	`xytToMatrix(double[] xyt)` convert x,y,theta vector to a 4x4 matrix
`static double[]`	`xyzrpyInverse(double[] xyzrpy)`
`static double[]`	`xyzrpyInvMul(double[] a, double[] b)`
`static double[]`	`xyzrpyMultiply(double[] a, double[] b)`
`static double[][]`	`xyzrpyToMatrix(double[] xyzrpy)`
`static double[][]`	`xyzrpyToMatrix2(double[] xyzrpy)`

Methods inherited from class java.lang.Object
clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

Constructor Detail
- LinAlg
```
public LinAlg()
```

Method Detail

manhattenDistance

public static double manhattenDistance(double[] a,
                                       double[] b)

Returns manhatten distance.

sq

public static final double sq(double v)

Returns the square of v

resize

public static final double[] resize(double[] v,
                                    int newlength)

squaredDistance

public static int squaredDistance(int[] a,
                                  int[] b)

Squared Euclidean distance

squaredDistance

public static int squaredDistance(int[] a,
                                  int[] b,
                                  int len)

Squared Euclidean distance using first 'len' elements

distance

public static double distance(int[] a,
                              int[] b)

squaredDistance

public static double squaredDistance(double[] a,
                                     double[] b)

Squared Euclidean distance

squaredDistance

public static double squaredDistance(double[] a,
                                     double[] b,
                                     int len)

Squared Euclidean distance using first 'len' elements

distance

public static double distance(double[] a,
                              double[] b)

Euclidean distance

distance

public static double distance(double[] a,
                              double[] b,
                              int len)

Euclidean distance using first 'len' elements

abs

public static double[][] abs(double[][] A)

Return absolute value of each element.

abs

public static double[] abs(double[] a)

Return absolute value of each element.

abs
```
public static float[] abs(float[] a)
```
Return absolute value of each element.

normF

public static double normF(double[] a)

sum of squared elements

magnitude

public static double magnitude(int[] a)

length of the vector

magnitude

public static float magnitude(float[] a)

length of the vector

magnitude

public static double magnitude(double[] a)

length of the vector

average

public static float average(float[] a)

average of elements in a vector

average

public static double average(double[] a)

average of elements in a vector

normL1

public static float normL1(float[] a)

normL1

public static double normL1(double[] a)

normalizeL1

public static float[] normalizeL1(float[] a)

normalizeL1

public static double[] normalizeL1(double[] a)

normalize

public static float[] normalize(float[] a)

Rescale so magnitude is 1.

normalize

public static double[] normalize(double[] a)

normalizeEquals

public static void normalizeEquals(double[] a)

add

public static double[] add(double[] a,
                           double[] b)

add

public static double[] add(double[] a,
                           double[] b,
                           double[] r)

addMany

public static double[][] addMany(double[][] a,
                                 double[][]... bs)

add

public static double[][] add(double[][] a,
                             double[][] b)

add

public static double[][] add(double[][] a,
                             double[][] b,
                             double[][] X)

subtract

public static double[][] subtract(double[][] a,
                                  double[][] b)

subtract

public static double[][] subtract(double[][] a,
                                  double[][] b,
                                  double[][] X)

timesEquals

public static void timesEquals(double[][] a,
                               double[][] b)

plusEquals

public static void plusEquals(double[][] a,
                              double[][] b)

inverse

public static double[][] inverse(double[][] A)

inverse

public static double[][] inverse(double[][] A,
                                 double[][] X)

min

public static double min(double[][] a)

max

public static double max(double[][] a)

max

public static double[] max(double[] a,
                           double[] b)

plusEquals

public static void plusEquals(double[] a,
                              double[] b)

a += b

minusEquals

public static void minusEquals(double[] a,
                               double[] b)

a -= b

minusEquals

public static void minusEquals(double[] a,
                               double[] b,
                               double scale)

plusEquals

public static void plusEquals(double[] a,
                              double[] b,
                              double scale)

subtract

public static double[] subtract(double[] a,
                                double[] b)

subtract

public static double[] subtract(double[] a,
                                double[] b,
                                double[] r)

crossProduct

public static double[] crossProduct(double[] a,
                                    double[] b,
                                    double[] r)

crossProduct

public static double[] crossProduct(double[] a,
                                    double[] b)

crossProduct

public static float[] crossProduct(float[] a,
                                   float[] b,
                                   float[] r)

crossProduct

public static float[] crossProduct(float[] a,
                                   float[] b)

pointLeftOf

public static final boolean pointLeftOf(double[] p0,
                                        double[] p1,
                                        double[] q)

Consider the directed 2D line that travels from p0 to p1. is q on the left (or on the line)?

pointInsideTriangle

public static final boolean pointInsideTriangle(double[] p0,
                                                double[] p1,
                                                double[] p2,
                                                double[] q)

Given the 2D triangle (p0,p1,p2), where the points are given such that we orbit the triangle clockwise (the interior of the triangle is on the left of each segment), is point q inside the triangle?

copyFloats

public static float[] copyFloats(double[] a)

copyDoubles

public static double[] copyDoubles(float[] a)

copyDoubles

public static double[] copyDoubles(int[] a)

copy

public static float[] copy(float[] a)

copy
```
public static int[] copy(int[] a)
```

copy

public static double[] copy(double[] a,
                            int len)

copy

public static double[] copy(double[] a,
                            int a0,
                            int len)

copy

public static double[] copy(double[] a)

copy

public static void copy(double[] a,
                        double[] r)

copy

public static double[][] copy(double[][] a)

copy

public static double[][] copy(double[][] a,
                              int i0,
                              int j0,
                              int nrows,
                              int ncols)

Copy the submatrix beginning at (i0,j0) with 'nrows' rows and 'ncols' columns

quatToRollPitchYaw

public static double[] quatToRollPitchYaw(double[] q)

rollPitchYawToQuat

public static double[] rollPitchYawToQuat(double[] rpy)

rollPitchYawToMatrix
```
public static double[][] rollPitchYawToMatrix(double[] rpy)
```
Returns the equivalent 4x4 transformation matrix.

scale

public static double[][] scale(double sxyz)

scale

public static double[][] scale(double sx,
                               double sy,
                               double sz)

Returns the 4x4 transformation matrix corresponding to scaling the dimensions

translate
```
public static double[][] translate(double[] xyz)
```
Return the 4x4 transformation matrix corresponding to translations in x, y, and z

translate

public static double[][] translate(double x,
                                   double y,
                                   double z)

Return the 4x4 transformation matrix corresponding to translations in x, y, and z

translate

public static double[][] translate(double x,
                                   double y)

rotateX
```
public static double[][] rotateX(double theta)
```
Returns the 4x4 transformation matrix due to rotate theta degrees around the X axis

rotateY
```
public static double[][] rotateY(double theta)
```
Returns the 4x4 transformation matrix due to rotate theta degrees around the Y axis

rotateZ
```
public static double[][] rotateZ(double theta)
```
Returns the 4x4 transformation matrix due to rotate theta degrees around the Z axis

quatMultiply

public static double[] quatMultiply(double[] a,
                                    double[] b)

Compose the rotations of quaternion a and quaternion b

quatInverse

public static double[] quatInverse(double[] q)

slerp

public static double[] slerp(double[] q0,
                             double[] q1,
                             double w)

Interpolate quaternions from q0 (w=0) to q1 (w=1).

quatRotate

public static double[] quatRotate(double[] q,
                                  double[] v)

Rotate the vector v by the quaternion q

quatCompute
```
public static double[] quatCompute(double[] a,
                                   double[] b)
```
Given two unit vectors 'a' and 'b', what is the minimum-rotation quaternion that projects 'a' onto 'b'?

matrixToXyzrpy

public static double[] matrixToXyzrpy(double[][] M)

matrixToRollPitchYaw

public static double[] matrixToRollPitchYaw(double[][] M)

matrixToRollPitchYaw

public static double[] matrixToRollPitchYaw(Matrix M)

matrixToQuat

public static double[] matrixToQuat(double[][] R)

matrixToQuat
```
public static double[] matrixToQuat(Matrix R)
```
Convert the 3x3 rotation matrix to a quaternion. (A 4x4 transformation matrix may be passed in: only the upper-left 3x3 block will be considered.)

xytInvMul31

public static final double[] xytInvMul31(double[] a,
                                         double[] b)

returns xytMultiply(xytInverse(a), b)

xytInvMul31

public static final double[] xytInvMul31(double[] a,
                                         double[] b,
                                         double[] X)

compute: X = xytMultiply(xytInverse(a), b)

xytMultiply

public static double[] xytMultiply(double[] a,
                                   double[] b)

multiply two (x,y,theta) 2D rigid-body transformations

xytMultiply

public static double[] xytMultiply(double[] a,
                                   double[] b,
                                   double[] r)

multiply two (x,y,theta) 2D rigid-body transformations

xytInverse
```
public static double[] xytInverse(double[] a)
```
compute the inverse of a (x,y,theta) transformation

xyzrpyToMatrix

public static double[][] xyzrpyToMatrix(double[] xyzrpy)

xyzrpyToMatrix2

public static double[][] xyzrpyToMatrix2(double[] xyzrpy)

xyzrpyInverse

public static double[] xyzrpyInverse(double[] xyzrpy)

xytToMatrix

public static double[][] xytToMatrix(double[] xyt)

convert x,y,theta vector to a 4x4 matrix

matrixToXYT
```
public static double[] matrixToXYT(double[][] M)
```
convert 4x4 matrix to XYT notation. Obviously, lossy.

angleAxisToQuat

public static double[] angleAxisToQuat(double[] angleAxis)

angleAxisToQuat

public static double[] angleAxisToQuat(double theta,
                                       double[] axis)

quatToAngleAxis

public static double[] quatToAngleAxis(double[] q)

return vector: [rad, x, y, z]

quatToMatrix

public static double[][] quatToMatrix(double[] q)

quatPosToMatrix

public static double[][] quatPosToMatrix(double[] q,
                                         double[] pos)

quatPosToXYT

public static double[] quatPosToXYT(double[] q,
                                    double[] pos)

quatPosToXyzrpy

public static double[] quatPosToXyzrpy(double[] q,
                                       double[] pos)

scaleEquals

public static void scaleEquals(double[] v,
                               double a)

v = v*a, storing result back into v

scale

public static float[] scale(float[] v,
                            double a)

scale

public static double[] scale(double[] v,
                             double a)

returns v*a, allocating a new result

scale

public static double[] scale(double[] v,
                             double a,
                             double[] nv)

scale

public static float[] scale(float[] v,
                            double a,
                            float[] nv)

scale

public static double[] scale(double[] v,
                             double[] s)

scale

public static double[] scale(double[] v,
                             double[] s,
                             double[] nv)

scale

public static double[][] scale(double[][] v,
                               double a)

scaleEquals

public static void scaleEquals(double[][] v,
                               double a)

printTranspose

public static void printTranspose(byte[] v)

printTranspose

public static void printTranspose(int[] v)

printTranspose

public static void printTranspose(long[] v)

printTranspose

public static void printTranspose(float[] v)

printTranspose

public static void printTranspose(double[] v)

printTranspose

public static void printTranspose(double[][] v)

print
```
public static void print(byte[] v)
```

print
```
public static void print(int[] v)
```

print
```
public static void print(long[] v)
```

print
```
public static void print(float[] v)
```

print
```
public static void print(double[] v)
```

print
```
public static void print(int[][] v)
```

print
```
public static void print(long[][] v)
```

print

public static void print(float[][] v)

print

public static void print(double[][] v)

readMatrix

public static double[][] readMatrix(java.io.BufferedReader ins,
                                    int rows,
                                    int cols)
                             throws java.io.IOException

Read a matrix; compatible with print method. Also strips out empty lines as necessary.

Throws:: java.io.IOException

printPattern
```
public static void printPattern(double[][] v)
```
print out the sparsity patern of v using "." and "X"

nz
```
public static int nz(double[][] v)
```
Count the number of non-zero elements.

makePerpendicular

public static double[] makePerpendicular(double[] a,
                                         double[] b)

return a vector close to a that is perpendicular to b.

dotProduct

public static double dotProduct(double[] a,
                                double[] b)

meanRadius

public static double meanRadius(java.util.List<double[]> points,
                                double[] center)

maxRadius

public static double maxRadius(java.util.List<double[]> points,
                               double[] center)

centroid

public static double[] centroid(java.util.List<double[]> points)

equals

public static boolean equals(double[] a,
                             double[] b,
                             double thresh)

equals

public static boolean equals(double[][] a,
                             double[][] b,
                             double thresh)

transform

public static final double[] transform(double[] xyt,
                                       double[] p)

Transforms 2d or 3d points according the xytheta transform.

transform

public static java.util.ArrayList<double[]> transform(double[] T,
                                                      java.util.List<double[]> points)

transformRotateOnly

public static final double[] transformRotateOnly(double[][] T,
                                                 double[] p)

transform

public static final double[] transform(double[][] T,
                                       double[] p)

transform

public static final double[] transform(Matrix T,
                                       double[] p)

transform

public static java.util.ArrayList<double[]> transform(Matrix T,
                                                      java.util.List<double[]> points)

transform

public static java.util.ArrayList<double[]> transform(double[][] T,
                                                      java.util.List<double[]> points)

transformInverse

public static final double[] transformInverse(double[][] T,
                                              double[] p)

transformInverseRotateOnly

public static final double[] transformInverseRotateOnly(double[][] T,
                                                        double[] p)

clamp

public static double clamp(double v,
                           double min,
                           double max)

clamp

public static int clamp(int v,
                        int min,
                        int max)

clear
```
public static void clear(int[] v)
```

clear
```
public static void clear(double[] v)
```

clear

public static void clear(double[][] v)

transpose

public static final double[][] transpose(double[][] A)

matrixABC

public static final double[][] matrixABC(double[][] A,
                                         double[][] B,
                                         double[][] C)

matrixABCt

public static final double[][] matrixABCt(double[][] A,
                                          double[][] B,
                                          double[][] C)

matrixAtBC

public static final double[][] matrixAtBC(double[][] A,
                                          double[][] B,
                                          double[][] C)

matrixAB

public static final double[][] matrixAB(double[][] A,
                                        double[][] B)

matrixAB

public static final double[][] matrixAB(double[][] A,
                                        double[][] B,
                                        double[][] X)

matrixAB

public static final double[] matrixAB(double[] A,
                                      double[][] B)

X = A' * B

matrixAB

public static final double[] matrixAB(double[][] A,
                                      double[] B)

X = A * B

matrixAB

public static final double[] matrixAB(double[][] A,
                                      double[] B,
                                      double[] X)

outerProduct

public static final double[][] outerProduct(double[] A,
                                            double[] B)

matrixAtB

public static final double[][] matrixAtB(double[][] A,
                                         double[][] B)

matrixAtB

public static final double[][] matrixAtB(double[][] A,
                                         double[][] B,
                                         double[][] X)

matrixAtB

public static final double[] matrixAtB(double[][] A,
                                       double[] B)

matrixAtB

public static final double[] matrixAtB(double[][] A,
                                       double[] B,
                                       double[] X)

matrixABt

public static final double[][] matrixABt(double[][] A,
                                         double[][] B)

matrixABt

public static final double[][] matrixABt(double[][] A,
                                         double[][] B,
                                         double[][] X)

rotate2

public static double[] rotate2(double[] v,
                               double theta)

min
```
public static int min(int[] v)
```

minIdx
```
public static int minIdx(int[] v)
```

min
```
public static double min(double[] v)
```

minIdx
```
public static int minIdx(double[] v)
```

max
```
public static double max(double[] v)
```

maxIdx
```
public static int maxIdx(double[] v)
```

min
```
public static float min(float[] v)
```

minIdx
```
public static int minIdx(float[] v)
```

max
```
public static float max(float[] v)
```

maxIdx
```
public static int maxIdx(float[] v)
```

linearlyInterpolate

public static double linearlyInterpolate(double[] xy0,
                                         double[] xy1,
                                         double x2)

Given two (x,y) pairs, find the y coordinate for a third x point.

select

public static double[][] select(double[][] A,
                                int row0,
                                int row1,
                                int col0,
                                int col1)

Create a new matrix from the indices (inclusive) [row0, row1] and [col0,col1]

select

public static double[] select(double[] A,
                              int col0,
                              int col1)

Create a new vector from the indices (inclusive) [col0, col1]

diag

public static double[][] diag(double[] d)

identity
```
public static double[][] identity(int sz)
```
Return the identity matrix of size 'sz'

randomMatrix

public static double[][] randomMatrix(int rows,
                                      int cols,
                                      java.util.Random r)

Generate a random matrix with each value drawn from uniform[0,1]

multiplyMany

public static double[][] multiplyMany(double[][] a,
                                      double[][]... bs)

main

public static void main(java.lang.String[] args)

xyzrpyInvMul

public static double[] xyzrpyInvMul(double[] a,
                                    double[] b)

xyzrpyMultiply

public static double[] xyzrpyMultiply(double[] a,
                                      double[] b)

trace

public static double trace(double[][] M)

det

public static double det(double[][] M)

fitPlaneNormal

public static double[] fitPlaneNormal(java.util.ArrayList<double[]> points)

Find the best plane fit to a set of points using SVD.

fitLine
```
public static double[] fitLine(java.util.ArrayList<double[]> xyws)
```
Fit y = Mx + b using weighted least-squares regression. Pass in a list of {x, y, weight} (weight can be omitted). Returns { slope, offset, chi2. } Specifically, we solve the overdetermined system Jx = b, where J = [ x1 1 ; x2 1; x3 1; ...], b = [ y1 ; y2; y3; ...], and x = [M ; b] (the slope and offset of the line). Each row of J and b is weighted by that observation's value of 'weight'. Weight corresponds to the standard deviation of the uncertainty (square root of the variance). The least-squares solution is x = inv(J'J) J'b The chi^2 error is: chi^2 = (Jx-b)'(Jx-b) = x'J'Jx - 2x'J'b + b'b

rayCollisionBox
```
public static double rayCollisionBox(double[] xyz,
                                     double[] dir,
                                     double[] sxyz)
```
compute the distance from xyz (in the direction dir) until it collides with an axis-aligned box with dimensions sxyz, centered at the origin. MAX_VALUE is returned if there is no collision.

transformPlane
```
public static double[] transformPlane(double[][] T,
                                      double[] p)
```
Let nd be the coefficients of the plane equation Ax + By + Cz + D = 0. If the coordinate system is rotated by T, what is the new plane equation?

transformPlanes

public static java.util.ArrayList<double[]> transformPlanes(double[][] T,
                                                            java.util.ArrayList<double[]> planes)

pointsAbovePlane
```
public static boolean pointsAbovePlane(java.util.ArrayList<double[]> points,
                                       double[] plane)
```
This function implements part of a convex hull collision test: each face of the hull is described in terms of a plane equation: Ax + By + Cz + D = 0 The sign of this equation must be configured so that the value of the expression is negative on the inside of the hull and positive on the other side. The plane and points must be in the same coordinate frame

Parameters:

points - an arraylist of 3D points (in coord frame A)

plane - vector {A, B, C, D} (also in coord frame A)

Returns:

boolean as true if all points are above plane.

See Also:

We then test each of the points; if they are all on the positive side of the plane, we return true. This is half of a convex hull collision test: for a full test, you must try this in both directions. This method is a simplified version of pointsOnWhichSideOfFace() method in order to speed up (by returning early) when checking the planes of one hull to the points of another. In these cases, we know that any negative value is a violation.

pointsOnWhichSideOfPlane
```
public static int pointsOnWhichSideOfPlane(java.util.ArrayList<double[]> points,
                                           double[] plane)
```
This function analyzes which side of a plane a set of points resides where the plane is defined by the plane equation: Ax + By + Cz + D = 0 The most common use case is for implementing part of a convex hull collision test, where each face of the hull is described in terms of a plane equation. An absence of a collision is defined as the presence of a plane such that all the points of one hull are above the plane and the points of the other hull are below this plane. First check the planes of the hull, with the simplified pointsAbovePlane() function, but the full convex hull collision test requires also checking additional planes (namely those with normals equal to the cross-product of 2 edges, one from each hull). This function is a more general formulation of the pointsAbovePlane() function, required for 6DOF hulls, and returns one of three values {-1, 0, 1} depending on what side of the face all the points are on.

Parameters:

points - an arraylist of 3D points (in coord frame A)

plane - vector {A, B, C, D} (also in coord frame A)

Returns:

{-1, 0, 1}: '1' when all points are above plane, '-1' when all points are below the plane and '0' when points are both above and below the plane.

pointsOnWhichSideOfPlane
```
public static int pointsOnWhichSideOfPlane(java.util.ArrayList<double[]> points,
                                           double[] normal,
                                           double[] p)
```
This function analyzes which side of a plane a set of points resides, where the plane is defined by its normal and a point on the plane. See @{link #pointsOnWhichSideOfPlane(ArrayList, double[])}, where the plane is defined by plane equation

Parameters:

points - an arraylist of 3D points (in coord. frame A)

normal - vector for a surface (need not be unit length)

p - is any point on the same surface as normal vector comes from (in coord. frame A)

Returns:

{-1, 0, 1}: '1' when all points are above plane, '-1' when all points are below the plane and '0' when points are both above and below the plane.

cholesky22

public static double[][] cholesky22(double[][] S)

det33

public static double det33(double[][] A)

det22

public static double det22(double[][] A)

svd22

public static void svd22(double[][] A,
                         double[] sv,
                         double[][] U)

Class LinAlg

Constructor Summary

Method Summary

Methods inherited from class java.lang.Object

Constructor Detail

LinAlg

Method Detail

manhattenDistance

sq

resize

squaredDistance

squaredDistance

distance

squaredDistance

squaredDistance

distance

distance

abs

abs

abs

normF

magnitude

magnitude

magnitude

average

average

normL1

normL1

normalizeL1

normalizeL1

normalize

normalize

normalizeEquals

add

add

addMany

add

add

subtract

subtract

timesEquals

plusEquals

inverse

inverse

min

max

max

plusEquals

minusEquals

minusEquals

plusEquals

subtract

subtract

crossProduct

crossProduct

crossProduct

crossProduct

pointLeftOf

pointInsideTriangle

copyFloats

copyDoubles

copyDoubles

copy

copy

copy

copy

copy

copy

copy

copy

quatToRollPitchYaw

rollPitchYawToQuat

rollPitchYawToMatrix

scale

scale

translate

translate

translate

rotateX

rotateY