クロソイド曲線で遊ぼう

クロソイド曲線とは?

クロソイド曲線とは、曲率（曲がり具合）の変化が急（飛び飛び）にならない曲線の一つです。
高速道路の曲がり具合や、ジェットコースターの曲線に使われたりするようです。

こんなやつです

wikipediaにも説明がありますので参考にどうぞ

直感的には、
１、曲がっていない状態からスタート
２、徐々に曲線が急になっていく
というようにイメージしても良いと思います。

現実世界での例

例えば車で左に曲がる場合を考えてみます
この時、

ハンドルがどのくらい回っているのか = 曲率
ハンドルを回す速度 = 曲率の変化率

と言えます。

私たちは、ハンドルを回す速度を秒間0°から秒間90°にわりとすぐ変化させることができます。でも、ハンドルを回すのは時間がかかりますから、ハンドルがどのくらい回っているのか（曲率）をすぐに変化させるのはとても大変です。
こういった事情から、道のカーブというのは、クロソイド曲線のような曲率が急激に変化しないような曲線というのが非常に適しているのです。

より具体的に

資料を探していると、
以下の論文が目につきました。

接線法を用いた自由点列のクロソイド補間
https://www.jstage.jst.go.jp/article/jjspe1986/60/1/60_1_80/_article/-char/ja/

この論文の目標は、自由点列の補間ですが、
最初の方で、パラメータ h, φ0, φv, φu の４つで任意のクロソイド曲線を表現する方法が述べられています。

今回はそれで遊んでみたいとおもいます。

具体的な考え方をちょっと追ってみます。
まず今回考える曲線の長さが h です。
この長さを一旦抽象化するために、0~1に正規化した数値 S を考えます。
s がスタート地点から曲線の距離だとすると、以下のように表します。

S = \frac{s}{h}

そして曲線上の点を P 初期位置を P0 とすると、

P = P_0 + h\int_0^S e^{j\phi} dS

\phi = \phi_0 + \phi_vS + \phi_uS^2

※ここではjが虚数単位です
※曲線は複素平面での曲線です

と、ちょっと式が入れ子になっていますが、
以上の数式で曲線が表現できます。

そしていくつか登場した定数パラメータ４つの意味は、
それぞれ以下のような意味を持ちます

h: 線の長さ
φ0: 初期角度
φv: もし曲率が変化しない場合にh分進んだ場合の角度増分(例えばφuが0なら、この曲線は円を描きます)
φu: クロソイドによる角度の増分

ではここまでをプログラムに落とし込んでみます。

言語はC++、カジュアルに描画したいのでopenframeworks(バージョン0.8.4)を使います。

まずはφの部分です

float phi(float phi0, float phiV, float phiU, float S) {
    return phi0 + phiV * S + phiU * S * S;
}

次に積分の内側です。
ついでに関数オブジェクトにしておきます。

std::complex<float> slope_f(float phi0, float phiV, float phiU, float S) {
    std::complex<float> j(0.0f, 1.0f);
    return std::exp(j * phi(phi0, phiV, phiU, S));
}
struct Slope {
    float phi0;
    float phiV;
    float phiU;
    std::complex<float> operator()(float S) {
        return slope_f(phi0, phiV, phiU, S);
    }
};

積分が出てきていますので、ひとまずシンプソン則での数値積分を用意します。

template <class T, class Real, class R>
void simpson_integral(T f, Real a, Real b, R *r) {
    Real mul = (b - a) * static_cast<Real>(1.0 / 6.0);
    *r = mul * (f(a) + static_cast<Real>(4.0) * f((a + b) * static_cast<Real>(0.5)) + f(b));
}

ここまで準備できたら、
あとは1000ステップほどに分割して積算して、ポリラインを作ります。

    int n = 1000;
    float stepS = 1.0f / n;
    std::vector<ofPoint> points;

    Slope slope;
    slope.phi0 = phi0;
    slope.phiV = phiV;
    slope.phiU = phiU;
    
    std::complex<float> P_Vector;
    for(int i = 0 ; i < n ; ++i) {
        float S = stepS * i;
        
        std::complex<float> r;
        simpson_integral(slope, S, S + stepS, &r);
        P_Vector += r;
        
        float x = P_Vector.real();
        float y = P_Vector.imag();
        points.push_back(h * ofVec2f(x, y));
    }

少々文字が続いてしまいましたが、これでパラメータを変えつつ（主にphiV, phiU）描画すると、以下のような見た目になります。

かわいい...!

なんかゼンマイみたいですね？

オチもついたところで、
最後にソースコード全体を貼って一区切りにします

# pragma once

# include "ofMain.h"
# include "ofxGui.h"

class ofApp : public ofBaseApp{
public:
    void setup();
    void update();
    void draw();
    
    void keyPressed(int key);
    void keyReleased(int key);
    void mouseMoved(int x, int y);
    void mouseDragged(int x, int y, int button);
    void mousePressed(int x, int y, int button);
    void mouseReleased(int x, int y, int button);
    void windowResized(int w, int h);
    void dragEvent(ofDragInfo dragInfo);
    void gotMessage(ofMessage msg);
    
    ofxPanel _gui;
    ofxFloatSlider _hValue;
    ofxFloatSlider _phi0Value;
    ofxFloatSlider _phiVValue;
    ofxFloatSlider _phiUValue;
};

# include "ofApp.h"

# include <complex>
# include <functional>

//--------------------------------------------------------------
void ofApp::setup(){
    ofSetVerticalSync(true);
    ofSetFrameRate(60);
    
    _gui.setup();
    
    _gui.add(_hValue.setup("h", 1.0f, 0.0f, 10.0f));
    _gui.add(_phi0Value.setup("phi 0", 1.0f, -10.0f, 10.0f));
    _gui.add(_phiVValue.setup("phi v", 1.0f, -10.0f, 10.0f));
    _gui.add(_phiUValue.setup("phi u", 1.0f, -10.0f, 10.0f));
    
    _gui.setSize(300, 300);
    _gui.setWidthElements(300);
}

//--------------------------------------------------------------
void ofApp::update(){
    
}

template <class T, class Real, class R>
void simpson_integral(T f, Real a, Real b, R *r) {
    Real mul = (b - a) * static_cast<Real>(1.0 / 6.0);
    *r = mul * (f(a) + static_cast<Real>(4.0) * f((a + b) * static_cast<Real>(0.5)) + f(b));
}

float phi(float phi0, float phiV, float phiU, float S) {
    return phi0 + phiV * S + phiU * S * S;
}

std::complex<float> slope_f(float phi0, float phiV, float phiU, float S) {
    std::complex<float> j(0.0f, 1.0f);
    return std::exp(j * phi(phi0, phiV, phiU, S));
}
struct Slope {
    float phi0;
    float phiV;
    float phiU;
    std::complex<float> operator()(float S) {
        return slope_f(phi0, phiV, phiU, S);
    }
};

//--------------------------------------------------------------
void ofApp::draw(){
    ofClear(0);
    
    ofSetMatrixMode(OF_MATRIX_PROJECTION);
    ofPushMatrix();
    ofLoadIdentityMatrix();
    ofSetMatrixMode(OF_MATRIX_MODELVIEW);
    ofPushMatrix();
    ofLoadIdentityMatrix();
    ofScale(1, -1, 1);
    
    float s = 0.5f;
    glScalef(s, s, s);

    float h = _hValue;
    float phi0 = _phi0Value;
    float phiV = _phiVValue;
    float phiU = _phiUValue;
    
    int n = 1000;
    float stepS = 1.0f / n;
    std::vector<ofPoint> points;

    Slope slope;
    slope.phi0 = phi0;
    slope.phiV = phiV;
    slope.phiU = phiU;
    
    std::complex<float> P_Vector;
    for(int i = 0 ; i < n ; ++i) {
        float S = stepS * i;
        
        std::complex<float> r;
        simpson_integral(slope, S, S + stepS, &r);
        P_Vector += r;
        
        float x = P_Vector.real();
        float y = P_Vector.imag();
        points.push_back(h * ofVec2f(x, y));
    }
    
    ofPolyline polyline(points);
    ofSetLineWidth(2);
    polyline.draw();
    ofSetLineWidth(1);
    
    ofSetMatrixMode(OF_MATRIX_PROJECTION);
    ofPopMatrix();
    ofSetMatrixMode(OF_MATRIX_MODELVIEW);
    ofPopMatrix();
    
    _gui.draw();
}

//--------------------------------------------------------------
void ofApp::keyPressed(int key){
    
}

//--------------------------------------------------------------
void ofApp::keyReleased(int key){

}

//--------------------------------------------------------------
void ofApp::mouseMoved(int x, int y){

}

//--------------------------------------------------------------
void ofApp::mouseDragged(int x, int y, int button){

}

//--------------------------------------------------------------
void ofApp::mousePressed(int x, int y, int button){

}

//--------------------------------------------------------------
void ofApp::mouseReleased(int x, int y, int button){

}

//--------------------------------------------------------------
void ofApp::windowResized(int w, int h){

}

//--------------------------------------------------------------
void ofApp::gotMessage(ofMessage msg){

}

//--------------------------------------------------------------
void ofApp::dragEvent(ofDragInfo dragInfo){ 

}