Static Readonly PROXIMITY_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.
Static debugContains the most recently created RigidBodySim, for debugging only. Provides a shortcut to make lines and circles from anywhere in the engine2D code.
Static addStatic checkStatic Private colinearityStatic countStatic distanceReturns 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)
distance from the point p2 to the line formed by erecting a normal
at point p1
Static findReturns 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:
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.
the starting 3 points for the search
the objective function to minimize, a function of two values contained in the GenericVector.
the search ends when the simplex edges are smaller than this value
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.
the two values where the minimum was found
Static formatReturns array formatted as string, showing index number and value of each element.
the array to print
Optional opt_start: numberindex of first item to print
Optional opt_n: numbernumber of items to print
Optional opt_nf: ((x) => string)number format function to use
the array formatted as a string
Static linesReturns 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.
the intersection point, or null if the line
segments do not intersect
Static matrixReturns 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.
an n by n matrix
number of rows
the row permutation vector
small positive number, anything smaller is regarded as zero
true if the given upper triangular matrix is singular
Static matrixStatic matrixSolves 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???
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.
vector of length n where the result from solving A x = b will be stored
small positive number, anything smaller is regarded as zero
where the row permutation vector will be stored
-1 if successful, or row number where error occurs
Static matrixSolves for x in the matrix problem A x = b.
an n x n square matrix and is not changed.
vector of length n where the result from solving A x = b will be stored
the b-vector of equation A x = b
Optional zero_tol: numbersmall positive number, anything smaller is regarded as zero
-1 if successful, or row number where error occurs
Static matrixStatic Private maxStatic maxStatic maxStatic minStatic nearnessReturns 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.
radius of first edge, negative means concave
radius of second edge, negative means concave
distance tolerance
a distance such that when points are within this distance then they are considered to be the same contact point.
Static newStatic newStatic newCreates 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.
number of rows of matrix
array with values, must be length n^2
a new square n x n matrix, filled with values from array a.
Static printStatic printStatic printStatic printStatic printpreamble
the array to print
the column permutation vector
Optional nf: ((x) => string)number format function to use
Optional opt_n: numberlength of array
Static printStatic printStatic printPrints 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.
preamble
the matrix to print
the row permutation vector
Static printpreamble
the matrix to print
the row permutation vector
the column permutation vector
Optional nf: ((x) => string)number format function to use
Optional n: numbernumber of rows of matrix
Static squareStatic Private swapStatic vectorStatic vectorGenerated using TypeDoc
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.