下半身制御用ライブラリ Odometry...自己位置推定 Mecanum...メカナムホイール用 Bezier...ベジエ曲線 RoboClaw...MD用
Dependents: TOUTEKI_all_mbed mbed_test_program
Bezier.cpp
- Committer:
- yuki17100
- Date:
- 2018-09-02
- Revision:
- 1:698586aa0c5a
- Parent:
- 0:62707e16531a
File content as of revision 1:698586aa0c5a:
#include "Bezier.h" BezierCurve::BezierCurve(){ length = 0; } //指定区間の長さを出す関数 double BezierCurve::range(double start,double stop){ double len = 0; double _posi[2] = {}; for(; start<stop; start+=WIDTH){ double posi[2] = {callX(start), callY(start)}; len += hypot( posi[0] - _posi[0], posi[1] - _posi[1]); _posi[0] = posi[0]; _posi[1] = posi[1]; } return len; } //ベジエ曲線のパラメータを設定する関数 void BezierCurve::set(double (&pX)[4], double (&pY)[4], double (&pT)[4]){ for(int i=0; i<4; i++){ pointX[i] = pX[i]; pointY[i] = pY[i]; pointT[i] = pT[i]; } length = range(0,1); } double BezierCurve::callX(double ratio){ return (1-ratio)*(1-ratio)*(1-ratio)*pointX[0] + (1-ratio)*(1-ratio)*ratio*pointX[1]+ (1-ratio)*ratio *ratio *pointX[2] + ratio *ratio *ratio*pointX[3]; } double BezierCurve::callY(double ratio){ return (1-ratio)*(1-ratio)*(1-ratio)*pointY[0] + (1-ratio)*(1-ratio)*ratio*pointY[1]+ (1-ratio)*ratio *ratio *pointY[2] + ratio *ratio *ratio*pointY[3]; } double BezierCurve::callT(double ratio){ return (1-ratio)*(1-ratio)*(1-ratio)*pointT[0] + (1-ratio)*(1-ratio)*ratio*pointT[1]+ (1-ratio)*ratio *ratio *pointT[2] + ratio *ratio *ratio*pointT[3]; } /* * * 指定位置から一定距離の点を算出する関数を作ろうとした。 * 参考のために残しておく double BezierCurve::aimPoint(double dist, double crrX, double crrY, double (&tgtPosi)[3]){ double r = 0; //これ0だといけない static かクラス変数つくるべき do { r += WIDTH; if(r >= 1){ break; } } while(hypot(callX(r) - crrX, callY(r) - crrY) < dist); tgtPosi[0] = callX(r); tgtPosi[1] = callY(r); tgtPosi[2] = callT(r); return range(r,1); } */