问题
I'm trying to rotate a rectangle(polyshape) in matlab to orient itself along the curve(a set of points). So far this is my code.
l=2;w=1;xc=-1;yc=2;
xvs= [xc+w/2 xc+w/2 xc-w/2 xc-w/2];
yvs= [yc-l/2 yc+l/2 yc+l/2 yc-l/2];
ax = gca;
polyin = polyshape(xvs,yvs); % %w/2,h/2% polyin = rectangle('Position',[-0.1 -0.1 0.2, 0.4]);
k=2;
% t = acos((y(k-1)*x(k-1)+y(k)*x(k))/(norm([x(k-1) y(k-1)])*norm([x(k) y(k)])));
t = atan(y(k)/x(k));
polyout = translate(polyin,[-xc -yc]);
polyout = translate(polyout,[x(1) y(1)]);
polyout = rotate(polyout, t, [x(2) y(2)]);
plot(polyout);
axis([-4 4 0 50]);
prev_t = atan(y(2)/x(2));
for k=2:length(x)
cla();
t = atan((y(k)-y(k-1))/(x(k)-x(k-1)));
% t = acos((y(k-1)*x(k-1)+y(k)*x(k))/(norm([x(k-1) y(k-1)])*norm([x(k) y(k)])));
% t = atan((y(k-1))/(x(k-1)));
polyout=translate(polyout,x(k)-x(k-1),y(k)-y(k-1));
[t3,t4]=centroid(polyout);
t-prev_t
polyout=rotate(polyout,rad2deg(t-prev_t) ,[t3, t4] );%, );
prev_t = t;
plot(polyout);
hold on;
plot(x(1:k),y(1:k));
hold on;
quiver(x(k), y(k), 1, 1);
axis([-4 4 0 50]);
drawnow;
end
x,y is listed below
x = [-1 -1.00972272933410 -1.01870478051530 -1.02695805761983 -1.03449438259277 -1.04132549543428 -1.04746305438566 -1.05291863611549 -1.05770373590578 -1.06182976783809 -1.06530806497965 -1.06814987956949 -1.07036638320459 -1.07196866702597 -1.07296774190485 -1.07337453862878 -1.07319990808774 -1.07245462146029 -1.07114937039970 -1.06929476722009 -1.06690134508251 -1.06397955818112 -1.06053978192930 -1.05659231314578 -1.05214737024076 -1.04721509340205 -1.04180554478121 -1.03592870867963 -1.02959449173473 -1.02281272310602 -1.01559315466128 -1.00794546116266 -0.999879240452816 -0.991404013641053 -0.982529225289427 -0.973264243598892 -0.963618360595424 -0.953600792316151 -0.943220678995479 -0.932487085251223 -0.921409000270737 -0.909995337997038 -0.898254937314939 -0.886196562237176 -0.873828902090537 -0.861160571701991 -0.848200111584816 -0.834955988124727 -0.821436593766009 -0.807650247197639 -0.793605193539421 -0.779309604528111 -0.764771578703544 -0.749999141594771 -0.735000245906179 -0.719782771703621 -0.704354526600550 -0.688723245944142 -0.672896593001429 -0.656882159145424 -0.640687464041251 -0.624319955832276 -0.607787011326233 -0.591095936181355 -0.574253965092497 -0.557268261977274 -0.540145920162182 -0.522893962568731 -0.505519341899570 -0.488028940824620 -0.470429572167200 -0.452727979090155 -0.434930835281988 -0.417044745142986 -0.399076243971348 -0.381031798149319 -0.362917805329311 -0.344740594620036 -0.326506426772637 -0.308221494366812 -0.289891921996946 -0.271523766458236 -0.253123016932825 -0.234695595175927 -0.216247355701956 -0.197784085970655 -0.179311506573228 -0.160835271418462 -0.142360967918861 -0.123894117176775 -0.105440174170524 -0.0870045279405308 -0.0685925017754495 -0.0502093533982922 -0.0318602751525592 -0.0135503941883670 0.00471522735142232 0.0229315921450729 0.0410937675058457 0.0591968851958715 0.0772361412400196 0.0952067957397721 0.113104172687091 0.130923659778296 0.148660708227926 0.166310832582623 0.183869610534992 0.201332682737479 0.218695752616237 0.235954586185006 0.253105011858973 0.270142920268653 0.287064264073754 0.303865057777052 0.320541377538260 0.337089360987901 0.353505207041178 0.369785175711844 0.385925587926079 0.401922825336353 0.417773330135305 0.433473604869608 0.449020212253846 0.464409774984379 0.479638975553221 0.494704556061906 0.509603318035363 0.524332122235782 0.538887888476493 0.553267595435832 0.567468280471012 0.581487039431996 0.595321026475369 0.608967453878208 0.622423591851952 0.635686768356277 0.648754368912964 0.661623836419772 0.674292670964308 0.686758429637899 0.699018726349463 0.711071231639382 0.722913672493370 0.734543832156346 0.745959549946308 0.757158721068200 0.768139296427783 0.778899282445511 0.789436740870399 0.799749788593894 0.809836597463747 0.819695394097884 0.829324459698281 0.838722129864827 0.847886794409204 0.856816897168752 0.865510935820345 0.873967461694257 0.882185079588040 0.890162447580389 0.897898276845016 0.905391331464524 0.912640428244272 0.919644436526252 0.926402278002956 0.932912926531251 0.939175407946249 0.945188799875176 0.950952231551246 0.956464883627534 0.961725987990839 0.966734827575568 0.971490736177594 0.975993098268136 0.980241348807631 0.984234973059598 0.987973506404514 0.991456534153687 0.994683691363123 0.997654662647400 1.00036918199354 1.00282703257487 1.00502804656492 1.00697210495126 1.00865913734939 1.01008912181662 1.01126208466593 1.01217810027981 1.01283729092421 1.01323982656233 1.01338592466854 1.01327585004223 1.01290991462170 1.01228847729801 1.01141194372885 1.01028076615244 1.00889544320138 1.00725651971651 1.00536458656082 1.00322028043328 1.00082428368272 0.998177324121733 0.995280174840507 0.992133654020716 0.988738624749389 0.985095994832775 0.981206716610224 0.977071786768050 0.972692246153409 0.968069179588162 0.963203715682755 0.958097026650084 0.952750328119371 0.947164878950031 0.941341981045545 0.935282979167332 0.928989260748622 0.922462255708321 0.915703436264890 0.908714316750211 0.901496453423459 0.894051444284976 0.886380928890139 0.878486588163234 0.870370144211326 0.862033360138129 0.853478039857879 0.844706027909207 0.835719209269005 0.826519509166304 0.817108892896138 0.807489365633420 0.797662972246815 0.787631797112606 0.777397963928569 0.766963635527843 0.756331013692799 0.745502338968920 0.734479890478659 0.723265985735320 0.711862980456928 0.700273268380098 0.688499281073906 0.676543487753763 0.664408395095284 0.652096547048159 0.639610524650029 0.626952945840348 0.614126465274265 0.601133774136485 0.587977599955151 0.574660706415706 0.561185893174769 0.547555995674006 0.533773884953999 0.519842467468121 0.505764684896401 0.491543513959404 0.477181966232097 0.462683087957718 0.448049959861653 0.433285696965303 0.418393448399959 0.403376397220668 0.388237760220109 0.372980787742464 0.357608763497285 0.342125004373370 0.326532860252634 0.310835713823975 0.295036980397154 0.279140107716656 0.263148575775570 0.247065896629457 0.230895614210219 0.214641304139976 0.198306573544930 0.181895060869242 0.165410435688903 0.148856398525599 0.132236680660593 0.115555043948583 0.0988152806315876 0.0820212131528061 0.0651766939704940 0.0482856053718364 0.0313518592868137 0.0143793971020798 -0.00262781052517255 -0.0196657638533383 -0.0367304342425269 -0.0538177643406945 -0.0709236682697698 -0.0880440318117838 -0.105174712595000 -0.122311540280039 -0.139450316746014 -0.156586816276652 -0.173716785746427 -0.190835944806689 -0.207939986071788 -0.225024575305211 -0.242085351605702 -0.259117927593395 -0.276117889595944 -0.293080797834649 -0.310002186610585 -0.326877564490733 -0.343702414494106 -0.360472194277880 -0.377182336323518 -0.393828248122908 -0.410405312364482 -0.426908887119349 -0.443334306027426 -0.459676878483560 -0.475931889823665 -0.492094601510844 -0.508160251321521 -0.524124053531569 -0.539981199102440 -0.555726855867290 -0.571356168717112 -0.586864259786862 -0.602246228641589 -0.617497152462564 -0.632612086233408 -0.647586062926218 -0.662414093687702 -0.677091168025301 -0.691612253993325 -0.705972298379074 -0.720166226888972 -0.734188944334692 -0.748035334819290 -0.761700261923330 -0.775178568891010 -0.788465078816297 -0.801554594829052 -0.814441900281157 -0.827121758932650 -0.839588915137848 -0.851838094031477 -0.863864001714800 -0.875661325441750 -0.887224733805055 -0.898548876922365 -0.909628386622385 -0.920457876631001 -0.931031942757411 -0.941345163080250 -0.951392098133724 -0.961167291093732 -0.970665267964000 -0.979880537762209 -0.988807592706122 -0.997440908399714 -1.00577494401930 -1.01380414249966 -1.02152293072018 -1.02892571969097 -1.03600690473899 -1.04276086569418 -1.04918196707561 -1.05526455827758 -1.06100297375575 -1.06639153321331 -1.07142454178703 -1.07609629023348 -1.08040105511509 -1.08433309898632 -1.08788667057976 -1.09105600499228 -1.09383532387113 -1.09621883560011 -1.09820073548567 -1.09977520594304 -1.10093641668239 -1.10167852489489 -1.10199567543892 -1.10188200102614 -1.10133162240765 -1.10033864856010 -1.09889717687183 -1.09700129332900 -1.09464507270172 -1.09182257873014 -1.08852786431066 -1.08475497168197 -1.08049793261124 -1.07575076858022 -1.07050749097139 -1.06476210125405 -1.05850859117050 -1.05174094292213 -1.04445312935557 -1.03663911414879 -1.02829285199726 -1.01940828880009 -1.00997936184609 -1]
y = [2 2.26499544506307 2.52345285165919 2.77551012741366 3.02130356526704 3.26096785177083 3.49463607538307 3.72243973476395 3.94450874707147 4.16097145625704 4.37195464136115 4.57758352480892 4.77798178070582 4.97327154313322 5.16357341444407 5.34900647355850 5.52968828425946 5.70573490348834 5.87726088964058 6.04437931086135 6.20720175334112 6.36583832961133 6.52039768683998 6.67098701512728 6.81771205580129 6.96067710971352 7.09998504553457 7.23573730804975 7.36803392645471 7.49697352265110 7.62265331954214 7.74516914932828 7.86461546180284 7.98108533264758 8.09467047172843 8.20546123139099 8.31354661475628 8.41901428401626 8.52195056872954 8.62244047411698 8.72056768935728 8.81641459588266 8.91006227567448 9.00159051955883 9.09107783550219 9.17860145690705 9.26423735090754 9.34806022666506 9.43014354366389 9.51055952000682 9.58937914071082 9.66667216600261 9.74250713961429 9.81695139707904 9.89007107402665 9.96193111447923 10.0325952791468 10.1021261537228 10.1705851571801 10.2380325500660 10.3045274427986 10.3701278039618 10.4348904686013 10.4988711465199 10.5621244305736 10.6247038049668 10.6866616535481 10.7480492681059 10.8089168566642 10.8693135517778 10.9292874188284 10.9888854643200 11.0481536441744 11.1071368720271 11.1658790275228 11.2244229646110 11.2828105198417 11.3410825206610 11.3992787937066 11.4574381731036 11.5155985087602 11.5737966746629 11.6320685771728 11.6904491633205 11.7489724291023 11.8076714277756 11.8665782781543 11.9257241729050 11.9851393868420 12.0448532852234 12.1048943320464 12.1652900983432 12.2260672704765 12.2872516584350 12.3488682041292 12.4109409896870 12.4734932457494 12.5365473597658 12.6001248842901 12.6642465452759 12.7289322503726 12.7942010972203 12.8600713817463 12.9265606064601 12.9936854887492 13.0614619691748 13.1299052197674 13.1990296523224 13.2688489266957 13.3393759590994 13.4106229303974 13.4826012944010 13.5553217861645 13.6287944302809 13.7030285491776 13.7780327714119 13.8538150399664 13.9303826205452 14.0077421098691 14.0858994439713 14.1648599064931 14.2446281369795 14.3252081391748 14.4066032893182 14.4888163444396 14.5718494506550 14.6557041514622 14.7403813960366 14.8258815475266 14.9122043913492 14.9993491434859 15.0873144587782 15.1760984392230 15.2656986422686 15.3561120891100 15.4473352729848 15.5393641674687 15.6321942347710 15.7258204340304 15.8202372296108 15.9154385993964 16.0114180430878 16.1081685904975 16.2056828098453 16.3039528160545 16.4029702790468 16.5027264320383 16.6032120798353 16.7044176071297 16.8063329867944 16.9089477881795 17.0122511854074 17.1162319656689 17.2208785375182 17.3261789391692 17.4321208467907 17.5386915828022 17.6458781241694 17.7536671107000 17.8620448533392 17.9709973424653 18.0805102561854 18.1905689686311 18.3011585582540 18.4122638161214 18.5238692542118 18.6359591137108 18.7485173733063 18.8615277574846 18.9749737448257 19.0888385762990 19.2031052635592 19.3177565972414 19.4327751552571 19.5481433110899 19.6638432420907 19.7798569377739 19.8961662081124 20.0127526918339 20.1295978647159 20.2466830478817 20.3639894160961 20.4814980060606 20.5991897247094 20.7170453575050 20.8350455767337 20.9531709498011 21.0714019475281 21.1897189524464 21.3081022670938 21.4265321223103 21.5449886855333 21.6634520690937 21.7819023385110 21.9003195207895 22.0186836127132 22.1369745891423 22.2551724113081 22.3732570351088 22.4912084194057 22.6090065343178 22.7266313695183 22.8440629425299 22.9612813070203 23.0782665610982 23.1949988556084 23.3114584024278 23.4276254827613 23.5434804554365 23.6590037652004 23.7741759510141 23.8889776543494 24.0033896274833 24.1173927417946 24.2309679960590 24.3440965247448 24.4567596063089 24.5689386714917 24.6806153116135 24.7917712868696 24.9023885346262 25.0124491777157 25.1219355327329 25.2308301183300 25.3391156635127 25.4467751159356 25.5537916501978 25.6601486761387 25.7658298471334 25.8708190683886 25.9751005052380 26.0786585914379 26.1814780374631 26.2835438388023 26.3848412842538 26.4853559642211 26.5850737790085 26.6839809471169 26.7820640135392 26.8793098580560 26.9757057035313 27.0712391242082 27.1658980540041 27.2596707948069 27.3525460247703 27.4445128066094 27.5355605958967 27.6256792493570 27.7148590331637 27.8030906312344 27.8903651535260 27.9766741443309 28.0620095905721 28.1463639300994 28.2297300599846 28.3121013448172 28.3934716250003 28.4738352250458 28.5531869618704 28.6315221530909 28.7088366253202 28.7851267224627 28.8603893140098 28.9346218033358 29.0078221359933 29.0799888080092 29.1511208741798 29.2212179563668 29.2902802517928 29.3583085413370 29.4253041978307 29.4912691943530 29.5562061125265 29.6201181508128 29.6830091328082 29.7448835155393 29.8057463977587 29.8656035282405 29.9244613140760 29.9823268289694 30.0392078215333 30.0951127235842 30.1500506584387 30.2040314492083 30.2570656270958 30.3091644396903 30.3603398592634 30.4106045910644 30.4599720816159 30.5084565270100 30.5560728812031 30.6028368643122 30.6487649709102 30.6938744783217 30.7381834549183 30.7817107684148 30.8244760941642 30.8664999234537 30.9078035718003 30.9484091872463 30.9883397586550 31.0276191240064 31.0662719786928 31.1043238838141 31.1418012744739 31.1787314680751 31.2151426726150 31.2510639949816 31.2865254492487 31.3215579649719 31.3561933954840 31.3904645261906 31.4244050828661 31.4580497399487 31.4914341288367 31.5245948461837 31.5575694621942 31.5903965289195 31.6231155885532 31.6557671817267 31.6883928558052 31.7210351731827 31.7537377195783 31.7865451123314 31.8195030086975 31.8526581141437 31.8860581906445 31.9197520649774 31.9537896370183 31.9882218880374 32.0231008889946 32.0584798088355 32.0944129227864 32.1309556206506 32.1681644151037 32.2060969499891 32.2448120086139 32.2843695220444 32.3248305774017 32.3662574261575 32.4087134924294 32.4522633812768 32.4969728869964 32.5429090014181 32.5901399222001 32.6387350611250 32.6887650523952 32.7403017609286 32.7934182906543 32.8481889928080 32.9046894742278 32.9629966056499 33.0231885300040 33.0853446707091 33.1495457399690 33.2158737470682 33.2844120066671 33.3552451470980 33.4284591186606 33.5041412019175 33.5823800159901 33.6632655268539 33.7468890556344 33.8333432869025 33.9227222769705 34.0151214621873 34.1106376672341 34.2093691134204 34.3114154269792 34.4168776473628 34.5258582355384 34.6384610822839 34.7547915164830 34.8749563134217 34.9990637030830 35.1272233784432 35.2595465037671 35.3961457229040 35.5371351675831 35.6826304657089 35.8327487496574 35.9876086645714 36.1473303766559 36.3120355814743 36.4818475122433 36.6568909481293 36.8372922225435 37.0231792314376 37.2146814415997 37.4119298989495 37.6150572368345 37.8241976843249 38.0394870745098 38.2610628527928 38.4890640851872 38.7236314666120 38.9649073291876 39.2130356505310 39.4681620620517 39.7304338572476 40]
However the rectangle doesn't orient itself exactly towards the curve in question, it still has a offset.Any help is appreciated. Thanks.
回答1:
Hi i'm posting the solution to the question here. Thanks to anyone who took time to take a look at it.
for k=2:length(x)
%%%Plot road
rl=-2; rr=0; w=4; l=100;
yroad = 0:l;
xroad = repmat(rl+w/2,l+1);
plot(xroad, yroad, 'w--','LineWidth',1.5);
hold on;
rectangle('Position',[rl,rr,w,l],'FaceColor',[0 0 0 0.9]);
axis([-4 4 0 50]);
hold on;
%%%%%
set(gca,'children',flipud(get(gca,'children')))
%%%time annotation
str = strcat('t=',string(t));
% annotation('textbox',[0.8 0.8 .5 .5],'String',str,'FitBoxToText','on');
text(1,47,str,'Color','w')
ratio = diff(get(gca, 'YLim'))/diff(get(gca, 'XLim'));
phi = atan2(y(k) - y(k - 1), (x(k) - x(k - 1))*ratio);
[x_rect_rot, y_rect_rot]=get_rectangle(phi, x(k), y(k));
plot(polyshape(x_rect_rot, y_rect_rot),'FaceColor','c','FaceAlpha',0.85);
hold on;
plot(x(1:k), y(1:k));
hold on;
drawnow;
end
function[xa,ya] =get_rectangle(phi,xc,yc,varargin)
if length(varargin)<2
h = 5;
w = 3;
else
h = varargin{4};
w = varargin{5};
end
x_rect = [-h, h, h, -h]/2;
y_rect = [-w, -w, w, w]/2;
% Consider aspect ratio of the axis
ratio = diff(get(gca, 'YLim'))/diff(get(gca, 'XLim'));
% Calculate rotated rectangle
x_rect_rot = x_rect*cos(phi) - y_rect*sin(phi);
y_rect_rot = x_rect*sin(phi) + y_rect*cos(phi);
% Incorporate ratio
x_rect_rot = x_rect_rot/ratio;
% Calculate offset
xa = x_rect_rot + xc;
ya = y_rect_rot + yc;
end
来源：https://stackoverflow.com/questions/54606363/simple-rotation-of-a-rectangle-along-a-curve-in-matlab