# Class UtilEngine

Provides utility methods for the physics engine.

TO DO document the Lin Bairstow polynomial solver.

TO DO find a more reliable polynomial solver; Mathematica comes up with the same answer each time, whereas for some tests, the polynomial solver here has low accuracy. Note that some tests of this polynomial solver must use a high tolerance. Actually, it is only the imaginary values that have low accuracy; when a result is real it seems to have full accuracy.

TO DO some methods/functions here are not currently being used, so either delete them or move to some other namespace.

## Properties

PROXIMITY_TEST: true = true

Proximity testing means we avoid expensive collision testing when bodies are so far apart there is no way they can collide. For debugging, you can turn off the proximity testing and then collision testing always happens even when objects are far apart.

debugEngine2D: any = null

Contains the most recently created RigidBodySim, for debugging only. Provides a shortcut to make lines and circles from anywhere in the engine2D code.

## Methods

• Returns a new matrix which is the square matrix A with column b appended.

#### Returns Float64Array[]

a new matrix which is the square matrix A with column b appended.

• Checks that all numbers in an array are numbers, not NaN.

#### Returns number

• Returns distance from the point `p2` to the line formed by erecting a normal at point `p1`.

``````Let the slope of the normal be k
The line is y - p1y = k (x - p1x)
The line of the normal through p2 is y - p2y = (-1/k)(x - p2x)
Solve these two equations to get the intersection point q
qx = (-p1y + p2y + k p1x + p2x/k ) / (1/k + k)
qy = p1y + k (qx - p1x)
``````

#### Parameters

• ##### p1: Vector

the point the normal passes thru

• ##### n: Vector

the normal at point `p1`

• ##### p2: Vector

the point of interest

#### Returns number

distance from the point `p2` to the line formed by erecting a normal at point `p1`

• Returns a minimum point of the given objective function, which is a function of two values. This method uses the Nelder-Mead Downhill Simplex algorithm. The two values can be considered as a point in a 2D plane, and the objective function is then a surface above that 2D plane. This method seeks the low point on that surface, starting from the given starting point.

A simplex in 2 dimensions is a triangle. The algorithm looks at the value of the function at the three Vertexes of a triangle and moves the worst vertex to a new better location.

There is debug code in the methods used to find nearest point between two OvalEdge. Includes a method `printFunction` that prints a table of values of the distance function over a grid of points. This table can then be displayed in Mathematica, using commands like:

``````a = Import['/Users/erikn/Desktop/test4.txt', 'Table'];
ListContourPlot[a, Contours -> 12, DataRange -> {{-2.6015, -2.6005}, {1.508, 1.509}}]
``````

where the DataRange is the range of input values to the function.

This debug code was used to determine why the Downhill Simplex algorithm was getting stuck or not finding the correct minimum occasionally when the ovals are deeply overlapping. It was due to several local minima appearing in the distance function when the ovals are deeply overlapping.

Added code for detecting this situation: we look for either of the following conditions:

1. the points of the simplex triangle become colinear or
2. the result value that is found is not close to zero.

Added code in the calling method for picking random starting points when this failure takes place. While the random starting points does result in valid solutions, the problem is that it can find any of several (4 or more) valid points in the space. Ultimately I decided its better to not give an answer at all in this situation, but instead indicate that the algorithm has failed.

Failure is OK here because it happens only when the ovals are deeply interpenetrating; when there is only shallow interpenetration, the algorithm works fine. And success with the shallow penetration case is all that is really needed for edge/edge collision detection, because vertex/edge collisions can catch the deeper penetrations. As the collision binary search process gets close to the time of collision, the penetration will become very shallow, and then the edge/edge calculation is valid.

UPDATE: turned off the test for value reaching zero; now successful completion only depends on the distance between points becoming small.

#### Parameters

• ##### p: MutableVector[]

the starting 3 points for the search

• ##### f: ((v) => number)

the objective function to minimize, a function of two values contained in the GenericVector.

• (v): number

#### Returns number

• ##### tolerance: number

the search ends when the simplex edges are smaller than this value

• ##### info: number[]

an array of a two ints, returns the number of iterations taken in `info[0]`, and whether the algorithm was successful in `info[1]` where 1 means failure, 0 means success.

#### Returns Vector

the two values where the minimum was found

• Returns array formatted as string, showing index number and value of each element.

#### Parameters

• ##### r: number[]

the array to print

• ##### `Optional`opt_start: number

index of first item to print

• ##### `Optional`opt_n: number

number of items to print

• ##### `Optional`opt_nf: ((x) => string)

number format function to use

• (x): string

#### Returns string

the array formatted as a string

• Returns point of intersection if the two line segments intersect, otherwise returns `null`. The first line is between points 1 and 2, the second line is between points 3 and 4.

May 27 2013: made parallel_tol smaller (1E-16 instead of 1E-10). This fixes a problem that showed up with Sumo game or RigidBodySim (without ContactSim) where dragging a block into the left wall would eventually fall thru the wall.

Oct 21 2016: add tolerance at endpoints. This fixes a problem where objects with acute angled corners are sliding on the floor, and their acute corners collide. Due to floating point errors, we can miss finding an intersection in that case. Adding a small tolerance extends the edge slightly and lets us find an intersection. See `test/StraightStraightTest#acute_corners_setup`.

#### Parameters

• ##### p1: Vector

point 1, start of first line

• ##### p2: Vector

point 2, end of first line

• ##### p3: Vector

point 3, start of second line

• ##### p4: Vector

point 4, end of second line

#### Returns null | Vector

the intersection point, or `null` if the line segments do not intersect

• Returns true if the given upper triangular matrix is singular NOTE THIS IS WRONG.

Estimate condition number of the matrix. Gill, Murray, Wright p. 152 give an estimation for condition of an upper triangular matrix `U`

``````cond(U) >= max_i |u_ii | / min_i | u_ii |
``````

So, we find the max and min element on the diagonal, and itâ€™s their ratio.

NOTE: THIS IS WRONG. The test is when `A` is factored into `L U` form, and `L` has all 1's on its diagonal.

#### Parameters

• ##### Acc: Float64Array[]

an `n` by `n` matrix

• ##### n: number

number of rows

• ##### nrow: number[]

the row permutation vector

• ##### tolerance: number

small positive number, anything smaller is regarded as zero

#### Returns boolean

true if the given upper triangular matrix is singular

• Returns `A . x - b`, matrix A multiplied by a vector `x`, and optionally subtracts a second vector `b`.

#### Parameters

matrix

vector

• ##### `Optional`opt_b: number[]

optional vector to subtrct

#### Returns number[]

result is `A . x - b`

• Solves `A x = b`, using Gaussian Elimination with Scaled Partial Pivoting, where `A` is an `n x n+1` augmented matrix with last column being `b`. This matrix solver can find solutions even when the matrix is singular, as long as the b vector is in the column space of the `A` matrix. Based on algorithm 6.3 in Numerical Analysis, 6th edition by Burden & Faires, page 371. If b is in the column space of `A`, then we should be able to solve `A x = b` even if `A` is a singular matrix. Singular means that `A` is not invertible, that some rows of `A` are linear combinations of other rows of `A`, that the null space of `A` is not empty.

New on Aug 24 2011: when we find a zero on diagonal, swap columns. This ensures that all the entries below the diagonal are zero, until the zero rows are reached.

Note that doing the scaling actually made the pile_20_random_blocks test go faster (13.5 seconds down to 13.0 seconds) in spite of the extra divide.

WARNING `A` matrix is modified into upper triangular format (when accessed by the row permutation vector).

TO DO the logic for back substitution can maybe be simplified now because we are guaranteed to have non-zero on diagonal until the last zero rows.

TO DO pass in ncol, so that the caller can display the matrix in correct format???

#### Parameters

• ##### A: Float64Array[]

an `n` by `n+1` matrix (or bigger), with the last column containing the `b` vector, it is modified into upper triangular format as a result of Gaussian Elimination.

• ##### x: number[]

vector of length `n` where the result from solving `A x = b` will be stored

• ##### zero_tol: number

small positive number, anything smaller is regarded as zero

• ##### nrow: number[]

where the row permutation vector will be stored

#### Returns number

-1 if successful, or row number where error occurs

• Solves for `x` in the matrix problem `A x = b`.

#### Parameters

• ##### A: Float64Array[]

an n x n square matrix and is not changed.

• ##### x: number[]

vector of length `n` where the result from solving `A x = b` will be stored

• ##### b: number[]

the b-vector of equation `A x = b`

• ##### `Optional`zero_tol: number

small positive number, anything smaller is regarded as zero

#### Returns number

-1 if successful, or row number where error occurs

• Returns the maximum error in the solution of matrix equation `A . x = b`.

#### Parameters

• ##### A: Float64Array[]

an `n x n` square matrix and is not changed.

• ##### x: number[]

solution vector

• ##### b: number[]

the b-vector of equation `A x = b`

#### Returns number

the maximum value of `|A . x - b|`

#### Returns number

• Returns the index of the largest (in absolute value) entry in the vector.

#### Parameters

• ##### r: number[]

vector to examine

#### Returns number

the index of the largest entry in the vector

• Returns the absolute value of the largest (in absolute value) entry in the vector.

#### Parameters

• ##### `Optional`n: number

length of list (optional)

#### Returns number

the absolute value of the largest entry in the vector

• Returns minimum value of a vector.

#### Parameters

• ##### r: number[]

the vector of interest

• ##### `Optional`n: number

length of list (optional)

#### Returns number

the minimum value of the vector

• Returns a distance such that when points are within this distance then they are considered to be the same contact point.

For example, consider a circular edge in contact with a straight edge. If the circular edge has several 'midpoint Vertexes' then we can potentially find several contacts clustered near the true point of contact. So you would have a contact between the curved edge and the straight edge, but also 1 or more contacts between the straight edge and the midpoint Vertexes on the curved edge nearby.

We want to know how far apart these contacts can potentially be. It is a function of the radius of curvature and the distance tolerance used for detecting when a point is in contact.

``````For curved edges, base the nearness test on radius of curvature.
The gap between circle and line is d = r (1 - cos t)
where t = angle and t = 0 is contact point between circle and line.
cos power series:  cos z = 1 - z^2 /2! + z^4/4! - etc
For small t, use the the first two terms only, so we get
d = r (1 - (1 - t^2 / 2)) = (r/2) t^2
d = (r/2) t^2
Let tol = the contact distance tolerance.
find angle t such that the gap is greater than tol:
tol < (r/2) t^2
sqrt(2 tol/r) < t
The distance is then t r.
Except that we return twice this, because this is distance between
the true contact point and neighboring Vertexes;  if you don't
have the true contact point (because edge/edge collisions are turned off)
then you could have twice that distance for two Vertexes at the same point.
For straight edges, just take a constant distance, or a percentage of
the length of the edge.

For two curves:  Suppose we are measuring the gap d at a distance h
from the contact point (a picture would help here);  this is a point
that is h away from the contact point along the tangent line to the
two curves at the contact point.
If you make a diagram, you can convince yourself that for small angles:
h = t_1 r_1 = t_2 r_2
where t_i is the small angle that the point at h is away from the
line going thru the circle centers and the contact point.
t_1 is the angle for curve 1 with radius r_1,
t_2 is the angle for curve 2 with radius r_2.
The gap is the sum of the two gaps derived above:
d = (r_1 /2) t_1 ^2 + (r_2 /2) t_2 ^2
We have that t_2 = (r_1 / r_2) t_1, so we can write
d = (r_1 /2) t_1 ^2 + (r_2 /2) (r_1 / r_2) ^2 t_1 ^2
d = (r_1 /2) t_1 ^2 (1 + (r_1 / r_2) t_1)
So we are looking for t_1 such that the gap is greater than tol:
tol < d
This gives us a cubic in t_1.
``````

#### Parameters

• ##### r1: number

radius of first edge, negative means concave

• ##### r2: number

radius of second edge, negative means concave

• ##### distTol: number

distance tolerance

#### Returns number

a distance such that when points are within this distance then they are considered to be the same contact point.

• Returns a new n x m matrix, but with no values filled in.

#### Parameters

• ##### n: number

number of rows

• ##### `Optional`m: number

number of columns. If omitted, then make an `n x n` matrix.

#### Returns Float64Array[]

• Creates a new square n x n matrix, filling it with values from the given array.

#### Parameters

• ##### n: number

number of rows of matrix

• ##### a: number[]

array with values, must be length `n^2`

#### Returns Float64Array[]

a new square `n x n` matrix, filled with values from array `a`.

• Creates a new square `n x n` matrix, filling it with values from the given array which is from an `n x (n+1)` matrix, so we ignore the last value in each 'row' of the array.

#### Parameters

• ##### n: number

number of rows of matrix

• ##### a: number[]

array with values, must be length `n^2`

#### Returns Float64Array[]

a new square `n x n` matrix, filled with values from array `a`.

• #### Parameters

preamble

• ##### r: number[]

the array to print

• ##### `Optional`nf: ((x) => string)

number format function to use

• (x): string

#### Returns string

• ##### `Optional`opt_n: number

length of array

• #### Parameters

preamble

• ##### r: Float64Array

the array to print

• ##### `Optional`nf: ((x) => string)

number format function to use

• (x): string

#### Returns string

• ##### `Optional`opt_n: number

length of array

• #### Parameters

preamble

• ##### r: Float64Array

the array to print

• ##### delim: string

delimiter between entries

#### Returns void

• Prints set of indices where the array has a boolean value of 'true'.

#### Parameters

preamble

• ##### r: boolean[]

array to print

• ##### n: number

length of array

• #### Parameters

preamble

• ##### r: Float64Array

the array to print

• ##### ncol: number[]

the column permutation vector

• ##### `Optional`nf: ((x) => string)

number format function to use

• (x): string

#### Returns string

• ##### `Optional`opt_n: number

length of array

• #### Parameters

preamble

• ##### m: Float64Array[]

the matrix to print

• ##### `Optional`nf: ((x) => string)

number format function to use

• (x): string

#### Returns string

• ##### `Optional`n: number

number of rows of matrix

#### Returns void

• Prints the matrix using the given permutation vector to rearrange the rows, and uses a special format that displays the matrix with aligned columns, and blank space for zeros.

#### Parameters

preamble

• ##### m: Float64Array[]

the matrix to print

• ##### nrow: number[]

the row permutation vector

• #### Parameters

preamble

• ##### m: Float64Array[]

the matrix to print

• ##### nrow: number[]

the row permutation vector

• ##### ncol: number[]

the column permutation vector

• ##### `Optional`nf: ((x) => string)

number format function to use

• (x): string

#### Returns string

• ##### `Optional`n: number

number of rows of matrix

x squared

#### Returns number[]

new vector containing sum: `v + u`

• Returns the length of the vector: the sqrt of sum of squared components.

#### Parameters

• ##### p: number[]

the vector of interest

#### Returns number

the length of the vector

Generated using TypeDoc