Jekyll2022-08-22T03:40:43+00:00https://zalo.github.io/feed.xmlSublucid GeometryA place for uncommon knowledgeJohnathon Selstadmakeshifted@gmail.comEllipsoidal Mirrors2020-01-18T20:10:33+00:002020-01-18T20:10:33+00:00https://zalo.github.io/blog/ellipsoids<p><a href="https://en.wikipedia.org/wiki/Ellipsoid">Ellipsoids</a> are spheres that have been stretched about one or more axes.</p>
<p>We can construct useful ellipsoidal mirrors called <a href="https://en.wikipedia.org/wiki/Spheroid#Prolate_spheroids">“Prolate Spheroids”</a> from two special points, called the “Foci”.
<!-- Hide the Table of Contents (but keep the navigation :^) ... -->
<script type="text/javascript">
document.getElementsByClassName('toc')[0].style.display = 'none';
</script>
<!-- Load the Three.js library, assorted helpers, and the actual line fitting script code... -->
<script type="text/javascript" src="../../assets/js/three.js"></script>
<script type="text/javascript" src="../../assets/js/DragControls.js"></script>
<script type="text/javascript" src="../../assets/js/OrbitControls.js"></script>
<script type="text/javascript" src="../../assets/js/IK/Environment.js"></script>
<script type="text/javascript" src="../../assets/js/Ellipsoids/Ellipsoid.js"></script>
<script type="text/javascript" src="../../assets/js/Ellipsoids/LineDrawer.js"></script>
<script type="text/javascript" src="../../assets/js/Ellipsoids/Projector.js"></script>
<script type="text/javascript" src="../../assets/js/Ellipsoids/EllipsoidRaytracer.js" config="0" orbit="enabled"></script></p>
<div class="language-javascript highlighter-rouge"><div class="highlight"><pre class="highlight"><code><span class="nx">ellipsoid</span> <span class="o">=</span> <span class="k">new</span> <span class="nx">Sphere</span><span class="p">(</span><span class="nx">radius</span> <span class="o">=</span> <span class="mf">0.5</span><span class="p">);</span>
<span class="nx">ellipsoid</span><span class="p">.</span><span class="nx">position</span> <span class="o">=</span> <span class="p">(</span><span class="nx">focus1</span> <span class="o">+</span> <span class="nx">focus2</span><span class="p">)</span> <span class="o">/</span> <span class="mi">2</span><span class="p">;</span>
<span class="nx">ellipsoid</span><span class="p">.</span><span class="nx">rotation</span> <span class="o">=</span> <span class="nx">orientZFromTo</span><span class="p">(</span><span class="nx">focus1</span><span class="p">,</span> <span class="nx">focus2</span><span class="p">);</span>
<span class="nx">majorAxis</span> <span class="o">=</span> <span class="nx">sqrt</span><span class="p">(</span><span class="nx">pow</span><span class="p">(</span><span class="nx">interFociDistance</span> <span class="o">/</span> <span class="mi">2</span><span class="p">,</span> <span class="mi">2</span><span class="p">)</span> <span class="o">+</span>
<span class="nx">pow</span><span class="p">(</span><span class="nx">minorAxis</span> <span class="o">/</span> <span class="mi">2</span><span class="p">,</span> <span class="mi">2</span><span class="p">))</span> <span class="o">*</span> <span class="mi">2</span><span class="p">;</span>
<span class="nx">ellipsoid</span><span class="p">.</span><span class="nx">scale</span> <span class="o">=</span> <span class="nx">Vec3</span><span class="p">(</span><span class="nx">minorAxis</span><span class="p">,</span> <span class="nx">minorAxis</span><span class="p">,</span> <span class="nx">majorAxis</span> <span class="p">);</span>
</code></pre></div></div>
<h4 id="raytracing-ellipsoids">Raytracing Ellipsoids</h4>
<p>Ellipsoids have some very special <em>optical</em> properties. So it’s useful to be able to raytrace against them quickly to determine how light will interact with them.</p>
<p>Conceptually, the simplest method is to transform both the ray origin and ray direction into the stretched coordinate space of the sphere. This allows one to raytrace against the sphere directly. After that, simply transform that hit point (and normal) out of the stretched coordinate space to use it.</p>
<div class="togglebox">
<input id="toggle2" type="checkbox" name="toggle" />
<label for="toggle2">Show Code</label>
<section id="content2">
<div class="language-javascript highlighter-rouge"><div class="highlight"><pre class="highlight"><code><span class="kd">function</span> <span class="nx">raytraceEllipsoid</span> <span class="p">(</span><span class="nx">rayOrigin</span><span class="p">,</span> <span class="nx">rayDirection</span><span class="p">)</span> <span class="p">{</span>
<span class="nx">sphereSpaceRayOrigin</span> <span class="o">=</span> <span class="nx">worldToSphereMatrix</span> <span class="o">*</span> <span class="nx">rayOrigin</span><span class="p">;</span>
<span class="nx">sphereSpaceRayDirection</span> <span class="o">=</span> <span class="p">(</span><span class="nx">worldToSphereMatrix</span> <span class="o">*</span> <span class="p">(</span><span class="nx">rayOrigin</span> <span class="o">+</span> <span class="nx">rayDirection</span><span class="p">))</span> <span class="o">-</span> <span class="nx">sphereSpaceRayOrigin</span><span class="p">).</span><span class="nx">normalized</span><span class="p">;</span>
<span class="nx">intersectionTime</span> <span class="o">=</span> <span class="nx">intersectRaySphere</span><span class="p">(</span><span class="nx">sphereSpaceRayOrigin</span><span class="p">,</span> <span class="nx">sphereSpaceRayDirection</span><span class="p">,</span> <span class="nx">Vector3</span><span class="p">.</span><span class="nx">zero</span><span class="p">,</span> <span class="nx">radius</span><span class="p">,</span> <span class="nx">insideSurface</span> <span class="o">=</span> <span class="kc">true</span><span class="p">);</span>
<span class="k">if</span> <span class="p">(</span><span class="nx">intersectionTime</span> <span class="o">></span> <span class="mi">0</span><span class="p">)</span> <span class="p">{</span>
<span class="nx">sphereSpaceHitPoint</span> <span class="o">=</span> <span class="nx">sphereSpaceRayOrigin</span> <span class="o">+</span> <span class="p">(</span><span class="nx">sphereSpaceRayDirection</span> <span class="o">*</span> <span class="nx">intersectionTime</span><span class="p">));</span>
<span class="nx">hitPoint</span> <span class="o">=</span> <span class="nx">worldToSphereMatrix</span><span class="p">.</span><span class="nx">inverse</span> <span class="o">*</span> <span class="nx">sphereSpaceHitPoint</span><span class="p">;</span>
<span class="k">return</span> <span class="nx">hitPoint</span><span class="p">;</span>
<span class="p">}</span> <span class="k">else</span> <span class="p">{</span>
<span class="k">return</span> <span class="kc">null</span><span class="p">;</span> <span class="c1">// No Hit</span>
<span class="p">}</span>
<span class="p">}</span>
</code></pre></div> </div>
<p><a href="https://github.com/zalo/zalo.github.io/blob/master/assets/js/Ellipsoids/Ellipsoid.js#L60-L94"><small>See Full Source</small></a></p>
</section>
</div>
<h4 id="foci-mirrors">Foci Mirrors</h4>
<p>Now, the Foci of Ellipsoids possess two very useful properties:</p>
<ol>
<li>
<p>The sum of distances from the foci to any point on the ellipsoid will add to a constant number. (This is true, <a href="https://en.wikipedia.org/wiki/N-ellipse">even for ellipsoids with more than two foci!</a>)</p>
</li>
<li>
<p>All rays that pass through one focus will always pass through the other focus when reflected from the internal surface of the ellipsoid.</p>
</li>
</ol>
<script type="text/javascript" src="../../assets/js/Ellipsoids/EllipsoidRaytracer.js" config="0" inverted="enabled" projector="enabled" projectorfov="165" orbit="enabled"></script>
<p>Due to property 1., the path length of each of these rays will be the same.</p>
<h4 id="chaining-ellipsoidal-mirrors">Chaining Ellipsoidal Mirrors</h4>
<p>One may even chain ellipsoids together by their their foci to reflect light through an arbitrary path.</p>
<script type="text/javascript" src="../../assets/js/Ellipsoids/EllipsoidRaytracer.js" config="1" inverted="enabled" projector="enabled" projectorfov="120" orbit="enabled"></script>
<p>It is also possible to switch some of the ellipsoidal reflectors to convex surfaces, as long as there is a concave reflector afterward to collect the rays again.</p>
<script type="text/javascript" src="../../assets/js/Ellipsoids/EllipsoidRaytracer.js" config="2" inverted="enabled" projector="enabled" projectorfov="120" orbit="enabled"></script>
<p>This configuration is a special case of an optical system called an “Offner Relay”. Convex mirrors inserted into the optical path tend to reverse the aberrations caused by the concave mirrors (and visa-versa).</p>
<div class="togglebox">
<input id="toggle1Long" type="checkbox" name="toggle" />
<label for="toggle1Long">The Only Limit Is Your Imagination</label>
<section id="content1Long">
<script type="text/javascript" src="../../assets/js/Ellipsoids/EllipsoidRaytracer.js" config="3" inverted="enabled" projector="enabled" projectorfov="120" orbit="enabled"></script>
<script type="text/javascript" src="../../assets/js/Ellipsoids/EllipsoidRaytracer.js" config="4" inverted="enabled" projector="enabled" projectorfov="120" orbit="enabled"></script>
</section>
</div>
<h4 id="fresnel-reflectors">Fresnel Reflectors</h4>
<p>Simple ellipsoidal optics are rare due to their bulk. This trade-off is apparent in <a href="http://blog.leapmotion.com/north-star-open-source/">Project North Star’s “Bird Bath” combiners</a>. When designing it, we chose to sacrifice form-factor for field-of-view and image quality.</p>
<p>Nearly all methods of reducing size (additional optical elements, folding the path, using holographic elements, etc.) tend increase both optical aberrations and cost. Aberrations tend to accumulate the more times light reflects or refracts through a surface.</p>
<p>However, there may be one powerful way to move light around while keeping the benefits of a single-bounce reflector…</p>
<script type="text/javascript" src="../../assets/js/Ellipsoids/FresnelEllipsoid.js" orbit="enabled"></script>
<p>By simply taking planar slices of many confocal ellipsoids to approximate a larger ellipsoid, one can preserve their optical properties while reducing their form-factor!</p>
<ul>
<li>Note how these slices emulate a refracting lens when the two foci are on opposite sides of the plate.</li>
</ul>
<h4 id="practical-fresnel-reflectors">Practical Fresnel Reflectors</h4>
<p>This is a relatively complex shape to assemble. <a href="https://web.archive.org/web/20220105014640/http://www.dr-iguana.com/prj_FlatPackMirror/index.html">There are great guides for building large, simplified fresnel reflectors at home to focus sunlight.</a></p>
<p>However, for near-eye displays, embedding these mirrored surfaces within clear material may be the most viable near-term structure.</p>
<p>There are two primary paths towards constructing them:</p>
<ul>
<li>Machining a Mold (High Volume, Slow Turnaround)
<ol>
<li>Use <a href="https://www.youtube.com/watch?v=pBueWfzb7P0">Wire EDM</a> to cut slices from many <a href="https: /www.edmundoptics.com/p/254mm-sq-2x-protected-aluminum-off-axis-ellipsoidal-mirror/41614/">traditionally machined ellipsoidal mirrors</a>.</li>
<li>Assemble these pieces concentrically to produce a mold.</li>
<li>This mold can be used to cast the first half of the part from a clear material like resin, epoxy, plastic, or glass.</li>
<li>Sputter or coat this internal surface with a 50% mirror coating.</li>
<li>Cast this part with the same material to produce a flat upper surface (to eliminate changes in refractive index at the internal mirror’s surface).</li>
<li>Coat the smooth exterior of the part with an antireflective film to minimize secondary reflections.</li>
</ol>
</li>
<li>3D Printing (Low Volume, Quick Turnaround)
<ol>
<li>Use an optical rapid-prototyping company like <a href="https://www.luxexcel.com/">Luxexcel</a> to simply print one half of the part.</li>
</ol>
<ul>
<li>One may print a larger cross-section than is necessary, and shave it down afterwards to accomodate the printing processes’ poor handling of discontinuities.</li>
</ul>
</li>
</ul>
<p>Afterwards, one may follow steps 4-6 from the first process.</p>
<hr />
<ul>
<li>Though the flat-plate structure of this technique undoes much of the <a href="https://en.wikipedia.org/wiki/Petzval_field_curvature">field-curvature (or “Petzval”) aberration</a> of traditional ellipsoidal reflectors, this technique can easily be extended to curved structures to suit even better form factors.</li>
</ul>Johnathon Selstadmakeshifted@gmail.comEllipsoids are spheres that have been stretched about one or more axes.Kabsch2019-03-16T20:10:33+00:002019-03-16T20:10:33+00:00https://zalo.github.io/blog/kabsch<p>The <a href="https://en.wikipedia.org/wiki/Kabsch_algorithm">Kabsch Algorithm</a> finds the optimal translation and rotation that minimizes the distance between two sets of matched points.</p>
<meta content="text/html; charset=UTF-8" http-equiv="Content-Type" />
<meta name="twitter:card" content="player" />
<meta name="twitter:site" content="@makeshifted" />
<meta name="twitter:title" content="Shape Matching Simulation" />
<meta name="twitter:description" content="The power of Kabsch in Shape Matching for physical simulation." />
<meta name="twitter:image" content="https://zalo.github.io/assets/images/Kabsch.png" />
<meta name="twitter:player" content="https://zalo.github.io/assets/cards/kabschCard.html" />
<meta name="twitter:player:width" content="500" />
<meta name="twitter:player:height" content="500" />
<meta property="og:url" content="https://zalo.github.io/blog/kabsch/" />
<meta property="og:image" content="https://zalo.github.io/assets/images/Kabsch.png" />
<meta property="og:video:type" content="text/html" />
<meta property="og:video:url" content="https://zalo.github.io/assets/cards/kabschCard.html" />
<meta property="og:video:height" content="500" />
<meta property="og:video:width" content="500" />
<meta property="og:type" content="video.other" />
<!-- Hide the Table of Contents (but keep the navigation :^) ... -->
<script type="text/javascript">
document.getElementsByClassName('toc')[0].style.display = 'none';
</script>
<!-- Load the Three.js library, assorted helpers, and the actual line fitting script code... -->
<script type="text/javascript" src="../../assets/js/three.js"></script>
<script type="text/javascript" src="../../assets/js/DragControls.js"></script>
<script type="text/javascript" src="../../assets/js/OrbitControls.js"></script>
<script type="text/javascript" src="../../assets/js/IK/Environment.js"></script>
<script type="text/javascript" src="../../assets/js/Kabsch/Kabsch.js" orbit="enabled" showaverage="false"></script>
<h4 id="optimal-translation">Optimal Translation</h4>
<p>Finding the optimal rigid transformation involves finding the optimal translations and rotations.</p>
<p>The optimal translation is simply <a href="https://en.wikipedia.org/wiki/Kabsch_algorithm#Translation">the offset between the averages of the two sets of points</a>.</p>
<div class="language-javascript highlighter-rouge"><div class="highlight"><pre class="highlight"><code><span class="nx">Vector3</span> <span class="nx">getAverage</span><span class="p">(</span><span class="nx">points</span><span class="p">){</span>
<span class="nx">centroid</span> <span class="o">=</span> <span class="nx">Vector3</span><span class="p">.</span><span class="nx">zero</span><span class="p">;</span>
<span class="nx">foreach</span> <span class="nx">point</span> <span class="k">in</span> <span class="nx">points</span> <span class="p">{</span>
<span class="nx">centroid</span> <span class="o">+=</span> <span class="nx">point</span><span class="p">;</span>
<span class="p">}</span>
<span class="nx">centroid</span> <span class="o">/=</span> <span class="nx">points</span><span class="p">.</span><span class="nx">count</span><span class="p">;</span>
<span class="p">}</span>
<span class="nx">optimalTranslation</span> <span class="o">=</span>
<span class="nx">getAverage</span><span class="p">(</span><span class="nx">toPoints</span><span class="p">)</span> <span class="o">-</span> <span class="nx">getAverage</span><span class="p">(</span><span class="nx">fromPoints</span><span class="p">);</span>
</code></pre></div></div>
<script type="text/javascript" src="../../assets/js/Kabsch/AverageMatching.js" orbit="enabled"></script>
<h4 id="optimal-rotation">Optimal Rotation</h4>
<p>The optimal rotation requires a few more steps.</p>
<p>First, you must mean-center the points (that is, subtract their means from them)</p>
<div class="language-javascript highlighter-rouge"><div class="highlight"><pre class="highlight"><code><span class="nx">fromPoints</span> <span class="o">=</span> <span class="nx">fromPoints</span> <span class="o">-</span> <span class="nx">getAverage</span><span class="p">(</span><span class="nx">fromPoints</span><span class="p">);</span>
<span class="nx">toPoints</span> <span class="o">=</span> <span class="nx">toPoints</span> <span class="o">-</span> <span class="nx">getAverage</span><span class="p">(</span><span class="nx">toPoints</span><span class="p">);</span>
</code></pre></div></div>
<p>Second, you must calculate the <a href="https://en.wikipedia.org/wiki/Kabsch_algorithm#Computation_of_the_covariance_matrix">3x3 Cross-Covariance Matrix</a> between them</p>
<div class="language-javascript highlighter-rouge"><div class="highlight"><pre class="highlight"><code><span class="nx">covariance</span> <span class="o">=</span> <span class="p">[</span>
<span class="k">new</span> <span class="nx">Vector3</span><span class="p">(</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">),</span>
<span class="k">new</span> <span class="nx">Vector3</span><span class="p">(</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">),</span>
<span class="k">new</span> <span class="nx">Vector3</span><span class="p">(</span><span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">,</span> <span class="mi">0</span><span class="p">)]</span>
<span class="k">for</span><span class="p">(</span><span class="nx">i</span> <span class="o">=</span> <span class="mi">0</span><span class="p">;</span> <span class="nx">i</span> <span class="o"><</span> <span class="mi">3</span><span class="p">;</span> <span class="nx">i</span><span class="o">++</span><span class="p">)</span> <span class="p">{</span>
<span class="k">for</span><span class="p">(</span><span class="nx">j</span> <span class="o">=</span> <span class="mi">0</span><span class="p">;</span> <span class="nx">j</span> <span class="o"><</span> <span class="mi">3</span><span class="p">;</span> <span class="nx">j</span><span class="o">++</span><span class="p">)</span> <span class="p">{</span>
<span class="k">for</span><span class="p">(</span><span class="nx">k</span> <span class="o">=</span> <span class="mi">0</span><span class="p">;</span> <span class="nx">k</span> <span class="o"><</span> <span class="nx">fromPoints</span><span class="p">.</span><span class="nx">length</span><span class="p">;</span> <span class="nx">k</span><span class="o">++</span><span class="p">)</span> <span class="p">{</span>
<span class="nx">covariance</span><span class="p">[</span><span class="nx">i</span><span class="p">][</span><span class="nx">j</span><span class="p">]</span> <span class="o">+=</span>
<span class="nx">fromPoints</span><span class="p">[</span><span class="nx">k</span><span class="p">][</span><span class="nx">i</span><span class="p">]</span> <span class="o">*</span> <span class="nx">toPoints</span><span class="p">[</span><span class="nx">k</span><span class="p">][</span><span class="nx">j</span><span class="p">];</span>
<span class="p">}</span>
<span class="p">}</span>
<span class="p">}</span>
</code></pre></div></div>
<p>And Third, you must find <a href="https://zalo.github.io/blog/polar-decomposition/#robust-polar-decomposition">the polar decomposition</a> of that matrix</p>
<div class="language-javascript highlighter-rouge"><div class="highlight"><pre class="highlight"><code><span class="k">for</span> <span class="p">(</span><span class="nx">iter</span> <span class="o">=</span> <span class="mi">0</span><span class="p">;</span> <span class="nx">iter</span> <span class="o"><</span> <span class="nx">iterations</span><span class="p">;</span> <span class="nx">iter</span><span class="o">++</span><span class="p">)</span> <span class="p">{</span>
<span class="nx">setBasesFromQuaternion</span><span class="p">(</span><span class="nx">curQuaternion</span><span class="p">,</span>
<span class="nx">curXBasis</span><span class="p">,</span> <span class="nx">curYBasis</span><span class="p">,</span> <span class="nx">curZBasis</span><span class="p">);</span>
<span class="nx">omega</span> <span class="o">=</span> <span class="p">(</span><span class="nx">cross</span><span class="p">(</span><span class="nx">curXBasis</span><span class="p">,</span> <span class="nx">covariance</span><span class="p">[</span><span class="mi">0</span><span class="p">])</span> <span class="o">+</span>
<span class="nx">cross</span><span class="p">(</span><span class="nx">curYBasis</span><span class="p">,</span> <span class="nx">covariance</span><span class="p">[</span><span class="mi">1</span><span class="p">])</span> <span class="o">+</span>
<span class="nx">cross</span><span class="p">(</span><span class="nx">curZBasis</span><span class="p">,</span> <span class="nx">covariance</span><span class="p">[</span><span class="mi">2</span><span class="p">]))</span> <span class="o">/</span>
<span class="nx">abs</span><span class="p">(</span><span class="nx">dot</span><span class="p">(</span><span class="nx">curXBasis</span><span class="p">,</span> <span class="nx">covariance</span><span class="p">[</span><span class="mi">0</span><span class="p">])</span> <span class="o">+</span>
<span class="nx">dot</span><span class="p">(</span><span class="nx">curYBasis</span><span class="p">,</span> <span class="nx">covariance</span><span class="p">[</span><span class="mi">1</span><span class="p">])</span> <span class="o">+</span>
<span class="nx">dot</span><span class="p">(</span><span class="nx">curZBasis</span><span class="p">,</span> <span class="nx">covariance</span><span class="p">[</span><span class="mi">2</span><span class="p">])</span> <span class="o">+</span> <span class="mf">0.000000001</span><span class="p">);</span>
<span class="nx">w</span> <span class="o">=</span> <span class="nx">omega</span><span class="p">.</span><span class="nx">magnitude</span><span class="p">;</span>
<span class="k">if</span> <span class="p">(</span><span class="nx">w</span> <span class="o"><</span> <span class="mf">0.000000001</span><span class="p">)</span> <span class="k">break</span><span class="p">;</span>
<span class="nx">curQuaternion</span> <span class="o">=</span> <span class="nx">angleAxis</span><span class="p">(</span><span class="nx">w</span><span class="p">,</span> <span class="nx">omega</span> <span class="o">/</span> <span class="nx">w</span><span class="p">)</span> <span class="o">*</span> <span class="nx">curQuaternion</span><span class="p">;</span>
<span class="p">}</span>
<span class="nx">optimalRotation</span> <span class="o">=</span> <span class="nx">curQuaternion</span><span class="p">;</span>
</code></pre></div></div>
<script type="text/javascript" src="../../assets/js/Kabsch/Kabsch.js" orbit="enabled" showaverage="enabled"></script>
<h4 id="shape-matching">Shape Matching</h4>
<p>One application of Kabsch is in Particle-based Physics Simulations. It allows you to tie multiple particles together into a single rigid-body</p>
<script type="text/javascript" src="../../assets/js/Kabsch/ShapeMatching.js" orbit="enabled"></script>
<div class="slidecontainer">
Stiffness: <input type="range" min="0" max="100" value="50" class="slider" id="stiffness" />
</div>
<div class="slidecontainer">
Number of Particles: <input type="range" min="3" max="50" value="10" class="slider" id="numParticles" />
</div>
<div class="slidecontainer">
Collision Iterations: <input type="range" min="1" max="10" value="5" class="slider" id="iterations" />
</div>
<div class="language-javascript highlighter-rouge"><div class="highlight"><pre class="highlight"><code><span class="c1">// Apply Particle Inertia and Gravity</span>
<span class="nx">rigidPoints</span> <span class="o">*=</span> <span class="nx">kabsch</span><span class="p">(</span><span class="nx">rigidPoints</span><span class="p">,</span> <span class="nx">physicsPoints</span><span class="p">);</span>
<span class="k">for</span><span class="p">(</span><span class="nx">i</span> <span class="o">=</span> <span class="mi">0</span><span class="p">;</span> <span class="nx">i</span> <span class="o"><</span> <span class="nx">physicsPoints</span><span class="p">.</span><span class="nx">length</span><span class="p">;</span> <span class="nx">i</span><span class="o">++</span><span class="p">)</span> <span class="p">{</span>
<span class="nx">lerp</span><span class="p">(</span><span class="nx">physicsPoints</span><span class="p">[</span><span class="nx">i</span><span class="p">],</span> <span class="nx">rigidPoints</span><span class="p">[</span><span class="nx">i</span><span class="p">],</span> <span class="nx">stiffnessAlpha</span><span class="p">);</span>
<span class="p">}</span>
<span class="c1">// Collide particles with the Ground</span>
</code></pre></div></div>
<p><a href="https://www.youtube.com/watch?v=CCIwiC37kks">Shape Matching</a> has a number of unique advantages over distance constraints:</p>
<ul>
<li>It’s cheap for large numbers (O(n) rather than O(n^2))</li>
<li>It is unconditionally stable; the configuration cannot be inverted</li>
<li>It’s branchless and requires relatively few square roots</li>
<li>Computation is iterative, allowing for dynamically scaling compute</li>
<li>Particles otherwise simulate totally in parallel, allowing for GPU Compute</li>
<li>Shockwaves travel instantaneously across the entire body of particles</li>
<li>Does not require a connectivity graph; topology can change dynamically</li>
</ul>Johnathon Selstadmakeshifted@gmail.comThe Kabsch Algorithm finds the optimal translation and rotation that minimizes the distance between two sets of matched points.Segment-Segment Distance in 3D2019-03-02T20:10:33+00:002019-03-02T20:10:33+00:00https://zalo.github.io/blog/closest-point-between-segments<p>One of my favorite functions projects points onto line segments.</p>
<!-- Hide the Table of Contents (but keep the navigation :^) ... -->
<script type="text/javascript">
document.getElementsByClassName('toc')[0].style.display = 'none';
</script>
<!-- Load the Three.js library, assorted helpers, and the actual line fitting script code... -->
<script type="text/javascript" src="../../assets/js/three.js"></script>
<script type="text/javascript" src="../../assets/js/DragControls.js"></script>
<script type="text/javascript" src="../../assets/js/OrbitControls.js"></script>
<script type="text/javascript" src="../../assets/js/IK/Environment.js"></script>
<script type="text/javascript" src="../../assets/js/ClosestSegment/ClosestSegment.js" orbit="enabled"></script>
<div class="language-javascript highlighter-rouge"><div class="highlight"><pre class="highlight"><code><span class="kd">function</span> <span class="nx">constrainToSegment</span><span class="p">(</span><span class="nx">position</span><span class="p">,</span> <span class="nx">a</span><span class="p">,</span> <span class="nx">b</span><span class="p">)</span> <span class="p">{</span>
<span class="nx">ba</span> <span class="o">=</span> <span class="nx">b</span> <span class="o">-</span> <span class="nx">a</span><span class="p">;</span> <span class="nx">t</span> <span class="o">=</span> <span class="nx">Dot</span><span class="p">(</span><span class="nx">position</span> <span class="o">-</span> <span class="nx">a</span><span class="p">,</span> <span class="nx">ba</span><span class="p">)</span> <span class="o">/</span> <span class="nx">Dot</span><span class="p">(</span><span class="nx">ba</span><span class="p">,</span> <span class="nx">ba</span><span class="p">);</span>
<span class="k">return</span> <span class="nx">Lerp</span><span class="p">(</span><span class="nx">a</span><span class="p">,</span> <span class="nx">b</span><span class="p">,</span> <span class="nx">Clamp01</span><span class="p">(</span><span class="nx">t</span><span class="p">));</span>
<span class="p">}</span>
</code></pre></div></div>
<p>It’s just so simple, fast, and useful! It works using the dot product to do vector projection.</p>
<p>But did you know that you can extend this trick to find the closest point between two line segments?</p>
<h3 id="line-segments">Line Segments</h3>
<script type="text/javascript" src="../../assets/js/ClosestSegment/SegmentSegment.js" orbit="enabled"></script>
<p>The key is to recognize that you can turn the pair of line segments INTO a point and a line segment by squishing all four points onto the plane defined by the first line segment!</p>
<script type="text/javascript" src="../../assets/js/ClosestSegment/SegmentSegment.js" orbit="enabled" debug="enabled"></script>
<div class="language-javascript highlighter-rouge"><div class="highlight"><pre class="highlight"><code><span class="nx">inPlaneA</span> <span class="o">=</span> <span class="nx">segA</span><span class="p">.</span><span class="nx">projectToPlane</span><span class="p">(</span><span class="nx">segC</span><span class="p">,</span> <span class="nx">segD</span><span class="o">-</span><span class="nx">segC</span><span class="p">);</span>
<span class="nx">inPlaneB</span> <span class="o">=</span> <span class="nx">segB</span><span class="p">.</span><span class="nx">projectToPlane</span><span class="p">(</span><span class="nx">segC</span><span class="p">,</span> <span class="nx">segD</span><span class="o">-</span><span class="nx">segC</span><span class="p">);</span>
<span class="nx">inPlaneBA</span> <span class="o">=</span> <span class="nx">inPlaneB</span><span class="o">-</span><span class="nx">inPlaneA</span><span class="p">;</span>
<span class="nx">t</span> <span class="o">=</span> <span class="nx">Dot</span><span class="p">(</span><span class="nx">segC</span><span class="o">-</span><span class="nx">inPlaneA</span><span class="p">,</span> <span class="nx">inPlaneBA</span><span class="p">)</span><span class="o">/</span><span class="nx">Dot</span><span class="p">(</span><span class="nx">inPlaneBA</span><span class="p">,</span> <span class="nx">inPlaneBA</span><span class="p">);</span>
<span class="nx">t</span> <span class="o">=</span> <span class="p">(</span><span class="nx">inPlaneA</span> <span class="o">!=</span> <span class="nx">inPlaneB</span><span class="p">)</span> <span class="p">?</span> <span class="nx">t</span> <span class="p">:</span> <span class="mi">0</span><span class="nx">f</span><span class="p">;</span> <span class="c1">// Zero's t if parallel</span>
<span class="nx">segABtoLineCD</span> <span class="o">=</span> <span class="nx">Lerp</span><span class="p">(</span><span class="nx">segA</span><span class="p">,</span> <span class="nx">segB</span><span class="p">,</span> <span class="nx">Clamp01</span><span class="p">(</span><span class="nx">t</span><span class="p">));</span>
<span class="nx">segCDtoSegAB</span> <span class="o">=</span> <span class="nx">constrainToSegment</span><span class="p">(</span><span class="nx">segABtoLineCD</span><span class="p">,</span> <span class="nx">segC</span><span class="p">,</span> <span class="nx">segD</span><span class="p">);</span>
<span class="nx">segABtoSegCD</span> <span class="o">=</span> <span class="nx">constrainToSegment</span><span class="p">(</span><span class="nx">segCDtoSegAB</span><span class="p">,</span> <span class="nx">segA</span><span class="p">,</span> <span class="nx">segB</span><span class="p">);</span>
</code></pre></div></div>
<p>Solving for the closest point in this reduced sub space gives you an answer that is valid in the full space!</p>
<h3 id="optimization">Optimization</h3>
<p>You can also do this function entirely without square roots in its slightly more optimized form:</p>
<div class="language-javascript highlighter-rouge"><div class="highlight"><pre class="highlight"><code><span class="nx">segDC</span> <span class="o">=</span> <span class="nx">segD</span><span class="o">-</span><span class="nx">segC</span><span class="p">;</span> <span class="nx">float</span> <span class="nx">lineDirSqrMag</span> <span class="o">=</span> <span class="nx">Dot</span><span class="p">(</span><span class="nx">segDC</span><span class="p">,</span> <span class="nx">segDC</span><span class="p">);</span>
<span class="nx">inPlaneA</span> <span class="o">=</span> <span class="nx">segA</span><span class="o">-</span><span class="p">((</span><span class="nx">Dot</span><span class="p">(</span><span class="nx">segA</span><span class="o">-</span><span class="nx">segC</span><span class="p">,</span> <span class="nx">segDC</span><span class="p">)</span><span class="o">/</span><span class="nx">lineDirSqrMag</span><span class="p">)</span><span class="o">*</span><span class="nx">segDC</span><span class="p">);</span>
<span class="nx">inPlaneB</span> <span class="o">=</span> <span class="nx">segB</span><span class="o">-</span><span class="p">((</span><span class="nx">Dot</span><span class="p">(</span><span class="nx">segB</span><span class="o">-</span><span class="nx">segC</span><span class="p">,</span> <span class="nx">segDC</span><span class="p">)</span><span class="o">/</span><span class="nx">lineDirSqrMag</span><span class="p">)</span><span class="o">*</span><span class="nx">segDC</span><span class="p">);</span>
<span class="nx">inPlaneBA</span> <span class="o">=</span> <span class="nx">inPlaneB</span><span class="o">-</span><span class="nx">inPlaneA</span><span class="p">;</span>
<span class="nx">t</span> <span class="o">=</span> <span class="nx">Dot</span><span class="p">(</span><span class="nx">segC</span><span class="o">-</span><span class="nx">inPlaneA</span><span class="p">,</span> <span class="nx">inPlaneBA</span><span class="p">)</span><span class="o">/</span><span class="nx">Dot</span><span class="p">(</span><span class="nx">inPlaneBA</span><span class="p">,</span> <span class="nx">inPlaneBA</span><span class="p">);</span>
<span class="nx">t</span> <span class="o">=</span> <span class="p">(</span><span class="nx">inPlaneA</span> <span class="o">!=</span> <span class="nx">inPlaneB</span><span class="p">)</span> <span class="p">?</span> <span class="nx">t</span> <span class="p">:</span> <span class="mi">0</span><span class="nx">f</span><span class="p">;</span> <span class="c1">// Zero's t if parallel</span>
<span class="nx">segABtoLineCD</span> <span class="o">=</span> <span class="nx">Lerp</span><span class="p">(</span><span class="nx">segA</span><span class="p">,</span> <span class="nx">segB</span><span class="p">,</span> <span class="nx">Clamp01</span><span class="p">(</span><span class="nx">t</span><span class="p">));</span>
<span class="nx">segCDtoSegAB</span> <span class="o">=</span> <span class="nx">constrainToSegment</span><span class="p">(</span><span class="nx">segABtoLineCD</span><span class="p">,</span> <span class="nx">segC</span><span class="p">,</span> <span class="nx">segD</span><span class="p">);</span>
<span class="nx">segABtoSegCD</span> <span class="o">=</span> <span class="nx">constrainToSegment</span><span class="p">(</span><span class="nx">segCDtoSegAB</span><span class="p">,</span> <span class="nx">segA</span><span class="p">,</span> <span class="nx">segB</span><span class="p">);</span>
</code></pre></div></div>
<p>(The only branch checks if the lines are completely parallel (which is unlikely in real-world data)).</p>Johnathon Selstadmakeshifted@gmail.comOne of my favorite functions projects points onto line segments.Orthonormalization2019-03-02T20:10:33+00:002019-03-02T20:10:33+00:00https://zalo.github.io/blog/polar-decomposition<p>A rotation matrix is really just an orthonormal basis (a set of three orthogonal, unit vectors representing the x, y, and z bases of your rotation).</p>
<p>Often times when doing vector math, you’ll want to find the closest rotation matrix to a set of vector bases.</p>
<h3 id="gram-schmidt-orthonormalization">Gram-Schmidt Orthonormalization</h3>
<p>The cheapest/default way is <a href="https://en.wikipedia.org/wiki/Gram%E2%80%93Schmidt_process">Gram-Schmidt Orthonormalization</a>. This process works in n-dimensions using vector projection.</p>
<p>A similar algorithm can be done in 3D with cross products:</p>
<div class="language-javascript highlighter-rouge"><div class="highlight"><pre class="highlight"><code><span class="nx">xBasis</span> <span class="o">=</span> <span class="nx">xBasis</span><span class="p">.</span><span class="nx">normalized</span><span class="p">;</span>
<span class="nx">yBasis</span> <span class="o">=</span> <span class="nx">cross</span><span class="p">(</span><span class="nx">zBasis</span><span class="p">,</span> <span class="nx">xBasis</span><span class="p">).</span><span class="nx">normalized</span><span class="p">;</span>
<span class="nx">zBasis</span> <span class="o">=</span> <span class="nx">cross</span><span class="p">(</span><span class="nx">xBasis</span><span class="p">,</span> <span class="nx">yBasis</span><span class="p">).</span><span class="nx">normalized</span><span class="p">;</span>
</code></pre></div></div>
<p>This algorithm is nice because it is short, analytic, and trivially differentiable (which can be useful in machine learning!) However, it only has one small issue….</p>
<!-- Hide the Table of Contents (but keep the navigation :^) ... -->
<script type="text/javascript">
document.getElementsByClassName('toc')[0].style.display = 'none';
</script>
<!-- Load the Three.js library, assorted helpers, and the actual line fitting script code... -->
<script type="text/javascript" src="../../assets/js/three.js"></script>
<script type="text/javascript" src="../../assets/js/DragControls.js"></script>
<script type="text/javascript" src="../../assets/js/OrbitControls.js"></script>
<script type="text/javascript" src="../../assets/js/IK/Environment.js"></script>
<script type="text/javascript" src="../../assets/js/PolarDecomposition/PolarDecomposition.js" orbit="enabled" baddecomposition="enabled"></script>
<p>It is terrible!</p>
<p>It does not spread the error evenly between the bases <em>at all</em>. The result is dependent on the order in which the bases are solved. The input x-direction is unmodified, the z-direction is just perpendicular to that, and the input y-direction is not even taken into account! It is by no means <em>the optimal</em> orthonormal matrix.</p>
<h3 id="robust-polar-decomposition">Robust Polar Decomposition</h3>
<p>The solution to this problem came from my favorite paper of 2016: <a href="https://animation.rwth-aachen.de/media/papers/2016-MIG-StableRotation.pdf">Matthias Müller’s Polar Decomposition</a>.</p>
<p>This paper offers a cheap, branchless iterative approximation to the orthonormalization problem that is extremely robust, where the error is spread out evenly across the three bases. The secret is that, instead of trying to find the optimal <em>orthonormal matrix</em>, it finds the the optimal <em>rotation</em> that matches an identity basis with the input basis.</p>
<p>You can play with it here
<script type="text/javascript" src="../../assets/js/PolarDecomposition/PolarDecomposition.js" orbit="enabled"></script></p>
<p>This algorithm looks like this</p>
<div class="language-javascript highlighter-rouge"><div class="highlight"><pre class="highlight"><code><span class="k">for</span> <span class="p">(</span><span class="nx">iter</span> <span class="o">=</span> <span class="mi">0</span><span class="p">;</span> <span class="nx">iter</span> <span class="o"><</span> <span class="nx">iterations</span><span class="p">;</span> <span class="nx">iter</span><span class="o">++</span><span class="p">)</span> <span class="p">{</span>
<span class="nx">setBasesFromQuaternion</span><span class="p">(</span><span class="nx">curQuaternion</span><span class="p">,</span>
<span class="nx">curXBasis</span><span class="p">,</span> <span class="nx">curYBasis</span><span class="p">,</span> <span class="nx">curZBasis</span><span class="p">);</span>
<span class="nx">omega</span> <span class="o">=</span> <span class="p">(</span><span class="nx">cross</span><span class="p">(</span><span class="nx">curXBasis</span><span class="p">,</span> <span class="nx">inputXBasis</span><span class="p">)</span> <span class="o">+</span>
<span class="nx">cross</span><span class="p">(</span><span class="nx">curYBasis</span><span class="p">,</span> <span class="nx">inputYBasis</span><span class="p">)</span> <span class="o">+</span>
<span class="nx">cross</span><span class="p">(</span><span class="nx">curZBasis</span><span class="p">,</span> <span class="nx">inputZBasis</span><span class="p">))</span> <span class="o">/</span>
<span class="nx">abs</span><span class="p">(</span><span class="nx">dot</span><span class="p">(</span><span class="nx">curXBasis</span><span class="p">,</span> <span class="nx">inputXBasis</span><span class="p">)</span> <span class="o">+</span>
<span class="nx">dot</span><span class="p">(</span><span class="nx">curYBasis</span><span class="p">,</span> <span class="nx">inputYBasis</span><span class="p">)</span> <span class="o">+</span>
<span class="nx">dot</span><span class="p">(</span><span class="nx">curZBasis</span><span class="p">,</span> <span class="nx">inputZBasis</span><span class="p">)</span> <span class="o">+</span> <span class="mf">0.000000001f</span><span class="p">);</span>
<span class="nx">w</span> <span class="o">=</span> <span class="nx">omega</span><span class="p">.</span><span class="nx">magnitude</span><span class="p">;</span>
<span class="k">if</span> <span class="p">(</span><span class="nx">w</span> <span class="o"><</span> <span class="mf">0.000000001f</span><span class="p">)</span> <span class="k">break</span><span class="p">;</span>
<span class="nx">curQuaternion</span> <span class="o">=</span> <span class="nx">angleAxis</span><span class="p">(</span><span class="nx">w</span><span class="p">,</span> <span class="nx">omega</span> <span class="o">/</span> <span class="nx">w</span><span class="p">)</span> <span class="o">*</span> <span class="nx">curQuaternion</span><span class="p">;</span>
<span class="p">}</span>
</code></pre></div></div>
<p>Or, in English, it executes these steps:</p>
<p>1) Compute the torque between each input basis and its current orthogonal estimate of each basis</p>
<p>2) Take the average of all the torques summed together</p>
<p>3) Apply this torque to its current rotation estimate using an Angle-Axis Quaternion</p>
<p>When there is no more torque left to apply, the <code class="language-plaintext highlighter-rouge">curQuaternion</code> has converged on the optimal rotation fitting the orthonormal basis (at least according to the “Frobenius Norm”).</p>
<h3 id="applications">Applications</h3>
<p>Matthias uses this technique to great effect in Nvidia FleX’s Cluster Shape Matching Solver (beginning at 1:15 )</p>
<!-- Courtesy of embedresponsively.com -->
<div class="responsive-video-container">
<iframe src="https://www.youtube-nocookie.com/embed/YOBjHpoImu8?t=74" frameborder="0" webkitallowfullscreen="" mozallowfullscreen="" allowfullscreen=""></iframe>
</div>
<p>The robust polar decomposition is a part of the algorithm that solves for the optimal rigid transformation between two sets of points (in the shape-matching simulation step).</p>
<p>Out of context, this simple application is called the <a href="https://github.com/zalo/mathutilities#kabsch">“Kabsch Algorithm”</a>. The Polar Decomposition stands in for the SVD here.</p>
<p>I have also found the concept of quaternion torque averaging to be useful when taking the <a href="https://github.com/zalo/MathUtilities/blob/master/Assets/Kabsch/AverageQuaternion.cs">spherical average of multiple quaternions</a>, and when performing <a href="https://github.com/zalo/MathUtilities#generalized-mesh-deformation">fast mesh deformation</a>.</p>
<p>Really, it can be used as an iterative, real-time substitute for an SVD. It can even be extended to any number of dimensions (which support constructing angle-axis rotations of course).</p>
<h3 id="the-future">The Future?</h3>
<p>Given that this algorithm is often located in the hottest regions of the program, there is a lot of pressure to optimize it even further.</p>
<p>A short while ago, I had an idea:</p>
<blockquote>
<p>shouldn’t you be able to orthonormalize rotation matrices by iteratively applying an orthogonality constraint via the cross product? Then you wouldn’t need Quaternions!</p>
</blockquote>
<p>Instead of iteratively rotating a basis to match the input, one might just “unfold” the input to its closest orthogonal representation.</p>
<p>So I set about putting this down into algorithm form…</p>
<div class="language-javascript highlighter-rouge"><div class="highlight"><pre class="highlight"><code><span class="nx">mB</span> <span class="o">=</span> <span class="p">[</span> <span class="nx">inX</span><span class="p">.</span><span class="nx">magnitude</span><span class="p">,</span> <span class="nx">inY</span><span class="p">.</span><span class="nx">magnitude</span><span class="p">,</span> <span class="nx">inZ</span><span class="p">.</span><span class="nx">magnitude</span> <span class="p">];</span>
<span class="k">for</span> <span class="p">(</span><span class="nx">int</span> <span class="nx">iter</span> <span class="o">=</span> <span class="mi">0</span><span class="p">;</span> <span class="nx">iter</span> <span class="o"><</span> <span class="mi">9</span><span class="p">;</span> <span class="nx">iter</span><span class="o">++</span><span class="p">)</span> <span class="p">{</span>
<span class="nx">unitX</span> <span class="o">=</span> <span class="p">(</span><span class="nx">cross</span><span class="p">(</span><span class="nx">inY</span><span class="p">,</span> <span class="nx">inZ</span><span class="p">)</span> <span class="o">+</span> <span class="nx">inX</span><span class="p">).</span><span class="nx">normalized</span><span class="p">;</span>
<span class="nx">unitY</span> <span class="o">=</span> <span class="p">(</span><span class="nx">cross</span><span class="p">(</span><span class="nx">inZ</span><span class="p">,</span> <span class="nx">inX</span><span class="p">)</span> <span class="o">+</span> <span class="nx">inY</span><span class="p">).</span><span class="nx">normalized</span><span class="p">;</span>
<span class="nx">unitZ</span> <span class="o">=</span> <span class="p">(</span><span class="nx">cross</span><span class="p">(</span><span class="nx">inX</span><span class="p">,</span> <span class="nx">inY</span><span class="p">)</span> <span class="o">+</span> <span class="nx">inZ</span><span class="p">).</span><span class="nx">normalized</span><span class="p">;</span>
<span class="nx">inX</span> <span class="o">=</span> <span class="nx">unitX</span> <span class="o">*</span> <span class="nx">mB</span><span class="p">[</span><span class="mi">0</span><span class="p">];</span> <span class="nx">inY</span> <span class="o">=</span> <span class="nx">unitY</span> <span class="o">*</span> <span class="nx">mB</span><span class="p">[</span><span class="mi">1</span><span class="p">];</span> <span class="nx">inZ</span> <span class="o">=</span> <span class="nx">unitZ</span> <span class="o">*</span> <span class="nx">mB</span><span class="p">[</span><span class="mi">2</span><span class="p">];</span>
<span class="p">}</span>
</code></pre></div></div>
<p>And you can play with the result here
<script type="text/javascript" src="../../assets/js/PolarDecomposition/PolarDecomposition.js" orbit="enabled" crossproductdecomposition="enabled"></script>
(the cyan basis is the cross-product function, and the colored basis is the quaternion torque function)</p>
<p>See any issues?</p>
<p>It converges almost instantly compared to the quaternion torque technique, and it matches up perfectly… except for when it does not.</p>
<p>It is good enough that I think the concept is sound, but there is something off in the implementation. The common wisdom in vector math is that normalization is a sign of weak understanding; it is highly likely that if those <code class="language-plaintext highlighter-rouge">normalized</code>’s are replaced with dot products in the right way, it will evaluate more quickly <em>and</em> converge on the correct answer.</p>
<p>If you can figure out the mystery of the faster optimal orthonormal matrix, I would like to hear from you!</p>
<p>(No, seriously, please shoot me an e-mail; I am enormously interested in hearing your solutions!)</p>Johnathon Selstadmakeshifted@gmail.comA rotation matrix is really just an orthonormal basis (a set of three orthogonal, unit vectors representing the x, y, and z bases of your rotation).Line Fitting in 3D2019-02-23T16:10:33+00:002019-02-23T16:10:33+00:00https://zalo.github.io/blog/line-fitting<p>Orthogonal Regression is the process of finding the line that best fits a set of points by minimizing their squared <em>orthogonal</em> distances to it.</p>
<!-- Hide the Table of Contents (but keep the navigation :^) ... -->
<script type="text/javascript">
document.getElementsByClassName('toc')[0].style.display = 'none';
</script>
<!-- Load the Three.js library, assorted helpers, and the actual line fitting script code... -->
<script type="text/javascript" src="../../assets/js/three.js"></script>
<script type="text/javascript" src="../../assets/js/DragControls.js"></script>
<script type="text/javascript" src="../../assets/js/OrbitControls.js"></script>
<script type="text/javascript" src="../../assets/js/IK/Environment.js"></script>
<script type="text/javascript" src="../../assets/js/LineFitting/LineFitting.js" orbit="enabled" residuals="enabled"></script>
<p>This is more powerful than traditional least squares in that it is invariant to global rotation.</p>
<p>Because of this, however, it has no analytic solution in 3D and above. There are a variety of iterative solutions available, but they often depend on an operation called the <strong>Singular Value Decomposition</strong> or <strong>SVD</strong>. The SVD is a complex matrix decomposition that is usually only found in heavy-weight matrix libraries. Avoiding the inclusion of these libraries is the motivation for the technique outlined in this post.</p>
<h3 id="the-centroid">The Centroid</h3>
<p>The first key insight is that the orthogonal regression line will always pass through the average (or <em>centroid</em>) of the points.</p>
<p>The centroid can be computed by adding the points together, and then dividing by the number of points.</p>
<div class="language-javascript highlighter-rouge"><div class="highlight"><pre class="highlight"><code><span class="nx">centroid</span> <span class="o">=</span> <span class="nx">Vector3</span><span class="p">.</span><span class="nx">zero</span><span class="p">;</span>
<span class="nx">foreach</span> <span class="nx">point</span> <span class="k">in</span> <span class="nx">points</span> <span class="p">{</span>
<span class="nx">centroid</span> <span class="o">+=</span> <span class="nx">point</span><span class="p">;</span>
<span class="p">}</span>
<span class="nx">centroid</span> <span class="o">/=</span> <span class="nx">points</span><span class="p">.</span><span class="nx">count</span><span class="p">;</span>
</code></pre></div></div>
<script type="text/javascript" src="../../assets/js/LineFitting/VisualizeAverage.js" orbit="enabled"></script>
<p>Now that we have a point that the line passes through, we just need to calculate its direction.</p>
<h3 id="getting-closer">Getting Closer</h3>
<p>Since this algorithm is iterative, we need an iteration function that gets us <em>closer</em> to the true line direction at each step.</p>
<p>We can choose almost any** direction to start, and we will iteratively work toward the real direction.</p>
<p>The key to calculating the next direction estimate is to multiply each point by the dot product of it and the current estimate. This moves all the points to the same hemisphere as the current guess, allowing you to simply sum them and normalize them for the new direction estimate.</p>
<div class="language-javascript highlighter-rouge"><div class="highlight"><pre class="highlight"><code><span class="c1">// Solve for the centroid (See Above)</span>
<span class="nx">nextDirection</span> <span class="o">=</span> <span class="nx">Vector3</span><span class="p">.</span><span class="nx">zero</span><span class="p">;</span>
<span class="nx">foreach</span> <span class="nx">point</span> <span class="k">in</span> <span class="nx">points</span> <span class="p">{</span>
<span class="nx">centeredPoint</span> <span class="o">=</span> <span class="nx">point</span> <span class="o">-</span> <span class="nx">centroid</span><span class="p">;</span>
<span class="nx">nextDirection</span> <span class="o">+=</span> <span class="nx">dot</span><span class="p">(</span><span class="nx">centeredPoint</span><span class="p">,</span> <span class="nx">direction</span><span class="p">)</span> <span class="o">*</span> <span class="nx">centeredPoint</span><span class="p">;</span>
<span class="p">}</span>
<span class="nx">direction</span> <span class="o">=</span> <span class="nx">nextDirection</span><span class="p">.</span><span class="nx">normalize</span><span class="p">();</span>
</code></pre></div></div>
<script type="text/javascript" src="../../assets/js/LineFitting/LineStepping.js" orbit="enabled"></script>
<p>In formal mathematics, this is known as <a href="https://en.wikipedia.org/wiki/Power_iteration">Power Iteration</a>.</p>
<h3 id="a-virtuous-cycle">A Virtuous Cycle</h3>
<p>We only need to run the stepping routine repeatedly to converge on the line of best fit.</p>
<div class="language-javascript highlighter-rouge"><div class="highlight"><pre class="highlight"><code><span class="c1">// Solve for the centroid (See Above)</span>
<span class="k">for</span><span class="p">(</span><span class="nx">i</span> <span class="o">=</span> <span class="mi">0</span><span class="p">;</span> <span class="nx">i</span> <span class="o"><</span> <span class="nx">iterations</span><span class="p">;</span> <span class="nx">i</span><span class="o">++</span><span class="p">){</span>
<span class="c1">// Step the best fit line direction (See Above)</span>
<span class="p">}</span>
</code></pre></div></div>
<script type="text/javascript" src="../../assets/js/LineFitting/LineFitting.js" orbit="enabled" residuals="disabled"></script>
<p>No SVD’s required!</p>
<h3 id="sequential-down-projection-for-secondary-axes">Sequential Down Projection for Secondary Axes</h3>
<p>Typically one might extend the line fitting code to fit planes by projecting the points onto the plane defined by the primary axis, and then fitting the secondary axis on that flattened set of points.</p>
<div class="language-javascript highlighter-rouge"><div class="highlight"><pre class="highlight"><code><span class="c1">// Solve for the centroid (See Above)</span>
<span class="c1">// Fit Primary Axis with normal points(See Above)</span>
<span class="nx">foreach</span><span class="p">(</span><span class="nx">point</span> <span class="k">in</span> <span class="nx">points</span><span class="p">){</span>
<span class="nx">flattenedPoints</span><span class="p">.</span><span class="nx">push</span><span class="p">(</span><span class="nx">projectOnPlane</span><span class="p">(</span><span class="nx">point</span><span class="p">,</span> <span class="nx">primaryAxis</span><span class="p">));</span>
<span class="p">}</span>
<span class="c1">// Fit Secondary Axis with flattened points (See Above)</span>
<span class="nx">normal</span> <span class="o">=</span> <span class="nx">cross</span><span class="p">(</span><span class="nx">primaryAxis</span><span class="p">,</span> <span class="nx">secondaryAxis</span><span class="p">).</span><span class="nx">normalized</span><span class="p">;</span>
</code></pre></div></div>
<h3 id="abusing-singularities-for-fun-and-profit">Abusing Singularities for Fun and Profit</h3>
<p>** The observant among you might have noticed that there exist starting angles where the progress towards the true fitting line is <strong>0</strong>. This is actually a “singularity” that occurs when the starting guess is perfectly orthogonal to the true answer. <small>One should check for these cases, though it’s rare in practice.</small></p>
<p>However, it turns out we can abuse this behaviour to fit all of the axes simultaneously! Instead of projecting the <em>points</em> onto the plane defined by the primary axis, we project the current estimation of the secondary axis itself! This lets us solve for both the primary and secondary axes simultaneously!</p>
<div class="language-javascript highlighter-rouge"><div class="highlight"><pre class="highlight"><code><span class="c1">// Solve for the centroid (See Above)</span>
<span class="k">for</span> <span class="p">(</span><span class="nx">int</span> <span class="nx">iter</span> <span class="o">=</span> <span class="mi">0</span><span class="p">;</span> <span class="nx">iter</span> <span class="o"><</span> <span class="nx">iters</span><span class="p">;</span> <span class="nx">iter</span><span class="o">++</span><span class="p">)</span> <span class="p">{</span>
<span class="nx">newPrimaryAxis</span> <span class="o">=</span> <span class="nx">Vector3</span><span class="p">.</span><span class="nx">zero</span><span class="p">,</span> <span class="nx">newSecondaryAxis</span> <span class="o">=</span> <span class="nx">Vector3</span><span class="p">.</span><span class="nx">zero</span><span class="p">;</span>
<span class="nx">foreach</span><span class="p">(</span><span class="nx">point</span> <span class="k">in</span> <span class="nx">points</span><span class="p">)</span> <span class="p">{</span>
<span class="nx">point</span> <span class="o">=</span> <span class="nx">worldSpacePoint</span> <span class="o">-</span> <span class="nx">origin</span><span class="p">;</span>
<span class="nx">newPrimaryAxis</span> <span class="o">+=</span> <span class="nx">dot</span><span class="p">(</span><span class="nx">primaryAxis</span><span class="p">,</span> <span class="nx">point</span><span class="p">)</span> <span class="o">*</span> <span class="nx">point</span><span class="p">;</span>
<span class="nx">newSecondaryAxis</span> <span class="o">+=</span> <span class="nx">dot</span><span class="p">(</span><span class="nx">secondaryAxis</span><span class="p">,</span> <span class="nx">point</span><span class="p">)</span> <span class="o">*</span> <span class="nx">point</span><span class="p">;</span>
<span class="p">}</span>
<span class="nx">primaryAxis</span> <span class="o">=</span> <span class="nx">newPrimaryAxis</span><span class="p">.</span><span class="nx">normalized</span><span class="p">;</span>
<span class="nx">secondaryAxis</span> <span class="o">=</span> <span class="nx">projectOnPlane</span><span class="p">(</span><span class="nx">newSecondaryAxis</span><span class="p">,</span>
<span class="nx">primaryAxis</span><span class="p">).</span><span class="nx">normalized</span><span class="p">;</span>
<span class="p">}</span>
<span class="nx">normal</span> <span class="o">=</span> <span class="nx">cross</span><span class="p">(</span><span class="nx">primaryAxis</span><span class="p">,</span> <span class="nx">secondaryAxis</span><span class="p">).</span><span class="nx">normalized</span><span class="p">;</span>
</code></pre></div></div>
<script type="text/javascript" src="../../assets/js/LineFitting/PlaneFitting.js" orbit="enabled" residuals="enabled"></script>
<p>From here, it is trivial to see how one might add <em>additional</em> secondary axes of fit. In formal math terminology, this is equivalent to alternating <a href="https://en.wikipedia.org/wiki/Power_iteration">Power Iteration</a> and <a href="https://en.wikipedia.org/wiki/Gram%E2%80%93Schmidt_process">Gram-Schmidt Orthonormalization</a>.</p>
<p>There might come a point when numerical instability will prevent the lower-order axes from fitting nicely in this paradigm. If that happens, I recommend just projecting your points down to the plane (defined by the previous axis) after solving each axis.</p>
<p>I hope these techniques will come in handy whenever someone wants to fit lines or planes without incurring large dependencies within their projects.</p>Johnathon Selstadmakeshifted@gmail.comOrthogonal Regression is the process of finding the line that best fits a set of points by minimizing their squared orthogonal distances to it.Inverse Kinematics2019-02-16T16:10:33+00:002019-02-16T16:10:33+00:00https://zalo.github.io/blog/inverse-kinematics<p>Inverse Kinematics is the process of finding a set of joint angles that reach a goal position.</p>
<p>My favorite way of doing Inverse Kinematics is called <a href="http://number-none.com/product/IK%20with%20Quaternion%20Joint%20Limits/">“Quaternion Cyclic Coordinate Descent”</a> or “CCDIK”:</p>
<!-- Hide the Table of Contents (but keep the navigation :^) ... -->
<script type="text/javascript">
document.getElementsByClassName('toc')[0].style.display = 'none';
</script>
<!-- Load the Three.js library, assorted helpers, and the actual IK script code... -->
<script type="text/javascript" src="../../assets/js/three.js"></script>
<script type="text/javascript" src="../../assets/js/DragControls.js"></script>
<script type="text/javascript" src="../../assets/js/OrbitControls.js"></script>
<script type="text/javascript" src="../../assets/js/IK/Environment.js"></script>
<script type="text/javascript" src="../../assets/js/IK/IKExample.js" ccd="enabled" hinge="enabled" limits="enabled"></script>
<h3 id="ccdik">CCDIK</h3>
<p>At its core, the algorithm is very simple</p>
<div class="language-javascript highlighter-rouge"><div class="highlight"><pre class="highlight"><code><span class="nx">foreach</span> <span class="nx">joint</span> <span class="k">in</span> <span class="nx">jointsTipToBase</span> <span class="p">{</span>
<span class="c1">// Point the effector towards the goal</span>
<span class="nx">directionToEffector</span> <span class="o">=</span> <span class="nx">effector</span><span class="p">.</span><span class="nx">position</span> <span class="o">-</span> <span class="nx">joint</span><span class="p">.</span><span class="nx">position</span><span class="p">;</span>
<span class="nx">directionToGoal</span> <span class="o">=</span> <span class="nx">goal</span><span class="p">.</span><span class="nx">position</span> <span class="o">-</span> <span class="nx">joint</span><span class="p">.</span><span class="nx">position</span><span class="p">;</span>
<span class="nx">joint</span><span class="p">.</span><span class="nx">rotateFromTo</span><span class="p">(</span><span class="nx">directionToEffector</span><span class="p">,</span> <span class="nx">directionToGoal</span><span class="p">);</span>
<span class="p">}</span>
</code></pre></div></div>
<p>We’re just telling each joint to <em>point the end effector</em> towards the goal position.</p>
<script type="text/javascript" src="../../assets/js/IK/IKExample.js" ccd="enabled" hinge="disabled" limits="disabled"></script>
<p>Of course, if it reaches for the goal without regard for the hinges, it looks unnatural!</p>
<h3 id="hinges">Hinges</h3>
<p>We can take the hinges into account by enforcing the hinge-axis after the CCD step</p>
<div class="language-javascript highlighter-rouge"><div class="highlight"><pre class="highlight"><code><span class="nx">foreach</span> <span class="nx">joint</span> <span class="k">in</span> <span class="nx">jointsTipToBase</span> <span class="p">{</span>
<span class="c1">// Point the effector towards the goal (See Above)</span>
<span class="c1">// Constrain to rotate about the axis</span>
<span class="nx">curHingeAxis</span> <span class="o">=</span> <span class="nx">joint</span><span class="p">.</span><span class="nx">rotation</span> <span class="o">*</span> <span class="nx">joint</span><span class="p">.</span><span class="nx">axis</span><span class="p">;</span>
<span class="nx">hingeAxis</span> <span class="o">=</span> <span class="nx">joint</span><span class="p">.</span><span class="nx">parent</span><span class="p">.</span><span class="nx">rotation</span> <span class="o">*</span> <span class="nx">joint</span><span class="p">.</span><span class="nx">axis</span><span class="p">;</span>
<span class="nx">joint</span><span class="p">.</span><span class="nx">rotateFromTo</span><span class="p">(</span><span class="nx">curHingeAxis</span><span class="p">,</span> <span class="nx">hingeAxis</span><span class="p">);</span>
<span class="p">}</span>
</code></pre></div></div>
<script type="text/javascript" src="../../assets/js/IK/IKExample.js" ccd="enabled" hinge="enabled" limits="disabled"></script>
<p>Even at one iteration per frame, this is beginning to look pretty good! But real joints often have limits…</p>
<h3 id="limits">Limits</h3>
<p>You can apply axis-aligned hinge limits in local-euler angle space</p>
<div class="language-javascript highlighter-rouge"><div class="highlight"><pre class="highlight"><code><span class="nx">foreach</span> <span class="nx">joint</span> <span class="k">in</span> <span class="nx">jointsTipToBase</span> <span class="p">{</span>
<span class="c1">// Point the effector towards the goal (See Above)</span>
<span class="c1">// Constrain to rotate about the axis (See Above)</span>
<span class="c1">// Enforce Joint Limits</span>
<span class="nx">joint</span><span class="p">.</span><span class="nx">localRotation</span><span class="p">.</span><span class="nx">clampEuler</span><span class="p">(</span><span class="nx">joint</span><span class="p">.</span><span class="nx">minLimit</span><span class="p">,</span> <span class="nx">joint</span><span class="p">.</span><span class="nx">maxLimit</span><span class="p">);</span>
<span class="p">}</span>
</code></pre></div></div>
<script type="text/javascript" src="../../assets/js/IK/IKExample.js" ccd="enabled" hinge="enabled" limits="enabled" orbit="enabled"></script>
<p><small><a href="https://github.com/zalo/zalo.github.io/blob/master/assets/js/IK/IKExample.js">Full Source</a></small></p>
<p>This final aspect gives you an iterative 3D IK algorithm that beats nearly every other Heuristic Inverse Kinematics algorithm out there.</p>
<h3 id="properties-of-various-ik-algorithms">Properties of Various IK Algorithms</h3>
<table>
<thead>
<tr>
<th>IK Algorithms</th>
<th>Analytic</th>
<th>Automatic Differentiation</th>
<th>Jacobian Transpose</th>
<th>FABRIK</th>
<th>Quaternion CCDIK</th>
</tr>
</thead>
<tbody>
<tr>
<td>Implementation Complexity?</td>
<td>Extremely Complex</td>
<td>Hard</td>
<td>Hard</td>
<td>Easy</td>
<td>Easy</td>
</tr>
<tr>
<td>Speed</td>
<td>Extremely Fast</td>
<td>Slow To Converge</td>
<td>Slow To Converge</td>
<td>Fast</td>
<td>Fast</td>
</tr>
<tr>
<td>Hinge Joints</td>
<td>Only Hinges?</td>
<td>Yes</td>
<td>Yes</td>
<td>No!</td>
<td>Yes</td>
</tr>
<tr>
<td>Joint Limits</td>
<td>Difficult</td>
<td>Yes</td>
<td>No</td>
<td>Conical Limits</td>
<td>Yes</td>
</tr>
<tr>
<td>Hits Singularities</td>
<td>Never</td>
<td>Often</td>
<td>Often</td>
<td>Never (w/out hinges)</td>
<td>Rarely (often anneals through them)</td>
</tr>
<tr>
<td>Convergence Behaviour</td>
<td>Instant</td>
<td>Stable</td>
<td>Stable</td>
<td>Very Well Behaved</td>
<td>Well Behaved across short distances</td>
</tr>
<tr>
<td>Number of Joints</td>
<td>Max ~5</td>
<td>Arbitrary</td>
<td>Arbitrary</td>
<td>Arbitrary</td>
<td>Arbitrary</td>
</tr>
</tbody>
</table>
<h3 id="bonus-direction">Bonus: Direction</h3>
<p>The astute among you might notice that this is a 5-DoF arm. This means it is over-actuated for just touching a 3-DoF point.</p>
<p>With 5-Dof, you can hypothetically touch any point <em>from any direction</em>.</p>
<div class="language-javascript highlighter-rouge"><div class="highlight"><pre class="highlight"><code><span class="nx">foreach</span> <span class="nx">joint</span> <span class="k">in</span> <span class="nx">jointsTipToBase</span> <span class="p">{</span>
<span class="k">if</span><span class="p">(</span><span class="nx">joint</span><span class="p">.</span><span class="nx">id</span> <span class="o">></span> <span class="mi">3</span><span class="p">)</span>
<span class="c1">// Point the effector along the desired direction</span>
<span class="nx">joint</span><span class="p">.</span><span class="nx">rotateFromTo</span><span class="p">(</span><span class="nx">effector</span><span class="p">.</span><span class="nx">direction</span><span class="p">,</span> <span class="nx">goal</span><span class="p">.</span><span class="nx">direction</span><span class="p">);</span>
<span class="p">}</span> <span class="k">else</span> <span class="p">{</span>
<span class="c1">// Point the effector towards the goal (See Above)</span>
<span class="p">}</span>
<span class="c1">// Constrain to rotate about the axis (See Above)</span>
<span class="c1">// Enforce Joint Limits (See Above)</span>
<span class="p">}</span>
</code></pre></div></div>
<script type="text/javascript" src="../../assets/js/IK/IKExample.js" ccd="enabled" hinge="enabled" limits="enabled" orbit="enabled" matchdirection="enabled"></script>
<p>Now that the system is only “sufficiently actuated” (where the IK is using each degree of freedom the arm possesses), you may notice that it hits joint-limit based singularities more often.</p>
<p>These concavities are impossible to avoid in a heuristic IK algorithm (read: all of them except <code class="language-plaintext highlighter-rouge">Analytic</code>). However, it is possible to “jump out of” concavities by adding large random offsets to each joint, and then attempting the IK solve again. This is known as <a href="https://en.wikipedia.org/wiki/Simulated_annealing">“Simulated Annealing”</a>. Implementing this is left as an exercise to the reader.</p>
<div class="togglebox">
<input id="toggle1Long" type="checkbox" name="toggle" />
<label for="toggle1Long">Click for Appendix: Rotation from Two Vectors</label>
<section id="content1Long">
<p>While 3D Engines (<a href="https://docs.unity3d.com/ScriptReference/Quaternion.FromToRotation.html">Unity</a>, <a href="https://threejs.org/docs/#api/en/math/Quaternion.setFromUnitVectors">Three.js</a>) come with this function, it can be useful to see how it is implemented.</p>
<p><a href="http://lolengine.net/blog/2013/09/18/beautiful-maths-quaternion-from-vectors">Sam</a> <a href="http://lolengine.net/blog/2014/02/24/quaternion-from-two-vectors-final">Hocevar</a>, <a href="https://iquilezles.org/www/articles/noacos/noacos.htm">Inigo Quilez</a>, <a href="http://number-none.com/product/IK%20with%20Quaternion%20Joint%20Limits/">Jonathon Blow</a>, and <a href="http://marc-b-reynolds.github.io/quaternions/2016/08/09/TwoNormToRot.html">Marc B. Reynolds</a> all give excellent implementations and derivations.</p>
<p>For convenience, this is a mirror of Sam Hocevar’s robust C++ implementation:</p>
<div class="language-c++ highlighter-rouge"><div class="highlight"><pre class="highlight"><code><span class="n">quat</span> <span class="n">quat</span><span class="o">::</span><span class="n">fromtwovectors</span><span class="p">(</span><span class="n">vec3</span> <span class="n">u</span><span class="p">,</span> <span class="n">vec3</span> <span class="n">v</span><span class="p">)</span> <span class="p">{</span>
<span class="kt">float</span> <span class="n">norm_u_norm_v</span> <span class="o">=</span> <span class="n">sqrt</span><span class="p">(</span><span class="n">dot</span><span class="p">(</span><span class="n">u</span><span class="p">,</span> <span class="n">u</span><span class="p">)</span> <span class="o">*</span> <span class="n">dot</span><span class="p">(</span><span class="n">v</span><span class="p">,</span> <span class="n">v</span><span class="p">));</span>
<span class="kt">float</span> <span class="n">real_part</span> <span class="o">=</span> <span class="n">norm_u_norm_v</span> <span class="o">+</span> <span class="n">dot</span><span class="p">(</span><span class="n">u</span><span class="p">,</span> <span class="n">v</span><span class="p">);</span>
<span class="n">vec3</span> <span class="n">w</span><span class="p">;</span>
<span class="k">if</span> <span class="p">(</span><span class="n">real_part</span> <span class="o"><</span> <span class="mf">1.e-6</span><span class="n">f</span> <span class="o">*</span> <span class="n">norm_u_norm_v</span><span class="p">)</span> <span class="p">{</span>
<span class="cm">/* If u and v are exactly opposite, rotate 180 degrees
* around an arbitrary orthogonal axis. Axis normalisation
* can happen later, when we normalise the quaternion. */</span>
<span class="n">real_part</span> <span class="o">=</span> <span class="mf">0.0</span><span class="n">f</span><span class="p">;</span>
<span class="n">w</span> <span class="o">=</span> <span class="n">abs</span><span class="p">(</span><span class="n">u</span><span class="p">.</span><span class="n">x</span><span class="p">)</span> <span class="o">></span> <span class="n">abs</span><span class="p">(</span><span class="n">u</span><span class="p">.</span><span class="n">z</span><span class="p">)</span> <span class="o">?</span> <span class="n">vec3</span><span class="p">(</span><span class="o">-</span><span class="n">u</span><span class="p">.</span><span class="n">y</span><span class="p">,</span> <span class="n">u</span><span class="p">.</span><span class="n">x</span><span class="p">,</span> <span class="mf">0.</span><span class="n">f</span><span class="p">)</span>
<span class="o">:</span> <span class="n">vec3</span><span class="p">(</span><span class="mf">0.</span><span class="n">f</span><span class="p">,</span> <span class="o">-</span><span class="n">u</span><span class="p">.</span><span class="n">z</span><span class="p">,</span> <span class="n">u</span><span class="p">.</span><span class="n">y</span><span class="p">);</span>
<span class="p">}</span> <span class="k">else</span> <span class="p">{</span>
<span class="cm">/* Otherwise, build quaternion the standard way. */</span>
<span class="n">w</span> <span class="o">=</span> <span class="n">cross</span><span class="p">(</span><span class="n">u</span><span class="p">,</span> <span class="n">v</span><span class="p">);</span>
<span class="p">}</span>
<span class="k">return</span> <span class="n">normalize</span><span class="p">(</span><span class="n">quat</span><span class="p">(</span><span class="n">real_part</span><span class="p">,</span> <span class="n">w</span><span class="p">.</span><span class="n">x</span><span class="p">,</span> <span class="n">w</span><span class="p">.</span><span class="n">y</span><span class="p">,</span> <span class="n">w</span><span class="p">.</span><span class="n">z</span><span class="p">));</span>
<span class="p">}</span>
</code></pre></div> </div>
<p>and Marc’s Simple C++ implementation:</p>
<div class="language-c++ highlighter-rouge"><div class="highlight"><pre class="highlight"><code><span class="n">vec4</span> <span class="nf">q_from_normals</span><span class="p">(</span><span class="n">vec3</span> <span class="n">a</span><span class="p">,</span> <span class="n">vec3</span> <span class="n">b</span><span class="p">)</span> <span class="p">{</span>
<span class="kt">float</span> <span class="n">k</span> <span class="o">=</span> <span class="mf">1.0</span><span class="o">+</span><span class="n">dot</span><span class="p">(</span><span class="n">a</span><span class="p">,</span><span class="n">b</span><span class="p">);</span> <span class="c1">// 1+d</span>
<span class="kt">float</span> <span class="n">s</span> <span class="o">=</span> <span class="n">inversesqrt</span><span class="p">(</span><span class="n">k</span><span class="o">+</span><span class="n">k</span><span class="p">);</span> <span class="c1">// 1/sqrt(2+2d)</span>
<span class="k">return</span> <span class="n">vec4</span><span class="p">(</span><span class="n">s</span><span class="o">*</span><span class="n">cross</span><span class="p">(</span><span class="n">a</span><span class="p">,</span><span class="n">b</span><span class="p">),</span> <span class="n">k</span><span class="o">*</span><span class="n">s</span><span class="p">);</span> <span class="c1">// (1+d)/sqrt(2+2d) + (a x b)/sqrt(2+2d)</span>
<span class="p">}</span>
</code></pre></div> </div>
</section>
</div>Johnathon Selstadmakeshifted@gmail.comInverse Kinematics is the process of finding a set of joint angles that reach a goal position.Constraints2019-02-06T20:05:33+00:002019-02-06T20:05:33+00:00https://zalo.github.io/blog/constraints<p>The essence of constraint is projection.</p>
<p><strong>Find the minimum movement that satisfies the constraint.</strong></p>
<h2 id="basic-distance-constraint">Basic Distance Constraint</h2>
<p>The most basic constraint is the distance constraint</p>
<!-- Load the Paper.js library -->
<script type="text/javascript">
document.getElementsByClassName('toc')[0].style.display = 'none';
</script>
<script type="text/javascript" src="../../assets/js/paper-full.min.js"></script>
<script type="text/paperscript" src="../../assets/js/Constraints/SimpleDistance.js" canvas="distance1"></script>
<canvas id="distance1" width="350" height="350"></canvas>
<div class="language-javascript highlighter-rouge"><div class="highlight"><pre class="highlight"><code><span class="kd">function</span> <span class="nx">ConstrainDistance</span><span class="p">(</span><span class="nx">point</span><span class="p">,</span> <span class="nx">anchor</span><span class="p">,</span> <span class="nx">distance</span><span class="p">)</span> <span class="p">{</span>
<span class="k">return</span> <span class="p">((</span><span class="nx">point</span> <span class="o">-</span> <span class="nx">anchor</span><span class="p">).</span><span class="nx">normalize</span><span class="p">()</span> <span class="o">*</span> <span class="nx">distance</span><span class="p">)</span> <span class="o">+</span> <span class="nx">anchor</span><span class="p">;</span>
<span class="p">}</span>
</code></pre></div></div>
<p>It is satisfied by <em>projecting</em> the point onto a circle around the anchor.</p>
<h2 id="distance-constraint-chain">Distance Constraint Chain</h2>
<p>As with all constraints, distance constraints can be chained together</p>
<script type="text/paperscript" src="../../assets/js/Constraints/Chain.js" canvas="distance2"></script>
<canvas id="distance2" width="350" height="350"></canvas>
<div class="togglebox">
<input id="toggle1" type="checkbox" name="toggle" />
<label for="toggle1">Show Code</label>
<section id="content1">
<div class="language-javascript highlighter-rouge"><div class="highlight"><pre class="highlight"><code><span class="c1">//Set the first link's position to be at the mouse</span>
<span class="nx">rope</span><span class="p">.</span><span class="nx">segments</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="nx">mousePos</span><span class="p">;</span>
<span class="k">for</span> <span class="p">(</span><span class="nx">i</span> <span class="o">=</span> <span class="mi">1</span><span class="p">;</span> <span class="nx">i</span> <span class="o"><</span> <span class="nx">segments</span><span class="p">.</span><span class="nx">length</span><span class="p">;</span> <span class="nx">i</span><span class="o">++</span><span class="p">)</span> <span class="p">{</span>
<span class="c1">//Pull the next segment to the previous one</span>
<span class="nx">rope</span><span class="p">.</span><span class="nx">segments</span><span class="p">[</span><span class="nx">i</span><span class="p">]</span> <span class="o">=</span> <span class="nx">ConstrainDistance</span><span class="p">(</span>
<span class="nx">rope</span><span class="p">.</span><span class="nx">segments</span><span class="p">[</span><span class="nx">i</span><span class="p">],</span> <span class="nx">rope</span><span class="p">.</span><span class="nx">segments</span><span class="p">[</span><span class="nx">i</span><span class="o">-</span><span class="mi">1</span><span class="p">],</span> <span class="nx">distance</span>
<span class="p">);</span>
<span class="p">}</span>
</code></pre></div> </div>
<p><a href="https://github.com/zalo/zalo.github.io/blob/master/assets/js/Constraints/Chain.js"><small>Full Source</small></a></p>
</section>
</div>
<p>The order in which constraints are satisfied is important. The ones here are solved stepping away from the mouse, which pulls them <em>towards</em> the mouse.</p>
<h2 id="fabrik-chain">FABRIK Chain</h2>
<p>If the distance constraints are first solved in one direction, and then the other, it creates a form of Inverse Kinematics called “FABRIK”, or “Forwards and Backwards Reaching Inverse Kinematics”.</p>
<script type="text/paperscript" src="../../assets/js/Constraints/FABRIK.js" canvas="distance3"></script>
<canvas id="distance3" width="350" height="350"></canvas>
<div class="togglebox">
<input id="toggle2" type="checkbox" name="toggle" />
<label for="toggle2">Show Code</label>
<section id="content2">
<div class="language-javascript highlighter-rouge"><div class="highlight"><pre class="highlight"><code><span class="c1">//Set the first link's position to be at the mouse</span>
<span class="nx">rope</span><span class="p">.</span><span class="nx">segments</span><span class="p">[</span><span class="mi">0</span><span class="p">]</span> <span class="o">=</span> <span class="nx">mousePos</span><span class="p">;</span>
<span class="k">for</span> <span class="p">(</span><span class="nx">i</span> <span class="o">=</span> <span class="mi">1</span><span class="p">;</span> <span class="nx">i</span> <span class="o"><</span> <span class="nx">segments</span><span class="p">.</span><span class="nx">length</span><span class="p">;</span> <span class="nx">i</span><span class="o">++</span><span class="p">)</span> <span class="p">{</span>
<span class="c1">//Pull the current segment to the previous one</span>
<span class="nx">rope</span><span class="p">.</span><span class="nx">segments</span><span class="p">[</span><span class="nx">i</span><span class="p">]</span> <span class="o">=</span> <span class="nx">ConstrainDistance</span><span class="p">(</span>
<span class="nx">rope</span><span class="p">.</span><span class="nx">segments</span><span class="p">[</span><span class="nx">i</span><span class="p">],</span> <span class="nx">rope</span><span class="p">.</span><span class="nx">segments</span><span class="p">[</span><span class="nx">i</span><span class="o">-</span><span class="mi">1</span><span class="p">],</span> <span class="nx">distance</span>
<span class="p">);</span>
<span class="p">}</span>
<span class="c1">//Set the base link's position to be at the ball</span>
<span class="nx">rope</span><span class="p">.</span><span class="nx">segments</span><span class="p">[</span><span class="nx">segments</span><span class="p">.</span><span class="nx">length</span> <span class="o">-</span> <span class="mi">1</span><span class="p">]</span> <span class="o">=</span> <span class="nx">ball</span><span class="p">;</span>
<span class="k">for</span> <span class="p">(</span><span class="nx">i</span> <span class="o">=</span> <span class="nx">segments</span><span class="p">.</span><span class="nx">length</span> <span class="o">-</span> <span class="mi">1</span><span class="p">;</span> <span class="nx">i</span> <span class="o">></span> <span class="mi">0</span><span class="p">;</span> <span class="nx">i</span><span class="o">--</span><span class="p">)</span> <span class="p">{</span>
<span class="c1">//Pull the previous segment to the current one</span>
<span class="nx">rope</span><span class="p">.</span><span class="nx">segments</span><span class="p">[</span><span class="nx">i</span><span class="o">-</span><span class="mi">1</span><span class="p">]</span> <span class="o">=</span> <span class="nx">ConstrainDistance</span><span class="p">(</span>
<span class="nx">rope</span><span class="p">.</span><span class="nx">segments</span><span class="p">[</span><span class="nx">i</span><span class="o">-</span><span class="mi">1</span><span class="p">],</span> <span class="nx">rope</span><span class="p">.</span><span class="nx">segments</span><span class="p">[</span><span class="nx">i</span><span class="p">],</span> <span class="nx">distance</span>
<span class="p">);</span>
<span class="p">}</span>
</code></pre></div> </div>
<p><a href="https://github.com/zalo/zalo.github.io/blob/master/assets/js/Constraints/FABRIK.js"><small>Full Source</small></a></p>
</section>
</div>
<h2 id="collision-constraint">Collision Constraint</h2>
<p>Distance Constraints can also be used to separate</p>
<script type="text/paperscript" src="../../assets/js/Constraints/Collision.js" canvas="distance4"></script>
<canvas id="distance4" width="350" height="350"></canvas>
<div class="togglebox">
<input id="toggle3" type="checkbox" name="toggle" />
<label for="toggle3">Show Code</label>
<section id="content3">
<div class="language-javascript highlighter-rouge"><div class="highlight"><pre class="highlight"><code><span class="c1">//Separate the balls from the mouse</span>
<span class="nx">float</span> <span class="nx">cRadius</span> <span class="o">=</span> <span class="nx">mRadius</span> <span class="o">+</span> <span class="nx">bRadius</span><span class="p">;</span>
<span class="k">for</span><span class="p">(</span><span class="nx">i</span> <span class="o">=</span> <span class="mi">0</span><span class="p">;</span> <span class="nx">i</span> <span class="o"><</span> <span class="nx">balls</span><span class="p">.</span><span class="nx">length</span><span class="p">;</span> <span class="nx">i</span><span class="o">++</span><span class="p">){</span>
<span class="c1">//If the mouse is closer than some distance</span>
<span class="k">if</span><span class="p">((</span><span class="nx">mousePos</span><span class="o">-</span><span class="nx">balls</span><span class="p">[</span><span class="nx">i</span><span class="p">]).</span><span class="nx">magnitude</span> <span class="o"><</span> <span class="nx">cRadius</span><span class="p">){</span>
<span class="c1">//Push the ball away from the mouse</span>
<span class="nx">balls</span><span class="p">[</span><span class="nx">i</span><span class="p">]</span> <span class="o">=</span> <span class="nx">ConstrainDistance</span><span class="p">(</span><span class="nx">balls</span><span class="p">[</span><span class="nx">i</span><span class="p">],</span> <span class="nx">mousePos</span><span class="p">,</span> <span class="nx">cRadius</span><span class="p">);</span>
<span class="p">}</span>
<span class="p">}</span>
<span class="c1">//Separate the balls from each other</span>
<span class="k">for</span><span class="p">(</span><span class="nx">i</span> <span class="o">=</span> <span class="mi">0</span><span class="p">;</span> <span class="nx">i</span> <span class="o"><</span> <span class="nx">balls</span><span class="p">.</span><span class="nx">length</span><span class="p">;</span> <span class="nx">i</span><span class="o">++</span><span class="p">){</span>
<span class="k">for</span><span class="p">(</span><span class="nx">j</span> <span class="o">=</span> <span class="nx">i</span><span class="p">;</span> <span class="nx">j</span> <span class="o"><</span> <span class="nx">balls</span><span class="p">.</span><span class="nx">length</span><span class="p">;</span> <span class="nx">j</span><span class="o">++</span><span class="p">){</span>
<span class="c1">//If the balls are closer than 2x their radius</span>
<span class="kd">var</span> <span class="nx">curDisplacement</span> <span class="o">=</span> <span class="nx">balls</span><span class="p">[</span><span class="nx">j</span><span class="p">].</span><span class="nx">position</span> <span class="o">-</span> <span class="nx">balls</span><span class="p">[</span><span class="nx">i</span><span class="p">].</span><span class="nx">position</span><span class="p">;</span>
<span class="k">if</span> <span class="p">(</span><span class="nx">curDisplacement</span><span class="p">.</span><span class="nx">magnitude</span> <span class="o"><</span> <span class="nx">bRadius</span><span class="o">*</span><span class="mi">2</span><span class="p">)</span> <span class="p">{</span>
<span class="c1">//Move each ball half of the distance away from the other</span>
<span class="kd">var</span> <span class="nx">temp</span> <span class="o">=</span> <span class="nx">ConstrainDistance</span><span class="p">(</span><span class="nx">balls</span><span class="p">[</span><span class="nx">i</span><span class="p">],</span> <span class="nx">balls</span><span class="p">[</span><span class="nx">j</span><span class="p">],</span> <span class="nx">bRadius</span><span class="p">);</span>
<span class="nx">balls</span><span class="p">[</span><span class="nx">j</span><span class="p">]</span> <span class="o">=</span> <span class="nx">ConstrainDistance</span><span class="p">(</span><span class="nx">balls</span><span class="p">[</span><span class="nx">j</span><span class="p">],</span> <span class="nx">balls</span><span class="p">[</span><span class="nx">i</span><span class="p">],</span> <span class="nx">bRadius</span><span class="p">);</span>
<span class="nx">balls</span><span class="p">[</span><span class="nx">i</span><span class="p">]</span> <span class="o">=</span> <span class="nx">temp</span><span class="p">;</span>
<span class="p">}</span>
<span class="p">}</span>
<span class="p">}</span>
</code></pre></div> </div>
<p><a href="https://github.com/zalo/zalo.github.io/blob/master/assets/js/Constraints/Collision.js"><small>Full Source</small></a></p>
</section>
</div>
<h2 id="collision-constraints-with-verlet">Collision Constraints with Verlet</h2>
<p>If the constraints act symmetrically (where each participant steps half-way towards satisfying the constraint), then one can simulate physics by adding momentum with Verlet Integration.</p>
<script type="text/paperscript" src="../../assets/js/Constraints/VerletCollision.js" canvas="distance5">
</script>
<canvas id="distance5" width="350" height="350"></canvas>
<div class="togglebox">
<input id="toggle4" type="checkbox" name="toggle" />
<label for="toggle4">Show Code</label>
<section id="content4">
<div class="language-javascript highlighter-rouge"><div class="highlight"><pre class="highlight"><code><span class="k">for</span><span class="p">(</span><span class="nx">i</span> <span class="o">=</span> <span class="mi">0</span><span class="p">;</span> <span class="nx">i</span> <span class="o"><</span> <span class="nx">balls</span><span class="p">.</span><span class="nx">length</span><span class="p">;</span> <span class="nx">i</span><span class="o">++</span><span class="p">){</span>
<span class="c1">//-*Use Verlet Integration to add inertia*-</span>
<span class="kd">var</span> <span class="nx">curPosition</span> <span class="o">=</span> <span class="nx">balls</span><span class="p">[</span><span class="nx">i</span><span class="p">];</span>
<span class="nx">balls</span><span class="p">[</span><span class="nx">i</span><span class="p">]</span> <span class="o">+=</span> <span class="nx">balls</span><span class="p">[</span><span class="nx">i</span><span class="p">]</span> <span class="o">-</span> <span class="nx">prevBalls</span><span class="p">[</span><span class="nx">i</span><span class="p">];</span>
<span class="nx">prevBalls</span><span class="p">[</span><span class="nx">i</span><span class="p">]</span> <span class="o">=</span> <span class="nx">curPosition</span><span class="p">;</span>
<span class="c1">//Exert gravity by translating downwards one pixel each frame</span>
<span class="nx">balls</span><span class="p">[</span><span class="nx">i</span><span class="p">]</span> <span class="o">+=</span> <span class="k">new</span> <span class="nx">Point</span><span class="p">(</span><span class="mi">0</span><span class="p">,</span> <span class="mi">1</span><span class="p">);</span>
<span class="p">}</span>
<span class="k">for</span><span class="p">(</span><span class="nx">iterations</span> <span class="o">=</span> <span class="mi">0</span><span class="p">;</span> <span class="nx">iterations</span> <span class="o"><</span> <span class="mi">5</span><span class="p">;</span> <span class="nx">iterations</span><span class="o">++</span><span class="p">){</span>
<span class="c1">//The previous example's code here!</span>
<span class="c1">//It must be iterated to fully resolve all collisions</span>
<span class="p">}</span>
</code></pre></div> </div>
<p><a href="https://github.com/zalo/zalo.github.io/blob/master/assets/js/Constraints/VerletCollision.js"><small>Full Source</small></a></p>
</section>
</div>
<h2 id="verlet-rope">Verlet Rope</h2>
<p>Solving constraints sequentially is called the <em>Gauss-Seidel Method</em>. It converges faster, but it is not technically correct.</p>
<script type="text/paperscript" src="../../assets/js/Constraints/RedRope.js" canvas="redRope"></script>
<canvas id="redRope" width="350" height="350"></canvas>
<p><a href="https://github.com/zalo/zalo.github.io/blob/master/assets/js/Constraints/RedRope.js"><small>Full Source</small></a></p>
<h2 id="volume-preserving-soft-body">Volume Preserving Soft Body</h2>
<p>The alternative is to average the contributions from each constraint before applying them. This is the <em>Jacobi Method</em>. It is more stable, and makes it so the order does not matter. However, it is “squishier” because it converges more slowly.</p>
<p>If one wraps the rope above into a circle, and constrains the shape’s volume, one can create a volume preserving soft-body</p>
<script type="text/paperscript" src="../../assets/js/Constraints/VolumeBlob.js" canvas="softBody"></script>
<canvas id="softBody" width="350" height="350"></canvas>
<p><a href="https://github.com/zalo/zalo.github.io/blob/master/assets/js/Constraints/VolumeBlob.js"><small>Full Source</small></a></p>
<p>The Jacobi Method is useful for keeping phantom forces from appearing in complex systems like this one.</p>
<p>This light introduction to constraints is the first in (hopefully) a series of blog posts exploring the power of simple mathematics.</p>Johnathon Selstadmakeshifted@gmail.comThe essence of constraint is projection.