Constrained optimization problem using Largange multipliers: ellipsoid collision detection and response

124 Views Asked by At

This one is purely for the mathematics so the result is far less important than the method itself.

My task is to implement a fast and efficient ellipsoid collision detection and response algorithm. Having done my literature research, I realized that the method outlined by Rimon and Boyd is probably the best for what I need to implement.

Here is the problem: An ellipsoid is a three dimensional generalization of an ellipse. It is a quadratic surface defined by the equation

\begin{equation*} \frac{x^2}{a^2} + \frac{y^2}{b^2} + \frac{z^2}{c^2} = 1 \end{equation*}

In its quadratic form, it can be written as

\begin{equation*} \textbf{(x - a)}^T A \textbf{(x - a)} = 1 \end{equation*}

where A is a symmetric positive definite diagonal matrix composed of the inverse squares of the semi-axes lengths and a is the centre point vector of the ellipsoid. This ellipsoid is then represented by $\varepsilon (A,a)$

Given: If $\textbf{(x - a)}^T A \textbf{(x - a)} < 1$ for $\textbf{x} \in \mathbb{R}^3$ then this point lies within the ellipsoid. If $\textbf{(x - a)}^T A \textbf{(x - a)} > 1$ then the point lies outside of it. This is therefore a good margin function to form a minimization problem with.

Constraint : The point of collision (or as is most frequently the case, overlap) must lie on the other ellipsoid as well. For convenience, let us call the first ellipsoid $\varepsilon_1(A, a)$ and the second one $\varepsilon_2(B, b)$.

Lagrange multiplier formulation $L(\textbf{x}, \lambda) = \textbf{(x - a)}^T A \textbf{(x - a)} + \lambda \left( \textbf{(x - b)}^T B \textbf{(x - b)} - 1 \right)$

The problem is to perform constrained minimization of the above formulation. Minimum checking is simply done by inserting the real values into the margin and comparing.

\begin{equation} \nabla_{\textbf{x}, \lambda} L(\textbf{x}, \lambda) = 0 \end{equation}

If simply expanded, this turns out to be a sextic (Lord save the God!) equation which can needless to say scarcely be implemented on a computer. There is no guaranteed solution, let alone a closed form.

However, this can apparently be turned into an eigenvalue problem. Which means I could then use the power method and be done with it, although I must say I fear this because the power method is notoriously hard to plan the convergence of.

Could anybody here help me in trying to make it an eigenvalue problem?

Cheers, Nitin