事の発端
クロソイドに対して平行な線を描きたい、なるべく自分の思惑通りに。
具体的には、
・単位距離を極小化しなくともある程度ズレない
・平行な曲線も可能な限りそれ単体で定量的に描く
巷に見かけるクロソイドの実装が、現在方位のみしか考えていないため、結構目に見えてズレていく(※記事を書いたのは最近ですが、もともと自分がクロソイド式を触ったのは結構昔なので、現在の事実と齟齬があるかもしれません)。
また、平行な曲線の描き方として、簡易的には、クロソイドを描き、そこから垂線を伸ばして座標を描けば達成できる方法が知られている。
しかし、そうやって求めた座標列の区間幅は均一ではなく、均一にするには多数の座標を求めて追い込んでいくといった高コストな作業になる。
この２つがイヤだったので、式をこねくり回してみました。
個人メモなので、胡散臭いことが記されています。
また、処理コストなんて考えてない状態です。
記事の元ネタは数年前に作っているので、今はもっとよい記事があるかもしれませんが、調査していません。
実装
#include <vector>
#include <cmath>
#include <stdexcept>
struct point_t{
double x_, y_;
};
/**
* @brief クロソイドに対して平行な曲線を、指定単位距離で描くための関数
* あくまで描画の正確性を保つためなので、求めた座標列から距離を逆算することには適していない
* @param base クロソイドの開始位置
* @param length クロソイドの長さ
* @param hs クロソイドの開始地点の方位
* @param cs クロソイドの開始地点曲率
* @param ce クロソイドの終了地点曲率
* @param drawunit 演算距離単位 0
* @param r クロソイドに対して、実際に描きたい曲線が何[単位]横移動した位置か(0=完全なクロソイド)
* @return std::vector<point_t>
*/
static std::vector<point_t> draw_clothoid(
const point_t& base,
double length,
double hs, double cs, double ce,
double drawunit,
double r = 0
){
std::vector<point_t> ret;
// パラメータチェック
if (length <= 0.0 || drawunit <= 0 || 1.0 < drawunit ){
// 距離が0以下は描く価値がない。
// 描く単位距離は精度を保つ都合で0～1.0とする
// 1.0以上にしたい場合(例えば1.5)、その値を1.0以下になる値で整数値で割って求め、求めた座標列から引いてくれ。
throw std::logic_error("invalid parameter(s).");
}
if (cs * r > 0){
// 座標系がどうかはともかく、曲率と平行移動距離を掛け合わせて1を超えることはあってはならない
// 曲率を半径に変換して比較した際、「円の中心を突き抜けて逆に発生してしまう、これは正しくない」ため。
// 実際のところ、cs * r >= 1 の判定でいいとおもう。
if (std::abs(1.0 / cs) <= std::abs(r)){
throw std::logic_error("invalid parameters, r and cs.");
}
}
if (ce * r > 0){
// 同上
if (std::abs(1.0 / ce) <= std::abs(r)){
throw std::logic_error("invalid parameters, r and ce.");
}
}
// 逐次導出用変数群
point_t cur_point = base;
double cur_heading = hs;
double cur_pos = 0;
if (r != 0.0){
// 始点の調整
// ※半径という存在と伸ばす方向のわかりやすさの都合で
// 面倒な数式にしている
auto asin_v = std::asin(double(1.0 - (r > 0.0) * 2.0));
cur_point.x_ += std::abs(r) * std::cos(hs + asin_v);
cur_point.y_ += std::abs(r) * std::sin(hs + asin_v);
// 単純には以下でいいはず。試してないので符号間違ってるかも
//cur_point.x_ +=(std::sin(hs) * r);
//cur_point.y_ -=(std::cos(hs) * r);
};
// max_length = これから描く曲線の全長
double dc = (ce - cs) / length;
double f_a, f_b;
double max_length = length;
if (r != 0.0){
f_a = 0.5 * dc * r;
f_b = 1 + r * cs;
// 定積分
max_length = (f_a * length + f_b) * length;
};
// 描画「クロソイドに対して平行な曲線を単位距離毎に求める」
ret.push_back(cur_point);
while (cur_pos < max_length) {
if (cur_pos + drawunit > max_length) {
// 単位距離の調整
drawunit = max_length - cur_pos;
cur_pos = max_length;
}
else {
cur_pos += drawunit;
};
// クロソイド上での距離に換算
double base_pos = cur_pos;
if (r != 0.0) {
// 二次方程式の解
// この式はrが作用しないと導出できない(よって、成立しない)
double f_c = -cur_pos;
base_pos = (-f_b + std::sqrt(f_b * f_b - 4 * f_a * f_c)) / (2 * f_a);
}
// 方位の差分を取るためのバックアップ
double prv_heading = cur_heading;
// 方位。クロソイドで積分といえば本来これ
// dcを用いずcs,ce,lengthを用いる方が計算誤差(情報の劣化)を抑えられるが、とりあえず
cur_heading = hs + (cs + 0.5 * dc * base_pos)* base_pos;
// 接線変化規模
// クロソイドに平行な曲線の方位とクロソイド上の方位は一致しているので、
// クロソイド上で方位を求めたものはR平行移動した地点でも利用できる
// 一方、R平行移動した地点には曲率Cを直接は利用しがたい。
// よって、次点の位置の計算には接線の変化量を利用する。
double diff_heading = cur_heading - prv_heading;
// 次点の発生方位(円弧近似として求める)
double direct_vector = prv_heading + diff_heading * 0.5;
// 次点までの直線距離(円弧近似として求める)
// 初期値は直線の場合の値
double direct_distance = drawunit;
if (diff_heading != 0){
// 円の中心角＝接線(進行方向)の変化量
double angle = diff_heading;
// 半径を求める
// drawunitは「半径x円の中心角=弧の長さ」の「弧の長さ」に相当する。
// 直線に近いと結構危ない部分だが、
// 「極小の変化を用いる実例があるか」というと、
// クロソイドを使う上でイメージできないため無視している
double radius = drawunit / angle;
// 弦の長さ＝次点までの直線距離を求める
direct_distance = radius * std::sin(angle * 0.5) * 2;
}
// 座標算出
cur_point.x_ += std::cos(direct_vector)*direct_distance;
cur_point.y_ += std::sin(direct_vector)*direct_distance;
ret.push_back(cur_point);
}
return ret;
};
使い方
auto output_points = [](std::vector<point_t>& rets)->void {
for (auto& i : rets) {
std::cout << i.x_ << "," << i.y_ << std::endl;
};
std::cout << std::endl;
};
point_t base; // クロソイドの始点
double cs = -0.2; // クロソイドの開始位置曲率
double ce = 0.2; // クロソイドの終了位置曲率
double l = 100.0; // クロソイドの全長
double hs = 5.0; // クロソイドの初期方位
base.x_ = 0;
base.y_ = 0;
// クロソイド
auto lbs = draw_clothoid(base, l, hs, cs, ce, 0.5, 0.0);
output_points(lbs);
// クロソイドに距離3で平行に描かれる曲線
auto lp3 = draw_clothoid(base, l, hs, cs, ce, 0.5, 3.0);
output_points(lp3);
// クロソイドに距離-3で平行に描かれる曲線
auto lm3 = draw_clothoid(base, l, hs, cs, ce, 0.5, -3.0);
output_points(lm3);
結果
青がクロソイド。黄色と橙色はクロソイドの平行曲線。
平行移動量(R)と曲率の旋回方向と方位(Cs,Ce,Hs)については、座標系に依存する。
自分の考えやすいように調整したらよいと思う。
目論見「どこの考え・式をこねくり回したか」
(その1) 半径x角度＝円弧の長さ
曲率半径x曲率=1
曲率は「瞬間的には進行方向の変化量」＝「瞬間的な弧を描くときの角度」。
→ 曲率半径x曲率=1 の「1」は、単位距離の1。
曲率φが距離Lを進む間、距離に比例して変化していくとする(つまりクロソイド)。
曲率半径rは常にφの逆数を保つ。
よって、始点から終点までの途中のあらゆる地点lでr(l)φ(l)=1が成立する。
この関係を式化すると、多分こう。
L =∫r(l)φ(l)dl l=0～L
このr(l)に対して常に一定のRを作用させる式に変形させることができれば、
「クロソイドに平行な曲線の全長を表す式」になる。
S=∫(r(l)+R)φ(l)dl
= ∫r(l)φ(l)dl + R∫φ(l)dl
この「クロソイドに平行な曲線(長さS)」を単位距離で描きたい。
ので、S上を単位距離で変化させた時、その距離を元のクロソイド上の距離に逆変換させることができれば、平行線の元のクロソイド上の曲率や方位を求めることができ、平行線における次点の発生場所を近似ながら算出できる。
(その2) 次の座標を発生させる方向と距離
数式上はオイラーの公式だかの各要素に分けたもので表したものになると思われるけれども、ディジタル的に処理する際に直線としてしまうと、位置ずれが発生する要因になってしまう(これが結果的にクロソイド描画で単位距離を極小に設定する必要性に繋がっている)。
なので、単位距離を考慮する。
クロソイドが微小な円弧の集合だとみなす。
単位距離進むとき、次点は方位の直線上ではなく、単位距離先の弧の地点に発生する。
実際にはクロソイドは少しずつ半径が変わっていくためこれも正しくはないが、両端地点の方位(つまり作用する曲率も考慮)を利用すれば、多少はマシ、と思われる。
実際、これを考慮しないと、単位距離を短くして描いたものと比較してずれが結構な大きさになってしまう(単位距離の半分程度横ずれしてみえる)。
このため、精度を上げようとすると結果的に単位距離を極小にして描くことになる。
本考慮を入れると、この誤差は単位距離をあまり小さくしなくともそこまでひどくならないことを確認済み。
トレードオフとして、三角関数といくつかの計算と言ったコストが発生しているが、単位距離を極小化して描くよりは低コストだと思われる。
曲率の作用をもう少し正しく反映できればもっと誤差は減らせると思うけれども、あいにく知識が足りていない。
考慮不足の話
いくつかの点でこの実装には問題がある。以下、改善の余地のある部位。
(1) 曲率が0をまたぐ地点は設定できないようにする、或いは分割して再投下するようにする。
理由：曲率をまたぐ地点で位置ずれが発生する。原因は以下の実装部。
cur_heading = hs + (cs + 0.5 * dc * base_pos)* base_pos;
曲率が直前の地点と次点で逆転する場合、この区間は円弧ではない事を表す。
この区間に対して、後続する実装(円弧近似の概念を用いている)が不適切であることが明白。
(2)の考慮不足を加味した時、入力禁止か、分割情報を個別に返却することが望ましい。
(2)座標列の区間を均一化する修正。
理由：指定した単位距離で描く場合、末端部分が「計算のあまり」の距離になるが、この結果、座標列から元の曲線の正確な長さを逆算できない。
よって、戻り値の座標列を必ず均一な距離になるようロジックを是正(つまり描画の距離を整数で割れる単位距離に修正)し、また、戻り値として修正した座標の単位距離を戻すなどの対策が考えられる。
あるいは、最終区間の長さだけを別途返却するなど。
その他
クロソイドで表現できるデータがあるなら、情報の圧縮に使えるかも（そんな情報記憶にないが、Rも関数としてとらえて式を改善すればどっかで役立ちそう)