├── .gitignore ├── HiddenFigure.m ├── LICENSE ├── NOTATION ├── Polygon.m ├── README.md ├── bresenham.m ├── circle.m ├── colorname.m ├── diff2.m ├── dockfigs.m ├── edgelist.m ├── filt1d.m ├── gaussfunc.m ├── mmlabel.m ├── mplot.m ├── mtools.m ├── pickregion.m ├── plotp.m ├── polydiff.m ├── protectfig.m ├── randinit.m ├── runscript.m ├── rvccheck.m ├── rvcpath.m ├── startup_rvc.m ├── stlRead.m ├── undockfigs.m ├── unit_test ├── PluckerTest.m ├── plotXTest.m └── tboptparseTest.m ├── usefig.m ├── xaxis.m └── yaxis.m /.gitignore: -------------------------------------------------------------------------------- 1 | # ignore png, gimp and pdf files 2 | *.png 3 | *.pdf 4 | *.xcf 5 | *.log 6 | *.aux 7 | *.idx 8 | *.asv 9 | -------------------------------------------------------------------------------- /HiddenFigure.m: -------------------------------------------------------------------------------- 1 | 2 | 3 | % Copyright (C) 1993-2017, by Peter I. Corke 4 | % 5 | % This file is part of The Robotics Toolbox for MATLAB (RTB). 6 | % 7 | % RTB is free software: you can redistribute it and/or modify 8 | % it under the terms of the GNU Lesser General Public License as published by 9 | % the Free Software Foundation, either version 3 of the License, or 10 | % (at your option) any later version. 11 | % 12 | % RTB is distributed in the hope that it will be useful, 13 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | % GNU Lesser General Public License for more details. 16 | % 17 | % You should have received a copy of the GNU Leser General Public License 18 | % along with RTB. If not, see . 19 | % 20 | % http://www.petercorke.com 21 | classdef HiddenFigure 22 | 23 | properties 24 | h % the graphics handle 25 | end 26 | 27 | methods 28 | function hf = HiddenFigure(varargin) 29 | hf.h = figure('HandleVisibility', 'off', varargin{:}); 30 | end 31 | 32 | function clf(hf) 33 | clf(hf.h) 34 | end 35 | 36 | function axes(hf, varargin) 37 | axes('Parent', hf.h, varargin{:}); 38 | end 39 | 40 | function axis(hf, varargin) 41 | axis(hf.h, varargin{:}); 42 | end 43 | 44 | function plot(hf, varargin) 45 | hf.axes(); 46 | plot(varargin{:}); 47 | end 48 | 49 | function title(hf, name) 50 | set(hf.h, 'Name', name); 51 | end 52 | end 53 | end 54 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | GNU LESSER GENERAL PUBLIC LICENSE 2 | Version 2.1, February 1999 3 | 4 | Copyright (C) 1991, 1999 Free Software Foundation, Inc. 5 | 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 6 | Everyone is permitted to copy and distribute verbatim copies 7 | of this license document, but changing it is not allowed. 8 | 9 | (This is the first released version of the Lesser GPL. It also counts 10 | as the successor of the GNU Library Public License, version 2, hence 11 | the version number 2.1.) 12 | 13 | Preamble 14 | 15 | The licenses for most software are designed to take away your 16 | freedom to share and change it. By contrast, the GNU General Public 17 | Licenses are intended to guarantee your freedom to share and change 18 | free software--to make sure the software is free for all its users. 19 | 20 | This license, the Lesser General Public License, applies to some 21 | specially designated software packages--typically libraries--of the 22 | Free Software Foundation and other authors who decide to use it. You 23 | can use it too, but we suggest you first think carefully about whether 24 | this license or the ordinary General Public License is the better 25 | strategy to use in any particular case, based on the explanations below. 26 | 27 | When we speak of free software, we are referring to freedom of use, 28 | not price. Our General Public Licenses are designed to make sure that 29 | you have the freedom to distribute copies of free software (and charge 30 | for this service if you wish); that you receive source code or can get 31 | it if you want it; that you can change the software and use pieces of 32 | it in new free programs; and that you are informed that you can do 33 | these things. 34 | 35 | To protect your rights, we need to make restrictions that forbid 36 | distributors to deny you these rights or to ask you to surrender these 37 | rights. These restrictions translate to certain responsibilities for 38 | you if you distribute copies of the library or if you modify it. 39 | 40 | For example, if you distribute copies of the library, whether gratis 41 | or for a fee, you must give the recipients all the rights that we gave 42 | you. You must make sure that they, too, receive or can get the source 43 | code. If you link other code with the library, you must provide 44 | complete object files to the recipients, so that they can relink them 45 | with the library after making changes to the library and recompiling 46 | it. And you must show them these terms so they know their rights. 47 | 48 | We protect your rights with a two-step method: (1) we copyright the 49 | library, and (2) we offer you this license, which gives you legal 50 | permission to copy, distribute and/or modify the library. 51 | 52 | To protect each distributor, we want to make it very clear that 53 | there is no warranty for the free library. Also, if the library is 54 | modified by someone else and passed on, the recipients should know 55 | that what they have is not the original version, so that the original 56 | author's reputation will not be affected by problems that might be 57 | introduced by others. 58 | 59 | Finally, software patents pose a constant threat to the existence of 60 | any free program. We wish to make sure that a company cannot 61 | effectively restrict the users of a free program by obtaining a 62 | restrictive license from a patent holder. Therefore, we insist that 63 | any patent license obtained for a version of the library must be 64 | consistent with the full freedom of use specified in this license. 65 | 66 | Most GNU software, including some libraries, is covered by the 67 | ordinary GNU General Public License. This license, the GNU Lesser 68 | General Public License, applies to certain designated libraries, and 69 | is quite different from the ordinary General Public License. We use 70 | this license for certain libraries in order to permit linking those 71 | libraries into non-free programs. 72 | 73 | When a program is linked with a library, whether statically or using 74 | a shared library, the combination of the two is legally speaking a 75 | combined work, a derivative of the original library. The ordinary 76 | General Public License therefore permits such linking only if the 77 | entire combination fits its criteria of freedom. The Lesser General 78 | Public License permits more lax criteria for linking other code with 79 | the library. 80 | 81 | We call this license the "Lesser" General Public License because it 82 | does Less to protect the user's freedom than the ordinary General 83 | Public License. It also provides other free software developers Less 84 | of an advantage over competing non-free programs. These disadvantages 85 | are the reason we use the ordinary General Public License for many 86 | libraries. However, the Lesser license provides advantages in certain 87 | special circumstances. 88 | 89 | For example, on rare occasions, there may be a special need to 90 | encourage the widest possible use of a certain library, so that it becomes 91 | a de-facto standard. To achieve this, non-free programs must be 92 | allowed to use the library. A more frequent case is that a free 93 | library does the same job as widely used non-free libraries. In this 94 | case, there is little to gain by limiting the free library to free 95 | software only, so we use the Lesser General Public License. 96 | 97 | In other cases, permission to use a particular library in non-free 98 | programs enables a greater number of people to use a large body of 99 | free software. For example, permission to use the GNU C Library in 100 | non-free programs enables many more people to use the whole GNU 101 | operating system, as well as its variant, the GNU/Linux operating 102 | system. 103 | 104 | Although the Lesser General Public License is Less protective of the 105 | users' freedom, it does ensure that the user of a program that is 106 | linked with the Library has the freedom and the wherewithal to run 107 | that program using a modified version of the Library. 108 | 109 | The precise terms and conditions for copying, distribution and 110 | modification follow. Pay close attention to the difference between a 111 | "work based on the library" and a "work that uses the library". The 112 | former contains code derived from the library, whereas the latter must 113 | be combined with the library in order to run. 114 | 115 | GNU LESSER GENERAL PUBLIC LICENSE 116 | TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION 117 | 118 | 0. This License Agreement applies to any software library or other 119 | program which contains a notice placed by the copyright holder or 120 | other authorized party saying it may be distributed under the terms of 121 | this Lesser General Public License (also called "this License"). 122 | Each licensee is addressed as "you". 123 | 124 | A "library" means a collection of software functions and/or data 125 | prepared so as to be conveniently linked with application programs 126 | (which use some of those functions and data) to form executables. 127 | 128 | The "Library", below, refers to any such software library or work 129 | which has been distributed under these terms. A "work based on the 130 | Library" means either the Library or any derivative work under 131 | copyright law: that is to say, a work containing the Library or a 132 | portion of it, either verbatim or with modifications and/or translated 133 | straightforwardly into another language. (Hereinafter, translation is 134 | included without limitation in the term "modification".) 135 | 136 | "Source code" for a work means the preferred form of the work for 137 | making modifications to it. For a library, complete source code means 138 | all the source code for all modules it contains, plus any associated 139 | interface definition files, plus the scripts used to control compilation 140 | and installation of the library. 141 | 142 | Activities other than copying, distribution and modification are not 143 | covered by this License; they are outside its scope. The act of 144 | running a program using the Library is not restricted, and output from 145 | such a program is covered only if its contents constitute a work based 146 | on the Library (independent of the use of the Library in a tool for 147 | writing it). Whether that is true depends on what the Library does 148 | and what the program that uses the Library does. 149 | 150 | 1. You may copy and distribute verbatim copies of the Library's 151 | complete source code as you receive it, in any medium, provided that 152 | you conspicuously and appropriately publish on each copy an 153 | appropriate copyright notice and disclaimer of warranty; keep intact 154 | all the notices that refer to this License and to the absence of any 155 | warranty; and distribute a copy of this License along with the 156 | Library. 157 | 158 | You may charge a fee for the physical act of transferring a copy, 159 | and you may at your option offer warranty protection in exchange for a 160 | fee. 161 | 162 | 2. You may modify your copy or copies of the Library or any portion 163 | of it, thus forming a work based on the Library, and copy and 164 | distribute such modifications or work under the terms of Section 1 165 | above, provided that you also meet all of these conditions: 166 | 167 | a) The modified work must itself be a software library. 168 | 169 | b) You must cause the files modified to carry prominent notices 170 | stating that you changed the files and the date of any change. 171 | 172 | c) You must cause the whole of the work to be licensed at no 173 | charge to all third parties under the terms of this License. 174 | 175 | d) If a facility in the modified Library refers to a function or a 176 | table of data to be supplied by an application program that uses 177 | the facility, other than as an argument passed when the facility 178 | is invoked, then you must make a good faith effort to ensure that, 179 | in the event an application does not supply such function or 180 | table, the facility still operates, and performs whatever part of 181 | its purpose remains meaningful. 182 | 183 | (For example, a function in a library to compute square roots has 184 | a purpose that is entirely well-defined independent of the 185 | application. Therefore, Subsection 2d requires that any 186 | application-supplied function or table used by this function must 187 | be optional: if the application does not supply it, the square 188 | root function must still compute square roots.) 189 | 190 | These requirements apply to the modified work as a whole. If 191 | identifiable sections of that work are not derived from the Library, 192 | and can be reasonably considered independent and separate works in 193 | themselves, then this License, and its terms, do not apply to those 194 | sections when you distribute them as separate works. But when you 195 | distribute the same sections as part of a whole which is a work based 196 | on the Library, the distribution of the whole must be on the terms of 197 | this License, whose permissions for other licensees extend to the 198 | entire whole, and thus to each and every part regardless of who wrote 199 | it. 200 | 201 | Thus, it is not the intent of this section to claim rights or contest 202 | your rights to work written entirely by you; rather, the intent is to 203 | exercise the right to control the distribution of derivative or 204 | collective works based on the Library. 205 | 206 | In addition, mere aggregation of another work not based on the Library 207 | with the Library (or with a work based on the Library) on a volume of 208 | a storage or distribution medium does not bring the other work under 209 | the scope of this License. 210 | 211 | 3. You may opt to apply the terms of the ordinary GNU General Public 212 | License instead of this License to a given copy of the Library. To do 213 | this, you must alter all the notices that refer to this License, so 214 | that they refer to the ordinary GNU General Public License, version 2, 215 | instead of to this License. (If a newer version than version 2 of the 216 | ordinary GNU General Public License has appeared, then you can specify 217 | that version instead if you wish.) Do not make any other change in 218 | these notices. 219 | 220 | Once this change is made in a given copy, it is irreversible for 221 | that copy, so the ordinary GNU General Public License applies to all 222 | subsequent copies and derivative works made from that copy. 223 | 224 | This option is useful when you wish to copy part of the code of 225 | the Library into a program that is not a library. 226 | 227 | 4. You may copy and distribute the Library (or a portion or 228 | derivative of it, under Section 2) in object code or executable form 229 | under the terms of Sections 1 and 2 above provided that you accompany 230 | it with the complete corresponding machine-readable source code, which 231 | must be distributed under the terms of Sections 1 and 2 above on a 232 | medium customarily used for software interchange. 233 | 234 | If distribution of object code is made by offering access to copy 235 | from a designated place, then offering equivalent access to copy the 236 | source code from the same place satisfies the requirement to 237 | distribute the source code, even though third parties are not 238 | compelled to copy the source along with the object code. 239 | 240 | 5. A program that contains no derivative of any portion of the 241 | Library, but is designed to work with the Library by being compiled or 242 | linked with it, is called a "work that uses the Library". Such a 243 | work, in isolation, is not a derivative work of the Library, and 244 | therefore falls outside the scope of this License. 245 | 246 | However, linking a "work that uses the Library" with the Library 247 | creates an executable that is a derivative of the Library (because it 248 | contains portions of the Library), rather than a "work that uses the 249 | library". The executable is therefore covered by this License. 250 | Section 6 states terms for distribution of such executables. 251 | 252 | When a "work that uses the Library" uses material from a header file 253 | that is part of the Library, the object code for the work may be a 254 | derivative work of the Library even though the source code is not. 255 | Whether this is true is especially significant if the work can be 256 | linked without the Library, or if the work is itself a library. The 257 | threshold for this to be true is not precisely defined by law. 258 | 259 | If such an object file uses only numerical parameters, data 260 | structure layouts and accessors, and small macros and small inline 261 | functions (ten lines or less in length), then the use of the object 262 | file is unrestricted, regardless of whether it is legally a derivative 263 | work. (Executables containing this object code plus portions of the 264 | Library will still fall under Section 6.) 265 | 266 | Otherwise, if the work is a derivative of the Library, you may 267 | distribute the object code for the work under the terms of Section 6. 268 | Any executables containing that work also fall under Section 6, 269 | whether or not they are linked directly with the Library itself. 270 | 271 | 6. As an exception to the Sections above, you may also combine or 272 | link a "work that uses the Library" with the Library to produce a 273 | work containing portions of the Library, and distribute that work 274 | under terms of your choice, provided that the terms permit 275 | modification of the work for the customer's own use and reverse 276 | engineering for debugging such modifications. 277 | 278 | You must give prominent notice with each copy of the work that the 279 | Library is used in it and that the Library and its use are covered by 280 | this License. You must supply a copy of this License. If the work 281 | during execution displays copyright notices, you must include the 282 | copyright notice for the Library among them, as well as a reference 283 | directing the user to the copy of this License. Also, you must do one 284 | of these things: 285 | 286 | a) Accompany the work with the complete corresponding 287 | machine-readable source code for the Library including whatever 288 | changes were used in the work (which must be distributed under 289 | Sections 1 and 2 above); and, if the work is an executable linked 290 | with the Library, with the complete machine-readable "work that 291 | uses the Library", as object code and/or source code, so that the 292 | user can modify the Library and then relink to produce a modified 293 | executable containing the modified Library. (It is understood 294 | that the user who changes the contents of definitions files in the 295 | Library will not necessarily be able to recompile the application 296 | to use the modified definitions.) 297 | 298 | b) Use a suitable shared library mechanism for linking with the 299 | Library. A suitable mechanism is one that (1) uses at run time a 300 | copy of the library already present on the user's computer system, 301 | rather than copying library functions into the executable, and (2) 302 | will operate properly with a modified version of the library, if 303 | the user installs one, as long as the modified version is 304 | interface-compatible with the version that the work was made with. 305 | 306 | c) Accompany the work with a written offer, valid for at 307 | least three years, to give the same user the materials 308 | specified in Subsection 6a, above, for a charge no more 309 | than the cost of performing this distribution. 310 | 311 | d) If distribution of the work is made by offering access to copy 312 | from a designated place, offer equivalent access to copy the above 313 | specified materials from the same place. 314 | 315 | e) Verify that the user has already received a copy of these 316 | materials or that you have already sent this user a copy. 317 | 318 | For an executable, the required form of the "work that uses the 319 | Library" must include any data and utility programs needed for 320 | reproducing the executable from it. However, as a special exception, 321 | the materials to be distributed need not include anything that is 322 | normally distributed (in either source or binary form) with the major 323 | components (compiler, kernel, and so on) of the operating system on 324 | which the executable runs, unless that component itself accompanies 325 | the executable. 326 | 327 | It may happen that this requirement contradicts the license 328 | restrictions of other proprietary libraries that do not normally 329 | accompany the operating system. Such a contradiction means you cannot 330 | use both them and the Library together in an executable that you 331 | distribute. 332 | 333 | 7. You may place library facilities that are a work based on the 334 | Library side-by-side in a single library together with other library 335 | facilities not covered by this License, and distribute such a combined 336 | library, provided that the separate distribution of the work based on 337 | the Library and of the other library facilities is otherwise 338 | permitted, and provided that you do these two things: 339 | 340 | a) Accompany the combined library with a copy of the same work 341 | based on the Library, uncombined with any other library 342 | facilities. This must be distributed under the terms of the 343 | Sections above. 344 | 345 | b) Give prominent notice with the combined library of the fact 346 | that part of it is a work based on the Library, and explaining 347 | where to find the accompanying uncombined form of the same work. 348 | 349 | 8. You may not copy, modify, sublicense, link with, or distribute 350 | the Library except as expressly provided under this License. Any 351 | attempt otherwise to copy, modify, sublicense, link with, or 352 | distribute the Library is void, and will automatically terminate your 353 | rights under this License. However, parties who have received copies, 354 | or rights, from you under this License will not have their licenses 355 | terminated so long as such parties remain in full compliance. 356 | 357 | 9. You are not required to accept this License, since you have not 358 | signed it. However, nothing else grants you permission to modify or 359 | distribute the Library or its derivative works. These actions are 360 | prohibited by law if you do not accept this License. Therefore, by 361 | modifying or distributing the Library (or any work based on the 362 | Library), you indicate your acceptance of this License to do so, and 363 | all its terms and conditions for copying, distributing or modifying 364 | the Library or works based on it. 365 | 366 | 10. Each time you redistribute the Library (or any work based on the 367 | Library), the recipient automatically receives a license from the 368 | original licensor to copy, distribute, link with or modify the Library 369 | subject to these terms and conditions. You may not impose any further 370 | restrictions on the recipients' exercise of the rights granted herein. 371 | You are not responsible for enforcing compliance by third parties with 372 | this License. 373 | 374 | 11. If, as a consequence of a court judgment or allegation of patent 375 | infringement or for any other reason (not limited to patent issues), 376 | conditions are imposed on you (whether by court order, agreement or 377 | otherwise) that contradict the conditions of this License, they do not 378 | excuse you from the conditions of this License. If you cannot 379 | distribute so as to satisfy simultaneously your obligations under this 380 | License and any other pertinent obligations, then as a consequence you 381 | may not distribute the Library at all. For example, if a patent 382 | license would not permit royalty-free redistribution of the Library by 383 | all those who receive copies directly or indirectly through you, then 384 | the only way you could satisfy both it and this License would be to 385 | refrain entirely from distribution of the Library. 386 | 387 | If any portion of this section is held invalid or unenforceable under any 388 | particular circumstance, the balance of the section is intended to apply, 389 | and the section as a whole is intended to apply in other circumstances. 390 | 391 | It is not the purpose of this section to induce you to infringe any 392 | patents or other property right claims or to contest validity of any 393 | such claims; this section has the sole purpose of protecting the 394 | integrity of the free software distribution system which is 395 | implemented by public license practices. Many people have made 396 | generous contributions to the wide range of software distributed 397 | through that system in reliance on consistent application of that 398 | system; it is up to the author/donor to decide if he or she is willing 399 | to distribute software through any other system and a licensee cannot 400 | impose that choice. 401 | 402 | This section is intended to make thoroughly clear what is believed to 403 | be a consequence of the rest of this License. 404 | 405 | 12. If the distribution and/or use of the Library is restricted in 406 | certain countries either by patents or by copyrighted interfaces, the 407 | original copyright holder who places the Library under this License may add 408 | an explicit geographical distribution limitation excluding those countries, 409 | so that distribution is permitted only in or among countries not thus 410 | excluded. In such case, this License incorporates the limitation as if 411 | written in the body of this License. 412 | 413 | 13. The Free Software Foundation may publish revised and/or new 414 | versions of the Lesser General Public License from time to time. 415 | Such new versions will be similar in spirit to the present version, 416 | but may differ in detail to address new problems or concerns. 417 | 418 | Each version is given a distinguishing version number. If the Library 419 | specifies a version number of this License which applies to it and 420 | "any later version", you have the option of following the terms and 421 | conditions either of that version or of any later version published by 422 | the Free Software Foundation. If the Library does not specify a 423 | license version number, you may choose any version ever published by 424 | the Free Software Foundation. 425 | 426 | 14. If you wish to incorporate parts of the Library into other free 427 | programs whose distribution conditions are incompatible with these, 428 | write to the author to ask for permission. For software which is 429 | copyrighted by the Free Software Foundation, write to the Free 430 | Software Foundation; we sometimes make exceptions for this. Our 431 | decision will be guided by the two goals of preserving the free status 432 | of all derivatives of our free software and of promoting the sharing 433 | and reuse of software generally. 434 | 435 | NO WARRANTY 436 | 437 | 15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO 438 | WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW. 439 | EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR 440 | OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY 441 | KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE 442 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 443 | PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE 444 | LIBRARY IS WITH YOU. SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME 445 | THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION. 446 | 447 | 16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN 448 | WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY 449 | AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU 450 | FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR 451 | CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE 452 | LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING 453 | RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A 454 | FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF 455 | SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH 456 | DAMAGES. 457 | 458 | END OF TERMS AND CONDITIONS 459 | 460 | How to Apply These Terms to Your New Libraries 461 | 462 | If you develop a new library, and you want it to be of the greatest 463 | possible use to the public, we recommend making it free software that 464 | everyone can redistribute and change. You can do so by permitting 465 | redistribution under these terms (or, alternatively, under the terms of the 466 | ordinary General Public License). 467 | 468 | To apply these terms, attach the following notices to the library. It is 469 | safest to attach them to the start of each source file to most effectively 470 | convey the exclusion of warranty; and each file should have at least the 471 | "copyright" line and a pointer to where the full notice is found. 472 | 473 | {description} 474 | Copyright (C) {year} {fullname} 475 | 476 | This library is free software; you can redistribute it and/or 477 | modify it under the terms of the GNU Lesser General Public 478 | License as published by the Free Software Foundation; either 479 | version 2.1 of the License, or (at your option) any later version. 480 | 481 | This library is distributed in the hope that it will be useful, 482 | but WITHOUT ANY WARRANTY; without even the implied warranty of 483 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 484 | Lesser General Public License for more details. 485 | 486 | You should have received a copy of the GNU Lesser General Public 487 | License along with this library; if not, write to the Free Software 488 | Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 489 | USA 490 | 491 | Also add information on how to contact you by electronic and paper mail. 492 | 493 | You should also get your employer (if you work as a programmer) or your 494 | school, if any, to sign a "copyright disclaimer" for the library, if 495 | necessary. Here is a sample; alter the names: 496 | 497 | Yoyodyne, Inc., hereby disclaims all copyright interest in the 498 | library `Frob' (a library for tweaking knobs) written by James Random 499 | Hacker. 500 | 501 | {signature of Ty Coon}, 1 April 1990 502 | Ty Coon, President of Vice 503 | 504 | That's all there is to it! 505 | 506 | -------------------------------------------------------------------------------- /NOTATION: -------------------------------------------------------------------------------- 1 | The Toolbox functions follow a number of conventions. 2 | 3 | For time series data such as a trajectory, rows correspond to increasing time. 4 | 5 | For position data, columns correspond to points. The matrix has 2 rows for 2D data 6 | such as image coordinates, and 3 rows for homogeneous image plane data or 3D points, 7 | and 4 rows for homogeneous 3D data. 8 | 9 | For functions with a complex number of options the common function tb_optparse is 10 | used. This allows a variable number of name/value argument pairs, and also supports 11 | simple enumeration, and loading of arguments from a structure rather than name/value 12 | pairs. All functions that use tb_optparse support a verbose switch that prints more 13 | detail as the function executes. 14 | 15 | This version of the Toolbox makes use of many object classes, and particularly 16 | vectors of objects to represent things such as image feature points or lines. 17 | Often we wish to obtain a vector of scalar properties from the object vector, but 18 | Matlab returns a comma separated list in this case. We therefore adopt the convention 19 | of naming the object properties with a trailing underscore, and creating methods 20 | that return the property in vector form. For example, the method u() returns the 21 | value of the property u_. 22 | 23 | Most objects in the Toolboxes are derived from the handle class, which make it possible 24 | for methods to change the state of the object in a convenient way, that is, writing 25 | 26 | x.modify() 27 | 28 | rather than 29 | 30 | x = x.modify() 31 | 32 | This introduces danger because 33 | 34 | y = x; 35 | y.modify() 36 | 37 | will change both x and y. 38 | 39 | Most object constructors accept an object or object vector and return a copy 40 | 41 | y = Object(x); 42 | 43 | All objects have a char() and display() method which provides a succinct summary of 44 | the object state. 45 | 46 | Imported packages live in the directory /private below the appropriate Toolbox or 47 | within a class directory. 48 | -------------------------------------------------------------------------------- /Polygon.m: -------------------------------------------------------------------------------- 1 | %POLYGON Polygon class 2 | % 3 | % A general class for manipulating polygons and vectors of polygons. 4 | % 5 | % Methods:: 6 | % plot Plot polygon 7 | % area Area of polygon 8 | % moments Moments of polygon 9 | % centroid Centroid of polygon 10 | % perimeter Perimter of polygon 11 | % transform Transform polygon 12 | % inside Test if points are inside polygon 13 | % intersection Intersection of two polygons 14 | % difference Difference of two polygons 15 | % union Union of two polygons 16 | % xor Exclusive or of two polygons 17 | % display print the polygon in human readable form 18 | % char convert the polgyon to human readable string 19 | % 20 | % Properties:: 21 | % vertices List of polygon vertices, one per column 22 | % extent Bounding box [minx maxx; miny maxy] 23 | % n Number of vertices 24 | % 25 | % Notes:: 26 | % - This is reference class object 27 | % - Polygon objects can be used in vectors and arrays 28 | % 29 | % Acknowledgement:: 30 | % 31 | % The methods: inside, intersection, difference, union, and xor are based on code 32 | % written by: 33 | % Kirill K. Pankratov, kirill@plume.mit.edu, 34 | % http://puddle.mit.edu/~glenn/kirill/saga.html 35 | % and require a licence. However the author does not respond to email regarding 36 | % the licence, so use with care, and modify with acknowledgement. 37 | 38 | 39 | % Copyright (C) 1993-2017, by Peter I. Corke 40 | % 41 | % This file is part of The Robotics Toolbox for MATLAB (RTB). 42 | % 43 | % RTB is free software: you can redistribute it and/or modify 44 | % it under the terms of the GNU Lesser General Public License as published by 45 | % the Free Software Foundation, either version 3 of the License, or 46 | % (at your option) any later version. 47 | % 48 | % RTB is distributed in the hope that it will be useful, 49 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 50 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 51 | % GNU Lesser General Public License for more details. 52 | % 53 | % You should have received a copy of the GNU Leser General Public License 54 | % along with RTB. If not, see . 55 | % 56 | % http://www.petercorke.com 57 | 58 | % TODO 59 | % split the code in two. Simple polygon functions in Polgon class, subclass with 60 | % Pankratov code to Polygon2. 61 | % add method to detect empty polygon, overload isempty 62 | 63 | classdef Polygon < handle 64 | 65 | properties 66 | vertices 67 | extent 68 | end 69 | 70 | properties (Dependent=true) 71 | n 72 | x 73 | y 74 | end 75 | 76 | methods 77 | 78 | function p = Polygon(v, wh) 79 | %Polygon.Polygon Polygon class constructor 80 | % 81 | % P = Polygon(V) is a polygon with vertices given by V, one column per 82 | % vertex. 83 | % 84 | % P = Polygon(C, WH) is a rectangle centred at C with dimensions 85 | % WH=[WIDTH, HEIGHT]. 86 | 87 | if nargin == 0 88 | p.n = 0; 89 | p.vertices = []; 90 | return; 91 | end 92 | 93 | if nargin < 2 94 | if numrows(v) ~= 2 95 | error('vertices must have two rows'); 96 | end 97 | p.vertices = v; 98 | end 99 | if nargin == 2 100 | if length(v) ~= 2 101 | error('first argument must be polygon centre'); 102 | end 103 | if length(wh) ~= 2 104 | error('second arugment must be width height'); 105 | end 106 | 107 | p.vertices = [ 108 | v(1)-wh(1)/2 v(1)+wh(1)/2 v(1)+wh(1)/2 v(1)-wh(1)/2 109 | v(2)-wh(2)/2 v(2)-wh(2)/2 v(2)+wh(2)/2 v(2)+wh(2)/2 ]; 110 | end 111 | 112 | % compute the extent 113 | p.extent(1,1) = min(p.x); 114 | p.extent(1,2) = max(p.x); 115 | p.extent(2,1) = min(p.y); 116 | p.extent(2,2) = max(p.y); 117 | end 118 | 119 | function r = get.n(p) 120 | r = numcols(p.vertices); 121 | end 122 | 123 | function r = get.x(p) 124 | r = p.vertices(1,:)'; 125 | end 126 | 127 | function r = get.y(p) 128 | r = p.vertices(2,:)'; 129 | end 130 | 131 | function r = set.n(p) 132 | error('cant set property'); 133 | end 134 | function r = set.x(p) 135 | error('cant set property'); 136 | end 137 | function r = set.y(p) 138 | error('cant set property'); 139 | end 140 | 141 | function ss = char(p) 142 | %Polygon.char String representation 143 | % 144 | % S = P.char() is a compact representation of the polgyon in human 145 | % readable form. 146 | ss = ''; 147 | for i=1:length(p) 148 | if p(i).n <= 4 149 | if length(p) > 1 150 | s = sprintf('%2d: ', i); 151 | else 152 | s = ''; 153 | end 154 | v = p(i).vertices; 155 | for k=1:p(i).n 156 | s = strcat(s, sprintf('(%g,%g)', v(:,k))); 157 | if k~=p(i).n 158 | s = strcat(s, ', '); 159 | end 160 | end 161 | 162 | else 163 | s = sprintf('... %d vertices', p.n) 164 | end 165 | ss =strvcat(ss, s); 166 | end 167 | end 168 | 169 | function display(p) 170 | %Polygon.display Display polygon 171 | % 172 | % P.display() displays the polygon in a compact human readable form. 173 | % 174 | % See also Polygon.char. 175 | loose = strcmp( get(0, 'FormatSpacing'), 'loose'); 176 | if loose 177 | disp(' '); 178 | end 179 | disp([inputname(1), ' = ']) 180 | if loose 181 | disp(' '); 182 | end 183 | disp(char(p)) 184 | if loose 185 | disp(' '); 186 | end 187 | end 188 | 189 | function plot(plist, varargin) 190 | %Polygon.plot Draw polygon 191 | % 192 | % P.plot() draws the polygon P in the current plot. 193 | % 194 | % P.plot(LS) as above but pass the arguments LS to plot. 195 | % 196 | % Notes:: 197 | % - The polygon is added to the current plot. 198 | 199 | opt.fill = []; 200 | [opt,args] = tb_optparse(opt, varargin); 201 | 202 | ish = ishold; 203 | hold all 204 | 205 | for p=plist 206 | % for every polygon in the list 207 | 208 | % get the vertices 209 | X = p.vertices(1,:)'; 210 | Y = p.vertices(2,:)'; 211 | 212 | while true 213 | 214 | % look for NaNs which indicate disjoint vertex sets 215 | k = find(isnan(X)); 216 | 217 | if length(k) > 0 218 | % if a NaN chop out the segment before and after 219 | k = k(1); 220 | x = X(1:k-1); 221 | y = Y(1:k-1); 222 | X = X(k+1:end); 223 | Y = Y(k+1:end); 224 | else 225 | x = X; y = Y; 226 | end 227 | 228 | % close the polygon 229 | x = [x; x(1)]; 230 | y = [y; y(1)]; 231 | 232 | if opt.fill 233 | patch(x, y, opt.fill); 234 | else 235 | plot(x, y, args{:}); 236 | end 237 | 238 | if length(k) == 0 239 | break; 240 | end 241 | end 242 | end 243 | if ~ish 244 | hold off 245 | end 246 | 247 | end 248 | 249 | function a = area(p) 250 | %Polygon.area Area of polygon 251 | % 252 | % A = P.area() is the area of the polygon. 253 | % 254 | % See also Polygon.moments. 255 | a = p.moments(0, 0); 256 | end 257 | 258 | function m = moments(p, mp, mq) 259 | %Polygon.moments Moments of polygon 260 | % 261 | % A = P.moments(p, q) is the pq'th moment of the polygon. 262 | % 263 | % See also Polygon.area, Polygon.centroid, mpq_poly. 264 | m = mpq_poly(p.vertices, mp, mq); 265 | end 266 | 267 | function q = transform(p, T) 268 | %Polygon.transform Transform polygon vertices 269 | % 270 | % P2 = P.transform(T) is a new Polygon object whose vertices have 271 | % been transformed by the SE(2) homgoeneous transformation T (3x3). 272 | if length(T) == 3 273 | T = se2(T); 274 | end 275 | q = Polygon( homtrans(T, p.vertices) ); 276 | end 277 | 278 | function f = inside(p, points) 279 | %Polygon.inside Test if points are inside polygon 280 | % 281 | % IN = P.inside(P) tests if points given by columns of P (2xN) are inside 282 | % the polygon. The corresponding elements of IN (1xN) are either true or 283 | % false. 284 | f = inpolygon(points(1,:), points(2,:), p.x, p.y); 285 | end 286 | 287 | function c = centroid(p) 288 | %Polygon.centroid Centroid of polygon 289 | % 290 | % X = P.centroid() is the centroid of the polygon. 291 | % 292 | % See also Polygon.moments. 293 | c = [p.moments(1,0) p.moments(0,1)] / p.area(); 294 | end 295 | 296 | function r = perimeter(p) 297 | %Polygon.perimeter Perimeter of polygon 298 | % 299 | % L = P.perimeter() is the perimeter of the polygon. 300 | p = sum(sqrt(diff(p.x).^2+diff(p.y).^2)); 301 | end 302 | 303 | function f = intersect(p, plist) 304 | %Polygon.intersect Intersection of polygon with list of polygons 305 | % 306 | % I = P.intersect(PLIST) indicates whether or not the Polygon P 307 | % intersects with 308 | % 309 | % i(j) = 1 if p intersects polylist(j), else 0. 310 | 311 | % Based on ISINTPL 312 | % Copyright (c) 1995 by Kirill K. Pankratov, 313 | % kirill@plume.mit.edu. 314 | % 06/20/95, 08/25/95 315 | f = []; 316 | for q=plist 317 | f = [f isintpl(p.x, p.y, q.x, q.y)]; 318 | end 319 | end 320 | 321 | function f = intersect_line(p, l) 322 | %Polygon.intersect_line Intersection of polygon and line segment 323 | % 324 | % I = P.intersect_line(L) is the intersection points of a polygon P with 325 | % the line segment L=[x1 x2; y1 y2]. I (2xN) has one column per 326 | % intersection, each column is [x y]'. 327 | 328 | f = []; 329 | % find intersections 330 | for i=1:p.n 331 | in = mod(i, p.n)+1; 332 | 333 | xv = [p.x(i); p.x(in)]; 334 | yv = [p.y(i); p.y(in)]; 335 | intsec = iscross(xv, yv, l(1,:)', l(2,:)'); 336 | if intsec 337 | [x,y] = intsecl(xv, yv, l(1,:)', l(2,:)'); 338 | f = [f [x;y]]; 339 | end 340 | end 341 | end 342 | 343 | function r = difference(p, q) 344 | %Polygon.difference Difference of polygons 345 | % 346 | % D = P.difference(Q) is polygon P minus polygon Q. 347 | % 348 | % Notes:: 349 | % - If polygons P and Q are not intersecting, returns 350 | % coordinates of P. 351 | % - If the result D is not simply connected or consists of 352 | % several polygons, resulting vertex list will contain NaNs. 353 | 354 | % POLYDIFF Difference of 2 polygons. 355 | % [XO,YO] = POLYDIFF(X1,Y1,X2,Y2) Calculates polygon(s) P 356 | % of difference of polygons P1 and P1 with coordinates 357 | % X1, Y1 and X2, Y2. 358 | % The resulting polygon(s) is a set of all points which belong 359 | % to P1 but not to to P2: P = P1 & ~P2. 360 | % The input polygons must be non-self-intersecting and 361 | % simply connected. 362 | % 363 | % If polygons P1, P2 are not intersecting, returns 364 | % coordinates of the first polygon X1, X2. 365 | % If the result P is not simply connected or consists of several 366 | % polygons, resulting boundary consists of concatenated 367 | % coordinates of these polygons, separated by NaN. 368 | % Copyright (c) 1995 by Kirill K. Pankratov, 369 | % kirill@plume.mit.edu. 370 | % 06/25/95 371 | 372 | % Call POLYBOOL with flag=3 373 | [xo,yo,ind] = polybool(p.x, p.y, q.x, q.y, 3); 374 | 375 | r = Polygon([xo(:) yo(:)]'); 376 | end 377 | 378 | function r = intersection(p, q) 379 | %Polygon.intersection Intersection of polygons 380 | % 381 | % I = P.intersection(Q) is a Polygon representing the 382 | % intersection of polygons P and Q. 383 | % 384 | % Notes:: 385 | % - If these polygons are not intersecting, returns empty polygon. 386 | % - If intersection consist of several disjoint polygons 387 | % (for non-convex P or Q) then vertices of I is the concatenation 388 | % of the vertices of these polygons. 389 | 390 | 391 | % POLYINTS Intersection of 2 polygons. 392 | % [XO,YO] = POLYINTS(X1,Y1,X2,Y2) Calculates polygon(s) 393 | % if intersection of polygons with coordinates X1, Y1 394 | % and X2, Y2. 395 | % The resulting polygon(s) is a set of all points which 396 | % belong to both P1 and P2: P = P1 & P2. 397 | % These polygons must be non-self-intersecting and 398 | % simply connected. 399 | % 400 | % If these polygons are not intersecting, returns empty. 401 | % If intersection consist of several disjoint polygons 402 | % (for non-convex P1 or P2) output vectors XO, YO consist 403 | % of concatenated cooddinates of these polygons, 404 | % Copyright (c) 1995 by Kirill K. Pankratov, 405 | % kirill@plume.mit.edu. 406 | % 06/25/95 407 | 408 | 409 | % Call POLYBOOL with flag=1 410 | [xo,yo,ind] = polybool(p.x, p.y, q.x, q.y, 1); 411 | 412 | 413 | r = Polygon([xo(:) yo(:)]'); 414 | end 415 | 416 | function r = union(p, q) 417 | %Polygon.union Union of polygons 418 | % 419 | % I = P.union(Q) is a polygon representing the 420 | % union of polygons P and Q. 421 | % 422 | % Notes:: 423 | % - If these polygons are not intersecting, returns a polygon with 424 | % vertices of both polygons separated by NaNs. 425 | % - If the result P is not simply connected (such as a polygon 426 | % with a "hole") the resulting contour consist of counter- 427 | % clockwise "outer boundary" and one or more clock-wise 428 | % "inner boundaries" around "holes". 429 | 430 | % POLYUNI Union of 2 polygons. 431 | % [XO,YO] = POLYINT(X1,Y1,X2,Y2) Calculates polygon(s) P 432 | % which is (are) union of polygons P1 and P2 with coordinates 433 | % X1, Y1 and X2, Y2. 434 | % The resulting polygon(s) is a set of all points which belong 435 | % either to P1 or to P2: P = P1 | P2. 436 | % The input polygons must be non-self-intersecting and 437 | % simply connected. 438 | % 439 | % If polygons P1, P2 are not intersecting, returns 440 | % coordinates of the both polygons separated by NaN. 441 | % If both P1 and P2 are convex, their boundaries can have no 442 | % more than 2 intersections. The result is also a convex 443 | % polygon. 444 | % If the result P is not simply connected (such as a polygon 445 | % with a "hole") the resulting contour consist of counter- 446 | % clockwise "outer boundary" and one or more clock-wise 447 | % "inner boundaries" around "holes". 448 | % Copyright (c) 1995 by Kirill K. Pankratov, 449 | % kirill@plume.mit.edu. 450 | % 06/25/95 451 | 452 | 453 | % Call POLYBOOL with flag=2 .......... 454 | [xo,yo,ind] = polybool(p.x, p.y, q.x, q.y, 2); 455 | 456 | 457 | r = Polygon([xo(:) yo(:)]'); 458 | end 459 | 460 | function r = xor(p, q) 461 | %Polygon.xor Exclusive or of polygons 462 | % 463 | % I = P.union(Q) is a polygon representing the 464 | % exclusive-or of polygons P and Q. 465 | % 466 | % Notes:: 467 | % - If these polygons are not intersecting, returns a polygon with 468 | % vertices of both polygons separated by NaNs. 469 | % - If the result P is not simply connected (such as a polygon 470 | % with a "hole") the resulting contour consist of counter- 471 | % clockwise "outer boundary" and one or more clock-wise 472 | % "inner boundaries" around "holes". 473 | 474 | % POLYXOR Exclusive OR of 2 polygons. 475 | % [XO,YO] = POLYXOR(X1,Y1,X2,Y2) Calculates polygon(s) P 476 | % of difference of polygons P1 and P1 with coordinates 477 | % X1, Y1 and X2, Y2. 478 | % The resulting polygon(s) is a set of all points which belong 479 | % either to P1 or to P2 but not to both: 480 | % P = (P1 & ~P2) | (P2 & ~P1). 481 | % The input polygons must be non-self-intersecting and 482 | % simply connected. 483 | % 484 | % If polygons P1, P2 are not intersecting, returns 485 | % coordinates of the both polygons separated by NaN. 486 | % If the result P is not simply connected or consists of several 487 | % polygons, resulting boundary consists of concatenated 488 | % coordinates of these polygons, separated by NaN. 489 | 490 | % Copyright (c) 1995 by Kirill K. Pankratov, 491 | % kirill@plume.mit.edu. 492 | % 06/25/95 493 | 494 | 495 | % Call POLYBOOL twice with flag=3 496 | [xx,yy,ind] = polybool(p.x, p.y, q.x, q.y, 3); 497 | 498 | xo = [xx; NaN]; yo = [yy; NaN]; 499 | [xx,yy,ind] = polybool(q.x, q.y, p.x, p.y, 3); 500 | 501 | xo = [xo; xx]; yo = [yo; yy]; 502 | 503 | r = Polygon([xo(:) yo(:)]'); 504 | 505 | 506 | end 507 | end % methods 508 | end % classdef 509 | 510 | 511 | function [is,in,un] = interval(x1,x2) 512 | 513 | % Intersection and union of 2 intervals. 514 | % [IS,IN,UN] = INTERVAL(X1,X2) calculates pair-wise 515 | % intersection IN and union UN of N pairs of 516 | % intervals with coordinates X1 and X2 (both are 517 | % 2 by N vectors). Returns 1 by N boolean vector IS 518 | % equal to 1 if intervals have non-empty intersection 519 | % and 0 if they don't. 520 | 521 | % Copyright (c) 1995 by Kirill K. Pankratov, 522 | % kirill@plume.mit.edu. 523 | % 08/24/95 524 | 525 | % Handle input ........................... 526 | if nargin==0, help interval, return, end 527 | if nargin==1 528 | un = x1; 529 | else 530 | un = [x1; x2]; 531 | end 532 | 533 | [in,un] = sort(un); % Sort both intervals together 534 | un = un(1:2,:)-1; 535 | is = sum(floor(un/2)); % Check for [0 0 1 1] or [1 1 0 0] 536 | is = (is==1); 537 | ii = find(in(2,:)==in(3,:)); 538 | is(ii) = .5*ones(size(ii)); 539 | 540 | % Extract intersection and union from sorted coordinates 541 | if nargout>1 542 | un = in([1 4],:); 543 | in = in(2:3,:); 544 | in(:,~is) = flipud(in(:,~is)); 545 | end 546 | end 547 | 548 | function [is,S] = iscross(x1,y1,x2,y2,tol) 549 | 550 | % ISCROSS Finds whether pairs of lines cross each other 551 | % [IS,S] = ISCROSS(X1,Y1,X2,Y2) where arguments X1, Y1, 552 | % X2, Y2 are all 2 by N matrices are coordinates of 553 | % ends of the pairs of line segments. 554 | % Returns vector IS (1 by N) consisting of ones if 555 | % corresponding pairs cross each other, zeros if they 556 | % don't and .5 if an end of one line segment lies on 557 | % another segment. 558 | % Also returns a matrix S (4 by N) with each row 559 | % consisting of cross products (double areas of 560 | % corresponding triangles) built on the following points: 561 | % (X2(1,:),Y2(1,:)),(X1(1,:),Y1(1,:)),(X2(2,:),Y2(2,:)), 562 | % (X2(1,:),Y2(1,:)),(X1(2,:),Y1(2,:)),(X2(2,:),Y2(2,:)) 563 | % (X1(1,:),Y1(1,:)),(X2(1,:),Y2(1,:)),(X1(2,:),Y1(2,:)) 564 | % (X1(1,:),Y1(1,:)),(X2(2,:),Y2(2,:)),(X1(2,:),Y1(2,:)) 565 | % The signs of these 4 areas can be used to determine 566 | % whether these lines and their continuations cross each 567 | % other. 568 | % [IS,S] = ISCROSS(X1,Y1,X2,Y2,TOL) uses tolerance TOL 569 | % for detecting the crossings (default is 0). 570 | 571 | % Copyright (c) 1995 by Kirill K. Pankratov 572 | % kirill@plume.mit.edu 573 | % 08/14/94, 05/18/95, 08/25/95 574 | 575 | % Defaults and parameters ....................... 576 | tol_dflt = 0; % Tolerance for area calculation 577 | is_chk = 1; % Check input arguments 578 | 579 | % Handle input .................................. 580 | if nargin==0, help iscross, return, end 581 | if nargin<4 % Check if all 4 entered 582 | error(' Not enough input arguments') 583 | end 584 | if nargin<5, tol = tol_dflt; end 585 | if tol < 0, is_chk = 0; tol = 0; end 586 | 587 | % Check the format of arguments ................. 588 | if is_chk 589 | [x1,y1,x2,y2] = linechk(x1,y1,x2,y2); 590 | end 591 | 592 | len = size(x1,2); 593 | o2 = ones(2,1); 594 | 595 | % Find if the ranges of pairs of segments intersect 596 | [isx,S,A] = interval(x1,x2); 597 | scx = diff(A); 598 | [isy,S,A] = interval(y1,y2); 599 | scy = diff(A); 600 | is = isx & isy; 601 | 602 | % If S values are not needed, extract only those pairs 603 | % which have intersecting ranges .............. 604 | if nargout < 2 605 | isx = find(is); % Indices of pairs to be checked 606 | % further 607 | x1 = x1(:,isx); 608 | x2 = x2(:,isx); 609 | y1 = y1(:,isx); 610 | y2 = y2(:,isx); 611 | is = is(isx); 612 | if isempty(is), is = zeros(1,len); return, end 613 | scx = scx(isx); 614 | scy = scy(isx); 615 | end 616 | 617 | % Rescale by ranges ........................... 618 | x1 = x1.*scx(o2,:); 619 | x2 = x2.*scx(o2,:); 620 | y1 = y1.*scy(o2,:); 621 | y2 = y2.*scy(o2,:); 622 | 623 | 624 | % Calculate areas ............................. 625 | S = zeros(4,length(scx)); 626 | S(1,:) = (x2(1,:)-x1(1,:)).*(y2(2,:)-y1(1,:)); 627 | S(1,:) = S(1,:)-(x2(2,:)-x1(1,:)).*(y2(1,:)-y1(1,:)); 628 | 629 | S(2,:) = (x2(1,:)-x1(2,:)).*(y2(2,:)-y1(2,:)); 630 | S(2,:) = S(2,:)-(x2(2,:)-x1(2,:)).*(y2(1,:)-y1(2,:)); 631 | 632 | S(3,:) = (x1(1,:)-x2(1,:)).*(y1(2,:)-y2(1,:)); 633 | S(3,:) = S(3,:)-(x1(2,:)-x2(1,:)).*(y1(1,:)-y2(1,:)); 634 | 635 | S(4,:) = (x1(1,:)-x2(2,:)).*(y1(2,:)-y2(2,:)); 636 | S(4,:) = S(4,:)-(x1(2,:)-x2(2,:)).*(y1(1,:)-y2(2,:)); 637 | 638 | 639 | % Find if they cross each other ............... 640 | is = (S(1,:).*S(2,:)<=0)&(S(3,:).*S(4,:)<=0); 641 | 642 | 643 | % Find very close to intersection 644 | isy = min(abs(S)); 645 | ii = find(isy<=tol & is); 646 | is(ii) = .5*ones(size(ii)); 647 | 648 | % Output 649 | if nargout < 2 650 | isy = zeros(1,len); 651 | isy(isx) = is; 652 | is = isy; 653 | 654 | else 655 | isy = scx.*scy; 656 | ii = find(~isy); 657 | isy(ii) = ones(size(ii)); 658 | S = S./isy(ones(4,1),:); 659 | 660 | end 661 | 662 | end 663 | 664 | function [xo,yo,ind] = polybool(x1,y1,x2,y2,flag) 665 | 666 | % [XO,YO] = POLYBOOL(X1,Y1,X2,Y2,FLAG) 667 | % calulates results of Boolean operations on 668 | % a pair of polygons. 669 | % FLAG Specifies the type of the operation: 670 | % 1 - Intersection (P1 & P2) 671 | % 2 - Union (P1 | P2) 672 | % 3 - Difference (P1 & ~P2) 673 | 674 | % Copyright (c) 1995 by Kirill K. Pankratov, 675 | % kirill@plume.mit.edu. 676 | % 06/25/95, 09/07/95 677 | 678 | % This program calls the following functions: 679 | % AREA, ISINTPL, ISCROSS, INTSECL. 680 | 681 | % Algorithm: 682 | % 1. Check boundary contour directions (area). 683 | % For intersection and union make all 684 | % counter-clockwise. For difference make the second 685 | % contour clock-wise. 686 | % 2. Calculate matrix of intersections (function ISINTPL). 687 | % Quick exit if no intersections. 688 | % 3. For intersecting segments calculate intersection 689 | % coordinates (function INTSECL). 690 | % 4. Sort intersections along both contours. 691 | % 5. Calculate sign of cross-product between intersectiong 692 | % segments. This will give which contour goes "in" and 693 | % "out" at intersections. 694 | % 695 | % 6. Start with first intersection: 696 | % Determine direction to go ("in" for intersection, 697 | % "out" for union). 698 | % Move until next intersection, switch polygons at each 699 | % intersection until coming to the initial point. 700 | % If not all intersections are encountered, the 701 | % resulting polygon is disjoint. Separate output 702 | % coordinates by NaN and repeat procedure until all 703 | % intersections are counted. 704 | 705 | % Default for flag 706 | flag_dflt = 1; % 1- intersec., 2-union, 3 - diff. 707 | 708 | % Handle input 709 | if nargin==0, help polybool, return, end 710 | if nargin < 4 711 | error(' Not enough input arguments') 712 | end 713 | if nargin<5, flag = flag_dflt; end 714 | 715 | x1 = x1(:); y1 = y1(:); 716 | x2 = x2(:); y2 = y2(:); 717 | l1 = length(x1); 718 | l2 = length(x2); 719 | 720 | % Check areas and reverse if negative 721 | nn1 = area(x1,y1); 722 | if nn1<0, x1 = flipud(x1); y1 = flipud(y1); end 723 | nn2 = area(x2,y2); 724 | if (nn2<0 & flag<3) | (nn2>0 & flag==3) 725 | x2 = flipud(x2); y2 = flipud(y2); 726 | end 727 | 728 | % If both polygons are identical ........ 729 | if l1==l2 730 | if all(x1==x2) & all(y1==y2) 731 | if flag<3, xo = x1; yo = y1; ind = 1:l1; 732 | else, xo = []; yo = []; ind = []; end 733 | return 734 | end 735 | end 736 | 737 | % Calculate matrix of intersections ..... 738 | [is,C] = isintpl(x1,y1,x2,y2); 739 | is = any(any(C)); 740 | 741 | % Quick exit if no intersections ........ 742 | if ~is 743 | if flag==1 % Intersection 744 | xo=[]; yo = []; 745 | elseif flag==2 % Union 746 | xo = [x1; nan; x2]; 747 | yo = [y1; nan; y2]; 748 | elseif flag==3 % Difference 749 | xo = x1; yo = y1; 750 | end 751 | return 752 | end 753 | 754 | % Mark intersections with unique numbers 755 | i1 = find(C); 756 | ni = length(i1); 757 | C(i1) = 1:ni; 758 | 759 | % Close polygon contours 760 | x1 = [x1; x1(1)]; y1 = [y1; y1(1)]; 761 | x2 = [x2; x2(1)]; y2 = [y2; y2(1)]; 762 | l1 = length(x1); l2 = length(x2); 763 | 764 | % Calculate intersections themselves 765 | [i1,i2,id] = find(C); 766 | xs1 = [x1(i1) x1(i1+1)]'; ys1 = [y1(i1) y1(i1+1)]'; 767 | xs2 = [x2(i2) x2(i2+1)]'; ys2 = [y2(i2) y2(i2+1)]'; 768 | 769 | % Call INTSECL ............................ 770 | [xint,yint] = intsecl(xs1,ys1,xs2,ys2); 771 | 772 | % For sements belonging to the same line 773 | % find interval of intersection ........... 774 | ii = find(xint==inf); 775 | if ~isempty(ii) 776 | [is,inx] = interval(xs1(:,ii),xs2(:,ii)); 777 | [is,iny] = interval(ys1(:,ii),ys2(:,ii)); 778 | xint(ii) = mean(inx); 779 | yint(ii) = mean(iny); 780 | end 781 | 782 | % Coordinate differences of intersecting segments 783 | xs1 = diff(xs1); ys1 = diff(ys1); 784 | xs2 = diff(xs2); ys2 = diff(ys2); 785 | 786 | % Calculate cross-products 787 | cp = xs1.*ys2-xs2.*ys1; 788 | cp = cp>0; 789 | if flag==2, cp=~cp; end % Reverse if union 790 | cp(ii) = 2*ones(size(ii)); 791 | 792 | % Sort intersections along the contours 793 | ind = (xint-x1(i1)').^2+(yint-y1(i1)').^2; 794 | ind = ind./(xs1.^2+ys1.^2); 795 | cnd = min(ind(ind>0)); 796 | ind = ind+i1'+i2'/(ni+1)*cnd*0; 797 | [xo,ii] = sort(ind); 798 | xs1 = id(ii); 799 | [xo,ind] = sort(xs1); 800 | ind = rem(ind,ni)+1; 801 | xs1 = xs1(ind); 802 | 803 | ind = (xint-x2(i2)').^2+(yint-y2(i2)').^2; 804 | ind = ind./(xs2.^2+ys2.^2); 805 | cnd = min(ind(ind>0)); 806 | [xo,ii] = sort(i2'+ind+i1'/(ni+1)*cnd*0); 807 | xs2 = id(ii); 808 | [xo,ind] = sort(xs2); 809 | ind = rem(ind,ni)+1; 810 | xs2 = xs2(ind); 811 | 812 | % Combine coordinates in one vector 813 | x1 = [x1; x2]; y1 = [y1; y2]; 814 | 815 | % Find max. possible length of a chain 816 | xo = find(any(C')); 817 | xo = diff([xo xo(1)+l1]); 818 | mlen(1) = max(xo); 819 | xo = find(any(C)); 820 | xo = diff([xo xo(1)+l2]); 821 | mlen(2) = max(xo); 822 | 823 | % Check if multiple intersections in one segment 824 | xo = diff([i1 i2]); 825 | is_1 = ~all(all(xo)); 826 | 827 | % Begin counting intersections ********************* 828 | 829 | % Initialization .................. 830 | int = zeros(size(xint)); 831 | nn = 1; % First intersection 832 | nn1 = i1(nn); nn2 = i2(nn); 833 | b = cp(nn); 834 | is2 = b==2; 835 | xo = []; yo = []; ind = []; 836 | closed = 0; 837 | 838 | % Proceed until all intersections are counted 839 | while ~closed % begin counting `````````````````````0 840 | 841 | % If contour closes, find new starting point 842 | if int(nn) & ~all(int) 843 | ii = find(int); 844 | C(id(ii)) = zeros(size(ii)); 845 | nn = min(find(~int)); % Next intersection 846 | nn1 = i1(nn); 847 | nn2 = i2(nn); 848 | xo = [xo; nan]; % Separate by NaN 849 | yo = [yo; nan]; 850 | ind = [ind; nan]; 851 | % Choose direction ...... 852 | b = cp(nn); 853 | end 854 | 855 | % Add current intersection ...... 856 | xo = [xo; xint(nn)]; 857 | yo = [yo; yint(nn)]; 858 | ind = [ind; 0]; 859 | int(nn) = 1; 860 | closed = all(int); 861 | 862 | % Find next segment 863 | % Indices for next intersection 864 | if ~b, nn = xs1(nn); 865 | else, nn = xs2(nn); 866 | end 867 | if ~b, pt0 = nn1; else, pt0 = nn2; end 868 | 869 | nn1 = i1(nn); 870 | nn2 = i2(nn); 871 | 872 | if b, pt = nn2; else, pt = nn1; end 873 | 874 | if b, pt0 = pt0+l1; pt = pt+l1; end 875 | ii = (pt0+1:pt); 876 | 877 | 878 | % Go through the beginning .............. 879 | cnd = pt1); 880 | if cnd 881 | if ~b, ii = [pt0+1:l1 1:pt]; 882 | else, ii = [pt0+1:l1+l2 l1+1:pt]; 883 | end 884 | end 885 | len = length(ii); 886 | cnd = b & len>mlen(2); 887 | cnd = cnd | (~b & len>mlen(1)); 888 | if is2 | cnd, ii=[]; end 889 | 890 | 891 | % Add new segment 892 | xo = [xo; x1(ii)]; 893 | yo = [yo; y1(ii)]; 894 | ind = [ind; ii']; 895 | 896 | % Switch direction 897 | if cp(nn)==2, b = ~b; is2 = 1; 898 | else, b = cp(nn); is2 = 0; 899 | end 900 | 901 | end % End while (all intersections) '''''''''''''''0 902 | 903 | % Remove coincident successive points 904 | ii = find(~diff(xo) & ~diff(yo)); 905 | xo(ii) = []; yo(ii) = []; ind(ii) = []; 906 | 907 | % Remove points which are 908 | ii = find(isnan(xo)); 909 | if ~isempty(ii) 910 | i2 = ones(size(xo)); 911 | ii = [ii; length(xo)+1]; 912 | 913 | i1 = find(diff(ii)==3); 914 | i1 = ii(i1); 915 | i1 = [i1; i1+1; i1+2]; 916 | i2(i1) = zeros(size(i1)); 917 | 918 | i1 = find(diff(ii)==2); 919 | i1 = ii(i1); 920 | i1 = [i1; i1+1]; 921 | i2(i1) = zeros(size(i1)); 922 | 923 | xo = xo(i2); yo = yo(i2); ind = ind(i2); 924 | end 925 | end 926 | 927 | function [xo,yo] = intsecpl(xv,yv,xl,yl,trace) 928 | 929 | % INTSECPL Intersection of a polygon and a line. 930 | % [XI,YI] = INTSECPL(XV,YV,XL,YL) calculates 931 | % intersections XI, YI of a polygon with vertices XV, 932 | % YV and a line specified by pairs of end coordinates 933 | % XL = [XL0 XL1], YL = [YL0 YL1]. Line is assumed to 934 | % continue beyond the range of end points. 935 | % INTSECPL(XV,YV,[A B]) uses another specification for 936 | % a line: Y = A*X+B. 937 | % 938 | % If a line does not intersect polygon, returns empty 939 | % XI, YI. 940 | % For convex polygon maximum number of intersections is 941 | % 2, for non-convex polygons multiple intersections are 942 | % possible. 943 | % 944 | % INTSECPL(XV,YV,XL,YL) by itself or 945 | % [XI,YI] = INTSECPL(XV,YV,XL,YL,1) plots polygon, 946 | % a line segment and intersection segment(s) 947 | % (part(s) of the same line inside the polygon). 948 | 949 | % Copyright (c) 1995 by Kirill K. Pankratov, 950 | % kirill@plume.mit.edu. 951 | % 06/25/95, 08/27/95, 09/27/95 952 | 953 | % Calls ISCROSS, INTSECL programs. 954 | 955 | 956 | % Defaults and parameters ................................. 957 | tol = 1e-14; % Tolerance 958 | marg = tol; % Margins for polygon frame 959 | is_ab = 0; % Default A*X+B mode 960 | 961 | % Handle input ............................................ 962 | if nargin==0, help intsecpl, return, end 963 | if nargin < 3 964 | error(' Not enough input arguments') 965 | end 966 | if nargin<5, trace = 0; end 967 | if nargin==4 % Check if 4-th arg is trace 968 | if max(size(yl))==1, trace = yl; is_ab = 1; end 969 | end 970 | if nargin==3, is_ab = 1; end 971 | trace = trace | nargin<2; 972 | if length(xv)~=length(yv) 973 | error(' Vectors X, Y must have the same size') 974 | end 975 | 976 | % Auxillary ........... 977 | xv = [xv(:); xv(1)]; 978 | yv = [yv(:); yv(1)]; 979 | ii = find(abs(diff(xv))1 & 0 % Do not execute 1077 | xx = [xo yo]; 1078 | yy = diff(xx)'; 1079 | ii = [1 find(any(abs(yy)>tol))+1]; 1080 | xo = xx(ii,1); yo = xx(ii,2); 1081 | oi = ones(size(xo)); 1082 | end 1083 | 1084 | 1085 | % Plotting ................................................ 1086 | if trace 1087 | oi(3:2:length(oi)) = oi(3:2:length(oi))+1; 1088 | oi = cumsum(oi); 1089 | len = max(oi); 1090 | xp = nan*ones(len,1); yp = xp; 1091 | xp(oi) = xo; 1092 | yp(oi) = yo; 1093 | 1094 | % Intersection with polygon frame 1095 | [xl,yl] = intsecpl(lim([1 2 2 1]),lim([3 3 4 4]),xl,yl); 1096 | 1097 | plot(xv,yv,xl,yl,xp,yp) % Plotting itself 1098 | end 1099 | end 1100 | 1101 | function [is,S] = isintpl(x1,y1,x2,y2) 1102 | 1103 | % ISINTPL Check for intersection of polygons. 1104 | % [IS,S] = ISINTPL(X1,Y1,X2,Y2) Accepts coordinates 1105 | % X1,Y1 and X2, Y2 of two polygons. Returns scalar 1106 | % IS equal to 1 if these polygons intersect each other 1107 | % and 0 if they do not. 1108 | % Also returns Boolean matrix S of the size length(X1) 1109 | % by length(X2) so that {ij} element of which is 1 if 1110 | % line segments i to i+1 of the first polygon and 1111 | % j to j+1 of the second polygon intersect each other, 1112 | % 0 if they don't and .5 if they "touch" each other. 1113 | 1114 | % Copyright (c) 1995 by Kirill K. Pankratov, 1115 | % kirill@plume.mit.edu. 1116 | % 06/20/95, 08/25/95 1117 | 1118 | 1119 | % Handle input ................................... 1120 | if nargin==0, help isintpl, return, end 1121 | if nargin<4 1122 | error(' Not enough input arguments ') 1123 | end 1124 | 1125 | % Make column vectors and check sizes ............ 1126 | x1 = x1(:); y1 = y1(:); 1127 | x2 = x2(:); y2 = y2(:); 1128 | l1 = length(x1); 1129 | l2 = length(x2); 1130 | if length(y1)~=l1 | length(y2)~=l2 1131 | error('(X1,Y1) and (X2,Y2) must pair-wise have the same length.') 1132 | end 1133 | 1134 | % Quick exit if empty 1135 | if l1<1 | l2<1, is = []; S = []; return, end 1136 | 1137 | % Check if their ranges are intersecting ......... 1138 | lim1 = [min(x1) max(x1)]'; 1139 | lim2 = [min(x2) max(x2)]'; 1140 | isx = interval(lim1,lim2); % X-ranges 1141 | lim1 = [min(y1) max(y1)]'; 1142 | lim2 = [min(y2) max(y2)]'; 1143 | isy = interval(lim1,lim2); % Y-ranges 1144 | is = isx & isy; 1145 | S = zeros(l2,l1); 1146 | 1147 | if ~is, return, end % Early exit if disparate limits 1148 | 1149 | % Indexing ....................................... 1150 | [i11,i12] = meshgrid(1:l1,1:l2); 1151 | [i21,i22] = meshgrid([2:l1 1],[2:l2 1]); 1152 | i11 = i11(:); i12 = i12(:); 1153 | i21 = i21(:); i22 = i22(:); 1154 | 1155 | % Calculate matrix of intersections .............. 1156 | S(:) = iscross([x1(i11) x1(i21)]',[y1(i11) y1(i21)]',... 1157 | [x2(i12) x2(i22)]',[y2(i12) y2(i22)]')'; 1158 | 1159 | S = S'; 1160 | is = any(any(S)); 1161 | end 1162 | 1163 | function [xo,yo] = intsecl(x1,y1,x2,y2,tol) 1164 | 1165 | % INTSECL Intersection coordinates of two line segments. 1166 | % [XI,YI] = INTSECL(X1,Y1,X2,Y2) where all 4 1167 | % arguments are 2 by N matrices with coordinates 1168 | % of ends of N pairs of line segments (so that 1169 | % the command such as PLOT(X1,Y1,X2,Y2) will plot 1170 | % these pairs of lines). 1171 | % Returns 1 by N vectors XI and YI consisting of 1172 | % coordinates of intersection points of each of N 1173 | % pairs of lines. 1174 | % 1175 | % Special cases: 1176 | % When a line segment is degenerate into a point 1177 | % and does not lie on line through the other segment 1178 | % of a pair returns XI=NaN while YI has the following 1179 | % values: 1 - when the first segment in a pair is 1180 | % degenerate, 2 - second segment, 0 - both segments 1181 | % are degenerate. 1182 | % When a pair of line segments is parallel, returns 1183 | % XI = Inf while YI is 1 for coincident lines, 1184 | % 0 - for parallel non-coincident ones. 1185 | % INTSECL(X1,Y1,X2,Y2,TOL) also specifies tolerance 1186 | % in detecting coincident points in different line 1187 | % segments. 1188 | 1189 | % Copyright (c) 1995 by Kirill K. Pankratov 1190 | % kirill@plume.mit.edu 1191 | % 04/15/94, 08/14/94, 05/10/95, 08/23/95 1192 | 1193 | 1194 | % Defaults and parameters ......................... 1195 | tol_dflt = 0; % Tolerance for coincident points 1196 | is_chk = 1; % Check input arguments 1197 | 1198 | % Handle input .................................... 1199 | if nargin==0, help intsecl, return, end 1200 | if nargin<4 % Check if all 4 entered 1201 | error(' Not enough input arguments') 1202 | end 1203 | if nargin<5, tol = tol_dflt; end 1204 | if tol < 0, is_chk = 0; tol = 0; end 1205 | 1206 | % Check the format of arguments ....... 1207 | if is_chk 1208 | [x1,y1,x2,y2] = linechk(x1,y1,x2,y2); 1209 | end 1210 | 1211 | 1212 | % Auxillary 1213 | o2 = ones(2,1); 1214 | i_pt1 = []; i_pt2 = []; i_pt12 = []; 1215 | 1216 | % Make first points origins ........... 1217 | xo = x1(1,:); 1218 | yo = y1(1,:); 1219 | x2 = x2-xo(o2,:); 1220 | y2 = y2-yo(o2,:); 1221 | 1222 | % Differences of first segments ....... 1223 | a = x1(2,:)-x1(1,:); 1224 | b = y1(2,:)-y1(1,:); 1225 | s = sqrt(a.^2+b.^2); % Lengths of first segments 1226 | i_pt1 = find(~s); 1227 | s(i_pt1) = ones(size(i_pt1)); 1228 | rr = rand(size(i_pt1)); 1229 | a(i_pt1) = cos(rr); 1230 | b(i_pt1) = sin(rr); 1231 | 1232 | % Normalize by length ................. 1233 | a = a./s; b = b./s; 1234 | 1235 | % Rotate coordinates of the second segment 1236 | tmp = x2.*a(o2,:)+y2.*b(o2,:); 1237 | y2 = -x2.*b(o2,:)+y2.*a(o2,:); 1238 | x2 = tmp; 1239 | 1240 | % Calculate differences in second segments 1241 | s = x2(2,:)-x2(1,:); 1242 | tmp = y2(2,:)-y2(1,:); 1243 | cc = tmp(i_pt1); 1244 | 1245 | % Find some degenerate cases ....................... 1246 | 1247 | % Find zeros in differences 1248 | izy2 = find(~tmp); 1249 | tmp(izy2) = ones(size(izy2)); 1250 | 1251 | % Find degenerate and parallel segments 1252 | bool = ~s(izy2); 1253 | i_par = izy2(~bool); 1254 | i_pt2 = izy2(bool); 1255 | 1256 | bool = abs(y2(1,i_pt2))<=tol; 1257 | i_pt2_off = i_pt2(~bool); 1258 | i_pt2_on = i_pt2(bool); 1259 | 1260 | if ~isempty(i_par) 1261 | bool = abs(y2(1,i_par))<=tol; 1262 | i_par_off = i_par(~bool); 1263 | i_par_on = i_par(bool); 1264 | end 1265 | 1266 | % Calculate intercept with rotated x-axis .......... 1267 | tmp = s./tmp; % Slope 1268 | tmp = x2(1,:)-y2(1,:).*tmp; 1269 | 1270 | 1271 | % Rotate and translate back to original coordinates 1272 | xo = tmp.*a+xo; 1273 | yo = tmp.*b+yo; 1274 | 1275 | % Mark special cases ................................... 1276 | % First segments are degenerate to points 1277 | if ~isempty(i_pt1) 1278 | bool = ~s(i_pt1) & ~cc; 1279 | i_pt12 = i_pt1(bool); 1280 | i_pt1 = i_pt1(~bool); 1281 | 1282 | bool = abs(tmp(i_pt1))<=tol; 1283 | i_pt1_on = i_pt1(bool); 1284 | i_pt1_off = i_pt1(~bool); 1285 | 1286 | xo(i_pt1_on) = x1(1,i_pt1_on); 1287 | yo(i_pt1_on) = y1(1,i_pt1_on); 1288 | 1289 | oo = ones(size(i_pt1_off)); 1290 | xo(i_pt1_off) = nan*oo; 1291 | yo(i_pt1_off) = oo; 1292 | end 1293 | 1294 | % Second segments are degenerate to points ... 1295 | if ~isempty(i_pt2) 1296 | oo = ones(size(i_pt2_off)); 1297 | xo(i_pt2_off) = nan*oo; 1298 | yo(i_pt2_off) = 2*oo; 1299 | end 1300 | 1301 | % Both segments are degenerate ............... 1302 | if ~isempty(i_pt12) 1303 | bool = x1(i_pt12)==xo(i_pt12); 1304 | i_pt12_on = i_pt12(bool); 1305 | i_pt12_off = i_pt12(~bool); 1306 | 1307 | xo(i_pt12_on) = x1(1,i_pt12_on); 1308 | yo(i_pt12_on) = y1(1,i_pt12_on); 1309 | 1310 | oo = ones(size(i_pt12_off)); 1311 | xo(i_pt12_off) = nan*oo; 1312 | yo(i_pt12_off) = 0*oo; 1313 | end 1314 | 1315 | % Parallel segments ......................... 1316 | if ~isempty(i_par) 1317 | oo = ones(size(i_par_on)); 1318 | xo(i_par_on) = inf*oo; 1319 | yo(i_par_on) = oo; 1320 | 1321 | oo = ones(size(i_par_off)); 1322 | xo(i_par_off) = inf*oo; 1323 | yo(i_par_off) = 0*oo; 1324 | end 1325 | 1326 | 1327 | 1328 | end 1329 | 1330 | function [x1,y1,x2,y2] = linechk(x1,y1,x2,y2) 1331 | 1332 | % LINECHK Input checking for line segments. 1333 | 1334 | % Copyright (c) 1995 by Kirill K. Pankratov 1335 | % kirill@plume.mit.edu 1336 | % 08/22/95, 1337 | 1338 | % String for transposing 1339 | str = ['x1=x1'';'; 'y1=y1'';'; 'x2=x2'';'; 'y2=y2'';']; 1340 | 1341 | % Sizes 1342 | sz = [size(x1); size(y1); size(x2); size(y2)]'; 1343 | psz = prod(sz); 1344 | 1345 | % Check x1, y1 1346 | if psz(1)~=psz(2) 1347 | error(' Arguments x1 and y1 must have the same size') 1348 | end 1349 | 1350 | % Check x2, y2 1351 | if psz(3)~=psz(3) 1352 | error(' Arguments x2 and y2 must have the same size') 1353 | end 1354 | 1355 | % Check if any arguments are less than 2 by 1 1356 | if any(max(sz)<2) 1357 | error(' Arguments x1, y1, x2, y2 must be at least 2 by 1 vectors') 1358 | end 1359 | 1360 | % Check if no size is equal to 2 1361 | if any(all(sz~=2)) 1362 | error(' Arguments x1, y1, x2, y2 must be 2 by 1 vectors') 1363 | end 1364 | 1365 | % Find aruments to be transposed ............................. 1366 | ii = find(sz(1,:)~=2); 1367 | for jj = 1:length(ii) 1368 | eval(str(ii(jj),:)); % Transpose if neccessary 1369 | end 1370 | sz(:,ii) = flipud(sz(:,ii)); 1371 | 1372 | % If vectors, extend to 2 by n matrices ...................... 1373 | n = max(sz(2,:)); 1374 | on = ones(1,n); 1375 | if sz(2,1)abs(dx); 63 | 64 | if steep 65 | % if slope > 1 swap the deltas 66 | t=dx; 67 | dx=dy; 68 | dy=t; 69 | end 70 | 71 | %The main algorithm goes here. 72 | if dy==0 73 | % 74 | q=zeros(dx+1,1); 75 | else 76 | q=[0;diff(mod([floor(dx/2):-dy:-dy*dx+floor(dx/2)]',dx))>=0]; 77 | end 78 | 79 | %and ends here. 80 | 81 | if steep 82 | if y1<=y2 83 | y=[y1:y2]'; 84 | else 85 | y=[y1:-1:y2]'; 86 | end 87 | if x1<=x2 88 | x=x1+cumsum(q); 89 | else 90 | x=x1-cumsum(q); 91 | end 92 | else 93 | if x1<=x2 94 | x=[x1:x2]'; 95 | else 96 | x=[x1:-1:x2]'; 97 | end 98 | if y1<=y2 99 | y=y1+cumsum(q); 100 | else 101 | y=y1-cumsum(q); 102 | end 103 | end 104 | 105 | p = [x y]; 106 | 107 | end 108 | -------------------------------------------------------------------------------- /circle.m: -------------------------------------------------------------------------------- 1 | %CIRCLE Compute points on a circle 2 | % 3 | % CIRCLE(C, R, OPTIONS) plots a circle centred at C (1x2) with radius R on the current 4 | % axes. 5 | % 6 | % X = CIRCLE(C, R, OPTIONS) is a matrix (2xN) whose columns define the 7 | % coordinates [x,y] of points around the circumferance of a circle 8 | % centred at C (1x2) and of radius R. 9 | % 10 | % C is normally 2x1 but if 3x1 then the circle is embedded in 3D, and X is Nx3, 11 | % but the circle is always in the xy-plane with a z-coordinate of C(3). 12 | % 13 | % Options:: 14 | % 'n',N Specify the number of points (default 50) 15 | 16 | 17 | % Copyright (C) 1993-2017, by Peter I. Corke 18 | % 19 | % This file is part of The Robotics Toolbox for MATLAB (RTB). 20 | % 21 | % RTB is free software: you can redistribute it and/or modify 22 | % it under the terms of the GNU Lesser General Public License as published by 23 | % the Free Software Foundation, either version 3 of the License, or 24 | % (at your option) any later version. 25 | % 26 | % RTB is distributed in the hope that it will be useful, 27 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 28 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 29 | % GNU Lesser General Public License for more details. 30 | % 31 | % You should have received a copy of the GNU Leser General Public License 32 | % along with RTB. If not, see . 33 | % 34 | % http://www.petercorke.com 35 | function out = circle(centre, rad, varargin) 36 | 37 | opt.n = 50; 38 | 39 | [opt,arglist] = tb_optparse(opt, varargin); 40 | 41 | % compute points on circumference 42 | th = [0:opt.n-1]'/ opt.n*2*pi; 43 | x = rad*cos(th) + centre(1); 44 | y = rad*sin(th) + centre(2); 45 | 46 | % add extra row if z-coordinate is specified, but circle is always in xy plane 47 | if length(centre) > 2 48 | z = ones(size(x))*centre(3); 49 | p = [x y z]'; 50 | else 51 | p = [x y]'; 52 | end 53 | 54 | if nargout > 0 55 | % return now 56 | out = p; 57 | return; 58 | else 59 | % else plot the circle 60 | p = [p p(:,1)]; % make it close 61 | if numrows(p) > 2 62 | plot3(p(1,:), p(2,:), p(3,:), arglist{:}); 63 | else 64 | plot(p(1,:), p(2,:), arglist{:}); 65 | end 66 | end 67 | -------------------------------------------------------------------------------- /colorname.m: -------------------------------------------------------------------------------- 1 | %COLORNAME Map between color names and RGB values 2 | % 3 | % RGB = COLORNAME(NAME) is the RGB-tristimulus value (1x3) corresponding to 4 | % the color specified by the string NAME. If RGB is a cell-array (1xN) of 5 | % names then RGB is a matrix (Nx3) with each row being the corresponding 6 | % tristimulus. 7 | % 8 | % XYZ = COLORNAME(NAME, 'xyz') as above but the XYZ-tristimulus value 9 | % corresponding to the color specified by the string NAME. 10 | % 11 | % XY = COLORNAME(NAME, 'xy') as above but the xy-chromaticity coordinates 12 | % corresponding to the color specified by the string NAME. 13 | % 14 | % NAME = COLORNAME(RGB) is a string giving the name of the color that is 15 | % closest (Euclidean) to the given RGB-tristimulus value (1x3). If RGB is 16 | % a matrix (Nx3) then return a cell-array (1xN) of color names. 17 | % 18 | % NAME = COLORNAME(XYZ, 'xyz') as above but the color is the closest (Euclidean) 19 | % to the given XYZ-tristimulus value. 20 | % 21 | % NAME = COLORNAME(XYZ, 'xy') as above but the color is the closest (Euclidean) 22 | % to the given xy-chromaticity value with assumed Y=1. 23 | % 24 | % Notes:: 25 | % - Color name may contain a wildcard, eg. "?burnt" 26 | % - Based on the standard X11 color database rgb.txt. 27 | % - Tristimulus values are in the range 0 to 1 28 | 29 | 30 | 31 | 32 | % Copyright (C) 1993-2017, by Peter I. Corke 33 | % 34 | % This file is part of The Robotics Toolbox for MATLAB (RTB). 35 | % 36 | % RTB is free software: you can redistribute it and/or modify 37 | % it under the terms of the GNU Lesser General Public License as published by 38 | % the Free Software Foundation, either version 3 of the License, or 39 | % (at your option) any later version. 40 | % 41 | % RTB is distributed in the hope that it will be useful, 42 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 43 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 44 | % GNU Lesser General Public License for more details. 45 | % 46 | % You should have received a copy of the GNU Leser General Public License 47 | % along with RTB. If not, see . 48 | % 49 | % http://www.petercorke.com 50 | 51 | function out = colorname(a, varargin) 52 | 53 | opt.space = {'rgb', 'xyz', 'xy', 'lab', 'ab'}; 54 | opt = tb_optparse(opt, varargin); 55 | 56 | persistent rgbtable; 57 | 58 | % ensure that the database is loaded 59 | if isempty(rgbtable) 60 | % load mapping table from file 61 | fprintf('loading rgb.txt\n'); 62 | f = fopen('data/rgb.txt', 'r'); 63 | k = 0; 64 | rgb = []; 65 | names = {}; 66 | xy = []; 67 | 68 | while ~feof(f) 69 | line = fgets(f); 70 | if line(1) == '#', 71 | continue; 72 | end 73 | 74 | [A,count,errm,next] = sscanf(line, '%d %d %d'); 75 | if count == 3 76 | k = k + 1; 77 | rgb(k,:) = A' / 255.0; 78 | names{k} = lower( strtrim(line(next:end)) ); 79 | end 80 | end 81 | s.rgb = rgb; 82 | s.names = names; 83 | rgbtable = s; 84 | end 85 | 86 | if isstr(a) 87 | % map name to rgb/xy 88 | if a(1) == '?' 89 | % just do a wildcard lookup 90 | out = namelookup(rgbtable, a(2:end), opt); 91 | else 92 | out = name2color(rgbtable, a, opt); 93 | end 94 | elseif iscell(a) 95 | % map multiple names to colorspace 96 | out = []; 97 | for name=a 98 | color = name2color(rgbtable, name{1}, opt); 99 | if isempty(color) 100 | warning('Color %s not found', name{1}); 101 | end 102 | out = [out; color]; 103 | end 104 | else 105 | % map values to strings 106 | out = {}; 107 | 108 | switch opt.space 109 | case {'rgb', 'xyz', 'lab'} 110 | assert(numcols(a) == 3, 'Color value must have 3 elements'); 111 | % convert reference colors to input color space 112 | table = colorspace(['RGB->' opt.space], rgbtable.rgb); 113 | for color=a' 114 | d = distance(color, table'); 115 | [~,k] = min(d); 116 | out = [out rgbtable.names{k}]; 117 | end 118 | 119 | case {'xy', 'ab'} 120 | assert(numcols(a) == 2, 'Color value must have 2 elements'); 121 | % convert reference colors to input color space 122 | 123 | switch opt.space 124 | case 'xy' 125 | table = colorspace('RGB->XYZ', rgbtable.rgb); 126 | table = table(:,1:2) ./ (sum(table,2)*[1 1]); 127 | case 'ab' 128 | table = colorspace('RGB->Lab', rgbtable.rgb); 129 | table = table(:,2:3); 130 | end 131 | 132 | for color=a' 133 | d = distance(color, table'); 134 | [~,k] = min(d); 135 | out = [out rgbtable.names{k}]; 136 | end 137 | 138 | end 139 | if length(out) == 1 140 | out = out{1}; 141 | end 142 | end 143 | end 144 | 145 | function r = namelookup(table, s, opt) 146 | s = lower(s); % all matching done in lower case 147 | 148 | r = {}; 149 | count = 1; 150 | for k=1:length(table.names), 151 | if ~isempty( findstr(table.names{k}, s) ) 152 | r{count} = table.names{k}; 153 | count = count + 1; 154 | end 155 | end 156 | end 157 | 158 | function out = name2color(table, s, opt) 159 | 160 | s = lower(s); % all matching done in lower case 161 | 162 | for k=1:length(table.names), 163 | if strcmp(s, table.names(k)), 164 | rgb = table.rgb(k,:); 165 | switch opt.space 166 | case {'rgb', 'xyz', 'lab'} 167 | out = colorspace(['RGB->' opt.space], rgb); 168 | case 'xy' 169 | XYZ = colorspace('RGB->XYZ', rgb); 170 | out = tristim2cc(XYZ); 171 | case 'ab'; 172 | Lab = colorspace('RGB->Lab', rgb); 173 | out = Lab(2:3); 174 | end 175 | return; 176 | end 177 | end 178 | out = []; 179 | end -------------------------------------------------------------------------------- /diff2.m: -------------------------------------------------------------------------------- 1 | %DIFF2 First-order difference 2 | % 3 | % D = DIFF2(V) is the first-order difference (1xN) of the series data in 4 | % vector V (1xN) and the first element is zero. 5 | % 6 | % D = DIFF2(A) is the first-order difference (MxN) of the series data in 7 | % each row of the matrix A (MxN) and the first element in each row is zero. 8 | % 9 | % Notes:: 10 | % - Unlike the builtin function DIFF, the result of DIFF2 has the same 11 | % number of columns as the input. 12 | % 13 | % See also DIFF. 14 | 15 | 16 | % Copyright (C) 1993-2017, by Peter I. Corke 17 | % 18 | % This file is part of The Robotics Toolbox for MATLAB (RTB). 19 | % 20 | % RTB is free software: you can redistribute it and/or modify 21 | % it under the terms of the GNU Lesser General Public License as published by 22 | % the Free Software Foundation, either version 3 of the License, or 23 | % (at your option) any later version. 24 | % 25 | % RTB is distributed in the hope that it will be useful, 26 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 27 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 28 | % GNU Lesser General Public License for more details. 29 | % 30 | % You should have received a copy of the GNU Leser General Public License 31 | % along with RTB. If not, see . 32 | % 33 | % http://www.petercorke.com 34 | function d = diff2(v) 35 | [r,c] =size(v); 36 | 37 | d = [zeros(1,c); diff(v)]; 38 | -------------------------------------------------------------------------------- /dockfigs.m: -------------------------------------------------------------------------------- 1 | %DOCKFIGS Control figure docking in the GUI 2 | % 3 | % dockfigs causes all new figures to be docked into the GUI 4 | % 5 | % dockfigs(1) as above. 6 | % 7 | % dockfigs(0) causes all new figures to be undocked from the GUI 8 | 9 | 10 | 11 | % Copyright (C) 1993-2017, by Peter I. Corke 12 | % 13 | % This file is part of The Robotics Toolbox for MATLAB (RTB). 14 | % 15 | % RTB is free software: you can redistribute it and/or modify 16 | % it under the terms of the GNU Lesser General Public License as published by 17 | % the Free Software Foundation, either version 3 of the License, or 18 | % (at your option) any later version. 19 | % 20 | % RTB is distributed in the hope that it will be useful, 21 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 22 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 23 | % GNU Lesser General Public License for more details. 24 | % 25 | % You should have received a copy of the GNU Leser General Public License 26 | % along with RTB. If not, see . 27 | % 28 | % http://www.petercorke.com 29 | 30 | function dockfigs(arg) 31 | if nargin == 0 32 | arg = 1; 33 | end 34 | 35 | if arg 36 | set(0, 'DefaultFigureWindowStyle', 'docked') 37 | else 38 | set(0, 'DefaultFigureWindowStyle', 'normal') 39 | end 40 | -------------------------------------------------------------------------------- /edgelist.m: -------------------------------------------------------------------------------- 1 | %EDGELIST Return list of edge pixels for region 2 | % 3 | % EG = EDGELIST(IM, SEED) is a list of edge pixels (2xN) of a region in the 4 | % image IM starting at edge coordinate SEED=[X,Y]. The edgelist has one column per 5 | % edge point coordinate (x,y). 6 | % 7 | % EG = EDGELIST(IM, SEED, DIRECTION) as above, but the direction of edge 8 | % following is specified. DIRECTION == 0 (default) means clockwise, non 9 | % zero is counter-clockwise. Note that direction is with respect to y-axis 10 | % upward, in matrix coordinate frame, not image frame. 11 | % 12 | % [EG,D] = EDGELIST(IM, SEED, DIRECTION) as above but also returns a vector 13 | % of edge segment directions which have values 1 to 8 representing W SW S SE E 14 | % NW N NW respectively. 15 | % 16 | % Notes:: 17 | % - Coordinates are given assuming the matrix is an image, so the indices are 18 | % always in the form (x,y) or (column,row). 19 | % - IM is a binary image where 0 is assumed to be background, non-zero 20 | % is an object. 21 | % - SEED must be a point on the edge of the region. 22 | % - The seed point is always the first element of the returned edgelist. 23 | % - 8-direction chain coding can give incorrect results when used with 24 | % blobs founds using 4-way connectivty. 25 | % 26 | % Reference:: 27 | % - METHODS TO ESTIMATE AREAS AND PERIMETERS OF BLOB-LIKE OBJECTS: A COMPARISON 28 | % Luren Yang, Fritz Albregtsen, Tor Lgnnestad and Per Grgttum 29 | % IAPR Workshop on Machine Vision Applications Dec. 13-15, 1994, Kawasaki 30 | % 31 | % See also ILABEL. 32 | 33 | 34 | % Copyright (C) 1993-2017, by Peter I. Corke 35 | % 36 | % This file is part of The Robotics Toolbox for MATLAB (RTB). 37 | % 38 | % RTB is free software: you can redistribute it and/or modify 39 | % it under the terms of the GNU Lesser General Public License as published by 40 | % the Free Software Foundation, either version 3 of the License, or 41 | % (at your option) any later version. 42 | % 43 | % RTB is distributed in the hope that it will be useful, 44 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 45 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 46 | % GNU Lesser General Public License for more details. 47 | % 48 | % You should have received a copy of the GNU Leser General Public License 49 | % along with RTB. If not, see . 50 | % 51 | % http://www.petercorke.com 52 | 53 | function [e,d] = edgelist(im, P, direction) 54 | 55 | % deal with direction argument 56 | if nargin == 2 57 | direction = 0; 58 | end 59 | 60 | if direction == 0 61 | neighbours = [1:8]; % neigbours in clockwise direction 62 | else 63 | neighbours = [8:-1:1]; % neigbours in counter-clockwise direction 64 | end 65 | 66 | P = P(:); 67 | try 68 | pix0 = im(P(2), P(1)); % color of pixel we start at 69 | catch 70 | error('TBCOMMON:edgelist', 'specified coordinate is not within image'); 71 | end 72 | P0 = []; 73 | 74 | % find an adjacent point outside the blob 75 | Q = adjacent_point(im, P, pix0); 76 | 77 | assert(~isempty(Q), 'TBCOMMON:edgelist', 'no neighbour outside the blob'); 78 | 79 | e = P; % initialize the edge list 80 | dir = []; % initialize the direction list 81 | 82 | % these are directions of 8-neighbours in a clockwise direction 83 | dirs = [-1 0; -1 1; 0 1; 1 1; 1 0; 1 -1; 0 -1; -1 -1]'; 84 | 85 | while true 86 | % find which direction is Q 87 | dQ = Q - P; 88 | for kq=1:8 89 | if all(dQ == dirs(:,kq)) 90 | break; 91 | end 92 | end 93 | 94 | 95 | % now test for directions relative to Q 96 | for j=neighbours 97 | % get index of neighbour's direction in range [1,8] 98 | k = j + kq; 99 | if k > 8 100 | k = k - 8; 101 | end 102 | dir = [dir; k]; 103 | 104 | % compute coordinate of the k'th neighbour 105 | Nk = P + dirs(:,k); 106 | try 107 | if im(Nk(2), Nk(1)) == pix0 108 | % if this neighbour is in the blob it is the next edge pixel 109 | P = Nk; 110 | break; 111 | end 112 | end 113 | Q = Nk; % the (k-1)th neighbour 114 | end 115 | 116 | % check if we are back where we started 117 | if isempty(P0) 118 | P0 = P; % make a note of where we started 119 | else 120 | if all(P == P0) 121 | break; 122 | end 123 | end 124 | 125 | % keep going, add P to the edgelist 126 | e = [e P]; 127 | end 128 | 129 | if nargout > 1 130 | d = dir; 131 | end 132 | end 133 | 134 | function P = adjacent_point(im, seed, pix0) 135 | % find an adjacent point not in the region 136 | dirs = [1 0; 0 1; -1 0; 0 -1; -1 1; -1 -1; 1 -1; 1 1]; 137 | for d=dirs' 138 | P = seed(:) + d; 139 | try 140 | if im(P(2), P(1)) ~= pix0 141 | return; 142 | end 143 | catch 144 | % if we get an exception then by definition P is outside the region, 145 | % since it's off the edge of the image 146 | return; 147 | end 148 | end 149 | P = []; 150 | end 151 | -------------------------------------------------------------------------------- /filt1d.m: -------------------------------------------------------------------------------- 1 | %FILT1D 1-dimensional rank filter 2 | % 3 | % Y = FILT1D(X, OPTIONS) is the minimum, maximum or median value (1xN) of the 4 | % vector X (1xN) compute over an odd length sliding window. 5 | % 6 | % Options:: 7 | % 'max' Compute maximum value over the window (default) 8 | % 'min' Compute minimum value over the window 9 | % 'median' Compute minimum value over the window 10 | % 'width',W Width of the window (default 5) 11 | % 12 | % Notes:: 13 | % - If the window width is even, it is incremented by one. 14 | % - The first and last elements of X are replicated so the output vector is the 15 | % same length as the input vector. 16 | 17 | 18 | 19 | % Copyright (C) 1993-2017, by Peter I. Corke 20 | % 21 | % This file is part of The Robotics Toolbox for MATLAB (RTB). 22 | % 23 | % RTB is free software: you can redistribute it and/or modify 24 | % it under the terms of the GNU Lesser General Public License as published by 25 | % the Free Software Foundation, either version 3 of the License, or 26 | % (at your option) any later version. 27 | % 28 | % RTB is distributed in the hope that it will be useful, 29 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 30 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 31 | % GNU Lesser General Public License for more details. 32 | % 33 | % You should have received a copy of the GNU Leser General Public License 34 | % along with RTB. If not, see . 35 | % 36 | % http://www.petercorke.com 37 | 38 | % Copyright (c) Peter Corke 6/93 39 | % vectorized version 8/95 pic 40 | 41 | function m = filt1d(s, varargin) 42 | 43 | opt.width = 5; 44 | opt.op = {'max', 'min', 'median'}; 45 | 46 | opt = tb_optparse(opt, varargin); 47 | 48 | % enforce a column vector 49 | s = s(:)'; 50 | 51 | % enforce odd window length 52 | w2 = floor(opt.width/2); 53 | w = 2*w2 + 1; 54 | 55 | n = length(s); 56 | m = zeros(w,n+w-1); 57 | s0 = s(1); sl = s(n); 58 | 59 | % replicate first and last elements 60 | for i=0:(w-1), 61 | m(i+1,:) = [s0*ones(1,i) s sl*ones(1,w-i-1)]; 62 | end 63 | 64 | switch (opt.op) 65 | case 'max' 66 | m = max(m); 67 | case 'min' 68 | m = min(m); 69 | case 'median' 70 | m = median(m); 71 | end 72 | 73 | m = m(w2+1:w2+n); 74 | -------------------------------------------------------------------------------- /gaussfunc.m: -------------------------------------------------------------------------------- 1 | %GAUSSFUNC Gaussian kernel 2 | % 3 | % G = GAUSSFUNC(MEAN, VARIANCE, X) is the value of the normal 4 | % distribution (Gaussian) function with MEAN (1x1) and VARIANCE (1x1), at 5 | % the point X. 6 | % 7 | % G = GAUSSFUNC(MEAN, COVARIANCE, X, Y) is the value of the bivariate 8 | % normal distribution (Gaussian) function with MEAN (1x2) and COVARIANCE 9 | % (2x2), at the point (X,Y). 10 | % 11 | % G = GAUSSFUNC(MEAN, COVARIANCE, X) as above but X (NxM) and the result 12 | % is also (NxM). X and Y values come from the column and row indices of 13 | % X. 14 | % 15 | % Notes:: 16 | % - X or Y can be row or column vectors, and the result will also be a vector. 17 | % - The area or volume under the curve is unity. 18 | 19 | 20 | % Copyright (C) 1993-2017, by Peter I. Corke 21 | % 22 | % This file is part of The Robotics Toolbox for MATLAB (RTB). 23 | % 24 | % RTB is free software: you can redistribute it and/or modify 25 | % it under the terms of the GNU Lesser General Public License as published by 26 | % the Free Software Foundation, either version 3 of the License, or 27 | % (at your option) any later version. 28 | % 29 | % RTB is distributed in the hope that it will be useful, 30 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 31 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 32 | % GNU Lesser General Public License for more details. 33 | % 34 | % You should have received a copy of the GNU Leser General Public License 35 | % along with RTB. If not, see . 36 | % 37 | % http://www.petercorke.com 38 | function g = gaussfunc(mu, variance, x, y) 39 | 40 | if length(mu) == 1 41 | % 1D case 42 | assert(all(size(variance) == [1 1]), 'covariance must be a 1x1 matrix') 43 | 44 | g = 1/sqrt(2*pi*variance) * exp( -((x-mu).^2)/(2*variance) ); 45 | elseif length(mu) == 2 46 | % 2D case 47 | assert(length(mu) == 2, 'mean must be a 2-vector'); 48 | assert(all(size(variance) == [2 2]), 'covariance must be a 2x2 matrix') 49 | 50 | if nargin < 4 51 | [x,y] = imeshgrid(x); 52 | end 53 | xx = x(:)-mu(1); yy = y(:)-mu(2); 54 | ci = inv(variance); 55 | g = 1/(2*pi*sqrt(det(variance))) * exp( -0.5*(xx.^2*ci(1,1) + yy.^2*ci(2,2) + 2*xx.*yy*ci(1,2))); 56 | g = reshape(g, size(x)); 57 | end 58 | -------------------------------------------------------------------------------- /mmlabel.m: -------------------------------------------------------------------------------- 1 | %MMLABEL labels for mplot style graph 2 | % 3 | % mmlabel({lab1 lab2 lab3}) 4 | % 5 | % Notes:: 6 | % - was previously (rev 9) named mlabel() but changed to avoid clash with the 7 | % Mapping Toolbox. 8 | 9 | 10 | % Copyright (C) 1993-2017, by Peter I. Corke 11 | % 12 | % This file is part of The Robotics Toolbox for MATLAB (RTB). 13 | % 14 | % RTB is free software: you can redistribute it and/or modify 15 | % it under the terms of the GNU Lesser General Public License as published by 16 | % the Free Software Foundation, either version 3 of the License, or 17 | % (at your option) any later version. 18 | % 19 | % RTB is distributed in the hope that it will be useful, 20 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 21 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 22 | % GNU Lesser General Public License for more details. 23 | % 24 | % You should have received a copy of the GNU Leser General Public License 25 | % along with RTB. If not, see . 26 | % 27 | % http://www.petercorke.com 28 | 29 | function mmlabel(lab, varargin) 30 | 31 | % find all child axes (subplots) 32 | h = findobj(gcf, 'Type', 'axes'); 33 | 34 | for i=1:length(h) 35 | 36 | if strcmp( get(h(i), 'visible'), 'on'), 37 | axes(h(i)) 38 | % get subplot number from user data (I don't know who 39 | % sets this but its very useful) 40 | sp = get(h(i), 'UserData'); 41 | if sp == 1, 42 | topplot = sp; 43 | end 44 | ylabel(lab{sp}, varargin{:}); 45 | end 46 | end 47 | 48 | if 0 49 | if nargin > 1, 50 | axes(h(topplot)); % top plot 51 | title(tit); 52 | end 53 | end 54 | -------------------------------------------------------------------------------- /mplot.m: -------------------------------------------------------------------------------- 1 | %MPLOT Plot time-series data 2 | % 3 | % A convenience function for plotting time-series data held in a matrix. 4 | % Each row is a timestep and the first column is time. 5 | % 6 | % MPLOT(Y, OPTIONS) plots the time series data Y(NxM) in multiple 7 | % subplots. The first column is assumed to be time, so M-1 plots are 8 | % produced. 9 | % 10 | % MPLOT(T, Y, OPTIONS) plots the time series data Y(NxM) in multiple 11 | % subplots. Time is provided explicitly as the first argument so M plots 12 | % are produced. 13 | % 14 | % MPLOT(S, OPTIONS) as above but S is a structure. Each field is assumed 15 | % to be a time series which is plotted. Time is taken from the field 16 | % called 't'. Plots are labelled according to the name of the 17 | % corresponding field. 18 | % 19 | % MPLOT(W, OPTIONS) as above but W is a structure created by the Simulink 20 | % write to workspace block where the save format is set to "Structure 21 | % with time". Each field in the signals substructure is plotted. 22 | % 23 | % MPLOT(R, OPTIONS) as above but R is a Simulink.SimulationOutput object 24 | % returned by the Simulink sim() function. 25 | % 26 | % Options:: 27 | % 'col',C Select columns to plot, a boolean of length M-1 or a list of 28 | % column indices in the range 1 to M-1 29 | % 'label',L Label the axes according to the cell array of strings L 30 | % 'date' Add a datestamp in the top right corner 31 | % 32 | % Notes:: 33 | % - In all cases a simple GUI is created which is invoked by a right 34 | % clicking on one of the plotted lines. The supported options are: 35 | % - zoom in the x-direction 36 | % - shift view to the left or right 37 | % - unzoom 38 | % - show data points 39 | % 40 | % See also plot2, plotp. 41 | 42 | 43 | % Copyright (C) 1993-2017, by Peter I. Corke 44 | % 45 | % This file is part of The Robotics Toolbox for MATLAB (RTB). 46 | % 47 | % RTB is free software: you can redistribute it and/or modify 48 | % it under the terms of the GNU Lesser General Public License as published by 49 | % the Free Software Foundation, either version 3 of the License, or 50 | % (at your option) any later version. 51 | % 52 | % RTB is distributed in the hope that it will be useful, 53 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 54 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 55 | % GNU Lesser General Public License for more details. 56 | % 57 | % You should have received a copy of the GNU Leser General Public License 58 | % along with RTB. If not, see . 59 | % 60 | % http://www.petercorke.com 61 | 62 | function mplot(varargin) 63 | 64 | opt.label = []; 65 | opt.date = false; 66 | opt.cols = []; 67 | 68 | [opt,args] = tb_optparse(opt, varargin); 69 | 70 | if isstruct(args{1}) 71 | s = args{1}; 72 | if isfield(s, 'signals'), 73 | % To Workspace type structure 74 | matplot(s.time, s.signals.values, opt); 75 | if isfield(s, 'blockName'), 76 | title(s.blockName) 77 | end 78 | else 79 | % retriever type structure 80 | structplot(args{:}) 81 | end 82 | 83 | elseif isa(args{1}, 'Simulink.SimulationOutput') 84 | % Simulink output object 85 | s = args{1}; 86 | matplot(s.find('tout'), s.find('yout'), opt); 87 | if isfield(s, 'blockName'), 88 | title(s.blockName) 89 | end 90 | else 91 | matplot(args{:}, opt) 92 | end 93 | 94 | if opt.date 95 | datestamp 96 | end 97 | 98 | if ~isempty(opt.label) 99 | mlabel(opt.label); 100 | end 101 | 102 | mtools 103 | end 104 | 105 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 106 | function structplot(s) 107 | if ~isfield(s, 't') & ~isfield(s, 'time') 108 | error('structure must include a time element t') 109 | end 110 | if isfield(s, 't') 111 | t = s.t; 112 | elseif isfield(s, 'time'), 113 | t = s.time; 114 | end 115 | f = fieldnames(s); 116 | n = length(f) - 1; 117 | sp = n*100 + 11; 118 | tmax = max(t); 119 | i = 1; 120 | for ff = f' 121 | fieldnam = char(ff); 122 | switch fieldnam, 123 | case {'t', 'time'}, 124 | otherwise, 125 | h(i) = subplot(sp); 126 | plot(t, getfield(s, fieldnam)); 127 | set(h(i), 'UserData', i); 128 | v = axis; 129 | v(2) = tmax; 130 | axis(v); 131 | grid 132 | xlabel('Time'); 133 | ylabel(fieldnam); 134 | sp = sp +1; 135 | i = i + 1; 136 | end 137 | end 138 | axes(h(1)); 139 | figure(gcf) 140 | 141 | end 142 | 143 | 144 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 145 | % legacy function for matrix input data, old mplot() compatible 146 | function matplot(a1, a2, a3) 147 | [r,c]=size(a1); 148 | if nargin == 2 149 | % [t y1 y2 ... yN] 150 | t = a1(:,1); 151 | y = a1(:,2:c); 152 | cols = 1:(c-1); 153 | gain = 1; 154 | opt = a2; 155 | elseif nargin == 3 156 | if isvector(a1) & ismatrix(a2) 157 | % t, [y1 y2 .. yN] 158 | t = a1(:); 159 | cols = 1:numcols(a2); 160 | y = a2; 161 | elseif isempty(a1) & ismatrix(a2) 162 | % [], [y1 y2 .. yN] 163 | cols = 1:numcols(a2); 164 | y = a2; 165 | t = [1:numrows(y)]'; 166 | end 167 | opt = a3; 168 | 169 | end 170 | t = t(:); 171 | [r,c]=size(y); 172 | sp = c*100 + 10; 173 | tmax = max(t); 174 | for i=1:c 175 | if (sp+i) == 111, 176 | clf 177 | plot(t,y(:,i)); 178 | h(i) = gca; 179 | else 180 | h(i) = subplot(sp+i); 181 | plot(t,y(:,i)); 182 | end 183 | set(h(i), 'UserData', i); 184 | set(h(i), 'Tag', 'mplot'); 185 | v = axis; 186 | v(2) = tmax; 187 | axis(v); 188 | grid 189 | xlabel('Time'); 190 | lab = sprintf('Y(%2d)', cols(i)); 191 | 192 | ylabel(lab); 193 | end 194 | axes(h(1)); 195 | figure(gcf) 196 | end 197 | 198 | function mlabel(lab, varargin) 199 | 200 | % find all child axes (subplots) 201 | h = findobj(gcf, 'Type', 'axes'); 202 | 203 | for i=1:length(h), 204 | 205 | if strcmp( get(h(i), 'visible'), 'on'), 206 | axes(h(i)) 207 | % get subplot number from user data (I don't know who 208 | % sets this but its very useful) 209 | sp = get(h(i), 'UserData'); 210 | if sp == 1, 211 | topplot = sp; 212 | end 213 | ylabel(lab{sp}, varargin{:}); 214 | end 215 | end 216 | 217 | if 0 218 | if nargin > 1, 219 | axes(h(topplot)); % top plot 220 | title(tit); 221 | end 222 | end 223 | end 224 | 225 | function mtools 226 | 227 | h = uicontextmenu; 228 | uimenu(h, 'Label', 'X zoom', 'CallBack', 'xaxis'); 229 | uimenu(h, 'Label', '-->', 'CallBack', 'xscroll(0.5)'); 230 | uimenu(h, 'Label', '<--', 'CallBack', 'xscroll(-0.5)'); 231 | uimenu(h, 'Label', 'CrossHairs', 'CallBack', 'crosshair'); 232 | uimenu(h, 'Label', 'X UNzoom', 'CallBack', 'unzoom'); 233 | uimenu(h, 'Label', 'Pick delta', 'CallBack', 'fprintf(''%f %f\n'', diff(ginput(2)))'); 234 | uimenu(h, 'Label', 'Line fit', 'CallBack', 'ilinefit'); 235 | uimenu(h, 'Label', 'Show points', 'CallBack', 'showpoints(gca)'); 236 | uimenu(h, 'Label', 'Apply X zoom to all', 'CallBack', 'xaxisall'); 237 | for c=get(gcf, 'Children')', 238 | set(c, 'UIContextMenu', h); 239 | l = get(c, 'Children'); 240 | end 241 | 242 | 243 | axes('pos', [0 0 1 0.05], 'visible', 'off') 244 | 245 | end 246 | 247 | function datestamp 248 | uicontrol('Style', 'text', ... 249 | 'String', date, ... 250 | 'Units', 'Normalized', ... 251 | 'HorizontalAlignment', 'Right', ... 252 | 'BackgroundColor', 'w', ... 253 | 'Position', [.8 0.97 .2 .03]); 254 | end 255 | -------------------------------------------------------------------------------- /mtools.m: -------------------------------------------------------------------------------- 1 | %MTOOLS add simple/useful tools to all windows in figure 2 | % 3 | 4 | 5 | % Copyright (C) 1993-2017, by Peter I. Corke 6 | % 7 | % This file is part of The Robotics Toolbox for MATLAB (RTB). 8 | % 9 | % RTB is free software: you can redistribute it and/or modify 10 | % it under the terms of the GNU Lesser General Public License as published by 11 | % the Free Software Foundation, either version 3 of the License, or 12 | % (at your option) any later version. 13 | % 14 | % RTB is distributed in the hope that it will be useful, 15 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | % GNU Lesser General Public License for more details. 18 | % 19 | % You should have received a copy of the GNU Leser General Public License 20 | % along with RTB. If not, see . 21 | % 22 | % http://www.petercorke.com 23 | function mtools 24 | global DDXFILENAME 25 | 26 | h = uicontextmenu; 27 | uimenu(h, 'Label', 'X zoom', 'CallBack', 'xaxis'); 28 | uimenu(h, 'Label', '-->', 'CallBack', 'xscroll(0.5)'); 29 | uimenu(h, 'Label', '<--', 'CallBack', 'xscroll(-0.5)'); 30 | uimenu(h, 'Label', 'CrossHairs', 'CallBack', 'crosshair'); 31 | uimenu(h, 'Label', 'X UNzoom', 'CallBack', 'unzoom'); 32 | uimenu(h, 'Label', 'Pick delta', 'CallBack', 'fprintf(''%f %f\n'', diff(ginput(2)))'); 33 | uimenu(h, 'Label', 'Line fit', 'CallBack', 'ilinefit'); 34 | uimenu(h, 'Label', 'Show points', 'CallBack', 'showpoints(gca)'); 35 | uimenu(h, 'Label', 'Apply X zoom to all', 'CallBack', 'xaxisall'); 36 | for c=get(gcf, 'Children')', 37 | set(c, 'UIContextMenu', h); 38 | l = get(c, 'Children'); 39 | end 40 | 41 | 42 | axes('pos', [0 0 1 0.05], 'visible', 'off') 43 | if 0 44 | if ~isempty(DDXFILENAME), 45 | s = sprintf('[%s] %s', DDXFILENAME, date); 46 | else 47 | s = sprintf('%s', date); 48 | end 49 | text(0.95, 0.1, s, 'horizontalalign', 'right', 'verticalalign', 'baseli') 50 | end 51 | -------------------------------------------------------------------------------- /pickregion.m: -------------------------------------------------------------------------------- 1 | %PICKREGION Pick a rectangular region of a figure using mouse 2 | % 3 | % [p1,p2] = PICKREGION() initiates a rubberband box at the current click point 4 | % and animates it so long as the mouse button remains down. Returns the first 5 | % and last coordinates in axis units. 6 | % 7 | % Options:: 8 | % 'axis',A The axis to select from (default current axis) 9 | % 'ls',LS Line style for foreground line (default ':y'); 10 | % 'bg'LS, Line style for background line (default '-k'); 11 | % 'width',W Line width (default 2) 12 | % 'pressed' Don't wait for first button press, use current position 13 | % 14 | % Notes:: 15 | % - Effectively a replacement for the builtin rbbox function which draws the box in 16 | % the wrong location on my Mac's external monitor. 17 | % 18 | % Author:: 19 | % Based on rubberband box from MATLAB Central written/Edited by Bob Hamans 20 | % (B.C.Hamans@student.tue.nl) 02-04-2003, in turn based on an idea of 21 | % Sandra Martinka's Rubberline. 22 | 23 | function [p1,p2]=pickregion(varargin) 24 | % handle options 25 | opt.axis = gca; 26 | opt.ls = ':y'; 27 | opt.bg = '-k'; 28 | opt.width = 2; 29 | opt.pressed = false; 30 | 31 | opt = tb_optparse(opt, varargin); 32 | 33 | h = opt.axis; 34 | 35 | % Get current user data 36 | cudata=get(gcf,'UserData'); 37 | hold on; 38 | 39 | % Wait for left mouse button to be pressed 40 | if ~opt.pressed 41 | k=waitforbuttonpress; 42 | end 43 | 44 | % get current point 45 | p1=get(h,'CurrentPoint'); %get starting point 46 | p1=p1(1,1:2); %extract x and y 47 | 48 | % create 2 overlaid lines for contrast: 49 | % black solid 50 | % color dotted 51 | lh1 = plot(p1(1),p1(2),opt.bg, 'LineWidth', opt.width); %plot starting point 52 | lh2 = plot(p1(1), p1(2), opt.ls, 'LineWidth', opt.width); 53 | 54 | % Save current point and handles in user data 55 | udata.p1=p1; 56 | udata.h=h; 57 | udata.lh1=lh1; 58 | udata.lh2=lh2; 59 | 60 | % Set handlers for mouse up and mouse motion 61 | udata.wbupOld = get(gcf, 'WindowButtonUp'); 62 | udata.wbmfOld = get(gcf, 'WindowButtonMotionFcn'); 63 | set(gcf, ... 64 | 'WindowButtonMotionFcn', @(src,event) wbmf(src,udata), ... 65 | 'WindowButtonUp', @(src,event) wbup(src,udata), ... 66 | 'DoubleBuffer','on'); 67 | 68 | % Wait until the lines have been destroyed 69 | waitfor(lh1); 70 | 71 | % Get data for the end point 72 | p2=get(h,'Currentpoint'); %get end point 73 | p2=p2(1,1:2); %extract x and y 74 | 75 | % Remove the mouse event handlers and restore user data 76 | set(gcf,'UserData',cudata,'DoubleBuffer','off'); 77 | end 78 | 79 | function wbmf(src, ud) %window motion callback function 80 | 81 | % get current coordinates 82 | P = get(ud.h,'CurrentPoint'); 83 | P = P(1,1:2); 84 | 85 | % Use 5 point to draw a rectangular rubberband box 86 | xdata = [P(1),P(1),ud.p1(1),ud.p1(1),P(1)]; 87 | ydata = [P(2),ud.p1(2),ud.p1(2),P(2),P(2)]; 88 | 89 | % draw the two lines 90 | set(ud.lh1,'XData', xdata,'YData', ydata); 91 | set(ud.lh2,'XData', xdata,'YData', ydata); 92 | 93 | end 94 | 95 | function wbup(src, ud) 96 | % remove motion handler 97 | set(gcf, 'WindowButtonMotionFcn', ud.wbmfOld); 98 | set(gcf, 'WindowButtonUpFcn', ud.wbupOld); 99 | 100 | 101 | % delete the lines 102 | delete(ud.lh2); 103 | delete(ud.lh1); 104 | end 105 | -------------------------------------------------------------------------------- /plotp.m: -------------------------------------------------------------------------------- 1 | %PLOTP Plot trajectory 2 | % 3 | % Convenience function to plot points stored columnwise. 4 | % 5 | % PLOTP(P) plots a set of points P, which by Toolbox convention are stored 6 | % one per column. P can be 2xN or 3xN. By default a linestyle of 'bx' 7 | % is used. 8 | % 9 | % PLOTP(P, LS) as above but the line style arguments LS are passed to plot. 10 | % 11 | % See also PLOT, PLOT2. 12 | 13 | 14 | % Copyright (C) 1993-2017, by Peter I. Corke 15 | % 16 | % This file is part of The Robotics Toolbox for MATLAB (RTB). 17 | % 18 | % RTB is free software: you can redistribute it and/or modify 19 | % it under the terms of the GNU Lesser General Public License as published by 20 | % the Free Software Foundation, either version 3 of the License, or 21 | % (at your option) any later version. 22 | % 23 | % RTB is distributed in the hope that it will be useful, 24 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 25 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 26 | % GNU Lesser General Public License for more details. 27 | % 28 | % You should have received a copy of the GNU Leser General Public License 29 | % along with RTB. If not, see . 30 | % 31 | % http://www.petercorke.com 32 | function h = plotp(p1, varargin) 33 | 34 | if length(varargin) == 0 35 | varargin = {'bx'}; 36 | end 37 | 38 | assert(any(numrows(p1) == [2 3]), 'RTB:plotp:badarg', '2D or 3D points, columnwise, only'); 39 | 40 | if numrows(p1) == 3 41 | hh = plot3(p1(1,:), p1(2,:), p1(3,:), varargin{:}); 42 | xyzlabel 43 | else 44 | hh = plot(p1(1,:), p1(2,:), varargin{:}); 45 | xlabel('x'); 46 | ylabel('y'); 47 | end 48 | if nargout == 1 49 | h = hh; 50 | end 51 | -------------------------------------------------------------------------------- /polydiff.m: -------------------------------------------------------------------------------- 1 | %POLYDIFF Differentiate a polynomial 2 | % 3 | % PD = POLYDIFF(P) is a vector of coefficients of a polynomial (1xN-1) which is the 4 | % derivative of the polynomial P (1xN). 5 | % 6 | % p = [3 2 -1]; 7 | % polydiff(p) 8 | % ans = 9 | % 6 2 10 | % 11 | % See also POLYVAL. 12 | 13 | 14 | % Copyright (C) 1993-2017, by Peter I. Corke 15 | % 16 | % This file is part of The Robotics Toolbox for MATLAB (RTB). 17 | % 18 | % RTB is free software: you can redistribute it and/or modify 19 | % it under the terms of the GNU Lesser General Public License as published by 20 | % the Free Software Foundation, either version 3 of the License, or 21 | % (at your option) any later version. 22 | % 23 | % RTB is distributed in the hope that it will be useful, 24 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 25 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 26 | % GNU Lesser General Public License for more details. 27 | % 28 | % You should have received a copy of the GNU Leser General Public License 29 | % along with RTB. If not, see . 30 | % 31 | % http://www.petercorke.com 32 | function pd = polydiff(p) 33 | n = length(p)-1; 34 | 35 | pd = [n:-1:1] .* p(1:n); 36 | -------------------------------------------------------------------------------- /protectfig.m: -------------------------------------------------------------------------------- 1 | 2 | 3 | % Copyright (C) 1993-2017, by Peter I. Corke 4 | % 5 | % This file is part of The Robotics Toolbox for MATLAB (RTB). 6 | % 7 | % RTB is free software: you can redistribute it and/or modify 8 | % it under the terms of the GNU Lesser General Public License as published by 9 | % the Free Software Foundation, either version 3 of the License, or 10 | % (at your option) any later version. 11 | % 12 | % RTB is distributed in the hope that it will be useful, 13 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | % GNU Lesser General Public License for more details. 16 | % 17 | % You should have received a copy of the GNU Leser General Public License 18 | % along with RTB. If not, see . 19 | % 20 | % http://www.petercorke.com 21 | function protectfig(h) 22 | if nargin == 0 23 | h = gcf; 24 | end 25 | 26 | set(h, 'HandleVisibility', 'off'); 27 | 28 | -------------------------------------------------------------------------------- /randinit.m: -------------------------------------------------------------------------------- 1 | %RANDINIT Reset random number generator 2 | % 3 | % RANDINIT resets the defaul random number stream. 4 | % 5 | % See also RandStream. 6 | 7 | 8 | % Copyright (C) 1993-2017, by Peter I. Corke 9 | % 10 | % This file is part of The Robotics Toolbox for MATLAB (RTB). 11 | % 12 | % RTB is free software: you can redistribute it and/or modify 13 | % it under the terms of the GNU Lesser General Public License as published by 14 | % the Free Software Foundation, either version 3 of the License, or 15 | % (at your option) any later version. 16 | % 17 | % RTB is distributed in the hope that it will be useful, 18 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 19 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 20 | % GNU Lesser General Public License for more details. 21 | % 22 | % You should have received a copy of the GNU Leser General Public License 23 | % along with RTB. If not, see . 24 | % 25 | % http://www.petercorke.com 26 | 27 | function randinit(seed) 28 | 29 | stream = RandStream.getGlobalStream; 30 | stream.reset() 31 | 32 | -------------------------------------------------------------------------------- /runscript.m: -------------------------------------------------------------------------------- 1 | %RUNSCRIPT Run an M-file in interactive fashion 2 | % 3 | % RUNSCRIPT(SCRIPT, OPTIONS) runs the M-file SCRIPT and pauses after every 4 | % executable line in the file until a key is pressed. Comment lines are shown 5 | % without any delay between lines. 6 | % 7 | % Options:: 8 | % 'delay',D Don't wait for keypress, just delay of D seconds (default 0) 9 | % 'cdelay',D Pause of D seconds after each comment line (default 0) 10 | % 'begin' Start executing the file after the comment line %%begin (default false) 11 | % 'dock' Cause the figures to be docked when created 12 | % 'path',P Look for the file SCRIPT in the folder P (default .) 13 | % 'dock' Dock figures within GUI 14 | % 'nocolor' Don't use cprintf to print lines in color (comments black, code blue) 15 | % 16 | % Notes:: 17 | % - If no file extension is given in SCRIPT, .m is assumed. 18 | % - A copyright text block will be skipped and not displayed. 19 | % - If cprintf exists and 'nocolor' is not given then lines are displayed 20 | % in color. 21 | % - Leading comment characters are not displayed. 22 | % - If the executable statement has comments immediately afterward (no blank lines) 23 | % then the pause occurs after those comments are displayed. 24 | % - A simple '-' prompt indicates when the script is paused, hit enter. 25 | % - If the function cprintf() is in your path, the display is more 26 | % colorful. You can get this file from MATLAB File Exchange. 27 | % - If the file has a lot of boilerplate, you can skip over and not display 28 | % it by giving the 'begin' option which searchers for the first line 29 | % starting with %%begin and commences execution at the line after that. 30 | % 31 | % See also eval. 32 | 33 | 34 | 35 | % Copyright (C) 1993-2017, by Peter I. Corke 36 | % 37 | % This file is part of The Robotics Toolbox for MATLAB (RTB). 38 | % 39 | % RTB is free software: you can redistribute it and/or modify 40 | % it under the terms of the GNU Lesser General Public License as published by 41 | % the Free Software Foundation, either version 3 of the License, or 42 | % (at your option) any later version. 43 | % 44 | % RTB is distributed in the hope that it will be useful, 45 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 46 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 47 | % GNU Lesser General Public License for more details. 48 | % 49 | % You should have received a copy of the GNU Leser General Public License 50 | % along with RTB. If not, see . 51 | % 52 | % http://www.petercorke.com 53 | 54 | function runscript(fname, varargin) 55 | 56 | opt.path = []; 57 | opt.delay = []; 58 | opt.begin = false; 59 | opt.cdelay = 0; 60 | opt.dock = false; 61 | opt.color = true; 62 | 63 | opt = tb_optparse(opt, varargin); 64 | 65 | if ~exist('cprintf') 66 | opt.color = false; 67 | end 68 | 69 | close all 70 | 71 | curDir = pwd(); 72 | 73 | prevDockStatus = get(0,'DefaultFigureWindowStyle'); 74 | if opt.dock 75 | set(0,'DefaultFigureWindowStyle','docked'); 76 | else 77 | set(0,'DefaultFigureWindowStyle','normal'); 78 | end 79 | 80 | 81 | if ~isempty(opt.path) 82 | fname = fullfile(opt.path, [fname '.m']); 83 | else 84 | fname = [fname '.m']; 85 | end 86 | 87 | fp = fopen(fname, 'r'); 88 | 89 | clc 90 | fprintf('--- runscript <-- %s\n', fname); 91 | 92 | running = false; 93 | shouldPause = false; 94 | savedText = []; 95 | 96 | if ~opt.begin 97 | running = true; 98 | end 99 | 100 | lineNum = 1; 101 | 102 | skipping = false; 103 | 104 | % stashMode 105 | % 0 normal 106 | % 1 loop 107 | % 2 continuation 108 | continMode = false; 109 | compoundDepth = 0; 110 | 111 | while 1 112 | % get the next line from the file, bail if EOF 113 | line = fgetl(fp); 114 | if line == -1 115 | break 116 | end 117 | lineNum = lineNum+1; 118 | 119 | if startswith(line, '% Copyright') 120 | skipping = true; 121 | continue; 122 | end 123 | 124 | % logic to skip lines until we see one beginning with %%begin 125 | if ~running 126 | if strcmp(line, '%%begin') 127 | running = true; 128 | else 129 | continue; 130 | end 131 | end; 132 | 133 | if length(strtrim(line)) == 0 134 | % blank line 135 | 136 | if skipping 137 | skipping = false; 138 | end 139 | fprintf('\n'); 140 | if shouldPause 141 | scriptwait(opt); 142 | shouldPause = false; 143 | end 144 | continue 145 | elseif skipping 146 | continue; 147 | elseif startswith(strtrim(line), '%') 148 | % line was a comment 149 | disp( strtrim(line(2:end)) ) 150 | pause(opt.cdelay) % optional comment delay 151 | continue; 152 | else 153 | if shouldPause 154 | scriptwait(opt); 155 | shouldPause = false; 156 | end 157 | end 158 | 159 | % if the start of a loop, stash the text for now 160 | if startswith(line, 'for') || startswith(line, 'while') || startswith(line, 'if') 161 | % found a compound block, don't eval it until we get to the end 162 | compoundDepth = compoundDepth + 1; 163 | end 164 | % if the statement has a continuation 165 | if endswith(line, '...') && compoundDepth == 0 166 | % found a compound statement, don't eval it until we get to the end 167 | continMode = true; 168 | end 169 | 170 | if compoundDepth == 0 && ~continMode 171 | prompt = '>> '; 172 | else 173 | prompt = ''; 174 | end 175 | 176 | % display the line with a pretend MATLAB prompt 177 | if opt.color 178 | cprintf('blue', '%s%s', prompt, line) 179 | else 180 | fprintf('%s', prompt); disp(line) 181 | end 182 | 183 | if compoundDepth > 0 || continMode 184 | % we're in stashing mode 185 | savedText = strcat(savedText, '\n', line); 186 | end 187 | 188 | if compoundDepth > 0 && startswith(line, 'end') 189 | % the compound block is fully unnested 190 | 191 | compoundDepth = compoundDepth - 1; 192 | if compoundDepth == 0 193 | evalSavedText(savedText, lineNum, opt); 194 | savedText = ''; 195 | shouldPause = true; 196 | end 197 | continue 198 | 199 | elseif continMode && ~endswith(line, '...') 200 | % no longer in continuation mode 201 | 202 | evalSavedText(savedText, lineNum, opt); 203 | savedText = ''; 204 | continMode = false; 205 | shouldPause = true; 206 | continue 207 | end 208 | 209 | if compoundDepth == 0 && ~continMode 210 | % it's a simple executable statement, execute it 211 | fprintf(' \n'); 212 | try 213 | evalSavedText(line, lineNum, opt); 214 | catch 215 | break 216 | end 217 | shouldPause = true; 218 | end 219 | end 220 | fprintf('------ done --------\n'); 221 | % restore the docking mode if we set it 222 | set(0,'DefaultFigureWindowStyle', prevDockStatus) 223 | cd(curDir) 224 | end 225 | 226 | function evalSavedText(text, lineNum, opt) 227 | if length(strtrim(text)) == 0 228 | return 229 | end 230 | 231 | text = sprintf(text); 232 | 233 | try 234 | if opt.color 235 | text = strrep(text, '''', ''''''); % fix single quotes 236 | t = evalin('base', strcat('evalc(''', text, ''')') ); 237 | cprintf('blue', '%s', t); 238 | else 239 | evalin('base', text); 240 | end 241 | catch m 242 | fprintf('error in script %s at line %d', fname, lineNum); 243 | m.rethrow(); 244 | end 245 | fprintf('\n'); 246 | end 247 | 248 | % delay or prompt according to passed options 249 | function scriptwait(opt) 250 | if isempty(opt.delay) 251 | %a = input('-', 's'); 252 | prompt = 'continue?'; 253 | bs = repmat('\b', [1 length(prompt)]); 254 | 255 | if opt.color 256 | cprintf('red', prompt); pause; 257 | cprintf('text', bs); 258 | else 259 | fprintf(prompt); pause; 260 | fprintf(bs); 261 | end 262 | else 263 | pause(opt.delay); 264 | end 265 | end 266 | 267 | % test if s2 is at the start of s1 (ignoring leading spaces) 268 | function res = startswith(s1, s2) 269 | 270 | s1 = strtrim(s1); % trim leading white space 271 | r = strfind(s1, s2); 272 | res = false; 273 | if ~isempty(r) && (r(1) == 1) 274 | res = true; 275 | end 276 | end 277 | 278 | % test if s2 is at the end of s1 279 | function res = endswith(s1, s2) 280 | 281 | if length(s1) < length(s2) 282 | res = false; 283 | else 284 | n2 = length(s2)-1; 285 | res = strcmp(s1(end-n2:end), s2); 286 | end 287 | 288 | end 289 | 290 | -------------------------------------------------------------------------------- /rvccheck.m: -------------------------------------------------------------------------------- 1 | function rvccheck(verbose) 2 | 3 | if nargin == 0 4 | verbose = true; 5 | end 6 | 7 | % display current versions of MATLAB 8 | year = version('-release'); 9 | if verbose 10 | fprintf('You are using:\n - MATLAB release %s\n', year); 11 | end 12 | 13 | % check how old it is 14 | today = datevec(now); 15 | age = today(1) - str2num(year(1:4)); 16 | if age >= 2 17 | fprintf(' ** this is at least %d years old, you may have issues\n', age); 18 | end 19 | 20 | % display versions of toolboxes (use unique RTB and MVTB functions) 21 | p = getpath('lspb'); 22 | a = ver( p ); 23 | rtb = ~isempty(a); 24 | 25 | p = getpath('idisp'); 26 | a = ver( p ); 27 | mvtb = ~isempty(a); 28 | 29 | if verbose 30 | if rtb 31 | if findstr(p, 'Add-Ons') 32 | where = 'mltbx install to Add-Ons'; 33 | else 34 | if exist( fullfile(p, '.git'), 'dir' ) 35 | where = 'local (git clone) install'; 36 | else 37 | where = 'local (zip) install'; 38 | end 39 | end 40 | fprintf(' - %s %s %s [%s]\n', a.Name, a.Version, a.Date, where); 41 | 42 | end 43 | 44 | if mvtb 45 | if findstr(p, 'Add-Ons') 46 | where = 'mltbx install to Add-Ons'; 47 | else 48 | if exist( fullfile(p, '.git'), 'dir' ) 49 | where = 'local (git clone) install'; 50 | else 51 | where = 'local (zip) install'; 52 | end 53 | end 54 | fprintf(' - %s %s %s [%s]\n', a.Name, a.Version, a.Date, where); 55 | end 56 | end 57 | 58 | % check for shadowed files 59 | k = 0; 60 | if rtb 61 | k = k + checkpath('rotx'); 62 | k = k + checkpath('roty'); 63 | k = k + checkpath('rotz'); 64 | k = k + checkpath('angdiff'); 65 | end 66 | if mvtb 67 | k = k + checkpath('im2col'); 68 | k = k + checkpath('col2im'); 69 | k = k + checkpath('angdiff'); 70 | end 71 | 72 | if k > 0 73 | fprintf('Some Toolbox files are "shadowed" and will cause problems with the use of this toolbox\n'); 74 | fprintf('Use path tool to move this Toolbox to the top of the path\n') 75 | end 76 | end 77 | 78 | function k = checkpath(funcname) 79 | 80 | funcpath = which(funcname); % find first instance in path 81 | k = 0; 82 | 83 | good = {'rvc', ... 84 | 'robotics-toolbox-matlab', 'Robotics Toolbox for MATLAB', ... 85 | 'machinevision-toolbox-matlab', 'Machine Vision Toolbox for MATLAB', ... 86 | 'smtb', 'spatial-math', 'Spatial Math Toolbox for MATLAB'}; 87 | if exist(funcname) 88 | if all( cellfun(@(x) isempty(strfind(funcpath, x)), good) ) 89 | fprintf('** Toolbox function %s is shadowed by %s\n', funcname, which(funcname) ); 90 | k = 1; 91 | end 92 | end 93 | end 94 | 95 | function p = getpath(funcname) 96 | funcpath = which(funcname); 97 | k = strfind( funcpath, [filesep funcname]); 98 | p = funcpath(1:k-1); 99 | end -------------------------------------------------------------------------------- /rvcpath.m: -------------------------------------------------------------------------------- 1 | %RVCPATH Install location of RVC tools 2 | % 3 | % p = RVCPATH is the path of the top level folder for the installed RVC 4 | % tools. 5 | % 6 | % p = RVCPATH(FOLDER) is the full path of the specified FOLDER which is relative to the 7 | % installed RVC tools. 8 | % 9 | 10 | % Copyright (C) 1993-2017, by Peter I. Corke 11 | % 12 | % This file is part of The Robotics Toolbox for MATLAB (RTB). 13 | % 14 | % RTB is free software: you can redistribute it and/or modify 15 | % it under the terms of the GNU Lesser General Public License as published by 16 | % the Free Software Foundation, either version 3 of the License, or 17 | % (at your option) any later version. 18 | % 19 | % RTB is distributed in the hope that it will be useful, 20 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 21 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 22 | % GNU Lesser General Public License for more details. 23 | % 24 | % You should have received a copy of the GNU Leser General Public License 25 | % along with RTB. If not, see . 26 | % 27 | % http://www.petercorke.com 28 | function p = rvcpath(a) 29 | p = fileparts( which('startup_rvc.m') ); 30 | 31 | if nargin > 0 32 | p = fullfile(p, a); 33 | end 34 | -------------------------------------------------------------------------------- /startup_rvc.m: -------------------------------------------------------------------------------- 1 | function startup_rvc 2 | disp('Robotics, Vision & Control: (c) Peter Corke 1992-2019 http://www.petercorke.com') 3 | 4 | if verLessThan('matlab', '7.0') 5 | warning('You are running a very old (and unsupported) version of MATLAB. You will very likely encounter significant problems using the toolboxes but you are on your own with this'); 6 | end 7 | tb = false; 8 | startup_path = fileparts( mfilename('fullpath') ); 9 | [~,folder]=fileparts(startup_path); 10 | if strcmp(folder, 'common') 11 | % startup_rvc is in common folder 12 | rvcpath = fileparts(startup_path); 13 | else 14 | % startup_rvc is in folder above common 15 | rvcpath = startup_path; 16 | end 17 | 18 | robotpath = fullfile(rvcpath, 'robot'); 19 | if exist(robotpath, 'dir') 20 | addpath(robotpath); 21 | tb = true; 22 | if exist('startup_rtb') == 2 23 | startup_rtb 24 | end 25 | end 26 | 27 | visionpath = fullfile(rvcpath, 'vision'); 28 | if exist(visionpath, 'dir') 29 | addpath(visionpath); 30 | tb = true; 31 | if exist('startup_mvtb') == 2 32 | startup_mvtb 33 | end 34 | end 35 | 36 | if tb 37 | % RTB or MVTB is present 38 | 39 | 40 | 41 | % add spatial math toolbox 42 | p = fullfile(rvcpath, 'smtb'); 43 | if exist(p, 'dir') 44 | try 45 | fp = fopen( fullfile(p, 'RELEASE'), 'r'); 46 | release = fgetl(fp); 47 | fclose(fp); 48 | catch ME 49 | release = []; 50 | end 51 | if release 52 | release = ['(release ' release ')']; 53 | else 54 | release = ''; 55 | end 56 | fprintf('- Spatial Math Toolbox for MATLAB %s\n', release) 57 | addpath(p); 58 | end 59 | 60 | % add a top-level simulink folder as created for mltbx installs 61 | p = fullfile(rvcpath, 'simulink'); 62 | if exist(p, 'dir') 63 | addpath(); 64 | end 65 | 66 | % add common files 67 | addpath(fullfile(rvcpath, 'common')); 68 | else 69 | fprintf('Neither Robotics Toolbox or MachineVision Toolbox found in %s\n', rvcpath); 70 | end 71 | 72 | % check for any install problems 73 | rvccheck(false) 74 | end -------------------------------------------------------------------------------- /stlRead.m: -------------------------------------------------------------------------------- 1 | %STLREAD reads any STL file not depending on its format 2 | % 3 | % [v, f, n, name] = stlRead(fileName) reads the STL format file (ASCII or 4 | % binary) and returns vertices V, faces F, normals N and NAME is the name 5 | % of the STL object (NOT the name of the STL file). 6 | % 7 | % Authors:: 8 | % - from MATLAB File Exchange by Pau Mico, https://au.mathworks.com/matlabcentral/fileexchange/51200-stltools 9 | % - Copyright (c) 2015, Pau Mico 10 | % - Copyright (c) 2013, Adam H. Aitkenhead 11 | % - Copyright (c) 2011, Francis Esmonde-White 12 | 13 | % 14 | % stlGetFormat: identifies the format of the STL file and returns 'binary' 15 | % or 'ascii'. This file is inspired in the 'READ-stl' file written and 16 | % published by Adam H. Aitkenhead 17 | % (http://www.mathworks.com/matlabcentral/fileexchange/27390-mesh-voxelisation). 18 | % Copyright (c) 2013, Adam H. Aitkenhead. 19 | % 20 | % stlReadAscii: reads an STL file written in ascii format. This file is 21 | % inspired in the 'READ-stl' file written and published by Adam H. 22 | % Aitkenhead 23 | % (http://www.mathworks.com/matlabcentral/fileexchange/27390-mesh-voxelisation). 24 | % Copyright (c) 2013, Adam H. Aitkenhead 25 | % 26 | % stlReadBinary: reads an STL file written in binary format. This file 27 | % is inspired in the 'READ-stl' file written and published by Adam H. 28 | % Aitkenhead 29 | % (http://www.mathworks.com/matlabcentral/fileexchange/27390-mesh-voxelisation). 30 | % Copyright (c) 2013, Adam H. Aitkenhead stlRead: uses 'stlGetFormat', 31 | % 32 | % 'stlReadAscii' and 'stlReadBinary' to make STL reading independent of the 33 | % format of the file 34 | % 35 | % stlSlimVerts: finds and removes duplicated vertices. This function is 36 | % written and published by Francis Esmonde-White as PATCHSLIM 37 | % (http://www.mathworks.com/matlabcentral/fileexchange/29986-patch-slim--patchslim-m-). 38 | % Copyright (c) 2011, Francis Esmonde-White.% 39 | % 40 | % 41 | % Redistribution and use in source and binary forms, with or without 42 | % modification, are permitted provided that the following conditions are 43 | % met: 44 | % 45 | % * Redistributions of source code must retain the above copyright 46 | % notice, this list of conditions and the following disclaimer. 47 | % * Redistributions in binary form must reproduce the above copyright 48 | % notice, this list of conditions and the following disclaimer in 49 | % the documentation and/or other materials provided with the distribution 50 | % * Neither the name of the The MathWorks, Inc. nor the names 51 | % of its contributors may be used to endorse or promote products derived 52 | % from this software without specific prior written permission. 53 | % * Neither the name of the The Christie NHS Foundation Trust nor the names 54 | % of its contributors may be used to endorse or promote products derived 55 | % from this software without specific prior written permission. 56 | % 57 | % THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 58 | % AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 59 | % IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 60 | % ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 61 | % LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 62 | % CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 63 | % SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 64 | % INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 65 | % CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 66 | % ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 67 | % POSSIBILITY OF SUCH DAMAGE. 68 | 69 | 70 | function [v, f, n, name] = stlRead(fileName) 71 | %STLREAD reads any STL file not depending on its format 72 | %V are the vertices 73 | %F are the faces 74 | %N are the normals 75 | %NAME is the name of the STL object (NOT the name of the STL file) 76 | 77 | format = stlGetFormat(fileName); 78 | if strcmp(format,'ascii') 79 | [v,f,n,name] = stlReadAscii(fileName); 80 | elseif strcmp(format,'binary') 81 | [v,f,n,name] = stlReadBinary(fileName); 82 | end 83 | end 84 | 85 | 86 | function format = stlGetFormat(fileName) 87 | %STLGETFORMAT identifies the format of the STL file and returns 'binary' or 88 | %'ascii' 89 | 90 | fid = fopen(fileName); 91 | 92 | assert(fid > 0, 'Cant find file %s', fileName); 93 | 94 | % Check the file size first, since binary files MUST have a size of 84+(50*n) 95 | fseek(fid,0,1); % Go to the end of the file 96 | fidSIZE = ftell(fid); % Check the size of the file 97 | if rem(fidSIZE-84,50) > 0 98 | format = 'ascii'; 99 | else 100 | % Files with a size of 84+(50*n), might be either ascii or binary... 101 | % Read first 80 characters of the file. 102 | % For an ASCII file, the data should begin immediately (give or take a few 103 | % blank lines or spaces) and the first word must be 'solid'. 104 | % For a binary file, the first 80 characters contains the header. 105 | % It is bad practice to begin the header of a binary file with the word 106 | % 'solid', so it can be used to identify whether the file is ASCII or 107 | % binary. 108 | fseek(fid,0,-1); % go to the beginning of the file 109 | header = strtrim(char(fread(fid,80,'uchar')')); % trim leading and trailing spaces 110 | isSolid = strcmp(header(1:min(5,length(header))),'solid'); % take first 5 char 111 | fseek(fid,-80,1); % go to the end of the file minus 80 characters 112 | tail = char(fread(fid,80,'uchar')'); 113 | isEndSolid = findstr(tail,'endsolid'); 114 | 115 | % Double check by reading the last 80 characters of the file. 116 | % For an ASCII file, the data should end (give or take a few 117 | % blank lines or spaces) with 'endsolid '. 118 | % If the last 80 characters contains the word 'endsolid' then this 119 | % confirms that the file is indeed ASCII. 120 | if isSolid & isEndSolid 121 | format = 'ascii'; 122 | else 123 | format = 'binary'; 124 | end 125 | end 126 | fclose(fid); 127 | end 128 | 129 | function [v, f, n, name] = stlReadAscii(fileName) 130 | %STLREADASCII reads a STL file written in ASCII format 131 | %V are the vertices 132 | %F are the faces 133 | %N are the normals 134 | %NAME is the name of the STL object (NOT the name of the STL file) 135 | 136 | %====================== 137 | % STL ascii file format 138 | %====================== 139 | % ASCII STL files have the following structure. Technically each facet 140 | % could be any 2D shape, but in practice only triangular facets tend to be 141 | % used. The present code ONLY works for meshes composed of triangular 142 | % facets. 143 | % 144 | % solid object_name 145 | % facet normal x y z 146 | % outer loop 147 | % vertex x y z 148 | % vertex x y z 149 | % vertex x y z 150 | % endloop 151 | % endfacet 152 | % 153 | % 154 | % 155 | % endsolid object_name 156 | 157 | fid = fopen(fileName); 158 | cellcontent = textscan(fid,'%s','delimiter','\n'); % read all the file and put content in cells 159 | content = cellcontent{:}(logical(~strcmp(cellcontent{:},''))); % remove all blank lines 160 | fclose(fid); 161 | 162 | % read the STL name 163 | line1 = char(content(1)); 164 | if (size(line1,2) >= 7) 165 | name = line1(7:end); 166 | else 167 | name = 'Unnamed Object'; 168 | end 169 | 170 | % read the vector normals 171 | normals = char(content(logical(strncmp(content,'facet normal',12)))); 172 | n = str2num(normals(:,13:end)); 173 | 174 | % read the vertex coordinates (vertices) 175 | vertices = char(content(logical(strncmp(content,'vertex',6)))); 176 | v = str2num(vertices(:,7:end)); 177 | nvert = length(vertices); % number of vertices 178 | nfaces = sum(strcmp(content,'endfacet')); % number of faces 179 | if (nvert == 3*nfaces) 180 | f = reshape(1:nvert,[3 nfaces])'; % create faces 181 | end 182 | 183 | % slim the file (delete duplicated vertices) 184 | [v,f] = stlSlimVerts(v,f); 185 | end 186 | 187 | function [v, f, n, name] = stlReadBinary(fileName) 188 | %STLREADBINARY reads a STL file written in BINARY format 189 | %V are the vertices 190 | %F are the faces 191 | %N are the normals 192 | %NAME is the name of the STL object (NOT the name of the STL file) 193 | 194 | %======================= 195 | % STL binary file format 196 | %======================= 197 | % Binary STL files have an 84 byte header followed by 50-byte records, each 198 | % describing a single facet of the mesh. Technically each facet could be 199 | % any 2D shape, but that would screw up the 50-byte-per-facet structure, so 200 | % in practice only triangular facets are used. The present code ONLY works 201 | % for meshes composed of triangular facets. 202 | % 203 | % HEADER: 204 | % 80 bytes: Header text 205 | % 4 bytes: (int) The number of facets in the STL mesh 206 | % 207 | % DATA: 208 | % 4 bytes: (float) normal x 209 | % 4 bytes: (float) normal y 210 | % 4 bytes: (float) normal z 211 | % 4 bytes: (float) vertex1 x 212 | % 4 bytes: (float) vertex1 y 213 | % 4 bytes: (float) vertex1 z 214 | % 4 bytes: (float) vertex2 x 215 | % 4 bytes: (float) vertex2 y 216 | % 4 bytes: (float) vertex2 z 217 | % 4 bytes: (float) vertex3 x 218 | % 4 bytes: (float) vertex3 y 219 | % 4 bytes: (float) vertex3 z 220 | % 2 bytes: Padding to make the data for each facet 50-bytes in length 221 | % ...and repeat for next facet... 222 | 223 | fid = fopen(fileName); 224 | header = fread(fid,80,'int8'); % reading header's 80 bytes 225 | name = deblank(native2unicode(header,'ascii')'); 226 | if isempty(name) 227 | name = 'Unnamed Object'; % no object name in binary files! 228 | end 229 | nfaces = fread(fid,1,'int32'); % reading the number of facets in the stl file (next 4 byters) 230 | nvert = 3*nfaces; % number of vertices 231 | % reserve memory for vectors (increase the processing speed) 232 | n = zeros(nfaces,3); 233 | v = zeros(nvert,3); 234 | f = zeros(nfaces,3); 235 | for i = 1 : nfaces % read the data for each facet 236 | tmp = fread(fid,3*4,'float'); % read coordinates 237 | n(i,:) = tmp(1:3); % x,y,z components of the facet's normal vector 238 | v(3*i-2,:) = tmp(4:6); % x,y,z coordinates of vertex 1 239 | v(3*i-1,:) = tmp(7:9); % x,y,z coordinates of vertex 2 240 | v(3*i,:) = tmp(10:12); % x,y,z coordinates of vertex 3 241 | f(i,:) = [3*i-2 3*i-1 3*i]; % face 242 | fread(fid,1,'int16'); % Move to the start of the next facet (2 bytes of padding) 243 | end 244 | fclose(fid); 245 | % slim the file (delete duplicated vertices) 246 | [v,f] = stlSlimVerts(v,f); 247 | end 248 | 249 | function [vnew, fnew]= stlSlimVerts(v, f) 250 | % PATCHSLIM removes duplicate vertices in surface meshes. 251 | % 252 | % This function finds and removes duplicate vertices. 253 | % 254 | % USAGE: [v, f]=patchslim(v, f) 255 | % 256 | % Where v is the vertex list and f is the face list specifying vertex 257 | % connectivity. 258 | % 259 | % v contains the vertices for all triangles [3*n x 3]. 260 | % f contains the vertex lists defining each triangle face [n x 3]. 261 | % 262 | % This will reduce the size of typical v matrix by about a factor of 6. 263 | % 264 | % For more information see: 265 | % http://www.esmonde-white.com/home/diversions/matlab-program-for-loading-stl-files 266 | % 267 | % Francis Esmonde-White, May 2010 268 | 269 | if ~exist('v','var') 270 | error('The vertex list (v) must be specified.'); 271 | end 272 | if ~exist('f','var') 273 | error('The vertex connectivity of the triangle faces (f) must be specified.'); 274 | end 275 | 276 | [vnew, indexm, indexn] = unique(v, 'rows'); 277 | fnew = indexn(f); 278 | end 279 | 280 | -------------------------------------------------------------------------------- /undockfigs.m: -------------------------------------------------------------------------------- 1 | 2 | 3 | % Copyright (C) 1993-2017, by Peter I. Corke 4 | % 5 | % This file is part of The Robotics Toolbox for MATLAB (RTB). 6 | % 7 | % RTB is free software: you can redistribute it and/or modify 8 | % it under the terms of the GNU Lesser General Public License as published by 9 | % the Free Software Foundation, either version 3 of the License, or 10 | % (at your option) any later version. 11 | % 12 | % RTB is distributed in the hope that it will be useful, 13 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | % GNU Lesser General Public License for more details. 16 | % 17 | % You should have received a copy of the GNU Leser General Public License 18 | % along with RTB. If not, see . 19 | % 20 | % http://www.petercorke.com 21 | set(0, 'DefaultFigureWindowStyle', 'normal') 22 | -------------------------------------------------------------------------------- /unit_test/PluckerTest.m: -------------------------------------------------------------------------------- 1 | 2 | function tests = PluckerTest 3 | tests = functiontests(localfunctions); 4 | end 5 | 6 | function constructor_test(tc) 7 | end 8 | 9 | function methods_test(tc) 10 | % intersection 11 | px = Plucker([0 0 0], [1 0 0]); % x-axis 12 | py = Plucker([0 0 0], [0 1 0]); % y-axis 13 | px1 = Plucker([0 1 0], [1 1 0]); % offset x-axis 14 | 15 | verifyEqual(tc, px.origin_distance(), 0); 16 | verifyEqual(tc, px1.origin_distance(), 1); 17 | verifyEqual(tc, px1.origin_closesst(), [0 1 0]'); 18 | 19 | 20 | 21 | px.intersect(px) 22 | px.intersect(py) 23 | px.intersect(px1) 24 | end 25 | 26 | function intersect_test(tc) 27 | px = Plucker([0 0 0], [1 0 0]); % x-axis 28 | py = Plucker([0 0 0], [0 1 0]); % y-axis 29 | 30 | plane.d = [1 0 0]; plane.p = 2; % plane x=2 31 | 32 | px.intersect_plane(plane) 33 | py.intersect_plane(plane) 34 | end 35 | 36 | -------------------------------------------------------------------------------- /unit_test/plotXTest.m: -------------------------------------------------------------------------------- 1 | % 2d outline, filled case 2 | % 3d outlien, filled case 3 | % with LS or edgecolor, color options etc. 4 | 5 | function tests = plotXTest 6 | tests = functiontests(localfunctions); 7 | close all 8 | end 9 | 10 | function teardownOnce(tc) 11 | close all 12 | end 13 | 14 | function plotpoint_test(tc) 15 | 16 | % simple 17 | points = rand(2,5); 18 | clf; plot_point(points); 19 | tc.verifyEqual(length(get(gca, 'Children')), 5); 20 | tc.verifyEqual(length(findobj(gca, 'Type', 'line')), 5); 21 | lines = findobj(gca, 'Type', 'line'); 22 | for i=1:5 23 | tc.verifyEqual(length(lines(i).XData), 1); 24 | tc.verifyEqual(length(lines(i).YData), 1); 25 | tc.verifyEqual(lines(i).LineStyle, 'none'); 26 | tc.verifyEqual(lines(i).MarkerSize, 6); 27 | tc.verifyEqual(lines(i).MarkerFaceColor, 'none'); 28 | tc.verifyEqual(lines(i).Marker, 'square'); 29 | end 30 | 31 | % markers specified 32 | clf; plot_point(points, 'rd'); 33 | tc.verifyEqual(length(get(gca, 'Children')), 5); 34 | tc.verifyEqual(length(findobj(gca, 'Type', 'line')), 5); 35 | lines = findobj(gca, 'Type', 'line'); 36 | for i=1:5 37 | tc.verifyEqual(lines(i).LineStyle, 'none'); 38 | tc.verifyEqual(lines(i).MarkerSize, 6); 39 | tc.verifyEqual(lines(i).MarkerFaceColor, 'none'); 40 | tc.verifyEqual(lines(i).Marker, 'diamond'); 41 | tc.verifyEqual(lines(i).Color, [1 0 0]); 42 | end 43 | 44 | % markers specified solid 45 | clf; plot_point(points, 'rd', 'solid'); 46 | tc.verifyEqual(length(get(gca, 'Children')), 5); 47 | tc.verifyEqual(length(findobj(gca, 'Type', 'line')), 5); 48 | lines = findobj(gca, 'Type', 'line'); 49 | for i=1:5 50 | tc.verifyEqual(lines(i).LineStyle, 'none'); 51 | tc.verifyEqual(lines(i).MarkerSize, 6); 52 | tc.verifyEqual(lines(i).MarkerFaceColor, [1 0 0]); 53 | tc.verifyEqual(lines(i).Marker, 'diamond'); 54 | tc.verifyEqual(lines(i).Color, [1 0 0]); 55 | end 56 | 57 | % sequential labels 58 | clf; plot_point(points, 'sequence'); 59 | tc.verifyEqual(length(get(gca, 'Children')), 10); 60 | tc.verifyEqual(length(findobj(gca, 'Type', 'text')), 5); 61 | labels = findobj(gca, 'Type', 'text'); 62 | set = [1:5]; 63 | for i=1:5 64 | set = setdiff(set, str2num(labels(i).String)); 65 | end 66 | tc.verifyEmpty(set, 'Not all labels found'); 67 | 68 | % specified labels 69 | L = {'A', 'B', 'C', 'D', 'E'}; 70 | clf; plot_point(points, 'label', L); 71 | tc.verifyEqual(length(get(gca, 'Children')), 10); 72 | tc.verifyEqual(length(findobj(gca, 'Type', 'text')), 5); 73 | labels = findobj(gca, 'Type', 'text'); 74 | set = [1:5]; 75 | for i=1:5 76 | set = setdiff(set, double(strip(labels(i).String))-'A'+1); 77 | end 78 | tc.verifyEmpty(set, 'Not all labels found'); 79 | 80 | % printf labels 81 | clf; plot_point(points, 'printf', {'label=%d', 21:25}); 82 | tc.verifyEqual(length(get(gca, 'Children')), 10); 83 | tc.verifyEqual(length(findobj(gca, 'Type', 'text')), 5); 84 | labels = findobj(gca, 'Type', 'text'); 85 | set = [21:25]; 86 | for i=1:5 87 | l = strip(labels(i).String); 88 | tc.verifyEqual(l(1:6), 'label='); 89 | set = setdiff(set, str2num(l(7:end))); 90 | end 91 | tc.verifyEmpty(set, 'Not all labels found'); 92 | 93 | % specify font size 94 | clf; plot_point(points, 'sequence', 'textsize', 23); 95 | tc.verifyEqual(length(get(gca, 'Children')), 10); 96 | 97 | tc.verifyEqual(length(findobj(gca, 'Type', 'line')), 5); 98 | tc.verifyEqual(length(findobj(gca, 'Type', 'text')), 5); 99 | labels = findobj(gca, 'Type', 'text'); 100 | for i=1:5 101 | tc.verifyEqual(labels(i).FontSize, 23); 102 | tc.verifyEqual(labels(i).FontWeight, 'normal'); 103 | end 104 | 105 | % specify font color 106 | clf; plot_point(points, 'sequence', 'textcolor', 'g'); 107 | tc.verifyEqual(length(get(gca, 'Children')), 10); 108 | 109 | tc.verifyEqual(length(findobj(gca, 'Type', 'line')), 5); 110 | tc.verifyEqual(length(findobj(gca, 'Type', 'text')), 5); 111 | labels = findobj(gca, 'Type', 'text'); 112 | for i=1:5 113 | tc.verifyEqual(labels(i).Color, [0 1 0]); 114 | tc.verifyEqual(labels(i).FontWeight, 'normal'); 115 | end 116 | 117 | % specify font color 118 | clf; plot_point(points, 'sequence', 'textcolor', [0.2 0.3 0.4]); 119 | tc.verifyEqual(length(get(gca, 'Children')), 10); 120 | 121 | tc.verifyEqual(length(findobj(gca, 'Type', 'line')), 5); 122 | tc.verifyEqual(length(findobj(gca, 'Type', 'text')), 5); 123 | labels = findobj(gca, 'Type', 'text'); 124 | for i=1:5 125 | tc.verifyEqual(labels(i).Color, [0.2 0.3 0.4]); 126 | tc.verifyEqual(labels(i).FontWeight, 'normal'); 127 | end 128 | 129 | % specify font weight 130 | clf; plot_point(points, 'sequence', 'bold'); 131 | tc.verifyEqual(length(get(gca, 'Children')), 10); 132 | 133 | tc.verifyEqual(length(findobj(gca, 'Type', 'line')), 5); 134 | tc.verifyEqual(length(findobj(gca, 'Type', 'text')), 5); 135 | labels = findobj(gca, 'Type', 'text'); 136 | for i=1:5 137 | tc.verifyEqual(labels(i).FontWeight, 'bold'); 138 | end 139 | 140 | % specify font size, weight, color 141 | clf; plot_point(points, 'sequence', 'textsize', 23, 'bold', 'textcolor', [0.2 0.3 0.4]); 142 | tc.verifyEqual(length(get(gca, 'Children')), 10); 143 | 144 | tc.verifyEqual(length(findobj(gca, 'Type', 'line')), 5); 145 | tc.verifyEqual(length(findobj(gca, 'Type', 'text')), 5); 146 | labels = findobj(gca, 'Type', 'text'); 147 | for i=1:5 148 | tc.verifyEqual(labels(i).FontSize, 23); 149 | tc.verifyEqual(labels(i).FontWeight, 'bold'); 150 | tc.verifyEqual(labels(i).Color, [0.2 0.3 0.4]); 151 | end 152 | end 153 | 154 | function plotpoly_test(tc) 155 | 156 | opt.animate = false; 157 | % 2d and 3D 158 | 159 | opt.tag = []; 160 | opt.axis = []; 161 | 162 | P = [1 3 5; 1 5 1]; 163 | 164 | clf; plot_poly(P); 165 | tc.verifyEqual(length(get(gca, 'Children')), 1); 166 | lines = findobj(gca, 'Type', 'line'); 167 | tc.verifyEqual(length(lines), 1); 168 | tc.verifyEqual(length(lines(1).XData), 4); 169 | tc.verifyEqual(length(lines(1).YData), 4); 170 | tc.verifyEqual(lines(1).Marker, 'none'); 171 | tc.verifyEqual(lines(1).LineStyle, '-'); 172 | 173 | clf; plot_poly(P, 'g--'); 174 | tc.verifyEqual(length(get(gca, 'Children')), 1); 175 | lines = findobj(gca, 'Type', 'line'); 176 | tc.verifyEqual(length(lines), 1); 177 | tc.verifyEqual(length(lines(1).XData), 4); 178 | tc.verifyEqual(length(lines(1).YData), 4); 179 | tc.verifyEqual(lines(1).Marker, 'none'); 180 | tc.verifyEqual(lines(1).LineStyle, '--'); 181 | tc.verifyEqual(lines(1).Color, [0 1 0]); 182 | 183 | clf; plot_poly(P, 'edgecolor', 'r'); 184 | tc.verifyEqual(length(get(gca, 'Children')), 1); 185 | lines = findobj(gca, 'Type', 'line'); 186 | tc.verifyEqual(length(lines), 1); 187 | tc.verifyEqual(length(lines(1).XData), 4); 188 | tc.verifyEqual(length(lines(1).YData), 4); 189 | tc.verifyEqual(lines(1).Marker, 'none'); 190 | tc.verifyEqual(lines(1).LineStyle, '-'); 191 | %tc.verifyEqual(lines(1).Color, [1 0 0]); 192 | 193 | clf; plot_poly(P, 'edgecolor', [0.2 0.3 0.4]); 194 | tc.verifyEqual(length(get(gca, 'Children')), 1); 195 | lines = findobj(gca, 'Type', 'line'); 196 | tc.verifyEqual(length(lines), 1); 197 | tc.verifyEqual(length(lines(1).XData), 4); 198 | tc.verifyEqual(length(lines(1).YData), 4); 199 | tc.verifyEqual(lines(1).Marker, 'none'); 200 | tc.verifyEqual(lines(1).LineStyle, '-'); 201 | %tc.verifyEqual(lines(1).Color, [0.2 0.3 0.4]); 202 | 203 | clf; plot_poly(P, 'tag', 'bob'); 204 | lines = findobj(gca, 'Type', 'line'); 205 | %tc.verifyEqual(lines(1).Tag, 'bob'); % no tag for line type 206 | 207 | %-------- patch mode 208 | 209 | clf; plot_poly(P, 'fillcolor', 'r'); 210 | tc.verifyEqual(length(get(gca, 'Children')), 1); 211 | patch = findobj(gca, 'Type', 'patch'); 212 | tc.verifyEqual(length(patch), 1); 213 | 214 | tc.verifyEqual(patch.FaceColor, [1 0 0]); 215 | tc.verifyEqual(patch.FaceAlpha, 1); 216 | tc.verifyEqual(patch.EdgeColor, [0 0 0]); 217 | tc.verifyEqual(patch.LineStyle, '-'); 218 | tc.verifySize(patch.Faces, [1 3]); 219 | tc.verifySize(patch.Vertices, [3 2]); 220 | 221 | clf; plot_poly(P, 'fillcolor', 'g', 'alpha', 0.5); 222 | tc.verifyEqual(length(get(gca, 'Children')), 1); 223 | patch = findobj(gca, 'Type', 'patch'); 224 | tc.verifyEqual(length(patch), 1); 225 | 226 | tc.verifyEqual(patch.FaceColor, [0 1 0]); 227 | tc.verifyEqual(patch.FaceAlpha, 0.5); 228 | tc.verifyEqual(patch.EdgeColor, [0 0 0]); 229 | tc.verifyEqual(patch.LineStyle, '-'); 230 | tc.verifySize(patch.Faces, [1 3]); 231 | tc.verifySize(patch.Vertices, [3 2]); 232 | 233 | clf; plot_poly(P, 'fillcolor', 'b', 'tag', 'bob'); 234 | patch = findobj(gca, 'Type', 'patch'); 235 | %tc.verifyEqual(patch.Tag, 'bob'); % no tag 236 | end 237 | 238 | function plot_sphere(tc) 239 | end 240 | 241 | function plot_box(tc) 242 | end 243 | 244 | function plot_arrow(tc) 245 | end 246 | 247 | function plot_homline(tc) 248 | end 249 | 250 | % plot_ellipse 251 | function ellipse2d_test(testCase) 252 | clf 253 | 254 | E = diag([9 4]); 255 | 256 | plot_ellipse(E) 257 | plot_ellipse(E, [4 0], 'r--'); 258 | plot_ellipse(E, [0 4], 'edgecolor', 'g'); 259 | plot_ellipse(E, [4 4], 'fillcolor', 'g'); 260 | plot_ellipse(E, [0 8 0.5], 'edgecolor', 'r', 'fillcolor', 'c', 'alpha', 0.5, 'LineWidth', 3); 261 | plot_ellipse(E, [4 8], 'b--', 'LineWidth', 3); 262 | 263 | axis equal 264 | end 265 | 266 | function ellipse2d_animate_test(testCase) 267 | clf 268 | axis([-4 4 -4 4]); 269 | 270 | E = diag([9 4]); 271 | 272 | h = plot_ellipse(E, 'g'); 273 | for x = circle([0 0], 1) 274 | plot_ellipse(E, x, 'alter', h) 275 | pause(0.1) 276 | end 277 | 278 | clf 279 | axis([-4 4 -4 4]); 280 | h = plot_ellipse(E, [0 0 0.5], 'edgecolor', 'r', 'fillcolor', 'c', 'LineWidth', 3); 281 | 282 | for x = circle([0 0], 1) 283 | plot_ellipse(E, x, 'alter', h) 284 | pause(0.1) 285 | end 286 | 287 | end 288 | 289 | % plot_ellipse 290 | function ellipse3d_test(testCase) 291 | clf 292 | 293 | E = diag([9 4 6]); 294 | 295 | plot_ellipse(E) 296 | pause 297 | 298 | clf 299 | plot_ellipse(E, 'edgecolor', 'g'); 300 | pause 301 | 302 | clf 303 | plot_ellipse(E, 'fillcolor', 'g'); 304 | pause 305 | 306 | clf 307 | plot_ellipse(E, 'fillcolor', 'g', 'shadow'); 308 | pause 309 | 310 | clf 311 | plot_ellipse(E, 'fillcolor', 'g', 'edgecolor', 'r', 'LineWidth', 2); 312 | pause 313 | 314 | plot_ellipse(E, [0 8], 'edgecolor', 'r', 'fillcolor', 'c'); 315 | plot_ellipse(E, [4 8], 'LineWidth', 3, 'MarkerStyle', '+'); 316 | 317 | axis equal 318 | end 319 | 320 | function ellipse3d_animate_test(testCase) 321 | clf 322 | axis([-4 4 -4 4 -4 4]); 323 | 324 | E = diag([9 4 6]); 325 | 326 | h = plot_ellipse(E, 'g'); 327 | for x = circle([0 0], 1) 328 | plot_ellipse(E, [x; 0], 'alter', h) 329 | pause(0.1) 330 | end 331 | end 332 | -------------------------------------------------------------------------------- /unit_test/tboptparseTest.m: -------------------------------------------------------------------------------- 1 | function tests = tboptparseTest() 2 | tests = functiontests(localfunctions); 3 | end 4 | 5 | function setupOnce(tc) 6 | opt.foo = false; 7 | opt.bar = true; 8 | opt.blah = []; 9 | opt.stuff = {}; 10 | opt.choose = {'this', 'that', 'other'}; 11 | opt.select = {'#no', '#yes'}; 12 | opt.old = '@foo'; 13 | opt.d_3d = false; 14 | tc.TestData.opt = opt; 15 | end 16 | 17 | function boolTest(tc) 18 | opt.foo = false; 19 | [out,args] = tb_optparse(opt, {}); 20 | tc.verifyEqual(out.foo, false); 21 | tc.verifySize(args, [0 0]); 22 | 23 | [out,args] = tb_optparse(opt, {'foo'}); 24 | tc.verifyEqual(out.foo, true); 25 | tc.verifySize(args, [0 0]); 26 | 27 | [out,args] = tb_optparse(opt, {'distract'}); 28 | tc.verifyEqual(out.foo, false); 29 | tc.verifyEqual(args, {'distract'}); 30 | 31 | [out,args] = tb_optparse(opt, {'foo', 'distract'}); 32 | tc.verifyEqual(out.foo, true); 33 | tc.verifyEqual(args, {'distract'}); 34 | 35 | [out,args] = tb_optparse(opt, {'distract', 'foo'}); 36 | tc.verifyEqual(out.foo, true); 37 | tc.verifyEqual(args, {'distract'}); 38 | 39 | end 40 | 41 | function noboolTest(tc) 42 | opt.foo = true; 43 | [out,args] = tb_optparse(opt, {}); 44 | tc.verifyEqual(out.foo, true); 45 | tc.verifySize(args, [0 0]); 46 | 47 | [out,args] = tb_optparse(opt, {'foo'}); 48 | tc.verifyEqual(out.foo, true); 49 | tc.verifySize(args, [0 0]); 50 | 51 | [out,args] = tb_optparse(opt, {'nofoo'}); 52 | tc.verifyEqual(out.foo, false); 53 | tc.verifySize(args, [0 0]); 54 | 55 | [out,args] = tb_optparse(opt, {'distract'}); 56 | tc.verifyEqual(out.foo, true); 57 | tc.verifyEqual(args, {'distract'}); 58 | 59 | [out,args] = tb_optparse(opt, {'foo', 'distract'}); 60 | tc.verifyEqual(out.foo, true); 61 | tc.verifyEqual(args, {'distract'}); 62 | 63 | [out,args] = tb_optparse(opt, {'distract', 'foo'}); 64 | tc.verifyEqual(out.foo, true); 65 | tc.verifyEqual(args, {'distract'}); 66 | 67 | [out,args] = tb_optparse(opt, {'nofoo', 'distract'}); 68 | tc.verifyEqual(out.foo, false); 69 | tc.verifyEqual(args, {'distract'}); 70 | 71 | [out,args] = tb_optparse(opt, {'distract', 'nofoo'}); 72 | tc.verifyEqual(out.foo, false); 73 | tc.verifyEqual(args, {'distract'}); 74 | end 75 | 76 | function setTest(tc) 77 | opt.foo = []; 78 | 79 | [out,args] = tb_optparse(opt, {}); 80 | tc.verifyEqual(out.foo, []); 81 | tc.verifySize(args, [0 0]); 82 | 83 | [out,args] = tb_optparse(opt, {'foo', 3}); 84 | tc.verifyEqual(out.foo, 3); 85 | tc.verifySize(args, [0 0]); 86 | 87 | [out,args] = tb_optparse(opt, {'foo', 'bar'}); 88 | tc.verifyEqual(out.foo, 'bar'); 89 | tc.verifySize(args, [0 0]); 90 | 91 | [out,args] = tb_optparse(opt, {'foo', [1 2 3]}); 92 | tc.verifyEqual(out.foo, [1 2 3]); 93 | tc.verifySize(args, [0 0]); 94 | 95 | [out,args] = tb_optparse(opt, {'foo', {1 2 3}}); 96 | tc.verifyEqual(out.foo, {1 2 3}); 97 | tc.verifySize(args, [0 0]); 98 | 99 | [out,args] = tb_optparse(opt, {'distract'}); 100 | tc.verifyEqual(out.foo, []); 101 | tc.verifyEqual(args, {'distract'}); 102 | 103 | [out,args] = tb_optparse(opt, {'distract', 'foo', 'bar'}); 104 | tc.verifyEqual(out.foo, 'bar'); 105 | tc.verifyEqual(args, {'distract'}); 106 | 107 | [out,args] = tb_optparse(opt, {'foo', 'bar', 'distract'}); 108 | tc.verifyEqual(out.foo, 'bar'); 109 | tc.verifyEqual(args, {'distract'}); 110 | end 111 | 112 | function cellsetTest(tc) 113 | opt.foo = {}; 114 | 115 | [out,args] = tb_optparse(opt, {}); 116 | tc.verifyEqual(out.foo, {}); 117 | tc.verifySize(args, [0 0]); 118 | 119 | [out,args] = tb_optparse(opt, {'foo', 3}); 120 | tc.verifyEqual(out.foo, {3}); 121 | tc.verifySize(args, [0 0]); 122 | 123 | [out,args] = tb_optparse(opt, {'foo', 'bar'}); 124 | tc.verifyEqual(out.foo, {'bar'}); 125 | tc.verifySize(args, [0 0]); 126 | 127 | [out,args] = tb_optparse(opt, {'foo', [1 2 3]}); 128 | tc.verifyEqual(out.foo, {[1 2 3]}); 129 | tc.verifySize(args, [0 0]); 130 | 131 | [out,args] = tb_optparse(opt, {'foo', {1 2 3}}); 132 | tc.verifyEqual(out.foo, {1 2 3}); 133 | tc.verifySize(args, [0 0]); 134 | 135 | [out,args] = tb_optparse(opt, {'distract'}); 136 | tc.verifyEqual(out.foo, {}); 137 | tc.verifyEqual(args, {'distract'}); 138 | 139 | [out,args] = tb_optparse(opt, {'distract', 'foo', 'bar'}); 140 | tc.verifyEqual(out.foo, {'bar'}); 141 | tc.verifyEqual(args, {'distract'}); 142 | 143 | [out,args] = tb_optparse(opt, {'foo', 'bar', 'distract'}); 144 | tc.verifyEqual(out.foo, {'bar'}); 145 | tc.verifyEqual(args, {'distract'}); 146 | end 147 | 148 | function chooseTest(tc) 149 | opt.choose = {'this', 'that', 'other'}; 150 | 151 | [out,args] = tb_optparse(opt, {}); 152 | tc.verifyEqual(out.choose, 'this'); 153 | tc.verifySize(args, [0 0]); 154 | 155 | [out,args] = tb_optparse(opt, {'this'}); 156 | tc.verifyEqual(out.choose, 'this'); 157 | tc.verifySize(args, [0 0]); 158 | 159 | [out,args] = tb_optparse(opt, {'that'}); 160 | tc.verifyEqual(out.choose, 'that'); 161 | tc.verifySize(args, [0 0]); 162 | 163 | [out,args] = tb_optparse(opt, {'other'}); 164 | tc.verifyEqual(out.choose, 'other'); 165 | tc.verifySize(args, [0 0]); 166 | 167 | [out,args] = tb_optparse(opt, {'yetanother'}); % this one not in the list 168 | tc.verifyEqual(out.choose, 'this'); % return default 169 | tc.verifyEqual(args, {'yetanother'}); % and the arg is returned here 170 | 171 | [out,args] = tb_optparse(opt, {'distract', 'that'}); 172 | tc.verifyEqual(out.choose, 'that'); 173 | tc.verifyEqual(args, {'distract'}); 174 | 175 | [out,args] = tb_optparse(opt, {'that','distract'}); 176 | tc.verifyEqual(out.choose, 'that'); 177 | tc.verifyEqual(args, {'distract'}); 178 | 179 | [out,args] = tb_optparse(opt, {'yetanother','distract'}); 180 | tc.verifyEqual(out.choose, 'this'); 181 | tc.verifyEqual(args, {'yetanother','distract'}); 182 | end 183 | 184 | function hashchooseTest(tc) 185 | opt.choose = {'#this', '#that', '#other'}; 186 | 187 | [out,args] = tb_optparse(opt, {}); 188 | tc.verifyEqual(out.choose, 1); 189 | tc.verifySize(args, [0 0]); 190 | 191 | [out,args] = tb_optparse(opt, {'this'}); 192 | tc.verifyEqual(out.choose, 1); 193 | tc.verifySize(args, [0 0]); 194 | 195 | [out,args] = tb_optparse(opt, {'that'}); 196 | tc.verifyEqual(out.choose, 2); 197 | tc.verifySize(args, [0 0]); 198 | 199 | [out,args] = tb_optparse(opt, {'other'}); 200 | tc.verifyEqual(out.choose, 3); 201 | tc.verifySize(args, [0 0]); 202 | 203 | [out,args] = tb_optparse(opt, {'yetanother'}); % this one not in the list 204 | tc.verifyEqual(out.choose, 1); % return default 205 | tc.verifyEqual(args, {'yetanother'}); % and the arg is returned here 206 | 207 | [out,args] = tb_optparse(opt, {'distract', 'that'}); 208 | tc.verifyEqual(out.choose, 2); 209 | tc.verifyEqual(args, {'distract'}); 210 | 211 | [out,args] = tb_optparse(opt, {'that','distract'}); 212 | tc.verifyEqual(out.choose, 2); 213 | tc.verifyEqual(args, {'distract'}); 214 | 215 | [out,args] = tb_optparse(opt, {'yetanother','distract'}); 216 | tc.verifyEqual(out.choose, 1); 217 | tc.verifyEqual(args, {'yetanother','distract'}); 218 | end 219 | 220 | function synonymTest(tc) 221 | opt.foo = false; 222 | opt.bar = '@foo'; 223 | 224 | [out,args] = tb_optparse(opt, {}); 225 | tc.verifyEqual(out.foo, false); 226 | tc.verifySize(args, [0 0]); 227 | 228 | [out,args] = tb_optparse(opt, {'foo'}); 229 | tc.verifyEqual(out.foo, true); 230 | tc.verifySize(args, [0 0]); 231 | 232 | [out,args] = tb_optparse(opt, {'bar'}); 233 | tc.verifyEqual(out.foo, true); 234 | tc.verifySize(args, [0 0]); 235 | 236 | [out,args] = tb_optparse(opt, {'distract'}); 237 | tc.verifyEqual(out.foo, false); 238 | tc.verifyEqual(args, {'distract'}); 239 | 240 | [out,args] = tb_optparse(opt, {'bar', 'distract'}); 241 | tc.verifyEqual(out.foo, true); 242 | tc.verifyEqual(args, {'distract'}); 243 | 244 | [out,args] = tb_optparse(opt, {'distract', 'bar'}); 245 | tc.verifyEqual(out.foo, true); 246 | tc.verifyEqual(args, {'distract'}); 247 | end 248 | 249 | function startDigitTest(tc) 250 | % bool 251 | opt.d_3foo = false; 252 | [out,args] = tb_optparse(opt, {}); 253 | tc.verifyEqual(out.d_3foo, false); 254 | tc.verifySize(args, [0 0]); 255 | 256 | [out,args] = tb_optparse(opt, {'3foo'}); 257 | tc.verifyEqual(out.d_3foo, true); 258 | tc.verifySize(args, [0 0]); 259 | 260 | % set 261 | opt.d_3foo = []; 262 | [out,args] = tb_optparse(opt, {}); 263 | tc.verifyEqual(out.d_3foo, []); 264 | tc.verifySize(args, [0 0]); 265 | 266 | [out,args] = tb_optparse(opt, {'3foo', [1 2 3]}); 267 | tc.verifyEqual(out.d_3foo, [1 2 3]); 268 | tc.verifySize(args, [0 0]); 269 | 270 | % choose 271 | opt.d_3foo = {'this', 'that', 'other'}; 272 | [out,args] = tb_optparse(opt, {}); 273 | tc.verifyEqual(out.d_3foo, 'this'); 274 | tc.verifySize(args, [0 0]); 275 | 276 | [out,args] = tb_optparse(opt, {'3foo', 'that'}); 277 | tc.verifyEqual(out.d_3foo, 'that'); 278 | tc.verifySize(args, [0 0]); 279 | end -------------------------------------------------------------------------------- /usefig.m: -------------------------------------------------------------------------------- 1 | %USEFIG Named figure windows 2 | % 3 | % usefig('Foo') makes figure 'Foo' the current figure, if it doesn't 4 | % exist create it. 5 | % 6 | % h = usefig('Foo') as above, but returns the figure handle 7 | 8 | function H = usefig(name) 9 | 10 | h = findobj('Name', name); 11 | if isempty(h), 12 | h = figure; 13 | set(h, 'Name', name); 14 | else 15 | figure(h); 16 | end 17 | 18 | if nargout > 0, 19 | H = h; 20 | end 21 | -------------------------------------------------------------------------------- /xaxis.m: -------------------------------------------------------------------------------- 1 | %XAXIS Set X-axis scaling 2 | % 3 | % XAXIS(MAX) set x-axis scaling from 0 to MAX. 4 | % 5 | % XAXIS(MIN, MAX) set x-axis scaling from MIN to MAX. 6 | % 7 | % XAXIS([MIN MAX]) as above. 8 | % 9 | % XAXIS restore automatic scaling for x-axis. 10 | % 11 | % See also YAXIS. 12 | 13 | 14 | % Copyright (C) 1993-2017, by Peter I. Corke 15 | % 16 | % This file is part of The Robotics Toolbox for MATLAB (RTB). 17 | % 18 | % RTB is free software: you can redistribute it and/or modify 19 | % it under the terms of the GNU Lesser General Public License as published by 20 | % the Free Software Foundation, either version 3 of the License, or 21 | % (at your option) any later version. 22 | % 23 | % RTB is distributed in the hope that it will be useful, 24 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 25 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 26 | % GNU Lesser General Public License for more details. 27 | % 28 | % You should have received a copy of the GNU Leser General Public License 29 | % along with RTB. If not, see . 30 | % 31 | % http://www.petercorke.com 32 | 33 | function xaxis(varargin) 34 | 35 | opt.all = false; 36 | [opt,args] = tb_optparse(opt, varargin); 37 | 38 | if length(args) == 0 39 | [x,y] = ginput(2); 40 | mn = x(1); 41 | mx = x(2); 42 | elseif length(args) == 1 43 | if length(args{1}) == 1 44 | mn = 0; 45 | mx = args{1}; 46 | elseif length(args{1}) == 2 47 | mn = args{1}(1); 48 | mx = args{1}(2); 49 | end 50 | elseif length(args) == 2 51 | mn = args{1}; 52 | mx = args{2}; 53 | end 54 | 55 | if opt.all 56 | for a=get(gcf,'Children')', 57 | if strcmp(get(a, 'Type'), 'axes') == 1, 58 | set(a, 'XLimMode', 'manual', 'XLim', [mn mx]) 59 | set(a, 'YLimMode', 'auto') 60 | end 61 | end 62 | else 63 | set(gca, 'XLimMode', 'manual', 'XLim', [mn mx]) 64 | set(gca, 'YLimMode', 'auto') 65 | end -------------------------------------------------------------------------------- /yaxis.m: -------------------------------------------------------------------------------- 1 | %YAYIS set Y-axis scaling 2 | % 3 | % YAXIS(MAX) set y-axis scaling from 0 to MAX. 4 | % 5 | % YAXIS(MIN, MAX) set y-axis scaling from MIN to MAX. 6 | % 7 | % YAXIS([MIN MAX]) as above. 8 | % 9 | % YAXIS restore automatic scaling for y-axis. 10 | % 11 | % See also YAXIS. 12 | 13 | 14 | % Copyright (C) 1993-2017, by Peter I. Corke 15 | % 16 | % This file is part of The Robotics Toolbox for MATLAB (RTB). 17 | % 18 | % RTB is free software: you can redistribute it and/or modify 19 | % it under the terms of the GNU Lesser General Public License as published by 20 | % the Free Software Foundation, either version 3 of the License, or 21 | % (at your option) any later version. 22 | % 23 | % RTB is distributed in the hope that it will be useful, 24 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 25 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 26 | % GNU Lesser General Public License for more details. 27 | % 28 | % You should have received a copy of the GNU Leser General Public License 29 | % along with RTB. If not, see . 30 | % 31 | % http://www.petercorke.com 32 | 33 | function yaxis(a1, a2) 34 | if nargin == 0, 35 | set(gca, 'YLimMode', 'auto') 36 | return 37 | elseif nargin == 1, 38 | if length(a1) == 1, 39 | mn = 0; 40 | mx = a1; 41 | elseif length(a1) == 2, 42 | mn = a1(1); 43 | mx = a1(2); 44 | end 45 | elseif nargin == 2, 46 | mn = a1; 47 | mx = a2; 48 | end 49 | 50 | set(gca, 'YLimMode', 'manual', 'YLim', [mn mx]) 51 | --------------------------------------------------------------------------------