4つの制御点で描くクロソイド曲線
4点（3辺）の制御点でクロソイド曲線→円弧→クロソイド曲線の3セグメントのカーブを描画します。
クロソイドカーブの基本についてはこのページを参考に クロソイド曲線について
場合分け
平行の場合
P0-P1とP2-P3が平行の場合。
Θ = 180° – (tau1 + tau2)
前方で交点をつくる場合
P1とP2の先で交点がつくられる場合。
Θ = 180° – I – (tau1 + tau2)
後方で交点をつくる場合
P0とP3の先で交点がつくられる場合。
Θ = I + 180° – (tau1 + tau2)
コード
//
// ガイドカーブをもとにクロソイド曲線を生成する（4ポイント）
// Run Over: Primitives
// input0: Polyline(Control Points)
//
// クロソイドと円弧のおおよその割合
float angleRatio = 0.25;
// クロソイド曲線を描く間隔(m)
float resampleLength = 0.1;
//
// 直線の交点を求める関数
//
vector CrossPointXY(vector p0; vector p1; vector p2; vector p3)
{
float a1 = p0.y - p1.y;
float b1 = p1.x - p0.x;
float c1 = (p1.x - p0.x) * p0.y - (p1.y - p0.y) * p0.x;
float a2 = p2.y - p3.y;
float b2 = p3.x - p2.x;
float c2 = (p3.x - p2.x) * p2.y - (p3.y - p2.y) * p2.x;
float x = (c1 * b2 - c2 * b1) / (a1 * b2 - a2 * b1);
float y = (a1 * c2 - a2 * c1) / (a1 * b2 - a2 * b1);
return set(x, y, 0);
}
// クロソイド曲線のX成分を求める関数(tauとAから)
function float clothoid_x(float tau; float A)
{
return A / sqrt(2) * 2 * sqrt(tau) * (1 - (pow(tau, 2) / 10.0) + (pow(tau, 4) / 216.0) - (pow(tau, 6) / 9360.0));
}
// クロソイド曲線のY成分を求める関数
function float clothoid_y(float tau; float A)
{
return A / sqrt(2) * (2/3.0) * sqrt(tau) * tau * (1 - (pow(tau, 2) / (42.0 / 3.0)) + (pow(tau, 4) / (1320.0 / 3.0)) - (pow(tau, 6) / (75600.0 / 3.0)) );
}
//
// プリミティブごとにクロソイドを描く
//
int npts[] = primpoints(0, @primnum);
// p0-p1を底辺とした行列をつくり、XY平面の原点へ変換する
vector p0 = point(0, "P", npts[0]);
vector p1 = point(0, "P", npts[1]);
vector p2 = point(0, "P", npts[2]);
vector p3 = point(0, "P", npts[3]);
vector tangent = normalize(p1 - p0);
vector normal = normalize(cross(tangent, p2 - p1));
vector binormal = cross(normal, tangent);
matrix worldPlane = maketransform(normal, binormal, p0);
matrix inversePlane = invert(worldPlane);
p0 *= inversePlane;
p1 *= inversePlane;
p2 *= inversePlane;
p3 *= inversePlane;
vector v0 = normalize(p1 - p0);
vector v1 = normalize(p2 - p3);
vector cross = CrossPointXY(p1, p1 + v0, p2, p2 + v1);
int debug = 0;
if(debug)
{
int primGuid = addprim(0, "polyline");
int pt = addpoint(0, p0);
addvertex(0, primGuid, pt);
pt = addpoint(0, p1);
addvertex(0, primGuid, pt);
pt = addpoint(0, p2);
addvertex(0, primGuid, pt);
pt = addpoint(0, p3);
addvertex(0, primGuid, pt);
addpoint(0, cross);
}
// 交角
float I = acos(dot(v0, v1));
//printf('I:' + sprintf('%g', degrees(I)) + '\n');
//
// 各パラメータを計算する
//
float R = 1;
float tau1, tau2;
float A1, A2;
float L1, L2;
float theta;
//
// Θを場合分けで計算する
//
vector left = cross(set(0,0,1), v0);
// 平行
if(dot(v0, v1) > 0.999999)
{
float I2 = PI;
tau1 = I2 * angleRatio/2;
tau2 = I2 * angleRatio/2;
theta = PI - (tau1 + tau2);
}
// 交わる先が鋭角で、交点がp0, p1の前方で交わっている場合
else if(dot(left, v1) < 0)
{
float I2 = PI - I;
tau1 = I2 * angleRatio/2;
tau2 = I2 * angleRatio/2;
theta = PI - I - (tau1 + tau2);
//printf('tau1:' + sprintf('%g', degrees(tau1)) + '\n');
//printf('I2:' + sprintf('%g', degrees(I2)) + '\n');
//printf('theta:' + sprintf('%g', degrees(theta)) + '\n');
//printf('type:0' + '\n');
}
// 交わる先が鋭角で、交点がp0, p1の後方で交わっている場合
else
{
float I2 = PI - I;
tau1 = I2 * angleRatio/2;
tau2 = I2 * angleRatio/2;
theta = I + PI - (tau1 + tau2);
//printf('theta:' + sprintf('%g', degrees(theta)) + '\n');
//printf('type:1' + '\n');
}
A1 = R * sqrt(tau1*2);
L1 = A1 * A1 / R;
A2 = R * sqrt(tau2*2);
L2 = A2 * A2 / R;
// 接線と同心円の接する座標（L1と同心円の接点）
float x0 = clothoid_x(tau1, A1);
float y0 = clothoid_y(tau1, A1);
//
// 原点にクロソイド曲線を描画する
//
// クロソイド曲線(L1)
vector curve_pos0[];
int num = max(int(L1 / resampleLength), 2);
for(int i = 0; i < num; i++)
{
float t = tau1 / float(num-1) * i;
float x = clothoid_x(t, A1);
float y = clothoid_y(t, A1);
curve_pos0[i] = set(x, y, 0);
}
// 円弧
vector vecTau1 = set(cos(tau1), sin(tau1), 0); // 円の中心座標と接線ベクトル
vector C = set(x0, y0, 0) + cross(set(0,0,1), vecTau1) * R;
vector diff = curve_pos0[-1] - C;
float startRot = atan2(diff.y, diff.x);
float arcPosLength = R * theta;
num = max(int(arcPosLength / resampleLength), 2);
float perAngle = theta / float(num);
vector arcPos[];
for(int i = 0; i < num+1; i++)
{
float angle = startRot + perAngle * i;
arcPos[i] = set(cos(angle), sin(angle), 0) * R + C;
}
// クロソイド曲線(L2)
vector curve_pos1[];
num = max(int(L2 / resampleLength), 2);
for(int i = 0; i < num; i++)
{
float t = tau2 / float(num-1) * i;
float x = clothoid_x(t, A2);
float y = clothoid_y(t, A2);
curve_pos1[i] = set(x, y, 0);
}
// 片方のクロソイド曲線を反転させる行列
matrix mirrorWorld = maketransform(set(0,0,1), set(0,1,0), set(0,0,0));
scale(mirrorWorld, set(-1, 1, 1));
translate(mirrorWorld, set(curve_pos1[-1].x, -curve_pos1[-1].y, 0));
rotate(mirrorWorld, tau1+theta+tau2, set(0,0,1)); // 交角
translate(mirrorWorld, arcPos[-1]);
for(int i = 0; i < num; i++)
curve_pos1[i] *= mirrorWorld;
//
// ガイド行列を場合分けで計算する
//
matrix guidWorld = maketransform(set(0,0,1), set(0,1,0), set(0,0,0));
float scale = 1;
int drawExtend0 = 0;
int drawExtend1 = 0;
// 平行
if(dot(v0, v1) > 0.999999)
{
vector local_p3 = curve_pos1[0];
// 底辺のほうが長い場合
float offset = 0;
scale = p3.y / local_p3.y;
scale(guidWorld, set(scale, scale, scale));
if(p0.x < p3.x) drawExtend0 = 1;
else drawExtend1 = 1;
if(p0.x < p3.x)
{
offset = p3.x - p0.x;
translate(guidWorld, set(offset, 0, 0));
}
}
// 交わる先が鋭角で、交点がp0, p1の前方で交わっている場合
//else if(dot(v1, (cross - p1)) > 0 && dot(v0, (cross - p0)) > 0)
else if(dot(left, v1) < 0)
{
// ローカルのガイドを描く
vector local_p0 = curve_pos0[0];
vector local_p3 = curve_pos1[0];
vector local_p4 = CrossPointXY(set(0,0,0), set(1,0,0), local_p3, local_p3 + (p3 - p2));
// ローカルのガイドは余白がない、元のガイドは余白を含むのでそこを比較する
float localRatio = length(local_p4-local_p3)/length(local_p4-local_p0);
float ratio = length(cross-p3)/length(cross-p0);
// 底辺のほうが長い場合
float offset = 0;
if(ratio < localRatio) drawExtend0 = 1;
else drawExtend1 = 1;
if(ratio < localRatio)
{
scale = length(cross-p3)/length(local_p4-local_p3);
offset = length(cross-p0)-length(local_p4-local_p0)*scale;
scale(guidWorld, set(scale, scale, scale));
translate(guidWorld, set(offset, 0, 0));
}
else
{
scale = length(cross-p0)/length(local_p4-local_p0);
scale(guidWorld, set(scale, scale, scale));
}
}
// 交わる先が鋭角で、交点がp0, p1の後方で交わっている場合
else
{
// ローカルのガイドを描く
vector local_p0 = curve_pos0[0];
vector local_p3 = curve_pos1[0];
vector local_p4 = CrossPointXY( set(0,0,0), set(-1,0,0), local_p3, local_p3 + (p3 - p2));
// ローカルのガイドは余白がない、元のガイドは余白を含むのでそこを比較する
float localRatio = length(local_p4-local_p0)/length(local_p4-local_p3);
float ratio = length(cross-p0)/length(cross-p3);
if(ratio < localRatio) drawExtend0 = 1;
else drawExtend1 = 1;
// 底辺のほうが長い場合
float offset = 0;
if(ratio < localRatio)
{
scale = length(cross-p3)/length(local_p4-local_p3);
offset = length(cross-p0)-length(local_p4-local_p0)*scale;
scale(guidWorld, set(scale, scale, scale));
translate(guidWorld, set(offset*-1, 0, 0));
}
else
{
scale = length(cross-p0)/length(local_p4-local_p0);
scale(guidWorld, set(scale, scale, scale));
}
}
//
// ポリラインを描画する
//
// 端（直線）
if(drawExtend0 == 1)
{
int prim = addprim(0, "polyline");
int pt = addpoint(0, p0 * worldPlane);
addvertex(0, prim, pt);
pt = addpoint(0, curve_pos0[0] * guidWorld * worldPlane);
addvertex(0, prim, pt);
}
// クロソイド
int prim = addprim(0, "polyline");
for(int i = 0; i < len(curve_pos0); i++)
{
int pt = addpoint(0, curve_pos0[i] * guidWorld * worldPlane);
addvertex(0, prim, pt);
//setpointattrib(0, "Cd", pt, set(1, 0.5, 0));
setprimgroup(0, "__clothoid", prim, 1);
}
// 円弧
prim = addprim(0, "polyline");
for(int i = 0; i < len(arcPos); i++)
{
int pt = addpoint(0, arcPos[i] * guidWorld * worldPlane);
addvertex(0, prim, pt);
//setpointattrib(0, "Cd", pt, set(0.5, 1, 0));
setprimgroup(0, "__arc", prim, 1);
}
// クロソイド
prim = addprim(0, "polyline");
curve_pos1 = reverse(curve_pos1);
for(int i = 0; i < len(curve_pos1); i++)
{
int pt = addpoint(0, curve_pos1[i] * guidWorld * worldPlane);
addvertex(0, prim, pt);
//setpointattrib(0, "Cd", pt, set(1, 0.5, 0));
setprimgroup(0, "__clothoid", prim, 1);
}
// 端（直線）
if(drawExtend1 == 1)
{
prim = addprim(0, "polyline");
int pt = addpoint(0, curve_pos1[-1] * guidWorld * worldPlane);
addvertex(0, prim, pt);
pt = addpoint(0, p3 * worldPlane);
addvertex(0, prim, pt);
}
// 元のポリラインを消す
removeprim(0, @primnum, 1);
3点ガイドのクロソイドカーブと組み合わせたもの。ガイドのポリラインは奥行に歪みのない4角形にする必要がある。