# Kabsch

The Kabsch Algorithm finds the optimal translation and rotation that minimizes the distance between two sets of matched points.

#### Optimal Translation

Finding the optimal rigid transformation involves finding the optimal translations and rotations.

The optimal translation is simply the offset between the averages of the two sets of points.

```
Vector3 getAverage(points){
centroid = Vector3.zero;
foreach point in points {
centroid += point;
}
centroid /= points.count;
}
optimalTranslation =
getAverage(toPoints) - getAverage(fromPoints);
```

#### Optimal Rotation

The optimal rotation requires a few more steps.

First, you must mean-center the points (that is, subtract their means from them)

```
fromPoints = fromPoints - getAverage(fromPoints);
toPoints = toPoints - getAverage(toPoints);
```

Second, you must calculate the 3x3 Cross-Covariance Matrix between them

```
covariance = [
new Vector3(0, 0, 0),
new Vector3(0, 0, 0),
new Vector3(0, 0, 0)]
for(i = 0; i < 3; i++) {
for(j = 0; j < 3; j++) {
for(k = 0; k < fromPoints.length; k++) {
covariance[i][j] +=
fromPoints[k][i] * toPoints[k][j];
}
}
}
```

And Third, you must find the polar decomposition of that matrix

```
for (iter = 0; iter < iterations; iter++) {
setBasesFromQuaternion(curQuaternion,
curXBasis, curYBasis, curZBasis);
omega = (cross(curXBasis, covariance[0]) +
cross(curYBasis, covariance[1]) +
cross(curZBasis, covariance[2])) /
abs(dot(curXBasis, covariance[0]) +
dot(curYBasis, covariance[1]) +
dot(curZBasis, covariance[2]) + 0.000000001);
w = omega.magnitude;
if (w < 0.000000001) break;
curQuaternion = angleAxis(w, omega / w) * curQuaternion;
}
optimalRotation = curQuaternion;
```

#### Shape Matching

One application of Kabsch is in Particle-based Physics Simulations. It allows you to tie multiple particles together into a single rigid-body

```
// Apply Particle Inertia and Gravity
rigidPoints *= kabsch(rigidPoints, physicsPoints);
for(i = 0; i < physicsPoints.length; i++) {
lerp(physicsPoints[i], rigidPoints[i], stiffnessAlpha);
}
// Collide particles with the Ground
```

Shape Matching has a number of unique advantages over distance constraints:

- It’s cheap for large numbers (O(n) rather than O(n^2))
- It is unconditionally stable; the configuration cannot be inverted
- It’s branchless and requires relatively few square roots
- Computation is iterative, allowing for dynamically scaling compute
- Particles otherwise simulate totally in parallel, allowing for GPU Compute
- Shockwaves travel instantaneously across the entire body of particles
- Does not require a connectivity graph; topology can change dynamically

## Comments