【S03】丸み不均一スプライン

　
この記事はアドカレに参加しています。

丸み不均一スプラインとは

　丸み不均一スプラインは、通常のスプライン曲線とは違う2つの特徴を持ちます。「丸み」と、「不均一」です。そのままですね。

　「丸み」は、点毎にある速度ベクトルを丸みを帯びたものにすることからきています。

　「不均一」は、2つの点から成る区間の長さに応じて通過時間が不均一になることからきています。2つの点の距離が長ければ、通過時間は長くなります。2つの点の距離が短ければ、通過時間は短くなります。区間ごとに通過時間は不均一です。

三次関数の式

　丸み不均一スプラインでは、2つの点とそれぞれの点に対応する2つの速度ベクトルから三次関数の式を求めます。

上図では、$p_{0}$と$p_{1}$の2点と、$p_{0}$の速度ベクトル$v_{0}$と、$p_{1}$の速度ベクトル$v_{1}$から三次関数を求めています。

　三次関数の式は係数$a～d$を用いて以下のように表すことができます。

y=at^3+bt^2+ct+d　　(0 \leqq t \leqq 1)

$t=0$のとき$y=p_{0}$、$t=1$のとき$y=p_{1}$です。

係数d

係数$d$

$t=0$のとき$y=p_{0}$なので、

\begin{align}
y&=at^3+bt^2+ct+d \\
p_{0}&=a(0)^3+b(0)^2+c(0)+d \\
p_{0}&=d \\
\end{align}

よって、$d=p_{0}$となります。

係数c

係数$c$

速度ベクトルは、三次関数の微分と考えることができるので、一次微分を考えます。

\begin{align}
y&=at^3+bt^2+ct+d \\
\frac{dy}{dt}&=3at^2+2bt+c
\end{align}

$t=0$のとき$y=p_{0}$なので、速度ベクトルは$v_{0}$です。

\begin{align}
\frac{dy}{dt}&=3at^2+2bt+c \\
v_{0}&=3a(0)^2+2b(0)+c \\
v_{0}&=c
\end{align}

よって、$c=v_{0}$となります。

係数b

係数$b$

$t=1$のとき$y=p_{1}$なので、

\begin{align}
y&=at^3+bt^2+ct+d \\
p_{1}&=a(1)^3+b(1)^2+c(1)+d \\
p_{1}&=a+b+c+d \\
b&=p_{1}-a-c-d
\end{align}

よって、$b=p_{1}-a-c-d$となります。

係数a

係数$a$

速度ベクトルは、三次関数の微分と考えることができるので、一次微分を考えます。

\begin{align}
y&=at^3+bt^2+ct+d \\
\frac{dy}{dt}&=3at^2+2bt+c
\end{align}

$t=1$のとき$y=p_{1}$なので、速度ベクトルは$v_{1}$です。

\begin{align}
\frac{dy}{dt}&=3at^2+2bt+c \\
v_{1}&=3a(1)^2+2b(1)+c \\
v_{1}&=3a+2b+c \\
3a&=v_{1}-2b-c \\
a&=\frac{v_{1}-2b-c}{3}
\end{align}

よって、$a=\frac{v_{1}-2b-c}{3}$となります。

すべての係数

　求めたすべての係数をまとめると以下のようになります。

\begin{align}
d&=p_{0} \\
c&=v_{0} \\
b&=p_{1}-a-c-d \\
a&=\frac{v_{1}-2b-c}{3}
\end{align}

$d$と$c$はそのままで大丈夫ですが、$b$と$a$は変形が必要です。

係数$b$と係数$a$の変形

\begin{align}
b&=p_{1}-a-c-d \\
a&=\frac{v_{1}-2b-c}{3}
\end{align}

$d$と$c$を$p_{0}$と$v_{0}$に置き換えます。

\begin{align}
b&=p_{1}-a-v_{0}-p_{0} \\
a&=\frac{v_{1}-2b-v_{0}}{3}
\end{align}

$a$と$b$の連立方程式になりましたね。$b$の式に$a=\frac{v_{1}-2b-v_{0}}{3}$を代入して式整理します。

\begin{align}
b&=p_{1}-\frac{v_{1}-2b-v_{0}}{3}-v_{0}-p_{0} \\
3b&=3p_{1}-(v_{1}-2b-v_{0})-3v_{0}-3p_{0} \\
3b&=3p_{1}-v_{1}+2b+v_{0}-3v_{0}-3p_{0} \\
3b-2b&=-3p_{0}+v_{0}-3v_{0}+3p_{1}-v_{1} \\
b&=-3p_{0}-2v_{0}+3p_{1}-v_{1}
\end{align}

今度は$a$に$b=-3p_{0}-2v_{0}+3p_{1}-v_{1}$を代入して式整理します。

\begin{align}
a&=\frac{v_{1}-2b-v_{0}}{3} \\
a&=\frac{v_{1}-2(-3p_{0}-2v_{0}+3p_{1}-v_{1})-v_{0}}{3} \\
3a&=v_{1}-2(-3p_{0}-2v_{0}+3p_{1}-v_{1})-v_{0} \\
3a&=v_{1}+6p_{0}+4v_{0}-6p_{1}+2v_{1}-v_{0} \\
3a&=6p_{0}+4v_{0}-v_{0}-6p_{1}+v_{1}+2v_{1} \\
3a&=6p_{0}+3v_{0}-6p_{1}+3v_{1} \\
a&=2p_{0}+v_{0}-2p_{1}+v_{1}
\end{align}

すべての係数を$p_{0}$、$v_{0}$、$p_{1}$、$v_{1}$で表わすと以下のようになります。

\begin{align}
d&=p_{0} \\
c&=v_{0} \\
b&=-3p_{0}-2v_{0}+3p_{1}-v_{1} \\
a&=2p_{0}+v_{0}-2p_{1}+v_{1}
\end{align}

行列

　求めた式から、行列を考えてみましょう。

\begin{align}
y&=at^3+bt^2+ct+d \\
d&=p_{0} \\
c&=v_{0} \\
b&=-3p_{0}-2v_{0}+3p_{1}-v_{1} \\
a&=2p_{0}+v_{0}-2p_{1}+v_{1}
\end{align}

まずは、$y=at^3+bt^2+ct+d$に$a～d$のそれぞれの係数を代入します。

\begin{align}
y&=at^3+bt^2+ct+d \\
y&=(2p_{0}+v_{0}-2p_{1}+v_{1})t^3+(-3p_{0}-2v_{0}+3p_{1}-v_{1})t^2+(v_{0})t+(p_{0}) \\
\end{align}

行列には以下のような性質があります。

a_{0}b_{0}+a_{1}b_{1}+a_{2}b_{2}+a_{3}b_{3}
=
\begin{bmatrix}a_{0} & a_{1} & a_{2} & a_{3} \end{bmatrix}
\begin{bmatrix}b_{0} \\ b_{1} \\ b_{2} \\ b_{3} \end{bmatrix}

これを$y$の式にも適用します。

\begin{align}
y&=(2p_{0}+v_{0}-2p_{1}+v_{1})t^3+(-3p_{0}-2v_{0}+3p_{1}-v_{1})t^2+(v_{0})t+(p_{0}) \\
y&=\begin{bmatrix}t^3 & t^2 & t & 1 \end{bmatrix}
\begin{bmatrix}
2p_{0}+v_{0}-2p_{1}+v_{1} \\
-3p_{0}-2v_{0}+3p_{1}-v_{1} \\
v_{0} \\
p_{0} \end{bmatrix}
\end{align}

さらに適用して、

\begin{align}
y&=\begin{bmatrix}t^3 & t^2 & t & 1 \end{bmatrix}
\begin{bmatrix}
\begin{bmatrix}2 & 1 & -2 & 1 \end{bmatrix}
\begin{bmatrix} p_{0} \\ v_{0} \\ p_{1} \\ v_{1} \end{bmatrix}
\\
\begin{bmatrix}-3 & -2 & 3 & -1 \end{bmatrix}
\begin{bmatrix} p_{0} \\ v_{0} \\ p_{1} \\ v_{1} \end{bmatrix}
\\
\begin{bmatrix}0 & 1 & 0 & 0 \end{bmatrix}
\begin{bmatrix} p_{0} \\ v_{0} \\ p_{1} \\ v_{1} \end{bmatrix}
\\
\begin{bmatrix}1 & 0 & 0 & 0 \end{bmatrix}
\begin{bmatrix} p_{0} \\ v_{0} \\ p_{1} \\ v_{1} \end{bmatrix}
\end{bmatrix} \\

y&=\begin{bmatrix}t^3 & t^2 & t & 1 \end{bmatrix}
\begin{bmatrix}
\begin{bmatrix}2 & 1 & -2 & 1 \end{bmatrix}\\
\begin{bmatrix}-3 & -2 & 3 & -1 \end{bmatrix}\\
\begin{bmatrix}0 & 1 & 0 & 0 \end{bmatrix}\\
\begin{bmatrix}1 & 0 & 0 & 0 \end{bmatrix}
\end{bmatrix}
\begin{bmatrix} p_{0} \\ v_{0} \\ p_{1} \\ v_{1} \end{bmatrix} \\

y&=\begin{bmatrix}t^3 & t^2 & t & 1 \end{bmatrix}
\begin{bmatrix}
2 & 1 & -2 & 1 \\
-3 & -2 & 3 & -1 \\
0 & 1 & 0 & 0 \\
1 & 0 & 0 & 0 \end{bmatrix}
\begin{bmatrix} p_{0} \\ v_{0} \\ p_{1} \\ v_{1} \end{bmatrix}
\end{align}

ここで、$T$、$A$、$P$を定義して、

\begin{align}
T&=\begin{bmatrix}t^3 & t^2 & t & 1 \end{bmatrix} \\
A&=\begin{bmatrix}
2 & 1 & -2 & 1 \\
-3 & -2 & 3 & -1 \\
0 & 1 & 0 & 0 \\
1 & 0 & 0 & 0 \end{bmatrix} \\
P&=\begin{bmatrix} p_{0} \\ v_{0} \\ p_{1} \\ v_{1} \end{bmatrix}
\end{align}

関数$y$は以下のように書くことができます。

y=TAP

丸み

　点の位置座標と、点の持つ速度ベクトルが分かれば2点間を補完する三次関数がわかります。このときの速度ベクトルを人が一々決めるのは少し面倒です。なにかしらの数値計算で、曲線が「丸み」を出せるような速度ベクトルが自動で決まっていくと楽でいいですよね。

丸み不均一スプラインでは、「丸み」を出せるような速度ベクトルを角の二等分線に垂直なベクトルと定めています。

上図は$p_{0}$と$p_{2}$から、$p_{1}$の持つ速度ベクトル$v_{1}$を求める例です。

$p_{1}$からみた$p_{0}$の相対位置ベクトル$p_{01}$と、$p_{1}$からみた$p_{2}$の相対位置ベクトル$p_{21}$のなす角の二等分線に垂直な方向が、速度ベクトル$v_{1}$の方向となります。

二等分線に垂直な方向というのは、$p_{01}$と$p_{21}$の単位ベクトルの差でもあります。($p_{02}$と$v_{1}$の方向は同じです。)

実際に$v_{1}$を求める式は以下のようになります。

v_{1}=v\times \frac{\hat{p_{21}}-\hat{p_{01}}}{|\hat{p_{21}}-\hat{p_{01}}|}

ここで、$v$は速度ベクトルの持つ速さの大きさです。丸み不均一スプラインではすべての速度ベクトルの大きさ$v$の値を変えることで、曲率を変えることができます。

$p_{n-1}$と$p_{n+1}$に挟まれた$p_{n}$の持つ速度ベクトルは以下のようになります。

\begin{align}
p_{a}&=p_{n-1}-p_{n}　(p_{n-1}の相対位置ベクトル) \\
p_{b}&=p_{n+1}-p_{n}　(p_{n+1}の相対位置ベクトル) \\
v_{n}&=v\times \frac{\hat{p_{b}}-\hat{p_{a}}}{|\hat{p_{b}}-\hat{p_{a}}|}
\end{align}

また、最初の点$p_{0}$と最後の点$p_{m}$は続く値の点が1つしかないので、その点との相対位置ベクトルがそのまま速度ベクトルの方向になります。

\begin{align}
p_{s}&=p_{1}-p_{0}　(p_{0}の相対位置ベクトル) \\
p_{f}&=p_{m}-p_{m-1}　(p_{m}の相対位置ベクトル) \\
v_{0}&=v\times \frac{\hat{p_{s}}}{|\hat{p_{s}}|} \\
v_{m}&=v\times \frac{\hat{p_{f}}}{|\hat{p_{f}}|}
\end{align}

不均一

　すべての区間において、進むスピードが等速になるためには三次関数の長さに応じて、$t$の進むスピードが変わればいいです。三次関数の長さが長い時は、$t$はゆっくり進めます。三次関数の長さが短いときは、$t$は速く進めます。

二次元の座標系での三次関数の長さは以下のような式で表わせます。積分です。

\begin{align}
l&=\int_{0}^{1}\sqrt{x^2+y^2}dt \\
l&=\int_{0}^{1}\sqrt{(TAP_{x})^2+(TAP_{y})^2}dt
\end{align}

正確な$l$を求めることはできませんが、数値積分をすることにより大体の近似値を求めることができます。三次関数の式はそこそこ重いので、2点間の距離で代用しても(誤差に目をつぶれば)問題ないです。

長さ$l$が分かったので、後は媒介変数$t$をスケーリングします。長さ$l=1$に対する途中点の精度$s$、パラメータを$p(0 \leqq p \leqq s\times l)$とすると、

t=\frac{p}{s\times l}　　(0 \leqq t \leqq 1)

のように表すことができます。

プログラム例

　luaで申し訳ないですが、プログラム例です。AviUtl用のスクリプトコードそのままです。「丸み」の部分は実装していますが、「不均一」の部分は未実装です。

@spline2
--track0:anchor,0,16,5,1
--track1:speed,-1000,1000,100,0.01
--check0:map,0
--dialog:set_anchor,pos={};

--------------------------------------------------
--ベクトルを扱うオブジェクトを定義
--------------------------------------------------
local vec={}
--vec.__index=vec
function vec.__index(p,index)
 if(index=="unit")then
  local k=math.sqrt(p.x*p.x+p.y*p.y+p.z*p.z)
  local p1={x=p.x/k,
            y=p.y/k,
            z=p.z/k}
  return setmetatable(p1,vec)
 else
  return p[index]
 end
end

--初期化
local function vec_new(px,py,pz)
 local x={x=px,y=py,z=pz}
 return setmetatable(x,vec)
end

--単位ベクトル(vec.unitよびだしだと上手くいかないときがあるのはどうして...)
local function vec_unit(p)
 local k=math.sqrt(p.x*p.x+p.y*p.y+p.z*p.z)
 local p1={x=p.x/k,
           y=p.y/k,
           z=p.z/k}
 return setmetatable(p1,vec)
end

--足し算
function vec.__add(p1,p2)
 local p3={x=p1.x+p2.x,
           y=p1.y+p2.y,
           z=p1.z+p2.z}
 return setmetatable(p3,vec)
end

--引き算
function vec.__sub(p1,p2)
 local p3={x=p1.x-p2.x,
           y=p1.y-p2.y,
           z=p1.z-p2.z}
 return setmetatable(p3,vec)
end

--掛け算
function vec.__mul(p1,p2)
 local p3={x=p1.x*p2.x,
           y=p1.y*p2.y,
           z=p1.z*p2.z}
 return setmetatable(p3,vec)
end
--------------------------------------------------
--------------------------------------------------


--------------------------------------------------
--速度ベクトルを求める関数
--------------------------------------------------
local function v_M(pos,v)

local k={}
local s={}
for i=1,#pos do
 s[i]={}
 if(i==1)then
  k=vec_new(pos[2].x-pos[1].x,
            pos[2].y-pos[1].y,
            pos[2].z-pos[1].z).unit
  s[1]=k
 elseif(i==#pos)then
  s[i]=k
 else
  local m=vec_new(pos[i+1].x-pos[i].x,
                  pos[i+1].y-pos[i].y,
                  pos[i+1].z-pos[i].z).unit
  s[i]=(m+k).unit
  k=m
 end
end

local c=vec_new(v,v,v)
for i=1,#pos do
 s[i]=s[i]*c
end

return s
end
--------------------------------------------------
--------------------------------------------------


--------------------------------------------------
--三次関数の係数を求める関数
--------------------------------------------------
local function pv_M(data,v)

local p={}
for i=0,math.floor(#data/3)-1 do
 p[i+1]=vec_new(data[i*3+1],data[i*3+2],data[i*3+3])
end

local sv=v_M(p,v)

local sol={}
local dn=vec_new(2,2,2)
local ds=vec_new(3,3,3)
for i=1,#sv-1 do
 sol[i]={}
 sol[i].a=dn*(p[i]-p[i+1])+sv[i]+sv[i+1]
 sol[i].b=ds*(p[i+1]-p[i])-dn*sv[i]-sv[i+1]
 sol[i].c=sv[i]
 sol[i].d=p[i]
end

local send_data
if(obj.check0)then send_data={p,sv} end

return sol,send_data
end
--------------------------------------------------
--------------------------------------------------


--------------------------------------------------
--三次式を求める関数
--------------------------------------------------
local function cM(a,b,c,d,t)
 return ((a*t+b)*t+c)*t+d
end
--------------------------------------------------
--------------------------------------------------


--------------------------------------------------
--93さん製の線描画関数。https://discord.com/channels/957447213636784149/986586170681073744/1114490046528094298の応用というか三次元ver.考えたくない
--------------------------------------------------
local polyline = function(
p0, --点座標0
p1, --点座標1
width, --線幅(省略可)
color, --線色(省略可)
alpha --透明度(省略可)
)
local g=obj.getvalue
local w,h=0,0
if color then
obj.load("figure","四角形",color,1)
else
w,h=obj.getpixel()
end
alpha = alpha or 1
width = width or 1
local c = obj.getoption("camera_param")
local a = {p1[1]-p0[1], p1[2]-p0[2], p1[3]-p0[3]}
local b = {c.x-p0[1], c.y-p0[2], c.z-p0[3]}
local n = {a[2]*b[3]-a[3]*b[2],a[3]*b[1]-a[1]*b[3],a[1]*b[2]-a[2]*b[1]}
local l = math.sqrt(n[1]*n[1] + n[2]*n[2] + n[3]*n[3])
local nx,ny,nz = (n[1]/l)*width*.5, (n[2]/l)*width*.5 ,(n[3]/l)*width*.5
local x0,y0,z0 = p0[1]-nx,p0[2]-ny,p0[3]-nz
local x1,y1,z1 = p1[1]-nx,p1[2]-ny,p1[3]-nz
local x2,y2,z2 = p1[1]+nx,p1[2]+ny,p1[3]+nz
local x3,y3,z3 = p0[1]+nx,p0[2]+ny,p0[3]+nz
obj.drawpoly(x0,y0,z0, x1,y1,z1, x2,y2,z2, x3,y3,z3,0,0,w,0,w,h,0,h,alpha)
end
--------------------------------------------------
--------------------------------------------------


--------------------------------------------------
--main
--------------------------------------------------

obj.setanchor("pos",obj.track0,"xyz")

local s,send_data=pv_M(pos,obj.track1)

obj.load("figure","六角形",0xff,5)
for i=1,#s do
 local t=vec_new(0,0,0)
 local dt=vec_new(0.01,0.01,0.01)
 for d=0,99 do
  local p=cM(s[i].a,s[i].b,s[i].c,s[i].d,t)
  obj.draw(p.x,p.y,p.z)
  t=t+dt
 end
end

if(send_data)then
 local p=send_data[1]
 local v=send_data[2]
 for i=1,#p do
  obj.load("figure","六角形",0xff0000,10)
  obj.draw(p[i].x,p[i].y,p[i].z)
  obj.draw(p[i].x+v[i].x,
           p[i].y+v[i].y,
           p[i].z+v[i].z)
  local p0={p[i].x,p[i].y,p[i].z}
  local p1={p[i].x+v[i].x,
            p[i].y+v[i].y,
            p[i].z+v[i].z}
  polyline(p0,p1)
 end
end
--------------------------------------------------
--------------------------------------------------

参考文献

その34 スプライン曲線上をおおよそ等速で移動する（丸み不均一スプライン）

むすび

　丸み不均一スプラインの説明でした。