下半身制御用ライブラリ Odometry...自己位置推定 Mecanum...メカナムホイール用 Bezier...ベジエ曲線 RoboClaw...MD用
Dependents: TOUTEKI_all_mbed mbed_test_program
Bezier.cpp@0:62707e16531a, 2018-09-01 (annotated)
- Committer:
- yuki17100
- Date:
- Sat Sep 01 09:07:35 2018 +0000
- Revision:
- 0:62707e16531a
- Child:
- 1:698586aa0c5a
???????????????MD??????????
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
yuki17100 | 0:62707e16531a | 1 | #include "Bezier.h" |
yuki17100 | 0:62707e16531a | 2 | |
yuki17100 | 0:62707e16531a | 3 | |
yuki17100 | 0:62707e16531a | 4 | BezierCurve::BezierCurve(){ |
yuki17100 | 0:62707e16531a | 5 | length = 0; |
yuki17100 | 0:62707e16531a | 6 | } |
yuki17100 | 0:62707e16531a | 7 | |
yuki17100 | 0:62707e16531a | 8 | //指定区間の長さを出す関数 |
yuki17100 | 0:62707e16531a | 9 | double BezierCurve::range(double start,double stop){ |
yuki17100 | 0:62707e16531a | 10 | |
yuki17100 | 0:62707e16531a | 11 | double len = 0; |
yuki17100 | 0:62707e16531a | 12 | double _posi[2] = {}; |
yuki17100 | 0:62707e16531a | 13 | |
yuki17100 | 0:62707e16531a | 14 | for(; start<stop; start+=WIDTH){ |
yuki17100 | 0:62707e16531a | 15 | |
yuki17100 | 0:62707e16531a | 16 | double posi[2] = {callX(start), callY(start)}; |
yuki17100 | 0:62707e16531a | 17 | |
yuki17100 | 0:62707e16531a | 18 | len += hypot( posi[0] - _posi[0], posi[1] - _posi[1]); |
yuki17100 | 0:62707e16531a | 19 | |
yuki17100 | 0:62707e16531a | 20 | _posi[0] = posi[0]; |
yuki17100 | 0:62707e16531a | 21 | _posi[1] = posi[1]; |
yuki17100 | 0:62707e16531a | 22 | |
yuki17100 | 0:62707e16531a | 23 | } |
yuki17100 | 0:62707e16531a | 24 | |
yuki17100 | 0:62707e16531a | 25 | return len; |
yuki17100 | 0:62707e16531a | 26 | } |
yuki17100 | 0:62707e16531a | 27 | |
yuki17100 | 0:62707e16531a | 28 | //ベジエ曲線のパラメータを設定する関数 |
yuki17100 | 0:62707e16531a | 29 | void BezierCurve::set(double (&pX)[4], double (&pY)[4], double (&pT)[4]){ |
yuki17100 | 0:62707e16531a | 30 | |
yuki17100 | 0:62707e16531a | 31 | for(int i=0; i<4; i++){ |
yuki17100 | 0:62707e16531a | 32 | pointX[i] = pX[i]; |
yuki17100 | 0:62707e16531a | 33 | pointY[i] = pY[i]; |
yuki17100 | 0:62707e16531a | 34 | pointT[i] = pT[i]; |
yuki17100 | 0:62707e16531a | 35 | } |
yuki17100 | 0:62707e16531a | 36 | |
yuki17100 | 0:62707e16531a | 37 | length = range(0,1); |
yuki17100 | 0:62707e16531a | 38 | } |
yuki17100 | 0:62707e16531a | 39 | |
yuki17100 | 0:62707e16531a | 40 | |
yuki17100 | 0:62707e16531a | 41 | double BezierCurve::callX(double ratio){ |
yuki17100 | 0:62707e16531a | 42 | return (1-ratio)*(1-ratio)*(1-ratio)*pointX[0] + (1-ratio)*(1-ratio)*ratio*pointX[1]+ |
yuki17100 | 0:62707e16531a | 43 | (1-ratio)*ratio *ratio *pointX[2] + ratio *ratio *ratio*pointX[3]; |
yuki17100 | 0:62707e16531a | 44 | } |
yuki17100 | 0:62707e16531a | 45 | |
yuki17100 | 0:62707e16531a | 46 | double BezierCurve::callY(double ratio){ |
yuki17100 | 0:62707e16531a | 47 | return (1-ratio)*(1-ratio)*(1-ratio)*pointY[0] + (1-ratio)*(1-ratio)*ratio*pointY[1]+ |
yuki17100 | 0:62707e16531a | 48 | (1-ratio)*ratio *ratio *pointY[2] + ratio *ratio *ratio*pointY[3]; |
yuki17100 | 0:62707e16531a | 49 | } |
yuki17100 | 0:62707e16531a | 50 | |
yuki17100 | 0:62707e16531a | 51 | double BezierCurve::callT(double ratio){ |
yuki17100 | 0:62707e16531a | 52 | return (1-ratio)*(1-ratio)*(1-ratio)*pointT[0] + (1-ratio)*(1-ratio)*ratio*pointT[1]+ |
yuki17100 | 0:62707e16531a | 53 | (1-ratio)*ratio *ratio *pointT[2] + ratio *ratio *ratio*pointT[3]; |
yuki17100 | 0:62707e16531a | 54 | } |
yuki17100 | 0:62707e16531a | 55 | /* |
yuki17100 | 0:62707e16531a | 56 | double BezierCurve::aimPoint(double dist, double crrX, double crrY, double (&tgtPosi)[3]){ |
yuki17100 | 0:62707e16531a | 57 | |
yuki17100 | 0:62707e16531a | 58 | double r = 0; |
yuki17100 | 0:62707e16531a | 59 | |
yuki17100 | 0:62707e16531a | 60 | do { |
yuki17100 | 0:62707e16531a | 61 | r += WIDTH; |
yuki17100 | 0:62707e16531a | 62 | if(r >= 1){ break; } |
yuki17100 | 0:62707e16531a | 63 | |
yuki17100 | 0:62707e16531a | 64 | } while(hypot(callX(r) - crrX, callY(r) - crrY) < dist); |
yuki17100 | 0:62707e16531a | 65 | |
yuki17100 | 0:62707e16531a | 66 | tgtPosi[0] = callX(r); |
yuki17100 | 0:62707e16531a | 67 | tgtPosi[1] = callY(r); |
yuki17100 | 0:62707e16531a | 68 | tgtPosi[2] = callT(r); |
yuki17100 | 0:62707e16531a | 69 | |
yuki17100 | 0:62707e16531a | 70 | return range(r,1); |
yuki17100 | 0:62707e16531a | 71 | } |
yuki17100 | 0:62707e16531a | 72 | */ |