Scale an ellipsoid about its center (#25)

compatible_clf_cbf/ellipsoid_utils.py

@@ -2,7 +2,7 @@
 The utility function for manipulating ellipsoids
 """
 
-from typing import Optional, Tuple
+from typing import List, Optional, Tuple, Union
 
 import numpy as np
 
@@ -281,3 +281,41 @@ def add_ellipsoid_contain_pts_constraint(
         linear_coeffs, np.full((pts.shape[0],), -np.inf), -constant, vars
     )
     return constraint
+
+
+def scale_ellipsoid(S: np.ndarray, b: np.ndarray, c: float, scale: float) -> float:
+    """
+    Scale the "radius" of the ellipsoid {x | x'*S*x + b'*x + c<=0} about its
+    center by a factor of `scale`.
+
+    Return: c_new The scaled ellipsoid is {x | x'*S*x + b'*x + c_new <= 0}
+    """
+    assert scale >= 0
+
+    # S and b are unchanged after scaling.
+    # The ellipsoid can be written as |Ax + A⁻ᵀb/2|² <= c−bᵀS⁻¹b/4
+    # where AᵀA=S
+    # So c_new -bᵀS⁻¹b/4= scale² * (c−bᵀS⁻¹b/4)
+    # namely c_new = scale² * c + (1-scale²)*bᵀS⁻¹b/4
+    scale_squared = scale**2
+    return scale_squared * c + (1 - scale_squared) * b.dot(np.linalg.solve(S, b)) / 4
+
+
+def in_ellipsoid(
+    S: np.ndarray, b: np.ndarray, c: float, pts: np.ndarray
+) -> Union[bool, List[bool]]:
+    """
+    Return if `pts` is/are in the ellipsoid {x | x'*S*x+b'*x+c <= 0}.
+
+    Args:
+      pts: A single point or a group of points. Each row is a point.
+    Return:
+      flag: If pts is a 1-D array, then we return a single boolean. If pts is a
+        2D array, then flag[i] indicates whether pts[i] is in the ellipsoid.
+    """
+    dim = S.shape[0]
+    if pts.shape == (dim,):
+        return pts.dot(S @ pts) + b.dot(pts) + c <= 0
+    else:
+        assert pts.shape[1] == dim
+        return (np.sum(pts * (pts @ S), axis=1) + pts @ b + c <= 0).tolist()

tests/test_ellipsoid_utils.py

@@ -274,3 +274,40 @@ def test_add_ellipsoid_contain_pts_constraint():
         len(prog.linear_constraints()) == 0
         and len(prog.linear_equality_constraints()) == 0
     )
+
+
+def test_scale_ellipsoid():
+    center = np.array([0, 1, 1.5])
+    A = np.diag(np.array([1, 2, 3]))
+    # Build an ellipsoid {A*(x+center) | |x|<=1 }
+    A_inv = np.linalg.inv(A)
+    S = A_inv.T @ A_inv
+    b = -2 * A_inv @ center
+    c = center.dot(center) - 1
+    c_scale = mut.scale_ellipsoid(S, b, c, scale=2)
+
+    assert mut.in_ellipsoid(S, b, c_scale, A @ (center + np.array([1, 0, 0])))
+    assert mut.in_ellipsoid(S, b, c_scale, A @ (center + 1.99 * np.array([0, 0, 1])))
+    assert not mut.in_ellipsoid(
+        S, b, c_scale, A @ (center + 2.01 * np.array([0, 1, 0]))
+    )
+
+
+def test_in_ellipsoid():
+    center = np.array([0, 1, 1.5])
+    A = np.diag(np.array([1, 2, 3]))
+    # Build an ellipsoid {A*(x+center) | |x|<=1 }
+    A_inv = np.linalg.inv(A)
+    S = A_inv.T @ A_inv
+    b = -2 * A_inv @ center
+    c = center.dot(center) - 1
+
+    assert mut.in_ellipsoid(S, b, c, A @ center)
+    assert mut.in_ellipsoid(S, b, c, A @ (center + np.array([0.5, 0.2, 0.3])))
+    assert not mut.in_ellipsoid(S, b, c, A @ (center + np.array([1.1, 0, 0])))
+    assert mut.in_ellipsoid(
+        S,
+        b,
+        c,
+        (center + np.array([[0.5, 0, 0.2], [1.1, 0, 0], [1.2, 0, -1]])) @ A.T,
+    ) == [True, False, False]